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

周婷婷　王剛　鄧嘉釩　馮子睿

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

關鍵詞：AGV 路徑跟隨 Stanley算法 聯合仿真

1 引言

近些年，隨著AGV的廣泛應用，基于ROS系統的智能駕駛控制技術也在逐漸興起[1]。路徑跟隨作為智能駕駛的先決條件，國內外許多學者取得了不菲的成果，對智能駕駛的發展起到了極大的促進作用[2-4]。

呂文杰等[5]因為路徑跟隨算法中的純追蹤算法前視距離受到車速、位置變化的影響，提出將模糊控制器與算法結合，提高了跟蹤的效果。寇發榮等[6]提出了一種雙反饋的MPC控制與轉角補償相結合的路徑跟隨控制器，使用Prescan與Simulink仿真證明，該控制器相比傳統MPC控制縮小了求解時間，同時航向誤差與橫向誤差也有大幅度的減小。Wang等[7]提出了一種改進的Stanley控制器（IMP-ST），該控制器采用多種群遺傳算法（MPGA）對IMP-ST的參數進行優化，獲得更好的跟蹤性能。與傳統的Stanley控制器相比，改進的Stanley控制器可以降低橫向跟蹤誤差和提高轉向性能，具有良好的控制穩定性。

路徑跟隨因為不需要人為干預可自主按照預設路徑來移動，按照這一特性可廣泛用于需要按一定路徑運動的物體上。如今，路徑跟隨已廣泛運用于AGV、餐飲機器人、無人機、農機、船舶等方面[8-10]。本文主要目的是研究低速狀態下，如餐廳、酒店、前臺等場景，控制AGV實現路徑跟隨功能。重點基于幾何原理的Stanley控制算法完成對車輛的控制，搭建基于Prescan、Carsim、Simulink和ROS系統的聯合仿真模型。

2 車輛路徑跟隨控制算法

Stanley算法是實現路徑跟隨的一種基礎控制算法，與純追蹤算法一樣是基于幾何原理計算預瞄點，進而得出前輪轉向角控制車輛跟蹤預定路徑。對于Stanley算法的幾何模型可用圖1來表示。

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

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

如果不考慮橫向誤差，要保持車輛沿給定軌跡運動，則前輪轉角需要時刻保持與給定軌跡上點的切線方向一致，即前輪轉角應為θe。

如果不考慮航向誤差，要保持車輛沿給定軌跡運動，則前輪轉角需要消除橫向誤差，前輪轉角應為δe。

由圖可以看出

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

當ke足夠小時，可認為v（t）與d（t）相等所以可認為

因為為航向誤差，所以可以用當前車輛狀態航向角與預瞄點的航向角之間的差值求得，即

為當前狀態車輛航向角，為車輛在預瞄點時的航向角。

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

即Stanley算法按照行駛軌跡計算的實時前輪轉角。

3 聯合仿真模型

本次設計使用了四款仿真軟件，分別是Prescan場景建模軟件、Carsim車輛動力學仿真軟件、Matlab下的Simulink軟件以及ROS系統，路徑跟隨聯合仿真閉環流程圖如圖2。

在Prescan、Simulink和Carsim的聯合仿真中，Prescan主要提供仿真的場景以及駕駛員模型。Carsim提供車輛動力學模型，Simulink主要為仿真提供聯合的環境，進行信息的處理，以及搭建仿真控制算法模塊等。

3.1 Prescan仿真場景模型建立

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

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

3.2 Carsim模型

在Prescan的動力學模型中選擇用戶自定義，導入生成的FMU文件，并選擇與Carsim關聯，完成后打開Simulink可生成Carsim動力學模塊如圖５。

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

為完成與Simulink聯合，需在Simulink中選擇FMU模塊，并把FMU文件導入。然后根據輸入輸出關系把接線連好，FMU模塊的節氣門開度，制動踏板壓力由駕駛員模型輸入。方向盤轉角輸入控制算法計算得到的值。輸出為3部分，輸出部分分別輸入到駕駛員模型中，實現駕駛員模型的在環仿真。至此Prescan、Carsim以及Simulink的仿真模型建立完成。

3.3 Simulink控制模型

根據Stanley控制算法公式（1）-（6），第一步需要由當前車輛前輪軸心位置與航向角計算預瞄點，如圖７所示。

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

把輸出的車輛速度、航向角、最近軌跡點坐標、預瞄點坐標以及當前狀態前軸中心點坐標輸入到根據公式（5）與公式（6）搭建的計算前輪轉角模塊中，運算之后就可得出前輪轉角的值。根據仿真過程中車輛的實時變化，前輪轉角也在實時變化。

所有的模塊搭建完成后根據輸入輸出關系將模塊輸入輸出接口連線，把所有的模塊聯合起來并進行位置拖動處理，將模塊按照合理的位置放置完成得到總的Simulink仿真模塊如圖９。

3.4 ROS系統建模

為進一步驗證算法的可行性，可在開源的Linux系統中，運用Ubuntu和ROS與Matlab進行聯合，實現使用樹莓派的硬件在環的仿真操作。在本次設計中主要對轉角信息進行樹莓派的使用替換。首先使用Simulink專業模型庫中的ROS工具箱搭建ROS與Matlab信息交換模型如圖10、圖11。

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

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

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

4 仿真結果

在搭建好模型之后，運行仿真計算如下：

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

根據路徑跟隨擬合曲線的實時動態變化，車輛在行駛進入彎道時會出現跟蹤誤差。總的來說，仿真效果良好，車輛的平順性、穩定性也能夠達到要求。

5 結論

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

基金項目：國家自然科學基金（12202112）、廣西自然科學基金（2021JJB160015）和廣西制造系統與先進制造技術重點實驗室主任項目（22-35-4-S006）、國家級大學生創新訓練項目“智能駕駛車輛車道偏離預警系統”資助（202310595057）。

參考文獻：

[1]王婷. 我國汽車行業發展現狀與趨勢[J]. 企業改革與管理，2020（16）： 214-215.

[2]黎宇科，劉宇. 國外智能網聯汽車發展現狀及啟示[J]. 汽車工業研究，2016（10）：30-36.

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

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

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

[6]寇發榮，鄭文博，張新乾，楊慧杰，賀嘉杰. 采用狀態擴展MPC與轉角補償的無人車路徑跟蹤控制[J]. 機械科學與技術，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]. 計算機工程，2012，38（10）： 112-117.

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

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