TWI394944B

TWI394944B - 車輛姿態估測系統與方法

Info

Publication number: TWI394944B
Application number: TW098135854A
Authority: TW
Inventors: Tsung Lin Chen; Ling Yuan Hsu
Original assignee: Univ Nat Chiao Tung
Priority date: 2009-10-22
Filing date: 2009-10-22
Publication date: 2013-05-01
Also published as: TW201115130A

Description

車輛姿態估測系統與方法

本案係關於車輛的控制，尤指一種車輛的姿態控制。

車輛姿態的獲得是現今車輛工業的一個重點發展項目，可以應用到汽車電子產品(Vehicle Electronics)、車輛導航系統(Vehicle Navigation System)、車輛線控系統(Vehicle Drive by Wire System)、夜間警示系統(Night Warning System)、行車紀錄器(Vehicle Event Data Recorder)、車輛防止顛簸系統(Vehicle Bumping Prevention System)、車輛姿態穩定系統(Vehicle Attitude Stability System)以及車輛防翻覆控制。然而，各項先前技術無法同時估測“車輛相對姿態”(車輛相對於道路的姿態)與“道路角度”。先前技術的缺失如下：先前技術大多以車輛動態模型為基礎，然而由於車輛動態過於複雜，所建構的數學模型與實際車輛動態之間的誤差將嚴重影響車輛相對姿態與道路角度估測的準確度。而在慣性測量器方面，先前技術則大多使用陀螺儀當感測器，將其所量測到資訊加以積分來獲得角度資訊。此做法容易造成誤差累積，使得所獲得的角度資訊無法適用一般狀況。再者，先前技術多為“分別”估測道路的兩個角度，甚少研究可“同時”估測道路的兩個角度。所用以估測車輛相對姿態與道路角度的感測器也是三種或三種以上。

此外，尚有一個重點，先前技術所定義的道路角度是相對於車頭方向，且定義方式異於傳統的尤拉角(Euler Angle)。因此縱使獲得兩個道路角度值，亦無法快速重現道路狀況。

之所以有上述的諸多缺失，在於傳統車輛姿態估測多以車輛動態為基礎，其準確度會受限於所建立之車輛動態數學模型。本發明運用三個座標系統的角度關係來建立數學演算法，並不需要車輛動態模型，因此將擁有較高的姿態估測準確度。以及，傳統道路角度估測多採用經驗公式，其準確度與適用性無法確定。然而本發明所利用之關係皆具有明確的物理意義，其適用性可用數學式來加以證明。而在計算上，傳統車輛航向角估測多採用角速度訊號積分、或是藉由兩個時間點的車輛動態互相比對而得知。本發明能夠即時提供車輛航向角來辨識車輛目前行駛方向，不需要透過兩個時間點的車輛動態資訊。

再者，傳統的車輛定位系統，在車輛的垂直方向的定位精度上還具有一些缺點，首先，車輛定位系統大多由車輛搭載一全球衛星定位系統(Global Position System,GPS)來實現。一般實用上，GPS可藉由三根天線與載波相位技術(Carrier Phase Technology)來獲得車輛「絕對姿態」(相對於一固定座標，非相對於行駛道路)，然而GPS所提供的的水平座標(x,y)精度遠高於其垂直座標(z)的精度，其主要原因在於GPS所採用的WGS84座標系統，此座標系統無法精確描述地球的橢圓表面，更忽略地球表面的地形起伏。因此GPS透過此座標系統所決定的垂直座標，會因實際地形高度而有明顯的誤差。

其次，縱使排除因座標系統所引起的誤差，GPS的定位精度的誤差亦遠大於車輛運動時的垂直位移。因此僅用GPS進行車輛定位時，在垂直方向上所獲得的定位精度無法滿足大部分車輛行駛控制系統及相關汽車電子產品的需求，例如：無法判定車輛行駛於高架橋上或是高架橋下的道路。

以往先前技術用以改善垂直方向精度方法為：為一區域建構一高度資料庫(Vertical Map)，即一區域中的3D座標資訊，當GPS獲得車輛的水平座標時，搜尋高度資料庫即可獲得車輛的垂直座標。以車輛定位/行駛控制應用而言，此技術的缺點是資料庫容量相當龐大，應用與維護費用昂貴，且仍然無法判定車輛行駛於高架橋上或是高架橋下的道路。此外亦有將慣性感測器結合GPS進行車輛定位與姿態量測，然而在垂直方向的精度上依舊無法精確的導航與行車控制的需求。

另一種先前技術的解決手段是在路面上裝設雷射定位系統(Laser Positioning System)，用雷射定位系統所量測的垂直座標來取代GPS所量測的垂直座標。然而此做法必須在一整條路徑上裝設許多雷射定位裝置，其成本相當昂貴，不容易實現，而且也易受外在天候環境的干擾。

因此，本發明結合車輛懸掛系統位移感測器、GPS、慣性感測器，來改善定位系統於垂直方向的精度與車輛姿態的感測，來提升GPS的量測精度(包含車輛垂直座標、側傾姿態、俯仰姿態)，使得感測系統的輸出資訊能滿足更多車輛導航系統、動態控制系統的需求。再者，本發明不涉及車輛本身的動態特性，因此可廣泛應用於各式車輛。

爰是之故，申請人有鑑於習知技術之缺失，發明出本案「車輛姿態估測系統與方法」，用以改善上述習用手段之缺失。

本發明之目的在於(1)能夠提供車輛相對於道路的姿態(包含車輛側傾角、車輛俯仰角、車輛航向角、車輛的縱向位置、側向位置、垂直位置)以及道路角度(包含道路傾斜角與道路坡度角)，這些動態角度資訊可以提供給汽車電子系統(例如車輛翻覆防止控制、車輛航向穩定控制、行車紀錄器…等)，做出適當的控制以提升行車安全、降低能源消耗。(2)本發明提供車輛航向角、以及兩個道路角度資訊，可用以判定車輛正在開往上坡或是下坡，此資訊可以連結於電子地圖，輔助導航系統獲知車輛正進入高架橋或是地下道。目前市面上使用的GPS系統無法判斷車輛是否進入高架橋或地下道。(3)本發明所定義的道路傾斜角與道路坡度角是相對於一個固定參考座標系，因而可以與「行車紀錄器(車輛黑盒子)」結合，可以還原道路角度，進而發展出各式不同的應用，例如：車輛事故的責任歸屬的判定…等。

為了達到上述之目的，本發明提供一種車輛姿態估測系統，是設置於一車輛上，該系統包括：一運算單元；一懸掛系統位移感測器，電連接至該運算單元，並用以產生一懸掛系統位移數據予該運算單元；以及一全球衛星定位系統，電連接至該運算單元，並用以產生該車輛的絕對姿態予該運算單元，其中，該運算單元透過該絕對姿態與該懸掛系統位移數據而得到該車輛的相對姿態。

如上所述的系統，其中該運算單元更產生一道路角度。

如上所述的系統，其中該道路角度更包括一道路傾斜角以及一道路側傾角。

如上所述的系統，其中該絕對姿態是該車輛相對於地球的姿態。

如上所述的系統，其中該絕對姿態包括一絕對航向角、一絕對俯仰角、以及一絕對側傾角。

如上所述的系統，其中該相對姿態是該車輛相對於行駛的表面的姿態。

如上所述的系統，其中該懸掛系統位移數據是來自懸掛系統兩端距離之改變。

如上所述的系統，其中該懸掛系統位移數據是來自輪胎與車體之間距離的改變。

如上所述的系統，其中該懸掛系統位移感測器是選自線性可變差分變壓器、超音波定位感測器、以及雷射定位感測器。

如上所述的系統，更包含一慣性測量單元，與該運算單元電連接，並提供一慣性數據予該運算單元。

如上所述的系統，其中該運算單元透過該絕對姿態、該懸掛系統位移數據、以及該慣性數據而得到該車輛的絕對垂直位置、絕對側向位置、與絕對垂直位置。

如上所述的系統，其中該慣性測量單元是一角速度測量器，並包含了對三個軸向的角速度、角加速度進行量測的功能。

為了達到上述之目的，本發明再提供一種車輛姿態估測的方法，是透過地球座標、道路座標、與車輛座標之間的位置關係，以取得車輛的姿態，該方法包括下列步驟：提供一感測系統於一車輛上；以該感測系統，量測該車輛的一絕對姿態與一懸掛系統狀態；藉由該懸掛系統狀態，得到該車輛的一相對姿態；使用該絕對姿態與該相對姿態，得到所述三個座標的一第一座標位置關係；透過該座標位置關係，計算出一道路資訊；使用該絕對姿態與該懸掛系統狀態，得到所述三個座標的一第二座標位置關係；以及透過該第一與第二座標位置關係、以及該道路資訊，計算出一車輛航向角，如此即估測出該車輛的姿態。

如上所述的方法，其中該絕對姿態是指車輛座標相對於地球座標的姿態，並包括：一絕對航向角、一絕對側傾角、以及一絕對俯仰角。

如上所述的方法，其中該懸掛系統狀態，是指懸掛系統的彈簧長度。

如上所述的方法，其中該相對姿態是指車輛座標相對於道路座標的姿態，並包括：一車輛側傾角、以及一車輛俯仰角。

如上所述的方法，其中該第一座標位置關係，是指道路座標與地球座標之間的關係。

如上所述的方法，其中該道路資訊包括一道路傾斜角與一道路坡度角。

如上所述的方法，其中該第二座標位置關係，是指車輛座標、道路座標、以及地球座標之間的關係。

如上所述的方法，其中所述的車輛座標、道路座標、以及地球座標，均以尤拉角描述。

如上所述的方法，其中該感測系統包括一運算單元、一全球衛星定位系統以及一懸掛系統位移感測器，且該全球衛星定位系統與該懸掛系統位移感測器均與該運算單元電連接。

為了達到上述之目的，本發明又提供一種車輛姿態估測的方法，是透過地球座標、道路座標、與車輛座標之間的位置關係，以取得車輛的姿態，該方法包括下列步驟：提供一感測系統於一車輛上；以該感測系統，量測該車輛的一絕對姿態、一懸掛系統狀態、以及一旋轉角速度；藉由該懸掛系統狀態，得到該車輛的一相對姿態；提供一待估測動態；提供一狀態觀察器、一估測系統動態模型與一估測系統輸出；進行一估測程序，首先處理該絕對姿態與該懸掛系統狀態，而得到一實際系統輸出，並將該實際系統輸出與該估測系統輸出相對照而得到一系統輸出誤差，並再回授該系統輸出誤差至該狀態觀察器，直到估測程序結束；以及計算出一道路資訊與一車輛航向角，如此即估測出該車輛的姿態。

較佳者，其中該感測系統包一運算單元、一懸掛系統位移感測器、一全球衛星定位系統、以及一三軸角速度感測器，而該懸掛系統位移感測器、該全球衛星定位系統、與該三軸角速度感測器均各自電連接至該運算單元。

較佳者，其中該待估測動態，包括一假設車輛絕對姿態、一假設車輛航向角、以及一假設道路角度。

較佳者，其中該狀態觀察器是一擴增卡曼濾波器(Extended Kalman Filter)。

較佳者，其中所述的估測程序包含下列步驟：選擇該車輛的相對姿態，以及道路資訊的初始條件；透過該狀態觀察器與該待估測動態，計算一增益值；將該增益值與該系統輸出誤差的乘積，加上該待估測動態，以得到次一個待估測動態；以及判斷該估測程序是否要結束，如是，則結束估測；如否，則以上一步驟所得到的次一個待估測動態，替換第二步驟內的該待估測動態，並進行第二步驟。

較佳者，本發明為了得到更為精確的車輛垂直位置資訊，更揭露了一訊號混合系統的訊號流程，首先是裝設一組感測系統於車輛上(上例採用全球衛星定位系統、慣性量測單元與懸掛系統位移感測器)。其次是利用上述感測系統獲得包括三根天線的絕對位置、三軸車輛線性加速度、三軸車輛旋轉角速度、以及四邊懸掛系統彈簧長度。再來是利用三軸車輛線性加速度運算一絕對縱向加速度、一絕對側向加速度、以及一絕對垂直加速度。並再利用三軸車輛旋轉角速度運算一絕對航向角速度、一絕對側傾角速度、以及一絕對俯仰角速度。接著將上述資訊輸入一處理器，並做為訊號混合系統的輸入。再利用四邊懸掛系統彈簧長度運算一車輛相對垂直位置、一車輛側傾角、一車輛俯仰角。之後利用三根天線的絕對位置運算一絕對縱向位置、一絕對側向位置、一絕對垂直位置、一絕對縱向速度、一絕對側向速度、一絕對垂直速度、一絕對航向角、一絕對側傾角、一絕對俯仰角。爾後再將上述資訊輸入一處理器，並做為訊號混合系統的輸出。並藉由訊號混合系統的輸出誤差與卡曼濾波器技術，可獲得一增益值並疊代運算，直到估測結束。最後本發明即可獲得改善過後的一絕對垂直位置、一絕對側傾角、一絕對俯仰角。

總體而言，本發明的特色在於：本發明僅需要兩種感測器即可獲得車輛姿態與道路角度，大幅降低車輛姿態估測所需要的費用與硬體設備。

本發明採用尤拉角來描述車輛姿態與道路角度的變化。此方式與一般對轉動物體的描述方式相同，此舉將有助於車輛姿態與道路角度資訊的應用。

本發明以三個座標系統的角度關係為基礎，可以將全球衛星定位系統所量測的車輛絕對姿態，成功地拆解為車輛相對姿態與道路角度。

本發明運用三個座標系統的角度關係來建立數學演算法，並不需要車輛動態的數學模型；亦不需要任何經驗公式。此方法的適用性可藉由數學分析來加以驗證。

本發明可以估測隨時間變化的道路角度。

本發明採用尤拉角來表示車輛姿態與道路角度變化情況，並且針對三個車輛相對姿態與兩個道路角度同時估測。

本發明需要車輛“絕對姿態”(相對於一固定座標，非相對於行駛道路)與懸掛系統彈簧長度，此兩種物理量可藉由數種感測器組合來獲得，其中一組合為全球衛星定位系統、懸掛系統位移感測器。

而實際上可以運用之較為產業別細目則如汽車電子、車輛導航、電子地圖、系統應用、機械控制等產業。而可能應用之產品則如汽車電子產品、車輛導航系統、車輛線控系統、夜間警示系統、行車紀錄器車輛防止顛簸系統、車輛姿態穩定系統以及車輛防翻覆控制。

以下針對本案「車輛姿態估測系統與方法」的較佳實施例進行描述，請參考附圖，但實際之配置及所採行的方法並不必須完全符合所描述的內容，熟習本技藝者當能在不脫離本案之實際精神及範圍的情況下，做出種種變化及修改。

本發明是透過描述三個座標，來達到描述車輛姿態之目的。此三個座標分別為地球座標、道路座標、與車輛座標，而車輛的絕對姿態是車輛座標與地球座標之間、車輛的相對姿態，則是車輛座標與道路座標之間，而通常車輛的絕對姿態來源是「道路座標與地球座標之間的關係」加上「車輛座標與道路座標之間的關係」而成，而本發明利用了全球衛星定位系統，即可得到車輛座標與地球座標之間的關係(即車輛的絕對姿態)，而再透過一懸掛系統量測裝置來得到車輛行駛在道路上的狀況，並進而得到車輛座標與道路座標之間的關係(即車輛的相對姿態)。

再者，本發明尚能達到更為精準的車輛位置座標，由於習用技術僅能在水平方向(即X軸與Y軸)對車輛精準定位，而在垂直方向上則無法精確定位。因此，透過本發明的懸掛系統量測裝置即可獲得車輛相對於路面垂直位移，進而提高了車輛在垂直方向的定位精確度。

此外，本發明更可再設置一慣性測量單元(Inertial Measurement Unit)，通常此一慣性測量單元是一個三軸加速度感測器與三軸角速度感測器，可以測量共六個自由度的速度與加速度的資料，如車輛的縱向加速度、側向加速度、垂直加速度、航向(偏航，Yaw)角速度、俯仰(Pitch)角速度、以及側傾(滾轉，Roll)角速度。本發明即可透過這些資料、數據，來修正車輛的絕對姿態的角度資訊，如絕對俯仰角、絕對側傾角，且亦可來修正車輛的位置資訊。本發明還可搭配狀態觀察器技術，以其回授系統輸出誤差來估測系統之動態的一種演算法。

第一實施例

此實施例是關於利用本案「車輛姿態估測系統與方法」來得到車輛的相對姿態、道路坡度角與側傾角的具體實施方式。

請參閱圖1a與圖1b，其中圖1a揭露本案第一估測系統11、而圖1b則揭露本案的第二估測系統12。本發明主要是利用車輛絕對姿態與懸掛系統彈簧長度，透過估測系統來獲得兩組尤拉角：分別是描述地球座標與道路座標的角度關係(道路角度)、以及描述道路座標與車體座標的角度關係(車輛相對姿態)。本發明的估測系統可以由兩種方式來建構而成：(1)如圖1a所示，裝設多天線的全球衛星定位系統2與懸掛系統位移感測器3(Suspension Displacement Sensor)來量測車輛絕對姿態與懸掛系統彈簧長度，並利用三個座標系統的角度關係方程式，獲得包含車輛相對姿態與道路角度的輸出資訊5；(2)如圖1b所示，除了如圖1a的裝置外，增設三軸角速度感測器4並建立狀態觀察器(State Observer)，來估測包含有車輛相對姿態與道路角度的輸出資訊5。本技術的優勢在於以三個座標系統的角度關係為基礎，提高車輛相對姿態與道路角度估測的準確度；可提供車輛航向角資訊；可估測隨時間變化的道路角度；可清楚描述道路狀況。

座標系與尤拉角

為了表示車輛絕對姿態、車輛相對姿態與道路角度之間的關係，本發明採用三組尤拉角與三個運動座標系：地球座標(Global Frame,E^g )、道路座標(Road Frame,E^r )、以及車體座標(Body Frame,E^b )。請參閱圖2，為本發明的三個座標的示意圖。其中，地球座標E^g 是固定於地表上的某一點，道路座標E^r 是固定於行駛道路並隨道路角度而改變，車體座標E^b 是固定於車體重心並隨車體運動而改變。第一組尤拉角(Ψ_g ,θ_g ,Φ_g )是用來描述車輛姿態相對於地球座標E^g 的角度關係，在本例子中稱為「絕對航向角(Absolute Yaw Angle)」，「絕對俯仰角(Absolute Pitch Angle)」，「絕對側傾角(Absolute Roll Angle)」，其旋轉次序為3-2-1；第二組尤拉角(Ψ_v ,θ_v ,Φ_v )是用來描述車輛姿態相對於道路的角度關係，在本例子中稱為「車輛航向角(Vehicle Yaw Angle)」，「車輛俯仰角(Vehicle Pitch Angle)」，「車輛側傾角(Vehicle Roll Angle)」，其旋轉次序為3-2-1；最後，第三組尤拉角(θ_r ,Φ_r ,Ψ_r )是用來描述行駛道路的地勢變化相對於地球座標E^g 的角度關係，在本例子中稱為「道路坡度角(Road Grade Angle)」，「道路傾斜角(Road Bank Angle)」，「道路彎曲角(Road Curve Angle)」，其旋轉次序為2-1-3。

藉由第一組尤拉角，我們可以將位於地球座標E^g 的三維向量轉換至車體座標E^b ，如下所示：

其中為一組三維向量且表示於車體座標E^b ；為一組三維向量且表示於地球座標E^g ；為第一組尤拉角的轉換矩陣，其上下標符號代表此轉換矩陣可將向量從g (地球座標)轉換至b (車體座標)。

藉由第二組尤拉角，我們可以將位於道路座標E^r 的三維向量轉換至車體座標E^b ，如下所示：

其中為一組三維向量且表示於道路座標E^r ；為第二組尤拉角的轉換矩陣，其上下標符號代表此轉換矩陣可將向量從r (道路座標)轉換至b (車體座標)。

藉由第三組尤拉角，我們可以將位於地球座標E^g 的三維向量轉換至道路座標E^r ，如下所示：

其中為第三組尤拉角的轉換矩陣，其上下標符號代表此轉換矩陣可將向量從g (地球座標)轉換至r (道路座標)。

從方程式(1)~(3)，可以獲得一條表示三個座標系統之間的角度對等式：

後續將會藉由方程式(4)發展兩套估測系統並說明其方法。除此之外，因為道路彎曲角並不會影響車體動態，所以在此假設道路彎曲角為零(ψ _r =0)。

感測系統

請參閱圖3a與圖3b，其中，圖3a，為本發明的第一估測系統的硬體示意圖；而圖3b，為本發明的第二估測系統的硬體示意圖。本發明所採用的感測系統包含全球衛星定位系統2與懸掛系統位移感測器(31-34)，或是全球衛星定位系統2、懸掛系統位移感測器(31-34)與三軸角速度感測器4。其中全球衛星定位系統包含了三個天線接收器Ant-a、Ant-b、以及Ant-c。後續將會介紹感測系統的量測資訊，也就是本發明所需要的輸入訊號。

全球衛星定位系統

請參閱圖4，為本發明的全球衛星定位系統與車輛的配置示意圖。如圖4所示，全球衛星定位系統的接收天線需為三根(圖4中Ant-a、Ant-b、Ant-c)將其裝設於車輛頂端，利用全球衛星定位系統量測三根天線分別相對於地球座標的位置資訊，藉由此三根天線的位置資訊即可計算出絕對航向角(ψ _g,gps )、絕對俯仰角(θ _g,gps )、以及絕對側傾角()，其下標符號gps 表示為全球衛星定位系統的量測資訊。此三個角度資訊的數學算式可表示如下：其中輔助角度α ₁ 是用以計算絕對航向角，如圖4所示；P _a,z ,P _c,z ,P _b,z ,P _d,z 代表接收天線相對於地球座標的垂直位置之量測資訊，其下標符號的a、b、c、d分別代表天線Ant-a、天線Ant-b、天線Ant-c、以及左右中點d四點之垂直位置，且左右中點d為天線Ant-a與天線Ant-b的中心點，而左右距離D₁ 代表天線Ant-a與天線Ant-b之間以車體橫向方向為準的左右距離；而前後距離D₂ 則代表Ant-a與天線Ant-c之間以車體縱向方向為準的前後距離。

或者是全球衛星定位系統採用載波相位技術(Carrier Phase Technology)，直接計算接收天線彼此之間的相對位移，即可量測出絕對航向角(ψ _g,gps )、絕對俯仰角(θ _g,gps )、以及絕對側傾角()。

因此藉由方程式(6)，即可獲得車輛絕對姿態，

懸掛系統位移感測器

懸掛系統位移感測器可以是如下的感測器：線性可變差分變壓器(Linear Variable Differential Transformer,LVDT)，超音波定位感測器(Ultrasonic Positioning Sensor)、或雷射定位感測器(Laser Positioning Sensor)…等。如圖3a所示，懸掛系統位移感測器裝設於車輛四邊的懸掛系統旁，可用來量測車輛四邊的懸掛系統彈簧長度。因此四邊彈簧長度的量測方程式可表示如下：其中H _1~4,sus 為懸掛系統位移感測器所量測的彈簧長度，其下標符號1~4依序代表車輛左前方、右前方、右後方、以及左後方的彈簧長度，而另一下標符號sus 則表示為懸掛系統位移感測器的量測資訊；z 為車輛重心的垂直位移量；t _f ,t _r 分別為車輛兩個前輪之間的長度與兩個後輪之間的一半長度；t _f ,t _r l _f ,l _r 分別為車體重心到前軸與後軸之長度。

三軸角速度感測器

如圖3b所示，三軸角速度感測器裝設於車體上並對準於車體座標的三軸，即可獲得車體三軸旋轉角速度(ω _x ,ω _y ,ω _z )：其中ω _x,gyro ,ω _y,gyro ,ω _z,gyro 為角速度感測器分別量測車體x、y、z三軸的旋轉角速度，其下標符號gyro 表示為角速度感測器的量測資訊。

估測系統及其方法 第一套估測系統及其方法

如圖1a所示，藉由第一估測系統11(全球衛星定位系統2與懸掛系統位移感測器3)，我們可以獲得車輛絕對姿態(方程式(6))以及四邊懸掛系統彈簧長度(方程式(7))，接著此估測系統將會藉由三個座標系統的角度關係(方程式(4))，來估測或計算剩下的角度，其包含車輛航向角(ψ _v )、道路傾斜角()、道路坡度角(θ _r )。首先整理方程式(7)，即可獲得車輛俯仰角(θ _v )與車輛側傾角()：

為了獲得道路傾斜角與道路坡度角的數學方程式，將方程式(4)整理成下式：其中P _ij 為矩陣中第i列第j行的元素。

接著方程式(3)取代方程式(10)的左側式子，並且將方程式(6)、(9)代入方程式(10)的右側式子，藉由角度轉換的對等關係、即可獲得道路傾斜角與道路坡度角的數學方程式：

相同地，為了獲得車輛航向角的數學方程式，再將方程式(4)整理成下式：其中Q _ij 為矩陣中第i列第j行的元素。

接著將方程式(2)取代方程式(12)的左側式子，並且將方程式(6)、(11)代入方程式(12)的右側式子，藉由角度轉換的對等關係、即可獲得車輛航向角的數學方程式：ψ _v =tan^-1 (Q ₁₂ /Q ₁₁ ) (方程式13)

因此第一套估測系統11僅需要全球衛星定位系統2與懸掛系統位移感測器3並透過上述的數學計算，即可將全球衛星定位系統2量測的車輛絕對姿態(ψ _g ,θ _g ,於方程式(6))，分離為車輛相對姿態(ψ _v ,θ _v ,於方程式(9)、(13))與道路角度(θ _r ,於方程式(11))。

第二套估測系統及其方法

如圖1b所示，藉由第二套感測系統12(全球衛星定位系統2、懸掛系統位移感測器3與三軸角速度感測器4)，我們可以獲得車輛絕對姿態(方程式(6))以及四邊懸掛系統彈簧長度(方程式(7))，接著此估測系統將會藉由狀態觀察器技術(State Observer Technology)並且搭配三軸角速度感測器的量測資訊(方程式(8))來修正車輛絕對姿態的角度資訊，其包含絕對航向角(ψ _g )、絕對側傾角()、絕對俯仰角(θ _g )；接著藉由三個座標系統的角度關係(方程式(4))與車輛絕對姿態的角度資訊，來估測其他角度資訊，其包含車輛航向角(ψ _v )、道路傾斜角()、道路坡度角(θ _r )。其中狀態觀察器技術是一種藉由回授系統輸出誤差來估測系統動態的演算法。

相同地，首先整理方程式(7)，即可獲得車輛俯仰角(θv )與車輛側傾角()：

藉由方程式(8)，三軸角速度感測器可以獲得車體三軸旋轉角速度，利用方程式(15)可以將車體三軸旋轉角速度轉換成車輛絕對姿態的角速度：

其中為車體絕對姿態的角速度。

為了估測車輛航向角與道路角度，必須新增三個待估測動態，由於目前沒有辦法利用數學方程式來描寫道路角度的變化，因此假設道路角度為常數；此外車輛航向角的動態模型可由方程式(13)微分而得：

為了確保此系統的觀察性(Observability)，本例子將會處理全球衛星定位系統2與懸掛系統位移感測器3的量測資訊，將其量測資訊處理為方程式(17)，並選取為實際系統輸出方程式(y )。

因此藉由方程式(15)、(16)建立一個估測系統動態模型，如方程式(18)所示，其待估測動態()包含假設車輛絕對姿態(,,)、假設車輛航向角()以及假設道路角度(,)，其系統輸入為三軸角速度感測器4的量測資訊(ω_x _, _gyro ,ω_y _, _gyro ,ω_z _, _gyro )，且估測系統輸出方程式如方程式(19)所示，因此估測系統即可建立完成：

其中R _ij 為矩陣中第i列第j行的元素。

透過回授系統輸出誤差()以及運用各種現有狀態觀察器技術，例如擴增卡曼濾波器(Extended Kalman Filter)，即可估測出車輛航向角、道路傾斜角、道路坡度角。其演算法步驟如下：

1.選擇車輛相對姿態與道路角度的初始條件()並設定k =0，而成為一初始條件()。

2.透過狀態觀察器技術與系統動態()，計算增益值(L )

3.藉由系統輸出誤差()來更新估測系統誤差：

4.判斷估測系統是否要結束，是，結束估測；否，設定k =k +1，並回到步驟2。

因此第二套估測系統僅需要全球衛星定位系統2、懸掛系統位移感測器3與三軸角速度感測器4並透過上述的狀態觀察器技術，即可將全球衛星定位系統2量測的車輛絕對姿態(Ψ_g ,θ_g ,Φ_g 於方程式(6))，分離為車輛相對姿態(Ψ_v ,θ_v ,Φ_v 於方程式(14)、(18))與道路角度(θ_r ,Φ_r 於方程式(18))。

操作方塊圖

圖1a為本發明第一估測系統的操作方塊圖：首先全球衛星定位系統2量測接收天線的相對位置並藉由方程式(5)計算絕對航向角、絕對側傾角與絕對俯仰角；懸掛系統位移感測器3量測車輛四邊的懸掛系統彈簧長度，接著將上述資訊傳遞進入本發明所提出的估測系統(第一套估測系統11及其方法)，即可獲得車輛相對姿態(Ψ_v ,θ_v ,Φ_v )，以及車輛行駛於路面的道路角度資訊(Φ_r ,θ_r )。

圖1b為本發明第二估測系統的操作方塊圖：首先全球衛星定位系統2量測接收天線的相對位置並藉由方程式(5)計算絕對航向角、絕對側傾角與絕對俯仰角；懸掛系統位移感測器3量測車輛四邊的懸掛系統彈簧長度；三軸角速度感測器4量測車體三軸旋轉角速度，接著將上述資訊傳遞進入本發明所提出的估測系統(第二套估測系統12及其方法)，即可獲得車輛相對姿態(Ψ_v ,θ_v ,Φ_v )，以及車輛行駛於路面的道路角度資訊(Φ_r ,θ_r )。

訊號流程圖 本發明提出兩種估測系統：

請參閱圖5a，為本發明的第一估測系統及其方法的流程圖。其中揭露第一估測系統之步驟a至g：

a.裝設一組感測系統於車輛上110(上例採用全球衛星定位系統與懸掛系統位移感測器)。

b.利用此感測系統獲得包括一絕對航向角、一絕對側傾角、一絕對俯仰角、四邊懸掛系統彈簧長度之量測資訊111。

c.利用四邊懸掛系統彈簧長度計算出一車輛側傾角、一車輛俯仰角112，如方程式(9)

d.用三個座標系統之間的角度關係，如方程式(4)所示113，整理為方程式(10)。

e.藉由方程式(11)與量測資訊，得到包括一道路傾斜角及一道路坡度角的道路角度資訊114。

f.用三個座標系統之間的角度關係，如方程式(4)所示115，整理為方程式(12)。

g.藉由方程式(13)、量測資訊與道路角度資訊，得到包括一道路傾斜角及一道路坡度角的道路角度116。

如果以方法詳言之，並請配合以上各實施例與圖式，本發明的車輛姿態的第一估測方法，是透過地球座標、道路座標、與車輛座標之間的位置關係，以取得車輛的姿態，該方法包括下列步驟：提供一感測系統於一車輛上；以該感測系統，量測該車輛的一絕對姿態與一懸掛系統狀態；藉由該懸掛系統狀態，得到該車輛的一相對姿態；使用該絕對姿態與該相對姿態，得到所述三個座標的一第一座標位置關係；透過該座標位置關係，計算出一道路資訊；使用該絕對姿態與該懸掛系統狀態，得到所述三個座標的一第二座標位置關係；以及透過該第一與第二座標位置關係、以及該道路資訊，計算出一車輛航向角，如此即估測出該車輛的姿態。

其中該絕對姿態是指車輛座標相對於地球座標的姿態，並包括：一絕對航向角、一絕對側傾角、以及一絕對俯仰角。而該懸掛系統狀態，是指懸掛系統的彈簧長度。而該相對姿態是指車輛座標相對於道路座標的姿態，並包括：一車輛側傾角、以及一車輛俯仰角。而該第一座標位置關係，是指道路座標與地球座標之間的關係。而該道路資訊包括一道路傾斜角與一道路坡度角。而該第二座標位置關係，是指車輛座標、道路座標、以及地球座標之間的關係。其中所述的車輛座標、道路座標、以及地球座標，均以尤拉角描述。其中該感測系統包括一運算單元、一全球衛星定位系統以及一懸掛系統位移感測器，且該全球衛星定位系統與該懸掛系統位移感測器均與該運算單元電連接。

請參閱圖5b，為本發明的第二估測系統及其方法的流程圖。其中揭露第二估測系統之步驟a至g：

a.裝設一組感測系統於車輛上120(上例採用全球衛星定位系統、懸掛系統位移感測器與三軸角速度感測器)。

b.利用此感測系統獲得包括一絕對航向角、一絕對側傾角、一絕對俯仰角、四邊懸掛系統彈簧長度、車體三軸旋轉角速度之量測資訊121。

c.利用四邊懸掛系統彈簧長度計算出一車輛側傾角、一車輛俯仰角122，如方程式(14)。

d.新增一組包括一假設道路坡度角、一假設道路側傾角、一假設車輛航向角之待估測動態123。

e.藉由方程式(15)與(16)建立估測系統動態模型，如方程式(18)所示；藉由方程式(4)與方程式(17)建立估測系統輸出方程式124，如方程式(19)所示。

f.藉由估測系統輸出誤差回授至狀態觀察器技術，即可獲得一增益值並疊代運算125，直到估測結束。

g.即可獲得包括一道路傾斜角與一道路坡度角的道路角度，以及一車輛航向角126。

如果以方法詳言之，並請配合以上各實施例與圖式，本發明的車輛姿態的第二估測方法，是透過地球座標、道路座標、與車輛座標之間的位置關係，以取得車輛的姿態，該方法包括下列步驟：提供一感測系統於一車輛上；以該感測系統，量測該車輛的一絕對姿態、一懸掛系統狀態、以及一旋轉角速度；藉由該懸掛系統狀態，得到該車輛的一相對姿態；提供一待估測動態；提供一狀態觀察器、一估測系統動態模型與一估測系統輸出；進行一估測程序，首先處理該絕對姿態與該懸掛系統狀態，而得到一實際系統輸出，並將該實際系統輸出與該估測系統輸出相對照而得到一系統輸出誤差，並再回授該系統輸出誤差至該狀態觀察器，直到估測程序結束；以及計算出一道路資訊與一車輛航向角，如此即估測出該車輛的姿態。

其中該感測系統包括一運算單元、一懸掛系統位移感測器、一全球衛星定位系統、以及一三軸角速度感測器，而該懸掛系統位移感測器、該全球衛星定位系統、與該三軸角速度感測器均各自電連接至該運算單元。該待估測動態，包括一假設車輛絕對姿態、一假設車輛航向角、以及一假設道路角度。該狀態觀察器是一擴增卡曼濾波器。

又，其中所述的估測程序包含下列步驟：選擇該車輛的相對姿態，以及道路資訊的初始條件；透過該狀態觀察器與該待估測動態，計算一增益值；將該增益值與該系統輸出誤差的乘積，加上該待估測動態，以得到次一個待估測動態；以及判斷該估測程序是否要結束，如是，則結束估測；如否，則以上一步驟所得到的次一個待估測動態，替換第二步驟內的該待估測動態，並進行第二步驟。

模擬結果

請參閱圖6a1至圖6a5，與圖6b1至圖6b5，其中，圖6a1至圖6a5為本發明的第一估測系統的模擬圖，而圖6b1至圖6b5為本發明的第二估測系統的模擬圖。其中，各圖中粗線代表真實車輛動態，而細線代表兩個估測系統的輸出，模擬的假設是道路角度會隨著時間而呈現正弦函數的變化，其頻率為0.25赫茲、峰值為5度。請先參閱圖6a1至圖6a5，首先圖6a1是道路傾斜角模擬圖、圖6a2是道路坡道角模擬圖、圖6a3是車輛航向角模擬圖，由此可見本發明提出的第一估測系統均有相當的準確度。再者，圖6a4是車輛俯仰角模擬圖、圖6a5是車輛側傾角模擬圖，此二圖是利用懸掛系統位移感測器作為量測之用，因此模擬圖中懸掛系統位移感測器與實際的車輛動態的俯仰、側傾完全符合，故於圖6a4、圖6a5僅劃出代表本發明估測系統的細線，也就是說本發明的估測系統可以準確的估測出車輛的俯仰角與側傾角。

請接著參閱圖6b1至圖6b5，為本發明的第二估測系統的模擬圖。一如前一段所述，各圖中粗線代表真實車輛動態，而細線代表兩個估測系統的輸出，模擬的假設是道路角度會隨著時間而呈現正弦函數的變化，其頻率為0.25赫茲、峰值為5度。與圖6a1-6a5不同之處在於第二套估測系統更增設了三軸角速度感測器來修正車輛絕對姿態的角度資訊。圖6b1是道路傾斜角模擬圖、圖6b2是道路坡道角模擬圖、圖6b3是車輛航向角模擬圖，由此三圖可見粗線細線高度重疊，故可說本發明提出的第二估測系統利用三軸角速度感測器的修正功能，更可提昇整個估測系統的準確度。此外，圖6b4是車輛俯仰角模擬圖、圖6b5是車輛側傾角模擬圖，此二圖是利用懸掛系統位移感測器作為量測之用，因此模擬圖中懸掛系統位移感測器與實際的車輛動態的俯仰、側傾完全符合，故於圖6b4、圖6b5僅劃出代表本發明估測系統的細線，也就是說本發明的估測系統可以準確的估測出車輛的俯仰角與側傾角。

第二實施例

此實施例是關於利用本案「車輛姿態估測系統與方法」來對車輛的垂直方向進行定位的具體實施方式。

以下針對本案「車輛姿態估測系統與方法」中關於車輛的垂直方向定位進行描述，請參考附圖，但實際之配置及所採行的方法並不必須完全符合所描述的內容，熟習本技藝者當能在不脫離本案之實際精神及範圍的情況下，做出種種變化及修改。

如圖3b所示，本發明主要是利用懸掛系統位移感測器(Suspension Displacement Sensor)來搭配全球衛星定位系統(Global Positioning System)與慣性量測單元(Inertial Measurement Unit)進而改善量測資訊的準確度。本發明的改善方式是藉由懸掛系統位移感測器來獲得車輛相對於路面的垂直位移、車輛相對於路面的側傾角、以及車輛相對於路面的俯仰角，將這些資訊傳入一處理器，其包含以全球衛星定位系統與慣性量測單元為基礎的訊號混合系統(Sensor Fusion System)，進而改善量測資訊的準確度，其包含絕對垂直座標、絕對側傾角、以及絕對俯仰角。

座標系與尤拉角

為了表示車輛絕對姿態、車輛相對姿態與道路角度之間的關係，本發明採用三組尤拉角與三個運動座標系：地球座標(Global Frame,E^g )、道路座標(Road Frame,E^r )、以及車體座標(Body Frame,E^b )。請配合圖2，地球座標E^g 是固定於地表上的某一點，道路座標E^r 是固定於行駛道路並隨道路角度而改變，車體座標E^b 是固定於車體重心並隨車體運動而改變。第一組尤拉角(Ψ_g ,θ_g ,Φ_g )是用來描述車輛姿態相對於地球座標E^g 的角度關係，在本例子中稱為「絕對航向角」，「絕對俯仰角」，「絕對側傾角」，其旋轉次序為3-2-1；第二組尤拉角(Ψ_v ,θ_v ,Φ_v )是用來描述車輛姿態相對於道路的角度關係，在本例子中稱為「車輛航向角」，「車輛俯仰角」，「車輛側傾角(Vehicle Roll Angle)」，其旋轉次序為3-2-1；最後，第三組尤拉角(θ_r ,Φ _r ,Ψ _r )是用來描述行駛道路的地勢變化相對於地球座標E^g 的角度關係，在本例子中稱為「道路坡度角」，「道路傾斜角」，「道路彎曲角」，其旋轉次序為2-1-3。

其中為第三組尤拉角的轉換矩陣，其上下標符號代表此轉換矩陣可將向量從g (地球座標)轉換至r (道路座標)。因為道路彎曲角並不會影響車體動態，所以在此假設道路彎曲角為零(ψ _r =0)。

感測系統

如圖3b所示，本例所採用的感測系統包含全球衛星定位系統、慣性量測單元、以及懸掛系統位移感測器。

全球衛星定位系統

如圖4所示，全球衛星定位系統的接收天線需為三根(圖4中Ant-a、Ant-b與Ant-c)將其裝設於車輛頂端，利用全球衛星定位系統量測三根天線分別相對於車體座標的位置資訊()，計算出車輛座標相對於地球座標的位置向量()，其上標符號g 表示為位置向量表示於地球座標、下標符號v 表示為車輛座標的位置向量、下標符號gps 表示為全球衛星定位系統2(圖1a、圖1b)的量測資訊：且其中分別代表全球衛星定位系統分別量測車體座標相對於地球座標的三軸位置向量，且表示於地球座標x、y、z三軸上。

分別代表全球衛星定位系統分別量測接收天線相對於地球座標的三軸位置向量，且表示於地球座標x、y、z三軸上，其下標符號m代表三根接收天線的編號(a ,b ,c )。

分別代表接收天線相對於車輛座標的三軸位置向量，且表示於地球座標x、y、z三軸上。

利用全球衛星定位系統量測任何一根天線相對於車體座標的速度資訊()，皆可計算出車體座標相對於地球座標的速度向量()：且其中分別代表全球衛星定位系統分別量測車體座標相對於地球座標的三軸速度向量，且表示於地球座標x、y、z三軸上。

ω _x ,ω _y ,ω _z 分別代表車體x、y、z三軸的旋轉角速度。再利用三根天線相對於地球座標的位置向量，即可計算出絕對航向角(ψ _g,gps )、絕對俯仰角(θ _g,gps )、以及絕對側傾角()。此三個角度資訊的數學算式可表示如下：其中α ₁ 為計算絕對航向角的輔助角度，如圖4所示；代表d點相對於地球座標的垂直位置之量測資訊，左右中點d為天線Ant-a與天線Ant-b的中心點；而左右距離D₁ 代表天線Ant-a與天線Ant-b之間以車體橫向方向為準的左右距離；而前後距離D₂ 則代表Ant-a與天線Ant-c之間以車體縱向方向為準的前後距離。

因此藉由方程式(4’)，即可獲得車輛相對於地球座標的位置，也就是車輛絕對座標，

因此藉由方程式(5’)，即可獲得車輛相對於地球座標的速度，也就是車輛絕對速度，

因此藉由方程式(6’)，即可獲得車輛相對於地球座標的角度，也就是車輛絕對姿態，

慣性量測單元

如圖3b所示，慣性量測單元4裝設於車體旋轉中心並對準於車體座標的三軸，其包含三軸加速度感測器與三軸角速度感測器。三軸加速度感測器4能夠獲得車體座標三軸的加速度，透過方程式(1’)可以將車體座標三軸的加速度轉換至地球座標表示：其中分別代表全球衛星定位系統2(圖1a、圖1b)分別量測車體座標相對於地球座標的三軸加速度向量，且表示於地球座標x、y、z三軸上。

為三軸加速度感測器4(圖3b)分別量測車體x、y、z三軸的線性加速度，其上標符號b 表示為加速度向量表示於車體座標，下標符號acc 表示為加速度感測器4的量測資訊。

分別代表三軸加速度感測器4的訊號偏差現象，是未知的參數。而三軸角速度感測器4能夠獲得車體三軸旋轉角速度(ω _x ,ω _y ,ω _z )：其中ω _x,gyro ,ω _y,gyro ,ω _z,gyro 為角速度感測器4分別量測車體x、y、z三軸的旋轉角速度，其下標符號gyro 表示為角速度感測器4的量測資訊；ω _x,bias ,ω _y,bias ,ω _z,bias 分別代表三軸角速度感測器4的訊號偏差現象，是未知的參數。

透過尤拉方程式，車輛絕對姿態角速度與車體旋轉角速度的關係式表示如下：其中為車體絕對姿態的角速度。

懸掛系統位移感測器

懸掛系統位移感測器3(圖1b)可以是如下的感測器：線性可變差分變壓器，超音波定位感測器、或雷射定位感測器等。如圖3所示，四個懸掛系統位移感測器(31-34)分別裝設於車輛四邊的懸掛系統旁，可用來量測車輛四邊的懸掛系統彈簧長度。因此四邊彈簧長度與車輛幾何參數的關係式可以表示如下：其中H _1~4,sus 為懸掛系統位移感測器所量測的彈簧長度，其下標符號1~4依序代表車輛左前方、右前方、右後方、以及左後方的彈簧長度，而另一下標符號sus 則表示為懸掛系統位移感測器的量測資訊；為車體座標相對於道路座標的垂直位移，其上標符號r 表示為位置向量表示於道路座標；t _f ,t _r 分別為車輛兩個前輪之間的長度與兩個後輪之間的一半長度；l _f ,l _r 分別為車體重心到前軸與後軸之長度。

整理方程式(13’)，即可利用四邊彈簧長度來計算車輛垂直位移()、車輛俯仰角(θ _v,sus )、車輛側傾角()，如下的方程式(14’)所示：

因此藉由方程式(14’)，即可獲得車輛相對於道路座標的垂直位移()、車輛相對於道路座標的俯仰角(θ_v )、以及車輛相對於道路座標的側傾角(Φ_v )，

訊號混合系統

先前技術利用全球衛星定位系統與慣性量測單元的物理關係來建立一套訊號混合系統，以補足全球衛星定位系統於取樣週期區間的訊號，並修正慣性量測單元訊號飄移(Drift)的現象。訊號混合系統一般是使用卡曼濾波器來處理全球衛星定位系統與慣性量測單元的量測訊號，用以獲得同步、準確的動態資訊。

卡曼濾波器技術是一種藉由回授系統的輸出來獲得系統動態的演算法，欲採用此法，首先選取全球衛星定位系統的量測資訊為訊號混合系統的輸出之一(y ₁ )：

截至目前為止，乃是先前技術的作法。但是GPS的輸出精度(包含絕對垂直位置、絕對側傾角、絕對俯仰角)無法滿足大部分車輛精確導航、行駛控制的需求。因此本發明將增加一組懸掛系統位移感測器來改善訊號混合系統的輸出準確度。

在此，懸掛系統位移感測器的輸出被選擇為第二組(y ₂ )與第三組(y ₃ )的訊號混合系統的輸出，如下所示：

整合方程式(16’)到(18’)，即可獲得本發明所使用的訊號混合系統輸出方程式(y =[y ₁ ,y ₂ ,y ₃ ]^T )。

因此透過方程式(7’)到(12’)來建立一個訊號混合系統動態模型，如下的方程式(19’)所示，其待估測動態()包含待估測車輛絕對位置(,,)、待估測車輛絕對速度(,,)、待估測車輛絕對姿態(,,)、待估測加速度感測器訊號偏差(,,)、待估測角速度感測器訊號偏差(,,)、以及待估測道路角度(,)。

其中為了估測道路角度(Φ_r ,θ_r )，必須新增兩個待估測動態(,)，由於目前沒有辦法「即時(Real Time)」獲得道路角度的變化，因此假設道路角度為常數。

接著，透過方程式(16’)到(18’)來建立訊號混合系統的輸出方程式()：

其中為車體座標相對於地球座標的估測速度向量。

P _i ₃ 為矩陣中第i列第3行的元素，其包含,。

Q _i ₃ 為矩陣中第i列第3行的元素，其包含,,,,。

藉由方程式(19’)與(20’)即可完成訊號混合系統的設計，此訊號混合系統利用卡曼濾波器技術與系統輸出誤差()，除了可以估測出加速度感測器訊號偏差(,,)與角速度感測器訊號偏差(,,)以修正慣性量測單元的訊號偏差現象，亦可估測出車輛絕對位置(,,)、車輛絕對速度(,,)、車輛絕對姿態(,,)以補足全球衛星定位系統取樣週期區間的訊號，更可以改善絕對垂直位置、絕對側傾角與絕對俯仰角的量測準確度。其演算法步驟如下：

1.選擇車輛相對姿態與道路角度的初始條件()並設定k =0

2.透過卡曼濾波器技術與系統動態()，計算增益值(L )

3.藉由訊號混合系統的輸出誤差()來更新訊號混合系統動態：

4.判斷訊號混合系統是否要結束，是，結束估測；否，設定k =k +1，並回到步驟2。

操作方塊圖

請參閱圖7，為本發明另一實施例的操作方塊圖。其中，首先訊號混合系統13(感測系統)包含全球衛星定位系統2、慣性量測單元4(三軸角速度感測器)、以及懸掛系統位移感測器3。全球衛星定位系統2量測接收天線的相對位置，並藉由GPS訊號處理2a的方程式(4’)計算車輛絕對位置(絕對縱向位置、絕對側向位置與絕對垂直位置)、與方程式(5’)計算車輛絕對速度(絕對縱向速度、絕對側向速度與絕對垂直速度)、與方程式(6’)計算車輛絕對姿態(絕對航向角、絕對側傾角與絕對俯仰角)；慣性量測單元4量測車體線性加速度與旋轉角速度，並藉由慣性訊號處理4a的方程式(10’)獲得車輛絕對加速度以及方程式(12’)獲得車輛絕對姿態的角速度；懸掛系統位移感測器3量測車輛四邊的懸掛系統彈簧長度，並藉由位移訊號處理3a的方程式(14’)計算車輛相對垂直位置、車輛側傾角、車輛俯仰角，接著將全球衛星定位系統2與懸掛系統位移感測器3的量測資訊處理為實際系統輸出方程式，如方程式(16’)到(18’)所示，最後將上述資訊輸入至包含本發明所提出的訊號混合系統13的一處理器，即可獲得比原始量測資訊更為準確的包含估測絕對垂直位置()、估測絕對側傾角()、估測絕對俯仰角()的輸出資訊5。

訊號流程圖

請參閱圖8，為本發明的另一實施例的訊號混合系統的訊號流程圖。其中揭露的混合步驟如下：

1.裝設一組感測系統於車輛上130(上例採用全球衛星定位系統、慣性量測單元與懸掛系統位移感測器)。

2.藉由上述感測系統獲得包括三根天線的絕對位置、三軸車輛線性加速度、三軸車輛旋轉角速度、以及四邊懸掛系統彈簧長度131。

3.藉由三軸車輛線性加速度運算一絕對縱向加速度、一絕對側向加速度、以及一絕對垂直加速度132，如方程式(10’)。

4.藉由三軸車輛旋轉角速度運算一絕對航向角速度、一絕對側傾角速度、以及一絕對俯仰角速度133，如方程式(12’)。

5.將上述資訊輸入一處理器，並做為訊號混合系統的輸入134，如方程式(19’)。

6.藉由四邊懸掛系統彈簧長度運算一車輛相對垂直位置、一車輛側傾角、一車輛俯仰角135，如方程式(14’)。

7.藉由三根天線的絕對位置運算一絕對縱向位置、一絕對側向位置、一絕對垂直位置、一絕對縱向速度、一絕對側向速度、一絕對垂直速度、一絕對航向角、一絕對側傾角、一絕對俯仰角136，如方程式(4’)到(6’)。

8.將上述資訊輸入一處理器，並做為訊號混合系統的輸出137，如方程式(20’)。

9.藉由訊號混合系統的輸出誤差與卡曼濾波器技術，可獲得一增益值並疊代運算，直到估測結束138。

10.即可獲得改善過後的一絕對垂直位置、一絕對側傾角、一絕對俯仰角139。

模擬結果

本發明透過模擬的方式來比較一般的信號混合系統與本發明所提出的訊號混合系統的量測誤差改善倍率。此例假設車輛行駛於平地路面(道路角度為0)，經過四秒後，駕駛者操作方向盤使車輛往左邊轉，並在八秒後將方向盤歸正，從模擬結果中可以看出，一般訊號混合系統僅能大致上獲得動態資訊，其準確度較低，甚至無法偵測絕對垂直位置，但是本發明所提出的訊號混合系統能夠成功地且準確地獲得動態資訊，其兩者訊號誤差標準差請參閱表1，為本發明與一般訊號混合系統的訊號誤差標準差比較表。

總體而言，本發明的能達到的效果是：本發明僅需要兩種感測器即可獲得車輛姿態與道路角度，大幅降低車輛姿態估測所需要的費用與硬體設備。

本發明以三個座標系統的角度關係為基礎，可以將全球衛星定位系統所量測的車輛絕對姿態，成功地拆解為車輛相對姿態與道路角度。且本發明所能夠提供的車輛定位(如絕對垂直座標)與姿態感測(包含：絕對側傾角、絕對俯仰角)是更為精準的。

本發明所提供之較準確之車輛動態資訊，可以與各式汽車電子產品相結合，例如：車輛姿態穩定系統、車輛防止顛簸系統、行車紀錄器、車輛導航系統…等，以提高行車安全、舒適性與車輛附加價值。

本發明能夠提供車輛較準確的垂直座標，以輔助「車輛導航系統」開啟正確的地圖連結，例如：可以判定目前車輛進入或離開高架橋。

本發明能夠提供車輛較準確的垂直座標，用來修正加速度感測器的量測偏差現象，以輔助「車輛防止顛簸系統」進行適當的控制。

本發明可以估測隨時間變化的道路角度。

由此可見，因為本發明使用了三種感測器，即全球衛星定位系統、懸掛系統位移感測器、以及三軸角速度感測器，而由於直接使用了此三種感測器中所慣用的尤拉角予以計算，使得本發明在計算工具上的需求得以降低，對於成本的降低更具有莫大的助益。

上述各個實施例僅係為了方便說明而舉例，雖遭熟悉本技藝之人士任施匠思而為諸般修飾，然皆不脫如附申請專利範圍所欲保護者。

11．．．第一估測系統

110．．．第一估測系統之步驟a

111．．．第一估測系統之步驟b

112．．．第一估測系統之步驟c

113．．．第一估測系統之步驟d

114．．．第一估測系統之步驟e

115．．．第一估測系統之步驟f

116．．．第一估測系統之步驟g

12．．．第二估測系統

120．．．第二估測系統之步驟a

121．．．第二估測系統之步驟b

122．．．第二估測系統之步驟c

123．．．第二估測系統之步驟d

124．．．第二估測系統之步驟e

125．．．第二估測系統之步驟f

126．．．第二估測系統之步驟g

13．．．訊號混合系統

130．．．混合步驟1

131．．．混合步驟2

132．．．混合步驟3

133．．．混合步驟4

134．．．混合步驟5

135．．．混合步驟6

136．．．混合步驟7

137．．．混合步驟8

138．．．混合步驟9

139．．．混合步驟10

2．．．全球衛星定位系統

2a．．．GPS訊號處理

3．．．懸掛系統位移感測器

3a．．．位移訊號處理

31-34．．．第一至第四懸掛系統位移感測器

4．．．三軸角速度感測器

4a．．．慣性訊號處理

5．．．輸出資訊

Ant-a、Ant-b、Ant-c．．．全球衛星定位系統天線接收器

D1．．．前後距離

D2．．．左右距離

d．．．左右中點

α₁ ．．．輔助角度

E^b ．．．車體座標系

E^r ．．．道路座標系

E^g ．．．地球座標系

圖1a，為本發明的第一估測系統的操作方塊圖；

圖1b，為本發明的第二估測系統的操作方塊圖；

圖2，為本發明的三個座標的示意圖；

圖3a，為本發明的第一估測系統的硬體示意圖；

圖3b，為本發明的第二估測系統的硬體示意圖；

圖4，為本發明的全球衛星定位系統與車輛的配置示意圖；

圖5a，為本發明的第一估測系統及其方法的流程圖；

圖5b，為本發明的第二估測系統及其方法的流程圖；

圖6a1至6a5，為本發明的第一估測系統的模擬圖；

圖6b1至6b5，為本發明的第二估測系統的模擬圖；

圖7，為本發明另一實施例的操作方塊圖；以及

圖8，為本發明的另一實施例的訊號混合系統的訊號流程圖。

31、32、33、34．．．懸掛系統位移感測器

Ant-a、Ant-b、Ant-c．．．全球衛星定位系統的天線

Claims

一種車輛姿態估測系統，是設置於一車輛上，該系統包括：一運算單元；一懸掛系統位移感測器，電連接至該運算單元，並用以產生一懸掛系統位移數據予該運算單元；以及一全球衛星定位系統，電連接至該運算單元，並用以產生該車輛的絕對姿態予該運算單元，其中，該運算單元透過該絕對姿態與該懸掛系統位移數據而得到該車輛的相對姿態。
如申請專利範圍第1項所述的系統，其中該運算單元更產生一道路角度。
如申請專利範圍第2項所述的系統，其中該道路角度更包括一道路傾斜角以及一道路側傾角。
如申請專利範圍第1項所述的系統，其中該絕對姿態是該車輛相對於地球的姿態。
如申請專利範圍第4項所述的系統，其中該絕對姿態包括一絕對航向角、一絕對俯仰角、以及一絕對側傾角。
如申請專利範圍第1項所述的系統，其中該相對姿態是該車輛相對於行駛的表面的姿態。
如申請專利範圍第6項所述的系統，其中該懸掛系統位移數據是來自懸掛系統兩端距離之改變。
如申請專利範圍第6項所述的系統，其中該懸掛系統位移數據是來自輪胎與車體之間距離的改變。
如申請專利範圍第6項所述的系統，其中該懸掛系統位移感測器是選自線性可變差分變壓器、超音波定位感測氣、以及雷射定位感測器。
如申請專利範圍第1項所述的系統，更包括一慣性測量單元，與該運算單元電連接，並提供一慣性數據予該運算單元。
如申請專利範圍第10項所述的系統，其中該慣性測量單元是一角速度測量器，且包含了對三個軸向的角速度、角加速度的測量。
如申請專利範圍第10項所述的系統，其中該運算單元透過該絕對姿態、該懸掛系統位移數據、以及該慣性數據而得到該車輛的絕對垂直位置、絕對側向位置、與絕對縱向位置。
一種車輛姿態估測的方法，是透過地球座標、道路座標、與車輛座標之間的位置關係，以取得車輛的姿態，該方法包括下列步驟：提供一感測系統於一車輛上；以該感測系統，量測該車輛的一絕對姿態與一懸掛系統狀態；藉由該懸掛系統狀態，得到該車輛的一相對姿態；使用該絕對姿態與該相對姿態，得到所述三個座標的一第一座標位置關係；透過該座標位置關係，計算出一道路資訊；使用該絕對姿態與該懸掛系統狀態，得到所述三個座標的一第二座標位置關係；以及透過該第一與第二座標位置關係、以及該道路資訊，計算出一車輛航向角，如此即估測出該車輛的姿態。
如申請專利範圍第13項所述的方法，其中該絕對姿態是指車輛座標相對於地球座標的姿態，並包括：一絕對航向角、一絕對側傾角、以及一絕對俯仰角。
如申請專利範圍第13項所述的方法，其中該懸掛系統狀態，是指懸掛系統的彈簧長度。
如申請專利範圍第13項所述的方法，其中該相對姿態是指車輛座標相對於道路座標的姿態，並包括：一車輛側傾角、以及一車輛俯仰角。
如申請專利範圍第13項所述的方法，其中該第一座標位置關係，是指道路座標與地球座標之間的關係。
如申請專利範圍第13項所述的方法，其中該道路資訊包括一道路傾斜角與一道路坡度角。
如申請專利範圍第13項所述的方法，其中該第二座標位置關係，是指車輛座標、道路座標、以及地球座標之間的關係。
如申請專利範圍第13項所述的方法，其中所述的車輛座標、道路座標、以及地球座標，均以尤拉角(Euler Angles)描述。
如申請專利範圍第13項所述的方法，其中該感測系統包括一運算單元、一全球衛星定位系統以及一懸掛系統位移感測器，且該全球衛星定位系統與該懸掛系統位移感測器均與該運算單元電連接。
如申請專利範圍第21項所述的方法，其中該感測系統更包括一慣性測量單元，用以提供一慣性數據給該運算單元，且所述的方法更包括一步驟：使用該慣性數據、該絕對姿態與該懸掛系統狀態，計算出該車輛的絕對縱向位置、絕對側向位置、以及絕對垂直位置。
如申請專利範圍第22項所述的方法，其中該慣性數據更包括該車輛的一絕對三軸線性加速度與一絕對三軸角速度中的一種。
如申請專利範圍第23項所述的方法，其中該絕對三軸線性加速度，包括一縱向加速度、一側向加速度、以及一垂直加速度。
如申請專利範圍第23項所述的方法，其中該絕對三軸角速度，包括一航向角速度、一側傾角速度、以及一俯仰角速度。
一種車輛姿態估測的方法，是透過地球座標、道路座標、與車輛座標之間的位置關係，以取得車輛的姿態，該方法包括下列步驟：提供一感測系統於一車輛上；以該感測系統，量測該車輛的一絕對姿態、一懸掛系統狀態、與一慣性狀態；藉由該懸掛系統狀態，得到該車輛的一相對姿態；提供一待估測動態；提供一狀態觀察器、一估測系統動態模型與一估測系統輸出，其中該估測系統動態模型是以該待估測動態建立；進行一估測程序，首先處理該絕對姿態與該懸掛系統狀態，而得到一實際系統輸出，並將該實際系統輸出與該估測系統輸出相對照而得到一系統輸出誤差，並再回授該系統輸出誤差至該狀態觀察器，直到估測程序結束；以及計算出一道路資訊與一車輛航向角，如此即估測出該車輛的姿態。
如申請專利範圍第26項所述的方法，其中該感測系統包括一運算單元、一懸掛系統位移感測器、一全球衛星定位系統、以及一慣性感測器，而該懸掛系統位移感測器、該全球衛星定位系統、與該慣性感測器均各自電連接至該運算單元。
如申請專利範圍第26項所述的方法，其中該絕對姿態是指車輛座標相對於地球座標的姿態，並包括：一絕對航向角、一絕對側傾角、以及一絕對俯仰角。
如申請專利範圍第26項所述的方法，其中該懸掛系統狀態，是指懸掛系統的彈簧長度。
如申請專利範圍第26項所述的方法，其中該相對姿態是指車輛座標相對於道路座標的姿態，並包括：一車輛側傾角、以及一車輛俯仰角。
如申請專利範圍第26項所述的方法，其中該待估測動態，包括一假設車輛絕對姿態、一假設車輛航向角、以及一假設道路角度。
如申請專利範圍第26項所述的方法，其中該待估測動態包括該車輛的一待估測絕對位置、一待估測絕對速度、一待估測慣性狀態訊號偏差。
如申請專利範圍第32項所述的方法，其中該待估測慣性狀態訊號偏差包括一待估測三軸線性加速度訊號偏差，以及一待估測三軸角速度訊號偏差。
如申請專利範圍第26項所述的方法，其中該狀態觀察器是一擴增卡曼濾波器(Extended Kalman Filter)。
如申請專利範圍第26項所述的方法，其中所述的估測程序包含下列步驟：選擇該車輛的相對姿態，以及道路資訊的初始條件；透過該狀態觀察器與該待估測動態，計算一增益值；將該增益值與該系統輸出誤差的乘積，加上該待估測動態，以得到次一個待估測動態；以及判斷該估測程序是否要結束，如是，則結束估測；如否，則以上一步驟所得到的次一個待估測動態，替換第二步驟內的該待估測動態，並進行第二步驟。
如申請專利範圍第26項所述的方法，其中該道路資訊包括一道路傾斜角與一道路坡度角。
如申請專利範圍第26項所述的方法，其中所述的車輛座標、道路座標、以及地球座標，均以尤拉角定義。
如申請專利範圍第26項所述的方法，其中該慣性狀態包含了該車輛的三軸線性加速度與三軸角速度。