前言

• 上篇从机械臂的结构、位姿表示、运动学、速度运动学到动力学，完成了机械臂的完整建模——怎么描述一个机械臂（第 1、2 章）、怎么算它的位置姿态（第 3 章）、怎么描述它的速度与力（第 4、5 章）
• 本文续接上篇，从运动规划开始，讲机械臂怎么"动起来"：
  • 第 6 章 运动规划：关节空间三次多项式/梯形/S 曲线、笛卡尔空间轨迹
  • 第 7 章 控制：串级 PID、计算力矩、阻抗/导纳
  • 第 8 章 全链路串联：把 1-8 章拼成一条完整的规划 → 执行 → 反馈闭环
  • 附录：全部可视化代码（FK/IK/DH/动力学对账），跑完会在当前目录生成 PNG
• 上篇建好了模型，下篇就看机械臂怎么真正动起来
  

上篇回顾

• 上篇覆盖第 1-5 章（机械臂结构、位姿表示、运动学、速度运动学、动力学），详见上篇：【玩转VLA具身智能机械臂】（一）：机械臂基础（上篇）——建模：结构、运动学与动力学

---

目录（接上篇，覆盖第 6-8 章 + 附录）

---

6 运动规划

• 运动规划回答：给定起点和终点，机械臂怎么"平滑"地过去
• 关键在"平滑"——不能瞬间到位（速度无穷），要满足速度/加速度/力矩限制，还要避开障碍物

6-1 关节空间与笛卡尔空间

• 规划发生在两个空间，各有取舍：

空间	规划变量	优点	缺点
关节空间	$q(t)$	计算简单、天然避开奇异、无路径曲率问题	末端路径不可控（可能画弧线）
笛卡尔空间	$x(t)$	末端路径可控（直线/圆弧）	计算量大、要反复做 IK、可能穿过奇异

• 关节空间规划：直接规划每个关节角随时间的变化 $q(t)$，最后用正运动学验证末端路径
• 笛卡尔空间规划：先规划末端位姿随时间的变化 $x(t)$，再通过 IK 转成关节角——注意 IK 可能多解、可能奇异

核心取舍：如果只关心"从 A 到 B 别撞上东西"，用关节空间；如果必须"末端走直线"（比如拧螺丝、沿直线涂胶），用笛卡尔空间

6-2 关节空间轨迹规划

• 关节空间规划的核心是：用插值函数让关节角 $q_i(t)$ 从起点 $q_i^s$ 平滑过渡到终点 $q_i^e$
• 插值函数有三种主流做法，正好覆盖"位置连续 → 速度连续 → 加速度连续"三个层次：
  • 6-2-1 三次多项式插值：位置连续，最简
  • 6-2-2 梯形速度规划：时间最短
  • 6-2-3 S 曲线：加速度连续，最稳
• 关节空间规划的好处：插值的是关节角，天然合法——每个关节角都在限位内，不经过奇异（除非路径本身穿过）

6-2-1 三次多项式插值（Cubic Polynomial）

• 最简单的三次多项式插值（满足位置 + 速度边界）：

$$q(t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3$$

• 代入边界条件 $q(0)=q^s,q(T)=q^e,\dot{q}(0)=0,\dot{q}(T)=0$，解得：

$$a_0=q^s,a_1=0,a_2=\frac{3(q^e-q^s)}{T^2},a_3=-\frac{2(q^e-q^s)}{T^3}$$

• 画出来就是下图——上图关节角平滑地从 $q^s$ 爬到 $q^e$，两端切线水平（速度为零）；下图为角速度，从 0 爬升到峰值再回落回 0，中间没有任何突变，这就是"平滑过渡"的全部含义：

6-2-2 梯形速度规划（Trapezoidal Velocity Profile）

• 三次多项式"从头软到尾"，运动时间拉满；梯形速度规划直接给速度画一个梯形：加速段 → 匀速段 → 减速段，每段都用满电机的最大加速度 $a_{max}$ 和最大速度 $v_{max}$，所以同样位移下总时间最短
• 设加速段时长 $t_a=v_{max}/a_{max}$，三段的速度、加速度、位置：

段	时间区间	加速度 $a$	速度 $v$	位置 $q$
加速段	$[0,t_a]$	$a_{max}$	$a_{max}t$	$q^s+\frac{1}{2}{a}_{max}{t}^2$
匀速段	$[t_a,T-t_a]$	$0$	$v_{max}$	$q^s+v_{max}t-\frac{v_{max}^2}{2a_{max}}$
减速段	$[T-t_a,T]$	$-a_{max}$	$v_{max}-a_{max}(t-T+t_a)$	$q^e-\frac{1}{2}{a}_{max}(T-t{)}^2$

• 总时间 = 加速 + 匀速 + 减速（匀速段位移 = 总位移减去两个加速段的位移），于是：

$$T=\frac{\Delta q}{v_{max}}+\frac{v_{max}}{a_{max}}$$

• 画出来长这样——速度是标准梯形，加速度是 $+a_{max}$ 和 $-a_{max}$ 两个方波脉冲，起停瞬间加速度阶跃，机械冲击就出在这里（这正是下一节 S 曲线要解决的）：

• 注意：如果位移太小、速度根本爬不到 $v_{max}$，梯形就退化成三角形（只剩加速 + 减速两段），此时峰值速度 $v_{peak}=\sqrt{a_{max}\Delta q}$

6-2-3 S 曲线（S-Curve）

• 梯形速度的加速度在起停瞬间阶跃，冲击大。S 曲线引入加加速度（jerk，即 $J=\frac{d\ddot{q}}{dt}$，加速度的变化率），让加速度连续渐变，机械冲击大幅减小
• 加加速段 $J$ 恒定，加速度从 0 线性爬到 $a_{max}$，位置按 $t^3$ 增长：

$$a(t)=Jt,v(t)=\frac{1}{2}J{t}^2,q(t)=\frac{1}{6}J{t}^3$$

• 对称地做减速、中间插一段匀速，就拼出标准的七段式（加加速 → 匀加速 → 减加速 → 匀速 → 加减速 → 匀减速 → 减减速）；去掉匀加速/匀减速段就是五段式。设 $T_a=a_{max}/J$ 为加加速段时长，各段的加加速度与加速度（减速段与加速段时间镜像对称）：

段	加加速度 $J$	加速度 $a(t)$
加加速	$+J$	$Jt$（$0\to a_{max}$）
匀加速	$0$	$a_{max}$
减加速	$-J$	$a_{max}-J(t-T_a-T_j)$（$\to 0$）
匀速	$0$	$0$
加减速	$-J$	$0\to -a_{max}$
匀减速	$0$	$-a_{max}$
减减速	$+J$	$-a_{max}\to 0$

• 画出来——加速度从梯形速度的"方波脉冲"变成连续渐变的山形（$J$ 限制了它的变化率），速度是平滑的 S 形：

说人话：关节空间规划就是"每个关节从 a 度平滑转到 b 度"。三次多项式管平滑，梯形速度管省时间，S 曲线管不抖——具体连续到哪一阶、哪里会突跳，见下面的对比表

• 三种插值方式一句话对比：

方法	保证连续到	会突跳的地方	代价 / 换来的
三次多项式	$\dot{q}$（速度）连续	$\ddot{q}$（加速度）起停瞬间阶跃	最省事，但全程最慢
梯形速度	只有 $q$ 连续，速度本身有折角	加速度是 $\pm a_{max}$ 方波	换来时间最短，冲击最大
S 曲线	$\ddot{q}$（加速度）连续	$\stackrel{...}{q}$（jerk）阶跃	最稳，但时间长、实现复杂

6-3 笛卡尔空间轨迹规划

• 笛卡尔空间规划的核心：让末端位姿 $x(t)$ 沿期望的路径（直线/圆弧）运动
• 末端位置线性插值（直线路径）：

$$p(t)=p^s+s(t)\cdot (p^e-p^s),s(t)\in [0,1]$$

• 其中 $s(t)$ 是标量路径参数，用梯形速度/S 曲线来安排它随时间的变化
• 姿态部分不能用欧拉角直接插值（会扭、会万向锁），要用四元数 Slerp：

$$q(t)=\text{Slerp}(q_s,q_e,s(t))$$

• 插值得到的 $x(t)$ 还要逐点转成关节角：对每个时刻做一次 IK
• 笛卡尔直线规划的典型问题：
  • 经过奇异位形：路径直线穿过奇异区域，IK 在该处发散——需要绕路或切换关节空间
  • 计算量大：每个插值点都要 IK，实时性要求高
  • 与关节限位冲突：直线路径上的点可能超出关节限位

说人话：笛卡尔规划是"让手沿直线走过去"。位置直线插值，姿态四元数插值，再逐点反解 IK。听着简单，但路径一旦穿过"手臂伸直"那种死锁姿势就翻车

---

7 控制

• 控制要解决的是：给定期望轨迹 $q_d(t)$，让机械臂实际按它运动
• 从最简单的位置伺服，到高阶的阻抗控制，我们按一整套"金字塔"来讲

7-1 从位置控制到力矩控制

• 机械臂控制的三个层次，从底层到顶层：

层次	控制量	本质
位置控制	期望位置/角度	让关节转到指定角度
速度控制	期望速度	让关节按指定速度转
力矩控制	期望力矩	让电机输出指定扭矩

• 现实中，底层电机驱动器内部就是电流（力矩）环 + 速度环 + 位置环的三环结构：
  • 最外层位置环输出速度指令
  • 中间速度环输出力矩指令
  • 最内层电流环直接驱动电机
• 我们常说的"位置控制模式"其实是外环位置 + 内环力矩的组合
• 力矩控制的本质：直接告诉电机"输出多大扭矩"——上层算法（阻抗、力控）才有发挥空间

值得一提的是：不是所有电机驱动器都支持力矩控制——力矩控制本质是电流闭环（电机扭矩 $\tau =K_{t}i$ 与电流成正比，$K_t$ 是力矩常数），要求驱动器有电流采样硬件、固件还开放电流环模式；而常见舵机、步进电机、简易直流驱动板要么没有电流采样、要么接口封闭，只能做位置/速度控制

• 所以如果你的目标是精细的控制效果——比如部署 VLA 具身智能——一般推荐选用带力矩控制的电机（伺服 + 电流环）；要做柔顺/力控，还需要关节力矩传感器

说人话：电机驱动器里三层套娃——位置环管"到哪"，速度环管"多快"，电流环管"多大力"。普通搬运用位置控制就够，但要做力控/柔顺，就必须下探到力矩控制

• 三环串级控制的结构——位置环给速度环发速度参考，速度环给电流环发力矩参考，电流环直接驱动电机，每一环都有自己的反馈闭环（各环具体用什么控制律，见 7-2 节）：

7-2 PID 控制

• 经典的单关节位置 PID：误差驱动的反馈控制

$$u(t)=K_{p}e(t)+K_i{\int }_0^{t}e(\tau )d\tau +K_d\dot{e}(t)$$

• 其中 $e(t)=q_d(t)-q(t)$ 是关节角误差，$K_p$、$K_i$、$K_d$ 分别是比例、积分、微分增益
• 每项的作用：
  • P（比例）：有偏差就出力，偏差越大越用力——“跟手”
  • I（积分）：消除稳态误差，把长期的小偏差"攒"起来补上——“较真”
  • D（微分）：抑制过冲，偏差变化太快就"踩刹车"——“稳重”
• 机械臂常用的是 PD 位置控制（一般不加积分，因为重力项通常单独补偿）：

$$\tau =K_p(q_d-q)+K_d({\dot{q}}_d-\dot{q})+g(q)$$

说人话：PID 就是"看偏差干活——偏差大用力推，偏差一直在就慢慢攒劲，偏差变太快就踩刹车"。三个旋钮 $K_p,K_i,K_d$ 调好了，机械臂又准又不抖

• 注意：PID 是把每个关节当独立系统来控制的，没有考虑关节间的耦合（科氏力、惯性耦合）。关节多、速度快、负载重时，纯 PID 效果会变差——这时候就需要模型化的控制方法

7-3 计算力矩控制

• 计算力矩控制（Computed Torque Control）的核心思想：用动力学模型把非线性系统"线性化"，再套 PD
• 如果动力学模型完全准确，我们令：

$$\tau =M(q)\cdot a+C(q,\dot{q})\dot{q}+g(q)$$

• 代入动力学方程 $M(q)\ddot{q}+C\dot{q}+g=\tau$，得到：

$$\ddot{q}=a$$

• 非线性系统变成了一个简单的双重积分器——$a$ 想让它怎么加速，它就怎么加速
• 再令 $a={\ddot{q}}_d+K_d\dot{e}+K_{p}e$，误差动力学：

$$\ddot{e}+K_d\dot{e}+K_{p}e=0$$

• 选择合适的 $K_p,K_d$，误差指数收敛到零——只要模型准，控制就是完美的

说人话：计算力矩控制 = “先用模型把重力、科氏力、惯量都算出来抵消掉，让机械臂变成一块简单的’假想质量块’，然后对这个’质量块’套 PD”。模型越准，效果越好——这就要靠 Pinocchio 这类动力学库实时算 $M,C,g$

• 一句话对比 PID 与计算力矩控制：

对比维度	PID 控制	计算力矩控制
看待机械臂	黑箱：只盯着误差 $e=q_d-q$ 调增益	懂行：提前知道 $M,C,g$，把耦合和重力都算出来补偿掉
对耦合的态度	无视（当扰动硬扛）	显式消除（前馈）
增益怎么调	逐关节试，调高了会震荡/失稳（因为忽略了耦合）	线性化后每关节是独立二阶系统，极点配置有理论保证
依赖	啥模型都不用，但精度上限低	要精确模型，模型错则性能崩（这正是第 5 章打地基的原因）

7-4 阻抗控制与导纳控制

• 前几类控制都在追位置，但在抓取、装配、人机协作场景，机械臂必须"柔软"地与环境交互——撞到东西不能硬顶
• 阻抗控制（Impedance Control）：调节机械臂的机械阻抗（惯量 + 阻尼 + 刚度），让末端像一个"弹簧 - 阻尼系统"
• 操作空间（笛卡尔空间）的阻抗控制方程：

$$M_d\ddot{\tilde{x}}+B_d\dot{\tilde{x}}+K_d\tilde{x}=F_{ext}$$

• 其中 $\tilde{x}=x_d-x$ 是末端位置误差，$M_d$、$B_d$、$K_d$ 是期望的惯量、阻尼、刚度，$F_{ext}$ 是外部接触力
• 物理含义：
  • $K_d$ 大 → 机械臂"硬"，位置精度高
  • $K_d$ 小 → 机械臂"软"，碰到东西会"退让"——人碰上去是安全的
  • $B_d$ 提供阻尼，防止震荡
• 实现上，把期望阻抗转换成关节力矩：

$$\tau =J^T\left(M_d\ddot{\tilde{x}}+B_d\dot{\tilde{x}}+K_d\tilde{x}\right)+C\dot{q}+g(q)$$

• 导纳控制（Admittance Control） 是阻抗控制的"对偶"——测力，改位置：
  • 阻抗控制：给定位置，算力（力的行为像弹簧）
  • 导纳控制：测得力，改期望位置（被推了就往后退）
• 选型的一般规律：
  • 有力传感器（测接触力）→ 导纳控制
  • 无力传感器、靠电机电流估计力矩 → 阻抗控制
• 拖动示教（Direct Teaching）就是典型的导纳/零重力场景：人推机械臂，导纳控制把推力变成位置修正，机械臂"跟着手走"

说人话：阻抗控制 = “给机械臂调硬度”——想要它硬就调大刚度，想要它软就调小刚度，碰墙不硬顶而是退让。导纳控制 = “你推它，它就让位”。一个是"力随位置变"，一个是"位置随力变"，本质是一对孪生兄弟

---

8 全链路串联

• 前七章是七块拼图：第 1 章机械臂结构、第 2 章位姿表示、第 3 章运动学、第 4 章速度运动学、第 5 章动力学、第 6 章运动规划、第 7 章控制
• 这一章把它们按机械臂"干一件活"的真实流程拼成一条流水线，看每个模块到底在哪一步起作用、怎么起作用
• 全局图——机械臂从接到任务到末端到位，是一个规划 → 执行 → 反馈的闭环：

• 每个环节对应哪一章、用什么工具、起什么作用，一张表看清：

环节	章节	关键内容	在这一步干什么
任务 → 期望位姿	第 2 章	位置 + 姿态、齐次变换 $T$	把"要干啥"翻译成数学目标 $x_d$
位姿 → 关节角	第 3 章	逆运动学 IK、DH 参数	末端位姿反解关节角（多解/无解/奇异三座山）
关节角 → 期望轨迹	第 6 章	三次多项式/梯形/S 曲线	让关节角平滑过渡，产出 $q_d(t),{\dot{q}}_d(t)$
期望轨迹 → 力矩	第 7 章	串级 PID、计算力矩	跟踪期望轨迹，算出电机该出多大力 $\tau$
力矩 → 实际运动	第 5 章	$M\ddot{q}+C\dot{q}+g=\tau$	真实物理：力矩驱动关节转起来
实际运动 → 反馈	第 3、4 章	FK、雅可比 $J$	末端走没走到；速度映射、奇异预警

8-1 规划层

• 这一层决定"机械臂要去哪、怎么走"，由第 2、3、6 章接力完成：
  • 第 2 章把任务抽象成数学目标：位置 + 姿态打包成齐次变换 $T$，这就是期望 $x_d$
  • 第 3 章 IK把末端位姿反解成关节角——多解（选哪组）、无解（目标太远）、奇异（反解发散）三座山都在这里
  • 第 6 章在关节空间把起点角到终点角用三次多项式/梯形/S 曲线平滑连起来，产出期望轨迹 $q_d(t)$
• 一句话：规划层负责"算账"——把抽象任务变成可执行的期望轨迹

8-2 执行层

• 这一层是实时闭环，由第 7、5 章配合：
  • 第 7 章控制器看着期望轨迹 $q_d(t)$ 和实测 $q(t)$ 的误差，算出该输出的关节力矩 $\tau$（串级 PID 或计算力矩）
  • 第 5 章动力学是"真实物理"：力矩 $\tau$ 加上惯性、科氏、重力，决定了关节到底怎么动——$M\ddot{q}+C\dot{q}+g=\tau$
• 这一层每秒循环几百到几千次，跑的就是 7-1 那张串级控制图

8-3 反馈层

• 控制要有反馈才叫闭环：
  • 编码器报告实际关节角 $q(t)$，回灌给控制器，形成位置/速度/电流三环
  • 第 3 章 FK把实际 $q(t)$ 正解成末端位姿 $x(t)$——任务层拿它判断"走没走到"
  • 第 4 章雅可比 $J$ 在速度层做两件事：把关节速度映射成末端速度；算 $\det J$ 检测是否接近奇异，提前预警
• 别忘了雅可比的另一面（7-4 阻抗控制在用）：$\tau =J^{T}F$——把末端想要的力映射成关节力矩，力控制靠的就是雅可比

8-4 仿真与真机

• 把上面链路里的"执行层"跑在电脑里，就是仿真：
  • 第 5 章正动力学：给定力矩 $\tau$，解出 $\ddot{q}$ 再积分成运动——仿真器就是循环做这件事（附录四验证的正是两套动力学算法一致）
  • 第 5 章逆动力学：给定期望运动，算出需要的力矩——用于控制（计算力矩、重力补偿）
• 所以同一套动力学方程，正着用是仿真，反着用是控制

8-5 VLA 接入点

• 回到本系列的主题。传统做法里，"规划层"是人工写的：给末端位姿、挑 IK 解、设计轨迹
• VLA 大模型（RT-2、OpenVLA、π0）做的事，本质是把"任务 → 期望轨迹"这一整层吞进去：输入视觉 + 语言，直接输出动作（关节角、末端位姿或轨迹片段）

• 注意：VLA 替换的是"决策"，替换不了"物理"——控制器、动力学、编码器反馈这一整条执行 + 反馈链路，无论谁来做决策都省不掉，这正是前七章的价值
• 下一期开始，我们把这些基础搬进 MoveIt2 + Gazebo，在仿真里把这条链路完整跑起来

说人话：机械臂干活 = 规划层"想清楚去哪" + 执行层"照着走" + 反馈层"走没走对"。VLA 只是把"想清楚去哪"换成了大模型，后面的执行和反馈，还是第 5、7 章那套物理和控制的活

---

附录 可视化代码

• 本节用于放置本期所有画图的代码实现，全部可独立运行
• 代码用纯 numpy + matplotlib 实现，跑完会在当前目录生成 PNG 图片

附录一 正运动学与工作空间

• 代码功能：
  • 画一个二连杆平面机械臂的某个位形（基座 - 连杆1 - 连杆2 - 末端）
  • 遍历关节角，画出末端可达的工作空间（外圆 $L_1+L_2$ 与内圆 $|L_1-L_2|$ 包围的环形区域）
• 对应正文 上篇 3-2 节 FK：

# -*- coding: utf-8 -*-
# 附录一: 二连杆平面机械臂 —— 正运动学 FK + 工作空间可视化
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import font_manager

# 配置中文字体（按自己系统路径调整）
font_manager.fontManager.addfont('/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc')
plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

L1, L2 = 1.0, 0.6  # 两连杆长度

def fk(q1, q2):
    """正运动学: 关节角 -> 末端位置 (二连杆平面臂), 对应 3-2 节公式"""
    x = L1 * np.cos(q1) + L2 * np.cos(q1 + q2)
    y = L1 * np.sin(q1) + L2 * np.sin(q1 + q2)
    return x, y

fig, axes = plt.subplots(1, 2, figsize=(12, 5))

# (a) 画某个位形
ax = axes[0]
q1, q2 = np.deg2rad(40), np.deg2rad(-30)
x0, y0 = 0.0, 0.0
x1, y1 = L1 * np.cos(q1), L1 * np.sin(q1)
x2, y2 = fk(q1, q2)
ax.plot([x0, x1, x2], [y0, y1, y2], 'o-', color='tab:blue', lw=3, ms=9, label='机械臂')
ax.plot(x2, y2, '*', color='tab:red', ms=22, label='末端')
ax.annotate('基座', (0, 0), textcoords='offset points', xytext=(-16, 8))
ax.annotate('关节 1', (x1, y1), textcoords='offset points', xytext=(8, 6))
ax.annotate('末端 $(x,y)$', (x2, y2), textcoords='offset points', xytext=(10, 8))
ax.set_title(r'(a) 二连杆机械臂位形 (FK: $q_1=40^\circ, q_2=-30^\circ$)')
ax.set_xlabel('x'); ax.set_ylabel('y')
ax.set_xlim(-1.8, 1.8); ax.set_ylim(-1.8, 1.8)
ax.set_aspect('equal'); ax.grid(alpha=0.3); ax.legend(loc='lower left')

# (b) 工作空间: 外圆半径 L1+L2, 内圆半径 |L1-L2|
ax = axes[1]
th = np.linspace(0, 2 * np.pi, 400)
ax.plot((L1 + L2) * np.cos(th), (L1 + L2) * np.sin(th), 'k--', lw=1.2,
        label=r'外圆 $L_1+L_2$')
ax.plot(np.abs(L1 - L2) * np.cos(th), np.abs(L1 - L2) * np.sin(th), 'k:', lw=1.2,
        label=r'内圆 $|L_1-L_2|$')
rng = np.random.default_rng(42)
for _ in range(3000):
    q1_ = rng.uniform(0, 2 * np.pi)
    q2_ = rng.uniform(0, 2 * np.pi)
    x, y = fk(q1_, q2_)
    ax.plot(x, y, '.', color='tab:blue', ms=1, alpha=0.4)
ax.set_title('(b) 二连杆可达工作空间')
ax.set_xlabel('x'); ax.set_ylabel('y')
ax.set_xlim(-1.8, 1.8); ax.set_ylim(-1.8, 1.8)
ax.set_aspect('equal'); ax.grid(alpha=0.3)
ax.legend(loc='upper left')

plt.tight_layout()
plt.savefig('附录一_fk_workspace.png', dpi=120)
print('已保存: 附录一_fk_workspace.png')

• 运行上面的代码会生成 附录一_fk_workspace.png，该图已嵌入正文 上篇 3-2 节：左图是某一具体位形，右图是随机采样关节角得到的可达工作空间——一个环形区域，最远半径是 $L_1+L_2$（两臂完全伸直），最近半径是 $|L_1-L_2|$（两臂完全折叠）

附录二 逆运动学与奇异位形

• 代码功能：
  • 用雅可比伪逆迭代求解二连杆 IK，画出迭代收敛过程
  • 画出雅可比行列式 $\det (J)=L_1{L}_2\sin q_2$ 随 $q_2$ 的变化——在 $q_2=0,\pm \pi$ 处为零，即奇异位形
  • 对比奇异位形下末端"某方向动不了"
• 本脚本会生成两张图：
  • 附录二_ik_iteration.png：IK 迭代收敛过程，已嵌入正文 上篇 3-3 节
  • 附录二_singularity.png：奇异位形（$\det J$ 曲线 + 方向锁死），已嵌入正文 上篇 4-3 节

# -*- coding: utf-8 -*-
# 附录二: 二连杆平面机械臂 —— 逆运动学 IK(雅可比伪逆) + 奇异位形可视化
# 输出两张图:
#   附录二_ik_iteration.png    IK 伪逆迭代收敛过程 (嵌入正文 3-3 节)
#   附录二_singularity.png     奇异位形: det(J) 曲线 + 方向锁死 (嵌入正文 4-3 节)
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import font_manager

# 配置中文字体（按自己系统路径调整）
font_manager.fontManager.addfont('/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc')
plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

L1, L2 = 1.0, 0.6

def fk(q1, q2):
    x = L1 * np.cos(q1) + L2 * np.cos(q1 + q2)
    y = L1 * np.sin(q1) + L2 * np.sin(q1 + q2)
    return np.array([x, y])

def jac(q1, q2):
    """2x2 雅可比矩阵, 对应 4-2 节"""
    return np.array([
        [-L1 * np.sin(q1) - L2 * np.sin(q1 + q2), -L2 * np.sin(q1 + q2)],
        [L1 * np.cos(q1) + L2 * np.cos(q1 + q2),  L2 * np.cos(q1 + q2)],
    ])

def ik(target, q_init, iters=30, lr=0.5):
    """雅可比伪逆迭代 IK, 对应 3-3 节公式 q += J^+ @ err"""
    q = np.array(q_init, dtype=float)
    traj = [q.copy()]
    for _ in range(iters):
        err = target - fk(*q)
        if np.linalg.norm(err) < 1e-6:
            break
        q = q + lr * np.linalg.pinv(jac(*q)) @ err
        traj.append(q.copy())
    return np.array(traj), q

# ---- 图 1: IK 伪逆迭代收敛过程 (嵌入正文 3-3 节) ----
fig, ax = plt.subplots(figsize=(6, 5))
target = np.array([1.1, 0.8])
traj, q_sol = ik(target, q_init=[0.2, 0.2])
for i, q in enumerate(traj):
    if i % 4 == 0:
        x1, y1 = L1 * np.cos(q[0]), L1 * np.sin(q[0])
        x2, y2 = fk(*q)
        ax.plot([0, x1, x2], [0, y1, y2], 'o-', color='tab:blue', lw=1.2, alpha=0.35)
ax.plot(target[0], target[1], '*', color='tab:red', ms=22, label='目标点')
q = q_sol
x1, y1 = L1 * np.cos(q[0]), L1 * np.sin(q[0])
x2, y2 = fk(*q)
ax.plot([0, x1, x2], [0, y1, y2], 'o-', color='tab:green', lw=3, ms=8, label='收敛解')
ax.plot(target[0], target[1], '*', color='tab:red', ms=22)
ax.set_title(r'IK 伪逆迭代收敛 ($q_2=$' + f'{np.rad2deg(q[1]):.1f}°)')
ax.set_xlabel('x'); ax.set_ylabel('y')
ax.set_xlim(-0.4, 1.9); ax.set_ylim(-0.4, 1.9)
ax.set_aspect('equal'); ax.grid(alpha=0.3); ax.legend(loc='lower left')
plt.tight_layout()
plt.savefig('附录二_ik_iteration.png', dpi=120)

# ---- 图 2: 奇异位形 (嵌入正文 4-3 节) ----
fig, axes = plt.subplots(1, 2, figsize=(12, 5))

# (a) det(J) = L1*L2*sin(q2) 随 q2 变化, 奇异位形处 det=0
ax = axes[0]
q2s = np.linspace(-np.pi, np.pi, 400)
ax.plot(q2s, L1 * L2 * np.sin(q2s), color='tab:blue', lw=2)
ax.axhline(0, color='k', lw=0.8)
for q2_ in [-np.pi, 0, np.pi]:
    ax.plot(q2_, 0, 'o', color='tab:red', ms=8)
    ax.annotate(f'$q_2={np.rad2deg(q2_):.0f}^\\circ$', (q2_, 0),
                textcoords='offset points', xytext=(0, 10), ha='center',
                color='tab:red')
ax.set_title(r'$det(J) = L_1 L_2 \sin q_2$')
ax.set_xlabel(r'$q_2$ (rad)'); ax.set_ylabel(r'$\det(J)$')
ax.set_ylim(-0.8, 0.8); ax.grid(alpha=0.3)

# (b) 奇异位形: 手臂完全伸直 (q2=0) 时末端可动方向降维
ax = axes[1]
q1 = np.deg2rad(45); q2 = 0.0   # 奇异位形
x1, y1 = L1 * np.cos(q1), L1 * np.sin(q1)
x2, y2 = fk(q1, q2)
ax.plot([0, x1, x2], [0, y1, y2], 'o-', color='tab:blue', lw=3, ms=8)
ax.plot(x2, y2, '*', color='tab:red', ms=22)
# 末端可行运动方向: 沿手臂方向(径向)可动, 切向不可动
d_rad = np.array([np.cos(q1), np.sin(q1)])          # 径向(可动)
d_tan = np.array([-np.sin(q1), np.cos(q1)])         # 切向(不可动)
ax.annotate('', xy=(x2 + 0.35 * d_rad[0], y2 + 0.35 * d_rad[1]), xytext=(x2, y2),
            arrowprops=dict(arrowstyle='->', color='tab:green', lw=2.5))
ax.annotate('', xy=(x2 + 0.35 * d_tan[0], y2 + 0.35 * d_tan[1]), xytext=(x2, y2),
            arrowprops=dict(arrowstyle='->', color='tab:red', lw=2.5))
ax.text(x2 + 0.45 * d_rad[0], y2 + 0.45 * d_rad[1], '可动(径向)',
        color='tab:green', ha='center')
ax.text(x2 + 0.45 * d_tan[0], y2 + 0.45 * d_tan[1], '不可动(切向)',
        color='tab:red', ha='center')
ax.set_title(r'奇异位形: $q_2=0^\circ$ 手臂伸直')
ax.set_xlabel('x'); ax.set_ylabel('y')
ax.set_xlim(-0.4, 1.9); ax.set_ylim(-0.4, 1.9)
ax.set_aspect('equal'); ax.grid(alpha=0.3)

plt.tight_layout()
plt.savefig('附录二_singularity.png', dpi=120)
print('已保存: 附录二_ik_iteration.png, 附录二_singularity.png')

• 运行上面的代码会生成 附录二_ik_iteration.png 和 附录二_singularity.png 两张图，已分别嵌入正文 上篇 3-3 节 和 上篇 4-3 节

附录三 DH 参数图解

• 把 上篇 3-4 节 的 DH 参数表画在二连杆平面机械臂上：坐标系、关节角弧线、连杆长度一目了然，完整代码如下：

# -*- coding: utf-8 -*-
# 附录三: 二连杆平面机械臂 —— DH 参数图解
# 把 DH 参数表 (a0=L1, a1=L2, theta1=q1, theta2=q2, d1=d2=0, alpha0=alpha1=0)
# 直接画在机械臂位形图上: 坐标系、关节角弧线、连杆长度一目了然
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import font_manager
from matplotlib.patches import Arc, Circle

# 配置中文字体（按自己系统路径调整）
font_manager.fontManager.addfont('/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc')
plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

L1, L2 = 1.0, 0.6                          # 连杆长度 a0, a1
q1, q2 = np.deg2rad(40), np.deg2rad(35)     # 关节角 theta1, theta2

base = np.array([0.0, 0.0])
j2 = np.array([L1 * np.cos(q1), L1 * np.sin(q1)])
ee = j2 + np.array([L2 * np.cos(q1 + q2), L2 * np.sin(q1 + q2)])

fig, ax = plt.subplots(figsize=(9.5, 8))

# ---- 画连杆 ----
ax.plot([base[0], j2[0], ee[0]], [base[1], j2[1], ee[1]],
        'o-', color='tab:blue', lw=4, ms=10, zorder=2)

# ---- 画 DH 坐标系: x 轴红 / y 轴绿, z 轴垂直纸面向外 ----
def draw_axes(origin, ang, xlab, ylab):
    xdir = np.array([np.cos(ang), np.sin(ang)])
    ydir = np.array([-np.sin(ang), np.cos(ang)])
    s = 0.42
    ax.annotate('', xy=origin + xdir * s, xytext=origin,
                arrowprops=dict(arrowstyle='->', color='tab:red', lw=2.2))
    ax.annotate('', xy=origin + ydir * s * 0.7, xytext=origin,
                arrowprops=dict(arrowstyle='->', color='tab:green', lw=1.4, alpha=0.7))
    ax.text(origin[0] + xdir[0] * (s + 0.10), origin[1] + xdir[1] * (s + 0.10), xlab,
            color='tab:red', fontsize=14, ha='center', va='center')
    ax.text(origin[0] + ydir[0] * (s * 0.7 + 0.10), origin[1] + ydir[1] * (s * 0.7 + 0.10), ylab,
            color='tab:green', fontsize=12, ha='center', va='center')

draw_axes(base, 0,     r'$x_0$', r'$y_0$')
draw_axes(j2,   q1,    r'$x_1$', r'$y_1$')
draw_axes(ee,   q1+q2, r'$x_2$', r'$y_2$')

# ---- z 轴(垂直纸面向外): 带点的圆表示, 同时标注坐标系编号 ----
for p, lab in [(base, r'$z_0$'), (j2, r'$z_1$'), (ee, r'$z_2$')]:
    ax.add_patch(Circle(p, 0.05, facecolor='tab:blue', edgecolor='k', zorder=3))
    ax.text(p[0] + 0.12, p[1] + 0.12, lab, fontsize=13, color='tab:blue', va='center')

# ---- 关节角弧线 theta1, theta2 ----
def draw_angle(center, a0, a1, radius, label, lr):
    arc = Arc(center, 2 * radius, 2 * radius, theta1=np.rad2deg(a0),
              theta2=np.rad2deg(a1), color='tab:orange', lw=2.4)
    ax.add_patch(arc)
    mid = (a0 + a1) / 2
    ax.text(center[0] + np.cos(mid) * lr, center[1] + np.sin(mid) * lr, label,
            color='tab:orange', fontsize=14, ha='center', va='center')

draw_angle(base, 0, q1, 0.42, r'$\theta_1 = q_1$', 0.70)
draw_angle(j2, q1, q1 + q2, 0.30, r'$\theta_2 = q_2$', 0.60)

# ---- 连杆长度标注 a0, a1 ----
ax.annotate(r'$a_0 = L_1$', xy=(base + j2) / 2, xytext=(0.08, -0.55),
            arrowprops=dict(arrowstyle='->', color='k'), fontsize=14)
ax.annotate(r'$a_1 = L_2$', xy=(j2 + ee) / 2, xytext=(1.38, 0.40),
            arrowprops=dict(arrowstyle='->', color='k'), fontsize=14)

# ---- 基座/关节/末端标注 ----
ax.annotate('基座(关节 1)', base, textcoords='offset points', xytext=(-58, 12), fontsize=11)
ax.annotate('关节 2', j2, textcoords='offset points', xytext=(12, 16), fontsize=11)
ax.annotate('末端', ee, textcoords='offset points', xytext=(14, 2), fontsize=11)

# ---- DH 说明框 ----
props = dict(boxstyle='round', facecolor='lightyellow', alpha=0.85, edgecolor='gray')
ax.text(0.02, 0.02,
        r'$d_1 = d_2 = 0$：三个 $x$ 轴共面，无 $z$ 向偏移'
        '\n' + r'$\alpha_0 = \alpha_1 = 0$：$z$ 轴全部平行（垂直纸面向外）',
        transform=ax.transAxes, fontsize=12, va='bottom', bbox=props)

ax.set_xlim(-1.0, 2.0); ax.set_ylim(-0.9, 2.1)
ax.set_aspect('equal'); ax.grid(alpha=0.25)
ax.set_title('二连杆平面机械臂 DH 参数图解', fontsize=15)
plt.tight_layout()
plt.savefig('附录三_dh_parameters.png', dpi=120)
print('已保存: 附录三_dh_parameters.png')

• 运行上面的代码会生成 附录三_dh_parameters.png，已嵌入正文 上篇 3-4 节：橙色弧线是关节角 ${\theta }_1=q_1$、${\theta }_2=q_2$，沿连杆标注的是连杆长度 $a_0=L_1$、$a_1=L_2$，红色/绿色箭头是各连杆坐标系，带点圆表示垂直纸面向外的 $z$ 轴

附录四 牛顿-欧拉与拉格朗日验证

• 用二连杆机械臂把两条动力学路线做个数值对账：拉格朗日闭式（5-2 节的 $M(q),C(q,\dot{q}),g(q)$）和牛顿-欧拉递推（5-3 节，平面臂退化成标量版）各算一遍关节力矩，随机抽 20 组状态比对，完整代码如下：

# -*- coding: utf-8 -*-
# 附录四: 二连杆平面机械臂 —— 牛顿-欧拉递推 vs 拉格朗日闭式 动力学对账
# 输出: 附录四_ne_lagrange_validation.png (嵌入正文 5-4 节)
#   左图: 20 组随机 (q, q', q'') 下两种方法算出的力矩, 全部落在 y=x 直线上
#   右图: 单个状态的力矩构成 (M q'' + C q' + g), 牛顿-欧拉总值打叉验证
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import font_manager

# 配置中文字体（按自己系统路径调整）
font_manager.fontManager.addfont('/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc')
plt.rcParams['font.sans-serif'] = ['Noto Sans CJK JP', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False

# 二连杆参数: 均匀细杆, 质心在中点; 注意这里把机械臂竖起来(重力沿 -y),
# 这样重力项 g(q) 才不为零, 5-2 节方程的三个分项都能验证到
L1, L2 = 1.0, 0.6
m1, m2 = 1.0, 0.8
lc1, lc2 = L1 / 2, L2 / 2
I1, I2 = m1 * L1 ** 2 / 12, m2 * L2 ** 2 / 12
g = 9.81

# ---- 方法一: 拉格朗日闭式 (5-2 节方程 M q'' + C q' + g = tau) ----
def lagrange_tau(q, qd, qdd):
    q1, q2 = q
    q1d, q2d = qd
    q1dd, q2dd = qdd
    c2, s2 = np.cos(q2), np.sin(q2)
    h = m2 * L1 * lc2 * s2                       # 科氏/离心强度
    M11 = m1 * lc1 ** 2 + I1 + m2 * (L1 ** 2 + lc2 ** 2 + 2 * L1 * lc2 * c2) + I2
    M12 = m2 * (lc2 ** 2 + L1 * lc2 * c2) + I2
    M22 = m2 * lc2 ** 2 + I2
    M = np.array([[M11, M12], [M12, M22]])
    C = np.array([[-h * q2d, -h * (q1d + q2d)], [h * q1d, 0.0]])
    gq = np.array([m1 * g * lc1 * np.cos(q1) + m2 * g * (L1 * np.cos(q1) + lc2 * np.cos(q1 + q2)),
                   m2 * g * lc2 * np.cos(q1 + q2)])
    return M @ qdd + C @ qd + gq, M, C @ qd, gq

# ---- 方法二: 牛顿-欧拉递推 (5-3 节, 平面臂退化为标量版) ----
def cross2(a, b):
    """2D 叉积的 z 分量: a x b = a_x b_y - a_y b_x"""
    return a[0] * b[1] - a[1] * b[0]

def rot_cross(alpha, r):
    """(alpha zhat) x r = alpha * (-r_y, r_x)"""
    return alpha * np.array([-r[1], r[0]])

def ne_tau(q, qd, qdd):
    q1, q2 = q
    q1d, q2d = qd
    q1dd, q2dd = qdd
    w1, a1 = q1d, q1dd                            # 连杆 1 角速度/角加速度
    w2, a2 = q1d + q2d, q1dd + q2dd               # 连杆 2
    u1 = np.array([np.cos(q1), np.sin(q1)])       # 连杆 1 方向单位向量
    u2 = np.array([np.cos(q1 + q2), np.sin(q1 + q2)])
    rj2 = L1 * u1                                 # 关节 2 位置
    r1 = lc1 * u1                                 # 连杆 1 质心
    r2 = rj2 + lc2 * u2                           # 连杆 2 质心
    rel2 = lc2 * u2                               # 连杆 2 质心相对关节 2
    # 前向递推: 质心加速度 = 关节点加速度 + 相对关节的旋转项
    ac1 = rot_cross(a1, r1) - w1 ** 2 * r1
    ac_j2 = rot_cross(a1, rj2) - w1 ** 2 * rj2    # 关节 2 点的加速度(连杆1上的点)
    ac2 = ac_j2 + rot_cross(a2, rel2) - w2 ** 2 * rel2
    gvec = np.array([0.0, -g])
    F1 = m1 * (ac1 - gvec)                        # 含重力: 关节要额外承担 mg
    F2 = m2 * (ac2 - gvec)
    N1, N2 = I1 * a1, I2 * a2
    # 后向递推: 关节力矩 (关于关节轴的力矩平衡)
    tau2 = N2 + cross2(rel2, F2)                  # 关节 2: 关于关节 2 的力矩平衡
    tau1 = N1 + N2 + cross2(r1, F1) + cross2(r2, F2)   # 关节 1: 关于基座的力矩平衡
    return np.array([tau1, tau2])

# ---- 数值对账: 20 组随机状态 ----
rng = np.random.default_rng(7)
errs, t_lags, t_nes = [], [], []
for _ in range(20):
    q = rng.uniform(-np.pi, np.pi, 2)
    qd = rng.uniform(-2, 2, 2)
    qdd = rng.uniform(-3, 3, 2)
    t_lag, *_ = lagrange_tau(q, qd, qdd)
    t_ne = ne_tau(q, qd, qdd)
    errs.append(np.linalg.norm(t_lag - t_ne))
    t_lags.append(t_lag); t_nes.append(t_ne)
t_lags, t_nes = np.array(t_lags), np.array(t_nes)
print('20 组随机状态最大误差:', max(errs))

# ---- 单一状态, 用于画力矩构成 ----
q0 = np.array([0.5, 1.2]); qd0 = np.array([0.8, -0.5]); qdd0 = np.array([1.2, -0.8])
t_lag0, M0, Cqd0, gq0 = lagrange_tau(q0, qd0, qdd0)
t_ne0 = ne_tau(q0, qd0, qdd0)
print('单一状态 拉格朗日:', t_lag0, ' 牛顿-欧拉:', t_ne0)

fig, axes = plt.subplots(1, 2, figsize=(12, 5))

# (a) 随机状态验证: 两种方法应完全落在 y=x 上
ax = axes[0]
lim = max(np.abs(t_lags).max(), np.abs(t_nes).max()) * 1.1
ax.plot([-lim, lim], [-lim, lim], 'k--', lw=1, label=r'$y = x$')
ax.plot(t_lags[:, 0], t_nes[:, 0], 'o', color='tab:blue', ms=6, label='关节 1')
ax.plot(t_lags[:, 1], t_nes[:, 1], 's', color='tab:orange', ms=6, label='关节 2')
ax.set_title('(a) 牛顿-欧拉 vs 拉格朗日（20 组随机状态）')
ax.set_xlabel(r'$\tau$ 拉格朗日闭式 (N·m)')
ax.set_ylabel(r'$\tau$ 牛顿-欧拉递推 (N·m)')
ax.set_xlim(-lim, lim); ax.set_ylim(-lim, lim)
ax.set_aspect('equal'); ax.grid(alpha=0.3); ax.legend(loc='lower right')
ax.text(0.04, 0.95, f'最大误差 {max(errs):.1e}', transform=ax.transAxes,
        va='top', fontsize=11, color='tab:red')

# (b) 单一状态力矩构成: 堆叠柱 = M q'' + C q' + g, 红叉 = 牛顿-欧拉总值
ax = axes[1]
Mj = M0 @ qdd0
x = np.arange(2)
ax.bar(x, Mj, 0.5, label=r'$M\ddot{q}$ 惯性', color='tab:blue')
ax.bar(x, Cqd0, 0.5, bottom=Mj, label=r'$C\dot{q}$ 科氏/离心', color='tab:orange')
ax.bar(x, gq0, 0.5, bottom=Mj + Cqd0, label=r'$g(q)$ 重力', color='tab:green')
ax.plot(x, t_ne0, 'x', color='red', ms=15, lw=2.5, label='牛顿-欧拉总值')
ax.set_xticks(x); ax.set_xticklabels(['关节 1', '关节 2'])
ax.axhline(0, color='k', lw=0.8)
ax.set_ylabel(r'$\tau$ (N·m)')
ax.set_title('(b) 力矩构成：拉格朗日分项 + 牛顿-欧拉总值')
ax.grid(alpha=0.3, axis='y'); ax.legend(loc='lower right')

plt.tight_layout()
plt.savefig('附录四_ne_lagrange_validation.png', dpi=120)
print('已保存: 附录四_ne_lagrange_validation.png')

• 运行上面的代码会生成 附录四_ne_lagrange_validation.png，已嵌入正文 上篇 5-4 节：左图 20 组随机状态全部落在 $y=x$ 直线上（最大误差 $10^{-15}$ 量级，纯浮点误差），说明牛顿-欧拉递推和拉格朗日闭式是同一条方程的两副面孔；右图把一个具体状态的力矩拆成 $M\ddot{q}$（惯性）、$C\dot{q}$（科氏/离心）、$g(q)$（重力）三个分项堆叠起来，红色叉号是牛顿-欧拉算出的总值，正好落在堆叠柱顶——三部分加起来的物理含义一目了然

总结

• 本文从机械臂的"骨架"讲到"神经"，把具身智能的第一步地基打牢了
• 我们先用 numpy + matplotlib 把二连杆机械臂的 FK、工作空间、IK、奇异位形全部画了出来，让公式变成看得见的图
• 核心要点回顾：
  • 机械臂结构：连杆 + 关节（旋转/移动）串联，6 个自由度 = 末端 3 位置 + 3 姿态
  • 位姿表示：位置 $p$ 用三维向量，姿态用旋转矩阵 $R$（机器算）/ 欧拉角（人看，注意万向锁）/ 四元数（插值最稳）；齐次变换矩阵 $T$ 把旋转平移打包成 $4\times 4$，正运动学就是 $T$ 的连乘
  • 运动学：FK 从 $q$ 求末端位姿（唯一解），IK 从末端位姿求 $q$（多解、可能奇异、可能无解），DH 参数 4 个数 $a,\alpha ,d,\theta$ 描述一个关节
  • 速度运动学：$\dot{x}=J(q)\dot{q}$，雅可比是关节空间和操作空间的"翻译官"；奇异位形（$\det J=0$）末端某方向锁死、关节速度爆炸，必须避开
  • 动力学：$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)=\tau$，正动力学用于仿真，逆动力学用于控制；重力补偿让机械臂"失重"，拖动示教全靠它
  • 运动规划：关节空间规划简单安全，笛卡尔空间规划末端路径可控但要做 IK；三次多项式 / 梯形速度 / S 曲线对应位置 / 速度 / 加速度连续
  • 控制：PID 简单但忽视耦合；计算力矩控制用模型把系统线性化；阻抗/导纳控制让机械臂"变软"，是人机协作的基础
• 下一期我们将基于这些基础，正式上手 MoveIt2 + Gazebo 机械臂仿真——把本章的 FK/IK、运动规划、控制全部在仿真环境里跑起来
• 如有错误，欢迎指出！
• 感谢观看！