堆取料機防碰檢測系統多毫米波雷達標定方法研究

王立成，孔德明,*，沈 閱，曹 帥，張 鈺

(1.燕山大學 電氣工程學院，河北 秦皇島 066004；2.河北燕大燕軟信息系統有限公司，河北 秦皇島 066000)

0 引言

近年來,隨著毫米波技術的不斷發展,毫米波雷達的檢測精度不斷提高,由于其對雨、雪、霧、塵等惡劣環境具有很高的適應性,因此可以應用在大型散料堆場堆取料機的防碰檢測和成像中。在實際應用中,需要利用多個毫米波雷達多角度掃描彌補單個毫米波雷達的視野盲區,這不可避免涉及到多個毫米波雷達之間的聯合標定。

毫米波雷達應用在工業中的時間較短,雖然國內外對毫米波雷達聯合標定方法的研究還比較少,但是對于雙目攝像機和激光雷達等傳統傳感器的標定技術已經相當成熟,許多在不同場景下的標定方法對毫米波雷達的聯合標定具有借鑒意義。這些標定方法根據標志物的特征類型主要可以分為兩類：1)基于特征點的標定方法;2)基于特征直線的標定方法。

基于特征點的標定方法通過不同傳感器捕捉相同標志物的特征點進行匹配從而獲取彼此的相對位姿參數。李為民等人提出了一種基于一維標定點標定雙目相機的算法[1],通過拍攝靶子在空間中多個位姿的圖片結合靶子上兩個編碼點間的距離約束完成雙目相機的標定,具有可現場標定、多相機同時標定的優點。郭清達等人[2]的標定方法中以棋盤為標志物,對棋盤標靶提取特征點,根據張氏標定獲取雙目視覺相機的內外參數,再通過迭代最近點算法對特征點進行匹配得出補償矩陣，提高了雙目視覺標定的精度。雖然文獻[1-2]方法中的標志物簡易，特征明顯方便現場標定，提高了標定精度，但是基于特征點的標定需要傳感器對標志物特征點檢測有較高穩定性和準確性,根據毫米波雷達對障礙物的檢測特性,在大場景下面積較小的障礙物其雷達散射截面積較小難以檢測,容易淹沒在噪點中,無法區分出來;對于面積較大的標志物,由于毫米波雷達數據點集具有稀疏、散亂的特點,無法確定標志物在不同毫米波雷達中的反射點是否應該重合,因此利用特征點對毫米波雷達進行標定容易造成較大誤差。

相比特征點,直線特征包含更豐富的信息,因此除了基于特征點的標定方法外,很多學者針對基于特征直線的標定方法展開了研究。俞奇奇等人[3]采用了一個可以精確控制的伺服旋轉平臺,通過設計激光雷達的固定位置減少標定參數,并以墻面作為標定物實現激光雷達坐標系的標定。韋邦國等人[4]以直角墻面作為標志物,利用最小二乘法擬合出墻面的直線特征并根據重合準則計算出兩二維激光雷達間的相對橫擺角和橫縱方向偏移量,最后通過遺傳算法獲得全局優化的標定參數完成標定。由于直線特征比點特征更容易區分,且在直線擬合過程中降低了傳感器分辨率對標定的影響,因此基于直線特征的標定方法一定程度上提高了標定精度,非常適合用于處理稀疏、散亂以及分辨率相對較低的毫米波雷達數據。但傳統的最小二乘法擬合效果受噪點影響較大,當存在偏差較大的噪點時,擬合直線容易偏離最佳擬合位置,顯然無法應對工業現場數據,因此需要一個對噪點有較強抗性、能夠精確提取直線特征的直線檢測方法。

直線檢測在計算機視覺與圖像分析領域一直是熱門的研究問題[5],目前存在的直線檢測方法可以分為兩類[6]：1)感知分組；2)霍夫變換(Hough transform,HT)。其中感知分組更多運用在圖像研究領域,該方法將直線描述為存在足夠相似和共線分量的連通區域。BURN等[7]首先提出了基于梯度方向的直線檢測方法。在他們的方法中,直線被檢測為一個直線圖像區域,其中內部像素(組件)共享大致相同的方向(相似性)。然而,毫米波雷達數據分布稀疏、散亂,并不適合準確計算梯度方向,因此直線檢測效果欠佳。AKINLAR等[8]提出的直線檢測方法中,雖然能夠直接從邊緣像素檢測直線,檢測速度明顯提高,但是忽略了直線周圍點對其精確定位的作用,在噪聲較大的情況下精度較差。

相比感知分組,HT則是從不同的角度解析直線特征。HT將點集映射到參數平面進行投票,多個共線點的投票對象中包含同一個參數單元,這使該參數單元所獲票數呈現局部極大值的特征,通過搜索峰值得到直線參數從而反解出直線方程。標準的HT在檢測速度和精度上有缺點,但前人們經過多年的研究提出了許多優秀的改進算法。XU等人[9]提出隨機霍夫變換(Randomized Hough Transform，RHT)，從待檢測點集中隨機選取兩點進行映射投票,將HT中的映射方式由一對多轉化成了多對一,避免了大量無用計算從而提高計算效率,被廣泛運用在車道線檢測等領域[10-13];在精度方面,郭斯羽等人[14]提出HT與雙點移除p-最小二乘法(p-Least Squarest with Dual Removal,pLS-DR)結合,通過較粗分辨率下的HT檢測出直線的大致位置并獲取直線附近的點作為待擬合點集,然后利用pLS-DR對待擬合點集進行擬合和迭代,每次擬合后同時去除正、負兩個方向上一對誤差最大的數據點,直到點集的當前點數與初始點數之比低于比例閾值時輸出直線方程。該方法結合了HT在無啟示信息的前提下對直線粗檢測的魯棒性以及最小二乘法對點集更精確擬合的優點,同時pLS-DR有效減少了噪點對擬合的影響。

基于以上分析,根據現場條件及毫米波雷達成像特點,本文提出一種優化的基于壩基直線特征的毫米波雷達現場聯合標定方法。以堆取料機走行壩基為標志物,根據壩基的直線特征實現多個毫米波雷達的聯合標定;為了提高對壩基邊緣直線特征的檢測精度,本文提出使用RHT與改進的pLS-DR相結合的直線檢測方法對壩基的直線特征進行提取;最后通過遺傳算法進行全局優化。實驗結果表明改進的直線檢測方法在本文場景中相對于傳統算法檢測精度明顯提高,標定方法標定精度較高。

1 標定準備工作

1.1 堆取料機防碰檢測系統

本文的標定方法是針對大型散料堆場中堆取料機防碰檢測系統中多個毫米波雷達現場標定的方法。系統硬件包括堆取料機、毫米波雷達、壩基(含軌道),其中堆取料機的主要部件又包括斗輪、臂架、回轉中心和走行裝置。在堆取料機作業過程中斗輪負責取料,臂架通過回轉中心和走行裝置進行回轉、行走等作業動作。堆取料機防碰檢測系統的主要傳感器是毫米波雷達,6個毫米波雷達分布在堆取料機臂架多個方位,對臂架回轉平面進行多角度掃描,安裝位置為：左側安裝3個雷達,其中雷達1安裝在臂架前部負責掃描左側前部范圍,雷達2和雷達3安裝在臂架中部分別負責掃描臂架左側的中部范圍和后部范圍;右側3個雷達與左側相似,均是一個安裝在前部兩個安裝在中部,并分別掃描臂架右側前、中、后部范圍。以雷達1和雷達2為例描述標定方法,引入3個坐標系：臂架坐標系O0-X0Y0,雷達1坐標系O1-X1Y1,雷達2坐標系O2-X2Y2。系統硬件平臺如圖1所示,圖中扇形為雷達的掃描范圍。

1.2 標定數學模型

一般情況下標定參數主要包括橫擺角、水平角、俯仰角以及三維空間平移量。在堆取料機防碰系統中,毫米波雷達的俯仰角和水平角可用水平尺和重錘測量并調整至一致[15],因此可認為各個雷達擁有相同的俯仰角和水平角;由于毫米波雷達安裝位置的高程值相差較小(<1 m)，遠遠小于成像范圍([0,250 m)),且成像為二維點集即只反映X、Y分量而不反映高程值,因此可忽略安裝位置高程值的影響認為雷達均被安裝在同一個平面上。所以標定需要確定的位姿參數為橫縱方向平移量Δx、Δy和相對橫擺角Δβ,則標定數學模型如圖2所示,其中β1和β2分別是O1-X1Y1和O2-X2Y2相對O0-X0Y0的橫擺角;Δx、Δy分別是在O0-X0Y0中O2-X2Y2相對O1-X1Y1的橫縱方向平移量。

空間某點M在O0-X0Y0,O1-X1Y1和O2-X2Y2中的坐標分別為M0(a0,b0),M1(a1,b1)和M2(a2,b2)。M0、M1和M2的關系為

其中：R10、R20分別為M1到M0和M2到M0的旋轉矩陣;T10和T20分別是對應的平移矩陣。但R10、R20和T10、T20是通過堆取料機回轉編碼器以及通過工具測量得到的,由于編碼器以及測量誤差的影響,容易造成嚴重的成像重影。為解決此問題,直接從成像著手,通過標定得到雷達間相對位姿參數,進一步得到對應的相對旋轉矩陣R21和相對平移矩陣T21,則有

變換后可得

M0=R10R21M2+R10T21+T10,

(1)

通過式(1)即可將多個雷達根據雷達間的相對位姿參數轉換到臂架坐標系中,降低成像重影。

1.3 標定原理

分別在臂架旋轉過程中采集到t1和t2時刻壩基的掃描數據,根據運動的相對性可知,當視角與臂架保持相對靜止時,旋轉運動表現為壩基繞回轉中心旋轉,記壩基(t1)和壩基(t2)分別為t1、t2時刻壩基的位置,L(t1)和L(t2)為兩時刻壩基的邊緣直線,L(t1)、L(t2)、交于Q點。堆取料機和壩基的位置關系如圖3所示。

圖3 不同時刻下堆取料機與壩基的相對位置圖Fig.3 The relative position between the stacking and retrieving machine and the dam foundation at different time

進一步將t1、t2時刻雷達1檢測到的壩基邊緣直線分別記為L1(t1)、L1(t2),雷達2檢測到的壩基邊緣直線分別記為L2(t1)、L2(t2),它們在O0-X0Y0中的位置如圖4(a)所示。由于雷達1與雷達2之間存在相對橫擺角Δβ,因此在同一時刻下兩雷達檢測到的直線存在夾角,該夾角即為相對橫擺角Δβ。分別設

L1(t1):y=k11x+d11,

L2(t1):y=k21x+d21,

L1(t2):y=k12x+d12,

L2(t2):y=k22x+d22,

根據正切差角公式有

(2)

(3)

其中,R21為O2-X2Y2到O1-X1Y1的旋轉矩陣。

(4)

d′=Z(β,k)·d,

(5)

其中,

(6)

式(5)、(6)描述了直線方程截距在坐標系旋轉前后的關系。根據這一關系,將雷達2坐標系旋轉Δβ角度后,壩基邊緣直線L2(t1)、L2(t2)的方程變為

其中：

(7)

(8)

(9)

(10)

(Δx,Δy)=Q′(L1)-Q′(L2),

(11)

根據式(7)～(10)所述的關系,上式可表示為

(12)

(13)

最終根據式(2)、(3)和式(12)、(13)得到兩雷達間的相對位姿參數[Δβ，Δx，Δy],旋轉矩陣R21和平移矩陣T21。

1.4 標定數據采集

根據標定原理,數據采集實驗為：采集在多個掃描角度下壩基的數據,以雷達1和雷達2為例,為使壩基在兩雷達成像中的直線特征更加明顯,控制堆取料機保持走行位置不變,俯仰角為0°,回轉角從20°回轉到60°,連續采集兩個雷達在回轉過程中的數據。其中,毫米波雷達近距離的檢測分辨率為0.39 m(0～70 m),遠距離檢測分辨率為1.79 m(>70 m)。

2 壩基邊緣直線檢測

毫米波雷達數據是稀疏、散亂的二維點集,采集的數據中壩基的邊緣點并非嚴格共線,且受雷達遠距和近距的檢測視角、檢測分辨率不同的影響,雷達數據伴隨較多噪點,因此準確提取壩基邊緣直線特征是提高標定精度的關鍵。

2.1 RHT直線檢測

RHT是在HT的基礎上提出的改進算法,其檢測思想與HT相同,均是根據笛卡爾坐標系中的直線與參數平面中的參數點一一映射的關系,在參數平面對參數點進行累加統計,將圖像域的全局模式檢測問題轉化為參數空間[1 6]的高效峰值檢測問題。不同的是HT的映射直線來源于窮盡經過每一點的每一直線,屬于一對多,而RHT的映射直線來源于隨機選取兩點所確定的直線,屬于多對一,大大降低了計算量。直線在參數平面的映射如圖5所示。

圖5 映射關系圖Fig.5 Mapping diagram

其中,l:y=k·x+d為笛卡爾坐標系下的直線,C(θ1,r1)為直線映射到參數空間中的點,k、d分別為直線l的斜率和截距,θ1為直線l的法線的方向,r1為坐標原點到直線l的距離。映射過程中,將參數平面的橫縱坐標按照一定的間隔量(Δθ,Δr)化成二維網格,(Δθ,Δr)稱為分辨率,每個網格作為參數單元統計落入其中的參數點,這一過程也稱為投票,最終通過搜索票數峰值所對應的參數單元完成直線檢測。

RHT步驟可以描述為：1)不斷從待檢測點集中隨機選取兩點,將兩點所確定的直線映射到參數平面,映射所對應的參數點(θ,r)的計算如式(14)～(15)所示,其中,隨機選取兩點坐標(x1,y1)和(x2,y2)確定的直線方程為y=k·x+d,(xo,yo)為原點到直線的垂足(以下簡稱垂點)。2)當存在參數單元所獲票數達到一定閾值時認為該參數單元存在直線,檢測的直線計數加一,同時將該直線上的點刪除,減少后續計算量,直到檢測的直線數量達到設定值或者點集清零時停止檢測。

r=x1sinθk-y1cosθk,

(14)

(15)

2.2 RHT結合改進的pLS-DR

RHT提高了檢測速度,但并沒有提高準確度,其檢測精度仍然受限于分辨率(Δθ,Δr),盡管提高分辨率可以提高直線檢測的精度,然而當Δθ和Δr過分細化時不嚴格共線的“共線點”的投票可能分散到其他參數單元,從而無法積累足夠的票數造成直線檢測失敗。

為提高直線檢測精度,將RHT與pLS-DR結合,通過RHT提高直線檢測速度,結合pLS-DR提高檢測精度。使用較粗分辨率下的RHT檢測出直線的大致位置并獲取距離該直線ε范圍以內的點作為待擬合點集,然后利用pLS-DR對待擬合點集進行擬合迭代,每次擬合后同時去除正、負兩個方向上一對誤差最大的數據點,直到點集點數與初始點數比值小于比例閾值時結束。

但是傳統的pLS-DR對本文場景中壩基邊緣直線的擬合存在一些缺陷：獲取直線附近的點集進行擬合時,擬合效果對ε的取值比較敏感,ε較大時雖然減少了遺漏共線點的可能,但是大量增加了噪點的引入,在比例閾值p不變的情況下,最終保留的點集包含的噪點數量會增加從而導致擬合直線位置出現偏差;若ε較小,則可能遺漏共線點,同樣導致擬合結果無法回歸到準確的直線位置。

根據實驗分析,造成以上問題的原因如下：

1)RHT檢測的直線由于分辨率的限制往往不能反映準確的直線位置并且與最佳擬合位置之間具有一定的夾角,在這一夾角的影響下距離直線垂點較遠的共線點偏離檢測直線的距離更大,需要更大的ε值才能將其引入待擬合點集。

2)毫米波雷達在近距離掃描范圍內的反射點更加密集,噪點也更多,而遠處的反射點則相對稀疏,共線點也更少。因此當ε較大時,更多地引入了近處的噪點,遠近點數的不均衡導致在擬合時擬合直線可能偏向近處噪點、遠離遠處的共線點,導致遠處的共線點由于相對擬合直線的誤差更大而被去除,增加了噪點的比例使擬合效果更不理想。

為解決以上缺陷,對pLS-DR提出以下兩點改進：

1)設置夾角閾值ΔθLS。使用較小的ε值減少引入近距離范圍噪點,同時設置夾角閾值ΔθLS進行篩選：當某一點與檢測直線垂點的連線相對于檢測直線的夾角小于夾角閾值ΔθLS時,將其引入待擬合點集。由于直線的偏移主要來自于RHT的分辨率,所以可取ΔθLS≈Δθ。

2)使用兩次pLS-DR。根據第一次pLS-DR回歸的直線方程再進行一次擬合迭代,確保直線回歸到更準確的位置。

根據以上兩點,RHT結合改進的pLS-DR直線檢測主要步驟如下：

1)使用RHT對待檢測點集進行直線檢測,得到檢測直線的方程y=kj·x+dj,j=0,1,…,j表示第j次迭代,將RHT檢測得到的直線視為第0次迭代所得直線。

3)給定保留數據點數比例閾值p,計算最終保留的點數Nf=pN0,N0為初始點數。

4)使用最小二乘法根據點集Pj擬合得到新的回歸直線y=kj+1·x+dj+1,最小二乘法回歸公式為

5)求取Pj中每個數據點與回歸直線之間的帶符號誤差ei，將最大正、負誤差的一對數據點移除，剩下的點集記為Pj+1={(xi,yi)|1≤i≤Nj+1}，Nj+1=Nj-2。誤差ei表達式為

6)如果Nj+1≤Nf，則返回直線方程作為回歸結果；否則至步驟2)。

7)將6)返回的直線方程作為初始直線位置，再使用一次pLS-DR進行迭代擬合，即以6)返回的直線方程作為1)中的直線y=kj·x+dj再重復步驟2)到6)，將這次回歸的直線方程作為最終的直線方程輸出。

改進后的方法在收集待擬合點時，既能減少遺漏遠距離共線點的可能，也減少了近距離噪點的引入，使直線檢測更加準確。

3 標定參數的計算及優化

3.1 標定參數計算

以雷達1和雷達2的標定為例,將采集的數據按一定時間間隔選出32幀數據,使用上一章的直線檢測方法對這些數據進行檢測,得到32組數據,每組數據包含了同一時刻下兩組分別在雷達1和雷達2坐標系中的壩基邊緣直線方程。根據這32組數據按式(3)計算得到的雷達相對橫擺角Δβ,將32組數據按照不同的選擇兩兩搭配形成177對組合,每對組合可按照式(10)計算出1組(Δx，Δy),結果如圖6所示。計算結果顯示,不同方程組解算出來的相對橫擺角之間有一定的差異,不同的組合計算的(Δx，Δy)之間也存在一定的差異。

圖6 多組數據計算結果圖Fig.6 Calculated results of multiple groups of data

產生差異的主要原因是檢測的壩基邊緣直線方程受毫米波雷達檢測分辨率相對較低、反射點散亂性以及噪點的影響從而與真實位置存在一定偏差,進而使計算得到的相對位姿參數產生誤差。為了盡可能降低成像重影,采用遺傳算法對標定參數進行優化,使標定結果趨向于全局最優。

3.2 基于遺傳算法的參數優化

遺傳算法是采用生物選擇和遺傳學機制的全局自適應概率的搜索算法[17]。無需指定搜索規則,能夠提供通用的優化框架達到全局最優,避免落入局部最優的陷阱。

使用遺傳算法首先需要確定目標函數。同一時刻下,根據雷達1檢測到的壩基邊緣直線轉換到臂架坐標系后,所得直線方程為l1:y=k1·x+d1,根據雷達2檢測到的壩基邊緣直線轉換到臂架坐標系后,所得直線方程為l2:y=k2·x+d2,如圖7所示。

臂架坐標系原點到兩直線的垂足分別為A(xoA,yoA)、B(xoB,yoB)兩點,其坐標的表達式為

理論上來說,A和B應該完全重合,但是由于誤差存在,A、B沒有重合。A和B的長度DAB可作為衡量兩個坐標系位姿關系準確性的指標。DAB的表達式為

以3.1節得到的32組數據作為樣本,得到最終優化的目標方程:

圖7 DAB示意圖Fig.7 DAB diagram

根據確定好的目標函數,定義U=[Δβ,Δx,Δy],則優化問題轉化為求解F(U)的最小值對應的U值。遺傳優化算法步驟如下：

1)初始化。設定初始種群Uini、上邊界Uub,下邊界Ulb。上邊界取多個位置中的最大值,Uub=[-0.012 1,0.99,11.69],下邊界取最小值,Ulb=[-0.014 3,0.001,9.7],在上邊界與下邊界之間以一定間隔設置多個初始種群。其他參數設定如下：交叉概率為0.8,種群大小設置為50,最大遺傳代數為200。

2)計算F(U)的值并更新F(U)。

3)重復步驟2)直到滿足循環條件,循環條件設置為λ>φ,λ為迭代次數。

4 實驗結果及分析

4.1 直線檢測算法實驗分析

直線檢測實驗分析以雷達1和雷達2對壩基的掃描數據為例。同一幀數據的不同處理結果如圖8所示。其中,圖8(a)、(b)使用pLS-DR進行擬合,距離閾值ε分別設定為2 m和4 m,比例閾值均為p=0.6。在使用pLS-DR進行擬合時,在ε=2下收集的是RHT檢測直線附近2 m內的點作為待擬合點集進行擬合迭代,但是在相同的偏移角度下,遠距離點比近距離點相對RHT檢測直線的距離更大(可能大于2 m),因此收集的遠距離共線點數較少,如圖8(a)所示,最終的擬合位置雖然比RHT檢測直線的位置更貼近共線點但是仍然存在較大誤差;若設置ε=4擴大搜索范圍,可收集更多遠距離點共線點,但是由于近距離點更密集,收集的近距離點數遠大于收集的遠距離點數,其中也包含較多噪點,因此在擬合迭代過程中受噪點的影響,擬合直線可能更偏向于噪點,遠離遠距離共線點,導致遠距離共線點不斷被作為最大誤差值點去除,最終的擬合位置也不理想,如圖8(b)所示。

改進的pLS-DR由于引進了夾角閾值ΔθLS,在相同的比例閾值p下,遠距離點即使距離直線更遠也能通過夾角閾值ΔθLS判定為待擬合點,因此在ε=2下仍然可以收集到更多遠距離點。更多的遠距離共線點使最終的擬合效果更接近理想位置。此外,進行兩次擬合迭代保證了RHT檢測直線偏移較大時也能回歸到更準確的位置。改進的pLS-DR最終保留的擬合點中共線點明顯更多,擬合效果也更貼近共線點,如圖8(c)所示。

4.2 標定參數優化結果分析

為防止遺傳算法陷入局部最優,在上邊界Uub,下邊界Ulb之間以相同的間隔量u=[0.002 7,1.065 6,1.863 6]設置了4個初始種群Uini,不同的Uini代入遺傳算法后計算的結果如表1所示。

表1 遺傳算法計算結果Tab.1 Results of genetic algorithm

不同的初始種群Uini的計算結果大致相同,選擇目標函數最小值對應的參數作為標定結果,即U=[-0.013,0.458,10.931]。比較標定算法計算結果與實際測量值,結果如表2所示。表中測量值為多次測量的平均值,J12表示雷達1和雷達2的歐氏距離。根據數據分析結果,Δx的誤差為0.228 m,Δy的誤差為0.16 m,兩雷達之間的歐式距離誤差為0.152 8 m,小于毫米波雷達的檢測分辨率(0.39 m),滿足工業需求。

結合相對位姿參數與式(1)、(3)、(13)可將雷達1、雷達2的數據轉換到臂架坐標系中。標定后不同回轉角度下的成像效果如圖9所示。在不同角度下兩雷達檢測到的壩基邊緣重合度較高,其中一些局部細節也能相契合。

最后,將標定方法推廣到其他4個雷達上,即可實現多個雷達的聯合標定。

5 結論

本文提出了一種針對堆場堆取料機防碰檢測系統中多個毫米波雷達的現場聯合標定方法：利用直線檢測算法提取多個角度下壩基的直線特征,根據多組直線特征形成的幾何約束計算出雷達相對位姿參數,并通過遺傳算法對相對位姿參數進行全局優化。該標定方法解決了毫米波雷達在散料堆場現場標定中一般標志物難以識別的問題,標定精度較高。其中,提出了一種改進的直線檢測算法,該算法在收集擬合點時增加了角度閾值進行判斷,從而能夠收集到更多遠處的共線點,提高了直線檢測精度,更適合處理稀疏、散亂的毫米波雷達數據。實驗結果表明,根據標定方法計算得到的參數與實際測量值的誤差控制在0.23 m以內,小于毫米波雷達檢測分辨率(0.39 m),精度較高,標定后成像重合度較高,滿足工業需求。