大地經緯度座標與地心地固座標的的轉換
1. 概述
要解決這個問題首先得理解地球橢球這個概念，這裡直接用武漢大學《大地測量學基礎》（孔詳元、郭際明、劉宗全）的解釋吧：
大地經緯度座標系是地理座標系的一種，也就是我們常說的經緯度座標+高度。經緯度座標用的雖然多，但是很多人並沒有理解經緯度的幾何意義：緯度是一種線面角度，是座標點P的法線與赤道面的夾角（注意這個法線不一定經過球心）；經度是面面角，是座標點P所在的的子午面與本初子午面的夾角。這也是為什麼經度範圍是-180 ~ +180，緯度範圍卻是-90 ~ +90：
地心地固座標系就是我們常用的笛卡爾空間直角座標系了。這個座標系以橢球球心為原點，本初子午面與赤道交線為X軸，赤道面上與X軸正交方向為Y軸，橢球的旋轉軸(南北極直線)為Z軸。顯然，這是個右手座標系：
顯然，兩者都是表達的都是空間中某點P，只不過一個是經緯度座標（BLH），一個是笛卡爾座標（XYZ）；兩者是可以相互轉換的。
2. 推導
2.1. BLH->XYZ
將P點所在的子午橢圓放在平面上，以圓心為座標原點，建立平面直接座標系：
對照地心地固座標系，很容易得出：
\[\begin{cases} Z = y\\ X = x \cdot cosL\\ Y = x \cdot sinL\\ \end{cases} \tag{1} \]那麼，關鍵問題在於求子午面直角座標系的x,y。過P點作原橢球的法線Pn，他與子午面直角座標系X軸的夾角為B；過P點作子午橢圓的切線，它與X軸的夾角為(90°+B):
根據橢圓的方程，位於橢圓的P點滿足：
\[\frac{x^2}{a^2} + \frac{y^2}{b^2} = 1 \tag{1.2} \]對x求導，有：
\[\frac{dy}{dx} = -\frac{b^2}{a^2} \cdot \frac{x}{y} \tag{2} \]又根據解析幾何可知，函式曲線（橢圓）某一點（就是P點）的倒數為該點切線的斜率，也就是正切值：
\[\frac{dy}{dx} = tan(90^o + B) = -cotB \tag{3} \]聯立公式（2）(3)，可得：
\[y = x(1-e^2)tanB \tag{4} \]其中，e為橢圓第一偏心率：
令Pn的距離為N，那麼顯然有：
\[x = NcosB \tag{4-2} \]根據（4）式可得：
\[y = N(1-e^2)sinB \tag{4-3} \]將其帶入（1）式，可得到橢球上P點的座標為：
\[\begin{cases} X = NcosBcosL\\ Y = NcosBsinL\\ Z = N(1-e^2)sinB\\ \end{cases} \tag{5} \]那麼唯一的未知量就是Pn的長度N了，將（4）式帶入到橢圓方程式(1.2)：
\[\frac{x^2}{a^2} + \frac{x^2(1-e^2)^2tan^2B}{b^2} = 1 \]化簡，得：
\[x = \frac{acosB}{\sqrt{1-e^2sin^2B}} \tag{6} \]聯立式（5）式（6），得：
\[N = \frac{a}{\sqrt{1-e^2sin^2B}} \tag{6} \]通過式（5）式（6），可以計算橢球上某一點的座標。但這個點並不是我們真正要求的點，我們要求的點P(B,L,H)是橢球面沿法向量向上H高度的點：
P點在橢球面上的點為\(P_0\)，那麼根據向量相加的性質，有：
\[P = P_0 + H \cdot n \tag{6} \]其中，\(P_0\)也就是式(5)，而n是\(P_0\)在橢球面的法線單位向量。
向量在任意位置的方向都是一樣的，那麼我們可以假設存在一個單位球(球的半徑為單位1)，將法線單位向量移動到球心位置，可得法線單位向量為：
\[n = \left[ \begin{matrix} cosBcosL \\ cosBsinL \\ sinB \\ \end{matrix} \right] \tag{7} \]因此有：
\[P = \left[ \begin{matrix} X \\ Y \\ Z \\ \end{matrix} \right]= \left[ \begin{matrix} (N+H)cosBcosL \\ (N+H)cosBsinL \\ [N(1-e^2) + H]sinB \\ \end{matrix} \right] \tag{8} \]其中：
\[N = \frac{a}{\sqrt{1-e^2sin^2B}} \tag{9} \]2.2. XYZ->BLH
根據式（8），可知：
\[\frac{Y}{X} = \frac{(N+H)cosBsinL}{(N+H)cosBcosL} = tanL \]因此有：
\[L = arctan(\frac{Y}{X}) \tag{10} \]不過緯度Ｂ就不是那麼好算了，首先需要計演算法線Pn在赤道兩側的長度。根據圖1，有：
\[y = PQsinB \]與式（4-3）比較可得：
\[PQ = N(1-e^2) \]顯然，由於：
\[Pn = N = PQ + Qn \]有：
\[Qn = Ne^2 \]接下來如下圖所示，對圖1做輔助線：
有：
\[\begin{cases} PP'' = Z\\ OP'' = \sqrt{x^2+y^2}\\ PP''' = OK_p = QK_psinB = Ne^2sinB\\ P''P''' = PP''' + PP'' \end{cases} \]因而可得：
\[tanB = \frac{Z+Ne^2sinB}{\sqrt{x^2+y^2}} \tag{11} \]這個式子兩邊都有待定量B，需要用迭代法進行求值。具體可參看程式碼實現，初始的待定值可取\(tanB = \frac{z}{\sqrt{x^2+y^2}}\)。
大地緯度B已知，那麼求高度H就非常簡單了，直接根據式（8）中的第三式逆推可得：
\[H = \frac{Z}{sinB} - N(1-e^2) \tag{12} \]彙總三式，可得：
\[\begin{cases} L = arctan(\frac{Y}{X})\\ tanB = \frac{Z+Ne^2sinB}{\sqrt{x^2+y^2}}\\ H = \frac{Z}{sinB} - N(1-e^2)\\ \end{cases} \]3. 實現
根據前面的推導過程，具體的C/C++程式碼實現如下：
#include <iostream>
using namespace std;
const double epsilon = 0.000000000000001;
const double pi = 3.14159265358979323846;
const double d2r = pi / 180;
const double r2d = 180 / pi;
const double a = 6378137.0; //橢球長半軸
const double f_inverse = 298.257223563; //扁率倒數
const double b = a - a / f_inverse;
//const double b = 6356752.314245; //橢球短半軸
const double e = sqrt(a * a - b * b) / a;
void Blh2Xyz(double &x, double &y, double &z)
{
double L = x * d2r;
double B = y * d2r;
double H = z;
double N = a / sqrt(1 - e * e * sin(B) * sin(B));
x = (N + H) * cos(B) * cos(L);
y = (N + H) * cos(B) * sin(L);
z = (N * (1 - e * e) + H) * sin(B);
}
void Xyz2Blh(double &x, double &y, double &z)
{
double tmpX = x;
double temY = y ;
double temZ = z;
double curB = 0;
double N = 0;
double calB = atan2(temZ, sqrt(tmpX * tmpX + temY * temY));
int counter = 0;
while (abs(curB - calB) * r2d > epsilon && counter < 25)
{
curB = calB;
N = a / sqrt(1 - e * e * sin(curB) * sin(curB));
calB = atan2(temZ + N * e * e * sin(curB), sqrt(tmpX * tmpX + temY * temY));
counter++;
}
x = atan2(temY, tmpX) * r2d;
y = curB * r2d;
z = temZ / sin(curB) - N * (1 - e * e);
}
int main()
{
double x = 113.6;
double y = 38.8;
double z = 100;
printf("原大地經緯度座標：%.10lf\t%.10lf\t%.10lf\n", x, y, z);
Blh2Xyz(x, y, z);
printf("地心地固直角座標：%.10lf\t%.10lf\t%.10lf\n", x, y, z);
Xyz2Blh(x, y, z);
printf("轉回大地經緯度座標：%.10lf\t%.10lf\t%.10lf\n", x, y, z);
}
其最關鍵的還是計算大地緯度B時的迭代過程，其餘的計算都只是套公式。數值計算中的很多演算法都是採用迭代趨近的方法來趨近一個最佳解。最後的執行結果如下：