《Python机器人基础原理》第二十六篇机械臂两点转运——给定起点终点关节角度生成多项式轨迹输出速度时间变化曲线1. 实际工业应用场景周五上午某新能源电池模组产线的电芯搬运工位。一台3-DOF垂直关节机械臂基座偏航θ₁ 大臂俯仰θ₂ 小臂俯仰θ₃负责将电芯从托盘抓取后转运到输送线上。机器人构型参数大臂长450 mm、质量3.2 kg质心距基座0.21 m小臂长350 mm、质量2.0 kg质心距肘部0.16 m末端真空吸盘及夹具0.8 kg轴向偏置15 mm。关节限位θ₁±170°、最大角速度2.5 rad/s、最大角加速度5.0 rad/s²θ₂±140°、最大角速度2.0 rad/s、最大角加速度4.0 rad/s²θ₃±180°、最大角速度3.0 rad/s、最大角加速度6.0 rad/s²。总运动时间要求从取料到放料不超过3.0 s。小赵接到任务给这条转运轨迹写一段离线程序。起点关节角θ_start [15°, -50°, 30°]终点关节角θ_end [75°, -110°, -20°]。他第一反应是线性插值呗每个关节从起点匀速走到终点时间3秒搞定。小李路过看了一眼他的屏幕你打算用线性插值对啊每个关节匀速转过去3秒到。简单粗暴。你算过θ₂的角速度吗从-50°到-110°60°的跨度3秒走完角速度0.35 rad/s。看着没问题对远小于限位2.0 rad/s。那加速度呢你从静止启动瞬间跳到0.35 rad/s加速度是无穷大。伺服驱动器会怎么处理小赵愣了一下……会触发S曲线滤波实际走出来不是匀速。而且你三个关节同时匀速但每个关节的跨度不一样——不对这次碰巧一样都是60°左右。但即便跨度一样你各关节独立匀速走末端走的是弧线真空吸盘到放料点可能对不准。那怎么办用多项式轨迹规划。给每个关节生成一个五次多项式指定起点和终点的位置、速度、加速度边界条件。这样启动和停止时速度平滑归零末端轨迹也可预测。本篇解决的问题给定起点和终点的关节角度为每个关节独立生成五次多项式轨迹指定起止位置、起止速度0、起止加速度0输出各关节角度、角速度、角加速度随时间的变化曲线校验是否满足速度/加速度限位。⚠️ 声明本篇全部代码为课程离线仿真运行在普通PC上用于离线轨迹规划校验。不可直接用于真实机器人控制器、伺服驱动器或产线设备。真实工业现场需结合实时插补、前馈控制、软限位保护。代码作者不对直接使用本代码于真实设备造成的任何后果负责。2. 痛点分析课程模块 本篇对应的工程痛点第6章 轨迹规划与控制 → 6.1 轨迹规划基本概念 线性插值导致加速度无穷大→伺服驱动器被迫滤波→实际轨迹偏离预期第6章 轨迹规划与控制 → 6.3 多项式轨迹规划 两点转运需要平滑的速度/加速度曲线→五次多项式满足起止位置/速度/加速度共6个边界条件第6章 轨迹规划与控制 → 6.2 速度规划 各关节独立轨迹需校验速度/加速度是否超出硬件限位本篇代码解决 L2 离线轨迹规划校验层。输入起点/终点关节角度、总运动时间、采样频率。为每个关节独立求解五次多项式系数逐点计算位置/速度/加速度与限位比对。输出关节位置/速度/加速度曲线、限位校验报告。代码不是设备实时控制器不做在线插补。它做的事情是在离线阶段验证多项式轨迹是否物理可行。不覆盖末端直线插补笛卡尔空间规划、梯形速度曲线T型曲线、S曲线正弦加速度、高阶样条连续、动力学约束优化。3. 核心逻辑讲解物理直觉多项式轨迹规划就像开车从A点到B点。线性插值相当于一脚油门踩到底然后急刹车——理论上能到但乘客会吐车会散。多项式轨迹相当于平滑地加速、匀速巡航、再平滑地减速停车。五次多项式额外保证了加速度也是连续的——这意味着没有猛踩油门或猛踩刹车的瞬间伺服电机不会承受冲击扭矩。为什么是五次因为每个关节需要指定6个边界条件起点位置、起点速度、起点加速度、终点位置、终点速度、终点加速度。6个未知数需要6个方程所以是最低五次的多项式a₀ a₁t a₂t² a₃t³ a₄t⁴ a₅t⁵共6个系数。解析峰值公式对于零边界速度/加速度条件的五次多项式速度和加速度的峰值有闭式解可通过对速度/加速度函数求导令为零得到- 速度峰值v_peak 15·Δ/(8·T)出现在 t T/2 处- 加速度峰值a_peak 5.7735·Δ/T²出现在 t ≈ 0.211·T 和 t ≈ 0.789·T 处其中Δ为关节角度变化量终点减起点取绝对值T为总运动时间。这两个公式是轨迹设计的解析依据——在写代码之前就能预判是否超限。本篇代码中的逐点采样取max只是数值验证手段解析公式才是设计阶段的快速校验工具。代入本篇数据验证θ₂的Δ |-110° - (-50°)| 60° 1.047 radT 3.0 s。v_peak 15×1.047/(8×3) 0.654 rad/sa_peak 5.7735×1.047/9 0.671 rad/s²。两者均远小于θ₂的限位2.0 rad/s, 4.0 rad/s²说明3.0 s是安全节拍。业务逻辑 → 代码映射输入起点关节角 q_start[3]、终点关节角 q_end[3]、总时间 T、采样数 N↓对每个关节 j 0,1,2构建边界条件方程组q_j(0) q_start[j] (位置)q_j(0) 0 (速度)q_j(0) 0 (加速度)q_j(T) q_end[j] (位置)q_j(T) 0 (速度)q_j(T) 0 (加速度)前三个系数直接确定a₀q_start, a₁0, a₂0后三个系数联立求解M·[a₃,a₄,a₅]ᵀ bM [[T³,T⁴,T⁵],[3T²,4T³,5T⁴],[6T,12T²,20T³]]b [q_end - q_start, 0, 0]ᵀ逐点 t_i i·T/Nq(t_i), q(t_i), q(t_i) 由多项式求值↓校验 |q_j(t_i)| ≤ v_max[j] 和 |q_j(t_i)| ≤ a_max[j]↓输出位置/速度/加速度曲线、超限统计、4张图表关键公式文字翻译五次多项式q(t) a₀ a₁t a₂t² a₃t³ a₄t⁴ a₅t⁵。a₀由起点位置决定。a₁由起点速度决定为零。a₂由起点加速度决定为零。剩下a₃、a₄、a₅由终点位置、速度、加速度三个条件联立求解。边界条件矩阵将tT代入位置、速度、加速度公式得到3个方程写成矩阵形式 M·[a₃,a₄,a₅]ᵀ b。M是3×3矩阵对角线元素为[T³,T⁴,T⁵]、[3T²,4T³,5T⁴]、[6T,12T²,20T³]。直接求逆解出系数。逐点采样取max代码中通过N200个采样点逐点计算速度和加速度取绝对值最大者作为峰值。对于单调轨迹本篇各关节角度单调变化峰值一定出现在区间内部采样足够密时数值峰值接近解析值。4. OOP代码实现技术栈严格限定numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。项目结构树polynomial_trajectory_sim/├── run_polynomial_trajectory_sim.py├── requirements.txt├── robot/│ ├── __init__.py│ ├── models.py│ ├── trajectory_generator.py│ └── visualize_trajectory.py├── tests/│ ├── __init__.py│ └── test_polynomial_trajectory_sim.py└── results/核心源码robot/models.py — 数据模型机器人数据模型关节参数、限位、轨迹边界条件from dataclasses import dataclass, fieldfrom typing import Listdataclassclass JointLimit:单关节限位结构体joint_name: strmin_angle: float # 最小角度 (rad)max_angle: float # 最大角度 (rad)max_velocity: float # 最大角速度 (rad/s)max_acceleration: float # 最大角加速度 (rad/s²)dataclassclass JointLimitSet:关节限位集合limits: List[JointLimit] field(default_factorylist)def check_all_positions(self, angles: List[float]) - tuple:校验一组关节角度是否全部在位置限位内返回 (bool通过标志, list超限索引列表)注单调插值轨迹通常不会超限位此校验对非单调多段轨迹更有意义violations []for i, angle in enumerate(angles):if i len(self.limits):if angle self.limits[i].min_angle or angle self.limits[i].max_angle:violations.append(i)return len(violations) 0, violationsdef check_all_velocities(self, velocities: List[float]) - tuple:校验一组关节角速度是否全部在速度限位内返回 (bool通过标志, list超限索引列表)violations []for i, vel in enumerate(velocities):if i len(self.limits):if abs(vel) self.limits[i].max_velocity:violations.append(i)return len(violations) 0, violationsdef check_all_accelerations(self, accelerations: List[float]) - tuple:校验一组关节角加速度是否全部在加速度限位内返回 (bool通过标志, list超限索引列表)violations []for i, acc in enumerate(accelerations):if i len(self.limits):if abs(acc) self.limits[i].max_acceleration:violations.append(i)return len(violations) 0, violationsdataclassclass TrajectoryBoundary:单关节轨迹边界条件start_position: float # 起点位置 (rad)end_position: float # 终点位置 (rad)start_velocity: float 0.0 # 起点速度 (rad/s)end_velocity: float 0.0 # 终点速度 (rad/s)start_acceleration: float 0.0 # 起点加速度 (rad/s²)end_acceleration: float 0.0 # 终点加速度 (rad/s²)robot/trajectory_generator.py — 五次多项式轨迹生成五次多项式轨迹生成器每个关节独立求解q(t) a0 a1*t a2*t^2 a3*t^3 a4*t^4 a5*t^5边界条件起止位置、起止速度、起止加速度默认速度加速度均为0import numpy as npfrom typing import List, Tuple, Dictfrom robot.models import JointLimitSet, TrajectoryBoundaryclass QuinticPolynomialTrajectory:单关节五次多项式轨迹系数向量 coeffs [a0, a1, a2, a3, a4, a5]def __init__(self, boundary: TrajectoryBoundary, total_time: float):if total_time 0:raise ValueError(f总时间必须大于0: {total_time})self.T total_timeself.boundary boundaryself.coeffs self._solve_coefficients()def _solve_coefficients(self) - np.ndarray:求解五次多项式系数边界条件q(0) q0 → a0 q0q(0) v0 → a1 v0q(0) a0_acc → 2*a2 a0_acc → a2 a0_acc/2q(T) qTq(T) vTq(T) aT_acc后三个条件联立求解 a3, a4, a5q0 self.boundary.start_positionqT self.boundary.end_positionv0 self.boundary.start_velocityvT self.boundary.end_velocitya0_acc self.boundary.start_accelerationaT_acc self.boundary.end_acceleration# 前三个系数直接由起点边界条件确定a0 q0a1 v0a2 a0_acc / 2.0# 构建线性方程组 M * [a3, a4, a5]^T bT self.TM np.array([[T**3, T**4, T**5],[3*T**2, 4*T**3, 5*T**4],[6*T, 12*T**2, 20*T**3]])# 右侧向量减去前三个系数已知的贡献b np.array([qT - (a0 a1*T a2*T**2),vT - (a1 2*a2*T),aT_acc - (2*a2)])# 求解 a3, a4, a5try:a3_a4_a5 np.linalg.solve(M, b)except np.linalg.LinAlgError:raise ValueError(f轨迹求解失败系数矩阵奇异总时间T{T}可能过小)coeffs np.array([a0, a1, a2, a3_a4_a5[0], a3_a4_a5[1], a3_a4_a5[2]])return coeffsdef evaluate(self, t: float) - Tuple[float, float, float]:计算指定时刻的位置、速度、加速度if t 0:t 0.0if t self.T:t self.Ta self.coeffs# 位置pos a[0] a[1]*t a[2]*t**2 a[3]*t**3 a[4]*t**4 a[5]*t**5# 速度vel a[1] 2*a[2]*t 3*a[3]*t**2 4*a[4]*t**3 5*a[5]*t**4# 加速度acc 2*a[2] 6*a[3]*t 12*a[4]*t**2 20*a[5]*t**3return pos, vel, accdef evaluate_trajectory(self, time_points: np.ndarray) - Dict[str, np.ndarray]:批量评估轨迹n len(time_points)positions np.zeros(n)velocities np.zeros(n)accelerations np.zeros(n)for i, t in enumerate(time_points):positions[i], velocities[i], accelerations[i] self.evaluate(t)return {time: time_points,positions: positions,velocities: velocities,accelerations: accelerations}class MultiJointTrajectoryGenerator:多关节轨迹生成器为每个关节独立生成五次多项式轨迹def __init__(self, joint_limits: JointLimitSet):self.limits joint_limitsself.trajectories: List[QuinticPolynomialTrajectory] []def generate_trajectories(self, start_positions: List[float],end_positions: List[float],total_time: float) - List[QuinticPolynomialTrajectory]:为所有关节生成轨迹n_joints len(start_positions)if len(end_positions) ! n_joints:raise ValueError(起点和终点关节数必须一致)if n_joints len(self.limits.limits):raise ValueError(关节数超过限位定义数量)self.trajectories []for j in range(n_joints):boundary TrajectoryBoundary(start_positionstart_positions[j],end_positionend_positions[j]# 速度加速度默认全0)traj QuinticPolynomialTrajectory(boundary, total_time)self.trajectories.append(traj)return self.trajectoriesdef evaluate_all(self, time_points: np.ndarray) - Dict:评估所有关节的轨迹n_joints len(self.trajectories)n_points len(time_points)all_positions np.zeros((n_points, n_joints))all_velocities np.zeros((n_points, n_joints))all_accelerations np.zeros((n_points, n_joints))for j in range(n_joints):result self.trajectories[j].evaluate_trajectory(time_points)all_positions[:, j] result[positions]all_velocities[:, j] result[velocities]all_accelerations[:, j] result[accelerations]# 逐点校验限位velocity_violations []acceleration_violations []position_violations []for i in range(n_points):pos_list all_positions[i, :].tolist()vel_list all_velocities[i, :].tolist()acc_list all_accelerations[i, :].tolist()_, v_viol self.limits.check_all_velocities(vel_list)_, a_viol self.limits.check_all_accelerations(acc_list)_, p_viol self.limits.check_all_positions(pos_list)for j in v_viol:velocity_violations.append((i, j, vel_list[j],self.limits.limits[j].max_velocity))for j in a_viol:acceleration_violations.append((i, j, acc_list[j],self.limits.limits[j].max_acceleration))for j in p_viol:position_violations.append((i, j, pos_list[j],self.limits.limits[j].min_angle,self.limits.limits[j].max_angle))return {time: time_points,positions: all_positions,velocities: all_velocities,accelerations: all_accelerations,velocity_violations: velocity_violations,acceleration_violations: acceleration_violations,position_violations: position_violations,n_joints: n_joints,n_points: n_points}robot/visualize_trajectory.py — 可视化模块多项式轨迹规划可视化4张核心图表关节位置曲线、关节速度曲线、关节加速度曲线、速度/加速度峰值对比import numpy as npimport matplotlib.pyplot as pltplt.rcParams[font.sans-serif] [SimHei, DejaVu Sans]plt.rcParams[axes.unicode_minus] Falseclass TrajectoryVisualizer:def __init__(self, results_dirresults):self.results_dir results_dirimport osos.makedirs(results_dir, exist_okTrue)def plot_joint_positions(self, timestamps, positions):① 关节角度随时间变化曲线验证起止点是否正确到达单调插值轨迹不会超位置限位此图主要验证平滑性fig, ax plt.subplots(figsize(12, 5))joint_names [fθ_{i1} for i in range(positions.shape[1])]colors [steelblue, orange, green]for j in range(positions.shape[1]):ax.plot(timestamps, positions[:, j], -,colorcolors[j], linewidth1.5, labelf{joint_names[j]})ax.set_xlabel(Time (s))ax.set_ylabel(Joint Angle (rad))ax.set_title(Joint Position Profiles — Quintic Polynomial Trajectory)ax.legend(locupper right, fontsize8)ax.grid(True, alpha0.3)plt.tight_layout()path f{self.results_dir}/01_joint_position_curves.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_joint_velocities(self, timestamps, velocities, joint_limits):② 关节角速度随时间变化曲线核心输出直接读出速度峰值和超限区间fig, axes plt.subplots(3, 1, figsize(12, 9), sharexTrue)joint_names [θ₁ (Base Yaw), θ₂ (Elbow Pitch), θ₃ (Wrist Pitch)]colors [steelblue, orange, green]for j in range(3):ax axes[j]ax.plot(timestamps, velocities[:, j], -,colorcolors[j], linewidth1.2, labelfActual {joint_names[j]})limit joint_limits.limits[j].max_velocityax.axhline(ylimit, colorred, linestyle--, linewidth1.0,labelfMax ({limit} rad/s))ax.axhline(y-limit, colorred, linestyle--, linewidth1.0,labelfMin (-{limit} rad/s))ax.axhline(y0, colorblack, linestyle-, linewidth0.5, alpha0.5)ax.set_ylabel(Angular Velocity (rad/s))ax.legend(locupper right, fontsize7)ax.grid(True, alpha0.3)ax.set_title(f{joint_names[j]} Velocity Profile, fontsize10)axes[2].set_xlabel(Time (s))plt.tight_layout()path f{self.results_dir}/02_joint_velocity_curves.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_joint_accelerations(self, timestamps, accelerations, joint_limits):③ 关节角加速度随时间变化曲线验证加速度是否超限fig, axes plt.subplots(3, 1, figsize(12, 9), sharexTrue)joint_names [θ₁ (Base Yaw), θ₂ (Elbow Pitch), θ₃ (Wrist Pitch)]colors [steelblue, orange, green]for j in range(3):ax axes[j]ax.plot(timestamps, accelerations[:, j], -,colorcolors[j], linewidth1.2, labelfActual {joint_names[j]})limit joint_limits.limits[j].max_accelerationax.axhline(ylimit, colorred, linestyle--, linewidth1.0,labelfMax ({limit} rad/s²))ax.axhline(y-limit, colorred, linestyle--, linewidth1.0,labelfMin (-{limit} rad/s²))ax.axhline(y0, colorblack, linestyle-, linewidth0.5, alpha0.5)ax.set_ylabel(Angular Acceleration (rad/s²))ax.legend(locupper right, fontsize7)ax.grid(True, alpha0.3)ax.set_title(f{joint_names[j]} Acceleration Profile, fontsize10)axes[2].set_xlabel(Time (s))plt.tight_layout()path f{self.results_dir}/03_joint_acceleration_curves.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_peak_comparison(self, velocities, accelerations, joint_limits):④ 各关节速度/加速度峰值与限位对比柱状图一目了然哪些关节接近极限fig, (ax1, ax2) plt.subplots(1, 2, figsize(12, 5))joint_names [θ₁, θ₂, θ₃]# 速度峰值vel_peaks [np.max(np.abs(velocities[:, j])) for j in range(3)]vel_limits [joint_limits.limits[j].max_velocity for j in range(3)]x np.arange(3)width 0.35bars1 ax1.bar(x - width/2, vel_peaks, width, labelPeak Velocity,colorsteelblue, alpha0.7)bars2 ax1.bar(x width/2, vel_limits, width, labelVelocity Limit,colorred, alpha0.5)ax1.set_xlabel(Joint)ax1.set_ylabel(Angular Velocity (rad/s))ax1.set_title(Peak Velocity vs Limit)ax1.set_xticks(x)ax1.set_xticklabels(joint_names)ax1.legend(fontsize8)ax1.grid(True, alpha0.3, axisy)for bar, val in zip(bars1, vel_peaks):ax1.text(bar.get_x() bar.get_width()/2, bar.get_height() 0.02,f{val:.3f}, hacenter, vabottom, fontsize7)# 加速度峰值acc_peaks [np.max(np.abs(accelerations[:, j])) for j in range(3)]acc_limits [joint_limits.limits[j].max_acceleration for j in range(3)]bars3 ax2.bar(x - width/2, acc_peaks, width, labelPeak Acceleration,colororange, alpha0.7)bars4 ax2.bar(x width/2, acc_limits, width, labelAcceleration Limit,colorred, alpha0.5)ax2.set_xlabel(Joint)ax2.set_ylabel(Angular Acceleration (rad/s²))ax2.set_title(Peak Acceleration vs Limit)ax2.set_xticks(x)ax2.set_xticklabels(joint_names)ax2.legend(fontsize8)ax2.grid(True, alpha0.3, axisy)for bar, val in zip(bars3, acc_peaks):ax2.text(bar.get_x() bar.get_width()/2, bar.get_height() 0.02,f{val:.3f}, hacenter, vabottom, fontsize7)plt.tight_layout()path f{self.results_dir}/04_peak_comparison.pngplt.savefig(path, dpi150)plt.close()return path5. 可视化要求序号 图表文件 服务结论①01_joint_position_curves.png 关节角度随时间变化验证起止点正确到达、运动平滑无突变②02_joint_velocity_curves.png 关节角速度曲线限位线读出速度峰值和超限时间段③03_joint_acceleration_curves.png 关节角加速度曲线限位线验证加速度连续且未超限④04_peak_comparison.png 各关节速度/加速度峰值与限位对比柱状图一目了然安全裕度6. README使用说明 核心知识点卡片快速开始pip install -r requirements.txtpython -m pytest tests/test_polynomial_trajectory_sim.py -v # 3/3 测试通过python run_polynomial_trajectory_sim.py # 运行多项式轨迹仿真数据格式说明参数 修改位置 说明起点/终点关节角run_polynomial_trajectory_sim.py中q_start/q_end 弧度制(rad)总运动时间 同文件total_time 秒(s)改小会增大速度/加速度关节限位JointLimitSet初始化 匹配实际机器人参数采样数 同文件n_samples 默认200影响曲线平滑度 知识点卡片五次多项式轨迹规划对应课本第6章6.3节- 边界条件起止位置、速度、加速度共6个条件→唯一确定五次多项式- 解析峰值v_peak 15Δ/(8T)a_peak 5.7735Δ/T²零边界速度/加速度条件- 平滑性速度连续无冲击启停、加速度连续无突变扭矩- 局限性各关节独立规划不保证末端走直线校验必须同时查速度和加速度7. 总结与工程师思考工业落地难处一、各关节独立规划不保证末端路径直线度。 本篇每个关节独立生成五次多项式末端在笛卡尔空间走的是曲线。如果工艺要求末端严格走直线如激光焊接、涂胶独立关节规划会导致路径偏差。工业现场通常用笛卡尔空间直线插补逆运动学实时求解或先规划末端路径再映射到关节空间。二、总时间T的选择需要同时利用AI解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛
《Python机器人基础原理》第二十六篇机械臂两点转运——给定起点终点关节角度生成多项式轨迹输出速度时间变化曲线1. 实际工业应用场景周五上午某新能源电池模组产线的电芯搬运工位。一台3-DOF垂直关节机械臂基座偏航θ₁ 大臂俯仰θ₂ 小臂俯仰θ₃负责将电芯从托盘抓取后转运到输送线上。机器人构型参数大臂长450 mm、质量3.2 kg质心距基座0.21 m小臂长350 mm、质量2.0 kg质心距肘部0.16 m末端真空吸盘及夹具0.8 kg轴向偏置15 mm。关节限位θ₁±170°、最大角速度2.5 rad/s、最大角加速度5.0 rad/s²θ₂±140°、最大角速度2.0 rad/s、最大角加速度4.0 rad/s²θ₃±180°、最大角速度3.0 rad/s、最大角加速度6.0 rad/s²。总运动时间要求从取料到放料不超过3.0 s。小赵接到任务给这条转运轨迹写一段离线程序。起点关节角θ_start [15°, -50°, 30°]终点关节角θ_end [75°, -110°, -20°]。他第一反应是线性插值呗每个关节从起点匀速走到终点时间3秒搞定。小李路过看了一眼他的屏幕你打算用线性插值对啊每个关节匀速转过去3秒到。简单粗暴。你算过θ₂的角速度吗从-50°到-110°60°的跨度3秒走完角速度0.35 rad/s。看着没问题对远小于限位2.0 rad/s。那加速度呢你从静止启动瞬间跳到0.35 rad/s加速度是无穷大。伺服驱动器会怎么处理小赵愣了一下……会触发S曲线滤波实际走出来不是匀速。而且你三个关节同时匀速但每个关节的跨度不一样——不对这次碰巧一样都是60°左右。但即便跨度一样你各关节独立匀速走末端走的是弧线真空吸盘到放料点可能对不准。那怎么办用多项式轨迹规划。给每个关节生成一个五次多项式指定起点和终点的位置、速度、加速度边界条件。这样启动和停止时速度平滑归零末端轨迹也可预测。本篇解决的问题给定起点和终点的关节角度为每个关节独立生成五次多项式轨迹指定起止位置、起止速度0、起止加速度0输出各关节角度、角速度、角加速度随时间的变化曲线校验是否满足速度/加速度限位。⚠️ 声明本篇全部代码为课程离线仿真运行在普通PC上用于离线轨迹规划校验。不可直接用于真实机器人控制器、伺服驱动器或产线设备。真实工业现场需结合实时插补、前馈控制、软限位保护。代码作者不对直接使用本代码于真实设备造成的任何后果负责。2. 痛点分析课程模块 本篇对应的工程痛点第6章 轨迹规划与控制 → 6.1 轨迹规划基本概念 线性插值导致加速度无穷大→伺服驱动器被迫滤波→实际轨迹偏离预期第6章 轨迹规划与控制 → 6.3 多项式轨迹规划 两点转运需要平滑的速度/加速度曲线→五次多项式满足起止位置/速度/加速度共6个边界条件第6章 轨迹规划与控制 → 6.2 速度规划 各关节独立轨迹需校验速度/加速度是否超出硬件限位本篇代码解决 L2 离线轨迹规划校验层。输入起点/终点关节角度、总运动时间、采样频率。为每个关节独立求解五次多项式系数逐点计算位置/速度/加速度与限位比对。输出关节位置/速度/加速度曲线、限位校验报告。代码不是设备实时控制器不做在线插补。它做的事情是在离线阶段验证多项式轨迹是否物理可行。不覆盖末端直线插补笛卡尔空间规划、梯形速度曲线T型曲线、S曲线正弦加速度、高阶样条连续、动力学约束优化。3. 核心逻辑讲解物理直觉多项式轨迹规划就像开车从A点到B点。线性插值相当于一脚油门踩到底然后急刹车——理论上能到但乘客会吐车会散。多项式轨迹相当于平滑地加速、匀速巡航、再平滑地减速停车。五次多项式额外保证了加速度也是连续的——这意味着没有猛踩油门或猛踩刹车的瞬间伺服电机不会承受冲击扭矩。为什么是五次因为每个关节需要指定6个边界条件起点位置、起点速度、起点加速度、终点位置、终点速度、终点加速度。6个未知数需要6个方程所以是最低五次的多项式a₀ a₁t a₂t² a₃t³ a₄t⁴ a₅t⁵共6个系数。解析峰值公式对于零边界速度/加速度条件的五次多项式速度和加速度的峰值有闭式解可通过对速度/加速度函数求导令为零得到- 速度峰值v_peak 15·Δ/(8·T)出现在 t T/2 处- 加速度峰值a_peak 5.7735·Δ/T²出现在 t ≈ 0.211·T 和 t ≈ 0.789·T 处其中Δ为关节角度变化量终点减起点取绝对值T为总运动时间。这两个公式是轨迹设计的解析依据——在写代码之前就能预判是否超限。本篇代码中的逐点采样取max只是数值验证手段解析公式才是设计阶段的快速校验工具。代入本篇数据验证θ₂的Δ |-110° - (-50°)| 60° 1.047 radT 3.0 s。v_peak 15×1.047/(8×3) 0.654 rad/sa_peak 5.7735×1.047/9 0.671 rad/s²。两者均远小于θ₂的限位2.0 rad/s, 4.0 rad/s²说明3.0 s是安全节拍。业务逻辑 → 代码映射输入起点关节角 q_start[3]、终点关节角 q_end[3]、总时间 T、采样数 N↓对每个关节 j 0,1,2构建边界条件方程组q_j(0) q_start[j] (位置)q_j(0) 0 (速度)q_j(0) 0 (加速度)q_j(T) q_end[j] (位置)q_j(T) 0 (速度)q_j(T) 0 (加速度)前三个系数直接确定a₀q_start, a₁0, a₂0后三个系数联立求解M·[a₃,a₄,a₅]ᵀ bM [[T³,T⁴,T⁵],[3T²,4T³,5T⁴],[6T,12T²,20T³]]b [q_end - q_start, 0, 0]ᵀ逐点 t_i i·T/Nq(t_i), q(t_i), q(t_i) 由多项式求值↓校验 |q_j(t_i)| ≤ v_max[j] 和 |q_j(t_i)| ≤ a_max[j]↓输出位置/速度/加速度曲线、超限统计、4张图表关键公式文字翻译五次多项式q(t) a₀ a₁t a₂t² a₃t³ a₄t⁴ a₅t⁵。a₀由起点位置决定。a₁由起点速度决定为零。a₂由起点加速度决定为零。剩下a₃、a₄、a₅由终点位置、速度、加速度三个条件联立求解。边界条件矩阵将tT代入位置、速度、加速度公式得到3个方程写成矩阵形式 M·[a₃,a₄,a₅]ᵀ b。M是3×3矩阵对角线元素为[T³,T⁴,T⁵]、[3T²,4T³,5T⁴]、[6T,12T²,20T³]。直接求逆解出系数。逐点采样取max代码中通过N200个采样点逐点计算速度和加速度取绝对值最大者作为峰值。对于单调轨迹本篇各关节角度单调变化峰值一定出现在区间内部采样足够密时数值峰值接近解析值。4. OOP代码实现技术栈严格限定numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。项目结构树polynomial_trajectory_sim/├── run_polynomial_trajectory_sim.py├── requirements.txt├── robot/│ ├── __init__.py│ ├── models.py│ ├── trajectory_generator.py│ └── visualize_trajectory.py├── tests/│ ├── __init__.py│ └── test_polynomial_trajectory_sim.py└── results/核心源码robot/models.py — 数据模型机器人数据模型关节参数、限位、轨迹边界条件from dataclasses import dataclass, fieldfrom typing import Listdataclassclass JointLimit:单关节限位结构体joint_name: strmin_angle: float # 最小角度 (rad)max_angle: float # 最大角度 (rad)max_velocity: float # 最大角速度 (rad/s)max_acceleration: float # 最大角加速度 (rad/s²)dataclassclass JointLimitSet:关节限位集合limits: List[JointLimit] field(default_factorylist)def check_all_positions(self, angles: List[float]) - tuple:校验一组关节角度是否全部在位置限位内返回 (bool通过标志, list超限索引列表)注单调插值轨迹通常不会超限位此校验对非单调多段轨迹更有意义violations []for i, angle in enumerate(angles):if i len(self.limits):if angle self.limits[i].min_angle or angle self.limits[i].max_angle:violations.append(i)return len(violations) 0, violationsdef check_all_velocities(self, velocities: List[float]) - tuple:校验一组关节角速度是否全部在速度限位内返回 (bool通过标志, list超限索引列表)violations []for i, vel in enumerate(velocities):if i len(self.limits):if abs(vel) self.limits[i].max_velocity:violations.append(i)return len(violations) 0, violationsdef check_all_accelerations(self, accelerations: List[float]) - tuple:校验一组关节角加速度是否全部在加速度限位内返回 (bool通过标志, list超限索引列表)violations []for i, acc in enumerate(accelerations):if i len(self.limits):if abs(acc) self.limits[i].max_acceleration:violations.append(i)return len(violations) 0, violationsdataclassclass TrajectoryBoundary:单关节轨迹边界条件start_position: float # 起点位置 (rad)end_position: float # 终点位置 (rad)start_velocity: float 0.0 # 起点速度 (rad/s)end_velocity: float 0.0 # 终点速度 (rad/s)start_acceleration: float 0.0 # 起点加速度 (rad/s²)end_acceleration: float 0.0 # 终点加速度 (rad/s²)robot/trajectory_generator.py — 五次多项式轨迹生成五次多项式轨迹生成器每个关节独立求解q(t) a0 a1*t a2*t^2 a3*t^3 a4*t^4 a5*t^5边界条件起止位置、起止速度、起止加速度默认速度加速度均为0import numpy as npfrom typing import List, Tuple, Dictfrom robot.models import JointLimitSet, TrajectoryBoundaryclass QuinticPolynomialTrajectory:单关节五次多项式轨迹系数向量 coeffs [a0, a1, a2, a3, a4, a5]def __init__(self, boundary: TrajectoryBoundary, total_time: float):if total_time 0:raise ValueError(f总时间必须大于0: {total_time})self.T total_timeself.boundary boundaryself.coeffs self._solve_coefficients()def _solve_coefficients(self) - np.ndarray:求解五次多项式系数边界条件q(0) q0 → a0 q0q(0) v0 → a1 v0q(0) a0_acc → 2*a2 a0_acc → a2 a0_acc/2q(T) qTq(T) vTq(T) aT_acc后三个条件联立求解 a3, a4, a5q0 self.boundary.start_positionqT self.boundary.end_positionv0 self.boundary.start_velocityvT self.boundary.end_velocitya0_acc self.boundary.start_accelerationaT_acc self.boundary.end_acceleration# 前三个系数直接由起点边界条件确定a0 q0a1 v0a2 a0_acc / 2.0# 构建线性方程组 M * [a3, a4, a5]^T bT self.TM np.array([[T**3, T**4, T**5],[3*T**2, 4*T**3, 5*T**4],[6*T, 12*T**2, 20*T**3]])# 右侧向量减去前三个系数已知的贡献b np.array([qT - (a0 a1*T a2*T**2),vT - (a1 2*a2*T),aT_acc - (2*a2)])# 求解 a3, a4, a5try:a3_a4_a5 np.linalg.solve(M, b)except np.linalg.LinAlgError:raise ValueError(f轨迹求解失败系数矩阵奇异总时间T{T}可能过小)coeffs np.array([a0, a1, a2, a3_a4_a5[0], a3_a4_a5[1], a3_a4_a5[2]])return coeffsdef evaluate(self, t: float) - Tuple[float, float, float]:计算指定时刻的位置、速度、加速度if t 0:t 0.0if t self.T:t self.Ta self.coeffs# 位置pos a[0] a[1]*t a[2]*t**2 a[3]*t**3 a[4]*t**4 a[5]*t**5# 速度vel a[1] 2*a[2]*t 3*a[3]*t**2 4*a[4]*t**3 5*a[5]*t**4# 加速度acc 2*a[2] 6*a[3]*t 12*a[4]*t**2 20*a[5]*t**3return pos, vel, accdef evaluate_trajectory(self, time_points: np.ndarray) - Dict[str, np.ndarray]:批量评估轨迹n len(time_points)positions np.zeros(n)velocities np.zeros(n)accelerations np.zeros(n)for i, t in enumerate(time_points):positions[i], velocities[i], accelerations[i] self.evaluate(t)return {time: time_points,positions: positions,velocities: velocities,accelerations: accelerations}class MultiJointTrajectoryGenerator:多关节轨迹生成器为每个关节独立生成五次多项式轨迹def __init__(self, joint_limits: JointLimitSet):self.limits joint_limitsself.trajectories: List[QuinticPolynomialTrajectory] []def generate_trajectories(self, start_positions: List[float],end_positions: List[float],total_time: float) - List[QuinticPolynomialTrajectory]:为所有关节生成轨迹n_joints len(start_positions)if len(end_positions) ! n_joints:raise ValueError(起点和终点关节数必须一致)if n_joints len(self.limits.limits):raise ValueError(关节数超过限位定义数量)self.trajectories []for j in range(n_joints):boundary TrajectoryBoundary(start_positionstart_positions[j],end_positionend_positions[j]# 速度加速度默认全0)traj QuinticPolynomialTrajectory(boundary, total_time)self.trajectories.append(traj)return self.trajectoriesdef evaluate_all(self, time_points: np.ndarray) - Dict:评估所有关节的轨迹n_joints len(self.trajectories)n_points len(time_points)all_positions np.zeros((n_points, n_joints))all_velocities np.zeros((n_points, n_joints))all_accelerations np.zeros((n_points, n_joints))for j in range(n_joints):result self.trajectories[j].evaluate_trajectory(time_points)all_positions[:, j] result[positions]all_velocities[:, j] result[velocities]all_accelerations[:, j] result[accelerations]# 逐点校验限位velocity_violations []acceleration_violations []position_violations []for i in range(n_points):pos_list all_positions[i, :].tolist()vel_list all_velocities[i, :].tolist()acc_list all_accelerations[i, :].tolist()_, v_viol self.limits.check_all_velocities(vel_list)_, a_viol self.limits.check_all_accelerations(acc_list)_, p_viol self.limits.check_all_positions(pos_list)for j in v_viol:velocity_violations.append((i, j, vel_list[j],self.limits.limits[j].max_velocity))for j in a_viol:acceleration_violations.append((i, j, acc_list[j],self.limits.limits[j].max_acceleration))for j in p_viol:position_violations.append((i, j, pos_list[j],self.limits.limits[j].min_angle,self.limits.limits[j].max_angle))return {time: time_points,positions: all_positions,velocities: all_velocities,accelerations: all_accelerations,velocity_violations: velocity_violations,acceleration_violations: acceleration_violations,position_violations: position_violations,n_joints: n_joints,n_points: n_points}robot/visualize_trajectory.py — 可视化模块多项式轨迹规划可视化4张核心图表关节位置曲线、关节速度曲线、关节加速度曲线、速度/加速度峰值对比import numpy as npimport matplotlib.pyplot as pltplt.rcParams[font.sans-serif] [SimHei, DejaVu Sans]plt.rcParams[axes.unicode_minus] Falseclass TrajectoryVisualizer:def __init__(self, results_dirresults):self.results_dir results_dirimport osos.makedirs(results_dir, exist_okTrue)def plot_joint_positions(self, timestamps, positions):① 关节角度随时间变化曲线验证起止点是否正确到达单调插值轨迹不会超位置限位此图主要验证平滑性fig, ax plt.subplots(figsize(12, 5))joint_names [fθ_{i1} for i in range(positions.shape[1])]colors [steelblue, orange, green]for j in range(positions.shape[1]):ax.plot(timestamps, positions[:, j], -,colorcolors[j], linewidth1.5, labelf{joint_names[j]})ax.set_xlabel(Time (s))ax.set_ylabel(Joint Angle (rad))ax.set_title(Joint Position Profiles — Quintic Polynomial Trajectory)ax.legend(locupper right, fontsize8)ax.grid(True, alpha0.3)plt.tight_layout()path f{self.results_dir}/01_joint_position_curves.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_joint_velocities(self, timestamps, velocities, joint_limits):② 关节角速度随时间变化曲线核心输出直接读出速度峰值和超限区间fig, axes plt.subplots(3, 1, figsize(12, 9), sharexTrue)joint_names [θ₁ (Base Yaw), θ₂ (Elbow Pitch), θ₃ (Wrist Pitch)]colors [steelblue, orange, green]for j in range(3):ax axes[j]ax.plot(timestamps, velocities[:, j], -,colorcolors[j], linewidth1.2, labelfActual {joint_names[j]})limit joint_limits.limits[j].max_velocityax.axhline(ylimit, colorred, linestyle--, linewidth1.0,labelfMax ({limit} rad/s))ax.axhline(y-limit, colorred, linestyle--, linewidth1.0,labelfMin (-{limit} rad/s))ax.axhline(y0, colorblack, linestyle-, linewidth0.5, alpha0.5)ax.set_ylabel(Angular Velocity (rad/s))ax.legend(locupper right, fontsize7)ax.grid(True, alpha0.3)ax.set_title(f{joint_names[j]} Velocity Profile, fontsize10)axes[2].set_xlabel(Time (s))plt.tight_layout()path f{self.results_dir}/02_joint_velocity_curves.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_joint_accelerations(self, timestamps, accelerations, joint_limits):③ 关节角加速度随时间变化曲线验证加速度是否超限fig, axes plt.subplots(3, 1, figsize(12, 9), sharexTrue)joint_names [θ₁ (Base Yaw), θ₂ (Elbow Pitch), θ₃ (Wrist Pitch)]colors [steelblue, orange, green]for j in range(3):ax axes[j]ax.plot(timestamps, accelerations[:, j], -,colorcolors[j], linewidth1.2, labelfActual {joint_names[j]})limit joint_limits.limits[j].max_accelerationax.axhline(ylimit, colorred, linestyle--, linewidth1.0,labelfMax ({limit} rad/s²))ax.axhline(y-limit, colorred, linestyle--, linewidth1.0,labelfMin (-{limit} rad/s²))ax.axhline(y0, colorblack, linestyle-, linewidth0.5, alpha0.5)ax.set_ylabel(Angular Acceleration (rad/s²))ax.legend(locupper right, fontsize7)ax.grid(True, alpha0.3)ax.set_title(f{joint_names[j]} Acceleration Profile, fontsize10)axes[2].set_xlabel(Time (s))plt.tight_layout()path f{self.results_dir}/03_joint_acceleration_curves.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_peak_comparison(self, velocities, accelerations, joint_limits):④ 各关节速度/加速度峰值与限位对比柱状图一目了然哪些关节接近极限fig, (ax1, ax2) plt.subplots(1, 2, figsize(12, 5))joint_names [θ₁, θ₂, θ₃]# 速度峰值vel_peaks [np.max(np.abs(velocities[:, j])) for j in range(3)]vel_limits [joint_limits.limits[j].max_velocity for j in range(3)]x np.arange(3)width 0.35bars1 ax1.bar(x - width/2, vel_peaks, width, labelPeak Velocity,colorsteelblue, alpha0.7)bars2 ax1.bar(x width/2, vel_limits, width, labelVelocity Limit,colorred, alpha0.5)ax1.set_xlabel(Joint)ax1.set_ylabel(Angular Velocity (rad/s))ax1.set_title(Peak Velocity vs Limit)ax1.set_xticks(x)ax1.set_xticklabels(joint_names)ax1.legend(fontsize8)ax1.grid(True, alpha0.3, axisy)for bar, val in zip(bars1, vel_peaks):ax1.text(bar.get_x() bar.get_width()/2, bar.get_height() 0.02,f{val:.3f}, hacenter, vabottom, fontsize7)# 加速度峰值acc_peaks [np.max(np.abs(accelerations[:, j])) for j in range(3)]acc_limits [joint_limits.limits[j].max_acceleration for j in range(3)]bars3 ax2.bar(x - width/2, acc_peaks, width, labelPeak Acceleration,colororange, alpha0.7)bars4 ax2.bar(x width/2, acc_limits, width, labelAcceleration Limit,colorred, alpha0.5)ax2.set_xlabel(Joint)ax2.set_ylabel(Angular Acceleration (rad/s²))ax2.set_title(Peak Acceleration vs Limit)ax2.set_xticks(x)ax2.set_xticklabels(joint_names)ax2.legend(fontsize8)ax2.grid(True, alpha0.3, axisy)for bar, val in zip(bars3, acc_peaks):ax2.text(bar.get_x() bar.get_width()/2, bar.get_height() 0.02,f{val:.3f}, hacenter, vabottom, fontsize7)plt.tight_layout()path f{self.results_dir}/04_peak_comparison.pngplt.savefig(path, dpi150)plt.close()return path5. 可视化要求序号 图表文件 服务结论①01_joint_position_curves.png 关节角度随时间变化验证起止点正确到达、运动平滑无突变②02_joint_velocity_curves.png 关节角速度曲线限位线读出速度峰值和超限时间段③03_joint_acceleration_curves.png 关节角加速度曲线限位线验证加速度连续且未超限④04_peak_comparison.png 各关节速度/加速度峰值与限位对比柱状图一目了然安全裕度6. README使用说明 核心知识点卡片快速开始pip install -r requirements.txtpython -m pytest tests/test_polynomial_trajectory_sim.py -v # 3/3 测试通过python run_polynomial_trajectory_sim.py # 运行多项式轨迹仿真数据格式说明参数 修改位置 说明起点/终点关节角run_polynomial_trajectory_sim.py中q_start/q_end 弧度制(rad)总运动时间 同文件total_time 秒(s)改小会增大速度/加速度关节限位JointLimitSet初始化 匹配实际机器人参数采样数 同文件n_samples 默认200影响曲线平滑度 知识点卡片五次多项式轨迹规划对应课本第6章6.3节- 边界条件起止位置、速度、加速度共6个条件→唯一确定五次多项式- 解析峰值v_peak 15Δ/(8T)a_peak 5.7735Δ/T²零边界速度/加速度条件- 平滑性速度连续无冲击启停、加速度连续无突变扭矩- 局限性各关节独立规划不保证末端走直线校验必须同时查速度和加速度7. 总结与工程师思考工业落地难处一、各关节独立规划不保证末端路径直线度。 本篇每个关节独立生成五次多项式末端在笛卡尔空间走的是曲线。如果工艺要求末端严格走直线如激光焊接、涂胶独立关节规划会导致路径偏差。工业现场通常用笛卡尔空间直线插补逆运动学实时求解或先规划末端路径再映射到关节空间。二、总时间T的选择需要同时利用AI解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛
