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

摘要

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

主权项

1.一种室内机器人定位与建图方法，其特征在于，包括以下步骤：步骤1、基于环境信息进行粒子采样，获取k时刻每个粒子的位姿，并且根据粒子个数平均分配各粒子的权值；步骤2、将获取的k时刻下机器人与目标或障碍物之间距离和偏离角度作为k时刻的实际观测值，根据k时刻的实际观测值和每个粒子的位姿，对每个粒子的权值进行更新并归一化处理；步骤3、根据步骤2更新并归一化处理后每个粒子的权值，计算得到有效粒子数；判断有效粒子数小于设定的阈值时，对粒子进行重采样，并结合k时刻的实际观测值和重采样粒子的位姿，计算并更新重采样粒子的权值；及判断有效粒子数大于设定的阈值时，保持原采样粒子及每个粒子的权值；步骤4、根据步骤3所得重采样粒子权值或原采样粒子权值，利用加权公式计算出当前状态的期望值对k时刻的机器人的实际位姿进行更新；步骤5、根据步骤1所获取k时刻每个粒子的位姿和k时刻的实际观测值，利用卡尔曼滤波算法更新每个粒子地图的均值和方差，以得全部粒子的地图信息；并由步骤4所更新k时刻机器人的实际位姿结合k时刻的实际观测值、机器人所在实际位姿对应粒子的地图信息，建立机器人实际地图。