CN111506070B - 一种基于路径点偏移的局部路径规划方法 - Google Patents

Abstract

本发明公开了一种基于路径点偏移的局部路径规划方法，包括步骤：S1：获取期望路径信息、当前车辆位姿和障碍物信息；S2：根据S1获取的各信息，进行初始期望路径点偏移绕过障碍物；S3：根据S2的偏移点，计算偏移后的路径点航向角增量和方位角增量；S4：车辆跟踪初始期望路径计算出的航向角和方位角，在此基础上加上S3中的航向角增量和方位角增量；S5：将S4的航向角和方位角输出给控制层对车辆进行控制。本发明的路径规划方法计算量小且研发周期短，便于后期的维护，而且在避障过程中，实时探测障碍物的横向距离来进行避障，即使不能获得障碍物长度也可进行实时的避障，提高了其适用性。

Description

一种基于路径点偏移的局部路径规划方法

技术领域

本发明涉及无人驾驶技术领域，更具体地说，涉及一种基于路径点偏移的局部路径规划方法。

背景技术

随着自动驾驶技术不断的突破，汽车行业得到前所未有的发展，但是目前在自动驾驶领域中，工业用车的技术相对于民用车落后，并没有大量的研究和应用，如矿用车辆、农用车辆和铰接车辆等。对于自动驾驶矿用车的研究，将极大地减少复杂作业环境导致的人员伤亡，也提高了矿用车运行效率和综合效益，而局部路径规划是研究自动驾驶的重要内容之一。

自动驾驶矿用车的局部路径规划是根据当前位姿信息、期望路径和障碍物信息，生成避开障碍物的平滑曲线，避开障碍物后仍能继续沿期望路径前行。目前针对自动驾驶矿用车的局部路径规划方法的专利和文献较少，且很难将现有的局部路径规划方法嵌入到自动驾驶矿用车。现有局部路径规划方法有图搜索、RRT等，但这些方法大部分容易陷入局部最优解，且只适用于有环境地图，计算时间长效率低，如专利CN201611097409.5公开了一种用于测试基于高精度地图规划的路径的方法和装置，其中路径规划方法是基于高精地图，该方法计算量大，难以得到广泛应用。专利CN201710044287.1公开了一种智能车局部路径规划方法及其装置、车辆，该专利中路径规划方法不需要提前进行地图采集，运算量小，但该路径规划方法成功的前提是障碍物大小为已知，在传感器检测障碍物的时候，障碍物的实际大小(如障碍物的长度)很难获取，而且该路径规划方法无法适应变化的环境。因此本发明采用路径偏移点的路径规划方法对障碍物进行避让，在不需要地图的基础上，该路径规划方法计算量小且研发周期短，便于后期的维护，而且在避障过程中，实时探测障碍物的横向距离来进行避障，即使不能获得障碍物长度也可进行实时的避障，提高了其适用性。

发明内容

本发明的目的在于针对上述存在的问题和不足，提供一种基于路径点偏移的局部路径规划方法，在避障过程中，实时探测障碍物的横向距离来进行避障，以解决上述技术问题。

为实现上述目的，本发明采用以下技术方案：

一种基于路径点偏移的局部路径规划算法，包括如下步骤：

S1：获取期望路径信息、当前车辆位姿和障碍物信息；

S2：根据S1获取的各信息，进行初始期望路径点偏移绕过障碍物；

S3：根据S2的偏移点，计算偏移后的路径点航向角增量和方位角增量；

S4：车辆跟踪初始期望路径计算出的航向角和方位角，在此基础上加上S3中的航向角增量和方位角增量；

S5：将S4的航向角和方位角输出给控制层对车辆进行控制。

进一步的，S1中的障碍物包括静态障碍物和动态障碍物，关于动态障碍物位置的判断，需要根据运动预测来判断是否阻碍初始期望路径，若动态障碍物阻碍初始期望路径，则将动态障碍物预测到的位置作为当前静态障碍物。

进一步的，S2中的路径点偏移分为四个阶段：

第一阶段：车辆逐渐靠近障碍物过程，即avoid_length<x1；avoid_length为避障累计距离；x1为雷达检测到障碍物后，车辆与障碍物之间的距离；若avoid_length>x1则跳转到第二阶段；

第二阶段：车辆与障碍物之间的距离超过x1，但雷达仍能够检测到障碍物，即avoid_length>x1，并且雷达与障碍物之间的横向距离不等于0；当雷达与障碍物之间的横向距离等于0时，跳转到第三阶段；

第三阶段：若雷达传感器有盲区时，障碍物进入到雷达的盲区时，需要保持方向盘不变，避免没有绕过障碍物就开始转弯；若雷达传感器没有盲区则不需要此阶段，直接跳到第四阶段；

第四阶段：经历完第三阶段后，回到初始期望路径。

进一步的，避障过程中车辆与初始期望路径最远横向距离：avoid_width＝dis_ob+vehicle_width/2+Δl；式中dis_ob为障碍物边界与车辆初始期望路径的最小距离，vehicle_width为车辆宽度，Δl为阈值。

进一步的，相应于S2的四个阶段，S3中航向角增量angle_h和方位角增量angle_p计算也分为四个阶段：

第一阶段：车辆避障已经行驶距离为x处的横向偏移距离：height＝A*cos(k*x)-A，式中A＝avoid_width/2，k＝π/x1，当x＝x1时，height＝avoid_width。根据此函数就可以确定行驶距离为x处的斜率为：slope＝-A*k*sin(k*x)，此阶段angle_h和angle_p：

第二阶段：此阶段仍能够检测到障碍物，保持方向盘转角不变行驶，并将跳出此阶段的行驶距离存为model2_length，此阶段angle_h和angle_p：

第三阶段：障碍物在雷达盲区中，保持方向盘转角不变行驶，并将跳出此阶段的行驶距离存为model3_length，此阶段angle_h和angle_p：

第四阶段：x处的横向偏移距离：height＝A*cos(k*(x-model2_length)+π)-A，其中A＝avoid_width/2，k＝π/x1；根据此函数可以确定行驶距离为x处的斜率为：slope＝-A*k*sin(k*(x-model2_length)+π)；pre_dis为预瞄距离；此阶段angle_h和angle_p：

本发明与现有技术相比所具有的有益效果：

(1)本发明使用期望路径点偏移对障碍物进行局部路径规划，所设计方法在未知或不确定障碍物长度条件下，仍能很好的规划出避开障碍物的路径。在S2中考虑到雷达传感器有盲区阶段，也考虑到雷达传感器不能够检测障碍物长度，S3中对期望路径点偏移后，计算出车辆当前点的航向角增量和方位角增量，输出给控制层进行控制，最后完成对障碍物的绕行。

(2)本发明基于路径点偏移的局部路径规划，解决了局部路径规划过程中雷达传感器有盲区的问题，如毫米波雷达；也解决了雷达传感器不能确切检测到障碍物的实际大小的问题，如障碍物在车辆正前方，雷达检测不到或误检障碍物的长度。

(3)本专利的局部路径规划方法计算量少，而且该方法可以作为一个模块很好的嵌入自动驾驶车辆系统中，包括自动驾驶矿用车。

附图说明

图1是本发明提供的一种基于路径点偏移的局部路径规划方法的流程图。

图2是步骤S2中避障阶段的四个阶段示意图。

图3是步骤S2中避障过程中参数说明。

图4是步骤S3中角度示意图。

图5是路径点偏移的局部路径规划流程图。

具体实施方式

下面结合附图对本发明做进一步详细说明，以令本领域技术人员能够将本发明进行具体实施。

本发明实施例提供了一种基于路径点偏移的局部路径规划方法，如图1所示，包括如下步骤：

S1：获取期望路径信息、当前车辆位姿和障碍物信息；

S2：根据S1获取的各信息，进行初始期望路径点偏移绕过障碍物；

S3：根据S2的偏移点，计算偏移后的路径点航向角增量和方位角增量；

S4：车辆跟踪初始期望路径计算出的航向角和方位角，在此基础上加上S3中的航向角增量和方位角增量；

S5：将S4的航向角和方位角输出给控制层对车辆进行控制。

步骤S1中的障碍物包括静态障碍物和动态障碍物，关于动态障碍物位置的判断，需要根据运动预测来判断是否阻碍初始期望路径，若动态障碍物阻碍初始期望路径，则将动态障碍物预测到的位置作为当前静态障碍物。

步骤S2中的路径点偏移分为四个阶段，阶段划分根据初始期望路径关键点计算避障累计距离avoid_length和雷达传感器信息，其中初始期望路径关键点是当前车辆与期望路径上最近的点。阶段划分具体如下：

第一阶段：车辆逐渐靠近障碍物过程，即avoid_length<x1，x1为雷达检测到障碍物后，车辆与障碍物之间的距离；若avoid_length>x1则跳转到第二阶段。

第二阶段：车辆与障碍物之间的距离超过x1，但雷达仍能够检测到障碍物，即avoid_length>x1，并且雷达与障碍物之间的横向距离不等于0；当雷达与障碍物之间的横向距离等于0时，跳转到第三阶段。

第三阶段：若雷达传感器有盲区时，障碍物进入到雷达的盲区时，需要保持方向盘不变，避免没有绕过障碍物就开始转弯；若雷达传感器没有盲区则不需要此阶段，直接跳到第四阶段。

第四阶段：经历完第三阶段后，回到初始期望路径。

综上，四个阶段可表示为如图2所示，避障过程中车辆与初始期望路径最远横向距离avoid_width。

需要说明的是，雷达是否有盲区需要根据传感器设备实际情况确定，避障过程中车辆与初始期望路径最远横向距离：avoid_width＝dis_ob+vehicle_width/2+Δl；式中dis_ob为障碍物边界与车辆初始期望路径的最小距离，vehicle_width为车辆宽度，Δl为阈值，各参数表达可参照图3。

根据上述四个阶段，S3中航向角增量angle_h和方位角增量angle_p计算也分为四个阶段，其中angle_h和angle_p的角度示意如图4所示，angle_h和angle_p计算具体如下：

第一阶段：车辆避障已经行驶距离为x(包含预瞄距离pre_dis)处的横向偏移距离：height＝A*cos(k*x)-A，式中A＝avoid_width/2，k＝π/x1，当x＝x1时，height＝avoid_width。根据此函数就可以确定行驶距离为x处的斜率为：slope＝-A*k*sin(k*x)，此阶段angle_h和angle_p：

第二阶段：此阶段仍能够检测到障碍物，保持方向盘转角不变行驶，并将跳出此阶段的行驶距离存为model2_length，此阶段angle_h和angle_p：

第三阶段：障碍物在雷达盲区中，保持方向盘转角不变行驶，并将跳出此阶段的行驶距离存为model3_length，此阶段angle_h和angle_p：

第四阶段：x处的横向偏移距离：height＝A*cos(k*(x-model2_length)+π)-A，其中A＝avoid_width/2，k＝π/x1。根据此函数就可以确定行驶距离为x处的斜率为：slope＝-A*k*sin(k*(x-model2_length)+π)；pre_dis为预瞄距离；此阶段angle_h和angle_p：

需要说明，车辆系统每个采样周期的预瞄距离不同，预瞄距离pre_dis的获得，可以采用公式：k*v，k为比例系数，v为当前车速。

综上所述，关于S2和S3的关系整理为如图5所示的路径点偏移的局部路径规划流程图。

Claims (2)

1.一种基于路径点偏移的局部路径规划方法，其特征在于，包括如下步骤：

S1：获取期望路径信息、当前车辆位姿和障碍物信息；

S2：根据S1获取的各信息，进行初始期望路径点偏移绕过障碍物；

S3：根据S2的偏移点，计算偏移后的路径点航向角增量和方位角增量；

S4：车辆跟踪初始期望路径计算出的航向角和方位角，在此基础上加上S3中的航向角增量和方位角增量；

S5：将S4的航向角和方位角输出给控制层对车辆进行控制；

所述S1中的障碍物包括静态障碍物和动态障碍物，关于动态障碍物位置的判断，需要根据运动预测来判断是否阻碍初始期望路径，若动态障碍物阻碍初始期望路径，则将动态障碍物预测到的位置作为当前静态障碍物；

所述S2中的路径点偏移分为四个阶段：

第一阶段：车辆逐渐靠近障碍物过程，即avoid_length<x1；avoid_length为避障累计距离；x1为雷达检测到障碍物后，车辆与障碍物之间的距离；若avoid_length>x1则跳转到第二阶段；

第二阶段：车辆与障碍物之间的距离超过x1，但雷达仍能够检测到障碍物，即avoid_length>x1，并且雷达与障碍物之间的横向距离不等于0；当雷达与障碍物之间的横向距离等于0时，跳转到第三阶段；

第三阶段：若雷达传感器有盲区时，障碍物进入到雷达的盲区时，需要保持方向盘不变，避免没有绕过障碍物就开始转弯；若雷达传感器没有盲区则不需要此阶段，直接跳到第四阶段；

第四阶段：经历完第三阶段后，回到初始期望路径；

相应于所述S2的四个阶段，所述S3中航向角增量angle_h和方位角增量angle_p计算也分为四个阶段：

第一阶段：车辆避障已经行驶距离为x处的横向偏移距离：height＝A*cos(k*x)-A，式中A＝avoid_width/2，k＝π/x1，avoid_width表示避障过程中车辆与初始期望路径最远横向距离；当x＝x1时，height＝avoid_width；根据此函数就可以确定行驶距离为x处的斜率为：slope＝-A*k*sin(k*x)，pre_dis为预瞄距离，此阶段angle_h和angle_p：

第二阶段：此阶段仍能够检测到障碍物，保持方向盘转角不变行驶，并将跳出此阶段的行驶距离存为model 2_length，此阶段angle_h和angle_p：

第三阶段：障碍物在雷达盲区中，保持方向盘转角不变行驶，并将跳出此阶段的行驶距离存为model 3_length，此阶段angle_h和angle_p：

第四阶段：x处的横向偏移距离：height＝A*cos(k*(x-model 2_length)+π)-A，其中A＝avoid_width/2，k＝π/x1；根据此函数可以确定行驶距离为x处的斜率为：slope＝-A*k*sin(k*(x-model 2_length)+π)；pre_dis为预瞄距离；此阶段angle_h和angle_p：

2.如权利要求1所述的一种基于路径点偏移的局部路径规划方法，其特征在于，避障过程中车辆与初始期望路径最远横向距离：avoid_width＝dis_ob+vehicle_width/2+Δl；式中dis_ob为障碍物边界与车辆初始期望路径的最小距离，vehicle_width为车辆宽度，Δl为阈值。