【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理

【ROS&GAZEBO】多旋翼无人机仿真（一）——搭建仿真环境
【ROS&GAZEBO】多旋翼无人机仿真（二）——基于rotors的仿真
【ROS&GAZEBO】多旋翼无人机仿真（三）——自定义多旋翼模型
【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理
【ROS&GAZEBO】多旋翼无人机仿真（五）——位置控制器

原理

在gazebo编写控制器有两种方式，一种是直接编写gazebo插件，另一种是编写ros节点，这两种方式的共同点是都得使用C++编程和CMakeLists，对这两个工具都需掌握一定的熟练度。

上一篇的模型中，我们没有写任何C++代码，但是多旋翼是怎样飞起来的呢，我们来探索一下我们编写的urdf文件的原理。

打开reed_simulator/reed_gazebo/urdf/reed_quad_x_base.xacro，看到下面的代码：

<robot name="reed_quad_x" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:include filename="$(find rotors_description)/urdf/component_snippets.xacro" />
  
  <xacro:include filename="$(find reed_gazebo)/urdf/reed_quad_x.xacro" />

  
  <xacro:controller_plugin_macro namespace="${namespace}" imu_sub_topic="imu" />
  
  
  

  <xacro:if value="$(arg enable_mavlink_interface)">
    
    <xacro:default_mavlink_interface namespace="${namespace}" imu_sub_topic="imu" rotor_count="6" />
  xacro:if>

  
  <xacro:default_imu namespace="${namespace}" parent_link="${namespace}/base_link" />

  <xacro:if value="$(arg enable_ground_truth)">
    <xacro:ground_truth_imu_and_odometry namespace="${namespace}" parent_link="${namespace}/base_link" />
  xacro:if>

  <xacro:if value="$(arg enable_logging)">
    
    <xacro:bag_plugin_macro
      namespace="${namespace}"
      bag_file="$(arg log_file)"
      rotor_velocity_slowdown_sim="${rotor_velocity_slowdown_sim}" 
      wait_to_record_bag="$(arg wait_to_record_bag)" />
  xacro:if>

robot>

第一行代码是将component_snippets.xacro文件引用到本xacro文件中，作用类似于c++中的引用头文件，如下：

  <xacro:include filename="$(find rotors_description)/urdf/component_snippets.xacro" />

第二行代码是插入reed_quad_x.xacro模型：

  <xacro:include filename="$(find reed_gazebo)/urdf/reed_quad_x.xacro" />

这一行代码的作用是插入一个和ROS交流的插件，作用是发布控制器指令：

  <xacro:controller_plugin_macro namespace="${namespace}" imu_sub_topic="imu" />

这几行是通过if判断命令来决定是否插入MAVLINK通信的插件：

  <xacro:if value="$(arg enable_mavlink_interface)">
    
    <xacro:default_mavlink_interface namespace="${namespace}" imu_sub_topic="imu" rotor_count="6" />
  xacro:if>

这几行是插入了一个IMU插件，模拟了一个ADIS16448用来获取导航信息：

  
  <xacro:default_imu namespace="${namespace}" parent_link="${namespace}/base_link" />

到此为止好像并没有发现控制器在哪里，不用着急，我们再深入的去挖掘，

打开reed_simulator/reed_gazebo/launch/mav_hovering_example.launch，在这里我们去找一找controller相关的信息

<launch>
  <arg name="mav_name" default="reed_quad_x"/>
  <arg name="world_name" default="basic"/>
  <arg name="enable_logging" default="false" />
  <arg name="enable_ground_truth" default="true" />
  <arg name="log_file" default="$(arg mav_name)" />
  <arg name="debug" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="paused" default="true"/>
  
  <arg name="verbose" default="false"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name).world" />
    <arg name="debug" value="$(arg debug)" />
    <arg name="paused" value="$(arg paused)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="verbose" value="$(arg verbose)"/>
  include>

  <group ns="$(arg mav_name)">
    <include file="$(find reed_gazebo)/launch/spawn_mav.launch">
      <arg name="mav_name" value="$(arg mav_name)" />
      <arg name="model" value="$(find reed_gazebo)/urdf/mav_generic_odometry_sensor.gazebo" />
      <arg name="enable_logging" value="$(arg enable_logging)" />
      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
      <arg name="log_file" value="$(arg log_file)"/>
    include>
    <node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
      <rosparam command="load" file="$(find reed_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
      <rosparam command="load" file="$(find reed_gazebo)/resource/$(arg mav_name).yaml" />
      <remap from="odometry" to="odometry_sensor1/odometry" />
    node>
    <node name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  group>
launch>

我们发现有这样的几行代码：

<node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">

这里的意思是创建了一个lee_position_controller_node节点，表面意思就是创建一个位置控制器节点，那有位置控制器，那姿态控制器在哪呢，我第一次看到这里时怀疑位置控制器中已经包含了姿态控制，先带着疑惑来看看lee_position_controller_node是如何写的，找到位置控制器的路径：rotors_simulator/rotors_control/src/nodes/lee_position_controller_node.cpp。

看到这里发现了，这个节点是由c++写的，那是怎么做到的，来梳理一下这里面的逻辑，

int main(int argc, char** argv) {
  ros::init(argc, argv, "lee_position_controller_node");

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");
  rotors_control::LeePositionControllerNode lee_position_controller_node(nh, private_nh);

  ros::spin();

  return 0;
}

在文件最后是main函数，里面创建了一个lee_position_controller_node节点，前几行是ros的一个通用写法，如果熟悉ros就不会感到奇怪（不熟悉的建议花点时间学习ros的基础知识），

rotors_control::LeePositionControllerNode lee_position_controller_node(nh, private_nh);实例化了LeePositionControllerNode类。

这个类里面有什么呢，打开头文件：rotors_simulator/rotors_control/src/nodes/lee_position_controller_node.h

namespace rotors_control {

class LeePositionControllerNode {
 public:
  LeePositionControllerNode(const ros::NodeHandle& nh, const ros::NodeHandle& private_nh);
  ~LeePositionControllerNode();

  void InitializeParams();
  void Publish();

 private:
  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;

  LeePositionController lee_position_controller_;

  std::string namespace_;

  // subscribers
  ros::Subscriber cmd_trajectory_sub_;
  ros::Subscriber cmd_multi_dof_joint_trajectory_sub_;
  ros::Subscriber cmd_pose_sub_;
  ros::Subscriber odometry_sub_;

  ros::Publisher motor_velocity_reference_pub_;

  mav_msgs::EigenTrajectoryPointDeque commands_;
  std::deque<ros::Duration> command_waiting_times_;
  ros::Timer command_timer_;

  void TimedCommandCallback(const ros::TimerEvent& e);

  void MultiDofJointTrajectoryCallback(
      const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_reference_msg);

  void CommandPoseCallback(
      const geometry_msgs::PoseStampedConstPtr& pose_msg);

  void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg);
};
}

看起来这个类不是很复杂，主要的函数就那么几个，void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg);是获取导航信息的回调函数，直觉告诉我这里面一定藏着关键信息，打开来看一下：

void LeePositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {

  ROS_INFO_ONCE("LeePositionController got first odometry message.");

  EigenOdometry odometry;
  eigenOdometryFromMsg(odometry_msg, &odometry);
  lee_position_controller_.SetOdometry(odometry);

  Eigen::VectorXd ref_rotor_velocities;
  lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);

  // Todo(ffurrer): Do this in the conversions header.
  mav_msgs::ActuatorsPtr actuator_msg(new mav_msgs::Actuators);

  actuator_msg->angular_velocities.clear();
  for (int i = 0; i < ref_rotor_velocities.size(); i++)
    actuator_msg->angular_velocities.push_back(ref_rotor_velocities[i]);
  actuator_msg->header.stamp = odometry_msg->header.stamp;

  motor_velocity_reference_pub_.publish(actuator_msg);
}

前几行是将导航信息转换成Eigen格式的数据：

  EigenOdometry odometry;
  eigenOdometryFromMsg(odometry_msg, &odometry);
  lee_position_controller_.SetOdometry(odometry);

lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);从名字来看是计算电机速度的函数，进去看一下：

void LeePositionController::CalculateRotorVelocities(Eigen::VectorXd* rotor_velocities) const {
  assert(rotor_velocities);
  assert(initialized_params_);

  rotor_velocities->resize(vehicle_parameters_.rotor_configuration_.rotors.size());
  // Return 0 velocities on all rotors, until the first command is received.
  if (!controller_active_) {
    *rotor_velocities = Eigen::VectorXd::Zero(rotor_velocities->rows());
    return;
  }

  Eigen::Vector3d acceleration;
  ComputeDesiredAcceleration(&acceleration);

  Eigen::Vector3d angular_acceleration;
  ComputeDesiredAngularAcc(acceleration, &angular_acceleration);

  // Project thrust onto body z axis.
  double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));

  Eigen::Vector4d angular_acceleration_thrust;
  angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
  angular_acceleration_thrust(3) = thrust;

  *rotor_velocities = angular_acc_to_rotor_velocities_ * angular_acceleration_thrust;
  *rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
  *rotor_velocities = rotor_velocities->cwiseSqrt();
}

这四行代码的意思是ComputeDesiredAcceleration(&acceleration);计算期望的加速度，ComputeDesiredAngularAcc(acceleration, &angular_acceleration);计算期望的角加速度。
加速度和角加速度有什么用呢，学过多旋翼控制的都知道，多旋翼位置控制的结果是期望的加速度，姿态控制的结果是期望的角加速度，角加速度通过混控器最后得到期望的电机转速，这是整个的闭环过程，这里正好对应的是这两个控制过程，说明和我们猜想的一样，位置控制包含了整个位置和姿态控制过程。

  Eigen::Vector3d acceleration;
  ComputeDesiredAcceleration(&acceleration);

  Eigen::Vector3d angular_acceleration;
  ComputeDesiredAngularAcc(acceleration, &angular_acceleration);

这里将推力映射到z轴上，至于为什么要映射到z轴上，是因为传统多旋翼电机的推力只能垂直于z轴：

  // Project thrust onto body z axis.
  double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));

这里的angular_acceleration_thrust是一个4*1的向量，由xyz角加速度和z轴推力组成：

  Eigen::Vector4d angular_acceleration_thrust;
  angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
  angular_acceleration_thrust(3) = thrust;

最后经过控制分配，也叫混控器，angular_acc_to_rotor_velocities_即为控制分配矩阵：

  *rotor_velocities = angular_acc_to_rotor_velocities_ * angular_acceleration_thrust;
  *rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
  *rotor_velocities = rotor_velocities->cwiseSqrt();

到这里，相信基本上应该有了rotors多旋翼控制器的思路，总结下来是创建了一个位姿控制器节点，里面包括了位置控制器、姿态控制器、混控器。位置控制器输出的结果是加速度，姿态控制器输出的是角加速度，角加速度和z轴推力经过混控器最终得到电机转速。

下一篇继续讲解位置和姿态控制器的原理。

喜欢的朋友可以点个赞，关注微信公众号相互交流：Reed UAV