《Python机器人基础原理》第一百九篇：控制器对比仿真——分别记录大P、小P参数下的响应曲线，分析抖动与收敛特性

 

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

 

1. 实际工业应用场景

 

周二下午，伺服压机装配工位。一台2自由度竖直平面机械臂正在做压装力控的调试。小赵盯着示波器上的关节1角度响应曲线——大P参数（Kp₁=300 N·m/rad）下，关节1在0.3秒就冲到了目标位π/4 rad附近，但随后在目标位上下剧烈震荡，峰值超调达到35%，稳定时间拖到2.8秒才收敛；换成小P参数（Kp₁=40 N·m/rad）后，曲线变得像温吞水一样平滑，但到位后停在距离目标0.54 rad的位置——稳态误差是带宽的34倍，误差根本进不了±2%带。

 

"P大了抖，P小了偏——你这不是调参，是在走钢丝。"老郑端着保温杯走过来，在屏幕前站定。

 

小赵揉了揉眼睛："连杆1质量5.8 kg、长度0.40 m、惯量0.35 kg·m²；连杆2质量4.2 kg、长度0.35 m、惯量0.18 kg·m²。在目标位q_des=[π/4, π/6] rad处，M₁₁≈2.07 kg·m²、M₂₂≈0.31 kg·m²。大P下ζ₁ = Kd₁/(2√(Kp₁·M₁₁)) = 15/(2√(300×2.07)) ≈ 0.30，欠阻尼当然抖；ζ₂ = 12/(2√(200×0.31)) ≈ 0.76，接近临界，所以关节2其实不怎么抖。小P下ζ₁ = 15/(2√(40×2.07)) ≈ 0.82，过阻尼到位慢；ζ₂ = 12/(2√(30×0.31)) ≈ 1.97，严重过阻尼。纯PD的稳态误差ess = G(q_des)/Kp，小P下关节1的ess = 21.57/40 ≈ 0.54 rad——根本到不了位。我需要把两组曲线和指标全拉出来，才能跟工艺部门解释为什么两组参数都不合格。"

 

老郑点头："对。大P抖的是关节1，关节2表现还行；小P两组都慢，而且关节1直接偏了0.54 rad。下一篇加重力前馈，把稳态误差从ess=G/Kp里拿掉，同样的Kp下稳态精度直接翻倍，你就有余量把Kp再降一点、把超调压下来。"

 

构型约定：本篇采用竖直平面内2R机械臂。连杆参数：L1 a₁=0.40 m / m₁=5.8 kg / I₁=0.35 kg·m² / com₁=0.20 m；L2 a₂=0.35 m / m₂=4.2 kg / I₂=0.18 kg·m² / com₂=0.175 m。目标位q_des=[π/4, π/6] rad。大P组：Kp=diag(300, 200) N·m/rad，Kd=diag(15, 12) N·m·s/rad。小P组：Kp=diag(40, 30) N·m/rad，Kd=diag(15, 12) N·m·s/rad。力矩限幅±80/±60 N·m。RK4步长0.001 s，仿真5.0 s。技术栈：numpy + matplotlib。

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于PD控制器参数对比分析。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。

 

2. 痛点分析

 

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

第5章 机器人关节驱动与控制 → PD控制器参数整定 大P导致欠阻尼振荡（ζ₁≈0.30）、小P导致稳态误差过大（ess≈0.54 rad），需定量对比找到折中

第4章 机器人动力学建模 → 2R刚体动力学 惯量随位形变化，固定Kd无法匹配不同Kp下的阻尼比；需逐关节计算M对角元

第9章 仿真实验 → 控制器对比仿真 离线仿真对比不同参数组的时域响应，为现场整定提供依据

 

本篇代码解决 L2 离线仿真校核层。输入：2R连杆参数、大P/小P两组PD增益、阶跃目标位、力矩限幅、仿真时长。处理：①构建2R刚体动力学（闭式M/C/G，对称正定）；②两组参数分别独立PD闭环仿真5.0 s；③RK4积分；④计算超调量、稳定时间（含未收敛分支）、稳态误差、控制力矩RMS。输出：4张对比图表 + 性能指标报告。

 

不覆盖：自适应增益、计算力矩控制、重力前馈（后续篇目）、多自由度扩展。

 

3. 核心逻辑讲解

 

物理直觉

 

PD控制器就像一个弹簧-阻尼系统。P（比例）是弹簧——越硬（Kp越大），把关节拉向目标的力越大，但拉得太猛会冲过头。D（微分）是阻尼器——吸收多余动能，防止来回振荡。问题在于：弹簧硬了，阻尼器没跟着加，系统就会"弹个不停"（欠阻尼）；弹簧软了，重力一拉就偏（稳态误差大）。

 

本篇的核心洞察：Kd固定时，Kp的变化直接改变系统的阻尼比ζ = Kd/(2√(Kp·M))，且稳态误差ess = G/Kp。大P把ζ₁压到0.30（欠阻尼→振荡），小P让ζ₁升到0.82（过阻尼→慢），但ess从0.072 rad暴涨到0.539 rad——误差根本进不了±2%带。两组参数各有致命缺陷，这正是纯PD的硬伤。

 

业务逻辑 → 代码映射

 

输入：2R连杆参数，大P/小P两组PD增益，阶跃目标位

  ↓

【步骤1：初始化】

  构建2R动力学（闭式M/C/G，解析公式无数值微分）

  初始化两组PD控制器

  初始状态 x = [0,0, 0,0]

  ↓

【步骤2：RK4仿真5.0秒（每组独立运行）】

  每步：读q/qd → PD算τ（含限幅clip）→ M(q)qdd + C(q,qd) + G(q) = τ → 解qdd → RK4

  记录 q(t), e(t), τ(t) 分别存入大P/小P容器

  ↓

【步骤3：性能指标计算（metrics.py，含未收敛分支）】

  超调量 = 首次越过目标后的正向最大偏差 / |q_des - q₀| × 100%

  稳定时间 = 误差首次进入±2%带且不再越出；若永不进带返回np.inf

  稳态误差 = 最后1.0s平均|e|（未收敛时标注"未收敛"）

  力矩RMS = √(mean(τ²))

  ↓

【步骤4：绘图输出（4张对比图）】

 

关键公式文字翻译

 

阻尼比ζ：ζᵢ = Kdᵢ / (2√(Kpᵢ·Mᵢᵢ(q_des)))。Mᵢᵢ是M矩阵在目标位处的对角元（不是固定值，随位形变）。ζ < 1 → 振荡衰减；ζ ≈ 0.7 → 快速无超调的黄金点；ζ > 1 → 过阻尼（慢但稳）。本篇逐关节计算：大P下ζ₁≈0.30（抖）、ζ₂≈0.76（稳）；小P下ζ₁≈0.82（慢）、ζ₂≈1.97（严重过阻尼）。

 

稳态误差ess：纯PD控制无法消除重力项，essᵢ ≈ Gᵢ(q_des) / Kpᵢ。大P下ess₁≈0.072 rad（在±2%带内），小P下ess₁≈0.539 rad（带宽的34倍，永不收敛）。这是纯PD的本质缺陷——稳态精度和响应速度不可兼得。

 

控制力矩RMS：反映驱动器持续出力需求。大P下τ峰值高且撞限幅（±80 N·m），但到位快、积分时间短；小P下τ全程不饱和但拖得久。

 

4. OOP代码实现

 

技术栈

 

限定：numpy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方重型库。全部自实现。Coriolis力使用闭式解析公式（无数值差分），保证RK4精度和速度。

 

项目结构树

 

controller_comparison/

├── run_controller_comparison.py # 主入口（完整，含指标计算与绘图调用）

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py # dataclass数据模型（含入参校验）

│ ├── dynamics.py # 2R闭式动力学（M/C/G解析公式）

│ ├── controller.py # PD控制器（含限幅）

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

│ ├── metrics.py # 性能指标计算（含未收敛分支）

│ └── visualize.py # 4张对比图封装

├── tests/

│ ├── __init__.py

│ ├── test_controller_comparison.py # 3类测试：入参/物理自洽/端到端

│ └── test_dynamics_crosscheck.py # 闭式vs差分交叉验证（Christoffel）

├── results/ # 4张对比图输出目录

└── output/ # 性能指标报告txt

 

核心源码

 

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

 

"""

Data models for 2R controller comparison simulation.

All parameters built-in, self-contained.

"""

 

 

from dataclasses import dataclass, field

import numpy as np

 

 

@dataclass

class LinkInertia:

    """Rigid link inertial parameters (planar 2R, vertical plane)"""

    name: str

    mass: float # kg

    length: float # m, distance from previous joint to next joint

    inertia: float # kg·m^2, about COM

    com_offset: float # m, distance from joint to COM

 

    def __post_init__(self):

        if self.mass <= 0:

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

        if self.length <= 0:

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

        if self.inertia <= 0:

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

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

            raise ValueError(

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

                f"got {self.com_offset} (length={self.length})"

            )

 

 

@dataclass

class PDGains:

    """

    PD gains. Only diagonal (decoupled) gains supported.

    Kp, Kd are stored as 1-D arrays; controller reshapes to (2,1).

    """

    Kp: np.ndarray

    Kd: np.ndarray

 

    def __post_init__(self):

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

        if kp.ndim != 1 or kp.shape != (2,):

            raise ValueError(f"Kp must be a 1-D array of length 2, got shape {kp.shape}")

        self.Kp = kp

 

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

        if kd.ndim != 1 or kd.shape != (2,):

            raise ValueError(f"Kd must be a 1-D array of length 2, got shape {kd.shape}")

        self.Kd = kd

 

 

@dataclass

class TorqueLimits:

    """Per-joint torque saturation (N·m)"""

    tau_max: np.ndarray

 

    def __post_init__(self):

        self.tau_max = np.asarray(self.tau_max, dtype=float).flatten()

        if self.tau_max.shape != (2,) or np.any(self.tau_max <= 0):

            raise ValueError("tau_max must be length-2 positive array")

 

 

@dataclass

class SimConfig:

    """Configuration for 2R controller comparison simulation."""

    links: list = field(default_factory=list)

    q_des: np.ndarray = field(default_factory=lambda: np.array([np.pi/4, np.pi/6]))

    gravity: float = 9.81

    dt: float = 0.001

    t_sim: float = 5.0

    ss_window: float = 1.0

    settling_band: float = 0.02 # fraction of step amplitude

 

    def __post_init__(self):

        if not self.links:

            self.links = [

                LinkInertia("L1", 5.8, 0.40, 0.35, 0.20),

                LinkInertia("L2", 4.2, 0.35, 0.18, 0.175),

            ]

        if self.t_sim <= 0:

            raise ValueError("t_sim must be positive")

        if self.ss_window <= 0 or self.ss_window >= self.t_sim:

            raise ValueError("ss_window must be positive and < t_sim")

        self.q_des = np.asarray(self.q_des, dtype=float).flatten()

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

            raise ValueError("q_des must be length-2")

 

"robot/dynamics.py" — 2R刚体动力学（闭式解析，无数值差分）

 

"""

2R planar rigid-body dynamics (vertical plane).

Closed-form M, C, G derived from Lagrangian — no numerical differentiation.

This ensures RK4 accuracy and speed for n=2.

"""

 

 

import numpy as np

import math

from robot.models import SimConfig

 

 

class Dynamics2R:

    def __init__(self, config: SimConfig):

        self.cfg = config

        self.links = config.links

        self.g = config.gravity

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

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

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

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

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

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

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

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

 

    # ------------------------------------------------------------------

    # Mass matrix M(q) — symmetric positive-definite by construction

    # Derived: M11 = I1 + I2 + m1*r1^2 + m2*(l1^2 + r2^2 + 2*l1*r2*cos(q2))

    # M12 = M21 = I2 + m2*(r2^2 + l1*r2*cos(q2))

    # M22 = I2 + m2*r2^2

    # ------------------------------------------------------------------

    def mass_matrix(self, q):

        q2 = q[1]

        c2 = math.cos(q2)

 

        M11 = (self.I1 + self.I2

               + self.m1 * self.r1**2

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

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

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

 

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

                        [M12, M22]], dtype=float)

 

    # ------------------------------------------------------------------

    # Coriolis and centrifugal vector C(q, qd)

    # Christoffel: only non-zero term is C2 = -h * qd1^2 - h * qd1*qd2

    # where h = m2*l1*r2*sin(q2)

    # C1 = h * qd2 * (2*qd1 + qd2)

    # ------------------------------------------------------------------

    def coriolis_vector(self, q, qd):

        q2 = q[1]

        s2 = math.sin(q2)

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

 

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

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

 

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

 

    # ------------------------------------------------------------------

    # Gravity vector G(q)

    # G1 = m1*g*r1*cos(q1) + m2*g*(l1*cos(q1) + r2*cos(q1+q2))

    # G2 = m2*g*r2*cos(q1+q2)

    # ------------------------------------------------------------------

    def gravity_vector(self, q):

        q1 = q[0]

        q2 = q[1]

        c1 = math.cos(q1)

        c12 = math.cos(q1 + q2)

 

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

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

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

 

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

 

"robot/controller.py" — PD控制器

 

"""

PD controller with per-joint torque saturation.

NOTE: Saturation is applied at the control output. In RK4 intermediate

stages (k2/k3/k4) the clipped torque is used; this causes derivative

discontinuity. For course-level simulation the effect is bounded, but

production code should use event-detection or saturated-trajectory

planning. This limitation is declared for teaching transparency.

"""

 

 

import numpy as np

from robot.models import PDGains, TorqueLimits

 

 

class PDController:

    def __init__(self, gains: PDGains, torque_limits: TorqueLimits):

        self.Kp = gains.Kp # stored as 1-D (2,)

        self.Kd = gains.Kd # stored as 1-D (2,)

        self.tau_max = torque_limits.tau_max.reshape(2, 1)

 

    def compute_torque(self, q, q_dot, q_des):

        """

        q, q_dot, q_des: can be (2,) or (2,1). Returns (2,1) clipped torque.

        """

        q = np.asarray(q, dtype=float).reshape(2, 1)

        q_dot = np.asarray(q_dot, dtype=float).reshape(2, 1)

        q_des = np.asarray(q_des, dtype=float).reshape(2, 1)

 

        tau_raw = self.Kp.reshape(2, 1) * (q_des - q) \

                  - self.Kd.reshape(2, 1) * q_dot

        tau_clipped = np.clip(tau_raw, -self.tau_max, self.tau_max)

        return tau_clipped

 

"robot/simulator.py" — 闭环仿真器

 

"""

2R PD simulation: RK4 integration.

"""

 

 

import numpy as np

from robot.dynamics import Dynamics2R

from robot.controller import PDController

from robot.models import SimConfig

 

 

class Simulator:

    def __init__(self, dynamics: Dynamics2R,

                 controller: PDController,

                 config: SimConfig):

        self.dyn = dynamics

        self.ctrl = controller

        self.cfg = config

        self.dt = config.dt

        self.n_steps = int(config.t_sim / config.dt)

 

    def derivatives(self, x):

        q = x[0:2]

        qd = x[2:4]

 

        tau_ctrl = self.ctrl.compute_torque(q, qd, self.cfg.q_des)

 

        M = self.dyn.mass_matrix(x[0:2])

        C = self.dyn.coriolis_vector(x[0:2], x[2:4])

        G = self.dyn.gravity_vector(x[0:2])

 

        qdd = np.linalg.solve(M, tau_ctrl - C - G)

        return np.concatenate([qd.flatten(), qdd.flatten()])

 

    def run(self, q_start=None, q_dot_start=None):

        if q_start is None:

            q_start = np.zeros(2)

        if q_dot_start is None:

            q_dot_start = np.zeros(2)

 

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

        x_hist = np.zeros((self.n_steps + 1, 4))

        x_hist[0] = x

 

        dt = self.dt

        for i in range(self.n_steps):

            xi = x_hist[i]

            k1 = self.derivatives(xi)

            k2 = self.derivatives(xi + 0.5 * dt * k1)

            k3 = self.derivatives(xi + 0.5 * dt * k2)

            k4 = self.derivatives(xi + dt * k3)

            x_hist[i+1] = xi + (dt / 6.0) * (k1 + 2*k2 + 2*k3 + k4)

 

        t = np.arange(self.n_steps + 1) * dt

        return t, x_hist

 

"robot/metrics.py" — 性能指标计算（含未收敛分支）

 

"""

Performance metrics for step response.

Includes handling of non-convergent cases (steady-state error exceeds band).

Step amplitude = |q_des - q_initial|, here q_initial = [0, 0].

"""

 

 

import numpy as np

 

 

def compute_metrics(t, x_hist, q_des, ss_window, settling_band):

    """

    Returns dict: keys 0, 1 → each joint's metrics.

    settling_time: np.inf if error never enters the band.

    overshoot_pct: 0.0 if monotonic approach (no crossing).

    ss_error: mean absolute error in last ss_window seconds.

    converged: bool, whether error enters band.

    """

    dt = t[1] - t[0]

    n_window = int(ss_window / dt)

    if n_window > len(t):

        n_window = len(t)

 

    step_amp = np.abs(q_des) # q0 = [0, 0]

    band = settling_band * step_amp

 

    metrics = {}

    for j in range(2):

        qj = x_hist[:, j]

        err = q_des[j] - qj

 

        # ---- Overshoot: first crossing of q_des, then positive deviation ----

        crossed_idx = len(t)

        for i in range(len(t)):

            if qj[i] >= q_des[j]:

                crossed_idx = i

                break

 

        if crossed_idx < len(t):

            post = qj[crossed_idx:] - q_des[j]

            pos = post[post > 1e-12]

            max_ov = float(np.max(pos)) if len(pos) > 0 else 0.0

        else:

            max_ov = 0.0

 

        overshoot_pct = max_ov / step_amp[j] * 100.0

 

        # ---- Settling time: first time error enters band and stays ----

        settling_time = np.inf

        for i in range(len(t)):

            if abs(err[i]) <= band[j]:

                if np.all(abs(err[i:]) <= band[j]):

                    settling_time = t[i]

                    break

 

        # ---- Steady-state error (last window) ----

        ss_err = float(np.mean(np.abs(err[-n_window:])))

 

        # ---- Convergence flag ----

        converged = settling_time < np.inf

 

        metrics[j] = {

            'overshoot_pct': overshoot_pct,

            'settling_time': settling_time,

            'ss_error': ss_err,

            'converged': converged,

            'band_halfwidth': band[j],

        }

 

    return metrics

 

"robot/visualize.py" — 可视化（完整版）

 

"""

Visualization for 2R controller comparison: 4 charts.

1. Joint 1 angle tracking (big P vs small P overlay)

2. Joint 2 angle tracking (big P vs small P overlay)

3. Position error comparison (big P vs small P, both joints, 2 subplots)

4. Control torque comparison (big P vs small P, both joints, 2 subplots)

All figures saved to results/ with snake_case names.

"""

 

 

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.results_dir = results_dir

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_joint1_tracking(self, t, x_bigp, x_smallp, q_des):

        """Chart 1: Joint 1 angle tracking overlay."""

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

        ax.plot(t, x_bigp[:, 0], color='red', linewidth=1.2,

                label='Big P')

        ax.plot(t, x_smallp[:, 0], color='steelblue', linewidth=1.2,

                linestyle='--', label='Small P')

        ax.axhline(q_des[0], color='black', linestyle=':',

                   linewidth=1.2, label=f'q1_des={q_des[0]:.3f}')

        ax.set_xlabel('Time (s)')

        ax.set_ylabel('Angle (rad)')

        ax.set_title('Joint 1 Angle Tracking: Big P vs Small P')

        ax.legend(fontsize=9)

        ax.grid(True, alpha=0.3)

 

        path = os.path.join(self.results_dir,

                           "01_joint1_angle_tracking.png")

        plt.tight_layout()

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_joint2_tracking(self, t, x_bigp, x_smallp, q_des):

        """Chart 2: Joint 2 angle tracking overlay."""

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

        ax.plot(t, x_bigp[:, 1], color='red', linewidth=1.2,

                label='Big P')

        ax.plot(t, x_smallp[:, 1], color='steelblue', linewidth=1.2,

                linestyle='--', label='Small P')

        ax.axhline(q_des[1], color='black', linestyle=':',

                   linewidth=1.2, label=f'q2_des={q_des[1]:.3f}')

        ax.set_xlabel('Time (s)')

        ax.set_ylabel('Angle (rad)')

        ax.set_title('Joint 2 Angle Tracking: Big P vs Small P')

        ax.legend(fontsize=9)

        ax.grid(True, alpha=0.3)

 

        path = os.path.join(self.results_dir,

                           "02_joint2_angle_tracking.png")

        plt.tight_layout()

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_position_error(self, t, x_bigp, x_smallp, q_des):

        """Chart 3: Position error for both joints, big P vs small P."""

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

 

        # Joint 1 error

        err1_big = q_des[0] - x_bigp[:, 0]

        err1_small = q_des[0] - x_smallp[:, 0]

        axes[0].plot(t, err1_big, color='red', linewidth=1.0,

                     label='Big P')

        axes[0].plot(t, err1_small, color='steelblue', linewidth=1.0,

                     linestyle='--', label='Small P')

        axes[0].axhline(0.0, color='black', linewidth=0.8, alpha=0.5)

        axes[0].set_ylabel('J1 Error (rad)')

        axes[0].set_title('Joint 1 Position Error Comparison')

        axes[0].legend(fontsize=8)

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

 

        # Joint 2 error

        err2_big = q_des[1] - x_bigp[:, 1]

        err2_small = q_des[1] - x_smallp[:, 1]

        axes[1].plot(t, err2_big, color='red', linewidth=1.0,

                     label='Big P')

        axes[1].plot(t, err2_small, color='steelblue', linewidth=1.0,

                     linestyle='--', label='Small P')

        axes[1].axhline(0.0, color='black', linewidth=0.8, alpha=0.5)

        axes[1].set_xlabel('Time (s)')

        axes[1].set_ylabel('J2 Error (rad)')

        axes[1].set_title('Joint 2 Position Error Comparison')

        axes[1].legend(fontsize=8)

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

 

        plt.tight_layout()

        path = os.path.join(self.results_dir,

                           "03_position_error_comparison.png")

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_control_torque(self, t, tau_bigp, tau_smallp):

        """Chart 4: Control torque comparison (saturation lines without labels)."""

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

 

        # Joint 1 torque

        axes[0].plot(t, tau_bigp[:, 0], color='red', linewidth=1.0,

                     label='Big P')

        axes[0].plot(t, tau_smallp[:, 0], color='steelblue',

                     linewidth=1.0, linestyle='--', label='Small P')

        # Saturation lines without labels to avoid legend bloat

        axes[0].axhline(80, color='red', linestyle=':', alpha=0.5)

        axes[0].axhline(-80, color='red', linestyle=':', alpha=0.5)

        axes[0].set_ylabel('tau1 (N·m)')

        axes[0].set_title('Joint 1 Control Torque: Big P vs Small P')

        axes[0].legend(fontsize=8)

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

 

        # Joint 2 torque

        axes[1].plot(t, tau_bigp[:, 1], color='red', linewidth=1.0,

                     label='Big P')

        axes[1].plot(t, tau_smallp[:, 1], color='steelblue',

                     linewidth=1.0, linestyle='--', label='Small P')

        axes[1].axhline(6

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