CN112526988A - 一种自主移动机器人及其路径导航和路径规划方法、系统 - Google Patents

一种自主移动机器人及其路径导航和路径规划方法、系统 Download PDF

Info

Publication number
CN112526988A
CN112526988A CN202011192592.3A CN202011192592A CN112526988A CN 112526988 A CN112526988 A CN 112526988A CN 202011192592 A CN202011192592 A CN 202011192592A CN 112526988 A CN112526988 A CN 112526988A
Authority
CN
China
Prior art keywords
path
navigation
global
loss
mobile robot
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.)
Granted
Application number
CN202011192592.3A
Other languages
English (en)
Other versions
CN112526988B (zh
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.)
Xian Jiaotong University
Original Assignee
Xian Jiaotong University
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 Xian Jiaotong University filed Critical Xian Jiaotong University
Priority to CN202011192592.3A priority Critical patent/CN112526988B/zh
Publication of CN112526988A publication Critical patent/CN112526988A/zh
Application granted granted Critical
Publication of CN112526988B publication Critical patent/CN112526988B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem

Abstract

本发明公开了一种自主移动机器人及其路径导航和路径规划方法、系统，方法包括全局规划过程和局部规划过程；具体如下：获取定位信息和感知信息；根据所述定位信息和感知信息生成代价地图，所述代价地图用于A星路径搜索，得到初始导航路径；对所述初始导航路径进行优化，对优化后路径的顶点进行采样，取出其中的关键点进行插值，得到全局导航路径；根据定位信息和所述全局导航路径生成待选路径组；根据实时感知信息对每一条待选路径进行评价，得到最优路径，并输出；能够在大范围内快速搜索出接近最优的导航路径，得到一系列描述全局导航路径的点，对全局导航路径的点进行调整和选择，得到能够满足曲率连续和无碰撞的需求全局导航路径。

Description

一种自主移动机器人及其路径导航和路径规划方法、系统
技术领域
本发明属于自主移动机器人技术领域，具体涉及一种自主移动机器人及其路径导航和路径规划方法、系统。
背景技术
在无人驾驶系统中，路径规划都被为了两个阶段，全局规划和局部规划。全局规划生成从起点到目标点的导航路径，而局部规划则在全局规划路径的引导下，生成满足机器人动力学限制并且无碰撞的行驶路径。在这种方式下，全局规划的结果将对局部规划的产生巨大影响。
A星算法是是全局规划中最常见和有效的方法之一。但是，它给出的结果大部分与最优解之间还存在间隔。为了解决这一问题，有研究者提出了在以混合A星为前端的基础上，通过共轭梯度下降数值优化的方法进行后端优化进行全局路径规划的方法。
但是，相比于传统的A星算法，混合A星算法需要较大的时间开销，并且不具备完备性。而A星算法得到的前端结果比混合A星算法得到的前端结果在解空间中离最优解更远，直接采用共轭梯度下降对A星算法结果进行优化难以达到理想的结果。
发明内容
为了解决现有技术中存在的问题，本发明提供一种新的全局导航方法，前端采用了一种改良后的A星算法，得到一系列描述全局导航路径的点，之后采用新的后端优化方法，对全局导航路径的点进行调整和选择，为了避免抖动的产生，通过道格拉斯-普客算法滤除了全局导航路径的点中的冗余，并通过三次B样条曲线对剩下的关键点进行插值，得到最终的全局导航路径。
为了实现上述目的，本发明采用的技术方案是：一种自主移动机器人路径导航和路径规划方法，包括以下步骤：
获取定位信息和感知信息；
根据所述定位信息和感知信息生成代价地图，所述代价地图用于A星路径搜索，得到初始导航路径；
对所述初始导航路径进行优化，对优化后路径的顶点进行采样，取出其中的关键点进行插值，得到全局导航路径；
根据定位信息和所述全局导航路径生成待选路径组；
根据实时感知信息对每一条待选路径进行评价，得到最优路径，并输出。
代价地图包括占据层、盲区层和一致性层；占据层表示障碍物或边界所处栅格，盲区层表示感知盲区所处的栅格，一致性层表示上一次规划周期所得到的全局路径经过的栅格；占据层用于在A星搜索过程中判断状态是否发生碰撞，而盲区层和一致性层用于计算状态的损失。
A星算法f-value的计算中状态
Figure BDA0002753184690000021
的f-value的计算包括启发和损失两项；其中，启发使用的是从当前状态到目标点的欧式距离进行衡量，而损失的计算如下：
Figure BDA0002753184690000022
其中，costact表示从s到
Figure BDA0002753184690000023
的运动损失，也就是两者之间的欧式距离，costblind表示盲区损失，如果
Figure BDA0002753184690000024
处于代价地图的盲区层所标注的盲区内，则costblind为一个可调值，反之，costblind为零，costcon表示一致性损失，其计算方法为
Figure BDA0002753184690000025
到代价地图的一致性层所标注的上一次规划的全局路径的最短距离。
对初始导航路径进行优化时提出优化目标函数：
Figure BDA0002753184690000031
Figure BDA0002753184690000032
Figure BDA0002753184690000033
其中，pi＝(xi,yi)T,i∈[0,N]为路径
Figure BDA0002753184690000034
的一个顶点；oi为距离pi最近的障碍物所处位置；Δpi＝pi-pi-1；
Figure BDA0002753184690000035
dmax、κmax和lexp为参量，分别表示路径离障碍物的距离，最大允许曲率和路径期望长度。
对最速梯度下降算法进行修改，其中包括迭代部分和非迭代部分，迭代部分包括梯度
Figure BDA0002753184690000036
的计算、学习率lr的计算和路径
Figure BDA0002753184690000037
更新，在迭代过程中通过学习率lr和梯度
Figure BDA0002753184690000038
对路径
Figure BDA0002753184690000039
进行更新：
Figure BDA00027531846900000310
非迭代部分包括路径关键点采样和插值，在迭代过程中引入顶点的删除与添加，将顶点聚集的区段中进行的顶点删除，对于顶点稀疏区段进行顶点插入；在插值之前对路径的顶点进行采样，选择出其中的关键点
Figure BDA00027531846900000311
利用道格拉斯-普客算法实现对顶点进行采样，得到关键点
Figure BDA00027531846900000312
后，再利用三次B样条插值的方法得到满足G2连续性的最终全局路径
Figure BDA00027531846900000313
待选路径组生成包括目标点采样和构建从当前位置到目标点的曲线，目标点采样沿全局路径的横向和纵向进行采样，为采样目标点的曲率都引入边界限制，对于每一个采样目标点生成一条从当前位置到采样目标点的曲线，使用五次样条表示所述曲线。
通过构建损失函数对每一条待选路径进行评价，计算得到待选路径的损失，损失最小的路径就是最优路径；所述损失函数为：
Figure BDA00027531846900000314
其中，
Figure BDA00027531846900000315
为安全性损失，
Figure BDA00027531846900000316
为曲率损失，
Figure BDA00027531846900000317
为舒适性损失，
Figure BDA00027531846900000318
为偏移损失；安全性损失
Figure BDA00027531846900000319
是根据路径
Figure BDA00027531846900000320
与障碍物之间的最小距离进行计算，将所述最小距离的计算转换为计算一个点到三个圆形的距离最小值。
对每一条待选路径进行评价时通过构建损失函数对每一条待选路径进行评价，所述损失函数中引入偏移损失
Figure BDA0002753184690000041
是用于降低局部路径与全局路径之间的偏移，使机器人尽可能贴近全局路径行驶，其计算表达式如下：
Figure BDA0002753184690000042
其中，
Figure BDA0002753184690000043
为局部路径
Figure BDA0002753184690000044
的终点，
Figure BDA0002753184690000045
为全局路径
Figure BDA0002753184690000046
中离
Figure BDA0002753184690000047
最近的点。
一种自主移动机器人路径导航和路径规划系统，全局规划模块和局部规划模块，全局规划模块包括信息获取单元、代价地图生成单元以及导航路径优化单元；局部规划模块包括待选路径组生成单元以及待选路径进行评价单元；
信息获取单元用于实时获取定位信息和感知信息；代价地图生成单元基于所述定位信息和感知信息生成代价地图；导航路径优化单元用于对所述初始导航路径进行优化，对优化后路径的顶点进行采样，取出其中的关键点进行插值，得到全局导航路径；待选路径组生成单元根据定位信息和所述全局导航路径生成待选路径组；待选路径进行评价单元根据实时感知信息对每一条待选路径进行评价，得到最优路径，并输出。
本发明还提供一种自主移动机器人，设置有至少一个计算机设备，所述计算机设备包括一个或多个处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取部分或全部所述计算机可执行程序并执行，处理器执行部分或全部计算可执行程序时能实现本发明所述自主移动机器人路径导航和路径规划方法。
与现有技术相比，本发明至少具有以下有益效果：
本发明提出以一种新的全局路径规划方法，此方法包含一个改良的A星前端和一个新的优化后端，能够在大范围内快速搜索出一条接近最优的导航路径，前端采用了一种改良后的A星算法，得到一系列描述全局导航路径的点，之后采用新的后端优化方法，对全局导航路径的点进行调整和选择，为了避免抖动的产生，通过道格拉斯-普客算法滤除了全局导航路径的点中的冗余，并通过三次B样条曲线对剩下的关键点进行插值，得到最终的全局导航路径，导航路径能够满足曲率连续和无碰撞的需求，同时，本发明使用了一种新的损失函数对局部待选路径进行评价，能够使局部路径在更加贴合全局导航路径的同时，满足车辆动力学限制；在全局导航路径的基础上，本发明采用了离散采样路径生成和最优路径选择的方式进行了局部路径规划，并设计了新的损失函数对每一条待选路径进行评价，得到最优局部路径。
附图说明
图1为路径导航、规划方法框架图。
图2为A星搜索流程图。
图3为障碍物距离计算示意图。
具体实施方式
下面结合附图对本发明进行详细阐述。
本发明所提出的路径规划方法框架如图1所示，全局规划和局部规划包括两个独立的进程；以不同的周期进行路径的更新；其中，全局规划在得到定位和感知信息后，进行代价地图的生成，生成的代价地图用于A星路径搜索，得到初始的导航路径，之后对此路径进行优化，并在优化完成后采样出其中的关键点进行插值；至此，全局导航路径生成完成。
但是，无法保证此时生成的全局路径是有效的，因此，引入判别器判断生成的全局路径是否有效，如果无效则将其抛弃。所述判别器的另一个功能是判断是否进行全局路径的更新，如果上一次全局路径无碰撞，则不进行更新。
局部规划根据定位信息和全局路径进行待选路径组的生成，之后根据感知信息对每一条待选路径进行评价，得到其中的最优路径输出到控制机构中；局部规划路径更新频率为10Hz。
全局规划分为了两个部分：前端和后端，前端包括了图1所示的代价地图生成和A星搜索，后端包括了图1中所示的优化、采样和插值。
前端首要是进行代价地图的生成。本发明所采用的代价地图包括占据层、盲区层和一致性层。其中，占据层表示障碍物或边界所处栅格，盲区层表示感知盲区所处的栅格；一致性层表示上一次规划周期所得到的全局路径经过的栅格；占据层用于在A星搜索过程中判断状态是否发生碰撞。而盲区层和一致性层用于计算状态的损失。
前端的A星搜索流程如图2左侧所示。搜索的过程与传统的A星算法基本相同：给出起点和目标点，并将起点加入Open集合中；从Open集合中得到f-value最小的状态s，判断其是否能够到达终点(与终点的连线是否无碰撞)；如果不能够到达终点，则逐一计算其邻居状态
Figure BDA0002753184690000061
的f-value；根据代价地图的占据层判断
Figure BDA0002753184690000062
是否无碰撞，如果不成立，则忽略该邻居；如果无碰撞，判断判断该邻居是否在Close集合中，如果在，则忽略该邻居；如果不在，则再判断是否在Open集中；如果该邻居不在Open中，则将其加入Open集合；反之，如果该邻居在Open集合中，判断Open集合中对应状态的f-value是否大于当前计算的该邻居的f-value，如果成立，则用当前邻居替代Open集合的对应状态；上述过程不断重复，直到搜索到目标点或Open集合为空集。
本发明主要对所述f-value的计算进行了改进。状态
Figure BDA0002753184690000063
的f-value的计算包括了两项，启发和损失。其中，启发使用的是从当前状态到目标点的欧式距离进行衡量。而损失的计算如下：
Figure BDA0002753184690000064
其中，costact表示从s到
Figure BDA0002753184690000065
的运动损失，也就是两者之间的欧式距离。costblind表示盲区损失。如果
Figure BDA0002753184690000066
处于代价地图的盲区层所标注的盲区内，则costblind为一个可调值。反之，costblind为零。costcon表示一致性损失，其计算方法为
Figure BDA0002753184690000067
到代价地图的一致性层所标注的上一次规划的全局路径的最短距离。
考虑盲区损失的意义是使全局路径尽可能引导机器人在已知环境中行驶。由于传感器的感知范围有限，以及障碍物的遮挡，机器人对于环境是部分可知的，在这种情况下，全局路径如果引导机器人前往未知区域，可能会使机器人陷入困境，造成不利影响，盲区损失正是对这种行为的惩罚。
考虑一致性损失的目的使为了使两相邻规划周期得到的路径尽可能接近。由于本发明中的全局规划以设定频率进行更新，很有可能出现以下问题：即两相邻规划周期得到的路径出现较大偏差；这种情况下，容易使机器人的方向摇摆不定，对行驶产生不利影响。
通过前端，可以得到搜索出的路径
Figure BDA0002753184690000071
下一步就是对
Figure BDA0002753184690000072
进行优化。首先，对优化的目标函数进行确定，提出了以下的优化目标函数
Figure BDA0002753184690000073
Figure BDA0002753184690000074
Figure BDA0002753184690000075
其中，pi＝(xi,yi)T,i∈[0,N]为路径
Figure BDA0002753184690000076
的一个顶点；oi为距离pi最近的障碍物所处位置；
Δpi＝pi-pi-1；
Figure BDA0002753184690000077
dmax、κmax和lexp为参量，分别表示路径离障碍物的距离，最大允许曲率和路径期望长度。
完成目标函数的构建后，本发明对最速梯度下降算法进行了修改，其中包括迭代部分和非迭代部分，迭代部分包括梯度
Figure BDA0002753184690000078
的计算、学习率lr的计算和路径
Figure BDA0002753184690000079
更新，在迭代过程中通过学习率lr和梯度
Figure BDA00027531846900000710
对路径
Figure BDA00027531846900000711
进行更新：
Figure BDA00027531846900000712
非迭代部分包括路径关键点采样和插值，在迭代过程中引入顶点的删除与添加，将顶点聚集的区段中进行的顶点删除，对于顶点稀疏区段进行顶点的插入；在插值之前对路径的顶点进行采样，选择出其中的关键点
Figure BDA00027531846900000713
利用道格拉斯-普客算法实现对顶点进行采样，得到关键点
Figure BDA0002753184690000081
后，再利用三次B样条插值的方法就能够得到满足G2连续性的最终全局路径
Figure BDA0002753184690000082
根据所述目标函数的优化的流程具体为：流程如算法1的优化过程所示，在最速梯度下降算法的基础上进行修改；算法包括了迭代部分和非迭代部分，迭代部分包括：梯度计算、学习率计算和路径更新；非迭代部分包括路径关键点采样和插值。
Figure BDA0002753184690000083
对于
Figure BDA0002753184690000084
梯度
Figure BDA0002753184690000085
的计算也就是分别对于它四项的梯度进行计算，再进行加权求和，首先是障碍物项ito的梯度
Figure BDA0002753184690000086
它的ith分量
Figure BDA0002753184690000087
也就是ito对于pi的偏导为
Figure BDA0002753184690000088
其次是效率项ite的梯度
Figure BDA0002753184690000089
它的ith分量
Figure BDA00027531846900000810
的表达式如下：
Figure BDA00027531846900000811
再在其次是平滑项its的梯度
Figure BDA00027531846900000812
它的ith分量
Figure BDA00027531846900000813
的表达式如下：
Figure BDA0002753184690000091
最后是曲率项itk的梯度
Figure BDA0002753184690000092
它的ith分量
Figure BDA0002753184690000093
的表达式如下：
Figure BDA0002753184690000094
其中，κi＝Δθi/|Δpi|，因此，要得到
Figure BDA0002753184690000095
就必须先计算出
Figure BDA0002753184690000096
的表达式。可以得到它们分别为：
Figure BDA0002753184690000097
Figure BDA0002753184690000098
Figure BDA0002753184690000099
其中
Figure BDA00027531846900000910
Figure BDA00027531846900000911
可以得到cos(Δθi)的导数的表达式为：
Figure BDA00027531846900000912
其中
Figure BDA00027531846900000913
至此，梯度
Figure BDA00027531846900000914
的计算就完成。
下一步就是计算学习率lr。学习率的计算方法与最速梯度下降相同，其表达式如下：
Figure BDA0002753184690000101
计算得到学习率lr后，就可以通过lr和
Figure BDA0002753184690000102
对路径
Figure BDA0002753184690000103
进行更新，更新的过程为：
Figure BDA0002753184690000104
下一步是进行顶点的删除与添加：在迭代过程中加入了顶点的删除与添加这一过程，将顶点聚集的区段中进行的顶点删除，对于顶点稀疏区段进行顶点的插入，顶点的删除与添加具体实现如算法2所示。
Figure BDA0002753184690000105
在完成优化之后，此时的路径顶点之间的间隔较大，并不能直接输出给局部规划进行使用。因此，对路径进行插值。但是，直接对路径的全部顶点进行插值，很容易导致震荡的产生。对此，在插值之前对路径的顶点进行采样，选择出其中的关键点
Figure BDA0002753184690000106
对顶点进行采样时利用道格拉斯-普客算法实现，该算法能够找出筛选出对于塑造路径形状最为重要的顶点，得到关键点
Figure BDA0002753184690000111
后，再利用三次B样条插值的方法就能够得到满足G2连续性的最终全局路径
Figure BDA0002753184690000112
局部规划部分为了待选路径生成部分和路径选择部分，前者根据全局路径的引导生成多样化的局部路径，后者在这些路径中选择出最优。
待选路径生成部分分为两步：第一步是进行目标点采样，第二步是构建从当前位置到目标点的曲线。
本发明使用的目标点采样方法为同时沿全局路径的横向和纵向进行采样，但是，本发明在生成全局路径的过程中并没有对曲率边界进行严格的限制。因此，得到的全局路径可能出现曲率大于最大曲率κmax。所以，给采样目标点的曲率都引入边界限制。
对于每一个采样目标点
Figure BDA0002753184690000113
都可以生成一条从当前位置到ri的曲线
Figure BDA0002753184690000114
为了保证曲率的连续性，使用的五次样条对曲线
Figure BDA0002753184690000115
进行表示，通过当前位置和目标点ri的边界限制条件，可以求解出该五次样条中的系数，最终得到曲线
Figure BDA0002753184690000116
路径选择是通过构建损失函数对每一条待选路径进行评价，计算得到待选路径的损失。损失最小的路径就是最优路径。
对损失函数进行了设置，分为了四项，其表达式如下：
Figure BDA0002753184690000117
其中，
Figure BDA0002753184690000118
为安全性损失，
Figure BDA0002753184690000119
为曲率损失，
Figure BDA00027531846900001110
为舒适性损失，
Figure BDA00027531846900001111
为偏移损失。
安全性损失
Figure BDA00027531846900001112
是根据路径
Figure BDA00027531846900001113
与障碍物之间的最小距离进行计算。因此，要计算
Figure BDA00027531846900001114
就需要先得到路径
Figure BDA00027531846900001115
到障碍物的最小距离
Figure BDA00027531846900001116
当机器人处于路径
Figure BDA00027531846900001117
的jth个点
Figure BDA00027531846900001118
时，我们可以用多个圆形来替代机器人进行与障碍物距离的计算，如图3所示。
在本发明中，设置n＝3。这种情况下，计算一个障碍物点o到机器人的距离就转换为计算到三个圆形的距离的最小值，也就是：
Figure BDA0002753184690000121
其中，
Figure BDA0002753184690000122
和
Figure BDA0002753184690000123
是机器人在
Figure BDA0002753184690000124
时生成的前后两个圆形的中心。而
Figure BDA0002753184690000125
就是得到全部的dgap中的最小值。之后，安全性损失
Figure BDA0002753184690000126
的计算表达式如下：
Figure BDA0002753184690000127
曲率损失
Figure BDA0002753184690000128
传统方法中都被仅仅表示为了路径
Figure BDA0002753184690000129
曲率平方的积分。实际上，
Figure BDA00027531846900001210
应该还与全局路径
Figure BDA00027531846900001211
的曲率相关：当
Figure BDA00027531846900001212
的曲率较小时，期望路径
Figure BDA00027531846900001213
的曲率也较小，从而保障舒适性；当
Figure BDA00027531846900001214
的曲率较大时，期望路径
Figure BDA00027531846900001215
的曲率也较大，保证机器人能够跟随全局路径的引导。
另一方面，如果路径
Figure BDA00027531846900001216
的曲率大于机器人允许的最大曲率κmax，则对路径
Figure BDA00027531846900001217
进行惩罚，极大提升它的曲率损失
Figure BDA00027531846900001218
因此，
Figure BDA00027531846900001219
的计算表达式如下：
Figure BDA00027531846900001220
其中，
Figure BDA00027531846900001221
为
Figure BDA00027531846900001222
的曲率，
Figure BDA00027531846900001223
为
Figure BDA00027531846900001224
在全局路径最近点的曲率，S(u)为单位阶跃函数，η为可调参数。
舒适性损失
Figure BDA00027531846900001225
的计算表达式如下：
Figure BDA00027531846900001226
其中，
Figure BDA00027531846900001227
为路径
Figure BDA00027531846900001228
起点的曲率变化率；
Figure BDA00027531846900001229
为上一次规划周期的局部路径
Figure BDA00027531846900001230
在
Figure BDA00027531846900001231
起点的曲率变化率。由于局部规划使用的五次样条只能保证连续规划周期的G2连续性，而无法保证G3连续性，因此，在进行局部路径更新时，会出现曲率变化率不连续的现象；曲率变化率的大阶跃可能会导致出现较大的横向冲击，影响机器人行驶的平稳性。因此，采用偏移损失
Figure BDA00027531846900001232
来对这一现象进行抑制。
偏移损失
Figure BDA00027531846900001233
是用于降低局部路径与全局路径之间的偏移，使机器人尽可能贴近全局路径行驶，其计算表达式如下
Figure BDA0002753184690000131
其中，
Figure BDA0002753184690000132
为局部路径
Figure BDA0002753184690000133
的终点，
Figure BDA0002753184690000134
为全局路径
Figure BDA0002753184690000135
中离
Figure BDA0002753184690000136
最近的点。
本发明还提供一种自主移动机器人，设置有至少一个计算机设备，包括但不限于一个或多个处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取部分或全部所述计算机可执行程序并执行，处理器执行部分或全部计算可执行程序时能实现本发明所述自主移动机器人路径导航和路径规划方法的部分步骤或所有步骤，存储器还能用于存储机器人携载传感器信息、道路信息以及地图信息。
所述计算机设备可以采用笔记本电脑、平板电脑、桌面型计算机、手机或工作站。
本发明还提供用于输出预测结果的输出装置，所述输出装置连接处理器的输出端，输出装置为显示器或打印机。
处理器可以是中央处理器(CPU)、数字信号处理器(DSP)、专用集成电路(ASIC)或现成可编程门阵列(FPGA)。
对于本发明所述存储器，可以是笔记本电脑、平板电脑、桌面型计算机、手机或工作站的内部存储单元，如内存、硬盘；也可以采用外部存储单元，如移动硬盘、闪存卡。

Claims (10)

1.一种自主移动机器人路径导航和路径规划方法，其特征在于，包括以下步骤：
获取定位信息和感知信息；
根据所述定位信息和感知信息生成代价地图，所述代价地图用于A星路径搜索，得到初始导航路径；
对所述初始导航路径进行优化，对优化后路径的顶点进行采样，取出其中的关键点进行插值，得到全局导航路径；
根据定位信息和所述全局导航路径生成待选路径组；
根据实时感知信息对每一条待选路径进行评价，得到最优路径，并输出。
2.根据权利要求1所述的自主移动机器人路径导航、路径规划方法，其特征在于，代价地图包括占据层、盲区层和一致性层；占据层表示障碍物或边界所处栅格，盲区层表示感知盲区所处的栅格，一致性层表示上一次规划周期所得到的全局路径经过的栅格；占据层用于在A星搜索过程中判断状态是否发生碰撞，而盲区层和一致性层用于计算状态的损失。
3.根据权利要求1所述的自主移动机器人路径导航、路径规划方法，其特征在于，A星算法f-value的计算中状态
Figure FDA0002753184680000011
的f-value的计算包括启发和损失两项；其中，启发使用的是从当前状态到目标点的欧式距离进行衡量，而损失的计算如下：
Figure FDA0002753184680000012
其中，costact表示从s到
Figure FDA0002753184680000013
的运动损失，也就是两者之间的欧式距离，costblind表示盲区损失，如果
Figure FDA0002753184680000014
处于代价地图的盲区层所标注的盲区内，则costblind为一个可调值，反之，costblind为零，costcon表示一致性损失，其计算方法为
Figure FDA0002753184680000015
到代价地图的一致性层所标注的上一次规划的全局路径的最短距离。
4.根据权利要求1所述的自主移动机器人路径导航和路径规划方法，其特征在于，对初始导航路径进行优化时提出优化目标函数：
Figure FDA0002753184680000021
Figure FDA0002753184680000022
Figure FDA0002753184680000023
其中，pi＝(xi,yi)T,i∈[0,N]为路径
Figure FDA0002753184680000024
的一个顶点；oi为距离pi最近的障碍物所处位置；Δpi＝pi-pi-1；
Figure FDA0002753184680000025
dmax、κmax和lexp为参量，分别表示路径离障碍物的距离，最大允许曲率和路径期望长度。
5.根据权利要求4所述的自主移动机器人路径导航和路径规划方法，其特征在于，对最速梯度下降算法进行修改，其中包括迭代部分和非迭代部分，迭代部分包括梯度
Figure FDA0002753184680000026
的计算、学习率lr的计算和路径
Figure FDA0002753184680000027
更新，在迭代过程中通过学习率lr和梯度
Figure FDA0002753184680000028
对路径
Figure FDA0002753184680000029
进行更新：
Figure FDA00027531846800000210
非迭代部分包括路径关键点采样和插值，在迭代过程中引入顶点的删除与添加，将顶点聚集的区段中进行的顶点删除，对于顶点稀疏区段进行顶点插入；在插值之前对路径的顶点进行采样，选择出其中的关键点
Figure FDA00027531846800000211
利用道格拉斯-普客算法实现对顶点进行采样，得到关键点
Figure FDA00027531846800000212
后，再利用三次B样条插值的方法得到满足G2连续性的最终全局路径
Figure FDA00027531846800000213
6.根据权利要求1所述的自主移动机器人路径导航和路径规划方法，其特征在于，待选路径组生成包括目标点采样和构建从当前位置到目标点的曲线，目标点采样沿全局路径的横向和纵向进行采样，为采样目标点的曲率都引入边界限制，对于每一个采样目标点生成一条从当前位置到采样目标点的曲线，使用五次样条表示所述曲线。
7.根据权利要求1所述的自主移动机器人路径导航和路径规划方法，其特征在于，通过构建损失函数对每一条待选路径进行评价，计算得到待选路径的损失，损失最小的路径就是最优路径；所述损失函数为：
Figure FDA0002753184680000031
其中，
Figure FDA0002753184680000032
为安全性损失，
Figure FDA0002753184680000033
为曲率损失，
Figure FDA0002753184680000034
为舒适性损失，
Figure FDA0002753184680000035
为偏移损失；安全性损失
Figure FDA0002753184680000036
是根据路径
Figure FDA0002753184680000037
与障碍物之间的最小距离进行计算，将所述最小距离的计算转换为计算一个点到三个圆形的距离最小值。
8.根据权利要求1所述的自主移动机器人路径导航和路径规划方法，其特征在于，对每一条待选路径进行评价时通过构建损失函数对每一条待选路径进行评价，所述损失函数中引入偏移损失
Figure FDA0002753184680000038
是用于降低局部路径与全局路径之间的偏移，使机器人尽可能贴近全局路径行驶，其计算表达式如下：
Figure FDA0002753184680000039
其中，
Figure FDA00027531846800000310
为局部路径
Figure FDA00027531846800000311
的终点，
Figure FDA00027531846800000312
为全局路径
Figure FDA00027531846800000313
中离
Figure FDA00027531846800000314
最近的点。
9.一种自主移动机器人路径导航和路径规划系统，其特征在于，全局规划模块和局部规划模块，全局规划模块包括信息获取单元、代价地图生成单元以及导航路径优化单元；局部规划模块包括待选路径组生成单元以及待选路径进行评价单元；
信息获取单元用于实时获取定位信息和感知信息；代价地图生成单元基于所述定位信息和感知信息生成代价地图；导航路径优化单元用于对所述初始导航路径进行优化，对优化后路径的顶点进行采样，取出其中的关键点进行插值，得到全局导航路径；待选路径组生成单元根据定位信息和所述全局导航路径生成待选路径组；待选路径进行评价单元根据实时感知信息对每一条待选路径进行评价，得到最优路径，并输出。
10.一种自主移动机器人，设置有至少一个计算机设备，其特征在于，所述计算机设备包括一个或多个处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取部分或全部所述计算机可执行程序并执行，处理器执行部分或全部计算可执行程序时能实现权利要求1～7任一项所述自主移动机器人路径导航和路径规划方法。
CN202011192592.3A 2020-10-30 2020-10-30 一种自主移动机器人及其路径导航和路径规划方法、系统 Active CN112526988B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011192592.3A CN112526988B (zh) 2020-10-30 2020-10-30 一种自主移动机器人及其路径导航和路径规划方法、系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011192592.3A CN112526988B (zh) 2020-10-30 2020-10-30 一种自主移动机器人及其路径导航和路径规划方法、系统

Publications (2)

Publication Number Publication Date
CN112526988A true CN112526988A (zh) 2021-03-19
CN112526988B CN112526988B (zh) 2022-04-22

Family

ID=74979299

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011192592.3A Active CN112526988B (zh) 2020-10-30 2020-10-30 一种自主移动机器人及其路径导航和路径规划方法、系统

Country Status (1)

Country Link
CN (1) CN112526988B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113108796A (zh) * 2021-04-19 2021-07-13 北京有竹居网络技术有限公司 导航方法、装置、存储介质及设备
CN113253726A (zh) * 2021-05-13 2021-08-13 金陵科技学院 一种物联网下的磁导航机器人及导航调度系统
CN114489087A (zh) * 2022-04-18 2022-05-13 北京理工大学 一种多无人驾驶车辆路径协同规划方法及系统
CN115046557A (zh) * 2022-08-11 2022-09-13 合肥井松智能科技股份有限公司 一种结合b样条曲线和a星算法的agv路径规划方法
CN115077534A (zh) * 2022-08-11 2022-09-20 合肥井松智能科技股份有限公司 一种基于b样条曲线的agv路径规划方法
CN116429145A (zh) * 2023-06-07 2023-07-14 福龙马城服机器人科技有限公司 一种复杂场景下无人车与垃圾桶自动对接导航方法及系统

Citations (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101077578A (zh) * 2007-07-03 2007-11-28 北京控制工程研究所 一种基于二元环境信息的移动机器人局部路径规划方法
US20110035051A1 (en) * 2009-08-10 2011-02-10 Samsung Electronics Co., Ltd Path planning apparatus and method for robot
CN101982735A (zh) * 2010-09-25 2011-03-02 上海美慧软件有限公司 一种关键路径实时动态旅行时间计算的方法
CN102385384A (zh) * 2011-08-30 2012-03-21 西安交通大学苏州研究院 一种导引机器人系统
CN103646298A (zh) * 2013-12-13 2014-03-19 中国科学院深圳先进技术研究院 一种自动驾驶方法及系统
CN106681335A (zh) * 2017-01-22 2017-05-17 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的避障路径规划及其控制方法
CN106909151A (zh) * 2017-01-22 2017-06-30 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的路径规划及其控制方法
CN107702716A (zh) * 2017-08-31 2018-02-16 广州小鹏汽车科技有限公司 一种无人驾驶路径规划方法、系统和装置
CN108549388A (zh) * 2018-05-24 2018-09-18 苏州智伟达机器人科技有限公司 一种基于改进a星策略的移动机器人路径规划方法
CN109489675A (zh) * 2017-09-11 2019-03-19 百度（美国）有限责任公司 用于自动驾驶车辆的基于成本的路径规划
CN109685286A (zh) * 2019-01-14 2019-04-26 哈尔滨工程大学 未知静态障碍环境下usv基于改进蚁群优化的避碰规划方法
US20190146498A1 (en) * 2017-02-16 2019-05-16 Faraday&Future Inc. Methods and systems for vehicle motion planning
CN109814557A (zh) * 2019-01-23 2019-05-28 西北工业大学 一种全局规划器主导的机器人路径规划方法
CN110214296A (zh) * 2017-12-29 2019-09-06 北京嘀嘀无限科技发展有限公司 用于路线确定的系统和方法
US20200117220A1 (en) * 2017-04-12 2020-04-16 Lawrence Livermore National Security, Llc Swarm path planner system for vehicles
CN111174798A (zh) * 2020-01-17 2020-05-19 长安大学 一种足式机器人路径规划方法
US20200156631A1 (en) * 2018-11-15 2020-05-21 Automotive Research & Testing Center Method for planning a trajectory for a self-driving vehicle
CN111582566A (zh) * 2020-04-26 2020-08-25 上海高仙自动化科技发展有限公司 路径规划方法及规划装置、智能机器人及存储介质
CN111736611A (zh) * 2020-07-06 2020-10-02 中国计量大学 一种基于a*算法和人工势场算法的移动机器人路径规划方法

Patent Citations (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101077578A (zh) * 2007-07-03 2007-11-28 北京控制工程研究所 一种基于二元环境信息的移动机器人局部路径规划方法
US20110035051A1 (en) * 2009-08-10 2011-02-10 Samsung Electronics Co., Ltd Path planning apparatus and method for robot
CN101982735A (zh) * 2010-09-25 2011-03-02 上海美慧软件有限公司 一种关键路径实时动态旅行时间计算的方法
CN102385384A (zh) * 2011-08-30 2012-03-21 西安交通大学苏州研究院 一种导引机器人系统
CN103646298A (zh) * 2013-12-13 2014-03-19 中国科学院深圳先进技术研究院 一种自动驾驶方法及系统
CN106681335A (zh) * 2017-01-22 2017-05-17 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的避障路径规划及其控制方法
CN106909151A (zh) * 2017-01-22 2017-06-30 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的路径规划及其控制方法
US20190146498A1 (en) * 2017-02-16 2019-05-16 Faraday&Future Inc. Methods and systems for vehicle motion planning
US20200117220A1 (en) * 2017-04-12 2020-04-16 Lawrence Livermore National Security, Llc Swarm path planner system for vehicles
CN107702716A (zh) * 2017-08-31 2018-02-16 广州小鹏汽车科技有限公司 一种无人驾驶路径规划方法、系统和装置
CN109489675A (zh) * 2017-09-11 2019-03-19 百度（美国）有限责任公司 用于自动驾驶车辆的基于成本的路径规划
CN110214296A (zh) * 2017-12-29 2019-09-06 北京嘀嘀无限科技发展有限公司 用于路线确定的系统和方法
CN108549388A (zh) * 2018-05-24 2018-09-18 苏州智伟达机器人科技有限公司 一种基于改进a星策略的移动机器人路径规划方法
US20200156631A1 (en) * 2018-11-15 2020-05-21 Automotive Research & Testing Center Method for planning a trajectory for a self-driving vehicle
CN109685286A (zh) * 2019-01-14 2019-04-26 哈尔滨工程大学 未知静态障碍环境下usv基于改进蚁群优化的避碰规划方法
CN109814557A (zh) * 2019-01-23 2019-05-28 西北工业大学 一种全局规划器主导的机器人路径规划方法
CN111174798A (zh) * 2020-01-17 2020-05-19 长安大学 一种足式机器人路径规划方法
CN111582566A (zh) * 2020-04-26 2020-08-25 上海高仙自动化科技发展有限公司 路径规划方法及规划装置、智能机器人及存储介质
CN111736611A (zh) * 2020-07-06 2020-10-02 中国计量大学 一种基于a*算法和人工势场算法的移动机器人路径规划方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
LIU WEITONG，等: "Swarm Intelligent Route Planning of UAV with Elastic Track Correction", 《2019 IEEE INTERNATIONAL CONFERENCE ON UNMANNED SYSTEMS (ICUS)》 *
毛德彩: "基于目标路径规划无人驾驶设备导航控制分析", 《机械设计与制造》 *
王楷文，等: "基于Ａ*算法优化的月面巡视器路径规划研究", 《航天器工程》 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113108796A (zh) * 2021-04-19 2021-07-13 北京有竹居网络技术有限公司 导航方法、装置、存储介质及设备
CN113108796B (zh) * 2021-04-19 2023-10-20 北京有竹居网络技术有限公司 导航方法、装置、存储介质及设备
CN113253726A (zh) * 2021-05-13 2021-08-13 金陵科技学院 一种物联网下的磁导航机器人及导航调度系统
CN113253726B (zh) * 2021-05-13 2023-01-24 金陵科技学院 一种物联网下的磁导航机器人及导航调度系统
CN114489087A (zh) * 2022-04-18 2022-05-13 北京理工大学 一种多无人驾驶车辆路径协同规划方法及系统
CN114489087B (zh) * 2022-04-18 2022-07-05 北京理工大学 一种多无人驾驶车辆路径协同规划方法及系统
CN115046557A (zh) * 2022-08-11 2022-09-13 合肥井松智能科技股份有限公司 一种结合b样条曲线和a星算法的agv路径规划方法
CN115077534A (zh) * 2022-08-11 2022-09-20 合肥井松智能科技股份有限公司 一种基于b样条曲线的agv路径规划方法
CN115077534B (zh) * 2022-08-11 2022-11-15 合肥井松智能科技股份有限公司 一种基于b样条曲线的agv路径规划方法
CN116429145A (zh) * 2023-06-07 2023-07-14 福龙马城服机器人科技有限公司 一种复杂场景下无人车与垃圾桶自动对接导航方法及系统
CN116429145B (zh) * 2023-06-07 2023-08-25 福龙马城服机器人科技有限公司 一种复杂场景下无人车与垃圾桶自动对接导航方法及系统

Also Published As

Publication number Publication date
CN112526988B (zh) 2022-04-22

Similar Documents

Publication Publication Date Title
CN112526988B (zh) 一种自主移动机器人及其路径导航和路径规划方法、系统
CN112068545B (zh) 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质
WO2021175313A1 (zh) 自动驾驶控制方法、装置、车辆及存储介质
CN109491376B (zh) 用于自动驾驶车辆的基于动态规划和梯度下降的决策和规划
CN109491377B (zh) 用于自动驾驶车辆的基于dp和qp的决策和规划
CN109521761B (zh) 用于自动驾驶车辆的基于约束平滑样条的速度优化方法、介质及系统
CN111771141B (zh) 自动驾驶车辆中使用3d cnn网络进行解决方案推断的lidar定位
CN110728014B (zh) 使用具有加权几何成本的分段螺旋曲线的参考线平滑方法
US10436595B2 (en) Method and system for updating localization maps of autonomous driving vehicles
CN111971574B (zh) 用于自动驾驶车辆的lidar定位的基于深度学习的特征提取
CN110487279B (zh) 一种基于改进a＊算法的路径规划方法
CN111771135B (zh) 自动驾驶车辆中使用rnn和lstm进行时间平滑的lidar定位
CN111427370B (zh) 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法
CN109955853B (zh) 用于操作自动驾驶车辆的方法、系统和存储介质
US20190235516A1 (en) Path and speed optimization fallback mechanism for autonomous vehicles
CN109521763A (zh) 用于自动驾驶车辆的基于约束平滑样条的路径优化
CN109521762A (zh) 用于自动驾驶车辆的基于2d约束平滑样条的平滑道路参考线路
CN110119140A (zh) 用于加速曲线投影的系统和方法
CN112362074A (zh) 一种结构化环境下的智能车辆局部路径规划方法
CN110389581A (zh) 用于为自动驾驶车辆生成障碍物的预测轨迹的方法
CN109489675A (zh) 用于自动驾驶车辆的基于成本的路径规划
WO2022016941A1 (zh) 行驶装置的避障路径的规划方法和装置
CN112799405B (zh) 基于动态障碍物环境下的无人船路径规划方法
CN113984080B (zh) 一种适用于大型复杂场景下的分层局部路径规划方法
WO2023159839A1 (zh) 换道路径规划方法、装置、智能驾驶汽车及存储介质

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
GR01 Patent grant
GR01 Patent grant