第4章

自主導航系統(tǒng)的環(huán)境感知前面章節(jié)介紹了自主導航的理論基礎(chǔ)及慣性導航技術(shù)，對于機器人應(yīng)用而言，僅用慣性導航系統(tǒng)是不完備的，需要其它信息輔助完成自主導航定位。對于海面艦船、航空航天等大型軍用設(shè)備，常采用慣性導航技術(shù)與天文、地磁、衛(wèi)星等導航方式進行組合導航。而在小型的機器人系統(tǒng)中，常采用激光雷達及視覺傳感器等小型設(shè)備完成自主導航定位。本章中以小型機器人應(yīng)用為背景，介紹常用的激光雷達、視覺傳感器，以及基于二者對環(huán)境進行感知，建立環(huán)境地圖的主要過程。4.1環(huán)境感知常用傳感器雷達是指利用探測介質(zhì)探測物體距離的設(shè)備，比如無線電測距雷達、激光測距雷達、超聲波測距雷達等，如圖4-1所示。由于激光具有很好的抗干擾性和直線傳播特性，因此激光測距具有很高的精度。激光雷達測距精度往往可以達到厘米級或毫米級，廣泛應(yīng)用于機器人導航避障、無人駕駛汽車、安防、智能交互等領(lǐng)域。4.1.1激光雷達原理及數(shù)據(jù)處理激光測距無線電測距超聲波測距圖4-1常見雷達種類4.1.1激光雷達原理及數(shù)據(jù)處理激光雷達測距方式主要是三角測距和TOF(TimeofFly，飛行時間)測距兩。1. 激光雷達測距原理(1)三角測距三角測距法的原理示意如圖4-2所示，發(fā)射一束激光，被物體A反射后，照射到圖像傳感器的

位置，這樣就形成了一個三角形，通過解算可以求出物體A到激光器的距離。圖4-2三角測距原理示意4.1.1激光雷達原理及數(shù)據(jù)處理(2)TOF測距最常見的測距傳感器基于飛行時間原理(TOF)。這類傳感器通常包含發(fā)射器和接收器兩個環(huán)節(jié)，發(fā)射器能夠主動向環(huán)境發(fā)射信號，信號在遇到環(huán)境中的障礙物后被反射，進而被接收器接收。圖4-3TOF傳感器的基本原理

4.1.1激光雷達原理及數(shù)據(jù)處理圖4-4二維激光傳感器的構(gòu)成圖4-5二維激光雷達（1線）圖4-6二維激光雷達掃描結(jié)果2.典型的激光雷達

當把單線的激光測距儀固定安裝到電動機上時，電動機的轉(zhuǎn)動可以帶來激光測距儀對周圍環(huán)境多個角度的測量，單線激光雷達掃描點通常處在同一平面上的360°范圍內(nèi)，如圖4-4所示，形成二維激光掃描儀，又稱2D激光雷達。4.1.1激光雷達原理及數(shù)據(jù)處理2.典型的激光雷達

二維激光雷達的單線掃描只能掃描同一平面上的障礙信息，也就是環(huán)境的某一個橫截面的輪廓，這樣掃描的數(shù)據(jù)信息很有限。在垂直方向同時發(fā)射多束激光，再結(jié)合旋轉(zhuǎn)機構(gòu)，就能掃描多個橫截面的輪廓，這就是多線激光雷達，也叫3D激光雷達。多線激光雷達的原理示意如圖4-7所示。16線的激光雷達如圖4-8所示。圖4-8三維激光雷達（16線）

(a)側(cè)視圖

(b)俯視圖

圖4-7多線激光雷達掃描點4.1.1激光雷達原理及數(shù)據(jù)處理3.激光雷達的性能參數(shù)

理解激光雷達的性能參數(shù)是挑選和使用激光雷達的前提。常用的激光雷達的主要性能參數(shù)包括以下幾個：激光線數(shù)、測距頻率、掃描頻率、測距量程、掃描角度、距離分辨率、角度分辨率、使用壽命。常見的激光雷達型號以供大家選擇，如表4-1所示。表4-1

常見激光雷達型號公司型號德國SICKLMS111、LMS151、TIM561日本

HOKUYOURG-04LX、UST-10LX、UTM-30LX美國

VelodyneVLP-16、VLP-32C、HDL-64E、VLS-128上海恩嵐科技RPLIDAR-A1、RPLIDAR-A2、RPLIDAR-A3深圳市鐳神智能LS01A、LS01D、LS01E、LS01B深圳玩智商科技YDLIDAR-G4、YDLIDAR-X4、YDLIDAR-X2大族激光3i-LIDAR-Delta2B、3i-LIDAR-Delta3速騰聚創(chuàng)RS-LiDAR-16、RS-LiDAR-32、RS-Ruby4.1.1激光雷達原理及數(shù)據(jù)處理4.激光雷達數(shù)據(jù)處理

激光雷達的數(shù)據(jù)處理主要包括兩個過程：濾波及校正。濾波處理是應(yīng)對激光雷達在測量的過程中，有時候激光雷達數(shù)據(jù)會受到一些干擾，需要經(jīng)過一些簡單的濾波處理。目前基于Ubuntu和ROS系統(tǒng)是機器人主要使用的操作系統(tǒng)，對于激光雷達的濾波處理，ROS系統(tǒng)中功能包laser_filters內(nèi)有較豐富的濾波函數(shù)可以參考，如表4-2所示。表4-2

功能包laser_filters中包含的濾波函數(shù)函數(shù)名描述LaserArrayFilter將雷達數(shù)據(jù)存入數(shù)組，便于后續(xù)處理ScanShadowsFilter濾除因自身遮擋而產(chǎn)生的干擾數(shù)據(jù)InterpolationFilter在可信任的掃描點LaserScanIntensityFilter濾除在設(shè)定強度閾值之外的數(shù)據(jù)LaserScanRangeFilter濾除在設(shè)定距離范圍之外的數(shù)據(jù)LaserScanAngularBoundsFilter濾除在設(shè)定掃描角度范圍之外的數(shù)據(jù)LaserScanAngularBoundsFilterInPlace濾除在設(shè)定掃描角度范圍之內(nèi)的數(shù)據(jù)LaserScanBoxFilter濾除在設(shè)定區(qū)域范圍之內(nèi)的數(shù)據(jù)4.1.1激光雷達原理及數(shù)據(jù)處理

由于雷達是通過旋轉(zhuǎn)進行掃描，所以掃描數(shù)據(jù)點會因機器人自身移動而產(chǎn)生偏差。當機器人靜止時。激光雷達旋轉(zhuǎn)一圈掃描到的點序列都是以機器人當前靜止位置作為參考的，激光雷達的測距誤差僅來自測距方法本身。而當機器人處于移動狀態(tài)時，因為激光雷達并不知道自身處于移動狀態(tài)，所以激光雷達旋轉(zhuǎn)一圈掃描到的點序列依然是以該幀時間戳時刻的機器人位置為參考，顯然激光雷達的測距誤差除了來自測距方法本身外，還來自機器人運動產(chǎn)生的畸變。因此，有必要對雷達運動的畸變做校正。不管是單線激光雷達還是多線激光雷達，都是繞z軸旋轉(zhuǎn)進行掃描，校正方法是一樣的，所以就只討論單線激光雷達的情況了。常用的方法如下：4.1.1激光雷達原理及數(shù)據(jù)處理

已知機器人中兩幀激光雷達掃描數(shù)據(jù)

和

求機器人的位置轉(zhuǎn)移關(guān)系

，也就是激光里程計的問題，最常用的求解方法是ICP算法。通過ICP算法求前后兩幀雷達數(shù)據(jù)

和

對應(yīng)機器人位置的轉(zhuǎn)移關(guān)系

，位置的轉(zhuǎn)移關(guān)系

近似表示其當前的運動

，然后利用這個運動信息

對當前的雷達數(shù)據(jù)

做補償。ICP算法的流程如圖4-11所示。圖4-11ICP迭代過程

（1）ICP(IterativeClosestPoint，最近鄰點迭代)算法及改進4.1.1激光雷達原理及數(shù)據(jù)處理

（2）里程計輔助法

里程計輔助法采用外部的IMU或者輪式里程計來提供機器人的運動信息，就可以直接對雷達運動畸變做校正。IMU雖然可以提供極高頻率的姿態(tài)更新，但是其里程計存在很高的累積誤差，所以選用輪式里程計會更穩(wěn)定，如圖4-12所示。圖4-12里程計輔助法4.1.2視覺傳感器

視覺傳感器可以感知了解移動機器人所到之處周圍的環(huán)境，并能夠同時判斷機器人在環(huán)境中的位置，是視覺環(huán)境感知建模中的重要環(huán)節(jié)。相機是機器人進行視覺感知的傳感器，相當于機器人的眼睛。在機器人中，常見3種類型的相機，即單目相機、雙目相機和RGB-D相機，如圖4-13所示。相機由于其能感知光強，并且角度測量精度較高，分辨率也較高，所以也是機器人應(yīng)用中常見的傳感器，也可以用于測量距離。單目

雙目

RGB-D

圖4-133種類型相機4.1.2視覺傳感器1.單目相機

單目相機其實就是大家通常說的攝像頭，由鏡頭和圖像傳感器(CMOS或CCD)構(gòu)成。相機包含鏡頭(透鏡)、光圈和感光器件三個部分，其中感光器件決定了圖像平面。根據(jù)光學原理，透鏡存在聚焦平面，透鏡和聚焦平面的距離為焦距，由透鏡決定。如圖4-14所示，物距

、焦距

和焦點距離透鏡的距離

之間的關(guān)系如下:

(4-2)圖4-14相機成像機理4.1.2視覺傳感器

進一步分析單目相機成像的數(shù)學原理，假設(shè)實際環(huán)境中的物體點

在相機坐標系下的坐標值為

，物體點

透過光心在圖像傳感器上形成點

，二者的關(guān)系如圖4-15所示。圖4-15小孔成像原理

4.1.2視覺傳感器

式(4-4)就是相機的無畸變內(nèi)參模型，式中的矩陣K就是相機內(nèi)參數(shù)。在小孔成像模型中，物體點

是直接沿直線透過光心形成圖像點

。實際的相機前面是一個大大的鏡頭，鏡頭能讓更多的光線進入以加快曝光速度，但是鏡頭會對光線產(chǎn)生折射，這樣成像會產(chǎn)生畸變，這種由鏡頭折射引起的圖像畸變叫徑向畸變。

(4-4)4.1.2視覺傳感器

除了上面的徑向畸變外，還有切向畸變。相機鏡頭和圖像傳感器平面由于安裝誤差導致不平行，因此引入了切向畸變，如圖4-16所示。

圖4-16切向畸變4.1.2視覺傳感器(4-5)

相機的成像模型就可以寫成式(4-6)所示形式。

(4-6)

式(4-6)就是相機的外參模型，式中的矩陣

就是相機外參數(shù)。外參

就是相機在世界坐標系下的位姿。4.1.2視覺傳感器2.雙目相機

單目相機無法測量物體點的深度信息。但是，用兩個單目相機組成的雙目相機就可以測量深度信息，有些地方也把雙目相機叫深度相機。為了方便理解，先說明理想情況下雙目相機測量深度的原理，如圖4-17所示。圖4-17雙目相機測量深度原理4.1.2視覺傳感器

圖4-18非理想雙目相機模型4.1.2視覺傳感器3.RGB_D相機

雙目相機雖然能測量深度信息，但是需要事先找到同一物體點在左右相機中成像點對，也就是要先匹配。匹配過程很容易受到光照強度等環(huán)境因素干擾，在沒有特征的環(huán)境中，匹配會失效，深度信息將無法測量。RGB-D相機是主動測量深度的傳感器，受環(huán)境的干擾會小一些。RGB-D相機一般有3個鏡頭：中間的鏡頭是普通的攝像頭，采集彩色圖像；另外兩個鏡頭分別用來發(fā)射紅外光和接收紅外光。圖4-19結(jié)構(gòu)光三維視覺透視攝影模型4.1.3視覺傳感器標定方法

前面我們介紹了單目、雙目及RGBD視覺傳感器的成像原理，建立圖像像素點與世界坐標下任意點之間的數(shù)學關(guān)系，定義了相機的內(nèi)部參數(shù)和外部參數(shù)。本節(jié)分別介紹直接線性標定法和張正友標定法。

1. 直線線性標定法DLT標定法需要將一個特制的立方體標定模板放置在所需標定相機前，其中標定模板上的標定點相對于世界坐標系的位置已知。這樣相機的參數(shù)可以利用相線性模型得到。根據(jù)無畸變內(nèi)參模型公式(4-4)寫出具體的內(nèi)參方法，如式(4-9)所示。

(4-9)(4-10)4.1.3視覺傳感器標定方法

(4-11)

(4-12)DLT方法的優(yōu)點是計算速度很快，操作簡單且易實現(xiàn)。缺點是由于沒有考慮攝像機鏡頭的畸變，因此不適合畸變系數(shù)很大的鏡頭，否則會帶來很大誤差。4.1.3視覺傳感器標定方法2. 張正友標定法

張正友標定法，也稱Zhang標定法，是由微軟研究院的張正友博士于1998年提出的一種介于傳統(tǒng)標定方法和自標定方法之間的平面標定方法。它既避免了傳統(tǒng)標定方法設(shè)備要求高、操作繁瑣等缺點，又比自標定的精度高、魯棒性好。該方法主要步驟如下:1) 打印一張黑白棋盤方格圖案，并將其貼在一塊剛性平面上作為標定板;2) 移動標定板或者相機，從不同角度拍攝若干照片(理論上照片越多，誤差越小);3) 對每張照片中的角點進行檢測，確定角點的圖像坐標與實際坐標;4) 在不考慮徑向畸變的前提下，即采用相機的線性模型。根據(jù)旋轉(zhuǎn)矩陣的正交性，通過求解線性方程，獲得攝像機的內(nèi)部參數(shù)和第一幅圖的外部參數(shù);5) 利用最小二乘法估算相機的徑向畸變系數(shù);6) 根據(jù)再投影誤差最小準則，對內(nèi)外參數(shù)進行優(yōu)化。以下介紹上述步驟的具體實現(xiàn)過程。4.1.4圖像特征的提取匹配方法-SIFT算法David.Lowe總結(jié)了現(xiàn)有的基于不變量技術(shù)的特征檢測方法，正式提出了一種基于尺度空間的，對圖像平移、旋轉(zhuǎn)、縮放、甚至仿射變換保持不變性的圖像局部特征，以及基于該特征的描述符。并將這種方法命名為尺度不變特征變換（ScaleInvariantFeatureTransform），簡稱SIFT算法。1. 高斯尺度空間的極值檢測

特征點檢測的第一步是能夠識別出目標的位置和尺度，對同一個目標在不同的視角下這些位置和尺度可以被重復地分配。并且這些檢測到的位置是不隨圖像尺度的變化而改變的，因為它們是通過搜索所有尺度上的穩(wěn)定特征得到的，所應(yīng)用的工具就是被稱為尺度空間的連續(xù)尺度函數(shù)。圖像的尺度空間用L(x,y,σ)函數(shù)表示，它是由一個變尺度的二維高斯函數(shù)G(x,y,σ)與圖像I(x,y)通過卷積產(chǎn)生的。

4.1.4圖像特征的提取匹配方法-SIFT算法

利用高斯拉普拉斯方法(LaplacianofGaussian，LoG)，即圖像的二階導數(shù)，能夠在不同的尺度下檢測到圖像的斑點特征，從而可以確定圖像的特征點。但LoG的效率不高，因此SIFT算法進行了改進，通過對兩個相鄰高斯尺度空間的圖像相減，得到一個DifferenceofGaussians(高斯差分，DoG)的響應(yīng)值圖像

來近似LoG:4.1.4圖像特征的提取匹配方法-SIFT算法

為了在連續(xù)的尺度下檢測圖像的特征點，需要建立DoG金字塔，而DoG金字塔的建立又離不開高斯金字塔的建立。如圖4-20所示，左側(cè)為高斯金字塔，右側(cè)為DoG金字塔。圖4-20高斯金字塔和DoG金字塔4.1.4圖像特征的提取匹配方法-SIFT算法

極值點的搜索是在DoG金字塔內(nèi)進行的，這些極值點就是候選的特征點。在搜索之前，需要在DoG金字塔內(nèi)剔除那些像素灰度值過小的點，因為這些像素具有較低的對比度，它們肯定不是穩(wěn)定的特征點。極值點的搜索不僅需要在它所在尺度空間圖像的鄰域內(nèi)進行,還需要在它的相鄰尺度空間的圖像內(nèi)進行，如圖4-21所示。圖4-21DoG中極值點的搜索4.1.4圖像特征的提取匹配方法-SIFT算法2.特征點位置的確定

上一步得到的極值點還僅僅是候選的特征點，因為它們還存在一些不確定的因素。首先是極值點的搜索是是在離散空間內(nèi)進行的，并且這些離散空間還是經(jīng)過不斷的降采樣得到的。如果把采樣點擬合成曲面后我們會發(fā)現(xiàn)，原先的極值點并不是真正的極值點，也就是離散空間的極值點并不一定是連續(xù)空間的極值點。在這里，我們是需要精確定位特征點的位置和尺度的，也就是要達到亞像素級精度，因此必須進行擬合處理。4.1.4圖像特征的提取匹配方法-SIFT算法

使用泰勒級數(shù)展開式作為擬合函數(shù)。如上所述，極值點是一個三維矢量，即它包括極值點所在的尺度，以及它的尺度圖像坐標，即

，因此需要三維函數(shù)的泰勒級數(shù)展開式，設(shè)在

處進行泰勒級數(shù)展開，則它的矩陣形式為:它的矢量表示形式為：4.1.4圖像特征的提取匹配方法-SIFT算法經(jīng)過變量變換后，又可寫成對上式求導，得:讓它的導數(shù)為0，就可得到極值點下的相對于插值中心

的偏移量:4.1.4圖像特征的提取匹配方法-SIFT算法該極值點下的極值：4.1.4圖像特征的提取匹配方法-SIFT算法3.方向角度的確定

經(jīng)過上面兩個步驟，一幅圖像的特征點就可以完全找到，而且這些特征點是具有尺度不變性的。但為了實現(xiàn)旋轉(zhuǎn)不變性，還需要為特征點分配一個方向角度，也就是需要根據(jù)檢測到的特征點所在的高斯尺度圖像的局部結(jié)構(gòu)求得一個方向基準。該高斯尺度圖像的尺度σ是已知的，并且該尺度是相對于高斯金字塔所在組的基準層的尺度。而所謂局部結(jié)構(gòu)指的是在高斯尺度圖像中以特征點為中心，以r為半徑的區(qū)域內(nèi)計算所有像素梯度的幅角和幅值，半徑r為

式中，σ就是上面提到的相對于所在組的基準層的高斯尺度圖像的尺度。4.1.4圖像特征的提取匹配方法-SIFT算法

像素梯度的幅值和幅角的計算公式為：

因為在以r為半徑的區(qū)域內(nèi)的像素梯度幅值對圓心處的特征點的貢獻是不同的，距離圓心越近，貢獻越大，因此還需要對幅值進行加權(quán)處理，這里采用的是高斯加權(quán)，該高斯函數(shù)的標準差

為:4.1.4圖像特征的提取匹配方法-SIFT算法

在完成特征點鄰域范圍內(nèi)的梯度計算后，還要應(yīng)用梯度方向直方圖來統(tǒng)計鄰域內(nèi)像素的梯度幅角所對應(yīng)的幅值大小。為了防止某個梯度幅角因受到噪聲的干擾而發(fā)生突變，我們還需要對梯度方向直方圖進行平滑處理。平滑公式:

式中，h

和

H分別表示平滑前和平滑后的直方圖。主方向圖4-22主梯度方向示意圖4.1.4圖像特征的提取匹配方法-SIFT算法

每個特征點除了必須分配一個主方向外，還可能有一個或更多個輔方向，增加輔方向的目的是增強圖像匹配的魯棒性。輔方向的定義是，當存在另一個柱體高度大于主方向柱體高度的80%時，則該柱體所代表的方向角度就是該特征點的輔方向。

在第二步中，我們實現(xiàn)了用兩個信息量來表示一個特征點，即位置和尺度。那么經(jīng)過上面的計算，我們對特征點的表示形式又增加了一個信息

方向，即

。如果某個特征點還有一個輔方向，則這個特征點就要用兩個值來表示

和

，其中

表示主方向，

表示輔方向，而其他的變量

不變。4.1.4圖像特征的提取匹配方法-SIFT算法

4.1.4圖像特征的提取匹配方法-SIFT算法

所以上述的特征點鄰域區(qū)域?qū)嶋H應(yīng)該有

個像素點。計算旋轉(zhuǎn)以后特征點鄰域范圍內(nèi)像素的梯度幅值和梯度幅角，梯度幅值根據(jù)其對中心特征點貢獻的大小進行加權(quán)處理，加權(quán)函數(shù)仍然采用高斯函數(shù)，方差為

。計算特征點描述符的時候，不需要精確知道鄰域內(nèi)所有像素的梯度幅值和幅角，只需要根據(jù)直方圖知道其統(tǒng)計值即可。這里的直方圖是三維直方圖，就是在特征點鄰域區(qū)域，劃分一個子區(qū)域，一共有

個正方體。而這個三維直方圖的值則是正方體內(nèi)所有鄰域像素的高斯加權(quán)后的梯度幅值之和，所以一共有128個值。我們把這128個數(shù)寫成一個128維的矢量，該矢量就是該特征點的特征矢量，所有特征點的矢量構(gòu)成了最終的輸入圖像的SIFT描述符。

經(jīng)過三維直方圖的計算，該特征點的特征矢量

。為了去除光照變化的影響，需要對特征矢量進行歸一化處理，即：則

為歸一化后的特征矢量。4.1.4圖像特征的提取匹配方法-SIFT算法

匹配結(jié)果如圖4-23所示。4.1.4圖像特征的提取匹配方法-SIFT算法圖4-23SIFT特征初始匹配結(jié)果

從圖4-23中可以看出匹配中存在許多誤匹配的情況，還需要進一步的剔除誤匹配點。RANSAC是一種經(jīng)典的方法，可以利用特征點集的內(nèi)在約束關(guān)系去除錯誤的匹配。其思想如下：首先選擇兩個點，這兩個點就確定了一條直線，將這條直線的一定距離范圍內(nèi)的點稱為這條直線的支撐，隨即選擇重復次數(shù)，然后具有最大支撐集的直線被確定為是此樣本點集合的擬合，在擬合的距離范圍內(nèi)的點被稱為內(nèi)點，反之為外點。去除無匹配結(jié)果如圖4-24所示。4.1.4圖像特征的提取匹配方法-SIFT算法圖4-24RANSAC剔除誤匹配點后的匹配結(jié)果

根據(jù)圖像中特征點匹配結(jié)果，求解基礎(chǔ)矩陣F，求解基礎(chǔ)矩陣通常有7點法、8點法、迭代算法、LMedS法和RANSAC等，在第5章有詳細的說明。根據(jù)其計算的魯棒性及準確性，并基本相機的內(nèi)參和外參關(guān)系，進而求得相機的姿態(tài)，從而實現(xiàn)載體的定位。4.2環(huán)境建模

環(huán)境建模其實就是對環(huán)境狀態(tài)進行描述，也就是構(gòu)建環(huán)境地圖。地圖可以用于定位，也可以用于避障，因此定位用到的地圖與避障用到的地圖并不一定相同。環(huán)境地圖有多種比如點云地圖、柵格地圖、特征地圖、拓撲地圖等。視覺SLAM通常以構(gòu)建特征地圖和點云地圖為主，激光SLAM則以構(gòu)建柵格地圖為主。另外需要注意的是在導航過程中需要避開障礙物，特征地圖或點云地圖必須轉(zhuǎn)換換成柵格地圖后才能導航，下面主要討論二維環(huán)境地圖和三維環(huán)境地圖的建立。4.2環(huán)境建模移動機器人利用配備的外部傳感器感知周圍環(huán)境，獲得的環(huán)境信息經(jīng)過處理將以一種的抽象形式對空間進行描述，并被存儲為地圖信息。地圖構(gòu)建屬于環(huán)境特征提取與知識表示方法的范疇，決定了系統(tǒng)如何存儲、利用和獲取知識。創(chuàng)建的地圖將應(yīng)用于機器人路徑規(guī)劃等任務(wù)，所以所構(gòu)建的地圖必須便于機器人處理，并具有良好的可擴展性。移動機器人地圖構(gòu)建方法與地圖空間的表述方式密切相關(guān)。移動機器人用于描述工作環(huán)境的地圖表示方法分類如圖4-25所示。圖4-25地圖表示方法分類4.2.1二維環(huán)境地圖建立

二維地圖一般應(yīng)用于室內(nèi)準結(jié)構(gòu)化環(huán)境，如:辦公室、家居室、博物館等場所，這類室內(nèi)環(huán)境由有限的結(jié)構(gòu)化物體組合而成，機器人在構(gòu)建室內(nèi)地圖時普遍基于以下前提：1)機器人所在運動平面水平；2)對于機器人的障礙物由一系列垂直于運動平面的豎直平面或曲面表示。因此，室內(nèi)環(huán)境的機器人地圖構(gòu)建通常可以簡化為平行于機器人運動平面的二維模型。接下來介紹一下幾種二維地圖構(gòu)建方法。1.點云地圖

點云地圖用空間中的點集合表示，即二維點云地圖中的點表示為

，三維點云地圖中的點則表示為

。4.2.1二維環(huán)境地圖建立

在構(gòu)建點云地圖時，可以隨著傳感器獲取數(shù)據(jù)，將新數(shù)據(jù)加入到地圖中，不需要預(yù)先定義地圖尺寸。需要注意的是：點云地圖僅僅描述了所測量得到的物體表面的空間坐標信息，其描述性較差，不能提供更高語義層次的信息以及環(huán)境結(jié)構(gòu)、特征之間的關(guān)聯(lián)性。對于移動導航來講，點云地圖并未區(qū)分所測量物體是道路還是障礙物，也沒有說明點與點之間的空間是空閑、被占還是未知，因此無法直接應(yīng)用于導航，需要對點云數(shù)據(jù)進行處理，分割出可行區(qū)域后才可以進行導航規(guī)劃，如圖4-26所示。圖4-26點云地圖的可行區(qū)域分割和導航規(guī)劃4.2.1二維環(huán)境地圖建立2.柵格地圖

柵格法是一種將物理環(huán)境抽象成二維平面環(huán)境的環(huán)境建模方法，該方法是將執(zhí)行的任務(wù)區(qū)域劃分為一系列大小相同的柵格，然后根據(jù)環(huán)境中不同的特征為這些柵格設(shè)置不同的數(shù)值，其中可以通行的區(qū)域稱為自由柵格賦值為0，而有障礙物的區(qū)域不可通行賦值為1。在概率架構(gòu)下，每個柵格存儲的是該柵格的被占概率。即地圖表示為

表示地圖中第

個柵格單元被占情況，取值為0或1，在程序?qū)崿F(xiàn)時柵格內(nèi)存儲的則是

，表示

的概率。當

時，表示柵格各確定被占;當

時，表示柵格確定空閑；當

取0~1之間的值時，表示柵格被占的不確定性;當=0.5時，意味著其被占不確定性最大。4.2.1二維環(huán)境地圖建立

利用二維柵格地圖可以表示某個平面的環(huán)境信息，被占概率表示的是對應(yīng)二維平面空間的被占情況。將柵格被占概率值與灰度或者顏色對應(yīng)，可以實現(xiàn)柵格地圖的圖像化顯示。如圖4-27所示的二維柵格地圖，其中黑色對應(yīng)

，白色對應(yīng)

，不同灰色對應(yīng)不同的被占概率。圖4-27二維柵格地圖圖像化顯示圖3.特征地圖

特征地圖以抽象的特征描述環(huán)境，如圖4-28所示，通常采用擬合障礙物的陸標、線段、平面、多邊形等結(jié)構(gòu)性幾何特征，這些基礎(chǔ)特征通過一組參數(shù)進行建模。早期的地圖構(gòu)建與定位研究主要采用陸標特征地圖，用抽象環(huán)境表示真實環(huán)境，每個陸標用其在地圖中的笛卡兒坐標描述，也可增加紋理等標識信息描述。在概率架構(gòu)下，一般采用高斯分布模型描述陸標信息，每個陸標采用一個高斯分布模型

描述，均值

表示估計值，其中

為平面坐標，

可包含各種特征標識，用于進行特征匹配，方差

表示估計的不確定性。4.2.1二維環(huán)境地圖建立圖4-28陸標特征地圖示意4.2.1二維環(huán)境地圖建立

對于室內(nèi)等結(jié)構(gòu)性環(huán)境，線段特征也經(jīng)常被采用，形成擬合障礙物邊緣的線段特征地圖。隨著二維地圖向三維地圖的發(fā)展，線段特征地圖也衍生為擬合障礙物表面的平面特征地圖，如圖4-29所示。具體表示時，需要根據(jù)特征擬合和匹配需要，找到合適的特征表示參數(shù)。例如，線段可以用線段中心點、斜率、長度表示，也可以用線段到坐標系原點距離、線段法線方向、線段中心點和長度來表示。圖4-29三維平面特征地圖

相較于點云地圖和柵格地圖，特征地圖具有簡潔、緊湊、內(nèi)存占用量小等優(yōu)點，同時對環(huán)境具有更高層次的描述性，使定位與建圖的魯棒性更強。因此，近年來研究人員考慮進一步提升環(huán)境描述的層次，引入基礎(chǔ)物體信息，以實現(xiàn)魯棒性更好的數(shù)據(jù)關(guān)聯(lián)推理。例如，基于RGBD信息進行環(huán)境的三維稠密表面重構(gòu)，

通過實時三維目標識別和跟蹤提取場景中的物體，從而實現(xiàn)更加魯棒的閉環(huán)檢測、移動對象檢測等，如圖4-30所示。但是特征地圖無法精確表征復雜環(huán)境細節(jié)

不能表示環(huán)境的被占用、空閑和未知情況，因此不能直接用于導航規(guī)劃，需要根據(jù)特征地圖生成可行區(qū)域。4.2.1二維環(huán)境地圖建立圖4-30基礎(chǔ)物體特征地圖4.2.1二維環(huán)境地圖建立3.拓撲地圖

拓撲法是將需要規(guī)劃的任務(wù)空間分割為具有拓撲特征的子空間，同時構(gòu)建拓撲網(wǎng)絡(luò)，并在拓撲網(wǎng)絡(luò)上找到起始點至目標點的拓撲路徑的方法。拓撲法可以將高維的復雜空間轉(zhuǎn)化為簡單的低維拓撲空間的，有效地減小了實際搜索的空間范圍。使用拓撲法不需要知道載體的具體工作環(huán)境，通過一些代表性的拓撲節(jié)點和拓撲線段構(gòu)成拓撲網(wǎng)絡(luò)，清晰體現(xiàn)環(huán)境中各個位置的關(guān)系。其優(yōu)點在于方法比較直觀，對于環(huán)境信息的要求不高；缺點在于如果環(huán)境中存在障礙物數(shù)目較多，則拓撲網(wǎng)絡(luò)比較復雜，且在實際應(yīng)用中存在位置信息的誤差，難以修改原有拓撲網(wǎng)絡(luò)。4.2.1二維環(huán)境地圖建立

拓撲地圖采用圖形式表示，圖中節(jié)點表示環(huán)境中的某個地點，節(jié)點之間的連線(邊)表示兩個節(jié)點之間可行，如圖4-31所示。

拓撲地圖表示形式簡潔緊湊，可以進行快速的路徑規(guī)劃。路徑規(guī)劃方法中的行車圖法和概率路圖法是根據(jù)環(huán)境障礙物分別通過解析或者概率采樣的方法來構(gòu)建環(huán)境的拓撲地圖，再應(yīng)用A*等搜索算法進行最優(yōu)路徑規(guī)劃。在拓撲地圖節(jié)點上存儲該場景的傳感器信息(如圖片、激光點云等)或者由傳感器信息提取得到的特征描述(如視覺特征、線段特征等)形成對該地點的描述，在定位時可以根據(jù)實時感知信息與地圖存儲信息匹配進行位置識別。但節(jié)點分布存在稀疏問題，難以實現(xiàn)任意時刻的精確定位。當兩個節(jié)點信息具有相似性時，可能導致定位錯誤。圖4-31拓撲地圖4.2.2三維環(huán)境地圖建立

目前，移動機器人二維環(huán)境地圖構(gòu)建技術(shù)已經(jīng)十分成熟，但隨著移動機器人的研究領(lǐng)域從結(jié)構(gòu)化室內(nèi)場景向非結(jié)構(gòu)化的室外場景過度，在室外場景中存在不同高度層面的環(huán)境特征，二維地圖表示方法已經(jīng)無法滿足機器人更高的環(huán)境感知要求，而三維地圖構(gòu)建能為機器人提供更加豐富的環(huán)境信息，成為當前移動機器人地圖構(gòu)建領(lǐng)域的研究熱點。三維環(huán)境地圖由于信息量的增大，給機器人處理帶來高存儲、高計算量、復雜數(shù)據(jù)處理算法等一系列問題。常用的三維地圖包括:三維幾何地圖、立體柵格地圖和高度地圖，它們在描述三維環(huán)境時存在各自的優(yōu)缺點，下面將分別進行介紹。4.2.2三維環(huán)境地圖建立1.三維幾何地圖

三維幾何地圖與二維幾何地圖類似，也是用從傳感器信息中提取的環(huán)境特征描述環(huán)境，只是在空間描述上添加了一維信息。最常用也是最直觀的方式是用三維點云數(shù)據(jù)描述三維環(huán)境(如圖4-32a所示)。(a)三維點云地圖

(b)三維平面地圖圖4-32三維幾何地圖4.2.2三維環(huán)境地圖建立2.立體柵格地圖

相比于二維柵格，三維柵格的數(shù)量更大。為了提高三維柵格地圖數(shù)據(jù)處理效率，通常采用八叉樹對三維柵格數(shù)據(jù)進行編碼存儲。八叉樹最早是由Hunter博士提出的一種數(shù)據(jù)結(jié)構(gòu)模型，它是二維平面四叉樹到三維立體空間的推廣。后來將其應(yīng)用于空間場景建模中，將空間結(jié)構(gòu)按照一定的規(guī)律劃分為八個卦限，形成一種八分支樹的表現(xiàn)形式。八叉樹在進行數(shù)據(jù)結(jié)構(gòu)分層時，每一個節(jié)點都遵從2×2×2的遞歸規(guī)則，直到達到設(shè)定的遞歸深度或預(yù)設(shè)的最小閾值停止劃分，否則會一直無休止的分割下去。如圖4-33a所示。在三維空間中，一個立方體的體積元素代表八叉樹的一個節(jié)點。其中，最大的立方體塊是八叉樹的“根節(jié)點”，最小的立方體塊是“葉節(jié)點”，其余的立方體塊是八叉樹的“子節(jié)點”。八叉樹中的每一個節(jié)點都可以看作由一個編碼和八個指向節(jié)點的指針(0，…，7)組成。圖4-33b所示的是一個具有三層樹狀結(jié)構(gòu)的八叉樹。4.2.2三維環(huán)境地圖建立

在八叉樹中，樹中的每一個節(jié)點都可以用

之間的浮點數(shù)表示其在存儲空間占據(jù)的概率情況。若數(shù)值為0，表示該節(jié)點在空間的存儲狀態(tài)是完全被釋放；若數(shù)值為0.5，表示該節(jié)點空間的存儲狀態(tài)未知；若數(shù)值為1，表示該節(jié)點在空間的存儲狀態(tài)是完全被占據(jù)。通常數(shù)值越大，表示樹節(jié)點被占據(jù)的概率越高；否則，樹節(jié)點被占據(jù)的概率越低。使用八叉樹數(shù)據(jù)結(jié)構(gòu)建圖的好處是當樹中的某個子節(jié)點都是“被占據(jù)”、“被釋放”或“未知”狀態(tài)時，就可以把它減去，減少存儲空間的占用。(a)八叉樹空間分割模型(b)八叉樹的樹狀表示圖4-33八叉樹數(shù)據(jù)結(jié)構(gòu)示意圖4.2.2三維環(huán)境地圖建立

圖4-34描述了八叉樹地圖的結(jié)果，其中圖4-34a為點云地圖；圖4-34b為根據(jù)點云地圖得到的八叉樹地圖，體素分辨率為0.08m，不同的體素分辨率可以取得不同的結(jié)果，可根據(jù)需要調(diào)整分辨率；圖4-34c和圖4-34d分別為體素分辨率0.64m和1.28m下的結(jié)果顯示。圖4-34不同最小分辨率定義下的八叉樹地圖4.2.2三維環(huán)境地圖建立3.高度地圖

二維柵格地圖只能表示某個平面的環(huán)境信息，無法表示地面的不平整性。而三維柵格地圖則過于耗費存儲資源和計算時間，雖然可以通過八叉樹表示來減少空間需求，但隨之帶來的是數(shù)據(jù)結(jié)構(gòu)和程序的復雜。為實現(xiàn)不平整地面上的高效導航規(guī)劃，比如行星探測、足式機器人落腳點規(guī)劃等，M.Herbert等人提出了高度地圖（ElevationMap）表示法，也稱為2.5維占用柵格地圖。如圖4-35所示，它采用二維柵格地圖表示方法，但在每個柵格中不是存儲該柵格被占概率，而是該柵格內(nèi)障礙物的高度信息，通過高斯分布來表示高度估計和不確定性。從而可以根據(jù)柵格內(nèi)存儲的高度信息來圖像化顯示的當下環(huán)境地貌。圖4-35高度地圖表示存儲信息

除了柵格被占概率或者障礙物高度信息，可以根據(jù)不同的應(yīng)用需求在柵格地圖中存儲不同信息。例如，一些基于優(yōu)化的軌跡規(guī)劃算法需要獲取地圖中每個點到障礙物的距離信息或者距離的梯度信息，因此一些應(yīng)用在每個柵格內(nèi)存儲距離其最近的被占用柵格與該柵格之間的歐氏距離，稱為有向歐氏距離(EuclideanSignedDistanceFields，ESDF)。ESDF是在傳感器觀察該柵格位置視線上柵格到最近障礙物表面的距離，如圖4-36所示。4.2.2三維環(huán)境地圖建立圖4-36ESDF計算示意圖4.2.2三維環(huán)境地圖建立

為了減少計算量，一些三維地圖構(gòu)建方法采用截斷距離場(TruncatedSignedDistanceFields，TSDF)描述地圖。如圖4-37所示，即只計算障礙物附近一定距離內(nèi)柵格的ESDF數(shù)值；對于超出計算范圍的柵格，在障礙物表面與傳感器之間的