《Python机器人基础原理》第一百三十七篇：机械臂仿真——记录PD控制下期望角度、实际角度与输出误差时程曲线

 

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

 

1. 实际工业应用场景

 

周一早班，3C电子装配线。一台2R竖直平面机械臂要把末端吸嘴从待料位移动到PCB焊盘上方，完成元件贴装。任务要求末端沿平滑轨迹运动，到位后姿态稳定，位置误差不超过±0.5 mm。

 

"第12个循环开始抖动。"上位机弹出关节1的实际角度曲线，小赵盯着屏幕："期望角度是一条平滑的三次多项式，起步段速度从零开始、到位时速度也回到零，但实际控制力矩在起步段末端有明显跳变，角度误差曲线在0.5 s处出现一个尖峰。"

 

他调出PD参数：Kp=[120, 80] N·m/rad，Kd=[12, 8] N·m·s/rad。连杆1质量3.0 kg、长度0.40 m、转动惯量0.16 kg·m²、质心0.20 m；连杆2质量1.5 kg、长度0.35 m、转动惯量0.06 kg·m²、质心0.18 m。重力9.81 m/s²。轨迹时长2.0 s，起步段0.5 s，保持段1.5 s。

 

"如果控制器里没有重力前馈，到位后比例项要一直扛着自重，稳态误差会很大。现在加上前馈，看看期望、实际、误差三组曲线到底长什么样。"小赵打开仿真脚本，"先跑一个带重力前馈的，再跑一个不带前馈的，对比稳态误差。"

 

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

 

2. 痛点分析

 

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

第4章 机器人动力学建模 → 刚体动力学 连杆质量、惯量、重力耦合形成非线性扰动，期望轨迹与实际运动之间存在动态偏差

第5章 机器人关节驱动与控制 → PD伺服 比例增益决定响应速度，微分增益抑制超调，重力前馈决定稳态精度

第6章 轨迹规划与控制 → 轨迹平滑性 起步段和保持段衔接处速度不连续会产生力矩阶跃，必须用高阶多项式保证速度连续

 

本篇代码解决 L2 离线仿真校核层。输入：2R连杆参数、PD增益、三次多项式轨迹。处理：RK4积分真实动力学，对比有/无重力前馈两种工况。输出：4张分析图表 + 单元测试。

 

不覆盖：摩擦力矩、关节柔性、编码器量化噪声、通信延迟。

 

3. 核心逻辑讲解

 

物理直觉

 

PD控制就像在两个关节上各装了一根"虚拟弹簧+阻尼器"。期望角度是当前位置，实际角度是弹簧另一端，偏差越大，弹簧拉力（比例项）越强；实际角度运动越快，阻尼力（微分项）越大。控制器每一时刻计算"弹簧+阻尼"的总拉力，再加上可选的重力前馈抵消连杆自重，这个合力矩作用到真实动力学上，推动连杆运动。

 

起步瞬间，期望角度开始平滑变化，偏差逐渐增大，比例项输出一个逐渐增强的力矩。接近目标时，偏差变小，微分项开始"刹车"。到位后，如果开了重力前馈，前馈力矩刚好平衡自重，PD只需补偿微小残余，稳态误差趋近于零；如果没开前馈，自重完全靠比例项扛，稳态误差等于自重除以Kp，是一个固定的常值。

 

轨迹平滑性很关键：如果起步段末端速度不为零，进入保持段时速度突然归零，期望速度跳变会让微分项瞬间输出一个巨大的"刹车"力矩，角度误差曲线上会出现一个尖峰。三次多项式同时约束起点和终点的速度都为零，保证速度连续，避免这个问题。

 

业务逻辑 → 代码映射

 

输入：2R连杆参数 + PD增益 + 三次多项式轨迹

  ↓

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

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

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

  ↓

【步骤2：轨迹生成（三次多项式）】

  起步段：q(t) = a·t³ + b·t²，满足q(0)=q_start, qd(0)=0, q(T)=q_target, qd(T)=0

  保持段：q_des恒等于q_target, qd_des=0

  ↓

【步骤3：RK4闭环仿真（双工况对比）】

  工况A：τ = Kp·e + Kd·ē + G(q_des) （带重力前馈）

  工况B：τ = Kp·e + Kd·ē （无重力前馈）

  每步记录 q_des, q_actual, e = q_des - q_actual

  ↓

【步骤4：可视化】

  4张图：期望vs实际角度、角度误差、角速度、控制力矩

 

关键公式文字翻译

 

三次多项式轨迹：q(t) = a·t³ + b·t² + c·t + d。四个系数由四个边界条件确定：起点位置、起点速度（零）、终点位置、终点速度（零）。这保证位置、速度都连续，加速度在衔接处可能不连续，但力矩不会出现阶跃。

 

PD控制律：τ = Kp·(q_des - q) + Kd·(qd_des - qd) + G_ff。Kp乘以角度偏差，产生与偏差方向相反的恢复力矩；Kd乘以角速度偏差，提供阻尼；G_ff是重力前馈项，可选。

 

稳态误差（无前馈）：e_ss ≈ G(q_target) / Kp。自重产生的重力矩完全由比例项平衡，偏差量等于重力矩除以比例增益。本篇参数下关节1稳态误差约9.0/120 ≈ 0.075 rad ≈ 4.3°，这是一个可观的偏差。

 

4. OOP代码实现

 

技术栈

 

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

 

项目结构树

 

pd_joint_tracking_sim/

├── run_sim.py # 主入口

├── requirements.txt

├── robot/

│ ├── __init__.py

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

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

│ ├── trajectory.py # 三次多项式轨迹

│ ├── 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 2R planar manipulator PD tracking 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

    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.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 warnings

import numpy as np

import math

from robot.models import SimConfig2R

 

 

class Dynamics2R:

    """

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

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

    """

    def __init__(self, config: SimConfig2R):

        self.cfg = config

        self.g = config.gravity

        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)。对2R竖直平面臂，M始终正定，无需奇异检测。"""

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

        c2 = math.cos(q[1])

 

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

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

        M12 = 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)

 

    def coriolis_vector(self, q, qd):

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

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

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

        s2 = math.sin(q[1])

 

        # h = m2·l1·r2，来自拉格朗日方程的交叉项系数

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

 

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

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

 

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

 

    def gravity_vector(self, q):

        """重力向量G(q)。q=0时连杆水平向右，重力矩为零。"""

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

        c1 = math.cos(q[0])

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

 

        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)

 

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

        """

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

        使用np.linalg.solve避免显式求逆，数值更稳定。

        """

        M = self.mass_matrix(q)

        C = self.coriolis_vector(q, qd)

        G = self.gravity_vector(q)

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

 

        # 2R臂M(q)始终正定，det>0，无需额外奇异检测

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

        return qdd

 

"robot/trajectory.py"

 

"""

三次多项式轨迹生成器：起步段（位置、速度连续）+ 保持段。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import SimConfig2R

 

 

class CubicTrajectory2R:

    """

    关节空间三次多项式轨迹：

    q(t) = a·t³ + b·t² + c·t + d

    边界条件：q(0)=q_start, qd(0)=0, q(T_ramp)=q_target, qd(T_ramp)=0

    起步段位置、速度均连续；保持段q_des恒等于q_target。

    """

    def __init__(self, q_start, q_target, T_ramp, T_total):

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

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

        self.T_ramp = float(T_ramp)

        self.T_total = float(T_total)

 

        if self.T_ramp <= 0 or self.T_total <= self.T_ramp:

            raise ValueError("Require T_ramp > 0 and T_total > T_ramp")

 

        # 求解三次多项式系数

        # q(0) = d = q_start

        # qd(0) = c = 0

        # q(T) = a·T³ + b·T² + q_start = q_target

        # qd(T) = 3·a·T² + 2·b·T = 0 → b = -1.5·a·T

        T = self.T_ramp

        delta = self.q_target - self.q_start

 

        self.a = 2.0 * delta / (T**3) # a = 2·Δ/T³

        self.b = -3.0 * delta / (T**2) # b = -3·Δ/T²

        self.c = np.zeros(2) # c = 0（起点速度为零）

        self.d = self.q_start.copy() # d = q_start

 

    def get_at(self, t):

        """返回 [q_des, qd_des, qdd_des]。"""

        if t <= 0:

            return self.q_start.copy(), np.zeros(2), np.zeros(2)

        if t >= self.T_ramp:

            return self.q_target.copy(), np.zeros(2), np.zeros(2)

 

        # 起步段内：三次多项式求值

        t2 = t * t

        t3 = t2 * t

 

        q_des = self.a * t3 + self.b * t2 + self.d

        qd_des = 3.0 * self.a * t2 + 2.0 * self.b * t

        qdd_des = 6.0 * self.a * t + 2.0 * self.b

 

        return q_des, qd_des, qdd_des

 

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

    - 无前馈：τ = Kp·(q_des - q) + Kd·(qd_des - qd)

    重力前馈使用期望位形下的重力向量，抵消大部分自重。

    """

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

                 use_gravity_ff=True):

        self.Kp = gains.Kp

        self.Kd = gains.Kd

        self.dyn = dynamics

        self.use_gravity_ff = use_gravity_ff

 

    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

 

        if self.use_gravity_ff:

            # 重力前馈：用期望关节角计算，避免反馈延迟

            grav_ff = self.dyn.gravity_vector(q_des)

            tau = tau_pd + grav_ff

        else:

            tau = tau_pd

 

        return tau

 

"robot/simulator.py"

 

"""

RK4仿真器：记录PD控制下期望角度、实际角度、输出误差。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from robot.dynamics import Dynamics2R

from robot.controller import PDController2R

from robot.models import SimConfig2R

 

 

class Simulator2R:

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

                 trajectory, config: SimConfig2R):

        self.dyn = dynamics

        self.ctrl = controller

        self.traj = trajectory

        self.config = config

 

        self.n_phys_steps = int(round(self.traj.T_total / config.dt_phys))

 

    def run(self):

        """

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

        初始状态从轨迹起点对齐，避免起步跳变。

        """

        q_start, qd_start, _ = self.traj.get_at(0.0)

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

 

        dt = self.config.dt_phys

        n = self.n_phys_steps

        t_arr = np.arange(n) * dt

 

        # 预分配历史数组

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

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

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

        error_hist = np.zeros((n, 2)) # q_des - q_actual

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

 

        for i in range(n):

            t = t_arr[i]

 

            # 期望轨迹

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

            q_des_hist[i] = q_des

 

            # 当前状态

            q = x[:2]

            qd = x[2:4]

 

            # 记录

            q_actual_hist[i] = q

            qd_actual_hist[i] = qd

            error_hist[i] = q_des - q

 

            # 控制力矩（RK4各子步内采用零阶保持，dt=0.1ms下精度足够）

            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 * dt * k1)

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

            k4 = deriv(x + dt * k3)

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

 

        return {

            't': t_arr,

            'q_des': q_des_hist,

            'q_actual': q_actual_hist,

            'qd_actual': qd_actual_hist,

            'error': error_hist,

            'tau': tau_hist,

        }

 

"robot/visualize.py"

 

"""

4张PD跟踪仿真可视化图表。

# 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 Visualizer2R:

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

        self.rd = results_dir

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_angle_tracking(self, result_ff, result_noff):

        """图1：期望角度 vs 实际角度时程（双工况对比）。"""

        t = result_ff['t']

        q_des = result_ff['q_des']

        q_act_ff = result_ff['q_actual']

        q_act_noff = result_noff['q_actual']

 

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

 

        # 关节1

        axes[0].plot(t, np.degrees(q_des[:, 0]), 'k--', linewidth=1.5,

                     label='期望角度')

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

                     label='实际（带前馈）', alpha=0.8)

        axes[0].plot(t, np.degrees(q_act_noff[:, 0]), 'r-', linewidth=0.8,

                     label='实际（无前馈）', alpha=0.6)

        axes[0].set_ylabel('关节1角度 (deg)')

        axes[0].legend(fontsize=7)

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

 

        # 关节2

        axes[1].plot(t, np.degrees(q_des[:, 1]), 'k--', linewidth=1.5,

                     label='期望角度')

        axes[1].plot(t, np.degrees(q_act_ff[:, 1]), 'b-', linewidth=1.0,

                     label='实际（带前馈）', alpha=0.8)

        axes[1].plot(t, np.degrees(q_act_noff[:, 1]), 'r-', linewidth=0.8,

                     label='实际（无前馈）', alpha=0.6)

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

        axes[1].set_ylabel('关节2角度 (deg)')

        axes[1].legend(fontsize=7)

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

 

        fig.suptitle('PD控制下期望角度 vs 实际角度（带/无重力前馈对比）')

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_angle_error(self, result_ff, result_noff):

        """图2：角度误差时程（双工况对比）。"""

        t = result_ff['t']

        err_ff = np.degrees(result_ff['error'])

        err_noff = np.degrees(result_noff['error'])

 

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

 

        axes[0].plot(t, err_ff[:, 0], 'b-', linewidth=1.0, label='带前馈')

        axes[0].plot(t, err_noff[:, 0], 'r-', linewidth=0.8, label='无前馈')

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

        axes[0].set_ylabel('关节1角度误差 (deg)')

        axes[0].legend(fontsize=7)

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

 

        axes[1].plot(t, err_ff[:, 1], 'b-', linewidth=1.0, label='带前馈')

        axes[1].plot(t, err_noff[:, 1], 'r-', linewidth=0.8, label='无前馈')

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

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

        axes[1].set_ylabel('关节2角度误差 (deg)')

        axes[1].legend(fontsize=7)

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

 

        fig.suptitle('PD控制角度误差时程（带/无重力前馈对比）')

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_velocity(self, result):

        """图3：实际角速度时程（带前馈工况）。"""

        t = result['t']

        qd = np.degrees(result['qd_actual'])

 

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

 

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

        axes[0].set_ylabel('关节1角速度 (deg/s)')

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

 

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

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

        axes[1].set_ylabel('关节2角速度 (deg/s)')

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

 

        fig.suptitle('实际关节角速度时程（带重力前馈）')

        plt.tight_layout()

        path = os.path.join(self.rd, "03_velocity.png")

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_torque(self, result_ff, result_noff):

        """图4：控制力矩时程（双工况对比）。"""

        t = result_ff['t']

        tau_ff = result_ff['tau']

        tau_noff = result_noff['tau']

 

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

 

        axes[0].plot(t, tau_ff[:, 0], 'b-', linewidth=1.0, label='带前馈')

        axes[0].plot(t, tau_noff[:, 0], 'r-', linewidth=0.8, label='无前馈')

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

        axes[0].set_ylabel('关节1力矩 (N·m)')

        axes[0].legend(fontsize=7)

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

 

        axes[1].plot(t, tau_ff[:, 1], 'b-', linewidth=1.0, label='带前馈')

        axes[1].plot(t, tau_noff[:, 1], 'r-', linewidth=0.8, label='无前馈')

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

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

        axes[1].set_ylabel('关节2力矩 (N·m)')

        axes[1].legend(fontsize=7)

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

 

        fig.suptitle('PD控制力矩时程（带/无重力前馈对比）')

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

5. 可视化要求

 

序号 图表文件 服务结论

① 

"01_angle_tracking.png" 期望vs实际角度——对比带/无重力前馈两种工况的跟踪效果

② 

"02_angle_error.png" 角度误差时程——量化前馈对稳态误差的消除效果

③ 

"03_velocity.png" 实际角速度——验证三次多项式起步段速度平滑、衔接处无跳变

④ 

"04_torque.png" 控制力矩——观察前馈如何分担自重、降低PD负担

 

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

 

快速开始

 

pip install -r requirements.txt

python -m pytest tests/ -v

python run_sim.py

 

数据格式说明

 

参数 修改位置 说明

连杆参数 

"run_sim.py" 中 

"Link2R(...)" 修改质量/长度/惯量

PD增益 

"PDGains2R(Kp=[...], Kd=[...])" 调整各关节增益

轨迹目标 

"CubicTrajectory2R(q_start, q_target, ...)" 修改起点/目标角度

起步时间 

"T_ramp=0.5" 调整起步段时长

 

📌 知识点卡片

 

PD关节伺服跟踪（对应课本第5、6章）

 

- 三次多项式轨迹：q(t)=a·t³+b·t²，满足起点/终点位置、速度（零）四边界条件，保证速度连续

- PD+重力前馈：τ = Kp·e + Kd·ē + G(q_des)，前馈抵消自重，稳态误差趋近于零

- 无前馈稳态误差：e_ss ≈ G(q_target) / Kp，比例项独自平衡自重

- RK4积分：四阶龙格-库塔法，每子步内控制量零阶保持，dt=0.1 ms下精度足够

 

7. 总结与工程师思考

 

工业落地难处

 

一、摩擦力矩未建模。 仿真中没有任何摩擦，实际关节有库仑摩擦和粘性摩擦，起步瞬间需要额外力矩克服静摩擦，到位后摩擦会帮助消除微小振荡。

 

二、参数辨识误差。 连杆质量、质心位置、惯量都是标称值，实际存在5%~10%的偏差，重力前馈不完全准确，稳态误差会略大于仿真结果。

 

三、实时控制器积分步长限制。 RK4需要很小的dt（本篇0.1 ms）才能保证数值稳定性，实时控制器受计算周期限制，通常用更高阶或隐式积分方法。

 

工程师心得

 

小赵看着图2的曲线："带重力前馈时，到位后关节1的稳态误差几乎压到零附近；关掉前馈，同样的Kp=120，稳态误差直接跳到约4°。三次多项式起步段速度连续，0.5 s衔接处没有任何尖峰——这跟之前用二阶多项式时那个突刺完全不同。"

 

老周点头："重力前馈就是干这个的。你看图4，带前馈时到位后关节1力矩稳定在约9 N·m，正好是平衡自重需要的数值；无前馈时这个9 N·m全靠比例项扛，误差自然就大了。调试PD的第一步是确认前馈是否正确，前馈对了，PD只需要处理动态跟踪，增益可以设得比较保守。"

 

"而且轨迹本身也得平滑。速度不连续的轨迹会在衔接处制造一个假的'冲击'，让你误以为是增益没调好，其实根子在轨迹规划。"

 

8. 写作与风格限制

 

本篇全部代码为课程离线仿真，运行在普通PC上，用于PD伺服跟踪的离线校核。代码中不包含任何真实机器人控制器的实时通信栈或硬件驱动逻辑。 完全去营销，中立客观。全部物理量标注完整单位。

 

9. 文末附录

 

环境配置

 

pip install numpy>=1.24.0 matplotlib>=3.7.0 pytest>=7.0.0

 

"requirements.txt"：

 

numpy>=1.24.0

matplotlib>=3.7.0

pytest>=7.0.0

 

运行说明

 

cd pd_joint_tracking_sim

pip install -r requirements.txt

python -m pytest tests/ -v

python run_sim.py

 

预期运行时间：纯Python实现，约10~30秒（取决于CPU），请耐心等待。

 

参数修改指南

 

想改什么 改哪里 示例

连杆质量 

"Link2R(..., mass=3.0, ...)" 改为5.0

PD增益 

"PDGains2R(Kp=[120,80], ...)" 调整为[200,150]

目标角度 

"q_target=[np.pi/4, np.pi/6]" 改为[np.pi/3, 0]

起步时间 

"T_ramp=0.5" 改为0.8

 

输出文件清单

 

文件 含义

 

"results/01_angle_tracking.png" 期望角度 vs 实际角度时程（双工况）

 

"results/02_angle_error.png" 角度误差时程（双工况）

 

"results/03_velocity.png" 实际角速度时程（带前馈）

 

"results/04_torque.png" 控制力矩时程（双工况）

 

正文完。以下为 

"run_sim.py" 完整主入口 + 

"tests/test_integration.py" 完整测试文件。

 

"run_sim.py"

 

"""

Main entry: 2R PD joint tracking simulation.

记录PD控制下期望角度、实际角度、输出误差随时间变化曲线。

对比带重力前馈与无重力前馈两种工况。

Run: python run_sim.py

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import os

import sys

import math

import numpy as np

 

sys.path.insert(0, os.path.dirname(__file__))

 

from robot.models import SimConfig2R, PDGains2R, Link2R

from robot.dynamics import Dynamics2R

from robot.trajectory import CubicTrajectory2R

from robot.controller import PDController2R

from robot.simulator import Simulator2R

 

 

def run_scenario(name, links, gains, traj, use_gravity_ff):

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