首先介绍两个基本概念，body系和local系，body系指无人机机身，原点在机体上，随无人机移动，常见的有前左上FLU，前右下FRD，local系则是以客观世界中的一个静止的点为原点（一般是无人机起飞点），常见的有东北天坐标系ENU和北东地坐标系NED。这些都是右手系。
mavros作为连接飞控端和ros端的程序，有这么四个坐标系，即飞控端的body系和local系及ros端的body系和local系：fcu_body,fcu_local,ros_body,ros_local。
其中fcu_body又叫aircraft坐标系，是FRD坐标系，是px4所使用的body系，ros_body又叫baselink坐标系，是FLU坐标系，是mavros所使用的body系(在ROS-kinetic版本的mavros中，ros_body系是RFU坐标系，X轴指向右，Y轴指向前。而在ROS-melodic及之后的版本中，ros_body系是FLU坐标系，可以参考https://blog.csdn.net/benchuspx/article/details/112970682和https://developer.baidu.com/article/details/3285454)。
我们在ROS层面看到的比如/mavros/local_position/pose或者/mavros/vision_pose/pose里面的位姿对应的是ros_body坐标系在ros_local坐标系下的位姿。
我们在QGC看到的飞控的一些mavlink消息如local_position和attitude里的位姿对应的是fcu_body在fcu_local下的位姿
所以我们在谈mavros对ros和飞控中的位姿做坐标系变换时，首先需要清楚这四个坐标系及它们之间的关系，否则可能会混乱。
mavros官方对它们的描述
https://github.com/mavlink/mavros/blob/master/mavros/README.md
MAVROS does translate Aerospace NED frames, used in FCUs to ROS ENU frames and vice-versa. For translate airframe related data we simply apply rotation 180° about ROLL (X) axis. For local we apply 180° about ROLL (X) and 90° about YAW (Z) axes. Please read documents from issue #473 for additional information.
https://github.com/mavlink/mavros/pull/473
mavros代码里也明确给了ros_local(ENU)和fcu_local(NED)以及ros_body(BASELINK/FLU)和fcu_body(AIRCRAFT/FRD)之间的关系
ENU(ros_local)转NED(fcu_local)或者NED(fcu_local)转ENU(ros_local)，都是先绕Z轴顺时针转90度，再绕X轴顺时针转180，这里所说的顺时针是符合右手法则下的顺时针。
BASELINK(ros_body)转AIRCRAFT(fcu_body)或者AIRCRAFT(fcu_body)转BASELINK(ros_body)，都是绕X轴顺时针转180度即可，这里所说的顺时针是符合右手法则下的顺时针。
/mavros/mavros/src/lib/ftf_frame_conversions.cpp
/**
* @brief Static quaternion needed for rotating between ENU and NED frames
* NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East)
* ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North)
*/
static const auto NED_ENU_Q = quaternion_from_rpy(M_PI, 0.0, M_PI_2);
/**
* @brief Static quaternion needed for rotating between aircraft and base_link frames
* +PI rotation around X (Forward) axis transforms from Forward, Right, Down (aircraft)
* Fto Forward, Left, Up (base_link) frames.
*/
static const auto AIRCRAFT_BASELINK_Q = quaternion_from_rpy(M_PI, 0.0, 0.0);
在mavlink里面，坐标系MAV_FRAME也是包含下面几种的 MAV_FRAME_LOCAL_NED MAV_FRAME_LOCAL_ENU MAV_FRAME_BODY_FRD MAV_FRAME_BODY_FLU
而且可以看到mavlink里面坐标系主要是三种，global,local,body
http://mavlink.io/zh/messages/common.html
已知fcu_body在fcu_local中的位姿，要求ros_body在ros_local中的位姿，得走两步是，先根据fcu_body和ros_body的关系得到ros_body在fcu_local中的位姿，再根据fcu_local和ros_local的关系得到ros_body在ros_local中的位姿。由于对于位置xyz而言，fcu_body的原点可以认为是和ros_body的原点重合的，所以对于位置xyz可以省去根据fcu_body和ros_body的关系得到ros_body在fcu_local中的位置这步，直接可以基于END到ENU的变换得到ros_body在ros_local中的位置，而对于姿态还是得按照标准的两步走得到ros_body在ros_local中的姿态。实际mavros中的代码也是这么分开处理位置和姿态的转换的。
已知ros_body在ros_local中的位姿，要求fcu_body在fcu_local中的位姿，也是分两步走，先根据fcu_body和ros_body的关系得到fcu_body在ros_local中的位姿，再根据fcu_local和ros_local的关系得到fcu_body在fcu_local中的位姿。同样由于fcu_body的原点可以认为是和ros_body的原点重合的，fcu_body坐标系和ros_body仅姿态不同，所以对于位置xyz的转换和对于姿态的转换可以分开处理，位置xyz的转换可以一步完成，姿态的转换还是两步完成，实际mavros中的代码也是这么分开处理的。
下面具体结合mavros代码看看
这里有受 https://mp.weixin.qq.com/s/17To6Y3eFNYUqm9xBQ5p8Q 启发
分为两种情况，第一种，ros端的位姿转到飞控端，比如把/mavros/vision_pose/pose话题中的位姿转为相应VISION_POSITION_ESTIMATE(#102)这个mavlink消息发给飞控，第二种，飞控端的位姿如LOCAL_POSITION_NED(#32)和ATTITUDE(#30)等消息转到ROS端并通过话题发出来，比如发出/mavros/local_position/pose话题。
对于把/mavros/vision_pose/pose里的位姿转换后通过应VISION_POSITION_ESTIMATE(#102)这个mavlink消息发给飞控，可以看vision_pose_estimate.cpp
可以首先看vision_pose_estimate.cpp的send_vision_estimate函数
/mavros/mavros_extras/src/plugins/vision_pose_estimate.cpp
/* -*- low-level send -*- */
/**
* @brief Send vision estimate transform to FCU position controller
*/
void send_vision_estimate(
const rclcpp::Time & stamp, const Eigen::Affine3d & tr,
const geometry_msgs::msg::PoseWithCovariance::_covariance_type & cov)
{
if (last_transform_stamp == stamp) {
RCLCPP_DEBUG_THROTTLE(
get_logger(),
*get_clock(), 10, "Vision: Same transform as last one, dropped.");
return;
}
last_transform_stamp = stamp;
auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
auto rpy = ftf::quaternion_to_rpy(
ftf::transform_orientation_enu_ned(
ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))));
auto cov_ned = ftf::transform_frame_enu_ned(cov);
ftf::EigenMapConstCovariance6d cov_map(cov_ned.data());
mavlink::common::msg::VISION_POSITION_ESTIMATE vp{};
vp.usec = stamp.nanoseconds() / 1000;
// [[[cog:
// for f in "xyz":
// cog.outl(f"vp.{f} = position.{f}();")
// for a, b in zip("xyz", ('roll', 'pitch', 'yaw')):
// cog.outl(f"vp.{b} = rpy.{a}();")
// ]]]
vp.x = position.x();
vp.y = position.y();
vp.z = position.z();
vp.roll = rpy.x();
vp.pitch = rpy.y();
vp.yaw = rpy.z();
// [[[end]]] (checksum: 0aed118405958e3f35e8e7c9386e812f)
// just the URT of the 6x6 Pose Covariance Matrix, given
// that the matrix is symmetric
ftf::covariance_urt_to_mavlink(cov_map, vp.covariance);
uas->send_message(vp);
}
send_vision_estimate函数里面关键代码是下面这部分，包含位置的转换和姿态的转换，mavros代码里是分开处理的，位置的转换上面解释过，由于ros_body和fcu_body系的原点是重合的，所以位置的转换可以一步完成，代码里用的transform_frame_enu_ned函数一次完成，姿态的转换过程是，通过/mavros/vision_pose/pose话题里的四元数可以得到的ros_body在ros_local下的旋转，通过transform_orientation_baselink_aircraft函数转为fcu_body在ros_local下的旋转，再通过transform_orientation_enu_ned函数转为fcu_body在fcu_local下的旋转。
auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
auto rpy = ftf::quaternion_to_rpy(
ftf::transform_orientation_enu_ned(
ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))));
auto cov_ned = ftf::transform_frame_enu_ned(cov);
ftf::EigenMapConstCovariance6d cov_map(cov_ned.data());
上面所用到的几个关键函数的定义都在这个frame_tf.hpp头文件里面，而且顺序挨着
/mavros/mavros/include/mavros/frame_tf.hpp
/**
* @brief Transform from attitude represented WRT NED frame to attitude
* represented WRT ENU frame
*/
template<class T>
inline T transform_orientation_ned_enu(const T & in)
{
return detail::transform_orientation(in, StaticTF::NED_TO_ENU);
}
/**
* @brief Transform from attitude represented WRT ENU frame to
* attitude represented WRT NED frame
*/
template<class T>
inline T transform_orientation_enu_ned(const T & in)
{
return detail::transform_orientation(in, StaticTF::ENU_TO_NED);
}
/**
* @brief Transform from attitude represented WRT aircraft frame to
* attitude represented WRT base_link frame
*/
template<class T>
inline T transform_orientation_aircraft_baselink(const T & in)
{
return detail::transform_orientation(in, StaticTF::AIRCRAFT_TO_BASELINK);
}
/**
* @brief Transform from attitude represented WRT baselink frame to
* attitude represented WRT body frame
*/
template<class T>
inline T transform_orientation_baselink_aircraft(const T & in)
{
return detail::transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT);
}
/**
* @brief Transform from attitude represented WRT aircraft frame to
* attitude represented WRT base_link frame, treating aircraft frame
* as in an absolute frame of reference (local NED).
*/
template<class T>
inline T transform_orientation_absolute_frame_aircraft_baselink(const T & in)
{
return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK);
}
/**
* @brief Transform from attitude represented WRT baselink frame to
* attitude represented WRT body frame, treating baselink frame
* as in an absolute frame of reference (local NED).
*/
template<class T>
inline T transform_orientation_absolute_frame_baselink_aircraft(const T & in)
{
return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT);
}
/**
* @brief Transform data expressed in NED to ENU frame.
*/
template<class T>
inline T transform_frame_ned_enu(const T & in)
{
return detail::transform_static_frame(in, StaticTF::NED_TO_ENU);
}
/**
* @brief Transform data expressed in ENU to NED frame.
*
*/
template<class T>
inline T transform_frame_enu_ned(const T & in)
{
return detail::transform_static_frame(in, StaticTF::ENU_TO_NED);
}
/**
* @brief Transform data expressed in Aircraft frame to Baselink frame.
*
*/
template<class T>
inline T transform_frame_aircraft_baselink(const T & in)
{
return detail::transform_static_frame(in, StaticTF::AIRCRAFT_TO_BASELINK);
}
/**
* @brief Transform data expressed in Baselink frame to Aircraft frame.
*
*/
template<class T>
inline T transform_frame_baselink_aircraft(const T & in)
{
return detail::transform_static_frame(in, StaticTF::BASELINK_TO_AIRCRAFT);
}
上面函数里所调用的一些函数又是在ftf_frame_conversions.cpp里定义的，ftf_frame_conversions.cpp里可以看清楚NED(fcu_local)到ENU(ros_local)做的旋转变换，以及AIRCRAFT(fcu_body)到BASELINK(ros_body)做的旋转变换
/mavros/mavros/src/lib/ftf_frame_conversions.cpp
/**
* @brief Static quaternion needed for rotating between ENU and NED frames
* NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East)
* ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North)
*/
static const auto NED_ENU_Q = quaternion_from_rpy(M_PI, 0.0, M_PI_2);
/**
* @brief Static quaternion needed for rotating between aircraft and base_link frames
* +PI rotation around X (Forward) axis transforms from Forward, Right, Down (aircraft)
* Fto Forward, Left, Up (base_link) frames.
*/
static const auto AIRCRAFT_BASELINK_Q = quaternion_from_rpy(M_PI, 0.0, 0.0);
/**
* @brief Static vector needed for rotating between ENU and NED frames
* +PI rotation around X (North) axis follwed by +PI/2 rotation about Z (Down)
* gives the ENU frame. Similarly, a +PI rotation about X (East) followed by
* a +PI/2 roation about Z (Up) gives the NED frame.
*/
static const Eigen::Affine3d NED_ENU_AFFINE(NED_ENU_Q);
/**
* @brief Static vector needed for rotating between aircraft and base_link frames
* +PI rotation around X (Forward) axis transforms from Forward, Right, Down (aircraft)
* Fto Forward, Left, Up (base_link) frames.
*/
static const Eigen::Affine3d AIRCRAFT_BASELINK_AFFINE(AIRCRAFT_BASELINK_Q);
/**
* @brief 3-D matrices to fill 6-D rotation matrix applied to change covariance matrices coordinate frames
*/
static const auto NED_ENU_R = NED_ENU_Q.normalized().toRotationMatrix();
static const auto AIRCRAFT_BASELINK_R = AIRCRAFT_BASELINK_Q.normalized().toRotationMatrix();
Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond & q, const StaticTF transform)
{
// Transform the attitude representation from frame to frame.
// The proof for this transform can be seen
// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
switch (transform) {
case StaticTF::NED_TO_ENU:
case StaticTF::ENU_TO_NED:
return NED_ENU_Q * q;
case StaticTF::AIRCRAFT_TO_BASELINK:
case StaticTF::BASELINK_TO_AIRCRAFT:
return q * AIRCRAFT_BASELINK_Q;
case StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK:
case StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT:
return AIRCRAFT_BASELINK_Q * q;
default:
rcpputils::require_true(false, "unsupported transform arg");
return q;
}
}
Eigen::Vector3d transform_static_frame(const Eigen::Vector3d & vec, const StaticTF transform)
{
switch (transform) {
case StaticTF::NED_TO_ENU:
case StaticTF::ENU_TO_NED:
return NED_ENU_REFLECTION_XY * (NED_ENU_REFLECTION_Z * vec);
case StaticTF::AIRCRAFT_TO_BASELINK:
case StaticTF::BASELINK_TO_AIRCRAFT:
return AIRCRAFT_BASELINK_AFFINE * vec;
default:
rcpputils::require_true(false, "unsupported transform arg");
return vec;
}
}
对于把飞控中LOCAL_POSITION_NED(#32)和ATTITUDE(#30) mavlink消息的位姿转换后发出/mavros/local_position/pose话题的情形，我们可以先从mavros的local_position.cpp看起。
可以首先看local_position.cpp的handle_local_position_ned()函数
可以看出飞控端NED坐标系下的位置(没有姿态)是来自于LOCAL_POSITION_NED这个mavlink消息，/mavros/local_position/pose话题中的姿态我们后面层层分析可以找到是来自于ATTITUDE这个mavlink消息，后面有细讲。
mavros/mavros/src/plugins/local_position.cpp
void handle_local_position_ned(
const mavlink::mavlink_message_t * msg [[maybe_unused]],
mavlink::common::msg::LOCAL_POSITION_NED & pos_ned,
plugin::filter::SystemAndOk filter [[maybe_unused]])
{
has_local_position_ned = true;
//--------------- Transform FCU position and Velocity Data ---------------//
auto enu_position = ftf::transform_frame_ned_enu(
Eigen::Vector3d(
pos_ned.x, pos_ned.y,
pos_ned.z));
auto enu_velocity =
ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.vx, pos_ned.vy, pos_ned.vz));
//--------------- Get Odom Information ---------------//
// Note this orientation describes baselink->ENU transform
auto enu_orientation_msg = uas->data.get_attitude_orientation_enu();
auto baselink_angular_msg = uas->data.get_attitude_angular_velocity_enu();
Eigen::Quaterniond enu_orientation; tf2::fromMsg(enu_orientation_msg, enu_orientation);
auto baselink_linear =
ftf::transform_frame_enu_baselink(enu_velocity, enu_orientation.inverse());
auto odom = nav_msgs::msg::Odometry();
odom.header = uas->synchronized_header(frame_id, pos_ned.time_boot_ms);
odom.child_frame_id = tf_child_frame_id;
odom.pose.pose.position = tf2::toMsg(enu_position);
odom.pose.pose.orientation = enu_orientation_msg;
tf2::toMsg(baselink_linear, odom.twist.twist.linear);
odom.twist.twist.angular = baselink_angular_msg;
// publish odom if we don't have LOCAL_POSITION_NED_COV
if (!has_local_position_ned_cov) {
local_odom->publish(odom);
}
// publish pose always
auto pose = geometry_msgs::msg::PoseStamped();
pose.header = odom.header;
pose.pose = odom.pose.pose;
local_position->publish(pose);
// publish velocity always
// velocity in the body frame
auto twist_body = geometry_msgs::msg::TwistStamped();
twist_body.header.stamp = odom.header.stamp;
twist_body.header.frame_id = tf_child_frame_id;
twist_body.twist.linear = odom.twist.twist.linear;
twist_body.twist.angular = baselink_angular_msg;
local_velocity_body->publish(twist_body);
// velocity in the local frame
auto twist_local = geometry_msgs::msg::TwistStamped();
twist_local.header.stamp = twist_body.header.stamp;
twist_local.header.frame_id = tf_child_frame_id;
tf2::toMsg(enu_velocity, twist_local.twist.linear);
tf2::toMsg(
ftf::transform_frame_baselink_enu(ftf::to_eigen(baselink_angular_msg), enu_orientation),
twist_local.twist.angular);
local_velocity_local->publish(twist_local);
// publish tf
publish_tf(odom);
}
其中，得到ENU的位置和姿态是handle_local_position_ned函数下面这部分代码，可以看到，通过transform_frame_ned_enu函数直接把NED坐标系下的位置转到ENU坐标系下了，一步完成，也就是ENU的位置是直接NED的位置做北东地到东北天的变换直接得到的。ENU的姿态四元数是直接通过get_attitude_orientation_enu()函数得到的，get_attitude_orientation_enu()函数的定义不在local_position.cpp里，需要转到uas_data.cpp。
//--------------- Transform FCU position and Velocity Data ---------------//
auto enu_position = ftf::transform_frame_ned_enu(
Eigen::Vector3d(
pos_ned.x, pos_ned.y,
pos_ned.z));
auto enu_velocity =
ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.vx, pos_ned.vy, pos_ned.vz));
//--------------- Get Odom Information ---------------//
// Note this orientation describes baselink->ENU transform
auto enu_orientation_msg = uas->data.get_attitude_orientation_enu();
auto baselink_angular_msg = uas->data.get_attitude_angular_velocity_enu();
Eigen::Quaterniond enu_orientation; tf2::fromMsg(enu_orientation_msg, enu_orientation);
auto baselink_linear =
ftf::transform_frame_enu_baselink(enu_velocity, enu_orientation.inverse());
上面的get_attitude_orientation_enu()函数来自这里，可以看到get_attitude_orientation_enu()函数是返回的imu_enu_data里的四元数，并且通过update_attitude_imu_enu()函数更新imu_enu_data里的四元数，update_attitude_imu_enu()函数又是在imu.cpp里被调用，我们可以转到imu.cpp里看。
/mavros/mavros/src/lib/uas_data.cpp
void Data::update_attitude_imu_enu(const sensor_msgs::msg::Imu & imu)
{
s_unique_lock lock(mu);
imu_enu_data = imu;
}
void Data::update_attitude_imu_ned(const sensor_msgs::msg::Imu & imu)
{
s_unique_lock lock(mu);
imu_ned_data = imu;
}
geometry_msgs::msg::Quaternion Data::get_attitude_orientation_enu()
{
s_shared_lock lock(mu);
return imu_enu_data.orientation;
}
下面imu.cpp的publish_imu_data函数里有用到uas_data.cpp里的update_attitude_imu_enu()函数，实现把imu.cpp里转换好的ENU坐标系下ros_body系的四元数更新到imu_enu_data.orientation里，local_position.cpp里通过get_attitude_orientation_enu()函数获取到ENU坐标系下ros_body系的四元数，最后转为ros话题发布出去，也就是/mavros/local_position/pose。
具体把NED的四元数转到ENU下是在handle_attitude函数里面完成的，可以看出NED的姿态是来自于飞控发来的ATTITUDE mavlink消息，handle_attitude函数里，ned_aircraft_orientation是来自飞控计算得到的NED(fcu_local)-Aircraft(fcu_body)的四元数姿态坐标，而enu_baselink_orientation是将坐标系经过两次转换变成了ENU(ros_local)-Baselink(ros_body)坐标系的四元数姿态坐标，这两步具体是，首先通过transform_orientation_ned_enu()函数把fcu_body在fcu_local下的姿态转到fcu_body在ros_local下的姿态，再通过transform_orientation_aircraft_baselink()函数把fcu_body在ros_local下的姿态转到ros_body再ros_local下的姿态。
得到ros_body再ros_local下的姿态后，handle_attitude函数最后还调用publish_imu_data函数发出了/mavros/imu/data话题，可以看出/mavros/imu/data话题里的四元数和/mavros/local_position/pose中的四元数是一样的。
/mavros/mavros/src/plugins/imu.cpp
/**
* @brief Fill and publish IMU data message.
* @param time_boot_ms Message timestamp (not syncronized)
* @param orientation_enu Orientation in the base_link ENU frame
* @param orientation_ned Orientation in the aircraft NED frame
* @param gyro_flu Angular velocity/rate in the base_link Forward-Left-Up frame
* @param gyro_frd Angular velocity/rate in the aircraft Forward-Right-Down frame
*/
void publish_imu_data(
uint32_t time_boot_ms, Eigen::Quaterniond & orientation_enu,
Eigen::Quaterniond & orientation_ned, Eigen::Vector3d & gyro_flu, Eigen::Vector3d & gyro_frd)
{
auto imu_ned_msg = sensor_msgs::msg::Imu();
auto imu_enu_msg = sensor_msgs::msg::Imu();
// Fill message header
imu_enu_msg.header = uas->synchronized_header(frame_id, time_boot_ms);
imu_ned_msg.header = uas->synchronized_header("aircraft", time_boot_ms);
// Convert from Eigen::Quaternond to geometry_msgs::Quaternion
imu_enu_msg.orientation = tf2::toMsg(orientation_enu);
imu_ned_msg.orientation = tf2::toMsg(orientation_ned);
// Convert from Eigen::Vector3d to geometry_msgs::Vector3
tf2::toMsg(gyro_flu, imu_enu_msg.angular_velocity);
tf2::toMsg(gyro_frd, imu_ned_msg.angular_velocity);
// Eigen::Vector3d from HIGHRES_IMU or RAW_IMU, to geometry_msgs::Vector3
tf2::toMsg(linear_accel_vec_flu, imu_enu_msg.linear_acceleration);
tf2::toMsg(linear_accel_vec_frd, imu_ned_msg.linear_acceleration);
// Pass ENU msg covariances
imu_enu_msg.orientation_covariance = orientation_cov;
imu_enu_msg.angular_velocity_covariance = angular_velocity_cov;
imu_enu_msg.linear_acceleration_covariance = linear_acceleration_cov;
// Pass NED msg covariances
imu_ned_msg.orientation_covariance = orientation_cov;
imu_ned_msg.angular_velocity_covariance = angular_velocity_cov;
imu_ned_msg.linear_acceleration_covariance = linear_acceleration_cov;
if (!received_linear_accel) {
// Set element 0 of covariance matrix to -1
// if no data received as per sensor_msgs/Imu defintion
imu_enu_msg.linear_acceleration_covariance[0] = -1;
imu_ned_msg.linear_acceleration_covariance[0] = -1;
}
/** Store attitude in base_link ENU
* @snippet src/plugins/imu.cpp store_enu
*/
// [store_enu]
uas->data.update_attitude_imu_enu(imu_enu_msg);
// [store_enu]
/** Store attitude in aircraft NED
* @snippet src/plugins/imu.cpp store_ned
*/
// [store_enu]
uas->data.update_attitude_imu_ned(imu_ned_msg);
// [store_ned]
/** Publish only base_link ENU message
* @snippet src/plugins/imu.cpp pub_enu
*/
// [pub_enu]
imu_pub->publish(imu_enu_msg);
// [pub_enu]
}
/**
* @brief Handle ATTITUDE MAVlink message.
* Message specification: https://mavlink.io/en/messages/common.html#ATTITUDE
* @param msg Received Mavlink msg
* @param att ATTITUDE msg
*/
void handle_attitude(
const mavlink::mavlink_message_t * msg [[maybe_unused]],
mavlink::common::msg::ATTITUDE & att,
plugin::filter::SystemAndOk filter [[maybe_unused]])
{
if (has_att_quat) {
return;
}
/** Orientation on the NED-aicraft frame:
* @snippet src/plugins/imu.cpp ned_aircraft_orient1
*/
// [ned_aircraft_orient1]
auto ned_aircraft_orientation = ftf::quaternion_from_rpy(att.roll, att.pitch, att.yaw);
// [ned_aircraft_orient1]
/** Angular velocity on the NED-aicraft frame:
* @snippet src/plugins/imu.cpp ned_ang_vel1
*/
// [frd_ang_vel1]
auto gyro_frd = Eigen::Vector3d(att.rollspeed, att.pitchspeed, att.yawspeed);
// [frd_ang_vel1]
/** The RPY describes the rotation: aircraft->NED.
* It is required to change this to aircraft->base_link:
* @snippet src/plugins/imu.cpp ned->baselink->enu
*/
// [ned->baselink->enu]
auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink(
ftf::transform_orientation_ned_enu(ned_aircraft_orientation));
// [ned->baselink->enu]
/** The angular velocity expressed in the aircraft frame.
* It is required to apply the static rotation to get it into the base_link frame:
* @snippet src/plugins/imu.cpp rotate_gyro
*/
// [rotate_gyro]
auto gyro_flu = ftf::transform_frame_aircraft_baselink(gyro_frd);
// [rotate_gyro]
publish_imu_data(
att.time_boot_ms, enu_baselink_orientation, ned_aircraft_orientation, gyro_flu,
gyro_frd);
}
经过上面对px4-mavros中的坐标系的详细分析后，现在再来回看T265定点时，板载端和飞控端的位姿变换，就非常清晰了。
T265定点时，初始时候，也就是启动T265并且把位姿传给飞控后，无人机和T265都没有移动的情况下，从飞控层面看，也即是在QGC地面站上看到的，机头初始方向是朝东的，并且会显示偏航是90度，如下图所示。此时板载层面看到的/mavros/local_position/pose的四元数是x:0 y:0 z:0 w:1，即板载层面对应的俯仰横滚偏航都是0度。
地面站端看到的飞控的姿态角和四元数
飞控发出的ATTITUDE mavlink消息
飞控发出的ATTITUDE_QUATERNION mavlink消息
板载端的/mavros/local_position/pose
如果不清楚fcu_body,fcu_local,ros_body,ros_local这几个坐标系的基本概念和关系，单纯地把MAVROS理解为把板载的ENU位姿转为飞控的NED位姿，把飞控的NED位姿转为板载的ENU位姿，仔细推导会发现有的地方对不上，比如直接把板载端横滚俯仰偏航都是0度的姿态，按照ENU和NED的关系转到NED下，会发现，NED下的姿态角应该是横滚180度，俯仰0度，偏航90度，会发现和QGC上看到的飞控姿态角不一样，横滚角对不上了，而且飞控横滚角应该是0度才对，不然无人机就翻转了，但是如果横滚角是0度，那么飞控的姿态角和板载的姿态角的变换关系就不符合NED和ENU的变换关系，清楚了fcu_body,fcu_local,ros_body,ros_local这几个坐标系的基本概念和关系后，这就好理解了，板载端显示的位姿是ros_body系在ros_local系下的位姿，飞控端显示的位姿是fcu_body系在fcu_local系下的位姿，刚才我们直接对板载的姿态做ENU到NED的转换，发现和飞控的姿态在横滚上还差180度，这180度就是因为ros_body和fcu_body在横滚上相差180度导致的。
所以T265定点时，为什么飞控端看到的偏航是90度不是0度，因为我们优先保证板载端的初始姿态是横滚俯仰偏航都为0度，也就是初始时ros_body系在ros_local系下的位置是(0,0,0),三个姿态角横滚俯仰偏航也都为0，方便我们在板载端在ROS层面处理无人机的位姿数据，对应转换得到的fcu_body系在fcu_local系下的姿态角就是横滚0度，俯仰0度，偏航90度。
T265定点时，无人机往机头方向移动，板载层面会看到local_position的x正向增大，QGC里面看到的mavlink中的local_position的y正向增大，自然也很好理解了。
为什么会出现一些新手刚开始飞T265指点时，会出现无人机起飞后偏航会转90度的情况，也是坐标系没有理清造成的。
一些早期的T265定点功能包，比如vision_to_mavros，只是为了单纯实现T265定点，是使得T265定点的机头初始方向是朝北的，板载端发出的/mavros/vision_pose/pose话题的四元数是x:0 y:0 z:0.707 w:0.707，对应的欧拉角是横滚0度，俯仰0度，偏航90度（四元数和欧拉角的转换可以用网站 https://quaternions.online/ 或者 https://www.andre-gaschler.com/rotationconverter/ ），也就是转换后得到的fcu_body系在fcu_local系下的姿态角横滚俯仰偏航都是0度，也好理解，飞控端local系是北东地坐标系，朝北时对机体而言偏航为0，不过板载端，初始姿态就不是横滚俯仰偏航为0度了，这对板载端处理无人机位姿可能就不太友好了，不过这么飞T265定点是没有问题的。其实要飞成T265定点，初始机头方向朝向哪个方向都可以，只用保证飞控Body系是FRD，local系是NED的关系就可以，这个NED可以是现实世界的NED，也可以是自己假定的NED，当然一般是假定偏航，比如初始偏航为0或者90度之类的，就是可以对T265输出的东北天坐标系下的odom，在偏航层面做其他度数的旋转再通过/mavros/vision_pose/pose发给飞控，理论上也是可以飞成T265定点的，就像GPS定点（偏航来自于磁力计），无人机初始朝哪个方向摆放，都是可以飞起来定住的，不过有一点需要注意，NED虽然偏航方向可以自己任意设定，但是一定得保证对应的xyz是符合这个所给定的NED坐标系的，就是比如无人机往这个NED的E方向移动的时候，飞控端得是y正向增大而x和z保持不变，一般我们同时把转换后的T265位姿和姿态发给飞控，这么是没有问题的，因为T265输出的xyz和姿态本身是匹配的，一起做变换后也依旧是匹配的，但是如果我们只把位置xyz发给飞控，但是姿态还是用的飞控自身IMU和磁力计的姿态，可能就不满足xyz和NED匹配，比如无人机往这个NED的E方向移动的时候，飞控端不满足y正向增大而x和z保持不变了，就可能会出问题，比如定点时飞控层面y在正向增大，那么飞控控制环会让无人机往飞控认为的西边飞进行调整保持定点，但是如果此时飞控认为的西边和y的负方向不是一致的，控制可能就紊乱了，便无法完成T265定点，定点时可能表现为无人机偏航不变的情况下螺旋发散转圈。
飞T265指点时，由于板载端发期望位置和偏航都是基于东北天坐标系发的，一般默认期望偏航就是0度了（比如发/mavros/setpoint_position/local话题），这个0度自然也是板载东北天坐标系(ros_local)下的ros_body的期望偏航为0度，转到北东地坐标系(ros_local)下，转到fcu_local系也就是北东地坐标系下就会变为fcu_body期望偏航是90度（东）了，而他们原本用的T265定点机头初始方向是正北，现在给了个正东的期望偏航，自然就会转90度，换作GPS定点的无人机，这么发期望偏航，只要无人机机头不是朝向正北指点起飞一样也会转动相应角度，道理是相同的。如果T265定点时使得fcu_local下的fcu_body的初始偏航就是朝向正东的话，也就是ros_local下的ros_body的初始偏航为0，就不会有这个问题。
用vision_to_mavros功能包飞T265定点时初始发出的/mavros/vision_pose/pose话题，注意四元数不是x:0 y:0 z:0 w:1了，是x:0 y:0 z:0.707 w:0.707，对应的欧拉角为横滚0度，俯仰0度，偏航90度。
扫一扫
2993
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
