# 算法说明

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

## 目录

- [AGV 运动学模型](#agv-运动学模型)
- [Pure Pursuit 算法](#pure-pursuit-算法)
- [Stanley 算法](#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` 是加速度

#### 离散化实现

使用欧拉法进行离散化：

```cpp
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`: 前视距离

### 实现代码

```cpp
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)
```

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

### 实现代码

```cpp
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. 二阶导数连续（加速度连续）

#### 实现

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

```cpp
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）
- 路径复杂多变
- 有充足计算资源

### 混合策略

可以根据速度动态切换：

```cpp
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: 圆形路径跟踪

```cpp
// 生成半径 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: 复杂路径跟踪

```cpp
// 从 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