[發明專利]一種基于勢能場啟發式搜索的自動駕駛換道軌跡規劃算法

有效

• 申請號: 202011539198.2
• 申請日: 2020-12-23
• 公開（公告）號: CN112622932B
• 公開（公告）日: 2022-02-01
• 發明（設計）人: 曾憲明;柴晨
• 申請（專利權）人: 同濟大學
• 主分類號: B60W60/00
• 分類號: B60W60/00;B60W40/00;B60W50/00
• 代理公司: 上?？坡蓪＠硎聞账?特殊普通合伙) 31290
• 代理人: 葉鳳

1.一種基于勢能場啟發式搜索的自動駕駛換道軌跡規劃算法，其特征在于，步驟如下：

步驟1：依據道路交通環境中各要素的特征，建立動態車輛、車道線和車道邊界的勢能場函數；道路環境按0.1m×0.1m分辨率計算環境柵格風險值，得到風險勢能場環境車輛勢能函數：

其中q＝(x_q,y_q)^T為環境中的某點在t時刻的位置，x_i＝(x_i,y_i)^T為t時刻每輛環境車的位置，Ω是對角矩陣的平方反比標準差，W₁為伸縮系數；

車道線勢能函數：

其中W₂為伸縮系數，y_lane,i為第i段車道線的縱坐標；

其中W₃為伸縮系數，y_road,i為t時刻第i段道路邊界的縱坐標，為使風險值相對一致，添加ò項；

步驟2：在風險勢能場的基礎上，采用A-star算法搜索風險值最小的路徑，每t時刻根據當前的風險勢能場生成車輛的參考軌跡；A-star算法通過以下函數來計算每個節點的優先級：f(n)＝g(n)+h(n)；

其中：f(n)是節點n的綜合優先級；當選擇下一個要遍歷的節點時，選取綜合優先級最高(值最小)的節點；g(n)是節點n距離起點的代價；h(n)是節點n距離終點的預計代價，即A-star算法的啟發函數，通過調節啟發函數控制算法的速度和精確度；

采用A-star算法規劃路徑，首先標定車輛不可接受的最小風險閾值，在這個的基礎上可劃分車輛不可侵入區域，在這個范圍內不進行軌跡規劃；采用的A-star啟發函數為節點與目標點的距離的高斯函數，h(n)小于等于節點n到終點的代價，保證A-star算法一定能夠找到最短路徑；而節點移動更新時設置移動代價為t時刻移動節點的風險值，使算法能盡快找到一條風險值較小的最優路徑；利用A-star規劃所得到的位置信息蘊含了減小動態車輛風險、車道風險、道路邊界風險目標；

步驟3：為求解實際軌跡設定約束條件；預測第p步時，車輛位置為以該時刻主車與后車的不可侵入區域的切線，設定車輛勢能場約束；預測第p步時，車輛橫向位置為η(p)，依該時刻車道不可侵入區域和道路邊界不可侵犯區域得到橫向位置的最大值和最小值約束；預測第p步時，車輛縱向位置為Gipps的安全距離算法計算縱向位置最大值，設定與前車避撞約束；

步驟4：設計預測控制(ModelPredictive Control,MPC)模型，采用Bicycle model建立自動駕駛車輛運動學模型，以A-star規劃軌跡為參考求解安全、舒適、可執行的自動駕駛換道軌跡；基于車輛運動學模型和步驟3中安全約束條件設置MPC兩個約束條件，舒適性的條件是通過在MPC中約束加速度的變化率來實現；MPC的優化目標是使得生成軌跡與參考軌跡的誤差最小，同時保證控制量的變化最??；

其中，J為損失函數，N_p為預測時域，N_c為控制時域；y^p(k+it)，分別為車輛狀態和車輛狀態的參考值，Δu(k+it)為控制增量，ρ為權重系數，ε為松弛因子；

s.t.

k＝0,…,N_p-1

其中，離散系統內部狀態；A為系統矩陣，B為控制輸入矩陣；為控制輸入變量，C、D為車輛狀態更新矩陣；U為控制量狀態空間；a_min,a_max分別為最大減速度和最高加速度；δ_min,δ_max為最小方向盤轉角和最大方向盤轉角；為預測時刻p的縱向位置，η(p)為橫向位置，a_k,b_k,c_k為風險場切線系數。