• 沒有找到結果。

整合擴展型卡爾曼濾波器與 PID 控制於智慧型

N/A
N/A
Protected

Academic year: 2021

Share "整合擴展型卡爾曼濾波器與 PID 控制於智慧型"

Copied!
78
0
0

加載中.... (立即查看全文)

全文

(1)

N a tio n a l U niversity of Kao hsi un g

國立高雄大學電機工程學系

碩士論文

整合擴展型卡爾曼濾波器與 PID 控制於智慧型

機器人之自主式避障

Integrating extended Kalman filtering and PID

controller for obstacle avoidance of mobile robots

研究生:洪偉洲撰

指導教授:吳志宏博士

(2)

N a tio n al U niversity of Kao hsi un g

整合擴展型卡爾曼濾波器與 PID 控制於智慧型

機器人之自主式避障

指導教授: 吳志宏博士 學生:洪偉洲 國立高雄大學電機工程學系

摘要

藉由環境感測器掃描以進行避障控制,是機器人應用中一項基本而重要的 功能。但因環境與感測器的不完美所造成的雜訊或誤差,往往使避障效果大打 折扣,因此雜訊過濾與感測器誤差改善是機器人自動避障中不可或缺的一環。 卡爾曼濾波器 (Kalman filtering) 是常用來作為修正量測誤差的方法之一,可以 適用於線性系統的量測誤差修正。但在機動型機器人避障問題中,通常無法以 簡單的線性系統假設加以描述,當應用於避障感測器量測誤差修正時,其修正 精確度往往有所限制。擴展型卡爾曼濾波器 (extend Kalman filtering) 為傳統卡爾 曼濾波器的修正版,用來處理非線性的誤差修正,具有比傳統卡爾曼濾波器更 好的抗干擾與雜訊的靈敏度與精確度。本研究利用擴展型卡爾曼濾波器搭配 PID(proportional-integral-derivative) 控制器,處理搭載多組超音波感應器之輪型機 器人的避障任務。論文中分析輪型機器人利用超音波感應器進行避障任務時的系 統模型,並探討如何將擴展型卡爾曼濾波器與 PID 控制器整合,以進行避障任 務。本研究實際蒐集靜態的感測器資料,從中調整可達最佳避障效果的參數,並 進行動態避障實驗,在不同複雜度的場地中進行避障效果測試。研究結果顯示, 使用擴展型卡爾曼濾波器的避障效果優於傳統卡爾曼濾波器。在不同的障礙環境 中,避障的有效程度提升 6.74% 〜 33.34%。 關鍵字:超音波、卡爾曼濾波器、擴展型卡爾曼濾波器、PID 控制、避障、導航、 機器人

(3)

N a tio n al U niversity of Kao hsi un g

Integrating extended Kalman filtering and PID

controller for obstacle avoidance of mobile robots

Advisor: Dr. WU, CHIH-HUNG Student: HONG, WEI-ZHOU

Department of Electrical Engineering, National University of Kaohsiung

ABSTRACT

Obstacle avoidance is an essential function for the navigation of mobile robots. Vari-ous sensors are used for scanning the existence of obstacles in the environment where mo-bile robots cruise. However, noise or inaccuracy unavoidedly exist in sensor data. Noise filtering improves the measurement accuracy of senors and plays an important role for ob-stacle avoidance in the applications of mobile robots. Kalman filtering (KF) is a popular method for noise filtering in linear systems. If the application system is non-linear, the performance of KF may be limited. Due to the non-linearity and unstability of sensoring results, the problem of obstacle avoidance is non-linear and KF has limited performance. Extended Kalman filtering (EKF) is an advanced version of KF for dealing with non-linear problems and usually has better ability of noise tolerance than KF. EKF linearizes the non-linear parts of the system under study and derives the measure via partial derivatives on non-linear measurement functions. This study integrates EKF and PID (proportional-integral-derivative) controller for obstacle avoidance on a two-wheeled mobile robot. The robot used in this study carries some sonar sensors that acquire signals of obstacles period-ically. Sonar data is filtered by EKF before they activate the PID controller. The systems model corresponding to the problem of obstacle avoidance is analyzed. The performance of EKF and EF on noise filtering for obstacle avoidance is compared. Several experiments of obstacle avoidance are conducted on the mobile robot and the results are analyzed. The results show that EKF provides reliable navigation information better than that from KF. A ratio of 6.74%–33.34% on performance improvement on obstacle avoidance is gained from EKF.

Keywords: Sonar, Kalman filtering, extended Kalman filtering, PID controller, obstacle avoidance, navigation, mobile robots.

(4)

N a tio n al U niversity of Kao hsi un g

致謝

首先,感謝指導教授吳志宏博士在研究所求學中的指導,老師費心的教 導使我得以了解機器人控制系統、雜訊過濾方法、與線性數學領域等的各種研究 與專業能力,研究過程中不時指點我正確的方向、教導我人生的準則與分享人生 的體驗,使我在求學過程中獲益良多。 在研究中也感謝潘欣泰老師、陳俊卿老師的指導,並在研究上提供許多建 議,豐富我研究上的內容。而接著要感謝 ICAL 的瓊輝學長、瓏軒學長、右棟學 長、阿鐵、育賢、冠文學弟與孟偉學弟等諸位良師益友陪伴著我一起瘋狂、一起 奮戰,為那些年我們一起待過的實驗室生涯中添增許多豐富的色彩,使得當初我 們一起寫程式的功力,都成為現今刻苦銘心的回憶。 最後,感謝這一路來默默在背後支持、關心與包容我的父母與親戚,因為有 你們的支持我的研究所路程才能毫無後顧之憂,並且順利的完成取得碩士學位。 再次感謝所有關心及支持我的朋友們。碩士生涯終於告一段落,隨之而來的是更 大、更艱鉅的挑戰。未來的日子中,希望大家還能有時間聚聚,互相的支持與鼓 勵。 洪偉洲謹誌於國立高雄大學大學電機工程學系 中華民國一零五年一月

(5)

N a tio n al U niversity of Kao hsi un g

目錄

中文摘要 . . . i 英文摘要 . . . ii 致謝 . . . iii 目錄 . . . iv 圖目錄 . . . vi 表目錄 . . . viii 1 緒論 . . . 1 1.1 研究背景與動機 . . . 1 1.2 研究動機與目的 . . . 2 1.3 研究流程與架構 . . . 4 2 文獻探討 . . . 6 2.1 PID 控制器 . . . 6 2.1.1 PID 控制與公式說明 . . . 6 2.1.2 PID 控制相關應用 . . . 8 2.2 卡爾曼濾波器 . . . 8 2.2.1 KF 公式推導 . . . 8 2.2.2 KF 相關應用 . . . 14 2.3 擴展型卡爾曼濾波器 . . . 16 2.3.1 EKF 公式推導 . . . 16 2.3.2 EKF 相關應用 . . . 22 3 問題定義與分析 . . . 24 3.1 靜態量測 . . . 24 3.2 動態量測 . . . 27 3.3 非線性系統分析 . . . 29 3.4 分析結果與討論 . . . 34 4 研究平台說明 . . . 35 4.1 機器人軟硬體、外觀、控制方式 . . . 35

(6)

N a tio n al U niversity of Kao hsi un g 4.2 PID 控制器設定 . . . 36 4.3 KF/EKF 參數設定 . . . 39 4.4 整合 KF/EKF 與 PID . . . 40 5 實驗與結果分析 . . . 43 5.1 實驗環境說明 . . . 43 5.2 靜態環境實驗 . . . 43 5.3 動態環境實驗 . . . 50 5.3.1 濾波修正實驗 . . . 52 5.3.2 機器人避障實驗 . . . 54 6 結論與未來展望 . . . 61 6.1 結論 . . . 61 6.2 未來展望 . . . 62 參考文獻 . . . 63

(7)

N a tio n al U niversity of Kao hsi un g

圖目錄

1.1 自主式機器人流程圖 . . . 2 1.2 研究進行流程圖 . . . 5 2.1 PID 控制系統方塊圖 . . . 7 2.2 KF 系統方塊圖 . . . 10 2.3 KF 處理流程 . . . 15 2.4 EKF 系統方塊圖 . . . 17 2.5 EKF 處理流程 . . . 22 3.1 測試場地示意圖 . . . 25 3.2 靜態定點量測示意圖 . . . 25 3.3 靜態定點量測數據 (1/2) . . . 26 3.4 靜態定點量測數據 (2/2) . . . 27 3.5 機器人動態量測數據 . . . 28 3.6 超音波避障流程示意圖 . . . 30 3.7 感測器讀數與實際量測值之回歸分析 (MAPE=0.2091) . . . 31 3.8 對應方向角度與距離回歸分析 (MAPE=20.19) . . . 31 3.9 角度與左右輪差回歸分析 (MAPE=0) . . . 33 4.1 兩輪型機器人實體照片 . . . 36 4.2 機器人軟硬體架構圖 . . . 36 4.3 PID 極限增益示意圖 . . . 37 4.4 PID 超調改善示意圖 . . . 38 4.5 PID 快速收斂示意圖 . . . 38 4.6 參數調整結果 . . . 41 4.7 整合系統流程圖 . . . 42 5.1 實驗環境示意圖 . . . 43 5.2 靜態實驗環境俯視圖 . . . 44

(8)

N a tio n al U niversity of Kao hsi un g 5.3 定點 1 量測數據 . . . 46 5.4 定點 2 量測數據 . . . 47 5.5 定點 3 量測數據 . . . 48 5.6 動態實驗環境俯視圖 . . . 51 5.7 場景 A 蒐集路徑 . . . 53 5.8 動態量測數據 . . . 54 5.9 場景 A 動態避障路徑 . . . 55 5.10 場景 B 動態避障路徑 . . . 55 5.11 場景 C 動態避障路徑 . . . 56 5.12 場景 D 動態避障路徑 . . . 56 5.13 場景 E 動態避障路徑 . . . 57

(9)

N a tio n al U niversity of Kao hsi un g

表目錄

4.1 齊格勒尼科爾斯法調整變數 . . . 37 4.2 PID 參數調整表 . . . 39 5.1 靜態環境濾波參數設置 . . . 45 5.2 靜態實驗 - 原始資料與各修正方法比較 . . . 50 5.3 動態環境濾波參數設置 . . . 53 5.4 動態實驗 - 原始資料、KF 與 EKF 誤差比較 . . . 53 5.5 場景 A 動態避障統計表 . . . 57 5.6 場景 B 動態避障統計表 . . . 57 5.7 場景 C 動態避障統計表 . . . 58 5.8 場景 D 動態避障統計表 . . . 58 5.9 場景 E 動態避障統計表 . . . 59 5.10 KF 動態避障各場景評估 . . . 60 5.11 EKF 動態避障各場景評估 . . . 60

(10)

N a tio n al U niversity of Kao hsi un g

第 1 章

緒論

1.1

研究背景與動機

隨著科技不斷演進,在機器人領域中有許多嶄新的突破。生活中日漸仰賴機器人 技術的應用,如保全居家照護、工業應用、軍事與娛樂等方面 [1, 2, 3]。行動機器 人結合多種感應器與導航技術,使設備能自主地執行各項任務 [4],其中以避障與 導航定位系統尤其重要 [5, 6]。而避障的方法已有相當眾多的研究與發展,像是整 合雷射測距儀與,讓機器人在現實環境中能緊急避障與自動駕駛 [7]。也有使用立 體攝影機,達成在 3D 立體環境中即時障礙物檢測和避障之自主導航 [8]。利用聲 納之避障系統也是常見的方法 [9],像是深海航行的水下機器人,可避免水下探測 行動時受阻 [10]。自主式機器人運作流程大致分為三個部分,分別為感測、計算、 動作,如圖 1.1所示。機器人通常藉由自身裝設的感測器來偵測訊號,訊號由內部 元件處理計算,再將得到的輸出透過線路傳遞到控制器進行動作,以達成避障的 功能。而避障動作所產生的狀態,又會成為下一時刻的感測器輸入。因此不同的 感測器、訊號處理方式,可能導致不同的控制結果,影響避障與導航效能。 任何量測設備與感測器皆存在量測誤差,環境也存在各種擾動與不確定性。 一個有效的雜訊過濾方法有助於提高精確度、減少控制器的複雜度,並增加行動 機器人的性能 [11]。卡爾曼濾波器 (Kalman filter, 以下簡稱 KF) [12] 由匈牙利數學 家 R.E. Kalman 於西元 1960 年提出,其結合最小平方法及狀態空間法,針對線性 的動態系統進行雜訊濾波,演算法流程為一連串遞迴計算,只需前一時刻與當前 時刻之資訊,不需記錄大量歷史資訊。計算與前一時刻的共變異數關係,進行當 前時刻的預測與修正,運算速度極快,並能建立出準確的動態及量測系統模型。 KF 為常見的訊號過濾方法之一,應用領域之廣泛包括導航、通訊、機器人控制、

(11)

N a tio n al U niversity of Kao hsi un g གෳ ीᆉ ୏բ 圖 1.1: 自主式機器人流程圖 飛航控制、影像邊緣檢測等 [13, 14, 15]。 KF 演算法是基於線性系統的假設前提,但實際應用中系統與環境屬於非線性 系統,機器人移動中過程中相當不穩定,運動狀況、感測器晃動、環境變化等因 素,都可能導致整個系統過程複雜化,無法以假設的線性系統模型來描述,可能 使過濾結果與實際落差過大,為此而使用了 KF 的改良版本,擴展型卡爾曼濾波 器 (extend Kalman filter, 以下簡稱 EKF)。EKF 由最早由 Bucy 及 Sunahara 等人 [16] 提出,EKF 以非線性系統描述系統模型與量測模型,是由 KF 架構延伸而來,故 流程架構及方式與卡爾曼濾波器相同,而且具有 KF 快速、遞迴且不需記錄大量 歷史資料的優點。EKF 對原本 KF 系統模型與量測模型取偏導數,成為新 EKF 為 濾波器的輸入,掌握了每一時刻的趨勢,比起 KF 更能掌握整個系統模型,且有 更高的靈敏度、更快修正誤差 [17]。EKF 常用於複雜的動態系統的訊號過濾 [18], 像是未知環境的車輛追蹤、車流預測 [19]。

1.2

研究動機與目的

在機器人避障與導航中,運用方法改善感測器的訊號來源,理應能增加機器人的 導航精確度,改善行走過程的穩定性。在此避障與導航問題中,機器人在不同環 境下,像是移動中與晃動可能讓感測器的產生雜訊影響精確度,環境中存在的擾 動與不穩定使整個系統呈現非線性,推測 EKF 方法理應在大多數場景勝過 KF。 因此在不同環境之下,整合感測器的與控制器的行動機器人,調適合理參數設 置,實驗過濾方法的有效性,KF 與 EKF 的對於問題是否有改善,比較兩方法為 本研究課題之一。本研究主要達成以下目的:

(12)

N a tio n al U niversity of Kao hsi un g • 針對機器人搭載之感測器訊號做改善,從靜態量測所得之資料進行濾波比 較,並調整其參數,以達到最佳避障效果的參數。 • 透過實驗能比較出 KF 與 EKF 兩方法的濾波效果,並參考相關指標,確認 EKF 修正數據相較於 KF 的改善狀況。 • 確認機器人整合各方法之效能,於不同環境場地進行動態避障,並可從實驗 結果清楚看出 EKF 相較於 KF 的穩定性。 • 讓機器人實際行走避障,並能從中能透過幾個常見指標,用數值化、視覺化 的方式得到 EKF 對於 KF 應用於機器人避障的改善程度。 為達成以上目的,本論文研究中將分別整合 EKF 與 KF 方法,研究中機器人裝載 多組超音波感應器,使機器人能獲得環境相關資訊,上述方法將用來過濾機器人 所搭載超音波感測器之訊號。機器人控制方面,若僅依靠簡單的規則函數進行避 障,可能發生相同情況下得到的控制結果有所落差,因此利用工業常使用的控制 方法,通常藉由迴授控制 (feedback) 的方式 [20],可在固定環境下得出可靠的系 統模型做出合適的控制。PID(proportional-integral-derivative) 控制是一種常使用於 機器人或工業設備的方式 [21],此方式能獲得更好的訊號以達到更佳的控制性 能,但也需要進行參數調適,因此還會用上相關調整方法。感測器訊號透過 KF 與 EKF 的前置處理經由轉換以進行 PID 之控制器的計算輸出,最後再經由轉換, 使輪型機器人能在相同的情況給予期望的控制,以便能達到自主式避障導航之目 的。本研究可分為兩階段,第一階段著重在機器人靜態的表現,主要進行 KF 與 EKF 參數調整與濾波效果的比較,參數的調整採用專家法,調整出能用於該環境 的系統參數。並透過指標比較,未使用過濾與使用 KF 和 EKF 的改善狀況。第二 階段著重在自主避障應用的方面,由架設在場地上方的攝影機錄影觀察機器人的 動態,並透過影像處理與資料紀錄,透過幾個指標與統計來找出 EKF 對於 KF 於 機器人避障的改善狀況。

(13)

N a tio n al U niversity of Kao hsi un g

1.3

研究流程與架構

本論文研究流程主要分為研究動機、相關文獻蒐集與探討、程式撰寫、場地架 設、實驗結果分析與結論等部分。說明如下,研究進行流程圖請參考圖1.2。 • 研究動機:包含相關的研究背景、問題之範圍及定義,參考機器人導航避 障、控制及感應器誤差相關資料及文獻,將問題範圍設定在機器人自主式避 障與導航的控制方式。 • 相關文獻蒐集與探討:蒐集與本研究相關之技術文獻,包含機器人避障與導 航、卡爾曼濾波器、擴展型卡爾曼濾波器、PID 控制等,深入了解相關技術 應用與優缺點。 • 問題分析:以機器人避障研究進行數學模型推導與簡易數據收集,針對機器 人裝載超音波測距器避障的問題,分析其系統是否為穩定、非線性。 • 程式撰寫:撰寫卡爾曼濾波器程式、擴展型卡爾曼濾波器程式、撰寫 PID 控 制程式、撰寫數據存取程式及影像後續處理程式。使機器人能在場地收集資 訊並進行實驗為目的。 • 實驗結果分析與討論:比較不同實驗環境、參數下所獲得之效能及分析應 用。 本論文之架構分為六章節,其簡述如下。第二章「文獻探討」,簡述卡爾曼濾波器 及擴展型卡爾曼濾波器等背景知識與相關文獻探討。第三章「問題定義與分析」, 分析機器人避障是否為非線性系統,並對於感測器之靜動態量測資料是否穩定做 驗證。第四章「研究方法與設計」,詳細說明機器人機構設計、方法設計及完整的 研究流程。第五章「實驗與結果分析」,根據設計之實驗架構設計,進行各種不同 環境與參數之實驗,並針對實驗結果分析及說明。第六章「結論與未來展望」,總 結本研究遇到的問題及方法,並相對提出未來可以研究及更深入的方向與構想。

(14)

N a tio n al U niversity of Kao hsi un g ࣴز୏ᐒ ࣬ᜢЎ᝘ᇆ໣ᆶ௖૸ ำԄኗቪ ჴᡍ่݀ϩ݋ᆶ่ፕ යтǵፕЎϷਜᝤ 1. PID ௓ڋำԄ 2. ьᅟୗᘠݢᏔำԄ 3. ᘉ৖ࠠьᅟୗᘠݢᏔำԄ ୢᚒϩ݋ 1. ߚጕ܄س಍ϩ݋ 2. ᓉǵ୏ᄊໆෳӸӧόᛙۓ܄ 圖 1.2: 研究進行流程圖

(15)

N a tio n al U niversity of Kao hsi un g

第 2 章

文獻探討

本章節簡述相關技術與文獻探討,依序說明 PID 控制、KF 與 EKF 公式推導 和其相關應用。

2.1

PID 控制器

2.1.1

PID 控制與公式說明

PID 控制器 [21] 最早由俄裔美國工程師 Minorsky 所提出,在設計美國海軍的自動 操作系統時所得分析,他基於對舵手的觀察,舵手控制船舶時不只是參考當前的 誤差,也考慮過去的誤差以及誤差的變化趨勢來控制。換句話說,由控制系統中 的量測裝置得到的測量結果,使控制器作出決定,並通過一個輸出設備來作出反 應,控制器從量測設備得到測量結果,該結果和操作者的設定值有所誤差,因此 用設定值減去測量值來得到誤差做為為迴授訊號。PID 藉由迴授訊號來計算,系 統就可以從它的輸出結果中改善誤差 [22]。以上可過程由 PID 系統方塊圖 2.1所 列,主要分為系統輸入、PID 各項控制、系統輸出結果與迴授訊號,其中方塊 Process 表示輸出設備之運作過程,數學符號表示如下: • u(t) :在時刻 t 控制系統之輸入。 • y(t) :在時刻 t 控制系統之輸入,作為時刻 t + 1 之迴授值。 • e(t) :在時刻 t 設定值 u(t) 與迴授值 y(t − 1) 之差值。

• Kp :比例控制項參數,調整當前誤差進行比例放大或縮小。

• Ki :積分控制項參數,調整到當前的累積誤差。

(16)

N a tio n al U niversity of Kao hsi un g ܭ௣݁ሺݐሻ ܭ௜න ݁ ߬ ݀߬ఛ ଴ ܭௗ݀݁ ݐ݀ݐ

є

є

Process ݕሺݐሻ D I P

н

н

н

݁ሺݐሻ

н

െ ݑሺݐሻ 圖 2.1: PID 控制系統方塊圖 • τ:從 0 到目前時刻 t 之積分變數。 整個系統以數學公式表示如下: Kpe(t) + Kit 0 e(τ )dτ + Kd d dt e(t) (2.1) 如何讓上述流程的結果為控制人員之設定,則須仰賴比例、積分、微分控制 項參數之調整,使其輸出值 y(t) 能收斂於使用者的設定值,讓系統更加準確而穩 定 [23]。讓系統輸出值不會有發散的震盪是首要條件,但使用試誤法調整 PID 參 數是一件耗時的工作,因此歷史上有許多不同的 PID 參數調試方法,像是齊格勒 尼科爾斯方法 (Ziegler–Nichols method) [24],其根據經驗法則的方式,觀察輸出結 果的震盪週期,依序調整 PID 三種控制項之參數。J. L. Meza 等人 [25] 提出一個 新的 PID 控制器控制方案,這項研究中提出的,是基於模糊系統對調整的機械手 臂的 PID 增益,根據機器人操縱的實際狀態更細膩選擇比例、積分、微分參數, 實驗結果表明該方法的有效性。Jin-Woo Jung 等人 [26] 所提出,基於梯度下降法 以在線的方式調整,該方案不僅簡單易於實現,而且保證準確和快速的速度。

(17)

N a tio n al U niversity of Kao hsi un g

2.1.2

PID 控制相關應用

PID 控制早期船舶自動操作系統中發展,可追溯到 1890 年代中的造船公司的調 速器設計 [20]。到 PID 控制器提出後,成為工業及機器人控制常用的元件之一。 學者 A. Shi 等人 [27] 採用模糊演算法與 PID 控制相結合,通過建立直流電動機 的數學模型,使用 MATLAB 模擬進行比較 PID 控制和模糊 PID 控制,模擬結果 表明模糊 PID 控制具有動態響應快,控制精確度高、超調小且控制系統是穩定 的,穩定性和適應性強。學者 Ping Xue [28] 提出了基於 FPGA(Field-programmable gate array),實現模糊 PID 控制演算法,仿真結果表明,基於 FPGA 實現模糊 PID 控制系統具有結構簡單、易修改的優勢,可以使系統達到的速度更穩定的效果。 學者 Derry Pratama [29] 使用 PID 的方法從起始位置上截取距離,並計算並用於 校正角度誤差,實驗結果證明 PID 參數調適後能有效使兩輪之人形機器人保持 平衡。學者 Jian Guo [30] 深入分析模糊控制和 PID 控制的不足,並提出了新的模 糊 PID 控制,模擬結果可以有效地降低控制系統的過衝,加快響應速度並縮短了 調整時間,該方法可以增強所用兩棲球形機器人的動態性能。學者 Francisco 和 Carlos [31] 提出移動機器人結合 PID 自適應類神經控制與運動控制器,藉由軌跡 和追蹤控制中的結果表明,運動控制器和自適應神經 PID 控制成功實現了良好的 追蹤性能。

2.2

卡爾曼濾波器

2.2.1

KF 公式推導

在此介紹 KF 對系統狀態預測與修正過程及其推導過程,其中, • xk:為系統在時刻 k 之狀態向量 (state vector) 且為 N × 1 矩陣,N 為欲預測 之個數。 • zk:為系統在時刻 k 之量測向量 (measurement vector) 且為 N × 1 矩陣。

(18)

N a tio n al U niversity of Kao hsi un g • wk:為系統在時刻 k 之狀態雜訊且為 N × 1 矩陣。 • vk:為系統在時刻 k 之量測雜訊且為 N × 1 矩陣。 • uk :為系統在時刻 k 之控制輸入且為 N × 1 矩陣。

• Ak:為系統在時刻 k 至 k + 1 之狀態轉移矩陣 (state transition matrix) 且為

N × N 矩陣。 • Hk:為系統在時刻 k 之轉換矩陣 (measurement matrix) 且為 N × N 矩陣。 • Bk:為系統在時刻 k 之控制矩陣 (control matrix) 且為 N × N 矩陣。 • Qk :為系統雜訊之共變異數矩陣且為 N× 1 矩陣。 • Rk:為量測雜訊之共變異數矩陣且為 N× 1 矩陣。 • ˆx− k :為未更新之預估值且為 N× 1 矩陣。 • ˆxk :為更新之預估值且為 N× 1 矩陣。 • ˆPk−:為未更新之共變異數矩陣且為 N × N 矩陣。 • ˆPk:為更新之共變異數矩陣且為 N× N 矩陣。 • Kk :為卡爾曼濾波器在時刻 k 之增益值且為 N × 1 矩陣。 • I :為單位 N × N 矩陣。 • ˆ:為系統之預估狀態。 :為系統之未更新之狀態。 KF 整個過程可用系統方塊圖 2.2表示,主要分為系統狀態與預測兩大塊,利用系 統的量測值作為迴授來對預測值修正。

(19)

N a tio n al U niversity of Kao hsi un g   ௞  ௞ିଵ  н н н  н н ௞     ௞ିଵ  ି н н н  н Ͳ   ௞ н 圖 2.2: KF 系統方塊圖 假設離散系統狀態模型及量測狀態模型分別為: xk+1 = Akxk+ Bkuk+ wk (2.2) zk= Hkxk+ vk (2.3) 其中公式 (2.2)、公式 (2.3) 中系統雜訊 (Wk) 與量測雜訊 (Vk) 均為獨立互不相關,

零均值 (zero mean) 的常態分布高斯白雜訊 (Gaussian white noise),即為

E[wkvkT] = 0 (2.4) 系統雜訊之共變異數矩陣 (covariance matrix) Qk為: E[wkwTj] =      0, j ̸= k Qk, j = k (2.5) 與量測雜訊之共變異數矩陣 (covariance matrix) Rk為: E[vkVjT] =      0, j ̸= k Rk, j = k (2.6) 假設在系統時刻 k 之前狀態程序有預估值為 ˆx−k ,其中’ˆ’ 表示為系統在時刻 k 之 預估狀態,’−’ 表示為系統在時刻 k 之未更新之狀態。 其假設定義預測誤差為: e−k = xk− ˆx−k (2.7)

(20)

N a tio n al U niversity of Kao hsi un g 預測誤差之共變異數矩陣為: ˆ Pk = E[e−ke−kT] = E[(xk− ˆx−k)(xk− ˆx−k) T] (2.8) 透過該時刻 k 之實際量測值與時刻 k 之未更新預測值 ˆx−k 來修正時刻 k 之更新預 估值,其計算中包含 ˆxk更新之預測值與尚未決定之卡爾曼增益值因子,其表示如 公式 (2.42)。假設 (zk− Hkxˆ−k) 若為零,即表示系統模型預測與實際模型一致,故 信賴其時刻 k 之預測結果。 ˆ xk = ˆx−k + Kk(zk− Hkxˆ−k) (2.9) 其假設定義更新預測誤差為: ek = xk− ˆxk (2.10) 更新預測誤差之共變異數矩陣為: ˆ Pk = E[ekekT] = E[(xk− ˆxk)(xk− ˆxk)T] = E[(xk− (ˆx−k + Kk(zk− Hkxˆ−k)))(xk− (ˆx−k + Kk(zk− Hkxˆ−k))) T ] = E[((I − KkHk)(Xk+ ˆx−k)− Kkvk)((I − KkHk)(xk+ ˆxk)− Kkvk)T] = (I − KkHk) ˆPk−(I− KkHk)T + KkRkKk (2.11) 由於更新預測誤差之共變異數矩陣之對角線元素為時刻 k 之狀態均分誤差總和, 藉由微分方式求得其極值,將得出卡爾曼增益值 Kk 使更新預測誤差之共變異數 矩陣對角線元素達最佳最小化。 d( ˆPk) d(Kk) = E[ekek T] d(Kk) = 2E[(xk− (ˆx−k + Kk(zk− Hkxˆ−k)))(zk− Hkxˆ−k)] = 2E[xkzk− ˆx−kzk− Hkxkxˆ−k + (ˆx−k) 2 ] − 2KkE[zk2− 2Hkxˆk−zk+ Hk2(ˆx−k) 2] (2.12)

(21)

N a tio n al U niversity of Kao hsi un g 其中公式 (2.45) 整理後如下: Kk = E[xkzk− ˆx−kzk− Hkxkxˆ−k + Hkx−k)2] E[z2 k− 2Hkxˆ−kzk+ Hk2(ˆx−k)2] (2.13) 將公式 (2.46) 拆解為兩部份作化簡,分子部分: E[xkzk− ˆx−kzk− Hkxkxˆ−k + Hkx−k) 2] = E[xk(Hkxk+ vk)− ˆx−k(Hkxk+ vk)− Hkxkxˆ−k + Hkx−k) 2] = E[Hkx2k− 2Hkxˆkxk+ Hkx−k)2+ (xk− ˆx−k)vk] (2.14) 因為公式 (2.47) 中量測誤差與量測雜訊各為獨立互不相關,故化簡為 E[(xk− ˆx−k)vk] = E[e−kvk] = 0 (2.15) 由公式 (2.48) 代入公式 (2.47) 化簡得出, E[Hkx2k− 2Hkxˆk−xk+ Hkx−k) 2] = HkE[x2k− 2ˆx−kxk+ (ˆx−k) 2] = HkE[(xk− x−k) 2 ] = HkE[(e−k)2] = HkPˆk− (2.16) 分母部分: E[zk2− 2Hkxˆ−kzk+ Hk2(ˆx−k) 2] = E[(Hkxk+ vk)2− 2Hkxˆk(Hkxk+ vk) + Hk2(ˆx−k) 2 ] = E[Hk2x2k− 2Hk2xˆkXk+ Hk2(ˆx−k) 2+ v2 k+ 2Hk(xk− ˆx−k)vk] = E[Hk2x2k− 2Hk2xˆkXk+ Hk2(ˆx−k) 2+ v2 k] = Hk2E[x2k− 2ˆx−kxk+ (ˆx−k) 2 ] + E[vk2] = Hk2Pˆk−+ Rk (2.17)

(22)

N a tio n al U niversity of Kao hsi un g 由公式 (2.49) 與公式 (2.50) 化簡後可得最佳增益值 Kk,即所謂之卡爾曼濾波增益 值: Kk= (HkPˆk−)(H 2 kPˆk−+ Rk)−1 (2.18) 定義其預測誤差之共變異數矩陣: ˆ Pk−= E[(xk− ˆx−k)(xk− ˆx−k) T] = E[((Ak−1xk−1+ Bkuk+ wk)− (Ak−1xˆk−1+ Bkuk)) ((Ak−1xk−1+ Bkuk+ wk)− (Ak−1xˆk−1+ Bkuk))T] = E[A2k−1(xk−1− ˆXk−−1) 2+ 2A k−1wk(Xk− ˆx−k) + wk2] (2.19) 其中公式 (2.52) 中量測誤差與系統雜訊各為獨立互不相關,故化簡為 E[2Ak−1wk(xk− ˆx−k)] = 2Ak−1E[wkek−1] = 0 (2.20) 由公式 (2.53) 代入公式 (2.52) 得出, ˆ Pk = E[A2k−1(xk−1− ˆx−k−1) 2+ (w k)2] = A2k−1E[(xk−1− ˆx−k−1) 2] + E[(w k)2] = A2k−1Pˆk−1+ QTk (2.21) 定義其更新預測誤差之共變異數矩陣: ˆ Pk= E[(xk− ˆxk)(xk− ˆxk)T] = E[(xk− ((ˆx−k)− HkKkxˆ−k + Kkzk))(Xk− ((ˆx−k)− HkKkxˆ−k + Kkzk))T] = E[(xk− ˆx−k) 2(1− H kKk)2− 2Kkvk(xk− ˆxk−)(I− HkKk) + Kk2v2k] (2.22) 其中公式 (2.55) 量測誤差與量測雜訊各為獨立互不相關,故化簡為 E[2Kkvk(xk− ˆx−k)(I − HkKk)] = 2Kk(I− HkKk)E[vk(xk− ˆx−k)] = 0 (2.23)

(23)

N a tio n al U niversity of Kao hsi un g 由公式 (2.56) 代入公式 (2.55) 化簡得出, ˆ Pk = E[(xk− ˆx−k) 2(I− H kKk)2+ Kk2v 2 k] = E[(xk− ˆx−k) 2(I− H kKk)2] + E[Kk2v 2 k] = (I − HkKk)2E[(xk− ˆx−k)2] + Kk2E[v 2 k] = (I − HkKk)2Pˆk−+ Kk2Rk (2.24) 再由公式 (2.51) 代入公式 (2.57),得出更新預測誤差之共變異數矩陣。 ˆ Pk= (I − HkKk)2Pˆk−+ K 2 ˆ Pk−(Hk− Hk2Kk) Kk = (I − HkKk) ˆPk− (2.25) 以上描述為卡爾曼濾波器之詳細推導計算步驟,KF 的操作由兩個階段–預測與修 正,迭代而成: Step-1: ˆx−k = Akxˆk−1+ Bkuk−1 (2.26) Step-2: Pk−= AkPk−1AkT + Qk (2.27) Step-3: Kk= Pk−Hk(HkPk−HkT + Rk)−1 (2.28) Step-4: ˆxk = ˆx−k + Kk(zk− Hkxˆ−k) (2.29) Step-5: ˆPk = (I− KkHk) ˆPk− (2.30) 在預測階段 Step-1 與 Step-2,濾波器使用上一狀態的預測,做出對當前狀態的預 測。在修正階段 Step-3、Step-4 與 Step-5,濾波器利用對當前狀態的測量值優化在 預測階段獲得的預測值,以獲得一個更精確的新估計值。圖2.3中描述了 KF 的處 理流程。

2.2.2

KF 相關應用

KF 最常被用來過濾量測值的誤差,KF 在發表五十周年之際,多達 20 餘種以上 KF 發展、大約 800 出版刊物,如建模、機器人控制、目標追蹤、監控搜索、識

(24)

N a tio n al U niversity of Kao hsi un g ି ௞ିଵ ௞ିଵ ି ௞ିଵ் K ି ்( ି ்+ ିଵ  ି   ି     ି Initial:,  Predict Correct 圖 2.3: KF 處理流程 別,以及機器人操作、定位、測繪,導航和探索 [32]。Y.A. 等人 [11] 在多感測器 模擬實驗下,驗證 KF 能夠有效減少感測器誤差即使其中有感測器損壞。Kwon 等人 [33] 提出結合 KF 的定位法,該方案的有效性透過以輪型機器人模擬和實驗 得到驗證,其定位誤差大大降低。Wonsang [34] 等人提出一視覺追蹤系統,編碼 器可以提供相當精確的機器人的位置信息,但只要機器人打滑其編碼器數據就 不可靠,藉由 KF 方法能有效解決不可靠的訊息。Qingxin 等人 [35] 整合了 KF 與 多感測器來預測機器人的方位,在 Matlab 的模擬結果下減少大量錯誤雜訊,能 夠更好地預測移動機器人的角度。學者 T. Yoshioka [36] 提出了一個瞬時狀態觀測 器(instantaneous state observer, ISOB)。然而加速度感測器一定會有測量雜訊突 波。為了克服這個問題使用 KF,實驗證明能有效減少突波,成功推定狀態。學者 B. H. Prasetio [37] 提出在不穩定環境中的移動機器人技術。PID 控制器和 KF 的結 合成為機器人的一個理想的試驗模型,實驗結果表明在特定的 KF 參數下,可以 通過處理雜訊共變異數矩陣,能夠減少系統的誤差。學者 J. Park [38] 介紹了一種 高性能的視覺追蹤系統,運動狀態的移動機器人很難保持目標物連續完整影像訊 號,為了解決這個問題,多感測器透過 KF 過濾相關訊息,該系統實現了在不同 的運動方案,有更完美的追蹤性能。 KF 也需要調整參數來使預測、修正更為準確,也透過與其他方法結合應用更

(25)

N a tio n al U niversity of Kao hsi un g 快調整出相應參數,亦能幫助其他演算法增加效能。學者 J. V. F. Neto [39] 提出一 種 KF 調適參數之方法,其主要根據參數 Q 與參數 R 對偶關係找出適合的參數, 透過 NASA 太空火箭模擬實驗,此調整方法能有效修正飛行時的誤差,讓導彈更 精確的到達目標地點。Yang 等人 [40] 實現自適應 KF 追蹤模型是在足球機器人系 統的應用,由於足球機器人的機動性和雜訊誤差的不確定性,KF 的預測可能是發 散的。自適應卡爾曼濾波可以自行調整模型的參數,以獲得準確的預測。學者 F. Sun [41] 介紹了對基於 KF 演算法與 LQR(Linear-quadratic regulator) 控制器之兩輪 自平衡機器人的方法,LQR 控制器被設計為機器人控制姿勢和位置,基於原型機 器人的實驗和模擬清楚地表明,該系統能夠實現很短的時間內平衡和位置追蹤。 T. H. S. Li 等人 [42] 提出了結合 KF 與模糊補償的校正方法,可以識別並抓住目標 對象、避開障礙物,並通過相對位置準確定位。H. Y. Chung 等人 [43] 結合多感測 器,KF 被用來預測系統狀態和減少由雜訊引起的影響,由保持平衡幾個實驗驗證 了該控制方案的可行性和有效性。

2.3

擴展型卡爾曼濾波器

2.3.1

EKF 公式推導

在此介紹 EKF 對系統狀態預測與修正過程及其推導過程,面對非線性系統時,其 系統及量測方程式變為非線性的形式: xk= f (xk−1, uk−1, wk−1) (2.31) zk= h(xk, vk) (2.32)

(26)

N a tio n al U niversity of Kao hsi un g ′ ௞ ௞  ௞ିଵ ௞ н н н  н н ௞ ௞ ′  ௞ିଵ ௞ି н н н  н Ͳ ௞ ̂௞ ̂௞ ௞н 圖 2.4: EKF 系統方塊圖 實際中顯然不知道每一個時刻的雜訊之值,但是可以將它們假設為零,從而取近 似值來預估狀態向量和量測向量: ˜ xk= f ( ˜xk−1, uk−1, 0) (2.33) ˜ zk= h( ˜xk, 0) (2.34) EKF 對系統及量測偏微分取導數,部分不同於 KF 之變數定義如下所列: • Ak: f 對 x 的偏微 Jacobian 矩陣 • Hk: z 對 h 的偏微 Jacobian 矩陣 • Vk: f 對 u 的偏微 Jacobian 矩陣 • Wk: f 對 w 的偏微 Jacobian 矩陣 EKF 是由 KF 架構延伸而來,故流程架構大致相同,差異於 EKF 是以線性逼近後 的系統模型來計算。EKF 整個過程可用系統方塊圖 2.4表示,同樣分為系統狀態 與預測兩大塊,但計算誤差的對象改變。 非線性系統和量測關係的模型將它用近似值的方式表示: xk ≈ ˜xk+ Ak(xk−1− ˜xk−1) + Wkwk−1 (2.35) zk ≈ ˜zk+ Hk(xk−1− ˜xk−1) + Vkvk (2.36)

(27)

N a tio n al U niversity of Kao hsi un g 其中公式 (2.35)、公式 (2.36) 中系統雜訊 (Wkwk) 與量測雜訊 (Vkvk) 均為獨立互不 相關,零均值 (zero mean) 的常態分布高斯白雜訊,即為 E[WkwkVkvkT] = 0 (2.37) 系統雜訊之共變異數矩陣 (covariance matrix) Qk為: E[WkWjT] =      0, j ̸= k Qk, j = k (2.38) 與量測雜訊之共變異數矩陣 (covariance matrix) Rk為: E[VkVjT] =      0, j ̸= k Rk, j = k (2.39) 假設在系統時刻 k 之前狀態程序有預估值為 ˆXk ,其中’ˆ’ 表示為系統在時刻 k 之 預估狀態,’−’ 表示為系統在時刻 k 之未更新之狀態。 其假設定義預測誤差為: e−k = Xk− ˆXk− (2.40) 預測誤差之共變異數矩陣為: ˆ Pk = E[e−ke−kT] = E[(Xk− ˆXk−)(Xk− ˆXk)T] (2.41) 透過該時刻 k 之實際量測值與時刻 k 之未更新預測值 ˆXk−來修正時刻 k 之更新預 估值,其計算中包含 ˆXk更新之預測值與尚未決定之卡爾曼增益值因子,其表示 如公式 (2.42)。假設 (Zk− HkXˆk−) 若為零,即表示系統模型預測與實際模型一致, 故信賴其時刻 k 之預測結果。 ˆ Xk= ˆXk−+ Kk(Zk− HkXˆk) (2.42)

(28)

N a tio n al U niversity of Kao hsi un g 其假設定義更新預測誤差為: ek= Xk− ˆXk (2.43) 更新預測誤差之共變異數矩陣為: ˆ Pk = E[ekekT] = E[(Xk− ˆXk)(Xk− ˆXk)T] = E[(Xk− ( ˆXk−+ Kk(Zk− HkXˆk−)))(Xk− ( ˆXk−+ Kk(Zk− HkXˆk−))) T] = E[((I− KkHk)(Xk+ ˆXk)− KkVk)((I − KkHk)(Xk+ ˆXk)− KkVk)T] = (I− KkHk) ˆPk−(I− KkHk)T + KkRkKk (2.44) 由於更新預測誤差之共變異數矩陣之對角線元素為時刻 k 之狀態均分誤差總和, 藉由微分方式求得其極值,將得出卡爾曼增益值 Kk 使更新預測誤差之共變異數 矩陣對角線元素達最佳最小化。 d( ˆPk) d(Kk) = E[ekek T] d(Kk) = 2E[(Xk− ( ˆXk−+ Kk(Zk− HkXˆk−)))(Zk− HkXˆk−)] = 2E[XkZk− ˆXk−Zk− HkXkXˆk+ ( ˆXk)2] − 2KkE[Zk2− 2HkXˆk−Zk+ Hk2( ˆXk−) 2] (2.45) 其中公式 (2.45) 整理後如下: Kk = E[XkZk− ˆXk−Zk− HkXkXˆk−+ Hk( ˆXk)2] E[Z2 k− 2HkXˆk−Zk+ Hk2( ˆXk−)2] (2.46) 將公式 (2.46) 拆解為兩部份作化簡,分子部分: E[XkZk− ˆXk−Zk− HkXkXˆk−+ Hk( ˆXk−) 2] = E[Xk(HkXk+ Vk)− ˆXk−(HkXk+ Vk)− HkXkXˆk−+ Hk( ˆXk−) 2] = E[HkXk2− 2HkXˆk−Xk+ Hk( ˆXk)2+ (Xk− ˆXk−)Vk] (2.47) 因為公式 (2.47) 中量測誤差與量測雜訊各為獨立互不相關,故化簡為 E[(Xk− ˆXk−)Vk] = E[e−kVk] = 0 (2.48)

(29)

N a tio n al U niversity of Kao hsi un g 由公式 (2.48) 代入公式 (2.47) 化簡得出, E[HkXk2− 2HkXˆk−Xk+ Hk( ˆXk−) 2] = HkE[Xk2− 2 ˆXk−Xk+ ( ˆXk−) 2] = HkE[(Xk− Xk−) 2 ] = HkE[(e−k)2] = HkPˆk− (2.49) 分母部分: E[Zk2− 2HkXˆk−Zk+ Hk2( ˆXk−) 2] = E[(HkXk+ Vk)2− 2HkXˆk−(HkXk+ Vk) + Hk2( ˆXk−) 2 ] = E[Hk2Xk2− 2Hk2Xˆk−Xk+ Hk2( ˆXk−) 2+ V2 k + 2Hk(Xk− ˆXk−)Vk] = E[Hk2Xk2− 2Hk2Xˆk−Xk+ Hk2( ˆXk−) 2+ V2 k] = Hk2E[Xk2− 2 ˆXk−Xk+ ( ˆXk−) 2 ] + E[+Vk2] = Hk2Pˆk−+ Rk (2.50) 由公式 (2.49) 與公式 (2.50) 化簡後可得最佳增益值 Kk,即所謂之卡爾曼濾波增益 值: Kk= (HkPˆk−)(Hk2Pˆk−+ Rk)−1 (2.51) 定義其預測誤差之共變異數矩陣: ˆ Pk = E[(Xk− ˆXk−)(Xk− ˆXk−) T] = E[((Ak−1Xk−1+ BkWk)− (Ak−1Xˆk−1)) ((Ak−1Xk−1+ BkWk)− (Ak−1Xˆk−1))T] = E[A2k−1(Xk−1− ˆXk−−1) 2+ 2A k−1Wk(Xk− ˆXk−) + (BkWk)2] (2.52) 其中公式 (2.52) 中量測誤差與系統雜訊各為獨立互不相關,故化簡為 E[2Ak−1Wk(Xk− ˆXk−)] = 2Ak−1E[Wkek−1] = 0 (2.53)

(30)

N a tio n al U niversity of Kao hsi un g 由公式 (2.53) 代入公式 (2.52) 得出, ˆ Pk = E[A2k−1(Xk−1− ˆXk−1)2+ (BkWk)2] = A2k−1E[(Xk−1− ˆXk−1 )2] + E[(BkWk)2] = A2k−1Pˆk−1+ BkQkBkT (2.54) 定義其更新預測誤差之共變異數矩陣: ˆ Pk= E[(Xk− ˆXk)(Xk− ˆXk)T] = E[(Xk− (( ˆXk−)− HkKkXˆk−+ KkZk))(Xk− (( ˆXk−)− HkKkXˆk−+ KkZk))T] = E[(Xk− ˆXk)2(1− HkKk)2− 2KkVk(Xk− ˆXk−)(I − HkKk) + Kk2V 2 k] (2.55) 其中公式 (2.55) 量測誤差與量測雜訊各為獨立互不相關,故化簡為 E[2KkVk(Xk− ˆXk−)(I − HkKk)] = 2Kk(I− HkKk)E[Vk(Xk− ˆXk−)] = 0 (2.56) 由公式 (2.56) 代入公式 (2.55) 化簡得出, ˆ Pk = E[(Xk− ˆXk−) 2(I− H kKk)2+ Kk2V 2 k] = E[(Xk− ˆXk−) 2(I− H kKk)2] + E[Kk2V 2 k] = (I − HkKk)2E[(Xk− ˆXk)2] + Kk2E[V 2 k] = (I − HkKk)2Pˆk−+ K 2 kRk (2.57) 再由公式 (2.51) 代入公式 (2.57),得出更新預測誤差之共變異數矩陣。 ˆ Pk= (I − HkKk)2Pˆk−+ K 2 ˆ Pk−(Hk− Hk2Kk) Kk = (I − HkKk) ˆPk (2.58) 以上描述為卡爾曼濾波器之詳細推導計算步驟,統整後擴展型類似於傳統離散處 理流程,在擴展型中狀態轉換和測量模型不需要是狀態的線性函數,函數 f 可以

(31)

N a tio n al U niversity of Kao hsi un g ି ௞ିଵ, ௞ିଵ, 0 ି ௞ିଵ ௞ିଵ ି்ି் ் ିଵ  ି  ି, 0     ି Initial:, Predict Correct 圖 2.5: EKF 處理流程 用來從過去的估計值中計算預測的狀態,函數 h 可以用來以預測的狀態計算預測 的測量值。然而 f 和 h 不能直接的應用在共變異數中,取而代之的是計算偏導 Jacobian 矩陣。 Step-1: ˆx−k = f (xk−1, uk−1, 0) (2.59) Step-2: Pk= AkPk−1AkT + WkQkWkT (2.60) Step-3: Kk = Pk−Hk(HkPk−HkT + VkRkVkT)−1 (2.61) Step-4: ˆxk = ˆx−k + Kk(zk− h(ˆx−k, 0)) (2.62) Step-5: Pk = (I − KkHk)Pk− (2.63) 圖2.5中描述了 EKF 的處理流程。

2.3.2

EKF 相關應用

學者 Ivanjko 和 Petrovic [44] 透過使用 EKF 結合里程計和超音波讀數來減少量測誤 差,以里程計累積誤差做為效正參考,實驗結果表示 EKF 能有效改善超音波測距 器之讀數,讓其預測值更為精準。學者 Sangale 和 Shend [45] 提出實驗證明非系統 性量測誤差源會影響機器人的動作,他們展示了一個有效的方法:在行動機器人 定位中結合陀螺儀和里程計的量測。EKF 有效地降低了定位誤差同時產生可行的

(32)

N a tio n al U niversity of Kao hsi un g 預測。Peter 等人 [46] 開發了兩種濾波器,分別為粒子濾波器 (PF) 與 EKF,模擬 結果表明,EKF 的預測精度比 PF 更高,初始狀態被正確地預估。學者 Bruno [47] 提出一自主定位系統,從一個 RFID 讀寫器即時獲得 RSSI 值移動機器人的研製 工作協議,該協議所傳遞訊號通過 EKF 進行處理過濾,能有效減少因讀取訊號 不穩定所產生的誤差。Xiao-gang 等人 [18] 提出了透過從慣性感測器過濾隨機漂 移誤差,並且自適應地補償外部加速度,該算法通過兩輪機器人自主平衡實驗驗 證。學者 Luigi [19] 在本文面臨著未知室內環境下進行 SLAM 的問題,機器人起 點的模型和結束點作為 SLAM 地標,使用 EKF 過濾機器人超聲波感測器測量訊 號,搭配演算法在模擬實驗中得到了良好的修正結果與測繪地圖。Fantian Kong 等人 [48] 提出一個方法,行動機器人透過擴展型理論分析和結合多個感測器在周 圍環境中的訓息,這使得機器人清楚地識別周圍的物件,並順利地指引自身。學 者 Songmin Jia 和 Yasuda [17] 提出一個使用 GUI 為行動機器人建立地圖的方法。 為了產生精準的地圖很多種感測器被使用在這個方法。結果印證 EKF 能夠改善行 動機器人的預測與修正輸出。Manika Saha 等人 [49] 提出一種方法,定義兩種參 考指標靈敏度和穩健度,透過此兩種指標的性質來調整 EKF 的參數,並從模擬實 驗中驗證了其改善效果。

(33)

N a tio n al U niversity of Kao hsi un g

第 3 章

問題定義與分析

本節描述的機器人避障的相關問題將於本節加以定義,如機器人在靜止過程 中收發資訊與動態的差別,機器人於移動過程中屬於線性或非線性系統,KF 與 EKF 等過濾方法是否適用於本問題,僅依賴過濾雜訊是否足以解決避障與導航的 不穩定性等。

3.1

靜態量測

為觀察避障與導航系統過程,將機器人型設置於長 200 cm 、寬 150cm 、高 60cm 之 保 麗 龍 牆 面 築 起 的 黑 色 地 磚 中,如 圖 3.1。測 試 中 採 用 設 備 為 利 基 小 車 模 組 [50],感測器採用利基超音波測距感應器模組 Sonar A,機體前方、左側及右側 各裝載一組超音波感應器模組,於靜態測試中僅只用前方一組,以進行障礙物距 離之偵測。 機器人搭載超音波測距器接收資訊,並參考皮尺的量測當作真實值。讓機器 人在場地內各點進行靜止量測,以下挑選幾個定點的數據進行分析,如圖 3.2所 示。可得到如圖 3.3與圖 3.4之統計資料。從統計資料觀察,在靜態的狀況下也會 有突波產生。即使是定點 1 也產生了少量突波,推斷其為測距器受到雜訊干擾, 或該時刻未正確收到距離資訊。定點 2、5、6 機器人皆垂直牆面但不同距離,可 以觀察到感測器隨著距離的增加,聲波所授到的干擾也隨之提升,突波的狀況因 此隨之增加。定點 3、4 的機器人面向牆面的角度皆不同,可從觀察統計資料分析 超音波測距器其限制角度。可發現到該兩點並非垂直於牆面,導致測距器本身收 發困難。對比前面提到之數據可以發現,即使定點 3 距離較定點 4 小,隨著角度 的增長,收發的困難與干擾隨之產生,聲波的反射接收可能會遺漏較多,因而推 斷此為定點 3 的突波較多之原因。 綜上所述,可得以下之結論:

(34)

N a tio n al U niversity of Kao hsi un g 60cm 圖 3.1: 測試場地示意圖 3 6 4 1 2 5 86cm 134cm 54cm 128cm 42cm 圖 3.2: 靜態定點量測示意圖 • 任何感測器都存在誤差,盡可能減少干擾,也無法消除誤差,因此需要有一 雜訊過濾的方法。 • 靜止不動的測量過程,只有特定情況符合完美線性,對於很多場合與環境仍 可能出現大量突波或劇烈變化,若只使用線性的過濾方法,可能在部分場景 不適用。 • 即便設定機器人長時間處於靜止,且保持相同距離,但從測試結果來看,看 似相同的環境設置,其實對此問題是屬於不同的變數。亦即我們無法設定出

(35)

N a tio n al U niversity of Kao hsi un g 82 83 84 85 86 87 88 89 90 5 10 15 20 25 30 35 40 45 50 Distance(cm) Iteration Real Sensor (a) 定點 1 131 132 133 134 135 136 137 138 5 10 15 20 25 30 35 40 45 50 Distance(cm) Iteration Real Sensor (b) 定點 2 51 52 53 54 55 56 57 58 5 10 15 20 25 30 35 40 45 50 Distance(cm) Iteration Real Sensor (c) 定點 3 圖 3.3: 靜態定點量測數據 (1/2) 能掌握環境變數的場地,因此線性的方法大多只能在模擬環境中實行。

(36)

N a tio n al U niversity of Kao hsi un g 82 83 84 85 86 87 88 89 5 10 15 20 25 30 35 40 45 50 Distance(cm) Iteration Real Sensor (a) 定點 4 125 126 127 128 129 130 131 132 5 10 15 20 25 30 35 40 45 50 Distance(cm) Iteration Real Sensor (b) 定點 5 39 40 41 42 43 44 45 46 5 10 15 20 25 30 35 40 45 50 Distance(cm) Iteration Real Sensor (c) 定點 6 圖 3.4: 靜態定點量測數據 (2/2)

3.2

動態量測

接續前一小節之量測,本小節分析機器人在實際避障的動態狀況,讓機器人以超 音波測距器於該場地進行避障,動態測試則使用三個超音波感測器模組,讓機器

(37)

N a tio n al U niversity of Kao hsi un g 0 50 100 150 200 250 300 350 400 10 20 30 40 50 60 70 80 90 100 Distance(cm) Iteration Real Sensor (a) 前方感測器 40 60 80 100 120 140 160 180 10 20 30 40 50 60 70 80 90 100 Distance(cm) Iteration Real Sensor (b) 右方感測器 60 70 80 90 100 110 120 130 10 20 30 40 50 60 70 80 90 100 Distance(cm) Iteration Real Sensor (c) 左方感測器 圖 3.5: 機器人動態量測數據 人於方形場地中沿著對角線直行,並觀察超音波測距器之數據。從動態測量的統 計資料圖 3.5觀察,得知在動態的狀況下,突波的狀況相當嚴重,加上機器人不可 能安穩的行走,可能隨著牆面距離不同而改變軌道,加上機器人本身的晃動等,

(38)

N a tio n al U niversity of Kao hsi un g 比起靜態存在更多不穩定的因素,量測數據比起靜態更明顯不穩定,數值常處於 劇烈變化的情況,由此可以得到以下結論: • 機器人避障的動態狀況是由連續的靜態所組合而成,而原本靜態就可能出現 不穩定的劇烈變化,動態的環境下理應比靜態更不穩定。 • 當機器人行動過程中,干擾與雜訊比靜態來的多,自更難以用簡單數學描述 該狀態。 • 動態環境下,改善不穩定的雜訊濾波方法需要考慮的層面更廣。南每一時刻 的環境變數與干擾皆不相同,若是針對縣性的過濾方法,其參數的設定可能 僅設置初始值是不足的,需要能隨著時間變化改變趨勢的方式,才能有效針 對動態去做收斂與修正。

3.3

非線性系統分析

機器人以超音波測距器進行避障時,其流程可分為感測、計算、動作等步驟,以 下逐一說明: • 步驟一:首先藉由超音波測距器發射聲波偵測反射波 (波速每秒 340 公尺), 得到發送與接收的時間差,此為最初之輸入。藉由波速與時間可以計算出波 行進之距離,得到機器人與障礙誤的距離,然而其感測器可能有其限制,部 分情況或限制距離下得到的毒數未必準確,因此需針對其校準。 • 步驟二:採用手動以皮尺量測資料與感測器讀數進行比對,實際距離和聲波 訊號計算的結果可能有些微誤差,因此將兩者資料以散布圖表示,使用多項 式回歸分析用以校準。 • 步驟三:校準的後的感測器資料得經過轉換,本研究採用 PID 控制調整機器 轉彎後的角度,因此在此將量測距離轉為機器人轉彎角度使其達到期望的目

(39)

N a tio n al U niversity of Kao hsi un g ݔሺݐሻ ݔሺݐሻ ݔሺݐሻ ݔସሺݐሻ ݔሺݐሻ ݂ଶ)Ϧϩᙯඤ* ݂)ຯᚆᙯඤ* ݂)فࡋᙯඤ* ݂)QJE௓ڋ* ݂)፺ೲ௓ڋ* གෳᏔௗԏ ᐒᏔΓ୏բ ݔሺݐሻ 圖 3.6: 超音波避障流程示意圖 標值。 • 步驟四:藉由 PID 控制器得出的角度,來給予相應的左右輪參數,用以驅動 機器人左右輪之動作。 以上步驟對應以下之函數,定義這些函數為 f1 、f2 、f3 、f4 、f5 ,其中的輸入 與輸出分別為 x2(t)、x3(t)、x4(t)、x5(t),整個過程最終輸出為 x6(t),可用公 式 ( 3.1) 來加以描述,如圖 3.6所示。 x6(t) = f5(f4(f3(f2(f1(x1(t)))))) (3.1) 以下推導過程中有使用之回歸分析,其數學函式與實驗之差距的衡量指標為平均 絕對百分差 (mean absolute percentage error, MAPE),分別針對各函式加以描述:

• f1 :輸入 x1(t)為聲波與障礙物的來回的總時間,輸出為理論距離 x2(t)。 x2(t) = f1(x1(t)) = 340x1(t) 2 = 170x1(t) (3.2) • f2 :輸入 x2(t)為理論距離 f1(x1(t)),輸出為感測器讀數 x3(t),實際距離和 聲波訊號計算的結果可能有些微誤差,因此將兩者資料以散布圖表示並使用 多項式回歸,此函數可表示成 (式 3.3),如圖 3.7所示。 x3(t) = f2(x2(t)) = 0.00002(170x1(t))2 + 1.032(170x1(t))− 1.75 = 175.44x1(t)− 1.75 (3.3)

(40)

N a tio n al U niversity of Kao hsi un g y = 0.00002 x2+ 1.03226 x - 1.75148 Ϭ ϭϬϬ ϮϬϬ ϯϬϬ ϰϬϬ ϱϬϬ Ϭ ϭϬϬ ϮϬϬ ϯϬϬ ϰϬϬ ϱϬϬ ག ෳ Ꮤ ᠐ ኧ )Ϧ ϩ * ჴሞໆෳຯᚆ)Ϧϩ* 圖 3.7: 感測器讀數與實際量測值之回歸分析 (MAPE=0.2091) y = -0.0573x2+ 6.6181x - 83.97 Ϭ ϯϬ ϲϬ ϵϬ ϭϱ ϮϬ Ϯϱ ϯϬ ϯϱ ϰϬ ჹ ᔈ ف ࡋ ჹᔈຯᚆ;ϦϩͿ 圖 3.8: 對應方向角度與距離回歸分析 (MAPE=20.19) • f3 :輸入為 x3(t),輸出為機器人轉向的角度 x4(t),直走為 90 度能轉向的 角度由 0 至 180 度,如圖 3.8。由於 90 至 180 其輸入對應如同 0 至 90 度 (左 右轉的區別),為避免回歸分析有相同輸入不同解的狀況,僅取 0 至 90 度的 散布來描述函數 (式 3.4)。

(41)

N a tio n al U niversity of Kao hsi un g x4(t) = f3(x3(t))) =−0.06(175.44x1(t)− 1.75)2+ 6.62(175.44x1(t)− 1.75) − 83.97 =−10.53x1(t)2 + 1512.06x1(t)− 97.39 (3.4) • f4 :輸 入 為 x4(t),輸 出 為 經 由 PID 控 制 器 計 算 誤 差 得 出 所 期 望 的 角 度 x5(t)。根據前一章節之公式 ( 2.1),將本函數表示如下式 ( 3.5)。相關變數說 明: ◦ Kp :比例控制項參數。 ◦ Ki :積分控制項參數。 ◦ Kd:微分控制項參數。 ◦ Sp :使用者之設定值。 x5(t) = f4(x4(t)) = Kp(Sp− (−10.53x1(t)2+ 1512.06x1(t)− 97.39)) + Kit 0 (Sp− (−110.88x1(t) 2 + 1512.06x1(t)− 97.39)) + Kd d dt (−110.88(x1(t− 1) + x1(t))2+ 1512.06(x1(t− 1) − x1(t))) (3.5) • f5 :輸入 x5(t)為期望的角度 f4(x4(t)),藉由 PID 控制器得出的角度,來給 予相應的左右輪參數,此處為簡單描述取左右輪之差,根據對應角度之散布 圖以線性回歸 (圖 3.9) 來描述此函數,最後的輸出 x6(t),如公式 ( 3.6)。

(42)

N a tio n al U niversity of Kao hsi un g y = 1.1071x - 99.643 ͲϭϮϬ ͲϵϬ ͲϲϬ ͲϯϬ Ϭ ϯϬ ϲϬ ϵϬ ϭϮϬ Ϭ ϯϬ ϲϬ ϵϬ ϭϮϬ ϭϱϬ ϭϴϬ Ѱ ѓ ፺ ୖ ኧ ৡ ᙯ៻فࡋ 圖 3.9: 角度與左右輪差回歸分析 (MAPE=0) x6(t) = f5(x5(t)) = 1.11Kp(Sp− (−10.53x1(t)2+ 1512.06x1(t)− 97.39)) + 1.11Kit 0 (Sp− (−110.88x1(t)2+ 1512.06x1(t)− 97.39)) + 1.11Kd d dt (−110.88(x1(t− 1) + x1(t))2+ 1512.06(x1(t− 1) − x1(t))) + 99.64 (3.6) 由式 3.6觀察出此一函數為三次以上之多項式方程式,屬於非線性系統,若使 用線性假設前提下的 KF,用來做誤差修正可能效果較差,從數學模型來看,在 f2 各別使用 KF 修正可能可以得到較正確的 x3(t),但若針對 f4 或者整體的 f5等 明顯非線性的函式,KF 修正速度可能無法趕上非線性下的快速劇變,因而導致無 法收斂,或收斂時間過常導致整個機器人系統停滯,為解決此問題,此時就需要 使用非線性系統下也適用的 EKF 方法。

(43)

N a tio n al U niversity of Kao hsi un g

3.4

分析結果與討論

綜上兩節分析比較,對於機器人避障此問題可得以下結論: • 機器人避障使屬於非線性系統的問題,第一小節所探討的是不考慮雜訊與干 擾的情況下,推導結果明顯為非線性之函式,再藉由第二節之量測結果印證 數學模型。 • 當機器人在實際行動時,整個問題會變得比原本靜態更複雜,干擾與雜訊更 多,與原本的數學模型差異甚多,更加證明用在不穩定的環境下,無法有效 用線性的方式進行過濾。 • 感測器的結果會因實際上機器人動作、環境的干擾而使得系統相當不穩定, 非理論數學式所能完整描述,因此再加上機器控制會更加複雜,要盡可能減 少控制機器人的無差才能針對 EKF 與 KF 比較。 • 若要解決不穩定問題首先要先針對輸入來源改善,除了針對量測儀器的穩定 性改善,改善輸入的過濾修正方法必須可以用於非線性的問題,因此本研究 使用線性與非線性的方法加以比較。

(44)

N a tio n al U niversity of Kao hsi un g

第 4 章

研究平台說明

4.1

機器人軟硬體、外觀、控制方式

本研究避障時所用之設備為二輪型機器人,硬體主要由利基公司之輪型自走車 模組構成,軟體部分採用利基公司所提供之 BASIC Commander 語言透過 C 語言 能與硬體作溝通。機體前方、左側及右側各裝載利基之 Sonar A 超音波感應模 組,總共有三組,分別對應不同方向做偵測。兩個輪子分別使用兩組 360 度伺服 機來驅動,伺服機內部包含了一個小型直流馬達,決策計算部分由 BC1 單晶片、 Education Board 組成,BC1 與各模組均安裝於 Education Board 上,能儲存程式並

透過訊號線控制各模組的運作,機器人組裝後大小為 14 公分×14 公分 ×12 公分, 其外觀如圖 4.1所示。 本研究機器人使用超音波感測器獲得環境資訊,Sonar A 模組可透過訊號線連 接,藉由簡單的指令設定超音波環境,如離地面高度、聲波發射頻率、測量距離 單位回傳格式等。Sonar A 使用上有所限制,超音波測距器偵測距離不得大於五公 尺,另外障礙物也不可以過小,且偵測角度也不能小於 45 度角。研究中設定發送 頻率為每 100 毫秒一次。伺服機接上 Education Board 上的 P10、P11 腳位,控制 方式為透過指令分別給予兩腳位參數,由高速轉動的直流馬達提供原始動力,帶 動減速齒輪組產生高扭力的輸出。電壓 4.8V 時速度為 60.0(R/M),扭力為 3.3 (公 斤/公分);電壓 6.0V 時速度為 70.0(R/M),扭力為 4.8 (公斤/公分)。減速齒輪組由 馬達驅動,每個伺服機的控制會有少許的差距。所採用的軟硬體架構整合如下, 採用超音波測距提供機器人環境資訊,決策部分由燒錄在 BC1 的程式計算決定, 透過 Education Board 控制模組進行動作,其軟硬體架構如圖 4.2所示。

(45)

N a tio n al U niversity of Kao hsi un g (a) 前方 (b) 側面 (c) 上方 (d) 後方 圖 4.1: 兩輪型機器人實體照片 BC1 Education Board ՛ܺᐒ Sonar A Sonar A Sonar A ՛ܺᐒ P10 P11 P0 P1 P2 PC USB C/C++ BASIC Commander 圖 4.2: 機器人軟硬體架構圖

4.2

PID 控制器設定

本研究以 PID 作為基本的行動控制,因利基公司原廠伺服機上的馬達控制會有所 誤差,轉動幅度可能非使用者所期望之設定。例如使用者目標設定為 θs,然而即

(46)

N a tio n al U niversity of Kao hsi un g ϮϬ ϮϮ Ϯϰ Ϯϲ Ϯϴ ϯϬ ϯϮ ϯϰ ϯϲ ϯϴ ϰϬ Ϭ ϭ Ϯ ϯ ϰ ϱ ϲ ϳ ϴ ϵ ϭϬ A ng le Time ƚĂƌŐĞƚ ĐƵƌƌĞŶƚ 圖 4.3: PID 極限增益示意圖 表 4.1: 齊格勒尼科爾斯法調整變數 Type Kp Ki Kd P Ku/2 0 0 PI Ku/2.2 Tu/1.2 0 PID Ku/1.7 Tu/2 Tu/8 使透過正確指令給左右輪腳位 P10、P11 下達相應之值,機器人可能實際轉彎的角 度為 θr而非 θs,此誤差使用 e 來表示,根據 PID 原理透過調整與觀察每一時刻, 以減少此誤差 e 為目標,令機器人的控制轉彎角度能達期望之設定值。其中,PID 控制之參數調整方式為齊格勒尼科爾斯法 [24],觀察每一次機器人的轉彎角度輸 出,針對準確轉彎 90、60、30 度進行調整並使之收斂,按照下列步驟決定 PID 三 個控制項之參數,以下以 30 度的調整來說明: • 步驟一:調整比例控制項,將 Ki 和 Kd 設置為 0,Kp 從 0 開始逐漸增加, 觀察每一次的輸出值,當控制器輸出值達到極限增益 (以 Ku 表示) 停止 增加,此時控制器輸出值呈現恆定的大小振蕩 (振蕩週期以 Tu 表示),如 圖 4.3所示。藉由得到的 Ku。與振蕩周期 Tu,根據不同的 PID 控制類型, 按照表 4.1變數來設置比例、積分和微分參數。

(47)

N a tio n al U niversity of Kao hsi un g ϮϬ ϮϮ Ϯϰ Ϯϲ Ϯϴ ϯϬ ϯϮ ϯϰ ϯϲ ϯϴ ϰϬ Ϭ ϭ Ϯ ϯ ϰ ϱ ϲ ϳ ϴ ϵ ϭϬ A ng le Time ƚĂƌŐĞƚ ĐƵƌƌĞŶƚ 圖 4.4: PID 超調改善示意圖 ϮϬ ϮϮ Ϯϰ Ϯϲ Ϯϴ ϯϬ ϯϮ ϯϰ ϯϲ ϯϴ ϰϬ Ϭ ϭ Ϯ ϯ ϰ ϱ ϲ ϳ ϴ ϵ ϭϬ A ng le Time ƚĂƌŐĞƚ ĐƵƌƌĞŶƚ 圖 4.5: PID 快速收斂示意圖 • 步驟二:調整積分控制項,Ki 從 0 開始逐漸增加,觀察每一次的輸出值, 直到其超調 (overshoot) 的情況出現改善停止增加,此時控制器輸出值從恆定 的大小,變成由大而小逐漸往設定值靠近,如圖 4.4所示。 • 步驟三:調整微分控制項,Kd 從 0 開始逐漸增加,觀察每一次的輸出值, 使其能在短時間內收斂並穩定。設定標準為能在 5 次以內收斂,且設定值與 輸出值誤差小於 5 度,達收斂後如圖 4.5所示。根據此流程所調整之參數, 將調整過程的參數紀錄於下表 4.2。

(48)

N a tio n al U niversity of Kao hsi un g 表 4.2: PID 參數調整表 Type Kp Ki Kd P 0.500 0.000 0.000 PI 0.450 0.540 0.000 PID 0.650 0.351 0.162

4.3

KF/EKF 參數設定

本研究所使用之濾波器需視環境調整參數,已達成最佳修正預測。KF 與 EKF 採 用學者 J. V. F. Neto 之參數調整法 [39]。在模擬環境下幾乎不會受到環境的干擾, 所以 Qk值通常是很小的數值。但本研究在實際環境中運行,因此將參數 Qk設為 接近 1 較大的值,量測裝置的先假設為準確,並把參數 Rk設為接近 0 之值,以下 簡單描述調整的過程: • 假設此物理過程中環境的真實值為 Tx,最後濾波器輸出的與真實值得誤差 要盡可能達到最小。因定義誤差為 EkEk = Tx− ˆxk (4.1)

然後藉由圖表統計資料,使用平均絕對誤差(Mean absolute error,以下簡稱 MAE)小於 1 當作收斂標準,如下所示: M AE = 1 n nk=1 |Ek| (4.2) • 先根據環境設置參數,若沒有的則設為 0,預測值與假設的實際值不能差異 過大。因此將條件設置如下: Tx− x0 < 5 (4.3) • 基本設置完成後先調整量測相關的參數 Rk,先將此值設為接近 0 的數值

數據

表 5.6: 場景 B 動態避障統計表
表 5.8: 場景 D 動態避障統計表

參考文獻

相關文件

For the proposed algorithm, we establish a global convergence estimate in terms of the objective value, and moreover present a dual application to the standard SCLP, which leads to

In 2013, the author and an associate professor of Faculty of Education at the University of Hong Kong jointly published a paper in Gifted and Talented

Because simultaneous localization, mapping and moving object tracking is a more general process based on the integration of SLAM and moving object tracking, it inherits the

• Learn the mapping between input data and the corresponding points the low dimensional manifold using mixture of factor analyzers. • Learn a dynamical model based on the points on

Multiple images from a sequence tracked with 6DOF SLAM on a client, while a localization server provides the global pose used to overlay the building outlines with transparent

[3] Haosong Gou, Hyo-cheol Jeong, and Younghwan Yoo, “A Bit collision detection based Query Tree protocol for anti-collision in RFID system,” Proceedings of the IEEE

Kyunghwi Kim and Wonjun Lee, “MBAL: A Mobile Beacon-Assisted Localization Scheme for Wireless Sensor Networks,” The 16th IEEE International Conference on Computer Communications

D.Wilcox, “A hidden Markov model framework for video segmentation using audio and image features,” in Proceedings of the 1998 IEEE Internation Conference on Acoustics, Speech,