《Python机器人基础原理》第十七篇：产线仿真——修改连杆质心位置，基于动力学术解，统计质心偏移对关节力矩的影响

 

1. 实际工业应用场景

 

周三夜班，某3C电子装配产线的例行保养窗口。一台4-DOF垂直关节装配机械臂（3旋转+1垂直平移）负责将手机中框从传送带拾取并放置到测试治具中。机器人构型：大臂长500 mm，小臂长350 mm，第三旋转关节（腕部俯仰）偏置距0 mm，z轴垂直行程±150 mm。关节硬件限位：θ₁ ±175°、θ₂ ±150°、θ₃ ±150°、d₄ ±150 mm。连杆质量：大臂3.5 kg（质心距基座0.22 m），小臂2.0 kg（质心距肘部0.15 m），腕部0.8 kg（质心距腕关节0.30 m，即距肘部0.65 m——腕部模块本身较长），末端真空吸盘+手机中框负载2.0 kg。额定负载3 kg。

 

保养时，维修工拆下小臂进行清洁，重新安装时把末端气管支架从原来的内侧位置挪到了外侧——为了走线更顺。没人注意到这个细节。

 

周四早班第一件产品，中框放歪了0.6 mm，卡进治具时刮伤了边框。

 

小李接到品质异常通知赶到现场。他检查了吸盘真空度——正常。检查了视觉对位——正常。检查了传送带定位——正常。

 

"手动慢速模式下没问题，"他对旁边的新员工说，"但自动模式下节拍一拉起来，第二轴在换向段就开始抖，放下去的位置就不对了。"

 

新员工翻了翻保养记录："昨晚拆过小臂做清洁。"

 

"小臂拆过……"小李调出伺服驱动器的实时力矩曲线，"第二轴的峰值力矩从原来的14.2 N·m跳到了15.7 N·m。小臂质量没变——还是2.0 kg。但质心位置可能偏了。"

 

"质心偏了也会影响？"

 

"当然。重力力矩 = 质量×力臂×重力。质量不变，力臂变了——气管支架从内侧挪到外侧，小臂质心外移了大概4 cm。重力力矩增量 = 2.0×9.81×0.04 ≈ 0.78 N·m。加上惯性项里com₂²项的变化和科氏力项h里com₂的线性变化，总增量大约1.5 N·m左右。控制器里的前馈模型还是旧质心参数，补偿力矩偏了，伺服在高速换向时靠反馈PID硬追，就出现跟踪误差了。"

 

"那第三轴呢？"

 

"第三轴在质点模型下，重力项含m₃·com₃ + m₄·(com₃+L₂偏置)——小臂质心com₂的变化不通过L₂传递，所以第三轴重力矩基本不受影响。但前馈失准导致的第二轴跟踪误差会传递到末端，最终变成定位偏位。"

 

小李打开笔记本电脑："我需要做一个质心偏移仿真：把小臂质心从0.15 m逐步偏移到0.25 m，观察各关节力矩的变化趋势。仿真能解释其中一部分——重力项约0.78 N·m，惯性/科氏项约0.4 N·m，剩下约0.3 N·m可能是装配残余应力或摩擦变化。确认质心偏移是不是主因。"

 

本篇解决的问题：基于拉格朗日动力学方程 τ = 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)含com²非线性项

第5章 机器人关节驱动与控制 → 5.3 前馈补偿 质心偏移导致前馈模型力臂参数失准，补偿力矩偏差

第7章 机器人非线性控制 → 7.2 自适应控制 质心变化属于"慢时变不确定性"，需要定期重新标定

 

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

 

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

 

3. 核心逻辑讲解

 

物理直觉

 

你提过水桶吗？水桶里的水一样重，但你把手从桶边移到桶底提——感觉完全不一样。不是重量变了，是力臂变了。机械臂也一样。小臂质量还是2.0 kg，但气管支架从内侧挪到外侧，质心外移了4 cm。第二轴要多花力气支撑这个偏移的力臂。更关键的是，质心位置还出现在惯性矩阵的交叉项里——M₁₂ = m₂·(com₂² + L₁·com₂·cosθ₂)，com₂变大，惯性耦合增强，第一轴加速时通过M₁₂传递给第二轴的耦合力矩也变大。

 

业务逻辑 → 代码映射

 

输入：基准质心参数 + 质心偏移范围（如小臂com₂从0.15→0.25 m）

  ↓

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

  ↓

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

  ↓

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

  ↓

步骤4：绘制力矩 vs 质心偏移曲线，叠加线性拟合线展示非线性偏离

  ↓

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

  ↓

步骤6：输出对比图表

 

关键公式文字翻译

 

重力项 G(q) 与质心的关系：τ_G2 = -g·[m₂·com₂·cos(θ₁+θ₂) + m₃·com₃·cos(θ₁+θ₂+θ₃) + m₄·(com₃+L₂)·cos(θ₁+θ₂+θ₃)]。com₂出现在第一项——小臂质心越远，第二轴重力力矩越大。线性关系。

 

惯性矩阵 M(q) 与质心的关系：M₁₁中 m₂·com₂² 项随com₂平方增长（非线性！）。M₁₂中 m₂·L₁·com₂·cosθ₂ 项随com₂线性增长。这意味着质心偏移对惯性项的影响比重力项更复杂——小偏移时近似线性，大偏移时平方项开始主导。

 

科氏力项与质心的关系：h = m₂·L₁·com₂ + (m₃+m₄)·L₁·L₂。com₂增大→h增大→科氏力矩线性增大。高速时影响显著。

 

第三轴重力项与质心的关系：τ_G3 = -g·[m₃·com₃ + m₄·(com₃+L₂)]·cos(θ₁+θ₂+θ₃)。com₃从腕关节量起，m₃和m₄的重力臂都通过com₃传递。这与M₃₃ = m₃·com₃² + m₄·(com₃+L₂)²中的坐标定义一致——本篇统一com₃从第三轴（腕关节）量起。

 

总结：重力项和科氏项与质心位置线性相关，惯性矩阵中的com₂²项带来非线性效应。小偏移（<5 cm）时近似线性，大偏移时需要完整仿真。

 

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

 

4. OOP代码实现

 

技术栈

 

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

 

项目结构树

 

robot_com_sensitivity_sim/

├── run_com_sensitivity_sim.py

├── requirements.txt

├── dynamics/

│ ├── __init__.py

│ ├── models.py

│ ├── trajectory.py

│ ├── dynamics_solver.py

│ └── visualize.py

├── tests/

│ ├── __init__.py

│ └── test_com_sensitivity_sim.py

└── results/

 

核心源码

 

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

 

"""

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

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

 

构型说明：3旋转+1垂直平移

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

 

质心坐标定义（全局统一）：

  - com₁: 大臂质心距基座关节沿连杆方向的距离 (m)

  - com₂: 小臂质心距肘部关节沿连杆方向的距离 (m)

  - com₃: 腕部质心距腕关节（第三轴）沿连杆方向的距离 (m)

  - 末端负载质心默认在腕关节处 (com₄=0)

"""

 

 

from dataclasses import dataclass, field

from typing import List, Tuple, Optional

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: ±175° θ2: ±150° θ3: ±150° d4: ±150 mm

        """

        pi = 3.141592653589793

        return cls(limits=[

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

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

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

            JointLimit(-0.150, 0.150, '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: 质心沿连杆方向距近端关节的距离 (m)

    com_y, com_z: 垂直于连杆方向的质心偏移 (m)，本篇质点模型设为0

    link_name: 连杆标识

    max_com_x: 质心允许的最大x值（由调用方传入连杆长度上限）

 

    质心坐标定义统一：com_x 为沿连杆方向距近端关节的距离。

    - 大臂：距基座关节

    - 小臂：距肘部关节

    - 腕部：距腕关节（第三轴）

    """

    mass: float

    com_x: float

    com_y: float

    com_z: float

    link_name: str = ""

    max_com_x: Optional[float] = None # 连杆长度上限，None表示不校验

 

    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}必须为有限数值")

        # 质心位置合理性校验：不能超出连杆长度

        if self.max_com_x is not None and self.com_x > self.max_com_x:

            raise ValueError(

                f"LinkMassProps({self.link_name}): 质心x={self.com_x}m "

                f"超出连杆长度上限{self.max_com_x}m")

 

 

@dataclass

class MassPropertySet:

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

    link_props: List[LinkMassProps]

    link_lengths: List[float] = field(default_factory=lambda: [0.0, 0.0, 0.0, 0.0])

 

    def __post_init__(self):

        if len(self.link_props) != 4:

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

        # 交叉校验：质心不能超出对应连杆长度

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

            if length > 0 and prop.com_x > length:

                raise ValueError(

                    f"连杆{i}({prop.link_name}): 质心{prop.com_x}m超出长度{length}m")

 

    @classmethod

    def default_assembly_arm(cls):

        """

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

        大臂: 3.5 kg, 质心距基座0.22 m (L1=0.50m)

        小臂: 2.0 kg, 质心距肘部0.15 m (L2=0.35m)

        腕部: 0.8 kg, 质心距腕关节0.30 m (腕部模块长0.35m)

        末端执行器+手机中框: 吸盘0.3 kg + 中框1.7 kg = 2.0 kg, com=0

        """

        link_lengths = [0.50, 0.35, 0.35, 0.0]

        props = [

            LinkMassProps(3.5, 0.22, 0.0, 0.0, 'link1_upper_arm', link_lengths[0]),

            LinkMassProps(2.0, 0.15, 0.0, 0.0, 'link2_forearm', link_lengths[1]),

            LinkMassProps(0.8, 0.30, 0.0, 0.0, 'link3_wrist', link_lengths[2]),

            LinkMassProps(2.0, 0.0, 0.0, 0.0, 'link4_end_effector', None),

        ]

        return cls(link_props=props, link_lengths=link_lengths)

 

    def copy_with_modified_com(self, link_index: int, new_com_x: float):

        """

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

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

        new_com_x: 新的质心x坐标 (m)

        """

        new_props = []

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

            if i == link_index:

                new_props.append(LinkMassProps(

                    prop.mass, new_com_x, prop.com_y, prop.com_z,

                    prop.link_name, prop.max_com_x))

            else:

                new_props.append(LinkMassProps(

                    prop.mass, prop.com_x, prop.com_y, prop.com_z,

                    prop.link_name, prop.max_com_x))

        return MassPropertySet(link_props=new_props, link_lengths=self.link_lengths)

 

 

@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垂直关节装配机械臂完整参数

    大臂500mm, 小臂350mm, 第三轴偏置0mm, d4=250mm基座高

    """

    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):

        links = [

            DHParam(0.0, 0.250, 0.500, 0.0),

            DHParam(0.0, 0.0, 0.350, 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）

 

"""

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

修复：三角形轨迹（加速+减速即覆盖全位移）时 t_const 强制为0

命名：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

        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

 

                # 校验：加速段位移 + 减速段位移 是否足够覆盖总位移

                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:

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

                    # 先尝试三角形轨迹

                    t_half = np.sqrt(abs_dq / max_acceleration)

                    if 2 * t_half > total_time:

                        t_half = total_time / 2.0

                    v_triangle = max_acceleration * t_half

                    t_acc_new = t_half

                    t_dec_new = t_half

                    t_const_new = total_time - 2 * t_half

                    if t_const_new < 0:

                        t_const_new = 0.0

                    v_candidate = v_triangle

                    t_acc = t_acc_new

                    t_dec = t_dec_new

                    t_const = t_const_new

                elif accel_dist + decel_dist >= abs_dq - 1e-6:

                    # 加速+减速已足够覆盖位移，这是三角形轨迹

                    # 重新计算精确的 t_half 使 0.5*a*t² + 0.5*a*t² = abs_dq

                    t_half = np.sqrt(abs_dq / max_acceleration)

                    v_candidate = max_acceleration * t_half

                    t_acc = t_half

                    t_dec = t_half

                    t_const = 0.0 # 关键修复：三角形轨迹无匀速段

            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计算关节位置、速度、加速度"""

        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:

                # 匀速段（三角形轨迹时 t_const=0，此分支不进入）

                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_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 if t_const > 0 else 0.0)

                        + v_max * t_dec_elapsed

                        + 0.5 * a_dec * t_dec_elapsed**2)

            else:

                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平移。

 

质心坐标定义（与models.py统一）：

  - com₁: 大臂质心距基座关节沿连杆方向 (m)

  - com₂: 小臂质心距肘部关节沿连杆方向 (m)

  - com₃: 腕部质心距腕关节（第三轴）沿连杆方向 (m)

  - 末端负载质心默认在腕关节处

 

物理声明：

  - 第三轴重力项 τ_G3 = -g·[m₃·com₃ + m₄·(com₃+L₂)]·cos(θ₁+θ₂+θ₃)

    与 M₃₃ = m₃·com₃² + m₄·(com₃+L₂)² 使用同一com₃定义（距腕关节）

  - 第三轴与θ₁θ₂的科氏耦合在质点+共线假设下忽略（τ_C3=0）

  - 小臂质心com₂变化不影响τ_G3（力臂走L₂，不经过com₂）

"""

 

 

import numpy as np

from dynamics.models import RobotParams, MassPropertySet

 

 

class DynamicsSolver:

    """

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

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

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

 

    注意：每次修改质量/质心参数后必须创建新的DynamicsSolver实例，

    因为__init__中缓存了h等派生参数。

    """

 

    @classmethod

    def from_mass_props(cls, params: RobotParams, mass_props: MassPropertySet):

        """工厂方法：从参数创建求解器（推荐用于参数扫描）"""

        return cls(params, mass_props)

 

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

        self.params = params

        self.mass_props = mass_props

        self.g = params.g

 

        # 提取参数

        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₂

        # com₂增大→h增大→科氏力矩线性增大

        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)

        com₂出现在M11的com₂²项和M12的com₂线性项中（非线性+线性混合）

        com₃从腕关节量起，M₃₃ = m₃·com₃² + m₄·(com₃+L₂)²

        """

        c2 = np.cos(theta2)

        c23 = np.cos(theta2 + theta3)

 

        # M11 = 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))

 

        # M12 = 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 # 第三轴与前两轴在质点模型下无惯性耦合

 

        # M22 = 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₂)²

        # com₃从腕关节量起，与G3定义一致

        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˙

        质点模型+共线假设下，第三轴与前两轴无科氏耦合（τ_C3=0）

        """

        s2 = np.sin(theta2)

 

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

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

        tau_C3 = 0.0

 

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

 

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

        """

        计算重力项向量 G(q)

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

        com₃从腕关节量起，与M₃₃定义一致

        小臂质心com₂变化不影响τ_G3

        """

        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₃·(L₁·cosθ₁+L₂·cos(θ₁+θ₂+θ₃)+com₃·cos(θ₁+θ₂+θ₃))

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

        tau_G1 = -self.g * (

            self.m1 * self.com1 * c1

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

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

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

        )

 

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

        # + m

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