《Python机器人基础原理》第五十四篇：装配工位仿真——给定两段相邻轨迹，校验两段衔接处速度加速度是否连续平滑

 

1. 实际工业应用场景

 

周五上午，某新能源汽车电池模组装配线。一台6-DOF轻型装配机器人（额定负载5 kg，重复定位精度±0.02 mm）正在执行电芯插装任务。工艺路线分两段：第一段是从取料位以直线轨迹移动到装配孔上方20 mm处（慢速精确定位），第二段是从孔上方以螺旋下插轨迹装入电芯（低速恒力控制过渡）。

 

调试工程师小王盯着示教器上的速度曲线，皱了皱眉："老张，你看这段衔接。第一段末端的设定速度是50 mm/s，第二段起始速度也是50 mm/s，数值上一样。但机器人每次走到衔接点都会'顿'一下，电芯插进去的时候外壳轻微刮伤。示教器上加速度曲线在衔接点有个尖峰，跳到了1200 mm/s²，然后瞬间掉到400 mm/s²。"

 

旁边的资深工程师老张凑过来看了一眼："你这俩轨迹段用的什么规划？"

 

"第一段是五次多项式插值，第二段也是五次多项式，但我是分别生成的，衔接点只保证了位置连续，没管速度和加速度。"

 

"那就对了。"老张直起身，"位置连续只是C⁰连续，速度不连续就是C⁰而不是C¹，加速度跳变就是C¹而不是C²。机器人控制器在衔接点检测到速度不连续，会强制减速到零再重新加速——这就是你看到的'顿'。你需要在离线仿真里把两段轨迹拼在一起，校验衔接点的速度、加速度是否连续，如果不连续就重新规划。"

 

"怎么校验？"

 

"把两段轨迹在衔接点处的位置、速度、加速度都算出来，做差。位置差应该严格为零（你保证了），速度差应该小于某个阈值（比如1 mm/s），加速度差也应该小于阈值（比如50 mm/s²）。如果超了，就得调整第二段的边界条件重新生成多项式系数。"

 

"那加速度跳变本身呢？"

 

"加速度跳变在理论上允许（C¹连续就够了），但如果跳变太大，减速器会受到冲击载荷。实际产线上一般要求加速度也尽量连续（C²），至少跳变幅度控制在关节扭矩限值的10%以内。另外——你算的都是关节空间的，末端笛卡尔空间的加速度跳变才是真正刮伤电芯的原因，得用雅可比映射换算过去。"

 

构型约定说明：本篇采用3-DOF平面机械臂简化模型（2个旋转关节+1个移动关节），用于轨迹衔接校验的算法验证。连杆参数：大臂长0.30 m、质量1.2 kg、质心位于0.15 m处、绕质心Z轴惯量0.018 kg·m²；小臂长0.25 m、质量0.8 kg、质心位于0.125 m处、绕质心Z轴惯量0.008 kg·m²；移动关节行程0.15 m。全部参数内置，自包含可复现。通用DH正运动学复用公共模块（

"robot/common/kinematics.py"）。

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于两段相邻轨迹衔接处速度加速度连续性的算法验证。不可直接用于真实机器人控制器或产线轨迹规划。真实工业机器人的轨迹规划需使用厂商专用软件并在控制器中实时执行。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

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

第6章 轨迹规划与控制 → 6.1 关节空间轨迹规划 多项式插值轨迹生成（五次多项式）

第6章 轨迹规划与控制 → 6.2 笛卡尔空间轨迹规划 两段轨迹衔接点的位置/速度/加速度连续性校验

第5章 机器人关节驱动与控制 → 5.1 关节驱动系统 加速度跳变导致减速器冲击载荷

第3章 连杆参数及运动学求解 → 3.2 正运动学 关节角→末端位置/速度/加速度映射（雅可比）

 

本篇代码解决 L2 离线轨迹校核层。输入：两段相邻轨迹的边界条件（起始/终止位置、速度、加速度）和时间分配。处理：分别生成五次多项式→在衔接点采样位置和前二阶导数→计算差值→判断连续性→雅可比映射到笛卡尔空间。输出：连续性校验报告+可视化曲线。

 

不覆盖：逆运动学实时求解、动力学前馈补偿、力控制、视觉引导、多机器人协同。

 

3. 核心逻辑讲解

 

物理直觉

 

轨迹衔接的本质是问"第一段结束时的运动状态和第二段开始时的运动状态能不能无缝对接"。位置连续意味着末端不会瞬间跳到另一个点（C⁰）。速度连续意味着末端不会瞬间改变运动快慢和方向（C¹）——如果不连续，机器人控制器只能急停再启动，产线上就表现为"顿一下"。加速度连续意味着驱动力不会瞬间跳变（C²）——如果不连续，减速器会受到冲击，长期运行会磨损。

 

本篇校验的是：给定两段轨迹的多项式系数，在衔接点t_junction处，分别计算第一段末端的p₁(t_junction)、v₁(t_junction)、a₁(t_junction)和第二段起始的p₂(t_junction)、v₂(t_junction)、a₂(t_junction)，然后做差。位置差应严格为零（两段共享衔接点）。速度差和加速度差应小于工程阈值。

 

业务逻辑 → 代码映射

 

输入：轨迹段1边界条件(q_start1, q_end1, v_start1, v_end1, acc_start1, acc_end1, T1)

      轨迹段2边界条件(q_start2, q_end2, v_start2, v_end2, acc_start2, acc_end2, T2)

      衔接点时间 t_junction

  ↓

【步骤1：为每段轨迹求解五次多项式系数】

  五次多项式：q(t) = c0 + c1·t + c2·t² + c3·t³ + c4·t⁴ + c5·t⁵

  6个边界条件（起止位置、速度、加速度）→ 6元线性方程组 → 求解c0~c5

  ↓

【步骤2：在衔接点采样】

  段1在t=T1处：p1, v1 = q1'(T1), a1 = q1''(T1)

  段2在t=0处：p2, v2 = q2'(0), a2 = q2''(0)

  ↓

【步骤3：连续性校验】

  Δp = |p2 - p1| （应=0，由构造保证）

  Δv = |v2 - v1| （应<阈值，如0.01 rad/s）

  Δa = |a2 - a1| （应<阈值，如1.0 rad/s²）

  ↓

【步骤4：笛卡尔空间映射】

  J(q) · Δv → 末端线速度跳变 (mm/s)

  J(q) · Δa → 末端线加速度跳变 (mm/s²)

  ↓

【步骤5：输出校验报告】

  每段每个关节的连续性判定（FAIL/WARN/PASS）

  ↓

输出：连续性报告 + 采样曲线数据

 

关键公式文字翻译

 

五次多项式：q(t) = c₀ + c₁·t + c₂·t² + c₃·t³ + c₄·t⁴ + c₅·t⁵。c₀是起始位置（t=0时的q值）。c₁是起始速度（一阶导数在t=0的值）。c₂是起始加速度的一半（二阶导数在t=0的值除以2）。c₃、c₄、c₅由终止边界条件（位置、速度、加速度）联立求解得到。

 

速度：v(t) = c₁ + 2·c₂·t + 3·c₃·t² + 4·c₄·t³ + 5·c₅·t⁴。

 

加速度：a(t) = 2·c₂ + 6·c₃·t + 12·c₄·t² + 20·c₅·t³。

 

连续性判定：数学上C¹连续要求Δv=0（解析连续），工程上用阈值（如<1%最大速度）作为容差判据。同理C²连续数学上要求Δa=0，工程上用<10%最大加速度作为容差。

 

4. OOP代码实现

 

技术栈

 

严格限定：numpy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。通用DH正运动学复用公共模块。

 

项目结构树

 

trajectory_continuity_sim/

├── run_trajectory_continuity_sim.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── common/

│ │ ├── __init__.py

│ │ └── kinematics.py

│ ├── models.py

│ ├── quintic_trajectory.py

│ ├── continuity_checker.py

│ ├── cartesian_mapper.py

│ └── visualize_continuity.py

├── tests/

│ ├── __init__.py

│ └── test_trajectory_continuity.py

└── results/

 

核心源码

 

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

 

"""

数据模型：3-DOF平面机械臂连杆参数、轨迹段配置

全部参数内置，自包含可复现

本篇纯运动学校验，mass/com/inertia为后续动力学篇预留

"""

 

 

from dataclasses import dataclass, field

from typing import List, Tuple

 

 

@dataclass

class LinkConfig:

    """单连杆物理参数"""

    length: float = 0.0 # 连杆长度 (m)

    mass: float = 0.0 # 连杆质量 (kg)，本篇未使用，为动力学篇预留

    com_position: float = 0.0 # 质心位置，沿连杆方向 (m)，本篇未使用

    inertia_zz: float = 0.0 # 绕质心Z轴的转动惯量 (kg·m²)，本篇未使用

    joint_limits: Tuple[float, float] = (-3.14, 3.14)

 

 

@dataclass

class TrajectorySegmentConfig:

    """单段轨迹的边界条件配置"""

    duration: float = 1.0 # 段时间 (s)

    q_start: List[float] = field(default_factory=list)

    q_end: List[float] = field(default_factory=list)

    v_start: List[float] = field(default_factory=list)

    v_end: List[float] = field(default_factory=list)

    acc_start: List[float] = field(default_factory=list)

    acc_end: List[float] = field(default_factory=list)

 

    def __post_init__(self):

        """默认3关节：2旋转+1移动"""

        n = 3

        if not self.q_start:

            self.q_start = [0.0] * n

        if not self.q_end:

            self.q_end = [0.0] * n

        if not self.v_start:

            self.v_start = [0.0] * n

        if not self.v_end:

            self.v_end = [0.0] * n

        if not self.acc_start:

            self.acc_start = [0.0] * n

        if not self.acc_end:

            self.acc_end = [0.0] * n

 

 

@dataclass

class RobotArmConfig:

    """

    3-DOF平面机械臂配置

    关节1：大臂旋转（肩），关节2：小臂旋转（肘），关节3：滑台移动（腕）

    """

    links: List[LinkConfig] = field(default_factory=list)

    base_height: float = 0.25 # 基座高度 (m)

 

    def __post_init__(self):

        if not self.links:

            self.links = [

                LinkConfig(length=0.30, mass=1.2, com_position=0.15,

                          inertia_zz=0.018, joint_limits=(-2.09, 2.09)),

                LinkConfig(length=0.25, mass=0.8, com_position=0.125,

                          inertia_zz=0.008, joint_limits=(-2.09, 2.09)),

                LinkConfig(length=0.15, mass=0.3, com_position=0.075,

                          inertia_zz=0.002, joint_limits=(0.0, 0.15)),

            ]

 

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

 

"""

五次多项式轨迹生成

q(t) = c0 + c1*t + c2*t^2 + c3*t^3 + c4*t^4 + c5*t^5

6个边界条件求解6个系数

参数命名规范：q0/qf=起止位置, v0/vf=起止速度, acc0/accf=起止加速度, T=段时间

系数命名：c0~c5

"""

 

 

import numpy as np

 

 

class QuinticTrajectory:

    """

    单关节五次多项式轨迹

    输入：起止位置、速度、加速度 + 段时间

    输出：任意时刻的位置、速度、加速度

    """

 

    def __init__(self, q_start: float, q_end: float,

                 v_start: float = 0.0, v_end: float = 0.0,

                 acc_start: float = 0.0, acc_end: float = 0.0,

                 duration: float = 1.0):

        if duration <= 0:

            raise ValueError(f"轨迹段时间必须大于0，得到{duration}")

        self.duration = duration

        self.q_start = q_start

        self.q_end = q_end

        self.v_start = v_start

        self.v_end = v_end

        self.acc_start = acc_start

        self.acc_end = acc_end

        self.coeffs = self._solve_coefficients(

            q_start, q_end, v_start, v_end, acc_start, acc_end, duration

        )

 

    @staticmethod

    def _solve_coefficients(q0, qf, v0, vf, acc0, accf, T):

        """

        求解五次多项式系数c0~c5

        边界条件：

          q(0)=q0, q(T)=qf

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

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

        """

        c0 = q0

        c1 = v0

        c2 = acc0 / 2.0

 

        T2 = T * T

        T3 = T2 * T

        T4 = T3 * T

        T5 = T4 * T

 

        # 关于c3, c4, c5的3×3线性方程组

        M = np.array([

            [T3, T4, T5],

            [3*T2, 4*T3, 5*T4],

            [6*T, 12*T2, 20*T3]

        ])

 

        rhs = np.array([

            qf - (c0 + c1*T + c2*T2),

            vf - (c1 + 2*c2*T),

            accf - (2*c2)

        ])

 

        try:

            c3c4c5 = np.linalg.solve(M, rhs)

        except np.linalg.LinAlgError:

            raise LinAlgError(

                f"五次多项式系数矩阵奇异（T={T}），"

                f"段时间过短或边界条件不可满足，请检查输入"

            )

 

        return np.array([c0, c1, c2, c3c4c5[0], c3c4c5[1], c3c4c5[2]])

 

    def position(self, t: float) -> float:

        """位置 q(t)，t超出范围时显式报错"""

        if t < 0 or t > self.duration:

            raise ValueError(

                f"时间t={t}超出轨迹段范围[0, {self.duration}]"

            )

        return (self.coeffs[0] + self.coeffs[1]*t

                + self.coeffs[2]*t**2 + self.coeffs[3]*t**3

                + self.coeffs[4]*t**4 + self.coeffs[5]*t**5)

 

    def velocity(self, t: float) -> float:

        """速度 v(t) = dq/dt，t超出范围时显式报错"""

        if t < 0 or t > self.duration:

            raise ValueError(

                f"时间t={t}超出轨迹段范围[0, {self.duration}]"

            )

        return (self.coeffs[1] + 2*self.coeffs[2]*t

                + 3*self.coeffs[3]*t**2 + 4*self.coeffs[4]*t**3

                + 5*self.coeffs[5]*t**4)

 

    def acceleration(self, t: float) -> float:

        """加速度 a(t) = d²q/dt²，t超出范围时显式报错"""

        if t < 0 or t > self.duration:

            raise ValueError(

                f"时间t={t}超出轨迹段范围[0, {self.duration}]"

            )

        return (2*self.coeffs[2] + 6*self.coeffs[3]*t

                + 12*self.coeffs[4]*t**2 + 20*self.coeffs[5]*t**3)

 

    def sample_trajectory(self, num_points: int = 100):

        """采样整段轨迹（内部使用，不暴露给校验器）"""

        t_vals = np.linspace(0, self.duration, num_points)

        q_vals = [self.position(t) for t in t_vals]

        v_vals = [self.velocity(t) for t in t_vals]

        a_vals = [self.acceleration(t) for t in t_vals]

        return t_vals, np.array(q_vals), np.array(v_vals), np.array(a_vals)

 

 

class LinAlgError(Exception):

    """轨迹求解线性代数错误"""

    pass

 

"robot/continuity_checker.py" — 连续性校验

 

"""

两段相邻轨迹衔接处连续性校验

校验位置（构造时保证）、速度、加速度在衔接点处是否连续平滑

分级报告：FAIL（位置或速度不连续）→ WARN（加速度不连续，C¹）→ PASS（C²）

"""

 

 

import numpy as np

 

 

class ContinuityChecker:

    """

    连续性校验器

    输入两段轨迹的QuinticTrajectory对象列表（每个关节一个）

    在衔接点（段1终点=段2起点）校验C0/C1/C2连续性

    """

 

    def __init__(self, pos_threshold=1e-12, vel_threshold=0.01,

                 acc_threshold=1.0):

        """

        pos_threshold: 位置差阈值 (rad或m)，默认1e-12（构造保证）

        vel_threshold: 速度差阈值 (rad/s或m/s)

        acc_threshold: 加速度差阈值 (rad/s²或m/s²)

        """

        self.pos_threshold = pos_threshold

        self.vel_threshold = vel_threshold

        self.acc_threshold = acc_threshold

 

    def check_junction(self, segment1_trajectories, segment2_trajectories,

                       junction_time_s1=None):

        """

        校验衔接点连续性

        位置连续性由构造保证（段2.q_start == 段1.q_end）

        速度/加速度连续性做数值比较

        """

        if len(segment1_trajectories) != len(segment2_trajectories):

            raise ValueError("两段轨迹的关节数不一致")

 

        n_joints = len(segment1_trajectories)

 

        if junction_time_s1 is None:

            junction_time_s1 = segment1_trajectories[0].duration

 

        results = []

 

        for j in range(n_joints):

            traj1 = segment1_trajectories[j]

            traj2 = segment2_trajectories[j]

 

            # 位置连续性：校验构造时是否共享衔接点

            pos_construction_ok = (

                abs(traj2.q_start - traj1.q_end) < self.pos_threshold

            )

 

            # 段1在衔接点（终点）的状态

            p1 = traj1.position(junction_time_s1)

            v1 = traj1.velocity(junction_time_s1)

            a1 = traj1.acceleration(junction_time_s1)

 

            # 段2在衔接点（起点，t=0）的状态

            p2 = traj2.position(0.0)

            v2 = traj2.velocity(0.0)

            a2 = traj2.acceleration(0.0)

 

            # 差值

            dp = abs(p2 - p1)

            dv = abs(v2 - v1)

            da = abs(a2 - a1)

 

            # 判定

            pos_ok = dp < self.pos_threshold

            vel_ok = dv < self.vel_threshold

            acc_ok = da < self.acc_threshold

 

            if pos_ok and vel_ok and acc_ok:

                status = "PASS"

            elif pos_ok and vel_ok:

                status = "WARN"

            else:

                status = "FAIL"

 

            results.append({

                'joint': j,

                'p1': p1, 'p2': p2, 'dp': dp,

                'v1': v1, 'v2': v2, 'dv': dv,

                'a1': a1, 'a2': a2, 'da': da,

                'pos_construction_ok': pos_construction_ok,

                'pos_continuous': pos_ok,

                'vel_continuous': vel_ok,

                'acc_continuous': acc_ok,

                'status': status

            })

 

        return results

 

    def generate_report(self, results):

        """生成文本校验报告（分级文案）"""

        lines = ["=" * 60, "轨迹衔接连续性校验报告", "=" * 60]

        lines.append(

            f"{'关节':<6} {'Δp':<14} {'Δv':<14} {'Δa':<14} {'状态':<8}"

        )

        lines.append("-" * 56)

 

        for r in results:

            lines.append(

                f"J{r['joint']:<5} "

                f"{r['dp']:<14.8e} "

                f"{r['dv']:<14.6f} "

                f"{r['da']:<14.6f} "

                f"{r['status']:<8}"

            )

 

        lines.append("-" * 56)

 

        # 分级判定文案

        fail_joints = [r['joint'] for r in results if r['status'] == 'FAIL']

        warn_joints = [r['joint'] for r in results if r['status'] == 'WARN']

        pass_joints = [r['joint'] for r in results if r['status'] == 'PASS']

 

        if fail_joints:

            lines.append(

                f"✗ FAIL: 关节{fail_joints} 位置不连续或速度不连续，"

                f"必须重新规划边界条件"

            )

        if warn_joints:

            lines.append(

                f"⚠ WARN: 关节{warn_joints} 位置+速度连续（C¹）"

                f"但加速度不连续，需评估减速器冲击是否可接受"

            )

        if pass_joints and not fail_joints:

            lines.append(

                f"✓ PASS: 关节{pass_joints} 位置+速度+加速度均连续（C²）"

            )

 

        return "\n".join(lines)

 

"robot/cartesian_mapper.py" — 笛卡尔空间映射

 

"""

关节空间连续性误差映射到笛卡尔空间

利用雅可比矩阵将Δv/Δa从关节空间转换到末端线速度/加速度

"""

 

 

import numpy as np

from robot.common.kinematics import DHParam, GenericDHKinematics

 

 

class CartesianContinuityMapper:

    """

    笛卡尔空间连续性映射器

    输入：机器人DH参数 + 当前关节角 + 关节空间Δv/Δa

    输出：末端笛卡尔空间线速度跳变(mm/s)、线加速度跳变(mm/s²)

    """

 

    def __init__(self, dh_params, base_height=0.0):

        self.kinematics = GenericDHKinematics(dh_params, base_height)

 

    def compute_jacobian_linear(self, joint_angles):

        """

        计算雅可比矩阵的速度部分（末端线速度对关节速度的映射）

        对3-DOF平面机械臂，取前2个旋转关节+1个移动关节

        返回3×n的雅可比线性速度子矩阵 J_v

        """

        n = len(joint_angles)

        J_v = np.zeros((3, n))

 

        # 平面2R+1P：前两个关节影响XY位置

        if n >= 2:

            q1, q2 = joint_angles[0], joint_angles[1]

            # 末端X位置对q1的偏导

            J_v[0, 0] = (-self.kinematics.dh_params[0].a

                         * np.sin(q1)

                         - self.kinematics.dh_params[1].a

                         * np.sin(q1 + q2))

            # 末端X位置对q2的偏导

            J_v[0, 1] = (-self.kinematics.dh_params[1].a

                         * np.sin(q1 + q2))

            # 末端Y位置对q1的偏导

            J_v[1, 0] = (self.kinematics.dh_params[0].a

                         * np.cos(q1)

                         + self.kinematics.dh_params[1].a

                         * np.cos(q1 + q2))

            # 末端Y位置对q2的偏导

            J_v[1, 1] = (self.kinematics.dh_params[1].a

                         * np.cos(q1 + q2))

 

        # 第三个移动关节直接影响Z

        if n >= 3:

            J_v[2, 2] = 1.0

 

        return J_v

 

    def map_velocity_error(self, joint_angles, delta_v_joint):

        """关节空间速度差 → 末端线速度跳变 (mm/s)"""

        J_v = self.compute_jacobian_linear(joint_angles)

        delta_v_cart = J_v @ np.array(delta_v_joint)

        return delta_v_cart * 1000.0 # rad/s → mm/s (假设小臂长0.25m量级)

 

    def map_acceleration_error(self, joint_angles, delta_a_joint):

        """关节空间加速度差 → 末端线加速度跳变 (mm/s²)"""

        J_v = self.compute_jacobian_linear(joint_angles)

        delta_a_cart = J_v @ np.array(delta_a_joint)

        return delta_a_cart * 1000.0 # rad/s² → mm/s²

 

"robot/visualize_continuity.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 ContinuityVisualizer:

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

        self.results_dir = results_dir

        import os

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_position_velocity_acceleration(self, seg1_trajs, seg2_trajs,

                                            junction_t, joint_idx=0):

        """① 位置/速度/加速度曲线（两段拼接）"""

        fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True)

 

        # 采样

        t1 = np.linspace(0, seg1_trajs[joint_idx].duration, 100)

        t2 = np.linspace(0, seg2_trajs[joint_idx].duration, 100)

        t2_shifted = t2 + junction_t

 

        # 位置

        q1 = [seg1_trajs[joint_idx].position(t) for t in t1]

        q2 = [seg2_trajs[joint_idx].position(t) for t in t2]

        axes[0].plot(t1, q1, 'b-', linewidth=2, label='Segment 1')

        axes[0].plot(t2_shifted, q2, 'r-', linewidth=2, label='Segment 2')

        axes[0].axvline(x=junction_t, color='k', linestyle='--', alpha=0.5)

        axes[0].set_ylabel('Position (rad / m)')

        axes[0].set_title(f'Joint {joint_idx} - Position')

        axes[0].legend()

        axes[0].grid(True, alpha=0.3)

 

        # 速度

        v1 = [seg1_trajs[joint_idx].velocity(t) for t in t1]

        v2 = [seg2_trajs[joint_idx].velocity(t) for t in t2]

        axes[1].plot(t1, v1, 'b-', linewidth=2)

        axes[1].plot(t2_shifted, v2, 'r-', linewidth=2)

        axes[1].axvline(x=junction_t, color='k', linestyle='--', alpha=0.5)

        axes[1].set_ylabel('Velocity (rad/s / m/s)')

        axes[1].set_title(f'Joint {joint_idx} - Velocity')

        axes[1].grid(True, alpha=0.3)

 

        # 加速度

        a1 = [seg1_trajs[joint_idx].acceleration(t) for t in t1]

        a2 = [seg2_trajs[joint_idx].acceleration(t) for t in t2]

        axes[2].plot(t1, a1, 'b-', linewidth=2)

        axes[2].plot(t2_shifted, a2, 'r-', linewidth=2)

        axes[2].axvline(x=junction_t, color='k', linestyle='--', alpha=0.5)

        axes[2].set_ylabel('Acceleration (rad/s² / m/s²)')

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

        axes[2].set_title(f'Joint {joint_idx} - Acceleration')

        axes[2].grid(True, alpha=0.3)

 

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_junction_zoom(self, seg1_trajs, seg2_trajs,

                           junction_t, joint_idx=0):

        """② 衔接点附近放大图（速度+加速度）"""

        fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8), sharex=True)

 

        window = 0.1

        t1_zoom = np.linspace(max(0, junction_t-window), junction_t, 50)

        t2_zoom = np.linspace(0, min(window, seg2_trajs[joint_idx].duration), 50)

        t2_zoom_shifted = t2_zoom + junction_t

 

        v1_zoom = [seg1_trajs[joint_idx].velocity(t) for t in t1_zoom]

        v2_zoom = [seg2_trajs[joint_idx].velocity(t) for t in t2_zoom]

        ax1.plot(t1_zoom, v1_zoom, 'b-', linewidth=2, label='Seg1 end')

        ax1.plot(t2_zoom_shifted, v2_zoom, 'r-', linewidth=2, label='Seg2 start')

        ax1.axvline(x=junction_t, color='k', linestyle='--', alpha=0.5)

        ax1.set_ylabel('Velocity')

        ax1.set_title(f'J{joint_idx} Velocity at Junction (±{window}s)')

        ax1.legend()

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