基于控制点拟合多项式自动驾驶避障路径规划系统及方法

摘要

本发明公开了一种基于控制点拟合多项式自动驾驶避障路径规划系统及方法，该方法包括S1、根据当前车辆位置，选取全局路径模块规划输出的路径作为参考基准线，获取障碍物信息；S2、根据参考基准线和障碍物信息，计算障碍物边沿点到参考基准线的垂直距离集合Lat；S3、基于当前车辆位置与障碍物边沿点计算圆弧半径Rot，判断左侧避障还是右侧避障，并分别计算1号控制点和2号控制点坐标；S4、若1号控制点纵向距离大于2号控制点纵向距离时，执行步骤S5；S5、基于当前车辆位置和2号控制点拟合的三次多项式作为避障路径。本发明对算例要求低，抗干扰能力强，能够在复杂路况下实现自动驾驶避障。

说明书

技术领域

本发明涉及车辆控制技术领域，具体涉及一种基于控制点拟合多项式自动驾驶避障路径规划系统及方法。

背景技术

随着人工智能技术的发展和车辆工业的不断进步，车辆的自动驾驶技术也是愈发成熟。车辆在自动驾驶过程中，可能会遇到各种复杂路况，在复杂路况实现对障碍物的避障是局部路径规划的核心算法，是自动驾驶走向商业化应用必须解决的关键技术之一。

针对机器人避障的局部路径规划有很多种算法，大体可以分为基于图搜索、基于采样、基于插值曲线与优化等方法，但是这些算法应用到车辆自动驾驶时，要充分考虑车辆动力学模型的要求进行改进，如A星算法在自动驾驶时需要改进为混合A星才适用。同时，在自动驾驶工程化过程中，除了车辆动力学约束，还需要考虑当前控制器运算能力、传感器感知水平、用户最终对规划路径导致方向盘抖动接受范围等约束。

如公开号为CN112577506A的发明专利公开了一种自动驾驶局部路径规划方法，其包括步骤：(1)接收车辆将要行驶区域的地图数据和车辆定位数据，以生成车辆可行驶区域的局部地图；(2)从所述局部地图中进行时间采样、速度采样和角度采样；(3)基于时间采样、速度采样和角度采样生成若干条预测行驶轨迹；(4)遍历所有预测行驶轨迹，对每一条预测行驶轨迹进行碰撞检测，如果检测结果是会发生碰撞，则将该条预测行驶轨迹剔除，如果检测结果是不会发生碰撞，则将该条预测行驶轨迹放入预测行驶轨迹数据库中；(5)从所述预测行驶轨迹数据库中选取最优轨迹，并将其输出给车辆。该专利基于采样点生成多条路径，然后对多条路径进优化，最终形成一条可执行的规划路径。此种算法对控制器的算例要求高，抗干扰能力不强。

因此，有必要开发一种对算例要求低，抗干扰能力强的避障路径规划系统，以在复杂路况下实现避障。

发明内容

针对上述现有技术的不足，本发明所要解决的技术问题是：如何提供一种对算例要求低，抗干扰能力强，在复杂路况下实现避障的基于控制点拟合多项式自动驾驶避障路径规划系统及方法。

为了解决上述技术问题，本发明采用了如下的技术方案：

基于控制点拟合多项式自动驾驶避障路径规划系统，包括传感器模块、参考基准线模块，局部路径规划模块和整车执行机构，参考基准线模块与局部路径规划模块之间、传感器模块与局部路径规划模块之间、局部路径规划模块与整车执行机构之间均为电连接，参考基准线模块分别电连接有定位模块和全局路径模块；

传感器模块，用于获取障碍物信息；

定位模块，用于获取当前车辆位置以及航向信息；

全局路径模块，用于规划一条连接起点到终点的路径；

参考基准线模块，用于根据当前车辆位置，匹配路径上的点作为参考基准线；

局部路径规划模块，用于根据参考基准线和障碍物信息进行避障路径规划，并输出避障路径至整车执行机构，由整车执行机构控制自动驾驶车辆沿避障路径行驶。

进一步地，在局部路径规划模块与整车执行机构之间还电连接有横纵向控制模块，横纵向控制模块，用于对避路径进行横向、纵向控制，以确保车辆在一定误差范围内沿避障路径行驶。

进一步地，传感器模块包括视觉传感器、激光雷达、毫米波雷达、超声波雷达中的一种或多种。

进一步地，整车执行机构包括EMS、ESP和EPS，其中EPS用于接收横向控制角度请求信息，EMS用于接收纵向控制扭矩请求信息，ESP用于接收纵向控制减速度请求信息。

本发明还提供一种基于控制点拟合多项式自动驾驶避障路径规划方法，本方法使用上述的基于控制点拟合多项式自动驾驶避障路径规划系统，具体包括以下步骤：

S1、获取当前车辆位置，根据当前车辆位置，选取全局路径模块规划输出的路径作为参考基准线，获取障碍物信息；

S2、根据参考基准线和障碍物信息，计算障碍物边沿点到参考基准线的垂直距离集合Lat，当集合Lat的绝对值均大于MinDis时，则证明参考基准线与障碍物没有碰撞风险，直接将参考基准线作为局部路径输出到横纵向控制模块实现自动驾驶，否则执行步骤S3；

S3、基于当前车辆位置与障碍物边沿点计算圆弧半径Rot，判断左侧避障还是右侧避障，并分别计算1号控制点和2号控制点坐标；

S4、若1号控制点纵向距离大于2号控制点纵向距离时，执行步骤S5，否则执行步骤S6；

S5、基于当前车辆位置和2号控制点拟合的三次多项式作为避障路径；

S6、判断1号控制点在三次多项式的内侧还是外侧，若1号控制点在三次多项式的内侧则执行步骤S5，否则执行步骤S7；

S7、基于当前车辆位置、1号控制点和2号控制点拟合四次多项式作为避障路径。

进一步地，在步骤S3中，若圆弧半径Rot大于0时，则把当前计算的圆弧半径Rot归类为右侧避障半径集合RtRot，计算最大右侧避障半径maxRtRot及其对应的右侧横向位移TarLatRt；

若圆弧半径Rot小于0时，则把当前计算的圆弧半径Rot归类为左侧避障半径集合RtRot，计算最大左侧避障半径maxRtRot及其对应的左侧横向位移TarLatRt；

结合通行区域信息、避障半径和横向位移，判断是左侧避障还是右侧避障，相同条件下，左侧避障优先执行。

进一步地，计算1号控制点坐标：

PntOneX＝max(6，TarMinLon-max(0.5，0.8×Hv_Spd_mps))

PntOneY＝PntTwoY

计算2号控制点坐标：

PntTwoX＝Max(25，Hv_Spd_mps×4)

当左侧避障时PntTwoY＝max(LnWidth，TarLatLf)，当右侧避障时PntTwoY＝max(LnWidth，TarLatRt)；

其中，Hv_Spd_mps为车速；TarMinLon为障碍物纵向距离；LnWidth为车道宽度；TarLatLf为左侧避障距离；TarLatRt为右侧避障距离。

进一步地，在步骤S5中，基于当前车辆位置和2号控制点拟合的三次多项式作为避障路径，其中三次多项式为：

B＝[LatErr-HeadErrRad PntTwoY 0]

三次多项式系数Coef＝A\B，其中，LatErr为相对参考基准线横向位置误差；HeadErrRad为参考基准线航向误差。

进一步地，在步骤S7中，基于当前车辆位置、1号控制点和2号控制点拟合四次多项式作为避障路径，其中四次多项式为：

D＝[LatErr-HeadErrRad PntTwoY 0]

四次多项式系数Coef＝C\D，其中，LatErr为相对参考基准线横向位置误差；HeadErrRad为参考基准线航向误差。

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

1、本发明基于当前车辆位置与障碍物边沿点计算圆弧半径Rot，判断左侧避障还是右侧避障，并分别计算1号控制点和2号控制点坐标，以当前车辆位置和2号控制点拟合的三次多项式作为避障路径和当前车辆位置、1号控制点和2号控制点拟合四次多项式作为避障路径。在实现避障过程中，不再基于格栅图搜索，也不再是生成大量可执行路径，再选取最优路径，而是直接寻找控制点，这样可以大量节省运行资源，对控制器的算例要求低。同时，通过针对已生成的单一路径进行保持或者滤波等，操作更加容易，可以增强规划轨迹对障碍物波动的抗干扰能力，能够在复杂路况下实现避障。

附图说明

为了使发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步的详细描述，其中：

图1为本发明基于控制点拟合多项式自动驾驶避障路径规划系统的结构示意图；

图2为本发明基于控制点拟合多项式自动驾驶避障路径规划方法的逻辑流程图；

图3为本发明避障示意图。

具体实施方式

下面结合附图对本发明作进一步的详细说明。

如图1所示，本实施例中提供一种基于控制点拟合多项式自动驾驶避障路径规划系统，包括传感器模块、参考基准线模块，局部路径规划模块和整车执行机构，参考基准线模块与局部路径规划模块之间、传感器模块与局部路径规划模块之间、局部路径规划模块与整车执行机构之间均为电连接，参考基准线模块分别电连接有定位模块和全局路径模块；

传感器模块，用于获取障碍物信息，包括速度、轮廓等信息；

定位模块，用于获取当前车辆位置以及航向信息；

全局路径模块，用于根据起点、终点和地图信息，规划一条连接起点到终点的路径；

参考基准线模块，用于根据当前车辆位置，匹配路径上的点，选取从当前车辆位置前行方向固定距离的路径点，作为参考基准线；

局部路径规划模块，用于根据参考基准线和障碍物信息进行避障路径规划，并输出避障路径至整车执行机构，由整车执行机构控制自动驾驶车辆沿避障路径行驶。

具体实施时，在局部路径规划模块与整车执行机构之间还电连接有横纵向控制模块，横纵向控制模块，用于对避路径进行横向、纵向控制，以确保车辆在一定误差范围内沿避障路径行驶。

具体实施时，传感器模块包括视觉传感器、激光雷达、毫米波雷达、超声波雷达中的一种或多种。

具体实施时，整车执行机构包括EMS、ESP和EPS，其中EPS用于接收横向控制角度请求信息，EMS用于接收纵向控制扭矩请求信息，ESP用于接收纵向控制减速度请求信息。

本发明，参考基准线模块，根据定位模块提供的当前车辆位置，匹配全局路径模块规划的全局路径上的点，从而选取从当前车辆位置前行方向固定距离的路径点，作为参考基准线。局部路径规划模块根据参考基准线模块提供的参考基准线和传感器模块提供的障碍物信息(感知信息)进行避障路径规划，输出避障路径给横纵向控制模块，再通整车执行机构，实现对整车的控制，从而实现自动驾驶车辆避障。

如图2所示，本实施例中提供一种基于控制点拟合多项式自动驾驶避障路径规划方法，包括以下步骤：

S1、获取当前车辆位置，根据当前车辆位置，选取全局路径模块规划输出的路径作为参考基准线，获取障碍物信息；

S2、根据参考基准线和障碍物信息，计算障碍物边沿点到参考基准线的垂直距离集合Lat，当集合Lat的绝对值均大于MinDis时，则证明参考基准线与障碍物没有碰撞风险，直接将参考基准线作为局部路径输出到横纵向控制模块实现自动驾驶，否则执行步骤S3；

S3、基于当前车辆位置与障碍物边沿点计算圆弧半径Rot，判断左侧避障还是右侧避障，并分别计算1号控制点和2号控制点坐标；

S4、若1号控制点纵向距离大于2号控制点纵向距离时，执行步骤S5，否则执行步骤S6；

S5、基于当前车辆位置和2号控制点拟合的三次多项式作为避障路径，输出给横向控制模块实现自动驾驶避障路径规划。

S6、判断1号控制点在三次多项式的内侧还是外侧，内外侧具体位置如图3所示。若1号控制点在三次多项式的内侧则执行步骤S5，否则执行步骤S7；

S7、基于当前车辆位置、1号控制点和2号控制点拟合四次多项式作为避障路径。

最后，对生成的轨迹进行滤波、保持、有效性检查等工作，最终输出局部路径规划轨迹。

本发明，基于当前车辆位置与障碍物边沿点计算圆弧半径Rot，判断左侧避障还是右侧避障，并分别计算1号控制点和2号控制点坐标，以当前车辆位置和2号控制点拟合的三次多项式作为避障路径和当前车辆位置、1号控制点和2号控制点拟合四次多项式作为避障路径。在实现避障过程中，不再基于格栅图搜索，也不再是生成大量可执行路径，再选取最优路径，而是直接寻找控制点，这样可以大量节省运行资源，对控制器的算例要求低。同时，通过针对已生成的单一路径进行保持或者滤波等，操作更加容易，可以增强规划轨迹对障碍物波动的抗干扰能力，能够在复杂路况下实现避障。

具体实施时，在步骤S3中，若圆弧半径Rot大于0时，则把当前计算的圆弧半径Rot归类为右侧避障半径集合RtRot，计算最大右侧避障半径maxRtRot及其对应的右侧横向位移TarLatRt；

若圆弧半径Rot小于0时，则把当前计算的圆弧半径Rot归类为左侧避障半径集合RtRot，计算最大左侧避障半径maxRtRot及其对应的左侧横向位移TarLatRt。

避障半径计算：

设A点为本车定位位置，B点为障碍物边沿点，X为B点相对于A点的横向位置，Y为B相对于A的纵向距离，O为基于A、B两点的圆心，R为基于A、B两点的半径，则避障半径R为：

结合通行区域信息(当前车辆位置、车道信息)、避障半径、横向位移，判断是左侧避障还是右侧避障，相同条件下，左侧避障优先。计算避障的目标位置点，即2号控制点，2号控制点具体位置如图3所示，其横向位置为TarLatRt(右侧避障)或者TarLatLf(左侧避障)，纵向距离为当前选取的障碍物横向位置点，匹配到参考基准线上的点，此参考基准线上的点到本车的距离。1号控制点的横向位置值与2号控制点横向位置一致，纵向位置为一个与当前车速相关的函数。1号控制点坐标为：

PntOneX＝max(6，TarMinLon-max(0.5，0.8×Hv_Spd_mps))

PntOneY＝PntTwoY

2号控制点坐标为：

PntTwoX＝Max(25，Hv_Spd_mps×4)

当左侧避障时PntTwoY＝max(LnWidth，TarLatLf)，当右侧避障时PntTwoY＝max(LnWidth，TarLatRt)；

其中，Hv_Spd_mps为车速；TarMinLon为障碍物纵向距离；LnWidth为车道宽度；TarLatLf为左侧避障距离；TarLatRt为右侧避障距离。

具体实施时，在步骤S5中，三次多项式为：

B＝[LatErr-HeadErrRad PntTwoY 0]

三次多项式系数Coef＝A\B，其中，LatErr为相对参考基准线横向位置误差；HeadErrRad为参考基准线航向误差。

具体实施时，在步骤S7中，四次多项式为：

D＝[LatErr-HeadErrRad PntTwoY 0]

四次多项式系数Coef＝C\D，其中，LatErr为相对参考基准线横向位置误差；HeadErrRad为参考基准线航向误差。

参见图2和图3，本发明工作流程：

根据定位系统获取的当前车辆位置(本车)，匹配路径上的点，选取从当前车辆位置前行方向固定距离的路径点，作为参考基准线；根据参考基准线和获取的障碍物信息(图3中的目标)，计算该障碍物四个边沿点到参考基准线的垂直距离集合Lat，并判断集合Lat的绝对值是否均大于MinDis(绕障横向距离阈值)。

当集合Lat的绝对值均大于MinDis时，则表明参考基准线模块输出的参考基准线，对当前车辆行驶无碰撞风险(不需要进行避障路径规划)，通过局部路径规划模块直接将该参考基准线作为路径规划轨迹并输出到横纵向控制模块，横纵向控制模块对该轨迹进行横向、纵向控制，确保车辆在一定误差范围内跟随该轨迹，最后通过整车执行机构控制自动驾驶车辆沿避障路径行驶。

当集合Lat的绝对值均小于MinDis时，则表明参考基准线模块输出的参考基准线，对当前车辆行驶存在碰撞风险(需要进行避障路径规划)。具体的，基于当前车辆位置与障碍物边沿点计算圆弧半径Rot，判断圆弧半径Rot是否大于0，如果圆弧半径Rot大于0，则把当前计算的圆弧半径归类为右侧避障半径集合RtRot，否则把当前计算的圆弧半径归类为左侧避障半径集合LfRot。计算最大计算最大右侧避障半径maxRtRot及其对应的右侧横向位移TarLatRt；计算最小左侧避障半径minRtRot及其对应的左侧横向位移TarLatLf，再给合通行区域信息、避障半径、横向位移，判断是左侧避障还是右侧避障，相同条件下，左侧避障优先，并计算避障的目标位置点(即2号控制点)，2号控制点具体位置如图2所示，其横向位置为TarLatRt(右侧避障)或者TarLatLf(左侧避障)，纵向距离为当前选取的障碍物横向位置点，匹配到参考基准线上的点，此参考基准线上的点到本车的距离。1号控制点的横向位置值与2号控制点横向位置一致，纵向位置为一个与当前车速相关的函数。

1号控制点纵向距离大于2号控制点的纵向距离时，基于当前车辆位置和2号控制点拟合的3次多项式作为避障路径，输出给横向控制模块，最后通过整车执行机构控制自动驾驶车辆沿避障路径行驶。

如果1号控制点在三次多项式的内侧，则也基于当前车辆位置和2号控制点拟合的3次多项式作为避障路径。否则基于当前车辆位置、1号控制点和2号控制点拟合四次多项式作为避障路径，输出给横向控制模块，最后通过整车执行机构控制自动驾驶车辆沿避障路径行驶。因此，本发明基于当前车辆位置与障碍物边沿点计算圆弧半径Rot，判断左侧避障还是右侧避障，并分别计算1号控制点和2号控制点坐标，以当前车辆位置和2号控制点拟合的三次多项式作为避障路径和当前车辆位置、1号控制点和2号控制点拟合四次多项式作为避障路径。在实现避障过程中，不再基于格栅图搜索，也不再是生成大量可执行路径，再选取最优路径，而是直接寻找控制点，这样可以大量节省运行资源，对控制器的算例要求低。同时，通过针对已生成的单一路径进行保持或者滤波等，操作更加容易，可以增强规划轨迹对障碍物波动的抗干扰能力，能够在复杂路况下实现避障。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管通过参照本发明的优选实施例已经对本发明进行了描述，但本领域的普通技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变。凡是属于本发明的技术方案所引申出的显而易见的改变仍处于本发明的保护范围之列。