齊次坐標轉(zhuǎn)換算法

一、坐標系的定義（1）大地坐標系 COc XcYcZcWGS-84大地坐標系原點位于地球質(zhì)心， Zc軸指向地球北極， Xc軸由原點指向格林尼治平子午面與赤道交點， Yc軸垂直XcOcZc平面與Zc軸，Xc軸共同組成右手坐標系稱為大地坐標系。某點大地坐標系下坐標用 B,L,H表示，分別表示該點的經(jīng)度、緯度和大地高程。（2）空間大地直角坐標系 GOg XgYgZg空間大地直角坐標系具有與大地坐標系相同的坐標軸。不同的是，空間大地直角坐標系為笛卡爾坐標系，大地坐標系為橢球坐標系。 某點的大地直角坐標系下的坐標用 xg,yg,zg表示。（3）地理坐標系SOXYZssss以無人機平臺為例，將無人機平臺看做質(zhì)點，飛機平臺的位置B,L,H即為地理坐標系的原點，可以表示為s,s,hs，Zs軸指向正北方向，Xs軸由原點指向天頂方向，Ys與Zs和Xs組合起來構(gòu)成右手坐標系被稱為地理坐標系。該坐標系也為笛卡爾坐標系。某點地理坐標系下坐標用xs,ys,zs表示。（4）無人機坐標系AOaXYZaaa無人機坐標系和地理坐標系具有相同的原點。無人機坐標系相當于地理坐標系經(jīng)過三軸旋轉(zhuǎn)而成的坐標系，當無人機姿態(tài)角均為零時，無人機坐標系與地理坐標系重合。設無人機三軸姿態(tài)角為，，，無人機坐標系即為地理坐標系繞Ya（俯仰），Za（橫滾），Xa（偏航）旋轉(zhuǎn)，，后形成的坐標系。某點載機坐標系下坐標用xa,ya,za表示。（5）攝像機坐標系TOXYZtttt質(zhì)點位于攝像機光軸與攝像機鏡頭平面的交點，Zt軸定義為指向目標物體時攝像機光軸的方向，當Zt軸處于水平方向時，Xt軸指向天頂方向，Yt軸與Zt軸和Xt軸共同組成的右手坐標系稱為攝像機坐標系。二、坐標系之間的轉(zhuǎn)換2.1目標物體在攝像機坐標系下的坐標我們把攝像機鏡頭中心稱為攝像機的光心， 光心即為攝像機坐標系的原點， 攝像機成像幾何示意圖如圖 2.1所示，C點為光心， Zc軸由光心指向攝像機視軸方向，并且垂直于像平面，Xc軸與

Yc軸和像平面的

x軸與

y軸平行，像平面坐標系原點

C1定義為視軸與像平面的交點，以

C點為坐標原點，由

Xc軸、

Yc軸和

Zc軸構(gòu)成空間笛卡爾直角坐標系稱為攝像機坐標系，

CC1為攝像機的焦距。圖

2.1

攝像機幾何成像示意圖目標物體在攝像機坐標系下坐標值用

T表示，目標物體像點在攝像機坐標系下的值用

K表示，

K

x,y,z,1T,其中

z=f，f為攝像機的焦距，當脫靶量很小的情況下，

T與K的比例因子近似為-R/f，R為目標物體到攝像機光心的距離，

T和

K滿足式（

2-1）和式（

2-2）所示的比例關系：Q1K(2-1)R/f0000R/f00Q10R/f（2-2）0000012.2攝像機坐標系T轉(zhuǎn)換為載機坐標系A圖2.2攝像機坐標系T到載機坐標系A的轉(zhuǎn)換示意圖如圖2.2所示，攝像機坐標系T繞X軸旋轉(zhuǎn)（方位）后的到變換矩陣Q：t21000Q20cossin00sincos（2-3）00001繞Yt（俯仰）軸旋轉(zhuǎn)后得到變換矩陣Q3：cos0-sin00100Q30cos（3-4）sin00001攝像機坐標系 B到載機坐標系 A的變換過程如式（ 2-5）所示：xa,ya,za,1T Q3Q2xt,yt,zt,1T （2-5）2.3載機坐標系 A轉(zhuǎn)換為地理坐標系 S圖2.3載機坐標系 A到地理坐標系 S的轉(zhuǎn)換示意圖如圖2.3所示，載機坐標系A繞Za軸旋轉(zhuǎn)后得到變換矩陣Q4：cossin00Q4sincos00001（2-6）00001繞Ys軸旋轉(zhuǎn)后的得到變換矩陣Q5：cos0sin0Q50100sin0cos（2-7）00001繞X旋轉(zhuǎn)后得到變換矩陣Q：s61000Q60cossin0（2-8）0sincos00001載機坐標系A到地理坐標系S的變換過程如式（2-9）所示：xs,ys,zs,1TQ6Q5Q4xa,ya,za,1T（2-9）2.4地理坐標系S轉(zhuǎn)換為空間大地直角坐標系G圖2.4地理坐標系 S轉(zhuǎn)換為空間大地直角坐標系 G的示意圖如圖2.4所示，地理坐標系沿 Xs軸平移hs后得到變換矩陣 Q7:100hs0100Q701（2-10）000001繞Ys軸旋轉(zhuǎn)-s后得到變換矩陣Q8：cos0Q8-sin0

ss

0 sin s00coss0 0

002-11）01繞Zs軸旋轉(zhuǎn) s后得到變換矩陣Q9：cos ssin sQ900

sincos00

ss

00（2-12）01沿Zg軸方向平移OgKOgKN0e2sins后得到變換矩陣Q10：10000100Q1001-N0e2sin（2-13）0s0001地理坐標系S到空間大地直角坐標系G的變換過程如式（2-14）所示：xg,yg,zg,1TQ7Q9Q8Q10xs,ys,zs,1T（3-14）a其中，N01（3-15）1e2sin2s2（5）空間大地直角坐標系G轉(zhuǎn)換為大地坐標系C：Bzgbe2sin3Uarctanyg2xg2ae2cos3ULarctanyg(3-16)xgxg2yg2HNcosB其中，Na11e2sin2B23-17）zgaUarctanyg2bxg2Xg=g*h1*i1-a*y*(b2*(c1*(e1*(f2*i2+f1*h2*i1)+e2*h1*i1)+c2*(d1*(e2*(f2*i2+f1*h2*i1)-e1*h1*i1)+d2*(f1*i2-f2*h2*i1)))-b1*(d2*(e2*(f2*i2+f1*h2*i1)-e1*h1*i1)-d1*(f1*i2-f2*h2*i1)))+a*z*(b1*(c1*(e1*(f2*i2+f1*h2*i1)+e2*h1*i1)+c2*(d1*(e2*(f2*i2+f1*h2*i1)-e1*h1*i1)+d2*(f1*i2-f2*h2*i1)))+b2*(d2*(e2*(f2*i2+f1*h2*i1)-e1*h1*i1)-d1*(f1*i2-f2*h2*i1)))+a*x*(c2*(e1*(f2*i2+f1*h2*i1)+e2*h1*i1)-c1*(d1*(e2*(f2*i2+f1*h2*i1)-e1*h1*i1)+d2*(f1*i2-f2*h2*i1)))Yg=g*h1*i2+a*y*(b2*(c1*(e1*(f2*i1-f1*h2*i2)-e2*h1*i2)+c2*(d1*(e2*(f2*i1-f1*h2*i2)+e1*h1*i2)+d2*(f1*i1+f2*h2*i2)))-b1*(d2*(e2*(f2*i1-f1*h2*i2)+e1*h1*i2)-d1*(f1*i1+f2*h2*i2)))-a*z*(b1*(c1*(e1*(f2*i1-f1*h2*i2)-e2*h1*i2)+c2*(d1*(e2*(f2*i1-f1*h2*i2)+e1*h1*i2)+d2*(f1*i1+f2*h2*i2)))+b2*(d2*(e2*(f2*i1-f1*h2*i2)+e1*h1*i2)-d1*(f1*i1+f2*h2*i2)))-a*x*(c2*(e1*(f2*i1-f1*h2*i2)-e2*h1*i2)-c1*(d1*(e2*(f2*i1-f1*h2*i2)+e1*h1*i2)+d2*(f1*i1+f2*h2*i2)))Zg=j-g*h2 -a*x*(c1*(d1*(e1*h2 +e2*f1*h1) -d2*f2*h1) +c2*(e2*h2 -e1*f1*h1)) +a*y*(b1*(d2*(e1*h2 +e2*f1*h1) +d1*f2*h1