《Python机器人基础原理》第二十七篇：多工位连续搬运——设置多组关节路点，完成多段连续关节空间多项式轨迹插值

 

1. 实际工业应用场景

 

周一早晨，某3C电子装配产线的上下料工位。一台4-DOF俯仰型关节臂+升降轴机械手（基座偏航θ₁ + 大臂俯仰θ₂ + 小臂俯仰θ₃ + 末端升降d₄）负责在三个工位之间连续转运手机中框零件。机器人构型参数：大臂长350 mm、质量2.8 kg（质心距基座0.18 m），小臂长250 mm、质量1.6 kg（质心距肘部0.12 m），升降轴行程150 mm、末端电磁夹爪0.5 kg。关节限位：θ₁±180°、最大角速度3.0 rad/s、最大角加速度8.0 rad/s²；θ₂±150°、最大角速度2.5 rad/s、最大角加速度6.0 rad/s²；θ₃±180°、最大角速度3.5 rad/s、最大角加速度10.0 rad/s²；d₄行程0~150 mm、最大线速度0.3 m/s、最大线加速度0.8 m/s²。

 

产线节拍要求：从取料工位A→中转工位B→装配工位C→待机位D，完成一个完整循环不超过6.0 s，且工位之间切换时机械臂不能停顿。

 

小赵拿到任务书：4个路点（取料→中转→装配→待机），每个路点的关节角度已知，需要生成连续平滑的多段轨迹。他翻出上篇写的单段五次多项式代码，试着把4个路点拆成3段独立轨迹，每段之间重新从静止启动、到静止停止，每段给3秒。

 

小李端着咖啡走过来："你这每段3秒，3段就是9秒。产线节拍卡的是6秒，你超了50%。"

 

小赵一拍脑门："……对。那我压缩一下，A→B给2秒，B→C给1.5秒，C→D给2.5秒，加起来正好6秒。"

 

"时间分配合理了。但你每段都从零速启动、零速停止？"

 

"对，上篇就是这么做的。"

 

"那你想过没有——A→B段结束时要停下来，B→C段又要重新加速。光加减速就浪费了多少时间？而且每段之间速度硬跳到零再跳上来，伺服驱动器每次都要重新建立位置环，末端夹爪上的零件会晃。"

 

小赵想了想："那让段与段之间速度不归零？"

 

"对。多段连续轨迹，段与段之间位置连续是基本要求，速度连续才是关键。你需要在每个中间路点指定速度——或者让算法自动计算中间路点的速度，然后每段用满足边界条件的多项式拼接。"

 

本篇解决的问题：给定N个关节空间路点（位置），为每个中间路点自动计算连续速度（采用三次样条的速度边界条件），然后相邻路点之间用五次多项式插值，保证段间位置和速度连续，输出各关节多段连续轨迹的位置、速度、加速度曲线，全程校验是否超限。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于离线多段轨迹规划校验。不可直接用于真实机器人控制器、伺服驱动器或产线设备。真实工业现场需结合实时插补、前瞻控制、软限位保护。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

课程模块 本篇对应的工程痛点

第6章 轨迹规划与控制 → 6.3 多项式轨迹规划 多段独立轨迹段间速度不连续→伺服频繁加减速→节拍损失+末端振动

第6章 轨迹规划与控制 → 6.4 样条插值 中间路点速度未知→需要自动计算（三次样条边界条件）→保证段间连续

第6章 轨迹规划与控制 → 6.2 速度规划 多段连续轨迹需全程校验速度/加速度是否超出硬件限位

 

本篇代码解决 L2 离线多段轨迹规划校验层。输入：N个关节路点位置、每段运动时间、采样频率。通过三次样条计算中间路点速度，相邻路点间用五次多项式插值，逐点计算位置/速度/加速度，全程校验限位。输出：多段连续轨迹曲线、段间连续性报告、限位校验报告。

 

不覆盖：在线轨迹修正（传感器反馈实时调整）、高阶连续（加加速度jerk连续→七次多项式）、时间最优分配（自动计算每段最短时间）、笛卡尔空间多段规划。

 

3. 核心逻辑讲解

 

物理直觉

 

多段连续搬运就像开车走城市快速路，中间经过多个匝道出入口。如果每到一个出入口你就把车完全刹停再重新起步，后面的车会按喇叭，你自己也浪费时间。正确的做法是：提前规划好每个匝道的速度，经过时不停车，只是稍微调整油门深浅。

 

单段五次多项式是"从家开车到公司，起点终点都停车"。多段连续轨迹是"从家出发，经过加油站、超市、朋友家，最后到公司——中间不停，但每个途经点的速度要提前算好，保证进出每个路段时车速匹配"。

 

中间路点速度怎么定？

 

中间路点的速度不是随便给的。工程上最常用的方法是三次样条插值：假设中间路点的加速度是连续的（即整条轨迹的二阶导数连续），利用这个条件和已知的起点/终点速度（通常为零），可以唯一确定每个中间路点的速度。

 

这本质上是求解一个三对角线性方程组。对每个内部路点i，加速度连续条件展开后得到方程：

 

t_i · v{i-1} + 2(t{i-1} + t_i) · v_i + t{i-1} · v{i+1} = 3[ (Δq_i/t_i)·t{i-1} + (Δq{i-1}/t_{i-1})·t_i ]

 

其中t_i是第i段的持续时间，Δq_i是第i段的关节角度变化量。左侧v{i-1}的系数是t_i（当前段时长），v_i的系数是2(t{i-1}+t_i)，v{i+1}的系数是t{i-1}（前一段时长）。右侧是位置差分与时间的交叉加权组合。所有内部路点的方程联立构成三对角矩阵，用托马斯算法（Thomas algorithm）求解。

 

本篇的混合方案：样条速度 + 五次多项式（加速度边界为零）

 

设计选择说明：样条计算给出路点速度后，标准做法是每段用三次Hermite插值（位置+速度共4个边界条件，段间C²连续）。但本篇选择每段用五次多项式，并强制起止加速度为零。这是一个有意为之的工程折中：

 

- 好处：五次多项式求解简单，加速度在路点处被钳制为零，波形更"温和"，计算量小

- 代价：样条推导假设加速度连续且取自然值，但零加速度边界人为覆盖了这个结果。加速度在路点处确实是连续的（两侧都是0），但这个"连续"是钳制出来的，不是样条推导的最优值。每段五次多项式为了同时满足非零端速度和零端加速度，内部加速度波形会比Hermite方案摆动更大，加速度峰值偏高

 

这是离线仿真中常见的简化策略。工业现场若对加速度波形敏感（如精密装配），应采用三次Hermite或七次多项式。

 

业务逻辑 → 代码映射

 

输入：路点位置 waypoints[n_wp][n_joints]、每段时间 segment_times[n_seg]、采样数 N

  ↓

对每个关节 j = 0,1,2（旋转关节）+ j=3（升降轴，单独处理单位）：

  步骤1：三次样条计算中间路点速度

    构建三对角矩阵（系数如上）

    边界条件：v[0] = 0, v[n_wp-1] = 0

    托马斯算法求解 → 得到所有路点速度 v[i]

  ↓

  步骤2：相邻路点间五次多项式插值

    对每段 s = 0..n_seg-1：

      边界条件：q(0) = wp[s], q(T) = wp[s+1]

                q'(0) = v[s], q'(T) = v[s+1]

                q''(0) = 0, q''(T) = 0

      求解6个系数 → 逐点评估

  ↓

步骤3：拼接所有段，全程校验限位（旋转关节用rad/s，升降轴用m/s）

步骤4：段间连续性回归检查（验证浮点拼接精度）

  ↓

输出：连续多段位置/速度/加速度曲线、4张图表

 

关键公式文字翻译

 

五次多项式系数求解（连续边界版）：与单段不同，这里起点和终点的速度不再为零，而是由样条计算给出。所以a₁ = v_start，a₂ = a_start/2（这里a_start=0）。剩下a₃、a₄、a₅由终点位置、终点速度、终点加速度（为零）三个条件联立求解3×3矩阵。

 

托马斯算法：三对角矩阵的前向消元将下对角线元素消去，变成上双对角矩阵；然后回代求解。时间复杂度O(n)，比高斯消元O(n³)高效得多，适合路点数量较多的场景。

 

4. OOP代码实现

 

技术栈

 

严格限定：numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。

 

项目结构树

 

multi_segment_trajectory_sim/

├── run_multi_segment_trajectory_sim.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py

│ ├── spline_velocity.py

│ ├── trajectory_generator.py

│ └── visualize_trajectory.py

├── tests/

│ ├── __init__.py

│ └── test_multi_segment_trajectory_sim.py

└── results/

 

核心源码

 

"robot/models.py" — 数据模型

 

"""

机器人数据模型：关节参数、限位、路点定义

本篇包含3个旋转关节(rad) + 1个平移升降轴(m)，单位分离处理

"""

 

 

from dataclasses import dataclass, field

from typing import List

 

 

@dataclass

class JointLimit:

    """单关节限位结构体"""

    joint_name: str

    min_value: float # 最小位置（旋转关节为rad，平移轴为m）

    max_value: float # 最大位置

    max_velocity: float # 最大速度（旋转关节为rad/s，平移轴为m/s）

    max_acceleration: float # 最大加速度（旋转关节为rad/s²，平移轴为m/s²）

    is_revolute: bool = True # True=旋转关节, False=平移轴

 

 

@dataclass

class JointLimitSet:

    """关节限位集合，4个关节：θ₁/θ₂/θ₃(旋转) + d₄(平移)"""

    limits: List[JointLimit] = field(default_factory=list)

 

    def check_all_positions(self, values: List[float]) -> tuple:

        """校验一组关节位置是否全部在限位内"""

        violations = []

        for i, val in enumerate(values):

            if i < len(self.limits):

                if val < self.limits[i].min_value or val > self.limits[i].max_value:

                    violations.append(i)

        return len(violations) == 0, violations

 

    def check_all_velocities(self, velocities: List[float]) -> tuple:

        """校验一组速度是否全部在限位内"""

        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, violations

 

    def check_all_accelerations(self, accelerations: List[float]) -> tuple:

        """校验一组加速度是否全部在限位内"""

        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, violations

 

 

@dataclass

class Waypoint:

    """单个路点的关节位置（前3个为弧度，第4个为米）"""

    positions: List[float]

    label: str = ""

 

 

@dataclass

class SegmentTime:

    """每段运动时间分配"""

    duration: float

 

"robot/spline_velocity.py" — 三次样条速度计算

 

"""

三次样条速度计算器

通过加速度连续条件求解中间路点速度

求解三对角线性方程组，使用托马斯算法（Thomas algorithm）

"""

 

 

import numpy as np

from typing import List

 

 

class CubicSplineVelocityCalculator:

    """

    对每个关节独立计算路点速度

    边界条件：起点和终点速度为零

    内部路点满足加速度连续（二阶导数连续）

    三对角矩阵系数：t_i·v_{i-1} + 2(t_{i-1}+t_i)·v_i + t_{i-1}·v_{i+1} = RHS

    """

 

    def __init__(self, waypoint_positions: List[List[float]],

                 segment_times: List[float]):

        """

        waypoint_positions: shape [n_waypoints, n_joints]

        segment_times: shape [n_segments], n_segments = n_waypoints - 1

        """

        self.waypoints = np.array(waypoint_positions)

        self.segment_times = np.array(segment_times)

        self.n_waypoints = self.waypoints.shape[0]

        self.n_joints = self.waypoints.shape[1]

        self.n_segments = len(segment_times)

 

        if self.n_waypoints != self.n_segments + 1:

            raise ValueError(

                f"路点数({self.n_waypoints})必须等于段数+1({self.n_segments + 1})"

            )

 

    def compute_velocities(self) -> np.ndarray:

        """为每个关节计算所有路点的速度"""

        velocities = np.zeros((self.n_waypoints, self.n_joints))

        for j in range(self.n_joints):

            velocities[:, j] = self._solve_single_joint(j)

        return velocities

 

    def _solve_single_joint(self, joint_idx: int) -> np.ndarray:

        """

        对单个关节求解三对角系统

        托马斯算法（Thomas algorithm / TDMA）

        系数与文字公式严格一致：

          v_{i-1}系数 = t_i

          v_i系数 = 2(t_{i-1} + t_i)

          v_{i+1}系数 = t_{i-1}

        """

        n = self.n_waypoints

        q = self.waypoints[:, joint_idx]

        t = self.segment_times

 

        # 主对角线a、下对角线b、上对角线c、右侧d

        a = np.zeros(n)

        b = np.zeros(n)

        c = np.zeros(n)

        d = np.zeros(n)

 

        # 边界条件：v[0] = 0, v[n-1] = 0

        a[0] = 1.0

        d[0] = 0.0

        a[n-1] = 1.0

        d[n-1] = 0.0

 

        for i in range(1, n-1):

            a[i] = 2.0 * (t[i-1] + t[i]) # v_i的系数

            b[i] = t[i] # v_{i-1}的系数（下对角线）

            c[i] = t[i-1] # v_{i+1}的系数（上对角线）

            d[i] = 3.0 * (

                (q[i+1] - q[i]) / t[i] * t[i-1]

                + (q[i] - q[i-1]) / t[i-1] * t[i]

            )

 

        # 托马斯算法前向消元

        for i in range(1, n):

            w = b[i] / a[i-1]

            a[i] = a[i] - w * c[i-1]

            d[i] = d[i] - w * d[i-1]

 

        # 回代

        v = np.zeros(n)

        v[n-1] = d[n-1] / a[n-1]

        for i in range(n-2, -1, -1):

            v[i] = (d[i] - c[i] * v[i+1]) / a[i]

 

        return v

 

"robot/trajectory_generator.py" — 多段五次多项式轨迹生成

 

"""

多段五次多项式轨迹生成器

相邻路点间用五次多项式插值，段间位置和速度连续

混合方案：样条速度 + 零加速度边界（工程折中，非最优C²连续）

"""

 

 

import numpy as np

from typing import List, Tuple, Dict

from robot.models import JointLimitSet

from robot.spline_velocity import CubicSplineVelocityCalculator

 

 

class QuinticSegmentTrajectory:

    """

    单段五次多项式轨迹（连续边界版）

    边界条件：起止位置、起止速度（非零）、起止加速度（强制为零）

    """

 

    def __init__(self, start_pos: float, end_pos: float,

                 start_vel: float, end_vel: float,

                 start_acc: float, end_acc: float,

                 duration: float):

        if duration <= 0:

            raise ValueError(f"段时间必须大于0: {duration}")

        self.T = duration

        self.coeffs = self._solve(start_pos, end_pos,

                                  start_vel, end_vel,

                                  start_acc, end_acc)

 

    def _solve(self, q0, qT, v0, vT, a0, aT) -> np.ndarray:

        """求解五次多项式系数"""

        a0_coeff = q0

        a1_coeff = v0

        a2_coeff = a0 / 2.0

 

        T = self.T

        M = 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_coeff + a1_coeff*T + a2_coeff*T**2),

            vT - (a1_coeff + 2*a2_coeff*T),

            aT - (2*a2_coeff)

        ])

 

        try:

            a3_a4_a5 = np.linalg.solve(M, b)

        except np.linalg.LinAlgError:

            raise ValueError(f"段轨迹求解失败：系数矩阵奇异，段时间T={T}可能过小")

 

        return np.array([a0_coeff, a1_coeff, a2_coeff,

                         a3_a4_a5[0], a3_a4_a5[1], a3_a4_a5[2]])

 

    def evaluate(self, t: float) -> Tuple[float, float, float]:

        """计算指定时刻的位置、速度、加速度"""

        if t < 0:

            t = 0.0

        if t > self.T:

            t = self.T

 

        a = 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**3

 

        return pos, vel, acc

 

 

class MultiSegmentTrajectoryGenerator:

    """

    多段连续轨迹生成器

    步骤：1) 三次样条计算中间路点速度 2) 每段五次多项式插值 3) 拼接校验

    """

 

    def __init__(self, joint_limits: JointLimitSet):

        self.limits = joint_limits

 

    def generate(self, waypoint_positions: List[List[float]],

                 segment_times: List[float]) -> Dict:

        """生成多段连续轨迹"""

        waypoints = np.array(waypoint_positions)

        n_waypoints, n_joints = waypoints.shape

        n_segments = len(segment_times)

 

        # 步骤1：三次样条计算路点速度

        spline_calc = CubicSplineVelocityCalculator(

            waypoint_positions, segment_times

        )

        velocities = spline_calc.compute_velocities()

 

        # 步骤2：为每段生成五次多项式

        all_time = []

        all_positions = []

        all_velocities = []

        all_accelerations = []

        all_segment_ids = []

 

        continuity_report = []

 

        current_time_offset = 0.0

 

        for s in range(n_segments):

            T_seg = segment_times[s]

            n_samples = max(int(T_seg * 200), 50)

            t_points = np.linspace(0, T_seg, n_samples)

 

            seg_positions = np.zeros((n_samples, n_joints))

            seg_velocities = np.zeros((n_samples, n_joints))

            seg_accelerations = np.zeros((n_samples, n_joints))

 

            for j in range(n_joints):

                traj = QuinticSegmentTrajectory(

                    start_pos=waypoints[s, j],

                    end_pos=waypoints[s+1, j],

                    start_vel=velocities[s, j],

                    end_vel=velocities[s+1, j],

                    start_acc=0.0,

                    end_acc=0.0,

                    duration=T_seg

                )

 

                for i, t in enumerate(t_points):

                    seg_positions[i, j], seg_velocities[i, j], seg_accelerations[i, j] = traj.evaluate(t)

 

            # 段间连续性回归检查（验证浮点拼接精度）

            if s < n_segments - 1:

                last_idx = n_samples - 1

                next_start_pos = waypoints[s+1, :]

                next_start_vel = velocities[s+1, :]

 

                pos_error = np.max(

                    np.abs(seg_positions[last_idx, :] - next_start_pos)

                )

                vel_error = np.max(

                    np.abs(seg_velocities[last_idx, :] - next_start_vel)

                )

 

                continuity_report.append({

                    'segment': s,

                    'waypoint_index': s+1,

                    'position_error_max': pos_error,

                    'velocity_error_max': vel_error,

                    'position_ok': pos_error < 1e-10,

                    'velocity_ok': vel_error < 1e-10

                })

 

            seg_time = t_points + current_time_offset

            current_time_offset += T_seg

 

            all_time.append(seg_time)

            all_positions.append(seg_positions)

            all_velocities.append(seg_velocities)

            all_accelerations.append(seg_accelerations)

            all_segment_ids.append(np.full(n_samples, s))

 

        # 拼接

        full_time = np.concatenate(all_time)

        full_positions = np.vstack(all_positions)

        full_velocities = np.vstack(all_velocities)

        full_accelerations = np.vstack(all_accelerations)

        full_segment_ids = np.concatenate(all_segment_ids)

 

        # 步骤3：全程限位校验

        velocity_violations = []

        acceleration_violations = []

 

        for i in range(len(full_time)):

            vel_list = full_velocities[i, :].tolist()

            acc_list = full_accelerations[i, :].tolist()

 

            _, v_viol = self.limits.check_all_velocities(vel_list)

            _, a_viol = self.limits.check_all_accelerations(acc_list)

 

            for j in v_viol:

                velocity_violations.append((

                    i, int(full_segment_ids[i]), j,

                    vel_list[j], self.limits.limits[j].max_velocity

                ))

            for j in a_viol:

                acceleration_violations.append((

                    i, int(full_segment_ids[i]), j,

                    acc_list[j], self.limits.limits[j].max_acceleration

                ))

 

        return {

            'time': full_time,

            'positions': full_positions,

            'velocities': full_velocities,

            'accelerations': full_accelerations,

            'segment_ids': full_segment_ids,

            'continuity_report': continuity_report,

            'velocity_violations': velocity_violations,

            'acceleration_violations': acceleration_violations,

            'waypoint_velocities': velocities,

            'n_joints': n_joints,

            'n_segments': n_segments,

            'n_waypoints': n_waypoints

        }

 

"robot/visualize_trajectory.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 TrajectoryVisualizer:

    def __init__(self, results_dir="results"):

        self.results_dir = results_dir

        import os

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_revolute_positions(self, timestamps, positions, segment_ids,

                                waypoint_times, waypoint_positions):

        """

        ① 旋转关节(θ₁/θ₂/θ₃)多段位置曲线+路点标记

        """

        fig, ax = plt.subplots(figsize=(14, 5))

 

        joint_names = ['θ₁ (Base Yaw)', 'θ₂ (Elbow Pitch)', 'θ₃ (Wrist Pitch)']

        colors = ['steelblue', 'orange', 'green']

 

        for j in range(3):

            ax.plot(timestamps, positions[:, j], '-',

                    color=colors[j], linewidth=1.2, label=f'{joint_names[j]}')

 

        for wp_t in waypoint_times:

            ax.axvline(x=wp_t, color='gray', linestyle=':', linewidth=0.8, alpha=0.5)

 

        for wp_t, wp_pos in zip(waypoint_times, waypoint_positions):

            for j in range(3):

                ax.plot(wp_t, wp_pos[j], marker='o', color=colors[j],

                        markersize=4, fillstyle='full')

 

        ax.set_xlabel('Time (s)')

        ax.set_ylabel('Joint Angle (rad)')

        ax.set_title('Multi-Segment Revolute Joint Position Profiles')

        ax.legend(loc='upper right', fontsize=8)

        ax.grid(True, alpha=0.3)

 

        plt.tight_layout()

        path = f"{self.results_dir}/01_revolute_position_curves.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_revolute_velocities(self, timestamps, velocities, segment_ids,

                                  joint_limits, waypoint_times):

        """

        ② 旋转关节多段角速度曲线+限位线

        """

        fig, axes = plt.subplots(3, 1, figsize=(14, 9), sharex=True)

 

        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], '-',

                    color=colors[j], linewidth=1.0, label=f'Actual {joint_names[j]}')

 

            limit = joint_limits.limits[j].max_velocity

            ax.axhline(y=limit, color='red', linestyle='--', linewidth=0.8,

                       label=f'Max (+{limit} rad/s)')

            ax.axhline(y=-limit, color='red', linestyle='--', linewidth=0.8,

                       label=f'Min (-{limit} rad/s)')

            ax.axhline(y=0, color='black', linestyle='-', linewidth=0.3, alpha=0.5)

 

            for wp_t in waypoint_times[1:-1]:

                ax.axvline(x=wp_t, color='gray', linestyle=':', linewidth=0.5, alpha=0.3)

 

            ax.set_ylabel('Angular Velocity\n(rad/s)')

            ax.legend(loc='upper right', fontsize=6)

            ax.grid(True, alpha=0.3)

            ax.set_title(f'{joint_names[j]} Velocity Profile', fontsize=9)

 

        axes[2].set_xlabel('Time (s)')

        plt.tight_layout()

        path = f"{self.results_dir}/02_revolute_velocity_curves.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_revolute_accelerations(self, timestamps, accelerations, segment_ids,

                                     joint_limits, waypoint_times):

        """

        ③ 旋转关节多段角加速度曲线+限位线

        """

        fig, axes = plt.subplots(3, 1, figsize=(14, 9), sharex=True)

 

        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], '-',

                    color=colors[j], linewidth=1.0, label=f'Actual {joint_names[j]}')

 

            limit = joint_limits.limits[j].max_acceleration

            ax.axhline(y=limit, color='red', linestyle='--', linewidth=0.8,

                       label=f'Max (+{limit} rad/s²)')

            ax.axhline(y=-limit, color='red', linestyle='--', linewidth=0.8,

                       label=f'Min (-{limit} rad/s²)')

            ax.axhline(y=0, color='black', linestyle='-', linewidth

利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！