4. 轨迹跟踪控制（纯追踪）

任务目标

实现纯追踪（Pure Pursuit）轨迹跟踪算法，使AGV能够跟踪预定义的直线与圆弧路径。算法根据车辆当前位姿与前瞻距离，在目标路径上选取追踪点，并计算输出前轮转角（或角速度），驱动机器人向追踪点运动。

实现方案

采用自行车运动学模型，输入为AGV当前坐标(x, y)与航向角θ，输出为前轮转角δ。核心步骤：

1. 在路径上寻找距离车辆前方前瞻距离Ld处的目标点；
2. 计算目标点与车辆连线的夹角α；
3. 根据几何关系 δ = atan2(2·L·sin(α), Ld) 计算前轮转角（L为轴距）。

路径以离散点序列表示，通过遍历查找满足距离条件且位于车辆前方的点作为追踪点。仿真中车辆以恒定速度前进，验证直线与圆弧两种路径的跟踪效果。

关键完整代码

import numpy as np
import math
import matplotlib.pyplot as plt

class PurePursuit:
    def __init__(self, wheelbase=0.5, Ld=1.0):
        self.L = wheelbase      # 轴距(m)
        self.Ld = Ld            # 前瞻距离(m)

    def find_target_point(self, path, x, y, theta):
        """在路径上寻找前瞻点（距离≈Ld且位于车辆前方）"""
        min_dist = float('inf')
        target = None
        for i, (px, py) in enumerate(path):
            dx, dy = px - x, py - y
            dist = math.hypot(dx, dy)
            # 车辆前方判断：目标点与航向夹角小于90°
            angle_to_point = math.atan2(dy, dx)
            diff = self._normalize_angle(angle_to_point - theta)
            if abs(diff) < math.pi / 2 and abs(dist - self.Ld) < min_dist:
                min_dist = abs(dist - self.Ld)
                target = (px, py)
        return target

    def compute_steering(self, path, x, y, theta):
        """计算前轮转角δ"""
        target = self.find_target_point(path, x, y, theta)
        if target is None:
            return 0.0
        tx, ty = target
        alpha = math.atan2(ty - y, tx - x) - theta
        alpha = self._normalize_angle(alpha)
        delta = math.atan2(2.0 * self.L * math.sin(alpha), self.Ld)
        return delta

    @staticmethod
    def _normalize_angle(angle):
        while angle > math.pi:
            angle -= 2 * math.pi
        while angle < -math.pi:
            angle += 2 * math.pi
        return angle

def simulate(path, x0=0.0, y0=0.0, theta0=0.0, v=0.5, dt=0.05, steps=1000):
    """运动学仿真：恒定速度，前轮转角由纯追踪输出"""
    pp = PurePursuit()
    x, y, theta = x0, y0, theta0
    traj = [(x, y)]
    for _ in range(steps):
        delta = pp.compute_steering(path, x, y, theta)
        # 自行车模型更新
        x += v * math.cos(theta) * dt
        y += v * math.sin(theta) * dt
        theta += (v / pp.L) * math.tan(delta) * dt
        traj.append((x, y))
        # 到达终点附近停止
        if math.hypot(x - path[-1][0], y - path[-1][1]) < 0.1:
            break
    return traj

if __name__ == "__main__":
    # 定义路径：直线 + 半圆弧
    straight = [(i * 0.1, 0.0) for i in range(30)]          # 3m直线
    arc = [(3.0 + 1.0 * math.cos(math.pi - t), 1.0 * math.sin(math.pi - t))
           for t in np.linspace(0, math.pi, 50)]            # 半径1m半圆
    path = straight + arc

    # 从(0,0)出发，航向0°
    traj = simulate(path, x0=0.0, y0=0.0, theta0=0.0)

    # 绘制结果
    px = [p[0] for p in path]
    py = [p[1] for p in path]
    tx = [t[0] for t in traj]
    ty = [t[1] for t in traj]
    plt.figure(figsize=(8, 5))
    plt.plot(px, py, 'b--', label='参考路径')
    plt.plot(tx, ty, 'r-', label='AGV轨迹')
    plt.xlabel('x (m)')
    plt.ylabel('y (m)')
    plt.legend()
    plt.axis('equal')
    plt.grid(True)
    plt.title('纯追踪轨迹跟踪仿真')
    plt.show()

运行与验证

依赖：Python 3.8+，numpy，matplotlib。

运行方式：将代码保存为 pure_pursuit.py，在终端执行：

python pure_pursuit.py

验证方法：

• 程序运行后弹出图形窗口，红色轨迹应贴合蓝色参考路径，直线段无偏移，圆弧段跟踪误差随仿真推进逐渐收敛；
• 修改simulate()中的初始航向角theta0为0.3（约17°），重新运行，观察AGV仍能收敛到参考路径，说明算法对初始偏差具有纠正能力；
• 将PurePursuit的Ld改为0.5后运行，可看到跟踪更紧但转向更频繁；改为2.0后轨迹更平滑但弯道处切弯更明显——验证前瞻距离对跟踪特性的影响。

通过条件：直线段跟踪横向误差小于0.05m，圆弧段最大跟踪误差小于0.15m（以Ld=1.0、v=0.5m/s参数为准）。

常见问题

• 找不到前瞻点：当车辆偏离路径过远（超过Ld）时，find_target_point可能返回None，此时转角输出0。实际部署中应增加路径重规划或加大Ld。
• 圆弧内侧切弯：Ld过大导致前瞻点跨越弯道，产生切弯。应根据曲率自适应调整Ld（如Ld与速度正相关）。