《Python机器人基础原理》第二十一篇：机械臂仿真——点位搬运任务，采用三次多项式完成关节空间插值，生成关节角度随时间变化序列
1. 实际工业应用场景
周五上午，某3C电子装配车间的上下料工位。一台3-DOF垂直关节机械臂（基座偏航+肘部俯仰+末端旋转）正在执行PCB板从进料皮带到测试工位的搬运任务。机器人构型：大臂长600 mm，小臂长450 mm，末端旋转轴位于小臂末端。连杆质量：大臂5.2 kg（质心距基座0.28 m），小臂3.1 kg（质心距肘部0.20 m），末端夹具1.5 kg（质心距肘部0.12 m，质心到旋转轴线横向偏置0.02 m）。关节额定扭矩：基座偏航轴60 N·m，肘部俯仰轴180 N·m，关节最大角速度限制：基座偏航2.5 rad/s、肘部俯仰2.0 rad/s、末端旋转3.0 rad/s；最大角加速度限制：基座偏航5.0 rad/s²、肘部俯仰4.0 rad/s²、末端旋转6.0 rad/s²。
调试工程师小李把示教器递给旁边的新人小赵："你试试让机器人从进料位走到测试位。起点：θ₁=0°、θ₂=-30°、θ₃=0°；终点：θ₁=60°、θ₂=-80°、θ₃=90°。走关节插值模式。"
小赵在示教器上设了起点和终点，选了"线性插值"，点下执行。机器人动起来了——但走到一半突然抖了一下，末端夹具上的PCB板差点飞出去。
小李一把按住急停："停。你用的线性插值，角度变化率是常数，意味着每个关节在起终点瞬间加速度无穷大——速度从零瞬间跳到某个值再瞬间归零，电机得承受理论上的冲击脉冲。那一下抖动就是冲击导致的。"
小赵愣了一下："那应该用什么？"
"三次多项式。给定起点和终点的角度、角速度（通常设为零），多项式自动保证位置和速度连续——也就是C¹连续。但注意，三次多项式的加速度在起终点是不连续的：从静止瞬间跳到一个非零值，加加速度（jerk）在起点是无穷大。这比线性插值好得多——至少速度不再有阶跃——但如果你看加速度曲线，起点有个跳变。这恰好说明了三次的局限：它只能保证C¹，不能保证C²。下一篇我们上五次多项式，把起终点加速度也约束为零。"
小赵在草稿纸上列式子："三次多项式θ(t) = a₀ + a₁t + a₂t² + a₃t³，四个约束解出四个系数。起终点速度为零的话，a₁=0，a₂=3Δq/T²，a₃=-2Δq/T³。速度峰值是1.5|Δq|/T，出现在t=T/2。"
"对。咱们取T=2.0 s。Joint2的Δq = -50° = -0.873 rad，峰值速度=1.5×0.873/2.0 ≈ 0.65 rad/s，峰值加速度=6×0.873/4.0 ≈ 1.31 rad/s²。都在限位以内——速度限2.0，加速度限4.0，余量很足。"
"好。那我写个离线仿真，T=2.0 s，用三次多项式生成θ₁、θ₂、θ₃各自的时间序列，画四张图验证。"
本篇解决的问题：给定一组关节空间点位（起点和终点的角度），采用三次多项式（Cubic Polynomial）完成关节空间插值，生成关节角度、角速度、角加速度随时间变化的连续序列。用于点位搬运任务的离线轨迹验证，确保位置和速度连续（C¹），校验是否超出物理限位。
⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于点位搬运任务的关节空间轨迹生成与可视化。不可直接用于真实机器人控制器、伺服驱动器或装配产线设备。真实工业现场需结合运动控制卡、实时插补、软硬限位保护。代码作者不对直接使用本代码于真实设备造成的任何后果负责。
2. 痛点分析
课程模块 本篇对应的工程痛点
第6章 轨迹规划与控制 → 6.1 关节空间轨迹规划 点位搬运需要位置和速度连续，线性插值导致速度阶跃→理论加速度无穷大→机械冲击
第6章 轨迹规划与控制 → 6.2 多项式插值 三次多项式提供C¹连续（位置、速度），满足点位运动基本平滑性需求；加速度在起终点有阶跃（C¹但不C²）
第5章 机器人关节驱动与控制 → 5.1 伺服驱动基础 电机有最大转速和最大加速度限制，轨迹需离线校核是否超限
本篇代码解决 L2 离线轨迹规划层。输入：起点/终点关节角度、运动时间T=2.0 s、采样点数。输出：各关节的θ(t)、θ̇(t)、θ̈(t)时间序列。代码不是设备实时控制器，不做伺服闭环。它做的事情是：给定起点终点，用三次多项式生成C¹连续轨迹，校验连续性和物理可行性。
不覆盖：笛卡尔空间直线/圆弧插补、五次多项式/梯形速度规划、B样条、实时插补、多轴联动、6轴完整轨迹。五次多项式（C²连续）留待下一篇。
3. 核心逻辑讲解
物理直觉
你开车从A点停到B点。线性插值就像一脚油门踩到底然后急刹——速度瞬间从零跳到满速再瞬间归零，车会猛冲再猛停。三次多项式的做法是：缓缓加速到某个速度再缓缓减速到零，速度曲线是平滑的S形。但注意——加速度从起步瞬间就不是零（你一踩油门就有加速度），在停车瞬间也跳回零。所以三次多项式保证速度不断裂（C¹），但加速度在起终点有跳变（不是C²）。想要加速度也连续，得上五次多项式。
业务逻辑 → 代码映射
输入：起点角度q_start、终点角度q_end、运动时间T=2.0 s、采样点数N=100
↓
步骤1：对每个关节，建立三次多项式 θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³
↓
步骤2：代入约束条件解出系数（线性方程组通解，支持非零起终点速度）：
- θ(0) = q_start → a₀ = q_start
- θ(T) = q_end → a₀ + a₁·T + a₂·T² + a₃·T³ = q_end
- θ'(0) = v_start → a₁ = v_start
- θ'(T) = v_end → a₁ + 2·a₂·T + 3·a₃·T² = v_end
↓
步骤3（起终点速度为零的特例）：
a₀ = q_start, a₁ = 0, a₂ = 3Δq/T², a₃ = -2Δq/T³
↓
步骤4：在N个采样点上计算 θ(t), θ'(t) = 2a₂t + 3a₃t², θ''(t) = 2a₂ + 6a₃t
↓
步骤5：解析计算峰值（非采样近似）：
速度峰值 = 1.5|Δq|/T（t=T/2）
加速度极值 = max(|2a₂|, |2a₂+6a₃T|)（边界值）
↓
步骤6：校验位置/速度/加速度三重限位，输出4张图表
关键公式文字翻译
三次多项式：θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³。a₀是起点角度，a₁是起点速度，a₂和a₃由终点约束决定。
系数求解（通解）：建立线性方程组求解a₂、a₃，支持任意起终点速度。本篇特例（起终点速度为零）：a₀=q_start，a₁=0，a₂=3Δq/T²，a₃=-2Δq/T³。
速度：θ̇(t) = 2a₂t + 3a₃t²。起终点为零，中间在t=T/2达到峰值1.5|Δq|/T。
加速度：θ̈(t) = 2a₂ + 6a₃t。从2a₂线性变化到2a₂+6a₃T。在t=0处加速度跳变到2a₂（非零），在t=T处跳变回零——这就是C¹但不C²的数学表现。
单位：角度 rad，角速度 rad/s，角加速度 rad/s²，时间 s。
4. OOP代码实现
技术栈
严格限定：numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。
项目结构树
point_to_point_sim/
├── run_point_to_point_sim.py
├── requirements.txt
├── trajectory/
│ ├── __init__.py
│ ├── models.py
│ ├── cubic_interpolator.py
│ └── visualize.py
├── tests/
│ ├── __init__.py
│ └── test_point_to_point_sim.py
└── results/
核心源码
"trajectory/models.py" — dataclass数据模型
"""
3-DOF垂直关节机械臂数据模型
点位搬运仿真：三次多项式关节空间插值
构型说明：基座偏航(θ1, 竖直轴) + 肘部俯仰(θ2, 水平轴) + 末端旋转(θ3)
物理声明：
- 本篇仅做轨迹规划，不涉及动力学计算
- 关节限位用于校验插值结果是否越界
- 速度/加速度限制用于校验轨迹可行性
"""
from dataclasses import dataclass, field
from typing import List, Tuple, Optional
import numpy as np
@dataclass
class JointWaypoint:
"""
单个关节的单个路点
angle: 关节角度 (rad)
velocity: 关节角速度 (rad/s)，设为零表示停靠
"""
angle: float
velocity: float = 0.0
joint_name: str = ""
def __post_init__(self):
if not np.isfinite(self.angle):
raise ValueError(f"JointWaypoint: 角度必须为有限数值")
if not np.isfinite(self.velocity):
raise ValueError(f"JointWaypoint: 速度必须为有限数值")
@dataclass
class JointLimit:
"""单关节硬件限位"""
min_val: float
max_val: float
joint_name: str = ""
max_velocity: float = 0.0 # 最大角速度 (rad/s)
max_acceleration: float = 0.0 # 最大角加速度 (rad/s²)
def __post_init__(self):
if self.min_val >= self.max_val:
raise ValueError(
f"JointLimit: min_val({self.min_val})必须严格小于"
f"max_val({self.max_val})")
if not np.isfinite(self.min_val) or not np.isfinite(self.max_val):
raise ValueError("JointLimit: 限位值必须为有限数值")
def contains(self, value: float) -> bool:
return (self.min_val - 1e-10) <= value <= (self.max_val + 1e-10)
def velocity_ok(self, vel: float) -> bool:
if self.max_velocity <= 0:
return True
return abs(vel) <= self.max_velocity + 1e-10
def acceleration_ok(self, accel: float) -> bool:
if self.max_acceleration <= 0:
return True
return abs(accel) <= self.max_acceleration + 1e-10
@dataclass
class JointLimitSet:
"""3-DOF机械臂关节限位集合"""
limits: List[JointLimit]
def __post_init__(self):
if len(self.limits) != 3:
raise ValueError(f"需要3个关节限位, 实际{len(self.limits)}个")
@classmethod
def default_3dof_arm(cls):
"""
默认3-DOF机械臂限位
θ1: ±170°, 最大速度2.5 rad/s, 最大加速度5.0 rad/s²
θ2: ±140°, 最大速度2.0 rad/s, 最大加速度4.0 rad/s²
θ3: ±180°, 最大速度3.0 rad/s, 最大加速度6.0 rad/s²
"""
pi = 3.141592653589793
return cls(limits=[
JointLimit(-170 * pi / 180, 170 * pi / 180, 'joint1_base_yaw',
2.5, 5.0),
JointLimit(-140 * pi / 180, 140 * pi / 180, 'joint2_elbow_pitch',
2.0, 4.0),
JointLimit(-180 * pi / 180, 180 * pi / 180, 'joint3_end_rotate',
3.0, 6.0),
])
def check_all_positions(self, joint_values: List[float]) -> Tuple[bool, List[int]]:
if len(joint_values) != 3:
raise ValueError(f"需要3个关节值, 实际{len(joint_values)}个")
all_pass = True
violation_indices = []
for i, (val, limit) in enumerate(zip(joint_values, self.limits)):
if not limit.contains(val):
all_pass = False
violation_indices.append(i)
return all_pass, violation_indices
def check_all_velocities(self, velocities: List[float]) -> Tuple[bool, List[int]]:
if len(velocities) != 3:
raise ValueError(f"需要3个速度值, 实际{len(velocities)}个")
all_pass = True
violation_indices = []
for i, (vel, limit) in enumerate(zip(velocities, self.limits)):
if not limit.velocity_ok(vel):
all_pass = False
violation_indices.append(i)
return all_pass, violation_indices
def check_all_accelerations(self, accelerations: List[float]) -> Tuple[bool, List[int]]:
if len(accelerations) != 3:
raise ValueError(f"需要3个加速度值, 实际{len(accelerations)}个")
all_pass = True
violation_indices = []
for i, (accel, limit) in enumerate(zip(accelerations, self.limits)):
if not limit.acceleration_ok(accel):
all_pass = False
violation_indices.append(i)
return all_pass, violation_indices
@dataclass
class RobotParams:
"""
3-DOF机械臂参数（本篇仅用于轨迹规划，不涉及动力学）
"""
joint_names: List[str] = field(default_factory=lambda: [
'joint1_base_yaw', 'joint2_elbow_pitch', 'joint3_end_rotate'
])
link_lengths: List[float] = field(default_factory=lambda: [0.600, 0.450, 0.0])
@classmethod
def default_3dof_arm(cls):
return cls()
"trajectory/cubic_interpolator.py" — 三次多项式插值器
"""
三次多项式关节空间轨迹插值器
θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³
约束：θ(0)=q_start, θ(T)=q_end, θ'(0)=v_start, θ'(T)=v_end
连续性说明：三次多项式保证C¹连续（位置和速度连续），
但加速度在段边界有阶跃（不是C²）。起终点加速度非零跳变。
"""
import numpy as np
from trajectory.models import JointWaypoint, JointLimitSet
class CubicPolynomialSegment:
"""
单个关节的单个三次多项式段
系数: θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³
"""
def __init__(self, q_start: float, q_end: float, duration: float,
qd_start: float = 0.0, qd_end: float = 0.0):
if duration <= 0:
raise ValueError(f"运动时间必须大于零, 实际{duration}")
if not np.isfinite(q_start) or not np.isfinite(q_end):
raise ValueError("起点/终点角度必须为有限数值")
if not np.isfinite(qd_start) or not np.isfinite(qd_end):
raise ValueError("起点/终点速度必须为有限数值")
self.q_start = q_start
self.q_end = q_end
self.duration = duration
self.qd_start = qd_start
self.qd_end = qd_end
# 求解系数（线性方程组通解，支持非零起终点速度）
# θ(0) = a₀ = q_start
# θ'(0) = a₁ = qd_start
# θ(T) = a₀ + a₁·T + a₂·T² + a₃·T³ = q_end
# θ'(T) = a₁ + 2·a₂·T + 3·a₃·T² = qd_end
self.a0 = q_start
self.a1 = qd_start
T = duration
mat = np.array([[T**2, T**3],
[2*T, 3*T**2]], dtype=float)
rhs = np.array([q_end - self.a0 - self.a1 * T,
qd_end - self.a1], dtype=float)
try:
coeffs = np.linalg.solve(mat, rhs)
except np.linalg.LinAlgError:
raise ValueError(f"三次多项式系数矩阵奇异, duration={duration}")
self.a2 = coeffs[0]
self.a3 = coeffs[1]
def evaluate(self, t: float) -> Tuple[float, float, float]:
"""计算 t 时刻的角度、角速度、角加速度"""
if t < -1e-12 or t > self.duration + 1e-12:
raise ValueError(f"时间t={t}超出范围[0, {self.duration}]")
t = max(0.0, min(t, self.duration))
theta = self.a0 + self.a1 * t + self.a2 * t**2 + self.a3 * t**3
theta_dot = self.a1 + 2 * self.a2 * t + 3 * self.a3 * t**2
theta_ddot = 2 * self.a2 + 6 * self.a3 * t
return theta, theta_dot, theta_ddot
def max_velocity(self) -> float:
"""
计算该段的最大绝对速度（解析解）
速度函数是二次函数 θ'(t) = a₁ + 2a₂t + 3a₃t²
极值点出现在 t = -a₂/(3a₃)（如果在[0,T]内）或边界
特例（零起终点速度）：峰值 = 1.5|Δq|/T，出现在 t=T/2
"""
candidates = [0.0, self.duration]
if abs(self.a3) > 1e-12:
t_extreme = -self.a2 / (3 * self.a3)
if 0 < t_extreme < self.duration:
candidates.append(t_extreme)
max_v = 0.0
for t in candidates:
_, v, _ = self.evaluate(t)
max_v = max(max_v, abs(v))
return max_v
def max_acceleration(self) -> float:
"""
计算该段的最大绝对加速度（解析解）
加速度是线性的 θ''(t) = 2a₂ + 6a₃t，极值在边界
"""
_, _, a_start = self.evaluate(0.0)
_, _, a_end = self.evaluate(self.duration)
return max(abs(a_start), abs(a_end))
def peak_velocity_formula(self) -> float:
"""
零起终点速度时的解析峰值速度：1.5|Δq|/T
用于校验max_velocity()计算结果
"""
if abs(self.qd_start) < 1e-12 and abs(self.qd_end) < 1e-12:
delta_q = self.q_end - self.q_start
return 1.5 * abs(delta_q) / self.duration
return self.max_velocity()
class CubicJointTrajectory:
"""
多关节三次多项式轨迹生成器
对每个关节独立生成三次多项式插值序列
构造时校验起终点路点是否在位置限位内（尽早失败）
"""
def __init__(self, start_waypoints: List[JointWaypoint],
end_waypoints: List[JointWaypoint],
duration: float, n_points: int = 100,
limits: JointLimitSet = None):
if len(start_waypoints) != 3 or len(end_waypoints) != 3:
raise ValueError("需要3个关节的起点和终点路点")
self.duration = duration
self.n_points = n_points
self.times = np.linspace(0, duration, n_points)
self.limits = limits
# 构造时尽早校验起终点位置限位
if limits is not None:
start_angles = [wp.angle for wp in start_waypoints]
end_angles = [wp.angle for wp in end_waypoints]
start_ok, start_violations = limits.check_all_positions(start_angles)
if not start_ok:
err_msgs = []
for vi in start_violations:
err_msgs.append(
f"起点关节'{limits.limits[vi].joint_name}' "
f"角度={start_angles[vi]:.3f}超出限位"
f"[{limits.limits[vi].min_val:.3f}, "
f"{limits.limits[vi].max_val:.3f}]"
)
raise ValueError("；".join(err_msgs))
end_ok, end_violations = limits.check_all_positions(end_angles)
if not end_ok:
err_msgs = []
for vi in end_violations:
err_msgs.append(
f"终点关节'{limits.limits[vi].joint_name}' "
f"角度={end_angles[vi]:.3f}超出限位"
f"[{limits.limits[vi].min_val:.3f}, "
f"{limits.limits[vi].max_val:.3f}]"
)
raise ValueError("；".join(err_msgs))
self.segments = []
for i in range(3):
seg = CubicPolynomialSegment(
q_start=start_waypoints[i].angle,
q_end=end_waypoints[i].angle,
duration=duration,
qd_start=start_waypoints[i].velocity,
qd_end=end_waypoints[i].velocity
)
self.segments.append(seg)
# 批量计算
self.q_array = np.zeros((n_points, 3))
self.q_dot_array = np.zeros((n_points, 3))
self.q_ddot_array = np.zeros((n_points, 3))
for i, t in enumerate(self.times):
for j in range(3):
q, qd, qdd = self.segments[j].evaluate(t)
self.q_array[i, j] = q
self.q_dot_array[i, j] = qd
self.q_ddot_array[i, j] = qdd
def validate_limits(self, limits: JointLimitSet) -> Tuple[bool, List[str]]:
"""
校验整条轨迹是否满足限位、速度限、加速度限
返回 (all_pass, messages)
"""
all_pass = True
messages = []
for i in range(self.n_points):
t = self.times[i]
q_vals = self.q_array[i, :].tolist()
qd_vals = self.q_dot_array[i, :].tolist()
qdd_vals = self.q_ddot_array[i, :].tolist()
# 位置校验
pos_ok, pos_violations = limits.check_all_positions(q_vals)
if not pos_ok:
all_pass = False
for vi in pos_violations:
messages.append(
f"t={t:.3f}s: 关节'{limits.limits[vi].joint_name}' "
f"角度={q_vals[vi]:.3f}超出限位"
f"[{limits.limits[vi].min_val:.3f}, "
f"{limits.limits[vi].max_val:.3f}]"
)
# 速度校验
vel_ok, vel_violations = limits.check_all_velocities(qd_vals)
if not vel_ok:
all_pass = False
for vi in vel_violations:
messages.append(
f"t={t:.3f}s: 关节'{limits.limits[vi].joint_name}' "
f"角速度={qd_vals[vi]:.3f}超出限制"
f"{limits.limits[vi].max_velocity:.3f}"
)
# 加速度校验
acc_ok, acc_violations = limits.check_all_accelerations(qdd_vals)
if not acc_ok:
all_pass = False
for vi in acc_violations:
messages.append(
f"t={t:.3f}s: 关节'{limits.limits[vi].joint_name}' "
f"角加速度={qdd_vals[vi]:.3f}超出限制"
f"{limits.limits[vi].max_acceleration:.3f}"
)
return all_pass, messages
def get_peak_values(self) -> dict:
"""返回各关节的峰值角度、速度、加速度"""
return {
'q_peak': np.max(np.abs(self.q_array), axis=0),
'qd_peak': np.max(np.abs(self.q_dot_array), axis=0),
'qdd_peak': np.max(np.abs(self.q_ddot_array), axis=0),
}
@classmethod
def from_angle_pairs(cls, q_start: List[float], q_end: List[float],
duration: float, n_points: int = 100,
limits: JointLimitSet = None):
"""
便捷工厂方法：直接从角度列表创建轨迹
"""
if len(q_start) != 3 or len(q_end) != 3:
raise ValueError("需要3个关节的起点和终点角度")
start_waypoints = [
JointWaypoint(q_start[0], 0.0, 'joint1_base_yaw'),
JointWaypoint(q_start[1], 0.0, 'joint2_elbow_pitch'),
JointWaypoint(q_start[2], 0.0, 'joint3_end_rotate'),
]
end_waypoints = [
JointWaypoint(q_end[0], 0.0, 'joint1_base_yaw'),
JointWaypoint(q_end[1], 0.0, 'joint2_elbow_pitch'),
JointWaypoint(q_end[2], 0.0, 'joint3_end_rotate'),
]
return cls(start_waypoints, end_waypoints, duration, n_points, limits)
"trajectory/visualize.py" — 可视化模块
"""
点位搬运仿真结果可视化：4张核心图表
"""
import numpy as np
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False
class Visualizer:
def __init__(self, results_dir="results"):
self.results_dir = results_dir
import os
os.makedirs(results_dir, exist_ok=True)
def plot_joint_angles(self, times, q_array):
"""
① 关节角度曲线：验证从起点平滑走到终点
"""
fig, axes = plt.subplots(3, 1, figsize=(10, 9), sharex=True)
joint_names = ['Joint1 (Base Yaw)', 'Joint2 (Elbow Pitch)', 'Joint3 (End Rotate)']
colors = ['steelblue', 'red', 'green']
for j_idx, (ax, name, color) in enumerate(zip(axes, joint_names, colors)):
ax.plot(times, q_array[:, j_idx] * 180 / np.pi, '-',
color=color, linewidth=1.5)
ax.set_ylabel(f'{name}\nAngle (deg)')
ax.set_title(f'{name.split("(")[0].strip()} — Angle Profile')
ax.grid(True, alpha=0.3)
axes[2].set_xlabel('Time (s)')
plt.tight_layout()
path = f"{self.results_dir}/01_joint_angles.png"
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_joint_velocities(self, times, q_dot_array):
"""
② 关节角速度曲线：验证起终点为零、中间平滑（C¹连续）
"""
fig, axes = plt.subplots(3, 1, figsize=(10, 9), sharex=True)
joint_names = ['Joint1 (Base Yaw)', 'Joint2 (Elbow Pitch)', 'Joint3 (End Rotate)']
colors = ['steelblue', 'red', 'green']
for j_idx, (ax, name, color) in enumerate(zip(axes, joint_names, colors)):
ax.plot(times, q_dot_array[:, j_idx], '-',
color=color, linewidth=1.5)
ax.axhline(y=0, color='black', linestyle='--', linewidth=0.8, alpha=0.5)
ax.set_ylabel(f'{name}\nVelocity (rad/s)')
ax.set_title(f'{name.split("(")[0].strip()} — Velocity Profile (C¹ Continuous)')
ax.grid(True, alpha=0.3)
axes[2].set_xlabel('Time (s)')
plt.tight_layout()
path = f"{self.results_dir}/02_joint_velocities.png"
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_joint_accelerations(self, times, q_ddot_array):
"""
③ 关节角加速度曲线：验证线性变化，注意起终点有阶跃（C¹但不C²）
"""
fig, axes = plt.subplots(3, 1, figsize=(10, 9), sharex=True)
joint_names = ['Joint1 (Base Yaw)', 'Joint2 (Elbow Pitch)', 'Joint3 (End Rotate)']
colors = ['steelblue', 'red', 'green']
for j_idx, (ax, name, color) in enumerate(zip(axes, joint_names, colors)):
ax.plot(times, q_ddot_array[:, j_idx], '-',
color=color, linewidth=1.5)
ax.axhline(y=0, color='black', linestyle='--', linewidth=0.8, alpha=0.5)
# 标注起点加速度跳变
ax.axvline(x=0, color='orange', linestyle=':', linewidth=1.0, alpha=0.7)
ax.axvline(x=times[-1], color='orange', linestyle=':', linewidth=1.0, alpha=0.7)
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！
