示教回放 + 柔性跟随的机械臂控制方案

示教回放的工作原理：
─────────────────────────────────────────────────────
示教阶段（人工操作）：
├─ 操作者手动引导机械臂末端到达目标位置
├─ 系统记录每个时刻的关节角度序列
│ └─ 记录内容：[θ₁(t), θ₂(t), d₃(t), θ₄(t), θ₅(t), θ₆(t)]
├─ 示教时末端实际到达了正确位置（人工保证）
└─ 记录的是"关节角度"，不是"末端位置"

回放阶段（自动执行）：
├─ 系统按照记录的关节角度序列驱动各关节
├─ 期望：末端重现示教时的轨迹
└─ 实际：末端位置 = 正运动学(关节角度 + 运动学模型)

天然缺陷：
├─ 回放时的末端位置 = FK(θ_recorded, model_actual)
├─ 示教时的末端位置 = FK(θ_recorded, model_teach)
│
├─ 如果 model_actual ≠ model_teach（运动学有误差）
│ └─ 回放末端位置 ≠ 示教末端位置
│
└─ 如果基座位置/方向发生偏移
 └─ 所有轨迹点整体偏移，偏差更大

双重误差叠加分析：
─────────────────────────────────────────────────────
误差源1：运动学模型误差
├─ 来源：连杆长度加工误差、关节零位偏差、减速器背隙
├─ 典型量级：
│ ├─ 连杆长度误差：±0.5–1mm/连杆
│ ├─ 关节零位偏差：±0.1–0.5°/关节
│ └─ 谐波减速器背隙：< 1 arcmin（极小，可忽略）
├─ 末端位置误差（6关节累积）：
│ └─ 约 ±3–8mm（典型值，未标定状态）
└─ 误差特征：固定偏差（每次回放误差相同）

误差源2：基座位置/方向偏移
├─ 来源：
│ ├─ 机器人底盘停靠位置不精确（每次进入马桶间位置不同）
│ ├─ 地面不平导致底盘倾斜
│ └─ 底盘轮子打滑导致位置偏差
├─ 典型量级：
│ ├─ 平移偏移：±10–20mm（底盘定位精度）
│ ├─ 旋转偏移：±1–3°（底盘朝向误差）
│ └─ 高度偏移：±2–5mm（地面不平）
└─ 误差特征：每次进入马桶间都不同（随机偏差）

双重误差叠加后的末端偏差：
─────────────────────────────────────────────────────
最坏情况（误差方向相同）：
├─ 运动学误差：8mm
├─ 基座平移偏移：20mm
├─ 基座旋转偏移（1°，臂长500mm）：500 × sin(1°) ≈ 9mm
└─ 总偏差：8 + 20 + 9 = 37mm

典型情况（误差随机方向）：
├─ 合成偏差：√(8² + 20² + 9²) ≈ 23mm
└─ 结论：末端偏差约 20–37mm

对比被动柔顺行程（±10mm）：
└─ 偏差远超被动柔顺能力 → 必须有主动补偿机制 

误差源详细分析（针对Scara机械臂构型）：
─────────────────────────────────────────────────────
J1底座旋转（谐波减速器）：
├─ 零位偏差：±0.1°（谐波减速器精度高）
├─ 末端影响：0.1° × 500mm臂长 = 0.87mm
└─ 影响方向：切向（绕Z轴）

J2肩旋转（谐波减速器）：
├─ 零位偏差：±0.1°
├─ 末端影响：0.1° × 400mm（J2到末端距离）= 0.70mm
└─ 影响方向：切向

J3 Z轴升降（滚珠丝杠）：
├─ 零位偏差：±0.5mm（丝杠行程起点误差）
├─ 末端影响：直接 ±0.5mm
└─ 影响方向：Z轴（高度）

J4手腕旋转（行星减速器）：
├─ 零位偏差：±0.5°（行星减速器精度较低）
├─ 末端影响：0.5° × 210mm（J4到末端距离）= 1.83mm
└─ 影响方向：切向

J5俯仰（行星减速器）：
├─ 零位偏差：±0.5°
├─ 末端影响：0.5° × 60mm（J5到末端距离）= 0.52mm
└─ 影响方向：法向（垂直于清洁面）

J6俯仰（行星减速器）：
├─ 零位偏差：±0.5°
├─ 末端影响：0.5° × 30mm（J6到末端距离）= 0.26mm
└─ 影响方向：法向

连杆长度加工误差：
├─ L1（280mm）：±0.5mm → 末端影响 ±0.5mm
├─ L2（250mm）：±0.5mm → 末端影响 ±0.5mm
├─ L5（150mm）：±0.3mm → 末端影响 ±0.3mm
└─ L6（60mm）：±0.2mm → 末端影响 ±0.2mm

运动学误差合计（RSS合成）：
├─ 切向：√(0.87² + 0.70² + 1.83² + 0.5² + 0.5²) ≈ 2.3mm
├─ 法向：√(0.52² + 0.26² + 0.3² + 0.2²) ≈ 0.7mm
├─ Z轴：0.5mm
└─ 总体：约 2–4mm（未标定状态，比之前估计乐观）

注：行星减速器零位偏差是主要误差源（J4影响最大）

误差对各清洁任务的影响分析：
─────────────────────────────────────────────────────
内壁清洁（最敏感）：
├─ 法向误差（径向）：2–4mm
├─ 马桶内壁到末端的设计距离：0mm（贴壁）
├─ 影响：
│ ├─ 误差向内（+2–4mm）→ 末端"顶进"内壁 → 接触力过大
│ └─ 误差向外（-2–4mm）→ 末端"悬空" → 无接触
└─ 结论：法向误差直接影响接触质量，必须补偿

外壁清洁（中等敏感）：
├─ 法向误差：2–4mm
├─ 影响：同内壁，但外壁曲率较小，容错性稍好
└─ 结论：需要补偿，但不如内壁紧迫

座垫清洁（较不敏感）：
├─ Z轴误差：0.5mm（运动学）+ 基座高度偏移
├─ 影响：接触力变化，但被动柔顺可吸收
└─ 结论：被动柔顺基本可以处理

基座偏移的影响（所有任务）：
├─ 平移偏移10–20mm → 所有轨迹点整体偏移
├─ 旋转偏移1–3° → 轨迹点绕基座旋转
└─ 结论：基座偏移是最大的误差源，必须优先处理

四层防御体系（从粗到细）：
─────────────────────────────────────────────────────

Layer 4：示教阶段优化（预防）
├─ 目标：在示教时记录更多信息，为回放提供参考
├─ 方法：同步记录关节角度 + 力传感器信号 + 接触状态
├─ 效果：回放时有"参考力信号"，可以做力信号对齐
└─ 解决：运动学误差的部分补偿

Layer 3：回放前基座偏移补偿（粗补偿）
├─ 目标：在回放开始前，估计并补偿基座偏移
├─ 方法：快速接触标定（2–3个接触点，约10秒）
├─ 效果：将基座偏移从 ±20mm 降低到 ±3mm
└─ 解决：基座平移和旋转偏移

Layer 2：回放中力引导自适应（细补偿）
├─ 目标：在回放过程中，实时用力信号修正末端位置
├─ 方法：比较实际力与示教力，调整法向位置
├─ 效果：将剩余误差（运动学误差+残余基座偏移）从 ±5mm 降低到 ±1mm
└─ 解决：运动学误差 + 残余基座偏移

Layer 1：被动柔顺兜底（最终保障）
├─ 目标：吸收所有层次未能补偿的残余误差
├─ 方法：弹簧+球铰+海绵（机械结构）
├─ 效果：吸收 ±10mm 法向误差，±10° 角度误差
└─ 解决：所有层次的残余误差

─────────────────────────────────────────────────────
各层误差处理能力：

误差源 Layer3补偿后 Layer2补偿后 Layer1兜底后
基座平移偏移 20mm → 3mm 3mm → 1mm 1mm → 0.5mm
基座旋转偏移 9mm → 2mm 2mm → 0.5mm 0.5mm → 0.2mm
运动学误差 不处理 4mm → 1mm 1mm → 0.5mm
─────────────────────────────────────────────────────
最终末端偏差：约 ±0.5mm（满足清洁需求）

系统架构（数据流）：
─────────────────────────────────────────────────────

示教阶段：
[人工引导末端] → [记录关节角度θ(t)] + [记录力信号F(t)] + [记录接触状态C(t)]
 ↓
 [示教数据库]
 ├─ 关节角度序列
 ├─ 力信号序列（参考力）
 └─ 接触状态序列

回放阶段：
[进入马桶间] → [Layer3：快速接触标定] → [计算基座偏移 ΔT]
 ↓
[读取示教数据] → [轨迹变换（应用ΔT）] → [变换后的关节角度序列]
 ↓
 [Layer2：力引导自适应控制]
 ├─ 执行关节角度指令
 ├─ 读取实际力信号 F_actual
 ├─ 对比参考力 F_reference
 ├─ 计算法向修正量 Δx_normal
 └─ 修正末端位置
 ↓
 [Layer1：被动柔顺兜底]
 └─ 弹簧+球铰+海绵吸收残余误差
 ↓
 [末端接触马桶表面]
 └─ 接触力 5–10N，稳定清洁

示教阶段需要记录的完整数据：
─────────────────────────────────────────────────────
必须记录（最小集）：
├─ 关节角度序列：[θ₁, θ₂, d₃, θ₄, θ₅, θ₆](t)
│ └─ 采样频率：100Hz（每10ms一帧）
│
├─ 力传感器信号：F_contact(t)
│ └─ 采样频率：100Hz（与关节角度同步）
│ └─ 重要：记录的是重力补偿后的接触力
│
└─ 接触状态：C(t) ∈ {未接触, 接触中, 过载}
 └─ 用于回放时判断何时开启力引导

推荐记录（增强集）：
├─ 末端笛卡尔位置：[x, y, z, roll, pitch, yaw](t)
│ └─ 通过正运动学计算得到
│ └─ 用于回放时的轨迹变换（比关节角度更直观）
│
├─ 参考马桶位置：马桶轴心坐标 (x_c, y_c)，座圈高度 H_seat
│ └─ 示教时手动测量或接触标定
│ └─ 用于回放时计算基座偏移
│
└─ 清洁任务标签：每段轨迹对应的清洁任务（内壁/外壁/座垫/底座）
 └─ 用于回放时选择对应的力控参数

示教操作规范（直接影响回放质量）：
─────────────────────────────────────────────────────
规范1：示教时必须保证末端真实接触马桶表面
├─ 不能"悬空示教"（末端离表面5mm）
├─ 必须让擦拭头真实贴合马桶表面
├─ 力传感器读数应在 5–10N 范围内
└─ 原因：回放时用力信号对齐，示教力是参考基准

规范2：示教速度要与回放速度一致
├─ 示教时的运动速度 = 回放时的运动速度（约50mm/s）
├─ 速度不一致会导致力信号时序错位
└─ 建议：示教时用固定速度模式（不要忽快忽慢）

规范3：示教前必须执行接触标定
├─ 示教前，记录马桶轴心位置 (x_c, y_c) 和座圈高度 H_seat
├─ 这些数据是回放时计算基座偏移的参考
└─ 标定方法：3点接触法（见模块二）

规范4：每种清洁任务单独示教
├─ 内壁清洁：单独示教一段完整轨迹
├─ 外壁清洁（有扶手侧）：单独示教
├─ 外壁清洁（无扶手侧）：单独示教
├─ 座垫清洁：单独示教
└─ 底座清洁：单独示教
 原因：不同任务的力控参数不同，分开管理更清晰

规范5：示教数据要多次验证
├─ 示教完成后，立即回放一次验证
├─ 确认回放轨迹与示教轨迹一致（在同一台马桶上）
└─ 验证通过后才保存为正式示教数据

示教数据存储格式（JSON）：

{
 "teach_id": "toilet_inner_wall_v1",
 "task_type": "inner_wall",
 "teach_time": "2026-04-07T10:00:00",
 "reference_toilet": {
 "center_x": 0.0, // 示教时马桶轴心X（mm，基座坐标系）
 "center_y": 500.0, // 示教时马桶轴心Y（mm）
 "seat_height": 400.0 // 示教时座圈高度（mm）
 },
 "frames": [
 {
 "t": 0.000, // 时间戳（秒）
 "joints": [θ₁, θ₂, d₃, θ₄, θ₅, θ₆], // 关节角度（度/mm）
 "end_pos": [x, y, z, roll, pitch, yaw], // 末端笛卡尔位置
 "force": 7.2, // 接触力（N，重力补偿后）
 "contact": true // 接触状态
 },
 {
 "t": 0.010,
 ...
 }
 ],
 "total_frames": 3000, // 总帧数（30秒 × 100Hz）
 "force_profile": {
 "mean": 7.5, // 平均接触力
 "std": 1.2, // 接触力标准差
 "min": 5.0,
 "max": 10.0
 }
}

存储大小估算：
├─ 每帧：约 100 字节
├─ 30秒轨迹：3000帧 × 100字节 = 300KB
└─ 5种清洁任务：约 1.5MB（极小，无存储压力）

基座偏移的数学描述：
─────────────────────────────────────────────────────
示教时的基座坐标系：{B_teach}
回放时的基座坐标系：{B_replay}

两者之间的变换关系（刚体变换）：
├─ 平移：[Δx, Δy, Δz]（基座位置偏移）
├─ 旋转：Δψ（基座绕Z轴的朝向偏移）
└─ 变换矩阵 T：
 │ cos(Δψ) -sin(Δψ) 0 Δx │
T = │ sin(Δψ) cos(Δψ) 0 Δy │
 │ 0 0 1 Δz │
 │ 0 0 0 1 │

示教轨迹点（基座坐标系）：P_teach = [x, y, z, 1]ᵀ
回放时对应的目标点：P_replay = T × P_teach

关键：只要估计出 T（即 Δx, Δy, Δz, Δψ），
 就可以将所有示教轨迹点变换到当前基座坐标系

快速接触标定方案（2点法 vs 3点法）：
─────────────────────────────────────────────────────
方案A：3点接触法（推荐，可估计平移+旋转）
─────────────────────────────────────────────────────
原理：
├─ 在马桶外壁3个已知角度位置（0°/120°/240°）接触
├─ 记录3个接触点的末端坐标（基座坐标系）
├─ 拟合圆心，得到当前马桶轴心位置
├─ 对比示教时的马桶轴心位置
└─ 计算基座偏移 [Δx, Δy, Δψ]

步骤：
Step 1：机械臂移动到0°方向，缓慢向马桶外壁靠近
├─ 速度：5mm/s
├─ 检测到接触（F > 1N）时停止
└─ 记录接触点 P1 = (x1, y1)

Step 2：机械臂移动到120°方向，重复接触
└─ 记录接触点 P2 = (x2, y2)

Step 3：机械臂移动到240°方向，重复接触
└─ 记录接触点 P3 = (x3, y3)

Step 4：三点拟合圆心（解析公式）
└─ 联立方程组求解圆心 (cx_replay, cy_replay)

Step 5：计算基座偏移
├─ Δx = cx_replay - cx_teach
├─ Δy = cy_replay - cy_teach
└─ Δψ = atan2(Δy, Δx)（近似，小角度时有效）

Step 6：高度标定
├─ 机械臂从上方缓慢下降，接触座圈
├─ 记录接触高度 H_replay
└─ Δz = H_replay - H_teach

标定时间：约 10–15秒
标定精度：
├─ 平移：±2mm（接触式标定精度）
└─ 旋转：±0.5°（三点法精度）

─────────────────────────────────────────────────────
方案B：2点法（更快，但只能估计平移）
─────────────────────────────────────────────────────
原理：
├─ 在马桶外壁2个位置（0°/180°）接触
├─ 两点中点即为马桶轴心（近似）
└─ 只能估计平移偏移，无法估计旋转偏移

适用场景：
├─ 底盘定位精度较高（旋转偏移 < 0.5°）
└─ 对清洁精度要求不高的场景

轨迹变换实现（伪代码）：

def transform_trajectory(teach_frames, delta_x, delta_y, delta_z, delta_psi):
 transformed_frames = []

 cos_psi = cos(delta_psi)
 sin_psi = sin(delta_psi)

 for frame in teach_frames:
 x, y, z = frame.end_pos[0:3]

 // 旋转变换（绕Z轴）
 x_new = cos_psi * x - sin_psi * y + delta_x
 y_new = sin_psi * x + cos_psi * y + delta_y
 z_new = z + delta_z

 // 更新末端位置
 frame_new = frame.copy()
 frame_new.end_pos[0:3] = [x_new, y_new, z_new]

 // 重新计算关节角度（逆运动学）
 frame_new.joints = inverse_kinematics(x_new, y_new, z_new,
 frame.end_pos[3:6])

 transformed_frames.append(frame_new)

 return transformed_frames

// 注意：逆运动学可能有多解，需要选择与示教关节角度最接近的解
// 选解策略：选择与 frame.joints 欧氏距离最小的解

力引导自适应的核心思想：
─────────────────────────────────────────────────────
示教时记录了"参考力信号" F_reference(t)
回放时实时读取"实际力信号" F_actual(t)

两者的差值反映了末端位置的法向偏差：
├─ F_actual > F_reference → 末端比示教时更深入表面
│ └─ 说明末端偏向表面内侧（法向误差为正）
│ └─ 需要末端向外退让（减小接触力）
│
├─ F_actual < F_reference → 末端比示教时更远离表面
│ └─ 说明末端偏向表面外侧（法向误差为负）
│ └─ 需要末端向内靠近（增大接触力）
│
└─ F_actual ≈ F_reference → 末端位置与示教时一致
 └─ 不需要修正

这个思路的优势：
├─ 不需要知道运动学误差的具体数值
├─ 不需要知道基座偏移的精确量（Layer3已处理大部分）
├─ 只需要力信号的差值，算法极简单
└─ 自动适应任何形式的法向误差（运动学+残余基座偏移）

法向偏移量转换为关节修正量：
─────────────────────────────────────────────────────
问题：力引导计算出的是"法向偏移量 x_normal_offset（mm）"
 但关节控制器需要的是"关节角度修正量 Δθ"

方法：基于当前任务类型的简化转换
─────────────────────────────────────────────────────
内壁/外壁清洁（法向 = 水平径向方向）：
├─ 法向偏移 Δr（径向）
├─ 当前末端角度 θ_arm = θ₁ + θ₂
├─ 笛卡尔修正：
│ ├─ Δx = Δr × cos(θ_arm)（内壁：负号；外壁：正号）
│ └─ Δy = Δr × sin(θ_arm)
├─ 转换为关节修正（SCARA逆运动学微分）：
│ ├─ 当前末端半径 r = √(x² + y²)
│ ├─ Δr → 调整J1+J2使末端半径变化Δr
│ └─ 具体：保持θ_arm不变，调整r（J1+J2联动）

座垫/底座清洁（法向 = 垂直Z轴方向）：
├─ 法向偏移 Δz（Z轴）
├─ 直接转换为J3升降修正：Δd₃ = Δz

注意：这种简化转换忽略了J5/J6俯仰对法向方向的影响
├─ 当J5/J6俯仰角较大时（>30°），法向方向偏离水平
├─ 简化误差：约 sin(30°) × Δr = 0.5 × Δr
└─ 对于清洁任务（Δr < 5mm），误差 < 2.5mm（可接受）
 如需更精确：使用雅可比矩阵转换（进阶方案）

力信号时间对齐（关键技术点）：
─────────────────────────────────────────────────────
问题：回放时的运动速度可能与示教时不完全一致
 导致力信号在时间轴上错位

例子：
├─ 示教时：在 t=5.0s 时末端到达内壁某点，力=7.5N
├─ 回放时：由于基座偏移补偿，运动路径稍有不同
│ └─ 在 t=5.1s 时才到达同一点
└─ 如果直接用 t=5.0s 的参考力（7.5N）对比 t=5.1s 的实际力
 → 时间错位导致力引导失准

解决方案：基于位置的力信号对齐（而非时间对齐）
─────────────────────────────────────────────────────
方法：
├─ 不用时间索引查找参考力
├─ 用当前末端位置（x,y,z）在示教轨迹中查找最近点
├─ 用最近点的参考力作为当前时刻的参考力
└─ 这样即使速度不同，力信号也能正确对齐

实现（伪代码）：

def find_reference_force(current_pos, teach_frames):
 // 在示教轨迹中找到与当前位置最近的帧
 min_dist = infinity
 best_frame = None
 for frame in teach_frames:
 dist = distance(current_pos, frame.end_pos[0:3])
 if dist < min_dist:
 min_dist = dist
 best_frame = frame
 return best_frame.force

// 注意：为了效率，可以只在当前帧附近搜索（±50帧）
// 而不是搜索全部3000帧

被动柔顺在示教回放场景中的特殊作用：
─────────────────────────────────────────────────────
场景1：Layer3标定误差（残余基座偏移 ±2–3mm）
├─ 标定精度 ±2mm → 轨迹变换后仍有 ±2mm 法向误差
├─ 被动柔顺行程 ±5mm（弹簧）
└─ 完全可以吸收 

场景2：运动学误差（约 ±2–4mm）
├─ Layer2力引导补偿后残余约 ±1mm
├─ 被动柔顺行程 ±5mm
└─ 完全可以吸收 

场景3：马桶表面局部突起（排水孔边缘等）
├─ 突起高度约 1–3mm
├─ 被动柔顺（海绵+弹簧）可吸收 ±5mm
└─ 完全可以吸收 

场景4：Layer3/Layer2均失效（极端情况）
├─ 总误差可能达到 ±20mm
├─ 被动柔顺行程 ±10mm（弹簧5mm + 海绵5mm）
├─ 超出被动柔顺行程的部分（±10mm）
└─ 触发过载保护（F > 20N → 立即退让）

结论：被动柔顺是最后一道防线，
 在Layer3+Layer2正常工作时，
 被动柔顺只需处理 ±1–2mm 的残余误差，
 工作在轻松状态，寿命更长。

被动柔顺参数优化（针对示教回放场景）：
─────────────────────────────────────────────────────
原参数（通用清洁场景）：
├─ 弹簧刚度：k = 5N/mm
├─ 预压量：1–2mm（初始接触力 5–10N）
└─ 最大压缩量：5mm

示教回放场景的特殊需求：
├─ Layer3+Layer2已处理大部分误差
├─ 被动柔顺只需处理 ±1–2mm 的残余误差
├─ 可以适当增大弹簧刚度（提高末端刚度，改善清洁效果）

优化参数：
├─ 弹簧刚度：k = 8N/mm（从5提高到8）
│ └─ 理由：残余误差小，不需要大行程柔顺
│ └─ 效果：接触力更稳定（刚度高，力对位置不敏感）
├─ 预压量：1.5mm（初始接触力 12N）
│ └─ 理由：示教时接触力约7.5N，预压稍大保证初始接触
└─ 最大压缩量：3mm（从5mm减小到3mm）
 └─ 理由：残余误差小，不需要大行程

注意：如果Layer3/Layer2效果不好，恢复原参数（k=5N/mm）

完整回放流程（时序图）：
─────────────────────────────────────────────────────
T=0s 机器人进入马桶间，底盘停靠
 │
T=0–15s [Layer3] 快速接触标定
 ├─ 3点接触马桶外壁（每点约3秒）
 ├─ 1次高度标定（约2秒）
 └─ 计算基座偏移 [Δx, Δy, Δz, Δψ]
 │
T=15–16s [轨迹变换] 将示教轨迹变换到当前坐标系
 ├─ 读取示教数据（关节角度+力信号）
 ├─ 应用变换矩阵T（约0.3秒计算）
 └─ 生成变换后的轨迹
 │
T=16s 开始回放
 │
T=16–46s [Layer2] 力引导自适应回放（内壁清洁，约30秒）
 ├─ 执行变换后的关节角度序列
 ├─ 实时读取力传感器
 ├─ 基于位置对齐查找参考力
 ├─ 计算力误差，修正法向位置
 └─ 被动柔顺（Layer1）持续兜底
 │
T=46–50s 更换清洁模组（磁吸快换，约4秒）
 │
T=50–80s [Layer2] 力引导自适应回放（外壁清洁，约30秒）
 │
T=80–84s 更换清洁模组
 │
T=84–104s [Layer2] 力引导自适应回放（座垫清洁，约20秒）
 │
T=104s 清洁完成，机器人退出马桶间

总时间：约 104秒（含标定15秒 + 清洁89秒）

各模块误差处理分工（量化）：
─────────────────────────────────────────────────────
初始误差（进入马桶间后）：
├─ 基座平移偏移：±20mm
├─ 基座旋转偏移：±3°（末端影响约±26mm）
└─ 运动学误差：±4mm

经过Layer3（接触标定）后：
├─ 基座平移偏移：±20mm → ±2mm（减少90%）
├─ 基座旋转偏移：±3° → ±0.5°（末端影响±4mm → ±0.7mm）
└─ 运动学误差：±4mm（不处理）
总残余误差：约 ±5mm

经过Layer2（力引导自适应）后：
├─ 法向误差：±5mm → ±1mm（减少80%）
└─ 切向误差：不处理（切向误差不影响接触质量）
总残余法向误差：约 ±1mm

经过Layer1（被动柔顺）后：
├─ 法向误差：±1mm → ±0.3mm（弹簧吸收）
└─ 角度误差：±5° → ±1°（球铰吸收）
最终末端接触误差：约 ±0.3mm

接触力稳定性：
├─ 目标接触力：7.5N
├─ 实际接触力范围：7.5 ± 1.5N（±20%）
└─ 清洁效果：优秀（接触力在5–10N范围内）

示教阶段的常见陷阱：
─────────────────────────────────────────────────────
陷阱1：示教时末端悬空（最常见错误）
├─ 现象：操作者认为末端"差不多到位了"，实际离表面3–5mm
├─ 后果：示教力信号为0，回放时力引导无参考
├─ 预防：示教时强制要求力传感器读数 > 3N 才允许记录
└─ 检查：回放验证时，观察力信号是否与示教时一致

陷阱2：示教时用力过大（过度压入）
├─ 现象：操作者用力推末端，接触力达到15–20N
├─ 后果：回放时力引导目标力过高，可能损坏马桶
├─ 预防：示教时限制接触力 < 12N（软件限制）
└─ 检查：示教数据中 force_profile.max < 12N

陷阱3：示教速度不均匀 
├─ 现象：操作者时快时慢，力信号时序不规律
├─ 后果：基于位置的力信号对齐效果变差
├─ 预防：使用速度辅助模式（系统限制最大速度）
└─ 检查：示教数据中相邻帧的位移量应均匀

陷阱4：示教时马桶位置与回放时不同 
├─ 现象：示教在A号马桶，回放在B号马桶（尺寸略有不同）
├─ 后果：轨迹变换后仍有系统性误差
├─ 预防：示教时记录马桶型号，回放时匹配对应示教数据
└─ 建议：对每种马桶型号单独示教

回放阶段的关键注意事项：
─────────────────────────────────────────────────────
注意1：接触标定失败的处理 
├─ 可能原因：马桶位置偏移过大（>30mm），接触点找不到
├─ 检测方法：3次接触尝试均失败（F < 1N）
├─ 处理方案：
│ ├─ 扩大搜索范围（从±20mm扩大到±40mm）
│ └─ 如果仍失败，报警并停止（不强行执行）
└─ 安全原则：标定失败宁可停止，不能盲目执行

注意2：力引导修正量的累积限制 
├─ 问题：力引导的累积偏移量 x_normal_offset 可能无限增大
├─ 场景：马桶表面有大面积凹陷，力持续偏小
│ └─ 力引导持续向内靠近，累积偏移量越来越大
├─ 限制：x_normal_offset 限幅在 ±10mm（被动柔顺行程内）
└─ 超出限幅时：停止力引导，保持当前偏移量

注意3：切换清洁任务时重置偏移量 
├─ 每次切换清洁任务（内壁→外壁→座垫）时
├─ 重置 x_normal_offset = 0
└─ 原因：不同任务的法向方向不同，偏移量不能跨任务累积

注意4：回放速度与示教速度的匹配 
├─ 回放速度应与示教速度一致（±10%以内）
├─ 速度差异过大会导致力信号时序错位
│ └─ 即使用位置对齐，速度差异也会影响动态力
└─ 建议：回放时使用与示教相同的速度参数

注意5：多次回放的一致性验证 
├─ 首次回放后，检查清洁覆盖率和接触力记录
├─ 如果接触力波动过大（>±3N），重新调整Kp
└─ 连续5次回放，接触力稳定性应一致

验收测试方案：
─────────────────────────────────────────────────────
测试1：基座偏移补偿效果验证
├─ 方法：将机器人底盘故意偏移 ±15mm 和 ±2°
├─ 执行接触标定，观察标定结果
├─ 验收标准：标定后轨迹误差 < ±3mm
└─ 重复5次，每次偏移量随机

测试2：力引导自适应效果验证
├─ 方法：在马桶表面贴3mm厚的垫片（模拟位置误差）
├─ 执行回放，观察力传感器读数
├─ 验收标准：接触力在 5–12N 范围内（不超过15N）
└─ 重复5次

测试3：双重误差叠加验证
├─ 方法：同时施加基座偏移（±15mm）+ 垫片（3mm）
├─ 执行完整回放流程（标定+变换+力引导）
├─ 验收标准：接触力在 5–12N 范围内
└─ 重复3次

测试4：连续可靠性验证
├─ 方法：连续执行20次完整清洁回放
├─ 每次随机改变底盘停靠位置（±10mm范围内）
├─ 验收标准：
│ ├─ 20次中无过载事件（F > 20N）
│ ├─ 接触力平均值：7.5 ± 2N
│ └─ 清洁覆盖率：≥ 90%
└─ 这是最终验收测试