🦉 猫头鹰都在卷机械臂逆解了,你确定不关注一下?

第 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。

  1. 算总动能 T:每个连杆 + 每个电机转子,平动 + 转动,全部加起来。
  2. 算总势能 U:只有重力(假设刚性连杆),等于 −Σ mᵢ g₀ᵀ pᵢ。
  3. 对 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⊤​⋅Ri​Iℓii​Ri⊤​⋅ω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ℓi​JP(li)⊤​JP(li)​+JO(li)⊤​Ri​Iℓii​Ri⊤​JO(li)​+mmi​JP(mi)⊤​JP(mi)​+JO(mi)⊤​Rmi​Imimi​Rmi⊤​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ℓi​g0⊤​pℓi​+mmi​g0⊤​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​=ξi​i=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˙​+Fv​q˙​+Fs​sgn(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∑​cijk​q˙​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

图 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)​=​−ℓ1​s1​0ℓ1​c1​000​​JP(l2)​=​−a1​s1​−ℓ2​s12​−ℓ2​s12​a1​c1​+ℓ2​c12​ℓ2​c12​00​​JO(l1)​=​000010​​JO(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

图 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​+kr12​Im1​+Iℓ2​+mℓ2​(a12​+ℓ22​+2a1​ℓ2​c2​)+Im2​+mm2​a12​b12​=b21​=Iℓ2​+mℓ2​(ℓ22​+a1​ℓ2​c2​)+kr2​Im2​b22​=Iℓ2​+mℓ2​ℓ22​+kr22​Im2​​

观察点说明
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ℓ2​a1​ℓ2​s2​≜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ϑ˙1​​h(ϑ˙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​+mm2​a1​+mℓ2​a1​)gc1​+mℓ2​ℓ2​gc12​g2​=mℓ2​ℓ2​gc12​​

读法: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​+kr12​Im1​+Iℓ2​+mℓ2​(a12​+ℓ22​+2a1​ℓ2​c2​)+Im2​+mm2​a12​]ϑ¨1​+[Iℓ2​+mℓ2​(ℓ22​+a1​ℓ2​c2​)+kr2​Im2​]ϑ¨2​−2mℓ2​a1​ℓ2​s2​ϑ˙1​ϑ˙2​−mℓ2​a1​ℓ2​s2​ϑ˙22​+(mℓ1​ℓ1​+mm2​a1​+mℓ2​a1​)gc1​+mℓ2​ℓ2​gc12​=τ1​[Iℓ2​+mℓ2​(ℓ22​+a1​ℓ2​c2​)+kr2​Im2​]ϑ¨1​+[Iℓ2​+mℓ2​ℓ22​+kr22​Im2​]ϑ¨2​+mℓ2​a1​ℓ2​s2​ϑ˙12​+mℓ2​ℓ2​gc12​=τ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ϑ˙2​hϑ˙2​​hϑ˙2​0​],2C=[2hϑ˙2​−2hϑ˙1​​2h(ϑ˙1​+ϑ˙2​)0​]N=B˙−2C=[02hϑ˙1​+hϑ˙2​​−2hϑ˙1​−hϑ˙2​0​]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 时,转子惯量折算到关节侧被放大多少倍?为什么是平方关系而不是一次方?

七、本讲小结

停下来想一想

把标准形式在某个工作点线性化、忽略耦合和摩擦后,每个关节退化成什么方程?它对应你在《自动控制原理》里学过的什么模型?

要点回顾
  1. 动力学回答:“要让机械臂按某加速度动,每个关节得出多少力矩”。拉格朗日法套路:算 T、算 U、套 L = T − U 与 d/dt(∂L/∂q̇) − ∂L/∂q = ξ。
  2. 刚体动能 = ½m ṗ_Cᵀṗ_C + ½ ωᵀ I ω(平动 + 转动,无交叉项);总动能 = ½ q̇ᵀ B(q) q̇,B 对称、正定、构型相关。
  3. 标准形式B(q)q̈ + C(q,q̇)q̇ + F_v q̇ + F_s sgn(q̇) + g(q) = τ − Jᵀ(q)h_e:惯性、科氏/离心、粘性摩擦、库仑摩擦、重力、末端接触力。
  4. 两连杆平面臂:b₁₁ 含 2a₁ℓ₂c₂,唯一的非零 Christoffel 组合是 h = −m_ℓ₂a₁ℓ₂s₂;ϑ₂ = 0(伸直)时所有速度耦合项消失。
  5. 两个关键性质:Ḃ − 2C 反对称(后面证明控制器稳定性的钥匙)、对动力学参数线性 τ = Y π(后面做参数辨识的基础)。
一句话总结

拉格朗日法把"算力矩"变成了"算能量再求导",而算出来的结果永远长成 B q̈ + C q̇ + F q̇ + g = τ 这个样子——左边四项就是"加速、被甩、摩擦、托住自己"。

Logo

DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐