二阶锥约束规划的机器人视觉闭环位姿自协调方法

仲训杲 罗家国 田军 仲训昱 彭侠夫 刘强

摘要：

将机器人“手眼”位姿自协调视为无标定约束规划问题，提出一种基于二阶锥约束规划的视觉闭环控制方法。在图像平面和笛卡儿空间分别建立基于图像与基于位姿的视觉伺服控制算法;在路径约束和局部极小约束准则下构建二阶锥凸优化模型，实现了图像特征轨迹和机器人运动路径折中最优;将所提出的二阶锥约束规划模型嵌入自适应状态估计器，实现了机器人雅可比矩阵在线映射学习，解决了“手眼”标定参数和视觉深度信息未知问题；
最后通过无标定机器人视觉定位证明了二阶锥约束规划模型的有效性，真实环境抓取操作表明了机器人位姿自协调控制的可行性。

关键词：位姿协调；
约束规划；
无标定视觉伺服；
混合闭环反馈控制

中图分类号：TP242.2

DOI：10.3969/j.issn.1004132X.2024.06.012

开放科学（资源服务）标识码（OSID）：

Robot Vision Closed-loop Pose Autonomous Coordination Method with

Second-order Cone Constrained Programming

ZHONG Xungao1，4  LUO Jiaguo1  TIAN Jun2  ZHONG Xunyu2  PENG Xiafu2  LIU Qiang3

1.School of Electrical Enginnering and Automation， Xiamen University of Technology， Xiamen，

Fujian，361024

2.School of Aerospace Engineering，Xiamen University，Xiamen，Fujian，361002

3.School of Engineering Mathematics and Technology，University of Bristol，Bristol，BS8 1TW

4.Xiamen Key Laboratory of Frontier Electric Power Equipment and Intelligent Control，Xiamen，

Fujian，361024

Abstract：
A robot “hand-eye” pose autonomous coordination was regarded as a uncalibration constrained programming  problem， and a visual closed-loop control method was proposed based on second-order cone constrained programming. Firstly， the visual servoing control algorithms were constructed in the image planes and Cartesian space， respectively based on images and positions. After that， by established the path constraint and the local minimal constraint rules， and a second-order cone convex optimization model was constructed to realize the compromise optimal control of image feature trajectory and robot motion path. Moreover， the proposed second-order cone constrained programming model was embedded with an adaptive state estimator， to realize robotic Jacobian matrix online mapping learning， and to solve the unknown problems of “hand-eye” calibration parameters and visual depth information. Finally， the uncalibrated robot visual positioning experiments prove the effectiveness of the convex optimization planning model， and the real grasping tasks illustrate the feasibility of the robot pose autonomous coordination.

Key words：
pose coordination; constrained programming; uncalibration visual servoing; hybrid closed-loop feedback control

收稿日期：20230807

基金项目：国家自然科学基金（61703356）；
福建省自然科学基金（2022J011256）

0  引言

当前，机器人视觉感知技术得到快速发展［1-2］，特别是基于深度学习的机器人任务操作备受研究人员关注［3-6］，代表性研究成果为Dex-Net［7］方法。该方法采用深度神经网络对目标姿态进行估计，机器人通过运动规划算法实施目标抓取操作，即基于深度学习的机器人抓取操作采用开环控制方式，机器人视觉感知系统不具备位姿在线协调能力［8］。

针对机器人位姿在线自协调问题，基于位置的视觉伺服（position-based visual servoing，PBVS）和基于图像的视觉伺服（image-based visual servoing，IBVS）是目前广泛研究的控制方式。PBVS又称3D视觉伺服，其反馈控制量为笛卡儿坐标值。该方法通过相机投影模型估计机器人与目标之间的相对位姿信息，控制系统根据位姿差分进行轨迹规划［9-10］。文献［11］通过引入3D可视集概念，提出一种带视场约束的全局稳定PBVS控制方法。相比之下，IBVS方法通过视觉特征调节机器人姿态，利用图像误差计算机器人运动控制量［12-14］，该方法因避免了目标三维重建而深受研究人员青睐［15-17］。

如REDWAN等［14］提出改进IBVS方法提升系统动态性能; WANG等［18］研究了深度独立图像雅可比矩阵问题，建立了图像误差与机器人关节之间的映射关系; LIANG等［19］研究了基于深度独立图像雅可比矩阵的移动机器人视觉伺服运动控制方法; 而HAO等［20］将Broyden-Gauss-Newton、Broyden递归最小二乘法统一到状态空间中，研究了基于状态变量的视觉伺服方法。一般地，IBVS和PBVS方法依赖机器人系统标定参数和视觉深度信息计算雅可比矩阵［21］。

在实际应用中，系统标定和视觉深度信息计算复杂，精度难以保证，直接影响控制系统的鲁棒性能，此外，机器人按照固定的标定模式工作难以应对不同的工作环境， 因此，“手眼”未标定视觉闭环控制方法绕开系统标定和视觉深度信息的求解，有利于拓展机器人应用场景［18-19］。

目前机器人视觉闭环控制已有广泛研究，但是现实应用还存在挑战，容易产生以下问题：
IBVS方法直接对图像特征实施控制，机器人位姿易失控，导致机器人迂回运动; PBVS方法直接对机器人位姿实施控制，目标易脱离相机视场，导致图像特征丢失。本文针对机器人位姿自协调控制，在未知“手眼”标定参数和视觉深度信息条件下，提出一种基于二阶锥约束规划的视觉闭环控制方法，本文主要研究内容包括：①将机器人位姿自协调视为约束规划，在图像平面和笛卡儿空间构建基于二阶锥约束规划的混合闭环反馈控制框架；
②建立机器人约束准则，以二阶锥凸优化模型求解机器人最优控制，有效避免图像特征丢失，克服机器人迂回振荡运动；
③提出的视觉闭环控制框架采用自适应状态估计器，实现了雅可比矩阵的在线学习，解决了相机标定参数和视觉深度信息未知的问题。

最后通过机器人视觉定位实验，验证了图像特征轨迹和机器人运动路径折中最优控制。机器人目标抓取实验说明了研究方法对机器人位姿协调的有效性。本文提出的带有约束准则的视觉闭环控制方法满足无标定机器人位姿自协调优化控制，一定程度上体现了机器人的自主性。

1  机器人位姿自协调控制问题描述

针对机器人位姿自协调，本文构建了图像反馈和位姿反馈控制方案。其中图像闭环控制采用“图像关节”映射描述当前图像特征s（t）（s（t）=（s1，s2，…，sm）T∈Rm）与机器人关节θ（t）（θ（t）=（θ1，θ2，…，θn）T∈Rn）之间的动态关系：s（t）θ（t），Rm→Rn。一般通过雅可比矩阵建立图像特征s（t）与机器人关节θ（t）之间的微分方程：

s·（t）=Js（s，θ）θ·（t）（1）

Js（s，θ）=s（t）θ（t）=

s1（t）θ1（t）s1（t）θ2（t）…s1（t）θn（t）

s2（t）θ1（t）s2（t）θ2（t）…s2（t）θn（t）

sm（t）θ1（t）sm（t）θ2（t）…sm（t）θn（t）（2）

Js（s，θ）被称为图像雅可比矩阵，Js（s，θ）∈Rm×n，其中，m为图像特征维度，n为机器人关节维度。

假设机器人期望图像特征为s*（t），s*（t）∈Rm，在图像平面内建立图像反馈误差es（t），则

es（t）=s*（t）－s（t）（3）

考虑期望图像特征s*（t）为常量，将式（3）对时间t求导并代入式（1），可得

e·s（t）=－Js（s，θ）θ·（t）（4）

由式（4）可建立图像闭环反馈控制律如下：

us（t）=λJ+s（s，θ）es（t）（5）

式中，us（t）为图像反馈控制器输出；
λ为控制系数；
J+s（s，θ）为雅可比矩阵Js（s，θ）的Penrose广义逆，当Js（s，θ）列满秩时，J+s=（JTsJs）－1JTs。

同理，机器人位姿闭环反馈控制采用“关节位姿”映射描述机器人关节θ（t）（θ（t）∈Rn）与位姿h（t）（h（t）=（h1，h2，…，hp）T∈Rp）之间的非线性关系：θ（t）h（t），Rn→Rp。通过雅可比矩阵建立关节与位姿之间的微分方程如下：

h·（t）=Jh（h，θ）θ·（t）（6）

其中，Jh（h，θ）被称为位姿雅可比矩阵，Jh（h，θ）∈Rp×n，表示为

Jh（h，θ）=hθ=

h1θ1h1θ2…h1θn

h2θ1h2θ2…h2θn

hpθ1hpθ2…hpθn（7）

假设机器人期望位姿为h*（t），（h*（t）∈Rp），在笛卡儿空间建立位姿反馈误差eh（t），如下：

eh（t）=h*（t）－h（t）（8）

考虑期望位姿h*（t）为常量，将式（8）对时间t求导，并代入式（6），可得

e·h（t）=－Jh（h，θ）θ·（t）（9）

由式（9）可建立位姿闭环反馈控制律如下：

uh（t）=βJ+h（h，θ）eh（t）（10）

式中，uh（t）为位姿反馈控制器输出；
β为控制系数；
J+h为Jh的Penrose广义逆，当Jh列满秩时，J+h=（JThJh）－1JTh。

2  机器人约束规划建模

在二维图像平面和三维笛卡儿空间内建立约束规则，构建二阶锥约束规划模型和雅可比矩阵在线学习模型，提出方法框架如图1所示。算法对图像轨迹和机器人运动路径进行有效规划，折中优化图像特征轨迹和机器人运动路径，克服图像特征丢失和机器人迂回问题，改善控制系统的动态性能。

2.1  图像特征轨迹约束

图像特征轨迹约束的目标处在相机视场范围内，同时约束特征点以最短路径收敛至期望位置。为此，如图2a所示，在2D图像平面中，定义ds（t）为下一时刻图像特征s（t+1）到当前图像特征s（t）与期望图像特征s*（t）连线的距离，其计算式如下：

ds（t）=‖（s（t+1）－s*（t））×（s（t）－s*（t））‖‖（s（t）－s*（t））‖（11）

由图像误差公式（式（3））可将式（11）化简为

ds（t）=‖es（t+1）×es（t）‖‖es（t）‖（12）

进一步可得

d2s（t）=‖es（t+1）×es（t）‖2‖es（t）‖2=

［（eTs（t+1）es（t+1））（eTs（t）es（t））－

（eTs（t+1）es（t））（eTs（t）es（t+1））］·

（eTs（t）es（t））-1=

eTs（t+1）（E－es（t）eTs（t）eTs（t）es（t））es（t+1）（13）

式中，E为单位矩阵。

令

Gs（t）=E－es（t）eTs（t）eTs（t）es（t）（14）

可得

ds（t）=（eTs（t+1）Gs（t）es（t+1））1/2=

‖Gs（t）es（t+1）‖（15）

根据图像闭环反馈控制律（式（5）），在图像平面中存在以下等式：

s（t+1）=Js（t）θ·（t）T+s（t）（16）

式中，T为采样周期。

那么，式（15）可变为

ds（t）=‖Gs（t）（es（t）+Js（t）θ·（t）T）‖ （17）

由式（11）可知，若ds（t）→0，则s（t+1）处在s（t）与s*（t）的连线上，即图像特征以直线路径收敛至期望位置。

2.2  机器人路径约束

机器人路径约束的约束末端操作器无回退运动，同时约束机器人以最短路径收敛至目标位姿。

为此，如图2b所示，在3D笛卡儿空间中定义dh（t）为下一时刻机器人位姿h（t+1）到当前位姿h（t）与期望位姿h*（t）连线的距离，其计算式如下：

dh（t）=‖（h（t+1）－h*（t））×（h（t）－h*（t））‖‖h（t）－h*（t）‖（18）

由式（17）推导过程可知

dh（t）=‖Gh（t）eh（t+1）‖=

‖Gh（t）（eh（t）+Jh（t）θ·（t）T）‖（19）

Gh（t）=E－eh（t）eTh（t）eTh（t）eh（t）（20）

由式（18）可知，若dh（t）→0，则h（t+1）处在h（t）与h*（t）的连线上，即机器人以直线最短路径收敛至期望位姿。

（a）图像平面

（b）机器人运动空间

为使控制系统在图像平面和笛卡儿空间同时满足路径最优，建立如下目标函数：

min （fds（t）+（1－f）dh（t））（21）

其中，f为权重因子，f∈［0，1］。若f=0，则系统为位姿闭环控制，机器人运动路径可达最优；
若f=1，则系统为图像闭环控制，图像特征轨迹可达最优。

2.3  局部极小约束

局部极小本质上是图像特征误差es（t）和机器人位姿误差eh（t）收敛不一致， 因此，为保证图像误差收敛，则有

‖es（t+1）‖≤γ‖es（t）‖（22）

式中，γ为图像误差收敛比例系数，γ∈（0，1）。

在图像平面中，依据图像闭环控制律，有

es（t+1）=es（t）－Js（t）θ·（t）T（23）

故可得图像误差递减约束规则为

‖es（t）－Js（t）θ·（t）T‖≤γ‖es（t）‖（24）

同理，笛卡儿空间机器人位姿误差递减约束规则为

‖eh（t）－Jh（t）θ·（t）T‖≤η‖eh（t）‖（25）

式中，η为位姿误差收敛比例系数，η∈（0，1）。

结合式（24）、式（25），即可同时确保图像特征误差与机器人位姿误差一致性收敛，克服局部极小问题。

2.4  物理约束

为保证目标可视，下一时刻图像特征应满足s≤s（t+1）≤s-，等价可得

es≤es（t）－Js（t）θ·（t）T≤e-s（26）

es=s－s*  e-s=s-－s*

其中，s、s-分别为图像特征的下界和上界。

为避免机器人关节受限，关节角与关节速度应满足

θ≤θ（t）+θ·（t）T≤θ-（27）

θ·min≤θ·（t）≤θ·max（28）

式中，θ、θ-分别为机器人关节角下限和上限；

θ·min、θ·max分别为关节速度最小值和最大值。

二阶锥规划模型是一种广泛用于求解凸优化问题的有效算法，是线性规划和二次规划的扩展形式。二阶锥规划基本原理为：建立目标函数min（fTx） ，设定锥体约束条件：

s.t.‖Aix+bi‖2≤cTix+di  i=1，2，…，m

其中，优化参数f∈Rn，Ai∈Rnin，bi∈Rni，ci∈Rn，di∈R，优化变量x∈Rn。

规划算法关键是利用二阶锥约束线性化非线性函数，将原问题转化为线性规划问题。综上约束准则（式（24）～式（28））和线性目标函数（式（21）），本文建立如下二阶锥凸优化模型：

min  （fψs（t）+（1-f）ψh（t））

s.t. ‖Gs（t）（es（t）－Js（t）θ·（t）T）‖≤ψs

‖Gh（t）（eh（t）－Jh（t）θ（t）T）‖≤ψh

‖es（t）－Js（t）θ·（t）T‖≤γ‖es（t）‖

‖eh（t）－Jh（t）θ·（t）T‖≤η‖eh（t）‖

es≤es（t）－Js（t）θ·（t）T≤e-s

θ≤θ（t）+θ·（t）T≤θ-

θmin≤θ·（t）≤θmax（29）

其中，θ·（t）、ψs（t）、ψh（t）为模型待优化参数。

3  雅可比矩阵估计模型

本文将凸优化模型（式（29））中未知雅可比矩阵转化为动态系统的状态估计问题，首先将雅可比矩阵写成如下状态向量：

X（t）=（j11（t），j12（t），…，jnm（t））TRnm×1（30）

式中，jnm（t）为雅可比矩阵的第n行与第m列元素。

考虑如下一类动态方程：

X（t+1）=Ψ（t+1，t）X（t）+W（t）（31）

Z（t）=H（t）X（t）+V（t）（32）

其中，X（t）为系统状态向量；
Ψ（t+1，t）为状态转移矩阵，不失一般性，本文取单位矩阵；
W（t）为系统过程噪声；
V（t）为观测噪声；
Z（t）为观测向量，定义为相邻时刻图像特征的变化量：

Z（t）=s（t+1）－s（t）=Js（t）θ·（t）（33）

可得观测矩阵H（t）为

H（t）=diag

{θ·（t）T，…，θ·（t）T}nm （34）

假设系统噪声W（t）和观测噪声V（t）为不相关白噪声序列，协方差分别为Q（t）和R（t），对于离散系统（式（31）和式（32））状态估计问题，可构建卡尔曼滤波最小方差学习方法在线估计雅可比矩阵，算法如下：

初始化：0|0，P0|0

For t=0，t

t+1|t=Ψt|tt|t;

Pt+1|t=Ψt|tPt|tΨTt|t+Qt;

Ht+1←Δθ（t+1）;

Kt+1=Pt+1|tHt+1（Ht+1Pt+1|tHTt+1+Rt+1）－1;

t+1|t+1=t+1|t+Kt+1（Zt+1－Ht+1t+1|t）;

Pt+1|t+1=（I－Kt+1Ht+1）Pt+1|t;

t+1|t+1←t+1|t+1;

End For

雅可比矩阵在线学习算法主要包括状态估计和观测更新两个基本过程，其中t|t表示t时刻状态X（t）的最优估计值；
t+1|t表示下一时刻t+1的预测值；
Pt|t表示估计误差协方差矩阵；
Pt+1|t表示预测误差协方差矩阵；
Kt为卡尔曼增益。在白噪声不相关条件下，卡尔曼滤波递推方程满足最优状态估计，即E{X（t）}=t|t，此时可实现雅可比矩阵在线学习，即从系统状态估计值中恢复雅可比矩阵t+1|t+1←t+1|t+1，雅可比矩阵在线估计流程如图3所示。

4  可行性分析

由误差递减约束规则（式（22））可得系统误差（图像误差和位姿误差）：

‖e～（tk）‖≤λk－1‖e～（t1）‖（35）

e～（tk）=（es（tk），eh（tk））

因此，假设e～（t1）≠0，易知，当k→∞时，e～（tk）→0，即h（tk）→h*以及s（tk）→s*，即图像轨迹和机器人运动轨迹一致性收敛。

在规划阶段，若雅可比矩阵估计值满秩可逆，则雅可比矩阵范数有界，即‖J～（tk）‖≤M，其中，J～（tk）=（Js（tk），Jh（tk）），此时存在系统误差收敛性e～（tk）≡0，否则由系统一致性收敛可得

‖e～（tk）+J～（tk）θ·（tk）T‖≤λ‖e～（tk+1）‖（36）

由式（36）可看出若系统误差收敛至零，说明机器人期望位姿不存在局部极小。反之，如果系统误差不等于零，则存在机器人运动使下一时刻误差收敛至零，所以凸优化模型的解存在。

本文借鉴的二阶锥凸优化方法、卡尔曼滤波、IBVS和PBVS闭环控制的可行性在其他文献已得到证明［1，22］。

5  实验分析

如图4所示，CCD相机安装在六自由度机器人末端，构建眼在手无标定实验平台。

机器人控制器作为下位机完成机器人运动学运算，同时驱动机器人各个关节运动，上位机采用Intel Corei5 2.67GHz CPU，8GBs RAM个人计算机，用于图像处理和执行视觉闭环控制算法。

计算机与机器人控制器通过TCP/IP连接，构成伺服控制系统。

实验过程确保图像特征轨迹和机器人运动轨迹最优，优化模型（式（29））权重因子f=0.5，收敛比例系数γ=η=0.75，系统采样间隔设定T=0.5 s。

5.1  性能验证

为验证提出的二阶锥约束规划视觉闭环控制方法的性能，采用基于图像的视觉伺服IBVS和基于位置的视觉伺服PBVS作为基准对比算法。如图5a所示，本次对比实验在目标板上打印四个圆点作为目标点，为验证提出方法的约束性能，实验要求初始图像特征点（图5b）和期望图像特征点（图5c）尽可能靠近图像边缘。

提取目标点在图像平面中的坐标值作为图像特征s（t）=（s1，s2，s3，s4）T，其中，si为第i个图像点，si=（ui，vi），此时图像雅可比矩阵Js大小为8× 6。机器人位姿采用7维度表示方法（包含位置和四元素姿态），所以位姿雅可比矩阵Jh大小为7× 6，机器人在初始位置通过试探运动对雅可比矩阵进行初始化，得到结果如下（图6）：

Js（0）=

－0.8600.11－0.02－1.02－0.160－0.86－0.101.020.02 －0.16－0.8600.110.02－1.020.160－0.860.111.02－0.02－0.16－0.860－0.11－0.02－1.020.160－0.860.111.020.020.16－0.860－0.100.02－1.02－0.160－0.86－0.111.02－0.020.16

Jh（0）=

7.26-1.77-6.23-0.650.54-1.389.52-7.61-7.16-2.671.220.58-1.49-2.393.16-1.41-1.850.190.030.02-0.020.040.040.080.11－0.08－0.010.030.02-0.04-0.08－0.06－0.020.02－0.01-0.0700.01－0.010.010.020.03

（1）图像特征轨迹比较。由实验结果图6a可知，IBVS方法直接对图像特征实施控制，初始特征点沿直线向期望特征点运动，图像特征运动轨迹呈直线。由图6c可知，PBVS方法图像特征轨迹出现严重转弯运动，当特征点处于图像边缘时特征丢失。而由图6e可知，相同的视觉伺服控制任务，本文方法得到的图像特征轨迹被约束在图像平面内，克服了特征点偏离相机视场，且避免了PBVS方法出现的转弯运动。因此，对比三种控制方法得到的图像特征轨迹可知，PBVS方法的性能不如IBVS方法和本文方法的性能。

（2）机器人运动路径比较。由实验结果图6b可知，IBVS方法机器人运动路径非常曲折，机器人在工作空间内出现大范围迂回运动。对于相同的控制任务，图6d显示的PBVS方法直接对机器人位姿实施控制，机器人从初始位置收敛到期望位置，运动路径呈直线。图6f显示本文方法得到的机器人运动路径与PBVS相似，机器人受控制器约束从初始位置收敛到期望位置，运动路径近似直线，避免了迂回运动。因此，对比三种方法得到的机器人运动路径可知，IBVS方法性能不如PBVS方法和本文方法的性能。

综合上述结果，IBVS方法满足图像特征以最短距离向期望图像运动，但是机器人容易产生迂回现象。PBVS方法满足机器人以最短路径向期望位置运动，但图像轨迹出现大幅度转弯，容易导致特征丢失。

而本文方法图像轨迹接近IBVS方法，机器人运动路径接近PBVS方法，说明提出的规划模型满足图像轨迹和机器人运动路径折中约束，兼顾IBVS和PBVS各自的优点。

此外，PBVS和IBVS方法依赖相机标定和目标深度信息计算雅可比矩阵，而本文提出的状态估计模型通过在线估计雅可比矩阵，绕开了系统标定和目标深度信息的计算。

5.2  视觉定位实验

进一步采用机器人视觉定位实验，验证二阶锥约束规划视觉闭环控制方法的可行性。

机器人定位实验选取Aruco图8个角点作为定位特征，因此图像特征向量s（t）=（s1，s2，…，s8）T，其中，si=（ui，vi）为第i个图像点，此时图像雅可比矩阵Js′大小为16× 6，初始值为Js′（0）=（Js（0），Js（0））T，其中Js（0）见式（37），位姿雅可比矩阵Jh大小为7× 6，初始值与式（38）相同。

图7a为机器人视觉定位场景图; 图7b所示为机器人初始位姿相机视角，可见机器人夹爪与目标物相距比较远，此时检测到的8个图像角点较小; 图7c 所示为机器人期望位姿相机视角，可见夹爪到达目标位置，此时图像角点位于图像平面中间位置。

5.2.1  定位实验1

图8所示为机器人平移运动定位实验结果，由图8a可知，特征点从初始位置沿直线向期望位置运动，尽管特征点处在图像边缘，但图像特征被控制器约束在相机视场范围内，避免特征丢失现象的发生。

图8b所示为机器人三维笛卡儿空间运动路径，可见机器人从初始位置沿直线向期望位置运动，机器人运动路径被控制器约束在工作空间内，避免了迂回和关节冲突现象的发生。

图8c所示为图像特征误差，可看出图像误差一致渐近收敛，控制系统未出现局部极小现象，机器人成功完成定位任务。

5.2.2  定位实验2

图9所示为机器人平移和旋转定位实验结果，其中图 9a表明图像特征轨迹沿直线移动，特征点被约束在相机视场内，避免了特征丢失。图9b显示机器人从初始位置沿着直线向期望位置运动，机器人未出现振荡或回退。

图9c说明图像特征误差一致渐近收敛，未出现局部极小现象。

由以上两组不同情形视觉定位实验可以看出，提出的二阶锥规划控制模型能同时约束图像特征轨迹和机器人运动路径，有效应对机器人平移和旋转运动控制任务。

5.3  机器人应用实验

本节通过机器人目标抓取实验验证本文方法的实用性。抓取目标随意放置在机器人工作台上，实验要求机器人通过位姿在线协调自主完成抓取任务。

如图10所示，提取物体8个角点作为图像特征向量s（t），s（t）=（s1，s2，…，s8）T，其中si=（ui，vi）为第i个角点，此时图像雅可比矩阵Js大小为16× 6，位姿雅可比矩阵Jh大小为7× 6，雅可比矩阵初始值与上述定位实验相同。图10a为机器人初始位置相机视角;图10b为机器人期望抓取位置的相机视角，可见本次实验特征点弱小极容易跟踪丢失，对本文控制模型是一种挑战。

（a）初始位置相机视角      （b） 抓取位置相机视角

图11所示为机器人抓取姿态自协调实验结果，由图11a可知，图像平面特征点从初始位置以最佳路径到达期望位置，图像特征运动轨迹近似于直线。

图11b所示为图像特征误差，可见图像误差逐渐收敛，最终一致性收敛于零点，说明控制系统能够有效克服局部极小问题。图11c为机械臂关节速度曲线，可看出关节速度被约束在一定范围内，未发生关节受限。

图11d所示为机器人三维空间抓取运动路径，图中Start为末端操作器起始位姿，End为末端操作器抓取目标时的位姿，可见机器人末端运动轨迹呈直线，未发生迂回运动，机器人抓取姿态自协调控制效果良好，机器人成功完成抓取任务。

抓取实验采用弱小角点作为伺服控制器的输入特征，若采用IBVS方法，机器人运动容易失控，其关节速度容易突变，机器人因振荡运动造成抓取任务失败; 而采用PBVS方法，难以保证所有图像特征点在相机视场范围内，导致抓取任务难以完成。

而本文提出的二阶锥约束规划视觉闭环控制方法对图像特征和机器人运动实施恰当的约束，有效克服了上述伺服控制问题。

图12展示了机械臂通过位姿自协调平稳地将目标抓起，并放置在工作台右下方的过程，可见本文提出方法对机器人位姿自协调控制真实有效。

6  结论

在未知“手眼”标定参数和视觉深度信息情况下，本文提出了一种二阶锥约束规划的机器人视觉闭环位姿自协调方法。在图像平面和笛卡儿空间建立图像与位姿闭环反馈控制框架，结合路径约束和局部极小约束规则，构建了二阶锥凸优化模型，

实现了图像特征轨迹和机器人运动路径折中最优，克服了特征点偏离相机视场和机器人回退问题，提出的视觉闭环控制方法对系统标定参数和视觉深度信息具有自适应性。通过机器人视觉定位与目标抓取实验，验证了本文提出方法对机器人位姿自协调控制的有效性，实验结果表明传统IBVS方法图像稳态误差约为43 pixel，PBVS方法图像稳态误差约为65 pixel，而本文算法图像稳态误差约为25 pixel，是一种保性能的视觉闭环控制方法。

参考文献：

［1］  LI T， YU J P， QIU Q. Hybrid Uncalibrated Visual Servoing Control of Harvesting Robots with RGB-D Cameras［J］. IEEE Transactions on Industrial Electronics， 2023， 70（3）：2729-2737.

［2］  MARWAN Q M， KWEK L C， CHUA S C. Review of Deep Reinforcement Learning-based Object Grasping：Techniques， Open Challenges， and Recommendations［J］. IEEE Access， 2020， 8：178450-178481.

［3］  LI R， QIAO H. A Survey of Methods and Strategies for High-precision Robotic Grasping and Assembly， Tasks-some New Trends［J］. IEEE/ASME Transactions on Mechatronics， 2019， 24（6）：2718-2732.

［4］  SUN F， LIU C， HUANG W， et al. Object Classification and Grasp Planning Using Visual and Tactile Sensing［J］. IEEE Transactions on Systems， Man， and Cybernetics：Systems， 2016，46（7）：969-979.

［5］  GOU M， FAND H S， ZHU Z， et al. RGB Matters：Learning 7-DoF Grasp Poses on Monocular RGBD Images［C］∥Proceeding of the 2021 IEEE International Conference on Robotics Automation（ICRA）. Xian：IEEE， 2021：1-8.

［6］  仲训杲， 徐敏， 仲训昱， 等. 基于多模特征深度学习的机器人抓取判别方法［J］. 自动化学报， 2016， 42（7）：1022-1029.

ZHONG Xungao， XU Min， ZHONG Xunyu，et al. Multimodal Features Deep Learning for Robotic Potential Grasp Recognition［J］. Acta Automatica Sinica， 2016， 42（7）：1022-1029.

［7］  MAHLER J，MATL M，SATISH V，et al. Learning Ambidextrous Robot Grasping Policies［J］. Science Robotics， 2019， 26（4）：1-12.

［8］  黄艳龙， 徐德， 谭民. 机器人运动轨迹的模仿学习综述［J］. 自动化学报， 2022， 48（2）：315-334.

HUANG Yanlong， XU De， TAN Min. On Imitation Learning of Robot Movement Trajectories：a Survey［J］. Acta Automatica Sinica， 2022， 48（2）：315-334.

［9］  SHIVANI K， SAMHITA M， BENJAMIN W， et al. Visual Servoing for Pose Control of Soft Continuum Arm in a Structured Environment［J］. IEEE Robotics and Automation Letters， 2022， 7（2）：5504-5511.

［10］  LEE C， SEO H， KIM J. Position-based Monocular Visual Servoing of an Unknown Target Using Online Self-supervised Learning［C］∥Proceeding of the 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems（IROS）. Macau：IEEE， 2019：4467-4473.

［11］  PARK D H， KWON J H， HA I J. Novel Position-based Visual Servoing Approach to Robust Global Stability Under Field-of-view Constraint［J］. IEEE Transactions on Industrial Electronics， 2012， 59（12）：4735-4752.

［12］  NICOLAS G A， CARLOS P V，JOSE M S ，et al. Robust and Cooperative Image-based Visual Servoing System Using a Redundant Architecture［J］. Sensors， 2011， 11（12）：11885-11900.

［13］  杨唐文， 高立宁， 阮秋琦，等. 移动双臂机械手系统协调操作的视觉伺服技术［J］. 控制理论与应用， 2015， 32（1）：69-74.

YAMG Tangwen， GAO Lining， RUAN Qiuqi， et al. Visual Servo Technology for Coordinated Manipulation of a Mobile Dual-arm Manipulator System［J］. Control Theory & Applications， 2015， 32（1）：69-74.

［14］  REDWAN D， NICOLAS A， YOUCEF M， et al. Dynamic Visual Servoing from Sequential Regions of Interest Acquisition［J］. The International Journal of Robotics Research， 2012， 31（4）：1-19.

［15］  LI J， XIE H， LOW K H， et al. Image-based Visual Servoing of Rotorcrafts to Planar Visual Targets of Arbitrary Orientation［J］. IEEE Robotics and Automation Letters， 2021， 6（4）：7861-7868.

［16］  李宝全， 方勇纯， 张雪波. 基于2D三焦点张量的移动机器人视觉伺服镇定控制［J］. 自动化学报， 2014， 40（12）：2706-2715.

LI Baoquan， FANG Yongchun， ZHANG Xuebo. 2D Trifocal Tensor Based Visual Servo Regulation of Nonholonomic Mobile Robots［J］. Acta Automatica Sinica， 2014， 40（12）：2706-2715.

［17］  徐璠， 王贺升. 软体机械臂水下自适应鲁棒视觉伺服［J］. 自动化学报， 2023， 49（4）：744-753.

XU Fang， WANG Hesheng. Adaptive Robust Visual Servoing Control of a Soft Manipulator in Underwater Environment［J］. Acta Automatica Sinica， 2023， 49（4）：744-753.

［18］  WANG H S， JIANG M K， CHEN W D， et al. Visual Servoing of Robots with Uncalibrated Robot and Camera Parameters［J］. Mechatronics， 2012， 22（7）：661-668.

［19］  LIANG X W， WANG H S， CHEN W D， et al. Adaptive Image-based Trajectory Tracking Control of Wheeled Mobile Robots with an Uncalibrated Fixed Camera［J］. IEEE Transactions on Control Systems Technology， 2015， 23（6）：2266-2282.

［20］  HAO M， SUN Z. A Universal State-space Approach to Uncalibrated Model-free Visual Servoing［J］. IEEE/ASME Transactions on Mechatronics， 2012， 17（5）：833-846.

［21］  CHESI G， HUNG Y S. Global Path-planning for Constrained and Optimal Visual Servoing［J］. IEEE Transactions on Robotics， 2007， 23（5）：1050-1060.

［22］  MARCO C， GIUSEPPE D M， CIRO N， et al.Stability and Convergence Analysis of 3D Feature-based Visual Servoing［J］. IEEE Robotics and Automation Letters， 2022， 7（4）：12022-12019.

（编辑  王艳丽）

作者简介：

仲训杲，男，1983年生，副教授。研究方向为机器人视觉伺服、机器人智能化等。E-mail：zhongxungao@163.com。

仲训昱（通信作者），男，1980年生，副教授、博士研究生导师。研究方向为无人系统的智能感知、导航定位与决策规划等。E-mail：zhongxunyu@xmu.edu.cn。