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

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

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

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

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

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

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

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

0 引言

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

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

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

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

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

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

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

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

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

2.1 iNEMO陀螺儀解算歐拉角

(1)

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

設(shè)載體坐標(biāo)系相對于導(dǎo)航坐標(biāo)系的轉(zhuǎn)動四元數(shù)為：

(2)

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

(3)

(4)

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

(5)

(6)

2.2iNEMO加速度計、磁強(qiáng)計解算歐拉角

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

(7)

(8)

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

(9)

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

(10)

(11)

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

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

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

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

(12)

其方差為：

(13)

3 北斗輔助iNEMO慣性模塊精對準(zhǔn)

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

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

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

(16)

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

(17)

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

(18)

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

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

(19)

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

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

4 試驗測試及結(jié)果分析

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

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

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

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

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

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

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

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

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

5 結(jié)論

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

[1] 徐國保,尹怡欣, 周美娟.智能移動機(jī)器人技術(shù)現(xiàn)狀及展望[J]. 機(jī)器人技術(shù)與應(yīng)用,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] 任文強(qiáng).多面體機(jī)器人姿態(tài)檢測技術(shù)研究[D].北京:北京交通大學(xué),2016.

[4] 秦 勇,臧希喆,王曉宇等.基于MEMS 慣性傳感器的機(jī)器人姿態(tài)檢測系統(tǒng)的研究[J].傳感技術(shù)學(xué)報,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組合導(dǎo)航系統(tǒng)動態(tài)對準(zhǔn)模型[J].煤炭學(xué)報,2013,38(8):1362-1367.

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

[9] 楊亞非,譚久彬,鄧正隆.慣導(dǎo)系統(tǒng)初始對準(zhǔn)技術(shù)綜述[J]. 中國慣性技術(shù)學(xué)報,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。

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

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

1671-4598(2017)02-0130-04

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

TP212.9

A