无人导航常见坐标系

在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)