Ruby - WGS84 (BLH) 座標 -> ENU 座標 変換!

更新日時:


少し前に、 BLH 座標(WGS84 の緯度(Beta)/経度(Lambda)/楕円体高(Height))から ECEF(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)座標への変換や、その逆の変換の処理を Ruby で実装しました。

今回は、 BLH 座標から ENU 座標(地平座標; EastNorthUp)への変換処理を Ruby で実装してみました。

0. 前提条件

  • Ruby 2.6.3 での動作を想定。
  • ここでは、 WGS84(World Geodetic System 1984) 測地系、 ECEF 座標(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)、 ENU 座標(地平座標)の詳細についての説明はしない。

1. Ruby スクリプトの作成

  • Shebang ストリング(1行目)では、フルパスでコマンド指定している。(当方の慣習
  • ENU 座標の他に方位角・仰角・距離も計算している。

File: blh2enu.rb

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
#! /usr/local/bin/ruby
#---------------------------------------------------------------------------------
# BLH -> ENU 変換
# : WGS84 の緯度(Beta)/経度(Lambda)/楕円体高(Height)を
#   ENU (East/North/Up; 地平) 座標に変換する。
#   * 途中、 ECEF(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)座標
#     への変換を経由。
#
# Date          Author          Version
# 2019.02.20    mk-mode.com     1.00 新規作成
#
# Copyright(C) 2019 mk-mode.com All Rights Reserved.
#---------------------------------------------------------------------------------
#
class ConvBlh2Enu
  USAGE =  "[USAGE] ./blh2enu.rb  BETA_O LAMBDA_O HEIGHT_O BETA LAMBDA HEIGHT\n"
  USAGE << "        (BETA: LATITUDE, LAMBDA: LONGITUDE)"
  PI_180 = Math::PI / 180.0
  # WGS84 座標パラメータ
  A      = 6378137.0                # a(地球楕円体長半径(赤道面平均半径))
  ONE_F  = 298.257223563            # 1 / f(地球楕円体扁平率=(a - b) / a)
  B      = A * (1.0 - 1.0 / ONE_F)  # b(地球楕円体短半径)
  E2     = (1.0 / ONE_F) * (2 - (1.0 / ONE_F))
                                    # e^2 = 2 * f - f * f
                                    #     = (a^2 - b^2) / a^2
  ED2    = E2 * A * A / (B * B)     # e'^2= (a^2 - b^2) / b^2

  # Initialization
  # * コマンドライン引数の取得
  def initialize
    if ARGV.size < 6
      puts USAGE
      exit
    end
    @blh_o = ARGV[0, 3].map(&:to_f)
    @blh   = ARGV[3, 3].map(&:to_f)
  end

  # Execution
  def exec
    puts "BLH_O: LATITUDE(BETA) = %12.8f°" % @blh_o[0]
    puts "    LONGITUDE(LAMBDA) = %12.8f°" % @blh_o[1]
    puts "               HEIGHT = %12.3fm"  % @blh_o[2]
    puts "BLH:   LATITUDE(BETA) = %12.8f°" % @blh[0]
    puts "    LONGITUDE(LAMBDA) = %12.8f°" % @blh[1]
    puts "               HEIGHT = %12.3fm"  % @blh[2]
    enu = blh2enu(@blh_o, @blh)
    az = Math.atan2(enu[0], enu[1]) / PI_180
    az += 360.0 if az < 0.0
    el = Math.atan2(
        enu[2],
        Math.sqrt(enu[0] * enu[0] + enu[1] * enu[1])
    ) / PI_180
    dst = Math.sqrt(enu.map { |a| a * a }.inject(:+))
    puts "--->"
    puts "ENU: E = %12.3fm"  % enu[0]
    puts "     N = %12.3fm"  % enu[1]
    puts "     U = %12.3fm"  % enu[2]
    puts "方位角 = %12.3f°" % az
    puts "  仰角 = %12.3f°" % el
    puts "  距離 = %12.3fm"  % dst
  rescue => e
    msg = "[#{e.class}] #{e.message}\n"
    e.backtrace.each { |tr| msg << "\t#{tr}\n" }
    $stderr.puts msg
    exit 1
  end

  private

  # BLH -> ENU 変換(Earth, North, Up)
  #
  # @param blh_o: BLH 座標(原点) [latitude, longitude, height]
  # @param   blh: BLH 座標(対象) [latitude, longitude, height]
  # @return  enu: ENU 座標 [e, n, u]
  def blh2enu(blh_o, blh)
    x_o, y_o, z_o = blh2ecef(blh_o)
    x, y, z = blh2ecef(blh)
    mat_0 = mat_z(90.0)
    mat_1 = mat_y(90.0 - blh_o[0])
    mat_2 = mat_z(blh_o[1])
    mat = mul_mat(mul_mat(mat_0, mat_1), mat_2)
    return rotate(mat, [x - x_o, y - y_o, z - z_o])
  rescue => e
    raise
  end

  # BLH -> ECEF 変換
  #
  # @param   blh: BLH  座標 [latitude, longitude, height]
  # @return ecef: ECEF 座標 [x, y, z]
  def blh2ecef(blh)
    lat, lon, ht = blh
    n = lambda do |x|
      s = Math.sin(x * PI_180)
      A / Math.sqrt(1.0 - E2 * s * s)
    end
    c_lat = Math.cos(lat * PI_180)
    c_lon = Math.cos(lon * PI_180)
    s_lat = Math.sin(lat * PI_180)
    s_lon = Math.sin(lon * PI_180)
    x = (n.call(lat) + ht) * c_lat * c_lon
    y = (n.call(lat) + ht) * c_lat * s_lon
    z = (n.call(lat) * (1.0 - E2) + ht) * s_lat
    return [x, y, z]
  rescue => e
    raise
  end

  # x 軸を軸とした回転行列
  #
  # @param  ang: 回転量(°)
  # @return    : 回転行列(3x3)
  def mat_x(ang)
    a = ang * PI_180
    c = Math.cos(a)
    s = Math.sin(a)
    return [
      [1.0, 0.0, 0.0],
      [0.0,   c,   s],
      [0.0,  -s,   c]
    ]
  rescue => e
    raise
  end

  # y 軸を軸とした回転行列
  #
  # @param  ang: 回転量(°)
  # @return    : 回転行列(3x3)
  def mat_y(ang)
    a = ang * PI_180
    c = Math.cos(a)
    s = Math.sin(a)
    return [
      [   c, 0.0,  -s],
      [ 0.0, 1.0, 0.0],
      [   s, 0.0,   c]
    ]
  rescue => e
    raise
  end

  # z 軸を軸とした回転行列
  #
  # @param  ang: 回転量(°)
  # @return    : 回転行列(3x3)
  def mat_z(ang)
    a = ang * PI_180
    c = Math.cos(a)
    s = Math.sin(a)
    return [
      [  c,   s, 0.0],
      [ -s,   c, 0.0],
      [0.0, 0.0, 1.0]
    ]
  rescue => e
    raise
  end

  # 2行列(3x3)の積
  #
  # @param  mat_a: 3x3 行列
  # @param  mat_b: 3x3 行列
  # @return   mat: 3x3 行列
  def mul_mat(mat_a, mat_b)
    mat = Array.new(3).map { |a| Array.new(3, 0.0) }
    0.upto(2) do |i|
      0.upto(2) do |j|
        0.upto(2) do |k|
          mat[i][j] += mat_a[i][k] * mat_b[k][j]
        end
      end
    end
    return mat
  rescue => e
    raise
  end

  # 点の回転
  #
  # @param mat_r: 3x3 回転行列
  # @param    pt: 回転前座標 [x, y, z]
  # @return  mat: 回転後座標 [x, y, z]
  def rotate(mat_r, pt)
    mat = Array.new(3, 0.0)
    0.upto(2) do |i|
      0.upto(2) do |j|
        mat[i] += mat_r[i][j] * pt[j]
      end
    end
    return mat
  rescue => e
    raise
  end
end

ConvBlh2Enu.new.exec if __FILE__ == $0

2. Ruby スクリプトの実行

まず、実行権限を付与。

$ chmod +x blh2enu.rb

そして、コマンドライン引数に原点と対象の緯度、経度、楕円体高を指定して実行する。

次は、参考文献内の実行例(仙台空の滑走路の長さ)と同じパラメータで実行。

$ ./blh2enu.rb 38.13877338 140.89872429 44.512 38.14227288 140.93265738 45.664
BLH_O: LATITUDE(BETA) =  38.13877338°
    LONGITUDE(LAMBDA) = 140.89872429°
               HEIGHT =       44.512m
BLH:   LATITUDE(BETA) =  38.14227288°
    LONGITUDE(LAMBDA) = 140.93265738°
               HEIGHT =       45.664m
--->
ENU: E =     2974.681m
     N =      388.988m
     U =        0.447m
方位角 =       82.550°
  仰角 =        0.009°
  距離 =     3000.006m

次は、島根県本庁舎から見た松江市本庁舎の位置の計算例。(但し、標高 0m と想定)

$ ./blh2enu.rb 35.472222 133.050556 0 35.468056 133.048611 0
BLH_O: LATITUDE(BETA) =  35.47222200°
    LONGITUDE(LAMBDA) = 133.05055600°
               HEIGHT =        0.000m
BLH:   LATITUDE(BETA) =  35.46805600°
    LONGITUDE(LAMBDA) = 133.04861100°
               HEIGHT =        0.000m
--->
ENU: E =     -176.539m
     N =     -462.213m
     U =       -0.019m
方位角 =      200.904°
  仰角 =       -0.002°
  距離 =      494.779m
  • U の値や仰角が負の値になっているのは地球が球体であるためで、誤りではない。
  • 概ね、島根県本庁舎から西に 176.539m, 南に 462.213m の所に松江市本庁舎が位置しているということ。

3. 参考文献

※上記の参考文献のソースコード内、方位角の計算に誤りがある(ように見受けられる)ので、注意。


人工衛星の正確な軌道計算等に利用できるでしょう。

以上。

 

Sponsored Link

 

コメントする