CN113460091A - 一种无保护十字路口无人车滚动优化决策方法 - Google Patents

Abstract

本发明提供了一种无保护十字路口无人车滚动优化决策方法，获取本车和障碍物的位置及速度信息以及路口的车道线信息，生成本车的参考轨迹及预测周车轨迹；然后根据本车的参考轨迹对车辆信息进行Frenet坐标转换，得到Frenet坐标系下的本车和周车轨迹；接着建立Frenet坐标系下的无人车十字路口决策模型，来描述本车在无保护十字路口的运动；最后设计模型预测控制的无保护十字路口决策控制器，通过求解优化问题并将纵向速度和期望轨迹作用到底层控制器即可实现基于模型预测控制的无保护十字路口无人车行为决策；本方法在无保护十字路口无人车行为决策过程中将本车与周围物体的相对位置关系在纵向和横向上分离开，更能描述本车与周车的碰撞危险。

Description

一种无保护十字路口无人车滚动优化决策方法

技术领域

本发明属于汽车驾驶控制领域，涉及一种无保护十字路口无人车滚动优化决策方法。

背景技术

无人驾驶是当前的一个研究热点，其主要由环境感知、行为决策、运动控制三大部分组成，其中，行为决策是无人驾驶的核心。无人驾驶车辆的行为决策系统是决定其安全性、稳定性的关键，是其智能程度的体现。当前无人驾驶车辆采用的决策方法主要分为两大类：基于学习的方法和基于规则的方法。基于学习的决策方法需要大量的数据和长时间的训练，并且其可解释性较差；而基于规则的方法很难覆盖无保护十字路口等复杂的交通场景。且现有的决策模型大部分将决策过程建模为马尔可夫过程，由于其复杂度相对较高，且需要状态之间的转移概率，不利于决策算法的设计。

发明内容

本发明针对无人车在无保护十字路口的决策问题，提出一种无保护十字路口无人车滚动优化决策方法；

本发明所述的一种无保护十字路口无人车滚动优化决策方法，是采用如下技术方案实现的：

一种无保护十字路口无人车滚动优化决策方法，其特征在于，步骤过程如下：

步骤一、建立路口大地坐标系，获取路口大地坐标系下的本车的位置及速度信息、周车的位置及速度信息以及路口的车道线信息，生成本车的参考轨迹及预测周车轨迹；

步骤二、根据步骤一中获得的本车的参考轨迹和本车位置建立Frenet坐标系，对本车位置和预测周车轨迹进行Frenet坐标转换，得到Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹；

步骤三、根据本车对本车的参考轨迹的跟踪特性，建立Frenet坐标系下的无人车十字路口决策模型，来描述本车在无保护十字路口的运动；

步骤四、根据步骤二中得到的Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹以及步骤三中获得的无人车十字路口决策模型，设计模型预测控制的无保护十字路口决策控制器，通过求解优化问题并将纵向速度和期望轨迹作用到底层控制器即可实现基于模型预测控制的无保护十字路口无人车行为决策。

进一步的技术方案包括：

步骤一的具体过程如下：

以十字路口中心为坐标原点，以本车在路口直行驶入的方向为横轴，以本车在路口左转驶入的方向为纵轴建立路口大地坐标系；本方法中，定义本车为具有无保护十字路口无人车滚动优化决策功能的一辆无人车，周车为在本车周围所有的能够使用传感器探测到的除本车外的其他车辆；

根据路口大地坐标系，十字路口的四个朝向分别为横轴正方向、横轴负方向、纵轴正方向、纵轴负方向，相邻朝向的路互为垂直关系；定义本车进入路口的朝向为驶入方向、本车从驶入方向直行的朝向为直行方向、本车从驶入方向左转的朝向为左转方向、本车从驶入方向右转的朝向为右转方向；

针对直行状态，只考虑本车直行驶入前方车道的情况，即本车只沿x轴方向行驶，针对左转状态和右转状态，本方法中定义期望转向方向为本车期望在路口执行的转向动作的方向，当左转状态时期望转向方向即为左转方向，当右转状态时期望转向方向即为右转方向，将左转状态和右转状态分为从起始时刻到质心越过驶入方向停止线进入路口的直行段、从质心越过驶入方向停止线进入路口的到质心进入期望转向方向车道线离开路口的转弯段和从质心进入期望转向方向车道线离开路口到结束时刻的直行三段，选取一段椭圆弧作为转弯段的期望轨迹，椭圆弧表达式如式(1)所示：

其中椭圆弧的范围为从质心越过驶入方向停止线进入路口，到质心进入期望转向方向的车道线离开路口的部分，(α,β)为椭圆中心，α＝-dr×nn，β＝dr×wn，dr为车道宽，单位m，nn为从左转方向可驶入路口的车道数，wn为从驶入方向可驶出路口的车道数，p为椭圆长轴，单位m，p＝dr×(nn+z+0.5)，q为椭圆短轴，单位m，q＝dr×(wn+z+0.5)，z＝1,2,…,Z，Z为期望转向方向的可驶出路口的车道数；根据无人车在期望转向方向的可驶出路口的车道数Z并根据式(1)生成Z条参考轨迹；

通过摄像头、激光雷达、惯性导航系统、高精度地图之一或者组合感知周车在路口大地坐标系下的位置和车速，将周车车速进行沿路口大地坐标系的横轴和纵轴分解，假设预测时域内周车在路口大地坐标系下的速度保持不变，可以预测周车轨迹，如式(2)所示：

其中，(X,Y)为周车当前时刻的横纵坐标，(X_j,Y_j)为预测的周车在第j个预测周期后的横纵坐标，j＝1,2…P，P为预测步长，T_j为采样时间，单位s，v_obs,x是周车车速沿路口大地坐标系的横轴分量，单位m/s，v_obs,y是周车车速沿路口大地坐标系的纵轴分量，单位m/s；

根据式(2)得到P×T_j时间内预测周车轨迹。

步骤二的具体过程如下：

采用Frenet坐标转换，以本车的参考轨迹为参考系，描述本车和周车在本车的参考轨迹下的位置关系；

在进行Frenet坐标转换时，将本车位置向本车的参考轨迹投影得到的本车投影点作为Frenet坐标系的原点，以本车的参考轨迹为Frenet坐标系s轴，沿着本车行驶方向为s轴的正方向，依据右手法则旋转九十度定义为Frenet坐标系d轴正方向；

坐标转换分为纵向距离S计算和横向距离D计算，计算公式如式(3)所示：

其中S为纵向距离，D为横向距离，x为路口大地坐标系下轨迹点序列的横坐标向量，y为路口大地坐标系下轨迹点序列的纵坐标向量，x和y由本车的参考轨迹进行等距采样获得，即每隔采样距离Δ取一个参考轨迹点，依沿轨迹行驶的先后顺序组成轨迹点序列，将轨迹点序列的横坐标组成x、纵坐标组成y，Δ为根据实际情况人为设定的采样距离，单位m，Δ越小时轨迹点数量越多、Frenet坐标转换结果越准确、但需要耗费的计算资源越多；x_i为x的第i个点，y_i为y的第i个点，i₀为Frenet坐标系的原点在路口大地坐标系下轨迹点序列中的编号，i₁为物体向参考轨迹s轴投影点在路口大地坐标系下轨迹点序列中的编号，X₀为待转换的在路口大地坐标系下的横坐标，Y₀为待转换的在路口大地坐标系下的纵坐标；

将本车当前时刻的横纵坐标(X_h,Y_h)代入式(3)中的(X₀,Y₀)，得到Frenet坐标系下相对于本车的参考轨迹的本车位移(s,d)，s为本车纵向位移，单位m，d为本车侧向位移，单位m；将预测的周车在第j个预测周期后的横纵坐标(X_j,Y_j)逐时刻代入式(3)中的(X₀,Y₀)，得到相对于本车的参考轨迹的预测周车轨迹(S(i,j),D(i,j))，S(i,j)为第i个周车在j时刻的纵向距离，D(i,j)为第i个周车在j时刻的横向距离。

步骤三的具体过程如下：

因为在车身坐标系中，车辆的侧向速度v_y对车速的影响很小，可忽略不计，如式(4)所示：

v＝v_x (4)

因此，纵向的车辆运动学方程如式(5)所示：

其中，v_x是本车的纵向速度，单位m/s，v是本车的速度，单位m/s，s为本车的纵向位移，单位m；

本车在大地坐标系下的直道换道表达式如式(6)所示：

其中，T为侧向换道时间常数，单位s，定义为本车跟踪本车的参考轨迹的速度快慢，当T越大时跟踪过程越长、从一条本车的参考轨迹换道至另一条本车的参考轨迹的调节过程越长，y_d为本车在大地坐标系下的侧向位移，单位m，y_t为本车在大地坐标系下期望的侧向位移，单位m；

做本车的参考轨迹的决策时，以本车的参考轨迹进行Frenet坐标转换，因为本车期望位移总是与Frenet坐标系s轴重合，因此本车期望的侧向位移y_t始终为0，推导出Frenet坐标系下换道模型如式(7)所示：

其中，f_d(d_c)为本车在参考轨迹d_c下的横向距离，单位m，d_c为期望轨迹编号，由式(3)、式(6)可知f_d(d_c)＝d；

建立Frenet坐标系下的车辆运动学模型，如式(8)所示：

选取状态变量

控制输入

可得状态空间表达式，如式(9)所示：

其中，

步骤四的具体过程如下：

将式(9)离散化，得到式(10)：

其中，

C＝C_c，T_s为离散采样时间，单位s；

设定预测时域为P，控制时域为N，且N≤P，则k时刻该系统的输出预测方程和状态预测方程分别为：

其中，

根据实际上横向距离和纵向距离对车辆行驶安全的影响，将周车与本车之间距离表示为如式(12)所示：

其中C₁＝{(S,D)|S∈[S₀(j)-w_s,S₀(j)+w_s],D∈[D₀(j)-w_d,D₀(j)+w_d]}为危险区域，

为潜在碰撞区域，

为安全区域，w_s为横向安全距离，含义为能够保证本车不与周车发生碰撞的最小的横向距离常值，w_d为纵向安全距离，含义为能够保证本车不与周车发生碰撞的最小的纵向距离常值，D_long表示本车与周车距离较远，短时间内不会发生碰撞，D_short表示本车与周车距离较近，容易发生碰撞，S₀(j)为本车在j时刻的纵向距离，D₀(j)为本车在j时刻的横向距离，S(i,j)为第i个周车在j时刻的纵向距离，D(i,j)为第i个周车在j时刻的横向距离，D_dist(i,j)表示本车与第i辆周车在预测时域第j时刻的距离指标，每一时刻找出一辆与本车距离指标最小的周车用于目标函数的计算中，即D_min(j)＝min(D_dist(i,j))；

为了保证车距尽量远，建立目标函数J₁如式(13)所示：

为了使车辆保持设定车速行驶，建立目标函数J₂如式(14)所示：

其中，Δv_x＝v_ref(k)-v_x(k)，v_ref(k)为设定车速，单位m/s，v_x(k)为期望车速，单位m/s；

为了减少不必要的换道动作，建立目标函数J₃如式(15)所示：

其中，sgn(Δd_c)为对Δd_c取符号的符号函数，Δd_c＝d_c(k)-d_c(k-1)，d_c为期望车道编号；

结合上述三个目标函数得到优化问题：

其中，J为目标函数，Γ₁，Γ₂，Γ₃为加权矩阵，Γ₁＝γ₁E_P×P，Γ₂＝γ₂E_N×N，Γ₃＝γ₃E_N×N，γ₁,γ₂,γ₃为权重系数，v_xmax为车辆的纵向速度的上界，v_xmin为车辆的纵向速度的下界，单位均为m/s；

滚动求解优化问题，就能够得到期望车速和期望轨迹序号，即可实现基于模型预测控制的无保护十字路口无人车行为决策。

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

本发明提供了一种无保护十字路口无人车滚动优化决策方法，设计了模型预测控制的无保护十字路口决策控制器，所以可以实现无保护十字路口无人车行为决策；建立了Frenet坐标系下的无人车十字路口决策模型，所以可以描述Frenet坐标系下本车相对于期望轨迹的运动关系；设计了对车辆信息进行Frenet坐标转换方法，所以可以在无保护十字路口无人车行为决策过程中将本车与周围物体的相对位置关系在纵向和横向上分离开，更易于描述本车与周车的碰撞危险。

附图说明

图1为本发明提供的一种无保护十字路口无人车滚动优化决策方法的流程图。

图2为本方法中车辆在十字路口左转行驶时本车的参考轨迹示意图。

图3为本方法中车辆在十字路口右转行驶时本车的参考轨迹示意图。

图4为本方法中车辆在十字路口直行行驶时本车的参考轨迹示意图。

图5为本发明所述对车辆信息进行Frenet坐标转换示意图。

具体实施方式

一种无保护十字路口无人车滚动优化决策方法，其特征在于，步骤过程如下：

步骤一、建立路口大地坐标系，获取路口大地坐标系下的本车的位置及速度信息、周车的位置及速度信息以及路口的车道线信息，生成本车的参考轨迹及预测周车轨迹；具体过程如下：

以十字路口中心为坐标原点，以本车在路口直行驶入的方向为横轴，以本车在路口左转驶入的方向为纵轴建立路口大地坐标系；本方法中，定义本车为具有无保护十字路口无人车滚动优化决策功能的一辆无人车，周车为在本车周围所有的能够使用传感器探测到的除本车外的其他车辆；

根据路口大地坐标系，十字路口的四个朝向分别为横轴正方向、横轴负方向、纵轴正方向、纵轴负方向，相邻朝向的路互为垂直关系；定义本车进入路口的朝向为驶入方向、本车从驶入方向直行的朝向为直行方向、本车从驶入方向左转的朝向为左转方向、本车从驶入方向右转的朝向为右转方向；

针对直行状态，只考虑本车直行驶入前方车道的情况，即本车只沿x轴方向行驶，针对左转状态和右转状态，本方法中定义期望转向方向为本车期望在路口执行的转向动作的方向，当左转状态时期望转向方向即为左转方向，当右转状态时期望转向方向即为右转方向，将左转状态和右转状态分为从起始时刻到质心越过驶入方向停止线进入路口的直行段、从质心越过驶入方向停止线进入路口的到质心进入期望转向方向车道线离开路口的转弯段和从质心进入期望转向方向车道线离开路口到结束时刻的直行三段，选取一段椭圆弧作为转弯段的期望轨迹，椭圆弧表达式如式(1)所示：

其中椭圆弧的范围为从质心越过驶入方向停止线进入路口，到质心进入期望转向方向的车道线离开路口的部分，(α,β)为椭圆中心，α＝-dr×nn，β＝dr×wn，dr为车道宽，单位m，nn为从左转方向可驶入路口的车道数，wn为从驶入方向可驶出路口的车道数，p为椭圆长轴，单位m，p＝dr×(nn+z+0.5)，q为椭圆短轴，单位m，q＝dr×(wn+z+0.5)，z＝1,2,…,Z，Z为期望转向方向的可驶出路口的车道数；根据无人车在期望转向方向的可驶出路口的车道数Z并根据式(1)生成Z条参考轨迹；

通过摄像头、激光雷达、惯性导航系统、高精度地图之一或者组合感知周车在路口大地坐标系下的位置和车速，将周车车速进行沿路口大地坐标系的横轴和纵轴分解，假设预测时域内周车在路口大地坐标系下的速度保持不变，可以预测周车轨迹，如式(2)所示：

其中，(X,Y)为周车当前时刻的横纵坐标，(X_j,Y_j)为预测的周车在第j个预测周期后的横纵坐标，j＝1,2…P，P为预测步长，T_j为采样时间，单位s，v_obs,x是周车车速沿路口大地坐标系的横轴分量，单位m/s，v_obs,y是周车车速沿路口大地坐标系的纵轴分量，单位m/s；

根据式(2)得到P×T_j时间内预测周车轨迹。

步骤二、根据步骤一中获得的本车的参考轨迹和本车位置建立Frenet坐标系，对本车位置和预测周车轨迹进行Frenet坐标转换，得到Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹；具体过程如下：

由于路口大地坐标系下的相对位置不能简便的描述本车和本车的参考轨迹的位置关系，以及本车和周车的位置关系，所以采用Frenet坐标转换，以本车的参考轨迹为参考系，描述本车和周车在本车的参考轨迹下的位置关系；

在进行Frenet坐标转换时，将本车位置向本车的参考轨迹投影得到的本车投影点作为Frenet坐标系的原点，以本车的参考轨迹为Frenet坐标系s轴，沿着本车行驶方向为s轴的正方向，依据右手法则旋转九十度定义为Frenet坐标系d轴正方向；

坐标转换分为纵向距离S计算和横向距离D计算，计算公式如式(3)所示：

其中S为纵向距离，D为横向距离，x为路口大地坐标系下轨迹点序列的横坐标向量，y为路口大地坐标系下轨迹点序列的纵坐标向量，x和y由本车的参考轨迹进行等距采样获得，即每隔采样距离Δ取一个参考轨迹点，依沿轨迹行驶的先后顺序组成轨迹点序列，将轨迹点序列的横坐标组成x、纵坐标组成y，Δ为根据实际情况人为设定的采样距离，单位m，Δ越小时轨迹点数量越多、Frenet坐标转换结果越准确、但需要耗费的计算资源越多；x_i为x的第i个点，y_i为y的第i个点，i₀为Frenet坐标系的原点在路口大地坐标系下轨迹点序列中的编号，i₁为物体向参考轨迹s轴投影点在路口大地坐标系下轨迹点序列中的编号，X₀为待转换的在路口大地坐标系下的横坐标，Y₀为待转换的在路口大地坐标系下的纵坐标；

将本车当前时刻的横纵坐标(X_h,Y_h)代入式(3)中的(X₀,Y₀)，得到Frenet坐标系下相对于本车的参考轨迹的本车位移(s,d)，s为本车纵向位移，单位m，d为本车侧向位移，单位m；将预测的周车在第j个预测周期后的横纵坐标(X_j,Y_j)逐时刻代入式(3)中的(X₀,Y₀)，得到相对于本车的参考轨迹的预测周车轨迹(S(i,j),D(i,j))，S(i,j)为第i个周车在j时刻的纵向距离，D(i,j)为第i个周车在j时刻的横向距离。

步骤三、根据本车对本车的参考轨迹的跟踪特性，建立Frenet坐标系下的无人车十字路口决策模型，来描述本车在无保护十字路口的运动；具体过程如下：

因为在车身坐标系中，车辆的侧向速度v_y对车速的影响很小，可忽略不计，如式(4)所示：

v＝v_x (4)

因此，纵向的车辆运动学方程如式(5)所示：

其中，v_x是本车的纵向速度，单位m/s，v是本车的速度，单位m/s，s为本车的纵向位移，单位m；

由于换道过程中的位移曲线y(t)相对复杂，可以采用一阶惯性环节拟合位移曲线，故本车在大地坐标系下的直道换道表达式如式(6)所示：

其中，T为侧向换道时间常数，单位s，定义为本车跟踪本车的参考轨迹的速度快慢，当T越大时跟踪过程越长、从一条本车的参考轨迹换道至另一条本车的参考轨迹的调节过程越长，y_d为本车在大地坐标系下的侧向位移，单位m，y_t为本车在大地坐标系下期望的侧向位移，单位m；

做本车的参考轨迹的决策时，以本车的参考轨迹进行Frenet坐标转换，因为本车期望位移总是与Frenet坐标系s轴重合，因此本车期望的侧向位移y_t始终为0，推导出Frenet坐标系下换道模型如式(7)所示：

其中，f_d(d_c)为本车在参考轨迹d_c下的横向距离，单位m，d_c为期望轨迹编号，由式(3)、式(6)可知f_d(d_c)＝d；

建立Frenet坐标系下的车辆运动学模型，如式(8)所示：

选取状态变量

控制输入

可得状态空间表达式，如式(9)所示：

其中，

步骤四、根据步骤二中得到的Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹以及步骤三中获得的无人车十字路口决策模型，设计模型预测控制的无保护十字路口决策控制器，通过求解优化问题并将纵向速度和期望轨迹作用到底层控制器即可实现基于模型预测控制的无保护十字路口无人车行为决策；具体过程如下：

将式(9)离散化，得到式(10)：

其中，

C＝C_c，T_s为离散采样时间，单位s；

设定预测时域为P，控制时域为N，且N≤P，则k时刻该系统的输出预测方程和状态预测方程分别为：

其中，

由于是在Frenet坐标系下建立的车辆运动学模型，通过坐标转换之后能够得到本车和周车之间的横、纵向距离，根据实际上横向距离和纵向距离对车辆行驶安全的影响，将周车与本车之间距离表示为如式(12)所示：

其中C₁＝{(S,D)|S∈[S₀(j)-w_s,S₀(j)+w_s],D∈[D₀(j)-w_d,D₀(j)+w_d]}为危险区域，

为潜在碰撞区域，

为安全区域，w_s为横向安全距离，含义为能够保证本车不与周车发生碰撞的最小的横向距离常值，w_d为纵向安全距离，含义为能够保证本车不与周车发生碰撞的最小的纵向距离常值，D_long表示本车与周车距离较远，短时间内不会发生碰撞，D_short表示本车与周车距离较近，容易发生碰撞，S₀(j)为本车在j时刻的纵向距离，D₀(j)为本车在j时刻的横向距离，S(i,j)为第i个周车在j时刻的纵向距离，D(i,j)为第i个周车在j时刻的横向距离，D_dist(i,j)表示本车与第i辆周车在预测时域第j时刻的距离指标，每一时刻找出一辆与本车距离指标最小的周车用于目标函数的计算中，即D_min(j)＝min(D_dist(i,j))；

为了保证车距尽量远，建立目标函数J₁如式(13)所示：

因为车辆在道路上行驶总是希望其保持设定车速行驶，建立目标函数J₂如式(14)所示：

其中，Δv_x＝v_ref(k)-v_x(k)，v_ref(k)为设定车速，单位m/s，v_x(k)为期望车速，单位m/s；

在保证本车能够顺利换道的同时，还要保证换道动作尽量小，建立目标函数J₃如式(15)所示：

其中，sgn(Δd_c)为对Δd_c取符号的符号函数，Δd_c＝d_c(k)-d_c(k-1)，d_c为期望车道编号；

结合上述三个目标函数得到优化问题：

其中，J为目标函数，Γ₁，Γ₂，Γ₃为加权矩阵，Γ₁＝γ₁E_P×P，Γ₂＝γ₂E_N×N，Γ₃＝γ₃E_N×N，γ₁,γ₂,γ₃为权重系数，v_xmax为车辆的纵向速度的上界，v_xmin为车辆的纵向速度的下界，单位均为m/s；

滚动求解优化问题，就能够得到期望车速和期望轨迹序号，即可实现基于模型预测控制的无保护十字路口无人车行为决策。

Claims (5)

1.一种无保护十字路口无人车滚动优化决策方法，其特征在于，步骤过程如下：

步骤一、建立路口大地坐标系，获取路口大地坐标系下的本车的位置及速度信息、周车的位置及速度信息以及路口的车道线信息，生成本车的参考轨迹及预测周车轨迹；

步骤二、根据步骤一中获得的本车的参考轨迹和本车位置建立Frenet坐标系，对本车位置和预测周车轨迹进行Frenet坐标转换，得到Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹；

步骤三、根据本车对本车的参考轨迹的跟踪特性，建立Frenet坐标系下的无人车十字路口决策模型，来描述本车在无保护十字路口的运动；

步骤四、根据步骤二中得到的Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹以及步骤三中获得的无人车十字路口决策模型，设计模型预测控制的无保护十字路口决策控制器，通过求解优化问题并将纵向速度和期望轨迹作用到底层控制器即可实现基于模型预测控制的无保护十字路口无人车行为决策。

2.根据权利要求1所述的一种无保护十字路口无人车滚动优化决策方法，其特征在于，步骤一的具体过程如下：

以十字路口中心为坐标原点，以本车在路口直行驶入的方向为横轴，以本车在路口左转驶入的方向为纵轴建立路口大地坐标系；本方法中，定义本车为具有无保护十字路口无人车滚动优化决策功能的一辆无人车，周车为在本车周围所有的能够使用传感器探测到的除本车外的其他车辆；

根据路口大地坐标系，十字路口的四个朝向分别为横轴正方向、横轴负方向、纵轴正方向、纵轴负方向，相邻朝向的路互为垂直关系；定义本车进入路口的朝向为驶入方向、本车从驶入方向直行的朝向为直行方向、本车从驶入方向左转的朝向为左转方向、本车从驶入方向右转的朝向为右转方向；

针对直行状态，只考虑本车直行驶入前方车道的情况，即本车只沿x轴方向行驶，针对左转状态和右转状态，本方法中定义期望转向方向为本车期望在路口执行的转向动作的方向，当左转状态时期望转向方向即为左转方向，当右转状态时期望转向方向即为右转方向，将左转状态和右转状态分为从起始时刻到质心越过驶入方向停止线进入路口的直行段、从质心越过驶入方向停止线进入路口的到质心进入期望转向方向车道线离开路口的转弯段和从质心进入期望转向方向车道线离开路口到结束时刻的直行三段，选取一段椭圆弧作为转弯段的期望轨迹，椭圆弧表达式如式(1)所示：

其中椭圆弧的范围为从质心越过驶入方向停止线进入路口，到质心进入期望转向方向的车道线离开路口的部分，(α,β)为椭圆中心，α＝-dr×nn，β＝dr×wn，dr为车道宽，单位m，nn为从左转方向可驶入路口的车道数，wn为从驶入方向可驶出路口的车道数，p为椭圆长轴，单位m，p＝dr×(nn+z+0.5)，q为椭圆短轴，单位m，q＝dr×(wn+z+0.5)，z＝1,2,…,Z，Z为期望转向方向的可驶出路口的车道数；根据无人车在期望转向方向的可驶出路口的车道数Z并根据式(1)生成Z条参考轨迹；

通过摄像头、激光雷达、惯性导航系统、高精度地图之一或者组合感知周车在路口大地坐标系下的位置和车速，将周车车速进行沿路口大地坐标系的横轴和纵轴分解，假设预测时域内周车在路口大地坐标系下的速度保持不变，可以预测周车轨迹，如式(2)所示：

其中，(X,Y)为周车当前时刻的横纵坐标，(X_j,Y_j)为预测的周车在第j个预测周期后的横纵坐标，j＝1,2…P，P为预测步长，T_j为采样时间，单位s，v_obs,x是周车车速沿路口大地坐标系的横轴分量，单位m/s，v_obs,y是周车车速沿路口大地坐标系的纵轴分量，单位m/s；

根据式(2)得到P×T_j时间内预测周车轨迹。

3.根据权利要求2所述的一种无保护十字路口无人车滚动优化决策方法，其特征在于，步骤二的具体过程如下：

采用Frenet坐标转换，以本车的参考轨迹为参考系，描述本车和周车在本车的参考轨迹下的位置关系；

在进行Frenet坐标转换时，将本车位置向本车的参考轨迹投影得到的本车投影点作为Frenet坐标系的原点，以本车的参考轨迹为Frenet坐标系s轴，沿着本车行驶方向为s轴的正方向，依据右手法则旋转九十度定义为Frenet坐标系d轴正方向；

坐标转换分为纵向距离S计算和横向距离D计算，计算公式如式(3)所示：

其中S为纵向距离，D为横向距离，x为路口大地坐标系下轨迹点序列的横坐标向量，y为路口大地坐标系下轨迹点序列的纵坐标向量，x和y由本车的参考轨迹进行等距采样获得，即每隔采样距离Δ取一个参考轨迹点，依沿轨迹行驶的先后顺序组成轨迹点序列，将轨迹点序列的横坐标组成x、纵坐标组成y，Δ为根据实际情况人为设定的采样距离，单位m，Δ越小时轨迹点数量越多、Frenet坐标转换结果越准确、但需要耗费的计算资源越多；x_i为x的第i个点，y_i为y的第i个点，i₀为Frenet坐标系的原点在路口大地坐标系下轨迹点序列中的编号，i₁为物体向参考轨迹s轴投影点在路口大地坐标系下轨迹点序列中的编号，X₀为待转换的在路口大地坐标系下的横坐标，Y₀为待转换的在路口大地坐标系下的纵坐标；

将本车当前时刻的横纵坐标(X_h,Y_h)代入式(3)中的(X₀,Y₀)，得到Frenet坐标系下相对于本车的参考轨迹的本车位移(s,d)，s为本车纵向位移，单位m，d为本车侧向位移，单位m；将预测的周车在第j个预测周期后的横纵坐标(X_j,Y_j)逐时刻代入式(3)中的(X₀,Y₀)，得到相对于本车的参考轨迹的预测周车轨迹(S(i,j),D(i,j))，S(i,j)为第i个周车在j时刻的纵向距离，D(i,j)为第i个周车在j时刻的横向距离。

4.根据权利要求3所述的一种无保护十字路口无人车滚动优化决策方法，其特征在于，步骤三的具体过程如下：

因为在车身坐标系中，车辆的侧向速度v_y对车速的影响很小，可忽略不计，如式(4)所示：

v＝v_x (4)

因此，纵向的车辆运动学方程如式(5)所示：

其中，v_x是本车的纵向速度，单位m/s，v是本车的速度，单位m/s，s为本车的纵向位移，单位m；

本车在大地坐标系下的直道换道表达式如式(6)所示：

其中，T为侧向换道时间常数，单位s，定义为本车跟踪本车的参考轨迹的速度快慢，当T越大时跟踪过程越长、从一条本车的参考轨迹换道至另一条本车的参考轨迹的调节过程越长，y_d为本车在大地坐标系下的侧向位移，单位m，y_t为本车在大地坐标系下期望的侧向位移，单位m；

做本车的参考轨迹的决策时，以本车的参考轨迹进行Frenet坐标转换，因为本车期望位移总是与Frenet坐标系s轴重合，因此本车期望的侧向位移y_t始终为0，推导出Frenet坐标系下换道模型如式(7)所示：

其中，f_d(d_c)为本车在参考轨迹d_c下的横向距离，单位m，d_c为期望轨迹编号，由式(3)、式(6)可知f_d(d_c)＝d；

建立Frenet坐标系下的车辆运动学模型，如式(8)所示：

选取状态变量

控制输入

可得状态空间表达式，如式(9)所示：

其中，

5.根据权利要求4所述的一种无保护十字路口无人车滚动优化决策方法，其特征在于，步骤四的具体过程如下：

将式(9)离散化，得到式(10)：

其中，

C＝C_c，T_s为离散采样时间，单位s；

设定预测时域为P，控制时域为N，且N≤P，则k时刻该系统的输出预测方程和状态预测方程分别为：

其中，

根据实际上横向距离和纵向距离对车辆行驶安全的影响，将周车与本车之间距离表示为如式(12)所示：

其中C₁＝{(S,D)|S∈[S₀(j)-w_s,S₀(j)+w_s],D∈[D₀(j)-w_d,D₀(j)+w_d]}为危险区域，

D∈[D₀(j)-w_d,D₀(j)+w_d]}为潜在碰撞区域，

为安全区域，w_s为横向安全距离，含义为能够保证本车不与周车发生碰撞的最小的横向距离常值，w_d为纵向安全距离，含义为能够保证本车不与周车发生碰撞的最小的纵向距离常值，D_long表示本车与周车距离较远，短时间内不会发生碰撞，D_short表示本车与周车距离较近，容易发生碰撞，S₀(j)为本车在j时刻的纵向距离，D₀(j)为本车在j时刻的横向距离，S(i,j)为第i个周车在j时刻的纵向距离，D(i,j)为第i个周车在j时刻的横向距离，D_dist(i,j)表示本车与第i辆周车在预测时域第j时刻的距离指标，每一时刻找出一辆与本车距离指标最小的周车用于目标函数的计算中，即D_min(j)＝min(D_dist(i,j))；

为了保证车距尽量远，建立目标函数J₁如式(13)所示：

为了使车辆保持设定车速行驶，建立目标函数J₂如式(14)所示：

其中，Δv_x＝v_ref(k)-v_x(k)，v_ref(k)为设定车速，单位m/s，v_x(k)为期望车速，单位m/s；

为了减少不必要的换道动作，建立目标函数J₃如式(15)所示：

其中，sgn(Δd_c)为对Δd_c取符号的符号函数，Δd_c＝d_c(k)-d_c(k-1)，d_c为期望车道编号；

结合上述三个目标函数得到优化问题：

s.t.v_xmin≤v_x≤v_xmax

1≤d_c≤Z,d_c∈N

其中，J为目标函数，Γ₁，Γ₂，Γ₃为加权矩阵，Γ₁＝γ₁E_P×P，Γ₂＝γ₂E_N×N，Γ₃＝γ₃E_N×N，γ₁,γ₂,γ₃为权重系数，v_xmax为车辆的纵向速度的上界，v_xmin为车辆的纵向速度的下界，单位均为m/s；

滚动求解优化问题，就能够得到期望车速和期望轨迹序号，即可实现基于模型预测控制的无保护十字路口无人车行为决策。

Images