1. 从“静”到“动”：为什么动力学是机器人的灵魂

搞机器人，尤其是做运动控制或者力控的朋友，肯定都绕不开动力学。如果说运动学解决的是“机器人末端执行器在哪里”的问题，那么动力学解决的，就是“机器人要如何到达那里”以及“到达那里需要付出多大代价”的问题。这听起来有点抽象，我打个比方：运动学就像给你一张地图，告诉你从A点到B点有哪些路可以走；而动力学则是告诉你，开一辆小轿车和开一辆满载的大卡车，走同一条路，需要踩多大的油门、打多大的方向盘、消耗多少油，以及会不会把路压坏。

很多新手，甚至一些做了几年机器人应用开发的朋友，常常会忽略动力学的重要性。他们觉得，只要运动学逆解算得准，关节电机给个位置指令，机器人就能乖乖听话。这在低速、轻载、对精度和能耗不敏感的场景下，或许勉强可行。但一旦涉及到高速运动、大负载搬运、与人协作（需要精确的力控），或者对能耗有严格要求的场合，不懂动力学，你的机器人要么动作笨拙、能耗惊人，要么干脆就抖得跟筛糠一样，甚至发生危险。

动力学分析的核心，就是建立关节驱动力/力矩（τ）与机器人运动状态（关节位置q、速度q̇、加速度q̈）之间的数学关系。这个关系式，就是机器人动力学方程。它本质上是一个二阶非线性微分方程，复杂得很。但正是这个复杂的方程，蕴藏着让机器人变得“聪明”和“高效”的钥匙。通过它，我们可以实现计算力矩控制（也叫前馈控制），提前补偿掉机器人的惯性力、科氏力、离心力和重力，让控制器只需处理很小的建模误差和外部扰动，从而获得极其平滑、快速、节能的运动性能。可以说，动力学模型的精度，直接决定了高端机器人性能的上限。

2. 拉格朗日力学：一把通往动力学方程的“万能钥匙”

要建立动力学方程，我们有好几把“钥匙”，比如牛顿-欧拉法、高斯原理等。但在我看来，对于从零开始理解机器人动力学，拉格朗日力学是最直观、最“物理”的一把钥匙。它不像牛顿-欧拉法那样需要分析每个连杆的受力和力矩平衡（虽然计算效率高，但推导过程容易晕），而是从能量的角度俯瞰整个系统。

拉格朗日方法的核心是定义一个叫拉格朗日函数L的量，它是系统总动能（T）和总势能（V）的差：L = T - V。这个定义非常巧妙，它把系统的所有运动学和力学信息都打包进去了。然后，对于每一个广义坐标（对我们来说就是每个关节的位置qi），都有对应的拉格朗日方程：

d/dt (∂L/∂q̇i) - ∂L/∂qi = τi

这里的τi就是作用在第i个广义坐标上的广义力。对于旋转关节，就是关节力矩；对于平移关节，就是关节力。

为什么这个方法强大？因为它把复杂的矢量力学问题（受力分析）转化成了相对标量的能量计算问题。我们不需要去画每个连杆的隔离体受力图，不需要纠结铰链处的内力（这些内力在拉格朗日方程中不会出现），只需要按部就班地做三件事：

1. 用广义坐标（关节角）表示出系统中所有运动部件的动能T。
2. 用广义坐标表示出系统的总势能V（主要是重力势能）。
3. 把L=T-V代入上面的拉格朗日方程，对每个关节进行求导运算。

这个过程虽然计算量可能大，但步骤清晰，不易出错，特别适合在数学软件（如Matlab、Mathematica）中符号推导。它让你清晰地看到，最终那个复杂的动力学方程里的每一项（惯性项、科氏力和离心力项、重力项）到底是从哪个能量项里“长”出来的。

注意：拉格朗日方程默认推导出的是“完整、理想、有势系统”的方程。对于机器人来说，“完整”指约束只与位置有关（我们的关节就是这样的约束），“理想”指约束反力不做功（光滑铰链满足），“有势”指主动力可以写成势能的负梯度（重力、弹簧力等满足）。电机驱动力τ是作为非有势力直接写在方程右边的。这完美契合了刚性连杆机器人的假设。

3. 庖丁解牛：一步步拆解单自由度机器人动力学

理论说多了容易懵，我们直接上手算一个最简单的例子：一个在竖直平面内旋转的单连杆机器人，也叫单摆。它只有一个旋转关节，连杆长度为l，质量为m，质心在连杆末端（为简化），转动惯量为I，关节处有驱动扭矩τ。

3.1 第一步：确定广义坐标显然，广义坐标就是连杆与竖直向下方向的夹角 θ。所以 q = [θ]。

3.2 第二步：计算系统动能T动能包含平动动能和转动动能。

• 质心的线速度：vc = l * θ̇ （方向垂直于连杆）。所以平动动能 T_trans = (1/2) * m * (lθ̇)^2 = (1/2) m l^2 θ̇^2。
• 绕质心的转动动能 T_rot = (1/2) I θ̇^2。 因此，总动能 T = T_trans + T_rot = (1/2)(m l^2 + I) θ̇^2。

我们可以定义一个等效的转动惯量 M = m l^2 + I， 那么 T = (1/2) M θ̇^2。看，动能是关节速度θ̇的二次函数，系数M代表了系统对加速运动的“惯性”。

3.3 第三步：计算系统势能V以关节轴心所在水平面为零势能面。连杆质心的高度为 -l cosθ（因为θ从竖直向下开始算，我们设竖直向上为正方向，所以cosθ在0到π之间是递减的）。但通常我们更习惯设竖直向下为θ=0。为了更直观，我们重新定义：设连杆与竖直向上方向夹角为θ，则质心高度为 l cosθ。重力势能 V = m g * (l cosθ)。

3.4 第四步：构造拉格朗日函数并求导L = T - V = (1/2) M θ̇^2 - m g l cosθ。

现在对广义坐标θ应用拉格朗日方程：

1. 求 ∂L/∂θ̇ = M θ̇。
2. 求 d/dt (∂L/∂θ̇) = M θ̈。
3. 求 ∂L/∂θ = m g l sinθ。
4. 代入方程：M θ̈ - ( - m g l sinθ ) = τ？ 等等，注意 ∂L/∂θ = - m g l sinθ 吗？不对，因为 V = m g l cosθ， 所以 ∂V/∂θ = -m g l sinθ， 那么 ∂L/∂θ = - ∂V/∂θ = m g l sinθ。 所以方程是：d/dt (∂L/∂θ̇) - ∂L/∂θ = τ -> M θ̈ - (m g l sinθ) = τ。

3.5 第五步：得到动力学方程整理一下：M θ̈ + m g l sinθ = τ。

这个简单的方程已经包含了动力学方程的所有核心成分：

• M θ̈： 惯性力项。表示让这个连杆产生角加速度θ̈需要克服的惯性力矩。M越大，加速越“费劲”。
• m g l sinθ： 重力项。表示重力对关节产生的力矩。当连杆水平时（θ=90°），sinθ=1，重力力矩最大；当连杆垂直时（θ=0°或180°），sinθ=0，重力力矩为0。
• τ： 关节驱动力矩，是输入。

看到了吗？没有科氏力和离心力项。因为这是单自由度系统，不存在由于不同关节运动耦合而产生的附加力。这是一个重要的伏笔。

4. 复杂度飙升：双自由度平面机器人的动力学推导

现在我们把问题升级到经典的双连杆平面机械臂。两个连杆，长度分别为l1, l2，质量分别为m1, m2，质心分别在各自连杆的中点，转动惯量为I1, I2。两个关节均为旋转关节，力矩分别为τ1, τ2。这是一个真正的多自由度系统，所有有趣的耦合项都会出现。

4.1 运动学准备：位置与速度设广义坐标 q = [θ1, θ2]^T。θ1是连杆1与水平线的夹角，θ2是连杆2与连杆1的延长线的夹角（即相对角）。

• 连杆1质心坐标： x1 = (l1/2) cosθ1, y1 = (l1/2) sinθ1。
• 连杆2质心坐标： x2 = l1 cosθ1 + (l2/2) cos(θ1+θ2), y2 = l1 sinθ1 + (l2/2) sin(θ1+θ2)。

对时间求导得到速度平方：

• v1^2 = ẋ1^2 + ẏ1^2 = (l1/2)^2 θ̇1^2。
• v2^2 = ẋ2^2 + ẏ2^2 = [ -l1 sinθ1 θ̇1 - (l2/2) sin(θ1+θ2)(θ̇1+θ̇2) ]^2 + [ l1 cosθ1 θ̇1 + (l2/2) cos(θ1+θ2)(θ̇1+θ̇2) ]^2。 展开这个式子是个体力活，但结果是：v2^2 = l1^2 θ̇1^2 + (l2/2)^2 (θ̇1+θ̇2)^2 + l1 l2 cosθ2 θ̇1 (θ̇1+θ̇2)。

4.2 动能与势能计算

• 连杆1动能： T1 = (1/2) m1 v1^2 + (1/2) I1 θ̇1^2 = (1/2) (m1*(l1/2)^2 + I1) θ̇1^2。 令 M1 = m1*(l1/2)^2 + I1。
• 连杆2动能： T2 = (1/2) m2 v2^2 + (1/2) I2 (θ̇1+θ̇2)^2。 将v2^2代入，得到： T2 = (1/2) m2 [l1^2 θ̇1^2 + (l2/2)^2 (θ̇1+θ̇2)^2 + l1 l2 cosθ2 θ̇1 (θ̇1+θ̇2)] + (1/2) I2 (θ̇1+θ̇2)^2。 令 M2 = m2*(l2/2)^2 + I2。 则 T2 = (1/2) m2 l1^2 θ̇1^2 + (1/2) M2 (θ̇1+θ̇2)^2 + (1/2) m2 l1 l2 cosθ2 θ̇1 (θ̇1+θ̇2)。

总动能 T = T1 + T2。 这是一个关于θ̇1和θ̇2的二次型。

• 势能： V = m1 g y1 + m2 g y2 = m1 g (l1/2) sinθ1 + m2 g [l1 sinθ1 + (l2/2) sin(θ1+θ2)]。

4.3 应用拉格朗日方程得到矩阵形式经过繁琐但机械的求导运算（强烈建议用符号计算软件），我们可以得到如下形式的动力学方程：

M(q) q̈ + C(q, q̇) q̇ + G(q) = τ

对于我们的两连杆机器人：

• M(q)是 2x2 的惯性矩阵，它依赖于关节位置θ2（因为cosθ2）： M11 = M1 + m2 l1^2 + M2 + m2 l1 l2 cosθ2 M12 = M21 = M2 + (1/2) m2 l1 l2 cosθ2 M22 = M2 这个矩阵是对称且正定的，物理意义是系统的广义质量。M12=M21体现了关节间的惯性耦合。
• C(q, q̇) q̇代表科氏力和离心力项。这部分最让人头疼。它可以写成矩阵C乘以速度向量q̇的形式，但矩阵C不是唯一的，常用的一种计算方式是使得矩阵Ṁ - 2C为斜对称矩阵。对于两连杆系统，这一项具体包含： 作用于关节1的项： -m2 l1 l2 sinθ2 ( θ̇1θ̇2 + (1/2)θ̇2^2 ) （这里包含了科氏力和离心力） 作用于关节2的项： (1/2) m2 l1 l2 sinθ2 θ̇1^2 （这是离心力） 可以看到，当第二个关节速度θ̇2不为零时，会在第一个关节上产生额外的力矩；当第一个关节速度θ̇1不为零时，也会在第二个关节上产生额外的力矩。这就是耦合。
• G(q)是重力项向量： G1 = (m1 g l1/2 + m2 g l1) cosθ1 + m2 g (l2/2) cos(θ1+θ2) G2 = m2 g (l2/2) cos(θ1+θ2)
• τ= [τ1, τ2]^T 是关节力矩向量。

实操心得：手工推导两连杆动力学是一次宝贵的“洗礼”。它能让你真切感受到每一项的物理意义。但超过两个连杆后，强烈建议使用如Matlab的Symbolic Toolbox或Python的SymPy进行符号推导。你的任务是正确列出动能和势能表达式，然后让计算机去处理那些容易出错的求导。推导完成后，一定要用数值例子（给一组q, q̇, q̈）代入计算左右两边是否平衡，或者用仿真软件（如Simulink、PyBullet）进行验证，这是检验模型正确性的关键一步。

5. 从二到N：建立通用多自由度机器人动力学方程

有了双自由度的基础，我们可以总结出建立任意n自由度串联机器人动力学方程的系统性方法。其方程形式依然是：

M(q) q̈ + C(q, q̇) q̇ + G(q) = τ

其中 q, q̇, q̈, τ 都是 n×1 的向量。

5.1 惯性矩阵 M(q) 的物理意义与性质M(q) 是一个 n×n 的对称正定矩阵，且是关节位置 q 的函数。

• 对称性M_ij = M_ji：源于动能表达式是速度的二次型，交叉项系数自然对称。
• 正定性： 对于任意非零速度向量 q̇，动能 T = (1/2) q̇^T M(q) q̇ > 0。这保证了系统动能始终为正，符合物理实际。
• 元素 M_ij 的含义： 可以理解为，使第j个关节产生单位加速度（q̈_j = 1）时，需要在第i个关节上施加的力矩。当 i ≠ j 时，这体现了关节间的惯性耦合。例如，一个重型连杆的加速运动，会对其他关节产生显著的动力耦合效应。

5.2 科氏力与离心力项 C(q, q̇) q̇ 的深入剖析这是动力学方程中最复杂的一项。它包含了所有与速度二次项有关的力。

• 离心力： 与自身关节速度的平方有关（如 θ̇_i^2）。它反映了由于连杆绕自身轴旋转而产生的“向外甩”的效应。
• 科氏力： 与不同关节速度的乘积有关（如 θ̇_i θ̇_j, i≠j）。它反映了由于一个关节的运动，在另一个关节上产生的附加力效应，典型例子就是地球自转对运动物体产生的偏转力。 在机器人中，这些力是真实存在的耦合效应。例如，当一个多关节机械臂高速运动时，即使你只命令最后一个关节运动，前面的关节电机也必须输出额外的力矩来“抵抗”由末端运动传递回来的科氏力和离心力，否则机器人就会发生不可预测的抖动。

C(q, q̇) 矩阵可以通过惯性矩阵 M(q) 来计算，常用的一种定义是使得矩阵Ṁ - 2C为斜对称矩阵。这个性质在控制器设计（如计算力矩控制）中非常有用，因为它与系统的能量变化率有关。

5.3 重力项 G(q) 的计算G(q) = ∂V/∂q， 即势能对广义坐标的偏导数。对于机器人，势能主要来自重力。G(q) 的计算相对直接，它只与机器人的构型 q 有关。在机器人垂直悬挂时，重力项是主要负载；在机器人处于失重或水平面运动时，此项为零。

5.4 建立方程的标准步骤对于一个新的机器人，我们可以遵循以下步骤：

1. D-H参数与正运动学： 首先确定机器人的D-H参数，推导出每个连杆的变换矩阵 i-1^T_i，进而得到每个连杆坐标系相对于基座标系的位置和姿态。
2. 速度传播（雅可比矩阵）： 计算每个连杆质心的线速度和角速度。这可以通过构造每个连杆的几何雅可比矩阵来完成，该矩阵建立了关节速度与连杆质心速度之间的关系。这是计算动能的关键。
3. 动能计算： 每个连杆的动能 T_i = (1/2) m_i v_i^T v_i + (1/2) ω_i^T I_i ω_i，其中 I_i 是在连杆质心坐标系中表示的惯性张量。总动能 T = Σ T_i。最终T一定能写成 (1/2) q̇^T M(q) q̇ 的形式，从而得到 M(q)。
4. 势能计算： V = Σ m_i g^T r_i，其中 g 是重力加速度向量（在基座标系中表示），r_i 是连杆i质心在基座标系中的位置向量。然后计算 G(q) = (∂V/∂q)^T。
5. 推导 C(q, q̇)： 利用 M(q) 通过公式计算 C(q, q̇) 矩阵的元素。常用Christoffel符号法： C_ij = Σ_{k=1}^n (1/2) ( ∂M_ij/∂q_k + ∂M_ik/∂q_j - ∂M_kj/∂q_i ) q̇_k。

6. 动力学模型的实战价值：超越理论公式

费了这么大劲推导出来的复杂方程，到底有什么用？这才是工程师最关心的问题。它的应用直接决定了机器人系统的性能天花板。

6.1 计算力矩控制（前馈控制）这是动力学模型最经典的应用。传统的PID控制是反馈控制，它等到有了误差（位置或速度偏差）才去调整输出，属于“事后补救”。而计算力矩控制是“事前预测”。 控制律通常设计为：τ = M(q) a + C(q, q̇) q̇ + G(q)其中，a 是一个新的控制输入，通常设计为： a = q̈_d + K_d (q̇_d - q̇) + K_p (q_d - q)。这里 q_d, q̇_d, q̈_d 是期望的位置、速度和加速度。 将这个控制律代入动力学方程 M q̈ + C q̇ + G = τ， 假设模型完全准确（M, C, G已知），方程就简化为：q̈ = a。 这意味着，整个复杂的、非线性的、耦合的机器人系统，被完美地“线性化”和“解耦”成了一个简单的双积分器系统！剩下的工作就是用PD控制器（K_p, K_d）去控制这个线性系统，使其跟踪期望轨迹。实测中，即使模型不完美，前馈补偿也能抵消掉80%-90%的非线性耦合项，使得反馈控制器只需要处理很小的残留误差和扰动，从而获得极佳的动态性能和稳定性。

6.2 系统仿真与数字孪生在没有实体机器人之前，一个精确的动力学模型就是你的“数字孪生”机器人。你可以在仿真环境中：

• 测试控制算法： 将你的控制器（无论是计算力矩、阻抗控制还是自适应控制）连接上动力学模型，观察其跟踪性能、鲁棒性，而无需担心损坏真实设备。
• 轨迹规划与优化： 规划一条让末端执行器走直线的轨迹很简单，但这条轨迹对关节而言是否可行？电机力矩是否饱和？能耗是否过高？通过动力学仿真，可以评估轨迹的动力学可行性，并优化轨迹使其时间最优、能耗最优或力矩最平滑。
• 预测性维护： 通过对比仿真中电机的理论力矩和实际电机的电流反馈，可以推断传动部件的磨损（如齿轮间隙变大导致力矩波动）、负载的变化等。

6.3 参数辨识与模型校准理论模型中的参数（质量、质心位置、惯性张量、摩擦系数）往往不准确。我们可以通过让机器人执行一组精心设计的激励轨迹，并记录关节的位置、速度和电流（换算为力矩），利用最小二乘法等系统辨识技术，反推出这些动力学参数的真实值。一个校准过的模型，能显著提升前馈控制的精度。

6.4 力矩前馈与扰动观测在工业机器人中，即便不实现完整的计算力矩控制，也普遍采用重力补偿和摩擦力补偿。即： τ = τ_PID + G(q) + F(q̇)。这能有效减轻PID控制器的负担，特别是在低速运动和点对点运动中的静止保持阶段。更高级的，还可以设计扰动观测器，将未建模的动态（如动力学模型误差、外部力）视为扰动，并估计和补偿它。

踩坑实录：忽略动力学的代价。我曾参与一个高速分拣项目，机械臂需要以超过2m/s的速度进行“点到点”运动。初期只用了位置控制，机器人到达目标点后持续低频振荡，无法稳定。增加阻尼（提高D增益）后，振荡减弱但响应变慢，且高速运动轨迹畸变。后来我们植入了基于动力学的重力补偿和惯性前馈（简化版计算力矩），振荡立刻消失，轨迹跟踪误差从±5mm降到±0.5mm以内，且电机峰值电流下降了约15%。这个案例生动说明，在动态性能要求高的场景，动力学不是“选修课”，而是“必修课”。

7. 从理论到代码：实现动力学计算的实用技巧

理论最终要落地为代码。这里分享一些在实现机器人动力学计算时的实用经验。

7.1 符号推导 vs 数值计算

• 符号推导： 使用Matlab Symbolic Toolbox或Python SymPy。优点是能得到解析表达式，便于分析、求导和生成高效代码。缺点是机器人自由度一高（>6），表达式会极度膨胀，导致代码冗长，计算效率可能反而不高。适用于模型固定、需要极致实时性的嵌入式代码生成。
• 数值计算： 使用递归牛顿-欧拉算法。这是工业界和实时控制中的主流方法。它分为两步：
  1. 向外迭代： 从基座向末端递推，计算每个连杆的速度、加速度（包含重力效应）以及由此产生的惯性力。
  2. 向内迭代： 从末端向基座递推，利用力平衡计算各关节所需的驱动力矩。 牛顿-欧拉算法计算复杂度是 O(n)，非常高效，且易于编程实现。很多机器人库（如ROS的KDL， MATLAB的Robotics System Toolbox）都内置了此算法。

7.2 惯性参数的获取这是建模准确性的基础。有几种途径：

1. CAD模型： 从SolidWorks, UG等软件中直接导出质量、质心和惯性张量。这是最方便的来源，但精度取决于建模的详细程度（是否包含螺丝、线缆等）。
2. 实验辨识： 如前所述，通过实验数据拟合。这是最准确的方法，但需要设计实验和数据处理。
3. 估算与测量： 对于简单形状的连杆，可用几何公式估算。质量可以用秤称，质心可以用悬挂法找，转动惯量可以用扭摆法测量。

7.3 摩擦模型的引入基本的拉格朗日方程没有包含摩擦。实际电机和减速器中存在复杂的摩擦：库伦摩擦（静摩擦）、粘性摩擦、Stribeck效应等。一个常用的简化模型是： τ_friction = F_c * sign(q̇) + F_v * q̇。其中F_c是库伦摩擦系数，F_v是粘性摩擦系数。这两个参数也需要通过实验辨识。在低速运动或需要精确定位的场景，摩擦补偿至关重要。

7.4 代码实现示例（概念性Python伪代码）以下是一个使用递归牛顿-欧拉算法计算逆动力学（给定q, q̇, q̈求τ）的简化伪代码框架，假设已知每个连杆的质心位置、质量、惯性张量（在连杆坐标系中表示）。

import numpy as np def inverse_dynamics(q, qd, qdd, gravity, robot_params): """ 递归牛顿-欧拉逆动力学算法 q, qd, qdd: 关节位置、速度、加速度 (n维向量) gravity: 基座标系下的重力加速度向量 [0, 0, -g] 或 [0, g, 0] 等 robot_params: 包含D-H参数、质量、质心位置、惯性张量等信息的列表/类 """ n = len(q) # 初始化变量：连杆的角速度、角加速度、线加速度、力、力矩 w = [np.zeros(3) for _ in range(n+1)] # 角速度 wd = [np.zeros(3) for _ in range(n+1)] # 角加速度 v = [np.zeros(3) for _ in range(n+1)] # 线速度 vd = [gravity.copy() for _ in range(n+1)] # 线加速度，基座加速度初始化为重力 f = [np.zeros(3) for _ in range(n+1)] # 连杆受到的作用力 t = [np.zeros(3) for _ in range(n+1)] # 连杆受到的作用力矩 tau = np.zeros(n) # 关节力矩输出 # 1. 向外迭代 (i: 0 -> n-1) for i in range(n): # 计算从连杆i到i+1的旋转矩阵 R 和位置向量 p (根据D-H参数) R = rotation_matrix(q[i], dh_params[i].alpha, ...) p = position_vector(dh_params[i].a, dh_params[i].d, ...) # 计算连杆i+1的角速度和角加速度（在连杆i+1坐标系中表示） w[i+1] = R.T @ w[i] + np.array([0, 0, qd[i]]) # 假设绕z轴旋转 wd[i+1] = R.T @ wd[i] + np.cross(R.T @ w[i], np.array([0,0,qd[i]])) + np.array([0,0,qdd[i]]) # 计算连杆i+1质心的线加速度 vd[i+1] = R.T @ (vd[i] + np.cross(wd[i], p) + np.cross(w[i], np.cross(w[i], p))) # 计算作用在连杆i+1质心上的惯性力/力矩 F = m[i] * vd[i+1] # m[i]为连杆i+1的质量 N = I[i] @ wd[i+1] + np.cross(w[i+1], I[i] @ w[i+1]) # I[i]为连杆i+1的惯性张量 f[i+1] = F t[i+1] = N # 2. 向内迭代 (i: n-1 -> 0) for i in range(n-1, -1, -1): # 同样获取 R, p，以及连杆i+1质心相对于关节i+1的位置向量 r R = rotation_matrix(q[i], dh_params[i].alpha, ...) p = position_vector(...) r = ... # 从关节i+1指向连杆i+1质心的向量 # 计算作用在关节i+1上的力/力矩（从连杆i+1传递到连杆i） f_i = R @ f[i+1] + ... # 还需要加上连杆i+1自身的惯性力？这里简化了，实际是力的平衡 t_i = R @ t[i+1] + np.cross(p, R @ f[i+1]) + np.cross(r, f[i+1]) + ... # 力矩平衡 # 提取关节i的驱动力矩（假设旋转关节，力矩在z轴分量） tau[i] = t_i[2] # 假设关节轴为z轴 # 将力/力矩传递给前一个连杆（如果需要） f[i] = f[i] + f_i # 简化表示，实际是矢量叠加 t[i] = t[i] + t_i + np.cross(..., f_i) # 简化表示 return tau

注意事项：以上是高度简化的伪代码，旨在展示算法流程。实际实现需要严格处理坐标系变换、向量在不同坐标系下的表达、以及力/力矩的传递关系。强烈建议参考《Robot Dynamics and Control》by Spong 或《Rigid Body Dynamics Algorithms》by Featherstone 中的详细算法描述，或者直接使用成熟的动力学库。自己从头实现一遍对于理解算法大有裨益，但用于生产环境，建议使用经过充分测试的库。