卡爾曼濾波器之探討

圖 4-23 障礙物橫向移動實驗示意圖

表 4-8 橫向移動於不同距離之追蹤結果

Distance of obstacle 5 m 7 m 10 m 13 m 15 m Number of detected obstacles 231 224 586 587 255

Succeed 231 224 585 571 224

Fail 0 0 1 16 31

Success Rates 100 % 100 % 99.83 % 97.27 % 87.84 %

4.4 避障模式

4.4.1 卡爾曼濾波器之探討

經過速度模型的建立後，我們已經約略可估算出障礙物速度了，這也表示我 們可利用速度模型計算下一個時間障礙物的概略位置，所使用的卡爾曼濾波器其 運動模型適合用在等速度直線運動上，在時間夠長的情況下會收斂，但是就應用 上來說障礙物的位置預測能越早收斂越好，且追蹤到的障礙物因為偵測距離上的 限制，所累積的 frames 數目不一定足夠多到讓系統收斂。為了解決此問題我們 提出一個改進 initial guess 的方法，將利用現在已知的速度模型分別計算 x 跟 z 方 向的速度大小進而得到下一個時間的位置，給予卡爾曼濾波器 initial guess 跟狀 態位置的估算，如此一來便可在較短時間內收斂並得到較低的誤差，最後經過測

98

試得到的結果如下圖 4-24 其中預測位置與實際目標物位置之誤差計算方式採用 RMSE (root-mean-square error)的計算方式，如下式

n

z z

x RMSE x

n

t realt predictt realt predictt



 ¹

2 , ,

2 ,

, ) ( ) ]

[(

(4.1) n 代表預測次數

(a)

(b)

99

(c)

圖 4-24 卡爾曼濾波器探討 (a) 不同 Initial guess 的誤差比較 (b) 較差的 Initial guess 之軌跡圖 (c) 較佳的 Initial guess 之軌跡圖

實驗結果如上圖 4-23 設攝影機位置在座標 (350,0) 處，我們以上視圖觀察 被追蹤之障礙物移動過程，圖中藍色為雙眼立體視覺偵測到並透過像差影像計算 出的障礙物座標軌跡，紅色則是預測的座標軌跡。圖 4-24 (a) 是利用速度模型輸 入 initial guess 給卡爾曼濾波器進行預測的軌跡，經過計算後預測的 RMSE 約為 25 公分，由於偵測範圍為距離雙眼立體視覺攝影機 20 公尺誤差百分比約為 1.25

%。圖 4-24 (b) 則是利用隨機產生的 initial guess 給卡爾曼濾波器進行預測的 RMSE 約為 120 公分，誤差百分比約為 6%。比較結果後可看出雖然理論上利用 隨機產生的 initial guess 進行預測最後會因為收斂而預測趨近實際的軌跡，但實 際上此實驗中在尚未收斂障礙物便已經遠離偵測範圍。因此若 initial guess 給予 卡爾曼濾波器較接近的預測位置時則可以快速收斂並有較低的 RMSD，對於本 系統偵測距離為 20 公尺可確保在障礙物偵測到後利用較短時間內達到較低的預 測誤差有實際的成效。

100

探討後可知一開始的量測誤差會決定一個系統收斂與預測的能力，因此以利 體視覺計算出的三維座標，加上可信任之速度估算結果決定出較佳的 initial guess 進行位置預測軌跡實驗。實驗場景於台大農業試驗場，實驗過程將以一個農人推 動中耕機來進行，攝影機高度 110 公分，偵測範圍 20 公尺。實驗時系統會偵測 並追蹤移動中的障礙物同時預測其移動軌跡，實驗結果如下圖 4-25 示。圖 4-25 (a) 可同時比較系統預測的軌跡與實際行走的軌跡，經過計算平均誤差約 11 公 分 (約 0.5 %)。

(a) (b)

圖 4-25 軌跡預測與實際軌跡的比較

(a) 軌跡記錄於上視圖的結果，(b) 中耕機移動的路線圖。

4.4.2 路徑規劃

利用權重地圖進行路徑規劃並不是每個時間都進行，配合著我們已經分成四

101

類的警示訊息，只有在「ALARM 」、「ALERT 」兩個狀態下才執行演算法，原 因是當障礙物距離過近也就是「BRAKE」的狀態下，會建議使用者直些剎車而 不建議採取閃避的策略，「CLEAR」則表示前方 20 公尺以內皆為 free space 的安 全區域，因此也沒又必要做導航。

路徑規劃實驗於市區道路進行，結果如下圖 4-26 所示，期間共進行約 90 次 的路徑規劃，權重地圖解析度為 100 × 125，圖中可看出團塊狀中間的深灰色代 表障礙物在地圖上所顯示的原始大小，周圍環繞的黑色則為經過膨脹運算後的結 果，代表著考慮車身大小後障礙物的影響範圍，淺灰色則代表路徑規劃後的結果。

# 30

102

# 50

# 80

圖 4-26 左影像與路徑規劃結果

在 3.5.3 時曾提到過 A* 演算法是一個區域式搜尋的演算法，較適合用在即 時系統上，因此將設計一實驗來探討地圖搜尋的範圍。實驗場景包含農場、校園 以及市區道路，並將路徑規劃之地圖搜尋範圍百分比記錄下來，實驗數據共 253 筆，經統計後算出地圖平均被搜尋範圍約為 49.1 %，如下圖所示。系統執行路徑 規劃時的平均速度約為 12 fps。

103

圖 4-27 路徑規劃之地圖搜尋百分比