[發明專利]一種采用加速收斂算法的定位方法及裝置

有效

• 申請號: 201410134235.X
• 申請日: 2014-04-03
• 公開（公告）號: CN103940442B
• 公開（公告）日: 2018-02-27
• 發明（設計）人: 紀家瑋;關文清
• 申請（專利權）人: 深圳市宇恒互動科技開發有限公司
• 主分類號: G01C23/00
• 分類號: G01C23/00
• 代理公司: 廣州嘉權專利商標事務所有限公司44205
• 代理人: 譚英強

技術領域

本發明涉及一種定位方法，尤其是一種采用加速收斂算法的定位方法及裝置。

背景技術

目前多數MEMS（微機電系統）定位算法多數采用卡爾曼濾波或者擴展卡爾曼濾波算法，由于卡爾曼濾波算法運用高斯牛頓法，計算復雜，運算時間長。而擴展卡爾曼濾波收斂速度慢，在傳感器慢速運動時輸出比較準確，但是當快速運動時，收斂速度慢將導致計算值輸出完全跟不上運動變化，出現計算值與實際值偏差太大，在空中鼠標的實際測試中反映的現象就是光標跑飛，并且需要很長時間光標才會回到實際位置的現象，因此這兩種算法沒辦法用于快速運動的定位。

發明內容

針對以上現有技術的缺陷，本發明利用三維加速度傳感器、陀螺儀、地磁儀來感測運動數據，并通過一種定位算法來加速收斂數據，實現快速精確定位。簡單地說，本發明通過感測陀螺儀數據并計算得到四元數的預測值，再感測得到加速度傳感器和地磁傳感器數據并計算得到四元數的觀測值，將兩種數據融合得到最后定位數據，由于采用了簡單的運算方法得到定位結果，因此解決了由于運算問題產生的明顯遲滯現象，實現快速收斂。

具體地說，本發明所提出的定位方法包括以下步驟:

1）獲取加速度和地磁力的初始三維數值，計算出姿態四元數的初始值Q_A；

2）獲取角速度的三維數值，計算出姿態四元數的預測值Q_B=Q_A+dQ/dt，其中Q為三維角速度的四元數；

3）獲取加速度和地磁力的當前三維數值，計算出姿態參數，如羅德里格參數、方向余弦或者歐拉角等；

4）將計算得出的姿態參數轉換為姿態四元數的觀測值Q_D；

5）對Q_D和Q_B進行融合計算，得到姿態四元數的融合值Q_E；

6）再次獲取角速度的三維數值，計算四元數的下一預測值Q_B1=Q_E+dQ/dt；

7）重復步驟3）至6），獲取四元數預測值Q_Bn。

作為以上技術方案的一種補充，所有計算出的四元數均以基準姿態為參考，其中以水平面向正北的姿態為基準姿態，在所述基準姿態上三維加速度的數值為g（0，0，最大），三維磁力的數值為M（最大，0，不管），基準姿態四元數為Q₀（1，0，0，0）。

作為以上技術方案的一種補充，在步驟3）中，通過比對不同軸的加速度數值和磁力數值計算出運動的空間姿態參數。

作為以上技術方案的一種補充，當姿態參數顯示，俯仰角的絕對值|θ|或Sin|θ|或Tan|θ|小于預設值時，執行步驟4）的處理；否則對加速度和磁力的三維數值進行往使往|θ|變小的方向旋轉90°的矩陣運算，再進行姿態參數的計算，之后執行步驟4）的運算得到姿態四元數后再進行轉回原角度θ的90°旋轉運算得到姿態四元數的觀測值Q_D。

作為以上技術方案的一種補充，|θ|的預設值為70°或者80°或其他接近90°的值。

同理，Sin|θ|或Tan|θ|中的|θ|為70°或者80°或其他接近90°的值。

作為以上技術方案的一種補充，在步驟4）中，選取因旋轉而產生的多個對應的四元數值中合適的四元數值作為觀測值Q_D。

作為以上技術方案的一種補充，在步驟5）中，Q_D和Q_B融合的方法為KF、EKF、UKF、加權平均、最小二乘和有權值的最小二乘中的一種或者多種。

本發明還提出了一種采用以上所述加速收斂算法的定位裝置，該定位裝置包括感測單元、運算單元以及輸出單元，其中所述感測單元包括三維陀螺儀、三維加速度傳感器和三維地磁傳感器，所述感測單元感測數據并傳送給運算單元，運算單元計算出融合后的四元數以得到裝置的姿態數據，所述輸出單元將計算得出的姿態數據發送給外部設備進行相關控制，其中所述運算單元執行以下步驟：

1）從三維加速度傳感器和三維地磁傳感器中獲取加速度和地磁力的初始三維數值，計算出姿態四元數的初始值Q_A；

2）從三維陀螺儀中獲取角速度的三維數值，計算出姿態四元數的預測值Q_B=Q_A+dQ/dt，其中Q為三維角速度的四元數；