齐次坐标转换算法

1、一、坐标系的定义（1）大地坐标系C Oc -XcZcWGS-84大地坐标系原点位于地球质心，Zc轴指向地球北极， Xc轴由原点指向格林尼治平子午面与赤道交点，Yc轴垂直XcOcZc平面与Zc轴，Xc轴共同组成右手坐标系称为大地坐标系。某点大地坐标系下坐标用B, L, H表示，分别表示该点的经度、纬度和大地高程。（2 ）空间大地直角坐标系 GOg -XgZg空间大地直角坐标系具有与大地坐标系相同的坐标轴。不同的是，空间大地直角坐标系为笛卡尔坐标系，大地坐标系为椭球坐标系。 某点的大地直角坐标系下的坐标用xg,yg,zg表示。（3）地理坐标系SOs-XsYsZs以无人机平台为例，将无人机平台看做质

2、点，飞机平台的位置B,L,H即为地理坐标系的原点，可以表示为 匕，：（，馆，Zs轴指向正北方向，Xs轴由原点指向天顶方向， Ys与Zs和Xs组合起来构成右手坐标系被称为地理坐标系。该坐标系也为笛卡尔坐标系。 某点地理坐标系下坐标用 xs, ys, zs表示。（4）无人机坐标系 AOa -XaYaZa无人机坐标系和地理坐标系具有相同的原点。无人机坐标系相当于地理坐标系经过三轴旋转而成的坐标系，当无人机姿态角均为零时， 无人机坐标系与地理坐标系重合。 设无人机三轴 姿态角为'，'，无人机坐标系即为地理坐标系绕 Ya （俯仰），Za （横滚），Xa （偏航） 旋转3'后形成的

3、坐标系。某点载机坐标系下坐标用xa,ya,za表示。（5）摄像机坐标系T（Ot XtYZt ）质点位于摄像机光轴与摄像机镜头平面的交点，Zt轴定义为指向目标物体时摄像机光轴的方向，当乙轴处于水平方向时， Xt轴指向天顶方向，Yt轴与Zt轴和Xt轴共同组成的右手 坐标系称为摄像机坐标系。二、坐标系之间的转换2.1目标物体在摄像机坐标系下的坐标我们把摄像机镜头中心称为摄像机的光心，光心即为摄像机坐标系的原点，摄像机成像几何示意图如图2.1所示，C点为光心，Zc轴由光心指向摄像机视轴方向，并且垂直于像平面，Xc轴与Yc轴和像平面的x轴与y轴平行，像平面坐标系原点Ci定义为视轴与像平面的交点，以C点为

4、坐标原点，由Xc轴、Yc轴和Zc轴构成空间笛卡尔直角坐标系称为摄像机坐 标系，CCi为摄像机的焦距。图2.1摄像机几何成像示意图目标物体在摄像机坐标系下坐标值用T表示，目标物体像点在摄像机坐标系下的值用K表示，K=（x, y,z,1,其中z=f，f为摄像机的焦距，当脱靶量很小的情况下，T与K的比例因子近似为-R/f, R为目标物体到摄像机光心的距离，T和K满足式（2-1）和式（2-2）所示的比例关系：T =QK （2-1）-R/f000|0-R/ f00Q1 =（ 2-2）00-R/f00001 一2.2摄像机坐标系T转换为载机坐标系 A图22摄像机坐标系T到载机坐标系A的转换示意图如图2.2

5、所示，摄像机坐标系 T绕Xt轴旋转（方位）后的到变换矩阵Q2 :1 0 000 cos。sin a 0Q2 =（ 2-3）0-si nm cos。0卫 001 一绕Y （俯仰）轴旋转 0后得到变换矩阵 Qs :cos ：0-sin：0(3-4)0100I sin P0cos P0.0001摄像机坐标系B到载机坐标系A的变换过程如式（2-5）所示:区小“,订=Q3Q2（Xt，yt，Zt1 T（ 2-5）2.3载机坐标系A转换为地理坐标系 S图2.3载机坐标系A到地理坐标系S的转换示意图如图2.3所示，载机坐标系A绕Za轴旋转-后得到变换矩阵Q4 :cos-sin0 0sin 直cosk00Q4

6、=（ 2-6）0010.0001_绕Ys轴旋转-v后的得到变换矩阵 Q5 :cost0si nv0I 0100Q5 =（ 2-7）I-sin 日0cos日0.0001 一j000_0cos九-sin 丸00sin几cosh0卫001 _绕Xs旋转-后得到变换矩阵Q6 :载机坐标系A到地理坐标系（2-8）S的变换过程如式2-9）所示:(Xs, ys,ZsT =QQ5Q4(Xa,ya,Za,1T( 2-9)2.4地理坐标系S转换为空间大地直角坐标系G图2.4地理坐标系S转换为空间大地直角坐标系G的示意图如图2.4所示，地理坐标系沿 Xs轴平移hs后得到变换矩阵 Q7:100 hs 0 10 0Q7

7、 =(2-10)0 0 100 0 0 1绕Ys轴旋转- s后得到变换矩阵Q:cos sin s01-sin 丸sCOS J.s(2-11)绕Zs轴旋转:s后得到变换矩阵 Q9 :cos : s-sin : s01(2-12)沿Zg轴方向平移OgK OgK 二 N0e2sin、后得到变换矩阵Q° :_1地理坐标系-N°e2 sin sS到空间大地直角坐标系(2-13)G的变换过程如式（2-14）所示:(3-14)(Xg,yg,Zg,1= Q7Q9QQ10 gS, Zs,1(3-15)其中，N01 -e2sin2 s 2（5）空间大地直角坐标系 G转换为大地坐标系 C:Zg

8、十 be"2 sin 3UB = arcta n Xg + yg - ae cos UL 二 arctanyg(3-16)Xgcos B其中,QQ一1 -e sin B 2(3-17)ZgaU = arcta n (卫x2 +y；bXg=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*i

9、2 -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*i

10、1)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) - b2