一种移动机器人路径规划方法及系统与流程

1.本发明涉及智能机器人技术领域，特别是涉及一种基于路径时间代价的移动机器人路径规划方法及系统。


背景技术：

2.随着人工智能的快速崛起，移动机器人不仅在空间探索方面获得极大地成功，还在无人驾驶汽车、服务行业等领域获得重大突破。机器人智能化的重要表现之一就是在不依靠人类控制的情况下自主地移动，这也是移动机器人执行任务的基础环节，因此自主导航成为了机器人领域的热点研究内容。移动机器人的自主导航技术被lenonard等人分解为三个经典问题：
3.(1)我现在在哪？
4.(2)我要到哪去？
5.(3)我要怎么去？
6.这三个经典问题中涉及到的技术要点有：实时定位、环境感知、避障决策、路径规划、运动控制等。自主移动机器人所处的环境含有大量不确定的因素，机器人要想在其中安全地行走，需要具备以下两个条件：
7.(1)机器人可以根据传感器感知外部环境及自身状态，需要实时定位与构图(simultaneous localization and mapping，slam)技术支持；
8.(2)机器人可以依据感知数据自我决策，在约束条件下，以最小代价实现在含有障碍物的环境中移动，需要路径规划算法支持。
9.机器人路径规划决定了机器人的行进路线以及是否能够到达指定的目标点，规划出的路线直接影响着机器人的一系列运动姿态，因此研究移动机器人的路径规划，具有重要的现实意义。机器人路径规划的基本任务是在考虑机器人物理性能约束、环境障碍约束、边值约束、队形约束等条件下，找到一条从起始点到目标点的无碰撞路径，并在此基础上使其实现如规划路径点最少、路径长度最短、机器人能量消耗最小等要求，最终为机器人确定一条最优的运动路径。
10.但当前已有研究一般直接采用路径长度作为寻优目标，即选取一条路径长度最短的运动路径，而实际任务常常以时间最短为需求，但目前没有一种能够规划出路径时间最小的规划路径的路径规划方法。


技术实现要素：

11.本发明的目的是提供一种移动机器人路径规划方法及系统，能够规划出一条路径时间最短的规划路径。
12.为实现上述目的，本发明提供了如下方案：
13.一种移动机器人路径规划方法，所述规划方法包括：
14.获取移动机器人的运行约束条件；所述运行约束条件包括所述移动机器人的可行
区域、起始点位置、目标点位置、步长、搜索步长和所述可行区域内的障碍物信息；
15.根据所述运行约束条件，利用rrt*算法生成多条可行路径；
16.利用转弯特性函数计算每一所述可行路径的路径时间代价，并选取所述路径时间代价最小的可行路径作为所述移动机器人的规划路径。
17.一种移动机器人路径规划系统，所述规划系统包括：
18.获取模块，用于获取移动机器人的运行约束条件；所述运行约束条件包括所述移动机器人的可行区域、起始点位置、目标点位置、步长、搜索步长和所述可行区域内的障碍物信息；
19.可行路径生成模块，用于根据所述运行约束条件，利用rrt*算法生成多条可行路径；
20.规划模块，用于利用转弯特性函数计算每一所述可行路径的路径时间代价，并选取所述路径时间代价最小的可行路径作为所述移动机器人的规划路径。
21.根据本发明提供的具体实施例，本发明公开了以下技术效果：
22.本发明所提供的一种移动机器人路径规划方法及系统，先根据移动机器人的运行约束条件，利用rrt*算法生成多条可行路径。然后利用转弯特性函数计算每一可行路径的路径时间代价，并选取路径时间代价最小的可行路径作为移动机器人的规划路径，进而能够为移动机器人规划出一条时间最短的规划路径，减少机器人实际任务的路径时间代价，从而提高了移动机器人执行任务的效率。
附图说明
23.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
24.图1为本发明实施例1所提供的规划方法的方法流程图；
25.图2为本发明实施例1所提供的规划方法的详细流程图；
26.图3为本发明实施例1所提供的生成多条可行路径所用方法的方法流程图；
27.图4为本发明实施例1所提供的转弯运动特性试验中选用的试验小车的结构示意图；
28.图5为本发明实施例1所提供的转弯运动特性试验的多个转弯夹角的示意图；
29.图6为本发明实施例1所提供的rrt*算法和ptc
‑
rrt*算法的路径规划结果示意图；
30.图7为本发明实施例2所提供的路径规划系统的系统框图。
具体实施方式
31.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
32.本发明的目的是提供一种移动机器人路径规划方法及系统，能够规划出一条路径
时间最短的规划路径。
33.为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
34.实施例1：
35.机器人路径规划算法是移动机器人研究的核心内容之一，它起始于20世纪70年代，迄今已有大量的研究成果，为机器人的广泛应用做出重大贡献。传统的路径规划算法可以分为基于节点的方法和基于采样的方法。基于节点的方法的代表为a*算法，a*算法是对dijkstra算法的改进，其引入了启发式函数和约束条件，并且在每轮寻优搜索中，需计算全局范围内每一个可行解对应的启发式函数的值，随后选出该轮搜索中具有最小启发式函数值的可行解作为当前轮次的最优解，然后继续下一轮的寻优搜索，最终保证全局最优路径能够被a*算法找到，其搜索能力较强，能够收敛到全局最优路径，但是其计算复杂度依赖网格分辨率，不适合高维环境路径规划。基于采样的方法中，rrt(rapidly
‑
exploring random tree)算法原理简单，节点扩展不需要进行预处理，具有较强的搜索能力，能够适应任务和环境的动态变化，因此其在机械臂、轮式机器人和无人机等路径规划领域得到了广泛应用。但是，rrt算法随机产生采样点导致其效率较低，且搜索的路径结果不稳定。为了提升算法效率，urmson等人提出一种加入节点偏移概率系数的hrrt(heuris
‑
tically rrt)，该算法设定概率上限，当节点符合概率约束时，将向目标点方向进行扩展，提升了路径规划效率。lavalle提出了bi
‑
rrt(bidirectional rrt)，又称双向随机树，其中心思想是分别将目标点和起始点作为两棵扩展树的树根节点进行扩展，将起始点扩展树作为树一，目标点扩展树作为树二，其中树二会将树一产生的新节点作为其扩展方向，直到两棵扩展树的节点距离小于限定条件，将两颗树进行连接，实现快速寻找路径的目的。上述两种对rrt算法的改进在寻径速度上进行了提升，但并未在路径结果方面进行优化。为了实现路径结果最优性，frazzoli等人提出了rrt*算法，其在rrt算法的基础上，在随机树节点扩展处进行了优化，增加了重新选择父节点和重布线两个环节，使得随机树的节点都能收敛到当前最优值。rrt*通过上述两处改进，具有了渐进最优性，在时间充足的情况下总能找到最优解。
36.但是，当前关于路径规划算法的研究一般直接采用路径长度作为寻优目标，而实际任务常常以时间最短作为寻优需求，现有的路径规划算法无法满足以时间最短为寻优需求的实际需要。为了解决这一问题，本实施例用于提供一种基于rrt*路径规划算法和路径时间代价的移动机器人路径规划方法，简称ptc
‑
rrt*路径规划方法。本实施例的目的是最小化任务时间，使得移动机器人能够在已知环境和任务的情况下，得到时间代价最低的规划路径。利用rrt*算法生成符合约束条件的可行路径，结合其转弯特性计算时间代价并进行排序，得到最短时间路径。
37.如图1和图2所示，本实施例所提供的一种移动机器人路径规划方法包括：
38.s1：获取移动机器人的运行约束条件；所述运行约束条件包括所述移动机器人的可行区域、起始点位置、目标点位置、步长、搜索步长和所述可行区域内的障碍物信息；
39.具体的，运行约束条件包括可行区域、障碍物信息、轮式机器人路径规划的起始点x
start
、目标点x
goal
，步长、搜索步长、初始方向角和轮式机器人的最大转弯角。
40.s2：根据所述运行约束条件，利用rrt*算法生成多条可行路径；
41.如图3所示，s2可以包括：
42.s21：根据所述运行约束条件生成初始扩展树；所述初始扩展树对应一可行路径；
43.s21具体包括：
44.1)建立一个空的扩展树t，并将起始点x
start
添加到扩展树t中，此时，起始点x
start
即为扩展树t中的一个节点；
45.2)在可行区域和障碍物信息的约束下，随机生成一个随机节点x
rand
；随机节点x
rand
处于可行区域内且未位于障碍物所在位置。
46.3)选取与随机节点x
rand
距离最近的扩展树中的一个节点作为邻近节点x
near
，将x
rand
与x
near
进行连接，根据搜索步长在邻近节点x
near
与随机节点x
rand
的连接线上选取一个新节点x
new
，使得||x
near
‑
x
new
||等于搜索步长，即x
near
与x
new
的距离等于搜索步长。随后进行避障检测，判断邻近节点x
near
与新节点x
new
的连接线是否与障碍物发生碰撞；若否，则将新节点x
new
作为节点，将邻近节点x
near
与新节点x
new
的连接线作为树干添加到扩展树t中，此时，邻近节点x
near
即为新节点x
new
的父节点；记录本步骤生成的x
near
和x
new
这两点的坐标信息，并对扩展树t中的节点进行更新，添加节点坐标信息。若邻近节点x
near
与新节点x
new
的连接线与障碍物发生碰撞，则返回2)重新生成随机节点x
rand
。
47.4)判断新节点x
new
和目标点x
goal
的距离是否小于步长，且新节点x
new
与目标点x
goal
的连接线是否与障碍物相碰撞；
48.5)若新节点x
new
和目标点x
goal
的距离小于步长，且新节点与目标点x
goal
的连接线未与障碍物相碰撞，则将新节点x
new
和目标点x
goal
相连接，得到初始扩展树；初始扩展树包括起始点、新节点和目标点，以及节点之间的连接线。在初始扩展树中对新节点x
new
进行父节点追溯，即可得到初始扩展树对应的一条从起始点到目标点的可行路径；
49.6)否则，则将新节点x
new
作为扩展树中的节点，返回“在可行区域和障碍物信息的约束下，随机生成一个随机节点x
rand”的步骤，为扩展树t增加节点和树干，直至满足4)所述的条件。
50.s22：对所述初始扩展树进行更新，得到更新后扩展树；所述更新后扩展树对应一所述可行路径；
51.s22具体包括：
52.1)以初始扩展树的新节点作为待测节点；初始扩展树的新节点即为初始扩展树上与目标点相连接的节点；
53.2)以待测节点作为圆心，以预设领域半径作为划分半径，确定重选区域；重选区域即为以待测节点作为圆心，以划分半径为半径所形成的圆。
54.3)选取初始扩展树中位于重选区域内的所有节点作为待测节点的备选新父节点，得到备选集合；需要说明的是，备选新父节点不包括目标点，但包括起始点。
55.4)判断每一备选新父节点与待测节点的连接线是否与障碍物相碰撞，并在备选集合中去除连接线与障碍物相碰撞的备选新父节点，得到更新后集合；
56.5)计算更新后集合中每一备选新父节点对应的路径代价，并选取路径代价最小的备选新父节点作为待测节点的新父节点；将新父节点以及新父节点与待测节点之间的连接线添加到初始扩展树中。
57.计算更新后集合中每一备选新父节点对应的路径代价具体包括：对于每一备选新父节点，分别计算备选新父节点到起始点的第一路径代价以及备选新父节点到待测节点的
第二路径代价，第一路径代价和第二路径代价的和即为备选新父节点对应的路径代价。在计算第一路径代价时，对备选新父节点进行父节点追溯，获取从备选新父节点到起始点所需经过的各个节点及经过顺序，即获知路径，依次取路径上的两个相邻节点，两个相邻节点之间的距离即为两个相邻节点之间的路径代价，将该路径上所有两个相邻节点之间的路径代价进行求和，即可得到第一路径代价。与计算第一路径代价同理，计算第二路径代价时，对待测节点进行父节点追溯，获取从待测节点到备选新父节点所需经过的各个节点及经过顺序，即获知路径，依次取路径上的两个相邻节点，两个相邻节点之间的距离即为两个相邻节点之间的路径代价，将该路径上所有两个相邻节点之间的路径代价进行求和，即可得到第二路径代价。
58.6)判断新父节点是否为起始点；
59.7)若是，则得到更新后扩展树；在更新后扩展树中进行新节点的新父节点追溯，得到更新后扩展树对应的一条从起始点到目标点的可行路径；
60.8)若否，则将新父节点作为下一迭代中的待测节点，返回“以待测节点作为圆心，以预设领域半径作为划分半径，确定重选区域”的步骤。
61.进而本实施例通过进行重选父节点，以对初始扩展树进行更新。
62.s23：判断是否达到预设更新次数；
63.s24：若否，则将所述更新后扩展树作为下次迭代中的所述初始扩展树，返回“对所述初始扩展树进行更新，得到更新后扩展树”的步骤；
64.s25：若是，则结束迭代，得到多条可行路径。
65.s3：利用转弯特性函数计算每一所述可行路径的路径时间代价，并选取所述路径时间代价最小的可行路径作为所述移动机器人的规划路径。
66.在s3之前，本实施例的规划方法还包括：对移动机器人进行转弯运动特性试验，得到转弯特性函数的步骤。
67.本实施例中应用的ptc
‑
rrt*方法，计算移动机器人路径时间代价的关键在于获取移动机器人转弯角度与转弯时间的函数关系。为了得到移动机器人的转弯时间，可以采用的方式为：通过使机器人按设定的速度和角速度进行匀速直线和不同角度的转弯运动，对其进行转弯运动特性试验。本实施例的转弯运动特性试验中选用的试验小车如图4所示。该试验小车为三轮支撑车体结构，前方左右两边各有一个驱动轮，采用永磁无刷直流伺服电机驱动，车体后方有一个万向轮，主要功能为随动和支撑，通过对前方两个驱动轮的电机转速进行控制实现试验小车的转向和移动。图4所示的试验小车的基本参数如下：车体长度为26cm，宽度为21cm，高度为13cm，驱动轮直径为6.5cm，万向轮直径为4cm。
68.转弯运动特性试验的具体操作为：程序提前指定试验小车在0
‑
2s，试验小车的线速度0.3m/s，角速度为0rad/s；在2
‑
ns，试验小车的线速度为0m/s，角速度为10rad/s；在n
‑
n 2s时，试验小车的线速度为0.3m/s，角速度为0rad/s。试验小车初始位姿为(0，0，0)，程序启动，0
‑
2s，试验小车直线移动0.6m；2
‑
ns，试验小车旋转(n
‑
2)
×
10rad；n
‑
n 2s，试验小车直线移动0.6m。通过修改n值大小来改变试验小车的旋转角度，进而完成0
‑
180度间隔30度的转弯运动特性试验。上述试验过程可简单描述为小车从起点a匀速直线运动0.6m到b点，转一定角度，再继续匀速直线运动0.6m到终点c。
69.上述对移动机器人进行转弯运动特性试验，得到转弯特性函数的步骤具体包括：
70.1)在0
‑
180度区间内，以预设角度为间隔进行划分，得到多个转弯夹角；预设角度可为30度。进而在0
°
到180
°
区间内，以30
°
为间隔所得到的转弯夹角一共有7组，移动机器人转弯运动特性试验的多个转弯夹角的示意图如图5所示。
71.2)对于每一转弯夹角，进行多次转弯运动特性试验，得到多个运行时间；
72.通过改变n值，获取相应的转弯夹角，即获取以b点为顶点的转弯夹角，然后按照上述转弯运动特性试验的过程进行多次实验，将每次实验的运行时间进行记录，即记录试验小车在实际场景中，从起始点a途径转弯点b后，到达目标点c的运行时间，得到该转弯夹角对应的多个运行时间，如表1所示。
73.表1
[0074][0075]
3)对于每一转弯夹角，根据任意两个运行时间之间的时间间隔，在多个运行时间中选取n个运行时间，并以n个运行时间的平均值作为该转弯夹角对应的运行时间；
[0076]
具体针对每一转弯夹角的实验数据，选取数值最接近的三组，计算其平均值，作为该转弯夹角对应的运行时间。
[0077]
4)对所有转弯夹角和转弯夹角对应的运行时间进行函数拟合，得到运行时间与转弯夹角之间的拟合函数；所述拟合函数即为转弯特性函数。
[0078]
利用多阶函数分别进行函数拟合，得到的运行时间t和转弯夹角θ的拟合函数如表2所示。对均方根误差结果进行比较，发现三阶函数的拟合效果最好，均方根误差仅为0.1187，一阶函数和四阶函数的拟合效果其次，二阶函数和五阶函数的均方根误差有较大的增幅，拟合效果较差。因此，本实施例选择三阶函数拟合结果进行后续运行时间的近似计算。根据上述信息，得到了本次试验中选用移动机器人的转弯运动特性。优选的，本实施例利用三阶函数对所有转弯夹角和转弯夹角对应的运行时间进行函数拟合。
[0079]
表2
[0080][0081]
在得到转弯特性函数后，s3可以包括：
[0082]
对于每一可行路径，计算可行路径中的每相邻三个连续节点之间所形成的夹角；所述夹角以第二个节点为顶点；
[0083]
以夹角作为输入，利用转弯特性函数计算每一夹角对应的运行时间；
[0084]
以所有运行时间的和作为可行路径的路径时间代价。
[0085]
举例而言，可行路径共有4个节点，包括起始点和目标点，则先选取1、2、3这组相邻三个连续节点，计算三者组成的以2为顶点的夹角，利用转弯特性函数计算该夹角对应的运行时间。然后再选取2、3、4这组相邻三个连续节点，计算三者组成的以3为顶点的夹角，利用转弯特性函数计算该夹角对应的运行时间。两个运行时间的和即为该可行路径的路径时间代价。
[0086]
以下，通过仿真试验对本实施例的规划方法的可行性加以验证，并通过与传统路径规划方法结果进行比较，对本实施例的规划方法的综合性能进行验证分析，并统计运算效率。在本次仿真试验中设定移动机器人的可行区域为500m*500m的正方形区域，并在可行区域内设定不可通行的障碍物，设置移动机器人的起始点为(10m，10m)，目标点为(480m，480m)，初始方向角为90
°
，步长为10m，搜索步长为30m，最大转弯角为180
°
，领域半径为50m。采用栅格法来构建移动机器人的导航环境地图模型，将移动机器人所处环境划分为一系列大小相同的正方形栅格，随后依据环境中障碍物信息将障碍物对应栅格准确标出，并用黑色区域表示存在障碍物，白色区域表示机器人能顺利通过的无障碍物地区。依据试验要求，建立二维坐标系，建立x轴和y轴描述起始点、目标点等位置。
[0087]
本实施例的ptc
‑
rrt*算法与rrt*算法的性能指标对比如表3所示。rrt*算法和ptc
‑
rrt*算法的路径规划结果如图6所示。路径长度方面，ptc
‑
rrt*算法得到的路径长度比rrt*算法得到的路径长度长3％。路径时间代价方面，ptc
‑
rrt*算法所得路径的时间代价较低，比rrt*算法规划出来的路径的时间代价缩短约20％。
[0088]
表3
[0089][0090]
随后对ptc
‑
rrt*算法的稳定性及运算效率进行验证。在地图区域内对起始点和目标点进行随机采样，在起始点和目标点的位置均处于非障碍物的情况下。将算法运行100次，记录算法的成功概率及平均运行时间，得到的ptc
‑
rrt*算法性能如表4所示。由表4可知，ptc
‑
rrt*算法能在大多数情况下成功规划出一条从目标点到终点的路径，表明算法稳定性较强。
[0091]
表4
[0092][0093]
本实施例结合实际应用中对时间代价的关注，并考虑小车的转弯时间，提出了一种基于路径时间代价的改进rrt*路径规划算法。仿真结果表明，ptc
‑
rrt*算法得到的路径长度虽稍逊于rrt*算法，但考虑转弯特性后，实际运动时间有所下降，验证了所提算法的可行性。同时，随机采样测试表明ptc
‑
rrt*算法具有较好的求解稳定性。
[0094]
轮式移动机器人(wheeled mobile robot，wmr)是移动机器人中应用最为广泛的一种机器人，其具有结构简单稳定、灵活性和转向性较强、易于控制、环境适应能力强等诸多优点。本实施例的路径规划方法还可用于轮式移动机器人。
[0095]
传统的路径规划算法以最小化路径长度为目标，但是实际任务一般以任务完成时间为指标。本实施例将评价标准由路径长度改为路径时间，对rrt*算法进行改进，使其更符合实际需要，提出一种以最小化任务时间为目标的改进rrt*算法，将传统的rrt*算法和移动机器人转弯特性相结合，通过移动机器人转弯特性试验确定转弯角度和转弯时间的近似函数，并将该函数作为路径时间代价的计算依据，在此基础上，提出一种基于路径时间代价的改进rrt*算法，利用rrt*算法生成符合约束条件的可行路径，再结合转弯特性计算其时间代价并进行排序，得到最短时间路径。因此可以减少实际任务的路径时间代价，从而提高了轮式机器人执行任务的效率。
[0096]
实施例2：
[0097]
本实施例用于提供一种移动机器人路径规划系统，如图7所示，所述规划系统包括：
[0098]
获取模块m1，用于获取移动机器人的运行约束条件；所述运行约束条件包括所述移动机器人的可行区域、起始点位置、目标点位置、步长、搜索步长和所述可行区域内的障碍物信息；
[0099]
可行路径生成模块m2，用于根据所述运行约束条件，利用rrt*算法生成多条可行路径；
[0100]
规划模块m3，用于利用转弯特性函数计算每一所述可行路径的路径时间代价，并
选取所述路径时间代价最小的可行路径作为所述移动机器人的规划路径。
[0101]
本说明书中每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0102]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。