簡易慣性導航系統對於軌跡預測之應用

國立交通大學

機械工程學系

碩士論文

簡易慣性導航系統對於軌跡預測之應用

Applying Simply Constructed Inertial Navigation System in

Trajectory Prediction

研 究 生： 顏翊凡

指導教授： 秦繼華 博士

簡易慣性導航系統對於軌跡預測之應用

Applying Simply Constructed Inertial Navigation System in

Trajectory Prediction

研 究 生：顏翊凡 Student：I-Fan Yen

指導教授：秦繼華 博士 Advisor：Dr. Jih-Hua Chin

國立交通大學

機械工程學系

碩 士 論 文

A Thesis

Submitted to Department of Mechanical Engineering

College of Engineering

National Chiao Tung University

in Partial Fulfillment of the Requirements

for the Degree of

Master of Science

in

Mechanical Engineering

June 2009

Hsinchu, Taiwan, Republic of China

簡易慣性導航系統對於軌跡預測之應用

研究生：顏翊凡 指導教授：秦繼華 博士

國立交通大學機械工程學系

論文摘要

本論文研究簡易慣性導航系統在車輛行走軌跡預測上之應用，從慣性導航系 統出發，結合車輛動態掌握車輛的軌跡及姿態，使用模擬軟體(MATLAB)驗證了 理論之可行性，並架設一組簡易慣性導航系統及數個軌道平台來實驗系統之準確 性，其中簡易慣性導航系統只需要一組陀螺儀以及兩組加速規，應用尤拉座標轉 換法提出了適用於此系統的數學座標轉換方法，計算上更容易，並且提出利用簡 易慣性導航系統的資訊應用於車輛軌跡預測之方法，經由實驗來證實此方法可以 迅速掌握車輛未來可能行駛之軌跡。

Applying Simply Constructed Inertial Navigation System in

Trajectory Prediction

Student：I-Fan Yen Advisor：Dr. Jih-Hua Chin

Department of Mechanical Engineering

National Chiao Tung University

ABSTRACT

This research is about applying SCINS (Simply Constructed Inertial Navigation System) in vehicle trajectory prediction, base on INS (Inertial Navigation System), combine with vehicle dynamic to know the vehicle’s trajectory and posture, use MATLAB to simulate and prove the theory, build up a SCINS and numbers of track to test the accuracy, only needs a gyro and two accelerometers, present a coordinate transform by Eular transform that can use in SCINS, and present a method that can predict vehicle’s trajectory by using the information from SCINS, and proved it by experimentation .

致謝

兩年的研究生涯光陰似箭，當初來實驗室報到的景相還好像昨天才發生，認 識了我的指導教授，秦繼華，也認識了所有實驗室的同學，不過心情卻有著巨大 的不同，當初選了車輛組這個領域，許多東西都是我沒有接觸過的，經過了許多 的努力終於到達今天這一步，其中有很多歡笑跟淚水，讓我成長了非常多。 一路走來有太多需要感謝的人，首先一定要感謝秦教授，他教給了我們很多 對於研究上的態度，他說：很多新穎的研究在一開始都是不被大眾所接受，所以 我不會對你們的想法有太多侷限，讓我印象深刻，他也給了我們很大的發展空 間，當然在其中也花了很多時間督促我們，教導我們；再來是我最重要的家人， 爸媽，我弟，我是南部的小孩，離開家鄉至北部求學多年，因為路程遙遠，所以 我很少有時間回家探望，但一路以來都是有他們的支持我才能順利完成學業；還 有元智大學的CH 學長，他是我 PIC 控制器的啟蒙恩師，在實驗途中給了我很多 幫助，沒有他的幫忙我就無法完成我的實驗；然後清大動機07 的大家，我們畢 業之後還能常常相聚真的很難得，也是有你們的陪伴讓我這兩年非常開心，你們 是我永遠的朋友；最後還要感謝我所有實驗室的同學，小風、mink、才琳、進益… 等，以及去年畢業的學長們，我們一起打球，一起討論實驗，一起出遊，一起實 驗室大掃除，一起吃尾牙，這些讓我的研究生涯與眾不同，我永遠不會忘記！謝 謝你們！

目錄

論文摘要...i ABSTRACT...ii 致謝... iii 目錄...iv 表目錄...vi 圖目錄...vii 符號說明...x 第一章 緒論...1 1.1 前言...1 1.2 文獻回顧...2 1.3 研究動機與本文架構...3 1.3.1 研究動機...4 1.3.2 本文架構...4 第二章 車輛動態數學模型...5 2.1 車體動態...5 2.2 輪胎模型...8 2.3 轉向模式...10 2.4 車輛整體動態...12 第三章 慣性導航系統...14 3.1 慣性導航系統原理與簡介...14 3.2 慣性導航系統的架構...16 3.2.1 環架式慣性導航系統...16 3.2.2 固裝式慣性導航系統...18

3.3 慣性導航系統數學模式...19 3.3.1 慣性導航系統之座標轉換...19 3.3.2 慣性導航系統運動方程式...24 3.4 簡易慣性導航系統...25 3.4.1 簡易座標轉換...25 3.4.2 軌跡預測...27 第四章 實驗與討論...29 4.1 模擬...29 4.1.1 模擬計算...29 4.1.2 模擬結果分析...32 4.1.3 模擬結果討論...41 4.2 實驗...43 4.2.1 簡易慣性導航系統平台...43 4.2.2 軌道平台...53 4.3 實驗結果分析與討論...58 4.3.1 實驗結果分析...58 4.3.2 實驗結果討論...68 第五章 結論與未來研究方向...71 參考文獻...72

表目錄

表 1.1 GPS與INS特性比較[19] ...3 表 4.1 實驗規劃表...29 表 4.2 模擬結果综合比較表...42 表 4.3 陀螺儀靈敏度表[20]...44 表 4.4 加速規靈敏度表[21]...45 表 4.5 SCINS靜止跳動值表 ...52 表 4.6 SCINS靜止感測誤差表 ...52 表 4.7 實驗結果綜合比較表...69

圖目錄

圖 2.1 車輛運動模型[14]...6 圖 2.2 輪胎側滑示意圖[14]...8 圖 2.3 車輛轉向幾何關係圖[14]...11 圖 3.1 慣性導航定位原理...14 圖 3.2 環架型慣性導航系統[12]...17 圖 3.3 環架式慣性導航系統原理...17 圖 3.4 固裝式慣性導航系統...18 圖 3.5 固裝式慣性導航系統原理...19 圖 3.6 載體座標系統相對於環境座標系統示意圖...20 圖 3.7 SCINS坐標轉換 ...26 圖 3.8 SCINS座標示意圖 ...26 圖 3.9 軌跡預測方塊圖...28 圖4.1 拋物線形模擬軌跡圖...31 圖4.2 半圓形模擬軌跡圖...32 圖 4.3 sin波形模擬軌跡圖...32 圖 4.4 拋物線形軌跡模擬預測結果圖(3 個時間點後)...33 圖 4.5 拋物線形軌跡模擬預測結果圖(6 個時間點後)...34 圖 4.6 拋物線形軌跡模擬預測結果圖(9 個時間點後)...34 圖 4.7 拋物線形軌跡模擬預測結果圖(12 個時間點後)...35 圖 4.8 拋物線形軌跡模擬預測誤差圖...35 圖 4.9 sin波形軌跡模擬預測結果圖(3 個時間點後) ...36 圖 4.10 sin波形軌跡模擬預測結果圖(6 個時間點後) ...36

圖 4.12 sin波形軌跡模擬預測結果圖(12 個時間點後) ...37 圖 4.13 sin波形軌跡模擬預測誤差圖...37 圖 4.14 圓弧形軌跡模擬預測結果圖(3 個時間點後)...38 圖 4.15 圓弧形軌跡模擬預測結果圖(6 個時間點後)...39 圖 4.16 圓弧形軌跡模擬預測結果圖(9 個時間點後)...39 圖 4.17 圓弧形軌跡模擬預測結果圖(12 個時間點後)...40 圖 4.18 圓弧形軌跡模擬預測誤差圖...40 圖 4.19 CINS結合軌道車示意圖 ...43 圖 4.20 陀螺儀(ADXRS300)圖[22]...44 圖 4.21 加速計(ADXL320)圖 [22] ...44 圖 4.22 陀螺儀電壓中心圖[20]...45 圖 4.23 SCINS模組圖 ...46 圖 4.24 SCINS模組分解圖 ...46 圖 4.25 YZ_PIC_PROTO Layout圖...47 圖 4.26 YZ_PIC_PROTO控制器圖 ...48 圖 4.27 RS232 對控制器之轉接頭 & 傳輸線圖 ...48 圖 4.28 RS232 對電腦USB之轉接頭圖...48 圖 4.29 控制器電源線圖...49 圖 4.30 SCINS系統圖 ...49 圖 4.31 SCINS感測器側面圖 ...50 圖 4.32 加速計靜止感測值圖...51 圖 4.33 陀螺儀靜止感測直圖...51 圖 4.34 SCINS與軌道車結合圖 ...54 圖 4.35 拋物線形軌道俯視圖...55 圖 4.36 拋物線形軌道斜視圖...55

圖 4.38 sin波形軌道斜視圖...56 圖 4.39 圓弧型軌道實驗圖...57 圖 4.40 SCINS紀錄軌跡(拋物線形) ...58 圖 4.41 SCINS紀錄軌跡(sin波形)...59 圖 4.42 SCINS紀錄軌跡(圓弧形)...59 圖 4.43 拋物線形軌跡實驗預測結果(3 個時間點後)...60 圖 4.44 拋物線形軌跡實驗預測結果(6 個時間點後)...60 圖 4.45 拋物線形軌跡實驗預測結果(9 個時間點後)...61 圖 4.46 拋物線形軌跡實驗預測結果(12 個時間點後)...61 圖 4.47 拋物線形軌跡實驗預測誤差圖...62 圖 4.48 sin波形軌跡實驗預測結果(3 個時間點後) ...63 圖 4.49 sin波形軌跡實驗預測結果(6 個時間點後) ...63 圖 4.50 sin波形軌跡實驗預測結果(9 個時間點後) ...63 圖 4.51 sin波形軌跡實驗預測結果(12 個時間點後) ...64 圖 4.52 sin波形軌跡實驗預測誤差圖...64 圖 4.53 圓弧形軌跡實驗預測結果(3 個時間點後)...65 圖 4.54 圓弧形軌跡實驗預測結果(6 個時間點後)...66 圖 4.55 圓弧形軌跡實驗預測結果(9 個時間點後)...66 圖 4.56 圓弧形軌跡實驗預測結果(12 個時間點後)...67 圖 4.57 圓弧形軌跡實驗預測誤差圖...67

符號說明

xLF F _{作用在左前輪之牽引力} yLF F 作用在左前輪之側向力 xRF F _{作用在右前輪之牽引力} yRF F 作用在右前輪之側向力 xLR F _{作用在左後輪之牽引力} yLR F 作用在左後輪之側向力 xRR F _{作用在右後輪之牽引力} yRR F 作用在右後輪之側向力 LL δ 左前輪轉向角 RL δ 右前輪轉向角 LR δ 左後輪轉向角 RR δ 右後輪轉向角 LF α 左前輪側滑角 RF α 右前輪側滑角 LR α 左後輪側滑角 RR α 右後輪側滑角 β 車輛質心側滑角 LF V _{左前輪速度} RF V _{右前輪速度}

LR V _{左後輪速度} RR V _{右後輪速度} V _{車輛質心速度} x V 車輛縱向速度 y V 車輛側向速度 f l 車輛質心與前輪軸距離 r l _{車輛質心與後輪軸距離} m _車體重量 D _車輪輪距 γ _偏駛率 V I 車輛質心轉動慣量 L w 左前輪旋轉角速度 R w 右前輪旋轉角速度 r 輪胎半徑

第一章

緒論

1.1 前言

本研究主要探討慣性導航系統(Inertial Navigation System)的應用，因此必須 先了解此系統的演進以及現在發展的狀況，慣性導航系統原先是發展應用於火箭 的導航，美國火箭先驅，羅柏特‧高達德（Robert Goddard），以早期的陀螺儀 進行實驗，之後INS也被應用於許多地方，例如太空梭，衛星，飛機，車輛等多 種載體之上。 典型的慣性導航系統由加速規以及陀螺儀組成，以微分方程式來運算所測得 之數據來估算出載體系統(好比車輛本身的座標系統)從一已知慣性系統(車輛行 駛之環境座標系統)出發後即時的位置與姿態，但由於加速計以及陀螺儀在量測 時一定都會有些許誤差，而這些誤差也會隨著INS 的使用時間累積，所以就衍 生出各種改進的方法；一方面感測器種類日新月異，出現了各種不同製程或是不 同量測方法的加速規與陀螺儀，當然精度也各有不同，甚至有些高精度的是屬於 軍方才能使用之管制品，近年來也因為微機電製程的發展出現了價位便宜而且體 積小的微機電式加速規與陀螺儀，因此在使用上可以因應各種不同的需求而採用 不同種類的感測器；另一方面也可以透過控制或是運算的方法來解決誤差累積的 問題，例如在路上可以用＂零速度修正＂（中間停下來修正INS 紀錄）之方法 來維持系統的準確，或是利用卡爾曼濾波器來做修正，還有許多研究與衛星導航 接受器（全球定位系統GPS）做結合，達到相輔相成的功效。另一方面慣性導航 系統的架構方法也有了許多的變化，除了最開始由環架式慣性導航系統演變至現

的架構，例如捨棄了陀螺儀，僅利用四到六顆加速規來計算座標轉換達到價格便 宜的目的。 然而本研究則是為了因應汽車工業的迅速發展，其中有一塊領域就是車輛行 駛的安全性，並且更注重的是車量的主動式安全，因此將慣性導航系統導入，提 出了新式的簡易架構來得到車輛的動態，進而預測車輛的行使軌跡來避免各種意 外發生。

1.2 文獻回顧

主動式安全系統的研究上人為的控制已經不再是直接對於輪子，而是使用所 謂的線傳控制系統(Steer-by-Ｗire Control System)，將控制的訊號經由處理器加入

主動安全系統給予的輔助訊號進而控制輪子的轉向達到較穩定安全的控制，2001 年Mammar，Sainte-Marie，Glaser[1]這三人提出一套主動轉向(active steering)的 演算法，當人為轉向過當(over-steer)時，可以藉由線傳控制系統來修正車輛行走 的軌跡。2003 年，Yih，Ryu，Gerdes[2][3]等人把全球定位系統(GPS)與慣性導航 系統(INS)整合到線傳控制系統中希望能達到更佳的控制，並使用腳踏車模型 (bicycle model)來表現車輛的動態模式做了一系列的實驗。 而一直以來人們對於車輛甚至於其他運輸運動器具(例如飛行器,機器人等) 的摸索最難得知的就是其位置與姿態，1995 年，Barshan 跟 Durrant-Whyte [4]提 出了將INS 架設於機器人上來計算機器人行動的位置以及其姿態，在這個領域 中有了新穎的作法，其中對於INS 應用於機器人有完整的研究，包括了 INS 的 架設和整體數學上的運算等，其中也對於感測器的誤差提出了誤差模式(Error model)來做補償，最後也將 INS 架設於車輛上做了實驗。由於 INS 的發展最初是

複雜的演算，因此到了2004 年，Peng 和 Golnaraghi[5]提出了一種新型的

INS(GF-INS)把原本其中的陀螺儀拿掉，只用了刻意排列過的四顆加速規來運算

車輛運行的軌跡，這在系統的架設以及花費上都省下了許多。然而INS 先天上

因為感測器有誤差累積的缺點，近年來許多INS 有關的研究都整合了 GPS 來補

正INS 的誤差，另一個角度 INS 也補足了 GPS 反應速度比較緩慢的缺點，由下

表1-1 就可以看出差別，在後面會更詳細的解說，2000 年的 Farrell JA, Givargis TD, Barth MJ[7]和 2003 年的 Farrell JA, Tan HS, Yang YC[8]等人，都是從事這方面的 研究並且有一定程度的研究成果。 表 1.1 GPS 與 INS 特性比較[19] 了解了車輛運行的歷史軌跡之後，當然主動式安全系統最終的目的還需要能 夠預測車輛未來的狀況，才能夠適時的提醒駕駛這甚至介入車輛動態的控制， 2006 年，交通大學機械所碩士孔俊凱[14]，提出車輛軌跡的預測方法與修正法， 整合工具機技術到車輛的軌跡修正上，發展出一套類似AFS 系統，從軌跡的角 度，提出兩階段的修正方式，分別修正具位置誤差與方向誤差，將車輛修正到原 來預定的軌跡上。

1.3 研究動機與本文架構

1.3.1 研究動機

綜觀主動式安全系統一路發展以來，其技術大多都是著重於車輛之穩定，透 過控制方式來達到安全的目的，而少部份研究如[14]的倫文中不僅利用工具機軌 跡修正的方法，應用於補償車輛偏駛誤差的修正，也提出了三種軌跡預測的方 法，企圖在穩定中預見不穩定，進而達到所謂的預防勝於治療，提供了新的研究 發展空間。 本文將以車輛軌跡為主要主軸，由於前面所述，想要先知，必須能夠掌握現 況，而車輛軌跡一直以來不易取得，所以將導入導航之概念來掌握車輛之動態， 並且將提出一種簡易之慣性導航系統，只需要由兩軸之加速規以及一軸之陀螺儀 組成，大幅減低數學運算，由其中感測器的資訊轉換成車輛的加速度、速度以及 位置，並且提出結合軌跡預測的方法來達到先知的目的。

1.3.2 本文架構

本文將分章詳細敘述研究，種點整理如下： 第二章『車輛動態數學模型』：藉著車輛動力學導出描寫車輛在平面運 動時，輪胎轉向角和所受之力相對於車輛速度、加速度及偏駛率間的數學關 係。 第三章『慣性導航系統』：詳細介紹導航系統的比較以及應用，也詳說了各 別慣性導航系統分類的架構模式，數學模式等，並提出本研究架構之簡易慣性導 航系統新的數學原理以及結合軌跡預測之方法。 第四章『實驗與討論』：針對新的簡易慣性導航系統提出實驗方法並於 模擬之後進行實驗，討論實驗與理論之誤差，並驗證此研究之可行性。

題提出未來研究之方向。

第二章 車輛動態數學模型

描述一個車輛的動態，主要可將車體分為幾個系統來探討，如：底盤系 統、懸吊系統、轉向系統、傳動及煞車系統等；駕駛者在操控車輛的行進時， 給予的命令是兩個自由度，分別為轉向角和前進後退，但實際上車體本身會 因為環境的因素，如：路面的顛簸，強風的擾動，輪胎的打滑及人為的不當 操作，而產生的六個自由度的動態行為，分別為：前進（Forward）、側移 （Lateral）、上下（Vertical）、翻滾（Roll）、顛簸（Pitch）及偏航（Yaw）。 故真實的車輛動態行為是一種在三度空間具有非線性的動態多質量 （Multi-Masses）系統。 本章推導的車輛動態模型，主要是探討車輛在平面上移動時的動態行 為，故不考慮懸吊系統及空間中（Z 方向）車體的動態，簡化原本繁複的汽 車模型，光就底盤部分及四個輪胎間的關係加以推導。

2.1 車體動態

一個機動車輛是由分佈於其外殼內許多元件所組成的，然而在許多的分 析中，所有的元件是一起運動的。因此可將其表示為一於質心位置，具有適 當集中質量及慣量性質的模型，並同時考慮四個輪胎的運動行為，這裡我們 所探討的汽車模型，是以前輪驅動及制動，後輪為從動的惰輪，因此可將車 輛運動時的速度及受力情形描繪如圖2.1 所示。

圖 2.1 車輛運動模型[14] 由牛頓第二定律可以簡單的將車輛運動分成平移和旋轉系統 [9][14] 。 平移系統－在一指定方向作用於一物體上之外力合等於其質量與這方 向加速度之乘積。 mar =

∑

Fur

(

) ( ) (

)

(

) (

)

x y x y x y y x a V w V V i V j k V i V j V V i V V j γ γ γ = + × = − + − × − = − − + ur r _& ur ur r r r r r & & r r & & 將ri和 jr方向的力分開來討論，可得 i r 方向分量：

(

)

cos cos sin

sin x y xLF L xRF R yLF L yRF R m V V F F F F γ δ δ δ δ − = + − − & （2.1） j r 方向分量：

( ) cos cos

sin sin

y x yLR yRR yLF L yRF R

xLF L xRF R m V V F F F F F F γ δ δ δ δ + = + + + + + & （2.2） （2.1）（2.2）整理後可得

( cos cos sin sin ) /

x xLF L xRF R yLF L yRF R y V F F F F m V δ δ δ δ γ = + − − + & （2.3）

( cos cos sin

sin ) /

y yLR yRR yLF L yRF R xLF L

xRF R x V F F F F F F m V δ δ δ δ γ = + + + + + − & （2.4） 旋轉系統－相對一固定軸、作用於一物體上之扭矩合等於其慣性矩與相 對於此軸轉動加速度之乘積。 Iαur uur=M =

∑

r Fr ur× 2 LF f D r =l i+ j uur r r 2 RF f D r =l i− j uur r r 2 LR r D r = − +l i j uur r r 2 RR r D r = − −l i j uur r r

(

sin cos

) (

cos sin

)

LF yLF L xLF L yLF L xLF L

F = −F δ +F δ i+ F δ +F δ j

uuur r r

(

sin cos

) (

cos sin

)

RF yRF R xRF R yRF R xRF R F = −F δ +F δ i+ F δ +F δ j uuur r r LR yLR F =F j uuur r RR yRR F =F j uuur r

(

) (

)

(

) (

)

cos sin sin cos

2

cos sin cos sin

2 V yLF L xLF L f yLF L xLF L yRF R xRF R f xRF R yRF R D I F F l F F D F F l F F l F l F γ δ δ δ δ δ δ δ δ = + + − + + + − − − &

可整理成

(

)

(

)

(

)

(

)

[ cos sin sin cos 2 cos sin cos sin 2 ]/ yLF L xLF L f yLF L xLF L yRF R xRF R f xRF R yRF R r yLR r yRR V F F l D F F F F l D F F l F l F I γ δ δ δ δ δ δ δ δ = + + − + + + − − − & （2.5）

2.2 輪胎模型

車輛直行前進行駛時，車輪的方向與車輪前進的方向一致，亦即車輪的 前進方向在車輪的旋轉面內。然而，當車輛有側向運動或偏轉運動時，車輛 相對於地面的前進方向未必在車輪的旋轉面內，此時輪胎就會有滑移角 （Slip Angle，α ）及側向力（Lateral Force，F ）的產生，側滑角定義為輪y

胎旋轉面方向和觸地面前進方向間的夾角，側向力則為垂直於輪胎旋轉面方 向的作用力，其間的關係如圖2.2 所示。 圖 2.2 輪胎側滑示意圖[14] 車身方向 V 觸地面前進方向 輪胎旋轉方向 α y F

求側滑角之前先要得到輪胎的速度分量，故先將四個輪胎的速度分解如 下所示 2 xLF x yLF y f x f D V V V V l V l γ γ β γ = − = + ≈ + （2.6） 2 xRF x yRF y f x f D V V V V l V l γ γ β γ = + = + ≈ + （2.7） 2 xLR x yLR y r x r D V V V V l V l γ γ β γ = − = − ≈ − （2.8） 2 xRR x yRR y r x r D V V V V l V l γ γ β γ = + = − ≈ − （2.9） 藉由（2.6）~（2.9）可以推導出四個輪子的側滑角如下所示[9] 1 1 tan tan 2 yLF x f f LF LF LF LF xLF x x V V l l D V V V β γ γ α δ δ β δ γ − − + = − ≈ − ≈ + − + （2.10） 1 1 tan tan 2 yRF x f f RF R R RF xRF x x V V l l D V _V V β γ γ α δ δ β δ γ − − + = − ≈ − ≈ + − + （2.11） 1 1 tan tan 2 yRF x r r LR LR LR LR xRF x x V _{V l l} D V V V β γ γ α δ δ β δ γ − − − = − ≈ − ≈ − − − （2.12） 1 1 tan tan 2 yRR x r r RR RR RR RR xRR x x V V l l D V _V V β γ γ α δ δ β δ γ − − − = − ≈ − ≈ − − + （2.13） 車輛做轉向運動時，會受到側向力的作用。對依固定輪胎負載而言，轉 向力係隨側滑角而增加。低側滑角時（5 度或更小），此關係為線性故轉向 力可以下式描述[11]

y F = −C_αα （2.14） 假設轉向角很小，且輪胎與地面無滾動阻力，即牽引力為零，則可整理 方程式如下 0 0 r yLR R LR x r yRR R RR x f yLF F LF x f yRF F RF x xLF xRF l F C V l F C V l F C V l F C V F F α α α α γ β δ γ β δ γ β δ γ β δ ⎛ ⎞ ≈ − _⎜ − − _⎟ ⎝ ⎠ ⎛ ⎞ ≈ − _⎜ − − _⎟ ⎝ ⎠ ⎛ ⎞ ≈ − _⎜ + − _⎟ ⎝ ⎠ ⎛ ⎞ ≈ − _⎜ + − _⎟ ⎝ ⎠ ≈ ≈ （2.15） 其中比例常數 C_α稱為“轉向剛度”（cornering stiffness）。轉向剛度與許 多變數相關，如：輪胎尺寸及形式（輻射與斜紋結構）、胎層數、胎線角度 及胎紋等均為重要變數，而速度並不會顯著地影響輪胎所產生的轉向力。

2.3 轉向模式

車輛在做轉向運動時，車體是繞著旋轉中心行駛，四個輪胎的瞬心落在 同一點，因外側車輪和內側車輪所行走的距離不一樣，在轉向時左輪和右輪 的轉向角不同，假設不考慮因為側向力及道路狀況而產生的側滑角影響，本 研究採用阿克曼原理（Ackerman Principle）所構成的轉向幾何，來決定駕駛 者輸入的轉向角，對應於左右輪轉向角度間的關係，其幾何關係如圖2.3 所 示[9]。

圖 2.3 車輛轉向幾何關係圖[14] 圖2.3 中符號所代表的意義分別為 L δ ：左前輪轉向角 R δ ：右前輪轉向角 L ：前後輪軸距 D ：車輪輪距 R ：車輛質心迴轉半徑 O：車輛迴轉中心 數學式參考文獻[11]表示如下 右轉時： 2 2 L R L D R L D R δ δ ≅ ⎛ ₊ ⎞ ⎜ ⎟ ⎝ ⎠ ≅ ⎛ ₋ ⎞ ⎜ ⎟ ⎝ ⎠ （2.16）

左轉時： 2 2 L R L D R L D R δ δ ≅ ⎛ ₋ ⎞ ⎜ ⎟ ⎝ ⎠ ≅ ⎛ ₊ ⎞ ⎜ ⎟ ⎝ ⎠ （2.17） 前兩輪之平均角度定義為阿克曼角（Ackerman angle） L R δ = （2.18）

2.4 車輛整體動態

為簡化模型，假設左輪轉向角等於右輪轉向角，且車輛之側滑角很小且 車輛縱向速度保持定值，則結合（2.15），可將（2.2）精簡成[15] ( )

x yLR yRR yLF yRF

mV β γ&+ =F +F +F +F （2.19） 整合（2.5）、（2.15）、（2.19）可得到 β、γ 的線性化狀態空間方程式如 下[15][16]： 2 2 2 2( ) 2( ) 1 2( ) 2( ) 2 2 R r F f F R x x R r F f R r F f v v x F x F f v C l C l C C mV mV C l C l C l C l I I V C mV C l I α α α α α α α α α α β β γ γ δ − − + − = + − − + ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ & & （2.20） 用 y x V V β = 代入（2.10）～（2.13）可將前後輪的側滑角表示如下[16]：

y f LF RF x y r LR RR x V l V V l V γ α α δ γ α α + = = − − = = （2.21） 結合（2.4）、（2.14）、（2.21）可以得到V&y的關係式如下[16]： 2( ) ) y 2 F y x V C V V m α α α α α δ γ ⎡− ⎤⎡ ⎤ =_⎢ − _{⎥ ⎢ ⎥}+ ⎣ ⎦ ⎣ ⎦ & R F R r F f x x C +C 2(C l -C l mV mV （2.22） 所得之方程式（2.20）、（2.22）即為一般常用之腳踏車模型（bicycle model）。

第三章

慣性導航系統

3.1 慣性導航系統原理與簡介

一慣性導航系統(Inertial Navigation System)在定義上主要包含了量測加速度 向量之裝置以及進行機分運算之裝置能夠達到導航的目的，一般利用加速規 (accelerometer)量測非重力之加速度和陀螺儀（gyroscope）量測載具旋轉之角速 度來定出加速度向量；而積分運算有些採用習見之運算處理器，有些則利用回授 控制機械平台方式來實現。 慣性導航系統的計算主要是利用力學方程式來推算出載具的位置以及姿 態。原則上，若導航系統可以隨時地量測出載具的加速度向量，就可以進行機分 得到速度向量，再經一次積分得到位置向量。圖3-1 表示了慣性導航定位的原理。 圖 3.1 慣性導航定位原理

∑

− = + = 1 0 0 cos k i i i k x S x θ (3.1)

∑

− = + = 1 0 0 sin k i i i k y S y θ (3.2) （x ,₀ y ）₀ ：在時間t 載體的起始位置 ₀ （x ,_k y ）_k ：在時間t 載體的位置 _i k S ：在時間t 載體的位移 _i k θ ：在時間t 載體的絕對角度 _i 一般來說，慣性導航系統會以當地水平座標系統來做定位，因此陀螺儀經過積分 推算出來的角度φ為位置向量與ｙ軸向量之夾角，故θ =₉₀o −φ 慣性導航系統具有多項優點包括高精度、自主性、擴充性。一般依精度可區 別成三等級：分別為戰術級(tactical-grade)、導航級(navigation-grade)及戰略級 (strategic-grade)。戰術級的精度一般在 10~20 海哩/小時，也就是每經過一小時的 使用會累積10~20 海哩的誤差，此等級主要應用於戰術飛彈，且較為輕巧。導航 級則約為1 海哩/小時，通常使用於民航機，而戰略級的精度更達到了 1 英尺/小 時，應用於長程飛彈或是軍用飛機居多。 在進行定位演算的時候所需要的資料都是由本身感測元件而來，不需要外來 資料，不受外界環境干擾，所以有高度的自主性。除了不受干擾以外，慣性導航 系統也不用仰賴外來的無線電修正，不受天候影響，也因此具有隱密的特性不易 被偵測。但是也並非沒有缺點，最主要就是因為內部的加速規和陀螺儀等感測器 在量測時會產生誤差，這些誤差還會隨著積分運算而持續的累積而不利於長時間 的使用，這點就不如全球定位系統(GPS)的誤差屬於絕對量測誤差不受使用時間 的影響。 另外由於慣性導航系統啟動時需要先對感測器載體座標系統與當地水平座 標系統做差異上的矯正，所以無法瞬間啟動，與GPS 比較起來為一缺點，不過 慣性導航系統在短時間內能有相當高的精確度，而且由於輸出資料速率很快，

GPS 則需要較久的時間計算衛星訊號，僅能以１Ｈz 輸出定位資料，所以慣性導 航系統能有較大的動態範圍，仍是較GPS 導航為佳。所以在此可回顧表 1-1 能 夠更清楚的了解慣性導航系統與全球定位系統的優缺點差異，因此也造就了INS 可以與GPS 甚至是其他導航系統結合來達到精確導航的擴充性。

3.2 慣性導航系統的架構

慣性導航系統的實現方式可以分為兩類：一種形式是環架式(gimbal)，另以 種則為固裝式(strapdown)。環架式系統對於加速規以及陀螺儀的規格需求較低， 比固裝式精度要來的高也較易校正對準，而固裝式系統則具有低成本，高可靠度 以及機構簡單之優點。

3.2.1 環架式慣性導航系統

所謂的『導航』就是希望能夠了解載具在環境座標系統中移動的距離以及載 具的位置，因為載體的座標系統和環境的座標系統是互相獨立的，所以在運動時 加速規所測得的數據還必須經過處理才能成為我們需要的資訊，環架式的慣性導 航系統在架構上採用一個穩定平台(stable platform)並且在平台上裝置加速規以 及陀螺儀。圖3-2 為一環架型慣性導航系統的概念圖。

圖 3.2 環架型慣性導航系統[12] 當載具轉動時利用平台上之陀螺儀可偵知載具的姿態，此資訊經由回受控制 可驅動平台上之控制器，使的平台上的載具座標系統能夠隨時與環境座標系統保 持同步，因此加速規所測得之加速度可直接進行積分來細算速度與位置。圖3-3 即為慣性導航系統回受控制之方塊圖。但由於使用了較複雜之機械結構以及許多 司服馬達所以體積上較為龐大，維護上也較為不便，且較為耗電，價格上也比較 昂貴。 圖 3.3 環架式慣性導航系統原理

3.2.2 固裝式慣性導航系統

固裝式導航系統是日漸普及的作法。所謂固裝式就是免去了環架式中的環 架機構，只剩下一個放置慣性元件的平台直接裝至於載具上，並且有系統地將元

件的感測軸互相垂直裝置，構成一個載體座標系統。圖3-4 即為固裝式慣性導航

系統架設之概念圖。近年來因微機電(Micro Electro-Mechanical System, MEMS) 技術的進步，利用微機電製程所設計製造的慣性元件與傳統的慣性元件相比起來 已經大幅的減少體積也提高精度，更有利於固裝式系統的發展。 圖 3.4 固裝式慣性導航系統 固裝式系統成為趨勢也由於近年來微電腦技術的進步，對於數學式運算的能 力大幅提升，所以固裝式與環架式最大的差異就在於固裝式使用了大量座標轉換 矩陣來替代環架式的機械環架結構。在運動的過程中，陀螺儀量測經由積分得到 載具的姿態則可用來計算載體座標系統與環境座標系統的轉換矩陣，就好比一個 虛擬的環架，使的加速規的讀值可以正確的用來作為導航之用。圖3-5 為固裝式 慣性導航系統運算流程之方塊圖。也因為如此固裝式系統在計算上較為複雜，且

圖 3.5 固裝式慣性導航系統原理

3.3 慣性導航系統數學模式

3.3.1 慣性導航系統之座標轉換

本文將設計一固裝式導航系統，因此有必要對於加速規感測值進行座標轉 換。常見的座標轉換計算方法有方向餘旋矩陣(direction cosine matrix)、尤拉角 (Euler angles)、四元素法(quaternions)、旋轉向量(rotation vector)、及 4×4 齊次 (homogeneous)等轉換。後面將使用尤拉角與四元素法之座標轉換[15][17][18]， 方法詳細說明如下：

圖 3.6 載體座標系統相對於環境座標系統示意圖 圖3-6 表示了一載體在環境座標之中的相對關係。首先假設一載體座標 xyz 在初始狀態與環境座標x₀y₀z₀重合，經過一段時間之後載體運動後新的載體座標 為x'''y'''z'''，而之間三個軸分別歷經α、β、γ 角度的轉動，因此可以求出一個轉換 矩陣C，使得 ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ''' ''' ''' z y x C z y x (3.3) 而所謂的尤拉角實係一系列之旋轉角度，為了推導出C，則依照三次轉動三 個軸，分別求出個別的轉換矩陣，再將三個轉換矩陣組合成尤拉角轉換矩陣。 第一步將載體座標系統 xyz 繞x軸依逆時針方向旋轉一角度α，得到轉動後

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ' ' ' cos sin 0 sin cos 0 0 0 1 z y x z y x α α α α (3.4) 其中轉換矩陣C_α = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − α α α α cos sin 0 sin cos 0 0 0 1 (3.5) 第二步將載體座標系統 ' ' ' z y x 繞y 軸依逆時針方向旋轉一角度 β，得到轉動後' 的座標x''y''z''，如圖3-6 橘色所示，則可得到 ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ '' '' '' ' ' ' cos 0 sin 0 1 0 sin 0 cos z y x z y x β β β β (3.6) 其中轉換矩陣C_β = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − β β β β cos 0 sin 0 1 0 sin 0 cos (3.7) 第三步將載體座標系統x''y''z''繞z 軸依逆時針方向旋轉一角度 γ，得到轉動'' 後的座標x'''y'''z'''，如圖3-6 藍色所示，則可得到 ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ''' ''' ''' '' '' '' 1 0 0 0 cos sin 0 sin cos z y x z y x γ λ λ γ (3.8) 其中轉換矩陣C_γ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − 1 0 0 0 cos sin 0 sin cos γ γ γ γ (3.9) 由式(3.3)、式(3.5)、式(3.6)可以得到 C =C_α C_β C (3.10) _γ = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − α α α α cos sin 0 sin cos 0 0 0 1 ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − β β β β cos 0 sin 0 1 0 sin 0 cos ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − 1 0 0 0 cos sin 0 sin cos γ γ γ γ

= ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ + − − − + − β α γ β α γ α γ β α γ α β α γ β α γ α γ β α γ α β γ β γ β cos cos sin sin cos cos sin cos sin cos sin sin cos sin sin sin sin cos cos cos sin sin sin cos sin sin cos cos cos 代回式(3.1)即可得 ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ z y x = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ + − − − + − β α λ β α λ α γ β α γ α β α γ β α λ α λ β α λ α β λ β λ β cos cos sin sin cos cos sin cos sin cos sin sin cos sin sin sin sin cos cos cos sin sin sin cos sin sin cos cos cos ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ''' ''' ''' z y x 而一般為了確保尤拉角之唯一，又要求 −π ≤α ≤π， 2 2 π β π _{≤ ≤} − ，−π ≤γ ≤π 另外當考慮環境座標有相對於載體座標有一相對角速度ω，我們可以用下式表示 c e ω = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 3 2 1 ω ω ω (3.11) 其中令 c e ω 為環境座標系統相對於載體座標系統的角速度向量於環境座標系統 之表示式。 根據尤拉角之定義可知 c e ω = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 3 2 1 ω ω ω =C_α ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 0 0 α_& + C_α C_β ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 0 0 β& + C_α C_β C_γ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ γ_& 0 0 = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − 1 0 sin 0 cos sin cos 0 sin cos cos β γ γ β γ γ β ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ γ β α & & & 或 ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ γ β α & & & = β cos 1 ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − − β β γ β γ β γ β γ β β cos sin sin sin cos 0 cos cos cos sin 0 sin cos ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ 3 2 1 ω ω ω (3.12)

接著說明四元素法，其實四元素法是衍伸尤拉角而來，採用四個變量而得到 很簡化的姿態修正方程式，並個別彌補了方向餘旋變數過多(高達 9 個)以及尤拉 角在 2 π β = 時會有奇異點(singularity)的產生之缺點。 若於載體座標系統定義一旋轉軸並進行一特定角度之旋轉，則轉換至環境座 標系統，令此特定角度為Ω ，而φ、θ 、ψ 各別為旋轉軸與體座標系統x、y、z 軸之夾角，定義四元素為[18] 2 cos 0 Ω = q cosφ 2 sin 1 Ω = q cosθ 2 sin 2 Ω = q cosψ 2 sin 3 Ω = q 而採用四元素表示時的轉換矩陣C為 ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ + − − − + + − + − − − + − − + = 2 3 2 2 2 1 2 0 1 0 3 2 2 0 3 1 1 0 3 2 2 3 2 2 2 1 2 0 3 0 2 1 2 0 3 1 3 0 2 1 2 3 2 2 2 1 2 0 ) ( 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 C (3.13) 其中需注意不要誤以為四元素具有四個自由度，因為他們必須滿足以下限制 q₀2 +q₁2 +q₂2 +q₃2 = 1 至於兩座標系統如果有相對轉動時其姿態方程式以四元素法表示則為 q dt dq ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − − − − − − = 0 0 0 0 1 2 3 1 3 2 2 3 1 3 2 1 ω ω ω ω ω ω ω ω ω ω ω ω (3.14) 其中 ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = 3 2 1 0 q q q q q

3.3.2 慣性導航系統運動方程式

有了座標轉換之基礎後，我們必須考慮載體座標的運動，現在考慮兩個座 標，如圖？所示，令於空間中任何一點P 在載體座標系統中的座標為

[

]

T r r r_{1 2 3} ，我們可利用由原點O 至 P 之向量 rv 予以描述，而 rv 可表示為 c rv =r₁cv₁+r₂cv₂ +r₃cv₃ (3.15) 此時速度向量為

vv =r&₁cv₁+r&₂cv₂ +r&₃cv₃ (3.16) 加速度向量為

av =r₁&&cv₁+r&&₂cv₂ +r&&₃cv₃ (3.17)

而P 在環境座標系統中的座標為

[

]

T s s s_{1 2 3} ，可利用由原點O 至 P 之向量 sv 及_e c O 至O 之向量 Re v 予以描述，而 sv 可表示為 sv =s₁ev₁+s₂ev₂ +s₃ev₃ (3.18) 並滿足 rv =Rv + sv (3.19) 若環境座標系統對於載體座標系統有一相對角速度ω ，則 i _i e dt de × =ωv (3.20) 則向量 sv 亦隨之轉動，即微分一次得到 = dt s dv 3 3 3 3 2 2 2 2 1 1 1 1e se s e s e s e se

s&v + &v + & v + &v + & v + &v (3.21) 或者可表示為 = dt s dv s se v v &v +ω× (3.22) 其中s&v_e =s&₁ev₁+s&₂ev₂ +s&₃ev₃，並且

= dt r dv ₊ R& s&v_e+ωv×sv (3.23) 再對 sv 進行二次微分得到 ₂ = 2 dt s d v ) ( 2 s s s

s&&v_e+ ωv×&v_e+ω&v×v+ωv× ωv×&v (3.24) 其中s&&v_e =s&&₁ev₁+s&&₂ev₂ +s&&₃ev₃，並且

₂ =

2

dt r d v ₊

R&& s&&v_e+2ωv×s&v_e+ω&v×sv+ωv×(ωv×s&v) (3.25) 上式中的 R&& 為環境座標系統原點之加速度，s&&v 為 p 在環境座標系統內之加速度，_e

e s&v v × ω 2 為角速度與直線速度耦合之柯氏(Coriolis)加速度，ω&v ×sv則為角加速度， 而ωv×(ωv×s&v)則為離心加速度。

3.4 簡易慣性導航系統

3.4.1 簡易座標轉換

本研究所架構之簡易慣性導航系統(Simply Constructed Inertial Navigation System, SCINS)將針對二維平面運動之載體來設計，所使用之環境座標即為本地 水平漫遊方位系統，所以可以將載體系統也就是慣性感測系統視為與環境座標系 統無相對之角速度，並假設車輛不會有翻滾（Roll）和顛簸（Pitch）之動作，因 此考慮一個二維i_x i 載體座標系統，一旋轉軸_y w_z各別與i 軸_x i 軸互相垂直，而環_y 境座標系統則以三維xyz三軸表示，如圖 3.7 所示，因此簡化出一非常簡易之轉 換座標：

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ z y x i i i z y x a a a a a a 1 0 0 0 cos sin 0 sin cos

θ

θ

θ

θ

(3.26) 其中θ 即可由陀螺儀於轉軸wz量測出角速度然後積分而得到。位置之計算可再經 由式(3.1)、式(3.2)利用前述之θ 經由座標轉換之後實現；簡易慣性導航系統座標 之示意圖則如圖3.8 所示，其中上方的方塊為單軸微機電式陀螺儀，負責量測出 z w 之感測值，而下方的方塊為雙軸微機電式加速計，負責量測i 及_x i 之感測值。 _y 圖 3.7 SCINS 坐標轉換

3.4.2 軌跡預測

以往之慣性導航系統都用於記錄行使資訊，並且已有充分之研究，而本研 究之SCINS 之最終目的是不同的，而是掌握載體的運動狀態用來預測載體未來 行進的模式，進而應用於現在汽車工業迅速發展之主動式安全系統；假設人在控 制車輛的時候仍有某一定的連續性及規則性，故其行駛之路線也必為連續可微分 路徑，為求得精確之預測軌跡，在[14]中提出了外插軌跡預測法、外插軌跡預測 誤差補償法、循圓軌跡預測法，皆藉由過去行車的資訊：點座標、偏駛角來預測 未來之軌跡，但過去的行車資訊必定會經過計算累積多層誤差，然而本研究所提 出的SCINS 不同之處在於可知道載體行駛的瞬間資訊：加速度、角速度，而經 由此資訊可提出一預測軌跡的方法，並由模擬不同的軌跡來比較預測之精準度； 而且此方法是利用了即時的運動狀態來計算，因此不會有誤差累積的問題。圖 3.9 即為如何利用感測器之資訊預測軌跡之方塊圖。 至此，只要得到了載體當下的角度、速度以及加速度即可藉由運動學中之運 動方程式於環境座標系統中得到車輛未來之軌跡。 車輛在環境座標系統中之加速度可以如式(3.26)表示成：

⎥

⎥

⎥

⎦

⎤

⎢

⎢

⎢

⎣

⎡

⎥

⎥

⎥

⎦

⎤

⎢

⎢

⎢

⎣

⎡

−

=

⎥

⎥

⎥

⎦

⎤

⎢

⎢

⎢

⎣

⎡

=

z y x i i i z y x

a

a

a

a

a

a

a

1

0

0

0

cos

sin

0

sin

cos

θ

θ

θ

θ

(3.27) 速度可表成：

⎥

⎥

⎥

⎦

⎤

⎢

⎢

⎢

⎣

⎡

=

=

∫

z y x

v

v

v

adt

v

_(3.28) 而可利用運動方程式表示下式:

=

[

cosθ sinθ 1

]

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ = p p p z y x P ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ z y x v v v t 2 1 +

[

cosθ sinθ 1

]

⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ z y x a a a 2 t ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ + + + = 2 2 2 2 1 sin 2 1 sin cos 2 1 cos t a t v t a t v t a t v z z y y x x θ θ θ θ (3.29) 式(3.29)即表示了在未來 t 時間時車輛在環境座標系統中之位置。所以 t 即為實驗 時的控制變數（sampling time），只要將 t 修改，預測的時間就會改變，例如t=0.3 則表示預測出未來0.3 秒時的軌跡。 圖 3.9 軌跡預測方塊圖

第四章 實驗與討論

本研究將先使用模擬軟體 MATLAB 2007a 架構出如圖 3.9 虛擬之慣性導航系 統來模擬軌跡預測之準確性，架構方法將使用MATLAB 之程式語言撰寫程式， 此程式大約300 行，所需之電腦硬體效能為現行之一般通用個人電腦都可以達到 所需。 實驗整體規劃如表 4.1(實驗規劃表)，第一小節中(4.2.1)將架設一組簡易慣性 導航系統，於控制器上架設一組加速計以及一組陀螺儀來得到所需要的量測資 訊，此量測資訊再經由RS2325 轉接頭由電腦讀取並加以分析處理，第二小節 (4.2.2)會以三組滑軌平台實驗，軌道分別將架設成如同模擬時之軌跡，然後將慣 性感測器架設於平台之上實驗，與模擬結果比較，並經由測試來驗證慣性感測器 之準確度以及最終預測結果之準確性。 表4.1 實驗規劃表 實驗規劃表 章節 規劃步驟 對應章節 4.2.1 架設簡易INS 系統 3.4 理論提出簡易INS 系統 測試簡易INS 系統之穩定及可用性(圖 4.31~4.32) 4.2.2 架設軌道平台(圖 4.34~4.38) 4.1.1 模擬軌跡(圖 4.1~4.3) 將簡易INS 系統裝設於軌道車上(圖 4.33) 然後軌道車置於軌道平台上做實驗 4.3.1 實驗結果分析 4.1.2 模擬結果分析 4.3.2 實驗結果討論 4.1.3 模擬結果討論

4.1 模擬

4.1.1 模擬計算

此模擬主要在於驗證本論文軌跡預測方法之準確性，因此將使用三個已知 的模擬軌跡來反推出簡易慣性導航系統中感測器之量值，由所得到之模擬值來模 擬軌跡預測，即可知道模擬軌跡預測與模擬軌跡之誤差，之後再與實驗之結果做 比對，三個軌跡分別為：拋物線軌跡 2 x y= 、sin 函數軌跡 y=sinx、半徑 r 之 圓軌跡 2 2 x r y= − ，其中之(x, y )即為車輛在環境導航座標中之座標，然而環 境座標必定處於一連續時間軸之下，所以x、 y 都必為一時間函數，因此我們可 以將三種軌跡分別表示成時間函數如下： 一、拋物線軌跡 2 x y= ) (t x =t y(t) = x2(t) = t2 用矩陣表示即為： _⎥ ⎦ ⎤ ⎢ ⎣ ⎡ = ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ 2 ) ( ) ( t t t y t x (4.1) 上式可經由兩次的微分分別得到速度與加速度，再經由一次座標轉換得到慣 性元件之讀值 _⎥ ⎦ ⎤ ⎢ ⎣ ⎡ 2 t t

_→

dt ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ t 2 1 dt

→

_⎥ ⎦ ⎤ ⎢ ⎣ ⎡ 2 0

_→

θ ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ θ θ cos 2 sin 2 (4.2) 二、sin 函數軌跡 y=sinx 同拋物線軌跡之作法也可用矩陣表示出此軌跡如下： _⎥ ⎦ ⎤ ⎢ ⎣ ⎡ = ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ t t t y t x sin ) ( ) ( (4.3) 經由兩次微分及一次座標轉換 ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ t t sin dt

→

_⎥ ⎦ ⎤ ⎢ ⎣ ⎡ t cos 1 dt

→

_⎥ ⎦ ⎤ ⎢ ⎣ ⎡ −sint 0

_→

θ ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ − − θ θ cos sin sin sin t t (4.4) − =

同拋物線軌跡之作法也可用矩陣表示出此軌跡如下： ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ = ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ t t t y t x cos sin ) ( ) ( (4.5) 經由兩次微分及一次座標轉換 ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ t t cos sin dt

→

_⎥ ⎦ ⎤ ⎢ ⎣ ⎡ − t t sin cos dt

→

_⎥ ⎦ ⎤ ⎢ ⎣ ⎡ − − t t cos sin

_→

θ ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ − − − θ θ θ θ cos cos cos sin sin cos cos sin t t t t (4.6) 由此即可使用MATLAB 來模擬實驗。 首先使用 MATLAB 模擬出三種模擬軌跡如圖 4.1，圖 4.2，圖 4.3 分別表示 拋物線，sin 波形，半圓形路徑。 圖4.1 拋物線形模擬軌跡圖

圖4.2 半圓形模擬軌跡圖 圖 4.3 sin 波形模擬軌跡圖

4.1.2 模擬結果分析

由上一節中所述，使用模擬值來模擬軌跡預測，而模擬軌跡的部分，三種軌 跡都先以無因次化表示，詳細的模擬軌跡範圍於之後說明，而軌跡預測分別採取 預測3（綠色圓圈）、6（黃色三角形）、9（紅色菱形框）、12（紫色方框）個時 間點後之軌跡來討論，而軌跡預測誤差圖中的顏色採取跟前4 張圖之預測軌跡顏 色相對應，以下將分別討論三種軌跡之模擬結果。

一、拋物線軌跡 2 x y= 這種軌跡之模擬由於曲線後段近乎直線而圖表過於拉長，所以這邊ｘ軸 與ｙ軸之間隔將分別調整至較容易辨別的大小，而模擬的軌跡長度ｘ軸為1 到10 個單位，ｙ軸長度為 1 到 100 個單位。軌跡共分為 100 個取樣點，行 進速度每個取樣點間隔時間為 0.1 杪，意即每個時間點為 0.1 杪（sampling time = 0.1s），共分為 100 個取樣點，也就是說在ｘ軸上之速度為等速度前 進，ｙ軸之速度則因為y=x2的關係速度持續增加。因此整體的運動速度為 加速度運動。(往下的模擬結果圖將分開展示，例如圖 4.4 為 Trajectory 和 Predict trajectory(after 3 sampling times)之比較，並不包含 sampling time 為 6、 9、12 之預測軌跡，圖 4.5 後同系列圖以此類推)

模擬結果如下：

1. 預測 3 個時間點後之軌跡

2. 預測 6 個時間點後之軌跡

圖 4.5 拋物線形軌跡模擬預測結果圖(6 個時間點後)

4. 預測 12 個時間點後之軌跡 圖 4.7 拋物線形軌跡模擬預測結果圖(12 個時間點後) 5. 軌跡預測誤差 圖 4.8 拋物線形軌跡模擬預測誤差圖 由以上模擬結果發現INS 系統對趨近於直線之拋物線軌跡預測的誤差值都 可以壓在很小的範圍(0.1 個單位)之內，從軌跡預測誤差圖中可看出因為預測的

時間越長遠而呈等比增加（由約0.025 增價至約 0.1）。 二、sin 函數軌跡 y=sinx 這種軌跡之模擬圖表採取ｘ軸與ｙ軸之比例為1:1，長度ｘ軸為 1 到 10 個單 位，ｙ軸長度則為1 到-1 個單位上下波動。軌跡共分為 100 個取樣點，行進速度 每個取樣點間隔時間為0.1 杪（sampling time = 0.1s），也就是在ｘ軸上之速度為 等速度前進，ｙ軸之速度則因為y=sinx的關係速度持續減少至0 之後再加速然 後減速，週而復始。因此整體的運動速度為變速度運動。 模擬結果如下： 1. 預測 3 個時間點後之軌跡 圖 4.9 sin 波形軌跡模擬預測結果圖(3 個時間點後) 2. 預測 6 個時間點後之軌跡 圖 4.10 sin 波形軌跡模擬預測結果圖(6 個時間點後)

3. 預測 9 個時間點後之軌跡

圖 4.11 sin 波形軌跡模擬預測結果圖(9 個時間點後)

4. 預測 12 個時間點後之軌跡

圖 4.12 sin 波形軌跡模擬預測結果圖(12 個時間點後)

由以上模擬結果發現INS 系統對於 sin 波形這種屬於較劇烈運動之模擬軌跡 預測結果較差，雖然在3 及 6 個時間點後之軌跡預測的誤差都還能壓在 0.1 個單 位左右以內，但到了9 及 12 個時間點後之軌跡預測誤差則已較明顯的增加，特 別是在轉彎處誤差也更為明顯；另外模擬預測誤差圖中，結果與拋物線軌跡有很 大的不同，呈現一單向波峰狀，而誤差值是隨著預測時間的增加而近似成一等比 增加，這與軌跡變動的激烈程度有很大的關係。 三、半徑 r 之圓軌跡 y= r2 −x2 這種軌跡之模擬圖表採取ｘ軸與ｙ軸之比例為 1:1，但由於在ｘ軸上為等速前 進，但這樣會造成在半圓的兩端會有不合理之加減速度產生所以以半徑為5 單位 之半圓截取ｘ軸為-3.5 到 3.5 個單位(直徑之 70%)之圓弧，而ｙ軸長度則依照原 軌跡改變。軌跡因為截取部份原弧因此只分為100 之 70%也就是 70 個取樣點， 行進速度每個取樣點間隔時間為0.1 杪（sampling time = 0.1s），也就是在ｘ軸上 之速度為等速度前進，ｙ軸之速度因為y= 52 −x2 的關係速度持續減少至0 之 後再加速。因而整體的運動速度為變速度運動。 模擬結果如下： 1. 預測 3 個時間點後之軌跡

2. 預測 6 個時間點後之軌跡

圖 4.15 圓弧形軌跡模擬預測結果圖(6 個時間點後)

3. 預測 9 個時間點後之軌跡

4. 預測 12 個時間點後之軌跡 圖 4.17 圓弧形軌跡模擬預測結果圖(12 個時間點後) 5. 軌跡預測誤差 圖 4.18 圓弧形軌跡模擬預測誤差圖 由以上模擬結果發現，對於此種較為小弧度且較規律的轉彎運動，INS 系統 的表現較sin 波形軌跡還準確，不過除此之外，也比拋物線軌跡要好，主要原因 是因為拋物線軌跡在一開始的彎曲程度比圓弧形還來的大，所以一開始計算之誤 差就較大一些，且拋物線型軌跡模擬的速度較高也是原因之一；再由軌跡模擬預

跡之特性，當運動保持著一定角度轉彎時預測軌跡就會與實際軌跡發生交叉處， 當然交叉處的誤差趨近於零，這點也可以在sin 波型軌跡之預測誤差圖中發現， 第二個原因是因為運動速度上到了中間時速度較低，所以也會使預測較為準確。 而誤差與預測秒數的關係也較偏向等比增加，較類似做劇烈轉彎運動的sin 波形軌跡；而誤差隨著預測時間的增加成等差或是等比即決定了是否可預測更長 時間之預測能力，也是一個重要的指標。

4.1.3 模擬結果討論

統整三種模擬軌跡的軌跡預測誤差圖討論，首先解釋為何三種誤差圖橫軸之 值都沒有達到100 個取樣點，原因是三種軌跡在模擬時，拋物線形及 sin 形軌跡 都取了100 個取樣點，而半圓形取了 70 個取樣點，但軌跡在預測的時候最大預 測到了2 秒鐘之後，也就是 20 個取樣點所需之時間，所以在計算誤差時會有最 後20 個預測點沒有已知軌跡取樣點與之搭配，因此誤差圖的取樣點都會比軌跡 之取樣點減少20 個。 然後探討各個取樣點的誤差值，經過整理比較之後可以得到表4.2，此表用 了兩種表示法來表示誤差，左邊是以單位來表示，而右邊是以百分比表示（因為 模擬時將x 軸是定位等速度，所以此處之百分比採用：誤差/y 總距），接著可以 把表4.2 分作三個部份來討論：

表 4.2模擬結果综合比較表 軌跡預測誤差(單位) 軌跡預測誤差(百分比) 軌跡 最大值 最小值 平均值 最大值 最小值 平均值 拋物線形 0.0248 0.0248 0.0248 0.02% 0.02% 0.02% sin 波形 0.0154 0.0002 0.009916 0.50% 0.25% 0.12% 預測3 圓弧形 0.0082 0.0031 0.004002 0.16% 0.06% 0.08% 拋物線形 0.0496 0.0496 0.0496 0.05% 0.05% 0.05% sin 波形 0.0389 0.0014 0.023378 1.17% 0.58% 0.29% 預測6 圓弧形 0.0224 0.0059 0.00887 0.45% 0.12% 0.18% 拋物線形 0.0744 0.0744 0.0744 0.07% 0.07% 0.07% sin 波形 0.1048 0.0018 0.061666 3.08% 1.54% 0.77% 預測9 圓弧形 0.0493 0.0055 0.015486 0.99% 0.11% 0.31% 拋物線形 0.0992 0.0992 0.0992 0.10% 0.10% 0.10% sin 波形 0.2453 0.0094 0.144596 7.23% 3.61% 1.81% 預測12 圓弧形 0.0935 0.0004 0.024248 1.87% 0.01% 0.48% 一、第一部份：最大值比較 表 4.2 中的最大值之縱列，有淺紅及紅色底的部份為預測 3、6、9、12 個時間點時，哪一種軌跡所得到的誤差最大，以單位來看會發現除了第一 個3 及 6 個時間點之預測外都是由最劇烈擺盪的 sin 波形軌跡奪冠，其中又 以紅色底的預測12 個單位點誤差最大，但以百分比來比較就都是 sin 波形 之誤差較大。 二、第二部份：最小值比較 從最小值縱列來看淺綠及綠色系底為各個不同預測秒數中之最小值， 綠色則為整體最小值之最低，因為sin 波形與半圓形都有之前所說預測軌跡 會與原軌跡有交會之現象，因此最小值都會趨近於0，所以必須以百分比來

點極貼近原軌跡)且拋物線也只有 0.1%，其他大致上都是以拋物線模擬軌跡 預測的結果誤差較低，因此整體表現還以拋物線軌跡為優。 三、第三部份：平均值比較 最後要從平均值來看，淺黃色代表最低值，黃色代表最高值，所以可以 從百分之結果來看誤差之整體表現，最可以代表本實驗之軌跡模擬預測表現 是以拋物線形最優，半圓形次之，sin 波形最差，但從單位來看，最差的 sin 波形12 個時間點預測都還可以低至約 0.24 個單位，因此模擬結果之表現都 相當不錯。

4.2 實驗

4.2.1 簡易慣性導航系統平台

圖 4.19 SCINS 結合軌道車示意圖

本實驗之簡易慣性導航系統(Simply Constructed Inertial Navigation System, SCINS)將設計與軌道車結合，示意圖如圖 4.19 所示，其中使用之陀螺儀(型號為 ADXRS300)及加速計(型號為ADXL320)如圖 4.19 和圖 4.20，這兩種感測器都為 類比式感測器，因此在使用時都需經過控制器做類比數位轉換之後再讀值。 圖 4.20 陀螺儀(ADXRS300)圖[22] 圖 4.21 加速計(ADXL320)圖 [22] 陀螺儀的靈敏度如表4.3[20]，最大的角速度承受範圍為每秒+/-300 度，足以 承受本實驗所需要之角速度範圍，另外可從data sheet 中看到圖 4.21[20]中電壓 中點為2.5V 而 0.25~4.75V 則代表陀螺儀之角速度，此點在之後利用控制器架設 SCINS 為一重要資訊。 表 4.3 陀螺儀靈敏度表[20]

圖 4.22 陀螺儀電壓中心圖[20] 加速規的靈敏度如表4.4[21]，使用的範圍為+/-5g，也足以承受本實驗所需 之加速度範圍，其中Sensitivity at Xout,Yout 的中間值為 174mV/g，這也是之後 架設SCINS 時使用控制器做類比數位轉換之一重要資訊。 表 4.4 加速規靈敏度表[21] 本實驗將SCINS 模組化並焊接電路板如圖 4.22，陀螺儀跟加速計都焊上針 腳之後再裝配於電路板之上，直接由控制器之CN2 提供 5V 電源，這樣做的好 處是能夠預防陀螺儀、加速計及電路板中任何一樣損壞或者需要修改時都可以分 開，圖4.23，並且減低電子線路的複雜程度，大幅減低實驗成本及時間，另外提 到的是本實驗加速計為六根針腳，陀螺儀為七根針腳，因此在電路板上也配備了 防呆的設計，可以避面因為感測器裝回電路板時插錯針腳造成感測器燒毀。

圖 4.23 SCINS 模組圖

圖 4.24 SCINS 模組分解圖

/ 1.2A ) 以及 5V / 0.8A 的低電壓負載，圖 4.24[]即為 YZ_PIC_PROTO 之 Layout 圖，此控制器之優點在於此控制器之控制語法較類似於C 語言，並內建了許多 函數可供利用，且內建燒錄器，燒錄程式進控制器無須再另外裝配，對於初次使 用控制器的人較易上手，而使用時為了與筆記型電腦做連結需使用RS232/USB 轉接頭使控制器連結上配套程式＂PIC C Compiler＂將程式燒錄至控制器。 圖 4.25 YZ_PIC_PROTO Layout 圖 以下將所需之設備列出： I. YZ_PIC_PROTO 控制器(圖 4.25) II. RS232 對控制器之轉接頭(圖 4.26 右) III. 傳輸線(圖 4.26 左) IV. RS232 對電腦 USB 之轉接頭(圖 4.27) V. 控制器電源線(圖 4.28) VI. 筆記型電腦 IBM X61

圖 4.26 YZ_PIC_PROTO 控制器圖

圖 4.29 控制器電源線圖 SCINS 模組架設於控制器上如圖 4.29 所示，可以發現從上方只能看到加速 計，而從側邊看可以發現本實驗之SCINS 將陀螺儀與加速計背對背設置，如圖 4.30，用意再於一方面可以使感測器幾乎處於同一轉動軸上，另一方面可再回顧 圖4.21 陀螺儀之感測電壓直是以順時鐘轉動為正電壓，反之為負電壓，而本實 驗將陀螺儀倒過來設置則使得感測值同右手定則，也就是逆時鐘為正電壓，順時 鐘為負電壓，在分析數據時較為方便清晰。

圖 4.31 SCINS 感測器側面圖 SCINS 的架設完成還必須經過初步之靜止測試來確定本實驗簡易 INS 系統 之準確性，確保實驗時不會有太多雜訊干擾，圖4.31 及 4.32（圖 4.31 中藍色為 x 軸，紅色為ｙ軸）分別是加速計與陀螺儀靜止時的讀值，橫軸為取樣點(500 點)， 以控制器設定sampling time 為 0.1 杪來算就是靜止 50 秒之讀值，縱軸分別以加 速計為加速度( _{/ s}2 m )而陀螺儀為角速度(degree /s)，測試結果發現 y 軸比 x 軸之 跳動幅度要來的大，原因是因為控制器在使用時ｙ軸之訊號是使用CN1 的 RA3 腳未來做類比數位轉換，而此接角因為控制器設計上又會接上一電阻，因此ｙ軸 的訊號電壓值會較為減低，須由類比數位轉換給予補償，因此經轉換之後跳動幅 度會較為放大，但這並不會影響y 軸之感測靈敏度。

圖 4.32 加速計靜止感測值圖

圖 4.33 陀螺儀靜止感測直圖

說明SCINS 的穩定性，而所謂的感測範圍就是感測器最大所能量測之範圍，而 使用範圍即為本實驗在使用時會使用到最大的量測範圍，剩下未使用的範圍則保 留作往後更劇烈之實驗時可用，其中加速規的使用範圍為+/- 1.5g 也就是 14.7 _{/ s}2 m ，陀螺儀的使用範圍為+/- 150degree /s；表4.5 可以看到所有感測器 之跳動範圍以及量測500 個時間點之後的總平均值，而以此平均值可再與感測範 圍以及使用範圍比較來表示出其誤差百分比，如表4.6 所示，其感測器之使用範 圍誤差都低於0.6%，所以本實驗之 SCINS 具備足夠之穩定性來作為之後的實驗 使用。 表 4.5 SCINS 靜止跳動值表 INS 靜止跳動值 最大值 最小值 平均值 感測範圍 單位 x 軸(ax) 0.15 -0.15 0.0861 +/- 49 m/s^2 y 軸(ay) 0.25 0 0.026 +/- 49 m/s^2 陀螺儀(dsita) 2.4 -2.4 0.0372 +/- 300 degree/s 表 4.6 SCINS 靜止感測誤差表 感測範圍誤差 使用範圍誤差 x 軸(ax) 0.08786% 0.58571% y 軸(ay) 0.02653% 0.17687% 陀螺儀(dsita) 0.00620% 0.01240%