張瑞賓 郭應(yīng)時(shí) 陳元華 劉曉剛 周揚(yáng)

（1.長安大學(xué)，西安 710064；2.桂林航天工業(yè)學(xué)院，桂林 541004）

主題詞：激光雷達(dá) 運(yùn)動(dòng)車輛 點(diǎn)法向量 位姿估計(jì)

1 前言

智能車輛通過激光雷達(dá)等環(huán)境感知系統(tǒng)對(duì)運(yùn)動(dòng)車輛位姿（Moving Vehicle Pose，MVP）進(jìn)行準(zhǔn)確估計(jì)，可有效提高危險(xiǎn)交通場景下的安全性。國內(nèi)外學(xué)者針對(duì)運(yùn)動(dòng)物體的位姿展開了相關(guān)研究。王巖等人[1]提出了一種基于迭代最近點(diǎn)（Iterative Closest Point，ICP）的井下無人機(jī)實(shí)時(shí)位姿估計(jì)方法，將煤礦井下四旋翼無人機(jī)位姿估計(jì)問題轉(zhuǎn)換為機(jī)載激光點(diǎn)云數(shù)據(jù)的掃描匹配問題。宋亮等人[2]提出了基于擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）的相對(duì)位姿估計(jì)算法，對(duì)相對(duì)位置和姿態(tài)進(jìn)行估計(jì)的同時(shí)，可辨識(shí)出目標(biāo)的未知參數(shù)。郝剛濤等人[3]提出一種基于單目相機(jī)與無掃描三維激光雷達(dá)融合的非合作目標(biāo)相對(duì)位姿估計(jì)方法，可實(shí)現(xiàn)尺度模糊下相對(duì)位姿的快速魯棒估計(jì)。暢春華等人[4]提出一種基于二維激光雷達(dá)的移動(dòng)機(jī)器人實(shí)時(shí)位姿估計(jì)算法，通過優(yōu)化位置序列間的方向角實(shí)現(xiàn)機(jī)器人的位姿估計(jì)。S.Akshay 等人[5]提出了一種基于無人機(jī)周圍點(diǎn)云特征的激光雷達(dá)定位誤差協(xié)方差矩陣自適應(yīng)估計(jì)方法，該方法比基于距離的協(xié)方差矩陣模型更精確。Opromolla等人[6]提出了基于在軌服務(wù)的激光雷達(dá)三維點(diǎn)云姿態(tài)估計(jì)算法，通過姿態(tài)跟蹤算法對(duì)姿態(tài)進(jìn)行校正，通過將模型點(diǎn)云和目標(biāo)點(diǎn)云進(jìn)行ICP 匹配實(shí)現(xiàn)姿態(tài)跟蹤。Y.Fang等人[7]提出了全等四面體對(duì)齊（Congruent Tetrahedron Align，CTA）算法，通過激光雷達(dá)掃描點(diǎn)云和模型點(diǎn)云中的全等四面體確定相對(duì)位姿，采用ICP算法進(jìn)行CTA后的姿態(tài)跟蹤。N.Philip等人[8]提出了基于相平面控制技術(shù)的相對(duì)位置控制和基于相對(duì)四元數(shù)反饋的相對(duì)姿態(tài)控制方法，證明了固定增益觀測器對(duì)位置和姿態(tài)的估計(jì)是有效的。

以上位姿估計(jì)方法在點(diǎn)云幾何信息較完整、分布特征顯著時(shí)是可行的。但由于激光雷達(dá)掃描視場有限，每幀僅能獲取單一視角下的場景，導(dǎo)致點(diǎn)云數(shù)據(jù)存在遮擋殘缺等現(xiàn)象，獲取目標(biāo)表面采樣點(diǎn)數(shù)量取決于激光雷達(dá)與目標(biāo)間的相對(duì)位置，從而影響位姿估計(jì)準(zhǔn)確率。因此，本文提出一種基于點(diǎn)法向量特征的方法，以解決動(dòng)態(tài)剛性目標(biāo)點(diǎn)云殘缺導(dǎo)致位姿信息難以提取的問題。

2 基于點(diǎn)法向量的目標(biāo)姿態(tài)估計(jì)算法

2.1 算法的提出

在傳統(tǒng)位姿估計(jì)算法中，要求模型庫具有較大樣本數(shù)量，且待測目標(biāo)三維點(diǎn)云數(shù)據(jù)具有較高完整性，為了減少算法對(duì)以上條件的限制，基于目標(biāo)車輛表面所具有的剛性特點(diǎn)，結(jié)合車載激光雷達(dá)采集運(yùn)動(dòng)目標(biāo)車輛點(diǎn)云數(shù)量隨距離變化的特性，本文提出基于點(diǎn)法向量的動(dòng)態(tài)車輛實(shí)時(shí)位姿估計(jì)方法，流程如圖1所示。

圖1 算法流程

2.2 點(diǎn)云數(shù)據(jù)的獲取

車載激光雷達(dá)安裝位置如圖2所示，在車輛縱向中心平面上，A點(diǎn)位于車輛背門最高處，B點(diǎn)位于發(fā)動(dòng)機(jī)罩最前端，C點(diǎn)和D點(diǎn)分別為A柱在縱向中心平面投影處的最高點(diǎn)和最低點(diǎn)，F(xiàn)點(diǎn)為車載激光雷達(dá)的幾何中心。取A點(diǎn)在地平面的投影點(diǎn)O作為坐標(biāo)系原點(diǎn)，以O(shè)為起點(diǎn)，在縱向中心平面內(nèi)沿地平面指向車頭為X軸正方向，過點(diǎn)O垂直縱向中心平面指向駕駛員側(cè)為Y軸正方向，過點(diǎn)O垂直地平面指向車身為Z軸正方向。A′與A′′、B′與B′′、C′與C′′分別關(guān)于A、B、C點(diǎn)對(duì)稱。激光雷達(dá)中心坐標(biāo)為(x0,y0,z0)，其輪廓底部投影對(duì)應(yīng)直線llow，安裝激光雷達(dá)時(shí)，為了保證點(diǎn)云采集的對(duì)稱性，應(yīng)將雷達(dá)置于車頂中心處，且確保其工作時(shí)不會(huì)掃描到車身（即關(guān)鍵點(diǎn)A、B、C、D），即滿足：

式中，ψlow為垂直視場角；zA、zB、zC、zD、z0分別為A、B、C、D、O點(diǎn)的z坐標(biāo)；xA、xB、xC、xD、x0分別為A、B、C、D、O點(diǎn)的x坐標(biāo)。

選用速騰聚創(chuàng)公司RS-LiDAR-32 型激光雷達(dá)，該激光雷達(dá)可實(shí)現(xiàn)垂直視場角-25°～15°、水平視角360°掃描周圍環(huán)境，測距最大范圍200 m，每秒輸出64×104個(gè)點(diǎn)，設(shè)置掃描頻率為20 Hz。

圖2 車載激光雷達(dá)安裝示意

2.3 平面點(diǎn)的判定及點(diǎn)法向量估計(jì)

本文通過剛性目標(biāo)表面局部擬合平面上的點(diǎn)法向量進(jìn)行姿態(tài)估計(jì)，以彎曲度特征濾除非平面上的點(diǎn)法向量。表面點(diǎn)云任意點(diǎn)P的彎曲度CP滿足：

由式（2）可知：當(dāng)CP近似為0或小于一定的閾值時(shí)，可判定P點(diǎn)為平面點(diǎn)；當(dāng)CP大于一定的閾值時(shí)，可判定P點(diǎn)為非平面點(diǎn)。該閾值可依據(jù)實(shí)際點(diǎn)云中的噪聲及其他誤差影響確定。

2.4 自適應(yīng)聚簇中心的點(diǎn)法向量分類方法

利用最小二乘法擬合求得目標(biāo)車輛三維點(diǎn)云各點(diǎn)的法向量后，由于車輛屬于剛性物體，表面由不同角度的近似平面組成，各表面估計(jì)的點(diǎn)法向量的方向不同，因此需要對(duì)全部點(diǎn)法向量進(jìn)行聚類分析。傳統(tǒng)的Kmeans 算法的聚簇?cái)?shù)量k是根據(jù)經(jīng)驗(yàn)事先人為設(shè)定的，由隨機(jī)方式確定初始聚類中心，車載激光雷達(dá)采集周圍運(yùn)動(dòng)車輛點(diǎn)云數(shù)據(jù)的角度實(shí)時(shí)發(fā)生變化，復(fù)雜場景下目標(biāo)物間存在遮擋問題，導(dǎo)致獲取目標(biāo)點(diǎn)云平面的數(shù)量存在差異，聚簇?cái)?shù)量應(yīng)與獲取的平面數(shù)量相關(guān)聯(lián)，應(yīng)采用自適應(yīng)聚簇中心的聚類算法流程確定聚簇?cái)?shù)量，算法自適應(yīng)性主要體現(xiàn)在根據(jù)目標(biāo)點(diǎn)云質(zhì)心向量與點(diǎn)法向量之間的夾角分布來自適應(yīng)求取k和各聚簇中心值c(k)。算法具體步驟為：

a.求取車輛目標(biāo)點(diǎn)云坐標(biāo)原點(diǎn)指向質(zhì)心的向量C；

b.求取每個(gè)點(diǎn)法向量與C之間的夾角θi；

c.求取θi分布直方圖，如圖3 所示，篩選出頻數(shù)f大于閾值λ的點(diǎn)法向量分布區(qū)間種類數(shù)量即為聚簇?cái)?shù)量k；

d.求取所篩選出的各類分布區(qū)間內(nèi)的點(diǎn)法向量數(shù)量；

e.求取每類點(diǎn)法向量均值，得到c(k)；

f.求取標(biāo)準(zhǔn)測度函數(shù)，評(píng)價(jià)其收斂性，得到最大迭代次數(shù)后停止，否則，循環(huán)a～e。最終得到點(diǎn)法向量聚類結(jié)果如圖4所示。

圖3 θi分布直方圖

2.5 坐標(biāo)軸的確定

由于復(fù)雜場景下聚類獲得的點(diǎn)云數(shù)據(jù)受到噪聲和遮擋因素的影響，點(diǎn)云并不集中在某一平面內(nèi)，導(dǎo)致待擬合平面的點(diǎn)法向量分布不均勻，因此應(yīng)選取抗噪性能較好的隨機(jī)采樣一致性（Random Sample Consensus，RSC）算法對(duì)聚類后的點(diǎn)進(jìn)行二次處理。

圖4 點(diǎn)法向量聚類結(jié)果

RSC算法先從樣本集合中隨機(jī)抽取一個(gè)樣本子集，采用最小方差估計(jì)算法計(jì)算該子集模型參數(shù)，對(duì)比樣本與模型間的偏差，利用預(yù)先設(shè)定的閾值與偏差比較，當(dāng)偏差小于等于閾值時(shí)，該樣本點(diǎn)被定義為內(nèi)點(diǎn)（Inliers），否則為外點(diǎn)（Outliers）。RSC 可以剔除外點(diǎn)的影響，最終得到全局最優(yōu)的參數(shù)估計(jì)值。RSC算法的關(guān)鍵在于確定閾值d、迭代次數(shù)K[9]：

式中，E、F、G、H為常量；p為置信度；w為內(nèi)點(diǎn)在數(shù)據(jù)集內(nèi)的占比；m為計(jì)算模型最小樣本數(shù)量。

算法具體步驟為：

a.從待擬合的點(diǎn)云集合中隨機(jī)選取3 個(gè)點(diǎn)，計(jì)算其構(gòu)成的擬合平面方程z=ax+by+c；

b.計(jì)算點(diǎn)云中各點(diǎn)至該平面的距離di=|axi+byi+zi|；

c.利用式（3）計(jì)算出閾值d，當(dāng)di≤d，判定該點(diǎn)為內(nèi)點(diǎn)，并統(tǒng)計(jì)內(nèi)點(diǎn)數(shù)量；

d.利用式（4）計(jì)算迭代次數(shù)K，通過迭代運(yùn)算統(tǒng)計(jì)出最多數(shù)量的內(nèi)點(diǎn)集合，并對(duì)該情況下獲取的內(nèi)點(diǎn)進(jìn)行平面擬合。

圖5 所示為采用目標(biāo)車輛表面點(diǎn)云擬合的平面點(diǎn)法向量，由圖5 可知，三維點(diǎn)云場景下的待測目標(biāo)坐標(biāo)系中X軸和Z軸即為剛性物體所占比重最大的2類點(diǎn)云垂直擬合平面的代表點(diǎn)法向量n和o，Y軸可通過n和o叉積的形式獲得，目標(biāo)車輛表面擬合平面點(diǎn)法向量與OXYZ坐標(biāo)系的關(guān)系如圖6所示。

2.6 目標(biāo)姿態(tài)的求取

確定出擬合平面代表法向量n(nx,ny,nz)、o(ox,oy,oz)、a(ax,ay,az)后，通過歐拉角旋轉(zhuǎn)矩陣對(duì)運(yùn)動(dòng)目標(biāo)的橫滾角?、俯仰角θ和航偏角ψ進(jìn)行解算：

由式（5）～式（7）解算出的(?,θ,ψ)即為目標(biāo)物三維姿態(tài)角。

圖5 目標(biāo)車輛表面點(diǎn)云擬合的平面點(diǎn)法向量

圖6 擬合平面點(diǎn)法向量與OXYZ坐標(biāo)系的關(guān)系

3 姿態(tài)估計(jì)算法試驗(yàn)

為了定量地分析所提出的動(dòng)態(tài)車輛實(shí)時(shí)姿態(tài)估計(jì)算法的效果，利用試驗(yàn)車采集某城市復(fù)雜交叉口點(diǎn)云數(shù)據(jù)，結(jié)果如圖7 所示，該車頂部裝配有三維激光雷達(dá)。激光雷達(dá)以10 幀/s 的掃描幀率，獲得目標(biāo)車輛運(yùn)動(dòng)時(shí)車輛運(yùn)動(dòng)點(diǎn)云圖，如圖8所示。采用基于點(diǎn)法向量的動(dòng)態(tài)車輛實(shí)時(shí)位姿估計(jì)方法，得到如圖9所示的連續(xù)位姿估計(jì)結(jié)果，通過計(jì)算車輛質(zhì)心點(diǎn)處點(diǎn)法向量位置變化，實(shí)時(shí)估計(jì)車輛位姿。

圖7 試驗(yàn)車采集的某城市復(fù)雜交叉口點(diǎn)云數(shù)據(jù)

圖8 激光雷達(dá)連續(xù)幀掃描目標(biāo)車輛點(diǎn)云

圖9 連續(xù)位姿估計(jì)結(jié)果

為了驗(yàn)證算法的有效性，采用均方根誤差（Root Mean Square Error，RMSE）R衡量觀測值與真值之間的偏差。R越小，代表位姿估計(jì)的測量精度值越高：

式中，Xobs,i、Xreal,i分別為目標(biāo)車輛第i幀時(shí)點(diǎn)云對(duì)應(yīng)的位姿估計(jì)值和真實(shí)值。

圖10、圖11所示分別為目標(biāo)車輛經(jīng)過交叉口時(shí)，直線運(yùn)動(dòng)和曲線運(yùn)動(dòng)狀態(tài)下的實(shí)際運(yùn)動(dòng)軌跡與試驗(yàn)車實(shí)時(shí)獲取的目標(biāo)車輛質(zhì)心坐標(biāo)點(diǎn)間的偏差關(guān)系，圖中所示車輛質(zhì)心點(diǎn)位置與實(shí)際運(yùn)動(dòng)軌跡具有一定的擬合度，因此可以通過計(jì)算質(zhì)心點(diǎn)處點(diǎn)法向量實(shí)時(shí)估計(jì)車輛位姿。

圖10 直線運(yùn)動(dòng)軌跡與質(zhì)心點(diǎn)位姿坐標(biāo)偏差關(guān)系

由圖7 可知，當(dāng)目標(biāo)車輛通過某城市交叉口時(shí)，運(yùn)動(dòng)軌跡主要由直線軌跡和曲線軌跡組成，表1所示為目標(biāo)車輛實(shí)時(shí)位姿估計(jì)RMSE分析結(jié)果。

由表1 可知：目標(biāo)車輛直線運(yùn)動(dòng)時(shí)，基于點(diǎn)云法向量的方法計(jì)算得到的運(yùn)動(dòng)目標(biāo)車輛橫滾角?、俯仰角θ和航偏角ψ的均方根誤差較低，且均小于傳統(tǒng)迭代最近點(diǎn)方法；曲線運(yùn)動(dòng)時(shí)，由于目標(biāo)車輛方向角變化較大，造成點(diǎn)云數(shù)據(jù)遮擋殘缺現(xiàn)象較為嚴(yán)重，基于點(diǎn)云法向量的方法計(jì)算得到的?、θ和ψ的均方根誤差值均高于直線運(yùn)動(dòng)的均方根誤差值，但仍小于傳統(tǒng)迭代最近點(diǎn)方法。因此，當(dāng)目標(biāo)車輛通過某城市交叉口時(shí)，在直線和曲線2種軌跡運(yùn)動(dòng)情況下，基于點(diǎn)云法向量算法對(duì)目標(biāo)車輛的位姿估計(jì)效果均優(yōu)于迭代最近點(diǎn)算法，且受點(diǎn)云數(shù)據(jù)遮擋殘缺現(xiàn)象的影響程度均低于傳統(tǒng)迭代最近點(diǎn)算法。

圖11 曲線運(yùn)動(dòng)軌跡與質(zhì)心點(diǎn)位姿坐標(biāo)偏差關(guān)系

表1 目標(biāo)車輛運(yùn)動(dòng)實(shí)時(shí)位姿估計(jì)RMSE分析結(jié)果 (°)

三維姿態(tài)估計(jì)的參數(shù)中，橫滾角?、俯仰角θ的均方根誤差較小，航偏角ψ的均方根誤差較大。目標(biāo)車輛在水平路面行駛，因此引起的橫滾角?、俯仰角θ的變化量較小，導(dǎo)致均方根誤差較??；目標(biāo)車輛在交叉口轉(zhuǎn)向，因此引起的航偏角ψ的變化量大，由式（7）可知，航偏角受多個(gè)因素影響，由于目標(biāo)車輛車體表面并非完全垂直，使得每個(gè)因素估計(jì)時(shí)都存在一定誤差，導(dǎo)致均方根誤差較大。

4 結(jié)束語

本文針對(duì)復(fù)雜城市交叉口場景下激光雷達(dá)獲取運(yùn)動(dòng)車輛點(diǎn)云殘缺而導(dǎo)致位姿信息難以提取的問題，提出一種基于點(diǎn)法向量特征的動(dòng)態(tài)剛性目標(biāo)實(shí)時(shí)位姿估計(jì)方法，采用自適應(yīng)聚類算法求解點(diǎn)法向量聚簇中心的數(shù)量，通過隨機(jī)采樣一致性算法對(duì)屬于同一聚類的點(diǎn)進(jìn)行平面擬合，通過運(yùn)動(dòng)學(xué)方程完成目標(biāo)的三維姿態(tài)角求解，采用均方根誤差來衡量不同方法獲得車輛位姿估計(jì)值與真值之間的誤差關(guān)系，結(jié)果表明，運(yùn)動(dòng)車輛進(jìn)行直線與曲線運(yùn)動(dòng)時(shí)采用點(diǎn)法向量位姿估計(jì)效果均優(yōu)于傳統(tǒng)基于迭代最近點(diǎn)的方法，具有較高的準(zhǔn)確性，能克服點(diǎn)云殘缺性。由于本方法僅對(duì)動(dòng)態(tài)剛性目標(biāo)進(jìn)行實(shí)時(shí)位姿估計(jì)，并未考慮運(yùn)動(dòng)狀態(tài)變化過程，后續(xù)應(yīng)對(duì)動(dòng)態(tài)目標(biāo)狀態(tài)估計(jì)及預(yù)測方法展開深入研究。