Memorandum

New version Kouhei's Homepage

GPSの座標変換

GPSの座標変換について
GPSはアンテナと衛星までの距離を測るセンサーです。
4つ以上の衛星とアンテナ間の距離を測ることによりアンテナの3次元的な位置を割り出します。
NMEA0183のGPGGAセンテンスでは緯度、経度、高さ、ジオイド高等が得られます。(ジオイド高をどうやって得ているのかは今のところ解りません)
移動ロボットの姿勢(位置と向いている方向を含めて姿勢と言っています。)に関しては、緯度経度ではだいぶ使いにくいので、直交座標系で話ができると楽です。
そこで緯度経度から地球中心の直交座標へ変換して更に、地表のある点を原点として、地面が平面だと近似した地平座標に変換してやります。



ECEF座標へ
地球の中心を原点としたデカルト座標をECEF(Earth Center Earth Fixed)座標と言います。変換式はプログラム言語風に書くと以下の様になります。

Lat:緯度
Lon:経度
High:海抜高度(標高)
N:ジオイド高
PI:円周率
Xe,Ye,Ze:ECEF座標

f = 1 / 298.257223563;

a = 6378137;

e = Sqrt(f * (2 - f));

R = a / Sqrt(1 - e*e * Sin(Lat * PI / 180) * Sin(Lat * PI /180));

Xe = (R + High + N) * Cos(Lat * PI / 180) * Cos(Lon * PI / 180);

Ye = (R + High + N) * Cos(Lat * PI / 180) * Sin(Lon * PI / 180);

Ze = (R * (1 - e*e) + High + N) * Sin(Lat * PI / 180);

ENU座標(地平座標)へ
地平座標をENU座標と言います。Eは東西の東のEast、Nは南北の北のNorth、UはアップダウンのUpを表します。緯度経度からENUへはECEFに変換したものもつかいます。またENUでは地球平面上の何処が原点なのかあらかじめ決めておきECEFで保持しておかなければなりません。以下に変換式をプログラム言語風に書くと以下の様になります。

Xe,Ye,Ze:位置を変換したい点のECEF座標
x0,y0,z0:地平座標の原点としたい点のECEF座標
Lat,Lon:位置を変換したい点の緯度経度

E = -Sin(Lon * PI / 180) * (Xe - x0) + Cos(Lon * PI / 180) * (Ye - y0);

N = -Sin(Lat * PI / 180) * Cos(Lon * PI / 180) * (Xe - x0) - Sin(Lat * PI / 180) * Sin(Lon * PI / 180) * (Ye - y0) + Cos(Lat * PI / 180) * (Ze - z0);

U = Cos(Lat * PI / 180) * Cos(Lon * PI / 180) * (Xe - x0) + Cos(Lat) * Sin(Lon * PI / 180) * (Ye - y0) + Sin(Lat * PI / 180) * (Ze - z0);

参考文献:http://www.enri.go.jp/~fks442/K_MUSEN/

ページ

Twitter

    follow me on Twitter

    フォロワー