# 机械臂轨迹平滑过渡实战：如何用Python实现抛物线插值（附完整代码） 在工业机器人开发领域，轨迹规划的质量直接决定了机械臂的运动性能。想象一下，一个六轴机械臂正在执行高精度的装配任务，当它从一个点直线移动到另一个点时，如果速度在路径点处突然改变，会产生巨大的加速度冲击。这种冲击不仅会导致机械振动、降低定位精度，还会加速电机和减速器的磨损，甚至可能引发系统共振。这就是为什么简单的直线插补在实际工程中往往不够用——我们需要更平滑的过渡。 抛物线过渡的直线插补算法，正是为了解决这个问题而生。它通过在直线段之间插入二次多项式（抛物线）段，让速度能够连续变化，避免突变。这种技术听起来可能有些理论化，但在实际应用中，它能让机械臂的运动如丝般顺滑，显著提升运动控制的品质。今天，我将从工程实践的角度，带你深入理解这一算法的原理，并用Python一步步实现一个完整的、可直接复用的轨迹生成器。 ## 1. 为什么需要抛物线过渡？从电机控制视角看轨迹平滑 在工业机器人控制系统中，轨迹规划的核心目标之一就是生成对电机友好的运动指令。电机，特别是伺服电机，有其物理极限：最大速度、最大加速度，以及更重要的——最大加加速度（Jerk）。当轨迹规划不考虑这些约束时，就会产生问题。 ### 1.1 传统直线插补的局限性 最基本的直线插补算法非常简单：给定起点和终点，按照时间均匀分配中间点。假设从点A(0,0)移动到点B(10,0)，总时间2秒，插补周期10ms，那么每个周期的位置增量就是： ```python # 简单直线插补示例 start_pos = 0.0 end_pos = 10.0 total_time = 2.0 # 秒 interp_period = 0.01 # 10ms num_points = int(total_time / interp_period) delta_pos = (end_pos - start_pos) / num_points positions = [] for i in range(num_points + 1): pos = start_pos + i * delta_pos positions.append(pos) ``` 这种方法的**速度曲线是阶跃的**——在起点瞬间达到匀速，在终点瞬间停止。从控制理论看，这意味着加速度在起点和终点处趋于无穷大。在实际电机驱动中，这会导致： - **电流冲击**：电机需要瞬间提供极大扭矩 - **机械振动**：传动系统承受冲击载荷 - **跟踪误差**：实际位置无法跟上指令位置 - **寿命缩短**：机械部件加速疲劳 ### 1.2 抛物线过渡的物理意义 抛物线过渡的本质是在速度变化阶段引入**恒定加速度**。从运动学角度看： - **直线段**：匀速运动，加速度为0 - **抛物线段**：匀加速/匀减速运动，加速度为常数 这种设计带来了几个关键优势： 1. **加速度有界**：避免了无限大的加速度冲击 2. **速度连续**：速度曲线可导，运动更平滑 3. **Jerk可控**：加加速度（加速度的变化率）也是有限的 > **注意**：在实际系统中，我们通常还会在抛物线段的基础上进一步平滑，使用S型速度曲线（七段式加减速），但抛物线过渡是最基础、最核心的平滑技术。 ### 1.3 工程中的实际考量 在真实的机器人控制系统中，轨迹平滑不仅仅是数学问题，还涉及： - **伺服带宽**：电机响应能力限制了可实现的加速度 - **谐振频率**：机械结构有固有频率，需要避免激发 - **控制周期**：离散化带来的量化误差 - **多轴同步**：多个关节需要协调运动 下面的表格对比了不同插补方法的特性： | 特性 | 简单直线插补 | 抛物线过渡 | 高阶多项式 | |------|-------------|-----------|-----------| | 速度连续性 | C0连续（位置连续） | C1连续（速度连续） | C2连续（加速度连续） | | 计算复杂度 | 极低 | 中等 | 较高 | | 实时性 | 优秀 | 良好 | 一般 | | 对电机冲击 | 大 | 小 | 很小 | | 适用场景 | 低速、低精度 | 中高速、一般精度 | 高速、高精度 | ## 2. 抛物线过渡算法的数学原理与实现框架 理解了为什么需要抛物线过渡后，我们来看看具体的数学实现。抛物线过渡的核心思想很简单：在直线路径的起点和终点处，各插入一段抛物线，让速度从0平滑加速到巡航速度，再平滑减速到0。 ### 2.1 基本数学模型 考虑一维情况下的运动规划。假设我们要从位置θ₀运动到θ₁，总时间为T。我们希望： 1. 在时间[0, t_a]内匀加速运动 2. 在时间[t_a, T-t_a]内匀速运动 3. 在时间[T-t_a, T]内匀减速运动 其中t_a是加速/减速时间。对于对称的抛物线过渡，加速和减速时间相等。 **运动方程如下**： - **加速段**（0 ≤ t ≤ t_a）： ``` θ(t) = θ₀ + 0.5 * a * t² v(t) = a * t a(t) = a (常数) ``` - **匀速段**（t_a ≤ t ≤ T - t_a）： ``` θ(t) = θ₀ + 0.5 * a * t_a² + v_cruise * (t - t_a) v(t) = v_cruise (常数) a(t) = 0 ``` - **减速段**（T - t_a ≤ t ≤ T）： ``` θ(t) = θ₁ - 0.5 * a * (T - t)² v(t) = a * (T - t) a(t) = -a (常数) ``` 其中巡航速度v_cruise = a * t_a，且需要满足总位移条件。 ### 2.2 多段路径的通用公式 在实际应用中，机械臂需要经过多个路径点。假设我们有n个路径点θ₁, θ₂, ..., θₙ，对应的时间点为t₁, t₂, ..., tₙ。 对于第i段路径（从θᵢ到θᵢ₊₁），我们定义： - **直线段速度**：vᵢ = (θᵢ₊₁ - θᵢ) / (tᵢ₊₁ - tᵢ) - **过渡时间**：Δt（通常每段相同） - **加速度**：aᵢ = (vᵢ - vᵢ₋₁) / Δt **关键点在于**：过渡段的时间Δt需要精心选择。太短则加速度过大，太长则效率低下。通常根据电机的最大加速度限制来确定： ```python def calculate_transition_time(max_accel, start_vel, end_vel): """计算过渡时间""" delta_v = abs(end_vel - start_vel) # 确保加速度不超过限制 required_time = delta_v / max_accel return required_time ``` ### 2.3 路径点处理策略 抛物线过渡有一个重要特性：**实际轨迹不会精确经过中间路径点**。这是因为我们在路径点前后都插入了抛物线过渡段。 如果应用要求必须精确经过路径点（比如避障点），有两种解决方案： 1. **虚拟点法**：在真实路径点前后添加两个虚拟点，确保真实点落在直线段上 2. **速度归零法**：在路径点处将速度降为0，但这会影响效率 下面的代码展示了如何为必须经过的路径点添加虚拟点： ```python def add_virtual_points(original_points, transition_distance): """ 为必须经过的路径点添加虚拟点 参数： original_points: 原始路径点列表 transition_distance: 过渡段长度 返回： 包含虚拟点的新路径点列表 """ virtual_points = [] # 第一个点直接添加 virtual_points.append(original_points[0]) for i in range(1, len(original_points) - 1): prev_point = original_points[i-1] curr_point = original_points[i] next_point = original_points[i+1] # 计算方向向量 dir_in = normalize(curr_point - prev_point) dir_out = normalize(next_point - curr_point) # 添加进入虚拟点 entry_point = curr_point - dir_in * transition_distance virtual_points.append(entry_point) # 添加实际路径点 virtual_points.append(curr_point) # 添加离开虚拟点 exit_point = curr_point + dir_out * transition_distance virtual_points.append(exit_point) # 最后一个点 virtual_points.append(original_points[-1]) return virtual_points ``` ## 3. Python实现：完整的抛物线过渡轨迹生成器 现在让我们用Python实现一个完整的抛物线过渡轨迹生成器。我们将使用NumPy进行数值计算，Matplotlib进行可视化。 ### 3.1 核心算法实现 首先定义核心的轨迹生成函数： ```python import numpy as np from typing import List, Tuple import matplotlib.pyplot as plt class ParabolicTransitionPlanner: """抛物线过渡轨迹规划器""" def __init__(self, max_acceleration: float = 1.0): """ 初始化规划器 参数： max_acceleration: 最大加速度限制 """ self.max_accel = max_acceleration def plan_single_axis(self, positions: List[float], times: List[float], dt: float = 0.001) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """ 单轴轨迹规划 参数： positions: 位置点列表 times: 对应的时间点列表 dt: 采样时间间隔 返回： time_array: 时间数组 pos_array: 位置数组 vel_array: 速度数组 acc_array: 加速度数组 """ # 输入验证 assert len(positions) == len(times), "位置和时间点数量必须相同" assert len(positions) >= 2, "至少需要两个点" n_points = len(positions) # 计算段间速度 segment_velocities = [] for i in range(n_points - 1): delta_pos = positions[i+1] - positions[i] delta_time = times[i+1] - times[i] if delta_time <= 0: raise ValueError("时间点必须递增") segment_velocities.append(delta_pos / delta_time) # 添加起始和结束速度为0 velocities = [0.0] + segment_velocities + [0.0] # 计算总时间范围内的采样点 total_time = times[-1] - times[0] n_samples = int(total_time / dt) + 1 time_array = np.linspace(times[0], times[-1], n_samples) # 初始化输出数组 pos_array = np.zeros(n_samples) vel_array = np.zeros(n_samples) acc_array = np.zeros(n_samples) current_segment = 0 t_accumulated = 0.0 for i in range(n_samples): t = time_array[i] # 确定当前所在的时间段 while current_segment < len(times) - 1 and t > times[current_segment + 1]: current_segment += 1 t_accumulated = times[current_segment] # 计算在当前时间段内的相对时间 t_rel = t - t_accumulated segment_duration = times[current_segment + 1] - times[current_segment] if current_segment == 0: # 第一段：只有加速段 if t_rel < segment_duration: # 加速段 acc = (velocities[1] - velocities[0]) / segment_duration pos = positions[0] + 0.5 * acc * t_rel**2 vel = acc * t_rel else: # 匀速段（如果有） pos = positions[0] + velocities[1] * (t_rel - segment_duration) vel = velocities[1] elif current_segment == len(times) - 2: # 最后一段：只有减速段 if t_rel < segment_duration: # 减速前的匀速段 pos = positions[-2] + velocities[-2] * t_rel vel = velocities[-2] else: # 减速段 t_decel = t_rel - segment_duration acc = (velocities[-1] - velocities[-2]) / segment_duration pos = positions[-1] - 0.5 * acc * (segment_duration - t_decel)**2 vel = velocities[-2] + acc * t_decel else: # 中间段：减速段 + 匀速段 + 加速段 transition_time = segment_duration * 0.2 # 过渡时间占20% if t_rel < transition_time: # 从前一段减速 acc = (velocities[current_segment] - velocities[current_segment-1]) / transition_time pos = positions[current_segment-1] + velocities[current_segment-1] * t_rel + 0.5 * acc * t_rel**2 vel = velocities[current_segment-1] + acc * t_rel elif t_rel < segment_duration - transition_time: # 匀速段 t_cruise = t_rel - transition_time pos = positions[current_segment] + velocities[current_segment] * t_cruise vel = velocities[current_segment] else: # 加速到下一段 t_accel = t_rel - (segment_duration - transition_time) acc = (velocities[current_segment+1] - velocities[current_segment]) / transition_time pos = positions[current_segment] + velocities[current_segment] * (segment_duration - transition_time) + \ velocities[current_segment] * t_accel + 0.5 * acc * t_accel**2 vel = velocities[current_segment] + acc * t_accel pos_array[i] = pos vel_array[i] = vel # 计算加速度（数值微分） if i > 0: acc_array[i] = (vel_array[i] - vel_array[i-1]) / dt return time_array, pos_array, vel_array, acc_array ``` ### 3.2 多轴同步处理 在实际的机械臂控制中，我们需要同时规划多个关节的运动。关键是要确保所有关节在同一时间到达目标位置，同时满足各自的加速度限制。 ```python def plan_multi_axis(self, positions_list: List[List[float]], times: List[float], dt: float = 0.001) -> dict: """ 多轴同步轨迹规划 参数： positions_list: 每个轴的位置点列表的列表 times: 时间点列表（所有轴共享） dt: 采样时间间隔 返回： trajectory: 包含各轴轨迹的字典 """ n_axes = len(positions_list) # 为每个轴单独规划 trajectories = [] max_duration = 0 for axis_idx in range(n_axes): t, pos, vel, acc = self.plan_single_axis( positions_list[axis_idx], times, dt ) trajectories.append({ 'time': t, 'position': pos, 'velocity': vel, 'acceleration': acc }) max_duration = max(max_duration, t[-1]) # 时间同步：确保所有轨迹有相同的时间数组 synced_time = np.arange(times[0], max_duration + dt, dt) # 重新采样使所有轨迹时间对齐 synced_trajectories = [] for traj in trajectories: # 线性插值到统一的时间网格 from scipy.interpolate import interp1d pos_interp = interp1d(traj['time'], traj['position'], kind='linear', fill_value='extrapolate') vel_interp = interp1d(traj['time'], traj['velocity'], kind='linear', fill_value='extrapolate') acc_interp = interp1d(traj['time'], traj['acceleration'], kind='linear', fill_value='extrapolate') synced_trajectories.append({ 'time': synced_time, 'position': pos_interp(synced_time), 'velocity': vel_interp(synced_time), 'acceleration': acc_interp(synced_time) }) return { 'time': synced_time, 'axes': synced_trajectories, 'n_axes': n_axes } ``` ### 3.3 可视化与分析工具 为了直观理解轨迹特性，我们创建可视化工具： ```python def plot_trajectory(self, trajectory: dict, axis_idx: int = 0): """绘制单轴轨迹曲线""" fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(10, 8), sharex=True) time = trajectory['time'] if 'axes' in trajectory: # 多轴情况 pos = trajectory['axes'][axis_idx]['position'] vel = trajectory['axes'][axis_idx]['velocity'] acc = trajectory['axes'][axis_idx]['acceleration'] title_suffix = f' (Axis {axis_idx})' else: # 单轴情况 pos = trajectory['position'] vel = trajectory['velocity'] acc = trajectory['acceleration'] title_suffix = '' # 位置曲线 ax1.plot(time, pos, 'b-', linewidth=2) ax1.set_ylabel('Position', fontsize=12) ax1.grid(True, alpha=0.3) ax1.set_title(f'Trajectory Profile{title_suffix}', fontsize=14) # 速度曲线 ax2.plot(time, vel, 'g-', linewidth=2) ax2.set_ylabel('Velocity', fontsize=12) ax2.grid(True, alpha=0.3) # 加速度曲线 ax3.plot(time, acc, 'r-', linewidth=2) ax3.set_ylabel('Acceleration', fontsize=12) ax3.set_xlabel('Time (s)', fontsize=12) ax3.grid(True, alpha=0.3) plt.tight_layout() return fig def analyze_trajectory(self, trajectory: dict, axis_idx: int = 0): """分析轨迹性能指标""" if 'axes' in trajectory: pos = trajectory['axes'][axis_idx]['position'] vel = trajectory['axes'][axis_idx]['velocity'] acc = trajectory['axes'][axis_idx]['acceleration'] else: pos = trajectory['position'] vel = trajectory['velocity'] acc = trajectory['acceleration'] # 计算关键指标 max_vel = np.max(np.abs(vel)) max_acc = np.max(np.abs(acc)) # 计算Jerk（加速度变化率） dt = trajectory['time'][1] - trajectory['time'][0] jerk = np.gradient(acc, dt) max_jerk = np.max(np.abs(jerk)) # 计算运动平滑度指标 # 速度均方根误差（相对于平均速度） mean_vel = np.mean(np.abs(vel)) vel_rms = np.sqrt(np.mean(vel**2)) # 加速度能量（积分平方） acc_energy = np.trapz(acc**2, trajectory['time']) metrics = { 'max_velocity': max_vel, 'max_acceleration': max_acc, 'max_jerk': max_jerk, 'velocity_rms': vel_rms, 'acceleration_energy': acc_energy, 'total_distance': pos[-1] - pos[0], 'total_time': trajectory['time'][-1] - trajectory['time'][0] } return metrics ``` ## 4. 实战案例：六轴机械臂的笛卡尔空间轨迹规划 现在让我们将抛物线过渡算法应用到更实际的场景：六轴机械臂的笛卡尔空间轨迹规划。这里的关键是将末端执行器的直线运动分解到各个关节。 ### 4.1 从笛卡尔空间到关节空间 在工业机器人中，我们通常先在笛卡尔空间（末端执行器的位置和姿态）规划路径，然后通过逆运动学转换到关节空间。抛物线过渡可以在这两个空间中的任意一个实施。 **笛卡尔空间抛物线过渡的优势**： - 末端路径精确可控 - 更容易处理障碍物避让 - 直观的路径可视化 **关节空间抛物线过渡的优势**： - 计算更简单 - 直接控制关节电机 - 避免奇异点问题 下面的代码展示了如何在笛卡尔空间实施抛物线过渡： ```python import numpy as np from scipy.spatial.transform import Rotation as R class CartesianParabolicPlanner: """笛卡尔空间抛物线过渡规划器""" def __init__(self, max_linear_accel: float = 1.0, max_angular_accel: float = 1.0): self.max_linear_accel = max_linear_accel self.max_angular_accel = max_angular_accel def plan_pose_trajectory(self, positions: List[np.ndarray], # 位置 (x, y, z) orientations: List[R], # 姿态 (四元数或旋转矩阵) times: List[float], dt: float = 0.001): """ 规划位姿轨迹（位置+姿态） 参数： positions: 位置列表，每个是3维向量 orientations: 姿态列表，每个是scipy Rotation对象 times: 时间点列表 dt: 采样时间 """ n_points = len(positions) assert len(orientations) == n_points assert len(times) == n_points # 分离位置和姿态 pos_x = [p[0] for p in positions] pos_y = [p[1] for p in positions] pos_z = [p[2] for p in positions] # 将姿态转换为欧拉角（或轴角）进行插值 euler_angles = [] for orient in orientations: # 使用ZYX欧拉角（根据实际机械臂定义调整） euler = orient.as_euler('zyx', degrees=False) euler_angles.append(euler) euler_x = [e[0] for e in euler_angles] euler_y = [e[1] for e in euler_angles] euler_z = [e[2] for e in euler_angles] # 为每个自由度单独规划抛物线过渡 planner = ParabolicTransitionPlanner() # 位置轨迹 traj_x = planner.plan_single_axis(pos_x, times, dt) traj_y = planner.plan_single_axis(pos_y, times, dt) traj_z = planner.plan_single_axis(pos_z, times, dt) # 姿态轨迹（欧拉角） traj_rx = planner.plan_single_axis(euler_x, times, dt) traj_ry = planner.plan_single_axis(euler_y, times, dt) traj_rz = planner.plan_single_axis(euler_z, times, dt) # 重新组合为完整的位姿轨迹 n_samples = len(traj_x[0]) # 时间点数量 cartesian_traj = { 'time': traj_x[0], 'position': np.column_stack([traj_x[1], traj_y[1], traj_z[1]]), 'linear_velocity': np.column_stack([traj_x[2], traj_y[2], traj_z[2]]), 'linear_acceleration': np.column_stack([traj_x[3], traj_y[3], traj_z[3]]), 'orientation': [], 'angular_velocity': np.column_stack([traj_rx[2], traj_ry[2], traj_rz[2]]), 'angular_acceleration': np.column_stack([traj_rx[3], traj_ry[3], traj_rz[3]]) } # 将欧拉角转换回Rotation对象 for i in range(n_samples): euler = np.array([traj_rx[1][i], traj_ry[1][i], traj_rz[1][i]]) rot = R.from_euler('zyx', euler) cartesian_traj['orientation'].append(rot) return cartesian_traj def check_constraints(self, trajectory: dict) -> dict: """检查轨迹是否满足约束条件""" violations = { 'linear_accel_exceeded': False, 'angular_accel_exceeded': False, 'max_linear_vel': 0.0, 'max_angular_vel': 0.0, 'max_linear_accel': 0.0, 'max_angular_accel': 0.0 } # 检查线加速度约束 linear_accel_norm = np.linalg.norm(trajectory['linear_acceleration'], axis=1) max_linear_accel = np.max(linear_accel_norm) violations['max_linear_accel'] = max_linear_accel violations['linear_accel_exceeded'] = max_linear_accel > self.max_linear_accel # 检查角加速度约束 angular_accel_norm = np.linalg.norm(trajectory['angular_acceleration'], axis=1) max_angular_accel = np.max(angular_accel_norm) violations['max_angular_accel'] = max_angular_accel violations['angular_accel_exceeded'] = max_angular_accel > self.max_angular_accel # 记录最大速度（用于参考） linear_vel_norm = np.linalg.norm(trajectory['linear_velocity'], axis=1) angular_vel_norm = np.linalg.norm(trajectory['angular_velocity'], axis=1) violations['max_linear_vel'] = np.max(linear_vel_norm) violations['max_angular_vel'] = np.max(angular_vel_norm) return violations ``` ### 4.2 处理姿态插值的特殊考虑 姿态插值比位置插值更复杂，因为三维旋转空间不是线性的。直接对欧拉角进行线性插值可能导致方向错误。更好的方法是使用四元数球面线性插值（SLERP）： ```python def slerp_orientation(self, q_start: np.ndarray, q_end: np.ndarray, t: float) -> np.ndarray: """ 四元数球面线性插值 参数： q_start, q_end: 起始和结束四元数（w, x, y, z） t: 插值参数 [0, 1] 返回： 插值后的四元数 """ # 确保四元数已归一化 q_start = q_start / np.linalg.norm(q_start) q_end = q_end / np.linalg.norm(q_end) # 计算点积 dot = np.dot(q_start, q_end) # 如果点积为负，取反其中一个四元数以走最短路径 if dot < 0.0: q_end = -q_end dot = -dot # 避免数值误差 dot = np.clip(dot, -1.0, 1.0) # 计算角度和插值 theta = np.arccos(dot) * t if abs(dot) > 0.9995: # 非常接近时使用线性插值避免除零 result = q_start + t * (q_end - q_start) return result / np.linalg.norm(result) q_rel = q_end - q_start * dot q_rel = q_rel / np.linalg.norm(q_rel) result = q_start * np.cos(theta) + q_rel * np.sin(theta) return result def plan_orientation_with_slerp(self, orientations: List[R], times: List[float], dt: float = 0.001): """使用SLERP进行姿态插值""" # 将Rotation对象转换为四元数 quaternions = [orient.as_quat() for orient in orientations] # 生成时间序列 time_array = np.arange(times[0], times[-1] + dt, dt) # 分段SLERP插值 interpolated_quats = [] current_segment = 0 for t in time_array: # 找到当前时间所在段 while current_segment < len(times) - 1 and t > times[current_segment + 1]: current_segment += 1 if current_segment >= len(times) - 1: # 超出最后一段，使用最后一个姿态 interpolated_quats.append(quaternions[-1]) continue # 计算段内插值参数 t_start = times[current_segment] t_end = times[current_segment + 1] segment_duration = t_end - t_start if segment_duration == 0: alpha = 0 else: alpha = (t - t_start) / segment_duration alpha = np.clip(alpha, 0.0, 1.0) # SLERP插值 q_start = quaternions[current_segment] q_end = quaternions[current_segment + 1] q_interp = self.slerp_orientation(q_start, q_end, alpha) interpolated_quats.append(q_interp) # 转换为Rotation对象列表 interpolated_rots = [R.from_quat(q) for q in interpolated_quats] return { 'time': time_array, 'orientation': interpolated_rots, 'quaternions': np.array(interpolated_quats) } ``` ### 4.3 完整示例：机械臂拾放任务 让我们看一个完整的拾放任务示例，机械臂需要从A点拾取物体，移动到B点，然后放置到C点： ```python def demo_pick_and_place(): """拾放任务演示""" import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # 创建规划器实例 planner = CartesianParabolicPlanner( max_linear_accel=2.0, # m/s² max_angular_accel=1.0 # rad/s² ) # 定义路径点（位置和姿态） # 点A：拾取位置 pos_a = np.array([0.5, 0.2, 0.1]) orient_a = R.from_euler('zyx', [0, 0, 0], degrees=True) # 点B：中间过渡点（提升高度） pos_b = np.array([0.5, 0.2, 0.3]) orient_b = R.from_euler('zyx', [0, 0, 0], degrees=True) # 点C：放置位置 pos_c = np.array([0.8, -0.2, 0.1]) orient_c = R.from_euler('zyx', [30, 0, 0], degrees=True) # 时间点（秒） times = [0.0, 2.0, 4.0, 6.0] # A->B: 2s, B->C: 2s, 停留: 2s # 规划轨迹 positions = [pos_a, pos_b, pos_b, pos_c] # 在B点停留 orientations = [orient_a, orient_b, orient_b, orient_c] trajectory = planner.plan_pose_trajectory(positions, orientations, times) # 检查约束 violations = planner.check_constraints(trajectory) print("轨迹约束检查结果：") for key, value in violations.items(): print(f" {key}: {value}") # 可视化 fig = plt.figure(figsize=(15, 10)) # 3D轨迹 ax1 = fig.add_subplot(2, 3, 1, projection='3d') ax1.plot(trajectory['position'][:, 0], trajectory['position'][:, 1], trajectory['position'][:, 2], 'b-', linewidth=2) ax1.scatter([p[0] for p in positions], [p[1] for p in positions], [p[2] for p in positions], c='r', s=100) ax1.set_xlabel('X (m)') ax1.set_ylabel('Y (m)') ax1.set_zlabel('Z (m)') ax1.set_title('3D Trajectory') # 位置分量 ax2 = fig.add_subplot(2, 3, 2) ax2.plot(trajectory['time'], trajectory['position'][:, 0], 'r-', label='X') ax2.plot(trajectory['time'], trajectory['position'][:, 1], 'g-', label='Y') ax2.plot(trajectory['time'], trajectory['position'][:, 2], 'b-', label='Z') ax2.set_xlabel('Time (s)') ax2.set_ylabel('Position (m)') ax2.legend() ax2.grid(True, alpha=0.3) ax2.set_title('Position Components') # 线速度 ax3 = fig.add_subplot(2, 3, 3) lin_vel_norm = np.linalg.norm(trajectory['linear_velocity'], axis=1) ax3.plot(trajectory['time'], lin_vel_norm, 'k-', linewidth=2) ax3.set_xlabel('Time (s)') ax3.set_ylabel('Linear Velocity (m/s)') ax3.grid(True, alpha=0.3) ax3.set_title('Linear Velocity Norm') # 线加速度 ax4 = fig.add_subplot(2, 3, 4) lin_acc_norm = np.linalg.norm(trajectory['linear_acceleration'], axis=1) ax4.plot(trajectory['time'], lin_acc_norm, 'k-', linewidth=2) ax4.axhline(y=planner.max_linear_accel, color='r', linestyle='--', label=f'Max: {planner.max_linear_accel} m/s²') ax4.set_xlabel('Time (s)') ax4.set_ylabel('Linear Acceleration (m/s²)') ax4.legend() ax4.grid(True, alpha=0.3) ax4.set_title('Linear Acceleration Norm') # 角速度 ax5 = fig.add_subplot(2, 3, 5) ang_vel_norm = np.linalg.norm(trajectory['angular_velocity'], axis=1) ax5.plot(trajectory['time'], ang_vel_norm, 'k-', linewidth=2) ax5.set_xlabel('Time (s)') ax5.set_ylabel('Angular Velocity (rad/s)') ax5.grid(True, alpha=0.3) ax5.set_title('Angular Velocity Norm') # 角加速度 ax6 = fig.add_subplot(2, 3, 6) ang_acc_norm = np.linalg.norm(trajectory['angular_acceleration'], axis=1) ax6.plot(trajectory['time'], ang_acc_norm, 'k-', linewidth=2) ax6.axhline(y=planner.max_angular_accel, color='r', linestyle='--', label=f'Max: {planner.max_angular_accel} rad/s²') ax6.set_xlabel('Time (s)') ax6.set_ylabel('Angular Acceleration (rad/s²)') ax6.legend() ax6.grid(True, alpha=0.3) ax6.set_title('Angular Acceleration Norm') plt.tight_layout() plt.show() return trajectory ``` ### 4.4 性能优化与实时考虑 在实际的机器人控制系统中，轨迹生成需要满足实时性要求。以下是一些优化技巧： ```python class OptimizedParabolicPlanner(ParabolicTransitionPlanner): """优化版的抛物线规划器，适用于实时系统""" def __init__(self, max_acceleration: float = 1.0, lookahead_points: int = 10): super().__init__(max_acceleration) self.lookahead_points = lookahead_points self.precomputed_segments = [] self.current_index = 0 def precompute_trajectory(self, positions: List[float], times: List[float], dt: float): """预计算轨迹段，减少实时计算量""" self.precomputed_segments = [] for i in range(len(positions) - 1): segment = self._compute_segment( positions[i], positions[i+1], times[i], times[i+1], dt ) self.precomputed_segments.append(segment) self.current_index = 0 self.segment_progress = 0.0 def _compute_segment(self, start_pos, end_pos, start_time, end_time, dt): """计算单个轨迹段""" duration = end_time - start_time n_points = int(duration / dt) # 计算速度 velocity = (end_pos - start_pos) / duration # 计算过渡时间（基于最大加速度） transition_time = min(duration * 0.2, abs(velocity) / self.max_accel) # 预计算加速段 accel_points = int(transition_time / dt) accel_segment = [] for i in range(accel_points): t = i * dt pos = start_pos + 0.5 * self.max_accel * t**2 vel = self.max_accel * t accel_segment.append((pos, vel, self.max_accel)) # 预计算匀速段 cruise_time = duration - 2 * transition_time if cruise_time > 0: cruise_points = int(cruise_time / dt) cruise_segment = [] for i in range(cruise_points): t = i * dt pos = start_pos + 0.5 * self.max_accel * transition_time**2 + velocity * t vel = velocity cruise_segment.append((pos, vel, 0.0)) else: cruise_segment = [] # 预计算减速段 decel_points = accel_points decel_segment = [] for i in range(decel_points): t = i * dt pos = end_pos - 0.5 * self.max_accel * (transition_time - t)**2 vel = self.max_accel * (transition_time - t) decel_segment.append((pos, vel, -self.max_accel)) return { 'accel': accel_segment, 'cruise': cruise_segment, 'decel': decel_segment, 'total_points': len(accel_segment) + len(cruise_segment) + len(decel_segment) } def get_next_point(self) -> Tuple[float, float, float]: """获取下一个轨迹点（实时调用）""" if self.current_index >= len(self.precomputed_segments): # 轨迹结束 return None segment = self.precomputed_segments[self.current_index] # 确定当前在哪个子段 if self.segment_progress < len(segment['accel']): # 加速段 point = segment['accel'][int(self.segment_progress)] elif self.segment_progress < len(segment['accel']) + len(segment['cruise']): # 匀速段 idx = int(self.segment_progress - len(segment['accel'])) point = segment['cruise'][idx] else: # 减速段 idx = int(self.segment_progress - len(segment['accel']) - len(segment['cruise'])) point = segment['decel'][idx] self.segment_progress += 1 # 检查是否进入下一段 if self.segment_progress >= segment['total_points']: self.current_index += 1 self.segment_progress = 0 return point # (position, velocity, acceleration) ``` 在实际项目中，我经常遇到需要平衡平滑性和效率的情况。抛物线过渡算法的一个实用技巧是**动态调整过渡时间**：在高速运动时使用较长的过渡时间以获得更好的平滑性，在低速或精细操作时使用较短的过渡时间以提高响应速度。这种自适应策略能显著提升机械臂的整体性能。