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

1、坐標(biāo)系問(wèn)題常用坐標(biāo)系1 .慣性坐標(biāo)系（i系）對(duì)牛頓運(yùn)動(dòng)定律適用的參考系；研究物體時(shí)，選取靜止或作勻速直線運(yùn)動(dòng)的參考 系，牛頓定律才能成立。常用慣性坐標(biāo)系：日心慣性坐標(biāo)系、地心慣性坐標(biāo)系等。2 .地球坐標(biāo)系（e系）即周連在地球上的坐標(biāo)系，該坐標(biāo)系隨地球一起轉(zhuǎn)動(dòng)。地球坐標(biāo)系原點(diǎn)在地球中心，軸沿地球自轉(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)系，原點(diǎn)位于運(yùn)載體所在 點(diǎn)，原點(diǎn)位于運(yùn)載體所在點(diǎn)，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系）機(jī)體坐標(biāo)系、船體坐標(biāo)系和彈體坐標(biāo)系等的統(tǒng)稱。 原 點(diǎn)與載體的質(zhì)心重合；x軸沿載體橫軸向右，y軸沿 載體縱軸向前，z軸沿載體豎軸并與x、y軸構(gòu)成右 手直角坐標(biāo)系。載體的俯仰（縱搖）角，橫滾（橫搖）角，航向（偏 航）角統(tǒng)稱為姿態(tài)角。載體的姿態(tài)角是根據(jù)載體坐標(biāo) 系相對(duì)地理坐標(biāo)系來(lái)確定的。二、坐標(biāo)系轉(zhuǎn)換（姿態(tài)角）1 .方向余弦法設(shè)空間直角坐標(biāo)系 OX1Y1乙，其三軸單位向量分別為 i1、jk1。任一向量 R可均可表

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

4、os ：2、cos那么有:xyz 1x2x2x2cosxicosyicosziy2y2y2cosxicosyicosziz2z2z2cosxicosyicosziCi2r2 c2 rR、R2 為 R在 OXiYi乙、OX2Y2Z2中的坐標(biāo)列向量。2 .矩陣Ci中的九個(gè)兀素均為兩坐標(biāo)系坐標(biāo)軸之間的方向余弦，反映了兩坐標(biāo)之間的角位置關(guān) 系，稱Ci2為從坐標(biāo)系OX1YZ1到OX2Y2Z2的方向余弦。2 .歐拉角法兩三維直角坐標(biāo)系之間的方向余弦矩陣有九個(gè)元素，由于有六個(gè)約束條件，只有三個(gè)元素是獨(dú)立的，這說(shuō)明任意兩個(gè)三維直角坐標(biāo)系之間的角度關(guān)系完全可以由 三個(gè)角度來(lái)描述。假定從坐標(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橫滾（橫搖）角，俯仰（縱搖）角一航向（偏航）角三個(gè)角度，可以形成原點(diǎn)與 OX0Y0Z0相 同的任意三維直角坐標(biāo)系。即任意一個(gè)三維直角坐標(biāo)系OXYZ均可以從OX0Y0Z0經(jīng)過(guò)上述三次旋轉(zhuǎn)得到。稱三0cosKy0sin Kycos Kzsin Kzsin KxC12 =010C；=sin Kzcos KzcosKxsin Ky0cosKy00101C0 = 0 cos Kx0 sin Kx001個(gè)旋轉(zhuǎn)角為歐拉角。由 m QT

6、國(guó)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)可以靠順序讓一個(gè)物體指到任何一個(gè)想要的方向， 但是如果在旋 轉(zhuǎn)過(guò)程中讓某些坐標(biāo)軸重合了就會(huì)發(fā)生

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

8、in 一 22式中，為實(shí)數(shù)，u為單位向量。41指數(shù)式51矩陣式TQqoqiq2q3(3)四元數(shù)的大小-范數(shù)四元數(shù)的大小用四元數(shù)的范數(shù)來(lái)表示：Qq0q；q2q3若|Q| 1 ,則Q稱為規(guī)范化四元數(shù)。(4)四元數(shù)的運(yùn)算-加減乘除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運(yùn)算律31除法PoPiP2P3根據(jù)范數(shù)的定義:又因?yàn)槟娴亩x，若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)因?yàn)?arctan 和 arcsin的角度范圍限制，所以需要 atan2代替arctan 。KxKyKz22atan2(2(q2q3 q°qi),i

10、2(qq?)arcsin(2(q°q2 qq3)22atan2(2(qiq2 q0q3),i2q3)注：atan2是一個(gè)函數(shù)，在C語(yǔ)言里返回的是指方位角，也可以理解為計(jì)算復(fù)數(shù)x+yi的輻角，計(jì)算時(shí)atan2 比atan穩(wěn)te。三、GPS坐標(biāo)轉(zhuǎn)換GPS的 WGS84經(jīng)緯度（ 再轉(zhuǎn)化為東北大坐標(biāo)系（1 . BLH轉(zhuǎn)換為XYZ 對(duì)空間某一點(diǎn)，大地坐標(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為該點(diǎn)的卯

11、酉圈曲率半徑；e= a2 b2 /a2, ab、e分別為該大地坐標(biāo)系對(duì)應(yīng)參考橢球的長(zhǎng)半軸、短半軸和第一偏心率。在WGS84大地坐標(biāo)系下，長(zhǎng)半軸 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