目標物相對距離與角度

全方位攝影機是利用雙曲面鏡反射，將四周圍的環境擷取下來，因此目標 物會隨著在曲面鏡上反射的距離不同而扭曲變形，圖 4-12為全方位鏡上所看到 的 一 個 座 標 系 統 ， 以 攝 影 機 的 中 心(xCen，y_Cen)為中心點，其中 x_Cen =173

age

、

，目標物的座標為( ， )，目標物與y軸的夾角稱為 Im

128

y_Cen = x_Tar y_Tar θ ，由以上

的定義，可以將求出目標物距離中心點的距離^DIm^age由( 4-5 )式所示：

D_Image = (x_Tar −x_Cen)² +(y_Tar −y_Cen)² ( 4-5 )

角度θ_Image下式( 4-6 )式所示：

tan ( ) 2

1 Im

Cen Tar

Cen Tar

age y y

y y

−

− −

=π ⁻

θ ( 4-6 )

由圖形上兩點像素距離所算出來的 ，必須將它換算為實際於平面上平台與 目標物的距離，因此將測量實際距離(cm)與像素距離(pixel)之間的關係，分別由 0 cm 至 200 cm 之間，每相隔 20 cm 測量一次所對應到的像素值。全方位攝影 機為 360 度完全對稱的形式，因此針對一條線去做距離的量測，即可映射至平 台對目標物距離。

D_Image

) )

y (x_tar、 _tar

y_cen

cen、 (x

圖 4-12 攝影機與目標物相對距離及角度

使用 curve fitting 的方式去找出一條轉換函式近似，所得到的轉換函式為 ( 4-7 )式。

y=3.0682×10⁻⁶x³−3.7984×10⁻³x² +1.7915x−288.24 ( 4-7 )

由所近似的方程式與實際量測所得到的圖形為圖 4-13所示。

300 350 400 450 500 550 600 650 700 750

-50 0 50 100 150 200 250

X-Y Distance*10 (pixel)

Distance (cm)

real estimate

圖 4-13 由全像鏡估測之 X-Y 距離對應至實際距離

4.2 Kalman Filter 於平台移動位置估測

4.2.1 Kalman Filter [22]

當影像收尋到所看到的目標物後，為了有效的追蹤目標物的位置，並以 平穩的路徑到達所指定的位置，預估其下一步的位置是相當重要，在眾多的 估測理論中，Kalman filter 是最常被用來做運動控制的估測，也因此針對 Kalman filter 做介紹。

這也是可以在電腦視覺的領域上，Kalman filter 始終佔有著一席之地，應用 (priori estimation error) 協方差為

−

其中Kk為尚未決定值的組合因子

Kalman filter 可以為兩個部份，分別為預測與更新：

預測部分：

根據以上所敘述，可以發現 Kalman filter 主要是利用量測到的數據不斷更新 預測，可以歸納出兩種特色：

一、以隨機程序的向量模型來建構而成，利用所有量測數據求得最佳估 測狀態。

二、採用遞迴的方式來處理具有雜訊干擾的量測資訊，只要用最新的量 測數據。

由以上 Kalman filter 的估測程序是以迴授控制為基礎，將某一時刻的預估值 回授給量測方程式，則未來的狀態的確可以經由過去的狀態推測而得。

4.2.2 Kalman Filter 平台位置估測實驗結果

在4.1.2節中，得到了目標物與平台之間的距離關係，因此可以運用影像 來測試平台的距離及角度，不過由於平台在移動的過程中，容易受到晃動，

導致影像偵測起來也會有晃動的情形，因此加入Kalman Filter來做距離及角 度位置的估測，假設量測系統狀態如下所示：

) 1 ( ) 1 ( )

(k = p k− +w k−

p ( 4-14 ) )

( ) ( )

(k p k v k

y = + ( 4-15 )

其中p(k−1)為前一時刻平台的位置狀態。

p(k)為現在這個時刻平台的位置狀態。

y(k)為目前平台所觀測到的位置狀態。

w(k)視為在輸入時的white noise。

v(k)視為在輸出觀測時的 white noise。

在對於輸入與輸出系統狀態所存在的 white noise 可能造成的原因，輸入狀 態方面其 white noise 是由於在平台移動時，對輸入的位置狀態有一隨機改變；

而在輸出狀態方面其 white noise 則可能因為平台移動期間本身存在一個震動的 問題。

而在這裡要處理相對位置雜訊干擾的問題，焦點集中在相對位置取線的估 測，所以將位置曲線取出，並且在單位上統一以距離以cm來做計算，角度以徑 度(rad.)做計算。因此要透過Kalman Filter來處理的位置曲線，分別有兩種由全方 位攝影機所得到距離位置X-Y與角度位置，平台於慢速影像所看到的位置如圖 4-15所示：

0 50 100 150

-50 0 50 100 150 200

Time (0.1s)

Distance (cm)

Y

(a) 以 encoder 為回授之直線運動影像量測

0 50 100 150

Time (0.1s)

Angle (rad.)

Fi

(b) 平台等速旋轉時影像角度量測 圖 4-15 透過影像所得到相對位置

運用Kalman Filter對圖 4-15作位置的估測，估測狀態如下所示：

)]

此位置估測的系統狀態空間參數中，a=c=1，輸入狀態的 white noise 為

， ， 輸 出 狀 態 的 white noise 為 ， 其 變 異 數 實際狀況及經過實驗調整後，選擇Q=0.1、R=1.0 這一組參數，利用其來處 理

)、 ⁾。

圖 4-15，其結果如圖 4-16所示：

0 50 100 150 -50

0 50 100 150 200

Time (0.1s)

Distance (cm)

Y

IF IF Kalman

(a) 低速時影像距離估測(V=30cm/s)

0 50 100 150

0 1 2 3 4 5 6 7

Time (0.1s)

Angle (rad.)

Fi

IF IF KF

(b) 影像角度估測

圖 4-16 Kalman Filter對圖 4-15的位置估測

於高速時，如圖 4-17所示，則平台在移動的過程中產生了較劇烈的震盪，

0 5 10 15 20 25 30 35 40 45 50 -50

0 50 100 150 200

Time (0.1s)

Distance (cm)

Y

IF IF Kalman

圖 4-17 平台行使於高速時(V=100 cm/s)

可以看出在 noise 的干擾之下，可以將大振幅的干擾降低，達到想要估 測的位置狀態，且估測狀態變化量也不大，在均方誤差上也是很小。

在此提出以一維的Kalman Filter 經過實驗數據調整輸入、輸出系統狀態 的 white noise variance，在位置估測運算上很方便且容易實現。並且對於在 不同的速度下，都有很好的位置估測曲線，將所得到的位置估測值作為平台 的回授，可以使得平台有更好的精確度。

4.3 感測融合架構

由4.2節中，將影像所得到的距離及角度經過Kalman Filter濾波估測後，

得到較為穩定的結果，因此接下來將Encoder與影像所得到的資訊做一整 合，利用其各自優勢截長補短：

Encoder：有較好的穩定性，但隨著時間的累積或打滑的產生而準確性下 降。

影像回授：有較好的準確性，但由於影像於較遠處的地方，容易產生較 大的振盪行為，因此於近距離有較好的精確度。

由圖 4-16、圖 4-17可以看到影像於低速及高速的運動下，當影像回授 小於 100cm以下容易產生不穩定，因此希望平台可以運動於平穩的狀況下，

將使用Fuzzy的觀念將encoder與影像所得到的數值融合，如圖 4-18所示，將 影像所偵測到的位置距離(^DImage)做為依據，並令兩信任程度函數分別為

μ

D

_Image Image

μ

Encoder

μ

圖 4-18 距離與角度信任權重比

4.4 結合影像融合機制實現

為探討感測融合對於路徑追跡的影響，由於影像感測容易產生震盪，因 此若平台位置控制器太大時，容易產生震盪，因此於網路化控制使用相同的 控 制 器(KP=0.1)做為評比的標準，而集中式的控制則使用相同的控制器 (KP=0.003)，讓平台低速行走於直線路徑，做為實驗條件。將實驗分成三部 份：

Type 1：無感測融合(encoder)。

Type 2：無感測融合(image)。

Type 3：感測融合(fusion)。

表 4-1、表 4-2、表 4-3為表示三種類型的XW、Y_W、φ 位置IAE值於網 路式與集中式控制，可以看出當平台完全採用encoder為回授依據時，則平台 行走較為穩定，因此IAE值較小；當平台完全使用image回授時，由於距離目

標物遠，平台一開始容易產生些微震盪，會影響平台的初始角度，因此有較 大的IAE值；運用encoder與image融合後，IAE值則介於encoder與image之 間。

表 4-1 網路化控制於不同感測方式之位置 IAE 比較 Sampling time=50ms

XW Y_W φ

Straight

Encoder Image Fusion Encoder Image Fusion Encoder Image Fusion Mean 0.02 7.43 5.2 899.68 958.62 953.49 0.05 0.13 0.06

Position

IAE Max 0.03 9.93 5.86 899.89 964.85 955.71 0.05 0.28 0.1 (單位:cm)

表 4-2 集中式控制於不同感測方式之位置 IAE 比較 Sampling time=50ms

XW Y_W φ

Straight

Encoder Image Fusion Encoder Image Fusion Encoder Image Fusion Mean 12.57 25.25 14.9 934.6 984.48 968.0 0.01 0.16 0.02

Position

IAE Max 13.71 30.49 16.5 936.82 988.41 971.2 0.01 0.37 0.04 (單位:cm)

表 4-3 集中式控制於不同感測方式之位置 IAE 比較 Sampling time=2ms

XW Y_W φ

Straight

Encoder Image Fusion Encoder Image Fusion Encoder Image Fusion

Mean 17.58 60.8 31.5 1375 1420 1417 0.01 0.06 0.01

Position

IAE Max 20.6 66.8 34.6 1376 1428 1422 0.01 0.12 0.01 (單位:cm)

Type 1：無感測融合(encoder)

使用encoder作為平台角度、位置回授，在位置IAE與位置誤差都有較佳 的表現，平台也可以控制的穩定，但其缺點為平台若產生打滑，導致累積誤

差的產生，如圖 4-19為平台三方向的軌跡，可以看出encoder確實有到最終

Time (0.1s)

Distance (cm)

Y

Command Encoder IF Kalman

(a) Y 軌跡圖

Time (0.1s)

Distance (cm)

X

Command Encoder IF Kalman

0 50 100 150

Time (0.1s)

Angle (rad.)

Fi

Command Encoder IF Kalman

(b) X 軌跡圖 (c) φ 軌跡圖 圖 4-19 Type 1 直線路徑圖(μ_Encoder=1)

Type 2：無感測融合(image)

使用影像所得到的位置、角度回授，影像經由Kalman Filter位置估測 後，減少影像的震盪，由全方位鏡上所得到的平台座標X、Y、φ 最為回授，

圖 4-20為使用image回授所 到的平台三方向軌跡，可以看出若平台距離偵 測物較遠時，則容易震盪，導致平台一開始所偵測到的角度稍為傾斜，因此 整體由影像所得到的回授有達到，實際誤差如表 4-4 所示，差了 1.5cm，與 Type 1 比較起來，整體平台實際精確度大為提升，因此為了消除平台一開始 行走的震盪現象，提出Type 3 的融合方法。

得

0 50 100 150

-50 0 50 100 150 200 250

Time (0.1s)

Distance (cm)

Y

Command Encoder IF Kalman

(a) Y 軌跡圖

0 50 100 150

Time (0.1s)

Distance (cm)

X

Time (0.1s)

Angle (rad.)

Fi

Command Encoder IF Kalman Command

Encoder IF Kalman

Oscillation

(b) X 軌跡圖 (c) φ 軌跡圖 圖 4-20 Type 2 直線路徑圖(μImage=1)

Type 3：感測融合(fusion)

將影像與encoder所得到的值做融合機制如圖 4-18所示，當距離目標

0 50 100 150

Time (0.1s)

Distance (cm)

Y

Command Encoder IF Kalman Fusion

Fusion

(a) Y 軌跡圖

Time (0.1s)

Distance (cm)

X

Command Encoder IF Kalman Fusion

Time (0.1s)

Angle (cm)

Fi

Command Encoder IF Kalman Fusion

Fusion

Fusion

(b) X 軌跡圖 (c) φ 軌跡圖 圖 4-21 Type 3 直線路徑圖(影像距離 50cm 啟動融合) ≥

表 4-2 網路化控制於不同感測方式之前後位置誤差比較 Sampling time=50ms Type 1 (Encoder) Type 2 (Image) Type 3 (Fusion) Straight Sensor

output

World coordinate

Sensor output

World coordinate

Sensor output

World coordinate

Mean 0.00 9.7 0.2 1.5 0.06 1.03

Position

error |Max| 0.00 10.1 0.36 1.8 0.14 1.5

表 4-3 集中式控制於不同感測方式之前後位置誤差比較 Sampling time=50ms Type 1 (Encoder) Type 2 (Image) Type 3 (Fusion)

Straight Sensor output

World coordinate

Sensor output

World coordinate

Sensor output

World coordinate

Mean 0.00 10.16 0.16 1.13 0.1 1.1

Position

error |Max| 0.00 10.3 0.19 1.8 0.12 1.8

(單位:cm)

表 4-4 集中式控制於不同感測方式之前後位置誤差比較 Sampling time=2ms Type 1 (Encoder) Type 2 (Image) Type 3 (Fusion)

Straight Sensor output

World coordinate

Sensor output

World coordinate

Sensor output

World coordinate

Mean 0.00 10.03 0.13 1.9 0.11 0.93

Position

error |Max| 0.00 10.1 0.15 2.5 0.12 1.5

4.5 網路化遠端控制平台

本節將介紹平台遠端遙控，並可將平台上的影像傳回供操作者觀看，並 可將平台上的回授資訊傳回遠端，如圖 4-22所示，底層控制由RS-232 以傳 輸率19200 bps串接而成，DSP與Server端電腦由RS-232 以傳輸率 115200 bps 溝通，Client端與Server端電腦以無線網路 802.11g作為無線傳輸，進而達到 整體平台網路化的控制，進而達到整體平台網路化的控制。

DSP F2812 PC(Server) Omnidirectional

Camera

Motor-4 Motor-3

Motor-2 Motor-1

RS-232 Video Grabber

NTSC

PC(Client) Image,Command

USB

Wireless

RS-232

圖 4-22 平台網路化遠端控制架構

在 遠 端 控 制 方 面 程 序 分 為 三 部 份 ： 無 線 網 路 傳 輸 、 影 像 展 開 與 PC(client)-PC(server)-DSP 之間的溝通協定，以下將依序介紹：

1、無線網路傳輸

機器人影像的資料較為龐大，因此未了可以達到影像傳輸的目的及遠端 下達平台命令，利用電腦的IEEE 802.11b/g無線通訊模式，藉由OFDM調變 技術最快速度可以到達 54Mbs，實現可移動式的監視系統。802.11 制訂了兩 種不同類型的無線區域網路基本架構，分別是「Ad Hoc Wireless LAN」和

「Infrastructure」[29]，在此選擇Ad Hoc Wireless LAN的無線網路架構，如圖 4-23所示，Ad Hoc架構能即時架設起無線通信網路，在這種架構中，通常任 二個用戶間都可直接通訊，可以達到分散式通訊的目的。

MS 1

MS 3

MS 2

圖 4-23 Ad Hoc 無線區域網路架構

2、全方位影像展開

由於全方位攝影機是藉由攝影機去照攝曲面鏡所得到的影像，而且為了 可以照射到 360 度的影像，所以整張圖形聚集於平台中心點如圖 4-24所

由於全方位攝影機是藉由攝影機去照攝曲面鏡所得到的影像，而且為了 可以照射到 360 度的影像，所以整張圖形聚集於平台中心點如圖 4-24所

在文檔中 全方位移動平台網路化控制及感測融合實現 (頁 58-0)