一种轮式机器人路径规划方法与流程

1.本发明属于机器人路径规划应用技术领域，具体涉及一种轮式机器人路径规划方法。


背景技术：

2.随着人工智能、物联网、云计算等技术的发展，机器人的研发设计、建造技术也获得了较大的提高，机器人正被人们越来越多地应用在各个领域。
3.slam(simultaneous localization and mapping),即时定位与地图构建以及路径规划一直是机器人研究领域中的核心问题。由于现实情况机器人所处的环境复杂多变，机器人的自动寻址能力受环境影响较大，其自动导航寻址能力仍然不够理想。
4.因此，基于上述问题，本发明提供一种轮式机器人路径规划方法。


技术实现要素：

5.发明目的：本发明的目的是提供一种轮式机器人路径规划方法，其目的旨在复杂环境下实现轮式机器人对障碍物、路面坡度、路面拥堵情况的识别，使其能找到一种较好地穿过障碍物、避免陡坡路面动力不足上不去以及避开拥堵路面的方法，使之能快速高效地到达目的地。
6.技术方案：本发明的一种轮式机器人路径规划方法，包括以下步骤，步骤1、建立生成三维点云地图，步骤2、根据三维点云地图生成包含路面拥堵情况、障碍物尺寸、路面坡度信息的三维格栅地图。步骤3、计算路面每个坡度节点间的坡度代价。步骤4、实时计算路面的拥堵代价。步骤5、将路面坡度代价及实时路面拥堵代价加入d*算法的总代价，利用d*算法进行路径规划。
7.本技术方案的，所述步骤3中根据机器人尺寸判断机器人是否可以穿过障碍物，更新三维格栅地图；对于上坡路段，根据机器人重量、路面摩擦系数、机器人最大动力生成d*路径搜索代价函数附加信息项a(n)的子项。
8.本技术方案的，所述步骤4中根据路面拥堵情况，生成d*路径搜索代价函数附加信息项a(n)的子项。
9.本技术方案的，所述步骤5中将代价函数附加信息项a(n)加入到d*路径搜索算法中的代价函数f(n)中，形成代价总函数f(n)＝g(n) h(n) a(n)；以代价总函数f(n)为依据，进行d*路径寻优。
10.本技术方案的，所述步骤5中增加d*路径搜索算法代价函数附加信息项a(n)，使之变为：f(n)＝g(n) h(n) a(n)，其中g(n)为目标节点到任意节点n的代价，h(n)为从节点n到起始点的启发式代价，其中，附加信息a(n)包含有路面拥堵情况代价信息、障碍物尺寸代价信息、路面坡度代价信息，利用d*路径搜索算法对路径进行规划。
11.本技术方案的，所述步骤3中对于上坡路段，根据机器人重量计算上坡阻力f＝μ(mgcosθ) mgsinθ，其中g为比例系数，大小约为9.8n/kg，θ路面坡度；步骤4中对于地图中包
含的拥堵信息，设置kh(n)为附加信息项a(n)的一个子项，k为路面的拥堵系数，路面拥堵系数k的范围为0≤k≤1,k＝0为路面畅通，k＝1为路面严重拥堵。
12.本技术方案的，θ为相邻节点与地平面所形成的坡度角，计算公式为：
[0013][0014][0015]
xa、xb、xc为a点的x轴、y轴、z轴坐标，ya、yb、yc为b点的x轴、y轴、z轴坐标。
[0016]
本技术方案的，附加信息项代价d为机器人所能输出的最大动力，将附加信息项a(n)代价加入d*总代价函数中，得到新的总代价函数f(n)＝g(n) h(n) a(n)。
[0017]
本技术方案的，若机器人动力系统不足以提供大于上坡阻力f的动力，则设置附加信息项a(n)的代价为无穷大，否则设置其中一子项为
[0018]
本技术方案的，h(n)为欧拉距离，计算公式为：
[0019][0020]
与现有技术相比，本发明的一种轮式机器人路径规划方法的有益效果在于：1、算法运行时间短，可以较快找到最优路径；2、实现了复杂环境下的路径智能规划，可以有效应对有障碍物及陡坡路段。
附图说明
[0021]
图1是本发明的一种轮式机器人路径规划方法的算法流程图。
具体实施方式
[0022]
下面结合附图和具体实施例，进一步阐明本发明。
[0023]
本发明的一种轮式机器人路径规划方法，包括以下步骤，步骤1、建立生成三维点云地图，步骤2、根据三维点云地图生成包含路面拥堵情况、障碍物尺寸、路面坡度信息的三维格栅地图。步骤3、计算路面每个坡度节点间的坡度代价。步骤4、实时计算路面的拥堵代价。步骤5、将路面坡度代价及实时路面拥堵代价加入d*算法的总代价，利用d*算法进行路径规划。
[0024]
本发明的轮式机器人路径规划方法，步骤3中根据机器人尺寸判断机器人是否可以穿过障碍物，更新三维格栅地图；对于上坡路段，根据机器人重量、路面摩擦系数、机器人最大动力生成d*路径搜索代价函数附加信息项a(n)的子项。
[0025]
本发明的轮式机器人路径规划方法，步骤4中根据路面拥堵情况，生成d*路径搜索代价函数附加信息项a(n)的子项。
[0026]
本发明的轮式机器人路径规划方法，步骤5中将代价函数附加信息项a(n)加入到
d*路径搜索算法中的代价函数f(n)中，形成代价总函数f(n)＝g(n) h(n) a(n)；以代价总函数f(n)为依据，进行d*路径寻优。
[0027]
进一步的，步骤5中增加d*路径搜索算法代价函数附加信息项a(n)，使之变为：f(n)＝g(n) h(n) a(n)，其中g(n)为目标节点到任意节点n的代价，h(n)为从节点n到起始点的启发式代价，其中，附加信息a(n)包含有路面拥堵情况代价信息、障碍物尺寸代价信息、路面坡度代价信息，利用d*路径搜索算法对路径进行规划。
[0028]
进一步的，步骤3中对于上坡路段，根据机器人重量计算上坡阻力f＝μ(mgcosθ) mgsinθ，其中g为比例系数，大小约为9.8n/kg，θ路面坡度；步骤4中对于地图中包含的拥堵信息，设置kh(n)为附加信息项a(n)的一个子项，k为路面的拥堵系数，路面拥堵系数k的范围为0≤k≤1,k＝0为路面畅通，k＝1为路面严重拥堵。
[0029]
本发明的轮式机器人路径规划方法，θ为相邻节点与地平面所形成的坡度角，计算公式为：
[0030][0031][0032]
xa、xb、xc为a点的x轴、y轴、z轴坐标，ya、yb、yc为b点的x轴、y轴、z轴坐标。
[0033]
本发明的轮式机器人路径规划方法，附加信息项代价本发明的轮式机器人路径规划方法，附加信息项代价d为机器人所能输出的最大动力，将附加信息项a(n)代价加入d*总代价函数中，得到新的总代价函数f(n)＝g(n) h(n) a(n)。
[0034]
本发明的轮式机器人路径规划方法，若机器人动力系统不足以提供大于上坡阻力f的动力，则设置附加信息项a(n)的代价为无穷大，否则设置其中一子项为
[0035]
本发明的轮式机器人路径规划方法，h(n)为欧拉距离，计算公式为：
[0036][0037]
如图1所示，首先建立三维格栅地图的时候需包含障碍物的尺寸大小，如桌子侧面的大小，以便于与机器人本身的尺寸进行比较，以此判断机器人是否可以穿过障碍物；其次，若机器人在移动的时候发现路径上有新的障碍物，需要重新扫描构建新的三维格栅地图；最后路面拥堵代价主要是应用于多台机器人同时工作的场景，以此来防止本机器人与其它机器人挤占路面的情况，路面的拥堵代价需要单独的进程进行实时计算。
[0038]
实施例
[0039]
一种机器人路径规划方法，包括如下步骤：
[0040]
1)本实施例轮式机器人配置了激光雷达和深度相机，用于建立三维点云地图，建立三维点云地图的方法很多，可以采用rtab-map等slam方法，本实施例采用深度相机，获取环境深度图，然后将环境深度图转为点云；
[0041]
2)本实施例通过投影模型计算点云世界坐标系坐标，采用三维刚体变换原理得
到，深度图像与点云坐标之间的转换关系如下：
[0042][0043][0044]
z＝d
[0045]
其中x,y,z是相机坐标系下的三维坐标，u,v为图像中像素的位置(所在的行和列)，c
x
和cy是相机光学在图像坐标系下的坐标，若相机没有畸变，c
x
和cy分别为w/2和h/2，其中w和h分别为图像的宽度和高度。f
x
和fy分别为相机在x轴和y轴的焦距。
[0046]
3)将路面拥堵情况，障碍物尺寸信息加入三维格栅地图中，形成具有路面拥堵情况、障碍物尺寸信息、路面坡度信息的三维格栅地图。
[0047]
4)建立一个优先队列(openlist)以目标点为起始点，将目标点置于openlist中；
[0048]
5)将地图中路径点用state表示，每个state包含指向前一个state的指针(backpointer)；当前state的状态tag，有new、open、closed三种状态，new表示该state未被放置于openlist中，open表示该state正放置于openlist中，closed表示该state已经从openlist中取出；
[0049]
6)将目标点的所有邻近节点(障碍物及不可达的节点除外)也放入优先队列(openlist)中，计算每个目标点的邻近点到目标点的代价及到机器人位置节点的估计代价，计算出openlist表中到目标节点代价最小的节点；
[0050]
7)从openlist表中移出到目标节点估计代价最小的节点，将该节点的邻居节点放入openlist表中；
[0051]
8)重复步骤6、7，直到机器人所在的节点的state的状态为closed，或者openlist表为空(表示不存在到目标节点的路径)
[0052]
9)如果机器人在移动的过程中检测到环境发生变化，进入步骤3重新开始搜索。
[0053]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。