叶羽泠，蔡乐才，黄洪斌，肖体刚

（1.四川轻化工大学自动化与信息工程学院，四川 自贡 643000；2.宜宾学院三江人工智能与机器人研究院，四川 宜宾 644000）

引言

随着人工智能的快速发展，无人驾驶技术广泛应用于各类车辆、机械设备以及移动机器人，实时定位与地图构建（Simultaneous Localization and Mapping algorithm，SLAM）［1-3］作为自动驾驶的核心技术，逐渐成为机器人领域研究的重要组成部分。较为经典的有扩展卡尔曼滤波 SLAM算法（Extended Kalman filter-SLAM，EKFSLAM）［4］，该算法基于贝叶斯理论，建立了一种高斯函数框架，对SLAM的方法做出了基本定位。扩展卡尔曼滤波算法在面对非线性较强的定位与建图环境下，关键点寻找的计算难度会有所加大，因此定位效率显著降低；Fast-SLAM［5］算法基于一种蒙特卡洛采样定律，建立起一种数学模型。尽管相比于EKF-SLAM算法，Fast-SLAM算法能够消除非线性干扰的影响，但随着位姿计算迭代次数的增加，会出现误差累加的情况。结合上述两种算法的优势，Sun等人［6］提出了混合 SLAM算法（Hybrid-SLAM），该算法使用Fast-SLAM作为前端里程视觉器信息采集的处理方法，同时使用EKF-SLAM作为后端非线性优化的位姿矩阵函数的处理。但由于Hybrid-SLAM直接采用机器人运动方程来判断其位姿数据，一定程度上会影响位姿采样结果的精确度［7］。

而在基于前端视觉里程与后端优化的卡尔曼滤波算法的研究方面，闫德立等人［8］提出了一种新的变步长LMS算法，将恒定容积卡尔曼滤波融合于SLAM，在容积点采样方面使用李群变量实现了容积变换，并表示出各个时刻机器人的视觉位姿和协方差。但在容积变换传递状态均值及方差的滤波方法方面，位姿精度提高的步骤较为繁琐复杂；燕必希等人［9］在研究搭载单目相机的机器人运动目标定位时，对加速度进行自适应处理，根据先前位置信息的关联函数，使用卡尔曼滤波算法得到设备的加速度和速度，从而建立运动方程和预测目标位置。但缺乏回环检测步骤，导致误差逐渐积累；穆绍硕等人［10］在研究提高机载光电平台目标状态位姿精度调整问题时，在使用扩展卡尔曼滤波的前提下，采用蒙特卡罗法目标定位的方法对卡尔曼滤波算法进行了改进，并分析了测量误差，提升了机器人自动驾驶设备系统的稳定性；赵伟等人［11］在使用非线性滤波方法来处理非线性系统的鲁棒控制时，为了降低或者消除系统的随机性，将多种类型的容积卡尔曼滤波进行奇异值分解，在基于卡尔曼滤波算法的对比上，展开了使用容积法则的系统鲁棒性研究。此外，无迹卡尔曼滤波（Unscented Kalman Filtering，UKF）作为一种较为经典的卡尔曼滤波方法，Bosov等人［12］选定了不同的信标点进行权值采样，并通过非线性函数的后端优化使用了无迹变换（Unscented Transformation，UT）的方式，得到机器人设备因外界干扰产生随机状态变化的基本特点。

以上几种基于前端视觉里程与后端优化的卡尔曼滤波算法的研究探索以及其他改进的SLAM算法，普遍存在关键点寻找困难、定位精度低、建图时间长等问题。本文提出一种基于位姿自适应的卡尔曼滤波的SLAM算法，从卡尔曼滤波算法的基本模型得出位姿基本状态，提出了一种关键信标寻找的自适应的路径规划方法。与前文提到的各类改进SLAM算法相比，其定位精度和循迹的准确性明显提升，建图的时间显著降低。

1 SLAM循迹的基本问题

自动驾驶的机器人设备同步定位与地图创建的一般工作流程如下：首先视觉里程计通过外部传感器，包括激光雷达、深度摄像头等传感器采集相邻的两幅图像信息或障碍物特点，估算机器人设备的运动姿态变化［13-14］。其次使用闭环检测的算法，以修正位姿状态因时间产生的累加误差，判断机器人设备运动后是否重新回到先前出现的位置［15-16］。由于经过前端视觉里程计和回环检测并计算出的数据结果存在累加误差，因此最后需要进行后端非线性优化，得到更加精确的位姿状态计算结果［17］。最后将得到的位姿数据使用关联矩阵的形式表现出，同时依靠相关数学模型建立起与任务要求对应的地图，最终判断出设备的具体位置以及前进方向。基本原理如图1所示。

图1 基于SLAM的机器人信标循迹

图1中考虑将机器人运动场地看作一个二维平面，在这个平面上分布着多个人为设定或者客观存在的可用于定位的信标点。信标点1表示机器人的初始位置或者已经寻找到的位置，信标点2表示当前位置，信标点1和信标点2之间的连线表示机器人运行轨迹。现在的任务便是寻找到下一个信标点3的坐标（X，Y），后续的信标点以此类推。而循迹过程，就是使用相应的数学方法，通过卡尔曼滤波的方式实现信标点定位。

假使机器人的位姿描述为Xp，信标点的位置为，则：

其中：uk表示控制输入，μk表示高斯白噪声分布，并且定义过程噪声协方差矩阵为Qk。机器人的运动状态观测Zj可表示为：

其中：i表示在信标点的序号，φk表示噪声分布总和，初始协方差矩阵分布为Rk。信标点定位的SLAM过程中，考虑uk和Zi（k）的值，求解Xp和的过程就是SLAM循迹的控制目标。

2 位姿自适应卡尔曼滤波算法

仅仅依靠信标循迹的基本数学方法，由于缺乏位姿调整的部分，可能会产生噪声干扰［18-19］。本文提出的位姿自适应卡尔曼滤波的方法能够在一定程度上解决此类问题。

2.1 位姿自适应的描述模型

如果需要得到随机信标的位置并进行估计，首先需要建立机器人的运动描述与观测描述。位姿点是相对分散的信标点，因此还需要离散化处理。离散化位姿描述模型为：

其中：F为机器人运动状态转移矩阵。然后建立观测模型为：

其中：G为观测矩阵。在基于卡尔曼滤波的位姿估计中，将位姿描述记作Xp=［Xpx，Xpy，Vx，Vy。定义在k时刻，（Xpx，Xpy）为机器人设备的位置坐标；（Vx，Vy）机器人设备的速度；（）为机器人设备在被观测到的第i个信标位置。因此综合位姿描述如式（5）～式（7）所示：

其中：E（k）表示机器人的运动状态转移矩阵。当状态更新时，下一时刻对状态的预测可看作本时刻系统的状态。一般来说循迹过程至少包含2个信标的位置信息。位姿自适应的要求是：根据k时刻的机器人的位姿描述Xp（k）和控制输入u（k），预测出系统在k+1时刻的位姿状态Xp（k+1）、第i个信标点的位置Zi（k+1以及机器人预估状态的协方差矩阵P（k+1）。

2.2 SLAM的卡尔曼滤波方法的基本原理

卡尔曼滤波本质上是一种实现最优估计的算法。一般情况下利用系统的状态描述公式，结合各类输入输出数据，对系统整体状态进行最优估计［20］。但由于系统存在的噪声干扰影响，所以卡尔曼滤波也可看作是一种噪声消除方法。

当信标点的个数逐渐增加，预估状态的维度数目也随之增加。此时，位姿描述Xp可表述为Xp=［Xpx，Xpy，Vx，Vy，…］T，其中：（）表示新增的坐标。

可观测的向量表示为：

其中：G表示观测矩阵。观测的目的是根据第k+1时刻对第i个信标的观测Zi（k+1）值，构造新的位姿观测点Ti（k+1）与新的协方差矩阵Si（k+1）。

2.3 卡尔曼滤波位姿自适应方式

当建立起机器人设备的协方差和观测矩阵之后，就可以建立基于卡尔曼滤波的位姿自适应方法，将去噪声的状态向量和关键信标点相对比，从而确定观测信标点的方向和距离［21］。当位姿状态过于陈旧时，会造成机器人定位轨迹滞后。位姿自适应的调整方法可由式（11）～式（13）表示：

其中：M为介质矩阵，Xp为位姿矩阵，P为初始协方差。系统位姿自适应主要作用就是使机器人始终保持最新的位姿状态，防止过于陈旧的位姿状态用于信标点定位。然后根据预测与观测完成位姿更新后，通过自主消除噪声的方式，完成信标循迹的任务形成。这个过程可看作是自适应性的调整过程。

同时，新的位姿点也存在噪声的可能。为了讨论噪声带来的影响，现在定义仿真环境下，信标测量噪声为ω、距离噪声为ωd、速度噪声为ωv、过程噪声为ωp。这四个参数统称为噪声参数。在本文提出的位姿自适应卡尔曼滤波的SLAM循迹算法中，定义初始协方差描述为Pk，测量噪声协方差描述为Rk，过程噪声协方差描述为Qk，分别表示为式（14）、式（15）和式（16）：

机器人系统的抗噪声能力也是位姿自适应能力的另一种表现方式。因此，不同噪声参数的值会对不同的噪声描述产生影响。而位姿自适应的方式是通过自主调整噪声参数的值来实现各种噪声描述保持在最优范围内，并对运行轨迹造成影响。

3 位姿自适应SLAM仿真实验

本文使用的虚拟机器人循迹定位，可以在一定程度上模仿真实机器人的运动轨迹，能够基于SLAM算法的基本原理进行定位建图，实现实时定位与路径规划的控制目标。实际机器人的运动状态描述为：

其中：Lk表示时刻机器人k时刻的位姿状态描述，αk-1表示机器人转向的角度，m为机器人轴距，θk为速度矢量角。

而在信标循迹的仿真研究中，借鉴实际机器人的运动状态描述，仿真机器人自动循迹车辆的观测矩阵为：

其中：（d Xi，d Yi）表示第i个路标的位置，d Xk、d Yk和 θk共同组成了机器人的观测位姿描述，D为机器人与信标点之间的距离，γ表示机器人前进方向与路标的夹角。

3.1 信标点循迹的仿真研究

本文实验仿真研究在操作系统为Windows10，CPU为 Intel（R）core（TM）i7（2.6 GHz），MATLAB版本为2016a，内存为8 GB的环境下进行仿真实验。分别对Hybrid-SLAM（图2）、Fast-SLAM（图3）、EK-SLAM（图4）和本文算法（图5）使用MATLAB进行分析，并比较各个算法之间的信标定位和路径规划精度差异。由于实验条件在仿真环境下进行，图（2）～图（5）中的坐标值均为定义为单位长度，其中各图的（a）中红色点为信标点，（b）中的蓝色的轨迹为模拟机器人运动轨迹，（c）中圆圈图为信标点寻找过程。

图2 Hybrid-SLAM算法得到的循迹仿真图

图3 Fast-SLAM算法得到的循迹仿真图

图4 EK-SLAM算法得到的循迹仿真图

图5 位姿自适应卡尔曼滤波SLAM算法得到的循迹仿真图

对比仿真实验结果可以明显发现，相对于其他三种算法，由于采用了基于卡尔曼滤波的位姿自适应模型，本文算法模拟的机器人运动，经过20个采样信标点时，定位点的精确度较高，基本上能与设定的虚拟信标点重合，能够实现按照预计的路径行驶。

3.2 定位与循迹误差比较

根据2.3的位姿自适应原理，信标点噪声ω、距离噪声ωd、速度噪声ωv以及过程噪声ωp都将直接影响初始协方差矩阵Pk、测量噪声矩阵Rk和过程噪声矩阵Qk，从而对SLAM算法的定位精度造成影响。而距离测量噪声ωd和速度测量噪声ωv对初始协方差矩阵Pk和噪声矩阵Rk的影响尤为严重。因此本文在几种不同的ωd和ωv条件下，对20个信标点处的横纵方向偏差以及速度矢量夹角进行仿真分析。每个定位信标点处，模拟机器人的实际位姿与设定信标的X方向和Y方向的坐标偏差，以及速度矢量和行进轨迹的夹角θ的偏差，如图6～图9所示。

图6 位置角度误差对比（保持ωd=1.5不变，ωv=0.5）

图7 位置角度误差对比（保持ωd=1.5不变，ωv=1.0）

图8 位置角度误差对比（保持ωd=3.0不变，ωv=0.5）

图9 位置角度误差对比（保持ωd=3.0不变，ωv=1.0）

由图6～图9可以看出，本文算法所模拟的机器人信标循迹定位点与实际信标点的偏差，无论是水平X方向的误差，还是竖直Y方向的误差，以及速度矢量角度的偏差，总体来说均小于Hybrid-SLAM、Fast-SLAM、EKSLAM等几种改进的SLAM算法的误差。由于本文算法的总体角度偏差较低，因此在整个循迹行驶过程中，在路径规划方面具备一定的鲁棒性，故循迹效果更好。比较平均误差（表1）可以发现，在每个信标的样点，使用Hybrid-SLAM、Fast-SLAM和 EK-SLAM的总误差，均在0.5～0.6个单位长度之间。而本文使用的位姿自适应卡尔曼滤波SLAM算法，机器人路径规划的实际位置与信标的坐标的平均总偏差只有0.2个单位长度左右，可见定位的精确度明显优于另外三种算法。对比每个信标点处的速度矢量和运动轨迹的夹角值平均偏差（表2），可以看出相对于其他三种算法，本文算法的角度偏差较小，意味着循迹精度更加精确。

表1 四种算法的平均误差比较

表2 四种算法的平均角度误差对比

3.3 几种不同算法的时间对比

循迹时间即完成整个信标循迹过程的耗时。通过对比四种算法的循迹时间（表3）可以知道，本文算法的循迹总耗时较低，即完成整个信标循迹过程的速度较其他三种算法较快。因此，本文算法在机器人整体信标循迹速度方面同样具备优势。

表3 四种算法的循迹时间比较

4 结束语

机器人实时定位与地图构建领域的发展趋势，要求定位速度再快、规划的路径更准确，并对关键点的寻找精度有着更高要求。而信标寻找作为机器人定位与路径规划的一个热点研究方向，目前仍存在较大的探索空间。仿真实验表明，本文提出的位姿自适应卡尔曼滤波算法在SLAM信标循迹的过程中，寻找关键信标点的速度和路径规划的时间也相对较短，同时循迹精度更高，为解决机器人设备关键点信标定位、路径规划和自动驾驶等方面的问题，提供了一个新的思路。