[發(fā)明專利]一種自動駕駛汽車低速自動泊車橫向控制方法

有效

• 申請?zhí)枺?/td>: 202110509744.6
• 申請日: 2021-05-10
• 公開（公告）號: CN113110500B
• 公開（公告）日: 2023-01-03
• 發(fā)明（設(shè)計）人: 吳昊
• 申請（專利權(quán)）人: 的盧技術(shù)有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 南京經(jīng)緯專利商標代理有限公司 32200
• 代理人: 徐瑩

1.一種自動駕駛汽車低速自動泊車橫向控制方法，其特征在于，包括以下步驟：

步驟1：自動駕駛車輛的規(guī)劃層根據(jù)當前環(huán)境規(guī)劃出泊車軌跡并發(fā)送到控制層；自動駕駛車輛的定位層對車輛的當前位置實時定位獲得定位信息并發(fā)送到控制層；自動駕駛車輛的底層接收控制層發(fā)出的控制指令并實時反饋車輛的當前狀態(tài)信息；

步驟2：自動駕駛車輛的控制層根據(jù)車輛運動學模型得出車輛運動的狀態(tài)空間方程，并建立LQR控制模型，具體包括：

步驟(2.1)獲取車輛運動學模型，具體為：車輛的后軸軸心位置由坐標(xr，yr)表示，后軸速度為vr，前軸軸心位置可由(xf，yf)表示，前軸速度為vf，車輛的后軸航向角表示為fai_r，前輪轉(zhuǎn)角表示為deltaf，車輛軸距為L；此時，車輛的后軸軸心速度表示為：

vr＝xr_dot*cos(fai_r)+yr_dot*sin(fai_r) (1)

其中，xr_dot和yr_dot分別為xr和yr的導數(shù)；

以及，前后軸的運動學約束為：

xf_dot*sin(fai_r+deltaf)-yf_dot*cos(fai_r+deltaf)＝0 (2)

xr_dot*sin(fai_r)-yr_dot*cos(fai_r)＝0 (3)

由式(1)(2)(3)可得：

xr_dot＝vr*cos(fai_r) (4)

yr_dot＝vr*sin(fai_r) (5)

根據(jù)前后輪的幾何關(guān)系可知：

xf＝xr+L*cos(fai_r) (6)

yf＝y(tǒng)r+L*sin(fai_r) (7)

將式(4)(5)(6)(7)代入(2)(3)，可解得：

fai_r_dot＝vr*tan(deltaf)/L (8)

其中，fai_r_dot為fai_r的導數(shù)；再聯(lián)合(4)(5)(8)獲得車輛的運動學模型：

[xr_dot；yr_dot；fai_r_dot]＝[cos(fai_r)；sin(fai_r)；tan(deltaf)/L]*vr；

步驟(2.2)根據(jù)車輛運動學模型得出車輛運動的狀態(tài)空間方程，并建立LQR控制模型，包括：假設(shè)在點(xr,yr,fai_r)處滿足車輛運動學模型，在該點處對車輛運動學模型進行線性化并據(jù)此構(gòu)造得到狀態(tài)空間矩陣：X_dot＝A*X+B*U；

其中，X_dot＝[x_dot-xr_dot；y_dot-yr_dot；fai_dot-fai_r_dot]；

并且，A＝[0,0,-vr*sin(fai_r)；

0,0,vr*cos(fai_r)；

0,0,0；]；

X＝[x-xr；y-yr；fai-fai_r]；

B＝[0；0；vr/L/cos(deltaf)/cos(deltaf)]；

U＝delta；

公式中，x,y,fai為目標軌跡點的坐標和橫擺角；x_dot,y_dot,fai_dot為x,y,fai的導數(shù)；L為車輛軸距；vr為后軸速度；X為跟蹤誤差矩陣；U為控制輸入矩陣，代表最優(yōu)前輪轉(zhuǎn)角；A與B為線性矩陣；

將狀態(tài)空間矩陣離散化獲得LQR控制模型：X(k+1)＝At*X(k)+Bt*U(k)，其公式中At＝I+A*T；Bt＝B*T；I為單位矩陣，T為單位時間；X(k+1)為k+1時刻的跟蹤誤差矩陣，X(k)為k時刻的跟蹤誤差矩陣；U(K)為k時刻控制輸入矩陣；

以及，定義如下的評價函數(shù)：

J(n)＝X(0)_T*Q*X(0)+...+X(n-1)_T*Q+X(n-1)+U(0)_T*R*U(0)+...+U(n-1)_T*R*U(n-1)

其中，X(i)為車輛在i處的狀態(tài)空間矩陣，U(i)為車輛在i處的控制輸入矩陣；X(i)_T和U(i)_T分別為X(i)和U(i)的矩陣轉(zhuǎn)置；Q和R分別為狀態(tài)量和控制量的權(quán)重矩陣；

步驟3：自動駕駛車輛的控制層根據(jù)建立的LQR控制模型進行求解，推導出泊車過程中的反饋車輛前輪轉(zhuǎn)角deltaf_feedback；

步驟4：自動駕駛車輛的控制層結(jié)合規(guī)劃層發(fā)送的泊車軌跡、定位層發(fā)送的定位信息、底層反饋的車輛的當前狀態(tài)信息，利用LQR控制模型計算得到實際的反饋前輪轉(zhuǎn)角deltaf_feedback，并結(jié)合求得的前饋輸出轉(zhuǎn)角deltaf_feedforward得到最終的總輸出轉(zhuǎn)角，根據(jù)總輸出轉(zhuǎn)角控制車輛泊車過程中前向行駛和后向行駛的過程。