[發明專利]一種自動駕駛車輛路徑與速度高度耦合的軌跡規劃方法

有效

• 申請號: 201910986350.2
• 申請日: 2019-10-17
• 公開（公告）號: CN110703754B
• 公開（公告）日: 2021-07-09
• 發明（設計）人: 徐燦;趙萬忠;周小川;王春燕;陳青云
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 江蘇圣典律師事務所 32237
• 代理人: 賀翔

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時刻對應的縱向位置，橫向位置，速度，橫擺角。