基于关键点 EKF 的无纹理物体感知-控制耦合视觉伺服

01 动机

视觉伺服是机器人精确定位与控制的基础技术。然而，无纹理物体（textureless objects） ——工业场景中大量存在的表面均匀、缺乏特征点的零件——令传统方法举步维艰：手工设计的特征无法建立稳定的视觉对应，深度学习方法又多依赖单帧预测，遭遇遮挡或光照突变时极易失稳。

"Visual servoing is fundamental to robotic applications, enabling precise positioning and control. However, applying it to textureless objects remains a challenge due to the absence of reliable visual features. Moreover, adverse visual conditions, such as occlusions, often corrupt visual feedback, leading to reduced accuracy and instability in visual servoing."
— 论文摘要原文

感知-控制耦合框架总览 — **图1：感知-控制耦合视觉伺服框架。** EKF 融合每帧关键点检测与运动先验，生成可靠的 6D 物体位姿供视觉伺服使用。相机运动反过来又增强后续关键点跟踪，形成闭环。 "An EKF integrates complementary information from keypoints and the motion prior, producing reliable 6D object poses for visual servoing."

95.1%正常条件伺服成功率（本文方法）

82.6%不利条件伺服成功率（本文方法）

89.9%真实抓取任务平均成功率

0.81速度不确定度与速度误差 Pearson 相关系数

现有方法的不足

IBVS 的局限

依赖手工设计特征，无纹理物体提取困难
存在局部极小值问题，convergence basin 有限
基于单帧预测，忽略时序信息

PBVS 的局限

依赖单帧位姿估计，遮挡/光变时容易失败
不输出控制不确定度，无安全降速机制
"often fail under adverse visual conditions, leading to unstable control"

02 方法

本文核心是将感知与控制形成双向闭环：感知阶段用 EKF 融合关键点与运动先验估计 6D 位姿；控制阶段用概率控制律生成相机速度并评估其不确定度；该速度再作为下一帧感知的运动先验，完成闭环。

系统框架图 — **图2：系统整体闭环流程。** 感知阶段：EKF 融合关键点检测与运动先验，估计 6D 物体位姿。控制阶段：概率控制律计算相机速度，驱动机器人运动，并将该速度作为下一感知帧的运动先验。 "Our framework operates in a closed-loop cycle: during the perception stage, the EKF fuses keypoints and the motion prior to estimate 6D poses; during the control stage, a probabilistic control law computes camera velocity, which actuates the robot and serves as the motion prior for the next perception stage."

感知阶段：基于关键点的 EKF 位姿估计

感知主干采用 PVNet 从 RGB 图像中提取每帧 2D 关键点及其不确定度。 EKF 以 离散时间恒速运动模型（discrete-time constant-velocity motion model）传播状态，状态向量包含 6-DOF 物体位姿（位置 + 旋转）。旋转采用李代数（Lie algebra）参数化的误差状态线性化，通过 Jacobian 将 2D–3D 关键点对应关系融合进测量更新步骤。即使某些帧关键点完全遮挡，EKF 仍能利用运动先验维持稳健估计。

控制阶段：概率控制律

区别于标准 PBVS，本文提出的概率控制律（probabilistic control law）不仅输出相机速度指令，还输出一个 6×6 速度协方差矩阵，用微分熵（differential entropy）量化控制置信度。当熵值超过安全阈值时，速度指令将被显著降低：

"When the entropy exceeds this threshold, the velocity is significantly reduced to ensure safety."

协方差传播通过对 PBVS 控制方程进行 Jacobian 线性化来实现，将 EKF 状态协方差传递至最终速度指令的不确定度中，形成端到端的不确定度感知控制链路。

实验对比条件 — **图3（原文 Figure 4-5）：实验平台与评估对象。** 顶部：Franka Emika 7-DOF 机械臂 + Intel RealSense D435 相机。底部：5种无纹理目标物体（Zigzag、Pipe、Gear、Cat、Connector）及不同背景、光照、遮挡条件。 "Experimental setup. Top: Hardware platform for visual servoing. Bottom: Target objects for evaluation."

03 实验

在真实的 7-DOF Franka Emika 机械臂平台上，针对 5 种无纹理物体，与 IBVS+PVNet 和 PBVS+PVNet 基准方法进行对比，评估指标包括伺服成功率（SR）、末端平移误差（TE）、旋转误差（RE）和轨迹长度比（LR）。

Table I：伺服精度对比（正常条件 vs 不利条件）

方法	成功率 SR (%)	平移误差 TE (mm)	旋转误差 RE (°)	轨迹长度比 LR
正常条件 (Normal Conditions)
IBVS + PVNet	87.81	3.27 ± 1.72	3.91 ± 2.69	1.25 ± 0.36
PBVS + PVNet	84.15	3.66 ± 1.76	3.98 ± 3.81	1.29 ± 0.58
本文方法（Proposed）	95.12	3.17 ± 1.45	3.81 ± 2.68	1.11 ± 0.15
不利条件 (Adverse Conditions — 遮挡、光变、复杂背景)
IBVS + PVNet	52.17	4.91 ± 4.09	6.15 ± 3.98	1.68 ± 0.78
PBVS + PVNet	40.58	5.53 ± 4.08	6.44 ± 4.16	1.92 ± 1.02
本文方法（Proposed）	82.61	3.99 ± 2.72	5.70 ± 3.98	1.18 ± 0.23

Table II：真实抓取实验成功率

物体	IBVS + PVNet (%)	PBVS + PVNet (%)	本文方法 (%)
Zigzag	—	—	95.8
Pipe	—	—	78.3
Gear	—	—	95.6
Cat	—	—	90.5
Connector	—	—	89.5
平均	—	—	89.9

"Our approach consistently outperforms both IBVS and PBVS for all objects, achieving the highest average success rate of 89.9%."

速度不确定度与误差相关性 — **图4（原文 Figure 8）：速度不确定度与实际速度误差的相关性分析。** Pearson 相关系数为 **0.81**，说明概率控制律输出的不确定度能有效预测控制误差，验证了安全降速机制的合理性。 "The predicted velocity uncertainty correlates well with the velocity error. We compute the Pearson correlation coefficient (0.81 in this example)."

**图5（原文 Figure 最终）：真实机器人抓取实验结果。** 本文方法在 5 种无纹理物体的抓取任务中均取得最高成功率，平均达到 89.9%，验证了方法在实际操作任务中的可靠性。

关键消融结论

实验表明 EKF 时序融合对在不利条件下维持高成功率至关重要： PBVS+PVNet 在不利条件下成功率仅 40.58%，而本文方法达到 82.61%，提升幅度超过 42 个百分点。概率控制律输出的不确定度（Pearson r = 0.81）能有效预测控制误差，支持自适应安全降速，进一步提升系统鲁棒性。

04 局限性

说明：论文未设独立的"局限性"章节。以下第1、2条为作者在结论/未来工作中明确提出（stated），第3、4条由方法设计推断得出（推断 / inferred）。

仅适用于静态环境（Stated）

当前框架假设目标物体静止，EKF 的恒速运动模型难以适应动态目标。作者明确指出"extending the framework to dynamic environments"为重要未来方向。

依赖已知 3D 模型（Stated）

PVNet 关键点检测和 EKF 测量更新均需要物体预先建立的 3D 模型。作者指出未来需"generalizing to more challenging object types such as CAD-less or deformable objects"。

依赖合成数据训练（推断 / Inferred）

PVNet 通常在合成渲染数据上训练，存在域差距（sim-to-real gap），可能在真实环境外观差异较大时泛化能力下降。论文未直接量化此问题，属推断局限。

无主动视角选择（Stated）

当前方法被动跟随相机运动，缺乏主动选择最优视角以减少遮挡的机制。作者明确将"incorporating active viewpoint selection"列为未来工作。