补充阿克曼转向符号排查说明
记录倒车舵机反向的 ROS 到 USART3 控制链定位过程，并同步校正当前控制模式、曲率符号及 44 字节遥测帧说明。
This commit is contained in:
cyy_mac
@@ -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 半弓高定，+ 为右）。
|
||||
|
||||
Reference in New Issue
Block a user