符秀輝 朱林楠 劉 然

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

1 引言

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

2 卡爾曼濾波

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

系統(tǒng)方程為

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

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

卡爾曼增益方程為

修正估計(jì)方程為

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

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

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

三維平面下橢球的方程為

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

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

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

可得方程組：

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

4 實(shí)驗(yàn)與結(jié)果分析

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

圖2 移動機(jī)器人

圖3 橫滾角卡爾曼濾波

圖4 俯仰角卡爾曼濾波

圖5 偏航角卡爾曼濾波

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

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

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

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

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

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

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

5 結(jié)語

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