程國(guó)勝

(中國(guó)船舶重工集團(tuán)公司第七一〇研究所,湖北 宜昌 443003)

0 引言

對(duì)矢量物理量的測(cè)量,如水中目標(biāo)的磁場(chǎng)、電場(chǎng)等目標(biāo)特性的測(cè)量,要求測(cè)量出該物理量在確定的正交坐標(biāo)系(通常為當(dāng)?shù)厮阶鴺?biāo)系或者導(dǎo)航坐標(biāo)系)中的3個(gè)方向分量,而在測(cè)量實(shí)施中,測(cè)量傳感器或其安裝平臺(tái)的姿態(tài)角往往是隨機(jī)但可以測(cè)量的。因此,在測(cè)量數(shù)據(jù)的處理中,需要對(duì)在任意姿態(tài)下測(cè)量的矢量物理量數(shù)據(jù)進(jìn)行修正,即對(duì)測(cè)量的矢量結(jié)果在不同坐標(biāo)系之間進(jìn)行轉(zhuǎn)換。在大多數(shù)現(xiàn)有文獻(xiàn)和工程實(shí)踐中,修正算法都是直接引用歐拉角旋轉(zhuǎn)變換公式[1-3]。但是仔細(xì)分析,歐拉角旋轉(zhuǎn)變換公式是基于有先后次序的繞坐標(biāo)軸的旋轉(zhuǎn)變換,而實(shí)際測(cè)量的姿態(tài)角,有陀螺儀測(cè)量、慣導(dǎo)系統(tǒng)測(cè)量、傾斜儀測(cè)量、磁場(chǎng)傳感器測(cè)量以及多傳感器組合測(cè)量,測(cè)量傳感器或設(shè)備輸出的姿態(tài)角有不同的定義,與旋轉(zhuǎn)歐拉角的定義不一定完全一致。因此,直接引用歐拉角旋轉(zhuǎn)變換公式進(jìn)行修正計(jì)算,可能存在原理性誤差。在運(yùn)動(dòng)載體中的矢量測(cè)量以及導(dǎo)航定位中的坐標(biāo)旋轉(zhuǎn)變換中也都存在類似的問(wèn)題。

本文對(duì)姿態(tài)角定義和傳感器測(cè)量原理進(jìn)行分析,推導(dǎo)姿態(tài)角與旋轉(zhuǎn)歐拉角的關(guān)系,進(jìn)而推導(dǎo)出利用可測(cè)量的姿態(tài)角進(jìn)行坐標(biāo)變換修正的坐標(biāo)轉(zhuǎn)換矩陣。

1 坐標(biāo)系及歐拉旋轉(zhuǎn)變換

1.1 常用坐標(biāo)系

在導(dǎo)航和矢量測(cè)量中常用的有以下5種正交右手坐標(biāo)系[4]:

1)地心慣性坐標(biāo)系OIXIYIZI,簡(jiǎn)記為I坐標(biāo)系;

2)地心地球固定坐標(biāo)系OEXEYEZE,簡(jiǎn)記為E坐標(biāo)系;

3)導(dǎo)航坐標(biāo)系ONXNYNZN,簡(jiǎn)記為N坐標(biāo)系,主要用于確定載體相對(duì)于東、北和天方向的速度和位置;

4)當(dāng)?shù)厮阶鴺?biāo)系OLXLYLZL,簡(jiǎn)記為L(zhǎng)坐標(biāo)系,主要用于確定載體相對(duì)于北、東和地方向的姿態(tài);

5)載體坐標(biāo)系OBXBYBZB,簡(jiǎn)記為B坐標(biāo)系,主要用于描述載體的姿態(tài)(通常為俯仰角、橫滾角和偏航角)。

導(dǎo)航應(yīng)用中的測(cè)量計(jì)算如姿態(tài)、位置、速度、加速度、角速度等參數(shù),常常需要在這些不同坐標(biāo)系之間進(jìn)行平移和旋轉(zhuǎn)變換計(jì)算,而一般矢量物理量的測(cè)量中,最常用到的是其中的B坐標(biāo)系以及N和L坐標(biāo)系間的變換計(jì)算。

1.2 歐拉角

如圖1(a)所示,固定坐標(biāo)系OXgYgZg(通常為L(zhǎng)坐標(biāo)系)和載體坐標(biāo)系OXYZ有共同原點(diǎn),ON為OXgYg平面和OXY平面的相交線,ON與平面ZgOZ垂直,稱為節(jié)線。

由坐標(biāo)系OXgYgZg到OXYZ的轉(zhuǎn)換步驟為:以軸OZg和OZ為基本軸,其垂直面OXgYg和OXY為基本平面。首先,在坐標(biāo)系OXgYgZg中繞OZg軸逆時(shí)針?lè)较?從OZg軸正向看)旋轉(zhuǎn)角度α,OXg軸到達(dá)ON;再繞ON軸逆時(shí)針旋轉(zhuǎn)角度β,OZg軸到達(dá)OZ;最后繞OZ軸逆時(shí)針旋轉(zhuǎn)角度γ,ON軸到達(dá)OX,得到坐標(biāo)系OXYZ。這種定義對(duì)應(yīng)的3個(gè)角度(α,β,γ)稱為歐拉角,其名稱來(lái)源于天文學(xué),最早由歐拉在1776年提出,其中β稱為章動(dòng)角,α稱為進(jìn)動(dòng)角,γ稱為自轉(zhuǎn)角[5]。

圖1 兩種不同順序歐拉角定義示意圖Fig.1 Schematic diagram of definition of Euler angles in 2 different orders

實(shí)際上,對(duì)歐拉角的名稱、符號(hào)以及旋轉(zhuǎn)順序并沒(méi)有統(tǒng)一的規(guī)定,使用時(shí)必須先明確定義。圖2(a)是按照Z(yǔ)軸-X軸-Z軸依次旋轉(zhuǎn)的順序,稱為ZXZ順序,也稱為313順序,或者按照中間字母X區(qū)分稱為X順規(guī)(實(shí)際上X順規(guī)有4種不同形式);對(duì)應(yīng)定義的歐拉角可以稱為Z-X-Z順序歐拉角。合乎規(guī)則的順規(guī)共有12種,分別是:ZXZ,ZXY,YXZ,YXY,XYX,XYZ,ZYX,ZYZ,XZX,XZY,YZX,YZY。歐拉角及其旋轉(zhuǎn)變換作為一種純粹的數(shù)學(xué)概念,12種合乎規(guī)則的順規(guī)都是可用的,不同順規(guī)對(duì)應(yīng)不同的歐拉角和不同的旋轉(zhuǎn)變換矩陣。

1.3 歐拉角旋轉(zhuǎn)變換

在慣性導(dǎo)航等運(yùn)動(dòng)分析中,常常使用Z-Y-X順序定義歐拉角,實(shí)際上不同的旋轉(zhuǎn)順序?qū)?yīng)不同的旋轉(zhuǎn)變換矩陣和旋轉(zhuǎn)結(jié)果,一種旋轉(zhuǎn)結(jié)果可以對(duì)應(yīng)許多組不同的旋轉(zhuǎn)歐拉角。為了與常用的姿態(tài)角定義和測(cè)量計(jì)算方法一致,這里選擇Z-Y-X順序歐拉角(ψ,θ,φ)對(duì)歐拉角旋轉(zhuǎn)變換進(jìn)行分析。

如圖1(b)所示,固定坐標(biāo)系OXgYgZg(一般是L坐標(biāo)系中的NED坐標(biāo)系)和載體坐標(biāo)系OXBYBZB有共同原點(diǎn)O,OXgYgZg依次經(jīng)過(guò)Z、Y、X軸3次連續(xù)旋轉(zhuǎn),得到OXBYBZB,具體旋轉(zhuǎn)過(guò)程及其坐標(biāo)變換如下:

首先,坐標(biāo)系繞Z軸旋轉(zhuǎn),旋轉(zhuǎn)角度ψ為正(沿Z軸正方向看,逆時(shí)針?lè)较蛐D(zhuǎn)ψ角度定義為正角度,下同),由OXgYgZg旋轉(zhuǎn)到OX1Y1Z1,此時(shí)OZ1軸與OZg軸重合;

第二步,坐標(biāo)系繞Y軸旋轉(zhuǎn)正角度θ,由OX1Y1Z1旋轉(zhuǎn)到OX2Y2Z2,此時(shí)OY1軸與OY2軸重合;

第三步,坐標(biāo)系繞X軸旋轉(zhuǎn)正角度φ,由OX2Y2Z2旋轉(zhuǎn)到OXBYBZB。

在上述每一步變換中,按照同一矢量在2個(gè)不同坐標(biāo)系中的正交分解進(jìn)行計(jì)算,求解同一矢量在不同坐標(biāo)系中的量值關(guān)系,最后得到:

式中:Xg=[xg,yg,zg]T和XB=[xB,yB,zB]T分別是同一矢量X在OXgYgZg和OXBYBZB坐標(biāo)系中的3個(gè)正交分量;是由OXgYgZg旋轉(zhuǎn)到OXBYBZB的方向余弦矩陣;CgB是由OXBYBZB旋轉(zhuǎn)到OXgYgZg的方向余弦矩陣,它們是互逆的正交矩陣,因此,

方向余弦矩陣的具體表達(dá)式如下:

應(yīng)用到矢量物理量的測(cè)量變換中,姿態(tài)測(cè)量傳感器固定在載體坐標(biāo)系中,該坐標(biāo)系從固定坐標(biāo)系OXgYgZg以Z-Y-X順序經(jīng)歐拉角(ψ,θ,φ)旋轉(zhuǎn)到OXBYBZB,在載體坐標(biāo)系OXBYBZB中的測(cè)量結(jié)果為XB。如果姿態(tài)測(cè)量傳感器測(cè)量得到的角度值是前述歐拉角(ψ,θ,φ),使用上面的公式,對(duì)在載體坐標(biāo)系中的傳感器測(cè)量值XB進(jìn)行修正,可以求出所測(cè)量的矢量值在固定坐標(biāo)系OXgYgZg中的3個(gè)分量,即求出Xg。

1.4 無(wú)限小角度轉(zhuǎn)動(dòng)

在上述Z-Y-X順序歐拉旋轉(zhuǎn)中,如果轉(zhuǎn)動(dòng)歐拉角(ψ,θ,φ)為無(wú)限小角度值,則在以歐拉角形式表示的方向余弦矩陣中有以下近似:

忽略二階及以上乘積項(xiàng)如sinθsinφ、sinψsinφ、sinψsinθsinφ等小角度乘積項(xiàng),則方向余弦矩陣可以近似簡(jiǎn)化為

可以看出,當(dāng)轉(zhuǎn)動(dòng)歐拉角為無(wú)限小角度值時(shí),以歐拉角形式表示的方向余弦矩陣均為對(duì)稱矩陣,矩陣乘法的順序不影響乘法結(jié)果。也就是說(shuō),當(dāng)轉(zhuǎn)動(dòng)歐拉角為無(wú)限小角度值時(shí),將轉(zhuǎn)動(dòng)分解為3次連續(xù)歐拉旋轉(zhuǎn),結(jié)果與旋轉(zhuǎn)順序無(wú)關(guān)。在慣性導(dǎo)航等設(shè)備的應(yīng)用中,設(shè)備測(cè)量載體的姿態(tài)角度時(shí),量化速率比較高,相當(dāng)于每次疊加計(jì)算所對(duì)應(yīng)的轉(zhuǎn)動(dòng)角度均為無(wú)限小角度,可以采用上述對(duì)稱矩陣形式的方向余弦矩陣進(jìn)行近似求解,大大簡(jiǎn)化求解過(guò)程,提高計(jì)算速度[5]。

2 姿態(tài)角及其坐標(biāo)變換

2.1 姿態(tài)角

姿態(tài)角也是一組能夠完整表述剛體相對(duì)于參照坐標(biāo)系轉(zhuǎn)動(dòng)位置的參量,包括偏航角、俯仰角和滾轉(zhuǎn)角,其中的俯仰角和滾轉(zhuǎn)角通常為傾斜角。姿態(tài)角的參照固定坐標(biāo)系通常為L(zhǎng)坐標(biāo)系中的北東地(NED)坐標(biāo)系[6]。

圖2 載體姿態(tài)角定義示意圖Fig.2 Schematic diagram of definition of carrier's attitude

按歐拉角定義方法來(lái)定義固定坐標(biāo)系和載體坐標(biāo)系,在載體坐標(biāo)系OXYZ中,OX軸相對(duì)于固定坐標(biāo)系OgXgYgZg的水平面以及OgXg(正北方向)、OY軸相對(duì)于OgXgYgZg的水平面,存在3個(gè)偏角,它們反映了載體的姿態(tài),稱為載體的姿態(tài)角,通常用(ψ,θ,φ)表示,參見(jiàn)圖2,具體定義如下。

偏航角ψ:載體坐標(biāo)系X軸在水平面上投影與固定坐標(biāo)系Xg軸之間的夾角,由載體X軸的投影線逆時(shí)針轉(zhuǎn)至Xg軸時(shí),偏航角為正,即機(jī)頭右偏航為正,反之為負(fù)。偏航角通常也稱為方位角或航向角。

俯仰角θ:載體坐標(biāo)系X軸與水平面的夾角。當(dāng)X軸的正半軸位于過(guò)坐標(biāo)原點(diǎn)的水平面之上(抬頭)時(shí),俯仰角為正,否則為負(fù)。

滾轉(zhuǎn)角φ:有2種定義,第一種定義為載體坐標(biāo)系Z軸與包含載體X軸的鉛垂面間的夾角,用φz表示,第二種定義為載體坐標(biāo)系Y軸與水平面的夾角,用φy表示;載體向右滾為正,反之為負(fù)。滾轉(zhuǎn)角的兩種定義在實(shí)際工程計(jì)算中都有人使用。

旋轉(zhuǎn)歐拉角主要是一種數(shù)學(xué)上的概念,姿態(tài)角的定義更具物理意義,容易通過(guò)測(cè)角傳感器測(cè)量得到。但是,由于各種測(cè)角傳感器的原理各異,具體使用時(shí),需要明確傳感器輸出角度值的物理意義,以及滾轉(zhuǎn)角的具體定義,準(zhǔn)確使用姿態(tài)修正變換公式。

2.2 姿態(tài)角坐標(biāo)變換中的問(wèn)題

前面的歐拉旋轉(zhuǎn)變換公式是基于3個(gè)歐拉角及其旋轉(zhuǎn)順序推導(dǎo)的,不同的旋轉(zhuǎn)順序?qū)?yīng)不同的變換系數(shù)矩陣,它表明即使是相同的旋轉(zhuǎn)角度,不同的旋轉(zhuǎn)順序?qū)?yīng)的最終旋轉(zhuǎn)結(jié)果也是不同的。

從歐拉角和姿態(tài)角的定義看,歐拉角和姿態(tài)角是不同的角度,因此,在矢量物理量測(cè)量及其坐標(biāo)變換修正中,應(yīng)仔細(xì)考慮角度測(cè)量傳感器所測(cè)量角度的實(shí)際物理意義;否則直接將姿態(tài)角測(cè)量值當(dāng)作旋轉(zhuǎn)歐拉角,應(yīng)用前述歐拉角旋轉(zhuǎn)變換公式(1)和(2)進(jìn)行坐標(biāo)變換修正計(jì)算,可能存在原理性誤差。

在實(shí)際工程應(yīng)用中,角度測(cè)量傳感器一般都是基于載體坐標(biāo)系的,通過(guò)與載體固定連接,實(shí)時(shí)測(cè)量載體的姿態(tài)角度。各種不同的姿態(tài)角度測(cè)量傳感器,往往原理不同,性能特點(diǎn)各異,適用的環(huán)境條件各不一樣。常用的測(cè)角傳感器有加速度計(jì)、磁力計(jì)、陀螺儀以及以陀螺儀為核心的慣導(dǎo)系統(tǒng)(包括平臺(tái)式慣導(dǎo)系統(tǒng)和捷聯(lián)慣導(dǎo)系統(tǒng))等。

2.3 由加速度計(jì)求解傾斜角

加速度計(jì)也稱重力感應(yīng)器或力感應(yīng)器,主要是通過(guò)測(cè)量組件在某個(gè)軸向的受力情況來(lái)計(jì)算軸向加速度的大小和方向。由于重力加速度只是地表垂直方向加速度,因此,借助三軸加速度計(jì)可以測(cè)得一個(gè)固定平臺(tái)相對(duì)地球表面的運(yùn)動(dòng)方向[7]。

通常加速度計(jì)的測(cè)量值包含了重力加速度和其它受力運(yùn)動(dòng)產(chǎn)生的加速度,而在靜態(tài)、勻速運(yùn)動(dòng)或接近勻速運(yùn)動(dòng)的低速(即維持慣性)條件下,可以認(rèn)為或近似認(rèn)為只有重力而無(wú)其它作用力,測(cè)量的三軸加速度值認(rèn)為是重力加速度產(chǎn)生的,由三軸加速度測(cè)量值可以求解載體的傾斜角度(姿態(tài)角中的2個(gè)量)。

實(shí)際上,以下2種求解過(guò)程是互為逆運(yùn)算關(guān)系:1)已知固定坐標(biāo)系下重力加速度的3個(gè)分量,通過(guò)加速度計(jì)測(cè)量載體坐標(biāo)系下的重力加速度,求解載體的傾斜角度;2)已知或者測(cè)量獲得載體坐標(biāo)系的傾斜角度,通過(guò)測(cè)量載體坐標(biāo)系下的重力加速度的3個(gè)分量,求解固定坐標(biāo)系下的重力加速度值。

圖3 加速度計(jì)測(cè)量計(jì)算姿態(tài)角的原理示意圖Fig.3 Principle diagram of measuring attitude angles by accelerometer

下面來(lái)推導(dǎo)由加速度計(jì)求解傾斜角度的具體求解過(guò)程。

如圖3所示,OXgYgZg為固定坐標(biāo)系,OXYZ為載體坐標(biāo)系,三分量加速度計(jì)固聯(lián)在載體坐標(biāo)系的坐標(biāo)軸上。圖中將水平面OXgYg旋轉(zhuǎn)到了鉛錘面上。

在OXgYgZg坐標(biāo)系中,重力加速度的值為G=[Gx,Gy,Gz]T=[0,0,g]T,g為重力加速度常數(shù);在OXYZ坐標(biāo)系中,加速度計(jì)的測(cè)量值為g=[gx,gy,gz]T。

我們假定由坐標(biāo)系OXgYgZg到坐標(biāo)系OXYZ是經(jīng)過(guò)Y-Z順序2個(gè)歐拉旋轉(zhuǎn)實(shí)現(xiàn)的,旋轉(zhuǎn)的具體步驟:

1)在OXgYgZg坐標(biāo)系內(nèi),繞OYg軸(順螺旋方向,即逆時(shí)針?lè)较?旋轉(zhuǎn)θ角,即OXg軸和OZg軸均在OXgZg平面內(nèi)旋轉(zhuǎn)了θ角度,得到坐標(biāo)系OX′Y′Z′,其中Y′與Yg重合;

2)在OX′Y′Z′坐標(biāo)系內(nèi),繞OX′軸(順螺旋方向,即逆時(shí)針?lè)较?旋轉(zhuǎn)φ角,即OY′軸和OZ′軸均在OY′Z′平面內(nèi)旋轉(zhuǎn)了φ角度,得到坐標(biāo)系OXYZ,其中X與X′重合。

至此,可以得到以下一些關(guān)系:

1)OX軸在OXgZg平面內(nèi),因此θ等于OX軸與水平面OXgYg的夾角,即目標(biāo)的俯仰角;

2)OY′與OYg重合,OYg⊥OXgZg平面,因此平面OY′Z′⊥OXgZg平面(即鉛錘面),OZ′為相交線;OY′、OY、OZ′、OZ在同一平面OY′Z′內(nèi),OY和OZ在OXgZg平面上的投影均為OZ′;因此,OZ軸與鉛錘面OXgZg的夾角等于φ,也就是滾轉(zhuǎn)角的第一種定義值φz=φ;

3)假設(shè)OY軸在水平面OXgYg上的投影為OP,OY與水平面OXgYg的夾角即為滾轉(zhuǎn)角的第二種定義值φy。用φy和φ兩個(gè)角度分別計(jì)算OY軸上的單位矢量在OZg上的投影分量,則有:sinφy=sinφcosθ。

根據(jù)以上關(guān)系,重力加速度在OXgYgZg坐標(biāo)系中的值G=[Gx,Gy,Gz]T=[0,0,g]T與加速度計(jì)的測(cè)量值g=[gx,gy,gz]T之間,滿足歐拉角旋轉(zhuǎn)變換關(guān)系,即:

由Gx=0和Gy=0可以得到:

由上式推導(dǎo)出:

所以,由加速度計(jì)求解傾斜角的計(jì)算方法:

1)在載體坐標(biāo)系中測(cè)量3個(gè)正交加速度計(jì)的值gx,gy,gz;

2)用上述2個(gè)式(4)和式(5)求解出θ和φ;

3)俯仰角為θ;

4)滾轉(zhuǎn)角φ的第一種定義值為φz=φ,第二種定義值φy由關(guān)系式sinφy=sinφcosθ求解;

由加速度計(jì)求解傾斜角適用于靜止、勻速或近似勻速運(yùn)動(dòng)的條件。在不考慮偏航角的情況下,可以通過(guò)測(cè)量?jī)A斜角對(duì)矢量測(cè)量值進(jìn)行傾斜修正。

假設(shè)在載體坐標(biāo)系下測(cè)量得到的矢量值為X=[x,y,z]T,載體坐標(biāo)系的傾斜角(θ,φ)由傳感器測(cè)量得到,則將X轉(zhuǎn)換到固定坐標(biāo)系下的值為Xg=[xg,yg,zg]T,按照公式(3),得到:

式中:θ為俯仰角測(cè)量值;滾轉(zhuǎn)角φ的傳感器測(cè)量值如果為第一種定義φz,則φ=φz,為第二種定義φy時(shí),則由sinφ=sinφy/cosθ計(jì)算得出φ。

2.4 由磁力計(jì)求解偏航角

磁力計(jì)也稱為磁感器,可用于測(cè)試磁場(chǎng)強(qiáng)度和方向,原理類似指南針。

在地球表面,地磁場(chǎng)始終是沿著磁感應(yīng)線(地磁場(chǎng)的磁力線)指向北的,某點(diǎn)的磁場(chǎng)強(qiáng)度方向?yàn)榇鸥袘?yīng)線在該點(diǎn)的切線方向,它在北向和垂直方向有相應(yīng)的分量,在東西方向的分量為0。因此,在固定坐標(biāo)系下,地磁場(chǎng)矢量可以表示為Mg=[Mxg,Myg,Mzg]T=[Mxg,0,Mzg]T,即Myg=0。如圖4所示,地磁場(chǎng)強(qiáng)度總量為Mg,其北向分量為Mxg=Mgcosδ,垂直分量為Mzg=Mgsinδ,東向分量為Myg=0。

圖4 地磁場(chǎng)及其三分量指向示意圖Fig.4 Schematic diagram of geomagnetic field and its tricomponent

由此可以看出,在載體目標(biāo)上安裝三軸磁力計(jì),測(cè)量出三軸地磁場(chǎng)強(qiáng)度M=[Mx,My,Mz]T,就可以計(jì)算出載體坐標(biāo)系的方位角(即偏航角)ψM。當(dāng)然,實(shí)際上地球的地理北極和地磁南極并不重合,有一定的夾角,稱為磁偏角Δψ。根據(jù)經(jīng)緯度可以查詢到該磁偏角Δψ的大小。固定坐標(biāo)系的北向指向地理北極,磁力計(jì)求得的北向指向地磁南極,因此,用磁力計(jì)求解的方位角ψM需要進(jìn)行磁偏角Δψ修正,才能得到地理方位,即真正的偏航角ψ[8]。

和加速度計(jì)的測(cè)量一樣,通常情況下,安裝磁力計(jì)的載體坐標(biāo)系也是處于運(yùn)動(dòng)狀態(tài),具有一定的傾斜角,因此,利用磁力計(jì)測(cè)量值求解方位時(shí),也需要進(jìn)行傾斜修正。

下面來(lái)推導(dǎo)由磁力計(jì)求解偏航角的具體求解過(guò)程。

假設(shè)在載體坐標(biāo)系下測(cè)量得到的地磁場(chǎng)為M=[Mx,My,Mz]T,載體坐標(biāo)系的傾斜角(θ,φ)通過(guò)加速度計(jì)求解得到,按照公式(6),可以將地磁場(chǎng)測(cè)量值M=[Mx,My,Mz]T進(jìn)行傾斜修正,得到相對(duì)于固定坐標(biāo)系OXgYgZg僅有偏航角ψM而無(wú)傾斜角的坐標(biāo)系下的值Mp=[Mxp,Myp,Mzp]T:

而Mp=[Mxp,Myp,Mzp]T是由地磁場(chǎng)在固定坐標(biāo)系下的值Mg=[Mx,0,Mzg]T經(jīng)過(guò)旋轉(zhuǎn)偏航角ψM得到的,而且,這個(gè)偏航角的旋轉(zhuǎn)在傾斜之前或之后,結(jié)果都是一樣的。因此有:

式中:θ為俯仰角測(cè)量值,φ=φz(當(dāng)滾轉(zhuǎn)角測(cè)量值為第一種定義值φz時(shí))或者由計(jì)算式sinφ=sinψy/cosθ計(jì)算得到(當(dāng)滾轉(zhuǎn)角測(cè)量值為第二種定義值φy時(shí))。

再考慮磁偏角Δψ的修正,相對(duì)于固定坐標(biāo)系的偏航角為:ψ=ψM+Δψ。

所以,通過(guò)測(cè)量或測(cè)量計(jì)算得到載體坐標(biāo)系的傾斜角(θ,φ),以及載體坐標(biāo)系下地磁場(chǎng)的值M=[Mx,My,Mz]T,可以求解偏航角ψM,再進(jìn)行磁偏角Δψ修正,就可以求得相對(duì)于固定坐標(biāo)系的偏航角ψ。最終得到載體的姿態(tài)角(ψ,θ,φ)。

2.5 姿態(tài)角的坐標(biāo)變換計(jì)算

綜合前面的分析,在載體坐標(biāo)系下測(cè)量某一矢量物理量時(shí),同步測(cè)量載體的姿態(tài)角,通過(guò)姿態(tài)角的坐標(biāo)變換,將載體坐標(biāo)系下的矢量測(cè)量結(jié)果變換到固定坐標(biāo)系。而姿態(tài)角與旋轉(zhuǎn)歐拉角在定義上是有區(qū)別的。因此,在姿態(tài)角的測(cè)量以及角度計(jì)算中,要明確角度輸出值的定義,特別是滾轉(zhuǎn)角的輸出值所對(duì)應(yīng)的定義,引用合適的變換矩陣,而不能簡(jiǎn)單直接地引用歐拉角旋轉(zhuǎn)變換公式。

一組姿態(tài)角對(duì)應(yīng)一種旋轉(zhuǎn)結(jié)果,但一種旋轉(zhuǎn)結(jié)果可以對(duì)應(yīng)多組旋轉(zhuǎn)歐拉角。下面以Z-Y-X旋轉(zhuǎn)順序?yàn)槔?在前述傾斜角修正和偏航角修正算法的基礎(chǔ)上,推導(dǎo)姿態(tài)角與Z-Y-X順序歐拉角之間的關(guān)系。

圖5為姿態(tài)角與Z-Y-X順序歐拉旋轉(zhuǎn)角的關(guān)系示意圖。圖中的旋轉(zhuǎn)步驟如下:

1)固定坐標(biāo)系為OXgYgZg,一般是L坐標(biāo)系(其中的NED坐標(biāo)系)。在坐標(biāo)系為OXgYgZg內(nèi),繞OZg軸(順螺旋方向,即逆時(shí)針?lè)较?旋轉(zhuǎn)ψ角,即OXg軸和OYg軸均在OXgYg平面(通常為水平面)內(nèi)旋轉(zhuǎn)了ψ角度,得到坐標(biāo)系OX1Y1Z1,其中OZ1軸與OZg軸重合,OX1軸和OY1軸均在OXgYg平面內(nèi);

2)在OX1Y1Z1坐標(biāo)系內(nèi),繞OY1軸(順螺旋方向,即逆時(shí)針?lè)较?旋轉(zhuǎn)θ角,即OX1軸和OZ1軸均在鉛錘面OX1Zg內(nèi)旋轉(zhuǎn)了θ角度,得到坐標(biāo)系OX2Y2Z2,其中OY2軸與OY1軸重合,OX2軸和OZ2軸均在鉛錘面OX1Zg內(nèi),OX2軸在水平面OXgYg上的投影為OX1軸;

3)在OX2Y2Z2坐標(biāo)系內(nèi),繞OX2軸(順螺旋方向,即逆時(shí)針?lè)较?旋轉(zhuǎn)φ角,得到坐標(biāo)系OX3Y3Z3,其中OX3軸在與OX2軸重合,OZ3軸在鉛錘面OX1Zg上的投影為OZ2軸,OZ3軸與鉛錘面OX1Zg的夾角即是滾轉(zhuǎn)角的第一種定義,記為φz,顯然有φz=φ;而OY3軸與水平面OXgYg的夾角記為φy。

圖5 姿態(tài)角與Z-Y-X順序歐拉旋轉(zhuǎn)角的關(guān)系示意圖Fig.5 Schematic diagram of relation between attitude angles and Z-Y-X order

從坐標(biāo)系OXgYgZg到OX3Y3Z3,根據(jù)姿態(tài)角的定義得到:ψ為偏航角,θ為俯仰角,φ為橫滾角第一種定義值,即φz=φ,下面來(lái)推導(dǎo)滾轉(zhuǎn)角第二種定義值φy的表達(dá)式。

假設(shè)OY3軸上有一個(gè)單位矢量uy3,在坐標(biāo)系OXgYgZg上進(jìn)行正交分解,它在OZg軸上的分量為sinψy;

如果在坐標(biāo)系OX2Y2Z2上對(duì)uy3進(jìn)行正交分解,它在OZ2軸上的分量為sinψy,在OY2軸上的分量為cosφ,在OX2軸上的分量為0;OY2軸上的分量為cosφ在坐標(biāo)系OXgYgZg上進(jìn)行正交分解,由于OY2在水平面OXgYg上,因此cosφ在OZg軸上分量為0;OZ2軸上的分量為sinφ在坐標(biāo)系OXgYgZg上進(jìn)行正交分解,它在OZg軸上分量為sinψcosθ。

OY3軸上的單位矢量uy3,用兩種不同的正交分解方法求解在OZg軸上的分量,結(jié)果應(yīng)該相等,因此有:sinψy=sinψcosθ。

由此得出姿態(tài)角與Z-Y-X順序歐拉角的關(guān)系為:歐拉角ψ等于偏航角;歐拉角θ等于俯仰角;歐拉角φ與2種定義橫滾角的關(guān)系:φz=φ,sinψy=sinψcosθ。

因此,矢量物理量的姿態(tài)變換計(jì)算方法:假設(shè)載體坐標(biāo)系下的矢量測(cè)量值為X=[x,y,z]T,載體坐標(biāo)系的姿態(tài)角測(cè)量值為(ψ,θ,φ),則矢量X在固定坐標(biāo)系下的修正結(jié)果Xg=[xg,yg,zg]T為

式中:ψ為偏航角測(cè)量值,如果偏航角是通過(guò)磁力計(jì)測(cè)量的,需要進(jìn)行磁偏角Δψ修正;θ為俯仰角測(cè)量值;當(dāng)滾轉(zhuǎn)角的測(cè)量值為第一種定義值φz時(shí),φ=φz;當(dāng)滾轉(zhuǎn)角測(cè)量值為第二種定義值φy時(shí),由計(jì)算式sinφ=sinφy/cosθ計(jì)算得到φ。

姿態(tài)角的取值范圍為:ψ,φ∈[-π,π],θ∈[-π/2,π/2]。

2.6 陀螺儀和慣導(dǎo)系統(tǒng)測(cè)量姿態(tài)角的比較

陀螺儀也稱作地感器,由陀螺構(gòu)成的慣導(dǎo)系統(tǒng),無(wú)論是傳統(tǒng)的機(jī)械轉(zhuǎn)子陀螺平臺(tái)式慣導(dǎo)系統(tǒng),還是新型光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng),都可以求解運(yùn)動(dòng)目標(biāo)的瞬時(shí)速度、位置以及姿態(tài)角度[4,8]。

在傳統(tǒng)的機(jī)械轉(zhuǎn)子陀螺和平臺(tái)式慣導(dǎo)系統(tǒng)中,經(jīng)過(guò)初始對(duì)準(zhǔn)后,輸出的傾斜角是載體坐標(biāo)系x、y軸相對(duì)于基準(zhǔn)水平面的角度,而偏航角則是相對(duì)于正北方向的。俯仰角和偏航角與姿態(tài)角中的定義完全一致,滾轉(zhuǎn)角則與姿態(tài)角中的第二種定義一致,使用變換計(jì)算式(10)時(shí),由計(jì)算得到φ。

而在捷聯(lián)慣導(dǎo)系統(tǒng)中,使用虛擬的數(shù)學(xué)平臺(tái)代替了物理平臺(tái),陀螺測(cè)量的是載體相對(duì)于慣性坐標(biāo)系的轉(zhuǎn)動(dòng)角速度,然后通過(guò)計(jì)算得到載體的姿態(tài)角,具體計(jì)算過(guò)程為

1)陀螺儀測(cè)量,運(yùn)用地球參數(shù)和位置參數(shù)計(jì)算;

3)用姿態(tài)四元數(shù)的元素表示姿態(tài)矩陣;

4)根據(jù)余弦矩陣和歐拉角定義,用歐拉角表示的姿態(tài)矩陣形式求解歐拉角(ψ,θ,φ)。

由上述導(dǎo)航解算原理可知,捷聯(lián)慣導(dǎo)系統(tǒng)實(shí)際給出的是Z-Y-X順序歐拉角,其中偏航角輸出已經(jīng)修正為正北航向,俯仰角和滾轉(zhuǎn)角經(jīng)過(guò)象限修正為歐拉角真值,滾轉(zhuǎn)角等同于姿態(tài)角中的第一種定義φz。坐標(biāo)修正變換可以直接使用變換計(jì)算式(10),其中φ=φz。

3 結(jié)束語(yǔ)

在實(shí)施任意姿態(tài)(也就是載體坐標(biāo)系)下矢量物理量的測(cè)量時(shí),一般需要同步測(cè)量載體的姿態(tài)角,然后通過(guò)坐標(biāo)變換公式對(duì)矢量測(cè)量值進(jìn)行修正,求解出固定坐標(biāo)系下的矢量值。

以測(cè)量水面艦船目標(biāo)的磁場(chǎng)為例,測(cè)量時(shí),目標(biāo)在水面上,測(cè)量傳感器沉底布放,傳感器的姿態(tài)(包括方位)與確定的固定坐標(biāo)系不平行,即存在一個(gè)傳感器坐標(biāo)系,其姿態(tài)角可以測(cè)量,因此,對(duì)傳感器的測(cè)量結(jié)果,應(yīng)該進(jìn)行姿態(tài)角變換修正。

很多文獻(xiàn)和工程實(shí)踐中,無(wú)論是基于陀螺測(cè)量的姿態(tài)角還是其它傳感器測(cè)量的傾斜角,往往直接引用旋轉(zhuǎn)歐拉角坐標(biāo)變換公式,進(jìn)行測(cè)量數(shù)據(jù)修正和坐標(biāo)變換計(jì)算。從前面的分析可以看出,各種不同的傳感器測(cè)量的姿態(tài)角,其輸出值所代表的物理意義不一定完全相同,特別是滾轉(zhuǎn)角存在兩種不同的定義,如果傳感器的滾轉(zhuǎn)角輸出值物理意義不同于歐拉角定義,直接引用旋轉(zhuǎn)歐拉角坐標(biāo)變換公式,就會(huì)產(chǎn)生原理性誤差。

正確的做法是:分析姿態(tài)角傳感器的工作原理,明確姿態(tài)角測(cè)量值的定義,確定姿態(tài)角測(cè)量值與某種歐拉角(通常采用Z-Y-X順序歐拉角)的數(shù)值關(guān)系,再引用歐拉角修正變換計(jì)算式。

如果滾轉(zhuǎn)角輸出值對(duì)應(yīng)第一種定義,即載體坐標(biāo)系Z軸與包含載體X軸的鉛垂面間的夾角,則與旋轉(zhuǎn)歐拉角定義一致,可以直接引用旋轉(zhuǎn)歐拉角坐標(biāo)變換公式;如果滾轉(zhuǎn)角輸出值對(duì)應(yīng)第二種定義,即載體坐標(biāo)系Y軸與水平面的夾角,則與旋轉(zhuǎn)歐拉角定義不一致,不能直接引用旋轉(zhuǎn)歐拉角坐標(biāo)變換公式。

另外,在工程應(yīng)用中,通常在靜態(tài)或接近靜態(tài)條件下,使用低成本的傾斜儀測(cè)量?jī)A斜角,使用磁方位儀測(cè)量方位角,滾轉(zhuǎn)角測(cè)量值為第二種定義值,在坐標(biāo)變換修正計(jì)算中應(yīng)用前述公式(10)時(shí),由計(jì)算φ。在運(yùn)動(dòng)狀態(tài)下,一般使用陀螺儀或基于陀螺儀的慣導(dǎo)系統(tǒng)測(cè)量姿態(tài)角。如果是傳統(tǒng)的機(jī)械轉(zhuǎn)子陀螺或者平臺(tái)式慣導(dǎo)系統(tǒng),姿態(tài)角測(cè)量值與傾斜儀+磁方位儀相同,滾轉(zhuǎn)角測(cè)量值為第二種定義值,坐標(biāo)變換修正計(jì)算中應(yīng)用前述公式(10)時(shí),由計(jì)算φ;現(xiàn)在更廣泛使用的是光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng),姿態(tài)角測(cè)量值一般為Z-Y-X順序歐拉角,滾轉(zhuǎn)角測(cè)量值為第一種定義值φz,在坐標(biāo)變換修正計(jì)算中應(yīng)用前述公式(10)時(shí),φ=φz。