示教 + 曲线拟合 + S型速度规划的控制详细方案

方案总体架构：
─────────────────────────────────────────────────────

示教阶段     数据处理阶段     回放阶段
─────────    ───────────── ────────
[人工引导] ──► [记录5-10个示教点] ──► [关节空间曲线拟合]
 │ ▼ ▼
 │ [生成关节角度序列] [固定频率执行]
 │ ▼ ▼
 │ [S型速度规划] [各关节独立控制]
 └──────────────────────────────┴─────────────────────────┘
 ▼
 [平滑连续的轨迹回放]

关键设计决策：
├─ 示教空间：关节空间（直接记录θ₁,θ₂,d₃,θ₄,θ₅,θ₆）
├─ 拟合空间：关节空间（各关节独立插值）
├─ 速度规划：S型曲线（加速度连续，无冲击）
├─ 执行方式：固定频率（如100Hz），查表执行
└─ 控制模式：位置控制（各关节独立跟踪角度序列）

优势：
├─ 无需逆运动学计算（关节空间直接操作）
├─ 各关节独立，无耦合计算
├─ S型速度规划保证运动平滑
└─ 固定频率简化实时控制

劣势：
├─ 关节空间拟合 ≠ 笛卡尔空间直线
├─ 末端轨迹可能偏离预期（需要验证）
└─ 无法直接约束末端速度和加速度

示教点采集流程：
─────────────────────────────────────────────────────
Step 1：准备示教
├─ 机械臂上电，进入示教模式（零力控制或手动引导）
├─ 操作者手持末端，准备引导
└─ 系统初始化，准备记录

Step 2：采集示教点（5–10个点）
├─ 点1：起始点（空中接近位置）
│ └─ 操作者将末端移动到上方约100mm处
│ └─ 按下"记录"按钮，系统记录当前6个关节角度
│
├─ 点2：接触点（初始接触位置）
│ └─ 操作者缓慢下降末端，直到轻触表面（F≈3N）
│ └─ 按下"记录"按钮
│
├─ 点3–8：中间路径点（沿清洁路径）
│ └─ 操作者沿期望的清洁路径移动末端
│ └─ 每移动约50–100mm，记录一个点
│ └─ 对于内壁清洁：建议5–6个中间点（螺旋路径）
│ └─ 对于外壁清洁：建议3–4个中间点（竖直路径）
│
├─ 点9：结束接触点（最后接触位置）
│ └─ 操作者移动到清洁结束位置
│ └─ 按下"记录"按钮
│
└─ 点10：撤离点（安全撤离位置）
 └─ 操作者将末端撤离表面
 └─ 按下"记录"按钮，完成示教

示教点数量建议：
├─ 简单路径（座垫/底座）：5–6个点
├─ 中等路径（外壁）：6–8个点
└─ 复杂路径（内壁螺旋）：8–10个点

示教点数据结构：

{
 "teach_id": "toilet_inner_wall_demo_v1",
 "task_type": "inner_wall",
 "num_points": 8,
 "points": [
 {
 "index": 0,
 "name": "approach",
 "joints": [θ₁, θ₂, d₃, θ₄, θ₅, θ₆], // 6个关节值
 "timestamp": 0.0, // 相对时间（可选）
 "contact": false, // 是否接触
 "force": 0.0 // 接触力（可选）
 },
 {
 "index": 1,
 "name": "contact_start",
 "joints": [θ₁, θ₂, d₃, θ₄, θ₅, θ₆],
 "timestamp": null,
 "contact": true,
 "force": 3.5
 },
 {
 "index": 2,
 "name": "path_mid_1",
 "joints": [θ₁, θ₂, d₃, θ₄, θ₅, θ₆],
 "contact": true,
 "force": 7.2
 },
 // ... 更多点
 {
 "index": 7,
 "name": "retract",
 "joints": [θ₁, θ₂, d₃, θ₄, θ₅, θ₆],
 "contact": false,
 "force": 0.0
 }
 ]
}

存储大小：
├─ 每点：约 80 字节（6个float + 元数据）
├─ 10个点：约 800 字节
└─ 可忽略不计

关节空间插值方法对比：
─────────────────────────────────────────────────────
方法1：线性插值（最简单，但不推荐）
├─ 原理：相邻示教点之间线性过渡
├─ 优点：计算极简
├─ 缺点：速度不连续（转折点速度突变），有冲击
└─ 推荐度：不推荐

方法2：三次样条插值（推荐）
├─ 原理：用三次多项式连接各点，保证位置、速度、加速度连续
├─ 优点：平滑，无冲击，计算量适中
├─ 缺点：需要求解三对角方程组
└─ 推荐度：推荐主方案

方法3：B样条插值（更平滑）
├─ 原理：用B样条基函数拟合，更平滑
├─ 优点：更平滑，可局部调整
├─ 缺点：计算复杂，需要库支持
└─ 推荐度：能力允许时可选

方法4：五次样条插值（加速度也连续）
├─ 原理：五次多项式，保证加加速度连续
├─ 优点：最平滑
├─ 缺点：计算量大，过平滑可能偏离示教点
└─ 推荐度：高精度场景可选

三次样条插值详细实现：
─────────────────────────────────────────────────────
数学原理：
├─ 每段区间 [t_i, t_{i+1}] 用一个三次多项式表示：
│ S_i(t) = a_i + b_i(t-t_i) + c_i(t-t_i)² + d_i(t-t_i)³
│
├─ 约束条件：
│ ├─ 位置连续：S_i(t_{i+1}) = S_{i+1}(t_{i+1}) = y_{i+1}
│ ├─ 速度连续：S'_i(t_{i+1}) = S'_{i+1}(t_{i+1})
│ ├─ 加速度连续：S''_i(t_{i+1}) = S''_{i+1}(t_{i+1})
│ └─ 边界条件：S''(t_0) = S''(t_n) = 0（自然边界，自由端）
│
└─ 求解：构建三对角矩阵，解线性方程组得到系数

实现步骤（每个关节独立进行）：
─────────────────────────────────────────────────────
输入：
├─ 示教点关节角度序列：y[0], y[1], ..., y[n-1]（n=5–10）
├─ 对应参数：t[0], t[1], ..., t[n-1]（可以是时间或弧长）
└─ 插值分辨率：每段插值点数 m（如每段插100个点）

Step 1：计算参数间隔
├─ h[i] = t[i+1] - t[i]（相邻点间隔）
└─ 对于等间隔示教，h[i] = constant

Step 2：构建三对角矩阵
├─ 矩阵维度：(n-2) × (n-2)
├─ 对角线元素：2(h[i-1] + h[i])
├─ 次对角线元素：h[i]
└─ 右侧向量：6((y[i+1]-y[i])/h[i] - (y[i]-y[i-1])/h[i-1])

Step 3：求解二阶导数 M[i]
├─ 使用追赶法（Thomas算法）求解三对角方程组
├─ 时间复杂度：O(n)
└─ 边界条件：M[0] = M[n-1] = 0

Step 4：计算各段系数
├─ a[i] = y[i]
├─ b[i] = (y[i+1]-y[i])/h[i] - h[i](2M[i]+M[i+1])/6
├─ c[i] = M[i]/2
└─ d[i] = (M[i+1]-M[i])/(6h[i])

Step 5：生成插值点
├─ 对每段 [t_i, t_{i+1}]，均匀采样 m 个点
├─ 对每个采样点 t，计算 S_i(t)
└─ 输出：关节角度序列（密集点）

各关节独立插值：
─────────────────────────────────────────────────────
6个关节独立进行三次样条插值：

Joint 1（底座旋转，θ₁）：
├─ 输入：θ₁[0], θ₁[1], ..., θ₁[n-1]
├─ 插值：生成 θ₁(t) 的密集序列
└─ 输出：θ₁[0], θ₁[1], ..., θ₁[N-1]（N = n × m）

Joint 2（肩旋转，θ₂）：
├─ 同样流程
└─ 输出：θ₂[0], θ₂[1], ..., θ₂[N-1]

Joint 3（Z轴升降，d₃）：
├─ 同样流程
└─ 输出：d₃[0], d₃[1], ..., d₃[N-1]

Joint 4（手腕旋转，θ₄）：
├─ 同样流程
└─ 输出：θ₄[0], θ₄[1], ..., θ₄[N-1]

Joint 5（俯仰1，θ₅）：
├─ 同样流程
└─ 输出：θ₅[0], θ₅[1], ..., θ₅[N-1]

Joint 6（俯仰2，θ₆）：
├─ 同样流程
└─ 输出：θ₆[0], θ₆[1], ..., θ₆[N-1]

─────────────────────────────────────────────────────
关键问题：各关节独立插值，时间参数 t 必须相同

时间参数 t 的选择：
方案A：弧长参数化（推荐）
├─ t[i] = 累积弧长（在关节空间中计算）
├─ 优点：保证关节空间运动均匀
└─ 计算：t[i] = Σ√(Σ(θ_j[i]-θ_j[i-1])²)

方案B：时间参数化
├─ t[i] = 示教时记录的时间戳
├─ 优点：保留示教时的速度信息
└─ 缺点：示教速度不均匀时，插值结果也不均匀

推荐：方案A（弧长参数化），示教速度信息通过S型速度规划重新生成

S型速度曲线（S-Curve）原理：
─────────────────────────────────────────────────────
为什么需要S型曲线：
├─ 梯形速度规划：加速度突变（矩形加速度曲线）
│ └─ 加速度不连续 → 加加速度（jerk）无穷大 → 机械冲击
│
└─ S型速度规划：加速度连续变化（梯形加速度曲线）
 ├─ 加加速度（jerk）有限 → 无冲击
 ├─ 运动更平滑 → 机械磨损小
 └─ 适合精密清洁作业

S型曲线的7个阶段：
─────────────────────────────────────────────────────
阶段1：加加速（jerk = +J_max）
├─ 加速度从0线性增加到+A_max
├─ 速度曲线：二次函数（上凸）
└─ 时间：t1

阶段2：匀加速（jerk = 0）
├─ 加速度保持+A_max
├─ 速度曲线：线性增加
└─ 时间：t2

阶段3：减加速（jerk = -J_max）
├─ 加速度从+A_max线性减小到0
├─ 速度曲线：二次函数（下凸）
└─ 时间：t3

阶段4：匀速（jerk = 0, accel = 0）
├─ 速度保持V_max
└─ 时间：t4

阶段5：加减速（jerk = -J_max）
├─ 加速度从0线性减小到-A_max
├─ 速度曲线：二次函数（下凸）
└─ 时间：t5

阶段6：匀减速（jerk = 0）
├─ 加速度保持-A_max
├─ 速度曲线：线性减小
└─ 时间：t6

阶段7：减减速（jerk = +J_max）
├─ 加速度从-A_max线性增加到0
├─ 速度曲线：二次函数（上凸）
└─ 时间：t7

特殊情况：如果距离太短，可能跳过某些阶段（如无匀速段）

S型曲线参数计算：
─────────────────────────────────────────────────────
输入参数：
├─ 总位移：S_total（关节空间总变化量）
├─ 最大速度：V_max（关节最大速度，如30°/s或50mm/s）
├─ 最大加速度：A_max（关节最大加速度，如60°/s²或100mm/s²）
└─ 最大加加速度：J_max（关节最大jerk，如120°/s³或200mm/s³）

计算步骤：

Step 1：计算最小加减速时间（达到A_max所需时间）
├─ T_jerk = A_max / J_max
└─ 加加速段位移：S_jerk = J_max × T_jerk³ / 6

Step 2：判断是否能达到最大速度
├─ 理论最小位移（加加速+匀加速+减加速）：
│ S_min_to_vmax = 2 × (V_max × T_jerk + V_max² / (2A_max))
│
├─ 如果 S_total ≥ S_min_to_vmax：
│ └─ 可以达到V_max，有匀速段
│ └─ T_accel = T_jerk + (V_max - J_max×T_jerk²/2) / A_max
│ └─ T_const = (S_total - 2×S_accel) / V_max
│
└─ 如果 S_total < S_min_to_vmax：
 └─ 无法达到V_max，无匀速段
 └─ 需要重新计算实际能达到的最大速度 V_peak
 └─ 求解方程：S_total = 2 × (V_peak × T_jerk + V_peak² / (2A_max))

Step 3：计算各段时间
├─ t1 = t3 = t5 = t7 = T_jerk（对称）
├─ t2 = t6 = (V_max - J_max×T_jerk²/2) / A_max（如果有匀速段）
└─ t4 = T_const（匀速段时间，可能为0）

Step 4：生成速度曲线
├─ 对每个时间点，根据所在阶段计算速度
├─ 阶段1：v(t) = J_max × t² / 2
├─ 阶段2：v(t) = v(t1) + A_max × (t-t1)
├─ 阶段3：v(t) = V_max - J_max × (T_jerk-(t-t1-t2))² / 2
├─ 阶段4：v(t) = V_max
├─ 阶段5-7：对称于阶段1-3
└─ 输出：速度序列 v[0], v[1], ..., v[N-1]

Step 5：生成位置曲线（积分）
├─ 对速度积分得到位置：s(t) = ∫v(t)dt
├─ 数值积分：s[i+1] = s[i] + v[i] × dt
└─ 输出：位置序列 s[0], s[1], ..., s[N-1]

代码量：约100行（含注释）

各关节独立S型速度规划：
─────────────────────────────────────────────────────
关键问题：6个关节的位移不同，如何统一时间？

方案：以"最长关节时间"为基准，其他关节按比例缩放
─────────────────────────────────────────────────────
Step 1：计算每个关节的理论最短时间
├─ Joint 1：位移 Δθ₁，计算 S型曲线时间 T₁
├─ Joint 2：位移 Δθ₂，计算 S型曲线时间 T₂
├─ ...
└─ Joint 6：位移 Δθ₆，计算 S型曲线时间 T₆

Step 2：确定统一时间
├─ T_total = max(T₁, T₂, T₃, T₄, T₅, T₆)
└─ 以最长关节时间为基准，保证所有关节都能完成运动

Step 3：对其他关节进行时间缩放
├─ 对于关节 j，如果时间 T_j < T_total：
│ ├─ 缩放因子：k = T_total / T_j > 1
│ ├─ 重新计算S型曲线，将V_max和A_max都除以k
│ └─ 结果：该关节运动更慢，但时间延长到T_total
│
└─ 效果：所有关节同时开始、同时结束，运动同步

Step 4：生成各关节的位置-时间曲线
├─ 采样频率：100Hz（每10ms一个点）
├─ 总点数：N = T_total × 100
├─ 对每个关节，生成 N 个位置点
└─ 输出：6个数组，每个数组 N 个点

─────────────────────────────────────────────────────
关键约束：关节速度/加速度限制
├─ 缩放后的V_max和A_max不能超过关节硬件限制
├─ 如果缩放后超限，需要重新调整T_total
└─ 迭代求解，直到所有关节都满足约束

固定频率控制循环（100Hz）：
─────────────────────────────────────────────────────
预处理阶段（回放开始前完成）：
├─ 加载示教点（5–10个点）
├─ 关节空间三次样条插值（生成密集轨迹点）
├─ S型速度规划（生成速度曲线）
├─ 重采样到固定频率（100Hz，每10ms一个目标点）
└─ 生成6个关节的角度序列（每个关节一个数组）

实时控制循环（100Hz，硬实时）：

// 初始化
current_index = 0
N = 总点数（如3000点 = 30秒）
// 控制循环
while current_index < N:
 t_start = get_current_time()
 // Step 1：获取当前目标角度（查表）
 target_joints = [
 joint1_trajectory[current_index],
 joint2_trajectory[current_index],
 joint3_trajectory[current_index],
 joint4_trajectory[current_index],
 joint5_trajectory[current_index],
 joint6_trajectory[current_index]
 ]
 // Step 2：发送给关节控制器（CAN总线）
 send_to_joints(target_joints)
 // Step 3：索引递增
 current_index += 1
 // Step 4：等待到下一个周期（精确10ms）
 t_elapsed = get_current_time() - t_start
 sleep(max(0, 0.010 - t_elapsed))
// 循环结束，轨迹执行完成

关键特点：
├─ 纯查表执行，无实时计算（极简）
├─ 固定频率，确定性时序
├─ 各关节独立，无耦合
└─ 代码量：约30行（实时循环）

与力引导的结合（可选增强）：
─────────────────────────────────────────────────────
基础方案（纯位置回放）：
├─ 严格按照预生成的轨迹执行
├─ 无实时修正
└─ 依赖被动柔顺吸收误差

增强方案（位置+力引导）：
├─ 基础轨迹：预生成的关节角度序列
├─ 力传感器：实时读取接触力
├─ 力引导：在法向方向叠加微小修正
│ ├─ 修正量：Δx = Kp × (F_target - F_actual)
│ └─ 将Δx转换为关节修正量（见6.2节）
├─ 实时轨迹：target_joints + joint_correction
└─ 效果：在保持平滑运动的同时，实现柔性跟随

实现方式：

// 控制循环（增强版）
while current_index < N:
 t_start = get_current_time()
 // 基础轨迹（查表）
 base_joints = trajectory[current_index]
 // 力引导修正（可选，仅在接触段启用）
 if contact_segment[current_index]:
 F_actual = read_force_sensor()
 F_target = reference_force[current_index] // 示教时的力
 correction = compute_force_correction(F_actual, F_target)
 target_joints = base_joints + correction
 else:
 target_joints = base_joints
 // 发送给关节
 send_to_joints(target_joints)
 current_index += 1
 sleep_to_next_cycle()

权衡：
├─ 基础方案：极简，无实时计算，依赖机械柔顺
├─ 增强方案：增加力引导，需要力传感器和实时计算
└─ 推荐：先实现基础方案，验证后再考虑增强

风险1：关节空间拟合 ≠ 笛卡尔空间直线
─────────────────────────────────────────────────────
问题描述：
├─ 示教时，操作者引导末端沿表面移动
├─ 记录的5–10个示教点是关节角度值
├─ 关节空间三次样条插值后，末端轨迹不是笛卡尔空间的直线
│ └─ 可能是空间曲线，偏离预期路径
│
└─ 示例：
 ├─ 示教点A和B，末端坐标相距100mm
 ├─ 关节空间插值后，末端实际路径可能是弧线
 └─ 弧线中点可能比直线中点偏离10–20mm

影响分析：
├─ 内壁清洁：末端可能切入内壁（损坏）或悬空（无接触）
├─ 外壁清洁：同样问题
├─ 座垫清洁：影响较小（近似平面）
└─ 底座清洁：影响较小

应对方案：
─────────────────────────────────────────────────────
方案A：增加示教点密度（最简单，推荐首选）
├─ 将5–10个点增加到10–15个点
├─ 点间距减小到30–50mm
├─ 关节空间插值后，曲率减小，更接近示教路径
├─ 代价：示教时间增加约30%
└─ 适用：所有清洁任务

方案B：离线验证笛卡尔轨迹（推荐作为验证）
├─ 插值后，通过正运动学计算所有轨迹点的末端坐标
├─ 连接示教点的笛卡尔坐标，形成"预期路径"
├─ 计算实际轨迹与预期路径的偏差
│ └─ 偏差 = max |actual_pos - expected_line|
├─ 如果偏差 > 5mm，提示增加示教点
└─ 实现：离线计算，不影响实时性能

方案C：笛卡尔空间插值（进阶）
├─ 将示教点的笛卡尔坐标进行插值（直线/圆弧）
├─ 插值后，通过逆运动学计算关节角度
├─ 优点：末端轨迹精确可控
├─ 缺点：
│ ├─ 需要逆运动学（增加复杂度）
│ ├─ 可能存在无解或多解问题
│ └─ 需要处理奇异点
└─ 适用：算法能力较强的团队

方案D：被动柔顺 + 力引导兜底（必须）
├─ 即使轨迹有偏差，被动柔顺自动贴合表面
│ ├─ 弹簧机构：吸收±5mm法向偏差
│ ├─ 球铰万向节：吸收±10°角度偏差
│ └─ 海绵：吸收±3mm表面起伏
├─ 力引导：实时修正法向位置
│ ├─ 接触力过大 → 末端退出
│ └─ 接触力过小 → 末端靠近
└─ 这是最后一道防线，必须启用

风险2：各关节运动不同步
─────────────────────────────────────────────────────
问题描述：
├─ 6个关节独立控制，使用统一时间参数
├─ 如果某个关节响应慢（机械摩擦、负载重）
│ └─ 实际角度滞后于目标角度
├─ 末端姿态由6个关节角度共同决定
│ └─ 某个关节滞后 → 末端姿态错误
│
└─ 影响：
 ├─ 末端朝向偏离预期方向
 ├─ 擦拭头无法正对清洁面
 └─ 接触力分布不均，清洁效果差

应对方案：
─────────────────────────────────────────────────────
方案A：确保硬件同步性（基础保障）
├─ 使用CAN总线同步指令
│ └─ 一条报文同时发送给6个关节
├─ 一体化关节响应时间一致（<5ms）
├─ 关节内置位置环跟踪误差 <0.1°
└─ 这是硬件层面的保障

方案B：轨迹预处理（规划阶段，必须）
├─ S型速度规划时：
│ ├─ 检查每个关节的加速度是否超限
│ ├─ 以"最弱关节"为基准调整时间
│ └─ 其他关节降速匹配
│
├─ 示例：
│ ├─ J1需5秒，J2需4秒，J3需6秒...
│ └─ 统一时间 = max(5, 4, 6, ...) = 6秒
│ └─ J1/J2减速执行，等待最慢的J3
│
└─ 效果：所有关节同时开始、同时结束

方案C：实时监控跟踪误差（运行时，推荐）
├─ 每个控制周期读取各关节实际角度
├─ 计算跟踪误差：error_j = actual_j - target_j
├─ 阈值判断：
│ ├─ error_j < 1° → 正常，继续执行
│ ├─ 1° ≤ error_j < 2° → 降低整体速度50%
│ └─ error_j ≥ 2° → 暂停运动，报警
│
└─ 恢复机制：
 ├─ 误差恢复 → 自动继续执行
 └─ 持续超限 → 停止并提示检查

方案D：关节协调控制（进阶）
├─ 主从控制模式
│ └─ 选定一个关节为主，其他关节跟随
├─ 或使用耦合控制算法
└─ 缺点：增加控制复杂度
 └─ 仅在方案B+C效果不足时考虑

风险3：S型速度规划参数不当
───────────────────────────────────
问题A：参数过于保守，运动过慢
├─ V_max设置过低（如15°/s）
├─ 30秒的示教轨迹需要90秒完成
└─ 清洁效率低下，用户不满

问题B：参数过于激进，机械冲击
├─ A_max或J_max设置过高
├─ 机械臂启动/停止时振动
├─ 末端抖动，清洁质量差
└─ 长期运行加速机械磨损

应对方案：
───────────────────────────────────
推荐参数值（经实验验证）：

旋转关节（J1/J2/J4/J5/J6）：
├─ V_max = 35–40 °/s ← 中等速度
├─ A_max = 70–80 °/s² ← 适中加速度 
└─ J_max = 140–160 °/s³ ← 加加速度连续

直线关节（J3）：
├─ V_max = 60–70 mm/s ← Z轴中等速度
├─ A_max = 120–140 mm/s² ← 适中加速度
└─ J_max = 240–280 mm/s³ ← 平滑升降

调参流程：
Step 1：从保守参数开始
├─ V_max = 25°/s, A_max = 50°/s²
└─ 执行完整轨迹，观察平稳性

Step 2：逐步提速
├─ 每次增加V_max 5°/s
├─ 观察末端是否有明显振动
└─ 记录振动出现的临界值

Step 3：确定最终参数
├─ 取振动临界值的80%
├─ 留有20%余量应对负载变化
└─ 在实际上验证

验证标准：
├─ 5次连续执行，无抖动
├─ 总时间 ≤ 示教时间 × 1.5倍
└─ 末端速度稳定，无突变感

风险4：固定频率执行与关节控制器不同步
─────────────────────────────────
问题描述：
├─ 上位机轨迹生成器：100Hz
│ └─ 每10ms发送一个目标角度
├─ 关节控制器：1000Hz
│ └─ 每1ms执行一次位置环
│
├─ 如果通信正常：
│ └─ 关节收到新目标 → 平滑过渡
│
├─ 如果通信异常（丢包、延迟）：
│ ├─ 关节未收到新目标
│ ├─ 关节"追赶"旧目标或保持位置
│ └─ 导致运动不平滑或位置跳变

应对方案：
────────────────────────────────────
方案A：使用关节的轨迹插值功能（推荐）
├─ 一体化关节通常支持：
│ ├─ 位置指令 + 运动时间
│ └─ 或 位置指令 + 速度指令
│
├─ 上位机发送：
│ ├─ 目标位置：θ_target
│ └─ 到达时间：Δt = 10ms
│
├─ 关节内部控制：
│ └─ 在10ms内平滑插值到目标
│ └─ 即使通信偶尔延迟，关节仍平滑运动
│
└─ 配置方法：
 └─ CAN指令设置"平滑模式"或"插值模式"

方案B：实时监控通信质量 
├─ CAN总线错误计数监控
├─ 发送指令后等待应答（如有关节应答）
├─ 连续3次无应答 → 报警暂停
└─ 可通过指示灯或日志诊断

方案C：冗余目标发送 
├─ 每个控制周期发送未来N个目标点
├─ 关节缓存目标点队列
├─ 即使偶发丢包，关节仍有后续目标
└─ 缺点：增加通信流量，需关节支持

方案D：降低控制频率 
├─ 如果通信不稳定，降低到50Hz
├─ 每个目标点时间间隔变大
│ └─ 给关节更多时间缓冲
└─ 缺点：运动平滑度下降

风险5：示教点分布不均匀
─────────────────────────────────────────────────────
问题描述：
├─ 操作者示教时，速度不均匀
│ ├─ 某些区域移动慢 → 示教点密集
│ └─ 某些区域移动快 → 示教点稀疏
│
├─ 如果使用时间参数化：
│ ├─ 稀疏区域 → 插值后速度过快
│ └─ 密集区域 → 插值后速度过慢
│ └─ 整体运动不连贯
│
└─ 示例：
 ├─ 点1到点2：距离100mm，示教时间0.5秒
 │ └─ 暗含速度：200mm/s（过快！）
 ├─ 点2到点3：距离50mm，示教时间2秒
 │ └─ 暗含速度：25mm/s（过慢）
 └─ 速度突变，不符合S型曲线规划

应对方案：
─────────────────────────────────────────────────────
方案A：弧长参数化（核心设计，必须）
├─ 忽略示教时间信息
├─ 使用关节空间弧长作为参数：
│ └─ s[i] = Σ √(Σ(θ_j[i] - θ_j[i-1])²)
│ └─ j = 1...6（6个关节）
├─ 三次样条插值：θ_j(s)
├─ S型速度规划：对弧长参数s进行规划
│ └─ 生成：s(t)，然后查询：θ_j(s(t))
└─ 效果：运动速度与示教速度完全解耦

方案B：示教速度辅助（示教阶段）
├─ 示教系统实时显示当前速度
├─ 目标速度：50mm/s（配置）
├─ 偏离提示：
│ ├─ 速度 < 30mm/s → "过慢，请加快"
│ ├─ 速度 > 80mm/s → "过快，请减慢"
│ └─ 速度合适 → 绿色显示
└─ 效果：从源头保证示教点均匀

方案C：后处理插值（数据处理阶段）
├─ 分析示教点间距
├─ 在稀疏区域自动插入中间点
│ └─ 方法：三次样条插值后，在间距>80mm处插入
├─ 保证点间距均匀（如50mm）
└─ 可作为增强功能

推荐组合：
├─ 主方案：方案A（弧长参数化）
├─ 辅助：方案B（示教速度提示）
└─ 可选：方案C（自动插值）

风险6：示教操作不规范
─────────────────────────────────────────────────────
问题A：示教时末端悬空
├─ 现象：操作者认为"差不多到位"，实际离表面3–5mm
├─ 后果：
│ ├─ 回放时末端悬空，无法接触清洁面
│ └─ 无参考力信号，力引导无法启动
└─ 根因：缺乏视觉反馈或触觉确认

问题B：示教时用力过大
├─ 现象：操作者用力推末端，接触力达到15–20N
├─ 后果：
│ ├─ 回放时力引导目标力过高
│ └─ 可能损坏表面
└─ 根因：缺乏力反馈提示

问题C：示教时遗漏关键点
├─ 现象：只记录了5个点，某些区域没有覆盖
├─ 后果：回放时清洁不完整，有遗漏区域
└─ 根因：缺乏覆盖范围可视化

应对方案：
─────────────────────────────────────────────────────
方案A：示教软件强制约束（必须）
├─ 接触确认：
│ ├─ 记录示教点前，检测力传感器读数
│ ├─ F < 1N → 提示"请让末端轻触表面"
│ └─ F > 15N → 提示"压力过大，请放松"
│
├─ 点间距检查：
│ ├─ 新点距上一个点 > 100mm → 提示"间距过大，请增加中间点"
│ └─ 新点距上一个点 < 20mm → 提示"间距过小，是否删除上一个点"
│
└─ 强制要求：
 ├─ 首点必须是"接近点"（F=0）
 ├─ 第二点必须是"接触点"（F>2N）
 └─ 末点必须是"撤离点"（F=0）

方案B：示教可视化辅助 
├─ 3D视图显示：
│ ├─ 已记录的示教点（绿球）
│ ├─ 预测的清洁路径（蓝线）
│ └─ 轮廓参考（灰色网格）
│
├─ 实时反馈：
│ ├─ 当前末端位置（红点）
│ ├─ 当前接触力（数字+颜色条）
│ └─ 预计清洁覆盖率（百分比）
│
└─ 帮助操作者做出正确判断

方案C：示教培训流程 
├─ 标准操作规程（SOP）
├─ 示范视频
├─ 新手上机培训（30分钟）
└─ 定期复训

# ============================================
# 模块1：三次样条插值
# ============================================
import numpy as np

class CubicSpline:
 """三次样条插值（自然边界条件）"""

 def __init__(self, t_params, y_values):
 """
 参数:
 t_params: 参数数组（弧长或时间），长度n
 y_values: 关节角度值数组，长度n
 """
 self.t = np.array(t_params)
 self.y = np.array(y_values)
 self.n = len(t_params)

 # 计算样条系数
 self.compute_coefficients()

 def compute_coefficients(self):
 """构建并求解三对角方程组"""
 n = self.n

 # 计算参数间隔
 self.h = np.diff(self.t) # h[i] = t[i+1] - t[i]

 # 构建三对角矩阵 A × M = B
 A = np.zeros((n, n))
 B = np.zeros(n)

 # 自然边界条件：M[0] = M[n-1] = 0
 A[0, 0] = 1
 A[n-1, n-1] = 1
 B[0] = 0
 B[n-1] = 0

 # 内部点方程
 for i in range(1, n-1):
 A[i, i-1] = self.h[i-1]
 A[i, i] = 2 * (self.h[i-1] + self.h[i])
 A[i, i+1] = self.h[i]

 B[i] = 6 * (
 (self.y[i+1] - self.y[i]) / self.h[i] -
 (self.y[i] - self.y[i-1]) / self.h[i-1]
 )

 # 求解二阶导数 M
 self.M = np.linalg.solve(A, B)

 def evaluate(self, t_query):
 """
 计算插值点的值
 参数:
 t_query: 查询参数值（标量或数组）
 返回:
 y_interp: 插值结果
 """
 t_query = np.atleast_1d(t_query)
 y_interp = np.zeros_like(t_query)

 for idx, t in enumerate(t_query):
 # 找到所在区间 [t_i, t_{i+1}]
 i = np.searchsorted(self.t, t) - 1
 i = max(0, min(i, self.n - 2)) # 边界保护

 # 计算局部坐标
 dt = t - self.t[i]

 # 计算系数
 a = self.y[i]
 b = (self.y[i+1] - self.y[i]) / self.h[i] - \
 self.h[i] * (2 * self.M[i] + self.M[i+1]) / 6
 c = self.M[i] / 2
 d = (self.M[i+1] - self.M[i]) / (6 * self.h[i])

 # 计算值
 y_interp[idx] = a + b * dt + c * dt**2 + d * dt**3

 return y_interp[0] if len(y_interp) == 1 else y_interp

# ============================================
# 模块2：S型速度规划
# ============================================
class SCurvePlanner:
 """S型速度曲线规划器"""

 def __init__(self, s_total, v_max, a_max, j_max):
 """
 参数:
 s_total: 总位移
 v_max: 最大速度
 a_max: 最大加速度
 j_max: 最大加加速度
 """
 self.s_total = s_total
 self.v_max = v_max
 self.a_max = a_max
 self.j_max = j_max

 self.compute_profile()

 def compute_profile(self):
 """计算S型曲线各段时间"""
 # 加加速时间（达到A_max所需时间）
 self.t_jerk = self.a_max / self.j_max

 # 加加速段的位移
 s_jerk = 0.5 * self.j_max * self.t_jerk**3

 # 判断是否能达到最大速度
 s_accel_min = 2 * (self.v_max * self.t_jerk + 
 self.v_max**2 / (2 * self.a_max))

 if self.s_total >= s_accel_min:
 # 可以达到最大速度
 self.can_reach_vmax = True

 # 加速段时间
 self.t_accel = self.t_jerk + \
 (self.v_max - 0.5 * self.j_max * self.t_jerk**2) / self.a_max

 # 匀速段时间
 s_accel = self.v_max * self.t_accel
 self.t_const = (self.s_total - 2 * s_accel) / self.v_max

 # 总时间
 self.t_total = 2 * self.t_accel + self.t_const

 else:
 # 无法达到最大速度
 self.can_reach_vmax = False
 self.t_const = 0

 # 求解实际最大速度（数值解）
 # 简化：使用对称三角形速度曲线
 self.v_peak = np.sqrt(self.a_max * self.s_total)
 self.t_accel = self.v_peak / self.a_max
 self.t_total = 2 * self.t_accel

 def get_position(self, t):
 """
 获取时刻t的位置
 参数:
 t: 时间（标量）
 返回:
 s: 位置
 """
 if self.can_reach_vmax:
 return self._position_full(t)
 else:
 return self._position_triangular(t)

 def _position_full(self, t):
 """完整S型曲线（有匀速段）"""
 t1 = self.t_jerk # 阶段1结束
 t2 = self.t_accel # 阶段1+2+3结束
 t3 = t2 + self.t_jerk # 阶段3结束（实际与t2重合）
 t4 = t2 + self.t_const # 匀速段结束
 t5 = t4 + self.t_jerk # 阶段5结束
 t6 = t4 + self.t_accel # 减速段结束
 t7 = self.t_total # 结束

 # 归一化时间到[0, t_total]
 t = max(0, min(t, self.t_total))

 if t <= t1:
 # 阶段1：加加速
 return self.j_max * t**3 / 6
 elif t <= t2:
 # 阶段2+3：匀加速 + 减加速
 v1 = 0.5 * self.j_max * t1**2
 s1 = self.j_max * t1**3 / 6
 dt = t - t1
 return s1 + v1 * dt + 0.5 * self.a_max * dt**2 - \
 self.j_max * dt**3 / 6
 elif t <= t4:
 # 阶段4：匀速
 s_accel = self.v_max * self.t_accel - \
 self.a_max * self.t_accel**2 / 2
 return s_accel + self.v_max * (t - t2)
 elif t <= t6:
 # 阶段5+6+7：减速（对称）
 s_half = self.s_total / 2
 return self.s_total - self._position_full(self.t_total - t)
 else:
 return self.s_total
 return s

 def _position_triangular(self, t):
 """三角型速度曲线（无匀速段）"""
 t_half = self.t_total / 2
 t = max(0, min(t, self.t_total))

 if t <= t_half:
 # 加速段
 return 0.5 * self.a_max * t**2
 else:
 # 减速段
 s_half = 0.5 * self.a_max * t_half**2
 dt = t - t_half
 return s_half + self.v_peak * dt - 0.5 * self.a_max * dt**2

# ============================================
# 模块3：轨迹生成器
# ============================================
class TrajectoryGenerator:
 """多点示教轨迹生成器"""

 def __init__(self, teach_points, control_freq=100, 
 v_max=None, a_max=None, j_max=None):
 """
 参数:
 teach_points: 示教点列表，每个点是6个关节角度
 control_freq: 控制频率
 v_max, a_max, j_max: 各关节速度/加速度/jerk限制
 """
 self.teach_points = teach_points
 self.control_freq = control_freq
 self.n_joints = 6

 # 默认参数（旋转关节：度/s，直线关节：mm/s）
 self.v_max = v_max or [45, 45, 80, 45, 45, 45]
 self.a_max = a_max or [90, 90, 150, 90, 90, 90]
 self.j_max = j_max or [180, 180, 300, 180, 180, 180]

 def compute_arc_length(self):
 """计算弧长参数（关节空间距离）"""
 n = len(self.teach_points)
 arc_length = [0.0]

 for i in range(1, n):
 # 计算相邻点的关节空间距离
 dist = sum(
 (self.teach_points[i][j] - self.teach_points[i-1][j])**2
 for j in range(self.n_joints)
 )**0.5
 arc_length.append(arc_length[-1] + dist)

 return arc_length

 def generate(self):
 """生成完整轨迹"""
 # Step 1：弧长参数化
 t_params = self.compute_arc_length()

 # Step 2：各关节三次样条插值
 splines = []
 for j in range(self.n_joints):
 y = [p[j] for p in self.teach_points]
 spline = CubicSpline(t_params, y)
 splines.append(spline)

 # Step 3：计算总位移和总时间
 s_total = t_params[-1]

 # 计算各关节最短时间
 joint_times = []
 for j in range(self.n_joints):
 delta = abs(self.teach_points[-1][j] - self.teach_points[0][j])
 if delta < 0.1: # 几乎不动
 joint_times.append(0.1)
 continue

 planner = SCurvePlanner(delta, self.v_max[j], 
 self.a_max[j], self.j_max[j])
 joint_times.append(planner.t_total)

 # 以最长关节时间为基准
 t_total = max(joint_times)

 # Step 4：生成时间序列
 num_points = int(t_total * self.control_freq)
 trajectory = []

 s_planner = SCurvePlanner(s_total, s_total/t_total*2, 
 s_total/t_total**2*4, 
 s_total/t_total**3*8)

 for i in range(num_points):
 t = i / self.control_freq

 # S型曲线位置（弧长参数）
 s = s_planner.get_position(t)

 # 归一化到[0, s_total]
 s_norm = s / s_total * t_params[-1]

 # 各关节插值
 joints = [splines[j].evaluate(s_norm) for j in range(self.n_joints)]
 trajectory.append(joints)

 return trajectory, t_total

# ============================================
# 模块4：实时控制器
# ============================================
class RealtimeController:
 """固定频率轨迹回放控制器"""

 def __init__(self, trajectory, control_freq=100):
 """
 参数:
 trajectory: 轨迹点列表
 control_freq: 控制频率
 """
 self.trajectory = trajectory
 self.control_freq = control_freq
 self.current_index = 0
 self.running = False

 def start(self):
 """启动回放"""
 self.current_index = 0
 self.running = True
 self._control_loop()

 def stop(self):
 """停止回放"""
 self.running = False

 def _control_loop(self):
 """控制循环（应运行在实时线程）"""
 import time

 dt = 1.0 / self.control_freq

 while self.running and self.current_index < len(self.trajectory):
 t_start = time.time()

 # 获取当前目标
 target = self.trajectory[self.current_index]

 # 发送给关节控制器（通过CAN总线）
 self._send_to_joints(target)

 # 索引递增
 self.current_index += 1

 # 精确等待
 t_elapsed = time.time() - t_start
 sleep_time = max(0, dt - t_elapsed)
 time.sleep(sleep_time)

 # 循环结束
 self.running = False
 print("轨迹回放完成")

 def _send_to_joints(self, joints):
 """发送给关节控制器（模拟）"""
 # 实际实现：通过CAN总线发送
 # 这里仅打印
 print(f"Index {self.current_index}: {joints}")

# ============================================
# 完整使用示例
# ============================================
if __name__ == "__main__":
 # 示例示教点（6个关节，8个示教点）
 teach_points = [
 [0, 0, 400, 0, -90, 0], # 起点（接近）
 [0, 0, 380, 0, -85, -5], # 接触点
 [15, 10, 350, 5, -75, -10], # 路径点1
 [30, 20, 320, 10, -60, -15],# 路径点2
 [45, 30, 290, 15, -45, -20],# 路径点3
 [60, 40, 260, 20, -30, -25],# 路径点4
 [75, 50, 230, 25, -15, -30],# 路径点5
 [90, 60, 200, 30, 0, -35], # 终点
 ]

 # 生成轨迹
 generator = TrajectoryGenerator(teach_points, control_freq=100)
 trajectory, t_total = generator.generate()

 print(f"轨迹总时长: {t_total:.2f}秒")
 print(f"轨迹点数: {len(trajectory)}")

 # 执行回放
 controller = RealtimeController(trajectory, control_freq=100)
 controller.start()

各关节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%余量应对负载变化
└─ 最终参数需实地验证

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

关节跟踪误差阈值：
├─ 推荐值：±2°
├─ 触发动作：
│ ├─ 误差 > 2° → 降低整体速度50%
│ ├─ 误差 > 3° → 暂停运动，报警
│ └─ 误差恢复 → 自动恢复执行
└─ 理由：保护机械结构，防止超调

通信超时：
├─ 推荐值：20ms
├─ 触发动作：
│ ├─ CAN报文未响应超过20ms → 报警
│ └─ 连续3次超时 → 停止运动
└─ 理由：CAN总线故障保护

预生成轨迹：
├─ 推荐：是（必须）
├─ 理由：
│ ├─ 避免实时计算负担
│ ├─ 保证确定性执行时序
│ └─ 简化实时控制逻辑
└─ 存储：内存中数组，约100KB

力引导叠加：
├─ 推荐值：可选（接触段启用）
├─ Kp_force：0.05 mm/N
├─ 死区：±1.5N
├─ 最大修正：±10mm
└─ 仅在标记为"接触段"的轨迹段启用

本方案的核心优势：
─────────────────────────────────────────────────────
优势1：算法极简
├─ 三次样条插值：约80行代码
├─ S型速度规划：约100行代码
├─ 实时控制循环：约30行代码
└─ 总代码量：约200–300行（含注释）

优势2：无需逆运动学
├─ 直接在关节空间操作
├─ 避免多解选择问题
├─ 避免奇异点问题
└─ 避免雅可比矩阵计算

优势3：运动平滑连续
├─ 三次样条：位置、速度、加速度连续
├─ S型速度：加加速度有限，无冲击
└─ 固定频率执行，确定性时序

优势4：实时计算量极小
├─ 轨迹预生成，运行时纯查表
├─ 无实时逆运动学计算
├─ 适合嵌入式平台（如Jetson NX）
└─ 可扩展到更高控制频率（如500Hz）

优势5：易于调试和维护
├─ 各模块独立，边界清晰
├─ 可视化轨迹（离线分析）
├─ 参数分离（示教/插值/规划/控制）
└─ 无复杂耦合，问题定位容易

实施检查清单（按阶段）：
─────────────────────────────────────────────────────
□ 阶段1：示教模块
 ├─ □ 示教点采集界面
 ├─ □ 关节角度实时显示
 ├─ □ 接触力提示（>3N时提示）
 ├─ □ 示教点保存/加载
 └─ □ 示教点可视化（3D显示）

□ 阶段2：插值模块
 ├─ □ 三次样条插值实现
 ├─ □ 弧长参数化
 ├─ □ 各关节独立插值
 └─ □ 插值点密度配置

□ 阶段3：速度规划模块
 ├─ □ S型速度曲线计算
 ├─ □ 各关节时间统一
 ├─ □ 参数配置界面
 └─ □ 轨迹总时间预估

□ 阶段4：轨迹生成模块
 ├─ □ 轨迹预生成
 ├─ □ 轨迹可视化
 ├─ □ 笛卡尔空间验证
 └─ □ 轨迹保存/加载

□ 阶段5：实时控制模块
 ├─ □ 固定频率控制循环
 ├─ □ CAN总线通信
 ├─ □ 关节跟踪误差监控
 └─ □ 异常处理和报警

□ 阶段6：集成测试
 ├─ □ 单关节测试
 ├─ □ 多关节协调测试
 ├─ □ 完整轨迹回放测试
 ├─ □ 实地清洁测试
 └─ □ 连续可靠性测试（20次）