一种实现移动机器人同时定位与地图构建的方法

摘要

本发明公开了一种实现移动机器人同时定位与地图构建的方法。该方法针对移动机器人在未知环境中利用航位推测传感器数据以及路标观测数据，借助于改进的强跟踪滤波技术实现移动机器人的自主定位，并同时构建出环境地图。本发明利用“强跟踪滤波器”，自适应调整卡尔曼增益；引入一种新的多重渐消因子，保证协方差矩阵的对称性，降低求解卡尔曼增益的计算复杂度；针对同时地位与地图构建问题中的观测不连续问题，提出了一种新的算法流程，并给出了一种新的多重渐消因子的计算方法。与传统方法相比，本发明降低了线性化过程引入的误差，提高了机器人的定位精度以及构建地图的精度，同时也能将协方差抑制在一个较小的范围内，提高了所建地图的可信度。

主权项

一种实现移动机器人同时定位与地图构建的方法，其特征在于，包括：步骤S1：确定全局坐标系以及该坐标系下的移动机器人的初始位姿值；步骤S2：记录k‑1时刻的状态估计量以及对应的协方差矩阵，其中状态估计量包括移动机器人的位姿以及路标位置；步骤S3：判断在k时刻是否能够观测到路标，如果在k时刻不能观测到路标，则进行时间更新，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；如果在k时刻能够观测到路标，则进行数据关联，然后执行步骤S4；步骤S4：判断观测到的路标是否为新的特征，如果观测到的路标是新的特征，则进行路标初始化，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；如果观测到的路标是已有的特征，则进行STF预测，然后执行步骤S5；步骤S5：对STF预测的结果进行观测更新，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；步骤S6：重复步骤S2～步骤S5，得到最后时刻的状态估计量，即移动机器人的位姿以及环境中路标的位置，实现同时定位与地图构建。