《Python机器人基础原理》第十五篇：高速搬运仿真——对比低速与高速工况，分析科氏力项对关节输出力矩带来的影响

 

1. 实际工业应用场景

 

周五夜班，某3C电子元件高速分拣线。节拍要求：每0.8秒完成一次"抓取-抬升-平移-放下"循环，末端负载0.8 kg，运动速度峰值2.4 m/s，加速度峰值12 m/s²。一台3-DOF平面关节机械臂（2旋转+1垂直平移）负责将PCB板从进料传送带搬运到检测工位。机器人构型：大臂长500 mm，小臂长350 mm，基座安装高度d₁=300 mm，z轴行程±150 mm。关节硬件限位：θ₁ ±175°、θ₂ ±150°、d₃ ±150 mm。连杆质量：大臂2.8 kg（质心距基座0.22 m），小臂1.6 kg（质心距肘部0.15 m），末端执行器+负载2.0 kg。额定负载3 kg。

 

凌晨一点，产线又停了。这次不是报警，是成品不良率突然从99.2%掉到87%。

 

小王揉着眼睛翻高速摄像头的回放录像。机械臂在快速换向时，末端轨迹出现了肉眼可见的抖动——大约±3 mm的偏差。PCB板放不到位，卡在检测工装边缘。

 

"低速的时候好好的，"他对旁边的老刘说，"同样的程序，我把速度从30%提到100%，末端就开始抖。不是控制参数的问题，我试过调PID了，没用。"

 

老刘拉了把椅子坐下来："你算过高速时的关节力矩吗？"

 

"上周低速码垛那篇我做了静力学分析，重力力矩最大54.5 N·m——但那是另一台大臂。这台高速搬运臂负载轻，水平伸直时重力力矩大约33 N·m，速度提到2.4 m/s之后惯性项会更大。"

 

"重力只是一部分。你速度提到2.4 m/s的时候，科氏力和离心力呢？"

 

小王愣住了。

 

"机械臂两个旋转关节同时运动的时候，大臂转动会带着小臂产生一个侧向的科氏力。速度越高这个力越大。它不直接体现在末端位置里，但它会让关节承受额外的动态负载——如果伺服驱动器没有为此预留扭矩余量，电机就会在换向瞬间出现力矩饱和，轨迹就跟着飘。"

 

小王打开笔记本电脑："我需要对比一下低速（0.5 m/s）和高速（2.4 m/s）两种工况下，科氏力项对关节力矩的贡献比例。如果高速时科氏力矩占比超过20%，那电机选型和前馈补偿策略都得重新来。"

 

本篇解决的问题：基于拉格朗日动力学方程 τ = M(q)q¨ + C(q,q˙)q˙ + G(q)，在相同的几何参数下，分别仿真低速（0.5 m/s）和高速（2.4 m/s）两种工况，提取科氏力/离心力项 C(q,q˙)q˙ 的数值，对比重力项 G(q) 和惯性项 M(q)q¨，量化科氏力在高速搬运中对关节输出力矩的影响比例。本篇将连杆视为集中质量（质点模型），忽略连杆绕自身质心的转动惯量——这是课程级别的简化，真实工业需加入惯性张量。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于科氏力/离心力对关节力矩影响的对比分析。不可直接用于真实机器人控制器、伺服驱动器或高速搬运产线。真实工业现场需结合实时动力学前馈补偿、关节扭矩传感器反馈与安全监控。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

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

第4章 机器人动力学建模 → 4.3 拉格朗日方程推导 完整动力学方程中科氏力/离心力项 C(q,q˙)q˙ 在高速时不可忽略

第4章 → 4.4 关节空间动力学方程 τ = M(q)q¨ + C(q,q˙)q˙ + G(q) 低速时 C≈0，高速时 C项可达总力矩的20~40%

第5章 机器人关节驱动与控制 → 5.3 前馈补偿 科氏力矩若不补偿，伺服在高速换向时力矩饱和导致轨迹偏差

第6章 轨迹规划与控制 → 6.2 速度/加速度约束 高速轨迹的加减速段是科氏力峰值出现的时刻

 

本篇代码解决 L3 离线动力学分析层。输入关节角度、角速度、角加速度，输出惯性项、科氏力/离心力项、重力项的分解数值。代码不是设备实时控制器，不做伺服闭环，不做在线力矩前馈。它做的事情是：在离线环境中对比低速与高速两种速度剖面下的各项力矩贡献，告诉控制工程师"科氏力在什么时候、多大程度上影响关节负载，前馈补偿是否必要"。

 

不覆盖：完整6轴动力学、柔性关节振动、摩擦力模型、实时操作系统代码、力控制。

 

3. 核心逻辑讲解

 

物理直觉

 

你坐过旋转木马吗？当木马转动时你向外倾——那是离心力。现在想象你手里拿着一根棍子，棍子末端还有个重物，你一边转手腕一边让棍子展开——这时候除了向外甩的离心力，还有一个"科氏力"垂直于你的运动方向。机械臂的大臂在转动时，小臂如果也在展开或收拢，小臂末端的质量就会受到这个侧向的科氏力。速度越快、两个关节同时运动越剧烈，这个力越大。

 

低速搬运时（<0.5 m/s），科氏力微乎其微——就像你慢慢转手腕，感觉不到侧向力。但高速搬运时（>2 m/s），科氏力可以占到关节总负载的20%以上。如果电机和减速器没有为此预留余量，换向瞬间就会出现力矩饱和，末端轨迹跟着飘——这就是小王看到的±3 mm抖动。

 

业务逻辑 → 代码映射

 

输入：关节角度 q、角速度 q˙、角加速度 q¨

  ↓

步骤1：计算惯性矩阵 M(q) —— 2×2对称矩阵，与速度无关

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

  ↓

步骤2：计算科氏力/离心力项 τ_C = C(q,q˙)·q˙

  采用Christoffel符号合并形式：

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

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

  其中 h = m₂·L₁·com₂ + m₃·L₁·L₂

  ↓

步骤3：计算重力项 G(q) —— 与第十四篇相同

  ↓

步骤4：计算惯性项 τ_M = M(q) · q¨

  ↓

步骤5：总力矩 τ_total = τ_M + τ_C + G

  ↓

步骤6：分别仿真低速(0.5 m/s)和高速(2.4 m/s)的同步梯形速度轨迹

  （各关节按位移比例独立缩放速度，避免超调）

  对比各时刻 τ_C / τ_total 的比例

  ↓

步骤7：输出对比图表

 

关键公式文字翻译

 

科氏力/离心力项（Christoffel合并形式）：τ_C = C(q,q˙)·q˙。对于2连杆平面臂，定义耦合系数 h = m₂·L₁·com₂ + m₃·L₁·L₂。则 τ_C1 = -h·sinθ₂·(2θ₁˙θ₂˙ + θ₂˙²)，τ_C2 = h·sinθ₂·θ₁˙²。每一项物理含义：耦合系数×sinθ₂×角速度乘积（科氏）或角速度平方（离心）。单位：N·m。注意：τ_C ∝ sinθ₂，θ₂=0°（两臂完全伸直）时科氏项为零；峰值出现在θ₂≈±90°附近的中间构型。

 

惯性矩阵 M(q)：M₁₁ = m₁·com₁² + m₂·(L₁²+com₂²+2L₁·com₂·cosθ₂) + m₃·(L₁²+L₂²+2L₁·L₂·cosθ₂)，M₁₂=M₂₁ = m₂·(com₂²+L₁·com₂·cosθ₂) + m₃·(L₂²+L₁·L₂·cosθ₂)，M₂₂ = m₂·com₂² + m₃·L₂²。本篇将连杆视为集中质量，故 M₁₁ 中没有 I₁（连杆绕自身质心的转动惯量）。课程简化可接受，但真实电机扭矩需求会系统性偏小约10~15%。

 

为什么高速时不可忽略：科氏力和离心力与速度的平方成正比。速度从0.5 m/s提到2.4 m/s（约4.8倍），关节角速度也同比放大，科氏力矩放大约(4.8)²≈23倍。低速时可能只有0.3 N·m的科氏力矩，高速时直接跳到7 N·m以上——从"可以忽略"变成"必须补偿"。

 

单位：力矩 N·m，角速度 rad/s，角加速度 rad/s²。

 

关于占比指标：

"compute_coriolis_ratio" 使用逐点 |τ_C|/|τ_total|。当 τ_total 过零（换向瞬间 τ_M + τ_C + G 互相抵消）时该比值会虚高。代码中已做1e-10防除零，但读者需注意：逐点占比反映的是"该时刻科氏力在总力矩中的相对权重"，而非"科氏力是否主导选型"。真实瓶颈通常是惯性项 τ_M（加减速段最大），科氏力是换向段的附加扰动。三项量级排序：|τ_M|峰值 > |τ_G| > |τ_C|（高速时τ_C可接近τ_G）。

 

4. OOP代码实现

 

技术栈

 

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

 

项目结构树

 

robot_coriolis_sim/

├── run_coriolis_sim.py

├── requirements.txt

├── dynamics/

│ ├── __init__.py

│ ├── models.py

│ ├── trajectory.py

│ ├── dynamics_solver.py

│ └── visualize.py

├── tests/

│ ├── __init__.py

│ └── test_coriolis_sim.py

└── results/

 

核心源码

 

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

 

"""

3-DOF高速搬运机械臂数据模型

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

"""

 

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:

    """3-DOF搬运机械臂关节限位集合"""

    limits: List[JointLimit]

 

    def __post_init__(self):

        if len(self.limits) != 3:

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

 

    @classmethod

    def default_3dof_transfer(cls):

        """

        默认3-DOF高速搬运机械臂限位

        θ1: ±175° θ2: ±150° d3: ±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(-0.150, 0.150, 'joint3_z_lift'),

        ])

 

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

        if len(joint_values) != 3:

            raise ValueError(f"需要3个关节值, 实际{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) != 3:

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

 

    @classmethod

    def default_highspeed_transfer(cls):

        """

        高速搬运机械臂质量参数

        大臂: 2.8 kg, 质心距基座0.22 m

        小臂: 1.6 kg, 质心距肘部0.15 m

        末端执行器+负载: 吸盘0.5 kg + PCB板0.8 kg + 夹具0.7 kg = 2.0 kg

        """

        return cls(link_props=[

            LinkMassProps(2.8, 0.22, 0.0, 0.0, 'link1_upper_arm'),

            LinkMassProps(1.6, 0.15, 0.0, 0.0, 'link2_forearm'),

            LinkMassProps(2.0, 0.0, 0.0, 0.0, 'link3_end_effector'),

        ])

 

 

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

    """

    3-DOF高速搬运机械臂完整参数

    大臂500mm, 小臂350mm, d1=300mm

    """

    links: List[DHParam]

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

        'joint1_base_yaw', 'joint2_elbow_yaw', 'joint3_z_lift'

    ])

    g: float = 9.81

 

    @classmethod

    def default_highspeed_transfer(cls):

        pi = 3.141592653589793

        links = [

            DHParam(0.0, 0.300, 0.500, 0.0),

            DHParam(0.0, 0.0, 0.350, 0.0),

            DHParam(0.0, 0.0, 0.0, 0.0),

        ]

        return cls(links=links)

 

"dynamics/trajectory.py" — 同步梯形速度轨迹生成器（修复超调bug）

 

"""

同步梯形速度轨迹生成器：各关节按位移比例独立缩放速度

修复：位移小的关节不再超调（不再共用同一max_velocity和t_const）

"""

 

import numpy as np

 

 

class SynchronizedTrapezoidalTrajectory:

    """

    同步梯形速度轨迹：各关节按位移比例分配速度上限

    保证所有关节在同一总时间内完成运动，无超调

    """

 

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

        """

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

        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

 

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

        # 核心约束：每个关节必须在 total_time 内到达 q_end

        # 如果位移太小，降低该关节的 max_velocity 使其匀速段不为负

        self.joint_params = []

 

        for i in range(3):

            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,

                    'direction': 1.0

                })

                continue

 

            direction = 1.0 if dq > 0 else -1.0

            abs_dq = abs(dq)

 

            # 计算理论加速时间

            t_acc = max_velocity / max_acceleration

            # 匀速段位移

            const_dist = max_velocity * (total_time - 2 * t_acc)

 

            if const_dist >= 0 and abs_dq <= max_velocity * t_acc + const_dist * 0.5 + max_velocity * (total_time - 2 * t_acc):

                # 可以用全局速度

                v_max = max_velocity

                a_max = max_acceleration

                t_const = total_time - 2 * t_acc

                if t_const < 0:

                    t_acc = total_time / 2.0

                    v_max = max_acceleration * t_acc

                    a_max = max_acceleration

                    t_const = 0.0

            else:

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

                # 三角形轨迹：加速到某个速度后立即减速

                # dq = a * t_acc² => t_acc = sqrt(dq/a)

                t_acc = np.sqrt(abs_dq / max_acceleration)

                if t_acc * 2 > total_time:

                    t_acc = total_time / 2.0

                v_max = max_acceleration * t_acc

                a_max = max_acceleration

                t_const = total_time - 2 * t_acc

                if t_const < 0:

                    t_acc = total_time / 2.0

                    v_max = max_acceleration * t_acc

                    t_const = 0.0

 

                # 校验：确保匀速段位移不为负且总位移匹配

                if t_const < 0:

                    t_const = 0.0

 

            self.joint_params.append({

                'v_max': direction * v_max,

                'a_max': direction * a_max,

                't_acc': t_acc,

                't_const': t_const,

                'direction': direction

            })

 

    def evaluate(self, t):

        """

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

        返回：(q, q_dot, q_ddot)

        所有关节在 t>=total_time 时精确停在 q_end，无跳变

        """

        if t <= 0:

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

        if t >= self.total_time:

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

 

        q = np.zeros(3)

        q_dot = np.zeros(3)

        q_ddot = np.zeros(3)

 

        for i in range(3):

            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']

 

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

            else:

                # 减速段

                t_dec = 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

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

                        + 0.5 * a_max * t_acc**2

                        + v_max * t_const

                        + v_max * t_dec

                        + 0.5 * a_dec * t_dec**2)

 

        return q, q_dot, q_ddot

 

    def generate_trajectory_array(self, dt=0.01):

        """

        生成完整轨迹数组

        返回：times, q_array, q_dot_array, q_ddot_array

        """

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

        n = len(times)

 

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

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

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

 

        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)

 

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

平面2连杆，z平移（关节3）不影响平面内M/C项——显式断言。

"""

 

import numpy as np

from dynamics.models import RobotParams, MassPropertySet

 

 

class DynamicsSolver:

    """

    基于拉格朗日方程的2连杆平面动力学求解器

    输入3关节状态，但仅用前2个旋转关节计算M/C/G

    平移关节d₃不影响平面内动力学项（显式断言）

    """

 

    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.com1 = mass_props.link_props[0].com_x

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

 

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

        self.h = self.m2 * self.L1 * self.com2 + self.m3 * self.L1 * self.L2

 

    def inertia_matrix(self, theta2):

        """

        计算2×2惯性矩阵 M(q)

        本篇将连杆视为集中质量，故 M₁₁ 中不含 I₁（绕质心转动惯量）

        课程简化，真实值需加 I₁+I₂

        """

        c2 = np.cos(theta2)

 

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

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

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

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

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

        M22 = self.m2 * self.com2**2 + self.m3 * self.L2**2

 

        return np.array([[M11, M12], [M12, M22]], dtype=float)

 

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

        """

        计算科氏力/离心力向量 τ_C = C(q,q˙)·q˙

        采用Christoffel符号合并形式（标准推导，非C矩阵逐元素展开）：

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

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

        其中 h = m₂·L₁·com₂ + m₃·L₁·L₂

        注意：τ_C ∝ sinθ₂，θ₂=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

 

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

 

    def gravity_vector(self, theta1, theta2):

        """

        计算重力项向量 G(q)

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

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

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

        """

        tau_G1 = -self.g * (

            self.m1 * self.com1 * np.cos(theta1)

            + self.m2 * (self.L1 * np.cos(theta1)

                         + self.com2 * np.cos(theta1 + theta2))

            + self.m3 * (self.L1 * np.cos(theta1)

                         + self.L2 * np.cos(theta1 + theta2))

        )

        tau_G2 = -self.g * (

            self.m2 * self.com2 * np.cos(theta1 + theta2)

            + self.m3 * self.L2 * np.cos(theta1 + theta2)

        )

 

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

 

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

        """

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

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

        z平移关节不影响平面内M/C/G项（已验证）

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

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

        """

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

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

 

        theta1 = q[0]

        theta2 = q[1]

        theta1_dot = q_dot[0]

        theta2_dot = q_dot[1]

        theta1_ddot = q_ddot[0]

        theta2_ddot = q_ddot[1]

 

        # 惯性项

        M = self.inertia_matrix(theta2)

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

 

        # 科氏力/离心力项

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

 

        # 重力项

        tau_G = self.gravity_vector(theta1, theta2)

 

        # 总力矩

        tau_total = tau_M + tau_C + tau_G

 

        return tau_M, tau_C, tau_G, tau_total

 

    def compute_coriolis_ratio(self, tau_C, tau_total):

        """

        计算科氏力项在总力矩中的逐点占比（绝对值比）

        注意：换向瞬间τ_total可能过零导致比值虚高

        此指标反映"该时刻科氏力的相对权重"，非"科氏力主导选型"

        真实瓶颈通常是惯性项τ_M（加减速段最大）

        """

        ratio = np.zeros(2)

        for i in range(2):

            if abs(tau_total[i]) > 1e-10:

                ratio[i] = abs(tau_C[i]) / abs(tau_total[i])

            else:

                ratio[i] = 0.0

        return ratio

 

    def energy_based_verification(self, q, q_dot, q_ddot, dt=0.001):

        """

        基于能量守恒的自检方法：

        用有限差分近似动能变化率，与 τ_M + τ_C 对比

        dK/dt ≈ (K(t+dt) - K(t-dt)) / (2dt)

        对于无重力保守系统（水平面），dK/dt 应等于 (τ_M + τ_C)·q˙

        本篇用于验证动力学方程自洽性

        """

        # 计算当前动能 K = 0.5·q˙ᵀ·M(q)·q˙

        M = self.inertia_matrix(q[1])

        K = 0.5 * np.array([q_dot[0], q_dot[1]]) @ M @ np.array([q_dot[0], q_dot[1]])

 

        # 计算 τ_M + τ_C

        tau_M = M @ np.array([q_ddot[0], q_ddot[1]])

        tau_C = self.coriolis_vector(q[1], q_dot[0], q_dot[1])

        power = np.dot(tau_M + tau_C, np.array([q_dot[0], q_dot[1]]))

 

        return K, power

 

"dynamics/visualize.py" — 可视化模块

 

"""

高速搬运动力学仿真结果可视化：4张核心图表

"""

 

import numpy as np

import matplotlib.pyplot as plt

 

plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']

plt.rcParams['axes.unicode_minus'] = False

 

 

class Visualizer:

    def __init__(self, results_dir="results"):

        self.results_dir = results_dir

        import os

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_torque_breakdown(self, times, tau_M_hist, tau_C_hist,

                               tau_G_hist, tau_total_hist, speed_label):

        """

        ① 低速/高速工况下各力矩项随时间变化

        工程结论：高速时τ_C项在换向段显著增大

        三项量级排序：|τ_M|峰值 > |τ_G| > |τ_C|（高速时τ_C可接近τ_G）

        """

        fig, axes = plt.subplots(2, 1, figsize=(12, 8), sharex=True)

 

        # 关节1

        ax = axes[0]

        ax.plot(times, np.abs(tau_M_hist[:, 0]), '-',

                 color='steelblue', linewidth=1.0, label='|τ_M| Inertia')

        ax.plot(times, np.abs(tau_C_hist[:, 0]), '-',

                 color='orange', linewidth=1.0, label='|τ_C| Coriolis')

        ax.plot(times, np.abs(tau_G_hist[:, 0]), '-',

                 color='green', linewidth=1.0, label='|τ_G| Gravity')

        ax.plot(times, np.abs(tau_total_hist[:, 0]), '-',

                 color='red', linewidth=1.5, label='|τ_total|')

        ax.set_ylabel('Joint1 Torque (N·m)')

        ax.set_title(f'Torque Breakdown Over Time — {speed_label}')

        ax.grid(True, alpha=0.3)

        ax.legend(fontsize=7, loc='upper right')

 

        # 关节2

        ax = axes[1]

        ax.plot(times, np.abs(tau_M_hist[:, 1]), '-',

                 color='steelblue', linewidth=1.0, label='|τ_M| Inertia')

        ax.plot(times, np.abs(tau_C_hist[:, 1]), '-',

                 color='orange', linewidth=1.0, label='|τ_C| Coriolis')

        ax.plot(times, np.abs(tau_G_hist[:, 1]), '-',

                 color='green', linewidth=1.0, label='|τ_G| Gravity')

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