基于改进人工势场法的车辆路径规划与跟踪控制

海振洋　王健　李大升　杨智勇　牟思凯　王云靖　邓欢

摘要：针对采用传统人工势场法进行车辆路径规划时易造成局部极小值与目标不可达的问题，通过改变斥力函数并增加车道边界约束条件函数的方式改进传统人工势场法，进行车辆路径规划。采用模型预测控制（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。