python的机器人基础原理工业场景模拟第一百二十篇:机械臂仿真，模拟采样周期变化，改变控制步长，观察闭环系统稳定性变化。
《Python机器人基础原理》第一百二十篇：机械臂仿真——模拟采样周期变化，改变控制步长，观察闭环系统稳定性变化
声明：本篇全部为课程离线仿真代码，不能直接用于真实机器人控制器，规避工程误导风险。
1. 实际工业应用场景
周三下午，电控实验室。小赵把新写好的PD控制算法烧进控制器，接上2R机械臂做空载测试。奇怪的事情发生了——Kp=300、Kd=45，这组参数在Simulink里仿真跑得稳稳当当，末端误差不到20 mm。可一上实机，机械臂到了轨迹中段就开始高频抖动，像得了帕金森一样。
"你控制器周期多少？"老郑放下手里的示波器探头。
"我设的1 ms，跟仿真一样。"
"你确定实机控制循环真的1 ms跑完一次？"老郑调出任务管理器，"你看这个——CPU占用率飙到90%的时候，你的控制任务被操作系统抢了三次，实际间隔在0.8 ms到3.2 ms之间跳。你的PD是在变步长下跑的。"
小赵愣住了："变步长？仿真里我用的固定1 ms啊……"
"对。仿真里时间是理想的，实机采样周期会因为中断延迟、任务调度、通信抖动而变化。你的控制器稳定性是建立在'每一步间隔完全相同'的假设上的——这个假设在产线上根本不成立。"
构型约定：竖直平面内2R机械臂。连杆参数：L1 a₁=0.55 m / m₁=8.0 kg / I₁=0.60 kg·m² / com₁=0.28 m；L2 a₂=0.45 m / m₂=10.0 kg（含5 kg负载）/ I₂=0.35 kg·m² / com₂=0.25 m。末端从(0.75, 0.55)到(0.30, 0.55)水平直线搬运，5次多项式5.0 s平滑轨迹。PD参数：Kp=[300, 225] N·m/rad，Kd=[45, 33.75] N·m·s/rad。力矩限幅±150/±120 N·m。仿真6.0 s。测试4种控制步长：dt∈{0.001, 0.002, 0.005, 0.010} s。技术栈：numpy + scipy + matplotlib。
⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于验证采样周期对闭环稳定性的影响。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。
2. 痛点分析
课程模块 本篇对应的工程痛点
第5章 机器人关节驱动与控制 → 离散时间控制 仿真用固定步长，实机采样周期受中断延迟/调度抖动影响，变步长可能导致失稳
第4章 机器人动力学建模 → 刚体动力学 重力矩约120 N·m，步长变大时PD离散化误差累积，重力补偿不足导致下垂
第6章 轨迹规划与控制 → 跟踪性能 步长变化直接影响位置反馈的新鲜度，大步长下高频抖动
本篇代码解决 L2 离线仿真校核层。输入：2R连杆参数、笛卡尔航点、PD增益、多组控制步长。处理：①5次多项式生成平滑轨迹→IK→关节样条；②4种步长分别仿真（RK4积分步长=控制步长，ZOH离散控制器）；③计算RMSE和峰值力矩。输出：4张对比图表+稳定性结论。
不覆盖：实时操作系统调度模型、通信延迟、多速率采样。以下第7节讨论的传感器噪声、调度抖动等属于延伸方向，不在本篇代码实现范围内。
3. 核心逻辑讲解
物理直觉
想象你蒙着眼睛推一辆小车沿直线走。每隔1秒有人告诉你一次"你现在偏左了10 cm"，你可以及时调整方向。如果变成每隔5秒才告诉你一次呢？等你收到反馈时，小车已经偏出去半米了——你猛打方向修正，结果矫枉过正，又偏到另一边去了。这就是离散化导致的相位滞后：反馈越不频繁，你的"修正动作"越容易跟实际偏差错位，最终系统开始振荡甚至发散。
控制步长dt本质上是控制器"看到"世界的时间分辨率。dt越小，控制器对偏差的响应越及时，系统越稳定；dt越大，相位滞后越严重，当dt超过某个临界值时，PD的"阻尼"效果变成了"激励振荡"。
业务逻辑 → 代码映射
输入：连杆参数 + 轨迹 + PD增益 + 步长列表[0.001, 0.002, 0.005, 0.010]
↓
【步骤1：轨迹生成（只做一次）】
5次多项式 → p_des(t) → IK → q_des(t)，构建关节样条
↓
【步骤2：对每个步长 dt 运行仿真】
① 每个控制周期 dt 内：
- 在周期起始时刻采样当前状态 q[k], qd[k]
- 采样参考值 q_des[k], qd_des[k]（ZOH：整个周期内恒定）
- 计算一次 τ[k] = PD(q[k], qd[k], q_des[k], qd_des[k])
- τ[k] 在整个 dt 内保持不变（零阶保持）
② RK4 用恒定 τ[k] 推进整个 dt（子步内不重算控制器）
↓
【步骤3：对比分析】
计算每组dt的RMSE、峰值力矩
绘制4张对比图
关键公式文字翻译
离散PD控制（ZOH）：τ[k] = Kp·(q_des[k] − q[k]) + Kd·(qd_des[k] − qd[k])。方括号[k]表示第k个控制周期采样的值。与连续PD的区别在于：误差不是实时计算的，而是每隔dt才更新一次，且τ在整个dt内保持恒定。这正是实机"定时中断+零阶保持"的真实行为。
ZOH等效延迟：零阶保持器引入约 dt/2 的等效时间延迟。这个延迟吃掉系统的相位裕度——当相位裕度耗尽时，离散闭环极点越出单位圆，系统开始振荡。直观指标：RMSE突然跳升、力矩曲线出现高频锯齿、关节角度出现"过冲-回弹"的反复振荡。
5次多项式：p(t) = a₀ + a₃t³ + a₄t⁴ + a₅t⁵，其中a₁=a₂=0（初始速度/加速度为零）。边界条件为起终点位置/速度/加速度均为零。确保轨迹平滑，不给PD控制器制造瞬态冲击。
4. OOP代码实现
技术栈
限定：numpy, scipy（interpolate）, matplotlib。不引入pybullet、roboticstoolbox等任何第三方重型库。
项目结构树
sampling_period_stability/
├── run_sim.py # 主入口
├── requirements.txt
├── robot/
│ ├── __init__.py
│ ├── models.py # dataclass数据模型
│ ├── dynamics.py # 2R闭式动力学
│ ├── kinematics.py # 正/逆运动学
│ ├── trajectory.py # 5次多项式轨迹 + IK + 样条
│ ├── controller.py # PD控制器（含限幅）
│ ├── simulator.py # ZOH离散步长RK4仿真器
│ ├── metrics.py # 评价指标
│ └── visualize.py # 4张图表
├── tests/
│ ├── __init__.py
│ └── test_stability.py # 3类测试
├── results/ # 4张图表
└── output/ # 指标报告txt
核心源码
"robot/models.py"
"""
Data models for sampling period stability simulation.
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
from dataclasses import dataclass
import numpy as np
@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 PDGains:
Kp: np.ndarray
Kd: np.ndarray
def __post_init__(self):
self.Kp = np.asarray(self.Kp, dtype=float).reshape(2,)
self.Kd = np.asarray(self.Kd, dtype=float).reshape(2,)
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")
@dataclass
class TorqueLimits:
tau_max: np.ndarray
def __post_init__(self):
self.tau_max = np.asarray(self.tau_max, dtype=float).reshape(2,)
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:
links: list # required — no default_factory
gravity: float = 9.81
t_sim: float = 6.0
def __post_init__(self):
if not self.links:
raise ValueError("SimConfig.links must be provided, cannot be empty")
"robot/dynamics.py"
"""
2R planar rigid-body dynamics (vertical plane).
Closed-form M, C, G — pure numpy, no symbolic dependency.
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
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
def mass_matrix(self, q):
"""M(q) — symmetric positive definite inertia matrix."""
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)
def coriolis_vector(self, q, qd):
"""C(q, qd)·qd — centrifugal and Coriolis forces."""
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)
def gravity_vector(self, q):
"""G(q) — gravity torques in vertical plane."""
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)
def forward_dynamics(self, q, qd, tau):
"""
Solve M(q)·qdd = τ - C(q,qd)·qd - G(q) for qdd.
Used by RK4 integrator at each step.
"""
M = self.mass_matrix(q)
Cqd = self.coriolis_vector(q, qd)
G = self.gravity_vector(q)
rhs = tau.reshape(2, 1) - Cqd - G
qdd = np.linalg.solve(M, rhs)
return qdd.flatten()
"robot/kinematics.py"
"""
2R planar forward/inverse kinematics.
FIXED: elbow_down branch uses signed q2 in atan2 — no abs() bug.
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import numpy as np
import math
class Kinematics2R:
def __init__(self, l1=0.55, l2=0.45):
self.l1 = l1
self.l2 = l2
def forward(self, q):
q1, q2 = q[0], q[1]
x = self.l1 * math.cos(q1) + self.l2 * math.cos(q1 + q2)
y = self.l1 * math.sin(q1) + self.l2 * math.sin(q1 + q2)
return np.array([x, y], dtype=float)
def inverse(self, x, y, elbow_down=True):
"""
Analytical IK for 2R planar arm.
q1 = alpha - atan2(l2*sin(q2), l1 + l2*cos(q2))
This formula works for BOTH elbow_up and elbow_down branches
because atan2 correctly handles the signed q2.
"""
r2 = x**2 + y**2
r = math.sqrt(r2)
r_min, r_max = abs(self.l1 - self.l2), self.l1 + self.l2
if r < r_min - 1e-6 or r > r_max + 1e-6:
raise ValueError(f"Point ({x:.4f}, {y:.4f}) outside workspace "
f"[{r_min:.3f}, {r_max:.3f}] m")
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 = math.acos(cos_q2)
if elbow_down:
q2 = -q2 # signed q2, used directly below
alpha = math.atan2(y, x)
# gamma uses signed q2 — works for both branches without abs()
gamma = math.atan2(self.l2 * math.sin(q2),
self.l1 + self.l2 * math.cos(q2))
q1 = alpha - gamma
return np.array([q1, q2], dtype=float)
"robot/trajectory.py"
"""
Cartesian straight-line trajectory via quintic polynomial + IK.
Standard coefficients: a3=10Δ/T³, a4=-15Δ/T⁴, a5=6Δ/T⁵
a1=a2=0 because initial velocity and acceleration are zero.
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import numpy as np
from scipy.interpolate import CubicSpline
from robot.kinematics import Kinematics2R
class TransferTrajectoryGenerator:
def __init__(self, kinematics: Kinematics2R, num_samples=600):
self.kin = kinematics
self.num_samples = num_samples
def generate_straight_line(self, p_start, p_end, t_total=5.0):
T = t_total
delta = np.asarray(p_end) - np.asarray(p_start)
# Standard quintic (verified: p(T)=p_end, p'(T)=0, p''(T)=0)
# a1=a2=0 from zero initial velocity/acceleration boundary conditions
a0 = np.asarray(p_start, dtype=float)
a3 = 10.0 * delta / T**3
a4 = -15.0 * delta / T**4
a5 = 6.0 * delta / T**5
t = np.linspace(0.0, T, self.num_samples)
p = np.zeros((self.num_samples, 2))
for i, ti in enumerate(t):
p[i] = a0 + a3*ti**3 + a4*ti**4 + a5*ti**5
# IK — path is fully inside workspace, failure = config error
q = np.zeros((self.num_samples, 2))
for i in range(self.num_samples):
try:
q[i] = self.kin.inverse(p[i, 0], p[i, 1], elbow_down=True)
except ValueError as e:
raise RuntimeError(
f"IK failed at sample {i}, t={t[i]:.3f}s: {e}") from e
# Note: CubicSpline uses natural boundary (2nd derivative=0)
# while quintic guarantees 0 velocity/acceleration at endpoints.
# For this simulation the difference is negligible (<1e-4 rad).
spl_q1 = CubicSpline(t, q[:, 0], bc_type='natural')
spl_q2 = CubicSpline(t, q[:, 1], bc_type='natural')
spl_x = CubicSpline(t, p[:, 0], bc_type='natural')
spl_y = CubicSpline(t, p[:, 1], bc_type='natural')
return t, p, spl_x, spl_y, spl_q1, spl_q2
"robot/controller.py"
"""
PD controller with torque saturation (clipping).
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import numpy as np
from robot.models import PDGains, TorqueLimits
class PDController:
def __init__(self, gains: PDGains, torque_limits: TorqueLimits,
enable_saturation=True):
self.Kp = gains.Kp
self.Kd = gains.Kd
self.tau_max = torque_limits.tau_max.reshape(2, 1)
self.enable_saturation = enable_saturation
def compute_torque(self, q, qd, q_des, qd_des):
"""
PD: τ = Kp*(q_des - q) + Kd*(qd_des - qd)
If saturation enabled, clip to [-τ_max, +τ_max].
"""
q = np.asarray(q).reshape(2, 1)
qd = np.asarray(qd).reshape(2, 1)
q_des = np.asarray(q_des).reshape(2, 1)
qd_des = np.asarray(qd_des).reshape(2, 1)
e_q = q_des - q
e_qd = qd_des - qd
tau_raw = (self.Kp.reshape(2, 1) * e_q
+ self.Kd.reshape(2, 1) * e_qd)
if self.enable_saturation:
tau = np.clip(tau_raw, -self.tau_max, self.tau_max)
else:
tau = tau_raw
return tau.flatten()
"robot/simulator.py"
"""
ZOH discrete-step simulator: RK4 with variable control period dt.
CORRECT ZOH: At each control period, compute torque ONCE from sampled
state and reference, then hold it constant for the entire dt.
Reference values are also sampled once per period (ZOH on q_des/qd_des).
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import numpy as np
import math
from robot.dynamics import Dynamics2R
from robot.controller import PDController
from robot.models import SimConfig
class DiscreteStepSimulator:
def __init__(self, dynamics: Dynamics2R, controller: PDController,
t_grid, spl_q1, spl_q2, dt_control: float,
config: SimConfig):
self.dyn = dynamics
self.ctrl = controller
self.t_grid = t_grid # original trajectory time grid (for ZOH sampling)
self.spl_q1 = spl_q1
self.spl_q2 = spl_q2
self.dt = dt_control # control period = RK4 step size
self.t_sim = config.t_sim
# Use round() to avoid float division truncation
self.n_steps = int(round(self.t_sim / dt_control))
# Sanity: verify no time lost
actual_t = self.n_steps * self.dt
if abs(actual_t - self.t_sim) > 1e-9:
print(f" Warning: t_sim={self.t_sim} not integer multiple of "
f"dt={self.dt}, actual sim time={actual_t:.4f}s")
def _get_ref_zoh(self, t_period_start):
"""
Sample reference trajectory ONCE at period start (ZOH).
Uses the original fine trajectory grid for lookup.
"""
t_clip = max(self.t_grid[0], min(t_period_start, self.t_grid[-1]))
q1 = float(self.spl_q1(t_clip))
q2 = float(self.spl_q2(t_clip))
qd1 = float(self.spl_q1(t_clip, 1))
qd2 = float(self.spl_q2(t_clip, 1))
return np.array([q1, q2]), np.array([qd1, qd2])
def run(self, q_start):
"""
Run simulation with ZOH discrete control.
At each control period:
1. Sample state and reference at period start
2. Compute torque ONCE
3. Hold tau constant for the entire dt (RK4 with fixed tau)
"""
x = np.concatenate([q_start, np.zeros(2)])
n = self.n_steps + 1
x_hist = np.zeros((n, 4))
tau_hist = np.zeros((n, 2))
t_arr = np.arange(n) * self.dt
x_hist[0] = x
for i in range(self.n_steps):
t = t_arr[i]
# --- ZOH: sample reference at period START, hold for entire dt ---
q_des, qd_des = self._get_ref_zoh(t)
# --- ZOH: compute torque ONCE from current state ---
q_curr, qd_curr = x[0:2], x[2:4]
tau = self.ctrl.compute_torque(q_curr, qd_curr, q_des, qd_des)
tau_hist[i] = tau
# --- RK4 with CONSTANT tau for the whole dt ---
# tau is closed over as a default argument (fixed at period start)
def deriv(xx, tau_fixed=tau):
qq = xx[0:2]
qqd = xx[2:4]
qdd = self.dyn.forward_dynamics(qq, qqd, tau_fixed)
return np.concatenate([qqd, qdd])
k1 = deriv(x)
k2 = deriv(x + 0.5*self.dt*k1)
k3 = deriv(x + 0.5*self.dt*k2)
k4 = deriv(x + self.dt*k3)
x = x + (self.dt/6.0)*(k1 + 2*k2 + 2*k3 + k4)
x_hist[i+1] = x
# Final tau at last state
q_des_final, qd_des_final = self._get_ref_zoh(t_arr[-1])
tau_hist[-1] = self.ctrl.compute_torque(
x[0:2], x[2:4], q_des_final, qd_des_final)
return t_arr, x_hist, tau_hist
"robot/metrics.py"
"""
Metrics: RMSE and peak torque for stability analysis.
# OFFLINE SIMULATION ONLY — not for production controllers.
"""
import numpy as np
from robot.kinematics import Kinematics2R
def compute_cartesian_error(t, x_hist, spl_x, spl_y, kin: Kinematics2R):
"""Cartesian end-effector error using original path spline."""
n = len(t)
e_norm = np.zeros(n)
for i in range(n):
q_actual = x_hist[i, 0:2]
p_actual = kin.forward(q_actual)
t_clip = max(spl_x.x[0], min(t[i], spl_x.x[-1]))
p_des = np.array([float(spl_x(t_clip)), float(spl_y(t_clip))])
e_norm[i] = np.linalg.norm(p_actual - p_des)
return e_norm
def compute_rmse(arr):
return np.sqrt(np.mean(arr**2))
def compute_peak_torque(tau_hist):
"""Maximum absolute torque across all joints and time steps."""
return float(np.max(np.abs(tau_hist)))
"robot/visualize.py"
"""
4 charts for sampling period stability analysis.
# 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 StabilityVisualizer:
def __init__(self, results_dir="results"):
self.results_dir = results_dir
os.makedirs(results_dir, exist_ok=True)
def plot_cartesian_error_comparison(self, results_dict):
"""Cartesian error for all dt values on one plot."""
fig, ax = plt.subplots(figsize=(10, 5))
for dt, (t, e) in results_dict.items():
ax.plot(t, e * 1000, linewidth=1.0, label=f'dt={dt*1000:.0f} ms')
ax.set_xlabel('Time (s)')
ax.set_ylabel('Cartesian Error (mm)')
ax.set_title('End-Effector Error vs Control Period')
ax.legend(fontsize=8)
ax.grid(True, alpha=0.3)
path = os.path.join(self.results_dir, "01_cartesian_error_comparison.png")
plt.tight_layout()
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_joint1_trajectory(self, results_dict):
"""Joint 1 angle for all dt values — shows oscillation onset."""
fig, ax = plt.subplots(figsize=(10, 5))
for dt, (t, x_hist) in results_dict.items():
ax.plot(t, np.rad2deg(x_hist[:, 0]), linewidth=1.0,
label=f'dt={dt*1000:.0f} ms')
ax.set_xlabel('Time (s)')
ax.set_ylabel('Joint 1 Angle (deg)')
ax.set_title('Joint 1 Trajectory vs Control Period (oscillation visible at large dt)')
ax.legend(fontsize=8)
ax.grid(True, alpha=0.3)
path = os.path.join(self.results_dir, "02_joint1_trajectory.png")
plt.tight_layout()
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_torque_comparison(self, results_dict):
"""Joint 1 torque for all dt values."""
fig, ax = plt.subplots(figsize=(10, 5))
for dt, (t, tau_hist) in results_dict.items():
ax.plot(t, tau_hist[:, 0], linewidth=0.8,
label=f'dt={dt*1000:.0f} ms')
ax.axhline(150, color='r', linestyle='--', linewidth=0.8,
label='Limit +150 N·m')
ax.axhline(-150, color='r', linestyle='--', linewidth=0.8,
label='Limit -150 N·m')
ax.set_xlabel('Time (s)')
ax.set_ylabel('Joint 1 Torque (N·m)')
ax.set_title('Joint 1 Torque vs Control Period')
ax.legend(fontsize=7, ncol=2)
ax.grid(True, alpha=0.3)
path = os.path.join(self.results_dir, "03_torque_comparison.png")
plt.tight_layout()
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_rmse_vs_dt(self, dt_list, rmse_list):
"""Bar chart: RMSE vs control period."""
fig, ax = plt.subplots(figsize=(8, 5))
dt_ms = [dt * 1000 for dt in dt_list]
rmse_mm = [r * 1000 for r in rmse_list]
# Color bars by actual RMSE value (not fixed position)
colors = []
for r in rmse_mm:
if r < 30:
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
0
0- 0
已为社区贡献172条内容
所有评论(0)