李寶全，尹成浩，師五喜

（天津工業(yè)大學(xué) 電氣工程與自動化學(xué)院，天津 300387）

視覺作為人類獲取信息的重要感官之一，人類每天從視覺圖像中獲得大量的信息。視覺傳感器是機(jī)器人的重要傳感器，再加上其非接觸式測量、成本低等特點(diǎn)，應(yīng)用范圍越來越廣泛[1]。最近幾十年對于移動機(jī)器人的研究有了前所未有的發(fā)展，比如：宇宙探索、深?？碧揭约凹彝シ?wù)等[2]。然而，視覺伺服技術(shù)在實際應(yīng)用中還有許多問題亟待解決。對應(yīng)現(xiàn)實中都是三維場景，投影到攝像機(jī)中成為了二維圖像，這就導(dǎo)致了深度信息的缺失[3]。因此，對于單目移動機(jī)器人來說，重建3D場景是有困難的。此外，輪式移動機(jī)器人是一個典型的具有非完整性約束的欠驅(qū)動系統(tǒng)[4]。如Brockett[5]所述，任何連續(xù)不變狀態(tài)反饋控制器都無法實現(xiàn)對非完整系統(tǒng)的穩(wěn)定控制。為了處理姿態(tài)調(diào)節(jié)中非完整約束，文獻(xiàn)[6]設(shè)計了平滑的時變控制器來控制機(jī)器人移動到期望位姿處。以上這些限制使視覺鎮(zhèn)定控制成為控制領(lǐng)域比較困難的任務(wù)之一，在動態(tài)場景下完成鎮(zhèn)定控制變得更具挑戰(zhàn)性。

在過去幾年，許多研究人員關(guān)注了機(jī)器人的視覺伺服問題，并且取得了豐碩的成果。傳統(tǒng)的視覺伺服方案一般是為6自由度的機(jī)械臂設(shè)計的，比如在文獻(xiàn)[7]中，利用整個圖像的光度信息來計算交互矩陣，使得該方法對遮擋和高光場景具有魯棒性。在文獻(xiàn)[8]中，根據(jù)從中央折反射系統(tǒng)圖像平面中獲得的直線投影控制一個6自由度機(jī)械臂。在特征點(diǎn)固定不移動的情況下，上述方法對于機(jī)器人機(jī)械臂的控制是有效的。其他一些控制方法，比如基于圖像矩、圖像亮度、整幅圖像以及直線等特征，仍需要在視覺特征靜止的情況下完成工作。如果在動態(tài)場景中移動機(jī)器人視覺系統(tǒng)仍能完成鎮(zhèn)定控制，它們會變得更加智能并能更好地適應(yīng)環(huán)境變化。因此，需要設(shè)計一種新的策略以解決動態(tài)場景下的視覺鎮(zhèn)定控制問題。

與機(jī)械臂相比，安裝有視覺傳感器的移動機(jī)器人系統(tǒng)，其應(yīng)用領(lǐng)域變得越來越廣泛。當(dāng)前，移動機(jī)器人視覺伺服實現(xiàn)的主要功能是：利用實時的視覺信號進(jìn)行反饋以控制移動機(jī)器人的運(yùn)動，最終使機(jī)器人到達(dá)指定的位姿處或跟蹤設(shè)定的軌跡。輪式移動機(jī)器人本身固有的非完整性限制，使得控制器的設(shè)計相比機(jī)械臂更加困難。此外，為了將視覺目標(biāo)保持在相機(jī)視野中，文獻(xiàn)[9]提出了一種主動視覺的方法。在文獻(xiàn)[10]中，通過設(shè)計非線性觀測器來檢測相機(jī)的運(yùn)動范圍信息。移動機(jī)器人的視覺鎮(zhèn)定控制一般利用固定的特征點(diǎn)的當(dāng)前圖像和期望圖像之間的誤差來獲得實時的位姿。如果特征點(diǎn)變動，那么將導(dǎo)致鎮(zhèn)定控制任務(wù)失敗。

盡管當(dāng)移動機(jī)器人在執(zhí)行軌跡跟蹤任務(wù)時[11]，特征點(diǎn)是動態(tài)的，但這與鎮(zhèn)定控制不是一個問題。比如，Liu等[12]設(shè)計了一種快速篩選異常值的方法并且利用未校準(zhǔn)的全景攝像機(jī)來完成視覺回歸任務(wù)。文獻(xiàn)[13]提出了一種基于自適應(yīng)的視覺伺服跟蹤控制的方法。雖然特征點(diǎn)在動態(tài)環(huán)境中，但這些是屬于跟蹤控制，不能用于鎮(zhèn)定控制。為了處理鎮(zhèn)定控制中未知的深度信息，視覺伺服系統(tǒng)通常引入自適應(yīng)補(bǔ)償機(jī)制[14]。Fang等[15]利用深度信息未知并且模型未知的特征點(diǎn)，通過單應(yīng)矩陣獲得位姿誤差，基于Lyapunov穩(wěn)定原理設(shè)計了自適應(yīng)線性控制器，最終解決了視覺伺服鎮(zhèn)定問題[16]。Zhang等[17]提出了一種單應(yīng)矩陣快速分解獲到含有未知比例因子的機(jī)器人位姿的方法。但這些鎮(zhèn)定控制任務(wù)是在特征點(diǎn)保持固定不變的情況下進(jìn)行的。

本文基于單目輪式移動機(jī)器人，設(shè)計了一種可在動態(tài)場景中利用模型未知的特征點(diǎn)完成視覺鎮(zhèn)定控制的策略。本方案大致有分為以下步驟：首先，在實驗場景中引入一個固定的監(jiān)視攝像機(jī)，通過獲取的信息可以得到不同場景下深度信息的比例；然后，通過包含未知特征深度的信息獲得極坐標(biāo)表示下的運(yùn)動學(xué)模型，再利用單應(yīng)矩陣分解和坐標(biāo)系變換來計算移動機(jī)器人當(dāng)前姿態(tài)與期望姿態(tài)之間實時的相對關(guān)系；最后，采用自適應(yīng)控制器驅(qū)動機(jī)器人到期望位姿處。

1 問題描述

1.1 系統(tǒng)描述

控制系統(tǒng)由移動機(jī)器人和車載攝像機(jī)組成。視覺伺服任務(wù)的坐標(biāo)系關(guān)系如圖1所示。

圖1 視覺伺服任務(wù)的坐標(biāo)系關(guān)系Fig.1 Coordinate systems relationship of the visual servoing task

1.2 控制方案

圖2所示為視覺伺服策略框圖。

圖2 動態(tài)場景下視覺伺服策略圖Fig.2 Block diagram of visual servoing strategy under dynam ic environment

圖2中：圖像1表示特征點(diǎn)移動前在監(jiān)視相機(jī)中的圖像；圖像2表示特征點(diǎn)移動后在監(jiān)視相機(jī)中的圖像。整個視覺伺服策略為：

（1）期望圖像是將移動機(jī)器人事先放到期望位姿處獲得，它與圖像1可以估計出第1個單應(yīng)矩陣。因此利用六自由度的單應(yīng)矩陣分解[22]，可得到監(jiān)視攝相機(jī)位姿與期望位姿之間旋轉(zhuǎn)矩陣和平移向量

（2）在視覺伺服過程中，針對移動后的特征點(diǎn)，利用車載視覺系統(tǒng)獲得的當(dāng)前圖像與圖像2結(jié)合可以得到第2個單應(yīng)矩陣。與第一步類似，可以得到機(jī)器人當(dāng)前坐標(biāo)系與監(jiān)視相機(jī)坐標(biāo)系之間旋轉(zhuǎn)矩陣和平移向量

（3）無論特征點(diǎn)移動前還是移動后，場景的深度信息均為未知，并且特征點(diǎn)移動前后場景深度信息不相同。考慮到這一點(diǎn)，文中利用特征點(diǎn)與視覺圖像之間的幾何關(guān)系，計算出該2個不同深度信息之間的比例λ。

（4）根據(jù)坐標(biāo)系變換規(guī)則，實時地計算出機(jī)器人當(dāng)前位姿與期望位姿之間的旋轉(zhuǎn)矩陣和平移向量進(jìn)而采用自適應(yīng)控制器將輪式移動機(jī)器人驅(qū)動到期望位姿處。

1.3 坐標(biāo)系關(guān)系的表示

1.4 圖像表示

在空間中有4個共面的動態(tài)特征點(diǎn)Mi（i=1，2，3，4），并且表示移動之后的特征點(diǎn)。期望圖像、當(dāng)前圖像、圖像1和圖像2的第i個像素點(diǎn)的表示分別是，具體表示如下：

2 控制策略

本節(jié)計算出輪式移動機(jī)器人的期望位姿與當(dāng)前位姿之間實時相對關(guān)系。

2.1 監(jiān)視相機(jī)與期望位姿的關(guān)系

式中：h（it）是一個與深度比有關(guān)的系數(shù)表示單應(yīng)矩陣表示如下攝像機(jī)內(nèi)參數(shù)矩陣：

式中：（u0，v0）表示圖像的主點(diǎn)；（fu，fv）為用像素塊表示的焦距。射影單應(yīng)矩陣G（t）可以通過最小二乘法獲得。因此，單應(yīng)矩陣得到如下：

通過對期望圖像和圖像1得到的單應(yīng)矩陣分解[22]，可以得到監(jiān)視相機(jī)與期望位姿之間的相對關(guān)系。由于可以分解出8種不同的結(jié)果，經(jīng)過算法篩選之后可以得到2個有效解，再根據(jù)實際的物理模型確定用哪一組解。最終得到的旋轉(zhuǎn)矩陣和平移向量如式（6）所示：

式中：dm表示從坐標(biāo)系m的原點(diǎn)沿平面的法向量nm到特征點(diǎn)平面的未知深度。

2.2 監(jiān)視相機(jī)與當(dāng)前位姿的關(guān)系

式中：dma表示從坐標(biāo)系m的原點(diǎn)沿平面的法向量nma到移動后特征點(diǎn)平面的未知深度。

2.3 兩個未知深度信息之間的比例因子

圖3 深度信息dmFig.3 Diagram show ing the distance dm

當(dāng)特征點(diǎn)移動后，特征點(diǎn)和深度信息也發(fā)生了相應(yīng)的改變。因此變成了同樣地，可以表示如下：

由式（8）和式（9）可以計算出不同深度信息之間的比例：

2.4 當(dāng)前位姿與期望位姿的關(guān)系

經(jīng)過以上計算，監(jiān)視相機(jī)位姿與機(jī)器人期望位姿之間的關(guān)系可以表示如下：

移動機(jī)器人的當(dāng)前位姿與監(jiān)視相機(jī)位姿之間的相對關(guān)系為：

因此通過坐標(biāo)系變換可以得到實時的移動機(jī)器人當(dāng)前位姿與期望位姿之間關(guān)系：

3 驅(qū)動機(jī)器人到期望位姿

為了使控制器的設(shè)計更加方便，將機(jī)器人在笛卡爾坐標(biāo)系下的位姿轉(zhuǎn)換成極坐標(biāo)下的表示[20]。將的范數(shù)定義為當(dāng)前位姿與期望位姿的距離誤差e（t），表示為由于深度信息dm和dma是未知的，所以e（t）是不可以直接測量出來。因此，將含有比例因子的可測距離誤差e（st）定義為：

角度 φ（t）和 α（t）可以表示為：

3.1 運(yùn)動學(xué)模型

基于極坐標(biāo)的機(jī)器人運(yùn)動學(xué)模型表示為

根據(jù)系統(tǒng)需要將其重新定義為：

3.2 自適應(yīng)控制器

由于深度信息是未知的，可以通過采用一個自適應(yīng)控制器[18]來完成鎮(zhèn)定控制任務(wù)。基于Lyapunov穩(wěn)定原理，通過利用可獲得的信號設(shè)計機(jī)器人的線速度和角速度為：

4 仿真及實驗結(jié)果

4.1 仿真結(jié)果

為驗證所提方案的可行性，首先在MATLAB上搭建模型進(jìn)行仿真實驗。在仿真場景中設(shè)置了4個共面特征點(diǎn)，并且設(shè)置了移動后的特征點(diǎn)。仿真中的虛擬相機(jī)內(nèi)參數(shù)設(shè)置為：

圖4為機(jī)器人的位姿變化的仿真結(jié)果。

圖4 機(jī)器人的位姿變化的仿真結(jié)果Fig.4 Simulation results of evolution of robot pose

圖5為移動機(jī)器人實時的線速度和角速度的仿真結(jié)果。

圖5 機(jī)器人的線速度和角速度的仿真結(jié)果Fig.5 Simulation results of linear velocity and angular velocity of mobile robot

由圖5可見，隨著時間的變化，線速度和角速度都趨于零，說明機(jī)器人鎮(zhèn)定到期望位姿時，可以停止運(yùn)動。

圖6為特征點(diǎn)圖像軌跡圖。

圖6 特征點(diǎn)圖像軌跡圖的仿真結(jié)果Fig.6 Simulation results of image trajectories of features

圖6中顯示的是特征點(diǎn)在圖像中的變化軌跡，圖中的圓點(diǎn)表示初始位姿時刻的特征點(diǎn)圖像坐標(biāo)，正方形表示的是機(jī)器人到達(dá)期望位姿時特征點(diǎn)在圖像中的位置，星形是特征點(diǎn)移動前在期望位姿處的圖像位置。由于特征點(diǎn)的移動，所以星形沒有與正方形重合。

圖7為機(jī)器人在三維空間中的移動路徑的仿真結(jié)果。

圖7 機(jī)器人移動軌跡的仿真結(jié)果Fig.7 Simulation results of mobile robot motion trajectory

4.2 實驗結(jié)果

經(jīng)過仿真之后，進(jìn)行實驗以進(jìn)一步驗證所提方法的有效性，實驗場景如圖8所示。

圖8 實驗場景Fig.8 Experiments scene

圖8中，實驗用的是帶有車載攝像機(jī)的IN-R型移動機(jī)器人，綠色棋盤格面板為共面特征點(diǎn)，左下角為固定的監(jiān)視相機(jī)。本文方法是在配備有OpenCV庫的VC++6.0環(huán)境下實現(xiàn)的。

以期望位姿作為參考坐標(biāo)系，將初始位姿隨機(jī)設(shè)置在-2.2 m，0.6 m，32°，所以期望位姿為0.0 m，0.0 m，0°。選擇的控制增益參數(shù)為

首先將移動機(jī)器人放置到期望位姿采集期望圖像，如圖8所示的機(jī)器人進(jìn)行采集圖像，然后將機(jī)器人隨機(jī)放置到其他位置（初始位姿），與此同時特征點(diǎn)做了相應(yīng)的移動（模擬特征點(diǎn)受到外界較大干擾）。

圖9為監(jiān)視相機(jī)捕獲的圖。圖9（a）是移動機(jī)器人在期望位姿時，監(jiān)視攝像機(jī)捕獲的未發(fā)生變化的特征點(diǎn)圖像，圖9（b）是移動機(jī)器人在初始位姿處時，監(jiān)視攝像機(jī)捕獲的發(fā)生變化的特征點(diǎn)圖像。

圖9 監(jiān)視相機(jī)捕獲圖Fig.9 Images of feature points in monitor camera

圖10為特征點(diǎn)在攝像機(jī)中的圖像變化。由圖10可見圖形的表示方法與仿真中敘述的一致。移動機(jī)器人的實時的變化如圖11所示。

圖10 特征點(diǎn)圖像軌跡圖的實驗結(jié)果Fig.10 Experimental results of image trajectories of features

圖11 機(jī)器人的位姿變化的實驗結(jié)果Fig.11 Experimental results of evolution of the robot pose

圖11中，紅色虛線是期望值，藍(lán)色線為實際的值，可以看出3條軌跡逐漸趨于期望值。

圖12為試驗中移動機(jī)器人的線速度和角速度變化，由圖12可見，線速度和角速度最終都趨于零。

圖12 機(jī)器人的線速度和角速度的實驗結(jié)果Fig.12 Experimental results of linear velocity and angular velocity of mobile robot

圖13為移動機(jī)器人從初始位姿到期望位姿的路徑。圖中，紅三角是期望位姿處，可見機(jī)器人最終鎮(zhèn)定到了期望位姿處。實驗表明所提出方法具有可行性。

圖13 機(jī)器人移動軌跡的實驗結(jié)果Fig.13 Experimental results of motion path of mobile robot

5 結(jié)論

本文針對移動機(jī)器人在鎮(zhèn)定控制中特征點(diǎn)不能移動的問題，提出了一種處理動態(tài)場景下的視覺伺服策略，并且場景中特征點(diǎn)的3D模型是未知的。首先，利用單應(yīng)矩陣分解方法計算出期望位姿與監(jiān)視相機(jī)之間的關(guān)系；然后利用同樣的方法計算出當(dāng)前位姿與監(jiān)視相機(jī)之間的關(guān)系。由于場景的變化導(dǎo)致深度信息的不同，所以需要得到兩者之間的比例，以用來將平移向量轉(zhuǎn)變成含有同一個深度信息；再結(jié)合坐標(biāo)系變換算出實時的當(dāng)前位姿與期望位姿之間的關(guān)系；最后，采用一個自適應(yīng)控制器將機(jī)器人驅(qū)動到期望位姿處。仿真和實驗結(jié)果表明：環(huán)境發(fā)生變化時，移動機(jī)器人可以完成鎮(zhèn)定控制，即隨著時間變化，在位姿誤差e→0的同時，線速v→0和角速度w→0，此時機(jī)器人鎮(zhèn)定到期望位姿處。通過與傳統(tǒng)鎮(zhèn)定方法的仿真和實驗對比研究發(fā)現(xiàn)：

（1）傳統(tǒng)的視覺鎮(zhèn)定控制一般利用固定的特征點(diǎn)完成鎮(zhèn)定任務(wù)。

（2）本文所使用的方法，在場景深度信息未知，特征點(diǎn)3D模型未知的情況下完成動態(tài)場景下的視覺伺服鎮(zhèn)定控制，從而提高了移動機(jī)器人環(huán)境適應(yīng)的能力。