机器人运动学与动力学基础

本文把运动学/动力学从”背公式”升级为”能推导、能写代码”。核心思路：DH 参数为什么是 4 个 → 正解就是矩阵连乘 → 逆解的本质是解方程（解析 or 数值）→ 雅可比连接速度层与力层 → 拉格朗日从能量导出 M/C/G → 麦轮底盘从辊子几何推导速度分解矩阵。全程结合 RoboMaster 云台/麦轮底盘和减速器课程设计的实战。

DH 参数：为什么恰好是 4 个参数

两个相邻连杆坐标系之间的变换有 6 个自由度（3 旋转 + 3 平移），但 DH 约定把关节轴线本身作为约束，把问题降维：

• 连杆 i 的坐标系 {i} 固连在关节 i+1 上，Z 轴沿关节轴线
• 相邻坐标系 {i-1} → {i} 的变换被拆成 4 次基本变换：绕 Z 转 θ、沿 Z 移 d、沿 X 移 a、绕 X 转 α

$$A_i = \operatorname{Rot}(z, \theta_i) \cdot \operatorname{Trans}(z, d_i) \cdot \operatorname{Trans}(x, a_i) \cdot \operatorname{Rot}(x, \alpha_i)$$

为什么能少掉 2 个自由度？因为旋转关节的 θ 是变量（1 个自由度），其余 3 个参数由机构几何固定——4 个参数里 1 个变量 + 3 个常量，正好描述”旋转副连接的两个刚体”的全部相对位姿。平动关节则 d 为变量。

标准 DH vs 改进 DH——最容易混的坑

	标准 DH (SDH)	改进 DH (MDH)
坐标系挂载	{i} 在连杆 i 的末端（关节 i+1 上）	{i} 在连杆 i 的起始端（关节 i 上）
变换顺序	Rot(z,θ) Trans(z,d) Trans(x,a) Rot(x,α)	Rot(x,α) Trans(x,a) Rot(z,θ) Trans(z,d)
谁在用	教材经典	URDF / KDL / RoboMaster rm_common 实际用 MDH

实战结论：写代码一律用改进 DH（URDF 的 origin 就是按 MDH 语义写的）。如果你对着教材的 SDH 参数表去填 URDF，末端位姿会差一个”连杆自身偏移”，云台角度解算直接偏几度。判断标准很简单：URDF 里 <joint> 的 origin 描述的是子连杆坐标系相对父连杆坐标系的位姿，这正是 MDH 的语义。

正运动学：矩阵连乘

$$T_n^0 = A_1 A_2 \cdots A_n$$

正解没有技巧，就是连乘。二连杆平面机械臂（MDH：α=0, a=L, d=0）：

import sympy as sp
th1, th2, L1, L2 = sp.symbols('th1 th2 L1 L2')

def dh_a(a, alpha, d, theta):
    """改进 DH 的 A 矩阵"""
    ct, st = sp.cos(theta), sp.sin(theta)
    ca, sa = sp.cos(alpha), sp.sin(alpha)
    return sp.Matrix([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,      sa,     ca,    d],
        [0,       0,      0,    1]])

T1 = dh_a(L1, 0, 0, th1)
T2 = dh_a(L2, 0, 0, th2)
T = sp.simplify(T1 * T2)   # 末端位姿

# 末端位置（期望结果：L1*cos(th1)+L2*cos(th1+th2), 同理 sin）
print(sp.simplify(T[0, 3]), sp.simplify(T[1, 3]))

输出正是平面几何直觉：L1*cos(th1) + L2*cos(th1 + th2)。这就是验证 DH 参数表是否正确的最快方法——用 SymPy 展开，看结果是否符合几何直觉。符号推导能省掉大量手算错误（详见《sympy-robot-dynamics》篇）。

逆运动学：解方程，两条路

给末端位姿求关节角。解析法：二连杆几何法（余弦定理）；数值法：雅可比迭代。工程上两者都常用——机械臂离线规划用解析解，在线自适应用数值解。

解析法：云台角度解算（RoboMaster 实战）

云台是 2 自由度（yaw + pitch），目标是让枪口指向装甲板。给定装甲板在云台系下的位置 $(x, y, z)$：

// 只考虑几何对准（忽略弹道下垂时的补偿项）
float yaw   = atan2f(x, z);            // 水平偏角
float pitch = atan2f(-y, sqrtf(x*x + z*z));  // 垂直俯角（y 向下为正）

为什么用 atan2 而不是 atan？ atan(y/x) 在 x<0 时丢失象限信息，可能直接反 180°——云台会”背对”目标。atan2f(y, x) 返回 $(-\pi, \pi]$ 四象限角，这是所有角度解算的第一原则。坑：pitch 的符号取决于坐标系定义（图像系 y 向下 vs 世界系 y 向上），写错符号云台会”低头打天”。真实弹道还要叠加：$\text{pitch}{real} = \text{pitch}{geo} + \text{gravity_drop}(dist)$，距离越远补偿越大，一般用实测表格或抛物线模型标定。

数值法：阻尼最小二乘（DLS）

雅可比迭代的本质：末端速度 $\dot{x} = J\dot{q}$，把”末端位姿误差”映射成”关节角增量”。但 J 可能奇异/非方阵，直接求逆数值不稳定，工程标准做法是阻尼最小二乘：

$$\Delta q = J^T (J J^T + \lambda^2 I)^{-1} \Delta x$$

$\lambda$（阻尼系数）保证 $JJ^T$ 即使奇异也可逆。迭代直到 $|\Delta x|$ 小于阈值：

// 数值逆解（6-DOF 通用骨架）
Eigen::VectorXd solveIK(const Eigen::Matrix4d& T_des,
                        Eigen::VectorXd q0, double lambda = 0.01) {
  Eigen::VectorXd q = q0;
  for (int iter = 0; iter < 100; ++iter) {
    Eigen::Matrix4d T = fk(q);                    // 正解
    Eigen::VectorXd dx = se3Error(T_des, T);      // 位姿误差(平移+旋转向量)
    if (dx.norm() < 1e-6) break;
    Eigen::MatrixXd J = jacobian(q);              // 几何雅可比 6xn
    Eigen::MatrixXd JJt = J * J.transpose();
    Eigen::MatrixXd Jinv = J.transpose() *
        (JJt + lambda * lambda * Eigen::MatrixXd::Identity(6, 6)).inverse();
    q += Jinv * dx;                               // 关节限位后钳制
  }
  return q;
}

多解选择：解析/数值解都可能多解。选择策略按优先级：① 关节限位内 ② 与当前关节角距离最小（避免跳变）③ 避障/避奇异。云台上就是”取离当前角度最近的那个解”，防止 180° 翻转。

雅可比矩阵：速度层与力层的桥梁

几何法求雅可比

对转动关节 i，末端线速度由”关节 i 的角速度叉乘末端相对关节 i 的位置”贡献，角速度直接是 $z_{i-1}$ 方向：

$$J_i = \begin{bmatrix} z_{i-1} \times (p_n - p_{i-1}) \ z_{i-1} \end{bmatrix}$$

雅可比的三个身份（同一个 J 贯穿全篇）：

1. 速度映射：$\dot{x} = J(q)\dot{q}$——上面数值逆解就用它
2. 力映射：$\tau = J^T F$——末端力/力矩折算到关节力矩，云台”输出力矩上限保护”就是先算 $F$ 再乘 $J^T$ 判断是否超限
3. 奇异位形判据：$\det(J) = 0$ 时该方向无法产生速度（如二连杆完全伸直时无法沿臂方向移动），且 $|J^{-1}| \to \infty$ 导致关节速度爆炸——这也是 DLS 里 λ 存在的意义

速度级逆解必须闭环

用 $\dot{q} = J^{-1}\dot{x}_{des}$ 积分得到 q 会漂移（数值误差累积），工程上必须加反馈修正：

$$\dot{q} = J^{-1}\left(\dot{x}{des} + K_p (x{des} - x_{cur})\right)$$

这跟 PID 文章里”积分项消除稳态误差”是同一个思想：位置误差通过 Kp 反馈进速度指令，保证长期不漂。RoboMaster 底盘”速度解算 → 轮速 PID”链路里，如果底盘实际速度与期望有偏差（打滑），最终定位偏差就会累积——所以底盘控制也要加里程计位置闭环，而不是纯开环积分。

拉格朗日动力学：为什么从能量出发

牛顿-欧拉法要对每个连杆做受力分析，约束反力一大堆；拉格朗日法只需要标量能量，约束被广义坐标自动吸收：

$$L = K - P, \qquad \frac{d}{dt}\frac{\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = \tau_i$$

展开后得到标准形式：

$$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = \tau$$

• $M(q)$：惯性矩阵，只与位形有关，对称正定
• $C(q,\dot{q})\dot{q}$：科氏力 + 离心力，是 $\dot{q}$ 的二次型（$C_{ij}$ 含 $\dot{q}_k$ 项）
• $G(q)$：重力项，只与位形有关

为什么 C 项是”速度的二次型”？

动能 $K = \frac{1}{2}\dot{q}^T M(q)\dot{q}$ 对 $q$ 求导产生 $\frac{\partial M}{\partial q}\dot{q}^2$ 型项——两个速度相乘。物理直觉：**科氏力是”转动参考系里的虚拟力”**，云台 yaw 快速转动时，pitch 轴会感受到与”yaw 角速度 × pitch 角速度”成正比的耦合力矩。所以”为什么高速回转时云台会抖/偏”——不是电机不行，是科氏力矩没补偿。

二连杆 M/C/G 的 SymPy 推导

import sympy as sp
q1, q2, dq1, dq2 = sp.symbols('q1 q2 dq1 dq2')
m1, m2, L1, L2, g = sp.symbols('m1 m2 L1 L2 g')

# 质心位置（连杆质心在杆中点）
p1 = sp.Matrix([L1/2*sp.cos(q1), L1/2*sp.sin(q1)])
p2 = sp.Matrix([L1*sp.cos(q1) + L2/2*sp.cos(q1+q2),
                L1*sp.sin(q1) + L2/2*sp.sin(q1+q2)])
dq = sp.Matrix([dq1, dq2])

# 动能（忽略转动惯量项简化演示）：K = 1/2 * m * v^2
def kin(pos, m):
    v = pos.jacobian([q1, q2]) * dq        # 链式法则求速度
    return sp.Rational(1, 2) * m * (v.dot(v))

K = sp.simplify(kin(p1, m1) + kin(p2, m2))
P = g * (m1 * p1[1] + m2 * p2[1])          # 势能

# 拉格朗日方程逐项展开，得到 M(q) 矩阵的 4 个元素
M11 = sp.simplify(sp.diff(K, dq1, 2))      # d²K/ddq1²
M12 = sp.simplify(sp.diff(K, dq1, dq2))
M22 = sp.simplify(sp.diff(K, dq2, 2))
print(M11)  # m2*L1**2 + m2*L1*L2*cos(q2) + (m1*L1**2)/4 + (m2*L2**2)/4

注意：$M_{12}$ 含 $\cos(q_2)$——惯性耦合随位形变化，这正是”机械臂末端负载不同，动态特性完全不同”的数学根源，也是”固定增益 PID 无法在所有位形下都好用”的原因之一。

计算力矩法（前馈 + 反馈）

动力学模型不只是用来分析的，直接用于控制——计算力矩法：

$$\tau = M(q)\left(\ddot{q}_d + K_d \dot{e} + K_p e\right) + C(q,\dot{q})\dot{q} + G(q)$$

模型项（M/C/G）做前馈抵消非线性，反馈项（PD）只负责修正模型误差，系统被”线性化”成解耦的二阶系统，各关节独立整定 $K_p, K_d$。工程意义：云台/机械臂用动力学前馈后，跟踪误差从几十 mrad 降到几 mrad。代价是模型要准——惯量参数靠辨识（激励轨迹 + 最小二乘拟合），这就是”参数辨识”的用武之地。

减速器课程设计：传动比怎么进动力学

电机轴动力学折算到负载轴（减速比 $n$，效率 $\eta$）：

• 惯量折算：$J_{eff} = J_m + \dfrac{J_l}{n^2}$（电机侧看）
• 力矩折算：$\tau_l = n \cdot \eta \cdot \tau_m$

为什么惯量除以 $n^2$？ 因为动能守恒：$\frac{1}{2}J_m\omega_m^2 = \frac{1}{2}J_{eff}\omega_l^2$，而 $\omega_m = n\omega_l$，代入即得 $J_{eff} = J_m + J_l/n^2$。结论：减速比越大，负载惯量折算到电机侧越小，电机越”轻松”——但转速降低、精度提高（位置分辨率乘 n）。选减速比本质是在”响应速度 vs 带载能力 vs 精度”之间权衡。RoboMaster 的 M3508 电机 + 减速器组合，官方给出的减速比 19:1 就是按”云台/底盘惯量匹配”标定的——直接用就行，自己改减速比必须重算 $J_{eff}$ 和速度环带宽。

麦克纳姆轮：从辊子几何推导速度分解

麦轮能”斜着走”的秘密是轮缘上 45° 斜装的辊子：轮子转动时，与地面的接触速度沿辊子轴线方向（被动自由滚动），沿辊子垂直方向则被锁死（辊子轴是刚性连接）。

推导单个轮子的约束方程

设轮 i 的辊子方向（轮系下）为 $\hat{r}_i$，轮子角速度 $\omega_i$ 产生的接触点速度 $v_c = R\omega_i \cdot \hat{r}_i$。底盘运动 $(v_x, v_y, \omega)$ 在轮 i 处产生的速度（轮系坐标，轮子安装角 $\theta_i$，到中心距离 $L$）：

$$v_c = \begin{bmatrix} \cos\theta_i & \sin\theta_i \ -\sin\theta_i & \cos\theta_i \end{bmatrix} \begin{bmatrix} v_x \ v_y \end{bmatrix} + L\omega \cdot \hat{t}_i$$

投影到辊子方向 $\hat{r}_i = [\cos\gamma_i, \sin\gamma_i]^T$（$\gamma_i = \pm 45°$，由安装方向决定符号），整理出单轮约束：

$$R\omega_i = \cos\gamma_i\left(\cos\theta_i v_x + \sin\theta_i v_y\right) + \sin\gamma_i\left(-\sin\theta_i v_x + \cos\theta_i v_y\right) + L\omega$$

为什么效率低？ 辊子与地面是滚动接触，但辊子轴线与轮子轴线不垂直——轮子驱动方向与辊子自由方向之间有 45° 夹角，**驱动功率的一部分被辊子滚动”泄掉”**（理论上效率上限约 $\cos 45° = 70.7%$，实际含摩擦更低）。这就是对比表里”麦轮效率较低”的数学来源。

逆运动学矩阵（RoboMaster 底盘标准 X 形布置）

四轮 X 形布置（轮 1/3 同向、轮 2/4 反向），$\theta_i$ 取各轮安装角，整理后就是经典公式：

$$\begin{bmatrix} \omega_1 \ \omega_2 \ \omega_3 \ \omega_4 \end{bmatrix} = \frac{1}{R} \begin{bmatrix}
1 & -1 & -(a+b) \
1 & 1 & (a+b) \
1 & 1 & -(a+b) \
1 & -1 & (a+b)
\end{bmatrix} \begin{bmatrix} v_x \ v_y \ \omega \end{bmatrix}$$

符号陷阱：矩阵里 $a+b$ 前的正负号完全取决于轮子编号和辊子朝向约定。RoboMaster 官方步兵底盘（RM 开源代码）的编号/朝向与网上教程经常不同——**抄公式前先做”单轮验证”**：令 $v_y=\omega=0$，$v_x>0$，看四个轮子是否都正转（前进）；再令 $v_x=\omega=0$，$v_y>0$，验证横移时对角线轮反转。符号错了底盘会”原地打转”而不是平移，这是麦轮调试第一大坑。

底盘速度分解必须做 yaw 补偿

遥控器给的 $v_x, v_y$ 通常是底盘系（车头方向），但裁判系统/导航给的是世界系。两者差一个 yaw 旋转：

$$\begin{bmatrix} v_x^{body} \ v_y^{body} \end{bmatrix} = \begin{bmatrix} \cos\psi & \sin\psi \ -\sin\psi & \cos\psi \end{bmatrix} \begin{bmatrix} v_x^{world} \ v_y^{world} \end{bmatrix}$$

坑：忘记 yaw 补偿的表现是”遥控器推前进，车斜着走”；yaw 方向（IMU 的 z 轴）标反则表现为”推前进车横着走”。调试顺序：先单轮正反转 → 再平移四向 → 最后自转，每步都验证，别直接上闭环。

麦轮打滑与里程计漂移

麦轮辊子与地面的接触本质是滑动+滚动混合，急加速/急停时打滑明显，纯轮速积分（$v = \omega R$）的里程计会漂。工程处理：

1. 加速度斜坡限制（S 曲线），降低峰值打滑
2. 里程计融合 IMU yaw：位置用轮速积分，航向用陀螺仪（陀螺不漂位置、但 yaw 积分也漂——比赛半场就要重对一次）
3. 视觉/裁判系统绝对位置修正（回环检测思想，见 SLAM 篇）

机器人常用底盘对比

类型	自由度	优点	缺点
差速轮	2	结构简单，成本低	不能横向移动
麦克纳姆轮	3	全向移动灵活	效率低（辊子 45° 损耗），对地面要求高
全向轮	3	全向移动，效率高于麦轮	承重能力有限，辊子易磨损
舵轮	3	承重大，越障好	结构复杂，需阿克曼转向协调（各轮转向中心交于一点，否则产生内应力损坏机构）
履带	2	越障能力强	转向磨损大，速度慢

选型建议：RoboMaster 步兵/英雄用麦轮（全向 + 场地平整）；室外巡检/送餐用差速或舵轮；全向轮适合轻载高精度平台（如视觉转台）。没有”最好”的底盘，只有”最匹配场地和任务”的底盘。

踩坑清单

1. SDH/MDH 混用：URDF 和 rm_common 是 MDH 语义，教材 SDH 参数表不能直接填——先用 SymPy 正解验证
2. atan 丢象限：所有角度解算用 atan2，且先确认坐标系 y 轴方向（图像系 vs 世界系）
3. 速度级逆解不闭环：$\dot q = J^{-1}\dot x$ 纯积分必漂，加 $K_p$ 位置反馈
4. 麦轮符号约定：抄矩阵前做单轮验证，编号/辊子朝向不同矩阵不同
5. 忘做 yaw 补偿：底盘系 vs 世界系混用导致”斜着走”，先开环四向验证再闭环
6. 固定增益打天下：$M(q)$ 随位形变化，重负载/大范围运动必须动力学前馈或增益调度
7. 减速比乱改：改减速比必须重算 $J_{eff}$ 与速度环带宽，否则电机”带不动”或”飞车”