李年裕， 王志輝， 李兆耿， 田 鵬

(1. 裝甲兵工程學院控制工程系， 北京 100072； 2. 裝甲兵學院裝備運用系， 安徽 蚌埠 233051； 3. 63729部隊， 山西 太原 030027)

基于參數樣條的輪式車跟蹤控制策略

李年裕1， 王志輝1， 李兆耿2， 田 鵬3

(1. 裝甲兵工程學院控制工程系， 北京 100072； 2. 裝甲兵學院裝備運用系， 安徽 蚌埠 233051； 3. 63729部隊， 山西 太原 030027)

以某6×6無人輪式車為對象研究跟蹤控制策略，使其能夠自主跟蹤引導車的運動軌跡，并與引導車保持一定的安全距離。首先研究了采用參數樣條曲線建立柔性虛擬曲桿模型的方法，并提出了虛擬曲桿的彈性力控制策略；然后又引入一種軌跡跟蹤控制律，實現了對6×6無人輪式車跟蹤運動的雙閉環(huán)控制。最后通過Matlab仿真和實車試驗對該控制策略進行了驗證，結果表明：該控制策略能夠保證車輛穩(wěn)定行駛，并有效地提高了跟蹤精度。

輪式車;跟蹤控制; 參數樣條；虛擬曲桿

車輛跟蹤控制策略的研究具有重大的軍事和社會經濟意義。近年來，歐洲一直在研究“公路列車”技術[1]，美國為未來國防而研制的Demo-2和Demo-3[2]等都具有車輛自動跟蹤功能。

車輛自動跟蹤技術是智能交通系統(tǒng)(Intelligent Transport System, ITS)研究中的重要內容，它能使后車(Follower)在一定安全距離下自動緊密跟隨前車(Leader)的運動軌跡行駛，兼顧了交通效率和安全2個方面，具有廣闊的應用前景[3-4]?，F有的車輛自動跟蹤系統(tǒng)控制策略有3類：1)直接以前車為目標點進行跟蹤；2)以后車前方的一個虛擬點為目標點進行跟蹤；3)以前車后方的一個虛擬點為目標點進行跟蹤。

若直接以前車為目標點進行跟蹤，在彎道行駛時，后車因前車轉彎而立即轉彎，可能會撞向路邊或其他車輛。Pham[5]模仿人的駕駛習慣，定義了一個后車運動方向上的虛擬點作為目標點，該點與后車轉角有角度偏差，并與后車車頭方向中心保持固定距離，實驗結果表明：以后車前方的一個虛擬點為目標點進行跟蹤，無法實現準確跟蹤前車軌跡。Ng等[6-7]借鑒歐洲Chauffeur項目[8]中的電子拖桿跟蹤系統(tǒng)，提出了“虛擬拖桿模型(Virtual Trailer Link Model)”，即前、后車由2根鉸接的虛擬直拖桿相連，兩桿之間的角度可以變化，桿的當前構型則通過前車當前位姿與歷史位姿進行貝葉斯估計得出[7]，后車不直接跟蹤前車，而是跟蹤虛擬拖桿的末端，實驗結果表明：這種跟蹤策略有效地減小了跟蹤誤差。

在“虛擬拖桿”的基礎上，張曉媚等[9-10]提出了“柔性虛擬曲桿模型”：虛擬曲桿就是前、后兩車之間的軌跡，長度根據車距可伸縮，前車轉彎時虛桿對后車有延時力的作用。張曉媚等采用系列短圓弧來描述柔性虛桿的形態(tài)，與文獻[6-7]中虛擬拖桿模型相比，采用短圓弧建立的柔性虛桿模型跟蹤精度更高；但是短圓弧法未能很好地保證航向角連續(xù)，擬合前車軌跡的準確性也欠佳。

為此，筆者以某6×6無人輪式車(圖1)為控制對象，擬采用參數樣條曲線建立柔性虛擬曲桿模型，以期能夠更加準確、平滑地描述前車行駛過的軌跡，并在此基礎上引入虛擬彈性力的控制，再和一種軌跡跟蹤控制律共同作用，實現對6×6無人輪式車跟蹤運動的雙閉環(huán)控制，最后通過Matlab仿真和實車試驗對該控制策略進行驗證。

圖1 6×6無人輪式車

1 基于參數樣條的柔性虛桿模型

在跟蹤過程中，后車每一采樣時刻利用傳感器對前車的位置進行一次檢測，在全局坐標系下獲得一系列前車的軌跡點Pi(xi,yi,θi)(i=1,2,…,n)，其中：xi、yi為車輛位置橫、縱坐標；θi為車輛姿態(tài)角。在傳統(tǒng)的跟蹤方法中，后車直接以這些離散點為跟蹤目標，難以確保后車運動的平滑性和準確性。

前、后車跟蹤坐標系如圖2所示，在全局坐標系下分析后車對前車的跟蹤。為了使后車平滑地沿著前車軌跡運動，需要利用采樣得到的離散點準確擬合前車軌跡，根據柔性虛擬曲桿的思想[9-10]，需要利用一種合理的曲線來建立虛擬曲桿模型，由虛擬曲桿“拉動”著后車，使后車跟蹤著虛桿的末端運動。

圖2 前、后車跟蹤坐標系

前車軌跡擬合的方法一般有逼近法和插值法，由于逼近法所構建的曲線不一定會經過這些點，構造的曲線曲率可能小于車輛的轉彎半徑，使得無人車的行駛出現問題，因此采用插值法進行前車軌跡擬合。選取插值曲線有2種極端情況：1)在每對{Pi,Pi+1}(i=1,2,…,n-1)間進行線性插值，在插值點附近會出現拐折現象；2)采用高次多項式對軌跡點進行插值，當數據點增加時，多項式次數會增加，可能會產生震蕩，出現“龍格現象”。

為了避免分段線性插值和單一的高階多項式插值的缺陷，筆者考慮采用分段多項式函數進行插值，通過大多數應用發(fā)現：三次樣條曲線是折中選擇。因此，本文采用如下樣條曲線：對于前車軌跡在Pi(xi,yi)與Pi+1(xi+1,yi+1)兩點間的形態(tài)，采用三次樣條曲線進行插值[11]，同時為了保證樣條曲線在各點處均有一階導數連續(xù)，在每個插值點附加邊界條件。但是，傳統(tǒng)的樣條曲線擬合會出現如下2個問題：

1) 對于點集Pi(xi,yi)，使用樣條函數擬合曲線的必要條件是x1

2) 如果點集Pi(xi,yi)中存在(xi,yi)和(xj,yj)2個點，其中xi=xj，即有一段垂直于x軸的路徑，此時曲線的斜率為無窮大，無法使用y=f(x)的形式對此進行描述。

為此，可以采用參數樣條的方式來對軌跡點進行擬合，通過合理地選擇參數，可以使參數曲線對型值點的方向不敏感[12-13]。

對于點集Pi(xi,yi)，每2個點(xi,yi)、(xi+1,yi+1) (i=1,2,…,n-1)間都構成一段曲線lk，以參數方程的形式寫成如下的形式：

(1)

式中:0≤t≤ti，為參數，這里采用弦長作為t值,其中ti為連接兩點間直線段長度。

對于(xi,yi)、(xi+1,yi+1)間的某一點(x,y)，可以得到該點與本段起點的直線距離，即弦長

(2)

對于每一個點，弦長與點的位置對應，設tmax為本段最大弦長,則

(3)

在[0,tmax]中的每一個值，都可以找到一個對應點。而從(xi,yi)指向(xi+1,yi+1)的曲線中，弦長t總是單調增加的。

以lk段xk(t)方程為例，有

xk(0)=a1=xi，

(4)

(5)

(6)

(7)

于是可以得到

(8)

(9)

從而有

0≤t≤ti。

(10)

(11)

(12)

同理，可以解得yk(t)參數方程，從而建立點集Pi(xi,yi)的參數樣條插值曲線：

{lk}={xk(t),yk(t)}|k=1,2,…,n-1。

(13)

式(13)為全局坐標系下前車軌跡的解析表達式，描述了本文所使用的虛擬曲桿形態(tài)(圖3)。后車的航向角表達式為

(14)

圖3 虛擬曲桿形態(tài)

如此描述的軌跡保證了其上每一點的一階導數都是連續(xù)的，即保證了前車的軌跡和航向角也是連續(xù)的，為后車更加平滑、準確地跟蹤前車提供了保證。

通過弧長公式可以得到前后車之間的軌跡長度，即虛擬曲桿的長度為

(15)

為了便于車載工控機運算處理，應用牛頓-柯特斯公式對式(15)進行求解,得到

(16)

至此得到虛擬曲桿的解析表達式和虛擬曲桿的長度，該曲桿模型完全擬合了全局坐標系下前車的歷史軌跡。

2 基于曲桿虛擬彈性力的控制算法

實際上，當車輛跟蹤系統(tǒng)處于穩(wěn)定狀態(tài)時，后車應該與前車保持一定安全距離，緊密地跟隨前車運動，后車的線速度等于前車的線速度。類似于北京加長版的公交車(圖4)中間部分的彈性連接，在柔性虛桿模型的跟蹤系統(tǒng)中，改變前車的運動狀態(tài)會導致柔性虛桿伸縮彎曲，虛桿的內部作用力隨之變化，進而改變后車的運動狀態(tài)，使整個系統(tǒng)達到平衡。

圖4 北京加長版公交車

圖5 跟蹤系統(tǒng)模型

跟蹤系統(tǒng)模型如圖5所示，將中間的柔性虛擬曲桿應用彈簧的物理性質，彈性系數為k，則虛桿的虛擬力為

F=k(l-D)，

(17)

式中：l為虛桿的當前長度；

(18)

為動態(tài)安全距離，即虛桿的固有長度。其中:al、af分別為前、后車加速度;vl、vf分別為前、后車速度;D_static為前后車靜止時的安全距離。

式(18)可以理解為虛桿固有長度等于前、后車靜止時的安全距離、后車正常制動距離與后車反應時間行駛距離之和減去前車制動距離。

后車在虛桿虛擬力的作用下產生的加速度為

(19)

式中：mf為后車的質量。k、mf取值只要讓后車速度盡量平滑即可。

后車在下一采樣時間將要運動的距離為

(20)

式中：T為采樣時間。

后車的速度變?yōu)?/p>

vfnew=vf+afT。

(21)

后車的速度不能取負值，如果為負值，則直接賦值0。后車沿著虛擬曲桿將要運動到A′(xA′,yA′,θA′)，可以近似認為

(22)

至此得到后車在柔性虛擬曲桿拉動下的速度、加速度信息。在理想情況下，6×6無人輪式車可以按照該命令完成跟蹤運動。但是在實際系統(tǒng)中，6×6無人輪式車受自身運動特性的限制，同時也存在干擾信息，故需要針對6×6無人輪式車設計一個軌跡跟蹤控制律來收斂位姿誤差，本文采用控制律[14]如下：

(23)

李輝等[14]已經驗證該系統(tǒng)是穩(wěn)定的,式(23)中采用vr=(vf+vfnew)/2，ωr=ωfnew可以使后車運動更加平滑。

3 跟蹤策略仿真驗證

6×6無人輪式車跟蹤控制系統(tǒng)的整體框架如圖6所示，系統(tǒng)包括2個反饋環(huán)：外環(huán)是通過車載傳感器對前車進行位姿檢測和虛擬曲桿模型控制，使后車沿著前車軌跡運動；內環(huán)是6×6無人輪式車自身的位姿檢測與控制，確保后車能夠準確地跟蹤虛桿的末端。

圖6 6×6無人輪式車跟蹤控制系統(tǒng)整體框架

利用Matlab軟件對該控制系統(tǒng)進行仿真，為了檢驗本文控制算法的跟蹤精度，把前車的軌跡設置成一個類似“S”型的曲線，包含了直駛、右轉、左轉，并與文獻[10]中跟蹤方法進行對比。將前車的速度設為4 m/s，安全距離設為固定值6 m，前后車初始距離設為5 m，采樣周期設為0.1 s，控制參數取kx=5，ky=0.81，kα=2，kθ=1.8，k/mf=0.1。圖7為采用2種跟蹤跟蹤控制策略仿真得到的車輛跟蹤軌跡對比，可以看出：本文提出的跟蹤控制策略可使跟蹤誤差更小。圖8是2種跟蹤控制策略下后車航向角的變化曲線對比，可以看出：在本文的跟蹤控制策略下，后車航向角變化和前車基本一致，只是具有延時效應，這正符合正常車輛跟蹤的特征，說明基于三次參數方程建立的虛桿模型能夠更好地保證車輛的航向角連續(xù)。

圖7 2種跟蹤控制策略下車輛跟蹤軌跡對比

圖8 2種跟蹤控制策略下后車航向角的變化曲線對比

4 實車試驗

圖9為實車跟蹤試驗情形，其中：有人駕駛車作為前車；6×6無人輪式車作為后車。前后車裝有GPS慣性導航組合裝置，可記錄前后車的軌跡，由于架設了差分GPS，因此定位精度在0.1 m以下。

圖9 實車跟蹤試驗

4.1 “虛桿”生成試驗

后車保持不動，采用參數樣條法擬合前車運動的離散軌跡點，不斷地生成“虛桿”。在全局坐標系下記錄前車軌跡、離散軌跡點、“虛桿”曲線，如圖10所示，其中：藍色星點是后車獲取的前車運動的離散軌跡點；藍線是算法生成的樣條軌跡，亦即“虛桿”軌跡；紅線是前車的軌跡，由GPS慣導裝置直接記錄得到。

圖10 “虛桿”生成試驗

由圖10可見：由離散點生成虛桿的軌跡和前車GPS記錄的軌跡基本一致，說明基于參數樣條的虛桿模型能夠有效地描述前車的運動軌跡，且軌跡光滑，符合車輛運動的特點，為后車的跟蹤控制提供了必要條件。

4.2 跟蹤試驗

在6×6無人輪式車內裝有車載工控機，根據本文提出的跟蹤控制方法，采用C++builder設計了相應的控制軟件。圖11記錄的是跟蹤控制系統(tǒng)進行彎道試驗的結果，其中：紅色是虛桿的軌跡；藍色是后車跟蹤的軌跡。

圖11 跟蹤控制系統(tǒng)彎道試驗結果

從圖11可以看出：6×6無人輪式車有效地完成了對虛桿末端的跟蹤，運動路線基本上與虛桿軌跡相符，實際誤差在0.5 m范圍內；在跟蹤過程中，平臺起步平穩(wěn)，直行和轉向控制平穩(wěn)，并在預期點停車，達到了預期的跟蹤效果。

5 結論

本文針對6×6無人輪式車的軌跡跟蹤問題，研究了用參數樣條曲線描述柔性虛擬曲桿形態(tài)的方法，并在參數樣條建立的虛桿模型基礎上，給出了一種雙閉環(huán)的跟蹤控制策略。文中提出的采用參數樣條擬合曲線的方法能夠較好地擬合前車的軌跡點并保證了航向角的連續(xù)性，使后車的跟蹤運動更加平滑，同時解決了一般樣條函數擬合時對型值點方向不敏感的問題，能夠很好地描述各種形態(tài)的軌跡曲線。仿真結果和實車試驗結果表明：本文研究的跟蹤控制策略是可行的。

前車行駛過的軌跡對于后車一般來說是可行的，但本文沒有考慮出現運動障礙物等突發(fā)情況。下一步，針對可能出現的運動障礙物，后車需要能夠規(guī)劃新的運動軌跡，以做出相應的避障和應急控制。

[1] 湯水. “公路列車”:讓汽車自動駕駛[J]. 發(fā)明與創(chuàng)新，2011(5):35-36.

[2] Yang H W. Tracking Control of a Nonholonomic Mobile Robot by Hybrid Feedback and Neural Dynamics Techniques [D]. Guelph,Canada: University of Guelph, 2003.

[3] Mo Y K, Deng J. Fundamentals of Intelligent Public Transportation Dispatching Systems Planning[C]∥Proceedings of 2009 Second ISECS International Colloquium on Computing, Communication, Control, and Management(CCCM 2009). Piscataway, NJ, USA: IEEE Computer Society, 2009:41-44.

[4] Gu J S, Yi S, Yi K. Human-centered Design of a Stop and Go Vehicle Cruise Control[J]. IEEE International Journal of Automotive Technology, 2006, 7(5):619-624.

[5] Pham H.Development and Implementation of a Trajectory Planner and a Tracking Controller for an Autonomous Vehicle[D]. Singapore: School of Electrical and Electronic Engineering, Nanyang Technological University,2001.

[6] Ng T C. Autonomous Vehicle Following: a Virtual Trailer Link Approach[D]. Sinapore: Sinapore Institute of Manufacturing Technology, Nanyang Technological University, 2009.

[7] Ng T C, Adams M D, Guzman J I. Bayesian Estimation of Follower and Leader Vehicle Poses with a Virtual Trailer Link Model[J]. International Journal of Robotics Research, 2008, 27(1):91-106.

[8] Borodani P. Full Automatic Control for Trucks in a Tow-bar System[C]∥Proceedings of International Symposium on Advanced Vehicle Control. Piscataway, NJ, USA:IEEE, 2000:131-138.

[9] 張曉媚, 陳偉海, 劉敬猛. 基于柔性曲桿的車輛跟蹤算法設計與實現[J]. 北京航空航天大學學報，2011，37 (3):789-194.

[10] 陳偉海, 張曉媚, 呂章剛. 適用于獨立式車輛自動跟蹤的力延時柔性虛桿建模[J]. 機器人，2011，33(5):599-605.

[11] Howard T M, Kelly A. Trajectory Generation for Car-like Robots Using Cubic Curvature Polynomials[J]. Field and Service Robots, 2001, 3(2):19-23.

[12] 蘇步青. 關于三次參數樣條曲線的一些注記[J]. 應用數學學報, 1976, 1(1):49-58.

[13] 蘇步青. 關于三次參數樣條曲線的一些定理[J]. 應用數學學報, 1977, 1(1):50-54.

[14] 李輝, 李年裕, 張豫南. 一種無人地面車的軌跡跟蹤控制方法[J]. 裝甲兵工程學院學報, 2012, 10(5):80-84.

(責任編輯:尚彩娟)

Tracking Control Strategy for Wheeled Vehicles Based on Parameter Spline

LI Nian-yu1, WANG Zhi-hui1, LI Zhao-geng2, TIAN Peng3

(1. Department of Control Engineering, Academy of Armored Force Engineering, Beijing 100072, China； 2. Department of Equipment Application, Academy of Armored Forces, Bengbu 233051, China; 3. Troop No.63729 of PLA, Taiyuan 030027, China)

The paper investigates the tracking control strategy for 6×6 unmanned wheeled vehicle to make it track the trajectory of the leader vehicle precisely under the condition of keeping a safe distance. Firstly, the method of establishing a flexible virtual curved bar model with parameter spline curve is studied and a control strategy based on virtual curved bar elastic force is proposed. Then a trajectory tracking control law is introduced to realize the double closed loop control of 6×6 unmanned wheeled vehicle’s tracking motion. Finally the control strategy is verified by Matlab simulation and real vehicle test. The results show that the vehicle tracking control strategy can guarantee the stability of the vehicle motion and can effectively improve the tracking precision.

wheeled vehicle; tracking control; parameter spline; virtual curved bar

1672-1497(2015)04-0062-06

2015-04-08

軍隊科研計劃項目

李年裕(1968-)，男，副教授，博士。

TP275

A

10.3969/j.issn.1672-1497.2015.04.013