九軸無(wú)線姿態(tài)傳感器（LPMS-B）檢測(cè)分析及應(yīng)用

黃悅峰，王 榜，張啟鵬，朱婉瑩，李 標(biāo)

（廣西制造系統(tǒng)與先進(jìn)制造技術(shù)重點(diǎn)實(shí)驗(yàn)室廣西大學(xué)機(jī)械工程學(xué)院，廣西 南寧530004）

信息時(shí)代的關(guān)鍵在于信息獲取，信息獲取關(guān)鍵在于傳感器。姿態(tài)信息獲取、姿態(tài)檢測(cè)具有廣泛和重要的應(yīng)用范圍，在無(wú)人機(jī)的慣性測(cè)量系統(tǒng)、車輛船舶傾斜角的測(cè)量、機(jī)器人運(yùn)動(dòng)的測(cè)量等都有著重要的應(yīng)用。目前，在姿態(tài)測(cè)量組合方面，比較成熟的有加速度計(jì)、陀螺儀和磁強(qiáng)計(jì)組合，現(xiàn)有的慣性姿態(tài)測(cè)量系統(tǒng)主要采用這種組合方式。隨著新材料，新原理，新技術(shù)的不斷出現(xiàn)，姿態(tài)測(cè)量組合逐漸向小型化、低成本、高精度的方向發(fā)展。國(guó)外對(duì)于MEMS（MEMS，Micro-Electro-Mechanical System）的研究較早，20世紀(jì)80年代在加拿大、韓國(guó)、日本以及澳大利亞等國(guó)家都開(kāi)展了深入的研究，此外，一些高校和企業(yè)投入大量的精力物力開(kāi)展 MEMS 研究[1，2，3]，都取得了不錯(cuò)的研究成果。許多高技術(shù)公司，例如：美國(guó)J.F.Lehman&Company公司[3]的最新產(chǎn)品 SiNAV型組合導(dǎo)航系統(tǒng)采用MIMU/GPS緊耦合方案，定位誤差小于10 m，速度誤差小于0.1 m/s，可耐受20 000 g的沖擊，陀螺測(cè)量范圍可達(dá)±14 000（°）/s.德國(guó) Litef公司[2]B-290 硅加速度計(jì)，量程為10 g，標(biāo)度因數(shù)穩(wěn)定性為3×10-4，偏置穩(wěn)定性為250×10-6g，已經(jīng)與光纖陀螺組合成IMU.國(guó)內(nèi)對(duì)于姿態(tài)傳感器檢測(cè)技術(shù)的發(fā)展起步較晚，和發(fā)達(dá)國(guó)家相比存在較大差距，但一些高校和相關(guān)研究部門(mén)也取得了很大進(jìn)步。特別是清華大學(xué)、北京航空航天大學(xué)、哈爾濱工業(yè)大學(xué)和哈爾濱工程大學(xué)以及中國(guó)時(shí)代電子集團(tuán)[3]等都對(duì)姿態(tài)傳感器檢測(cè)相關(guān)技術(shù)進(jìn)行了大量研究，而且取得顯著研究成果。

LPMS-B作為姿態(tài)傳感器當(dāng)中的一種，可以測(cè)量三個(gè)全局坐標(biāo)軸的360°方向。它的測(cè)量是以數(shù)字方式進(jìn)行的，并以定向四元數(shù)或歐拉角的形式傳輸?shù)綌?shù)據(jù)分析系統(tǒng)。而歐拉角是描述物體取向的傳統(tǒng)方式，四元數(shù)允許定向測(cè)量而不會(huì)遇到所謂的萬(wàn)向節(jié)鎖定，通過(guò)使用四元向量來(lái)表示圍繞所有軸的取向而不受奇點(diǎn)的限制[4]。LPMS-B應(yīng)用范圍廣泛，其中小型運(yùn)動(dòng)傳感器的應(yīng)用領(lǐng)域之一就是人體運(yùn)動(dòng)損傷康復(fù)測(cè)量，步態(tài)周期分析，手術(shù)技能訓(xùn)練和評(píng)估等。該傳感器還可以應(yīng)用于虛擬現(xiàn)實(shí)，導(dǎo)航，機(jī)器人或者是測(cè)量車輛動(dòng)態(tài)性能。本文首先對(duì)九軸無(wú)線姿態(tài)傳感器（LPMS-B）的基本功能進(jìn)行介紹，然后對(duì)其性能進(jìn)行測(cè)試分析，最后把LPMS-B應(yīng)用于機(jī)器人末端姿態(tài)測(cè)試。試驗(yàn)結(jié)果表明，對(duì)于單軸轉(zhuǎn)動(dòng)，改進(jìn)的四元數(shù)互補(bǔ)濾波數(shù)據(jù)融合算法更能精確檢測(cè)機(jī)器人的末端姿態(tài)，對(duì)于多軸聯(lián)動(dòng)，由于各軸的相互影響，使得各算法之間的差異不明顯。

1　LPMS-B的概述

1.1　LPMS-B的基本參數(shù)

目前市場(chǎng)上主要有兩種不同的LPMS-B傳感器封裝，分別命名為L(zhǎng)PMS-B標(biāo)準(zhǔn)版和OEM版。其實(shí)物圖如圖1所示，主要參數(shù)如表1所示。

圖1　LPMS-B實(shí)物圖

表1　LPMS-B（標(biāo)準(zhǔn)版）和LPMS-B（OEM版）主要參數(shù)表

1.2　LPMS-B的濾波

來(lái)自三個(gè)MEMS傳感器的數(shù)據(jù)通過(guò)擴(kuò)展互補(bǔ)卡爾曼濾波器（LP-Filter）進(jìn)行組合，以計(jì)算方位數(shù)據(jù)，如四元數(shù)和歐拉角。為了使濾波器正常工作，需要以適當(dāng)?shù)姆绞皆O(shè)置其參數(shù)。首先，需要選擇濾波器的模式，可以通過(guò)LpmsControl軟件或固件命令來(lái)設(shè)置，濾波模式分為：陀螺儀（Gyr）、陀螺儀+加速度計(jì)（Gyr+Acc）、陀螺儀+加速度計(jì)+磁強(qiáng)計(jì)（Gyr+Acc+Mag）、加速度計(jì)+磁強(qiáng)計(jì)（Acc+Mag）。各功能如表2所示。

表2　LPMS-B濾波模式

1.3　LPMS-B的校準(zhǔn)方法

LPMS-B的校準(zhǔn)包括陀螺儀、加速度計(jì)和磁力計(jì)這三個(gè)傳感器的校準(zhǔn)。當(dāng)傳感器靜止時(shí)，陀螺儀的輸出數(shù)據(jù)應(yīng)該在零附近，來(lái)自陀螺儀傳感器的原始數(shù)據(jù)具有一定值的恒定偏差。要校準(zhǔn)這個(gè)偏差，需要通過(guò)固件命令或使用LpmsControl軟件觸發(fā)陀螺儀校準(zhǔn)程序，在校準(zhǔn)的過(guò)程中，陀螺儀閾值也將被調(diào)整。在磁力計(jì)校準(zhǔn)過(guò)程中，要確定幾個(gè)參數(shù)：磁力計(jì)偏置和X，Y和Z軸上的增益；地磁場(chǎng)矢量的長(zhǎng)度和方向。由于地球磁場(chǎng)受電源線、金屬等電磁噪聲源的影響，會(huì)使得磁場(chǎng)變得偏心和變形，因此在準(zhǔn)期間，計(jì)算去中心和變形的量以及磁場(chǎng)矢量的平均長(zhǎng)度。加速度計(jì)相對(duì)于LPMS-B外殼的錯(cuò)位是由錯(cuò)位矩陣來(lái)表示的，LpmsControl軟件不僅可以對(duì)錯(cuò)位矩陣進(jìn)行校準(zhǔn)，還可以評(píng)估加速度計(jì)的偏移量。

1.4　LpmsControl的介紹

LpmsControl應(yīng)用程序可以控制LPMS-B傳感器的各方面。其功能有：通過(guò)一個(gè)藍(lán)牙接收器可以連接7個(gè)傳感器、調(diào)整傳感器參數(shù)（傳感器范圍等）、重置方向和參考矢量、啟動(dòng)陀螺儀和磁力計(jì)校準(zhǔn)、以線形圖或3D立方體的形式實(shí)時(shí)顯示采集的數(shù)據(jù)、將傳感器的數(shù)據(jù)記錄到CSV數(shù)據(jù)文件中。上位機(jī)應(yīng)用程序窗口，其中包括采用頻率設(shè)置，頻率范圍為5.75 Hz～133 Hz；陀螺儀、加速度計(jì)以及磁強(qiáng)計(jì)的變化范圍分別為：250 dps～2 000 dps、2 G～16 G 以及 130 uT～810 uT.LPMS-B上位機(jī)界面如圖2所示。

圖2　LpmsControl應(yīng)用程序窗口

2　LPMS-B的測(cè)試分析

為了驗(yàn)證LpmsControl軟件獲取傳感器采集得到數(shù)據(jù)的正確性，需要對(duì)傳感器隨機(jī)采集到的數(shù)據(jù)進(jìn)行驗(yàn)證，也就是驗(yàn)證基于四元數(shù)求得的轉(zhuǎn)換矩陣T和基于歐拉角求得的轉(zhuǎn)換矩陣C是否近似相等。驗(yàn)證結(jié)果如圖3所示（取兩轉(zhuǎn)換矩陣其中一列數(shù)值作差得到的結(jié)果作圖）?？梢钥闯鲈搨鞲衅鳙@得的數(shù)據(jù)是有效的。

圖3　轉(zhuǎn)換矩陣T與C的誤差

2.1　陀螺儀的數(shù)據(jù)測(cè)試與處理

一般陀螺儀在靜態(tài)采集數(shù)據(jù)時(shí)，隨時(shí)間的推移會(huì)產(chǎn)生較大的漂移和累積誤差，這種情況嚴(yán)重影響了其測(cè)量精度，但是在動(dòng)態(tài)時(shí)，產(chǎn)生的漂移和累積誤差會(huì)相對(duì)減小。在這里將采用對(duì)照實(shí)驗(yàn)，分別測(cè)量靜態(tài)和動(dòng)態(tài)時(shí)陀螺儀yaw角的變化，一組是將LPMS-B傳感器至于水平桌面上，z軸向上，采用陀螺儀（Gyr Only）濾波模式，采樣頻率 100 Hz，時(shí)間為 60 s.另一組則把LPMS-B傳感器至于轉(zhuǎn)椅上，z軸向上，測(cè)量過(guò)程中轉(zhuǎn)椅轉(zhuǎn)動(dòng)一定角度，采用陀螺儀（Gyr Only）濾波模式，采樣頻率100 Hz，時(shí)間為60 s.圖4和圖5分別是靜態(tài)和動(dòng)態(tài)下的測(cè)量結(jié)果。從圖4的趨勢(shì)來(lái)看，隨著時(shí)間的推移，漂移會(huì)越來(lái)越嚴(yán)重，累積誤差則逐漸變大。由圖5可以看出，陀螺儀動(dòng)態(tài)測(cè)量時(shí)依舊存在著漂移，但漂移程度有所減小。這說(shuō)明動(dòng)態(tài)性較能好，但是單獨(dú)采用陀螺儀進(jìn)行數(shù)據(jù)測(cè)量，還是會(huì)存在較大的誤差。

圖4　陀螺儀濾波模式下靜態(tài)測(cè)量航向角

圖5　陀螺儀濾波模式下動(dòng)態(tài)測(cè)量航向角

2.2　加速度計(jì)的數(shù)據(jù)測(cè)試與處理

加速度計(jì)可以通過(guò)計(jì)算其傾角來(lái)檢測(cè)傳感器安裝是否水平，這對(duì)加速計(jì)采集數(shù)據(jù)的準(zhǔn)確性有著重要的影響作用。加速度計(jì)獲得的三軸加速度分別是ax，ay，az，三軸加速度矢量和為a→,可得：

對(duì)矢量a→歸一化

■

由式（2）得到當(dāng)前時(shí)刻的歸一化重力方向矢量R→acc.

由歸一化矢量方向可得到重力矢量同坐標(biāo)軸的夾角 θx，θy，θz如圖 6 所示。

圖6　重力矢量轉(zhuǎn)換坐標(biāo)

3　機(jī)器人末端姿態(tài)測(cè)試的應(yīng)用

實(shí)驗(yàn)檢測(cè)末端姿態(tài)精度的機(jī)器人名稱為OTC機(jī)器人，實(shí)驗(yàn)平臺(tái)采用的傳感器類型為九軸無(wú)線姿態(tài)傳感器，采用（Gyr+Acc+Mag）濾波模式?，F(xiàn)場(chǎng)實(shí)驗(yàn)圖如圖7所示。

圖7　現(xiàn)場(chǎng)試驗(yàn)

本實(shí)驗(yàn)將從機(jī)器人的單軸轉(zhuǎn)動(dòng)和多軸聯(lián)動(dòng)采集數(shù)據(jù)，然后通過(guò)更新的四元數(shù)算法、卡爾曼濾波數(shù)據(jù)融合算法和互補(bǔ)濾波數(shù)據(jù)融合算法進(jìn)行姿態(tài)解算，對(duì)解算的結(jié)果進(jìn)行分析和比較。

3.1　機(jī)器人單軸轉(zhuǎn)動(dòng)實(shí)驗(yàn)

實(shí)驗(yàn)將對(duì)歐拉角進(jìn)行測(cè)量，分別試驗(yàn)5次。

（1）測(cè)橫滾角：機(jī)器人末端執(zhí)行器繞傳感器X軸勻速旋轉(zhuǎn)，通過(guò)示教盒將其旋轉(zhuǎn)角度為20°左右。

（2）測(cè)俯仰角：機(jī)器人末端執(zhí)行器繞傳感器Y軸勻速旋轉(zhuǎn)，通過(guò)示教盒將其旋轉(zhuǎn)角度為20°左右。

（3）測(cè)航向角：機(jī)器人末端執(zhí)行器繞傳感器Z軸勻速旋轉(zhuǎn)，通過(guò)示教盒將其旋轉(zhuǎn)角度為20°左右。

對(duì)于機(jī)器人單軸轉(zhuǎn)動(dòng)采集到的數(shù)據(jù)，利用改進(jìn)的高斯牛頓算法、改進(jìn)的四元數(shù)卡爾曼濾波數(shù)據(jù)融合算法和改進(jìn)的四元數(shù)互補(bǔ)濾波數(shù)據(jù)融合算法進(jìn)行解算，結(jié)果如圖8、圖9、圖10所示。由于機(jī)器人運(yùn)動(dòng)平穩(wěn)，無(wú)出現(xiàn)較為明顯的抖動(dòng)現(xiàn)象，歐拉角曲線變化平滑，能很好的檢測(cè)出機(jī)器人末端姿態(tài)精度，從圖中可以看出基于改進(jìn)的四元數(shù)互補(bǔ)濾波數(shù)據(jù)融合算法更接近于實(shí)際的測(cè)量曲線。

圖8　單軸轉(zhuǎn)動(dòng)三種算法比較（roll角）

圖9　單軸轉(zhuǎn)動(dòng)三種算法比較（pitch角）

圖10　單軸轉(zhuǎn)動(dòng)三種算法比較（yaw角）

3.2　機(jī)器人多軸聯(lián)動(dòng)實(shí)驗(yàn)

（1）測(cè)橫滾角和俯仰角：傳感器坐標(biāo)系和機(jī)器人基座標(biāo)系重合，多軸聯(lián)動(dòng)使橫滾角和俯仰角轉(zhuǎn)動(dòng)角度均為 20°（航向角為 20°）。

（2）測(cè)量航向角：傳感器坐標(biāo)系和機(jī)器人基座標(biāo)系重合，多軸聯(lián)動(dòng)使航向角轉(zhuǎn)動(dòng)角度為20°（橫滾角和航向角均為20°）。

對(duì)于機(jī)器人多軸聯(lián)動(dòng)采集到的數(shù)據(jù)，利用改進(jìn)的高斯牛頓算法、改進(jìn)的四元數(shù)卡爾曼濾波數(shù)據(jù)融合算法和改進(jìn)的四元數(shù)互補(bǔ)濾波數(shù)據(jù)融合算法進(jìn)行解算，結(jié)果如圖11、圖12、圖13所示。從圖中可以看出，對(duì)于多軸聯(lián)動(dòng)，由于多個(gè)軸間的相互影響，出現(xiàn)較為明顯的抖動(dòng)現(xiàn)象，使得歐拉角的累積誤差增加，歐拉角曲線變化出現(xiàn)較大波動(dòng)，使得各算法之間的效果不明顯。

圖11　多軸聯(lián)動(dòng)三種算法比較（roll角）

圖12　多軸聯(lián)動(dòng)三種算法比較（pitch角）

圖13　多軸聯(lián)動(dòng)三種算法比較（yaw角）

3.3　三種算法的比較分析

如表3為對(duì)單軸轉(zhuǎn)動(dòng)和多軸聯(lián)動(dòng)三種算法的比較，單軸轉(zhuǎn)動(dòng)時(shí)，采用改進(jìn)的四元數(shù)互補(bǔ)濾波數(shù)據(jù)融合算法解算末端橫滾角、俯仰角和航向角的均方根誤差值分別為 0.145°、0.180°和 0.195°；多軸聯(lián)動(dòng)時(shí)，采用改進(jìn)的四元數(shù)互補(bǔ)濾波數(shù)據(jù)融合算法解算末端橫滾角、俯仰角和航向角的均方根誤差值分別為0.343°、0.239°和 0.610°.這說(shuō)明，對(duì)于單軸轉(zhuǎn)動(dòng)時(shí)，由于運(yùn)動(dòng)平穩(wěn)，更夠更好的檢測(cè)機(jī)器人的末端姿態(tài)，而對(duì)于多軸聯(lián)動(dòng)，由于俯仰角受到的干擾程度比其他橫滾角、航向角干擾小，所以精度較高為0.239°，而航向角受到的干擾最大，均方根誤差值最大為0.610°.

表3　三種算法比較

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

通過(guò)三種算法分別對(duì)九軸無(wú)線姿態(tài)傳感器（LPMS-B）進(jìn)行檢測(cè)，試驗(yàn)結(jié)果表明，對(duì)于單軸轉(zhuǎn)動(dòng)，由于運(yùn)動(dòng)平穩(wěn)，基于改進(jìn)的四元數(shù)互補(bǔ)濾波數(shù)據(jù)融合算法效果更好，更能精確檢測(cè)機(jī)器人的末端姿態(tài)，但是對(duì)于多軸聯(lián)動(dòng)，由于多個(gè)軸間的相互影響，出現(xiàn)較為明顯的抖動(dòng)現(xiàn)象，使得歐拉角的累積誤差增加，歐拉角曲線變化出現(xiàn)較大波動(dòng)，使得各算法之間的效果不明顯。在研究的過(guò)程中，也存在著一些不足，機(jī)器人多軸聯(lián)動(dòng)時(shí)，由于機(jī)器人各個(gè)軸相互影響產(chǎn)生累積誤差，使得采用LPMS-B進(jìn)行精度檢測(cè)時(shí)出現(xiàn)較大的誤差，在姿態(tài)解算過(guò)程如何采用誤差補(bǔ)償提高姿態(tài)檢測(cè)精度這一問(wèn)題沒(méi)能解決。

參考文獻(xiàn)：

[1]李小朋.微慣性組合的抗磁干擾和溫度補(bǔ)償方法研究[D].上海：上海交通大學(xué)，2011.

[2]姚成凱.基于嵌入式的MEMS加速度傳感器測(cè)控仿真系統(tǒng)[D].哈爾濱：哈爾濱工程大學(xué)，2008.

[3]張 英.MEMS航天慣導(dǎo)產(chǎn)品及技術(shù)發(fā)展簡(jiǎn)介[J].航天標(biāo)準(zhǔn)化，2010（3）：65-68.

[4]盛慶軒.MIMU/磁強(qiáng)計(jì)航姿參考系統(tǒng)研究[D].長(zhǎng)沙：國(guó)防科學(xué)技術(shù)大學(xué)，2009.