王 浩，林 棻，張堯文

(南京航空航天大學(xué)能源與動力學(xué)院車輛工程系，南京 210016)

基于模擬退火算法的無人駕駛車輛軌跡跟蹤控制

王 浩，林 棻，張堯文

(南京航空航天大學(xué)能源與動力學(xué)院車輛工程系，南京 210016)

研究了基于模擬退火算法(SA)的無人駕駛車輛橫向控制策略。建立了線性離散車輛運動學(xué)預(yù)測模型，考慮位姿偏差和控制量的變化設(shè)計目標(biāo)函數(shù)，運用模擬退火算法(SA)對目標(biāo)函數(shù)進行求解并獲得當(dāng)前時刻的控制增量，進而實現(xiàn)無人駕駛車輛的軌跡跟蹤控制。通過在低速組合軌跡工況下的仿真結(jié)果表明:在不同的車速下，該控制策略能夠保證車輛快速平穩(wěn)地跟蹤給定參考軌跡，位置誤差能夠控制在0.05 m之內(nèi)，具有較好的控制效果。

無人駕駛車輛;模型預(yù)測控制;模擬退火算法;軌跡跟蹤

隨著科技的日益進步，無人駕駛車輛開始逐漸進入人們的視野［1］。目前，軌跡跟蹤控制是無人駕駛車輛研究的基本問題，只有在具備跟蹤預(yù)期路徑的能力下才能更進一步地探討無人駕駛車輛的其他性能。

目前，國內(nèi)外許多科研機構(gòu)對無人駕駛車輛的軌跡跟蹤問題進行了研究。文獻［2］采用滑?？刂品椒ㄔO(shè)計軌跡跟蹤控制器，但產(chǎn)生的抖振現(xiàn)象對車輛和駕乘人員有不利影響。文獻［3］采用模糊控制方法設(shè)計軌跡跟蹤控制器，但穩(wěn)定性判據(jù)較難建立，且不具備自學(xué)習(xí)能力。文獻［4］采用神經(jīng)網(wǎng)絡(luò)PID方法設(shè)計軌跡跟蹤控制器，具備自學(xué)習(xí)能力，但魯棒性欠佳。

無人駕駛車輛系統(tǒng)具有非線性、時變性等特性。模擬退火算(simulated annealing，SA)是一種求解非線性問題的全局優(yōu)化算法，對初值魯棒性強，且應(yīng)用簡單。因此，本文將SA算法與模型預(yù)測控制相結(jié)合設(shè)計了無人駕駛車輛軌跡跟蹤控制器。

1 車輛運動學(xué)模型

無人駕駛車輛的軌跡跟蹤控制是通過對車輛系統(tǒng)的控制來實現(xiàn)的。在路面狀況良好的低速行駛工況下，一般不需要考慮車輛穩(wěn)定性控制等動力學(xué)問題［5］。因此，本文采用車輛的運動學(xué)模型作為預(yù)測模型［6］。

無人駕駛車輛運動學(xué)模型可以簡化為兩輪自行車模型。在平面直角坐標(biāo)系下，車輛的運動學(xué)模型如圖1所示。在慣性坐標(biāo)系oxy下，(xr，yr)和(xf，yf)分別為車輛后軸和前軸軸心的坐標(biāo)，vr和vf分別為車輛后軸和前軸中點的速度，φ為車輛的航向角，δf為前輪轉(zhuǎn)角，l為車輛前后軸軸距。

圖1 車輛運動學(xué)模型

由圖1中的運動學(xué)關(guān)系得:

整理化簡式(1)～(4)，得車輛的運動學(xué)模型:

式中:x˙r為車輛后軸中點處的橫向速度;y˙r為車輛后軸中點處的縱向速度。

為了表示方便，下文中的x=［x，y，φ］T，u=［v，δ］T與上文中的x=［xr，yr，φ］T，u=［vr，δf］T意義相同。

式(5)的一般形式表示為

軌跡跟蹤問題一般是通過控制實際車輛跟蹤參考車輛來實現(xiàn)的。因此，假設(shè)參考車輛行駛通過給定的參考軌跡，則參考車輛各個時刻的狀態(tài)量和控制量滿足上述方程。用d表示參考量，其一般形式表示為［5］

其中:xd=［xd，yd，φd］T;ud=［vd，δd］T。

在任意參考軌跡點處將式(6)以泰勒級數(shù)形式展開，并忽略高階項:

式(8)與式(7)相減得

式中:

本文采用的線性模型預(yù)測控制(model predictive control，MPC)［7］是一種離散時間控制方法，因此需要對式(9)表達的車輛運動學(xué)模型的連續(xù)狀態(tài)方程進行離散化處理:

其中，T為采樣時間。

式(11)和(12)即為線性離散化的車輛運動學(xué)模型，將該模型作為MPC的預(yù)測模型。

2 基于MPC的軌跡跟蹤控制算法

MPC通過建立的預(yù)測模型，預(yù)測系統(tǒng)在當(dāng)前狀態(tài)施加一定的控制作用后未來的狀態(tài)或輸出，并根據(jù)其約束條件和性能指標(biāo)求解最優(yōu)的控制序列，對控制對象施加當(dāng)前控制量，通過實時檢測來修正對未來狀態(tài)或輸出的預(yù)測。如此往復(fù)循環(huán)，以實現(xiàn)對控制對象的控制［7］。

MPC易于建模、控制效果好、魯棒性強，可以處理多變量和有約束的問題，并且能夠?qū)崿F(xiàn)反復(fù)在線優(yōu)化［8-9］。

2.1 預(yù)測方程

在式(11)和(12)的基礎(chǔ)上推導(dǎo)預(yù)測方程，以實現(xiàn)對車輛系統(tǒng)未來各個時刻輸出的預(yù)測。設(shè)預(yù)測時域為Np，控制時域為Nc。由式(11)和(12)可以預(yù)測出在Nc個控制增量的作用下未來Np個時刻車輛系統(tǒng)的輸出，用矩陣形式描述為

2．2 目標(biāo)函數(shù)和約束條件設(shè)計

車輛系統(tǒng)當(dāng)前的狀態(tài)量是已知量，控制量是未知量，因此需要設(shè)計合理的目標(biāo)函數(shù)，將車輛系統(tǒng)的控制量作為設(shè)計變量，通過求解目標(biāo)函數(shù)得到控制時域內(nèi)的控制量序列。

為了保證無人駕駛車輛能夠快速且平穩(wěn)地跟蹤參考軌跡，需要對車輛預(yù)測位姿與參考位姿的偏差和控制量的變化進行優(yōu)化。由于系統(tǒng)模型的時變特性，在目標(biāo)函數(shù)中加入松弛因子，以確保每個時刻都可求得可行解［5］。因此，選取目標(biāo)函數(shù):

式中:第1項考慮了車輛預(yù)測位姿與參考位姿的偏差，表征了車輛系統(tǒng)跟蹤參考軌跡的能力;第2項考慮了控制量的變化，以保證控制量變化平穩(wěn);Q和R為目標(biāo)函數(shù)中適當(dāng)維數(shù)的狀態(tài)和控制加權(quán)矩陣，Q=diag(q q…q)，R=diag(r r…r);ρ為權(quán)重系數(shù);ε為松弛因子;xp(k+i|k)為k時刻對未來第k+i時刻車輛系統(tǒng)的實際狀態(tài)量的預(yù)測;xd(k+i|k)為未來k+i時刻車輛系統(tǒng)的參考狀態(tài)量;y(k+i|k)的實際含義為未來第k+i時刻車輛系統(tǒng)的實際狀態(tài)量的預(yù)測值與參考狀態(tài)量的偏差;為第k+i時刻車輛系統(tǒng)的實際控制增量的預(yù)測值。

為了避免控制量的突變，保證控制量變化的連續(xù)性，使無人駕駛車輛軌跡跟蹤過程更加平穩(wěn)，對控制增量設(shè)置約束條件:

2．3 目標(biāo)函數(shù)求解

將上述問題轉(zhuǎn)化為求解以下帶有上下界約束的多變量單目標(biāo)優(yōu)化問題:

本文采用模擬退火算法(SA)對目標(biāo)函數(shù)進行優(yōu)化求解。SA算法可以求解不同的非線性問題，對初值有較強的魯棒性，簡單通用，并且易于實現(xiàn)，是一種全局優(yōu)化算法［10］。對于該非線性優(yōu)化問題，設(shè)計變量和目標(biāo)函數(shù)已確定，運用SA算法進行優(yōu)化求解，步驟如圖2所示。

求解所得的設(shè)計變量最優(yōu)解即為當(dāng)前時刻車輛系統(tǒng)的控制增量序列:

到下一時刻，預(yù)測時域和控制時域隨著時間的推進向前滾動推移。如此循環(huán)往復(fù)，便可以實現(xiàn)無人駕駛車輛的軌跡跟蹤控制。

圖2 SA算法流程

3 仿真驗證

為了驗證所設(shè)計的MPC軌跡跟蹤控制器的性能，本文在Matlab環(huán)境下進行了仿真試驗。

仿真試驗所采用的參考軌跡為直線軌跡與圓形軌跡的組合軌跡。分別在車輛參考縱向速度為5，10 m/s的情況下進行仿真試驗。直線軌跡跟蹤階段的參考前輪轉(zhuǎn)角為0 rad，圓形軌跡跟蹤階段的參考前輪轉(zhuǎn)角為0.129 5 rad。車輛的初始狀態(tài)為［0 -5 0］，即初始橫向位置為0 m，初始縱向位置為-5 m，初始航向角為0 rad。采樣時間T=0.05 s，軸距l(xiāng)=2.6 m。所采用的MPC軌跡跟蹤控制器的主要性能參數(shù)為:預(yù)測時域Np=60，控制時域Nc=40，Q=diag(q，q，…，q)Np×Np，q=diag(10 1 1)，R=500×INu×Nc，ρ=10，Δvmin= -0.05 m/s，Δvmax=0.05 m/s，Δδmin=-0.008 2 rad，Δδmax=0.008 2 rad。所采用的模擬退火算法的參數(shù)如下:最大迭代次數(shù)為500，初始溫度為100℃，采用指數(shù)降溫。

仿真試驗結(jié)果如圖3～7所示。圖3為不同參考速度下的車輛實際軌跡與參考軌跡;圖4～5分別為不同參考速度下的位姿偏差。由圖3～5可知:在車輛初始位置與參考軌跡存在初始偏差、參考速度不同的情況下，車輛在MPC軌跡跟蹤控制器的作用下均能快速地跟蹤上參考軌跡，隨后其位置偏差控制在0.05 m內(nèi)，航向角偏差也在較短時間內(nèi)收斂為0;在參考速度較低的工況下，車輛的實際軌跡與參考軌跡更為接近，且跟蹤上參考軌跡后的位姿偏差更小，但在參考速度較高的工況下車輛依然有良好的跟蹤效果，參考速度的提高并沒有導(dǎo)致跟蹤性能的下降。因此，本文所設(shè)計的MPC軌跡跟蹤控制器能夠?qū)崿F(xiàn)車輛對參考軌跡的快速跟蹤，并且對車輛縱向速度的變化具有較強的魯棒性。

圖3 不同參考速度下的車輛實際軌跡與參考軌跡

圖4 參考速度為5 m/s時的位姿偏差

圖5 參考速度為10 m/s時的位姿偏差

圖6～7分別為不同參考速度下的控制量變化。由圖6～7可知:由于存在初始偏差，在初始的一段時間內(nèi)，車輛的縱向速度和前輪轉(zhuǎn)角不斷調(diào)整，以保證車輛能夠較快地跟蹤參考軌跡。隨后車輛的縱向速度和前輪轉(zhuǎn)角逐漸跟蹤上參考值，并幾乎保持不變。由于圓形軌跡與直線軌跡

圖6 參考速度為5 m/s時的控制量變化

4 結(jié)束語

將SA算法與MPC進行結(jié)合解決無人駕駛車輛的軌跡跟蹤問題。建立了車輛運動學(xué)模型，設(shè)計了MPC軌跡跟蹤控制器，并運用SA算法對控制量進行優(yōu)化，對車輛的縱向速度和前輪轉(zhuǎn)角進行控制。

本文設(shè)計的控制器能實現(xiàn)無人駕駛車輛對參考軌跡的快速跟蹤，并且能保證軌跡跟蹤的平穩(wěn)性和魯棒性。軌跡跟蹤的位置誤差能夠控制在0.05 m之內(nèi)。

在參考速度較低的工況下，車輛的實際軌跡與參考軌跡更為接近，且跟蹤上參考軌跡后的位的參考前輪轉(zhuǎn)角有所不同，因此，車輛從直線軌跡向圓形軌跡過渡時，其前輪轉(zhuǎn)角會有所變化。隨著參考速度的提高，控制量的變化加大，但均在約束范圍內(nèi)，變化比較平穩(wěn)，因此本文所設(shè)計的控制器能夠?qū)崿F(xiàn)車輛對參考軌跡的平穩(wěn)跟蹤。姿偏差更小，控制量的變化也更小。在參考速度較高的工況下車輛依然有良好的跟蹤效果，控制量的變化也均在約束范圍內(nèi)，參考速度的提高并沒有導(dǎo)致跟蹤性能的下降。

圖7 參考速度為10 m/s時的控制量變化

［1］Ozguner U，Acarman T，Redmill K.Auto-nomous Ground Vehicles［M］.Boston:Artech House，2011.

［2］Hamerlain F.Trajectory tracking control of a car-like mobile robot in presence of sliding［C］//2012 UKACC International Conference on Control.Cardiff:IEEE，2012: 502-507.

［3］Mahapatra S，Patle B K，Jha A K，et al.Fuzzy logic control of a WMR［C］//2012 International Conference on Computing，Communication and Applications.Dindigul: IEEE，2012:1-5.

［4］Singh A，Bisht G，Padhy P K.Neural network based adaptive nonlinear PID controller for non-holonomic mobile robot［C］//2013 IEEE International Conference on Control，Automation，Robotics and Embedded Systems.Jabalpur:IEEE，2013:1-6.

［5］徐威，龔建偉，姜巖.無人駕駛車輛模型預(yù)測控制［M］.北京:北京理工大學(xué)出版社，2014.

［6］劉子龍，楊汝清，楊明，等.無人駕駛車輛橫向位置最優(yōu)跟蹤控制［J］.上海交通大學(xué)學(xué)報，2008(2):257 -261.

［7］席裕庚.預(yù)測控制［M］.2版.北京:國防工業(yè)出版社，2013.

［8］胡國龍，孫優(yōu)賢.預(yù)測控制進展及其應(yīng)用研究［J］.電力系統(tǒng)及其自動化學(xué)報，2003(1):109-114.

［9］Mousavi M A，Heshmati Z，Moshiri B.LTV-MPC based path planning of an autonomous vehicle via convex optimization［C］//2013 21st Iranian Conference on Electrical Engineering.Mashhad:IEEE，2013:1-7.

［10］Afifi S，Dang D C，Moukrim A.A Simulated Annealing Algorithm for the Vehicle Routing Problem with Time Windows and Synchronization Constraints［J］.Lecture Notes in Computer Science，2013:259-265.

(責(zé)任編輯楊黎麗)

Research on Trajectory Tracking Control of Self-Driving Vehicle Based on Simulated Annealing Algorithm

WANG Hao，LIN Fen，ZHANG Yao-wen
(Department of Automotive Engineering，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China)

Lateral control based on simulated annealing(SA)algorithm was investigated for trajectory tracking of self-driving vehicle.Firstly，the linear discrete kinematics model was established.Based on this model，considering position and pose error，the objective function and constraint condition were designed，and simulated annealing(SA)algorithm was used for optimization problems and obtaining the optimal control variables and the trajectory tracking control was achieved.In the condition of low speed and combined trajectory，the simulation results show that in different speeds，the control strategy guarantees that the vehicle can follow the reference trajectory stably and rapidly with good effect，and the position error can be kept within 0.05 m.

self-driving vehicle;model predictive control;simulated annealing;trajectory tracking

TP273

A

1674-8425(2015)11-0106-06

10.3969/j.issn.1674-8425(z).2015.11.018

2015-06-28

國家自然科學(xué)基金資助項目(10902049);江蘇省博士后科研資助計劃項目(1302020C);中央高?；究蒲袠I(yè)務(wù)費專項資金資助項目(NS2015015);南京航空航天大學(xué)研究生創(chuàng)新基地(實驗室)開放基金資助項目(kfjj201404)

王浩(1989—)，男，碩士研究生，主要從事車輛系統(tǒng)動力學(xué)研究;通訊作者 林棻(1980—)，男，博士，副教授，主要從事車輛動力學(xué)與控制、汽車現(xiàn)代設(shè)計方法等方面研究。

王浩，林棻，張堯文.基于模擬退火算法的無人駕駛車輛軌跡跟蹤控制［J］.重慶理工大學(xué)學(xué)報:自然科學(xué)版，2015(11):106-111.

format:WANG Hao，LIN Fen，ZHANG Yao-wen.Research on Trajectory Tracking Control of Self-Driving Vehicle Based on Simulated Annealing Algorithm［J］.Journal of Chongqing University of Technology:Natural Science，2015(11):106-111.