楊 超， 李天劍， 胡 歡， 黃 民

(北京信息科技大學機電工程學院， 北京 100192)

在移動機器人領域，機器人的位姿估計是關鍵性問題，關于此問題的研究進行了很多年?？梢杂糜趯C器人展開位姿估計的傳感器包括激光傳感器、視覺傳感器、輪式編碼器以及慣性測量傳感器等[1]。國外以Boston Dynamics及Willow Garage為首的機器人公司主要以立體視覺為主要導航控制系統(tǒng)來定位與建圖，而這種設備造價昂貴，至今無法實現(xiàn)大規(guī)模商業(yè)化[2]。小型機器人因體積小、視角低、運算能力及視場有限，即便使用廉價視覺設備，其實時理解場景的能力無法滿足實際需求[3]。因此目前主流的移動機器人傳感器仍以2D激光傳感器為主，例如亞馬遜的Kiva。同時，SLAM(simultaneous localization and mapping)技術的大規(guī)模應用使激光雷達在以掃地機器人為主的移動設備上得到了廣泛應用。2D激光雷達的使用主要通過對兩幀激光進行掃描匹配來估計傳感器位姿[4]。掃描匹配是解決位姿估計的主要步驟[5]。迭代最鄰近點(iterative closest point,ICP)算法作為解決掃描匹配問題的主流算法廣泛應用于3D重建、SLAM、視覺特征匹配等領域[6]。其優(yōu)勢在于求解局部極值時具有較高效率[7]。

目前，2D激光匹配存在激光數(shù)據(jù)畸變以及誤匹配等問題，針對誤匹配問題的研究比較多，Zhang等[8]通過點到面的匹配提升室內環(huán)境下激光的匹配精度，Jo等[9]針對2D激光提出了一種錯誤匹配去除的魯棒ICP算法。而對于數(shù)據(jù)畸變問題，ICP算法的匹配精度直接取決于數(shù)據(jù)是否精確，激光的捕獲過程往往伴隨傳感器的運動，當激光幀率較小時，其捕獲的激光數(shù)據(jù)與真實環(huán)境會存在誤差，誤差得到累積最終影響算法的最終效果，即便是建圖效果較好的cartographer[10]，當激光幀率較小時，其前端建圖仍會面臨畸變帶來的建圖誤差，這些誤差難以通過后期回環(huán)檢測手段得到修復，因此針對此問題的研究具有較大的實際意義。Bezet等[11]曾對激光雷達的掃描畸變問題展開過具體研究，他們根據(jù)時間誤差對此問題進行修正。但該方案的角度固定與二次插值假設過于簡單化，無法滿足更為一般的室內環(huán)境。Hong等[12]針對此問題提出VICP(velocity updating iterative closest point)算法，該算法是ICP算法的另一變種，算法考慮機器人的運動，認為激光雷達在掃描過程中進行勻速運動，并進行速度估計。該算法有效解決了激光運動過程中機器人的位姿估計問題，但其關于勻速運動的假設過于理想，以5 Hz激光雷達為例，激光捕獲一幀數(shù)據(jù)所需時間為200 ms，這對于以1～2 m/s運動的室內機器人而言是不可忽略的時間間隔，勻速運動的假設并不適用。Zhang等[13]曾在3D激光算法的掃描匹配算法中采用速度估計算法對運動畸變進行去除，并應用于SLAM技術，但勻速假設的缺陷仍然存在。

針對以上問題，提出一種創(chuàng)新算法，融合速度估計與輪式里程計位姿估計對ICP算法進行改進。

1 基本算法

ICP算法的初始階段依賴初始位姿估計與兩幀激光數(shù)據(jù)展開坐標變換，通過重復捕捉最鄰近點來迭代優(yōu)化激光匹配，獲取坐標變換，并將最終累計誤差最小化。ICP算法在柵格尺度上會單調收斂于局部極值，因此對初始位姿有較高要求，若初始位姿離真值偏差較大，ICP算法將最終收斂于局部最小值，非全局最小值，因此在初始階段對初始位姿進行優(yōu)化，此時可通過里程計進行初始估計[14]。

假設X={xi}為一幀激光數(shù)據(jù)，Y={yi}為下一幀激光數(shù)據(jù)。主要目標在于尋求X與Y之間的坐標變換以使X與Y實現(xiàn)最大化匹配。最小化目標函數(shù)定義如下：

(1)

式(1)中：R為旋轉矩陣；p為平移矩陣。

該目標函數(shù)的處理步驟如下：

(1)給定xi代表X中的激光點位置，yi代表Y中與xi對應的激光點位置。

(3)對點云進行平移操作，將點云平移到以平均值為圓心的位姿處X′={xi-ux}={x′i}，Y′={yi-uy}={y′i}。

(4)構造W矩陣并進行奇異值分解(SVD)：

(2)

(5)求解得到關于目標函數(shù)的解：R=UVT,p=ux-Ruy。

圖1 ICP算法流程Fig.1 ICP diagram flow

由于ICP算法使用最鄰近點作為對應點，對初始值就會相對敏感。當初始值與真實環(huán)境不完全接近時，重復迭代過程會讓最終的結果盡可能收斂于真實狀態(tài)[15]。圖1展示了ICP算法流程，圖1(a)、圖1(b)為第i幀第i+1幀掃描，圖1(c)為匹配開始，從圖1(d)中展開的迭代過程發(fā)現(xiàn)了X與Y幀中的鄰近點，第一次迭代后，ICP得到了X與Y之間的估計坐標變換T1，通過坐標變換可對Y進行位姿調整，如圖1(e)所示。通過重復迭代過程，最終可使Y與X達到接近狀態(tài)，如圖1(f)，最終的坐標變換通過T=Tn，Tn-1,…,T1來求解最終的位姿估計。

xi=TnTn-1…T2T1yi，i=1,2,…,n

(3)

算法流程如下：

輸入：激光點集Xi,Yi

輸出：最終位姿變換T

T=T0

While ||T′|| >e do

For k=1:n do

yk=Tyk//對激光點進行位姿變換

End for

For k=1:n do

xk=FindClosetPoint(X,yk)//迭代尋找鄰近點

End for

For k=1:n do

x′k=xk-xm;y′k=yk-ym//將xk，yk做平移

End for

[U,S,V]=SVD(∑y′k?x′k)//進行SVD分解

R=UVT//得到旋轉向量R

p=xm-Rym//得到平移向量p

T=T′T//將變換矩陣迭代相乘

End while

Return T

2 激光雷達環(huán)境掃描畸變問題分析

每幀激光數(shù)據(jù)都由數(shù)量固定的離散激光束組成[16]。一幀激光數(shù)據(jù)的捕獲并非瞬時發(fā)生。激光雷達的掃描畸變與激光雷達捕獲數(shù)據(jù)時機器人的運動轉換有關。假設以搭載2D激光雷達的移動機器人為研究對象，機器人通過激光雷達獲取環(huán)境數(shù)據(jù)，當激光雷達對環(huán)境進行一幀數(shù)據(jù)掃描時，機器人很可能在此時間段內存在運動變換，這就造成了對環(huán)境的錯誤表達。也就是說，其數(shù)據(jù)捕獲過程不可避免的會伴隨機器人的運動，當激光雷達幀率較低時，機器人的運動就不能被忽略，尤其在機器人進行旋轉運動時，激光雷達的數(shù)據(jù)畸變會尤其明顯。

以圖2所示環(huán)境為例，A為機器人捕獲第i幀數(shù)據(jù)的初始位姿，當機器人完成第i幀數(shù)據(jù)的捕獲，進行i+1幀數(shù)據(jù)掃描時，機器人移動到B點所示位置，那么，從A點到B點的過程機器人恰好完成了一幀數(shù)據(jù)的捕獲，在這個過程中，激光雷達會向圖示環(huán)境發(fā)射離散的激光束，假設機器人在A點發(fā)射了第i束激光束，而在途中的C點發(fā)射了第i幀激光中的第j束激光束。

圖2 第i幀激光Fig.2 ith frame laser

當機器人捕獲一幀數(shù)據(jù)時，機器人會將這一幀數(shù)據(jù)打包進行處理，也就是說，機器人捕獲一幀數(shù)據(jù)的過程中，會認為其自身位姿未發(fā)生變化，但正如上面的情況所述，機器人在捕獲一幀環(huán)境數(shù)據(jù)時，它的自身位姿實際上產生了運動變化。機器人完成一幀數(shù)據(jù)捕獲后對第i幀數(shù)據(jù)進行打包處理時，認為第j束激光束發(fā)生時其位姿仍為A點，最終造成環(huán)境畸變，如圖3所示。

圖3 激光掃描失真問題形成Fig.3 Results of the laser scanning distortion problem

(4)

3 里程計位姿與速度估計融合

3.1 引入里程計位姿估計

為補償激光掃描數(shù)據(jù)，引入里程計信息對激光雷達位姿展開估計，具體是對一幀激光中的每束激光對應的機器人位姿通過里程計信息進行補償。而為實現(xiàn)通過里程計信息對激光雷達位姿進行估計，需要求里程計的更新頻率遠高于激光雷達，同時里程計需能夠對機器人的旋轉和平移運動進行同時測量和估計，因此選用輪式編碼器作為里程計。

假設存在激光幀位姿隊列與里程計位姿隊列分別為Ps0，Ps1,…,Psm;Oo0，Oo1,…,Oon。其中，Psi代表第i束激光束對應的機器人位姿，Ooi代表里程計測量得到的機器人在oi時刻位姿。事實上，在一般情況下，里程計位姿隊列并不能與激光幀位姿隊列保持嚴格對齊。此時由于二者位姿隊列非嚴格對齊，無法直接從里程計位姿隊列中獲取激光幀位姿，此時可通過對里程計線性插值獲取激光幀位姿。

首先求取激光幀起始位姿，以激光幀開始位姿Ps0為例，假設里程計位姿隊列中存在位姿OL、OK，且TL

Ps0=(OKOL)αOL,α∈[0,1]

(5)

式(5)中：Ps0、OK、OL∈ SO(3),且α=(Ts0-TL)/(TK-TL)。

同理可求得激光幀終止位姿Psm。

此時，從激光幀位姿隊列中取K個位姿(K>3),對激光幀位姿隊列進行分段：

Ps0,…,Psk1,Psk2,…,Psm

[Ps0,…,Psk1]

[Psk1,…,Psk2]

?

[Psk,Psm]。

其中，K個位姿中每個位姿都通過對里程計位姿隊列進行線性插值獲取。隨后需要通過分段點位姿對每個隊列子集中的激光點位姿進行位姿轉換：

x′i=PTxi

(6)

式(6)中，x′i為轉換后激光點位姿；xi為轉換前激光點位姿；P為分斷點機器人位姿(里程計位姿)。

算法2實現(xiàn)如下：

輸入：里程計位姿隊列P 激光位姿隊列X

輸出：激光隊列Xn

Ps=LinarInterp(Odemtery);Pe=LinarInterp(Odometry)//對起始位姿進行里程計線性插值

For beamnumber=1:n do//按每幀激光點數(shù)進行循環(huán)

If t>interpolation_time_duration then//設定相應的分段時間

Pmid=LinarInterp(Odometry)//對分段點進行里程計線性插值

While i

End While

P=Pmid//更新分斷點

End if

end

3.2 速度估計與數(shù)據(jù)補償

對機器人位姿進行分段處理后，需對分段子集激光點進行再次數(shù)據(jù)補償。此時對子集內激光雷達的速度進行估計。假設在掃描時間范圍內雷達的速度是恒定的。以Vi表示雷達在ti時刻的速度。首先，從兩幀相鄰激光數(shù)據(jù)集中對應子集Xi和Xi-1之間的相對運動變換中估計速度Vi為

(7)

估算速度Vi用于進一步轉換子集Xi中的每個激光坐標點。假設Xi中的激光點數(shù)量為n，相鄰激光點之間的時間間隔為Δt，那么，x0,x1,…,xn為Xi中的激光點，同時txj-txj-1=Δts(j=0,1,…,n-1)。在Xi中的每個激光點有自己的局部坐標系，短時間內的運動變換T(ti+jΔts)如下：

T(ti+jΔts)=TiejΔtsVi

(8)

(9)

通過式(8)與式(9)可將點集進行運動轉換。當點集中第一個激光點被捕獲時，式(9)所述點集的運動轉換就會發(fā)生。由于第一個激光點與最后一個激光點之間存在不小的時間間隔，這會造成與實際運動之間的延時。為處理這一問題，采用后向補償，以最后一個激光點對應時間為參考時間ti，每個激光點對應時刻為[ti-nΔts,ti-(n-1)Δts,…,ti-Δts,ti]。因此，式(7)修改為

T[ti-(n-j)Δts]=Tie(n-j)Δts(-Vi)

(10)

式(9)也被修改為

(11)

算法的具體實現(xiàn)如下：

輸入：激光點集X

輸出：運動變換T

Vi=Vi-1//速度初始化

TΔts=eΔts(-Vi)//通過速度估計運動變換T

For j=n:1 do//對每個激光點執(zhí)行以下循環(huán)

TjΔts=T(j-1)ΔtsTΔts//計算每個激光點運動變換

End for

V=Vi//速度更新

Vi=1/ΔlgT//進行下一次速度估計

End while

4 仿真實驗

通過仿真環(huán)境分別對VICP與本文改進ICP算法進行仿真對比實驗。仿真環(huán)境使用turtlebot3[17]機器人搭載激光雷達為激光設備，其激光掃描半徑為3.5 m，5 Hz掃描頻率(每幀200 ms)，360°掃描范圍，每幀激光包含360個激光點(1°角分辨率)。測試設備處理器為INTEL CORE I7-6560U CPU 2.20 GHz，內存8 GB。

設置三種仿真環(huán)境，在仿真環(huán)境中，機器人以1 m/s速度運動，通過里程計及激光雷達，同時以里程計、VICP及本文算法記錄三條機器人運動軌跡。由于仿真環(huán)境中的機器人里程計不存在打滑與安裝誤差的問題，這里將里程計得到的軌跡作為真值，同時分別通過VICP及本文算法分別在三種仿真環(huán)境中對兩種算法進行測試。得到仿真結果如圖4所示。

圖4 仿真實驗及軌跡Fig.4 Simulation experiment and trajectory

圖5 VICP及本文算法誤差對比Fig.5 The error comparison of the VICP and our algorithm

根據(jù)里程計，本文給定了機器人的真實運動軌跡，以此軌跡為真值對三種算法誤差進行分析，分別取關鍵幀里程計位姿間平移與旋轉變換與VICP以及本文算法對應激光幀間平移與旋轉變換進行對比，誤差如圖5所示，可看出，本文算法的幀間匹配效果更接近于真值(里程計)，誤差小于VICP。同時分別記錄平移與旋轉的累計誤差，量化兩種算法的誤差對比，累計誤差對比如表1所示。

從仿真實驗效果來看，在三種不同環(huán)境中，VICP與真值軌跡偏離較遠，幀間平移與旋轉偏差較真值相差較大，而本文算法軌跡與真值軌跡較為貼合，幀間平移與旋轉偏差較真值相差腳下。而表1累計誤差顯示，VICP與本文所示算法相比，本文算法結果更優(yōu)，在三種仿真環(huán)境中的激光匹配效果，本文所述算法匹配效果更好，無論平移與旋轉誤差，都明顯低于VICP。

表1 仿真累積誤差Table 1 Accumulated error of simulation experiment

5 實驗與分析

真實實驗環(huán)境為42 m2大小的廳(圖6)。使用turtlebot3_waffle_pi為實驗平臺，激光雷達參數(shù)與仿真環(huán)境相同圖7。所有算法的實現(xiàn)采用C++語言，實現(xiàn)平臺為筆記本計算機，搭載INTEL CORE I7-6560U CPU 2.20 GHz，內存8 GB。由于真實環(huán)境中，機器人的真值軌跡無法精確獲得，因此采用回環(huán)的方式將起始位姿與終止位姿重合，通過算法對機器人位姿進行還原，以回環(huán)效果判斷算法的誤差累計，實驗平臺由人員遠程操控。

圖6 環(huán)境示意圖Fig.6 Environmental diagram

圖7 實驗平臺及環(huán)境Fig.7 Experimental platform and environment

圖6展示了實驗環(huán)境平面圖與移動機器人回環(huán)大致軌跡，機器人從起點到終點完成一個回環(huán)，實驗中機器人分別以平均0.5 m/s及1 m/s速度運動，激光雷達收集到相應激光數(shù)據(jù)集，將此數(shù)據(jù)集用來進行對比實驗。分別通過軌跡展示了三種算法在同一數(shù)據(jù)集下的位姿還原效果，如圖8、圖9所示。

圖8 ICP與本文算法軌跡對比Fig.8 The trajectory comparison of ICP and our algorithm

圖9 VICP與本文算法軌跡對比Fig.9 The trajectory comparison of VICP and our algorithm

圖8、圖9所示軌跡顯示，在不同運行速度下，圖8中，原始ICP算法在激光雷達的連續(xù)畸變效果下，其誤差累積導致無法完成回環(huán)，與真實運動軌跡存在較大誤差，本文改進ICP算法基本完成了回環(huán)，其誤差累計遠小于原始ICP算法。圖9為VICP與本文算法對比，VICP算法的回環(huán)效果好于原始ICP，但仍然出現(xiàn)了軌跡交叉等誤差，并未真正回環(huán)，其軌跡也存在不同程度與真實軌跡的偏移，本文改進ICP算法相較VICP更為接近真實軌跡狀態(tài)，最終的回環(huán)累計誤差如表2所示。

表2 實驗累計誤差Table 2 Accumulated error of experiment

6 結論

(1)激光幀率較低時，雷達的運動會帶來不可忽略的掃描畸變問題，影響最終的ICP算法匹配效果。

(2)單純的速度估計及勻速假設并不能有效消除掃描畸變對ICP匹配的影響，尤其在雷達運動速度并非緩慢的情況。

(3)將里程計輔助位姿估計及速度估計相融合，能夠提供較為精確的激光點位姿，仿真實驗及真實場景實驗均表明，與原始ICP算法及融合速度估計的VICP算法相比，本文算法能夠更好地還原真實的位姿軌跡。