基于三維點(diǎn)云聚類的坡度估計(jì)方法

李海波， 曹云峰， 丁 萌， 莊麗葵(南京航空航天大學(xué)， 江蘇 南京 210016)

1 引 言

為了對(duì)火星進(jìn)行更全面的科學(xué)考察，通常需要探測器著陸到火星表面。探測器在著陸過程中，應(yīng)避免著陸到坡度較大的斜坡上，以免造成側(cè)滑或傾翻[1]。因此探測器在著陸階段，需要對(duì)候選著陸區(qū)域的坡度進(jìn)行估計(jì)?；鹦潜砻娲嬖谳^大的巖石[2],巖石會(huì)使巖石區(qū)域的測量數(shù)據(jù)相對(duì)周圍區(qū)域測量數(shù)據(jù)產(chǎn)生較大的突兀值，影響坡度估計(jì)。目前,對(duì)坡度的估計(jì)方法主要有基于機(jī)器視覺的方法與基于激光雷達(dá)(light detection and ranging，LIDAR)的方法。機(jī)器視覺在計(jì)量領(lǐng)域有多種應(yīng)用[3～5]，其設(shè)備具有體積小、重量輕的優(yōu)點(diǎn)，但獲得的數(shù)據(jù)受光照變化影響大;激光雷達(dá)可以應(yīng)用到不同測量目標(biāo)上[6,7]，其測量數(shù)據(jù)不受光照影響，是一種理想的坡度估計(jì)信息源;文獻(xiàn)[8]提出了一種利用激光雷達(dá)測量數(shù)據(jù)進(jìn)行坡度估計(jì)的方法，該方法采用最小二乘法擬合平面，擬合平面的傾角即為該區(qū)域的坡度;文獻(xiàn)[9]也提出了一種基于激光雷達(dá)測量的高程數(shù)據(jù)進(jìn)行坡度估計(jì)的方法，該方法采用最小平方中位數(shù)法求取局部地形的法向量，法向量與天體重力方向的夾角即認(rèn)為是局部地形的坡度，但該方法需要預(yù)先設(shè)定一些先驗(yàn)參數(shù);文獻(xiàn)[10]針對(duì)激光雷達(dá)測量的數(shù)據(jù)利用最小二乘法擬合一個(gè)平面，求取該平面的法向量，定義法向量與當(dāng)?shù)刂亓铀俣确较蛑g所夾的銳角即為局部地形坡度，也就是擬合平面與基準(zhǔn)面之間的夾角;此外其他相關(guān)文獻(xiàn)中關(guān)于坡度估計(jì)的方法大都基于上述方法，如文獻(xiàn)[11]等。但這些方法對(duì)坡度進(jìn)行估計(jì)均存在估計(jì)誤差較大的問題，為此，本文研究了一種基于三維點(diǎn)云數(shù)據(jù)聚類與隨機(jī)搜索最優(yōu)擬合平面的坡度估計(jì)方法，可以顯著降低估計(jì)誤差，提高坡度估計(jì)精度。

2 坡度估計(jì)原理

2.1 測量數(shù)據(jù)點(diǎn)的稀疏表示

首先定義測量區(qū)域空間坐標(biāo)系，其定義及數(shù)據(jù)點(diǎn)三維表示如圖1所示。空間直角坐標(biāo)系在三維激光掃描測量中起著關(guān)鍵作用[12]，以測量區(qū)域的左下角為原點(diǎn)，x軸與水平邊重合指向正東；y軸與x軸垂直，指向正北；z軸與x軸和y軸垂直并按右手定則確定，見圖1(a)。通過空間坐標(biāo)系，激光雷達(dá)的測量數(shù)據(jù)可用三維坐標(biāo)表示，即Di=[xi,yi,zi]，其中，D表示數(shù)據(jù)點(diǎn)；i為測量點(diǎn)數(shù)據(jù)序號(hào);x、y、z分別表示測量點(diǎn)數(shù)據(jù)在三軸上的坐標(biāo)，見圖1(b)，圖中模擬的數(shù)據(jù)點(diǎn)可用三維坐標(biāo)表示。

圖1 坐標(biāo)系定義及數(shù)據(jù)點(diǎn)三維表示

X[x1…xN]=[X1…Xn]Γ

(1)

式中：Xl∈R3×Nl；?！蔙N×N，是未知的置換矩陣。由于子空間的基是未知的，無法確定測量數(shù)據(jù)點(diǎn)屬于哪個(gè)子空間。為解決這一問題，首先將數(shù)據(jù)點(diǎn)進(jìn)行稀疏化表示，使每一個(gè)數(shù)據(jù)點(diǎn)可以用空間中少數(shù)其他數(shù)據(jù)點(diǎn)進(jìn)行表示，即

xi=Xci,cii=0

(2)

式中：ci[ci1ci2…ciN]T；cii=0為限制條件，防止得到平凡解。由于在子空間中數(shù)據(jù)點(diǎn)的數(shù)量大于數(shù)據(jù)點(diǎn)的維數(shù)，解的形式不唯一，即每個(gè)數(shù)據(jù)點(diǎn)由其他數(shù)據(jù)點(diǎn)表示的方式不唯一，具有多種方式。在這些解中，存在一種稀疏解ci，其解的非零部分與來自同一子空間的數(shù)據(jù)點(diǎn)xi相對(duì)應(yīng)[13]， 即對(duì)于子空間Sl中的任一數(shù)據(jù)點(diǎn)xi，可以用來自同一子空間中的其他數(shù)據(jù)點(diǎn)組合表示。因此，離散數(shù)據(jù)點(diǎn)的稀疏表示能夠發(fā)現(xiàn)屬于同一子空間的數(shù)據(jù)點(diǎn)。由于式(2)中存在多個(gè)解，故采用最小化解的1范數(shù)方式獲得稀疏解，即

(3)

式(3)的解可通過凸規(guī)劃工具求得[14～16]。通過式(3)，將所有數(shù)據(jù)點(diǎn)的稀疏優(yōu)化寫成矩陣形式：

(4)

式中:C[c1c2…cN]∈RN×N，其列向量對(duì)應(yīng)數(shù)據(jù)點(diǎn)的稀疏表示。式(4)的解即為所有數(shù)據(jù)點(diǎn)的稀疏表示，解中非零元素與空間中數(shù)據(jù)點(diǎn)相對(duì)應(yīng)。

2.2 測量數(shù)據(jù)點(diǎn)分割

通過構(gòu)造好的非負(fù)對(duì)稱相似度矩陣W構(gòu)造對(duì)角矩陣A，形式如下：

(5)

式中對(duì)角元素aii(i=1,…,N)為相似度矩陣W中列向量ci所有元素之和。利用對(duì)角矩陣A構(gòu)建Laplacian矩陣，即L=I-A-1/2WA-1/2，I為單位矩陣。對(duì)構(gòu)建的Laplacian矩陣L進(jìn)行SVD(singular value decomposition)分解，對(duì)分解后得到的右正交矩陣V應(yīng)用K-means算法進(jìn)行聚類，從而完成對(duì)數(shù)據(jù)點(diǎn)的分割。

2.3 子空間測量點(diǎn)平面擬合

對(duì)分割后的子空間測量點(diǎn)單獨(dú)進(jìn)行平面擬合，設(shè)某一子空間測量數(shù)據(jù)點(diǎn)預(yù)確定的平面的方程為：

ax+by+cz=d

(6)

式中：a、b、c為所設(shè)定平面單位法向量中的元素值，即單位法向量為(a,b,c)，則有a2+b2+c2=1；d表示原點(diǎn)到所設(shè)定平面的距離，d≥0。

在子空間中隨機(jī)選取一個(gè)數(shù)據(jù)點(diǎn)，計(jì)算與其歐式距離最近的4點(diǎn)，設(shè)這5點(diǎn)為{(xi,yi,zi),i=1,…,5}，由式(6)可知每個(gè)點(diǎn)到平面的距離為

hi=|axi+byi+czi-d|

(7)

式中h表示點(diǎn)到平面的距離。

(8)

當(dāng)e的值最小時(shí)，平面擬合最好。為求取e的最小值，根據(jù)約束條件：a2+b2+c2=1，利用拉格朗日乘子法構(gòu)造函數(shù)如下：

λ(a2+b2+c2-1)

(9)

式中λ為拉格朗日乘子。

將式(9)對(duì)d求偏導(dǎo),并令其為0，即

(10)

整理得

(11)

將式(11)代入式(7)得

(12)

將式(11)代入式(9)得

(13)

將式(13)分別對(duì)a、b、c求偏導(dǎo),并令其為0，

(14)

將式(14)進(jìn)一步整理轉(zhuǎn)換得：

(15)

Bx=μx

(16)

B為3×3實(shí)對(duì)稱矩陣，由矩陣?yán)碚摽芍?/p>

(17)

式中(，)表示兩個(gè)向量做點(diǎn)積運(yùn)算。由于a2+b2+c2=1，所以(x,x)=1，則

(18)

當(dāng)e取最小值時(shí)擬合的平面最好，所以μ應(yīng)取最小值，其對(duì)應(yīng)的特征向量為a、b、c的值。

利用正交三角分解可求出矩陣B的所有特征值，設(shè)最小特征值為μmin，I為單位矩陣。求其對(duì)應(yīng)的特征向量，令

(B-μminI)x=0

(19)

通過式(19)求得的非零解即為對(duì)應(yīng)的特征向量，從而獲得參數(shù)a、b、c的值，即平面的法向量元素值。將所求的a、b、c值代入式(11)中，可求出d的值。

設(shè)子空間中另一數(shù)據(jù)點(diǎn)為Dj={(xj,yj,zj),j=1,…,n,j≠i}，由式(7)可知，其與平面的距離為hj。當(dāng)hj的值小于設(shè)定閾值時(shí)，則該點(diǎn)屬于統(tǒng)計(jì)的點(diǎn)； 當(dāng)hj的值大于或等于設(shè)定閾值時(shí)，該點(diǎn)不屬于統(tǒng)計(jì)的點(diǎn)。

按此方法計(jì)算子空間中其余點(diǎn)與該平面的距離，統(tǒng)計(jì)與平面距離小于設(shè)定閾值的點(diǎn)，當(dāng)統(tǒng)計(jì)的數(shù)目大于預(yù)先設(shè)定值時(shí)，便認(rèn)為該平面為最優(yōu)擬合平面，其法向量為(a,b,c)；若統(tǒng)計(jì)的數(shù)目小于或等于預(yù)先設(shè)定值，便認(rèn)為該點(diǎn)不是最優(yōu)擬合平面，需重新隨機(jī)選取一點(diǎn)，重復(fù)上述步驟，直至統(tǒng)計(jì)的數(shù)目大于預(yù)先設(shè)置值為止。同理，用該方法可以求得另一子空間數(shù)據(jù)點(diǎn)的最優(yōu)擬合平面及法向量。

2.4 坡度角估計(jì)

在獲得兩個(gè)最優(yōu)擬合平面后，可以通過其法向量夾角求得兩個(gè)平面的夾角，設(shè)兩平面的法向量分別為n1、n2，則其夾角可通過下式計(jì)算。

(20)

式中θ為兩法向量之間夾角。根據(jù)幾何關(guān)系，θ在數(shù)值上等于兩平面夾角，即坡度角。

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

隨機(jī)選取兩個(gè)斜坡角度進(jìn)行算法測量實(shí)驗(yàn)，模擬生成的待測量斜坡角度如圖2所示。

如圖2所示兩種斜坡角度對(duì)應(yīng)的模擬激光雷達(dá)測量數(shù)據(jù)點(diǎn)如圖3所示。

圖2 模擬生成的待測量斜坡角度

圖3 斜坡角度測量數(shù)據(jù)點(diǎn)

對(duì)于圖3的測量數(shù)據(jù)點(diǎn)，首先利用2.1節(jié)所述方法進(jìn)行稀疏表示；在此基礎(chǔ)上，利用2.2節(jié)所述方法進(jìn)行數(shù)據(jù)分割，劃分子空間數(shù)據(jù)點(diǎn)；對(duì)子空間數(shù)據(jù)點(diǎn)使用2.3節(jié)所述方法進(jìn)行平面擬合，求出平面法向量；根據(jù)所求法向量，利用公式(20)計(jì)算兩平面夾角，即坡度角，平面擬合及計(jì)算結(jié)果如圖4所示。

對(duì)斜坡角度測量結(jié)果進(jìn)行統(tǒng)計(jì)分析見表1。

表1 測量結(jié)果分析

由表1可以看出，本文所述方法測量誤差極小，測量相對(duì)誤差較小。

考慮到在實(shí)際工作中坡度可能很不規(guī)則及火星地表存在較大巖石等情況，在34°斜坡上模擬添加存在上述情況產(chǎn)生的突出測量值，如圖5(a)中橢圓內(nèi)的測量值。利用2.2、2.3節(jié)所述方法對(duì)數(shù)據(jù)點(diǎn)進(jìn)行分割與尋找最優(yōu)平面擬合數(shù)據(jù)點(diǎn)，數(shù)據(jù)點(diǎn)尋找結(jié)果如圖5(b)所示，可以看出突出測量值數(shù)據(jù)點(diǎn)沒有被統(tǒng)計(jì)到擬合數(shù)據(jù)點(diǎn)范圍內(nèi)，利用圖5(b)所示的數(shù)據(jù)點(diǎn)進(jìn)行平面擬合，結(jié)果如圖5(c)所示。

從圖5(c)可以看出，坡度測量結(jié)果為33.970 7°，與圖4(b)的測量結(jié)果相同，這表明本方法對(duì)突出的測量值具有一定的魯棒性。

圖5 存在突出測量值的坡度估計(jì)

4 與常用方法比較

利用激光雷達(dá)測量數(shù)據(jù)進(jìn)行坡度估計(jì)的常用方法是最小二乘法(least square method,LSM)，將文中提出的方法與最小二乘法進(jìn)行比較。隨機(jī)選取一個(gè)坡度，模擬生成的斜坡角度及測量數(shù)據(jù)點(diǎn)如圖6所示。分別用最小二乘法和本方法對(duì)測量數(shù)據(jù)進(jìn)行處理，計(jì)算結(jié)果如圖7所示。對(duì)測量結(jié)果進(jìn)行比較分析，結(jié)果如表2所示。從表2可以看出，本文提出的方法測量相對(duì)誤差較小；與LSM方法比較，測量誤差得到了顯著降低。

圖6 44°斜坡及測量數(shù)據(jù)點(diǎn)

圖7 測量結(jié)果比較

表2 不同方法測量結(jié)果比較分析

5 結(jié) 論

為降低火星探測器著陸時(shí)對(duì)坡度估計(jì)的誤差，研究了一種基于三維點(diǎn)云數(shù)據(jù)聚類與隨機(jī)搜索最優(yōu)擬合平面的坡度估計(jì)方法。對(duì)于三維點(diǎn)云測量數(shù)據(jù)，首先進(jìn)行數(shù)據(jù)點(diǎn)的稀疏表示；然后根據(jù)稀疏表示系數(shù)對(duì)數(shù)據(jù)點(diǎn)進(jìn)行聚類與分割，劃分子空間；對(duì)子空間中的數(shù)據(jù)點(diǎn)進(jìn)行平面擬合，隨機(jī)搜索最優(yōu)擬合平面；根據(jù)最優(yōu)擬合平面計(jì)算其法向量及向量間夾角，完成坡度估計(jì)。實(shí)驗(yàn)表明：本文方法可以對(duì)坡度進(jìn)行較為準(zhǔn)確的估計(jì),與常用的坡度估計(jì)方法相比較，可以顯著降低估計(jì)誤差，提高坡度估計(jì)精度，為探測器著陸時(shí)決策是否需要規(guī)避機(jī)動(dòng)提供更加準(zhǔn)確的信息。

[參考文獻(xiàn)]

[1] Lunghi P, Ciarambino M, Lavagna M. A multilayer perceptron hazard detector for vision-based autonomous planetary landing[J].AdvancesinSpaceResearch,2016,58(1):131-144.

[2] Craddock R A, Golombek M P. Characteristics of terrestrial basaltic rock populations:Implications for Mars lander and rover science and safety[J].Icarus,2016,274:50-72.

[3] 劉慶民,張蕾,吳立群,等.基于機(jī)器視覺的非均勻分布點(diǎn)圓度誤差評(píng)定[J].計(jì)量學(xué)報(bào),2016,37(6):567-570.

[4] 劉源泂,孔建益,徐福軍,等.基于圖像金字塔的鋼板表面深度信息提取方法[J].計(jì)量學(xué)報(bào),2015,36(4):356-359.

[5] 朱奇光,張興家,陳衛(wèi)東,等.基于顏色矩的改進(jìn)尺度不變特征變換的移動(dòng)機(jī)器人定位算法[J].計(jì)量學(xué)報(bào),2016,37(2):118-122.

[6] 王濤,仲思東.彈丸三維輪廓激光掃描測量方法[J].計(jì)量技術(shù),2015,(2):14-18.

[7] 魏凱,宋述古,劉子勇.基于三維激光掃描原理的球形罐容量計(jì)量方法研究[J].計(jì)量學(xué)報(bào),2015,36(6):607-609.

[8] 梁棟,王鵬基,劉良棟.一種基于LIDAR的精確月球軟著陸目標(biāo)點(diǎn)選定方法[J].空間控制技術(shù)與應(yīng)用,2009,35(6):24-29.

[9] 姜肖楠,韓誠山,李祥之.多傳感器月面障礙模糊識(shí)別方法[J].計(jì)算機(jī)仿真,2013,30(4):97-102.

[10] 張澤旭,王衛(wèi)東,崔平遠(yuǎn),等.一種行星軟著陸地形風(fēng)險(xiǎn)評(píng)估方法[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2011,43(5):25-29.

[11] 于正湜,朱圣英,崔平遠(yuǎn).基于LIDAR的月球著陸區(qū)評(píng)估與選擇方法[C]// 中國宇航學(xué)會(huì)深空探測技術(shù)專業(yè)委員會(huì)第九屆學(xué)術(shù)年會(huì).杭州,2012.

[12] 李明慈,黃桂平,李冰,等.坐標(biāo)轉(zhuǎn)換在應(yīng)用中的問題及解決方法[J].計(jì)量技術(shù),2015,(8):25-28.

[13] Elhamifar E, Vidal R. Sparse subspace clustering:algorithm,theory,and applications[J].IEEETransactionsonPatternAnalysis&MachineIntelligence,2013,35(11):2765-2781.

[14] Boyd S, Vandenberghe L, Faybusovich L. Convex Optimization[J].IEEETransactionsonAutomaticControl,2006,51(11):1859-1859.

[15] Kim S J, Koh K, Lustig M,etal. An Interior-Point Method for Large-Scale l1-Regularized Least Squares[J].IEEEJournalofSelectedTopicsinSignalProcessing,2007,1(4):606-617.

[16] Boyd S, Parikh N, Chu E,etal. Distributed Optimization and Statistical Learning via the Alternating Direction Method of Multipliers[J].FoundationsandTrendsinMachineLearning,2011,3(1):1-122.