基于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.