齐次坐标转换算法.doc

1、 坐标系的定义（1） 大地坐标系WGS-84大地坐标系原点位于地球质心，轴指向地球北极，轴由原点指向格林尼治平子午面与赤道交点，轴垂直平面与轴，轴共同组成右手坐标系称为大地坐标系。某点大地坐标系下坐标用表示，分别表示该点的经度、纬度和大地高程。（2） 空间大地直角坐标系 空间大地直角坐标系具有与大地坐标系相同的坐标轴。不同的是，空间大地直角坐标系为笛卡尔坐标系，大地坐标系为椭球坐标系。某点的大地直角坐标系下的坐标用表示。（3）地理坐标系以无人机平台为例，将无人机平台看做质点，飞机平台的位置即为地理坐标系的原点，可以表示为，轴指向正北方向，轴由原点指向天顶方向，与和组合起来构成右手坐标系被称为地理坐标系。该坐标系也为笛卡尔坐标系。某点地理坐标系下坐标用表示。（4） 无人机坐标系无人机坐标系和地理坐标系具有相同的原点。无人机坐标系相当于地理坐标系经过三轴旋转而成的坐标系，当无人机姿态角均为零时，无人机坐标系与地理坐标系重合。设无人机三轴姿态角为，无人机坐标系即为地理坐标系绕（俯仰），（横滚），（偏航）旋转后形成的坐标系。某点载机坐标系下坐标用表示。（5） 摄像机坐标系质点位于摄像机光轴与摄像机镜头平面的交点，轴定义为指向目标物体时摄像机光轴的方向，当轴处于水平方向时，轴指向天顶方向，轴与轴和轴共同组成的右手坐标系称为摄像机坐标系。2、 坐标系之间的转换2.1 目标物体在摄像机坐标系下的坐标我们把摄像机镜头中心称为摄像机的光心，光心即为摄像机坐标系的原点，摄像机成像几何示意图如图2.1所示，C点为光心，轴由光心指向摄像机视轴方向，并且垂直于像平面，轴与轴和像平面的x轴与y轴平行，像平面坐标系原点定义为视轴与像平面的交点，以C点为坐标原点，由轴、轴和轴构成空间笛卡尔直角坐标系称为摄像机坐标系，为摄像机的焦距。图2.1 摄像机几何成像示意图 目标物体在摄像机坐标系下坐标值用T表示，目标物体像点在摄像机坐标系下的值用K表示，,其中z=f，f为摄像机的焦距，当脱靶量很小的情况下，T与K的比例因子近似为-R/f，R为目标物体到摄像机光心的距离，T和K满足式（2-1）和式（2-2）所示的比例关系： (2-1) （2-2）2.2摄像机坐标系T转换为载机坐标系A图2.2 摄像机坐标系T到载机坐标系A的转换示意图如图2.2所示，摄像机坐标系T绕轴旋转（方位）后的到变换矩阵： （2-3）绕（俯仰）轴旋转后得到变换矩阵： （3-4）摄像机坐标系B到载机坐标系A的变换过程如式（2-5）所示： （2-5）2.3载机坐标系A转换为地理坐标系S图2.3 载机坐标系A到地理坐标系S的转换示意图如图2.3所示，载机坐标系A绕轴旋转后得到变换矩阵： （2-6）绕轴旋转后的得到变换矩阵： （2-7）绕旋转后得到变换矩阵： （2-8）载机坐标系A到地理坐标系S的变换过程如式（2-9）所示： （2-9）2.4地理坐标系S转换为空间大地直角坐标系G图2.4 地理坐标系S转换为空间大地直角坐标系G的示意图如图2.4所示，地理坐标系沿轴平移后得到变换矩阵: （2-10）绕轴旋转后得到变换矩阵： （2-11）绕轴旋转后得到变换矩阵： （2-12）沿轴方向平移后得到变换矩阵： （2-13）地理坐标系S到空间大地直角坐标系G的变换过程如式（2-14）所示： （3-14）其中， （3-15）（5） 空间大地直角坐标系G转换为大地坐标系C： (3-16)其中， （3-17）Xg=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) - b2*(c2*(d1*(e1*h2 + e2*f1*h1) - d2*f2*h1) -