108 lines
4.6 KiB
Markdown
108 lines
4.6 KiB
Markdown
# 垄沟行走无法居中/持续侧墙问题诊断报告
|
||
|
||
## 现象描述
|
||
|
||
- 垄沟内可以**笔直向前走**，说明航向控制（IMU）正常。
|
||
- 但**无法在垄沟正中行驶**，车身稳定偏向一侧墙壁。
|
||
- 表现为：持续侧墙行驶，无法回中。
|
||
|
||
> **初步判断**：不是 IMU 精度问题（你确认 IMU 精度很高），而是**横向居中控制存在系统性偏差**。
|
||
|
||
---
|
||
|
||
## 根因分析
|
||
|
||
### 1. 核心 Bug：`corridor_ctrl.c` 控制器绕过 EKF 滤波结果
|
||
|
||
**文件：** `App/nav/corridor_ctrl.c:75-85`
|
||
|
||
```c
|
||
float e_y_ctrl = state->e_y;
|
||
bool left_ok = /* 左前+左后有效 */;
|
||
bool right_ok = /* 右前+右后有效 */;
|
||
|
||
if (left_ok && right_ok) {
|
||
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
|
||
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
|
||
e_y_ctrl = 0.5f * (d_right - d_left); // <-- 直接覆盖 EKF 结果！
|
||
}
|
||
```
|
||
|
||
**问题：**
|
||
|
||
当双侧激光都有效时，控制器**丢弃了 EKF 输出**的 `state->e_y`，改用原始的 `(d_right - d_left) / 2` 作为横向偏差。
|
||
|
||
这个做法隐含一个致命假设：**"车在正中时，左右激光读数相等"**。
|
||
|
||
但实际情况是：
|
||
- 左右 VL53L0X 传感器安装**不可能绝对对称**，哪怕差 2~3mm，在 40cm 垄沟 + 20cm 车体 = 每边仅 10cm 的裕量下，就是 **2~3% 的系统误差**。
|
||
- 控制器拿到这个带偏差的 `e_y_ctrl`，认为"偏左"的位置实际上是正中，于是稳定地把车锁在偏右的位置行驶。
|
||
|
||
**为什么车头能走直但回不了中？**
|
||
|
||
- `kp_theta * e_th` + `kd_theta * imu_wz` 这组负责**航向**，与激光无关 → 车头笔直。
|
||
- `kp_y * e_y_ctrl` 这组负责**横向回中**，但 `e_y_ctrl` 本身带系统性偏置 → 回中目标点本身就是歪的。
|
||
|
||
---
|
||
|
||
### 2. 叠加因素：传感器内缩参数 `left/right_sensor_inset` 未标定
|
||
|
||
**文件：** `App/robot_params.h:107,113`
|
||
|
||
```c
|
||
#define PARAM_VL53_LEFT_INSET 0.0f // 注释：当前为 0.0 表示未标定
|
||
#define PARAM_VL53_RIGHT_INSET 0.0f // 注释：当前为 0.0 表示未标定
|
||
```
|
||
|
||
虽然控制器 bug 是直接原因，但 `left/right_sensor_inset` 未标定也会影响：
|
||
|
||
- **EKF 层面：** `corridor_ekf.c:453-454` 用 `d_center_left = (W - Rw)/2 + left_sensor_inset` 计算期望居中读数。`inset = 0` 时，EKF 本身对单侧退化场景的计算也存在偏差。
|
||
- **单侧退化时恶性循环：** 车因 bug 偏向一侧 → 近墙激光进入饱和/退化区（< 3cm）→ 系统退化为单侧观测 → 未标定的 `inset` 让 EKF 进一步算错位置 → 偏差放大 → 更贴墙。
|
||
|
||
---
|
||
|
||
## 诊断结论
|
||
|
||
| 因素 | 影响程度 | 说明 |
|
||
|------|---------|------|
|
||
| **控制器绕过 EKF（Bug）** | **主因** | 双侧有效时直接丢弃滤波结果，用原始差值驱动回中，引入不可消除的安装不对称误差 |
|
||
| **left/right_sensor_inset 未标定** | 叠加 | 即使修复 bug，单侧退化场景下 EKF 输出仍不精确；标定后整体鲁棒性更好 |
|
||
| IMU 精度 | 无关 | 航向控制正常，IMU 工作良好 |
|
||
| 激光硬件 | 无关 | 传感器能正常输出读数，问题在于怎么用读数 |
|
||
|
||
**总体结论：这是软件/参数问题，不是硬件问题。**
|
||
|
||
---
|
||
|
||
## 修复建议
|
||
|
||
### 方案 A：改代码（推荐，一劳永逸）
|
||
|
||
删除 `App/nav/corridor_ctrl.c` 中第 `75-85` 行的 `if (left_ok && right_ok)` 分支，**始终使用 `state->e_y`** 作为横向偏差输入控制器。
|
||
|
||
理由：
|
||
- EKF 已经综合了 `corridor_width`、`robot_width`、`left/right_sensor_inset` 做了完整几何补偿。
|
||
- EKF 还做了马氏距离异常检测和协方差加权，噪声滤波远好于控制器层直接算差值。
|
||
- 控制器不应该"越级"跳过 EKF 做原始观测融合。
|
||
|
||
### 方案 B：标定参数（也必须做）
|
||
|
||
按 `robot_params.h` 注释中的方法：
|
||
1. 用卷尺将车**精确放在垄沟正中央**。
|
||
2. 记录左右 VL53 前后平均读数。
|
||
3. 计算：
|
||
- `left_inset = 0.10 - 实测左侧平均读数`
|
||
- `right_inset = 0.10 - 实测右侧平均读数`
|
||
4. 填入 `PARAM_VL53_LEFT_INSET` 和 `PARAM_VL53_RIGHT_INSET`。
|
||
|
||
> 标定完成后，即使不删代码，控制器的原始差值 `d_right - d_left` 也会更接近真实居中偏差（因为 EKF 标定后输出的 `state->e_y` 更准确，但 bug 依然意味着双侧有效时 EKF 被绕过）。
|
||
|
||
---
|
||
|
||
## 验证方法
|
||
|
||
修复后上垄沟测试，预期表现：
|
||
1. **笔直 + 居中**：车头不偏，车身稳定在垄沟正中间。
|
||
2. 故意用手把车推向一侧，松手后应自动回中。
|
||
3. 近墙时近墙脱离保护（`wall_escape`）不应频繁触发——因为车本就在中间，不需要"救"。
|