劉佳豪，羅天放，王桐

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

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

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

1 激光雷達運動畸變產(chǎn)生原理

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

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

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

圖2 實際的激光掃描情況

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

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

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

2 算法主要思想

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

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

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

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

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

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

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

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

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

圖5 算法流程

3 輪式里程計初步校正

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

如圖6 所示，設(shè)激光點P在{Ri}中的坐標為(xr,yr)，在{O}中的坐標為(xo,yo)，則存在該激光點對應(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āng)前數(shù)據(jù)幀的激光總數(shù)為N，激光束之間的間隔時間為Δt。由于不一定能夠找到與激光束發(fā)出時刻完全對應(yīng)的輪式里程計數(shù)據(jù)，為了實現(xiàn)第i束激光對應(yīng)的激光雷達坐標系{Ri}在輪式里程計坐標系{O}當(dāng)中的準確匹配，如圖7 所示，需要對雷達數(shù)據(jù)幀進行分段，針對分段始末對應(yīng)的{Ri}與{O}變換關(guān)系進行線性插值。

圖7 激光雷達點云分段

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

第i個點P的發(fā)出時間為ti，分段號為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]進行插值，插值原理見式(3)、(4)以及圖8、9。

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

向量線性插值為

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

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

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

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

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

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

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

如圖10 所示，設(shè)激光點P在{B}中的坐標為(xb,yb)，則存在該激光點對應(yīng)的輪式里程計坐標系{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}的夾角θ：

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

4 IMU 激光雷達角度誤差校正

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

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

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

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

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

未校正的雷達數(shù)據(jù)曲線在220°之后（尾部）有明顯的上升趨勢，這是因為一幀中越靠后的激光束發(fā)射點位移越大，激光雷達距離測量誤差越大。

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

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

5 實驗驗證與結(jié)果分析

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

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

表2 MPU6050 參數(shù)

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

5.1 位移誤差

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

未校正雷達一幀點云數(shù)據(jù)中的前部數(shù)據(jù)誤差不大，但由于RPLIDAR A1 是逆時針掃描，誤差按照逆時針方向逐漸增大，在當(dāng)前幀的尾部達到最大，地圖左下方邊緣與實際地圖存在較大錯位。

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

圖14 激光雷達位移誤差曲線

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

5.2 角度誤差

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

圖15 低速角度校正點云

圖16 低速角度誤差曲線

對比分析里程計初步校正與聯(lián)合校正后的數(shù)據(jù)，兩者點云均與靜態(tài)地圖基本重合，因為在低速情況下驅(qū)動輪不易打滑，里程計數(shù)據(jù)尚且較為準確。但是里程計校正誤差曲線整體略高于聯(lián)合校正誤差曲線，可知相對于未校正的雷達數(shù)據(jù)，兩者角度誤差均得到有效校正，但聯(lián)合校正精度相比里程計校正更高。

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

圖17 高速角度校正點云

圖18 高速角度誤差曲線

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

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

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

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

圖19 低速建圖對比

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

圖20 高速建圖對比

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

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

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

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

6 結(jié)論

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

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