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）：

#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	差速幅度上限（占
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|)。