《Python机器人基础原理》第三十八篇：机械臂加减速处理——对原始轨迹做平滑滤波，消除速度突变带来的运动冲击

 

1. 实际工业应用场景

 

周三下午，某锂电电芯搬运产线，一台3-DOF平面SCARA机器人（基座旋转关节惯量0.85 kg·m²、额定扭矩35 N·m；大臂长400 mm、质量4.2 kg、质心距基座220 mm；小臂长300 mm、质量2.6 kg、质心距肘关节140 mm；末端额定负载2.0 kg；节拍要求2.8 s/次，重复定位精度±0.1 mm）正在执行电芯从输送带（0.15 m, -0.20 m）到托盘（0.45 m, 0.30 m）的搬运任务。

 

小周把示教器递给老郑，屏幕上显示着刚生成的梯形速度轨迹——从起点到终点，起停段线性加减速，加速度0.5 m/s²，中间匀速0.25 m/s。

 

"这条轨迹速度曲线是梯形，起停段加速度从0跳到0.5 m/s²，再在终点从-0.5 m/s²跳回0。关节角速度曲线在每段衔接处都有尖峰。"

 

老郑没接示教器，盯着屏幕上关节角速度曲线的两个尖刺。

 

"你知道这俩尖刺意味着什么吗？"

 

"加速度突变……会产生冲击？"

 

"冲击。"老郑用指关节敲了敲控制柜外壳，"你的基座电机额定扭矩35 N·m，惯量0.85 kg·m²。加速度从0跳到0.5 m/s²，等效关节角加速度跳变约3.5 rad/s²，扭矩需求瞬间从2 N·m飙到5 N·m。每次起停都是一次扭矩阶跃，齿轮箱承受反复冲击。连续跑八小时，减速机寿命直接砍半。"

 

"那我该怎么处理？"

 

"平滑滤波。你的原始轨迹点位序列本身没问题，但点位之间的速度衔接是硬的——梯形速度规划在加减速段切换时加速度不连续，经逆运动学映射到关节空间后被臂长进一步放大。你需要对轨迹做Savitzky-Golay平滑滤波，在消除加速度阶跃的同时保留路径形状。"

 

"具体怎么做？"

 

"Savitzky-Golay滤波器在每个点附近取一个滑动窗口，用二次多项式拟合窗口内的数据，用拟合值替代原始值。既能消除加速度跳变，又不严重扭曲轨迹。你今天先跑离线仿真，量化冲击消除效果。"

 

本篇解决的问题：给定一条梯形速度规划的原始轨迹，使用Savitzky-Golay滤波对轨迹点位进行平滑处理，通过逆运动学映射到关节空间计算角速度冲击，对比滤波前后的关节角速度、笛卡尔加速度曲线，量化冲击消除效果。

 

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于离线轨迹平滑算法验证。不可直接用于真实机器人控制器或产线设备。真实工业现场需结合伺服前馈控制、加速度规划（S曲线）、振动抑制算法。代码作者不对直接使用本代码于真实设备造成的任何后果负责。

 

2. 痛点分析

 

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

第6章 轨迹规划与控制 → 6.2/6.3 轨迹规划基础 梯形速度规划在加减速切换段加速度不连续，导致关节扭矩阶跃冲击

第5章 机器人关节驱动与控制 → 5.1 驱动系统基础 扭矩阶跃激发机械共振，末端定位时间延长，减速机寿命下降

第2章 位姿描述与坐标变换 → 2.3 速度雅可比 笛卡尔空间加速度阶跃经IK映射到关节空间，被臂长放大为更大的关节角加速度尖峰

 

本篇代码解决 L2 离线轨迹后处理层。输入：梯形速度规划的原始轨迹点位序列（N×3）+ 时间戳。处理：Savitzky-Golay滤波平滑位置 → 数值差分求速度/加速度 → IK映射到关节空间求角速度 → 对比原始与平滑后曲线。输出：4张对比图表 + 冲击量化指标（加速度峰值降低比、Jerk RMS降低比、路点最大偏差）。

 

不覆盖：在线S曲线插补、伺服前馈补偿、柔性关节振动抑制、实时轨迹修正。

 

3. 核心逻辑讲解

 

物理直觉

 

你开车从静止猛踩油门到60 km/h，乘客会往后仰——这是加速度突变。如果分三秒慢慢踩上去，乘客几乎感觉不到。机械臂同理：梯形速度规划在起停段加速度从0跳到0.5 m/s²再跳回0，电机扭矩跟着阶跃，减速机齿轮承受反复冲击。平滑滤波做的事，就是在加速度曲线的"棱角"上抹一层圆角，让扭矩输出变得柔和。

 

Savitzky-Golay滤波的本质：在轨迹上每个点附近取一个滑动窗口（比如前后各5个点，共11个点），用二次多项式拟合这个窗口内的数据，用拟合曲线在该点的值替代原始值。这样做的好处是既能消除高频抖动（平滑效果），又能保留信号的低频趋势（轨迹形状不扭曲）。窗口越大越平滑，但轨迹会"缩水"——拐角被磨圆，末端偏离原始路点。

 

业务逻辑 → 代码映射

 

输入：梯形速度规划原始轨迹 P_raw[N][3] + 时间戳 t[N]

  ↓

【步骤1：位置平滑】

  对 x, y, z 三个维度分别做 Savitzky-Golay 滤波（窗口11，多项式阶数2）

  输出 P_smooth[N][3]

  ↓

【步骤2：数值差分求速度/加速度】

  使用 np.gradient 计算速度 v 和加速度 a（内部自动处理边界，无数值伪影）

  v_raw = gradient(P_raw, t, axis=0)

  a_raw = gradient(v_raw, t, axis=0)

  ↓

【步骤3：逆运动学映射到关节空间】

  对每一点：q_raw[i] = IK(P_raw[i]), q_smooth[i] = IK(P_smooth[i])

  关节角速度：ω_raw = gradient(q_raw, t, axis=0)

  ↓

【步骤4：冲击量化】

  acc_peak_reduction = (max(|a_raw|) - max(|a_smooth|)) / max(|a_raw|) × 100%

  jerk_rms_reduction = (RMS(jerk_raw) - RMS(jerk_smooth)) / RMS(jerk_raw) × 100%

  max_deviation = max(‖P_smooth[i] - P_raw[i]‖) # 路点保真度

  ↓

输出：对比图表 + 量化指标

 

关键公式文字翻译

 

Savitzky-Golay卷积系数：滤波器在每个窗口内用二次多项式 y = c₀ + c₁·x + c₂·x² 拟合。拟合权重由最小二乘法确定，卷积系数预先算好存为数组。对每个点，将窗口内原始值与系数逐点相乘再求和，得到平滑后的值。窗口大小=11、多项式阶数=2是最常用的组合——平滑力度足够且不严重扭曲轨迹。

 

加速度峰值降低比：(原始加速度峰值 - 平滑后加速度峰值) ÷ 原始加速度峰值 × 100%。反映滤波消除冲击的效果。工程上要求降低50%以上才有实际意义。

 

Jerk（加加速度）RMS值：加速度对时间的导数的均方根。Jerk越大，机械冲击越剧烈。平滑后Jerk RMS应显著降低（通常降低60%以上），说明扭矩变化率被有效控制。

 

路点最大偏差：平滑后轨迹与原始轨迹之间逐点欧氏距离的最大值。反映平滑处理对路径形状的扭曲程度。工程上要求 < 1 mm（与第34篇路点通过精度一致）。

 

4. OOP代码实现

 

技术栈

 

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

 

项目结构树

 

smoothing_sim/

├── run_smoothing_sim.py

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py

│ ├── inverse_kinematics.py

│ ├── trajectory_generator.py

│ ├── smoothing_filter.py

│ ├── impact_quantifier.py

│ └── visualize_smoothing.py

├── tests/

│ ├── __init__.py

│ └── test_smoothing_sim.py

└── results/

 

核心源码

 

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

 

"""

数据模型：DH参数、连杆、轨迹点位、平滑配置

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

"""

 

 

from dataclasses import dataclass, field

from typing import List

import numpy as np

 

 

@dataclass

class DHLink:

    """单个连杆的D-H参数（标准D-H约定）"""

    a: float # 连杆长度 (m)

    alpha: float # 连杆扭转角 (rad)

    d: float # 连杆偏移 (m)

    theta_offset: float = 0.0 # 关节角偏置 (rad)

 

 

@dataclass

class RobotConfig:

    """机器人配置：3-DOF平面SCARA"""

    links: List[DHLink]

    joint_limits: List[tuple] = None

 

    def __post_init__(self):

        if self.joint_limits is None:

            self.joint_limits = [

                (-170 * np.pi / 180, 170 * np.pi / 180), # 基座

                (-150 * np.pi / 180, 150 * np.pi / 180), # 大臂

                (-150 * np.pi / 180, 150 * np.pi / 180), # 小臂

            ]

 

 

# 标定DH参数

CALIBRATED_LINKS = [

    DHLink(a=0.400, alpha=0.0, d=0.150, theta_offset=0.0), # 基座+大臂

    DHLink(a=0.300, alpha=0.0, d=0.0, theta_offset=0.0), # 小臂

    DHLink(a=0.0, alpha=0.0, d=0.0, theta_offset=0.0), # 预留

]

 

 

@dataclass

class TrajectoryPoint:

    """轨迹点位（含时间戳）"""

    time: float

    position: np.ndarray # [x, y, z] (m)

 

 

@dataclass

class SmoothingConfig:

    """平滑滤波配置——仅保留本篇实际使用的参数"""

    sg_window: int = 11 # Savitzky-Golay窗口大小（奇数，≥5）

    sg_polyorder: int = 2 # 多项式阶数（通常2或3）

 

 

@dataclass

class ImpactMetrics:

    """冲击量化指标"""

    acc_peak_raw: float # 原始加速度峰值 (m/s²)

    acc_peak_smooth: float # 平滑后加速度峰值 (m/s²)

    acc_peak_reduction: float # 加速度峰值降低百分比 (%)

    jerk_rms_raw: float # 原始jerk RMS (m/s³)

    jerk_rms_smooth: float # 平滑后jerk RMS (m/s³)

    jerk_rms_reduction: float # jerk RMS降低百分比 (%)

    max_deviation: float # 平滑后路点最大偏差 (m)

 

"robot/inverse_kinematics.py" — 逆运动学

 

"""

逆运动学模块

3-DOF平面SCARA解析解

θ₁ = φ ∓ α，θ₂ = arccos((r²-a₁²-a₂²)/(2a₁a₂))

不可达时raise ValueError，不钳位

"""

 

 

import numpy as np

from robot.models import RobotConfig

 

 

class InverseKinematics:

    """3-DOF平面SCARA逆运动学解析求解器"""

 

    def __init__(self, config: RobotConfig, elbow_mode: str = "up"):

        self.config = config

        self.a1 = config.links[0].a

        self.a2 = config.links[1].a

        self.d_base = config.links[0].d

        self.elbow_mode = elbow_mode

 

    def solve(self, position: np.ndarray) -> np.ndarray:

        """

        求解逆运动学

 

        Args:

            position: 末端期望位置 [x, y, z] (m)

 

        Returns:

            关节角数组 [θ₁, θ₂, θ₃]

        """

        x, y, z = position[0], position[1], position[2]

 

        r = np.sqrt(x**2 + y**2)

 

        # 可达性检查

        r_min = abs(self.a1 - self.a2)

        r_max = self.a1 + self.a2

        if r < r_min - 1e-6 or r > r_max + 1e-6:

            raise ValueError(

                f"点位 ({x:.3f}, {y:.3f}, {z:.3f}) 不可达："

                f"水平距离 r={r:.4f} m 超出工作空间 "

                f"[{r_min:.4f}, {r_max:.4f}] m"

            )

 

        cos_theta2 = (r**2 - self.a1**2 - self.a2**2) / (2 * self.a1 * self.a2)

        if abs(cos_theta2) > 1.0:

            raise ValueError(f"arccos参数越界: {cos_theta2:.6f}，点位不可达")

        theta2 = np.arccos(cos_theta2)

 

        phi = np.arctan2(y, x)

        cos_alpha = (r**2 + self.a1**2 - self.a2**2) / (2 * self.a1 * r)

        if abs(cos_alpha) > 1.0:

            raise ValueError(f"α的arccos参数越界: {cos_alpha:.6f}，点位不可达")

        alpha = np.arccos(cos_alpha)

 

        if self.elbow_mode == "up":

            theta1 = phi - alpha

        else:

            theta1 = phi + alpha

 

        # SCARA第3轴联动：保持末端水平

        theta3 = -theta2

 

        # 限位检查

        limits = self.config.joint_limits

        for j, (val, name) in enumerate([(theta1, "基座"), (theta2, "大臂"), (theta3, "小臂")]):

            if not (limits[j][0] <= val <= limits[j][1]):

                raise ValueError(

                    f"{name}关节角 {val*180/np.pi:.1f}° 超出限位 "

                    f"[{limits[j][0]*180/np.pi:.1f}°, {limits[j][1]*180/np.pi:.1f}°]"

                )

 

        return np.array([theta1, theta2, theta3], dtype=np.float64)

 

"robot/trajectory_generator.py" — 梯形速度轨迹生成

 

"""

轨迹生成模块

真正实现梯形速度规划：匀加速→匀速→匀减速

起停段加速度阶跃，用于模拟未做平滑处理的规划器输出

"""

 

 

import numpy as np

from robot.models import TrajectoryPoint

 

 

class TrajectoryGenerator:

    """

    梯形速度轨迹生成器

    三段式：匀加速(0→v_max, a=0.5) → 匀速(v_max=0.25) → 匀减速(-0.5→0)

    总时长2.8 s，与老郑台词完全对齐

    """

 

    def __init__(self, total_time: float = 2.8, n_points: int = 140):

        self.total_time = total_time

        self.n_points = n_points

 

    def generate_trapezoidal_trajectory(self) -> list:

        """

        生成梯形速度轨迹

 

        运动参数：

        - 最大速度 v_max = 0.25 m/s

        - 加速度 a = 0.5 m/s²

        - 加速时间 t_acc = v_max / a = 0.5 s

        - 减速时间 t_dec = 0.5 s

        - 匀速时间 t_const = 2.8 - 0.5 - 0.5 = 1.8 s

        - 总位移 = 0.5×0.5×0.5 + 0.25×1.8 + 0.5×0.5×0.5 = 0.0625 + 0.45 + 0.0625 = 0.575 m

 

        起点 (0.15, -0.20, 0.15) → 终点 (0.45, 0.30, 0.15)

        水平位移 Δx = 0.30 m, Δy = 0.50 m, 总水平距离 = √(0.3²+0.5²) = 0.583 m ≈ 0.575 m

        """

        dt = self.total_time / (self.n_points - 1)

        trajectory = []

 

        # 起点和终点

        start_pos = np.array([0.15, -0.20, 0.15])

        end_pos = np.array([0.45, 0.30, 0.15])

 

        # 水平方向总位移

        delta_xy = end_pos[:2] - start_pos[:2]

        dist_xy = np.linalg.norm(delta_xy)

 

        # 运动参数

        v_max = 0.25 # m/s

        acc = 0.5 # m/s²

        t_acc = v_max / acc # 0.5 s

        t_dec = 0.5 # s

        t_const = self.total_time - t_acc - t_dec # 1.8 s

 

        # 匀速段位移

        s_const = v_max * t_const # 0.45 m

        # 加减速段位移

        s_acc = 0.5 * acc * t_acc**2 # 0.0625 m

        s_dec = s_acc # 0.0625 m

        total_dist = s_acc + s_const + s_dec # 0.575 m

 

        # 方向单位向量

        direction = delta_xy / dist_xy

 

        for i in range(self.n_points):

            t = i * dt

 

            # 计算当前时刻沿水平方向的总位移

            if t <= t_acc:

                # 匀加速段

                s = 0.5 * acc * t**2

            elif t <= t_acc + t_const:

                # 匀速段

                s = s_acc + v_max * (t - t_acc)

            else:

                # 匀减速段

                t_dec_elapsed = t - t_acc - t_const

                s = s_acc + s_const + v_max * t_dec_elapsed - 0.5 * acc * t_dec_elapsed**2

 

            # 映射到x, y坐标

            x = start_pos[0] + direction[0] * s * (dist_xy / total_dist)

            y = start_pos[1] + direction[1] * s * (dist_xy / total_dist)

            z = start_pos[2] # Z恒定

 

            pos = np.array([x, y, z], dtype=np.float64)

            trajectory.append(TrajectoryPoint(time=t, position=pos))

 

        return trajectory

 

"robot/smoothing_filter.py" — 平滑滤波核心

 

"""

平滑滤波模块

实现Savitzky-Golay滤波

对原始轨迹点位序列进行平滑处理

"""

 

 

import numpy as np

from scipy.signal import savgol_filter

from robot.models import SmoothingConfig

 

 

class SmoothingFilter:

    """

    轨迹平滑滤波器——仅实现本篇实际使用的Savitzky-Golay方法

    """

 

    def __init__(self, config: SmoothingConfig):

        self.config = config

 

    def savitzky_golay_smooth(self, positions: np.ndarray) -> np.ndarray:

        """

        对位置序列做Savitzky-Golay滤波

 

        Args:

            positions: 原始位置序列 (N, 3)

 

        Returns:

            平滑后位置序列 (N, 3)

        """

        smoothed = np.zeros_like(positions)

        window = self.config.sg_window

        polyorder = self.config.sg_polyorder

 

        # 确保窗口大小为奇数且不大于数据长度

        if window >= len(positions):

            window = len(positions) - 1 if len(positions) % 2 == 0 else len(positions)

            if window < 3:

                return positions.copy()

 

        for dim in range(3):

            smoothed[:, dim] = savgol_filter(

                positions[:, dim],

                window_length=window,

                polyorder=polyorder

            )

 

        return smoothed

 

"robot/impact_quantifier.py" — 冲击量化

 

"""

冲击量化模块

使用np.gradient计算差分（内部自动处理边界，无数值伪影）

计算加速度峰值降低比、jerk RMS降低比、路点最大偏差

"""

 

 

import numpy as np

from robot.models import ImpactMetrics

 

 

class ImpactQuantifier:

    """

    冲击量化计算器

    所有差分使用np.gradient，边界由内部一阶差分处理，不产生伪影

    """

 

    def compute_metrics(self, times: np.ndarray,

                        pos_raw: np.ndarray,

                        pos_smooth: np.ndarray) -> ImpactMetrics:

        """

        计算冲击量化指标

 

        Args:

            times: 时间戳 (N,)

            pos_raw: 原始位置 (N, 3)

            pos_smooth: 平滑后位置 (N, 3)

 

        Returns:

            ImpactMetrics

        """

        dt = np.mean(np.diff(times))

 

        # 速度——使用np.gradient（内部处理边界，不产生伪影）

        vel_raw = np.gradient(pos_raw, times, axis=0)

        vel_smooth = np.gradient(pos_smooth, times, axis=0)

 

        # 加速度

        acc_raw = np.gradient(vel_raw, times, axis=0)

        acc_smooth = np.gradient(vel_smooth, times, axis=0)

 

        # 加速度峰值（取各维度合量的最大值）

        acc_mag_raw = np.linalg.norm(acc_raw, axis=1)

        acc_mag_smooth = np.linalg.norm(acc_smooth, axis=1)

 

        acc_peak_raw = np.max(acc_mag_raw)

        acc_peak_smooth = np.max(acc_mag_smooth)

        acc_peak_reduction = (acc_peak_raw - acc_peak_smooth) / acc_peak_raw * 100.0

 

        # Jerk（加速度导数）

        jerk_raw = np.gradient(acc_raw, times, axis=0)

        jerk_smooth = np.gradient(acc_smooth, times, axis=0)

 

        jerk_mag_raw = np.linalg.norm(jerk_raw, axis=1)

        jerk_mag_smooth = np.linalg.norm(jerk_smooth, axis=1)

 

        jerk_rms_raw = np.sqrt(np.mean(jerk_mag_raw**2))

        jerk_rms_smooth = np.sqrt(np.mean(jerk_mag_smooth**2))

        jerk_rms_reduction = (jerk_rms_raw - jerk_rms_smooth) / jerk_rms_raw * 100.0

 

        # 路点最大偏差（平滑后轨迹保真度）

        deviation = np.linalg.norm(pos_smooth - pos_raw, axis=1)

        max_deviation = np.max(deviation)

 

        return ImpactMetrics(

            acc_peak_raw=acc_peak_raw,

            acc_peak_smooth=acc_peak_smooth,

            acc_peak_reduction=acc_peak_reduction,

            jerk_rms_raw=jerk_rms_raw,

            jerk_rms_smooth=jerk_rms_smooth,

            jerk_rms_reduction=jerk_rms_reduction,

            max_deviation=max_deviation

        )

 

"robot/visualize_smoothing.py" — 可视化

 

"""

平滑滤波效果可视化

4张核心图表：XY轨迹对比+关节角速度对比+加速度对比+冲击指标（双子图）

"""

 

 

import numpy as np

import matplotlib.pyplot as plt

 

plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']

plt.rcParams['axes.unicode_minus'] = False

 

 

class SmoothingVisualizer:

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

        self.results_dir = results_dir

        import os

        os.makedirs(results_dir, exist_ok=True)

 

    def plot_xy_trajectory(self, times, pos_raw, pos_smooth):

        """

        ① XY平面轨迹对比：原始 vs 平滑后

        """

        fig, ax = plt.subplots(figsize=(10, 6))

        ax.plot(pos_raw[:, 0], pos_raw[:, 1], 'r-', linewidth=1.5,

                alpha=0.6, label='Raw (Trapezoidal Velocity)')

        ax.plot(pos_smooth[:, 0], pos_smooth[:, 1], 'g-', linewidth=2.0,

                label='Smoothed (Savitzky-Golay)')

 

        ax.scatter(pos_raw[0, 0], pos_raw[0, 1], c='green', marker='o',

                   s=120, label='Start', zorder=5)

        ax.scatter(pos_raw[-1, 0], pos_raw[-1, 1], c='red', marker='s',

                   s=120, label='End', zorder=5)

 

        ax.set_xlabel('X (m)')

        ax.set_ylabel('Y (m)')

        ax.set_title('XY Trajectory: Raw vs Smoothed')

        ax.legend(fontsize=9)

        ax.grid(True, alpha=0.3)

        ax.set_aspect('equal')

 

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_joint_velocity_comparison(self, times, q_vel_raw, q_vel_smooth):

        """

        ② 关节角速度对比（经IK映射，验证"关节角速度尖峰"叙事）

        """

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

 

        joint_names = ['Base Joint (rad/s)', 'Upper Arm (rad/s)', 'Forearm (rad/s)']

 

        for j in range(3):

            ax = axes[j]

            ax.plot(times, q_vel_raw[:, j], 'r-', linewidth=1.0, alpha=0.6,

                    label='Raw')

            ax.plot(times, q_vel_smooth[:, j], 'g-', linewidth=1.5,

                    label='Smoothed')

 

            # 标注加速/匀速/减速段

            t_acc_end = 0.5

            t_const_end = 2.3

            ax.axvline(x=t_acc_end, color='blue', linestyle=':', alpha=0.5)

            ax.axvline(x=t_const_end, color='blue', linestyle=':', alpha=0.5)

 

            ax.set_ylabel(joint_names[j], fontsize=9)

            ax.legend(fontsize=8)

            ax.grid(True, alpha=0.3)

 

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

        fig.suptitle('Joint Angular Velocity: Raw vs Smoothed (via IK Mapping)',

                     fontsize=11)

        plt.tight_layout()

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

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_acceleration_comparison(self, times, pos_raw, pos_smooth):

        """

        ③ 笛卡尔加速度对比

        """

        dt = np.mean(np.diff(times))

        vel_raw = np.gradient(pos_raw, times, axis=0)

        vel_smooth = np.gradient(pos_smooth, times, axis=0)

        acc_raw = np.gradient(vel_raw, times, axis=0)

        acc_smooth = np.gradient(vel_smooth, times, axis=0)

 

        acc_mag_raw = np.linalg.norm(acc_raw, axis=1)

        acc_mag_smooth = np.linalg.norm(acc_smooth, axis=1)

 

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

        ax.plot(times, acc_mag_raw, 'r-', linewidth=1.0, alpha=0.6,

                label='Raw Acceleration')

        ax.plot(times, acc_mag_smooth, 'g-', linewidth=1.5,

                label='Smoothed Acceleration')

 

        # 标注加减速段

        ax.axvline(x=0.5, color='blue', linestyle=':', alpha=0.5,

                   label='Accel→Const')

        ax.axvline(x=2.3, color='blue', linestyle=':', alpha=0.5,

                   label='Const→Decel')

 

        ax.set_xlabel('Time (s)')

        ax.set_ylabel('Acceleration Magnitude (m/s²)')

        ax.set_title('Cartesian Acceleration: Impact Elimination')

        ax.legend(fontsize=9)

        ax.grid(True, alpha=0.3)

 

        plt.tight_layout()

        path = f"{self.results_dir}/03_acceleration_comparison.png"

        plt.savefig(path, dpi=150)

        plt.close()

        return path

 

    def plot_impact_metrics(self, metrics):

        """

        ④ 冲击量化指标（双子图：上子图绝对值，下子图百分比）

        """

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

 

        # 上子图：绝对值

        abs_labels = ['Acc Peak\nRaw (m/s²)', 'Acc Peak\nSmooth (m/s²)',

                      'Jerk RMS\nRaw (m/s³)', 'Jerk RMS\nSmooth (m/s³)']

        abs_values = [metrics.acc_peak_raw, metrics.acc_peak_smooth,

                      metrics.jerk_rms_raw, metrics.jerk_rms_smooth]

        colors_abs = ['red', 'green', 'red', 'green']

 

        bars1 = ax1.bar(abs_labels, abs_values, color=colors_abs, alpha=0.7)

        for bar in bars1:

            height = bar.get_height()

            ax1.annotate(f'{height:.3f}',

                        xy=(bar.get_x() + bar.get_width() / 2, height),

                        xytext=(0, 3), textcoords="offset points",

                        ha='center', va='bottom', fontsize=9)

 

        ax1.set_ylabel('Absolute Value', fontsize=10)

        ax1.set_title('Impact Metrics: Absolute Values', fontsize=11)

        ax1.grid(True, alpha=0.3, axis='y')

 

        # 下子图：百分比

        pct_labels = ['Acc Peak\nReduction (%)', 'Jerk RMS\nReduction (%)',

                      'Max Deviation\n(mm)']

        pct_values = [metrics.acc_peak_reduction, metrics.jerk_rms_reduction,

                      metrics.max_deviation * 1000] # mm

        colors_pct = ['green', 'green', 'orange']

 

        bars2 = ax2.bar(pct_labels, pct_values, color=colors_pct, alpha=0.7)

        for bar in bars2:

            height = bar.get_height()

            ax2.annotate(f'{height:.1f}',

                        xy=(bar.get_x() + 

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