关节电机底层控制与应用

01问题引入：关节是怎么动起来的

上一节提到了机械臂的 joint（关节）：它是连杆的连接处，是一个可以旋转的关节；而这个关节实际上是一台可以旋转的电机（也就是我们实验里真正用的舵机），带着连杆一起转动。关节怎么动的、为什么能动、怎么控制它按照要求运动，正是本章要解释的原理和应用的基础。为了解释这一系列问题，我们先从下面的流程图里查看关节被控制运动的完整流程，然后打开关节的黑盒，看看它的内部世界是如何运转的。

仿真里模型会动，是因为渲染软件收到了角度。真机上没人"渲染"，每一微秒的转角都是电机真实出的力。下图把两条链画在一起：上排是你已经会的仿真链，中间泳道那个问号，就是本章要拆开的黑盒。

本实验要拿到的六样东西：

• 能默写帧结构 0xAA 0x55 + func + len + data + CRC8，并解释每一段的作用
• 完成串口权限授权，验证 /dev/stm32（小车 /dev/car1_stm32）可读写
• 运行 python3 test_servo_direct.py，用 interactive_mode 控制 5 关节 + 夹爪
• 用 s 1 1520 单点微调、work 一键回工作位 [1500,1100,2200,1850,1500]
• 录制 pre_grasp → gripper_open → grasp_down → gripper_close → lift → back_work 六步动作，导出 pick_and_place.yaml 并 colcon build 安装
• 用 cutecom 抓一条真实帧，逐字节对照协议结构

02舵机原理：一整套伺服系统

给舵机发一个数，过一会儿关节就停在对应角度；用手去掰，它还会顶回来（除非先释放力矩）。一个数就能换来"听话又倔强"的动作，它内部是怎么做到的？

拆开一台舵机，里面是四件套，各干一件事：

串起来就一个回路：目标 − 现状 = 误差 → 电机朝误差变小的方向转 → 误差归零就停。这叫闭环控制，"伺服（Servo）"这个词指的就是这种自动纠偏。拿空调打比方：你设 26℃ 是目标，室温计读数是现状，温差让压缩机启停。像的部分是"设定值对测量值、误差驱执行"；不像的部分是空调量温度、舵机量角度，而且舵机的"室温计"是芯片从磁场里实时解出的角度，0.1° 的偏差它都看得见。

四件套拼在一起，才是那颗"听话又倔强"的舵机。上面四个卡片是一块块拆开看，下面的 3D 实验室把它们装回同一台真舵机——先看它们怎么协同转起来，再学怎么指挥它。

那"目标"从哪来？舵机只认一种表达：脉宽。高电平持续 1500μs 表示"立正"（中位），500~2500μs 是安全窗，脉冲越宽转得越远。脉宽和角度的换算只有一条公式，记住结论即可，推导不必背：

观察任务（打开即有默认演示值 1500μs）：① 拖 PWM 滑杆，盯住右上读数区——蓝线反馈总是追着红线目标，两条线的差就是误差；② 点"越界 3000"试试安全窗外会发生什么，再点"复位"回到中位；③ 点四个部件卡，3D 里对应零件会亮，对照左侧一句话说明。

你刚才证明了：PWM 只是目标位置的"一句话表达"，真正的转角是编码器一直盯着、误差一点点磨出来的。

还有一个问题没解决：机械臂上 6 颗舵机，总不能给每颗单独拉一根信号线。我们的舵机是总线舵机：多颗共享一条串口线，脉宽这个数要装进"信封"、写上收件人编号才发得出去。这就是下一节的协议。

03舵机控制协议

先把上一节的四件套合起来看：电机 + 减速器 + 磁编码器 + 驱动板，四样拼齐，才是一颗能自己纠偏的"伺服舵机"。要是每做一个关节，都得自己绕电机绕组、切减速齿轮、焊驱动板、再写一套闭环固件，那机械臂 6 个关节就是 6 倍的重复工作——又难又不划算。

好在市面上有现成的舵机：它已经把四件套全部装进了同一个小盒子里，出厂就是一套完整的伺服系统。我们不用拆开它、也不必碰它内部，只要学会用它对外的接口说话就行。

我们这套设备给舵机留的接口是串口，说话的内容叫协议：不碰电机和电路，只发几条约定好的字节指令，就能让舵机转到指定位置、或回报它现在的状态。下面要学的，就是这门"语言的语法"。

一条线上挂 6 颗舵机，谁该动、动多少、怎么防止听错和收错？发帧用挂号信那一套：先验信封，再拆正文，章不对整封退回。跟挂号信不一样的地方在于，这封"信"比真信多一个字段——信的类别（功能码），下指令和查状态是两种信。

一帧的完整结构（本课程全系列通用）：

正文里的子命令（脚本 test_servo_direct.py 实际用到的全部六种）：

观察任务（互动 2-2）：① 表里点"写多舵机目标"，按"发送演示帧"，看右侧 3D 输出轴跟着转，同时左下字节日志亮出 AA 55 05 … 和随数据变化的 CRC；② 点"0x0B 释放力矩"，用出现的手掰滑块直接掰输出轴；③ 什么都不点，盯住 0x08 缓存表自己在跳。

你刚才证明了：去程帧里装的就是脉宽这个数加上收件人 ID；回程 0x08 是下位机白送的状态播报，save 不发问就有数据，靠的是它。

04实操安全须知与环境准备

接下来三节要动真家伙。机械臂关节是带力矩的舵机，指令错了它不会自己知道错，只会执行到位或顶到限位为止。三条铁律，动手前逐条过：

• 上电前：机械臂展开摆到安全姿态，Workspace 里没有障碍物和手指；先确认知道急停/断电开关在哪，再合电。
• 运行中：第一次发指令只发近中位值（如 s 1 1520）或直接 work 回工作位，禁止一上来就大角度横甩。
• 异常时：出现异响、抖动、顶死，立刻 Ctrl+C 退出脚本并断电；按"线与权限 → 波特率与帧 → 上层逻辑"的顺序查（排障路径见 09 节）。

能向同伴口头复述这三条，再往下走。环境侧三件事，缺一不可：

# ① 清理占用串口的应用（本实验全程不启动 bringup！）
bash cleanup_ros.sh

# ② 确认设备与权限
ls -l /dev/stm32        # 小车换成 /dev/car1_stm32
# 预期：软链存在且可读写；打不开 → sudo chmod 666 /dev/stm32
dmesg | grep tty     # 找不到设备时看内核插入日志

05实操全景：test_servo_direct.py

动手前先见森林。这个脚本干的事可以一句话说完：打开串口，拉起一个后台收线程，然后给你一个敲命令的菜单。下图四个泳道对应四件事，主路径从左到右就是程序的一生。

读图任务：① 沿主路径从"打开 /dev/stm32"走到"yaml 导出"，每个格子念一遍它干啥；② 重点看中间两格的反向关系：你敲命令是下行，0x08 状态是上行，两条路同时在一条串口线上跑；③ 右下三张卡片回答了本节最常被问的两个问题——save 的数据哪来的、动作怎么才能不丢。

接下来三节把主路径按由易到难拆成三步走：先打开（06），再聊起来（07），最后录成果（08）。

一句话复述：这个脚本 = 一个开串口的主线程 + 一个永远在收帧的后台线程 + 一个你敲命令的菜单。

06功能①：打开串口，发出第一条指令

先走通最小的一条往返：程序打开 /dev/stm32，发一帧，舵机动一下。

# 启动与判定
python3 test_servo_direct.py
# 成功标志：打印 打开串口 /dev/stm32 @ 1000000... 后出现 串口已打开
# 失败会提示三种可能：bringup 没停 / 设备不存在 / 权限不够（对照 04 节逐条排除）

# 可选：串口助手抓一条真帧逐字节看（观察任务）
cutecom --device /dev/stm32 --baud 1000000
# 看点：发 work 时应出现 AA 55 开头的一串字节；1Mbps 下字节会瞬间刷过，
# 抓完暂停再翻，对照 03 节数字段

判定成功：日志出现 /dev/stm32 @ 1000000 且无 Permission denied；work 后 5 关节与夹爪同步摆到工作位姿态。你刚才证明了：不启动任何 ROS2 节点，一段 Python 也能直接跟舵机对话。

07功能②：interactive_mode 让关节听指挥

进到交互菜单后，常用命令就下面这些。规律很明显：每条命令最后都变成同一类帧，只有 data 段的 ID 和脉宽在换（对照 03 节子命令表）。

# 安全流程：先回家，再微调
work            # 第一条永远是它：回到已知安全位
s 1 1520        # 1 号舵机中位+20μs，微调神器（换算 ≈ +0.0471 rad）
g 800           # 张开夹爪；g 1500 全闭（与 EXP-05 的 servo 口径一致）
free            # 手掰摆位；load 锁回去

判定成功：s 1 1520 只动 1 号舵机；free 后能推动、load 后推不动。你刚才证明了：ID + 脉宽两个数，足够在一条线上精确指挥任何一颗舵机。

08功能③：录制动作，交棒 EXP-05

最后一步把刚才摆过的姿势存成序列，导出给下一章回放。先弄清 save 的数据从哪来：下位机每隔几十毫秒广播一帧 0x08 状态，后台线程收到就覆盖缓存；你按 save 那一刻，脚本只是给缓存拍了张快照，从头到尾没现问过舵机（03 节互动面板的 0x08 表自己在跳，就是这条通道）。

# 导出与安装（YAML 默认落在包的 config/actions/ 下）
yaml             # 按提示输入动作组名，如 pick_front
colcon build --packages-select car1_arm_controller
# 不 build 的后果：EXP-05 的 action_player 找不到文件

判定成功：六条 save 全部命名成功，yaml 导出后 install/share 下存在新文件。你刚才证明了：裸机录下的不过是一行行"ID: 脉宽"清单，EXP-05 的 action_player 读的就是这份清单。

09重点与易错点

出问题时按自底向上的三层查，别一上来就怀疑上层逻辑。有分支的路径画出来是这样：

三层排障观就是本章的方法论：线与权限（04）→ 波特率与帧（06 的 cutecom）→ 上层逻辑，从便宜的查起，从贵的停手。这条思路 EXP-05 接 ROS2 之后照样用。

10验收与总结

• 能向同伴复述 04 节三条安全铁律（上电前 / 首指令 / 异常时）
• 串口打开日志显示 /dev/stm32 @ 1000000，无 Permission denied
• work 后 5 关节与夹爪同步到达工作位姿态
• s 1 1520 只动 1 号舵机，说明 ID 寻址正确
• 六条 save 全部命名成功，yaml 导出后 install/share 下存在新 YAML
• 能口头解释：为什么这条路线"不依赖 ROS2 启动任何节点"，以及它与 EXP-05 ROS2 路线的分工