无人机主动SLAM技术研究

杨祥红，王希彬

（海军航空工程学院1.军事教育训练系；2.控制工程系，山东烟台 264001）

无人机主动SLAM技术研究

杨祥红1，王希彬2

（海军航空工程学院1.军事教育训练系；2.控制工程系，山东烟台 264001）

针对传统被动SLAM算法存在盲目地图构建的问题，提出了基于约束局部子图的无人机主动SLAM算法。首先，在约束局部子图滤波方法的基础上创建局部地图，接着通过构建的主动SLAM的目标函数寻找局部地图的最优解，最后将局部地图和全局地图进行融合。针对无人机系统进行仿真，验证了基于局部子图的主动SLAM算法的有效可行性。

无人机；主动SLAM；约束局部子图滤波

1 引 言

SLAM（Simultaneous Location and Mapping，同时定位与地图创建）问题只考虑了无人机如何利用获得的信息构建环境地图，并在地图中对无人机进行定位，没有考虑无人机如何有效地探测未知环境，即如何控制无人机运动，以达到更好地和更快地构建地图的目的。现有的环境探测策略基本上都是最大限度地、尽快地引导无人机向未探测区域运动，它们关注的是尽快减少为探测区域，而尽量避免在已探测区域内运动。这类探测策略显然不能直接用于SLAM中［1］，因为无人机需要重复探测已探测过的环境来对自身进行自定位来提高定位的精度，从而提高地图的精度。

目前所提出方法存在以下局限性［2］：（1）所规划的无人机的运动轨迹是在完全局部优化的条件下获得的；（2）只针对降低地图和定位的不确定性来作出规划，没有考虑无人机环境探测速度和代价；（3）传统的对信息增益的测量不能得出最精确的地图。

本文针对无人机对象设计了一个仿真系统，分析验证了基于EKF的主动SLAM算法的性能。系统验证了基于局部子图方法的主动SLAM算法，并给出了仿真结果。

2 基于局部子图滤波的主动SLAM算法

2.1 局部子图滤波

局部子图方法在k时刻系统的状态［5］为：

其中，GXL（k）表示全局地图和局部地图的关系，也就是全局坐标系中局部坐标系的位置，图1给出了形象的表示；GXm（k）和LXm（k）分别表示全局地图和局部地图中地标的位置；LXr（k）表示局部地图中无人机的位姿。

图1 局部坐标系与全局坐标系的转换Fig.1 The transformationbetween local frame and global frame

方差矩阵具有以全局地图表示一个方块、局部地图表示另一个方块的块对角结构形式，包含无人机状态的方差和地标的方差。状态方差矩阵为：

为获得系统状态的估计值，采用EKF估计无人机的位姿和观测地标的位置。与最优控制方法不同的是，局部子图方法在一次观测中，只更新局部坐标系中已观测到的地标，当局部地图中的信息融合进全局地图时，才会更新全局地图中剩余地标的状态。文［6，7］已证明局部估计值与全局估计值是不相关的。

预测：

回顾二十多年的教学经历，自己一直以来都认认真真备课，教案越写越详尽，课件越做越多，教学似乎变成了日复一日的重复，激情也慢慢随着时间的推移消失了。以往在语文教学中，我常常担心讲不深、讲不透，喜欢在课堂上滔滔不绝，实际上学生不是在学语文，而是在学教师对课文的心得体会。事实证明，由于忽视了学生的主体地位，再加上学生自主阅读课文时间太少，他们对课文本身认识不真切，思考不深刻，因此教学效果并不好。而模块教学的实施注定了教师再不能在课堂上满堂灌了，必须把时间还给学生。在单元模块教学中教师又该怎么做呢？

更新：

在探索过程中，无人机在局部子地图中运动，由于需要采取主动运动方式创建局部地图，即无人机在子地图中通过优化目标函数选取控制输入。我们采用第3节中设计的目标函数，通过选择使目标函数最大的控制决策确定无人机下一步的运动，并在运动过程中创建局部子地图，在恰当的时刻将现在局部子图中的信息融合进全局地图，从而完成整个SLAM过程。

2.2 局部子地图与全局地图之间的融合

局部子地图到全局地图的映射关系为T（k），反之，全局地图到子地图的映射关系为T－1（k），则局部子地图与全局地图之间的融合关系为：

3 目标函数的设计

改进的多目标函数包含下面四项：

（1）定位与作图的精度：状态方差矩阵P包含了无人机位姿和环境特征的不确定性。将矩阵P的迹作为对不确定性的定量描述，用表示。若Pmin表示P的下界［33］，则越大，定位与作图的精度越高。

（2）相对信息增益：在EKF过程中，不确定性下降往往意味着SLAM的收敛。将C（P与的差值作为相对信息增益，于是，可以用表示不确定性下降的速度。

（3）未探索区域：前面（1）、（2）两项趋向于最小化定位与作图的精度，忽视了环境探索的完整性。为此，引入Snew／Ssensor表示探测环境的效率，引导无人机进入未探索区域，其中，Snew表示在控制输入下无人机已观测的面积与现在观测面积的交集，Ssensor表示机载传感器扫掠的面积。

（4）导航代价：从尽量减小导航代价的角度讲，需要最小化无人机每一时刻的运动距离r，因此可以用1／r表示导航代价的大小。综上，多目标函数为：

其中，ω1、ω2、ω3和ω4为加权值，C（Pmin）为阈值。

通过最大化式（16）中的目标函数J所确定的u就是最优的控制输入：

4 无人机简化运动模型

为验证提出的主动SLAM算法的有效性，采用简化的无人机平面运动模型［8］，如图2所示。

图2 无人机平面模型Fig.2 The planemodel of UAV

无人机的状态Xv＝ [xyψ]T，其中，x、y为无人机在两维平面中的位置，ψ为航向角；控制量u＝［Δd Δψ］T，其中，Δd为无人机一个时间步长的运动距离，Δψ为方位变化量。各状态的变化方程为：

无人机状态方程为：

第i地标的状态为mi＝［mxmy］T，假设地标是静止的，则：

无人机与第i地标的观测方程为：

观测量Zi，k为k时刻无人机对第i地标观测的视线角φi，k。

5 仿真试验与分析

仿真采用了节4中的无人机平面运动模型，控制量为运动步长和偏转角。在无人机的运动范围内，每一时刻从无人机所有可能的运动步长和偏转角中，选择使得目标函数J最大的偏转角和运动步长作为控制量。仿真条件如下：初始时刻，无人机位于坐标原点并朝向 x轴正向，在（［－20，20］，［－20，20］）环境中分布着20个地标。目标函数中权值参数ω1、ω2、ω3和ω4都取为0.25。无人机的控制噪声方差Q和传感器的测量噪声方差R分别为：

图3和4分别给出了仿真运行100步后，采用随机SLAM和基于局部子图的主动SLAM算法估计的无人机轨迹和地标位置情况。其中，连续实线表示估计的无人机运动轨迹，离散的“＋”点及周围椭圆分别表示估计的地标位置和不确定性，离散的“□”点表示真实地标位置。从图3和图4对比中可以看出随机运动探测环境有限，且无人机的运动混乱无规律，经常会陷入某个区域而不能离开。而基于局部子图的主动SLAM能够克服这种情况，无人机的航迹是有序的，并且就观测地标数而言，也大大多于随机控制SLAM的方法，这样无人机可以探测到更大范围的环境。因此基于局部子图的无人机主动SLAM要明显优于随机SLAM。

图3 无人机随机SLAM的结果Fig.3 The results of random SLAM for UAV

图4 基于局部子图的无人机主动SLAM的仿真结果Fig.4 The results of active SLAM based on local sub-map for UAV

6 结 论

局部子图方法把主动SLAM策略和CLSF结合起来，利用最优控制算法中的多目标函数，使用现在的局部地图信息，引导无人机探索未知区域。对于多无人机协作的SLAM，每个无人机分别主动构建自己的局部地图，通过CLSF将其融入到全局地图中，大大提高了无人机探索环境的效率。因此，研究基于CLSF的多无人机主动SLAM，具有重要的价值。

［1］ Newman PM.On the structure and solution of the simultaneous localization and map building problem［D］.Australian：University of Sydney，1999.

［2］ 张恒，樊晓平.移动机器人同步定位与地图构建过程中的轨迹规划研究［J］.机器人，2006，28（3）：285－290.

ZHANG Heng，Fan Xiaoping.Research on trajectory planning in simultaneous localization and mapping technology formobile robot［J］.Robot，2006，28（3）：285－290.

［3］ Feder H，Leonard J，Smith C.Adaptive mobile robot navigation andmapping.International Journal of Robotics Research，1999，18（7）：650－668.

［4］ Tao T，Huang Y.Motion Planning for SLAM Based on Frontier Exploration［C］.Proceeding of IEEE International Conference on Mechatronics and Automation，Harbin，China，2007，2120－2125.

［5］ 苑晶.未知环境中移动机器人主动同时定位与地图构建技术研究［D］.南开大学，2007.

YUAN Jing.Active simultaneous localization and mapping technology for mobile robot in unknown environment［D］.NanKai University，2007.

［6］ Willianms S.Efficient solutions to autonomous mapping and navigation problems［D］.University of Sydney，2001.

［7］ Liu Y，Sun F.A solution to active simultaneouslocalization and mapping problem based on optimal control［C］.Proceeding of IEEE International Conference on Mechatronics and Automation，Harbin，2007：314－319.

［8］ 王希彬.无人机同时定位与地图创建技术研究［D］.海军航空工程学院，2012.

WANG Xibin.Research on simultaneous localization and mapping technology for UAV［D］.Naval Aeronautical and Astronautical University，2012.

杨祥红 男（1978－），山东临沂人，讲师，主要研究方向为兵种战术学。

王希彬 男（1985－），山东人，博士，讲师，主要研究方向为导航制导与控制。

Research on Active SLAM Technology For UAV

YANG Xianghong1，WANG Xibin2

（Naval Aeronautical and Astronautical University 1.Department of Military Education and Training；2.Department of Control Engineering，Yantai，264001，China）

To conquer the idle mapping problem of passive simultaneous localization and mapping for UAV，an active SLAM algorithm is proposed based on constrained local sub-map.First，local sub-map is built based on constrained local sub-map filtering，and then，the optimal solution is searched by the active SLAM objective function，and at last，the local sub-map and the global map are fused.Based on the simplified UAV planemotion model，this algorithm is simulated and tested.Simulation results show that this algorithm is effective and feasible.

UAV；active SLAM；constrained local sub-map filtering

V 24

A

国家自然科学基金（61473306）。