机械臂控制系统全链路：从轨迹规划到FOC精准控制
导读：机械臂控制系统三条主线——主控规划轨迹发给关节，关节FOC驱动电机，开机标定确保精度。本文用流程图拆解每条链路，并附开源库选型指南，全部基于 GitHub 高星项目。
一、系统架构全景
机械臂控制链路分为三层：主控层（轨迹规划）、通信层（CAN总线）、关节层（FOC驱动）。每层各司其职，通过CAN总线串联成完整闭环。
flowchart TB
subgraph 主控层["🔵 主控层"]
direction TB
HOST["上位机 / 工业PC<br/>轨迹规划 + 逆运动学"]
MCU["主控制器 MCU<br/>实时轨迹插补"]
end
subgraph 通信层["🟡 通信层 CAN Bus"]
direction TB
CAN["CAN Bus 总线<br/>100K~1Mbps<br/>差分双绞线"]
PROTOCOL["应用层协议<br/>位置+速度+力矩"]
end
subgraph 关节层["🟢 关节层 × N"]
direction TB
JMCU["关节MCU<br/>接收指令 + 本地PID"]
FOC["FOC驱动器<br/>SVPWM输出"]
MOTOR["无刷电机<br/>BLDC / PMSM"]
ENC["磁编码器<br/>反馈位置"]
end
HOST -->|"轨迹点|逆解"| MCU
MCU -->|"CAN报文<br/>目标位置+速度"| CAN
CAN -->|"CAN报文"| JMCU
JMCU -->|"电流环|PID"| FOC
FOC -->|"U/V/W 三相"| MOTOR
ENC -->|"位置反馈"| JMCU
style HOST fill:#E3F2FD,stroke:#1976D2
style MCU fill:#E3F2FD,stroke:#1976D2
style CAN fill:#FFF8E1,stroke:#F57C00
style JMCU fill:#E8F5E9,stroke:#388E3C
style FOC fill:#E8F5E9,stroke:#388E3C
style MOTOR fill:#E8F5E9,stroke:#388E3C
style ENC fill:#FFF8E1,stroke:#F57C00
二、主控到关节：CAN总线通信时序
主控制器不是实时控制每个关节，而是分两阶段：先预发运动参数（位置/速度/加速度），最后发一条"同步开始"命令，各关节在同一时刻启动——这就是分布式轨迹控制的核心。
flowchart TB
subgraph 主控["🔵 主控制器"]
TP["轨迹规划<br/>N个插补点"]
PRE["预发参数<br/>逐关节发送<br/>位置+速度+力矩"]
SYNC["发送同步命令<br/>START frame"]
end
subgraph CAN总线["🟡 CAN Bus"]
B1["帧1: J1位置+速度"]
B2["帧2: J2位置+速度"]
B3["帧3: J3位置+速度"]
B4["帧4: J4位置+速度"]
B5["帧5: SYNC_START"]
end
subgraph 关节1["🟢 关节1 MCU"]
J1["存储目标值<br/>等待SYNC"]
J1B["SYNC到达<br/>同时启动"]
end
subgraph 关节2["🟢 关节2 MCU"]
J2["存储目标值<br/>等待SYNC"]
J2B["SYNC到达<br/>同时启动"]
end
TP --> PRE
PRE --> B1
B1 --> J1
B2 --> J2
PRE --> B3
PRE --> B4
PRE --> SYNC
SYNC --> B5
B5 --> J1B
B5 --> J2B
style PRE fill:#FFF8E1,stroke:#F57C00
style SYNC fill:#FFEBEE,stroke:#D32F2F
style J1 fill:#E8F5E9,stroke:#388E3C
style J1B fill:#F3E5F5,stroke:#7B1FA2
style J2 fill:#E8F5E9,stroke:#388E3C
style J2B fill:#F3E5F5,stroke:#7B1FA2
关键时序逻辑：
1. 主控按关节ID逐一发送 SetPosition、SetVelocity、SetTorque 帧
2. 各关节MCU只存储，不立即执行
3. 主控发 SYNC_START 帧（全局广播，无目标ID）
4. 各关节同时将存储值写入FOC驱动，开启运动
这种设计避免了各关节因通信延迟不同而产生的"起步不齐"问题，是高精度协作的基础。
三、关节FOC驱动：多层PID嵌套控制链
FOC（磁场定向控制）将三相交流电转换为旋转磁场正交分量（d/q轴），实现对电机转矩和磁场的独立精准控制。内环是电流环，外环套速度环，最外环套位置环，三环级联 PID。
flowchart TB
START(["关节控制器启动"]) --> REF["外环：位置参考<br/>目标角度 θ_target"]
REF --> P_PID["位置 PID<br/>输出 = 速度参考 ω_target"]
P_PID --> S_PID["中环：速度 PID<br/>输出 = 力矩参考 τ_target"]
S_PID --> C_PID["内环：电流 PID<br/>输出 = d/q轴电压"]
C_PID --> CLARK["Clark变换<br/>Iα / Iβ"]
CLARK --> PARK["Park变换<br/>Id / Iq"]
PARK --> SV["SVPWM<br/>空间矢量调制"]
SV --> INV["三相逆变器<br/>U / V / W"]
INV --> MOTOR["BLDC电机<br/>产生旋转磁场"]
MOTOR --> ENC["磁编码器<br/>反馈机械角度"]
ENC -->|"角度反馈"| REF
style REF fill:#FFF8E1,stroke:#F57C00
style P_PID fill:#FFF8E1,stroke:#F57C00
style S_PID fill:#FFF8E1,stroke:#F57C00
style C_PID fill:#FFF8E1,stroke:#F57C00
style CLARK fill:#E3F2FD,stroke:#1976D2
style PARK fill:#E3F2FD,stroke:#1976D2
style SV fill:#E3F2FD,stroke:#1976D2
style INV fill:#E3F2FD,stroke:#1976D2
style MOTOR fill:#E8F5E9,stroke:#388E3C
style ENC fill:#FFF8E1,stroke:#F57C00
三层 PID 各司其职：
| 环路 | 输入 | 输出 | 带宽 | 典型频率 |
|---|---|---|---|---|
| 位置环 | 目标角度 | 速度参考 | 最低 ~50Hz | 运动规划层 |
| 速度环 | 速度参考 | 力矩参考 | 中等 ~200Hz | 轨迹跟踪 |
| 电流环 | 力矩参考 | d/q电压 | 最高 ~10kHz | FOC底层 |
四、三步标定：从零位到精准定位
4.1 标定总览
机械臂从零件到能精准抓取，必须过三道标定关：
flowchart TB
A(["上电"]) --> B["第一步：零点标定<br/>电动找零位<br/>记录各关节机械零点"]
B --> C["第二步：运动学标定<br/>建误差模型<br/>补偿装配误差"]
C --> D["第三步：手眼标定<br/>眼在手上 / 眼在手外<br/>相机与末端执行器对齐"]
D --> E(["标定完成<br/>精度 < 0.5mm"])
style A fill:#E3F2FD,stroke:#1976D2
style B fill:#FFF8E1,stroke:#F57C00
style C fill:#FFF8E1,stroke:#F57C00
style D fill:#FFF8E1,stroke:#F57C00
style E fill:#E8F5E9,stroke:#388E3C
4.2 第一步：零点标定
机械臂上电后不知道"零位在哪"。零点标定的目标是让控制器知道每个关节的机械零角度对应编码器的哪个读数。
来自 linuxros.cn · linuxROS
flowchart TB
START(["上电"]) --> HOME["发出归零指令<br/>各关节以低速电动"]
HOME --> SWITCH["触动零位开关<br/>或检测编码器过零脉冲"]
SWITCH --> RECORD["记录此时编码器读数<br/>设为该关节零点偏置"]
RECORD --> ALL{"所有关节<br/>都找到零位?"}
ALL -->|"否"| HOME
ALL -->|"是"| DONE["零点标定完成<br/>写入控制器参数"]
style HOME fill:#FFF8E1,stroke:#F57C00
style SWITCH fill:#FFF8E1,stroke:#F57C00
style RECORD fill:#E3F2FD,stroke:#1976D2
style DONE fill:#E8F5E9,stroke:#388E3C
常用方法：往一个方向缓慢电动，触发限位开关或检测磁编码器的索引脉冲（Z信号），记录当前读数即为机械零。
4.3 第二步：运动学标定
零点标定后，各关节角度已知，但实际DH参数与标称值有偏差（装配误差、关节柔性变形）。运动学标定通过多次测量末端实际位置，反解出DH参数误差并补偿。
flowchart TB
START(["采集N个末端位置数据<br/>N≥17个标定点"]) --> MODEL["建立误差模型<br/>ΔDH参数为待求量"]
MODEL --> SOLVE["最小二乘法求解<br/>ΔDH参数最优解"]
SOLVE --> UPDATE["更新URDF / 控制器<br/>写入补偿后DH参数"]
UPDATE --> VERIFY["验证：末端精度 < 0.5mm?"]
VERIFY -->|"否"| START
VERIFY -->|"是"| DONE(["运动学标定完成"])
style START fill:#E3F2FD,stroke:#1976D2
style MODEL fill:#FFF8E1,stroke:#F57C00
style SOLVE fill:#FFF8E1,stroke:#F57C00
style UPDATE fill:#E3F2FD,stroke:#1976D2
style VERIFY fill:#FFF8E1,stroke:#F57C00
style DONE fill:#E8F5E9,stroke:#388E3C
推荐工具：robot_calibration（ROS2 Humble，v0.8.3）可标定关节角度偏移、相机内外参、末端执行器偏移，输出更新后的 URDF。
4.4 第三步：手眼标定
相机装上机械臂后，控制器不知道"相机看到的物体在机器人坐标系下是哪个位置"。手眼标定求解相机与末端执行器（或基座）之间的齐次变换矩阵。
flowchart TB
subgraph 场景["两种标定场景"]
EIH["眼在手上 Eye-in-Hand<br/>相机固定在末端执行器<br/>标定板固定在桌面"]
EOB["眼在手外 Eye-on-Base<br/>相机固定在三脚架<br/>标定板固定在末端执行器"]
end
subgraph 标定流程["统一标定流程"]
P1["移动机械臂到N个不同姿态<br/>N≥5，确保姿态多样性"]
P1 --> P2["相机拍摄标定板<br/>aruco_ros 检测角点"]
P2 --> P3["记录：末端执行器位姿<br/>+ 相机中标定板位姿"]
P3 --> P4["AX = XB 方程求解<br/>手眼变换矩阵 T_cam_ee"]
P4 --> P5["将T_cam_ee写入控制器<br/>相机视野映射到机器人坐标"]
end
EIH --> 标定流程
EOB --> 标定流程
style EIH fill:#FFF8E1,stroke:#F57C00
style EOB fill:#FFF8E1,stroke:#F57C00
style P1 fill:#E3F2FD,stroke:#1976D2
style P2 fill:#E3F2FD,stroke:#1976D2
style P3 fill:#E3F2FD,stroke:#1976D2
style P4 fill:#FFF8E1,stroke:#F57C00
style P5 fill:#E8F5E9,stroke:#388E3C
推荐工具链：
| 工具 | 用途 | GitHub |
|---|---|---|
| MoveIt Calibration | RViz图形化手眼标定插件 | ⭐105+ forks |
| robot_calibration | 运动学标定+相机标定 | ROS2官方 |
| easy_handeye | ROS1手眼标定 | ⭐200+ stars |
| aruco_ros | ArUco标定板检测 | 广泛使用 |
五、全链路数据流总结
从上位机到机械臂末端，一条轨迹的完整数据流：
flowchart TB
subgraph 上位机["🔵 上位机"]
TRAJ["生成轨迹<br/>N个笛卡尔坐标点"]
IK["逆运动学求解<br/>N组关节角度"]
SPLINE["轨迹插补<br/>生成关节空间样条"]
end
subgraph 主控["🔵 主控制器"]
CAN_PRE["CAN预发参数<br/>逐关节发送位置+速度"]
CAN_SYNC["CAN同步命令<br/>SYNC_START"]
end
subgraph 关节MCU["🟢 关节 × 6"]
STORAGE["存储目标值"]
PID_POS["位置PID"]
PID_VEL["速度PID"]
PID_CUR["电流PID"]
end
subgraph FOC["🟢 FOC驱动"]
CLARK["Clark"]
PARK["Park"]
SV["SVPWM"]
end
subgraph 电机["🟡 电机"]
MOTOR["BLDC输出<br/>转矩 + 转速"]
ENC["磁编码器<br/>角度反馈"]
end
TRAJ --> IK --> SPLINE
SPLINE --> CAN_PRE
CAN_PRE --> CAN_SYNC
CAN_SYNC --> STORAGE
STORAGE --> PID_POS --> PID_VEL --> PID_CUR
PID_CUR --> CLARK --> PARK --> SV
SV --> MOTOR
MOTOR --> ENC
ENC -.->|"闭环反馈"| PID_POS
style 上位机 fill:#E3F2FD,stroke:#1976D2
style 主控 fill:#E3F2FD,stroke:#1976D2
style 关节MCU fill:#E8F5E9,stroke:#388E3C
style FOC fill:#E8F5E9,stroke:#388E3C
style MOTOR fill:#FFF8E1,stroke:#F57C00
口诀：主控规划发命令，关节FOC驱动，标定确保精度，开源库搞定一切。
六、开源库选型指南
| 层级 | 推荐库 | 语言 | GitHub | 核心功能 |
|---|---|---|---|---|
| FOC驱动 | SimpleFOC | C++/Arduino | ⭐4.9k | BLDC/步进FOC、7种控制模式、6PWM |
| FOC驱动 | Danyachello/foc-based-control | C++/ESP32 | — | 梯形轨迹规划+CAND bus集成 |
| 轨迹规划 | ROS MoveIt | C++/Python | ⭐2.8k | 逆运动学、碰撞检测、轨迹优化 |
| 运动学标定 | robot_calibration | C++/ROS2 | ROS2官方 | DH参数/关节偏移/相机标定 |
| 手眼标定 | MoveIt Calibration | C++/MoveIt | ⭐105 forks | RViz图形化手眼标定 |
| 手眼标定 | easy_handeye | Python/ROS1 | ⭐200+ | 眼在手上/眼在手外 |
| 标定板检测 | aruco_ros | C++/ROS | 广泛使用 | ArUco标定板位姿检测 |
| CAN通信 | SocketCAN | C | Linux内核 | CAN总线工具集 |
| 视觉抓取仿真 | robot_sim_ws | Python/ROS | — | 仿真环境标定+几何抓取+GPD |
七、总结
机械臂控制三大核心链路：
- 架构链路：上位机轨迹规划 → 主控CAN广播参数 → 关节MCU同步启动 → FOC驱动电机
- 控制链路：三环PID（位置→速度→电流）+ Clark/Park变换 + SVPWM = 精准FOC
- 标定链路：电动找零位 → DH误差建模补偿 → 手眼标定求变换矩阵
一条口诀记全链路：主控规划发命令，关节FOC驱动，标定确保精度，开源库搞定一切。
参考来源
- SimpleFOC v2.4.0 源码架构 — simplefoc.com
- CAN Bus + SimpleFOC 机械臂控制 — dokkev/MotorControl-CAN
- FOC + 轨迹规划 ESP32 控制 — Danyachello/foc-based-control
- 同步多轴 FOC 关节控制 — SimpleFOC 社区讨论
- robot_calibration ROS2 — mikeferguson/robot_calibration
- MoveIt 手眼标定教程 — moveit2_tutorials/hand_eye_calibration
- MoveIt Calibration Fork — WILDER-SYSTEMS-LLC/moveit_calibration
- ROS 机械臂仿真与标定 — lijunyang22/robot_sim_ws
- 机械臂手眼标定详解 — 阿里云开发者社区, HermanYe996