买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：深圳亿嘉和科技研发有限公司

摘要：本发明公开了适应动态环境变化的AMCL定位方法，包括步骤：1机器人运动获取观测数据，根据机器人坐标系在地图坐标系中的位姿将观测数据投影至地图坐标系下；2计算观测数据与离线地图的匹配度，并与设定阈值进行比对；3以各观测数据对应的机器人坐标系位姿为节点通过约束构建位姿图，以匹配度高于设定阈值的观测数据对应的节点为锚点，以匹配度低于设定阈值的观测数据对应的节点为优化项；4得到优化后的机器人坐标系位姿，得到更新后的离线地图；5机器人使用更新后的离线地图进行定位。本发明改善了原有的AMCL仅仅依赖离线地图进行定位，可以提高AMCL定位算法适应动态场景的能力，提升机器人定位的稳定性。

主权项：1.适应动态环境变化的AMCL定位方法，其特征在于：包括步骤：1机器人运动过程中通过测距传感器获取当前观测数据，根据当前机器人坐标系在地图坐标系中的位姿将当前观测数据投影至地图坐标系下；2计算当前观测数据与离线地图的匹配度，并与设定阈值进行比对；若匹配度高于设定阈值，则根据当前观测数据使用概率栅格更新算法更新当前离线地图，并缓存当前观测数据及当前机器人坐标系在地图坐标系的位姿；若匹配度低于设定阈值，则缓存当前观测数据和当前机器人坐标系在地图坐标系的位姿；3以各观测数据对应的机器人坐标系在地图坐标系的位姿为节点通过约束关系构建位姿图，其中，约束关系为各节点之间的相对位姿，以匹配度高于设定阈值的观测数据对应的节点为锚点，以匹配度低于设定阈值的观测数据对应的节点为优化项；4求解步骤3构建的位姿图得到优化后各观测数据对应的机器人坐标系在地图坐标系下的位姿，并将各观测数据投影至地图坐标系下，并使用概率栅格更新算法更新当前地图，得到更新后的离线地图；其中，对匹配度低于设定阈值的观测数据对应的机器人坐标系位姿进行优化具体如下：给定两组观测数据所对应的机器人坐标系在地图坐标系下的位姿：pi＝[xi；θi]T,pj＝[xj；θj]T；其中，xi，xj分别表示第i、j组观测数据所对应的机器人坐标系在地图坐标系下的位置，θi、θj分别表示第i、j组观测数据所对应的机器人坐标系在地图坐标系下的姿态；则从pj到pi的相对变换关系由下面的公式得到： 其中，RθiT表示pj到pi的姿态变换；而通过里程计采集得到的两个对应的机器人坐标系在地图坐标系下的位姿pi'和pj'之间的约束也是一个相对变换其中，表示pi'和pj'之间的位置变换，表示pi'和pj'之间的姿态变换；最终得到的约束方程如下： 由此构建非线性最小二乘问题： 以上公式中，clamp将角度限定在[-π,π]之间；使用非线性优化进行求解对应的机器人坐标系在地图坐标系下的位姿，进而得到需要优化的节点位姿；5机器人使用更新后的离线地图进行定位。

全文数据：

权利要求：

百度查询： 深圳亿嘉和科技研发有限公司 适应动态环境变化的AMCL定位方法