APA(自动泊车辅助系统)_路径跟踪控制（Stanly）

APA(自动泊车辅助系统):路径跟踪控制（Stanly）

目录

0. 算法简介

1. 算法原理分析

2. 算法相关代码（MATLAB代码与C++代码）

0. 算法简介

路径跟踪算法中的几何路径跟踪算法最为经典的两种算法Pure Persuit与Stanly，其中Pure Persuit是基于车辆后轴中心点与道路几何的关系计算初前轮转角来实现对车辆的控制，Stanly算法则以前轮为参考点，但二者的相同点都是通过控制前轮转角来消除横向误差，实现对路径的跟踪且都适用在低速场景的工况下；

1. 算法原理分析

Stanly 算法的基本思路：

(1). 根据车辆当前停车位置（前轴中心点位置）和一连串离散期望路径点的集合，找到一个在路径点集合内的距离前轴中心点最近的路径点；

(2). 计算出最近的路径点与前轴中心点的横向误差；

(3). 基于横向误差，求出基于横向误差引起的期望转角（即消除横向误差应转多少角度）；

(4). 基于航向偏差求出基于航向偏差引起的期望转角；

车辆航向角：车辆质心速度与地面坐标系X轴的夹角；

航向角偏差：即车辆转向前后航向角的差值；

图 1 Stanly示意图

下面针对上述控制量表达式，分别对式（1）中各部分进行分析：

2. 算法相关代码

（1）matlab代码为引用别人的代码用于学习，参考以下博客（matlab练习程序（Stanley路径跟踪） - Dsp Tian - 博客园）

（2）C++代码参考学习github中相关代码链接用于学习

https://github.com/CHH3213/chhRobotics_CPP/tree/master/PathTracking/Stanley

以上代码均参源于链接处，仅用于作者学习总结。如有侵犯知识产权，请联系作者，谢谢！