一種綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種移動設(shè)備姿態(tài)的校正方法，其特征是綜合考慮移動設(shè)備上陀螺儀 和加速度計的特點，提出一種綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法。
【背景技術(shù)】
[0002] 目前，基于移動設(shè)備的很多應(yīng)用，諸如增強現(xiàn)實、感應(yīng)游戲等應(yīng)用需要對移動設(shè)備 的當(dāng)前姿態(tài)進行準(zhǔn)確判斷，從而提供精準(zhǔn)的信息推送或者良好的用戶操控體驗。目前對移 動設(shè)備姿態(tài)的判斷主要有以下兩種方法： 一種判斷移動設(shè)備姿態(tài)的方法是基于移動設(shè)備內(nèi)置的磁場傳感器和加速度傳感器，利 用磁場傳感器獲得的地磁場方向和加速度傳感器獲得的地球重力方向，聯(lián)合起來計算移動 設(shè)備當(dāng)前的姿態(tài)。
[0003] 這種方法有局限性。首先，移動設(shè)備的磁場傳感器容易受到其他磁場的干擾，特別 在室內(nèi)環(huán)境下，諸如電視、音響、冰箱、電暖器等都會產(chǎn)生自己的穩(wěn)定或不穩(wěn)定的磁場，而移 動設(shè)備內(nèi)置的磁場傳感器是無法區(qū)分地磁場和這些額外產(chǎn)生的磁場的，因此，會造成由磁 場傳感器獲取到的地磁向量出現(xiàn)較大的偏差。其次，地球重力向量是不能直接獲取到的，需 利用移動設(shè)備的內(nèi)置加速度計經(jīng)過計算來獲取。在對加速度計的輸出進行低通濾波后，可 以獲取到近似的重力向量，這意味著，在移動設(shè)備姿態(tài)變化較為快速的時候，重力向量的獲 取是具有較大偏差的。
[0004] 另一種判斷移動設(shè)備姿態(tài)的方法是利用移動設(shè)備的內(nèi)置陀螺儀來計算移動設(shè)備 的當(dāng)前姿態(tài)。陀螺儀可以輸出移動設(shè)備的瞬時角速度，利用角速度和相應(yīng)的采樣時間間隔 可以計算出移動設(shè)備在時隙內(nèi)的轉(zhuǎn)動角度，將這些角度累積起來，即可計算出移動設(shè)備相 對于初始位置的轉(zhuǎn)動向量，從而獲取到當(dāng)前的姿態(tài)。
[0005] 這種方法同樣具有局限性。首先，采樣間隔不可無限小，由此所做的積分并非數(shù)學(xué) 意義上的積分，只是一個近似，帶來了積分誤差。其次，采樣的頻率并非理想，在移動設(shè)備姿 態(tài)變化比較快的情況下，采樣輸出和真實值之間有較大偏差，而由于偏差會累積，造成接下 來的計算是在錯誤的基礎(chǔ)之上，因此移動設(shè)備的姿態(tài)計算出現(xiàn)偏差，而且隨著時間的持續(xù) 偏差會越來越大。

【發(fā)明內(nèi)容】

[0006] 為了實現(xiàn)更為精確高效的移動設(shè)備的當(dāng)前姿態(tài)判斷，本發(fā)明提出一種綜合利用加 速度計和陀螺儀來校準(zhǔn)移動設(shè)備姿態(tài)的方法，在利用移動設(shè)備上陀螺儀進行設(shè)備姿態(tài)判斷 的基礎(chǔ)之上，利用加速度計來進行校準(zhǔn)，從而提供更為準(zhǔn)確的移動設(shè)備當(dāng)前姿態(tài)判斷，為諸 如增強現(xiàn)實、感應(yīng)游戲等應(yīng)用提供更好的用戶體驗。
[0007] 本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案是： 第一步：結(jié)合移動設(shè)備初始姿態(tài)利用內(nèi)置陀螺儀積分計算移動設(shè)備當(dāng)前姿態(tài)，具體步 驟為：步驟1 :利用陀螺儀獲取移動設(shè)備的旋轉(zhuǎn)角速度；步驟2 :利用旋轉(zhuǎn)角速度計算旋轉(zhuǎn) 向量；步驟3 :通過積分來計算移動設(shè)備當(dāng)前姿態(tài)及旋轉(zhuǎn)矩陣。
[0008] 第二步，利用內(nèi)置加速度計和陀螺儀來產(chǎn)生需要校正的判斷條件，具體判斷條件 為：條件1 :利用陀螺儀所計算出的旋轉(zhuǎn)角速度向量模小于〇. 0005 ;條件2 :利用陀螺儀所 計算出的當(dāng)前姿態(tài)狀態(tài)為俯仰角大于75度且小于89度。
[0009] 第三步，如果達(dá)到第二步所述的判斷條件，則利用加速度計來進行姿態(tài)校正，具體 校正步驟為：步驟1:利用低通濾波器對內(nèi)置加速度計輸出進行處理以獲取重力向量；步驟 2 :利用重力向量計算移動設(shè)備的俯仰角和滾動角；步驟3 :利用第一步所獲取到的旋轉(zhuǎn)矩 陣求出俯仰角、滾動角、方向角；步驟4 :利用步驟2獲取到的俯仰角和滾動角去替代步驟3 獲取到的俯仰角和滾動角；步驟5 :利用步驟4所獲取到的俯仰角、滾動角和方向角來計算 的旋轉(zhuǎn)矩陣為校正后的移動設(shè)備姿態(tài)旋轉(zhuǎn)矩陣。
[0010] 與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：充分利用陀螺儀在計算瞬時角速度值時 的準(zhǔn)確性，從而可獲取較準(zhǔn)確的瞬時設(shè)備變化姿態(tài)，同時，為了解決利用陀螺儀來計算姿態(tài) 時不可避免的由采樣頻率引起的積分誤差，利用加速度計來進行補償，從而獲取更為準(zhǔn)確 的移動設(shè)備的當(dāng)前姿態(tài)。
【具體實施方式】
[0011] 本發(fā)明的具體實施步驟如下： 第一步，結(jié)合移動設(shè)備初始姿態(tài)利用內(nèi)置陀螺儀積分計算移動設(shè)備當(dāng)前姿態(tài)。具體 步驟為：步驟1:利用陀螺儀獲取移動設(shè)備的旋轉(zhuǎn)角速度。記陀螺儀輸出的角速度向量為 values,采樣時間間隔為dT，則旋轉(zhuǎn)角度為dT*values，記為dTheta。步驟2 :利用旋轉(zhuǎn)角度 計算旋轉(zhuǎn)向量。將dTheta轉(zhuǎn)換為四元數(shù)，求得旋轉(zhuǎn)向量rotV。步驟3:通過積分來計算移 動設(shè)備當(dāng)前姿態(tài)及旋轉(zhuǎn)矩陣。記前次移動設(shè)備姿態(tài)對應(yīng)的旋轉(zhuǎn)矩陣為R[n]，當(dāng)前姿態(tài)矩陣 為R[n+1]，則R[n+1] =R[n] *rotV。
[0012] 第二步，利用內(nèi)置加速度計和陀螺儀來產(chǎn)生需要校正的判斷條件。具體的判斷條 件為：條件1 :利用陀螺儀所計算出的旋轉(zhuǎn)角速度向量模小于0. 0005,即（|rotV|=sqrt(rot V[0] *rotV[0] +rotV[l] *rotV[l] +rotV[2] *rotV[2]))〈0? 0005 ;條件 2 :利用陀螺儀所計算 出的當(dāng)前姿態(tài)狀態(tài)為俯仰角大于75度且小于89度。當(dāng)條件1和條件2都滿足時，進入第 三步，否則，直接輸出移動設(shè)備姿態(tài)。
[0013]第三步，如果達(dá)到第二步所述的判斷條件，則利用加速度計來 進行姿態(tài)校正，具體步驟為：步驟1:利用低通濾波器對內(nèi)置加速度計 輸出進行處理以獲取重力向量。低通濾波器設(shè)計如下：記重力向量為 gllc:: _ :齡夏，alpha為常量，取為0? 75,加速度計直接輸出向量為values，則 濾波器為：gravity=alpha*gravity+(l_alpha)*values。步驟2:利用重力向量計算移動設(shè) 備的俯仰角和滾動角。記方向角為轉(zhuǎn)俯仰角，滾動角為 ，重力向量為gravity=|_ %jSs|F，則俯仰角%，滾動角^療^ 。步驟3 :利用第一步所獲取到的旋轉(zhuǎn)矩陣求出俯仰角、滾動角、方向角；記旋轉(zhuǎn) 矩陣
，則俯仰角__:為_「:養(yǎng)iii，滾動角_!為__1(|[|】，方向 角
步驟4 :利用步驟2獲取到的俯仰角和滾動角去替代步驟3獲取 到的俯仰角和滾動角，獲取當(dāng)前姿態(tài)新的歐拉角分別為：俯仰角li_H_為纖，滾 動角額為雙，方向角細(xì)保持不變，為麵蠢_。步驟5 :利用步驟4所獲 取到的俯仰角、滾動角和方向角來計算的旋轉(zhuǎn)矩陣為校正后的移動設(shè)備姿態(tài)旋轉(zhuǎn)矩陣。設(shè) 方向角為麵，俯仰角為|，滾動角為f，即賴i 瞧今痛 陣戈
該旋轉(zhuǎn)矩 陣即為校正后的移動設(shè)備姿態(tài)旋轉(zhuǎn)矩陣。
【主權(quán)項】
1. 一種綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，其特征在于：第一步， 結(jié)合移動設(shè)備初始姿態(tài)利用內(nèi)置陀螺儀積分計算移動設(shè)備當(dāng)前姿態(tài)；第二步，利用內(nèi)置加 速度計和陀螺儀來產(chǎn)生需要校正的判斷條件；第三步，如果達(dá)到所需的判斷條件，則利用加 速度計來進行姿態(tài)校正。2. 如權(quán)利要求1所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，所述的 結(jié)合移動設(shè)備初始姿態(tài)利用內(nèi)置陀螺儀積分計算移動設(shè)備當(dāng)前姿態(tài)，其特征在于：首先，利 用陀螺儀獲取移動設(shè)備的旋轉(zhuǎn)角速度；然后，利用旋轉(zhuǎn)角速度計算旋轉(zhuǎn)向量；最后，通過積 分來計算移動設(shè)備當(dāng)前姿態(tài)及旋轉(zhuǎn)矩陣。3. 如權(quán)利要求1所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，所述的 利用內(nèi)置加速度計和陀螺儀來產(chǎn)生需要校正的判斷條件，其特征在于：校正判斷條件1為 利用陀螺儀所計算出的旋轉(zhuǎn)角速度向量模小于〇. 0005 ;校正判斷條件2為利用陀螺儀所計 算出的當(dāng)前姿態(tài)狀態(tài)為俯仰角大于75度且小于89度。4. 如權(quán)利要求1所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，所述的 利用加速度計來進行姿態(tài)校正，其特征在于：具體校正步驟為：步驟1 :利用低通濾波器對 內(nèi)置加速度計輸出進行處理以獲取重力向量；步驟2 :利用重力向量計算移動設(shè)備的俯仰 角和滾動角；步驟3 :利用第一步中所獲取到的旋轉(zhuǎn)矩陣求出俯仰角、滾動角、方向角；步驟 4 :利用校正步驟2獲取到的俯仰角和滾動角去替代步驟3獲取到的俯仰角和滾動角；步驟 5 :利用步驟4所獲取到的俯仰角、滾動角和方向角來計算的旋轉(zhuǎn)矩陣為校正后的移動設(shè)備 姿態(tài)旋轉(zhuǎn)矩陣。5. 如權(quán)利要求2所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，所述 的利用陀螺儀獲取移動設(shè)備的旋轉(zhuǎn)角速度，其特征在于：記陀螺儀輸出的角速度向量為 values,米樣時間間隔為dT，貝U旋轉(zhuǎn)角速度為dT*values，記為dTheta。6. 如權(quán)利要求2所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，所述的 利用旋轉(zhuǎn)角速度計算旋轉(zhuǎn)向量，其特征在于：將dTheta轉(zhuǎn)換為四元數(shù)，求得旋轉(zhuǎn)向量rotV。7. 如權(quán)利要求2所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，所述的 通過積分來計算移動設(shè)備當(dāng)前姿態(tài)及旋轉(zhuǎn)矩陣，其特征在于：記前次移動設(shè)備姿態(tài)對應(yīng)的 旋轉(zhuǎn)矩陣為R[n]，當(dāng)前姿態(tài)矩陣為R[n+1]，則R[n+l]=R[n]*rotV。8. 如權(quán)利要求3所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，所述的 利用陀螺儀所計算出的旋轉(zhuǎn)角速度向量模小于〇. 0005,其特征在于：其具體實現(xiàn)方法為： (|rotV|=sqrt(rotV[0]*rotV[0]+rotV[l]*rotV[l]+rotV[2]*rotV[2]))<0. 0005〇9. 如權(quán)利要求4所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，所述的 利用低通濾波器對內(nèi)置加速度計輸出進行處理以獲取重力向量，其特征在于：記重力向量 為氣隨::編|?，alpha為常量，取為0? 75,加速度計直接輸出向量為values,則 濾波器為：gravity=alpha氺gravity+ (l_alpha)氺values。10. 如權(quán)利要求4所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，所述 的利用重力向量計算移動設(shè)備的俯仰角和滾動角，其特征在于：記方向角為- 俯仰角為__￥_@，滾動角為重力向量為gravity: !& %兔f::，則俯仰角11. 如權(quán)利要求4所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法，所 述的利用第一步所獲取到的旋轉(zhuǎn)矩陣求出俯仰角、滾動角、方向角，其特征在于：記旋轉(zhuǎn)矩方向角12. 如權(quán)利要求4所述的綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài) 的方法，所述的利用步驟4所獲取到的俯仰角、滾動角和方向角來計算的旋轉(zhuǎn) 矩陣為校正后的移動設(shè)備姿態(tài)旋轉(zhuǎn)矩陣，其特征在于：設(shè)方向角為，俯仰角 為|，滾動角為f，即I旋轉(zhuǎn)矩陣為該旋轉(zhuǎn)矩陣即 為校正后的移動設(shè)備姿態(tài)旋轉(zhuǎn)矩陣。
【專利摘要】本發(fā)明公開一種移動設(shè)備姿態(tài)的校正方法，其特征是綜合考慮移動設(shè)備上陀螺儀和加速度計的特點，提出一種綜合利用加速度計和陀螺儀校準(zhǔn)移動設(shè)備姿態(tài)的方法。該方法在利用移動設(shè)備上陀螺儀進行設(shè)備姿態(tài)判斷的基礎(chǔ)之上，利用加速度計來進行校準(zhǔn)，從而提供更為準(zhǔn)確的移動設(shè)備當(dāng)前姿態(tài)判斷，為諸如增強現(xiàn)實、感應(yīng)游戲等應(yīng)用提供更好的用戶體驗。
【IPC分類】G01C21/16
【公開號】CN105021189
【申請?zhí)枴緾N201410168399
【發(fā)明人】張弢, 高民, 王周紅, 倪明選
【申請人】廣州市香港科大霍英東研究院
【公開日】2015年11月4日
【申請日】2014年4月25日