《Python机器人基础原理》第一百三十四篇：焊接直线轨迹仿真——笛卡尔路径经过奇异附近，统计全程末端位置跟踪误差

 

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

 

1. 实际工业应用场景

 

周三上午，车身侧围弧焊工位。2R平面焊接机械臂末端夹着焊枪，沿一条0.30 m长的水平直线从(0.98 m, 0.05 m)走到(0.70 m, 0.10 m)，速度0.15 m/s。轨迹中段机械臂接近完全伸直——末端距基座约0.98 m，几乎达到最大工作半径1.00 m，肘部关节角q2趋近于0 rad。

 

"又抖了。"调试工程师小刘盯着HMI上跳动的位置误差曲线，"中段误差从0.5 mm飙到4.3 mm，焊缝宽度明显不均匀。"

 

"这是典型的肘型2R臂完全伸直奇异。"控制工程师老周调出离线仿真，"2R平面构型，L1=0.55 m / L2=0.45 m，连杆质量8.0 kg / 5.0 kg，转动惯量0.60 kg·m² / 0.18 kg·m²，质心0.28 m / 0.22 m，末端负载2.0 kg。PD用Kp=[600, 450] N·m/rad、Kd=[60, 45] N·m·s/rad。轨迹从(0.98 m, 0.05 m)水平走到(0.70 m, 0.10 m)，中途经过q2≈0的奇异区域——此时雅可比最小奇异值趋近0，末端几乎失去一个方向的运动能力。跑一下全程末端位置误差，看奇异附近到底放大了多少。"

 

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

 

2. 痛点分析

 

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

第3章 连杆参数及运动学求解 → 逆运动学 奇异构型附近逆解对笛卡尔误差极度敏感，微小笛卡尔偏差需要极大关节运动补偿

第4章 机器人动力学建模 → 闭式动力学 奇异附近质量矩阵条件数恶化，相同控制力矩产生的关节加速度剧烈变化

第6章 轨迹规划与控制 → 跟踪误差量化 直线轨迹经过奇异附近时，全程误差分布不均匀，需要逐点统计而非只看均值

 

本篇代码解决 L2 离线仿真校核层。输入：2R连杆参数、直线路点、速度、PD增益。处理：①笛卡尔直线插值生成连续期望轨迹；②以固定微步长（0.1 ms）做RK4物理积分；③PD+重力前馈控制；④逐点统计末端位置误差及雅可比条件数。输出：4张分析图表 + 单元测试。

 

不覆盖：六轴逆解数值迭代、实时操作系统调度、通信抖动、奇异回避轨迹重规划。

 

3. 核心逻辑讲解

 

物理直觉

 

机械臂在奇异点附近就像人伸直胳膊去够正前方的东西——胳膊完全打直时，你稍微想往左挪一点，整个肩膀都要大幅度转动；而胳膊弯曲时，手腕轻轻一动就能到。这就是肘型2R臂完全伸直奇异的本质：当肘角q2=0时，两连杆共线，末端沿连杆延长线方向的速度完全由肩关节决定，肘关节的微小转动对末端位置几乎无贡献。笛卡尔空间的一个小位移，在关节空间需要巨大的运动来响应。PD控制器按关节误差输出力矩，但奇异附近关节误差对末端位置的"杠杆率"急剧下降，同样的力矩推不动末端，误差就被放大了。

 

业务逻辑 → 代码映射

 

输入：连杆参数 + 直线路点 + 速度 + PD增益

  ↓

【步骤1：生成连续期望轨迹】

  笛卡尔直线插值 → 密集路点 → 解析逆解（多解按最近分支筛选）

  → 关节空间三次样条平滑插值，保证qddes连续

  → 物理时间轴以 dt_phys=0.1 ms 均匀铺开

  ↓

【步骤2：RK4仿真】

  每步计算期望关节角/速度 → PD+重力前馈 → 分级力矩限幅

  → 正向动力学 → RK4积分

  ↓

【步骤3：逐点误差与条件数统计】

  记录每拍的末端位置误差、雅可比条件数、奇异度量

  ↓

【步骤4：可视化】

  4张图：误差时程、误差-条件数关系、空间分布、奇异度量时程

 

关键公式文字翻译

 

雅可比矩阵条件数：κ(J) = σ_max / σ_min，即最大奇异值与最小奇异值的比值。κ越大，矩阵越接近奇异，单位关节运动产生的末端位移越小，控制器的等效笛卡尔刚度越低。

 

奇异度量：σ_min(J)，即最小奇异值。它直接反映距离奇异点的远近——值越小越危险，为0时完全奇异。

 

末端位置误差：e_pos(t) = || FK(q_act(t)) - FK(q_des(t)) ||。单位毫米，方便与焊接工艺窗口（通常±0.5 mm）对比。

 

4. OOP代码实现

 

技术栈

 

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

 

项目结构树

 

singularity_sim/

├── run_sim.py # 主入口（奇异附近直线轨迹仿真）

├── requirements.txt

├── robot/

│ ├── __init__.py

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

│ ├── inverse_kinematics.py # 解析逆解

│ ├── trajectory.py # 直线插值+关节样条轨迹

│ ├── dynamics.py # 2R闭式动力学（含参数化FK和雅可比）

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

│ ├── simulator.py # RK4仿真器

│ └── visualize.py # 4张图表

├── tests/

│ ├── __init__.py

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

├── results/ # 4张图表

└── README.md

 

核心源码

 

"robot/models.py"

 

"""

Data models for singularity-proximity welding trajectory simulation.

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from dataclasses import dataclass

 

 

@dataclass

class LinkInertia:

    """连杆惯性参数结构体。"""

    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, length]")

 

 

@dataclass

class Payload:

    """

    末端负载。

    质心默认位于末端执行器末端（距关节2距离 = l2），

    即负载力臂为 l2，与连杆2末端重合。

    """

    mass: float = 0.0 # kg

    def __post_init__(self):

        if self.mass < 0:

            raise ValueError("Payload mass cannot be negative")

 

 

@dataclass

class PDGains:

    """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 SimConfig:

    """

    仿真全局配置。

    tau_limit_j1 / tau_limit_j2 分别为关节1、关节2的力矩限幅 (N·m)。

    """

    links: list

    gravity: float = 9.81

    dt_phys: float = 0.0001 # 物理积分步长（固定0.1 ms）

    tau_limit_j1: float = 100.0 # 关节1力矩限幅 (N·m)

    tau_limit_j2: float = 80.0 # 关节2力矩限幅 (N·m)

 

    def __post_init__(self):

        if not self.links or len(self.links) < 2:

            raise ValueError("SimConfig.links must contain at least 2 links")

        if self.dt_phys <= 0:

            raise ValueError("SimConfig.dt_phys must be positive")

        if self.tau_limit_j1 <= 0 or self.tau_limit_j2 <= 0:

            raise ValueError("tau_limit must be positive")

 

"robot/inverse_kinematics.py"

 

"""

解析逆运动学：平面2R机械臂。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import SimConfig

 

 

class InverseKinematics2R:

    """

    平面2R解析逆解，多解按与上一拍最近分支筛选。

    肘型2R臂的奇异位形为 q2=0（完全伸直）和 q2=±π（完全折叠）。

    """

    def __init__(self, config: SimConfig):

        if len(config.links) < 2:

            raise ValueError("InverseKinematics2R requires at least 2 links")

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

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

 

    def solve_single(self, x, y, q_target=None):

        """

        返回最优逆解。

        工作空间判断：r 超出 [|L1-L2|, L1+L2] 时抛出 ValueError。

        内环 r < |L1-L2| 同样不可达（折叠奇异）。

        """

        x, y = float(x), float(y)

        r2 = x * x + y * y

        r = math.sqrt(r2)

 

        max_reach = self.l1 + self.l2

        min_reach = abs(self.l1 - self.l2)

        if r > max_reach + 1e-9 or r < min_reach - 1e-9:

            raise ValueError(

                f"Position ({x:.4f}, {y:.4f}) unreachable: "

                f"r={r:.4f} outside [{min_reach}, {max_reach}]")

 

        cos_q2 = (r2 - self.l1**2 - self.l2**2) / (2.0 * self.l1 * self.l2)

        cos_q2 = max(-1.0, min(1.0, cos_q2))

        q2_a = math.acos(cos_q2)

        q2_b = -q2_a

 

        solutions = []

        for q2 in [q2_a, q2_b]:

            phi = math.atan2(self.l2 * math.sin(q2),

                             self.l1 + self.l2 * math.cos(q2))

            q1 = math.atan2(y, x) - phi

            solutions.append(np.array([q1, q2]))

 

        if q_target is not None:

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

            solutions.sort(key=lambda q: np.linalg.norm(q - q_target))

 

        return solutions[0].copy()

 

"robot/trajectory.py"

 

"""

直线笛卡尔插值 + 关节空间三次样条平滑轨迹。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from scipy.interpolate import CubicSpline

 

 

class LinearCartesianTrajectory:

    """

    直线轨迹：按弧长等比例密集采样。

    """

    def __init__(self, start, end, speed):

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

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

        self.speed = float(speed)

 

        if self.speed <= 0:

            raise ValueError(f"Speed must be positive, got {self.speed}")

 

        self.length = np.linalg.norm(self.end - self.start)

        self.total_time = self.length / self.speed

 

    def get_position_at_time(self, t):

        """返回t时刻的笛卡尔坐标。"""

        if t <= 0:

            return self.start.copy()

        if t >= self.total_time:

            return self.end.copy()

 

        alpha = t / self.total_time

        return (1.0 - alpha) * self.start + alpha * self.end

 

 

class SplineJointTrajectory:

    """

    关节空间三次样条轨迹。

    对逆解序列做三次样条插值，保证 q 和 qd 连续，

    避免线性插值导致的期望速度阶梯跳变污染误差统计。

    """

    def __init__(self, time_grid, joint_positions):

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

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

        if len(self.time_grid) != len(self.joint_positions):

            raise ValueError("time_grid and joint_positions length mismatch")

 

        self.t_start = self.time_grid[0]

        self.t_end = self.time_grid[-1]

 

        # 对每个关节分别拟合三次样条

        self.splines = [

            CubicSpline(self.time_grid, self.joint_positions[:, i])

            for i in range(2)

        ]

 

    def get_at(self, t):

        """返回t时刻的关节角和速度。"""

        if t <= self.t_start:

            q = np.array([self.splines[i](self.t_start) for i in range(2)])

            qd = np.array([self.splines[i](self.t_start, 1) for i in range(2)])

            return q, qd

        if t >= self.t_end:

            q = np.array([self.splines[i](self.t_end) for i in range(2)])

            qd = np.array([self.splines[i](self.t_end, 1) for i in range(2)])

            return q, qd

 

        q = np.array([self.splines[i](t) for i in range(2)])

        qd = np.array([self.splines[i](t, 1) for i in range(2)])

        return q, qd

 

"robot/dynamics.py"

 

"""

2R planar rigid-body dynamics with payload support.

含参数化FK和雅可比矩阵，用于奇异分析。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import warnings

import numpy as np

import math

from robot.models import SimConfig, Payload

 

 

class Dynamics2R:

    def __init__(self, config: SimConfig):

        self.cfg = config

        self.links = config.links

        self.g = config.gravity

        self.payload = Payload(0.0)

 

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

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

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

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

 

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

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

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

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

 

    def set_payload(self, p: Payload):

        self.payload = p

 

    def forward_kinematics(self, q):

        """参数化正运动学，从config读取连杆长度。"""

        x = self.l1 * math.cos(q[0]) + self.l2 * math.cos(q[0] + q[1])

        y = self.l1 * math.sin(q[0]) + self.l2 * math.sin(q[0] + q[1])

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

 

    def geometric_jacobian(self, q):

        """

        几何雅可比矩阵 J(q) ∈ R^{2×2}。

        列向量分别为各关节单位角速度引起的末端线速度。

        """

        q1, q2 = q[0], q[1]

        J = np.zeros((2, 2))

        J[0, 0] = -self.l1 * math.sin(q1) - self.l2 * math.sin(q1 + q2)

        J[1, 0] = self.l1 * math.cos(q1) + self.l2 * math.cos(q1 + q2)

        J[0, 1] = -self.l2 * math.sin(q1 + q2)

        J[1, 1] = self.l2 * math.cos(q1 + q2)

        return J

 

    def jacobian_condition_number(self, q):

        """雅可比矩阵条件数 κ(J) = σ_max / σ_min。"""

        J = self.geometric_jacobian(q)

        try:

            singular_values = np.linalg.svd(J, compute_uv=False)

            if singular_values[1] < 1e-12:

                return float('inf')

            return singular_values[0] / singular_values[1]

        except np.linalg.LinAlgError:

            return float('inf')

 

    def manipulability(self, q):

        """

        可操纵性度量 = |det J|。

        对2×2平面雅可比，det(J) = L1·L2·sin(q2)，

        值越小越接近奇异（q2→0或±π时 det→0）。

        """

        J = self.geometric_jacobian(q)

        try:

            return abs(np.linalg.det(J))

        except np.linalg.LinAlgError:

            return 0.0

 

    def mass_matrix(self, q):

        """2R质量矩阵M(q)。"""

        q2 = q[1]

        c2 = math.cos(q2)

        mp = self.payload.mass

        M11 = (self.I1 + self.I2 + mp * self.l2**2

               + self.m1 * self.r1**2

               + self.m2 * (self.l1**2 + self.r2**2

                            + 2 * self.l1 * self.r2 * c2)

               + mp * (self.l1**2 + self.l2**2

                       + 2 * self.l1 * self.l2 * c2))

        M12 = (self.I2

               + self.m2 * self.r2 * (self.r2 + self.l1 * c2)

               + mp * self.l2 * (self.l2 + self.l1 * c2))

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

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

 

    def coriolis_vector(self, q, qd):

        """科氏力/离心力向量C(q,qd)·qd。"""

        q2 = q[1]

        s2 = math.sin(q2)

        mp = self.payload.mass

        h = (self.m2 * self.l1 * self.r2

             + mp * self.l1 * self.l2) * s2

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

        C2 = -h * qd[0]**2

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

 

    def gravity_vector(self, q):

        """重力向量 G(q)。"""

        q1, q2 = q[0], q[1]

        c1 = math.cos(q1)

        c12 = math.cos(q1 + q2)

        mp = self.payload.mass

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

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

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

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

              + mp * self.g * self.l2 * c12)

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

 

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

        """

        正向动力学：τ - C(q,qd) - G(q) = M(q)·qdd

        解析2×2求逆加速计算；行列式接近零时警告。

        """

        M = self.mass_matrix(q)

        Cqd = self.coriolis_vector(q, qd)

        G = self.gravity_vector(q)

        rhs = tau - Cqd - G

 

        det = M[0, 0] * M[1, 1] - M[0, 1] * M[1, 0]

        if abs(det) < 1e-12:

            warnings.warn(f"Mass matrix near-singular, det={det:.2e}",

                          RuntimeWarning)

            return np.zeros(2)

        inv_M = np.array([[M[1, 1], -M[0, 1]],

                           [-M[1, 0], M[0, 0]]]) / det

        return inv_M @ rhs

 

"robot/controller.py"

 

"""

PD控制器 + 重力前馈。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from robot.models import PDGains

 

 

class PDController:

    def __init__(self, gains: PDGains, gravity_compensation=True):

        self.Kp = gains.Kp

        self.Kd = gains.Kd

        self.gravity_compensation = gravity_compensation

 

    def compute_torque(self, q, qd, q_des, qd_des, gravity_torque=None):

        """

        PD + 重力前馈。

        重力前馈使用期望位形下的重力向量 G(q_des)。

        """

        e = q_des - q

        ed = qd_des - qd

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

 

        if self.gravity_compensation and gravity_torque is not None:

            tau = tau_pd + gravity_torque

        else:

            tau = tau_pd

 

        return tau

 

"robot/simulator.py"

 

"""

RK4仿真器：焊接直线轨迹，经过奇异附近。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from robot.dynamics import Dynamics2R

from robot.controller import PDController

from robot.models import SimConfig, Payload

 

 

class Simulator:

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

                 trajectory, dt_phys, t_total, config: SimConfig):

        self.dyn = dynamics

        self.ctrl = controller

        self.traj = trajectory

        self.dt_phys = dt_phys

        self.t_total = t_total

        self.config = config

        self.n_phys_steps = int(round(t_total / dt_phys))

 

    def run(self, q_start, payload_mass=2.0):

        """

        运行仿真，返回历史字典。

        每步记录积分后的最终状态，避免丢失最后一拍。

        """

        n = self.n_phys_steps

        t_arr = np.arange(n) * self.dt_phys

 

        # 预分配

        hist = {

            't': t_arr,

            'q1': np.zeros(n), 'q2': np.zeros(n),

            'qd1': np.zeros(n), 'qd2': np.zeros(n),

            'tau1': np.zeros(n), 'tau2': np.zeros(n),

            'q1_des': np.zeros(n), 'q2_des': np.zeros(n),

            'qd1_des': np.zeros(n), 'qd2_des': np.zeros(n),

            'pos_errors_mm': np.zeros(n),

            'condition_numbers': np.zeros(n),

            'manipulabilities': np.zeros(n),

            'x_act': np.zeros(n),

            'y_act': np.zeros(n),

        }

 

        self.dyn.set_payload(Payload(payload_mass))

 

        x = np.concatenate([q_start, np.zeros(2)])

 

        for i in range(n):

            t = t_arr[i]

 

            # 期望轨迹

            q_des, qd_des = self.traj.get_at(t)

 

            # 记录期望

            hist['q1_des'][i] = q_des[0]

            hist['q2_des'][i] = q_des[1]

            hist['qd1_des'][i] = qd_des[0]

            hist['qd2_des'][i] = qd_des[1]

 

            # 正运动学误差（参数化FK，与连杆参数同步）

            p_act = self.dyn.forward_kinematics(x[:2])

            p_des = self.dyn.forward_kinematics(q_des)

            hist['pos_errors_mm'][i] = np.linalg.norm(

                p_act - p_des) * 1000.0

 

            # 记录实际末端位置（供空间分布图使用）

            hist['x_act'][i] = p_act[0]

            hist['y_act'][i] = p_act[1]

 

            # 雅可比条件数和可操纵性

            hist['condition_numbers'][i] = \

                self.dyn.jacobian_condition_number(x[:2])

            hist['manipulabilities'][i] = \

                self.dyn.manipulability(x[:2])

 

            # 记录实际状态

            hist['q1'][i] = x[0]

            hist['q2'][i] = x[1]

            hist['qd1'][i] = x[2]

            hist['qd2'][i] = x[3]

 

            # 控制力矩

            grav_ff = self.dyn.gravity_vector(q_des)

            tau = self.ctrl.compute_torque(

                x[:2], x[2:4], q_des, qd_des, gravity_torque=grav_ff)

 

            # 按关节分级力矩限幅（对应驱动器饱和）

            tau[0] = np.clip(tau[0],

                              -self.config.tau_limit_j1,

                               self.config.tau_limit_j1)

            tau[1] = np.clip(tau[1],

                              -self.config.tau_limit_j2,

                               self.config.tau_limit_j2)

            hist['tau1'][i] = tau[0]

            hist['tau2'][i] = tau[1]

 

            # 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_phys * k1)

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

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

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

 

        return hist

 

"robot/visualize.py"

 

"""

4张焊接直线轨迹仿真可视化图表。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import os

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

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

        self.rd = results_dir

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_error_time_series(self, hist):

        """图1：末端位置误差时程，标注奇异区域。"""

        fig, ax = plt.subplots(figsize=(11, 5))

 

        ax.plot(hist['t'], hist['pos_errors_mm'], 'b-',

                linewidth=0.6, alpha=0.8)

 

        # 标注奇异区域（条件数 > 50 的区间）

        cond = hist['condition_numbers']

        t = hist['t']

        singular_mask = cond > 50

        if np.any(singular_mask):

            ax.fill_between(t, 0, hist['pos_errors_mm'],

                            where=singular_mask,

                            color='red', alpha=0.2,

                            label='奇异区域 (κ>50)')

 

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

        ax.set_ylabel('末端位置误差 (mm)')

        ax.set_title('焊接直线轨迹全程末端位置误差时程')

        ax.legend(fontsize=8)

        ax.grid(True, alpha=0.3)

        plt.tight_layout()

 

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_error_vs_condition_number(self, hist):

        """图2：末端位置误差 vs 雅可比条件数。"""

        fig, ax = plt.subplots(figsize=(11, 5))

 

        cond = hist['condition_numbers']

        err = hist['pos_errors_mm']

 

        # 只绘制有限值

        mask = np.isfinite(cond) & (cond < 500)

        ax.scatter(cond[mask], err[mask], c='steelblue',

                   s=3, alpha=0.5, edgecolors='none')

 

        ax.set_xlabel('雅可比条件数 κ(J)')

        ax.set_ylabel('末端位置误差 (mm)')

        ax.set_title('末端位置误差 vs 雅可比条件数')

        ax.grid(True, alpha=0.3)

        plt.tight_layout()

 

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_spatial_error_distribution(self, hist, traj_start, traj_end):

        """图3：末端位置误差空间分布。"""

        fig, ax = plt.subplots(figsize=(11, 5))

 

        # 生成期望轨迹点用于背

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