CN103438906B - 适用于机器人导航的视觉与卫星定位传感器联合校准方法 - Google Patents

Abstract

本发明提供一种适用于机器人导航的视觉与卫星定位传感器联合校准方法：装配时确保卫星定位传感器天线的中心点在视觉传感器的每帧图像中都可见；获取视觉传感器的前期校正结果、视觉传感器与卫星定位传感器天线之间的直线距离、卫星定位传感器天线中心点在某帧图像的相面坐标系中的坐标，求取卫星定位传感器天线中心点在相机中心坐标系中的坐标的绝对值及各项的正负；确定相机中心坐标系的原点在等效坐标系中的坐标；以及，基于每个采样时刻卫星定位传感器测得的经度、纬度和高度，求取相机中心坐标系原点的经度、纬度和高度，以便联合校准由于卫星定位传感器与视觉传感器中心装配于不同空间点而引起的误差。

Description

适用于机器人导航的视觉与卫星定位传感器联合校准方法

技术领域

本发明适用于机器人导航及普通载体的组合导航领域，特别涉及一种适用于机器人导航的视觉与卫星定位传感器联合校准方法。

背景技术

例如在机器人导航及普通载体的组合导航领域中，为了提高导航的精度及在多种环境中的适应性，常常在载体上装配多种传感器，但这些传感器往往无法装配于空间中的同一点，也就是说多种传感器的视场不同，因此，如果直接假设不同的传感器之间的观测源于同样的基准进行坐标投影，则会造成最终结果的全局误差。因此如何对多种传感器进行校准是必须的技术环节。最常见的是如何校准卫星定位传感器与视觉传感器中心装配点不重合的问题。

目前广泛应用的卫星定位系统(包括GPS、伽利略、北斗、GLONASS)的中高端的终端接收机都需要较大的外置天线，这导致天线的装配无法和视觉传感器的中心点重合，而卫星定位系统接收机的观测值(通常都可以转化为经度、纬度和高度)通常指其天线的中心点的空间方位。而由于前述装配上的不吻合，使卫星定位系统接收机的观测值并不是视觉传感器中心点的空间位置。但后续的多传感器数据融合过程中，需要多传感器的观测值处于统一的基准点。也就是说如何能将卫星定位传感器的经度、纬度和高度转换为视觉传感器中心点的经度、纬度和高度，基于此才能将后续由视觉传感器的观测转换为与卫星定位传感器统一的基准上。

但是现有技术需要复杂的校验过程，有的甚至需要专门的校验场，因此限制了系统的广泛适用性。

发明内容

本发明旨在解决视觉传感器和卫星定位传感器的联合校准的问题，提供一种适用于机器人导航的视觉与卫星定位传感器联合校准方法，能够在任意时刻基于卫星定位传感器的输出经度、纬度和高度求取视觉传感器中心的经度、纬度和高度。本发明所描述的方法的简单易行，有很强的独创性。

为了达到上述目的，本发明的技术方案是提供一种适用于机器人导航的视觉与卫星定位传感器联合校准方法，其包含以下步骤：

第一步，按照特定方式在移动机器人上装配视觉传感器和卫星定位传感器天线，该特定方式是指能确保后续作业中卫星定位传感器天线的中心点在视觉传感器所观测的每帧图像中都是可见的装配方式；

第二步，提取视觉传感器的前期校正结果用于后续步骤；

第三步，测量视觉传感器与卫星定位传感器天线之间的直线距离；

第四步，使用视觉传感器采集一帧图像，获取卫星定位传感器天线中心点在该帧图像所对应的相面坐标系中的坐标；

第五步，求取卫星定位传感器天线中心点在相机中心坐标系中的坐标的绝对值；

第六步，确定卫星定位传感器天线中心点在相机中心坐标系中的坐标的各项的正负；

第七步，确定相机中心坐标系的原点在等效坐标系中的坐标；该等效坐标系原点为卫星定位传感器天线中心，该等效坐标系坐标轴与相机中心坐标系坐标轴平行；

所述的第一步至第七步，在移动机器人作业之前进行一次；

第八步，在移动机器人作业过程中的每个采样时刻都要进行本步骤；

即，基于每个采样时刻卫星定位传感器测得的经度、纬度和高度，求取相机中心坐标系原点的经度、纬度和高度：该求取过程中包含基于GPS的地心地固坐标系下的大地坐标，求得相机中心点的大地坐标；基于本步骤的计算结果，将后续视觉传感器所有观测的数值都转换为地心地固坐标系下的大地坐标，实现对视觉传感器和卫星定位传感器的联合校准。

因此，本发明能够校准由于卫星定位传感器与视觉传感器中心装配于不同空间点而引起的误差。

附图说明

图1是本发明所述联合校准方法的流程示意图；

图2是本发明示例中视觉传感器和GPS接收机天线装配的侧视示意图；

图3是本发明示例中视觉传感器和GPS接收机天线装配的俯视示意图；

图4是本发明示例中视觉传感器采集的一帧观测图像的示意图；

图5是本发明示例中相面坐标系的一个示意图。

具体实施方式

为了能够更好地说明问题，这里首先声明本发明中涉及的坐标系：

地心地固坐标系(本发明中用e代表)，为右手坐标系。地球球心为原点，x轴指向本初子午线，y轴指向东经90度，z轴与地球自转轴重合，指向北极。根据表述不同，e下的坐标有两种等效的表达方式：笛卡尔坐标，用(x_e，y_e，z_e)表示；大地坐标，用经度λ，纬度高度h表示。

局部地理坐标系(本发明中用g代表)，为右手坐标系。该坐标系的原点任意，因此在本发明的后续论述中凡表述局部地理坐标系时，都需指明其原点。x轴指向东向，y轴指向北极点，z轴指向地球球心的反方向。也称为东北天坐标系。本发明中所使用惯性传感器测量特定坐标系坐标轴与g的旋转角度。

相机中心坐标系，为右手坐标系。原点为相机成像中心，即光心，x轴指向摄像头前方，y轴指向左方，z轴指向相机上方。

惯导载体坐标系为右手坐标系。原点为惯性导航传感器的中心点，坐标轴与相机中心坐标系的坐标轴平行。

机器人中心坐标系为右手坐标系。原点为机器人等效中心点，坐标轴与相机中心坐标系的坐标轴平行。

注意，在本发明中将，机器人的等效中心点与相机中心坐标系原点重合，通过将惯导传感器与相机紧密捆绑，可使相机中心坐标系与惯导载体坐标系重合。因此，将上述三个坐标系统称为c坐标系，其原点称为点c。

相面坐标系(本发明中用m代表)，该坐标系存在于视觉传感器采集的每一帧图像，每一帧图像都具有自己的相面坐标系。它是一个二维坐标系(一个该坐标系的示例示于图5)，其原点位于一帧图像的左上角，按照惯例，其横轴记为u，纵轴记为v。某点在相面坐标系中的坐标一般以该点在沿横轴和沿纵轴方向距离相面坐标系原点多少个像素点单位表示。

等效坐标系(本发明中用k代表)；该坐标系是本发明为了让坐标转换过程的描述更清晰扼要而假设出的一个坐标系。该坐标系的原点位于GPS接收机天线的中心点，而坐标轴与相机中心坐标系的坐标轴平行。

本发明后续的介绍中将频繁出现关于特定点在特定坐标系中坐标的描述。为了方便表述，特将点以如下方式声明：将某一点α点在某个坐标系z下的坐标，表示为

在本实施例中，使用GPS接收机作为卫星定位传感器进行说明。而在其他一些不同的实施例中，提到的卫星定位传感器不限定为GPS接收机，可以是伽利略、格朗纳斯和北斗等卫星导航系统。

所述联合校准方法具体的实施步骤如下：

第一步，按照特定方式装配视觉传感器20和GPS接收机天线30。要点是必须保证GPS接收机天线30的中心点31在视觉传感器20所观测的每帧图像中都是可见的，视觉传感器20和GPS接收机天线30装配的示意图示于图1(此图为侧视图)和图2(此图为俯视图)。

本实施例中作为示例，将GPS接收机天线30安装于视觉传感器20的右前方，且摄像头视角下倾，以确保可以在每个采样时刻都能将GPS接收机天线30涵盖在观测的图像中。这种方式装配的系统在移动机器人10后续的作业过程中采集出的每一帧图像的右下方都会出现GPS接收机天线30及该天线的中心点31(因GPS接收机输出的经度、纬度和高度是指该中心点的经度、纬度和高度)，经这种方式装配的系统在一帧的观测图像的示意图(此为示意图，而非实图)，示于图3；通过这一步可以确保惯性导传感器所测量的三个欧拉角航向角ψ、俯仰角γ和横滚角θ反映了c坐标系与局部地理坐标系坐标轴之间的旋转角度。

第二步，从视觉传感器的前期校正结果中提取本发明后续步骤需要的参数；视觉传感器的校正是领域内通用的技术，领域内有很多开源的相机校正工具包可免费使用，例如，使用Matlab语言编写的相机校正工具包，和使用了C++语言编写的相机校正工具包，后者依托于开源的机器视觉库OpenCV。本发明实施例的相机前期校正可使用上述两个工具包中的任意一个进行校正，他们的效果是等效的。视觉传感器的前期校正中，能得到相机经校正后的结果即固有矩阵K，它可以表示为：

$K=\left[\begin{array}{ccc}{\mathrm{\α}}_u& {\mathrm{\α}}_{\mathrm{\θ}}& u_0\\ 0& {\mathrm{\α}}_v& v_0\\ 0& 0& 1\end{array}\right],$

分别从校正后的固有矩阵中的相应位置提取参数u₀、v₀、α_u和α_v供本发明后续步骤使用。这四个参数的具体含义简述如下：u₀和v₀表示光心在相面坐标系m中沿u和v方向的坐标；α_u表示相机的焦距长度相对于多少个沿u方向上的像素单位；α_v表示相机的焦距长度相对于多少个沿v方向上的像素单位。

第三步，测量图像传感器与GPS接收机天线之间的直线距离。将测量一端置于已按第一步中特定方式装配好的GPS接收机天线中心点处，将测量的另一端置于视觉传感器的中心，测量二者之间的距离，记为L。

第四步，使用视觉传感器采集一帧图像，获取GPS接收机天线中心点在该帧图像所对应的相面坐标系中的坐标。该步骤可以通过人工操作也可以通过编程自动操作，其操作步骤均相同，可以划分为如下子步骤：

a)从该帧图像中辨识出GPS接收机的天线中心点，记为go；

b)确定go在本帧图像相面坐标系中的坐标，记为(u_g，v_g)。

第五步，基于前述步骤得到的结果，求取GPS接收机天线中心点在相机中心坐标系中的坐标的绝对值，即该步骤的具体操作如下：

$\mathrm{\σ}=\frac{u_0-u_g}{{\mathrm{\α}}_u}$

$\mathrm{\ω}=\frac{v_0-v_g}{{\mathrm{\α}}_v}$

$\mathrm{\ξ}=\sqrt{\frac{L^2}{1+{\mathrm{\σ}}^2+{\mathrm{\ω}}^2}}$

$\left|x_c^{go}\right|=\mathrm{\ξ}$

$\left|y_c^{go}\right|=\mathrm{\σ}\mathrm{\ξ}.$

$\left|z_c^{go}\right|=\mathrm{\ω}\mathrm{\ξ}$

第六步，确定GPS接收机天线中心点在相机坐标系中的坐标的各项的正负。具体操作如下：

a)恒为正，即 $x_c^{go}=\mathrm{\ξ};$

b)对于求取u₀-u_g。如果u₀-u_g大于0，则如果u₀-u_g小于0，则如果u₀-u_g等于0，则

c)对于求取v₀-v_g。如果v₀-v_g大于0，则如果v₀-v_g小于0，则如果v₀-v_g等于0，则

第七步，根据前述步骤中求得的GPS接收机天线中心点go在相机坐标系中的坐标即求取相机中心坐标系的原点co在以本发明所声明的等效坐标系k(如前已声明，该等效坐标系原点为GPS接收机天线中心，该坐标系坐标轴与相机中心坐标系坐标轴平行)中的坐标即该步骤的具体操作为:

$x_k^{co}=-x_c^{go},$

$y_k^{co}=-y_c^{go},$

$z_k^{co}=-z_c^{go},$

基于第一步至第七步，在机器人作业之前进行一次即可。

第八步，在移动机器人的作业过程中每个采样时刻t时刻都会执行本步骤。具体操作为，根据t时刻GPS接收机测得的经度、纬度和高度，即GPS接收机天线中心点go在地心地固坐标系e下的大地坐标(λ^go，φ^go，h^go)，相机中心坐标系的原点co在本发明所声明的等效坐标系k中的坐标即和惯导传感器输出的三个角度航向角ψ_t、横滚角θ_t和俯仰角γ_t，求取t时刻相机中心坐标系原点co的经度、纬度和高度(λ^co，φ^co，h^co)。后续的视觉传感器的所有观测都可以基于该步结果转换为地心地固坐标系e下的大地坐标，从而联合校准了由于系统中的GPS接收机天线和相机坐标系中心点装配于不同空间点而引起的误差。该步骤又分为如下子步骤，其具体操作如下:

a)求取相机中心坐标系原点co在以GPS接收机天线中心go为原点的局部地理坐标系中的坐标即该步骤需用到相机中心坐标系的原点co在本发明所声明的等效坐标系k中的坐标即和惯导传感器输出的三个角度航向角ψ_t、横滚角θ_t和俯仰角γ_t，的操作如下：

$R_k^g=\left[\begin{array}{ccc}\cos \left({\mathrm{\γ}}_t\right)\cos \left({\mathrm{\ψ}}_t\right)-\sin \left({\mathrm{\γ}}_t\right)\sin \left({\mathrm{\ψ}}_t\right)\sin \left({\mathrm{\θ}}_t\right)& \cos \left({\mathrm{\γ}}_t\right)\sin \left({\mathrm{\ψ}}_t\right)+\cos \left({\mathrm{\ψ}}_t\right)\sin \left({\mathrm{\γ}}_t\right)\sin \left({\mathrm{\θ}}_t\right)& -\cos \left({\mathrm{\θ}}_t\right)\sin \left({\mathrm{\γ}}_t\right)\\ -\cos \left({\mathrm{\θ}}_t\right)\sin \left({\mathrm{\ψ}}_t\right)& \cos \left({\mathrm{\ψ}}_t\right)\cos \left({\mathrm{\θ}}_t\right)& \sin \left({\mathrm{\θ}}_t\right)\\ \cos \left({\mathrm{\ψ}}_t\right)\sin \left({\mathrm{\γ}}_t\right)+\cos \left({\mathrm{\γ}}_t\right)\sin \left({\mathrm{\γ}}_t\right)\sin \left({\mathrm{\θ}}_t\right)& \sin \left({\mathrm{\γ}}_t\right)\sin \left({\mathrm{\ψ}}_t\right)-\cos \left({\mathrm{\γ}}_t\right)\cos \left({\mathrm{\ψ}}_t\right)\sin \left({\mathrm{\θ}}_t\right)& \cos \left({\mathrm{\γ}}_t\right)\cos \left({\mathrm{\θ}}_t\right)\end{array}\right]$

$\left[\begin{array}{c}{x}_g^{co}\\ y_g^{co}\\ z_g^{co}\end{array}\right]=R_k^g\left[\begin{array}{c}{x}_k^{co}\\ y_k^{co}\\ z_k^{co}\end{array}\right]$

b)将t时刻GPS接收机输出的经度、纬度和高度即GPS接收机天线中心点go在地心地固坐标系e下的大地坐标(λ^go，φ^go，h^go)转换为地心地固坐标系e下的笛卡尔坐标即该步的操作如下：

$N^{go}=\frac{a}{\sqrt{1-e^2{\sin }^2{\mathrm{\φ}}^{go}}}$

$x_e^{go}=(N^{go}+h^{go})\cos \left({\mathrm{\φ}}^{go}\right)\cos \left({\mathrm{\λ}}^{go}\right)$

$y_e^{go}=(N^{go}+h^{go})cos\left({\mathrm{\φ}}^{go}\right)\sin \left({\mathrm{\λ}}^{go}\right)$

$z_e^{\mathrm{go}}=[(1-e^2)N^{\mathrm{go}}+h^{\mathrm{go}}]\sin \left({\mathrm{\φ}}^{\mathrm{go}}\right)$

c)求取t时刻相机中心坐标系的原点co在地心地固坐标系e下的笛卡尔坐标即该步的操作如下：

$\left[\begin{array}{c}{x}_e^{co}\\ y_e^{co}\\ z_e^{co}\end{array}\right]={\left[\begin{array}{ccc}-\sin \left({\mathrm{\λ}}^{go}\right)& \cos \left({\mathrm{\λ}}^{go}\right)& 0\\ -\sin \left({\mathrm{\φ}}^{go}\right)\cos \left({\mathrm{\λ}}^{go}\right)& -\sin \left({\mathrm{\φ}}^{go}\right)\sin \left({\mathrm{\λ}}^{go}\right)& \cos \left({\mathrm{\φ}}^{go}\right)\\ \cos \left({\mathrm{\φ}}^{go}\right)\cos \left({\mathrm{\λ}}^{go}\right)& \cos \left({\mathrm{\φ}}^{go}\right)\sin \left({\mathrm{\λ}}^{go}\right)& \sin \left({\mathrm{\φ}}^{go}\right)\end{array}\right]}^{-1}\left[\begin{array}{c}{x}_g^{co}\\ y_g^{co}\\ z_g^{co}\end{array}\right]+\left[\begin{array}{c}{x}_e^{go}\\ y_e^{go}\\ z_e^{go}\end{array}\right]$

d)将t时刻相机中心坐标系的原点co在地心地固坐标系e下的笛卡尔坐标转换为经度、纬度和高度即(λ^co，φ^co，h^co)。该步的操作如下：

i.求取经度：

${\mathrm{\&lambda;}}^{co}=\left\{\begin{array}{cc}\arctan \left(y_e^{co}/x_e^{co}\right)& x_e^{co}\&GreaterEqual;0\\ \arctan \left(y_e^{co}/x_e^{co}\right)+\mathrm{\&pi;}& x_e^{co}<0{\mathrm{andy}}_e^{co}\&GreaterEqual;0\\ \arctan \left(y_e^{co}/x_e^{co}\right)-\mathrm{\&pi;}& x_e^{co}<0{\mathrm{andy}}_e^{co}<0\end{array}\right.$

ii.求取纬度：

$r=\sqrt{{\left(x_e^{co}\right)}^2+{\left(y_e^{co}\right)}^2}$

$F=54{\left({\mathrm{bz}}_e^{co}\right)}^2$

η＝1-e²

其中，b＝6356752.3142为地球短半轴，单位为米。e²＝0.006694为地球第一数理偏心率。

$G=r^2+\mathrm{\&eta;}{\left(z_e^{co}\right)}^2-e^2(a^2-b^2);$

$C=\frac{e^4{\mathrm{Fr}}^2}{G^3};$

$s={\left(1+C+\sqrt{\sqrt{C^2+2C}}\right)}^{\frac{1}{3}};$

$P=\frac{F}{3{(S+\frac{1}{S}+1)}^2{G}^2};$

$Q=\sqrt{1+2e^{4}P};$

$\mathrm{\&rho;}=\frac{-{\mathrm{Pe}}^{2}r}{1+Q}+\sqrt{\frac{a^2}{2}(1+\frac{1}{Q})-\frac{P\mathrm{\&eta;}{\left(z_e^{co}\right)}^2}{Q\left(1+Q\right)}-\frac{{\mathrm{Pr}}^2}{2}};$

${\mathrm{\&phi;}}^{co}=\arctan \left(\frac{z_e^{co}+\frac{e^2{\mathrm{az}}_e^{co}}{V}}{r}\right);$

iii.求取高度：

综上所述，本发明能够在任意时刻基于卫星定位传感器的输出经度、纬度和高度求取视觉传感器中心的经度、纬度和高度，解决视觉传感器和卫星定位传感器的联合校准的问题。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (8)

1.一种适用于机器人导航的视觉与卫星定位传感器联合校准方法，其特征在于，包含以下步骤：

第一步，按照特定方式在移动机器人上装配视觉传感器和卫星定位传感器天线，该特定方式是指能确保后续作业中卫星定位传感器天线的中心点在视觉传感器所观测的每帧图像中都是可见的装配方式；

第二步，提取视觉传感器的前期校正结果用于后续步骤；

第三步，测量视觉传感器与卫星定位传感器天线之间的直线距离；

第四步，使用视觉传感器采集一帧图像，获取卫星定位传感器天线中心点在该帧图像所对应的相面坐标系m中的坐标；所述相面坐标系m是该帧图像中对应存在的一个二维坐标系，其原点位于该帧图像的左上角；任意一点在所述相面坐标系m中的坐标，通过该点在沿横轴u和沿纵轴v方向距离该相面坐标系m原点有多少个像素点单位来表示；

第五步，求取卫星定位传感器天线中心点在相机中心坐标系中的坐标的绝对值；所述相机中心坐标系，其原点为相机成像中心，即光心，x轴指向摄像头前方，y轴指向左方，z轴指向相机上方；

第六步，确定卫星定位传感器天线中心点在相机中心坐标系中的坐标的各项的正负；

第七步，确定相机中心坐标系的原点在等效坐标系k中的坐标；该等效坐标系k原点为卫星定位传感器天线中心，该等效坐标系k坐标轴与相机中心坐标系坐标轴平行；

所述的第一步至第七步，在移动机器人作业之前进行一次；

第八步，在移动机器人作业过程中的每个采样时刻都要进行本步骤；

即，基于每个采样时刻卫星定位传感器测得的经度、纬度和高度，求取相机中心坐标系原点的经度、纬度和高度：该求取过程中包含基于GPS的地心地固坐标系e下的大地坐标，求得相机中心点的大地坐标；基于本步骤的计算结果，将后续视觉传感器所有观测的数值都转换为地心地固坐标系e下的大地坐标，实现对视觉传感器和卫星定位传感器的联合校准；其中，所述地心地固坐标系e中，以地球球心为原点，x轴指向本初子午线，y轴指向东经90度，z轴与地球自转轴重合，指向北极；大地坐标，用经度λ，纬度高度h表示。

2.如权利要求1所述的联合校准方法，其特征在于，

所述的第二步中，视觉传感器的前期校正结果包含固有矩阵K：

从校正后的固有矩阵K中的相应位置分别提取参数u₀、v₀、α_u和α_v：其中，u₀和v₀表示光心在相面坐标系m中沿横轴u和纵轴v方向的坐标；α_u表示相机的焦距长度相对于多少个沿横轴u方向上的像素单位；α_v表示相机的焦距长度相对于多少个沿纵轴v方向上的像素单位。

3.如权利要求2所述的联合校准方法，其特征在于，

所述的第三步中，测量的一端是第一步中特定方式装配好的卫星定位传感器天线中心点处，测量的另一端是视觉传感器的中心，测量二者之间的直线距离，记为L。

4.如权利要求3所述的联合校准方法，其特征在于，

所述的第四步中，包含以下的子步骤：从视觉传感器采集的一帧图像中辨识出卫星定位传感器天线中心点，记为go；确定go在该帧图像的相面坐标系m中的坐标，记为(u_g，v_g)。

5.如权利要求4所述的联合校准方法，其特征在于，

所述的第五步中，求取卫星定位传感器天线中心点在相机中心坐标系中的坐标的绝对值包含以下过程：

。

6.如权利要求5所述的联合校准方法，其特征在于，

所述的第六步中，确定卫星定位传感器天线中心点在相机中心坐标系中的坐标的各项的正负，包含以下操作：

恒为正，即

对于求取u₀-u_g；如果u₀-u_g大于0，则如果u₀-u_g小于0，则如果u₀-u_g等于0，则

对于求取v₀-v_g；如果v₀-v_g大于0，则如果v₀-v_g小于0，则如果v₀-v_g等于0，则

7.如权利要求6所述的联合校准方法，其特征在于，

所述的第七步中，根据卫星定位传感器天线中心点go在相机中心坐标系中的坐标即来求取所述相机中心坐标系的原点co在等效坐标系k中的坐标即包含以下操作：

8.如权利要求7所述的联合校准方法，其特征在于，

每个采样时刻t进行的所述第八步中，包含以下操作：

a)基于相机中心坐标系的原点co在等效坐标系k中的坐标即和惯导传感器输出的三个角度航向角ψ_t、横滚角θ_t和俯仰角γ_t，来求取相机中心坐标系原点co在以卫星定位传感器天线中心go为原点的局部地理坐标系中的坐标即

b)将t时刻卫星定位传感器输出的经度、纬度和高度，即卫星定位传感器天线中心点go在地心地固坐标系e下的大地坐标(λ^go，φ^go，h^go)转换为地心地固坐标系e下的笛卡尔坐标即

c)求取t时刻相机中心坐标系的原点co在地心地固坐标系e下的笛卡尔坐标即

d)将t时刻相机中心坐标系的原点co在地心地固坐标系e下的笛卡尔坐标转换为经度、纬度和高度，即(λ^co，φ^co，h^co)，包含以下操作：

i.求取经度：

ii.求取纬度：

η＝1-e²

其中，b＝6356752.3142为地球短半轴，单位为米；e²＝0.006694为地球第一数理偏心率；

iii.求取高度：