基于Stanley算法的自动驾驶车辆路径跟踪控制研究
摘要：随着自动驾驶技术的快速发展，车辆路径跟踪控制已成为自动驾驶系统中的关键研究内容之一。针对自动驾驶车辆在已知参考轨迹条件下的路径跟踪问题，本文基于车辆运动学自行车模型，研究并实现了一种基于 Stanley 算法 的车辆路径跟踪控制方法。通过构建车辆与参考轨迹之间的横向误差和航向误差模型，利用 Stanley 前轮反馈控制策略，将航向误差项与横向误差引导项相结合，实时计算车辆前轮转角，实现车辆的横向路径跟踪控制。同时，为保证车辆行驶速度的稳定性，设计了基于比例控制的纵向速度控制器，使车辆速度能够平稳跟踪给定目标速度。
作者：Bob(原创)
项目概述
路径跟踪控制是自动驾驶车辆运动控制系统中的关键问题之一。针对自动驾驶车辆在已知参考轨迹条件下的路径跟踪需求，本文基于车辆运动学自行车模型，研究并实现了一种基于 Stanley 算法 的车辆路径跟踪控制方法。首先，根据离散参考轨迹点计算参考航向角，并建立车辆相对于参考轨迹的横向误差和航向误差模型；在此基础上，采用 Stanley 前轮反馈控制策略，将航向误差项与横向误差引导项相结合，实时计算车辆前轮转角，实现车辆横向路径跟踪控制。同时，引入比例型纵向速度控制器，使车辆纵向速度能够稳定跟踪目标速度。
在 MATLAB 仿真环境下，选取圆形轨迹和 S 型轨迹作为典型测试工况，对所设计的路径跟踪控制算法进行仿真验证。仿真结果表明，该方法能够有效抑制车辆在行驶过程中的横向跟踪误差，使车辆轨迹平稳逼近参考路径，并在不同轨迹条件下表现出较好的跟踪精度与稳定性。研究结果验证了 Stanley 控制算法在自动驾驶车辆路径跟踪控制中的有效性和工程可行性，为自动驾驶车辆横向控制算法的研究与应用提供了参考。
系统设计
本系统基于车辆运动学自行车模型，采用 Stanley 前轮反馈控制算法实现车辆路径跟踪控制，并结合比例型纵向速度控制器，在 MATLAB 仿真环境下完成自动驾驶车辆的轨迹跟踪与性能验证。
图1 系统整体流程图
硬件配置
该系统硬件配置如上，如果您的电脑配置低于下述规格，运行速度可能会与本系统的存在差异，请注意。
表1 惠普(HP)暗影精灵10台式整机配置(系统硬件配置)
软件环境
对本实验所需的各类软件及工具的基本信息进行了清晰汇总。
表2 系统软件配置(真实运行环境)
运行展示
运行Stanley.m
图2 车辆在圆形轨迹下的路径跟踪效果
由图可知，采用 Stanley 控制算法后，车辆实际行驶轨迹能够较好地贴合参考圆形轨迹，整体跟踪误差较小，表明该算法具有良好的路径跟踪性能。
图3 横向跟踪误差与前轮转角随时间变化曲线
仿真结果表明，车辆横向误差在初始阶段出现一定振荡后逐渐收敛并保持稳定，同时前轮转角趋于平稳，说明系统具有较好的稳定性和收敛性。
图4 参考航向角与车辆实际航向角对比
由图可见，车辆实际航向角能够快速跟随参考航向角变化趋势，二者基本保持一致，验证了 Stanley 算法在航向控制方面的有效性。