專利名稱：用于確定物體姿態(tài)的方法及裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及計(jì)算機(jī)技術(shù)領(lǐng)域，更具體地，本發(fā)明涉及一種用于確定物體姿態(tài)的方法及裝置。
背景技術(shù)：
姿態(tài)定向參考系統(tǒng)(AttitudeHeading Reference System, AHRS)是一種用于提供物體的姿態(tài)以及運(yùn)動(dòng)方向等信息的系統(tǒng)。物體姿態(tài)的信息可以被用于例如自動(dòng)控制、導(dǎo)航、虛擬現(xiàn)實(shí)等應(yīng)用中。通常地，姿態(tài)定向參考系統(tǒng)包括多個(gè)多軸傳感器，例如陀螺儀、加速度傳感器以及磁強(qiáng)計(jì)等。姿態(tài)定向參考系統(tǒng)通過(guò)這些傳感器來(lái)采集與物體姿態(tài)相關(guān)的數(shù)據(jù)信息(例如，角速度、加速度或磁場(chǎng)強(qiáng)度等)，并采用特定的算法來(lái)計(jì)算并表示物體的姿態(tài)和/或運(yùn)動(dòng)方向。在傳統(tǒng)的姿態(tài)定向參考系統(tǒng)中，物體相對(duì)于物體坐標(biāo)系(即物體自身所在的局部坐標(biāo)系)的姿態(tài)通常由歐拉角(即，對(duì)應(yīng)于三個(gè)旋轉(zhuǎn)軸的章動(dòng)角、進(jìn)動(dòng)角以及自旋角)表示。然而，采用歐拉角表示物體的姿態(tài)或物體的旋轉(zhuǎn)時(shí)，會(huì)出現(xiàn)萬(wàn)向節(jié)鎖定(Gimbal Lock)問(wèn)題，即，當(dāng)物體的旋轉(zhuǎn)使得三個(gè)旋轉(zhuǎn)軸中的兩個(gè)旋轉(zhuǎn)軸相互平行時(shí)，繞一個(gè)旋轉(zhuǎn)軸的旋轉(zhuǎn)可能會(huì)覆蓋繞另一個(gè)旋轉(zhuǎn)軸的旋轉(zhuǎn)，從而失去一個(gè)旋轉(zhuǎn)自由度?！N避免出現(xiàn)萬(wàn)向節(jié)鎖定的方法采用四元數(shù)(Quaternion)參數(shù)來(lái)描述物體的姿態(tài)。四元數(shù)由實(shí)數(shù)與三個(gè)元素1、j、k組成,其中1、j、k滿足i2+k2+j2 = ijk = -1的關(guān)系，每個(gè)四元數(shù)都可以表示為1、1、j、k的線性組合。由于四元數(shù)的表達(dá)式?jīng)]有類(lèi)似于歐拉角的奇點(diǎn)，因此，以四元數(shù)來(lái)描述的物體姿態(tài)不會(huì)出現(xiàn)萬(wàn)向節(jié)鎖定的問(wèn)題。在傳統(tǒng)的姿態(tài)定向參考系統(tǒng)中，可以利用陀螺儀測(cè)量物體運(yùn)動(dòng)的角速度，并進(jìn)一步地對(duì)該角速度進(jìn)行積分，從而得到以四元數(shù)參數(shù)表示的物體的姿態(tài)。然而，基于陀螺儀的姿態(tài)定向參考系統(tǒng)是一種相對(duì)姿勢(shì)定向參考系統(tǒng)，在每次確定物體姿態(tài)時(shí)，其需要基于前一次或前一時(shí)刻物體姿態(tài)的參數(shù)?？紤]到陀螺儀的各種測(cè)量誤差，例如實(shí)際靈敏度誤差、靈敏度的非線性、靈敏度的溫度漂移等，這種相對(duì)姿態(tài)定向參考系統(tǒng)所提供的關(guān)于物體姿態(tài)的信息會(huì)累積測(cè)量誤差，這使得很難在較長(zhǎng)的時(shí)間內(nèi)精確地表示物體姿態(tài)。因此，需要一種能夠穩(wěn)定并且精確地確定物體姿態(tài)的方法及裝置。

發(fā)明內(nèi)容
為了解決上述問(wèn)題，根據(jù)本發(fā)明的一個(gè)方面，提供了一種用于確定物體姿態(tài)的方法，包括接收重力加速度信號(hào)以及地磁場(chǎng)信號(hào)，所述重力加速度信號(hào)與所述地磁場(chǎng)信號(hào)響應(yīng)于物體的姿態(tài)的變化而變化；基于所述重力加速度信號(hào)確定物體坐標(biāo)系中的重力場(chǎng)表示，并基于所述地磁場(chǎng)信號(hào)確定物體坐標(biāo)系中的地磁場(chǎng)表示；基于所述物體坐標(biāo)系中的所述重力場(chǎng)表示與所述地磁場(chǎng)表示計(jì)算所述物體坐標(biāo)系與地球坐標(biāo)系之間的坐標(biāo)系轉(zhuǎn)換參數(shù)；基于所述坐標(biāo)系轉(zhuǎn)換參數(shù)確定所述物體的姿態(tài)。
根據(jù)本發(fā)明的另一方面，還提供了一種用于確定物體姿態(tài)的裝置，包括接收模塊，用于接收重力加速度信號(hào)以及地磁場(chǎng)信號(hào)，所述重力加速度信號(hào)與所述地磁場(chǎng)信號(hào)響應(yīng)于物體的姿態(tài)的變化而變化；第一確定模塊，用于基于所述重力加速度信號(hào)確定物體坐標(biāo)系中的重力場(chǎng)表不，并基于所述地磁場(chǎng)信號(hào)確定物體坐標(biāo)系中的地磁場(chǎng)表不；轉(zhuǎn)換模塊，用于基于所述物體坐標(biāo)系中的所述重力場(chǎng)表示與所述地磁場(chǎng)表示計(jì)算所述物體坐標(biāo)系與地球坐標(biāo)系之間的坐標(biāo)系轉(zhuǎn)換參數(shù)；第二確定模塊，用于基于所述坐標(biāo)系轉(zhuǎn)換參數(shù)確定所述物體的姿態(tài)。根據(jù)本發(fā)明的又一方面，還提供了一種姿態(tài)定向參考系統(tǒng)，包括前述方面的用于確定物體姿態(tài)的裝置。不同于現(xiàn)有技術(shù)，在本發(fā)明的用于確定物體姿態(tài)的方法和裝置中，每次確定物體姿態(tài)的操作相互獨(dú)立，這就避免了由于傳感器測(cè)量誤差的累積所引起的偏差，使得能夠在長(zhǎng)時(shí)間內(nèi)穩(wěn)定且精確地確定物體姿態(tài)；此外，本發(fā)明采用四元數(shù)的坐標(biāo)系轉(zhuǎn)換參數(shù)來(lái)確定物體姿態(tài)，由于四元數(shù)表達(dá)式?jīng)]有類(lèi)似于歐拉角的奇點(diǎn)，因此，本發(fā)明還避免了萬(wàn)向節(jié)鎖定的問(wèn)題。


通過(guò)對(duì)結(jié)合附圖所示出的實(shí)施方式進(jìn)行詳細(xì)說(shuō)明，本發(fā)明的上述以及其他特征將更加明顯，本發(fā)明附圖中相同的標(biāo)號(hào)表示相同或相似的元素。在附圖中圖1示出了適于應(yīng)用本發(fā)明的方法及裝置的姿態(tài)定向參考系統(tǒng)；圖2示出了根據(jù)本發(fā)明一個(gè)方面的用于確定物體姿態(tài)的方法的流程；圖3示出了重力場(chǎng)與地磁場(chǎng)在物體坐標(biāo)系與地球坐標(biāo)系中的表示；圖4示出了根據(jù)本發(fā)明一個(gè)方面的用于確定物體姿態(tài)的裝置。在附圖中，相同或?qū)?yīng)的標(biāo)號(hào)表不相同或?qū)?yīng)的部分。
具體實(shí)施例方式下文將參考若干示例性實(shí)施方式來(lái)描述本發(fā)明的原理和精神。應(yīng)當(dāng)理解，給出這些實(shí)施方式僅僅是為了使本領(lǐng)域技術(shù)人員能夠更好地理解進(jìn)而實(shí)現(xiàn)本發(fā)明，而并非以任何方式限制本發(fā)明的范圍。參考圖1，示出了適于應(yīng)用本發(fā)明的方法及裝置的姿態(tài)定向參考系統(tǒng)100。姿態(tài)定向參考系統(tǒng)100通常至少部分地裝載在可移動(dòng)的物體10上。在圖1中，物體10被表示為移動(dòng)通信終端，但是可以理解，根據(jù)具體應(yīng)用的不同，物體10亦可為例如工業(yè)設(shè)備、虛擬現(xiàn)實(shí)設(shè)備、便攜式電子設(shè)備或其他機(jī)電設(shè)備，不應(yīng)限制其范圍。圖1中示出了物體10的物體坐標(biāo)系(即物體自身的局部坐標(biāo)系)，其具有相互正交的三個(gè)坐標(biāo)軸BX軸、By軸以及Bz軸。其中，物體坐標(biāo)系以物體10的中心為坐標(biāo)原點(diǎn)，物體10的旋轉(zhuǎn)或平移等操作都是圍繞該物體坐標(biāo)系進(jìn)行的；當(dāng)物體10進(jìn)行旋轉(zhuǎn)或平移等操作時(shí)，物體坐標(biāo)系也執(zhí)行相應(yīng)的旋轉(zhuǎn)或平移操作?？梢钥闯?，由于物體坐標(biāo)系隨著物體10姿勢(shì)的變化而改變，這通常與處于靜止?fàn)顟B(tài)或者不跟隨物體10姿勢(shì)變化的觀測(cè)設(shè)備的用于觀測(cè)的基準(zhǔn)坐標(biāo)系不同。因此，可以選取相對(duì)靜止的地球坐標(biāo)系作為描述物體10姿態(tài)的基準(zhǔn)坐標(biāo)系，并利用該基準(zhǔn)坐標(biāo)系中的四元數(shù)參數(shù)來(lái)表示物體姿勢(shì)。具體地,在基準(zhǔn)坐標(biāo)系中，物體10的姿勢(shì)可以表示為繞某一歸一化的旋轉(zhuǎn)軸NUtl, y0, Z0)旋轉(zhuǎn)0的角度,這對(duì)應(yīng)于四元數(shù)參數(shù)Q = (cos( 0 /2),x0sin ( 0 /2), y0sin( 0 /2), z0sin( 0 /2))。通常地，觀測(cè)設(shè)備的基準(zhǔn)坐標(biāo)系通常選取地球坐標(biāo)系。地球坐標(biāo)系包括三個(gè)相互正交的坐標(biāo)軸，這三個(gè)坐標(biāo)軸的位置關(guān)系符合右手螺旋法則例如東北天坐標(biāo)系，其包括E軸(朝向地平面的正東方向)、N軸(朝向地平面的正北方向)以及U軸(朝向天頂)；再例如北東地坐標(biāo)系,其包括N軸、E軸以及D軸(朝向地心)。本領(lǐng)域技術(shù)人員可以理解，這些地球坐標(biāo)系易于相互轉(zhuǎn)換，在接下來(lái)的實(shí)施例中，均以地球坐標(biāo)系為東北天坐標(biāo)系為例進(jìn)行說(shuō)明，但本發(fā)明并不限于此。姿態(tài)定向參考系統(tǒng)100包括裝載在物體10上的加速度傳感器101以及磁強(qiáng)計(jì)103。其中，該加速度傳感器101至少具有三個(gè)相互正交的測(cè)量軸，以沿這三個(gè)測(cè)量軸測(cè)量重力加速度并輸出重力加速度信號(hào)，而該磁強(qiáng)計(jì)103至少具有三個(gè)相互正交的測(cè)量軸，以沿這三個(gè)測(cè)量軸測(cè)量地磁場(chǎng)并輸出地磁場(chǎng)信號(hào)。可以理解，當(dāng)物體10的姿態(tài)變化而使得加速度傳感器101以及磁強(qiáng)計(jì)103的測(cè)量軸方向變化時(shí)，重力加速度信號(hào)以及地磁場(chǎng)信號(hào)相應(yīng)變化，即不同測(cè)量軸所測(cè)得的重力加速度或地磁場(chǎng)的分量的測(cè)量值變化。優(yōu)選地，加速度傳感器101與磁強(qiáng)計(jì)103的測(cè)量軸與物體10物體坐標(biāo)系的坐標(biāo)軸分別重合，以使得所測(cè)得的重力加速度與地磁場(chǎng)分別對(duì)應(yīng)到物體坐標(biāo)系的Bx軸、By軸以及Bz軸?？梢岳斫?，加速度傳感器101與磁強(qiáng)計(jì)103的測(cè)量軸亦可與物體坐標(biāo)系的坐標(biāo)軸不完全重合，而是偏離一定的角度；相應(yīng)地，在此情況下，在利用傳感器的測(cè)量結(jié)果確定物體姿態(tài)時(shí)，還需要計(jì)入該角度的偏差。為了便于說(shuō)明，在下文中，均以加速度傳感器101與磁強(qiáng)計(jì)103的測(cè)量軸與物體坐標(biāo)系的坐標(biāo)軸重合為例進(jìn)行說(shuō)明，但本發(fā)明并不限于此。在實(shí)際應(yīng)用中，姿態(tài)定向參考系統(tǒng)100還包括數(shù)據(jù)處理設(shè)備105，其用于接收由加速度傳感器101與磁強(qiáng)計(jì)103輸出的重力加速度信號(hào)與地磁場(chǎng)信號(hào)，并對(duì)這些測(cè)量信號(hào)進(jìn)行數(shù)據(jù)處理。在圖1所示的實(shí)施例中，該數(shù)據(jù)處理設(shè)備105與物體10分離，例如其可以為諸如工作站、個(gè)人計(jì)算機(jī)或服務(wù)器等遠(yuǎn)程數(shù)據(jù)處理設(shè)備；該遠(yuǎn)程數(shù)據(jù)處理設(shè)備可以通過(guò)有線或無(wú)線連接與加速度傳感器101與磁強(qiáng)計(jì)103通信地耦接，以接收測(cè)量信號(hào)并進(jìn)行數(shù)據(jù)處理?？梢岳斫?，在其他的實(shí)施例中，該數(shù)據(jù)處理設(shè)備105也可以裝載在物體10上，其例如為與加速度傳感器101以及磁強(qiáng)計(jì)103相耦接的微處理器(MCU)。需要說(shuō)明的是，由于應(yīng)用本發(fā)明的方法及裝置的姿態(tài)定向參考系統(tǒng)100基于重力場(chǎng)與地磁場(chǎng)來(lái)確定物體姿態(tài)，因此，在姿態(tài)定向參考系統(tǒng)100進(jìn)行確定物體姿態(tài)的操作時(shí)，加速度傳感器101應(yīng)在物體10處于基本靜止或勻速運(yùn)動(dòng)狀態(tài)下(相對(duì)于地球而言)測(cè)量重力加速度，即物體10基本沒(méi)有受到除重力之外的其他力作用形成的加速度而影響其重力加速度的測(cè)量。同時(shí)，磁強(qiáng)計(jì)103應(yīng)在物體10附近基本不存在除地磁場(chǎng)之外的其他磁場(chǎng)的情況下測(cè)量物體10所在位置的環(huán)境磁場(chǎng)，以避免其他磁場(chǎng)干擾地磁場(chǎng)的測(cè)量。參考圖2，示出了根據(jù)本發(fā)明一個(gè)方面的用于確定物體姿態(tài)的方法的流程。應(yīng)當(dāng)理解，該方法中記載的各個(gè)步驟可以按照不同的順序執(zhí)行，和/或并行執(zhí)行。此外，該方法可以包括附加的步驟和/或省略執(zhí)行示出的步驟。本發(fā)明的范圍在此方面不受限制。如圖2所示，在步驟S202中，接收重力加速度信號(hào)與地磁場(chǎng)信號(hào)，所述重力加速度信號(hào)與所述地磁場(chǎng)信號(hào)響應(yīng)于物體的姿態(tài)的變化而變化。
正如前述，物體的重力加速度信號(hào)表示物體所受到的重力加速度的幅度以及方向，其由裝載在物體上的加速度傳感器測(cè)得。例如，對(duì)于三軸加速度傳感器，其具有三個(gè)相互正交的測(cè)量軸，在測(cè)量物體的所受到的重力加速度時(shí)，重力加速度會(huì)根據(jù)其方向與測(cè)量軸的夾角分別地映射到各個(gè)測(cè)量軸上。當(dāng)三軸加速度傳感器的三個(gè)測(cè)量軸與物體坐標(biāo)系的三個(gè)坐標(biāo)軸重合時(shí)，所測(cè)得的重力加速度信號(hào)對(duì)應(yīng)于重力場(chǎng)相對(duì)于物體坐標(biāo)系各個(gè)坐標(biāo)軸的偏移。類(lèi)似地，物體的地磁場(chǎng)信號(hào)表示物體所在位置的地磁場(chǎng)的幅度以及方向，其由裝載在物體上的磁強(qiáng)計(jì)測(cè)得。例如，對(duì)于三軸磁強(qiáng)計(jì)，其具有三個(gè)相互正交的測(cè)量軸，在測(cè)量地磁場(chǎng)時(shí)，地磁場(chǎng)會(huì)根據(jù)其方向與測(cè)量軸的夾角分別地映射到各個(gè)測(cè)量軸上。當(dāng)三軸加速度傳感器的三個(gè)測(cè)量軸與物體坐標(biāo)系的三個(gè)坐標(biāo)軸重合時(shí)，所測(cè)得的地磁場(chǎng)信號(hào)對(duì)應(yīng)于地磁場(chǎng)相對(duì)于物體坐標(biāo)系各個(gè)坐標(biāo)軸的偏移。接著，在步驟S204中，基于所述重力加速度信號(hào)確定物體坐標(biāo)系中的重力場(chǎng)表75，并基于所述地磁場(chǎng)信號(hào)確定物體坐標(biāo)系中的地磁場(chǎng)表不。正如前述，所測(cè)得的重力加速度信號(hào)對(duì)應(yīng)于重力場(chǎng)相對(duì)于物體坐標(biāo)系各個(gè)坐標(biāo)軸的偏移，并且所測(cè)得的地磁場(chǎng)信號(hào)對(duì)應(yīng)于地磁場(chǎng)相對(duì)于物體坐標(biāo)系各個(gè)坐標(biāo)軸的偏移，因此，可以基于這些測(cè)量信號(hào)確定物體坐標(biāo)系中的重力場(chǎng)表示與地磁場(chǎng)表示。由于地球坐標(biāo)系實(shí)質(zhì)上是基于重力場(chǎng)方向以及地磁場(chǎng)相關(guān)的方向來(lái)定義的，因此，易于確定地球坐標(biāo)系中的重力場(chǎng)表示以及地磁場(chǎng)表示。進(jìn)一步地，基于物體坐標(biāo)系中的重力場(chǎng)表示，即可確定物體坐標(biāo)系中的重力場(chǎng)表示與地球坐標(biāo)系中的重力場(chǎng)表示的對(duì)應(yīng)關(guān)系；而基于物體坐標(biāo)系中的地磁場(chǎng)表示，即可確定物體坐標(biāo)系中的地磁場(chǎng)表示與地球坐標(biāo)系中的地磁場(chǎng)表示的對(duì)應(yīng)關(guān)系。參考圖3，同時(shí)示出了重力場(chǎng)與地磁場(chǎng)在物體坐標(biāo)系與地球坐標(biāo)系中的表示。具體地，物體坐標(biāo)系包括三個(gè)相互正交的坐標(biāo)軸Bx軸、By軸以及Bz軸，而重力加速度信號(hào)包括映射在這三個(gè)坐標(biāo)軸上的測(cè)量值Gx、Gy以及Gz，則物體坐標(biāo)系中重力場(chǎng)表示即為由原點(diǎn)(0，0，0)指向(Gx，Gy, Gz)的矢量G ;而地磁場(chǎng)信號(hào)包括在這三個(gè)坐標(biāo)軸上的測(cè)量值Mx、My以及Mz，則物體坐標(biāo)系中的地磁場(chǎng)表示即為由原點(diǎn)(0,0,0)指向(Mx, My, Mz)的矢量M。進(jìn)一步地，對(duì)于地球坐標(biāo)系，其通常是針對(duì)地平面而言的，例如東北天坐標(biāo)系中的N軸，其是指地平面(物體所在位置的地球的切平面)上的正北方向。而對(duì)于地平面而言，地磁場(chǎng)并非指向正北，而是基于地理緯度的不同而與該切平面存在沿U軸方向的夾角。因此，對(duì)于地磁場(chǎng)而言，其在地平面的投影矢量Nh(Mhx，Mhy，Mhz)更易于表示?？梢岳斫猓摰卮艌?chǎng)的投影矢量(Mhx，Mhy，Mhz)可以由(Gx，Gy，Gz)及(Mx，My，Mz)確定，這會(huì)在下文中詳述。另一方面，對(duì)于地球坐標(biāo)系，例如東北天坐標(biāo)系或北東地坐標(biāo)系，或者其他具有相互正交的三個(gè)坐標(biāo)軸的地球坐標(biāo)系，重力場(chǎng)以及地磁場(chǎng)亦具有對(duì)應(yīng)的表示。例如，對(duì)于東北天坐標(biāo)系，指向地心的重力場(chǎng)的方向與U軸方向重合，因此，重力場(chǎng)的方向在東北天坐標(biāo)系中可以表示為(0，0，1)。類(lèi)似地，地磁場(chǎng)在地平面的投影矢量(Mhx，Mhy，Mhz)的方向與東北天坐標(biāo)系中的N軸方向重合，那么，地磁場(chǎng)在地平面的投影矢量(Mhx，Mhy，Mhz)的方向在東北天坐標(biāo)系中可以被表示為(0，1，0)。此外，對(duì)于東北天坐標(biāo)系的E軸，其在東北天坐標(biāo)系中被表示為(1，0，0)，而其在物體坐標(biāo)系中的表不(Ex, Ey, Ez)由表不N軸方向的矢量Nh與表不U軸方向的矢量G的外積確定，即(MhyGz-GyMhz, MhzGx-GzMhx, MhxGy-GxMhy)，或基于(Gx，Gy, Gz)及(Mx，My, Mz)的外積確定。正如前述，在物體坐標(biāo)系中，地磁場(chǎng)在地平面的投影矢量(Mh^MhyWhz)可以由(Gx，Gy, Gz)及(Mx，My，Mz)確定。具體地，在物體坐標(biāo)系中，地平面由下述地平面方程表示xGx+yGy+zGz = 0(I)點(diǎn)(Mhx，Mhy, Mhz)位于該地平面上，即MtaGx+MhyGy+MhzGz = 0(2)而由點(diǎn)(Mhx，Mhy，Mhz)到點(diǎn)(Mx，My，Mz)的直線由下述方程表示
X-Mx y~My Z-Mz-=-=--(3)
Mhx-Mx Mhy-My Mhz-Mz并且該直線垂直于地平面，SP :-￡-=---=-￡-= K(4)
Mhx-Mx Mhy-My Mhz-Mz由方程(I)、(2)、(3)、(4)，可以得到用(Gx，Gy,Gz)及(Mx，My, Mz)表示的(Mhx, Mhy,Mhz)，具體為K =-(GxMx+GyMy+GzMz)/(GxGx+GyGy+GzGz)(5)Mhx = KGX+MX(6)Mhy = KGy+My(7)Mhz = KGZ+MZ(8)需要說(shuō)明的是，上述對(duì)(Mhx，Mhy，Mhz)的計(jì)算是示例性而非限制性的，本領(lǐng)域技術(shù)人員可以通過(guò)其他適合的方式來(lái)確定(Mhx, Mhy, Mhz)的表達(dá)式。仍參考圖2，接著，在步驟S206中，基于所述物體坐標(biāo)系中的所述重力場(chǎng)表示與所述地磁場(chǎng)表示計(jì)算所述物體坐標(biāo)系與地球坐標(biāo)系之間的坐標(biāo)系轉(zhuǎn)換參數(shù)。由于四元數(shù)的表達(dá)式?jīng)]有類(lèi)似于歐拉角的奇點(diǎn)，以四元數(shù)來(lái)描述的物體姿態(tài)不會(huì)出現(xiàn)萬(wàn)向節(jié)鎖定。因此，優(yōu)選地，采用四元數(shù)參數(shù)Q(a，b，c，d) (Q優(yōu)選為單位四元數(shù)參數(shù))表示并計(jì)算坐標(biāo)系轉(zhuǎn)換參數(shù)。具體地,對(duì)于物體坐標(biāo)系中任意的矢量V(x, y, z),其對(duì)應(yīng)于地球坐標(biāo)系中的矢量r (x’，y’，z’)，相應(yīng)地，以四元數(shù)形式表示的這兩個(gè)矢量分別為V = (0，X，y, z)(9)V，= (0, x，，y，，z，)(10)這兩個(gè)矢量滿足下述坐標(biāo)系轉(zhuǎn)換方程V = QV’ CT1(11)其中，Q= (a, b, c, d)(12)Q-1 = (a, -b, ~c, _d)(13)可以對(duì)方程(11)進(jìn)行求解，以得到坐標(biāo)系轉(zhuǎn)換參數(shù)Q。具體地，基于四元數(shù)乘法規(guī)則，將等式(9)、(10)、(12)、(13)代入等式(3)可得，a2+b2+c2+d2 = I(14)(a2+b2-c2_d2) x，+2(bc_ad)y，+2(bd+ac)z’ = x (15)2(bc+ad)x，+ (a2-b2+c2_d2) y，+2(cd_ab)z，= y (16)
2(bd_ac)x，+2(cd+ab)y，+ (a2-b2_c2+d2) z，= z (17)正如前述，物體坐標(biāo)系中的矢量表示與地球坐標(biāo)系中的矢量表示存在對(duì)應(yīng)關(guān)系。對(duì)于重力場(chǎng)G，在物體坐標(biāo)系中，其被表示為(Gx，Gy, Gz)，而在地球坐標(biāo)系中，其被表示為(0,0,1);對(duì)于地磁場(chǎng)在地平面投影矢量Mh，在物體坐標(biāo)系中，其被表示為(Mhx, Mhy, Mhz)，而在地球坐標(biāo)系中，其被表示為(0，1，0);對(duì)于重力場(chǎng)與地磁場(chǎng)的外積E，在物體坐標(biāo)系中，其被表示為(Ex，Ey，Ez)，而在地球坐標(biāo)系中，其被表示為(1，0，0)。相應(yīng)地，利用在步驟S204中獲得的對(duì)應(yīng)關(guān)系來(lái)求解坐標(biāo)系轉(zhuǎn)換方程?？梢詫⑸鲜?組對(duì)應(yīng)關(guān)系代入等式(15)、(16)、(17)，可以得到2 (bd+ac) =Gx(18)2(cd-ab) = Gy(19)(a2-b2-c2+d2) = Gz(20)2 (bc-ad) = Mhx(21)(a2-b2+c2-d2) = Mhy(22)2 (cd+ab) = Mhz(23)(a2+b2-c2-d2) = Ex(24)2 (bc+ad) = Ey(25)2 (bd-ac) = Ez(26)對(duì)上述方程求解可得a、b、c、d的表達(dá)式，從而進(jìn)一步得到坐標(biāo)系轉(zhuǎn)換參數(shù)Q (a，b，c，d)。需要說(shuō)明的是，對(duì)于求解坐標(biāo)系轉(zhuǎn)換參數(shù)Q，上述等式(18)-(26)存在冗余等式，該冗余等式用于驗(yàn)證求得的解，并刪除不符合初始條件(即3組對(duì)應(yīng)關(guān)系)的虛根。因此，在實(shí)際應(yīng)用中，亦可僅選取上述3組對(duì)應(yīng)關(guān)系中的兩組，例如物體坐標(biāo)系中的重力場(chǎng)表示與地球坐標(biāo)系中的重力場(chǎng)表示的對(duì)應(yīng)關(guān)系以及物體坐標(biāo)系中的地磁場(chǎng)表示與地球坐標(biāo)系中的地磁場(chǎng)表示的對(duì)應(yīng)關(guān)系來(lái)求解坐標(biāo)系轉(zhuǎn)換參數(shù)Q (a, b, c, d)。在一個(gè)實(shí)施例中，坐標(biāo)系轉(zhuǎn)換參數(shù)Q (a，b，c，d)的解為a = (f2+f3-f4)/2 ;b = (-f2+f3+f4) /2c= (2f1-f2-f3+f4) /權(quán)利要求
1.一種用于確定物體姿態(tài)的方法，包括 接收重力加速度信號(hào)以及地磁場(chǎng)信號(hào)，所述重力加速度信號(hào)與所述地磁場(chǎng)信號(hào)響應(yīng)于物體的姿態(tài)的變化而變化； 基于所述重力加速度信號(hào)確定物體坐標(biāo)系中的重力場(chǎng)表不，基于所述地磁場(chǎng)信號(hào)確定物體坐標(biāo)系中的地磁場(chǎng)表示； 基于所述物體坐標(biāo)系中的所述重力場(chǎng)表示與所述地磁場(chǎng)表示計(jì)算所述物體坐標(biāo)系與地球坐標(biāo)系之間的坐標(biāo)系轉(zhuǎn)換參數(shù)； 基于所述坐標(biāo)系轉(zhuǎn)換參數(shù)確定所述物體的姿態(tài)。
2.根據(jù)權(quán)利要求1所述的方法，其特征在于，通過(guò)裝載在物體上的加速度傳感器測(cè)得所述重力加速度信號(hào)，以及通過(guò)裝載在物體上的磁強(qiáng)計(jì)測(cè)得所述地磁場(chǎng)信號(hào)。
3.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述重力加速度信號(hào)至少包括沿相互正交的三個(gè)測(cè)量軸測(cè)量的重力加速度測(cè)量值，所述地磁場(chǎng)信號(hào)至少包括沿相互正交的三個(gè)測(cè)量軸測(cè)量的地磁場(chǎng)測(cè)量值。
4.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述確定重力場(chǎng)表示與地磁場(chǎng)表示的步驟進(jìn)一步包括 基于所述物體坐標(biāo)系中的重力場(chǎng)表示確定所述物體坐標(biāo)系中的重力場(chǎng)表示與地球坐標(biāo)系中的重力場(chǎng)表示的對(duì)應(yīng)關(guān)系； 基于所述物體坐標(biāo)系中的地磁場(chǎng)表示確定所述物體坐標(biāo)系中的地磁場(chǎng)表示與地球坐標(biāo)系中的地磁場(chǎng)表示的對(duì)應(yīng)關(guān)系。
5.根據(jù)權(quán)利要求4所述的方法，其特征在于，所述地球坐標(biāo)系是東北天坐標(biāo)系，所述重力場(chǎng)表示為所述東北天坐標(biāo)系中的U軸，而所述地磁場(chǎng)在地平面的投影矢量表示為所述東北天坐標(biāo)系中的N軸。
6.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述計(jì)算坐標(biāo)系轉(zhuǎn)換參數(shù)的步驟進(jìn)一步包括 采用四元數(shù)參數(shù)計(jì)算所述坐標(biāo)系轉(zhuǎn)換參數(shù)。
7.根據(jù)權(quán)利要求6所述的方法，其特征在于，所述計(jì)算坐標(biāo)系轉(zhuǎn)換參數(shù)的步驟進(jìn)一步包括 對(duì)坐標(biāo)系轉(zhuǎn)換方程V’ = QVQ-1進(jìn)行求解以得到所述坐標(biāo)系轉(zhuǎn)換參數(shù)，其中，Q代表四元數(shù)形式的所述坐標(biāo)系轉(zhuǎn)換參數(shù)，V代表任一矢量在物體坐標(biāo)系中的表示，而V’代表所述任一矢量在地球坐標(biāo)系中的表不。
8.根據(jù)權(quán)利要求7所述的方法，其特征在于，所述求解坐標(biāo)系轉(zhuǎn)換方程進(jìn)一步包括 基于所述物體坐標(biāo)系中的重力場(chǎng)表示確定所述物體坐標(biāo)系中的重力場(chǎng)表示與地球坐標(biāo)系中的重力場(chǎng)表示的對(duì)應(yīng)關(guān)系； 基于所述物體坐標(biāo)系中的地磁場(chǎng)表示確定所述物體坐標(biāo)系中的地磁場(chǎng)表示與地球坐標(biāo)系中的地磁場(chǎng)表示的對(duì)應(yīng)關(guān)系；以及 將所述對(duì)應(yīng)關(guān)系代入所述坐標(biāo)系轉(zhuǎn)換方程以求解所述坐標(biāo)系轉(zhuǎn)換參數(shù)。
9.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述基于所述坐標(biāo)系轉(zhuǎn)換參數(shù)確定物體的姿態(tài)的步驟還包括 基于所述坐標(biāo)系轉(zhuǎn)換參數(shù)顯示所述物體。
10.一種用于確定物體姿態(tài)的裝置，包括 接收模塊，用于接收重力加速度信號(hào)以及地磁場(chǎng)信號(hào)，所述重力加速度信號(hào)與所述地磁場(chǎng)信號(hào)響應(yīng)于物體的姿態(tài)的變化而變化； 第一確定模塊，用于基于所述重力加速度信號(hào)確定物體坐標(biāo)系中的重力場(chǎng)表示，基于所述地磁場(chǎng)信號(hào)確定物體坐標(biāo)系中的地磁場(chǎng)表; 轉(zhuǎn)換模塊，用于基于所述物體坐標(biāo)系中的所述重力場(chǎng)表示與所述地磁場(chǎng)表示計(jì)算所述物體坐標(biāo)系與地球坐標(biāo)系之間的坐標(biāo)系轉(zhuǎn)換參數(shù)； 第二確定模塊，用于基于所述坐標(biāo)系轉(zhuǎn)換參數(shù)確定所述物體的姿態(tài)。
11.根據(jù)權(quán)利要求10所述的裝置，其特征在于，還包括 加速度傳感器，其裝載在所述物體上，用于測(cè)量所述重力加速度并生成所述重力加速度信號(hào)； 磁強(qiáng)計(jì)，其裝載在所述物體上，用于測(cè)量地磁場(chǎng)并生成所述地磁場(chǎng)信號(hào)。
12.根據(jù)權(quán)利要求10所述的裝置，其特征在于，所述重力加速度信號(hào)至少包括沿相互正交的三個(gè)測(cè)量軸測(cè)量的重力加速度測(cè)量值，所述地磁場(chǎng)信號(hào)至少包括沿相互正交的三個(gè)測(cè)量軸測(cè)量的地磁場(chǎng)測(cè)量值。
13.根據(jù)權(quán)利要求10所述的裝置，其特征在于，所述第一確定模塊進(jìn)一步用于 基于所述物體坐標(biāo)系中的重力場(chǎng)表示確定所述物體坐標(biāo)系中的重力場(chǎng)表示與地球坐標(biāo)系中的重力場(chǎng)表示的對(duì)應(yīng)關(guān)系； 基于所述物體坐標(biāo)系中的地磁場(chǎng)表示確定所述物體坐標(biāo)系中的地磁場(chǎng)表示與地球坐標(biāo)系中的地磁場(chǎng)表示的對(duì)應(yīng)關(guān)系。
14.根據(jù)權(quán)利要求13所述的裝置，其特征在于，所述地球坐標(biāo)系是東北天坐標(biāo)系，所述重力場(chǎng)表示為所述東北天坐標(biāo)系中的U軸，而所述地磁場(chǎng)在地平面的投影矢量表示為所述東北天坐標(biāo)系中的N軸。
15.根據(jù)權(quán)利要求10所述的裝置，其特征在于，所述轉(zhuǎn)換模塊進(jìn)一步用于 采用四元數(shù)參數(shù)計(jì)算所述坐標(biāo)系轉(zhuǎn)換參數(shù)。
16.根據(jù)權(quán)利要求15所述的裝置，其特征在于，所述轉(zhuǎn)換模塊進(jìn)一步包括方程求解器 所述方程求解器用于對(duì)坐標(biāo)系轉(zhuǎn)換方程V’ = QVQ-1進(jìn)行求解以得到所述坐標(biāo)系轉(zhuǎn)換參數(shù)，其中，Q代表四元數(shù)形式的所述坐標(biāo)系轉(zhuǎn)換參數(shù)，V代表任一矢量在物體坐標(biāo)系中的表示，而V’代表所述任一矢量在地球坐標(biāo)系中的表示。
17.根據(jù)權(quán)利要求16所述的裝置，其特征在于，所述第一確定模塊進(jìn)一步用于 基于所述物體坐標(biāo)系中的重力場(chǎng)表示確定所述物體坐標(biāo)系中的重力場(chǎng)表示與地球坐標(biāo)系中的重力場(chǎng)表示的對(duì)應(yīng)關(guān)系； 基于所述物體坐標(biāo)系中的地磁場(chǎng)表示確定所述物體坐標(biāo)系中的地磁場(chǎng)表示與地球坐標(biāo)系中的地磁場(chǎng)表示的對(duì)應(yīng)關(guān)系； 以及所述方程求解器進(jìn)一步用于接收所述對(duì)應(yīng)關(guān)系并將所述對(duì)應(yīng)關(guān)系代入所述坐標(biāo)系轉(zhuǎn)換方程以求解所述坐標(biāo)系轉(zhuǎn)換參數(shù)。
18.根據(jù)權(quán)利要求10所述的裝置，其特征在于，還包括 顯示模塊，用于基于所述坐標(biāo)系轉(zhuǎn)換參數(shù)顯示所述物體。
19.一種姿態(tài)定向參考系統(tǒng),包括權(quán)利要求10-18中任一項(xiàng)所述的裝置。
全文摘要
本發(fā)明公開(kāi)了一種用于確定物體姿態(tài)的方法及裝置。該用于確定物體姿態(tài)的方法包括接收重力加速度信號(hào)以及地磁場(chǎng)信號(hào)，所述重力加速度信號(hào)與所述地磁場(chǎng)信號(hào)響應(yīng)于物體的姿態(tài)的變化而變化；基于所述重力加速度信號(hào)確定物體坐標(biāo)系中的重力場(chǎng)表示，基于所述地磁場(chǎng)信號(hào)確定物體坐標(biāo)系中的地磁場(chǎng)表示；基于所述物體坐標(biāo)系中的所述重力場(chǎng)表示與所述地磁場(chǎng)表示計(jì)算所述物體坐標(biāo)系與地球坐標(biāo)系之間的坐標(biāo)系轉(zhuǎn)換參數(shù)；基于所述坐標(biāo)系轉(zhuǎn)換參數(shù)確定所述物體的姿態(tài)。
文檔編號(hào)G01C21/12GK102997913SQ20111028143
公開(kāi)日2013年3月27日 申請(qǐng)日期2011年9月14日 優(yōu)先權(quán)日2011年9月14日
發(fā)明者涂仲軒 申請(qǐng)人:意法半導(dǎo)體(中國(guó))投資有限公司