基于PreScan/CarSim/Simulink/ROS的路徑跟隨控制仿真

周婷婷　王剛　鄧嘉釩　馮子睿

摘 要：路徑跟隨可控制AGV沿著參考軌跡行駛，提高行駛過程中的可行性和穩(wěn)定性，是智能駕駛核心技術(shù)之一。文章主要研究使用Prescan、Carsim、Simulink和ROS系統(tǒng)搭建AGV路徑跟隨控制模型，運(yùn)用基于幾何模型的Stanley算法來實(shí)現(xiàn)路徑跟隨的聯(lián)合仿真測試。最后使用樹莓派搭建硬件在環(huán)的仿真模型，聯(lián)合仿真驗(yàn)證了Stanley算法的可行性，該方法可快速驗(yàn)證和部署基于ROS系統(tǒng)的路徑跟隨控制方法，為后續(xù)研究奠定技術(shù)基礎(chǔ)。

關(guān)鍵詞：AGV 路徑跟隨 Stanley算法 聯(lián)合仿真

1 引言

近些年，隨著AGV的廣泛應(yīng)用，基于ROS系統(tǒng)的智能駕駛控制技術(shù)也在逐漸興起[1]。路徑跟隨作為智能駕駛的先決條件，國內(nèi)外許多學(xué)者取得了不菲的成果，對智能駕駛的發(fā)展起到了極大的促進(jìn)作用[2-4]。

呂文杰等[5]因?yàn)槁窂礁S算法中的純追蹤算法前視距離受到車速、位置變化的影響，提出將模糊控制器與算法結(jié)合，提高了跟蹤的效果?？馨l(fā)榮等[6]提出了一種雙反饋的MPC控制與轉(zhuǎn)角補(bǔ)償相結(jié)合的路徑跟隨控制器，使用Prescan與Simulink仿真證明，該控制器相比傳統(tǒng)MPC控制縮小了求解時(shí)間，同時(shí)航向誤差與橫向誤差也有大幅度的減小。Wang等[7]提出了一種改進(jìn)的Stanley控制器（IMP-ST），該控制器采用多種群遺傳算法（MPGA）對IMP-ST的參數(shù)進(jìn)行優(yōu)化，獲得更好的跟蹤性能。與傳統(tǒng)的Stanley控制器相比，改進(jìn)的Stanley控制器可以降低橫向跟蹤誤差和提高轉(zhuǎn)向性能，具有良好的控制穩(wěn)定性。

路徑跟隨因?yàn)椴恍枰藶楦深A(yù)可自主按照預(yù)設(shè)路徑來移動(dòng)，按照這一特性可廣泛用于需要按一定路徑運(yùn)動(dòng)的物體上。如今，路徑跟隨已廣泛運(yùn)用于AGV、餐飲機(jī)器人、無人機(jī)、農(nóng)機(jī)、船舶等方面[8-10]。本文主要目的是研究低速狀態(tài)下，如餐廳、酒店、前臺等場景，控制AGV實(shí)現(xiàn)路徑跟隨功能。重點(diǎn)基于幾何原理的Stanley控制算法完成對車輛的控制，搭建基于Prescan、Carsim、Simulink和ROS系統(tǒng)的聯(lián)合仿真模型。

2 車輛路徑跟隨控制算法

Stanley算法是實(shí)現(xiàn)路徑跟隨的一種基礎(chǔ)控制算法，與純追蹤算法一樣是基于幾何原理計(jì)算預(yù)瞄點(diǎn)，進(jìn)而得出前輪轉(zhuǎn)向角控制車輛跟蹤預(yù)定路徑。對于Stanley算法的幾何模型可用圖1來表示。

其中（px，py）點(diǎn)為當(dāng)前狀態(tài)下車輛前軸中心到所需跟蹤軌跡的最近點(diǎn)；e為橫向誤差，即前軸中心到軌跡最近點(diǎn)的距離；θe為航向誤差，即前軸中心離軌跡的最近點(diǎn)的切線方向平行線與車輛軸向的夾角；δe是前輪線速度方向與車身航向的夾角；δ角為前輪要行駛到預(yù)瞄點(diǎn)所需的轉(zhuǎn)角；L為車輛的軸距。

由圖1可以看出，前輪轉(zhuǎn)角δ可由航向誤差和最近點(diǎn)切線與車輛軸線的夾角相加求得

如果不考慮橫向誤差，要保持車輛沿給定軌跡運(yùn)動(dòng)，則前輪轉(zhuǎn)角需要時(shí)刻保持與給定軌跡上點(diǎn)的切線方向一致，即前輪轉(zhuǎn)角應(yīng)為θe。

如果不考慮航向誤差，要保持車輛沿給定軌跡運(yùn)動(dòng)，則前輪轉(zhuǎn)角需要消除橫向誤差，前輪轉(zhuǎn)角應(yīng)為δe。

由圖可以看出

為保證橫向誤差收斂于0，可令。則

當(dāng)ke足夠小時(shí)，可認(rèn)為v（t）與d（t）相等所以可認(rèn)為

因?yàn)闉楹较蛘`差，所以可以用當(dāng)前車輛狀態(tài)航向角與預(yù)瞄點(diǎn)的航向角之間的差值求得，即

為當(dāng)前狀態(tài)車輛航向角，為車輛在預(yù)瞄點(diǎn)時(shí)的航向角。

將式（4）與式（5）帶入式（1）可得式（6）。

即Stanley算法按照行駛軌跡計(jì)算的實(shí)時(shí)前輪轉(zhuǎn)角。

3 聯(lián)合仿真模型

本次設(shè)計(jì)使用了四款仿真軟件，分別是Prescan場景建模軟件、Carsim車輛動(dòng)力學(xué)仿真軟件、Matlab下的Simulink軟件以及ROS系統(tǒng)，路徑跟隨聯(lián)合仿真閉環(huán)流程圖如圖2。

在Prescan、Simulink和Carsim的聯(lián)合仿真中，Prescan主要提供仿真的場景以及駕駛員模型。Carsim提供車輛動(dòng)力學(xué)模型，Simulink主要為仿真提供聯(lián)合的環(huán)境，進(jìn)行信息的處理，以及搭建仿真控制算法模塊等。

3.1 Prescan仿真場景模型建立

如圖3為Simulink生成的Prescan車輛信息輸出GPS慣導(dǎo)模塊，由圖３可看出由Prescan提供的車輛模型輸入信息經(jīng)過模塊處理之后輸出了Prescan坐標(biāo)系下車輛對于坐標(biāo)系x軸、y軸以及z軸的位置信息，繞Prescan x軸、y軸、z軸的轉(zhuǎn)角，GPS提供的經(jīng)度/緯度與水平高度的數(shù)值以及目標(biāo)車輛的絕對速度、航向角和角速度。路徑跟隨仿真需要錄入軌跡的位置，用來計(jì)算最近點(diǎn)以及畫圖時(shí)進(jìn)行跟隨效果的比較。所以要通過GPS來進(jìn)行采點(diǎn)記錄軌跡點(diǎn)，考慮到現(xiàn)實(shí)中會出現(xiàn)GPS信號弱，無法感知的情況，可在車上加一個(gè)雷達(dá)傳感器，在信號弱的情況下代替GPS采點(diǎn)工作。

駕駛員模型如圖４所示，其輸入了車輛x、y的位置信息、繞y軸旋轉(zhuǎn)的橫擺角、車輛的初速度、期望速度以及本車當(dāng)前速度（velocity）。沿x軸的分速度，沿y軸的分速度，航向角等數(shù)據(jù)，同時(shí)輸出了方向盤轉(zhuǎn)角（steerwheel），節(jié)氣門開度（throttle），制動(dòng)壓力（break pressure）等控制車輛的信息。在聯(lián)合好Carsim之后，駕駛員模型輸出的信息輸入到Carsim生成的動(dòng)力學(xué)模型中，即可控制車輛的仿真。

3.2 Carsim模型

在Prescan的動(dòng)力學(xué)模型中選擇用戶自定義，導(dǎo)入生成的FMU文件，并選擇與Carsim關(guān)聯(lián)，完成后打開Simulink可生成Carsim動(dòng)力學(xué)模塊如圖５。

圖5即是Carsim車輛參數(shù)信息、輸入輸出信息等設(shè)置之后按照前后關(guān)系生成的模塊，其內(nèi)部如下圖6。

為完成與Simulink聯(lián)合，需在Simulink中選擇FMU模塊，并把FMU文件導(dǎo)入。然后根據(jù)輸入輸出關(guān)系把接線連好，F(xiàn)MU模塊的節(jié)氣門開度，制動(dòng)踏板壓力由駕駛員模型輸入。方向盤轉(zhuǎn)角輸入控制算法計(jì)算得到的值。輸出為3部分，輸出部分分別輸入到駕駛員模型中，實(shí)現(xiàn)駕駛員模型的在環(huán)仿真。至此Prescan、Carsim以及Simulink的仿真模型建立完成。

3.3 Simulink控制模型

根據(jù)Stanley控制算法公式（1）-（6），第一步需要由當(dāng)前車輛前輪軸心位置與航向角計(jì)算預(yù)瞄點(diǎn)，如圖７所示。

之后利用求解得到的預(yù)瞄點(diǎn)通過Matlab編寫的最近點(diǎn)求解程序計(jì)算出離前輪軸心的最近軌跡點(diǎn)，如圖8所示。

把輸出的車輛速度、航向角、最近軌跡點(diǎn)坐標(biāo)、預(yù)瞄點(diǎn)坐標(biāo)以及當(dāng)前狀態(tài)前軸中心點(diǎn)坐標(biāo)輸入到根據(jù)公式（5）與公式（6）搭建的計(jì)算前輪轉(zhuǎn)角模塊中，運(yùn)算之后就可得出前輪轉(zhuǎn)角的值。根據(jù)仿真過程中車輛的實(shí)時(shí)變化，前輪轉(zhuǎn)角也在實(shí)時(shí)變化。

所有的模塊搭建完成后根據(jù)輸入輸出關(guān)系將模塊輸入輸出接口連線，把所有的模塊聯(lián)合起來并進(jìn)行位置拖動(dòng)處理，將模塊按照合理的位置放置完成得到總的Simulink仿真模塊如圖９。

3.4 ROS系統(tǒng)建模

為進(jìn)一步驗(yàn)證算法的可行性，可在開源的Linux系統(tǒng)中，運(yùn)用Ubuntu和ROS與Matlab進(jìn)行聯(lián)合，實(shí)現(xiàn)使用樹莓派的硬件在環(huán)的仿真操作。在本次設(shè)計(jì)中主要對轉(zhuǎn)角信息進(jìn)行樹莓派的使用替換。首先使用Simulink專業(yè)模型庫中的ROS工具箱搭建ROS與Matlab信息交換模型如圖10、圖11。

在這一過程中轉(zhuǎn)角運(yùn)算模型與整體模型進(jìn)行了分離，將求轉(zhuǎn)角需要的預(yù)瞄距離以及夾角與ROS工具箱中的Blank Message（空白信息）模塊經(jīng)過Bus Assignment（總線分配）模塊生成ROS信號，經(jīng)由ROS信息發(fā)布模塊發(fā)布出來。之后在計(jì)算轉(zhuǎn)角的模型中運(yùn)用Subscribe（訂閱）模塊接收信號進(jìn)行前輪轉(zhuǎn)角的計(jì)算，計(jì)算得出的前輪轉(zhuǎn)角再生成ROS信號發(fā)布。最后在整體模塊接收該信號，完成方向盤轉(zhuǎn)角計(jì)算，實(shí)現(xiàn)閉環(huán)的硬件仿真操作。

為完成這一操作需要將Matlab與ROS在同一局域網(wǎng)上實(shí)現(xiàn)通訊。在這個(gè)過程中，第一步需要知道樹莓派的IP地址，可以在樹莓派終端輸入ifconfig命令，即可看見IP地址。之后在ROS中的bashrc文件中把地址添加進(jìn)去。在Matlab命令窗口輸入Steven命令把ROS地址作為命令值。之后輸入rosinit命令可看到matlab與樹莓派已經(jīng)完成通訊如圖12。

完成上述操作后，對Simulink中的轉(zhuǎn)角計(jì)算模型進(jìn)行設(shè)置，將前輪轉(zhuǎn)角計(jì)算模型生成為C++代碼，并將該代碼燒錄到ROS的工作空間里面，至此完成所有模型的搭建與聯(lián)合。

4 仿真結(jié)果

在搭建好模型之后，運(yùn)行仿真計(jì)算如下：

仿真過程中應(yīng)保證所有仿真軟件都處于開啟狀態(tài)，仿真時(shí)根據(jù)Prescan里面設(shè)置的視角，以及算法模塊里的擬合曲線畫圖模塊，應(yīng)出現(xiàn)四個(gè)仿真動(dòng)畫界面：駕駛員視角界面、自由視角界面、車后俯視角界面以及擬合曲線路徑跟隨界面如圖13。動(dòng)畫中可觀察到仿真時(shí)車輛的行駛狀態(tài)，方便對路徑跟隨仿真效果進(jìn)行分析。

根據(jù)路徑跟隨擬合曲線的實(shí)時(shí)動(dòng)態(tài)變化，車輛在行駛進(jìn)入彎道時(shí)會出現(xiàn)跟蹤誤差?？偟膩碚f，仿真效果良好，車輛的平順性、穩(wěn)定性也能夠達(dá)到要求。

5 結(jié)論

純追蹤算法與Stanley算法的控制原理類似，因其基于幾何模型計(jì)算預(yù)瞄點(diǎn)的特性，跟蹤誤差都與彎道曲率有關(guān)。根據(jù)Stanley算法推導(dǎo)過程，以及對模塊分析可以推算出該情況的出現(xiàn)主要與用前輪轉(zhuǎn)角速度計(jì)算橫向跟蹤誤差時(shí)所設(shè)的增益k有關(guān)。經(jīng)過對控制算法模型里面增益參數(shù)的修改以及仿真嘗試，由結(jié)果分析發(fā)現(xiàn)增益愈大跟蹤誤差愈大但穩(wěn)定性與平順性隨之下降。反之增益越小跟蹤誤差越小，但穩(wěn)定性與平順性相應(yīng)提高。

基金項(xiàng)目：國家自然科學(xué)基金（12202112）、廣西自然科學(xué)基金（2021JJB160015）和廣西制造系統(tǒng)與先進(jìn)制造技術(shù)重點(diǎn)實(shí)驗(yàn)室主任項(xiàng)目（22-35-4-S006）、國家級大學(xué)生創(chuàng)新訓(xùn)練項(xiàng)目“智能駕駛車輛車道偏離預(yù)警系統(tǒng)”資助（202310595057）。

參考文獻(xiàn)：

[1]王婷. 我國汽車行業(yè)發(fā)展現(xiàn)狀與趨勢[J]. 企業(yè)改革與管理，2020（16）： 214-215.

[2]黎宇科，劉宇. 國外智能網(wǎng)聯(lián)汽車發(fā)展現(xiàn)狀及啟示[J]. 汽車工業(yè)研究，2016（10）：30-36.

[3]王羽，曲婕. 智能駕駛發(fā)展現(xiàn)狀及對地方開放智能駕駛車輛測試道路的建議[J]. 汽車工業(yè)研究，2018 （11）： 4-11.

[4]鄧歡，羅山. 車道偏離決策算法設(shè)計(jì)與仿真[J]. 山西電子技術(shù)，2021（06）： 37-39.

[5]呂文杰，馬戎，李歲勞，付維平. 基于純追蹤模型的路徑跟蹤改進(jìn)算法[J]. 測控技術(shù)，2011，30（07）： 93-96.

[6]寇發(fā)榮，鄭文博，張新乾，楊慧杰，賀嘉杰. 采用狀態(tài)擴(kuò)展MPC與轉(zhuǎn)角補(bǔ)償?shù)臒o人車路徑跟蹤控制[J]. 機(jī)械科學(xué)與技術(shù)，2020（1）： 1-8.

[7]Wang Liang，Zhai Zhiqiang，Zhu Zhongxiang，Mao Enrong. Path pursuit Control of an Auton-omous Tractor Using Improved Stanley Controller Optimized with Multiple-Population Genetic Algorithm[J]. Actuators，2022，11（1）： 2-6.

[8]修彩靖，陳慧. 無人駕駛車路徑跟蹤控制研究[J]. 計(jì)算機(jī)工程，2012，38（10）： 112-117.

[9]陸一弘. Prescan/Carsim/Simulink聯(lián)合仿真方法研究[J]. 佳木斯大學(xué)學(xué)報(bào)（自然科學(xué)版），2020，38（05）： 118-121.

[10]張紳. 基于Carsim的整車動(dòng)力性建模[J]. 時(shí)代汽車，2019（09）： 44-49.