agv/RCS-3000
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

initial

Browse Source
This commit is contained in:
CaiXiang
2025-11-27 15:20:21 +08:00
parent 3e2884ea53
commit 7b6c956b6a
49 changed files with 2042 additions and 10294 deletions
Download Patch File Download Diff File Expand all files Collapse all files

492
docs/ALGORITHM.md Normal file
View File

@@ -0,0 +1,492 @@
# 算法说明
本文档详细说明 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