面向觀測融合和吸引因子的多機器人主動SLAM

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

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

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

多機器人系統在探索能力、魯棒性、容錯性等方面比單機器人更具優勢[6]，各個機器人通過通信網絡進行信息傳遞與共享，相互協調工作，共享環境信息，極大擴展了機器人系統對環境的感知范圍與定位的精度。因此，自20世紀90年代以來，在研究單機器人SLAM的基礎上，多機器人SLAM逐步成為移動機器人領域研究的熱門[5，7-8]。

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

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

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

1 問題描述

考慮一個擁有n個機器人的系統，符合以下運動模型：

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

在主動SLAM任務中要求機器人感知環境主動選擇控制量去定位未知環境下的所有地標，地標的狀態 yt是靜態的：

機器人運動時在每個時刻進行觀測，其傳感器觀測模型如下：

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

如圖1所示為EKF-SLAM簡圖[17]，其中橢圓代表機器人位置估計的不確定度，當機器人沿如圖1所示直線運動時，可以看出當機器人一直觀測不到地標時，位置估計不確定度會越來越大，在完全未知環境中無法預測機器人定位誤差的上限[18]，直到觀測到新的地標時其不確定度會下降。這也是現有大多數算法不能遍歷完環境的原因，因為在完全未知環境下，當沒有地標作用時，機器人自身定位誤差越來越大，其會傾向于去探索過的地方降低自身不確定度，而不是選擇繼續探索環境。

圖 1 使用EKF-SLAM進行地標定位簡圖Fig. 1 Landmark localization using extended Kalman Filter

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

互信息可以衡量觀測與目標的關聯程度，互信息越大說明，目標與觀測之前的關聯越大，則更 有可能從觀測中獲取更多信息。

2 基于觀測融合的主動SLAM

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

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

量測更新：

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

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

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

其中 ∏ 為歸一化符號。

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

觀測階段中，每個機器人在不同位置對環境中的地標進行觀測，如圖2所示。

圖 2 地標估計融合示意Fig. 2 Landmark estimation fusion schematic

當多個機器人觀測到同一地標時，多機器人系統可以分先后觀測到同一地標，每個機器人都會對地標位置產生一個估計，同一地標的估計位置可能有多個，這樣會帶來更多的不確定性，造成錯誤的數據關聯。因此，在多個機器人觀測到同一地標時，采用凸組合融合對各個機器人對地標的估計值融合，從而降低地標的不確定度。地標的不確定度越小其地標估計誤差協方差的跡越小，凸組合融合可以表示為

(2)明確區域角色，尋求優勢互補。一般而言，核心區是旅游經濟的增長區，而邊緣區是旅游經濟的低迷區。核心區在不斷增強旅游競爭力的同時，應該積極加強與邊緣區旅游經濟的聯動發展，最大化發揮核心區的“涓滴效應”。邊緣區在穩定承接游客溢出的同時，還應主動與核心區進行旅游戰略合作，學習借鑒核心區成熟的旅游發展經驗，深度開發邊緣區的旅游資源，將資源優勢轉化成旅游經濟優勢。

3 吸引因子

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

吸引因子是用于指引機器人進行控制策略選擇的一個向量，當其出現時機器人會去選擇朝著吸引因子的方向進行運動規劃。其發揮作用從出現到消失為3步，滿足一定條件后會重新出現。當機器人在t步內無新增探索面積時，吸引因子會出現，其朝向為最近的虛擬探索地標，去引導多機器人團隊進行環境探索，如圖3所示，當機器人自身的位置估計協方差的跡 t race(Pr) 大于一定閾值 ε 時，ε 閾值根據實驗設定，吸引因子會出現，其朝向為定位最精確的地標，修正自身位姿估計。

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

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

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

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

4 仿真分析

為驗證多機器人主動SLAM算法有效性，設計仿真進行驗證。機器人運動模型如下[18]：

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

目標為靜態地標，在([0,200],[0,200]) 環境中隨機分布200個地標，傳感器觀測模型包含機器人與目標yt距離和角(度，觀)測模型如下：

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

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

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

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

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

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

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

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

對于無吸引因子的主動SLAM來說，即使目標函數式(11)中含有對新增面積的權重，仍然難以對環境進行完全覆蓋。因為多機器人團隊，在建圖過程中，當機器人在已經建圖的環境中走動的時候，無新增覆蓋面積。因此目標函數式(11)退化成了式(6),這樣會導致機器人在已經建圖區域徘徊，不能遍歷整個環境。

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

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

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

圖 11 凸組合融合前后地標估計誤差協方差跡的對比Fig. 11 Comparison of landmark estimation error covariance trace before and after convex combination

表1所示為當仿真步數為167步時5種SLAM方 法平均地標估計與環境覆蓋率的對比。

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

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

5 結束語

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