[發明專利]汽車換道軌跡規劃與動態軌跡跟蹤控制方法

在審

• 申請號: 202110280970.1
• 申請日: 2021-03-16
• 公開（公告）號: CN112947469A
• 公開（公告）日: 2021-06-11
• 發明（設計）人: 魏振亞;陳無畏;張先鋒;孟建平;丁雨康;崔國良
• 申請（專利權）人: 安徽卡思普智能科技有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 合肥市長遠專利代理事務所(普通合伙) 34119
• 代理人: 孫麗麗

1.一種汽車換道軌跡規劃與動態軌跡跟蹤控制方法，其特征在于，包括以下步驟：

S1、規劃直線道路或彎曲道路條件下的換道軌跡；

S2、設計動態軌跡跟蹤控制器跟蹤參考軌跡，且構建參考軌跡引力函數和障礙車斥力函數，規避動態障礙物。

2.根據權利要求1所述的汽車換道軌跡規劃與動態軌跡跟蹤控制方法，其特征在于，步驟S1的具體步驟如下：

S11、根據自車與前車的距離確定換道起始條件，即當兩車車距減小到最小縱向安全距離S_safe后開始換道；

S12、規劃出直線道路上的換道軌跡和彎曲道路條件下的換道軌跡。

3.根據權利要求2所述的汽車換道軌跡規劃與動態軌跡跟蹤控制方法，其特征在于，所述步驟S11具體為：

確定被控車輛開始換道時與前車之間的最小縱向安全距離S_safe，其公式為：

S_safe＝max{S_f，ε_v}

其中，ε_v為道路法規規定的行車間距；S_f為換道時間內自車行駛的縱向距離，S_f＝(v-v_obs)T₁+D₃；T₁為換道時間，D₃為換道結束時在縱向上與原車道前方障礙車輛仍留有一定的安全裕度值，v表示自車車速，v_obs表示前方障礙車輛車速。

4.根據權利要求2所述的汽車換道軌跡規劃與動態軌跡跟蹤控制方法，其特征在于，規劃直線道路上的換道軌跡的具體步驟如下：

S131、采用五次多項式來描述橫向位移軌跡y_ref(t)，其公式表示為：

y_ref(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

S132、在直線道路上，根據換道起始時行車狀態和換道終止時刻目標狀態，沿y軸的行車狀態為：

獲取多項式系數：

其中：H為車道寬度；T₁為換道時間。

5.根據權利要求2所述的汽車換道軌跡規劃與動態軌跡跟蹤控制方法，其特征在于，規劃曲線道路上的換道軌跡的具體步驟如下：

S141、設定當前車道和目標車道中心線曲率相等，且當前車道中心線方程已知：Y＝f(x)；換道時，沿車道中心相切的位移l為：

換道時汽車保持勻速，即l＝vt，得到：

S142、向心運動采用與直線換道相同的軌跡形式，即：

將g(x)代入G(t)即可得：

彎曲道路條件下換道軌跡為：

其中，θ為完成換道時車輛的位移角。

6.根據權利要求1所述的汽車換道軌跡規劃與動態軌跡跟蹤控制方法，其特征在于，步驟S2具體包括以下步驟：

S21、搭建模型預測所需的動力學模型和運動微分公式：

其中，C_f為前輪輪胎側偏剛度之和；C_r為后輪輪胎側偏剛度之和；l_f、l_r分別為質心到前軸、后軸距離，I_z為汽車繞z軸轉動慣量，δ_f為前輪轉角，v_y為車輛橫向速度，ω為車輛橫擺角速度，為車輛橫擺角加速度，為車輛橫向加速度，m為整車質量；

地面坐標系下車輛運動學方程：

S22、以為系統狀態量，以前輪轉角為控制量u＝δ_f，系統輸出量為：Y＝[x，y]^T，并對上述非線性系統線性化處理得到線性系統：

S23、對步驟S22中得到的線性系統進行離散化采用離散化周期為T_s，得到離散化系統狀態方程：

S24、根據S23中得到的離散化系統狀態方程設計模型預測控制器，采用前輪轉角增量代替原來的前輪轉角值作為系統控制輸入，并對系統狀態量進行如下轉換：

轉換后新的系統控制輸入為：

Δu(k|t)＝u(k|t)-u(k-1|t)

得到新的離散線性化狀態方程：

其中，η(k|t)＝Y(k|t)；

預測時域內系統輸出：

Y_p＝A_pζ(k|t)+B_pΔU+D_p

其中，

S25、構建的參考軌跡引力函數J_gra和障礙車斥力函數J_rep(t)分別為：

為了限制控制量的大幅變化，設置控制增量變化代價函數J_ctr(t)：

基于模型預測控制算法預測在當前狀態不同控制輸入下被控車輛未來有限時域內的位置，并以預測位置處勢場函數值和控制增量平方和為優化目標函數J：

S26、考慮對控制量和控制增量的約束，控制量即前輪轉角u(t)應被約束在機械結構所允許的最大范圍之內：

u_min≤u(t)≤u_max

控制增量Δu(t)應滿足以下約束：

Δu_min≤Δu(t)≤Δu_max

側向加速a_y(t)進行約束：

a_ymin≤a_y(t)≤a_ymax

車輛橫向位移y(t)約束：

其中，D為被控車輛寬度，77為安全裕值。