基于改進人工勢場法的車輛路徑規劃與跟蹤控制

海振洋　王健　李大升　楊智勇　牟思凱　王云靖　鄧歡

摘要：針對采用傳統人工勢場法進行車輛路徑規劃時易造成局部極小值與目標不可達的問題，通過改變斥力函數并增加車道邊界約束條件函數的方式改進傳統人工勢場法，進行車輛路徑規劃。采用模型預測控制（model predictive control，MPC）算法跟蹤控制改進人工勢場法生成的規劃路徑，采用軟件CarSim與Simulink搭建聯合仿真模型對路徑跟蹤效果進行仿真試驗。結果表明：改進人工勢場法路徑規劃合理有效；跟蹤路徑與規劃路徑的橫向誤差小于0.4 m。改進人工勢場法和MPC算法應用于無人駕駛車輛的路徑規劃與跟蹤控制具有可行性。

關鍵詞：人工勢場法；MPC算法；路徑規劃；跟蹤控制；聯合仿真

中圖分類號：U46;TP391.9文獻標志碼：A文章編號：1672-0032（2023）02-0001-07

引用格式：海振洋，王健，李大升，等.基于改進人工勢場法的車輛路徑規劃與跟蹤控制［J］.山東交通學院學報，2023，31（2）：1-7.

HAI Zhenyang， WANG Jian， LI Dasheng， et al.Vehicle path planning and tracking control based on improved artificial potential field method［J］.Journal of Shandong Jiaotong University，2023，31（2）：1-7.

0 引言

無人駕駛車輛是汽車工業進步的新興方向，經過多年發展，在技術構架及關鍵技術上形成了“三橫兩縱”的格局，智能決策技術是車輛關鍵技術三橫中的重要內容之一[1]，車輛路徑規劃與跟蹤控制是智能決策的難點。路徑規劃的作用是使車輛自動躲避障礙物，在復雜的交通路況中找到安全、準確的路線抵達目的地。路徑規劃算法有自然啟發算法[2]、神經網絡算法[3]和人工勢場法[4]等。人工勢場法計算量小，能適應未知環境下的路徑規劃，可進行優化改良，結果可控。張珂等[5]提出1種可變邊界斥力勢場的人工勢場法改進方案，加入根據車速變化的斥力勢場范圍，使路徑規劃具有更好的實時性，能適應更復雜的環境，綜合性能優于傳統人工勢場法；Tian等[6]基于轉向角改進人工勢場法，減小了路徑曲率及方向盤轉角，提高了車輛的行駛穩定性；張鵬等[7]在引力勢場中加入影響距離極限值，與模擬退火方案進行算法融合，提高了算法脫離局部極小值的概率，改善了震蕩問題，提高了車輛避障穩定性；Liu等[8]通過優化斥力勢場函數，調整斥力分量在坐標軸的方向，提高了算法脫離局部極小值的概率及準確性。

通過算法生成路徑后，需進行車輛跟蹤，使車輛按既定路線行駛，跟蹤誤差是衡量路徑跟蹤是否優秀的指標之一，常用的路徑跟蹤算法有純跟蹤[9]、線性二次型調節器（linear quadratic regulator，LQR）[10]和模型預測控制（model predictive control，MPC）[11]等。與純跟蹤、LQR算法相比，MPC算法能進行滾動優化，根據規劃路徑與實際情況的誤差添加約束條件，但必須使用具有較高運算處理能力的處理器，成本較高。Wang等[12]改進了MPC算法的適應性，可更好地實現人機交互，能兼容車輛不同傳感器和輪胎的特征，橫向、縱向控制的穩定性較好，能提高車輛轉向和移位的精度；李駿等[13]根據行駛路徑彎曲度計算車輛在平坦路面上不發生滑移的最大縱向速度，基于MPC算法構建車輛運動學模型，添加前輪轉角和車速約束條件，設置基于位置偏差和控制增量的目標函數，獲得最優行駛速度和前輪轉角，通過仿真與實車測試，改進的MPC算法提升了車輛在多變環境下跟蹤精度和車輛穩定性。

本文基于傳統人工勢場法，通過修改斥力勢場函數的方法避免規劃路徑時陷入局部極小值而無法到達目標位置的問題，增加道路邊界約束條件函數以改進傳統人工勢場法；采用MPC算法進行路徑跟蹤，采用CarSim軟件調節車輛模型參數，與軟件Simulink通過COM接口連接進行聯合仿真試驗。根據仿真結果驗證改進人工勢場法和MPC算法應用于無人駕駛車輛的路徑規劃與跟蹤控制的可行性。

1 基于人工勢場法的路徑規劃

人工勢場法是Khatib在1985年提出的一種虛擬力場構想，其實現原理是把被控對象的工作區域定義為一個勢場空間，目標位置釋放的勢場吸引被控對象，障礙物釋放的勢場排斥被控對象，被控對象按照斥力勢場和引力勢場合力指引的路徑行駛，若在行駛過程中遇到障礙物，在障礙物斥力勢場影響下，被控對象避開障礙物[14]。該算法結構簡潔，性能穩定，運動軌跡平滑，面對未知環境有較好的表現，但在計算過程中易陷入局部極小值的陷阱中，導致目標不可達。

人工勢場由多個勢場構成，勢場組合在一起后，單個勢場應能履行特定角色。勢場數學函數應具有如下性質：

1）勢場具有連續梯度，防止產生動力學變化不連續；2）能根據車輛位置調整勢場強度，以適應距離變化帶來的影響；3）勢場的計算必須有效。車輛行駛時的人工勢場示意圖如圖1所示，箭頭方向為單個勢場的方向。

假設車輛在二維空間工作，其行駛坐標位置為q=（x，y），從初始位置qo=（0，0）出發到目標位置qgoal=（xgoal，ygoal），由于qgoal對車輛的引力，車輛逐漸靠近qgoal，在行駛過程中，若受到來自障礙物的斥力勢場，車輛路徑改變以避開障礙物，障礙物位置為qobs=（xobs，yobs）。人工勢場的引力勢場函數

3 仿真驗證

改進人工勢場法與MPC算法聯合仿真的步驟為：通過改進人工勢場法進行路徑規劃，將規劃路徑導入MPC算法控制器中，根據前輪轉角建立車輛動力學仿真模型，通過軟件CarSim將動力學參數輸入MPC算法控制器中，經過滾動優化和反饋校正得到控制量，控制車輛的運動路徑。

軟件CarSim便于調整車輛參數，可表現車輛的動力學特性，首先在軟件CarSim中設置車輛動力學參數，將CarSim中的變量輸出賦予軟件Simulink中的S-Function模塊，計算后將δ返回軟件CarSim中。車輛參數取值如表1所示，CarSim經COM接口與Simulink結合，搭建聯合仿真模型如圖4所示。MPC算法的跟蹤路徑及跟蹤路徑的橫向距離誤差分別如圖5、6所示。

由圖5、6可知：MPC算法能較好地跟蹤規劃路徑，跟蹤路徑與規劃路徑的橫向距離誤差小于0.4 m，誤差較大的地方出現在縱向距離為15、30、43 m的區域附近，因規劃路徑時在這3個區域有障礙物，車輛轉向行駛，因速度、路徑平滑度等因素跟蹤效果較差。在后續研究中，可對這3部分區域的代碼進行優化處理，使跟蹤路線更平滑，為算法改進提供數據參考。

4 結語

為解決傳統人工勢場法算法易陷入局部極小值或出現不達目標位置的缺陷，考慮車道邊界斥力勢場，采用修改斥力勢場函數的方法改進算法進行車輛路徑規劃，避免了因合力平衡造成不達目標位置的問題，規劃的路徑更準確、穩定。在改進人工勢場法路徑規劃的基礎上采用模型預測控制算法進行路徑跟蹤控制，在軟件CarSim中設置車輛動力學參數，并將其導入軟件Simulink中進行聯合仿真，仿真結果驗證了改進人工勢場法與模型預測控制算法應用于車輛路徑規劃與跟蹤控制的可行性。

參考文獻：

[1] 彭育輝，江銘，馬中原，等.汽車自動駕駛關鍵技術研究進展[J].福州大學學報（自然科學版），2021，49（5）：691-703.

PENG Yuhui， JIANG Ming， MA Zhongyuan， et al. Overview of target detection methods for autonomous vehicles[J].Journal of Fuzhou University（Natural Science Edition），2021，49（5）：691-703.

[2] 張春苗.時間依賴綠色選址-車輛路徑問題及算法研究[D].杭州：浙江工業大學，2020.

ZHANG Chunmiao. Time-dependent green location-routing problem and algorithm research[D].Hangzhou： Zhejiang University of Technology，2020.

[3] 衛玉梁，靳伍銀.基于神經網絡Q-learning算法的智能車路徑規劃[J].火力與指揮控制，2019，44（2）：46-49.

WEI Yuliang， JIN Wuyin. Intelligent vehicle path planning based on neural network Q-learning algorithm[J].Fire Control & Command Control， 2019，44（2）：46-49.

[4] LIU L S， WANG B， XU H. Research on path-planning algorithm integrating optimization A-star algorithm and artificial potential field method[J].Electronics，2022，11（22）：3660.

[5] 張珂，劉暢，蘭鵬宇.基于改進人工勢場法的局部路徑規劃[J].汽車文摘，2021（7）：59-62.

ZHANG Ke， LIU Chang， LAN Pengyu. Local path planning based on improved artificial potential field method[J].Automotive Digest，2021（7）：59-62.

[6] TIAN J， BEI S Y， LI B， et al. Research on active obstacle avoidance of intelligent vehicles based on improved artificial potential field method[J].World Electric Vehicle Journal，2022，13（6）：97.

[7] 張鵬，葛中盛，徐效農，等.一種基于人工勢場法的自動駕駛汽車主動避障路徑規劃算法[J].智能計算機與應用，2020，10（9）：174-177.

ZHANG Peng， GE Zhongsheng， XU Xiaonong， et al. An active obstacle avoidance path planning algorithm for automatic driving vehicle based on artificial potential field method[J].Intelligent Computers and Applications， 2020，10（9）：174-177.

[8] LIU X J ， DOU Y K. Research on obstacle avoidance of small cruise vehicle based on improved artificial potential field method[J].Journal of Physics： Conference Series， 2021， 1965（1）：012036.

[9] 周君武，覃立仁，楊航.基于“純跟蹤算法”的汽車平行泊車研究[J].汽車電器，2022（6）：1-3.

ZHOU Junwu， QIN Liren， YANG Hang. Research on parallel parking based on pure tracking algorithm[J].Auto Electric Parts， 2022（6）：1-3.

[10] 左帥，劉成昊，張笑，等.基于LQR的工程車輛自動駕駛路徑跟蹤技術研究[J].工程機械，2022，53（3）：54-58.

ZUO Shuai， LIU Chenghao， ZHANG Xiao， et al. Research on automatic driving path tracking technology of engineering vehicles based on LQR[J].Construction Machinery and Equipment， 2022，53（3）：54-58.

[11] 宋曉華，邵毅明，屈治華，等.基于模型預測控制的車輛軌跡跟蹤穩定性研究[J].重慶理工大學學報（自然科學），2020，34（8）：1-8.

SONG Xiaohua， SHAO Yiming， QU Zhihua， et al. A study of the vehicle track tracking stability based on the model predictive control[J].Journal of Chongqing University of Technology （Natural Science），2020，34（8）：1-8.

[12] WANG Z X， ZHANG Q Y， JIA T， et al. Research on control algorithm of a automatic driving robot based on improved model predictive control[J].Journal of Physics： Conference Series， 2021， 1920（1）：012116.

[13] 李駿，萬文星，郝三強，等.復雜路況下無人駕駛路徑跟蹤模型預測控制研究[J].汽車工程，2022，44（5）：664-674.

LI Jun， WAN Wenxing， HAO Sanqiang， et al. Research on model predictive control of autonomous vehicle path tracking under complex road condition[J].Automotive Engineering，2022，44（5）：664-674.

[14] KHATIB O. Real-time obstract avoidance for manipulators and mobile robots[C]//1985 IEEE International Conference on Robotics and Automation Proceedings. St. Louis， MO， USA： Institute of Electrical and Electronics Engineers， 1985：500-505.

[15] 寧君，馬昊冉，李鐵山.基于改進人工勢場法的船舶路徑規劃與跟蹤控制[J].哈爾濱工程大學學報， 2022， 43（10）：1414-1423.

NING Jun， MA Haoran， LI Tieshan. Underactuated surface vessel path planning and following control based on an improved artificial potential field method[J].Journal of Harbin Engineering University， 2022，43（10）：1414-1423.

[16] 林潔，張志安.改進人工勢場法的路徑規劃研究[J].機械與電子，2022，40（3）：65-70.

LIN Jie， ZHANG Zhian. Research on path planning of improved artificial potential field[J].Machinery & Electronics， 2022，40（3）：65-70.

[17] LI Y Y， YANG W， ZHANG X R ，et al. Research on automatic driving trajectory planning and tracking control based on improvement of the artificial potential field method[J].Sustainability，2022，14（19）：1-28.

[18] 趙明，鄭澤宇，么慶豐，等.基于改進人工勢場法的移動機器人路徑規劃方法[J].計算機應用研究，2020（增刊2）：66-68.

[19] 趙奉奎，成海飛，朱少華，等.基于模型預測控制的路徑跟蹤技術研究[J].重慶交通大學學報（自然科學版），2022，41（8）：142-148.

ZHAO Fengkui， CHENG Haifei， ZHU Shaohua， et al. Path following technology based on model predictive control[J].Journal of Chongqing Jiaotong University（Natural Science），2022，41（8）：142-148.

[20] 梁棟.基于模型預測控制的無人駕駛汽車橫縱向綜合控制[D].重慶：重慶理工大學，2022.

LIANG Dong. Lateral and longitudinal integrated control of driverless cars based on model predictive control[D].Chongqing： Chongqing University of Technology，2022.

Vehicle path planning and tracking control based on improved artificial potential field method

HAI Zhenyang1， WANG Jian1， LI Dasheng2， YANG Zhiyong2，MU Sikai1， WANG Yunjing1， DENG Huan1

1.School of Automotive Engineering， Shandong Jiaotong University， Jinan 250357， China; 2.Shandong Lukuo Vehicle Manufacturing Co.， Ltd.， Heze 274400， China

Abstract：To solve the problem that the local minimum and the target are easily unreachable when the traditional artificial potential field method is used for vehicle path planning， it is improved by changing the repulsive force function and adding the constraint function of lane boundary. A model predictive control （MPC） algorithm is used to track and control the planned path generated by the improved artificial potential field method， and a joint simulation model with software CarSim and Simulink is built to test the path tracking effect. The simulation results show that the improved artificial potential field method is reasonable and effective; the lateral error between tracking path and planning path is less than 0.4 m. It is feasible to apply the improved artificial potential field method and MPC algorithm in the path planning and tracking control of unmanned vehicles.

Keywords：artificial potential field method; MPC algorithm; path planning; tracking control; joint simulation

（責任編輯：郭守真）

收稿日期：2022-09-09

基金項目：山東省交通運輸廳科技計劃項目（2022B107）；山東省高等學校青創科技支持計劃項目（2021KJ039）；山東省重點扶持區域引進急需緊缺人才項目（2022-13）；山東交通學院研究生科技創新項目（2022YK005）

第一作者簡介：海振洋（1993—），男，鄭州人，碩士研究生，主要研究方向為自動駕駛及主動安全，E-mail：104750063@qq.com。

*通信作者簡介：王健（1986—），男，山東濰坊人，副教授，工學博士，主要研究方向為自動駕駛及主動安全，E-mail：wangjian@sdjtu.edu.cn。