TWI584157B - 移動設備及求取移動設備姿態之方法 - Google Patents

Description

移動設備及求取移動設備姿態之方法

本申請主要關於移動裝置的姿態信息求取方案，特別係有關於整合由多種不同的傳感器所產生的數據來求取移動裝置的姿態信息的技術提案。

隨著移動設備的普及，體感技術相關的應用在近年來成為熱門的發展領域，有越來越多的應用能夠透過移動設備內建的傳感器來取得移動設備的姿態信息，用以提供更為人性化及智慧的人機互動方式，其應用領域除了遊戲設計之外，還可運用在室內/室外導航系統、醫療看護、室內導覽等，適用範圍相當多元。

在一傳統做法中，移動設備裡除了主處理器之外，並未額外配置專屬的處理器用於控制傳感器，因此，即便在移動設備處於休眠模式時，主處理器仍需對傳感器進行控制以維持對姿態信息的即時掌握，若是為了確保姿態信息的精確度，主處理器必須對傳感器進行頻繁的控制，此將造成主處理器的負載及功耗增加；然而，若是為了降低主處理器的負載及功耗而降低對傳感器進行控制的頻率，則會犧牲姿態信息的精確度。

本申請之一實施例提供了一種移動設備，其包括一加速度傳感器、一磁力計傳感器、一陀螺儀傳感器、以及一處理器。上述加速度傳感器係用以產生一加速度數據。上述磁力計傳感器係用以產生一磁場強度數據。上述陀螺儀傳感器係用以產生一角速度數據。上述處理器係用以根據上述加速度數據決定一俯仰角及一橫滾角，根據上述磁場強度數據決定一航向角，決定上述俯仰角、上述橫滾角、以及上述航向角所對應之一四元數，根據上述四元數及上述角速度數據決定上述移動設備之一姿態信息，以及根據上述加速度數據及上述磁場強度數據對上述姿態信息進行修正。

本申請之另一實施例提供了一種求取移動設備姿態之方法，適用於包括一加速度傳感器、一磁力計傳感器、以及一陀螺儀傳感器之一移動設備。上述求取移動設備姿態之方法包括以下步驟：根據由一加速度傳感器產生之一加速度數據決定一俯仰角及一橫滾角；根據由一磁力計傳感器產生之一磁場強度數據決定一航向角；決定上述俯仰角、上述橫滾角、以及上述航向角所對應之一四元數；根據上述四元數以及由一陀螺儀傳感器產生之一角速度數據決定上述移動設備之一姿態信息；以及根據上述加速度數據及上述航向角對上述姿態信息進行修正。

關於本發明其他附加的特徵與優點，此領域之熟習技術人士，在不脫離本申請之精神和範圍內，當可根據本案實施方法中所揭露之移動設備及求取移動設備姿態之方法做 些許的更動與潤飾而得到。

100‧‧‧移動設備

10‧‧‧主控制器

20‧‧‧傳感器中樞

21‧‧‧微處理器

22‧‧‧記憶體

23‧‧‧傳輸介面

30‧‧‧加速度傳感器

40‧‧‧磁力計傳感器

50‧‧‧陀螺儀傳感器

60‧‧‧輸入輸出裝置

70‧‧‧儲存裝置

S301‧‧‧S311‧‧‧步驟

第1圖係根據本申請一實施例所述之移動設備之示意圖。

第2圖係根據本申請一實施例所述之傳感器中樞20之示意圖。

第3圖係根據本身請一實施例所述之求取移動設備姿態之方法流程圖。

第4圖係根據本申請一實施例所述之參考坐標系及載體座標系之示意圖。

第5圖係根據本申請一實施例所述之移動設備之運動示意圖。

第6圖係根據本申請一實施例所述針對移動設備偵測磁場強度向量之示意圖。

本章節所敘述的是實施本申請之最佳方式，目的在於說明本申請之精神而非用以限定本申請之保護範圍，應理解下列實施例可經由軟體、硬體、韌體、或上述任意組合來實現。

第1圖係根據本申請一實施例所述之移動設備之示意圖。移動設備100包括主控制器10、傳感器中樞(sensor hub)20、加速度傳感器30、磁力計傳感器40、陀螺儀傳感器50、輸入輸出裝置60、以及儲存裝置70，其中加速度傳感器30、磁力計傳感器40、陀螺儀傳感器50係操作上耦合至傳感器中樞 20，而傳感器中樞20、輸入輸出裝置60、以及儲存裝置70係操作上耦合至主控制器10。

移動設備100可為功能型手機、智慧型手機、平板電腦、智能交通載具、無人機、智能電子玩具、或任何移動式電子計算設備。

主控制器10可為通用處理器、微處理器(Micro Control Unit，MCU)、應用處理器(Application Processor，AP)、或數位訊號處理器(Digital Signal Processor，DSP)等。明確來說，主控制器10係用以提供數據處理及運算之功能、控制傳感器中樞20的運作並接收傳感器中樞20所輸出的姿態信息、從輸入輸出裝置60接收外部訊號(例如：使用者輸入訊號)或輸出訊號、以及從儲存裝置70讀取或儲存數據。

傳感器中樞20主要用以控制加速度傳感器30、磁力計傳感器40、以及陀螺儀傳感器50之運作，例如：控制各傳感器的採樣率。此外，還用以接收傳感器所產生的量測數據，並對量測數據進行處理以產生姿態信息。值得注意的是，相較於主控制器10，傳感器中樞20僅需低電量即可運作。

加速度傳感器30係用以偵測移動設備100的加速度並產生加速度數據。

磁力計傳感器40係用以偵測移動設備100的磁場強度變化並產生磁場強度數據。

陀螺儀傳感器50係用以偵測移動設備100的角速度並產生角速度數據。

在一實施例，加速度傳感器30、磁力計傳感器40、 以及陀螺儀傳感器50可個別包括一或多個硬體元件，以機構式、電子式、或電容式的方式實現數據量測的目的。

輸入輸出裝置60可包括一或多個按鈕、鍵盤、滑鼠、觸碰板、視訊鏡頭、麥克風、顯示螢幕(例如：液晶顯示器、發光二極體顯示器、或電子紙顯示器等)、以及/或喇叭等，用以作為人機介面與使用者互動。

儲存裝置70為非暫態(non-transitory)之電腦可讀取儲存媒體，例如：隨機存取記憶體、快閃記憶體、磁性儲存裝置(如：磁帶或硬碟)、光學儲存裝置(如：唯讀光碟)、或上述之任意組合，用以儲存指令集、應用及/或通訊協定之程式碼等。

當可理解的是，第1圖所示之元件僅用以提供一說明之範例，並非用以限制本發明之保護範圍。舉例來說，移動設備100還可包括一無線通訊裝置(包括：基頻訊號處理元件、射頻訊號處理元件、以及天線)，用以提供無線通訊之功能，或者，移動設備100還可包括一電源供應器、全球定位系統儀等。

第2圖係根據本申請一實施例所述之傳感器中樞20之示意圖。傳感器中樞20包括微處理器21、記憶體22、以及傳輸介面23，其中記憶體22及傳輸介面23係操作上耦合至微處理器21，而傳感器中樞20可藉由傳輸介面23操作上耦合至主控制器10、加速度傳感器30、磁力計傳感器40、以及陀螺儀傳感器50。

微處理器21可為一微型處理器，例如：微控制器 (Micro-Control Unit，MCU)，其可包括各式電路邏輯，用以透過傳輸介面23控制加速度傳感器30、磁力計傳感器40、以及陀螺儀傳感器50之運作、處理各個傳感器的量測數據以決定移動設備100的姿態信息、以及從記憶體22中讀取或寫入資料。特別是，微處理器21係用以協調控制加速度傳感器30、磁力計傳感器40、以及陀螺儀傳感器50之運作，以執行本申請的求取移動設備姿態之方法。

該領域之熟習技藝人士當可理解，微處理器21中的電路邏輯通常可包括多個電晶體，用以控制該電路邏輯之運作以提供所需之功能及作業。更進一步的，電晶體的特定結構及其之間的連結關係通常是由編譯器所決定，例如：暫存器轉移語言(Register Transfer Language，RTL)編譯器可由處理器所運作，將類似組合語言碼的指令檔(script)編譯成適用於設計或製造該電路邏輯所需之形式。

記憶體22可為非暫態之電腦可讀取儲存媒體，例如：隨機存取記憶體、或快閃記憶體，用以儲存本申請之方法的指令集或程式碼。另外，記憶體22還可儲存來自加速度傳感器30、磁力計傳感器40、以及陀螺儀傳感器50之量測數據以及數據運算過程中的暫存數據。

傳輸介面23係用以提供傳感器中樞20與傳感器中樞20中的其他元件之間的數據及訊號傳輸與接收，包括：接收來自加速度傳感器30、磁力計傳感器40、以及陀螺儀傳感器50之量測數據，傳送控制訊號至加速度傳感器30、磁力計傳感器40、以及陀螺儀傳感器50，接收來自主控制器10的控制信號， 以及傳送移動設備100的姿態信息給主控制器10。

第3圖係根據本身請一實施例所述之求取移動設備姿態之方法流程圖。在此實施例，本申請所述之求取移動設備姿態之方法係適用於移動設備100，特別係由傳感器中樞20的微處理器21所執行。

首先，在步驟S301，微處理器21決定以導航座標系(North East Down，NED)為參考座標系O-X _n Y _n Z _n ，如第4圖所示，即x軸指向正北方(以N標示)、y軸指向正東方(以E標示)、z軸往下指向地心(以D標示)，同時，將移動設備100平放於例如桌面的水平平面，決定移動設備100的載體座標系(body frame)O-X _b Y _b Z _b ，即x軸指向移動設備100的前方(以x標示)、y軸指向移動設備100的右方(以y標示)、z軸指向移動設備100的下方(以z標示)。

接著，在步驟S302，微處理器21決定參考座標系與載體座標系之間的座標變換矩陣。明確來說，當移動設備100的空間姿態因一運動而產生變化時，其姿態(orientation)可視為是在導航座標系中依次繞z軸、y軸、x軸作3次基本旋轉(依次以Yaw、Pitch、Roll標示)而得到的結果。其中，繞z軸旋轉的角度可定義為航向角ψ，繞y軸旋轉的角度可定義為俯仰角θ，繞x軸旋轉的角度可定義為橫滾角。

根據上述定義的歐拉角(由航向角ψ、俯仰角θ、橫滾角所構成)，進一步決定空間變換的關係可如下表示： 其中，由座標系O-X _n Y _n Z _n 繞Z _n 軸旋轉ψ角度而得到座標系O -X ₁ Y ₁ Z ₁ ，其變換的方向余弦矩陣為R _z (ψ)；由座標系O-X ₁ Y ₁ Z ₁ 繞Y ₁ 軸旋轉θ角度而得到座標系O-X ₂ Y ₂ Z ₂ ，其變換的方向余弦矩陣為R _x (θ)；由座標系O-X ₂ Y ₂ Z ₂ 繞X ₂ 軸旋轉角度而得到座標系O-X _b Y _b Z _b ，其變換的方向余弦矩陣為R _x ()。

其中，座標系O-X _n Y _n Z _n 為參考座標系(即導航座標系)，座標系O-X _b Y _b Z _b 為載體座標系。根據上述空間變換的關係，決定導航座標系與載體座標系之間的座標變換矩陣，如下公式(1)所示： 由於旋轉屬於正交的轉換關係，而正交矩陣的性質可表示為以下公式(2)： 其中，座標變換矩陣即為姿態矩陣，其可表示為以下公式(3)：

在確定座標系以及姿態矩陣之後，流程前進到步驟S303，微處理器21首先將加速度傳感器30所偵測到的加速度數據進行向量標準化處理，得到標準化的加速度向量。在一實施例，假設{x,y,z}為代表加速度數據的向量值，則向量標準化處理可以是如以下公式(4)所示：

在步驟S304，微處理器21根據標準化的加速度向量決定對應之俯仰角θ及橫滾角。明確來說，移動設備100的 運動如第5圖所示，其中，座標系O-X _n Y _n Z _n 為參考座標系(即導航座標系)，座標系O-X _b Y _b Z _b 為載體座標系，並通過以下公式(5)計算出俯仰角θ及橫滾角。

其中，[A_x,A_y,A_z]^T為標準化的加速度向量。

在步驟S305，微處理器21根據磁力計傳感器40所產生的磁場強度數據(即磁場強度向量)決定對應之航向角ψ。明確來說，磁力計傳感器40對移動設備100的偵測如第6圖所示，其中，與為磁場強度在水平座標系的x軸與y軸上的分量，而磁力計傳感器40針對3個軸所測得的地磁強度與姿態矩陣之間的關係可表示為以下公式(6)。

故可透過上述公式(6)計算出航向角。

在步驟S306，微處理器21根據步驟S304及S305所得到的歐拉角來計算其相應之四元數q=[q₀,q₁,q₂,q₃]^T。其中，q₀、q₁、q₂、q₃由以下公式(7)計算得到。

在步驟S307，微處理器21對四元數q進行向量標 準化處理，得到標準化的四元數Q。在一實施例，向量標準化處理可以是如以下公式(8)所示。

在步驟S308，微處理器21對標準化的四元數進行求異，其具體計算過程如以下公式(9)。

其中，w為陀螺儀傳感器50針對移動設備100所偵測到的角速度，Q為步驟S307所得到的標準化的四元數。根據公式(9)可進一步得到以下公式(10)。

接著對公式(10)進行泰勒展開(具體來說是對公式中以e為底的指數部分進行泰勒展開)則可得到四元數的更新公式，即擴展卡爾曼濾波器中的狀態轉移矩陣F。

在步驟S309，微處理器21使用擴展卡爾曼濾波器將四元數作為估計量建立狀態預測公式(11)如下。

其中，F為步驟S380所得到的狀態轉移矩陣，為使用四元數表示的預測姿態(q₀,q₁,q₂,q₃)，為使用四元數表示的上一時刻的最佳估計量，P _t-1為上一時刻的斜方差矩陣，為當前時刻推測的斜方差矩陣，Q為預測模型所引入的噪聲斜方差矩陣，針對不同的傳感器及不同的應用場景，Q的值可作適當調整， 例如：一個合理的值可以是10^-4量級。也就是說，透過公式(11)可得到移動設備100的預測姿態信息。

在步驟S310，微處理器21根據擴展卡爾曼濾波器的狀態轉移矩陣以及加速度傳感器30的偵測數據(即加速度數據)對步驟S309所得到的預測姿態信息進行修正。明確來說，量測方程如以下公式(12)所示。

其中(C₀₂,C₁₂,C₂₂)’為姿態矩陣的第三列，接著再根據如以下公式(13)計算出修正後的狀態和協方差。

其中Z_t為當前時刻加速度傳感器30的量測，h()為當前的測量預測值。

在步驟S311，微處理器21根據擴展卡爾曼濾波器的狀態轉移矩陣以及磁力計傳感器40的偵測數據(即磁場強度數據)對步驟S309所得到的預測姿態信息進行修正。明確來說，量測方程如以下公式(14)所示。

其中C₀₁為姿態矩陣的第一行第二列元素，C₀₀為姿態矩陣的第一行第一列元素。

在步驟S311結束之後所得到的四元數即為完成修正的預測姿態信息，預期將更精準的反應移動設備100的實際 姿態。在一實施例，微處理器21還可進一步將四元數轉換為相應的歐拉角。

根據第3圖之實施例，當可理解的是，本申請的求取移動設備姿態之方法其特徵在於使用擴展卡爾曼濾波器來預測移動設備的姿態，此外，還進一步將加速度傳感器及磁力計傳感器的偵測數據反饋對預測姿態信息進行修正。由於陀螺儀傳感器在靜態時容易隨著時間而產生較大誤差的偵測數據，所以本申請透過加速度傳感器及磁力計傳感器的再次反饋來修正預測姿態信息，預期將比傳統作法能更精準的反應移動設備的實際姿態。

本申請雖以各種實施例揭露如上，然而其僅為範例參考而非用以限定本申請的範圍，任何熟習此項技藝者，在不脫離本申請之精神和範圍內，當可做些許的更動與潤飾。因此上述實施例並非用以限定本申請之範圍，本申請之保護範圍當視後附之申請專利範圍所界定者為準。

100‧‧‧移動設備

10‧‧‧主控制器

20‧‧‧傳感器中樞

30‧‧‧加速度傳感器

40‧‧‧磁力計傳感器

50‧‧‧陀螺儀傳感器

60‧‧‧輸入輸出裝置

70‧‧‧儲存裝置

Claims (16)

1. 一種移動設備，包括：一加速度傳感器，用以產生一加速度數據；一磁力計傳感器，用以產生一磁場強度數據；一陀螺儀傳感器，用以產生一角速度數據；以及一處理器，用以根據上述加速度數據決定一俯仰角及一橫滾角，根據上述磁場強度數據決定一航向角，決定上述俯仰角、上述橫滾角、以及上述航向角所對應之一四元數，根據上述四元數及上述角速度數據決定上述移動設備之一姿態信息，以及根據上述加速度數據及上述磁場強度數據對上述姿態信息進行修正。
2. 如申請專利範圍第1項所述之移動設備，其中上述處理器係根據上述四元數及上述角速度數據決定一擴展卡爾曼濾波器中之一狀態轉移矩陣，以及使用上述擴展卡爾曼濾波器並根據上述狀態轉移矩陣及上述四元數決定上述姿態信息。
3. 如申請專利範圍第2項所述之移動設備，其中上述處理器係使用上述擴展卡爾曼濾波器並根據上述狀態轉移矩陣及上述加速度數據對上述姿態信息進行修正，以及使用上述擴展卡爾曼濾波器並根據上述狀態轉移矩陣及上述磁場強度數據對上述姿態信息進行修正。
4. 如申請專利範圍第1項所述之移動設備，其中上述姿態信息包括一估計之四元數。
5. 如申請專利範圍第4項所述之移動設備，其中上述處理器還決定上述估計之四元數所對應之一歐拉角。
6. 如申請專利範圍第1項所述之移動設備，其中上述處理器還決定以一導航座標系為決定上述俯仰角、上述橫滾角、以及上述航向角之參考座標系，以及決定上述導航座標系與上述移動設備之一載體座標系之間之一座標變換矩陣。
7. 如申請專利範圍第1項所述之移動設備，其中上述處理器還在決定上述俯仰角及上述橫滾角之前先對上述加速度數據進行向量標準化處理。
8. 如申請專利範圍第1項所述之移動設備，其中上述處理器還在決定上述姿態信息之前先對上述四元數進行向量標準化處理。
9. 一種求取移動設備姿態之方法，適用於包括一加速度傳感器、一磁力計傳感器、以及一陀螺儀傳感器之一移動設備，上述求取移動設備姿態之方法包括：根據由上述加速度傳感器產生之一加速度數據決定一俯仰角及一橫滾角；根據由上述磁力計傳感器產生之一磁場強度數據決定一航向角；決定上述俯仰角、上述橫滾角、以及上述航向角所對應之一四元數；根據上述四元數以及由上述陀螺儀傳感器產生之一角速度數據決定上述移動設備之一姿態信息；以及根據上述加速度數據及上述磁場強度數據對上述姿態信息進行修正。
10. 如申請專利範圍第9項所述之求取移動設備姿態之方法，其 中上述決定姿態信息之步驟包括：根據上述四元數及上述角速度數據決定一擴展卡爾曼濾波器中之一狀態轉移矩陣；以及使用上述擴展卡爾曼濾波器並根據上述狀態轉移矩陣及上述四元數決定上述姿態信息。
11. 如申請專利範圍第10項所述之求取移動設備姿態之方法，其中上述對姿態信息進行修正之步驟包括：使用上述擴展卡爾曼濾波器並根據上述狀態轉移矩陣及上述加速度數據對上述姿態信息進行修正；以及使用上述擴展卡爾曼濾波器並根據上述狀態轉移矩陣及上述磁場強度數據對上述姿態信息進行修正。
12. 如申請專利範圍第9項所述之求取移動設備姿態之方法，其中上述姿態信息包括一估計之四元數。
13. 如申請專利範圍第12項所述之求取移動設備姿態之方法，還包括：決定上述估計之四元數所對應之一歐拉角。
14. 如申請專利範圍第9項所述之求取移動設備姿態之方法，還包括：決定以一導航座標系為決定上述俯仰角、上述橫滾角、以及上述航向角之參考座標系；以及決定上述導航座標系與上述移動設備之一載體座標系之間之一座標變換矩陣。
15. 如申請專利範圍第9項所述之求取移動設備姿態之方法，還包括：在決定上述俯仰角及上述橫滾角之前先對上述加速度數據進行向量標準化處理。
16. 如申請專利範圍第9項所述之求取移動設備姿態之方法，還包括：在決定上述姿態信息之前先對上述四元數進行向量標準化處理。