無人導航常見坐標系

在UAV（Unmanned Aerial Vehicle）中，常見的坐標系有以下幾種：

1. 大地坐標系，WGS84（WorldGeodeticCoordinateSystem1984）

　　這是為GPS全球定位系統建立的坐標系統。WGS-84坐標系的原點在地球質心，Z軸指向BIH1984.0定義的協定地球極（CTP）方向，X軸指向BIH1984.0的零度子午面和CTP赤道的交點，Y軸和Z、X軸構成右手坐標系。其參數為經度、緯度、海拔高度。

　　其基本參數如下：

　　　　　　　　　　長半徑：a=6378137±2（m）；

　　　　　　　　　　地球引力和地球質量的乘積：GM=3986005×108m3s-2±0.6×108m3s-2；

　　　　　　　　　　正常化二階帶諧系數：C20=-484.16685×10-6±1.3×10-9；

　　　　　　　　　　地球重力場二階帶球諧系數：J2=108263×10-8

　　　　　　　　　　地球自轉角速度：ω=7292115×10-11rads-1±0.150×10-11rads-1

　　　　　　　　　　扁率f=0.003352810664

 

2. 地球中心坐標系，ECEF（Earth-Centered, Earth-Fixed）

　　ECEF坐標系與地球固聯，且隨着地球轉動。圖中O即為坐標原點，位置在地球質心。X軸通過格林尼治線和赤道線的交點，正方向為原點指向交點方向。Z軸通過原點指向北極。Y軸與X、Z軸構成右手坐標系。

　　下圖中可以直觀的看出ECEF和WGS84坐標系的區別：

　　圖中，φ、λ表示緯度和經度，是WGS84坐標系的參數，x、y、z為ECEF坐標系的描述。從圖中可以看出，目標點X標記處在不同坐標系下描述的區別。

 

3. 局部切線平面

　　從定義來分類，局部切線平面可分為基於垂直和水平尺寸定義的平面，其表現在縱坐標為上還是下。縱坐標為上時，稱為ENU（東、北、天）坐標系，主要用於地理方面；縱坐標為下時，稱為NED（北、東、地）坐標系，特別用於航空航天。

　　

　　上圖為ENU坐標系，該坐標系即為控制裝置所在位置的“平面坐標系”，又稱為地理坐標系。

 

4.  載體坐標系（Body Frame）

　　載體坐標系指的是以載體的質心為原點，OX沿縱軸方向，即載體前進方向，Z軸沿載體側軸方向，指向右翼，Y沿載體豎軸方向，是右手坐標系而成（即指向天）。總的來說，載體坐標系相對於地理坐標系的關系就是載體的姿態。在我們的實際控制當中，我們關心的顯然是載體坐標系相對於地理坐標系之間的變化，所以我們通常使用的旋轉矩陣是把“地理”坐標系轉到“載體”坐標系的矩陣，從而實現對控制目標（載體）的姿態控制。由地理坐標系到載體坐標系的轉換常用的有三種方式：四元數、歐拉角、方向余弦矩陣。

　　導航的基本原則就是保證兩個基本坐標系的正確轉化，沒有誤差。只有實現了這個原則，載體才可以在自己的坐標系中完成一系列動作而被轉換到地理坐標系中看起來是正確的。導航的基本原則就是保證兩個基本坐標系的正確轉化，沒有誤差。只有實現了這個原則，載體才可以在自己的坐標系中完成一系列動作而被轉換到地理坐標系中看起來是正確的。

 

5. 代碼實現

　　這是之前寫的一個坐標轉換的代碼，經測試可以使用。

#!/usr/bin/env python   
# -*- coding: utf-8 -*-

"""
Created on 2019.01.18
@Author:  Ethan
"""

import math
import numpy as np
import client_socket.py as client
PI= math.pi
a=6378137
b=6356752.314245
E = (a*a - b*b)/(a*a)

class Goal:
    def __init__(self,x,y):
        self.x=0
        self.y=0

def deg(x):
    x=(x/180)*PI
    return x

#latitude：緯度 longitude:經度 altitude：海拔高度
#latitude0 longitude0----current_local_pos
def Translate(latitude:float,longitude:float,    
              latitude0:float,longitude0:float):      
    LAT=deg(latitude)
    LON=deg(longitude)

    LAT0=deg(latitude0)
    LON0=deg(longitude0)

    altitude0=0
    print("GPS坐標(經度、緯度、高度）為： ",latitude,longitude,altitude)
#ECEF坐標系
    N = a /(math.sqrt(1 - E*math.sin(LAT)*math.sin(LAT)))
    X=(N+altitude)*math.cos(LAT)*math.cos(LON)
    Y=(N+altitude)*math.cos(LAT)*math.sin(LON)
    Z=N*(1-e*e)*math.sin(LAT)
  
    N0 = a /(math.sqrt(1 - E*math.sin(LAT0)*math.sin(LAT0)))
    X0=(N+altitude)*math.cos(LAT0)*math.cos(LON0)
    Y0=(N+altitude)*math.cos(LAT0)*math.sin(LON0)
    Z0=N*(1-E*E)*math.sin(LAT0)

    print("空間直角坐標系下X軸、Y軸、高度為",'%.3f'%X,'%.3f'%Y,'%.3f'%Z)

    #旋轉矩陣
    mat=np.array([[-math.sin(LON),math.cos(LON),0],\
                 [-math.sin(LAT)*math.cos(LON),-math.sin(LAT)*math.sin(LON),math.cos(LAT)],\
                 [math.cos(LAT)*math.cos(LON),math.cos(LAT)*math.sin(LON),math.sin(LAT)]])

    arr=np.array([[X-X0],[Y-Y0],[Z-Z0]])  #坐標原點糾正
    res=np.dot(mat,arr)
    X=res[0]
    Y=res[1]
    print("站心坐標系下東、北"，'%.3f'%X,'%.3f'%Y) 

    return res

#獲取航點
def get_waypoints(x0,y0,x1,y1,cx,cy):
    waypoints=client.way_choose(x0,y0,x1,y1)
    for i in waypoints:
        str=i.split(",")
        longitude=float(str[0])
        latitude=float(str[1])

        res=Translate(latitude,longitude,x0,y0)
        g=Goal()
        g.x=res[0]
        g.y=res[1]

        cx=list.append(g.x)
        cy=list.append(g.y)