張亞婉，陸興華，吳宏裕

(廣東工業(yè)大學(xué)華立學(xué)院，廣東 廣州 511325)

0 引 言

機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸是機(jī)器人進(jìn)行抓取、攀爬以及行走等作業(yè)的重要部件，該部件的穩(wěn)定控制是保障機(jī)器人穩(wěn)定可靠性運行的關(guān)鍵。對機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的準(zhǔn)確、可靠控制是建立在對關(guān)節(jié)的準(zhǔn)確定位和測姿基礎(chǔ)上的。結(jié)合對運動參數(shù)的融合處理，提高對移動機(jī)器人旋轉(zhuǎn)關(guān)鍵軸的可靠控制能力，最終將控制律嵌入到機(jī)器人的慣性導(dǎo)航系統(tǒng)(inertia navigation system，INS)中[1]，提高機(jī)器人的動態(tài)穩(wěn)定控制能力，研究機(jī)器人的穩(wěn)定控制律，在機(jī)器人的控制設(shè)計中具有很好的應(yīng)用價值。

傳統(tǒng)方法中，對機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的內(nèi)控制方法主要有PID控制方法、反演積分控制方法以及卡爾曼濾波控制方法等。無論哪種方法，都需要對機(jī)器人的旋轉(zhuǎn)關(guān)節(jié)軸進(jìn)行絕對角度解算，得到姿態(tài)角，結(jié)合慣性參量融合，進(jìn)行機(jī)器人的魯棒性控制。Marins等提出了一種四元素的擴(kuò)展卡爾曼濾波方法(EKF)[2]，采用三軸坐標(biāo)系模型構(gòu)造機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的運動學(xué)方程，使用歐拉角計算姿態(tài)角，從而消除了姿態(tài)估計誤差，但該方法的計算開銷較大，對機(jī)器人控制的實時性不好[3-4]。文獻(xiàn)[5]中提出一種基于機(jī)器人姿態(tài)跟蹤的旋轉(zhuǎn)關(guān)節(jié)軸控制方法，融合擴(kuò)展卡爾曼濾波和無跡卡爾曼濾波(UKF)，提高姿態(tài)估計和控制參數(shù)的融合能力，但該方法進(jìn)行機(jī)器人控制的魯棒性較差，抗擾動性不好。

針對上述方法存在的問題，文中提出一種基于運動參數(shù)融合的機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸魯棒性控制律。構(gòu)建機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的動力學(xué)模型，采用姿態(tài)傳感器進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的運動姿態(tài)參數(shù)采集，對采集的姿態(tài)參數(shù)采用Kalman濾波算法進(jìn)行誤差補(bǔ)償和信息融合處理，根據(jù)運動參數(shù)的融合結(jié)果采用模糊控制方法進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制律設(shè)計。最后進(jìn)行仿真實驗分析，驗證該方法在提高機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸魯棒性控制能力方面的優(yōu)越性能。

1 被控對象描述和參量分析

1.1 機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的動力學(xué)模型

為了實現(xiàn)機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸魯棒性控制優(yōu)化，首先構(gòu)建機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的動力學(xué)模型，采用姿態(tài)傳感器進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的運動姿態(tài)參數(shù)采集[6]，構(gòu)建機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的無阻尼自由振動系統(tǒng)方程：

(1)

假設(shè)機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的末端位姿運動是一個縱向的線性運動模型，根據(jù)阻尼力矩與末端位姿協(xié)同控制的關(guān)聯(lián)關(guān)系，得到位姿修正的反饋最小信息熵泛函數(shù)學(xué)模型：

(2)

(3)

δ·p1-2p2+ρ2A2-δρ1A1+c2+cr=0

(4)

ρ2(p2-c2-cr)-δ·(1-δ)μ2A2=0

(5)

1.2 機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的運動姿態(tài)參數(shù)分析

(6)

根據(jù)磁力計對旋轉(zhuǎn)關(guān)節(jié)軸的采集結(jié)果進(jìn)行信息融合，用Ψ2(d2(t))表示矩陣L(Z2+Z3)-1LT與MT(Z2+Z3)-1MT的Bergmann核[8]，由此判定機(jī)器人的旋轉(zhuǎn)關(guān)節(jié)軸控制系統(tǒng)是否穩(wěn)定，判決方程為：

(7)

采用滑膜積分控制方法進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸運動的穩(wěn)態(tài)控制，構(gòu)建機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸運動姿態(tài)穩(wěn)定控制的數(shù)學(xué)模型和動力學(xué)模型[9]，得到機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸助力轉(zhuǎn)向系統(tǒng)的動能T和勢能V：

(8)

V=MPgLcosθP

(9)

對采集的姿態(tài)參數(shù)采用Kalman濾波算法進(jìn)行誤差補(bǔ)償和信息融合處理[10]，得到機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制的平衡解滿足：

ρiyi(t-δ(t))]=-2fT(y(t))T1f(y(t))-

2fT(y(t-δ(t)))T2f(y(t-δ(t)))+

2yT(t)ΣT1f(y(t))+2yT(t-δ(t))ΣT2f(y

(t-δ(t)))

(10)

結(jié)合約束條件，可以得到機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸運動控制的穩(wěn)態(tài)條件[11]。

2 控制律的優(yōu)化設(shè)計

2.1 運動參數(shù)的Kalman濾波融合算法

在構(gòu)建機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的動力學(xué)模型，采用姿態(tài)傳感器進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的運動姿態(tài)參數(shù)分析的基礎(chǔ)上，進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制律的優(yōu)化設(shè)計。文中提出一種基于運動參數(shù)融合的機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸魯棒性控制律。機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的運動姿態(tài)調(diào)節(jié)的概率分布項為{p(x1),p(x2),…,p(xm)}。機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸在7自由度空間內(nèi)進(jìn)行穩(wěn)態(tài)調(diào)節(jié)，在控制約束方程的兩端同時除以pqrsdp，得到：

(11)

構(gòu)建機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸問題控制的微分方程代數(shù)方程組：

(12)

其中，穩(wěn)定性平衡點滿足：

(13)

采用Kalman濾波算法進(jìn)行誤差補(bǔ)償和信息融合處理[12]，Kalman濾波傳遞函數(shù)為：

(14)

(15)

誤差補(bǔ)償和信息融合處理后機(jī)器人穩(wěn)態(tài)控制的穩(wěn)定性參數(shù)模型滿足：

(16)

當(dāng)([pHj,pLj])?([pHi,pLi])時，表示命題Aj支持Ai，即Aj?Ai，由此得到了機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制的周期解穩(wěn)定性約束條件[13]。

2.2 機(jī)器人魯棒性控制律

根據(jù)運動參數(shù)的融合結(jié)果采用模糊控制方法進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制律設(shè)計，采用非交換微分調(diào)控方程[14]，得到機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸魯棒性控制的邊值周期解向量的分配函數(shù)為：

(17)

當(dāng)雙邊界條件下，若矩陣A滿足A=AH,其中AH=(A*)T,“*”代表求共軛，引入誤差反饋調(diào)節(jié)，得到機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸避障控制參量調(diào)節(jié)的控制修正分配函數(shù)。機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的位姿平衡點位于平面坐標(biāo)系象限時，即(x1,x2≥0)才有實際意義。由Schwartz不等式可知|ρik|≤1，那么對機(jī)器人旋轉(zhuǎn)關(guān)節(jié)問題控制的邊值周期解的插值擬合式為：

(18)

采用模糊控制方法進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制律設(shè)計，模糊控制函數(shù)m(A)滿足：

(19)

(20)

令A(yù)∈Cn×n(n×n維復(fù)數(shù)空間)，采用運動參數(shù)融合方法，得到機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸魯棒控制的最優(yōu)解分別為：

(22)

將機(jī)器人的魯棒控制周期解問題轉(zhuǎn)換為求超定方程的最優(yōu)解問題[15]，收斂性評價規(guī)則如下:

Ni,j= (f(A)i,1?f(B)1,j)⊕(f(A)i,2?f(B)2,j)⊕…⊕(f(A)i,h?f(B)h,j)=

rp+r2p2)modn

(23)

根據(jù)穩(wěn)定性判決條件，得到文中設(shè)計的機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸抗擾動魯棒控制律是漸進(jìn)穩(wěn)定的。

3 仿真實驗與結(jié)果分析

通過仿真測試文中方法在提高機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制中的應(yīng)用性能。采用AD公司出品的MSP430系列超低功耗16位信號傳感器進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸信息采集，設(shè)定PLL_LOCKCNT寄存器，OUTD管腳輸出電壓為0～15 V，對機(jī)器人的運動參數(shù)采集的初始頻率為100 Hz，截止頻率為300 Hz，在小擾動信噪比(SNR)分別為-5 dB、5 dB、20 dB的條件下，得到姿態(tài)參數(shù)融合結(jié)果，如圖1所示。

圖1 機(jī)器人的運動參數(shù)采樣結(jié)果

對采集的姿態(tài)參數(shù)采用Kalman濾波算法進(jìn)行誤差補(bǔ)償和信息融合處理，根據(jù)運動參數(shù)的融合結(jié)果進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸魯棒性控制，得到控制參數(shù)融合結(jié)果輸出，如圖2所示。

圖2 機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的參數(shù)融合輸出

分析圖2得知，采用文中方法進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制的魯棒性較好。測試不同方法進(jìn)行控制的精度，得到的對比結(jié)果如圖3所示。分析圖3得知，該方法進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制的精度較高，抗擾動能力較強(qiáng)。

圖3 控制精度對比

4 結(jié)束語

對機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的準(zhǔn)確、可靠控制是建立在對關(guān)節(jié)的準(zhǔn)確定位和測姿基礎(chǔ)上的，結(jié)合對運動參數(shù)的融合處理，提高對移動機(jī)器人旋轉(zhuǎn)關(guān)鍵軸的可靠控制能力。文中提出一種基于運動參數(shù)融合的機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸魯棒性控制律。構(gòu)建機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的動力學(xué)模型，采用姿態(tài)傳感器進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸的運動姿態(tài)參數(shù)采集，對采集的姿態(tài)參數(shù)采用Kalman濾波算法進(jìn)行誤差補(bǔ)償和信息融合處理，根據(jù)運動參數(shù)的融合結(jié)果采用模糊控制方法進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制律設(shè)計。

仿真結(jié)果表明，利用該方法進(jìn)行機(jī)器人旋轉(zhuǎn)關(guān)節(jié)軸控制的魯棒性較好、精度較高。