复合清洁机器人底座偏差时，底盘的自适应标定方案

问题场景：
─────────────────────────────────────────────────────
示教阶段：
├─ 机器人底座位于位置A，朝向θA
├─ 操作者示教内壁清洁轨迹
├─ 示教路径记录在"示教坐标系"中
└─ 示教完成，机器人离开

回放阶段：
├─ 机器人重新部署，底座位于位置A'，朝向θA'
├─ 实际偏差：
│ ├─ 位置偏差：1–2cm
│ └─ 朝向偏差：1–2°
├─ 位置相对于地面不变（但机器人坐标系变了）
├─ 机器人需要自动标定，自动贴合
└─ 不能依赖人工重新示教或点位标定

核心矛盾：
─────────────────────────────────────────────────────
示教坐标系 ≠ 回放坐标系

示教时记录的点位（机器人关节角度）→ 对应末端在"示教位置A"
回放时机器人底座在"位置A'" → 直接回放会跑偏

必须解决：如何让机器人在新位置自动知道"在哪里"

核心挑战分析：
─────────────────────────────────────────────────────
挑战1：机器人不知道自己"在哪里"
├─ 示教时：机器人坐标系原点 = 位置A
├─ 回放时：机器人坐标系原点 = 位置A'
├─ 机器人只知道自己的关节角度，不知道在哪
└─ 必须通过某种方式让机器人"感知"位置

挑战2：偏差类型复杂
├─ 位置偏差：Δx, Δy（水平面），Δz（高度基本不变）
├─ 朝向偏差：Δθ（绕Z轴旋转）
└─ 三者可能同时存在，组合成刚体变换

挑战3：无法人工干预
├─ 回放时没有操作者进行示教或标定
├─ 机器人必须全自动完成标定和执行
└─ 标定过程不能打扰用户（可能在卫生间无人时运行）

挑战4：贴合精度要求
├─ 内壁清洁需要末端贴合表面
├─ 贴合间隙要求 < 3mm
├─ 偏差1–2cm + 贴合要求3mm → 必须精确补偿
└─ 仅靠力控柔顺不够（间隙太大）

偏差来源分析：
─────────────────────────────────────────────────────
来源1：机器人底盘重新部署
├─ 每次清洁前，机器人从充电座移动到旁
├─ 移动精度：人为推/拉，约±1–2cm，±1–2°
└─ 这是最主要的偏差来源 

来源2：位置微小变动
├─ 通过螺栓固定在地面
├─ 基本固定，偏差 < 1mm
└─ 可忽略 

来源3：机器人关节角度重复性
├─ 一体化关节位置环精度：±0.05°
├─ 多次回放轨迹一致性高
└─ 不是主要问题 

来源4：温度/负载变化
├─ 长时间运行后，关节柔性变形
├─ 影响 < 0.5mm
└─ 可忽略 

结论：主要问题是底盘重新部署导致的坐标系偏差
 解决思路：机器人"自感知"位置

三层解决方案架构：
─────────────────────────────────────────────────────
┌─────────────────────────────────────────────────┐
│ 第3层：力柔顺控制（最后兜底） │
│ ├─ 末端接触力实时监控 │
│ ├─ 接触力过大 → 关节主动退让 │
│ └─ 保证接触力在安全范围（3–8N） │
└─────────────────────────────────────────────────┘
 ▲
 │ 补充
┌─────────────────────────────────────────────────┐
│ 第2层：轨迹自适应（核心补偿） │
│ ├─ 坐标变换补偿底座位置偏差 │
│ ├─ 将示教坐标系轨迹映射到回放坐标系 │
│ ├─ 保证末端"理论上"到达正确位置 │
│ └─ 残余误差 < 3mm │
└─────────────────────────────────────────────────┘
 ▲
 │ 输入
┌─────────────────────────────────────────────────┐
│ 第1层：自标定（感知位置） │
│ ├─ 机器人上电后自动感知位置 │
│ ├─ 通过视觉/传感器检测特征 │
│ ├─ 计算当前底座到的坐标变换 │
│ └─ 输出：变换矩阵 T（Δx, Δy, Δθ） │
└─────────────────────────────────────────────────┘

三层之间的关系：
├─ 第1层确定"在哪里"（感知层）
├─ 第2层补偿"让末端去正确位置"（规划层）
├─ 第3层兜底"万一有误差，用力控吸收"（控制层）
└─ 三层联动，实现无需人工干预的柔性贴合

两种部署模式对比：
─────────────────────────────────────────────────────
模式A：每次清洁前重新标定（推荐）
├─ 流程：机器人到位 → 自标定 → 执行清洁 → 回充电座
├─ 优点：每次都是新鲜标定，精度最高
├─ 适用：日常清洁场景
└─ 标定时间：10–15秒（可接受）
模式B：首次示教 + 长期使用
├─ 流程：首次示教时记录位置 → 之后每次复用
├─ 优点：无需每次标定
├─ 缺点：机器人每次必须回到相同位置（需要定位辅助）
├─ 适用：固定基站场景
└─ 风险：累积误差需要定期重新标定
推荐：模式A（每次重新标定）
├─ 每次清洁前花10秒标定，换来高精度贴合
├─ 避免累积误差
└─ 标定过程全自动，用户无感知

方案总体流程：
─────────────────────────────────────────────────────
示教阶段（一次性）：
├─ Step 1：机器人部署到标准位置（人工粗定位，±5cm内即可）
├─ Step 2：执行自标定（感知位置）
├─ Step 3：操作者示教清洁轨迹
├─ Step 4：保存轨迹，同时保存"在示教坐标系中的描述"
└─ 输出：示教轨迹 + 几何描述（轮廓线/特征点）

回放阶段（每次清洁）：
├─ Step 1：机器人部署（人工粗定位，±5cm内即可）
├─ Step 2：执行自标定（感知位置）
├─ Step 3：轨迹自适应变换（坐标变换补偿偏差）
├─ Step 4：力柔顺控制执行轨迹
└─ 输出：清洁动作完成

坐标系统定义：
─────────────────────────────────────────────────────
坐标系说明：
├─ {B_teach}：示教时的机器人底座坐标系
│ └─ 原点 = 示教时机器人底座中心
│ └─ X轴 = 机器人正前方
│
├─ {B_play}：回放时的机器人底座坐标系
│ └─ 原点 = 回放时机器人底座中心
│ └─ X轴 = 回放时机器人正前方
│
├─ {T}：坐标系（固定在地面的世界坐标系）
│ └─ 原点 = 几何中心（固定不变）
│ └─ 相对于地面不动
│
└─ {E}：末端执行器坐标系
 └─ 原点 = 末端工具中心

各坐标系之间的关系：
─────────────────────────────────────────────────────
示教时：
├─ 示教轨迹上的点 P_teach 在 {B_teach} 中的坐标
├─ 示教轨迹在 {B_teach} 中的表达：P_teach ∈ {B_teach}
├─ 坐标系 {T} 在 {B_teach} 中的位置：
│ └─ T_in_B_teach = (x_tt, y_tt, θ_tt)
└─ 变换关系：P_world = R(θ_tt) × P_teach + T(x_tt, y_tt)

回放时：
├─ 机器人底座在 {T} 中的位置：T_in_B_play = (Δx, Δy, Δθ)
│ └─ 这就是我们需要标定的量！
├─ 回放轨迹需要从 {B_teach} 映射到 {B_play}
└─ 变换关系：P_play = R(-Δθ) × (P_teach - ΔT)

坐标变换公式：
─────────────────────────────────────────────────────
已知：
├─ 示教轨迹点：P_teach（关节角度 → 正运动学 → {B_teach}坐标）
├─ 示教时在 {B_teach} 中的位置：T_teach（x_t, y_t, θ_t）
└─ 回放时标定得到的偏差：Δ = (Δx, Δy, Δθ)

目标：
├─ 计算回放轨迹点：P_play（关节角度）
└─ 使得末端到达相同的世界位置（表面）

变换公式：
─────────────────────────────────────────────────────
Step 1：示教轨迹点在世界坐标系中的位置
P_world = R(θ_t) × P_teach + T(x_t, y_t)

其中：
R(θ) = [cos(θ) -sin(θ)]
 [sin(θ) cos(θ)]

Step 2：考虑回放时的底座偏差
{B_play} 到 {T} 的变换 = (Δx, Δy, Δθ)

{B_teach} 到 {B_play} 的变换 = (-Δx, -Δy, -Δθ)

Step 3：回放轨迹点在 {B_play} 中的坐标
P_play_in_Bplay = R(-Δθ) × (P_world - T(Δx, Δy))

展开：
P_play = R(-Δθ) × R(θ_t) × P_teach + R(-Δθ) × T(x_t, y_t) - T(Δx, Δy)

简化理解：
├─ 先把示教点转换到世界坐标
├─ 再把世界坐标转换到回放坐标系
└─ 得到回放时需要到达的关节角度

实际应用：
├─ 示教时：记录示教轨迹 + 记录在示教坐标系中的位置 T_teach
├─ 回放时：标定得到 Δ = (Δx, Δy, Δθ)
├─ 计算：T_play = R(-Δθ) × T_teach - Δ
└─ 所有示教点加上偏移 T_play 即可

变换的物理意义：
─────────────────────────────────────────────────────
场景类比：
├─ 想象你在房间A的某个位置画了一幅画
├─ 然后把画框移到房间B的另一个位置
├─ 如果你知道移动了多少（Δx, Δy, Δθ）
└─ 就能在新位置重新画出同样的画

在机器人清洁中：
├─ 示教时：机器人在"位置A"记录了清洗内壁的轨迹
├─ 回放时：机器人在"位置A'"（偏差了1–2cm）
├─ 如果知道偏差量 Δ = (Δx, Δy, Δθ)
└─ 就能自动调整轨迹，在新位置执行同样的清洁

关键：Δ 如何获取？
├─ 不能靠人工测量
├─ 必须让机器人"自己感知"
└─ → 自标定方案

自标定原理：
─────────────────────────────────────────────────────
核心思想：
├─ 是固定的（世界坐标系中的固定参照物）
├─ 机器人通过传感器"看"到
├─ 根据在视野中的位置，推算自己在世界中的位置
└─ 从而计算出相对于示教时的偏差 Δ

类比：
├─ 你闭着眼睛在房间里走动，不知道自己在哪里
├─ 突然摸到了熟悉的桌子 → 知道了自己的位置
├─ 机器人"触摸" → 知道在哪里
└─ 再结合自己的关节角度 → 知道自己的位置

如何被"感知"？
─────────────────────────────────────────────────────
方案1：视觉感知（推荐）
├─ 在机器人末端安装摄像头
├─ 机器人移动到附近
├─ 拍摄图像，检测轮廓
├─ 根据在图像中的位置，计算相对于机器人的位置
└─ 结合机器人关节角度 → 得到在 {B_play} 中的位置

方案2：超声/红外测距
├─ 在机器人末端安装超声/红外传感器
├─ 沿边缘扫描，测量距离
├─ 根据多个测量点，拟合轮廓
└─ 得到圆心和半径

方案3：末端接触探测
├─ 机器人末端轻轻接触内壁
├─ 记录接触点的关节角度（对应末端位置）
├─ 多个接触点 → 拟合圆弧
└─ 得到几何中心

推荐：视觉感知（方案1）
├─ 非接触，不损伤或传感器
├─ 速度快（<5秒完成标定）
├─ 精度高（位置误差 < 2mm）
└─ 技术成熟（单目相机 + 深度学习）

视觉自标定详细设计：
─────────────────────────────────────────────────────
标定流程：
Step 1：机器人移动到标定视角位置
├─ 机器人将末端移动到正上方
├─ 位置要求：
│ ├─ 摄像头朝向内壁
│ ├─ 距离内壁约 50–100mm
│ └─ 能看到内壁的完整轮廓
└─ 该位置由预设的"标定姿态"决定（关节角度固定）

Step 2：采集图像
├─ 末端摄像头拍摄内壁图像
├─ 分辨率：640×480 或更高
└─ 同时记录当前关节角度（用于正运动学计算）

Step 3：特征检测
├─ 使用图像处理/深度学习检测边缘
├─ 识别内壁轮廓（椭圆/圆形）
├─ 检测水位线（水平线）
├─ 检测边缘标记（如果贴了定位标记）
└─ 输出：轮廓的图像坐标

Step 4：三维位置计算
├─ 已知：
│ ├─ 摄像头内参（标定已知）
│ ├─ 摄像头外参（通过正运动学从关节角度计算）
│ ├─ 边缘在图像中的位置
│ └─ 半径（预设参数，或从图像估算）
├─ 计算：
│ └─ 圆心在机器人基坐标系 {B_play} 中的位置
└─ 输出：圆心坐标 T_play = (x_t, y_t, θ_t)

Step 5：计算偏差 Δ
├─ 示教时记录了在 {B_teach} 中的位置：T_teach
├─ 回放时计算了在 {B_play} 中的位置：T_play
├─ 偏差 = T_play - T_teach（经过坐标变换）
└─ 输出：Δ = (Δx, Δy, Δθ)

标定参数要求：
├─ 位置偏差容限：±3cm
├─ 朝向偏差容限：±5°
├─ 标定精度：< 2mm（位置），< 0.5°（朝向）
└─ 标定时间：< 10秒

自标定代码流程：
─────────────────────────────────────────────────────
自标定主流程：
├─ Step 1：机器人移动到标定视角
│ └─ 发送预设的"标定关节角度"→ 机器人移动到位
│
├─ Step 2：采集图像并处理
│ ├─ 摄像头拍摄图像
│ ├─ 检测边缘轮廓（椭圆拟合）
│ └─ 输出：圆心像素坐标 (u, v)
│
├─ Step 3：计算三维位置
│ ├─ 读取当前关节角度 q
│ ├─ 正运动学 FK(q) → 得到摄像头在 {B} 中的位置和姿态
│ ├─ 根据针孔相机模型，从像素坐标计算在相机坐标系中的三维坐标
│ └─ 坐标变换 → 在 {B_play} 中的位置 T_play
│
├─ Step 4：从示教数据中获取 T_teach
│ └─ 示教完成后保存了在示教坐标系中的描述
│
├─ Step 5：计算偏差 Δ
│ └─ Δx = T_play.x - T_teach.x
│ └─ Δy = T_play.y - T_teach.y
│ └─ Δθ = T_play.θ - T_teach.θ
│
└─ Step 6：验证标定结果
 ├─ 标定完成后，再次检测边缘
 ├─ 确认位置与预期一致
 └─ 如果偏差过大，重新标定或报警

标定视角预设关节角度：
├─ 预设值：[θ₁, θ₂, d₃, θ₄, θ₅, θ₆]（通过示教确定）
├─ 作用：每次标定时，机器人移动到相同姿态
└─ 保证标定结果可重复

无视觉传感器的替代标定方案：
─────────────────────────────────────────────────────
方案A：超声/红外探测标定
├─ 在末端安装超声或红外测距传感器
├─ 机器人沿预设路径扫描边缘
├─ 记录每个扫描点的距离值
├─ 根据距离变化拟合轮廓
├─ 计算圆心位置
└─ 优点：成本低，无需视觉算法

方案B：接触式探测标定
├─ 末端安装轻触式探针或力传感器
├─ 机器人以低力（<2N）轻触内壁
├─ 记录多个接触点的关节角度
├─ 通过正运动学计算接触点在 {B} 中的位置
├─ 多个接触点拟合圆弧
├─ 得到几何参数
└─ 优点：精度高，物理接触无歧义

方案C：固定标记物标定
├─ 在地面固定一个定位标记（如ArUco码）
├─ 标记位置与位置有固定关系
├─ 机器人末端安装相机读取标记
├─ 从标记推算位置
└─ 优点：定位精度高，算法简单

推荐优先级：
├─ 有相机：方案C（固定标记）> 方案A（视觉）
├─ 无相机：方案A（超声/红外）> 方案B（接触探测）
└─ 无论哪种方案，核心都是"机器人感知相对于自己的位置"

轨迹自适应原理：
─────────────────────────────────────────────────────
核心思想：
├─ 示教轨迹记录的是"关节角度序列"
├─ 每个关节角度对应末端在 {B_teach} 中的一个位置
├─ 如果 {B_play} 与 {B_teach} 有偏差 Δ
└─ 需要将关节角度序列调整为适应新坐标系的序列

不能直接变换关节角度：
├─ 关节角度是关节空间的量，不能直接做坐标变换
├─ 正确的做法：在笛卡尔空间变换末端位置
└─ 再通过逆运动学得到新的关节角度

轨迹自适应流程：
─────────────────────────────────────────────────────
Step 1：示教轨迹点 → 笛卡尔位置
├─ 对每个示教点，做正运动学 FK
├─ P_teach[i] = FK(θ_teach[i])
└─ 得到示教轨迹的末端位置序列

Step 2：坐标变换
├─ P_play[i] = transform(P_teach[i], Δ)
│ └─ 即：P_play[i] = R(-Δθ) × (P_teach[i] - ΔT)
└─ 得到回放时末端应该到达的位置

Step 3：笛卡尔位置 → 关节角度
├─ 对每个回放位置，做逆运动学 IK
├─ θ_play[i] = IK(P_play[i])
└─ 得到回放时各关节的目标角度

注意事项：
├─ 逆运动学可能有多解
├─ 需要选择与示教时一致的解（避免关节姿态突变）
├─ 如果目标位置超出关节限位 → 报警
└─ 如果目标位置接近奇异点 → 警告

轨迹自适应的三种策略：
─────────────────────────────────────────────────────
策略1：全局坐标变换（最常用）
├─ 原理：将整个示教轨迹在笛卡尔空间做刚性变换
├─ 变换量：Δ = (Δx, Δy, Δθ)（标定得到）
├─ 优点：
│ ├─ 实现简单，只需一次变换
│ ├─ 示教轨迹的几何形状完全保持
│ └─ 适合底座偏差较小的场景（< 3cm）
├─ 缺点：
│ └─ 偏差过大时，关节可能超出限位
└─ 适用：底座偏差 1–2cm，2° 的场景

策略2：关键点对齐 + 局部插值（进阶）
├─ 原理：
│ ├─ 保留示教轨迹的关键路径形状
│ ├─ 仅对关键点（如接近点、接触点）做坐标变换
│ └─ 中间路径点通过插值生成
├─ 优点：
│ ├─ 更好地适应底座较大偏差
│ └─ 减少关节超限风险
├─ 缺点：计算复杂，需要重新规划路径
└─ 适用：底座偏差 3–5cm，5° 的场景

策略3：基于轮廓重规划（最复杂）
├─ 原理：
│ ├─ 将示教轨迹转换为"表面参数"
│ │ └─ 如：沿内壁的弧长位置、高度位置
│ ├─ 回放时根据新标定的位置，重新计算笛卡尔轨迹
│ └─ 最后逆运动学得到关节角度
├─ 优点：轨迹与表面完全贴合
├─ 缺点：需要精确的几何模型
└─ 适用：不同型号需要不同轨迹的场景

推荐：策略1（全局坐标变换）
├─ 原因：
│ ├─ 底座偏差 1–2cm，2° 在策略1的容限内
│ ├─ 实现最简单，调试最方便
│ └─ 结合力柔顺控制，残余误差可被吸收
└─ 备选：如果策略1不够，再升级到策略2

轨迹自适应详细流程：
─────────────────────────────────────────────────────
输入：
├─ 示教轨迹：θ_teach[i], i = 0, 1, ..., N-1（关节角度序列）
├─ 示教时描述：T_teach（圆心位置和朝向）
└─ 回放时标定结果：T_play（当前位置）

Step 1：计算坐标变换量 Δ
├─ Δx = T_play.x - T_teach.x
├─ Δy = T_play.y - T_teach.y
├─ Δθ = T_play.θ - T_teach.θ
└─ 验证：|Δx|, |Δy| < 3cm, |Δθ| < 5°（在容限内则继续）

Step 2：关节角度 → 笛卡尔位置
├─ for i in range(N):
│ ├─ P_base[i] = FK(θ_teach[i]) # 正运动学
│ └─ 得到末端在示教底座坐标系 {B_teach} 中的位置
└─ 输出：P_base[i], i = 0, ..., N-1

Step 3：笛卡尔坐标变换
├─ for i in range(N):
│ ├─ R = rotation_matrix(-Δθ)
│ ├─ P_shifted = R × (P_base[i] - [Δx, Δy, 0])
│ └─ P_play[i] = P_shifted
└─ 输出：P_play[i]（变换后的末端位置）

Step 4：检查关节限位
├─ for i in range(N):
│ ├─ 检查 P_play[i] 对应的关节角度是否超限
│ └─ 如果超限 → 报警，提示"偏差过大，需重新标定"
└─ 通过 → 继续

Step 5：笛卡尔位置 → 关节角度（逆运动学）
├─ for i in range(N):
│ ├─ 候选解 = IK(P_play[i])（可能有多个解）
│ ├─ 选择与 θ_teach[i] 最接近的解（关节距离最小）
│ └─ θ_play[i] = 选中的逆运动学解
└─ 输出：θ_play[i]（自适应的关节角度序列）

Step 6：速度规划（保持与示教相同）
├─ 使用与示教时相同的速度规划参数
├─ 生成带时间戳的轨迹
└─ 输出：轨迹就绪，可执行

总时间估算：
├─ N = 1000个轨迹点（10秒轨迹 @ 100Hz）
├─ 正运动学：1000次（可批量计算）
├─ 坐标变换：1000次（矩阵运算，很快）
├─ 逆运动学：1000次（稍慢，但可接受）
├─ 总时间：< 1秒（现代CPU完全可满足）

逆运动学多解处理：
─────────────────────────────────────────────────────
问题：逆运动学通常有多个解
├─ 6轴机械臂在大多数位置有2–4个可行解
├─ 不同解对应不同的关节姿态
├─ 如果选错解，关节会"翻转"，轨迹混乱
└─ 必须选择与示教时一致的解

解决：关节空间距离最小化
├─ 示教时的关节角度：θ_teach[i]
├─ 逆运动学候选解：θ_candidates[k]
├─ 选择标准：min ||θ_candidates[k] - θ_teach[i]||
└─ 选择与示教时关节角度最接近的解

示例：
├─ IK求解得到4个候选解：[100°, 45°, 300, 20°, -30°, 10°]
├─ 示教时关节角度：[105°, 42°, 300, 18°, -28°, 8°]
├─ 计算距离：解1=√(25+9+0+4+4+4)=√46=6.8°
├─ 关节距离最小 → 选择解1
└─ 结果：关节运动平滑，与示教姿态一致

边界情况处理：
├─ 情况1：所有候选解都与示教角度差距大（>30°）
│ └─ 原因：标定偏差过大，该点不可达 → 报警
│
├─ 情况2：某候选解会导致关节超限
│ └─ 排除该候选解，选择其他解
│
└─ 情况3：接近奇异点（J5/J6接近0°）
 └─ 警告，该区域关节速度可能突变