前言

在之前gazebo + mavros + px4的环境中，我们在启动环境后只需要发布目标位姿即可，mavros会自动将位姿转递给px4飞控，后者分析出推力的大小并发送给gazebo的动力学系统，计算出无人机的位置，同时更新视觉渲染。而在flightmare的环境中，视觉渲染与动力学更新被拆分开来，即无人机飞了多远和能看到什么并不是同步计算的，在之前的基础使用流程中，已经知道了如何去渲染画面（发送无人机的位置，由unity计算出当前无人机能看到什么），但是无人机的位置是由setState直接设定的，所以这里需要补上从给出期望的无人机位姿，到获取无人机位置的过程。

更具体而言，这个过程可以分为两个阶段：第一个阶段是输出期望的位姿，经由飞控处理，给出无人机的推力；第二个阶段是根据无人机的推力，动力学仿真器积分计算出无人机运动了多远。其中飞控部分，flightmare由于没有PX4的中介插件，故无法使用PX4，而是使用自带的轻量化飞控。而对于动力学，flightmare有两种动力学思路，一种是“Flightmare 自带轻量动力学 + Unity 渲染”的flightlib路线；另一种是之前在安装测试时用过的，使用“Gazebo 物理 + RPG 控制器 + Flightmare 仅做渲染/可视化”的RotorS + RPG路线。本文中首先对自带轻量化动力学的使用进行学习。

注意：本文中部分内容由AI生成，故仅供参考

Flightmare自带的轻量化动力学

轻量化动力学

和视觉渲染一样，flightmare的动力学也需要手动操作，即需要手动调用接口输入无人机的推力，然后计算下一帧无人机的位置。而为了简单起见，这里先不管推力是如何得到的（飞控的工作），核心代码如下：

quad_ptr->run(cmd, kDt);

其中quad_ptr是无人机指针，而cmd是推力指令，kDt是运动学仿真的单位时间。每调用一次run，动力学便计算一帧。并且要注意run指令在计算出无人机的位置后会直接写入到Quadrotor内部，即不再需要手动setState来设置无人机的位置，而可以直接getRender去渲染画面。

更完整的部分代码如下所示（此处先忽略推力是怎么来的）：

// 控制指令数据结构
#include "flightlib/common/command.hpp"
...
// 声明无人机指针并位置归零
std::shared_ptr<Quadrotor> quad_ptr = std::make_shared<Quadrotor>();
quad_state_.setZero();
quad_ptr_->reset(quad_state_);
...
// 声明指令体
Command cmd;
cmd.t = 0.0;
...
循环{
	quad_ptr->getState(&quad_state_);// 读取当前位置
	...
	cmd.t += kDt;
    cmd.collective_thrust = ...;//推力设置
    cmd.omega.setZero();
	...
    // 动力学推进下一时刻状态。
    if (!quad_ptr->run(cmd, kDt)) {
      ROS_ERROR("quad_ptr->run failed.");
      return 1;
    }
	...
    // 把当前状态发送给 Unity 显示。
    unity_bridge_ptr->getRender(frame_id);
    unity_bridge_ptr->handleOutput();
}

首先需要无人机位置归零；其次在每一个循环中，需要先读取当前位置，然后计算推力指令，然后动力学推进下一时刻，最后推送给unity显示。

轻量化飞控

在上面的轻量化动力学中，我们暂时省略了推力是怎么产生的，这通常是飞控的作用，即根据期望的无人机位姿去计算出电机的推力。但是flightmare缺少对于PX4的支持，转而使用自己的轻量化飞控，故与之前的Mavros+PX4的工作流程又有不同。虽然在github上能找到一个flightmare+PX4项目gipsa-lab-uav/flightmare_px4，但似乎年代久远无法正常运行。要注意的是，官方好像没有专门提供关于飞控信息与使用的文档，只提供了如何使用gazebo作为仿真的示例

flightmare所提供的飞控叫autopilot，类似于PX4的功能，也存在不同的状态、需要传入心跳信号等，不过因为flightmare也在项目文件下，所以可以直接看到并调用一些飞控的基础功能接口。其中最为关键的部分为position_controller，即计算从目标位姿到推力输出的模块，但并不建议直接手动调用该接口，而是使用完整的autopilot。在使用时，与PX4在仿真时的使用相同，autopilot也是作为一个单独节点来启动，而不是放在控制节点中；参考官方的使用方式，autopilot通过launch文件来启动。而注意，autopilot在使用时需要通过.yaml文件来传递飞控参数，这里可以使用官方默认的参数。

<!-- Autopilot -->
    <node pkg="autopilot" type="autopilot" name="autopilot" output="screen">
      <rosparam file="$(find state_predictor)/parameters/hummingbird.yaml" />
      <rosparam file="$(find rpg_rotors_interface)/parameters/position_controller.yaml" />
      <rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml" />

      <param name="position_controller/use_rate_mode" value="True" />

      <param name="velocity_estimate_in_world_frame" value="false" />
      <param name="state_estimate_timeout" value="0.1" />
      <param name="control_command_delay" value="0.05" />
      <param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)" />

      <remap from="autopilot/state_estimate" to="ground_truth/odometry" />
    </node>

---

具体而言，autopilot作为封装好的飞控，对外一般使用如下几个接口：
输入：

• 话题：autopilot/state_estimate 数据类型：nav_msgs/Odometry 作用：无人机当前位置
• 话题: autopilot/pose_command 数据类型：geometry_msgs::PoseStamped 作用：无人机期望的位置
• 话题：autopilot/start 数据类型：std_msgs:Empty 作用：切换飞控模式为启动模式

输出：

• 话题：control_command 数据类型：quadrotor_msgs/ControlCommand 作用：控制指令

使用时，在launch文件中启动飞控之后，需要发送一个start模式切换信号，然后才能在循环中发送odom与pose_command以及接收controlcommand

---

除了使用ROS话题作为接口之外，还需要添加一些flightlib到autopilot的中介模块，因为两者并不是一开始就设计一起使用的
数据格式转换： 从flightlib使用的quad_state转换为autopilot接收的nav_msgs/Odometry：

nav_msgs::Odometry quadStateToOdometry(const QuadState& quad_state) {
  nav_msgs::Odometry odom;
  odom.header.stamp = ros::Time::now();
  odom.header.frame_id = "world";
  odom.child_frame_id = "body";
  odom.pose.pose.position.x = quad_state.x[QS::POSX];
  odom.pose.pose.position.y = quad_state.x[QS::POSY];
  odom.pose.pose.position.z = quad_state.x[QS::POSZ];
  odom.pose.pose.orientation.w = quad_state.x[QS::ATTW];
  odom.pose.pose.orientation.x = quad_state.x[QS::ATTX];
  odom.pose.pose.orientation.y = quad_state.x[QS::ATTY];
  odom.pose.pose.orientation.z = quad_state.x[QS::ATTZ];
  odom.twist.twist.linear.x = quad_state.x[QS::VELX];
  odom.twist.twist.linear.y = quad_state.x[QS::VELY];
  odom.twist.twist.linear.z = quad_state.x[QS::VELZ];
  odom.twist.twist.angular.x = quad_state.x[QS::OMEX];
  odom.twist.twist.angular.y = quad_state.x[QS::OMEY];
  odom.twist.twist.angular.z = quad_state.x[QS::OMEZ];
  return odom;
}

指令接收缓存： 缓存从autopliot发出的指令

class ControlCommandBuffer {
 public:
  void callback(const quadrotor_msgs::ControlCommand::ConstPtr& msg) {
    std::lock_guard<std::mutex> lock(mutex_);
    latest_cmd_ = *msg;
    has_cmd_ = true;
  }

  bool getLatest(quadrotor_msgs::ControlCommand* cmd) const {
    std::lock_guard<std::mutex> lock(mutex_);
    if (!has_cmd_) {
      return false;
    }
    *cmd = latest_cmd_;
    return true;
  }

 private:
  mutable std::mutex mutex_;
  quadrotor_msgs::ControlCommand latest_cmd_;
  bool has_cmd_{false};
};

结果测试以及总结

测示结果如图，无人机成功起飞，且可以观察到不是之前setStates时的匀速运动，而是存在加减速的过程

参考内容

Basic Usage with ROS · uzh-rpg/flightmare Wiki