### 使用 Python 实现起重小车运动轨迹规划的简单脚本 为了实现起重小车的运动轨迹规划，可以采用一种常见的方法——多项式插值法。这种方法适用于平滑地连接起点和终点的位置、速度及加速度约束条件。下面是一个基于三次多项式的例子，它能够满足给定的时间范围内完成指定的任务。 假设我们需要规划一个小车从初始位置 \( p_0 \) 移动到目标位置 \( p_f \)，并且希望在时间区间 [\( t_0 \), \( t_f \)] 内完成这一动作。此外，还可以设定初末的速度和加速度边界条件以确保平稳过渡[^1]。 ```python import numpy as np import matplotlib.pyplot as plt def cubic_trajectory(t, p0, pf, v0=0, vf=0): """ 定义一个三次多项式轨迹生成函数 参数: t : float or array-like 时间变量 p0 : 初始位置 pf : 终止位置 v0 : 初速度，默认为零 vf : 末速度，默认为零 返回: position : 当前时刻对应的位置值 velocity : 当前时刻对应的瞬时速度 acceleration : 当前时刻对应的瞬时加速度 """ T = max(t) - min(t) # 计算系数矩阵A及其逆阵inv_A A = np.array([ [0, 0, 0, 1], [T**3, T**2, T, 1], [0, 0, 1, 0], [3*T**2, 2*T, 1, 0] ]) inv_A = np.linalg.inv(A) B = np.array([p0, pf, v0, vf]).reshape((4, 1)) coeffs = np.dot(inv_A, B).flatten() a3, a2, a1, a0 = coeffs position = a3*t**3 + a2*t**2 + a1*t + a0 velocity = 3*a3*t**2 + 2*a2*t + a1 acceleration = 6*a3*t + 2*a2 return position, velocity, acceleration # 设置参数 t_start = 0 # 开始时间(s) t_end = 5 # 结束时间(s) pos_initial = 0 # 起始位移(m) pos_final = 10 # 目标位移(m) vel_initial = 0 # 起始速度(m/s) vel_final = 0 # 目标速度(m/s) time_vector = np.linspace(t_start, t_end, num=500) positions, velocities, accelerations = cubic_trajectory(time_vector, pos_initial, pos_final, vel_initial, vel_final) # 可视化结果 plt.figure(figsize=(12, 8)) plt.subplot(3, 1, 1) plt.plot(time_vector, positions, label="Position", color='blue') plt.title('Cubic Trajectory Planning Results') plt.ylabel('Position (m)') plt.legend() plt.grid(True) plt.subplot(3, 1, 2) plt.plot(time_vector, velocities, label="Velocity", color='green') plt.ylabel('Velocity (m/s)') plt.legend() plt.grid(True) plt.subplot(3, 1, 3) plt.plot(time_vector, accelerations, label="Acceleration", color='red') plt.xlabel('Time (s)') plt.ylabel('Acceleration ($m/s^2$)') plt.legend() plt.grid(True) plt.tight_layout() plt.show() ``` 此代码片段展示了如何创建一条由三次曲线构成的空间路径，并绘制出随时间变化的位置、速度以及加速度图形。用户可以根据具体应用场景调整输入参数如总耗时或末端状态等进一步定制所需的行为模式。 ---