基于激光SLAM的AGV卡尔曼滤波算法

王亚伟

摘要：AGV自动导航问题研究的是AGV在工作环境中寻找从起始点位置到指定目标点位置的各种可能路径，在AGV安全、快速运动到目标位置点的前提下，用激光SLAM进行导航，通过路径算法决策出一条最优的运动路径，再对AGV进行运动分析及建模，从而准确的确定AGV定位信息。本文从激光SLAM着手，通过扩展卡尔曼滤波和无迹卡尔曼滤波两种算法的仿真分析，选择出最优算法。

关键词：AGV;激光SLAM;扩展卡尔曼滤波;无迹卡尔曼滤波;

1、SLAM的定义

SLAM（Simultaneous Localization and Mapping），也称为CML（Concurrent Mapping and Localization），即定位与地图构建。SLAM最早由Smith、Self和Cheeseman于1988年提出。是指机器人在自身位置不确定的条件下，在完全未知环境中创建地图，同时利用地图进行自主定位和导航。SLAM问题可描述为：机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置预测和传感器数据进行自身定位，同时建造增量式地图。

2、卡尔曼滤波的定义

1960年，卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文（A New Approach to Linear Filtering and Prediction Problems）。在这篇文章里，一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。卡尔曼滤波可以预测信号的过去和当前状态，甚至能预测将来的状态，即使不知道模型的确切性质。本质上来讲，滤波就是一个信号处理与变换的过程，这个过程可以通过硬件来实现，也可以通过软件来实现。卡尔曼滤波属于一种软件滤波方法，基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的预测值和当前时刻的观测值来更新对状态变量的预测，求出当前时刻的预测值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。

3、基于激光SLAM的AGV导航模型与卡尔曼滤波算法

激光SLAM的AGV导航如图2所示。设为AGV瞬时状态时候和障碍物位置的系统状态矩阵，其中表示AGV所处状态，为障碍物的坐标。

设为激光雷达检测到的障碍物位置数据，根据卡尔曼滤波算法，AGV状态转移方程可表示为：

在式中，各量说明如下：

根据t-1时刻预测的系统状态可推算出t时刻的系统先验状态预测，根据系统测量矩阵Zt推算出t时刻的系统后验状态预测。由此可获得基于t-1时刻对t时刻状态的预测值与真实值之间的先验状态预测误差，其协方差矩阵为

同时也得到系统后验状态预测与系统真实值的后验状态预测误差，其协方差矩阵与测量矩阵的预测分别

其中是预测值与测量值之间的误差，若等于0，说明预测值与测量值完全吻合。Kt表示卡尔曼增益，其求取原则是令式（5）的系统后验状态误差趋于零，即将系统后验状态预测带入后验状态预测误差，可解得Kt矩阵值为

可见，测量噪声协方差R越小，卡尔曼增益Kt越大。先验预测协方差越小，Kt越小。也可以说Kt表示预测值与测量值在状态预测时的权重，Kt越大表示测量值Zt的权重越大，Zt预测值的权重越小;反之亦然。

卡尔曼滤波框架包含两个部分：时间更新和测量更新。时间更新推算出当前时间状态变量和误差协方差矩阵，以便为下一个时间状态构造先验预测。测量更新将先验状态预测与新的测量状态结合以构造改进的后验状态预测。

时间更新和测量更新过程循序迭代，这种递归实现的卡尔曼滤波框架相比于粒子滤波算法更加简单。

4、基于扩展的卡尔曼滤波算法

经典的卡尔曼滤波算法要求系统是线性的，而在现实中，系统往往是非线性的。因此，如果利用泰勒级数将非线性系统展开，之后取一次项，这样非线性系统就近似成了线性系统，这样就可以根据卡尔曼滤波框架递推地推算出系统状态预测信息，这就是基于扩展卡尔曼滤波算法的SLAM（EKF-SLAM）。

EKF-SLAM算法实现过程中，AGV的运动模型与测量模型可表示为

其中，与为非线性函数，Xt、Ut、Zt含义同上Wt和Vt表示符合高斯分布的噪声，Wt～N（0，Qt）和Vt～N（0，Rt）。

由于AGV的运动模型和测量模型是非线性的，需要对其进行泰勒级数展开并取一次项进行近视线性化。

5、基于无迹卡尔曼滤波算法

无迹卡尔曼滤波又称无损卡尔曼滤波（Unscented Kalman Filter，UKF），是无损变换（Unscented Transform，UT）与标准卡尔曼滤波体系的结合，通过无损变换使非线性系统方程适用于线性假设下的标准卡尔曼体系。UKF使用的是统计线性化技术，这一技术主要通过n个在先验分布中采集的点的线性回归来线性化随机变量的非线性函数，由于我们考虑的是随机变量的扩展，所以这种线性化要比泰勒级数线性化更准确。

UKF-SLAM算法的复杂性与EKF-SLAM近似，但是UKF-SLAM不用求雅可比矩阵，某些情况下，雅可比矩阵是很难确定的，所以EKF-SLAM不能适用于所有系统。但是如果系统是非高斯的，UKF-SLAM就会有较大的误差，会导致滤波失败。

6、SLAM算法仿真分析

比较EKF-SLAM与UKF-SLAM算法的差别，利用MATLAB对两种算法进行了模拟仿真实验，通过实验进行比较分析。

实验条件：AGV车体上安装激光雷达，最大测量距离为80m，测量分辨率为0.01m，最大测量角度为180°，分辨率为1°。测量结果均加入高斯噪声中。测量采样周期为25ms。AGV速度0.5m/s，轮间距为0.8m，最大转角为30°。

模拟仿真实验分两部分：在约100m长的直线路径附近设置150个路标点，AGV沿该直线路径行走。分别使用EKF-SLAM和UKF-SLAM进行模拟仿真，所得到的AGV实际轨迹和估计轨迹如图3;估计轨迹与实际轨迹的误差如图4。

由仿真结果可知，两种算法的误差最大均是发生在Y轴方向，EKF-SLAM耗时约19分钟，最大误差约为1.57m，而UKF-SLAM耗时约21分钟，最大误差约为0.8m。这两种算法在运行过程中，由于误差的不断累计，AGV预测路径逐渐偏离了真实路径。但是UKF-SLAM在直线行走路径预测比EKF-SLAM误差值小。由此可见UKF-SLAM相比于EKF-SLAM有更好的准确性。

7、总结

基于激光SLAM的运动仿真，对比EKF-SLAM和UKE-SLAM两种算法在直线路径的仿真结果，可知，当检测数据中包含已知信息时，定位构建会更加精确，即如果在定位构建时加入闭环检测环节，SLAM误差会降低。而且UKF-SLAM算法精确度高于EKF-SLAM。当路标密度增加时，两种算法处理时间都会增加，且UKF-SLAM耗时会更多;而当路标密度较低时，UKF-SLAM与EKF-SLAM耗時相差不大。由此可见，UKF-SLAM适用于小范围环境导航，在环境尺寸较大时，导航多采用EKF-SLAM算法。

参考文献：

[1]于洋.AGV及其控制器设计与开发[D].杭州：浙江大学，2008.

[2]白彪才，马殷元.组合导航的AGV定位精度的改善[J].测控技术，2018，37（4）：138.

[3]苏永华.全方位移动式AGV技术研究[J].制造业自动化，2014，36（15）：10.

[4]孙宇，张世琪等.AGV自动导航技术的研究[J].中国机械过程，1996，（6）：31-32.

[5]任永益，饶聃，潘孟春.自动导向车控制系统的数学模型及应用.国防科技大学学报，1997，19（6）：42-47.