二階錐約束規劃的機器人視覺閉環位姿自協調方法

仲訓杲 羅家國 田軍 仲訓昱 彭俠夫 劉強

摘要：

將機器人“手眼”位姿自協調視為無標定約束規劃問題，提出一種基于二階錐約束規劃的視覺閉環控制方法。在圖像平面和笛卡兒空間分別建立基于圖像與基于位姿的視覺伺服控制算法;在路徑約束和局部極小約束準則下構建二階錐凸優化模型，實現了圖像特征軌跡和機器人運動路徑折中最優;將所提出的二階錐約束規劃模型嵌入自適應狀態估計器，實現了機器人雅可比矩陣在線映射學習，解決了“手眼”標定參數和視覺深度信息未知問題；最后通過無標定機器人視覺定位證明了二階錐約束規劃模型的有效性，真實環境抓取操作表明了機器人位姿自協調控制的可行性。

關鍵詞：位姿協調；約束規劃；無標定視覺伺服；混合閉環反饋控制

中圖分類號：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。