《Python机器人基础原理》第一百一十篇产线仿真——给关节输出叠加高斯噪声模拟传感器噪声观察PD控制抗干扰效果声明本篇全部为课程离线仿真代码不能直接用于真实机器人控制器规避工程误导风险。1. 实际工业应用场景周三上午弧焊工作站。一台2自由度竖直平面机械臂正在做焊缝跟踪的空载试运行。小赵盯着HMI上的关节角度曲线——没有噪声时大P参数Kp₁120 N·m/rad下关节1响应干脆利落0.8秒到位超调约5%。但一打开真实的编码器反馈通道HMI上显示的角度值在目标位附近±0.015 rad约±0.86°之间高频抖动。控制力矩τ₁的频谱里出现了明显的毛刺。你那PD参数是在理想仿真里调的吧老郑靠在护栏上手里捏着一杯浓茶。实机的编码器分辨率虽然标称17位但电气噪声、机械振动耦合、电缆串扰叠在一起等效到角度反馈上就是σ≈0.015 rad的高斯噪声。你的微分项Kd·qd把噪声放大了——Kd₁20如果速度是从位置差分算出来的噪声带宽大约50 Hz放大倍数你自己算。小赵调出Python快速估算Kd₁20噪声σ_qd ≈ σ_q·ω ≈ 0.015×50×2π ≈ 4.7 rad/sτ_noise ≈ 20×4.7 ≈ 94 N·m——比重力矩21.6 N·m还大三倍多难怪驱动器在限幅边缘反复横跳。但如果速度是从独立测速机或观测器来的σ_qd可能只有0.15 rad/sτ_noise ≈ 20×0.15 3.0 N·m占控制量14%那就只是毛刺。构型约定本篇采用竖直平面内2R机械臂。连杆参数L1 a₁0.40 m / m₁5.8 kg / I₁0.35 kg·m² / com₁0.20 mL2 a₂0.35 m / m₂4.2 kg / I₂0.18 kg·m² / com₂0.175 m。目标位q_des[π/4, π/6] rad。PD增益Kpdiag(120, 80) N·m/radKddiag(20, 15) N·m·s/rad。传感器噪声高斯白噪声叠加到q和qd反馈σ_q0.015 radσ_qd0.15 rad/s独立测速方案。力矩限幅±80/±60 N·m。RK4步长0.001 s仿真5.0 s。技术栈numpy matplotlib。⚠️ 声明本篇全部代码为课程离线仿真运行在普通PC上用于PD控制抗噪声干扰分析。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。2. 痛点分析课程模块 本篇对应的工程痛点第5章 机器人关节驱动与控制 → PD控制器抗扰性 编码器噪声经微分项放大导致控制力矩出现毛刺驱动器发热第9章 仿真实验 → 传感器噪声建模 离线仿真中叠加高斯噪声模拟真实反馈评估PD参数鲁棒性第4章 机器人动力学建模 → 2R刚体动力学 噪声注入后动力学方程不变控制器接收的反馈信号被污染本篇代码解决 L2 离线仿真校核层。输入2R连杆参数、PD增益、噪声参数σ_q, σ_qd、阶跃目标位、力矩限幅、仿真时长。处理①构建2R闭式动力学M/C/G解析公式②PD闭环仿真5.0 s反馈通道叠加高斯噪声零阶保持每步注入一次③RK4积分④计算无噪声/有噪声两组指标对比。输出4张对比图表 性能指标报告。不覆盖自适应滤波、观测器设计如Kalman滤波、滑模控制、多自由度扩展。3. 核心逻辑讲解物理直觉PD控制器中的微分项DKd·qd是速度阻尼器——它感知关节速度的变化率来提供阻力。但速度qd不是直接测量的是从位置差分算出来的。编码器有噪声差分操作会把高频噪声放大微分是高通滤波器。Kd越大放大的噪声越强。这就像你在嘈杂的酒吧里试图听清一个人的说话声——背景噪音编码器噪声被你的注意力增益Kd放大了结果你听到的全是嗡嗡声。本篇的核心洞察噪声对PD控制的影响集中在微分通道。比例项P只依赖位置误差噪声被Kp直接放大但幅度有限Kp·σ_q ≈ 120×0.015 1.8 N·m微分项D把噪声放大了ω倍噪声角频率Kd再乘一次总放大倍数≈Kd·σ_qd。当这个放大后的噪声力矩超过控制力矩本身的量级系统就会失控抖动。两种速度获取方案的本质区别- 差分方案速度 (q[k] - q[k-1])/dt噪声被放大ω倍。σ_qd ≈ σ_q·ω ≈ 0.015×314 ≈ 4.7 rad/sτ_noise ≈ 94 N·m——灾难性。- 独立测速/观测器方案σ_qd 0.15 rad/s独立噪声源τ_noise ≈ 3.0 N·m——可控。本篇采用独立测速方案σ_qd0.15模拟配备独立测速传感器的系统。差分方案的灾难性后果作为对照讨论。业务逻辑 → 代码映射输入2R连杆参数PD增益噪声参数σ_q/σ_qd阶跃目标位↓【步骤1初始化】构建2R闭式动力学M/C/G初始化PD控制器初始状态 x [0,0, 0,0]设置随机数种子seed42可复现↓【步骤2RK4仿真5.0秒无噪声基线 有噪声对比】每步读q/qd真实状态无噪声组直接用真实q/qd算τ有噪声组每步生成一次噪声零阶保持q_noisy q N(0, σ_q²)qd_noisy qd N(0, σ_qd²)用噪声反馈算τ不进RK4中间级τ → M(q)qdd C(q,qd) G(q) τ → 解qdd → RK4记录两组 q_true(t), e_true(t), τ(t)↓【步骤3性能指标计算metrics.py】稳态误差 ess mean(|q_des - q_true|) over last 1.0s均方根误差 RMSE √(mean((q_des - q_true)²))控制力矩RMS √(mean(τ²))力矩噪声RMS √(mean((τ_noisy_raw - τ_clean_raw)²))轨迹平滑度 var(q_true) over last 2.0s↓【步骤4绘图输出4张对比图】关键公式文字翻译稳态误差ess纯PD控制无法消除重力项essᵢ ≈ Gᵢ(q_des) / Kpᵢ。本篇G₁(q_des) ≈ 21.57 N·mKp₁120 → ess₁ ≈ 0.180 rad约10.3°。这是物理下限噪声无法改变它——噪声只叠加在真实状态上做微小扰动。微分噪声放大τ_noise,RMS ≈ Kd·σ_qd。本篇Kd₁20σ_qd0.15 → τ_noise,RMS ≈ 3.0 N·m占稳态力矩21.6 N·m的约14%。这是可控的。测量抖动 vs 真实抖动HMI上看到的±0.015 rad抖动是测量值的抖动直接反映σ_q而真实关节角度的抖动远小于此——因为3.0 N·m的噪声力矩经过惯性滤波M₁₁≈2.07 kg·m²后产生的随机加速度约1.45 rad/s²双重积分后真实位置抖动约10⁻³ rad量级。两者必须区分。4. OOP代码实现技术栈限定numpy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方重型库。全部自实现。动力学使用闭式解析公式噪声在RK4外层零阶保持注入。项目结构树sensor_noise_simulation/├── run_sensor_noise_sim.py # 主入口完整含指标计算与绘图调用├── requirements.txt├── robot/│ ├── __init__.py│ ├── models.py # dataclass数据模型含入参校验│ ├── dynamics.py # 2R闭式动力学M/C/G解析公式│ ├── controller.py # PD控制器支持噪声注入零阶保持│ ├── simulator.py # RK4闭环仿真器噪声在run()外层注入│ ├── metrics.py # 性能指标计算│ └── visualize.py # 4张对比图封装├── tests/│ ├── __init__.py│ └── test_sensor_noise_sim.py # 3类测试入参/物理自洽/端到端├── results/ # 4张对比图输出目录└── output/ # 性能指标报告txt核心源码robot/models.py — 数据模型Data models for 2R sensor noise simulation.All parameters built-in, self-contained.from dataclasses import dataclass, fieldimport numpy as npdataclassclass LinkInertia:Rigid link inertial parameters (planar 2R, vertical plane)name: strmass: float # kglength: float # minertia: float # kg·m^2, about COMcom_offset: float # m, distance from joint to COMdef __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], fgot {self.com_offset} (length{self.length}))dataclassclass PDGains:PD gains. Only diagonal (decoupled) gains supported.Kp: np.ndarrayKd: np.ndarraydef __post_init__(self):kp np.asarray(self.Kp, dtypefloat)if kp.ndim ! 1 or kp.shape ! (2,):raise ValueError(fKp must be 1-D length-2, got shape {kp.shape})self.Kp kpkd np.asarray(self.Kd, dtypefloat)if kd.ndim ! 1 or kd.shape ! (2,):raise ValueError(fKd must be 1-D length-2, got shape {kd.shape})self.Kd kddataclassclass NoiseParams:Gaussian sensor noise parameters.sigma_q: float # rad, std of position noisesigma_qd: float # rad/s, std of velocity noiseseed: int 42def __post_init__(self):if self.sigma_q 0:raise ValueError(sigma_q must be non-negative)if self.sigma_qd 0:raise ValueError(sigma_qd must be non-negative)dataclassclass TorqueLimits:Per-joint torque saturation (N·m)tau_max: np.ndarraydef __post_init__(self):self.tau_max np.asarray(self.tau_max, dtypefloat).flatten()if self.tau_max.shape ! (2,) or np.any(self.tau_max 0):raise ValueError(tau_max must be length-2 positive array)dataclassclass SimConfig:Configuration for 2R sensor noise simulation.links: list field(default_factorylist)q_des: np.ndarray field(default_factorylambda: np.array([np.pi/4, np.pi/6]))gravity: float 9.81dt: float 0.001t_sim: float 5.0ss_window: float 1.0def __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),]self.q_des np.asarray(self.q_des, dtypefloat).flatten()if self.q_des.shape ! (2,):raise ValueError(q_des must be length-2)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)robot/dynamics.py — 2R闭式动力学2R planar rigid-body dynamics (vertical plane).Closed-form M, C, G derived from Lagrangian.import numpy as npimport mathfrom robot.models import SimConfigclass Dynamics2R:def __init__(self, config: SimConfig):self.cfg configself.links config.linksself.g config.gravityself.m1 self.links[0].massself.m2 self.links[1].massself.l1 self.links[0].lengthself.l2 self.links[1].lengthself.r1 self.links[0].com_offsetself.r2 self.links[1].com_offsetself.I1 self.links[0].inertiaself.I2 self.links[1].inertiadef 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**2return np.array([[M11, M12], [M12, M22]], dtypefloat)def coriolis_vector(self, q, qd):q2 q[1]s2 math.sin(q2)h self.m2 * self.l1 * self.r2 * s2C1 h * qd[1] * (2.0 * qd[0] qd[1])C2 -h * qd[0]**2return np.array([[C1], [C2]], dtypefloat)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 * c12return np.array([[G1], [G2]], dtypefloat)robot/controller.py — PD控制器噪声注入零阶保持PD controller with optional sensor noise injection.Noise is injected at the feedback level (controller receives noisymeasurements), NOT inside the plant dynamics. The noise is generatedONCE per simulation step in the outer RK4 loop (zero-order hold),NOT inside RK4 intermediate stages (k1/k2/k3/k4).import numpy as npfrom robot.models import PDGains, TorqueLimits, NoiseParamsclass PDController:def __init__(self, gains: PDGains, torque_limits: TorqueLimits,noise_params: NoiseParams None):self.Kp gains.Kpself.Kd gains.Kdself.tau_max torque_limits.tau_max.reshape(2, 1)self.noise_params noise_paramsif noise_params is not None:self.rng np.random.default_rng(noise_params.seed)else:self.rng Nonedef compute_torque(self, q, q_dot, q_des, noise_qNone, noise_qdNone):Compute PD torque.q, q_dot: true state (2,) or (2,1)noise_q, noise_qd: pre-generated noise for this step (None no noise)q np.asarray(q, dtypefloat).reshape(2, 1)q_dot np.asarray(q_dot, dtypefloat).reshape(2, 1)q_des np.asarray(q_des, dtypefloat).reshape(2, 1)if noise_q is not None and noise_qd is not None:# Use noisy feedback for control lawq_fb q noise_q.reshape(2, 1)qd_fb q_dot noise_qd.reshape(2, 1)tau_raw self.Kp.reshape(2, 1) * (q_des - q_fb) \- self.Kd.reshape(2, 1) * qd_fbelse:tau_raw self.Kp.reshape(2, 1) * (q_des - q) \- self.Kd.reshape(2, 1) * q_dottau_clipped np.clip(tau_raw, -self.tau_max, self.tau_max)return tau_clipped, tau_raw # return both clipped and rawrobot/simulator.py — 闭环仿真器噪声在RK4外层零阶保持2R PD simulation with sensor noise: RK4 integration.KEY DESIGN: Noise is generated ONCE per RK4 step in the outer loop(zero-order hold). It is NOT injected inside k1/k2/k3/k4 intermediatestages, preserving RK4 accuracy assumptions and matching real sensorbehavior (sample-and-hold at control rate).import numpy as npfrom robot.dynamics import Dynamics2Rfrom robot.controller import PDControllerfrom robot.models import SimConfigclass Simulator:def __init__(self, dynamics: Dynamics2R,controller: PDController,config: SimConfig,noise_modenone):self.dyn dynamicsself.ctrl controllerself.cfg configself.dt config.dtself.n_steps int(config.t_sim / config.dt)self.noise_mode noise_modedef _derivatives(self, x, tau_ctrl):Compute state derivative given an external control torque.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([x[2:4], qdd.flatten()])def run(self, q_startNone, q_dot_startNone):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# Pre-allocate noise arraysif self.noise_mode sensor and self.ctrl.noise_params is not None:np_ self.ctrl.noise_paramsnq np.zeros((self.n_steps 1, 2))nqd np.zeros((self.n_steps 1, 2))rng self.ctrl.rngfor i in range(self.n_steps 1):nq[i] rng.normal(0, np_.sigma_q, 2)nqd[i] rng.normal(0, np_.sigma_qd, 2)else:nq Nonenqd Nonedt self.dtfor i in range(self.n_steps):xi x_hist[i]# Generate control torque for this stepif self.noise_mode sensor and nq is not None:tau_clipped, _ self.ctrl.compute_torque(xi[0:2], xi[2:4], self.cfg.q_des,noise_qnq[i], noise_qdnqd[i])else:tau_clipped, _ self.ctrl.compute_torque(xi[0:2], xi[2:4], self.cfg.q_des,noise_qNone, noise_qdNone)# RK4 using the SAME torque for all sub-steps (zero-order hold)k1 self._derivatives(xi, tau_clipped)k2 self._derivatives(xi 0.5 * dt * k1, tau_clipped)k3 self._derivatives(xi 0.5 * dt * k2, tau_clipped)k4 self._derivatives(xi dt * k3, tau_clipped)x_hist[i1] xi (dt / 6.0) * (k1 2*k2 2*k3 k4)t np.arange(self.n_steps 1) * dtreturn t, x_histrobot/metrics.py — 性能指标计算Performance metrics for sensor noise simulation.All metrics computed on TRUE state (not noisy measurements).import numpy as npdef compute_metrics(t, x_hist, q_des, ss_window):Compute RMSE, steady-state error, torque RMS, trajectory smoothness.Uses last ss_window seconds of data.dt t[1] - t[0]n_window int(ss_window / dt)if n_window 0 or n_window len(t):raise ValueError(fss_window{ss_window} invalid for t_sim{t[-1]:.1f}s)# Last window indicesstart_idx max(0, len(t) - n_window)x_tail x_hist[start_idx:]metrics {}for j in range(2):qj x_hist[:, j]qj_tail x_tail[:, j]err q_des[j] - qjerr_tail q_des[j] - qj_tail# RMSE over full simulation (excluding first 0.5s transient)transient_skip min(int(0.5 / dt), len(t) // 4)err_post_transient err[transient_skip:]rmse np.sqrt(np.mean(err_post_transient**2))# Steady-state error (mean absolute over last window)ess float(np.mean(np.abs(err_tail)))# Trajectory smoothness (variance of position in last window)smoothness float(np.var(qj_tail))metrics[j] {rmse: rmse,ess: ess,smoothness_var: smoothness,}return metricsdef compute_torque_rms(tau_hist):Compute RMS of control torque for both joints.return np.sqrt(np.mean(tau_hist**2, axis0))def compute_torque_noise_rms(tau_clean_hist, tau_noisy_hist):Compute RMS of torque difference (noise contribution).diff tau_noisy_hist - tau_clean_histreturn np.sqrt(np.mean(diff**2, axis0))robot/visualize.py — 可视化Visualization for 2R sensor noise simulation: 4 charts.1. Joint 1 angle tracking (clean vs noisy) — TRUE state2. Joint 2 angle tracking (clean vs noisy) — TRUE state3. Joint 1 control torque (clean vs noisy)4. Joint 1 position error histogram (clean vs noisy, last 2s)import osimport numpy as npimport matplotlib.pyplot as pltplt.rcParams[font.sans-serif] [SimHei, DejaVu Sans,Noto Sans CJK SC]plt.rcParams[axes.unicode_minus] Falseclass Visualizer:def __init__(self, results_dirresults):self.results_dir results_diros.makedirs(results_dir, exist_okTrue)def plot_joint1_tracking(self, t, x_clean, x_noisy, q_des):Chart 1: Joint 1 TRUE angle tracking, clean vs noisy.fig, ax plt.subplots(figsize(10, 6))ax.plot(t, x_clean[:, 0], colorsteelblue, linewidth1.2,labelClean (no noise))ax.plot(t, x_noisy[:, 0], colorred, linewidth0.8,alpha0.7, labelNoisy (σ_q0.015 rad))ax.axhline(q_des[0], colorblack, linestyle:,linewidth1.2, labelfq1_des{q_des[0]:.3f})ax.set_xlabel(Time (s))ax.set_ylabel(Angle (rad))ax.set_title(Joint 1 TRUE Angle Tracking: Clean vs Sensor Noise)ax.legend(fontsize9)ax.grid(True, alpha0.3)path os.path.join(self.results_dir,01_joint1_angle_tracking.png)plt.tight_layout()plt.savefig(path, dpi150)plt.close()return pathdef plot_joint2_tracking(self, t, x_clean, x_noisy, q_des):Chart 2: Joint 2 TRUE angle tracking, clean vs noisy.fig, ax plt.subplots(figsize(10, 6))ax.plot(t, x_clean[:, 1], colorsteelblue, linewidth1.2,labelClean (no noise))ax.plot(t, x_noisy[:, 1], colorred, linewidth0.8,alpha0.7, labelNoisy (σ_q0.015 rad))ax.axhline(q_des[1], colorblack, linestyle:,linewidth1.2, labelfq2_des{q_des[1]:.3f})ax.set_xlabel(Time (s))ax.set_ylabel(Angle (rad))ax.set_title(Joint 2 TRUE Angle Tracking: Clean vs Sensor Noise)ax.legend(fontsize9)ax.grid(True, alpha0.3)path os.path.join(self.results_dir,02_joint2_angle_tracking.png)plt.tight_layout()plt.savefig(path, dpi150)plt.close()return pathdef plot_torque_comparison(self, t, tau_clean, tau_noisy):Chart 3: Joint 1 control torque, clean vs noisy.fig, ax plt.subplots(figsize(10, 6))ax.plot(t, tau_clean[:, 0], colorsteelblue, linewidth1.2,labelClean τ1)ax.plot(t, tau_noisy[:, 0], colorred, linewidth0.6,alpha0.7, labelNoisy τ1)ax.axhline(80, colorred, linestyle:, alpha0.4)ax.axhline(-80, colorred, linestyle:, alpha0.4)ax.set_xlabel(Time (s))ax.set_ylabel(tau1 (N·m))ax.set_title(Joint 1 Control Torque: Clean vs Sensor Noise)ax.legend(fontsize9)ax.grid(True, alpha0.3)path os.path.join(self.results_dir,03_joint1_torque_comparison.png)plt.tight_layout()plt.savefig(path, dpi150)plt.close()return pathdef plot_error_histogram(self, err_clean, err_noisy):Chart 4: Position error histogram for Joint 1, last 2 seconds.fig, ax plt.subplots(figsize(10, 6))ax.hist(err_clean, bins50, colorsteelblue, alpha0.6,densityTrue, labelClean error)ax.hist(err_noisy, bins50, colorred, alpha0.6,densityTrue, labelNoisy error)ax.set_xlabel(Position Error (rad))ax.set_ylabel(Probability Density)ax.set_title(Joint 1 Position Error Distribution (last 2 s, TRUE state))ax.legend(fontsize9)ax.grid(True, alpha0.3)path os.path.join(self.results_dir,04_position_error_histogram.png)plt.tight_layout()plt.savefig(path, dpi150)plt.close()return path5. 可视化要求序号 图表文件 服务结论①01_joint1_angle_trackin利用AI解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛