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

背景技術(shù)：

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

針對(duì)在無(wú)失真的地磁場(chǎng)中的靜止物體，可以通過(guò)加速計(jì)數(shù)據(jù)來(lái)計(jì)算靜止物體的姿態(tài)，或者基于加速計(jì)數(shù)據(jù)和磁力計(jì)數(shù)據(jù)來(lái)充分且精確地計(jì)算靜止物體的姿態(tài)。然而，針對(duì)運(yùn)動(dòng)物體，將重力加速度和運(yùn)動(dòng)加速度合并為來(lái)自加速計(jì)的測(cè)量。在知道物體姿態(tài)之前，無(wú)法明確地將它們分開(kāi)。此外，在物體周?chē)牡卮艌?chǎng)常因鐵或磁性材料而失真。這種影響給基于加速計(jì)數(shù)據(jù)或基于加速計(jì)數(shù)據(jù)和磁力計(jì)數(shù)據(jù)的姿態(tài)估計(jì)結(jié)果帶來(lái)偏差。

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

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

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

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

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

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

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

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

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

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

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

附圖說(shuō)明

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

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

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

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

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

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

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

具體實(shí)施方式

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

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

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

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

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

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

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

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

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

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

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

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

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

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

校正增益確定單元202基于來(lái)自傳感器的即時(shí)測(cè)量和校正增益的歷史數(shù)據(jù)，來(lái)計(jì)算校正增益。此外，基于在最近一次估計(jì)處理中計(jì)算的姿態(tài)估計(jì)偏差，來(lái)驗(yàn)證并調(diào)整計(jì)算出的校正增益?；谝韵赂拍钸M(jìn)行設(shè)計(jì)：

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

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

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

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

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

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

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

在又一實(shí)施例中，如上述所述的、基于加速計(jì)數(shù)據(jù)的靜止?fàn)顟B(tài)檢測(cè)方法，來(lái)計(jì)算磁力計(jì)數(shù)據(jù)的變化率。

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

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

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

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

在步驟S404中，CPU 101估計(jì)在體坐標(biāo)系的3個(gè)軸上的重力分量。在一個(gè)實(shí)施例中，計(jì)算在體坐標(biāo)系的3個(gè)軸上的重力分量。

從更新的四元數(shù)(q′₀，q′₁，q′₂，q′₃)，能夠計(jì)算從地心坐標(biāo)系到體坐標(biāo)系的轉(zhuǎn)換矩陣(CRb)。利用CRb，通過(guò)以CRb左乘原始矢量，能夠?qū)⒌匦淖鴺?biāo)系中的矢量轉(zhuǎn)換到體坐標(biāo)系。

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

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

在步驟S405中，CPU 101通過(guò)從加速計(jì)數(shù)據(jù)中去除重力分量來(lái)計(jì)算物體的運(yùn)動(dòng)加速度。當(dāng)加速計(jì)數(shù)據(jù)被直接輸入時(shí)，從加速計(jì)數(shù)據(jù)中減去體坐標(biāo)系中的重力分量來(lái)獲得運(yùn)動(dòng)加速度

因此，以來(lái)計(jì)算并表示物體運(yùn)動(dòng)加速度的范數(shù)。

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

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

y＝0.0 else (6)

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

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

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

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

y＝ε else (7)

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

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

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

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

y＝0.0 else (8)

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

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

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

y＝ε else (9)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

在步驟S603中，CPU 101基于加速計(jì)數(shù)據(jù)、或基于加速計(jì)數(shù)據(jù)和磁力計(jì)數(shù)據(jù)來(lái)更新最近的四元數(shù)。除了陀螺儀數(shù)據(jù)之外，加速計(jì)數(shù)據(jù)和磁力計(jì)數(shù)據(jù)也能夠用于粗略地估計(jì)當(dāng)前物體姿態(tài)。假定當(dāng)前物體處于靜止?fàn)顟B(tài)，并且周?chē)艌?chǎng)未失真。因此能夠通過(guò)下式估計(jì)物體的歐拉角(俯仰角、滾轉(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)

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

通過(guò)下式將歐拉角轉(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ù)來(lái)計(jì)算第二轉(zhuǎn)換矩陣。將更新的四元數(shù)轉(zhuǎn)換為第二轉(zhuǎn)換矩陣，該第二轉(zhuǎn)換矩陣能將矢量從地心坐標(biāo)系轉(zhuǎn)換到體坐標(biāo)系。這與在步驟S404中描述的過(guò)程相同。

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

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

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

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

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

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

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

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

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

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

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

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

一些應(yīng)用需要?dú)W拉角而非來(lái)自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ā)明提供了僅基于加速計(jì)數(shù)據(jù)和陀螺儀數(shù)據(jù)的俯仰、滾轉(zhuǎn)和偏航估計(jì)方法。因此，本發(fā)明是AHRS的一部分。

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

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

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