143 lines
4.9 KiB
Markdown
143 lines
4.9 KiB
Markdown
# 垄沟横向偏移问题 — 代码层面分析
|
||
|
||
> 基于 `问题诊断报告.md` 的二次审查，对根因优先级提出不同判断。
|
||
|
||
---
|
||
|
||
## 现象回顾
|
||
|
||
- 车头笔直（IMU 航向控制正常）
|
||
- 车身稳定偏向一侧墙壁，无法居中
|
||
|
||
---
|
||
|
||
## 代码审查发现
|
||
|
||
### 发现 1：EKF 的 `side_sensor_inset` fallback 从未实现（主因）
|
||
|
||
**文件：** `App/est/corridor_ekf.c:451-454`
|
||
|
||
```c
|
||
float d_center_left = (W - Rw) / 2.0f + s_cfg.left_sensor_inset; // = 0.10 + 0.0 = 0.10
|
||
float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset; // = 0.10 + 0.0 = 0.10
|
||
```
|
||
|
||
**文件：** `App/robot_params.h:96-113`
|
||
|
||
```c
|
||
#define PARAM_VL53_SIDE_INSET (-(0.018f)) // 已实测！但从未被 EKF 使用
|
||
#define PARAM_VL53_LEFT_INSET 0.0f // 未标定
|
||
#define PARAM_VL53_RIGHT_INSET 0.0f // 未标定
|
||
```
|
||
|
||
**问题：**
|
||
|
||
`robot_params.h` 注释写着 `side_sensor_inset` 是"未分侧设置时的默认值"，但 `corridor_ekf.c` 中**没有任何 fallback 逻辑**——它直接用 `left_sensor_inset`（0.0f）和 `right_sensor_inset`（0.0f），完全忽略了已经实测好的 `-0.018f`。
|
||
|
||
**影响：** EKF 观测方程在双侧有效时退化为：
|
||
|
||
```
|
||
z_ey = (d_right_avg - d_left_avg) / 2
|
||
```
|
||
|
||
没有任何安装偏置补偿。如果传感器安装面距车体边缘实际有 18mm 的偏差（`side_sensor_inset = -0.018`），或者左右不对称，EKF **本身就会算出带系统性偏置的 `e_y`**。
|
||
|
||
### 发现 2：控制器 bypass 绕过 EKF（阻塞性 bug）
|
||
|
||
**文件：** `App/nav/corridor_ctrl.c:75-85`
|
||
|
||
```c
|
||
float e_y_ctrl = state->e_y;
|
||
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 结果
|
||
}
|
||
```
|
||
|
||
**问题：**
|
||
|
||
当双侧激光都有效时，控制器丢弃 `state->e_y`，改用原始 `(d_right - d_left)/2`。这个公式**不含任何 inset 补偿**。
|
||
|
||
**关键推论：** 当前 `left/right_sensor_inset` 都是 0.0f 时，EKF 的 `z_ey` 和 bypass 的公式**完全等价**：
|
||
|
||
```
|
||
EKF: z_ey = [(W-Rw)/2 + 0 - d_left] + [d_right - (W-Rw)/2 - 0] / 2 = (d_right - d_left) / 2
|
||
Bypass: e_y = (d_right - d_left) / 2
|
||
```
|
||
|
||
所以**当前状态下 bypass 不是偏移的直接制造者**——EKF 输出与 bypass 结果几乎相同。但 bypass 的架构问题在于：即使将来标定了 inset，控制器依然用不上——它直接跳过了 EKF 的补偿结果。
|
||
|
||
### 发现 3：无其他符号/算法错误
|
||
|
||
- 控制律符号正确：`e_y > 0`（偏左）→ `w_cmd < 0`（右转回中）
|
||
- EKF 预测/更新、Joseph 协方差更新、马氏距离检验均无明显问题
|
||
- IMU 航向观测通道独立且正确
|
||
|
||
---
|
||
|
||
## 与原诊断报告的分歧
|
||
|
||
| 因素 | 原报告评级 | 本次评级 | 理由 |
|
||
|------|-----------|---------|------|
|
||
| 控制器 bypass | **主因** | 阻塞性 bug | 当前 inset=0 时 bypass 与 EKF 输出等价，不是偏移的直接来源；但堵死了标定修复的路 |
|
||
| inset 未生效 | 叠加因素 | **主因** | `-0.018f` 已实测但 EKF 从未使用；left/right=0 导致观测方程无偏置补偿 |
|
||
|
||
**核心观点：** 原报告说"删 bypass 就一劳永逸"，但如果只删 bypass 而不修 inset，EKF 自身的 `state->e_y` 同样带偏——问题不会改善。
|
||
|
||
---
|
||
|
||
## 修复建议
|
||
|
||
### 修复 1：EKF 添加 inset fallback（立刻见效）
|
||
|
||
**文件：** `App/est/corridor_ekf.c` 第 451-454 行附近
|
||
|
||
```c
|
||
// 修改前：
|
||
float d_center_left = (W - Rw) / 2.0f + s_cfg.left_sensor_inset;
|
||
float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset;
|
||
|
||
// 修改后：
|
||
float l_inset = (s_cfg.left_sensor_inset != 0.0f) ? s_cfg.left_sensor_inset : s_cfg.side_sensor_inset;
|
||
float r_inset = (s_cfg.right_sensor_inset != 0.0f) ? s_cfg.right_sensor_inset : s_cfg.side_sensor_inset;
|
||
float d_center_left = (W - Rw) / 2.0f + l_inset;
|
||
float d_center_right = (W - Rw) / 2.0f + r_inset;
|
||
```
|
||
|
||
这样 `-0.018f` 会立即作为两侧默认值生效，消除统一偏置。
|
||
|
||
### 修复 2：删除控制器 bypass（必须配合修复 1）
|
||
|
||
**文件：** `App/nav/corridor_ctrl.c` 第 75-85 行
|
||
|
||
删除 `if (left_ok && right_ok) { ... }` 分支，始终使用 `state->e_y`。
|
||
|
||
### 修复 3：实车标定分侧 inset（消除左右不对称）
|
||
|
||
1. 将车精确放在垄沟正中央（卷尺确认）
|
||
2. 记录左右 VL53 前后平均读数
|
||
3. 计算：
|
||
- `left_inset = 0.10 - 实测左侧平均读数`
|
||
- `right_inset = 0.10 - 实测右侧平均读数`
|
||
4. 填入 `PARAM_VL53_LEFT_INSET` 和 `PARAM_VL53_RIGHT_INSET`
|
||
|
||
---
|
||
|
||
## 验证方法
|
||
|
||
### 快速验证（不需要上车）
|
||
|
||
在调试日志中对比以下两个值：
|
||
- `state->e_y`（EKF 输出）
|
||
- `(d_right - d_left) / 2`（bypass 计算）
|
||
|
||
如果两者几乎相同，证实 bypass 不是偏移的直接来源，inset 缺失才是。
|
||
|
||
### 上车验证
|
||
|
||
修复 1+2 后：
|
||
1. 车应从垄沟正中启动后保持居中
|
||
2. 手推偏后应能自动回中
|
||
3. `wall_escape` 不应频繁触发
|