二階錐約束規(guī)劃的機(jī)器人視覺閉環(huán)位姿自協(xié)調(diào)方法

仲訓(xùn)杲 羅家國 田軍 仲訓(xùn)昱 彭俠夫 劉強(qiáng)

摘要：

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

關(guān)鍵詞：位姿協(xié)調(diào)；約束規(guī)劃；無標(biāo)定視覺伺服；混合閉環(huán)反饋控制

中圖分類號：TP242.2

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

開放科學(xué)（資源服務(wù)）標(biāo)識碼（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，F(xiàn)ujian，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

基金項(xiàng)目：國家自然科學(xué)基金（61703356）；福建省自然科學(xué)基金（2022J011256）

0? 引言

當(dāng)前，機(jī)器人視覺感知技術(shù)得到快速發(fā)展［1-2］，特別是基于深度學(xué)習(xí)的機(jī)器人任務(wù)操作備受研究人員關(guān)注［3-6］，代表性研究成果為Dex-Net［7］方法。該方法采用深度神經(jīng)網(wǎng)絡(luò)對目標(biāo)姿態(tài)進(jìn)行估計(jì)，機(jī)器人通過運(yùn)動規(guī)劃算法實(shí)施目標(biāo)抓取操作，即基于深度學(xué)習(xí)的機(jī)器人抓取操作采用開環(huán)控制方式，機(jī)器人視覺感知系統(tǒng)不具備位姿在線協(xié)調(diào)能力［8］。

針對機(jī)器人位姿在線自協(xié)調(diào)問題，基于位置的視覺伺服（position-based visual servoing，PBVS）和基于圖像的視覺伺服（image-based visual servoing，IBVS）是目前廣泛研究的控制方式。PBVS又稱3D視覺伺服，其反饋控制量為笛卡兒坐標(biāo)值。該方法通過相機(jī)投影模型估計(jì)機(jī)器人與目標(biāo)之間的相對位姿信息，控制系統(tǒng)根據(jù)位姿差分進(jìn)行軌跡規(guī)劃［9-10］。文獻(xiàn)［11］通過引入3D可視集概念，提出一種帶視場約束的全局穩(wěn)定PBVS控制方法。相比之下，IBVS方法通過視覺特征調(diào)節(jié)機(jī)器人姿態(tài)，利用圖像誤差計(jì)算機(jī)器人運(yùn)動控制量［12-14］，該方法因避免了目標(biāo)三維重建而深受研究人員青睞［15-17］。 如REDWAN等［14］提出改進(jìn)IBVS方法提升系統(tǒng)動態(tài)性能; WANG等［18］研究了深度獨(dú)立圖像雅可比矩陣問題，建立了圖像誤差與機(jī)器人關(guān)節(jié)之間的映射關(guān)系; LIANG等［19］研究了基于深度獨(dú)立圖像雅可比矩陣的移動機(jī)器人視覺伺服運(yùn)動控制方法; 而HAO等［20］將Broyden-Gauss-Newton、Broyden遞歸最小二乘法統(tǒng)一到狀態(tài)空間中，研究了基于狀態(tài)變量的視覺伺服方法。一般地，IBVS和PBVS方法依賴機(jī)器人系統(tǒng)標(biāo)定參數(shù)和視覺深度信息計(jì)算雅可比矩陣［21］。 在實(shí)際應(yīng)用中，系統(tǒng)標(biāo)定和視覺深度信息計(jì)算復(fù)雜，精度難以保證，直接影響控制系統(tǒng)的魯棒性能，此外，機(jī)器人按照固定的標(biāo)定模式工作難以應(yīng)對不同的工作環(huán)境， 因此，“手眼”未標(biāo)定視覺閉環(huán)控制方法繞開系統(tǒng)標(biāo)定和視覺深度信息的求解，有利于拓展機(jī)器人應(yīng)用場景［18-19］。

目前機(jī)器人視覺閉環(huán)控制已有廣泛研究，但是現(xiàn)實(shí)應(yīng)用還存在挑戰(zhàn)，容易產(chǎn)生以下問題： IBVS方法直接對圖像特征實(shí)施控制，機(jī)器人位姿易失控，導(dǎo)致機(jī)器人迂回運(yùn)動; PBVS方法直接對機(jī)器人位姿實(shí)施控制，目標(biāo)易脫離相機(jī)視場，導(dǎo)致圖像特征丟失。本文針對機(jī)器人位姿自協(xié)調(diào)控制，在未知“手眼”標(biāo)定參數(shù)和視覺深度信息條件下，提出一種基于二階錐約束規(guī)劃的視覺閉環(huán)控制方法，本文主要研究內(nèi)容包括：①將機(jī)器人位姿自協(xié)調(diào)視為約束規(guī)劃，在圖像平面和笛卡兒空間構(gòu)建基于二階錐約束規(guī)劃的混合閉環(huán)反饋控制框架；②建立機(jī)器人約束準(zhǔn)則，以二階錐凸優(yōu)化模型求解機(jī)器人最優(yōu)控制，有效避免圖像特征丟失，克服機(jī)器人迂回振蕩運(yùn)動；③提出的視覺閉環(huán)控制框架采用自適應(yīng)狀態(tài)估計(jì)器，實(shí)現(xiàn)了雅可比矩陣的在線學(xué)習(xí)，解決了相機(jī)標(biāo)定參數(shù)和視覺深度信息未知的問題。 最后通過機(jī)器人視覺定位實(shí)驗(yàn)，驗(yàn)證了圖像特征軌跡和機(jī)器人運(yùn)動路徑折中最優(yōu)控制。機(jī)器人目標(biāo)抓取實(shí)驗(yàn)說明了研究方法對機(jī)器人位姿協(xié)調(diào)的有效性。本文提出的帶有約束準(zhǔn)則的視覺閉環(huán)控制方法滿足無標(biāo)定機(jī)器人位姿自協(xié)調(diào)優(yōu)化控制，一定程度上體現(xiàn)了機(jī)器人的自主性。

1? 機(jī)器人位姿自協(xié)調(diào)控制問題描述

針對機(jī)器人位姿自協(xié)調(diào)，本文構(gòu)建了圖像反饋和位姿反饋控制方案。其中圖像閉環(huán)控制采用“圖像關(guān)節(jié)”映射描述當(dāng)前圖像特征s（t）（s（t）=（s1，s2，…，sm）T∈Rm）與機(jī)器人關(guān)節(jié)θ（t）（θ（t）=（θ1，θ2，…，θn）T∈Rn）之間的動態(tài)關(guān)系：s（t）θ（t），Rm→Rn。一般通過雅可比矩陣建立圖像特征s（t）與機(jī)器人關(guān)節(jié)θ（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為機(jī)器人關(guān)節(jié)維度。

假設(shè)機(jī)器人期望圖像特征為s*（t），s*（t）∈Rm，在圖像平面內(nèi)建立圖像反饋誤差es（t），則

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

考慮期望圖像特征s*（t）為常量，將式（3）對時(shí)間t求導(dǎo)并代入式（1），可得

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

由式（4）可建立圖像閉環(huán)反饋控制律如下：

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

式中，us（t）為圖像反饋控制器輸出；λ為控制系數(shù)；J+s（s，θ）為雅可比矩陣Js（s，θ）的Penrose廣義逆，當(dāng)Js（s，θ）列滿秩時(shí)，J+s=（JTsJs）－1JTs。

同理，機(jī)器人位姿閉環(huán)反饋控制采用“關(guān)節(jié)位姿”映射描述機(jī)器人關(guān)節(jié)θ（t）（θ（t）∈Rn）與位姿h（t）（h（t）=（h1，h2，…，hp）T∈Rp）之間的非線性關(guān)系：θ（t）h（t），Rn→Rp。通過雅可比矩陣建立關(guān)節(jié)與位姿之間的微分方程如下：

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

其中，Jh（h，θ）被稱為位姿雅可比矩陣，Jh（h，θ）∈Rp×n，表示為

Jh（h，θ）=hθ=

h1θ1h1θ2…h(huán)1θn

h2θ1h2θ2…h(huán)2θn

hpθ1hpθ2…h(huán)pθn（7）

假設(shè)機(jī)器人期望位姿為h*（t），（h*（t）∈Rp），在笛卡兒空間建立位姿反饋誤差eh（t），如下：

eh（t）=h*（t）－h(huán)（t）（8）

考慮期望位姿h*（t）為常量，將式（8）對時(shí)間t求導(dǎo)，并代入式（6），可得

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

由式（9）可建立位姿閉環(huán)反饋控制律如下：

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

式中，uh（t）為位姿反饋控制器輸出；β為控制系數(shù)；J+h為Jh的Penrose廣義逆，當(dāng)Jh列滿秩時(shí)，J+h=（JThJh）－1JTh。

2? 機(jī)器人約束規(guī)劃建模

在二維圖像平面和三維笛卡兒空間內(nèi)建立約束規(guī)則，構(gòu)建二階錐約束規(guī)劃模型和雅可比矩陣在線學(xué)習(xí)模型，提出方法框架如圖1所示。算法對圖像軌跡和機(jī)器人運(yùn)動路徑進(jìn)行有效規(guī)劃，折中優(yōu)化圖像特征軌跡和機(jī)器人運(yùn)動路徑，克服圖像特征丟失和機(jī)器人迂回問題，改善控制系統(tǒng)的動態(tài)性能。

2.1? 圖像特征軌跡約束

圖像特征軌跡約束的目標(biāo)處在相機(jī)視場范圍內(nèi)，同時(shí)約束特征點(diǎn)以最短路徑收斂至期望位置。為此，如圖2a所示，在2D圖像平面中，定義ds（t）為下一時(shí)刻圖像特征s（t+1）到當(dāng)前圖像特征s（t）與期望圖像特征s*（t）連線的距離，其計(jì)算式如下：

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）

進(jìn)一步可得

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）

根據(jù)圖像閉環(huán)反饋控制律（式（5）），在圖像平面中存在以下等式：

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

式中，T為采樣周期。

那么，式（15）可變?yōu)?/p>

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

由式（11）可知，若ds（t）→0，則s（t+1）處在s（t）與s*（t）的連線上，即圖像特征以直線路徑收斂至期望位置。

2.2? 機(jī)器人路徑約束

機(jī)器人路徑約束的約束末端操作器無回退運(yùn)動，同時(shí)約束機(jī)器人以最短路徑收斂至目標(biāo)位姿。 為此，如圖2b所示，在3D笛卡兒空間中定義dh（t）為下一時(shí)刻機(jī)器人位姿h（t+1）到當(dāng)前位姿h（t）與期望位姿h*（t）連線的距離，其計(jì)算式如下：

dh（t）=‖（h（t+1）－h(huán)*（t））×（h（t）－h(huán)*（t））‖‖h（t）－h(huán)*（t）‖（18）

由式（17）推導(dǎo)過程可知

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）的連線上，即機(jī)器人以直線最短路徑收斂至期望位姿。

（a）圖像平面

（b）機(jī)器人運(yùn)動空間

為使控制系統(tǒng)在圖像平面和笛卡兒空間同時(shí)滿足路徑最優(yōu)，建立如下目標(biāo)函數(shù)：

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

其中，f為權(quán)重因子，f∈［0，1］。若f=0，則系統(tǒng)為位姿閉環(huán)控制，機(jī)器人運(yùn)動路徑可達(dá)最優(yōu)；若f=1，則系統(tǒng)為圖像閉環(huán)控制，圖像特征軌跡可達(dá)最優(yōu)。

2.3? 局部極小約束

局部極小本質(zhì)上是圖像特征誤差es（t）和機(jī)器人位姿誤差eh（t）收斂不一致， 因此，為保證圖像誤差收斂，則有

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

式中，γ為圖像誤差收斂比例系數(shù)，γ∈（0，1）。

在圖像平面中，依據(jù)圖像閉環(huán)控制律，有

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

故可得圖像誤差遞減約束規(guī)則為

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

同理，笛卡兒空間機(jī)器人位姿誤差遞減約束規(guī)則為

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

式中，η為位姿誤差收斂比例系數(shù)，η∈（0，1）。

結(jié)合式（24）、式（25），即可同時(shí)確保圖像特征誤差與機(jī)器人位姿誤差一致性收斂，克服局部極小問題。

2.4? 物理約束

為保證目標(biāo)可視，下一時(shí)刻圖像特征應(yīng)滿足s≤s（t+1）≤s-，等價(jià)可得

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

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

其中，s、s-分別為圖像特征的下界和上界。

為避免機(jī)器人關(guān)節(jié)受限，關(guān)節(jié)角與關(guān)節(jié)速度應(yīng)滿足

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

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

式中，θ、θ-分別為機(jī)器人關(guān)節(jié)角下限和上限； θ·min、θ·max分別為關(guān)節(jié)速度最小值和最大值。

二階錐規(guī)劃模型是一種廣泛用于求解凸優(yōu)化問題的有效算法，是線性規(guī)劃和二次規(guī)劃的擴(kuò)展形式。二階錐規(guī)劃基本原理為：建立目標(biāo)函數(shù)min（fTx） ，設(shè)定錐體約束條件：

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

其中，優(yōu)化參數(shù)f∈Rn，Ai∈Rnin，bi∈Rni，ci∈Rn，di∈R，優(yōu)化變量x∈Rn。

規(guī)劃算法關(guān)鍵是利用二階錐約束線性化非線性函數(shù)，將原問題轉(zhuǎn)化為線性規(guī)劃問題。綜上約束準(zhǔn)則（式（24）～式（28））和線性目標(biāo)函數(shù)（式（21）），本文建立如下二階錐凸優(yōu)化模型：

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）為模型待優(yōu)化參數(shù)。

3? 雅可比矩陣估計(jì)模型

本文將凸優(yōu)化模型（式（29））中未知雅可比矩陣轉(zhuǎn)化為動態(tài)系統(tǒng)的狀態(tài)估計(jì)問題，首先將雅可比矩陣寫成如下狀態(tài)向量：

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

式中，jnm（t）為雅可比矩陣的第n行與第m列元素。

考慮如下一類動態(tài)方程：

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

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

其中，X（t）為系統(tǒng)狀態(tài)向量；Ψ（t+1，t）為狀態(tài)轉(zhuǎn)移矩陣，不失一般性，本文取單位矩陣；W（t）為系統(tǒng)過程噪聲；V（t）為觀測噪聲；Z（t）為觀測向量，定義為相鄰時(shí)刻圖像特征的變化量：

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

可得觀測矩陣H（t）為

H（t）=diag

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

假設(shè)系統(tǒng)噪聲W（t）和觀測噪聲V（t）為不相關(guān)白噪聲序列，協(xié)方差分別為Q（t）和R（t），對于離散系統(tǒng)（式（31）和式（32））狀態(tài)估計(jì)問題，可構(gòu)建卡爾曼濾波最小方差學(xué)習(xí)方法在線估計(jì)雅可比矩陣，算法如下：

初始化：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

雅可比矩陣在線學(xué)習(xí)算法主要包括狀態(tài)估計(jì)和觀測更新兩個(gè)基本過程，其中t|t表示t時(shí)刻狀態(tài)X（t）的最優(yōu)估計(jì)值；t+1|t表示下一時(shí)刻t+1的預(yù)測值；Pt|t表示估計(jì)誤差協(xié)方差矩陣；Pt+1|t表示預(yù)測誤差協(xié)方差矩陣；Kt為卡爾曼增益。在白噪聲不相關(guān)條件下，卡爾曼濾波遞推方程滿足最優(yōu)狀態(tài)估計(jì)，即E{X（t）}=t|t，此時(shí)可實(shí)現(xiàn)雅可比矩陣在線學(xué)習(xí)，即從系統(tǒng)狀態(tài)估計(jì)值中恢復(fù)雅可比矩陣t+1|t+1←t+1|t+1，雅可比矩陣在線估計(jì)流程如圖3所示。

4? 可行性分析

由誤差遞減約束規(guī)則（式（22））可得系統(tǒng)誤差（圖像誤差和位姿誤差）：

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

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

因此，假設(shè)e～（t1）≠0，易知，當(dāng)k→∞時(shí)，e～（tk）→0，即h（tk）→h*以及s（tk）→s*，即圖像軌跡和機(jī)器人運(yùn)動軌跡一致性收斂。

在規(guī)劃階段，若雅可比矩陣估計(jì)值滿秩可逆，則雅可比矩陣范數(shù)有界，即‖J～（tk）‖≤M，其中，J～（tk）=（Js（tk），Jh（tk）），此時(shí)存在系統(tǒng)誤差收斂性e～（tk）≡0，否則由系統(tǒng)一致性收斂可得

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

由式（36）可看出若系統(tǒng)誤差收斂至零，說明機(jī)器人期望位姿不存在局部極小。反之，如果系統(tǒng)誤差不等于零，則存在機(jī)器人運(yùn)動使下一時(shí)刻誤差收斂至零，所以凸優(yōu)化模型的解存在。

本文借鑒的二階錐凸優(yōu)化方法、卡爾曼濾波、IBVS和PBVS閉環(huán)控制的可行性在其他文獻(xiàn)已得到證明［1，22］。

5? 實(shí)驗(yàn)分析

如圖4所示，CCD相機(jī)安裝在六自由度機(jī)器人末端，構(gòu)建眼在手無標(biāo)定實(shí)驗(yàn)平臺。 機(jī)器人控制器作為下位機(jī)完成機(jī)器人運(yùn)動學(xué)運(yùn)算，同時(shí)驅(qū)動機(jī)器人各個(gè)關(guān)節(jié)運(yùn)動，上位機(jī)采用Intel Corei5 2.67GHz CPU，8GBs RAM個(gè)人計(jì)算機(jī)，用于圖像處理和執(zhí)行視覺閉環(huán)控制算法。 計(jì)算機(jī)與機(jī)器人控制器通過TCP/IP連接，構(gòu)成伺服控制系統(tǒng)。 實(shí)驗(yàn)過程確保圖像特征軌跡和機(jī)器人運(yùn)動軌跡最優(yōu)，優(yōu)化模型（式（29））權(quán)重因子f=0.5，收斂比例系數(shù)γ=η=0.75，系統(tǒng)采樣間隔設(shè)定T=0.5 s。

5.1? 性能驗(yàn)證

為驗(yàn)證提出的二階錐約束規(guī)劃視覺閉環(huán)控制方法的性能，采用基于圖像的視覺伺服IBVS和基于位置的視覺伺服PBVS作為基準(zhǔn)對比算法。如圖5a所示，本次對比實(shí)驗(yàn)在目標(biāo)板上打印四個(gè)圓點(diǎn)作為目標(biāo)點(diǎn)，為驗(yàn)證提出方法的約束性能，實(shí)驗(yàn)要求初始圖像特征點(diǎn)（圖5b）和期望圖像特征點(diǎn)（圖5c）盡可能靠近圖像邊緣。 提取目標(biāo)點(diǎn)在圖像平面中的坐標(biāo)值作為圖像特征s（t）=（s1，s2，s3，s4）T，其中，si為第i個(gè)圖像點(diǎn)，si=（ui，vi），此時(shí)圖像雅可比矩陣Js大小為8× 6。機(jī)器人位姿采用7維度表示方法（包含位置和四元素姿態(tài)），所以位姿雅可比矩陣Jh大小為7× 6，機(jī)器人在初始位置通過試探運(yùn)動對雅可比矩陣進(jìn)行初始化，得到結(jié)果如下（圖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）圖像特征軌跡比較。由實(shí)驗(yàn)結(jié)果圖6a可知，IBVS方法直接對圖像特征實(shí)施控制，初始特征點(diǎn)沿直線向期望特征點(diǎn)運(yùn)動，圖像特征運(yùn)動軌跡呈直線。由圖6c可知，PBVS方法圖像特征軌跡出現(xiàn)嚴(yán)重轉(zhuǎn)彎運(yùn)動，當(dāng)特征點(diǎn)處于圖像邊緣時(shí)特征丟失。而由圖6e可知，相同的視覺伺服控制任務(wù)，本文方法得到的圖像特征軌跡被約束在圖像平面內(nèi)，克服了特征點(diǎn)偏離相機(jī)視場，且避免了PBVS方法出現(xiàn)的轉(zhuǎn)彎運(yùn)動。因此，對比三種控制方法得到的圖像特征軌跡可知，PBVS方法的性能不如IBVS方法和本文方法的性能。

（2）機(jī)器人運(yùn)動路徑比較。由實(shí)驗(yàn)結(jié)果圖6b可知，IBVS方法機(jī)器人運(yùn)動路徑非常曲折，機(jī)器人在工作空間內(nèi)出現(xiàn)大范圍迂回運(yùn)動。對于相同的控制任務(wù)，圖6d顯示的PBVS方法直接對機(jī)器人位姿實(shí)施控制，機(jī)器人從初始位置收斂到期望位置，運(yùn)動路徑呈直線。圖6f顯示本文方法得到的機(jī)器人運(yùn)動路徑與PBVS相似，機(jī)器人受控制器約束從初始位置收斂到期望位置，運(yùn)動路徑近似直線，避免了迂回運(yùn)動。因此，對比三種方法得到的機(jī)器人運(yùn)動路徑可知，IBVS方法性能不如PBVS方法和本文方法的性能。

綜合上述結(jié)果，IBVS方法滿足圖像特征以最短距離向期望圖像運(yùn)動，但是機(jī)器人容易產(chǎn)生迂回現(xiàn)象。PBVS方法滿足機(jī)器人以最短路徑向期望位置運(yùn)動，但圖像軌跡出現(xiàn)大幅度轉(zhuǎn)彎，容易導(dǎo)致特征丟失。 而本文方法圖像軌跡接近IBVS方法，機(jī)器人運(yùn)動路徑接近PBVS方法，說明提出的規(guī)劃模型滿足圖像軌跡和機(jī)器人運(yùn)動路徑折中約束，兼顧IBVS和PBVS各自的優(yōu)點(diǎn)。

此外，PBVS和IBVS方法依賴相機(jī)標(biāo)定和目標(biāo)深度信息計(jì)算雅可比矩陣，而本文提出的狀態(tài)估計(jì)模型通過在線估計(jì)雅可比矩陣，繞開了系統(tǒng)標(biāo)定和目標(biāo)深度信息的計(jì)算。

5.2? 視覺定位實(shí)驗(yàn)

進(jìn)一步采用機(jī)器人視覺定位實(shí)驗(yàn)，驗(yàn)證二階錐約束規(guī)劃視覺閉環(huán)控制方法的可行性。 機(jī)器人定位實(shí)驗(yàn)選取Aruco圖8個(gè)角點(diǎn)作為定位特征，因此圖像特征向量s（t）=（s1，s2，…，s8）T，其中，si=（ui，vi）為第i個(gè)圖像點(diǎn)，此時(shí)圖像雅可比矩陣Js′大小為16× 6，初始值為Js′（0）=（Js（0），Js（0））T，其中Js（0）見式（37），位姿雅可比矩陣Jh大小為7× 6，初始值與式（38）相同。

圖7a為機(jī)器人視覺定位場景圖; 圖7b所示為機(jī)器人初始位姿相機(jī)視角，可見機(jī)器人夾爪與目標(biāo)物相距比較遠(yuǎn)，此時(shí)檢測到的8個(gè)圖像角點(diǎn)較小; 圖7c 所示為機(jī)器人期望位姿相機(jī)視角，可見夾爪到達(dá)目標(biāo)位置，此時(shí)圖像角點(diǎn)位于圖像平面中間位置。

5.2.1? 定位實(shí)驗(yàn)1

圖8所示為機(jī)器人平移運(yùn)動定位實(shí)驗(yàn)結(jié)果，由圖8a可知，特征點(diǎn)從初始位置沿直線向期望位置運(yùn)動，盡管特征點(diǎn)處在圖像邊緣，但圖像特征被控制器約束在相機(jī)視場范圍內(nèi)，避免特征丟失現(xiàn)象的發(fā)生。

圖8b所示為機(jī)器人三維笛卡兒空間運(yùn)動路徑，可見機(jī)器人從初始位置沿直線向期望位置運(yùn)動，機(jī)器人運(yùn)動路徑被控制器約束在工作空間內(nèi)，避免了迂回和關(guān)節(jié)沖突現(xiàn)象的發(fā)生。 圖8c所示為圖像特征誤差，可看出圖像誤差一致漸近收斂，控制系統(tǒng)未出現(xiàn)局部極小現(xiàn)象，機(jī)器人成功完成定位任務(wù)。

5.2.2? 定位實(shí)驗(yàn)2

圖9所示為機(jī)器人平移和旋轉(zhuǎn)定位實(shí)驗(yàn)結(jié)果，其中圖 9a表明圖像特征軌跡沿直線移動，特征點(diǎn)被約束在相機(jī)視場內(nèi)，避免了特征丟失。圖9b顯示機(jī)器人從初始位置沿著直線向期望位置運(yùn)動，機(jī)器人未出現(xiàn)振蕩或回退。 圖9c說明圖像特征誤差一致漸近收斂，未出現(xiàn)局部極小現(xiàn)象。

由以上兩組不同情形視覺定位實(shí)驗(yàn)可以看出，提出的二階錐規(guī)劃控制模型能同時(shí)約束圖像特征軌跡和機(jī)器人運(yùn)動路徑，有效應(yīng)對機(jī)器人平移和旋轉(zhuǎn)運(yùn)動控制任務(wù)。

5.3? 機(jī)器人應(yīng)用實(shí)驗(yàn)

本節(jié)通過機(jī)器人目標(biāo)抓取實(shí)驗(yàn)驗(yàn)證本文方法的實(shí)用性。抓取目標(biāo)隨意放置在機(jī)器人工作臺上，實(shí)驗(yàn)要求機(jī)器人通過位姿在線協(xié)調(diào)自主完成抓取任務(wù)。 如圖10所示，提取物體8個(gè)角點(diǎn)作為圖像特征向量s（t），s（t）=（s1，s2，…，s8）T，其中si=（ui，vi）為第i個(gè)角點(diǎn)，此時(shí)圖像雅可比矩陣Js大小為16× 6，位姿雅可比矩陣Jh大小為7× 6，雅可比矩陣初始值與上述定位實(shí)驗(yàn)相同。圖10a為機(jī)器人初始位置相機(jī)視角;圖10b為機(jī)器人期望抓取位置的相機(jī)視角，可見本次實(shí)驗(yàn)特征點(diǎn)弱小極容易跟蹤丟失，對本文控制模型是一種挑戰(zhàn)。

（a）初始位置相機(jī)視角? ????（b） 抓取位置相機(jī)視角

圖11所示為機(jī)器人抓取姿態(tài)自協(xié)調(diào)實(shí)驗(yàn)結(jié)果，由圖11a可知，圖像平面特征點(diǎn)從初始位置以最佳路徑到達(dá)期望位置，圖像特征運(yùn)動軌跡近似于直線。 圖11b所示為圖像特征誤差，可見圖像誤差逐漸收斂，最終一致性收斂于零點(diǎn)，說明控制系統(tǒng)能夠有效克服局部極小問題。圖11c為機(jī)械臂關(guān)節(jié)速度曲線，可看出關(guān)節(jié)速度被約束在一定范圍內(nèi)，未發(fā)生關(guān)節(jié)受限。 圖11d所示為機(jī)器人三維空間抓取運(yùn)動路徑，圖中Start為末端操作器起始位姿，End為末端操作器抓取目標(biāo)時(shí)的位姿，可見機(jī)器人末端運(yùn)動軌跡呈直線，未發(fā)生迂回運(yùn)動，機(jī)器人抓取姿態(tài)自協(xié)調(diào)控制效果良好，機(jī)器人成功完成抓取任務(wù)。

抓取實(shí)驗(yàn)采用弱小角點(diǎn)作為伺服控制器的輸入特征，若采用IBVS方法，機(jī)器人運(yùn)動容易失控，其關(guān)節(jié)速度容易突變，機(jī)器人因振蕩運(yùn)動造成抓取任務(wù)失敗; 而采用PBVS方法，難以保證所有圖像特征點(diǎn)在相機(jī)視場范圍內(nèi)，導(dǎo)致抓取任務(wù)難以完成。 而本文提出的二階錐約束規(guī)劃視覺閉環(huán)控制方法對圖像特征和機(jī)器人運(yùn)動實(shí)施恰當(dāng)?shù)募s束，有效克服了上述伺服控制問題。

圖12展示了機(jī)械臂通過位姿自協(xié)調(diào)平穩(wěn)地將目標(biāo)抓起，并放置在工作臺右下方的過程，可見本文提出方法對機(jī)器人位姿自協(xié)調(diào)控制真實(shí)有效。

6? 結(jié)論

在未知“手眼”標(biāo)定參數(shù)和視覺深度信息情況下，本文提出了一種二階錐約束規(guī)劃的機(jī)器人視覺閉環(huán)位姿自協(xié)調(diào)方法。在圖像平面和笛卡兒空間建立圖像與位姿閉環(huán)反饋控制框架，結(jié)合路徑約束和局部極小約束規(guī)則，構(gòu)建了二階錐凸優(yōu)化模型，

實(shí)現(xiàn)了圖像特征軌跡和機(jī)器人運(yùn)動路徑折中最優(yōu)，克服了特征點(diǎn)偏離相機(jī)視場和機(jī)器人回退問題，提出的視覺閉環(huán)控制方法對系統(tǒng)標(biāo)定參數(shù)和視覺深度信息具有自適應(yīng)性。通過機(jī)器人視覺定位與目標(biāo)抓取實(shí)驗(yàn)，驗(yàn)證了本文提出方法對機(jī)器人位姿自協(xié)調(diào)控制的有效性，實(shí)驗(yàn)結(jié)果表明傳統(tǒng)IBVS方法圖像穩(wěn)態(tài)誤差約為43 pixel，PBVS方法圖像穩(wěn)態(tài)誤差約為65 pixel，而本文算法圖像穩(wěn)態(tài)誤差約為25 pixel，是一種保性能的視覺閉環(huán)控制方法。

參考文獻(xiàn)：

［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］? 仲訓(xùn)杲， 徐敏， 仲訓(xùn)昱， 等. 基于多模特征深度學(xué)習(xí)的機(jī)器人抓取判別方法［J］. 自動化學(xué)報(bào)， 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ī)器人運(yùn)動軌跡的模仿學(xué)習(xí)綜述［J］. 自動化學(xué)報(bào)， 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ī)械手系統(tǒng)協(xié)調(diào)操作的視覺伺服技術(shù)［J］. 控制理論與應(yīng)用， 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三焦點(diǎn)張量的移動機(jī)器人視覺伺服鎮(zhèn)定控制［J］. 自動化學(xué)報(bào)， 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ī)械臂水下自適應(yīng)魯棒視覺伺服［J］. 自動化學(xué)報(bào)， 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.

（編輯? 王艷麗）

作者簡介：

仲訓(xùn)杲，男，1983年生，副教授。研究方向?yàn)闄C(jī)器人視覺伺服、機(jī)器人智能化等。E-mail：zhongxungao@163.com。

仲訓(xùn)昱（通信作者），男，1980年生，副教授、博士研究生導(dǎo)師。研究方向?yàn)闊o人系統(tǒng)的智能感知、導(dǎo)航定位與決策規(guī)劃等。E-mail：zhongxunyu@xmu.edu.cn。