CN108375976A - 一种服务机器人导航方法及*** - Google Patents
一种服务机器人导航方法及*** Download PDFInfo
- Publication number
- CN108375976A CN108375976A CN201810058588.4A CN201810058588A CN108375976A CN 108375976 A CN108375976 A CN 108375976A CN 201810058588 A CN201810058588 A CN 201810058588A CN 108375976 A CN108375976 A CN 108375976A
- Authority
- CN
- China
- Prior art keywords
- map
- sub
- odds
- service 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 230000008569 process Effects 0.000 claims abstract description 26
- 238000001514 detection method Methods 0.000 claims description 23
- 238000011156 evaluation Methods 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 3
- 230000008030 elimination Effects 0.000 claims description 2
- 238000003379 elimination reaction Methods 0.000 claims description 2
- 238000000059 patterning Methods 0.000 abstract description 8
- 238000012360 testing method Methods 0.000 abstract description 2
- 238000004804 winding Methods 0.000 description 11
- 238000005457 optimization Methods 0.000 description 6
- 238000009825 accumulation Methods 0.000 description 4
- 230000004888 barrier function Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 239000002245 particle Substances 0.000 description 3
- 241001269238 Data Species 0.000 description 2
- 230000010391 action planning Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000013500 data storage Methods 0.000 description 2
- 230000008520 organization Effects 0.000 description 2
- 238000012797 qualification Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000010408 sweeping Methods 0.000 description 2
- 238000012952 Resampling Methods 0.000 description 1
- 230000001154 acute effect Effects 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000007792 addition Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007257 malfunction Effects 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种服务机器人导航方法及***，用以解决现有的导航技术没有回环测试定位会产生误差的问题。该方法包括：利用cartographer算法通过闭环检测消除地图构建过程中产生的累积误差；利用快速遍历随机树算法对构建的地图进行全局路径规划；利用局部避障的动态窗口算法对构建的地图进行局部路径规划。本发明基于cartographer算法通过闭环检测消除构图过程中的误差，使定位更加精确，减小误差。
Description
技术领域
本发明涉及定位导航技术领域，尤其涉及一种服务机器人导航方法及***。
背景技术
服务机器人是机器人家族中的一个年轻成员，到目前为止尚没有一个严格的定义。不同国家对服务机器人的认识不同。
可以分为专业领域服务机器人和个人/家庭服务机器人，服务机器人的应用范围很广，主要从事维护保养、修理、运输、清洗、保安、救援、监护等工作。
ROS是一个机器人软件平台，能为异质计算机集群提供类似操作***的功能。起源于2007年，是一种分布式的处理框架，一种开源的机器人后操作***。例如，硬件抽象描述、底层驱动程序管理、程序间消息的传递等。
Gmapping定位算法主要应用了粒子滤波中的RBPF方法，扫描匹配的过程是首先估计机器人位姿，利用梯度下降的方法，以当前构建的地图、激光点和机器人位姿作为初始估计值，为了解决PF中的粒子耗散问题，提出了自适应重采样的机制，在一定程度上可以减少为保证定位精度所需的粒子个数。但是缺点在于依赖里程计，无法适用无人机及地面不平情况，且由于PF遵循马尔科夫假设，当前的状态只与前一时刻有关，所以无法进行回环。
Hector定位算法主要是将激光点与已有的地图进行匹配，利用已经获得的地图对激光束点阵进行优化，估计激光点在地图中的表示，和占据网格的概率。利用高斯牛顿解决scan-match(scan-map)的问题，找到激光点集映射到已有地图的刚体转换；为避免局部最小而非全剧最小，使用多分辨率地图。缺点在于没有地图的修正，一旦地图出错，之后的匹配也会出问题，同样该算法没有回环检测。
没有回环检测会产生误差累积，不能及时消除误差累积，则会导致定位不准确的问题。
发明内容
本发明要解决的技术问题目的在于提供一种服务机器人导航方法及***，用以解决现有的导航技术没有回环测试定位会产生误差的问题。
为了实现上述目的，本发明采用的技术方案为：
一种服务机器人导航方法，包括步骤：
利用cartographer算法通过闭环检测消除地图构建过程中产生的累积误差；
利用快速遍历随机树算法对构建的地图进行全局路径规划；
利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
进一步地，所述利用cartographer算法通过闭环检测消除地图构建过程中产生的累积误差的步骤具体包括：
建立闭环检测的子地图并赋予所述子地图的概率网格的网格点的概率值；
利用Ceres-based匹配扫描信息点相对于当前子地图的位姿以找到在所述子地图的概率最大的扫描信息点集的位姿；
利用SPA优化所有扫描信息点和子地图的位姿并将所述相对位姿进行存储；
采用分支界限和DFS搜索策略进行闭环检测和相对位姿的求解以消除累积误差。
进一步地，所述赋予所述子地图的概率网格的网格点的概率值的方法具体包括：
判断所述网格点是否已赋值，若否，根据所述网格点是否命中赋予命中概率值或未命中概率值；
若所述网格点已赋值，则根据以下公式更新概率值：
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(pmiss)))。
进一步地，所述利用快速遍历随机树算法对构建的地图进行全局路径规划的步骤具体包括：
通过在人工势场法路径规划引入快速遍历随机树算法引导随机树朝目标方向生长；所述引入快速遍历随机树算法的公式如下：
进一步地，所述利用局部避障的动态窗口算法对构建的地图进行局部路径规划的步骤具体包括：
对服务机器人的当前状态进行采样；
针对每个采样速度计算所述服务机器人预设时间后的行驶路线；
利用预设评价标准对多条行驶路线评分；
根据评分结果选择路径。
一种服务机器人导航***，包括：
误差消除模块，用于利用cartographer算法通过闭环检测消除地图构建过程中产生的累积误差；
全局路径规划模块，用于利用快速遍历随机树算法对构建的地图进行全局路径规划；
局部路径规划模块，用于利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
进一步地，所述误差消除模块具体包括：
子地图建立单元，用于建立闭环检测的子地图并赋予所述子地图的概率网格的网格点的概率值；
局部匹配扫描单元，用于利用Ceres-based匹配扫描信息点相对于当前子地图的位姿以找到在所述子地图的概率最大的扫描信息点集的位姿；
全局闭环优化单元，用于利用SPA优化所有扫描信息点和子地图的位姿并将所述相对位姿进行存储；
累积误差消除单元，用于采用分支界限和DFS搜索策略进行闭环检测和相对位姿的求解以消除累积误差。
进一步地，子地图建立单元具体用于判断所述网格点是否已赋值，若否，根据所述网格点是否命中赋予命中概率值或未命中概率值；若所述网格点已赋值，则根据以下公式更新概率值：
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(pmiss)))。
进一步地，所述全局路径规划模块具体包括：
引入单元，用于通过在人工势场法路径规划引入快速遍历随机树算法引导随机树朝目标方向生长；所述引入快速遍历随机树算法的公式如下：
进一步地，所述局部路径规划模块具体包括：
采样单元，用于对服务机器人的当前状态进行采样；
计算单元，用于针对每个采样速度计算所述服务机器人预设时间后的行驶路线；
评分单元，用于利用预设评价标准对多条行驶路线评分；
选择单元，用于根据评分结果选择路径。
本发明与传统的技术相比，有如下优点：
本发明基于cartographer算法通过闭环检测消除构图过程中的误差，使定位更加精确，减小误差。
附图说明
图1是实施例一提供的一种服务机器人导航方法流程图；
图2是实施例二提供的一种服务机器人导航***结构图。
具体实施方式
以下是本发明的具体实施例并结合附图，对本发明的技术方案作进一步的描述，但本发明并不限于这些实施例。
实施例一
本实施例提供了一种服务机器人导航方法，如图1所示，包括步骤：
S11：利用cartographer算法通过闭环检测消除地图构图过程中产生的累积误差；
S12：利用快速遍历随机树算法对构建的地图进行全局路径规划；
S13：利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
本实施例的cartographer算法是通过闭环检测来消除地图构图过程中产生的累积误差。用于闭环检测的基本单元是子地图。一个子地图是由一定数量的激光扫描构成，将一个激光扫描***其对应的子地图时，会基于子地图已有的激光扫描及其他传感器数据估计其在子地图中的最佳位置。子地图在段时间内的误差足够小，然而随着时间的推移，误差累积会越来越大。
本实施例中，步骤S11为利用cartographer算法通过闭环检测消除地图构图过程中产生的累积误差。
通过闭环检测适当地优化子地图的位姿进而消除累积误差，将问题转化为位姿优化的问题。位姿优化问题即求解非线性的最小二乘问题。当一个子地图的构建完成时，也就是不会再有新的激光扫描***到该子地图时，该子地图会加入闭环检测中。闭环检测会考虑所有的已完成创建的子地图。当一个新的激光扫描加入到地图中时，如果该激光扫描的估计位姿与地图中某个子地图的某个激光扫描的位姿比较接近，那么通过对应的激光匹配策略就会找到该闭环。该激光匹配策略通过在新加入地图的激光扫描的估计位姿附近取一个窗口，进而在该窗口内寻找该激光扫描的一个可能的匹配，如果找到了一个足够好的匹配，则会将该匹配的闭环约束加入到位姿优化问题中。
本实施例中，步骤S11具体包括：
建立闭环检测的子地图并赋予子地图的概率网络的网格点的概率值；
利用Ceres-based匹配扫描信息点相对于当前子地图的位姿以找到子地图的概率最大的扫描信息点集的位姿；
利用SPA优化所有扫描信息点和子地图的位姿并将相对位姿进行存储；
采用分支界限和DFS搜索策略进行闭环检测和相对位姿的求解以消除累积误差。
其中，赋予子地图的概率网格的网格点的概率值的步骤具体包括：
判断所述网格点是否已赋值，若否，根据所述网格点是否命中赋予命中概率值或未命中概率值；
若所述网格点已赋值，则根据以下公式更新概率值：
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(pmiss)))。
具体的，算法流程可分为以下几个步骤：
a.建立子地图：每当一个扫描点集被***这个概率网格中，一个包含网格点的命中集合以及一个未命中集合都会被重新计算。对于每一次命中，将这个最近的网格点加入命中集合，对于每一次未命中，***与扫描起点和每个扫描点间的一条连线相交的每个像素相关联的网格点，已经在命中集合中的网格点不需要***。对于每一个之前没有观察到的网格点都会根据他们所在集合是命中集合还是未命中集合赋予一个概率值phit或pmiss，如果一个网格点x已经被观察到了，将会通过以下的方式更新这个概率值：
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(pmiss)))。
b.局部扫描匹配：在将一个扫描信息点集***子地图中之前，这个扫描信息点集的位姿使用Ceres-based的扫描匹配方法相对于当前的局部子地图进行优化。扫描匹配过程的目的是为了找到一个在子地图中的概率最大的扫描信息点集的位姿，将这一过程作为非线性最小二乘问题。
c.全局闭环优化：Cartographer通过创建许多小的子地图来处理较大的空间。由于扫描信息只和几个理它最近的扫描信息的子地图匹配，所以局部扫描匹配过程会缓慢地累积误差。这里使用SPA来优化。除了这些相对位姿外，当子地图不再改变时，所有其他由扫描信息和其对应的自赌徒组成的对都被人为是闭环的。SLAM后端运行扫描匹配过程，当找到一个很好的扫描匹配时，相应的相对位姿就被添加到优化问题中。
d.消除累积误差：采用分支界限和DFS搜索的策略进行回环的检测和相对位姿求解，以消除累积误差。相比传统的先检测回环，在求解相对位姿来说，显得结构上更为统一，将建立回环的过程转化为一个查找过程。而另一方面，当完成对离散候选解空间的树形构造后，在树中对解的查找过程速度很快，剩下的对于建树过程中的节点的选择通过对子地图的与计算来完成，使用子地图这样的中间结构使得回环过程可以实时完成，从而通过回环不断调整子地图以消除累积误差。
本实施例中，步骤S12为利用快速遍历随机树算法对构建的地图进行全局路径规划。
其中，步骤S12具体包括：
通过在人工势场法路径规划引入快速遍历随机树算法引导随机树朝目标方向生长；所述引入快速遍历随机树算法的公式如下：
快速遍历随机树RRT是一种树形数据存储结构和算法，通过递增的方法建立，并快速减小随机选择点同树的距离，用于有效地搜索非凸的高维度的空间，特别适用于包含障碍物和非完整***或反向动力学微分约束条件下的动作规划。
RRT的特点是算法建构简单，并且可以快速遍历空间的未探索区域，而通过手动设定找到实现这种效果的函数却是很难的。同时RRT可以在基本算法基础上，对空间的搜索加入带有输入参数的控制函数，使算法可以适用于不同约束条件下的路径搜索。
人工势场法路径规划是由Khatib提出的一种虚拟力法。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。
本实施例使用改进的快速搜索随机树算法，具体包括：
A*算法是一种静态路网中求解最短路径最有效的直接搜索方法；
D*算法是动态的A*算法。
为了解决随机性问题，将人工势场中引力的思想引入RRT算法，引导随机树朝着目标方向生长，无需对全局环境进行建模，减少了规划时间，可表示为：
应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。
本实施例中，步骤S13为利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
其中，步骤S13具体包括：
对服务机器人的当前状态进行采样；
针对每个采样速度计算服务机器人预设时间后的行驶路线；
利用预设评价标准对多条行驶路线评分；
根据评分结果选择路径。
具体的，局部路径规划采用的是局部避障的动态窗口DWA算法。先对服务机器人当前状态进行采样，针对每个采样速度，计算服务机器人以当前速度行驶一段时间后的状态，得出一条行驶路线。再利用评价标准对多条路线进行打分，根据打分，选择分数最高的最优路径。继续重复上述过程，进行下一个局部路径规划。这样弥补了全局路径规划的缺点。使路径规划更加科学全面。
本实施例采用cartographer算法对服务器机器人进行精确定位和导航，避免了累积误差以及局部最优点的问题，使定位更加精准。
实施例二
本实施例提供了一种服务机器人导航***，如图2所示，包括：
误差消除模块21，用于利用cartographer算法通过闭环检测消除地图构图过程中产生的累积误差；
全局路径规划模块22，用于利用快速遍历随机树算法对构建的地图进行全局路径规划；
局部路径规划模块23，用于利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
本实施例的cartographer算法是通过闭环检测来消除地图构图过程中产生的累积误差。用于闭环检测的基本单元是子地图。一个子地图是由一定数量的激光扫描构成，将一个激光扫描***其对应的子地图时，会基于子地图已有的激光扫描及其他传感器数据估计其在子地图中的最佳位置。子地图在段时间内的误差足够小，然而随着时间的推移，误差累积会越来越大。
本实施例中，误差消除模块21用于利用cartographer算法通过闭环检测消除地图构图过程中产生的累积误差。
通过闭环检测适当地优化子地图的位姿进而消除累积误差，将问题转化为位姿优化的问题。位姿优化问题即求解非线性的最小二乘问题。当一个子地图的构建完成时，也就是不会再有新的激光扫描***到该子地图时，该子地图会加入闭环检测中。闭环检测会考虑所有的已完成创建的子地图。当一个新的激光扫描加入到地图中时，如果该激光扫描的估计位姿与地图中某个子地图的某个激光扫描的位姿比较接近，那么通过对应的激光匹配策略就会找到该闭环。该激光匹配策略通过在新加入地图的激光扫描的估计位姿附近取一个窗口，进而在该窗口内寻找该激光扫描的一个可能的匹配，如果找到了一个足够好的匹配，则会将该匹配的闭环约束加入到位姿优化问题中。
本实施例中，误差消除模块21具体包括：
子地图建立单元，用于建立闭环检测的子地图并赋予子地图的概率网络的网格点的概率值；
局部匹配扫描单元，用于利用Ceres-based匹配扫描信息点相对于当前子地图的位姿以找到子地图的概率最大的扫描信息点集的位姿；
全局闭环优化单元，用于利用SPA优化所有扫描信息点和子地图的位姿并将相对位姿进行存储；
累积误差消除单元，用于采用分支界限和DFS搜索策略进行闭环检测和相对位姿的求解以消除累积误差。
其中，自突入建立单元具体用于判断所述网格点是否已赋值，若否，根据所述网格点是否命中赋予命中概率值或未命中概率值；
若所述网格点已赋值，则根据以下公式更新概率值：
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(pmiss)))。
具体的，算法流程可分为以下几个步骤：
a.建立子地图：每当一个扫描点集被***这个概率网格中，一个包含网格点的命中集合以及一个未命中集合都会被重新计算。对于每一次命中，将这个最近的网格点加入命中集合，对于每一次未命中，***与扫描起点和每个扫描点间的一条连线相交的每个像素相关联的网格点，已经在命中集合中的网格点不需要***。对于每一个之前没有观察到的网格点都会根据他们所在集合是命中集合还是未命中集合赋予一个概率值phit或pmiss，如果一个网格点x已经被观察到了，将会通过以下的方式更新这个概率值：
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(pmiss)))。
b.局部扫描匹配：在将一个扫描信息点集***子地图中之前，这个扫描信息点集的位姿使用Ceres-based的扫描匹配方法相对于当前的局部子地图进行优化。扫描匹配过程的目的是为了找到一个在子地图中的概率最大的扫描信息点集的位姿，将这一过程作为非线性最小二乘问题。
c.全局闭环优化：Cartographer通过创建许多小的子地图来处理较大的空间。由于扫描信息只和几个理它最近的扫描信息的子地图匹配，所以局部扫描匹配过程会缓慢地累积误差。这里使用SPA来优化。除了这些相对位姿外，当子地图不再改变时，所有其他由扫描信息和其对应的自赌徒组成的对都被人为是闭环的。SLAM后端运行扫描匹配过程，当找到一个很好的扫描匹配时，相应的相对位姿就被添加到优化问题中。
d.消除累积误差：采用分支界限和DFS搜索的策略进行回环的检测和相对位姿求解，以消除累积误差。相比传统的先检测回环，在求解相对位姿来说，显得结构上更为统一，将建立回环的过程转化为一个查找过程。而另一方面，当完成对离散候选解空间的树形构造后，在树中对解的查找过程速度很快，剩下的对于建树过程中的节点的选择通过对子地图的与计算来完成，使用子地图这样的中间结构使得回环过程可以实时完成，从而通过回环不断调整子地图以消除累积误差。
本实施例中，全局路径规划模块22用于利用快速遍历随机树算法对构建的地图进行全局路径规划。
其中，全局路径规划模块22具体包括：
引入单元，用于通过在人工势场法路径规划引入快速遍历随机树算法引导随机树朝目标方向生长；所述引入快速遍历随机树算法的公式如下：
快速遍历随机树RRT是一种树形数据存储结构和算法，通过递增的方法建立，并快速减小随机选择点同树的距离，用于有效地搜索非凸的高维度的空间，特别适用于包含障碍物和非完整***或反向动力学微分约束条件下的动作规划。
人工势场法路径规划是由Khatib提出的一种虚拟力法。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。
本实施例使用改进的快速搜索随机树算法，具体包括：
A*算法是一种静态路网中求解最短路径最有效的直接搜索方法；
D*算法是动态的A*算法。
为了解决随机性问题，将人工势场中引力的思想引入RRT算法，引导随机树朝着目标方向生长，无需对全局环境进行建模，减少了规划时间，可表示为：
应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。
本实施例中，局部路径规划模块23用于利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
其中，局部路径规划模块23具体包括：
采样单元，用于对服务机器人的当前状态进行采样；
计算单元，用于针对每个采样速度计算服务机器人预设时间后的行驶路线；
评分单元，用于利用预设评价标准对多条行驶路线评分；
选择单元，用于根据评分结果选择路径。
具体的，局部路径规划采用的是局部避障的动态窗口DWA算法。先对服务机器人当前状态进行采样，针对每个采样速度，计算服务机器人以当前速度行驶一段时间后的状态，得出一条行驶路线。再利用评价标准对多条路线进行打分，根据打分，选择分数最高的最优路径。继续重复上述过程，进行下一个局部路径规划。这样弥补了全局路径规划的缺点。使路径规划更加科学全面。
本实施例采用cartographer算法对服务器机器人进行精确定位和导航，避免了累积误差以及局部最优点的问题，使定位更加精准。
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
Claims (10)
1.一种服务机器人导航方法，其特征在于，包括步骤：
利用cartographer算法通过闭环检测消除地图构建过程中产生的累积误差；
利用快速遍历随机树算法对构建的地图进行全局路径规划；
利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
2.根据权利要求1所述的一种服务机器人导航方法，其特征在于，所述利用cartographer算法通过闭环检测消除地图构建过程中产生的累积误差的步骤具体包括：
建立闭环检测的子地图并赋予所述子地图的概率网格的网格点的概率值；
利用Ceres-based匹配扫描信息点相对于当前子地图的位姿以找到在所述子地图的概率最大的扫描信息点集的位姿；
利用SPA优化所有扫描信息点和子地图的位姿并将所述相对位姿进行存储；
采用分支界限和DFS搜索策略进行闭环检测和相对位姿的求解以消除累积误差。
3.根据权利要求2所述的一种服务机器人导航方法，其特征在于，所述赋予所述子地图的概率网格的网格点的概率值的方法具体包括：
判断所述网格点是否已赋值，若否，根据所述网格点是否命中赋予命中概率值或未命中概率值；
若所述网格点已赋值，则根据以下公式更新概率值：
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(pmiss)))。
4.根据权利要求1所述的一种服务机器人导航方法，其特征在于，所述利用快速遍历随机树算法对构建的地图进行全局路径规划的步骤具体包括：
通过在人工势场法路径规划引入快速遍历随机树算法引导随机树朝目标方向生长；所述引入快速遍历随机树算法的公式如下：
5.根据权利要求1所述的一种服务机器人导航方法，其特征在于，所述利用局部避障的动态窗口算法对构建的地图进行局部路径规划的步骤具体包括：
对服务机器人的当前状态进行采样；
针对每个采样速度计算所述服务机器人预设时间后的行驶路线；
利用预设评价标准对多条行驶路线评分；
根据评分结果选择路径。
6.一种服务机器人导航***，其特征在于，包括：
误差消除模块，用于利用cartographer算法通过闭环检测消除地图构建过程中产生的累积误差；
全局路径规划模块，用于利用快速遍历随机树算法对构建的地图进行全局路径规划；
局部路径规划模块，用于利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
7.根据权利要求1所述的一种服务机器人导航***，其特征在于，所述误差消除模块具体包括：
子地图建立单元，用于建立闭环检测的子地图并赋予所述子地图的概率网格的网格点的概率值；
局部匹配扫描单元，用于利用Ceres-based匹配扫描信息点相对于当前子地图的位姿以找到在所述子地图的概率最大的扫描信息点集的位姿；
全局闭环优化单元，用于利用SPA优化所有扫描信息点和子地图的位姿并将所述相对位姿进行存储；
累积误差消除单元，用于采用分支界限和DFS搜索策略进行闭环检测和相对位姿的求解以消除累积误差。
8.根据权利要求7所述的一种服务机器人导航***，其特征在于，子地图建立单元具体用于判断所述网格点是否已赋值，若否，根据所述网格点是否命中赋予命中概率值或未命中概率值；若所述网格点已赋值，则根据以下公式更新概率值：
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(phits))；
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(pmiss)))。
9.根据权利要求6所述的一种服务机器人导航***，其特征在于，所述全局路径规划模块具体包括：
引入单元，用于通过在人工势场法路径规划引入快速遍历随机树算法引导随机树朝目标方向生长；所述引入快速遍历随机树算法的公式如下：
10.根据权利要求6所述的一种服务机器人导航***，其特征在于，所述局部路径规划模块具体包括：
采样单元，用于对服务机器人的当前状态进行采样；
计算单元，用于针对每个采样速度计算所述服务机器人预设时间后的行驶路线；
评分单元，用于利用预设评价标准对多条行驶路线评分；
选择单元，用于根据评分结果选择路径。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810058588.4A CN108375976A (zh) | 2018-01-22 | 2018-01-22 | 一种服务机器人导航方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810058588.4A CN108375976A (zh) | 2018-01-22 | 2018-01-22 | 一种服务机器人导航方法及*** |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108375976A true CN108375976A (zh) | 2018-08-07 |
Family
ID=63016520
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810058588.4A Pending CN108375976A (zh) | 2018-01-22 | 2018-01-22 | 一种服务机器人导航方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108375976A (zh) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109407665A (zh) * | 2018-09-28 | 2019-03-01 | 浙江大学 | 一种小型半自动无人驾驶配送车及配送路径规划方法 |
| CN109557920A (zh) * | 2018-12-21 | 2019-04-02 | 华南理工大学广州学院 | 一种自动导航建图机器人及控制方法 |
| CN109753072A (zh) * | 2019-01-23 | 2019-05-14 | 西安工业大学 | 一种移动机器人混合路径规划方法 |
| CN109782766A (zh) * | 2019-01-25 | 2019-05-21 | 北京百度网讯科技有限公司 | 用于控制车辆行驶的方法和装置 |
| CN110083167A (zh) * | 2019-06-05 | 2019-08-02 | 浙江大华技术股份有限公司 | 一种移动机器人的路径跟踪方法及装置 |
| CN110162063A (zh) * | 2019-06-12 | 2019-08-23 | 北京洛必德科技有限公司 | 一种用于机器人自动充电的路径规划方法和装置 |
| CN110412596A (zh) * | 2019-07-10 | 2019-11-05 | 上海电机学院 | 一种基于图像信息和激光点云的机器人定位方法 |
| CN111077495A (zh) * | 2019-12-10 | 2020-04-28 | 亿嘉和科技股份有限公司 | 一种基于三维激光的定位恢复方法 |
| CN111735451A (zh) * | 2020-04-16 | 2020-10-02 | 中国北方车辆研究所 | 一种基于多源先验信息的点云匹配高精度定位方法 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105867381A (zh) * | 2016-04-25 | 2016-08-17 | 广西大学 | 一种基于概率地图的工业机器人路径搜索优化算法 |
| CN106406338A (zh) * | 2016-04-14 | 2017-02-15 | 中山大学 | 一种基于激光测距仪的全向移动机器人的自主导航装置及其方法 |
| CN106406320A (zh) * | 2016-11-29 | 2017-02-15 | 重庆重智机器人研究院有限公司 | 机器人路径规划方法及规划路线的机器人 |
-
2018
- 2018-01-22 CN CN201810058588.4A patent/CN108375976A/zh active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106406338A (zh) * | 2016-04-14 | 2017-02-15 | 中山大学 | 一种基于激光测距仪的全向移动机器人的自主导航装置及其方法 |
| CN105867381A (zh) * | 2016-04-25 | 2016-08-17 | 广西大学 | 一种基于概率地图的工业机器人路径搜索优化算法 |
| CN106406320A (zh) * | 2016-11-29 | 2017-02-15 | 重庆重智机器人研究院有限公司 | 机器人路径规划方法及规划路线的机器人 |
Non-Patent Citations (3)
| Title |
|---|
| WOLFGANG HESS等: "Real-Time Loop Closure in 2D LIDAR SLAM", 《 2016 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 * |
| 白巧克力亦唯心: "机器人局部避障的动态窗口法（dynamic window approach）", 《CSDN》 * |
| 郝利波: "基于一种改进RRT算法的足球机器人路径规划", 《西安科技大学学报》 * |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109407665A (zh) * | 2018-09-28 | 2019-03-01 | 浙江大学 | 一种小型半自动无人驾驶配送车及配送路径规划方法 |
| CN109557920A (zh) * | 2018-12-21 | 2019-04-02 | 华南理工大学广州学院 | 一种自动导航建图机器人及控制方法 |
| CN109753072A (zh) * | 2019-01-23 | 2019-05-14 | 西安工业大学 | 一种移动机器人混合路径规划方法 |
| CN109782766A (zh) * | 2019-01-25 | 2019-05-21 | 北京百度网讯科技有限公司 | 用于控制车辆行驶的方法和装置 |
| US11378957B1 (en) | 2019-01-25 | 2022-07-05 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Method and apparatus for controlling vehicle driving |
| CN110083167A (zh) * | 2019-06-05 | 2019-08-02 | 浙江大华技术股份有限公司 | 一种移动机器人的路径跟踪方法及装置 |
| CN110162063A (zh) * | 2019-06-12 | 2019-08-23 | 北京洛必德科技有限公司 | 一种用于机器人自动充电的路径规划方法和装置 |
| CN110412596A (zh) * | 2019-07-10 | 2019-11-05 | 上海电机学院 | 一种基于图像信息和激光点云的机器人定位方法 |
| CN111077495A (zh) * | 2019-12-10 | 2020-04-28 | 亿嘉和科技股份有限公司 | 一种基于三维激光的定位恢复方法 |
| CN111077495B (zh) * | 2019-12-10 | 2022-02-22 | 亿嘉和科技股份有限公司 | 一种基于三维激光的定位恢复方法 |
| CN111735451A (zh) * | 2020-04-16 | 2020-10-02 | 中国北方车辆研究所 | 一种基于多源先验信息的点云匹配高精度定位方法 |
| CN111735451B (zh) * | 2020-04-16 | 2022-06-07 | 中国北方车辆研究所 | 一种基于多源先验信息的点云匹配高精度定位方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108375976A (zh) | 一种服务机器人导航方法及*** | |
| CN111024092B (zh) | 一种多约束条件下智能飞行器航迹快速规划方法 | |
| CN110231833B (zh) | 一种基于多无人机的油田巡检定点数据采集***及方法 | |
| KR20210116576A (ko) | 시각적 로봇 기반의 과거 지도 이용 방법 | |
| CN111639811A (zh) | 基于改进蚁群算法的多农机协同作业远程管理调度方法 | |
| CN106873599A (zh) | 基于蚁群算法和极坐标变换的无人自行车路径规划方法 | |
| CN106767820B (zh) | 一种室内移动定位与制图方法 | |
| US20220404836A1 (en) | Method for route optimization based on dynamic window and redundant node filtering | |
| JP5429901B2 (ja) | ロボット及び情報処理装置のプログラム | |
| CN109470254A (zh) | 地图车道线的生成方法、装置、***及存储介质 | |
| CN109828578B (zh) | 一种基于YOLOv3的仪表巡检机器人最优路线规划方法 | |
| CN109270927A (zh) | 道路数据的生成方法及装置 | |
| CN110989352A (zh) | 一种基于蒙特卡洛树搜索算法的群体机器人协同搜索方法 | |
| CN106899306A (zh) | 一种保持移动特征的车辆轨迹线数据压缩方法 | |
| CN112530158B (zh) | 一种基于历史轨迹的路网补充方法 | |
| CN108209743A (zh) | 一种定点清洁方法、装置、计算机设备和存储介质 | |
| CN108645769A (zh) | 一种基于无人机的环境空气质量监测方法 | |
| CN112344945A (zh) | 室内配送机器人路径规划方法、***及室内配送机器人 | |
| CN114326825A (zh) | 输电线路无人机巡检路径规划及缺陷分析云平台 | |
| CN111401611B (zh) | 一种化工厂设备巡检点路线优化方法 | |
| CN106529678A (zh) | 一种基于最大最小蚂蚁***优化的slam数据关联方法 | |
| CN114186112B (zh) | 一种基于贝叶斯优化多重信息增益探索策略的机器人导航方法 | |
| CN114879660A (zh) | 一种基于目标驱动的机器人环境感知方法 | |
| CN109910015A (zh) | 一种砂浆喷抹建筑机器人的末端路径规划算法 | |
| JP7295106B2 (ja) | 軌道ネットワーク内でのナビゲートのためのシステムおよび方法 |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20180807 |