纯跟踪法——轨迹跟踪控制——学习记录

 纯跟踪控制算法（Pure Pursuit）是一种典型的横向控制方法，该方法对外界的鲁棒性较好。

算法思想：

（1）基于后轮中心在参考路径上按照自定义的距离（Ld）匹配一个预瞄点

（2）后轮中心点按照一定的转弯半径R行驶到预瞄点

（3）预瞄距离Ld,转弯半径R，朝向角2α确定前轮转角

OA=OC是一个等腰三角形——>OB是∠AOC的角平分线

AB⊥AO

得到（5式如下）：

“纯跟踪”在控制理论中可以近似看作是一个P控制器。

在控制系统中，P控制器（比例控制器）是一种最简单的控制器类型。它基于反馈信号的误差，直接与一个比例增益Kp相乘，并将其作为控制输出。P控制器的数学表达式可以表示为：输出 = Kp * 误差。纯跟踪是一种控制算法，它的目标是使跟踪信号与被跟踪信号尽可能地一致。在跟踪过程中，纯跟踪通过调整输出信号来减小跟踪误差。将纯跟踪算法与P控制器进行比较，可以发现它们的本质相似。在纯跟踪中，跟踪误差起到类似于反馈误差的作用，而输出信号的调整类似于P控制器中的比例增益调节。因此，可以将纯跟踪近似地看作是一个P控制器。

需要注意的是，纯跟踪算法可能还包括其他因素，如速度限制、饱和控制等。

扩展一下：

阿克曼转向，也称为Ackermann转向，是一种用于汽车前轮转向的机械设计。它通过将两个前轮以不同的转向角度转动来实现车辆的曲线运动。阿克曼转向机构通常由一个转向杆和两个转向连杆组成。阿克曼转向的原理是通过使内侧前轮转动更大的角度，从而实现车辆前轮的转弯半径差异。当车辆转弯时，内侧前轮需要更大的转动角度以克服车轮滑移的问题，而外侧前轮的转动角度较小。这样可以确保车辆稳定地行驶在转弯中，并减少轮胎的磨损和摩擦。阿克曼转向的设计需要综合考虑车辆的转弯半径、车轮的转动角度和车辆的长度等因素。一个合适的阿克曼转向机构可以使车辆在转弯时更加灵活和稳定，提高驾驶操控性和安全性。值得注意的是，阿克曼转向是一种近似设计，仍存在一定的误差。在极端情况下，比如车辆急剧转弯时，阿克曼转向机构可能无法完全解决车轮滑移的问题。因此，在设计和制造阿克曼转向机构时，需要对车辆的使用情况和使用条件进行准确的分析和计算，以确保其安全性和可靠性

%% 首先在参考轨迹上搜索离当前车辆位置最近的点
function  [lookaheadPoint,idx_target] = findLookaheadPoint(pos, v, RefPos, Kv, Ld0)

% 找到距离当前位置最近的一个参考轨迹点的序号
sizeOfRefPos = size(RefPos,1);
for i = 1:sizeOfRefPos
    dist(i,1) = norm(RefPos(i,:) - pos);   
end
[~,idx] = min(dist); 


% 从该点开始向轨迹前方搜索，找到与预瞄距离最相近的一个轨迹点
L_steps = 0;           % 参考轨迹上几个相邻点的累计距离
Ld = Kv*v + Ld0;       % Ld0是预瞄距离的下限值；
while L_steps < Ld && idx < sizeOfRefPos
    L_steps = L_steps + norm(RefPos(idx + 1,:) - RefPos(idx,:));
    idx = idx+1;
end
idx_target = idx;
lookaheadPoint = RefPos(idx,:);
end