[發(fā)明專利]一種面向規(guī)劃控制聯(lián)合優(yōu)化的無人車輛運(yùn)動規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202010645446.5
• 申請日: 2020-07-07
• 公開（公告）號: CN111665853B
• 公開（公告）日: 2023-03-31
• 發(fā)明（設(shè)計）人: 張興龍;徐昕;陸陽;周星;方強(qiáng)
• 申請（專利權(quán)）人: 中國人民解放軍國防科技大學(xué)
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 長沙國科天河知識產(chǎn)權(quán)代理有限公司 43225
• 代理人: 周達(dá)

1.一種面向規(guī)劃控制聯(lián)合優(yōu)化的無人車輛運(yùn)動規(guī)劃方法，其特征在于，包括如下步驟：

步驟1，基于模型辨識的方法得到無人車輛的高維全局線性化模型；

步驟2，對高維全局線性化模型進(jìn)行降維，得到與無人車輛控制層動力學(xué)特性具有一致性的簡化模型；

步驟3，對無人車輛運(yùn)動路徑上的非凸障礙物進(jìn)行凸化處理，得到無人車輛運(yùn)動規(guī)劃中的凸約束形式；

步驟4，基于簡化模型與無人車輛運(yùn)動規(guī)劃的凸約束進(jìn)行不斷地滾動優(yōu)化，學(xué)習(xí)得到無人車輛在當(dāng)前時刻的近似最優(yōu)速度與參考軌跡曲線；

步驟5，將無人車輛在當(dāng)前時刻的近似最優(yōu)速度與參考軌跡曲線輸出至無人車輛的控制層，在更新環(huán)境障礙信息及簡化模型的狀態(tài)變量后重復(fù)步驟3-5；

步驟4的具體過程為：

步驟4.1，根據(jù)狀態(tài)量得到無人車輛在τ時刻速度v(τ)以及加速度a(τ)：

式中，Δt表示預(yù)測時域的采樣間隔，τ∈[k，k+N-1]表示預(yù)測時域內(nèi)第τ時刻，其中l(wèi)(τ)表示在全局坐標(biāo)系下無人車在第τ時刻的橫坐標(biāo)，d(τ)表示在全局坐標(biāo)系下無人車在第τ時刻的縱坐標(biāo)，v(τ)表示在第τ時刻的速度；

步驟4.2，將無人車輛在當(dāng)前時刻的近似最優(yōu)速度與參考軌跡曲線的求解轉(zhuǎn)換為代價最小化形式：

式中s(τ+1)表示簡化模型在第τ+1時刻的狀態(tài)變量，V(s(τ+1))表示第τ+1時刻的值函數(shù)；s(τ)表示簡化模型在第τ時刻的狀態(tài)變量，s(τ)＝[l(τ)，d(τ)，v(τ)]；表示折扣因子；u(τ)表示τ時刻的動作；r(τ)表示τ時刻的回報函數(shù)，表示為：

式中，分別表示回報函數(shù)中與無人車輛的加速度、位置、速度以及車輛動力學(xué)約束相關(guān)的權(quán)值系數(shù)，J_a，τ，J_x，τ，J_v，τ，J_e，τ分別表示回報函數(shù)中在τ時刻與加速度、位置、速度以及車輛動力學(xué)約束相關(guān)的代價；

步驟4.3，將狀態(tài)量約束、控制量約束以及凸約束轉(zhuǎn)換成包含在代價函數(shù)中的對數(shù)障礙函數(shù)的軟約束形式，具體表示為連續(xù)可微的障礙函數(shù)乘以標(biāo)量加權(quán)矩陣，因此當(dāng)前時刻的近似最優(yōu)速度與參考軌跡曲線的代價最小化形式就可以轉(zhuǎn)換成只含模型等式約束問題：

式中B(s)，B(u)，B_f(s)，B(Mz≤N)分別表示含有系統(tǒng)狀態(tài)、控制量、終端狀態(tài)以及凸約束的對數(shù)障礙函數(shù)；式中μ表示標(biāo)量系數(shù)；式中其中Q表示對狀態(tài)的懲罰矩陣；式中R表示對控制量的懲罰矩陣；/為終端值函數(shù)，終端狀態(tài)不變集可表示為式中Z為對稱正定矩陣，可表示為/矩陣P是通過計算李雅普諾夫方程得到的：

式中K是一個反饋增益矩陣，使得F＝A+BK是舒爾穩(wěn)定的；

步驟4.4，采用滾動時域ADP方法對步驟4.3中只含模型等式約束問題進(jìn)行求解，得到無人車輛在當(dāng)前時刻的近似最優(yōu)速度與參考軌跡曲線。

2.根據(jù)權(quán)利要求1所述面向規(guī)劃控制聯(lián)合優(yōu)化的無人車輛運(yùn)動規(guī)劃方法，其特征在于，步驟1中，所述高維全局線性化模型為：

式中，∑表示高維全局線性化模型；是高維的抽象狀態(tài)變量，N_ξ表示高維全局線性化模型的狀態(tài)變量維數(shù)，N_ξ＞n_ξ，其中n_ξ表示系統(tǒng)原始狀態(tài)變量維數(shù)；/是線性狀態(tài)轉(zhuǎn)移矩陣；/是系統(tǒng)輸入映射矩陣，其中m表示控制輸入矩陣的維數(shù)；/是系統(tǒng)輸出矩陣，其中n表示系統(tǒng)輸出的維數(shù)，/m是控制變量的維數(shù)。