[发明专利]一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法

权利要求书

1.一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法，其特征在于，包括步骤如下：

1)获取自车的运动状态信息及周围车辆的相对运动状态信息；

2)根据自车当前运动状态信息，分别建立自车的候选路径模型和候选速度模型；得到以车辆的法向加速度序列为输入，横摆角为输出的路径序列；以及切向加速度序列为输入，速度为输出的速度序列；

3)建立一个以车辆切向加速度序列和法向加速度序列为输入，速度、横摆角、坐标为输出的点运动模型，并将上述车辆切向加速度序列和法向加速度序列的输入利用矩阵的方式耦合起来，输入到该点运动模型得到速度与路径相耦合的候选轨迹序列；

4)利用RMSProp优化器实时获取当前时刻车辆最优的轨迹，将该轨迹对应的控制量作为输入，实现路径与速度的耦合规划；

所述步骤1)中通过GPS获取的自车的运动状态信息为：其中，是自车的纵向位置，是自车的侧向位置，是自车的横摆角，是自车的速度，是自车的横摆角速度，是自车的加速度，是自车的角加速度；通过毫米波雷达和激光雷达获取周围车辆的相对运动信息为其中，Δs_t，Δl_t，分别是周围车辆相对于自车的相对纵向位置，相对横向位置，相对横摆角，是周围车辆的速度，是周围车辆的横摆角，是周围车辆的加速度，是周围车辆的角加速度；

所述步骤2)中的候选路径模型利用4次多项式建立车辆侧向位置l与纵向位置s的函数并得到以法向加速度序列为输入，横摆角为输出的路径序列，具体包括如下步骤：

21)根据道路边界约束，得到候选路径终点时刻的侧向位置l_t+Np序列：

l_t+Np＝l_min:Δl/C_p:l_max

其中，l_min和l_max为道路的上、下边界；Δl＝l_max-l_min；C_p为候选路径的个数；

22)根据自车当前运动状态，以及给定的候选终点时刻的位置，利用4次多项式拟合出相应的候选路径，即侧向位置l相对于纵向位置s的方程，该过程将车辆的速度看成匀速的，具体如下：

l＝a₀+a₁s+a₂s²+a₃s³+a₄s⁴

其中，a_i是4次多项式拟合出的车辆路径参数，i＝1,2,3,4；为对应的终点时刻的纵向位置，N_p是预测时域；

23)根据拟合出的路径函数，即根据该路径函数的曲率离散出候选法向加速度序列，第i条候选路径对应的法向加速度序列为：

其中，为预测出的t+k时刻的法向加速度；

所述步骤2)中的候选速度模型利用4次多项式建立车辆纵向位置s与时间t的函数，并得到以切向加速度序列为输入，速度为输出的速度序列，具体包括如下步骤：

24)根据车辆加速性能约束，候选速度序列可由候选终点时刻的纵向位置序列s_t+Np表示如下：

s_t+Np＝s_min:Δs/C_s:s_max

其中，s_min和s_max为车辆所能达到的距离的上、下边界；Δs＝s_max-s_min；C_s为候选速度的个数；

25)根据自车当前运动状态，以及给定的候选纵向距离序列，利用4次多项式拟合出相应的候选速度函数，即纵向位置s关于时间t的函数，具体如下：

s(t)＝p₀+p₁t+p₂t²+p₃t³+p₄t⁴

其中，是终点时刻车辆对应的速度；

26)根据 拟合出的速度 函数，即根据该函数的二次斜率离散出候选切向加速度序列，第j条候选切向加速度序列为：

其中，为预测出的t+k时刻的切向加速度；

所述步骤3)中的以车辆切向加速度和法向加速度为输入，速度、横摆角、坐标为输出的点运动模型具体分为：

31)用抽象函数将车辆位置随时间变化的关系表示如下：

其中，f是纵向距离随时间变化的函数，g是侧向距离随时间变化的函数，这两个函数即可将车辆轨迹表示出来；

32)将上述函数用泰勒公式展开，保留到二次项，得到如下方程：

其中，各阶导数表示如下：

33)将以上各阶导数代入到轨迹方程中，得到所建立的以自车切向加速度和法向加速度为输入，自车速度、横摆角、坐标为输出的点运动模型：

其中，为该点运动模型输出的下一时刻的自车位置坐标，为自车的纵向位置和侧向位置，T是自车的规划周期，为自车的切向加速度，为自车的法向加速度；

所述步骤4)中利用RMSProp优化器进行优化时，建立的优化函数需考虑安全性、高效性、舒适性，并通过调节这三个特性的权重来满足个性化驾驶；具体如下：

41)建立的优化函数J具体如下：

其中，为第i个候选速度对应终点时刻的纵向位置；为第j个候选路径对应终点时刻的侧向位置；为第i个候选速度与第j个候选路径耦合得到的候选轨迹对应的危险度，R_ref为参考危险度，这一项代表安全性，M为权重；为第i个候选速度与第j个候选路径耦合得到的轨迹对应的速度，v_ref为参考速度，这一项代表高效性，N为权重；分母中的s_ref为参考纵向位置，l_ref为参考横向位置，代表舒适性。

2.根据权利要求1所述的自动驾驶车辆路径与速度高度耦合的轨迹规划方法，其特征在于，将步骤3)中的两个加速度序列的输入利用矩阵的方式耦合起来，输入到该点运动模型得到速度与路径相耦合的候选轨迹序列，具体包括如下步骤：

34)速度与路径两个方向加速度序列用矩阵方式耦合如下：

其中，C_p是候选路径的个数，C_s是候选速度的个数，A_ij(t)是第i条候选路径输入序列与第j条候选速度输入序列耦合得到的候选轨迹的输入序列，具体如下：

35)将上述耦合后的加速度序列输入到建立的点运动模型中，即可得到如下轨迹序列：

P_ij(t)＝[p_ij(t+1|t),p_ij(t+2|t),…,p_ij(t+k|t),…,p_ij(t+N_p|t)]

其中，为预测得到的t+k时刻对应的纵向位置，横向位置，速度，横摆角。