《Python机器人基础原理》第一百一十三篇末端轨迹跟踪仿真——结合逆运动学与PD控制实现机械臂跟踪笛卡尔目标路径声明本篇全部为课程离线仿真代码不能直接用于真实机器人控制器规避工程误导风险。1. 实际工业应用场景周五上午弧焊工位。一台2R竖直平面机械臂要沿着一段预设的圆弧焊缝移动末端焊枪。小赵把CAD导出的笛卡尔路径点x_des, y_des灌进仿真环境用逆运动学把每个路径点转成关节角再用三次样条插值生成关节空间轨迹PD参数设为Kp300、Kd50并加入重力前馈补偿。仿真跑完他调出末端实际轨迹——蓝线和黑线几乎重合末端线误差全程在8 mm以内。重力前馈起了大作用。老郑指着误差曲线。上篇纯PD驻留段偏差0.33 rad末端飘了160 mm。这次加了τ_ff G(q_des)重力矩被预先抵消Kp300只管动态残差稳态误差直接压到亚毫米级。不过你注意看前0.3秒——起始时刻有个约1.9 rad的关节跳变PD全力拉回来暂态误差飙到25 mm之后才收敛进跟踪段。小赵点头对起始位q_start默认[0,0]但轨迹起点对应的关节角不是零。我应该把q_start设成IK在t0的解模拟焊枪已经在起弧点待机的状态。这样暂态段就没了全程误差更均匀。构型约定本篇采用竖直平面内2R机械臂弧焊/涂胶作业。连杆参数L1 a₁0.50 m / m₁6.5 kg / I₁0.45 kg·m² / com₁0.25 mL2 a₂0.40 m / m₂4.0 kg / I₂0.20 kg·m² / com₂0.20 m。末端目标路径圆心(0.45, 0.30) m、半径0.15 m、角度范围-30°~30°的圆弧21个航点总时长4.0 s。该路径端点到基座距离r∈[0.542, 0.598] m对应q₂∈[-106.8°, -97.5°]远离两个奇异位形q₂0°伸直/r0.90 mq₂±180°折叠/r0.10 mIK条件数全程良好。逆运动学解析几何法余弦定理atan2取肘向下分支q₂0。PD参数Kp[300, 200] N·m/radKd[50, 35] N·m·s/rad。重力前馈τ_ff G(q_des)。力矩限幅±120/±90 N·m。RK4步长0.001 s仿真5.0 s留1.0 s收敛余量。技术栈numpy scipyinterpolate matplotlib。⚠️ 声明本篇全部代码为课程离线仿真运行在普通PC上用于末端轨迹跟踪验证。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。2. 痛点分析课程模块 本篇对应的工程痛点第3章 连杆参数及运动学求解 → 逆运动学 笛卡尔路径点需实时转换为关节角解析解需处理多解和workspace边界第6章 轨迹规划与控制 → 笛卡尔空间轨迹 末端路径直接规划比关节空间更直观但需IK作为桥梁第5章 机器人关节驱动与控制 → PD重力前馈 重力前馈补偿主导偏差PD只管动态跟踪残差本篇代码解决 L2 离线仿真校核层。输入2R连杆参数、笛卡尔路径航点x_wp, y_wp, t_wp、IK参数、PD增益、仿真时长。处理①笛卡尔路径三次样条插值②逆运动学逐点求解关节角含连续性校验③关节轨迹三次样条插值补速度/加速度④PD重力前馈控制⑤RK4闭环仿真⑥正运动学计算末端实际轨迹。输出4张末端跟踪分析图表 误差报告。不覆盖任务空间控制雅可比转置/伪逆控制、奇异回避策略、冗余解析、力控柔顺。3. 核心逻辑讲解物理直觉末端轨迹跟踪的核心思路是笛卡尔路径 → 逆运动学 → 关节轨迹 → PD前馈控制。就像你开车导航给你的是地图上的路线笛卡尔空间但车只能控制方向盘和油门关节空间。你需要一个翻译把地图路线转成方向盘角度——这就是逆运动学。逆运动学不是万能的。2R机械臂的workspace是一个圆环半径从|L1-L2|0.10 m到L1L20.90 m。路径点如果落在这个环外IK无解如果靠近边界奇异区IK对数值扰动极度敏感。本篇的圆弧路径r∈[0.542, 0.598] m远离两个奇异区差60°以上IK条件数全程良好不存在病态跳变——这是刻意选择的良性教学路径让读者先看到正确做法的结果。有了关节角轨迹后控制器在关节空间运行PD反馈修正跟踪残差重力前馈预先抵消重力矩。重力前馈的加入是本篇与上篇的核心区别——上篇纯PD下e_ssG/Kp≈0.33 rad末端160 mm本篇τ_ffG(q_des)把重力吃掉稳态误差压到亚毫米级。业务逻辑 → 代码映射输入笛卡尔航点 (x_wp, y_wp, t_wp) 连杆参数 PD增益↓【步骤1笛卡尔路径插值】对x和y分别用CubicSpline → x_des(t), y_des(t), xd_des(t), yd_des(t)↓【步骤2逆运动学】对每个插值点余弦定理求q₂atan2求q₁取肘向下分支q₂0检查workspace边界连续性校验相邻采样点关节角跳变阈值↓【步骤3关节轨迹插值】对q₁(t), q₂(t)三次样条 → q_des(t), qd_des(t), qdd_des(t)↓【步骤4控制器】τ Kp·(q_des - q) Kd·(qd_des - qd) G(q_des)重力前馈在期望位形求值标准做法↓【步骤5仿真循环】RK4积分5.0秒每步查询时变参考起始状态 IK(t0)的解模拟焊枪已在起弧点待机记录 q(t) → 正运动学算末端 (x_end, y_end)↓【步骤6指标统计】末端位置误差 ||p_des - p_end||₂、各段RMSE↓【步骤7绘图输出4张图】① 末端实际轨迹vs期望圆弧XY平面② 末端X坐标跟踪③ 末端Y坐标跟踪④ 末端线误差时序关键公式文字翻译余弦定理逆解已知末端(x,y)距离r√(x²y²)。由三角形边长r² L1²L2²2·L1·L2·cos(q₂)反解q₂ ±arccos((r²-L1²-L2²)/(2·L1·L2))。取负为肘向下。atan2(y,x)结合三角形内角求q₁。正运动学x_end L1·cos(q₁) L2·cos(q₁q₂)y_end L1·sin(q₁) L2·sin(q₁q₂)。PD重力前馈τ Kp·e_q Kd·e_qd G(q_des)。重力项在期望位形求值非实际位形这是标准做法——前馈是开环的不依赖反馈状态避免耦合。末端线误差e_end(t) √((x_des-x_end)² (y_des-y_end)²)单位m。4. OOP代码实现技术栈限定numpy, scipyinterpolate仅用于轨迹插值, matplotlib。不引入pybullet、roboticstoolbox等任何第三方重型库。项目结构树end_effector_tracking/├── run_end_tracking.py # 主入口完整未截断├── requirements.txt├── robot/│ ├── __init__.py│ ├── models.py # dataclass数据模型│ ├── kinematics.py # 正/逆运动学解析解│ ├── dynamics.py # 2R闭式动力学│ ├── trajectory_cartesian.py # 笛卡尔路径插值IK生成关节轨迹│ ├── controller.py # PD重力前馈控制器│ ├── simulator.py # RK4仿真器│ ├── metrics.py # 末端误差指标│ └── visualize.py # 4张图表封装├── tests/│ ├── __init__.py│ └── test_end_tracking.py # 3类测试11个用例├── results/ # 4张图表└── output/ # 误差报告txt核心源码robot/models.pyData models for 2R end-effector trajectory tracking.All parameters built-in, self-contained.from dataclasses import dataclass, fieldimport numpy as npdataclassclass LinkInertia:name: strmass: float # kglength: float # minertia: float # kg·m^2com_offset: float # mdef __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])dataclassclass PDGains:Kp: np.ndarrayKd: np.ndarraydef __post_init__(self):kp 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: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:links: list field(default_factorylist)gravity: float 9.81dt: float 0.001t_sim: float 5.0def __post_init__(self):if not self.links:self.links [LinkInertia(L1, 6.5, 0.50, 0.45, 0.25),LinkInertia(L2, 4.0, 0.40, 0.20, 0.20),]robot/kinematics.py2R planar forward and inverse kinematics (analytical, closed-form).Includes workspace check, elbow-down selection, and continuity guard.import numpy as npimport mathfrom robot.models import SimConfigclass Kinematics2R:def __init__(self, config: SimConfig):self.l1 config.links[0].lengthself.l2 config.links[1].lengthdef forward(self, q):Forward kinematics: joint angles → end-effector (x, y).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])def inverse(self, x, y, elbow_downTrue):Inverse kinematics: (x, y) → (q1, q2).elbow_downTrue → q2 negative (elbow below).Raises ValueError if (x,y) is outside workspace.r2 x**2 y**2r math.sqrt(r2)r_min abs(self.l1 - self.l2)r_max self.l1 self.l2if r r_min - 1e-6 or r r_max 1e-6:raise ValueError(fPoint ({x:.4f}, {y:.4f}) outside workspace f[{r_min:.3f}, {r_max:.3f}] m)# Cosine law for q2cos_q2 (r2 - self.l1**2 - self.l2**2) / (2.0 * self.l1 * self.l2)cos_q2 max(-1.0, min(1.0, cos_q2)) # numerical clampq2 math.acos(cos_q2)if elbow_down:q2 -q2# q1 from triangle geometryalpha math.atan2(y, x)beta math.atan2(self.l2 * math.sin(abs(q2)),self.l1 self.l2 * math.cos(abs(q2)))if elbow_down:q1 alpha - betaelse:q1 alpha betareturn np.array([q1, q2])def jacobian(self, q):Geometric Jacobian at current joint angles.q1, q2 q[0], q[1]return np.array([[-self.l1 * math.sin(q1) - self.l2 * math.sin(q1 q2),-self.l2 * math.sin(q1 q2)],[self.l1 * math.cos(q1) self.l2 * math.cos(q1 q2),self.l2 * math.cos(q1 q2)]])robot/dynamics.py2R planar rigid-body dynamics (vertical plane).M, C, G — closed form. Gravity vector exposed for feedforward.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_cartesian.pyCartesian path interpolation and IK-based joint trajectory generation.Includes elbow-consistency check across samples.import numpy as npfrom scipy.interpolate import CubicSplinefrom robot.kinematics import Kinematics2Rclass CartesianTrajectory:def __init__(self, waypoint_times, waypoints_xy, kinematics: Kinematics2R):self.t_wp np.asarray(waypoint_times, dtypefloat)self.p_wp np.asarray(waypoints_xy, dtypefloat)if self.t_wp.ndim ! 1:raise ValueError(waypoint_times must be 1-D)if self.p_wp.ndim ! 2 or self.p_wp.shape[1] ! 2:raise ValueError(fwaypoints_xy must be (N, 2), got {self.p_wp.shape})if len(self.t_wp) ! self.p_wp.shape[0]:raise ValueError(time and position arrays must have same length)if not np.all(np.diff(self.t_wp) 0):raise ValueError(waypoint_times must be strictly increasing)self.kin kinematicsself._spline_x CubicSpline(self.t_wp, self.p_wp[:, 0], bc_typenatural)self._spline_y CubicSpline(self.t_wp, self.p_wp[:, 1], bc_typenatural)self.t_start self.t_wp[0]self.t_end self.t_wp[-1]def evaluate_cartesian(self, t):t np.clip(t, self.t_start, self.t_end)x self._spline_x(t)y self._spline_y(t)xd self._spline_x(t, 1)yd self._spline_y(t, 1)xdd self._spline_x(t, 2)ydd self._spline_y(t, 2)return np.array([x, y]), np.array([xd, yd]), np.array([xdd, ydd])def generate_joint_trajectory(self, num_samples200, elbow_downTrue,continuity_tol0.5):Sample Cartesian path, solve IK at each point, return joint angles.continuity_tol: max allowed jump between adjacent samples (rad).t_samples np.linspace(self.t_start, self.t_end, num_samples)q_samples np.zeros((num_samples, 2))for i, t in enumerate(t_samples):p, _, _ self.evaluate_cartesian(t)q self.kin.inverse(p[0], p[1], elbow_downelbow_down)q_samples[i, :] q# Continuity check: detect elbow-branch flipdiff np.abs(np.diff(q_samples, axis0))max_jump np.max(diff)if max_jump continuity_tol:raise RuntimeError(fIK elbow branch flip detected: max joint jump {max_jump:.3f} rad {continuity_tol})return t_samples, q_samplesclassmethoddef builtin_arc_trajectory(cls, kinematics: Kinematics2R):Built-in arc path for welding demo.Center (0.45, 0.30) m, radius 0.15 m, angle -30° to 30°.21 waypoints, 4.0s total.This path is WELL within workspace, far from singularities.center_x, center_y 0.45, 0.30radius 0.15angles np.linspace(-np.pi/6, np.pi/6, 21)t_wp np.linspace(0.0, 4.0, 21)waypoints_xy np.zeros((21, 2))for i, theta in enumerate(angles):waypoints_xy[i, 0] center_x radius * np.cos(theta)waypoints_xy[i, 1] center_y radius * np.sin(theta)return cls(t_wp, waypoints_xy, kinematics)robot/controller.pyPD feedback gravity feedforward controller.τ Kp·(q_des - q) Kd·(qd_des - qd) G(q_des)Gravity feedforward evaluated at DESIRED posture (standard practice).import numpy as npfrom robot.models import PDGains, TorqueLimitsfrom robot.dynamics import Dynamics2Rclass PDGravityController:def __init__(self, gains: PDGains, torque_limits: TorqueLimits,dynamics: Dynamics2R):self.Kp gains.Kpself.Kd gains.Kdself.tau_max torque_limits.tau_max.reshape(2, 1)self.dyn dynamicsdef compute_torque(self, q, q_dot, q_des, qd_des):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)e_q q_des - qe_qd qd_des - q_dot# Gravity feedforward at DESIRED posture (open-loop, no feedback coupling)G_ff self.dyn.gravity_vector(q_des)tau_raw (self.Kp.reshape(2, 1) * e_q self.Kd.reshape(2, 1) * e_qd G_ff)tau_clipped np.clip(tau_raw, -self.tau_max, self.tau_max)return tau_clippedrobot/simulator.py2R end-effector tracking simulation with time-varying reference.RK4 integration, queries joint trajectory splines at each step.import numpy as npfrom robot.dynamics import Dynamics2Rfrom robot.controller import PDGravityControllerfrom robot.models import SimConfigfrom scipy.interpolate import CubicSplineclass EndTrackingSimulator:def __init__(self, dynamics: Dynamics2R,controller: PDGravityController,joint_traj_times, joint_traj_q,config: SimConfig):self.dyn dynamicsself.ctrl controllerself.cfg configself.dt config.dtself.n_steps int(config.t_sim / config.dt)# Build cubic splines for joint trajectoriesself._spl_q1 CubicSpline(joint_traj_times, joint_traj_q[:, 0],bc_typenatural)self._spl_q2 CubicSpline(joint_traj_times, joint_traj_q[:, 1],bc_typenatural)self._t_min joint_traj_times[0]self._t_max joint_traj_times[-1]def _get_ref(self, t):Get joint reference at time t, clamped to trajectory bounds.t_clip max(self._t_min, min(t, self._t_max))q_des np.array([self._spl_q1(t_clip), self._spl_q2(t_clip)])qd_des np.array([self._spl_q1(t_clip, 1), self._spl_q2(t_clip, 1)])return q_des, qd_desdef _derivatives(self, x, t):q x[0:2]qd x[2:4]q_des, qd_des self._get_ref(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:# Default: start at zero (will cause transient, see main script)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]k1 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.pyEnd-effector tracking error metrics.All numbers generated by code — no hand-written conclusions.import numpy as npdef compute_end_effector_error(t, x_hist, kinematics, cartesian_traj):Compute end-effector position error at each time step.Returns: p_des_hist (N,2), p_actual_hist (N,2), e_end_hist (N,2), e_norm (N,)n len(t)p_des_hist np.zeros((n, 2))p_actual_hist np.zeros((n, 2))e_end_hist np.zeros((n, 2))e_norm np.zeros(n)for i in range(n):p_des, _, _ cartesian_traj.evaluate_cartesian(t[i])p_des_hist[i, :] p_desq_actual x_hist[i, 0:2]p_actual kinematics.forward(q_actual)p_actual_hist[i, :] p_actuale_end_hist[i, :] p_des - p_actuale_norm[i] np.linalg.norm(p_des - p_actual)return p_des_hist, p_actual_hist, e_end_hist, e_normdef compute_rmse(arr, axis0):return np.sqrt(np.mean(arr**2, axisaxis))robot/visualize.pyVisualization for end-effector 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_end_trajectory(self, p_des, p_actual):fig, ax plt.subplots(figsize(7, 7))ax.plot(p_des[:, 0], p_des[:, 1], k--, linewidth1.5, labelDesired arc)ax.plot(p_actual[:, 0], p_actual[:, 1], b-, linewidth1.0, labelActual path)ax.set_xlabel(X position (m))ax.set_ylabel(Y position (m))ax.set_title(End-Effector Trajectory Tracking (PD Gravity FF))ax.legend(fontsize8)ax.axis(equal)ax.grid(True, alpha0.3)path os.path.join(self.results_dir, 01_end_trajectory.png)plt.tight_layout()plt.savefig(path, dpi150)plt.close()return pathdef plot_x_tracking(self, t, p_des, p_actual):fig, ax plt.subplots(figsize(10, 4))ax.plot(t, p_des[:, 0], k--, linewidth1.5, labelx_des)ax.plot(t, p_actual[:, 0], b-, linewidth1.0, labelx_actual)ax.set_xlabel(Time (s))ax.set_ylabel(X position (m))ax.set_title(End-Effector X Coordinate Tracking)ax.legend(fontsize8)ax.grid(True, alpha0.3)path os.path.join(self.results_dir, 02_x_coordinate_tracking.png)plt.tight_layout()plt.savefig(path, dpi150)plt.close()return pathdef plot_y_tracking(self, t, p_des, p_actual):fig, ax plt.subplots(figsize(10, 4))ax.plot(t, p_des[:, 1], k--, linewidth1.5, labely_des)ax.plot(t, p_actual[:, 1], b-, linewidth1.0, labely_actual)ax.set_xlabel(Time (s))ax.set_ylabel(Y position (m))ax.set_title(End-Effector Y Coordinate Tracking)ax.legend(fontsize8)利用AI解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛