一種地圖構(gòu)建方法研究*

符秀輝 朱林楠 劉 然

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

1 引言

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

2 卡爾曼濾波

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

系統(tǒng)方程為

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

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

卡爾曼增益方程為

修正估計(jì)方程為

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

式中：Uk是輸入的白噪聲，A是狀態(tài)轉(zhuǎn)移矩陣，B是噪聲驅(qū)動(dòng)矩陣，H是觀測(cè)矩陣，yk是k時(shí)刻的測(cè)量值，vk是觀測(cè)噪聲，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對(duì)應(yīng)的協(xié)方差，Q是系統(tǒng)過(guò)程的協(xié)方差，K是卡爾曼增益，R是觀測(cè)噪聲協(xié)方差，I是系統(tǒng)單位矩陣。

3 橢球擬合

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

圖1 存在羅差的水平磁場(chǎng)分布

三維平面下橢球的方程為

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

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

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

可得方程組：

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

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

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

圖2 移動(dòng)機(jī)器人

圖3 橫滾角卡爾曼濾波

圖4 俯仰角卡爾曼濾波

圖5 偏航角卡爾曼濾波

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

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

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

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

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

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

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

5 結(jié)語(yǔ)

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