專利名稱：測(cè)量物體位置和方位的方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種測(cè)量物體方位的方法，所述物體在人體控制下運(yùn)動(dòng)，例如游戲手柄、遙控器、鼠標(biāo)等。
背景技術(shù)：
物體的位置，是指物體在特定的坐標(biāo)系中相對(duì)于給定參考點(diǎn)(通常是坐標(biāo)系的原點(diǎn))的線性位移。在數(shù)學(xué)上，描述位置就等價(jià)于描述線性位移。在概念上，位置是指單一狀態(tài)，而線性位移是指兩個(gè)位置間的差別。物體的方位(orientation)，也稱為物體的姿態(tài)(attitude)，描述的是物體的朝向。物體的方位是通過(guò)與已知方位(通常稱為源方位)的旋轉(zhuǎn)來(lái)描述的，旋轉(zhuǎn)的量稱作角位移。在數(shù)學(xué)上，描述方位就等價(jià)于描述角位移。在概念上，方位是指單一狀態(tài)，而角位移是指兩個(gè)狀態(tài)間的差別。描述物體的方位或角位移具有多種表示方式，通常用歐拉角(Euler angles)的形式來(lái)表示方位，用四元數(shù)(quaternion)或旋轉(zhuǎn)矩陣(rotation matrix)的形式來(lái)表示角位移。這幾種表示方式之間都是可以互相轉(zhuǎn)換的。更詳細(xì)的信息可參考《3D數(shù)學(xué)基礎(chǔ)圖形與游戲開(kāi)發(fā)》(Fletcher Dunn, Ian Parberry著，清華大學(xué)出版社，2005年8月出版)歐拉角具有多種定義方式，本發(fā)明中示例性地采用泰特-布萊恩角(Tait-Bryan angles)，又稱為導(dǎo)航角(navigation angles)、卡丹角(Cardan angles)，這是一種最常見(jiàn)的歐拉角形式。泰特-布萊恩角包括偏航角(yaw)、俯仰角(pitch)和滾動(dòng)角(roll)。其中，偏航角以Ψ表示，取值范圍是-180 180度；俯仰角以f表示，取值范圍是-90 90 度；滾動(dòng)角以θ表示，取值范圍是-180 180度。隨著微機(jī)電系統(tǒng)(Micro Electromechanical System, MEMS)技術(shù)的不斷發(fā)展，基于這種技術(shù)的傳感器在消費(fèi)電子產(chǎn)品中也得到了廣泛地應(yīng)用。例如，陀螺儀應(yīng)用于數(shù)碼相機(jī)中做防抖動(dòng)處理；加速度傳感器集成于筆記本電腦內(nèi)部實(shí)現(xiàn)防跌落硬盤保護(hù)；磁傳感器應(yīng)用于手機(jī)中實(shí)現(xiàn)指南針的功能等等。陀螺儀(gyroscope)是一種常用的方位測(cè)量裝置，陀螺儀輸出角速度，是瞬時(shí)量。 通常將角速度與時(shí)間做積分運(yùn)算得到角度變化量(角位移)，將該角位移與初始方位相加， 得到目標(biāo)方位。其中積分時(shí)間越小，計(jì)算的角位移越準(zhǔn)確。但陀螺儀的測(cè)量基準(zhǔn)是自身，并沒(méi)有系統(tǒng)外的絕對(duì)參照物，加上積分時(shí)間不可能無(wú)限小，因此積分運(yùn)算的累積誤差會(huì)隨著時(shí)間流逝迅速增加，最終導(dǎo)致輸出角度與實(shí)際角度不符。陀螺儀具有自校準(zhǔn)功能，當(dāng)陀螺儀在使用過(guò)程中處于靜止?fàn)顟B(tài)并維持一段時(shí)間后，其會(huì)對(duì)中心點(diǎn)進(jìn)行自校準(zhǔn)。加速度傳感器(accelerometer)是一種常用的位置測(cè)量裝置，加速度傳感器輸出加速度，也是瞬時(shí)量。通常將加速度與時(shí)間做兩次積分運(yùn)算得到位置變化量(線性位移)， 將該線性位移與初始位置相加，得到目標(biāo)位置。加速度傳感器測(cè)量得到的加速度信號(hào)是重力加速度分量、向心加速度分量和線性加速度分量的總和。地磁傳感器(geomagnetic sensor)是一種常用的方向測(cè)量裝置。地磁傳感器在使用過(guò)程中只有在確保自身方位正確的前提下，才能測(cè)量出準(zhǔn)確的地磁信號(hào)。地磁傳感器也具有自校準(zhǔn)功能，當(dāng)?shù)卮艂鞲衅髟谑褂眠^(guò)程中采集到足夠多的特征點(diǎn)(運(yùn)行角度較多)， 其將會(huì)進(jìn)行自校準(zhǔn)。特征點(diǎn)數(shù)量是否足夠多，由地磁傳感器內(nèi)置的自校準(zhǔn)算法進(jìn)行判斷。

發(fā)明內(nèi)容
本發(fā)明所要解決的技術(shù)問(wèn)題是提供一種測(cè)量物體位置和方位的方法，該方法可以減小使用陀螺儀測(cè)量物體方位時(shí)的累計(jì)誤差問(wèn)題。為解決上述技術(shù)問(wèn)題，本發(fā)明測(cè)量物體位置和方位的方法，所述物體上包括三軸陀螺儀、三軸加速度傳感器和三軸地磁傳感器，所述方法為其中，測(cè)量物體位置的方法是三軸加速度傳感器測(cè)量物體的加速度并輸出給處理器，處理器將該加速度中剔除掉重力加速度分量和向心加速度分量，得到線性加速度分量；處理器將該線性加速度分量與時(shí)間進(jìn)行兩次積分運(yùn)算得到物體在該時(shí)間內(nèi)的線性位移；處理器將物體的初始位置與該線性位移相加，得到物體的當(dāng)前位置；其中，測(cè)量物體方位的方法是三軸陀螺儀測(cè)量物體的角速度并輸出給處理器，處理器將該角速度與時(shí)間進(jìn)行積分運(yùn)算得到物體在該時(shí)間內(nèi)的角位移，處理器將物體的初始方位與該角位移相加稱為第一方位；處理器至少給出四元數(shù)形式和歐拉角形式表示的第一方位；當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)，三軸加速度傳感器測(cè)量得到的物體的加速度就是該物體的重力加速度，處理器根據(jù)此時(shí)的物體的加速度值計(jì)算物體的歐拉角形式的俯仰角和滾動(dòng)角；當(dāng)計(jì)算出的俯仰角的絕對(duì)值小于或等于80度時(shí)，將計(jì)算出的俯仰角、滾動(dòng)角以及上述第一方位的偏航角作為第二方位；當(dāng)計(jì)算出的俯仰角的絕對(duì)值大于80度時(shí)，將計(jì)算出的俯仰角、上述第一方位的滾動(dòng)角和偏航角作為第二方位；處理器給出四元數(shù)形式表示的第二方位；當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)、或者當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間在預(yù)設(shè)值之內(nèi)時(shí)，三軸地磁傳感器測(cè)量地磁信號(hào)并輸出給處理器，處理器根據(jù)該地磁信號(hào)計(jì)算物體的歐拉角形式的偏航角，并將俯仰角和滾動(dòng)角分別設(shè)為歐拉角形式的上述第一方位的俯仰角和滾動(dòng)角，作為第三方位；處理器給出四元數(shù)形式表示的第三方位；當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)，處理器計(jì)算第二方位與第一方位的差值作為補(bǔ)償量一， 還計(jì)算第三方位與第一方位的差值作為補(bǔ)償量二，處理器將第一方位與補(bǔ)償量一、補(bǔ)償量二相加作為物體的方位；當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間在預(yù)設(shè)值之內(nèi)，處理器計(jì)算第三方位與第一方位的差值作為補(bǔ)償量二，處理器將第一方位與補(bǔ)償量二相加作為物體的方位；當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間超出預(yù)設(shè)值，處理器將第一方位作為物體的方位。傳統(tǒng)的測(cè)量物體方位的方法僅使用三軸陀螺儀，這會(huì)產(chǎn)生累計(jì)誤差而導(dǎo)致長(zhǎng)時(shí)間測(cè)量的結(jié)果不準(zhǔn)確。本發(fā)明在使用三軸陀螺儀的基礎(chǔ)上，又采用三軸加速度傳感器和三軸地磁傳感器對(duì)三軸陀螺儀的方位測(cè)量結(jié)果進(jìn)行修正，從而可以獲得較為準(zhǔn)確的物體方位。


圖1是物體向心運(yùn)動(dòng)的示意圖；圖2是物體線性運(yùn)動(dòng)的示意圖；圖3是本發(fā)明所述測(cè)量物體位置和方位的方法的示意圖；圖4是靜止坐標(biāo)系和運(yùn)動(dòng)坐標(biāo)系的對(duì)比示意圖；圖5是本發(fā)明所述測(cè)量物體位置和方位的方法的一個(gè)實(shí)施例的流程圖。
具體實(shí)施例方式物體在三維空間中的任何運(yùn)動(dòng)，都可以分解為向心運(yùn)動(dòng)(即轉(zhuǎn)動(dòng))和線性運(yùn)動(dòng) (即移動(dòng))的組合。其中向心運(yùn)動(dòng)如圖1所示，向心運(yùn)動(dòng)改變物體的朝向，不改變物體的位置。線性運(yùn)動(dòng)如圖2所示，線性運(yùn)動(dòng)改變物體的位置，不改變物體的朝向。本發(fā)明測(cè)量物體位置和方位的方法，首先要求運(yùn)動(dòng)物體上具有三軸陀螺儀、三軸加速度傳感器和三軸地磁傳感器。對(duì)于物體位置的測(cè)量，本發(fā)明所述方法為無(wú)論物體處于靜止還是運(yùn)動(dòng)狀態(tài)，物體上的三軸加速度傳感器都會(huì)測(cè)量物體的加速度并輸出給處理器。處理器將該加速度中剔除掉重力加速度分量和向心加速度分量，得到線性加速度分量。處理器將該線性加速度分量與時(shí)間進(jìn)行兩次積分運(yùn)算得到物體在該時(shí)間內(nèi)的線性位移；處理器將物體的初始位置與該線性位移進(jìn)行矢量相加，得到物體的當(dāng)前位置。對(duì)于物體方位的測(cè)量，本發(fā)明所述方法為三軸陀螺儀測(cè)量物體的角速度并輸出給處理器，處理器將該角速度與時(shí)間進(jìn)行積分運(yùn)算得到物體在該時(shí)間內(nèi)的角位移，處理器將物體的初始方位與該角位移相加稱為第一方位；處理器至少給出四元數(shù)形式和歐拉角形式表示的第一方位；當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)，三軸加速度傳感器測(cè)量得到的物體的加速度就是該物體的重力加速度，處理器根據(jù)此時(shí)的物體的加速度值計(jì)算物體的歐拉角形式的俯仰角和滾動(dòng)角；當(dāng)計(jì)算出的俯仰角的絕對(duì)值小于或等于80度(即-80度 < 俯仰角<80度)時(shí)，將計(jì)算出的俯仰角、滾動(dòng)角以及上述第一方位的偏航角三者構(gòu)成第二方位；當(dāng)計(jì)算出的俯仰角的絕對(duì)值大于80度(即-180度<俯仰角< -80度或80度<俯仰角< 180度)時(shí)，將計(jì)算出的俯仰角、上述第一方位的滾動(dòng)角和偏航角三者構(gòu)成第二方位；處理器給出四元數(shù)形式表示的第二方位；當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)、或者當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間在預(yù)設(shè)值之內(nèi)時(shí)，三軸地磁傳感器測(cè)量地磁信號(hào)并輸出給處理器，處理器根據(jù)該地磁信號(hào)計(jì)算物體的歐拉角形式的偏航角，并將俯仰角和滾動(dòng)角分別設(shè)為歐拉角形式的上述第一方位的俯仰角和滾動(dòng)角，作為第三方位；處理器給出四元數(shù)形式表示的第三方位；當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)，處理器計(jì)算第二方位與第一方位的差值作為補(bǔ)償量一， 還計(jì)算第三方位與第一方位的差值作為補(bǔ)償量二，處理器將第一方位與補(bǔ)償量一、補(bǔ)償量二相加作為物體的方位；當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間在預(yù)設(shè)值之內(nèi)，處理器計(jì)算第三方位與第一方位的差值作為補(bǔ)償量二，處理器將第一方位與補(bǔ)償量二相加作為物體的方位；當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間超出預(yù)設(shè)值，處理器將第一方位作為物體的方位。
上述方位的加減運(yùn)算均以四元數(shù)的形式進(jìn)行，具體而言上述第二方位與第一方位的差值作為補(bǔ)償量一以四元數(shù)相減的形式進(jìn)行，第三方位與第一方位的差值作為補(bǔ)償量二以四元數(shù)相減的形式進(jìn)行，第一方位與補(bǔ)償量一、補(bǔ)償量二相加以四元數(shù)相加的形式進(jìn)行，第一方位與補(bǔ)償量二相加以四元數(shù)相加的形式進(jìn)行。以四元數(shù)的形式進(jìn)行方位的加減運(yùn)算可以避免以歐拉角形式進(jìn)行方位的加減運(yùn)算可能出現(xiàn)的“死角”問(wèn)題。請(qǐng)參閱圖3，在任意情況下，物體的方位首先由陀螺儀的測(cè)量結(jié)果初步確定，即第一方位。加速度傳感器僅在物體處于靜止?fàn)顟B(tài)時(shí)測(cè)量計(jì)算得到第二方位。三軸地磁傳感器在物體處于靜止?fàn)顟B(tài)時(shí)、或者在物體持續(xù)運(yùn)動(dòng)一定時(shí)間內(nèi)測(cè)量計(jì)算得到第三方位。當(dāng)?shù)诙轿?、第三方位存在時(shí)，則將第一方位通過(guò)補(bǔ)償?shù)姆绞较虻诙轿?、第三方位靠攏。其中，第一方位中的俯仰角、滾動(dòng)角向第二方位的俯仰角、滾動(dòng)角靠攏，第一方位中的偏航角向第三方位的偏航角靠攏。如圖3所示，加速度傳感器、地磁傳感器并非在所有情況下都可對(duì)陀螺儀的方位測(cè)量結(jié)果進(jìn)行補(bǔ)償。例如，加速度傳感器在物體處于運(yùn)動(dòng)狀態(tài)時(shí)，其無(wú)法根據(jù)重力加速度分量計(jì)算出物體當(dāng)前的歐拉角形式的俯仰角和滾動(dòng)角，因而無(wú)法得到第二方位。又如，地磁傳感器只有在確定自身方位的情況下，才能測(cè)量得到準(zhǔn)確的地磁信號(hào)。當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)，地磁傳感器的自身方位為第二方位。當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間在預(yù)設(shè)值之內(nèi)，地磁傳感器的自身方位為第一方位。當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間超出預(yù)設(shè)值，則很難確定地磁傳感器自身方位，因而無(wú)法得到第三方位。所述持續(xù)運(yùn)動(dòng)時(shí)間的預(yù)設(shè)值，根據(jù)三軸地磁傳感器的精度、數(shù)據(jù)傳輸穩(wěn)定性、用戶對(duì)偏差的可接受程度等有關(guān)，在通常的游戲系統(tǒng)中，建議值一般在10 20秒，優(yōu)選為15秒。本申請(qǐng)中所述處理器可以是微控制器(MCU)、計(jì)算機(jī)的CPU等。三軸陀螺儀、三軸加速度傳感器、三軸地磁傳感器與所述處理器之間可以有線或無(wú)線的通訊方式進(jìn)行數(shù)據(jù)的傳輸。常用的三軸陀螺儀根據(jù)輸出信號(hào)為模擬信號(hào)還是數(shù)字信號(hào)分為兩種。一種三軸陀螺儀向所述處理器輸出模擬信號(hào)的角速度，此時(shí)該模擬信號(hào)的角速度經(jīng)過(guò)模數(shù)轉(zhuǎn)換模塊轉(zhuǎn)換為數(shù)字信號(hào)后輸出給處理器，(該模數(shù)轉(zhuǎn)換模塊也可能在處理器內(nèi)部)，此時(shí)該模數(shù)轉(zhuǎn)換模塊將采樣時(shí)間傳輸給所述處理器，所述處理器計(jì)算第一方位時(shí)的積分時(shí)間即來(lái)自于該模數(shù)轉(zhuǎn)換模塊所輸出的采樣時(shí)間。另一種三軸陀螺儀向所述處理器直接輸出數(shù)字信號(hào)的角速度，此時(shí)該三軸陀螺儀內(nèi)部必然具有模數(shù)轉(zhuǎn)換模塊，那么三軸陀螺儀還向所述處理器輸出該模數(shù)轉(zhuǎn)換模塊的采樣時(shí)間，此時(shí)所述處理器計(jì)算第一方位時(shí)的積分時(shí)間即來(lái)自于該模數(shù)轉(zhuǎn)換模塊所輸出的采樣時(shí)間。所述處理器在收到三軸陀螺儀、三軸加速度傳感器、三軸地磁傳感器輸出的信號(hào)后，通常會(huì)進(jìn)行相應(yīng)的信號(hào)處理，比如濾波。濾波的實(shí)現(xiàn)方法很多，例如可以采用卡爾曼濾波算法(The Kalman Filter Algorithm)。經(jīng)過(guò)濾波處理的數(shù)據(jù)根據(jù)具體的功能進(jìn)行分類三軸陀螺儀輸入的角速度數(shù)據(jù)通過(guò)積分運(yùn)算獲得角位移，該角位移與初始方位相加得到第一方位。由于初始方位通常為零， 該角位移通常就是第一方位。在物體處于靜態(tài)時(shí)三軸加速度傳感器輸入的加速度數(shù)據(jù)通過(guò)坐標(biāo)轉(zhuǎn)換獲得第二方位。在物體處于運(yùn)動(dòng)時(shí)三軸加速度傳感器輸入的加速度數(shù)據(jù)的線性運(yùn)動(dòng)分量通過(guò)二次積分運(yùn)算獲得線性位移。該線性位移與物體的初始位置相加得到物體的當(dāng)前位置。由于初始位置通常為零，該線性位移通常就是物體的位置。當(dāng)物體持續(xù)運(yùn)動(dòng)的時(shí)間小于預(yù)設(shè)值時(shí)三軸地磁傳感器輸入的地磁信號(hào)用來(lái)計(jì)算第三方位。其中將角速度與時(shí)間進(jìn)行積分運(yùn)算獲得第一方位可以采用許多計(jì)算方法，例如可以采用經(jīng)典四階龍格-庫(kù)塔法(classical fourth-order Runge-Kutta method)計(jì)算出四元數(shù)形式表示的第一方位。在計(jì)算過(guò)程中如果四元數(shù)的平方和不等于1，則對(duì)四元數(shù)進(jìn)行歸一化處理，這些均為現(xiàn)有技術(shù)，在此不作贅述。四元數(shù)形式表示的第一方位可以由處理器轉(zhuǎn)換為歐拉角形式的方位、和/或旋轉(zhuǎn)矩陣形式的方位，這三種表示方式之間可以自由轉(zhuǎn)換，且均為現(xiàn)有技術(shù)。請(qǐng)參閱圖4，點(diǎn)M是物體上的一個(gè)點(diǎn)，初始狀態(tài)下，點(diǎn)M在靜止坐標(biāo)系A(chǔ)中的坐標(biāo)為 (x，y，z)。一段時(shí)間后，物體的位置和方位都可能發(fā)生了變化。當(dāng)物體的方位變化，實(shí)際上是物體的朝向在三維空間中相對(duì)于靜止坐標(biāo)系A(chǔ)發(fā)生了變化。換一種角度思考，也可以認(rèn)為是物體的朝向在三維空間中保持不變，而參考坐標(biāo)系由靜止坐標(biāo)系A(chǔ)變?yōu)榱诉\(yùn)動(dòng)坐標(biāo)系 A’，如圖3所示。為簡(jiǎn)化起見(jiàn)，圖3中假設(shè)物體的位置不變，即假設(shè)運(yùn)動(dòng)坐標(biāo)系A(chǔ)’和靜止坐標(biāo)系A(chǔ)的原點(diǎn)相同。對(duì)于點(diǎn)M，只要知道其在運(yùn)動(dòng)坐標(biāo)系A(chǔ)’下的坐標(biāo)(x’，y’，z’)，就可以描述物體的方位變化。(X，1，ζ)與(χ’，y’，ζ’ )之間的關(guān)系為(χ，，y，，ζ，)= (X，y，z)*Tarr其中，Tm是上述測(cè)量物體方位的方法的三種情況下與初始方位相加的量的旋轉(zhuǎn)矩陣形式。三軸加速度傳感器測(cè)量得到的加速度信號(hào)是重力加速度分量、向心加速度分量和線性加速度分量的矢量總和。由于重力加速度的方向、大小已知，并且靜止坐標(biāo)系A(chǔ)的一個(gè)軸通常選取鉛垂方向，因此加速度信號(hào)中的重力加速度分量在靜止坐標(biāo)系A(chǔ)下的坐標(biāo)值是已知的。在加速度信號(hào)中剔除重力加速度分量是這樣實(shí)現(xiàn)的。當(dāng)物體處于運(yùn)動(dòng)狀態(tài)時(shí)，取三軸加速度傳感器的三軸方向?yàn)檫\(yùn)動(dòng)坐標(biāo)系A(chǔ)’的三軸方向，這樣對(duì)于三軸加速度傳感器測(cè)量出來(lái)的三軸為運(yùn)動(dòng)坐標(biāo)系A(chǔ)’三軸方向的加速度信號(hào)，可以通過(guò)坐標(biāo)轉(zhuǎn)換轉(zhuǎn)換為三軸為靜止坐標(biāo)系A(chǔ)三軸方向的加速度信號(hào)，然后就可以方便地將其中的重力加速度分量剔除出來(lái)。在剔除掉重力加速度分量的加速度信號(hào)后，進(jìn)一步剔除掉向心加速度分量。向心加速度大小基于以下公式計(jì)算A (point) =kXw2Xr。其中A(point)為向心加速度分量的大小。k是比例因子，用來(lái)與保持加速度傳感器測(cè)量結(jié)果與公式計(jì)算結(jié)果保持一致。w為角速度，由三軸陀螺儀獲得。r為旋轉(zhuǎn)半徑，一般根據(jù)系統(tǒng)的經(jīng)驗(yàn)值來(lái)定，不同的運(yùn)動(dòng)狀態(tài)具有不同的經(jīng)驗(yàn)值。例如在通常的游戲系統(tǒng)中，如果整個(gè)手臂運(yùn)動(dòng)，轉(zhuǎn)動(dòng)半徑為手臂長(zhǎng)度，大約在65 85cm之間，如果是手腕運(yùn)動(dòng)，半徑大約在10 20cm之間，如果是手肘運(yùn)動(dòng)，半徑大約在30 50cm之間，根據(jù)實(shí)際情況來(lái)定。實(shí)際應(yīng)用中，我們會(huì)要求物體在用戶手中的握法，一般以物體的前端為圓弧運(yùn)動(dòng)的外延，也就是說(shuō)，物體基本垂直于運(yùn)動(dòng)圓弧，垂直點(diǎn)在物體的前端，這樣可以較好的排除滾動(dòng)角部分對(duì)系統(tǒng)計(jì)算的不利影響。最后，獲得的向心加速度的大小為A(point) = kX (Wl2+W22) Xr，其中，W1為偏航角速度，W2為俯仰角速度，均由陀螺儀獲得。向心加速度的方向與物體實(shí)際轉(zhuǎn)動(dòng)的弧形的切線垂直，指向圓弧外側(cè)，所以在以上條件的限制下，向心加速度的方向沿物體豎直指向物體前端。這樣便得到了向心加速度分量的大小和方向，從而可在剔除掉重力加速度分量的加速度信號(hào)中剔除。三軸加速度傳感器所測(cè)量的加速度信號(hào)，在剔除重力加速度分量和向心加速度分量后，剩余的就是線性加速度分量。將該線性加速度分量與時(shí)間進(jìn)行一次積分運(yùn)算可以獲得線性運(yùn)動(dòng)速度，將該線性加速度分量與時(shí)間進(jìn)行二次積分運(yùn)算可以獲得線性運(yùn)動(dòng)位移。 通常將靜止坐標(biāo)系A(chǔ)的原點(diǎn)設(shè)為物體的初始位置，這樣該線性位移就是物體的目標(biāo)位置。當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)，三軸加速度傳感器測(cè)量的加速度信號(hào)就等于重力加速度分量，將該重力加速度分量通過(guò)坐標(biāo)轉(zhuǎn)換轉(zhuǎn)換為三軸為運(yùn)動(dòng)坐標(biāo)系A(chǔ)’三軸方向，便可以計(jì)算出以歐拉角形式表示的當(dāng)前物體方位中的俯仰角f和滾動(dòng)角Θ。在此原理下，本發(fā)明僅示例性地給出如下一種計(jì)算方法
權(quán)利要求
1.一種測(cè)量物體位置和方位的方法，所述物體上包括三軸陀螺儀、三軸加速度傳感器和三軸地磁傳感器，其特征是，其中，測(cè)量物體位置的方法是三軸加速度傳感器測(cè)量物體的加速度并輸出給處理器， 處理器將該加速度中剔除掉重力加速度分量和向心加速度分量，得到線性加速度分量；處理器將該線性加速度分量與時(shí)間進(jìn)行兩次積分運(yùn)算得到物體在該時(shí)間內(nèi)的線性位移；處理器將物體的初始位置與該線性位移相加，得到物體的當(dāng)前位置；其中，測(cè)量物體方位的方法是三軸陀螺儀測(cè)量物體的角速度并輸出給處理器，處理器將該角速度與時(shí)間進(jìn)行積分運(yùn)算得到物體在該時(shí)間內(nèi)的角位移，處理器將物體的初始方位與該角位移相加稱為第一方位；處理器至少給出四元數(shù)形式和歐拉角形式表示的第一方位；當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)，三軸加速度傳感器測(cè)量得到的物體的加速度就是該物體的重力加速度，處理器根據(jù)此時(shí)的物體的加速度值計(jì)算物體的歐拉角形式的俯仰角和滾動(dòng)角； 當(dāng)計(jì)算出的俯仰角的絕對(duì)值小于或等于80度時(shí)，將計(jì)算出的俯仰角、滾動(dòng)角以及上述第一方位的偏航角作為第二方位；當(dāng)計(jì)算出的俯仰角的絕對(duì)值大于80度時(shí)，將計(jì)算出的俯仰角、上述第一方位的滾動(dòng)角和偏航角作為第二方位；處理器給出四元數(shù)形式表示的第二方位；當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)、或者當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間在預(yù)設(shè)值之內(nèi)時(shí)，三軸地磁傳感器測(cè)量地磁信號(hào)并輸出給處理器，處理器根據(jù)該地磁信號(hào)計(jì)算物體的歐拉角形式的偏航角，并將俯仰角和滾動(dòng)角分別設(shè)為歐拉角形式的上述第一方位的俯仰角和滾動(dòng)角，作為第三方位；處理器給出四元數(shù)形式表示的第三方位；當(dāng)物體處于靜止?fàn)顟B(tài)時(shí)，處理器計(jì)算第二方位與第一方位的差值作為補(bǔ)償量一，還計(jì)算第三方位與第一方位的差值作為補(bǔ)償量二，處理器將第一方位與補(bǔ)償量一、補(bǔ)償量二相加作為物體的方位；當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間在預(yù)設(shè)值之內(nèi)，處理器計(jì)算第三方位與第一方位的差值作為補(bǔ)償量二，處理器將第一方位與補(bǔ)償量二相加作為物體的方位；當(dāng)物體處于運(yùn)動(dòng)狀態(tài)且持續(xù)運(yùn)動(dòng)的時(shí)間超出預(yù)設(shè)值，處理器將第一方位作為物體的方位。
2.根據(jù)權(quán)利要求1所述的測(cè)量物體位置和方位的方法，其特征是，所述計(jì)算第二方位與第一方位的差值作為補(bǔ)償量一是以四元數(shù)形式的減法進(jìn)行，所述計(jì)算第三方位與第一方位的差值作為補(bǔ)償量二是以四元數(shù)形式的減法進(jìn)行，所述將第一方位與補(bǔ)償量一和/或補(bǔ)償量二相加作為物體的方位是以四元數(shù)形式的加法進(jìn)行。
3.根據(jù)權(quán)利要求1所述的測(cè)量物體位置和方位的方法，其特征是，將第一方位、第二方位、第三方位由歐拉角形式轉(zhuǎn)換為四元數(shù)形式時(shí)，先進(jìn)行試運(yùn)算，當(dāng)試運(yùn)算中出現(xiàn)任何一個(gè)歐拉角從取值范圍的一頭跳變到另一頭時(shí)為試運(yùn)算失敗，否則為試運(yùn)算成功；出現(xiàn)試運(yùn)算失敗時(shí)，將所有歐拉角的取值范圍整數(shù)倍擴(kuò)大，再進(jìn)行試運(yùn)算，反復(fù)該步直至試運(yùn)算成功；出現(xiàn)試運(yùn)算成功時(shí)，再進(jìn)行實(shí)際運(yùn)算。
4.根據(jù)權(quán)利要求1所述的測(cè)量物體位置和方位的方法，其特征是，當(dāng)三軸陀螺儀向所述處理器輸出模擬信號(hào)的角速度，此時(shí)該模擬信號(hào)的角速度經(jīng)過(guò)模數(shù)轉(zhuǎn)換模塊轉(zhuǎn)換為數(shù)字信號(hào)后輸出給處理器，此時(shí)該模數(shù)轉(zhuǎn)換模塊將采樣時(shí)間傳輸給所述處理器，所述處理器計(jì)算第一方位時(shí)的積分時(shí)間即來(lái)自于該模數(shù)轉(zhuǎn)換模塊所輸出的采樣時(shí)間；當(dāng)三軸陀螺儀向所述處理器直接輸出數(shù)字信號(hào)的角速度，此時(shí)該三軸陀螺儀內(nèi)部必然具有模數(shù)轉(zhuǎn)換模塊，那么三軸陀螺儀還向所述處理器輸出該模數(shù)轉(zhuǎn)換模塊的采樣時(shí)間，此時(shí)所述處理器計(jì)算第一方位時(shí)的積分時(shí)間即來(lái)自于該模數(shù)轉(zhuǎn)換模塊所輸出的采樣時(shí)間。
全文摘要
本發(fā)明公開(kāi)了一種測(cè)量物體位置和方位的方法，物體的方位首先由三軸陀螺儀的測(cè)量結(jié)果計(jì)算出第一方位。三軸加速度傳感器僅在物體處于靜止?fàn)顟B(tài)時(shí)計(jì)算第二方位。三軸地磁傳感器在物體處于靜止?fàn)顟B(tài)時(shí)、或者在物體持續(xù)運(yùn)動(dòng)一定時(shí)間內(nèi)計(jì)算第三方位。第二方位、第三方位是對(duì)第一方位的修正。傳統(tǒng)的測(cè)量物體方位的方法僅使用三軸陀螺儀，這會(huì)產(chǎn)生累計(jì)誤差而導(dǎo)致長(zhǎng)時(shí)間測(cè)量的結(jié)果不準(zhǔn)確。本發(fā)明在使用三軸陀螺儀的基礎(chǔ)上，又采用三軸加速度傳感器和三軸地磁傳感器對(duì)三軸陀螺儀的方位測(cè)量結(jié)果進(jìn)行修正，從而可以獲得較為準(zhǔn)確的物體方位。
文檔編號(hào)G01C21/00GK102297693SQ201010208770
公開(kāi)日2011年12月28日 申請(qǐng)日期2010年6月24日 優(yōu)先權(quán)日2010年6月24日
發(fā)明者喻應(yīng)東 申請(qǐng)人:鼎億公司, 鼎億數(shù)碼科技(上海)有限公司