用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统

技术领域

本发明涉及自动驾驶技术领域，特别是关于一种用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统。

背景技术

主动避障技术是保障自动驾驶车辆安全行驶的关键技术之一，随着智能车辆技术的发展，主动避障技术得到了深入研究和应用。该技术主要通过先进的现代信息技术和传感器技术实时获取道路交通环境等多维信息，根据自车运动状况和道路交通环境信息识别潜在的行车风险因素，并根据风险等级采取不同的避障行为(换道避障、制动避障、巡航跟车等)，特别在紧急工况下，避障系统将采取主动制动以确保行车安全，通过紧急制动防止碰撞事故发生或降低事故损失。

循迹自动驾驶车辆通常应用于限定区域的固定场景，如景区观光车、巡检车等低速自动驾驶场景。车辆在自主循迹过程中需要进行局部路径规划和避撞控制，但实际车辆周围环境复杂，存在多类型且运动状态不确定的障碍物，如果采用基于规则的避撞控制方法无法系统地考虑全局避撞约束，而基于全局约束优化算法的路径规划计算量大，难以满足车辆实时性需求。因此，如何设计既能满足车辆动力学和全局碰撞约束，又满足车辆控制实时性要求的局部路径规划和最优避障行为决策方法，是实现车辆全自动驾驶的一个关键问题。

发明内容

本发明的目的在于提供一种用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明提供一种用于循迹自动驾驶车辆的防撞路径规划与控制方法，该方法包括：

步骤1，获取车辆相关信息及障碍物信息；

步骤2，规划防撞路径；

步骤3，根据所规划的防撞路径对应的目标车速，选择最佳防撞方式；

其中，步骤2具体包括：

步骤21，根据当前车速v和道路曲率ρ，计算预瞄距离；

步骤22，根据预瞄距离d

步骤23，根据车载激光雷达点云信息解析车辆循迹路径上是否存在障碍物，若不存在障碍物，则按照循迹路径继续行驶；若有障碍物，则进行防撞路径规划；

步骤24，在车辆坐标系xOy中，将车辆前方纵向长度H、横向宽度L的区域划分成等大小的网格，并给每个网格进行编号，根据激光雷达获取的前方障碍物点云信息确定网格的占用状态，取步骤23所规划的防撞路径上的等间隔离散点，根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算。

进一步地，步骤21具体包括：

步骤211，通过如下式(1)示出的插值方式计算预瞄距离初始值d

式中，K

步骤212，通过如下式(2)计算预瞄距离修正值d

d

式中，K为车速预瞄系数；

步骤213，通过式(3)计算预瞄距离d

d

进一步地，步骤22具体包括：

步骤221，计算循迹路径上距离车辆当前位置A

步骤222，计算点A

进一步地，步骤23具体包括：

步骤231，以车辆实际位置点为起点，采用如下坐标变换公式(5)和(6)，将预瞄点P

x

y

式中，Δx＝x

步骤232，以P

x

y

其中，ΔH为防撞路径终点距离参考点的横向基本偏移量，n为非零整数，θ

步骤233，通过式(9)所示的四个控制点确定三阶贝塞尔曲线，作为防撞路径C(t)：

式中，Δs为防撞曲线调整参数，

进一步地，步骤24具体包括：

步骤241，根据步骤233所规划的防撞路径C(t)上的所述等间隔离散点P在车辆坐标系xOy中的坐标，利用式(10)计算点P沿x轴方向所对应的网格编号：

Index(P)＝yIndex*n+xIndex (10)

式中，

步骤242，根据点P在沿x轴方向的邻近区域内网格激活状态进行横向车速规划，该邻近区域车载激光雷达探测的横向安全距离L

若点P沿x轴方向的邻近区域

若点P沿x轴方向的邻近区域

若点P有且仅有沿x轴方向的邻近区域

式中，x

步骤243，根据点P与车载激光雷达探测的纵向安全距离相对位置关系进行纵向车速规划，车载激光雷达探测的纵向安全距离为H

若0≤y

若H

若H

V

式中，V

步骤244，根据横向车速规划和纵向车速规划，按照式(13)计算车辆防撞路径C(t)对应的目标车速V

V

重复步骤241至步骤244得到防撞路径C(t)对应的目标车速集合{V

进一步地，如果网格被占用，则认为网格处于激活转态，f(i)＝1；若果网格未被占用，则认为网格未被激活，f(i)＝0。

进一步地，步骤3具体包括：

若所规划的防撞路径对应的目标车速V

若所规划的防撞路径有一条路径对应的目标车速V

若所规划的防撞路径有两条或以上路径对应的目标车速V

本发明还提供一种用于循迹自动驾驶车辆的防撞路径规划与控制系统，该系统包括：

信息采集装置，其用于获取车辆相关信息及障碍物信息；

防撞路径规划装置，其用于规划防撞路径；

控制装置，其用于根据所规划的防撞路径对应的目标车速，选择最佳防撞方式；

其中，防撞路径规划装置具体包括：

预瞄距离计算单元，其用于根据当前车速v和道路曲率ρ，计算预瞄距离；

预瞄点计算单元，其用于根据预瞄距离d

障碍物识别与防撞路径规划单元，其用于根据车载激光雷达点云信息解析车辆循迹路径上是否存在障碍物，若不存在障碍物，则按照循迹路径继续行驶；若有障碍物，则进行防撞路径规划；

车辆控制参数计算单元，其用于在车辆坐标系xOy中，将车辆前方纵向长度H、横向宽度L的区域划分成等大小的网格，并给每个网格进行编号，根据激光雷达获取的前方障碍物点云信息确定网格的占用状态，取障碍物识别与防撞路径规划单元所规划的防撞路径上的等间隔离散点，根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算。

进一步地，障碍物识别与防撞路径规划单元包括：

坐标转换子单元，其用于以车辆实际位置点为起点，采用如下坐标变换公式(5)和(6)，将预瞄点P

x

y

式中，Δx＝x

终点设置子单元，其用于以P

x

y

其中，ΔH为防撞路径终点距离参考点的横向基本偏移量，n为非零整数，θ

防撞曲线计算子单元，其用于通过式(9)所示的四个控制点确定三阶贝塞尔曲线，作为防撞路径C(t)：

式中，Δs为防撞曲线调整参数，

进一步地，车辆控制参数计算单元包括：

网格编号计算子单元，其用于根据障碍物识别与防撞路径规划单元所规划的防撞路径C(t)上的所述等间隔离散点P在车辆坐标系xOy中的坐标，利用式(10)计算点P沿x轴方向所对应的网格编号：

Index(P)＝yIndex*n+xIndex

(10)

式中，

横向车速规划子单元，其用于根据点P在沿x轴方向的邻近区域内网格激活状态进行横向车速规划，该邻近区域车载激光雷达探测的横向安全距离L

若点P沿x轴方向的邻近区域

若点P沿x轴方向的邻近区域

若点P有且仅有沿x轴方向的邻近区域

式中，x

纵向车速规划子单元，其用于根据点P与车载激光雷达探测的纵向安全距离相对位置关系进行纵向车速规划，车载激光雷达探测的纵向安全距离为H

若0≤y

若H

若H

V

式中，V

目标车速计算子单元，其用于根据横向车速规划和纵向车速规划，按照式(13)计算车辆防撞路径C(t)对应的目标车速V

V

本发明在满足车辆全局碰撞约束和控制实时性要求的前提下，解决循迹自动驾驶车辆局部路径规划和最优避障行为决策问题。

附图说明

图1为本发明实施例提供的用于循迹自动驾驶车辆的防撞路径规划与控制系统的原理示意图。

图2为本发明实施例提供的预瞄点计算原理示意图。

图3为本发明实施例提供的车辆前方障碍物网格划分示意图。

图4为本发明实施例提供的激光雷达感知安全区域定义示意图。

图5为本发明实施例提供的防撞横向车速规划示意图。

图6为本发明实施例提供的防撞纵向车速规划示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明实施例提供的用于循迹自动驾驶车辆的防撞路径规划与控制方法包括：

步骤1，获取车辆定位及障碍物信息。

车辆利用车载导航与定位系统获取车辆实时位置信息，包括车辆位置和位姿，记为(x

步骤2，规划防撞路径。

步骤21，根据当前车速v和道路曲率ρ计算预瞄距离。其中，当前车速v可通过CAN通信获取，也通过IMU或者GPS测量得到。道路曲率ρ可以通过车辆循迹轨迹方法获得。

步骤211，根据当前道路曲率ρ，通过如下式(1)示出的插值方式计算预瞄距离初始值d

式中，K

步骤212，根据当前车速v，通过如下式(2)计算预瞄距离修正值d

d

式中，K为车速预瞄系数，其常用数值在1.0-4.0范围内。

步骤213，通过式(3)计算初始值和修正值之和，得到预瞄距离d

d

步骤22，如图2所示，根据预瞄距离d

步骤221，计算循迹路径上距离车辆当前位置A

步骤222，计算循迹真实位置点A

当然，也可以在模型预测控制、驾驶员模型等预测轨迹获取方法获得的预测轨迹(预期参考轨迹)上选择预瞄点，根据预瞄距离来计算预瞄点。

步骤23，根据车载激光雷达点云信息解析车辆循迹路径上是否存在障碍物，若不存在障碍物，则按照循迹路径继续行驶；若有障碍物，则需要进行防撞路径规划，具体规划流程如下：

步骤231，首先以车辆实际位置点为起点，采用如下坐标变换公式(5)和(6)，将预瞄点P

x

y

式中，Δx＝x

步骤232，以车辆坐标系xOy中的预瞄点P

x

y

其中，ΔH为防撞路径终点距离参考点的横向基本偏移量，为常数，具体数值可以根据道路宽度、车辆尺寸和障碍物信息进行确定，实车测试中，我们通常选择2-4m作为初始参数，然后通过测试进一步标定；n为非零整数，其取值范围根据障碍物位置和道路约束条件确定；θ

除了上述实施例中设置的终点位置P

步骤233，本实施例采用贝塞尔曲线规划防撞路径，根据三阶贝塞尔曲线特点，四个控制点即可确定三阶贝塞尔曲线，作为防撞路径C(t)，四个控制点如下式(9)所示：

式中，Δs为防撞曲线调整参数，Δs的选择决定了防撞路径的形状，实际工程中，可以选择一个初始参数(例如3)，然后通过实车测试进行标定；

上述实施例中的贝塞尔曲线规划防撞路径也可以由曲线拟合、数值优化、人工势能场和图搜索等自动驾驶车辆轨迹规划方法替代。

步骤24，计算控制参数。

如图3和图4所示，在车辆坐标系xOy中，将车辆前方纵向长度H、横向宽度L的区域划分成等大小的网格，每个网格的长和宽均为Δ，横向网格数n＝L/Δ，纵向网格数m＝H/Δ。沿x轴方向给每个网格进行编号，每个网点对应唯一的编号i，则1≤i≤m*n。其中，“车辆前方纵向长度H、横向宽度L的区域”要求能够覆盖车传感器的感知范围，本实施例中可以理解为激光雷达的探测范围。

根据激光雷达获取的前方障碍物点云信息，从障碍物顶部向下做投影，可以得到网格是否被障碍物占用，将网格激活状态函数定义为f(i)。如果网格被占用，则认为网格处于激活转态，f(i)＝1；若果网格未被占用，则认为网格未被激活，f(i)＝0。

取步骤233所规划的防撞路径C(t)上的等间隔离散点{A,B,C,D,E,F…}，根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算，具体计算流程如下：

步骤241，根据步骤233所规划的防撞路径C(t)上的点P在车辆坐标系xOy中的坐标计算对应网格编号，计算公式如下式(10)所示：

Index(P)＝yIndex*n+xIndex (10)

式中，

步骤242，如图5所示，根据轨迹点P在沿x轴方向的邻近区域内网格激活状态进行横向车速规划。该邻近区域车载激光雷达探测的横向安全距离L

若轨迹点P沿x轴方向的邻近区域

若轨迹点P沿x轴方向的邻近区域

若轨迹点P有且仅有沿x轴方向的邻近区域

式中，x

步骤243，如图6所示，根据轨迹点P与车载激光雷达探测的纵向安全距离相对位置关系进行纵向车速规划。车载激光雷达探测的纵向安全距离为H

若0≤y

若H

若H

V

式中，V

步骤244，综合考虑横向车速规划和纵向车速规划，按照式(13)计算车辆防撞路径C(t)对应的目标车速V

V

重复前述步骤就可以得到所规划的防撞路径对应的目标车速集合{V

步骤3，根据所规划的防撞路径对应的目标车速选择最佳防撞方式。

若所规划的防撞路径对应的目标车速V

若所规划的防撞路径有一条路径对应的目标车速V

若所规划的防撞路径有两条或以上路径对应的目标车速V

本发明实施例还提供一种用于循迹自动驾驶车辆的防撞路径规划与控制系统，该系统包括信息采集装置、防撞路径规划装置和控制装置，其中：

信息采集装置用于获取车辆相关信息及障碍物信息。车辆利用车载导航与定位系统获取车辆实时位置信息，包括车辆位置和位姿，记为(x

防撞路径规划装置用于规划防撞路径。作为防撞路径规划装置的一种优选实施方式，防撞路径规划装置具体包括预瞄距离计算单元、预瞄点计算单元、障碍物识别与防撞路径规划单元和车辆控制参数计算单元。

预瞄距离计算单元用于根据当前车速v和道路曲率ρ，计算预瞄距离d

在一个实施例中，预瞄距离计算单元包括预瞄距离初始值计算子单元、预瞄距离修正值计算子单元和预瞄距离计算子单元。

预瞄距离初始值计算子单元用于根据当前道路曲率ρ，通过如下式(1)示出的插值方式计算预瞄距离初始值d

式中，K

预瞄距离修正值计算子单元根据当前车速v，通过如下式(2)计算预瞄距离修正值d

d

式中，K为车速预瞄系数，其常用数值在1.0-4.0范围内。

预瞄距离计算子单元通过式(3)计算初始值和修正值之和，得到预瞄距离d

d

如图2所示，预瞄点计算单元用于根据预瞄距离d

在一个实施例中，预瞄点计算单元包括循迹真实位置坐标计算子单元和预瞄点确定子单元。

循迹真实位置坐标计算子单元用于计算循迹路径上距离车辆当前位置A

预瞄点确定子单元用于计算循迹真实位置点A

当然，也可以在模型预测控制、驾驶员模型等预测轨迹获取方法获得的预测轨迹(预期参考轨迹)上选择预瞄点，根据预瞄距离来计算预瞄点。

障碍物识别与防撞路径规划单元用于根据车载激光雷达点云信息解析车辆循迹路径上是否存在障碍物，若不存在障碍物，则按照循迹路径继续行驶；若有障碍物，则进行防撞路径规划。

在一个实施例中，障碍物识别与防撞路径规划单元包括坐标转换子单元、终点设置子单元和防撞曲线计算子单元：

坐标转换子单元用于以车辆实际位置点为起点，采用如下坐标变换公式(5)和(6)，将预瞄点P

x

y

式中，Δx＝x

终点设置子单元用于以P

x

y

其中，ΔH为防撞路径终点距离参考点的横向基本偏移量，n为非零整数，θ

防撞曲线计算子单元用于根据三阶贝塞尔曲线特点，通过式(9)所示的四个控制点确定三阶贝塞尔曲线，作为防撞路径C(t)：

式中，Δs为防撞曲线调整参数，Δs的选择决定了防撞路径的形状，实际工程中，可以选择一个初始参数(例如3)，然后通过实车测试进行标定；

车辆控制参数计算单元用于在车辆坐标系xOy中，将车辆前方纵向长度H、横向宽度L的区域划分成等大小的网格，并给每个网格进行编号，根据激光雷达获取的前方障碍物点云信息确定网格的占用状态，取障碍物识别与防撞路径规划单元所规划的防撞路径上的等间隔离散点，根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算。

在一个实施例中，如图3至图6所示，车辆控制参数计算单元包括网格编号计算子单元、横向车速规划子单元、纵向车速规划子单元和目标车速计算子单元。

网格编号计算子单元用于根据障碍物识别与防撞路径规划单元所规划的防撞路径C(t)上的所述等间隔离散点P在车辆坐标系xOy中的坐标，利用式(10)计算点P沿x轴方向所对应的网格编号：

Index(P)＝yIndex*n+xIndex (10)

式中，

横向车速规划子单元用于根据点P在沿x轴方向的邻近区域内网格激活状态进行横向车速规划，该邻近区域车载激光雷达探测的横向安全距离L

若点P沿x轴方向的邻近区域

若点P沿x轴方向的邻近区域

若点P有且仅有沿x轴方向的邻近区域

式中，x

纵向车速规划子单元用于根据点P与车载激光雷达探测的纵向安全距离相对位置关系进行纵向车速规划，车载激光雷达探测的纵向安全距离为H

若0≤y

若H

若H

V

(12)

式中，V

目标车速计算子单元用于根据横向车速规划和纵向车速规划，按照式(13)计算车辆防撞路径C(t)对应的目标车速V

V

控制装置用于根据所规划的防撞路径对应的目标车速，选择最佳防撞方式。

若所规划的防撞路径对应的目标车速V

若所规划的防撞路径有一条路径对应的目标车速V

若所规划的防撞路径有两条或以上路径对应的目标车速V

本发明所提出的用于循迹自动驾驶车辆的防撞路径规划与控制系统，基于循迹自动驾驶车辆配备的定位和障碍物探测传感器，能够在满足车辆全局碰撞约束和控制实时性要求的前提下，采用三阶贝塞尔曲线完成循迹自动驾驶车辆局部路径规划，通过对所规划防撞路径的目标车速计算实现最优避障行为决策。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。