（91404部隊 秦皇島 066000）

1 引言

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

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

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

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

3 基于擴展卡爾曼濾波的SLAM算法

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

無人作戰(zhàn)平臺在未知環(huán)境中的運動過程和地圖創(chuàng)建過程是極為復雜的，可以用兩個非線性模型來進行描述：無人作戰(zhàn)平臺的運動模型和觀測模型。

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

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

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

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

1）系統(tǒng)模型

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

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

其中，ωx/y/?∈N(0，σx/y/?)為機器人運動模型中的噪聲。

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

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

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

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

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

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

4 仿真實驗與結(jié)果分析

由于周圍環(huán)境的復雜性以及算法的難度，仿真實驗中預先選取適量的固定位置的環(huán)境特征點作為先驗信息，此時無人作戰(zhàn)平臺的同步定位與地圖創(chuàng)建過程是一個在給定部分環(huán)境信息下同步定位與地圖創(chuàng)建的過程。仿真實驗中無人作戰(zhàn)平臺沿著折線運動，在軌跡的周圍有著數(shù)量適當?shù)沫h(huán)境特征，結(jié)果如圖2所示。

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

圖2 折線軌跡算法仿真

5 結(jié)語

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