路径规划系统及路径规划方法与流程

[0001]
本发明涉及机器人导航技术领域，尤其涉及路径规划系统及路径规划方法。


背景技术：

[0002]
现有的路径规划算法主要有：dijkstra算法、a*算法、d*路径搜索算法、prm、rrt路径规划算法和人工势场法。
[0003]
dijkstra算法是较早期的路径规划算法，是一种经典的广度优先的状态空间搜索算法，即算法会从初始点开始一层一层地搜索整个自由空间直到到达目标点。dijkstra算法在规划过程会做出很多无用的计算。
[0004]
a*算法在dijkstra的基础上增加了启发式，减少了大量不必要的搜索。a* 算法已在移动机器人导航中使用广泛。a*虽然有效的降低了规划过程中不必要的搜索量，但仍然没有区分不同范围机器人导航对路径的要求，造成了资源的浪费，规划频率难以提高。
[0005]
d*路径搜索算法是一种启发式的路径搜索算法，适合面对周围环境未知或者周围环境存在动态变化的场景。d*路径搜索算法在离线路径规划时采用类似等势线逐级扩展的方式，从目标点开始对地图节点进行遍历，导致搜索范围较大，搜索效率低，尤其当搜索地图区域较大时，问题更为突出。
[0006]
prm是一种基于图搜索的方法，它在空间中随机设置路标点并进行碰撞检测，再利用a*等搜索算法在路线图上寻找路径。当规划环境中存在狭窄通道时，prm算法很难成功规划，稳定性不足。
[0007]
rrt路径规划算法是一种通过在规划空间中不断生长随机树的方法，通过快速的随机方向的扩展随机树，rrt算法可以快速的完成复杂空间下的规划。然而，rrt在扩展过程中随机树生长方向较随机，导致算法搜索效率低，收敛速度较慢；此外，rrt在动态环境下，每次规划的结果都可能不同，无法形成稳定的路径；人工势场法是一种人为制造势场的方法，通过对障碍物和目标点分别添加排斥和吸引机器人的作用，在势差引导下让机器人避开障碍到达目标点。人工势场法在个别情况下会陷入局部极小值陷阱，导致无法完成规划任务，不够稳定。
[0008]
现有的路径规划算法存在共同的缺陷：在规划时不区分规划的范围，均以同样的精度进行规划，造成了不必要的计算，降低了规划频率。不适合大范围的规划，或者在三维空间等计算量较大的场景使用。


技术实现要素：

[0009]
本发明为了解上述问题提供一种路径规划系统及路径规划方法。
[0010]
本发明通过以下技术方案实现：路径规划系统，包括至少两级规划器，每级规划器的规划范围不同，规划器的规划精度、规划频率与规划范围成反比。
[0011]
进一步的，包括三级规划器，分别为：
第一级规划器：负责从起点到终点的规划；第二级规划器：负责第二规划范围的规划，所述第二规划范围小于从起点到终点的范围；第三级规划器：负责第三规划范围的规划，所述第三规划范围小于所述第二规划范围。
[0012]
优选地，所述第二规划范围大于传感器监测范围。
[0013]
路径规划方法，它采用所述的路径规划系统来规划路径，规划器的级数越高规划范围越小，每级规划器均从起点开始规划；取每级规划器规划的路径，相邻两级规划器的规划路径求差集得分段路径；将分段路径与最高级数的规划器的规划路径求并集得最终路径。
[0014]
进一步的，每级规划器有各自的线程，每级规划器分别通过一标志位控制启动；这个标志将在输入目标坐标时置真；标志置真时，线程开始工作；否则线程进入休眠状态。
[0015]
进一步的，下一级规划器的目标来源于上一级规划器的路径。
[0016]
进一步的，第一条路径的规划流程为：判断机器人与终点的距离是否小于第一阀值，若是，则第一级规划器就进入休眠；若否，则启动第一级规划器生成第一条路径。
[0017]
进一步的，第二条路径的规划流程为：判断机器人与第二级规划器目标点的距离是否小于第二阀值，若是，则第二级规划器就进入休眠；若否，则判断机器人与总目标点的距离：当机器人与总目标点的距离大于第二级规划器的规划范围时，将所述第一条路径上的距离为第二级规划器范围的点设定为第二级规划器的目标点；当机器人与总目标点的距离小于第二级规划器范围时，将总目标点设定为第二级规划器的目标点；第二级规划器的目标点设定完成后，启动第二级规划器生成第二条路径；若第一条路径没有生成，则进入循环休眠等待；所述第二阀值小于第一阀值。
[0018]
进一步的，第三条路径的规划流程为：判断机器人与第三级规划器目标点的距离是否小于第三阀值，若是，则第三级规划器就进入休眠；若否，则判断机器人与总目标点的距离：当机器人与总目标点的距离大于第三级规划器的规划范围时，将所述第二条路径上的距离为第三级规划器范围的点设定为第三级规划器的目标点；当机器人与总目标点的距离小于第三级规划器范围时，将总目标点设定为第三级规划器的目标点；第三级规划器的目标点设定完成后，启动第三级规划器生成第三条路径；若第二条路径没有生成，则进入循环休眠等待；所述第三阀值小于第二阀值。
[0019]
优选地，所述第一阀值为第二级规划器的规划距离；第二阀值为第三级规划器的规划距离。
[0020]
与现有技术相比，本发明具有以下有益效果：本发明根据机器人导航的实际需求，减少了机器人远距离规划不必要的消耗，不仅能得出有效稳定的路径，增强机器人在导航中的反应能力，增强机器人导航的实时性，提高对实时障碍的反应速度，而且可减少规划中的资源浪费。
附图说明
[0021]
此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定。
[0022]
图1是在第一种障碍下a*的搜索量示意图；图2是在第二种障碍下a*的搜索量示意图；图3是实施例一的流程图；图4是三级规划器路径规划效果图；图5是三条路径组合成最终路径的示意图。
具体实施方式
[0023]
为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。
[0024]
为了让四足机器人完成导航任务，同时发挥其优秀的运动能力来完成上下楼梯等任务，故需要在三维空间中做三维路径的规划，其运算量相对于二维平面上的运算量要大的多。但是导航系统对与实时性有着很高的要求，所以路径规划必须要达到一定的运行速度。
[0025]
对于a*算法来说，在规划路径时需要遍历的节点越多所花费的时间也越多，所以希望节点数量能够尽可能的少。通常，当地图分辨率一定时，设定的目标点距离起点越远，需要遍历的节点也就越多。当地形越复杂，需要遍历的节点也会越多。且距离越远，地形也会相对更复杂，如图1、2所示，黑色部分为障碍，红色为已遍历节点。
[0026]
但是，目标点当然是不可更改的，所以减少规划量的方法可以是反过来降低地图精度以减少搜索量。但这样做的后果是作为机器人运动指导的路径精度不足导致机器人很有可能撞上障碍。所以规划的一定精度是必要的，同时规划的速度也是必要的。但这些在一条规划线上想要实现是较为困难的。
[0027]
虽然希望机器人能沿一条精确的路径运动，但是机器人并不应该对远距离的地形或障碍做出过于精确的反应，这对机器人的导航并没有帮助。所以，只需要距离机器人较近范围内的路径的精度高，而远距离只需要提供大致的路径即可。其次，因受到传感器范围的限制，机器人无法得知超过传感器范围的障碍。所以，未在机器人传感器范围内的路径不需要快速的更新，而且通常目标点到机器人传感器范围边界的距离占了整个路径的大部分，快速的更新会消耗大量的算力。
[0028]
总的来说，希望路径能够做到在近距离时规划频率要高，规划精度要高，在远距离时则反之。
[0029]
通过上述思路，本发明公开的路径规划系统，包括至少两级规划器，每级规划器的规划范围不同，规划器的规划精度、规划频率与规划范围成反比。
[0030]
规划器采用现有的路径规划算法进行路径规划，这是本领域的常规技术，此处不再赘述。
[0031]
本发明公开的路径规划方法，采用上述路径规划系统来规划路径，规划器的级数越高规划范围越小；
每级规划器均从起点开始规划，取每级规划器规划的路径，相邻两级规划器的规划路径求差集得分段路径；将分段路径与最高级数的规划器的规划路径求并集得最终路径。
[0032]
每级规划器有各自的线程，并非串行的工作方式。每级规划器分别通过一标志来控制启动；这个标志将在输入目标坐标时置真，标志置真时，线程开始工作，否则线程进入休眠状态。
[0033]
基于上述路径规划系统，本发明公开一实施例。
[0034]
实施例1本实施例中规划器分为三级，分别为第一级规划器、第二级规划器和第三级规划器。第一级规划器的规划精度、规划频率小于第二级规划器，第二级规划器的规划精度、规划频率小于第三级规划器。
[0035]
第一级规划器：负责从起点到终点的规划。通过降低规划精度的方式来提高规划速度，但不要求非常高的频率，即做到规划有较大的广度，较低的精度和速度。
[0036]
第二级规划器：负责从起点到大于传感器范围的规划。由于超过了传感器范围，避免了目标不可达的情况。第二级规划器承担了快速避开障碍的作用，较高的速度，一般的精度。其为第三级规划器提供了顺利规划的条件；第二级规划器的目标则是来源于第一级规划器的路径。
[0037]
第三级规划器：仅在起点到一米范围内规划，有较高的规划精度，较高的规划速度，保证了机器人能精确快速的避开障碍。第三级规划器的目标来源于第二级规划器的路径。
[0038]
如图3所示，将三级规划器分为三个线程。
[0039]
在第一级规划器的构造函数中将a*规划器的分辨率按需求设高。
[0040]
第一级规划器规划路径的主要流程为：为整个规划器设置一个启动标志量goal_reach并初始置假，这个标志将在输入目标坐标时被置为真，开始工作；否则线程进入休眠状态，避免占用资源。
[0041]
开始工作后，先要更新当前机器人的坐标，判断机器人与终点的距离是否小于第一阀值，若是，则清空路径并进入休眠；若否，则更新局部地图（全局地图在系统启动时载入），获取实时点云后将其通过体素滤波得到与规划器相同分辨率的点云，再转换为导航用地图并碰撞后给a*规划器使用；然后将启动a*规划器寻路，将返回的路径按分辨率缩放后得到实际世界坐标系下的路径点，生成第一条路径。
[0042]
第一阀值根据需要设置，本实施例中第一阀值为第二级规划器的规划距离，第二级规划器负责3米范围内的路径规划，所以第一阀值为3米；当机器人坐标到达目标点3米范围内后，第一级规划器就进入休眠。
[0043]
为了控制规划器的工作频率，使它不必无意义的占用计算机资源，计算该线程功能所花时间，设定工作频率后将使线程剩余的时间进入休眠，避免占用其他线程资源。
[0044]
第二级规划器与第一级规划器大体相同，除了参数不同以外，第二级规划器在设定目标时还要先判定机器人当前坐标与总目标点的距离。其主要流程如下：开始工作后，判断机器人与第二级规划器目标点的距离是否小于第二阀值，若是，则第二级规划器就进入休眠；若否，则判断机器人当前坐标与总目标点的距离：
当机器人与总目标点的距离大于第二级规划器的规划范围时，将所述第一条路径上的距离为第二级规划器范围的点设定为第二级规划器的目标点；若第一条路径没有生成，则进入循环休眠等待；当机器人与总目标点的距离小于第二级规划器范围时，将总目标点设定为第二级规划器的目标点；第二级规划器的目标点设定完成后，启动第二级规划器生成第二条路径。
[0045]
第二阀值根据需要设置，但第二阀值应当小于第一阀值。本实施例中第二阀值为第三级规划器的规划距离，第三级规划器负责1米范围内的路径规划，所以第二阀值为1米；当机器人坐标到达目标点1米范围内后，第二级规划器就进入休眠。
[0046]
第三级规划器与第二级规划器基本相同，但是第三级规划器还担任了完成整个规划任务的角色。在第三级规划器判定机器人已经到达目标点范围后，将整个规划器的启动标志量goal_reach置假，停止整个规划系统。
[0047]
第三级规划器规划路径的主要流程为：判断机器人与第三级规划器目标点的距离是否小于第三阀值，若是，则第三级规划器就进入休眠；若否，则判断机器人当前坐标与总目标点的距离：当机器人与总目标点的距离大于第三级规划器的规划范围时，将所述第二条路径上的距离为第三级规划器范围的点设定为第三级规划器的目标点；若第二条路径没有生成，则进入循环休眠等待；当机器人与总目标点的距离小于第三级规划器范围时，将总目标点设定为第三级规划器的目标点；第三级规划器的目标点设定完成后，启动第三级规划器生成第三条路径。
[0048]
第三阀值应当小于第二阀值，第三阀值可根据第三级规划器的规划精度、效率等因素合理设置。
[0049]
如图4、5所示，三个规划器返回的三条路径，分别为第一级规划器规划的第一条路径，第二级规划器规划的第二条路径和第三级规划器规划的第三条路径。
[0050]
取第三条路径为第一段路径
①
，所述第二条路径中除掉与第三条路径重合部分后的剩余部分为第二段路径
②
，取所述第一条路径中除掉与第二条路径重合部分后的剩余部分为第三段路径
③
。
[0051]
然后，将第一段路径
①
、第二段路径
②
和第三段路径
③
组合形成最终路径，这样就得到了一个靠近机器人部分的路径规划频率高、规划精度高；在远离机器人的部分消耗资源少且总体避障反应快速的路径。
[0052]
本发明根据机器人导航的实际需求，减少了机器人远距离规划不必要的消耗，不仅能得出有效稳定的路径，增强机器人在导航中的反应能力，增强机器人导航的实时性，提高对实时障碍的反应速度，而且可减少规划中的资源浪费。
[0053]
以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。