CN113447026A - 适应动态环境变化的amcl定位方法 - Google Patents

适应动态环境变化的amcl定位方法 Download PDF

Info

Publication number
CN113447026A
CN113447026A CN202110794761.9A CN202110794761A CN113447026A CN 113447026 A CN113447026 A CN 113447026A CN 202110794761 A CN202110794761 A CN 202110794761A CN 113447026 A CN113447026 A CN 113447026A
Authority
CN
China
Prior art keywords
coordinate system
observation data
map
robot
pose
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110794761.9A
Other languages
English (en)
Inventor
程敏
李勇兵
刘文博
闫宗涛
徐伟
李奇
朱超
郭永春
侯铁研
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Yijiahe Technology R & D Co ltd
Original Assignee
Shenzhen Yijiahe Technology R & D Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Yijiahe Technology R & D Co ltd filed Critical Shenzhen Yijiahe Technology R & D Co ltd
Priority to CN202110794761.9A priority Critical patent/CN113447026A/zh
Publication of CN113447026A publication Critical patent/CN113447026A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G01C21/3811—Point data, e.g. Point of Interest [POI]
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3837—Data obtained from a single source

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

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

Description

适应动态环境变化的AMCL定位方法
技术领域
本发明涉及定位领域，尤其涉及适应动态环境变化的AMCL定位方法。
背景技术
AMCL算法是一种基于滤波的定位算法，主要通过测距传感器的实时数据和加载的离线地图进行匹配从而计算得到机器人在地图中的准确位姿。AMCL定位算法广泛应用于无人驾驶和机器人等行业。
蒙特卡洛定位(MCL)基于粒子滤波算法，其一大优点是不受场景的限制，算法简捷快速，同时也可以兼顾算法的精度问题。粒子滤波使用粒子密度(也就是单位区间内的粒子数)表征事件的发生概率。根据选定的评估方程来推算事件的置信程度，并根据该结果重新调整粒子的分布情况。经过若干次迭代之后，粒子就集中分布在可能性高的区域了。基于这种推论，MCL将复杂的数学运算，转换成了计算机更易理解的迭代求解，在机器人定位问题上得到了很好的泛化。
当前AMCL定位算法的缺点在于其加载的是离线的地图文件。而在实际应用中，环境经常是变化的，比如商场中不断变化的人流，室外环境中路边不时停靠的车辆等等，在动态环境中AMCL定位算法的稳定性较差。
发明内容
发明目的：针对上述不足，本发明提出了一种适应动态环境变化的AMCL定位方法，改进了AMCL定位算法对于离线地图的依赖性，使其可以在局部动态环境下具有更强的适应性。
技术方案：
适应动态环境变化的AMCL定位方法，包括步骤：
(1)机器人运动过程中通过测距传感器获取当前观测数据，根据当前机器人坐标系在地图坐标系中的位姿将当前观测数据投影至地图坐标系下；
(2)计算当前观测数据与离线地图的匹配度，并与设定阈值进行比对；
若匹配度高于设定阈值，则根据当前观测数据使用概率栅格更新算法更新当前离线地图，并缓存当前观测数据及当前机器人坐标系在地图坐标系的位姿；
若匹配度低于设定阈值，则缓存当前观测数据和当前机器人坐标系在地图坐标系的位姿；
(3)以各观测数据对应的机器人坐标系在地图坐标系的位姿为节点通过约束构建位姿图，其中，约束关系为各节点之间的相对位姿，以匹配度高于设定阈值的观测数据对应的节点为锚点，以匹配度低于设定阈值的观测数据对应的节点为优化项；
(4)求解步骤(3)构建的位姿图得到优化后各观测数据对应的机器人坐标系在地图坐标系下的位姿，并将各观测数据投影至地图坐标系下，并使用概率栅格更新算法更新当前地图，得到更新后的离线地图；
(5)机器人使用更新后的离线地图进行定位。
适应动态环境变化的AMCL定位方法，包括步骤：
(1)机器人运动过程中通过测距传感器获取观测数据并缓存，同时缓存对应的机器人坐标系在地图坐标系的位姿；
(2)以步骤(1)得到的各观测数据对应的机器人坐标系在地图坐标系的位姿为节点通过约束构建位姿图，其中，约束关系为各节点之间的相对位姿，优化项为各观测数据对应的机器人坐标系在地图坐标系的位姿；
(3)根据各观测数据对应的机器人坐标系在地图坐标系中的位姿，将相应观测数据投影至地图坐标系下，并计算相应观测数据与离线地图的匹配度，并与设定阈值进行比对；
(4)以匹配度高于设定阈值的观测数据对应的机器人坐标系位姿为锚点，对匹配度低于设定阈值的观测数据对应的机器人坐标系位姿进行优化得到优化后的所有机器人坐标系位姿，并利用概率栅格图更新方式将所有观测数据更新到当前离线地图得到更新后的离线地图；
(5)机器人使用更新后的离线地图进行定位。
所述当前机器人坐标系在地图坐标系中的位姿通过机器人置于离线地图中的初始位姿及机器人的里程计和机器人转动编码器计算得到。
所述测距传感器采用激光雷达或深度相机，通过激光雷达或深度相机实时获取当前帧激光点云。
计算观测数据与离线地图的匹配度具体如下：
(31)寻找得到当前离线地图中与当前观测数据投影至地图坐标系中的各个像素距离最近的占据栅格，并计算各个像素与其距离最近的占据栅格之间的距离；
其中，某一像素与其距离最近的占据栅格之间的距离计算如下：
占据栅格为当前离线地图中障碍物所在栅格，其像素值为1；若某一像素在当前离线地图中与其对应的栅格的像素值也为1，则某一像素与其距离最近的占据栅格之间的距离为0；否则，根据某一像素在当前离线地图中对应的栅格在地图坐标系中的坐标，计算得到与其距离最近的占据栅格之间沿x方向相距m个像素，沿y方向相距n个像素，那么二者之间的距离为
Figure BDA0003162251610000031
当前离线地图上某一像素与其距离最近的占据栅格之间的距离设定上限值u，若二者距离超过上限值，则认定二者的距离为上限值u；
(32)根据步骤(31)计算得到的所有像素与其距离最近的占据栅格之间的距离求和并取平均值q，进而得到匹配度p，其中p＝1-q/u。
当前离线地图上某一像素与其距离最近的占据栅格之间的距离设定上限值u＝10r。
设定阈值根据实际需要设定，取值范围0～1。
对匹配度低于设定阈值的观测数据对应的机器人坐标系位姿进行优化具体如下：
给定两组观测数据所对应的机器人坐标系在地图坐标系下的位姿：pi＝[xi；θi]T，pj＝[xj；θj]T；
其中，xi，xj分别表示第i、j组观测数据所对应的机器人坐标系在地图坐标系下的位置，θi、θj分别表示第i、j组观测数据所对应的机器人坐标系在地图坐标系下的姿态；则从pj到pi的相对变换关系由下面的公式得到：
Figure BDA0003162251610000032
其中，R(θi)T表示pj到pi的姿态变换；
而通过里程计采集得到的两个对应的机器人坐标系在地图坐标系下的位姿pi′和pj′之间的约束也是一个相对变换
Figure BDA0003162251610000041
其中，
Figure BDA0003162251610000042
表示pi′和pj′之间的位置变换，
Figure BDA0003162251610000043
表示pi′和pj′之间的姿态变换；
最终得到的约束方程如下：
Figure BDA0003162251610000044
由此构建非线性最小二乘问题：
Figure BDA0003162251610000045
以上公式中，clamp将角度限定在[-π，π]之间；
使用非线性优化进行求解对应的机器人坐标系在地图坐标系下的位姿，进而得到需要优化的节点位姿。
所述步骤(2)中，缓存观测数据采用如下策略：
每隔设定时间或者每当机器人运动一定距离进行观测数据缓存。
所述位姿图中的节点数量设为d，当缓存有d个机器人坐标系位姿即通过步骤(3)进行一次优化。
有益效果：通过本发明的处理方式既可以充分利用离线地图作为先验进行定位，同时又利用了实时的观测数据对动态环境进行实时地图更新。本发明改善了原有的AMCL仅仅依赖离线地图进行定位，可以提高AMCL定位算法适应动态场景的能力，提升机器人定位的稳定性。
附图说明
图1为本发明的流程图；
图2为位姿图；
图3为原始加载地图；
图4为通过本发明的动态更新后的地图。
具体实施方式
下面结合附图和具体实施例，进一步阐明本发明。
图1为本发明的流程图，如图1所示，本发明的适应动态环境变化的AMCL定位方法包括如下步骤：
(1)机器人置于离线地图中，并据此获取机器人坐标系在地图坐标系中的初始位姿；
(2)机器人运动过程中，通过安装在机器人上的测距传感器实时获取当前观测数据，并根据当前机器人坐标系在地图坐标系中的位姿将当前观测数据投影至地图坐标系下；其中，当前机器人坐标系在地图坐标系中的位姿通过步骤(1)得到机器人坐标系在地图坐标系中的初始位姿与机器人的里程计和机器人转动编码器计算得到；
在本发明中，测距传感器采用激光雷达或深度相机，通过激光雷达或深度相机实时获取当前帧激光点云，并根据当前机器人坐标系在地图坐标系中的位姿将当前帧激光点云投影至地图坐标系下；
(3)计算当前观测数据与当前离线地图的匹配度，并与设定阈值进行比对；
(31)寻找得到当前离线地图中与当前观测数据投影至地图坐标系中的各个像素距离最近的占据栅格，并计算各个像素与其距离最近的占据栅格之间的距离；
其中，某一像素与其距离最近的占据栅格之间的距离计算如下：
离线地图使用图片格式，1个像素对应实际的距离长度r，比如r＝5cm，则1个像素表示5cm；占据栅格为当前离线地图中障碍物所在栅格，其像素值为1；观测数据返回的是障碍物像素信息；
若某一像素在当前离线地图中与其对应的栅格的像素值也为1，则某一像素与其距离最近的占据栅格之间的距离为0；否则，根据某一像素在当前离线地图中对应的栅格在地图坐标系中的坐标，计算得到与其距离最近的占据栅格之间沿x方向相距m个像素，沿y方向相距n个像素，那么二者之间的距离为
Figure BDA0003162251610000051
在本发明中，当前离线地图上某一像素与其距离最近的占据栅格之间的距离设定上限值u＝10r；若二者距离超过上限值，则认定二者的距离为上限值u；
(32)根据步骤(31)计算得到的所有像素与其距离最近的占据栅格之间的距离求和并取平均值q，进而得到匹配度p，其中p＝1-q/u；
(33)将当前观测数据的匹配度p与设定阈值进行比对；
若匹配度p高于设定阈值，则将当前观测数据投影至地图坐标系后的数据使用概率栅格更新算法插入至当前离线地图中进行离线地图更新，并缓存当前观测数据，由于当前观测数据的参考坐标系是机器人坐标系，所以缓存时需要同时保存当前机器人坐标系在地图坐标系的位姿；
其中，概率栅格更新算法具体为：
通过计算当前地图中各个栅格为占据栅格的概率值，其中各个栅格为占据栅格的概率值为a/N，其中，a表示激光雷达或深度相机扫描得到栅格的次数，只有栅格为占据栅格的情况下，该栅格才会被激光雷达或深度相机扫描到；N表示激光雷达或深度相机的扫描总次数；
若匹配度p低于设定阈值，则缓存当前观测数据和当前机器人坐标系在地图坐标系的位姿；
在本发明中，设定阈值根据实际需要设定，取值范围0～1。
(4)以步骤(2)缓存得到的各组观测数据对应的机器人坐标系在地图坐标系的位姿为节点，通过约束构建位姿图；如图2所示，该位姿图的优化项每个节点在地图坐标系下的位置(x1，x2，...，xk)及姿态(θ1，θ2...，θk)，k表示第k组观测数据；约束关系为各节点之间的相对位姿；
其中，在位姿图中，匹配度p高于设定阈值的观测数据所对应的节点机器人坐标系在地图坐标系的位姿固定，匹配度p低于设定阈值的观测数据所对应的节点机器人坐标系在地图坐标系的位姿需要优化；
(5)本发明采用非线性优化库对节点位姿进行优化计算，最终求解得到优化后的各节点在地图坐标系下的位姿，具体如下：
给定两组观测数据所对应的机器人坐标系在地图坐标系下的位姿：pi＝[xi；θi]T，pj＝[xj；θj]T，xi，xj分别表示第i、j组观测数据所对应的机器人坐标系在地图坐标系下的位置，θi、θj分别表示第i、j组观测数据所对应的机器人坐标系在地图坐标系下的姿态；则从pj到pi的相对变换关系由下面的公式得到：
Figure BDA0003162251610000061
其中，R(θi)T表示pj到pi的姿态变换；
而通过里程计采集得到的两个对应的机器人坐标系在地图坐标系下的位姿pi′和pj′之间的约束也是一个相对变换
Figure BDA0003162251610000071
其中，
Figure BDA0003162251610000072
表示pi′和pj′之间的位置变换，
Figure BDA0003162251610000073
表示pi′和pj′之间的姿态变换；
最终得到的约束方程如下：
Figure BDA0003162251610000074
由此可以构建非线性最小二乘问题：
Figure BDA0003162251610000075
以上公式中，clamp将角度限定在[-π，π]之间，这样通过局部观测坐标系(即对应的机器人坐标系)之间构建约束方程就可以使用非线性优化进行求解局部观测坐标系(即对应的机器人坐标系)在地图坐标系下的位姿，进而得到需要优化的节点位姿；
由于机器人在使用中是一直开机运行的，所以观测数据的规模会不断增加，那么位姿图的规模也会不断增加，所以在缓存观测数据的时候需要设定策略，比如每隔一定的时间或者每当机器人运动一定距离才缓存一次观测数据，这样可以有效降低算力，同时为了保证地图更新具有一定的实时性，观测位姿图的规模不能无限增加；本发明使用滑动窗口的方式管理位姿图中，即设定每次优化项的数目d，当缓存有d个机器人坐标系位姿就进行一次非线性优化，如此循环。
(5)位姿图优化完成后，就得到了各组观测数据所对应的机器人坐标系在地图坐标系下的位姿，然后将各组观测数据利用概率栅格更新算法更新当前地图，得到更新后的离线地图；
(6)机器人定位就可以使用更新后的离线地图进行定位。
通过对比图3和图4，可以发现，动态更新地图策略可以将动态物体更新到原始地图中，增加了地图特征，因此可以提高定位稳定性。
在本发明中，通过测距传感器采集观测数据，将测距传感器的观测数据转换到地图坐标系下，与离线地图进行匹配得到匹配度；根据匹配度将高于设定阈值的观测数据利用概率栅格图更新方式更新到当前地图中，并缓存该部分观测数据；对于匹配度低于设定阈值的观测数据进行缓存，缓存数据以时间间隔或者机器人行走的距离间隔作为参考进行缓存；缓存的匹配度低于设定阈值的观测数据和匹配度高于设定阈值的观测数据通过机器人的编码器数据或者观测数据之间匹配构建约束关系从而构建位姿图，并以匹配度高于设定阈值的观测数据对应的机器人坐标系位姿为锚点，对匹配度低于设定阈值的观测数据所对应的机器人坐标系位姿进行优化得到优化后的所有机器人坐标系位姿，并利用概率栅格图更新方式将观测数据更新到当前离线地图得到更新后的离线地图；其中，位姿图的优化使用当前主流的非线性优化库进行优化；为了防止位姿图的规模不断增大，使用滑动窗口的方式进行优化。
但本发明还提供了另一种方案，本发明首先通过测距传感器采集一系列观测数据并缓存，同时缓存观测数据所对应的机器人坐标系位姿；然后对一系列观测数据使用滑动窗口的方式取若干观测数据通过约束构建位姿图；之后将对应观测数据转换到地图坐标系下，并与离线地图进行匹配得到匹配度；并以匹配度高于设定阈值的观测数据对应的机器人坐标系位姿为锚点，对匹配度低于设定阈值的观测数据所对应的机器人坐标系位姿进行优化得到优化后的所有机器人坐标系位姿，并利用概率栅格图更新方式将所有观测数据更新到当前离线地图；其中，位姿图的优化使用当前主流的非线性优化库进行优化；最终得到更新后的离线地图，机器人使用更新后的离线地图进行定位。
本发明利用传感器的实时观测数据可以对离线地图不断进行局部更新，这样可以提高AMCL定位算法在动态环境下的定位稳定性。
以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。

Claims (10)

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；否则，根据某一像素在当前离线地图中对应的栅格在地图坐标系中的坐标，计算得到与其距离最近的占据栅格之间沿x方向相距m个像素，沿y方向相距n个像素，那么二者之间的距离为
Figure FDA0003162251600000021
当前离线地图上某一像素与其距离最近的占据栅格之间的距离设定上限值u，若二者距离超过上限值，则认定二者的距离为上限值u；
(32)根据步骤(31)计算得到的所有像素与其距离最近的占据栅格之间的距离求和并取平均值q，进而得到匹配度p，其中p＝1-q/u。
6.根据权利要求5所述的AMCL定位方法，其特征在于：当前离线地图上某一像素与其距离最近的占据栅格之间的距离设定上限值u＝10r。
7.根据权利要求1或2所述的AMCL定位方法，其特征在于：设定阈值根据实际需要设定，取值范围0～1。
8.根据权利要求1或2所述的AMCL定位方法，其特征在于：对匹配度低于设定阈值的观测数据对应的机器人坐标系位姿进行优化具体如下：
给定两组观测数据所对应的机器人坐标系在地图坐标系下的位姿：pi＝[xi；θi]T,pj＝[xj；θj]T；
其中，xi，xj分别表示第i、j组观测数据所对应的机器人坐标系在地图坐标系下的位置，θi、θj分别表示第i、j组观测数据所对应的机器人坐标系在地图坐标系下的姿态；则从pj到pi的相对变换关系由下面的公式得到：
Figure FDA0003162251600000031
其中，R(θi)T表示pj到pi的姿态变换；
而通过里程计采集得到的两个对应的机器人坐标系在地图坐标系下的位姿pi′和pj′之间的约束也是一个相对变换
Figure FDA0003162251600000032
其中，
Figure FDA0003162251600000033
表示pi′和pj′之间的位置变换，
Figure FDA0003162251600000034
表示pi′和pj′之间的姿态变换；
最终得到的约束方程如下：
Figure FDA0003162251600000035
由此构建非线性最小二乘问题：
Figure FDA0003162251600000036
以上公式中，clamp将角度限定在[-π，π]之间；
使用非线性优化进行求解对应的机器人坐标系在地图坐标系下的位姿，进而得到需要优化的节点位姿。
9.根据权利要求1或2所述的AMCL定位方法，其特征在于：所述步骤(2)中，缓存观测数据采用如下策略：
每隔设定时间或者每当机器人运动一定距离进行观测数据缓存。
10.根据权利要求1或2所述的AMCL定位方法，其特征在于：所述位姿图中的节点数量设为d，当缓存有d个机器人坐标系位姿即通过步骤(3)进行一次优化。
CN202110794761.9A 2021-07-14 2021-07-14 适应动态环境变化的amcl定位方法 Pending CN113447026A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110794761.9A CN113447026A (zh) 2021-07-14 2021-07-14 适应动态环境变化的amcl定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110794761.9A CN113447026A (zh) 2021-07-14 2021-07-14 适应动态环境变化的amcl定位方法

Publications (1)

Publication Number Publication Date
CN113447026A true CN113447026A (zh) 2021-09-28

Family

ID=77816196

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110794761.9A Pending CN113447026A (zh) 2021-07-14 2021-07-14 适应动态环境变化的amcl定位方法

Country Status (1)

Country Link
CN (1) CN113447026A (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20200226782A1 (en) * 2018-05-18 2020-07-16 Boe Technology Group Co., Ltd. Positioning method, positioning apparatus, positioning system, storage medium, and method for constructing offline map database
CN111429574A (zh) * 2020-03-06 2020-07-17 上海交通大学 基于三维点云和视觉融合的移动机器人定位方法和系统
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN112987027A (zh) * 2021-01-20 2021-06-18 长沙海格北斗信息技术有限公司 一种基于高斯模型的amcl算法的定位方法及存储介质

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20200226782A1 (en) * 2018-05-18 2020-07-16 Boe Technology Group Co., Ltd. Positioning method, positioning apparatus, positioning system, storage medium, and method for constructing offline map database
CN111429574A (zh) * 2020-03-06 2020-07-17 上海交通大学 基于三维点云和视觉融合的移动机器人定位方法和系统
CN112987027A (zh) * 2021-01-20 2021-06-18 长沙海格北斗信息技术有限公司 一种基于高斯模型的amcl算法的定位方法及存储介质
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
SUN DIHUA: "Adaptive KLD Sampling Based Monte Carlo Localization", THE 30TH CHINESE CONTROL AND DECISION CONFERENCE (2018 CCDC), pages 4154 - 4158 *
王炜 等: "基于概率栅格地图的移动机器人可定位性估计", 机器人, no. 04, pages 485 - 491 *

Similar Documents

Publication Publication Date Title
CN111429574B (zh) 基于三维点云和视觉融合的移动机器人定位方法和系统
CN112132893B (zh) 一种适用于室内动态环境的视觉slam方法
CN112767490B (zh) 一种基于激光雷达的室外三维同步定位与建图方法
CN113467456B (zh) 一种未知环境下用于特定目标搜索的路径规划方法
CN111324848B (zh) 移动激光雷达测量系统车载轨迹数据优化方法
CN111551184B (zh) 一种移动机器人slam的地图优化方法及系统
CN112762937B (zh) 一种基于占据栅格的2d激光序列点云配准方法
CN111862214A (zh) 计算机设备定位方法、装置、计算机设备和存储介质
CN113593035A (zh) 一种运动控制决策生成方法、装置、电子设备及存储介质
CN112180396B (zh) 一种激光雷达定位及地图创建方法
CN116203972B (zh) 一种未知环境探索路径规划方法、系统、设备及介质
CN117029870A (zh) 一种基于路面点云的激光里程计
CN113447026A (zh) 适应动态环境变化的amcl定位方法
CN116380039A (zh) 一种基于固态激光雷达和点云地图的移动机器人导航系统
CN113554705B (zh) 一种变化场景下的激光雷达鲁棒定位方法
CN116400349A (zh) 一种低分辨率毫米波雷达与光学相机的标定方法
CN111239761B (zh) 一种用于室内实时建立二维地图的方法
CN114648561A (zh) 旋转式三维激光雷达的点云匹配方法及系统
Hu et al. Accurate fiducial mapping for pose estimation using manifold optimization
Fang et al. Ground texture matching based global localization for intelligent vehicles in urban environment
CN110414337B (zh) 目标姿态检测系统及其检测方法
Song et al. SSF-MOS: Semantic Scene Flow assisted Moving Object Segmentation for Autonomous Vehicles
Stiens et al. Local elevation mapping for automated vehicles using lidar ray geometry and particle filters
CN116879870A (zh) 一种适用于低线束3d激光雷达的动态障碍物去除方法
Kotyuzanskiy et al. Object recognition technology for autonomous unmanned control based on stereo data analysis and voxel representation of visible objects

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination