【机器人知识体系搭建】11_动力学(上):拉格朗日建模
🦉 猫头鹰都在卷机械臂逆解了,你确定不关注一下?
第 11 讲 · 动力学(上):用拉格朗日法算出"每个关节得出多少力矩"
一句话核心:运动学只管"位姿对不对",动力学管的是"要让机械臂按某个加速度动起来,每个关节电机到底该出多大力矩"。拉格朗日法是算它的万能套路:先算能量,再对能量求导。
- 本讲目标:掌握 L = T − U 与拉格朗日方程;读懂标准形式 B q̈ + C q̇ + F_v q̇ + g = τ 每一项的物理意义;能独立跟完两连杆平面臂的完整推导。
一、为什么需要这个
小讲 1:为什么运动学不够用,动力学要算的是力矩
1.1 为什么运动学不够用
你已经会正/逆运动学和雅可比了:给定关节角,能算出末端在哪;给定末端速度,能算出关节速度。但这些都不含有"力"和"质量"。
生活比喻:同一个动作——把手臂平举。
- 空着手做:很轻松;
- 手里举 10 kg 哑铃:立刻沉;
- 慢慢举 vs 猛地甩起来:后者费力得多,而且甩到一半手腕还会被"带偏"(对应"加速度项 + 科氏/离心项")。
运动学只描述"手臂举到哪个角度",动力学描述"肌肉要出多少力"。机器人没有本体感觉,全靠动力学模型算。
1.2 动力学拿来干什么:三个用途
开篇就点明动力学的三个用途:(1)仿真——不用真机就能测试控制策略和规划方法;(2)结构设计——算出典型运动需要多大的力和力矩,才能选关节、传动和驱动器;(3)控制算法设计。
1.3 拉格朗日法的特点:概念简单、与坐标系无关
本讲讲第一种方法:拉格朗日法Lagrange formulation。它的特点是概念简单、步骤系统,而且与参考坐标系的选取无关——只要选好一组能描述连杆位置的广义坐标 q 就行。
停下来想一想
空手平举、举 10 kg 哑铃、猛地甩起来——这三种情况分别对应标准方程里的哪一项(惯性项 / 重力项 / 科氏离心项)?
二、核心概念
小讲 2:拉格朗日三步套路与刚体动能的两项拆分
2.1 拉格朗日法的三步套路
提示
比喻:记账。你要知道"这个月能剩多少钱",不用逐笔追踪每一次消费,只要知道总收入 − 总支出。拉格朗日法也一样:它不管每个零件之间互相推来推去的内部力(那些力成对抵消),只盯住两个标量——动能 T 和势能 U。
- 算总动能 T:每个连杆 + 每个电机转子,平动 + 转动,全部加起来。
- 算总势能 U:只有重力(假设刚性连杆),等于 −Σ mᵢ g₀ᵀ pᵢ。
- 对 L = T − U 求导:套拉格朗日方程 d/dt(∂L/∂q̇) − ∂L/∂q = ξ。
2.2 一个刚体的动能怎么算
把连杆切成无数微元,积分之后发现:交叉项(平动 × 转动)恰好为 0(因为质心的定义),最后只剩下两项。
Tℓi=12⋅mℓi⋅p˙ℓi⊤p˙ℓi+12⋅ωi⊤⋅RiIℓiiRi⊤⋅ωi T_{\ell i} = \tfrac{1}{2} \cdot m_{\ell i} \cdot \dot{p}^{\top}_{\ell i} \dot{p}_{\ell i} + \tfrac{1}{2} \cdot \omega_{i}^{\top} \cdot R_{i} I^{i}_{\ell i} R_{i}^{\top} \cdot \omega_{i} Tℓi=21⋅mℓi⋅p˙ℓi⊤p˙ℓi+21⋅ωi⊤⋅RiIℓiiRi⊤⋅ωi
| 符号 | 含义 | 大白话 |
|---|---|---|
| m_ℓi | 连杆 i 的质量 | 这根杆多重 |
| ṗ_ℓi | 连杆 i 质心的线速度 | 杆的重心跑得多快 |
| ωᵢ | 连杆 i 的角速度 | 杆转得多快 |
| Iⁱ_ℓi | 连杆 i 相对于质心的惯性张量(在连杆自身坐标系里是常数矩阵) | 杆绕自己重心转起来有多"沉" |
| R_i | 从连杆 i 坐标系到基坐标系的旋转矩阵 | Iⁱ_ℓi = R_i Iⁱ_ℓi R_iᵀ 是把"常量惯量"转到基坐标系 |
直觉上这是在说什么:一个刚体的动能 =重心平移的动能+绕重心自转的动能,两项直接相加,中间没有交叉项。这一条让整个推导瞬间变简单。
注意:惯性张量写在基坐标系里是随构型变化的(因为杆的位置变),但写回连杆自身坐标系就是常数——所以坚持把 ω 也表示在连杆系里。
小讲 3:用雅可比换成关节速度,得到惯性矩阵 B(q) 与势能 U(q)
2.3 用雅可比把速度换成关节速度
上面那式子里的 ṗ_ℓi 和 ωᵢ 都是"直角坐标速度",而我们想要的是关节变量 q。用几何法求雅可比,对中间连杆也同样成立:
p˙ℓi=JP(li)⋅q˙ωi=JO(li)⋅q˙JP的第j列:zj−1(棱柱/移动关节)zj−1×(pℓi−pj−1)(旋转关节)JO的第j列:0(移动关节)zj−1(旋转关节) \begin{aligned} \dot{p}_{\ell i} = J^{(li)}_{P} \cdot \dot{q} \omega_{i} = J^{(li)}_{O} \cdot \dot{q} \\ J_{P} \text{的第} j \text{列:} z_{j-1} \text{(棱柱}/\text{移动关节)} \\ z_{j-1} \times (p_{\ell i} - p_{j-1}) \text{(旋转关节)} \\ J_{O} \text{的第} j \text{列:} 0 \text{(移动关节)} \\ z_{j-1} \text{(旋转关节)} \end{aligned} p˙ℓi=JP(li)⋅q˙ωi=JO(li)⋅q˙JP的第j列:zj−1(棱柱/移动关节)zj−1×(pℓi−pj−1)(旋转关节)JO的第j列:0(移动关节)zj−1(旋转关节)
关键点:J_P 和 J_O 的列,只到第 i 列为止,后面全是 0。因为第 i 根杆只会被它前面的关节带动,后面的关节动不影响它。这就是"开链结构"带来的简化。
代回去,把所有连杆和所有转子(转子动能形式完全类似,额外有个减速比 k_ri 和转子轴单位向量 z_mi)加起来,总动能变成一个漂亮的二次型:
T=12⋅q˙⊤B(q)q˙B(q)=∑i[mℓiJP(li)⊤JP(li)+JO(li)⊤RiIℓiiRi⊤JO(li)+mmiJP(mi)⊤JP(mi)+JO(mi)⊤RmiImimiRmi⊤JO(mi)] \begin{aligned} T = \tfrac{1}{2} \cdot \dot{q}^{\top} B(q) \dot{q} \\ B(q) = \sum_{i} [ m_{\ell i} J^{(li)\top}_{P} J^{(li)}_{P} + J^{(li)\top}_{O} R_{i} I^{i}_{\ell i} R_{i}^{\top} J^{(li)}_{O} \\ {}+ m_{mi} J^{(mi)\top}_{P} J^{(mi)}_{P} + J^{(mi)\top}_{O} R_{mi} I^{mi}_{mi} R_{mi}^{\top} J^{(mi)}_{O} ] \end{aligned} T=21⋅q˙⊤B(q)q˙B(q)=i∑[mℓiJP(li)⊤JP(li)+JO(li)⊤RiIℓiiRi⊤JO(li)+mmiJP(mi)⊤JP(mi)+JO(mi)⊤RmiImimiRmi⊤JO(mi)]
B(q) 惯性矩阵inertia matrix,n×n。明确列出它的三条性质:
- 对称(B = Bᵀ);
- 正定(任何非零 q̇ 都有 T = ½q̇ᵀBq̇ > 0 —— 动起来了动能必然为正,这在物理上是显然的,在控制上意味着 B⁻¹ 一定存在);
- 一般随构型变化(所以写成 B(q) 而不是 B)。
大白话:B 就是"机械臂在当前这个姿势下的等效惯量"。手臂伸直时绕肩转动惯量大,缩回来就小——B 把这个直觉定量化了。
2.4 势能只跟位置有关
U=−∑i(mℓig0⊤pℓi+mmig0⊤pmi) U = - \sum_{i} ( m_{\ell i} g_{0}^{\top} p_{\ell i} + m_{mi} g_{0}^{\top} p_{mi} ) U=−i∑(mℓig0⊤pℓi+mmig0⊤pmi)
| 符号 | 含义 |
|---|---|
| g₀ | 基坐标系下的重力加速度向量,例如平面臂取 g₀ = [0 −g 0]ᵀ(y 轴朝上时重力沿 −y) |
| p_ℓi , p_mi | 连杆质心 / 转子质心在基坐标系下的位置 |
关键结论:U 通过 p_ℓi、p_mi 只依赖于关节位置q,跟关节速度 q̇ 无关。这一条在后面求导时能省掉一半工作量。
停下来想一想
第 i 根杆的雅可比 J_P 为什么从第 i+1 列开始全是 0?这反映了开链结构的什么性质?
三、关键公式
小讲 4:拉格朗日方程、标准形式五项与两个关键性质
3.1 拉格朗日方程
L(q,q˙)=T(q,q˙)−U(q)d/dt(∂L/∂q˙i)−∂L/∂qi=ξii=1,…,n紧凑写法:d/dt(∂L/∂q˙)⊤−(∂L/∂q)⊤=ξ \begin{aligned} L(q, \dot{q}) = T(q, \dot{q}) - U(q) \\ d/dt ( \partial L / \partial \dot{q}_{i} ) - \partial L / \partial q_{i} = \xi_{i} i = 1, \dots , n \\ \text{紧凑写法:} d/dt ( \partial L/ \partial \dot{q} )^{\top} - ( \partial L/ \partial q )^{\top} = \xi \end{aligned} L(q,q˙)=T(q,q˙)−U(q)d/dt(∂L/∂q˙i)−∂L/∂qi=ξii=1,…,n紧凑写法:d/dt(∂L/∂q˙)⊤−(∂L/∂q)⊤=ξ
| 符号 | 含义 | 大白话 |
|---|---|---|
| L | 拉格朗日量 = 动能 − 势能 | 系统的"净活跃程度" |
| qᵢ | 第 i 个广义坐标(对开链机械臂就是关节变量) | 描述这个系统最少需要几个数 |
| ξᵢ | 与 qᵢ 对应的广义力 | 作用在这个关节上的"净力矩" |
| ξᵢ 的组成 | 关节驱动力矩 τᵢ − 关节摩擦力矩 − 末端接触力折算到关节的力矩 | 非保守力才能进 ξ(重力已算进 U 了) |
直觉上这是在说什么:这个方程是牛顿第二定律 F = ma 在"任意坐标"下的推广。∂L/∂q̇ 相当于"广义动量",对它求时间导就是"动量的变化率";∂L/∂q 相当于"回复力/保守力";两者相减等于外力。你已经学过拉氏变换和特征根,后面会看到这套方程线性化之后就是一个二阶系统。
3.2 标准形式(本讲最重要的一行)
B(q)q¨+C(q,q˙)q˙+Fvq˙+Fssgn(q˙)+g(q)=τ−J⊤(q)he B(q) \ddot{q} + C(q, \dot{q}) \dot{q} + F_{v} \dot{q} + F_{s} \operatorname{sgn}(\dot{q}) + g(q) = \tau - J^{\top}(q) h_{e} B(q)q¨+C(q,q˙)q˙+Fvq˙+Fssgn(q˙)+g(q)=τ−J⊤(q)he
| 项 | 名字 | 物理意义 | 生活中的对应 |
|---|---|---|---|
| B(q) q̈ | 惯性项 | 让关节产生加速度所需的力矩。B 的对角元 b_ii = 其他关节锁死时,关节 i 轴上看到的转动惯量;非对角元 b_ij = 关节 j 加速对关节 i 的牵连 | 把哑铃从静止猛地举起的那一下 |
| C(q, q̇) q̇ | 科氏 / 离心项 | 由速度引起的耦合力矩:h_ijj q̇_j² 是离心效应(关节 j 的速度甩到 i),h_ijk q̇_j q̇_k 是科氏效应。注意 h_iii = 0 | 甩水桶时手臂被带着走的横向力 |
| F_v q̇ | 粘性摩擦 | 转得越快阻力越大,F_v 是对角阵 | 手在蜂蜜里划动 |
| F_s sgn(q̇) | 库仑摩擦(静摩擦的简化模型) | 与速度大小无关、只与方向有关的阻力 | 推动重物前要克服的"粘滞" |
| g(q) | 重力项 | 在当前构型下,为"托住"机械臂本身所需的关节力矩 | 平举哑铃不动时手臂的酸痛 |
| Jᵀ(q) h_e | 末端接触力 | 末端与环境接触力 h_e 折算到关节上的力矩(形式与静力学 Jᵀ h 一致) | 手顶在墙上 |
直觉上这是在说什么:左边五项加起来 = 电机必须出的力矩 τ。换句话说:τ = (我要加速) + (我被甩) + (摩擦损耗) + (我要托住自己)。做控制时,这四部分里只有 B q̈ 是"我想要的运动",其余全是"需要被补偿掉的干扰"——这就是下一讲之后"计算力矩控制 / 动力学前馈"的出发点。
还给了分量写法:Σⱼ b_ij q̈_j + Σⱼ Σₖ h_ijk q̇_k q̇_j + g_i = ξᵢ,其中 h_ijk = ∂b_ij/∂q_k − ½ ∂b_jk/∂q_i。
3.3 两个重要性质
① Ḃ − 2C 是反对称矩阵skew-symmetry,这两式
如果把 C 的元素按"第一类克里斯托弗符号"来选:
cijk=12(∂bij/∂qk+∂bik/∂qj−∂bjk/∂qi)cij=∑kcijkq˙k \begin{aligned} c_{ijk} = \tfrac{1}{2} ( \partial b_{ij}/ \partial q_{k} + \partial b_{ik}/ \partial q_{j} - \partial b_{jk}/ \partial q_{i} ) \\ c_{ij} = \sum_{k} c_{ijk} \dot{q}_{k} \end{aligned} cijk=21(∂bij/∂qk+∂bik/∂qj−∂bjk/∂qi)cij=k∑cijkq˙k
那么 N(q, q̇) = Ḃ(q) − 2C(q, q̇) 满足Nᵀ = −N,即任意向量 w 都有wᵀ N w = 0。
停下来想一想
为什么这件事值得记:因为它意味着 q̇ᵀ (Ḃ − 2C) q̇ = 0,也就是科氏/离心项不产生也不消耗能量,只是把能量在关节之间搬来搬去。后面第 13 讲证明"计算力矩 + PD 控制器"的闭环稳定性时,构造李雅普诺夫函数 V = ½q̇ᵀBq̇ + …,求导后出现的 Ḃ − 2C 项就是靠这个性质被消掉的。**没有反对称性,稳定性证明就做不下去。**本讲只需记住结论,不必深究证明。
要特别提醒:q̇ᵀ N q̇ = 0 这个较弱的结论对任意选法的 C 都成立(它其实就是能量守恒),而 wᵀ N w = 0 这个强结论只在按这两式选 C 时成立。
② 对动力学参数是线性的linearity in the dynamic parameters
把"连杆 i + 装在它上面的电机 i+1 的转子"打包成一个增广连杆,用 Steiner 定理把惯量搬到统一参考点,动能和势能都会变成"参数 × 运动量"的一次式。结果:每个连杆只有11 个参数
πi=[mi,miℓCix,miℓCiy,miℓCiz,I^ixx,I^ixy,I^ixz,I^iyy,I^iyz,I^izz,Imi]⊤τ=Y(q,q˙,q¨)⋅π \begin{aligned} \pi_{i} = [ m_{i} , m_{i} \ell_{Cix} , m_{i} \ell_{Ciy} , m_{i} \ell_{Ciz} , \\ \hat{I}_{ixx} , \hat{I}_{ixy} , \hat{I}_{ixz} , \hat{I}_{iyy} , \hat{I}_{iyz} , \hat{I}_{izz} , I_{mi} ]^{\top} \\ \tau = Y(q, \dot{q}, \ddot{q}) \cdot \pi \end{aligned} πi=[mi,miℓCix,miℓCiy,miℓCiz,I^ixx,I^ixy,I^ixz,I^iyy,I^iyz,I^izz,Imi]⊤τ=Y(q,q˙,q¨)⋅π
Y 叫回归矩阵regressor,(n×p) 矩阵,只含关节位置/速度/加速度,不含任何质量惯量;π 是 (p×1) 的常参数向量,p ≤ 13n(把粘性摩擦 F_vi 和库仑摩擦 F_si 也算进去就是每关节 13 个)。
这个性质看着平淡,却极其重要:它把"猜参数"变成了一个线性最小二乘问题,下一讲的辨识全靠它。
停下来想一想
标准方程 B(q)q̈ + C q̇ + F_v q̇ + F_s sgn(q̇) + g = τ 里,做控制时只有哪一项是"我想要的运动"?其余各项该怎么处理?

图 1:标准形式各项的物理意义对照。做动力学前馈控制时,控制器就是要把 C q̇ + F_v q̇ + g 这几项"提前算出来并抵消掉",让剩下的系统看起来像一个纯惯性 B q̈。
四、看图串讲
小讲 5:两连杆参数图与标准方程各项的物理意义对照
第一张把标准方程拆成并排的五块:每一项对应一个物理来源,只有第一项是「我想要的运动」,其余四项都是要被补偿掉的。第二张回到符号本身:a 是杆长,ℓ 是关节轴到质心的距离,红点 C₁、C₂ 是两根杆的质心——这两个量在后面的推导里反复出现,一旦混了,整条式子都会错。看完两张再回读一遍标准形式,就知道每一项该去哪儿找对应。
停下来想一想
图 2 里 a₁ 和 ℓ₁ 分别表示什么?如果误把 ℓ 当成杆长,推导会在哪一步开始出错?
五、例题:完整推导两连杆平面臂的动力学
这是最经典的例题,也是整个动力学里最值得亲手跟一遍的推导。记号:c₁ = cos ϑ₁,s₂ = sin ϑ₂,c₁₂ = cos(ϑ₁+ϑ₂),以此类推。g₀ = [0 −g 0]ᵀ。
小讲 6:两连杆推导(一):雅可比、惯性矩阵、C 与重力项
第 1 步:选广义坐标,写雅可比
q=[ϑ1ϑ2]⊤JP(l1)=[−ℓ1s10ℓ1c1000]JP(l2)=[−a1s1−ℓ2s12−ℓ2s12a1c1+ℓ2c12ℓ2c1200]JO(l1)=[000010]JO(l2)=[000011] \begin{aligned} q = [ \vartheta_{1} \vartheta_{2} ]^{\top} \\ J^{(l1)}_{P} = \begin{bmatrix} - \ell_{1}s_{1} 0 \\ \ell_{1}c_{1} 0 \\ 0 0 \end{bmatrix} \\ J^{(l2)}_{P} = \begin{bmatrix} -a_{1}s_{1} - \ell_{2}s_{12} - \ell_{2}s_{12} \\ a_{1}c_{1} + \ell_{2}c_{12} \ell_{2}c_{12} \\ 0 0 \end{bmatrix} \\ J^{(l1)}_{O} = \begin{bmatrix} 0 0 \\ 0 0 \\ 1 0 \end{bmatrix} J^{(l2)}_{O} = \begin{bmatrix} 0 0 \\ 0 0 \\ 1 1 \end{bmatrix} \end{aligned} q=[ϑ1ϑ2]⊤JP(l1)=−ℓ1s10ℓ1c1000JP(l2)=−a1s1−ℓ2s12−ℓ2s12a1c1+ℓ2c12ℓ2c1200JO(l1)=000010JO(l2)=000011
看懂 J⁽ˡ²⁾_P 第一列:它由两项组成——a₁s₁ 来自"关节 1 转动带动整个连杆 2 的基座(关节 2)平移",ℓ₂s₁₂ 来自"连杆 2 自己的质心相对关节 2 的平移"。两连杆平面臂之所以有耦合,根源就在这里。
转子的雅可比:J⁽ᵐ¹⁾_P 全零(转子 1 质心在原点,不动);J⁽ᵐ²⁾_P = [−a₁s₁ 0; a₁c₁ 0; 0 0];J⁽ᵐ¹⁾_O 第三行 [k_r1 0];J⁽ᵐ²⁾_O 第三行 [1 k_r2](转子 2 的角速度 = 连杆 1 的角速度 + 自身相对转速 k_r2 ϑ̇₂)。

图 2:两连杆平面臂的动力学参数。红点是两根杆的质心 C₁、C₂。假定电机装在关节轴上、转子质心就在相应坐标系原点(p_mi = p_{i−1},z_mi = z_{i−1})。两根杆都在同一平面内运动,所以角速度都沿 z₀ 方向,可以直接用标量转动惯量 I_ℓi。
第 2 步:由 B(q) = Σ (…) 得到惯性矩阵
b11=Iℓ1+mℓ1ℓ12+kr12Im1+Iℓ2+mℓ2(a12+ℓ22+2a1ℓ2c2)+Im2+mm2a12b12=b21=Iℓ2+mℓ2(ℓ22+a1ℓ2c2)+kr2Im2b22=Iℓ2+mℓ2ℓ22+kr22Im2 \begin{aligned} b_{11} = I_{\ell1} + m_{\ell1} \ell_{1}^{2} + k_{r1}^{2} I_{m1} \\ {}+ I_{\ell2} + m_{\ell2} ( a_{1}^{2} + \ell_{2}^{2} + 2 a_{1} \ell_{2} c_{2} ) \\ {}+ I_{m2} + m_{m2} a_{1}^{2} \\ b_{12} = b_{21} = I_{\ell2} + m_{\ell2} ( \ell_{2}^{2} + a_{1} \ell_{2} c_{2} ) + k_{r2} I_{m2} \\ b_{22} = I_{\ell2} + m_{\ell2} \ell_{2}^{2} + k_{r2}^{2} I_{m2} \end{aligned} b11=Iℓ1+mℓ1ℓ12+kr12Im1+Iℓ2+mℓ2(a12+ℓ22+2a1ℓ2c2)+Im2+mm2a12b12=b21=Iℓ2+mℓ2(ℓ22+a1ℓ2c2)+kr2Im2b22=Iℓ2+mℓ2ℓ22+kr22Im2
| 观察点 | 说明 |
|---|---|
| b₁₁ 里出现了 c₂ | 说明关节 1 看到的惯量取决于肘关节的弯曲程度——手臂伸直时惯量大,弯起来时小。这就是"构型相关" |
| b₁₂ = b₂₁ ≠ 0 | 两个关节的加速度互相牵连,B 不是对角阵 |
| b₂₂ 里没有 c₂ | 关节 2 的惯量就是"连杆 2 绕自己关节转"的惯量,与 ϑ₂ 无关 |
| 2 a₁ ℓ₂ c₂ 项 | 来自 |J⁽ˡ²⁾_P q̇|² 展开时的交叉项,是耦合的来源 |
| k_r₂ I_m₂(非对角)vs k_r₁² I_m₁(对角) | 特别解释:非对角上的 k_r₂ I_m₂ 来自"转子总角速度 = 前一级连杆角速度 + 自身转速";减速比大时可近似忽略,那时电机惯量只以 k_ri² I_mi 出现在对角线上 |
第 3 步:算克里斯托弗符号,得到 C
c111=12∂b11/∂ϑ1=0c112=c121=12∂b11/∂ϑ2=−mℓ2a1ℓ2s2≜hc122=∂b12/∂ϑ2−12∂b22/∂ϑ1=hc211=∂b21/∂ϑ1−12∂b11/∂ϑ2=−hc212=c221=12∂b22/∂ϑ1=0c222=12∂b22/∂ϑ2=0C(q,q˙)=[h ϑ˙2h (ϑ˙1+ϑ˙2)−h ϑ˙10] \begin{aligned} c_{111} = \tfrac{1}{2} \partial b_{11}/ \partial \vartheta_{1} = 0 \\ c_{112} = c_{121} = \tfrac{1}{2} \partial b_{11}/ \partial \vartheta_{2} = - m_{\ell2} a_{1} \ell_{2} s_{2} \triangleq h \\ c_{122} = \partial b_{12}/ \partial \vartheta_{2} - \tfrac{1}{2} \partial b_{22}/ \partial \vartheta_{1} = h \\ c_{211} = \partial b_{21}/ \partial \vartheta_{1} - \tfrac{1}{2} \partial b_{11}/ \partial \vartheta_{2} = -h \\ c_{212} = c_{221} = \tfrac{1}{2} \partial b_{22}/ \partial \vartheta_{1} = 0 \\ c_{222} = \tfrac{1}{2} \partial b_{22}/ \partial \vartheta_{2} = 0 \\ C(q,\dot{q}) = \begin{bmatrix} h\,\dot{\vartheta}_{2} & h\,(\dot{\vartheta}_{1}+\dot{\vartheta}_{2}) \\ -h\,\dot{\vartheta}_{1} & 0 \end{bmatrix} \end{aligned} c111=21∂b11/∂ϑ1=0c112=c121=21∂b11/∂ϑ2=−mℓ2a1ℓ2s2≜hc122=∂b12/∂ϑ2−21∂b22/∂ϑ1=hc211=∂b21/∂ϑ1−21∂b11/∂ϑ2=−hc212=c221=21∂b22/∂ϑ1=0c222=21∂b22/∂ϑ2=0C(q,q˙)=[hϑ˙2−hϑ˙1h(ϑ˙1+ϑ˙2)0]
直觉上这是在说什么:唯一的非零系数就是 h = −m_ℓ₂ a₁ ℓ₂ s₂。注意 s₂ = sin ϑ₂——当 ϑ₂ = 0(手臂完全伸直)时 h = 0,科氏/离心效应全部消失。这符合物理直觉:伸直了就没有"甩"的几何了。
第 4 步:重力项
g1=(mℓ1ℓ1+mm2a1+mℓ2a1)gc1+mℓ2ℓ2gc12g2=mℓ2ℓ2gc12 \begin{aligned} g_{1} = ( m_{\ell1} \ell_{1} + m_{m2} a_{1} + m_{\ell2} a_{1} ) g c_{1} + m_{\ell2} \ell_{2} g c_{12} \\ g_{2} = m_{\ell2} \ell_{2} g c_{12} \end{aligned} g1=(mℓ1ℓ1+mm2a1+mℓ2a1)gc1+mℓ2ℓ2gc12g2=mℓ2ℓ2gc12
读法:g₁ 由两部分组成——"连杆 1 自重 + 连杆 2 和电机 2 压在关节 1 上(力臂 a₁)“产生 (… )g c₁,加上"连杆 2 自重”(力臂由 ϑ₁+ϑ₂ 决定)产生的 m_ℓ₂ ℓ₂ g c₁₂。g₂ 只有连杆 2 自己的那份。
小讲 7:两连杆推导(二):运动方程、反对称验证与参数线性
第 5 步:合起来,得到运动方程
[Iℓ1+mℓ1ℓ12+kr12Im1+Iℓ2+mℓ2(a12+ℓ22+2a1ℓ2c2)+Im2+mm2a12]ϑ¨1+[Iℓ2+mℓ2(ℓ22+a1ℓ2c2)+kr2Im2]ϑ¨2−2mℓ2a1ℓ2s2ϑ˙1ϑ˙2−mℓ2a1ℓ2s2ϑ˙22+(mℓ1ℓ1+mm2a1+mℓ2a1)gc1+mℓ2ℓ2gc12=τ1[Iℓ2+mℓ2(ℓ22+a1ℓ2c2)+kr2Im2]ϑ¨1+[Iℓ2+mℓ2ℓ22+kr22Im2]ϑ¨2+mℓ2a1ℓ2s2ϑ˙12+mℓ2ℓ2gc12=τ2 \begin{aligned} [ I_{\ell1} + m_{\ell1} \ell_{1}^{2} + k_{r1}^{2}I_{m1} + I_{\ell2} + m_{\ell2}(a_{1}^{2} + \ell_{2}^{2} + 2a_{1} \ell_{2}c_{2}) + I_{m2} + m_{m2}a_{1}^{2} ] \ddot{\vartheta}_{1} \\ {}+ [ I_{\ell2} + m_{\ell2}( \ell_{2}^{2} + a_{1} \ell_{2}c_{2}) + k_{r2}I_{m2} ] \ddot{\vartheta}_{2} \\ {}- 2 m_{\ell2} a_{1} \ell_{2} s_{2} \dot{\vartheta}_{1} \dot{\vartheta}_{2} - m_{\ell2} a_{1} \ell_{2} s_{2} \dot{\vartheta}_{2}^{2} \\ {}+ ( m_{\ell1} \ell_{1} + m_{m2}a_{1} + m_{\ell2}a_{1} ) g c_{1} + m_{\ell2} \ell_{2} g c_{12} = \tau_{1} \\ [ I_{\ell2} + m_{\ell2}( \ell_{2}^{2} + a_{1} \ell_{2}c_{2}) + k_{r2}I_{m2} ] \ddot{\vartheta}_{1} \\ {}+ [ I_{\ell2} + m_{\ell2} \ell_{2}^{2} + k_{r2}^{2}I_{m2} ] \ddot{\vartheta}_{2} \\ {}+ m_{\ell2} a_{1} \ell_{2} s_{2} \dot{\vartheta}_{1}^{2} + m_{\ell2} \ell_{2} g c_{12} = \tau_{2} \end{aligned} [Iℓ1+mℓ1ℓ12+kr12Im1+Iℓ2+mℓ2(a12+ℓ22+2a1ℓ2c2)+Im2+mm2a12]ϑ¨1+[Iℓ2+mℓ2(ℓ22+a1ℓ2c2)+kr2Im2]ϑ¨2−2mℓ2a1ℓ2s2ϑ˙1ϑ˙2−mℓ2a1ℓ2s2ϑ˙22+(mℓ1ℓ1+mm2a1+mℓ2a1)gc1+mℓ2ℓ2gc12=τ1[Iℓ2+mℓ2(ℓ22+a1ℓ2c2)+kr2Im2]ϑ¨1+[Iℓ2+mℓ2ℓ22+kr22Im2]ϑ¨2+mℓ2a1ℓ2s2ϑ˙12+mℓ2ℓ2gc12=τ2
核对几个细节:
- 第一式里的 −2 m_ℓ₂a₁ℓ₂s₂ ϑ̇₁ϑ̇₂ 是科氏项(两个关节的速度相乘),−m_ℓ₂a₁ℓ₂s₂ ϑ̇₂² 是离心项(关节 2 自己的速度平方甩到关节 1);
- 第二式里只有 +m_ℓ₂a₁ℓ₂s₂ ϑ̇₁²,是离心项(关节 1 的速度甩到关节 2);关节 2 上没有科氏项,因为末端相对连杆 2 是固定的;
- 没有摩擦项,是因为在这里假定"无摩擦、无末端接触力"。
第 6 步:验证反对称性
B˙=[2h ϑ˙2h ϑ˙2h ϑ˙20],2C=[2h ϑ˙22h(ϑ˙1+ϑ˙2)−2h ϑ˙10]N=B˙−2C=[0−2h ϑ˙1−h ϑ˙22h ϑ˙1+h ϑ˙20]N⊤=−N✓ \begin{aligned} \dot{B} = \begin{bmatrix} 2h\,\dot{\vartheta}_{2} & h\,\dot{\vartheta}_{2} \\ h\,\dot{\vartheta}_{2} & 0 \end{bmatrix}\text{,}\quad 2C = \begin{bmatrix} 2h\,\dot{\vartheta}_{2} & 2h(\dot{\vartheta}_{1}+\dot{\vartheta}_{2}) \\ -2h\,\dot{\vartheta}_{1} & 0 \end{bmatrix} \\ N = \dot{B} - 2C = \begin{bmatrix} 0 & -2h\,\dot{\vartheta}_{1}-h\,\dot{\vartheta}_{2} \\ 2h\,\dot{\vartheta}_{1}+h\,\dot{\vartheta}_{2} & 0 \end{bmatrix} \quad N^{\top} = -N \quad \checkmark \end{aligned} B˙=[2hϑ˙2hϑ˙2hϑ˙20],2C=[2hϑ˙2−2hϑ˙12h(ϑ˙1+ϑ˙2)0]N=B˙−2C=[02hϑ˙1+hϑ˙2−2hϑ˙1−hϑ˙20]N⊤=−N✓
对角为 0、非对角互为相反数,正是反对称矩阵的样子。这个结论会被第 13 讲的稳定性证明直接拿去用。
第 7 步:写成对参数线性的形式
π=[π1π2π3π4π5π6π7π8]⊤π1=m1=mℓ1+mm2π5=m2=mℓ2π2=m1ℓC1=mℓ1(ℓ1−a1)π6=m2ℓC2=mℓ2(ℓ2−a2)π3=I^1=Iℓ1+mℓ1(ℓ1−a1)2+Im2π7=I^2=Iℓ2+mℓ2(ℓ2−a2)2π4=Im1π8=Im2τ=Y(q,q˙,q¨)π其中Y是(2×8)回归矩阵 \begin{aligned} \pi = [ \pi_{1} \pi_{2} \pi_{3} \pi_{4} \pi_{5} \pi_{6} \pi_{7} \pi_{8} ]^{\top} \\ \pi_{1} = m_{1} = m_{\ell1} + m_{m2} \pi_{5} = m_{2} = m_{\ell2} \\ \pi_{2} = m_{1} \ell_{C1} = m_{\ell1}( \ell_{1} - a_{1}) \pi_{6} = m_{2} \ell_{C2} = m_{\ell2}( \ell_{2} - a_{2}) \\ \pi_{3} = \hat{I}_{1} = I_{\ell1} + m_{\ell1}( \ell_{1}-a_{1})^{2} + I_{m2} \pi_{7} = \hat{I}_{2} = I_{\ell2} + m_{\ell2}( \ell_{2}-a_{2})^{2} \\ \pi_{4} = I_{m1} \pi_{8} = I_{m2} \\ \tau = Y(q, \dot{q}, \ddot{q}) \pi \text{其中} Y \text{是} (2 \times 8) \text{回归矩阵} \end{aligned} π=[π1π2π3π4π5π6π7π8]⊤π1=m1=mℓ1+mm2π5=m2=mℓ2π2=m1ℓC1=mℓ1(ℓ1−a1)π6=m2ℓC2=mℓ2(ℓ2−a2)π3=I^1=Iℓ1+mℓ1(ℓ1−a1)2+Im2π7=I^2=Iℓ2+mℓ2(ℓ2−a2)2π4=Im1π8=Im2τ=Y(q,q˙,q¨)π其中Y是(2×8)回归矩阵
这里要指出:两连杆平面臂按最大可能算有 22 个参数(2×11),但真正出现在模型里的只有8 个;进一步做参数消减后,最少的只有5 个。这就是下一讲要说的"可辨识性"问题。
对照:另外两种典型结构
| 结构 | 惯性矩阵 B | 科氏/离心 C | 意义 |
|---|---|---|---|
| 两连杆笛卡尔臂 | 常数对角阵:diag( m_ℓ₁+m_m₂+k_r₁²I_m₁+m_ℓ₂ , m_ℓ₂+k_r₂²I_m₂ ) | C = O | 动力学完全解耦,两个轴各管各的。要强调这既归功于笛卡尔结构,也归功于"两关节轴互相垂直"这个特殊几何(见 Problem 7.1) |
| 两连杆平面臂 | 构型相关、非对角 | ≠ O | 典型的耦合系统 |
| 平行四边形臂 | 只要按 m_ℓ₃′ ℓ̄₃′ / (m_ℓ₂′ ℓ₂′) = a₁′/a₁″ 设计质心位置,B 就能做成对角且与构型无关 | 无科氏/离心项 | 封闭运动链的优势:运动学与平面臂等价,但动力学模型轻得多,对轨迹规划和控制极有利。这是工业机械臂大量采用闭链的原因 |
数值感受一下各项的量级
a1=a2=1mℓ1=ℓ2=0.5mmℓ1=mℓ2=50kgIℓ1=Iℓ2=10kg⋅m2kr1=kr2=100mm1=mm2=5kgIm1=Im2=0.01kg⋅m2两个关节各转π/2rad,用时0.5s(三角形速度剖面) \begin{aligned} a_{1} = a_{2} = 1 m \ell_{1} = \ell_{2} = 0.5 m m_{\ell1} = m_{\ell2} = 50 kg \\ I_{\ell1} = I_{\ell2} = 10 kg \cdot m^{2} \\ k_{r1} = k_{r2} = 100 m_{m1} = m_{m2} = 5 kg I_{m1} = I_{m2} = 0.01 kg \cdot m^{2} \\ \text{两个关节各转} \pi/2 rad\text{,用时} 0.5 s\text{(三角形速度剖面)} \end{aligned} a1=a2=1mℓ1=ℓ2=0.5mmℓ1=mℓ2=50kgIℓ1=Iℓ2=10kg⋅m2kr1=kr2=100mm1=mm2=5kgIm1=Im2=0.01kg⋅m2两个关节各转π/2rad,用时0.5s(三角形速度剖面)
给出了的力矩分解图说明了几件事:
- 惯性力矩(inert_11)完全跟着加速度曲线走——加速度多大,力矩就多大;
- inert_22 是分段常数,因为关节 2 轴的转动惯量是常数;
- inert_12 = inert_21,验证了惯性矩阵的对称性(两条轨迹的加速度剖面相同);
- 科氏效应只出现在关节 1,因为末端相对连杆 2 是固定的、相对连杆 1 是移动的;
- 惯性力矩(数千 N·m 量级)远远大于重力项(几百 N·m)——高加速运动时,惯性项占绝对主导。
停下来想一想
两连杆平面臂在 ϑ₂ = 0(手臂完全伸直)时,科氏/离心项会变成多少?为什么这个结果符合物理直觉?
六、常见误区
6.1 记号别混:ℓ 是质心距离,a 是杆长
常见误区
误区 1:把 ℓ 和 a 搞混。
a₁、a₂ 是杆长(关节轴之间的距离),ℓ₁、ℓ₂ 是质心到各自关节轴的距离。很多同学推错公式就是因为把这两个当成同一个量。记住:ℓ 通常约等于 a/2(均质杆),但它们是独立参数。
6.2 B(q) 不是常数矩阵
常见误区
误区 2:以为 B(q) 是个常数矩阵。
只有笛卡尔臂那种特殊结构才是常数。一般机械臂的 B 随构型变化,所以才有 C 项(C 本质上来自 ∂b_ij/∂q_k)。看到 B 随 q 变,就该预期有科氏/离心项。
6.3 科氏项不耗能,但 C 的选法不唯一
常见误区
误区 3:以为科氏/离心项会耗能。
不会。反对称性 q̇ᵀ(Ḃ−2C)q̇ = 0 说明它只做能量在关节间的搬运,不做功。真正耗能的只有摩擦项 F_v q̇ 和 F_s sgn(q̇)。
常见误区
误区 4:以为 C 矩阵是唯一的。
要说明:满足 Σⱼ c_ij q̇_j = ΣⱼΣₖ h_ijk q̇_k q̇_j 的 C 有很多种选法。只有按克里斯托弗符号选,才能得到 Ḃ−2C 反对称这个强性质。别的选法只保证 q̇ᵀ(Ḃ−2C)q̇ = 0。
6.4 电机惯量要乘减速比的平方
常见误区
误区 5:把电机惯量写成 I_m 就完事。
电机在关节 i 上,但经过减速比 k_ri,折算到关节侧的惯量是k_ri² I_mi(出现在对角线上)。平方关系意味着减速比 100 时,转子惯量被放大 1 万倍——所以电机惯量常常不能忽略。
停下来想一想
减速比 k_ri = 100 时,转子惯量折算到关节侧被放大多少倍?为什么是平方关系而不是一次方?
七、本讲小结
停下来想一想
把标准形式在某个工作点线性化、忽略耦合和摩擦后,每个关节退化成什么方程?它对应你在《自动控制原理》里学过的什么模型?
要点回顾
- 动力学回答:“要让机械臂按某加速度动,每个关节得出多少力矩”。拉格朗日法套路:算 T、算 U、套 L = T − U 与 d/dt(∂L/∂q̇) − ∂L/∂q = ξ。
- 刚体动能 = ½m ṗ_Cᵀṗ_C + ½ ωᵀ I ω(平动 + 转动,无交叉项);总动能 = ½ q̇ᵀ B(q) q̇,B 对称、正定、构型相关。
- 标准形式B(q)q̈ + C(q,q̇)q̇ + F_v q̇ + F_s sgn(q̇) + g(q) = τ − Jᵀ(q)h_e:惯性、科氏/离心、粘性摩擦、库仑摩擦、重力、末端接触力。
- 两连杆平面臂:b₁₁ 含 2a₁ℓ₂c₂,唯一的非零 Christoffel 组合是 h = −m_ℓ₂a₁ℓ₂s₂;ϑ₂ = 0(伸直)时所有速度耦合项消失。
- 两个关键性质:Ḃ − 2C 反对称(后面证明控制器稳定性的钥匙)、对动力学参数线性 τ = Y π(后面做参数辨识的基础)。
一句话总结
拉格朗日法把"算力矩"变成了"算能量再求导",而算出来的结果永远长成 B q̈ + C q̇ + F q̇ + g = τ 这个样子——左边四项就是"加速、被甩、摩擦、托住自己"。
DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐


所有评论(0)