地理测绘基础知识(2)-椭球最短距离计算

在上一篇中，我们介绍了ECEF坐标系和经纬度的互换。

本篇，主要介绍已知A\B两个点的经纬度，如何求取椭球上的最短距离、路径。

在标准椭球面上，从A点运动到B点，距离如何，轨迹、每个阶段的方向又是如何呢？ 要讨论方向，会引出两个概念。第一个是切平面坐标系，这是讨论"方向"的基础。第二个是运动，即考虑不同时刻、不同位置之间的关系与变化规律。

1 切面坐标系

站在地表，一般使用的是东北坐标系ENU，即切平面坐标。从ECEF坐标转换为ENU （East-North-Up）坐标，如下如所示：

图中，由法向量$\overrightarrow{tT}$定义的切平面构成了“天圆地方”的参考平面。法向量就是这个坐标系的Z轴。由于尺度不变，这个坐标系和ECEF坐标系是一个旋转关系。

E(East) 单位矢量在经度、纬度均为0度时，切面坐标系的E轴恰好指向正东，就是ECEF的Y方向，取值{0,1,0} 。当经度提高，在X-方向开始有了投影。坐标轴E和纬度无关。旋转经度$\theta$后得到；ECEF坐标系下，E轴的单位矢量：

E ⃗ = { − sin ⁡ θ , cos ⁡ θ , 0 } \vec E=\{-\sin \theta,\cos \theta,0\} E ={−sinθ,cosθ,0}

N(North) 单位矢量在经度、纬度均为0度时，切面坐标系的E轴恰好指向正北，就是ECEF的Z方向，取值{0,0,1} 。当纬度上升时，在X-方向开始有了投影；ECEF坐标系下，N轴的单位矢量：

N ⃗ = { − sin ⁡ φ c o s θ , − sin ⁡ φ sin ⁡ θ , cos ⁡ φ } \vec N=\{-\sin \varphi cos\theta,-\sin \varphi \sin \theta,\cos \varphi\} N ={−sinφcosθ,−sinφsinθ,cosφ}

U(Up) 单位矢量在经度、纬度均为0度时，切面坐标系的E轴恰好指向{1,0,0}方向。经过纬度旋转、经度旋转后，ECEF坐标系下，U轴的单位矢量：

U ⃗ = { cos ⁡ φ c o s θ , cos ⁡ φ sin ⁡ θ , sin ⁡ φ } \vec U=\{\cos \varphi cos\theta,\cos \varphi \sin \theta,\sin \varphi\} U ={cosφcosθ,cosφsinθ,sinφ}

要计算站在t位置下，查看任意一点 M 的 ENU 坐标，只要执行运算

$$\begin{gathered} {\overrightarrow{M_{ENU}}}^T=\left[\begin{array}{c}\vec{E} \\ \vec{N} \\ \vec{U}\end{array}\right]\times {\left(\overrightarrow{M_{ECEF}}-\vec{t}\right)}^T=R\times {\left(\overrightarrow{M_{ECEF}}-\vec{t}\right)}^T \end{gathered}$$

上式中坐标都是行向量,转置后是列向量。需要注意的是，这个旋转矩阵R的左逆、右逆都是自身的转置,故而两类坐标的转换就很方便了。

$$R=\left[\begin{array}{ccc}-\sin \theta & \cos \theta & 0\\ -\sin \phi cos\theta & -\sin \phi \sin \theta & \cos \phi \\ \cos \phi cos\theta & \cos \phi \sin \theta & \sin \phi \end{array}\right]$$

$$R\times R^T=R^T\times R=I$$

主要接口:

/*!* \brief enu_R_from_lla 计算东北天ENU坐标系的旋转矩阵R* \param lla 原点经纬度* \param R   R为行向量* \param rad 角度量纲开关，false 是度，true 是弧度* \param latfirst 经纬度顺序，false 是经度\纬度\高度，true 是纬度\经度\高度*/
inline void enu_R_from_lla(const double lla[/*3*/],double R[/*3*/][3],const bool rad = false,const bool latfirst = true);/*!* \brief ecef2enu	从ECEF到东北天ENU坐标系* \param R_A		原点A的旋转阵，通过enu_R_from_lla计算* \param A_ecef	原点A的ECEF* \param B_ecef	B的ECEF* \param B_enu		B的东北天ENU坐标*/
inline void ecef2enu(const double R_A	[/*3*/][3],const double A_ecef	[/*3*/],const double B_ecef	[/*3*/],double B_enu		[/*3*/]);/*!* \brief enu2ecef	从东北天ENU坐标系到ECEF* \param R_A		原点A的旋转阵，通过enu_R_from_lla计算* \param A_ecef	原点A的ECEF* \param B_enu		B的东北天ENU坐标* \param B_ecef	B的ECEF*/
inline void enu2ecef(const double R_A	[/*3*/][3],const double A_ecef	[/*3*/],const double B_enu	[/*3*/],double B_ecef		[/*3*/]);

2 两点间椭球轨迹规划

椭球的最短路径是一个三维空间曲线。在轨迹的各个位置上，由于经纬度不同，椭球的曲率不断变化。同时，理想参考椭球是局部平坦的凸曲面，没有陡峭的局部峰和谷，这使得可以通过"碎步迭代"的方法，较为精确地获得最短路径。

迭代命题：已知起点A、终点B的经纬度，求取在高度H上从A运动到B的最短椭球路径，并计算总距离。

设置距离数值积分量$D$ 初始化为0.

迭代步骤:

(1) 在当前ENU坐标系下确定方向

把 A,B换算到ECEF下，此刻ECEF向量$\overrightarrow{B-A}$在切平面上的投影指示了最佳方向。在A点建立ENU坐标系，A是原点，此时，B的ENU坐标：

$$\begin{gathered} {\vec{B}}_{ENU}=\left[\begin{array}{c}{E}_B \\ {N}_B \\ {U}_B\end{array}\right]=R_A\times {\left(\vec{B}-\vec{A}\right)}^T \end{gathered}$$

在ENU坐标系里，方向定义为正北为0度，顺时针递增:

$$\tan C=\frac{E_B}{N_B}$$

计算出C后，即可计算ENU单位矢量

C E N U ⃗ = { sin ⁡ C , cos ⁡ C , 0 } \vec{C_{ENU}}=\{ \sin C, \cos C,0 \} CENU​ ​={sinC,cosC,0}

该单位矢量转换到 ECEF下为

$$\overrightarrow{C^T}=R_A^T\overrightarrow{C_{ENU}}$$

(2) 小步推进

已经知道当前起点A、初始方向C后，在以当前位置A为原点的ENU坐标系下，沿着设置的方向前进一个小距离$\Delta$, 得到下一个位置 A’。

由于$\Delta$很小，我们认为当前行进的距离，就是以A所在位置为切面的正球上行进的距离。

A 的正球坐标为 A s ⃗ = A ⃗ + { 0 , 0 , − d } \vec {A_s}= \vec A +\{0,0,-d\} As​ ​=A +{0,0,−d}

同时，方向C的单位矢量不受平移的影响，指向不变，$\overrightarrow{C_s}=\vec{C}$。

沿着方向矢量${\vec{C}}_s$、正球矢量$\overrightarrow{A_s}$生成的正圆，从矢量$\overrightarrow{A_s}$处，旋转$\omega$弧度，就是下一个位置的正球坐标。旋转的弧度可以用下式求解。

$$cos\omega =\frac{\Delta }{r_A+H}$$

这里关键是通过弧度得到旋转后的矢量A’,需要用到三维旋转，也就是罗德里格旋转公式.

设

$$\vec{\mu }=\frac{{\vec{A}}_s\otimes \overrightarrow{C_s}}{\left|{\vec{A}}_s\right|}$$

$$\overrightarrow{A_s^{\prime }}=cos\omega \cdot \overrightarrow{A_s}+(1-\cos \omega )(\vec{\mu }\odot \overrightarrow{A_s})+sin\omega \cdot \vec{\mu }\otimes \overrightarrow{A_s}$$

由于法向量本身就是垂直的，因此投影项为0，上式简化为:
$$\overrightarrow{A_s^{\prime }}=cos\omega \cdot \overrightarrow{A_s}+sin\omega \cdot \vec{\mu }\otimes \overrightarrow{A_s}$$

最后，把正球下的坐标系平移到ECEF

A ’ ⃗ = A s ′ ⃗ + { 0 , 0 , d } \vec {A’}= \vec {A'_s} +\{0,0,d\} A’ =As′​ ​+{0,0,d}

通过A’的ECEF，计算A’的经纬度坐标，并设置高度为H。此时，A’的经纬度就是下一位置。

(3) 距离累加

由于$\Delta$很小，我们认为当前行进的距离，就是以A所在位置为切面的正球上行进的距离。

$$D=D+(r_A+H)\omega$$

(4) 继续迭代

推进A到新的位置 A’

$$A=A^{\prime }$$

并转到(1)。

(5)收敛条件

当A、B距离小于$\Delta$时，以AB的正球距离为距离，最终确定总距离。

软件接口：

/*!* \brief ellips_geodesic	椭球的最短测地线求取* \param llaA_in			A点经纬度* \param llaB_in			B点经纬度* \param maxResults		结果的最大长度* \param resultSpan		结果的间隔为多个步长* \param res_lla			存储经纬度的结果* \param res_ecef			存储ECEF的结果* \param res_Distance		存储距离的结果* \param res_Direction		存储方向的结果* \param res_err			存储剩余距离的结果* \param rad				经纬度为弧度* \param latfirst			纬度在前* \param enudelta			最大步长* \return					结果个数*/
inline int ellips_geodesic(const double llaA_in[/*3*/],const double llaB_in[/*3*/],const int maxResults,const int resultSpan=1,double (*res_lla)[3] = nullptr,double (*res_ecef)[3] = nullptr,double (*res_Distance) = nullptr,double (*res_Direction) = nullptr,double (*res_err) = nullptr,const bool rad = false,const bool latfirst = true,const double enudelta = 1000)

3 进行测试

设置起点与终点，进行测试：

void demo_ellips_geodesic()
{printf("\n\n");const double LLA_A[] = {31.847774515, 117.292115092, 10000};const double LLA_B[] = {37.807300349, -122.366731167,10000};const int maxRes = 32768;double (* lla)[3] = new double [maxRes][3];double (* ecef)[3] = new double [maxRes][3];double * distance = new double [maxRes];double * direction = new double [maxRes];double * err = new double [maxRes];using namespace CES_GEOCALC;const int step = 1000;int res = ellips_geodesic(LLA_A,LLA_B,maxRes,100000/step,lla,ecef,distance,direction,err,false,true,step);for(int i=0;i<res;++i){printf("I=%d LAT=%12.7lf LON=%12.7lf ALT=%7.1lf DIS=%8.1lf DIR=%12.7lf ERR=%lf\n",i,lla[i][0],lla[i][1],lla[i][2],distance[i],direction[i],err[i]);}delete [] lla;delete [] ecef;delete [] distance;delete [] direction;delete [] err;
}

输出：

I=0 LAT=  31.8477745 LON= 117.2921151 ALT=10000.0 DIS=  1000.0 DIR=  43.0147959 ERR=9113951.151266
I=1 LAT=  32.5040600 LON= 118.0168980 ALT=10000.0 DIS=101000.0 DIR=  43.3985947 ERR=9043525.166938
I=2 LAT=  33.1560649 LON= 118.7522233 ALT=10000.0 DIS=201000.0 DIR=  43.7951513 ERR=8972543.435380
I=3 LAT=  33.8036190 LON= 119.4984456 ALT=10000.0 DIS=301000.1 DIR=  44.2047633 ERR=8901010.433060
I=4 LAT=  34.4465456 LON= 120.2559264 ALT=10000.0 DIS=401000.1 DIR=  44.6277359 ERR=8828930.668846
……
I=102 LAT=  37.8073003 LON=-122.3667312 ALT=10000.0 DIS=10143664.8 DIR= 132.8475527 ERR=0.000000

此轨迹在莫卡托投影下会发生形变，如下图：

4 起点方向椭球轨迹迭代

上面的命题是已知起点、终点，进行路径规划。如果是已知起点、初始方向，则需要进行路径迭代了。由于椭球和正球的曲率不同，虽然在切点t处，二者的法向量是重合的，但一旦离开t处，便会形成误差。

需要在正球模型较为局部的 $\omega$弧度内（弧长 $\Delta$），通过向量旋转求取下一个位置、方向。 上面的第(2)步，已经求取了 A’的位置。在A’上切线的方向，原先是通过观测终点B确定的。然而，在本节的推导中，B是不存在的。我们可以使用正球的切线，近似为真实的方向。

在A’的正球模型下，$d_2$是新位置的Z截距，新的切线:

A s 2 ′ = A ’ ⃗ + { 0 , 0 , − d 2 } As_2' = \vec {A’} + \{0,0,-d_2\} As2′​=A’ +{0,0,−d2​}

$$C_2^{\prime }=C_{s2}^{\prime }=\frac{\mu \otimes A{s}_2^{\prime }}{|A{s}_2^{\prime }|}$$

该单位矢量需要旋转到 $A_{ENU}^{\prime }$ 坐标系下

$$C_{ENU}^{\prime }=R_{A^{\prime }}\times C_2^{\prime }$$

最终，通过反正切求取角度：

$$\tan C^{\prime }=\frac{E^{\prime }}{N^{\prime }}$$

只要不断使用C’进行迭代，就可以得到完整的轨迹。

软件接口：

/*!* \brief ellips_range	椭球的最短测地线求取* \param llaA_in			A点经纬度* \param range				行进的距离* \param range				起始ENU方向* \param maxResults		结果的最大长度* \param resultSpan		结果的间隔为多个步长* \param res_lla			存储经纬度的结果* \param res_ecef			存储ECEF的结果* \param res_Distance		存储距离的结果* \param res_Direction		存储方向的结果* \param res_err			存储剩余距离的结果* \param rad				经纬度为弧度* \param latfirst			纬度在前* \param enudelta			最大步长* \return					结果个数*/
inline int ellips_range(const double llaA_in[/*3*/],const double range,const double direction,const int maxResults,const int resultSpan=1,double (*res_lla)[3] = nullptr,double (*res_ecef)[3] = nullptr,double (*res_Distance) = nullptr,double (*res_Direction) = nullptr,double (*res_err) = nullptr,const bool rad = false,const bool latfirst = true,const double enudelta = 1000)

5 完整工程

代码与工程参考

https://gitcode.net/coloreaglestdio/geocalc/-/blob/master/geocalc.h