[发明专利]一种无保护十字路口无人车滚动优化决策方法

权利要求书

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

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

步骤二、根据步骤一中获得的本车的参考轨迹和本车位置建立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)所示：

其中，

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

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

其中，C_D＝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；

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