CN109238298A - 一种无人驾驶带避障的路径规划方法 - Google Patents

Abstract

本发明公开一种无人驾驶的带避障的路径规划方法，包括采集工作地块地理信息，获得多边形顺序坐标点，以规定安全边界距离将多边形向内部缩进，以当前工作车辆位置为参考，以与多边形顶点距离最近的为下一个目标点，以此点相邻较长边，生成路径待定点并生成路径；遇到障碍物通过平移待定点绕过障碍物，并能通过比较左右平移距离获得较优路径，通过平移路径待定点生成下一组路径，直至路径能够遍历整个工作地块。本发明通过采集地块和障碍物坐标后，可在路径生成的时候根据障碍物坐标自动生成带有避障路径的行驶路径，极大提高了无人驾驶作业的效率和作业安全性。本发明可以用于含有障碍物的封闭地块中农用机械无人驾驶作业的路径规划。

Description

一种无人驾驶带避障的路径规划方法

技术领域

本发明涉及农机无人驾驶与智能控制领域，尤其是无人驾驶并且带避障的路径规划方法。

背景技术

无人驾驶车辆在传统人工智能领域可认为是一种轮式移动机器人，其实现涉及多个学科与领域，包括计算机软件、算法、数据结构、工业控制硬件、物体识别、传感器感知等，其利用功能不一的传感器来感知车辆周围环境，并根据感知所获得的道路、车辆位置和障碍物信息，规划一条安全无碰撞的路径，控制车辆的速度和转向，从而使车辆能够安全地在道路上自主驾驶。

近年来，人工智能技术的发展给无人驾驶工业带来了革命性变化的机会。与此同时，智能化技术在无人驾驶领域也得到了广泛的应用，这项技术使得车辆的操作更为简单，行驶安全性也更好。无人驾驶在实现上可分为两个步骤：第一，对地理环境的探知，车辆要能够按照人的要求以规定路径行驶，这需要地理定位的支持，目前北斗卫星定位技术快速发展，为这项工作提供了方便、快捷的解决方案。同时路径的规划也要能够规避路径上已经知道的固定障碍物，以免引起行驶安全性问题。第二是路径确定后的控制技术，使车辆能够在规划好的路径上精确行驶，不能出现比较大的偏差。

无人驾驶车辆的路径规划问题，就是在要工作的地块形状下，规划出无人驾驶车辆能够遍历整个工作地块、没有障碍物碰撞的一条安全行驶路径。

目前无人驾驶车辆采用的工作路径规划在作业时多需要人工干预，如在遇到需要转弯、掉头、规避障碍物时需要人工控制车辆完成动作，耗费人力，受时间、环境等客观条件限制，在面对大面积工作地块进行标准化作业时，驾驶员受生理条件限制无法长时间按标准完成工作，限制了作业效率。

发明内容

为了解决上述问题，本发明的目的在于为自动驾驶系统根据工作地块提供一条可以遍历整个地块，实现无漏作业，又可以绕过障碍物的自动路径规划方法，该方法仅需采集工作地块形状和障碍物位置，就可自动规划出行驶路径，无需其他人工代价，一次信息采集后，针对同一地块下次作业时无需再做信息采集工作，方便简单，降低人工成本，可绕过障碍物，保障作业安全。

本发明解决其技术问题所采用的技术方案是：

一种无人驾驶的带避障的路径规划方法，采集工作地块地理信息，获得多边形顺序坐标点，以规定安全边界距离将多边形向内部缩进，以当前工作车辆位置为参考，以与多边形顶点距离最近的为下一个目标点，以此点相邻较长边，生成路径待定点并生成路径；遇到障碍物通过平移待定点绕过障碍物，并能通过比较左右平移距离获得较优路径，通过平移路径待定点生成下一组路径，直至路径能够遍历整个工作地块。

一种无人驾驶的带避障的路径规划方法，所述生成路径包括以下步骤：

S1建立地块地理模型：预先对将要作业的地块进行地理信息采集、障碍物地理信息采集，建立地块地理模型，采集信息包括行驶误差，记为Bias、安全边界距离，记为Threshold、工作机具宽度，记为Stride。障碍物地理信息采集，若障碍物模型为圆形，采集包括原点坐标和半径。

S2采集地块顶点坐标：顺、逆时针绕地块采集地块多边形顶点坐标，根据将要作业的地块，顺或逆时针预先采集地块特征点，如四边形、五边形或其他多边形地块的顶点。

S3边界缩进：将地块多边形按照工作时离地边界安全距离Threshold向内部缩进，生成边界特征点。

S4依次获取边界特征点：获得当前车辆所在位置坐标点，记为P[0]，取距P[0]最近的边界特征点，记为P[1]，另一个邻点记为P。计算P[1]相邻的两个点与P[1]的距离，取最长的一条为参考线段，记为P[1]P[2]线段。

S5生成路径待定点：在此线段上以Stride/10的间隔生成路径待定点，遍历此组路径待定点，是否有在障碍物圆形内，如有加上一个不在圆内的点、下一个不在原内的点组成障碍物点，此组点先向左平移，直至此组点没有在障碍物圆内的点，若都在边界内则保留，超出边界则放弃，向右平移判断，若向左、向右平移都在边界内则，取平移距离较短的一组，取头尾两个路径待定点、取障碍物点头尾点，加入路经特征点，以真实路径间隔realStride为间距、生成平行P[1]P[2]的另一组特征点记为（X1，Y1）。

S6遍历路径待定点：通过遍历路径待定点即可确定路径特征点，将路径特征点首尾相接即为规划完成的路径。

本发明有益效果是：

本发明通过采集地块和障碍物坐标后，可在路径生成的时候根据障碍物坐标自动生成带有避障路径的行驶路径，极大提高了无人驾驶作业的效率和作业安全性。本发明可以用于含有障碍物的封闭地块中农用机械无人驾驶作业的路径规划。

附图说明

图1示出地块按安全边界距离向内缩进示意图。

图2示出判断坐标点是否在边界内示意图。

图3示出路径规划算法流程图。

图4示出路径平行线生成示例示意图。

图5示出判断车辆绕过障碍物路径生成示意图。

图6示出判断车辆是否行驶过目标点示意图。

具体实施方式

尽管将结合实例来描述本发明，但应理解，这些实例并未将本发明局限。相反，本发明意欲涵盖包括在由所附权利要求书限定的本发明的各种替代、修改和等效方案。此外，在以下本发明的详细描述中，阐述了许多具体的细节，以便提供对本发明的透彻理解。然而，对本技术领域技术人员显而易见的是，可以在没有这些具体的细节的情况下实践本发明。众所周知的方法、程序等不进行详细描述，在实际应用中，面对不同的车辆情况、不同的工作要求、不同的地理环境都需要做出相应的超参数调整。

本发明在具体实施中，首先预先对将要作业的地块进行地理信息采集、障碍物地理信息采集，建立地块地理模型，然后根据此模型可自动规划出作业车辆在此地块中的行驶路径，行驶路径由目标点构成，车辆朝目标点行驶实现遍历整个作业地块、对整个地块的全覆盖作业，作业路径能够避开采集的障碍物。

所述自动规划出作业车辆在此地块中的行驶路径，仅需采集地块地理信息，然后根据路径规划算法可自动生成绕开障碍物的行驶路径，无需其他人工协同。

所述行驶路径由目标点构成，车辆行驶由当前位置朝向目标点行驶，到达目标点后，目标点更换为下一目标点。

所述实现对整个地块的全覆盖作业，以车载机具宽度、行驶误差的差值（记为realStride）为依据、不越过地界为标准，实现机具对整个地块的作业，不存在遗漏。

下面以实施例一来说明所述路径规划算法，该算法包括以下步骤：

(1)确定行驶误差，记为Bias、根据车载机具宽度确定路径间隔，记为Stride，设定工作时离地边界安全距离设为Threshold，地块多边形按照工作时离地边界安全距离Threshold向内部缩进，生成边界特征点；

(2) 以当前车辆所在目标点为起始点，记为P[0]；

(3) 计算P[0]与所有边界特征点的距离，以距离最短的点为下一目标点，记为P[1]，P[0]为路径特征点第一个点；

(4) 所有边界点为顺或逆时针顺序采集，计算P[1]相邻的两个点与P[1]的距离，取最长的一条为参考线段（或手动选取某一邻点取参考线），记为P[1]P[2]线段，另一个邻点记为P，在此线段上以Stride/10（可按实际情况设置此项）的间隔生成路径待定点，遍历此组路径待定点，是否有在障碍物圆形内，如有加上上一个不在圆内的点、下一个不在原内的点组成障碍物点，此组点先向左平移，直至此组点没有在障碍物圆内的点，若都在边界内则保留，超出边界则放弃，向右平移判断，若向左、向右平移都在边界内则，取平移距离较短的一组，取头尾两个路径待定点、取障碍物点头尾点，加入路经特征点；

以真实路径间隔realStride(路径间隔Stride与行驶误差Bias的差值)为间距、以P[1]P方向生成平行的另一组路径待定点，

a)判断此组路径待定点是否有超出边界的点，有则删去，判断是否有需要绕过的障碍物，生成障碍物点；

b)如路径待定点没有超出边界的点，则在此组路径待定点两端按照Stride/10（可按实际情况设置此项）的距离生成路径待定点，判断是否有需要绕过的障碍物，生成障碍物点，如生成的路径待定点在边界内则加入路径待定点组，将此组路径待定点取头尾两个路径待定点、取障碍物点头尾点然后逆序，加入路经特征点；

重复生成平行路径待定点，至生成的平行路径待定点的个数小于2为止；

(5)最后以距离最后一个生成的边界特征点最近的边界点为起始，将边界点依次加入路径特征点，将边界点依次加入路径特征点，用作边界工作路径，绕地工作一周，遍历到边界未工作的区域，则全部路径特征点生成完毕；

下面结合附图3所示是路径规划算法流程图和实施例二，具体说明路径规划和生成的实施步骤如下：

步骤1：采集地块信息

确定车辆行驶误差，记为Bias。确定车辆安全边界距离，记为Threshold。确定车辆工作宽度，记为Stride。通过手持、车载定位设备，顺、逆时针绕工作地块采集地块多边形顶点坐标和地块中的障碍物坐标以及大小。

步骤2：以安全距离向内缩进

如图1所示,是地块按安全边界距离向内缩进示意图，以离地边界安全距离Threshold为基准向地块多边形内部进行缩进，缩进采用解析几何方法，使用矢量法实现，生成边界特征点。将工作地块多边形记为四边形：PP[2]CP[1],将顶点向内部缩进，实现多边形缩进效果，以P[1]点为例，L为平行线间距离，也即缩进距离，α为P[1]P、P[1]P[2]夹角，P_new 点为P[1]点缩进后的生成点，P_new坐标可用向量运算得到，计算方法如下：

P_new=P[1] + L /*(Normalize()+ Normalize())

其中为向量，为向量积绝对值、、为向量的模，Normalize为向量取单位向量，L为缩进距离，α为向量的夹角。

步骤3：以当前车辆最近点为起点

获得当前车辆所在位置坐标点，记为P[0]，取距P[0]最近的边界特征点，记为P[1]，地块多边形顶点为顺序采集，计算P[1]相邻的两个点与P[1]的距离，取最长的一条为参考线段（或手动选取某一邻点取参考线），记为P[1]P[2]线段，另一个邻点记为P。

步骤4：生成待定路径点

(1)根据车载机具宽度确定路径间隔，记为Stride，设定工作时离地边界安全距离设为Threshold，以车载机具宽度、行驶误差的差值，记为realStride；

(2)以当前车辆所在目标点为起始点，记为P[0]；

(3)计算P[0]与所有边界特征点的距离，以距离最短的点为下一目标点，记为P[1]，P[0]为路径特征点的第一个点;

(4)所有边界点为顺或逆时针顺序采集，计算P[1]相邻的两个点与P[1]的距离，取最长的一条为参考线段（或手动选取某一邻点取参考线），记为P[1]P[2]线段，另一个点记为P，在此线段上以Stride/10（可按实际情况设置此项）的间隔生成路径待定点。

步骤5：遍历路径待定并生成绕过障碍物路径，参考图5，是判断车辆绕过障碍物路径生成示意图，是否有在障碍物圆形内，如有加上一个不在圆内的点、下一个不在原内的点组成障碍物点，此组点先向左平移，直至此组点没有在障碍物圆内的点，若都在边界内则保留，超出边界则放弃，向右平移判断，若向左、向右平移都在边界内则，取平移距离较短的一组，取头尾两个路径待定点、取障碍物点头尾点，加入路经特征点。

步骤6：生成平行路径待定点，以真实路径间隔realStride为间距、以P[1]P方向生成平行的另一组路径待定点，判断此组路径待定点是否有超出边界的点，有则删去。如路径待定点没有超出边界的点，则在此组路径待定点两端按照Stride/10（可按实际情况设置此项）的距离生成路径待定点，如生成的路径待定点在边界内则加入路径待定点组，

如图4所示是路径平行线生成示例示意图，路径平行线生成计算过程如下：

以真实路径间隔realStride为间距、生成平行P[1]P[2]的另一组特征点，以其中某个点（X0，Y0）为例计算过程如下：

1)：P[1]P[2]与X轴夹角小于45度

将生成点记为（X1，Y1）

X1=X0

Y1=Y0+flag*realStride/

其中为向量与横轴正向的夹角,flag为P的纵坐标值与与P[1]的纵坐标值差值的符号,

负号flag=-1，正号flag=1，零值flag=0。

2)：P[1]P[2]与X轴夹角大于45度

将生成点记为（X1，Y1）

X1=X0+ flag*realStride/

Y1=Y0

其中为向量与横轴正向的夹角,flag为P的横坐标值与与P[1]的横坐标值差值的符号,

负号flag=-1，正号flag=1，零值flag=0。

如图2所示是判断坐标点是否在边界内示意图，为判断点是否超出了地块多边形的计算过程。P为在多边形内的点，为不在多边形内部的点。判断方法如下：

以当前点向横轴正方向引射线，若此射线与多边形各边相交边数（相交于非端点）为奇数则点在多变形内，为偶数点在多变形外。

遍历多边形各边，判断点向横轴正向引的射线与边是否相交与非端点，以边a:(X0,Y0),(X1,Y1)，点P:(X,Y)为例

1）若以P向横轴正向引的设想与边a相交于非端点，判断方法为：

(Y0>Y)!=(Y1>Y)此语句为真，则Y在Y0、Y1两值之间。

2）判断点不在边上：

X<(X0-X1)/((Y0-Y1)*(Y-Y1))+X0，此语句为真，点P不在边a上

若同时满足(1)、(2)则点P向横轴正方向引的射线与边a相交。

不越过地界为标准，将地块多边形按照工作时离地边界安全距离Threshold向内部缩进，生成缩进多边形，按照此为实际规划路径时的边界。

判断此组路径特征点是否有超出边界的点、生成的路径特征点是否在边界内，判断点是否在多边形内，多边形可为凹多边形或凸多边形，判断方法如下：

(1)首先判断点的纵、横坐标是否超过多边形最大、小于最小纵横坐标，如超过最大、小于最小纵、横坐标则一定不在多边形内；

(2)以当前点向横轴正方向引射线，若此射线与多边形各边相交边数（非相交于端点）为奇数则点在多边形内，为偶数点在多边形外。

步骤7生成所有路径特征点：此组路径待定点取头尾两个路径待定点、取障碍物点头尾点然后逆序，加入路经特征点。重复生成平行路径待定点，至生成的平行路径待定点的个数小于2为止。最后以距离为判断条件最近的点为起始，将边界点依次加入路径特征点，用作边界工作路径，则全部路径特征点生成完毕。

如图6所示，判断车辆是否行驶过目标点示意图，到达目标点后的判断方法如下：

(1)判断行驶过程中车辆与目标点的距离，记为d，若小于设定阈值（由作业要求、车辆质量决定，人为设置），认为到达目标点；

(2)以A:(X0,Y0),B:(X1,Y1)组成的线段为路径，车当前点P:(X,Y)，车辆由A驶向B为例

计算：，即向量的夹角余弦值，若小于0，则超过目标B。

考虑自动驾驶行驶速度，确定控制频率，以上二者满足任一，可认为行驶到达目标点，车辆行驶过程中要采集自身定位数据，由此采取控制策略，采集定位数据的频率与车辆速度成正比，防止出现由于采集定位数据频率过小，引起车辆在一个控制周期内行驶的距离超过目标点大于设定阈值。

Claims (2)

1.一种无人驾驶的带避障的路径规划方法，其特征是：采集工作地块地理信息，获得多边形顺序坐标点，以规定安全边界距离将多边形向内部缩进，以当前工作车辆位置为参考，以与多边形顶点距离最近的为下一个目标点，以此点相邻较长边，生成路径待定点并生成路径；遇到障碍物通过平移待定点绕过障碍物，并能通过比较左右平移距离获得较优路径，通过平移路径待定点生成下一组路径，直至路径能够遍历整个工作地块。

2.根据权利要求1所述的一种无人驾驶的带避障的路径规划方法，其特征是：所述生成路径包括以下步骤：

S1建立地块地理模型：预先对将要作业的地块进行地理信息采集、障碍物地理信息采集，建立地块地理模型，采集信息包括行驶误差，记为Bias、安全边界距离，记为Threshold、工作机具宽度，记为Stride；

S2采集地块顶点坐标：顺、逆时针绕地块采集地块多边形顶点坐标；

S3边界缩进：将地块多边形按照工作时离地边界安全距离Threshold向内部缩进，生成边界特征点；

S4依次获取边界特征点：获得当前车辆所在位置坐标点，记为P[0]，取距P[0]最近的边界特征点，记为P[1]，另一个邻点记为P；

计算P[1]相邻的两个点与P[1]的距离，取最长的一条为参考线段，记为P[1]P[2]线段；

S5生成路径待定点：在此线段上以Stride/10的间隔生成路径待定点，遍历此组路径待定点，是否有在障碍物圆形内，如有加上一个不在圆内的点、下一个不在原内的点组成障碍物点，此组点先向左平移，直至此组点没有在障碍物圆内的点，若都在边界内则保留，超出边界则放弃，向右平移判断，若向左、向右平移都在边界内则，取平移距离较短的一组，取头尾两个路径待定点、取障碍物点头尾点，加入路经特征点，以真实路径间隔realStride为间距、生成平行P[1]P[2]的另一组特征点记为（X1，Y1）；

S6遍历路径待定点：通过遍历路径待定点即可确定路径特征点，将路径特征点首尾相接即为规划完成的路径。