适应动态环境变化的AMCL定位方法技术

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

【技术实现步骤摘要】
适应动态环境变化的AMCL定位方法


[0001]本专利技术涉及定位领域，尤其涉及适应动态环境变化的AMCL定位方法。

技术介绍

[0002]AMCL算法是一种基于滤波的定位算法，主要通过测距传感器的实时数据和加载的离线地图进行匹配从而计算得到机器人在地图中的准确位姿。AMCL定位算法广泛应用于无人驾驶和机器人等行业。
[0003]蒙特卡洛定位(MCL)基于粒子滤波算法，其一大优点是不受场景的限制，算法简捷快速，同时也可以兼顾算法的精度问题。粒子滤波使用粒子密度(也就是单位区间内的粒子数)表征事件的发生概率。根据选定的评估方程来推算事件的置信程度，并根据该结果重新调整粒子的分布情况。经过若干次迭代之后，粒子就集中分布在可能性高的区域了。基于这种推论，MCL将复杂的数学运算，转换成了计算机更易理解的迭代求解，在机器人定位问题上得到了很好的泛化。
[0004]当前AMCL定位算法的缺点在于其加载的是离线的地图文件。而在实际应用中，环境经常是变化的，比如商场中不断变化的人流，室外环境中路边不时停靠的车辆等等，在动态环境中AMCL定位算法的稳定性较差。

技术实现思路

[0005]专利技术目的：针对上述不足，本专利技术提出了一种适应动态环境变化的AMCL定位方法，改进了AMCL定位算法对于离线地图的依赖性，使其可以在局部动态环境下具有更强的适应性。
[0006]技术方案：
[0007]适应动态环境变化的AMCL定位方法，包括步骤：
[0008](1)机器人运动过程中通过测距传感器获取当前观测数据，根据当前机器人坐标系在地图坐标系中的位姿将当前观测数据投影至地图坐标系下；
[0009](2)计算当前观测数据与离线地图的匹配度，并与设定阈值进行比对；
[0010]若匹配度高于设定阈值，则根据当前观测数据使用概率栅格更新算法更新当前离线地图，并缓存当前观测数据及当前机器人坐标系在地图坐标系的位姿；
[0011]若匹配度低于设定阈值，则缓存当前观测数据和当前机器人坐标系在地图坐标系的位姿；
[0012](3)以各观测数据对应的机器人坐标系在地图坐标系的位姿为节点通过约束构建位姿图，其中，约束关系为各节点之间的相对位姿，以匹配度高于设定阈值的观测数据对应的节点为锚点，以匹配度低于设定阈值的观测数据对应的节点为优化项；
[0013](4)求解步骤(3)构建的位姿图得到优化后各观测数据对应的机器人坐标系在地图坐标系下的位姿，并将各观测数据投影至地图坐标系下，并使用概率栅格更新算法更新当前地图，得到更新后的离线地图；
[0014](5)机器人使用更新后的离线地图进行定位。
[0015]适应动态环境变化的AMCL定位方法，包括步骤：
[0016](1)机器人运动过程中通过测距传感器获取观测数据并缓存，同时缓存对应的机器人坐标系在地图坐标系的位姿；
[0017](2)以步骤(1)得到的各观测数据对应的机器人坐标系在地图坐标系的位姿为节点通过约束构建位姿图，其中，约束关系为各节点之间的相对位姿，优化项为各观测数据对应的机器人坐标系在地图坐标系的位姿；
[0018](3)根据各观测数据对应的机器人坐标系在地图坐标系中的位姿，将相应观测数据投影至地图坐标系下，并计算相应观测数据与离线地图的匹配度，并与设定阈值进行比对；
[0019](4)以匹配度高于设定阈值的观测数据对应的机器人坐标系位姿为锚点，对匹配度低于设定阈值的观测数据对应的机器人坐标系位姿进行优化得到优化后的所有机器人坐标系位姿，并利用概率栅格图更新方式将所有观测数据更新到当前离线地图得到更新后的离线地图；
[0020](5)机器人使用更新后的离线地图进行定位。
[0021]所述当前机器人坐标系在地图坐标系中的位姿通过机器人置于离线地图中的初始位姿及机器人的里程计和机器人转动编码器计算得到。
[0022]所述测距传感器采用激光雷达或深度相机，通过激光雷达或深度相机实时获取当前帧激光点云。
[0023]计算观测数据与离线地图的匹配度具体如下：
[0024](31)寻找得到当前离线地图中与当前观测数据投影至地图坐标系中的各个像素距离最近的占据栅格，并计算各个像素与其距离最近的占据栅格之间的距离；
[0025]其中，某一像素与其距离最近的占据栅格之间的距离计算如下：
[0026]占据栅格为当前离线地图中障碍物所在栅格，其像素值为1；若某一像素在当前离线地图中与其对应的栅格的像素值也为1，则某一像素与其距离最近的占据栅格之间的距离为0；否则，根据某一像素在当前离线地图中对应的栅格在地图坐标系中的坐标，计算得到与其距离最近的占据栅格之间沿x方向相距m个像素，沿y方向相距n个像素，那么二者之间的距离为
[0027]当前离线地图上某一像素与其距离最近的占据栅格之间的距离设定上限值
u
，若二者距离超过上限值，则认定二者的距离为上限值u；
[0028](32)根据步骤(31)计算得到的所有像素与其距离最近的占据栅格之间的距离求和并取平均值q，进而得到匹配度p，其中p＝1
‑
q/u。
[0029]当前离线地图上某一像素与其距离最近的占据栅格之间的距离设定上限值u＝10r。
[0030]设定阈值根据实际需要设定，取值范围0～1。
[0031]对匹配度低于设定阈值的观测数据对应的机器人坐标系位姿进行优化具体如下：
[0032]给定两组观测数据所对应的机器人坐标系在地图坐标系下的位姿：p
i
＝[x
i
；θ
i
]T
，p
j
＝[x
j
；θ
j
]T
；
[0033]其中，x
i
，x
j
分别表示第i、j组观测数据所对应的机器人坐标系在地图坐标系下的
位置，θ
i
、θ
j
分别表示第i、j组观测数据所对应的机器人坐标系在地图坐标系下的姿态；则从p
j
到p
i
的相对变换关系由下面的公式得到：
[0034][0035]其中，R(θ
i
)
T
表示p
j
到p
i
的姿态变换；
[0036]而通过里程计采集得到的两个对应的机器人坐标系在地图坐标系下的位姿p
i
′
和p
j
′
之间的约束也是一个相对变换其中，表示p
i
′
和p
j
′
之间的位置变换，表示p
i
′
和p
j
′
之间的姿态变换；
[0037]最终得到的约束方程如下：
[0038][0039]由此构建非线性最小二乘问题：
[0040][004本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.适应动态环境变化的AMCL定位方法，其特征在于：包括步骤：(1)机器人运动过程中通过测距传感器获取当前观测数据，根据当前机器人坐标系在地图坐标系中的位姿将当前观测数据投影至地图坐标系下；(2)计算当前观测数据与离线地图的匹配度，并与设定阈值进行比对；若匹配度高于设定阈值，则根据当前观测数据使用概率栅格更新算法更新当前离线地图，并缓存当前观测数据及当前机器人坐标系在地图坐标系的位姿；若匹配度低于设定阈值，则缓存当前观测数据和当前机器人坐标系在地图坐标系的位姿；(3)以各观测数据对应的机器人坐标系在地图坐标系的位姿为节点通过约束构建位姿图，其中，约束关系为各节点之间的相对位姿，以匹配度高于设定阈值的观测数据对应的节点为锚点，以匹配度低于设定阈值的观测数据对应的节点为优化项；(4)求解步骤(3)构建的位姿图得到优化后各观测数据对应的机器人坐标系在地图坐标系下的位姿，并将各观测数据投影至地图坐标系下，并使用概率栅格更新算法更新当前地图，得到更新后的离线地图；(5)机器人使用更新后的离线地图进行定位。2.适应动态环境变化的AMCL定位方法，其特征在于：包括步骤：(1)机器人运动过程中通过测距传感器获取观测数据并缓存，同时缓存对应的机器人坐标系在地图坐标系的位姿；(2)以步骤(1)得到的各观测数据对应的机器人坐标系在地图坐标系的位姿为节点通过约束构建位姿图，其中，约束关系为各节点之间的相对位姿，优化项为各观测数据对应的机器人坐标系在地图坐标系的位姿；(3)根据各观测数据对应的机器人坐标系在地图坐标系中的位姿，将相应观测数据投影至地图坐标系下，并计算相应观测数据与离线地图的匹配度，并与设定阈值进行比对；(4)以匹配度高于设定阈值的观测数据对应的机器人坐标系位姿为锚点，对匹配度低于设定阈值的观测数据对应的机器人坐标系位姿进行优化得到优化后的所有机器人坐标系位姿，并利用概率栅格图更新方式将所有观测数据更新到当前离线地图得到更新后的离线地图；(5)机器人使用更新后的离线地图进行定位。3.根据权利要求1或2所述的AMCL定位方法，其特征在于：所述当前机器人坐标系在地图坐标系中的位姿通过机器人置于离线地图中的初始位姿及机器人的里程计和机器人转动编码器计算得到。4.根据权利要求1或2所述的AMCL定位方法，其特征在于：所述测距传感器采用激光雷达或深度相机，通过激光雷达或深度相机实时获取当前帧激光点云。5.根据权利要求1或2所述的AMCL定位方法，其特征在于：计算观测数据与离线地图的匹配度具体如下：(31)寻找得到当前离线地图中与当前观测数据投影至地图坐标系中的各个像素距离最近的占据栅格，并计算各个像素与其距离最近的占据栅格之间的距离；其中，某一像素与其距离最近的占据栅格之间的距离计算如下：占据栅格为当前离线地图中障碍物所在栅格，其像素值为1；若某一像素在当前离线地
图中与其对应的栅格的像素值也为1，则某一像素与其距离最近的占据栅格之间的距离为0；否则，根据某一像素在当前离线地图中对应的栅格在地图坐标系中的坐标，计算得到与其距离...

【专利技术属性】
技术研发人员：程敏，李勇兵，刘文博，闫宗涛，徐伟，李奇，朱超，郭永春，侯铁研，
申请(专利权)人：深圳亿嘉和科技研发有限公司，
类型：发明
国别省市：