一种基于自适应无迹卡尔曼滤波的激光SLAM算法的制作方法

本发明属于slam导航定位算法技术领域，特别是一种基于自适应无迹卡尔曼滤波的激光slam算法。

背景技术：

即时定位与地图构建(slam)技术于1986年的ieee机器人与自动化会议正式提出，经过曲折而漫长的探索，slam技术的发展逐渐成熟。近年来，随着优化理论的引入，slam技术进入了新的高速发展期。2011年stefankohlbrecher，oskarvonstryk等人提出了一种基于优化理论的hectorslam算法，hectorslam是一款高效快速的在线slam算法，在仅依靠激光数据的情况下，也能得到较高的定位和构图精度。但hectorslam算法在长廊等特征雷同的环境仍然无法得到准确的定位。此外，激光定位有可能扫描到行人或装饰等干扰特征，此时单纯依靠激光容易产生错误的定位。因此通过卡尔曼滤波算法，融合激光数据与里程计数据，可以消除或减少这些缺陷。

标准的卡尔曼滤波是针对线性系统提出的，但实际机器人系统往往是较为复杂的非线性系统。针对非线性系统，可以使用扩展卡尔曼滤波(ekf)或者无迹卡尔曼滤波(ukf)进行融合估计。前者对非线性部分进行一阶泰勒展开以完成线性化，但对于高阶部分难以忽略，或者求导困难的系统则难以适用。后者则通过无迹变换(ut)来近似得到变量经非线性变换后的概率分布，即对一个随机变量按分布进行sigma点采样，对这些点进行非线性变换，并用得到的结果描述变量非线性变换后的变量的分布。

无迹卡尔曼滤波的最优估计建立在系统噪声和量测噪声特性已知的基础上，而这一点在现实中往往难以满足，系统噪声和量测噪声的统计特性通常未知，甚至可能是变化的。综上，现有技术存在的问题是：实际中的系统噪声和量测噪声的统计特性通常未知，甚至可能是变化的，往往导致陷入局部极值的结果，激光扫描到的干扰数据较多，使最终的滤波结果不理想，定位精度差。

技术实现要素：

本发明的目的在于提供一种基于自适应无迹卡尔曼滤波的激光slam算法，能够在线估计系统噪声或者量测噪声，使得非线性系统能得到更好的估计效果。

实现本发明目的的技术解决方案为：一种基于自适应无迹卡尔曼滤波的激光slam算法，包括以下步骤：

步骤1、利用hectorslam算法，基于激光束末端与已知地图的匹配优化实现定位，读取激光数据，再根据激光传感器坐标系与机器人坐标系的关系，求出机器人在全局坐标中的位姿；

步骤2、利用无迹卡尔曼滤波算法，在非线性系统中，融合激光预估位姿数据和里程计推算位姿数据，作为实际估计位姿；

步骤3、计算自适应向量，更新测量噪声协方差矩阵，然后使用自适应无迹卡尔曼滤波算法，对非线性系统进行滤波处理。

进一步地，步骤1所述的利用hectorslam算法，基于激光束末端与已知地图的匹配优化实现定位，读取激光数据，再根据激光传感器坐标系与机器人坐标系的关系，求出机器人在全局坐标中的位姿，具体如下：

步骤1.1、利用hectorslam算法，基于激光束末端与已知地图的匹配优化实现定位，将定位问题转换为一个优化问题：在全局坐标系中寻找一个位姿ξ＝[x,y,θ]^t，使得最小，再根据激光传感器坐标系与机器人坐标系的关系，求出机器人在全局坐标中的位姿；其中x、y是点ξ的横、纵坐标，θ是该点的位姿角度；

步骤1.2、读取激光数据，si(ξ)表示激光中心在位姿ξ时第i束激光末端在全局坐标系中的位置；设第i束激光末端在激光传感器坐标系中的位置si＝[six,siy]^t，则：

其中，m(si(ξ))表示该点的占据概率，ξk、ξk-1分别是k和k-1时刻点ξ的位置；

步骤1.3、根据激光传感器坐标系与机器人坐标系的关系，求出机器人在全局坐标中的位姿。

进一步地，步骤2所述的利用无迹卡尔曼滤波算法，在非线性系统中，融合激光预估位姿数据和里程计推算位姿数据，作为实际估计位姿，具体如下：

步骤2.1、初始化状态量和协方差矩阵：

其中，为估计的状态初值，x0为估计过程中的状态变量，p0为估计的协方差矩阵初值；

步骤2.2、将状态量进行sigma点采样，并做无迹变换：

其中，即k-1时刻的第i个采样点；λ＝α²×(n+κ)-n，n为变量维数，α为决定sigma点分布状态的变量，调节α使高阶项影响最小，取值范围为0～1；κ取值范围为3～n；l为协方差矩阵p的cholesky分解，p＝l×l^t，为估计状态k-1时刻的值；

步骤2.3、由时间传播方程得出状态量的时间更新，即依据运动模型，根据上一时刻的状态量和协方差矩阵预估当前时刻的状态：

其中：分别为计算状态量时和计算协方差矩阵时的第i个样本的权重，β取值为2；为第i个样本的预估观测值，zk为计算k时刻的预估观测值，为加权计算的k时刻前的预估观测值，f为非线性函数，是分别将的每一个列向量通过f变换得到的每一个列向量，为估计状态k时刻前的值，为点x在k时刻前的协方差矩阵，qk系统噪声方差矩阵；类似于变换；wi^(m)是均值的权重，wi^(c)是协方差的权重；

步骤2.4、测量更新方程，根据观测到的传感器数据对预估值进行更新，从而得到最优状态量估计值：

其中，rk为观测噪声方差矩阵，pz,k为k时刻的预估协方差矩阵，pxz,k为x点在k时刻的预估协方差矩阵，k是修正系数。

进一步地，步骤3所述的计算自适应向量，更新测量噪声协方差矩阵，然后使用自适应无迹卡尔曼滤波算法，对非线性系统进行滤波处理，具体如下：

步骤3.1、计算自适应向量：

其中，dk为n×1的加权系数向量，ones(n,1)表示n×1的全一列向量，b为n×1的遗忘因子向量，b^k表示b中各元素k次方得到的向量；

步骤3.2、更新噪声协方差矩阵：

其中，diag(a)表示取向量a或方阵a的对角向量构成对角矩阵，abs(b)表示对向量b或方阵b内元素取绝对值，为加权的观测噪声方差矩阵，pz,k-1为k-1时刻的预估协方差矩阵；

步骤3.3、更新测量方程，使用自适应无迹卡尔曼滤波算法，对非线性系统进行滤波。

进一步地，设定系统噪声已知，使用sage-husa自适应算法根据系统噪声对量测噪声进行估计。

本发明与现有技术相比，其显著优点为：(1)能够在仅依靠激光数据的情况下，得到较高的定位和构图精度；(2)能够避免陷入局部极值的情况，消除或减少激光扫描到的干扰数据；(3)解决了实际系统噪声或量测噪声特性未知或变化的问题，使非线性系统能得到更好的估计效果，提高了定位的精度。

附图说明

图1为本发明基于自适应无迹卡尔曼滤波的激光slam算法的流程示意图。

图2为本发明中的自适应无迹卡尔曼滤波的流程示意图。

具体实施方式

下面结合附图和实施例来详细说明本发明。

如图1所示，本发明基于自适应无迹卡尔曼滤波的激光slam算法，包括如下步骤：

步骤1、利用hectorslam算法，基于激光束末端与已知地图的匹配优化实现定位，读取激光数据，再根据激光传感器坐标系与机器人坐标系的关系，求出机器人在全局坐标中的位姿；

步骤2、利用无迹卡尔曼滤波算法，在非线性系统中，融合激光预估位姿数据和里程计推算位姿数据，作为实际估计位姿；

步骤3、计算自适应向量，更新测量噪声协方差矩阵，然后使用自适应无迹卡尔曼滤波算法，对非线性系统进行滤波处理。

优选的，步骤1所述的利用hectorslam算法，基于激光束末端与已知地图的匹配优化实现定位，读取激光数据，再根据激光传感器坐标系与机器人坐标系的关系，求出机器人在全局坐标中的位姿，具体如下：

步骤1.1、利用hectorslam算法，基于激光束末端与已知地图的匹配优化实现定位，将定位问题转换为一个优化问题：在全局坐标系中寻找一个位姿ξ＝[x,y,θ]^t，使得最小，再根据激光传感器坐标系与机器人坐标系的关系，求出机器人在全局坐标中的位姿；其中x、y是点ξ的横、纵坐标，θ是该点的位姿角度；

步骤1.2、读取激光数据，si(ξ)表示激光中心在位姿ξ时第i束激光末端在全局坐标系中的位置；设第i束激光末端在激光传感器坐标系中的位置si＝[six,siy]^t，则：

其中，m(si(ξ))表示该点的占据概率，ξk、ξk-1分别是k和k-1时刻点ξ的位置；

步骤1.3、根据激光传感器坐标系与机器人坐标系的关系，求出机器人在全局坐标中的位姿。

优选的，步骤2所述的利用无迹卡尔曼滤波算法，在非线性系统中，融合激光预估位姿数据和里程计推算位姿数据，作为实际估计位姿，具体如下：

步骤2.1、初始化状态量和协方差矩阵：

其中，为估计的状态初值，x0为估计过程中的状态变量，p0为估计的协方差矩阵初值；

步骤2.2、将状态量进行sigma点采样，并做无迹变换：

其中，即k-1时刻的第i个采样点；λ＝α²×(n+κ)-n，n为变量维数，α为决定sigma点分布状态的变量，调节α使高阶项影响最小，取值范围为0～1；κ取值范围为3～n；l为协方差矩阵p的cholesky分解，p＝l×l^t，为估计状态k-1时刻的值；

步骤2.3、由时间传播方程得出状态量的时间更新，即依据运动模型，根据上一时刻的状态量和协方差矩阵预估当前时刻的状态：

其中：分别为计算状态量时和计算协方差矩阵时的第i个样本的权重，β取值为2；为第i个样本的预估观测值，zk为计算k时刻的预估观测值，为加权计算的k时刻前的预估观测值，f为非线性函数，是分别将的每一个列向量通过f变换得到的每一个列向量，为估计状态k时刻前的值，为点x在k时刻前的协方差矩阵，qk系统噪声方差矩阵；类似于变换；wi^(m)是均值的权重，wi^(c)是协方差的权重；

步骤2.4、测量更新方程，根据观测到的传感器数据对预估值进行更新，从而得到最优状态量估计值：

其中，rk为观测噪声方差矩阵，pz,k为k时刻的预估协方差矩阵，pxz,k为x点在k时刻的预估协方差矩阵，k是修正系数。

优选的，步骤3所述的计算自适应向量，更新测量噪声协方差矩阵，然后使用自适应无迹卡尔曼滤波算法，对非线性系统进行滤波处理，具体如下：

步骤3.1、计算自适应向量：

其中，dk为n×1的加权系数向量，ones(n,1)表示n×1的全一列向量，b为n×1的遗忘因子向量，b^k表示b中各元素k次方得到的向量；

步骤3.2、更新噪声协方差矩阵：

其中，diag(a)表示取向量a或方阵a的对角向量构成对角矩阵，abs(b)表示对向量b或方阵b内元素取绝对值，为加权的观测噪声方差矩阵，pz,k-1为k-1时刻的预估协方差矩阵；

步骤3.3、更新测量方程，使用自适应无迹卡尔曼滤波算法，对非线性系统进行滤波。

优选的，sage-husa自适应算法只能在系统噪声和量测噪声已知其一的情况下，估计另一个，由于机器人系统运动学模型和运动环境简单可靠，系统噪声相对稳定，因此设定系统噪声为已知噪声。

优选的，在完成步骤3之后，即在步骤2中加入自适应向量后，只需每次采样重复步骤2.2～步骤2.4即可在线得到最优的估计状态。

综上所述，本发明能够在仅依靠激光数据的情况下，得到较高的定位和构图精度；并且能够避免陷入局部极值的情况，消除或减少激光扫描到的干扰数据；最终解决了实际系统噪声或量测噪声特性未知或变化的问题，使非线性系统能得到更好的估计效果，提高了定位的精度。