《Python机器人基础原理》第九十八篇离线仿真——搬运机器人关节空间轨迹规划与末端轨迹可视化声明本篇全部为课程离线仿真代码不能直接用于真实机器人控制器规避工程误导风险。1. 实际工业应用场景周一早晨发动机缸体搬运工位。新到的2R平面搬运机械臂刚完成安装上位机工程师小赵正在调试首条搬运路径。任务很简单让机械臂从起始姿态移动到目标姿态把缸体送到下料台。小赵在离线编程时设了起始关节角(0°, 30°)和目标关节角(60°, -45°)用关节空间线性插值生成中间角度想看看末端实际走的轨迹。老郑我按起始和目标角线性插值采样了50个中间点。末端走的到底是不是直线老郑端着咖啡走过来关节空间线性插值末端在笛卡尔空间走的不是直线。两个旋转关节的角速度恒定但末端位置涉及三角函数画出来的是弧线叠加。你跑一下程序把末端轨迹画出来就知道了。构型约定本篇采用2-DOF平面搬运机械臂2R连杆1 a₁0.40 m连杆2 a₂0.35 m关节1基座回转范围±170°关节2大臂俯仰范围±120°。起始关节角 q_start [0.0 rad, 0.524 rad]0°, 30°目标关节角 q_target [1.047 rad, -0.785 rad]60°, -45°。插值步数 N50。技术栈限定 numpy matplotlib。⚠️ 声明本篇全部代码为课程离线仿真运行在普通PC上用于关节空间轨迹规划与末端轨迹可视化。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。2. 痛点分析课程模块 本篇对应的工程痛点第3章 连杆参数及运动学求解 → 3.3 正运动学 关节角→末端坐标映射理解关节空间与笛卡尔空间的非线性关系第6章 轨迹规划与控制 → 6.1 关节空间规划 给定起止关节角线性插值生成中间构型第6章 → 6.2 轨迹平滑与约束 关节限位校验、采样点数对轨迹质量的影响第9章 仿真实验 → 离线调试 搬运路径预演轨迹形状可视化验证本篇代码解决 L2 离线仿真校核层。输入DH参数、起始/目标关节角、插值步数。处理①校验起止关节角是否在限位内②在关节空间对起止角进行线性插值采样N个中间构型③逐点校验中间关节角是否越限④对每个构型调用正运动学计算末端坐标⑤计算末端线速度曲线。输出4张分析图表 轨迹数据CSV。不覆盖逆运动学、笛卡尔空间直线规划、动力学约束、实时控制。3. 核心逻辑讲解物理直觉想象你的手臂从桌面左边拿一个杯子放到右边。你可以让肩膀和手肘匀速转动关节空间线性但你的手端画出的路径是一条弧线不是水平直线。这是因为关节角和手端位置之间不是简单的正比关系——它涉及三角函数。关节空间线性插值就是在每个关节的角变量之间等比例取中间值。对于2R臂两个关节各自从起始角匀速走到目标角末端位置由正运动学公式算出。因为正运动学里有cos和sin两个线性变化的角经过非线性映射后末端轨迹是两段圆弧运动的矢量和叠加一般不是严格的圆弧更不是直线。理解这一点是轨迹规划的基础。工业现场中如果你需要末端走直线比如激光切割、涂胶就不能用关节空间线性插值而要做笛卡尔空间直线规划再反解逆运动学。但在此之前你必须先搞清楚关节空间插值到底走出什么形状——这就是本篇的任务。业务逻辑 → 代码映射输入q_start [q1_s, q2_s], q_target [q1_t, q2_t], N50↓【步骤0限位校验】检查 q_start, q_target 每个分量是否在 JointLimit 范围内↓【步骤1关节空间线性插值】for k 0..N:t k / Nq1_k q1_s t * (q1_t - q1_s)q2_k q2_s t * (q2_t - q2_s)检查 q1_k, q2_k 是否越限↓【步骤2正运动学批量计算】for k 0..N:x_k a1 * cos(q1_k) a2 * cos(q1_k q2_k)y_k a1 * sin(q1_k) a2 * sin(q1_k q2_k)↓【步骤3末端线速度】v_k sqrt((dx/dt)² (dy/dt)²) (中心差分)↓【步骤44张图表】关键公式文字翻译正运动学解析解平面2R臂-x a₁·cos(θ₁) a₂·cos(θ₁θ₂)末端X坐标 大臂水平投影 小臂水平投影-y a₁·sin(θ₁) a₂·sin(θ₁θ₂)末端Y坐标 大臂垂直投影 小臂垂直投影- 为什么不用DH矩阵2R平面臂有闭式解析解直接三角函数比4×4矩阵快一个数量级适合逐点计算。线性插值-q(t) q_start t * (q_target - q_start)其中 t ∈ [0, 1]- 物理含义每个关节以恒定角速度从起始角走到目标角角速度 (q_target - q_start) / T末端线速度中心差分-vx[i] ≈ (x[i1] - x[i-1]) / (t[i1] - t[i-1])vy同理-v[i] sqrt(vx² vy²)- 为什么用中心差分端点处用前向/后向差分中间点用中心差分精度O(h²)。这能清晰展示末端线速度在关节空间线性插值下如何变化——不是恒定的。轨迹长度估算- 用折线累加各相邻点间的欧氏距离近似末端实际走过的路径长度。这比直线距离起点到终点的线段长更能反映真实运动范围。4. OOP代码实现技术栈限定numpy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方重型库。全部自实现。项目结构树trajectory_planner/├── run_trajectory_planner.py├── requirements.txt├── robot/│ ├── __init__.py│ ├── models.py│ ├── kinematics.py│ ├── trajectory.py│ └── visualize.py├── tests/│ ├── __init__.py│ └── test_trajectory_planner.py├── results/└── output/核心源码robot/models.py — 数据模型Data models: DH parameters, robot config, joint limits, trajectory paramsAll parameters built-in, self-containedimport mathfrom dataclasses import dataclass, fieldfrom typing import List, Tupledataclassclass JointLimit:Joint angle limits (rad)joint_name: strlower: floatupper: floatdef __post_init__(self):if self.lower self.upper:raise ValueError(f{self.joint_name}: lower upper)if not (math.isfinite(self.lower)and math.isfinite(self.upper)):raise ValueError(f{self.joint_name}: limits must be finite)dataclassclass RobotConfig:2-DOF planar搬运臂 (2R)a10.40m, a20.35ma1: float 0.40a2: float 0.35joint_limits: List[JointLimit] field(default_factorylist)def __post_init__(self):if not self.joint_limits:self.joint_limits [JointLimit(J1, math.radians(-170),math.radians(170)),JointLimit(J2, math.radians(-120),math.radians(120)),]for a in [self.a1, self.a2]:if a 0:raise ValueError(Link lengths must be positive)dataclassclass TrajectoryParams:Joint-space trajectory parameters with limit checkingq_start: Tuple[float, float]q_target: Tuple[float, float]n_steps: int 50_config: RobotConfig field(defaultNone, reprFalse)def __post_init__(self):for q in list(self.q_start) list(self.q_target):if not math.isfinite(q):raise ValueError(Joint angles must be finite)if self.n_steps 2:raise ValueError(n_steps must be 2)# Limit check on start and targetif self._config is not None:limits self._config.joint_limitsfor i, (q_val, lim) in enumerate([(self.q_start[0], limits[0]),(self.q_start[1], limits[1]),(self.q_target[0], limits[0]),(self.q_target[1], limits[1])]):if not (lim.lower q_val lim.upper):raise ValueError(fq{_start if i 2 else _target}f[{i % 2}] {math.degrees(q_val):.1f}deg foutside {lim.joint_name} limits f[{(math.degrees(lim.lower)):.0f}deg, f{(math.degrees(lim.upper)):.0f}deg])robot/kinematics.py — 正运动学Forward kinematics for 2-DOF planar armAnalytical solution for speedimport numpy as npfrom robot.models import RobotConfigclass Planar2RKinematics:def __init__(self, config: RobotConfig):self.config configdef fk_single(self, q1, q2):Single forward kinematicsReturns: np.array([x, y])Why array: consistent API with batch method and cross-articlea1, a2 self.config.a1, self.config.a2x a1 * np.cos(q1) a2 * np.cos(q1 q2)y a1 * np.sin(q1) a2 * np.sin(q1 q2)return np.array([x, y], dtypefloat)def fk_batch(self, q1_array, q2_array):Vectorized forward kinematics for trajectory pointsReturns: (x_array, y_array)a1, a2 self.config.a1, self.config.a2x a1 * np.cos(q1_array) a2 * np.cos(q1_array q2_array)y a1 * np.sin(q1_array) a2 * np.sin(q1_array q2_array)return x, yrobot/trajectory.py — 轨迹生成Joint-space linear interpolation trajectory generatorSamples intermediate joint angles and computes end-effector positionsIncludes joint limit checking at every interpolated pointimport numpy as npfrom robot.models import RobotConfig, TrajectoryParamsfrom robot.kinematics import Planar2RKinematicsclass JointSpaceTrajectory:def __init__(self, config: RobotConfig,params: TrajectoryParams):self.config configself.params paramsself.kin Planar2RKinematics(config)def interpolate(self):Linear interpolation in joint spaceReturns: q1_traj, q2_traj (arrays of length n_steps1)Why n_steps1: includes both start and end pointsq1_s, q2_s self.params.q_startq1_t, q2_t self.params.q_targetn self.params.n_stepst np.linspace(0.0, 1.0, n 1)q1_traj q1_s t * (q1_t - q1_s)q2_traj q2_s t * (q2_t - q2_s)# Per-point limit checklimits self.config.joint_limitsfor i in range(len(q1_traj)):if not (limits[0].lower q1_traj[i] limits[0].upper):raise ValueError(fq1 at step {i} ({np.degrees(q1_traj[i]):.1f}deg) fexceeds J1 limit)if not (limits[1].lower q2_traj[i] limits[1].upper):raise ValueError(fq2 at step {i} ({np.degrees(q2_traj[i]):.1f}deg) fexceeds J2 limit)return q1_traj, q2_trajdef compute_trajectory_points(self):Full pipeline: interpolate - FK - collect pointsReturns: q1_traj, q2_traj, x_traj, y_trajq1_traj, q2_traj self.interpolate()x_traj, y_traj self.kin.fk_batch(q1_traj, q2_traj)return q1_traj, q2_traj, x_traj, y_trajdef compute_end_effector_velocity(self, x, y):Compute end-effector linear velocity using central differenceWhy: to demonstrate that constant joint velocity does NOTproduce constant Cartesian velocityn len(x)v np.zeros(n)# Central difference for interior pointsfor i in range(1, n - 1):dx_dt (x[i 1] - x[i - 1]) / 2.0dy_dt (y[i 1] - y[i - 1]) / 2.0v[i] np.sqrt(dx_dt**2 dy_dt**2)# Forward/backward difference for endpointsif n 2:dx_fwd x[1] - x[0]dy_fwd y[1] - y[0]v[0] np.sqrt(dx_fwd**2 dy_fwd**2)dx_bwd x[-1] - x[-2]dy_bwd y[-1] - y[-2]v[-1] np.sqrt(dx_bwd**2 dy_bwd**2)return vdef compute_trajectory_length(self, x_traj, y_traj):Estimate actual path length by summing segment lengthsWhy: linear interpolation in joint space does NOT producestraight line in Cartesian space, so path length Euclidean distdx np.diff(x_traj)dy np.diff(y_traj)segment_lengths np.sqrt(dx**2 dy**2)return float(np.sum(segment_lengths))def evaluate(self):End-to-end evaluationReturns dict with all trajectory dataq1, q2, x, y self.compute_trajectory_points()path_length self.compute_trajectory_length(x, y)euclidean_dist float(np.sqrt((x[-1] - x[0])**2 (y[-1] - y[0])**2))velocity self.compute_end_effector_velocity(x, y)return {q1: q1,q2: q2,x: x,y: y,path_length: path_length,euclidean_dist: euclidean_dist,velocity: velocity,}robot/visualize.py — 可视化Trajectory visualization: 4 charts1. End-effector Cartesian trajectory (X-Y plane)2. Joint angle profiles over time3. End-effector linear velocity profile4. Trajectory overlaid on workspace boundary arcsimport 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 TrajectoryVisualizer:def __init__(self, results_dirresults):self.results_dir results_diros.makedirs(results_dir, exist_okTrue)def plot_cartesian_trajectory(self, x, y):1. End-effector path in X-Y planefig, ax plt.subplots(figsize(8, 8))ax.plot(x, y, b.-, linewidth1.5, markersize3,labelEnd-Effector Trajectory (joint-space linear))ax.plot(x[0], y[0], go, markersize10, labelStart, zorder5)ax.plot(x[-1], y[-1], ro, markersize10, labelTarget, zorder5)# Annotate key pointsfor idx in [0, len(x)//4, len(x)//2, 3*len(x)//4, -1]:ax.annotate(f{idx}, (x[idx], y[idx]),fontsize7, hacenter, vabottom)ax.set_xlabel(X (m))ax.set_ylabel(Y (m))ax.set_title(End-Effector Cartesian Trajectory (Joint-Space Linear Interpolation))ax.legend()ax.grid(True, alpha0.3)ax.set_aspect(equal, adjustablebox)plt.tight_layout()path os.path.join(self.results_dir,01_cartesian_trajectory.png)plt.savefig(path, dpi150)plt.close()return pathdef plot_joint_profiles(self, q1, q2):2. Joint angle profiles over interpolation parameter tfig, (ax1, ax2) plt.subplots(2, 1, figsize(10, 6), sharexTrue)t np.linspace(0, 1, len(q1))ax1.plot(t, np.degrees(q1), b.-, linewidth1.5)ax1.set_ylabel(J1 Angle (deg))ax1.set_title(Joint 1 Angle Profile (Linear Interpolation))ax1.grid(True, alpha0.3)ax2.plot(t, np.degrees(q2), r.-, linewidth1.5)ax2.set_xlabel(Interpolation Parameter t (0→1))ax2.set_ylabel(J2 Angle (deg))ax2.set_title(Joint 2 Angle Profile (Linear Interpolation))ax2.grid(True, alpha0.3)plt.tight_layout()path os.path.join(self.results_dir,02_joint_angle_profiles.png)plt.savefig(path, dpi150)plt.close()return pathdef plot_velocity_profile(self, velocity):3. End-effector linear velocity over timefig, ax plt.subplots(figsize(10, 5))t np.linspace(0, 1, len(velocity))ax.plot(t, velocity, m.-, linewidth1.5, markersize3)ax.set_xlabel(Interpolation Parameter t (0→1))ax.set_ylabel(End-Effector Linear Velocity (m/s, arbitrary time scale))ax.set_title(End-Effector Linear Velocity (Constant Joint Velocity → Non-Constant Cartesian Velocity))ax.grid(True, alpha0.3)plt.tight_layout()path os.path.join(self.results_dir,03_velocity_profile.png)plt.savefig(path, dpi150)plt.close()return pathdef plot_workspace_with_trajectory(self, x, y,a10.40, a20.35):4. Trajectory overlaid on workspace boundary arcsfig, ax plt.subplots(figsize(8, 8))theta np.linspace(0, 2*np.pi, 200)ax.plot((a1a2)*np.cos(theta), (a1a2)*np.sin(theta),k:, linewidth1, labelMax Reach Boundary)r_inner abs(a1 - a2)ax.plot(r_inner*np.cos(theta), r_inner*np.sin(theta),k:, linewidth1, labelMin Reach Boundary)ax.plot(x, y, b.-, linewidth1.5, markersize3,labelPlanned Trajectory)ax.plot(0, 0, ks, markersize8, labelBase (0,0))ax.plot(x[0], y[0], go, markersize10, labelStart)ax.plot(x[-1], y[-1], ro, markersize10, labelTarget)ax.set_xlabel(X (m))ax.set_ylabel(Y (m))ax.set_title(Trajectory Overlaid on Workspace Boundaries)ax.legend(fontsize8)ax.grid(True, alpha0.3)ax.set_aspect(equal, adjustablebox)plt.tight_layout()path os.path.join(self.results_dir,04_workspace_with_trajectory.png)plt.savefig(path, dpi150)plt.close()return path5. 可视化要求序号 图表文件 服务结论①01_cartesian_trajectory.png 末端在笛卡尔空间的真实运动轨迹可直观确认轨迹形状弧线叠加非直线②02_joint_angle_profiles.png 关节1和关节2的角度随时间线性变化曲线验证插值正确性和限位合规性③03_velocity_profile.png 末端线速度曲线证明恒定角速度→非恒定笛卡尔速度支撑第7节速度不均匀论点④04_workspace_with_trajectory.png 轨迹叠加在工作空间边界上检查是否超出可达范围6. README使用说明 核心知识点卡片快速开始pip install -r requirements.txtpython -m pytest tests/test_trajectory_planner.py -vpython run_trajectory_planner.py数据格式说明# 修改起始/目标关节角编辑 TrajectoryParams.q_start, q_target# 修改插值步数编辑 TrajectoryParams.n_steps# 修改臂长编辑 RobotConfig.a1, a2参数 修改位置 说明起始关节角TrajectoryParams.q_start rad, (q1, q2)目标关节角TrajectoryParams.q_target rad, (q1, q2)插值步数TrajectoryParams.n_steps 无量纲L1臂长RobotConfig.a1 mL2臂长RobotConfig.a2 m 知识点卡片关节空间轨迹规划对应课本第6章6.1节- 关节空间线性插值q(t) q_start t·(q_target - q_start)t∈[0,1]- 非线性映射关节空间线性 ≠ 笛卡尔空间直线因为正运动学含三角函数末端轨迹是两段圆弧运动的矢量和- 速度不均匀恒定关节角速度经非线性映射后末端线速度非恒定搬运中可能导致负载晃动- 轨迹长度折线累加距离 欧氏距离比值量化弯曲程度- 工程铁律需要末端走直线时必须做笛卡尔空间规划逆运动学而非关节空间插值7. 总结与工程师思考工业落地难处一、关节空间插值导致末端路径不可控。 本篇演示了关节空间线性插值走出的弧线叠加轨迹。工业现场中如果末端需要严格沿直线移动如激光焊接、密封涂胶弧线轨迹会导致加工质量不合格。解决方案是笛卡尔空间直线规划但这需要实时逆运动学求解对控制器算力要求更高。二、末端线速度不均匀。 关节空间线性插值只保证角速度恒定末端在笛卡尔空间的速度是变化的。图3清晰展示了速度波动——在关节角变化率恒定的情况下末端线速度在轨迹中段出现峰值。实际搬运中速度突变会导致负载晃动甚至脱落。需要加入速度/加速度平滑如梯形速度规划、S曲线。三、关节限位必须逐点校验。 即使起始和目标角在限位内中间插值点也可能越限例如经过奇异构型时。本篇在interpolate()中逐点检查确保生成轨迹物理可行。但真实控制器还需要考虑速度/加速度限制不是简单的位置限位。工程师心得小赵看着终端输出的数字起点位置(0.7030, 0.1751) m——0.40×cos(0°)0.35×cos(30°)0.7031 m0.35×sin(30°)0.1750 m对上了。终点(0.5381, 0.4370) m欧氏距离0.3095 m实际路径长度0.3360 m比值1.086——多了8.6%。老郑指着图3的线速度曲线你看速度不是平的。关节匀速转末端速度却忽快忽慢。如果搬运的是易碎品这个速度波动就可能把东西甩出去。好关节空间插值只适合精度要求不高的搬运。下一步该做逆运动学了有了逆解才能做笛卡尔空间直线规划。对。下一篇就写带关节限位的逆解。8. 写作与风格限制本篇全部代码为课程离线仿真运行在普通PC上用于关节空间轨迹规划与末端轨迹可视化。代码中不包含任何真实机器人控制器实时运算或安全逻辑。 完全去营销中立客观。拒绝论文式生硬开头保持故事叙事感。全部物理量标注完整单位。9. 文末附录环境配置pip install numpy1.24.0 matplotlib3.7.0requirements.txtnumpy1.24.0matplotlib3.7.0运行说明cd trajectory_plannerpip install -r requirements.txtpython -m pytest tests/ -vpython run_trajectory_planner.py参数修改指南想改什么 改哪里 示例起始角TrajectoryParams.q_start 改(0,0.524)→(0,0)目标角TrajectoryParams.q_target 改(1.047,-0.785)→(1.57,0)插值步数TrajectoryParams.n_steps 改50→100L1臂长RobotConfig.a1 改0.40→0.50L2臂长RobotConfig.a2 改0.35→0.30输出文件清单文件 含义results/01_cartesian_trajectory.png 末端笛卡尔空间轨迹results/02_joint_angle_profiles.png 关节角随时间变化曲线results/03_velocity_profile.png 末端线速度曲线results/04_workspace_with_trajectory.png 轨迹叠加工作空间边界output/trajectory_points.csv 轨迹点坐标数据仿真结果摘要真实运行数据从终端输出复制统计项 数值 数据来源机器人模型 2-DOF平面搬运臂 (2R) 配置连杆总长 0.75 m a1a2起始关节角 (0.000 rad, 0.524 rad) 内置目标关节角 (1.047 rad, -0.785 rad) 内置插值步数 50 内置起点位置 (0.7030, 0.1751) m 仿真输出终点位置 (0.5381, 0.4370) m 仿真输出欧氏距离 0.3095 m 仿真输出实际路径长度 0.3360 m 仿真输出路径长度/欧氏距离 1.086 仿真输出测试通过 3/3 pytest正文完。以下为run_trajectory_planner.py 完整主入口 tests/test_trajectory_planner.py 完整测试文件。run_trajectory_planner.pyJoint-Space Trajectory Planner - Main EntryRun: python run_trajectory_planner.pyimport osimport csvimport numpy as npfrom robot.models import RobotConfig, TrajectoryParamsfrom robot.trajectory import JointSpaceTrajectoryfrom robot.visualize import TrajectoryVisualizerdef main():print( * 65)print(Offline Sim: Transfer Robot - Joint Space Trajectory Planning)print( * 65)# 1. Initconfig RobotConfig()params TrajectoryParams(q_start(0.0, 0.524), # (0 deg, 30 deg)q_target(1.047, -0.785), # (60 deg, -45 deg)n_steps50)# Attach config for limit checking in TrajectoryParamsparams._config configplanner JointSpaceTrajectory(config, params)viz TrajectoryVisualizer(results_dirresults)print(f\n--- Robot Config ---)print(f a1 {config.a1:.2f} m, a2 {config.a2:.2f} m)print(f q_start ({params.q_start[0]:.3f}, f{params.q_start[1]:.3f}) rad)print(f q_target ({params.q_target[0]:.3f}, f{params.q_target[1]:.3f}) rad)print(f n_steps {params.n_steps})# 2. Evaluate trajectoryprint(\n--- Computing Trajectory ---)result planner.evaluate()q1, q2, x, y result[q1], result[q2], result[x], result[y]velocity result[velocity]print(f\n--- Results (Real Computation) ---)print(f Start position: ({x[0]:.4f}, {y[0]:.4f}) m)print(f End position: ({x[-1]:.4f}, {y[-1]:.4f}) m)print(f Euclidean distance: {result[euclidean_dist]:.4f} m)print(f Actual path length: {result[path_length]:.4f} m)print(f Path length / Euclidean: f{result[path_length]/result[euclidean_dist]:.4f})print(f Max利用AI解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛
