From 4dadbeeb5cb59459a6ad213e1af5a17edeffe8c0 Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Wed, 12 Aug 2026 19:29:35 +0800 Subject: [PATCH] =?UTF-8?q?=E8=A1=A5=E5=85=85=E9=98=BF=E5=85=8B=E6=9B=BC?= =?UTF-8?q?=E8=BD=AC=E5=90=91=E7=AC=A6=E5=8F=B7=E6=8E=92=E6=9F=A5=E8=AF=B4?= =?UTF-8?q?=E6=98=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 记录倒车舵机反向的 ROS 到 USART3 控制链定位过程，并同步校正当前控制模式、曲率符号及 44 字节遥测帧说明。 --- doc/ackermann_steering_calibration.md | 135 ++++++++++++++++++++++---- 1 file changed, 116 insertions(+), 19 deletions(-) diff --git a/doc/ackermann_steering_calibration.md b/doc/ackermann_steering_calibration.md index f85f4a8..994f957 100644 --- a/doc/ackermann_steering_calibration.md +++ b/doc/ackermann_steering_calibration.md @@ -18,8 +18,11 @@ - 符号约定： - 上位机 `Vz` 遵循 ROS 约定：**Vz > 0 = 逆时针 = 左转**。 - 标定表 / 舵机拟合使用相反符号：**曲率 κ > 0 = 右转**。 - - 代码中 `kappa_fit = -Vz/Vx`，使 `Vz > 0` 得到 `kappa_fit < 0`（左转）， + - 代码中 `kappa_fit = -Vz/|Vx|`，使 `Vz > 0` 得到 `kappa_fit < 0`（左转）， 与拟合域一致。 +- **转向方向只由 `Vz` 决定**：`Vx` 的正负只表示前进/倒车，不允许改变前轮的 + 物理偏转方向。因此从上位机到 MCU 的任何一层都不应执行 + `if (Vx < 0) Vz = -Vz` 一类处理。 ## 2. 标定方法（手推法） @@ -67,7 +70,7 @@ x 向半弓高的符号用于判定左右：PWM > ~1670 为右转（κ 取正） ## 3. 单位换算（关键，曾导致 100 倍错误） 标定表 `1/R` 列以 **1/cm** 为单位（R 用厘米）。而代码里的曲率来自 -`kappa = wz / Vx`（均为 SI），单位是 **1/m**。两者相差 100 倍。 +`kappa = wz / |Vx|`（均为 SI），单位是 **1/m**。两者相差 100 倍。 拟合前必须把 R 从 cm 换算为 m，再取 `kappa = 1/R_m`（并带符号）： @@ -135,12 +138,12 @@ servo = 1656.373 + 140.548 * kappa - 7.654 * kappa^2 后轮差速只用轮距 track 与 κ，本就不含 L。阿克曼几何没有丢，而是以更贴合 实车的实测形式嵌入拟合。 -## 5. 控制律（Mode 0：标定运动学，默认） +## 5. 控制律（Mode 0：标定运动学） -`AKM_DIRECT_MAP = 0` 时： +`AKM_DIRECT_MAP = 0` 且 `AKM_YAW_ASSIST = 0` 时： ``` -kappa_geom = Vz / Vx (Vx≈0 时置 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) # 后轮差速 @@ -149,7 +152,8 @@ MOTOR_B(右) = Vx * (1 - 0.5*track*kappa_fit) 物理特性：舵机角只决定转弯半径 R = 1/κ。 - 固定舵机角、改 Vx → wz 随之变（走同一圆，快慢不同）。 -- 固定 wz、改 Vx → κ = wz/Vx 变 → 舵机角变（正确行为）。 +- 固定 wz、改变 `|Vx|` → `|κ| = |wz|/|Vx|` 变 → 舵机角大小改变。 +- 只把 `Vx` 从正值改成同幅值负值 → 舵机方向和大小均保持不变。 ### Vx–wz 可行域（κ_max ≈ 3.33 /m） @@ -200,7 +204,7 @@ MOTOR_B(右) = Vx `AKM_DIRECT_MAP = 0` 且 `AKM_YAW_ASSIST = 1` 时启用。 -**动机（为什么要有 Mode 2）**：Mode 0 里 `κ = Vz/Vx`，同一个转向指令 `Vz` 在 +**动机（为什么要有 Mode 2）**：Mode 0 里 `κ = Vz/|Vx|`，同一个转向指令 `Vz` 在 高速时曲率被 `Vx` 除小，舵机自动回正 —— 这就是"高速转弯打不动"的根源。Mode 2 **主动解耦 w 与 v**：舵机由 `Vz` 直接决定（与 Mode 1 一样，不再除以 Vx）， 所以大 `Vz` 在任何速度都给出大前轮角；阿克曼只作为后轮差速的**前馈参考**， @@ -234,7 +238,7 @@ MOTOR_B(右) = Vx + dv ``` 要点： -- **解耦是核心**：`vz_norm` 只由 `Vz` 决定，不再 `Vz/Vx`，所以高速大转向不再被 +- **解耦是核心**：`vz_norm` 只由 `Vz` 决定，不再 `Vz/|Vx|`，所以高速大转向不再被 几何"稀释"。`Vx` 独立设定驱动基速。 - **舵机走直接满行程映射，不走拟合**：Mode 2 的舵机与 Mode 1 / CH1 调试一致， 用 `Akm_Norm_To_Servo(vz_norm)` 把转向指令线性铺满行程（以 `SERVO_INIT` 为 @@ -309,10 +313,10 @@ R = 1 / |1/R(Servo)| # 1/R 由 §2 标定表(1/cm) | 宏 | 默认 | 含义 | |----|-----:|------| -| `AKM_SERVO_DEBUG_REMOTE_CH1` | 1 | 1 = 舵机直接由遥控 CH1 驱动，覆盖所有控制律（板级调试） | -| `AKM_DIRECT_MAP` | 1 | 1 = 直接映射调试模式；0 = 交给 Mode 0 / Mode 2 | +| `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` | 0 | 1 = 横摆闭环（Mode 2）；仅在 `AKM_DIRECT_MAP=0` 时生效 | +| `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） | @@ -338,9 +342,12 @@ R = 1 / |1/R(Servo)| # 1/R 由 §2 标定表(1/cm) | 发送（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 中断里由 +- 遥测发送只保留 **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 非阻塞发送 @@ -358,14 +365,104 @@ R = 1 / |1/R(Servo)| # 1/R 由 §2 标定表(1/cm) 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，两档都绰绰有余；提速主要是留余量。 +- 一帧 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. 复现拟合 +## 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 半弓高定，+ 为右）。