《Python机器人基础原理》第十六篇：零部件装配——修改连杆质量参数，仿真连杆重量变化，观察各关节力矩变化趋势

 

1. 实际工业应用场景

 

周二上午，某汽车零部件装配线例行维护后重启。一台4-DOF垂直关节装配机械臂（3旋转+1垂直平移，构型与第十四、十五篇同系但增加第三旋转轴）负责将变速箱齿轮组从托盘抓取并压装到壳体中。机器人构型：大臂长600 mm，小臂长450 mm，第三旋转关节（腕部俯仰）偏置距0 mm，z轴垂直行程±200 mm。关节硬件限位：θ₁ ±170°、θ₂ ±135°、θ₃ ±150°、d₄ ±200 mm。连杆质量原始设计值：大臂4.2 kg（质心距基座0.28 m），小臂2.8 kg（质心距肘部0.20 m），腕部1.2 kg（质心距肘部0.40 m），末端执行器+齿轮组负载5.5 kg。额定负载8 kg。

 

维护工把大臂拆下来更换了内部谐波减速器。装回去的时候，在大臂末端多缠了两圈线缆护套——为了防磨损，多用了约0.6 kg的尼龙编织带。没人把这个当回事。

 

下午两点，第一件产品压装偏位0.4 mm，报废。

 

小王接到报警赶到现场。他检查了视觉定位系统——正常。检查了末端夹具气压——正常。检查了轨迹程序——和上个班次一模一样。

 

"低速手动模式下没问题，"他对旁边的新员工说，"但自动模式下速度一上去，第二轴就出现跟踪误差，末端放不到位。"

 

新员工翻了翻维护记录："上午换过减速器。会不会是装配问题？"

 

"减速器本身没问题，回零精度正常。但我注意到一件事——"小王调出伺服驱动器的实时力矩曲线，"第二轴的峰值力矩从原来的19.7 N·m跳到了22.8 N·m。大臂换了减速器，多缠了0.6 kg线缆，质心还往后移了3 cm。"

 

"质量变化会影响第二轴？"

 

"第二轴直接承受大臂传来的重力矩。大臂质量增加0.6 kg、力臂增加0.03 m，重力力矩增量约0.6×9.81×0.31≈1.8 N·m。加上惯性项也跟着变大，第二轴总峰值多了约3.1 N·m。控制器里的动力学前馈模型还是旧参数——它以为大臂只有4.2 kg，实际已经4.8 kg了。前馈补偿少算了这部分，伺服在高速换向时要靠反馈PID硬追，跟踪误差就冒出来了。"

 

"那第三轴呢？"

 

"第三轴是腕部俯仰，它的重力项里含有(m₂+m₃+m₄)·L₂·cos(θ₁+θ₂+θ₃)——大臂质量变化不改变第三轴的重力矩（大臂力臂通过L₁，不经过L₂）。但在质点模型下，第三轴惯性项M₃₃只含腕部和末端质量，不受大臂质量影响。所以第三轴力矩不会直接变大——真正出问题的是第二轴跟踪误差传递到末端，导致整体定位偏。"

 

小王打开笔记本电脑："我需要做一个参数敏感性仿真：把大臂质量从4.2 kg逐步改到5.5 kg，观察各关节力矩的变化趋势。验证第二轴力矩的线性增长，同时确认第三轴在质点模型下确实不受大臂质量影响——这样就能确定故障根源在第二轴前馈失准，而不是第三轴过载。"

 

本篇解决的问题：基于拉格朗日动力学方程 τ = M(q)q¨ + C(q,q˙)q˙ + G(q)，在固定轨迹下，系统性地修改连杆质量参数（大臂、小臂、腕部、末端负载），仿真各关节力矩随质量变化的趋势。量化"连杆重量变化对关节负载的影响程度"，为产线维护后的参数更新提供依据。本篇将连杆视为集中质量（质点模型），明确声明此简化下第三轴惯性项与θ₁、θ₂质量解耦。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于连杆质量参数变化对关节力矩影响的敏感性分析。不可直接用于真实机器人控制器、伺服驱动器或装配产线。真实工业现场需结合在线参数辨识、关节扭矩传感器反馈与安全监控。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

课程模块 本篇对应的工程痛点

第4章 机器人动力学建模 → 4.2 连杆坐标系与D-H参数 连杆质量/质心位置是动力学计算的前提输入，参数错误直接导致力矩计算偏差

第4章 → 4.4 关节空间动力学方程 τ = M(q)q¨ + C(q,q˙)q˙ + G(q) 重力项G(q)和质量参数线性相关，惯性项M(q)也含质量项

第5章 机器人关节驱动与控制 → 5.3 前馈补偿 控制器内动力学模型参数与实际不符时，前馈补偿失准→跟踪误差

第7章 机器人非线性控制 → 7.2 自适应控制 质量参数变化属于"慢时变不确定性"，需要在线辨识或自适应律

 

本篇代码解决 L3 离线参数敏感性分析层。输入不同连杆质量参数组合，输出各关节力矩随质量变化的曲线。代码不是设备实时控制器，不做伺服闭环，不做在线参数辨识。它做的事情是：在离线环境中量化"质量变化→力矩变化"的映射关系，告诉维护工程师"换了个减速器多缠了线缆，力矩会偏多少，需不需要重新标定"。

 

不覆盖：完整6轴动力学、摩擦力辨识、实时自适应控制律、力控制。

 

3. 核心逻辑讲解

 

物理直觉

 

你背过书包吗？书包里多放两本书，你肩膀就多承受几斤。机械臂也一样——大臂上多缠了0.6 kg线缆，大臂关节（第一轴）要多花力气抬起来，小臂关节（第二轴）要花力气支撑大臂传过来的力矩。第三轴（腕部俯仰）在质点模型下不直接承受大臂质量变化——它的重力臂走的是L₂（小臂长度），不经过L₁（大臂长度）。但第二轴力矩变了，控制器前馈补偿如果还是旧参数，伺服就要靠PID硬追，跟踪误差就冒出来，最终体现在末端定位偏位上。

 

关键是：质量变化对不同关节的影响不是均匀的，也不是每个关节都受影响。第一轴直接承受大臂自重，影响最大。第二轴承受大臂+小臂的重力矩，大臂质量变化会线性传递。第三轴在质点模型下与大臂质量解耦——这是本篇要验证的一个重要结论。

 

业务逻辑 → 代码映射

 

输入：基准质量参数 + 质量变化范围（如大臂+0~2.0 kg）

  ↓

步骤1：生成N组质量参数组合（单变量扫描 or 全组合）

  ↓

步骤2：对每组参数，在固定梯形轨迹下运行动力学求解

  ↓

步骤3：记录各关节的峰值力矩（τ_M峰值、τ_C峰值、τ_G峰值、τ_total峰值）

  ↓

步骤4：绘制力矩 vs 质量变化曲线

  ↓

步骤5：计算灵敏度系数（每kg质量变化引起的力矩变化量，单位N·m/kg）

  ↓

步骤6：输出对比图表

 

关键公式文字翻译

 

重力项 G(q) 与质量的关系：τ_G1 = -g·[m₁·com₁·cosθ₁ + m₂·(L₁·cosθ₁ + com₂·cos(θ₁+θ₂)) + (m₃+m₄)·(L₁·cosθ₁ + L₂·cos(θ₁+θ₂+θ₃))]。每一项都是"质量×力臂×重力×三角函数"。质量增加Δm，力矩直接增加Δm·g·力臂。线性关系。

 

惯性矩阵 M(q) 与质量的关系：M₁₁中 m₁·com₁² 项随m₁线性变化。M₃₃ = m₃·com₃² + m₄·(com₃+L₂)²——只含腕部和末端质量，不含m₁、m₂。这是质点模型下第三轴与大臂/小臂质量解耦的数学体现。

 

科氏力项与质量的关系：τ_C ∝ h·sinθ₂·(2θ₁˙θ₂˙+θ₂˙²)，其中 h = m₂·L₁·com₂ + (m₃+m₄)·L₁·L₂。h中不含m₁——大臂质量变化不影响科氏力项。

 

总结：在质点模型下，大臂质量m₁只影响τ_G1、τ_G2（通过L₁力臂项）和τ_M中的M₁₁项。对第三轴完全无直接影响。所有受影响项与质量都是线性关系——扫几个点就能外推。

 

单位：力矩 N·m，质量 kg，力臂 m，角速度 rad/s。

 

4. OOP代码实现

 

技术栈

 

严格限定：numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。

 

项目结构树

 

robot_mass_sensitivity_sim/

├── run_mass_sensitivity_sim.py

├── requirements.txt

├── dynamics/

│ ├── __init__.py

│ ├── models.py

│ ├── trajectory.py

│ ├── dynamics_solver.py

│ └── visualize.py

├── tests/

│ ├── __init__.py

│ └── test_mass_sensitivity_sim.py

└── results/

 

核心源码

 

"dynamics/models.py" — dataclass数据模型

 

"""

4-DOF垂直关节装配机械臂数据模型

连杆参数、质量、质心位置、关节限位、重力常量

 

构型说明：3旋转+1垂直平移，与第十四/十五篇同系

θ1: 基座偏航, θ2: 肘部偏航, θ3: 腕部俯仰, d4: z轴升降

"""

 

from dataclasses import dataclass, field

from typing import List, Tuple

import numpy as np

 

 

@dataclass

class JointLimit:

    """单关节硬件限位"""

    min_val: float

    max_val: float

    joint_name: str = ""

 

    def __post_init__(self):

        if self.min_val >= self.max_val:

            raise ValueError(

                f"JointLimit: min_val({self.min_val})必须严格小于"

                f"max_val({self.max_val})")

        if not np.isfinite(self.min_val) or not np.isfinite(self.max_val):

            raise ValueError("JointLimit: 限位值必须为有限数值")

 

    def contains(self, value: float) -> bool:

        return (self.min_val - 1e-10) <= value <= (self.max_val + 1e-10)

 

 

@dataclass

class JointLimitSet:

    """4-DOF垂直关节装配机械臂关节限位集合"""

    limits: List[JointLimit]

 

    def __post_init__(self):

        if len(self.limits) != 4:

            raise ValueError(f"需要4个关节限位, 实际{len(self.limits)}个")

 

    @classmethod

    def default_4dof_assembly(cls):

        """

        默认4-DOF垂直关节装配机械臂限位

        θ1: ±170° θ2: ±135° θ3: ±150° d4: ±200 mm

        """

        pi = 3.141592653589793

        return cls(limits=[

            JointLimit(-170 * pi / 180, 170 * pi / 180, 'joint1_base_yaw'),

            JointLimit(-135 * pi / 180, 135 * pi / 180, 'joint2_elbow_yaw'),

            JointLimit(-150 * pi / 180, 150 * pi / 180, 'joint3_wrist_pitch'),

            JointLimit(-0.200, 0.200, 'joint4_z_lift'),

        ])

 

    def check_all_with_indices(self, joint_values: List[float]) -> Tuple[bool, List[int]]:

        if len(joint_values) != 4:

            raise ValueError(f"需要4个关节值, 实际{len(joint_values)}个")

        all_pass = True

        violation_indices = []

        for i, (val, limit) in enumerate(zip(joint_values, self.limits)):

            if not limit.contains(val):

                all_pass = False

                violation_indices.append(i)

        return all_pass, violation_indices

 

 

@dataclass

class LinkMassProps:

    """

    连杆质量与质心属性

    mass: 质量 (kg)

    com_x, com_y, com_z: 质心在连杆坐标系中的位置 (m)

    本篇将连杆视为集中质量（质点模型），忽略绕质心转动惯量

    """

    mass: float

    com_x: float

    com_y: float

    com_z: float

    link_name: str = ""

 

    def __post_init__(self):

        if self.mass < 0:

            raise ValueError(f"LinkMassProps: 质量不能为负, 实际{self.mass}")

        for name, val in [('com_x', self.com_x), ('com_y', self.com_y),

                           ('com_z', self.com_z)]:

            if not np.isfinite(val):

                raise ValueError(f"LinkMassProps.{name}必须为有限数值")

 

 

@dataclass

class MassPropertySet:

    """完整连杆质量属性集合"""

    link_props: List[LinkMassProps]

 

    def __post_init__(self):

        if len(self.link_props) != 4:

            raise ValueError(f"需要4个连杆质量属性, 实际{len(self.link_props)}个")

 

    @classmethod

    def default_assembly_arm(cls):

        """

        4-DOF垂直关节装配机械臂质量参数

        大臂: 4.2 kg, 质心距基座0.28 m

        小臂: 2.8 kg, 质心距肘部0.20 m

        腕部: 1.2 kg, 质心距肘部0.40 m（在L2=0.45m范围内，合理）

        末端执行器+齿轮组: 吸盘0.8 kg + 齿轮组4.7 kg = 5.5 kg

        """

        return cls(link_props=[

            LinkMassProps(4.2, 0.28, 0.0, 0.0, 'link1_upper_arm'),

            LinkMassProps(2.8, 0.20, 0.0, 0.0, 'link2_forearm'),

            LinkMassProps(1.2, 0.40, 0.0, 0.0, 'link3_wrist'),

            LinkMassProps(5.5, 0.0, 0.0, 0.0, 'link4_end_effector'),

        ])

 

    def copy_with_modified_mass(self, link_index: int, new_mass: float):

        """

        返回修改后的质量属性副本（不修改原对象）

        link_index: 0=大臂, 1=小臂, 2=腕部, 3=末端

        使用deepcopy语义，保证多次调用不互相污染

        """

        new_props = []

        for i, prop in enumerate(self.link_props):

            if i == link_index:

                new_props.append(LinkMassProps(

                    new_mass, prop.com_x, prop.com_y, prop.com_z, prop.link_name))

            else:

                new_props.append(LinkMassProps(

                    prop.mass, prop.com_x, prop.com_y, prop.com_z, prop.link_name))

        return MassPropertySet(link_props=new_props)

 

 

@dataclass

class DHParam:

    """标准D-H参数"""

    theta: float

    d: float

    a: float

    alpha: float

 

    def __post_init__(self):

        for name, val in [('theta', self.theta), ('d', self.d),

                           ('a', self.a), ('alpha', self.alpha)]:

            if not np.isfinite(val):

                raise ValueError(f"DHParam.{name}必须为有限数值")

 

 

@dataclass

class RobotParams:

    """

    4-DOF垂直关节装配机械臂完整参数

    大臂600mm, 小臂450mm, 第三轴偏置0mm, d4=300mm基座高

    """

    links: List[DHParam]

    joint_names: List[str] = field(default_factory=lambda: [

        'joint1_base_yaw', 'joint2_elbow_yaw',

        'joint3_wrist_pitch', 'joint4_z_lift'

    ])

    g: float = 9.81

 

    @classmethod

    def default_assembly_arm(cls):

        pi = 3.141592653589793

        links = [

            DHParam(0.0, 0.300, 0.600, 0.0),

            DHParam(0.0, 0.0, 0.450, 0.0),

            DHParam(0.0, 0.0, 0.0, 0.0),

            DHParam(0.0, 0.0, 0.0, 0.0),

        ]

        return cls(links=links)

 

"dynamics/trajectory.py" — 独立梯形速度轨迹生成器（修复减速段bug）

 

"""

独立梯形速度轨迹生成器：各关节按位移比例独立计算加速/匀速/减速时长

修复：减速段时长 = v_max / |a_max|，不再复用t_acc

命名：IndependentTrapezoidalTrajectory（各关节参数独立）

"""

 

import numpy as np

 

 

class IndependentTrapezoidalTrajectory:

    """

    独立梯形速度轨迹：每个关节根据自身位移独立计算梯形参数

    保证所有关节在各自时间内精确到达目标，无超调、无速度反向

    """

 

    def __init__(self, q_start, q_end, total_time, max_velocity, max_acceleration):

        """

        q_start: 起始关节角度 [θ1, θ2, θ3, d4]

        q_end: 终止关节角度

        total_time: 总运动时间 (s)，各关节共享（时间同步）

        max_velocity: 峰值关节角速度上限 (rad/s)

        max_acceleration: 峰值关节角加速度上限 (rad/s²)

        """

        self.q_start = np.array(q_start, dtype=float)

        self.q_end = np.array(q_end, dtype=float)

        self.total_time = total_time

        self.max_velocity = max_velocity

        self.max_acceleration = max_acceleration

 

        self.delta_q = self.q_end - self.q_start

 

        # 为每个关节计算独立的梯形参数

        # 核心修复：减速段时长 t_dec = v_max / |a_max|，独立于 t_acc

        self.joint_params = []

 

        for i in range(4):

            dq = self.delta_q[i]

            if abs(dq) < 1e-10:

                self.joint_params.append({

                    'v_max': 0.0, 'a_max': 0.0,

                    't_acc': 0.0, 't_const': total_time,

                    't_dec': 0.0, 'direction': 1.0

                })

                continue

 

            direction = 1.0 if dq > 0 else -1.0

            abs_dq = abs(dq)

 

            # 理论加速段时长

            t_acc_theory = max_velocity / max_acceleration

 

            if 2 * t_acc_theory < total_time:

                # 尝试使用全局速度上限

                v_candidate = max_velocity

                t_acc = t_acc_theory

                t_dec = t_acc_theory # 减速段同样时长

                t_const = total_time - t_acc - t_dec

 

                # 校验：加速段位移 + 匀速段位移 + 减速段位移 >= 总位移

                # 加速段位移 = 0.5 * a * t_acc² = 0.5 * v_candidate * t_acc

                # 减速段位移 = 0.5 * v_candidate * t_dec

                accel_dist = 0.5 * v_candidate * t_acc

                decel_dist = 0.5 * v_candidate * t_dec

                const_dist = v_candidate * t_const if t_const > 0 else 0.0

 

                if accel_dist + const_dist + decel_dist < abs_dq - 1e-6:

                    # 位移太大，需要降低速度

                    # 三角形轨迹：加速到v_peak后立即减速

                    # dq = 0.5 * a * t_acc² + 0.5 * v_peak * t_dec

                    # 且 v_peak = a * t_acc = |a| * t_dec

                    # 设 t_acc = t_dec = t_half, 则 dq = a * t_half²

                    t_half = np.sqrt(abs_dq / max_acceleration)

                    if 2 * t_half > total_time:

                        t_half = total_time / 2.0

                    v_candidate = max_acceleration * t_half

                    t_acc = t_half

                    t_dec = t_half

                    t_const = total_time - 2 * t_half

                    if t_const < 0:

                        t_const = 0.0

                elif t_const < 0:

                    t_const = 0.0

                    # 退化为三角形

                    t_half = total_time / 2.0

                    v_candidate = max_acceleration * t_half

                    t_acc = t_half

                    t_dec = t_half

            else:

                # 总时间不够完整梯形，退化为三角形

                t_half = total_time / 2.0

                v_candidate = max_acceleration * t_half

                t_acc = t_half

                t_dec = t_half

                t_const = 0.0

 

            self.joint_params.append({

                'v_max': direction * v_candidate,

                'a_max': direction * max_acceleration,

                't_acc': t_acc,

                't_const': t_const,

                't_dec': t_dec,

                'direction': direction

            })

 

    def evaluate(self, t):

        """

        在时刻t计算关节位置、速度、加速度

        所有关节在 t>=total_time 时精确停在 q_end，无跳变、无速度反向

        """

        if t <= 0:

            return self.q_start.copy(), np.zeros(4), np.zeros(4)

        if t >= self.total_time:

            return self.q_end.copy(), np.zeros(4), np.zeros(4)

 

        q = np.zeros(4)

        q_dot = np.zeros(4)

        q_ddot = np.zeros(4)

 

        for i in range(4):

            dq = self.delta_q[i]

            if abs(dq) < 1e-10:

                q[i] = self.q_start[i]

                continue

 

            params = self.joint_params[i]

            v_max = params['v_max']

            a_max = params['a_max']

            t_acc = params['t_acc']

            t_const = params['t_const']

            t_dec = params['t_dec']

 

            if t <= t_acc:

                # 加速段

                q_ddot[i] = a_max

                q_dot[i] = a_max * t

                q[i] = self.q_start[i] + 0.5 * a_max * t**2

            elif t <= t_acc + t_const:

                # 匀速段

                q_ddot[i] = 0.0

                q_dot[i] = v_max

                q[i] = (self.q_start[i]

                        + 0.5 * a_max * t_acc**2

                        + v_max * (t - t_acc))

            elif t <= t_acc + t_const + t_dec:

                # 减速段：时长 = t_dec, 减速度 = -|a_max|*direction

                t_dec_elapsed = t - t_acc - t_const

                a_dec = -abs(a_max) * params['direction']

                q_ddot[i] = a_dec

                q_dot[i] = v_max + a_dec * t_dec_elapsed

                # 速度在减速段结束时精确归零

                q[i] = (self.q_start[i]

                        + 0.5 * a_max * t_acc**2

                        + v_max * t_const

                        + v_max * t_dec_elapsed

                        + 0.5 * a_dec * t_dec_elapsed**2)

            else:

                # 安全兜底：应在 t_acc+t_const+t_dec 时已到达终点

                q[i] = self.q_end[i]

                q_dot[i] = 0.0

                q_ddot[i] = 0.0

 

        return q, q_dot, q_ddot

 

    def generate_trajectory_array(self, dt=0.01):

        """生成完整轨迹数组"""

        times = np.arange(0, self.total_time + dt, dt)

        n = len(times)

 

        q_array = np.zeros((n, 4))

        q_dot_array = np.zeros((n, 4))

        q_ddot_array = np.zeros((n, 4))

 

        for i, t in enumerate(times):

            q, qd, qdd = self.evaluate(t)

            q_array[i, :] = q

            q_dot_array[i, :] = qd

            q_ddot_array[i, :] = qdd

 

        return times, q_array, q_dot_array, q_ddot_array

 

"dynamics/dynamics_solver.py" — 动力学求解器（核心业务类）

 

"""

动力学求解器：计算惯性项、科氏力/离心力项、重力项

支持修改连杆质量参数，仿真质量变化对各关节力矩的影响

τ = M(q)q¨ + C(q,q˙)q˙ + G(q)

 

本篇将连杆视为集中质量（质点模型），忽略绕质心转动惯量。

4-DOF垂直关节臂：前3个旋转关节参与平面内动力学，第4关节为z平移。

物理声明：

  - 第三轴（腕部俯仰）在质点模型下与大臂质量m₁解耦

  - M₃₃只含m₃、m₄，不含m₁、m₂

  - τ_G3含(m₃+m₄)·L₂项，不含m₁

  - τ_C3 = 0（第三轴与前两轴无速度耦合）

"""

 

import numpy as np

from dynamics.models import RobotParams, MassPropertySet

 

 

class DynamicsSolver:

    """

    基于拉格朗日方程的3旋转+1平移垂直关节臂动力学求解器

    输入4关节状态，前3个旋转关节参与M/C/G计算

    z平移关节（关节4）仅产生轴向力，不影响旋转关节力矩

    """

 

    def __init__(self, params: RobotParams, mass_props: MassPropertySet):

        self.params = params

        self.mass_props = mass_props

        self.g = params.g

 

        # 提取参数（前3个旋转关节）

        self.L1 = params.links[0].a

        self.L2 = params.links[1].a

        self.m1 = mass_props.link_props[0].mass

        self.m2 = mass_props.link_props[1].mass

        self.m3 = mass_props.link_props[2].mass

        self.m4 = mass_props.link_props[3].mass

        self.com1 = mass_props.link_props[0].com_x

        self.com2 = mass_props.link_props[1].com_x

        self.com3 = mass_props.link_props[2].com_x

 

        # 科氏力耦合系数 h = m₂·L₁·com₂ + (m₃+m₄)·L₁·L₂

        # 不含m₁——大臂质量不影响科氏力项

        self.h = (self.m2 * self.L1 * self.com2

                  + (self.m3 + self.m4) * self.L1 * self.L2)

 

    def inertia_matrix(self, theta2, theta3):

        """

        计算3×3惯性矩阵 M(q)（前3个旋转关节）

        本篇将连杆视为集中质量，忽略绕质心转动惯量

        物理声明：M₃₃不含m₁、m₂，第三轴与大臂/小臂质量解耦

        """

        c2 = np.cos(theta2)

        c23 = np.cos(theta2 + theta3)

 

        # M₁₁ = m₁·com₁² + m₂·(L₁²+com₂²+2L₁·com₂·cosθ₂)

        # + (m₃+m₄)·(L₁²+L₂²+2L₁·L₂·cos(θ₂+θ₃))

        M11 = (self.m1 * self.com1**2

               + self.m2 * (self.L1**2 + self.com2**2 + 2*self.L1*self.com2*c2)

               + (self.m3 + self.m4) * (self.L1**2 + self.L2**2

                                        + 2*self.L1*self.L2*c23))

 

        # M₁₂ = M₂₁ = m₂·(com₂²+L₁·com₂·cosθ₂)

        # + (m₃+m₄)·(L₂²+L₁·L₂·cos(θ₂+θ₃))

        M12 = (self.m2 * (self.com2**2 + self.L1*self.com2*c2)

               + (self.m3 + self.m4) * (self.L2**2 + self.L1*self.L2*c23))

 

        M13 = 0.0 # 第三轴与前两轴在质点模型下无惯性耦合

 

        # M₂₂ = m₂·com₂² + (m₃+m₄)·L₂²

        M22 = self.m2 * self.com2**2 + (self.m3 + self.m4) * self.L2**2

 

        M23 = 0.0

 

        # M₃₃ = m₃·com₃² + m₄·(com₃+L₂)²

        # 只含腕部和末端质量，不含m₁、m₂

        M33 = self.m3 * self.com3**2 + self.m4 * (self.com3 + self.L2)**2

 

        return np.array([[M11, M12, M13],

                         [M12, M22, M23],

                         [M13, M23, M33]], dtype=float)

 

    def coriolis_vector(self, theta2, theta3, theta1_dot, theta2_dot, theta3_dot):

        """

        计算科氏力/离心力向量 τ_C = C(q,q˙)·q˙（前3关节）

        质点模型下第三轴与前两轴无速度耦合，τ_C3 = 0

        """

        s2 = np.sin(theta2)

        s23 = np.sin(theta2 + theta3)

 

        # τ_C1 = -h·sinθ₂·(2·θ₁˙·θ₂˙ + θ₂˙²)

        tau_C1 = -self.h * s2 * (2 * theta1_dot * theta2_dot + theta2_dot**2)

 

        # τ_C2 = h·sinθ₂·θ₁˙²

        tau_C2 = self.h * s2 * theta1_dot**2

 

        # τ_C3 = 0（质点模型下腕部俯仰与前两轴无科氏耦合）

        tau_C3 = 0.0

 

        return np.array([tau_C1, tau_C2, tau_C3], dtype=float)

 

    def gravity_vector(self, theta1, theta2, theta3):

        """

        计算重力项向量 G(q)（前3关节）

        τ_G1含m₁·com₁项（大臂自重直接作用）

        τ_G2含m₂·com₂ + (m₃+m₄)·L₂项

        τ_G3含(m₃+m₄)·L₂·cos(θ₁+θ₂+θ₃)项——不含m₁

        """

        c1 = np.cos(theta1)

        c12 = np.cos(theta1 + theta2)

        c123 = np.cos(theta1 + theta2 + theta3)

 

        # τ_G1 = -g·[m₁·com₁·cosθ₁ + m₂·(L₁·cosθ₁+com₂·cos(θ₁+θ₂))

        # + (m₃+m₄)·(L₁·cosθ₁+L₂·cos(θ₁+θ₂+θ₃))]

        tau_G1 = -self.g * (

            self.m1 * self.com1 * c1

            + self.m2 * (self.L1 * c1 + self.com2 * c12)

            + (self.m3 + self.m4) * (self.L1 * c1 + self.L2 * c123)

        )

 

        # τ_G2 = -g·[m₂·com₂·cos(θ₁+θ₂) + (m₃+m₄)·L₂·cos(θ₁+θ₂+θ₃)]

        tau_G2 = -self.g * (

            self.m2 * self.com2 * c12

            + (self.m3 + self.m4) * self.L2 * c123

        )

 

        # τ_G3 = -g·[(m₃+m₄)·L₂·cos(θ₁+θ₂+θ₃)]

        # 不含m₁——大臂质量不直接影响第三轴重力矩

        tau_G3 = -self.g * (

            (self.m3 + self.m4) * self.L2 * c123

        )

 

        return np.array([tau_G1, tau_G2, tau_G3], dtype=float)

 

    def compute_all_terms(self, q, q_dot, q_ddot):

        """

        计算完整动力学方程的各项分解

        输入4关节状态，前3个旋转关节参与计算

        第4关节（z平移）不影响旋转关节力矩

        返回：(tau_M, tau_C, tau_G, tau_total)

        全部为3元素数组，单位 N·m

        """

        if len(q) < 3 or len(q_dot) < 3 or len(q_ddot) < 3:

            raise ValueError("需要至少3个关节的角度、速度、加速度")

 

        theta1 = q[0]

        theta2 = q[1]

        theta3 = q[2]

        theta1_dot = q_dot[0]

        theta2_dot = q_dot[1]

        theta3_dot = q_dot[2]

        theta1_ddot = q_ddot[0]

        theta2_ddot = q_ddot[1]

        theta3_ddot = q_ddot[2]

 

        # 惯性项

        M = self.inertia_matrix(theta2, theta3)

        tau_M = M @ np.array([theta1_ddot, theta2_ddot, theta3_ddot])

 

        # 科氏力/离心力项

        tau_C = self.coriolis_vector(theta2, theta3, theta1_dot, theta2_dot, theta3_dot)

 

        # 重力项

        tau_G = self.gravity_vector(theta1, theta2, theta3)

 

        # 总力矩

        tau_t

利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！