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

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

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

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

1　LPMS-B的概述

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

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

圖1　LPMS-B實物圖

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

1.2　LPMS-B的濾波

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

表2　LPMS-B濾波模式

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

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

1.4　LpmsControl的介紹

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

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

2　LPMS-B的測試分析

為了驗證LpmsControl軟件獲取傳感器采集得到數(shù)據(jù)的正確性，需要對傳感器隨機采集到的數(shù)據(jù)進行驗證，也就是驗證基于四元數(shù)求得的轉(zhuǎn)換矩陣T和基于歐拉角求得的轉(zhuǎn)換矩陣C是否近似相等。驗證結(jié)果如圖3所示（取兩轉(zhuǎn)換矩陣其中一列數(shù)值作差得到的結(jié)果作圖）。可以看出該傳感器獲得的數(shù)據(jù)是有效的。

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

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

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

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

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

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

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

對矢量a→歸一化

■

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

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

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

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

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

圖7　現(xiàn)場試驗

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

3.1　機器人單軸轉(zhuǎn)動實驗

實驗將對歐拉角進行測量，分別試驗5次。

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

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

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

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

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

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

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

3.2　機器人多軸聯(lián)動實驗

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

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

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

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

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

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

3.3　三種算法的比較分析

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

表3　三種算法比較

4　結(jié)束語

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

參考文獻：

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

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

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

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