[发明专利]一种室内机器人定位与建图方法及装置

说明书

本发明公开了一种室内机器人定位与建图方法及装置，包括：进行粒子采样，获取k时刻每个粒子的位姿，并且根据粒子个数平均分配各粒子的权值；获取k时刻的实际观测值，结合每个粒子的位姿对每个粒子的权值进行更新并归一化处理；根据更新后的每个粒子权值，计算得到有效粒子数；判断有效粒子数小于设定的阈值时，对粒子进行重采样及计算更新重采样粒子权值，否则大于时保持原采样粒子和其权值；根据更新后重采样或原采样粒子权值，利用当前状态的期望值对k时刻的机器人的实际位姿更新；利用卡尔曼滤波算法更新地图的均值和方差，以得全部粒子的地图信息，根据实际位姿建立机器人地图。本发明可在保证地图精度的同时大大提高了建图的实时性。

技术领域

本发明涉及一种室内机器人定位与建图方法及装置，属于移动机器人技术领域。

背景技术

室内自主移动机器人具有广泛的应用前景，其实现自主移动需要同时具备环境建图、自主定位、路径规划等能力，而实现机器人的定位与建图功能是核心问题，也是当前机器人领域的研究热点。

基于激光雷达方式的SLAM因其稳定、可靠、高性能等技术优势，依然占据着机器人定位导航的重要位置。目前SLAM问题的解决方法以概率估计为主，主要分为卡尔曼滤波算法和粒子滤波算法，基于粒子滤波方式的算法由于不受强线性和强高斯噪声的限制，得到了更多的使用，当前使用最广泛的粒子滤波算法是RBPF算法，所述方法将SLAM问题分解为机器人自身定位和环境估计两个问题，用粒子滤波器进行路径的估计，用卡尔曼滤波器进行环境特征的估计，可以实现不错的效果。

传统的RBPF-SLAM算法在环境特征比较少的情况下可以实现比较精确地定位，随着所需粒子数目的增多、环境地图尺寸增大时，算法会占用大量的内存空间，导致算法效率比较低。

发明内容

本发明所要解决的技术问题在于，克服传统建图算法在复杂环境下带来的占用大量的内存空间、存在延迟的问题，提供一种室内机器人定位与建图方法及装置，在进行环境特征估计时利用粒子的统计特性可以把复杂的运算进行相应的简化，提高了系统实时性。

本发明具体采用以下技术方案解决上述技术问题：

一种室内机器人定位与建图方法，包括以下步骤：

步骤1、基于环境信息进行粒子采样，获取k时刻每个粒子的位姿，并且根据粒子个数平均分配各粒子的权值；

步骤2、将获取的k时刻下机器人与目标或障碍物之间距离和偏离角度作为k时刻的实际观测值，根据k时刻的实际观测值和每个粒子的位姿，对每个粒子的权值进行更新并归一化处理；

步骤3、根据步骤2更新并归一化处理后每个粒子的权值，计算得到有效粒子数；判断有效粒子数小于设定的阈值时，对粒子进行重采样，并结合k时刻的实际观测值和重采样粒子的位姿，计算并更新重采样粒子的权值；及判断有效粒子数大于设定的阈值时，保持原采样粒子及每个粒子的权值；

步骤4、根据步骤3所得重采样粒子权值或原采样粒子权值，利用加权公式计算出当前状态的期望值对k时刻的机器人的实际位姿进行更新；

步骤5、根据步骤1所获取k时刻每个粒子的位姿和k时刻的实际观测值，利用卡尔曼滤波算法更新每个粒子地图的均值和方差，以得全部粒子的地图信息；并由步骤4所更新k时刻机器人的实际位姿结合k时刻的实际观测值、机器人所在实际位姿对应粒子的地图信息，建立机器人实际地图。

进一步地，作为本发明的一种优选技术方案，所述步骤1还包括判断k时刻是否为初始时刻，是则生成初始时刻的粒子位姿，否则根据上一代粒子集中粒子的位姿结合运动模型得到k时刻每个粒子的预估位姿。

进一步地，作为本发明的一种优选技术方案，所述步骤2中对每个粒子的权值进行更新，采用公式：