从示教到柔顺轨迹的生成、规划全流程设计

示教到执行的完整链路：
─────────────────────────────────────────────────────
[物理世界] [数字世界]
 │ 拖动示教 │
 ▼ ▼
示教点采集 ──────────────────► 示教数据存储
(6关节角度) (JSON/YAML文件)
 │ 正运动学计算 │
 ▼ ▼
末端位置标定 ─────────────────► 笛卡尔空间验证
 │ 曲线插值 │
 ▼ ▼
关节轨迹生成 ─────────────────► 关节角度序列
(每关节独立) (100Hz, N×6数组)
 │ S型速度规划 │
 ▼ ▼
时间参数化 ───────────────────► 带时间戳的轨迹
 │ 固定频率发送 │
 ▼ ▼
CAN总线 ──────────────────────► 一体化关节
(100Hz目标角度) (1000Hz位置环)
 │ 关节执行 │
 ▼ ▼
末端执行 ──────────────────────► 清洁轨迹完成

数据流向：
─────────────────────────────────────────────────────
示教数据（5–10个点）
1. 数据预处理 │ ← 质量检查、坐标统一、单位转换
2. 曲线规划 │ ← 三次样条插值
3. 速度规划 │ ← S型速度曲线
4. 轨迹预生成 │ ← 固定频率采样（100Hz）
执行控制器（100Hz查表）
CAN总线 ──► 关节驱动器 ──► 电机

示教数据文件结构示例：

示教文件 = {
 "元数据": {
 "示教ID": "inner_wall_v1",
 "任务类型": "inner_wall | outer_wall | seat",
 "型号": "TOTO_CS325",
 "示教日期": "2026-04-08",
 "操作者": "张工"
 },
 "路径点列表": [
 {
 "编号": 0,
 "类型": "approach | contact | path | retreat",
 "关节角度": [θ₁, θ₂, d₃, θ₄, θ₅, θ₆],
 "末端位置": [x, y, z],
 "接触力": F,
 "接触状态": true/false
 },
 ...（5–10个点）
 ],
 "质量检查": {
 "通过": true/false,
 "平均接触力": 5.3,
 "平均点间距": 65.2
 }
}

示教数据的两种使用方式：
─────────────────────────────────────────────────────
方式A：直接复用（关节空间）
├─ 示教点直接作为插值节点
├─ 在关节空间进行曲线规划
├─ 优点：简单，无需逆运动学
├─ 缺点：笛卡尔空间轨迹可能与示教有偏差
└─ 适用：清洁贴合（末端始终接触，偏差可接受）

方式B：笛卡尔空间校验（推荐）
├─ 示教点转换为笛卡尔空间
├─ 在笛卡尔空间规划路径（直线/螺旋）
├─ 通过逆运动学映射回关节空间
├─ 优点：笛卡尔空间轨迹精确
├─ 缺点：需要逆运动学，增加复杂度
└─ 适用：需要精确笛卡尔轨迹的场景

清洁贴合任务的推荐方式：方式A（关节空间）
├─ 原因：
│ ├─ 清洁任务只需末端贴合，不要求精确笛卡尔轨迹
│ ├─ 关节空间规划无需逆运动学，实现简单
│ └─ 关节空间轨迹天然满足关节限位约束
└─ 注意：通过力传感器实时修正残余偏差

示教数据预处理流程：
─────────────────────────────────────────────────────
Step 1：数据完整性检查
├─ 检查点数量（5–10个）
├─ 检查关节角度是否在限位范围内
├─ 检查接触力是否在合理范围（0–15N）
└─ 检查数据格式是否正确

Step 2：异常点过滤
├─ 删除明显的异常记录（如记录时机械臂抖动）
├─ 方法：检查相邻两点的关节角度差是否过大
│ └─ 单关节角度突变 > 10° → 异常，删除该点
└─ 如果删除了异常点，需要重新示教该段

Step 3：点间距优化
├─ 计算相邻示教点的末端距离
├─ 距离 > 100mm → 警告，建议插入中间点
├─ 距离 < 20mm → 警告，点过于密集
└─ 保持合理间距（50–80mm）

Step 4：数据格式转换
├─ 关节角度：度 → 弧度（内部计算用）
├─ 直线关节：mm → m（内部计算用）
├─ 时间戳：统一为相对时间（0, 1, 2, ...秒）
└─ 保存为标准格式

三次样条插值原理：
─────────────────────────────────────────────────────
什么是三次样条插值？
├─ 通过相邻示教点，拟合平滑的曲线
├─ 每两个点之间用三次多项式连接
├─ 保证整条曲线一阶、二阶导数连续
└─ 效果：位置、速度、加速度连续，无突变

为什么用三次样条？
├─ 三次多项式是最低阶的多项式
│ 能保证位置、速度、加速度连续
├─ 计算量适中，适合嵌入式平台
├─ 数值稳定，不易震荡
└─ 在机械臂轨迹规划中广泛使用

三次样条的几何意义：
─────────────────────────────────────────────────────
示教点：P0, P1, P2, P3, ...（关节角度）
曲线段：S0(P0→P1), S1(P1→P2), S2(P2→P3), ...
每段曲线是三次多项式：
 S(t) = a + b·t + c·t² + d·t³
约束条件（每段曲线有两个端点）：
├─ S(0) = P_start（一阶导数连续）
├─ S(1) = P_end
├─ S'(0) = S'previous(1)（速度连续）
└─ S''(0) = S''previous(1)（加速度连续）
边界条件（自然边界）：
├─ 首末端二阶导数为0
└─ 曲线自然弯曲，无额外约束

弧长参数化原理：
问题：示教时记录的点，参数是什么？
├─ 直接记录：每个点的关节角度
├─ 相邻点的"距离"是什么？
│ └─ 不是时间（示教时不记录时间）
│ └─ 是关节空间的弧长
└─ 弧长 = 关节角度的加权范数
 s = sqrt((Δθ₁)² + (Δθ₂)² + ... + (Δθ₆)²)
为什么需要弧长参数化？
├─ 保证各关节运动速度均匀
├─ 避免示教时"快一段慢一段"的问题
├─ 使速度规划与示教速度无关
└─ 轨迹平滑、可控

弧长参数化的步骤：
─────────────────────────────────────────────────────
Step 1：计算各相邻示教点的弧长
├─ s[i] = ||P[i+1] - P[i]||（关节空间距离）
└─ 累积弧长 S[i] = sum(s[0:i])

Step 2：将示教点参数化为弧长
├─ 新参数：P(S[0]), P(S[1]), P(S[2]), ...
├─ 插值在弧长域进行
└─ 插值函数：θ = f(s)

Step 3：S型速度规划在弧长域进行
├─ 计算期望的末端速度曲线 v(s)
├─ 将速度转换为弧长变化率 ds/dt = v(s)
└─ 积分得到 s(t)，再查表得到 θ(t)

多关节同步原理：
─────────────────────────────────────────────────────
问题：6个关节独立插值，如何保证同时到达各路径点？
解决：统一时间尺度，按弧长比例分配
示例：
├─ Joint 1：从0°到30°，弧长s₁=30°
├─ Joint 2：从0°到60°，弧长s₂=60°
├─ 假设总时间T=3秒

各关节在相同时间到达终点：
├─ t=0：所有关节在起点
├─ t=3s：所有关节在终点
└─ t=1.5s：所有关节都在50%位置

如何实现？
├─ 每个关节独立计算自己的S型速度曲线
├─ 但所有关节使用相同的"时间进度"
├─ 关节i的位置 = 关节i的S曲线(t/T)
└─ 结果：各关节同时到达各路径点

关键：时间进度是统一的（0→1），但各关节
 根据自己的弧长比例映射到实际角度

关节空间三次样条规划流程：
─────────────────────────────────────────────────────
输入：示教点列表（5–10个），每个点6个关节角度

Step 1：构建插值函数
├─ 对每个关节分别构建三次样条
├─ 参数域：累积弧长
├─ 值域：该关节的角度序列
└─ 输出：6个样条函数 f₁(s), f₂(s), ..., f₆(s)

Step 2：计算总弧长
├─ S_total = sum(相邻点弧长)
└─ 这是参数域的总范围

Step 3：速度规划（见第五章）
├─ 确定末端速度曲线 v(s)
├─ 计算 ds/dt = v(s)
└─ 积分得到 s(t)

Step 4：轨迹查询
├─ 输入：时间t
├─ 计算：弧长s(t)
├─ 查询：各关节角度 θᵢ = fᵢ(s)
└─ 输出：6个关节的目标角度

三次样条的特点：
├─ 位置连续（曲线经过所有示教点）
├─ 速度连续（关节运动无突变）
├─ 加速度连续（无冲击）
├─ 笛卡尔空间轨迹可能与直线有偏差
└─ 实现简单，适合清洁贴合任务

笛卡尔空间路径校验流程：
─────────────────────────────────────────────────────
目的：检查关节空间插值后的笛卡尔空间轨迹是否满足要求

Step 1：关节角度 → 笛卡尔位置
├─ 对每个插值点，计算正运动学
├─ FK(P[i]) = [x, y, z, roll, pitch, yaw]
└─ 得到笛卡尔空间的轨迹点序列

Step 2：计算轨迹偏差
├─ 示教时记录的末端位置序列：P_teach[i]
├─ 插值后的末端位置序列：P_interp[i]
├─ 偏差 = ||P_teach[i] - P_interp[i]||
└─ 偏差应 < 5mm

Step 3：偏差处理
├─ 偏差 < 3mm 通过
├─ 偏差 3–5mm 可接受，需确认
├─ 偏差 > 5mm 偏差过大
│ └─ 解决：增加示教点密度
└─ 偏差 > 10mm 严重问题
 └─ 解决：重新示教

为什么允许一定偏差？
├─ 清洁任务只需末端贴合表面
├─ 5mm偏差可被力控制器的柔顺机构吸收
├─ 力传感器会检测接触力，过深则退出
└─ 因此：对笛卡尔轨迹精度要求不高

路径规划的类型选择：
─────────────────────────────────────────────────────
类型1：点对点路径（P2P）
├─ 描述：仅关心起点和终点
├─ 示教点：仅首末端（2个点）
├─ 轨迹：关节空间直线插值
├─ 特点：简单，但速度不均匀
└─ 适用：接近/撤离动作

类型2：过点路径（Through Points）✅ 推荐
├─ 描述：必须经过所有示教点
├─ 示教点：5–10个点
├─ 轨迹：三次样条插值
├─ 特点：平滑、可控
└─ 适用：清洁贴合轨迹

类型3：笛卡尔空间路径
├─ 描述：在笛卡尔空间规划特定形状
├─ 示教点：仅关键点，通过插值生成目标形状
├─ 轨迹：直线插值/圆弧插值
├─ 特点：笛卡尔轨迹精确
└─ 适用：需要精确笛卡尔轨迹的场景

清洁贴合任务的推荐：类型2（过点路径）
├─ 理由：
│ ├─ 清洁轨迹需要沿马桶表面移动
│ ├─ 示教点已经定义了合理的路径
│ ├─ 三次样条保证运动平滑
│ └─ 无需额外的笛卡尔空间规划
└─ 注意：确保示教点覆盖整个清洁面

S型速度曲线原理：
─────────────────────────────────────────────────────
什么是S型速度曲线？
├─ 速度随时间呈S形变化
├─ 包含：加加速段、匀加速段、减加速段、匀速段、减减速段、匀减速段、加减速段
├─ 特点：加加速度有限，无冲击
└─ 广泛应用于机械臂速度规划

S型速度曲线的物理意义：
├─ 加加速段：加速度从0开始逐渐增大
├─ 匀加速段：加速度保持恒定
├─ 减加速段：加速度逐渐减小到0
├─ 匀速段：速度保持恒定
└─ 后续段：对称的减速过程

为什么需要S型速度？
├─ 避免速度突变（产生冲击）
├─ 避免加速度突变（产生振动）
├─ 保护机械结构，延长寿命
├─ 提高运动平稳性，清洁效果更好
└─ 减小电机电流冲击

S型速度规划参数推荐值：
─────────────────────────────────────────────────────
旋转关节（J1/J2/J4/J5/J6）：
├─ 最大速度 V_max：30–45 °/s
│ └─ 理由：清洁作业中等速度，兼顾效率和平稳
│
├─ 最大加速度 A_max：60–90 °/s²
│ └─ 理由：避免过大的惯性力，保护机械结构
│
└─ 最大加加速度 J_max：120–180 °/s³
 └─ 理由：加加速度连续，无冲击感

直线关节（J3）：
├─ 最大速度 V_max：50–80 mm/s
│ └─ 理由：Z轴升降速度适中
│
├─ 最大加速度 A_max：100–150 mm/s²
│ └─ 理由：避免垂直方向冲击
│
└─ 最大加加速度 J_max：200–300 mm/s³
 └─ 理由：升降运动更平滑

参数调整方法：
─────────────────────────────────────────────────────
Step 1：从保守参数开始
├─ V_max = 30°/s（旋转）/ 50mm/s（直线）
├─ A_max = 60°/s²（旋转）/ 100mm/s²（直线）
└─ J_max = 120°/s³（旋转）/ 200mm/s³（直线）

Step 2：逐步提速
├─ 每次增加V_max 10%
├─ 观察末端运动平稳性
└─ 直到出现轻微抖动

Step 3：回退到安全值
├─ 抖动临界值 × 80%
├─ 留有20%余量应对负载变化
└─ 最终参数需实地验证

多关节速度同步原理：
─────────────────────────────────────────────────────
问题：6个关节的运动范围不同，如何统一速度规划？
解决：以"最慢关节"为基准，其他关节按比例缩放
示例：
├─ Joint 1：弧长30°，V_max=30°/s，理论时间=1s
├─ Joint 2：弧长60°，V_max=60°/s，理论时间=1s
├─ Joint 3：弧长100mm，V_max=50mm/s，理论时间=2s（最慢）

统一时间：T = 2s（以Joint 3为基准）

各关节缩放后的速度：
├─ Joint 1：V_new = 30°/2s = 15°/s（缩放到2s）
├─ Joint 2：V_new = 60°/2s = 30°/s（缩放到2s）
└─ Joint 3：V_new = 50mm/s（保持不变）

结果：
├─ 所有关节同时开始、同时结束
├─ 各关节根据自己的弧长分配速度
└─ 轨迹平滑、无冲击

轨迹基本要求：
─────────────────────────────────────────────────────
要求1：位置连续
├─ 轨迹必须经过所有示教点
├─ 示教点之间的曲线必须平滑
├─ 验收：检查相邻轨迹点的位置差，无突变
└─ 标准：位置差 < 0.1mm

要求2：速度连续
├─ 关节速度必须连续，无突变
├─ 验收：检查速度-时间曲线，无尖峰
└─ 标准：速度变化率 < A_max

要求3：加速度连续
├─ 关节加速度必须连续，无冲击
├─ 验收：检查加速度-时间曲线，无突变
└─ 标准：加速度变化率 < J_max

要求4：时间同步
├─ 各关节必须同时到达各路径点
├─ 验收：检查各关节的时间进度是否一致
└─ 标准：时间偏差 < 10ms

要求5：关节限位
├─ 所有轨迹点必须在关节限位范围内
├─ 验收：检查所有轨迹点的关节角度
└─ 标准：所有角度在 [θ_min, θ_max] 内

轨迹验收标准检查表：
─────────────────────────────────────────────────────
1. 位置精度
 ├─ 各示教点的关节角度误差 < 0.1°
 └─ 各示教点的末端位置误差 < 1mm

2. 速度性能
 ├─ 最大关节速度 < V_max
 ├─ 最大关节加速度 < A_max
 └─ 最大关节加加速度 < J_max

3. 时间性能
 ├─ 轨迹总时间符合预期（可接受±10%偏差）
 └─ 各关节时间同步误差 < 10ms

4. 运动平稳性
 ├─ 关节速度无突变
 ├─ 关节加速度无冲击
 └─ 末端运动无抖动

5. 安全性
 ├─ 所有轨迹点在关节限位范围内
 ├─ 无奇异点姿态
 └─ 接触力在安全范围内（<15N）

轨迹可视化验证流程：
─────────────────────────────────────────────────────
Step 1：关节角度曲线
├─ X轴：时间
├─ Y轴：各关节角度（6条曲线）
├─ 检查：曲线是否平滑经过示教点
└─ 异常：尖峰、突变 → 重新规划

Step 2：关节速度曲线
├─ X轴：时间
├─ Y轴：各关节速度（6条曲线）
├─ 检查：速度是否在限制范围内
└─ 异常：超速 → 降低V_max

Step 3：末端轨迹曲线
├─ X/Y/Z轴：笛卡尔位置
├─ 显示：3D轨迹曲线
├─ 检查：轨迹是否覆盖目标清洁面
└─ 异常：偏离目标区域 → 重新示教

Step 4：接触力曲线（回放时）
├─ X轴：时间
├─ Y轴：接触力
├─ 检查：接触力是否在3–10N范围内
└─ 异常：过大/过小 → 调整示教点或速度参数

为什么需要固定频率？
├─ 机械臂控制系统需要确定性时序
├─ 避免因计算负载变化导致控制抖动
├─ 简化实时性设计
└─ 提高系统可靠性

控制频率选择：
├─ 推荐值：100Hz（每10ms一个控制周期）
├─ 理由：
│ ├─ CAN总线带宽充足（1Mbps）
│ ├─ 一体化关节响应快（<5ms）
│ ├─ 末端速度50mm/s时，每周期移动0.5mm
│ └─ 足够平滑，兼顾计算负载
└─ 范围：50–200Hz均可

固定频率控制的工作流程：
─────────────────────────────────────────────────────
每个控制周期（10ms）：
├─ Step 1：读取当前时间t
├─ Step 2：查表获取目标角度
│ ├─ 在预生成的轨迹数组中查找
│ ├─ θ_target = trajectory[ index(t) ]
│ └─ index = floor(t × 100Hz)
│
├─ Step 3：发送目标角度到关节
│ ├─ 通过CAN总线发送
│ └─ 报文格式：[关节1角度, 关节2角度, ...]
│
├─ Step 4：关节内部执行
│ ├─ 关节控制器接收目标角度
│ ├─ 关节内部1000Hz位置环执行
│ └─ 电机驱动关节运动到目标角度
│
└─ Step 5：等待下一个周期
 └─ sleep(10ms - 计算耗时)

轨迹预生成机制：
─────────────────────────────────────────────────────
为什么预生成轨迹？
├─ 实时计算轨迹会占用大量CPU
├─ 预生成后，运行时仅需查表
├─ 保证确定性执行时序
└─ 简化实时控制逻辑

预生成时机：
├─ 方案A：示教完成后立即生成 ✅ 推荐
│ ├─ 用户点击"保存示教"
│ ├─ 系统自动生成轨迹
│ ├─ 保存到轨迹文件
│ └─ 回放时直接加载
│
└─ 方案B：回放前生成
 ├─ 用户点击"开始回放"
 ├─ 系统从示教数据生成轨迹
 ├─ 保存到内存
 └─ 回放时从内存读取

预生成的数据格式：
─────────────────────────────────────────────────────
trajectory = {
 "轨迹点数": N,
 "控制频率": 100, // Hz
 "总时间": T, // 秒
 "关节1角度序列": [θ₁₀, θ₁₁, ..., θ₁ₙ],
 "关节2角度序列": [θ₂₀, θ₂₁, ..., θ₂ₙ],
 ...
 "关节6角度序列": [θ₆₀, θ₆₁, ..., θ₆ₙ],
 "时间戳序列": [t₀, t₁, ..., tₙ]
}

存储估算：
├─ 100Hz × 10秒轨迹 = 1000个点
├─ 每个点：6个float64 = 48字节
├─ 总存储：1000 × 48 ≈ 48KB
└─ 加上时间戳：约50KB，非常小

通信与监控设计：
─────────────────────────────────────────────────────
CAN总线通信：
├─ 波特率：1Mbps
├─ 控制频率：100Hz
├─ 报文周期：10ms
├─ 每周期发送：6个关节目标角度（6 × 4字节 = 24字节）
├─ 带宽占用：24 × 100 = 2.4KB/s（远低于1Mbps）
└─ 留有充足余量用于其他报文

关节跟踪监控：
├─ 实时读取各关节实际角度
├─ 与目标角度对比，计算跟踪误差
├─ 误差阈值：
│ ├─ > 2° → 降低整体速度50%
│ ├─ > 3° → 暂停运动，报警
│ └─ 误差恢复 → 自动恢复执行
└─ 目的：保护机械结构，防止超调

通信超时处理：
├─ 超时阈值：20ms（连续2个周期无响应）
├─ 处理：
│ ├─ 立即停止运动
│ ├─ 报警提示
│ └─ 等待故障恢复
└─ 目的：CAN总线故障保护

从示教到执行的完整流程：
─────────────────────────────────────────────────────
阶段1：示教采集
├─ 1.1 操作者进入示教模式（零力矩控制）
├─ 1.2 拖动机械臂沿目标路径移动
├─ 1.3 在关键位置按下"记录点"
├─ 1.4 重复直到完成所有路径点
├─ 1.5 保存示教数据到文件
└─ 输出：示教数据文件（5–10个点）

阶段2：轨迹生成（离线）
├─ 2.1 加载示教数据
├─ 2.2 数据预处理（质量检查、格式转换）
├─ 2.3 三次样条插值（关节空间）
├─ 2.4 S型速度规划
├─ 2.5 重采样到固定频率（100Hz）
├─ 2.6 轨迹质量验证
├─ 2.7 保存轨迹文件
└─ 输出：轨迹数据文件（N×6数组）

阶段3：轨迹验证（离线）
├─ 3.1 轨迹可视化（关节角度、速度、末端轨迹）
├─ 3.2 关节限位检查
├─ 3.3 奇异点检查
├─ 3.4 空中预演（不接触）
└─ 输出：验证报告

阶段4：轨迹执行（在线）
├─ 4.1 加载轨迹文件到内存
├─ 4.2 移动机械臂到轨迹起点
├─ 4.3 启动控制循环（100Hz）
│ ├─ 查表获取当前时刻目标角度
│ ├─ 发送目标角度到关节
│ └─ 监控关节跟踪误差
├─ 4.4 轨迹执行完成
└─ 输出：清洁动作完成

阶段5：效果评估
├─ 5.1 记录接触力曲线
├─ 5.2 分析清洁覆盖率
├─ 5.3 与示教数据对比
└─ 输出：执行报告

关键文件格式：
─────────────────────────────────────────────────────
示教数据文件（.json）：
├─ teach_id: 示教编号
├─ task_type: 任务类型（inner_wall/outer_wall/seat）
├─ points: 路径点列表
│ ├─ joints: 关节角度 [θ₁, θ₂, d₃, θ₄, θ₅, θ₆]
│ ├─ force: 接触力
│ ├─ type: 点类型（approach/contact/path/retreat）
│ └─ end_pos: 末端笛卡尔位置
└─ quality_check: 质量检查结果

轨迹数据文件（.npz 或 .bin）：
├─ trajectory_joints: shape=(N, 6), dtype=float64
├─ trajectory_time: shape=(N,), dtype=float64
└─ metadata: 包含控制频率、总时间等

配置文件（.yaml）：
├─ 速度规划参数：V_max, A_max, J_max
├─ 控制参数：控制频率、误差阈值
├─ 关节限位：θ_min, θ_max
└─ 安全参数：最大接触力、碰撞阈值