裴 東，高文輝，任 琪，但唐旭

(西北師范大學(xué) 物理與電子工程學(xué)院/甘肅省智能信息技術(shù)與應(yīng)用工程研究中心，甘肅 蘭州 730070)

在計(jì)算機(jī)視覺、移動(dòng)機(jī)器人等領(lǐng)域中，由于硬件條件以及環(huán)境因素受限，常使得激光雷達(dá)等傳感器獲得的點(diǎn)云信息出現(xiàn)殘缺、錯(cuò)位，以及在三維物體模型構(gòu)建中需要對(duì)模型進(jìn)行拼接等操作，這些都需要用到點(diǎn)云配準(zhǔn)，而Besl[1]提出的ICP算法是解決點(diǎn)云配準(zhǔn)問題的常用方法[2-3].ICP算法是2組點(diǎn)云按照一定的約束條件，通過KD-Tree搜索算法[4-5]找到一組點(diǎn)云在另外一組點(diǎn)云中的最近鄰點(diǎn)，然后將點(diǎn)到點(diǎn)的誤差度量作為目標(biāo)函數(shù)，也可以將點(diǎn)到線[6]或點(diǎn)到面[7]的誤差度量作為目標(biāo)函數(shù)，通過ICP算法計(jì)算出2組點(diǎn)云在空間中的變換位姿，將不同坐標(biāo)系的點(diǎn)云數(shù)據(jù)合并到同一個(gè)坐標(biāo)系中.ICP算法對(duì)初值比較敏感，每次配準(zhǔn)都要對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行全局搜索查找對(duì)應(yīng)點(diǎn)，而且點(diǎn)云稀疏區(qū)域的配準(zhǔn)容易受到密集區(qū)域的影響，這將會(huì)造成計(jì)算量過大以及ICP噪聲和離群值等問題.針對(duì)傳統(tǒng)的ICP算法的缺點(diǎn)，研究者在ICP算法的基礎(chǔ)上進(jìn)行了改進(jìn)，提出了PICP(Probability ICP)[8]、MICP(Modified ICP)[9]和 CICP(Cluster ICP)[10]以及引入熵[11]等方法.

ICP算法求解方式主要有2種，一種是利用線性代數(shù)的方式求解；另一種是利用非線性優(yōu)化的方式求解，即最小二乘法.最小二乘法的解法通常為迭代法，包括最速下降法、Newton法、Gauss-Newton(GN)法和Levenberg-Marquardt(LM)法.最速下降法過于貪心，容易走出鋸齒路線從而增加迭代次數(shù)，牛頓法需要計(jì)算目標(biāo)函數(shù)的海森矩陣H，在問題規(guī)模較大時(shí)海森矩陣的計(jì)算將會(huì)耗費(fèi)更多資源.使用GN法和LM法時(shí)需要求解雅可比矩陣J，但歐拉角求解雅可比矩陣的常用方法是將歐拉角分步驟轉(zhuǎn)換為繞x軸、y軸和z軸旋轉(zhuǎn)的3個(gè)旋轉(zhuǎn)矩陣，然后分別對(duì)3個(gè)旋轉(zhuǎn)矩陣做相應(yīng)的變換從而達(dá)到間接求解雅可比矩陣的目的.

1 問題描述

?pi∈P,?qi∈Q|=qi-Tpi==0,

(1)

其中，T為第i對(duì)配準(zhǔn)點(diǎn)pi和qi之間的變換矩陣.變換T由平移t和旋轉(zhuǎn)R兩種操作構(gòu)成.變換過程如下

(2)

為求出旋轉(zhuǎn)矩陣R和平移向量t，需要構(gòu)建配準(zhǔn)點(diǎn)Tpi即Rpi+t和qi之間的偏差ep，下標(biāo)p表示兩個(gè)點(diǎn)云數(shù)據(jù)集點(diǎn)與點(diǎn)的配準(zhǔn)方式

ep=qi-(Rpi+t).

(3)

為方便計(jì)算，取ep的二范數(shù)的平方項(xiàng)作為點(diǎn)到點(diǎn)的損失函數(shù)，求出損失函數(shù)取得最小時(shí)R和t的值.

上述變換矩陣T的求解過程是利用點(diǎn)到點(diǎn)之間的距離作為誤差度量.由于2組點(diǎn)云表示的點(diǎn)不可能是相同位置的點(diǎn)，所以點(diǎn)到點(diǎn)的距離作為誤差度量會(huì)引入隨機(jī)誤差.為了減小這種隨機(jī)誤差帶來的影響，采用點(diǎn)到面的距離.當(dāng)使用點(diǎn)到面的距離時(shí)，誤差度量是每個(gè)源點(diǎn)與其對(duì)應(yīng)目標(biāo)點(diǎn)所在切平面之間的距離.取源數(shù)據(jù)集P中的任意一點(diǎn)pi和目標(biāo)數(shù)據(jù)集Q中與之對(duì)應(yīng)的點(diǎn)qi，點(diǎn)到面之間的距離誤差可以用en來表示，下標(biāo)n表示2個(gè)點(diǎn)云數(shù)據(jù)集點(diǎn)到面的配準(zhǔn)方式

(4)

其中l(wèi)i為qi所在區(qū)域的單位法向量.取en的平方項(xiàng)作為點(diǎn)到面的損失函數(shù)，求出使損失函數(shù)取得最小值時(shí)R和t的值.(3)式和(4)式分別給出了ICP中涉及到的點(diǎn)到點(diǎn)配準(zhǔn)以及點(diǎn)到面配準(zhǔn)所用到的損失函數(shù).對(duì)于兩類損失函數(shù)，都需要求解出使它們各自取得最小值時(shí)的R和t.

2 問題求解

下面將從流形和非流形不同的解決方法中分別介紹點(diǎn)到點(diǎn)和點(diǎn)到面對(duì)應(yīng)的損失函數(shù)中t和R的求解方法.

為求使損失函數(shù)取得最小時(shí)t和R的值，首先將誤差e進(jìn)行一階泰勒展開

e(T+ΔT)≈e(T)+J(T)TΔT,

(5)

求出損失函數(shù)關(guān)于ΔT的導(dǎo)數(shù)，并令其為零

J(T)Te(T)+J(T)TJ(T)ΔT=0,

(6)

令

(7)

則迭代步長

(8)

得到GN法的實(shí)現(xiàn)流程如算法1所示.

算法1Gauss-newton method

begin

k=0

H=JTJ;G=JTe

while(notfound)and(k

begin

k=k+1;SolveHΔT=-G

T=T+ΔT

end

end

LM法如算法2所示.

算法2Levenberg-Marquardt method

begin

λ=0.01;v0=2;k=0

H=JTJ;G=JTe

while(notfound)and(k

begin

k=k+1;Solve (H+λI)ΔT=-G

Tnew=T+ΔT

ρ=ΔC/ΔS

ifρ>0

T=Tnew

else

λ=λ*v;v=*v

end

end

第1節(jié)介紹了點(diǎn)到點(diǎn)以及點(diǎn)到面的損失函數(shù).位姿變換T可以用平移t(tx,ty,tz)和旋轉(zhuǎn)R(α,β,γ)來表示.平移向量中的參數(shù)tx,ty和tz分別表示3維坐標(biāo)系中x,y,z3個(gè)坐標(biāo)軸方向上的偏移量.旋轉(zhuǎn)矩陣的參數(shù)α,β和γ分別表示俯仰角、偏航角以及翻滾角.

2.1 非流形方法

非流形的求解方式是求解雅可比矩陣的常用方式.需要將α,β和γ轉(zhuǎn)換成相應(yīng)的旋轉(zhuǎn)矩陣Rx(α),Ry(β)和Rz(γ)，分別對(duì)旋轉(zhuǎn)矩陣Rx(α),Ry(β)和Rz(γ)中的α,β和γ進(jìn)行求導(dǎo)，得到

然后通過一定的運(yùn)算規(guī)則求解出雅可比矩陣.本小節(jié)將介紹非流形算法中點(diǎn)到點(diǎn)以及點(diǎn)到面的距離為誤差度量時(shí)雅可比矩陣的推導(dǎo)過程以及ICP 迭代方式中如何采用非流形的方式求解雅克比矩陣.

2.2 流形方法

下面將介紹 ICP 迭代方式中如何采用流形算法.假設(shè)物體表面有一點(diǎn)τ，在τ點(diǎn)處的切空間就是其自身的局部坐標(biāo)系空間.向量空間中的元素映射到李代數(shù)空間，該過程用(·)^表示.李代數(shù)空間的元素經(jīng)過指數(shù)運(yùn)算映射到李群空間中.同理，李群空間中的元素經(jīng)過對(duì)數(shù)運(yùn)算映射到李代數(shù)空間.

2.2.1 點(diǎn)到點(diǎn) 2.1節(jié)中非流形的 ICP 是在切空間對(duì)歐拉角進(jìn)行的分解變換求導(dǎo)等操作，流形中對(duì)雅可比矩陣J的求解放在李群流形空間中.在李群空間中，假設(shè)對(duì)三維點(diǎn)p進(jìn)行了旋轉(zhuǎn)，得到Rp，如果對(duì)Rp中的R進(jìn)行求導(dǎo)，可以看成對(duì)R進(jìn)行了一次左擾動(dòng)ΔR.設(shè)左擾動(dòng)ΔR對(duì)應(yīng)的李代數(shù)為θ,則結(jié)果相對(duì)于擾動(dòng)的變換率為

所以流形中的雅可比矩陣

J=[-I3-(Rp)^].

(16)

流形中點(diǎn)到點(diǎn)的GN法中的誤差ep和非流形解法相似.在求取J和e后，對(duì)每次迭代求H和G，同樣地，H和G的求解方式參考(7)式.而后求取迭代步長ΔT.此時(shí)迭代步長ΔT為切空間中的步長，需要將其轉(zhuǎn)換到流形空間

將得到的變換步長ΔTi與變換矩陣Ti相乘得到下一次的變換矩陣最終得到變換矩陣

2.2.2 點(diǎn)到面 對(duì)比流形中點(diǎn)到點(diǎn)與非流形點(diǎn)到面的 GN法，流形中點(diǎn)到面的高斯牛頓法求解變換矩陣T所需要的雅可比矩陣J的求解形式為

J=[-lTI3-lT(Rp)^].

(19)

3 實(shí)驗(yàn)分析

下面將在斯坦福數(shù)據(jù)集中通過平移和旋轉(zhuǎn)，分別對(duì)流形和非流形中點(diǎn)到點(diǎn)及點(diǎn)到面的 LM以及 GN法進(jìn)行實(shí)驗(yàn).通過流形和非流形的方式先對(duì)KIT數(shù)據(jù)集中的相鄰兩幀數(shù)據(jù)進(jìn)行實(shí)驗(yàn)，而后對(duì)前1 550幀數(shù)據(jù)進(jìn)行位姿計(jì)算，通過位姿還原傳感器的運(yùn)動(dòng)軌跡.所有的程序在配置為Intel(R)Core(TM)i5-7500 CPU @ 3.40GHz 和 16 GB RAM 的計(jì)算機(jī)上使用Matlab 2020a 編寫.

3.1 斯坦福數(shù)據(jù)中的對(duì)比

使用斯坦福三維掃描庫比較 ICP 非流形和流形算法性能差異.先對(duì)原始數(shù)據(jù)使用方形網(wǎng)格過濾器進(jìn)行降采樣以提高運(yùn)行速度，其中方形網(wǎng)格長度為 0.002.然后對(duì)源點(diǎn)云集做一次位姿變換產(chǎn)生目標(biāo)點(diǎn)云集.迭代的初始位姿為4×4的單位陣.迭代次數(shù)最大為 50，根據(jù)數(shù)據(jù)集尺度的大小確定閾值，去除K-D樹中歐氏距離大于該閾值的配準(zhǔn)點(diǎn).當(dāng)?shù)介L范數(shù)小于1×10-6時(shí)，停止迭代.為方便標(biāo)記，使用英文大寫字母表示下面用到的一些專業(yè)名稱，流形(M)、非流形(N)、點(diǎn)到面(S)、點(diǎn)到點(diǎn)(P)、GN法(G)、LM法(L).例如 ICP 非流形點(diǎn)到點(diǎn)GN法可以簡寫為 NPG，依此類比.

將掃描庫中的一幀點(diǎn)云數(shù)據(jù)集作為目標(biāo)點(diǎn)云，如圖1(a)所示，然后將其繞x軸順時(shí)針旋轉(zhuǎn)30°，將變換后的點(diǎn)云為源點(diǎn)云圖1(b).經(jīng)過ICP 求解出位姿，經(jīng)過位姿變換的源點(diǎn)云與目標(biāo)點(diǎn)云的配準(zhǔn)結(jié)果如圖1(c)所示.

圖1 ICP 配準(zhǔn)結(jié)果

圖2給出了ICP算法的迭代偏差.從圖2a和2b可以看出使用非流形的迭代偏差與使用流形的迭代偏差相似.使用點(diǎn)到點(diǎn)的求解方式比使用點(diǎn)到面的求解方式在該實(shí)例中收斂更快.

對(duì)流形和非流形中點(diǎn)到點(diǎn)與點(diǎn)到面的GN和LM法分別在同樣點(diǎn)云數(shù)據(jù)集位姿變換中進(jìn)行了實(shí)驗(yàn)，得到8種算法的計(jì)算時(shí)間和迭代次數(shù),如圖3所示.結(jié)果表明,非流形和流形中點(diǎn)到點(diǎn)迭代次數(shù)為9次， 少于點(diǎn)到面的迭代次數(shù)12次. 而同等條件下，流形完成迭代所用的時(shí)長比較流形完成迭代所用的時(shí)長短.點(diǎn)到點(diǎn)相比點(diǎn)到面消耗的時(shí)間更短.算法的計(jì)算誤差包括旋轉(zhuǎn)誤差和平移誤差.旋轉(zhuǎn)誤差弧度Δθ為

圖2 ICP迭代偏差

圖3 使用斯坦福數(shù)據(jù)集ICP算法不同方法的迭代次數(shù)與執(zhí)行時(shí)長

Δt==tTrue-tICP=2,

(21)

即平移誤差Δt為平移向量偏差的二范數(shù).

從圖4中可以看出，使用點(diǎn)到面的計(jì)算誤差接近于零.流形和非流形的計(jì)算誤差結(jié)果相似.實(shí)驗(yàn)結(jié)果表明使用點(diǎn)到點(diǎn)作為配準(zhǔn)點(diǎn)云計(jì)算偏差小于使用點(diǎn)到面作為計(jì)算配準(zhǔn)點(diǎn)云偏差；使用流形的方式進(jìn)行迭代要比使用非流形的方式效果好.

圖4 使用斯坦福數(shù)據(jù)集ICP算法不同方法的計(jì)算誤差

3.2 KITTI數(shù)據(jù)中的對(duì)比

使用 KITTI數(shù)據(jù)集來對(duì)比算法使用流形與流形的性能.因?yàn)?KITTI 的數(shù)據(jù)不成均勻分布的狀態(tài)，所以與對(duì) 3.1 節(jié)中斯坦數(shù)據(jù)集的處理過程不同，使用隨機(jī)采樣的方式對(duì)原始數(shù)據(jù)集進(jìn)行降采樣.本實(shí)驗(yàn)的采樣率為3%.采樣的原始數(shù)據(jù)集和目標(biāo)數(shù)據(jù)集分別為間隔兩幀的激光數(shù)據(jù).同樣地，設(shè)定最大迭代次數(shù)為50次，每次迭代去除大于K-D tree 配準(zhǔn)點(diǎn)歐式距離中大于設(shè)定閾值(2 m)的數(shù)據(jù)點(diǎn).當(dāng)?shù)介L小于0.001 的時(shí)候停止迭代.由于點(diǎn)到點(diǎn)的ICP 配準(zhǔn)魯棒性較差，本小節(jié)僅使用點(diǎn)到面的方式對(duì)流形和非流形的計(jì)算方法進(jìn)行比較.4種方式分別為NSG,NSL,MSG和MSL.

取KITTI 數(shù)據(jù)集中的一幀數(shù)據(jù)以及與其間隔2幀的數(shù)據(jù)分別作為源點(diǎn)云和目標(biāo)點(diǎn)云.2幀數(shù)據(jù)的圖像如圖5所示.4種算法完成配準(zhǔn)后的圖像相似,所以使用經(jīng)過MSL 配準(zhǔn)后的圖像來展現(xiàn)配準(zhǔn)的結(jié)果.2種方式的迭代次數(shù)相同，但SG和SL的迭代時(shí)間流形相比非流形分別提高了53%和49%,如圖6所示.

從圖7中可以看到4種算法的平移誤差近似相等，旋轉(zhuǎn)誤差GN略高于LM.實(shí)驗(yàn)數(shù)據(jù)表明流形和非流形的迭代次數(shù)及計(jì)算誤差近似相等，但算法消耗的時(shí)長明顯減少.

圖5 KITTI兩幀點(diǎn)云數(shù)據(jù)ICP配準(zhǔn)結(jié)果

圖6 使用KITTI數(shù)據(jù)集ICP算法不同方法的迭代次數(shù)與執(zhí)行時(shí)長

圖7 使用KITTI數(shù)據(jù)集ICP算法不同方法的計(jì)算誤差

3.3 KIT數(shù)據(jù)還原軌跡對(duì)比

上述實(shí)驗(yàn)過程僅進(jìn)行了2幀點(diǎn)云之間的配準(zhǔn)，下面使用 KITTI數(shù)據(jù)集中的 1 550 幀3D 激光點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)測試.經(jīng)過測試發(fā)現(xiàn) NSG 和 MSG 兩種的魯棒性能夠保證每一幀點(diǎn)云的完整匹配，所以流形和非流形對(duì)比將使用點(diǎn)到面的 GN 算法.為了保證配準(zhǔn)精度，將原始的點(diǎn)云數(shù)據(jù)進(jìn)行降采樣時(shí)采用隨機(jī)抽取10%點(diǎn)云的方式，其余的參數(shù)配置與 3.2 節(jié)中一致.

圖8為使用ICP迭代計(jì)算得到的位姿繪制的軌跡和誤差.可以看出，流形和非流形兩種方式得到的軌跡之間的偏差較小，原因是兩種迭代方式在進(jìn)行隨機(jī)降采樣后的點(diǎn)云數(shù)據(jù)并不一致，這將會(huì)導(dǎo)致計(jì)算得到的位姿不一樣.由圖8(b)可以看出流形與非流形的計(jì)算得到的位姿因?yàn)檎`差不斷累積，導(dǎo)致最終計(jì)算出的軌跡和真實(shí)軌跡均有較大差距.非流形和流形迭代所消耗的時(shí)長分別為12 614 s,5 421 s.非流形平均每幀時(shí)長為8.1381 s，而流形平均每幀時(shí)長為3.4974 s.經(jīng)過計(jì)算得出ICP 迭代中雅克比的計(jì)算使用流形比使用非流形的方式效率提高了約57%.

圖8 ICP算法解算后的軌跡對(duì)比

4 結(jié)論

文中針對(duì)傳統(tǒng)的最小二乘法在數(shù)據(jù)量較大的情況下配準(zhǔn)速度慢的問題，提出了一種基于流形求解雅可比矩陣的最小二乘法.該方法通過改變目標(biāo)函數(shù)關(guān)于變量的求導(dǎo)思路，將成同構(gòu)關(guān)系向量空間中的元素通過李代數(shù)空間映射到李群流形空間中，通過對(duì)流行空間進(jìn)行擾動(dòng)的方式解出所求矩陣的導(dǎo)數(shù)，進(jìn)而實(shí)現(xiàn)對(duì)迭代的優(yōu)化.3個(gè)實(shí)驗(yàn)驗(yàn)證優(yōu)化算法的有效性，前2個(gè)實(shí)驗(yàn)分別在斯坦福和KITTI數(shù)據(jù)集中進(jìn)行算法性能的對(duì)比，第3個(gè)實(shí)驗(yàn)對(duì)比多幀數(shù)據(jù)配準(zhǔn)時(shí)二者的執(zhí)行效率和計(jì)算精度.實(shí)驗(yàn)結(jié)果表明，在點(diǎn)云數(shù)據(jù)相同的情況下流形的ICP算法較非流形的ICP算法迭代偏差與迭代次數(shù)基本一致，但迭代效率有顯著提升.文中提出的ICP流形迭代對(duì)初始位姿有較高的要求，如果2幀點(diǎn)云數(shù)據(jù)位姿偏差較大很容易陷入局部最優(yōu)解.后續(xù)將針對(duì)改變點(diǎn)云的分布對(duì)ICP的影響進(jìn)行研究.