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

01 动机Motivation

视觉伺服是机器人精确定位与控制的基础技术。然而，无纹理物体（textureless objects） ——工业场景中大量存在的表面均匀、缺乏特征点的零件——令传统方法举步维艰：手工设计的特征无法建立稳定的视觉对应，深度学习方法又多依赖单帧预测，遭遇遮挡或光照突变时极易失稳。Visual servoing is a foundational technique for precise robot positioning and control. Yet textureless objects — the uniform-surfaced, feature-poor parts that abound in industrial scenes — leave traditional methods struggling: hand-crafted features cannot establish stable visual correspondences, and deep learning methods mostly rely on single-frame prediction, becoming highly unstable under occlusion or abrupt illumination change.

"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."
— 论文摘要原文— from the paper abstract

感知-控制耦合框架总览 — **图1：感知-控制耦合视觉伺服框架。** EKF 融合每帧关键点检测与运动先验，生成可靠的 6D 物体位姿供视觉伺服使用。相机运动反过来又增强后续关键点跟踪，形成闭环。**Figure 1: the perception-control coupled visual servoing framework.** The EKF fuses per-frame keypoint detection with the motion prior to produce reliable 6D object poses for visual servoing. Camera motion in turn strengthens subsequent keypoint tracking, closing the loop. "An EKF integrates complementary information from keypoints and the motion prior, producing reliable 6D object poses for visual servoing."

95.1%正常条件伺服成功率（本文方法）Servoing success rate under normal conditions (ours)

82.6%不利条件伺服成功率（本文方法）Servoing success rate under adverse conditions (ours)

89.9%真实抓取任务平均成功率Average success rate on real grasping tasks

0.81速度不确定度与速度误差 Pearson 相关系数Pearson correlation coefficient between velocity uncertainty and velocity error

现有方法的不足Shortcomings of existing methods

IBVS 的局限Limitations of IBVS

依赖手工设计特征，无纹理物体提取困难Relies on hand-crafted features, which are hard to extract on textureless objects
存在局部极小值问题，convergence basin 有限Suffers from local minima, with a limited convergence basin
基于单帧预测，忽略时序信息Based on single-frame prediction, ignoring temporal information

PBVS 的局限Limitations of PBVS

依赖单帧位姿估计，遮挡/光变时容易失败Relies on single-frame pose estimation and fails easily under occlusion or illumination change
不输出控制不确定度，无安全降速机制Outputs no control uncertainty, hence no safety slow-down mechanism
"often fail under adverse visual conditions, leading to unstable control"

02 方法Method

本文核心是将感知与控制形成双向闭环：感知阶段用 EKF 融合关键点与运动先验估计 6D 位姿；控制阶段用概率控制律生成相机速度并评估其不确定度；该速度再作为下一帧感知的运动先验，完成闭环。The core idea of this paper is to form a bidirectional closed loop between perception and control: in the perception stage, an EKF fuses keypoints with the motion prior to estimate the 6D pose; in the control stage, a probabilistic control law generates the camera velocity and evaluates its uncertainty; that velocity then serves as the motion prior for the next perception frame, closing the loop.

系统框架图 — **图2：系统整体闭环流程。** 感知阶段：EKF 融合关键点检测与运动先验，估计 6D 物体位姿。控制阶段：概率控制律计算相机速度，驱动机器人运动，并将该速度作为下一感知帧的运动先验。**Figure 2: the overall closed-loop pipeline of the system.** Perception stage: the EKF fuses keypoint detection with the motion prior to estimate the 6D object pose. Control stage: the probabilistic control law computes the camera velocity, which actuates the robot and serves as the motion prior for the next perception frame. "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 位姿估计Perception stage: keypoint-based EKF pose estimation

感知主干采用 PVNet 从 RGB 图像中提取每帧 2D 关键点及其不确定度。 EKF 以 离散时间恒速运动模型（discrete-time constant-velocity motion model）传播状态，状态向量包含 6-DOF 物体位姿（位置 + 旋转）。旋转采用李代数（Lie algebra）参数化的误差状态线性化，通过 Jacobian 将 2D–3D 关键点对应关系融合进测量更新步骤。即使某些帧关键点完全遮挡，EKF 仍能利用运动先验维持稳健估计。The perception backbone uses PVNet to extract per-frame 2D keypoints and their uncertainties from RGB images. The EKF propagates the state with a discrete-time constant-velocity motion model, and the state vector contains the 6-DOF object pose (position + rotation). Rotation is linearized in an error-state form parameterized on the Lie algebra, and 2D–3D keypoint correspondences are fused into the measurement update step through the Jacobian. Even when keypoints are fully occluded in some frames, the EKF can still maintain a robust estimate using the motion prior.

控制阶段：概率控制律Control stage: the probabilistic control law

区别于标准 PBVS，本文提出的概率控制律（probabilistic control law）不仅输出相机速度指令，还输出一个 6×6 速度协方差矩阵，用微分熵（differential entropy）量化控制置信度。当熵值超过安全阈值时，速度指令将被显著降低：Unlike standard PBVS, the probabilistic control law proposed here outputs not only a camera velocity command but also a 6×6 velocity covariance matrix, quantifying control confidence with the differential entropy. When the entropy exceeds a safety threshold, the velocity command is significantly reduced:

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

协方差传播通过对 PBVS 控制方程进行 Jacobian 线性化来实现，将 EKF 状态协方差传递至最终速度指令的不确定度中，形成端到端的不确定度感知控制链路。Covariance propagation is realized by Jacobian linearization of the PBVS control equations, carrying the EKF state covariance through to the uncertainty of the final velocity command and forming an end-to-end uncertainty-aware control chain.

实验对比条件 — **图3（原文 Figure 4-5）：实验平台与评估对象。** 顶部：Franka Emika 7-DOF 机械臂 + Intel RealSense D435 相机。底部：5种无纹理目标物体（Zigzag、Pipe、Gear、Cat、Connector）及不同背景、光照、遮挡条件。**Figure 3 (Figures 4-5 in the paper): experimental platform and evaluation objects.** Top: a Franka Emika 7-DOF manipulator plus an Intel RealSense D435 camera. Bottom: 5 textureless target objects (Zigzag, Pipe, Gear, Cat, Connector) under varying background, illumination and occlusion conditions. "Experimental setup. Top: Hardware platform for visual servoing. Bottom: Target objects for evaluation."

03 实验Experiments

在真实的 7-DOF Franka Emika 机械臂平台上，针对 5 种无纹理物体，与 IBVS+PVNet 和 PBVS+PVNet 基准方法进行对比，评估指标包括伺服成功率（SR）、末端平移误差（TE）、旋转误差（RE）和轨迹长度比（LR）。On a real 7-DOF Franka Emika manipulator platform, the method is compared with the IBVS+PVNet and PBVS+PVNet baselines on 5 textureless objects. Evaluation metrics include servoing success rate (SR), end-effector translation error (TE), rotation error (RE) and trajectory length ratio (LR).

Table I：伺服精度对比（正常条件 vs 不利条件）Table I: servoing accuracy comparison (normal vs. adverse conditions)

方法Method	成功率 SR (%)Success rate SR (%)	平移误差 TE (mm)Translation error TE (mm)	旋转误差 RE (°)Rotation error RE (°)	轨迹长度比 LRTrajectory length ratio LR
正常条件 (Normal Conditions)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）Ours (Proposed)	95.12	3.17 ± 1.45	3.81 ± 2.68	1.11 ± 0.15
不利条件 (Adverse Conditions — 遮挡、光变、复杂背景)Adverse Conditions — occlusion, illumination change, cluttered background
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）Ours (Proposed)	82.61	3.99 ± 2.72	5.70 ± 3.98	1.18 ± 0.23

Table II：真实抓取实验成功率Table II: success rates of the real-robot grasping experiments

物体Object	IBVS + PVNet (%)	PBVS + PVNet (%)	本文方法 (%)Ours (%)
Zigzag	—	—	95.8
Pipe	—	—	78.3
Gear	—	—	95.6
Cat	—	—	90.5
Connector	—	—	89.5
平均Average	—	—	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**，说明概率控制律输出的不确定度能有效预测控制误差，验证了安全降速机制的合理性。**Figure 4 (Figure 8 in the paper): correlation analysis between velocity uncertainty and the actual velocity error.** The Pearson correlation coefficient is **0.81**, showing that the uncertainty output by the probabilistic control law effectively predicts control error and validating the rationale of the safety slow-down mechanism. "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%，验证了方法在实际操作任务中的可靠性。**Figure 5 (final figure in the paper): results of the real-robot grasping experiments.** Our method achieves the highest success rate on the grasping tasks for all 5 textureless objects, averaging 89.9% and confirming the reliability of the method in practical manipulation tasks.

关键消融结论Key ablation findings

实验表明 EKF 时序融合对在不利条件下维持高成功率至关重要： PBVS+PVNet 在不利条件下成功率仅 40.58%，而本文方法达到 82.61%，提升幅度超过 42 个百分点。概率控制律输出的不确定度（Pearson r = 0.81）能有效预测控制误差，支持自适应安全降速，进一步提升系统鲁棒性。The experiments show that EKF temporal fusion is essential for maintaining a high success rate under adverse conditions: PBVS+PVNet reaches only 40.58% under adverse conditions, whereas our method attains 82.61%, an improvement of more than 42 percentage points. The uncertainty output by the probabilistic control law (Pearson r = 0.81) effectively predicts control error and supports adaptive safety slow-down, further improving system robustness.

04 局限性Limitations

说明：论文未设独立的"局限性"章节。以下第1、2条为作者在结论/未来工作中明确提出（stated），第3、4条由方法设计推断得出（推断 / inferred）。Note: the paper has no standalone "Limitations" section. Items 1 and 2 below are explicitly put forward by the authors in the conclusion / future work (stated); items 3 and 4 are inferred from the method design (inferred).

仅适用于静态环境（Stated）Applies only to static environments (Stated)

当前框架假设目标物体静止，EKF 的恒速运动模型难以适应动态目标。作者明确指出"extending the framework to dynamic environments"为重要未来方向。The current framework assumes the target object is stationary, and the constant-velocity motion model of the EKF adapts poorly to dynamic targets. The authors explicitly point to "extending the framework to dynamic environments" as an important future direction.

依赖已知 3D 模型（Stated）Depends on a known 3D model (Stated)

PVNet 关键点检测和 EKF 测量更新均需要物体预先建立的 3D 模型。作者指出未来需"generalizing to more challenging object types such as CAD-less or deformable objects"。Both PVNet keypoint detection and the EKF measurement update require a pre-built 3D model of the object. The authors note that future work needs "generalizing to more challenging object types such as CAD-less or deformable objects".

依赖合成数据训练（推断 / Inferred）Depends on training with synthetic data (Inferred)

PVNet 通常在合成渲染数据上训练，存在域差距（sim-to-real gap），可能在真实环境外观差异较大时泛化能力下降。论文未直接量化此问题，属推断局限。PVNet is usually trained on synthetically rendered data, so a domain gap (sim-to-real gap) exists and generalization may degrade when real-world appearance differs substantially. The paper does not directly quantify this issue, so it is an inferred limitation.

无主动视角选择（Stated）No active viewpoint selection (Stated)

当前方法被动跟随相机运动，缺乏主动选择最优视角以减少遮挡的机制。作者明确将"incorporating active viewpoint selection"列为未来工作。The current method passively follows camera motion and lacks a mechanism for actively choosing the optimal viewpoint to reduce occlusion. The authors explicitly list "incorporating active viewpoint selection" as future work.