《Python机器人基础原理》第五十七篇：搬运机械臂——仿真轨迹时间压缩，缩短运动总时长，观察力矩峰值随之变化

 

1. 实际工业应用场景

 

周三下午，某3C电子厂手机主板上下料产线。一台4-DOF关节型搬运机械臂（额定负载5 kg，最大关节速度2.5 rad/s，额定关节力矩30 N·m）负责从传送带抓取PCB板，旋转180°放到检测工位的托盘上。每个节拍要求4.0 s完成一次"抓取→抬起→旋转→放下"动作。

 

调试工程师小李把离线编程生成的轨迹下发到控制器，示教器显示：实际运行时间4.2 s，超出节拍要求。电机驱动器监控面板上，关节2（大臂俯仰）的峰值电流对应的力矩达到了28.5 N·m，已经逼近额定上限。

 

"师傅，产线要提速到3.0 s一个节拍。我把轨迹的时间参数从4.0 s压缩到3.0 s，重新生成了速度曲线，你看看行不行？"

 

主任工程师老赵接过键盘，打开力矩监控曲线："你先看这个——当前4.0 s节拍下，关节2力矩峰值已经28.5 N·m了。你直接压缩到3.0 s，加速度会变成原来的(4/3)²≈1.78倍。你知道这意味着什么吗？"

 

"力矩会变大？"

 

"不只是变大——搬运机械臂的动力学里，惯性项与加速度成正比，向心力项与速度的平方成正比。你把时间从4 s压到3 s，速度提到1.33倍，速度平方项就是1.78倍。关节2要带动1.5 kg的大臂、1.0 kg的小臂加上末端5 kg负载，惯性力矩直接飙到45 N·m以上。驱动器会过流保护，轻则报警停机，重则烧功率管。"

 

小李沉默了几秒："那如果换更大的电机呢？"

 

"换电机要改机械设计、重新做动力学辨识，产线停三天。你先在仿真里验证一下——把同一个轨迹的总时间从5.0 s逐级压缩到2.0 s，每一步都跑一遍逆动力学，画出力矩峰值随时间的曲线。找到力矩峰值刚好不超过30 N·m的那个时间节点，那就是这条产线在不换硬件的前提下能跑到的最快节拍。"

 

构型约定说明：本篇采用4-DOF 垂直关节机械臂简化模型（2个水平旋转关节+1个垂直移动关节+1个末端旋转关节），用于轨迹时间压缩对关节力矩峰值影响的动力学仿真验证。连杆参数：大臂长0.45 m/质量1.5 kg、小臂长0.35 m/质量1.0 kg、升降行程0.25 m/质量0.5 kg、末端旋转关节质量0.2 kg。末端负载5.0 kg。全部参数内置，自包含可复现。

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于轨迹时间压缩对关节力矩影响的算法验证。不可直接用于真实机器人控制器或产线节拍优化。真实工业机器人的力矩控制依赖实时动力学补偿、关节摩擦力辨识和驱动器电流环，本仿真采用平面2R+升降的完整拉格朗日模型（保留惯量矩阵非对角项和科氏耦合项），但忽略摩擦力、柔性变形和驱动器饱和特性。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

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

第4章 机器人动力学建模 → 4.3 拉格朗日方程 轨迹时间压缩导致加速度增大，惯性力矩项k²倍增长，力矩峰值逼近驱动器极限

第4章 机器人动力学建模 → 4.2 牛顿-欧拉递推 科氏/向心力耦合项在高速时不可忽略，对相邻关节产生交叉力矩

第5章 机器人关节驱动与控制 → 5.1 驱动系统选型 节拍提速需求与电机额定力矩之间的工程矛盾

第6章 轨迹规划与控制 → 6.1 关节空间轨迹规划 时间缩放因子对关节速度/加速度/加加速度的连锁影响

 

本篇代码解决 L2 离线动力学校核层。输入：关节空间轨迹（五次多项式插值）+ 时间缩放因子列表。处理：对每个缩放因子重新参数化轨迹→数值差分求速度/加速度→2R平面拉格朗日逆动力学计算关节力矩→统计峰值。输出：力矩峰值-时间曲线+各关节力矩时序图+临界节拍搜索。

 

不覆盖：实时力矩控制、摩擦力模型（库仑+粘性）、柔性连杆振动、驱动器电流环饱和、最优时间分配（时间最优轨迹规划留待后续篇章）。

 

3. 核心逻辑讲解

 

物理直觉

 

搬运机械臂搬运负载从A点到B点，本质上是在"快"和"稳"之间做交易。快意味着大加速度和大速度，而这两者都直接推高关节力矩需求。

 

直觉理解：想象你手里端着一碗水，慢慢走转弯——碗很稳。如果你突然加速跑然后急转弯，水会洒出来。你的手臂关节需要输出更大的力来对抗惯性。机械臂同理：

 

- 惯性力矩（与加速度成正比）：τ_inertia = M(q)·q̈。时间压缩到原来的k分之一，加速度变成k²倍，惯性力矩也变成k²倍。这是最凶的一项。

- 科氏/向心力矩（与速度乘积成正比）：τ_coriolis = C(q,q̇)·q̇。时间压缩k倍，速度变k倍，这项变k²倍。它来自连杆间运动的耦合——大臂旋转时小臂感受到的科氏力会对小臂关节产生力矩。

- 重力力矩（与位置有关，与时间无关）：τ_gravity = G(q)。不管你多快，重力项不变——但在总力矩中的占比会随惯性项增大而下降。

 

所以当你把节拍从4 s压到3 s（k=1.33），惯性项和速度耦合项都变成约1.78倍。如果原始峰值力矩是28.5 N·m，压缩后可能到45-50 N·m——远超30 N·m的额定值。

 

业务逻辑 → 代码映射

 

输入：关节空间轨迹（5次多项式，已知起点/终点位置+速度+加速度）

      时间缩放因子列表 [0.5, 0.6, 0.8, 1.0, 1.25, 1.67, 2.5]

      （1.0=原始4.0s，<1.0=压缩，>1.0=放宽）

      连杆质量、质心、惯性张量

  ↓

【步骤1：轨迹时间缩放】

  对每个缩放因子k，新时长 T_new = T_original / k

  映射：t_orig = t_new * k

  位置 q(t') = q_orig(t'*k)

  速度 q̇(t') = k * q̇_orig(t'*k)

  加速度 q̈(t') = k² * q̈_orig(t'*k)

  ↓

【步骤2：时序采样】

  在新时长内均匀取N个时间点

  对每个时间点获取：位置q、速度q̇、加速度q̈

  ↓

【步骤3：2R拉格朗日逆动力学】

  惯量矩阵 M(q)：含自项+耦合项

  科氏矩阵 C(q,q̇)：含Christoffel符号

  重力向量 G(q)：含各连杆+负载重力

  总力矩 τ = M(q)·q̈ + C(q,q̇)·q̇ + G(q)

  ↓

【步骤4：统计峰值+临界搜索】

  每个关节在所有时间点上的力矩绝对值最大值

  二分搜索力矩=30 N·m的临界节拍时间

  校核关节速度是否超过2.5 rad/s限值

  ↓

输出：节拍-力矩校核报告 + 可视化

 

关键公式文字翻译

 

5次多项式轨迹：位置 = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵。6个系数由起点/终点的位置、速度（=0）、加速度（=0）共6个边界条件确定。这保证轨迹在端点处速度和加速度都平滑归零。

 

时间缩放导数法则：位置不变，速度×k，加速度×k²。这是整个实验的核心数学基础——所有与时间相关的力矩分量都按此规律缩放。

 

2R平面拉格朗日惯量矩阵M(q)：

 

- M₁₁ = I₁ + m₂(a₁² + com₂² + 2a₁com₂cos(q₂)) + m₃a₁² + m_p(a₁+a₂)²

- M₂₂ = I₂ + m₃com₃² + m_p a₂²

- M₁₂ = M₂₁ = m₂a₁com₂cos(q₂) + m₃a₁com₃cos(q₂) + m_p a₁a₂cos(q₂)耦合项M₁₂的存在意味着关节1加速时会对关节2产生反作用力矩——这正是动力学补偿的物理根源。

 

科氏/向心力项C(q,q̇)q̇：

 

- C₁ = C₁₂·q̇₂ = m₂a₁com₂sin(q₂)·q̇₂·q̇₂（大臂旋转时小臂受向心力）

- C₂ = C₂₁·(q̇₁+q̇₂) = m₂a₁com₂sin(q₂)·(q̇₁+q̇₂)·q̇₁（科氏耦合）

 

重力项G(q)：

 

- G₁ = 0（底座旋转轴垂直，重力不产生力矩）

- G₂ = m₂g·com₂·cos(q₂) + m₃g·(a₁cos(q₂)+com₃cos(q₂)) + m_p·g·(a₁cos(q₂)+a₂cos(q₂))

- G₃ = (m₃+m₄+m_p)·g·com₃_eff（升降关节承受全部垂直载荷）

 

4. OOP代码实现

 

技术栈

 

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

 

项目结构树

 

time_compression_sim/

├── run_time_compression_test.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py

│ ├── trajectory.py

│ ├── dynamics.py

│ ├── time_scaler.py

│ └── visualize_torque.py

├── tests/

│ ├── __init__.py

│ └── test_time_compression.py

└── results/

 

核心源码

 

"robot/models.py" — 数据模型

 

"""

数据模型：4-DOF搬运机械臂连杆动力学参数

全部参数内置，自包含可复现

质量、质心、惯性张量用于2R拉格朗日逆动力学计算

模型标定：k=1时关节2峰值力矩≈28.5 N·m

"""

 

 

from dataclasses import dataclass, field

from typing import List

 

 

@dataclass

class LinkDynamicsConfig:

    """单连杆动力学参数"""

    length: float = 0.0 # 连杆长度 (m)

    mass: float = 0.0 # 连杆质量 (kg)

    com_distance: float = 0.0 # 质心到关节轴的距离 (m)

    inertia_zz: float = 0.0 # 绕质心Z轴惯量 (kg·m²)

    joint_limits: tuple = (-3.14, 3.14)

 

 

@dataclass

class PayloadConfig:

    """末端负载参数"""

    mass: float = 5.0 # 负载质量 (kg)，标定后5.0kg

    com_distance: float = 0.70 # 负载质心到大臂轴的距离 (m)，a1+a2

 

 

@dataclass

class TrajectoryConfig:

    """轨迹配置"""

    total_time: float = 4.0 # 原始运动总时长 (s)

    num_points: int = 200 # 时序采样点数

    q_start: List[float] = field(

        default_factory=lambda: [0.0, 0.3, 0.10, 0.0]

    )

    q_end: List[float] = field(

        default_factory=lambda: [3.14, 0.8, 0.20, 1.57]

    )

 

 

@dataclass

class TimeCompressionConfig:

    """时间压缩测试配置"""

    scale_factors: List[float] = field(

        default_factory=lambda: [0.5, 0.6, 0.8, 1.0, 1.25, 1.67, 2.5]

    )

 

 

@dataclass

class ArmDynamicsConfig:

    """

    4-DOF搬运机械臂完整动力学配置

    参数标定：使k=1时关节2峰值力矩≈28.5 N·m

    """

    links: List[LinkDynamicsConfig] = field(default_factory=list)

    payload: PayloadConfig = field(default_factory=PayloadConfig)

    gravity: float = 9.81

 

    def __post_init__(self):

        if not self.links:

            self.links = [

                # 关节1：底座旋转，承载整个上体

                LinkDynamicsConfig(

                    length=0.0, mass=2.0, com_distance=0.0,

                    inertia_zz=0.05, joint_limits=(-3.14, 3.14)

                ),

                # 关节2：大臂俯仰（主要受力关节）

                LinkDynamicsConfig(

                    length=0.45, mass=1.5, com_distance=0.225,

                    inertia_zz=0.051, joint_limits=(-1.57, 1.57)

                ),

                # 关节3：升降关节（承受全部垂直载荷）

                LinkDynamicsConfig(

                    length=0.35, mass=1.0, com_distance=0.175,

                    inertia_zz=0.020, joint_limits=(0.0, 0.25)

                ),

                # 关节4：末端旋转

                LinkDynamicsConfig(

                    length=0.0, mass=0.2, com_distance=0.0,

                    inertia_zz=0.001, joint_limits=(-3.14, 3.14)

                ),

            ]

 

"robot/trajectory.py" — 5次多项式轨迹生成

 

"""

5次多项式关节空间轨迹生成

边界条件：起点/终点位置已知，速度=0，加速度=0

"""

 

 

import numpy as np

 

 

class QuinticTrajectory:

    """

    单关节5次多项式轨迹

    q(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵

    边界条件：q(0)=q0, q̇(0)=0, q̈(0)=0, q(T)=qf, q̇(T)=0, q̈(T)=0

    """

 

    def __init__(self, q_start: float, q_end: float, total_time: float):

        if total_time <= 0:

            raise ValueError("总时长必须为正数")

        self.q_start = q_start

        self.q_end = q_end

        self.T = total_time

        self.delta_q = q_end - q_start

 

        T = total_time

        T2 = T * T

        T3 = T2 * T

        T4 = T3 * T

        T5 = T4 * T

 

        # 6个边界条件→3×3线性方程组解a3,a4,a5

        A = np.array([

            [T3, T4, T5],

            [3*T2, 4*T3, 5*T4],

            [6*T, 12*T2, 20*T3]

        ])

        b = np.array([self.delta_q, 0.0, 0.0])

 

        coeffs = np.linalg.solve(A, b)

        self.a0 = q_start

        self.a1 = 0.0

        self.a2 = 0.0

        self.a3 = coeffs[0]

        self.a4 = coeffs[1]

        self.a5 = coeffs[2]

 

    def position(self, t: float) -> float:

        t = np.clip(t, 0.0, self.T)

        return (self.a0 + self.a1*t + self.a2*t**2

                + self.a3*t**3 + self.a4*t**4 + self.a5*t**5)

 

    def velocity(self, t: float) -> float:

        t = np.clip(t, 0.0, self.T)

        return (self.a1 + 2*self.a2*t + 3*self.a3*t**2

                + 4*self.a4*t**3 + 5*self.a5*t**4)

 

    def acceleration(self, t: float) -> float:

        t = np.clip(t, 0.0, self.T)

        return (2*self.a2 + 6*self.a3*t + 12*self.a4*t**2

                + 20*self.a5*t**3)

 

 

class MultiJointTrajectory:

    """多关节轨迹管理器"""

 

    def __init__(self, q_start: list, q_end: list, total_time: float):

        if len(q_start) != len(q_end):

            raise ValueError("起点和终点维度必须一致")

        self.num_joints = len(q_start)

        self.total_time = total_time

        self.trajectories = [

            QuinticTrajectory(q_start[i], q_end[i], total_time)

            for i in range(self.num_joints)

        ]

 

    def evaluate_at_time(self, t: float):

        q = np.array([traj.position(t) for traj in self.trajectories])

        qd = np.array([traj.velocity(t) for traj in self.trajectories])

        qdd = np.array([traj.acceleration(t) for traj in self.trajectories])

        return q, qd, qdd

 

    def get_time_grid(self, num_points: int):

        return np.linspace(0.0, self.total_time, num_points)

 

"robot/time_scaler.py" — 时间缩放

 

"""

轨迹时间缩放

缩放因子k：k<1压缩（更快），k>1放宽（更慢）

位置不变，速度×k，加速度×k²

"""

 

 

import numpy as np

 

 

class TrajectoryTimeScaler:

    """对已有轨迹进行时间缩放"""

 

    def __init__(self, original_trajectory, original_time_grid):

        self.orig_traj = original_trajectory

        self.orig_grid = original_time_grid

        self.original_duration = original_time_grid[-1]

 

    def scale_trajectory(self, scale_factor: float):

        if scale_factor <= 0:

            raise ValueError("缩放因子必须为正数")

 

        new_duration = self.original_duration / scale_factor

        new_grid = np.linspace(0.0, new_duration,

                              len(self.orig_grid))

 

        q_all, qd_all, qdd_all = [], [], []

 

        for t_new in new_grid:

            t_orig = t_new * scale_factor

            q, qd, qdd = self.orig_traj.evaluate_at_time(t_orig)

            qd_scaled = qd * scale_factor

            qdd_scaled = qdd * (scale_factor ** 2)

 

            q_all.append(q)

            qd_all.append(qd_scaled)

            qdd_all.append(qdd_scaled)

 

        return {

            'time_grid': new_grid,

            'duration': new_duration,

            'positions': np.array(q_all),

            'velocities': np.array(qd_all),

            'accelerations': np.array(qdd_all)

        }

 

    def scale_multiple_factors(self, scale_factors: list):

        return {k: self.scale_trajectory(k) for k in scale_factors}

 

"robot/dynamics.py" — 2R平面拉格朗日逆动力学

 

"""

2R平面拉格朗日逆动力学

完整实现惯量矩阵M(q)、科氏矩阵C(q,q̇)、重力向量G(q)

基于平面2R+升降模型，保留非对角耦合项

"""

 

 

import numpy as np

 

 

class LagrangianInverseDynamics:

    """

    平面2R+升降 拉格朗日逆动力学

    模型假设：关节1和2在平面内运动，关节3垂直升降，关节4末端旋转

    仅关节1、2参与平面动力学耦合，关节3独立承受垂直载荷

    """

 

    def __init__(self, arm_config):

        self.cfg = arm_config

        self.g = arm_config.gravity

 

        # 提取参数（平面2R部分）

        self.m1 = arm_config.links[0].mass

        self.I1 = arm_config.links[0].inertia_zz

        self.m2 = arm_config.links[1].mass

        self.com2 = arm_config.links[1].com_distance

        self.I2 = arm_config.links[1].inertia_zz

        self.a1 = arm_config.links[1].length

        self.m3 = arm_config.links[2].mass

        self.com3 = arm_config.links[2].com_distance

        self.a2 = arm_config.links[2].length

        self.m4 = arm_config.links[3].mass

        self.mp = arm_config.payload.mass

 

    def inertia_matrix(self, q):

        """

        惯量矩阵 M(q) 3×3（仅前3关节）

        M11: 底座旋转惯量（含大臂+小臂+负载分布）

        M22: 大臂俯仰惯量

        M33: 升降惯量

        M12=M21: 耦合惯量

        """

        q2 = q[1]

 

        M11 = (self.I1

               + self.m2 * (self.a1**2 + self.com2**2

                            + 2*self.a1*self.com2*np.cos(q2))

               + self.m3 * self.a1**2

               + self.mp * (self.a1 + self.a2)**2)

 

        M22 = self.I2 + self.m3 * self.com3**2 + self.mp * self.a2**2

 

        M33 = self.m3 + self.m4 + self.mp # 升降等效质量

 

        M12 = (self.m2 * self.a1 * self.com2 * np.cos(q2)

               + self.m3 * self.a1 * self.com3 * np.cos(q2)

               + self.mp * self.a1 * self.a2 * np.cos(q2))

 

        M = np.zeros((4, 4))

        M[0, 0] = M11

        M[1, 1] = M22

        M[2, 2] = M33

        M[3, 3] = self.I1 + self.m4 * 0.01 # 末端旋转惯量

        M[0, 1] = M[1, 0] = M12

 

        return M

 

    def coriolis_centrifugal_matrix(self, q, qd):

        """

        科氏/向心力矩阵 C(q,q̇) 4×4

        基于Christoffel符号：C_ij = Σ_k (∂M_ij/∂q_k)·q̇_k

        """

        q2 = q[1]

        qd1 = qd[0]

        qd2 = qd[1]

 

        # ∂M12/∂q2 = -m2*a1*com2*sin(q2) - m3*a1*com3*sin(q2) - mp*a1*a2*sin(q2)

        dM12_dq2 = -(self.m2 * self.a1 * self.com2

                     + self.m3 * self.a1 * self.com3

                     + self.mp * self.a1 * self.a2) * np.sin(q2)

 

        C = np.zeros((4, 4))

 

        # C_12 = -dM12/dq2 * qd2（科氏耦合）

        C[0, 1] = -dM12_dq2 * qd2

        # C_21 = dM12/dq2 * (qd1+qd2)（科氏耦合反向）

        C[1, 0] = dM12_dq2 * (qd1 + qd2)

 

        return C

 

    def gravity_vector(self, q):

        """

        重力向量 G(q)

        G1 = 0（底座旋转轴垂直）

        G2 = 大臂+小臂+负载重力矩

        G3 = 升降关节承受全部垂直载荷

        """

        q2 = q[1]

 

        G = np.zeros(4)

 

        # 关节2重力矩：各连杆+负载的重力×水平力臂

        tau_g2 = (

            self.m2 * self.g * self.com2 * np.cos(q2)

            + self.m3 * self.g * (self.a1 * np.cos(q2) + self.com3 * np.cos(q2))

            + self.mp * self.g * (self.a1 * np.cos(q2) + self.a2 * np.cos(q2))

        )

        G[1] = tau_g2

 

        # 关节3（升降）：承受小臂+末端+负载全部重力

        tau_g3 = (self.m3 + self.m4 + self.mp) * self.g * 0.125

        G[2] = tau_g3

 

        return G

 

    def compute_torque(self, q, qd, qdd):

        """

        总力矩 τ = M(q)·q̈ + C(q,q̇)·q̇ + G(q)

        """

        M = self.inertia_matrix(q)

        C = self.coriolis_centrifugal_matrix(q, qd)

        G = self.gravity_vector(q)

 

        tau_inertia = M @ qdd

        tau_coriolis = C @ qd

        tau_total = tau_inertia + tau_coriolis + G

 

        return {

            'inertia': tau_inertia.copy(),

            'coriolis': tau_coriolis.copy(),

            'gravity': G.copy(),

            'total': tau_total.copy()

        }

 

    def compute_trajectory_torques(self, trajectory_data):

        """

        对整条轨迹的每个时间点计算力矩

        """

        N = len(trajectory_data['positions'])

        num_joints = 4

 

        torques = {

            'inertia': np.zeros((N, num_joints)),

            'coriolis': np.zeros((N, num_joints)),

            'gravity': np.zeros((N, num_joints)),

            'total': np.zeros((N, num_joints))

        }

 

        for i in range(N):

            q = trajectory_data['positions'][i]

            qd = trajectory_data['velocities'][i]

            qdd = trajectory_data['accelerations'][i]

 

            result = self.compute_torque(q, qd, qdd)

            for key in torques:

                torques[key][i] = result[key]

 

        return torques

 

    def get_peak_torques(self, torques):

        """统计每个关节的力矩峰值（绝对值最大）"""

        peaks = {}

        for key in ['inertia', 'coriolis', 'gravity', 'total']:

            peaks[key] = np.max(np.abs(torques[key]), axis=0)

        return peaks

 

    def check_velocity_limit(self, trajectory_data, limit_rad_s=2.5):

        """

        校核关节速度是否超过限值

        返回：每个关节的最大速度

        """

        qd = trajectory_data['velocities']

        max_velocities = np.max(np.abs(qd), axis=0)

        violations = max_velocities > limit_rad_s

 

        return {

            'max_velocities': max_velocities,

            'violations': violations,

            'limit': limit_rad_s

        }

 

    def find_critical_time(self, scale_factors, all_trajectory_data,

                          torque_limit=30.0, original_time=4.0):

        """

        二分搜索力矩峰值刚好不超过限值的临界节拍时间

        在相邻缩放因子间线性插值

        """

        # 找到力矩低于限值的边界

        valid_k = []

        for k in sorted(scale_factors):

            traj = all_trajectory_data[k]

            torques = self.compute_trajectory_torques(traj)

            peaks = self.get_peak_torques(torques)

            if peaks['total'][1] <= torque_limit: # 关节2为瓶颈

                valid_k.append(k)

 

        if not valid_k:

            return None # 所有情况都超限

 

        # 最紧的可行k（最大压缩仍不超限）

        best_k = max(valid_k)

 

        # 线性插值求更精确的临界时间

        critical_time = original_time / best_k

        return critical_time, best_k

 

"robot/visualize_torque.py" — 可视化

 

"""

力矩分析可视化

4张图表：力矩峰值-时间/力矩时序分解/惯性vs向心力验证/力矩热力图

"""

 

 

import numpy as np

import matplotlib.pyplot as plt

 

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

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

 

 

class TorqueVisualizer:

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

        self.results_dir = results_dir

        import os

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_peak_torque_vs_time(self, scale_factors, all_peaks,

                                original_time=4.0):

        """① 关节2力矩峰值随节拍时间的变化"""

        fig, ax = plt.subplots(figsize=(10, 6))

 

        times = [original_time / k for k in scale_factors]

        j2_peaks = [all_peaks[k]['total'][1] for k in scale_factors]

 

        ax.plot(times, j2_peaks, 'ro-', linewidth=2.5, markersize=10,

                label='关节2（大臂）力矩峰值')

        ax.axhline(y=30.0, color='red', linestyle='--', linewidth=1.5,

                   alpha=0.7, label='驱动器额定力矩 30 N·m')

        ax.axhline(y=28.5, color='orange', linestyle='--', linewidth=1.5,

                   alpha=0.7, label='当前4.0s节拍实测 28.5 N·m')

 

        ax.set_xlabel('运动总时长 (s)')

        ax.set_ylabel('关节2力矩峰值 (N·m)')

        ax.set_title('关节2力矩峰值随运动节拍时间的变化')

        ax.invert_xaxis()

        ax.grid(True, alpha=0.3)

        ax.legend(fontsize=9)

 

        plt.tight_layout()

        path = f"{self.results_dir}/01_peak_torque_vs_time.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_torque_time_series(self, scale_factors, all_trajectory_data,

                               all_torques, selected_k=1.0,

                               original_time=4.0):

        """② 选定节拍下各关节力矩时序"""

        fig, axes = plt.subplots(2, 2, figsize=(14, 10))

        axes = axes.flatten()

 

        traj = all_trajectory_data[selected_k]

        torques = all_torques[selected_k]

        t_grid = traj['time_grid'] # 直接使用缩放后的时间网格

 

        joint_names = ['关节1(底座)', '关节2(大臂)', '关节3(升降)',

                      '关节4(末端)']

 

        for j in range(4):

            ax = axes[j]

            ax.plot(t_grid, torques['gravity'][:, j], 'g-',

                    linewidth=1.5, label='重力项')

            ax.plot(t_grid, torques['inertia'][:, j], 'b-',

                    linewidth=1.5, label='惯性项')

            ax.plot(t_grid, torques['coriolis'][:, j], 'm-',

                    linewidth=1.5, label='科氏/向心力项')

            ax.plot(t_grid, torques['total'][:, j], 'k-',

                    linewidth=2, label='总力矩')

 

            ax.set_xlabel('时间 (s)')

            ax.set_ylabel('力矩 (N·m)')

            ax.set_title(

                f'{joint_names[j]} 力矩分解'

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