RCS-3000/docs/ALGORITHM.md

算法说明

本文档详细说明 AGV 路径跟踪与控制系统中使用的算法原理和实现。

目录

• AGV 运动学模型
• Pure Pursuit 算法
• Stanley 算法
• 路径平滑算法
• 算法比较

---

AGV 运动学模型

阿克曼转向模型

本系统使用阿克曼转向几何模型来描述 AGV 的运动。

模型假设

1. AGV 在平面上运动
2. 使用前轮转向，后轮驱动
3. 不考虑侧滑
4. 速度和转向角在限制范围内

状态变量

AGV 的状态由四个变量描述：

x     - x 坐标（米）
y     - y 坐标（米）
θ     - 航向角（弧度）
v     - 速度（米/秒）

控制输入

v     - 驱动速度（米/秒）
δ     - 前轮转向角（弧度）

运动学方程

ẋ = v · cos(θ)
ẏ = v · sin(θ)
θ̇ = (v / L) · tan(δ)
v̇ = a

其中：

• L 是轴距（前后轮距离）
• a 是加速度

离散化实现

使用欧拉法进行离散化：

State AGVModel::step(double v, double delta, double dt) const {
    State new_state = current_state_;

    // 限制转向角
    delta = std::max(-max_steering_angle_,
                     std::min(max_steering_angle_, delta));

    // 更新状态
    new_state.x += v * cos(current_state_.theta) * dt;
    new_state.y += v * sin(current_state_.theta) * dt;
    new_state.theta += (v / wheelbase_) * tan(delta) * dt;
    new_state.v = v;

    // 归一化角度到 [-π, π]
    while (new_state.theta > M_PI) new_state.theta -= 2 * M_PI;
    while (new_state.theta < -M_PI) new_state.theta += 2 * M_PI;

    return new_state;
}

---

Pure Pursuit 算法

Pure Pursuit 是一种基于几何的路径跟踪算法，通过追踪前视点来计算转向角。

算法原理

1. 在参考路径上选择一个前视点（lookahead point）
2. 计算从当前位置到前视点的曲率半径
3. 根据曲率计算转向角

数学推导

前视距离

L_d = k_v · v + L_base

其中：

• k_v: 速度相关系数（通常为 1.0-2.0）
• v: 当前速度
• L_base: 基础前视距离

前视点选择

沿参考路径找到距离当前位置约为 L_d 的点。

转向角计算

根据几何关系：

α = atan2(l_y, l_x)  # 前视点相对角度
δ = atan(2 · L · sin(α) / L_d)

其中：

• L: 轴距
• α: 前视点相对于车辆坐标系的角度
• L_d: 前视距离

实现代码

double ControlGenerator::purePursuit(
    const AGVModel::State& state,
    const PathCurve& path,
    double lookahead_distance
) {
    const auto& path_points = path.getPathPoints();
    if (path_points.empty()) return 0.0;

    // 找到最近点
    size_t closest_idx = 0;
    double min_dist = 1e9;
    for (size_t i = 0; i < path_points.size(); ++i) {
        double dx = path_points[i].x - state.x;
        double dy = path_points[i].y - state.y;
        double dist = sqrt(dx * dx + dy * dy);
        if (dist < min_dist) {
            min_dist = dist;
            closest_idx = i;
        }
    }

    // 找到前视点
    size_t lookahead_idx = closest_idx;
    for (size_t i = closest_idx; i < path_points.size(); ++i) {
        double dx = path_points[i].x - state.x;
        double dy = path_points[i].y - state.y;
        double dist = sqrt(dx * dx + dy * dy);
        if (dist >= lookahead_distance) {
            lookahead_idx = i;
            break;
        }
    }

    // 计算转向角
    const auto& target = path_points[lookahead_idx];
    double dx = target.x - state.x;
    double dy = target.y - state.y;

    // 转换到车体坐标系
    double cos_theta = cos(state.theta);
    double sin_theta = sin(state.theta);
    double local_x = dx * cos_theta + dy * sin_theta;
    double local_y = -dx * sin_theta + dy * cos_theta;

    // 计算转向角
    double alpha = atan2(local_y, local_x);
    double L = 2.0;  // 轴距
    double steering = atan(2 * L * sin(alpha) / lookahead_distance);

    return steering;
}

参数调整

• lookahead_distance:
  • 过小：响应快，但可能振荡
  • 过大：平滑，但转弯时可能跟踪不及时
  • 推荐：速度的 1-2 倍

优缺点

优点：

• 算法简单，计算量小
• 对路径平滑性要求低
• 适合高速场景

缺点：

• 跟踪精度相对较低
• 不考虑横向误差
• 低速时性能下降

---

Stanley 算法

Stanley 算法结合了横向误差和航向误差，提供更精确的路径跟踪。

算法原理

Stanley 算法由两部分组成：

1. 航向误差控制：修正车辆朝向与路径切线的偏差
2. 横向误差控制：修正车辆位置与路径的垂直距离

数学公式

δ = θ_e + atan(k · e / v)

其中：

• θ_e: 航向误差（车辆朝向与路径切线的夹角）
• e: 横向误差（车辆到路径的垂直距离）
• k: 增益参数
• v: 当前速度

详细推导

航向误差

θ_e = θ_path - θ_vehicle

归一化到 [-π, π]。

横向误差

计算车辆前轮中心到路径的垂直距离：

e = (x_vehicle - x_path) · sin(θ_path) - (y_vehicle - y_path) · cos(θ_path)

正值表示在路径左侧，负值表示右侧。

实现代码

double ControlGenerator::stanley(
    const AGVModel::State& state,
    const PathCurve& path,
    double k
) {
    const auto& path_points = path.getPathPoints();
    if (path_points.empty()) return 0.0;

    // 找到最近的路径点
    size_t closest_idx = 0;
    double min_dist = 1e9;
    for (size_t i = 0; i < path_points.size(); ++i) {
        double dx = path_points[i].x - state.x;
        double dy = path_points[i].y - state.y;
        double dist = sqrt(dx * dx + dy * dy);
        if (dist < min_dist) {
            min_dist = dist;
            closest_idx = i;
        }
    }

    const auto& closest_point = path_points[closest_idx];

    // 计算横向误差
    double dx = state.x - closest_point.x;
    double dy = state.y - closest_point.y;
    double path_theta = closest_point.theta;
    double cross_track_error = -dx * sin(path_theta) + dy * cos(path_theta);

    // 计算航向误差
    double heading_error = state.theta - path_theta;
    while (heading_error > M_PI) heading_error -= 2 * M_PI;
    while (heading_error < -M_PI) heading_error += 2 * M_PI;

    // Stanley 控制律
    double v = std::max(0.1, std::abs(state.v));  // 避免除零
    double steering = heading_error + atan(k * cross_track_error / v);

    return steering;
}

参数调整

• k（增益参数）：
  • 过小：横向误差响应慢
  • 过大：可能引起振荡
  • 推荐范围：0.5 - 2.0
  • 低速场景：使用较大的 k
  • 高速场景：使用较小的 k

优缺点

优点：

• 跟踪精度高
• 同时考虑位置和方向误差
• 适合低速和高精度场景
• 理论上能保证稳定性

缺点：

• 对路径平滑性要求较高
• 需要路径切线信息
• 参数调整相对复杂

---

路径平滑算法

三次样条插值

为了生成平滑的路径，系统使用三次样条插值。

算法原理

对于 n 个路径点，三次样条插值构造 n-1 段三次多项式：

S_i(t) = a_i + b_i·t + c_i·t² + d_i·t³,  t ∈ [t_i, t_{i+1}]

满足条件：

1. 通过所有路径点
2. 一阶导数连续（速度连续）
3. 二阶导数连续（加速度连续）

实现

使用自然样条（边界二阶导数为零）：

PathCurve PathCurve::generateSmoothPath(
    const std::vector<PathPoint>& waypoints,
    int points_per_segment
) {
    std::vector<PathPoint> smooth_points;

    for (size_t i = 0; i < waypoints.size() - 1; ++i) {
        const auto& p0 = waypoints[i];
        const auto& p1 = waypoints[i + 1];

        for (int j = 0; j < points_per_segment; ++j) {
            double t = static_cast<double>(j) / points_per_segment;

            // 三次 Hermite 插值
            double h00 = 2*t*t*t - 3*t*t + 1;
            double h10 = t*t*t - 2*t*t + t;
            double h01 = -2*t*t*t + 3*t*t;
            double h11 = t*t*t - t*t;

            PathPoint pt;
            pt.x = h00 * p0.x + h10 * dx0 + h01 * p1.x + h11 * dx1;
            pt.y = h00 * p0.y + h10 * dy0 + h01 * p1.y + h11 * dy1;

            smooth_points.push_back(pt);
        }
    }

    return PathCurve(smooth_points);
}

---

算法比较

性能对比表

特性	Pure Pursuit	Stanley
计算复杂度	低	中
跟踪精度	中	高
稳定性	好	很好
高速性能	优秀	良好
低速性能	一般	优秀
参数调整	简单	中等
适用场景	平滑路径	精确跟踪

选择建议

使用 Pure Pursuit 当：

• 速度较高（> 2 m/s）
• 路径相对平滑
• 对精度要求不高
• 计算资源有限

使用 Stanley 当：

• 需要高精度跟踪
• 低速场景（< 1 m/s）
• 路径复杂多变
• 有充足计算资源

混合策略

可以根据速度动态切换：

std::string selectAlgorithm(double velocity) {
    if (velocity > 2.0) {
        return "pure_pursuit";
    } else {
        return "stanley";
    }
}

---

误差分析

跟踪误差定义

e_lateral = |位置到路径的垂直距离|
e_heading = |车辆朝向 - 路径切线方向|

误差来源

1. 算法固有误差

   • Pure Pursuit: 前视点选择
   • Stanley: 参数设置

2. 离散化误差

   • 时间步长 dt 的选择
   • 路径点密度

3. 模型误差

   • 理想运动学模型 vs 实际物理
   • 侧滑、轮胎摩擦等忽略因素

误差优化

• 减小时间步长 dt
• 增加路径点密度
• 调整算法参数
• 使用更精确的模型

---

实际应用示例

示例 1: 圆形路径跟踪

// 生成半径 5m 的圆形路径
PathCurve path = PathCurve::generateCircularPath(5.0, 100);

// Pure Pursuit
tracker.generateControlSequence("pure_pursuit", 0.1, 10.0, 1.5);
// 前视距离: 1.5 * 1.5 = 2.25m

// Stanley
tracker.generateControlSequence("stanley", 0.1, 10.0, 0.5);
// k = 1.0（默认）

示例 2: 复杂路径跟踪

// 从 CSV 加载路径
PathCurve path = PathCurve::loadFromCSV("complex_path.csv");

// 使用 Stanley 以获得更高精度
tracker.generateControlSequence("stanley", 0.05, 15.0, 1.0);

---

参考文献

1. Pure Pursuit:

   • Coulter, R. C. (1992). "Implementation of the Pure Pursuit Path Tracking Algorithm". Carnegie Mellon University.

2. Stanley:

   • Hoffmann, G. M., et al. (2007). "Autonomous Automobile Trajectory Tracking for Off-Road Driving: Controller Design, Experimental Validation and Racing". American Control Conference.

3. 阿克曼转向:

   • Rajamani, R. (2011). "Vehicle Dynamics and Control". Springer.

---

最后更新: 2025-11-27