一種采用加速收斂算法的定位方法及裝置制造方法
【專利摘要】本發(fā)明提出一種采用加速收斂算法的定位方法，包括以下步驟：獲取加速度和地磁力的初始三維數(shù)值，計算出姿態(tài)四元數(shù)的初始值QA；獲取角速度的三維數(shù)值，計算出姿態(tài)四元數(shù)的預(yù)測值QB=QA+dQ/dt；獲取加速度和地磁力的當(dāng)前三維數(shù)值，計算出姿態(tài)參數(shù)，所述姿態(tài)參數(shù)為羅德里格參數(shù)、方向余弦和歐拉角中的至少一種；通過相關(guān)的計算方法將計算得出的姿態(tài)參數(shù)轉(zhuǎn)換為姿態(tài)四元數(shù)的觀測值QD；對QD和QB進行融合計算，得到姿態(tài)四元數(shù)的融合值QE；再次獲取角速度的三維數(shù)值，計算四元數(shù)的下一預(yù)測值QB1=QE+dQ/dt；重復(fù)以上相關(guān)步驟，獲取四元數(shù)預(yù)測值QBn。本發(fā)明采用了簡單的運算方法得到定位結(jié)果，因此解決了由于運算問題產(chǎn)生的明顯遲滯現(xiàn)象，能夠?qū)崿F(xiàn)快速收斂。
【專利說明】一種采用加速收斂算法的定位方法及裝置
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種定位方法，尤其是一種采用加速收斂算法的定位方法及裝置。
【背景技術(shù)】
[0002]目前多數(shù)MEMS(微機電系統(tǒng))定位算法多數(shù)采用卡爾曼濾波或者擴展卡爾曼濾波算法，由于卡爾曼濾波算法運用高斯牛頓法，計算復(fù)雜，運算時間長。而擴展卡爾曼濾波收斂速度慢，在傳感器慢速運動時輸出比較準(zhǔn)確，但是當(dāng)快速運動時，收斂速度慢將導(dǎo)致計算值輸出完全跟不上運動變化，出現(xiàn)計算值與實際值偏差太大，在空中鼠標(biāo)的實際測試中反映的現(xiàn)象就是光標(biāo)跑飛，并且需要很長時間光標(biāo)才會回到實際位置的現(xiàn)象，因此這兩種算法沒辦法用于快速運動的定位。
【發(fā)明內(nèi)容】
[0003]針對以上現(xiàn)有技術(shù)的缺陷，本發(fā)明利用三維加速度傳感器、陀螺儀、地磁儀來感測運動數(shù)據(jù)，并通過一種定位算法來加速收斂數(shù)據(jù)，實現(xiàn)快速精確定位。簡單地說，本發(fā)明通過感測陀螺儀數(shù)據(jù)并計算得到四元數(shù)的預(yù)測值，再感測得到加速度傳感器和地磁傳感器數(shù)據(jù)并計算得到四元數(shù)的觀測值，將兩種數(shù)據(jù)融合得到最后定位數(shù)據(jù)，由于采用了簡單的運算方法得到定位結(jié)果，因此解決了由于運算問題產(chǎn)生的明顯遲滯現(xiàn)象，實現(xiàn)快速收斂。
[0004]具體地說，本發(fā)明所提出的定位方法包括以下步驟:
[0005]I)獲取加速度和地磁力的初始三維數(shù)值，計算出姿態(tài)四元數(shù)的初始值Qa ;
[0006]2)獲取角速度的三維數(shù)值，計算出姿態(tài)四元數(shù)的預(yù)測值QB=QA+dQ/dt，其中Q為三維角速度的四元數(shù)；
[0007]3)獲取加速度和地磁力的當(dāng)前三維數(shù)值，計算出姿態(tài)參數(shù)，如羅德里格參數(shù)、方向余弦或者歐拉角等；
[0008]4)將計算得出的姿態(tài)參數(shù)轉(zhuǎn)換為姿態(tài)四元數(shù)的觀測值Qd ；
[0009]5)對Qd和Qb進行融合計算，得到姿態(tài)四元數(shù)的融合值Qe ；
[0010]6)再次獲取角速度的三維數(shù)值，計算四元數(shù)的下一預(yù)測值QB1=QE+dQ/dt ；
[0011]7)重復(fù)步驟3)至6)，獲取四元數(shù)預(yù)測值QBn。
[0012]作為以上技術(shù)方案的一種補充，所有計算出的四元數(shù)均以基準(zhǔn)姿態(tài)為參考，其中以水平面向正北的姿態(tài)為基準(zhǔn)姿態(tài)，在所述基準(zhǔn)姿態(tài)上三維加速度的數(shù)值為g(0，0，最大)，三維磁力的數(shù)值為M (最大，0，不管)，基準(zhǔn)姿態(tài)四元數(shù)為Qtl (1，0，0，0)。
[0013]作為以上技術(shù)方案的一種補充，在步驟3)中，通過比對不同軸的加速度數(shù)值和磁力數(shù)值計算出運動的空間姿態(tài)參數(shù)。
[0014]作為以上技術(shù)方案的一種補充，當(dāng)姿態(tài)參數(shù)顯示，俯仰角的絕對值I Θ I或Sin Θ I或Tan I Θ |小于預(yù)設(shè)值時，執(zhí)行步驟4)的處理；否則對加速度和磁力的三維數(shù)值進行往使往I Θ I變小的方向旋轉(zhuǎn)90°的矩陣運算，再進行姿態(tài)參數(shù)的計算，之后執(zhí)行步驟4)的運算得到姿態(tài)四元數(shù)后再進行轉(zhuǎn)回原角度Θ的90°旋轉(zhuǎn)運算得到姿態(tài)四元數(shù)的觀測值qd。
[0015]作為以上技術(shù)方案的一種補充，I Θ I的預(yù)設(shè)值為70°或者80°或其他接近90°的值。
[0016]同理，Sin| Θ I或Tan I Θ |中的| Θ |為70。或者80?；蚱渌咏?0。的值。
[0017]作為以上技術(shù)方案的一種補充，在步驟4)中，選取因旋轉(zhuǎn)而產(chǎn)生的多個對應(yīng)的四元數(shù)值中合適的四元數(shù)值作為觀測值qd。
[0018]作為以上技術(shù)方案的一種補充，在步驟5)中，Qd和Qb融合的方法為KF、EKF、UKF、加權(quán)平均、最小二乘和有權(quán)值的最小二乘中的一種或者多種。
[0019]本發(fā)明還提出了一種采用以上所述加速收斂算法的定位裝置，該定位裝置包括感測單元、運算單元以及輸出單元，其中所述感測單元包括三維陀螺儀、三維加速度傳感器和三維地磁傳感器，所述感測單元感測數(shù)據(jù)并傳送給運算單元，運算單元計算出融合后的四元數(shù)以得到裝置的姿態(tài)數(shù)據(jù)，所述輸出單元將計算得出的姿態(tài)數(shù)據(jù)發(fā)送給外部設(shè)備進行相關(guān)控制，其中所述運算單元執(zhí)行以下步驟:
[0020]I)從三維加速度傳感器和三維地磁傳感器中獲取加速度和地磁力的初始三維數(shù)值，計算出姿態(tài)四元數(shù)的初始值Qa ；
[0021]2)從三維陀螺儀中獲取角速度的三維數(shù)值，計算出姿態(tài)四元數(shù)的預(yù)測值％=(^+(10/dt，其中Q為三維角速度的四元數(shù)；
[0022]3)從三維加速度傳感器和三維地磁傳感器中獲取加速度和地磁力的當(dāng)前三維數(shù)值，計算出姿態(tài)參數(shù)，如羅德里格參數(shù)、方向余弦或者歐拉角等。
[0023]4)將計算得出的姿態(tài)參數(shù)轉(zhuǎn)換為合適的姿態(tài)四元數(shù)的觀測值Qd ；
[0024]5)對Qd和Qb進行融合計算，得到姿態(tài)四元數(shù)的融合值Qe ;融合的方法為KF、EKF、UKF、加權(quán)平均、最小二乘和有權(quán)值的最小二乘中的一種或者多種。
[0025]6)再次從三維陀螺儀中獲取角速度的三維數(shù)值，計算四元數(shù)的下一預(yù)測值QB1=QE+dQ/dt ；
[0026]7)重復(fù)步驟3)至6)，獲取四元數(shù)預(yù)測值QBn。
[0027]作為以上技術(shù)方案的一種補充，所有計算出的四元數(shù)均以基準(zhǔn)姿態(tài)為參考，其中以水平面向正北的姿態(tài)為基準(zhǔn)姿態(tài)，在所述基準(zhǔn)姿態(tài)上三維加速度的數(shù)值為g(0，0，最大)，三維磁力的數(shù)值為M (最大，0，MZ)，基準(zhǔn)姿態(tài)四元數(shù)為Qtl (1，0，0，0)。
[0028]作為以上技術(shù)方案的一種補充，在步驟3)中，通過比對不同軸的加速度計值，和磁力數(shù)值，得到相關(guān)姿態(tài)參數(shù)，如羅德里格參數(shù)、方向余弦或者歐拉角等。
[0029]作為以上技術(shù)方案的一種補充，當(dāng)姿態(tài)參數(shù)顯示，俯仰角的絕對值I Θ I或Sin Θ I或Tan I Θ |小于預(yù)設(shè)值時，執(zhí)行步驟4)的處理；否則對加速度和磁力的三維數(shù)值進行往使往I Θ I變小的方向旋轉(zhuǎn)90°的矩陣運算，再進行姿態(tài)參數(shù)歐拉角C的計算，之后執(zhí)行步驟4)的運算得到姿態(tài)四元數(shù)后再進行轉(zhuǎn)回原角度Θ的90°旋轉(zhuǎn)運算得到姿態(tài)四元數(shù)的觀測值QD。
[0030]作為以上技術(shù)方案的一種補充，I Θ I的預(yù)設(shè)值為70°或者80°。
[0031]同理，Sin| Θ I或Tan I Θ |中的| Θ |為70?；蛘?0?；蚱渌咏?0。的值。
[0032]作為以上技術(shù)方案的一種補充，在步驟4)中，選取因旋轉(zhuǎn)而產(chǎn)生的多個對應(yīng)的四元數(shù)值中合適的 四元數(shù)值，作為觀測值qd。[0033]作為以上技術(shù)方案的一種補充，在步驟5)中，Qd和Qb融合的方法為KF、EKF、UKF、加權(quán)平均、最小二乘和有權(quán)值的最小二乘中的一種或者多種。
[0034]利用得到的姿態(tài)數(shù)據(jù)可以進行如下應(yīng)用:
[0035]一方面通過將姿態(tài)數(shù)據(jù)發(fā)送到外部進行的控制，例如:
[0036]俯仰角和航向角，經(jīng)滾動角轉(zhuǎn)動后可以轉(zhuǎn)化為二維空中鼠標(biāo)所需要的光標(biāo)控制信號，實現(xiàn)空中鼠標(biāo)功能；
[0037]通過姿態(tài)的定義及變化數(shù)據(jù)檢測，實現(xiàn)姿態(tài)對連續(xù)量的控制，如音量調(diào)節(jié)、電視頻道改變、亮度改變、對比度改變、畫面移動及瀏覽、選曲、碟片的快進、快退、網(wǎng)頁的上下移動；
[0038]實現(xiàn)裝置與三維圖形的聯(lián)動控制，通過外部旋轉(zhuǎn)裝置來直接控制三維圖形的旋轉(zhuǎn)觀察；
[0039]實現(xiàn)對三維游戲的控制；
[0040]另一方面可以直接對裝置內(nèi)部的三維姿態(tài)控制，比如:
[0041]對裝置內(nèi)部的三維畫面、游戲進行直接的姿態(tài)控制。
【專利附圖】
【附圖說明】
[0042]以下結(jié)合附圖和實施例對本發(fā)明作進一步說明，其中:
[0043]圖1為本發(fā)明示例用的空間軸向分布圖；
[0044]圖2為航向角、俯仰角和滾動角的示意圖；
[0045]圖3為本發(fā)明的方法流程圖；
[0046]圖4為本發(fā)明的裝置方框圖。
【具體實施方式】
[0047]參見圖3所示的方法流程圖，設(shè)備上電時，系統(tǒng)獲取加速度和地磁力的初始三維數(shù)值，即讀入初始姿態(tài)下的三維加速度傳感器的值g (X，1，Z)以及三維地磁傳感器的值M(X, y, z),如圖1所示,其中x=A, y=B, z=C,各代表三個軸(注意,本發(fā)明不拘泥于空間中X,Y，Z軸的具體指向，也不受左手坐標(biāo)系或右手坐標(biāo)系的限制，為方便描述下文采用圖1所示的空間軸向分布作為示例，其中A軸方向為應(yīng)用本發(fā)明的感測裝置的前方)。
[0048]首先計算初始姿態(tài)的四元數(shù)，所有計算出的四元數(shù)都是以基準(zhǔn)姿態(tài)為參考，基準(zhǔn)姿態(tài)是將傳感器所構(gòu)成的模塊面向正北極水平放置狀態(tài)，此時三維加速度值為g (0，0，最大)，三維地磁傳感器的值為M (最大，0，MZ)。
[0049]利用基準(zhǔn)姿態(tài)值計算出歐拉角，并轉(zhuǎn)換為四元數(shù)，得到基準(zhǔn)四元數(shù)值Qtl (1,0,0,O)。
[0050]以基準(zhǔn)姿態(tài)為原始基準(zhǔn)，根據(jù)初始姿態(tài)的三維加速度傳感器以及三維地磁傳感器的值計算得到初始姿態(tài)四元數(shù)Qa。
[0051]讀取三維陀螺儀數(shù)據(jù)，得到三軸角速度(ωχ、coy、ωζ)，并以此計算三維陀螺儀的四元數(shù)Q，再通過對Q的求導(dǎo)，再與Qa累積作為四元素的預(yù)測值Qb，即QB=QA+dQ/dt。
[0052]將以下用羅德里格參數(shù)歐拉角作為姿態(tài)參數(shù)描述本發(fā)明的一個實施例。
[0053]以羅德里格參數(shù)表示姿態(tài)，羅德里格參數(shù)按照如下定義:[0054]
【權(quán)利要求】
1.一種采用加速收斂算法的定位方法，其特征在于，包括以下步驟: O獲取加速度和地磁力的初始三維數(shù)值，計算出姿態(tài)四元數(shù)的初始值Qa ； 2)獲取角速度的三維數(shù)值，計算出姿態(tài)四元數(shù)的預(yù)測值QB=QA+dQ/dt，其中Q為三維角速度的四元數(shù)； 3)獲取加速度和地磁力的當(dāng)前三維數(shù)值，計算出姿態(tài)參數(shù)，所述姿態(tài)參數(shù)為羅德里格參數(shù)、方向余弦和歐拉角中的至少一種； 4)將計算得出的姿態(tài)參數(shù)轉(zhuǎn)換為姿態(tài)四元數(shù)的觀測值Qd； 5)對Qd和Qb進行融合計算，得到姿態(tài)四元數(shù)的融合值Qe； 6)再次獲取角速度的三維數(shù)值，計算四元數(shù)的下一預(yù)測值QB1=QE+dQ/dt； 7)重復(fù)步驟3)至6)，獲取四元數(shù)預(yù)測值QBn。
2.根據(jù)權(quán)利要求1所述的采用加速收斂算法的定位方法，其特征在于，所有計算出的四元數(shù)均以基準(zhǔn)姿態(tài)為參考，其中以水平面向正北的姿態(tài)為基準(zhǔn)姿態(tài)，在所述基準(zhǔn)姿態(tài)上三維加速度的數(shù)值為g (0，0，最大)，三維磁力的數(shù)值為M (最大，O，不管)，基準(zhǔn)姿態(tài)四元數(shù)為 Q0 (1，0，0，0)。
3.根據(jù)權(quán)利要求1所述的采用加速收斂算法的定位方法，其特征在于，在步驟3)中，通過比對不同軸的加速度計值和磁力數(shù)值，計算得到姿態(tài)參數(shù)。
4.根據(jù)權(quán)利要求3所述的采用加速收斂算法的定位方法，其特征在于，當(dāng)姿態(tài)參數(shù)中俯仰角的絕對值I Θ I或Sin I Θ I或Tan I Θ |小于預(yù)設(shè)值時，執(zhí)行步驟4)的處理；否則對加速度和磁力的三維數(shù)值進行往使往I Θ I或Sin I Θ I或Tan I Θ |變小的方向旋轉(zhuǎn)90°的矩陣運算，再進行歐拉角C的計算，之后執(zhí)行步驟4)的運算得到姿態(tài)四元數(shù)后再進行轉(zhuǎn)回原角度Θ的90°旋轉(zhuǎn)運算得到姿態(tài)四元數(shù)的觀測值Qd。
5.根據(jù)權(quán)利要求5所述的采用加速收斂算法的定位方法，其特征在于，IΘ I的預(yù)設(shè)值為70°或者80°。
6.根據(jù)權(quán)利要求1所述的采用加速收斂算法的定位方法，其特征在于，在步驟4)中，選取因旋轉(zhuǎn)姿態(tài)參數(shù)而產(chǎn)生的多個對應(yīng)的四元數(shù)值中合適的值作為觀測值Qd。
7.根據(jù)權(quán)利要求1所述的采用加速收斂算法的定位方法，其特征在于，在步驟5)中，Qd和Qb融合的方法為KF、EKF、UKF、加權(quán)平均、最小二乘和有權(quán)值的最小二乘中的一種或者多種。
8.一種采用加速收斂算法的定位裝置，其特征在于，包括感測單元、運算單元以及輸出單元，其中所述感測單元包括三維陀螺儀、三維加速度傳感器和三維地磁傳感器，所述感測單元感測數(shù)據(jù)并傳送給運算單元，運算單元計算出融合后的四元數(shù)以得到裝置的姿態(tài)數(shù)據(jù)，所述輸出單元將計算得出的姿態(tài)數(shù)據(jù)用于自身控制和/或發(fā)送給外部設(shè)備進行相關(guān)控制。
【文檔編號】G01C23/00GK103940442SQ201410134235
【公開日】2014年7月23日 申請日期:2014年4月3日 優(yōu)先權(quán)日:2014年4月3日
【發(fā)明者】紀(jì)家瑋, 關(guān)文清 申請人:深圳市宇恒互動科技開發(fā)有限公司