Delta并聯(lián)清洗機(jī)構(gòu)軌跡跟蹤控制方法研究

董慧芬，王 滲，宋金海

(中國(guó)民航大學(xué)電子信息與自動(dòng)化學(xué)院，天津 300300)

1 引言

機(jī)場(chǎng)嵌入式助航燈具由于外界環(huán)境污染，會(huì)造成其發(fā)光強(qiáng)度降低，危及航空安全，故機(jī)場(chǎng)需要定期進(jìn)行人工清洗。實(shí)現(xiàn)燈具自動(dòng)清洗是提高清洗效率，降低機(jī)場(chǎng)運(yùn)行成本的重要手段。目前為止，國(guó)內(nèi)跑道助航燈具大多采用人工清洗，清洗效率低，人工成本高是機(jī)場(chǎng)運(yùn)營(yíng)的痛點(diǎn)，而國(guó)外的自動(dòng)化清洗設(shè)備價(jià)格高昂，絕大多數(shù)機(jī)場(chǎng)難以負(fù)擔(dān)，在此背景下，渴望一款高效自主研發(fā)的機(jī)場(chǎng)跑道助航燈具清洗設(shè)備運(yùn)用而生。本文首次提出一款Delta并聯(lián)清洗機(jī)構(gòu)，對(duì)機(jī)場(chǎng)跑道助航燈具進(jìn)行自動(dòng)清洗。并聯(lián)清洗機(jī)構(gòu)末端對(duì)準(zhǔn)助航燈具發(fā)光口實(shí)現(xiàn)精確控制是實(shí)現(xiàn)自動(dòng)清洗的關(guān)鍵，因此，本文重點(diǎn)研究Delta并聯(lián)清洗機(jī)構(gòu)末端的軌跡跟蹤控制的問(wèn)題。

1985年，Clavel博士發(fā)明了一類三自由度空間平移并聯(lián)結(jié)構(gòu)，即著名的Delta機(jī)械人[1]。Delta機(jī)器人克服了并聯(lián)機(jī)構(gòu)的很多缺點(diǎn)，有著精度高、剛度高、速度快和承載能力大等特點(diǎn)，因而被廣泛應(yīng)用。早期的并聯(lián)清洗機(jī)構(gòu)控制策略大多采用的是傳統(tǒng)的PID控制，但其建立的運(yùn)動(dòng)學(xué)模型會(huì)存在一定的不確定性，原因歸結(jié)于該機(jī)構(gòu)本身系統(tǒng)復(fù)雜，輸入輸出變量多，是一個(gè)多參數(shù)耦合的非線性系統(tǒng)，在物理上就已經(jīng)非常難獲取相應(yīng)的準(zhǔn)確值，因此，傳統(tǒng)的 PID 控制難以取得很好的控制效果。針對(duì)并聯(lián)機(jī)構(gòu)的控制和優(yōu)化問(wèn)題，已經(jīng)有很多國(guó)內(nèi)外學(xué)者做了研究。在文獻(xiàn)[2]中，為一種直線類型Delta機(jī)構(gòu)設(shè)計(jì)了一種神經(jīng)網(wǎng)絡(luò)控制器，并仿真驗(yàn)證了神經(jīng)網(wǎng)絡(luò)算法良好的時(shí)效性。2016年李麗麗等人采用PC作為上位機(jī)和ARM作為下位機(jī)的控制策略，建立了一個(gè)三自由度Delta高速機(jī)器人協(xié)調(diào)控制系統(tǒng)，具備了系統(tǒng)性能好，研發(fā)周期短，經(jīng)濟(jì)效益高等優(yōu)點(diǎn)[3]。這種控制方法儼然已成為Delta機(jī)器人控制手段的新方向。模糊控制因其具備非線性控制和魯棒性優(yōu)良等特征，非常適合用來(lái)控制時(shí)變參數(shù)、非線性系統(tǒng)，使其控制手段簡(jiǎn)單，控制效果非常明顯。蘭州理工大學(xué)的陳艷娟進(jìn)行了Delta機(jī)器人軌跡跟蹤控制仿真研究[4]。盧興國(guó)針對(duì)于軌跡跟蹤控制問(wèn)題，提出了模糊神經(jīng)網(wǎng)絡(luò)PID控制器提高穩(wěn)定性[5]。哈爾冰工業(yè)大學(xué)的劉明等人提出一種自學(xué)習(xí)區(qū)間型模糊神經(jīng)網(wǎng)絡(luò)(SLIT2FNN)控制方案進(jìn)行仿真，證明了所提出控制器的性能和魯棒性[6]。Bionotes等人提出一種在對(duì)齊條件下的迭代學(xué)習(xí)控制器(ILC)，用于并行Delta機(jī)器人的軌跡跟蹤，得到位置和速度誤差隨著迭代次數(shù)的增加而減小，仿真驗(yàn)證該控制器的有效性[7]。很可惜他們都只停留在仿真階段，沒(méi)有具體的運(yùn)用于實(shí)際且進(jìn)行實(shí)驗(yàn)驗(yàn)證。

綜述以上，提出模糊自適應(yīng)PID控制器并運(yùn)用于Delta并聯(lián)清洗機(jī)構(gòu)，仿真和實(shí)驗(yàn)驗(yàn)證了該控制器較傳統(tǒng)PID控制器相比，不僅節(jié)省清洗時(shí)間，而且還提高了清洗效率。

2 并聯(lián)清洗機(jī)構(gòu)分析

自動(dòng)清洗機(jī)構(gòu)采用的四自由度Delta并聯(lián)機(jī)器人，因其具有結(jié)構(gòu)簡(jiǎn)單、承載能力強(qiáng)，動(dòng)作快速，剛度大，精度高，又有良好的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性等眾多特點(diǎn)。四自由度Delta并聯(lián)清洗機(jī)構(gòu)主要由移動(dòng)單元、清洗單元和并聯(lián)機(jī)構(gòu)單元三部分組成。清洗單元包括：空壓機(jī)、粉末罐、電磁閥，高壓軟管，末端夾持機(jī)構(gòu)，噴嘴等裝置；并聯(lián)機(jī)構(gòu)單元包括：動(dòng)平臺(tái)、靜平臺(tái)、伺服驅(qū)動(dòng)電機(jī)、限位開(kāi)關(guān)、主動(dòng)臂、從動(dòng)臂、旋轉(zhuǎn)軸、攝像頭等。移動(dòng)單元包括搭載的整個(gè)車體，方便靈活，可也以快速移動(dòng)，清洗一排排跑道助航燈具。Delta并聯(lián)機(jī)器人動(dòng)平臺(tái)攜帶清洗噴嘴定位到達(dá)助航燈具發(fā)光口，打開(kāi)清洗開(kāi)關(guān)進(jìn)行自動(dòng)清洗，對(duì)洗完后的燈具進(jìn)行光效評(píng)估，判斷是否需要優(yōu)化清洗參數(shù)二次清洗。具體結(jié)構(gòu)如圖1所示。

圖1 四自由度Delta并聯(lián)清洗機(jī)構(gòu)

如圖1所示，并聯(lián)機(jī)構(gòu)單元有兩個(gè)平臺(tái)：靜平臺(tái)和動(dòng)平臺(tái)。靜平臺(tái)一般與固定裝置連接，是主要的承載裝置，用來(lái)安裝固定零件，舵機(jī)等主要部件；動(dòng)平臺(tái)主要是用來(lái)安裝和固定末端控制器的機(jī)構(gòu)，并且能夠阻止末端執(zhí)行器與機(jī)械臂的相對(duì)運(yùn)動(dòng)。舵機(jī)均勻分散安裝在靜平臺(tái)的三個(gè)角上面。主動(dòng)臂和從動(dòng)臂將動(dòng)平臺(tái)和靜平臺(tái)連接起來(lái)，每個(gè)從動(dòng)臂都由四個(gè)球鉸和桿件組成的平行四邊形閉環(huán)組成。工作時(shí)，舵機(jī)將動(dòng)力傳遞給主動(dòng)臂，主動(dòng)臂通過(guò)球鉸將動(dòng)力傳遞給從動(dòng)臂，從動(dòng)臂最后將動(dòng)力傳遞給動(dòng)平臺(tái)，來(lái)完成對(duì)末端執(zhí)行器的控制。

Delta并聯(lián)清洗機(jī)構(gòu)采用單目視覺(jué)定位提取助航燈具發(fā)光口坐標(biāo)，并輸入目標(biāo)點(diǎn)到并聯(lián)機(jī)構(gòu)單元進(jìn)行運(yùn)動(dòng)學(xué)逆解，得到電機(jī)旋轉(zhuǎn)角度，通知下位機(jī)實(shí)現(xiàn)自動(dòng)清洗，對(duì)清洗結(jié)果進(jìn)行光效評(píng)估。具體清洗控制流程如圖2所示。

圖2 清洗控制流程圖

2.1 Delta并聯(lián)清洗機(jī)構(gòu)運(yùn)動(dòng)學(xué)正解

運(yùn)動(dòng)學(xué)正解就是在已知電機(jī)的輸出角位移情況下，來(lái)求解清洗機(jī)構(gòu)末端位姿輸出的位置參數(shù)。并聯(lián)機(jī)構(gòu)正解求解過(guò)程相對(duì)于逆解的求解過(guò)程來(lái)說(shuō)十分復(fù)雜，逆解大多數(shù)采用上述的矢量法進(jìn)行求解，而對(duì)于正解的求解過(guò)程一般采用幾何解析的算法來(lái)進(jìn)行求解，韋巖等人[8]通過(guò)將并聯(lián)機(jī)構(gòu)進(jìn)行簡(jiǎn)化建模，發(fā)現(xiàn)靜平臺(tái)和動(dòng)平臺(tái)始終平行，通過(guò)平移模型中的從動(dòng)臂，從而將機(jī)構(gòu)的正解求解問(wèn)題轉(zhuǎn)化為對(duì)一個(gè)已知所有邊長(zhǎng)及3個(gè)頂點(diǎn)坐標(biāo)的三棱錐求解頂點(diǎn)坐標(biāo)的問(wèn)題。已知的三個(gè)頂點(diǎn)坐標(biāo)為

(1)

2.2 Delta并聯(lián)清洗機(jī)構(gòu)運(yùn)動(dòng)學(xué)逆解

運(yùn)動(dòng)學(xué)逆解就是在已知目標(biāo)位置的前提下，求解各舵機(jī)的輸入和末端位姿輸出的關(guān)系。運(yùn)用空間幾何學(xué)和矢量代數(shù)的方法建立了四自由度Delta并聯(lián)清洗機(jī)構(gòu)的簡(jiǎn)化模型，求解得到并聯(lián)清洗機(jī)構(gòu)位置的逆解方程[9]。清洗機(jī)構(gòu)的靜平臺(tái)和動(dòng)平臺(tái)之間通過(guò)主動(dòng)臂與從動(dòng)臂相連接，工作時(shí)由舵機(jī)驅(qū)動(dòng)主動(dòng)臂運(yùn)動(dòng)，然后主動(dòng)臂帶動(dòng)從動(dòng)臂一起運(yùn)動(dòng)，最后從動(dòng)臂控制末端動(dòng)平臺(tái)的運(yùn)動(dòng)狀態(tài)。由上述分析可知，求解清洗機(jī)構(gòu)逆解就是在已知?jiǎng)悠脚_(tái)中心O′在靜平臺(tái)坐標(biāo)系的坐標(biāo)下，求解電機(jī)的旋轉(zhuǎn)角度，即主動(dòng)臂相對(duì)于靜平臺(tái)的角度。由此可得Delta并聯(lián)機(jī)器人逆運(yùn)動(dòng)的位置解為

(2)

(3)

3 末端模糊PID軌跡跟蹤控制器

模糊自適應(yīng)PID控制器原理簡(jiǎn)單，使用方便，適應(yīng)性強(qiáng)，靈活穩(wěn)定。對(duì)于非線性和時(shí)變性較大的控制系統(tǒng)，在控制方面相較于傳統(tǒng)的PID控制器，優(yōu)點(diǎn)就顯得更加突出。

3.1 模糊自適應(yīng)PID控制器

需要定義輸入、輸出模糊集并且確定個(gè)數(shù)類別，參考經(jīng)典PID的控制方法以及依據(jù)模糊PID控制器的控制規(guī)律，于此同時(shí)要兼顧控制精度，提高系統(tǒng)的響應(yīng)特性[10]，為此取出3個(gè)模糊集N(負(fù))，Z(零)，P(正)。本文模糊控制集取

e，ec={N，Z，P}

(4)

輸出的ΔKp，ΔKd，ΔKi也同樣取3個(gè)模糊集

ΔKp，ΔKd，ΔKi={N，Z，P}

(5)

根據(jù)控制要求，確定輸入輸出變量的實(shí)際論域，對(duì)各個(gè)輸入輸出變量做如下劃定

e，ec={-1，0，1}

(6)

ΔKp，ΔKd，ΔKi={-0.01，0，0.01}

(7)

應(yīng)用模糊合成推理PID參數(shù)的整定算法。第k個(gè)采樣時(shí)間的整定為

(8)

式中，Kp0，Ki0，Kd0為經(jīng)典PID控制器的初始參數(shù)，具體的控制流程如圖3所示。

圖3 模糊自適應(yīng)PID控制系統(tǒng)

3.2 模糊規(guī)則的建立

針對(duì)Kp，Ki，Kd三個(gè)參數(shù)對(duì)系統(tǒng)輸出特性的不同表現(xiàn)，建立了一套合適的關(guān)于e，ec，Kp，Ki，Kd的模糊規(guī)則推理表，具體如表1，表2所示。

表1 比例參數(shù)取值的模糊規(guī)則表

表2 積分參數(shù)取值模糊規(guī)則表

4 清洗機(jī)構(gòu)末端跟蹤控制建模

并聯(lián)清洗機(jī)構(gòu)末端的閉環(huán)控制系統(tǒng)框圖如圖4所示。

圖4 閉環(huán)控制原理框圖

4.1 清洗末端傳統(tǒng)PID控制仿真模型

建立圖5所示的閉環(huán)控制仿真模型[11]，其中控制器選用PID控制器。位置采用PID控制，仿真時(shí)調(diào)整PID的參數(shù)。IK和FK依然是并聯(lián)清洗機(jī)構(gòu)逆解和正解的求解程序，generator為電機(jī)的封裝模塊，這里將負(fù)載從模塊中獨(dú)立出來(lái)，可以方便在仿真時(shí)隨時(shí)改變給定的負(fù)載。將正解輸出作為位置反饋，構(gòu)成閉環(huán)。

4.2 清洗末端模糊PID控制仿真模型

將圖5中的PID環(huán)節(jié)替換為Fuzzy PID模塊，其它模塊與環(huán)節(jié)保持不變，模糊PID可以PID的參數(shù)進(jìn)行自整定，圖中的Fuzzy PID模塊為封裝模塊，它的子系統(tǒng)如圖6所示。

圖5 傳統(tǒng)PID仿真模型

圖6 Fuzzy PID子系統(tǒng)

5 仿真結(jié)果分析

仿真時(shí)選擇一組目標(biāo)值，輸入坐標(biāo)選擇為(150，50，-100)的階躍信號(hào)，即X的輸入為150mm，Y的輸入為50mm，Z的輸入為-100mm。仿真X，Y，Z的輸出的曲線如圖7所示，其中圖(a)為X的輸出曲線，圖(b)為Y的輸出曲線，圖(c)為Z的輸出曲線。為方便與PID輸出的曲線做比較，將PID輸出和模糊自適應(yīng)PID輸出的曲線在同一個(gè)界面顯示出來(lái)。其中虛線型曲線是模糊自適應(yīng)PID控制時(shí)的位置輸出，實(shí)線型曲線是PID控制時(shí)的位置輸出。

圖7 階躍信號(hào)位置輸出對(duì)比曲線

從上述三圖可以看出，圖(a)為X位置輸出曲線，采用模糊自適應(yīng)PID控制，從曲線中可以看出在仿真開(kāi)始初期的0.8s時(shí)間內(nèi)，曲線變化很快，在1s左右的時(shí)候達(dá)到了穩(wěn)定，沒(méi)有超調(diào)量。采用PID控制設(shè)定好參數(shù)后，在2.5s左右的時(shí)間才達(dá)到穩(wěn)定，此時(shí)X軸穩(wěn)定在了148mm的位置，穩(wěn)態(tài)誤差為2mm。曲線圖(b)為位置輸出Y的曲線，采用模糊自適應(yīng)PID控制，從曲線中可以看出在仿真開(kāi)始初期的0.8s時(shí)間內(nèi)，曲線變化很快，并且變化趨勢(shì)比較平緩。在1s左右的時(shí)候達(dá)到了穩(wěn)定，此時(shí)X軸穩(wěn)定在了150mm的位置。而采用PID控制設(shè)定好參數(shù)后，在2.5s左右的時(shí)間才達(dá)到穩(wěn)定，此時(shí)Y軸穩(wěn)定在了49mm的位置，穩(wěn)態(tài)誤差為1mm。曲線圖(c)為位置輸出Z的曲線，采用模糊自適應(yīng)PID控制，從曲線中可以看出在仿真開(kāi)始初期的0.7s時(shí)間內(nèi)，曲線變化很快，在0.8s左右的時(shí)候達(dá)到了穩(wěn)定，此時(shí)Z軸穩(wěn)定在了-100mm的位置。而采用PID控制設(shè)定好參數(shù)后，在2.7s左右的時(shí)間才達(dá)到穩(wěn)定，此時(shí)Z軸穩(wěn)定在了-99mm的位置，存在1mm的誤差。

仿真結(jié)果表明，模糊自適應(yīng)PID與傳統(tǒng)PID閉環(huán)控制相比提高了響應(yīng)的速度和精度，可以很好地滿足清洗要求，有效節(jié)省了助航燈具的清洗時(shí)間，降低清洗成本。Delta并聯(lián)清洗機(jī)構(gòu)采用模糊PID閉環(huán)控制使系統(tǒng)實(shí)現(xiàn)對(duì)目標(biāo)位置的快速準(zhǔn)確跟蹤，可以看出該算法是高效可行的。

6 實(shí)驗(yàn)結(jié)果與分析

6.1 階躍信號(hào)的實(shí)驗(yàn)結(jié)果分析

選擇與仿真相同的目標(biāo)點(diǎn)進(jìn)行實(shí)驗(yàn)，即(150，50，-150)的階躍信號(hào)。X，Y，Z軸的實(shí)驗(yàn)結(jié)果如圖8所示，其中圖(a)為X的輸出曲線，圖(b)為Y的輸出曲線，圖(c)為Z的輸出曲線。PID輸出和模糊自適應(yīng)PID輸出的曲線顯示在同一界面進(jìn)行比較。

圖8 階躍信號(hào)實(shí)驗(yàn)輸出對(duì)比曲線

從上述三圖可以看出，實(shí)際控制當(dāng)中，傳統(tǒng)PID控制存在超調(diào)。X軸位置輸出曲線，采用模糊自適應(yīng)PID控制在0.9s達(dá)到了穩(wěn)定，沒(méi)有超調(diào)量。采用PID控制在0.7s有較大超調(diào)，2.3s后穩(wěn)定在148mm的位置，穩(wěn)態(tài)誤差為2mm。Y軸位置輸出曲線，采用模糊自適應(yīng)PID控制在0.8s達(dá)到了穩(wěn)定，沒(méi)有超調(diào)量，穩(wěn)定在50mm位置。采用PID控制在0.4s有較大超調(diào)，達(dá)到了60mm，1.7s后穩(wěn)定在51mm的位置，穩(wěn)態(tài)誤差為1mm。Z軸位置輸出曲線，采用模糊自適應(yīng)PID控制在1s達(dá)到了穩(wěn)定，沒(méi)有超調(diào)量。采用PID控制在0.5s有較大超調(diào)，2.3s后穩(wěn)定在-98mm的位置，穩(wěn)態(tài)誤差為2mm。

實(shí)驗(yàn)結(jié)果可以看出，并聯(lián)清洗機(jī)構(gòu)試驗(yàn)過(guò)程當(dāng)中，由于機(jī)械安裝等原因，PID控制會(huì)不約而同的產(chǎn)生超調(diào)，控制響應(yīng)度和精確度較差。相比之下，模糊自適應(yīng)PID控制算法優(yōu)越性更強(qiáng)，在該模型運(yùn)用當(dāng)中的誤差小，非常適合用在并聯(lián)清洗機(jī)構(gòu)當(dāng)中。

6.2 軌跡跟蹤實(shí)驗(yàn)結(jié)果分析

在Delta并聯(lián)清洗機(jī)構(gòu)的清洗工作空間中，輸入螺旋上升狀的理想軌跡信號(hào)，為了盡可能看清楚軌跡跟蹤的細(xì)節(jié)狀態(tài)，特此截取觀察2個(gè)周期的跟蹤軌跡，觀測(cè)對(duì)比模糊自適應(yīng)PID控制算法和經(jīng)典PID控制算法的軌跡跟蹤情況。采用立體圖使視覺(jué)效果更佳，并且將理想軌跡曲線，PID輸出曲線和模糊自適應(yīng)PID輸出曲線同時(shí)顯示在同一界面當(dāng)中，如圖9所示。

圖9 三維立體軌跡跟蹤曲線

單從上面三維立體軌跡跟蹤曲線可以看出，Delta并聯(lián)清洗機(jī)構(gòu)的響應(yīng)速度很快，模糊自適應(yīng)PID控制和PID控制都可以在非常短的時(shí)間內(nèi)完成追蹤螺旋理想軌跡?？梢园l(fā)現(xiàn)，模糊自適應(yīng)PID控制的跟蹤能力比PID控制更強(qiáng)，響應(yīng)曲線更加貼近理想跟蹤信號(hào)，微小幅度的超調(diào)，后期跟蹤過(guò)程當(dāng)中也沒(méi)有出現(xiàn)超調(diào)幅度大或者嚴(yán)重的跟蹤現(xiàn)象。而PID控制在剛開(kāi)始時(shí)候出現(xiàn)了微小的偏差，慢慢的到后期，偏差逐漸明顯，并且越往后偏差越大，跟蹤偏差與模糊自適應(yīng)PID控制算法相比較而言偏大，整體跟蹤效果和性能略顯稍差一些。

實(shí)驗(yàn)結(jié)果分析可知，模糊自適應(yīng)PID控制算法提高了清洗機(jī)構(gòu)的響應(yīng)速度和精度，相較于助航燈具發(fā)光口大小，控制誤差非常小，在允許的范圍內(nèi)，可以滿足清洗要求。特別是運(yùn)用在Delta并聯(lián)清洗機(jī)構(gòu)上，在線整定PID參數(shù)，能夠使系統(tǒng)較快地趨于穩(wěn)定，實(shí)現(xiàn)對(duì)目標(biāo)位置的快速準(zhǔn)確跟蹤，精度高并且動(dòng)態(tài)性能好，節(jié)省清洗時(shí)間，提高清洗效率，降低了機(jī)場(chǎng)清洗成本。

7 結(jié)論

運(yùn)用Delta并聯(lián)清洗機(jī)構(gòu)清洗機(jī)場(chǎng)跑道嵌入式助航燈具發(fā)光口過(guò)程中，針對(duì)Delta并聯(lián)清洗機(jī)構(gòu)動(dòng)平臺(tái)末端定位難，誤差大，難以滿足機(jī)場(chǎng)清洗要求等問(wèn)題，因此建立了Delta清洗機(jī)構(gòu)模型，設(shè)計(jì)模糊自適應(yīng)PID控制器，輸入階躍信號(hào)到Delta并聯(lián)清洗機(jī)構(gòu)中進(jìn)行仿真和實(shí)驗(yàn)，并且與傳統(tǒng)PID控制器進(jìn)行對(duì)比。仿真實(shí)驗(yàn)結(jié)果充分表明，Delta并聯(lián)清洗機(jī)構(gòu)采用模糊自適應(yīng)PID控制算法，清洗機(jī)構(gòu)的響應(yīng)速度提升1.3s，清洗末端控制精度達(dá)到2mm，單個(gè)燈具發(fā)光口清洗時(shí)間減少至1.5s，滿足機(jī)場(chǎng)清洗要求且降低了機(jī)場(chǎng)清洗成本。模糊PID控制算法運(yùn)用于Delta并聯(lián)清洗機(jī)構(gòu)是高效可行的。