基于Python的带加减速机械臂运动建模问题求助

问题分析 #

你的核心需求是实现机械臂的梯形速度曲线规划（目标角度足够大时）或三角速度曲线规划（目标角度较小时），并计算关键时间点。现有代码的问题在于：

• 第一版未结合目标角度判断轨迹类型，且恒速段速度计算错误（vf*i是位移而非速度）；
• 第二版通过手动拼接数组的方式生成轨迹，逻辑混乱，未基于运动学公式先计算各阶段的边界参数，导致超调问题。

正确思路 #

先通过运动学公式计算各阶段的时间、位移边界值，再根据这些边界生成连续的时间序列，最后对应计算每个时间点的速度和位置，避免循环依赖和手动拼接的误差。

关键公式推导 #

• 加速/减速阶段（初/末速度为0）：
  • 加速到最大速度的时间：t_acc = vf / a
  • 加速阶段位移：s_acc = 0.5 * a * t_acc² = vf² / (2*a)
  • 减速阶段与加速阶段完全对称，时间t_dec = t_acc，位移s_dec = s_acc
• 临界位移（刚好能达到最大速度的总位移）：s_critical = s_acc + s_dec = vf² / a
• 分两种轨迹类型：
  1. 三角轨迹（目标角度m ≤ s_critical）：无法达到最大速度，加速后直接减速
  2. 梯形轨迹（目标角度m > s_critical）：加速→恒速→减速

修正后的代码 #

import numpy as np
import matplotlib.pyplot as plt

# 运动参数
u = 0       # 初始速度 (deg/s)
a = 0.41    # 加速度 (deg/s²)
vf = 2.3    # 最大速度 (deg/s)
m = 15      # 目标角度 (deg)

# 计算基础参数
t_acc = vf / a  # 加速到最大速度的时间
s_acc = 0.5 * a * t_acc**2  # 加速阶段位移
s_critical = 2 * s_acc  # 临界总位移（刚好到最大速度后就减速）

# 计算各阶段时间和位移边界
if m <= s_critical:
    # 三角轨迹：无法达到最大速度
    t_total = 2 * np.sqrt(m / a)
    t_acc_actual = t_total / 2  # 加速时间=减速时间
    v_max_actual = np.sqrt(a * m)  # 实际达到的最大速度
    # 生成时间序列
    t = np.linspace(0, t_total, 100)
    # 计算速度
    v = np.where(t <= t_acc_actual, a*t, v_max_actual - a*(t - t_acc_actual))
    # 计算位置
    p = np.where(t <= t_acc_actual, 0.5*a*t**2, 
                 0.5*a*t_acc_actual**2 + v_max_actual*(t - t_acc_actual) - 0.5*a*(t - t_acc_actual)**2)
else:
    # 梯形轨迹：加速→恒速→减速
    s_const = m - 2*s_acc  # 恒速阶段位移
    t_const = s_const / vf  # 恒速时间
    t_total = 2*t_acc + t_const  # 总时间
    t_start_dec = t_acc + t_const  # 开始减速的时间
    # 生成时间序列
    t = np.linspace(0, t_total, 100)
    # 计算速度
    v = np.piecewise(t, 
                    [t <= t_acc, 
                     (t > t_acc) & (t <= t_start_dec), 
                     t > t_start_dec],
                    [lambda x: a*x,
                     vf,
                     lambda x: vf - a*(x - t_start_dec)])
    # 计算位置
    p = np.piecewise(t,
                    [t <= t_acc,
                     (t > t_acc) & (t <= t_start_dec),
                     t > t_start_dec],
                    [lambda x: 0.5*a*x**2,
                     lambda x: s_acc + vf*(x - t_acc),
                     lambda x: s_acc + vf*t_const + vf*(x - t_start_dec) - 0.5*a*(x - t_start_dec)**2])

# 输出关键时间点
print(f"加速到最大速度的时间: {t_acc if m > s_critical else t_acc_actual:.2f} s")
print(f"开始减速的时间: {t_start_dec if m > s_critical else t_acc_actual:.2f} s")
print(f"到达目标角度的总时间: {t_total:.2f} s")

# 绘图：时间-速度图
plt.figure(figsize=(10, 4))
plt.plot(t, v, label='速度', color='b')
plt.xlabel('时间 (s)')
plt.ylabel('速度 (deg/s)')
plt.title('机械臂速度-时间曲线')
plt.grid(True)
plt.legend()

# 绘图：时间-位置图
plt.figure(figsize=(10, 4))
plt.plot(t, p, label='位置', color='r')
plt.axhline(y=m, color='g', linestyle='--', label='目标角度')
plt.xlabel('时间 (s)')
plt.ylabel('角度 (deg)')
plt.title('机械臂位置-时间曲线')
plt.grid(True)
plt.legend()

plt.show()

代码说明 #

1. 参数计算：先通过公式算出加速时间、临界位移等核心参数，判断轨迹类型；
2. 时间序列生成：生成连续的时间轴，避免手动拼接数组的混乱；
3. 速度/位置计算：用np.where或np.piecewise根据时间区间计算对应值，保证轨迹连续无超调；
4. 关键时间输出：根据轨迹类型输出加速到最大速度的时间、开始减速的时间、总时间。

错误点修正 #

• 原代码手动分配位移的逻辑错误：直接用n=m/2分配加速/减速位移仅适用于三角轨迹，梯形轨迹中加速/减速位移固定为s_acc，剩余位移由恒速阶段完成；
• 避免了数组拼接的误差：通过连续时间轴计算每个点的速度和位置，保证轨迹平滑且准确到达目标角度。

内容的提问来源于stack exchange，提问作者cat_herder