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

王 浩，林 棻，張堯文

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

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

王 浩，林 棻，張堯文

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

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

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

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

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

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

1 車輛運動學模型

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

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

圖1 車輛運動學模型

由圖1中的運動學關系得:

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

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

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

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

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

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

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

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

式中:

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

其中，T為采樣時間。

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

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

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

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

2.1 預測方程

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

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

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

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

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

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

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

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

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

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

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

圖2 SA算法流程

3 仿真驗證

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

仿真試驗所采用的參考軌跡為直線軌跡與圓形軌跡的組合軌跡。分別在車輛參考縱向速度為5，10 m/s的情況下進行仿真試驗。直線軌跡跟蹤階段的參考前輪轉角為0 rad，圓形軌跡跟蹤階段的參考前輪轉角為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ù)為:預測時域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ù)降溫。

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

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

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

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

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

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

4 結束語

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

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

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

圖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］徐威，龔建偉，姜巖.無人駕駛車輛模型預測控制［M］.北京:北京理工大學出版社，2014.

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

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

［8］胡國龍，孫優(yōu)賢.預測控制進展及其應用研究［J］.電力系統(tǒng)及其自動化學報，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.

(責任編輯楊黎麗)

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

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

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

王浩，林棻，張堯文.基于模擬退火算法的無人駕駛車輛軌跡跟蹤控制［J］.重慶理工大學學報:自然科學版，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.