無(wú)人作戰(zhàn)平臺(tái)的同步定位與地圖創(chuàng)建研究

（91404部隊(duì) 秦皇島 066000）

1 引言

為打贏未來(lái)高技術(shù)條件下的局部戰(zhàn)爭(zhēng)，需建設(shè)具有高度機(jī)動(dòng)能力和毀傷力的無(wú)人作戰(zhàn)平臺(tái)。如今科學(xué)技術(shù)正朝著智能化、無(wú)人化、多系統(tǒng)化等方面發(fā)展，在國(guó)外尤其是科技力量比較發(fā)達(dá)的歐美國(guó)家涌現(xiàn)了大量的無(wú)人化設(shè)備和操縱平臺(tái)，例如無(wú)人機(jī)、水下智能機(jī)器人、水面無(wú)人船以及無(wú)人車(chē)等。我國(guó)對(duì)無(wú)人作戰(zhàn)平臺(tái)的研究很重視，已經(jīng)取得了一定的研究成果，但許多方面技術(shù)仍然與國(guó)際先進(jìn)水平有著一定的差距。無(wú)人作戰(zhàn)平臺(tái)實(shí)現(xiàn)智能化及自主化涉及多方面技術(shù)，其中自主導(dǎo)航是極為關(guān)鍵的技術(shù)，而無(wú)人作戰(zhàn)平臺(tái)在未知環(huán)境中的定位與地圖創(chuàng)建則是無(wú)人作戰(zhàn)平臺(tái)實(shí)現(xiàn)自主導(dǎo)航的基礎(chǔ)［1～2］。本文提出了一種基于擴(kuò)展擴(kuò)爾曼濾波的同步定位與地圖創(chuàng)建方法，基于該方法無(wú)人作戰(zhàn)平臺(tái)可以在未知環(huán)境中，只利用其自身攜帶的傳感器獲得的信息建立環(huán)境地圖，同時(shí)利用創(chuàng)建的地圖進(jìn)行自主定位。

2 同步定位與地圖創(chuàng)建概述

無(wú)人作戰(zhàn)平臺(tái)在未知環(huán)境中會(huì)遇到既無(wú)導(dǎo)航信號(hào)同時(shí)也無(wú)已知的環(huán)境地圖的情形，若只靠?jī)?nèi)部傳感器來(lái)獲得自身的位姿信息很容易出現(xiàn)累計(jì)誤差，那么無(wú)人作戰(zhàn)平臺(tái)就需要利用內(nèi)部傳感器來(lái)不斷地獲取環(huán)境信息，實(shí)時(shí)地創(chuàng)建特征地圖，并利用特征地圖反過(guò)來(lái)對(duì)定位信息進(jìn)行校正，實(shí)現(xiàn)精確定位，從而更加有效地完成任務(wù)。上述過(guò)程便是同步定位與地圖創(chuàng)建的常見(jiàn)過(guò)程，同步定位與地圖創(chuàng)建通常被稱(chēng)為SLAM（Simultaneous Localization and map Building）［3～6］。SLAM 問(wèn)題可以描述為運(yùn)動(dòng)學(xué)模型已知的無(wú)人作戰(zhàn)平臺(tái)以一個(gè)未知的姿態(tài)從原點(diǎn)出發(fā)，在一個(gè)充滿(mǎn)多障礙物的環(huán)境中運(yùn)動(dòng)，通過(guò)自身攜帶傳感器感知周?chē)h(huán)境特征，從而利用這些數(shù)據(jù)確定環(huán)境特征的坐標(biāo)與自身的位置和姿態(tài)。

同步定位與地圖創(chuàng)建一般而言有以下四個(gè)環(huán)節(jié)［7］：獲取傳感器信息、特征匹配、定位和地圖創(chuàng)建，其流程如圖1所示，傳感器測(cè)量得到的數(shù)據(jù)用Sk表示，第k-1時(shí)刻的局部地圖表示為T(mén)k-1，k時(shí)刻機(jī)器人的位姿信息表示為Mk。無(wú)人作戰(zhàn)平臺(tái)自身的位姿信息和環(huán)境特征信息通過(guò)傳感器獲取。同步定位與地圖創(chuàng)建的關(guān)鍵步驟是特征匹配，被用于判斷當(dāng)前環(huán)境特征和已有的特征是否匹配。特征匹配時(shí)，對(duì)于和已有特征匹配失敗的特征，需要將其加入到臨時(shí)特征存儲(chǔ)區(qū)，在下次匹配過(guò)程中將其和已有特征匹配。若一個(gè)特征連續(xù)兩次匹配失敗，那么則將該特征視為不合格特征并從臨時(shí)存儲(chǔ)區(qū)中刪除。若一個(gè)特征在第一次匹配時(shí)失敗，在第二次匹配時(shí)成功，那么就將該特征加入到已有的特征中，同時(shí)對(duì)環(huán)境特征向量進(jìn)行更新。在未知環(huán)境中無(wú)人作戰(zhàn)平臺(tái)的定位與地圖創(chuàng)建是一個(gè)相互補(bǔ)償信息、不斷校正的過(guò)程，定位的結(jié)果可以補(bǔ)充地圖信息，而已有的局部地圖又可以用來(lái)校正定位信息。

3 基于擴(kuò)展卡爾曼濾波的SLAM算法

在處理線(xiàn)性問(wèn)題時(shí)卡爾曼濾波可以最優(yōu)估計(jì)目標(biāo)的狀態(tài)，獲得比較好的效果［8～9］。原始的卡爾曼濾波在處理非線(xiàn)性問(wèn)題時(shí)效果并不理想，為了解決該問(wèn)題，通常采用線(xiàn)性化技巧將線(xiàn)性濾波問(wèn)題轉(zhuǎn)化為非線(xiàn)性問(wèn)題?；谏鲜鏊枷耄瑪U(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）利用泰勒函數(shù)將系統(tǒng)的非線(xiàn)性函數(shù)進(jìn)行一階展開(kāi)，獲得相近的線(xiàn)性化系統(tǒng)方程，進(jìn)而對(duì)目標(biāo)進(jìn)行預(yù)估［10～12］。

無(wú)人作戰(zhàn)平臺(tái)在未知環(huán)境中的運(yùn)動(dòng)過(guò)程和地圖創(chuàng)建過(guò)程是極為復(fù)雜的，可以用兩個(gè)非線(xiàn)性模型來(lái)進(jìn)行描述：無(wú)人作戰(zhàn)平臺(tái)的運(yùn)動(dòng)模型和觀測(cè)模型。

其中，系統(tǒng)第k+1步的狀態(tài)用 Xk+1表示，系統(tǒng)第k+1步的觀測(cè)值用Zk+1表示，Gk和Vk表示白噪聲序列，協(xié)方差分別是Q(k)和R(k)。假定Gk和Vk序列是彼此獨(dú)立的，并且已知初始的狀態(tài)估計(jì)X?0和協(xié)方差矩陣P0。我們可以得到如下擴(kuò)展卡爾曼濾波算法，有預(yù)測(cè)和更新兩個(gè)步驟。

上述公式中，前兩個(gè)為狀態(tài)和協(xié)方差預(yù)測(cè)過(guò)程，后三個(gè)為狀態(tài)及協(xié)方差的更新過(guò)程。其中，狀態(tài)轉(zhuǎn)移矩陣φk+1，k和量測(cè)矩陣Hk+1用非線(xiàn)性函數(shù)f和h的雅可比矩陣代替。

假設(shè)狀態(tài)變量有n維，即 X=[x1x2…xn] ，則用運(yùn)動(dòng)模型方程和觀測(cè)模型方程對(duì)各維求偏導(dǎo)：

在無(wú)人作戰(zhàn)平臺(tái)的同步定位與地圖創(chuàng)建的過(guò)程中，系統(tǒng)狀態(tài)變量包括的位姿和環(huán)境特征的位置，假設(shè)系統(tǒng)狀態(tài)變量都服從正態(tài)分布，那么狀態(tài)變量之間的不確定關(guān)系就可以用狀態(tài)變量的協(xié)方差矩陣來(lái)表示，因而基于擴(kuò)展卡爾曼濾波的同步定位與地圖創(chuàng)建算法的核心問(wèn)題就是計(jì)算并更新協(xié)方差矩陣。

1）系統(tǒng)模型

在未知環(huán)境中無(wú)人作戰(zhàn)平臺(tái)的同步定位與地圖創(chuàng)建對(duì)位置和特征的估計(jì)需同時(shí)進(jìn)行，擴(kuò)展卡爾曼濾波的輸出量就是待估計(jì)量，因此系統(tǒng)狀態(tài)包括平臺(tái)的位姿信息和環(huán)境特征的位置信息，它們組成的高維狀態(tài)向量為

本文中的無(wú)人作戰(zhàn)平臺(tái)采用的控制量為u（t）=（v，ω），主要變量是平臺(tái)的旋轉(zhuǎn)角速度ω和前向速度v，因此用控制指令u（t）表示的無(wú)人作戰(zhàn)平臺(tái)的運(yùn)動(dòng)模型作為平臺(tái)位姿的時(shí)間更新模型：

其中，ωx/y/?∈N(0，σx/y/?)為機(jī)器人運(yùn)動(dòng)模型中的噪聲。

平臺(tái)傳感器在不同時(shí)刻都有測(cè)量值，但是環(huán)境中的特征是固定不變的，因此：

聯(lián)合平臺(tái)的位姿和環(huán)境特征的位置就能得到平臺(tái)系統(tǒng)的時(shí)間更新模型：

2）系統(tǒng)狀態(tài)變量估計(jì)過(guò)程

由于假設(shè)環(huán)境特征是靜止不動(dòng)的，因而用EKF進(jìn)行最小均方差誤差估計(jì)，從而得到無(wú)人作戰(zhàn)平臺(tái)系統(tǒng)協(xié)方差矩陣的替換公式：

式中，3×3維無(wú)人作戰(zhàn)平臺(tái)的位置協(xié)方差矩陣表示為Prr(k)；3×2n維平臺(tái)與環(huán)境特征間的協(xié)方差矩陣表示為Prm(k)；2n×2n維環(huán)境特征間的協(xié)方差矩陣表示為 Pmm(k)。 P(k)描述了無(wú)人作戰(zhàn)平臺(tái)與環(huán)境特征之間的關(guān)系、傳感器觀測(cè)的不確定性以及平臺(tái)運(yùn)動(dòng)的不確定性。P(k)的引入是擴(kuò)展卡爾曼濾波的最大的特點(diǎn)，對(duì)P(k)進(jìn)行計(jì)算是基于擴(kuò)展Kalman的SLAM算法的核心。P(k)隨著估計(jì)校正的不斷進(jìn)行單調(diào)遞減，因而正常情況下基于擴(kuò)展Kalman濾波的SLAM算法一定是收斂的。

將傳感器觀測(cè)到的環(huán)境特征與傳感器之間的相對(duì)距離 ρ(k)和方向θ(k)作為系統(tǒng)的觀測(cè)模型，v(k)為傳感器的觀測(cè)噪聲：

4 仿真實(shí)驗(yàn)與結(jié)果分析

由于周?chē)h(huán)境的復(fù)雜性以及算法的難度，仿真實(shí)驗(yàn)中預(yù)先選取適量的固定位置的環(huán)境特征點(diǎn)作為先驗(yàn)信息，此時(shí)無(wú)人作戰(zhàn)平臺(tái)的同步定位與地圖創(chuàng)建過(guò)程是一個(gè)在給定部分環(huán)境信息下同步定位與地圖創(chuàng)建的過(guò)程。仿真實(shí)驗(yàn)中無(wú)人作戰(zhàn)平臺(tái)沿著折線(xiàn)運(yùn)動(dòng)，在軌跡的周?chē)兄鴶?shù)量適當(dāng)?shù)沫h(huán)境特征，結(jié)果如圖2所示。

圖2（a）表示基于擴(kuò)展卡爾曼濾波的SLAM算法的處理過(guò)程，而圖2（b）則是算法處理后得到的對(duì)環(huán)境特征和軌跡的估計(jì)結(jié)果。圖2（b）中的紅色線(xiàn)表示無(wú)人作戰(zhàn)平臺(tái)在運(yùn)動(dòng)過(guò)程中對(duì)自身軌跡的估計(jì)，而實(shí)際軌跡則是起始點(diǎn)、轉(zhuǎn)折點(diǎn)和終點(diǎn)連接而成的折線(xiàn)，通過(guò)對(duì)比可以獲知若無(wú)人作戰(zhàn)平臺(tái)沿著折線(xiàn)運(yùn)動(dòng)，算法對(duì)自身軌跡的估計(jì)與實(shí)際運(yùn)動(dòng)的軌跡基本上是一致的。圖2（b）環(huán)境特征的估計(jì)位置用藍(lán)色圓圈表示，對(duì)比紅點(diǎn)表示的環(huán)境特征實(shí)際位置，整個(gè)算法基本上能夠趨近于環(huán)境特征的最優(yōu)估計(jì)。圖2（a）顯示算法執(zhí)行時(shí)得到的預(yù)測(cè)點(diǎn)都在實(shí)際環(huán)境特征位置附近，隨著環(huán)境特征數(shù)目的增多，基于擴(kuò)展卡爾曼濾波的SLAM算法的收斂效果越好。

圖2 折線(xiàn)軌跡算法仿真

5 結(jié)語(yǔ)

本文主要對(duì)無(wú)人作戰(zhàn)平臺(tái)的同步定位與地圖創(chuàng)建進(jìn)行研究，同步定位與地圖創(chuàng)建是未知環(huán)境下無(wú)人作戰(zhàn)平臺(tái)進(jìn)行自主導(dǎo)航的關(guān)鍵問(wèn)題，具有重要的研究?jī)r(jià)值和應(yīng)用價(jià)值。針對(duì)無(wú)人作戰(zhàn)平臺(tái)處于未知環(huán)境、通信系統(tǒng)不穩(wěn)定的情況，提出了一種基于擴(kuò)展卡爾曼濾波的同步定位與地圖創(chuàng)建算法，以確保無(wú)人作戰(zhàn)平臺(tái)可以繼續(xù)執(zhí)行任務(wù)或自主返航。最后通過(guò)仿真實(shí)驗(yàn)得到了該算法在無(wú)人作戰(zhàn)平臺(tái)多種運(yùn)動(dòng)軌跡下的定位與地圖創(chuàng)建結(jié)果，仿真實(shí)驗(yàn)結(jié)果驗(yàn)證了基于擴(kuò)展Kalman濾波的SLAM算法的有效性。