未知环境下UCAV SLAM研究*

周阳，吴德伟，邰能建，杜佳

(空军工程大学 信息与导航学院，陕西 西安 710077)

0 引言

随着战争环境的日益严酷，UCAV(unmanned combat aerial vehicle)在战场上的运用得到广泛关注，加速了各国对其关键技术的研究，导航系统的自主性和智能性便是其中核心内容。UCAV认知导航系统[1-2]可通过多种传感方式感知外界环境，实时配置导航方式，模拟人类海马机理实现具有知识记忆、学习及推理特性的导航信息处理，实现UCAV在最优航迹上的高精度智能自主导航。其中未知环境下的自主导航能力决定了认知导航系统对环境的适应性，有待研究。

Durrent-Whyte[3]首次将SLAM(simultaneous localization and mapping)作为专有名词使用，其基本思想是将运行体定位与环境地图创建融为一体，预示着运行体在没有任何先验信息的情况下可进行导航。这一经出现就成为运行体自主导航领域研究热点，同时为未知环境下UCAV自主导航的实现提供了思路。

UCAV在未知环境下的SLAM可描述为：UCAV利用机载传感器估计自身位姿和环境特征状态，以建立的状态模型和观测模型为基础，通过实时获取的观测数据，采用具体的滤波技术得到较准确UCAV位姿和环境特征状态。在具体实现中通过自身携带的外部传感器识别环境中的特征，即“路标”，然后从传感信息中计算出路标与UCAV相对位置，再依据坐标系间的转换关系定位路标的全局位置，以此创建环境地图，并在随后的运动过程中识别和匹配这些路标，通过有效滤波技术同步更新UCAV位姿和地图信息。

目前国外对航空SLAM的研究已取得初步成果。Jonghyuk Kim[4-5]等人研究了无人机(unmanned aerial vehicle，UAV)SLAM，分析了UAV SLAM中惯性器件位姿推算的机理，给出了路标状态在不同坐标系下转换关系，并指出UAV SLAM的误差主要源于外部传感器；David Tö rnqvist[6]等人将粒子滤波(particle filter，PF)引入到航空SLAM中，实现了UAV高维运动模型下的自主导航；S.Sukkarieh等[7]人将视觉传感器与惯性测量单元相结合应用于航空SLAM中，并证明了该方法可有效修正惯导漂移；Jorge Artieda[8]等人以提取的SURF(speeded up roubust features)特征点作为路标构建观测模型，实现了基于三维视觉的UAV SLAM。目前国内主要对移动机器人[9]及水下运行体[10]的SLAM进行了研究，且主要针对不同传感器下实现原理[11]及滤波实现上[12]，对航空SLAM的研究较少。作为航空运行体的UCAV，其自主性依赖于具体导航方式，为此本文对UCAV SLAM进行了研究。

1 航空SLAM模型分析

航空SLAM模型[4]主要包括状态模型和观测模型，其中状态模型包含了运行体的运动状态及环境特征状态；观测模型表示了观测与状态的关系，是运行体实现位姿校正基础。

1.1 状态模型

状态模型p(X(k)X(k-1),u(k))用于描述一个输入控制命令u(k)下，前一时刻状态X(k-1)与当前状态X(k)之间的关系。状态模型离散时间的差分方程为

X(k)=F(X(k-1),u(k),k)+Wv(k),

(1)

式中：F(·)为非线性状态变换函数；Wv(k)为均值为零方差为Q(k)过程噪声矢量；X(k)由运行体位姿状态Xv(k)和路标状态Xm(k)构成，具体表示为

(2)

式中：Xv(k)=x,y,z,ψ,θ,γ为运行体在导航坐标系下位置(x,y,z)和姿态(ψ,θ,γ)构成的状态矢量；N为k时刻观测到的路标个数；Xmi(k)为k时刻第i个路标在导航坐标系中位置，且当路标位置固定时路标状态存在以下关系

Xmi(k)=Xmi(k-1).

(3)

Xv(k)中6自由度x，y，z，ψ，θ，γ可由惯性测量器件解算，具体计算式为[4]

(4)

(5)

经过以上内容的分析，可建立航空SLAM的状态模型。

1.2 观测模型

观测模型用于在已知k时刻运行体位姿Xv(k)与观测路标状态Xm(k)条件下求k时刻观测值Zk，其离散时间的差分方程形式为

Z(k) =H(X(k))+V(k)=

H(Xv(k),Xm(k))+V(k)，

(6)

式中：H(·)为非线性观测模型；V(k)为均值0方差为R(k)的观测噪声。

图1 路标在不同坐标系下位置示意图Fig.1 Sketch map of landmark’s location in different coordinate systems

(7)

当坐标系确定后，路标位置的表示形式随选取的状态变化，在具体分析中可以极坐标或笛卡尔坐标形式给出。

经过以上内容的分析，可建立航空SLAM的观测模型。

2 UCAV SLAM涉及的滤波技术

以得到的航空SLAM模型为基础，UCAV SLAM实现必须以具体滤波技术为支撑。滤波技术主要用于对系统状态进行递归估计，并以测量误差为依据对状态进行校正，使系统状态逼近真实值。目前较常用的有：卡尔曼滤波[5](Kalman filter，KF)、扩展卡尔曼滤波(extended Kalman filter，EKF)[13]、粒子滤波[6]等，针对不同应用条件及背景，学者们也对上述滤波技术进行了改进[14]。

基于KF的SLAM模型是通过不断进行预测和修正的计算过程实现，适用于高斯噪声假设下线性SLAM模型。其基本思想是：将UCAV的位姿和环境特征组成一个状态向量，并认为被估计状态的后验概率在任何时刻都服从高斯分布，采用KF作最小均方误差估计。具体实施上主要通过处理一系列带有误差UCAV传感器数据得到状态的最佳估计。

基于EKF的SLAM不限于线性系统及高斯噪声影响的假设，其基本思想是：将UCAV SLAM状态方程和观测方程泰勒展开，舍去高阶项，实现模型线性化，以KF原理估计UCAV位姿和环境特征状态。该方法在运算中需求解非线性状态函数及非线性观测模型相对具体变量雅可比矩阵，且在模型建立中不可避免引入了线性化误差，从根本上影响了SLAM的滤波收敛情况和状态的估计效果，同时还面临着随路标数量增加出现高计算量和维数灾难的问题。

基于PF的SLAM是非线性条件下实现形式，适用于任何能用状态空间模型表示的非线性系统及传统卡尔曼滤波无法表示的非线性系统，较贝叶斯估计在线性条件下实现形式更具有环境适应性[11]。该方法通过非参数化的蒙特卡罗模拟方法(Monte Carlo method)实现递推贝叶斯滤波。其基本思想是：选取一个重要性概率密度来得到带有相关权值的随机样本以符合UCVA位姿状态和环境特征状态的后验概率分布，然后在UCAV运行过程中，以传感器数据为基础调整权值的大小和粒子的位置。该方法在实现过程中主要通过“状态预测—权值更新—重采样”的步骤完成，最终得到较准确的UCAV位姿和环境特征估计值。

针对计算复杂度和高维数据处理问题，Montmerlo等人[15]将Rao-Blackwellise粒子滤波器引入SLAM，利用粒子簇表征非线性、非高斯系统的状态分布，提出了FastSLAM。FastSLAM认为当运行体状态轨迹已知且各路标之间相互独立时，可将状态空间分离为独立的部分，并边缘化该部分的一个或者多个组成成分。其具体实现上将EKF和PF结合，用单一粒子滤波器实现运行体轨迹滤波，对轨迹中路标通过EKF估计。该方法可有效解决粒子滤波器地图创建困难问题，计算效率和可扩展性也优于全状态的EKF-SLAM[11]。

3 UCAV SLAM需解决的问题

UCAV的运行特点及作战背景，使其在未知环境下SLAM较其他运行体复杂，具体实现上有许多关键问题有待解决，具体如下：

(1) 传感器的选择

传感器分为内部传感器和外部传感器。内部传感器是实现UCAV位姿推算的基础，其精度的高低决定了位姿估计的漂移量，进一步影响了UCAV定位及地图创建的准确性。内部传感器通常由惯性测量器件承担，鉴于UCAV的高机动性，必然要求所选的惯性测量器件在满足高精度的同时保证高动态环境下的稳定性；外部传感器用于识别解算环境路标状态，在具体选择上需考传感器如何适于高动态运行环境、如何实现路标准确识别及如何与UCAV配套等问题。

(2) 数据关联问题

数据关联是指UCAV找出当前时刻观测到的路标与路标数据库中路标的对应关系。数据关联的根本是存在路标数据库，在具体构建上需解决如何选择环境中特征作为路标、如何描述路标和如何将路标归类等问题。以得到的数据库为基础，在具体关联上，UCAV需解决如何快速识别环境特征及数据高效检索等问题。

(3) 算法高效性问题

UCAV未知环境下自主导航必然面临复杂环境。要实现UCAV高精度的定位和地图构建，需不间断感知环境信息，用于更新或添加路标，这导致估计状态维数的增加，最终增加了模型解算的时间，为此需寻求一种高效的滤波方法，以适应于UCAV SLAM实时性要求。

(4) 地图构建问题

地图的构建是UCAV实现SLAM的重要环节，其与UCAV定位相辅相成。在具体构建上需考虑选择的地图如何便于创建和维护、如何实现环境的唯一表征、如何便于识别并利于UCAV完成自身位姿确定和路径规划等问题。

4 结束语

本文以航空SLAM模型的建立、UCAV SLAM涉及的滤波技术和待解决的问题为重点，对UCAV SLAM进行了研究，为认知导航系统用于UCAV未知环境下自主导航开阔思路。

参考文献：

[1] 吴德伟,邰能建,戚君宜. 基于认知理论的UCAV智能导航研究新进展[J]. 空军工程大学学报:自然科学版, 2011,12(4): 52-57.

WU De-wei, TAI Neng-jian, QI Jun-yi. A New Research Progress of UCAV Intelligent Navigation Based on Cognitive Theory [J]. Journal of Air Force Engineering University: Natural Science Edition, 2011, 12 (4):52-57.

[2] 邰能建,吴德伟,戚君宜.基于改进SIFT的高鲁棒性特征点提取方法[J]. 航空学报, 2012, 33(12): 2313-2321.

TAI Neng-jian, WU De-wei, QI Jun-yi. A Method to Extract High Robust Keypoints Based on Improved SIFT[J]. Acta Aeronautica et Astronautica Sinica, 2012, 33(12): 2313-2321.

[3] DURRANT-WHYTE H,RYE D,NEBOT E.Localisation of Automatic Guided Vehicles[J].International Symposium Robotics Research，1996(7):613-625.

[4] KIM J H, SUKKARICH S. Airborne Simultaneous Localization and Map Building [C]∥Proceedings of IEEE International Conference on Robotics and Automation,2003: 406-411.

[5] Jonghyuk Kim, Salah Sukkarieh. Real-Time Implementation of Airborne Inertial-SLAM [J]. Robotics and Autonomous Systems，2007,55: 62-71.

[6] David Törnqvist,Thomas B Schön,Rickard Karlsson,et al. Particle Filter SLAM with High Dimensional Vehicle Model[J].J Intell Robot Syst,2009,55:249-266.

[7] SUKKARIEH S,DURRANT-WHYTE H.Towards the Development of Simultaneous Localization and Map Building Algorthms[C]∥Proc.Int.Conference of Field and Service Robotics,2001:193-200.

[8] Jorge Artieda,Jose M Sebastian,Pascual Campoy,et al.Visual 3-D SLAM from UAVs[J].J Intell Robot Syst ,2009,55:299-321.

[9] 涂刚毅,金世俊,祝雪芬,等. 基于粒子滤波的移动机器人SLAM算法[J].东南大学学报：自然科学版,2010,40(1):117-122.

TU Gang-yi,JIN Shi-jun,ZHU Xue-fen,et al. Particle Filter SLAM Method for Mobile Robot[J].Journal of Sou Theast University：Natural Science Edition, 2010,40(1):117-122.

[10] 曲镜圆.基于声纳的AUV环境感知与地形建模方法研究[D].哈尔滨：哈尔滨工业大学,2009.

QU Jing-yuan.Rearch on Environment Perception and Terrian Modeling for AUV Based on Sonar[D].Harbin:Harbin Engineering University,2009.

[11] 林睿.基于图像特征点的移动机器人立体视觉SLAM研究[D].哈尔滨：哈尔滨工业大学,2011.

LIN Rui. Research on Image Feature-Based Mobile Robot Stereo Visual SLAM[D]. Harbin:Harbin Engineering University,2011.

[12] 祝继华,郑南宁,袁泽剑,等.基于中心差分粒子滤波的SLAM算法[J].自动化学报,2010,36(2):249-257.

ZHU Ji-hua,ZHENG Nan-ning,YUAN Ze-jian,et al. A SLAM Algorithm Based on Central Difference Particle Filter[J].Acta Automatica Sinica, 2010,36(2):249-257.

[13] Zahra Mohajerani. Vision-Based UAV Pose Estimation[D].America: Northeastern University,2008.

[14] 朱磊,樊继壮,赵杰,等. 未知环境下的移动机器人SLAM方法[J]. 华中科技大学学报：自然科学版,2011,39(7):9-13.

ZHU Lei,FAN Ji-zhuang,ZHAO Jie,et al.SLAM Mehtod for Mobile Robot in Unknow Environment[J].J.Huazhong Univ.of & Tech.：Natural Science Edition, 2011,39(7):9-13.

[15] MONTEMERLO M, THRUN S, KOLLER D, et al. FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem [C]∥ Proc.of the National Conference on Artifical Intelligence, AAAI, Edmonton, Canada, 2002: 593-598.