origincar_controller/doc/ackermann_steering_calibration.md

# 阿克曼转向标定与拟合说明

本文记录阿克曼小车「(Vx, Vz) → 舵机 PWM + 左右后轮速度」控制律的标定数据、
拟合方法、单位约定，三种控制模式（标定运动学 / 直接映射 / 横摆闭环），以及
串口通信（200 Hz DMA 发送、921600 波特率）。对应代码：
- 控制律：`BALANCE/balance.c` 的 `Akm_Car` 分支，
  `Akm_Curvature_To_Servo()`（拟合）与 `Akm_Norm_To_Servo()`（直接满行程映射）。
- 开关宏：`BALANCE/robot_select_init.h`；舵机直行点 `SERVO_INIT` 在 `HARDWARE/motor.h`。
- 串口：`HARDWARE/usartx.c`（`data_task` / `USART3_SEND` / `uart3_dma_tx_init`），
  波特率在 `BALANCE/system.c` 的 `systemInit`。

## 1. 硬件与坐标约定

- 后驱阿克曼：`MOTOR_A` = 左后轮，`MOTOR_B` = 右后轮，前轮由舵机（TIM12 CCR2）转向。
- 输入语义（上位机下发）：
  - `Vx` = 后轴中心线速度，单位 m/s。
  - `Vz` = 绕转弯中心的旋转角速度 wz，单位 rad/s。
- 符号约定：
  - 上位机 `Vz` 遵循 ROS 约定：**Vz > 0 = 逆时针 = 左转**。
  - 标定表 / 舵机拟合使用相反符号：**曲率 κ > 0 = 右转**。
  - 代码中 `kappa_fit = -Vz/Vx`，使 `Vz > 0` 得到 `kappa_fit < 0`（左转），
    与拟合域一致。

## 2. 标定方法（手推法）

电机失能、舵机使能，手动推动小车走出稳定圆弧，对每个舵机 PWM 记录：

- `y 向半截距`（弧对应的纵向半截距，cm）
- `x 向半弓高`（弦的矢高 / 半弓高，cm）
- 由几何反推后轴中心转弯半径 `R`（cm）与 `1/R`

x 向半弓高的符号用于判定左右：PWM > ~1670 为右转（κ 取正），
低于该点为左转（κ 取负）。

### 三个"中位"值（易混，务必区分）

| 名称 | 值 | 含义 | 代码位置 |
|------|---:|------|----------|
| 遥控 CH1 中位 | 1500 | 遥控摇杆物理中点 | `AKM_REMOTER_CH1_MID`（balance.c） |
| 舵机机械中位 | 1600 | 舵机行程几何中点 (MIN+MAX)/2 | —（不再直接使用） |
| **直行点 SERVO_INIT** | **1670** | **实测 1/R≈0、车真正走直线的点** | `SERVO_INIT`（motor.h:52） |

关键：**所有控制律的舵机中位都对齐到 `SERVO_INIT = 1670`**（真正的直行点），
而非机械中位 1600。开机上电、Mode 1 / Mode 2 的零位、CH1 覆盖都以它为基准：
- CH1 覆盖时舵机 = `Remoter_Ch1 + (SERVO_INIT - AKM_REMOTER_CH1_MID)`，即整体
  平移 `1670 - 1500 = +170`，使摇杆居中 = 车轮回正。
- 改 `SERVO_INIT` 一处，下游所有路径自动对齐。

### 原始标定数据

| servo(pwm) | y半截距/cm | x半弓高/cm | R/cm | 1/R (1/cm) |
|-----------:|----------:|----------:|-----:|-----------:|
| 2000 | 34    | 34   | 34.00   | 0.029412 |
| 1900 | 52.5  | 49.5 | 52.59   | 0.019015 |
| 1800 | 90    | 60   | 97.50   | 0.010256 |
| 1700 | 167.2 | 60   | 262.97  | 0.003803 |
| 1670 | 120   | -1   | 7200.5  | 0.000139 |
| 1600 | 120   | -23  | 324.54  | 0.003081 |
| 1500 | 89    | -60  | 96.01   | 0.010416 |
| 1400 | 61    | -60  | 61.01   | 0.016391 |
| 1300 | 43    | -44  | 43.01   | 0.023250 |
| 1200 | 35.5  | -34.3| 35.52   | 0.028152 |
| 1100 | 30    | -28.9| 30.02   | 0.033310 |

> 直行点约在 PWM 1670（1/R ≈ 0，x 半弓高 ≈ 0），并非舵机机械中位 1600。

## 3. 单位换算（关键，曾导致 100 倍错误）

标定表 `1/R` 列以 **1/cm** 为单位（R 用厘米）。而代码里的曲率来自
`kappa = wz / Vx`（均为 SI），单位是 **1/m**。两者相差 100 倍。

拟合前必须把 R 从 cm 换算为 m，再取 `kappa = 1/R_m`（并带符号）：

| servo | kappa (1/m, +=右) |
|------:|------------------:|
| 2000  | +2.9412 |
| 1900  | +1.9015 |
| 1800  | +1.0256 |
| 1700  | +0.3803 |
| 1670  | +0.0139 |
| 1600  | -0.3081 |
| 1500  | -1.0416 |
| 1400  | -1.6391 |
| 1300  | -2.3250 |
| 1200  | -2.8152 |
| 1100  | -3.3310 |

- 曲率量程：κ ∈ [−3.331, +2.941] 1/m。
- 最小转弯半径 R_min ≈ 0.30 m（左）/ 0.34 m（右）。
- 代码中 `AKM_KAPPA_MAX = 3.331f`。

## 4. 拟合方法与结果

以 `servo = f(kappa)`（kappa 为自变量，1/m）做多项式最小二乘。
比较一/二/三次，并用留一交叉验证（LOO）评估对未见点的泛化：

| 拟合 | 全量 max残差 | 全量 rms | LOO max误差 | LOO rms |
|------|-----------:|--------:|----------:|-------:|
| 一次 | 56.8 | 28.3 | — | — |
| 二次 | 12.3 | 7.6 | 15.1 | 10.2 |
| 三次 | 12.6 | 7.5 | — | — |

结论：**二次拟合最优**。三次不再改善（过拟合），一次残差过大。

### 最终系数（1/m 单位）

```
servo = 1656.373 + 140.548 * kappa - 7.654 * kappa^2
```

代码宏（`BALANCE/balance.c`）：

```c
#define AKM_SERVO_C0   1656.373f
#define AKM_SERVO_C1    140.548f
#define AKM_SERVO_C2     (-7.654f)
#define AKM_SERVO_MIN   1100
#define AKM_SERVO_MAX   2000
#define AKM_KAPPA_MAX   3.331f
```

### 为什么用拟合而非查表 + 线性插值

- 数据是手推测得，含测量噪声。查表被迫穿过每个噪声点，两点间直线段会
  放大噪声；LOO 显示查表 max 误差 23 / rms 14，明显差于二次拟合（15 / 10）。
- 拟合在端点外推时形状正确（曲线两端明显弯），线性插值只能按最后一段
  斜率外推，越推越偏。
- 拟合在节点处平滑，无斜率突变；运行成本仅两次乘加，比查找区间还省。

### 轴距 L 去哪了

标定表端到端直接测了 `servo → R`，已把「舵机 PWM → 前轮转角 →
`R = L/tan(δ)`」整条链路及传动比、轮胎侧偏等真实效应吸收进拟合系数。
因此无需再显式写 `R = Axle_spacing/tan(δ)`（那反而依赖不准的假设传动比）。
后轮差速只用轮距 track 与 κ，本就不含 L。阿克曼几何没有丢，而是以更贴合
实车的实测形式嵌入拟合。

## 5. 控制律（Mode 0：标定运动学，默认）

`AKM_DIRECT_MAP = 0` 时：

```
kappa_geom = Vz / Vx            (Vx≈0 时置 0，阿克曼无前进不能转向)
kappa_fit  = clamp(-kappa_geom, ±AKM_KAPPA_MAX)
Servo      = f(kappa_fit)                          # 二次拟合
MOTOR_A(左) = Vx * (1 + 0.5*track*kappa_fit)       # 后轮差速
MOTOR_B(右) = Vx * (1 - 0.5*track*kappa_fit)
```

物理特性：舵机角只决定转弯半径 R = 1/κ。
- 固定舵机角、改 Vx → wz 随之变（走同一圆，快慢不同）。
- 固定 wz、改 Vx → κ = wz/Vx 变 → 舵机角变（正确行为）。

### Vx–wz 可行域（κ_max ≈ 3.33 /m）

`|wz| <= Vx * κ_max`：

| Vx (m/s) | 可用 wz 范围 (rad/s) |
|---------:|:--------------------|
| 0.2 | [−0.67, 0.67] |
| 0.3 | [−1.00, 1.00] |
| 0.5 | [−1.67, 1.67] |
| 1.0 | [−3.33, 3.33] |

> 例：固定 wz=0.12，需 Vx ≥ 0.036 m/s 才不被夹紧。之前 `AKM_KAPPA_MAX`
> 误设为 0.0334（对应 R≈30 m）导致任何速度下都被夹死、改速度舵机不动，
> 已随单位修正解决。

## 6. 控制律（Mode 1：直接映射，调试用）

`AKM_DIRECT_MAP = 1` 时（脱离阿克曼物理，用于隔离调试舵机/电机）：

```
vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, ±1)
Servo   = Akm_Norm_To_Servo(vz_norm)     # 以 SERVO_INIT 为零位的分段满行程映射
MOTOR_A(左) = Vx                          # 无差速
MOTOR_B(右) = Vx
```

- `Vz` 线性铺满整个舵机行程 [AKM_SERVO_MIN, AKM_SERVO_MAX]，与速度无关。
- **零位对齐直行点**：`Akm_Norm_To_Servo` 以 `SERVO_INIT=1670`（而非机械中位
  1600）为中心，左右两侧各自缩放到自己的端点，即使中位偏置也能用满全行程：
  ```
  span = (norm>=0) ? (SERVO_INIT - AKM_SERVO_MIN)   # 左侧行程 1670-1100=570
                   : (AKM_SERVO_MAX - SERVO_INIT)   # 右侧行程 2000-1670=330
  Servo = SERVO_INIT - norm * span
  ```
- `Vz > 0`（左）→ `norm>0` → 靠近 AKM_SERVO_MIN（左端），符号与 ROS 一致。
- `Vx` 原样给左右电机，不做曲率/差速运算。
- 满量程输入由 `AKM_DIRECT_VZ_FULL`（默认 **1.0 rad/s**）设定。

> **`AKM_DIRECT_VZ_FULL` 的物理含义** = 上位机会发的最大 angular.z，Vz 到该值时
> 舵机打满。它只决定"Vz→行程"的比例，不改变峰值转角（峰值由舵机端点 1100/2000
> 决定，恒为 ~28°左 / 25°右）。太小→小 Vz 就饱和、失去比例控制；太大→常用区间
> 只用到一小段行程、转角偏小。当前定为 1.0（Vz=±1.0 打满），把行程摊到 ±1 rad/s
> 全区间，规划分辨率比 0.5 时翻倍。Vz 在 RX 中断里以 rad/s 到达
> （usartx.c `XYZ_Target_Speed_transition`: raw/1000）。

## 7. 控制律（Mode 2：横摆角速度闭环 / 简化扭矩矢量）

`AKM_DIRECT_MAP = 0` 且 `AKM_YAW_ASSIST = 1` 时启用。

**动机（为什么要有 Mode 2）**：Mode 0 里 `κ = Vz/Vx`，同一个转向指令 `Vz` 在
高速时曲率被 `Vx` 除小，舵机自动回正 —— 这就是"高速转弯打不动"的根源。Mode 2
**主动解耦 w 与 v**：舵机由 `Vz` 直接决定（与 Mode 1 一样，不再除以 Vx），
所以大 `Vz` 在任何速度都给出大前轮角；阿克曼只作为后轮差速的**前馈参考**，
再叠加 IMU 横摆角速度 PI 闭环（简化扭矩矢量）。控制链路：

```
Vz ─(直接满行程映射, 与Vx无关)→ 舵机主转向 Akm_Norm_To_Servo   ← 方向主控，解耦
Vx ───────────────────────────→ 左右轮基速                      ← 驱动主控，解耦
(Vx, κ_cmd) ─(阿克曼几何)→ r_ref ─→ 后轮差速前馈 + IMU PI          ← akm 仅做前馈
```

计算（fit 域 κ>0=右；ROS Vz>0=左=CCW；r>0=左转）：

```
vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, ±1)   # 转向指令，不含 Vx —— 关键解耦点
Servo   = Akm_Norm_To_Servo(vz_norm)          # 与 Mode 1 完全相同的满行程直接映射
                                              # 以 SERVO_INIT 为零位，不走拟合曲线
κ_cmd   = -vz_norm * AKM_KAPPA_MAX             # 仅用于构造下方前馈参考，不驱动舵机

r_ref   = -κ_cmd * Vx                         # 阿克曼几何前馈的期望横摆角速度
r_meas  = AKM_GYRO_Z_SIGN * gyro[2] / AKM_GYRO_Z_TO_RADPS
r_filt += AKM_YAW_IMU_LPF * (r_meas - r_filt)  # 轻度一阶低通

e_r     = r_ref - r_filt
dv_ff   = AKM_YAW_FF_ALPHA * 0.5 * track * r_ref  # 几何前馈
dv_fb   = AKM_YAW_KP * e_r + AKM_YAW_KI * ∫e_r    # PI 反馈（矩形积分, dt=1/200s）
dv      = clamp(dv_ff + dv_fb, ±AKM_YAW_MAX_DIFF_RATIO*|Vx|)  # 带抗积分饱和

MOTOR_A(左) = Vx - dv
MOTOR_B(右) = Vx + dv
```

要点：
- **解耦是核心**：`vz_norm` 只由 `Vz` 决定，不再 `Vz/Vx`，所以高速大转向不再被
  几何"稀释"。`Vx` 独立设定驱动基速。
- **舵机走直接满行程映射，不走拟合**：Mode 2 的舵机与 Mode 1 / CH1 调试一致，
  用 `Akm_Norm_To_Servo(vz_norm)` 把转向指令线性铺满行程（以 `SERVO_INIT` 为
  零位），**不再调用标定拟合 `f(κ)`**。这样"高速转弯打不动"从根上消失——舵机角
  只看指令、与速度无关。（早期版本这里错用了 `f(κ_cmd)`，仍隐含耦合，已改正。）
- **阿克曼降级为纯前馈**：`κ_cmd = -vz_norm*AKM_KAPPA_MAX` 只用来构造 `r_ref`
  ——"若车按该曲率走出的理想横摆角速度"，仅喂给后轮差速前馈；实际横摆由 IMU PI
  收敛，可克服前轮几何/打滑带来的偏差。`κ_cmd` 不参与舵机计算。
- **退化关系**：`AKM_YAW_FF_ALPHA = 1` 且 `AKM_YAW_KP = AKM_YAW_KI = 0` 时，
  `dv = 0.5*track*r_ref = -0.5*track*κ_cmd*Vx`，即纯几何差速。α 是平滑旋钮：
  0 = 纯 IMU 反馈，1 = 纯几何前馈。
- **低速冻结**：`|Vx| < AKM_YAW_MIN_SPEED` 时清零积分并停用差速（低速横摆角
  速度信噪比太差）。
- 用原始角速度（`gyro[2]` 去零偏后的 LSB），不做航向角积分。

### 上位机规划用：发布 Vz → 大致前轮转角 δ

Mode 2 里舵机只由 `Vz` 决定（与 `Vx` 无关），所以上位机可直接按下表估转角。
换算链路（非线性，因为 PWM→R 那段是标定曲线）：

```
vz_norm = clamp(Vz / AKM_DIRECT_VZ_FULL, ±1)      # 默认 VZ_FULL=1.0 → Vz=±1.0 打满
Servo   = SERVO_INIT - vz_norm * span             # 左 span=570, 右 span=330
R       = 1 / |1/R(Servo)|                         # 1/R 由 §2 标定表(1/cm)插值再×100
δ       = atan(L / R)                              # L = Akm_axlespacing = 0.160 m
```

用当前宏值（`SERVO_INIT=1670`, `MIN=1100`, `MAX=2000`, `VZ_FULL=1.0`）、按 §2 标定表
`1/R` 列插值算出的对照表（δ 为后轴等效前轮转角，正数只表大小，方向见末列）：

| Vz (rad/s) | vz_norm | Servo(PWM) | R(m) | δ(°) | 方向 |
|-----------:|--------:|-----------:|-----:|-----:|:----:|
| +1.0 | +1.0 | 1100 | 0.30 | 28.1 | 左 |
| +0.8 | +0.8 | 1214 | 0.36 | 23.7 | 左 |
| +0.6 | +0.6 | 1328 | 0.47 | 18.8 | 左 |
| +0.5 | +0.5 | 1385 | 0.57 | 15.6 | 左 |
| +0.4 | +0.4 | 1442 | 0.72 | 12.5 | 左 |
| +0.3 | +0.3 | 1499 | 0.95 |  9.5 | 左 |
| +0.2 | +0.2 | 1556 | 1.59 |  5.8 | 左 |
| +0.1 | +0.1 | 1613 | 3.95 |  2.3 | 左 |
|  0.0 |  0.0 | 1670 | ∞    |  0.0 | 直 |
| −0.1 | −0.1 | 1703 | 2.50 |  3.7 | 右 |
| −0.2 | −0.2 | 1736 | 1.63 |  5.6 | 右 |
| −0.3 | −0.3 | 1769 | 1.21 |  7.5 | 右 |
| −0.4 | −0.4 | 1802 | 0.96 |  9.5 | 右 |
| −0.5 | −0.5 | 1835 | 0.75 | 12.0 | 右 |
| −0.6 | −0.6 | 1868 | 0.62 | 14.5 | 右 |
| −0.8 | −0.8 | 1934 | 0.44 | 19.8 | 右 |
| −1.0 | −1.0 | 2000 | 0.34 | 25.2 | 右 |

要点（上位机规划务必注意）：
- **左右不对称**：直行点 1670 偏向右端，左侧行程 570、右侧仅 330，所以同样
  `|Vz|` 左转角比右转角略大（满量程 28° 左 vs 25° 右）。这是机械中位偏置导致的，
  已被 `Akm_Norm_To_Servo` 的分段缩放吸收。
- **`|Vz| ≥ 1.0` 全部饱和**到端点（28°左 / 25°右）；再大的 Vz 不会有更大转角。
  `VZ_FULL` 从 0.5 提到 1.0 后，满量程指令 = ±1.0 rad/s，同样的舵机行程摊到更宽的
  Vz 区间，规划分辨率翻倍（峰值转角不变）。
- **非线性**：Vz→δ 不是直线（低 Vz 段每 0.1 约 +3°，高 Vz 段趋缓），规划时按表
  插值而非线性外推。
- R 是**后轴中心转弯半径**，δ = atan(L/R) 是等效前轮转角；实车受轮胎侧偏/打滑
  影响，表值为标定静推的近似，动态下 IMU 横摆环会再做修正。

调参顺序（用户建议）：
1. 先标定陀螺零偏（开机静止采样，代码已做）。
2. **实测确认 `gyro[2]` 符号**：命令左转，确认 `gyro[2] > 0`；若相反把
   `AKM_GYRO_Z_SIGN` 改为 `-1.0f`。
3. 小 `Kp`（0.05~0.10）起步，逐步加大到临界前。
4. 有稳态误差再加一点 `Ki`。
5. 最后引入前馈 `α = 0.2~0.5` 减轻 PI 负担。

## 8. 开关一览（`BALANCE/robot_select_init.h`）

| 宏 | 默认 | 含义 |
|----|-----:|------|
| `AKM_SERVO_DEBUG_REMOTE_CH1` | 1 | 1 = 舵机直接由遥控 CH1 驱动，覆盖所有控制律（板级调试） |
| `AKM_DIRECT_MAP` | 1 | 1 = 直接映射调试模式；0 = 交给 Mode 0 / Mode 2 |
| `AKM_DIRECT_VZ_FULL` | 1.0f | Mode 1 & Mode 2 共用：Vz 满行程量程（rad/s）= 上位机会发的最大 angular.z |
| `AKM_YAW_ASSIST` | 0 | 1 = 横摆闭环（Mode 2）；仅在 `AKM_DIRECT_MAP=0` 时生效 |
| `AKM_YAW_KP` / `AKM_YAW_KI` | 0.10 / 0.00 | 横摆角速度误差 PI 增益（m/s per rad/s） |
| `AKM_YAW_FF_ALPHA` | 0.00f | 几何前馈混合系数（0=纯反馈，1=纯几何差速=Mode 0） |
| `AKM_YAW_MIN_SPEED` | 0.10f | 低于此速度冻结横摆环（m/s） |
| `AKM_YAW_MAX_DIFF_RATIO` | 0.35f | 差速幅度上限（占 |Vx| 的比例） |
| `AKM_GYRO_Z_TO_RADPS` | 3754.9f | gyro[2] LSB→rad/s（FS ±500dps） |
| `AKM_GYRO_Z_SIGN` | +1.0f | IMU +z 与 ROS（+=左）符号对齐，实测确认 |
| `AKM_YAW_IMU_LPF` | 0.30f | 横摆角速度一阶低通系数（0=无滤波） |

模式互斥与优先级：
- `AKM_SERVO_DEBUG_REMOTE_CH1 = 1` 时，**舵机总是由 CH1 覆盖**，优先于任何控制
  律计算出的舵机值（电机差速仍按所选模式运行）。
- `AKM_DIRECT_MAP` 与 `AKM_YAW_ASSIST` 都作用于整条控制律：`AKM_DIRECT_MAP = 1`
  优先（直接映射）；`= 0` 时再看 `AKM_YAW_ASSIST`（1 = Mode 2 横摆闭环，
  0 = Mode 0 标定运动学）。

## 9. 串口通信（ROS ↔ STM32）

### 频率与方向

| 方向 | 频率 | 机制 | 代码位置 |
|------|-----:|------|----------|
| 控制环 | 200 Hz | `Balance_task`，`RATE_200_HZ` | balance.c:350 |
| 发送（TX，STM32→ROS） | 200 Hz | **DMA 非阻塞** | usartx.c `data_task` |
| 接收（RX，ROS→STM32） | 中断驱动 | `USART3_IRQHandler`（RXNE） | usartx.c |

- 只保留 **USART3（ROS）**。原先 `data_task` 20 Hz 阻塞式群发 USART1/3/5+CAN，
  现已删掉 USART1/USART5/CAN 发送，只留 USART3。
- 帧长 24 字节（`SEND_DATA_SIZE`），`Vz` 在 RX 中断里由
  `XYZ_Target_Speed_transition` 解析：`raw/1000 + (raw%1000)*0.001`（rad/s）。

### 为什么改 DMA 非阻塞发送

旧的 `usart3_send` 是忙等：`USART3->DR = data; while((USART3->SR&0x40)==0);`。
`data_task` 与 `Balance_task` **同为优先级 4**（FreeRTOS 抢占 + 时间片，1ms tick）。
同优先级下忙等无法被控制环抢占，20→200 Hz 会放大抖动、白耗 CPU。改成 DMA 后：

- `USART3_SEND` 只触发一次 DMA 传输（DMA1 Stream3 / Channel4 = USART3_TX），
  CPU 立即返回，shift-out 期间不占用控制环。
- 上一帧未发完则跳过本周期（`DMA_GetCmdStatus != DISABLE` 判定），不忙等。
- 需要把 `FWLIB/src/stm32f4xx_dma.c` 加入 Makefile（原先未编译该驱动）。

### 波特率 921600

USART3 波特率 **115200 → 921600**（system.c:129，`uart3_init(921600)`）。

- 一帧 24 字节 = 240 bit（8N1，10 bit/byte）的 shift-out 时间：
  - 115200：240/115200 ≈ **2.08 ms**
  - 921600：240/921600 ≈ **0.26 ms**（快 8 倍，远低于 5 ms 周期）
- 带宽占用：200 Hz × 24 B = 4800 B/s，两档都绰绰有余；提速主要是留余量。
- APB1=42 MHz，16 倍过采样分频后实际约 913 k，偏差 ~0.9%（UART 容忍 <2.5%）。
- **ROS 端必须同步改 921600**，否则乱码——这是最易漏的一步。

## 10. 复现拟合

标定原始脚本与中间产物在 `calibration/` 目录。核心步骤：
1. R(cm) → R(m)，`kappa = sign / R_m`（sign 由 x 半弓高定，+ 为右）。
2. 对 `(kappa, servo)` 做二次最小二乘，得 C0/C1/C2。
3. 用 LOO 交叉验证确认二次优于查表与三次。
4. `AKM_KAPPA_MAX = max(|kappa|)`。