(完整版)慣導(dǎo)與GPS坐標(biāo)轉(zhuǎn)換問題

1、坐標(biāo)系問題常用坐標(biāo)系1 .慣性坐標(biāo)系（i系）對牛頓運動定律適用的參考系；研究物體時，選取靜止或作勻速直線運動的參考 系，牛頓定律才能成立。常用慣性坐標(biāo)系：日心慣性坐標(biāo)系、地心慣性坐標(biāo)系等。2 .地球坐標(biāo)系（e系）即周連在地球上的坐標(biāo)系，該坐標(biāo)系隨地球一起轉(zhuǎn)動。地球坐標(biāo)系原點在地球中心，軸沿地球自轉(zhuǎn)軸方向，x 軸在赤道平面與本初子午線相交；y軸也位于赤道平 面內(nèi)，與x、z軸構(gòu)成右手直角坐標(biāo)系。導(dǎo)航中可用地球坐標(biāo)系的直角坐標(biāo)表示也可用經(jīng)緯度表示。（常見GPS標(biāo) WGS-843 .地理坐標(biāo)系（t系）地理坐標(biāo)系也稱當(dāng)?shù)卮咕€坐標(biāo)系，原點位于運載體所在 點，原點位于運載體所在點，x軸沿當(dāng)?shù)鼐暰€切線方向，

2、y軸沿當(dāng)?shù)亟?jīng)線切線方向，z軸沿當(dāng)?shù)氐乩泶咕€方向并 與x、y軸構(gòu)成右手直角坐標(biāo)系。根據(jù)坐標(biāo)軸方向的不 同，地理坐標(biāo)系的方向可選為“東北天”、“北東地”、 “北西天”等。4 .載體坐標(biāo)系（b系）機體坐標(biāo)系、船體坐標(biāo)系和彈體坐標(biāo)系等的統(tǒng)稱。 原 點與載體的質(zhì)心重合；x軸沿載體橫軸向右，y軸沿 載體縱軸向前，z軸沿載體豎軸并與x、y軸構(gòu)成右 手直角坐標(biāo)系。載體的俯仰（縱搖）角，橫滾（橫搖）角，航向（偏 航）角統(tǒng)稱為姿態(tài)角。載體的姿態(tài)角是根據(jù)載體坐標(biāo) 系相對地理坐標(biāo)系來確定的。二、坐標(biāo)系轉(zhuǎn)換（姿態(tài)角）1 .方向余弦法設(shè)空間直角坐標(biāo)系 OX1Y1乙，其三軸單位向量分別為 i1、jk1。任一向量 R可均可表

3、示為:R=RxiiiRyijiRziki這里的分量Rxp Ryi、Rzi為向量R在三個軸上的投影:RxiRy1RziRcosRcosRcosRxiRyiRzi式中，XI、 yi、 Z1分別為R與坐標(biāo)系OXiYiZi三軸的夾角，將cos XI、cos yi、cos z；稱為向量R在坐標(biāo)系OXM乙中的方向余弦。設(shè)另一坐標(biāo)系OX2Y2Z2, X2軸對于OXNZi坐標(biāo)系的方向余弦為 cos Xi2、cos ；、cos,； x x xxyzi，Y2軸對于OXiYiZi坐標(biāo)系的方向余弦為cos Xi2> cos ；、cos z；2 ； Z2軸對于OXiY£i坐標(biāo)系的方向余弦為cos康2、c

4、os ：2、cos那么有:xyz 1x2x2x2cosxicosyicosziy2y2y2cosxicosyicosziz2z2z2cosxicosyicosziCi2r2 c2 rR、R2 為 R在 OXiYi乙、OX2Y2Z2中的坐標(biāo)列向量。2 .矩陣Ci中的九個兀素均為兩坐標(biāo)系坐標(biāo)軸之間的方向余弦，反映了兩坐標(biāo)之間的角位置關(guān) 系，稱Ci2為從坐標(biāo)系OX1YZ1到OX2Y2Z2的方向余弦。2 .歐拉角法兩三維直角坐標(biāo)系之間的方向余弦矩陣有九個元素，由于有六個約束條件，只有三個元素是獨立的，這說明任意兩個三維直角坐標(biāo)系之間的角度關(guān)系完全可以由 三個角度來描述。假定從坐標(biāo)系 OX0Y0Z0經(jīng)由

5、三次旋轉(zhuǎn)可以得到坐標(biāo)系 OXYZ。繞X0旋車k kx繞Yi旋轉(zhuǎn)ky繞Z2旋車k kzOX0Y0Z0> OXiYiZi'=OX2Y2Z2PXYZ變化Kx、Ky、Kz橫滾（橫搖）角，俯仰（縱搖）角一航向（偏航）角三個角度，可以形成原點與 OX0Y0Z0相 同的任意三維直角坐標(biāo)系。即任意一個三維直角坐標(biāo)系OXYZ均可以從OX0Y0Z0經(jīng)過上述三次旋轉(zhuǎn)得到。稱三0cosKy0sin Kycos Kzsin Kzsin KxC12 =010C；=sin Kzcos KzcosKxsin Ky0cosKy00101C0 = 0 cos Kx0 sin Kx001個旋轉(zhuǎn)角為歐拉角。由 m QT

6、國4 9 QXYZ設(shè)由OX0Y0Z0至OXYZ的方向余弦矩陣為 C,則根據(jù)方向余弦矩陣的傳遞性:_ 3 _ 2_1CC2C1C0cosKz cosKy cosKz sin Kysin Kx sin Kz cosKx sin Kzsin Kx cosKzsin Ky cosKx Cbsin Kz cosKy cosKzcosKx sin Kzsin Kysin Kx sin Kzsin KycosKx cosKzsinKxsin KycosKysin KxcosKy cosKx3 .四元數(shù)理論上歐拉旋轉(zhuǎn)可以靠順序讓一個物體指到任何一個想要的方向， 但是如果在旋 轉(zhuǎn)過程中讓某些坐標(biāo)軸重合了就會發(fā)生

7、萬向鎖， 就會丟失一個方向上的旋轉(zhuǎn)能力， 除非打破原先的旋轉(zhuǎn)順序或者同時旋轉(zhuǎn)三個坐標(biāo)軸。 所以引進(jìn)四元數(shù)法來表示姿 態(tài)角。(1)四元數(shù)定義Q q0,q1,q2,q3q。 q/ q2j q3k也是虛單位。滿足下列關(guān)系:其中，q0,qi,q2,q3是實數(shù)，i, j,k既是互相正交的單位向量,222i2j2k21ijk, jikjki,kjikij,ikj(2)四元數(shù)的表達(dá)方式11矢量式Qqoq其中，q0稱四元素Q的標(biāo)量部分,q稱四元素Q的矢量部分，對比與復(fù)數(shù)式,q可以看出為三維空間的一個向量。2復(fù)數(shù)式Q qoqi q2 jq3k共軻復(fù)數(shù)為：Q q0 q1i q2j q3k31三角式Q cos us

8、in 一 22式中，為實數(shù)，u為單位向量。41指數(shù)式51矩陣式TQqoqiq2q3(3)四元數(shù)的大小-范數(shù)四元數(shù)的大小用四元數(shù)的范數(shù)來表示：Qq0q；q2q3若|Q| 1 ,則Q稱為規(guī)范化四元數(shù)。(4)四元數(shù)的運算-加減乘除11加法和減法設(shè)Q qo qi q?j q3kP Po 由 P2j P3kQ PqoPbqP1 i q2P2 j q3 P3 k21乘法aQ aq0aqi aq2j aq3kr。r 歸r3k其中ri運算律31除法PoPiP2P3根據(jù)范數(shù)的定義:又因為逆的定義，若P Q所以PlP2P3PoP3P21,(5)四元數(shù)方向余弦矩陣1 2(q1cnP3PoP2Piqiq2則稱Poq3

9、Q是P的逆,*QQ-1|QQ-i* Q Q-q；)2(q1q2 q°q3)2(qq3 qoqb)2(q1q2 q0q3) i 2(q； q；) 2(q2q3 qoqi)2(qq32aq31 2(q；qoq2)qoqi)2、q2)(6)四元數(shù)與歐拉角轉(zhuǎn)換KxKyKzarctan2(q2q3 q°q)1 2(q2 q2)arcsin(2(q°q2 q%)2(qq2 q0q3)arctan 2才1 2(q2 q2)因為 arctan 和 arcsin的角度范圍限制，所以需要 atan2代替arctan 。KxKyKz22atan2(2(q2q3 q°qi),i

10、2(qq?)arcsin(2(q°q2 qq3)22atan2(2(qiq2 q0q3),i2q3)注：atan2是一個函數(shù)，在C語言里返回的是指方位角，也可以理解為計算復(fù)數(shù)x+yi的輻角，計算時atan2 比atan穩(wěn)te。三、GPS坐標(biāo)轉(zhuǎn)換GPS的 WGS84經(jīng)緯度（ 再轉(zhuǎn)化為東北大坐標(biāo)系（1 . BLH轉(zhuǎn)換為XYZ 對空間某一點，大地坐標(biāo)系（B、X、L、H）轉(zhuǎn)化為 WGS84直角坐標(biāo)系（X、Y、Z）,V、z）。B、L、H）到直角坐標(biāo)（X, Y, Z）系的轉(zhuǎn)換關(guān)系如下:H cosBcosLH cosBsin Le2 H sinB式中：N a/We2 sin2 B , N為該點的卯

11、酉圈曲率半徑；e= a2 b2 /a2, ab、e分別為該大地坐標(biāo)系對應(yīng)參考橢球的長半軸、短半軸和第一偏心率。在WGS84大地坐標(biāo)系下，長半軸 a=6378137m,短半軸 b=6356.7523142km,2e2 0.0066943799。2.直角坐標(biāo)與東北大坐標(biāo)的相互轉(zhuǎn)化X1sinBcosLY1sin Bsin LZ1 cosBsin L cosBcosL y cosL cosBsin L x0 sinB zN H cosBcosLN H cosBsinLN 1 e2H sinB設(shè)坐標(biāo)轉(zhuǎn)換矩陣sin BcosLsin L cos B cos L)X)Y)Z 乙 1R)X)Y)Z X y z令 則sin Bsin L cos L cosBsin LcosB 0 sin BN H cos