00-数学基础 · 机器人学基础:正运动学 · 逆运动学 · 动力学¶
秋招 / 自动驾驶 / 具身智能岗数学地基 · 第 7 篇 读完这篇,你能用大白话解释:机械臂怎么"知道"自己末端在哪(正运动学)、 怎么"算出"每个关节该转多少(逆运动学)、为什么有的姿态一碰就"卡死"(奇异位形)、 为什么机器人高速挥臂时关节之间会"互相拉扯"(动力学耦合)。 全程中文、公式配 LaTeX、代码用 numpy 从零实现:旋转矩阵 → DH 参数 → 正解 → 反解 → 雅可比 → 拉格朗日动力学。
本节要解决的问题¶
| 要解决的问题 | 对应内容 |
|---|---|
| 机械臂末端到底在空间的哪个位置、什么朝向? | 刚体变换与齐次变换矩阵(SE(3)) |
| 给定每个关节的角度,末端位姿怎么算? | 正运动学(FK):DH 参数与矩阵连乘 |
| 反过来:要让末端到达某位姿,各关节该转多少? | 逆运动学(IK):解析法 + 数值法 |
| 关节转得快,末端会以多快、往哪个方向动? | 雅可比矩阵与速度学、奇异位形 |
| 要让机械臂动起来并保持稳定,关节需要多大扭矩? | 动力学:M(q)q̈ + C(q,q̇)q̇ + G(q) = τ |
一条主线:位姿(几何)→ 正/逆解(运动学)→ 速度/奇异(一阶微分)→ 力/扭矩(二阶动力学), 每一步都是前面一步的"微分"或"求逆"——本质都是线性代数与微积分在刚体上的应用。
故事引入:端咖啡的机械臂在想什么¶
假设你面前是一台六关节机械臂,任务是"把咖啡从桌子一端端到另一端"。它脑子里要过四道题:
- 它在哪? 六个关节角分别是 $(\theta_1,...,\theta_6)$,末端夹爪的位置和朝向是多少?——这是正运动学:关节空间 → 笛卡尔空间,答案唯一,闭式可算。
- 怎么过去? 目标是"夹爪移动到 (x, y, z) 且朝向向下",那六个关节角应该是多少?——这是逆运动学:答案常常不唯一(肘上/肘下、腕翻转),还可能无解(目标太远够不着)。
- 动多快? 关节以速度 $\dot{\boldsymbol{q}}$ 转,末端以多快、往哪个方向走?——雅可比矩阵 $\boldsymbol{J}$ 把关节速度映射成末端速度 $\dot{\boldsymbol{x}}=\boldsymbol{J}\dot{\boldsymbol{q}}$;有些姿态下某个方向"怎么动都动不了",就是奇异位形。
- 要多大劲? 末端扛着咖啡的重量、还要抵抗惯性,每个关节电机该输出多大扭矩?——动力学 $\boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}} + \boldsymbol{G}(\boldsymbol{q}) = \boldsymbol{\tau}$:惯性、科氏力、重力三座大山。
这四道题,就是机器人学的"牛顿三定律"——而它们的数学工具,恰好全是你前六篇打下的地基(线性代数、矩阵、微积分)。
核心概念¶
(a) 🗂️ 机器人学基础 · 知识结构树¶
机器人学的核心是回答"末端与关节之间,位置/速度/力如何双向换算"。从描述刚体的旋转开始,一步步长出正解、反解、雅可比和动力学。
flowchart TB
A["🦾 刚体变换<br/>旋转矩阵 R 与齐次矩阵 T"] --> B["🦾 正运动学 FK<br/>关节角 → 末端位姿(DH 连乘)"]
B --> C["🦾 逆运动学 IK<br/>末端位姿 → 关节角(解析/数值)"]
B --> D["🦾 雅可比矩阵 J<br/>关节速度 → 末端速度(奇异位形)"]
D --> E["🦾 动力学<br/>M q̈ + C q̇ + G = τ"]
C --> F["🦾 应用:轨迹规划与控制<br/>(计算力矩 / 阻抗控制)"]
E --> F
- 刚体变换:旋转矩阵 $\boldsymbol{R}\in SO(3)$(正交、行列式为 1)描述朝向,齐次矩阵 $\boldsymbol{T}=\begin{bmatrix}\boldsymbol{R} & \boldsymbol{t}\\ \boldsymbol{0} & 1\end{bmatrix}\in SE(3)$ 同时描述位置+朝向,复合变换就是矩阵连乘。
- 正运动学 FK:给定关节角 $\boldsymbol{q}$,末端位姿 $\boldsymbol{T}=\boldsymbol{T}_{01}\boldsymbol{T}_{12}\cdots\boldsymbol{T}_{n-1,n}$;DH 四参数($a,\alpha,d,\theta$)把"相邻连杆关系"写成标准矩阵,唯一、闭式。
- 逆运动学 IK:给定末端位姿反求 $\boldsymbol{q}$;解析法(几何/代数)快但只适用特定结构,数值法(雅可比伪逆迭代)通用但依赖初值;多解、无解、奇异都要处理。
- 雅可比矩阵:$\dot{\boldsymbol{x}}=\boldsymbol{J}(\boldsymbol{q})\dot{\boldsymbol{q}}$,列对应每个关节的"速度贡献";$\det(\boldsymbol{J}^\top\boldsymbol{J})=0$ 时奇异——末端在该方向"失去自由度"。
- 动力学:$\boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{q}}+\boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}}+\boldsymbol{G}(\boldsymbol{q})=\boldsymbol{\tau}$;质量矩阵(惯性耦合)、科氏/离心项(高速互相拉扯)、重力项;控制上做"模型前馈补偿"。
深入原理¶
下面 5 个小节沿用同一套路:大白话直觉 → 严格公式 → 关键推导 → 面试怎么问。 机器人学的符号多,但每个符号都对应一个非常具体的机械画面,别怕。
1. 刚体变换:先学会"描述"末端在哪¶
大白话直觉:机械臂末端是一块"刚体"——它不能被弯曲变形,只能整体平移 + 旋转。 要描述它,需要两样东西:位置 $\boldsymbol{t}\in\mathbb{R}^3$ 和朝向 $\boldsymbol{R}$。朝向用旋转矩阵:矩阵的每一列,就是刚体局部坐标系的三个坐标轴在全局坐标系里的投影。
旋转矩阵 SO(3) 的性质¶
$$ \boldsymbol{R} = \begin{bmatrix} \boldsymbol{r}_1 & \boldsymbol{r}_2 & \boldsymbol{r}_3 \end{bmatrix},\qquad \boldsymbol{R}^\top\boldsymbol{R}=\boldsymbol{I},\quad \det(\boldsymbol{R})=1 $$
- 列向量两两正交、单位长 → $\boldsymbol{R}$ 是正交矩阵:旋转不改变长度与夹角(保度量)。
- $\det=1$ 排除"镜像"(反射),保证是真正的旋转。
- 逆旋转就是转置:$\boldsymbol{R}^{-1}=\boldsymbol{R}^\top$——"把世界坐标转回局部坐标"。
三种基本旋转(绕坐标轴)¶
$$ \boldsymbol{R}_x(\theta)=\begin{bmatrix}1&0&0\\0&\cos\theta&-\sin\theta\\0&\sin\theta&\cos\theta\end{bmatrix},\quad \boldsymbol{R}_y(\theta)=\begin{bmatrix}\cos\theta&0&\sin\theta\\0&1&0\\-\sin\theta&0&\cos\theta\end{bmatrix},\quad \boldsymbol{R}_z(\theta)=\begin{bmatrix}\cos\theta&-\sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1\end{bmatrix} $$
- 复合旋转按作用顺序右乘:先绕 z 转 $\varphi$ 再绕新 y 转 $\theta$,是 $\boldsymbol{R}=\boldsymbol{R}_z(\varphi)\boldsymbol{R}_y(\theta)$(注意:绕"转动后的轴"旋转 = 矩阵右乘)。
欧拉角与四元数(一句话对比)¶
- ZYX 欧拉角:$\boldsymbol{R}=\boldsymbol{R}_z(\varphi)\boldsymbol{R}_y(\theta)\boldsymbol{R}_x(\psi)$,直观但会万向锁(中间角 $\theta=\pm90^\circ$ 时丢失一个自由度)。
- 四元数 $\boldsymbol{q}=(w,x,y,z)$,$\|\boldsymbol{q}\|=1$:无万向锁、插值平滑,SLAM/无人机/游戏引擎都用它;与旋转矩阵可互相转换。
齐次变换矩阵 SE(3):位置 + 朝向一把装¶
$$ \boldsymbol{T}=\begin{bmatrix}\boldsymbol{R}_{3\times3} & \boldsymbol{t}_{3\times1}\\ \boldsymbol{0}_{1\times3} & 1\end{bmatrix},\qquad \boldsymbol{T}^{-1}=\begin{bmatrix}\boldsymbol{R}^\top & -\boldsymbol{R}^\top\boldsymbol{t}\\ \boldsymbol{0} & 1\end{bmatrix} $$
- 点 $\boldsymbol{p}$ 变换:$\boldsymbol{p}'=\boldsymbol{R}\boldsymbol{p}+\boldsymbol{t}$,写成齐次就是 $\tilde{\boldsymbol{p}}'=\boldsymbol{T}\tilde{\boldsymbol{p}}$。
- 复合变换 = 矩阵连乘:从基座到第 3 个连杆 $\boldsymbol{T}_{03}=\boldsymbol{T}_{01}\boldsymbol{T}_{12}\boldsymbol{T}_{23}$——"先到 1,再在 1 的坐标系里到 2,再在 2 的坐标系里到 3"。
面试怎么问:"旋转矩阵为什么一定是正交阵?"——因为刚体旋转保持长度与夹角,坐标轴投影必然构成单位正交基;"欧拉角的缺点?"——万向锁与不直观的旋转顺序;"四元数为什么好?"——无万向锁、可球面线性插值(slerp)。
2. 正运动学(FK):关节角 → 末端位姿¶
大白话直觉:每个关节"告诉"下一个连杆:我相对你偏移多少、转了多大角。把这些"相对关系"一个个串起来(矩阵连乘),就能从基座一路算到末端。给定关节角,末端位姿唯一确定。
DH 四参数(Denavit–Hartenberg)¶
相邻两个连杆之间的相对位姿,用四个数字描述:
| 参数 | 含义 | 对应变换 |
|---|---|---|
| $a_{i-1}$ | 连杆长度(沿公共法线的距离) | $\text{Trans}(x, a)$ |
| $\alpha_{i-1}$ | 连杆扭转(绕公共法线转角) | $\text{Rot}(x, \alpha)$ |
| $d_i$ | 连杆偏距(沿 z 轴距离) | $\text{Trans}(z, d)$ |
| $\theta_i$ | 关节角(绕 z 轴转角,变量) | $\text{Rot}(z, \theta)$ |
相邻连杆变换矩阵¶
$$ \boldsymbol{A}_i = \boldsymbol{R}_z(\theta_i)\boldsymbol{T}_z(d_i)\boldsymbol{T}_x(a_{i-1})\boldsymbol{R}_x(\alpha_{i-1}) = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_{i-1} & \sin\theta_i\sin\alpha_{i-1} & a_{i-1}\cos\theta_i\\ \sin\theta_i & \cos\theta_i\cos\alpha_{i-1} & -\cos\theta_i\sin\alpha_{i-1} & a_{i-1}\sin\theta_i\\ 0 & \sin\alpha_{i-1} & \cos\alpha_{i-1} & d_i\\ 0 & 0 & 0 & 1 \end{bmatrix} $$
- 正运动学:$\boldsymbol{T}_{0n} = \boldsymbol{A}_1\boldsymbol{A}_2\cdots\boldsymbol{A}_n$,取左上角是旋转、右上角是末端位置。
2 连杆平面臂:徒手推导¶
两根杆(长 $l_1,l_2$)在平面内转动,关节角 $\theta_1,\theta_2$。末端坐标:
$$ x = l_1\cos\theta_1 + l_2\cos(\theta_1+\theta_2),\qquad y = l_1\sin\theta_1 + l_2\sin(\theta_1+\theta_2) $$
推导:第一根杆末端在 $(l_1\cos\theta_1, l_1\sin\theta_1)$;第二根杆相对第一根再转 $\theta_2$,方向角是 $\theta_1+\theta_2$,再加 $l_2(\cos,\sin)$。末端朝向 $\varphi=\theta_1+\theta_2$(实验 1 用矩阵与这个公式对拍)。
面试怎么问:"正运动学为什么是唯一的?"——每个关节角被代入的都是确定矩阵,连乘结果唯一;"DH 四参数分别描述什么运动?"——$\theta$ 绕 z 旋转(通常是关节变量)、$d$ 沿 z 平移、$a$ 沿 x 平移、$\alpha$ 绕 x 旋转。
✍️ §2.1 手算 2 连杆正运动学:60° + 30°,末端在哪¶
平面 2 连杆正运动学(FK):$x=L_1\cos\theta_1+L_2\cos(\theta_1+\theta_2)$、$y=L_1\sin\theta_1+L_2\sin(\theta_1+\theta_2)$——代具体角度算一遍,再顺手验证逆解。
设定:$L_1=L_2=1$,$\theta_1=60^\circ$、$\theta_2=30^\circ$。
第 1 步:列出三角函数(角度选得好,计算很干净)
$$\cos60^\circ=0.5,\ \sin60^\circ=0.8660,\qquad \cos(60^\circ+30^\circ)=\cos90^\circ=0,\ \sin90^\circ=1$$
第 2 步:末端位置
$$x=1\times\cos60^\circ+1\times\cos90^\circ=0.5+0=0.5$$
$$y=1\times\sin60^\circ+1\times\sin90^\circ=0.8660+1=1.866$$
第 3 步:逆解验证(从末端反推第二关节角)
$$d^2=x^2+y^2=0.25+3.484=3.734\Rightarrow\cos\theta_2=\frac{d^2-L_1^2-L_2^2}{2L_1L_2}=0.866\Rightarrow\theta_2=30^\circ\ \checkmark$$
手算收获:
- FK 是"关节角 → 末端位姿"的链条:每根杆的方向角是"前面所有关节角之和"($\theta_1+\theta_2$),位置是每根杆端点向量相加
- 逆运动学(IK)是非线性的(反余弦/多解):$\cos\theta_2=0.866$ 其实还对应 $-30^\circ$(肘向上/向下两种构型)——这就是机器人 IK 有多解、要加关节限位的原因
- 雅可比矩阵就是 $J=\frac{\partial(x,y)}{\partial(\theta_1,\theta_2)}$:把"关节速度"映射到"末端速度"(本 notebook §4)
- 面试速答:正解闭式简单,逆解靠数值迭代/解析解,动力学再叠加惯性项——这条主线贯穿机械臂设计
下面 numpy 复算。
# ---- §2.1 验证:numpy 复算 2 连杆正运动学 ----
import numpy as np
L1 = L2 = 1.0
th1, th2 = np.deg2rad(60), np.deg2rad(30)
x = L1 * np.cos(th1) + L2 * np.cos(th1 + th2)
y = L1 * np.sin(th1) + L2 * np.sin(th1 + th2)
print(f'x = {x:.3f}, y = {y:.3f}(手算 (0.5, 1.866))')
assert abs(x - 0.5) < 1e-9 and abs(y - 1.8660) < 1e-3
# 逆解验证
d2 = x**2 + y**2
c2 = (d2 - L1**2 - L2**2) / (2 * L1 * L2)
th2_rec = np.rad2deg(np.arccos(np.clip(c2, -1, 1)))
print(f'cos(θ2) = {c2:.4f} → θ2 = {th2_rec:.1f}°')
assert abs(th2_rec - 30) < 1e-6
print('\n✓ 与手算一致:FK 正解 + 逆解互相印证')
# 中文输出编码保护:普通 Python 下包装成 UTF-8;Jupyter 内核中 OutStream 无 buffer,直接跳过
import sys, io
try:
sys.stdout = io.TextIOWrapper(sys.stdout.buffer, encoding='utf-8', errors='replace')
except Exception:
pass # Jupyter 内核下 sys.stdout 已是 UTF-8,无需包装
# ══════════════════════════════════════════════════════════
# 实验 1:旋转矩阵 / 齐次变换 / 2 连杆正运动学(numpy 从零实现)
# 目标:验证 RᵀR=I、复合变换 = 矩阵连乘、FK 公式与矩阵法一致
# ══════════════════════════════════════════════════════════
import os
import numpy as np
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['Microsoft YaHei', 'SimHei']
plt.rcParams['axes.unicode_minus'] = False
os.makedirs('images', exist_ok=True)
def rot_x(a): return np.array([[1,0,0],[0,np.cos(a),-np.sin(a)],[0,np.sin(a),np.cos(a)]])
def rot_y(a): return np.array([[np.cos(a),0,np.sin(a)],[0,1,0],[-np.sin(a),0,np.cos(a)]])
def rot_z(a): return np.array([[np.cos(a),-np.sin(a),0],[np.sin(a),np.cos(a),0],[0,0,1]])
def make_T(R, t):
T = np.eye(4); T[:3,:3] = R; T[:3,3] = t
return T
# (1) 验证旋转矩阵正交:RᵀR = I,det = 1
R = rot_z(0.7) @ rot_y(0.3)
err = np.abs(R.T @ R - np.eye(3)).max()
print('RᵀR−I 最大误差 = %.2e, det(R) = %.6f → 正交性%s' %
(err, np.linalg.det(R), '✓' if err < 1e-12 else '✗'))
# (2) 复合变换 = 矩阵连乘:T03 = T01·T12·T23
T01 = make_T(rot_z(0.5), [0.4, 0.0, 0.3])
T12 = make_T(rot_z(-0.2), [0.8, 0.1, 0.0])
T23 = make_T(rot_x(0.9), [0.0, 0.0, 0.5])
T03 = T01 @ T12 @ T23
print('复合变换 T03 位置 =', np.round(T03[:3,3], 4), '(矩阵连乘结果)')
# (3) 2 连杆平面臂正运动学:公式 vs DH 矩阵
def fk_2link(theta1, theta2, l1=1.0, l2=0.8):
x = l1*np.cos(theta1) + l2*np.cos(theta1+theta2)
y = l1*np.sin(theta1) + l2*np.sin(theta1+theta2)
return np.array([x, y]), theta1 + theta2 # 位置 + 末端朝向角
# DH 矩阵法:两个旋转关节(α=0, a=l1/l2, d=0, θ 为变量)
def dh_mat(theta, a):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c,-s,0,a*c],[s,c,0,a*s],[0,0,1,0],[0,0,0,1.]])
th1, th2 = 0.6, -1.1
A1, A2 = dh_mat(th1, 1.0), dh_mat(th2, 0.8)
pos_mat = (A1 @ A2)[:2, 3]
pos_f, phi = fk_2link(th1, th2)
print('FK 公式 =', np.round(pos_f, 6), '朝向 = %.4f' % phi)
print('FK 矩阵 =', np.round(pos_mat, 6))
print('两法一致 ✓' if np.abs(pos_f - pos_mat).max() < 1e-9 else '不一致 ✗')
# (4) 画 2 连杆臂位形图(多条随机姿态)
fig, ax = plt.subplots(figsize=(5,5))
for k in range(8):
t1 = np.random.uniform(-1.2, 1.2); t2 = np.random.uniform(-1.5, 1.5)
p1 = np.array([np.cos(t1), np.sin(t1)])
p2 = p1 + np.array([np.cos(t1+t2), np.sin(t1+t2)]) * 0.8
ax.plot([0, p1[0], p2[0]], [0, p1[1], p2[1]], 'o-', lw=2, ms=4, alpha=.7)
ax.set_title('2 连杆平面臂正运动学:同一基座、不同关节角')
ax.set_xlim(-2, 2); ax.set_ylim(-2, 2); ax.set_aspect('equal'); ax.grid(alpha=.3)
fig.savefig('images/math06_fk.png', dpi=110, bbox_inches='tight'); plt.close(fig)
plt.show()
print('已保存 images/math06_fk.png')
RᵀR−I 最大误差 = 1.11e-16, det(R) = 1.000000 → 正交性✓ 复合变换 T03 位置 = [1.0541 0.4713 0.8 ] (矩阵连乘结果) FK 公式 = [1.527402 0.181102] 朝向 = -0.5000 FK 矩阵 = [1.527402 0.181102] 两法一致 ✓
已保存 images/math06_fk.png
3. 逆运动学(IK):末端位姿 → 关节角¶
大白话直觉:正解是"按钮按下就知道灯在哪",反解是"看到灯在哪,反推按钮该按几下"。反解难在:同一个灯位置可能对应多种按法(多解)、有些位置根本按不出来(无解)、甚至有几组解之间会"打架"(奇异)。
2 连杆解析解(几何法,白板推导)¶
目标点 $(x,y)$,杆长 $l_1,l_2$。先求 $\theta_2$——利用余弦定理(末端到基座的距离 $d=\sqrt{x^2+y^2}$):
$$ \cos\theta_2 = \frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2},\qquad \theta_2 = \pm\arccos\left(\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}\right) $$
$\theta_2>0$ 是"肘上"、$\theta_2<0$ 是"肘下"——两个解。再求 $\theta_1$:
$$ \theta_1 = \operatorname{atan2}(y,x) - \operatorname{atan2}\left(l_2\sin\theta_2,\; l_1+l_2\cos\theta_2\right) $$
推导:向量 $(x,y)$ 的极角 = 基座角 $\theta_1$ + 第二根杆相对方向角 $\operatorname{atan2}(l_2\sin\theta_2,\,l_1+l_2\cos\theta_2)$。
- 可达性:必须有 $(l_1-l_2)^2 \le x^2+y^2 \le (l_1+l_2)^2$,否则无解(够不着 / 太近折回)。
数值 IK:雅可比伪逆迭代(通用方法)¶
任何机械臂都能用迭代逼近:设当前关节角 $\boldsymbol{q}$,末端误差 $\boldsymbol{e}=\boldsymbol{x}_{\text{des}}-f(\boldsymbol{q})$,
$$ \boldsymbol{q} \leftarrow \boldsymbol{q} + \boldsymbol{J}(\boldsymbol{q})^+\boldsymbol{e} \qquad\left(\boldsymbol{J}^+=\boldsymbol{J}^\top(\boldsymbol{J}\boldsymbol{J}^\top)^{-1}\text{ 右伪逆}\right) $$
直观:$\dot{\boldsymbol{x}}=\boldsymbol{J}\dot{\boldsymbol{q}}$ 说明"关节速度怎么影响末端速度",伪逆就是"要末端朝目标走,关节最快怎么转"(最小二乘意义下的最小范数解)。迭代直到 $\|\boldsymbol{e}\|<\epsilon$。
多解 / 冗余 / 无解¶
- 6 自由度一般结构:最多 8 组解析解(肩肘腕各有 2 个翻转选择);
- 冗余机械臂(如 7 自由度):解无穷多 → 常加"避障/舒适度"等次级目标选择最优解;
- 无解:目标在工作空间之外,或处于奇异位形。
面试怎么问:"正解唯一、反解不唯一,为什么?"——正解是确定映射,反解是高维到低维的方程(非线性三角方程),多解天然存在;"解析 IK 与数值 IK 的取舍?"——解析快、可枚举全部解,但依赖结构;数值通用、实现简单,但依赖初值、迭代可能陷入局部。
# 中文输出编码保护:普通 Python 下包装成 UTF-8;Jupyter 内核中 OutStream 无 buffer,直接跳过
import sys, io
try:
sys.stdout = io.TextIOWrapper(sys.stdout.buffer, encoding='utf-8', errors='replace')
except Exception:
pass # Jupyter 内核下 sys.stdout 已是 UTF-8,无需包装
# ══════════════════════════════════════════════════════════
# 实验 2:2 连杆逆运动学——解析解(肘上/肘下)+ 数值解(雅可比伪逆)
# 目标:fk(ik(x,y)) ≈ (x,y) 对拍;数值法收敛到同一目标
# ══════════════════════════════════════════════════════════
import os
import numpy as np
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['Microsoft YaHei', 'SimHei']
plt.rcParams['axes.unicode_minus'] = False
os.makedirs('images', exist_ok=True)
L1, L2 = 1.0, 0.8
def fk(q, l1=L1, l2=L2):
return np.array([l1*np.cos(q[0]) + l2*np.cos(q[0]+q[1]),
l1*np.sin(q[0]) + l2*np.sin(q[0]+q[1])])
def ik_analytic(x, y, l1=L1, l2=L2):
# 解析逆解:返回 [肘上, 肘下] 两组 (θ1,θ2);cosθ2 = (x²+y²−l1²−l2²)/(2·l1·l2)
d2 = x*x + y*y
cos2 = (d2 - l1*l1 - l2*l2) / (2*l1*l2)
if not (-1 - 1e-9 <= cos2 <= 1 + 1e-9):
return None, None # 目标不可达
cos2 = np.clip(cos2, -1, 1)
th2_up, th2_dn = np.arccos(cos2), -np.arccos(cos2)
th1_up = np.arctan2(y, x) - np.arctan2(l2*np.sin(th2_up), l1+l2*np.cos(th2_up))
th1_dn = np.arctan2(y, x) - np.arctan2(l2*np.sin(th2_dn), l1+l2*np.cos(th2_dn))
return np.array([th1_up, th2_up]), np.array([th1_dn, th2_dn])
def jac(q, l1=L1, l2=L2):
c1, s1 = np.cos(q[0]), np.sin(q[0])
c12, s12 = np.cos(q[0]+q[1]), np.sin(q[0]+q[1])
return np.array([[-l1*s1 - l2*s12, -l2*s12],
[ l1*c1 + l2*c12, l2*c12]])
def ik_numeric(xd, q0, steps=200, tol=1e-9):
q = q0.copy()
for _ in range(steps):
e = xd - fk(q)
if np.linalg.norm(e) < tol:
break
J = jac(q)
q = q + J.T @ np.linalg.solve(J @ J.T + 1e-8*np.eye(2), e) # 阻尼伪逆
return q, np.linalg.norm(xd - fk(q))
# ---- 解析解对拍 ----
target = np.array([1.2, 0.9])
q_up, q_dn = ik_analytic(*target)
print('肘上解 θ =', np.round(q_up, 4), '→ FK 回到', np.round(fk(q_up), 5))
print('肘下解 θ =', np.round(q_dn, 4), '→ FK 回到', np.round(fk(q_dn), 5))
print('解析 IK 对拍 ✓' if (np.abs(fk(q_up)-target).max() < 1e-9 and
np.abs(fk(q_dn)-target).max() < 1e-9) else '对拍失败 ✗')
# ---- 数值解:不同初值收敛到不同解(体现多解) ----
q_num1, e1 = ik_numeric(target, q0=np.array([0.2, 0.2]))
q_num2, e2 = ik_numeric(target, q0=np.array([2.2, 2.2]))
print('数值IK 初值A →', np.round(q_num1,4), '误差=%.2e' % e1)
print('数值IK 初值B →', np.round(q_num2,4), '误差=%.2e' % e2)
# ---- 工作空间 + 双解可视化 ----
fig, ax = plt.subplots(figsize=(5.5,5))
th = np.linspace(0, 2*np.pi, 400)
outer = np.stack([(L1+L2)*np.cos(th), (L1+L2)*np.sin(th)], axis=1) # 外边界
inner = np.stack([(L1-L2)*np.cos(th), (L1-L2)*np.sin(th)], axis=1) # 内边界
ax.fill(np.r_[outer[:,0], inner[::-1,0]], np.r_[outer[:,1], inner[::-1,1]],
color='#d6eaf8', alpha=.6, label='可达工作空间(环)')
for q in [q_up, q_dn, q_num1, q_num2]:
p1 = np.array([L1*np.cos(q[0]), L1*np.sin(q[0])])
p2 = fk(q)
ax.plot([0,p1[0],p2[0]], [0,p1[1],p2[1]], 'o-', lw=2, ms=5)
ax.plot(*target, 'r*', ms=14, label='目标点')
ax.legend(fontsize=9); ax.set_aspect('equal'); ax.grid(alpha=.3)
ax.set_title('逆运动学:同一目标的多组解(肘上/肘下)')
fig.savefig('images/math06_ik.png', dpi=110, bbox_inches='tight'); plt.close(fig)
plt.show()
print('已保存 images/math06_ik.png')
肘上解 θ = [0.1279 1.1796] → FK 回到 [1.2 0.9] 肘下解 θ = [ 1.1591 -1.1796] → FK 回到 [1.2 0.9] 解析 IK 对拍 ✓ 数值IK 初值A → [0.1279 1.1796] 误差=7.30e-11 数值IK 初值B → [1.1591 5.1035] 误差=3.08e-10
已保存 images/math06_ik.png
4. 雅可比矩阵与速度学:关节快慢 → 末端速度¶
大白话直觉:雅可比矩阵是"速度放大倍数表"——第 $j$ 列告诉你"关节 $j$ 每转 1 单位速度,末端往各方向贡献多少速度"。它还揭示了奇异位形:某列失效时,末端在那个方向"怎么使劲都动不了"。
定义与构成¶
$$ \dot{\boldsymbol{x}} = \boldsymbol{J}(\boldsymbol{q})\dot{\boldsymbol{q}},\qquad \boldsymbol{J}=\begin{bmatrix}\boldsymbol{J}_v\\\boldsymbol{J}_\omega\end{bmatrix}\in\mathbb{R}^{6\times n} $$
对旋转关节 $j$(关节轴单位向量 $\boldsymbol{z}_j$,末端位置 $\boldsymbol{p}_e$,关节位置 $\boldsymbol{p}_j$):
$$ \boldsymbol{J}_{v,j} = \boldsymbol{z}_j\times(\boldsymbol{p}_e-\boldsymbol{p}_j),\qquad \boldsymbol{J}_{\omega,j} = \boldsymbol{z}_j $$
推导:关节 $j$ 以角速度 $\dot{q}_j\boldsymbol{z}_j$ 转动时,末端获得线速度 $\dot{q}_j\, \boldsymbol{z}_j\times(\boldsymbol{p}_e-\boldsymbol{p}_j)$(绕关节轴转动的"圆周切线")。
奇异位形(singularity)¶
$$ \operatorname{rank}(\boldsymbol{J}) < \min(6, n)\quad\Longleftrightarrow\quad \det(\boldsymbol{J}^\top\boldsymbol{J})=0 $$
- 此时某些末端速度方向不可达(比如 2 连杆伸直 $\theta_2=0$ 时,末端只能沿臂方向动,垂直方向"锁死");
- 奇异附近雅可比条件数爆炸 → 数值 IK 会要求极大关节速度(伪逆解发散),实际控制中要规避。
力对偶:关节力矩 ↔ 末端力¶
由虚功原理 $\boldsymbol{\tau}^\top\dot{\boldsymbol{q}}=\boldsymbol{F}^\top\dot{\boldsymbol{x}}$,代入 $\dot{\boldsymbol{x}}=\boldsymbol{J}\dot{\boldsymbol{q}}$:
$$ \boldsymbol{\tau} = \boldsymbol{J}^\top \boldsymbol{F} $$
推导:虚功相等——"关节做的功"必须等于"末端力做的功"(能量守恒),两边消去 $\dot{\boldsymbol{q}}$ 即得转置关系。这就是"阻抗控制/力控"的数学基础。
面试怎么问:"什么是奇异位形?"——雅可比降秩,末端在某方向失去自由度,该方向需要无穷大关节速度;"为什么力矩和力的关系是 $\boldsymbol{J}^\top$ 而不是 $\boldsymbol{J}^{-1}$?"——$\boldsymbol{J}$ 通常不是方阵,$\boldsymbol{J}^\top$ 由虚功原理自然导出。
# 中文输出编码保护:普通 Python 下包装成 UTF-8;Jupyter 内核中 OutStream 无 buffer,直接跳过
import sys, io
try:
sys.stdout = io.TextIOWrapper(sys.stdout.buffer, encoding='utf-8', errors='replace')
except Exception:
pass # Jupyter 内核下 sys.stdout 已是 UTF-8,无需包装
# ══════════════════════════════════════════════════════════
# 实验 3:2 连杆雅可比——奇异位形与条件数
# 目标:验证 det(JᵀJ)=0 在 θ₂=0 或 π 时发生;末端"某方向动不了"
# ══════════════════════════════════════════════════════════
import os
import numpy as np
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['Microsoft YaHei', 'SimHei']
plt.rcParams['axes.unicode_minus'] = False
os.makedirs('images', exist_ok=True)
L1, L2 = 1.0, 0.8
def jac(q):
c1, s1 = np.cos(q[0]), np.sin(q[0])
c12, s12 = np.cos(q[0]+q[1]), np.sin(q[0]+q[1])
return np.array([[-L1*s1 - L2*s12, -L2*s12],
[ L1*c1 + L2*c12, L2*c12]])
# (1) 奇异位形扫描:θ₂ 从 -π 到 π,观察 det 与条件数
ths2 = np.linspace(-np.pi, np.pi, 201)
det_vals, cond_vals = [], []
for t2 in ths2:
J = jac([0.0, t2])
det_vals.append(np.linalg.det(J))
cond_vals.append(np.linalg.cond(J))
det_vals, cond_vals = np.array(det_vals), np.array(cond_vals)
zeros = ths2[np.abs(det_vals) < 1e-9]
print('det(J)=0 出现于 θ₂ =', np.round(zeros, 3), '(伸/折成直线 → 奇异)')
# (2) 末端各方向"可动性":在奇异位形 θ₂=0 时,伪逆给出的关节速度方向
q_sing = np.array([0.6, 0.0])
J = jac(q_sing)
print('奇异位形 J =', np.round(J, 3))
print('det(J) = %.2e → 秩亏(第 2 列 = 第 1 列 × 倍数,只剩 1 个自由度)' % np.linalg.det(J))
# (3) 条件数曲线:越接近奇异,条件数越大
fig, ax = plt.subplots(figsize=(6,4))
ax.plot(np.degrees(ths2), np.log10(cond_vals), lw=2, color='#c0392b')
ax.set_xlabel('theta2 (deg)'); ax.set_ylabel('log10 条件数')
ax.set_title('2 连杆雅可比条件数:theta2 = 0 / ±180° 处发散(奇异)')
ax.grid(alpha=.3)
fig.savefig('images/math06_singular.png', dpi=110, bbox_inches='tight'); plt.close(fig)
plt.show()
print('已保存 images/math06_singular.png')
det(J)=0 出现于 θ₂ = [-3.142 0. 3.142] (伸/折成直线 → 奇异) 奇异位形 J = [[-1.016 -0.452] [ 1.486 0.66 ]] det(J) = -8.25e-17 → 秩亏(第 2 列 = 第 1 列 × 倍数,只剩 1 个自由度)
已保存 images/math06_singular.png
5. 动力学:要让机械臂动起来,需要多大扭矩¶
大白话直觉:关节电机不仅要"扛住重力",还要克服"惯性"(杆越重、越远离关节越难转),更要对付"科氏/离心力"(高速挥舞时关节之间互相拉扯)。动力学方程就是把这些力写成:$惯性 + 科氏离心 + 重力 = 驱动力矩$。
拉格朗日方程¶
对广义坐标 $\boldsymbol{q}$(就是关节角),拉格朗日量 $L=T-U$(动能 − 势能):
$$ \frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial L}{\partial\dot{\boldsymbol{q}}} - \frac{\partial L}{\partial\boldsymbol{q}} = \boldsymbol{\tau} $$
标准动力学方程(2 连杆平面臂,质心在杆中点)¶
$$ \boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}} + \boldsymbol{G}(\boldsymbol{q}) = \boldsymbol{\tau} $$
以 $r_1=l_1/2, r_2=l_2/2$、质量 $m_1,m_2$、转动惯量 $I_1,I_2$、重力 $g$:
- 质量矩阵(惯性耦合,对称正定): $$\boldsymbol{M}=\begin{bmatrix}\alpha+2\beta\cos\theta_2+\delta & \beta\cos\theta_2+\delta\\\beta\cos\theta_2+\delta & \delta\end{bmatrix},\qquad \alpha=m_1r_1^2+m_2(l_1^2+r_2^2)+I_1+I_2,\;\beta=m_2l_1r_2,\;\delta=m_2r_2^2+I_2$$
- 科氏/离心项(只与速度有关,$h=-m_2l_1r_2\sin\theta_2$): $$\boldsymbol{C}=\begin{bmatrix}h\dot{\theta}_2 & h(\dot{\theta}_1+\dot{\theta}_2)\\ -h\dot{\theta}_1 & 0\end{bmatrix}$$
- 重力项: $$\boldsymbol{G}=\begin{bmatrix}(m_1r_1+m_2l_1)g\cos\theta_1 + m_2r_2g\cos(\theta_1+\theta_2)\\ m_2r_2g\cos(\theta_1+\theta_2)\end{bmatrix}$$
推导要点:① 写动能 $T=\tfrac12\dot{\boldsymbol{q}}^\top\boldsymbol{M}\dot{\boldsymbol{q}}$(每个连杆 = 平动 + 转动),势能 $U=\sum m_igh_i$;② 对 $\dot{\boldsymbol{q}}$ 求导得 $\boldsymbol{M}\ddot{\boldsymbol{q}}$,对 $\boldsymbol{q}$ 求导产生科氏/重力项。验证方法:无摩擦、无外力时机械能 $E=T+U$ 守恒——实验 4 用数值积分验证这一点。
计算力矩控制(前馈补偿 + PD)¶
要跟踪目标轨迹 $\boldsymbol{q}_d$,取控制律:
$$ \boldsymbol{\tau} = \boldsymbol{M}(\boldsymbol{q})\Big(\ddot{\boldsymbol{q}}_d + \boldsymbol{K}_d(\dot{\boldsymbol{q}}_d-\dot{\boldsymbol{q}}) + \boldsymbol{K}_p(\boldsymbol{q}_d-\boldsymbol{q})\Big) + \boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}} + \boldsymbol{G}(\boldsymbol{q}) $$
代入动力学方程后:$\ddot{\boldsymbol{e}} + \boldsymbol{K}_d\dot{\boldsymbol{e}} + \boldsymbol{K}_p\boldsymbol{e}=\boldsymbol{0}$($\boldsymbol{e}=\boldsymbol{q}_d-\boldsymbol{q}$)→ 误差按二阶系统指数收敛。直观:先"算清楚"惯性/科氏/重力要吃掉多少扭矩,再"精确补偿",剩下的交给 PD 纠偏——这就是机器人控制的"模型前馈"思想。
面试怎么问:"为什么高速运动时关节之间互相拉扯?"——科氏项 $\propto \dot{\theta}_1\dot{\theta}_2$:一个关节的运动在另一个关节坐标系里产生惯性力;"计算力矩控制为什么稳?"——模型补偿把非线性系统"线性化"成解耦的二阶误差方程,PD 增益可独立设计。
# 中文输出编码保护:普通 Python 下包装成 UTF-8;Jupyter 内核中 OutStream 无 buffer,直接跳过
import sys, io
try:
sys.stdout = io.TextIOWrapper(sys.stdout.buffer, encoding='utf-8', errors='replace')
except Exception:
pass # Jupyter 内核下 sys.stdout 已是 UTF-8,无需包装
# ══════════════════════════════════════════════════════════
# 实验 4:2 连杆拉格朗日动力学——数值仿真 + 能量守恒 + 计算力矩控制
# 目标:① τ=0 重力摆能量守恒 ② 模型补偿控制跟踪目标角
# ══════════════════════════════════════════════════════════
import os
import numpy as np
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['Microsoft YaHei', 'SimHei']
plt.rcParams['axes.unicode_minus'] = False
os.makedirs('images', exist_ok=True)
m1, m2, l1, l2 = 1.0, 1.0, 1.0, 0.8
I1 = m1*l1*l1/12; I2 = m2*l2*l2/12
g = 9.81
def dyn(q, qd, tau):
# 返回 q̈:解 M q̈ = τ − C q̇ − G(Spong 标准 2 连杆模型)
th1, th2 = q
r1, r2 = l1/2, l2/2
c2, s2 = np.cos(th2), np.sin(th2)
alpha = m1*r1*r1 + m2*(l1*l1 + r2*r2) + I1 + I2
beta = m2*l1*r2
delta = m2*r2*r2 + I2
M = np.array([[alpha + 2*beta*c2 + delta, beta*c2 + delta],
[beta*c2 + delta, delta]])
h = -m2*l1*r2*s2
C = np.array([[h*qd[1], h*(qd[0]+qd[1])],
[-h*qd[0], 0.0]])
G = np.array([(m1*r1 + m2*l1)*g*np.cos(th1) + m2*r2*g*np.cos(th1+th2),
m2*r2*g*np.cos(th1+th2)])
return np.linalg.solve(M, tau - C@qd - G)
def energy(q, qd):
# 机械能 T+U(用于守恒验证)
th1, th2 = q
r1, r2 = l1/2, l2/2
c2 = np.cos(th2)
alpha = m1*r1*r1 + m2*(l1*l1+r2*r2) + I1 + I2
beta = m2*l1*r2; delta = m2*r2*r2 + I2
M = np.array([[alpha+2*beta*c2+delta, beta*c2+delta],[beta*c2+delta, delta]])
T = 0.5 * qd @ M @ qd
y1 = r1*np.sin(th1)
y2 = l1*np.sin(th1) + r2*np.sin(th1+th2)
return T + m1*g*y1 + m2*g*y2
def simulate(tau_fn, q0, qd0, dt=1e-3, T=2.0):
q, qd = np.array(q0, float), np.array(qd0, float)
ts, Q = [0.0], [q.copy()]
for _ in range(int(T/dt)):
k1 = dyn(q, qd, tau_fn(q, qd))
q2 = q + dt*qd; qd2 = qd + dt*k1
k2 = dyn(q2, qd2, tau_fn(q2, qd2))
q = q + dt*(qd + qd2)/2
qd = qd + dt*(k1 + k2)/2
ts.append(ts[-1] + dt); Q.append(q.copy())
return np.array(ts), np.array(Q)
# ---- (1) τ=0:重力摆,能量守恒验证 ----
ts, Q = simulate(lambda q, qd: np.zeros(2), [0.5, 0.0], [0.0, 0.0], T=2.0)
E0 = energy(Q[0], np.array([0., 0.]))
Emax = max(abs(energy(Q[i], (Q[i+1]-Q[i-1])/(2*1e-3))) - E0 for i in range(1, len(Q)-1, 10))
print('自由摆(τ=0):2 秒内机械能最大漂移 = %.2e(应接近 0 → 动力学方程自洽 ✓)' % Emax)
# ---- (2) 计算力矩控制:跟踪阶跃目标角 ----
q_des = np.array([1.2, -0.8]); Kp, Kd = 100.0, 20.0
# 显式实现 M/C/G(Spong 标准模型),控制律 τ = M(Kp·e + Kd·ė) + C q̇ + G
def m_c_g(q, qd):
th1, th2 = q
r1, r2 = l1/2, l2/2
c2, s2 = np.cos(th2), np.sin(th2)
alpha = m1*r1*r1 + m2*(l1*l1+r2*r2) + I1 + I2
beta = m2*l1*r2; delta = m2*r2*r2 + I2
M = np.array([[alpha+2*beta*c2+delta, beta*c2+delta],[beta*c2+delta, delta]])
h = -m2*l1*r2*s2
C = np.array([[h*qd[1], h*(qd[0]+qd[1])],[-h*qd[0], 0.0]])
G = np.array([(m1*r1+m2*l1)*g*np.cos(th1) + m2*r2*g*np.cos(th1+th2),
m2*r2*g*np.cos(th1+th2)])
return M, C, G
def ctrl_tau(q, qd):
M, C, G = m_c_g(q, qd)
e = q_des - q; ed = -qd
return M @ (Kp*e + Kd*ed) + C@qd + G # 模型前馈 + PD
ts2, Q2 = simulate(ctrl_tau, [0.1, 0.2], [0.0, 0.0], dt=2e-3, T=3.0)
q_end = Q2[-1]
print('计算力矩控制:目标 θ =', q_des, '→ 3 秒后 θ =', np.round(q_end, 4),
'误差 =', np.round(np.abs(q_end - q_des).max(), 4))
print('控制收敛 ✓' if np.abs(q_end - q_des).max() < 0.02 else '未收敛 ✗')
# ---- 画图:自由摆(左上)与控制跟踪(右下) ----
fig, axes = plt.subplots(1, 2, figsize=(9, 3.8))
axes[0].plot(ts, Q[:,0], lw=2, label='theta1'); axes[0].plot(ts, Q[:,1], lw=2, label='theta2')
axes[0].set_title('tau=0 重力摆(能量守恒)'); axes[0].set_xlabel('t (s)'); axes[0].legend(); axes[0].grid(alpha=.3)
axes[1].plot(ts2, Q2[:,0], lw=2, label='theta1'); axes[1].plot(ts2, Q2[:,1], lw=2, label='theta2')
axes[1].axhline(q_des[0], ls='--', color='gray', lw=1); axes[1].axhline(q_des[1], ls='--', color='gray', lw=1)
axes[1].set_title('计算力矩控制:跟踪目标角'); axes[1].set_xlabel('t (s)'); axes[1].legend(); axes[1].grid(alpha=.3)
fig.tight_layout()
fig.savefig('images/math06_dyn.png', dpi=110, bbox_inches='tight'); plt.close(fig)
plt.show()
print('已保存 images/math06_dyn.png')
自由摆(τ=0):2 秒内机械能最大漂移 = 7.38e-05(应接近 0 → 动力学方程自洽 ✓)
计算力矩控制:目标 θ = [ 1.2 -0.8] → 3 秒后 θ = [ 1.2 -0.8] 误差 = 0.0 控制收敛 ✓
已保存 images/math06_dyn.png
面试考点与自测¶
🎤 高频追问速答(背诵骨架)¶
- 旋转矩阵为什么正交:刚体旋转保长度保夹角 → 列向量是单位正交基 → $\boldsymbol{R}^\top\boldsymbol{R}=\boldsymbol{I}$;$\det=1$ 排除镜像。
- 正解 vs 反解:正解唯一(确定矩阵连乘);反解多解(肘上/肘下、腕翻转)、可能无解(超出工作空间);6 自由度最多 8 组解析解。
- DH 四参数:$\theta$ 绕 z 转(常是关节变量)、$d$ 沿 z 移、$a$ 沿 x 移、$\alpha$ 绕 x 转;相邻连杆变换 $\boldsymbol{A}_i$ 右乘连乘得末端位姿。
- 奇异位形:$\det(\boldsymbol{J}^\top\boldsymbol{J})=0$,末端在某方向失去自由度,附近条件数爆炸 → 数值 IK 需大关节速度。
- $\boldsymbol{\tau}=\boldsymbol{J}^\top\boldsymbol{F}$:虚功原理(能量守恒)导出,是力控/阻抗控制的基础。
- 动力学三项:惯性 $\boldsymbol{M}\ddot{\boldsymbol{q}}$、科氏离心 $\boldsymbol{C}\dot{\boldsymbol{q}}$(高速互相拉扯)、重力 $\boldsymbol{G}$;计算力矩控制 = 模型前馈 + PD 线性化误差方程。
✅ 自测清单(学完打勾)¶
- 能写出绕 z 轴旋转矩阵,并验证 $\boldsymbol{R}^\top\boldsymbol{R}=\boldsymbol{I}$
- 能用 DH 四参数描述相邻连杆变换,讲清每个参数的运动含义
- 能徒手推导 2 连杆平面臂的正运动学公式($x,y$ 关于 $\theta_1+\theta_2$)
- 能写出 2 连杆解析 IK 的 $\theta_2=\pm\arccos\left(\frac{l_1^2+l_2^2-x^2-y^2}{2l_1l_2}\right)$ 并解释双解
- 能解释奇异位形的数学条件(雅可比降秩)与工程后果
- 能写出 $\boldsymbol{M}\ddot{\boldsymbol{q}}+\boldsymbol{C}\dot{\boldsymbol{q}}+\boldsymbol{G}=\boldsymbol{\tau}$ 并说出三项的物理含义
- 能讲清计算力矩控制为什么能把非线性系统变成二阶线性误差方程
🧭 本模块回顾:00-高数 → 01-线性代数 → 02-概率统计 → 03-信息论 → 04-最优化 → 05-面试八股 → 06-机器人学基础。 机器人学把前六篇的矩阵(旋转/齐次变换)、微积分(速度/加速度)、优化(数值 IK)串成了一条完整的应用链: 位姿描述 → 正解 → 反解 → 雅可比/奇异 → 动力学/控制。 若后续要深入,可继续补:四元数与姿态插值、冗余机械臂规划、阻抗/导纳控制、移动机器人里程计与位姿图优化(SLAM 基础)。