从受力图到状态方程：为什么要建动力学模型

从通用建模走向后续综合实验

后续综合实验将从一艘具体八推进器 ROV 出发，做观测器、反馈和仿真实验。这里先回答：换一台机械臂或轮式机器人，怎样自己得到模型，而不是只会代入别人给出的矩阵？ 学完可继续到八推模型实验核查每个参数从哪里来。

运动学回答“关节如何转动，末端在哪里”；动力学回答“施加多大的力，运动怎样变化”。“位置指令”也不是天然的物理输入：底层若已实现位置闭环，上层看到的对象可能是一个有延迟的伺服系统；若直接控制电机电压，就必须考虑电流和反电动势。建模第一步是确定控制接口边界。

1. 一张受力图先于一张矩阵

从一个沿水平导轨运动的滑块开始。设位置为 $p$（m），速度 $v=\dot p$（m/s），质量 $m$（kg），驱动力 $u$（N），线性阻力系数 $b$（N·s/m），弹簧刚度 $k$（N/m），额外外力 $d$（N）。正方向统一向右。

弹簧力为 $-kp$，阻力为 $-bv$，因为它们分别反抗正位移和正速度。牛顿定律给出

这一行每一项都是 N。若把 $u$ 误写成 PWM 比例，右边单位就不成立；应该再引入“比例指令到推力”的执行器模型。

模型不是装饰。已知当前 $p,v$ 和未来输入 $u$，它可预测加速度，再推进速度和位置；反过来，给定期望加速度，也可以计算应施加的力。

2. 为什么状态必须含速度

两辆车在同一位置，一辆静止，一辆以 $1$ m/s 前进；相同未来输入产生不同未来位置。因此位置不是足够的记忆。定义

$A$ 的第一行表达 $\dot p=v$，第二行表达受力产生加速度。矩阵元素的单位不必相同：$-k/m$ 为 $\mathrm{s^{-2}}$，$-b/m$ 为 $\mathrm{s^{-1}}$，它们分别乘 m 和 m/s，结果均为 m/s²。

如果只装位置传感器，测量为

$y$ 是测到的位置，$n$ 是测量误差。状态是为了预测选的，测量是硬件实际提供的，它们不必同维。 这正是观测器与卡尔曼滤波存在的原因。动力学参数 $m,b,k$ 通常不放进基础状态；只有确实需要在线估计且有辨识条件时，才增广它们。

3. 从牛顿到广义坐标

机械臂若每个连杆都用笛卡尔坐标描述，还要添加很多铰链约束。用关节角描述则更紧凑。能够唯一描述允许构型的一组独立变量称为广义坐标 $q\in\mathbb R^n$。旋转关节分量用 rad，移动关节分量用 m。

末端位置为 $r(q)$。对小变化做一阶展开：

$\delta q$ 是满足约束的虚位移，并不是实际经过某个时间后的位移。外力 $f$ 的虚功为

因此等效广义力是 $Q=J_v^Tf$。若还存在外力矩 $\mu$，加上角速度雅可比 $J_\omega$ 后，$Q=J_v^Tf+J_\omega^T\mu$。这个推导说明了为什么是雅可比转置而不是逆矩阵：它要保持功率/虚功，不是在解位置。

对转动关节 $Q_i$ 是 N·m，对移动关节是 N。混合关节模型中的 $Q$ 并非所有分量同单位，但 $Q^T\dot q$ 必须是 W。

4. 能量法为什么可以替代逐个写内力

定义动能 $T(q,\dot q)$、势能 $U(q)$ 和拉格朗日量 $L=T-U$。设已用独立坐标消去完整约束，并且约束理想、内力不对允许虚位移做功。对作用量的变分有

因为 $\delta\dot q=d(\delta q)/dt$，第二项的分部积分具体为

端点固定意味着 $\delta q(t_0)=\delta q(t_1)=0$，所以边界项消失。加上非保守力的虚功 $\int Q^T\delta q\,dt$，剩余积分中乘在 $\delta q$ 前的系数若在任一小区间非零，便可选一个同向、只在该区间非零的变分，使积分不为零。因此要求所有允许变分都给零，迫使该系数逐时为零，得到

这就是 Euler–Lagrange 方程。它不是把牛顿定律推翻了，而是用能量把约束内力消去。非完整滚动约束不能直接当作已积分的几何关系塞进坐标消元；后文会用约束投影。

用滑块把每一步算出来

$T=\tfrac12m\dot p^2$，$U=\tfrac12kp^2$，所以 $L=\tfrac12m\dot p^2-\tfrac12kp^2$。

• 对速度求偏导：$\partial L/\partial\dot p=m\dot p$，即动量。
• 再对时间求导：$d(m\dot p)/dt=m\ddot p$，这里假设质量恒定。
• 对位置求偏导：$\partial L/\partial p=-kp$。
• 非保守广义力：$Q=u-b\dot p+d$。

代回就得到 $m\ddot p+kp=u-b\dot p+d$，与受力图完全相同。先用简单对象做两种推导的交叉检查，能尽早发现符号错误。

5. 正向动力学与逆向动力学

对一般刚体机械系统可整理成

$M$ 是惯性矩阵；$C\dot q$ 是速度乘积项；$g=\partial U/\partial q$；$f$ 是摩擦等非保守阻力；$B_u$ 把执行器输入映成广义力。$B_u$ 不一定方阵：欠驱动系统输入少，冗余驱动系统输入多。

正向动力学已知力求加速度，要解线性方程

代码应优先解方程而不是显式计算 $M^{-1}$。逆向动力学已知期望加速度求所需广义力；若执行器能力不足，这份力只是需求，并不一定能实现。

6. 从 CAD、实验和日志得到参数

几何尺寸、质心、惯量可先由 CAD 提供；质量可称重；阻尼、摩擦、执行器效率则往往需要实验。参数辨识不是“把所有未知数丢进优化器”。以无弹簧轴为例：

这里 $u$ 表示已知施加的力。每个采样点构成一行回归：

把多行叠成 $Y=\Phi\vartheta$，最小二乘是在求令 $\|Y-\Phi\vartheta\|^2$ 最小的参数。仅匀速单方向实验无法同时辨认所有量：加速度列为零，质量没有信息；恒定速度时阻力列和常量列可能相关。需要不同速度、加减速、双向激励，且不能超越设备安全范围。

数值微分会放大速度噪声，且误差会进入回归自变量，引起偏差；可使用平滑、积分形式或输出误差法。训练数据拟合好不等于模型正确，要用没有参与辨识的轨迹检查预测。若推力只有未知比例的 PWM，质量与推力系数还可能存在尺度歧义，不能声称单靠这份日志得到绝对质量。

7. 模型最少要过哪些检查

1. 量纲：每一行单位一致。
2. 极限情况：无输入无阻力时能量不凭空增减；静止时速度乘积项为零。
3. 对称性：正常独立机械坐标的 $M=M^T\succ0$；不要靠强行对称化掩盖推导错误。
4. 能量：阻尼应耗能，而不是在正负速度中一侧发散。
5. 工作点：$f(x_0,u_0)=0$ 才是平衡点；静态重力补偿可能使 $u_0\ne0$。
6. 留出验证：预测位置/速度/力矩残差，检查残差是否随速度、姿态、温度系统性变化。
7. 接口：输入到底是力、转速还是归一化指令，输出测量是否延迟。

8. 手写练习：把“预期加速度”换成力

给定 $m=2$ kg、$b=3$ N·s/m、$k=4$ N/m、$p=0.5$ m、$v=-0.2$ m/s，期望加速度 $a_d=1$ m/s²。先忽略外扰，需要多大输入？不要先看答案，分别从受力图和能量方程算一次。

#include <cassert>
#include <cmath>
double acceleration(double p, double v, double force,
                    double m, double b, double k) {
    assert(m > 0.0);
    return (force - b*v - k*p) / m;
}
int main() {
    assert(std::abs(acceleration(0.5,-0.2,3.4,2,3,4)-1.0)<1e-12);
    assert(std::abs(acceleration(0,0,0,2,3,4))<1e-12);
}

来源与继续学习

本文滑块、回归及逐步变分为独立教学推导。机器人动力学的能量/牛顿–欧拉两条路线可对照作者官方材料：Modern Robotics 8.1。下一课把这一过程完整用于两连杆机械臂。