本發(fā)明涉及物體姿態(tài)估計的方法及裝置，更具體地，涉及用于估計運動物體的姿態(tài)的方法及裝置。

背景技術(shù)：

現(xiàn)在為了估計運動物體的姿態(tài)和方向，傳統(tǒng)上通過四元數(shù)或歐拉角(俯仰(pitch)角、滾轉(zhuǎn)(roll)角和偏航(yaw)角)來表示物體姿態(tài)。兩種類型的表示可以互相轉(zhuǎn)換。此外，已知四元數(shù)表示從一個坐標系統(tǒng)到另一個坐標系統(tǒng)的變換。這種變換也可以表示為轉(zhuǎn)換矩陣。轉(zhuǎn)換矩陣的典型示例用于將矢量從地心坐標系(level frame)(例如，東、北、上)變換到體坐標系。

針對在無失真的地磁場中的靜止物體，可以通過加速計數(shù)據(jù)來計算靜止物體的姿態(tài)，或者基于加速計數(shù)據(jù)和磁力計數(shù)據(jù)來充分且精確地計算靜止物體的姿態(tài)。然而，針對運動物體，將重力加速度和運動加速度合并為來自加速計的測量。在知道物體姿態(tài)之前，無法明確地將它們分開。此外，在物體周圍的地磁場常因鐵或磁性材料而失真。這種影響給基于加速計數(shù)據(jù)或基于加速計數(shù)據(jù)和磁力計數(shù)據(jù)的姿態(tài)估計結(jié)果帶來偏差。

另一方面，如果知道上次的物體姿態(tài)，則能夠通過陀螺儀數(shù)據(jù)來估計物體姿態(tài)。來自陀螺儀的測量基本上不受上述環(huán)境噪聲(物體運動、磁場失真等)的影響。然而，估計處理基于積分運算。常常遇到零漂移問題，這會對姿態(tài)估計結(jié)果引起累積偏差。

在相關(guān)文獻中，公開了不同姿態(tài)估計方法。針對該問題的典型解決方案是將來自兩種類型的傳感器數(shù)據(jù)的估計結(jié)果融合在一起。兩種廣泛使用的數(shù)據(jù)融合方法是基于卡爾曼濾波器法和基于增益調(diào)度法。其中，增益調(diào)度是利用線性控制器族來控制非線性系統(tǒng)的最流行的方法之一。 增益調(diào)度基于調(diào)度變量的當前測量來調(diào)整控制器增益。用戶能夠設(shè)計控制器增益如何隨測量的變量而改變。在姿態(tài)估計系統(tǒng)中，利用預(yù)定義調(diào)度基于傳感器測量來計算增益。該增益用于合并所述兩個估計結(jié)果或校準陀螺儀數(shù)據(jù)以改善物體姿態(tài)。

美國專利第US8688403 B2號公報公開了基于慣性傳感器數(shù)據(jù)來估計物體姿態(tài)的方法。該方法使用來自加速計和磁力計的數(shù)據(jù)計算第一姿態(tài)矩陣梯度，然后使用來自陀螺儀的數(shù)據(jù)計算第二姿態(tài)矩陣梯度。通過混合系數(shù)來使第一姿態(tài)矩陣梯度與第二姿態(tài)矩陣梯度融合。最后，融合的梯度用于更新精細的姿態(tài)矩陣。在此，通過將來自加速計的數(shù)據(jù)與重力進行比較，并且還將來自磁力計的數(shù)據(jù)與一般已知當?shù)卮艌鲞M行比較，來確定所述混合系數(shù)。

作為基于增益調(diào)度法的非常重要的步驟，增益(混合系數(shù))計算步驟并未被清楚地記載在該專利中，而是在發(fā)明說明書中僅被粗略地說明并憑經(jīng)驗被定義。當物體突然或不規(guī)則地運動時，運動物體的姿態(tài)改變很大，如何設(shè)計基于測量的、適應(yīng)的校正增益是針對該方法的關(guān)鍵問題。

此外，理想地講，在地心坐標系中重力矢量是常數(shù)(0，0，g)，但是來自不同加速計的重力測量可能略微不同，因此如何精確地從加速計數(shù)據(jù)中去除重力分量是物體姿態(tài)估計的挑戰(zhàn)所在。

另外，不存在定量地描述物體的運動加速度與基于姿態(tài)估計的加速計/磁力計數(shù)據(jù)的偏差之間的關(guān)系的理論數(shù)學函數(shù)。在該專利中憑經(jīng)驗來估計混合系數(shù)。不存在驗證計算出的混合系數(shù)的有效性和正確性的步驟。

技術(shù)實現(xiàn)要素：

因此，在本公開中提出了新的姿態(tài)估計方法和裝置，以解決上述問題中的至少一個，來提高在真實世界中的運動物體的姿態(tài)估計的精確度。本發(fā)明基于陀螺儀數(shù)據(jù)來估計姿態(tài)，然后基于傳感器數(shù)據(jù)來計算校正增益和校正矢量。校正增益和校正矢量用于校準陀螺儀數(shù)據(jù)，最后校準的陀螺儀數(shù)據(jù)用于更新物體姿態(tài)。

根據(jù)本發(fā)明的一個方面，提供了一種姿態(tài)估計方法，所述姿態(tài)估計方法包括：校正增益確定步驟，其用于基于利用所述陀螺儀數(shù)據(jù)和加速計數(shù)據(jù)而測量的物體運動程度，來確定校正增益；校正矢量計算步驟，其用于通過分別基于所述陀螺儀數(shù)據(jù)和加速計，將所述參考矢量從地心坐標系轉(zhuǎn)換到體坐標系，來計算校正矢量，然后進行兩個轉(zhuǎn)換的參考矢量之間的叉積運算，以生成校正矢量；傳感器數(shù)據(jù)校準步驟，其用于使用所述校正增益和所述校正矢量，來校準所述陀螺儀數(shù)據(jù)；姿態(tài)估計步驟，用于利用經(jīng)校準的陀螺儀數(shù)據(jù)來估計所述姿態(tài)；此外，所述校正增益確定步驟還包括以下步驟：基于所述加速計數(shù)據(jù)來估計在體坐標系中的重力分量；通過從加速度數(shù)據(jù)中去除重力分量來計算所述物體的運動加速度；基于所述物體的所述運動加速度的預(yù)定義函數(shù)來計算所述校正增益。此外，所述姿態(tài)估計方法還包括調(diào)整步驟，所述調(diào)整步驟基于利用當前陀螺儀數(shù)據(jù)和經(jīng)校準的陀螺儀數(shù)據(jù)計算的姿態(tài)估計偏差，來調(diào)整所述校正增益。

本發(fā)明旨在使用校正矢量和適應(yīng)的校正增益來校準陀螺儀數(shù)據(jù)，以精確地估計物體姿態(tài)。因此，本發(fā)明由三個模塊構(gòu)成：校正矢量生成、校正增益確定和物體姿態(tài)校準，由此在物體處于靜止的情況下，本發(fā)明能夠增強基于加速計數(shù)據(jù)的物體姿態(tài)估計，而在物體以高加速度運動的情況下，減弱基于加速計數(shù)據(jù)的物體姿態(tài)估計。此外，基于姿態(tài)估計偏差來調(diào)整在上次估計處理中計算出的校正增益。

在本文中呈現(xiàn)的本發(fā)明的優(yōu)勢是，能夠使得物體姿態(tài)估計更精確，具體而言，當物體突然運動時，由于本發(fā)明通過使用校正矢量和校正增益以直接校準陀螺儀數(shù)據(jù)，因此可以將不同的傳感器數(shù)據(jù)融合以精確地估計物體姿態(tài)。

附圖說明

圖1是示出能夠執(zhí)行本發(fā)明的實施例的計算機系統(tǒng)的硬件構(gòu)造的框圖。

圖2是示出物體姿態(tài)估計裝置的構(gòu)造的框圖。

圖3是示出根據(jù)本發(fā)明的一個實施例的裝置進行物體姿態(tài)估計的流程圖。

圖4是根據(jù)本發(fā)明的一個實施例的校正增益的確定方法的流程圖。

圖5A是示出根據(jù)本發(fā)明的一個實施例的校正增益確定的線性函數(shù)的示意圖。

圖5B是示出根據(jù)本發(fā)明的一個實施例的校正增益確定的高斯函數(shù)的示意圖。

圖6是根據(jù)本發(fā)明的一個實施例的校正矢量的生成方法的流程圖。

具體實施方式

參照上述所列附圖，本部分描述了特定實施例及其詳細結(jié)構(gòu)和操作。請注意，下文將要描述的實施例僅以說明而不是限制的方式進行闡述，因此它們不限制本發(fā)明的范圍且能夠在本發(fā)明的范圍內(nèi)被改變?yōu)楦鞣N形式。在此，鑒于該教導(dǎo)，本領(lǐng)域的技術(shù)人員能夠認識到存在與在此描述的示例實施例等同的范圍。

圖1是示出能夠執(zhí)行本發(fā)明的實施例的計算機系統(tǒng)的硬件構(gòu)造的框圖。

如圖1所示，系統(tǒng)至少包括計算機100，該計算機100包括CPU 101、RAM 102、ROM 103、系統(tǒng)總線104、輸入設(shè)備105、輸出設(shè)備106和驅(qū)動器107。例如，計算機100是物體姿態(tài)估計系統(tǒng)或姿態(tài)航向參考系統(tǒng)(AHRS)。注意，計算機100可以包括一個或多個計算機，并且多個計算機可以個別地實現(xiàn)計算機100的各功能。

CPU 101根據(jù)RAM 102或ROM 103中存儲的程序執(zhí)行全部過程。當CPU 101進行諸如本發(fā)明的實施例等的各種處理時，RAM 102被用作臨時存儲區(qū)域。

輸入設(shè)備105可以是測量設(shè)備(例如，傳感器、加速計、陀螺儀或磁力計)或用戶輸入接口或網(wǎng)絡(luò)接口，其使得用戶能夠向計算機100發(fā) 出各種指令。

輸出設(shè)備106包括輸出外圍接口、顯示設(shè)備(即，監(jiān)視器、CRT、液晶顯示器或圖形控制器)，其使得用戶能夠輸出本發(fā)明的校準的姿態(tài)結(jié)果。

驅(qū)動器107被構(gòu)造為驅(qū)動諸如硬盤、存儲卡或光盤(例如，CD-ROM或DVD ROM)等存儲介質(zhì)。例如，測量數(shù)據(jù)或用于進行物體姿態(tài)估計處理的程序被存儲在存儲介質(zhì)中，并被驅(qū)動器107驅(qū)動。

系統(tǒng)總線104連接CPU 101、RAM 102、ROM 103、輸入設(shè)備105、輸出設(shè)備106和驅(qū)動器107。數(shù)據(jù)在系統(tǒng)總線104上進行通信。如本文所用，術(shù)語“連接”是指邏輯上或物理上直接地連接或者通過一個或更多媒介間接地連接。

圖1中示出的系統(tǒng)僅是說明性的，并不旨在限制本發(fā)明，包括其應(yīng)用或使用。例如，當啟動用于進行物體姿態(tài)估計處理的程序時，通過執(zhí)行本發(fā)明中公開的所有步驟(例如，圖3、圖4和圖6中例示的步驟)，CPU 101從輸入設(shè)備105獲得輸入測量數(shù)據(jù)，計算用于校準輸入數(shù)據(jù)的校正矢量和校正增益，并獲得重新估計的或更新的姿態(tài)。之后，CPU 101通過系統(tǒng)總線104將結(jié)果發(fā)送到輸出設(shè)備106。也能夠?qū)⒔Y(jié)果存儲在RAM 102中。經(jīng)由網(wǎng)絡(luò)接口，也能夠?qū)⒔Y(jié)果發(fā)送到遠程計算機以用于其他應(yīng)用。

此外，能夠執(zhí)行本發(fā)明的裝置(例如，圖2例示的裝置)的各個單元、設(shè)備、部件和/或裝配，它們被構(gòu)造為通過軟件、硬件、固件及其任意組合來進行姿態(tài)估計。

圖2是示出物體姿態(tài)估計裝置的構(gòu)造的框圖。圖3是示出由圖2示出的裝置進行的物體姿態(tài)估計的流程圖。CPU 101使用RAM 102或ROM 103中存儲的程序和測量數(shù)據(jù)來執(zhí)行本發(fā)明的方法。

如圖2所示，在此，物體姿態(tài)估計裝置200可以包括：傳感器數(shù)據(jù)輸入單元201、校正增益確定單元202、校正矢量生成單元203、傳感器數(shù)據(jù)校準單元204以及姿態(tài)更新和輸出單元205。

裝置200將首先從陀螺儀數(shù)據(jù)的積分來估計大致的當前姿態(tài)。因此， 傳感器數(shù)據(jù)輸入單元201被構(gòu)造為針對姿態(tài)估計裝置200，通過測量設(shè)備206來獲取運動物體的當前姿態(tài)。在一個實施例中，輸入可以是來自加速計和陀螺儀的三維數(shù)據(jù)，有時，磁力計數(shù)據(jù)也可以是輸入。

校正增益確定單元202被構(gòu)造為通過考慮加速度值的幅值以使其變?yōu)檫m應(yīng)的值，來確定校正增益。CPU 101控制校正增益確定單元202來執(zhí)行圖3所示的步驟S301，以基于當前物體運動程度計算校正增益。在一個實施例中，校正增益確定單元202能夠基于物體運動加速度的范數(shù)來計算校正增益。此外，能夠基于歷史姿態(tài)估計結(jié)果、歷史校正增益以及基于陀螺儀或加速計/磁力計數(shù)據(jù)的當前姿態(tài)估計，來調(diào)整校正增益。

校正增益確定單元202基于來自傳感器的即時測量和校正增益的歷史數(shù)據(jù)，來計算校正增益。此外，基于在最近一次估計處理中計算的姿態(tài)估計偏差，來驗證并調(diào)整計算出的校正增益。基于以下概念進行設(shè)計：

1.物體的運動加速度越接近零(靜止或以勻速速度運動)，則基于加速計數(shù)據(jù)計算的物體的傾斜角(俯仰和滾轉(zhuǎn))越精確。

2.磁力計數(shù)據(jù)越接近當前位置的理論地磁場，則基于磁力計數(shù)據(jù)計算的物體的方向(偏航角)越精確。

3.此外，陀螺儀數(shù)據(jù)基本上不受物體加速度和磁場失真的影響?；谕勇輧x數(shù)據(jù)的姿態(tài)估計偏差來自陀螺儀數(shù)據(jù)的零漂移和附加的機械噪聲。如果已知最近一次的物體的真實姿態(tài)，并且從最近估計到當前估計的時間差固定，則推論出使用陀螺儀數(shù)據(jù)的姿態(tài)估計偏差是穩(wěn)定的。利用校準的陀螺儀數(shù)據(jù)，能夠精確地估計被認為是物體的真實姿態(tài)的物體姿態(tài)。利用所述真實姿態(tài)，能夠計算并分析基于當前陀螺儀數(shù)據(jù)的姿態(tài)估計偏差。如果觀察到某些反常的姿態(tài)估計的偏差，則能夠調(diào)整校正增益以解決該問題。這是校正增益驗證和調(diào)整的步驟。

現(xiàn)在轉(zhuǎn)到圖4，圖4是根據(jù)本發(fā)明的一個實施例的校正增益的確定方法的流程圖。

在步驟S401中，CPU 101基于給定的傳感器數(shù)據(jù)檢測物體的靜止狀態(tài)。在一個實施例中，陀螺儀數(shù)據(jù)用于判斷靜止狀態(tài)。首先，定義在靜 止狀態(tài)下的最大角速率幅值的閾值。由用戶根據(jù)應(yīng)用的環(huán)境，將各軸上的陀螺儀數(shù)據(jù)與第一預(yù)定義閾值進行比較。例如，當用戶手握住物體時，可以給出相對更大的閾值，以過濾無意識的手抖動；而可以給出相對更小的閾值以增大檢測的靈敏度。假設(shè)角速率的單位是弧度/秒，本發(fā)明中的閾值的默認值是0.05。如果每個軸上的陀螺儀數(shù)據(jù)的幅值都小于閾值，則物體被判斷為處于靜止，反之亦然。

在一個實施例中，陀螺儀數(shù)據(jù)的范數(shù)被計算，并與第二預(yù)定義閾值進行比較。如果范數(shù)小于閾值，則物體被判斷為處于靜止。

在另一實施例中，加速計數(shù)據(jù)的變化率被計算，并與第三預(yù)定義閾值進行比較，以判斷物體是否靜止。能夠通過不同時間在3個軸上的加速計數(shù)據(jù)的差來計算變化率。然后，通過兩個采樣時間的間隔來分割在3個軸上的加速計數(shù)據(jù)的差。最后，計算加速計數(shù)據(jù)的3個歸一化的差的范數(shù)，作為變化率。在本發(fā)明中，閾值的默認值是0.5，然而，如前所述，可以根據(jù)應(yīng)用環(huán)境來進行調(diào)整。

在又一實施例中，如上述所述的、基于加速計數(shù)據(jù)的靜止狀態(tài)檢測方法，來計算磁力計數(shù)據(jù)的變化率。

在步驟S402中，CPU 101計算處于靜止狀態(tài)的物體的加速度的范數(shù)的平均值。如果物體被判斷為處于靜止狀態(tài)，則計算并存儲加速計數(shù)據(jù)的范數(shù)。在本發(fā)明中，處于靜止的物體的累積加速度數(shù)據(jù)用于計算范數(shù)的平均值以替換重力的理論值。理論上來講，靜止物體的加速度等于重力，然而，無法完全去除與加速計數(shù)據(jù)相關(guān)的偏差及機械噪聲。在一個實施例中，靜止物體的最近的加速度數(shù)據(jù)樣本被存儲在固定長度緩沖器中。一旦只有新數(shù)據(jù)被推入到緩沖器中，就重新計算平均值。緩沖器的長度與傳感器的采樣率相關(guān)。在本發(fā)明中，緩沖器的長度是50，而采樣率是50Hz。

在步驟S403中，與步驟S401和步驟S402并行地，CPU 101利用陀螺儀數(shù)據(jù)計算物體姿態(tài)。假設(shè)由四元數(shù)(q0，q1，q2，q3)來表示最近的物體姿態(tài)。以ω_x，ω_y和ω_z來表示陀螺儀數(shù)據(jù)。通過對間隔時間Δt內(nèi)的 角速率進行積分，能夠得到在各軸上的旋轉(zhuǎn)角度。

通過將旋轉(zhuǎn)角度與初始角度相加，能夠計算更新的四元數(shù)(q′₀，q′₁，q′₂，q′₃)。

然后，將四元數(shù)歸一化，以使其變?yōu)閱挝皇噶浚?/p>

在步驟S404中，CPU 101估計在體坐標系的3個軸上的重力分量。在一個實施例中，計算在體坐標系的3個軸上的重力分量。

從更新的四元數(shù)(q′₀，q′₁，q′₂，q′₃)，能夠計算從地心坐標系到體坐標系的轉(zhuǎn)換矩陣(CRb)。利用CRb，通過以CRb左乘原始矢量，能夠?qū)⒌匦淖鴺讼抵械氖噶哭D(zhuǎn)換到體坐標系。

計算以G表示的、重力的范數(shù)的平均值。眾所周知，重力的方向是垂直向下。因此已知地心坐標系(東，北，上)中的重力矢量是

通過左乘上述轉(zhuǎn)換矩陣CRb，能夠如下計算在體坐標系中的重力矢量(重力分量)

在步驟S405中，CPU 101通過從加速計數(shù)據(jù)中去除重力分量來計算物體的運動加速度。當加速計數(shù)據(jù)被直接輸入時，從加速計數(shù)據(jù)中減去體坐標系中的重力分量來獲得運動加速度

因此，以來計算并表示物體運動加速度的范數(shù)。

在步驟S406中，CPU 101基于運動加速度的范數(shù)的函數(shù)，來計算校正增益。針對該計算設(shè)計了運動加速度的范數(shù)的分段函數(shù)。在一個實施例中，定義分段函數(shù)為如下的線性函數(shù)：

y＝1.0-x/M if 0.0≤x≤M

y＝0.0 else (6)

其中，x是作為物體運動加速度的范數(shù)的函數(shù)的變量；y是作為校正增益的函數(shù)的因變量；M是能夠由用戶調(diào)整的預(yù)定義常量。

在本發(fā)明中，M的默認值等于在步驟S402中已經(jīng)計算出的靜止物體的加速度的累積范數(shù)的平均值。

在又一實施例中，本發(fā)明提供了用于由用戶調(diào)諧校正增益的范圍的方法?；谏鲜鰧嵤├?，如下列出了改進的函數(shù)定義：

y＝K*(1.0-x/M)+ε if 0.0≤x≤M

y＝ε else (7)

其中，x、y、M與在上述式(6)中說明的相同。K是常量且是針對用戶的可調(diào)整參數(shù)，以控制校正增益的范圍，并且其默認值是2.0。ε是另一常量且也是針對用戶的可調(diào)整參數(shù)，以控制最小校正增益值，并且其默認值是0.0。

圖5A是示出用于校正增益確定的線性函數(shù)的示意圖。校正的增益在范圍[0，1]內(nèi)。如果物體運動加速度的范數(shù)等于0，是指物體處于靜止狀態(tài)，則校正增益等于1。然而，如果物體運動加速度的范數(shù)超過預(yù)定義常量M，則校正增益等于0。

在本發(fā)明的另一實施例中，以如下另一種方式來定義分段函數(shù)：

y＝exp(-x²/(2M²)) if 0.0≤x≤M

y＝0.0 else (8)

其中，x是作為物體運動加速度的范數(shù)的函數(shù)的變量；y是作為校正增益的函數(shù)的因變量；M是能夠由用戶調(diào)整的預(yù)定義常量且其默認值等于靜止物體的加速度的累積范數(shù)的平均值。

此外，本發(fā)明提供了用于由用戶調(diào)諧校正增益的范圍的方法?；谏鲜鰧嵤├?，改進的函數(shù)如下：

y＝K*exp(-x²/(2M²))+ε if 0.0≤x≤M

y＝ε else (9)

其中，x、y、M與在上述式(8)中說明的相同。K是常量且是針對用戶可調(diào)參數(shù)，以控制校正增益的范圍，并且其默認值是2.0。ε是另一常量且也是針對用戶可調(diào)的參數(shù)，以控制最小校正增益值，并且其默認值是0.0。

圖5B是示出用于校正增益確定的高斯函數(shù)的示意圖。校正的增益在范圍[0，1]內(nèi)。如果物體運動加速度的范數(shù)等于0，是指物體處于靜止狀態(tài)，則校正增益等于1。然而，如果物體運動加速度的范數(shù)超過預(yù)定義常量M，則校正增益等于0。

在步驟S406之后，存在基于最近一次的姿態(tài)估計偏差來調(diào)整校正增益的可選步驟(S407)。通常，如下所示，陀螺儀傳感器產(chǎn)生與物體角速率成比例的輸出，加上偏移且加上機械噪聲。

Ω＝K*Ω₀+OF+N (10)

其中，Ω是來自陀螺儀的輸出，Ω₀是物體的真實加速度，K是比例系數(shù)，OF是偏移，N是噪聲。

傳統(tǒng)上，陀螺儀數(shù)據(jù)不受物體運動加速度和周邊磁場的失真的影響。在大多數(shù)應(yīng)用中，定期收集陀螺儀數(shù)據(jù)。這是指兩個鄰近樣本之間的時間間隔幾乎固定。由于基于積分運算來計算物體的角度變化，因此在兩個鄰近樣本之間，基于陀螺儀數(shù)據(jù)的姿態(tài)估計偏差是穩(wěn)定的。

在本發(fā)明中，基于當前陀螺儀數(shù)據(jù)來估計物體姿態(tài)，并且通過稍后描述的校準陀螺儀數(shù)據(jù)來估計更新的物體姿態(tài)。因此能夠計算兩個估計結(jié)果之間的偏差。如上所述，偏差相對穩(wěn)定。如果識別到來自前次周期的反常偏差，則是指校正增益需要被調(diào)整。在一個實施例中，收集每個周期的偏差以計算其平均值(μ)。如果平均值與給定偏差(dn)之間的差超過預(yù)定閾值(T)，則校正增益(CG)將被強制設(shè)置為預(yù)定義值(N)。 在本發(fā)明中，T的默認值等于平均值的1/3，并且N是0.0。

CG＝N if |μ-dn|≥T (11)

在一個實施例中，收集每個周期的偏差以計算其平均值(μ)和標準方差(δ)。如果平均值與給定偏差(dn)之間的距離大于δ的預(yù)定閾值倍數(shù)(D)，則校正增益被設(shè)定為預(yù)定義值(N)。在本發(fā)明中，D的默認值是3，而N是0.0。

CG＝N if |μ-dn|≥D*δ (12)

校正矢量生成單元203被構(gòu)造為基于傳感器數(shù)據(jù)的不同類型來生成校正矢量。CPU 101控制校正矢量生成單元203來執(zhí)行圖3所示的步驟S302，以計算校正矢量?；谙铝懈拍顏碓O(shè)計校正矢量。

1.根據(jù)基于陀螺儀數(shù)據(jù)計算出的轉(zhuǎn)換矩陣，將地心坐標系中的參考矢量轉(zhuǎn)換到體坐標系。

2.根據(jù)基于加速計數(shù)據(jù)、或基于加速計數(shù)據(jù)和磁力計數(shù)據(jù)計算出的轉(zhuǎn)換矩陣，將地心坐標系中的參考矢量轉(zhuǎn)換到體坐標系。

3.執(zhí)行在兩個轉(zhuǎn)換的參考矢量之間的叉積運算，以生成校正矢量。

現(xiàn)在轉(zhuǎn)向圖6，圖6是根據(jù)本發(fā)明的一個實施例的校正矢量的生成方法的流程圖。

在步驟S601中，CPU 101基于陀螺儀數(shù)據(jù)更新最近的四元數(shù)。換言之，陀螺儀數(shù)據(jù)用于更新物體姿態(tài)。這與在步驟S403中描述的過程相同。

在步驟S602中，CPU 101基于更新的四元數(shù)來計算第一轉(zhuǎn)換矩陣。將更新的四元數(shù)轉(zhuǎn)換為第一轉(zhuǎn)換矩陣，該第一轉(zhuǎn)換矩陣能夠?qū)⑹噶繌牡匦淖鴺讼缔D(zhuǎn)換到體坐標系。這與在步驟S404中說明的過程相同。

在步驟S603中，CPU 101基于加速計數(shù)據(jù)、或基于加速計數(shù)據(jù)和磁力計數(shù)據(jù)來更新最近的四元數(shù)。除了陀螺儀數(shù)據(jù)之外，加速計數(shù)據(jù)和磁力計數(shù)據(jù)也能夠用于粗略地估計當前物體姿態(tài)。假定當前物體處于靜止狀態(tài)，并且周圍磁場未失真。因此能夠通過下式估計物體的歐拉角(俯仰角、滾轉(zhuǎn)角和偏航角)。

roll＝-1*atan2(a_y，a_z) (13)

pitch＝atan2(a_x，a_z) (14)

yaw＝atan2(h_y，h_x) (15)

其中，

h_x＝m_x*cos(pitch)+m_y*sin(roll)*sin(pitch)-m_z*cos(roll)*sin(pitch)

h_y＝m_y*cos(roll)+m_z*sin(roll)

加速計數(shù)據(jù)：(a_x，a_y，a_z)；磁力計數(shù)據(jù)：(m_x，m_y，m_z)

通過下式將歐拉角轉(zhuǎn)換為四元數(shù)。

q0＝CosHRoll*CosHPitch*CosHYaw+SinHRoll*SinHPitch*SinHYaw (16)

q1＝CosHRoll*SinHPitch*CosHYaw+SinHRoll*CosHPitch*SinHYaw (17)

q2＝CosHRoll*CosHPitch*SinHYaw-SinHRoll*SinHPitch*CosHYaw (18)

q3＝SinHRoll*CosHPitch*CosHYaw-CosHRoll*SinHPitch*SinHYaw (19)

其中：

最后，將四元數(shù)歸一化，以確定其為單位矢量：

其中，

在步驟S604中，CPU 101基于更新的四元數(shù)來計算第二轉(zhuǎn)換矩陣。將更新的四元數(shù)轉(zhuǎn)換為第二轉(zhuǎn)換矩陣，該第二轉(zhuǎn)換矩陣能將矢量從地心坐標系轉(zhuǎn)換到體坐標系。這與在步驟S404中描述的過程相同。

在步驟S605中，CPU 101分別基于第一轉(zhuǎn)換矩陣和第二轉(zhuǎn)換矩陣， 將參考矢量從地心坐標系轉(zhuǎn)換到體坐標系。在本發(fā)明的一個實施例中，在全局坐標系統(tǒng)(東，北，上)中的理論重力矢量被用作參考矢量，并且假定在當前位置處的重力幅值是9.8m/s²時，理論重力矢量的值是(0，0，9.8)。在東-北-上坐標系統(tǒng)中的理論地磁場矢量被用作參考矢量，并且假定標準地磁是50uG且在當前位置處的斜角是60度時，理論地磁場矢量的值是(0，50*sin(PI/3.0)，50*cos(PI/3.0)。在一個實施例中，在將已知矢量從地心坐標系轉(zhuǎn)換到體坐標系之前，將該已知矢量標準化為單位矢量。

在步驟S606中，CPU 101基于兩個轉(zhuǎn)換的矢量來計算校正矢量?；趦蓚€轉(zhuǎn)換的矢量來計算校正矢量。其涉及執(zhí)行在兩個轉(zhuǎn)換的矢量之間的叉積運算。

其中，是校正矢量。是基于依據(jù)陀螺儀數(shù)據(jù)估計的第一轉(zhuǎn)換矩陣，從地心坐標系轉(zhuǎn)換到體坐標系的參考矢量。是基于由加速計和/或磁力計數(shù)據(jù)估計的第二轉(zhuǎn)換矩陣，從地心坐標系轉(zhuǎn)換到體坐標系的參考矢量。是之間的交叉角度的正弦。是與矢量和正交的單位矢量。

傳感器數(shù)據(jù)校準單元204被構(gòu)造為使用校正矢量和校正增益，精確地校準用于重新估計物體姿態(tài)的陀螺儀數(shù)據(jù)。CPU 101控制傳感器數(shù)據(jù)校準單元204，以執(zhí)行圖3所示的步驟S303。在一個實施例中，傳感器數(shù)據(jù)校準單元204基于針對傳感器數(shù)據(jù)補償?shù)男Ｕ鲆?，將校正矢量融合到陀螺儀數(shù)據(jù)中。補償?shù)耐勇輧x數(shù)據(jù)被用于重新估計四元數(shù)。在可選步驟中，將更新的四元數(shù)轉(zhuǎn)換為歐拉角和/或轉(zhuǎn)換矩陣。

例如，傳感器數(shù)據(jù)校準單元204通過下式，基于校正增益，將陀螺儀數(shù)據(jù)與校正矢量融合：

其中，是當前陀螺儀數(shù)據(jù)。

是補償?shù)耐勇輧x數(shù)據(jù)。

CG是在步驟S301中確定的校正增益。

是在步驟S302中生成的校正矢量。

在得到校準的陀螺儀數(shù)據(jù)之后，CPU 101計算陀螺儀數(shù)據(jù)校準前后的姿態(tài)估計偏差。在一個實施例中，校準(例如，步驟S403)前的當前陀螺儀數(shù)據(jù)減去校準的陀螺儀數(shù)據(jù)(例如，步驟S303)，得到差。然后，計算差的范數(shù)作為姿態(tài)估計偏差。在另一實施例中，當前陀螺儀數(shù)據(jù)(例如，步驟S403)和校準的陀螺儀數(shù)據(jù)(例如，步驟S303)被施加到叉積運算。來自叉積運算的輸出矢量的范數(shù)被用作姿態(tài)估計偏差。此外，在相應(yīng)的歐拉角之間的差的求和也能夠被用作姿態(tài)估計偏差。

姿態(tài)更新和輸出單元205被構(gòu)造為以不同方式更新并輸出物體姿態(tài)。補償?shù)耐勇輧x數(shù)據(jù)被用于精確地估計物體姿態(tài)。CPU 101控制姿態(tài)更新和輸出單元205以執(zhí)行圖3所示的步驟S304。在一個實施例中，姿態(tài)更新和輸出單元205基于校準的陀螺儀數(shù)據(jù)來更新四元數(shù)，其中，更新物體姿態(tài)的方法與在步驟S403中描述的相同。

發(fā)明應(yīng)用

一些應(yīng)用需要歐拉角而非來自AHRS的四元數(shù)。在本發(fā)明中，CPU 101如下式將更新的四元數(shù)轉(zhuǎn)換為歐拉角(俯仰、滾轉(zhuǎn)和偏航)。

pitch＝-1*asin(2.0*(q0*q2-q1*q3)) (23)

roll＝-1*atan2(2*(q2*q3+q0*q1)，1.0-2.0*(q1*q1+q2*q2)) (24)

yaw＝-1*atan2(2.0*(q1*q2+q0*q3)，1-2.0*(q2*q2+q3*q3)) (25)

本發(fā)明提供了僅基于加速計數(shù)據(jù)和陀螺儀數(shù)據(jù)的俯仰、滾轉(zhuǎn)和偏航估計方法。因此，本發(fā)明是AHRS的一部分。

在另一應(yīng)用中，CPU 101基于更新的四元數(shù)來計算轉(zhuǎn)換矩陣，以滿足來自外部應(yīng)用的潛在需求。用于計算轉(zhuǎn)換矩陣的方法與在步驟S404中描述的相同。

已經(jīng)針對一定優(yōu)選實施例及其特定特征詳細示出并描述了本發(fā)明。然而，應(yīng)當注意，上述實施例旨在描述發(fā)明的原理，并非限制其范圍。 因此，如本領(lǐng)域常用技術(shù)那樣顯而易見的，在不脫離由所附的權(quán)利要求提出的本發(fā)明精神和范圍的情況下，在形式和詳情上，可以做出各種改變和變型。針對描述的實施例的其他實施例和變型對于本領(lǐng)域中的技術(shù)人員是顯而易見的，并且可以在不脫離由以下權(quán)利要求限定的本發(fā)明的精神和范圍的情況下做出。此外，發(fā)明者預(yù)期追蹤多種類型的鉸鏈式剛體的姿態(tài)，這些剛體包括但不限于蜂窩電話、假肢器官、機械臂以及運動物體。同樣地，發(fā)明者預(yù)期其他的校正增益實施例，這些校正增益實施例包括但不限于校正增益的調(diào)整。此外，除非明確規(guī)定，否則在權(quán)利要求中對以單數(shù)形式的元素的參考并不旨在是指“一個且僅一個”，而是“一個或更多”。此外，能夠?qū)崿F(xiàn)在此示例性公開的實施例，而無需任何未在此特別公開的元素。

上述針對圖2描述的單元和裝置是用于實現(xiàn)各種步驟的示例性和/或優(yōu)選的模塊。模塊可以是硬件單元(例如，處理器、專用集成電路等)和/或軟件模塊(例如，計算機程序)。以上并非窮盡性地描述了用于實現(xiàn)各種步驟的模塊。然而，由于存在進行一定處理的步驟，因此存在用于實現(xiàn)同樣處理的對應(yīng)模塊或單元(由硬件和/或軟件實現(xiàn))。由以上和以下描述的步驟以及與這些步驟相對應(yīng)的單元的所有組合的技術(shù)方案被包括在本申請的公開中，只要它們組成的技術(shù)方案是完整的并且可應(yīng)用的即可。