基于北斗/iNEMO慣性模塊的機器人運動姿態(tài)解算方法

陳 凱，王廣龍，王 成，薄洪波

(1.66008部隊，天津 300250；2.軍械工程學院 納米技術與微系統(tǒng)實驗室，石家莊 050003)

基于北斗/iNEMO慣性模塊的機器人運動姿態(tài)解算方法

陳 凱1，王廣龍2，王 成2，薄洪波1

(1.66008部隊，天津 300250；2.軍械工程學院 納米技術與微系統(tǒng)實驗室，石家莊 050003)

基于北斗/iNEMO慣性模塊研究了一種組合式機器人運動姿態(tài)測量系統(tǒng)和解算方法;設計了以ARM為核心的嵌入式組合姿態(tài)解算平臺，通過四元數(shù)法和卡爾曼濾波技術，對iNEMO慣性組件測量數(shù)據(jù)進行融合，進而在高動態(tài)環(huán)境中實時解算出機器人的運動姿態(tài);同時，以北斗接收機輸出的1PPS上升沿脈沖作為數(shù)據(jù)融合的同步信號，并利用其輸出的導航信息輔助iNEMO慣性模塊實現(xiàn)精對準;測試結果驗證了設計方案的可行性和正確性，為機器人姿態(tài)解算提供了一種高精度、高穩(wěn)定性、小體積、低功耗的解決方案。

姿態(tài)解算；機器人；慣性組件；北斗系統(tǒng)；初始對準

0 引言

移動機器人是一個集環(huán)境感知、動態(tài)決策與規(guī)劃、行為控制與執(zhí)行等多功能于一體的綜合系統(tǒng)，是目前科學技術發(fā)展最活躍的領域之一[1]。它可以協(xié)助或取代人類的工作，已經廣泛地應用于地質勘查、軍事偵察、災難救援等領域[2]。姿態(tài)檢測作為機器人研究領域一個重要的組成部分，對于評定機器人的運動特性和姿態(tài)反饋控制具有非常重要的意義[3]。

目前，機器人的姿態(tài)檢測方法按照接觸方式分為間接檢測法和直接檢測法兩種。間接檢測法主要是使用超聲波、激光或者CCD攝像機進行姿態(tài)測量，需要在機器人上安裝多個外部檢測元件，結構復雜，成本較高，不適合微小型機器人。直接檢測法主要是利用陀螺儀、加速度計以及磁強計等慣性敏感元器件和計算機，實時測量運載體的空間位置、姿態(tài)和重力場等參數(shù)，慣性測量的好處是響應速度快、成本低，隨著MEMS技術的發(fā)展和對功耗、體積的要求，低成本的微型慣性組件已經大量的在機器人的姿態(tài)測量中使用[4-6]。慣性系統(tǒng)在姿態(tài)解算前必須對其進行初始對準，對準的計算值將作為姿態(tài)解算的初始值，因此它的精度將直接影響姿態(tài)解算的精度。但低成本的慣性器件由于誤差較大，即使在靜基座下也無法準確辨識地球自轉速率等重要信息，需要使用外部信息輔助動態(tài)對準[7]。

為解決對機器人運動姿態(tài)進行動態(tài)檢測的問題，本文基于北斗/iNEMO慣性模塊，設計了以ARM為核心的嵌入式組合姿態(tài)解算平臺。在此基礎上，對實時姿態(tài)解算和初始對準的算法進行了研究。最后通過試驗，驗證了硬件和算法設計的有效性。

1 硬件系統(tǒng)總體設計

硬件采用ARM為核心的組合姿態(tài)解算平臺，可采集iNEMO慣性模塊輸出量，接收北斗模塊輸出信號，完成姿態(tài)數(shù)據(jù)的解算、融合處理和存儲，系統(tǒng)結構如圖1所示。其中，iNEMO慣性模塊采用的是ST(意法半導體)公司的LSM9DS1芯片，這最一種基于MEMS工藝的9軸運動位置傳感器模塊，內置一個3軸加速度計、一個3軸陀螺儀和一個3軸磁強計。北斗接收機采用的和芯星通公司的UM220-III N芯片，是一種雙系統(tǒng)高性能的GNSS 模塊，能夠同時支持 BD2 B1、GPS L1 兩個頻點。主處理器選用的是ST公司的ARM芯片STM32F405，有較強的數(shù)字信號處理能力和多任務管理能力，有較多的應用接口。為了避免處理頻繁的響應中斷，協(xié)處理單元由ARM芯片STM32F103和FPGA芯片XC6SLX75共同構成。整個系統(tǒng)以無線指令模塊的高電平輸出作為測試的啟動指令。測試開始后，協(xié)處理器ARM從北斗接收機輸出的NEMA0813信息中提取姿態(tài)解算所需的UTC時間、位置、速度、定位狀態(tài)等信息并暫存入FPGA中；FPGA內包含一個雙口RAM存儲器和一個高精度的計數(shù)器，其中計數(shù)器以北斗接收機有效輸出時的1PPS上升沿脈沖作為清零信號；主處理器ARM根據(jù)FPGA輸出的上升沿信號產生中斷，通過FSMC總線、兩路SPI總線分別讀取FPGA內的北斗導航信息、iNEMO慣性模塊的加速度/角速度和地磁信息，利用這些信息完成姿態(tài)數(shù)據(jù)的解算，將結果存入FLASH芯片中。同時，為防止出現(xiàn)時序的競爭和冒險，并協(xié)調各個器件的工作，系統(tǒng)以有源晶振的輸出作為唯一的頻率基準。

圖1 硬件系統(tǒng)總體框圖

2 實時姿態(tài)解算方法

由于iNEMO慣性模塊的輸出數(shù)據(jù)速率遠高于北斗接收機數(shù)據(jù)更新率，因此機器人姿態(tài)的實時解算主要針對iNEMO慣性模塊(陀螺儀、加速度計、磁強計)的組合系統(tǒng)。其中，陀螺儀、加速度計、磁強計都能檢測到載體的姿態(tài)角，但由于器件自身的局限性和外界環(huán)境的干擾，單一慣性器件無法實現(xiàn)對載體姿態(tài)角準確而完整的檢測，因此必須將這幾種慣性傳感器的測量數(shù)據(jù)進行融合。本文實時姿態(tài)解算的總體思想是，由陀螺儀單獨定姿、加速度計和磁強計組合定姿分別解算出姿態(tài)角，采用一維卡爾曼濾波對兩類輸出的三個方位的姿態(tài)角數(shù)據(jù)分別進行互補融合，進而得到更精確的姿態(tài)角，如圖2所示。

圖2 實時姿態(tài)解算原理

2.1 iNEMO陀螺儀解算歐拉角

(1)

式中，ψ∈[-π,π]為方位角， θ∈[-π/2,π/2]為俯仰角，γ∈[-π,π]為橫滾角，這三個角稱為載體的歐拉角，用于表示載體的運動姿態(tài)，此處采用四元數(shù)法求解歐拉角。

設載體坐標系相對于導航坐標系的轉動四元數(shù)為：

(2)

iNEMO陀螺儀x、y、z軸的角速度輸出為(ωx、ωy、ωz)，采樣周期為T，采用一階龍格-庫塔法更新四元數(shù)：

(3)

(4)

若用四元數(shù)Q表示載體坐標系相對于導航坐標系的變換，即：

(5)

(6)

2.2iNEMO加速度計、磁強計解算歐拉角

iNEMO加速度計x、y、z軸的線性加速度輸出為(ax、ay、az)，則可估算俯仰角θ2、橫滾角γ2：

(7)

(8)

iNEMO磁強計x、y、z軸的磁場強度輸出為(mx、my、mz)，水平狀態(tài)下可以估算出方位角的主值：

(9)

機器人在運動過程中由于顛簸或者上下坡度而不會總處于水平位置上，此處采用幾何旋轉法，通過姿態(tài)矩陣建立載體坐標系下iNEMO慣性模塊測得的重力矢量和磁場強度矢量的對應方程，用加速計估算出的俯仰角θ、橫滾角γ的值對磁強計輸出的進行傾角補償：

(10)

(11)

由于地球的地理北向與磁北方向之間存在一個磁偏角，經過磁偏角修正，就可以得到載體真實的方位角ψ2。

iNEMO加速度計和磁強度計的估算值都通過泰勒級數(shù)的展開完成解算。

2.3 卡爾曼濾波的姿態(tài)數(shù)據(jù)融合

由于陀螺儀存在溫度漂移，積分運算的累積誤差會迅速放大，因此陀螺儀解算出的歐拉角必須進一步融合加速度計和磁強計的數(shù)據(jù)。以方位角為例， 陀螺儀解算出ψ1、加速度和磁強計組合解算出ψ2，對應的方差δ1和δ2。為減小計算量，采用一維卡爾曼(Kalman)濾波器，方位角的最佳估計值ψ為[8]：

(12)

其方差為：

(13)

3 北斗輔助iNEMO慣性模塊精對準

機器人姿態(tài)解算時，首先要對系統(tǒng)進行初始化，完成測量平臺的導航數(shù)據(jù)初始對準工作，消除慣性模塊的初始誤差，并為姿態(tài)解算賦予初始值，為解算的迭代過程打下基礎。初始對準即在最短的時間內以一定的精度確定從載體坐標系到導航坐標系的初始姿態(tài)變換矩陣[13]。

對準分兩步進行，粗對準和精對準。在粗對準階段，將iNEMO加速度計、磁強計輸出估算的歐拉角代入方程(1)，即可得到粗對準階段的初始姿態(tài)矩陣。粗對準可以縮短對準的時間，但對準的精度較低，需要精對準進一步提高對準精度。在精對準階段，本文采用組合對準的方式，應用拓展卡爾曼濾波方法，從慣導參數(shù)相對于北斗導航系統(tǒng)提供的參數(shù)的偏差中估計出慣性模塊的誤差量，并進行校正，用實時更新的姿態(tài)矩陣替代粗對準階段得到的姿態(tài)矩陣。

為有效地提高計算精度和效率，此處直接采用北斗系統(tǒng)參照的地球坐標系(e系)實現(xiàn)對準。將北斗的導航解直接作為卡爾曼濾波器的輸入，這是一種松組合的數(shù)據(jù)融合方法，因而其數(shù)學模型并不需要包含北斗狀態(tài)方程。由三軸位置誤差δre、速度誤差δve、姿態(tài)誤差φ，慣性模塊三軸加速度計偏置誤差▽b、陀螺儀漂移εb共同組成15維的系統(tǒng)狀態(tài)向量：

(16)

設理想的地球坐標系為e系(簡稱地球系)，計算的地球坐標系為c系(簡稱計算系)。地球系的位置誤差由于不涉及坐標系統(tǒng)轉換，其微分形式即是速度誤差，比較簡單。但慣性元件存在測量誤差會導致兩個坐標系之間存在一個轉動誤差，即平臺歐拉誤差角，需要予以考慮。短時間內，三軸陀螺儀漂移、加速度計誤差可認為為常值，不隨時間變化，其微分為0。在地球坐標系下，任意失準角情況的系統(tǒng)狀態(tài)誤差微分形式為[7]：

(17)

則系統(tǒng)的狀態(tài)方程可寫為：

(18)

式中，F(xiàn)(15×15)為系統(tǒng)的狀態(tài)轉移矩陣，G為系統(tǒng)的動態(tài)噪聲矩陣，W為系統(tǒng)的過程白噪聲矢量。

在慣性模塊與北斗的松組合模式中，一般取速度和位置為觀測信息。但在靜止與勻速運動狀態(tài)下，系統(tǒng)的方位角誤差不可觀測。因此，本系統(tǒng)在選取北斗和慣性模塊中輸出的位置和速度之差的基礎上，將慣性模塊中陀螺儀與磁強計輸出的方位角之差作為觀測量，構造量測方程：

(19)

式中，rI和vI為慣性模塊輸出的載體3維速度和位置信息，rB、vB為北斗接收機輸出的載體3維速度和位置信息，ψ1、ψ2分別為慣性模塊中陀螺儀與磁強計輸出的方位角。H為量測轉移矩陣，V為量測白噪聲矩陣。

將狀態(tài)方程(18)和量測方程(19)離散化， 即可利用拓展卡爾曼濾波，通過時間更新和狀態(tài)更新，反饋修正姿態(tài)信息，完成姿態(tài)數(shù)據(jù)的初始對準。

4 試驗測試及結果分析

4.1 基于Kalman濾波的姿態(tài)信息融合數(shù)據(jù)分析

使用微系統(tǒng)中的iNEMO慣性模塊進行實時姿態(tài)測量實驗，陀螺儀和加速度計采樣頻率為100 Hz，磁強計的采用頻率為10 Hz，將兩種姿態(tài)測量結果進行Kalman濾波信息融合，得到的姿態(tài)信息如圖3所示。其中，上圖為陀螺儀輸出的歐拉角，中圖為加速度計和磁強計組合輸出的歐拉角，下圖為運用經過Kalman濾波后輸出的歐拉角，γ、θ、ψ分別表示系統(tǒng)的橫滾角、俯仰角和方位角。由圖中可以看出，經過Kalman濾波信息融合，對兩種方式測得的姿態(tài)誤差進行了補償，經濾波得到的結果較為平滑且與系統(tǒng)真實的姿態(tài)變化相吻合。

圖3 實時姿態(tài)解算Kalman濾波結果

4.2 系統(tǒng)初始對準精度分析

初始對準在靜基座條件下進行。實驗中，慣組模塊陀螺儀和加速度計采樣頻率為100 Hz、磁強計的采用頻率為10 Hz，北斗接收機的數(shù)據(jù)更新率為1 Hz。采用慣組輸出進行粗對準，得到姿態(tài)角誤差如圖4所示。北斗輔助iNEMO慣性模塊進行精對準，得到姿態(tài)角誤差如圖5所示。表1列出了兩種對準方式下的姿態(tài)角誤差的均值和標準差。從測試結果可以看出，粗對準中俯仰角誤差和方位角誤差較大，誤差均值在0.7°左右，精對準后，輸出的姿態(tài)角誤差的均值和標準差均變小，得到的

圖4 粗對準時靜基座的姿態(tài)角誤差

圖5 精對準時靜基座的姿態(tài)角誤差

方位角和俯仰角的誤差控制在±0.1°以內，橫滾角誤差在±0.15°以內，表明基于上述模型的初始對準很好的滿足了姿態(tài)解算精度的要求。

表1 靜態(tài)測試時的姿態(tài)角誤差結果

5 結論

本文設計了一種移動機器人的姿態(tài)測量系統(tǒng)，可以完成對機器人運動姿態(tài)的實時采集和存儲。研究了對iNEMO慣性模塊測量數(shù)據(jù)進行融合的方法，可以較精確地實時解算出姿態(tài)數(shù)據(jù)。利用北斗輸出的導航信息輔助iNEMO慣性模塊實現(xiàn)精對準，能夠有效的改善系統(tǒng)的性能。軟硬件的測試結果驗證了設計方案的可行性和正確性，該方法方便快捷、功耗低、試驗范圍不受限制，并且用來支持試驗的地面設備少，節(jié)約經費，設備可重復使用，能夠滿足機器人姿態(tài)解算所要求的高精度、穩(wěn)定性，具有很強的通用性和可拓展性。

[1] 徐國保,尹怡欣, 周美娟.智能移動機器人技術現(xiàn)狀及展望[J]. 機器人技術與應用,2007(2):29-34.

[2] Hua M D, Ducard G, Hamel T, et al. Introduction to Nonlinear Attitude Estimation for Aerial Robotic Systems[J]. Aerospace Lab, 2014: AL08-04.

[3] 任文強.多面體機器人姿態(tài)檢測技術研究[D].北京:北京交通大學,2016.

[4] 秦 勇,臧希喆,王曉宇等.基于MEMS 慣性傳感器的機器人姿態(tài)檢測系統(tǒng)的研究[J].傳感技術學報,2007,20(2):298-301.

[5] Renaudin V, Combettes C. Magnetic, Acceleration Fields and Gyroscope Quaternion (MAGYQ)-Based Attitude Estimation with Smartphone Sensors for Indoor Pedestrian Navigation[J]. Sensors, 2014, 14(12): 22864-22890.

[6] Di L, Fromm T, Chen Y Q. A Data Fusion System for Attitude Estimation of Low-cost Miniature UAVs[J]. Journal of Intelligent & Robotic Systems, 2012,65(1-4): 621-635.

[7] 張秋昭, 張書畢, 王 堅,等.露天礦卡車低成本GPS/INS組合導航系統(tǒng)動態(tài)對準模型[J].煤炭學報,2013,38(8):1362-1367.

[8] 姜 蘭.超長航時無人機持久組合導航系統(tǒng)設計[D].南京:南京理工大學,2014.

[9] 楊亞非,譚久彬,鄧正隆.慣導系統(tǒng)初始對準技術綜述[J]. 中國慣性技術學報,2002,10(2):68-72.

Method of Robot Attitude Calculation Based on Beidou/ iNEMO Inertial Module

Chen Kai1, Wang Guanglong2,Wang Cheng2,Bo Hongbo1

(1.No.66008 Unit of PLA, Tianjin 300250, China; 2.Laboratory of Nanotechnology and Microsystems,Ordnance Engineering College, Shijiazhuang 050003, China)

This paper researches a combined robot posture measurement system and attitude algorithm based on Beidou/iNEMO. Here designed an embedded combination attitude calculation platform cored by ARM, to calculate posture of robot in real-time under high dynamic condition, by Quaternion and Kalman filter technology fusing measured data of iNEMO inertial measurement unit. Meanwhile, using the Beidou receiver outputted 1pps rising edge pulse as synchronous time of subsequent data fusion, as well as its outputted navigation data aid iNEMO inertial measurement unit, fine alignment can be realized. The feasibility and correctness of the designed test has been verified, it provides robot attitude calculation with high precision, high stability, small volume, low power consumption.

attitude calculation; inertial measurement unit(IMU); robot; BeiDou system; initial alignment

2016-04-07；

2016-06-21。

國家自然科學青年基金(61501493)。

陳 凱(1983-)，男，重慶榮昌人，碩士研究生，工程師，主要從事測試計量技術及儀器的應用研究。

1671-4598(2017)02-0130-04

10.16526/j.cnki.11-4762/tp.2017.02.036

TP212.9

A