一种基于安全通行走廊的最优折线路径规划方法

技术领域

本发明涉及机器人自主路径规划技术领域，尤其涉及一种基于安全通行走廊的最优折线路径规划方法。

背景技术

随着科学技术与人工智能的不断发展，机器人技术引起了众多学者和工程师的关注。移动机器人作为机器人领域的重要分支，因其具有更大的使用灵活性，移动机器人路径规划问题逐渐成为机器人领域的研究焦点。

路径规划问题可以总结为一个二维空间中的搜索问题，最终规划出来的路径必须安全无碰，并且符合动力学约束。文献(Dolgov D,Thrun S,Montemerlo M,et al.Pathplanning for autonomous vehicles in unknown semi-structured environments[J].The international journal of robotics research,2010,29(5):485-501.)提出了一种可以在半结构化或非结构化环境中运行的自动驾驶车辆的实用路径规划算法。该算法通过前端混合A*算法搜索运动学可行的路径，后端共轭梯度优化路径质量。该算法只适用于非完整性约束的机器人，对于一些特殊场景(如狭窄环境或者道路曲率过大)，该算法将无法在生成可行路径。

为了解决确定性的规划方法无法满足的时序约束的问题，文献(Hwan Jeon J,Cowlagi R V,Peters S C,et al.Optimal motion planning with the half-cardynamical model for autonomous high-speed driving[C].2013American controlconference,2013:188-193.)通过随机地在半结构化环境中采样搜索，可以实现快速地进行规划，并且通过对采样到的节点进行限制，可以将车辆的非完整约束考虑进来。但是该方法的采样点很多都是无用的点，浪费了很多计算性能，并且由于采样过程的随机性，该方法的稳定性也较差，寻找到的解往往也是次优的。对于同样的问题，前后两次的解也不一样，因此在严格要求稳定性的场合并不适用。

大规模和非凸的防撞约束使路径规划的计算复杂度过高，针对这个问题，在无人机路径规划领域，提出了安全飞行走廊(SFC)这种方式。该方法通过安全飞行走廊的划定使得无人机与周围障碍物分开，从而降低了规划问题的复杂性，提高了路径规划的速度。文献(Liu S,Watterson M,Mohta K,et al.Planning dynamically feasible trajectoriesfor quadrotors using safe flight corridors in 3-d complex environments[J].IEEE Robotics and Automation Letters,2017,2(3):1688-1695.)提出了一种安全飞行走廊的概念，将轨迹生成公式化为二次规划的方法。并且推导出一种有效的凸分解方法，该方法从使用快速图搜索技术获得的分段线性路径构建安全飞行走廊。安全飞行走廊在二次规划中提供了一组线性不等式约束，允许实时运动规划。但是飞行走廊将机器人视为质点，实际规划过程中，对于体积较大的移动机器人来说，机器人的不同部分可能停留在不同的走廊上，需要考虑机器人的体积对规划的影响。

综上所述，如何找到一种一体化的满足移动机器人运动学约束的最优路径规划方法，既能保证在障碍物密集环境中的自主避碰，又能实现在大曲率狭窄通道中的安全通行，这对移动机器人在复杂场景中的自主导航是非常重要的。

发明内容

本发明所要解决的技术问题是克服现有技术中存在的不足，提供一种基于安全通行走廊的最优折线路径规划方法。

本发明是通过以下技术方案予以实现：

一种基于安全通行走廊的最优折线路径规划方法，包括如下步骤：

S1.通过三维激光进行环境感知，对激光数据进行预处理，生成代价地图；

S2.基于所述代价地图进行路径规划，生成路径初值；

S3.根据所述路径初值生成椭圆区域，以所述椭圆区域为基础，膨胀成凸多边形的安全区域；

S4.各所述安全区域重叠部分为关键区域，在所述关键区域均匀采样，生成一系列采样点，获取最优路径。

根据上述技术方案，优选地，步骤S1中所述“对激光数据进行预处理”包括：剔除离群点和地面点云。

根据上述技术方案，优选地，步骤S1包括：对激光数据进行预处理；生成栅格地图，将预处理后的激光数据投射到占据栅格地图中；对占据栅格进行膨胀处理，生成代价地图。

根据上述技术方案，优选地，步骤S3中所述“安全区域”包括多个凸多边形，且将障碍物排除在各凸多边形之外的区域。

根据上述技术方案，优选地，步骤S3包括：根据所述路径初值生成椭圆区域；膨胀所述椭圆区域，直到与障碍物发生碰撞，得到碰撞点；在所述碰撞点处做椭圆区域的切线，切线将障碍物分为椭圆侧和非椭圆侧；删除所述非椭圆侧的障碍物；重复上述步骤，直到将椭圆周围所有障碍物删除，所有切线围成的凸多边形即为安全区域。

根据上述技术方案，优选地，步骤S3还包括：采用A*算法生成一条由多段直线组成的路径；分别以路径中的每一段直线为长轴，生成一个满足面积最大且与障碍物无碰的椭圆区域。

根据上述技术方案，优选地，步骤S4包括：在所述关键区域均匀采样，生成一系列采样点；通过直线依次拟合各关键区域的采样点，生成一系列备选折线路径；计算各所述备选折线路径的代价，获取最优路径。

本发明的有益效果是：

本发明提供了一种基于安全通行走廊的最优折线路径规划方法，其生成的多边形安全区域保证了机器人运动的安全性，构造的折线路径则符合完整性约束机器人的运动学模型，并且输出直线的路径对于下层控制模块来说可以达到更好的跟踪效果，对代价的计算可以保证输出路径的最优性。本方法既能保证在障碍物密集环境中的自主避碰，又能实现在大曲率狭窄通道中的安全通行，可以有效满足完整性约束机器人在二维空间内快速路径规划的应用需求。

附图说明

图1是本发明的方法流程示意图。

图2为步骤S3中椭圆区域生成安全区域的方法流程示意图。

图3为椭圆膨胀过程示意图。

图4为根据接触点生成切线过程示意图。

图5为删除非椭圆侧障碍物过程示意图。

图6为持续膨胀椭圆并生成切线删除障碍物过程示意图。

图7为安全区域生成过程示意图。

图8为膨胀后的代价地图。

图9为基于代价地图生成的路径初值。

图10为基于路径初值中的直线生成的椭圆区域。

图11为基于椭圆区域生成的安全区域。

图12为基于重叠的安全区域进行采样。

图13为依据采样的路点生成的最优路径。

具体实施方式

为了使本技术领域的技术人员更好地理解本发明的技术方案，下面结合附图和最佳实施例对本发明作进一步的详细说明。基于发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于发明保护的范围。

在发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对发明的限制。

如图所示，本发明包括如下步骤：

S1.通过三维激光进行环境感知，对激光数据进行预处理，生成代价地图。

S11.剔除离群点和地面点云。

在规划之前，首先对激光数据进行预处理。激光雷达测量过程存在误差会产生离群点以及坏点，如距离移动机器人过高和过远的激光点，首先将这些点云进行去除，其次，需要对地面点云进行剔除，采用随机抽样一致(RANSAC)平面拟合的方法识别路面并去除。该算法鲁棒性较强，相比于其他最小二乘方法拟合效果更好，可适应周围噪声较多的环境。

在本例中的预处理阶段，为了防止激光数据受到移动机器人自身凸起结构的影响，将0.3米内的数据进行剔除，由于机器人高度有限，为了更准确的描述机器人通行的区域，同时减小计算量，对高于1米的数据进行剔除，最后通过随机抽样一致算法对地面点云进行识别并剔除。

S12.对激光数据进行预处理，生成栅格地图，将预处理后的激光数据投射到占据栅格地图中，对占据栅格进行膨胀处理，生成代价地图。

根据处理后的激光点云数据，生成栅格地图，然后将预处理后的激光点云投射到占据栅格地图中，根据栅格的占据情况来表示机器人周围的环境。栅格有两种状态：0和1，0代表安全1代表当前区域存在障碍物。移动机器人在路径规划过程中，根据当前位置以及栅格地图情况进行搜索，最终输出一条到达目标点的安全无碰路径。但是在这个过程中并未考虑机器人的自身形状大小，因此需要对占据栅格进行膨胀处理。为了保证规划的安全性，移动机器人在运动过程中不能碰撞地图中障碍物的膨胀区域。栅格地图膨胀大小为移动机器人最长车身距离的一半，膨胀后的地图为代价地图。

S2.基于所述代价地图进行路径规划，生成路径初值。

代价地图生成后，需要基于代价地图进行路径规划，生成路径初值，以便开辟安全多边形区域进行后续优化。由于上一步已经生成了代价地图，因此可采用图搜索算法。

路径规划算法采用A*算法，A*算法是一种图搜索的启发式算法，该方法是基于迪杰斯特拉算法的改进，通过引入与目标点有关的启发项，来提高搜索速度。该算法需要维护一个优先级队列，优先级队列存放着待搜索的栅格节点，每次从优先级队列中弹出代价最小的节点。其代价函数设计如下：

f(n)＝g(n)+h(h)

其中g(n)是节点到起点的距离代价，h(n)是节点到终点的距离代价，这两个代价的线性叠加是最终代价f(n)。

S3.根据所述路径初值生成椭圆区域，以所述椭圆区域为基础，膨胀成凸多边形的安全区域。

安全区域是由一系列凸多边形组成，并且将所有障碍物排除在区域之外的区域，该区域可以保证移动机器人运动的安全性。安全区域约束了移动机器人的运动，但是若该区域过小，则会将很多可行区域划分为不可行，造成机器人运动的范围受限。生成安全区域分为两部分，首先根据路径初值生成椭圆，其次膨胀椭圆，尽可能扩大安全区域。

S31.采用A*算法生成一条由多段直线组成的路径；分别以路径中的每一段直线为长轴，生成一个满足面积最大且与障碍物无碰的椭圆区域。

所生成椭圆表式如下：

该方程表示以点(m,n)为中心，长轴长度为a短轴长度为b，长轴与x轴夹角为α的椭圆。该椭圆表达式可以写成二次曲线的一般式：

Ax

其中A、B、C、D、E、F是描述椭圆一般式的六个参数，由如下公式推出：

a

2(b

a

-2Am-Bn＝D

-Bm-2Cn＝E

k(Am

其中k为非零常数。

S32.以第一步所生成的椭圆为基础，进一步膨胀成凸多边形，该多边形需满足面积最大、且将障碍物排除在多边形之外的约束，将该多边形定义为安全区域。

具体地，包括如下步骤：

S32-1：膨胀所述椭圆区域，直到与障碍物发生碰撞，得到碰撞点。

生成凸多边形的步骤是在原有的椭圆上开始膨胀，当检测到与障碍物发生碰撞时，记碰撞的接触点为p(x

S32-2：在所述碰撞点处做椭圆区域的切线，切线将障碍物分为椭圆侧和非椭圆侧。

如图4所示，该切线可以直接用椭圆参数表达：

S32-3：删除所述非椭圆侧的障碍物，如图5所示。

S32-4：重复上述步骤，直到将椭圆周围所有障碍物删除(图6)，所有切线围成的凸多边形即为安全区域(图7)。

S4.各所述安全区域重叠部分为关键区域，在所述关键区域均匀采样，生成一系列采样点，通过直线依次拟合各关键区域的采样点，生成一系列备选折线路径，计算各所述备选折线路径的代价，获取最优路径。

生成多边形安全区域后，将路径的约束在多边形内，即可保证移动机器人运动的绝对安全。由于多边形区域有重叠部分，重叠部分即视为关键区域。此关键区域是从一个安全多边形运动到另一个安全多边形的必经区域，并且使机器人运动过程中仅在此区域进行转向操作，以达到从一个安全区域运动到另一个安全区域的目的，最终行驶到目标点。

首先在所有关键区域进行采样操作生成路点(采样点)：

n＝kS

其中，n为采样点数量，S为关键区域面积，k为采样系数，路点数量与该区域面积有关。该区域越大，采样点越多，反之，越少。采样后，通过直线依次拟合各区域采样点，即可形成一系列备选折线路径。对于任意两点x

需要评选出一条最优的路径，计算每一条备选路径的代价。代价函数设计如下：

其中，i为第i个关键区域，共有N个关键区域。对于每一处关键区域的路点，有x

代价函数中第一项为距离代价，w

最后采用穷举法，穷举所有备选路径代价，选取代价最小的一条路径作为最优路径进行输出。

本发明通过三维激光进行环境感知，首先根据机器人周围障碍物分布，生成多边形安全区域，区域相交处即为机器人路径规划的关键区域。在此区域进行均匀采样，生成一系列采样点，然后对不同区域的点进行直线连接生成一系列折线路径，对每一条路径的代价进行计算，最终得到最优路径。多边形安全区域保证了机器人运动的安全性，构造的折线路径则符合完整性约束机器人的运动学模型，并且输出直线的路径对于下层控制模块来说可以达到更好的跟踪效果，对代价的计算可以保证输出路径的最优性。该方法可以有效满足完整性约束机器人在二维空间内快速路径规划的应用需求。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。