IMU 和輪式里程計(jì)聯(lián)合的雷達(dá)畸變校正算法

劉佳豪，羅天放，王桐

1. 哈爾濱工程大學(xué) 信息與通信工程學(xué)院，黑龍江 哈爾濱 150001

2. 哈爾濱工程大學(xué) 先進(jìn)船舶通信與信息技術(shù)工業(yè)和信息化部重點(diǎn)實(shí)驗(yàn)室，黑龍江 哈爾濱 150001

諸多商用機(jī)器人為了控制硬件成本，采用數(shù)據(jù)幀率較低的2D 激光雷達(dá)，導(dǎo)致機(jī)器人在快速移動(dòng)時(shí)雷達(dá)數(shù)據(jù)將產(chǎn)生較大運(yùn)動(dòng)畸變[1]。迭代最近點(diǎn)(iterative closest point, ICP)算法[2]為經(jīng)典的畸變校正算法，在兩幀數(shù)據(jù)之間進(jìn)行掃描匹配，但未考慮機(jī)器人的運(yùn)動(dòng)因素，雖然通過(guò)不斷迭代能夠收斂，但仍存在誤差[3]；融合主成分分析算法(principal component analysis, PCA)的ICP 算法作為ICP 算法的改進(jìn)，利用有效點(diǎn)對(duì)中歐式距離較大的點(diǎn)對(duì)完成精配準(zhǔn)，但在點(diǎn)云較稠密時(shí)配準(zhǔn)誤差較大[4]；輪式里程計(jì)輔助法[5]在理想狀態(tài)下精度較高，但在速度過(guò)快時(shí)驅(qū)動(dòng)輪打滑的概率增加，所導(dǎo)致的角度誤差難以處理[6]，致使高速狀態(tài)下激光雷達(dá)角度誤差不能夠被準(zhǔn)確校正；有學(xué)者提出輪式里程計(jì)與ICP 融合算法[7-8]，改進(jìn)了輪式里程計(jì)位置線性假設(shè)的缺陷，將其轉(zhuǎn)為位置誤差的線性假設(shè)[9]，算法能夠達(dá)到更高的畸變校正上限但無(wú)法處理輪式里程計(jì)打滑的問(wèn)題。為解決輪式里程計(jì)輔助法缺陷，本文利用慣性測(cè)量單元(inertial measurement unit, IMU)傳感器[10]在快速移動(dòng)時(shí)仍然能夠提供高頻率準(zhǔn)確航向角信息的特點(diǎn)，彌補(bǔ)輪式里程計(jì)的機(jī)械缺陷，提出了IMU 和輪式里程計(jì)融合的激光雷達(dá)運(yùn)動(dòng)畸變校正的方法。

1 激光雷達(dá)運(yùn)動(dòng)畸變產(chǎn)生原理

雷達(dá)運(yùn)動(dòng)畸變產(chǎn)生的主要原因是：激光雷達(dá)掃描數(shù)據(jù)并非瞬間完成卻將其理想化為瞬間完成進(jìn)行數(shù)據(jù)封裝，忽略了激光雷達(dá)在單個(gè)數(shù)據(jù)周期內(nèi)的運(yùn)動(dòng)[11]。設(shè)機(jī)器人處于封閉圓形區(qū)域，機(jī)器人向前做速度為v的勻速運(yùn)動(dòng)，雷達(dá)數(shù)據(jù)幀率f，其理想化數(shù)據(jù)應(yīng)為圖1 所示。

圖1 理想的激光雷達(dá)數(shù)據(jù)

實(shí)際上，激光雷達(dá)在第1 束與最后1 束激光的時(shí)間間隔內(nèi)隨機(jī)器人運(yùn)動(dòng)，激光的發(fā)射點(diǎn)不斷向前平移，實(shí)際的激光數(shù)據(jù)掃描情況為圖2 所示，一幀中越靠后的激光束發(fā)射點(diǎn)位移越大，誤差越大。

圖2 實(shí)際的激光掃描情況

最后數(shù)據(jù)封裝時(shí)，雷達(dá)被理想化為靜止，所有激光發(fā)射點(diǎn)被統(tǒng)一到第1 束激光發(fā)射點(diǎn)處，最終得到畸變的雷達(dá)數(shù)據(jù)，如圖3 所示。誤差順著掃描方向逐漸增加。當(dāng)前幀數(shù)據(jù)最大畸變量ΔLmax約為一幀數(shù)據(jù)周期內(nèi)機(jī)器人所移動(dòng)的距離，在v=0.5 m/s、f=5 Hz 條件下有：

圖3 畸變的激光雷達(dá)數(shù)據(jù)

可見(jiàn)數(shù)據(jù)畸變較大，不可忽略。上述論述的是機(jī)器人平移過(guò)程的位移畸變產(chǎn)生原理，具有一幀中越靠后的激光束發(fā)射點(diǎn)位移越大誤差越大的特點(diǎn)。旋轉(zhuǎn)產(chǎn)生的角度畸變?cè)砼c之類似，并且同樣具有一幀中激光束越靠后角度誤差越大的特點(diǎn)。

2 算法主要思想

低成本2D 激光雷達(dá)數(shù)據(jù)幀率低至5 Hz，輪式里程計(jì)數(shù)據(jù)幀率一般在100 Hz 以上，而IMU 數(shù)據(jù)幀率最高，可達(dá)200~1 000 Hz。雷達(dá)、輪式里程計(jì)、IMU 數(shù)據(jù)幀率對(duì)比如圖4 所示。通過(guò)多傳感器融合補(bǔ)全雷達(dá)在單數(shù)據(jù)周期內(nèi)的運(yùn)動(dòng)信息[12]，從而校正由于運(yùn)動(dòng)帶來(lái)的點(diǎn)云畸變。

圖4 雷達(dá)、輪式里程計(jì)、IMU 數(shù)據(jù)幀率對(duì)比

激光雷達(dá)在一個(gè)數(shù)據(jù)周期內(nèi)的移動(dòng)使得每一束激光對(duì)應(yīng)的雷達(dá)坐標(biāo)系位置均不相同。算法通過(guò)輪式里程計(jì)數(shù)據(jù)獲取激光雷達(dá)坐標(biāo)系在一個(gè)數(shù)據(jù)周期內(nèi)的運(yùn)動(dòng)情況，據(jù)此來(lái)初步修正每束激光數(shù)據(jù)。針對(duì)輪式里程計(jì)在機(jī)器人快速移動(dòng)時(shí)容易打滑造成角度測(cè)量誤差較大的問(wèn)題，通過(guò)讀取IMU 數(shù)據(jù)來(lái)獲取一個(gè)數(shù)據(jù)周期內(nèi)機(jī)器人準(zhǔn)確的旋轉(zhuǎn)運(yùn)動(dòng)情況，對(duì)角度誤差實(shí)現(xiàn)更加精準(zhǔn)的校準(zhǔn)[13]。

校正過(guò)程涉及到多次坐標(biāo)系轉(zhuǎn)換，以下對(duì)相關(guān)坐標(biāo)系進(jìn)行說(shuō)明：

1）里程計(jì)坐標(biāo)系Odom[14]：{O}(OOXOYOZO)，全局唯一，數(shù)據(jù)來(lái)源于電機(jī)編碼器。

2）一幀雷達(dá)數(shù)據(jù)中第i束激光對(duì)應(yīng)的激光雷達(dá)坐標(biāo)系Radari：{Ri}(ORXRYRZR)，每一束激光都擁有自己對(duì)應(yīng)的{Ri}，該坐標(biāo)系原點(diǎn)即為激光雷達(dá)發(fā)射點(diǎn)，激光雷達(dá)運(yùn)動(dòng)時(shí)對(duì)于不同i，{Ri}并不相同，即如果一幀雷達(dá)數(shù)據(jù)包含360 個(gè)激光點(diǎn)，則該數(shù)據(jù)周期內(nèi)有360 個(gè)不同{Ri}。

3）基準(zhǔn)坐標(biāo)系Base：{B}(OBXBYBZB)，一幀雷達(dá)數(shù)據(jù)中第0 束激光對(duì)應(yīng)雷達(dá)坐標(biāo)系，{B}={R0}，所有激光點(diǎn)都需要校正到該坐標(biāo)系進(jìn)行數(shù)據(jù)封裝。

首先借助里程計(jì)坐標(biāo)系{O}為中間媒介，將所有激光各自對(duì)應(yīng)的激光雷達(dá)坐標(biāo)系{Ri}轉(zhuǎn)換到該幀雷達(dá)數(shù)據(jù)的基準(zhǔn)坐標(biāo)系{B}下，完成輪式里程計(jì)輔助的初步校正；然后再通過(guò)IMU 數(shù)據(jù)獲取激光雷達(dá)坐標(biāo)系{Ri}與基準(zhǔn)坐標(biāo)系{B}之間精確的旋轉(zhuǎn)變換關(guān)系，對(duì)初步校正后的各激光束再次進(jìn)行精度更高的角度校正；最后以基準(zhǔn)坐標(biāo)系{B}來(lái)封裝數(shù)據(jù)。

通常不帶有磁力校準(zhǔn)的IMU 在測(cè)量航向角時(shí)會(huì)存在逐漸增大的累計(jì)誤差[15-16]，但算法中計(jì)算的是IMU 雷達(dá)掃描周期內(nèi)的旋轉(zhuǎn)增量，誤差不會(huì)累積。算法的主要流程如圖5 所示。

圖5 算法流程

3 輪式里程計(jì)初步校正

3.1 激光束i 對(duì)應(yīng){Ri}與{O}的變換關(guān)系

如圖6 所示，設(shè)激光點(diǎn)P在{Ri}中的坐標(biāo)為(xr,yr)，在{O}中的坐標(biāo)為(xo,yo)，則存在該激光點(diǎn)對(duì)應(yīng)的{Ri}到{O}變換關(guān)系，該變換關(guān)系由旋轉(zhuǎn)矩陣RR-O和平移向量TR-O組成。

圖6 {Ri}和{O}變換關(guān)系

旋轉(zhuǎn)矩陣RR-O變換為四元數(shù)QR-O便于插值：

設(shè)激光雷達(dá)當(dāng)前數(shù)據(jù)幀的激光總數(shù)為N，激光束之間的間隔時(shí)間為Δt。由于不一定能夠找到與激光束發(fā)出時(shí)刻完全對(duì)應(yīng)的輪式里程計(jì)數(shù)據(jù)，為了實(shí)現(xiàn)第i束激光對(duì)應(yīng)的激光雷達(dá)坐標(biāo)系{Ri}在輪式里程計(jì)坐標(biāo)系{O}當(dāng)中的準(zhǔn)確匹配，如圖7 所示，需要對(duì)雷達(dá)數(shù)據(jù)幀進(jìn)行分段，針對(duì)分段始末對(duì)應(yīng)的{Ri}與{O}變換關(guān)系進(jìn)行線性插值。

圖7 激光雷達(dá)點(diǎn)云分段

每個(gè)分段中激光束個(gè)數(shù)為k，則第j段中的第1 束激光在雷達(dá)數(shù)據(jù)幀中編號(hào)為j×k，發(fā)出時(shí)刻為tstart，對(duì)應(yīng){Ri}在{O}中的變換關(guān)系由旋轉(zhuǎn)矩陣RR-O[j×k]和平移向量TR-O[j×k]共2 部分組成，二者可通過(guò)輪式里程計(jì)數(shù)據(jù)獲取。同理，最后1 束激光在數(shù)據(jù)幀中編號(hào)為(j+1)×k-1，發(fā)出時(shí)刻為tend，旋轉(zhuǎn)矩陣RR-O[(j+1)×k-1]和平移向量TR-O[(j+1)×k-1]。

第i個(gè)點(diǎn)P的發(fā)出時(shí)間為ti，分段號(hào)為j，則有：

根據(jù)式(2)，將RR-O[j×k]轉(zhuǎn)換為四元數(shù)QR-O[j×k]，RR-O[(j+1)×k-1]轉(zhuǎn)換為四元數(shù)QR-O[(j+1)×k-1]進(jìn)行插值，插值原理見(jiàn)式(3)、(4)以及圖8、9。

四元數(shù)球面線性插值為

向量線性插值為

機(jī)器人在Vx=0.4 m/s、Az=0.45 rad/s 條件下，2D 激光雷達(dá)數(shù)據(jù)幀按每10 束激光進(jìn)行分段處理，其中第7 段激光四元數(shù)QR-O[7×10]到QR-O[8×10]球面線性插值與向量TR-O[7×10]到TR-O[8×10]線性插值情況如圖9 所示。

圖9 四元數(shù)與向量插值

3.2 激光坐標(biāo)系{Ri}轉(zhuǎn){O}

激光點(diǎn)需要由對(duì)應(yīng)激光雷達(dá)坐標(biāo)系{Ri}變換到輪式里程計(jì)坐標(biāo)系{O}。假設(shè)第i個(gè)激光點(diǎn)P在雷達(dá)極坐標(biāo)系中角度為angles[i]，距離為ranges[i]；轉(zhuǎn)換為{R}中的坐標(biāo)(xr,yr)，此時(shí){Ri}與{O}變換關(guān)系由四元數(shù)QR-O[i]和平移向量TR-O[i]表示，有：

結(jié)合式(2)得到圖6 中{Ri}與{O}夾角α：

結(jié)合式(1)將該激光點(diǎn)在{Ri}的坐標(biāo)(xr,yr)轉(zhuǎn)換為在輪式里程計(jì)坐標(biāo)系{O}中的坐標(biāo)(xo,yo)：

3.3 激光坐標(biāo)系{O}轉(zhuǎn){B}

如圖10 所示，設(shè)激光點(diǎn)P在{B}中的坐標(biāo)為(xb,yb)，則存在該激光點(diǎn)對(duì)應(yīng)的輪式里程計(jì)坐標(biāo)系{O}與基準(zhǔn)坐標(biāo)系{B}變換關(guān)系。

該變換關(guān)系由旋轉(zhuǎn)矩陣RO-B和平移向量TO-B表示，設(shè):

將RO-B轉(zhuǎn)換為四元數(shù)QO-B：

QO-B轉(zhuǎn)歐拉角得到圖10 中{O}與{B}的夾角θ：

該激光點(diǎn)在{O}中的坐標(biāo)(xo,yo)轉(zhuǎn)換為在{B}中坐標(biāo)(xb,yb)完成激光束數(shù)據(jù)的里程計(jì)初步校正：

4 IMU 激光雷達(dá)角度誤差校正

通過(guò)讀取IMU 數(shù)據(jù)可以得到激光束i對(duì)應(yīng)激光雷達(dá)坐標(biāo)系{Ri}與輪式里程計(jì)坐標(biāo)系{O}之間的旋轉(zhuǎn)變換關(guān)系，該關(guān)系通過(guò)四元數(shù)QIMU[i]給出：

四元數(shù)轉(zhuǎn)歐拉角得到從IMU 獲取到的激光雷達(dá)坐標(biāo)系{Ri}與輪式里程計(jì)坐標(biāo)系{O}夾角γ：

初步校正中通過(guò)輪式里程計(jì)數(shù)據(jù)已知輪式里程計(jì)坐標(biāo)系{O}與基準(zhǔn)坐標(biāo)系{B}之間的夾角θ，則由式(5)可得{R}與{B}基準(zhǔn)坐標(biāo)系之間的夾角?，即當(dāng)前激光束從第1 束激光發(fā)出到當(dāng)前激光束時(shí)刻發(fā)生的角度偏移量，angles[i]- ? 即為激光束i經(jīng)過(guò)IMU 修正后的最終角度。至此激光數(shù)據(jù)校正完畢。

為了體現(xiàn)各個(gè)校正變換的效果，將機(jī)器人置于一半徑1.6 m 規(guī)則圓形場(chǎng)地內(nèi)進(jìn)行實(shí)驗(yàn)，機(jī)器人在Vx=0.4 m/s、Az=0.45 rad/s 條件下，激光雷達(dá)幀按每10 束激光進(jìn)行分段處理，數(shù)據(jù)如圖11 所示。

圖11 校正前后激光雷達(dá)數(shù)據(jù)曲線

未校正的雷達(dá)數(shù)據(jù)曲線在220°之后（尾部）有明顯的上升趨勢(shì)，這是因?yàn)橐粠性娇亢蟮募す馐l(fā)射點(diǎn)位移越大，激光雷達(dá)距離測(cè)量誤差越大。

完成輪式里程計(jì)初步校正后的雷達(dá)數(shù)據(jù)無(wú)明顯上升趨勢(shì)，其距離數(shù)值在場(chǎng)地的半徑（1.6 m）附近正常波動(dòng)，但是其角度校正精度較差，雷達(dá)數(shù)據(jù)存在角度誤差。

對(duì)比分析完成IMU 激光雷達(dá)角度誤差校正后的雷達(dá)數(shù)據(jù)，算法利用IMU 數(shù)據(jù)完成了角度滯后誤差的校正，所以數(shù)據(jù)曲線相對(duì)于完成輪式里程計(jì)初步校正后的雷達(dá)數(shù)據(jù)有一定的角度超前，且一幀中越靠后的數(shù)據(jù)角度超前量越大，符合前述的角度誤差特點(diǎn)。

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

實(shí)驗(yàn)采用機(jī)器人操作系統(tǒng)(robot operating system, ROS)麥克納姆輪全向機(jī)器人為平臺(tái)，采用Rviz 插件作為雷達(dá)數(shù)據(jù)可視化工具，相關(guān)誤差數(shù)據(jù)利用ros_bag 工具導(dǎo)出由origin 軟件繪圖。激光雷達(dá)采用思嵐RPLIDAR A1 單線激光雷達(dá)，IMU 采用MPU6050，直流電機(jī)自帶霍爾編碼器。機(jī)器人、激光雷達(dá)、IMU 霍爾編碼器實(shí)物及相關(guān)參數(shù)分別如圖12、表1~表3 所示。

表1 思嵐RPLIDAR A1 參數(shù)

表2 MPU6050 參數(shù)

表3 直流電機(jī)參數(shù)

5.1 位移誤差

機(jī)器人前向速度Vx=1.15 m/s，所得激光雷達(dá)點(diǎn)云數(shù)據(jù)如圖13 所示。

未校正雷達(dá)一幀點(diǎn)云數(shù)據(jù)中的前部數(shù)據(jù)誤差不大，但由于RPLIDAR A1 是逆時(shí)針掃描，誤差按照逆時(shí)針?lè)较蛑饾u增大，在當(dāng)前幀的尾部達(dá)到最大，地圖左下方邊緣與實(shí)際地圖存在較大錯(cuò)位。

對(duì)比分析聯(lián)合校正后的點(diǎn)云數(shù)據(jù)，各部分均與實(shí)際靜態(tài)地圖貼合。校正前后位移誤差曲線如圖14 所示。

圖14 激光雷達(dá)位移誤差曲線

聯(lián)合校正后位移誤差相比校正前大大減小，誤差最大值由10.5 cm 減小為0.85 cm，由此可見(jiàn)聯(lián)合校正下位移誤差得到有效矯正。

5.2 角度誤差

在慢速（Vx=0.4 m/s，Az=0.45 rad/s）情況下，如圖15、16 所示，未校正的數(shù)據(jù)幀前部角度誤差較小，中部誤差略大，尾部誤差最大約為5°，雷達(dá)數(shù)據(jù)與靜態(tài)地圖存在較明顯錯(cuò)位。

圖15 低速角度校正點(diǎn)云

圖16 低速角度誤差曲線

對(duì)比分析里程計(jì)初步校正與聯(lián)合校正后的數(shù)據(jù)，兩者點(diǎn)云均與靜態(tài)地圖基本重合，因?yàn)樵诘退偾闆r下驅(qū)動(dòng)輪不易打滑，里程計(jì)數(shù)據(jù)尚且較為準(zhǔn)確。但是里程計(jì)校正誤差曲線整體略高于聯(lián)合校正誤差曲線，可知相對(duì)于未校正的雷達(dá)數(shù)據(jù)，兩者角度誤差均得到有效校正，但聯(lián)合校正精度相比里程計(jì)校正更高。

在高速（Vx=0.85 m/s，Az=1.2 rad/s）情況下，如圖17、18 所示，未校正的雷達(dá)數(shù)據(jù)存在最大為15°的角度誤差，而輪式里程計(jì)校正的數(shù)據(jù)結(jié)果其數(shù)據(jù)幀尾部也存在10°的角度誤差，因?yàn)楦咚贍顟B(tài)下驅(qū)動(dòng)輪多次不可避免地細(xì)微滑動(dòng)，輪式里程計(jì)并不能夠提供準(zhǔn)確的機(jī)器人旋轉(zhuǎn)數(shù)據(jù)，且該誤差將持續(xù)累積，導(dǎo)致雷達(dá)數(shù)據(jù)與地圖錯(cuò)位。

圖17 高速角度校正點(diǎn)云

圖18 高速角度誤差曲線

對(duì)比分析IMU 里程計(jì)聯(lián)合校正的數(shù)據(jù)，其點(diǎn)云始終與靜態(tài)地圖吻合，角度誤差曲線不超過(guò)2°。由于IMU 不存在輪式里程計(jì)驅(qū)動(dòng)輪打滑的問(wèn)題，機(jī)器人角速度較大時(shí)依然能夠準(zhǔn)確提供姿態(tài)信息，將角度誤差進(jìn)行良好校正。所以在高速狀態(tài)下，角度誤差校正實(shí)際表現(xiàn)遠(yuǎn)優(yōu)于輪式里程計(jì)校正。

綜上所述，低速狀態(tài)下輪式里程計(jì)校正和聯(lián)合校正均具備較好的角度校正效果，但是高速狀態(tài)下聯(lián)合校正克服了輪式里程計(jì)的機(jī)械性缺陷，具備更好的角度校正性能。

5.3 地圖構(gòu)建質(zhì)量

低速（Vx=0.4 m/s，Az=0.45 rad/s）情況下的機(jī)器人地圖構(gòu)建結(jié)果如圖19 所示。輪式里程計(jì)輔助法與IMU 輪式里程計(jì)聯(lián)合校正下的地圖構(gòu)建結(jié)果均較為理想，沒(méi)有發(fā)生明顯的地圖錯(cuò)位，說(shuō)明在機(jī)器人速度較低的條件下，驅(qū)動(dòng)輪不容易發(fā)生打滑現(xiàn)象，2 種方法均能夠很好地校正激光雷達(dá)運(yùn)動(dòng)畸變。

圖19 低速建圖對(duì)比

高速（Vx=0.85 m/s，Az=1.2 rad/s）情況下的機(jī)器人地圖構(gòu)建結(jié)果如圖20 所示。地圖可大致分為A、B、C 共3 大區(qū)域，其中C 為地圖主體區(qū)域。機(jī)器人由A 經(jīng)過(guò)咽喉要道1 后進(jìn)入C 區(qū)域，再經(jīng)過(guò)咽喉要道2 進(jìn)入B 區(qū)域完成探索。

圖20 高速建圖對(duì)比

輪式里程計(jì)輔助法中，機(jī)器人從A 進(jìn)入C 時(shí)在1 處急轉(zhuǎn)彎時(shí)，由于高速行駛下驅(qū)動(dòng)輪打滑，根據(jù)輪式里程計(jì)數(shù)據(jù)進(jìn)行矯正的激光雷達(dá)數(shù)據(jù)發(fā)生較大角度誤差，使剛進(jìn)入C 區(qū)域時(shí)獲取到的C 區(qū)域地形信息隨之附帶相應(yīng)的角度錯(cuò)誤，導(dǎo)致A 區(qū)域構(gòu)圖結(jié)果相對(duì)于地圖的主體區(qū)域C 區(qū)域存在明顯的順時(shí)針角度錯(cuò)位。同樣，從C 區(qū)域經(jīng)由咽喉要道2 進(jìn)入B 區(qū)域時(shí)也發(fā)生了類似情況，導(dǎo)致B 區(qū)域地圖相對(duì)于C 區(qū)域也存在逆時(shí)針?lè)较虻慕嵌儒e(cuò)位。

IMU 輪式里程計(jì)聯(lián)合校正算法中，由于激光雷達(dá)數(shù)據(jù)的角度信息最終是由IMU 進(jìn)行校正，雷達(dá)數(shù)據(jù)的角度誤差校正不受輪式里程計(jì)打滑的影響，所以即使是在高速狀態(tài)下依然能夠?qū)崿F(xiàn)較好的建圖質(zhì)量，不會(huì)產(chǎn)生地圖不同區(qū)域之間的角度錯(cuò)位。少數(shù)墻體因?yàn)闄C(jī)器人速度太快、采集激光束過(guò)少，造成墻體構(gòu)建較薄、障礙物點(diǎn)稀疏，該問(wèn)題可通過(guò)反復(fù)探索來(lái)解決。

綜上所述，機(jī)器人在構(gòu)建地圖時(shí)通過(guò)狹窄咽喉通道進(jìn)入新區(qū)域，由于進(jìn)入新區(qū)域后難以再掃描到舊區(qū)域的環(huán)境，新區(qū)域與舊區(qū)域之間的連接區(qū)域的信息大部分在該通道中獲得（如圖20 中的1、2 處），如果此時(shí)機(jī)器人驅(qū)動(dòng)輪打滑會(huì)使得里程計(jì)數(shù)據(jù)錯(cuò)誤從而使得激光雷達(dá)運(yùn)動(dòng)畸變反而加劇，2 個(gè)區(qū)域的連接處產(chǎn)生角度錯(cuò)位，得到錯(cuò)誤的地圖。

而IMU 不存在該機(jī)械性缺陷，即使在高速情況下依然能夠準(zhǔn)確反映激光雷達(dá)在自身一個(gè)數(shù)據(jù)周期內(nèi)的旋轉(zhuǎn)情況，從而消除在此間隔內(nèi)由于激光雷達(dá)本身的運(yùn)動(dòng)造成的激光雷達(dá)運(yùn)動(dòng)畸變。

6 結(jié)論

針對(duì)低幀率2D 激光雷達(dá)的運(yùn)動(dòng)畸變問(wèn)題，本文分析了雷達(dá)運(yùn)動(dòng)畸變的產(chǎn)生原因，闡述了輪式里程計(jì)輔助校正的不足，針對(duì)其在快速移動(dòng)時(shí)容易打滑的問(wèn)題，提出了IMU 和輪式里程計(jì)融合激光雷達(dá)運(yùn)動(dòng)畸變校正方法。

算法通過(guò)輪式里程計(jì)獲取校正位移誤差的運(yùn)動(dòng)信息，通過(guò)IMU 獲取校正角度誤差的運(yùn)動(dòng)信息，利用線性插值完成2 個(gè)傳感器與雷達(dá)數(shù)據(jù)的時(shí)間戳匹配。輪式機(jī)器人實(shí)機(jī)驗(yàn)證表明，該算法相比于傳統(tǒng)里程計(jì)輔助法具備更高的角度誤差校正精度，能夠有效克服輪式里程計(jì)在機(jī)器人快速移動(dòng)時(shí)打滑的機(jī)械缺陷，消除此問(wèn)題所帶來(lái)的角度誤差，大大提升機(jī)器人地圖構(gòu)建的質(zhì)量，極大改善了低幀率2D 激光雷達(dá)的動(dòng)態(tài)性能。