一种运动障碍物的三元素自主避障方法

摘要

本发明公开了一种运动障碍物的三元素自主避障方法，包括以下步骤，所述(1)首先通过GNSS设备采集期望路径的waypoint点，并在期望路径上构建Frenet坐标系；(2)通过Frenet坐标系在局部环境下划分自适应栅格点，有利于确定自车在局部环境下的具体位置，然后引入速度障碍物的概念，障碍物的姿态信息通过激光雷达测出；(3)通过对障碍物进行圆形近似提出三元素的动态避障方法。本发明通过三元素原理结合自适应栅格法和MPC安全高效的规划出无碰撞安全路径，为局部动态避障提供了一种全新的方法，具有一定的应用前景，并且大大减少了计算量，仿真结果表明该算法能成功的避开动态障碍物，在自车较低速度时，算法能满足平顺性、安全性、实时性和道路交通规则的要求。

说明书

技术领域

本发明涉及汽车自动驾驶技术领域，具体为一种运动障碍物的三元素自主避障方法。

背景技术

自动驾驶技术既能有效解决交通安全问题，还能缓解交通拥堵、提高交通效率，给人们生活带来便利，自动驾驶汽车是通过车载传感器检测和识别环境信息并进行分析判断，并利用算法控制车辆自主运动，构成自动驾驶的系统一般包括环境感知、决策规划、控制执行等，路径规划是自动驾驶的关键技术之一，自动驾驶汽车要在道路环境中对可行使路线进行规划，实现规划路径的安全、平顺及高效，必须有高效的规划技术作保障，路径规划常分为全局路径规划与局部路径规划，全局路径规划是空间上的位置信息，是一条从起始点到终点的路径，但局部路径规划是一种二维的规划，依托车载传感器对局部环境信息的感知，还需要考虑时间和局部避障问题，目前常用的避障算法有图形搜索法、采样法、智能仿生法、人工势场法等，每一种单一方法对于速度较低的机器人避障有一定的效果，但这些算法都存在搜索范围广、计算量大、对动态障碍物避障不明显以及陷入局部极小值等情况，若在自动驾驶车辆上应用需要对其进行整合，对于动态障碍物避障问题，在有运动障碍物的道路环境中进行路径规划是比较复杂的，需要考虑到多种情况，涉及到空间上的路径规划以及时间域上的速度规划，为了使自动驾驶车辆在局部环境下安全高效的避开动态障碍物，必须开发一种新的算法来应对这种问题。

现有技术中运动障碍物自主避障方法存在的缺陷是：

1、专利文件CN113985896A公开一种自动驾驶的带避障的路径规划方法，包括“采集工作地块地理信息，获得多边形顺序坐标点，以规定安全边界距离将多边形向内部缩进，以当前工作车辆位置为参考，以与多边形顶点距离最近的为下一个目标点，以此点相邻较长边，生成路径待定点并生成路径；遇到障碍物通过平移待定点绕过障碍物，并能通过比较左右平移距离获得较优路径，通过平移路径待定点生成下一组路径，直至路径能够遍历整个工作地块”，但是该方法在使用时，需要遍历整个工作地块，计算量偏大。

发明内容

本发明的目的在于提供一种运动障碍物的三元素自主避障方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：一种运动障碍物的三元素自主避障方法，包括以下步骤，所述(1)首先通过GNSS设备采集期望路径的waypoint点，并在期望路径上构建Frenet坐标系；(2)通过Frenet坐标系在局部环境下划分自适应栅格点，有利于确定自车在局部环境下的具体位置，然后引入速度障碍物的概念，障碍物的姿态信息通过激光雷达测出； (3)通过对障碍物进行圆形近似提出三元素的动态避障方法；(4)接着构建各种约束的代价函数，根据权重值确定最终规划的路径点[1]；(5)最后进行仿真验证；

优选的，所述步骤(1)局部栅格的划分，进行局部避障时先对局部道路环境进行栅格划分，首先引入Frenet坐标系，如附图12所示，附图12是车辆在大地坐标系与Frenet坐标系下的位置，该图形象描述了自车在两个坐标系之间的关系，自车在大地坐标系下的坐标是K(x

shange_a＝-3.75:0.25:3.75

Shange_a和shange_b分别是Frenet坐标系中的S、L，但是S值需要加上自车当前位置的路径长度s(n_out),R_lj是道路曲率半径；道路曲率半径可以通过三点法计算，自车在期望路径上的投影点为中间点，选取此投影点前后各5m的位置有两个点，通过这三个点就可以确定道路曲率半径；

计算公式如下，已知三个点的坐标分别是,圆的方程一般可以写成二次多项式：

A(x

将上式进行变形得到圆的标准方程：

再将三个坐标点带入到A(x

在三点不共线的情况下，齐次方程组有非零的解，也就是系数矩阵不满秩，此时，

根据自适应栅格划分原则，在道路上进行栅格点的划分，附图3是自适应栅格画法，在道路曲率较小时，划分的前向距离较大，当经过大区率弯道时划分的前向距离较小。

优选的，所述步骤(2)建立三元素避障模型如附图4所示，附图4中自车为A所在的位置，障碍车为B所在位置，自车速度为V

当f

式中k

优选的，所述步骤(3)设计基于MPC的代价函数，为了从空间中解决自动驾驶车辆最优行驶路线，需要重新定义一个评价路径优劣的准则，也就是代价函数。在自动驾驶中，路径决策的代价函数主要体现在行车的安全性、舒适性、高效性上；其中安全性是最重要的一个环节，它反映了车辆与障碍物之间的潜在危险，这是顺利通行的保证；舒适性体现在车辆横纵向速度的变化幅度，以及连续两个时间段的前轮转角的变化情况；高效性体现在车辆通行效率上，代价函数的设计如下所示：

1)左侧优先变道代价函数的设计

根据道路法和行车准则，中国道路上的车辆都是靠右行驶，当自车需要超车时，一般情况下需要向左变道，于是设计如下的权重函数：

k＝0,1…N-1，k

2)逐次变道代价函数

由于上面的约束条件引入了不是连续变化的布尔变量作为权重函数，此种约束条件会导致最后的目标函数为非凸的情况，非凸的约束存在局部最优解不是全局最优解现象，需要对此进行如下的变形：

上式中k

3)侧向加速度代价函数

其中k

4)方向盘转角变化率代价函数

在自动驾驶汽车局部路径规划时，为了使跟踪的轨迹平滑，需要考虑方向盘转角变化率，防止程序在计算时，出现方向盘转角变化率过大导致车辆方向盘剧烈抖动失控的现象。仿真车方向盘与前轮转角的传动比为15.8。得到如下公式：

f

其中k

5)基于几何三元素的威胁度权重函数

根据推导的基于几何三元素的动态避障方法，得到动态避障的理论公式：

此代价函数可以解决多运动障碍物的避障情况，自车与障碍车越靠近时对应的栅格点的权重越大，所付出的代价越大；

6)车道对中代价函数

车道对中代价函数也叫路径偏移代价函数。在车辆正常行驶时，需要符合行驶规则，自车应该行驶在每个车道的最中间，所以在道路中间对应的点的权重应该小，而道路边缘的点所对应的权重应该较大；

7)安全度的代价函数

安全度是衡量有障碍物情况下的一个权重函数，与几何三元素的威胁度类似，需要提前知道自车与障碍车的速度，通过评估车辆的制动性能与制动距离，得到安全车距公式：

f

V

上式是安全度代价函数公式，此代价函数只用在存在一个障碍车辆下；

8)前视点权重函数

将前视点权重函数加入到整个栅格点选取的评价中，越靠近此前视点的栅格点，代价越小，得到如下公式：

(x

最终得到总的目标代价函数公式，该代价函数的设计都是基于Frenet坐标系，这样有效减少了无效栅格点的搜索时间，提高了算法的实时性，同时能准确定位自车在局部环境的具体位置，可以有效的进行路径跟踪；

f

得到总的权重函数之后通过min函数求出每个循环时刻权重最小的栅格点：

为了安全性加上硬约束：前轮极限转角约束，道路边界约束[2]：

-38°≤Steer≤38°

其中Steer是前轮转角，d是自车的宽度，

优选的，所述步骤(4)动态避障仿真实验：为了验证三元素方法的动态避障效果，进行了联合仿真；采用Uturn路线，同时在道路上设置一个运动障碍物，从距离起点25米处逆向以10km/h的速度往起点运动，自车从起点向终点按照曲率规划的速度进行运动；附图6是自车的行驶路线图，在程序里面设置了障碍物的运动轨迹之后，自车会根据三元素的避障算法，进行转向避障处理，在避开障碍物之后自车会迅速跟踪上期望路径；附图7是避障时车辆的航向角变化曲线，根据图示，5s之后，由于自车进行转向操作，所以航向角会有一个小范围的变化，如附图7中的第一个小的波峰所示；后面的一个凸字形波峰是过S弯的时自车两个极限转角对应的航向角变化曲线；自车先进行极限的左转，之后又进行右转，最后航向角回到0°；附图8是自车方向盘转角变化图，一开始自车就进行了避障处理，所以一开始自车的转角就从0°变化到120°左右，直线行驶又变为0°左右；然后在行驶了5s后进入第一个极限弯道，方向盘最高转到+310°，又经过15s的直线运动，进入第二个弯道，此时方向盘转角最大为－295°，最后进入直道方向盘转角又回到0°；将自车航向角与方向盘转角随时间的变化曲线画在一起就是附图9。附图10是整个程序的循环耗时，最高耗时为25.5ms最低9.5ms，总体的平均耗时13.5ms；附图11为仿真时自车跟踪规划的速度，最高规划速度为15km/h。

与现有技术相比，本发明的有益效果是：

1、本发明通过使用方法结合自适应栅格法和MPC原理安全高效的规划出无碰撞安全路径，为局部动态避障提供了一种全新的方法，具有一定的应用前景，并且大大减少了计算量，仿真结果表明该算法能成功的避开动态障碍物，在自车较低速度时，算法能满足平顺性、安全性、实时性和道路交通规则的要求。

附图说明

图1为本发明的整体步骤示意图；

图2为本发明的基于Frenet坐标系的栅格法示意图；

图3为本发明的自适应栅格法示意图；

图4为本发明的避障结合模型示意图；

图5为本发明的优化避障几何模型示意图；

图6为本发明的自车的行驶路线图示意图；

图7为本发明的避障时车辆的航向角变化曲线示意图；

图8为本发明的自车方向盘转角变化示意图；

图9为本发明的自车航向角与方向盘转角随时间的变化曲线示意图；

图10为本发明的整个程序的循环耗时示意图；

图11为本发明的仿真时自车跟踪规划的速度示意图；

图12为本发明的Frenet坐标系示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

请参阅图1、图2、图3和图12，本发明提供一种实施例：一种运动障碍物的三元素自主避障方法；

实施例一：

包括以下步骤，(1)首先通过GNSS设备采集期望路径的waypoint点，并在期望路径上构建Frenet坐标系。

请参阅图4和图5所示，一种运动障碍物的三元素自主避障方法；

实施例二：(2)通过Frenet坐标系在局部环境下划分自适应栅格点，有利于确定自车在局部环境下的具体位置，然后引入速度障碍物的概念，障碍物的姿态信息通过激光雷达测出，首先引入Frenet坐标系，如附图12所示，附图12是车辆在大地坐标系与Frenet坐标系下的位置，该图形象描述了自车在两个坐标系之间的关系，自车在大地坐标系下的坐标是K(x

shange_a＝-3.75:0.25:3.75

Shange_a和shange_b分别是Frenet坐标系中的S、L，但是S值需要加上自车当前位置的路径长度s(n_out),R_lj是道路曲率半径；道路曲率半径可以通过三点法计算，自车在期望路径上的投影点为中间点，选取此投影点前后各5m的位置有两个点，通过这三个点就可以确定道路曲率半径；

计算公式如下，已知三个点的坐标分别是,圆的方程一般可以写成二次多项式：

A(x

将上式进行变形得到圆的标准方程：

再将三个坐标点带入到A(x

在三点不共线的情况下，齐次方程组有非零的解，也就是系数矩阵不满秩，此时，

根据自适应栅格划分原则，在道路上进行栅格点的划分，附图3是自适应栅格画法，在道路曲率较小时，划分的前向距离较大，当经过大区率弯道时划分的前向距离较小。

请参阅图1、图4和图5所示，一种运动障碍物的三元素自主避障方法；

实施例三：(3)通过对障碍物进行圆形近似提出三元素的动态避障方法，附图4中自车为A所在的位置，障碍车为B所在位置，自车速度为V

当f

式中k

请参阅图1所示，一种运动障碍物的三元素自主避障方法；

实施例四：(4)接着构建各种约束的代价函数，根据权重值确定最终规划的路径点[1]，设计基于MPC的代价函数，为了从空间中解决自动驾驶车辆最优行驶路线，需要重新定义一个评价路径优劣的准则，也就是代价函数；在自动驾驶中，路径决策的代价函数主要体现在行车的安全性、舒适性、高效性上；其中安全性是最重要的一个环节，它反映了车辆与障碍物之间的潜在危险，这是顺利通行的保证；舒适性体现在车辆横纵向速度的变化幅度，以及连续两个时间段的前轮转角的变化情况；高效性体现在车辆通行效率上，代价函数的设计如下所示：

1)左侧优先变道代价函数的设计

根据道路法和行车准则，中国道路上的车辆是靠右行驶，当自车需要超车时，一般情况下需要向左变道，于是设计如下的权重函数：

k＝0,1…N-1，k

2)逐次变道代价函数

由于上面的约束条件引入了不是连续变化的布尔变量作为权重函数，此种约束条件会导致最后的目标函数为非凸的情况，非凸的约束存在局部最优解不是全局最优解现象，需要对此进行如下的变形：

上式中k

3)侧向加速度代价函数

其中k

4)方向盘转角变化率代价函数

在自动驾驶汽车局部路径规划时，为了使跟踪的轨迹平滑，需要考虑方向盘转角变化率，防止程序在计算时，出现方向盘转角变化率过大导致车辆方向盘剧烈抖动失控的现象。仿真车方向盘与前轮转角的传动比为15.8。得到如下公式：

f

其中k

5)基于几何三元素的威胁度权重函数

根据推导的基于几何三元素的动态避障方法，得到动态避障的理论公式：

此代价函数可以解决多运动障碍物的避障情况，自车与障碍车越靠近时对应的栅格点的权重越大，所付出的代价越大。

6)车道对中代价函数

车道对中代价函数也叫路径偏移代价函数；在车辆正常行驶时，需要符合行驶规则，自车应该行驶在每个车道的最中间，所以在道路中间对应的点的权重应该小，而道路边缘的点所对应的权重应该较大。

7)安全度的代价函数

安全度是衡量有障碍物情况下的一个权重函数，与几何三元素的威胁度类似，需要提前知道自车与障碍车的速度，通过评估车辆的制动性能与制动距离，得到安全车距公式：

f

V

上式是安全度代价函数公式，此代价函数只用在存在一个障碍车辆下。

8)前视点权重函数

将前视点权重函数加入到整个栅格点选取的评价中，越靠近此前视点的栅格点，代价越小，得到如下公式：

(x

最终得到总的目标代价函数公式，该代价函数的设计都是基于Frenet坐标系，这样有效减少了无效栅格点的搜索时间，提高了算法的实时性，同时能准确定位自车在局部环境的具体位置，可以有效的进行路径跟踪。

f

得到总的权重函数之后通过min函数求出每个循环时刻权重最小的栅格点：

为了安全性加上硬约束：前轮极限转角约束，道路边界约束[2]：

-38°≤Steer≤38°

其中Steer是前轮转角，d是自车的宽度，

请参阅图1、图6-11所示，一种运动障碍物的三元素自主避障方法；

实施例四：(5)最后进行仿真验证，为了验证三元素方法的动态避障效果，进行了联合仿真。采用Uturn路线，同时在道路上设置一个运动障碍物，从距离起点25米处逆向以10km/h的速度往起点运动，自车从起点向终点按照曲率规划的速度进行运动。附图6是自车的行驶路线图，在程序里面设置了障碍物的运动轨迹之后，自车会根据三元素的避障算法，进行转向避障处理，在避开障碍物之后自车会迅速跟踪上期望路径。附图7是避障时车辆的航向角变化曲线，根据图示，5s之后，由于自车进行转向操作，所以航向角会有一个小范围的变化，如附图7中的第一个小的波峰所示。后面的一个凸字形波峰是过S弯的时自车两个极限转角对应的航向角变化曲线。自车先进行极限的左转，之后又进行右转，最后航向角回到0°。附图8是自车方向盘转角变化图，一开始自车就进行了避障处理，所以一开始自车的转角就从0°变化到120°左右，直线行驶又变为0°左右。然后在行驶了5s后进入第一个极限弯道，方向盘最高转到+310°，又经过15s的直线运动，进入第二个弯道，此时方向盘转角最大为－295°，最后进入直道方向盘转角又回到0°。将自车航向角与方向盘转角随时间的变化曲线画在一起就是附图9。附图10 是整个程序的循环耗时，最高耗时为25.5ms最低9.5ms，总体的平均耗时 13.5ms。附图11为仿真时自车跟踪规划的速度，最高规划速度为15km/h。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内，不应将权利要求中的任何附图标记视为限制所涉及的权利要求。