本發(fā)明涉及自動(dòng)化計(jì)算機(jī)領(lǐng)域，尤其涉及一種確定終端姿態(tài)的方法及裝置。

背景技術(shù)：

隨著智能手機(jī)的硬件水平的不斷提升，很多智能手機(jī)已經(jīng)配備了加速度傳感器、陀螺儀、磁力計(jì)等傳感器，從而具備感知加速度、角速度、磁場(chǎng)強(qiáng)度的能力。然而，由于智能手機(jī)中的傳感器精度低，以及室內(nèi)環(huán)境的復(fù)雜性，智能手機(jī)空間姿態(tài)角的精度受到以下多種因素影響：

(1)傳感器本身的誤差，包括系統(tǒng)性誤差和隨機(jī)誤差。系統(tǒng)性誤差為常值，隨機(jī)誤差在每次傳感器啟動(dòng)過程中都不相同。

(2)累積誤差。利用陀螺儀的測(cè)量數(shù)據(jù)，通過姿態(tài)更新算法可以得到智能手機(jī)的姿態(tài)角，其短時(shí)間內(nèi)精度較高，但是由于陀螺儀的積分特性，其精度隨時(shí)間下降。

(3)磁場(chǎng)干擾。由于磁場(chǎng)干擾，導(dǎo)致磁傳感器數(shù)據(jù)異常，給姿態(tài)角計(jì)算帶來(lái)誤差，嚴(yán)重時(shí)會(huì)導(dǎo)致系統(tǒng)輸出的姿態(tài)角偏離正常值；當(dāng)磁干擾消除后，需要很長(zhǎng)時(shí)間才能使姿態(tài)角恢復(fù)到正常值。

現(xiàn)有的智能手機(jī)傳感器的空間姿態(tài)算法為：將智能手機(jī)的狀態(tài)分為快速運(yùn)動(dòng)狀態(tài)與接近靜止?fàn)顟B(tài)；在快速運(yùn)動(dòng)狀態(tài)下采用陀螺儀對(duì)時(shí)間積分計(jì)算手機(jī)的空間姿態(tài)，在接近靜止的狀態(tài)下采用加速度傳感器與電子羅盤計(jì)算手機(jī)的空間姿角，并對(duì)當(dāng)前的空間姿態(tài)進(jìn)行修正與誤差消除。主要應(yīng)用于室外高速運(yùn)動(dòng)導(dǎo)航，由于高速運(yùn)動(dòng)對(duì)空間姿態(tài)精度要求不高，且可以使用位置參數(shù)對(duì)結(jié)果進(jìn)行修正，故實(shí)際應(yīng)用效果較好。

然而，基于智能手機(jī)傳感器的低速運(yùn)動(dòng)定位導(dǎo)航，即基于智能手機(jī)的行人 導(dǎo)航受到智能手機(jī)中的傳感器精度低以及室內(nèi)環(huán)境的復(fù)雜性等因素影響，應(yīng)用效果并不理想。

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

本發(fā)明提供一種確定終端姿態(tài)的方法及裝置，用以解決現(xiàn)有技術(shù)中智能手機(jī)傳感器的空間姿態(tài)算法不適用于低速運(yùn)動(dòng)定位導(dǎo)航的問題。

本發(fā)明實(shí)施例提供一種確定終端姿態(tài)的方法，包括：

在當(dāng)前采樣周期，確定終端的陀螺儀輸出的采樣角速度值，并補(bǔ)償確定的所述采樣角速度值；

根據(jù)補(bǔ)償?shù)牟蓸咏撬俣戎岛统跏甲藨B(tài)矩陣，確定更新姿態(tài)矩陣；

根據(jù)所述確定補(bǔ)償?shù)牟蓸咏撬俣戎岛退龈伦藨B(tài)矩陣，通過卡爾曼濾波確定所述終端的姿態(tài)最優(yōu)估計(jì)值。

可選的，所述初始姿態(tài)矩陣根據(jù)以下方法確定：

根據(jù)加速度傳感器的系統(tǒng)誤差，確定終端的俯仰角和翻滾角；

根據(jù)所述終端的俯仰角和翻滾角，確定所述終端的初始姿態(tài)矩陣。

可選的，所述根據(jù)加速度傳感器的系統(tǒng)誤差，確定終端的俯仰角和翻滾角，包括：

在接收到傳感器觸發(fā)指令時(shí)，獲取所述加速度傳感器輸出的初始加速度值；根據(jù)所述初始加速度值和所述加速度傳感器的系統(tǒng)誤差，確定所述俯仰角和所述翻滾角；

所述根據(jù)所述終端的俯仰角和翻滾角，確定所述終端的初始姿態(tài)矩陣，包括：

在接收到傳感器觸發(fā)指令時(shí)，獲取磁傳感器輸出的方位角；

根據(jù)所述方位角、所述俯仰角和所述翻滾角確定所述終端的初始姿態(tài)矩陣。

可選的，所述根據(jù)所述補(bǔ)償?shù)牟蓸咏撬俣戎岛退龈伦藨B(tài)矩陣，通過卡爾曼濾波確定所述終端的姿態(tài)最優(yōu)估計(jì)值，包括：

根據(jù)所述補(bǔ)償?shù)牟蓸咏撬俣戎荡_定卡爾曼濾波器的預(yù)測(cè)值，根據(jù)所述更新姿態(tài)矩陣確定所述卡爾曼濾波器的量測(cè)值；

根據(jù)確定的所述量測(cè)值修正所述預(yù)測(cè)值，得到所述終端的最優(yōu)姿態(tài)估計(jì)值。

可選的，所述終端的最優(yōu)姿態(tài)估計(jì)包括陀螺儀的隨機(jī)誤差；

所述補(bǔ)償確定的所述采樣角速度值，包括：

根據(jù)上一個(gè)采樣周期最優(yōu)姿態(tài)估計(jì)中的所述陀螺儀的隨機(jī)誤差，以及陀螺儀的系統(tǒng)誤差，補(bǔ)償當(dāng)前采樣周期陀螺儀輸出的采樣角速度值。

可選的，所述在當(dāng)前采樣周期，確定終端的陀螺儀輸出的采樣角速度值，并補(bǔ)償確定的所述采樣角速度值之前，還包括：

在接收到傳感器校正指令時(shí)，獲取所述加速度傳感器輸出的校驗(yàn)加速度的值，根據(jù)所述校驗(yàn)加速度的值確定所述加速度傳感器的系統(tǒng)誤差。

可選的，所述在當(dāng)前采樣周期，確定終端的陀螺儀輸出的采樣角速度值，并補(bǔ)償確定的所述采樣角速度值之前，還包括：

在接收到傳感器校正指令時(shí)，獲取所述陀螺儀輸出的校驗(yàn)角速度的值，根據(jù)所述校驗(yàn)角速度的值確定所述陀螺儀的系統(tǒng)誤差。

本發(fā)明實(shí)施例還提供一種確定終端姿態(tài)的裝置，包括：

補(bǔ)償模塊：用于在當(dāng)前采樣周期，確定終端的陀螺儀輸出的采樣角速度值，并補(bǔ)償確定的所述采樣角速度值；

姿態(tài)矩陣模塊：用于根據(jù)補(bǔ)償?shù)牟蓸咏撬俣戎岛统跏甲藨B(tài)矩陣，確定更新姿態(tài)矩陣；

估計(jì)值確定模塊：用于根據(jù)所述確定補(bǔ)償?shù)牟蓸咏撬俣戎岛退龈伦藨B(tài)矩陣，通過卡爾曼濾波確定所述終端的姿態(tài)最優(yōu)估計(jì)值。

可選的，所述初始姿態(tài)矩陣根據(jù)以下方法確定：

根據(jù)加速度傳感器的系統(tǒng)誤差，確定終端的俯仰角和翻滾角；

根據(jù)所述終端的俯仰角和翻滾角，確定所述終端的初始姿態(tài)矩陣。

可選的，所述姿態(tài)矩陣模塊，具體用于：

在接收到傳感器觸發(fā)指令時(shí)，獲取所述加速度傳感器輸出的初始加速度值；

根據(jù)所述初始加速度值和所述加速度傳感器的系統(tǒng)誤差，確定所述俯仰角和所述翻滾角；

在接收到傳感器觸發(fā)指令時(shí)，獲取磁傳感器輸出的方位角；

根據(jù)所述方位角、所述俯仰角和所述翻滾角確定所述終端的初始姿態(tài)矩陣。

可選的，所述估計(jì)值確定模塊，具體用于：

根據(jù)所述補(bǔ)償?shù)牟蓸咏撬俣戎荡_定卡爾曼濾波器的預(yù)測(cè)值，根據(jù)所述更新姿態(tài)矩陣確定所述卡爾曼濾波器的量測(cè)值；

根據(jù)確定的所述量測(cè)值修正所述預(yù)測(cè)值，得到所述終端的最優(yōu)姿態(tài)估計(jì)值。

可選的，所述終端的最優(yōu)姿態(tài)估計(jì)包括陀螺儀的隨機(jī)誤差；

所述補(bǔ)償模塊，具體用于：

根據(jù)上一個(gè)采樣周期最優(yōu)姿態(tài)估計(jì)中的所述陀螺儀的隨機(jī)誤差，以及陀螺儀的系統(tǒng)誤差，補(bǔ)償當(dāng)前采樣周期陀螺儀輸出的采樣角速度值。

可選的，所述補(bǔ)償模塊，還用于：

在接收到傳感器校正指令時(shí)，獲取所述加速度傳感器輸出的校驗(yàn)加速度的值，根據(jù)所述校驗(yàn)加速度的值確定所述加速度傳感器的系統(tǒng)誤差。

可選的，所述補(bǔ)償模塊，還用于：

在接收到傳感器校正指令時(shí)，獲取所述陀螺儀輸出的校驗(yàn)角速度的值，根據(jù)所述校驗(yàn)角速度的值確定所述陀螺儀的系統(tǒng)誤差。

本發(fā)明提供了一種確定終端姿態(tài)的方法和裝置，在當(dāng)前采樣周期，確定終端的陀螺儀輸出的采樣角速度值，并補(bǔ)償確定的所述采樣角速度值；根據(jù)補(bǔ)償?shù)牟蓸咏撬俣戎岛统跏甲藨B(tài)矩陣，確定更新姿態(tài)矩陣；根據(jù)所述確定補(bǔ)償?shù)牟蓸咏撬俣戎岛退龈伦藨B(tài)矩陣，通過卡爾曼濾波確定所述終端的姿態(tài)最優(yōu)估計(jì)值。本發(fā)明實(shí)施例提供的確定終端姿態(tài)的方法和裝置，提高低速運(yùn)動(dòng)導(dǎo)航的精度、減小了終端姿態(tài)估計(jì)的誤差。

附圖說明

為了更清楚地說明本發(fā)明實(shí)施例中的技術(shù)方案，下面將對(duì)實(shí)施例描述中所需要使用的附圖作簡(jiǎn)要介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域的普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)性的前提下，還可以根據(jù)這些附圖獲得其他的附圖。

圖1為本發(fā)明實(shí)施例提供的一種確定終端姿態(tài)的方法流程示意圖；

圖2為本發(fā)明實(shí)施例提供的一種導(dǎo)航坐標(biāo)系示意圖；

圖3為本發(fā)明實(shí)施例提供的一種終端坐標(biāo)系示意圖；

圖4為本發(fā)明實(shí)施例提供的一種終端姿態(tài)的方法流程圖；

圖5為本發(fā)明實(shí)施例提供的一種確定終端姿態(tài)的裝置結(jié)構(gòu)示意圖。

具體實(shí)施方式

為了使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步地詳細(xì)描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部份實(shí)施例，而不是全部的實(shí)施例?；诒景l(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其它實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。

本發(fā)明實(shí)施例中的終端為指向用戶提供語(yǔ)音和/或數(shù)據(jù)連通性的設(shè)備(device)，包括無(wú)線終端或有線終端。無(wú)線終端可以是具有無(wú)線連接功能的手持式設(shè)備、或連接到無(wú)線調(diào)制解調(diào)器的其他處理設(shè)備，經(jīng)無(wú)線接入網(wǎng)與一個(gè)或多個(gè)核心網(wǎng)進(jìn)行通信的移動(dòng)終端。例如，無(wú)線終端可以是移動(dòng)電話(或稱為“蜂窩”電話)和具有移動(dòng)終端的計(jì)算機(jī)。又如，無(wú)線終端也可以是便攜式、袖珍式、手持式、計(jì)算機(jī)內(nèi)置的或者車載的移動(dòng)裝置。本發(fā)明實(shí)施例中的確定終端姿態(tài)的方法和裝置應(yīng)用于終端導(dǎo)航系統(tǒng)中。

本發(fā)明實(shí)施例提供一種確定終端姿態(tài)的方法，如圖1所示，為本發(fā)明實(shí)施例提供的一種確定終端姿態(tài)的方法流程示意圖，包括：

步驟101：在當(dāng)前采樣周期，確定終端的陀螺儀輸出的采樣角速度值，并補(bǔ) 償確定的采樣角速度值；

步驟102：根據(jù)補(bǔ)償?shù)牟蓸咏撬俣戎岛统跏甲藨B(tài)矩陣，確定更新姿態(tài)矩陣；

步驟103：根據(jù)確定補(bǔ)償?shù)牟蓸咏撬俣戎岛透伦藨B(tài)矩陣，通過卡爾曼濾波確定終端的姿態(tài)最優(yōu)估計(jì)值。

具體地，在步驟101之前，終端首次使用會(huì)對(duì)終端中的所有傳感器進(jìn)行校驗(yàn)，本發(fā)明實(shí)施例終端中的傳感器至少包含加速度傳感器、陀螺儀和磁傳感器。使終端保持靜止一段時(shí)間如30秒。在接收到傳感器校正指令時(shí)，獲取加速度傳感器輸出的校驗(yàn)加速度的值，并根據(jù)校驗(yàn)加速度的值確定加速度傳感器每個(gè)傳感軸的系統(tǒng)誤差；同時(shí)獲取所述陀螺儀輸出的校驗(yàn)角速度的值，根據(jù)校驗(yàn)角速度的值確定陀螺儀每個(gè)傳感軸的系統(tǒng)誤差。

需要說明的是，傳感器的校驗(yàn)可在只終端初次使用的時(shí)候進(jìn)行一次，得到加速度傳感器的系統(tǒng)誤差和陀螺儀的系統(tǒng)誤差，也可以在需要的時(shí)候手動(dòng)校驗(yàn)。

步驟102中，初始姿態(tài)矩陣根據(jù)以下方法確定：根據(jù)加速度傳感器的系統(tǒng)誤差，確定終端的俯仰角和翻滾角；根據(jù)終端的俯仰角和翻滾角，確定終端的初始姿態(tài)矩陣。

在每次導(dǎo)航定位時(shí)，使終端保持靜止一段時(shí)間如5秒，根據(jù)加速度傳感器、磁傳感器的輸出數(shù)據(jù)進(jìn)行姿態(tài)的初始化，計(jì)算出初始姿態(tài)矩陣。具體地，在接收到傳感器觸發(fā)指令時(shí)，獲取加速度傳感器輸出的初始加速度值；根據(jù)初始加速度值和加速度傳感器的系統(tǒng)誤差，確定所述俯仰角和所述翻滾角；此外，在接收到傳感器觸發(fā)指令時(shí)，獲取磁傳感器輸出的方位角；根據(jù)方位角、俯仰角和翻滾角確定所述終端的初始姿態(tài)矩陣。

步驟103中，首先根據(jù)補(bǔ)償?shù)牟蓸咏撬俣戎荡_定卡爾曼濾波器的預(yù)測(cè)值，然后根據(jù)更新姿態(tài)矩陣確定所述卡爾曼濾波器的量測(cè)值；最后根據(jù)確定的量測(cè)值修正預(yù)測(cè)值，得到終端的最優(yōu)姿態(tài)估計(jì)值。其中，最優(yōu)姿態(tài)估計(jì)包括加速度傳感器的隨機(jī)誤差、陀螺儀的隨機(jī)誤差以及姿態(tài)角誤差。此外，根據(jù)上一個(gè)采樣周期最優(yōu)姿態(tài)估計(jì)中的陀螺儀的隨機(jī)誤差，以及陀螺儀的系統(tǒng)誤差，補(bǔ)償當(dāng) 前采樣周期陀螺儀輸出的采樣角速度值。

本發(fā)明提供了一種確定終端姿態(tài)的方法，在當(dāng)前采樣周期，確定終端的陀螺儀輸出的采樣角速度值，并補(bǔ)償確定的所述采樣角速度值；根據(jù)補(bǔ)償?shù)牟蓸咏撬俣戎岛统跏甲藨B(tài)矩陣，確定更新姿態(tài)矩陣；根據(jù)所述確定補(bǔ)償?shù)牟蓸咏撬俣戎岛退龈伦藨B(tài)矩陣，通過卡爾曼濾波確定所述終端的姿態(tài)最優(yōu)估計(jì)值。本發(fā)明實(shí)施例提供的確定終端姿態(tài)的方法，提高低速運(yùn)動(dòng)導(dǎo)航的精度、減小了終端姿態(tài)估計(jì)的誤差。

下面通過具體的計(jì)算分析過程對(duì)本發(fā)明實(shí)施例提供的確定終端姿態(tài)的方法進(jìn)行詳細(xì)說明。

本發(fā)明實(shí)施例終端中的傳感器至少包含加速度傳感器、陀螺儀和磁傳感器。終端首次使用會(huì)對(duì)終端中的所有傳感器進(jìn)行校驗(yàn)使終端保持靜止一段時(shí)間如30秒，獲取陀螺儀和加速度傳感器的輸出數(shù)據(jù)，并計(jì)算得出30秒內(nèi)陀螺儀的xyz三軸輸出的角速度均值為30秒內(nèi)加速度傳感器的xyz三軸輸出的加速度均值為則根據(jù)公式(1)得到陀螺儀的系統(tǒng)誤差：

式中，bsx,bsy,bsz表示陀螺儀的三軸系統(tǒng)誤差，即陀螺儀的三軸常值零偏值。根據(jù)公式(2)得到加速度傳感器的系統(tǒng)誤差：

式中，dsx,dsy,dsz表示加速度傳感器的三軸系統(tǒng)誤差，即加速度傳感器的三軸常值零偏值；g表示重力加速度的值。

需要說明的是，傳感器的校驗(yàn)可在只終端初次使用的時(shí)候進(jìn)行一次，得到，也可以在需要的時(shí)候手動(dòng)校驗(yàn)。

在計(jì)算終端的初始姿態(tài)矩陣之前，首先需要確定坐標(biāo)系的方向，坐標(biāo)系包 括導(dǎo)航坐標(biāo)系和終端坐標(biāo)系，其中導(dǎo)航坐標(biāo)系定義為l坐標(biāo)系，終端坐標(biāo)系定義為d坐標(biāo)系。

圖2為本發(fā)明實(shí)施例提供的一種導(dǎo)航坐標(biāo)系示意圖，如圖2所示，導(dǎo)航坐標(biāo)系原點(diǎn)位于地球表面，三軸指向如下：x軸沿卯酉圈指向東，y軸沿子午圈指向北，z軸沿法線指向天頂。圖3為本發(fā)明實(shí)施例提供的一種終端坐標(biāo)系示意圖，如圖3所示，終端坐標(biāo)系的原點(diǎn)位于設(shè)備中心，x軸指向設(shè)備右側(cè)，y軸指向設(shè)備前方，z軸向上。

導(dǎo)航坐標(biāo)系和終端坐標(biāo)系之間的旋轉(zhuǎn)關(guān)系可以用一個(gè)矩陣表示，稱為姿態(tài)矩陣，如公式(3)：

式中，代表姿態(tài)矩陣，y,p,r表示姿態(tài)角，其中，y表示方向角(yaw)，p表示俯仰角(pitch)，r表示翻滾角(roll)。

在每次導(dǎo)航定位時(shí)，使終端保持靜止一段時(shí)間如5秒，根據(jù)初始加速度值和加速度傳感器的系統(tǒng)誤差，確定俯仰角p和翻滾角r，如公式(4)：

式中，表示靜止時(shí)間5秒內(nèi)加速度計(jì)x軸和y軸輸出加速度的均值。結(jié)合磁傳感器輸出的方向角y，將公式(4)代入公式(3)，即得到終端的初始姿態(tài)矩陣。

在導(dǎo)航定位過程中，根據(jù)陀螺儀的輸出數(shù)據(jù)進(jìn)行姿態(tài)更新，獲得每個(gè)采樣周期的姿態(tài)，如公式(5)：

式中，表示每個(gè)采樣周期的姿態(tài)矩陣，表示反對(duì)稱陣，其元素由陀螺 儀的輸出數(shù)據(jù)構(gòu)成，表示終端坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的旋轉(zhuǎn)角速度。根據(jù)公式(6)確定：

式中，ωx,ωy,ωz為陀螺儀的輸出的角速度經(jīng)過陀螺儀的系統(tǒng)誤差和陀螺儀的隨機(jī)補(bǔ)償之后的值。

陀螺儀的輸出數(shù)據(jù)一般為離散的，按照一定的采樣周期輸出。采樣周期可以配置，如0.02s，0.05s，0.1s等。因此將公式(6)代入公式(5)，得到更新姿態(tài)矩陣如式(7)所示：

式中，k,k+1分別表示離散化前后兩個(gè)采樣周期的時(shí)刻，δt表示采樣周期。

下面通過采用卡爾曼濾波進(jìn)行姿態(tài)的最優(yōu)估計(jì)?？柭鼮V波器的狀態(tài)向量為9階，包括3個(gè)姿態(tài)角誤差、3軸陀螺儀隨機(jī)誤差、3軸加速度傳感器隨機(jī)誤差。以陀螺儀姿態(tài)角的誤差方程作為濾波器的狀態(tài)方程，以姿態(tài)角誤差作為濾波器的量測(cè)值。

具體地，根據(jù)公式(8)確定狀態(tài)向量：

x＝(δpδrδybxbybzdxdydz)^t(8)

式中，x表示狀態(tài)向量，δp、δr、δy分別表示俯仰角、翻滾角、方向角的姿態(tài)誤差，bx，by，bz表示陀螺儀的三軸隨機(jī)誤差，dxdydz表示加速度傳感器的三軸隨機(jī)誤差。

具體地，根據(jù)公式(9)確定狀態(tài)方程：

xk+1＝fk·xk+gk·wk(9)

式中，xk，xk+1分別表示前后采樣周期的狀態(tài)值；fk表示狀態(tài)轉(zhuǎn)移矩陣；gk 表示噪聲驅(qū)動(dòng)矩陣，其值取為單位陣；wk表示系統(tǒng)的動(dòng)態(tài)噪聲。fk根據(jù)公式(10)確定：

式中，表示3×3階的單位陣，表示3×3階的零矩陣。

wk根據(jù)公式(11)確定：

式中，e,d分別表示數(shù)學(xué)期望和方差。qk+1表示動(dòng)態(tài)噪聲的方差。一般在傳感器的說明文檔中會(huì)提供每個(gè)軸的動(dòng)態(tài)噪聲。

具體地，根據(jù)公式(12)確定卡爾曼濾波器的量測(cè)方程：

zk+1＝hk+1·xk+1+vk+1(12)

式中，zk+1為量測(cè)值，hk+1為量測(cè)矩陣，vk+1為量測(cè)噪聲。

具體地，公式(12)中的量測(cè)值z(mì)k+1根據(jù)公式(13)確定：

式中，pg,rg,yg分別表示經(jīng)過姿態(tài)更新后得到的三個(gè)姿態(tài)角，pam,ram分別表示加速度傳感器輸出數(shù)據(jù)計(jì)算出來(lái)的更新俯仰角和更新翻滾角，ym表示當(dāng)前采樣周期磁傳感器輸出的更新方向角。pg,rg,yg根據(jù)公式(14)確定：

式中，rij(i＝1,2,3；j＝1,2,3)對(duì)應(yīng)公式(3)中的元素。

pam,ram根據(jù)公式(15)確定：

具體地，根據(jù)公式(16)確定公式(12)中的量測(cè)值噪聲vk+1：

式中，rk+1表示觀測(cè)噪聲的方差，根據(jù)公式(17)確定觀測(cè)噪聲的方差rk+1:

上式中，表示觀測(cè)噪聲方差的基準(zhǔn)值，ythre為方向角誤差的閾值。具體地，在卡爾曼濾波過程中，根據(jù)卡爾曼濾波量測(cè)值與預(yù)測(cè)值之間的相容性，動(dòng)態(tài)調(diào)整觀測(cè)噪聲的方差rk+1。

具體地，根據(jù)公式(18)確定公式(12)中的量測(cè)矩陣hk+1：

至此，卡爾曼濾波的模型已建立，卡爾曼濾波過程分為兩步：時(shí)間更新和量測(cè)更新。其中時(shí)間更新的方程根據(jù)公式(19)確定：

量測(cè)更新的方程為根據(jù)公式(20)確定：

式(20)中的為經(jīng)過卡爾曼濾波之后的姿態(tài)最優(yōu)估值，包括姿態(tài)角誤差、陀螺儀隨機(jī)誤差、加速度傳感器隨機(jī)誤差。當(dāng)前采樣周期的姿態(tài)最優(yōu)估值可以反饋到下一個(gè)周期的姿態(tài)更新和濾波過程，即根據(jù)當(dāng)前采樣周期最優(yōu)姿態(tài)估計(jì)中的所述陀螺儀的隨機(jī)誤差，以及陀螺儀的系統(tǒng)誤差，補(bǔ)償下一個(gè)采樣周期陀螺儀輸出的采樣角速度值。

本發(fā)明實(shí)施例中，在導(dǎo)航定位過程中不斷的重復(fù)計(jì)算姿態(tài)更新矩陣和卡爾曼濾波的步驟，即可為每個(gè)采樣周期提供最優(yōu)的姿態(tài)角信息。

下面通過具體的方法流程，對(duì)本發(fā)明實(shí)施例提供的一種確定終端姿態(tài)的方法進(jìn)行詳細(xì)說明，圖4為本發(fā)明實(shí)施例提供的一種確定終端姿態(tài)的方法流程圖，包括：

步驟401：校驗(yàn)加速度傳感器和陀螺儀，獲取加速度傳感器的系統(tǒng)誤差和陀螺儀的系統(tǒng)誤差。

具體地，終端首次使用會(huì)對(duì)終端中的所有傳感器進(jìn)行校驗(yàn)，本發(fā)明實(shí)施例終端中的傳感器至少包含加速度傳感器、陀螺儀和磁傳感器。使終端保持靜止一段時(shí)間如30秒。在接收到傳感器校正指令時(shí)，獲取加速度傳感器輸出的校驗(yàn)加速度的值，并根據(jù)校驗(yàn)加速度的值確定加速度傳感器每個(gè)傳感軸的系統(tǒng)誤差；同時(shí)獲取所述陀螺儀輸出的校驗(yàn)角速度的值，根據(jù)校驗(yàn)角速度的值確定陀螺儀每個(gè)傳感軸的系統(tǒng)誤差。

需要說明的是，傳感器的校驗(yàn)可在只終端初次使用的時(shí)候進(jìn)行一次，得到加速度傳感器的系統(tǒng)誤差和陀螺儀的系統(tǒng)誤差，也可以在需要的時(shí)候手動(dòng)校驗(yàn)。

步驟402：確定終端的初始姿態(tài)矩陣。

具體地，根據(jù)加速度傳感器的系統(tǒng)誤差，確定終端的俯仰角和翻滾角；根據(jù)終端的俯仰角和翻滾角，確定終端的初始姿態(tài)矩陣。

在每次導(dǎo)航定位時(shí)，使終端保持靜止一段時(shí)間如5秒，根據(jù)加速度傳感器、 磁傳感器的輸出數(shù)據(jù)進(jìn)行姿態(tài)的初始化，計(jì)算出初始姿態(tài)矩陣。具體地，在接收到傳感器觸發(fā)指令時(shí)，獲取加速度傳感器輸出的初始加速度值；根據(jù)初始加速度值和加速度傳感器的系統(tǒng)誤差，確定所述俯仰角和所述翻滾角；此外，在接收到傳感器觸發(fā)指令時(shí)，獲取磁傳感器輸出的方位角；根據(jù)方位角、俯仰角和翻滾角確定所述終端的初始姿態(tài)矩陣。

步驟403：在當(dāng)前采樣周期確定終端的陀螺儀輸出的采樣角速度值，并補(bǔ)償確定的采樣角速度值。

具體地，根據(jù)上一個(gè)采樣周期最優(yōu)姿態(tài)估計(jì)中的陀螺儀的隨機(jī)誤差，以及陀螺儀的系統(tǒng)誤差，補(bǔ)償當(dāng)前采樣周期陀螺儀輸出的采樣角速度值。

步驟404：根據(jù)補(bǔ)償?shù)牟蓸咏撬俣戎岛统跏甲藨B(tài)矩陣，確定更新姿態(tài)矩陣。

步驟405：根據(jù)確定補(bǔ)償?shù)牟蓸咏撬俣戎岛透伦藨B(tài)矩陣，通過卡爾曼濾波確定終端的姿態(tài)最優(yōu)估計(jì)值。

具體地，根據(jù)補(bǔ)償?shù)牟蓸咏撬俣戎荡_定卡爾曼濾波器的預(yù)測(cè)值，根據(jù)更新姿態(tài)矩陣確定所述卡爾曼濾波器的量測(cè)值；根據(jù)確定的量測(cè)值修正預(yù)測(cè)值，得到終端的最優(yōu)姿態(tài)估計(jì)值。其中，最優(yōu)姿態(tài)估計(jì)包括加速度傳感器的隨機(jī)誤差、陀螺儀的隨機(jī)誤差以及姿態(tài)角誤差。此外，根據(jù)當(dāng)前采樣周期最優(yōu)姿態(tài)估計(jì)中的陀螺儀的隨機(jī)誤差，以及陀螺儀的系統(tǒng)誤差，補(bǔ)償下一個(gè)采樣周期陀螺儀輸出的采樣角速度值。

基于同樣的發(fā)明構(gòu)思，本發(fā)明實(shí)施例還提供一種確定終端姿態(tài)的裝置，如圖5所示，為本發(fā)明實(shí)施例提供的一種確定終端姿態(tài)的裝置，包括：

補(bǔ)償模塊501：用于在當(dāng)前采樣周期，確定終端的陀螺儀輸出的采樣角速度值，并補(bǔ)償確定的所述采樣角速度值；

姿態(tài)矩陣模塊502：用于根據(jù)補(bǔ)償?shù)牟蓸咏撬俣戎岛统跏甲藨B(tài)矩陣，確定更新姿態(tài)矩陣；

估計(jì)值確定模塊503：用于根據(jù)所述確定補(bǔ)償?shù)牟蓸咏撬俣戎岛退龈伦藨B(tài)矩陣，通過卡爾曼濾波確定所述終端的姿態(tài)最優(yōu)估計(jì)值。

可選的，所述初始姿態(tài)矩陣根據(jù)以下方法確定：

根據(jù)加速度傳感器的系統(tǒng)誤差，確定終端的俯仰角和翻滾角；

根據(jù)所述終端的俯仰角和翻滾角，確定所述終端的初始姿態(tài)矩陣。

可選的，所述姿態(tài)矩陣模塊502，具體用于：

在接收到傳感器觸發(fā)指令時(shí)，獲取所述加速度傳感器輸出的初始加速度值；

根據(jù)所述初始加速度值和所述加速度傳感器的系統(tǒng)誤差，確定所述俯仰角和所述翻滾角；

在接收到傳感器觸發(fā)指令時(shí)，獲取磁傳感器輸出的方位角；

根據(jù)所述方位角、所述俯仰角和所述翻滾角確定所述終端的初始姿態(tài)矩陣。

可選的，所述估計(jì)值確定模塊503，具體用于：

根據(jù)所述補(bǔ)償?shù)牟蓸咏撬俣戎荡_定卡爾曼濾波器的預(yù)測(cè)值，根據(jù)所述更新姿態(tài)矩陣確定所述卡爾曼濾波器的量測(cè)值；

根據(jù)確定的所述量測(cè)值修正所述預(yù)測(cè)值，得到所述終端的最優(yōu)姿態(tài)估計(jì)值。

可選的，所述終端的最優(yōu)姿態(tài)估計(jì)包括陀螺儀的隨機(jī)誤差；

所述補(bǔ)償模塊501，具體用于：

根據(jù)上一個(gè)采樣周期最優(yōu)姿態(tài)估計(jì)中的所述陀螺儀的隨機(jī)誤差，以及陀螺儀的系統(tǒng)誤差，補(bǔ)償當(dāng)前采樣周期陀螺儀輸出的采樣角速度值。

可選的，所述補(bǔ)償模塊501，還用于：

在接收到傳感器校正指令時(shí)，獲取所述加速度傳感器輸出的校驗(yàn)加速度的值，根據(jù)所述校驗(yàn)加速度的值確定所述加速度傳感器的系統(tǒng)誤差。

可選的，所述補(bǔ)償模塊501，還用于：

在接收到傳感器校正指令時(shí)，獲取所述陀螺儀輸出的校驗(yàn)角速度的值，根據(jù)所述校驗(yàn)角速度的值確定所述陀螺儀的系統(tǒng)誤差。

本發(fā)明提供了一種確定終端姿態(tài)的裝置，在當(dāng)前采樣周期，確定終端的陀螺儀輸出的采樣角速度值，并補(bǔ)償確定的所述采樣角速度值；根據(jù)補(bǔ)償?shù)牟蓸咏撬俣戎岛统跏甲藨B(tài)矩陣，確定更新姿態(tài)矩陣；根據(jù)所述確定補(bǔ)償?shù)牟蓸咏撬? 度值和所述更新姿態(tài)矩陣，通過卡爾曼濾波確定所述終端的姿態(tài)最優(yōu)估計(jì)值。本發(fā)明實(shí)施例提供的確定終端姿態(tài)的裝置，提高低速運(yùn)動(dòng)導(dǎo)航的精度、減小了終端姿態(tài)估計(jì)的誤差。

本發(fā)明是參照根據(jù)本發(fā)明實(shí)施例的方法、設(shè)備(系統(tǒng))、和計(jì)算機(jī)程序產(chǎn)品的流程圖和/或方框圖來(lái)描述的。應(yīng)理解可由計(jì)算機(jī)程序指令實(shí)現(xiàn)流程圖和/或方框圖中的每一流程和/或方框、以及流程圖和/或方框圖中的流程和/或方框的結(jié)合?？商峁┻@些計(jì)算機(jī)程序指令到通用計(jì)算機(jī)、專用計(jì)算機(jī)、嵌入式處理機(jī)或其他可編程數(shù)據(jù)處理設(shè)備的處理器以產(chǎn)生一個(gè)機(jī)器，使得通過計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備的處理器執(zhí)行的指令產(chǎn)生用于實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或多個(gè)方框中指定的功能的系統(tǒng)。

這些計(jì)算機(jī)程序指令也可存儲(chǔ)在能引導(dǎo)計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備以特定方式工作的計(jì)算機(jī)可讀存儲(chǔ)器中，使得存儲(chǔ)在該計(jì)算機(jī)可讀存儲(chǔ)器中的指令產(chǎn)生包括指令系統(tǒng)的制造品，該指令系統(tǒng)實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或多個(gè)方框中指定的功能。

這些計(jì)算機(jī)程序指令也可裝載到計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備上，使得在計(jì)算機(jī)或其他可編程設(shè)備上執(zhí)行一系列操作步驟以產(chǎn)生計(jì)算機(jī)實(shí)現(xiàn)的處理，從而在計(jì)算機(jī)或其他可編程設(shè)備上執(zhí)行的指令提供用于實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或多個(gè)方框中指定的功能的步驟。

盡管已描述了本發(fā)明的優(yōu)選實(shí)施例，但本領(lǐng)域內(nèi)的技術(shù)人員一旦得知了基本創(chuàng)造性概念，則可對(duì)這些實(shí)施例作出另外的變更和修改。所以，所附權(quán)利要求意欲解釋為包括優(yōu)選實(shí)施例以及落入本發(fā)明范圍的所有變更和修改。

顯然，本領(lǐng)域的技術(shù)人員可以對(duì)本發(fā)明進(jìn)行各種改動(dòng)和變型而不脫離本發(fā)明的精神和范圍。這樣，倘若本發(fā)明的這些修改和變型屬于本發(fā)明權(quán)利要求及其等同技術(shù)的范圍之內(nèi)，則本發(fā)明也意圖包含這些改動(dòng)和變型在內(nèi)。