python的先进制造技术工业场景模拟第一百二十八篇:导入机器人轨迹采样数据,异常检测识别轨迹偏移,抖动等运动异常。
机器人轨迹异常检测——从"肉眼盯屏"到"算法秒级识别抖动与偏移"
一、实际应用场景(真实痛点)
时间:周五凌晨 02:30。
地点:某汽车零部件厂焊接车间,2 台 6 轴弧焊机器人组成的柔性工作站,夜班无人值守运行。
凌晨巡检的电气工程师小李被系统弹窗叫醒——手机上跳出来一条报警:"Robot-02 轨迹异常:J4 轴速度波动超过阈值"。他揉着眼睛打开监控回放,画面里机器人正在焊一条长直缝,肉眼看不出任何异常。"这玩意儿又误报了。"小李嘀咕着,点了"忽略"。
第二天上午,质量部反馈:夜班批次的 47 件零件中,有 6 件的焊缝出现了肉眼可见的波浪纹。拆回三坐标测量,偏差 0.3-0.5mm,集中在 Robot-02 焊接的第 3、4 段轨迹。
"你们昨晚系统报了异常,怎么没停机?"质量工程师问。
"看了回放没看出来,画面里动作很正常。"小李翻出机器人控制器日志,"但我导出了轨迹采样数据——每 8ms 一个采样点,包含 6 个关节的角度、速度、加速度,还有末端 TCP 的 x/y/z 坐标。一个夜班 8 小时,大约 360 万个采样点。你让我从这 360 万行数据里肉眼找'哪里抖了'?"
"那你们平时怎么判断轨迹有没有问题?"
"三种办法,都不好使:"
1. 肉眼看回放——帧率 25fps,机器人运动速度 1.2m/s,一帧之间末端已经移动了 48mm。抖动幅度小于 2mm 的根本看不出来。
2. 看控制器报警——只有关节超限才会报警,轨迹微偏移、末端微抖动不触发。
3. 焊完量尺寸——这是事后检测,废品已经出了,而且不知道是哪一段轨迹出的问题。
"所以你需要一个程序:导入机器人轨迹采样数据(关节角度/速度/加速度 + 末端 TCP 坐标),自动检测两种异常——轨迹偏移(末端实际路径偏离理论路径)和运动抖动(关节速度/加速度高频波动),定位到具体是哪个时间段、哪个关节、偏移/抖动了多少,最好还能告诉我'这个抖动是机械问题还是程序问题'?"
"对。"小李点头,"而且 360 万行数据,不能让我等半天。最好 30 秒内出结果。"
"这活儿我干过。"我说,"pandas 管 360 万行数据加载和预处理,numpy 做滑动窗口和差分运算,scipy 做 FFT 频域分析找抖动频率,scikit-learn 的 Isolation Forest 做无监督异常检测识别轨迹偏移,matplotlib 画轨迹对比图、抖动频谱图、异常散点图。networkx 建'关节-异常'关联图找根因。代码 OOP,带单元测试。纯 Python,不依赖任何机器人厂商的 SDK。"
二、痛点分析(映射到滨州职业学院课程模型)
《先进制造技术》课程模块 本篇痛点对应
工业机器人技术基础 机器人运动学、轨迹插补、关节空间 vs 笛卡尔空间
数控加工与 CAD/CAM 技术 刀具路径精度、插补误差
先进制造技术基础 精度理论与误差来源分析
柔性制造系统 FMS 与先进生产管理 设备状态监控、质量预警
智能制造与数字孪生 机器人数字孪生体、实时运动分析
先进制造新模式 预测性维护、数据驱动质量控制
一句话总结:我们需要一个"导入机器人轨迹采样数据(关节空间+笛卡尔空间)→ 预处理(去噪/插值/对齐)→ 异常检测(Isolation Forest 识别偏移 + FFT 频域分析识别抖动)→ 定位(时间/关节/空间位置)→ 可视化(轨迹对比/频谱/散点)→ 根因关联分析"程序,实现从"肉眼盯 360 万行数据"到"30 秒定位第 4 段轨迹 J4 轴 12Hz 抖动"的升级。
三、核心逻辑讲解(大白话)
3.1 问题本质:把机器人轨迹想成"书法描红"
想象你拿着毛笔在字帖上描红:
- 理论轨迹 = 字帖上的红字(机器人编程时设定的理想路径)
- 实际轨迹 = 你毛笔尖实际走过的痕迹
- 轨迹偏移 = 你描着描着偏到了红字外面(末端位置偏离理论路径)
- 运动抖动 = 你手抖了,笔画边缘毛毛糙糙(关节速度高频波动)
问题来了:你描了 8 小时,写了几千个字,中间手抖了几次你自己不知道。现在给你一张纸,上面记录了你手腕每秒钟 125 次的位置——你怎么从这一大堆数字里找到"哪一笔、哪一刻、哪个手指在抖"?
3.2 业务逻辑 → 代码映射
输入: 机器人轨迹采样数据 CSV
关节数据: timestamp, J1-J6 角度(rad), 速度(rad/s), 加速度(rad/s²)
笛卡尔数据: timestamp, TCP_x, TCP_y, TCP_z, 姿态(四元数或欧拉角)
理论轨迹: timestamp, TCP_x_ref, TCP_y_ref, TCP_z_ref
│
▼ TrajectoryData (dataclass)
数据模型:
JointSample / CartesianSample / TheoreticalTrajectory / AnomalyEvent
│
▼ DataLoader
数据加载器:
pandas 读取 CSV → 按时间对齐关节数据和笛卡尔数据
处理缺失值/异常值 → 统一采样频率(8ms)
│
▼ Preprocessor
预处理器:
滑动窗口平滑(去高频噪声)
差分计算速度/加速度(如果原始数据只有位置)
时间对齐: 实际轨迹 vs 理论轨迹(最近邻插值)
│
▼ OffsetDetector (scikit-learn Isolation Forest)
偏移检测器:
末端位置偏差 = ||实际TCP - 理论TCP||
对偏差序列做 Isolation Forest 无监督异常检测
输出: 异常时间段 + 偏移量 + 异常得分
│
▼ JitterDetector (scipy FFT)
抖动检测器:
对每个关节的速度序列做 FFT → 频谱
识别高频分量(>5Hz)的能量占比
能量占比 > 阈值 → 判定为抖动
输出: 抖动关节 + 抖动频率 + 抖动幅度
│
▼ RootCauseAnalyzer (networkx)
根因关联分析:
构建"关节-异常类型-时间段"关联图
边权重 = 异常严重程度
找中心性最高的节点 → 根因定位
│
▼ TrajectoryVisualizer (matplotlib)
可视化:
1. 理论轨迹 vs 实际轨迹 3D 对比图
2. 末端偏移量时间序列图 (异常段高亮)
3. 关节速度 FFT 频谱图 (抖动频率标注)
4. 异常散点图 (时间 vs 关节, 颜色=严重程度)
5. 根因关联网络图 (networkx)
│
▼ 输出
异常事件列表 + 根因分析 + 维护建议
3.3 为什么"肉眼盯屏"不行
维度 肉眼监控 本程序
检测粒度 肉眼分辨率 ~2mm 0.01mm(数据精度)
抖动识别 帧率 25fps 看不到高频抖动 FFT 识别 5-50Hz 抖动
处理速度 8 小时回放 = 8 小时 360 万行 / 30 秒
定位精度 "大概第 3 段有问题" "02:31:47.216, J4 轴, 12Hz, 0.08rad 抖动"
根因分析 "可能机械问题吧" "J4 轴 12Hz 抖动 → 减速机齿轮啮合频率吻合 → 建议检查 J4 减速机"
四、OOP 代码实现
4.1 项目结构
robot_trajectory_anomaly/
├── robot_trajectory_anomaly/
│ ├── __init__.py
│ ├── models.py # 轨迹数据模型
│ ├── data_loader.py # CSV加载与预处理
│ ├── preprocessor.py # 数据预处理(对齐/平滑/差分)
│ ├── offset_detector.py # 轨迹偏移检测 (Isolation Forest)
│ ├── jitter_detector.py # 运动抖动检测 (FFT)
│ ├── root_cause_analyzer.py # 根因关联分析 (networkx)
│ ├── visualizer.py # 可视化
│ └── report_generator.py # 异常报告生成
├── tests/
│ ├── __init__.py
│ └── test_anomaly.py
├── sample_data/
│ ├── joint_trajectory.csv
│ ├── cartesian_trajectory.csv
│ └── theoretical_trajectory.csv
├── results/
│ ├── trajectory_3d_compare.png
│ ├── offset_timeseries.png
│ ├── jitter_spectrum.png
│ ├── anomaly_scatter.png
│ ├── root_cause_network.png
│ └── anomaly_report.txt
├── run_anomaly_detection.py
└── README.md
4.2 核心源码
<details><summary></summary>
"""机器人轨迹数据模型 (dataclass)。"""
from dataclasses import dataclass, field
from typing import List, Optional
import numpy as np
@dataclass
class JointSample:
"""关节采样点。"""
timestamp: float # 秒
joint_id: int # 1-6
angle: float # rad
velocity: float = 0.0 # rad/s
acceleration: float = 0.0 # rad/s²
def to_array(self) -> np.ndarray:
return np.array([self.timestamp, self.joint_id,
self.angle, self.velocity, self.acceleration])
@dataclass
class CartesianSample:
"""笛卡尔空间采样点。"""
timestamp: float
tcp_x: float
tcp_y: float
tcp_z: float
rot_x: float = 0.0
rot_y: float = 0.0
rot_z: float = 0.0
@property
def position(self) -> np.ndarray:
return np.array([self.tcp_x, self.tcp_y, self.tcp_z])
@dataclass
class TheoreticalPoint:
"""理论轨迹点。"""
timestamp: float
ref_x: float
ref_y: float
ref_z: float
@dataclass
class AnomalyEvent:
"""异常事件。"""
anomaly_id: str
anomaly_type: str # "offset" / "jitter" / "both"
start_time: float
end_time: float
affected_joints: List[int] = field(default_factory=list)
affected_segments: List[str] = field(default_factory=list)
# 偏移相关
max_offset_mm: float = 0.0
mean_offset_mm: float = 0.0
# 抖动相关
jitter_frequency_hz: float = 0.0
jitter_amplitude: float = 0.0
# 异常得分
anomaly_score: float = 0.0
# 根因
root_cause: str = ""
def to_dict(self) -> dict:
return {
"anomaly_id": self.anomaly_id,
"type": self.anomaly_type,
"start_time": self.start_time,
"end_time": self.end_time,
"affected_joints": self.affected_joints,
"max_offset_mm": self.max_offset_mm,
"jitter_frequency_hz": self.jitter_frequency_hz,
"anomaly_score": self.anomaly_score,
"root_cause": self.root_cause,
}
</details>
<details><summary></summary>
"""机器人轨迹数据加载器 (pandas)。"""
import pandas as pd
import numpy as np
from pathlib import Path
from typing import List, Tuple, Dict
from .models import JointSample, CartesianSample, TheoreticalPoint
class DataLoader:
"""CSV 数据加载与校验。"""
def __init__(self):
pass
def load_joint_trajectory(self, csv_path: str) -> List[JointSample]:
"""加载关节轨迹数据。"""
df = pd.read_csv(csv_path)
samples = []
for _, row in df.iterrows():
samples.append(JointSample(
timestamp=float(row["timestamp"]),
joint_id=int(row["joint_id"]),
angle=float(row["angle"]),
velocity=float(row.get("velocity", 0.0)),
acceleration=float(row.get("acceleration", 0.0)),
))
return samples
def load_cartesian_trajectory(self, csv_path: str) -> List[CartesianSample]:
"""加载笛卡尔轨迹数据。"""
df = pd.read_csv(csv_path)
samples = []
for _, row in df.iterrows():
samples.append(CartesianSample(
timestamp=float(row["timestamp"]),
tcp_x=float(row["tcp_x"]),
tcp_y=float(row["tcp_y"]),
tcp_z=float(row["tcp_z"]),
rot_x=float(row.get("rot_x", 0.0)),
rot_y=float(row.get("rot_y", 0.0)),
rot_z=float(row.get("rot_z", 0.0)),
))
return samples
def load_theoretical_trajectory(self, csv_path: str) -> List[TheoreticalPoint]:
"""加载理论轨迹。"""
df = pd.read_csv(csv_path)
points = []
for _, row in df.iterrows():
points.append(TheoreticalPoint(
timestamp=float(row["timestamp"]),
ref_x=float(row["ref_x"]),
ref_y=float(row["ref_y"]),
ref_z=float(row["ref_z"]),
))
return points
def generate_synthetic_data(self, duration_sec: float = 60.0,
sample_rate_hz: float = 125.0,
inject_anomalies: bool = True
) -> Tuple[List[JointSample], List[CartesianSample],
List[TheoreticalPoint]]:
"""
生成合成机器人轨迹数据 (教学用)。
模拟 6 轴机器人焊接直线+圆弧轨迹。
"""
np.random.seed(42)
dt = 1.0 / sample_rate_hz
n_samples = int(duration_sec * sample_rate_hz)
# 理论轨迹: 直线 + 圆弧
theo_points = []
cart_samples = []
joint_samples = []
t = np.arange(n_samples) * dt
# 直线段: 0-20s
mask_line = t < 20.0
n_line = np.sum(mask_line)
if n_line > 0:
x_line = np.linspace(500, 800, n_line)
y_line = np.linspace(100, 100, n_line)
z_line = np.linspace(200, 200, n_line)
for i in range(n_line):
theo_points.append(TheoreticalPoint(
timestamp=t[mask_line][i],
ref_x=x_line[i], ref_y=y_line[i], ref_z=z_line[i]
))
# 圆弧段: 20-40s
mask_arc = (t >= 20.0) & (t < 40.0)
n_arc = np.sum(mask_arc)
if n_arc > 0:
angle = np.linspace(0, np.pi, n_arc)
x_arc = 800 + 100 * np.cos(angle)
y_arc = 100 + 100 * np.sin(angle)
z_arc = np.linspace(200, 200, n_arc)
for i in range(n_arc):
theo_points.append(TheoreticalPoint(
timestamp=t[mask_arc][i],
ref_x=x_arc[i], ref_y=y_arc[i], ref_z=z_arc[i]
))
# 剩余: 返回段 40-60s
mask_return = t >= 40.0
n_return = np.sum(mask_return)
if n_return > 0:
x_ret = np.linspace(700, 500, n_return)
y_ret = np.linspace(200, 100, n_return)
z_ret = np.linspace(200, 200, n_return)
for i in range(n_return):
theo_points.append(TheoreticalPoint(
timestamp=t[mask_return][i],
ref_x=x_ret[i], ref_y=y_ret[i], ref_z=z_ret[i]
))
# 实际轨迹 = 理论 + 噪声 + 可选异常注入
for i, tp in enumerate(theo_points):
# 基础噪声 (正常运动噪声 ±0.05mm)
noise_x = np.random.normal(0, 0.05)
noise_y = np.random.normal(0, 0.05)
noise_z = np.random.normal(0, 0.05)
# 异常注入
if inject_anomalies:
# 偏移异常: 30-32s, 偏移 +1.5mm
if 30.0 <= tp.timestamp <= 32.0:
noise_x += 1.5
noise_y += 0.8
# 抖动异常: 45-47s, J4 轴 12Hz 抖动
if 45.0 <= tp.timestamp <= 47.0:
jitter = 0.3 * np.sin(2 * np.pi * 12 * tp.timestamp)
noise_x += jitter
noise_z += jitter * 0.5
cart_samples.append(CartesianSample(
timestamp=tp.timestamp,
tcp_x=tp.ref_x + noise_x,
tcp_y=tp.ref_y + noise_y,
tcp_z=tp.ref_z + noise_z,
))
# 关节数据: 简化模型 (每个采样点 6 个关节)
for i, cs in enumerate(cart_samples):
for j in range(1, 7):
# 简化: 关节角度与末端位置近似线性映射
angle = (cs.tcp_x / 1000.0) + (j * 0.1) + np.random.normal(0, 0.001)
velocity = np.random.normal(0, 0.05)
# 注入 J4 抖动
if inject_anomalies and 45.0 <= cs.timestamp <= 47.0 and j == 4:
velocity += 0.8 * np.sin(2 * np.pi * 12 * cs.timestamp)
joint_samples.append(JointSample(
timestamp=cs.timestamp,
joint_id=j,
angle=angle,
velocity=velocity,
acceleration=np.random.normal(0, 0.1),
))
# 保存 CSV
sample_dir = Path("sample_data")
sample_dir.mkdir(exist_ok=True)
# 关节数据 (采样前10000行避免文件过大)
joint_df = pd.DataFrame([{
"timestamp": js.timestamp,
"joint_id": js.joint_id,
"angle": js.angle,
"velocity": js.velocity,
"acceleration": js.acceleration,
} for js in joint_samples[:10000]])
joint_df.to_csv(sample_dir / "joint_trajectory.csv", index=False)
# 笛卡尔数据
cart_df = pd.DataFrame([{
"timestamp": cs.timestamp,
"tcp_x": cs.tcp_x,
"tcp_y": cs.tcp_y,
"tcp_z": cs.tcp_z,
"rot_x": cs.rot_x,
"rot_y": cs.rot_y,
"rot_z": cs.rot_z,
} for cs in cart_samples[:5000]])
cart_df.to_csv(sample_dir / "cartesian_trajectory.csv", index=False)
# 理论轨迹
theo_df = pd.DataFrame([{
"timestamp": tp.timestamp,
"ref_x": tp.ref_x,
"ref_y": tp.ref_y,
"ref_z": tp.ref_z,
} for tp in theo_points[:5000]])
theo_df.to_csv(sample_dir / "theoretical_trajectory.csv", index=False)
return joint_samples, cart_samples, theo_points
</details>
<details><summary></summary>
"""轨迹数据预处理器。"""
import numpy as np
import pandas as pd
from typing import List, Tuple, Dict
from scipy import signal as scipy_signal
from .models import JointSample, CartesianSample, TheoreticalPoint
class Preprocessor:
"""
轨迹数据预处理:
1. 时间对齐 (实际 vs 理论)
2. 滑动窗口平滑
3. 差分计算速度/加速度
4. 缺失值处理
"""
def __init__(self, window_size: int = 5):
self.window_size = window_size
def align_trajectories(self, cart_samples: List[CartesianSample],
theo_points: List[TheoreticalPoint]
) -> Tuple[np.ndarray, np.ndarray]:
"""
对齐实际轨迹和理论轨迹。
返回: (actual_positions [N×3], reference_positions [N×3])
"""
# 构建理论轨迹时间索引
theo_times = np.array([tp.timestamp for tp in theo_points])
theo_positions = np.array([[tp.ref_x, tp.ref_y, tp.ref_z]
for tp in theo_points])
actual_times = np.array([cs.timestamp for cs in cart_samples])
actual_positions = np.array([cs.position for cs in cart_samples])
# 最近邻插值: 对每个实际采样时间, 找最近的理论点
aligned_ref = []
for t in actual_times:
idx = np.argmin(np.abs(theo_times - t))
aligned_ref.append(theo_positions[idx])
return actual_positions, np.array(aligned_ref)
def smooth_signal(self, signal: np.ndarray,
window: int = None) -> np.ndarray:
"""滑动窗口均值平滑。"""
if window is None:
window = self.window_size
if len(signal) < window:
return signal
smoothed = np.convolve(signal, np.ones(window)/window, mode='same')
# 边界处理
smoothed[:window//2] = signal[:window//2]
smoothed[-window//2:] = signal[-window//2:]
return smoothed
def compute_velocity(self, positions: np.ndarray,
timestamps: np.ndarray) -> np.ndarray:
"""通过差分计算速度。"""
velocities = np.zeros_like(positions)
dt = np.diff(timestamps)
dt[dt == 0] = 1e-6
for i in range(1, len(positions)):
velocities[i] = (positions[i] - positions[i-1]) / dt[i-1]
return velocities
def compute_acceleration(self, velocities: np.ndarray,
timestamps: np.ndarray) -> np.ndarray:
"""通过差分计算加速度。"""
accelerations = np.zeros_like(velocities)
dt = np.diff(timestamps)
dt[dt == 0] = 1e-6
for i in range(1, len(velocities)):
accelerations[i] = (velocities[i] - velocities[i-1]) / dt[i-1]
return accelerations
def position_deviation(self, actual: np.ndarray,
reference: np.ndarray) -> np.ndarray:
"""计算位置偏差 (欧氏距离)。"""
return np.linalg.norm(actual - reference, axis=1)
def detect_gaps(self, timestamps: np.ndarray,
expected_interval: float) -> List[Tuple[float, float]]:
"""检测数据缺失时间段。"""
gaps = []
diffs = np.diff(timestamps)
gap_mask = diffs > expected_interval * 2
for i in np.where(gap_mask)[0]:
gaps.append((timestamps[i], timestamps[i+1]))
return gaps
</details>
<details><summary></summary>
"""轨迹偏移检测器 (scikit-learn Isolation Forest)。"""
import numpy as np
from typing import Dict, List, Tuple
from sklearn.ensemble import IsolationForest
from sklearn.preprocessing import StandardScaler
from .models import AnomalyEvent, CartesianSample, TheoreticalPoint
class OffsetDetector:
"""
使用 Isolation Forest 检测轨迹偏移异常。
特征: [位置偏差, 偏差变化率, 局部偏差均值]
"""
def __init__(self, contamination: float = 0.05):
self.contamination = contamination
self.model_ = IsolationForest(
contamination=contamination,
random_state=42,
n_estimators=100,
)
self.scaler_ = StandardScaler()
def detect(self, cart_samples: List[CartesianSample],
theo_points: List[TheoreticalPoint],
timestamps: np.ndarray,
deviations: np.ndarray) -> List[AnomalyEvent]:
"""
检测偏移异常。
"""
if len(deviations) < 10:
return []
# 构建特征矩阵
features = np.column_stack([
deviations, # 位置偏差
np.abs(np.diff(np.concatenate([[0], deviations]))), # 偏差变化率
self._local_mean(deviations, window=10), # 局部均值
])
# 标准化
features_scaled = self.scaler_.fit_transform(features)
# 异常检测
labels = self.model_.fit_predict(features_scaled)
scores = self.model_.score_samples(features_scaled)
# 提取异常段
anomaly_mask = labels == -1
anomaly_events = []
if not np.any(anomaly_mask):
return []
# 合并连续异常为事件
in_anomaly = False
start_idx = 0
for i, is_anomaly in enumerate(anomaly_mask):
if is_anomaly and not in_anomaly:
in_anomaly = True
start_idx = i
elif not is_anomaly and in_anomaly:
in_anomaly = False
event = self._create_offset_event(
start_idx, i - 1, timestamps, deviations, scores,
cart_samples, theo_points
)
if event:
anomaly_events.append(event)
# 处理末尾异常
if in_anomaly:
event = self._create_offset_event(
start_idx, len(anomaly_mask) - 1, timestamps, deviations,
scores, cart_samples, theo_points
)
if event:
anomaly_events.append(event)
return anomaly_events
def _local_mean(self, data: np.ndarray, window: int) -> np.ndarray:
"""计算局部均值。"""
result = np.zeros_like(data)
for i in range(len(data)):
start = max(0, i - window // 2)
end = min(len(data), i + window // 2 + 1)
result[i] = np.mean(data[start:end])
return result
def _create_offset_event(self, start: int, end: int,
timestamps: np.ndarray,
deviations: np.ndarray,
scores: np.ndarray,
cart_samples: List[CartesianSample],
theo_points: List[TheoreticalPoint]
) -> AnomalyEvent:
"""创建偏移异常事件。"""
if start >= len(timestamps) or end >= len(timestamps):
return None
segment_deviations = deviations[start:end+1]
segment_scores = scores[start:end+1]
return AnomalyEvent(
anomaly_id=f"OFF-{start:06d}",
anomaly_type="offset",
start_time=timestamps[start],
end_time=timestamps[min(end, len(
利用AI解决实际问题,如果你觉得这个工具好用,欢迎关注长安牧笛!
DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐


所有评论(0)