示教機(jī)械臂姿態(tài)解算改進(jìn)方法仿真研究

黃 洋，姜文剛

江蘇科技大學(xué) 電子信息學(xué)院，江蘇 鎮(zhèn)江 212003

1 引言

基于慣性傳感器的機(jī)械臂示教系統(tǒng)是通過(guò)采集固定于人體手臂各個(gè)位置的慣性傳感器數(shù)據(jù)，經(jīng)過(guò)姿態(tài)解算與空間位置計(jì)算得到機(jī)械臂姿態(tài)運(yùn)動(dòng)軌跡，其核心在于慣性傳感器解算姿態(tài)角度的精確性。Rehbinder和Hu在其研究姿態(tài)解算算法的時(shí)候，采用了基于MEMS（微機(jī)電傳感器）技術(shù)的陀螺儀和加速度計(jì)傳感器組合的姿態(tài)解算算法，俯仰角和橫滾角得到了一定的校正，但是航向角容易發(fā)生漂移。Bachmann在進(jìn)行三維空間姿態(tài)測(cè)量的研究中加入了磁力計(jì)，由磁力計(jì)數(shù)據(jù)對(duì)航向角進(jìn)行校正。由于姿態(tài)測(cè)量環(huán)境中的隨機(jī)干擾對(duì)MEMS傳感器測(cè)量精度的影響較大，同時(shí)受制于其本身的精度問(wèn)題，導(dǎo)致經(jīng)過(guò)誤差標(biāo)定的MEMS傳感器在采集數(shù)據(jù)以及之后的姿態(tài)解算過(guò)程中不可避免的引起姿態(tài)角較大的偏差，最終導(dǎo)致姿態(tài)數(shù)據(jù)的漂移。

針對(duì)上述問(wèn)題，參考捷聯(lián)慣導(dǎo)相關(guān)算法[1-2]，設(shè)計(jì)了一種采用組合傳感器測(cè)量[3]和組合濾波的姿態(tài)解算方法。使用六個(gè)慣性傳感器單元對(duì)載體坐標(biāo)系的三軸正負(fù)軸向分別測(cè)量加速度，磁場(chǎng)強(qiáng)度和角速度數(shù)據(jù)，采用四元數(shù)法對(duì)姿態(tài)角進(jìn)行求取。將磁羅盤(pán)算法和四元數(shù)估計(jì)法得到的兩組姿態(tài)角的誤差分別作為模糊調(diào)節(jié)器和PI調(diào)節(jié)器的輸入?yún)?shù)，使用互補(bǔ)濾波[4-5]分別修正兩組角速度的輸出數(shù)據(jù)，通過(guò)卡爾曼濾波器[6]減小誤差，得到更精確的姿態(tài)角數(shù)據(jù)。該方法結(jié)合了模糊調(diào)節(jié)的快速性和PI調(diào)節(jié)的精確性，通過(guò)互補(bǔ)濾波來(lái)抑制陀螺儀角速度漂移，保證姿態(tài)解算系統(tǒng)的穩(wěn)定性，在姿態(tài)解算過(guò)程中通過(guò)卡爾曼濾波能有效抑制噪聲[7]，通過(guò)迭代估計(jì)提高姿態(tài)角解算的精度。該方法相較于普通互補(bǔ)濾波解算姿態(tài)的方法更適合于示教機(jī)器人的姿態(tài)解算系統(tǒng)。

2 人體姿態(tài)解算方法原理

人體姿態(tài)變化即人體坐標(biāo)系相對(duì)于基坐標(biāo)系的空間轉(zhuǎn)動(dòng)。假設(shè)人體運(yùn)動(dòng)為剛體運(yùn)動(dòng)，在慣性導(dǎo)航系統(tǒng)中用俯仰角θ，橫滾角γ和航向角ψ作為姿態(tài)角的表示方法。取人體坐標(biāo)系為b系，參考坐標(biāo)系為n系。xnynzn為參考坐標(biāo)系的東北地方向，初始xbybzb與參考坐標(biāo)系三軸重合。經(jīng)過(guò)旋轉(zhuǎn)后參考坐標(biāo)系與人體坐標(biāo)系有如下關(guān)系[8]：

其中Cbn以歐拉角形式表示為：器之間存在的耦合等因素，常規(guī)使用單組MEMS傳感器進(jìn)行姿態(tài)解算的方法難以保證示教機(jī)械臂姿態(tài)角解算的精度和穩(wěn)定性。而采用六組傳感器可以有效減小微傳感器之間的耦合所產(chǎn)生的量測(cè)誤差，組合調(diào)節(jié)能夠結(jié)合模糊調(diào)節(jié)的快速性和PI調(diào)節(jié)的精確性，經(jīng)過(guò)互補(bǔ)濾波對(duì)正負(fù)兩組三軸角速度數(shù)據(jù)同時(shí)調(diào)節(jié)來(lái)抑制陀螺儀漂移，得到更精確的角速度數(shù)據(jù)，EKF（拓展卡爾曼濾波）濾波器可以根據(jù)調(diào)節(jié)后的角速度數(shù)據(jù)進(jìn)行姿態(tài)估計(jì)并抑制噪聲，能夠滿(mǎn)足系統(tǒng)精確性和穩(wěn)定性的要求，因此設(shè)計(jì)了一種采用模糊和PI組合調(diào)節(jié)，互補(bǔ)濾波和EKF組合濾波的多組傳感器姿態(tài)解算方法。

3 算法結(jié)構(gòu)及分析過(guò)程

3.1 算法結(jié)構(gòu)

設(shè)計(jì)的算法總體結(jié)構(gòu)如圖1所示。載體坐標(biāo)系中按坐標(biāo)軸固定的六組傳感器分別測(cè)得各個(gè)軸向上加速度，磁場(chǎng)強(qiáng)度和角速度數(shù)據(jù)，可歸為正負(fù)兩組數(shù)據(jù)。將傳感器測(cè)量數(shù)據(jù)與四元數(shù)法估計(jì)數(shù)據(jù)的向量積分別作為模糊調(diào)節(jié)器和PI調(diào)節(jié)器的輸入，分別與兩組角速度數(shù)據(jù)進(jìn)行互補(bǔ)濾波，然后通過(guò)EKF濾波進(jìn)行四元數(shù)求解，進(jìn)而轉(zhuǎn)化為姿態(tài)角。

由此可以計(jì)算出歐拉角方式下姿態(tài)角數(shù)據(jù)，即：

圖1 算法總體結(jié)構(gòu)框圖

式（3）即為示教機(jī)器人姿態(tài)解算的歐拉角方程。在示教機(jī)器人勻速運(yùn)動(dòng)的過(guò)程中，傳統(tǒng)磁羅盤(pán)算法可以通過(guò)各傳感器數(shù)據(jù)得出姿態(tài)角。載體坐標(biāo)系b下加速度計(jì)測(cè)得加速度ab=[]T，磁力計(jì)測(cè)得磁場(chǎng)強(qiáng)度mb=[]T，考慮東向磁場(chǎng)強(qiáng)度近似為0，參考坐標(biāo)系下加速度與磁場(chǎng)強(qiáng)度an=[0 0 g]T，mn=[0]T，根據(jù)坐標(biāo)變換矩陣可以得到姿態(tài)角數(shù)據(jù)，即：

其中角速度調(diào)節(jié)部分是根據(jù)傳感器量測(cè)誤差，利用模糊調(diào)節(jié)和PI調(diào)節(jié)，根據(jù)互補(bǔ)濾波原理可以調(diào)節(jié)角速度原始數(shù)據(jù)，能夠快速并精確抑制陀螺儀在運(yùn)動(dòng)過(guò)程中的漂移，減小角速度誤差，提高系統(tǒng)穩(wěn)定性。EKF濾波部分是根據(jù)調(diào)節(jié)后得到更精確的角速度數(shù)據(jù)來(lái)構(gòu)建狀態(tài)方程，傳感器量測(cè)數(shù)據(jù)構(gòu)建觀測(cè)方程，利用EKF濾波器進(jìn)行姿態(tài)估計(jì)并抑制噪聲。在循環(huán)迭代的過(guò)程中，得到更精確的四元數(shù)值，能夠有效地減小傳感器量測(cè)誤差對(duì)姿態(tài)解算精度的影響，提高系統(tǒng)精度。

3.2 傳感器組合測(cè)量

由于低成本MEMS傳感器本身的精度局限，陀螺儀的漂移，外界的隨機(jī)干擾和同一傳感器單元中微傳感

設(shè)計(jì)的算法采用按坐標(biāo)軸固定的六組傳感器分別測(cè)得各個(gè)軸向的傳感器數(shù)據(jù)，安裝位置如圖2所示。

圖2 傳感器安裝位置示意圖

由示意圖所示，傳感器1～3可以測(cè)得人體加速度a1=[]T，磁場(chǎng)m1=[]T，角速度ω1=[]T，傳感器4～6可以測(cè)得符號(hào)相反的加速度，磁力和角速度數(shù)據(jù)。根據(jù)坐標(biāo)變換原理可得：

Δax,Δay,Δaz為加速度計(jì)測(cè)量噪聲。由式（7）、（8）可以求出俯仰角θ與橫滾角γ。

與上述方法一致，考慮東向磁場(chǎng)近似為0，可以得到航向角ψ：

Δmx,Δmy,Δmz為磁力計(jì)測(cè)量噪聲。根據(jù)組合方式解算出的歐拉角可以通過(guò)推導(dǎo)得到四元數(shù)[9-10]初值，即：

3.3 角速度調(diào)節(jié)

在實(shí)際應(yīng)用過(guò)程中，由于加速度計(jì)和磁力計(jì)靜態(tài)特性較好，在解算過(guò)程中不會(huì)產(chǎn)生累計(jì)誤差，航時(shí)間的穩(wěn)定性較好，但是短時(shí)間內(nèi)精度較差。陀螺儀動(dòng)態(tài)特性較好，短時(shí)間內(nèi)精度較高，但是在積分過(guò)程中會(huì)產(chǎn)生累計(jì)誤差，長(zhǎng)時(shí)間工作會(huì)引起漂移，最終導(dǎo)致解算方法失效。因此，可以采用互補(bǔ)濾波的方法對(duì)傳感器信息進(jìn)行融合[11]?；パa(bǔ)濾波公式變換可得：

轉(zhuǎn)換到時(shí)域?yàn)椋?/p>

由公式（14）可知互補(bǔ)濾波為通過(guò)磁羅盤(pán)算法和四元數(shù)估計(jì)法得到的姿態(tài)角差值作為輸入量調(diào)節(jié)陀螺儀角速度輸出[12]。由于對(duì)陀螺儀角速度的校正中誤差的計(jì)算通常采用的梯度下降法比較復(fù)雜且計(jì)算量大，因此在低速小范圍運(yùn)動(dòng)中采用向量積法代替來(lái)計(jì)算誤差量[13-14]，即：

其中，eθγ,eψ為加速度計(jì)和磁力計(jì)的測(cè)量向量與四元數(shù)下的觀測(cè)向量的叉乘，an,hn為參考坐標(biāo)系下加速度和磁力單位向量，,為載體坐標(biāo)系下傳感器測(cè)量值單位向量。加速度計(jì)1～3得到，加速度計(jì)4～6得到，磁力計(jì)1～3得到，磁力計(jì)4～6得到，e1=+，e2=+，通過(guò)采用模糊調(diào)節(jié)器[15]對(duì)陀螺儀1～3輸出的角速度進(jìn)行調(diào)節(jié)，誤差e1和其在采樣周期內(nèi)的積分為輸入量，u1為輸出量，則：

通過(guò)PI調(diào)節(jié)器對(duì)陀螺儀4～6輸出的角速度進(jìn)行調(diào)節(jié)，誤差e2為輸入量，u2為輸出量，則：

由于陀螺儀1～3與陀螺儀4～6安裝方向相反，則載體角速度可由均值得到：

3.4 EKF姿態(tài)解算

由四元數(shù)作為狀態(tài)向量，根據(jù)四元數(shù)微分方程可以建立EKF的狀態(tài)方程，即：

其中

其中ω(k)為根據(jù)式（20）得到的調(diào)節(jié)后的角速度數(shù)據(jù)，W(k -1)近似為白噪聲矩陣，Γ(k -1)為噪聲驅(qū)動(dòng)矩陣。

以傳感器1～3測(cè)量的加速度和磁力數(shù)據(jù)的單位向量為觀測(cè)量，建立觀測(cè)方程：

在小范圍手臂示教運(yùn)動(dòng)情況下，可近似認(rèn)為有害加速度趨近于0，磁場(chǎng)無(wú)變化，即：

則可以得到

4 仿真驗(yàn)證

為了驗(yàn)證所設(shè)計(jì)的姿態(tài)解算方法，MATLAB中設(shè)定六組傳感器兩兩相差90°，固定于手臂同一位置，其中加速度計(jì)隨機(jī)誤差為1×10-4g，陀螺儀隨機(jī)誤差為0.1°/h，磁力計(jì)隨機(jī)誤差為50 nt，各傳感器誤差相互獨(dú)立，量測(cè)過(guò)程中互不影響。人體手臂運(yùn)動(dòng)屬于低速小范圍運(yùn)動(dòng)，頻率一般低于15 Hz，則當(dāng)截止頻率 fc為15 Hz時(shí)，K為0.8，采樣周期ts為0.1 s，仿真時(shí)長(zhǎng)30 s。在陀螺儀角速度校正部分，模糊調(diào)節(jié)器設(shè)定誤差，誤差積分和輸出量的量化因子分別為3，1，1。論域分別為[- 6 6]，[-3 3]，[- 9 9]。模糊化過(guò)程采用三角形隸屬度函數(shù)，模糊規(guī)則表根據(jù)經(jīng)驗(yàn)設(shè)計(jì)。PI調(diào)節(jié)器設(shè)定P為0.8，I為0.02?？柭鼮V波器中，根據(jù)文獻(xiàn)[12]的方法取各協(xié)方差矩陣[16]為 Q=0.000 1I3×3，R=0.001I6×6，P0=0.001I4×4。分別采用文獻(xiàn)[11]中PI調(diào)節(jié)方法和設(shè)計(jì)的組合調(diào)節(jié)方法對(duì)姿態(tài)角靜態(tài)和動(dòng)態(tài)情況下姿態(tài)角變化做仿真，得到姿態(tài)角誤差曲線(xiàn)圖，靜態(tài)仿真圖如圖3～5所示。

靜態(tài)情況下，調(diào)節(jié)前后的姿態(tài)角誤差如圖3～5所示。圖3為使用單組傳感器無(wú)調(diào)節(jié)姿態(tài)角誤差，圖4為加入PI調(diào)節(jié)后的姿態(tài)角誤差，圖5為使用多組傳感器，并采用模糊PI組合調(diào)節(jié)的姿態(tài)角誤差，誤差參數(shù)見(jiàn)表1，range表示誤差范圍，std表示誤差標(biāo)準(zhǔn)差。

圖3 無(wú)調(diào)節(jié)靜態(tài)姿態(tài)角誤差

圖4 PI調(diào)節(jié)靜態(tài)姿態(tài)角誤差

圖5 模糊PI組合調(diào)節(jié)靜態(tài)姿態(tài)角誤差

表1 靜態(tài)姿態(tài)角誤差參數(shù)比較

由表1中可以看出，圖4中采用文獻(xiàn)[11]中PI調(diào)節(jié)的方法，姿態(tài)角誤差的標(biāo)準(zhǔn)差從0.507°、0.643°、0.519°降低到0.136°、0.157°、0.206°。圖5中PI調(diào)節(jié)與模糊調(diào)節(jié)進(jìn)行組合后，姿態(tài)角標(biāo)準(zhǔn)差進(jìn)一步變?yōu)?.096°、0.098°、0.142°，姿態(tài)角誤差標(biāo)準(zhǔn)差有進(jìn)一步減小。表明在靜態(tài)情況下，設(shè)計(jì)的算法相比文獻(xiàn)[11]中的算法能夠減小姿態(tài)角誤差和提高系統(tǒng)穩(wěn)定性的效果。

在動(dòng)態(tài)仿真的過(guò)程中，假設(shè)航向角由0°勻速變化為－90°，同時(shí)適當(dāng)增大各傳感器的隨機(jī)誤差，動(dòng)態(tài)仿真圖如圖6所示。

圖6 模糊PI組合調(diào)節(jié)航向角變化曲線(xiàn)

圖6 為航向角變化的曲線(xiàn)，即從0°勻速變化為－90°。從圖中可以看出，雖然航向角在變化過(guò)程中存在微小的誤差波動(dòng)，但是曲線(xiàn)整體變化近似于真實(shí)曲線(xiàn)。

動(dòng)態(tài)情況下，調(diào)節(jié)前后的姿態(tài)角誤差如圖7～9所示。圖7為使用單組傳感器無(wú)調(diào)節(jié)姿態(tài)角誤差，圖8為加入PI調(diào)節(jié)后的姿態(tài)角誤差，圖9為使用多組傳感器，并采用模糊PI組合調(diào)節(jié)的姿態(tài)角誤差，誤差參數(shù)見(jiàn)表2。

圖7 無(wú)調(diào)節(jié)動(dòng)態(tài)姿態(tài)角誤差

圖8 PI調(diào)節(jié)動(dòng)態(tài)姿態(tài)角誤差

圖9 模糊PI組合調(diào)節(jié)動(dòng)態(tài)姿態(tài)角誤差

表2 動(dòng)態(tài)姿態(tài)角誤差參數(shù)比較

從表2中可以看出，圖8中采用文獻(xiàn)[11]中PI調(diào)節(jié)的方法，動(dòng)態(tài)姿態(tài)角誤差的標(biāo)準(zhǔn)差分別從0.792°、0.997°、1.135°降低為0.298°、0.275°、0.608°。圖9中采用模糊PI組合調(diào)節(jié)方法后，姿態(tài)角誤差的標(biāo)準(zhǔn)差進(jìn)一步減小為0.193°、0.188°、0.308°，這表明在航向角動(dòng)態(tài)變化過(guò)程中采用模糊PI組合調(diào)節(jié)的方法相比在單一PI調(diào)節(jié)能進(jìn)一步減小姿態(tài)角誤差，并且提高系統(tǒng)穩(wěn)定性，尤其對(duì)變化的航向角有更好的效果。

組合了模糊調(diào)節(jié)和PI調(diào)節(jié)，融合了互補(bǔ)濾波和卡爾曼濾波的多組傳感器姿態(tài)解算方法對(duì)靜態(tài)和動(dòng)態(tài)情況下姿態(tài)角的誤差消除都有比較好的效果，尤其是在航向角變化的情況下相比使用單組傳感器PI調(diào)節(jié)的姿態(tài)角的精度和穩(wěn)定性有進(jìn)一步的提高，使得航向角變化接近于真值。因此，設(shè)計(jì)的算法在示教機(jī)械臂姿態(tài)解算中能有效提高姿態(tài)解算的精度，提高系統(tǒng)的穩(wěn)定性。

5 結(jié)束語(yǔ)

在傳統(tǒng)加速度計(jì)，磁力計(jì)和陀螺儀信息融合算法的基礎(chǔ)上，將傳感器組增加到六個(gè)，使用六軸向安裝的MEMS傳感器分別測(cè)得各自軸向的傳感器數(shù)據(jù)。以該方法得到的傳感器量測(cè)向量與四元數(shù)法得到的估計(jì)向量的兩個(gè)向量積分別作為模糊調(diào)節(jié)器和PI調(diào)節(jié)器的輸入量，輸出量經(jīng)過(guò)互補(bǔ)濾波分別調(diào)節(jié)兩組角速度向量，取均值后輸入卡爾曼濾波器中，得到更精確的四元數(shù)估計(jì)值，再經(jīng)過(guò)變換得到姿態(tài)角。仿真結(jié)果表明，設(shè)計(jì)的算法在靜態(tài)和動(dòng)態(tài)情況下均能有效提高姿態(tài)角解算精度，并且對(duì)變化的航向角精度和穩(wěn)定性提高有比較好的效果。