移動機器人室外環(huán)境分級地形分類與參數(shù)估計

鐘晨，王偉＊

（1.大連理工大學(xué) 信息與控制研究中心，遼寧 大連 116024；2.大連理工大學(xué) 機器人學(xué)國家重點實驗室，遼寧 大連 116024）

0 引言

隨著移動機器人環(huán)境通過能力與環(huán)境適應(yīng)能力的不斷增強，對地形的有效識別與建模成為機器人完成導(dǎo)航及運動規(guī)劃等任務(wù)的基礎(chǔ).由于室外場景中地形具有多樣性和復(fù)雜性，并且在不同地形條件下機器人往往需要采用不同的運動策略，對地形表述方法的選擇既要考慮各類地形特點又要滿足機器人運動規(guī)劃的需求.

基于三維激光點云的地形描述可以快速準(zhǔn)確地獲得場景的幾何信息.Broten等［1］通過計算各個柵格的加權(quán)平均高度構(gòu)建高程地圖，能夠輔助機器人穿越未知室外環(huán)境，但無法準(zhǔn)確表示場景中的垂直或斷層特征.Pfaff等［2］對高程圖表示方法進(jìn)行了有效擴展，采用分層高程圖能夠更準(zhǔn)確地表示場景中懸空結(jié)構(gòu)，但對于機器人在不平坦路面上的可通過性仍會產(chǎn)生誤判.Gingras等［3－4］通過高程劃分可通行區(qū)域與障礙區(qū)域，采用改進(jìn)的三角網(wǎng)格法對可通行區(qū)域進(jìn)行地形重構(gòu)以及路徑規(guī)劃，但該方法不適合帶有一定坡度的路面環(huán)境.文獻(xiàn)［5－6］將監(jiān)督學(xué)習(xí)的方法用于室外場景點云分類，訓(xùn)練樣本的選擇會對點云分類結(jié)果產(chǎn)生明顯的影響，而且場景分類結(jié)果不便應(yīng)用于地形的可通過性判別.Rusu等［7－8］針對室內(nèi)廚房環(huán)境建模問題，采用點云分割和幾何形體匹配等方法提取幾何圖元，并根據(jù)功能推理法判別幾何圖元所屬的物體類別.

機器人執(zhí)行室外任務(wù)時所面對的地形類型及地形復(fù)雜程度會不斷變化.對于具有一定越障能力的移動機器人，如果以提高其環(huán)境通過效率為根本目標(biāo)，需要根據(jù)不同的地形特征采取不同的地形建模策略并相應(yīng)調(diào)整機器人的運動模式.本文中所采用的機器人平臺為中國科學(xué)院沈陽自動化研究所自主研發(fā)的輪腿式機構(gòu)可變機器人.該機器人根據(jù)其所通過地形類型的不同，主要采用3種運動模式：（1）當(dāng)處于平坦路面上時，要求機器人以較快速度行進(jìn)并實現(xiàn)避障，因此地形建模僅需要將場景簡單分為可通行路面與障礙物區(qū)域.（2）當(dāng)處于不平坦路面上時，要求機器人以較慢速度行駛并通過調(diào)整輪腿姿態(tài)降低車體重心以增強車體運動穩(wěn)定性，地形建模需要能夠更細(xì)致地區(qū)分可通行的不平坦路面和不可通行的障礙或植被區(qū)域.（3）當(dāng)遇到需進(jìn)行機構(gòu)變化才能通過的典型地形結(jié)構(gòu)時，要求機器人以越障模式通過，地形建模需要能夠?qū)?、臺等典型地形結(jié)構(gòu)進(jìn)行識別并估計其幾何參數(shù)，從而輔助機器人的運動規(guī)劃.

根據(jù)機器人在通過室外環(huán)境時的上述實際需求，本文提出一種分級的地形識別與建模方法.利用高程模型計算速度快、存儲空間小等優(yōu)勢，滿足機器人通過平坦路面時地形建模的要求.當(dāng)高程地圖無法找到可通行路徑時，采用基于模糊規(guī)則的場景點云分類方法，消除高程地圖對不平坦路面可通行性可能產(chǎn)生的誤判.如果仍然無法找到可通行路徑，則通過典型地形的語義分類及建模檢測可供機器人實現(xiàn)越障通過的典型地形結(jié)構(gòu).

1 基于分層高程地圖的場景表述

高程地圖是對占據(jù)柵格地圖的一種改進(jìn)，以平面二維柵格內(nèi)數(shù)據(jù)點的高程分布描述三維場景的地形信息，具有計算速度快、數(shù)據(jù)存儲量小等優(yōu)勢［7］.移動機器人執(zhí)行室外任務(wù)大多處于較平坦路面上，這種地形的特點是地面的起伏變化不大且地面與障礙物區(qū)別明顯.在這種情況下，往往要求機器人以較快速度行進(jìn)并區(qū)分地面區(qū)域與障礙區(qū)域，因此采用一種改進(jìn)的分層高程地圖建模方法［9］可以滿足機器人通過此類地形的基本要求.

假設(shè)三維點云中某一數(shù)據(jù)點坐標(biāo)為pi，在XY坐標(biāo)平面上劃分柵格Gmn，其中m和n為柵格序號.激光點的分布可以認(rèn)為近似符合正態(tài)分布：

為了描述樹冠、橋梁等懸空結(jié)構(gòu)對機器人可通過性的影響，對各柵格內(nèi)激光點按高度聚類，并根據(jù)聚類間高度差Δh對激光點分層.每個柵格可能包含一個或多個分層單元.激光點pi對各分層單元Uk的影響因子為

其中X＋、X－、Y＋和Y－為Uk所在柵格Gmn的有效范圍常量.將各分層單元高程初始化為單元內(nèi)激光點高度的平均值，當(dāng)新的激光點pi＝（xiyi zi）T添加到分層單元時，按式（3）更新各單元的高程和權(quán)值參數(shù)

為了便于對移動機器人可通行區(qū)域的識別，將各分層單元分為垂直單元和水平單元兩類.以式（4）計算各分層單元內(nèi)激光點的高度方差Ep，當(dāng)Ep大于閾值時，認(rèn)為單元內(nèi)激光點在豎直方向上分布較分散，將此分層單元定義為垂直單元，否則將其定義為水平單元.計算某一水平單元與其鄰域內(nèi)各水平單元的高程差，當(dāng)高程差小于閾值時，將它們劃入同一個可行區(qū)域，再采用鄰域生長的方式對可通行區(qū)域進(jìn)行擴充.

圖1 不同場景下高程地圖在機器人與目的地之間是否具有連通可通行區(qū)域的狀況Fig.1 Passable area between robot and destination in elevation maps with different scenes

在地面較平坦的室外環(huán)境中，基于高程地圖的場景建模方法可以找到機器人與目的地之間連通的可通行區(qū)域，如圖1（a）所示.但是，由于高程地圖自身的局限性，當(dāng)具有一定通過能力的機器人面對較復(fù)雜的室外環(huán)境時，通過高程地圖可能無法找到機器人與目的地之間連通的可通行區(qū)域，此時需要對場景做更精準(zhǔn)的建模以辨別當(dāng)前地形是否滿足機器人的通過條件，并相應(yīng)調(diào)整機器人自身機構(gòu)和運動方式，如圖1（b）所示.其中，場景可通行區(qū)域中的路徑搜索可采用A＊算法實現(xiàn).

2 基于點云模糊分類的地形表示

由于高程地圖僅通過局部柵格高程差是否大于閾值來識別可通行區(qū)域，對于不夠平坦的地面時常無法找到可通行路徑，此時便需要對點云做更細(xì)致的地形分類.基于監(jiān)督學(xué)習(xí)的室外場景點云分類方法已得到廣泛應(yīng)用［5－6］，但對樣本的標(biāo)記和訓(xùn)練是一項復(fù)雜、耗時的工作并且實際分類效果與樣本的選擇密切相關(guān)，且對于不同類型室外環(huán)境的通用性有限.因此采用一種改進(jìn)的模糊點云分類方法［7］，基于場景表面特征提取、模糊推理以及極大模糊熵原理實現(xiàn)與機器人通過性相關(guān)的室外地形分類，從而克服了高程地圖在復(fù)雜環(huán)境表述上存在的不足.

在室外場景三維點云數(shù)據(jù)中，分布在地面上的數(shù)據(jù)點下方的區(qū)域往往是空的，因此通過計算各數(shù)據(jù)點下方一定區(qū)域內(nèi)點的密度可以表征該點為地面點的可能性.假設(shè)與各點pi屬同一柵格Gmn的數(shù)據(jù)點為gmn，Bi表示gmn中分布在pi下方的點集，如圖2（a）所示.特征描述符Fg估計pi為地面點的可能性，計算方法如下：

其中pi＝（pixpiypiz），gmn＝（gmnxgmnygmnz），Bi＝｛gmn∈Gmn｜gmnz＜piz－ε｝，τ＝1（piz＜0）或τ＝0.5（piz≥0），余量常數(shù)ε的作用是防止鄰近數(shù)據(jù)點的干擾（例如ε＝0.01m），np表示歸一化因子（例如np＝6）.當(dāng)piz≥0時，表示pi在豎直方向上高于激光掃描原點，此時pi點作為地面點的置信度要小于當(dāng)piz＜0時，采用指數(shù)τ實現(xiàn)非線性補償.

在激光掃描面上，選取各點pi固定數(shù)目的鄰域點作為其支撐點.支撐點協(xié)方差矩陣的3個最大特征值為λ1＞λ2＞λ3，相對應(yīng)的特征向量分別為n1、n2、n3，如圖2（b）所示.特征描述符Fr估計局部表面粗糙程度，特征描述符Fa估計法向量與豎直方向的夾角，計算方法如下：

圖2 場景點云數(shù)據(jù)局部特征描述符提取Fig.2 Local feature descriptor extraction of the point cloud data

模糊推理系統(tǒng)的3個輸入量Fg、Fr和Fa表征數(shù)據(jù)點幾何分布，3個輸出量μg、μf和μv分別表征隸屬于地面、立面和植被的模糊隸屬度.這3類地形特征具有較好的可區(qū)分性和旋轉(zhuǎn)不變性，同時它們也是室外環(huán)境中構(gòu)成典型地形結(jié)構(gòu)的基本元素.室外地形特征通常符合以下3個簡單的語義規(guī)則：（1）地面通常位于場景底部且為近似水平面.（2）立面通常為近似豎直的平面.（3）植被通常立于地面上且表面粗糙、不規(guī)則.模糊規(guī)則根據(jù)以上地形語義規(guī)則產(chǎn)生，并覆蓋所有輸入量的可能組合.其中，地面點特征Fg的語義標(biāo)簽為｛N，G｝，分別代表非地面和地面；粗糙度特征Fr的語義標(biāo)簽為｛S，R｝，分別代表光滑和粗糙；傾斜度特征Fa的語義標(biāo)簽為｛F，I，V｝，分別代表水平、傾斜和豎直；輸出隸屬度μg、μf和μv的語義標(biāo)簽為｛L，M，H｝，分別代表低、中和高.所有模糊規(guī)則均通過AND 操作符連接，例如：IF（Fgis G）AND（Fris S）AND（Fais F），THEN（μgis H）AND（μfis L）AND（μvis L），模糊推理系統(tǒng)隸屬度函數(shù)及模糊規(guī)則的設(shè)置方法參見文獻(xiàn)［10］.

通過逆模糊化過程得到各數(shù)據(jù)點pi地形隸屬度的實數(shù)值μg、μf和μv.設(shè)地形類別標(biāo)簽集L＝｛g，f，v｝分別代表｛地面，立面，植被｝，由式（8）計算pi各鄰域點pin的模糊熵Ein，該模糊熵表征pin地形類別分布的不確定性，取值范圍為0到1.則根據(jù)極大模糊熵原理，pi點的地形類別lt可按式（9）方法計算.

其中μmax＝max｛μg，μf，μv｝.地形模糊分類結(jié)果如圖3所示.

圖3 地形模糊分類結(jié)果Fig.3 Result of fuzzy terrain classification

3 典型地形結(jié)構(gòu)的語義分類與建模

對于具有一定越障能力的機器人，即使通過上述高程模型以及地形模糊分類均無法找到可通行路徑，但如果場景中存在滿足機器人越障能力的地形結(jié)構(gòu)，仍可以實現(xiàn)機器人的越障通過.因此需要進(jìn)一步對與機器人避障和越障相關(guān)的地形結(jié)構(gòu)進(jìn)行建模并估計其幾何參數(shù)，從而實現(xiàn)機器人的運動控制輔助.本文將“語義地圖”“語義分類”等概念［7－8，11］應(yīng)用于室外地形結(jié)構(gòu)分析與表示中，提出一種新的室外地形結(jié)構(gòu)建模方法.

設(shè)經(jīng)過地形模糊分類的點云數(shù)據(jù)二維掃描矩陣中某列數(shù)據(jù)為Sp，將Sp中固定數(shù)目的相鄰數(shù)據(jù)點定義為檢測域，檢測域尺度參數(shù)為Dr（例如Dr＝5），用以檢測是否出現(xiàn)新的地形片段.地形片段標(biāo)簽Se初始化為－1，當(dāng)其取值為1、2、3、4時分別代表片段的地形屬性為地面（G）、立面（F）、植被（V）和間斷（I）.令Tg、Tf和Tv為表征地形片段連續(xù)性的域值，用以限定各類地形片段的最小數(shù)據(jù)點數(shù)，均不大于Dr（例如Tg＝4，Tf＝4和Tv＝3）.Pe（i）表示數(shù)據(jù)列Sp中第i點p（i）的地形類型，當(dāng)其取值為1、2、3時分別代表p（i）的地形屬性為地面（G）、立面（F）、植被（V），ρ（i）表示數(shù)據(jù)列Sp中第i點的激光測距讀數(shù)，ng、nf和nv為相鄰數(shù)據(jù)點檢測域內(nèi)不同地形屬性點的數(shù)目，it標(biāo)識當(dāng)前片段類型是否為間斷（1為間斷，0為非間斷），間斷判別的閾值計算如下：

其中設(shè)定常數(shù)k為比例因子（例如k＝5），θ為三維掃描點云的俯仰角，nr為點云矩陣數(shù)據(jù)列的序號.對各列點云數(shù)據(jù)Sp按其距離掃描儀由近及遠(yuǎn)的順序遍歷各點p（i），數(shù)據(jù)列的地形片段分割算法流程如圖4所示.

由于各列點云數(shù)據(jù)被分割為地形片段序列，可以認(rèn)為相鄰片段間的拓?fù)潢P(guān)系反映了地形結(jié)構(gòu)的語義關(guān)系，例如：地形屬性為｛地面－立面－地面｝的片段組合可以表示“臺階”這一典型語義地形結(jié)構(gòu).因此，地形片段序列中各片段不僅具有地形屬性同時也具有地形結(jié)構(gòu)屬性，不妨限定以下幾種與機器人避障和越障相關(guān)的典型的地形結(jié)構(gòu)屬性：平地面（L）、臺平面（T）、溝底面（B）、臺立面（S）、障礙立面（O）和植物（P）.各片段的地形結(jié)構(gòu)屬性僅取決于其前一片段的地形結(jié)構(gòu)屬性、當(dāng)前片段的地形屬性以及后一片段的地形屬性.地形片段拓?fù)潢P(guān)系及對應(yīng)地形結(jié)構(gòu)屬性如圖5所示.

對各列片段序列的地形結(jié)構(gòu)屬性，可根據(jù)表1所示的地形結(jié)構(gòu)語義關(guān)系進(jìn)行判別.表1 中第一列表示前一片段的地形結(jié)構(gòu)屬性的類別，第一行表示當(dāng)前片段和后一片段的地形屬性類別，各行列交叉處表示當(dāng)前片段的地形結(jié)構(gòu)屬性.其中，交叉處的復(fù)選項表示當(dāng)前片段首尾之間高度差的絕對值大于閾值ls（例如ls＝0.1m）時的取值.

圖4 點云數(shù)據(jù)列地形片段分割的算法流程圖Fig.4 Terrain fragment segmentation algorithm of point cloud data columns

圖5 地形片段拓?fù)潢P(guān)系及對應(yīng)地形結(jié)構(gòu)Fig.5 Topology of terrain fragments and the corresponding terrain structures

對各地形片段按典型地形結(jié)構(gòu)進(jìn)行分類，采用區(qū)域生長的方法對各地形片段聚類，并分別構(gòu)建各聚類的片段起始點集合以及終止點集合，如圖6（b）所示.根據(jù)語義地形結(jié)構(gòu)分類的結(jié)果，可按不同類型的地形結(jié)構(gòu)對場景進(jìn)行建模：地形結(jié)構(gòu)為O 或P的區(qū)域視為障礙；地形結(jié)構(gòu)為L 或T的區(qū)域視為通路；地形結(jié)構(gòu)為B 的區(qū)域視為溝；地形結(jié)構(gòu)為T 的區(qū)域視為臺階.對具有一定越障能力的機器人，在場景典型地形結(jié)構(gòu)分類的基礎(chǔ)上，需要對典型地形結(jié)構(gòu)進(jìn)行幾何建模以輔助機器人規(guī)劃運動策略.通過最小平方中值法（LMedS）對各聚類片段的起始點或終止點集合進(jìn)行迭代回歸，從而提取出反映典型地形結(jié)構(gòu)的邊緣線段的幾何參數(shù)，如圖6（c）所示.

表1 典型地形結(jié)構(gòu)語義關(guān)系Tab.1 Semantic relations of typical terrain structures

圖6 典型地形結(jié)構(gòu)語義分類與幾何建模Fig.6 Semantic classification and geometric modeling of typical terrain structures

4 實驗結(jié)果

本文實驗采用中國科學(xué)院沈陽自動化研究所自主研發(fā)的六輪腿機構(gòu)可變式移動機器人平臺，并配備了基于SICK 公司的LMS291激光測距儀的三維激光掃描系統(tǒng)，如圖7（a）所示.

圖7 三維激光掃描系統(tǒng)Fig.7 3Dlaser scan system

采用三維激光掃描系統(tǒng)可以得到極坐標(biāo)形式的點云數(shù)據(jù)pp＝（ρθφ），其中ρ表示激光掃描儀光心到某一數(shù)據(jù)點的距離，θ表示激光掃描儀一組掃描數(shù)據(jù)中各點的測量角度，φ表示三維激光云臺的轉(zhuǎn)角；a、b、c三個長度參數(shù)通過系統(tǒng)標(biāo)定得到.點云數(shù)據(jù)由極坐標(biāo)表示到三維直角坐標(biāo)表示的轉(zhuǎn)換如圖7（b）所示，計算公式如下：

這里列舉了對多組室外場景采用分級地形識別與建模的實驗結(jié)果.在圖8（a）所示的具有不平坦路面的場景中，高程地圖環(huán)境表述將地面的坡度起伏或雜草等干擾誤判為障礙區(qū)域，如圖8（b）所示.通過模糊地形分類，將場景分為地面、立面和植被，消除了高程地圖對不平坦路面的誤判從而可以重新找到可通行路徑，如圖8（c）所示.

圖8 不平坦路面的分級地形識別與建模結(jié)果Fig.8 Hierarchical terrain recognition and modeling result on uneven road

圖9、10為對兩組具有典型地形結(jié)構(gòu)的室外場景采用分級地形識別與建模的實驗結(jié)果.圖9（a）、10（a）為帶有臺階（溝壑）地形結(jié)構(gòu)的場景；圖9（b）、10（b）為高程地圖地形表示結(jié)果，無法找到可通行路徑；圖9（c）、10（c）為基于模糊點云分類的地形表示結(jié)果，仍然無法找到可通行路徑；圖9（d）、10（d）為典型地形結(jié)構(gòu)語義分類與建模結(jié)果，能夠識別出場景中的臺階（溝壑）結(jié)構(gòu)并給出了其三維幾何參數(shù)，機器人可根據(jù)所得地形建模參數(shù)規(guī)劃運動策略實現(xiàn)越障通過.

通過多組室外環(huán)境典型地形結(jié)構(gòu)建模結(jié)果的數(shù)據(jù)分析（見表2），可以看出本文方法地形建模平均誤差一般在1cm 左右，能夠滿足機器人越障運動規(guī)劃的需要.

表2 多組室外場景典型地形結(jié)構(gòu)建模誤差分析Tab.2 Error analysis of typical terrain structures in outdoor scene

5 結(jié)語

本文針對具有一定越障能力的移動機器人室外環(huán)境可通過性判別這一常見任務(wù)，提出了一種分級的地形識別與建模方法.根據(jù)室外地形復(fù)雜程度的差異及機器人運動模式的特點，逐級分別采用分層高程地圖環(huán)境表述、模糊地形分類和典型地形結(jié)構(gòu)語義分類及建模的方法，有效提高了地形識別效率，為實現(xiàn)機器人環(huán)境可通過性判別及運動策略規(guī)劃提供了可靠的地形建?；A(chǔ).實驗結(jié)果驗證了該方法的有效性和實用性.

［1］Broten G，Collier J.Continuous motion，outdoor，2 1／2D grid map generation using an inexpensive nodding 2－D laser rangefinder ［C］／／ IEEE International Conference on Robotics and Automation（ICRA）.Piscataway：IEEE，2006：4240－4245.

［2］Pfaff P，Triebel R，Burgard W.An efficient extension to elevation maps for outdoor terrain mapping and loop closing［J］.International Journal of Robotic Research，2007，26（2）：217－230.

［3］Gingras D，Lamarche T，Bebwani J L，etal.Rough terrain reconstruction for rover motion planning［C］／／The 7th Canadian Conference on Computer and Robot Vision（CRV）.Piscataway：IEEE，2010：191－198.

［4］Gingras D，Payre P，de Lafontain E J，etal.Path planning based on fluid mechanics for mobile robot using unstructured terrain model ［C］／／IEEE International Conference on Robotics and Automation（ICRA）.Piscataway：IEEE，2010：1978－1984.

［5］Xiong X，Munoz D，Bagnell J A，etal.3－D scene analysis via sequenced predictions over points and regions［C］／／IEEE International Conference on Robotics and Automation（ICRA）.Piscataway：IEEE，2011：2609－2616.

［6］Ross S，Munoz D，Hebert M，etal.Learning message－passing inference machines for structured prediction［C］／／IEEE Computer Vision and Pattern Recognition（CVPR）.Piscataway：IEEE，2011：2737－2744.

［7］Rusu R B，Marton Z C，Blodow N，etal.Modelbased and learned semantic object labeling in 3D point cloud maps of kitchen environments［C］／／IEEE／RSJ International Conference on Intelligent Robots and Systems（IROS）.Piscataway：IEEE，2009：3601－3608.

［8］Rusu R B，Marton Z C，Blodow N，etal.Towards 3D point cloud based object maps for household environments ［J］.Robotics and Autonomous Systems，2008，56（11）：927－941.

［9］YAN Fei，ZHUANG Yan，BAI Ming，etal.3D outdoor environment modeling and path planning based on topology－elevation model ［J］.Acta Automatica Sinica，2010，36（11）：1493－1501.

［10］ZHONG Chen，ZHUANG Yan，WANG Wei.Fuzzy classification of 3D point cloud in outdoor environment［J］.ICIC Express Letters：Part B，2011，5（8）：2741－2746.

［11］Nuechter A，Hertzberg J.Towards semantic maps for mobile robots ［J］.Robotics and Autonomous Systems，2008，56（11）：915－926.