• 沒有找到結果。

行政院國家科學委員會專題研究計畫 成果報告

N/A
N/A
Protected

Academic year: 2022

Share "行政院國家科學委員會專題研究計畫 成果報告"

Copied!
42
0
0

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

全文

(1)

行政院國家科學委員會專題研究計畫 成果報告

智慧型聲控汽車導航與通訊系統整合研究(2/2)

計畫類別: 個別型計畫

計畫編號: NSC94-2213-E-216-004-

執行期間: 94 年 08 月 01 日至 95 年 07 月 31 日 執行單位: 中華大學機械工程學系

計畫主持人: 林君明

計畫參與人員: 江政嶽.黃安槿

報告類型: 完整報告

處理方式: 本計畫可公開查詢

中 華 民 國 95 年 10 月 20 日

(2)

中文摘要

本研究是利用數位訊號處理器(DSP)及卡門濾波器技術將慣性導航系統與全球衛 星定位系統整合之方法,運用在載具上做即時(Real-Time)的導航與定位。系統整合的方法,

整合全球衛星定位系統(GPS)、慣性導航系統(INS),與地理資訊系統(GIS)之導航系統,由 數位訊號處理器(DSP)作為運算核心,以卡門濾波器(Kalman Filter),進行信號的整合理 論為基礎,構建一系統階數較低,而容易實現的閉迴路分散式 (Loosely Coupled-Closed Loop) 複合式導航系統。並進行載具之平面即時定位,另一方面在整合 GIS 系統方面,主要是以 電子地圖為輔助,將即時運算的座標值,傳給所設計的使用者介面,即時的展示在電子地 圖上。而我們只要觀測地圖,就可獲知現在的所在位置。

本文中也有針對 GPS 脫鎖時的情況,進行 INS 系統的分析,發現只要 GPS 鎖定的 時間夠長,INS 系統的誤差就會很快的收斂。而當 GPS 脫鎖時,INS 系統的發散速率就不 會太快,所以如果 GPS 在短暫時間內脫鎖時,系統仍能維持不錯的導航運作。這也就是本 文為何要把 GPS 和 INS 系統整合在一起的緣故。利用語音控制板,整合 8051 控制馬達正 反轉,完成前近、後退,停止、左轉、右轉五個動作,製作ㄧ台簡易式聲控車,將導航系 統放置在上面,來模擬智慧型聲控導航系統的建構。

關鍵字:全球衛星定位系統(Global Positioning System ,GPS)、慣性導航系統(Inertial Navigation System,INS)、卡門濾波器(Kalman Filter)、閉迴路分散式

(Loosely-Coupled Loop)、地理資訊系統(Geographic Information System ,GIS)、

閉迴路分散式複合法(Loosely-Coupled Closed-Loop structure)、聲控(voice control)。

Abstract

The purpose of this research is by using DSP and Kalman Filter technology to Inertial Navigation (INS) System, Global Position System (GPS) as well as Geographic Information System (GIS) into a real-time vehicle navigation .The INS system is consisted by three accelerometer as well as three gyro-compass, and aided by GPS with loosely-coupled closed-loop structure, such that it can be easily implemented with lower order computation requirement and shows the result on GIS.

The loose track condition of GPS for about one minute is also included, and which shows that the resulting system can also provide navigation information. Combine 8051 and control the motor to imitate the constructing of the intelligent voice control navigation system.

Keywords: (Global Positioning System ,GPS)、(Inertial Navigation System,INS)、 (Kalman Filter)、(Loosely-Coupled Loop)、(Geographic Information System ,GIS)、( voice control)

ㄧ、前言

目前在 GPS 導航系統中,有單獨的 GPS 系統、DGPS、GPS/INS 複合式系統、及 GPS/DR 複合式系統,全球定位系統(Global Positioning System 簡稱,GPS)使用最為廣泛,這是因 為它具有全球性(Global)、全天候(All-weather)、精度高、即時定位速度快等多項優點,

而且可運用在許多不同方式之定位需要。由於 GPS 系統是接收衛星的訊號,所以會受天候、

(3)

環境、以及載具本身的運動及姿態的影響,而產生遮蔽、干擾甚至脫鎖,所以無法完全仰 賴 GPS 系統,提供不中斷的的導航資訊,是為最主要之缺點。

慣性導航系統(Inertial Navigation System,簡稱 INS ),是一自主式的電子導航系統,

一般是使用加速儀(Acce1erometer)與陀螺儀(Gyroscope)等感測器,利用地球的慣性重力與 載具相對運動,來感測載具之加速度及角速度。再利用慣性導航系統之動態方程式,就可 即時(Real-Time)求得載具之速度、位置的變化,而得到載具於慣性座標系統之定位資訊。

但由於 INS 所需要感測元件數目,如果要用作 3D 的導航上,GPS/INS 系統在做整合的精 度上的確可以達到更好的定位精度,成本上價格較高,是其主要的缺點。

慣性導航系統不會受環境、天候與地形等外在因素的影響,但其系統會發生漂移的現 象。在長時間的工作會使系統誤差增加,進而產生系統發散的現象。因此不適合做長時間 的定位,若要做長時間的使用,則需搭配其他的導航系統做校正的動作。

也因為有這個原因,所以可將全球定位系統與慣性導航系統作整合,選擇 GPS 定位系 統在一般性情況下有長時間穩定的特性,與慣性導航系統所具有自主、完整的特性,互相 搭配,故可應用於日常生活中的汽車定位導航系統設計,使之成為完整的電子式導航。

二、研究目的

目前全球定位系統 GPS (Global Positioning System)已佈置完成,並進行正式運作,所 以各式各樣的應用產品也都在開發之中,其中可配合一般用途極廣,而發展的就是「智慧 型聲控式汽車導航與通訊及多媒體系統的整合設計」。本計畫的目的是運用嵌入式 DSP,

將 GPS、慣性導航系統(Inertial Navigation System , INS),地理資訊系統(Geography Information System ,GIS)及 GSM(Global System for Mobile Communication )系統整合起 來,由於採用之嵌入式數位訊號處理器,其功能非常強大,而體積又小又便宜,非常有競 爭力,可將 GPS,GSM,GIS,INS 同時整合起來,所以比以往的研究,又向前邁進了一 大步。

運用此系統的優點在於,能及時將行車位置顯示出來,供給使用者了解附近的地理資 訊為何;並且也可回報行車中心,或是其他站台,作為行車班次調度、緊急情況處理及乘 客候車之參考。在嵌入式 DSP,可整合第 3 代行動通訊系統(Global System for Mobile Communication, GSM)。因此可以隨時上網,提供給乘客最新各類新聞及旅遊的網路資訊。

且本計畫將會加上智慧型聲控模組,除地理資訊系統之整合外,更供給了一般正常人在開 車中,只需要聆聽就可獲取目前的所在位置,可增加行車的安全與方便性。

INS 慣性導航系統,是利用微機電加速儀和數位羅盤(MEMS Accelerometer & Gyro Compass )組成,但是長期會有漂移的現象產生,因此需要與 GPS 接收機相互配合,來達到 準確定位的效果。使用嵌入式 DSP 將此 GPS/INS 配合卡門濾波器,並與電子地圖,和 GSM 行動通訊系統,以及多媒體影像壓縮整合成 GPS/INS/GIS/GSM/多媒體,此一多效能的系統。

本計畫擬研究發展之智慧型聲控式汽車導航與通訊系統整合架構,如下圖 1 所示。

(4)

圖 1 智慧型聲控式汽車之導航與通訊及多媒體系統整合架構圖

三、文獻探討

大部份的車輛導航系統(Vehicle Navigation System),均都採用全球定位系統(GPS),

作為車輛導航的輔助修正而慣性導航系統則為主體,其整合的方式有 GPS/INS、

DGPS/INS,使用的硬體方面,包括有磁羅盤(Magent Compass)、陀螺儀、加速儀等。

車輛導航使用方位推估器(Dead-Reckoning Method,簡稱 DR),或慣性導航系統(INS) 各有其優、缺點,而為一互補的關係。對於 GPS/DR 兩者的整合的文獻,利用卡門濾波器 (Kalman Filter),將所有資訊整合在一起,可使導航定位方面更加精確,更增加其附加價值。

在導航定位方面,為使其定位的精準度更加精確,其改進的方法也有如載波相位、差 分載波相位、及 FUZZY Control 等,但缺點是計算量較大,很難達成即時定位的需求。

語音辨識最主要的目的是希望電腦聽懂人類說話的聲音,進而命令微電腦執行相對應 的工作。一般語音辨識事先都儲存好的聲音樣本,然後當有輸入的測試聲音樣本就可進行 比對工作。比對完成後輸入一個它認為最“像"的聲音樣本序號,我們就可以知道使用者 的聲音代表何意,進而命令微電腦做事。語音辨識的應用的文獻,均是利用語音辨識製作 身體障礙者設備之應用,提供身心障礙者一個無障礙的環境。整合智慧型導航定位系統,

將可增加其實用價值。

四、研究方法

4.1 GPS (Global Positioning System):

全球定位系統(Global Position system ,簡稱 GPS)為美國國防部於 1973 年 5 月開始 發展的,是一套全球、全天候、24 小時的三度空間高精度定位系統,剛開始主要是為了軍 事上的定時、定位、導航用,於 1993 年 6 月部署完成,目前美國已開放 GPS 系統,免費 提供世界各國與民間其它用途之應用,如圖 2 所示。

(5)

圖 2 GPS 衛星及軌道分布圖

全球定位系統的架構,主要可分為三大部分:即太空部分(Space Segment),控制部 分(Control Segment),及使用者部分(User Segment),如圖 3 所示。

分別敘述如下:

圖 3 全球定位系統

運用衛星進行定位,可得到長時間的準確座標位置。所以本專題是利用 GPS,進行快 速而又方便的定位,而更要強調的是將進一步把 GPS 晶片整合於嵌入式系統中。

4.2 GPS 訊息輸出格式

(6)

衛星導航訊息(Navigation Message)架構如圖 4 所示:傳送頻率為每秒 50 位元,其 導航訊息資料主篇幅(Main Frame)之整個長度為 1500 個位元(Bits)。內容分為五個子 篇幅(Subframe),每個子篇幅長度為 10 個字元(Word),每個字元含 30 個位元,所以每 個子篇幅之長度為 300 位元。每 6 秒接收器即可收到一個子篇幅的訊息內容,而每一資料 框需花費 30 秒接收。每一總框資料由 25 頁資料框構成,共需 12.5 分鐘的時間。在此 25 頁中,子框一、二、與三是不變的。但子框四與五隨頁數不同而更動。

2

1 3

5 4

1 2 3 4 5 6 7 8 9 10

0.02 sec 30 sec

6 sec

0.6 sec

Subframes 4 & 5 have 25 pages 1 Frame

=5 Subframe

1 Subframe

=10 WORDS

1 WORD

=30 bits

圖 4 衛星導航訊息(Navigation Message)架構圖

本計劃所使用的是標準 NMEA 0183 格式的輸出訊息,也是國際上通訊、導航功用所制 定之標準輸出格式。格式中共有六組語句輸出,採用 ASCII 字元內容包含 GPGGA、GPGLL、

GPGSV、GPSGSA、GPRMC、GPVTG 六組資料,內含衛星定位資訊,經度及緯度、誤差資 訊、相對移動速度、精度等資訊。其中 GPGGA 資訊,為本研究中所使用的資訊。

4.3 INS 導航系統

慣性導航系統(Inertial Navigation System,INS)於 1950 年開發完成,是一個高性能、

獨立自主性的導航系統,利用地球的慣性重力運作的電子導航系統,不受天候、距離等限 制,也不受外來的干擾,而且,它不需借助外來的資料,完全靠本身之慣性組件

(Inertial-Components),它是由陀螺儀(Gyroscope)和加速儀(Acce1erometer)等感測器所構 成,將它裝在載具上,用來測量載具運動狀態,而根據載具的角速度和線性加速度隨時間 的變化情形,再利用系統的導航計算機來運算,就可以即時(Real-Time)得知載具現在的 位置、速度和姿態了,以獲得載具於慣性座標系統之導航資料。

慣性導航系統優於其他導航系統,可歸納出下列幾項優點:

(1) 可即時,連續地送出位置及速度的訊息。

(2) 為一自給式系統,因此無輻射,不受敵人及外在環境的干擾。

(3) 不需地面輔助站的幫忙,任何氣候、任何位置(在任一經緯度,包括極區)均能得 到導航資料。

(4) 無論飛行載具加速度多大,同樣能得到完整的導航資料,而其他如都卜勒導航等,

則另有限制,甚至全球定位系統也會因加速度太大而脫鎖。

(5) 慣性導航系統可提供位置、對地速度、航向及高度等,是運動載具傳統量測航向、

高度與位置最好的方法。

(7)

慣性導航系統亦有下列的缺點:

(1) 無論載具是靜止或運動的狀態下,其位置與速度的誤差會隨時間的增加而發散。

(2) 設備十分昂貴,維修比較困難。

(3) 起始值之正確性,直接而嚴重地影響慣性導航系統的精度,尤其是最初之方位,確 有失之毫厘差之千里的影響,因此初始校正非常重要,但若緯度超過75度,或在進 行中的載具,則校準不易且精確度會降低。

INS系統架構

慣性導航系統的理論基礎是依據,牛頓(Newton)於 18 世紀所提出的力學三大定律,一 般將慣性導航系統分為兩大類,其一是穩定平台系統(Stabilized Platform System),簡稱「環 架系統」,其二是固裝系統(Strap-Down System)。

(一)穩定平台系統(Stabilized Platform System)

主要是利用環架及陀螺儀所組成之控制迴路來隔絕載具運動,使平台保持適當姿態,

以建立穩定參考座標。其結構如圖 5 所示,乃利用陀螺儀來控制環架平台,以地面座標為 校正標準,當環架離開慣性水平位置成一小角度傾斜時,陀螺儀的檢測信號裝置(Pick-off coil)可偵測出陀螺儀外殼的傾斜角度,此信號經過放大後送到環架裝置的馬達,驅動馬達 使環架裝置再回到慣性水平位置,如此即可建立一穩定參考座標,隔絕載具運動造成之影 響。

此系統具有精度高的優點,因加速儀保持水平就可省掉重力加速度分量的影響,而感 測器的校準也只須藉由平台轉動即可,不需整個載具轉動或拆離載具來做校準,但其缺點 為環架結構的複雜性與它的高成本。此外,環架裝置的磁場效應也會有些微的影響。

圖 5 環架平台系統 (二)固裝系統(Strap-Down System)

將陀螺儀、加速儀直接固裝於載具上,由陀螺儀、加速儀直接量測載具之角速度、加 速度,經由計算機的運算,求出瞬時載具對慣性座標的關係,進而計算出載具運動的位置、

速度和姿態的變化,以作為導航參考之用,其系統架構見圖 6。它的優點有:結構簡單成 本低,容易安裝,且少了環架機構的磨損與磁場效應。但因陀螺儀、加速儀等隨著載具運 動,會產生不少的誤差,且座標的轉換是以陀螺儀量到之數值為參考值,因此對陀螺儀之

(8)

特別高,因其需利用即時計算器來計算龐大的 DCM(Direction Cosine Matrix)。目前因計 算機技術大有進步,所以使用固裝式者居多。一般就精度而言,「環架系統」比「固裝系統」

為佳,且環架式的陀螺儀比較不受外界環境的影響,可作較長時間的使用。但不管如何,

任何慣性導航系統皆有誤差隨工作時間增加而發散的先天缺點。不過,就慣性導航系統而 言,由於其在高頻響應的特性良好,所以其可在較短的工作時間及高動態的環境下,即時 的提供較正確的位置、速度和姿態的變化資料,達到即時導航的目的。

方向餘 弦元素 Ax

Ay Az

Gx Gy Gz

載具座標與 大地座標間 之方向餘弦

矩陣

姿態及方位角 計 算

位置

速度

姿態 沿載具軸

之加速度 分量

沿載具軸 測得之 角速率值

沿大地 座標之 加速度 分量

修正大 地座標

計 算 機 慣性元件

圖6 固裝式導航系統架構圖

如果導航系統完全只採用 GPS 訊號,則會因 GPS 接收機的不同,而有不同的誤差值。

例如一般商用的 GPS 接收機位置誤差較大(約為 30 公尺),且易受地形地物影響,而造成誤 差加大,或信號被遮蔽,而發生脫鎖的現象,使得導航精度及功能有所影響。所以本研究 是將 GPS 與 INS 的導航訊號作整合,使之成為一完整的導航系統,其硬體架構如圖 7 所示,

而實體情況如圖 8 所示。本系統首先將三軸向的陀螺儀及加速儀所組成的 INS,與 GPS 接 收機置於汽車載具上,並與 STC-VC33 模組作連接。利用 STC-VC33 擷取 INS 之角速度及 加速度,並轉換成數位的資料,傳送給 TMS320C6711 DSK。然後以四元素導航法則運算,

求出 INS 未修正前之位置、速度及方位角。並以串列方式,接收 GPS 接收機原始之資料(Raw Data),解算出位置、速度、GDOP 及其他相關資訊。最後再以卡門濾波器(Loosely

Coupled-Closed Loop)的方式,整合 INS 與 GPS,以得到載具的位置、速度、及方位角之最 佳估測值。

(9)

圖 7 GPS/INS 整合導航系統方塊示意圖

圖 8 GPS/INS 整合之導航系統硬體示意圖 4.4 慣性導航系統的運算流程及步驟

慣性導航系統主要是由三個陀螺儀和三個加速儀所組成,其安裝方式是以右手定則的 三個方向,如圖 9 所示,互相垂直排列,所以慣性導航系統共有 6 個感測軸。當載具運動 時,在載具上的陀螺儀會立刻提供三個軸向的角速度資料,而加速儀則提供三個軸向的加 速度資料。以這些資料和輸入的初始位置、速度、姿態等參考資料,經過計算機的運算之 後,就可以求出載具目前所在的位置、速度和姿態。

慣性導航系統的計算流程表示如圖 10,計算過程分為下列幾個步驟:

(a)設定滾轉角(Roll)、俯仰角(Pitch)及偏航角(Yaw)的初始值,並計算四元素的初 始值。將這些角度轉換成四元素(Quaternion)的主要目的是,預防cossin在某些角 度時的值為零並且又出現在分母,將導致一個無窮大的值,造成系統發散。

四元素與初始姿態之關係如下:

Ashtech GPS RECEIVER

ENGINE

POWER SOURCE

Inertial Sensor (Vibration GYRO*3) (Linear Accelerometer*3)

SIO Interface TMS320C6711 DSK

STC-VC33 SIO Interface

STC-VC33 DSP C6711

GPS 接收機

(10)

cos( )cos( )cos( ) sin( )sin( )sin( ) 2 2

2 2

2

1 2

ψ θ

φ ψ

θ

φ

+

=

q (1-1)

cos( )sin sin( ) sin( )cos( )cos( ) 2 2

2 2

2

2 2

ψ θ

φ ψ

θ

φ

+

=

q (1-2)

cos( )sin( )cos( ) sin( )cos( )sin( ) 2 2

2 2

2

3 2

ψ θ

φ ψ

θ

φ

+

=

q (1-3)

cos( )cos( )sin( ) sin( )sin( )cos( ) 2 2

2 2

2

4 2

ψ θ

φ ψ

θ

φ

q = (1-4)

其中

φ

,

θ

,

ψ

分別為初始滾轉角、俯仰角及偏航角

圖 9 滾轉(Roll),俯仰(Pitch),偏航(Yaw)示意圖

(11)

圖 10 慣性導航系統計算流程圖

(12)

(b)取得陀螺儀之量測值

系統每隔 0.3 msec時,依序讀取陀螺儀 1,2,3(或加速儀 1,2,3)之量測值,讀 取一次完整的資料共需 1.8 msec

(c)地球轉速補償計算

此步驟是輸入載具角速率、地球轉速、四元素而計算出地球轉速之補償值以供下列步 驟(e)中使用,說明如下:若為飛行載具,則由於地球的自轉,所以我們若以地球座 標來觀察,陀螺儀似乎有進動(precession)的現象,但事實不然,所以必須加以補償,

而使平台隨時都保持與地表水平,如此加速儀所測得的加速度才是在導航座標的加速 度分量。

⎥⎥

⎢⎢

⎥⎥

⎢⎢

⎡ Δ Δ

Δ

=

⎥⎥

⎢⎢

⎡ Δ Δ Δ

L L

ie

yaw pitch

roll

yaw pitch

roll

sin cos

/ / /

ω

0

θ

θ θ θ

θ θ

(1-5)

T1 =0 (1-6)

T2 =−q3×Δ

θ

yaw (1-7)

T3 =q2×Δ

θ

yawq4×Δ

θ

roll (1-8)

T4 =q3×Δ

θ

roll (1-9)

其中 Δ

θ

roll,Δ

θ

pitch,Δ

θ

yaw為陀螺儀每次中斷時的角度增量

Δ

θ

roll,Δ

θ

pitch,Δ

θ

yaw是Δ

θ

roll,Δ

θ

pitch,Δ

θ

yaw經過地球轉速補償計算後之值 T1,T2,T3,T4是四元素經地球轉速補償計算後之值

(d)將四元素作正常化(Normalize)運算 依理論四元素值有下列關係,

q12 +q22 +q32 +q42 =1 (1-10)

但由於計算次數的增加,而產生的截位誤差(Truncation error)和捨位誤差(Round-off) 將使得式(3-10)不滿足,所以計算一段時間後須再對四元素做正常化,以免誤差擴大。

d2 =q12 +q22 +q32 +q42 =1−∈ (1-11)

( ) (

2 2

)

1 2 3 2

1 1

1 q q d

q d q

qi′ = i = i −∈≈ i + ∈ = i − (1-12)

q′ 為正常化後之四元素值 i

(e)將姿態由載具(Body)座標轉換至切面(Tangent Plane)座標 我們輸入載具角速率、地球轉速補償值,經計算而得到四元素值。

姿態微分方程式為

q&bt = 21qbt

ω

tbb = 21qbt

ω

ibb21qbt

ω

itb (1-13)

(13)

ω

itb =

ω

ieb表地球轉速,式(1-13)右側之第二項為地球轉速補償,若無補償時則使用 式(1-14)

q&bt = 21qbt

ω

ibb (1-14)

式(1-14)的解可寫為 q(tt)=q(t)P(t,Δt)

(q1(t) ivq2(t) vjq3(t) kvq4(t))(P1 ivP2 vjP3 kvP4) + + + +

+ +

= (1-15)

其中Δt為每次中斷的時間,而P1,P2,P3,P4的求法如下:

S =Δθx2 +Δθ2y +Δθz2 (1-16)

P1 =1−S 8 (1-17)

2

(

12 48

) ( )

24

*

*

y z z y

x S

P =Δθ × − − Δθ Δθ −Δθ Δθ (1-18)

3

(

1 2 48

) ( )

24

*

*

z x x z

y S

P =Δθ × − − Δθ Δθ −Δθ Δθ (1-19)

4

(

1 2 48

) ( )

24

*

*

x y y x

z S

P =Δθ × − − Δθ Δθ −Δθ Δθ (1-20)

其中 Δθx*,Δθ*y,Δθz*為陀螺儀之前一次中斷時間內的角度增量。

其次計算轉換後之四元素值:

q1

(

tt

)

=q1

( )

t P1q2

( )

t P2 +q3

( )

t P3q4

( )

t P4 (1-21)

q2

(

tt

)

=q1

( )

t P2 +q2

( )

t P1+q3

( )

t P4q4

( )

t P3 (1-22)

q3

(

tt

)

=q1

( )

t P3q2

( )

t P4 +q3

( )

t P1+q4

( )

t P2 (1-23)

q4

(

tt

)

=q1

( )

t P4 +q2

( )

t P3q3

( )

t P2 +q4

( )

t P1 (1-24)

(f)取得加速儀之量測值與步驟(b)相同

(g)將四元素轉換成方向餘弦矩陣

( ) ( )

( ) ( )

( ) ( )

⎢⎢

+

− +

− +

− +

+

− +

=

2 4 2 3 2 2 2 1 2 1 4 3 3

1 2 4

2 1 4 3 2

4 2 3 2 2 2 1 3 2 1 4

3 1 2 4 4

1 3 2 2

4 2 3 2 2 2 1

2 2

2 2

2 2

q q q q q

q q q q

q q q

q q q q q

q q q q q q q

q q q q q

q q q q

q q q

Cbt (1-25)

或改寫成

⎥⎥

⎢⎢

=

zz zy zx

yz yy yx

xz xy xx t

b

C C C

C C C

C C C

C (1-26)

其中每個元素代表一座標軸在另一座標系統中之方向餘弦,例如C 、xx C 、xy C 為彈xz 體座標 X 軸在慣性座標中之三個方向餘弦,亦可視作載具座標 X 軸上單位向量在慣 性座標之三分量。

(14)

所以可將將其中的九個元素,依行或列組成三個行向量(Column vector):

⎥⎥

⎢⎢

=

zx yx xx

C C C

C1

⎥⎥

⎢⎢

=

zy yy xy

C C C

C2

⎥⎥

⎢⎢

=

zz yz xz

C C C

C3 (1-27)

及三個列向量(Row vector) R1 =

[

Cxx Cxy Cxz

]

R2 =

[

Cyx Cyy Cyz

]

(1-28)

R3 =

[

Czx Czy Czz

]

因行與列向量均正交座標軸上一單位向量之三向量,所以 CiCj =1 若i= j

C

i

C

j

= 0

i≠ (1-29) j i,j = x,y,z

同理

RiRj =1 若i = j

R

i

R

j

= 0

i≠ (1-30) j

所以各元素間並非完全獨立,事實上,九個元素中僅有三個元素獨立,所以三度空間 中,一座標之方位(Orientation)對另一座標而言僅有三個自由度。

(h)將載具的所有速度增量由載具座標轉換成切面座標

即以載具速度增量(載具座標)及方向餘弦矩陣為輸入,將載具的速度增量由載具座標 轉為慣性座標,並且累加起來,即可得出載具在慣性座標的速度增量及累積值,以供 計算導航位置及速度之用。

一般而言,載具之運動方程式為

⎩⎨

×

− +

=

=

v g

a v

v R

ωie

& 2

&

(1-31)

其中

R :為載具在慣性座標之位置向量 v:為載具在慣性座標之速度向量

a:為載具受接觸力(Contact force) 所生之加速度 g :為由地球引力(Gravitational force)所生之重力加速度

ω

ie:為地球自轉角速率

加速儀僅能測出接觸力所致之加速度,欲求全部加速度,必須考慮地球引力所生的影 響。而在式(1-31)中,向量 R 、v、 g 通常均由其在慣性座標中之分量表示,在固

(15)

裝式系統內,三個加速儀是直接安裝在載具上,故輸出之加速度a係為彈體座標(Body axes)上之三個加速度分量,故式(1-31)如果成立,則其每一向量均須以同一座標之 分量表示。

所以首先須將加速度a由載具座標轉換至慣性座標。對於不同參考點的座標系 統,彼此之間有一定的轉換關係,藉著這個轉換過程,可以將不同參考點的座標轉換 到相同的座標系統,以便作更進一步的計算,這種轉換的過程,謂之「座標轉換」。 一般而言,常見座標轉換的方法有二:其一是方向餘弦矩陣(Direction Cosine Matrix),其二是尤拉角(Eular Angle)。

在此,我們使用方向餘弦矩陣的轉換方法。即

Δvt =CbtΔvb (1-32)

其中t ,b分別表示慣性(切面)座標,及載具座標。

(i)計算慣性座標系的速度及位置

我們將輸入載具速度增量、慣性座標速度值,而求得中間點(t =tt/2)及終端

t =tt/2)之慣性導航座標的速度及位置。慣性座標系的導航方程式即為式

(1-31),而式(1-31)可 改寫成差分方程式

⎩⎨

Δ

×

− Δ + Δ

= Δ

Δ

= Δ

t v t

g t a v

t v R

ωie

2 (1-33)

中間點(t =tt/2)的速度位置為

( ) ( ) ( )

( ) ( ) ( )

⎩⎨

+ +

= +

Δ

× +

= +

Δ Δ

Δ

2 2

2 2

2

t m t

m

m t m

t v t R t

R

t t g t v t

v (1-34)

終端(t =tt)的速度位置為

( ) ( ) [ ( ) ( ) ]

( ) ( ) [ ( ) ( ) ]

⎩⎨

Δ Δ + + +

= Δ +

Δ +

− + +

= Δ

+ Δ Δ

t t t v t v t R t t R

t t

v t

g t v t t

v t ie t

2 1

2

2

(1-35)

(j)重力模式

輸入彈體的位置,經過重力模式的計算,即可得到在慣性導航座標下重力加速度的三 分量。重力在當地(Local)水平座標的北東下三個分量(N、E、D)可表示如下 gNL =Gc1×H (1-36)

gEL =0 (1-37)

gDL =Gc2×(1−Gc3×H) (1-38)

其中參數

Gc1 =1.63×10−8×sinL×cosL

Gc2 =9.7803522+0.05163042×sin2 L+2.294569×104×sin4 L

(16)

Gc3 =3.1570474×107 −2.1026532×109×sin2 L L :緯度

H :高度

將當地水平座標重力值轉換為切面座標(即導航座標)重力值,關係式如下:

⎥⎥

⎢⎢

Δ Δ

Δ

− Δ

Δ

− Δ

=

1 1

1

= ,

0

0 0

0

L L

L L L

L L

C g C

gt tL L Lt

cos

cos sin

sin l l

l

(1-39)

其中

L :初始緯度 0 Δ :緯度增量 L Δl:經度增量 而

Z X

R R L R

≈ − Δ

φ

(

R R

)

L0

R

Z Y

cos

≈ Δ

φ

l

H =

(

RX2 +RY2 +

(

RφRZ

)

2

)

12Rφ

RXRYRZ:彈體所在位置 R :地球半徑 φ

經由計算可知導航座標重力的三個分量為:

gtN = gNgD×ΔL (1-40)

gtE =−Δl×

(

gN sinL0 +gDcosL0

)

(1-41)

gtD = gN ×ΔL+gD (1-42)

式(1-40~42)可作為下次計算式(1-36~38)使用。

(k)收集下一時刻陀螺儀及加速儀之輸出資料及求出載具姿態由式 (3-21~24)可求得轉換後之載具滾轉角、俯仰角及偏航角。

( )

⎟⎟⎠

⎜⎜ ⎞

− +

= + 2

3 2 2 2 4 2 1

2 1 4 1 2 3

q q q q

q q q tan q

φ (1-43)

( (

2 4 1 3

) )

1 −2q qqq

=sin

θ

(1-44)

( )

⎟⎟⎠

⎜⎜ ⎞

− +

= + 2

4 2 3 2 2 2 1

4 1 3 1 2 2

q q q q

q q q tan q

ψ (1-45)

由式(1-35)亦可求得速度及位置 4.7 GPS/DR 複合式導航系統之理論

本研究是採用分散式閉迴路複合法作為系統架構。它是以三個單軸加速儀和三個單軸 陀螺儀,整合GPS的資料,加入到卡門濾波器中,以求得最佳之定位資訊。而後再顯示在

(17)

GIS系統上,以實現實際2D導航的複合式定位系統,如圖11所示之分散式閉迴路卡門濾波 器的整合方式。

圖11 分散式閉迴路卡門濾波器系統圖

GPS/DR的整合方式,主要是考慮各系統的特性,取各家的優點,達到互補的功能,其 特性如表1與表2所示。

表1 GPS優缺點比較表 表2 INS優缺點比較表 慣性導航系統的誤差動態方程式

根據牛頓力學定理,在以地球北、東、下(N,E,D)之導航座標下推導出來,如下:

ε δϕ ω ϕ δ

δ δ ρ δ

δϕ δ

ω δ

=

× +

=

× +

Δ +

×

− Δ

=

× + Ω +

&

&

&

v r r

g f v

v ( )

(1-46) 其中: δ 是載具在導航座標上的速度誤差向量 v&

δ 是載具在導航座標上的位置誤差向量 r&

δ ϕ

& 是載具在導航座標上的姿態誤差向量 Ω 地球自轉角速度向量

ω 地球自轉角速度在慣性座標系統的分量 Δ 是加速儀在導航座標上的誤差向量

f 是載具在導航座標上的比力(Specific Force,載具的加速度和所在位置的 重力加速度之比)

Δ 是重力加速度模式的誤差向量 g

ρ

是載具在導航座標上對地球的旋轉角速度向量 ε 是陀螺儀在導航座標上的誤差向量

而地球自轉角速度向量在導航參考座標上的向量Ω 則為:

(18)

⎥⎥

⎢⎢

Ω

− Ω

= Ω

L L

e e

sin cos

0 (1-47)

其中: L 是 INS 所在的緯度,Ω 是地球自轉角速率。 e

若考慮地球的經緯度及地球自轉的角速度向量,則載具在慣性座標系統的旋轉角速度 ω 可表示為:

ω

=Ω+

ρ

(1-48)

⎥⎥

⎢⎢

=

L L

L

sin cos

λ λ ρ

&

&

&

(1-49)

其中: λ為載具的經度(Longitude)。

式(4-1)中運算子(Operator)× 之定義如下

⎥⎥

⎢⎢

⎟=

⎟⎟

⎜⎜

×

⎥⎥

⎢⎢

0 0 0

x y

x z

y z

z y x

a a

a a

a a

a a a

(1-50)

由式(4-1~4)可得 INS 的狀態誤差方程式為:

δ

X&INS(t)= AINS

δ

XINS(t)+

η

INS(t) (1-51)

XINS =

[

rN,rE,rD,vN,vE,vDNED

]

T (1-52)

+ Ω

+ Ω +

Ω

+ Ω

+

Ω

+

Ω +

Ω

+

Ω

=

0 )

( 0

0 0

0 0 0

) ( 0 )

( 0 0

0 0

0 0

) ( 0 0

0 0

0 0 0

0 0

) ( 2 2

0 0

0 )

( 2 0

) ( 2 0 0

0 )

( 2 0

0 0

0 0

0 1

0 0

0

0 0

0 0

1 0

0

0 0

0 0

0 1

0

cL L

cL sL

L sL f f

cL R L

g

f f

cL R sL

g

f f

L R sL

g

cL L

cL sL

L sL

A

N E

N D

E D

INS

λ

λ λ

λ λ

λ λ

λ λ

λ λ

λ

&

&

&

&

&

&

&

&

&

&

&

&

&

&

&

&

&

&

(1-53)

ηINS =

[

0 0 0 ΔN ΔE ΔD εN εE εD

]

T (1-54)

式(1-53)中sLcL分別定義為:sLsin( L)cLcos(L)R 為載具與地心之距離, g 為重力加速度。

接著我們做整體性的考慮,考慮陀螺儀和加速儀的誤差分析。整個 INS 的誤差模型可 擴增(Augment)如下:

δ

X&INS = AINS

δ

XINS +

η

INS (1-55)

(19)

δ

X&GYRO = AGYRO

δ

XGYRO +

ω

GYRO (1-56)

δ

V&acc = &

δ

Vacc(Previous)+(Bacc + faccSFacc)T (1-57)

由式(1-55)、(1-56)、(1-57)可得

TURE TRUE TRUE

TRUE A X

X δ η

δ& = + (1-58)

其中

[

N E D N E D N E D vN vE vD N E D N E D

]

T

TRUE r r r v v v B B B SF SF SF

X = , , , , , ,

ϕ

,

ϕ

,

ϕ

,

ω

,

ω

,

ω

, , , , , , (1-59)

⎥⎥

⎥⎥

⎥⎥

⎥⎥

⎢⎢

⎢⎢

⎢⎢

⎢⎢

=

×

×

×

×

×

×

×

×

×

×

×

×

×

×

×

×

×

×

3 3 3 3 3 3

3 3 3 3 3 3 9 9

3 3 3 3

3 3 3 3 3 3

3 3 3 3 3 3

3 3 3 3 3 3

0 0 0

0 0 0

0

0 0

0 0 0

0 0 0

GYRO INS

TRUE A

I

f I A

A (1-60)

⎥⎥

⎢⎢

× =

D E N

f f f f

0 0

0 0

0 0

3

3 (1-61)

T

D GD

GD E

GE GE N

GN GN D

E N

TRUE t t t

Δ Δ Δ

= 2 2 2 0 0 0 0 0 0

0 0 0 0

0 0

2 2

2

, , , , , ), ( ),

( ),

( ,

, , , , , , ,

, ω

τ ω σ

τ ω σ

τ

η σ

(1-62)

由這些狀態方程式,我們就可以詳細且完整的分析出整個INS的誤差。

全球定位系統誤差動態方程式

GPS 誤差最主要的來源是接收機時鐘偏差量(User Clock Bias)和時鐘漂移量(User Clock Drift),所以我們在建立 GPS 的動態方程式時,就是以此兩項誤差為主要考量因素。

GPS 的動態方程式,可由 8 個分量來表示,前三個分量為x1,x2,x3,表示使用者的位 置。第四個分量為x4,表示使用者的時鐘偏差量(User Clock Bias)。x5,x6,x7,表示使用 者的速度分量。x 表使用者的時鐘漂移量(User Clock Drift)8 。所以其動態方程式可描述如 下:

(20)

f f

v v

v v

v v

p

x x

x x

x x

x x

x x

x x

x x

x x

τ η τ η τ η τ η η

⎟ +

⎜ ⎞

−⎛

=

⎟ +

⎜ ⎞

−⎛

=

⎟ +

⎠⎞

⎜⎝

−⎛

=

⎟ +

⎜ ⎞

−⎛

= +

=

=

=

=

8 8

7 7

6 6

5 5

8 4

7 3

6 2

5 1

1 1 1 1

&

&

&

&

&

&

&

&

(1-62)

其中

τ

v,

τ

f 是速度和時鐘漂移量的時間常數(Time Constant)

η

v,

η

f,

η

p是外加的輸入雜訊(Additive Random Noise)

上式可寫成狀態方程式,如下:

X&GPS = AGPSXGPS

( )

t +

η

GPS

( )

t (1-63)

其中 XGPS =

[

x1 x2 x3 x4 x5 x6 x7 x8

]

T (1-64)

η

GPS =

[

0 0 0

η

p

η

v

η

v

η

v

η

f

]

T (1-65)

⎥⎥

⎥⎥

⎥⎥

⎢⎢

⎢⎢

⎢⎢

×

= −

×

×

×

×

×

×

×

×

×

×

f v

GPS I

I

A

τ τ

0 1 0

1 0 0

1 0

0

0 0

3 1 4

1

1 3 3 3 4

3

3 1 4

1

1 3 3

3 4

3

(1-66)

又其中 0m

n × m

的零矩,I3 是3×3的單位矩陣 4.8卡門濾波器(Kalman Filter)

自從 1960 年卡門(Kalman)利用狀態空間法直接作系統之最佳估測,重解了韋納

(Wiener)濾波器的動態方程式後,造成控制系統理論方面的突破性進展,這種濾波器被 稱為卡門濾波器(Kalman filter)。卡門濾波器不僅可做動態系統的狀態估測,亦可做整個 估測系統的性能分析,所以卡門濾波器的成功發展對於解決控制系統方面的問題,有著莫 大的貢獻。

我們假設一個連續或數位(Continuous or Discrete)隨機系統(Stochastic System),它的系 統和量測模型(Plant and Measurement),可描述如下表所示:

表 3 卡門濾波器系統描述表

參考文獻

相關文件

Thus, the proposed approach is a feasible and effective method for process parameter optimization in MIMO plastic injection molding and can result in significant quality and

The final results of experiment show that the performance of DBR system declines when labor utilization increases and the CCR-WIP dispatching rule facilitate to

(1995), Land Mosaics: The Ecology of Landscape Ecology and Regions, Cambridge: Cambridge University Press. Weaver.(1979),Territory

二、 本計畫已將部分研究結果整理,發表於國際研討會(Chan, Y.-H., Lin, S.-P., (2010/7), A new model for service improvement design, The 2010 International Conference

This project is the optical electro-mechanic integration design and manufacturing research of high magnifications miniaturized size zoom lens and novel active high accuracy laser

Florida, R.,2002, The Rise of the Creative Class and How It's Transforming Work, Leisure, Community and Everyday Life, Basic Books, New York. (Ed), Toward Scientific

Some efficient communication scheduling methods for the Block-Cyclic redistribution had been proposed which can help reduce the data transmission cost.. The previous work [9,

With the advancement in information technology and personal digital mobile device upgrade, RFID technology is also increasingly common use of the situation, but for