模块名称： stm32_keyboard_control
核心节点： keyboard_control_node
固件对应： USER/main.c (STM32 Firmware)
适用阶段： 硬件调试 / SLAM 建图预备
1. 设计概述 (Design Overview)
在早期架构中，控制逻辑（Controller）与硬件接口（Driver）被物理分离。考虑到 STM32 固件采用了高效的字符流控制协议 ，我们将架构精简为集成式驱动模式 。本模块 keyboard_control_node 充当 ROS 2 上位机与底层硬件的唯一交互网关，通过多线程并发机制，同时实现毫秒级的人机交互控制与高频传感器数据采集。
1.1 系统上下文架构 (System Context Architecture)
2. 软件详细设计 (Software Detailed Design)
2.1 并发模型 (Concurrency Model)
为了保证控制的实时性与数据的完整性，节点内部采用异步多线程模型：
-
输入监听线程 (Daemon Thread)：
- 库 ：
pynput.keyboard - 职责：以非阻塞方式监听键盘中断事件（Press/Release）。
- 同步机制 ：通过原子变量或线程锁（Lock）更新全局控制状态
self.target_cmd。 - 逻辑 ：
on_press: 映射WASD为对应字符，置位active_flag。on_release: 触发死人开关 (Deadman Switch) 逻辑，立即发送停止指令或置位停止标志。
- 库 ：
-
IO 主线程 (Main Loop)：
- 库 ：
pyserial,rclpy.timer - 周期：20Hz (50ms)
- 职责 (下行) ：检查
self.target_cmd，向串口写入对应的 ASCII 字符（如b'W'）。 - 职责 (上行) ：读取串口缓冲区，以
\n为定界符提取传感器数据帧。
- 库 ：
2.2 通信协议规范 (Protocol Specification)
本系统采用非对称混合协议：下行控制追求极简低延迟，上行数据追求可读性与解析便利。
2.2.1 下行控制协议 (Host -> MCU)
类型： 单字节字符指令 (Single-Byte Character Command)
波特率： 115200 bps
| 动作 | 键位 | 发送载荷 (Hex) | 固件响应函数 (main.c) |
运动学描述 |
|---|---|---|---|---|
| 前进 | W |
0x57 |
SmoothControlSpeed(v, v, v, v) |
vx>0,ωz=0v_x > 0, \omega_z = 0vx>0,ωz=0 |
| 后退 | S |
0x53 |
SmoothControlSpeed(-v, -v, -v, -v) |
vx<0,ωz=0v_x < 0, \omega_z = 0vx<0,ωz=0 |
| 左转 | A |
0x41 |
SmoothControlSpeed(-v, -v, v, v) |
vx=0,ωz>0v_x = 0, \omega_z > 0vx=0,ωz>0 (原地左旋) |
| 右转 | D |
0x44 |
SmoothControlSpeed(v, v, -v, -v) |
vx=0,ωz<0v_x = 0, \omega_z < 0vx=0,ωz<0 (原地右旋) |
| 急停 | Space |
0x20 |
SmoothControlSpeed(0, 0, 0, 0) |
强制阻尼制动 |
- 心跳保活 ：STM32 端的
CheckCommandTimeout设定了 500ms 超时。上位机必须以至少 2Hz 的频率发送指令，否则机器人将自动停车。
2.2.2 上行反馈协议 (MCU -> Host)
类型： ASCII CSV 文本流
帧结束符： \n (0x0A)
-
里程计数据帧
-
格式 ：
/four_wheel_encoder,<enc1>,<enc2>,<enc3>,<enc4>,<timestamp> -
解析示例 ：
python# 原始数据: b'/four_wheel_encoder,102,-98,105,-100,12054\n' parts = line.decode().strip().split(',') if parts[0] == '/four_wheel_encoder': fl, fr, rl, rr = map(int, parts[1:5]) ts = int(parts[5]) -
物理意义 ：
encX代表自上一次采样以来（约 20ms）各个轮子的脉冲增量 (Delta Pulse)。
-
-
IMU 数据帧
- 格式 ：
/imu_data,<ax>,<ay>,<az>,<gx>,<gy>,<gz>,<temp>,<timestamp> - 物理意义 ：MPU6050 的原始 16 位寄存器值 (LSB)。需在上位机根据量程（如 ±2g, ±2000°/s）转换为标准单位 (m/s2m/s^2m/s2, rad/srad/srad/s)。
- 格式 ：
3. 关键算法实现 (Critical Implementation)
3.1 里程计解算 (Odometry Calculation)
为了支持 SLAM，keyboard_control_node 必须实现运动学正解 ，将编码器脉冲转换为机器人的位姿 (x,y,θ)(x, y, \theta)(x,y,θ)。
- 参数定义 ：
PULSE_PER_METER: ≈1571.0\approx 1571.0≈1571.0 (基于轮径 0.067m, 11线霍尔, 30:1 减速比)WHEEL_TRACK: 0.17m0.17m0.17m (轮距)
- 计算逻辑 (差速模型)：
- 计算左右轮平均位移：
ΔdL=enc1+enc32⋅Kscale,ΔdR=enc2+enc42⋅Kscale\Delta d_L = \frac{enc_1 + enc_3}{2} \cdot K_{scale}, \quad \Delta d_R = \frac{enc_2 + enc_4}{2} \cdot K_{scale}ΔdL=2enc1+enc3⋅Kscale,ΔdR=2enc2+enc4⋅Kscale - 计算中心位移与旋转：
Δs=ΔdR+ΔdL2,Δθ=ΔdR−ΔdLWtrack\Delta s = \frac{\Delta d_R + \Delta d_L}{2}, \quad \Delta \theta = \frac{\Delta d_R - \Delta d_L}{W_{track}}Δs=2ΔdR+ΔdL,Δθ=WtrackΔdR−ΔdL - 更新全局位姿：
xt=xt−1+Δs⋅cos(θt−1+Δθ2)x_t = x_{t-1} + \Delta s \cdot \cos(\theta_{t-1} + \frac{\Delta \theta}{2})xt=xt−1+Δs⋅cos(θt−1+2Δθ)
yt=yt−1+Δs⋅sin(θt−1+Δθ2)y_t = y_{t-1} + \Delta s \cdot \sin(\theta_{t-1} + \frac{\Delta \theta}{2})yt=yt−1+Δs⋅sin(θt−1+2Δθ)
θt=θt−1+Δθ\theta_t = \theta_{t-1} + \Delta \thetaθt=θt−1+Δθ
- 计算左右轮平均位移：
3.2 数据发布规范 (ROS 2 Interface)
解析后的数据需封装为以下标准消息：
-
里程计 (
/odom)- 类型：
nav_msgs/msg/Odometry - 坐标系：
frame_id="odom",child_frame_id="base_link" - 内容：包含位姿
pose和 速度twist。
- 类型：
-
TF 变换 (
tf2_ros)- 必须 广播
odom -> base_link的动态变换。这是 SLAM 算法（如 Cartographer 或 Slam Toolbox）定位机器人的基础。 - 实现 ：使用
tf2_ros.TransformBroadcaster。
- 必须 广播
4. 调试与集成指南 (Integration Guide)
4.1 启动步骤
-
授予串口权限 ：
bashsudo chmod 777 /dev/ttyUSB0 -
运行节点 ：
bashros2 run stm32_keyboard_control keyboard_control_node -
验证数据 ：
-
新开终端，检查是否收到 TF 数据：
bashros2 run tf2_ros tf2_echo odom base_link -
如果只能控制但没有 TF 输出，说明上行数据解析逻辑缺失，需立即补全，否则 SLAM 无法运行。
-
4.2 常见问题排查
- 现象 ：机器人移动断断续续。
- 原因：心跳发送频率低于 2Hz，触发了 STM32 的超时保护。
- 对策 ：检查
timer回调频率，建议设为 10Hz 或 20Hz。
- 现象 ：TF 树断裂。
- 原因 ：未正确发布
/odom话题或TransformBroadcaster未实例化。 - 对策 ：确保
keyboard_control_node中包含里程计解算代码。
- 原因 ：未正确发布