基于視覺反饋的板球系統(tǒng)定位控制

武凡凱， 付 偉， 張釜榮, 謝慕君*

(1.長春工業(yè)大學(xué) 電氣與電子工程學(xué)院， 吉林 長春 130012; 2.吉林石化公司有機合成廠， 吉林 吉林 132000)

0 引 言

板球系統(tǒng)是一個多變量、非線性控制對象，是球桿系統(tǒng)[1]的二維擴展。其作為一個具有雙輸入、雙輸出的兩自由度機械系統(tǒng)，經(jīng)常作為經(jīng)典模型用于檢測各種控制理論，并對一些動態(tài)系統(tǒng)具有重要的研究意義。板球系統(tǒng)的控制對象是球盤，使其偏轉(zhuǎn)來控制球盤上的小球，目的是為了得到小球在球盤坐標(biāo)系中的坐標(biāo)。文中主要是對板球系統(tǒng)做攝像機的標(biāo)定，并將結(jié)果應(yīng)用到實物中。板球系統(tǒng)如圖1所示。

圖1 板球系統(tǒng)示意圖

1 板球系統(tǒng)物理建模

板球系統(tǒng)X軸和Y軸彼此垂直，為了更方便研究，我們把它當(dāng)作是互相垂直方向上的兩個球桿，即球桿系統(tǒng)的二維擴展。 球盤具有兩個自由度，分別是繞X軸旋轉(zhuǎn)和繞Y軸旋轉(zhuǎn)，其與繞X軸轉(zhuǎn)動的傾角α和繞Y軸轉(zhuǎn)動的傾角β相對應(yīng)。當(dāng)球盤的傾斜角度很小時，可以獲得α和β與X軸電機轉(zhuǎn)角θm1和Y軸電機轉(zhuǎn)角θm2之間相對應(yīng)的關(guān)系：

式中:d1----電機軸與X軸連接桿下端點的距離；

d2----電機軸與Y軸連接桿下端點的距離；

L1----球盤中心支點與X軸連接桿上端點的距離；

L2----球盤中心支點與Y軸連接桿上端點的距離。

板球機械示意圖如圖2所示。

圖2 板球機械示意圖

對于本系統(tǒng)，d1=d2=23 mm，L1=L2=98 mm。

通過對圖2系統(tǒng)進(jìn)行動力學(xué)分析，使用拉格朗日方程，獲得上述模型完整的非線性耦合的動力學(xué)方程：

X方向：

Y方向：

式中：mb----小球的質(zhì)量；

rb----小球的半徑；

Ib----小球繞質(zhì)心的轉(zhuǎn)動慣量；

h----球盤平面與其回轉(zhuǎn)中心的距離；

α----球盤繞X軸旋轉(zhuǎn)的角度；

β----球盤繞Y軸旋轉(zhuǎn)的角度；

xb、yb----小球在球盤坐標(biāo)系下的坐標(biāo)。

考慮小球在球盤的工作點附近時，對式(2)、式(3)進(jìn)行線性化處理，得到線性化方程：

(4)

2 攝像機標(biāo)定

攝像機的標(biāo)定[2-3]是獲得攝像機的內(nèi)外部參數(shù)，得到攝像機的幾何模型和所獲得圖像信息與真實世界信息的幾何成像關(guān)系[4]，是獲取運動物體真實位置不可或缺的一步，也是視覺傳感方式實現(xiàn)的前提條件。

文中采用線性模型標(biāo)定方法對板球系統(tǒng)的攝像機進(jìn)行標(biāo)定，景物點世界坐標(biāo)系(Xw，Yw，Zw)與其投影點圖像坐標(biāo)(u,v)的關(guān)系如下:

(5)

式中:ax=f/dx，ay=f/dy；

R----3×3旋轉(zhuǎn)矩陣；

P[(Px,Py,Pz)T]----世界坐標(biāo)系原點在攝像機坐標(biāo)系中的位置；

M1----完全由ax，ay，u0，v0決定，由于ax，ay，u0，v0只與由攝像機內(nèi)部結(jié)構(gòu)有關(guān)，為攝像機內(nèi)部參數(shù)；

M2----完全由攝像機相對于世界坐標(biāo)系的方位決定，為攝像機外部參數(shù)；

M----3×4矩陣，稱為投影矩陣。

將式(5)寫成：

(6)

式中: (Xwi,Ywi，Zwi,1)----空間第i個點的坐標(biāo)；

(ui,vi，1)----第i點的圖像坐標(biāo);

mij----投影矩陣M的第i行j列元素，其中m34=PZ，PZ不為零[5]，M矩陣乘以一個除零以外的任意常數(shù)，其并不會使(Xw,Yw，Zw)與(u,v)的關(guān)系改變，所以可令m34=1。

展開式(6)，消去Zci，可得如下兩個關(guān)于mij的線性方程：

m11Xwi+m12Ywi+m13Zwi+m14-uim31Xwi-uim32Ywi-uim33Zwi=uim34

m21Xwi+m22Ywi+m23Zwi+m24-vim31Xwi-vim32Ywi-vim33Zwi=vim34(7)

如果有n個已知點，它們的圖像坐標(biāo)(ui,vi),(i=1,2,…,n)與其世界坐標(biāo)(Xwi,Ywi，Zwi),(i=1,2,…,n)是已知的，則可以得到關(guān)于矩陣元素的2n個線性方程。矩陣形式方程如下：

(8)

上述方程的未知數(shù)為11個，記做m，則式(8)可以化為：

Km=U(9)

式中:K----式(8)左邊2n×11矩陣;

m----未知的11維向量;

U----式(8)右邊的2n維向量。

當(dāng)2n>11,即n≥6時，對上述線性方程用最小二乘法[6-8]求解。

設(shè)定球盤的三個位置如圖3所示。

(a) 位置1(球盤水平)(b) 位置2(球盤與水平位置呈夾角α1(β1))(c) 位置3(球盤與水平位置呈夾角α2(β2))

圖3 球盤的三個位置

當(dāng)球盤處于位置2時，板球繞X軸旋轉(zhuǎn)的角度α1與其繞Y軸旋轉(zhuǎn)的角度β1的值為:α1=β1=arctan0.19。當(dāng)球盤處于位置3時，其繞X軸旋轉(zhuǎn)的角度α2與其繞Y軸旋轉(zhuǎn)的角度β2的值為:α2=β2=-arctan0.19。

通過計算獲得標(biāo)定點的圖像坐標(biāo)和標(biāo)定點在世界坐標(biāo)系中的坐標(biāo)，見表1。

表1 標(biāo)定點坐標(biāo)

選用3組18個標(biāo)定點通過最小二乘法求得矩陣如下:

(10)

3 小球空間位置解算

計算出M矩陣的各個元素后，對于空間任意一點P實際坐標(biāo)(Xw,Yw，Zw)，可以很容易由M矩陣求得其圖像坐標(biāo)(u,v)；然而卻無法由圖像坐標(biāo)p(u,v)通過逆過程得到空間點P的實際坐標(biāo)(Xw,Yw，Zw)。

在板球系統(tǒng)中，通過電機的轉(zhuǎn)動能夠得到球盤在任何時候繞X軸和繞Y軸的轉(zhuǎn)動角度,通過三維空間旋轉(zhuǎn)矩陣的計算，可得球盤的平面方程,如圖4所示。

圖4 板球轉(zhuǎn)動示意圖

直線OP與球盤平面方程的交點就是小球質(zhì)心在空間坐標(biāo)系中的坐標(biāo)點。

3.1 球盤平面方程

球盤繞X軸、Y軸轉(zhuǎn)動角度分別為α、β,球盤的旋轉(zhuǎn)矩陣為:

(11)

根據(jù)旋轉(zhuǎn)矩陣獲得球盤平面方程為:

Xwsinβcosα-Ywsinα+Zwcosβcosα=0(12)

3.2 小球?qū)嶋H坐標(biāo)計算

假設(shè)在圖像坐標(biāo)系中，小球的質(zhì)心坐標(biāo)是(u,v),通過M矩陣可得圖像坐標(biāo)系與世界坐標(biāo)系之間的映射關(guān)系，其線性方程為:

(m11-m31u)Xw+(m12-m32u)Yw+(m13-m33u)Zc=um34-m14

(m21-m31v)Xw+(m22-m32v)Yw+(m33-m33v)Zc=vm34-m14(13)

聯(lián)立球盤平面方程與上述線性方程所組成的線性方程組為:

Xwsinβcosα-Ywsinα+Zwcosβcosα=0

(m11-m31u)Xw+(m12-m32u)Yw+(m13-m33u)Zw=um34-m14

(m21-m31v)Xw+(m22-m32v)Yw+(m33-m33v)Zw=vm34-m14(14)

系統(tǒng)實際運行時，根據(jù)測得的α、β及通過視覺反饋獲得的小球圖像坐標(biāo)(u,v)代入上述方程組，可計算出在世界坐標(biāo)系下小球的實際坐標(biāo)(Xw,Yw，Zw)。

4 系統(tǒng)控制結(jié)構(gòu)及控制器設(shè)計

針對板球系統(tǒng)的小球定位控制，系統(tǒng)采用雙閉環(huán)結(jié)構(gòu)，板球系統(tǒng)的閉環(huán)控制系統(tǒng)結(jié)構(gòu)如圖5所示。

圖5 板球閉環(huán)控制系統(tǒng)結(jié)構(gòu)

由式(4)可以看出，板球坐標(biāo)系中小球的X坐標(biāo)值xb僅與β有關(guān)，Y軸坐標(biāo)值yb僅與α有關(guān)，所以此系統(tǒng)就可以作為兩個獨立子系統(tǒng)進(jìn)行同步控制。

外環(huán)進(jìn)行小球定位閉環(huán)控制，采用視覺傳感器進(jìn)行小球的位置測量；內(nèi)環(huán)進(jìn)行球盤的傾角閉環(huán)控制，光電編碼器進(jìn)行角度測量。雙環(huán)均采用PID控制器[9-10]。

設(shè)系統(tǒng)給定值r(t),實際輸出值為c(t)，則誤差e(t)=r(t)-c(t)，通過PID調(diào)節(jié)后的輸出值u(t)為：

(15)

則傳遞函數(shù)為：

(16)

式中：Kp、Ti和Td----分別為PID控制器的比例系數(shù)、積分時間常數(shù)和微分時間常數(shù)。

5 實驗及結(jié)論

通過上述攝像機標(biāo)定及PID控制器設(shè)計后，在固高公司的板球?qū)嶒炏到y(tǒng)上進(jìn)行了多組定位實驗，小球的定位誤差均在1 mm以內(nèi)，達(dá)到了預(yù)期的控制效果。選取三組實驗數(shù)據(jù)見表2。

表2 小球?qū)嶒灁?shù)據(jù)

第一組實驗對應(yīng)的小球?qū)崟r控制定位如圖6所示。

由圖6可知，“+”為正方向，小球初始位置(0,0)，預(yù)期位置(50,50)，X方向上誤差為0.79 mm，Y方向上誤差為0.93 mm，小球的實際位置(49.21,50.93)。