CH-04 刚体与多体动力学
机械臂带着负载加速时,电机既要抵抗重力,也要推动有质量和惯量的连杆。同一条几何轨迹,缓慢执行与快速执行需要不同的力矩;第一关节处在相同角度时,手臂伸展与收拢也会改变它承受的惯性负载。动力学用运动方程计算这些力矩,并描述关节经同一机构产生的相互影响。
沿用上一章的两连杆机构,可以分别从受力和能量出发得到同一组方程,再解释其中的惯性、速度耦合与重力。除特别说明外,这里假定基座固定、连杆刚性、质量参数已知,并忽略摩擦和关节弹性。将模型用于真实机器人时,需要补入这些被忽略的作用。
1. 刚体动力学
负载加速的快慢由驱动力矩和机构惯性共同决定。先分析每根连杆的受力,再汇总到关节,就能计算几何轨迹所需的驱动。
1.1 Newton–Euler 方程
刚体运动可以分解为质心的平动和绕质心的转动。对应地,动力学方程也分成两部分:Newton 方程描述质量与线加速度的关系,Euler 方程描述惯量与角运动的关系。
在机器人中,第 $i$ 根连杆受到相邻连杆施加的力与力矩,以及重力。取固定在基座上的惯性参考系,记其质量为 $m_i$,质心速度为 $v_{c_i}$,重力加速度向量为 $g$。若 $f_{i-1,i}$ 表示前一根连杆施加的力,$-f_{i,i+1}$ 表示后一根连杆施加的力,则
把右侧移到左侧,就得到在静力平衡式中加入惯性力 $-m_i\dot v_{c_i}$ 的形式。这也是 D’Alembert 原理的用法:用惯性项把动态问题写成形式上的力平衡。

自由体图选定一根连杆,标出其他物体对它施加的力。相邻连杆的作用与反作用在整个机构中可以抵消,在单根连杆的方程中却决定着加速度。机械连接还提供维持连接的约束力,电机则沿关节允许的方向驱动。后面的消元会保留独立输入,去掉这些内部反力。
1.2 惯性张量
刚体绕质心旋转的质量性质由 $3\times3$ 惯性矩阵 $I_i$ 描述。令 $r$ 是相对质心的位置向量,$\rho$ 是质量密度,则惯性矩阵可以紧凑写成
例如其 $xx$ 元素为 $\int\rho(y^2+z^2)dV$,$xy$ 元素为 $-\int\rho xy\,dV$。它是对称矩阵,完整表达刚体对不同转动轴的惯性。
固定在刚体上的坐标系看到的质量分布不变,但固定惯性坐标系看到的矩阵会随刚体方向改变。角动量为 $I_i\omega_i$,其中 $\omega_i$ 是角速度。对其求时间导数,除了 $I_i\dot\omega_i$,还会出现由矩阵方向变化产生的项。因此,绕质心的总外力矩满足
第二项称为陀螺项。对平面运动,转动轴方向固定,角速度和惯量可以简化为标量;一般三维运动则需要保留矩阵和向量形式。
惯量的定义需要同时给出参考点和坐标轴。这里绕质心定义,移到关节处时要通过平行轴关系加入质心偏移的贡献;只旋转坐标轴时,改变的则是同一张量的分量。这些表示变化可以发生在质量分布完全不变的刚体上。
陀螺项来自角动量与角速度的方向关系。三维刚体的角动量未必平行于角速度,因此维持指定角运动可能需要力矩。沿惯量主轴的特定转动使叉乘项消失;平面机构的固定转轴也允许使用标量惯量。一般空间手臂仍要保留完整的矩阵表达。
2. 关节空间动力学
逐连杆受力图包含内部作用和电机驱动。控制器需要的关节空间方程,则直接联系独立关节运动与驱动力矩。
对全部连杆列 Newton–Euler 方程,便得到完整的受力方程组。要由电机输入求关节运动,还需要结合机构几何,消去内部作用。
每根连杆既是前一根的负载,也承受后一根的反作用。连杆之间的力维持连接,力矩既驱动关节允许的转动,也限制其余方向。自由体图能逐一标出这些作用,但整组方程仍要按关节运动关系整理。
各质心的运动通过同一套关节连接关系联系起来。因此,方程中的驱动力矩和约束反力需要区分,质心位置也应由关节坐标计算。将所有质心位置都独立列为输出,会重复描述机构运动。
更适合分析和控制的形式,是使用一组完整、独立的广义坐标 $q$,并显式保留对应输入。对固定基座串联机械臂,关节位移通常可以作为这样的坐标,关节驱动力矩组成 $\tau$。消去内部约束力后,运动方程便描述 $q$ 与 $\tau$ 之间的关系。
2.1 平面 2R 模型
考虑位于竖直平面中的两转动关节机械臂。设 $l_1$ 为第一连杆长度,$l_{c_1},l_{c_2}$ 分别为各连杆质心到其近端关节的距离;$m_1,m_2$ 为质量,$I_1,I_2$ 为绕质心、垂直于平面的转动惯量。第一关节角 $\theta_1$ 从固定 $x$ 轴测量,第二关节角 $\theta_2$ 相对第一连杆测量。重力沿负 $y$ 轴,大小为 $g$。

连杆的绝对角速度为
第二连杆的绝对角速度由两部分相加:随第一连杆转动的速度,以及第二关节相对转动的速度。质心位置也包含这两部分几何关系:
对这些位置求导,就能把质心速度及加速度全部写成关节变量的函数。
第一关节改变时,第二连杆的质心随整根第一杆运动;第二关节改变时,质心又相对肘部运动。两种速度相加后,速度平方中出现交叉项,最终进入惯量矩阵的非对角元素。这些耦合可从机构几何和质量分布推导出来。
2.2 运动方程
先看第二连杆。末端没有额外外力时,它的平动力平衡直接给出
这个力既加速第二连杆,也传回第一连杆。以第二关节为转动中心写力矩关系,令 $r_{2c}$ 为第二关节到第二连杆质心的向量,有
在平面中,叉乘结果取垂直于运动平面的标量分量。右侧仍然使用质心加速度,需要对质心位置求两次时间导数,再改写为关节角、角速度和角加速度。第一连杆的方程还包含第二关节的反向作用力矩;把两根连杆的关系联立,才能消去内部力并留下驱动输入。
逐连杆列出受力和力矩关系、消去内部作用,得到
其中
下面的物理解释与能量推导都使用这套系数,其中多体惯量随关节姿态改变。
按所乘变量,可以将方程分为惯量、速度和重力项。角加速度前的系数描述惯量,角速度平方或乘积描述速度相关作用,静止时剩下的则是重力项。拆开后可以检查量纲与极限:静止状态对应重力补偿,去掉重力和速度后恢复惯性与加速度的关系。
整体转动不改变本模型中质心之间的相对距离,所以惯量只依赖肘角。重力力矩取决于连杆相对竖直方向的朝向,因此还依赖整体方向。空间机构或不同质量分布,需要从各自几何重新计算这种构型依赖。
3. 惯性与运动耦合
电机既要承受重力,也要加速连杆,并承担其他关节传来的作用。拆开这些负载,才能解释姿态和速度为什么会改变力矩需求。
3.1 等效惯量
固定第二关节并暂时忽略重力,第一条方程变成 $\tau_1=H_{11}\ddot\theta_1$。因此,$H_{11}$ 是第一关节看到的总等效惯量。
第一连杆的贡献为 $I_1+m_1l_{c_1}^2$。第二连杆的贡献取决于它的质心到第一关节轴的距离 $L$:
由平行轴定理,这部分惯量为 $I_2+m_2L^2$。机械臂完全伸直时,第二连杆质心距第一关节较远,等效惯量较大;完全折叠时,距离缩短,等效惯量较小。相同的关节力矩,在不同姿态下产生的角加速度可以不同。
第二关节保持固定,通常需要锁定机构或由控制器施加相应力矩。若把它的驱动力矩设为零,关节便可能随第一关节加速而运动,第一关节的有效惯性也会改变。因此,使用惯量矩阵对角元素时,需要先说明其他关节的约束或控制状态。
3.2 加速度耦合
第一条方程中的 $H_{12}\ddot\theta_2$ 说明,即使第一关节自身没有角加速度,它仍可能需要力矩抵抗第二连杆加速所产生的反作用。第二条方程中的 $H_{12}\ddot\theta_1$ 则描述相反方向的耦合。
两个加速度耦合项的系数相同,体现了惯量矩阵的对称性。一根连杆经连接关系影响其他关节,多关节运动的力矩需求因而需要联立计算。
负耦合系数表示:在当前构型和坐标下,另一关节的正向加速度可能减少本关节所需的正向力矩。总动能是否为正,要检查完整惯量矩阵的二次型;系统是否稳定,还要分析运动方程。第 1 章的正定性判断可用于前一项检查。
3.3 速度耦合
与速度平方成比例的项对应离心效应。例如,当第一关节以恒定速度旋转、第二关节相对它保持不动时,第二连杆质心绕第一关节运动,离心作用会在第二关节产生力矩,其大小在运动方程中由 $h\dot\theta_1^2$ 表示。
当两个关节同时运动时,第一连杆上的坐标系也在旋转,第二连杆质心又相对这个旋转坐标系移动。对于以角速度 $\omega$ 旋转的坐标系,质量 $m$ 以相对速度 $v_b$ 运动,会出现与 $2m\omega\times v_b$ 对应的 Coriolis 作用。两连杆方程中的 $-2h\dot\theta_1\dot\theta_2$ 就是这种速度耦合。
重力项 $G_1,G_2$ 则来自各连杆质量绕关节轴形成的重力力矩。机械臂沿水平轴伸展时,重力力臂较大。惯量、速度耦合和重力都具有明确的几何来源。
速度相关项给出维持指定运动所需的力矩。同一姿态下,全部速度同时反号,二次速度项保持不变;速度整体加倍,力矩大小增至四倍。加速度项则随指定加速度线性变化。因此,更改轨迹执行时间会改变两类动态负载,静态重力负载仍然存在。
3.4 惯量与力矩算例
为同一套两连杆方程指定参数:$l_1=l_2=1\,\mathrm m$,$m_1=m_2=1\,\mathrm{kg}$,$l_{c_1}=l_{c_2}=0.5\,\mathrm m$,$I_1=I_2=1/12\,\mathrm{kg\,m^2}$。代入上一节的系数后,多体惯量矩阵化为
肘角分别取 $0$、$\pi/2$ 和 $\pi$,得到
三个矩阵的单位均为 $\mathrm{kg\,m^2}$。从伸直到折叠,$H_{11}$ 由 $8/3$ 降为 $2/3$,相差四倍。$H_{12}$ 虽可为负,完整矩阵仍为正定,因为本例有 $H_{11}>0$,且
下图上方是三个肘角的机构形状,下方是整个角度区间内的三个独立矩阵元素。折叠构型中两根等长连杆重叠,第二连杆回到基座;曲线则把这一几何变化与等效惯量、关节耦合联系起来。
再看维持静止时的重力力矩。取 $g=9.81\,\mathrm{m/s^2}$、$\theta_1=0$,若第二关节也为零,则
若第二关节改为 $\pi/2$,第二连杆竖直向上,得到 $G=(14.715,0)^{\mathsf T}\,\mathrm{N\,m}$。第二连杆在这个姿态下绕自身近端关节的重力力臂为零,但它仍然作为负载影响第一关节。
保持后一配置,在某一瞬间令 $\dot q=0$、$\ddot q=(0,1)^{\mathsf T}\,\mathrm{rad/s^2}$。速度耦合项此刻为零,加速度所需的惯性力矩为
第一关节自身没有角加速度,却仍需 $1/3\,\mathrm{N\,m}$ 来承担第二关节加速的耦合。再加上重力,完整驱动力矩为
这三个计算使用同一参数模型,依次区分了姿态相关惯量、静态重力负载和加速度耦合。可下载绘图与核算脚本、惯量曲线数据及矩阵与力矩结果。脚本逐一检查了 $0^\circ$ 至 $180^\circ$ 的 181 个采样惯量矩阵。
锁定与自由响应可以用同一模型定量比较。取肘角 90°,忽略速度项,并假定重力准确补偿。第二关节锁定时,第一关节获得 1 牛米净力矩,产生 0.6 弧度每秒平方的加速度;第二关节需施加 0.2 牛米才能保持锁定。若两电机的净输入改为 (1, 0) 牛米,第二关节自由随动,解完整矩阵得到角加速度 (0.75, −0.75)。
后一条件下,消去第二关节加速度得到第一关节的有效惯性
小于锁定情况下的 5/3。标准库核算脚本的 --chapter 04 选项可以复算这两种边界条件,JSON 结果保存输出。关节所见的有效惯性由质量模型和其他关节的控制方式共同决定。
4. Lagrange 动力学
关节增多后,逐个追踪内部反力会越来越繁杂。用动能与势能构造方程,可以系统地汇总这些连杆的作用。
Lagrange 方法先用广义坐标计算系统能量,再通过微分得到运动方程。对于独立坐标描述的理想约束系统,内部约束反力不做虚功,最终方程中便无需显式求出它们。Newton–Euler 方法直接追踪受力,两条路线描述的是同一物理系统。
定义动能 $T(q,\dot q)$、势能 $U(q)$,以及 Lagrangian
第 $i$ 个广义坐标满足
$Q_i$ 是与坐标 $q_i$ 配对的广义力。它的定义需要通过虚功关系核对;坐标与电机转角不同,$Q_i$ 的分量也会不同。
独立广义坐标已经在位置表达中满足连接条件,不做虚功的内部约束力因而可以省去。闭环机构若仍使用冗余坐标,就要显式加入约束或乘子。接触、摩擦等非保守作用则要写成相应广义力放到右侧。
4.1 动能与势能
每根连杆的动能由质心平动和绕质心转动两部分组成:
把速度全部改写为关节变量后,得到
其中 $H$ 的元素就是上一节已经得到的系数。取高度零点后,重力势能为
对 $U$ 求关节偏导得到重力项:$G_i=\partial U/\partial\theta_i$。把动能和势能代入 Lagrange 方程,会得到与 Newton–Euler 方法完全相同的两连杆运动方程。
能量推导先求速度,再由微分得到加速度项。这种组织方式便于扩展到更多关节,得到的物理规律与逐连杆受力法一致。
以第一关节为例,动量型偏导为
再作时间求导,需要同时对速度和惯量系数求导。另一方面,$H_{11},H_{12}$ 在这个模型中只依赖 $\theta_2$,所以 $\partial L/\partial\theta_1$ 来自势能,等于 $-G_1$。把两部分放回 Lagrange 方程,就得到第一条关节力矩方程。第二关节则还要处理动能对 $\theta_2$ 的偏导,因为改变第二关节角会改变质心距离与惯量。
本模型中,第一惯量系数对肘角的导数是速度耦合系数的负二倍,交叉惯量系数的导数是其负一倍。作时间求导时,还要乘肘部角速度,于是产生第一方程中的两种速度乘积。姿态相关系数的导数,正是动能推导中形成速度力矩的来源。
势能加上常量,关节偏导和重力力矩都保持不变。势能梯度写在方程左侧,表示实现指定运动所需抵消的重力负载;重力方向已包含在势能定义中。可以用静止姿态下的力臂核对符号。
5. 多体惯量矩阵
仿真和控制需要整条机械臂在当前配置下的惯性。把关节速度映射到每根连杆,再汇总它们的动能,就能构造多体惯量矩阵。
5.1 质心 Jacobian
对于一般三维刚体,第 $i$ 根连杆的动能为
所有速度与惯量必须使用一致的坐标表达。定义质心线速度 Jacobian $J_i^L$ 和角速度 Jacobian $J_i^A$:
在串联机构中,第 $i$ 根连杆只受到它之前的关节运动影响。因此,相应 Jacobian 中位于它之后的关节列为零。
把各连杆动能相加并代入速度关系:
同一组关节速度先映射为各连杆的质心速度与角速度,再分别计算动能并累加。因此,质心位置决定平动 Jacobian,惯量须与角速度采用相同坐标,工具和负载也要加入汇总。更新工具模型时,应一起给出质量、质心和惯量。
5.2 对称性与正定性
单根连杆的 $I_i$ 是 $3\times3$ 矩阵,描述它绕质心转动的质量性质;多体惯量矩阵 $H$ 是 $n\times n$ 矩阵,描述整套机构投影到独立关节坐标后的惯性。
从构造式可以看出,$H$ 是对称的。它的二次型给出总动能,因此在非退化的质量模型和独立坐标下,非零关节速度具有正动能,$H$ 为正定矩阵。它又包含随机构配置变化的 Jacobian,因此一般依赖 $q$。
对角元素描述一个关节的运动所看到的惯性;非对角元素描述关节间的加速度耦合。$H_{ij}=H_{ji}$ 意味着,关节 $j$ 的加速度作用于关节 $i$ 的耦合系数,与反方向耦合的系数相同。这一对称关系也称为 Maxwell 互易关系。
$H(q)$ 描述当前配置下的综合惯性。机械臂从伸展变为收拢时,各连杆速度由关节速度合成的方式也随之改变。Jacobian 将几何变化带入动能,因而得到依赖配置的惯量矩阵。
对称性和正定性约束整个矩阵,非对角元素仍可能为负。数值检查可以核对对称误差和非零速度的动能符号,质量参数是否符合真实机器则需实测。坐标冗余、零质量连杆或不合理惯量都可能影响可逆性,遇到异常应先检查建模假设;任意加上小对角值,可能只是在求解中掩盖问题。
6. Coriolis 与离心项
高速动作中,即使瞬时关节加速度相同,不同关节速度也会改变力矩需求。速度二次项记录了这种运动耦合。
从
求导时,不能把 $H$ 当成常量。例如
再减去 $\partial T/\partial q_i$,得到速度二次项
当 $j=k$ 时,这些项与某一关节速度的平方成比例;当 $j\ne k$ 时,它们与不同关节速度的乘积成比例,分别对应离心和 Coriolis 耦合。它们共同反映了惯量随配置变化的非线性效果。
这样,固定基座机械臂在没有额外外力时的运动方程写为
左侧依次为加速度对应的惯性力矩、速度耦合力矩和重力力矩。这里的 $c$ 是速度项向量,与文献中用于分解它的系数矩阵有所区别。
速度项也常写成矩阵乘关节速度,矩阵构造可以有不同形式,最终产生的速度力矩向量应相同。对接动力学库时,要先确定返回的是完整向量、系数矩阵,还是已包含重力的偏置力,再组合各项,以免重复计算。
6.1 用能量与功率核对模型
对当前理想模型,动能与势能的总变化率应当等于执行器做功率:
惯量变化与速度项共同保证输入功率等于机械能变化率。有驱动输入时,机械能一般会变化;若省略速度项,只留下加速度与重力,通常会破坏这一功率关系。
沿用第 7.5 节的状态和加速度,核算脚本把所需力矩拆为惯性项约 (0.438159, 0.125439)、速度项约 (−0.015577, 0.001947) 和重力项约 (17.809327, 3.751551) 牛米。按当前关节速度加权,驱动功率约为 2.598978469 瓦。另一条独立检查沿局部位置与速度轨迹,对总机械能作中心差分,得到 2.598978469 瓦,两者差异小于十亿分之一瓦。这里检查的是一个状态附近的功率关系,未运行真实机械臂。
7. 广义力与逆动力学
环境外力与电机力矩都通过广义力影响运动。将外力按作用点和坐标转换后,就能反求指定运动所需的驱动力矩。
7.1 重力项
系统受力可以分为保守力与非保守力。保守力可以通过势能的偏导进入 Lagrange 方程。如果重力是唯一保守力,各连杆总势能为
这里 $g$ 是含方向的重力加速度向量,$r_{c_j}$ 是第 $j$ 根连杆质心相对固定原点的位置。因此
质心位置的偏导就是其线速度 Jacobian 的第 $i$ 列。这与静力学中用力和力臂求关节重力力矩的结果一致,只是在能量方法中通过势能梯度得到。用同一组质心几何同时计算动能和势能,能够保持各个动力学项之间的一致性。
不同姿态下的静态力矩,是驱动选型与重力补偿需要考虑的负载。水平伸展的手臂即使不动,也可能需要很大的电机力矩。更换工具后,应更新质量与质心,否则逆动力学和反馈控制都会继承重力模型的偏差。
7.2 广义力
广义力由虚功定义:
考虑两连杆机械臂,如果使用相对关节角 $\theta_1,\theta_2$,虚功为
如果改用两根连杆相对固定轴的绝对角 $\phi_1,\phi_2$,则
第二关节力矩 $\tau_2$ 作用在第二连杆上,同时给第一连杆一个反向力矩 $-\tau_2$。因此
对应的广义力是 $Q_1=\tau_1-\tau_2$、$Q_2=\tau_2$。同样的物理输入,在不同坐标下具有不同的广义力分量;虚功关系保证二者一致。
电机力矩 (5, 2) 牛米与相对关节速度 (0.1, 0.2) 弧度每秒,对应输入功率 0.9 瓦。改用绝对角时,速度变为 (0.1, 0.3),广义力变为 (3, 2),功率仍为 0.9 瓦。速度与力必须配套转换,虚功或功率一致便可用于检查转换是否正确。
7.3 外力映射
如果外力 $F_{\mathrm{ext}}$ 作用在末端,其虚功为 $F_{\mathrm{ext}}^{\mathsf T}\delta p$。利用 $\delta p=J\delta q$,得到
于是方程右侧应相应改为 $\tau+J^{\mathsf T}F_{\mathrm{ext}}$。如果外力作用于其他位置 $r(q)$,需要使用该作用点的 Jacobian $J_r=\partial r/\partial q$,不能直接使用末端 Jacobian。
位置 Jacobian 与作用点的力配对;完整刚体运动的 Twist Jacobian 则与 wrench 配对,二者需采用相同参考系、参考点和排列。转置映射由功率或虚功相等得到。力的作用点或参考原点改变时,力矩分量也要相应转换。
7.4 逆动力学
与正向动力学从输入求运动相对,逆动力学寻找实现指定运动所需的关节力矩。对上述模型,给定 $q,\dot q,\ddot q$ 及外力后,代入各项即可得到
可以用末端负载检查逆动力学中的外力符号。取后文递归算例的状态,让环境在末端施加固定坐标中的 (2, −1) 牛顿外力。核算脚本按位置 Jacobian 计算出广义外力约 (−3.599654, −2.053278) 牛米。维持同样运动时,执行器力矩增加为约 (21.831564, 5.932215) 牛米。
该外力阻碍当前运动,功率约为 −0.770621 瓦,电机因而需要补足这部分功率。用关节速度与广义外力计算,或用末端速度与实际外力计算,结果相同。反转外力方向,补偿方向也会反转。这个算例把外力作为给定输入;真实接触中,外力还要由环境、摩擦与接触约束共同确定。
7.5 递归动力学
递归 Newton–Euler 算法(RNEA)按连杆顺序计算力矩,无需手写全部惯量系数。它先从基座向末端传播运动,再从末端向基座传播受力,两次遍历使用一致的连杆坐标和关节轴。
取 $A_i$ 为关节轴在连杆 $i$ 坐标中的表达,$X_i=\operatorname{Ad}_{T_{i,i-1}}$ 把上一连杆的 Twist 变换到当前坐标。采用 $[\omega;v]$ 顺序,前向传播为
$\operatorname{ad}_V$ 是六维 Lie bracket 的线性算子,满足 $\operatorname{ad}_VW=[V,W]$,用于计算速度耦合;有限坐标变换则使用 $\operatorname{Ad}_T$。固定基座取 $V_0=0$,重力写入初始加速度 $\dot V_0=[0;-g]$,使后续连杆计算包含维持重力负载所需的贡献。
用 $\mathcal G_i$ 表示连杆的 $6\times6$ 空间惯量,以区别前文的重力向量 $G(q)$。在质心坐标中,它把转动惯量和质量放入同一矩阵。反向传播得到
$F_i=[\mu_i;f_i]$ 是先力矩、后力的 wrench,$\mu_i$ 表示力矩向量,末式从整根连杆的受力中取出关节轴对应的广义力。各步骤只需邻接连杆的数据,算法返回的仍然是前面关节空间方程中的 $\tau$。
InverseDynamics 的 Mlist 每项给出零位下当前连杆坐标系相对前一连杆坐标系的位姿,末项是末端相对最后连杆的位姿;Glist 保存空间惯量,Slist 保存零位空间螺旋轴。其 Ftip 表示末端施加给环境、在末端坐标中表达的 wrench;这与第 7.3 节环境施加给机器人的 $F_{\mathrm{ext}}$ 方向相反,因此不能把两个参数不加转换地互换。
仍使用第 3.4 节的两连杆质量模型,选择 $q=(0.3,0.4)$、$\dot q=(0.1,0.2)$、$\ddot q=(0.2,-0.1)$,角度、角速度与角加速度分别使用弧度、弧度每秒和弧度每秒平方;末端 wrench 取零。递归计算得到
1 | tau = mr.InverseDynamics(q, dq, ddq, g, Ftip, Mlist, Glist, Slist) |
回代得到 $\ddot q_{\mathrm{back}}\approx(0.2,-0.1)$,最大分量误差约 $4.50\times10^{-15}$,力矩也与两连杆解析方程一致。MassMatrix 用单位关节加速度的逆动力学调用逐列构造惯量矩阵,ForwardDynamics 则按惯量、速度项、重力和末端负载求关节加速度。二者分别计算同一模型中由运动到力矩、由力矩到运动的关系。
可下载坐标、惯量及核算脚本和数值结果。脚本依赖 NumPy 与固定版本模块,同时复算了第 3.4 节加速度耦合算例,检查递归与解析形式的惯量及力矩一致性。
前向递归计算各连杆的运动,反向递归计算它们向近端传递的受力。力采用伴随转置变换,以保持与速度变换的功率关系,所以六维力与速度的分量顺序必须配对。基座的负重力加速度是算法中的重力处理方式,使静止模型输出支撑力矩,物理基座仍然固定。
正向回代与解析方程一致,可以核对接口、坐标和模型组织。真实参数是否准确,需要另外测量:两次计算若共用错误质量参数,也可能彼此一致。因此,先用独立几何和能量关系检查推导,再用实测质量、质心与负载检查物理模型。配套脚本完成了前一层核对,未进行真实机器的参数辨识。
关节运动所需的力矩由连杆质量、质心、惯量、坐标约定和外力作用点共同决定。下一章将在这些动力学关系上加入反馈,根据实际运动偏差调整输入。
参考资料
H. Harry Asada. Introduction to Robotics, MIT 2.12, Chapter 7, §§7.1–7.2, Examples 7.1 and 7.2. © Massachusetts Institute of Technology;中文节选重排、记号校订与例题补充;译编采用 CC BY-NC-SA 4.0。
Kevin M. Lynch and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017; updated first-edition preprint, 2019. §§8.3.2, 8.5; Eqs. (8.50)–(8.54), (8.77), (8.81), pp. 292–293, 296–297. 概念整理与公式核对。
NxRLab. Modern Robotics Python core.py, commit cbd41fdf0bf75dbd5e986d832ae14226964ff0ac. Functions ad, InverseDynamics, MassMatrix, ForwardDynamics. © 2019 NxRLab;API 与代码例译编,MIT License;公开模块附许可文本。




