1.2 自主導(dǎo)航定位技術(shù)

1.2.1 基于激光地圖的長(zhǎng)期視覺-慣性定位

1.2.1.1 視覺-慣性定位問題分析

高精度的定位是移動(dòng)機(jī)器人在環(huán)境中實(shí)驗(yàn)自主導(dǎo)航的前提。傳統(tǒng)的定位系統(tǒng)多是基于激光傳感器實(shí)現(xiàn)的。然而，由于激光傳感器價(jià)格昂貴、體積與質(zhì)量較大，因此設(shè)計(jì)成本低且安裝靈活的定位系統(tǒng)具有較高的研究?jī)r(jià)值與需求。其中，基于相機(jī)的定位系統(tǒng)則是目前的研究熱點(diǎn)。近年來，隨著有關(guān)基于視覺的同時(shí)定位與建圖算法的研究不斷深入，基于相機(jī)可以實(shí)現(xiàn)短期內(nèi)的高精度位姿估計(jì)。但是對(duì)于長(zhǎng)期視覺定位問題，由于天氣、光照、動(dòng)態(tài)物體等的影響，定位時(shí)傳感器感知到的環(huán)境信息可能與地圖構(gòu)建時(shí)的環(huán)境信息不同，使得正確關(guān)聯(lián)的數(shù)據(jù)較少，從而引起定位失敗。

目前，有關(guān)長(zhǎng)期視覺定位的研究從地圖維護(hù)的角度可以劃分為兩個(gè)方向。第一個(gè)方向是積累地圖的多樣性，即當(dāng)新一段數(shù)據(jù)中檢測(cè)到定位失敗時(shí)，地圖維護(hù)算法將發(fā)生定位失敗區(qū)域的特征累加入地圖中。這種方法維護(hù)的地圖占用存儲(chǔ)空間較大，且索引時(shí)需要較多的計(jì)算資源與耗時(shí)。第二個(gè)方向多采用深度學(xué)習(xí)的方法，基于對(duì)環(huán)境的重復(fù)觀測(cè)學(xué)習(xí)定位。這種方式目前仍未能取得較為穩(wěn)定的長(zhǎng)期定位結(jié)果，且可解釋性較差，仍處于探索階段。

分析基于激光的定位算法的長(zhǎng)期穩(wěn)定性，利用幾何信息實(shí)現(xiàn)數(shù)據(jù)關(guān)聯(lián)應(yīng)是其克服環(huán)境外觀變化的原因。這種數(shù)據(jù)關(guān)聯(lián)方式使得激光定位不會(huì)受到外界氣候、光照、季節(jié)等變化的影響，只有當(dāng)環(huán)境的結(jié)構(gòu)信息發(fā)生較大程度的改變時(shí)，激光定位算法才會(huì)受到影響。因此，目前也有學(xué)者嘗試?yán)脦缀涡畔?shí)現(xiàn)視覺定位。由于視覺三維重建在大范圍場(chǎng)景下實(shí)現(xiàn)的難度較大，且部分場(chǎng)景的精度難以達(dá)到激光地圖的精度，目前大部分基于幾何信息的視覺定位方法借助于激光地圖實(shí)現(xiàn)定位。這種定位模式下，視覺與激光地圖信息的關(guān)聯(lián)可以由兩種方式實(shí)現(xiàn)。一種是利用稠密激光點(diǎn)云信息實(shí)時(shí)渲染并生成虛擬位姿的圖像，將其與當(dāng)前相機(jī)獲取的圖像相匹配，從而實(shí)現(xiàn)對(duì)當(dāng)前位姿的估計(jì)。這種方式需要稠密地圖進(jìn)行渲染，對(duì)計(jì)算量要求較高，且需要環(huán)境具有含明顯反射率的區(qū)域或者結(jié)構(gòu)性較強(qiáng)。另一種則是通過連續(xù)圖像信息重構(gòu)局部視覺點(diǎn)云，利用點(diǎn)云匹配的方式將視覺點(diǎn)云與激光地圖點(diǎn)云相匹配。這種關(guān)聯(lián)方式的普適性更強(qiáng)，但是點(diǎn)云匹配的方式無法考慮到視覺點(diǎn)云不同于激光地圖點(diǎn)云的不確定度與空間分布，從而造成定位誤差。

結(jié)合動(dòng)態(tài)環(huán)境下長(zhǎng)期視覺定位的需求與目前學(xué)界的研究進(jìn)展，本節(jié)介紹了一套針對(duì)長(zhǎng)期視覺定位的激光地圖濾波系統(tǒng)和與之匹配的基于激光地圖的視覺定位方法。考慮到視覺與激光點(diǎn)云的分布不同，在構(gòu)建地圖時(shí)，首先根據(jù)視覺特征點(diǎn)的分布對(duì)激光數(shù)據(jù)進(jìn)行降采樣，隨后基于對(duì)同段場(chǎng)景的不同時(shí)間觀測(cè)，篩選出環(huán)境中的穩(wěn)定點(diǎn)云數(shù)據(jù)。為進(jìn)一步降低激光與視覺點(diǎn)云誤匹配的概率，尤其是針對(duì)生成自無紋理地面區(qū)域的視覺特征點(diǎn)，對(duì)激光地圖中的地面數(shù)據(jù)進(jìn)行檢測(cè)與標(biāo)注。經(jīng)實(shí)驗(yàn)驗(yàn)證，該方法可以基于短期內(nèi)構(gòu)建的激光地圖，實(shí)現(xiàn)在室外大范圍場(chǎng)景下時(shí)間跨度長(zhǎng)達(dá)一年的魯棒定位，且定位精度符合實(shí)際要求。

1.2.1.2 基于激光地圖的長(zhǎng)期視覺-慣性定位算法

該方法所研究的基于激光地圖的長(zhǎng)期視覺-慣性定位算法分為離線的激光地圖構(gòu)建模塊與在線的視覺-慣性模塊兩個(gè)部分，如圖1-22所示，下文將針對(duì)這兩個(gè)模塊分別進(jìn)行詳細(xì)的方法介紹。

（1）離線激光地圖構(gòu)建與濾波

為了提高激光地圖與視覺地圖點(diǎn)云數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性，在構(gòu)建激光地圖時(shí)，基于視覺觀測(cè)提取其中可穩(wěn)定與視覺特征點(diǎn)云穩(wěn)定關(guān)聯(lián)的子集。文中將視覺點(diǎn)云地圖表示為，激光點(diǎn)云地圖表示為，其中n_v、n_l分別為視覺和激光點(diǎn)云地圖的地圖點(diǎn)數(shù)目，ξ∈SE（3）表示機(jī)器人位姿。若將視覺點(diǎn)云視為稠密激光點(diǎn)云的觀測(cè)數(shù)據(jù)，則視覺定位的似然函數(shù)可以寫為

引入c_i=j表示視覺特征點(diǎn)與激光地圖點(diǎn)相關(guān)聯(lián)，則該似然概率L可推導(dǎo)得

假設(shè)Q_i（c_i）表示c_i的密度函數(shù)，則似然函數(shù)L的下界可由杰森不等式推導(dǎo)得

當(dāng)Q_i（c_i）是c_i的后驗(yàn)，即時(shí)，等號(hào)成立。參考Wang等人的工作，利用高斯假設(shè)將概率函數(shù)建模為

其中R（ξ）和t（ξ）表示位姿ξ的旋轉(zhuǎn)與平移分量。則當(dāng)c_i無先驗(yàn)時(shí)，后驗(yàn)概率為

若將此后驗(yàn)概率視為中心為c_i=j的單峰分布，則搜索視覺與激光點(diǎn)的數(shù)據(jù)關(guān)聯(lián)步驟可轉(zhuǎn)化為對(duì)在激光點(diǎn)云中的最近鄰搜索。當(dāng)且僅當(dāng)ξ為真值時(shí)，似然函數(shù)達(dá)到最大值。為最大化正確匹配的概率，該后驗(yàn)概率應(yīng)近似為

基于Kullback-Leibler散度可衡量該分布與實(shí)際后驗(yàn)概率的相似程度

該KL散度的衡量結(jié)果為基于視覺觀測(cè)的地圖濾波提供的指導(dǎo)，即使的近鄰激光點(diǎn)具有較大的G（c_i），而遠(yuǎn)離的激光點(diǎn)G（c_i）值較小，據(jù)此可篩選出激光地圖中適用于視覺定位的子集。但由于單次數(shù)據(jù)中存在大量高動(dòng)態(tài)和緩慢變化的物體，仍需對(duì)該子集進(jìn)行處理，尋找其中靜態(tài)的成分用于最終的視覺定位。

在實(shí)際實(shí)驗(yàn)中，由于對(duì)視覺地圖點(diǎn)進(jìn)行三維重建的位置不確定度較大，在搜尋視覺點(diǎn)的最近鄰激光點(diǎn)時(shí)，利用相機(jī)與激光的外參將激光點(diǎn)云投影到圖像平面上，經(jīng)深度濾波后保留距離視覺二維特征最近的激光點(diǎn)作為其匹配的激光地圖點(diǎn)。單次采集的數(shù)據(jù)中包含大量動(dòng)態(tài)以及緩變物體，需要通過多次觀測(cè)予以剔除，該部分功能集成于統(tǒng)計(jì)濾波子模塊中，通過統(tǒng)計(jì)地圖點(diǎn)經(jīng)多次數(shù)據(jù)采集后被觀測(cè)的次數(shù)，分析地圖中的靜態(tài)成分。

當(dāng)新地圖被視覺濾波模塊處理后，對(duì)于該地圖中的每一個(gè)地圖點(diǎn)，在已累積的地圖M^vl中尋找與其空間位置最接近的點(diǎn)。如果二者的空間距離大于d^α，則認(rèn)為新地圖中的點(diǎn)未被觀測(cè)過；否則，認(rèn)為現(xiàn)存地圖中的相應(yīng)位置的點(diǎn)被重復(fù)觀測(cè)。

當(dāng)融合的地圖數(shù)量較大，涉及的情景較為全面時(shí)，累積地圖M^vl可被認(rèn)為已包含來自不同情景的信息。環(huán)境中穩(wěn)定的部分應(yīng)能在大部分地圖數(shù)據(jù)采集過程中被觀測(cè)到，而不穩(wěn)定物體的被觀測(cè)次數(shù)則較少。因此，可以通過設(shè)定被觀測(cè)次數(shù)的閾值對(duì)地圖進(jìn)行濾波。

雖然室外環(huán)境內(nèi)的路面上大部分地區(qū)沒有顯著的紋理信息，但是在視覺里程計(jì)算法實(shí)時(shí)運(yùn)行時(shí)，仍會(huì)有一些特征點(diǎn)從這些無紋理的地面上被提取。這些點(diǎn)被提取的位置較為隨機(jī)，且極易隨著光照和視角的變化而發(fā)生變化。因此，在地圖構(gòu)建時(shí)采集自這些地方的地圖點(diǎn)幾乎不被保留在地圖中。但當(dāng)定位算法實(shí)時(shí)運(yùn)行，尤其當(dāng)周圍環(huán)境較為空曠時(shí)，大部分的特征被提取自地面，如果環(huán)境地圖中不包含地面信息，則實(shí)時(shí)提取的視覺地圖點(diǎn)極易關(guān)聯(lián)上錯(cuò)誤的地圖點(diǎn)。為提供地面約束信息，并避免地面點(diǎn)提取較為隨機(jī)的影響，從實(shí)時(shí)數(shù)據(jù)中按高度信息提取地面點(diǎn)，計(jì)算其表面法向量，并將其柵格化濾波后加入地圖中。這些點(diǎn)只為定位算法提供平面約束，避免點(diǎn)到點(diǎn)匹配關(guān)聯(lián)不準(zhǔn)確的誤差。該功能集成于地面提取模塊中。

（2）在線視覺-慣性定位算法

在線視覺-慣性定位算法基于定長(zhǎng)滑窗的視覺-慣性里程計(jì)算法研發(fā)，將定位的求解緊耦合于視覺-慣性里程計(jì)的聯(lián)合優(yōu)化中。當(dāng)檢測(cè)到新一幀的視覺關(guān)鍵幀建立后，滑窗內(nèi)的視覺地圖點(diǎn)與相關(guān)聯(lián)的激光地圖點(diǎn)之間的約束被加入到優(yōu)化的目標(biāo)函數(shù)中。

式中，是視覺點(diǎn)與匹配到的非地面點(diǎn)之間的誤差函數(shù)；是視覺點(diǎn)與匹配到的地面點(diǎn)之間的誤差函數(shù)；Ω是相應(yīng)的信息矩陣；是激光點(diǎn)的法向量；是視覺里程坐標(biāo)系與激光地圖坐標(biāo)系之間的外參。整體聯(lián)合優(yōu)化的緊耦合圖模型如圖1-23所示。

基于優(yōu)化的結(jié)果，即可將視覺里程計(jì)的結(jié)果與激光地圖的全局位置信息相關(guān)聯(lián)，從而得到全局一致的定位結(jié)果。

上文所述的方法中，由于地圖的約束緊耦合于視覺里程的聯(lián)合優(yōu)化中，所以激光地圖高精度的幾何信息可以作用于視覺點(diǎn)云的優(yōu)化，從而提高定位的精度。然而該優(yōu)化方式計(jì)算量較大，且對(duì)偏置和速度的優(yōu)化易受到錯(cuò)誤數(shù)據(jù)關(guān)聯(lián)的影響。除這種方式外，也可以先優(yōu)化視覺里程計(jì)的約束，再基于點(diǎn)云匹配的方式計(jì)算外參，即將地圖的約束松耦合于定位框架內(nèi)。為兼顧兩種方法的優(yōu)勢(shì)，采用迭代進(jìn)行兩種優(yōu)化的混合優(yōu)化模型進(jìn)行視覺定位。

（3）實(shí)驗(yàn)結(jié)果分析

此方法基于采集自浙江大學(xué)某校區(qū)的真實(shí)場(chǎng)景數(shù)據(jù)進(jìn)行測(cè)試。用于構(gòu)建地圖的數(shù)據(jù)采集自2017年3月，其中激光傳感器為VLP-16 Velodyne LiDAR，相機(jī)為ZED的雙目相機(jī)。用于測(cè)試定位的數(shù)據(jù)分別采集自2017年8月與2018年1月，包含了與構(gòu)建地圖時(shí)相同行駛方向采集的數(shù)據(jù)與逆向的數(shù)據(jù)，且數(shù)據(jù)采集自不同的時(shí)間段與天氣。實(shí)驗(yàn)算法基于C++實(shí)現(xiàn)，其中視覺-慣性里程計(jì)基于開源代碼ORB-SLAM開發(fā)。具體的實(shí)驗(yàn)數(shù)據(jù)細(xì)節(jié)見表1-2。

利用采集自3天內(nèi)的21條數(shù)據(jù)進(jìn)行面向視覺定位的激光地圖構(gòu)建。每條數(shù)據(jù)利用基于激光的定位建圖方法估計(jì)軌跡作為位姿真值。所有的數(shù)據(jù)按流程圖示意的過程進(jìn)行融合，最后濾波的結(jié)果如圖1-24b所示。由圖可見，經(jīng)過濾波后的激光地圖點(diǎn)云分布不同于原始激光點(diǎn)云圖（圖1-24a）的均勻分布，而更接近于傳統(tǒng)的視覺點(diǎn)云地圖。

為評(píng)估該定位算法的效果，利用采集自2017年8月的4條數(shù)據(jù)與2018年1月雪后的數(shù)據(jù)進(jìn)行定位測(cè)試。各條數(shù)據(jù)在不同定位模型下的結(jié)果見表1-3。

由表中數(shù)據(jù)可見，混合優(yōu)化模型的定位結(jié)果優(yōu)于純緊耦合與純松耦合的定位結(jié)果，且在大部分的測(cè)試數(shù)據(jù)中，“5∶1”比例的松、緊耦合迭代方式效果最佳。每段定位數(shù)據(jù)的軌跡與定位過程中水平、偏航角的誤差統(tǒng)計(jì)如圖1-25所示。

其中第一列表示第二列軌跡圖中矩形框位置的真實(shí)場(chǎng)景圖，第二列中的箭頭指示數(shù)據(jù)采集的方向，第三、四列分別統(tǒng)計(jì)了水平與偏航角的誤差。

為驗(yàn)證地圖濾波算法對(duì)視覺定位性能提升的有效性，進(jìn)一步在原始與濾波后的激光地圖上進(jìn)行定位測(cè)試，測(cè)試結(jié)果見表1-4。

由表中結(jié)果可見，經(jīng)濾波的地圖可以顯著提升視覺定位算法的精度。在對(duì)比實(shí)驗(yàn)中，我們也測(cè)試了在未濾波的地圖中松耦合定位結(jié)果的性能。經(jīng)測(cè)試，在一些數(shù)據(jù)中，于未濾波的地圖中進(jìn)行松耦合定位也可以取得較好的定位精度，但是由于未對(duì)地圖中動(dòng)態(tài)物體進(jìn)行處理，且未濾除地圖中與視覺地圖點(diǎn)無關(guān)的點(diǎn)集，在一些數(shù)據(jù)段上無法成功進(jìn)行整條路徑的定位，從而進(jìn)一步驗(yàn)證了地圖濾波算法對(duì)提升定位魯棒性的作用。

1.2.2 基于點(diǎn)、線特征的視覺定位

1.2.2.1 視覺定位問題分析

定位對(duì)于機(jī)器人自主導(dǎo)航是至關(guān)重要的。與基于激光雷達(dá)（Light Detection And Ranging，LiDAR）的定位方法相比，視覺定位在許多應(yīng)用中是有利的，因?yàn)榈统杀厩曳€(wěn)定的相機(jī)可以被廣泛使用。視覺定位的典型工作流程是檢測(cè)圖像中的特征點(diǎn)，將圖像特征點(diǎn)與地圖點(diǎn)匹配，并根據(jù)一組匹配估計(jì)位姿。在此過程中幾乎無法避免錯(cuò)誤的匹配，這會(huì)導(dǎo)致不準(zhǔn)確的位姿估計(jì)。隨機(jī)抽樣一致（Random Sample Consensus，RANSAC）是一種常用于實(shí)現(xiàn)魯棒的位姿估計(jì)的方法。然而，RANSAC被限制用于外觀嚴(yán)重變化的環(huán)境中，在該環(huán)境中特征誤匹配比例可能會(huì)顯著增長(zhǎng)。因此，對(duì)天氣、光照或季節(jié)變化魯棒的可靠視覺定位仍然是一個(gè)具有挑戰(zhàn)性的問題。

兩個(gè)因素對(duì)于RANSAC找到內(nèi)點(diǎn)是至關(guān)重要的，即集合中的內(nèi)點(diǎn)數(shù)量（越多越好）以及估計(jì)姿勢(shì)所需的最小匹配數(shù)量（越少越好）。對(duì)于第一個(gè)因素，一些先前的工作通過利用多種類型的特征（例如點(diǎn)、線和圓）來增加內(nèi)點(diǎn)的數(shù)量，因?yàn)樵诟髨D像區(qū)域上定義的特征通常會(huì)導(dǎo)致對(duì)光照變化更高的魯棒性。對(duì)于第二個(gè)因素，現(xiàn)有的3D-2D定位方法通常需要3或4對(duì)特征匹配，例如三點(diǎn)法（Perspective-Three-Points，P3P）和多點(diǎn)法（Efficient Perspective-n-Point Camera Pose Estimation，EPnP）。本節(jié)的主要研究點(diǎn)是：在機(jī)器人導(dǎo)航場(chǎng)景中，點(diǎn)和線特征的匹配數(shù)可以進(jìn)一步減少，從而導(dǎo)致對(duì)誤匹配更高的魯棒性。

具體而言，在慣性測(cè)量單元（Inertial Measurement Unit，IMU）的輔助下，先將預(yù)先構(gòu)建的地圖與當(dāng)前查詢圖像之間的重力方向進(jìn)行對(duì)準(zhǔn)。之后的目標(biāo)是利用直接觀測(cè)到的俯仰角和滾轉(zhuǎn)角來將解析估計(jì)需要的點(diǎn)特征匹配數(shù)量從3減少到2。此外，對(duì)于1點(diǎn)和1線的情況也推導(dǎo)出解析解，從而可以考慮線特征匹配。因此，本節(jié)提出了一種2實(shí)體RANSAC用于持久視覺定位，如圖1-26所示，其中綠線表示在黑色預(yù)建3D地圖定位。理論上，由于2個(gè)觀測(cè)值（2D）可以提供4個(gè)自由度（Degree of Freedom，DOF）的約束，因此所提出的解決方案應(yīng)該是能得出解析解的情況下需要觀測(cè)值最少的。與現(xiàn)有的2點(diǎn)RANSAC方法相比，2點(diǎn)RANSAC適用于視覺慣性測(cè)距或同時(shí)定位與映射中的連續(xù)姿態(tài)估計(jì)，本節(jié)的方法側(cè)重于3D-2D匹配，而不涉及相對(duì)偏航角的測(cè)量，使其適用于定位。

這些解決方案用于所提出的點(diǎn)線定位方法，其對(duì)于誤匹配更加魯棒，因?yàn)榫€和點(diǎn)特征可以同時(shí)使用并且位姿估計(jì)所需的匹配數(shù)量也會(huì)減少。此外，我們引入了三種具有不同優(yōu)勢(shì)的特征采樣策略，實(shí)現(xiàn)了自動(dòng)選擇機(jī)制。通過該機(jī)制，我們的2實(shí)體RANSAC可以適應(yīng)不同特征類型分布的環(huán)境。

1.2.2.2 基于點(diǎn)、線特征的視覺定位算法

本節(jié)提出的定位方法包括以下步驟：

Step1：獲取當(dāng)前場(chǎng)景的先驗(yàn)三維地圖，其中地圖是事先構(gòu)建的。

Step2：獲取機(jī)器人的當(dāng)前圖像和慣性傳感器的測(cè)量數(shù)據(jù)。

Step3：根據(jù)當(dāng)前慣性傳感器數(shù)據(jù)與所述先驗(yàn)地圖中的慣性傳感器數(shù)據(jù)計(jì)算當(dāng)前機(jī)器人位姿的俯仰角和翻滾角。

Step4：根據(jù)當(dāng)前圖像檢測(cè)到的二維點(diǎn)、線特征與先驗(yàn)地圖中的三維點(diǎn)、線特征進(jìn)行匹配。

Step5：根據(jù)所述匹配到的兩對(duì)點(diǎn)特征或者一對(duì)點(diǎn)特征加一對(duì)線特征計(jì)算當(dāng)前機(jī)器人位姿的其余未知量。

（1）重力對(duì)齊

本節(jié)所提的方法對(duì)先驗(yàn)三維地圖有如下要求：要求包含重力加速度在所述地圖坐標(biāo)系下的測(cè)量數(shù)據(jù)，一般由IMU測(cè)量所得，并且建立的地圖還要包含三維點(diǎn)、線特征。在定位過程中，機(jī)器人采集的當(dāng)前IMU數(shù)據(jù)包含重力加速度在當(dāng)前機(jī)器人坐標(biāo)系下的測(cè)量值。根據(jù)重力加速度在所述先驗(yàn)地圖坐標(biāo)系下的測(cè)量值，將重力加速度在兩個(gè)坐標(biāo)下進(jìn)行對(duì)齊，從而得到地圖坐標(biāo)系相對(duì)于當(dāng)前機(jī)器人坐標(biāo)系的俯仰角和翻滾角，具體根據(jù)以下算式進(jìn)行計(jì)算

式中，[x_wy_wz_w]^T是重力加速度在地圖坐標(biāo)系下的測(cè)量值；[x_cy_cz_c]^T是重力坐標(biāo)系在當(dāng)前機(jī)器人坐標(biāo)系下的測(cè)量值；β、γ分別是所求的當(dāng)前機(jī)器人坐標(biāo)系相對(duì)于地圖坐標(biāo)系的俯仰角和翻滾角。

（2）位姿解算—2點(diǎn)

本節(jié)所提出的方法根據(jù)當(dāng)前圖像和地圖中的三維點(diǎn)、線特征可以進(jìn)行二維到三維的點(diǎn)、線特征的匹配。先從獲取的當(dāng)前圖像提取多個(gè)二維特征點(diǎn)和多條二維線段，然后將當(dāng)前圖像的二維點(diǎn)、線特征與地圖中的三維點(diǎn)、線特征進(jìn)行匹配。衡量匹配成功與否的標(biāo)準(zhǔn)是計(jì)算相應(yīng)描述子的歐式距離，小于一定閾值則認(rèn)為匹配成功，之后迭代獲取多組二維與三維的匹配點(diǎn)、線特征。

根據(jù)匹配到的2組二維到三維匹配的點(diǎn)特征，可以計(jì)算地圖相對(duì)于當(dāng)前機(jī)器人坐標(biāo)系的位姿的其余4DOF未知量，具體步驟如下。

根據(jù)地圖坐標(biāo)系{W₀}中匹配到的兩個(gè)三維點(diǎn)，引入一個(gè)中間坐標(biāo)系{W₁}，其中兩個(gè)三維點(diǎn)在所述中間坐標(biāo)系下表示為P₁=（0 0 0）^T、P₂=（X₂Y₂Z₂）^T，而{W₀}到{W₁}的變換則表示為的簡(jiǎn)單平移，具體為

然后根據(jù)當(dāng)前圖像中匹配到的二維點(diǎn)和相機(jī)標(biāo)定的內(nèi)參矩陣K，可得二維點(diǎn)在機(jī)器人相機(jī)坐標(biāo)系{C}下的歸一化平面上的三維坐標(biāo)為

式中，C是相機(jī)的光心。

根據(jù)圖1-27所示的投影幾何約束可得三點(diǎn)（C，D₁，RP₁+t）共線，且三點(diǎn)（C，D₂，RP₂+t）共線，其中R為{W₁}相對(duì)于{C}的旋轉(zhuǎn)變換矩陣，t為{W₁}相對(duì)于{C}的平移變換向量；根據(jù)前述所得的{C}相對(duì)于{W₀}的俯仰角和翻滾角，可將R簡(jiǎn)化為

式中，α是未知量，表示為機(jī)器人當(dāng)前相機(jī)坐標(biāo)系相對(duì)于地圖坐標(biāo)系的偏航角；設(shè)t=[t₁t₂t₃]^T，則全部未知量一共有四個(gè)，分別為α、t₁、t₂、t₃。

根據(jù)三點(diǎn)（C，D₁，RP₁+t）共線，可得兩個(gè)方程如下

根據(jù)三點(diǎn)（C，D₂，RP₂+t）共線，可得兩個(gè)方程如下

聯(lián)立以上四個(gè)方程即可求得四個(gè)未知量，從而求得機(jī)器人在地圖中的當(dāng)前位姿。

（3）位姿解算—1點(diǎn)1線

根據(jù)匹配到的1組點(diǎn)特征加1組線特征，可以計(jì)算地圖相對(duì)于當(dāng)前機(jī)器人坐標(biāo)系的位姿的其余4DOF未知量，具體步驟如下。

根據(jù)地圖坐標(biāo)系{W₀}中匹配到的一個(gè)三維點(diǎn)和一條三維線段，引入一個(gè)中間坐標(biāo)系{W₁}，其中三維點(diǎn)在所述中間坐標(biāo)系下表示為P₁=（0 0 0）^T，{W₀}到{W₁}的變換則表示為的簡(jiǎn)單平移，易得三維線段的端點(diǎn)在所述中間坐標(biāo)系下表示為L₂=（X₂Y₂Z₂）^T和L₃=（X₃Y₃Z₃）^T。

然后根據(jù)當(dāng)前圖像中匹配到的一個(gè)二維點(diǎn)和一條二維線段，以及相機(jī)標(biāo)定的內(nèi)參矩陣K可得二維點(diǎn)和二維線段的端點(diǎn)在相機(jī)坐標(biāo)系{C₀}下的歸一化平面上的三維坐標(biāo)和；引入一個(gè)中間坐標(biāo)系{C₁}，使得相機(jī)光心C以及匹配到的點(diǎn)D₁和線段端點(diǎn)的表示如下

中間坐標(biāo)系{C₁}到{C₀}的變換可由（C⁰，）→（C，D₂，D₃）的變換求得，坐標(biāo)系變換示意圖如圖1-28所示；接下來求解中間坐標(biāo)系中D₁的具體坐標(biāo)，將投影射線d₁經(jīng)過變換到中間坐標(biāo)系下，并延長(zhǎng)取其與XY平面的交點(diǎn)即得D₁

根據(jù)圖1-28所示的投影幾何約束可得三點(diǎn)（C，D₁，RP₁+t）共線，四點(diǎn)（C，D₂，D₃，RL₂+t）以及（C，D₂，D₃，RL₃+t）共面，其中R為{W₁}相對(duì)于{C₁}的旋轉(zhuǎn)變換矩陣，t為{W₁}相對(duì)于{C₁}的平移變換向量。

根據(jù)三點(diǎn)（C，D₁，RP₁+t）共線，可得兩個(gè)方程如下

根據(jù)四點(diǎn)（C，D₂，D₃，RL₂+t）共面，可得方程

根據(jù)四點(diǎn)（C，D₂，D₃，RL₃+t）共面，可得方程

聯(lián)立以上四個(gè)方程即可求得四個(gè)未知量，從而求得機(jī)器人在地圖中的當(dāng)前位姿。

1.2.2.3 實(shí)驗(yàn)結(jié)果

對(duì)所提出的方法在仿真數(shù)據(jù)上進(jìn)行評(píng)估，以證明在視覺定位中優(yōu)于其他方法。本節(jié)驗(yàn)證了本方法對(duì)圖像特征噪聲的精度，對(duì)于不準(zhǔn)確的俯仰角和滾轉(zhuǎn)角的靈敏度，以及對(duì)誤匹配的魯棒性。在實(shí)物實(shí)驗(yàn)數(shù)據(jù)上，我們還進(jìn)行了成功率的比較，以顯示所提出方法的有效性。

（1）精度

在模擬實(shí)驗(yàn)中，我們?cè)谝粋€(gè)立方體中生成了一些3D點(diǎn)和線，并計(jì)算了不同相機(jī)位姿的2D投影，以獲得3D-2D特征匹配。對(duì)于每種方法，執(zhí)行100次RANSAC迭代。在優(yōu)化中，有兩種可能的方式：4DOF或6DOF優(yōu)化。4DOF優(yōu)化意味著我們固定VINS提供的俯仰角和滾轉(zhuǎn)角，并且僅優(yōu)化姿勢(shì)中的其他四個(gè)變量。當(dāng)內(nèi)點(diǎn)很少時(shí)，4DOF優(yōu)化十分有效。

在仿真實(shí)驗(yàn)評(píng)估的最小的解決方案是EPnP、P3P、2P1L和提出的解決方案，包括1P1L-6DOF、1P1L-4DOF、2P-6DOF、2P-4DOF。mixed-6DOF和mixed-4DOF也在對(duì)抗誤匹配魯棒性方面進(jìn)行了評(píng)估。

為了量化不同最小解的精度，我們?cè)?D投影上添加了零均值和標(biāo)準(zhǔn)差的高斯噪聲，并在四個(gè)級(jí)別中改變了特征匹配對(duì)的數(shù)量：10、5、4、3（10組數(shù)據(jù)意味著在場(chǎng)景中有10對(duì)點(diǎn)匹配和10對(duì)線匹配），結(jié)果如圖1-29所示。可以發(fā)現(xiàn)，當(dāng)特征匹配足夠時(shí)（參見10組數(shù)據(jù)），隨著噪聲的標(biāo)準(zhǔn)偏差增加，所提出的方法可以實(shí)現(xiàn)與其他方法相同的精度。但特征匹配對(duì)的數(shù)量減少，與2P1L、EPnP和P3P相比，2實(shí)體方法的優(yōu)勢(shì)變得更大。

（2）靈敏度

借助IMU的觀測(cè)，我們利用傳感器提供的俯仰角和翻滾角可以將定位問題降低到4DOF。因此，有必要研究俯仰角和翻滾角對(duì)最終定位精度的影響。我們?cè)诟┭鼋呛头瓭L角上添加了零均值和標(biāo)準(zhǔn)差的高斯噪聲，并研究了所提出的方法在三個(gè)特征匹配級(jí)別中的性能：10、5、3，結(jié)果如圖1-30所示。當(dāng)有足夠的特征匹配時(shí)（參見10組數(shù)據(jù)），所提出的方法可以在俯仰角和滾轉(zhuǎn)角上容忍近25°的噪聲。在這種情況下，6DOF優(yōu)化的定位精度優(yōu)于4DOF。但是，隨著特征匹配對(duì)的數(shù)量減少，6DOF和4DOF方法之間的差異變小。這是合理的，因?yàn)楫?dāng)內(nèi)點(diǎn)數(shù)很少時(shí)，由優(yōu)化中的附加DOF引起的誤差可能大于由噪聲位姿估計(jì)引起的誤差。因此，如果在長(zhǎng)期定位的情況下，可靠的特征匹配非常有限，4DOF優(yōu)化是一個(gè)不錯(cuò)的選擇。

（3）魯棒性

為了驗(yàn)證所提方法的魯棒性，設(shè)計(jì)了一些實(shí)驗(yàn)，通過將內(nèi)點(diǎn)率從0到80%改變?yōu)椴煌膬?nèi)部數(shù)量級(jí)別：10、5、4、3。在每個(gè)級(jí)別，盡管誤匹配率在增加，但內(nèi)點(diǎn)數(shù)量不變。誤匹配是通過錯(cuò)誤地關(guān)聯(lián)原始數(shù)據(jù)中的像素而生成的。當(dāng)估計(jì)的位姿結(jié)果的平移誤差小于10%且旋轉(zhuǎn)誤差小于5°時(shí)，任務(wù)定位成功。為每種方法做了200次實(shí)驗(yàn)來平均成功率，結(jié)果如圖1-31所示。正如預(yù)期的那樣，當(dāng)誤匹配增加時(shí)，所提出的2實(shí)體RANSAC優(yōu)于其他最小解，當(dāng)內(nèi)點(diǎn)數(shù)減少時(shí)更為明顯。注意，在具有足夠數(shù)量?jī)?nèi)點(diǎn)的情況下，當(dāng)誤匹配率為80%時(shí)，所提出的方法可以實(shí)現(xiàn)超過90%的成功率。

1.2.3 基于拓?fù)渚植砍叨鹊貓D的定位

1.2.3.1 基于拓?fù)渚植砍叨鹊貓D的定位問題分析

對(duì)機(jī)器人的可靠移動(dòng)的研究已經(jīng)有很多年了，其中一個(gè)最主要的難點(diǎn)在于機(jī)器人定位。為了解決這個(gè)問題，其中一類方法是構(gòu)建一個(gè)全局一致的尺度地圖，然后利用機(jī)器人觀測(cè)到的傳感器數(shù)據(jù)與地圖進(jìn)行匹配，從而得到機(jī)器人位置。由于環(huán)境的動(dòng)態(tài)變化，這些方法在機(jī)器人的長(zhǎng)期運(yùn)行中存在全局一致性問題。對(duì)此，大量研究嘗試融合不同時(shí)間段的傳感器數(shù)據(jù)，形成一個(gè)全局地圖，為機(jī)器人提供定位服務(wù)。這些方法依賴于不同時(shí)間段數(shù)據(jù)之間對(duì)齊的精度，這在室外大尺度環(huán)境是難以保證的。除此以外，有學(xué)者研究利用拓?fù)浣Y(jié)構(gòu)進(jìn)行建圖和定位，降低了對(duì)匹配精度的要求。然而，這些方法沒有提供多段數(shù)據(jù)之間的尺度信息。在某些研究中，尺度信息保存在拓?fù)浣Y(jié)構(gòu)中，但這類方法依然建立在全局一致性上，當(dāng)有太多觀測(cè)誤差時(shí)仍然存在問題。

在具體實(shí)現(xiàn)時(shí)，將機(jī)器人運(yùn)行中采集到的數(shù)據(jù)保存在拓?fù)涞貓D上。定位時(shí)，實(shí)時(shí)傳感器數(shù)據(jù)與地圖鄰域內(nèi)的數(shù)據(jù)進(jìn)行匹配，得到的定位結(jié)果是相對(duì)于局部而言的，從而避免了全局一致性要求。如果定位失敗了，則將該數(shù)據(jù)插入到地圖中，并與已有的數(shù)據(jù)建立拓?fù)渎?lián)系。在導(dǎo)航中，通過在拓?fù)涞貓D上搜索出一條能到達(dá)目標(biāo)點(diǎn)的路徑并依此到達(dá)，可以保證局部定位誤差保持在一定范圍內(nèi)。主要?jiǎng)?chuàng)新點(diǎn)包括：

1）提出了一個(gè)基于拓?fù)浣Y(jié)構(gòu)的局部尺度框架（Topological Local-metric Framework，TLF），將不同時(shí)間段的傳感器數(shù)據(jù)組織在一個(gè)地圖下。

2）設(shè)計(jì)了針對(duì)TLF的定位和導(dǎo)航算法，讓機(jī)器人在無全局坐標(biāo)系的前提下自主運(yùn)行。

3）提出了一個(gè)記憶機(jī)制，通過在TLF上增加或刪除信息，讓機(jī)器人記錄未經(jīng)過或改變了的地方，從而使機(jī)器人擁有不同時(shí)間段的地圖信息。

4）為了驗(yàn)證算法的有效性和性能，我們將TLF應(yīng)用于激光雷達(dá)和相機(jī)，在室外環(huán)境中不同時(shí)間段進(jìn)行了21次導(dǎo)航試驗(yàn)。

1.2.3.2 基于拓?fù)渚植砍叨鹊貓D的定位算法

（1）地圖表示

地圖可以用圖M={N，E}表示，其中N是節(jié)點(diǎn)的集合，E是邊的集合。對(duì)于節(jié)點(diǎn)n_i∈N，其屬性定義為

n_i={s_i，l_i，N_i}

式中，s_i是由傳感器構(gòu)建的子地圖，如點(diǎn)云累積得到的局部激光地圖或一幀圖像數(shù)據(jù)；l_i是與傳感相關(guān)的定位器，可視為函數(shù)p_t，i=l_i（d_t，s_i），用于計(jì)算子地圖s_i和當(dāng)前傳感器測(cè)量值d_t之間的相對(duì)位姿，如激光中的ICP和圖像中的RANSAC；N_i是其他關(guān)于該節(jié)點(diǎn)的屬性，如GPS數(shù)據(jù)、采集日期等，這些屬性可以幫助定位和導(dǎo)航。

邊e_i∈E定義為

e_i={p_i，j，E_i，j}

式中，p_i，j是節(jié)點(diǎn)n_i和n_j之間的相對(duì)位姿，在建圖過程中，該位姿一方面可以由里程計(jì)數(shù)據(jù)獲得，在局部是精確的；另一方面可以由定位器或全局閉環(huán)得到，在局部也是精確的；E_i，j是邊的其他屬性，例如在視覺導(dǎo)航中，由于相機(jī)的觀測(cè)具有方向性，需要記錄節(jié)點(diǎn)訪問的順序，即邊的遍歷方向。當(dāng)有多段數(shù)據(jù)或者多個(gè)機(jī)器人時(shí)，這些地圖可以通過尋找閉環(huán)融合到一起。TLF中會(huì)保存不同地圖上節(jié)點(diǎn)間的相對(duì)關(guān)系。

（2）相對(duì)定位

本文提出的定位算法基于滑窗優(yōu)化。在定位誤差不大的假設(shè)下，可以在拓?fù)涞貓D上尋找出離當(dāng)前機(jī)器人最近的一個(gè)節(jié)點(diǎn)Q（t），以Q（t）為中心在拓?fù)涞貓D上找出鄰域C_Q。同時(shí)，由歷史里程計(jì)可得到里程計(jì)滑窗C_odom。C_Q和C_odom可由定位器給出首位相對(duì)位姿，如圖1-32所示。在該優(yōu)化圖中，三種邊的代價(jià)函數(shù)定義如下

式中，ρ（·）是robust kernel，用于減少觀測(cè)誤差帶來的影響；p是待估計(jì)位姿；∑表示加權(quán)矩陣，即。E_odom是里程計(jì)數(shù)據(jù)；E_loc是由定位器給出的相對(duì)位姿；E_Q是拓?fù)涞貓D中保存的相對(duì)位姿。待估計(jì)位姿可由下式給出

（3）基于流形的導(dǎo)航

地圖是保存在三維空間中的，但實(shí)際上，該拓?fù)涞貓D可視為局部平滑的二維流形。因此，基于TLF的機(jī)器人無法在整個(gè)三維空間中運(yùn)動(dòng)。基于此，我們提出一個(gè)雙層的導(dǎo)航算法。在頂層，從機(jī)器人當(dāng)前位置出發(fā)，我們可以在拓?fù)涞貓D上尋找出一條到達(dá)目標(biāo)地點(diǎn)的路徑，實(shí)際上是一個(gè)節(jié)點(diǎn)序列H= {h_i}。在下層，每當(dāng)機(jī)器人到達(dá)了H中的一個(gè)節(jié)點(diǎn)，該節(jié)點(diǎn)就從H中移除，并按順序確定下一個(gè)目標(biāo)點(diǎn)。不難證明，該方法能保證對(duì)最終目標(biāo)點(diǎn)的估計(jì)誤差會(huì)越來越小。流形導(dǎo)航示意圖如圖1-33所示。

（4）記憶機(jī)制

為了提高在不同時(shí)間的定位成功率，需要不斷豐富地圖的信息。本書提出的記憶機(jī)制如圖1-34所示。當(dāng)重新定位后（定位失敗一段時(shí)間后全局定位成功），無法與地圖匹配的數(shù)據(jù)會(huì)被加入到地圖中（包括定位失敗前最后一次定位成功的節(jié)點(diǎn)和最新一次定位成功的節(jié)點(diǎn)）。下次運(yùn)行時(shí)，新增的數(shù)據(jù)即可用于定位，從而提高定位成功率。

圖1-34中，灰色點(diǎn)是地圖點(diǎn)，綠色點(diǎn)是定位成功的點(diǎn)，紅色點(diǎn)是定位失敗的點(diǎn)，綠色邊則表示一次成功的定位。為了保持地圖規(guī)模在合理范圍內(nèi)，還需要對(duì)地圖進(jìn)行刪減。地圖維護(hù)針對(duì)的目標(biāo)是動(dòng)態(tài)變化的，如移動(dòng)的車輛等。這類變化是不可持續(xù)的，在以后的定位中無法提供有效信息，因此需要?jiǎng)h除。我們可以統(tǒng)計(jì)每個(gè)節(jié)點(diǎn)定位成功和失敗的次數(shù)，算出該節(jié)點(diǎn)定位成功率的變化趨勢(shì)。定位成功率穩(wěn)定下降的節(jié)點(diǎn)對(duì)應(yīng)的就是該類節(jié)點(diǎn)，應(yīng)該從地圖上刪除。

1.2.3.3 實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)采用一個(gè)四輪移動(dòng)機(jī)器人，機(jī)器人上安裝了一個(gè)ZED雙目相機(jī)和一個(gè)16線激光雷達(dá)VLP-16，如圖1-35所示。在三天中的不同時(shí)間段，我們用該平臺(tái)在校園里同一段路上運(yùn)行了21次，每次運(yùn)行約1km（圖1-36），共23km。

為了驗(yàn)證記憶機(jī)制的作用，我們分別統(tǒng)計(jì)了有/無記憶機(jī)制下的定位成功率，如圖1-37所示。

在記憶機(jī)制的作用下，定位成功率明顯增加。圖中s1～s8、s9～s16、s17～s21分別采集于第一、二、三天。沒有記憶機(jī)制時(shí)，每天第一次運(yùn)行的定位成功率是最高的，說明該時(shí)段與原地圖最相似。s16的定位成功率最低，這是因?yàn)橐呀?jīng)進(jìn)入晚上，光照變化最大。在三天中，沒有記憶機(jī)制的定位成功率呈現(xiàn)了類似的趨勢(shì)，與我們的采集時(shí)間吻合。

每次運(yùn)行結(jié)束后地圖節(jié)點(diǎn)數(shù)如圖1-38所示。在第一天結(jié)束后，地圖維護(hù)機(jī)制開始生效。隨著機(jī)器人運(yùn)行次數(shù)增加，每次增加的節(jié)點(diǎn)數(shù)呈下降趨勢(shì)，偶爾會(huì)不增反減。在s16結(jié)束后，節(jié)點(diǎn)出現(xiàn)了較大的增加，這是因?yàn)閟16觀測(cè)到了晚上的圖像，這是之前沒有經(jīng)歷過的。總體而言，地圖規(guī)模緩慢增加，并趨于穩(wěn)定。

圖1-39所示為算法的計(jì)算時(shí)間。除了首次運(yùn)行，運(yùn)行時(shí)間均在每幀0.1s左右，該速度能滿足機(jī)器人的導(dǎo)航需求。

1.2.4 機(jī)器人可行域檢測(cè)

1.2.4.1 可行域檢測(cè)問題分析

無結(jié)構(gòu)化環(huán)境中的可行域分割是自動(dòng)駕駛中不可或缺的技術(shù)，也是安全導(dǎo)航和行為控制的基礎(chǔ)。在現(xiàn)有方案中，昂貴的激光雷達(dá)等主動(dòng)傳感器由于數(shù)據(jù)稀疏，難以檢測(cè)出較小的障礙物。因此，在路面感知研究中，低成本的相機(jī)受到了越來越多的關(guān)注。近年來，深度神經(jīng)網(wǎng)絡(luò)在各個(gè)領(lǐng)域均取得了不小的突破。其中，用于語義分割的卷積神經(jīng)網(wǎng)絡(luò)進(jìn)入了實(shí)用階段，為無結(jié)構(gòu)化環(huán)境中的可行域分割帶來了可能性。

當(dāng)前，大部分高級(jí)駕駛輔助系統(tǒng)（ADAS）局限在路面環(huán)境，主要完成車道線保持任務(wù)。這些系統(tǒng)依賴車道線檢測(cè)、交通標(biāo)志識(shí)別等技術(shù)，無法直接用于非結(jié)構(gòu)化環(huán)境。基于監(jiān)督學(xué)習(xí)的語義分割方法在可行域分割上取得了不錯(cuò)的效果，但由于這類方法需要大量帶標(biāo)簽數(shù)據(jù)以及大量人力，難以獲得海量數(shù)據(jù)。為了減少人工過程，有人提出了一種自學(xué)習(xí)的方法，通過給定初始地面假設(shè)，不斷擴(kuò)大可行域，但這種在線學(xué)習(xí)的方法無法利用已有數(shù)據(jù)。之后又有人提出了一種弱監(jiān)督方法來生成標(biāo)簽數(shù)據(jù)，但他們標(biāo)注的可行域太窄，限制了機(jī)器人的運(yùn)動(dòng)空間。同時(shí)，在有不同前進(jìn)方向的分岔路口處，該方法會(huì)失效。

本節(jié)在先前工作的基礎(chǔ)上提出了一個(gè)用于多時(shí)段多傳感器的融合框架，并應(yīng)用于自動(dòng)生成帶標(biāo)簽數(shù)據(jù)，為可行域分割模型提供訓(xùn)練數(shù)據(jù)。我們可以使用這些標(biāo)簽數(shù)據(jù)訓(xùn)練一個(gè)語義分割深度卷積神經(jīng)網(wǎng)絡(luò)。在訓(xùn)練階段，該方法只需要一個(gè)激光和單目相機(jī)；在部署階段，該方法只需要一個(gè)單目相機(jī)。

在采集到多段數(shù)據(jù)后，通過構(gòu)建全局激光地圖，定義一個(gè)全局坐標(biāo)，并將數(shù)據(jù)組織在該框架下。每幀激光的位姿由激光在地圖中定位給出，通過矩陣的鏈?zhǔn)椒▌t合一得到其他傳感器的位姿。

1.2.4.2 可行域檢測(cè)算法

我們提出了一個(gè)數(shù)據(jù)融合框架與地圖，該框架可以融合不同空間、不同時(shí)間、不同傳感器的數(shù)據(jù)，如圖1-40所示。融合的關(guān)鍵在于如何將數(shù)據(jù)表達(dá)在同一個(gè)框架下。首先，借助某個(gè)時(shí)段采集到的激光數(shù)據(jù)，構(gòu)建一個(gè)全局地圖M。對(duì)于從激光傳感器L在t_i刻采集到的激光數(shù)據(jù)，可以用ICP定位算法得到該幀數(shù)據(jù)在M中的全局位姿。其中，t_i包含兩部分信息：一是該幀數(shù)據(jù)來自于哪段數(shù)據(jù)；二是來自該段的哪個(gè)時(shí)間點(diǎn)。

該框架還支持多傳感器配置。框架還可以融合激光數(shù)據(jù)以外的信息，如相機(jī)得到的圖像數(shù)據(jù)。本方法假設(shè)相機(jī)C和激光L的相對(duì)位姿固定，可以由標(biāo)定給出。后續(xù)算法中，常常要知道不同時(shí)刻的圖像數(shù)據(jù)間的相對(duì)位姿。對(duì)于t_i刻和t_j刻的圖像數(shù)據(jù)和，它們之間的相對(duì)位姿可以由對(duì)應(yīng)激光的定位結(jié)果和激光-相機(jī)標(biāo)定結(jié)果給出，即

算法涉及的其余定義如下：在t_i時(shí)刻采集到的點(diǎn)云上的第k個(gè)點(diǎn)記為；t_j刻車輪和地面的接觸點(diǎn)記為，其中l和r分別表示左輪和右輪，接觸點(diǎn)的位置可以由標(biāo)定或者測(cè)量給出。

本方法將路面分為三種語義，即可行域、障礙物區(qū)域和未知區(qū)域。我們用一個(gè)移動(dòng)機(jī)器人在校園里的同一條道路上采集了不同時(shí)間段的傳感器數(shù)據(jù)。方法的核心即假設(shè)機(jī)器人走過的地方是可行域；地面以上的激光點(diǎn)是障礙物。標(biāo)簽數(shù)據(jù)生成過程如下：

1）可行域：本方法提出的可行域是基于先前提出的“候選道路”的概念的。在第s段數(shù)據(jù)上，t_i刻的“候選道路”P_s是未來機(jī)器人將要經(jīng)過的路徑在當(dāng)前圖像上的投影。這條路徑是由機(jī)器人輪子與地面接觸點(diǎn)形成的軌跡決定的，并且是在相機(jī)坐標(biāo)系下的，它在圖像上的投影可以表示為

式中，是上文提到的激光定位結(jié)果；是激光-相機(jī)標(biāo)定結(jié)果；K是相機(jī)內(nèi)參。

在此定義下，“候選道路”P_s可以表示為一個(gè)頂點(diǎn)如下的多邊形

對(duì)于每段數(shù)據(jù)，我們可以得到一條“候選道路”P_s。在采集到多段數(shù)據(jù)后，通過融合多條“候選道路”，我們可以得到更寬的可行域。可行域融合采用圖像上的“并”操作。圖1-40是該融合過程的一個(gè)例子，分別展示了融合1、2、3、4、5和13段數(shù)據(jù)后的可行域。可以看到，可行域越來越寬闊。我們的融合方法收益與統(tǒng)一的數(shù)據(jù)框架，先前提出的基于局部的算法無法有效利用多段數(shù)據(jù)。

2）障礙物區(qū)域：障礙物區(qū)域表示車輛無法通過的地方。上面提出的可行域標(biāo)簽生成方法存在一定瑕疵，在邊界的地方容易產(chǎn)生歧義。如果直接將可行域以外的地方標(biāo)記為障礙物，會(huì)影響實(shí)際避障效果。因此，找出障礙物區(qū)域尤為重要。

在我們的傳感器配置中，激光可以檢測(cè)出周圍環(huán)境的結(jié)構(gòu)，因此可以通過激光檢測(cè)障礙物。激光點(diǎn)投影為，像素點(diǎn)上方的像素標(biāo)記為障礙物。為了避免將地面點(diǎn)云標(biāo)記為障礙物，還需要對(duì)地面進(jìn)行平面擬合。我們?cè)诋?dāng)前點(diǎn)云上進(jìn)行平面擬合，將距離平面小于0.25m的點(diǎn)從點(diǎn)云中刪除，刪除后的點(diǎn)云按照上述步驟在圖像上標(biāo)記障礙物。由于激光的稀疏性，我們還對(duì)圖像進(jìn)行了形態(tài)學(xué)膨脹操作，以填補(bǔ)空洞。為了行駛安全，障礙物優(yōu)先級(jí)高于可行域，即如果一個(gè)點(diǎn)同時(shí)被標(biāo)記為二者，則該點(diǎn)視為障礙物。

3）未知區(qū)域：上述自動(dòng)標(biāo)記算法完成后，有一些像素會(huì)沒有標(biāo)簽。這是因?yàn)橛行┑孛婕葲]有被車輛行駛過，也沒有激光點(diǎn)掃描到。對(duì)于這些區(qū)域，我們將其標(biāo)記為未知區(qū)域。

上述方法可以產(chǎn)生大量標(biāo)簽數(shù)據(jù)，數(shù)據(jù)的數(shù)量取決于采集車行走的路程。理論上這些數(shù)據(jù)適用于任意的語義分割網(wǎng)絡(luò)。在實(shí)際部署中，將單目相機(jī)得到的圖像數(shù)據(jù)輸入網(wǎng)絡(luò)，即可獲得可行域預(yù)測(cè)結(jié)果。本文采用DeepLab作為像素級(jí)別分類器，該網(wǎng)絡(luò)在精度和效率上均有不錯(cuò)的表現(xiàn)。與先前類似的我們還對(duì)角度進(jìn)行了直方圖采樣，來保證數(shù)據(jù)的平衡性。

1.2.5 面向智能巡航的多傳感器復(fù)合導(dǎo)航

1.2.5.1 多傳感器復(fù)合導(dǎo)航問題分析

廣義定義導(dǎo)航是監(jiān)督和控制一架航空器或者車輛從一個(gè)地方到另一個(gè)地方移動(dòng)的過程。對(duì)于旋翼飛行機(jī)器人系統(tǒng)來說，導(dǎo)航可以定義為數(shù)據(jù)獲取、數(shù)據(jù)分析和提取以及關(guān)于機(jī)體狀態(tài)和其周邊環(huán)境信息的推測(cè)，并且成功和安全地完成分配的任務(wù)。根據(jù)一個(gè)導(dǎo)航系統(tǒng)的自主等級(jí)功能，從低到高可以分為：

1）傳感：一種傳感系統(tǒng)涉及一個(gè)或者一組傳感器，這些傳感器對(duì)特定的物理現(xiàn)象或者激勵(lì)做出反應(yīng)并且產(chǎn)生反應(yīng)目標(biāo)物或者物理現(xiàn)象的一些特征或者信息。旋翼飛行機(jī)器人系統(tǒng)機(jī)載常用的傳感器有陀螺儀、加速度計(jì)、地磁計(jì)、靜態(tài)和動(dòng)態(tài)壓力傳感器、攝像頭、激光雷達(dá)等。這些傳感器可以為狀態(tài)估計(jì)和感知算法提供原始的測(cè)量信息。

2）狀態(tài)估計(jì)：狀態(tài)估計(jì)主要涉及原始傳感測(cè)量值的處理，從而估計(jì)與機(jī)體相關(guān)的狀態(tài)變量，特別是其相關(guān)的位置和運(yùn)動(dòng)，例如姿態(tài)、位置和速度。這些估計(jì)可以是絕對(duì)的或者相對(duì)的，定位是一種局限于關(guān)聯(lián)地圖或其他地點(diǎn)的位置估計(jì)的特殊情況。

3）感知：旋翼移動(dòng)類機(jī)器人系統(tǒng)感知是能夠使用來自傳感器的輸入信息構(gòu)建機(jī)體操作環(huán)境的內(nèi)部模型，并且可以分配實(shí)體、事件，能夠根據(jù)感知的環(huán)境狀況來分類。分類/識(shí)別處理涉及旋翼飛行機(jī)器人系統(tǒng)觀測(cè)到的信息與先驗(yàn)信息進(jìn)行對(duì)比。感知根據(jù)不同的層級(jí)進(jìn)一步可以分為多種功能，例如建圖、避障、目標(biāo)檢測(cè)和目標(biāo)識(shí)別。

4）航行態(tài)勢(shì)感知：態(tài)勢(shì)感知的概念通常用于航空領(lǐng)域并且關(guān)于態(tài)勢(shì)感知的定義有很多。在本次調(diào)研報(bào)告中，我們采用“一定時(shí)間和空間環(huán)境中的元素的感知，對(duì)它們的含義的理解，并對(duì)他們稍后狀態(tài)的投影（恩茲利，1999）”作為態(tài)勢(shì)感知的定義。態(tài)勢(shì)感知比感知更高一層，因?yàn)樗枰獙?duì)形勢(shì)進(jìn)行理解然后對(duì)未來一段時(shí)間的信息進(jìn)行推斷或者投影來決定將如何影響操作環(huán)境的未來狀態(tài)。

上述分級(jí)導(dǎo)航內(nèi)容的整體結(jié)構(gòu)圖如圖1-41所示。

按照導(dǎo)航采用傳感器的不同，將導(dǎo)航分為慣性導(dǎo)航、全球?qū)Ш叫l(wèi)星系統(tǒng)（GNSS）、慣性導(dǎo)航系統(tǒng)（INS）和GNSS復(fù)合導(dǎo)航、視覺導(dǎo)航、激光導(dǎo)航、多傳感器融合導(dǎo)航、感知導(dǎo)航系統(tǒng)等。基于IMU/GNSS、IMU/激光雷達(dá)的傳統(tǒng)導(dǎo)航方式應(yīng)用最為普遍，目前已基本成熟。高校、科研機(jī)構(gòu)及各大飛行機(jī)器人企業(yè)把研發(fā)重心放在了視覺、多傳感器融合導(dǎo)航等其他導(dǎo)航方式上。

1.2.5.2 多傳感器融合導(dǎo)航

單一的導(dǎo)航方式都有著各自的優(yōu)缺點(diǎn)，多傳感器融合才能得到更加可靠的導(dǎo)航性能。典型的多傳感器融合導(dǎo)航系統(tǒng)由IMU、GPS、視覺、激光雷達(dá)等構(gòu)成，可以實(shí)現(xiàn)各種復(fù)雜環(huán)境下的自主飛行。千葉大學(xué)在該種導(dǎo)航系統(tǒng)的基礎(chǔ)上，實(shí)現(xiàn)了核泄漏現(xiàn)場(chǎng)的飛行機(jī)器人偵察。麻省理工學(xué)院（MIT）也多次公開發(fā)布了室內(nèi)外自由移動(dòng)的飛行機(jī)器人融合導(dǎo)航科研成果。此外，賓夕法尼亞大學(xué)、密歇根理工大學(xué)、卡內(nèi)基·梅隆大學(xué)、哈爾濱工程大學(xué)等高校也在研究應(yīng)用于飛行機(jī)器人導(dǎo)航的多傳感器融合技術(shù)，如圖1-42和圖1-43所示。國(guó)際飛行機(jī)器人賽事也把該項(xiàng)內(nèi)容列為主要比賽項(xiàng)目。

（1）復(fù)合導(dǎo)航飛行實(shí)驗(yàn)平臺(tái)

本項(xiàng)研究定制的復(fù)合導(dǎo)航飛行實(shí)驗(yàn)平臺(tái)（圖1-44）搭載有三維激光雷達(dá)、視覺傳感器、GNSS接收機(jī)/實(shí)時(shí)動(dòng)態(tài)（RTK）載波相位差分技術(shù)、地磁計(jì)、慣性測(cè)量單元（IMU）、氣壓計(jì)等傳感器，通過融合以上多種傳感器來實(shí)現(xiàn)位置復(fù)雜多種環(huán)境下的自主導(dǎo)航。

（2）硬件系統(tǒng)

復(fù)合導(dǎo)航板微處理器采用STM32F4系列處理器，提供了工作頻率為168MHz的Cortex^TM_M4內(nèi)核（具有浮點(diǎn)單元）的性能，閃存高達(dá)1MB、具有先進(jìn)互聯(lián)功能和加密功能，可用于復(fù)合導(dǎo)航多種傳感器信號(hào)中轉(zhuǎn)、計(jì)算，可接入IMU、RGB-D相機(jī)、激光雷達(dá)、光流傳感器等多種傳感器，能同時(shí)實(shí)現(xiàn)4路串口數(shù)據(jù)收發(fā)，2路CAN接口數(shù)據(jù)收發(fā)，另外預(yù)留了1 路IIC接口。支持DC5～12V的電源輸入，支持DC3.3V/1A的電源輸出。每路的數(shù)據(jù)輸入輸出口均有指示燈提示。為了避免誤插，接口采用防接錯(cuò)設(shè)計(jì)。復(fù)合導(dǎo)航印制電路板（PCB）如圖1-45所示。

（3）多傳感器數(shù)據(jù)融合架構(gòu)

多傳感器數(shù)據(jù)融合架構(gòu)圖如圖1-46所示。一級(jí)模塊包含模塊1、模塊2，一級(jí)融合模塊1融合加速度計(jì)、陀螺儀、地磁計(jì)、光流傳感器和單線激光雷達(dá)輸出Roll、Pitch、Yaw及三軸加速度、角速度數(shù)據(jù)、位置、速度和高度數(shù)據(jù)；一級(jí)融合模塊2使用3D激光雷達(dá)/視覺傳感器通過融合估計(jì)得到的三維位置數(shù)據(jù)。二級(jí)融合模塊的數(shù)據(jù)來源于一級(jí)融合模塊，采用EKF多傳感器融合方法，程序獨(dú)立運(yùn)行于多傳感器數(shù)據(jù)融合板（MSDFB）中。

（4）GNSS/IMU融合導(dǎo)航

通過EKF融合GNSS（BDS/GPS）接收機(jī)、姿態(tài)角、加速度等數(shù)據(jù)輸出位姿信息，該位姿信息可以用于室外環(huán)境下的自主導(dǎo)航，通過將融合速度數(shù)據(jù)與原始速度數(shù)據(jù)對(duì)比來驗(yàn)證算法的有效性，如圖1-47所示。其中，紅色實(shí)線為融合后的，藍(lán)色虛線為原始數(shù)據(jù)。

對(duì)比融合經(jīng)緯度數(shù)據(jù)與原始經(jīng)緯度數(shù)據(jù)來驗(yàn)證算法的有效性，如圖1-48所示，其中紅色實(shí)線為融合后的，藍(lán)色虛線為原始數(shù)據(jù)。