基于改進(jìn)人工勢(shì)場(chǎng)法的車(chē)輛路徑規(guī)劃與跟蹤控制

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

摘要：針對(duì)采用傳統(tǒng)人工勢(shì)場(chǎng)法進(jìn)行車(chē)輛路徑規(guī)劃時(shí)易造成局部極小值與目標(biāo)不可達(dá)的問(wèn)題，通過(guò)改變斥力函數(shù)并增加車(chē)道邊界約束條件函數(shù)的方式改進(jìn)傳統(tǒng)人工勢(shì)場(chǎng)法，進(jìn)行車(chē)輛路徑規(guī)劃。采用模型預(yù)測(cè)控制（model predictive control，MPC）算法跟蹤控制改進(jìn)人工勢(shì)場(chǎng)法生成的規(guī)劃路徑，采用軟件CarSim與Simulink搭建聯(lián)合仿真模型對(duì)路徑跟蹤效果進(jìn)行仿真試驗(yàn)。結(jié)果表明：改進(jìn)人工勢(shì)場(chǎng)法路徑規(guī)劃合理有效；跟蹤路徑與規(guī)劃路徑的橫向誤差小于0.4 m。改進(jìn)人工勢(shì)場(chǎng)法和MPC算法應(yīng)用于無(wú)人駕駛車(chē)輛的路徑規(guī)劃與跟蹤控制具有可行性。

關(guān)鍵詞：人工勢(shì)場(chǎng)法；MPC算法；路徑規(guī)劃；跟蹤控制；聯(lián)合仿真

中圖分類號(hào)：U46;TP391.9文獻(xiàn)標(biāo)志碼：A文章編號(hào)：1672-0032（2023）02-0001-07

引用格式：海振洋，王健，李大升，等.基于改進(jìn)人工勢(shì)場(chǎng)法的車(chē)輛路徑規(guī)劃與跟蹤控制［J］.山東交通學(xué)院學(xué)報(bào)，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 引言

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

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

本文基于傳統(tǒng)人工勢(shì)場(chǎng)法，通過(guò)修改斥力勢(shì)場(chǎng)函數(shù)的方法避免規(guī)劃路徑時(shí)陷入局部極小值而無(wú)法到達(dá)目標(biāo)位置的問(wèn)題，增加道路邊界約束條件函數(shù)以改進(jìn)傳統(tǒng)人工勢(shì)場(chǎng)法；采用MPC算法進(jìn)行路徑跟蹤，采用CarSim軟件調(diào)節(jié)車(chē)輛模型參數(shù)，與軟件Simulink通過(guò)COM接口連接進(jìn)行聯(lián)合仿真試驗(yàn)。根據(jù)仿真結(jié)果驗(yàn)證改進(jìn)人工勢(shì)場(chǎng)法和MPC算法應(yīng)用于無(wú)人駕駛車(chē)輛的路徑規(guī)劃與跟蹤控制的可行性。

1 基于人工勢(shì)場(chǎng)法的路徑規(guī)劃

人工勢(shì)場(chǎng)法是Khatib在1985年提出的一種虛擬力場(chǎng)構(gòu)想，其實(shí)現(xiàn)原理是把被控對(duì)象的工作區(qū)域定義為一個(gè)勢(shì)場(chǎng)空間，目標(biāo)位置釋放的勢(shì)場(chǎng)吸引被控對(duì)象，障礙物釋放的勢(shì)場(chǎng)排斥被控對(duì)象，被控對(duì)象按照斥力勢(shì)場(chǎng)和引力勢(shì)場(chǎng)合力指引的路徑行駛，若在行駛過(guò)程中遇到障礙物，在障礙物斥力勢(shì)場(chǎng)影響下，被控對(duì)象避開(kāi)障礙物[14]。該算法結(jié)構(gòu)簡(jiǎn)潔，性能穩(wěn)定，運(yùn)動(dòng)軌跡平滑，面對(duì)未知環(huán)境有較好的表現(xiàn)，但在計(jì)算過(guò)程中易陷入局部極小值的陷阱中，導(dǎo)致目標(biāo)不可達(dá)。

人工勢(shì)場(chǎng)由多個(gè)勢(shì)場(chǎng)構(gòu)成，勢(shì)場(chǎng)組合在一起后，單個(gè)勢(shì)場(chǎng)應(yīng)能履行特定角色。勢(shì)場(chǎng)數(shù)學(xué)函數(shù)應(yīng)具有如下性質(zhì)：

1）勢(shì)場(chǎng)具有連續(xù)梯度，防止產(chǎn)生動(dòng)力學(xué)變化不連續(xù)；2）能根據(jù)車(chē)輛位置調(diào)整勢(shì)場(chǎng)強(qiáng)度，以適應(yīng)距離變化帶來(lái)的影響；3）勢(shì)場(chǎng)的計(jì)算必須有效。車(chē)輛行駛時(shí)的人工勢(shì)場(chǎng)示意圖如圖1所示，箭頭方向?yàn)閱蝹€(gè)勢(shì)場(chǎng)的方向。

假設(shè)車(chē)輛在二維空間工作，其行駛坐標(biāo)位置為q=（x，y），從初始位置qo=（0，0）出發(fā)到目標(biāo)位置qgoal=（xgoal，ygoal），由于qgoal對(duì)車(chē)輛的引力，車(chē)輛逐漸靠近qgoal，在行駛過(guò)程中，若受到來(lái)自障礙物的斥力勢(shì)場(chǎng)，車(chē)輛路徑改變以避開(kāi)障礙物，障礙物位置為qobs=（xobs，yobs）。人工勢(shì)場(chǎng)的引力勢(shì)場(chǎng)函數(shù)

3 仿真驗(yàn)證

改進(jìn)人工勢(shì)場(chǎng)法與MPC算法聯(lián)合仿真的步驟為：通過(guò)改進(jìn)人工勢(shì)場(chǎng)法進(jìn)行路徑規(guī)劃，將規(guī)劃路徑導(dǎo)入MPC算法控制器中，根據(jù)前輪轉(zhuǎn)角建立車(chē)輛動(dòng)力學(xué)仿真模型，通過(guò)軟件CarSim將動(dòng)力學(xué)參數(shù)輸入MPC算法控制器中，經(jīng)過(guò)滾動(dòng)優(yōu)化和反饋校正得到控制量，控制車(chē)輛的運(yùn)動(dòng)路徑。

軟件CarSim便于調(diào)整車(chē)輛參數(shù)，可表現(xiàn)車(chē)輛的動(dòng)力學(xué)特性，首先在軟件CarSim中設(shè)置車(chē)輛動(dòng)力學(xué)參數(shù)，將CarSim中的變量輸出賦予軟件Simulink中的S-Function模塊，計(jì)算后將δ返回軟件CarSim中。車(chē)輛參數(shù)取值如表1所示，CarSim經(jīng)COM接口與Simulink結(jié)合，搭建聯(lián)合仿真模型如圖4所示。MPC算法的跟蹤路徑及跟蹤路徑的橫向距離誤差分別如圖5、6所示。

由圖5、6可知：MPC算法能較好地跟蹤規(guī)劃路徑，跟蹤路徑與規(guī)劃路徑的橫向距離誤差小于0.4 m，誤差較大的地方出現(xiàn)在縱向距離為15、30、43 m的區(qū)域附近，因規(guī)劃路徑時(shí)在這3個(gè)區(qū)域有障礙物，車(chē)輛轉(zhuǎn)向行駛，因速度、路徑平滑度等因素跟蹤效果較差。在后續(xù)研究中，可對(duì)這3部分區(qū)域的代碼進(jìn)行優(yōu)化處理，使跟蹤路線更平滑，為算法改進(jìn)提供數(shù)據(jù)參考。

4 結(jié)語(yǔ)

為解決傳統(tǒng)人工勢(shì)場(chǎng)法算法易陷入局部極小值或出現(xiàn)不達(dá)目標(biāo)位置的缺陷，考慮車(chē)道邊界斥力勢(shì)場(chǎng)，采用修改斥力勢(shì)場(chǎng)函數(shù)的方法改進(jìn)算法進(jìn)行車(chē)輛路徑規(guī)劃，避免了因合力平衡造成不達(dá)目標(biāo)位置的問(wèn)題，規(guī)劃的路徑更準(zhǔn)確、穩(wěn)定。在改進(jìn)人工勢(shì)場(chǎng)法路徑規(guī)劃的基礎(chǔ)上采用模型預(yù)測(cè)控制算法進(jìn)行路徑跟蹤控制，在軟件CarSim中設(shè)置車(chē)輛動(dòng)力學(xué)參數(shù)，并將其導(dǎo)入軟件Simulink中進(jìn)行聯(lián)合仿真，仿真結(jié)果驗(yàn)證了改進(jìn)人工勢(shì)場(chǎng)法與模型預(yù)測(cè)控制算法應(yīng)用于車(chē)輛路徑規(guī)劃與跟蹤控制的可行性。

參考文獻(xiàn)：

[1] 彭育輝，江銘，馬中原，等.汽車(chē)自動(dòng)駕駛關(guān)鍵技術(shù)研究進(jìn)展[J].福州大學(xué)學(xué)報(bào)（自然科學(xué)版），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] 張春苗.時(shí)間依賴綠色選址-車(chē)輛路徑問(wèn)題及算法研究[D].杭州：浙江工業(yè)大學(xué)，2020.

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

[3] 衛(wèi)玉梁，靳伍銀.基于神經(jīng)網(wǎng)絡(luò)Q-learning算法的智能車(chē)路徑規(guī)劃[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ìn)人工勢(shì)場(chǎng)法的局部路徑規(guī)劃[J].汽車(chē)文摘，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] 張鵬，葛中盛，徐效農(nóng)，等.一種基于人工勢(shì)場(chǎng)法的自動(dòng)駕駛汽車(chē)主動(dòng)避障路徑規(guī)劃算法[J].智能計(jì)算機(jī)與應(yīng)用，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] 周君武，覃立仁，楊航.基于“純跟蹤算法”的汽車(chē)平行泊車(chē)研究[J].汽車(chē)電器，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的工程車(chē)輛自動(dòng)駕駛路徑跟蹤技術(shù)研究[J].工程機(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] 宋曉華，邵毅明，屈治華，等.基于模型預(yù)測(cè)控制的車(chē)輛軌跡跟蹤穩(wěn)定性研究[J].重慶理工大學(xué)學(xué)報(bào)（自然科學(xué)），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] 李駿，萬(wàn)文星，郝三強(qiáng)，等.復(fù)雜路況下無(wú)人駕駛路徑跟蹤模型預(yù)測(cè)控制研究[J].汽車(chē)工程，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ìn)人工勢(shì)場(chǎng)法的船舶路徑規(guī)劃與跟蹤控制[J].哈爾濱工程大學(xué)學(xué)報(bào)， 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ìn)人工勢(shì)場(chǎng)法的路徑規(guī)劃研究[J].機(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ìn)人工勢(shì)場(chǎng)法的移動(dòng)機(jī)器人路徑規(guī)劃方法[J].計(jì)算機(jī)應(yīng)用研究，2020（增刊2）：66-68.

[19] 趙奉奎，成海飛，朱少華，等.基于模型預(yù)測(cè)控制的路徑跟蹤技術(shù)研究[J].重慶交通大學(xué)學(xué)報(bào)（自然科學(xué)版），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] 梁棟.基于模型預(yù)測(cè)控制的無(wú)人駕駛汽車(chē)橫縱向綜合控制[D].重慶：重慶理工大學(xué)，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

（責(zé)任編輯：郭守真）

收稿日期：2022-09-09

基金項(xiàng)目：山東省交通運(yùn)輸廳科技計(jì)劃項(xiàng)目（2022B107）；山東省高等學(xué)校青創(chuàng)科技支持計(jì)劃項(xiàng)目（2021KJ039）；山東省重點(diǎn)扶持區(qū)域引進(jìn)急需緊缺人才項(xiàng)目（2022-13）；山東交通學(xué)院研究生科技創(chuàng)新項(xiàng)目（2022YK005）

第一作者簡(jiǎn)介：海振洋（1993—），男，鄭州人，碩士研究生，主要研究方向?yàn)樽詣?dòng)駕駛及主動(dòng)安全，E-mail：104750063@qq.com。

*通信作者簡(jiǎn)介：王健（1986—），男，山東濰坊人，副教授，工學(xué)博士，主要研究方向?yàn)樽詣?dòng)駕駛及主動(dòng)安全，E-mail：wangjian@sdjtu.edu.cn。