《Python机器人基础原理》第一百三篇：装配工位——PD控制器抵抗外部冲击的离线仿真

 

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

 

1. 实际工业应用场景

 

周三上午十点，精密减速器装配工位。机器人正把一颗重2.5 kg的行星架缓缓压入壳体，压装力设定120 N。突然，传送带上一只脱落的定位销滚到机器人基座旁，撞上了连杆2的末端——"砰"的一声闷响，末端瞬间偏了十几毫米，力传感器飙到380 N。急停触发，产线停了四分钟。

 

上位机工程师小赵调出力矩曲线回放："PD控制器的Kp=120 N·m/rad、Kd=15 N·m·s/rad，冲击后恢复到目标位用了多久、超调多大，离线仿真能先跑一遍吗？对了，关节1和2的额定扭矩分别是45 N·m和28 N·m，仿真里力矩超了的话得标出来。"

 

老郑把安全帽往后推了推："先跑纯PD反馈，不加重力前馈——看看稳态偏差有多大，这正好能说明为什么要加前馈或者积分项。连杆参数：连杆1长0.35 m、质量4.2 kg、惯量0.18 kg·m²，连杆2长0.28 m、质量2.8 kg、惯量0.08 kg·m²。关节角约定：从水平轴逆时针起量，所以重力项是cos(q)。扰动力矩用0.15秒的脉冲近似碰撞冲击，持续推挤是简化，真实碰撞更接近速度跳变——这个局限仿真报告里得注明。"

 

构型约定：本篇采用2-DOF平面机械臂（2R），连杆角从水平轴起量（重力项含cos）。连杆1 a₁=0.35 m、m₁=4.2 kg、I₁=0.18 kg·m²，连杆2 a₂=0.28 m、m₂=2.8 kg、I₂=0.08 kg·m²。目标位姿q_des=[π/4, π/6] rad。PD控制器（纯反馈，无重力前馈）：Kp=diag(120, 80) N·m/rad，Kd=diag(15, 10) N·m·s/rad。力矩限幅±45 N·m（关节1）、±28 N·m（关节2）。外部扰动力矩τ_dist=diag(8, 5) N·m，作用时间t=1.5~1.65 s（持续0.15 s，简化为持续推挤型脉冲）。仿真时长3.0 s，RK4积分步长0.001 s。技术栈限定 numpy + matplotlib。

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

 

2. 痛点分析

 

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

第5章 机器人关节驱动与控制 → 5.3 PD控制 装配工位受外部冲击（零件碰撞），PD控制器需快速恢复目标位姿

第4章 机器人动力学建模 → 拉格朗日方程 双连杆刚体动力学为PD控制提供精确的惯性/科氏/重力项

第9章 仿真实验 → 离线控制器调参 不同Kp/Kd组合的恢复时间/超调量离线对比，避免上机试错风险

 

本篇代码解决 L2 离线仿真校核层。输入：连杆质量/长度/惯量、PD增益矩阵、力矩限幅、扰动力矩幅值/作用窗口、仿真时长/步长。处理：①构建2R刚体动力学（含重力、科氏力）；②PD控制律计算关节力矩（带限幅）；③RK4数值积分；④在指定时刻注入脉冲扰动力矩；⑤记录关节角/角速度/控制力矩/恢复指标/饱和时段。输出：4张分析图表 + 仿真报告。

 

不覆盖：柔性关节、摩擦模型、自适应控制、力/位混合控制、实时系统。

 

3. 核心逻辑讲解

 

物理直觉

 

你用手端着一杯水，有人从侧面推了一下杯子——你的手臂会晃，但肌肉马上发力把杯子拉回原位。PD控制器干的就是这个活：P（比例）像肌肉的"位置觉"，偏差越大发力越大，负责把关节拽回目标位；D（微分）像肌肉的"阻尼觉"，转得快就反向拽，负责吃掉振荡。但水有惯性、手臂有关节摩擦——这就是动力学。冲击来的时候，P和D配合得好，杯子晃两下就稳住了；配合不好，要么晃个不停，要么反应迟钝水洒出来。

 

本篇用的是纯反馈PD，没有重力前馈。这意味着即使稳态时，关节也要靠P项持续输出去"扛住"重力——所以稳态误差不会是零，而是 e_ss = G(q_des)/Kp。这个偏差量级恰好能让学生直观理解为什么工业控制器要加重力补偿或积分项。

 

业务逻辑 → 代码映射

 

输入：连杆参数(m,a,I)，PD增益(Kp,Kd)，力矩限幅(±τ_max)，目标位q_des，扰动(τ_dist, t_start, t_end)

  ↓

【步骤1：动力学模型】

  M(q)·q'' + C(q,q')·q' + G(q) = τ_ctrl + τ_dist(t)

  ↓

【步骤2：PD控制律 + 限幅】

  τ_raw = Kp·(q_des - q) - Kd·q'

  τ_ctrl = clip(τ_raw, -τ_max, +τ_max)

  ↓

【步骤3：RK4积分】

  状态向量 x = [q1, q2, q1', q2']ᵀ

  x' = f(x,t) = [x[2:4], M⁻¹·(τ_ctrl + τ_dist(t) - C·x[2:4] - G)]

  每步调用4次f，更新x

  ↓

【步骤4：扰动注入 + 饱和记录】

  if t_start ≤ t ≤ t_end: τ_dist 非零，否则为零

  记录每步是否饱和

  ↓

【步骤5：恢复指标 + 绘图】

 

关键公式文字翻译

 

PD控制力矩：

"τ = Kp·(q_des - q) - Kd·q_dot"。Kp乘以位置误差（拉回力度），Kd乘以角速度（阻尼力度）。减号因为阻尼要对抗运动方向。注意：本篇无重力前馈，稳态时Kp·e_ss = G(q_des)，故e_ss ≈ G/Kp ≈ 0.15 rad量级，不会是零。

 

动力学方程：

"M·q_ddot + C·q_dot + G = τ"。M是质量矩阵（惯性），C是科氏/离心力项，G是重力项。本篇关节角从水平轴起量，重力势能与cos(q)相关，故G含cos。

 

力矩限幅：

"τ_clipped = clip(τ_raw, -τ_max, +τ_max)"。模拟驱动器饱和。初始时刻q_des - q_init ≈ [0.785, 0.524] rad，τ₁_raw ≈ 94 N·m，远超额定45 N·m，必然饱和——这正是为什么上机后恢复比仿真慢（真实驱动器饱和时力矩不再增长）。

 

RK4积分：每步用4个斜率k1~k4加权平均来更新状态，精度O(h⁴)，比欧拉法稳定得多。步长0.001 s保证3秒仿真不发散。

 

4. OOP代码实现

 

技术栈

 

限定：numpy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方重型库。全部自实现。

 

项目结构树

 

pd_disturbance_sim/

├── run_pd_sim.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py

│ ├── dynamics.py

│ ├── controller.py

│ ├── simulator.py

│ └── visualize.py

├── tests/

│ ├── __init__.py

│ └── test_pd_sim.py

├── results/

└── output/

 

核心源码

 

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

 

"""

Data models: link inertial parameters, PD gains, disturbance spec

All parameters built-in, self-contained

"""

 

 

import math

from dataclasses import dataclass, field

import numpy as np

 

 

@dataclass

class LinkInertia:

    """Rigid link inertial parameters (planar 2R)"""

    name: str

    mass: float # kg

    length: float # m

    inertia: float # kg·m², about center of mass

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

        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 "

                             f"in (0, length]")

 

 

@dataclass

class PDGains:

    """

    PD controller gains.

    Accepts 1D array/list of length 2 (diagonal) or 2x2 array.

    """

    Kp: np.ndarray # 2x2 or [kp1, kp2]

    Kd: np.ndarray # 2x2 or [kd1, kd2]

 

    def __post_init__(self):

        # Normalize Kp: if 1D length-2, make diagonal 2x2

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

        if kp.shape[0] == 2:

            self.Kp = np.diag(kp)

        elif kp.shape[0] == 4:

            self.Kp = kp.reshape(2, 2)

        else:

            raise ValueError("Kp must be length-2 or 2x2")

 

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

        if kd.shape[0] == 2:

            self.Kd = np.diag(kd)

        elif kd.shape[0] == 4:

            self.Kd = kd.reshape(2, 2)

        else:

            raise ValueError("Kd must be length-2 or 2x2")

 

        if np.any(self.Kp < 0):

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

        if np.any(self.Kd < 0):

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

 

 

@dataclass

class TorqueLimits:

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

    tau_max: np.ndarray

 

    def __post_init__(self):

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

        if np.any(self.tau_max <= 0):

            raise ValueError("tau_max must be positive")

 

 

@dataclass

class DisturbanceSpec:

    """External disturbance torque pulse"""

    torque: np.ndarray # N·m, 2x1

    t_start: float # s

    t_end: float # s

 

    def __post_init__(self):

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

        if self.t_end <= self.t_start:

            raise ValueError("t_end must be > t_start")

 

    def is_active(self, t):

        return self.t_start <= t <= self.t_end

 

 

@dataclass

class SimConfig:

    """Simulation configuration"""

    links: list = field(default_factory=list)

    q_des: np.ndarray = field(default_factory=lambda: np.array(

        [math.pi/4, math.pi/6]))

    gravity: float = 9.81

    dt: float = 0.001

    t_end: float = 3.0

    q_init: np.ndarray = field(default_factory=lambda: np.array(

        [0.0, 0.0]))

    q_dot_init: np.ndarray = field(default_factory=lambda: np.array(

        [0.0, 0.0]))

    torque_limits: TorqueLimits = field(default_factory=lambda:

        TorqueLimits(np.array([45.0, 28.0])))

 

    def __post_init__(self):

        if not self.links:

            self.links = [

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

                LinkInertia("L2", 2.8, 0.28, 0.08, 0.140),

            ]

 

"robot/dynamics.py" — 2R刚体动力学

 

"""

2R planar rigid-body dynamics: M(q), C(q,qd), G(q)

Derived from Lagrangian formulation.

Joint angles measured from horizontal axis (counterclockwise positive).

Thus gravity potential ~ cos(q), gravity torque ~ cos(q).

"""

 

 

import numpy as np

import math

from robot.models import SimConfig

 

 

class Dynamics2R:

    def __init__(self, config: SimConfig):

        self.cfg = config

        self.l1 = config.links[0]

        self.l2 = config.links[1]

        self.g = config.gravity

 

    def mass_matrix(self, q):

        """

        Symmetric inertia matrix M(q) 2x2.

        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

        """

        l1, l2 = self.l1.length, self.l2.length

        r1, r2 = self.l1.com_offset, self.l2.com_offset

        m1, m2 = self.l1.mass, self.l2.mass

        I1, I2 = self.l1.inertia, self.l2.inertia

        c2 = math.cos(q[1])

 

        M11 = (I1 + I2 + m1*r1**2

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

        M12 = I2 + m2*(r2**2 + l1*r2*c2)

        M22 = I2 + m2*r2**2

 

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

 

    def coriolis_vector(self, q, qd):

        """

        Coriolis/centrifugal vector C_eff(q,qd) 2x1.

        C_eff_1 = -m2*l1*r2*sin(q2)*(2*qd1*qd2 + qd2^2)

        C_eff_2 = m2*l1*r2*sin(q2)*qd1^2

        """

        l1 = self.l1.length

        r2 = self.l2.com_offset

        m2 = self.l2.mass

        s2 = math.sin(q[1])

 

        h = m2 * l1 * r2 * s2

        qd1, qd2 = qd[0], qd[1]

 

        C_eff_1 = -h * (2*qd1*qd2 + qd2**2)

        C_eff_2 = h * (qd1**2)

 

        return np.array([[C_eff_1], [C_eff_2]], dtype=float)

 

    def gravity_vector(self, q):

        """

        Gravity vector G(q) 2x1.

        Angles measured from horizontal axis, so

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

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

        """

        l1 = self.l1.length

        r1 = self.l1.com_offset

        r2 = self.l2.com_offset

        m1, m2 = self.l1.mass, self.l2.mass

        g = self.g

 

        c1 = math.cos(q[0])

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

 

        G1 = ((m1*r1 + m2*l1)*g*c1

              + m2*r2*g*c12)

        G2 = m2*r2*g*c12

 

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

 

"robot/controller.py" — PD控制器（带限幅）

 

"""

PD controller for 2R planar arm with torque saturation.

tau = clip(Kp*(q_des - q) - Kd*q_dot, -tau_max, +tau_max)

"""

 

 

import numpy as np

from robot.models import PDGains, TorqueLimits

 

 

class PDController:

    def __init__(self, gains: PDGains, q_des: np.ndarray,

                 torque_limits: TorqueLimits):

        self.Kp = gains.Kp

        self.Kd = gains.Kd

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

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

 

    def compute_torque(self, q, q_dot):

        """

        PD control law with saturation.

        Returns (tau_clipped 2x1, tau_raw 2x1, saturated bool 2x1).

        """

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

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

 

        tau_raw = self.Kp @ (self.q_des - q) - self.Kd @ q_dot

 

        # Clip to torque limits

        tau_clipped = np.clip(tau_raw,

                               -self.tau_max,

                               self.tau_max)

 

        saturated = np.abs(tau_clipped) >= self.tau_max * 0.999

 

        return tau_clipped, tau_raw, saturated.flatten()

 

"robot/simulator.py" — RK4仿真器

 

"""

RK4 ODE integrator for 2R PD control with disturbance injection

Records saturation flags at each step.

"""

 

 

import numpy as np

from robot.dynamics import Dynamics2R

from robot.controller import PDController

from robot.models import DisturbanceSpec, SimConfig

 

 

class Simulator:

    def __init__(self, dynamics: Dynamics2R,

                 controller: PDController,

                 disturbance: DisturbanceSpec,

                 config: SimConfig):

        self.dyn = dynamics

        self.ctrl = controller

        self.dist = disturbance

        self.dt = config.dt

        self.t_end = config.t_end

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

 

    def derivatives(self, x, t):

        """

        x = [q1, q2, qd1, qd2]

        Returns dx/dt

        """

        q = x[0:2].reshape(2, 1)

        qd = x[2:4].reshape(2, 1)

 

        # PD torque (with saturation)

        tau_ctrl, _, _ = self.ctrl.compute_torque(q, qd)

 

        # Disturbance

        if self.dist.is_active(t):

            tau_total = tau_ctrl + self.dist.torque

        else:

            tau_total = tau_ctrl

 

        # Dynamics: qdd = M^-1(tau - C*qd - G)

        M = self.dyn.mass_matrix(q.flatten())

        C_vec = self.dyn.coriolis_vector(q.flatten(), qd.flatten())

        G_vec = self.dyn.gravity_vector(q.flatten())

 

        qdd = np.linalg.solve(M, tau_total - C_vec - G_vec)

 

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

 

    def run(self, q_init, q_dot_init):

        """

        RK4 integration. Returns time array, state history,

        and saturation history.

        """

        n = self.n_steps

        dt = self.dt

        x = np.zeros((n + 1, 4))

        x[0, 0:2] = q_init

        x[0, 2:4] = q_dot_init

 

        t_arr = np.linspace(0, self.t_end, n + 1)

        sat_hist = np.zeros((n + 1, 2), dtype=bool)

 

        for i in range(n):

            xi = x[i]

            t = t_arr[i]

 

            k1 = self.derivatives(xi, t)

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

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

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

 

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

 

            # Record saturation at end of step

            q_new = x[i+1, 0:2].reshape(2, 1)

            qd_new = x[i+1, 2:4].reshape(2, 1)

            _, _, sat = self.ctrl.compute_torque(q_new, qd_new)

            sat_hist[i+1] = sat

 

        return t_arr, x, sat_hist

 

"robot/visualize.py" — 可视化

 

"""

PD disturbance simulation visualization: 4 charts

1. Joint angle tracking with disturbance marker + saturation shading

2. Joint velocity response

3. Control torque over time (clipped)

4. Phase portrait (q1 error vs q1 velocity)

"""

 

 

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

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

        self.results_dir = results_dir

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_joint_tracking(self, t, x, q_des, t_dist_start,

                            t_dist_end, sat_hist):

        """

        1. Joint angle vs time, with disturbance window and saturation

        """

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

 

        labels = ['Joint 1', 'Joint 2']

        colors = ['steelblue', 'coral']

        q_des_arr = np.array(q_des).flatten()

 

        for i in range(2):

            axes[i].plot(t, x[:, i], color=colors[i],

                         linewidth=1.2, label=f'Actual {labels[i]}')

            axes[i].axhline(q_des_arr[i], color='red', linestyle='--',

                           linewidth=1.5, label='Desired')

            axes[i].axvspan(t_dist_start, t_dist_end,

                           alpha=0.15, color='red',

                           label='Disturbance')

            # Shade saturation periods

            sat_mask = sat_hist[:, i]

            if np.any(sat_mask):

                sat_t = t[sat_mask]

                for st in sat_t[::50]: # sparse markers

                    axes[i].axvline(st, color='orange',

                                   alpha=0.3, linewidth=0.5)

            axes[i].set_ylabel(f'q{i+1} (rad)')

            axes[i].legend(fontsize=7)

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

 

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

        fig.suptitle('Joint Angle Tracking under External Disturbance\n'

                     '(Orange vertical lines = torque saturation)')

        plt.tight_layout()

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

                            "01_joint_tracking.png")

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_joint_velocity(self, t, x, t_dist_start, t_dist_end):

        """

        2. Joint velocity response

        """

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

 

        for i in range(2):

            axes[i].plot(t, x[:, i+2], color='steelblue',

                         linewidth=1.2, label=f'qd{i+1}')

            axes[i].axvspan(t_dist_start, t_dist_end,

                           alpha=0.15, color='red',

                           label='Disturbance')

            axes[i].axhline(0, color='red', linestyle='--',

                           linewidth=1, label='Zero')

            axes[i].set_ylabel(f'qd{i+1} (rad/s)')

            axes[i].legend(fontsize=7)

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

 

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

        fig.suptitle('Joint Velocity Response')

        plt.tight_layout()

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

                            "02_joint_velocity.png")

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_control_torque(self, t, tau_hist, t_dist_start,

                            t_dist_end, tau_max):

        """

        3. PD control torque over time (clipped)

        """

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

 

        for i in range(2):

            axes[i].plot(t, tau_hist[:, i], color='steelblue',

                         linewidth=1.2, label='Torque (clipped)')

            axes[i].axhline(tau_max[i], color='red', linestyle='--',

                           linewidth=1, label=f'+Limit {tau_max[i]} N·m')

            axes[i].axhline(-tau_max[i], color='red', linestyle='--',

                           linewidth=1, label=f'-Limit')

            axes[i].axvspan(t_dist_start, t_dist_end,

                           alpha=0.15, color='red',

                           label='Disturbance')

            axes[i].set_ylabel(f'tau{i+1} (N·m)')

            axes[i].legend(fontsize=7)

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

 

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

        fig.suptitle('Control Torque (with Saturation Limits)')

        plt.tight_layout()

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

                            "03_control_torque.png")

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_phase_portrait(self, x, q_des, dt):

        """

        4. Phase portrait: position error vs velocity for joint 1

        """

        q_des = np.array(q_des).flatten()

        err = x[:, 0] - q_des[0]

        vel = x[:, 2]

 

        # Disturbance onset index

        dist_idx = int(1.5 / dt)

 

        fig, ax = plt.subplots(figsize=(7, 7))

        ax.plot(err, vel, color='steelblue', linewidth=0.8,

                alpha=0.7)

        ax.axhline(0, color='red', linestyle='--', linewidth=1)

        ax.axvline(0, color='red', linestyle='--', linewidth=1)

 

        ax.plot(err[dist_idx], vel[dist_idx], 'ro', markersize=6,

                label=f'Disturbance onset (t=1.5s, idx={dist_idx})')

 

        ax.set_xlabel('Position Error q1_err (rad)')

        ax.set_ylabel('Velocity qd1 (rad/s)')

        ax.set_title('Phase Portrait: Joint 1\n'

                     '(Position Error vs Velocity)')

        ax.legend(fontsize=8)

        ax.grid(True, alpha=0.3)

 

        plt.tight_layout()

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

                            "04_phase_portrait.png")

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

5. 可视化要求

 

序号 图表文件 服务结论

① 

"01_joint_tracking.png" 关节角跟踪曲线，红色虚线为目标位，红色阴影区为扰动注入窗口，橙色竖线为力矩饱和时段，读出冲击后恢复时间和稳态偏差

② 

"02_joint_velocity.png" 关节角速度响应，读出冲击瞬间的速度峰值及阻尼衰减过程

③ 

"03_control_torque.png" 控制力矩（带限幅），红色虚线为额定扭矩限制，验证初始阶段饱和

④ 

"04_phase_portrait.png" 关节1相图（位置误差 vs 速度），螺旋收敛体现阻尼特性，原点附近残留偏差体现稳态误差

 

6. README使用说明 + 核心知识点卡片

 

快速开始

 

pip install -r requirements.txt

python -m pytest tests/test_pd_sim.py -v

python run_pd_sim.py

 

数据格式说明

 

# 修改连杆参数：编辑 SimConfig.links 中 LinkInertia 的构造参数

# 修改PD增益：PDGains(Kp=[120, 80], Kd=[15, 10]) # 支持1D列表

# 修改目标位：编辑 SimConfig.q_des

# 修改力矩限幅：编辑 SimConfig.torque_limits

# 修改扰动：编辑 DisturbanceSpec(torque=..., t_start=..., t_end=...)

 

参数 修改位置 说明

连杆1质量 

"LinkInertia("L1", 4.2, ...)" kg

连杆1长度 

"LinkInertia("L1", ..., 0.35, ...)" m

Kp 

"PDGains(Kp=[120, 80], ...)" N·m/rad

Kd 

"PDGains(..., Kd=[15, 10])" N·m·s/rad

力矩限幅 

"TorqueLimits(np.array([45.0, 28.0]))" N·m

扰动力矩 

"DisturbanceSpec(torque=np.array([[8],[5]]), ...)" N·m

 

📌 知识点卡片

 

PD控制与抗扰分析（对应课本第5章）

 

- 纯反馈PD：τ = Kp·(q_des - q) - Kd·q_dot。无重力前馈时稳态误差 e_ss = G(q_des)/Kp ≈ 0.15 rad，非零

- 力矩饱和：驱动器有物理限幅，仿真中须clip，否则初始大偏差会导致不真实的无限大输出

- 动力学方程：M·q'' + C·q' + G = τ。PD输出作为τ的一部分，与扰动叠加后驱动系统

- RK4积分：四阶龙格-库塔，精度O(h⁴)，步长0.001 s保证3秒仿真数值稳定

- 冲击建模局限：本篇用0.15 s恒值力矩脉冲近似碰撞，真实碰撞更接近速度跳变（冲量型），仿真结论仅作定性参考

 

7. 总结与工程师思考

 

工业落地难处

 

一、无重力补偿的PD稳态偏差大。 本篇纯反馈PD在目标位(45°,30°)的稳态偏差约0.14 rad（关节1），远超精密装配要求的0.01 rad。工业控制器必须加重力前馈（τ += G(q)）或积分项（PID），否则末端永远"挂"在目标下方。

 

二、力矩饱和导致恢复变慢。 初始时刻P项输出94 N·m远超关节1额定45 N·m，仿真中clip后实际只有45 N·m在推——恢复力被砍了一半。真实产线上机后恢复比仿真慢，往往就是这个原因。调参时Kp不能盲目大，得配合驱动器额定扭矩。

 

三、冲击建模简化与真实碰撞有差距。 本篇用0.15 s恒值力矩脉冲模拟"定位销撞击"，实际碰撞是毫秒级的冲量，表现为q_dot的瞬时跳变而非持续力矩。定性趋势（PD抵抗冲击恢复）是对的，但定量指标（恢复时间、超调）不能直接搬到真实碰撞场

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