最近傍探索による測位座標の補正および取得

Pythonで出発点と進む距離・方位から到達点の緯度経度を計算する方法

Pythonのpyprojライブラリを使用した。pypojは地球が楕円体であることを考慮して、地理的な位置情報を使った計算ができるライブラリです．
また，出発点と方位角・距離を与えて到達点の緯度経度を計算する方法がfwdメソッドを使えば簡単に求めることができる。

fwdメソッドには引数として出発点の経度・緯度、方位角・距離を与えます。戻り値は到着点の経度・緯度・逆方位角（到着点から出発点を見た時の方位角）です。

また最近傍探索を用い座標の補正を行うにあたり緯度経度と平面直角座標の相互変換を実装した。

import csv
import numpy as np
from numpy.linalg import norm
import pyproj

grs80 = pyproj.Geod(ellps='GRS80')
coordinate_path = "coordinate.csv"
azimuth_path = "azimuth.csv"
azimuth_data = []

def write_data(file_path,latitude,longitude):
    data_string = '{},{}\n'.format(latitude,longitude)
    f = open(file_path,'a')
    f.write(str(data_string))
    f.close()

def azimuth_read(file_path):
    with open(file_path, encoding='utf8') as f:
        azimuth_reader = csv.reader(f)
        for azimuth in azimuth_reader:
            azimuth_data.append(float(azimuth[0]))
    f.close()



def calc_xy(phi_deg, lambda_deg, phi0_deg, lambda0_deg):
    """ 緯度経度を平面直角座標に変換する
    # https://qiita.com/sw1227/items/e7a590994ad7dcd0e8ab 参照
    - input:
        (phi_deg, lambda_deg): 変換したい緯度・経度[度]（分・秒でなく小数であることに注意）
        (phi0_deg, lambda0_deg): 平面直角座標系原点の緯度・経度[度]（分・秒でなく小数であることに注意）
    - output:
        x: 変換後の平面直角座標[m]
        y: 変換後の平面直角座標[m]
    """
    # 緯度経度・平面直角座標系原点をラジアンに直す
    phi_rad = np.deg2rad(phi_deg)
    lambda_rad = np.deg2rad(lambda_deg)
    phi0_rad = np.deg2rad(phi0_deg)
    lambda0_rad = np.deg2rad(lambda0_deg)

    # 補助関数
    def A_array(n):
        A0 = 1 + (n**2)/4. + (n**4)/64.
        A1 = -     (3./2)*( n - (n**3)/8. - (n**5)/64. ) 
        A2 =     (15./16)*( n**2 - (n**4)/4. )
        A3 = -   (35./48)*( n**3 - (5./16)*(n**5) )
        A4 =   (315./512)*( n**4 )
        A5 = -(693./1280)*( n**5 )
        return np.array([A0, A1, A2, A3, A4, A5])

    def alpha_array(n):
        a0 = np.nan # dummy
        a1 = (1./2)*n - (2./3)*(n**2) + (5./16)*(n**3) + (41./180)*(n**4) - (127./288)*(n**5)
        a2 = (13./48)*(n**2) - (3./5)*(n**3) + (557./1440)*(n**4) + (281./630)*(n**5)
        a3 = (61./240)*(n**3) - (103./140)*(n**4) + (15061./26880)*(n**5)
        a4 = (49561./161280)*(n**4) - (179./168)*(n**5)
        a5 = (34729./80640)*(n**5)
        return np.array([a0, a1, a2, a3, a4, a5])

    # 定数 (a, F: 世界測地系-測地基準系1980（GRS80）楕円体)
    m0 = 0.9999 
    a = 6378137.
    F = 298.257222101

    # (1) n, A_i, alpha_iの計算
    n = 1. / (2*F - 1)
    A_array = A_array(n)
    alpha_array = alpha_array(n)

    # (2), S, Aの計算
    A_ = ( (m0*a)/(1.+n) )*A_array[0] # [m]
    S_ = ( (m0*a)/(1.+n) )*( A_array[0]*phi0_rad + np.dot(A_array[1:], np.sin(2*phi0_rad*np.arange(1,6))) ) # [m]

    # (3) lambda_c, lambda_sの計算
    lambda_c = np.cos(lambda_rad - lambda0_rad)
    lambda_s = np.sin(lambda_rad - lambda0_rad)

    # (4) t, t_の計算
    t = np.sinh( np.arctanh(np.sin(phi_rad)) - ((2*np.sqrt(n)) / (1+n))*np.arctanh(((2*np.sqrt(n)) / (1+n)) * np.sin(phi_rad)) )
    t_ = np.sqrt(1 + t*t)

    # (5) xi', eta'の計算
    xi2  = np.arctan(t / lambda_c) # [rad]
    eta2 = np.arctanh(lambda_s / t_)

    # (6) x, yの計算
    x = A_ * (xi2 + np.sum(np.multiply(alpha_array[1:],
                                       np.multiply(np.sin(2*xi2*np.arange(1,6)),
                                                   np.cosh(2*eta2*np.arange(1,6)))))) - S_ # [m]
    y = A_ * (eta2 + np.sum(np.multiply(alpha_array[1:],
                                        np.multiply(np.cos(2*xi2*np.arange(1,6)),
                                                    np.sinh(2*eta2*np.arange(1,6)))))) # [m]
    # return
    return x, y # [m]

def calc_lat_lon(x, y, phi0_deg, lambda0_deg):
    """ 平面直角座標を緯度経度に変換する
    - input:
        (x, y): 変換したいx, y座標[m]
        (phi0_deg, lambda0_deg): 平面直角座標系原点の緯度・経度[度]（分・秒でなく小数であることに注意）
    - output:
        latitude:  緯度[度]
        longitude: 経度[度]
        * 小数点以下は分・秒ではないことに注意
    """
    # 平面直角座標系原点をラジアンに直す
    phi0_rad = np.deg2rad(phi0_deg)
    lambda0_rad = np.deg2rad(lambda0_deg)

    # 補助関数
    def A_array(n):
        A0 = 1 + (n**2)/4. + (n**4)/64.
        A1 = -     (3./2)*( n - (n**3)/8. - (n**5)/64. ) 
        A2 =     (15./16)*( n**2 - (n**4)/4. )
        A3 = -   (35./48)*( n**3 - (5./16)*(n**5) )
        A4 =   (315./512)*( n**4 )
        A5 = -(693./1280)*( n**5 )
        return np.array([A0, A1, A2, A3, A4, A5])

    def beta_array(n):
        b0 = np.nan # dummy
        b1 = (1./2)*n - (2./3)*(n**2) + (37./96)*(n**3) - (1./360)*(n**4) - (81./512)*(n**5)
        b2 = (1./48)*(n**2) + (1./15)*(n**3) - (437./1440)*(n**4) + (46./105)*(n**5)
        b3 = (17./480)*(n**3) - (37./840)*(n**4) - (209./4480)*(n**5)
        b4 = (4397./161280)*(n**4) - (11./504)*(n**5)
        b5 = (4583./161280)*(n**5)
        return np.array([b0, b1, b2, b3, b4, b5])

    def delta_array(n):
        d0 = np.nan # dummy
        d1 = 2.*n - (2./3)*(n**2) - 2.*(n**3) + (116./45)*(n**4) + (26./45)*(n**5) - (2854./675)*(n**6)
        d2 = (7./3)*(n**2) - (8./5)*(n**3) - (227./45)*(n**4) + (2704./315)*(n**5) + (2323./945)*(n**6)
        d3 = (56./15)*(n**3) - (136./35)*(n**4) - (1262./105)*(n**5) + (73814./2835)*(n**6)
        d4 = (4279./630)*(n**4) - (332./35)*(n**5) - (399572./14175)*(n**6)
        d5 = (4174./315)*(n**5) - (144838./6237)*(n**6)
        d6 = (601676./22275)*(n**6)
        return np.array([d0, d1, d2, d3, d4, d5, d6])

    # 定数 (a, F: 世界測地系-測地基準系1980（GRS80）楕円体)
    m0 = 0.9999 
    a = 6378137.
    F = 298.257222101

    # (1) n, A_i, beta_i, delta_iの計算
    n = 1. / (2*F - 1)
    A_array = A_array(n)
    beta_array = beta_array(n)
    delta_array = delta_array(n)

    # (2), S, Aの計算
    A_ = ( (m0*a)/(1.+n) )*A_array[0]
    S_ = ( (m0*a)/(1.+n) )*( A_array[0]*phi0_rad + np.dot(A_array[1:], np.sin(2*phi0_rad*np.arange(1,6))) )

    # (3) xi, etaの計算
    xi = (x + S_) / A_
    eta = y / A_

    # (4) xi', eta'の計算
    xi2 = xi - np.sum(np.multiply(beta_array[1:], 
                                  np.multiply(np.sin(2*xi*np.arange(1,6)),
                                              np.cosh(2*eta*np.arange(1,6)))))
    eta2 = eta - np.sum(np.multiply(beta_array[1:],
                                   np.multiply(np.cos(2*xi*np.arange(1,6)),
                                               np.sinh(2*eta*np.arange(1,6)))))

    # (5) chiの計算
    chi = np.arcsin( np.sin(xi2)/np.cosh(eta2) ) # [rad]
    latitude = chi + np.dot(delta_array[1:], np.sin(2*chi*np.arange(1, 7))) # [rad]

    # (6) 緯度(latitude), 経度(longitude)の計算
    longitude = lambda0_rad + np.arctan( np.sinh(eta2)/np.cos(xi2) ) # [rad]

    # ラジアンを度になおしてreturn
    return np.rad2deg(latitude), np.rad2deg(longitude) # [deg]

#マップデータ
def calc_distance_and_neighbor_point(coordinate_a, coordinate_b, coordinate_p):
    # 近傍を求める
    a = np.array([coordinate_a[0],coordinate_a[1]])
    b = np.array([coordinate_b[0],coordinate_b[1]])
    p = np.array([coordinate_p[0],coordinate_p[1]])
    
    ap = p - a
    ab = b - a
    ba = a - b
    bp = p - b
    if np.dot(ap, ab) < 0:
        distance = norm(ap)
        neighbor_point = a
    elif np.dot(bp, ba) < 0:
        distance = norm(p - b)
        neighbor_point = b
    else:
        ai_norm = np.dot(ap, ab)/norm(ab)
        neighbor_point = a + (ab)/norm(ab)*ai_norm
        distance = norm(p - neighbor_point)
        #print(f"IN a:{coordinate_a} b:{coordinate_b} c:{coordinate_p} OUT:{neighbor_point.tolist()}")
    return neighbor_point.tolist(), distance

def map_mapping(fig,ax,location,road_data):
    """ 道路に沿って位置の修正
    - input:
        road_data : 道データ
        location : (緯度,経度)の入ったタプル
    - output:
        new_xy : マップマッピング後の座標
        neighbor_point_subscript: 最近傍点を取る際に使用した直線の添字のタプル
    """
    # 元の位置情報を座標に変換
    xy = calc_xy(location[0], location[1], phi0_deg, lambda0_deg)

    # 道データを一番近い順に取得
    neighbor_point_data = [] #最近傍点の座標と距離が入る
    for point_a_subscript in road_data: #すべて回すのは無駄な処理が多いなぁ・・・(とりあえず動いてるからいいか！)
        for point_b_subscript in road_data[point_a_subscript][2]: #座標に一番近い道路情報の接続している添字
            coordinate_a = road_data[point_a_subscript][1]
            coordinate_b = road_data[point_b_subscript][1]
            coordinate_p = xy
            neighbor_point,distance = calc_distance_and_neighbor_point(coordinate_a, coordinate_b, coordinate_p)
            neighbor_point_data.append((neighbor_point,distance,(point_a_subscript,point_b_subscript)))

    neighbor_point_data = sorted(neighbor_point_data, key=lambda x:(x[1]))
    new_xy = (neighbor_point_data[0][0][0], neighbor_point_data[0][0][1])
    neighbor_point_subscript = neighbor_point_data[0][2]
    return fig,ax,new_xy,neighbor_point_subscript


def read_load_csv(data_path):
    """ 道情報の入ったCSVファイルを読み込む
        google mymap のCSVファイル緯度と経度を読み込む場合に使用
    - input:
        data_path: ファイルの場所
    - output:
        places 
    """
    with open(data_path) as f:
        reader = csv.reader(f)
        l = [row for row in reader]

    tut_map_data = {} 

    # google mymap から書き出したCSVを変換
    # 形式は "POINT (139.3377123 35.6276553)",ポイント 1,2
    #       "POINT ([緯度] [経度])",ポイント [自分の番号],[接続先の番号]
    for data in l:
        if("POINT" in data[0]):
            data_arr = data[0].replace("(","").replace(")","").split(" ")
            point_number = int(data[1].split(" ")[1])
            connection_numbers = []
            for number_str in data[2].split("-"):
                try:
                    connection_numbers.append(int(number_str))
                except:
                    pass
            place = (float(data_arr[1])),(float(data_arr[2]))     
            coordinate = calc_xy(float(data_arr[2]), float(data_arr[1]), phi0_deg, lambda0_deg)
            
            tut_map_data[point_number] = place,coordinate,connection_numbers

    
    # 接続先の接続ポイント情報にも、自分のポイントを入れる
    for subscript in tut_map_data:
        for connection_subscript in tut_map_data[subscript][2]:
            if not subscript in tut_map_data[connection_subscript][2]:
                tut_map_data[connection_subscript][2].append(subscript)

    return tut_map_data


#東京？
phi0_deg = 36.
lambda0_deg = 139+50./60

# 出発点の緯度経度
lat0 = 35.62589 #緯度
lon0 = 139.34129 #経度

# 方位角と移動距離
step = 1.64 
azimuth_read(azimuth_path)

map_data = read_load_csv("tut_map_data.csv")

for azimuth in azimuth_data:
    print()

    #print(f"grs80.fwd 入力：(緯度){lat0:.10f},(経度){lon0:.10f}")
    lon1, lat1, back_azimuth = grs80.fwd(lon0, lat0, azimuth-190, step) # 到着点の緯度経度の算出
    print(f"grs80.fwd 出力：(緯度){lat1:.10f},(経度){lon1:.10f}")
    
    with open("test.csv",mode="a") as f:
        f.write(f"{lat1},{lon1}\n")
    f.close()
        
    fig,ax,new_xy,neighbor_point_subscript = map_mapping("","",(lat1,lon1),map_data)
    lat0,lon0 = calc_lat_lon(new_xy[0], new_xy[1], phi0_deg, lambda0_deg)

自律航法を行う上で上記のコードを用い値磁気センサ、ジャイロセンサから得られれた方位データおよび歩幅（距離）から緯度経度の算出および前もって用意した地理データを用い路上に緯度経度を補正するため用いた．