《Python机器人基础原理》第一百一十二篇码垛工位仿真——导入预先规划关节轨迹使用PD控制器完成时序轨迹跟随仿真声明本篇全部为课程离线仿真代码不能直接用于真实机器人控制器规避工程误导风险。1. 实际工业应用场景周三夜班纸箱码垛线。一台2自由度竖直平面码垛机械臂正在跑新换的堆叠模板——5层×4列每层Z轴递增120 mm俯仰平面完成取放。小赵把离线规划软件导出的关节轨迹灌进仿真环境PD参数设为Kp₁200、Kd₁40按下启动。仿真跑完他调出跟随误差曲线驻留段关节1的稳态偏差约0.33 rad18.9°末端线误差折算下来超过160 mm——纸箱早就偏出托盘了。你这误差不是拐点的锅。老郑调出重力项数值。home位重力矩G₁≈65 N·mKp₁才200稳态偏差e_ssG₁/Kp₁≈0.33 rad这是纯PD的物理天花板。轨迹是三次样条速度全程连续根本不存在速度突变——误差最大的地方反而是驻留段因为重力一直在拉PD拉不回来。小赵翻出动力学公式对样条C²连续加速度也是连续的。误差来源按量级排重力矩准静态偏差~0.3 rad排第一然后是惯性力矩加速段叠加~0.1 rad科氏力最小。纯反馈只能吃残差主力得靠前馈——下一篇加重力前馈把65 N·m先抵消掉Kp就能有效管跟踪了。构型约定本篇采用竖直平面内2R机械臂码垛俯仰作业。连杆参数L1 a₁0.55 m / m₁8.2 kg / I₁0.62 kg·m² / com₁0.28 mL2 a₂0.45 m / m₂5.6 kg / I₂0.31 kg·m² / com₂0.23 m。目标轨迹6个航点、5段3段运动2段驻留总时长4.0 s三次样条插值生成q_des(t)、qd_des(t)、qdd_des(t)。PD参数Kp[200, 120] N·m/radKd[40, 24] N·m·s/rad。力矩限幅±120/±90 N·m较上篇放宽避免全程饱和掩盖跟随特性。RK4步长0.001 s仿真5.0 s留1.0 s收敛余量。技术栈numpy scipyinterpolate matplotlib。⚠️ 声明本篇全部代码为课程离线仿真运行在普通PC上用于轨迹跟随性能验证。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。2. 痛点分析课程模块 本篇对应的工程痛点第6章 轨迹规划与控制 → 关节空间轨迹插值 离线规划只给位置航点需三次样条补速度/加速度剖面才能做跟踪仿真第5章 机器人关节驱动与控制 → PD反馈跟踪 纯PD对时序轨迹跟随误差大重力矩是主导偏差源e_ssG/Kp第9章 仿真实验 → 轨迹跟随仿真 离线仿真暴露纯反馈的局限性为下一篇前馈控制提供定量动机本篇代码解决 L2 离线仿真校核层。输入2R连杆参数、关节轨迹航点时间戳、PD参数、仿真时长。处理①三次样条插值补速度/加速度②PD反馈控制器计算力矩纯反馈无模型前馈③RK4闭环仿真④分段RMSE统计。输出4张轨迹跟随分析图表 误差报告。不覆盖模型前馈重力/惯性补偿、计算力矩控制、笛卡尔空间轨迹、碰撞检测。3. 核心逻辑讲解物理直觉轨迹跟随和阶跃响应是两码事。阶跃响应是从静止到静止的一次性过程而轨迹跟随要求机械臂每个时刻都踩在预定位置上——这就像开车跟着导航线走导航线全程平滑三次样条C²连续但你的车有重量、有惯性导航线一转弯你就因为离心力偏出去了。纯PD反馈只管现在偏了多少位置误差e_q和偏的速度有多快速度误差e_qd但不管你本来应该出多少力来克服重力。本篇连杆更重m₁8.2 kghome位重力矩高达65 N·m。Kp200时稳态偏差e_ss65/200≈0.33 rad——这是纯PD的物理下限跟轨迹平滑不平滑无关。误差最大的地方不是拐点样条根本没有速度跳变而是驻留段——因为重力一直在拉PD只能靠弹簧力顶着顶不住就偏。本篇的核心做法读入航点 → 三次样条插值补出q_des(t)、qd_des(t)、qdd_des(t) → 控制器输出 τ Kp·(q_des - q) Kd·(qd_des - qd)纯反馈→ RK4仿真5.0 s → 统计各段RMSE。全部数字由代码生成不手写结论。业务逻辑 → 代码映射输入航点时间t_wp、航点位置q_wp6×2、连杆参数、PD增益↓【步骤1轨迹插值】scipy.interpolate.CubicSpline 构建自然边界三次样条给定任意t返回 q_des(t), qd_des(t), qdd_des(t)C²连续无速度跳变↓【步骤2控制器】τ Kp·(q_des(t) - q) Kd·(qd_des(t) - qd)纯反馈无模型前馈基线版本↓【步骤3仿真循环】RK4积分5.0秒每子步传入当前时间t查询时变参考记录 q(t), 误差 e(t) q_des(t) - q(t)↓【步骤4指标统计】各段RMSE、最大跟随误差、稳态偏差↓【步骤5绘图输出4张图】① 关节1期望vs实际位置② 关节2期望vs实际位置③ 两关节跟随误差时序④ 各段RMSE柱状图关键公式文字翻译三次样条插值给定N个时间点的位置点构造N-1段三次多项式每段 a·t³b·t²c·td。约束每段两端位置连续、一阶导数连续、二阶导数连续两端点二阶导0自然边界。这样补出的速度和加速度全程平滑不会引入虚假跳变。PD反馈力矩τ_fb Kp·e_q Kd·e_qd其中e_q q_des - q位置误差e_qd qd_des - qd速度误差。Kp把偏差拉回来Kd抑制振荡。单位N·m。稳态偏差重力主导驻留段qd_des0、qdd_des0PD输出需平衡重力Kp·e_ss G(q_des)所以e_ss G/Kp。home位G₁≈65 N·mKp₁200 → e_ss₁≈0.33 rad。这是纯反馈的物理下限。跟随误差RMSERMSE_i √(1/T ∫(q_des - q)²dt)。反映该段跟踪精度单位rad。4. OOP代码实现技术栈限定numpy, scipyinterpolate仅用于轨迹插值, matplotlib。不引入pybullet、roboticstoolbox等任何第三方重型库。项目结构树trajectory_tracking/├── run_tracking.py # 主入口完整未截断├── requirements.txt├── robot/│ ├── __init__.py│ ├── models.py # dataclass数据模型│ ├── dynamics.py # 2R闭式动力学│ ├── trajectory.py # 轨迹加载与三次样条插值│ ├── controller.py # PD反馈控制器│ ├── simulator.py # RK4仿真器时变期望│ ├── metrics.py # RMSE等跟随误差指标│ └── visualize.py # 4张图表封装├── tests/│ ├── __init__.py│ └── test_tracking.py # 3类测试11个用例├── results/ # 4张图表└── output/ # 误差报告txt核心源码robot/models.pyData models for 2R trajectory tracking 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):# Validate each field BEFORE any reshape/cast that could mask errorsif 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. Diagonal (decoupled) gains.Kp: np.ndarrayKd: np.ndarraydef __post_init__(self):# Check ndim and size BEFORE reshape, so errors are descriptivekp np.asarray(self.Kp, dtypefloat)if kp.ndim ! 1 or kp.size ! 2:raise ValueError(fKp must be 1-D length-2, got ndim{kp.ndim}, size{kp.size})self.Kp kpkd np.asarray(self.Kd, dtypefloat)if kd.ndim ! 1 or kd.size ! 2:raise ValueError(fKd must be 1-D length-2, got ndim{kd.ndim}, size{kd.size})self.Kd kddataclassclass 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 trajectory tracking simulation.links: list field(default_factorylist)gravity: float 9.81dt: float 0.001t_sim: float 5.0def __post_init__(self):if not self.links:# Heavier links for palletizing demo (m18.2kg, m25.6kg)self.links [LinkInertia(L1, 8.2, 0.55, 0.62, 0.28),LinkInertia(L2, 5.6, 0.45, 0.31, 0.23),]robot/dynamics.py2R planar rigid-body dynamics (vertical plane).Closed-form M, C, G — verified against hand calculations.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/trajectory.pyTrajectory loading and cubic spline interpolation.Built-in trajectory: 6 waypoints, 5 segments (3 move 2 dwell).Total duration 4.0s, simulation runs 5.0s (1.0s settling margin).import numpy as npfrom scipy.interpolate import CubicSplineclass JointTrajectory:Joint-space trajectory with cubic spline interpolation.Provides q_des(t), qd_des(t), qdd_des(t) at any time.def __init__(self, waypoint_times, waypoints):waypoint_times: 1D array of N time points (s), strictly increasingwaypoints: (N, 2) array of joint positions (rad)self.t_way np.asarray(waypoint_times, dtypefloat)self.q_way np.asarray(waypoints, dtypefloat)# Validate inputs before using themif self.t_way.ndim ! 1:raise ValueError(waypoint_times must be 1-D)if self.q_way.ndim ! 2 or self.q_way.shape[1] ! 2:raise ValueError(fwaypoints must be (N, 2), got {self.q_way.shape})if len(self.t_way) ! self.q_way.shape[0]:raise ValueError(waypoint_times and waypoints must have same length)if not np.all(np.diff(self.t_way) 0):raise ValueError(waypoint_times must be strictly increasing)# Build cubic splines for each joint (natural boundary zero 2nd deriv at ends)self._splines []for j in range(2):cs CubicSpline(self.t_way, self.q_way[:, j], bc_typenatural)self._splines.append(cs)self.t_start self.t_way[0]self.t_end self.t_way[-1]def evaluate(self, t):Return q_des, qd_des, qdd_des at time t.Clamped to trajectory bounds to avoid extrapolation.t np.clip(t, self.t_start, self.t_end)q np.array([cs(t) for cs in self._splines])qd np.array([cs(t, 1) for cs in self._splines])qdd np.array([cs(t, 2) for cs in self._splines])return q, qd, qddclassmethoddef builtin_trajectory(cls):Built-in trajectory for palletizing demo.6 waypoints, 5 segments:Seg1 (0-0.8): Home → approach pick (move)Seg2 (0.8-1.6): dwell at pickSeg3 (1.6-2.4): pick → approach place (move)Seg4 (2.4-3.2): dwell at placeSeg5 (3.2-4.0): place → home (move)t_wp np.array([0.0, 0.8, 1.6, 2.4, 3.2, 4.0])# Joint 1: sweeping left to right (pick→place)# Joint 2: folding/unfolding for height changesq_wp np.array([[0.0, 0.0], # home[0.52, 0.35], # approach pick[0.52, 0.35], # dwell at pick (repeated waypoint → zero velocity)[-0.52, 0.52], # approach place[-0.52, 0.52], # dwell at place (repeated waypoint)[0.0, 0.0], # return home])return cls(t_wp, q_wp)robot/controller.pyPD feedback controller for trajectory tracking.τ Kp·(q_des - q) Kd·(qd_des - qd)This is PURE FEEDBACK (error-driven). Model-based feedforward is NOT included— that is the subject of the next article.import numpy as npfrom robot.models import PDGains, TorqueLimitsclass PDTrackingController:def __init__(self, gains: PDGains, torque_limits: TorqueLimits):self.Kp gains.Kpself.Kd gains.Kdself.tau_max torque_limits.tau_max.reshape(2, 1)def compute_torque(self, q, q_dot, q_des, qd_des):Compute PD feedback torque.q_des, qd_des: 2-element arrays from trajectory interpolator at current time.q np.asarray(q).reshape(2, 1)q_dot np.asarray(q_dot).reshape(2, 1)q_des np.asarray(q_des).reshape(2, 1)qd_des np.asarray(qd_des).reshape(2, 1)# Position error and velocity errore_q q_des - qe_qd qd_des - q_dot# Pure feedback: Kp * e_q Kd * e_qdtau_raw self.Kp.reshape(2, 1) * e_q self.Kd.reshape(2, 1) * e_qdtau_clipped np.clip(tau_raw, -self.tau_max, self.tau_max)return tau_clippedrobot/simulator.py2R trajectory tracking simulation with time-varying reference.RK4 integration, passes current time t to _derivatives for trajectory query.import numpy as npfrom robot.dynamics import Dynamics2Rfrom robot.controller import PDTrackingControllerfrom robot.trajectory import JointTrajectoryfrom robot.models import SimConfigclass TrackingSimulator:def __init__(self, dynamics: Dynamics2R,controller: PDTrackingController,trajectory: JointTrajectory,config: SimConfig):self.dyn dynamicsself.ctrl controllerself.traj trajectoryself.cfg configself.dt config.dtself.n_steps int(config.t_sim / config.dt)def _derivatives(self, x, t):Compute state derivative dx/dt at time t.q x[0:2]qd x[2:4]# Query time-varying reference at CURRENT time tq_des, qd_des, _ self.traj.evaluate(t)tau self.ctrl.compute_torque(q, qd, q_des, qd_des)M self.dyn.mass_matrix(q)C self.dyn.coriolis_vector(q, qd)G self.dyn.gravity_vector(q)qdd np.linalg.solve(M, tau - C - G)return np.concatenate([qd, 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] xt_arr np.arange(self.n_steps 1) * self.dtdt self.dtfor i in range(self.n_steps):t t_arr[i]xi x_hist[i]# RK4 sub-steps use t dt/2, t dt for trajectory queryk1 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_hist[i1] xi (dt / 6.0) * (k1 2*k2 2*k3 k4)return t_arr, x_histrobot/metrics.pyTrajectory tracking error metrics.RMSE, max error, steady-state error per dwell segment.All numbers generated by code — no hand-written conclusions.import numpy as npdef compute_tracking_errors(t, q_hist, trajectory):Compute tracking error e(t) q_des(t) - q(t) at each time step.Vectorized where possible — loop retained for clarity at 1kHz.n len(t)e_hist np.zeros((n, 2))for i in range(n):q_des, _, _ trajectory.evaluate(t[i])e_hist[i, 0] q_des[0] - q_hist[i, 0]e_hist[i, 1] q_des[1] - q_hist[i, 1]return e_histdef compute_rmse(e_hist, t, segment_maskNone):Compute RMSE of tracking error over specified time mask.if segment_mask is None:mask slice(None)else:mask segment_maske e_hist[mask, :]if len(e) 2:return np.array([np.inf, np.inf])return np.sqrt(np.mean(e**2, axis0))def compute_segment_rmse(t, e_hist, segment_bounds):Compute RMSE for each trajectory segment.segment_bounds: list of (t_start, t_end) tuples.Returns array of shape (n_segments, 2).n_seg len(segment_bounds)rmse_seg np.zeros((n_seg, 2))for i, (t0, t1) in enumerate(segment_bounds):mask (t t0) (t t1)n_valid np.sum(mask)if n_valid 1:rmse_seg[i, :] compute_rmse(e_hist, t, mask)return rmse_segdef compute_dwell_error(t, e_hist, trajectory, dwell_bounds):Compute mean absolute steady-state error during dwell segments.This reveals the gravity-induced bias e_ss ≈ G/Kp.dwell_err []for t0, t1 in dwell_bounds:mask (t t0) (t t1)if np.sum(mask) 1:dwell_err.append(np.mean(np.abs(e_hist[mask, :]), axis0))return dwell_errrobot/visualize.pyVisualization for trajectory tracking: 4 charts.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_joint_position_tracking(self, t, q_hist, trajectory, joint_idx):Chart 1/2: Expected vs actual position for one joint.q_des_hist np.array([trajectory.evaluate(ti)[0][joint_idx]for ti in t])fig, ax plt.subplots(figsize(10, 5))ax.plot(t, q_des_hist, k--, linewidth1.5,labelfq{joint_idx1}_des (expected))ax.plot(t, q_hist[:, joint_idx], b-, linewidth1.0,labelfq{joint_idx1}_actual)# Mark segment boundariesfor seg_t in [0.8, 1.6, 2.4, 3.2, 4.0]:ax.axvline(seg_t, colorgray, linestyle:, linewidth0.8, alpha0.6)ax.set_xlabel(Time (s))ax.set_ylabel(Angle (rad))ax.set_title(fJoint {joint_idx1} Position Tracking (Pure PD Feedback))ax.legend(fontsize8)ax.grid(True, alpha0.3)fname f0{joint_idx1}_joint{joint_idx1}_position_tracking.pngpath os.path.join(self.results_dir, fname)plt.tight_layout()plt.savefig(path, dpi150)plt.close()return pathdef plot_tracking_error(self, t, e_hist):Chart 3: Tracking error for both joints.fig, ax plt.subplots(figsize(10, 5))ax.plot(t, e_hist[:, 0], r-, linewidth1.0, labelJoint 1 error)ax.plot(t, e_hist[:, 1], b-, linewidth1.0, labelJoint 2 error)ax.axhline(0, colork, linestyle-, linewidth0.5)# Mark segment boundariesfor seg_t in [0.8, 1.6, 2.4, 3.2, 4.0]:ax.axvline(seg_t, colorgray, linestyle:, linewidth0.8, alpha0.6)ax.set_xlabel(Time (s))ax.set_ylabel(Tracking Error (rad))ax.set_title(Trajectory Tracking Error — Gravity Dominates (Pure PD))ax.legend(fontsize8)ax.grid(True, alpha0.3)path os.path.join(self.results_dir, 03_tracking_error.png)plt.tight_layout()plt.savefig(path, dpi150)plt.close()return pathdef plot_segment_rmse(self, segment_labels, rmse_seg):Chart 4: RMSE bar chart per segment.fig, ax plt.subplots(figsize(10, 5))x np.arange(len(segment_labels))w 0.35bars1 ax.bar(x - w/2, rmse_seg[:, 0], w, labelJoint 1 RMSE,colorred, alpha0.7)bars2 ax.bar(x w/2, rmse_seg[:, 1], w, labelJoint 2 RMSE,colorblue, alpha0.7)# Annotate values on barsfor bar in bars1:h bar.get_height()if h 0.01:continueax.text(bar.get_x() bar.get_width()/2, h 0.005,f{h:.3f}, hacenter, vabottom, fontsize7)for bar in bars2:h bar.get_height()if h 0.01:continueax.text(bar.get_x() bar.get_width()/2, h 0.005,f{h:.3f}, hacenter, vabottom, fontsize7)ax.set_xlabel(Trajectory Segment)ax.set_ylabel(RMSE (rad))ax.set_title(Tracking RMSE per Segment — Dwell Segments Show Gravity Bias)ax.set_xticks(x)ax.set_xticklabels(segment_labels, rotation15, haright)ax.legend(fontsize8)ax.grid(True, alpha0.3, axisy)path os.path.join(self.results_dir, 04_segment_rmse.png)plt.tight_layout()p利用AI解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛