一、纯跟踪算法概述

非完整（Nonholonomic）动力学约束就是假设车辆在运动过程中没有滑移。很多在平坦路面运动的无人车辆或移动机器人都用这种假设设计轨迹跟踪控制器，特别是纯跟踪算法（Pure Pursuit），已经被成功地应用在很多机器人轨迹跟踪控制中，包括2005年DAPAR沙漠挑战赛冠军的斯坦福大学的Stanley赛车。——摘自《无人驾驶车辆模型预测控制》

非线性MC：自动驾驶——车辆运动学模型推导及Matlab验证

之所以放在这里介绍，是因为纯跟踪算法是一种基于运动学约束的算法，对该算法的学习可以更好地理解车辆运动学模型。根据上一文档的讲解，我们可以建立后轮速度、前轮偏角和车辆大地坐标系下的位置、航向角的关系，那么我们在忽略轮胎力学、执行机构特性等情况下，就能根据后轮速度和前轮偏角任意控制车辆的速度、位置。本文只讨论横向控制，假设后轮速度恒定，也就是只控制前轮偏角。

纯跟踪算法提出“预瞄距离”的概念，根据预瞄距离寻找目标轨迹中符合条件的目标路径点，判断逻辑就是寻找目标轨迹上哪个点和当前车辆位置（图中的当前后轮位置）的相对距离等于预瞄距离（可以以当前位置为圆心，预瞄距离为半径，画圆，寻找与目标轨迹的交点），则该点就是当前时刻的目标点（图中的目标后轮位置）。控制目标则是计算多大的前轮偏角，可以使当前后轮位置运动到目标后轮位置。

图一 目标路径点（预瞄点）示意图

二、纯跟踪算法原理

这里做一个模型验证，基于上个文档推导的运动学模型，恒定后轮速度，恒定前轮偏角，那么后轮的运动轨迹将是一个圆，即车辆绕着转动中心做圆周运动，如下图所示：

图二 恒定速度和恒定前轮偏角下的圆周运动

基于这个现象，可以认为在每个运行周期内（速度恒定，前轮偏角恒定），车辆绕转动中心做圆周运动。那么控制目标就可以简化为两点：①是怎样的圆周运动轨迹可以将后轮从当前位置运动至目标位置？②如何控制前轮偏角实现这个圆周运动？

图三 纯跟踪算法示意图

几个关键词，圆周运动，切向速度（后轮速度）垂直于转向半径，等腰三角形。根据三角几何就能推导出圆周运动需要运动2θ角度。设定预瞄距离为len_ahead，瞬时转向半径为R，根据正弦定理可得：

[公式]

根据三角函数公式，进一步化简：

[公式]

 再结合上个文档中，前轮偏角和转向半径R、车辆轴距L的关系：

[公式] 

可得所需前轮偏角：

[公式]

三、matlab实现

Matlab实现逻辑：

  1. 设计目标轨迹并离散化；
  2. 根据预瞄距离寻找目标路径点；
  3. 根据纯跟踪算法计算前轮偏角；
  4. 根据前轮偏角、后轮速度更新车辆状态量。

无人驾驶汽车系统入门（十八）--使用pure pursuit实现无人车轨迹追踪

Matlab模型参考csdn上的大佬文章，个人修改到Matlab代码。本文主要目的是根据纯跟踪算法学习什么叫做基于运动学约束的控制，没有刻意完善纯跟踪算法的实现功能，代码个人已测出很多bug，所以只粘贴主代码，表示一下逻辑即可。

%% 主代码
vehicle_state = [0, 0, 0];
dt = 0.1;               % 状态更新间隔 s
t_total = 50;           % 运行总时间 s
n_steps = t_total / dt; % 运行总步长

t = zeros();            % 运行时间 s
state_x = zeros();      % 水平位移 大地坐标系
state_y = zeros();      % 垂直位移 大地坐标系
state_psi = zeros();    % 航向角 大地坐标系

num_now = 1; % 初始化目标路径点

for i = 1 : n_steps
    % 根据当前位置存在预瞄点
    num_new = find_target_num(state_x(i,1),state_y(i,1),num_now);
    % 更新并记录当前时刻目标路径点
    num_now = num_new;
    % 目标路径点和当前位置向量航向角 - 当前时刻车辆航向角
    alpha = atan2(trj_target(num_new,2)-state_y(i,1),trj_target(num_new,1)-state_x(i,1)) - state_psi(i,1);
    % 根据公式计算当前时刻前轮偏角
    delta = atan(2*len_wheel*sin(alpha)/len_ahead);
    % 更新车辆状态
    vehicle_state = dynamic_state(vehicle_state, [delta,vel_r], dt);
    % 记录制图所需数据
    t(i,1) = i * dt;
    state_x(i+1,1) = vehicle_state(1,1);
    state_y(i+1,1) = vehicle_state(1,2);
    state_psi(i+1,1) = vehicle_state(1,3);
end

figure(1);
scatter(trj_target(:,1),trj_target(:,2));hold on;
plot(state_x,state_y);grid on;title('车辆位移图');
legend('目标路径点','车辆实际跟踪路径','FontSize',15);
图四 纯跟踪算法效果图

上图中蓝色点为离散的目标轨迹，红色为使用纯跟踪算法实现的车辆位置，简单实现了轨迹跟踪的功能。

再次强调一下，不必过分纠结，只是学习什么叫做运动学约束~