GNSS學習筆記-座標轉換

GNSS 座標轉換

GNSS計算主要涉及三個座標系，地心地固座標系，地理座標系和站心座標系。這裏主要介紹一下三個座標的含義和轉換公式。

• 地心地固座標系如圖X,Y,Z表示 (ECEF座標系)，以地心O爲座標原點，Z軸指向協議地球北極，X軸指向參考子午面與地球赤道的交點，也叫地球座標系。通常GNSS座標計算都在地心地固座標系下進行的。因爲地球是橢圓形，有WGS-84和CGC2000等多種標準

• 地理座標系則經過經度(longitude)，緯度(latitude)和高度(altitude)來表示地球的位置，也叫經緯高座標系(LLA座標系)。

• 站心座標系以用戶所在位置P爲座標原點，三個軸分別指向東向，北向和天向，也叫東北天座標系(enu座標系)。站心座標系的天向方向和地理座標系的高度方向是一致的。站心座標系用在慣性導航和衛星俯仰角計算中較多。

參數	WGS-84	CGC200
基準橢球體的長半徑a	6378137.0 m	6378137.0 m
基準橢球體的極扁率f	1/298.257223565	1/298.257223563
地球自轉角速度We	7.2921151467*1e-5	7.2921151467*1e-5
地球引力和地球質量的乘積GM	3986004.418*1e8	3986004.418*1e8
光速	2.99792458*1e8 m/s	2.99792458*1e8 m/s

LLA座標系轉ECEF座標系

LLA座標系下的(lon,lat,alt)轉換爲ECEF座標系下點(X,Y,Z)

\[\begin{cases} X=(N+alt)cos(lat)cos(lon)\\ Y=(N+alt)cos(lat)sin(lon)\\ Z=(N(1-e^2)+alt)sin(lat) \end{cases}\]

其中e爲橢球偏愛率，N爲基準橢球體的曲率半徑

\[\begin{cases} e^2=\frac{a^2-b^2}{a^2}\\ N=\frac{a}{\sqrt{1-e^2sin^2lat}} \end{cases}\]

因爲WGS-84下極扁率\(f=\frac{a-b}{a}\),偏愛率e和極扁率f之間的關係：

\[e^2=f(2-f) \]

座標轉換公式也能夠爲

\[\begin{cases} X=(N+alt)cos(lat)cos(lon)\\ Y=(N+alt)cos(lat)sin(lon)\\ Z=(N(1-f)^2+alt)sin(lat) \end{cases}\]

\[N=\frac{a}{\sqrt{1-f(2-f)sin^2lat}} \]

python實現

def lla2ecef(lat,lon,alt):
    WGS84_A = 6378137.0
    WGS84_f = 1/298.257223565
    WGS84_E2 = WGS84_f*(2-WGS84_f)
    deg2rad = math.pi/180.0
    rad2deg = 180.0/math.pi
    lat *= deg2rad
    lon *= deg2rad
    N = WGS84_A/(math.sqrt(1-WGS84_E2*math.sin(lat)*math.sin(lat)))
    x = (N+alt)*math.cos(lat)*math.cos(lon)
    y = (N+alt)*math.cos(lat)*math.sin(lon)
    z = (N*(1-WGS84_f)*(1-WGS84_f)+alt)*math.sin(lat)
    return [x,y,z]

ECEF座標系轉LLA座標系

ECEF座標系下點(X,Y,Z)轉換爲LLA座標系下的(lon,lat,alt)

\[lon=arctan(\frac{y}{x}) \]

\[alt=\frac{p}{cos(lat)-N} \]

\[lat=arctan\bigg[\frac{z}{p}\bigg(1-e^2\frac{N}{N+alt}\bigg)^{-1}\bigg] \]

\[p=\sqrt{x^2+y^2} \]

一開始lon是未知的，能夠假設爲0，通過幾回迭代以後就能收斂

ECEF座標系轉enu座標系

用戶所在座標點\(P_0=(x_0,y_0,z_0)\),，計算點\(P=(x,y,z)\)在以點\(P_{0}\)爲座標原點的enu座標系位置\((e,n,u)\)這裏須要用到LLA座標系的數據，\(P_0\)的LLA座標點爲\(LLA_0=(lon_0,lat_0,alt_0)\)

\[\begin{gathered} \left[ \begin{array}{ccc} \Delta{x}\\\Delta{y}\\\Delta{z} \end{array} \right]= \left[ \begin{array}{ccc} x\\y\\z\end{array}\right]- \left[ \begin{array}{ccc} x_0\\y_0\\z_0\end{array}\right] \end{gathered} \]

\[\begin{gathered} \left[ \begin{array}{ccc} e\\n\\u \end{array} \right]=S\cdot \left[ \begin{array}{ccc} \Delta{x}\\\Delta{y}\\\Delta{z} \end{array} \right] \end{gathered}= \left[ \begin{array}{ccc} -sin(lon_0) & cos(lon_0) & 0 \\ -sin(lat_0)cos(lon_0) & -sin(lat_0)sin(lon_0) & cos(lat_0) \\ cos(lat_0)cos(lon_0) & cos(lat_0)sin(lon_0) & sin(lat_0) \end{array} \right]\cdot \left[ \begin{array}{ccc} \Delta{x}\\\Delta{y}\\\Delta{z} \end{array} \right] \]

即座標變換矩陣\(S=\left[ \begin{array}{ccc} -sin(lon_0) & cos(lon_0) & 0 \\ -sin(lat_0)cos(lon_0) & -sin(lat_0)sin(lon_0) & cos(lat_0) \\ cos(lat_0)cos(lon_0) & cos(lat_0)sin(lon_0) & sin(lat_0) \end{array} \right]\)

enu座標系轉ECEF座標系

\(S\)爲單位正交矩陣

\[\mathbf{S}^{-1}=\mathbf{S}^\mathrm{T} \]

反之

\[\begin{gathered} \left[ \begin{array}{ccc} \Delta{x}\\\Delta{y}\\\Delta{z}\end{array} \right]=S^{-1}\cdot\left[ \begin{array}{ccc} e\\n\\u\end{array} \right]= \mathbf{S}^\mathrm{T}\cdot\left[ \begin{array}{ccc} e\\n\\u\end{array} \right] \end{gathered} \]

LLA座標系轉enu座標系

上述能夠看到，從LLA座標系轉換到enu座標系有較多計算量，在考慮地球偏愛率\(e\)很小的前提下，能夠作必定的近似公式計算

\[\left[ \begin{array}{ccc} \Delta e\\ \Delta n \\ \Delta u \end{array} \right]= \left[\begin{array}{ccc} a\cdot cos(lat)\cdot \Delta lon & 0 & 0 \\ 0 & a \cdot \Delta lat & 0 \\ 0 & 0 & \Delta alt \end{array} \right] \]