472 lines
22 KiB
Markdown
472 lines
22 KiB
Markdown
# 阿克曼转向标定与拟合说明
|
||
|
||
本文记录阿克曼小车「(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`（左转），
|
||
与拟合域一致。
|
||
- **转向方向只由 `Vz` 决定**：`Vx` 的正负只表示前进/倒车，不允许改变前轮的
|
||
物理偏转方向。因此从上位机到 MCU 的任何一层都不应执行
|
||
`if (Vx < 0) Vz = -Vz` 一类处理。
|
||
|
||
## 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` 且 `AKM_YAW_ASSIST = 0` 时：
|
||
|
||
```
|
||
kappa_geom = Vz / |Vx| (Vx≈0 时置 0，Vx 正负不改变舵机方向)
|
||
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` 从正值改成同幅值负值 → 舵机方向和大小均保持不变。
|
||
|
||
### 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` | 0 | 1 = 舵机直接由遥控 CH1 驱动，覆盖所有控制律（板级调试） |
|
||
| `AKM_DIRECT_MAP` | 0 | 1 = 直接映射调试模式；0 = 交给 Mode 0 / Mode 2 |
|
||
| `AKM_DIRECT_VZ_FULL` | 1.0f | Mode 1 & Mode 2 共用：Vz 满行程量程（rad/s）= 上位机会发的最大 angular.z |
|
||
| `AKM_YAW_ASSIST` | 1 | 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。
|
||
这不表示其他串口没有初始化：当前固件仍初始化 USART1、USART2、USART3、UART5，
|
||
且接收中断仍可用。
|
||
- 帧长 44 字节（`SEND_DATA_SIZE`），在原 24 字节布局后增加 MCU 会话 ID、编码器
|
||
采样时间戳和 IMU 采样时间戳。`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)`）。
|
||
|
||
- 一帧 44 字节 = 440 bit（8N1，10 bit/byte）的 shift-out 时间：
|
||
- 115200：440/115200 ≈ **3.82 ms**
|
||
- 921600：440/921600 ≈ **0.48 ms**（快 8 倍，远低于 5 ms 周期）
|
||
- 带宽占用：200 Hz × 44 B = 8800 B/s；921600 为扩展帧和后续调试留足余量。
|
||
- APB1=42 MHz，16 倍过采样分频后实际约 913 k，偏差 ~0.9%（UART 容忍 <2.5%）。
|
||
- **ROS 端必须同步改 921600**，否则乱码——这是最易漏的一步。
|
||
|
||
## 10. 倒车时舵机反向：控制链排查记录
|
||
|
||
### 现象
|
||
|
||
期望发送 `Vz > 0` 时前轮始终保持左转，但实机表现为：`Vx` 从正值切换为负值后，
|
||
舵机转向随之反转。
|
||
|
||
### MCU 侧检查结果
|
||
|
||
USART3 接收路径为：
|
||
|
||
```
|
||
USART3_IRQHandler
|
||
-> Move_X = received Vx
|
||
-> Move_Z = received Vz
|
||
-> Drive_Motor(Move_X, Move_Y, Move_Z)
|
||
-> Servo
|
||
```
|
||
|
||
- USART3 中断直接执行 `Move_Z = Vz`，没有根据 `Vx` 翻转符号。
|
||
- Mode 1 和当前 Mode 2 的舵机值只读取 `Vz`，与 `Vx` 无关。
|
||
- Mode 0 已使用 `Vz/|Vx|`，倒车同样不会改变舵机方向。
|
||
- TIM12 CCR2 是最终舵机 PWM 输出，控制任务没有在输出端根据电机方向再次翻转。
|
||
|
||
因此，如果实机舵机随 `Vx` 正负反向，第一步应检查 MCU **实际收到的 `Vz`**，
|
||
不能只看发送界面上显示的目标值。
|
||
|
||
### 远端 ROS 实测证据
|
||
|
||
实机运行环境：`root@192.168.11.169:/root/yiliao2026`。实际节点配置为：
|
||
|
||
- `/cmd_vel` 唯一订阅者：`origincar_base`
|
||
- `/cmd_vel` 唯一发布者：`rosbridge_websocket`
|
||
- `akm_cmd_vel = none`，未运行 `cmd_vel_to_ackermann_drive.py`
|
||
- `origincar_base::Cmd_Vel_Callback()` 分别独立编码 `linear.x` 和 `angular.z`，没有改号
|
||
|
||
但 `origincar_base` 的 launch 日志记录到同一次按键切换中的真实输入为：
|
||
|
||
```text
|
||
linear.x = -1, angular.z = -0.74
|
||
linear.x = +1, angular.z = +0.74
|
||
linear.x = -1, angular.z = -0.74
|
||
```
|
||
|
||
这证明舵机反向发生在 USART3 之前：发布端已经在倒车时把 `angular.z` 反号，MCU
|
||
只是执行收到的命令。
|
||
|
||
### 根因与修复
|
||
|
||
本机 rosbridge 键盘客户端：
|
||
|
||
`/Users/chenyouyuan/cyy_ws/smart_car_2026/src/bridge_client.py`
|
||
|
||
静态映射本身正确：
|
||
|
||
- `down_left`：`linear.x < 0`、`angular.z > 0`
|
||
- `down_right`：`linear.x < 0`、`angular.z < 0`
|
||
|
||
但旧的组合键分支在按下 `S` 时交换了 A/D：
|
||
|
||
```python
|
||
# 旧逻辑：错误
|
||
S + D -> down_left
|
||
S + A -> down_right
|
||
```
|
||
|
||
现已改为保持方向键与 `Vz` 符号一致：
|
||
|
||
```python
|
||
# 新逻辑
|
||
W + A -> Vx > 0, Vz > 0
|
||
S + A -> Vx < 0, Vz > 0
|
||
W + D -> Vx > 0, Vz < 0
|
||
S + D -> Vx < 0, Vz < 0
|
||
```
|
||
|
||
修改客户端后必须重启 `bridge_client.py` 才会生效。该文件位于
|
||
`origincar_controller` Git 仓库之外，不包含在控制器固件提交中。
|
||
|
||
### 推荐排查顺序
|
||
|
||
1. 在 `/cmd_vel` 入口记录 `linear.x` 与 `angular.z`，确认发布端实际输出。
|
||
2. 在 `origincar_base::Cmd_Vel_Callback()` 记录串口打包前的值。
|
||
3. 在 MCU 调试器中观察 `test_movz`、`Move_X`、`Move_Z` 和 `Servo`。
|
||
4. 检查 `Remote_ON_Flag`，避免 TIM8 航模遥控输入抢占 USART3 控制模式。
|
||
5. 最后检查 `TIM12->CCR2`，确认是否还有其他路径覆盖最终 PWM。
|
||
|
||
判断原则：若 `Vx` 改号时上游 `angular.z` 也改号，应修发布端；只有在 MCU 收到的
|
||
`Move_Z` 符号保持不变而 `Servo` 仍反向时，才继续修改 MCU 控制律。
|
||
|
||
## 11. 复现拟合
|
||
|
||
标定原始脚本与中间产物在 `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|)`。
|