張燁林, 王養(yǎng)柱, 韓 震

(北京航空航天大學(xué)自動(dòng)化科學(xué)與電氣工程學(xué)院,北京 100191)

基于FastSLAM算法的無(wú)人機(jī)著陸方法

張燁林, 王養(yǎng)柱, 韓 震

(北京航空航天大學(xué)自動(dòng)化科學(xué)與電氣工程學(xué)院,北京 100191)

為解決無(wú)人機(jī)在GPS信號(hào)失效下的著陸導(dǎo)航問(wèn)題，采用基于FastSLAM算法的導(dǎo)航方法。首先，基于機(jī)載多普勒雷達(dá)測(cè)量的地速，用含有高斯噪聲的非線性方程來(lái)描述無(wú)人機(jī)著陸的運(yùn)動(dòng)模型；搭載在無(wú)人機(jī)上的激光雷達(dá)測(cè)量相對(duì)環(huán)境路標(biāo)的位置和方位角用于構(gòu)建系統(tǒng)的觀測(cè)模型。基于無(wú)人機(jī)著陸段的系統(tǒng)模型，采用粒子濾波估計(jì)無(wú)人機(jī)的路徑，擴(kuò)展卡爾曼濾波估計(jì)環(huán)境路標(biāo)位置。獲得無(wú)人機(jī)的路徑估計(jì)后,采用反正切形式的跟蹤微分器獲得速度估計(jì)。仿真驗(yàn)證了在選取適當(dāng)?shù)牧Ｗ訑?shù)和環(huán)境路標(biāo)情況下，F(xiàn)astSLAM算法滿足無(wú)人機(jī)著陸定位精度和實(shí)時(shí)性要求，是一種可行的著陸段導(dǎo)航方法。

無(wú)人機(jī)著陸； FastSLAM； 粒子濾波； 擴(kuò)展卡爾曼濾波

0 引言

著陸是無(wú)人機(jī)飛行過(guò)程中非常重要的階段，因而，高精度的導(dǎo)航系統(tǒng)是安全著陸的重要保證。目前無(wú)人機(jī)著陸使用的導(dǎo)航技術(shù)主要包括慣性導(dǎo)航系統(tǒng)、GPS導(dǎo)航系統(tǒng)、慣性/GPS組合導(dǎo)航系統(tǒng)、計(jì)算機(jī)視覺(jué)導(dǎo)航系統(tǒng)等。慣性導(dǎo)航屬于自主導(dǎo)航，可以提供完備的導(dǎo)航信息，但其導(dǎo)航誤差隨時(shí)間積累。GPS導(dǎo)航具有精度高的優(yōu)點(diǎn)，但如果在著陸過(guò)程中受到干擾會(huì)影響導(dǎo)航精度。計(jì)算機(jī)視覺(jué)具有大視場(chǎng)、非接觸的優(yōu)點(diǎn)，但是圖像處理需要大量計(jì)算和存儲(chǔ)，在著陸過(guò)程中實(shí)時(shí)性難以保證。近年來(lái)，環(huán)境地圖創(chuàng)建和同步定位(SLAM)技術(shù)迅速發(fā)展，由于SLAM技術(shù)定位精度高，它已成為移動(dòng)機(jī)器人導(dǎo)航研究熱點(diǎn)[1-4]，并已越來(lái)越多地?cái)U(kuò)展到無(wú)人機(jī)導(dǎo)航研究中[5-9]。

將SLAM技術(shù)擴(kuò)展到三維空間應(yīng)用于無(wú)人機(jī)導(dǎo)航是新的研究方向。文獻(xiàn)[7]提出了一種通過(guò)機(jī)載相機(jī)定位無(wú)人機(jī)的技術(shù)，視覺(jué)里程計(jì)集成到SLAM框架下用于減少里程計(jì)累計(jì)誤差的影響；文獻(xiàn)[8]中，小型直升機(jī)依靠機(jī)載慣性傳感器和單目攝像機(jī)，采用SLAM算法進(jìn)行導(dǎo)航，克服了慣性器件漂移的影響和對(duì)GPS的依賴(lài)；文獻(xiàn)[10]提出了一種小型無(wú)人機(jī)的FastSLAM算法，并通過(guò)仿真驗(yàn)證了FastSLAM算法具有比EKF-SLAM算法更高的定位精度；文獻(xiàn)[10-11]研究表明，在四旋翼無(wú)人機(jī)的飛行過(guò)程中采用FastSLAM技術(shù)可以獲得較高定位精度，然而，其僅基于四旋翼無(wú)人機(jī)，并非針對(duì)著陸過(guò)程。本文將FastSLAM算法用于固定翼無(wú)人機(jī)著陸階段，通過(guò)仿真驗(yàn)證了在選取合適粒子數(shù)和環(huán)境路標(biāo)的情況下，該算法能夠滿足無(wú)人機(jī)著陸定位精度和實(shí)時(shí)性的要求，是一種可行的著陸導(dǎo)航方法。

1 無(wú)人機(jī)著陸軌跡與系統(tǒng)模型

1.1 無(wú)人機(jī)著陸軌跡

無(wú)人機(jī)著陸過(guò)程包含下滑段、拉平段和地面滑跑段，進(jìn)場(chǎng)著陸縱向軌跡如圖1所示。

圖1 無(wú)人機(jī)著陸軌跡Fig.1 UAV landing trajectory

圖中：AB段為下滑段，無(wú)人機(jī)以恒定姿態(tài)角做勻速直線運(yùn)動(dòng)；γ1為航跡傾角；H0為下滑段起點(diǎn)高度；R0為下滑起點(diǎn)的待飛距離；H1為拉平起點(diǎn)高度；BC段為拉平段，軌跡為指數(shù)形式；E為拉平段任意一點(diǎn)，其高度和距離著陸點(diǎn)水平距離的關(guān)系為

H=a1exp(a2R)+a3

(1)

式中的參數(shù)及更多軌跡細(xì)節(jié)可參考文獻(xiàn)[12]。

1.2 無(wú)人機(jī)著陸段系統(tǒng)模型

無(wú)人機(jī)狀態(tài)x=[x,y,z,ψ,θ]T。其中，x,y,z分別表示地面坐標(biāo)系下x軸，y軸，z軸坐標(biāo)位置。如圖2所示，ψ,θ分別表示速度矢量與x軸，xoy平面的夾角。

圖2 ψ和θ示意圖Fig.2 Schematic diagram of ψ and θ

運(yùn)動(dòng)模型和觀測(cè)模型可以用含有高斯噪聲的非線性方程表示為[10]

(2)

z=h(x(t),m(t),t)+εt

(3)

式中：δt表示運(yùn)動(dòng)方程的噪聲項(xiàng),噪聲的協(xié)方差矩陣為Qt；εt為量測(cè)噪聲,Rt為噪聲的協(xié)方差矩陣；u(t)=v為控制量，v為機(jī)載多普勒雷達(dá)獲得的無(wú)人機(jī)地速大小。在無(wú)人機(jī)著陸軌跡上選擇若干個(gè)航點(diǎn)，初始待達(dá)航點(diǎn)為第一個(gè)航點(diǎn)。若無(wú)人機(jī)當(dāng)前位置和待達(dá)航點(diǎn)之間距離小于閾值Rmin,則待達(dá)航點(diǎn)變?yōu)橄乱粋€(gè)航點(diǎn)。式(2)寫(xiě)成離散形式為

(4)

式中，速度方位角增量Δψ,Δθ分別定義為

(5)

(6)

其中，(wpx,wpy,wpz)為待達(dá)航點(diǎn)的坐標(biāo)。

環(huán)境中分布有環(huán)境路標(biāo)，無(wú)人機(jī)在著陸過(guò)程中使用激光雷達(dá)獲得相對(duì)環(huán)境路標(biāo)的距離和方位角。激光雷達(dá)的量測(cè)向量是z=[r,α,β]T，具體形式為

(7)

式中：(xm,ym,zm)為環(huán)境路標(biāo)的坐標(biāo)；r為無(wú)人機(jī)和環(huán)境路標(biāo)之間的距離；α,β為無(wú)人機(jī)與環(huán)境路標(biāo)之間位置矢量的方位角。

2 無(wú)人機(jī)著陸段FastSLAM算法設(shè)計(jì)

無(wú)人機(jī)著陸段FastSLAM問(wèn)題可以描述為根據(jù)環(huán)境路標(biāo)的量測(cè)信息zt=[z1,…,zt]和無(wú)人機(jī)速度的控制信息ut=[u1,…,ut]來(lái)估計(jì)無(wú)人機(jī)著陸路徑xt和所有環(huán)境路標(biāo)m位置的過(guò)程[13]，用概率形式描述為

p(xt,m|zt,ut,nt)

(8)

式中：nt=[n1,…,nt]為數(shù)據(jù)關(guān)聯(lián)；m={m1,…,mN}表示N個(gè)環(huán)境路標(biāo)構(gòu)成的地圖。根據(jù)SLAM問(wèn)題的條件獨(dú)立特性，式(8)可以表示為[13]

(9)

用概率形式重新表示無(wú)人機(jī)著陸系統(tǒng)模型和觀測(cè)模型，可以分別表示為

p(xt|xt-1,ut)=f(xt-1,ut)+δt

(10)

p(zt|xt,m,nt)=h(x(t),mnt)+εt 。

(11)

基于FastSLAM算法的著陸導(dǎo)航過(guò)程可以分為采樣新?tīng)顟B(tài)，更新環(huán)境路標(biāo)，計(jì)算粒子權(quán)重并重采樣[14]，算法流程如圖3所示。

圖3 FastSLAM算法流程圖Fig.3 FastSLAM flow chart

2.1 采樣新?tīng)顟B(tài)

以第k個(gè)粒子為例，新?tīng)顟B(tài)要從后驗(yàn)概率p(xt|xt-1,[k],zt,ut,nt)中采樣[15]，即

x[k]～p(xt|xt-1,[k],zt,ut,nt)

(12)

根據(jù)貝葉斯公式和馬爾可夫法則，可得

p(xt|xt-1,[k],zt,ut,nt)=λp(zt|xt,xt-1,[k],zt-1,ut,nt)p(xt|xt-1,[k],zt-1,ut,nt)

(13)

式中，λ為歸一化常數(shù)。事實(shí)上，t時(shí)刻的狀態(tài)是由t-1時(shí)刻的狀態(tài)和t時(shí)刻的控制量確定，因此式(13)可以轉(zhuǎn)化為

(14)

由于無(wú)法直接對(duì)式(14)進(jìn)行采樣，因此可以對(duì)觀測(cè)方程式(11)進(jìn)行線性化處理近似該建議分布。對(duì)式(11)進(jìn)行泰勒展開(kāi)可獲得

(15)

p(xt|xt-1,[k],zt,ut,nt)=κexp(-ζt)

(16)

(17)

根據(jù)高斯概率分布的相關(guān)理論，可得

(18)

(19)

從

(20)

的高斯分布中采樣獲得新?tīng)顟B(tài)。

2.2 更新環(huán)境路標(biāo)

計(jì)算路標(biāo)估計(jì)的后驗(yàn)概率是依據(jù)t時(shí)刻路標(biāo)mnt是否被觀測(cè)到，若沒(méi)有被觀測(cè)到，則后驗(yàn)概率保持不變，否則根據(jù)貝葉斯公式和馬爾可夫法則進(jìn)行計(jì)算[15]，即

p(mn≠nt|xt,zt,ut,nt)=p(mn≠nt|xt-1,zt-1,ut-1,nt-1)

(21)

p(mn=nt|xt,zt,ut,nt)∝p(zt|mnt,xt,zt-1,ut,nt)·p(mnt|xt,zt-1,ut,nt)=p(zt|mnt,xt,nt)·p(mnt|xt-1,zt-1,ut-1,nt-1)

(22)

采用一階線性化方法對(duì)觀測(cè)方程進(jìn)行近似，即

(23)

(24)

(25)

。

(26)

2.3 計(jì)算粒子權(quán)重系數(shù)并進(jìn)行重采樣

權(quán)重系數(shù)定義為目標(biāo)分布和建議分布之比[15]，可得

(27)

根據(jù)貝葉斯公式和馬爾可夫法則、卷積定理，權(quán)重系數(shù)可進(jìn)一步表達(dá)為

(28)

其中

(29)

為中間變量。

隨著迭代進(jìn)行，會(huì)出現(xiàn)“粒子退化”現(xiàn)象，為克服粒子退化，需對(duì)概率密度函數(shù)進(jìn)行重采樣，權(quán)重系數(shù)較小的粒子被去除，保留并復(fù)制權(quán)重系數(shù)較大的粒子[17]。

2.4 獲得速度估計(jì)

FastSLAM算法可以獲得無(wú)人機(jī)位置估計(jì)，分別讓三軸位置估計(jì)通過(guò)跟蹤微分器獲得無(wú)人機(jī)的速度估計(jì)。由于反正切形式的跟蹤微分器可以兼顧跟蹤快速性與過(guò)渡過(guò)程平穩(wěn)性，待調(diào)節(jié)參數(shù)較少[18]，因此，可以用反正切形式的跟蹤微分器獲得無(wú)人機(jī)的速度估計(jì)，即

(30)

式中：a1>0,a2>0,l1>0,l2>0,R>0均為待設(shè)計(jì)參數(shù)；v(t)為輸入信號(hào)；x1(t)和x2(t)分別為其跟蹤和微分信號(hào)。

3 仿真驗(yàn)證及分析

在Matlab仿真環(huán)境下對(duì)基于FastSLAM算法的無(wú)人機(jī)著陸過(guò)程進(jìn)行仿真。在下滑段，下滑點(diǎn)高度H0=100 m,航跡傾角γ1=-3.5°，升降速度Vz=-2 m/s,在拉平段，接地點(diǎn)高度H2=0.7 m，升降速度Vz=-0.08 m/s,拉平末端航跡傾角γ2=-1°,下滑點(diǎn)的待飛距離R0=1 985.1 m，環(huán)境路標(biāo)數(shù)分別取69和134，無(wú)人機(jī)速度控制噪聲為±0.3 m/s，角度控制噪聲為±0.3°,系統(tǒng)采樣周期為0.05 s，機(jī)載激光雷達(dá)最大探測(cè)距離為50 m,觀測(cè)距離噪聲為±0.1 m,觀測(cè)角度噪聲為±0.3°,觀測(cè)周期為0.4 s。粒子數(shù)分別取12,18,24。跟蹤微分器參數(shù)分別設(shè)計(jì)如下:Rx=10,a1x=2,l1x=3,a2x=2,l2x=3；Ry=20,a1y=2,l1y=3,a2y=2,l2y=3；Rz=5,a1z=2,l1z=3,a2z=2,l2z=3。在著陸軌跡上選取12個(gè)航點(diǎn)，如圖4所示。

圖4 基于FastSLAM算法的無(wú)人機(jī)著陸示意圖Fig.4 Diagram of UAV landing based on FastSLAM

為了進(jìn)一步考察FastSLAM算法中粒子數(shù)量以及環(huán)境路標(biāo)數(shù)量對(duì)估計(jì)結(jié)果的影響，進(jìn)行兩組仿真：1)分別取12,18,24個(gè)粒子，在相同的134個(gè)環(huán)境路標(biāo)條件下進(jìn)行仿真；2)分別選取69,134個(gè)環(huán)境路標(biāo)，在18個(gè)粒子情況下進(jìn)行仿真。結(jié)果如圖5～圖9所示。

圖5 位置估計(jì)誤差Fig.5 Position estimation errors

1) 粒子數(shù)量對(duì)路徑估計(jì)誤差的影響。

如圖5所示，無(wú)人機(jī)的側(cè)偏距(X軸誤差)和高度誤差(Z軸誤差)均保持在1 m范圍內(nèi)，可以滿足無(wú)人機(jī)著陸階段的導(dǎo)航精度要求。增加粒子數(shù)可以增加路徑選擇樣本，更多的粒子采樣有利于逼近無(wú)人機(jī)的真實(shí)路徑，從而降低路徑估計(jì)誤差。

2) 粒子數(shù)量對(duì)速度估計(jì)誤差的影響。

不同粒子數(shù)目條件下的速度估計(jì)誤差如圖6所示，在著陸過(guò)程中，3軸的速度估計(jì)誤差均在接近0的較小范圍內(nèi)。升降率比較如圖7所示，升降率估計(jì)和真實(shí)值之間誤差較小，無(wú)人機(jī)著陸過(guò)程中下滑段升降率恒定，拉平段升降率逐漸降低到接近0。

圖6 速度估計(jì)誤差 Fig.6 Velocity estimation errors

圖7 Z軸速度估計(jì)與Z軸理想速度比較Fig.7 Comparison between estimated and ideal velocity of Z axis

3) 環(huán)境路標(biāo)數(shù)量對(duì)位置估計(jì)誤差的影響。

分析圖8可知，環(huán)境路標(biāo)數(shù)量增加，無(wú)人機(jī)可以獲得更多的觀測(cè)量，有利于降低無(wú)人機(jī)的位置估計(jì)誤差。

為驗(yàn)證地圖的收斂性，作出環(huán)境路標(biāo)協(xié)方差陣跡的曲線，如圖9所示。每次觀測(cè)到新的環(huán)境路標(biāo)后，環(huán)境路標(biāo)協(xié)方差陣的跡會(huì)增大，表明地圖不確定性都會(huì)增大，然后隨著無(wú)人機(jī)的運(yùn)動(dòng)而逐漸減小，收斂到0。

圖8 不同路標(biāo)數(shù)量情況下的位置估計(jì)誤差Fig.8 Position estimation errors under different landmark number

圖9 路標(biāo)協(xié)方差陣跡Fig.9 Landmark’s trace of covariance matrix

為驗(yàn)證算法實(shí)時(shí)性，對(duì)上述算例中134個(gè)路標(biāo)、不同粒子數(shù)情況下的運(yùn)行時(shí)間進(jìn)行統(tǒng)計(jì)，結(jié)果見(jiàn)表1。表1系統(tǒng)采樣周期為0.05 s，觀測(cè)間隔為0.4 s，即第8個(gè)采樣周期獲得觀測(cè)量,其中，單步運(yùn)行時(shí)間1是獲得觀測(cè)量的單步運(yùn)行時(shí)間，單步運(yùn)行時(shí)間2是未獲得觀測(cè)量的單步運(yùn)行時(shí)間?？梢钥闯?，上述3種算例單步運(yùn)行時(shí)間均小于系統(tǒng)采樣間隔，可以滿足實(shí)時(shí)性的要求。

表1 不同粒子數(shù)情況下平均仿真時(shí)間步長(zhǎng)比較

4 結(jié)束語(yǔ)

本文針對(duì)無(wú)人機(jī)著陸過(guò)程，采用基于FastSLAM算法的導(dǎo)航方法，從仿真結(jié)果可以看出，當(dāng)選取適當(dāng)?shù)牧Ｗ訑?shù)和環(huán)境路標(biāo)，基于FastSLAM算法的無(wú)人機(jī)著陸方法，能夠保證無(wú)人機(jī)著陸段的定位精度要求和實(shí)時(shí)性要求，是一種有效的著陸段導(dǎo)航方法。

[1] SAEEDI S,TRENTINI M,SETO M,et al.Multiple-robot si-multaneous localization and mapping:a review[J].Journal of Field Robotics,2016,33(1):3-46.

[2] SAEEDI S,PAUL L,TRENTINI M,et al.Occupancy grid map merging for multiple robot simultaneous location and mapping[J].International Journal of Robotics & Automation,2015,30(2):149-157.

[3] HOWARD A.Multi-robot simultaneous location and mapping using particle filters[J].International Journal of Robotics Research,2006,25(12):1243-1256.

[4] CHATTERJEE A,RAY O,CHATTERJEE A,et al.Deve-lopment of a real-life EKF based SLAM system for mobile robots employing vision sensing[J].Expert Systems with Application,2011,38(7):8266-8274.

[5] HENG L,LEE G H,POLLEFEYS M.Self-calibration and visual SLAM with a multi-camera system on a micro aerial vehicle[J].Autonomous Robots,2015,39(3):259-277.

[6] WEISS S,SCRAMUZZA D,SIEGWART R.Monocular-SLAM-based navigation for autonomous micro helicopters in GPS-denied environments[J].Journal of Field Robo-tics,2011,28(6):854-874.

[7] CABALLERO F,MERINO L,FERRUZ J,et al.Vision-based odometry and SLAM for medium and high altitude flying UAVs[J].Journal of Intelligent and Robotic Systems,2009,54(1):137-161.

[8] ACHTELIK M,BACHRACH A,HE R,et al.Stereo vision and laser odometry for autonomous helicopters in GPS-denied indoor environments[C]//Proceedings of the SPIE Unmanned Systems Technology XI,Orlando,FL,2009:733219-1-733219-10.

[9] HE B,YING L L,ZHANG S J,et al.Autonomous navigation based on unscented-FastSLAM using particle swarm optimization for autonomous underwater vehicles[J].Measurement,2015,71:89-101.

[10] 叢楚瀅,王從慶,丁臻極,等.一種小型無(wú)人機(jī)的FastSLAM算法[J].華中科技大學(xué)學(xué)報(bào):自然科學(xué)版,2015,43(s1):420-425,427.

[11] 王偉,陳華慶,韓衛(wèi).無(wú)人機(jī)自主導(dǎo)航控制的FastSLAM算法研究[J].計(jì)算機(jī)仿真,2015,32(8):68-72.

[12] LIU Z,WANG Y,HAO X W.Coordinated landing control of unmanned aerial vehicle[C]//International Conference on Electronics,Communications & Control,2011:1965-1970.

[13] KIM A,EUSTICE R M.Perception-driven navigation:active visual SLAM for robotic area coverage [C]//International Conference on Robotics and Automation,2013:3196-3203.

[14] MONTEMERLO M.FastSLAM:a factored solution to the simultaneous localization and mapping problem with unknown data association [D].Pittsburgh:Carnegie Mellon University,2003.

[15] MONTEMERLO M,THRUN S.Simultaneous localization and mapping with unknown data association using FastSLAM[C]//International Conference on Robotics and Automation,2003:1985-1991.

[16] MONTEMERLO M,THRUN S,KOLLER D,et al.Fast-SLAM 2.0:an improved particle filtering algorithm for simultaneous localization and mapping that provably converges[C]//International Joint Conference on Artificial Intelligence,2003:1151-1156.

[17] BAILEY T,NIETO J,NEBOT E.Consistency of the FastSLAM algorithm[C]//International Conference on Robotics and Automation,2006:424-429.

[18] 董小萌,張平.反正切形式跟蹤微分器設(shè)計(jì)及相平面分析[J].控制理論與應(yīng)用,2010,27(4):533-537.

Research on UAV Landing Based on FastSLAM Algorithm

ZHANG Ye-lin, WANG Yang-zhu, HAN Zhen

(School of Automation Science and Electrical Engineering,Beijing University of Aeronautics and Astronautics, Beijing 100191,China )

In order to solve the problem of UAV’s autonomous landing when GPS signal is failed,a UAV landing method based on FastSLAM algorithm is adopted.First,based on the ground speed measured by Doppler radar,the motion model is described by nonlinear equation with Gaussian noise.The UAV uses its laser radar to obtain the distance and azimuth relative to environmental landmarks for constructing the system observation model.Path estimation and environmental landmark estimation are implemented by particle filter and extended Kalman filter respectively.The velocity estimation is obtained by using tracking differentiator of the arctangent form.Simulation results show that by choosing appropriate number of particles and environmental landmarks,FastSLAM algorithm can meet the requirement on landing precision and real-time performance,and is an effective landing navigation method.

UAV landing; FastSLAM; particle filter; extended Kalman filter

張燁林,王養(yǎng)柱,韓震.基于FastSLAM算法的無(wú)人機(jī)著陸方法[J].電光與控制，2017，24(9)：83-87,108.ZHANG Y L,WANG Y Z,HAN Z.Research on UAV landing based on FastSLAM algorithm[J].Electronics Optics & Control,2017,24(9):83-87,108.

2016-09-12

2017-06-11

張燁林(1993 —),男，江蘇徐州人，碩士生，研究方向?yàn)榻M合導(dǎo)航。

V249.3

A

10.3969/j.issn.1671-637X.2017.09.018