自動駕駛車輛軌跡跟蹤控制算法研究

宋吉超 周成才 宋卓宇

摘要：在智能車輛的自動駕駛系統(tǒng)中，需要采用合適的控制算法，控制自動駕駛車輛沿預(yù)先規(guī)劃的軌跡運(yùn)行。本文基于車輛的運(yùn)動學(xué)模型，分析了自動駕駛車輛軌跡跟蹤控制的純跟蹤算法（Pure Pursuit）。通過純跟蹤算法控制車輛后輪的運(yùn)動軌跡，實(shí)現(xiàn)了調(diào)整車輛當(dāng)前位置與規(guī)劃軌跡之間的偏差，從而使車輛達(dá)到了跟蹤預(yù)設(shè)軌跡的效果。并且分析了車輛跟蹤過程中，速度調(diào)節(jié)系數(shù)對跟蹤效果的影響，驗(yàn)證了純跟蹤算法的有效性與精確性。

Abstract： In the automatic driving system of intelligent vehicle， it is necessary to adopt appropriate control algorithm to control the automatic driving vehicle along the pre-planned trajectory line. Based on the vehicle kinematics model， this paper analyzes Pure Pursuit algorithm for trajectory tracking control of autonomous vehicle. By using the Pure Pursuit algorithm for trajectory tracking control of autonomous vehicle. By using the Pure Pursuit algorithm to control the trajectory of the rear wheel of the vehicle， the deviation between the current position of the vehicle and the planned trajectory can be adjusted， so that the vehicle can track the preset trajectory. In addition， the influence of the speed adjustment coefficient on the tracking effect in the process of vehicle tracking is analyzed， and the effectiveness and accuracy of the Pure Pursuit algorithm are verified.

關(guān)鍵詞：自動駕駛;軌跡跟蹤;純跟蹤算法;物聯(lián)網(wǎng)技術(shù)

Key words： automatic driving;trajectory tracking;pure pursuit algorithm;internet of things technology

中圖分類號：U461.1? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?文獻(xiàn)標(biāo)識碼：A? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 文章編號：1674-957X（2021）23-0215-02

0? 引言

智能車輛的自動駕駛系統(tǒng)是一項(xiàng)集成多學(xué)科復(fù)雜技術(shù)，該系統(tǒng)通常采用定位-感知-規(guī)劃-決策-控制這一技術(shù)路線來完成相關(guān)任務(wù)。自誕生以來，自動駕駛被認(rèn)為是解決道路安全事故、交通擁堵等問題的良好方案。特別是近幾年，隨著新能源汽車及物聯(lián)網(wǎng)技術(shù)的普及，自動駕駛車輛成為汽車行業(yè)大力發(fā)展的新方向。毫不夸張的說，具有自動駕駛功能的智能車輛是未來工業(yè)界最大的風(fēng)口之一。

1? 車輛運(yùn)動學(xué)模型

要控制車輛的運(yùn)動，首先要對車輛的運(yùn)動建立數(shù)字化模型，模型建立的越準(zhǔn)確，對車輛運(yùn)動的描述越準(zhǔn)確，對車輛的跟蹤控制的效果就越好。除了真實(shí)反映車輛特性外，建立的模型也應(yīng)該盡可能的簡單易用。車輛模型一般分為運(yùn)動學(xué)和動力學(xué)模型。其中車輛運(yùn)動學(xué)模型把車輛完全視為剛體，主要考慮車輛的位姿、速度、前輪轉(zhuǎn)角等的關(guān)系，不考慮任何力的影響。車輛運(yùn)動學(xué)模型常采用二自由度自行車模型，基于如下假設(shè)：①不考慮車輛在垂直方向的運(yùn)動，即假設(shè)車輛的運(yùn)動是一個二維平面上的運(yùn)動。②假設(shè)車輛左右側(cè)輪胎在任意時刻都擁有相同的轉(zhuǎn)向角度和轉(zhuǎn)速;這樣車輛的左右兩個輪胎的運(yùn)動可以合并為一個輪胎來描述。③假設(shè)車輛行駛速度變化緩慢，忽略前后軸載荷的轉(zhuǎn)移。④假設(shè)車身和懸架系統(tǒng)都是剛性系統(tǒng)。⑤假設(shè)車輛的運(yùn)動和轉(zhuǎn)向是由前輪驅(qū)動（front-wheel-only）的。

則簡化后的車輛二自由度模型如圖1所示。

2? 純跟蹤算法

2.1 純跟蹤算法概述

純跟蹤控制算法（Pure Pursuit）是一種典型的橫向控制方法，最早由 R. Wallace 在1985年提出，該方法的本質(zhì)是基于幾何原理對車輛的運(yùn)動軌跡進(jìn)行控制，對外界的魯棒性較好，即受外界各種干擾的影響較小。

該算法的思想：基于當(dāng)前車輛后輪中心位置，在參考路徑上向ld（自定義）的距離匹配一個預(yù)瞄點(diǎn)，假設(shè)車輛后輪中心點(diǎn)可以按照一定的轉(zhuǎn)彎半徑行駛抵達(dá)該預(yù)瞄點(diǎn)，然后根據(jù)預(yù)瞄距離ld、車輛的轉(zhuǎn)彎半徑、記憶車輛坐標(biāo)系下預(yù)瞄點(diǎn)的朝向角之間的幾何關(guān)系來確定前輪轉(zhuǎn)角。純跟蹤算法示意圖如圖2所示。

2.2 純跟蹤算法原理

為了使車輛后輪跟蹤圓弧虛線軌跡到達(dá)C點(diǎn)，已知幾何關(guān)系：■

■

則在△AOC中，需要滿足的正弦定理關(guān)系為：

■

化簡上式，得到：■

因?yàn)槎杂啥溶囕v運(yùn)動學(xué)模型采用阿克曼轉(zhuǎn)向，前輪的方向始終是后輪行駛圓弧的切線方向，則前輪轉(zhuǎn)角關(guān)系滿足則可得到：■

另外，定義橫向誤差為車輛當(dāng)前姿態(tài)和預(yù)瞄點(diǎn)在橫向上的誤差：■

聯(lián)立上式，并考慮小角度假設(shè)，可以得到：

■

純跟蹤本質(zhì)上是一個P控制器，那么跟蹤效果將由ld決定，通常定義ld為關(guān)于速度v的一次多項(xiàng)式：

■

式中：

ld0：初始時刻的預(yù)瞄距離，為常數(shù)。

kv：速度調(diào)節(jié)系數(shù)。

那么純追蹤算法控制器的調(diào)整就變成對速度調(diào)節(jié)系數(shù)kv的調(diào)整。

2.3 純跟蹤算法驗(yàn)證

為驗(yàn)證純跟蹤算法的跟蹤效果，現(xiàn)采用matlab工具進(jìn)行編程，對車輛二自由度的運(yùn)動學(xué)模型及純跟蹤算法進(jìn)行仿真驗(yàn)證。

在matlab編程中，車輛規(guī)劃軌跡通過已有數(shù)據(jù)集導(dǎo)入，車輛的軸距L=2.9m，預(yù)瞄距離值ld0=1m。

對純跟蹤算法效果驗(yàn)證的結(jié)果如圖3所示，由圖2可知，純跟蹤法可以有效的使車輛的實(shí)際軌跡，跟蹤到車輛的規(guī)劃軌跡，但不同的速度調(diào)節(jié)系數(shù)將產(chǎn)生不同的效果。

當(dāng)速度調(diào)節(jié)系數(shù)為0.05時，車輛快速的跟蹤到軌跡，但在跟蹤的初期圍繞著規(guī)劃軌跡出現(xiàn)了明顯的上下震蕩，之后跟蹤精度較為精確，完成全程軌跡跟蹤。

當(dāng)速度調(diào)節(jié)系數(shù)為0.5時，隨著系數(shù)的增大，可有效緩解初始階段的震蕩，但響應(yīng)速度變慢。

在隨后的軌跡跟蹤過程中，特別是在非直線工況下，跟蹤誤差明顯增大。同時系數(shù)的增大預(yù)瞄距離也增大，存在更長距離的末端規(guī)劃軌跡無法得到有效跟蹤，算法即停止運(yùn)行。

3? 結(jié)論

由上述分析可知，基于運(yùn)動學(xué)模型的純跟蹤算法能夠有效控制車輛沿規(guī)劃的目標(biāo)軌跡行駛。在軌跡跟蹤控制過程中，調(diào)節(jié)系數(shù)的選取將會對軌跡跟蹤的精度產(chǎn)生較大的影響，因此如何確定最優(yōu)的調(diào)節(jié)系數(shù)，是軌跡跟蹤控制的關(guān)鍵所在，可進(jìn)一步結(jié)合智能控制理論，優(yōu)化軌跡跟蹤效果。同時，由于運(yùn)動學(xué)模型的限制，純跟蹤算法無法適用于車輛高速運(yùn)動的情況。而針對車輛高速運(yùn)動的情況，需要基于車輛動力學(xué)模型，開展其他控制算法的研究。

參考文獻(xiàn)：

[1]陳樂.自動泊車路徑規(guī)劃算法及CarSim仿真研究[D].長安大學(xué)，2020.

[2]王軍雷，李健明，吳學(xué)易.基于預(yù)瞄的智能車位姿補(bǔ)償軌跡跟蹤算法[J].汽車實(shí)用技術(shù)，2020，45（21）：40-45.

[3]鄧濤，李鑫.智能車輛橫縱向運(yùn)動綜合控制方法研究[J].重慶交通大學(xué)學(xué)報（自然科學(xué)版），2021，40（04）：133-140.