0
点赞
收藏
分享

微信扫一扫

ardupilot 中关键坐标系


目录


文章目录

  • ​​目录​​
  • ​​摘要​​
  • ​​1.NED坐标系​​
  • ​​2.东北天坐标系(ENU)​​
  • ​​3.北东天坐标系(NEU)​​
  • ​​4.ardupilot 机体坐标系​​

摘要

本节主要记录ardupilot中重要的坐标系,四旋翼无人机是四输入,六输出(姿态(横滚,俯仰,偏航)+位置(x,y,z))的欠驱动系统,在进行无人机姿态控制,位置控制以及旋转过程中,会经常使用坐标系,了解ardupilot中坐标系非常重要,欢迎批准指正!

1.NED坐标系

NED(North East Down)坐标系,即北东地坐标系,简称为n坐标系,也叫做导航坐标系,是在导航时根据导航系统工作的需要而选取的用于导航解算的参考坐标系。

NED坐标系各轴的定义:
N——北轴指向地球北;
E——东轴指向地球东;
D——地轴垂直于地球表面并指向下。

我们看下飞控中使用到的地方

//This represents a quaternion rotation in NED frame to the target (setpoint)
//attitude used in the attitude controller.
//这表示NED坐标系中的四元数旋转到这个目标姿态(设定点),被使用在姿态控制器中
Quaternion _attitude_target_quat;
// This represents a 321-intrinsic rotation in NED frame to the target (setpoint)
// attitude used in the attitude controller, in radians.
//这表示321(Z-Y-X)固有旋转在NED坐标系到这个目标(设定点)姿态被使用在姿态控制器中,单位是弧度
Vector3f _attitude_target_euler_angle;
//状态输出元素信息
struct output_elements {
//四元数定义旋转从当地NED坐标系到机体坐标系
Quaternion quat; // quaternion defining rotation from local NED earth frame to body frame
//机体坐标系速度原点在NED坐标系中(m/s)
Vector3f velocity; // velocity of body frame origin in local NED earth frame (m/sec)
//机体位置坐标系原点在NED坐标系中(m)
Vector3f position; // position of body frame origin in local NED earth frame (m)
};

2.东北天坐标系(ENU)

ENU也叫站心坐标系以用户所在位置P为坐标原点。

坐标系定义为: X轴:指向东边 Y轴:指向北边 Z轴:指向天顶

ENU局部坐标系采用三维直角坐标系来描述地球表面,实际应用较为困难,因此一般使用简化后的二维投影坐标系来描述。在ardupilot中主要使用NEU,因此这里不深入讲解这个坐标系。

3.北东天坐标系(NEU)

NEU表示北-东-天坐标系

private:
Vector3f _relpos_cm; // NEU
Vector3f _velocity_cm; // NEU
struct Location _abspos; //定位绝对位置信息
bool _haveabspos;
AP_AHRS_NavEKF &_ahrs_ekf;
void AP_InertialNav_NavEKF::update(bool high_vibes)
{
//获取相对于本地地球坐标系原点的NE位置
Vector2f posNE;
//获取位置信息
if (_ahrs_ekf.get_relative_position_NE_origin(posNE))
{
_relpos_cm.x = posNE.x * 100; // 转换从m到cm
_relpos_cm.y = posNE.y * 100; // 转换从m到cm
}

// 获取相对于本地地球坐标系原点的D位置
float posD;
if (_ahrs_ekf.get_relative_position_D_origin(posD))
{
_relpos_cm.z = - posD * 100; //转换坐标系从NED到NEU
}
//获取这个全局的WGS-84位置
//WGS84坐标系是1984世界大地坐标系(World Geodetic System)的简称。
//它是美国国防制图局于1984年建立的,是GPS卫星星历的参考基准,也是协议地球参考系的一种。
//该系列先后有WGS60、WGS72以及WGS84,其后的发展演变为WGS84(G730)、WGS84(G873)和2001年完成的最新的WGS84(G1150)。
_haveabspos = _ahrs_ekf.get_position(_abspos);
//得到相对于局部地球坐标系的速度
Vector3f velNED;
//获取惯性导航速度
if (_ahrs_ekf.get_velocity_NED(velNED))
{
// 在高振动事件中,使用垂直位置变化
if (high_vibes)
{
float rate_z;
//获取垂直速度
if (_ahrs_ekf.get_vert_pos_rate(rate_z))
{
velNED.z = rate_z;
}
}
_velocity_cm = velNED * 100; //转换到cm/s
_velocity_cm.z = -_velocity_cm.z; //转换坐标系从NED到NEU
}
}

这里可以看出,代码中有实现NED到ENU的转换。

4.ardupilot 机体坐标系

ardupilot 机体坐标系定义为

ardupilot 中关键坐标系_sed

void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
{
float accel_right, accel_forward;

//rotate accelerations into body forward-right frame
//todo: this should probably be based on the desired heading not the current heading
//将加速度旋转到机体前右坐标系
//todo:这可能应该基于所需的机头,而不是当前机头
accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw();
accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw();

// update angle targets that will be passed to stabilize controller
//更新目标角度将会传递到姿态控制中
//俯仰角度是上正下负号,横滚左负有正
pitch_target = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
float cos_pitch_target = cosf(pitch_target * M_PI / 18000.0f);
roll_target = atanf(accel_right * cos_pitch_target / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
}

从代码注释中,可以看出机体坐标系,前—右—下

ardupilot 中关键坐标系_人工智能_02



举报

相关推荐

0 条评论