應(yīng)用激光雷達(dá)與相機(jī)信息融合的障礙物識(shí)別

黃 興，應(yīng)群偉

(上海宇航系統(tǒng)工程研究所，上海 201109)

0 引言

近年來(lái)，NASA宣布重啟探月任務(wù)(EM)，計(jì)劃首先開始實(shí)施無(wú)人繞月飛行(EM-1階段)，技術(shù)成熟后實(shí)施載人探月(EM-2階段)，同時(shí)其火星、木星、太陽(yáng)等地外天體的探測(cè)活動(dòng)持續(xù)開展。俄羅斯在2007提出了自己的“探月三階段”發(fā)展戰(zhàn)略，預(yù)計(jì)2025年前實(shí)現(xiàn)載人登月，2032年前在月球建立常駐考察基地，同時(shí)計(jì)劃與歐空局合作開展火星著陸巡視探測(cè)及采樣返回[1]。日本則在小行星探測(cè)領(lǐng)域一枝獨(dú)秀，“隼鳥一號(hào)”、“隼鳥二號(hào)”相繼成功在小行星表面采樣返回，令人矚目[2]。歐空局、印度等機(jī)構(gòu)或國(guó)家也都規(guī)劃了其地外天體探測(cè)計(jì)劃[3]，深空探測(cè)將逐漸成為未來(lái)航天領(lǐng)域發(fā)展的一個(gè)重要方向。

以中國(guó)探月工程與美國(guó)火星探測(cè)為代表，地外天體探測(cè)將向著陸巡視、采樣返回及載人探測(cè)返回的方向發(fā)展，軟著陸過(guò)程中對(duì)星體表面的障礙物精確識(shí)別尤為關(guān)鍵[4-5]，現(xiàn)有工程應(yīng)用上，尚未采用信息融合進(jìn)行障礙物識(shí)別。

為此，本文提出了一種融合三維點(diǎn)云與灰度圖像進(jìn)行精確障礙物識(shí)別的方法。

1 方法概述

地外天體探測(cè)任務(wù)中，探測(cè)器軟著陸的過(guò)程一般分為主減速段、接近段和垂直著陸段[6]，如圖1所示。

圖1 星體表面軟著陸過(guò)程

本文所述方法應(yīng)用在著陸器垂直著陸段，實(shí)現(xiàn)探測(cè)器在垂直著陸過(guò)程中提供精確障礙物識(shí)別功能，同時(shí)為后續(xù)的著陸點(diǎn)選取及相對(duì)導(dǎo)航提供基礎(chǔ)。

垂直著陸段一般距離星體表面在百米量級(jí)，經(jīng)過(guò)姿態(tài)控制，探測(cè)器此時(shí)已經(jīng)垂直于星體表面。距離與姿態(tài)都滿足導(dǎo)航相機(jī)與激光雷達(dá)的工作要求，導(dǎo)航相機(jī)開機(jī)工作，獲取探測(cè)器下方星體表面的光學(xué)圖像信息；激光雷達(dá)開機(jī)工作，獲取探測(cè)器下方，雷達(dá)視場(chǎng)內(nèi)環(huán)境的三維激光點(diǎn)云數(shù)據(jù)。

圖2 方法流程圖

如圖2所示，探測(cè)器視覺(jué)導(dǎo)航計(jì)算機(jī)首先進(jìn)行兩類傳感器數(shù)據(jù)的融合，該環(huán)節(jié)實(shí)質(zhì)是獲取相機(jī)三維坐標(biāo)系與激光雷達(dá)三維坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，從而將激光雷達(dá)三維坐標(biāo)系下的空間點(diǎn)坐標(biāo)轉(zhuǎn)換到相機(jī)三維坐標(biāo)系下，最終獲得視場(chǎng)環(huán)境內(nèi)各點(diǎn)的灰度信息與空間位置信息。其次，分別對(duì)灰度圖像與三維點(diǎn)云數(shù)據(jù)進(jìn)行障礙物識(shí)別獲取光學(xué)障礙圖與點(diǎn)云障礙圖。最后通過(guò)雷達(dá)到相機(jī)坐標(biāo)系下的轉(zhuǎn)換矩陣，將點(diǎn)云障礙圖與光學(xué)障礙圖進(jìn)行歸一化，形成最終的障礙圖，實(shí)現(xiàn)精確障礙物識(shí)別。

2 三維點(diǎn)云數(shù)據(jù)與圖像數(shù)據(jù)的融合

本方案采用坐標(biāo)歸一化的方法來(lái)完三維點(diǎn)云數(shù)據(jù)與灰度圖像數(shù)據(jù)的融合，通過(guò)求解激光雷達(dá)坐標(biāo)系與相機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系進(jìn)行數(shù)據(jù)的歸一化處理。

如圖3所示，導(dǎo)航相機(jī)與激光雷達(dá)的安裝位置是固定的，分別定義兩者的坐標(biāo)系，則相機(jī)坐標(biāo)系與激光雷達(dá)坐標(biāo)系存在固定的轉(zhuǎn)換關(guān)系。

圖3 相機(jī)與激光雷達(dá)安裝示意圖

2.1 坐標(biāo)系定義及相機(jī)內(nèi)參

相機(jī)坐標(biāo)系Oc-XYZ：相機(jī)光心為坐標(biāo)原點(diǎn)，相機(jī)光軸指向月面為Z軸，OXY平面同相機(jī)像平面，形成右手坐標(biāo)系。

激光雷達(dá)坐標(biāo)系Ol-XYZ：以掃描中心為坐標(biāo)原點(diǎn)，Z軸指向月面，X、Y軸與相機(jī)坐標(biāo)系保持一致，形成右手系。

相機(jī)內(nèi)參主要描述相機(jī)三維坐標(biāo)系下空間點(diǎn)到像平面的投影矩陣，一般使用小孔成像的模型對(duì)相機(jī)的投影關(guān)系進(jìn)行模擬，圖4為相機(jī)模型[7]。

圖4 相機(jī)模型

相機(jī)內(nèi)參矩陣的形式一般采用齊次坐標(biāo)形式，如式1所示。

(1)

其中：f為相機(jī)的焦距；dx*dy為相機(jī)感光器件每個(gè)像素對(duì)應(yīng)的物理尺寸；u0、v0為對(duì)應(yīng)像平面像素原點(diǎn)與像平面中心的偏移量，單位為像素。

相機(jī)內(nèi)參矩陣為相機(jī)的固有屬性，一般出廠后能夠通過(guò)相關(guān)參數(shù)的計(jì)算獲取。工程應(yīng)用上會(huì)再通過(guò)棋盤圖像進(jìn)行標(biāo)定，修正內(nèi)參矩陣。

2.2 激光雷達(dá)坐標(biāo)系轉(zhuǎn)換矩陣

本方案選用線陣掃描式激光雷達(dá)，它一般有16線、32線和64線等規(guī)格，通過(guò)安裝在平行于激光雷達(dá)Z軸的轉(zhuǎn)動(dòng)軸上的16/32/64個(gè)激光收發(fā)單元進(jìn)行距離計(jì)算(基于激光Time of Flight原理)，收發(fā)單元在轉(zhuǎn)動(dòng)軸縱向按固定角度分布，稱為激光雷達(dá)的垂直角分辨率，轉(zhuǎn)軸的轉(zhuǎn)動(dòng)角范圍為激光雷達(dá)的水平視場(chǎng)角[8]。

激光雷達(dá)轉(zhuǎn)動(dòng)軸水平方向以一定的頻率持續(xù)旋轉(zhuǎn)，掃描計(jì)算得到的數(shù)據(jù)以數(shù)據(jù)流的形式通過(guò)網(wǎng)絡(luò)傳輸至視覺(jué)計(jì)算機(jī)。在現(xiàn)有的技術(shù)水平下，工業(yè)級(jí)的激光雷達(dá)可達(dá)到120萬(wàn)點(diǎn)/秒的測(cè)量速度，相當(dāng)于百萬(wàn)級(jí)像素的相機(jī)，轉(zhuǎn)動(dòng)頻率在5～30 Hz的范圍內(nèi)，能夠滿足距離維度測(cè)量的實(shí)時(shí)性[9]。

圖5 激光雷達(dá)掃描示意圖

采集數(shù)據(jù)的處理上，首先根據(jù)激光點(diǎn)對(duì)應(yīng)的激光線ID可以查詢激光線傾角ω，根據(jù)測(cè)距值r計(jì)算激光點(diǎn)在掃描面平面坐標(biāo)系下的坐標(biāo)X0。

(2)

當(dāng)激光雷達(dá)進(jìn)行掃描時(shí)，輸出的掃描角α為掃描面與激光雷達(dá)坐標(biāo)系XOZ平面的夾角。將掃描面平面坐標(biāo)轉(zhuǎn)換至激光雷達(dá)三維坐標(biāo)，旋轉(zhuǎn)矩陣Rx為:

(3)

最后可得到空間測(cè)量點(diǎn)在激光雷達(dá)坐標(biāo)系下的坐標(biāo)：

(4)

通過(guò)導(dǎo)航相機(jī)坐標(biāo)系與激光雷達(dá)坐標(biāo)系的建立，可以得到相機(jī)和激光雷達(dá)兩種傳感器在各自視場(chǎng)內(nèi)空間點(diǎn)的坐標(biāo)。而相機(jī)與激光雷達(dá)安裝在同一水平支架的兩處，相機(jī)坐標(biāo)系與激光雷達(dá)坐標(biāo)系存在固定的轉(zhuǎn)換關(guān)系：

(5)

3 基于灰度圖像的障礙物識(shí)別

綜合技術(shù)特點(diǎn)和工程應(yīng)用兩方面考慮，采用基于改進(jìn)K均值聚類的光學(xué)障礙物識(shí)別算法。該算法首先通過(guò)對(duì)灰度圖片進(jìn)行中值濾波預(yù)處理，濾除成像過(guò)程中產(chǎn)生的噪聲；然后采用K均值聚類算法獲取區(qū)分背景與非背景的最佳分離像素值，并進(jìn)行二值化；最后采用邊緣檢測(cè)與輪廓提取方法，分離出障礙物，生成光學(xué)障礙物圖。

3.1 圖像預(yù)處理

通過(guò)分析發(fā)現(xiàn)星體表面灰度圖像中的噪聲主要包含加性噪聲和隨機(jī)干擾噪聲，為此首先采用中值濾波器對(duì)圖像預(yù)處理，濾除圖像的噪聲。同時(shí)為更好地適應(yīng)光照對(duì)圖像采集的影響，采用直方圖均衡化的方式，對(duì)灰度圖像進(jìn)行增強(qiáng)[10]。

3.2 基于改進(jìn)K均值聚類的灰度圖像分割

傳統(tǒng)K均值聚類是基于誤差平方和最小準(zhǔn)則對(duì)樣本數(shù)據(jù)進(jìn)行聚類分析，算法對(duì)樣本的區(qū)分依據(jù)是同類樣本數(shù)據(jù)的相似性原則。本方案中，它根據(jù)灰度信息相似性度量原則對(duì)圖像進(jìn)行分割，獲取能最優(yōu)區(qū)分背景與非背景的分割像素值，依據(jù)背景像素值占據(jù)圖像絕大部分，而非背景物體則占據(jù)少數(shù)，同時(shí)星表灰度圖像中，背景暗，像素值偏低，非背景物體亮，像素值偏高的規(guī)律，對(duì)非背景最有可能出現(xiàn)的圖像灰度中值至最大值的像素范圍進(jìn)行均值分類，選取相似性距離測(cè)度最大的像素值作為最優(yōu)區(qū)分值，實(shí)現(xiàn)圖像分割。算法的收斂性好，實(shí)際效果理想[11]。

定義誤差度量函數(shù)Jk為：

(6)

mj=E(Cj)

(7)

定義相似性距離測(cè)度Lj為:

(8)

(9)

其中:Cj(j=1,2,3…k)表示k個(gè)聚類，mj為k個(gè)聚類的中心，這里采用聚類像素值的平均值作為聚類中心。

算法輸入為預(yù)處理后圖像，輸出為分割后的二值化圖。算法步驟如下。

步驟1：首先初始化k個(gè)聚類中心，本方法采用圖像平均像素值到最大像素值進(jìn)行k階等分形成k個(gè)聚類中心；

步驟2：利用直方圖對(duì)平均像素到最大像素值之間的所有像素進(jìn)行誤差度量計(jì)算，根據(jù)誤差度量函數(shù)Jk最小的原則，將每個(gè)像素劃分至相應(yīng)的聚類中；

步驟3：根據(jù)聚類重新劃分后的結(jié)果，重新計(jì)算每個(gè)聚類的像素均值作為聚類中心，更新k個(gè)聚類中心；

步驟4：循環(huán)運(yùn)行步驟3，直至更新后的k個(gè)聚類中心與更新前的k個(gè)聚類中心無(wú)變化，退出循環(huán)；

步驟5：根據(jù)式(8)計(jì)算步驟4得到的k個(gè)聚類中心的距離測(cè)度Lj，選取距離測(cè)度最大的Lx，然后根據(jù)式(9)計(jì)算最優(yōu)分割像素值T；

步驟6：以最優(yōu)分割像素值T為閾值對(duì)灰度圖進(jìn)行二值化，得到分割二值化圖，算法結(jié)束。

3.3 邊緣檢測(cè)輪廓提取

利用3.2節(jié)得到的二值化圖像可以使背景與非背景物體進(jìn)行最優(yōu)分割。然后對(duì)二值化圖像進(jìn)行Canny算子邊緣檢測(cè)，并將二值化圖與Canny邊緣檢測(cè)圖相加，最后對(duì)相加的圖像進(jìn)行輪廓提取，對(duì)每個(gè)輪廓進(jìn)行分析，根據(jù)輪廓大小及面積決定是否判定為障礙物。下圖為本算法在月貌仿真實(shí)驗(yàn)室進(jìn)行檢測(cè)的實(shí)際效果圖，算法效果較為理想。

圖6 基于K均值聚類的灰度圖像分割效果

4 基于激光三維點(diǎn)云的精確障礙物識(shí)別

激光測(cè)距能夠有效避免光照影響，實(shí)時(shí)性也能滿足垂直著陸段精確避障的要求，在星體軟著陸任務(wù)中有良好的應(yīng)用前景。

激光雷達(dá)測(cè)量數(shù)據(jù)傳輸至視覺(jué)計(jì)算機(jī)后，經(jīng)過(guò)坐標(biāo)轉(zhuǎn)換，形成激光雷達(dá)坐標(biāo)系下的三維點(diǎn)云。本方案中借助開源點(diǎn)云數(shù)據(jù)處理庫(kù)PCL(point cloud library, PCL) 對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行處理[12]，算法流程如圖7。

圖7 點(diǎn)云數(shù)據(jù)處理流程

4.1 點(diǎn)云數(shù)據(jù)預(yù)處理

對(duì)激光雷達(dá)數(shù)據(jù)進(jìn)行Kalman濾波的主要作用是修正雷達(dá)的系統(tǒng)誤差以及探測(cè)器振動(dòng)帶來(lái)的測(cè)量誤差[13]。

激光雷達(dá)由于其垂直角分辨率的原因，對(duì)應(yīng)的空間分辨率會(huì)隨高度的升高的降低，在200 m的距離下，點(diǎn)云的空間分辨率在幾十個(gè)公分的量級(jí)，為提高其空間分辨率，依據(jù)預(yù)選著陸區(qū)地理環(huán)境一般都是平坦連續(xù)的考慮，采用插值的方法提高點(diǎn)云密度。

4.2 地形水平面提取

地形水平面提取是指從三維點(diǎn)云中提取星體表面，并從點(diǎn)云中移除，這樣剩下的點(diǎn)云數(shù)據(jù)即為高于星表的突起物或者低于星表的凹坑，以此來(lái)實(shí)現(xiàn)對(duì)星體表面突起物、凹坑及斜坡等障礙物的提取。

采用隨機(jī)采樣一致性算法(Random Sample Consensus，RANSAC)提取環(huán)境三維點(diǎn)云的地形水平面[14]。隨機(jī)采樣一致性算法是針對(duì)特定模型的最優(yōu)估計(jì)算法，如平面、圓柱等模型，算法假設(shè)觀測(cè)數(shù)據(jù)集是一組包含模型局外點(diǎn)和局內(nèi)點(diǎn)的集合，它通過(guò)重復(fù)迭代達(dá)到獲取包含最多局內(nèi)點(diǎn)的模型參數(shù)之目的，算法與最小二乘法的區(qū)別在于隨機(jī)采樣一致性算法區(qū)分局內(nèi)點(diǎn)與局外點(diǎn)，并只是期望找到能夠包含盡可能多局內(nèi)點(diǎn)的具體模型；而最小二乘法則是試圖找到一種模型能夠?qū)λ杏^測(cè)數(shù)據(jù)點(diǎn)進(jìn)行最優(yōu)匹配[15]。針對(duì)環(huán)境三維點(diǎn)云來(lái)說(shuō)，地形水平面與突起物、凹坑分別屬于不同的類型，使用最小二乘法來(lái)找尋一個(gè)平面包含所有物體是不合理的，而隨機(jī)采樣一致性算法則最有可能從包含少量突起物和凹坑的環(huán)境三維點(diǎn)云中獲取地形平面，并把突起物和凹坑與水平面區(qū)分開來(lái)。

算法的關(guān)鍵參數(shù)主要包括算法迭代次數(shù)kc、距離閾值Tn。其中kc用于約束算法的最大迭代次數(shù)，RANSAC算法從理論上講只有一定的概念保證找到正確的模型參數(shù)，迭代次數(shù)越高得到正確估計(jì)的概率就越大。算法采用歐式距離作為局內(nèi)點(diǎn)與局外點(diǎn)的度量，對(duì)每個(gè)點(diǎn)計(jì)算其與每輪迭代得到的平面的歐式距離，小于距離閾值Tn屬于局內(nèi)點(diǎn)，否則屬于局外點(diǎn)。

算法的具體計(jì)算流程如下。

步驟1：隨機(jī)選取點(diǎn)云中3個(gè)不共線樣本點(diǎn)(xa,xb,xc)，可以唯一地確定平面：

n·x+d=0

(10)

其中：平面法向量為n=(xb-xa)×(xc-xa)，平面截距為d=-n×xa。

步驟2：利用步驟1 中得到的平面，計(jì)算點(diǎn)云中每個(gè)樣本點(diǎn)xi到該平面的距離：

ri=(n×xi+d)2

(11)

步驟3：對(duì)步驟2計(jì)算得到的ri，如果ri≤Tn，則該點(diǎn)屬于局內(nèi)點(diǎn)，否則屬于局外點(diǎn)，記錄本輪迭代局內(nèi)點(diǎn)的個(gè)數(shù)Nj及模型參數(shù)(n、d)；

Nj=NUM(xi),ri≤Tn

(12)

步驟4：算法退出條件：

j=kc,迭代次數(shù)達(dá)到設(shè)定值；or

Nj=Num(xi),Num(xi)表示樣本點(diǎn)個(gè)數(shù)

(13)

步驟5：重復(fù)步驟1、2、3，直到滿足步驟4的算法退出條件，取Nj最大值所對(duì)應(yīng)的平面參數(shù)(nbest,dbest)作為當(dāng)?shù)氐匦蔚淖罴褦M合水平面。

借助PCL點(diǎn)云庫(kù)可實(shí)現(xiàn)上述算法，下圖為實(shí)際效果圖(kc=1000、Tn=0.15m)。

圖8 三維點(diǎn)云平面提取示意圖

4.3 點(diǎn)云分割

提取點(diǎn)云中的地形水平面后，剩余點(diǎn)云中就只包含突起物、凹坑等障礙物。采用歐式聚類作為度量，基于距離最小原理對(duì)點(diǎn)云進(jìn)行聚類，分割出一個(gè)個(gè)獨(dú)立的子點(diǎn)云。分割后的子點(diǎn)云根據(jù)其在地形水平面法向量方向上的投影判斷該聚類是高于地形平面還是低于地形水平面，相應(yīng)地區(qū)分子點(diǎn)云為突起物或者為凹坑。

最后對(duì)分割后的子點(diǎn)云進(jìn)行輪廓提取，提取各子點(diǎn)云的輪廓，對(duì)輪廓進(jìn)行尺寸分析，結(jié)合障礙物的定義，確定該輪廓是否確定為障礙物，從而去除一些較小的、不影響探測(cè)器著陸的物體。同時(shí)，通過(guò)激光雷達(dá)坐標(biāo)系與相機(jī)坐標(biāo)的轉(zhuǎn)換關(guān)系可以將點(diǎn)云障礙物轉(zhuǎn)換至相機(jī)坐標(biāo)系中，并投影至像平面上，從而實(shí)現(xiàn)障礙物的精確識(shí)別，生成最終障礙圖。

基于三維點(diǎn)云的障礙物識(shí)別可以有效避免圖像紋理信息對(duì)障礙物識(shí)別的影響，且光照影響對(duì)激光雷達(dá)數(shù)據(jù)的生成影響較小。在垂直著陸段的精確障礙物識(shí)別應(yīng)用環(huán)境下，效果良好。

5 實(shí)驗(yàn)結(jié)果與分析

通過(guò)以下實(shí)驗(yàn)對(duì)本文算法進(jìn)行了驗(yàn)證。

5.1 實(shí)驗(yàn)設(shè)備

固定相機(jī)與激光雷達(dá)，對(duì)室內(nèi)模擬場(chǎng)景進(jìn)行成像。

實(shí)驗(yàn)結(jié)果如圖10～12。

圖10 實(shí)驗(yàn)現(xiàn)場(chǎng)圖

圖11 點(diǎn)云及分割圖

圖12 激光雷達(dá)點(diǎn)云處理后生成的障礙圖

5.2 結(jié)果分析

實(shí)驗(yàn)中，使用矩形紙盒、圓形鐵盒及一個(gè)易拉罐模擬障礙物。同時(shí)，實(shí)驗(yàn)場(chǎng)景的光學(xué)圖像中包含地磚縫隙、日光燈倒影等難以使用光學(xué)障礙物算法剔除的干擾物，但點(diǎn)云數(shù)據(jù)可以有效避免這些干擾的影響。通過(guò)點(diǎn)云分割、輪廓提取及對(duì)像平面的投影，成功生成基于激光雷達(dá)點(diǎn)云處理后生成的障礙圖，實(shí)驗(yàn)效果比較理想。

不足之處主要有幾點(diǎn)：

1) 16線激光雷達(dá)的角分辨率(1～2°)較低，當(dāng)工作距離比較高時(shí)，存在漏檢的可能；

2) 物體尺寸較小時(shí)，點(diǎn)云數(shù)據(jù)稀疏，歸一化、投影過(guò)程中可能漏檢；

3) 算法的運(yùn)算量大，實(shí)時(shí)性不足。

6 結(jié)束語(yǔ)

本文提出了一種融合激光三維點(diǎn)云數(shù)據(jù)與光學(xué)圖像的精確障礙物識(shí)別方法。與嫦娥三號(hào)探測(cè)器垂直著陸段采用導(dǎo)航避障相機(jī)+激光三維成像敏感器接力避障的方式相比具有以下優(yōu)勢(shì)[16-18]：通過(guò)融合光學(xué)灰度信息與三維點(diǎn)云信息，可以同時(shí)獲取視場(chǎng)環(huán)境內(nèi)各點(diǎn)的灰度與空間坐標(biāo)位置信息，為后續(xù)的最佳落點(diǎn)選取和相對(duì)導(dǎo)航解算提供數(shù)據(jù)基礎(chǔ)，拓展視覺(jué)系統(tǒng)的導(dǎo)航功能，而在嫦娥三號(hào)任務(wù)中，相機(jī)和激光三維成像儀僅用作避障，不提供導(dǎo)航信息；此外，融合光學(xué)灰度信息與三維點(diǎn)云信息進(jìn)行障礙物識(shí)別，可以有效解決采用單一灰度信息進(jìn)行粗避障處理中圖像紋理對(duì)障礙物識(shí)別的干擾，提高障礙物識(shí)別的準(zhǔn)確度，避免單一信息源存在的缺陷，豐富避障信息來(lái)源，提高了算法的魯棒性和可靠性。經(jīng)過(guò)仿真運(yùn)算，本文方法具備可行性，效果較為理想，但算法的運(yùn)算量較大，對(duì)視覺(jué)計(jì)算機(jī)的運(yùn)算能力要求較高。

隨著航天技術(shù)的發(fā)展，地外星體的巡視探測(cè)將越來(lái)越普遍，人類航天器走向深空的步伐也會(huì)越來(lái)越遠(yuǎn)，對(duì)探測(cè)器的自主性要求更高，本文提出的方法可以很好地適應(yīng)深空探測(cè)的應(yīng)用需求。