基于秩卡尔曼滤波的无人车SLAM导航方法技术

本发明专利技术提供一种基于秩卡尔曼滤波的无人车SLAM导航方法以解决现有的SLAM导航定位精度不高、稳定性不足的技术问题。本发明专利技术包括以下步骤：1、对移动机器人进行建模，建立移动机器人的动力学模型和观测模型，并初始化；2、SLAM的预测和更新；3、采用秩卡尔曼滤波对移动机器人的位姿信息进行更新并输出。本发明专利技术的有益效果在于：对动力学模型是否满足高斯分布没有要求，而误差小，在未知环境中的定位与建图精度更高。

Slam navigation method of unmanned vehicle based on rank Kalman filter

【技术实现步骤摘要】
基于秩卡尔曼滤波的无人车SLAM导航方法
本专利技术涉及无人车自主导航领域，具体涉及一种基于秩卡尔曼滤波的无人车SLAM导航方法。
技术介绍
SLAM是一种解决无人车在未知环境中导航问题的技术。这种技术就是无人车运用自身所带的传感器进行数据的获得，计算并确定自身所在的环境信息，同时又用自身所得的环境信息更新自身的位置。同时也成为机器人导航算法中的热门算法。SLAM算法的研究侧重于使用滤波器理论，一般使用里程计的输入为滤波器的预测输入，激光雷达或者视觉传感器输出为滤波器更新阶段的输入。最为传统的方法就是将扩展卡尔曼滤波(EKF)引入SLAM里边，其实际就是将非线性化的模型使用扩展卡尔曼滤波进行处理。EKF是将非线性函数展开成Taylor级数并略去二阶及以上项，它可以解决一部分的非线性问题，但是当系统非线性较严重时，它近似线性化所产生的截断误差会降低滤波精度和稳定性，因此在实际工程中，EKF只能对弱非线性系统进行局部线性化处理。
技术实现思路
本专利技术提供一种基于秩卡尔曼滤波的无人车SLAM导航方法以解决现有的SLAM导航定位精度不高、稳定性不足的技术问题。为解决上述技术问题，本专利技术采用如下技术方案：设计一种基于秩卡尔曼滤波的无人车SLAM导航方法，包括以下步骤：步骤一：建立无人车的动力学模型和观测模型如下：其中，为k时刻移动机器人的位姿信息，uk是nu维控制向量，zk是nz维的观测向量，wk和vk分别为系统噪声向量和量测噪声向量；步骤二：建立SLAM模型并进行预测和更新，所述SLAM模型为：式中为k时刻的特征地图；所述预测是通过无人车的状态模型和k-1时刻的后验概率分布来得到k时刻的先验概率分布：这一概率分布对应着上述无人车的动力学模型所述更新是量测方程获得k时刻的测量值zk，然后得到后验概率分布：这一概率分布对应着上述无人车的观测模型zk＝h(xk)+vk。因此，整个无人车SLAM导航方法就可以用卡尔曼滤波来解决。步骤三：对无人车的位姿信息采样，得到秩采样点集为：式中，χk-1，i为的第i个采样点，共有4n个样本点；n为的维数。表示的是Pk-1平方根的第i列向量；up为标准正态偏量；为无人车第k-1时刻的状态；步骤四：获取无人车第k步的状态一步预测：xk/k-1，i＝f(χk-1，i)，i＝1，2，…，4n(3)式中xk/k-1，i表示的是第i个采样点的状态预测值；表示的是移动机器人的状态预测值；步骤五：获取协方差的一步预测：式中，Pk/k-1表示的是移动机器人的状态一步预测误差协方差；Qk-1表示的是过程噪声的协方差；步骤六：由于步骤五中获取的协方差的一步预测中包含过程噪声的协方差Qk-1，因此将进行对状态一步预测的重新秩采样：式中，χk/k-1，i为的第i个采样点；为状态的一步预测；Pk/k-1为协方差的一步预测；步骤七：获取量测预测值zk/k-1，i：zk/k-1，i＝h(χk/k-1，i)，i＝1，2，…，4n(7)式中，zk/k-1，i表示的是第i个采样点的测量预测值；表示的是移动机器人的测量预测值；步骤八：获取滤波增益矩阵Kk：式中，Pzz表示移动机器人测量预测值协方差；Pxz表示状态预测值与测量预测值的交叉协方差；Kk表示秩卡尔曼滤波的增益；步骤九：获取状态更新：步骤十：获取状态估计误差方差Pk：步骤十一：循环迭代步骤三至步骤十，得到无人车的状态估计值进一步的，步骤1中wk和vk的方差阵分别为Qk和Rk，并且满足：进一步的，Qk和Rk分别为：和进一步的，步骤1中无人车的初始状态为：进一步的，无人车的初始状态值为初始误差方差矩阵与现有技术相比，本专利技术的有益技术效果在于：本专利技术中秩卡尔曼滤波的SLAM导航方法对于无人车的动力学模型是否是高斯分布没有要求，并且本专利技术采用秩滤波的误差更小，效果更好，对于无人车在未知环境中的定位与建图精度更高，对于未知环境的适应能力也更强。附图说明图1为本专利技术的仿真实验结果图；图2为本专利技术在无人车导航时使用EKF-SLAM和RKF-SLAM算法的x轴均方根误差对比图；图3为本专利技术在无人车导航时使用EKF-SLAM和RKF-SLAM算法的y轴的均方根误差图。具体实施方式下面结合附图和实施例来说明本专利技术的具体实施方式，但以下实施例只是用来详细说明本专利技术，并不以任何方式限制本专利技术的范围。以下实施例中所涉及或依赖的程序均为本
的常规程序或简单程序，本领域技术人员均能根据具体应用场景做出常规选择或者适应性调整。实施例1：一种基于秩卡尔曼滤波的无人车SLAM导航方法，包括以下步骤：步骤一：对无人车进行建模，建立无人车的动力学模型和观测模型，并初始化；选取系统的状态信息向量为则系统的动力学模型和观测模型：其中，为k时刻无人车的位姿信息，为k时刻的特征地图，uk是nu维控制向量，zk是nz维的观测向量，wk和vk分别为系统噪声向量和量测噪声向量，其方差阵分别为Qk和Rk，并且满足其中，初始化该系统的状态模型其中，初始状态初始误差方差矩阵P0、Qk和Rk均不相关；步骤二：SLAM的预测和更新SLAM模型为：其中，zk，uk可由zk＝h(xk)+vk，uk＝(v，γ)(包含前向速度与角速度)计算。采用贝叶斯滤波的原理计算上式的后验概率分布，这样就可以得到最优状态估计，也就是当规划完路径后，通过计算使无人车在导航的过程中可以沿着最接近规划路径的轨迹行驶。整个计算步骤分为两步：第一步预测是通过小车的状态模型和k-1时刻的后验概率分布来得到k时刻的先验概率分布：这一概率分布对应着上述无人车的动力学模型第二步更新是由量测方程获得k时刻的测量值zk，然后算后验概率分布：这一概率分布对应着上述无人车的观测模型zk＝h(xk)+vk。因此，整个无人车SLAM导航方法就可以用卡尔曼滤波来解决。步骤三：采用秩卡尔曼(RKF)滤波对无人车的位姿信息进行更新并输出1、计算秩采样点：针对m＝2采样策略和对称分布情况的秩采样点集{χk-1，i}为式中，χk-1，i为的第i个采样点，共有4n个样本点；n为的维数。表示的是Pk-1平方根的第i列向量；为无人车第k-1时刻的状态；up为标准正态偏量，用中位秩计算pj＝(j+2.7)/5.4，p1＝0.6852，p2＝0.8704，ω表示的是协方差权重系数，通常2、计算状态一步预测：设k-1时刻的状态估计及其误差本文档来自技高网...

【技术保护点】
1.一种基于秩卡尔曼滤波的无人车SLAM导航方法，其特征在于，包括：/n步骤一：建立无人车的动力学模型和观测模型如下：/n

【技术特征摘要】
1.一种基于秩卡尔曼滤波的无人车SLAM导航方法，其特征在于，包括：
步骤一：建立无人车的动力学模型和观测模型如下：



其中，为k时刻移动机器人的位姿信息，uk是nu维控制向量，zk是nz维的观测向量，wk和vk分别为系统噪声向量和量测噪声向量；
步骤二：建立SLAM模型并进行预测和更新，所述SLAM模型为：式中为k时刻的特征地图；
所述预测是通过无人车的状态模型和k-1时刻的后验概率分布来得到k时刻的先验概率分布：这一概率分布对应着上述无人车的动力学模型所述更新是量测方程获得k时刻的测量值zk，然后得到后验概率分布：这一概率分布对应着上述无人车的观测模型zk＝h(xk)+vk；
步骤三：对无人车的位姿信息采样，得到秩采样点集为：



式中，χk-1，i为的第i个采样点，共有4n个样本点；n为的维数。表示的是Pk-1平方根的第i列向量；up为标准正态偏量；为无人车第k-1时刻的状态；
步骤四：获取无人车第k步的状态一步预测：
xk/k-1，i＝f(χk-1，i)，i＝1，2，…，4n(3)

式中xk/k-1，i表示的是第i个采样点的状态预测值；表示的是移动机器人的状态预测值；
步骤五：获取协方差的一步预测：



式中，Pk/k-1表示的是移动机器人的状态一步预测误差协方差；Qk-1表示的是过程噪声的协方差；
步骤六：对状态一步预测进行重新秩采样：



式中，χk/k...

【专利技术属性】
技术研发人员：娄泰山，班鸿业，赵素娜，贺振东，丁国强，焦玉召，王晓雷，
申请(专利权)人：郑州轻工业大学，
类型：发明
国别省市：河南;41