，， ，

(西北工業(yè)大學(xué) 自動化學(xué)院, 西安 710072)

0 引言

近年來，隨著旋翼無人機在軍事和民用的領(lǐng)域得到了廣泛的應(yīng)用，旋翼無人機導(dǎo)航算法也得到了深入的研究與探討。室外環(huán)境中，GPS和慣導(dǎo)系統(tǒng)組合導(dǎo)航算法在旋翼無人機組合導(dǎo)航系統(tǒng)中得到了良好的應(yīng)用[1]，然而在缺乏GPS信號的室內(nèi)環(huán)境或GPS信號受到干擾的情況下，需要其它的輔助或替代方法來實現(xiàn)精確的導(dǎo)航定位[2]。同步定位與地圖構(gòu)建算法(SLAM)指的是搭載特定傳感器的主體，在沒有環(huán)境先驗信息的情況下，在運動過程中建立起環(huán)境的模型，同時估計自己的運動[3]。因此，可以在組合導(dǎo)航系統(tǒng)引入SLAM算法實現(xiàn)在GPS信號缺失情況下的精確導(dǎo)航定位[4]。

SLAM起源于1986年在舊金山舉行的IEEE機器人與自動化會議，大多SLAM工作以激光雷達作為主要傳感器。SLAM算法在陸地、室內(nèi)和水下載體中的應(yīng)用已經(jīng)取得很大的進展，將其應(yīng)用于旋翼無人機是組合導(dǎo)航領(lǐng)域的一個新的研究方向[5]。慣性/SLAM導(dǎo)航系統(tǒng)應(yīng)用組合導(dǎo)航思想，將SLAM輸出的位置姿態(tài)估計與慣性測量單元(IMU)的測量相融合[6]，獲得全狀態(tài)估計，它不僅能夠達到較高的定位精度，導(dǎo)航參數(shù)的輸出頻率也得到統(tǒng)一。基于上述慣性/SLAM組合導(dǎo)航的特點，本文提出一種基于SLAM的組合導(dǎo)航算法，該算法首先利用掃描匹配的方法，將當(dāng)前幀與地圖之間的關(guān)系作為一個優(yōu)化問題尋找全局最優(yōu)解，然后采用組合導(dǎo)航算法融合慣性元件和掃描匹配算法輸出的導(dǎo)航參數(shù)，獲得精確的導(dǎo)航信息。最后通過仿真實驗驗證所設(shè)計組合導(dǎo)航系統(tǒng)穩(wěn)定性和有效性。

1 系統(tǒng)構(gòu)成

本文研究對象是旋翼無人機組合導(dǎo)航系統(tǒng)，該系統(tǒng)的主要組成有嵌入式計算機、慣性測量單元(加速度計、陀螺儀)、磁力計、單點測距激光雷達和二維激光雷達等，圖1是實驗平臺的實物圖。平臺選用SLAMTEC公司的Rplidar A2激光雷達，它可以實現(xiàn)對周圍環(huán)境的360度全方位掃描測距檢測，從而獲得周圍環(huán)境的輪廓圖[7]?？紤]到測量精度和成本，選用單點測距激光雷達TF mini作為高度計，TF mini基于ToF原理，可以實現(xiàn)穩(wěn)定的距離測量。

圖1 搭載激光雷達的實驗平臺

慣性測量單元和導(dǎo)航計算機構(gòu)成捷聯(lián)慣導(dǎo)系統(tǒng)，捷聯(lián)慣導(dǎo)系統(tǒng)可以輸出無人機姿態(tài)信息和位置信息。磁力計可以解算出姿態(tài)角信息。激光雷達數(shù)據(jù)經(jīng)過掃描匹配后可以獲得旋翼無人機的位置信息，通過聯(lián)邦卡爾曼濾波的方法對捷聯(lián)慣導(dǎo)系統(tǒng)和激光雷達的數(shù)據(jù)進行融合，更新無人機狀態(tài)，獲得無人機精確的位姿估計。系統(tǒng)原理圖如圖2所示。

圖2 系統(tǒng)原理圖

2 基于掃描匹配的SLAM算法

幀到地圖全局掃描匹配算法是一種相對定位過程[8]，假設(shè)一個旋翼無人機在平面上從位置x0移動到x1，在位置x0和x1，它會分別接受到激光雷達掃描到的點云S0和S1。掃描匹配的過程就是找到一個剛體轉(zhuǎn)換ξ，使得t時刻點云St能與t-1時刻點云St-1匹配。只考慮二維平面情況下，可以用ξ=(px,py,ψ)T表示旋翼無人機的位置平移量和航向角。

高斯-牛頓迭代算法可以用于求解一個最優(yōu)值，該方法最早出現(xiàn)在計算機視覺研究中[9]，它是以梯度下降法為基礎(chǔ)的算法。梯度下降法又稱最速下降法[10]，這種算法計算量小，能迅速找到最優(yōu)解，因此在很多大規(guī)模數(shù)據(jù)集的處理上都會運用到梯度下降法或其改進算法。

考慮激光雷達工作的二維平面，高斯-牛頓方法的基本思想是通過多次迭代，尋找出一個最優(yōu)解ξ=(px,py,ψ)T，使得對應(yīng)的這幀的激光雷達點云在柵格地圖上表示的概率值的殘差之和ξ*最小，其中ξ*表示為：

(1)

這里Si(ξ)是激光掃描點集si=(si,x,si,y)T，它們都是ξ的函數(shù)。n為一幀激光雷達點云中點的總個數(shù)。將激光雷達掃描點的全部坐標(biāo)si=(si,x,si,y)T轉(zhuǎn)換到導(dǎo)航坐標(biāo)系坐標(biāo)Si(ξ)，激光掃描點集在在導(dǎo)航坐標(biāo)系下位姿由下式表示：

(2)

Si(ξ)對應(yīng)的位姿在柵格地圖中的映射的概率值可以表示為M(Si(ξ))，接下來開始求解最優(yōu)迭代步長Δξ，求解公式如下：

(3)

首先將函數(shù)M(Si(ξ+Δξ))進行一階泰勒展開：

(4)

其次，要求出Δξ取何值使函數(shù)(4)的值最小，需要對Δξ求導(dǎo)并且令導(dǎo)數(shù)等于 0，即：

(5)

根據(jù)公式(5)求解Δξ的的過程，引入高斯牛頓方程求解最優(yōu)步長：

(6)

式中,

(7)

H叫Hessian矩陣，對公式(2)求導(dǎo)可得到：

(8)

在柵格地圖中應(yīng)用雙線性插值算法，地圖梯度▽M(Si(ξ))可以表示為：

(9)

已知▽M(Si(ξ))和?Si(ξ)/?ξ，則可以估計高斯牛頓迭代的最優(yōu)步長Δξ。累加獲得的最優(yōu)步長Δξ，從而可以得到旋翼無人機在以起始點為原點的北東地導(dǎo)航坐標(biāo)系中的二維位置。

通過基于掃描匹配SLAM算法可以對旋翼無人機二維位置進行更新，同時，由單點測距激光雷達構(gòu)成的高度計可以對旋翼無人機的高度進行更新，因此，由激光雷達和高度計組成的SLAM組件實現(xiàn)了對旋翼無人機三維位置估計。

3 組合導(dǎo)航算法設(shè)計

為了提高多旋翼無人機導(dǎo)航定位的精度和可靠性，本節(jié)應(yīng)用組合導(dǎo)航理論設(shè)計導(dǎo)航系統(tǒng)，在上節(jié)得到SLAM組件輸出的位置信息的基礎(chǔ)上，融合捷聯(lián)慣導(dǎo)系統(tǒng)輸出信息，設(shè)計基于聯(lián)邦濾波組合導(dǎo)航算法。

捷聯(lián)慣導(dǎo)系統(tǒng)模型中，將姿態(tài)誤差角、位置誤差、速度誤差、陀螺儀以及加速度計測量誤差作為狀態(tài)量[11]，得到捷聯(lián)慣導(dǎo)系統(tǒng)的誤差模型[12]。針對旋翼無人機自身特點與應(yīng)用環(huán)境，采用簡化的捷聯(lián)慣導(dǎo)系統(tǒng)誤差模型進行卡爾曼濾波算法的設(shè)計，簡化后的捷聯(lián)慣導(dǎo)系統(tǒng)模型如公式(10)所示。

(10)

將慣性傳感器測量誤差模型從捷聯(lián)慣導(dǎo)系統(tǒng)的誤差模型中提取出來，它的數(shù)學(xué)表達式為：

(11)

通過上式可以看出，可以通過一個單獨設(shè)計的卡爾曼濾波器對姿態(tài)角誤差和陀螺儀誤差進行估計。本節(jié)使用加速度計、陀螺儀和磁力計組成一個聯(lián)邦濾波子系統(tǒng)，將該子系統(tǒng)稱為子濾波器1，用來對姿態(tài)角誤差和陀螺儀測量誤差進行估計。

在子濾波器1中，只有對姿態(tài)角誤差和陀螺儀誤差的觀測量，所以子濾波器1估計的狀態(tài)量也只包含姿態(tài)角誤差和陀螺儀誤差，這樣可以有效降低狀態(tài)維數(shù)，提高運算速度。

SLAM組件、陀螺儀和加速度計組成聯(lián)邦濾波器的另一個濾波子系統(tǒng)，將該子系統(tǒng)稱為子濾波器2，公式(10)通過子濾波器2進行狀態(tài)量的估計。分別通過子濾波器1和子濾波器2對捷聯(lián)慣導(dǎo)系統(tǒng)的誤差進行估計，大大提高了組合導(dǎo)航算法的精度。

綜上，本節(jié)所采用的的聯(lián)邦濾波器結(jié)構(gòu)設(shè)計如圖3所示。

圖3 聯(lián)邦濾波器結(jié)構(gòu)設(shè)計圖

子濾波器1包含加速度計、陀螺儀以及磁力計3種傳感器?？柭鼮V波器1的量測量為加速度計、磁力計解算的姿態(tài)角和組合導(dǎo)航算法給出的當(dāng)前時刻姿態(tài)角的差值。子濾波器1結(jié)構(gòu)如圖4所示。

圖4 子濾波器1結(jié)構(gòu)圖

子濾波器1中，選取包含姿態(tài)角誤差和陀螺儀誤差的六維向量作為狀態(tài)方程的狀態(tài)量。狀態(tài)方程為:

(12)

根據(jù)加速度計和磁力計解算旋翼無人機姿態(tài)角方法，將組合導(dǎo)航算法輸出姿態(tài)角與傳感器解算姿態(tài)角之差作為子濾波器1的量測量。則子濾波器1的量測方程有如下形式：

(13)

子濾波器2包含陀螺儀、加速度計以及SLAM組件。首先利用SLAM組件獲得導(dǎo)航坐標(biāo)系中的位置信息，子濾波器2以SLAM組件輸出的位置與導(dǎo)航算法給出的當(dāng)前位置的差值作為位置誤差的量測信息。子濾波器2結(jié)構(gòu)如圖5所示。

圖5 濾波子系統(tǒng)2結(jié)構(gòu)圖

子濾波器2中，選取選取姿態(tài)誤差角、位置誤差、速度誤差、陀螺儀以及加速度計測量誤差共15維的變量作為卡爾曼濾波器狀態(tài)量。根據(jù)公式(10)，子濾波器2狀態(tài)方程可以表示為：

(14)

由于子濾波器2使用SLAM組件輸出的位置信息與組合導(dǎo)航算法給出的位置之差作為子濾波器2的量測量，而且SLAM組件給出的數(shù)據(jù)本身就是離散形式，因此子濾波器2的量測方程可以直接用離散化的形式表示：

(15)

(16)

主濾波器對兩個子濾波器中的公共狀態(tài)量進行融合，比較兩個濾波子系統(tǒng)的狀態(tài)變量可知，兩個子濾波器的公共狀態(tài)為：

(17)

本文中聯(lián)邦濾波器公共狀態(tài)融合算法如下：

(18)

由于子濾波器2中的獨有狀態(tài)可以直接作為主濾波器的狀態(tài)量輸出，因此主濾波器輸出的狀態(tài)量為：

(19)

公式(18)和公式(19)中的下標(biāo)[a,b]表示只取相應(yīng)量對應(yīng)部分的值。

同時，需要將主濾波器狀態(tài)量中的陀螺儀誤差和加速度計誤差部分反饋到慣性導(dǎo)航系統(tǒng)中，作為導(dǎo)航系統(tǒng)的更新中的一部分，從而減小導(dǎo)航參數(shù)更新時的誤差。

4 仿真實驗分析

4.1 實驗數(shù)據(jù)的采集

利用圖1所示的實驗平臺，使飛控組件和SLAM組件同時采集陀螺儀、加速度計和磁力計輸出信息和SLAM組件輸出的位置和高度信息，將采集到的數(shù)據(jù)通過MATLAB對所設(shè)計的聯(lián)邦濾波算法進行仿真驗證。

數(shù)據(jù)采集環(huán)境為西北工業(yè)大學(xué)自動化學(xué)院樓走廊，實驗環(huán)境如圖6所示，圖中線為遙控的實驗平臺運動路線，白色圓圈處為起點位置，深色圓圈處為終點位置。

圖6 數(shù)據(jù)采集環(huán)境

基于掃描匹配的SLAM算法在嵌入計算機TK1上實時輸出旋翼無人機的二維位置，高度信息由TF mini直接輸出并存儲。二維位置數(shù)據(jù)記錄在.rosbag文件中，通過在PC平臺上對rosbag文件進行回放，將/pose輸出位置信息生成位置信息文件，在MALTAB中繪制建圖過程中實驗平臺軌跡圖如圖7，圖中坐標(biāo)系為以起點處位置為原點的北東地導(dǎo)航坐標(biāo)系。

圖7 實驗平臺運動軌跡

觀察圖7中的實驗平臺軌跡，實驗平臺位置的估計軌跡同遙控給出的運動指令基本一致，只是局部由于實驗平臺運動過程中的抖動出現(xiàn)了位置的畸變，說明通過基于掃描匹配的SLAM算法能夠?qū)崿F(xiàn)有效的同步定位與室內(nèi)建圖。

4.2 仿真結(jié)果與分析

4.2.1 姿態(tài)角濾波結(jié)果分析

根據(jù)圖8可以看出，使用基于聯(lián)邦濾波組合導(dǎo)航算法之后，姿態(tài)角被鉗制在一個很小的范圍內(nèi)，有效抑制了姿態(tài)角的發(fā)散，為速度和位置的解算提供了保障。

圖8 聯(lián)邦濾波器解算姿態(tài)角

4.2.2 速度濾波結(jié)果分析

觀察圖9，使用基于聯(lián)邦濾波組合導(dǎo)航算法之后，北向速度首先穩(wěn)定在0.2 m/s左右,442 s以后北向速度回歸到0值左右。東向速度首先在0值左右浮動，442 s后逐漸在[-0.1,-0.2]區(qū)間變化，地向速度由于采樣設(shè)備抖動，其值在[-0.4,0.4]區(qū)間變化。三個方向的速度變化趨勢基本同采樣時無人機運動先北向運動后西向運動的運動趨勢相吻合。

圖9 聯(lián)邦濾波器解算速度

4.2.3 位置濾波結(jié)果分析

對比圖10中的聯(lián)邦濾波器曲線和SLAM組件曲線可以看出，位置數(shù)據(jù)整體比較接近，SLAM輸出數(shù)據(jù)噪聲較大，經(jīng)測量北向和東向位置誤差值可達50 mm，經(jīng)過聯(lián)邦濾波器濾波有效濾除噪聲，濾波后北向和東向位置噪聲值不超過20 mm，提高了導(dǎo)航數(shù)據(jù)的精度。受采樣設(shè)備抖動的影響，SLAM組件輸出的地向位置抖動幅度較大，聯(lián)邦濾波器能夠有效跟蹤地向位置的變化，經(jīng)測量濾波后地向位置誤差在40 mm以內(nèi)，能夠滿足旋翼無人機室內(nèi)導(dǎo)航的需求。

圖10 聯(lián)邦濾波器解算位置

5 結(jié)論

本研究針對旋翼無人機組合導(dǎo)航系統(tǒng)，引入基于掃描匹配的SLAM算法，提出一種基于SLAM的旋翼無人機組合導(dǎo)航算法。通過仿真實驗，將組合導(dǎo)航算法與單獨的SLAM算法進行對比，實驗結(jié)果表明了基于SLAM的組合導(dǎo)航算法的可行性與優(yōu)越性，為旋翼無人機在未知環(huán)境中的自主飛行提供了更精確的定位方法。以后將在論文基礎(chǔ)上，繼續(xù)研究旋翼無人機的自主導(dǎo)航工作。