本公開要求在2014年10月1日提交的題為“用于垂直軌跡確定的方法和系統(tǒng)(METHODS AND SYSTEMS FOR VERTICAL TRAJECTORY DETERMINATION)”的美國(guó)臨時(shí)專利申請(qǐng)?zhí)?2/058,517和在2015年1月9日提交的題為“用于垂直軌跡確定和自動(dòng)跳躍檢測(cè)的方法和系統(tǒng)(METHODS AND SYSTEMS FOR VERTICAL TRAJECTORY DETERMINATION AND AUTOMATIC JUMP DETECTION)”的美國(guó)臨時(shí)專利申請(qǐng)?zhí)?2/101,942的權(quán)益，其全部?jī)?nèi)容通過引用并入本文。

技術(shù)領(lǐng)域

本公開涉及包括MEMS慣性測(cè)量單元的慣性導(dǎo)航系統(tǒng)和用于捕獲和跟蹤人在垂直方向上的運(yùn)動(dòng)的其它系統(tǒng)。

背景技術(shù)：

關(guān)鍵表現(xiàn)變量(KPV)是運(yùn)動(dòng)員表現(xiàn)的定量測(cè)量。通過訪問和查看運(yùn)動(dòng)員的KPV，運(yùn)動(dòng)員可以顯著地提高整體表現(xiàn)。KPV允許運(yùn)動(dòng)員與教練共享表現(xiàn)信息、記錄和跟蹤隨時(shí)間推移的表現(xiàn)并向運(yùn)動(dòng)員提供實(shí)時(shí)反饋。

目前可用的基于視頻或基于攝像機(jī)的“動(dòng)作捕獲”(MOCAP)方法提供了很少的定量變量。此外，這些MOCAP技術(shù)被限制在室內(nèi)使用或不適合戶外運(yùn)動(dòng)(諸如滑雪、單板滑雪、溜旱冰和騎自行車)的動(dòng)作捕獲的限制區(qū)域，因?yàn)檫@些活動(dòng)發(fā)生在較大距離上。

另一方面，慣性導(dǎo)航系統(tǒng)(INS)是獨(dú)立成套的，并因此可以提供對(duì)高級(jí)動(dòng)作和位置信息的無約束可達(dá)性。INS被廣泛用在各種MOCAP應(yīng)用，諸如船舶和飛機(jī)導(dǎo)航、緊固工具和鋼筆跟蹤以及運(yùn)動(dòng)分析，以提供諸如姿態(tài)、速度和位置等信息。

近來，微型微機(jī)電系統(tǒng)(MEMS)慣性設(shè)備已經(jīng)變得更加普遍，并且MEMS慣性設(shè)備的小尺寸致使使用可穿戴MOCAP技術(shù)進(jìn)行人體動(dòng)作跟蹤的INS的出現(xiàn)。通常，可穿戴MOCAP設(shè)備利用MEMS慣性測(cè)量單元(MEMS-IMU)和/或絕對(duì)位置傳感器來捕獲用于室內(nèi)/室外定位的動(dòng)作。例如，MOCAP攝像機(jī)系統(tǒng)可以用作絕對(duì)位置傳感器，并且被添加到用于在室內(nèi)環(huán)境中進(jìn)行更精確的人體定位的慣性設(shè)備。對(duì)于室外環(huán)境，全球定位系統(tǒng)(GPS)為示例性的絕對(duì)位置傳感器，其可以被用于加強(qiáng)慣性測(cè)量單元以用于更精確的人體定位。

上述GPS/MEMS-IMU集成方法的缺點(diǎn)在于消費(fèi)者級(jí)GPS導(dǎo)出的垂直位置(或海拔高度)信息通常比水平位置信息精確度低得多。GPS海拔高度測(cè)量精度可以變化高達(dá)40米(通常為10米到20米)；這種不精確性的最重要的原因是衛(wèi)星可見度(即在地平線上幾乎沒有可見的衛(wèi)星)以及多徑信號(hào)效應(yīng)。實(shí)時(shí)GPS技術(shù)(諸如實(shí)時(shí)運(yùn)動(dòng)學(xué)GPS和差分GPS)提供更高的位置精度，但是其高昂的成本是運(yùn)動(dòng)消費(fèi)電子市場(chǎng)的限制因素。

附圖說明

現(xiàn)在將參考附圖僅以示例的方式描述本公開的實(shí)施例。

圖1為根據(jù)本公開的實(shí)施例的垂直位置確定系統(tǒng)的示意性框圖。

圖2為根據(jù)本公開的實(shí)施例的垂直位置確定的方法的流程圖。

圖3為根據(jù)本公開的實(shí)施例的用于垂直軌跡確定的設(shè)備的示意性框圖。

具體實(shí)施方式

本公開致力于改進(jìn)消費(fèi)級(jí)設(shè)備中的垂直位置確定的需求。

圖1為根據(jù)本公開的實(shí)施例的示例垂直位置和速度確定系統(tǒng)100的示意性框圖。如下所述，在一些實(shí)施例中，系統(tǒng)100可提供增加的計(jì)算效率，并且可在可穿戴電子設(shè)備中實(shí)現(xiàn)，以向設(shè)備的佩戴者提供垂直動(dòng)作信息。

系統(tǒng)100包括用于接收轉(zhuǎn)向速率信息的輸入101、用于接收加速度信息的輸入102和用于接收氣壓信息的輸入103。轉(zhuǎn)向速率信息輸入101可例如被連接到陀螺儀，諸如三軸陀螺儀(在圖3中示出為MEMS-IMU 304的一部分)。加速度信息輸入102可例如被連接到加速度計(jì)，諸如三軸加速度計(jì)(在圖3中示為MEMS-IMU 304的一部分)。氣壓信息輸入103可例如被連接到氣壓高度計(jì)(在圖3中示為MEMS氣壓高度計(jì)302)。系統(tǒng)100可與提供加速度計(jì)和陀螺儀并且還具有與其集成的氣壓高度計(jì)的慣性測(cè)量單元(IMU)結(jié)合使用。如本文所使用的，術(shù)語“IMU-baro”用于指具有集成氣壓高度計(jì)的這種IMU。系統(tǒng)100也包括垂直位置估計(jì)輸出端104和垂直速度估計(jì)輸出端105。

垂直位置和速度確定系統(tǒng)100另外包括級(jí)聯(lián)兩步卡爾曼濾波器(KF)系統(tǒng)，其包括方位KF 110和垂直位置/速度KF 120。第一步驟(方位KF 110)的輸出被設(shè)置為第二步驟(垂直位置/速度KF 120)的輸入。

方位KF 110包括重力矢量時(shí)間更新器111和重力矢量測(cè)量更新器112。重力矢量時(shí)間更新器111根據(jù)轉(zhuǎn)向速率信息輸入101生成傳感器坐標(biāo)系(IMU-baro的坐標(biāo)系)中的重力矢量的預(yù)測(cè)113。重力矢量測(cè)量更新器112從重力矢量預(yù)測(cè)113和加速度信息輸入102兩者的加權(quán)組合生成傳感器坐標(biāo)系中的校正重力矢量。然后，通過使用傳感器坐標(biāo)系中的校正重力矢量，重力矢量測(cè)量更新器112生成滾轉(zhuǎn)角和俯仰角信息114。如本文所使用的，術(shù)語“滾轉(zhuǎn)角”是指圍繞沿著第一(例如前后)方向的水平軸線的旋轉(zhuǎn)，以及術(shù)語“俯仰角”是指圍繞沿著垂直于第一方向的第二(例如左右)方向的水平軸線的旋轉(zhuǎn)。

垂直位置/速度KF 120包括垂直位置/速度時(shí)間更新器121、垂直位置/速度測(cè)量更新器122和可選的零速度更新檢測(cè)器123。垂直位置/速度時(shí)間更新器121從滾轉(zhuǎn)和俯仰角信息114以及加速度信息輸入102生成垂直位置/速度預(yù)測(cè)124。

垂直位置/速度測(cè)量更新器122基于垂直位置/速度預(yù)測(cè)124和氣壓信息輸入103的加權(quán)組合生成校正垂直位置估計(jì)104和垂直速度估計(jì)105。

可選零速度更新檢測(cè)器123從加速度信息輸入102生成零速度信號(hào)125。當(dāng)靜止相位(still phase)被零速度更新檢測(cè)器123檢測(cè)到時(shí)，零速度信號(hào)125迫使垂直速度估計(jì)105為零。

INS通過求解捷聯(lián)式慣性導(dǎo)航方程通過外部加速度值的積分來估計(jì)包括位置和速度的導(dǎo)航參數(shù)。因此，重要的是INS通過從加速度計(jì)信號(hào)中減去重力加速度來精確地估計(jì)動(dòng)態(tài)狀況期間的外部加速度。為了實(shí)現(xiàn)該減法函數(shù)，INS需要對(duì)其方位進(jìn)行精確的估計(jì)。

圖1的實(shí)施例包括采取方位KF與垂直位置/速度KF級(jí)聯(lián)的形式的級(jí)聯(lián)KF。級(jí)聯(lián)KF提供良好的計(jì)算效率。級(jí)聯(lián)KF具有比全局KF小得多的計(jì)算開銷，因?yàn)榕c方位狀態(tài)相關(guān)的計(jì)算與垂直位置/速度KF分離。因此，級(jí)聯(lián)KF可以使用線性函數(shù)來實(shí)現(xiàn)，而全局KF將需要非線性卡爾曼濾波方法，諸如由擴(kuò)展卡爾曼濾波器(EKF)或無跡卡爾曼濾波器(UKF)所利用的那些方法。除了改善的計(jì)算成本之外，級(jí)聯(lián)KF允許增加靈活性并且更容易在全局KF上實(shí)現(xiàn)和調(diào)諧。

雖然級(jí)聯(lián)KF理論上產(chǎn)生與全局KF相比次優(yōu)(不太精確)的估計(jì)，但實(shí)際上為了慣性導(dǎo)航的目的，級(jí)聯(lián)KF的表現(xiàn)與全局KF正相當(dāng)。根據(jù)本文所公開的示例的示例級(jí)聯(lián)KF系統(tǒng)的初步測(cè)試指示分別用于慢動(dòng)作、垂直跳躍和下降跳躍(step-down jump)垂直軌跡跟蹤的垂直軌跡跟蹤誤差約為26.9cm、27.2cm和28.1cm。實(shí)驗(yàn)設(shè)置的細(xì)節(jié)和初步測(cè)試的進(jìn)一步結(jié)果在附錄A中的文件中列出。

使用Rauch-Tung-Striebel(RTS)平滑器觀察到垂直軌跡跟蹤精度的顯著改善，如下所述。另外，基于實(shí)驗(yàn)結(jié)果，通過使用RTS平滑器，用于垂直跳躍和下降跳躍的平均跳躍KPV(高度/下降)確定誤差分別約為2.9cm和5.8cm(與在垂直方向上的一些點(diǎn)中的GPS的～40m誤差相比)。這個(gè)所實(shí)現(xiàn)的精度應(yīng)足以用于跳躍高度的量級(jí)為小的娛樂目的。另一方面，運(yùn)動(dòng)員運(yùn)動(dòng)跳躍中的跳躍高度的量級(jí)遠(yuǎn)遠(yuǎn)大于娛樂跳躍中的跳躍高度的量級(jí)。因此，所實(shí)現(xiàn)的約2.9-5.8cm的跳躍KPV確定精度對(duì)于使用可穿戴技術(shù)的戶外測(cè)量目的來說也應(yīng)當(dāng)是足夠的。

現(xiàn)在將描述級(jí)聯(lián)KF中的各個(gè)濾波器的結(jié)構(gòu)，即方位KF 110和垂直位置/速度KF 120。在下面的描述中，下標(biāo)1和2分別表示方位KF 110和垂直位置/速度KF 120的變量。

方位KF 110生成滾轉(zhuǎn)角和俯仰角信息114。滾轉(zhuǎn)角和俯仰角信息114對(duì)于垂直方向上的重力加速度補(bǔ)償和慣性動(dòng)作跟蹤來說是足夠的。因此，不需要完整的三維方位的信息。

三軸陀螺儀和三軸加速度計(jì)分別向方位KF 110提供轉(zhuǎn)向速率信息輸入101和加速度信息輸入102。重力矢量時(shí)間更新器111執(zhí)行下面的方程式1的函數(shù)，以根據(jù)轉(zhuǎn)向速率信息輸入101生成傳感器坐標(biāo)系中的重力矢量的預(yù)測(cè)113。

x₁(k)＝A₁(k-1)x₁(k-1)+w₁(k-1) (方程式1)

在方程式1中，x₁(k)為在步驟k處的方位KF 110的3×1狀態(tài)矢量；因此，x₁(k)為傳感器坐標(biāo)系中的歸一化重力矢量；A₁為狀態(tài)轉(zhuǎn)移矩陣；以及w₁為過程模型噪聲矢量。重力矢量時(shí)間更新器111使用先前的重力矢量預(yù)測(cè)x₁(k-1)從預(yù)測(cè)模型生成傳感器坐標(biāo)系中的重力矢量x₁(k)的預(yù)測(cè)113。

重力矢量測(cè)量更新器112執(zhí)行下面的方程式2的函數(shù)，以利用來自加速度信息輸入102的測(cè)量來更新重力矢量預(yù)測(cè)113。

z₁(k)＝C₁(k)x₁(k)+v₁(k) (方程式2)

在方程式2中，z₁為測(cè)量矢量(即在加速度計(jì)的傳感器坐標(biāo)系中測(cè)量的重力矢量)；C₁為3×3觀察矩陣；以及v₁為測(cè)量模型噪聲矢量。

方程式1和2的矩陣從下面的方程式3至13計(jì)算。

生成方位KF的重力矢量預(yù)測(cè)

狀態(tài)轉(zhuǎn)移矩陣A₁(k-1)從I₃導(dǎo)出，I₃為3×3單位矩陣；以及為三軸陀螺儀測(cè)量的3×3斜對(duì)稱矩陣。

過程模型噪聲矢量w₁(k-1)基于其為x₁的3×3斜對(duì)稱矩陣；以及n_G，其為陀螺儀測(cè)量噪聲矢量，并且被假定為不相關(guān)和零均值白高斯。

因此，方程式1、3和4定義重力矢量預(yù)測(cè)113。以此方式，重力矢量時(shí)間更新器111從轉(zhuǎn)向速率信息輸入101導(dǎo)出x₁(k)，重力矢量預(yù)測(cè)113。

生成方位KF的滾轉(zhuǎn)角和俯仰角信息

C₁(k)＝gI₃ (方程式5)

在方程式5中，從I₃導(dǎo)出3×3觀察矩陣C₁，I₃為3×3單位矩陣；以及g，其為重力矢量的范數(shù)。

在方程式6中，從為加速度計(jì)的傳感器坐標(biāo)系中的外部加速度誤差的導(dǎo)出測(cè)量模型噪聲矢量v₁；以及n_A，其為陀螺儀測(cè)量噪聲矢量，并且被假定為不相關(guān)和零均值白高斯。

方程式7和8描述了加速度計(jì)的傳感器坐標(biāo)系中的外部加速度誤差。

^Sa^-(k)＝c_a^Sa⁺(k+1) (方程式8)

在方程7中，^Sa(k)＝c_a^Sa(k-1)+ε(k)，其中，ε(k)為外部加速度過程模型的時(shí)變誤差。在方程式7和8中，上標(biāo)+和-分別表示方位KF 110中的“后驗(yàn)”和“先驗(yàn)”估計(jì)；上標(biāo)S表示該量在傳感器坐標(biāo)系中；c_a為在0和1之間的無量綱常數(shù)，其用于確定外部加速度模型中的截止頻率。

z₁(k)＝y(tǒng)_A(k)-c_a^Sa⁺(k-1) (方程式9)

方程式2中的測(cè)量矢量z₁由方程式9和10定義，其中，y_A(k)為加速度計(jì)的偏置補(bǔ)償輸出矢量；以及傳感器坐標(biāo)系中所估計(jì)的歸一化重力矢量為因此，方程式2和5-10定義了傳感器坐標(biāo)系中所估計(jì)的歸一化重力矢量

方程式11使用傳感器坐標(biāo)系中所估計(jì)的歸一化重力矢量來生成所計(jì)算的滾轉(zhuǎn)角(γ)和俯仰角(β)。根據(jù)方程式2和5-11，重力矢量測(cè)量更新器112從y_A(k)、加速度信息輸入102以及從x₁(k)、重力矢量預(yù)測(cè)113生成指示滾轉(zhuǎn)角(γ)和俯仰角(β)的方位信號(hào)114。

R₁(k)＝∑_acc+∑_A (方程式13)

方程式12和13定義方位KF 110中的過程和測(cè)量噪聲協(xié)方差矩陣。使用過程模型噪聲矢量w₁(k-1)計(jì)算過程噪聲協(xié)方差矩陣Q₁(k-1)，并且其等于E[w₁(k-1)w₁(k-1)^T]，其中，E為期望算子。Σ_G為被定義為E[n_Gn_G^T]的陀螺儀測(cè)量噪聲的協(xié)方差矩陣。通過假設(shè)陀螺噪聲方差在三個(gè)軸上等于σ_G，Σ_G被設(shè)置為

使用測(cè)量模型噪聲矢量v₁(k)來計(jì)算測(cè)量噪聲協(xié)方差矩陣R₁(k)，并且其等于E[v₁(k)v₁(k)^T]，為加速度計(jì)測(cè)量噪聲的協(xié)方差矩陣的Σ_A被設(shè)置為Σ_acc為加速度模型的協(xié)方差，并被設(shè)置為

生成垂直位置/速度KF的垂直位置/速度預(yù)測(cè)

垂直位置/速度KF 120生成垂直位置估計(jì)104和垂直速度估計(jì)105。方位KF 110、三軸加速度計(jì)和氣壓高度計(jì)分別向垂直位置/速度KF 120提供滾轉(zhuǎn)角(γ)和俯仰角(β)(自方位信號(hào)114)、提供加速度信息(自輸入102)和氣壓信息(自輸入103)。

垂直位置/速度時(shí)間更新器121執(zhí)行下面的方程式14的函數(shù)，以從來自方位信號(hào)114的滾轉(zhuǎn)角和俯仰角以及來自輸入102的加速度信息生成垂直位置/速度預(yù)測(cè)124。

x₂(k)＝A₂(k-1)x₂(k-1)+B₂(k-1)u₂(k-1)+w₂(k-1) (方程式14)

在方程式14中，x₂(k)＝[h(k)v(k)]^T為垂直位置/速度KF 120的狀態(tài)矢量，其包括垂直位置和垂直速度分量；A₂(k-1)和B₂(k-1)為垂直位置/速度KF 120的狀態(tài)變換和輸入矩陣；u₂(k-1)為輸入矢量，其包括導(dǎo)航坐標(biāo)系中的重力補(bǔ)償加速度的垂直分量，并且使用輸入102和114來計(jì)算；w₂(k-1)為過程噪聲的2×1矢量。

方程式15和方程式16分別定義狀態(tài)變換矩陣和輸入矩陣A₂(K-1)和B₂(K-1)。

方程式17定義導(dǎo)航坐標(biāo)系中的重力補(bǔ)償加速度的垂直分量。導(dǎo)航坐標(biāo)系中的重力補(bǔ)償?shù)募铀俣鹊拇怪狈至康闹祷谛D(zhuǎn)矩陣其使用來自方位信號(hào)114的滾轉(zhuǎn)角(γ)和俯仰角(β)和來自輸入102的加速度y_A(k-1)來計(jì)算。將加速度計(jì)傳感器坐標(biāo)系的z軸對(duì)準(zhǔn)導(dǎo)航坐標(biāo)系；

方程式18和19分別描述了將加速度計(jì)傳感器坐標(biāo)系的z軸與導(dǎo)航坐標(biāo)系對(duì)準(zhǔn)的旋轉(zhuǎn)矩陣以及過程噪聲的2×1矢量w₂(k-1)。

根據(jù)方程式14至19，垂直位置/速度時(shí)間更新器121根據(jù)來自方位信號(hào)114的滾轉(zhuǎn)角(γ)和俯仰角(β)以及加速度信息輸入102生成x₂(k)、垂直位置/速度預(yù)測(cè)124。

生成垂直位置/速度KF的垂直位置估計(jì)和垂直速度估計(jì)

垂直位置/速度測(cè)量更新器122執(zhí)行下面的方程式20的函數(shù)，以從垂直位置/速度預(yù)測(cè)124和氣壓計(jì)信息輸入103生成垂直位置估計(jì)104和垂直速度估計(jì)105。

z₂(k)＝C₂(k)x₂(k)+v₂(k) (方程式20)

在方程式20中，z₂(k)為從氣壓計(jì)信息輸入103計(jì)算的相對(duì)高度(Δh_baro)；C₂(k)為觀察矩陣；以及v₂(k)為測(cè)量噪聲。

方程式21描述了氣壓計(jì)信息輸入103，P如何在垂直位置/速度測(cè)量更新器122中使用。在方程式21中，Δh_baro為相對(duì)于參考位置的初始高度(h_init)的相對(duì)高度；h_init使用在參考位置處的初始?jí)毫?shù)據(jù)來計(jì)算(h_init可例如使用如下所述的靜態(tài)初始化來計(jì)算)；以及P₀為等于101,325Pa的標(biāo)準(zhǔn)壓力。由于氣壓高度計(jì)中存在顯著的量化噪聲，應(yīng)使用滾轉(zhuǎn)平均濾波器來提高氣壓高度測(cè)量的精度，并且在測(cè)量噪聲v₂(k)中應(yīng)考慮其剩余誤差。

C₂(k)＝[1 0] (方程式22)

方程式22定義了觀察矩陣C₂(k)。

方程式23定義了使用過程噪聲矢量w₂(k-1)和輸入矩陣B₂(k-1)來計(jì)算的過程噪聲協(xié)方差矩陣Q₂(k-1)。

方程式24定義了測(cè)量噪聲協(xié)方差矩陣R₂(k)，其中，為氣壓計(jì)噪聲方差。測(cè)量噪聲協(xié)方差矩陣R₂(k)通過E[v₂(k)v₂(k)^T]獲得。

根據(jù)方程式20至24，垂直位置/速度測(cè)量更新器122利用從氣壓計(jì)信息輸入103導(dǎo)出的相對(duì)高度測(cè)量值z(mì)₂(k)校正x₂(k)＝[h(k)v(k)]^T垂直位置/速度預(yù)測(cè)124，以生成垂直位置估計(jì)104，h(k)和垂直速度估計(jì)105，h(k)。

圖1的垂直位置/速度確定系統(tǒng)有利地提供了不使用GPS的相對(duì)高度測(cè)量的精確估計(jì)。因此，垂直位置和速度確定系統(tǒng)100可以與GPS/MEMS-IMU組合以增強(qiáng)具有改進(jìn)的垂直位置確定的GPS/MEMS-IMU。垂直位置和速度確定系統(tǒng)100特別地與GPS/MEMS-IMU互補(bǔ)，因?yàn)樵诜€(wěn)定狀態(tài)下，GPS可以被用于初始化垂直位置確定系統(tǒng)的氣壓高度計(jì)(到絕對(duì)高度)；而在動(dòng)態(tài)狀態(tài)下，垂直位置和速度確定系統(tǒng)100可以提供相對(duì)高度測(cè)量的精確估計(jì)，而不依賴于GPS的不精確的動(dòng)態(tài)高度測(cè)量。

在圖1的另一實(shí)施例中，垂直位置和速度確定系統(tǒng)100可選地包括零速度更新檢測(cè)器123，以限制INS中的漂移誤差?？蛇x的零速度更新檢測(cè)器123從加速度信息輸入102生成零速度信號(hào)125。當(dāng)靜止相位被零速度更新檢測(cè)器123檢測(cè)到時(shí)，零速度信號(hào)125迫使垂直速度估計(jì)105為零。在一些實(shí)施例中，靜止相位通過對(duì)由加速度計(jì)測(cè)量的加速度信號(hào)的范數(shù)設(shè)置閾值來檢測(cè)，并且當(dāng)加速度信號(hào)低于閾值時(shí)確定靜止相位。

在一些實(shí)施例中，固定初始化可被用于校準(zhǔn)垂直位置和速度確定系統(tǒng)100。在靜止初始化期間，計(jì)算初始姿態(tài)和傳感器偏差。三軸加速度計(jì)和三軸陀螺儀數(shù)據(jù)被用于在方位KF 110中計(jì)算初始傾斜角的平均值。傳感器坐標(biāo)系中的外部加速度^Sa⁺(k)為方位KF 110的副產(chǎn)品。然而，由于在靜止初始化步驟期間外部加速度應(yīng)該為零，所以^Sa⁺(k)的平均值被認(rèn)為是傳感器坐標(biāo)系中的加速度計(jì)偏差矢量。假設(shè)MEMS陀螺儀不夠精確以測(cè)量地球的旋轉(zhuǎn)速率，則陀螺儀偏置矢量被計(jì)算為在該靜止初始化步驟期間三軸陀螺儀測(cè)量的平均值。假定這些偏置值在靜止初始化過程之間的運(yùn)算期間是恒定的。然后從所測(cè)量的加速度計(jì)和陀螺儀信號(hào)中減去所估計(jì)的加速度計(jì)和陀螺儀偏差以用于垂直軌跡估計(jì)。在靜止初始化步驟期間，速度被設(shè)置為零，并且氣壓高度計(jì)中的h_init通過從參考系獲得的初始高度來校準(zhǔn)。該參考系可以提供用于絕對(duì)高度跟蹤的實(shí)際橢球高度(諸如從GPS獲得的高度)或相對(duì)高度跟蹤的相對(duì)于任意坐標(biāo)系的相對(duì)高度(諸如從基于攝像機(jī)的MOCAP系統(tǒng)獲得的高度)。

在本公開的又一實(shí)施例中，垂直位置和速度確定系統(tǒng)100包括軌跡平滑器(在圖3中示為RTS 306)。在不需要實(shí)時(shí)數(shù)據(jù)處理的應(yīng)用中，可以通過將前向和后向處理的數(shù)據(jù)組合的平滑來進(jìn)一步提高垂直位置估計(jì)的精度。在一個(gè)示例中，垂直位置確定系統(tǒng)100的軌跡平滑器被配置為Rauch-Tung-Striebel(RTS)軌跡平滑器。RTS平滑器為在導(dǎo)航應(yīng)用中廣泛使用的軌跡平滑器，其用于平滑位置和速度軌跡。RTS平滑器包括一個(gè)前向數(shù)據(jù)處理部分和一個(gè)后向數(shù)據(jù)處理部分。RTS平滑器的前向處理部分為垂直位置和速度確定系統(tǒng)100的垂直位置/速度KF 120。RTS平滑器存儲(chǔ)來自前向處理部分的估計(jì)及其協(xié)方差，然后使用以下方程式25至27在后向掃描中遞歸地更新所平滑的估計(jì)及其協(xié)方差：

K_s(k)＝P₂⁺(k)A₂(k)[P₂^-(k+1)]^-1 (方程式25)

P_s(k)＝P₂⁺(k)+K_s(k)[P_s(k+1)-P₂^-(k+1)]K_s(k)^T (方程式26)x_s(k)＝x₂⁺(k)+K_s(k)[x_s(k+1)-x₂^-(k+1)] (方程式27)

在方程式25至27中，和為“后驗(yàn)”和“先驗(yàn)”協(xié)方差估計(jì)；K_s為平滑器增益；和為“后驗(yàn)”和“先驗(yàn)”狀態(tài)估計(jì)；以及x_s為所平滑的狀態(tài)矢量。所有上述變量涉及垂直位置/速度KF 120。

圖2示出了根據(jù)本公開的實(shí)施例的垂直位置/速度確定的方法200的流程圖。該方法可以由有形地實(shí)施在非暫態(tài)計(jì)算機(jī)可讀介質(zhì)上的計(jì)算機(jī)程序產(chǎn)品來執(zhí)行。該計(jì)算機(jī)程序產(chǎn)品在被執(zhí)行時(shí)可操作執(zhí)行方法200。

在201，該方法測(cè)量同一設(shè)備中具有集成氣壓高度計(jì)的慣性測(cè)量單元(IMU)(IMU-baro)的轉(zhuǎn)向速率、IMU-baro的加速度，以及使用來自氣壓高度計(jì)的氣壓信息的IMU-baro的高度。在一個(gè)實(shí)施例中，這些測(cè)量包括有噪聲數(shù)據(jù)；也就是說，測(cè)量值固有地包括諸如由傳感器噪聲引起的誤差的誤差。

在202，該方法根據(jù)第一卡爾曼濾波器運(yùn)算基于轉(zhuǎn)向速率測(cè)量和加速度測(cè)量來估計(jì)IMU-baro的滾轉(zhuǎn)和俯仰。在實(shí)施例中，在202處的估計(jì)另外包括卡爾曼濾波器中的遞歸預(yù)測(cè)和校正步驟。

預(yù)測(cè)包括基于使用來自陀螺儀的轉(zhuǎn)向速率測(cè)量和IMU-baro傳感器坐標(biāo)系中的先前重力加速度的捷聯(lián)式積分來預(yù)測(cè)IMU-baro壓力傳感器坐標(biāo)系中的重力加速度矢量。校正包括將傳感器坐標(biāo)系中的重力加速度矢量的預(yù)測(cè)與來自加速度計(jì)的測(cè)量組合以生成傳感器坐標(biāo)系中的重力加速度矢量的估計(jì)。最后，滾轉(zhuǎn)角和俯仰角函數(shù)根據(jù)傳感器坐標(biāo)系中的估計(jì)重力加速度矢量來計(jì)算估計(jì)的滾轉(zhuǎn)角和俯仰角。

在203，該方法向第二卡爾曼濾波器運(yùn)算提供來自第一卡爾曼濾波器運(yùn)算的滾轉(zhuǎn)和俯仰估計(jì)。在實(shí)施例中，第一卡爾曼濾波器運(yùn)算的輸出為第二卡爾曼濾波器運(yùn)算的輸入。

在204，該方法根據(jù)第二卡爾曼濾波器運(yùn)算基于加速度測(cè)量、氣壓測(cè)量以及滾轉(zhuǎn)和俯仰估計(jì)來估計(jì)IMU-baro的垂直位置/速度。在實(shí)施例中，在204處的估計(jì)另外包括卡爾曼濾波器中的遞歸預(yù)測(cè)和校正。

預(yù)測(cè)包括基于使用IMU-baro的加速度和先前的垂直位置和垂直速度的捷聯(lián)式積分來預(yù)測(cè)垂直位置(高度)和垂直速度。

校正包括在卡爾曼濾波器中將預(yù)測(cè)的垂直位置和垂直速度與來自氣壓高度計(jì)的測(cè)量垂直位置(高度測(cè)量)組合以生成垂直位置和垂直速度估計(jì)。

圖3為根據(jù)本公開的實(shí)施例的用于垂直軌跡確定的設(shè)備300的示意性框圖。設(shè)備300包括可佩戴設(shè)備302，其可以容納圖1和圖3所示的各種硬件和軟件組件。設(shè)備300包括用于接收轉(zhuǎn)向速率信息的輸入101、用于接收加速度信息的輸入102和用于接收氣壓信息的輸入103。設(shè)備300可與提供加速度計(jì)和陀螺儀的慣性測(cè)量單元(IMU)結(jié)合使用。在一些情況下，IMU也可具有與其集成的氣壓高度計(jì)。如本文所使用的，術(shù)語“IMU-baro”用于指具有集成氣壓高度計(jì)的這種IMU。

轉(zhuǎn)向速率信息輸入101可例如被連接到陀螺儀，諸如三軸陀螺儀(在圖3中示出為MEMS-IMU 304的一部分)。加速度信息輸入102可例如被連接到加速度計(jì)，諸如三軸加速度計(jì)(在圖3中示為MEMS-IMU 304的一部分)。氣壓信息輸入103可例如被連接到氣壓高度計(jì)(在圖3中示為MEMS氣壓高度計(jì)302)。設(shè)備300另外包括垂直位置估計(jì)輸出104和垂直速度估計(jì)輸出105。

在本公開的一些實(shí)施例中，垂直位置和速度確定設(shè)備300包括軌跡平滑器306(在圖3中示為RTS 306)。在不需要實(shí)時(shí)數(shù)據(jù)處理的應(yīng)用中，可以通過將前向和后向處理的數(shù)據(jù)組合的平滑來進(jìn)一步提高垂直位置估計(jì)的精度。在示例中，垂直位置確定設(shè)備300的軌跡平滑器被配置為Rauch-Tung-Striebel(RTS)軌跡平滑器306。RTS平滑器306為在導(dǎo)航應(yīng)用中廣泛使用的軌跡平滑器，其用于平滑位置和速度軌跡。

在前面的描述中，出于解釋的目的，闡述了許多細(xì)節(jié)以便提供對(duì)實(shí)施例的透徹理解。然而，對(duì)本領(lǐng)域的技術(shù)人員顯而易見的是，這些具體細(xì)節(jié)不是必需的。在其它實(shí)例中，以框圖形式示出了公知的電氣結(jié)構(gòu)和電路，以免混淆理解。例如，沒有提供關(guān)于本文所述的實(shí)施例是作為軟件例程、硬件電路、固件還是其組合來實(shí)現(xiàn)的具體細(xì)節(jié)。

本公開的實(shí)施例可以被表示為存儲(chǔ)在機(jī)器可讀介質(zhì)(也稱為計(jì)算機(jī)可讀介質(zhì)、處理器可讀介質(zhì)或其中包含計(jì)算機(jī)可讀程序代碼的計(jì)算機(jī)可用介質(zhì))中的計(jì)算機(jī)程序產(chǎn)品。機(jī)器可讀介質(zhì)可以為任何合適的有形的非暫態(tài)介質(zhì)，包括磁、光或電存儲(chǔ)介質(zhì)，包括磁盤、光盤只讀存儲(chǔ)器(CD-ROM)、存儲(chǔ)器設(shè)備(易失性或非易失性)或類似的存儲(chǔ)機(jī)制。機(jī)器可讀介質(zhì)可以包含各種指令集、代碼序列、配置信息或其它數(shù)據(jù)，其在被執(zhí)行時(shí)使處理器執(zhí)行根據(jù)本公開的實(shí)施例的方法中的步驟。本領(lǐng)域普通技術(shù)人員應(yīng)理解，實(shí)現(xiàn)所述的實(shí)施方案所必需的其它指令和運(yùn)算也可以被存儲(chǔ)在機(jī)器可讀介質(zhì)上。存儲(chǔ)在機(jī)器可讀介質(zhì)上的指令可以由處理器或其它合適的處理設(shè)備執(zhí)行，并且可以與電路交互以執(zhí)行所述的任務(wù)。

上述實(shí)施例旨在僅為示例。本領(lǐng)域的技術(shù)人員可以對(duì)特定實(shí)施例實(shí)施變更、修改和變動(dòng)。權(quán)利要求的范圍不應(yīng)受到本文所闡述的特定實(shí)施例的限制，而是應(yīng)以與整個(gè)說明書一致的方式來解釋。

本公開提供了用于垂直位置和速度確定的方法和系統(tǒng)。垂直位置和速度確定將來自垂直位置傳感器(諸如使用壓力測(cè)量導(dǎo)出高度信息的MEMS氣壓高度計(jì))的垂直位置信息與從MEMS-IMU導(dǎo)出的方位(傾斜角)和加速度信息組合。MEMS氣壓高度計(jì)的輸出與使用級(jí)聯(lián)兩步卡爾曼濾波器(KF)系統(tǒng)的MEMS-IMU的輸出相融合。

所提出的方法需要無磁力計(jì)的MEMS-IMU來提供傾斜角，并因此，垂直位置確定對(duì)抗磁擾動(dòng)是魯棒的。另外，使用級(jí)聯(lián)的KF避免需要傳播附加狀態(tài)，從而提高了位置確定方法的計(jì)算效率。因此，本位置確定方法適合于運(yùn)動(dòng)員穿戴的小且輕質(zhì)的電池供電的電子設(shè)備。

本公開的一個(gè)方面提供了用于同一設(shè)備中具有集成氣壓高度計(jì)的慣性測(cè)量單元(IMU)(IMU-baro)的垂直位置和速度確定系統(tǒng)。該系統(tǒng)包括被連接以接收所測(cè)量的IMU-baro方位的轉(zhuǎn)向速率輸入；被連接以接收所測(cè)量的IMU-baro加速度的加速度輸入；被連接以接收所測(cè)量的IMU-baro高度的氣壓輸入；被連接到轉(zhuǎn)向速率輸入和加速度輸入的第一卡爾曼濾波器，以基于所測(cè)量的IMU-baro方位和所測(cè)量的IMU-baro加速度來估計(jì)IMU-baro的滾轉(zhuǎn)和俯仰；以及連接到加速度輸入、氣壓輸入和第一卡爾曼濾波器的第二卡爾曼濾波器；第二卡爾曼濾波器與第一卡爾曼濾波器級(jí)聯(lián)以從第一卡爾曼濾波器接收IMU-baro的估計(jì)的滾轉(zhuǎn)角和俯仰角，并且基于所測(cè)量的IMU-baro加速度、使用氣壓高度計(jì)測(cè)量的氣壓以及IMU-baro的估計(jì)滾轉(zhuǎn)和俯仰來估計(jì)IMU-baro的垂直位置和垂直速度。

本公開的另一方面提供了用于確定IMU-baro的高度的設(shè)備。該設(shè)備包括：第一卡爾曼濾波器，其被配置為基于所測(cè)量的IMU-baro轉(zhuǎn)向速率和基于所測(cè)量的IMU-baro加速度來估計(jì)IMU-baro的滾轉(zhuǎn)和俯仰；以及第二卡爾曼濾波器，其以級(jí)聯(lián)布置被連接到第一卡爾曼濾波器以接收所估計(jì)的IMU-baro的滾轉(zhuǎn)和俯仰。第二卡爾曼濾波器被配置為基于所估計(jì)的IMU-baro的側(cè)切和俯仰、所測(cè)量的IMU-baro加速度和所測(cè)量的氣壓來估計(jì)IMU-baro的垂直位置和垂直速度，并輸出所估計(jì)的IMU-baro的垂直位置和垂直速度。

本公開的另一方面提供了有形地實(shí)施在非暫態(tài)計(jì)算機(jī)可讀介質(zhì)上的計(jì)算機(jī)程序產(chǎn)品，該計(jì)算機(jī)程序產(chǎn)品在被執(zhí)行時(shí)可操作以執(zhí)行確定IMU-baro的垂直位置和速度的方法。該方法包括測(cè)量IMU-baro的轉(zhuǎn)向速率、IMU-baro的加速度和氣壓；根據(jù)第一卡爾曼濾波器運(yùn)算基于轉(zhuǎn)向速率測(cè)量和加速度測(cè)量來估計(jì)IMU-baro的滾轉(zhuǎn)和俯仰；將該滾轉(zhuǎn)和俯仰估計(jì)從第一卡爾曼濾波器運(yùn)算提供給第二卡爾曼濾波器運(yùn)算；根據(jù)第二卡爾曼濾波運(yùn)算基于加速度測(cè)量、氣壓測(cè)量以及滾轉(zhuǎn)和俯仰估計(jì)來估計(jì)IMU-baro的垂直位置和速度；以及基于所估計(jì)的垂直位置和速度來確定IMU-baro的垂直位置和速度；以及輸出所確定的垂直位置和速度。

通過結(jié)合附圖閱讀以下對(duì)具體實(shí)施例的描述，本公開的其它方面和特征對(duì)于本領(lǐng)域普通技術(shù)人員將變得顯而易見。