王賀彬，葛泉波，劉華平，袁小虎

（1. 杭州電子科技大學(xué) 自動化學(xué)院，浙江 杭州 310018; 2. 同濟(jì)大學(xué) 電子與信息工程學(xué)院，上海 201804; 3. 清華大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)系，北京 100084; 4. 清華大學(xué) 自動化系，北京 100084）

同步定位與地圖構(gòu)建 (simultaneous localization and mapping, SLAM)問題可以描述為：移動機(jī)器人在未知環(huán)境中運(yùn)動時，逐步構(gòu)建其周圍環(huán)境的地圖，同時運(yùn)用此地圖對機(jī)器人位置和姿態(tài)進(jìn)行估計(jì)[1]。SLAM技術(shù)廣泛應(yīng)用于移動機(jī)器人、無人駕駛汽車、無人機(jī)、增強(qiáng)現(xiàn)實(shí)、導(dǎo)航或水下無人潛航器等領(lǐng)域[2-4]。但是目前已有的SLAM算法，大多是單機(jī)器人系統(tǒng)且假設(shè)運(yùn)動軌跡已知，這種被動式SLAM與SLAM問題中的環(huán)境未知相矛盾[5]。

多機(jī)器人系統(tǒng)在探索能力、魯棒性、容錯性等方面比單機(jī)器人更具優(yōu)勢[6]，各個機(jī)器人通過通信網(wǎng)絡(luò)進(jìn)行信息傳遞與共享，相互協(xié)調(diào)工作，共享環(huán)境信息，極大擴(kuò)展了機(jī)器人系統(tǒng)對環(huán)境的感知范圍與定位的精度。因此，自20世紀(jì)90年代以來，在研究單機(jī)器人SLAM的基礎(chǔ)上，多機(jī)器人SLAM逐步成為移動機(jī)器人領(lǐng)域研究的熱門[5，7-8]。

由Feder[9]提出的基于信息論的自適應(yīng)導(dǎo)航與建圖方法為主動SLAM奠定了基礎(chǔ)。文獻(xiàn)[10]，基于EKF提出局部子地圖的SLAM方法，證明局部子地圖與全局地圖相互獨(dú)立，機(jī)器人只維護(hù)自己所建立的子圖，然后周期性地將子圖融合到全局地圖中，但是其機(jī)器人的路徑事先指定，建圖過程中僅按規(guī)定路線進(jìn)行環(huán)境探索，因此缺乏多機(jī)協(xié)同。文獻(xiàn)[11]擴(kuò)展了基于局部子圖的多機(jī)器人主動SLAM方法，將主動SLAM問題轉(zhuǎn)為多目標(biāo)優(yōu)化問題，并通過EKF對子地圖進(jìn)行融合得到全局地圖。文獻(xiàn)[12]通過引入吸引因子，實(shí)現(xiàn)主動SLAM，但是只適用于單機(jī)器人。文獻(xiàn)[13]將主動信息收集任務(wù)應(yīng)用于多機(jī)器人主動SLAM中，并引入虛擬地標(biāo)與吸引因子引導(dǎo)多機(jī)器人探索，但是沒有進(jìn)行估計(jì)的數(shù)據(jù)融合。

多機(jī)器人主動SLAM是指機(jī)器人在未知環(huán)境下多機(jī)器人協(xié)同主動感知環(huán)境，并通過目標(biāo)函數(shù)選擇一系列的控制輸入使得機(jī)器人能夠高效地完成探索與建圖的任務(wù)，其目標(biāo)函數(shù)的優(yōu)化的目標(biāo)一般包括定位與建圖的準(zhǔn)確、新增探索區(qū)域、導(dǎo)航代價等因素。但以上優(yōu)化目標(biāo)之間往往是相互制約、相互對立的，例如機(jī)器人為了提高自身定位與建圖的準(zhǔn)確性，常局限于一定的范圍內(nèi)徘徊，而向外探索時，定位與建圖的不確定性又會增大[14]。即使進(jìn)行多目標(biāo)優(yōu)化，對常見的優(yōu)化函數(shù)進(jìn)行線性疊加，在實(shí)際中還是會出現(xiàn)不能遍歷完整個環(huán)境、定位與建圖精度不理想的情況，因?yàn)?，定位所有地?biāo)不等于遍歷整個環(huán)境，不遍歷完整個環(huán)境，是不知道地標(biāo)的具體數(shù)目位置的。因此，本文擴(kuò)展了文獻(xiàn)[13]的吸引因子，將吸引因子作用域放大，不僅僅用來糾正自身定位不好的機(jī)器人，還用于去指導(dǎo)多機(jī)器人團(tuán)隊(duì)進(jìn)行環(huán)境的探索，使其能夠遍歷完整個環(huán)境，定位環(huán)境中所有的未知地標(biāo)，吸引因子作為媒介促進(jìn)多機(jī)器人系統(tǒng)的交流，增強(qiáng)多機(jī)器人系統(tǒng)與環(huán)境的交互，使其能夠快速協(xié)同完成定位建圖任務(wù)。多機(jī)器人主動SLAM中的誤差主要來自于兩個方面：一是機(jī)器人自身位置誤差，二是傳感器觀測誤差。當(dāng)機(jī)器人自身定位精準(zhǔn)時，其估計(jì)的視野范圍內(nèi)的地標(biāo)精度也比較高。當(dāng)多個機(jī)器人觀測到同一地標(biāo)時，通過凸組合融合[15]利用多個機(jī)器人對地標(biāo)的估計(jì)值修正當(dāng)前地標(biāo)的狀態(tài)值，可以大大降低該地標(biāo)的不確定度，減少多機(jī)器人系統(tǒng)定位與建圖的誤差。

針對上述問題，本文基于EKF-SLAM算法[16]提出一種多機(jī)器人主動SLAM算法，其主要創(chuàng)新性工作包含改進(jìn)吸引因子作用范圍和建立信息融合技術(shù)為基礎(chǔ)的地標(biāo)估計(jì)融合方法，將主動SLAM問題轉(zhuǎn)化為最優(yōu)控制選取問題，并通過引入吸引因子促進(jìn)了多機(jī)器人間的信息交流，一方面用于提升機(jī)器人自身位置估計(jì)準(zhǔn)確性，另一方面用于指引多機(jī)器人進(jìn)行環(huán)境的探索。當(dāng)多個機(jī)器人觀測到同一地標(biāo)時，采用凸組合融合[15]，進(jìn)行地標(biāo)的估計(jì)，降低被估計(jì)地標(biāo)的不確定度，提升建圖與定位的精度。

1 問題描述

考慮一個擁有n個機(jī)器人的系統(tǒng)，符合以下運(yùn)動模型：

式中：xri,t+1是第i個機(jī)器人在時間t時刻的 nx維狀態(tài)向量；f(·) 是非線性狀態(tài)轉(zhuǎn)移函數(shù)； ui,t∈Ui是在t時刻第i個在機(jī)器人的控制量； Ui是有限的控制輸入； wi,t∈Rnx是 nx維高斯過程噪聲。

在主動SLAM任務(wù)中要求機(jī)器人感知環(huán)境主動選擇控制量去定位未知環(huán)境下的所有地標(biāo)，地標(biāo)的狀態(tài) yt是靜態(tài)的：

機(jī)器人運(yùn)動時在每個時刻進(jìn)行觀測，其傳感器觀測模型如下：

式中：zi,t∈Rnzi為在t時刻第i個機(jī)器人的觀測向量；h(·) 是非線性量測函數(shù)； vi,t∈Rnzi是 nzi維量測噪聲。為了簡化符號，令u2,t···un,t] ，U=U1×U2×···×Un。

如圖1所示為EKF-SLAM簡圖[17]，其中橢圓代表機(jī)器人位置估計(jì)的不確定度，當(dāng)機(jī)器人沿如圖1所示直線運(yùn)動時，可以看出當(dāng)機(jī)器人一直觀測不到地標(biāo)時，位置估計(jì)不確定度會越來越大，在完全未知環(huán)境中無法預(yù)測機(jī)器人定位誤差的上限[18]，直到觀測到新的地標(biāo)時其不確定度會下降。這也是現(xiàn)有大多數(shù)算法不能遍歷完環(huán)境的原因，因?yàn)樵谕耆粗h(huán)境下，當(dāng)沒有地標(biāo)作用時，機(jī)器人自身定位誤差越來越大，其會傾向于去探索過的地方降低自身不確定度，而不是選擇繼續(xù)探索環(huán)境。

圖 1 使用EKF-SLAM進(jìn)行地標(biāo)定位簡圖Fig. 1 Landmark localization using extended Kalman Filter

多機(jī)器人主動SLAM問題可以描述為最大化目標(biāo)yt與zt之間的互信息I(yt|zt)。

互信息可以衡量觀測與目標(biāo)的關(guān)聯(lián)程度，互信息越大說明，目標(biāo)與觀測之前的關(guān)聯(lián)越大，則更 有可能從觀測中獲取更多信息。

2 基于觀測融合的主動SLAM

主動SLAM可以用式(5)描述為隨機(jī)最優(yōu)控制問題，通常，對于此類問題，自適應(yīng)閉環(huán)控制策略比非自適應(yīng)開環(huán)策略具有顯著優(yōu)勢。但是，由于觀測模型式(4)在目標(biāo)狀態(tài)下的線性和高斯噪聲假設(shè)，可以證明式(5)簡化為確定性的最優(yōu)控制問題，對于該問題，開環(huán)策略是最優(yōu)的[19]。在目標(biāo)狀態(tài)是高斯分布的情況下，存在最優(yōu)控制序列u使得式(5)最優(yōu)，與此同時式(5)等價于以下最優(yōu)控制問題：

其中Fi,t和Gi,t分別為運(yùn)動模型f(·)對機(jī)器人狀態(tài)向量,t與運(yùn)動噪聲向量wi,t的雅可比矩陣。

量測更新：

其中Hi,t+1是觀測模型h(·)對狀態(tài)向量-i,t+1的雅可比矩陣：

主動SLAM問題式(6)指多機(jī)器人系統(tǒng)在t時刻，主動選取最優(yōu)控制序列u使得其在t+1時刻目標(biāo)函數(shù)J最小。本文利用貪婪算法[12]可以用于解決第i個機(jī)器人的主動SLAM問題，在t時刻分析控制空間Ui中可用的控制量ui,t對機(jī)器人的運(yùn)動產(chǎn)生的影響，找到能使機(jī)器人估計(jì)誤差協(xié)方差 trace(Pi,t+1)最小的控制量ui,t。

但是在EKF-SLAM建圖中，機(jī)器人為了提升自身的估計(jì)精度，常會在一定范圍局部運(yùn)動，在探測過的區(qū)域進(jìn)行徘徊。當(dāng)機(jī)器人與兩個陸標(biāo)的距離相等，且到兩個陸標(biāo)的向量正交時，系統(tǒng)更新是最穩(wěn)定的[14]。因此需要對式(6)進(jìn)行優(yōu)化，不僅僅考慮機(jī)器人估計(jì)誤差協(xié)方差還要考慮新增探索面積 ΔSit+1|t,優(yōu)化后的目標(biāo)函數(shù)為

其中 ∏ 為歸一化符號。

新增探索面積為相對于已探索區(qū)域，機(jī)器人視野掃過新增加的面積。

觀測階段中，每個機(jī)器人在不同位置對環(huán)境中的地標(biāo)進(jìn)行觀測，如圖2所示。

圖 2 地標(biāo)估計(jì)融合示意Fig. 2 Landmark estimation fusion schematic

當(dāng)多個機(jī)器人觀測到同一地標(biāo)時，多機(jī)器人系統(tǒng)可以分先后觀測到同一地標(biāo)，每個機(jī)器人都會對地標(biāo)位置產(chǎn)生一個估計(jì)，同一地標(biāo)的估計(jì)位置可能有多個，這樣會帶來更多的不確定性，造成錯誤的數(shù)據(jù)關(guān)聯(lián)。因此，在多個機(jī)器人觀測到同一地標(biāo)時，采用凸組合融合對各個機(jī)器人對地標(biāo)的估計(jì)值融合，從而降低地標(biāo)的不確定度。地標(biāo)的不確定度越小其地標(biāo)估計(jì)誤差協(xié)方差的跡越小，凸組合融合可以表示為

(2)明確區(qū)域角色，尋求優(yōu)勢互補(bǔ)。一般而言，核心區(qū)是旅游經(jīng)濟(jì)的增長區(qū)，而邊緣區(qū)是旅游經(jīng)濟(jì)的低迷區(qū)。核心區(qū)在不斷增強(qiáng)旅游競爭力的同時，應(yīng)該積極加強(qiáng)與邊緣區(qū)旅游經(jīng)濟(jì)的聯(lián)動發(fā)展，最大化發(fā)揮核心區(qū)的“涓滴效應(yīng)”。邊緣區(qū)在穩(wěn)定承接游客溢出的同時，還應(yīng)主動與核心區(qū)進(jìn)行旅游戰(zhàn)略合作，學(xué)習(xí)借鑒核心區(qū)成熟的旅游發(fā)展經(jīng)驗(yàn)，深度開發(fā)邊緣區(qū)的旅游資源，將資源優(yōu)勢轉(zhuǎn)化成旅游經(jīng)濟(jì)優(yōu)勢。

3 吸引因子

由于多機(jī)器人系統(tǒng)不知道環(huán)境中地標(biāo)的數(shù)目和位置，基于式(11)的目標(biāo)函數(shù)在機(jī)器人視野范圍內(nèi)無新增探索面積時，其會采用估計(jì)誤差協(xié)方差作為主動控制選取的原則，這樣會導(dǎo)致機(jī)器人在已經(jīng)探索過的區(qū)域徘徊。因此有必要讓多機(jī)器人團(tuán)隊(duì)去探索環(huán)境，故增加虛擬探索地標(biāo)，在機(jī)器人所建圖的邊界上，當(dāng)機(jī)器人擴(kuò)展環(huán)境時虛擬地標(biāo)會建立，并移除舊的虛擬探索地標(biāo)，當(dāng)虛擬探索地標(biāo)集合為空時證明環(huán)境已經(jīng)遍歷完畢。

吸引因子是用于指引機(jī)器人進(jìn)行控制策略選擇的一個向量，當(dāng)其出現(xiàn)時機(jī)器人會去選擇朝著吸引因子的方向進(jìn)行運(yùn)動規(guī)劃。其發(fā)揮作用從出現(xiàn)到消失為3步，滿足一定條件后會重新出現(xiàn)。當(dāng)機(jī)器人在t步內(nèi)無新增探索面積時，吸引因子會出現(xiàn)，其朝向?yàn)樽罱奶摂M探索地標(biāo)，去引導(dǎo)多機(jī)器人團(tuán)隊(duì)進(jìn)行環(huán)境探索，如圖3所示，當(dāng)機(jī)器人自身的位置估計(jì)協(xié)方差的跡 t race(Pr) 大于一定閾值 ε 時，ε 閾值根據(jù)實(shí)驗(yàn)設(shè)定，吸引因子會出現(xiàn)，其朝向?yàn)槎ㄎ蛔罹_的地標(biāo)，修正自身位姿估計(jì)。

吸引因子的作用有兩個，一是提升機(jī)器人自身的定位精度修正自身位姿估計(jì)，二是引導(dǎo)機(jī)器人探索環(huán)境。吸引因子促進(jìn)了多機(jī)器人團(tuán)隊(duì)進(jìn)行交流與通信。在完全未知的環(huán)境下，吸引因子的必要性是保證多機(jī)器人團(tuán)隊(duì)能夠遍歷完整個環(huán)境，不加吸引因子的主動SLAM算法，會出現(xiàn)局部環(huán)境遍歷不完全的情況。

圖 3 用于位姿修正的吸引因子Fig. 3 Attractors for pose correction

如圖4所示，吸引因子的應(yīng)用范圍是位姿修正與探索環(huán)境，虛線代表滿足一定的條件所采取的控制策略。在機(jī)器人位置估計(jì)偏差較大時，會觸發(fā)吸引因子，此時不采用貪婪算法，而吸引因子會去引導(dǎo)機(jī)器人修正其位姿，而不會進(jìn)行環(huán)境探索。當(dāng)機(jī)器人在t步內(nèi)無新增擴(kuò)展面積時，則會觸發(fā)吸引因子，引導(dǎo)多機(jī)器人團(tuán)隊(duì)去探索環(huán)境。

圖 4 多機(jī)器人主動SLAM算法流程Fig. 4 Multi-robot active SLAM algorithm

4 仿真分析

為驗(yàn)證多機(jī)器人主動SLAM算法有效性，設(shè)計(jì)仿真進(jìn)行驗(yàn)證。機(jī)器人運(yùn)動模型如下[18]：

機(jī)器人的狀態(tài)向量為xr=[xyθ]T，其中x、y為機(jī)器人在環(huán)境內(nèi)的位置，θ 為其方位角。離散控制入U(xiǎn)={(v,ω)|v=10m/s,ω∈{0,±π/6,±π/12}(°)/s} 。采樣周期 Δt=0.5,無論機(jī)器人選擇哪個控制量，其一步可以移動的最大距離為d=10m/s×0.5s=5m 。

目標(biāo)為靜態(tài)地標(biāo)，在([0,200],[0,200]) 環(huán)境中隨機(jī)分布200個地標(biāo)，傳感器觀測模型包含機(jī)器人與目標(biāo)yt距離和角(度，觀)測模型如下：

機(jī)器人所攜帶的傳感器能探測的最大范圍為25 m，機(jī)器人可視角度為 φ =π/3,具有前向視野(-φ/2＜α(x,y)＜φ/2),超過最大范圍和視野的地標(biāo)，僅對其進(jìn)行預(yù)測而不更新，設(shè)定吸引因子的觸發(fā)條件 ε=10，t=3。

為探究本文算法有效性，本文分別對比單機(jī)器人主動SLAM、隨機(jī)SLAM、無吸引因子的多機(jī)器人主動SLAM、基于EKF融合的多機(jī)器人主動SLAM和帶吸引因子的多機(jī)器人主動SLAM，這5種方式的SLAM算法以驗(yàn)證本文算法有效性。

圖5、6為基于EKF融合的多機(jī)器人主動SLAM，圖7、8為無吸引因子的多機(jī)器人主動SLAM，圖9、10帶吸引因子的多機(jī)器人主動SLAM。其中，綠色三角代表機(jī)器人，紅色橢圓代表機(jī)器人的位置協(xié)方差，青色圓圈代表真實(shí)的地標(biāo)位置，藍(lán)色點(diǎn)和橢圓分別代表估計(jì)的地標(biāo)位置與協(xié)方差，紅色虛線代表機(jī)器人可視范圍，洋紅色的五角星代表吸引因子，黃色方塊代表虛擬探索地標(biāo)。深紅色的點(diǎn)和橢圓代表融合估計(jì)的地標(biāo)位置和協(xié)方差，初始的地標(biāo)不確定度為：tr(Plm)/m=2000。

圖 5 基于EKF融合的多機(jī)器主動SLAM建圖80步Fig. 5 80 steps of multi-robot active SLAM based on EKF fusion

其中圖5、圖7、圖9為建圖的中間過程對比，從圖6和圖8中可以看出這兩種方式的主動SLAM可以遍歷大部分的環(huán)境，但是不能遍歷完全部環(huán)境。

圖 6 基于EKF融合的多機(jī)器主動SLAM建圖167步Fig. 6 167 steps of multi-robot active SLAM based on EKF fusion

圖 7 無吸引因子的多機(jī)器人主動SLAM建圖80步Fig. 7 80 steps of multi-robot active SLAM without attractor

圖 8 無吸引因子的多機(jī)器人主動SLAM建圖167步Fig. 8 167 steps of multi-robot active SLAM without attractors

對于無吸引因子的主動SLAM來說，即使目標(biāo)函數(shù)式(11)中含有對新增面積的權(quán)重，仍然難以對環(huán)境進(jìn)行完全覆蓋。因?yàn)槎鄼C(jī)器人團(tuán)隊(duì)，在建圖過程中，當(dāng)機(jī)器人在已經(jīng)建圖的環(huán)境中走動的時候，無新增覆蓋面積。因此目標(biāo)函數(shù)式(11)退化成了式(6),這樣會導(dǎo)致機(jī)器人在已經(jīng)建圖區(qū)域徘徊，不能遍歷整個環(huán)境。

圖 9 帶吸引因子的多機(jī)器主動SLAM建圖80步Fig. 9 80 steps of multi-robot active SLAM with attractors

圖 10 帶吸引因子的多機(jī)器主動SLAM建圖完成Fig. 10 Multi-robot active SLAM with attractors completed

如圖11所示，為帶吸引因子的多機(jī)器主動SLAM仿真中凸組合融合前后地標(biāo)估計(jì)誤差協(xié)方差跡的對比，其中共有184個地標(biāo)是融合地標(biāo)，縱坐標(biāo)為該地標(biāo)融合前后估計(jì)誤差協(xié)方差的跡的大小，可以看出凸組合融合可以有效地提升地標(biāo)估計(jì)的精度，降低估計(jì)的不確定度。

圖 11 凸組合融合前后地標(biāo)估計(jì)誤差協(xié)方差跡的對比Fig. 11 Comparison of landmark estimation error covariance trace before and after convex combination

表1所示為當(dāng)仿真步數(shù)為167步時5種SLAM方 法平均地標(biāo)估計(jì)與環(huán)境覆蓋率的對比。

表 1 5種SLAM方法對比Table 1 Comparison of five SLAM methods

從表1中可以看出，本文改進(jìn)方法即帶吸引因子的多機(jī)器人主動SLAM不僅可以覆蓋全部環(huán)境而且平均地標(biāo)估計(jì)均低于其他方法。通過與單機(jī)器人的對比可以看出多機(jī)器人系統(tǒng)的優(yōu)越性，多機(jī)器人團(tuán)隊(duì)可以同時不同地作業(yè)，協(xié)作完成任務(wù)。通過與無吸引因子的方法對比，說明吸引因子的有效性，帶吸引因子的多機(jī)器人團(tuán)隊(duì)，可以在機(jī)器人位置估計(jì)大于一定閾值下，通過吸引因子，讓機(jī)器人先去定位精度較高的地標(biāo)，修正自己的位姿，然后去擴(kuò)展環(huán)境，自身定位精度提升有利于提升估計(jì)地標(biāo)的精度。無吸引因子的主動SLAM方法和本文改進(jìn)方法的平均地標(biāo)估計(jì)均低于基于EKF融合的方法，說明凸組合融合可以很好地降低地標(biāo)估計(jì)的不確定度。綜上所訴，本文改進(jìn)方法可以很好地遍歷完環(huán)境，而且對地標(biāo)的估計(jì)精度有所提升。

5 結(jié)束語

本文研究基于EKF-SLAM的多機(jī)器人主動SLAM問題，將主動SLAM問題轉(zhuǎn)化為最優(yōu)控制選擇問題，通過引入吸引因子，既提升了機(jī)器人自身定位建圖準(zhǔn)確性，又引導(dǎo)多機(jī)器人團(tuán)隊(duì)機(jī)器人探索環(huán)境。對于多個機(jī)器人觀測到的同一地標(biāo)，采用凸組合融合，降低該地標(biāo)的不確定度。并通過仿真驗(yàn)證算法有效性，實(shí)現(xiàn)多機(jī)器人主動SLAM，其魯棒性、定位建圖精度都大大提升。但是本文算法依賴于多機(jī)器人之間通信良好，對多機(jī)器人在通信受限的情況下的主動SLAM研究還需要進(jìn)一步完善。