Fusing Phase Map Servoing and MPC for High-Precision Robotic Tracking of Dynamic Objects

问题背景

工业机器人常采用“停—拍—动”的作业方式：先移动到固定位姿、采集图像，再执行任务。要处理在传送带上持续运动的工件，机器人必须依靠连续的视觉反馈闭环控制，即视觉伺服。点、边缘、轮廓等常规视觉特征在工业中常见的弱纹理或高反光表面上难以可靠提取。条纹投影得到的相位图提供了另一种选择：投影仪投射正弦条纹，解调得到的相位无需三维重建，就能给出稠密的亚像素级几何信息。然而，以往基于相位图的伺服研究只针对静止或缓慢变化的场景。

运动目标带来两方面困难。其一，感知和图像处理延迟使测得的相位图滞后于物体的实际状态，且目标速度越快，滞后越严重。其二，模型预测控制（MPC）能在约束下对未来状态进行规划，天然适合动态跟踪；但若直接作用于相位图中成千上万个像素，求解代价过高，难以高频运行。

方法原理

该框架保留稠密相位图作为反馈信号，但压缩由其构建的控制问题，使 MPC 能以相机帧率运行；同时引入扩展卡尔曼滤波（EKF），越过感知延迟进行前瞻预测。

1. 以相位图为特征。 相机和投影仪安装在机械臂末端。观测相位图与期望相位图之差，经解析推导的相位交互矩阵 L（每个相位像素对应一行）映射为末端的六自由度速度。
2. 梯度引导稀疏化。 相位梯度低于阈值 ε 的像素几乎不含运动信息，其在 L 中对应的行被剔除。所有实验中，ε 取最大梯度幅值的 0.15 倍。
3. PCA 嵌入。 将剩余的相位特征投影到 d = 8 个主成分上，MPC 因而只需处理 8 × 6 的交互矩阵，而非 m × 6（m 为相位图的像素数）。在相位梯度充足的场景中，降维后的矩阵仍保持秩 6，六个运动方向均可控。
4. 自适应预测时域。 MPC 的预测时域随相位变化率调整：相位变化缓慢时保持 N_min = 3，目标加速时经 tanh 函数平滑增大，趋近 N_max = 10。
5. 以 EKF 预测作为 MPC 参考。 EKF 估计目标的位置和速度，在预测时域内递推，并将每个预测位置转换为预测相位图。MPC 以这一序列为参考轨迹进行跟踪，从而补偿延迟，每次只执行优化解中的第一个控制量。

实验结果

仿真在 MATLAB 中进行，比较了八种控制器：两种基线（A 使用完整相位图并在每一步重新计算交互矩阵；B 在 A 的基础上加入热启动），以及使用一种、两种或全部三种降维策略的六种变体（M1–M6）。同时结合 PCA、稀疏交互矩阵和自适应时域的 M6 在约 20 次迭代内收敛，RMS 误差最低，求解时间分布也最低、最集中。对降维参数的扫描表明 ε = 0.15、d = 8 是较好的折中：求解时间 1.9 ms，RMS 误差 0.048 mm（表 2）；较宽松的设置（ε = 0.05，d = 12）为 3.5 ms 和 0.042 mm，较激进的设置（ε = 0.30，d = 6）为 1.3 ms 和 0.081 mm（表 2）。相机曝光、相位计算和 EKF 更新的耗时与是否降维无关，节省的时间来自 MPC/QP 求解环节。实验使用 640 × 480、120 Hz 的相机，在 Intel Core i7-12700 上单线程运行，求解器为 qpOASES。

实物实验中，投影仪–相机安装在机械臂上，物体在传送带上运动。物体静止时，所提控制器的精度与原始相位图伺服控制律相当，误差约 0.3 mm，但收敛所需迭代次数由约 30 次减少到 20 次。目标高速运动时 EKF 的作用最为明显：不加 EKF 时相位误差峰值接近 0.2 rad，加入 EKF 后典型峰值低于 0.1 rad。

作者指出，可达到的跟踪速度受限于感知吞吐量。在 120 Hz 相机、约 8.3 ms 控制周期下，系统可稳定跟踪约 0.12 m/s 以内的目标；速度更高时，相位图生成跟不上，相位噪声显著增大，因此论文未报告更高速度下的结果。目前的实现针对相对受控视觉条件下的单目标跟踪；加快相位图生成和多目标跟踪留待后续工作。