《Python机器人基础原理》第一百三十六篇：产线仿真——模拟D-H参数存在标定偏差，观察闭环状态下末端姿态误差变化

 

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

 

1. 实际工业应用场景

 

周三下午，汽车发动机缸体螺栓拧紧工位。一台3R竖直平面装配机械臂要从初始位形出发，沿一段平面圆弧轨迹运动，把末端拧紧枪的姿态维持在固定角度0°，完成6个螺栓的依次拧紧。

 

"第4个螺栓开始，枪口歪了。"视觉复检工位报警。工程师小赵调出轨迹回放："末端期望姿态始终指令为0°，但闭环反馈显示实际姿态在±0.8°之间波动，螺栓拧紧扭矩超差。"

 

现场标定报告摆在桌上：这台3R机械臂的D-H参数存在未完全补偿的标定偏差。连杆1的名义长度l1=0.55 m，实际测量值0.553 m，偏差+3 mm；连杆2名义长度l2=0.45 m，实际0.448 m，偏差-2 mm；关节2的零位偏移δq2=+0.005 rad（约0.29°）。控制器内部仍使用名义D-H参数做运动学解算和PD闭环，而真实物理连杆是实际尺寸。

 

"名义模型算出的控制量，作用在真实连杆上，末端姿态误差会怎么变化？"小赵打开仿真脚本，"建一个3R竖直平面模型，连杆质量8.0 kg / 5.0 kg / 2.0 kg，长度0.55/0.45/0.30 m，转动惯量0.60/0.18/0.05 kg·m²，质心0.28/0.22/0.15 m。PD用Kp=[800, 600, 200] N·m/rad，Kd=[80, 60, 20] N·m·s/rad。注入三组D-H标定偏差，跑圆弧轨迹闭环仿真，看末端姿态误差的时程和分布。"

 

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

 

2. 痛点分析

 

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

第3章 连杆参数及运动学求解 → D-H参数标定 名义D-H参数与真实几何不一致，控制器基于错误模型计算关节指令，末端位姿出现系统性偏差

第4章 机器人动力学建模 → 重力/惯性 标定偏差改变实际质心位置，重力前馈与实际重力不匹配，产生稳态姿态下垂

第5章 机器人关节驱动与控制 → 伺服闭环 闭环能抑制部分运动学误差，但无法消除参数型偏差，姿态误差在轨迹全程持续存在

 

本篇代码解决 L2 离线仿真校核层。输入：3R名义D-H参数、真实D-H偏差量、圆弧轨迹、PD增益。处理：①分别建立名义运动学模型和真实动力学模型；②控制器用名义参数计算期望轨迹和重力前馈；③真实动力学用含偏差的参数积分；④统计末端姿态误差时程。输出：4张分析图表 + 单元测试。

 

不覆盖：编码器量化误差、减速器柔性和背隙、温度漂移、视觉引导闭环。

 

3. 核心逻辑讲解

 

物理直觉

 

D-H参数是描述连杆几何关系的"户口本"。控制器以为连杆长0.55 m，实际它长0.553 m——这个3 mm的"认知偏差"会贯穿整个控制回路。控制器用名义参数把笛卡尔空间的直线或圆弧轨迹转换成关节角度指令，再用这个指令去驱动真实连杆。真实连杆按自己的实际尺寸运动，末端到达的位置和姿态自然与期望不同。

 

PD闭环会拼命修正，但它只能在名义模型算出的期望关节角附近做小范围调整，无法弥补模型本身的几何错误。结果就是：末端姿态误差不会发散，但也无法收敛到零，而是稳定在一个由标定偏差决定的残余值附近波动。偏差越大、离奇异构型越近，这个残余值越明显。

 

姿态误差由两部分叠加：一部分来自零位偏移δq_offset，它直接进入真实关节角，PD从编码器读数里看不到，属于结构性误差，闭环修不掉；另一部分来自长度偏差Δl，它改变运动学映射，控制器逆解算出的关节角与实际需要的不同，PD能抑制一部分，但重力前馈失配会留下稳态残余。

 

业务逻辑 → 代码映射

 

输入：名义D-H参数 + 真实偏差量 + 圆弧轨迹 + PD增益

  ↓

【步骤1：建立两套模型】

  名义模型：控制器视角，用零偏差D-H参数做逆解和重力前馈

  真实模型：物理世界视角，用（名义+偏差）D-H参数做动力学积分

  ↓

【步骤2：轨迹生成】

  平面圆弧：末端位置 = (xc + R·cosθ, yc + R·sinθ)

  数值微分生成 qd_des、qdd_des，避免PD靠速度误差"追赶"

  ↓

【步骤3：闭环仿真】

  每一步：

    控制器用名义参数算重力前馈 G_nominal(q_des)

    PD输出 = Kp·(q_des - q_actual) + Kd·(qd_des - qd_actual) + G_nominal

    真实动力学用实际D-H参数积分得到 q_actual(t+dt)

  ↓

【步骤4：误差统计】

  末端姿态误差 = |q_actual[0]+q_actual[1]+q_actual[2] - 0°|

  对比无偏差/仅长度偏差/长度+零位偏差三种情形

 

关键公式文字翻译

 

D-H标定偏差模型：真实连杆长度 = 名义长度 + Δl，真实零位 = 名义零位 + Δq_offset。偏差量Δl和Δq_offset是标定值，本篇作为已知注入。

 

名义重力前馈：τ_ff = G_nominal(q_des)。控制器用自己"以为"的连杆参数算重力矩。当真实参数不同时，这个前馈与实际重力矩之间存在残差ΔG = G_actual(q_actual) - G_nominal(q_des)，形成持续的稳态扰动。

 

末端姿态误差：Δθ = |q1+q2+q3 - θ_desired|。单位度，统计均值、最大值、标准差，评估标定偏差对姿态控制的影响程度。

 

4. OOP代码实现

 

技术栈

 

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

 

项目结构树

 

dh_calibration_sim/

├── run_sim.py # 主入口（D-H标定偏差仿真）

├── requirements.txt

├── robot/

│ ├── __init__.py

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

│ ├── kinematics.py # 名义正/逆运动学

│ ├── dynamics.py # 真实3R动力学（含D-H偏差）

│ ├── trajectory.py # 平面圆弧轨迹（含数值微分）

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

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

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

├── tests/

│ ├── __init__.py

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

├── results/ # 4张图表

└── README.md

 

核心源码

 

"robot/models.py"

 

"""

Data models for D-H calibration偏差 simulation.

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from dataclasses import dataclass

 

 

@dataclass

class Link3R:

    """3R竖直平面臂连杆参数（简化D-H：仅长度和关节零位偏移）。"""

    name: str

    mass: float # kg

    length: float # m（名义值）

    length_bias: float # m（标定偏差，真实长度 = length + length_bias）

    inertia: float # kg·m^2

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

    q_zero_offset: float # rad（零位标定偏差）

 

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

        actual_len = self.length + self.length_bias

        if not 0 < self.com_offset <= actual_len:

            raise ValueError(

                f"{self.name}: com_offset must be positive "

                f"and <= actual length {actual_len:.4f}")

 

    @property

    def actual_length(self):

        """真实连杆长度 = 名义值 + 标定偏差。"""

        return self.length + self.length_bias

 

 

@dataclass

class PDGains3R:

    """3R 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 != (3,):

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

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

            raise ValueError(f"Kd must be length-3, 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 SimConfig3R:

    """

    3R仿真全局配置。

    要求恰好3个连杆，与动力学公式维度严格匹配。

    """

    links: list

    gravity: float = 9.81

    dt_phys: float = 0.0001

    tau_limit: float = 150.0

 

    def __post_init__(self):

        if len(self.links) != 3:

            raise ValueError(

                f"SimConfig3R requires exactly 3 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/kinematics.py"

 

"""

3R竖直平面臂名义正/逆运动学（控制器视角，使用名义D-H参数）。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.models import Link3R

 

 

class Kinematics3R:

    """

    名义运动学：控制器以为连杆长度就是名义值，

    不考虑标定偏差。用于轨迹逆解和重力前馈计算。

    """

    def __init__(self, links):

        if len(links) != 3:

            raise ValueError("Kinematics3R requires exactly 3 links")

        self.l1 = links[0].length

        self.l2 = links[1].length

        self.l3 = links[2].length

 

    def forward_position(self, q):

        """名义正运动学：关节角→末端位置（前两个关节决定位置）。"""

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

        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 forward_orientation(self, q):

        """末端姿态 = q0 + q1 + q2（平面3R）。"""

        return q[0] + q[1] + q[2]

 

    def inverse_position(self, x, y, q_prev=None):

        """

        位置逆解（2R部分）：求q1, q2。

        多解按与q_prev最近分支选取。

        """

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

 

        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_prev is not None:

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

            solutions.sort(key=lambda q: np.linalg.norm(q - q_prev[:2]))

 

        return solutions[0].copy()

 

    def inverse_full(self, x, y, theta_des, q_prev=None):

        """

        完整逆解：给定末端位置(x,y)和姿态theta_des，求[q1, q2, q3]。

        theta_des = q1 + q2 + q3 → q3 = theta_des - q1 - q2

        """

        q12 = self.inverse_position(x, y, q_prev)

        q3 = theta_des - q12[0] - q12[1]

        return np.array([q12[0], q12[1], q3], dtype=float)

 

"robot/dynamics.py"

 

"""

3R竖直平面真实动力学（使用实际D-H参数，含标定偏差）。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import warnings

import numpy as np

import math

from robot.models import SimConfig3R, Link3R

 

 

class Dynamics3R:

    """

    真实动力学模型：使用连杆的实际长度（名义+偏差）

    和零位偏移。这是物理世界真正遵循的模型。

    """

    def __init__(self, config: SimConfig3R):

        self.cfg = config

        self.g = config.gravity

        self.links = config.links

 

        # 实际连杆参数（含标定偏差）

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

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

        self.l3 = self.links[2].actual_length

 

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

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

        self.m3 = self.links[2].mass

 

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

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

        self.r3 = self.links[2].com_offset

 

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

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

        self.I3 = self.links[2].inertia

 

        # 零位偏移：真实关节角 = 编码器读数 + offset

        self.q_zero_offsets = np.array([

            self.links[0].q_zero_offset,

            self.links[1].q_zero_offset,

            self.links[2].q_zero_offset,

        ], dtype=float)

 

    def actual_joint_angles(self, q_enc):

        """真实关节角 = 编码器读数 + 零位偏移。"""

        return np.asarray(q_enc, dtype=float) + self.q_zero_offsets

 

    def forward_kinematics(self, q_enc):

        """真实正运动学：用实际连杆长度计算末端位姿。"""

        q_act = self.actual_joint_angles(q_enc)

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

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

        theta = q_act[0] + q_act[1] + q_act[2]

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

 

    def mass_matrix(self, q_enc):

        """3R质量矩阵M(q)，使用实际参数。"""

        q_act = self.actual_joint_angles(q_enc)

        q1, q2, q3 = q_act[0], q_act[1], q_act[2]

        c2 = math.cos(q2)

        c3 = math.cos(q3)

        s2 = math.sin(q2)

        s3 = math.sin(q3)

        c23 = math.cos(q2 + q3)

        s23 = math.sin(q2 + q3)

 

        # 惯性项聚合

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

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

               + self.m3 * (self.l1**2 + self.l2**2 + self.r3**2

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

                            + 2 * self.l1 * self.r3 * c23

                            + 2 * self.l2 * self.r3 * c3))

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

               + self.m3 * (self.l2**2 + self.l2 * self.l1 * c2

                            + self.r3 * self.l1 * c23

                            + self.r3 * self.l2 * c3))

        M13 = self.m3 * self.r3 * (self.l1 * c23 + self.l2 * c3)

 

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

               + self.m3 * (self.l2**2 + self.r3**2 + 2 * self.l2 * self.r3 * c3))

        M23 = self.m3 * self.r3 * (self.l2 * c3 + self.r3)

 

        M33 = self.I3 + self.m3 * self.r3**2

 

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

                         [M12, M22, M23],

                         [M13, M23, M33]], dtype=float)

 

    def coriolis_vector(self, q_enc, qd):

        """科氏力/离心力向量（基于拉格朗日方程推导，与M一致）。"""

        q_act = self.actual_joint_angles(q_enc)

        q2, q3 = q_act[1], q_act[2]

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

 

        s2 = math.sin(q2)

        s3 = math.sin(q3)

        s23 = math.sin(q2 + q3)

 

        h12 = self.m2 * self.l1 * self.r2 + self.m3 * self.l1 * self.l2

        h13 = self.m3 * self.l1 * self.r3

        h23 = self.m3 * self.l2 * self.r3

 

        # C1: 含qd1、qd2、qd3全部耦合项

        C1 = (h12 * s2 * qd2 * (2 * qd1 + qd2)

              + h13 * s23 * (qd2 + qd3) * (2 * qd1 + qd2 + qd3))

        # C2: qd1^2项 + qd3通过qd2的耦合

        C2 = (-h12 * s2 * qd1**2

              + h23 * s3 * qd3 * (2 * qd2 + qd3)

              + h13 * s23 * (qd1 + qd2 + qd3) * qd1)

        # C3: 来自qd1和qd2对q3的耦合

        C3 = (-h13 * s23 * qd1 * (qd1 + 2 * qd2 + qd3)

              - h23 * s3 * qd2**2)

 

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

 

    def gravity_vector(self, q_enc):

        """重力向量 G(q)，使用实际参数。"""

        q_act = self.actual_joint_angles(q_enc)

        q1, q2, q3 = q_act[0], q_act[1], q_act[2]

 

        c1 = math.cos(q1)

        c12 = math.cos(q1 + q2)

        c123 = math.cos(q1 + q2 + q3)

 

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

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

              + self.m3 * self.g * (self.l1 * c1 + self.l2 * c12 + self.r3 * c123))

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

              + self.m3 * self.g * (self.l2 * c12 + self.r3 * c123))

        G3 = self.m3 * self.g * self.r3 * c123

 

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

 

    def gravity_vector_nominal(self, q_des):

        """

        名义重力向量：忽略零位偏移，仅用名义长度计算。

        这是控制器视角的前馈——它"以为"连杆就是名义尺寸。

        """

        q1, q2, q3 = float(q_des[0]), float(q_des[1]), float(q_des[2])

 

        c1 = math.cos(q1)

        c12 = math.cos(q1 + q2)

        c123 = math.cos(q1 + q2 + q3)

 

        # 使用名义长度（不含偏差）

        l1 = self.links[0].length

        l2 = self.links[1].length

        l3 = self.links[2].length

        m1, m2, m3 = self.links[0].mass, self.links[1].mass, self.links[2].mass

        r1, r2, r3 = self.links[0].com_offset, self.links[1].com_offset, self.links[2].com_offset

 

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

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

              + m3 * self.g * (l1 * c1 + l2 * c12 + r3 * c123))

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

              + m3 * self.g * (l2 * c12 + r3 * c123))

        G3 = m3 * self.g * r3 * c123

 

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

 

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

        """

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

        """

        M = self.mass_matrix(q_enc)

        Cqd = self.coriolis_vector(q_enc, qd)

        G = self.gravity_vector(q_enc)

        rhs = tau - Cqd - G

 

        det = np.linalg.det(M)

        if abs(det) < 1e-12:

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

                          RuntimeWarning)

            return np.zeros(3)

        return np.linalg.solve(M, rhs)

 

"robot/trajectory.py"

 

"""

平面圆弧轨迹生成器（含数值微分生成qd_des、qdd_des）。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

import math

from robot.kinematics import Kinematics3R

 

 

class ArcTrajectory:

    """

    平面圆弧轨迹：末端沿圆弧从θ_start到θ_end匀速运动。

    末端姿态恒为theta_fixed。

    通过中心差分生成qd_des和qdd_des，避免PD速度误差"追赶"。

    """

    def __init__(self, kinematics: Kinematics3R,

                 center, radius, theta_start, theta_end,

                 theta_fixed, T_total, dt=0.001):

        self.kin = kinematics

        self.cx, self.cy = center[0], center[1]

        self.R = radius

        self.theta_start = theta_start

        self.theta_end = theta_end

        self.theta_fixed = theta_fixed

        self.T_total = T_total

        self.dt = dt

 

        if T_total <= 0:

            raise ValueError("T_total must be positive")

 

        self.theta_span = theta_end - theta_start

 

        # 预计算轨迹表（用于数值微分）

        self._precompute()

 

    def _precompute(self):

        """预计算各时刻的期望关节角，用于中心差分。"""

        n = max(3, int(self.T_total / self.dt) + 1)

        self._t_grid = np.linspace(0, self.T_total, n)

 

        self._q_grid = np.zeros((n, 3))

        for i, t in enumerate(self._t_grid):

            theta = self._theta_at(t)

            x = self.cx + self.R * math.cos(theta)

            y = self.cy + self.R * math.sin(theta)

            self._q_grid[i] = self.kin.inverse_full(

                x, y, self.theta_fixed)

 

    def _theta_at(self, t):

        """t时刻的圆弧角度。"""

        if t <= 0:

            return self.theta_start

        if t >= self.T_total:

            return self.theta_end

        return self.theta_start + self.theta_span * (t / self.T_total)

 

    def get_at(self, t):

        """

        返回t时刻的 [q_des, qd_des, qdd_des]。

        使用中心差分从预计算表中获取导数。

        """

        # 关节角

        theta = self._theta_at(t)

        x = self.cx + self.R * math.cos(theta)

        y = self.cy + self.R * math.sin(theta)

        q_des = self.kin.inverse_full(x, y, self.theta_fixed)

 

        # 数值微分

        if t <= self._t_grid[0]:

            qd_des = np.zeros(3)

            qdd_des = np.zeros(3)

        elif t >= self._t_grid[-1]:

            qd_des = np.zeros(3)

            qdd_des = np.zeros(3)

        else:

            # 找到最近索引

            idx = np.searchsorted(self._t_grid, t)

            if idx == 0:

                idx = 1

            if idx >= len(self._t_grid):

                idx = len(self._t_grid) - 2

 

            dt_grid = self._t_grid[idx + 1] - self._t_grid[idx - 1]

            qd_des = (self._q_grid[idx + 1] - self._q_grid[idx - 1]) / dt_grid

 

            # 二阶导（三点差分）

            if idx + 1 < len(self._t_grid) and idx - 1 >= 0:

                dt2 = self._t_grid[idx + 1] - self._t_grid[idx]

                dt0 = self._t_grid[idx] - self._t_grid[idx - 1]

                # 简化：用相邻差分的差商

                qdd_des = np.zeros(3) # 对圆弧轨迹，关节加速度较小，近似为零

            else:

                qdd_des = np.zeros(3)

 

        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 PDGains3R

from robot.kinematics import Kinematics3R

from robot.dynamics import Dynamics3R

 

 

class PDController3R:

    """

    控制器使用名义D-H参数计算重力前馈。

    这模拟了真实场景：控制器"以为"连杆是名义尺寸，

    但实际物理连杆有标定偏差。

    """

    def __init__(self, gains: PDGains3R,

                 nominal_kinematics: Kinematics3R,

                 nominal_dynamics: Dynamics3R):

        self.Kp = gains.Kp

        self.Kd = gains.Kd

        self.nom_kin = nominal_kinematics

        self.nom_dyn = nominal_dynamics

 

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

        """

        PD + 名义重力前馈 G_nominal(q_des)。

        重力前馈使用期望位形下的名义重力向量，

        不含零位偏移——模拟控制器"不知道"偏差的工况。

        """

        e = q_des - q_enc

        ed = qd_des - qd

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

 

        grav_ff = self.nom_dyn.gravity_vector_nominal(q_des)

        tau = tau_pd + grav_ff

 

        return tau

 

"robot/simulator.py"

 

"""

RK4仿真器：D-H标定偏差闭环仿真。

# OFFLINE SIMULATION ONLY — not for production controllers.

"""

 

import numpy as np

from robot.dynamics import Dynamics3R

from robot.controller import PDController3R

from robot.models import SimConfig3R

 

 

class Simulator3R:

    def __init__(self, dynamics: Dynamics3R, controller: PDController3R,

                 trajectory, config: SimConfig3R):

        self.dyn = dynamics

        self.ctrl = controller

        self.traj = trajectory

        self.config = config

 

        self.t_total = trajectory.T_total

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

 

    def run(self, q_start_enc):

        """

        运行仿真，返回完整历史。

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

        """

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

        q_des_start, qd_des_start, _ = self.traj.get_at(0.0)

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

 

        # 如果初始状态与期望起点偏差较大，以期望起点为准

        if np.linalg.norm(q_start_enc - q_des_start) > 0.1:

            q_start_enc = q_des_start.copy()

 

        x = np.concatenate([q_start_enc, np.zeros(3)])

 

        dt = self.config.dt_phys

        n = self.n_phys_steps

        t_arr = np.arange(n) * dt

 

        # 预分配历史

        q_hist = np.zeros((n, 3))

        qd_hist = np.zeros((n, 3))

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

        pose_error_hist = np.zeros(n) # 末端位置误差 (mm)

        orient_error_hist = np.zeros(n) # 末端姿态误差 (deg)

 

        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_enc = x[:3]

            qd = x[3:6]

 

            # 记录

            q_hist[i] = q_enc

            qd_hist[i] = qd

 

            # 末端实际位姿

            pose_actual = self.dyn.forward_kinematics(q_enc)

            pose_des = self._desired_pose(q_des)

            pose_error_hist[i] = np.linalg.norm(

                pose_actual[:2] - pose_des[:2]) * 1000.0

            orient_error_hist[i] = abs(pose_actual[2] - self.traj.theta_fixed) * 180.0 / np.pi

 

            # 控制力矩

            tau = self.ctrl.compute_torque(q

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