文章目錄
在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)