《Python机器人基础原理》第一百三十九篇：装配产线仿真——闭环运动误差统计与三类评价指标计算

 

声明：本篇全部为课程离线仿真代码，不能直接用于真实机器人控制器，规避工程误导风险。

 

1. 实际工业应用场景

 

周五上午，汽车零部件装配线。一台2R竖直平面机械臂要把活塞销压入连杆小头孔，孔位直径12.00 mm，销直径11.96 mm，单边间隙仅0.02 mm。机械臂从待料位出发，沿一段封闭矩形轨迹运动——下降接近、水平插入、保持、拔出、返回——完成一个闭环装配循环，全程末端需要维持亚毫米级精度。

 

"第42件开始，有3件压装力超限报警。"上位机弹出误差报告，小赵放大曲线："末端X方向最大偏差跑到1.8 mm，销子斜着进孔，单边啃到孔壁。1.8 mm的偏差是单边间隙0.02 mm的90倍，销子根本不可能进入导向段，位置干涉会远早于力超限触发——现实中报警会先报位置超差，再报压装力异常。"

 

他调出参数：连杆1质量3.5 kg、长度0.45 m、转动惯量0.25 kg·m²、质心0.22 m；连杆2质量2.0 kg、长度0.35 m、转动惯量0.10 kg·m²、质心0.18 m。末端工具质量0.8 kg，PD增益Kp=[600, 400] N·m/rad，Kd=[30, 20] N·m·s/rad，重力9.81 m/s²。闭环轨迹由四段三次多项式拼接：下降段0.6 s、插入段0.8 s、保持段0.5 s、返回段0.8 s，总周期2.7 s。

 

"把整条闭环跑一遍，用最大误差、平均误差、均方根误差三类指标评价跟踪性能，看看PD参数能不能把末端压在工艺窗口内。"

 

⚠️ 声明：本代码仅用于课程离线仿真，运行在普通PC上。不包含任何真实机器人控制器的实时通信栈或硬件驱动。不可直接用于产线设备。

 

2. 痛点分析

 

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

第3章 运动学求解 → 正运动学 末端位置由关节角经方向余弦累加得到，角度跟踪误差经构型映射为末端位置误差

第4章 机器人动力学建模 → 刚体动力学 2R连杆+工具形成耦合惯性，各工位重力矩随构型变化，影响跟踪精度

第5章 机器人关节驱动与控制 → PD伺服 PD增益决定闭环带宽，增益不足则误差大，增益过大会激发高频振荡

第6章 轨迹规划与控制 → 多段轨迹拼接 封闭矩形轨迹由多段子轨迹组成，每段需独立规划并保证衔接平滑

 

本篇代码解决 L2 离线仿真校核层。输入：2R连杆参数、工具质量、多段三次多项式轨迹。处理：RK4积分真实动力学，逐段记录末端误差。输出：4张分析图表 + 三类误差评价指标 + 单元测试。

 

不覆盖：力控柔顺装配、奇异位形、碰撞检测、摩擦模型、视觉引导。

 

3. 核心逻辑讲解

 

物理直觉

 

装配闭环就像人拿笔在纸上画一个长方形：笔尖要从起点落下去、横着画一条线、停一下、再原路回来。每段动作对应一组关节角的起点和终点，中间怎么走由三次多项式决定——保证起步和到位时速度都为零，不会"撞"到目标位置。

 

PD控制器像两只手上的"虚拟肌肉"：关节偏离目标，比例项把关节拉回去；关节转得太快，微分项踩刹车。重力前馈则像提前估算好的"配重"——知道当前姿势下重力要把臂往下拽多少，提前给电机一个相反的力矩抵消掉，让PD只需处理动态跟踪的残差。

 

末端误差来自关节角的跟踪偏差。角度偏差经过正运动学传递：水平方向主要受关节1影响，竖直方向受两个关节共同影响。同一角度误差在不同构型下产生的末端偏差不同——臂伸得越直，同样的关节误差被放得越大，这背后就是雅可比矩阵条件数对误差传递的放大作用。

 

业务逻辑 → 代码映射

 

输入：2R连杆参数 + 工具质量 + 多段轨迹航点

  ↓

【步骤1：建立2R动力学模型】

  质量矩阵M(q)、科氏力C(q,qd)、重力G(q)

  正向动力学：qdd = M⁻¹·(τ - C - G)

  ↓

【步骤2：正运动学计算末端位置】

  x = l1·cos(q1) + l2·cos(q1+q2)

  y = l1·sin(q1) + l2·sin(q1+q2)

  ↓

【步骤3：多段三次多项式轨迹】

  每段：q(t) = a·t³ + b·t²，满足四边界条件

  预计算所有时刻的期望量，仿真循环内直接索引

  ↓

【步骤4：RK4闭环仿真（准RK4，力矩整步零阶保持）】

  每步记录：q_des, q_actual, τ, 末端位置, 末端误差

  ↓

【步骤5：计算三类误差指标】

  最大误差 = max(‖e_end‖)

  平均误差 = mean(‖e_end‖)

  RMS误差 = sqrt(mean(‖e_end‖²))

  ↓

【步骤6：输出报告】

  4张图 + 误差指标打印 + CSV数据文件

 

关键公式文字翻译

 

三次多项式轨迹：q(t) = a·t³ + b·t² + d。四个系数由起点位置、起点速度（零）、终点位置、终点速度（零）确定，保证每段内位置、速度连续。

 

PD+重力前馈控制律：τ = Kp·(q_des - q) + Kd·(qd_des - qd) + G(q_des)。Kp乘以角度偏差产生恢复力矩；Kd乘以角速度偏差提供阻尼；G是按期望构型计算的重力前馈。

 

末端位置误差：e_end = [x_des - x_actual, y_des - y_actual]。由角度误差经运动学传递得到，反映装配的实际跟踪精度。

 

三类误差指标：

 

- 最大误差：全程末端位置误差范数的最大值，反映最恶劣工况下的偏差，决定能否通过间隙最小的装配孔。

- 平均误差：全程误差范数的算术平均，反映整体跟踪水平，评估工艺能力指数。

- 均方根误差：误差平方均值的平方根，对大偏差敏感，惩罚偶发的尖峰误差，比平均误差更能反映装配冲击风险。

 

科氏力向量（Spong标准结论）：在M·qdd + C + G = τ约定下，C1 = -h·sin(q2)·(2·qd1·qd2 + qd2²)，C2 = h·sin(q2)·qd1²，其中h = m2·l1·r2 + mt·l1·l2。这一项描述关节速度之间的耦合——高速段速度大，科氏力误差会显著污染仿真结果。

 

4. OOP代码实现

 

技术栈

 

限定：numpy, matplotlib。不引入pybullet、roboticstoolbox等第三方重型机器人库。

 

项目结构树

 

assembly_line_sim/

├── run_sim.py # 主入口

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py # dataclass数据模型

│ ├── dynamics.py # 2R刚体动力学

│ ├── kinematics.py # 正运动学

│ ├── trajectory.py # 多段三次多项式轨迹（预计算）

│ ├── controller.py # PD+重力前馈

│ ├── simulator.py # RK4仿真器（准RK4）

│ ├── metrics.py # 三类误差评价指标

│ └── visualize.py # 4张图表 + CSV报告

├── tests/

│ ├── __init__.py

│ └── test_integration.py # 3类10个测试

├── results/ # 4张图表 + CSV

└── README.md

 

核心源码

 

"robot/models.py"

 

"""

Data models for 2R planar manipulator assembly simulation.

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from dataclasses import dataclass

 

 

@dataclass

class Link2R:

    """2R竖直平面臂单连杆参数。"""

    name: str

    mass: float # kg

    length: float # m

    inertia: float # kg·m^2

    com_offset: float # m（质心距前一关节）

 

    def __post_init__(self):

        if self.mass <= 0:

            raise ValueError(f"{self.name}: mass must be positive")

        if self.length <= 0:

            raise ValueError(f"{self.name}: length must be positive")

        if self.inertia <= 0:

            raise ValueError(f"{self.name}: inertia must be positive")

        if not 0 < self.com_offset <= self.length:

            raise ValueError(

                f"{self.name}: com_offset must be in (0, {self.length}]")

 

 

@dataclass

class PDGains2R:

    """2R PD增益。"""

    Kp: np.ndarray

    Kd: np.ndarray

 

    def __post_init__(self):

        self.Kp = np.asarray(self.Kp, dtype=float)

        self.Kd = np.asarray(self.Kd, dtype=float)

        if self.Kp.shape != (2,):

            raise ValueError(f"Kp must be length-2, got {self.Kp.shape}")

        if self.Kd.shape != (2,):

            raise ValueError(f"Kd must be length-2, got {self.Kd.shape}")

        if np.any(self.Kp <= 0) or np.any(self.Kd < 0):

            raise ValueError("Kp must be positive, Kd non-negative")

 

 

@dataclass

class SimConfig2R:

    """2R仿真全局配置。"""

    links: list

    tool_mass: float = 0.0 # kg，末端工具质量

    gravity: float = 9.81

    dt_phys: float = 0.0001

    tau_limit: float = 50.0

 

    def __post_init__(self):

        if len(self.links) != 2:

            raise ValueError(

                f"SimConfig2R requires exactly 2 links, got {len(self.links)}")

        if self.tool_mass < 0:

            raise ValueError("tool_mass must be non-negative")

        if self.dt_phys <= 0:

            raise ValueError("dt_phys must be positive")

        if self.tau_limit <= 0:

            raise ValueError("tau_limit must be positive")

 

"robot/dynamics.py"

 

"""

2R竖直平面刚体动力学。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import SimConfig2R

 

 

class Dynamics2R:

    """

    2R竖直平面臂动力学：M(q)·qdd + C(q,qd) + G(q) = τ

    连杆+工具参数全部内置，自包含可复现。

    质量矩阵与科氏力向量均按Spong标准结论推导，并经Christoffel符号与能量守恒检验。

    """

    def __init__(self, config: SimConfig2R):

        self.cfg = config

        self.g = config.gravity

        self.m_tool = config.tool_mass

 

        self.l1 = config.links[0].length

        self.l2 = config.links[1].length

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

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

        self.r1 = config.links[0].com_offset

        self.r2 = config.links[1].com_offset

        self.I1 = config.links[0].inertia

        self.I2 = config.links[1].inertia

 

    def mass_matrix(self, q):

        """

        2×2质量矩阵M(q)。

        M11 = I1 + m1·r1² + m2·(l1²+r2²+2·l1·r2·cos(q2))

              + mt·(l1²+l2²+2·l1·l2·cos(q2))

        M12 = I2 + m2·r2² + m2·l1·r2·cos(q2)

              + mt·(l2² + l1·l2·cos(q2))

        M22 = I2 + m2·r2² + mt·l2²

        """

        q = np.asarray(q, dtype=float)

        c2 = math.cos(q[1])

 

        l1, l2 = self.l1, self.l2

        r1, r2 = self.r1, self.r2

        m1, m2 = self.m1, self.m2

        mt = self.m_tool

 

        # 连杆+工具（末端质点）的惯量项

        M11 = (self.I1 + m1*r1**2

               + m2*(l1**2 + r2**2 + 2*l1*r2*c2)

               + mt*(l1**2 + l2**2 + 2*l1*l2*c2))

 

        # M12必须包含I2，否则耦合项偏小，直接改变跟踪误差数值

        M12 = (self.I2 + m2*r2**2 + m2*l1*r2*c2

               + mt*l2**2 + mt*l1*l2*c2)

 

        M22 = self.I2 + m2*r2**2 + mt*l2**2

 

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

                         [M12, M22]], dtype=float)

 

    def coriolis_vector(self, q, qd):

        """

        科氏力/离心力向量C(q,qd)，Spong标准结论。

        令 h = m2·l1·r2 + mt·l1·l2，则：

        C1 = -h·sin(q2)·(2·qd1·qd2 + qd2²)

        C2 = h·sin(q2)·qd1²

        """

        q = np.asarray(q, dtype=float)

        qd = np.asarray(qd, dtype=float)

        s2 = math.sin(q[1])

 

        l1, l2 = self.l1, self.l2

        m2 = self.m2

        mt = self.m_tool

 

        # 耦合惯量系数h

        h = m2 * l1 * r2 + mt * l1 * l2

 

        C1 = -h * s2 * (2.0 * qd[0] * qd[1] + qd[1]**2)

        C2 = h * s2 * (qd[0]**2)

 

        return np.array([C1, C2], dtype=float)

 

    def gravity_vector(self, q):

        """重力向量G(q)，含工具质量。"""

        q = np.asarray(q, dtype=float)

        c1 = math.cos(q[0])

        c12 = math.cos(q[0] + q[1])

 

        l1, l2 = self.l1, self.l2

        r1, r2 = self.r1, self.r2

        m1, m2 = self.m1, self.m2

        mt = self.m_tool

 

        G1 = (m1*self.g*r1*c1

              + m2*self.g*(l1*c1 + r2*c12)

              + mt*self.g*(l1*c1 + l2*c12))

 

        G2 = (m2*self.g*r2*c12

              + mt*self.g*l2*c12)

 

        return np.array([G1, G2], dtype=float)

 

    def forward_dynamics(self, q, qd, tau):

        """正向动力学：qdd = M⁻¹·(τ - C - G)。"""

        M = self.mass_matrix(q)

        C = self.coriolis_vector(q, qd)

        G = self.gravity_vector(q)

        rhs = np.asarray(tau, dtype=float) - C - G

        qdd = np.linalg.solve(M, rhs)

        return qdd

 

"robot/kinematics.py"

 

"""

2R竖直平面臂正运动学。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import SimConfig2R

 

 

class ForwardKinematics2R:

    """末端位置 x = l1·cos(q1) + l2·cos(q1+q2), y = l1·sin(q1) + l2·sin(q1+q2)。"""

 

    def __init__(self, config: SimConfig2R):

        self.l1 = config.links[0].length

        self.l2 = config.links[1].length

 

    def end_effector_pos(self, q):

        """返回末端坐标 [x, y]。"""

        q = np.asarray(q, dtype=float)

        theta1 = q[0]

        theta2 = q[0] + q[1]

 

        x = self.l1 * math.cos(theta1) + self.l2 * math.cos(theta2)

        y = self.l1 * math.sin(theta1) + self.l2 * math.sin(theta2)

 

        return np.array([x, y], dtype=float)

 

    def end_effector_pos_batch(self, q_arr):

        """批量计算末端位置。"""

        n = len(q_arr)

        pos = np.zeros((n, 2))

        for i in range(n):

            pos[i] = self.end_effector_pos(q_arr[i])

        return pos

 

"robot/trajectory.py"

 

"""

多段三次多项式轨迹生成器（预计算所有时刻的期望量）。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import SimConfig2R

 

 

class MultiSegmentCubicTrajectory:

    """

    多段三次多项式轨迹：每段 q(t) = a·t³ + b·t² + d

    满足 q(0)=q_start, qd(0)=0, q(T)=q_end, qd(T)=0。

    预计算全部时刻的 q_des, qd_des, qdd_des，仿真循环内直接索引。

    """

    def __init__(self, waypoints, segment_durations, total_time, dt):

        self.waypoints = [np.asarray(wp, dtype=float) for wp in waypoints]

        self.segment_durations = [float(d) for d in segment_durations]

        self.total_time = float(total_time)

        self.dt = float(dt)

 

        if len(self.waypoints) != len(self.segment_durations) + 1:

            raise ValueError("waypoints must be one more than segment_durations")

 

        # 验证总时长一致性

        sum_dur = sum(self.segment_durations)

        if abs(sum_dur - self.total_time) > 1e-9:

            raise ValueError(

                f"Sum of segment durations ({sum_dur}) != total_time ({total_time})")

 

        # 预计算每段系数

        self._segments = [] # (a, b, d, T, t_start)

        t_start = 0.0

        for i in range(len(self.segment_durations)):

            q_start = self.waypoints[i]

            q_end = self.waypoints[i + 1]

            T = self.segment_durations[i]

            delta = q_end - q_start

 

            a = 2.0 * delta / (T**3)

            b = -3.0 * delta / (T**2)

            d = q_start.copy()

 

            self._segments.append((a, b, d, T, t_start))

            t_start += T

 

        # 预计算所有时刻的期望量

        n_steps = int(round(total_time / dt))

        self.n_steps = n_steps

        self.t_arr = np.arange(n_steps) * dt

        self.q_des = np.zeros((n_steps, 2))

        self.qd_des = np.zeros((n_steps, 2))

        self.qdd_des = np.zeros((n_steps, 2))

 

        for i in range(n_steps):

            q_des, qd_des, qdd_des = self._eval_at(self.t_arr[i])

            self.q_des[i] = q_des

            self.qd_des[i] = qd_des

            self.qdd_des[i] = qdd_des

 

    def _eval_at(self, t):

        """在给定时刻t求三次多项式值。"""

        if t <= 0:

            return self.waypoints[0].copy(), np.zeros(2), np.zeros(2)

        if t >= sum(self.segment_durations):

            return self.waypoints[-1].copy(), np.zeros(2), np.zeros(2)

 

        t_local = t

        for a, b, d, T, t_start in self._segments:

            if t_local <= t_start + T:

                t_rel = t_local - t_start

                t2 = t_rel * t_rel

                t3 = t2 * t_rel

 

                q = a * t3 + b * t2 + d

                qd = 3.0 * a * t2 + 2.0 * b * t_rel

                qdd = 6.0 * a * t_rel + 2.0 * b

 

                return q, qd, qdd

            # 不在本段，继续

 

        # 不应到达

        return self.waypoints[-1].copy(), np.zeros(2), np.zeros(2)

 

    def get_at_index(self, idx):

        """按索引直接返回预计算的期望量。"""

        return (self.q_des[idx], self.qd_des[idx], self.qdd_des[idx])

 

"robot/controller.py"

 

"""

PD控制器 + 重力前馈。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from robot.models import PDGains2R

from robot.dynamics import Dynamics2R

 

 

class PDController2R:

    """

    PD控制律：τ = Kp·(q_des - q) + Kd·(qd_des - qd) + G(q_des)

    重力前馈使用期望位形下的重力向量（含工具质量）。

    """

    def __init__(self, gains: PDGains2R, dynamics: Dynamics2R):

        self.Kp = gains.Kp

        self.Kd = gains.Kd

        self.dyn = dynamics

 

    def compute_torque(self, q, qd, q_des, qd_des):

        """计算控制力矩。"""

        e = q_des - q

        ed = qd_des - qd

        tau_pd = self.Kp * e + self.Kd * ed

 

        # 重力前馈：用期望关节角计算

        grav_ff = self.dyn.gravity_vector(q_des)

        tau = tau_pd + grav_ff

 

        return tau

 

"robot/simulator.py"

 

"""

RK4仿真器：记录装配闭环的力矩、角度、末端误差。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from robot.dynamics import Dynamics2R

from robot.kinematics import ForwardKinematics2R

from robot.controller import PDController2R

from robot.models import SimConfig2R

 

 

class AssemblySimulator:

    def __init__(self, dynamics: Dynamics2R, controller: PDController2R,

                 kinematics: ForwardKinematics2R, trajectory,

                 config: SimConfig2R):

        self.dyn = dynamics

        self.ctrl = controller

        self.fk = kinematics

        self.traj = trajectory

        self.config = config

 

        self.dt = config.dt_phys

        self.n_steps = trajectory.n_steps

 

    def run(self):

        """

        运行完整仿真，返回时程数据。

        采用准RK4：控制力矩在整步内零阶保持，RK4各子步内使用同一个tau_fixed。

        这是控制延迟的合理工程近似，dt=0.1ms下精度足够。

        """

        # 初始状态从轨迹起点对齐

        q_start, qd_start, _ = self.traj.get_at_index(0)

        x = np.concatenate([q_start, qd_start])

 

        t_arr = self.traj.t_arr

 

        # 预计算期望末端位置

        end_pos_des = self.fk.end_effector_pos_batch(self.traj.q_des)

 

        # 预分配历史数组

        q_actual = np.zeros((self.n_steps, 2))

        qd_actual = np.zeros((self.n_steps, 2))

        error_hist = np.zeros((self.n_steps, 2))

        tau_hist = np.zeros((self.n_steps, 2))

        end_pos_hist = np.zeros((self.n_steps, 2))

        end_error_hist = np.zeros((self.n_steps, 2))

 

        for i in range(self.n_steps):

            # 期望量（直接索引预计算结果）

            q_des, qd_des, _ = self.traj.get_at_index(i)

 

            # 当前状态

            q = x[:2]

            qd = x[2:4]

 

            # 记录

            q_actual[i] = q

            qd_actual[i] = qd

            error_hist[i] = q_des - q

 

            # 末端位置

            end_pos_hist[i] = self.fk.end_effector_pos(q)

            end_error_hist[i] = end_pos_des[i] - end_pos_hist[i]

 

            # 控制力矩（整步内零阶保持，RK4各子步使用同一tau）

            tau = self.ctrl.compute_torque(q, qd, q_des, qd_des)

            tau = np.clip(tau, -self.config.tau_limit, self.config.tau_limit)

            tau_hist[i] = tau

 

            # RK4积分

            def deriv(xx, tau_fixed=tau):

                qdd = self.dyn.forward_dynamics(xx[:2], xx[2:4], tau_fixed)

                return np.concatenate([xx[2:4], qdd])

 

            k1 = deriv(x)

            k2 = deriv(x + 0.5 * self.dt * k1)

            k3 = deriv(x + 0.5 * self.dt * k2)

            k4 = deriv(x + self.dt * k3)

            x = x + (self.dt / 6.0) * (k1 + 2*k2 + 2*k3 + k4)

 

        return {

            't': t_arr,

            'q_des': self.traj.q_des.copy(),

            'q_actual': q_actual,

            'qd_actual': qd_actual,

            'error': error_hist,

            'tau': tau_hist,

            'end_pos': end_pos_hist,

            'end_pos_des': end_pos_des,

            'end_error': end_error_hist,

        }

 

"robot/metrics.py"

 

"""

装配闭环运动误差三类评价指标：最大误差、平均误差、均方根误差。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

 

 

class ErrorMetrics:

    """

    计算末端位置误差的三类指标：

    - 最大误差：max(‖e_end‖)

    - 平均误差：mean(‖e_end‖)

    - 均方根误差：sqrt(mean(‖e_end‖²))

    """

 

    def __init__(self, end_error):

        """

        end_error: (n_steps, 2) array，末端位置误差 [m]

        """

        self.end_error = np.asarray(end_error, dtype=float)

        if self.end_error.ndim != 2 or self.end_error.shape[1] != 2:

            raise ValueError("end_error must be (n_steps, 2)")

 

        # 各时刻误差范数

        self.norms = np.linalg.norm(self.end_error, axis=1)

 

    def max_error(self):

        """最大误差（m）。"""

        return float(np.max(self.norms))

 

    def mean_error(self):

        """平均误差（m）。"""

        return float(np.mean(self.norms))

 

    def rms_error(self):

        """均方根误差（m）。"""

        return float(np.sqrt(np.mean(self.norms**2)))

 

    def report(self):

        """返回格式化报告字典。"""

        return {

            'max_error_m': self.max_error(),

            'mean_error_m': self.mean_error(),

            'rms_error_m': self.rms_error(),

            'max_error_mm': self.max_error() * 1000,

            'mean_error_mm': self.mean_error() * 1000,

            'rms_error_mm': self.rms_error() * 1000,

        }

 

"robot/visualize.py"

 

"""

4张装配闭环仿真可视化图表 + CSV报告。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import os

import csv

import numpy as np

import matplotlib.pyplot as plt

 

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

                                    'Noto Sans CJK SC']

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

 

 

class Visualizer2R:

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

        self.rd = results_dir

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_angle_tracking(self, result):

        """图1：期望角度 vs 实际角度时程。"""

        t = result['t']

        q_des = np.degrees(result['q_des'])

        q_act = np.degrees(result['q_actual'])

 

        fig, axes = plt.subplots(2, 1, figsize=(11, 7), sharex=True)

 

        labels = ['关节1', '关节2']

        for j in range(2):

            axes[j].plot(t, q_des[:, j], 'k--', linewidth=1.5, label='期望角度')

            axes[j].plot(t, q_act[:, j], 'b-', linewidth=1.0, label='实际角度', alpha=0.8)

            axes[j].set_ylabel(f'{labels[j]}角度 (deg)')

            axes[j].legend(fontsize=7)

            axes[j].grid(True, alpha=0.3)

 

        axes[-1].set_xlabel('时间 (s)')

        fig.suptitle('装配闭环关节角度跟踪')

        plt.tight_layout()

        path = os.path.join(self.rd, "01_angle_tracking.png")

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_torque(self, result):

        """图2：控制力矩时程。"""

        t = result['t']

        tau = result['tau']

 

        fig, axes = plt.subplots(2, 1, figsize=(11, 7), sharex=True)

 

        labels = ['关节1', '关节2']

        for j in range(2):

            axes[j].plot(t, tau[:, j], 'r-', linewidth=1.0)

            axes[j].axhline(0, color='k', linestyle='-', linewidth=0.5)

            axes[j].set_ylabel(f'{labels[j]}力矩 (N·m)')

            axes[j].grid(True, alpha=0.3)

 

        axes[-1].set_xlabel('时间 (s)')

        fig.suptitle('装配闭环控制力矩时程')

        plt.tight_layout()

        path = os.path.join(self.rd, "02_torque.png")

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_end_effector_error(self, result):

        """图3：末端位置误差时程（XY分量）。"""

        t = result['t']

        e_end = result['end_error'] * 1000 # m → mm

 

        fig, axes = plt.subplots(2, 1, figsize=(11, 7), sharex=True)

 

        axes[0].plot(t, e_end[:, 0], 'b-', linewidth=1.0)

        axes[0].axhline(0, color='k', linestyle='-', linewidth=0.5)

        axes[0].set_ylabel('末端X误差 (mm)')

        axes[0].grid(True, alpha=0.3)

 

        axes[1].plot(t, e_end[:, 1], 'r-', linewidth=1.0)

        axes[1].axhline(0, color='k', linestyle='-', linewidth=0.5)

        axes[1].set

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