符秀輝 朱林楠 劉 然

（沈陽化工大學(xué)信息工程學(xué)院 沈陽 110142）

1 引言

機器人同步定位與地圖構(gòu)建（SLAM）是近年來的熱點之一［1］，在構(gòu)建地圖時準(zhǔn)確的實時姿態(tài)角獲取是前提和關(guān)鍵。目前大多數(shù)慣性導(dǎo)航儀器存在漂移問題，在實際應(yīng)用中，常見的姿態(tài)測量單元，主要由三軸陀螺儀、三軸加速度計和三軸磁力計組成［2］，一般采用三軸陀螺儀和三軸加速度計的配合使用來測算姿態(tài)角，進(jìn)而得到機器人在地圖中的位姿信息。運動載體的姿態(tài)可以由三軸陀螺儀通過積分獲得［3］，短時間內(nèi)解算的姿態(tài)精度高，而且具有良好的動態(tài)響應(yīng)特性，但是由于陀螺儀本身存在漂移，而且在積分過程中會產(chǎn)生累積誤差，因此長時間情況下，通過陀螺儀測量的數(shù)據(jù)解算得到的姿態(tài)精度較差。加速度計也可較精確測量運動載體的水平姿態(tài)角［4］，但是它對運動加速度十分敏感，當(dāng)載體處于運動狀態(tài)時，加速度計的測量會出現(xiàn)一定程度上的偏差，從而導(dǎo)致姿態(tài)精度下降［5］?？紤]到三軸陀螺儀、三軸加速度計和三軸磁力計的動靜態(tài)特性，擬采用數(shù)據(jù)融合方法對陀螺儀和加速度計測得的數(shù)據(jù)進(jìn)行融合處理，采用卡爾曼濾波算法對角度進(jìn)行誤差修正［6］，以提高運動載體姿態(tài)信息的精度和系統(tǒng)的抗干擾能力，考慮到磁力計受到磁場強度的影響，參考航空飛行器對空中姿態(tài)角的測量方法，采用基于最小二乘法的橢球擬合對磁力計傳感器數(shù)據(jù)進(jìn)行校正。實驗結(jié)果表明，對陀螺儀和加速度計的數(shù)據(jù)融合效果和對磁力計的數(shù)據(jù)校正會使誤差更小，提高了系統(tǒng)的動態(tài)特性，從而實現(xiàn)對機器人在行進(jìn)過程中的地圖信息進(jìn)行實時構(gòu)建。

2 卡爾曼濾波

卡爾曼濾波在工程應(yīng)用中較為成熟［7］，通過前一個時刻的估計值和現(xiàn)在的量測值來計算現(xiàn)在狀態(tài)的估計值，具有實時性好、計算量小、易于工程實現(xiàn)的優(yōu)點［8~9］。在實際導(dǎo)航系統(tǒng)中，陀螺儀存在溫漂，短時間測量誤差小，加速度計存在噪聲，測量誤差大，因此利用卡爾曼濾波融合陀螺儀與加速度計傳感器的數(shù)據(jù)，減小數(shù)據(jù)偏差，獲得相對準(zhǔn)確的姿態(tài)角。卡爾曼濾波過程如下：

系統(tǒng)方程為

狀態(tài)預(yù)測方程為

誤差協(xié)方差方程為

卡爾曼增益方程為

修正估計方程為

更新誤差協(xié)方差方程為

式中：Uk是輸入的白噪聲，A是狀態(tài)轉(zhuǎn)移矩陣，B是噪聲驅(qū)動矩陣，H是觀測矩陣，yk是k時刻的測量值，vk是觀測噪聲，X?k-1是k-1 時刻的系統(tǒng)狀態(tài)，X?k|k-1是k-1 時刻系統(tǒng)最優(yōu)狀態(tài)，Pk|k-1是X?k|k-1對應(yīng)的協(xié)方差，Q是系統(tǒng)過程的協(xié)方差，K是卡爾曼增益，R是觀測噪聲協(xié)方差，I是系統(tǒng)單位矩陣。

3 橢球擬合

磁航向測量系統(tǒng)主要包括控制器、磁力計、加速度計和陀螺儀幾大部分［10］。磁航向測量系統(tǒng)中的主要測量元件通常由于固定的原因無法和水平面處于同一平面內(nèi)，與其存在兩個夾角［11］：俯仰角和橫滾角［12］，其大小由加速度計和陀螺儀共同測得。在同一環(huán)境中理想情況下，磁力計測得的x,y,z軸值，在空間中形成一個圓球面，由于磁力計會受到地磁場和周圍磁性材料如電機等產(chǎn)生的磁場的影響，磁力計測得的數(shù)據(jù)在空間中形成一個橢球，但橢球的中心并不是傳感器的中心，橢球的各個軸也不是傳感器的各個軸［13］。如圖1 所示，受到磁場強度影響的磁力計其橢球中心坐標(biāo)為(x0,y0,z0)，三個軸的半周長分別是a,b,c。因此有必要對磁力計進(jìn)行校準(zhǔn)。過程如下：

圖1 存在羅差的水平磁場分布

三維平面下橢球的方程為

為了求取理想情況下的磁力計數(shù)據(jù)，需要進(jìn)行橢球擬合并通過最小二乘法求解橢球的六個元素：橢中心中心(x0,y0,z0)、x軸半軸長a、y軸半軸長為b，及z軸半軸長為c。任意橢球在空間坐標(biāo)系下可表示為

將磁力計讓其中心旋轉(zhuǎn)360°得N(N≥5)個采樣點(xi,yi,zi)，根據(jù)最小二乘法原理可得目標(biāo)函數(shù)為

使得式（9）值最小來求解五個參數(shù)，則由式（10）：

可得方程組：

求解線性方程組得A，B,C,D,E,F的值。則橢圓五個參數(shù)可由A，B,C,D,E,F得到

4 實驗與結(jié)果分析

移動機器人如圖2 所示采用STM32F103ZET6控制，搭載激光雷達(dá)Rplider A1、慣性導(dǎo)航MPU6050 和三軸磁力計（數(shù)字羅盤）HMC5883L，在ROS 系統(tǒng)下運行。設(shè)加速度計的過程噪聲協(xié)方差q_angle=0.001 ，陀螺儀的過程噪聲協(xié)方差q_gyro=0.003 ，加速度計的測量噪聲協(xié)方差r_angle=0.5 ，采樣時間t=0.0002 ，陀螺儀漂移q_bias=0。橫滾角卡爾曼濾波效果如圖3所示，俯仰角卡爾曼濾波效果如圖4 所示，偏航角卡爾曼濾波效果如圖5所示。

圖2 移動機器人

圖3 橫滾角卡爾曼濾波

圖4 俯仰角卡爾曼濾波

圖5 偏航角卡爾曼濾波

采用卡爾曼濾波后姿態(tài)角前后數(shù)據(jù)對比如表1所示。

表1 姿態(tài)角誤差

采用磁力計測得數(shù)據(jù)時，理想情況下圓心坐標(biāo)為(21,-20,20)，三個軸的半長軸分別為a=96，b=90，c=85。采用橢球擬合方法對磁力計進(jìn)行校準(zhǔn)并通過的最小二乘法對其擬合方程求解，得到橢球的六個參數(shù)對磁力計進(jìn)行校正。在校準(zhǔn)數(shù)據(jù)采樣的過程中需要通過將磁力計進(jìn)行旋轉(zhuǎn)一周，通過加入陀螺儀輔助加速度計對磁力計的俯仰角和橫滾角的進(jìn)行測量，橢球擬合效果如圖6 所示，前后數(shù)據(jù)對比如表2所示。

表2 橢球擬合前后數(shù)據(jù)對比

經(jīng)過卡爾曼濾波和磁力計校正后，數(shù)據(jù)誤差更小，可以使機器人在室內(nèi)環(huán)境下建圖效果更好。機器人在室內(nèi)環(huán)境下地圖創(chuàng)建前后效果對比如圖7和圖8 所示。圖中白色區(qū)域為自由區(qū)，黑色區(qū)域為障礙區(qū)。由圖7~8 對比可以看出校正后的建圖效果更好，能清晰地識別出障礙物。

圖7 未校正前的室內(nèi)地圖

圖8 校正后的室內(nèi)地圖

5 結(jié)語

在地圖創(chuàng)建過程中，采用卡爾曼濾波的慣性導(dǎo)航融合了姿態(tài)角的信號，與以往只通過加速度計來測量俯仰角和橫滾角相比，可以有效地減小數(shù)據(jù)誤差，更精確獲得姿態(tài)角信息，并橢球擬合法校準(zhǔn)磁力計，增加了系統(tǒng)的動態(tài)響應(yīng)能力，提高了的環(huán)境適應(yīng)能力，提高了磁航向的測量精度。通過以上方法可以使機器人創(chuàng)建的地圖更加精確，對于后續(xù)的工作比如機器人定位和路徑規(guī)劃有更好的效果。