CN103439727B - 一种地面坐标的测量方法 - Google Patents

Abstract

本发明涉及测绘领域，提供了一种地面坐标的测量方法，包括，获取实时动态RTK主机的姿态；根据所述RTK主机姿态和对中杆高度获取传感器延伸点坐标；根据GPS点坐标、所述延伸点坐标获取需要的地面坐标。采用本发明的技术方案，通过测量设备与初始化状态的姿态(倾斜角)，并通过三轴的磁力计(方位角)，通过空间几何算法，利用高精度测量型接收机实现了非居中情况下，进行待测点高精度坐标测试，提高了作业效率。

Description

一种地面坐标的测量方法

技术领域

本发明涉及测绘领域，特别地涉及一种地面坐标的测量方法。

背景技术

RTK(RealTimeKinematic)实时动态测量技术，是以载波相位观测为根据的实时差分GPS(RTDGPS)技术，它是测量技术发展里程中的一个突破，它由基准站接收机、数据链、流动站接收机三部分组成。

CN201010222116提供摇摆状态下低精度捷联惯导系统快速初始对准方法。分以下步骤：确定载体的初始位置参数；采集加速度计和陀螺仪输出的数据，采用二阶调平和方位估计法完成捷联惯导系统的粗对准，初步确定载体的姿态；估计杆臂长度，计算杆臂效应引起的干扰加速度，对加速度计的输出进行补偿；建立卡尔曼滤波状态方程及卡尔曼滤波量测方程；估计出系统的失准角，并在精对准结束时刻用其来修正系统的捷联姿态矩阵，完成初始对准。

CN200910073154提高船用光纤陀螺捷联惯导系统定位精度的方法。采集光纤陀螺和石英挠性加速度计的输出数据；计算初始的捷联矩阵，完成初始对准；根据系统的误差模型建立动基座下系统的状态方程和观测方程；对状态方程和观测方程进行离散化，建立Krein空间下的系统的卡尔曼滤波方程，把GPS接收机提供的速度信息运用到卡尔曼滤波方程中进行滤波计算；根据估计出的捷联惯导系统的纬度误差和经度误差在导航过程中进行补偿。本发明中建立的Krein空间下的卡尔曼滤波方程中的Re，i是不定的，当外辅导航设备的噪声特性发生变化时，卡尔曼滤波仍然能够准确的估计出捷联惯导系统的误差参数，对捷联惯导系统的定位误差进行补偿，提高捷联惯导系统的定位精度。

上述以及常规用RTK测量地面点时必须先对中问题，再采集数据，这样工作效率很低，而且测量的精度比较低。

发明内容

本发明解决的技术问题在于提供了测量设备非对中状态下的一种地面坐标的测量方法，以提高工作效率，并且提高测量精度。

为解决上述问题，本发明提供了一种地面坐标的测量方法，包括，

获取实时动态RTK主机的姿态；

根据所述RTK主机姿态和对中杆高度获取传感器延伸点坐标；

根据GPS点坐标、所述延伸点坐标获取需要的地面坐标。

上述的测量方法，其中，在所述获取实时动态RTK主机的姿态时，还包括，获取所述RTK主机的GPS点坐标。

上述的测量方法，其中，所述RTK主机的姿态包括方位角Ψ、俯仰角θ和横滚角γ。

上述的测量方法，其中，所述方位角ψ为：

$\left\{\begin{array}{cc}\mathrm{\&psi;}=\arctan (H_y^n/H_x^n),& (H_y^n>0,H_x^n>0)\\ \mathrm{\&psi;}=\mathrm{PI}+\arctan (H_y^n/H_x^n),& (H_y^n>0,H_x^n<0)\\ \mathrm{\&psi;}=\mathrm{PI}+\arctan (H_y^n/H_x^n),& (H_y^n<0,H_x^n<0)\\ \mathrm{\&psi;}=2*\mathrm{PI}+\arctan (H_y^n/H_x^n),& (H_y^n<0,H_x^n>0)\end{array}\right.---\left(3\right)$

其中地理坐标系ox_ny_nz_n中的向量

机体坐标系ox_by_b平面与地理坐标系ox_ny_n平面的平面夹角α为：

$\mathrm{\&alpha;}=\arcsin \left(\sqrt{{\sin }^2\mathrm{\&theta;}+{\sin }^2\mathrm{\&gamma;}}\right)---\left(4\right)$

其中，θ、γ为机体俯仰角和横滚角。

上述的测量方法，其中，所述根据所述RTK主机姿态和对中杆高度获取传感器延伸点坐标具体包括，

设对中杆长为L1，设定传感器上延长线上点到GPS点的距离和杆长相等，设传感器x轴和y轴上的延伸点到地面点的距离为L2和L3，且L2和L3相等，那么：

$L2=L3=\sqrt[2]{{L1}^2+{L1}^2};$

设地面点坐标为(x，y，h)，GPS点坐标为(x1，y1，h1)，通过姿态参数计算出传感器x轴和y轴上延伸点坐标(x2，y2，h2)和(x3，y3，h3)；

设计算出的俯仰角为Pitch，横滚角为Roll，方位角为Yaw，根据几何关系计算出传感器轴线上延伸坐标点：

X2＝x1+L2*cos(Pitch)*cos(Yaw)；

y2＝y1+L2*cos(Pitch)*sin(Yaw)；

h2＝h1+L2*sin(Pitch)；

x3＝x1+L3*cos(Roll)*cos(Yaw)；

y3＝y1+L3*cos(Roll)*sin(Yaw)；

h3＝h1+L3*sin(Roll)。

上述的测量方法，其中，根据GPS点坐标、所述延伸点坐标获取需要的地面坐标具体包括，

根据GPS点坐标和传感器的x轴和y轴的延伸坐标，并且计算出这三个点到地面点的距离，可以得出下面方程组：

(x-x1)²+(y-y1)²+(z-h1)²＝L1²；

(x-x2)²+(y-y2)²+(z-h2)²＝L2²；

(x-x3)²+(y-y3)²+(z-h3)²＝L3²；

展开得：

x²+y²+z²-2(x1x+y1y+h1z)+x1²+y1²+h1²＝L1²；①

x²+y²+z²-2(x2x+y2y+h2z)+x2²+y2²+h2²＝L2²；②

x²+y²+z²-2(x3x+y3y+h3z)+x3²+y3²+h3²＝L3²；③

分别作①-②、②-③、③-①得：

$(x2-x1)x+(y2-y1)y+(h2-h1)h=\frac{1}{2}({L1}^2-{x1}^2-{y1}^2-h{1}^2);$

$(x3-x2)x+(y3-y2)y+(h3-h2)h=\frac{1}{2}({L2}^2-{x2}^2-{y2}^2-h{2}^2);$

$(x1-x3)x+(y1-y3)y+(h1-h3)h=\frac{1}{2}({L3}^2-{x3}^2-{y3}^2-h{3}^2);$

设置中间变量A，B，C：

$A=\frac{1}{2}({L1}^2-{x1}^2-{y1}^2-{h1}^2);$

$B=\frac{1}{2}({L2}^2-{x2}^2-{y2}^2-{h2}^2);$

$C=\frac{1}{2}({L3}^2-{x3}^2-{y3}^2-{h3}^2);$

消去中间变量，得出x跟y、h的关系式：

$y=\frac{c1(A-B)-c(B-C)}{\mathrm{bc}1-b1c}-\frac{\mathrm{ac}1-a1c}{\mathrm{bc}1-b1c}x$

$h=\frac{b1(A-B)-b(B-C)}{\mathrm{cb}1-\mathrm{bc}1}-\frac{\mathrm{ab}1-a1b}{\mathrm{cb}1-\mathrm{bc}1}x$

其中变量a，b，c，a1，b1，c1为：

a＝x1-x2；b＝y2-y1；c＝h2-h1；a1＝x3-x2；b1＝y3-y2；c1＝h3-h2；将这两个变量关系式带入①中，计算出x、y、h的解。

采用本发明的技术方案，通过测量设备与初始化状态的姿态(倾斜角)，并通过三轴的磁力计(方位角)，通过空间几何算法，利用高精度测量型接收机实现了非居中情况下，进行待测点高精度坐标测试，提高了作业效率。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本发明第一实施例流程图；

图2是本发明实施例空间几何示意图。

具体实施方式

为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚、明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

如图1所示，是本发明第一实施例流程图，提供了一种地面坐标的测量方法，具体包括，

步骤S101，获取实时动态RTK主机的姿态；

作为一个示例，获取高精度的GPS点坐标，所述GPS点坐标为RTK主机天线面的坐标；通过三轴磁传感器，三轴重力加速度传感器和三轴陀螺加速度传感器来计算RTK(RealTimeKinematic，实时动态)主机的姿态。

具体地，所述RTK主机的姿态方位角Ψ、俯仰角θ和横滚角γ(倾斜角)来表示。

a，坐标系

(1)地理坐标系(n系NED轴)：原点在载体的质心，N轴指北，E轴指东，D轴沿地垂线指向地，这个坐标系也称之为北东地坐标系。地理坐标系可以有不同的选取方法，例如“东、北、天”，三者按顺序构成右手直角坐标系。

(2)机体坐标系(b系x_by_bz_b轴)：机体坐标系和载体固连，坐标原点在载体的质心，x_b沿载体的纵轴方向，y_b指向载体的右侧，z_b垂直于x_b轴、y_b轴构成右手直角坐标系。机体坐标系相对于地理坐标系的方位即为载体的姿态。

b，载体的姿态表示

载体在空间中的姿态可以用载体坐标系相对于地理坐标系的运动来表示，运动的角度称为载体的姿态角。导航学中常用方位角Ψ、俯仰角θ和横滚角γ(倾斜角)作为载体的姿态角。

(1)、方位角：飞行器纵向轴(x_b轴)在水平面上的投影与地理子午线之间的夹角，数值以地理北向为起点顺时针方向为正，定义域为0～360度。飞行器的纵轴在水平面上的投影与地磁子午线之间的夹角称为磁方位角，用Ψ_M来表示。

(2)、俯仰角：飞行器的纵向轴(x_b轴)和当地水平面之间的夹角，向上为正，向下为负，定义域为-90～90度。

(3)、横滚角：飞行器的横向轴(y_b轴)和当地水平面之间的夹角，右倾为正，左倾为负，定义域为-90～90度。

c，姿态矩阵的推导

由空间三维坐标系之间的旋转变换关系可知：从地理坐标系ox_ny_nz_n中的向量最后旋转到机体坐标系ox_by_bz_b中的向量的关系如下：

$\left(\begin{array}{c}{H}_x^b\\ H_y^b\\ H_z^b\end{array}\right)=\left(\begin{array}{ccc}\cos \mathrm{\&gamma;}& 0& -\sin \mathrm{\&gamma;}\\ 0& 1& 0\\ \sin \mathrm{\&gamma;}& 0& \cos \mathrm{\&gamma;}\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ 0& -\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right)\left(\begin{array}{ccc}{\cos \mathrm{\&Psi;}}_M& {\sin \mathrm{\&Psi;}}_M& 0\\ -{\sin \mathrm{\&Psi;}}_M& {\cos \mathrm{\&Psi;}}_M& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}{H}_x^n\\ H_y^n\\ H_z^n\end{array}\right)$

因为方向余弦矩阵都是正交矩阵，所以它的逆矩阵存在、且等于它的转置矩阵，故从ox_by_bz_b旋转到ox_ny_nz_n的逆过程的转换关系如下：

$\left(\begin{array}{c}{H}_x^n\\ H_y^n\\ H_z^n\end{array}\right)=\left(\begin{array}{ccc}\cos {\mathrm{\&Psi;}}_M& -\sin {\mathrm{\&Psi;}}_M& 0\\ \sin {\mathrm{\&Psi;}}_M& \cos {\mathrm{\&Psi;}}_M& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&theta;}& -\sin \mathrm{\&theta;}\\ 0& \sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right)\left(\begin{array}{ccc}\cos \mathrm{\&gamma;}& 0& \sin \mathrm{\&gamma;}\\ 0& 1& 0\\ -\sin \mathrm{\&gamma;}& 0& \cos \mathrm{\&gamma;}\end{array}\right)\left(\begin{array}{c}{H}_x^b\\ H_y^b\\ H_z^b\end{array}\right)$

即姿态矩阵为：

$C_b^n=\left(\begin{array}{ccc}\cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}& \sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}\\ \cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}& \sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}\\ -\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&theta;}\cos \mathrm{\&gamma;}\end{array}\right)$

姿态测量系统中的加速度计的三轴分别沿机体坐标系的三轴安装，分别测出加速度计输出机体坐标系三轴上的投影分量，记为：

f^b=(accXaccYaccZ)^T

已知在地理坐标系下，加速度计的输出应该为：

fⁿ=(001)^T。

由于方位角的大小不影响加速度计的输出，因此假设ψ=0，则姿态矩阵简化为：

$C_b^n=\left(\begin{array}{ccc}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}& \sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}\\ 0& \cos \mathrm{\&gamma;}& -\sin \mathrm{\&gamma;}\\ -\sin & \cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&theta;}\cos \mathrm{\&gamma;}\end{array}\right).$

d，机体姿态角的求取

(1)、由 $f^n=C_b^n{f}^b\&DoubleRightArrow;\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)=\left(\begin{array}{ccc}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}& \sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}\\ 0& \cos \mathrm{\&gamma;}& -\sin \mathrm{\&gamma;}\\ -\sin & \cos \mathrm{\&theta;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&theta;}\cos \mathrm{\&gamma;}\end{array}\right)\left(\begin{array}{c}\mathrm{accX}\\ \mathrm{accY}\\ \mathrm{accZ}\end{array}\right).$

解该方程组，可得：

$\left\{\begin{array}{c}\mathrm{\&gamma;}=\arctan (\mathrm{accY}/\mathrm{accZ}),(\cos \mathrm{\&gamma;}\&NotEqual;0,\mathrm{\&gamma;}\&NotEqual;90)\\ \mathrm{\&theta;}=\arctan (-\mathrm{accX}/\sqrt{{\mathrm{accY}}^2+\mathrm{acc}{Z}^2})\end{array}\right.---\left(1\right)$

(2)、将由加速度计得到的姿态角θ、γ代入根据可以得到地磁场在水平面上的分量：

$\left\{\begin{array}{c}{H}_x^n=H_x^b\cos \mathrm{\&theta;}+H_y^b\sin \mathrm{\&theta;}\sin \mathrm{\&gamma;}+H_z^b\sin \mathrm{\&theta;}\cos \mathrm{\&gamma;}\\ H_y^n=H_y^b\cos \mathrm{\&gamma;}-H_z^b\sin \mathrm{\&gamma;}\end{array}\right.---\left(2\right)$

则方位角ψ为：

$\left\{\begin{array}{c}\mathrm{\&psi;}=\arctan (H_y^n/H_x^n),(H_y^n>0,H_x^n>0)\\ \mathrm{\&psi;}=\mathrm{PI}+\arctan (H_y^n/H_x^n),(H_y^n>0,H_x^n<0)\\ \mathrm{\&psi;}=\mathrm{PI}+\arctan (H_y^n/H_x^n),(H_y^n<0,H_x^n<0)\\ \mathrm{\&psi;}=2*\mathrm{PI}+\arctan (H_y^n/H_x^n),(H_y^n<0,H_x^n>0)\end{array}\right.---\left(3\right)$

(3)、由几何关系计算可知，机体坐标系ox_by_b平面与地理坐标系ox_ny_n平面的平面夹角α为：

$\mathrm{\&alpha;}=\arcsin \left(\sqrt{{\sin }^2\mathrm{\&theta;}+{\sin }^2\mathrm{\&gamma;}}\right)---\left(4\right)$

其中，θ、γ为机体俯仰角和横滚角。

步骤S102，根据所述RTK主机姿态和对中杆高度获取传感器延伸点坐标；

具体地，如图2所示，是本发明实施例空间几何示意图，其中GPS点为201，传感器x轴为202，传感器y轴为203，对中杆为204，地面点为205。

设对中杆长为L1，设定传感器上延长线上点到GPS点的距离和杆长相等，设传感器x轴和y轴上的延伸点到地面点的距离为L2和L3，且L2和L3相等，那么：

$L2=L3=\sqrt[2]{{L1}^2+{L1}^2}$

设地面点坐标为(x，y，h)，GPS点坐标为(x1，y1，h1)，通过姿态参数计算出传感器x轴和y轴上延伸点坐标(x2，y2，h2)和(x3，y3，h3)。

设计算出的俯仰角为Pitch，横滚角为Roll，方位角为Yaw。所述方位角是传感器上x轴的正方向和真正北方向的夹角(沿顺时针方向旋转)。

根据几何关系计算出传感器轴线上延伸坐标点：

X2=x1+L2*cos(Pitch)*cos(Yaw)；

y2=y1+L2*cos(Pitch)*sin(Yaw)；

h2=h1+L2*sin(Pitch)；

x3=x1+L3*cos(Roll)*cos(Yaw)；

y3=y1+L3*cos(Roll)*sin(Yaw)；

h3=h1+L3*sin(Roll)。

步骤S103，根据GPS点坐标、延伸点坐标获取需要的地面坐标。

在上面计算出GPS点和传感器的x轴和y轴的延伸坐标，并且计算出这三个点到地面点的距离，可以得出下面方程组：

(x-x1)²+(y-y1)²+(z-h1)²=L1²；

(x-x2)²+(y-y2)²+(z-h2)²=L2²；

(x-x3)²+(y-y3)²+(z-h3)²=L3²；

展开得：

x²+y²+z²-2(x1x+y1y+h1z)+x1²+y1²+h1²=L1²；①

x²+y²+z²-2(x2x+y2y+h2z)+x2²+y2²+h2²=L2²；②

x²+y²+z²-2(x3x+y3y+h3z)+x3²+y3²+h3²=L3²；③

分别作①-②、②-③、③-①得：

$(x2-x1)x+(y2-y1)y+(h2-h1)h=\frac{1}{2}({L1}^2-{x1}^2-{y1}^2-{h1}^2);$

$(x3-x2)x+(y3-y2)y+(h3-h2)h=\frac{1}{2}({L2}^2-{x2}^2-{y2}^2-{h2}^2);$

$(x1-x3)x+(y1-y3)y+(h1-h3)h=\frac{1}{2}({L3}^2-{x3}^2-{y3}^2-{h3}^2);$

设置中间变量A，B，C：

$A=\frac{1}{2}({L1}^2-{x1}^2-{y1}^2-{h1}^2);$

$B=\frac{1}{2}({L2}^2-{x2}^2-{y2}^2-{h2}^2);$

$C=\frac{1}{2}({L3}^2-{x3}^2-{y3}^2-{h3}^2);$

消去中间变量，得出x跟y、h的关系式：

$y=\frac{c1(A-B)-c(B-C)}{\mathrm{bc}1-b1c}-\frac{\mathrm{ac}1-a1c}{\mathrm{bc}1-b1c}x$

$h=\frac{b1(A-B)-b(B-C)}{\mathrm{cb}1-\mathrm{bc}1}-\frac{\mathrm{ab}1-a1b}{\mathrm{cb}1-\mathrm{bc}1}x$

其中变量a，b，c，a1，b1，c1为：

a=x1-x2；

b=y2-y1；

c=h2-h1；

a1=x3-x2；

b1=y3-y2；

c1=h3-h2；

将这两个变量关系式带入①中，利用一元二次方程的求根公式计算出x的两个解，再分别解出y和h的解。

对于两组方程解，因为待测点不可能在GPS点之上，所以取h最小的那个解为最终结果，即为地面点坐标。

如下表所示，为俯仰角为Pitch、横滚角为Roll、方位角为Yaw对应的地面点坐标(Dx，Dy，Dh)：

通过计算得到的修正数据dh小于mm级，完全可以修正定位数据；

常规用RTK测量地面点时必须先对中问题，再采集数据，这样工作效率很低，而且测量的精度和对中的精度相关，采用上述方式测量，不用对中，只需要对中杆底部在待测点上保持一定时间就可以得出精确坐标，解决人为对中问题和提高实际工作效率。

上述说明示出并描述了本发明的一个优选实施例，但如前所述，应当理解本发明并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述发明构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。

Claims (1)

1.一种地面坐标的测量方法，其特征在于，包括，

获取实时动态RTK主机的姿态，获取所述RTK主机的GPS点坐标；

根据所述RTK主机姿态和对中杆高度获取传感器延伸点坐标；

根据GPS点坐标、所述延伸点坐标获取需要的地面坐标；

所述RTK主机的姿态包括方位角Ψ、俯仰角θ和横滚角γ；

所述方位角ψ为：

$\left\{\begin{array}{c}\mathrm{\&psi;}=\arctan (H_y^n/H_x^n),(H_y^n>0,H_x^n>0)\\ \mathrm{\&psi;}=\mathrm{PI}+\arctan (H_y^n/H_x^n),(H_y^n>0,H_x^n<0)\\ \mathrm{\&psi;}=\mathrm{PI}+\arctan (H_y^n/H_x^n),(H_y^n<0,H_x^n<0)\\ \mathrm{\&psi;}=2*\mathrm{PI}+\arctan (H_y^n/H_x^n),(H_y^n<0,H_x^n>0)\end{array}\right.---\left(3\right)$

其中地理坐标系ox_ny_nz_n中的向量

机体坐标系ox_by_b平面与地理坐标系ox_ny_n平面的平面夹角α为：

$\mathrm{\&alpha;}=\arcsin \left(\sqrt{{\sin }^2\mathrm{\&theta;}+{\sin }^2\mathrm{\&gamma;}}\right)---\left(4\right)$

其中，θ、γ为机体俯仰角和横滚角；

所述根据所述RTK主机姿态和对中杆高度获取传感器延伸点坐标具体包括，

设对中杆长为L1，设定传感器上延长线上点到GPS点的距离和杆长相等，设传感器x轴和y轴上的延伸点到地面点的距离为L2和L3，且L2和L3相等，那么：

$L2=L3=\sqrt[2]{{L1}^2+{L1}^2};$

设地面点坐标为(x，y，h)，GPS点坐标为(x1，y1，h1)，通过姿态参数计算出传感器x轴和y轴上延伸点坐标(x2，y2，h2)和(x3，y3，h3)；

设计算出的俯仰角为Pitch，横滚角为Roll，方位角为Yaw，根据几何关系计算出传感器轴线上延伸坐标点：

X2＝x1+L2*cos(Pitch)*cos(Yaw)；

y2＝y1+L2*cos(Pitch)*sin(Yaw)；

h2＝h1+L2*sin(Pitch)；

x3＝x1+L3*cos(Roll)*cos(Yaw)；

y3＝y1+L3*cos(Roll)*sin(Yaw)；

h3＝h1+L3*sin(Roll)；

根据GPS点坐标、所述延伸点坐标获取需要的地面坐标具体包括，

根据GPS点坐标和传感器的x轴和y轴的延伸坐标，并且计算出这三个点到地面点的距离，可以得出下面方程组：

(x-x1)²+(y-y1)²+(z-h1)²＝L1²；

(x-x2)²+(y-y2)²+(z-h2)²＝L2²；

(x-x3)²+(y-y3)²+(z-h3)²＝L3²；

展开得：

x²+y²+z²-2(x1x+u1y+h1z)+x1²+y1²+h1²＝L1²；①

x²+y²+z²-2(x2x+y2y+h2z)+x2²+y2²+h2²＝L2²；②

x²+y²+z²-2(x3x+y3y+h3z)+x3²+y3²+h3²＝L3²；③

分别作①-②、②-③、③-①得：

$(x2-x1)x+(y2-y1)y+(h2-h1)h=\frac{1}{2}({L1}^2-{x1}^2-{y1}^2-{h1}^2);$

$(x3-x2)x+(y3-y2)y+(h3-h2)h=\frac{1}{2}({L2}^2-{x2}^2-{y2}^2-{h2}^2);$

$(x1-x3)x+(y1-y3)y+(h1-h3)h=\frac{1}{2}({L3}^2-{x3}^2-{y3}^2-{h3}^2);$

设置中间变量A，B，C：

$A=\frac{1}{2}({L1}^2-{x1}^2-{y1}^2-{h1}^2);$

$B=\frac{1}{2}({L2}^2-{x2}^2-{y2}^2-{h2}^2);$

$C=\frac{1}{2}({L3}^2-{x3}^2-{y3}^2-{h3}^2);$

消去中间变量，得出x跟y、h的关系式：

$y=\frac{c1(A-B)-c(B-C)}{\mathrm{bc}1-b1c}-\frac{\mathrm{ac}1-a1c}{\mathrm{bc}1-b1c}x$

$h=\frac{b1(A-B)-b(B-C)}{\mathrm{cb}1-\mathrm{bc}1}-\frac{\mathrm{ab}1-a1b}{\mathrm{cb}1-\mathrm{bc}1}x$

其中变量a，b，c，a1，b1，c1为：

a＝x1-x2；b＝y2-y1；c＝h2-h1；a1＝x3-x2；b1＝y3-y2；c1＝h3-h2；

将这两个变量关系式带入①中，计算出x、y、h的解。