[发明专利]基于抗差秩卡尔曼滤波的轮式机器人状态估计方法及自主导航方法

说明书

本发明公开了一种基于抗差秩卡尔曼滤波的轮式机器人状态估计方法及自主导航方法，旨在解决现有的SLAM技术无法处理非线性不确定系统、导航定位精度不高的技术问题。本发明中抗差秩卡尔曼滤波‑SLAM自主导航方法对于轮式机器人的系统模型是否是高斯分布没有要求，且当量测数据的噪声统计受未知环境影响不准确时具有更强的鲁棒性和导航定位精度，有利于提高轮式机器人在未知环境中的定位与建图的精度。

技术领域

本发明涉及机器人自主导航技术领域，具体涉及一种基于抗差秩卡尔曼滤波的轮式机器人状态估计方法及自主导航方法。

背景技术

轮式机器人在人们的生产、生活的各个领域发挥着重要作用，比如生活助理、流水线生产、巡逻、灾难救援、空间探测等等。轮式机器人要实现在未知环境中的自主移动，首先要对周围环境进行感知和识别，以及精确定位。机器人的精确定位和环境感知问题是机器人实现自主导航的关键技术，也是机器人研究领域的热点。

同步地定位与地图创建(Simultaneous Localization and Mapping，SLAM)技术，是解决机器人在未知环境中导航问题的最佳手段。机器人的SLAM技术在移动过程中通过传感器来精确估计自身位姿，同时增量式的构建真实环境地图。SLAM技术主要包括两方面内容：建立准确的环境地图和实现精确的定位能力。现有扩展卡尔曼滤波(EKF)-SLAM技术解决了卡尔曼滤波智能处理线性高斯系统的限制，但EKF是利用泰勒展开的一阶项来近似非线性系统，无法处理强非线性系统问题，处理过程中出现的截断误差严重影响了机器人的导航定位精度。

发明内容

本发明提供一种基于抗差秩卡尔曼滤波-SLAM(抗差RKF-SLAM)的轮式机器人状态估计方法及自主导航方法，旨在解决现有的SLAM方法无法处理非线性不确定系统、导航定位精度不高的技术问题。

为解决上述技术问题，本发明采用如下技术方案：

设计一种基于抗差秩卡尔曼滤波的轮式机器人状态估计方法，包括如下步骤：

S1)初始化轮式机器人的初始状态和初始方差P₀：

S2)对轮式机器人的位置、方位角等状态进行采样，得到秩采样点集为：

式中，χ_k-1,i为轮式机器人第k-1时刻的状态的第i个采样点，共有4n个样本点；n为的维数；表示的是P_k-1平方根的第i列向量；u_p1与u_p2为正态偏量，其中p₁＝0.6852,p₂＝0.8704,u_p1＝0.4823,u_p2＝1.1281；

S3)对轮式机器人第k步的状态进行一步预测：

式中，为第i个采样点的预测值；为轮式机器人的状态预测值；

S4)获取协方差的一步预测：

式中，P_k/k-1为轮式机器人的一步预测误差；Q_k-1为过程噪声的方差；为协方差权重系数；

S5)对状态一步预测的进行重新秩采样：

式中，χ_k,i为的第i个采样点；

S6)计算量测预测值和量测预测方差P_zz：