模型未知?jiǎng)討B(tài)場(chǎng)景下移動(dòng)機(jī)器人的視覺鎮(zhèn)定

李寶全，尹成浩，師五喜

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

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

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

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

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

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

1 問(wèn)題描述

1.1 系統(tǒng)描述

控制系統(tǒng)由移動(dò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 動(dòng)態(tài)場(chǎng)景下視覺伺服策略圖Fig.2 Block diagram of visual servoing strategy under dynam ic environment

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

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

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

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

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

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

1.4 圖像表示

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

2 控制策略

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

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

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

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

通過(guò)對(duì)期望圖像和圖像1得到的單應(yīng)矩陣分解[22]，可以得到監(jiān)視相機(jī)與期望位姿之間的相對(duì)關(guān)系。由于可以分解出8種不同的結(jié)果，經(jīng)過(guò)算法篩選之后可以得到2個(gè)有效解，再根據(jù)實(shí)際的物理模型確定用哪一組解。最終得到的旋轉(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到移動(dòng)后特征點(diǎn)平面的未知深度。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 仿真及實(shí)驗(yàn)結(jié)果

4.1 仿真結(jié)果

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

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

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

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

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

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

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

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

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

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

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

4.2 實(shí)驗(yàn)結(jié)果

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

圖8 實(shí)驗(yàn)場(chǎng)景Fig.8 Experiments scene

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

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

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

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

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

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

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

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

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

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

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

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

圖13 機(jī)器人移動(dòng)軌跡的實(shí)驗(yàn)結(jié)果Fig.13 Experimental results of motion path of mobile robot

5 結(jié)論

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

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

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