無人機(jī)主動SLAM技術(shù)研究

楊祥紅，王希彬

（海軍航空工程學(xué)院1.軍事教育訓(xùn)練系；2.控制工程系，山東煙臺 264001）

無人機(jī)主動SLAM技術(shù)研究

楊祥紅1，王希彬2

（海軍航空工程學(xué)院1.軍事教育訓(xùn)練系；2.控制工程系，山東煙臺 264001）

針對傳統(tǒng)被動SLAM算法存在盲目地圖構(gòu)建的問題，提出了基于約束局部子圖的無人機(jī)主動SLAM算法。首先，在約束局部子圖濾波方法的基礎(chǔ)上創(chuàng)建局部地圖，接著通過構(gòu)建的主動SLAM的目標(biāo)函數(shù)尋找局部地圖的最優(yōu)解，最后將局部地圖和全局地圖進(jìn)行融合。針對無人機(jī)系統(tǒng)進(jìn)行仿真，驗(yàn)證了基于局部子圖的主動SLAM算法的有效可行性。

無人機(jī)；主動SLAM；約束局部子圖濾波

1 引 言

SLAM（Simultaneous Location and Mapping，同時定位與地圖創(chuàng)建）問題只考慮了無人機(jī)如何利用獲得的信息構(gòu)建環(huán)境地圖，并在地圖中對無人機(jī)進(jìn)行定位，沒有考慮無人機(jī)如何有效地探測未知環(huán)境，即如何控制無人機(jī)運(yùn)動，以達(dá)到更好地和更快地構(gòu)建地圖的目的?，F(xiàn)有的環(huán)境探測策略基本上都是最大限度地、盡快地引導(dǎo)無人機(jī)向未探測區(qū)域運(yùn)動，它們關(guān)注的是盡快減少為探測區(qū)域，而盡量避免在已探測區(qū)域內(nèi)運(yùn)動。這類探測策略顯然不能直接用于SLAM中［1］，因?yàn)闊o人機(jī)需要重復(fù)探測已探測過的環(huán)境來對自身進(jìn)行自定位來提高定位的精度，從而提高地圖的精度。

目前所提出方法存在以下局限性［2］：（1）所規(guī)劃的無人機(jī)的運(yùn)動軌跡是在完全局部優(yōu)化的條件下獲得的；（2）只針對降低地圖和定位的不確定性來作出規(guī)劃，沒有考慮無人機(jī)環(huán)境探測速度和代價(jià)；（3）傳統(tǒng)的對信息增益的測量不能得出最精確的地圖。

本文針對無人機(jī)對象設(shè)計(jì)了一個仿真系統(tǒng)，分析驗(yàn)證了基于EKF的主動SLAM算法的性能。系統(tǒng)驗(yàn)證了基于局部子圖方法的主動SLAM算法，并給出了仿真結(jié)果。

2 基于局部子圖濾波的主動SLAM算法

2.1 局部子圖濾波

局部子圖方法在k時刻系統(tǒng)的狀態(tài)［5］為：

其中，GXL（k）表示全局地圖和局部地圖的關(guān)系，也就是全局坐標(biāo)系中局部坐標(biāo)系的位置，圖1給出了形象的表示；GXm（k）和LXm（k）分別表示全局地圖和局部地圖中地標(biāo)的位置；LXr（k）表示局部地圖中無人機(jī)的位姿。

圖1 局部坐標(biāo)系與全局坐標(biāo)系的轉(zhuǎn)換Fig.1 The transformationbetween local frame and global frame

方差矩陣具有以全局地圖表示一個方塊、局部地圖表示另一個方塊的塊對角結(jié)構(gòu)形式，包含無人機(jī)狀態(tài)的方差和地標(biāo)的方差。狀態(tài)方差矩陣為：

為獲得系統(tǒng)狀態(tài)的估計(jì)值，采用EKF估計(jì)無人機(jī)的位姿和觀測地標(biāo)的位置。與最優(yōu)控制方法不同的是，局部子圖方法在一次觀測中，只更新局部坐標(biāo)系中已觀測到的地標(biāo)，當(dāng)局部地圖中的信息融合進(jìn)全局地圖時，才會更新全局地圖中剩余地標(biāo)的狀態(tài)。文［6，7］已證明局部估計(jì)值與全局估計(jì)值是不相關(guān)的。

預(yù)測：

回顧二十多年的教學(xué)經(jīng)歷，自己一直以來都認(rèn)認(rèn)真真?zhèn)湔n，教案越寫越詳盡，課件越做越多，教學(xué)似乎變成了日復(fù)一日的重復(fù)，激情也慢慢隨著時間的推移消失了。以往在語文教學(xué)中，我常常擔(dān)心講不深、講不透，喜歡在課堂上滔滔不絕，實(shí)際上學(xué)生不是在學(xué)語文，而是在學(xué)教師對課文的心得體會。事實(shí)證明，由于忽視了學(xué)生的主體地位，再加上學(xué)生自主閱讀課文時間太少，他們對課文本身認(rèn)識不真切，思考不深刻，因此教學(xué)效果并不好。而模塊教學(xué)的實(shí)施注定了教師再不能在課堂上滿堂灌了，必須把時間還給學(xué)生。在單元模塊教學(xué)中教師又該怎么做呢？

更新：

在探索過程中，無人機(jī)在局部子地圖中運(yùn)動，由于需要采取主動運(yùn)動方式創(chuàng)建局部地圖，即無人機(jī)在子地圖中通過優(yōu)化目標(biāo)函數(shù)選取控制輸入。我們采用第3節(jié)中設(shè)計(jì)的目標(biāo)函數(shù)，通過選擇使目標(biāo)函數(shù)最大的控制決策確定無人機(jī)下一步的運(yùn)動，并在運(yùn)動過程中創(chuàng)建局部子地圖，在恰當(dāng)?shù)臅r刻將現(xiàn)在局部子圖中的信息融合進(jìn)全局地圖，從而完成整個SLAM過程。

2.2 局部子地圖與全局地圖之間的融合

局部子地圖到全局地圖的映射關(guān)系為T（k），反之，全局地圖到子地圖的映射關(guān)系為T－1（k），則局部子地圖與全局地圖之間的融合關(guān)系為：

3 目標(biāo)函數(shù)的設(shè)計(jì)

改進(jìn)的多目標(biāo)函數(shù)包含下面四項(xiàng)：

（1）定位與作圖的精度：狀態(tài)方差矩陣P包含了無人機(jī)位姿和環(huán)境特征的不確定性。將矩陣P的跡作為對不確定性的定量描述，用表示。若Pmin表示P的下界［33］，則越大，定位與作圖的精度越高。

（2）相對信息增益：在EKF過程中，不確定性下降往往意味著SLAM的收斂。將C（P與的差值作為相對信息增益，于是，可以用表示不確定性下降的速度。

（3）未探索區(qū)域：前面（1）、（2）兩項(xiàng)趨向于最小化定位與作圖的精度，忽視了環(huán)境探索的完整性。為此，引入Snew／Ssensor表示探測環(huán)境的效率，引導(dǎo)無人機(jī)進(jìn)入未探索區(qū)域，其中，Snew表示在控制輸入下無人機(jī)已觀測的面積與現(xiàn)在觀測面積的交集，Ssensor表示機(jī)載傳感器掃掠的面積。

（4）導(dǎo)航代價(jià)：從盡量減小導(dǎo)航代價(jià)的角度講，需要最小化無人機(jī)每一時刻的運(yùn)動距離r，因此可以用1／r表示導(dǎo)航代價(jià)的大小。綜上，多目標(biāo)函數(shù)為：

其中，ω1、ω2、ω3和ω4為加權(quán)值，C（Pmin）為閾值。

通過最大化式（16）中的目標(biāo)函數(shù)J所確定的u就是最優(yōu)的控制輸入：

4 無人機(jī)簡化運(yùn)動模型

為驗(yàn)證提出的主動SLAM算法的有效性，采用簡化的無人機(jī)平面運(yùn)動模型［8］，如圖2所示。

圖2 無人機(jī)平面模型Fig.2 The planemodel of UAV

無人機(jī)的狀態(tài)Xv＝ [xyψ]T，其中，x、y為無人機(jī)在兩維平面中的位置，ψ為航向角；控制量u＝［Δd Δψ］T，其中，Δd為無人機(jī)一個時間步長的運(yùn)動距離，Δψ為方位變化量。各狀態(tài)的變化方程為：

無人機(jī)狀態(tài)方程為：

第i地標(biāo)的狀態(tài)為mi＝［mxmy］T，假設(shè)地標(biāo)是靜止的，則：

無人機(jī)與第i地標(biāo)的觀測方程為：

觀測量Zi，k為k時刻無人機(jī)對第i地標(biāo)觀測的視線角φi，k。

5 仿真試驗(yàn)與分析

仿真采用了節(jié)4中的無人機(jī)平面運(yùn)動模型，控制量為運(yùn)動步長和偏轉(zhuǎn)角。在無人機(jī)的運(yùn)動范圍內(nèi)，每一時刻從無人機(jī)所有可能的運(yùn)動步長和偏轉(zhuǎn)角中，選擇使得目標(biāo)函數(shù)J最大的偏轉(zhuǎn)角和運(yùn)動步長作為控制量。仿真條件如下：初始時刻，無人機(jī)位于坐標(biāo)原點(diǎn)并朝向 x軸正向，在（［－20，20］，［－20，20］）環(huán)境中分布著20個地標(biāo)。目標(biāo)函數(shù)中權(quán)值參數(shù)ω1、ω2、ω3和ω4都取為0.25。無人機(jī)的控制噪聲方差Q和傳感器的測量噪聲方差R分別為：

圖3和4分別給出了仿真運(yùn)行100步后，采用隨機(jī)SLAM和基于局部子圖的主動SLAM算法估計(jì)的無人機(jī)軌跡和地標(biāo)位置情況。其中，連續(xù)實(shí)線表示估計(jì)的無人機(jī)運(yùn)動軌跡，離散的“＋”點(diǎn)及周圍橢圓分別表示估計(jì)的地標(biāo)位置和不確定性，離散的“□”點(diǎn)表示真實(shí)地標(biāo)位置。從圖3和圖4對比中可以看出隨機(jī)運(yùn)動探測環(huán)境有限，且無人機(jī)的運(yùn)動混亂無規(guī)律，經(jīng)常會陷入某個區(qū)域而不能離開。而基于局部子圖的主動SLAM能夠克服這種情況，無人機(jī)的航跡是有序的，并且就觀測地標(biāo)數(shù)而言，也大大多于隨機(jī)控制SLAM的方法，這樣無人機(jī)可以探測到更大范圍的環(huán)境。因此基于局部子圖的無人機(jī)主動SLAM要明顯優(yōu)于隨機(jī)SLAM。

圖3 無人機(jī)隨機(jī)SLAM的結(jié)果Fig.3 The results of random SLAM for UAV

圖4 基于局部子圖的無人機(jī)主動SLAM的仿真結(jié)果Fig.4 The results of active SLAM based on local sub-map for UAV

6 結(jié) 論

局部子圖方法把主動SLAM策略和CLSF結(jié)合起來，利用最優(yōu)控制算法中的多目標(biāo)函數(shù)，使用現(xiàn)在的局部地圖信息，引導(dǎo)無人機(jī)探索未知區(qū)域。對于多無人機(jī)協(xié)作的SLAM，每個無人機(jī)分別主動構(gòu)建自己的局部地圖，通過CLSF將其融入到全局地圖中，大大提高了無人機(jī)探索環(huán)境的效率。因此，研究基于CLSF的多無人機(jī)主動SLAM，具有重要的價(jià)值。

［1］ Newman PM.On the structure and solution of the simultaneous localization and map building problem［D］.Australian：University of Sydney，1999.

［2］ 張恒，樊曉平.移動機(jī)器人同步定位與地圖構(gòu)建過程中的軌跡規(guī)劃研究［J］.機(jī)器人，2006，28（3）：285－290.

ZHANG Heng，F(xiàn)an Xiaoping.Research on trajectory planning in simultaneous localization and mapping technology formobile robot［J］.Robot，2006，28（3）：285－290.

［3］ Feder H，Leonard J，Smith C.Adaptive mobile robot navigation andmapping.International Journal of Robotics Research，1999，18（7）：650－668.

［4］ Tao T，Huang Y.Motion Planning for SLAM Based on Frontier Exploration［C］.Proceeding of IEEE International Conference on Mechatronics and Automation，Harbin，China，2007，2120－2125.

［5］ 苑晶.未知環(huán)境中移動機(jī)器人主動同時定位與地圖構(gòu)建技術(shù)研究［D］.南開大學(xué)，2007.

YUAN Jing.Active simultaneous localization and mapping technology for mobile robot in unknown environment［D］.NanKai University，2007.

［6］ Willianms S.Efficient solutions to autonomous mapping and navigation problems［D］.University of Sydney，2001.

［7］ Liu Y，Sun F.A solution to active simultaneouslocalization and mapping problem based on optimal control［C］.Proceeding of IEEE International Conference on Mechatronics and Automation，Harbin，2007：314－319.

［8］ 王希彬.無人機(jī)同時定位與地圖創(chuàng)建技術(shù)研究［D］.海軍航空工程學(xué)院，2012.

WANG Xibin.Research on simultaneous localization and mapping technology for UAV［D］.Naval Aeronautical and Astronautical University，2012.

楊祥紅 男（1978－），山東臨沂人，講師，主要研究方向?yàn)楸N戰(zhàn)術(shù)學(xué)。

王希彬 男（1985－），山東人，博士，講師，主要研究方向?yàn)閷?dǎo)航制導(dǎo)與控制。

Research on Active SLAM Technology For UAV

YANG Xianghong1，WANG Xibin2

（Naval Aeronautical and Astronautical University 1.Department of Military Education and Training；2.Department of Control Engineering，Yantai，264001，China）

To conquer the idle mapping problem of passive simultaneous localization and mapping for UAV，an active SLAM algorithm is proposed based on constrained local sub-map.First，local sub-map is built based on constrained local sub-map filtering，and then，the optimal solution is searched by the active SLAM objective function，and at last，the local sub-map and the global map are fused.Based on the simplified UAV planemotion model，this algorithm is simulated and tested.Simulation results show that this algorithm is effective and feasible.

UAV；active SLAM；constrained local sub-map filtering

V 24

A

國家自然科學(xué)基金（61473306）。