CN115079699A - 一种基于模型预测控制的人机共驾汽车的运动规划方法 - Google Patents

Abstract

本发明涉及一种基于模型预测控制的人机共驾汽车的运动规划方法，属于自动驾驶的运动规划技术领域，具体说是一种基于模型预测控制的适用于多车道场景的人机共驾汽车的运动规划方法。本发明的方法中构建基于模型预测控制的运动规划问题时，构建带有约束的优化问题，用来计算未来设定时间段内的轨迹，基于改进的MPCC方法，首先，代价函数考虑：跟踪推荐路径的代价、舒适度代价、推荐速度的代价、人类输入的代价；其次是线性约束，考虑输入的约束，汽车状态的约束；最后是非线性约束，包括：与左右边界的避碰约束、侧翻约束、与其他车的碰撞约束。

Description

一种基于模型预测控制的人机共驾汽车的运动规划方法

技术领域

本发明涉及一种基于模型预测控制的人机共驾汽车的运动规划方法，属于自动驾驶的运动规划技术领域，具体说是一种基于模型预测控制的适用于多车道场景的人机共驾汽车的运动规划方法。

背景技术

自动驾驶是人工智能领域最为热门的研究方向之一，典型的自动驾驶技术按照层级可以分为：环境感知理解、决策与规划、底层控制，作为“大脑”的决策与规划层在整个自动驾驶技术中起到了决定性作用。决策与规划部分又可划分为：行为决策与运动规划，其中运动规划是最为基本、最为重要的模块，因此已经成为研究热点，其主要任务是：考虑汽车周围障碍物、汽车物理约束和交通规则，在全局导航路线的指引下，为智能车规划安全的行驶轨迹，即一连串带有速度和方向信息的离散航点。人机共驾汽车作为一种新型的智能车种类，相较于完全自动驾驶汽车，能够赋予人类更多的驾驶权限，将人类的决策融入自动驾驶***，在提升驾驶员的驾驶体验的同时保证驾驶的安全性。

现有的自动驾驶汽车运动规划方法可以分为四类：输入空间离散化法、随机采样规划法、端到端的机器学习方法、最优化方法。本发明属于最优化方法，克服了第一类方法的安全性不足的问题，克服了第二类方法的轨迹连续性较差、最优性难以保证的问题，解决了第四类方法可解释性不足的问题。本发明将汽车的轨迹规划问题抽象为带有各种约束的优化问题，能够考虑各种真实的约束和性能指标，具备较高的安全性，能够用于人机共驾汽车的运动规划，同时能够自动选择最佳的车道来行驶。

发明内容

有鉴于此，本发明的目的是提供一种基于模型预测控制的人机共驾汽车的运动规划方法，考虑与周围的其他汽车或道路边界的避碰、自身汽车的性能限制等，为人机共驾汽车规划最佳的行驶轨迹，该方法适用于多车道场景并考虑部分交通规则。

本发明的一种基于模型预测控制的人机共驾汽车的运动规划方法，主要分为六个步骤：

步骤一：数据读取；

提取人机共驾汽车质心的位置，从该质心点出发沿着道路参考线向前方延伸设定值，比如100米，获取可行驶区域的左右边界、车道的编号、车道的个数、各条车道中心线的位置，还获取人类驾驶员的输入的加速度、前轮转角以及周围其他车辆的位置、角度、速度信息；

步骤二：推荐路径拓扑网(晶格图)的构建；

从人机共驾汽车质心的位置出发，每隔设定距离对各条车道中心线采样并命名为端点，由此形成很多排端点；

使用光滑的三次多项式曲线有规律的连接排与排之间的端点，该曲线是曲率关于弧长的三次多项式，曲线在端点处的角度和曲率与所在车道的中心线一致；

由此得到由多条三次多项式曲线以及多排端点组成的推荐路径拓扑网G；

步骤三：推荐路径的选择；

首先对各条曲线进行评估，在推荐路径拓扑网G中任取其中一条三次多项式曲线，对其进行均匀采样评估，采样每隔为0.5米，命名为评估点。曲线的代价考虑：曲率变化率的代价、曲率代价、变道代价、在不可行驶区域O内评估点的个数、与周围其他汽车碰撞的评估点个数(在其他车质心周围

距离以内的评估点的个数)；

得到各条三次多项式曲线的代价之后，对拓扑网G使用A^*算法或Dijkstra算法，计算从汽车质心位置到最后一排端点的最短路径，并将其作为“推荐路径”；

步骤四：推荐速度的选择；

综合考虑车辆当前速度、车道限速、停止标识等信息，按照一定的规则计算推荐速度；

步骤五：构建基于模型预测控制的运动规划问题；

构建带有约束的优化问题，用来计算未来一段时间内的轨迹，基于改进的MPCC(model predictive contouring control,模型预测轮廓控制)方法。首先，代价函数考虑：跟踪推荐路径的代价、舒适度代价、推荐速度的代价、人类输入的代价；

其次是线性约束，考虑输入的约束(包括加速度的上下限约束、前轮转角变化率的上下限约束)，汽车状态的约束(包括任务区域的上下限约束、速度的上下限约束、角度的上下限约束、前轮转角的上下限约束等)。

最后是非线性约束，包括：与左右边界的避碰约束、侧翻约束、与其他车的碰撞约束；

步骤六：问题求解；

使用现有的非线性优化求解器求解该问题，初始解设置为全0，即可求解出规划的轨迹，该轨迹包含位置信息、速度信息、角度信息，以及“自行车模型”下的前轮转角信息、加速度信息。当汽车按照规划的轨迹行驶一定的时间后(一般是到达第一个规划的轨迹航点)，重新执行步骤一到步骤六，不断滚动优化运动轨迹，从而实现自动驾驶。

有益效果

(1)本发明能够考虑汽车各种真实的约束和性能指标，具备较高的安全性，能够用于人机共驾汽车的运动规划，同时能够自动选择最佳的车道来行驶，在多种驾驶场景中具有较高的适用性。

(2)本发明的方法中周围环境信息包括可行驶区域的左右边界、车道的编号、车道的个数、各条车道中心线的位置，还包括人类驾驶员的输入的加速度、前轮转角以及周围车辆的位置、角度、速度信息。

(3)本发明的方法中获取可行驶区域的左右边界、车道的编号、车道的个数、各条车道中心线的位置的信息时，向前方延伸80-120米。

(4)本发明的方法中从人机共驾汽车质心位置出发，每隔设定距离对各条车道中心线采样并命名为端点，由此形成很多排端点；使用光滑的三次多项式曲线有规律的连接排与排之间的端点，该曲线是曲率关于弧长的三次多项式，曲线在端点处的角度和曲率与所在车道的中心线一致；由此得到由多条三次多项式曲线以及多排端点组成的推荐路径拓扑网G。

(5)本发明的方法中对推荐路径拓扑网中的各条曲线进行评估时，在推荐路径拓扑网G中任取其中一条三次多项式曲线，对其进行均匀采样评估，采样每隔设定距离命名为评估点；各条曲线的代价考虑：曲率变化率的代价、曲率代价、变道代价、在不可行驶区域O内评估点的个数、与周围其他汽车碰撞的评估点个数，得到各条三次多项式曲线的代价之后，对拓扑网G使用A^*算法或Dijkstra算法，计算从汽车质心位置到最后一排端点的最短路径，并将其作为“推荐路径”。

(6)本发明的方法中构建基于模型预测控制的运动规划问题时，构建带有约束的优化问题，用来计算未来设定时间段内的轨迹，基于改进的MPCC方法，首先，代价函数考虑：跟踪推荐路径的代价、舒适度代价、推荐速度的代价、人类输入的代价；其次是线性约束，考虑输入的约束，汽车状态的约束；最后是非线性约束，包括：与左右边界的避碰约束、侧翻约束、与其他车的碰撞约束。

附图说明

图1为本发明的一种运动规划方法的汽车模型与“本车坐标系”示意图；

图2为本发明的一种运动规划方法的需要采集的数据示意图，淡蓝色区域是汽车的可行驶区域Ω，Ω拥有左右两条边界。两条相反方向的车道之间有道路参考线，道路参考线是描述车道信息的基准；

图3为本发明的一种运动规划方法的推荐路径生成算法中采样点和端点的示意图，黄色点是位于道路参考线上的采样点，红色点是位于车道中心线上的端点；

图4为本发明的一种运动规划方法的推荐路径生成算法中的三种基本连接模式示意图；

图5为本发明的一种运动规划方法的推荐路径生成算法中的其他汽车椭圆包裹示意图，其中

是周围第i个其他车的速度，aⁱ是椭圆的长半轴，

是包围该车的最小的长半轴长度；

图6为本发明的一种运动规划方法在U型两车道公路的连续滚动规划结果。

具体实施方式

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

符号说明

1)如图1所示，人机共驾汽车在二维平面

上运动。以车头所指的方向为x轴、汽车质心为原点建立二维右手坐标系，命名为“本车坐标系”。在“本车坐标系”下进行运动规划，且轨迹点/规划周期的个数为m。在第k个规划周期/轨迹点(k＝0，1，...，m)，人机共驾汽车的质心位置坐标记为(x_k，y_k)，沿着车头方向的纵向速度为v_k，车头方向与x轴的夹角为φ_k，前轮转角为δ_k，汽车从第0时刻到第k时刻行驶的里程为s_k.用6元组描述汽车在第k个规划周期/轨迹点的状态，记为z_k＝{x_k，y_k，v_k，φ_k，δ_k，s_k}。其中若k＝0，则汽车处于初始状态，且根据“本车坐标系”的定义，可知x₀＝0，y₀＝0，φ₀＝0，s₀＝0.汽车前轴中心到汽车质心的长度为l_f，汽车后轴中心到汽车质心的长度为l_r，汽车的宽度为W_V，汽车长度为L_V.

汽车的运动模型是“运动学自行车模型”，

其中u_k是输入(包括加速度

和前轮转角的角速度

)，Δt_k是第k个规划周期的时间间隔：

一般而言，为保证规划的准确性和实时性，Δt_k的典型值为0.05s至0.2s，且规划周期/轨迹点的个数m＞20.

2)依据OPENDRIVE的格式对周围道路进行描述，如图2所示，一条道路可由多条车道组成，两条相反方向的车道之间有道路参考线。道路参考线拥有编号0，由道路参考线向外拓展，每个车道都有相应的编号，依次是1、2、3......，相反方向车道的编号增加负号。可行驶区域Ω定义为一系列车道的组合，汽车可以在Ω内的车道之间进行变道，Ω的边界一般为道路参考线、路肩等。不可行驶的区域定义为

内不属于Ω的部分，记为O。

一种基于模型预测控制的人机共驾汽车的运动规划方法，包括以下步骤：

步骤一：数据读取；

如图3所示，读取OPENDRIVE格式的道路信息(包括可行驶区域Ω的左右边界、Ω内车道的编号、车道的个数、各条车道中心线的位置)，并转换到“本车坐标系”下，找到道路参考线上距离人机共驾汽车质心最近的点R₀，从该点出发沿着道路参考线向汽车行驶的方向延伸100米。读取周围汽车在“本车坐标系”下的位置

角度

速度

长度

宽度

其中上标i表示第i个汽车。当前时刻，人类驾驶员输入的加速度为

人类驾驶员输入的前轮转角为

步骤二：推荐路径拓扑网(晶格图)的构建；

如图3所示，从R₀出发沿着道路参考线向汽车行驶的方向，每隔Δs_c米对道路参考线采样，根据汽车的速度v₀设定采样点的个数为Q(一般情况下，汽车速度v₀越大，Δs_c越大，Q越大，但过大的Q会减慢算法的执行速度)，采样点依次记为R₁，R₂，...，R_Q，对每个采样点R_i(i＝1，2，...，Q)依次进行如下操作：

寻找各条车道中心线上距离R_i最近的点，并记为第i排端点：

(假定在采样点R_i处有ρ_i个车道)。在“本车坐标系”下读取车道中心线在第i排各个端点的角度、曲率、所在车道的编号，分别记为：

第0排采样点是汽车的质心(即“本车坐标系”的原点)，曲率为

分别取第i排端点和第j排(i，j＝0，1，2，...，Q，且i≠j)中的某两个端点

和

和

之间可以使用光滑的三次多项式曲线连接，该曲线是曲率关于弧长的三次多项式。曲线在端点处的角度和曲率分别是：

和

和

通过端点处角度和曲率的约束，可以通过牛顿迭代法[1]得到唯一的曲线。使用牛顿迭代法的曲线需要进行错误检测，对于触发错误的曲线，不对其进行连接。定义以下三种错误：

迭代次数错误：算法若迭代次数超过30次则触发该错误

形成圆环错误：若算法计算的曲线形成圆环则触发该错误

奇异值错误：若算法中关于矩阵的求逆运算出现奇异值错误则触发该错误

然后是连接策略，为了连接第i排和第j端点之间的各个端点，首先定义三种基本的连接模式，如图4所示：

直连模式：只连接具有相同车道编号的两个端点

非直连模式：只连接具有不同车道编号的两个端点，且车道编号的差距不大于1；

全连接模式：连接第i排和第j排之间的所有可能的端点组合，且车道编号的差距不大于1；

按照如下步骤，连接第i排和第j排端点之间的各个端点：

第1步：设定ρ₀＝1，ρ_-1＝0，ρ_-2＝0。然后执行第2步。

第2步：设定q＝1，如果ρ_q＝ρ_q-1，则对第q排和第q-1排端点之间做直连模式，否则对第q排和第q-1排端点之间做全连接模式。然后执行第3步。

第3步：如果ρ_q＝ρ_q-2，则对第q排和第q-2排端点之间做非直连模式，否则对第q排和第q-2排端点之间做全连接模式。然后执行第4步。

第4步：如果ρ_q＝ρ_q-3，则对第q排和第q-3排端点之间做非直连模式。然后执行第5步。

第5步：如果q＜Q，则q＝q+1并执行第2步，否则结束。

由此得到由多条三次多项式曲线以及多排端点组成的推荐路径拓扑网G。

步骤三：推荐路径的选择；

首先对各条曲线进行评估，在G中任取其中一条三次多项式曲线，该三次多项式曲线的长度记为l^len，两个端点的车道编号分别是I₁和I₂。对其进行均匀采样评估，采样每隔为0.5米。假设该曲线有w个评估点，其中第j(j＝1，2，...，w)个评估点的曲率为k_j，在“本车坐标系”下的位置为p_j。所有评估点在“本车坐标系”下的位置集合记为p_1：w＝{p₁，...，p_w}.

曲线的代价计算方式如下：

曲率变化率的代价：

曲率代价：

变道代价：J_lane＝|I₁-I₂|

在不可行驶区域O内评估点的个数：J_bound＝card(p_1：w∩O)，通过计算评估点与左右边界的距离，可以判断该评估点是否在不可行驶区域O内。

与周围其他汽车碰撞的评估点个数(在其他车质心周围

距离以内的评估点的个数)：

其中

是第i个周围汽车在“本车坐标系”下质心的位置。

该曲线的代价为：

J＝α₁·J_dcurv+α₂·J_curv+α₃·J_lane+α₄·J_bound+α₅·J_dynamic

其中参数α₁，...，α₅＞0。

得到各条三次多项式曲线的代价之后，对拓扑网G使用A*算法或Dijkstra算法，计算从第0排到第Q排端点的最短路径(即从本车质心位置到第Q排端点的最短路径)，并将其作为“推荐路径”。“推荐路径”可以引导汽车进入不拥挤的车道，并且符合交通规则。

通过离散的评估点可以计算“推荐路径”在“本车坐标系”下的位置(包括横坐标和纵坐标)、角度、与Ω的左右边界的距离。使用分段拟合方式得到“推荐路径”的位置、角度、与Ω的左右边界的距离三种函数，函数的自变量是“推荐路径”从本车位置出发的弧长s_r，分别记为：X(s_r)，Y(s_r)，Φ(s_r)，B_l(s_r)，B_r(s_r)

步骤四：推荐速度的选择

汽车当前的速度为v₀，“推荐道路”中各个端点所在车道的速度限制为v_limit，则推荐速度v_ref＝min{v₀+6，v_limit}，如果汽车前方有停止信号，则将推荐速度设为0.

步骤五：构建基于模型预测控制的运动规划问题

本步骤需要构建带有约束的优化问题，用来计算未来一段时间内的轨迹，基于改进的MPCC方法。首先是代价函数，第k个规划周期的代价函数为：

跟踪推荐路径的代价：J_r(z_k)＝β₁·(e′_c)²，其中：

e′_c＝(x_k-X(s_k-e_l(s_k)))sinΦ(s_k-e_l(s_k))-(y_k-Y(s_k-e_l(s_k)))cosΦ(s_k-e_l(s_k))，

e_l(s_k)＝-(x_k-X(s_k))cosΦ(s_k)-(y_k-Y(s_k))sinΦ(s_k)

舒适度代价：

推荐速度的代价：J_s(z_k)＝β₃·(v_k-v_ref)²

人类输入的代价：

第k个规划周期的总代价：

J^k＝(1-exp(-β₅k))·(J_r(z_k)+J_c(z_k，u_k)+J_s(z_k))+β₆ exp(-β₅k)·J_h(z_k)

其中β₁，β₂₁，β₂₂，β₂₃，β₃，β₄₁，β₄₂，β₅，β₆＞0.

其次是线性约束，对于任意的k＝0，1，...，m，包括：输入的约束u_min≤u_k≤u_max(加速度的上下限约束

前轮转角变化率的上下限约束

)，汽车状态的约束z_min≤z_k≤z_max(任务区域的上下限约束x_min≤x_k≤x_max和y_min≤y_k≤y_max、速度的上下限约束v_min≤v_k≤v_max、角度的上下限约束-π≤φ_k≤π、前轮转角的上下限约束δ_min≤δ_k≤δ_max、里程的下限约束s_k≥0)。

最后是非线性约束，对于任意的k＝0，1，...，m，包括：

与左右边界的避碰约束：

e′_c-B_r(s_k-e_l(s_k))-W≥0

-e′_c+B_l(s_k-e_l(s_k))-W≥0

其中

侧翻约束：

与其他车的碰撞约束：

如图5所示，第i个周围其他车的轮廓被椭圆形所包围，椭圆形的中心是第i个周围其他车的质心，椭圆的长半轴aⁱ短半轴是bⁱ，长半轴保持一定的长度不变，短半轴的长度随周围其他车的速度变化，满足

其中

是包围该车的最小的长半轴长度。该椭圆占据的区域记为

本车被N个圆形包裹，N个圆沿着汽车轴线均匀分布，圆的半径为R_V，这些圆记为

则约束为所有：

通过计算

圆心与Oⁱ边界的距离，可以判断

与Oⁱ是否有重叠。

步骤六：问题求解

若规划的总周期为m，周围其他车的个数为n，包围本车需要的圆个数为N最后需要求解的优化问题是：

u_min＜u_k＜u_max

z_min＜z_k＜z_max

z_k+1＝F(z_K，u_K)

s.t.B_r(s_k-e_l(s_k))+W≤e′_c≤B_l(s_k-e_l(s_k))-W

使用现有的非线性优化求解器求解该问题，初始解设置为全0，即可求解出规划的轨迹z₀，z₁，...，z_m，该轨迹包含位置信息、速度信息、角度信息，以及“自行车模型”下的前轮转角信息、加速度信息。当汽车按照规划的轨迹行驶一定的时间后(一般是到达第一个规划的轨迹航点)，重新执行步骤一到步骤六，不断滚动优化运动轨迹，从而实现自动驾驶。图6展示了本方法在U型两车道公路的连续运动规划示意图。

Claims (10)

1.一种基于模型预测控制的人机共驾汽车的运动规划方法，其特征在于步骤包括：

步骤一：从人机共驾汽车质心位置出发沿着道路参考线向前方延伸，获取周围环境信息；

步骤二：构建推荐路径拓扑网；

步骤三：对步骤二构建的推荐路径拓扑网中的各条曲线进行评估，得到各条曲线的代价，根据得到的各条曲线的代价选择推荐路径；

步骤四：根据车辆当前速度、车道限速、停止标识信息，按照已知的规则计算推荐速度；

步骤五：构建基于模型预测控制的运动规划问题；

步骤六：对步骤五构建的运动规划问题求解，得到规划的轨迹。

2.根据权利要求1所述的一种基于模型预测控制的人机共驾汽车的运动规划方法，其特征在于：

重新执行步骤一到步骤六，不断滚动优化运动轨迹，从而实现自动驾驶。

3.根据权利要求1或2所述的一种基于模型预测控制的人机共驾汽车的运动规划方法，其特征在于：

所述的步骤一中，周围环境信息包括可行驶区域的左右边界、车道的编号、车道的个数、各条车道中心线的位置，还包括人类驾驶员的输入的加速度、前轮转角以及周围车辆的位置、角度、速度信息。

4.根据权利要求3所述的一种基于模型预测控制的人机共驾汽车的运动规划方法，其特征在于：

获取可行驶区域的左右边界、车道的编号、车道的个数、各条车道中心线的位置的信息时，向前方延伸80-120米。

5.根据权利要求3所述的一种基于模型预测控制的人机共驾汽车的运动规划方法，其特征在于：

所述的步骤二中，构建推荐路径拓扑网的方法为：

从人机共驾汽车质心位置出发，每隔设定距离对各条车道中心线采样并命名为端点，由此形成很多排端点；

使用光滑的三次多项式曲线有规律的连接排与排之间的端点，该曲线是曲率关于弧长的三次多项式，曲线在端点处的角度和曲率与所在车道的中心线一致；

由此得到由多条三次多项式曲线以及多排端点组成的推荐路径拓扑网G。

6.根据权利要求5所述的一种基于模型预测控制的人机共驾汽车的运动规划方法，其特征在于：

所述的步骤三中，对推荐路径拓扑网中的各条曲线进行评估时，在推荐路径拓扑网G中任取其中一条三次多项式曲线，对其进行均匀采样评估，采样每隔设定距离命名为评估点；各条曲线的代价考虑：曲率变化率的代价、曲率代价、变道代价、在不可行驶区域O内评估点的个数、与周围其他汽车碰撞的评估点个数，得到各条三次多项式曲线的代价之后，对拓扑网G使用A^*算法或Dijkstra算法，计算从汽车质心位置到最后一排端点的最短路径，并将其作为“推荐路径”。

7.根据权利要求6所述的一种基于模型预测控制的人机共驾汽车的运动规划方法，其特征在于：

曲线的代价计算方式如下：

曲率变化率的代价：

曲率代价：

变道代价：J_lane＝|I₁-I₂|

在不可行驶区域O内评估点的个数：J_bound＝card(p_1：w∩O)，通过计算评估点与左右边界的距离，判断该评估点是否在不可行驶区域O内；

与周围其他汽车碰撞的评估点个数：

其中

是第i个周围汽车在“本车坐标系”下质心的位置。

8.根据权利要求7所述的一种基于模型预测控制的人机共驾汽车的运动规划方法，其特征在于：

所述的步骤五中，构建基于模型预测控制的运动规划问题时，构建带有约束的优化问题，用来计算未来设定时间段内的轨迹，基于改进的MPCC方法，首先，代价函数考虑：跟踪推荐路径的代价、舒适度代价、推荐速度的代价、人类输入的代价；

其次是线性约束，考虑输入的约束，汽车状态的约束；

最后是非线性约束，包括：与左右边界的避碰约束、侧翻约束、与其他车的碰撞约束。

9.根据权利要求8所述的一种基于模型预测控制的人机共驾汽车的运动规划方法，其特征在于：

输入的约束包括加速度的上下限约束、前轮转角变化率的上下限约束，汽车状态的约束包括任务区域的上下限约束、速度的上下限约束、角度的上下限约束、前轮转角的上下限约束。

10.根据权利要求8或9所述的一种基于模型预测控制的人机共驾汽车的运动规划方法，其特征在于：

所述的步骤六中，使用现有的非线性优化求解器求解该问题，初始解设置为全0，求解出规划的轨迹，该轨迹包含位置信息、速度信息、角度信息，以及“自行车模型”下的前轮转角信息、加速度信息，当汽车按照规划的轨迹行驶到达第一个规划的轨迹航点，重新执行步骤一到步骤六，不断滚动优化运动轨迹，从而实现自动驾驶。

Images