劉江濤，陳奕梅

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

0 引 言

視覺(jué)傳感器相較于其它傳感器視覺(jué)傳感器可以獲得更為豐富的環(huán)境信息，隨著圖像處理與計(jì)算機(jī)視覺(jué)技術(shù)的發(fā)展，視覺(jué)傳感器越來(lái)越多的被應(yīng)用到機(jī)器人運(yùn)動(dòng)控制領(lǐng)域，從而進(jìn)一步加強(qiáng)了機(jī)器人的靈活性并擴(kuò)展了機(jī)器人的工作空間[1-5]。文獻(xiàn)[6]對(duì)視覺(jué)伺服系統(tǒng)的最新研究成果進(jìn)行了詳細(xì)的介紹，其中對(duì)于移動(dòng)機(jī)器人上搭載單目攝像機(jī)的視覺(jué)伺服控制系統(tǒng)的研究，主要集中在提高視覺(jué)系統(tǒng)對(duì)圖像噪聲、特征誤差匹配等因素的魯棒性和對(duì)移動(dòng)機(jī)器人運(yùn)動(dòng)控制的實(shí)時(shí)性、靈活性的進(jìn)一步改善。文獻(xiàn)[7]中提出了一種基于單應(yīng)性矩陣的切換控制算法，實(shí)現(xiàn)了移動(dòng)機(jī)器人的視覺(jué)伺服鎮(zhèn)定。Mezouar等提出基于圖像的輪式移動(dòng)機(jī)器人視覺(jué)路徑控制方案，視覺(jué)伺服方面采用基于幾何約束的信息反饋：對(duì)極幾何或三焦張量[7]。

本文采用的2DTT即二維三焦點(diǎn)張量，2DTT視覺(jué)信息采集系統(tǒng)與單應(yīng)矩陣相比能夠充分利用二維圖像中的信息，從而獲得機(jī)器人在視覺(jué)場(chǎng)景中的估計(jì)值與真實(shí)值之間的固定比例關(guān)系，進(jìn)而可以得到機(jī)器人在運(yùn)動(dòng)中的唯一的相對(duì)位姿關(guān)系。文獻(xiàn)[9]在采用2DTT估計(jì)機(jī)器人實(shí)時(shí)位姿的基礎(chǔ)上采用了一種反饋線性化控制器以實(shí)現(xiàn)機(jī)器人的位置鎮(zhèn)定，但對(duì)于2DTT中的未知深度問(wèn)題并未實(shí)現(xiàn)自適應(yīng)控制。

文獻(xiàn)[10]中采用單應(yīng)矩陣實(shí)現(xiàn)了移動(dòng)機(jī)器人的自適應(yīng)伺服鎮(zhèn)定，本文在文獻(xiàn)自適應(yīng)伺服鎮(zhèn)定的基礎(chǔ)上采用了2DTT來(lái)進(jìn)行視覺(jué)信息的采集。文獻(xiàn)[11]提出了通過(guò)滑模算法使移動(dòng)機(jī)器人的速度趨于期望值而實(shí)現(xiàn)軌跡跟蹤。考慮到在機(jī)器人視覺(jué)伺服控制的過(guò)程中存在由于期望速度與機(jī)器人實(shí)際運(yùn)動(dòng)速度差值過(guò)大，引起的抖動(dòng)問(wèn)題，本文采用將自適應(yīng)控制鎮(zhèn)定器得到的運(yùn)動(dòng)路徑作為虛擬軌跡，采用滑模速度跟蹤控制來(lái)使機(jī)器人的實(shí)際運(yùn)動(dòng)速度趨于期望速度，來(lái)解決機(jī)器人鎮(zhèn)定過(guò)程中的抖動(dòng)問(wèn)題。

1 視覺(jué)場(chǎng)景描述

1.1 視覺(jué)場(chǎng)景中機(jī)器人的坐標(biāo)系關(guān)系

圖1中給出了機(jī)器人視覺(jué)伺服過(guò)程中的坐標(biāo)關(guān)系描述，如圖Di，Dr，De分別表示機(jī)器人的初始位姿，當(dāng)前位姿，期望位姿坐標(biāo)(本文中將攝像機(jī)坐標(biāo)系與機(jī)器人做重合處理)。坐標(biāo)De中ze坐標(biāo)軸為機(jī)器人前進(jìn)方向，xe坐標(biāo)軸為機(jī)器人輪軸方向，ye軸垂直地面向下(對(duì)Di，Dr坐標(biāo)系的描述同上)。β為zi軸與ze軸的夾角，φ為zr軸與ze軸夾角。Pi為特征點(diǎn)中的第i個(gè)點(diǎn)。(注：本文中的坐標(biāo)定義符合右手定則，β，φ取逆時(shí)針?lè)较驗(yàn)檎?

根據(jù)上述場(chǎng)景描述下可以得出從De到Dr的旋轉(zhuǎn)矩陣為Rre與平移向量tre，從De到Di的旋轉(zhuǎn)矩陣為Rie與平移向量tie

(1)

(2)

其中，sφ、cφ分別為sinφ、cosφ的簡(jiǎn)寫(xiě)，sβ、cβ分別為sinβ、cosβ的簡(jiǎn)寫(xiě)。

以De為參考坐標(biāo)系得出De,Dr,Di對(duì)應(yīng)的攝像機(jī)矩陣分別為Qe,Qr,Qi[8]

(3)

1.2 視覺(jué)系統(tǒng)測(cè)取的信息分析

對(duì)于特征點(diǎn)Pi在坐標(biāo)系De,Dr,Di中的真實(shí)位置坐標(biāo)為分別為Pei,Pri,Pii，特征點(diǎn)Pi在坐標(biāo)系De,Dr,Di中攝像機(jī)采集到的圖像上的坐標(biāo)為分別為pei,pri,pii。

由攝像機(jī)的成像原理可以得到，在同一參考坐標(biāo)系下，特征點(diǎn)的真實(shí)坐標(biāo)Pi與攝像機(jī)采集到的圖像坐標(biāo)pi有如下關(guān)系

(4)

其中，K為攝像機(jī)的內(nèi)參數(shù)矩陣，zi為特征點(diǎn)在參考坐標(biāo)系下前向軸的真實(shí)坐標(biāo)。

則可以得到Pi在參考坐標(biāo)系下的歸一化坐標(biāo)

(5)

1.3 機(jī)器人的運(yùn)動(dòng)學(xué)模型

本文鎮(zhèn)定控制采用機(jī)器人極坐標(biāo)運(yùn)動(dòng)學(xué)模型

(6)

如圖1所示，式(6)中的運(yùn)動(dòng)模型是以De為參考坐標(biāo)系建立的，用(zer,xer)表示機(jī)器人在De下的相對(duì)當(dāng)前位置，θ表示從機(jī)器人當(dāng)前位置到期望位置的向量與ze軸正方向的夾角。α為從機(jī)器人當(dāng)前位置到期望位置的向量與zr軸正方向的夾角，e為從機(jī)器人當(dāng)前位置到期望位置的向量的模。

圖1 機(jī)器人視覺(jué)伺服過(guò)程中的坐標(biāo)關(guān)系

2 采用2DTT估計(jì)機(jī)器人位姿

2.1 三角點(diǎn)張量矩陣的推導(dǎo)

三角點(diǎn)張量的矩陣為H={T1,T2,T3}，其中

(7)

H與攝像機(jī)矩陣(3)的關(guān)系如下[8]

Tj=qij(qr4)T-qi4(qrj)T

(8)

其中，取j=1,2,3,qij,qrj分別表示Qi,Qr的第j列，qi4,qr4分別Qi,Qr表示的第4列，整理可得出

(9)

式(9)中給出了本文相關(guān)的兩個(gè)運(yùn)算變量的整理結(jié)果，具體的整理結(jié)果可以參見(jiàn)文獻(xiàn)[12]。

2.2 估計(jì)機(jī)器人的位姿信息

H與Pi在De,Dr,Di下對(duì)應(yīng)的歸一化圖像坐標(biāo)有如相關(guān)系[12]

(10)

(11)

采用De與Di之間的實(shí)際距離L對(duì)H進(jìn)行歸一化有

(12)

(13)

其中，因?yàn)楣烙?jì)的位姿信息與實(shí)際的位姿信息僅相差一個(gè)比例因子，所估計(jì)的角度值與實(shí)際值相等。

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

3.1 估計(jì)位姿的運(yùn)動(dòng)學(xué)誤差模型分析

由式(6)、式(13)得出

(14)

根據(jù)式(6)、式(13)可以得出在視覺(jué)估計(jì)的機(jī)器人在極坐標(biāo)下的開(kāi)環(huán)運(yùn)動(dòng)學(xué)誤差模型為

(15)

當(dāng)es，α，θ趨于0時(shí)機(jī)器人可以從當(dāng)前位置到達(dá)期望位置，所以本文采用自適應(yīng)控制器控制式(15)中的誤差量趨于零。

3.2 控制器設(shè)計(jì)

設(shè)計(jì)自適應(yīng)控制器為

(16)

由于視覺(jué)估計(jì)的位置信息與實(shí)際的位置之間相差一個(gè)未知量k∈R+，所以需要針對(duì)這個(gè)未知量設(shè)計(jì)一個(gè)更新律

(17)

(18)

(19)

控制器穩(wěn)定性驗(yàn)證取Lyapunov函數(shù)如下

(20)

對(duì)V1求導(dǎo)并將式(16)代入得

(21)

其中

(22)

由式(17)和式(19)可以得出

(23)

由式(18)中限制條件可得

F?≤0

(24)

4 動(dòng)力學(xué)滑?？刂破髟O(shè)計(jì)

以De為參考坐標(biāo)系，如圖1所示移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型為

(25)

非完整約束條件為

(26)

移動(dòng)機(jī)器人動(dòng)力學(xué)模型為

(27)

將式(25)求導(dǎo)代入式(27)得

(28)

其中，τ=[τrτl]T，τr,τl分別為右輪和左輪的輸出力矩

(29)

m為機(jī)器人質(zhì)量，r為驅(qū)動(dòng)輪半徑輪，驅(qū)動(dòng)輪軸為2L，I為轉(zhuǎn)動(dòng)慣量。

取τ1=(τr+τl)/r，τ1=L(τr+τl)/r則可以寫(xiě)為

(30)

將自適應(yīng)控制器的輸出速度信號(hào)作為期望速度Ve=[vω]T，則對(duì)于實(shí)際速度Vr速度誤差σ，選擇如下滑膜面實(shí)現(xiàn)滑膜控制

(31)

(32)

其中，η=[ηd1ηd2]T為滑模積分常數(shù)且η∈R+。

對(duì)式(32)求導(dǎo)并將式(30)、式(31)代入得

(33)

(34)

切換控制器τsw

(35)

其中，ζ1，ζ2>0。

則動(dòng)力學(xué)控制器為

(36)

控制器穩(wěn)定性驗(yàn)證取Lyapunov函數(shù)如下

(37)

對(duì)求導(dǎo)得

(38)

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

機(jī)器人參數(shù)m=33 kg,2L=0.5 m,d=0.4 m,2r=0.19 m。其中，m為機(jī)器人質(zhì)量，2L為機(jī)器人直徑，d為機(jī)器人兩輪軸間距，2r為驅(qū)動(dòng)輪的直徑。

實(shí)驗(yàn)中參數(shù)設(shè)置如下機(jī)器人的初始位姿為(-1.05 m,-1.18 m,-0.49 rad),機(jī)器人的期望位姿為(0 m,0 m,0 rad),自適應(yīng)控制器參數(shù)為λ=0.1,l=0.23,H=051,J=0.12,kmin=1.56。

圖2～圖5均為采用滑模速度控制后的實(shí)驗(yàn)曲線圖，圖2為運(yùn)動(dòng)軌跡圖。圖3中為位姿誤差收斂曲線，e1為距離誤差，e2為機(jī)器人朝向角誤差。圖4為線速度變化曲線，(圖4～圖7中點(diǎn)劃線為自適應(yīng)控制器得出的期望速度，實(shí)線為通過(guò)采集機(jī)器人輪轉(zhuǎn)速折算出的實(shí)際的運(yùn)動(dòng)速度),圖5為角速度變化曲線，圖6、圖7給出了未采用滑模速度控制的線速度變化曲線和角速度變化曲線。

圖2 機(jī)器人運(yùn)動(dòng)軌跡

圖3 位姿誤差收斂曲線

圖4 線速度變化曲線

圖5 角速度變化曲線

圖6 未使用滑模控制線速度變化曲線

圖7 未使用滑?？刂平撬俣茸兓€

通過(guò)圖2～圖5的收斂曲線可以看出本文提出的伺服系統(tǒng)具有良好的穩(wěn)定性與收斂性能。2DTT可以充分利用二維圖像中的信息，所以可以更為準(zhǔn)確得到機(jī)器人在移動(dòng)中的相對(duì)位置信息并且不受機(jī)器人運(yùn)動(dòng)狀態(tài)的干擾，但是由于數(shù)據(jù)處理量較大，在控制機(jī)器人運(yùn)動(dòng)過(guò)程中存在是性較差的問(wèn)題。由圖6～圖7的速度曲線可以看到在未使用滑?？刂七M(jìn)行速度跟蹤時(shí)，由于2DTT的實(shí)時(shí)性較差，實(shí)際的速度曲線與期望速度曲線存在較大的差值且存在一定的抖動(dòng)。為了改善這一問(wèn)題，在圖4～圖5的速度曲線中采用了滑?？刂七M(jìn)行速度跟蹤控，可以看出實(shí)際的速度曲線與期望的速度曲線間的差值明顯減小且逐漸趨于一致，速度曲線的抖動(dòng)問(wèn)題也得到明顯改善。由于實(shí)際速度趨于期望速度的過(guò)程變快也使機(jī)器人的運(yùn)動(dòng)速度在一定程度上得到提高，控制機(jī)器人到達(dá)期望位置的時(shí)間縮短，控制效率得到改善。

圖8給出了和圖2同次實(shí)驗(yàn)中特征點(diǎn)在圖像平面上的運(yùn)動(dòng)軌跡，其中三角與星形分別為機(jī)器人在期望位姿處與初始位姿處對(duì)應(yīng)的圖像特征點(diǎn)。圖9給出了自適應(yīng)比例因子k的變化曲線。

圖8 特征點(diǎn)在圖像平面上的運(yùn)動(dòng)軌跡

圖9 自適應(yīng)比例因子k

6 結(jié)束語(yǔ)

本文首先介紹了2DTT視覺(jué)信息估計(jì)方法，并在文獻(xiàn)[10]中視覺(jué)伺服控制的基礎(chǔ)上，采用比文獻(xiàn)中具有更強(qiáng)視覺(jué)魯棒性的2DTT來(lái)對(duì)機(jī)器人的實(shí)時(shí)位置進(jìn)行估計(jì)。增強(qiáng)了視覺(jué)伺服系統(tǒng)的魯棒性。并通過(guò)設(shè)計(jì)滑膜速度跟蹤器，使機(jī)器人的實(shí)際運(yùn)動(dòng)速度趨于自適應(yīng)控制器得出的期望速度，解決機(jī)器人在運(yùn)動(dòng)中存在的速度變化時(shí)的抖動(dòng)問(wèn)題，由于趨于期望速度使機(jī)器人的實(shí)際運(yùn)動(dòng)速度加快，可以加快機(jī)器人向期望位姿的收斂。通過(guò)實(shí)驗(yàn)分析驗(yàn)證了本文提出視覺(jué)自適應(yīng)系統(tǒng)的魯棒性和穩(wěn)定性，以及滑膜速度跟蹤的可行性與有效性。