ardupilot开发 --- 坐标系篇

0. 提出问题

你是否对飞控中的坐标变换很迷惑？惯性系？地面系？global系？极坐标系？经纬度？导航坐标系？local系？地固系？NED？NEU？机体系？FRU？FRD？Home点？EKF origin？
飞机的lean角？欧拉角？四元素？
他们之间有什么关系？是如何转换的？为什么要进行坐标转换？
如果你存在以上问题，往下看就对了！

1. 分类及简述

• global类
  WGS84，又称大地经纬度坐标系或称global系或称地球系或称地面系；
  GPS定位信息一般是基于此系；
  经度，维度，海拔高度，有的GNSS模块甚至可以提供航向yaw信息;
  原点在地心；
• local类
  local系，又称地固系；
  local系又可分NED系和NEU系；
  NED系又被称为导航坐标系，Ardupilot中的位置控制算法、导航与路径规划算法全部是基于此系！
  Ardupilot 中的 position target 量是基于NED坐标系的量，原文：Set the vehicle’s target position (as an offset in NED from the EKF origin)
  原点为 EKF origin；
• body类
  body系，又称机体系或称移动系;
  body系又可分FRD系和FRU系；
  姿态控制器、角速度控制器、动力分配、欧拉角、飞机倾角(lean angle)的计算全部是基于FRD系；
  原点在飞机重心；
• 惯性系
  惯性坐标系是为了简化世界坐标系到物体坐标系的转化而产生的。惯性坐标系的原点与物体坐标系的原点重合，惯性坐标系的轴平行于世界坐标系的轴。引入了惯性坐标系之后，物体坐标系转换到惯性坐标系只需旋转，从惯性坐标系转换到世界坐标系只需平移。
  在航空领域中，惯性系即：原点在飞机重心的local系，原点随飞机移动而移动，但轴的方向保持不变！
• Home点
  定义在global系中，默认取电机解锁时的位置点，可在飞行时随时手动更改；
  RTL模式的最终位置点；
• EKF origin
  EKF原点又称位置估计原点(position-estimator origin)，定义在global系中，是所有local系原点的global系坐标量；一般取电机解锁时的位置点，设置后在飞行过程中不可更改！(仅限于本次解锁)；
  设置EKF原点，是为了能够在global系和local系之间进行坐标转换；
  mavlink设置EKF原点的方法：发送 SET_GPS_GLOBAL_ORIGIN 消息；

2. Ardupilot中涉及的坐标系说明

• MAV_FRAME_BODY_FRD 机体坐标系，x轴指向前，y向右，z向下；控制量要结算到该坐标系上；
• MAV_FRAME_LOCAL_NED 地固坐标系，x: North, y: East, z: Down；欧拉角即MAV_FRAME_BODY_FRD坐标系与该坐标系的旋转关系；位置量xyz的EKF估计的原点就是该坐标系的原点；
• MAV_FRAME_GLOBAL 全球坐标系WGS84，经度纬度高度；GPS传感器所在的坐标系，最终要换为MAV_FRAME_LOCAL_NED系的位置量来提供给飞控的位置控制器使用；
• position target是基于NED坐标系的，也就是地固坐标系，又称local坐标系，坐标系的原点是 EKF origin，解锁电机并获得清晰GPS信息的位置点；原文：Set the vehicle’s target position (as an offset in NED from the EKF origin)
• In frames, OFFSET means “relative to vehicle position” while LOCAL is “relative to home position” (these have no impact on velocity directions). BODY means that velocity components are relative to the heading of the vehicle rather than the NED frame.

参考：https://mavlink.io/en/messages/common.html#MAV_FRAME

3. 坐标系详解

3.1 WGS84

重要参数：经度、纬度、海拔高度；
子午线：待补充…
赤道：待补充…
WGS-84坐标系（World Geodetic System一1984 Coordinate System）是美国国防部建立的一种地心地固坐标系，全球卫星定位系统GPS是基于WGS-84坐标系的。WGS-84坐标系原点是地球的质心，空间直角坐标系的Z轴指向BIH（1984.0）定义的地极（CTP）方向，即国际协议原点CIO，它由IAU和IUGG共同推荐。X轴指向BIH定义的零度子午面和CTP赤道的交点，Y轴和Z、X轴构成右手坐标系。

参考：https://blog.csdn.net/EagleLY5894/article/details/104118465/

3.2 NED NEU FRD FRU

NED：xyz分别指向北东地，原点在EKF origin.
NEU：xyz分别指向北东天，原点在EKF origin.
FRD：xyz分别指向前右地，原点在飞机重心。
FRU：xyz分别指向前右天，原点在飞机重心。

4. Ardupilot代码中几个重要坐标系间的相互转换关系

待补充…

修订记录

2023-08-18 初始
2024-01-19 根据自己理解之后的大修