九軸無線姿態傳感器（LPMS-B）檢測分析及應用

黃悅峰，王 榜，張啟鵬，朱婉瑩，李 標

（廣西制造系統與先進制造技術重點實驗室廣西大學機械工程學院，廣西 南寧530004）

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

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

1　LPMS-B的概述

1.1　LPMS-B的基本參數

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

圖1　LPMS-B實物圖

表1　LPMS-B（標準版）和LPMS-B（OEM版）主要參數表

1.2　LPMS-B的濾波

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

表2　LPMS-B濾波模式

1.3　LPMS-B的校準方法

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

1.4　LpmsControl的介紹

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

圖2　LpmsControl應用程序窗口

2　LPMS-B的測試分析

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

圖3　轉換矩陣T與C的誤差

2.1　陀螺儀的數據測試與處理

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

圖4　陀螺儀濾波模式下靜態測量航向角

圖5　陀螺儀濾波模式下動態測量航向角

2.2　加速度計的數據測試與處理

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

對矢量a→歸一化

■

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

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

圖6　重力矢量轉換坐標

3　機器人末端姿態測試的應用

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

圖7　現場試驗

本實驗將從機器人的單軸轉動和多軸聯動采集數據，然后通過更新的四元數算法、卡爾曼濾波數據融合算法和互補濾波數據融合算法進行姿態解算，對解算的結果進行分析和比較。

3.1　機器人單軸轉動實驗

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

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

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

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

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

圖8　單軸轉動三種算法比較（roll角）

圖9　單軸轉動三種算法比較（pitch角）

圖10　單軸轉動三種算法比較（yaw角）

3.2　機器人多軸聯動實驗

（1）測橫滾角和俯仰角：傳感器坐標系和機器人基座標系重合，多軸聯動使橫滾角和俯仰角轉動角度均為 20°（航向角為 20°）。

（2）測量航向角：傳感器坐標系和機器人基座標系重合，多軸聯動使航向角轉動角度為20°（橫滾角和航向角均為20°）。

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

圖11　多軸聯動三種算法比較（roll角）

圖12　多軸聯動三種算法比較（pitch角）

圖13　多軸聯動三種算法比較（yaw角）

3.3　三種算法的比較分析

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

表3　三種算法比較

4　結束語

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

參考文獻：

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

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

[3]張 英.MEMS航天慣導產品及技術發展簡介[J].航天標準化，2010（3）：65-68.

[4]盛慶軒.MIMU/磁強計航姿參考系統研究[D].長沙：國防科學技術大學，2009.