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