无人机控制器geometric_control代码详解（1）

引言

本篇博文意在整体梳理一下geometric_control无人机控制器的算法逻辑，顺便加深以下自己对该算法的理解...

整个无人机的定位和控制执行流程如下：

LIO ——> uav_vison_control ——> geometric_control ——> FCU

/odometry ——> /mavros/vision_pose/pose ——> /mavros/local_position/pose ——> /mavros/setpoint_raw/attitude

首先通过视觉里程计(VIO)或激光里程计(LIO)获取无人机定位信息，然后将通过定位信息转换节点，将里程计通过/mavros/vision_pose/pose话题输入给无人机飞控，与飞控IMU作EKF融合后，通过/mavros/local_position/pose话题输出基于飞控坐标系的定位信息，以此来作为无人机的位置控制基准。

该算法总体围绕几大飞行状态展开：

起飞点载入------WAITING_FOR_HOME_POSE

起飞------TAKING_OFF

任务执行------MISSION_EXCUTION

任务保持------MISSION_HOLD

开始降落------LANDING

降落完成------LANDED

enum FlightState {
    WAITING_FOR_HOME_POSE,
    TAKING_OFF,
    MISSION_EXECUTION,
    MISSION_HOLD,
    LANDING,
    LANDED
  } node_state;

飞行状态详解

控制算法程序通过如下控制循环回调函数进行,通过switch开关来进行不同的无人机控制状态

void geometricCtrl::cmdloopCallback(const ros::TimerEvent &event) { 
  switch (node_state) {
      case WAITING_FOR_HOME_POSE:   
      case TAKING_OFF:
      case MISSION_EXECUTION:
      case MISSION_HOLD:
      case LANDING:
      case LANDED:
  }
}

1. WAITING_FOR_HOME_POSE

首先订阅无人机飞控/mavros/local_position/pose，获取飞控位置

 mavposeSub_ = nh_.subscribe("mavros/local_position/pose", 1,
                              &geometricCtrl::mavposeCallback, this,
                              ros::TransportHints().tcpNoDelay());

void geometricCtrl::mavposeCallback(const geometry_msgs::PoseStamped &msg) {
  if (!received_home_pose) {
    received_home_pose = true;
    home_pose_ = msg.pose;
    ROS_INFO_STREAM("Home pose initialized to: " << home_pose_);
  }
  mavPos_ = toEigen(msg.pose.position);
  mavAtt_(0) = msg.pose.orientation.w;
  mavAtt_(1) = msg.pose.orientation.x;
  mavAtt_(2) = msg.pose.orientation.y;
  mavAtt_(3) = msg.pose.orientation.z;
}

 执行case：WAITING_FOR_HOME_POSE

 waitForPredicate（）获取无人机初始位置，获取到无人机位置后，切换为TAKING_OFF起飞状态

case WAITING_FOR_HOME_POSE:
    waitForPredicate(&received_home_pose, "Waiting for home pose...");
    ROS_INFO("Got pose! Drone Ready to be armed.");
    // node_state = MISSION_EXECUTION;
    node_state = TAKING_OFF;
    break;

void waitForPredicate(const T *pred, const std::string &msg,
                        double hz = 2.0) {
    ros::Rate pause(hz);
    ROS_INFO_STREAM(msg);
    while (ros::ok() && !(*pred)) {
      ros::spinOnce();
      pause.sleep();
    }
  }

2. TAKING_OFF

执行case：TAKING_OFF

通过/mavros/setpoint_position/local话题，发布起飞目标点位置信息：initTargetPos_x_，initTargetPos_y_，initTargetPos_z_，初始位置由rosparam手动提供，初定为(0, 0, 1)点

  nh_private_.param<double>("init_pos_x", initTargetPos_x_, 0.0);
  nh_private_.param<double>("init_pos_y", initTargetPos_y_, 0.0);
  nh_private_.param<double>("init_pos_z", initTargetPos_z_, 1.0);

 case TAKING_OFF: {
    geometry_msgs::PoseStamped takingoff_msg;
    takingoff_msg.header.stamp = ros::Time::now();
    takingoff_msg.pose.position.x = initTargetPos_x_;
    takingoff_msg.pose.position.y = initTargetPos_y_;
    takingoff_msg.pose.position.z = initTargetPos_z_;
    target_pose_pub_.publish(takingoff_msg);
    ros::spinOnce();
    break;
  }

 target_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>(
      "mavros/setpoint_position/local", 10);

3. MISSION_EXCUTION

当路径规划模块通过/planning/pos_cmd话题发布路径信息时，无人机进入任务执行状态，执行case：MISSION_EXCUTION

  quadcmdSub_ =
      nh_.subscribe("planning/pos_cmd", 1, &geometricCtrl::quadmsgCallback,
                    this, ros::TransportHints().tcpNoDelay());

 从/planning/pos_cmd话题获取规划路径的目标点，目标速度，目标加速度和航向角

void geometricCtrl::quadmsgCallback(
    const quadrotor_msgs::PositionCommand::ConstPtr &cmd) {
  node_state = MISSION_EXECUTION;
  reference_request_last_ = reference_request_now_;

  targetPos_prev_ = targetPos_;
  targetVel_prev_ = targetVel_;

  reference_request_now_ = ros::Time::now();
  cmdpos_time_last_ = reference_request_now_;

  reference_request_dt_ =
      (reference_request_now_ - reference_request_last_).toSec();

  targetPos_ =
      Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
  targetVel_ =
      Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
  targetAcc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y,
                               cmd->acceleration.z);
  targetJerk_ = Eigen::Vector3d::Zero();
  targetSnap_ = Eigen::Vector3d::Zero();
  mavYaw_ = double(cmd->yaw);
  // cmdBodyRate_[2] = cmd->yaw_dot;
}

 执行case：MISSION_EXCUTION。总体分为几个模块和步骤：

1. 计算机身速度

2. 发布参考位姿

3. 发布控制指令（xyz欧拉角和油门大小）

4. 添加历史点

5. 发布历史点

 case MISSION_EXECUTION:
  {

      std::cout << "mission execution  " << dtttt << std::endl;
      if (!feedthrough_enable_)
      computeBodyRateCmd(cmdBodyRate_, targetPos_, targetVel_, targetAcc_);
      pubReferencePose(targetPos_, q_des);
      pubRateCommands(cmdBodyRate_);
      appendPoseHistory();
      pubPoseHistory();
      ros::spinOnce();
    //  }
    break;
  }

1. 计算机身速度

ctrl_mode_ == ERROR_QUATERNION，进入非线性的无人机姿态控制器attcontroller()

void geometricCtrl::computeBodyRateCmd(Eigen::Vector4d &bodyrate_cmd,
                                       const Eigen::Vector3d &target_pos,
                                       const Eigen::Vector3d &target_vel,
                                       const Eigen::Vector3d &target_acc) {
  /// Compute BodyRate commands using differential flatness
  /// Controller based on Faessler 2017
  const Eigen::Vector3d a_ref = target_acc;
  if (velocity_yaw_) {
    mavYaw_ = getVelocityYaw(mavVel_);
  }
  const Eigen::Vector4d q_ref = acc2quaternion(a_ref - g_, mavYaw_);
  const Eigen::Matrix3d R_ref = quat2RotMatrix(q_ref);

  const Eigen::Vector3d pos_error = mavPos_ - target_pos;
  const Eigen::Vector3d vel_error = mavVel_ - target_vel;
  std::cout << "the position error is: " << pos_error(2) << std::endl;
  Eigen::Vector3d a_fb =
      Kpos_.asDiagonal() * pos_error +
      Kvel_.asDiagonal() * vel_error; // feedforward term for trajectory error
  if (a_fb.norm() > max_fb_acc_)
    a_fb = (max_fb_acc_ / a_fb.norm()) *
           a_fb; // Clip acceleration if reference is too large

  const Eigen::Vector3d a_rd =
      R_ref * D_.asDiagonal() * R_ref.transpose() * target_vel; // Rotor drag
  const Eigen::Vector3d a_des = a_fb + a_ref - a_rd - g_;

  q_des = acc2quaternion(a_des, mavYaw_);
 

  if (ctrl_mode_ == ERROR_GEOMETRIC) {
    bodyrate_cmd =
        geometric_attcontroller(q_des, a_des, mavAtt_); // Calculate BodyRate
  } else {
    bodyrate_cmd = attcontroller(q_des, a_des, mavAtt_); // Calculate BodyRate
  }
}

计算返回ratecmd。ratecmd(0),ratecmd(1),ratecmd(2)分别是x,y,z方向的欧拉角，ratecmd(3)为无人机油门大小

Eigen::Vector4d geometricCtrl::attcontroller(const Eigen::Vector4d &ref_att,
                                             const Eigen::Vector3d &ref_acc,
                                             Eigen::Vector4d &curr_att) {
  // Geometric attitude controller
  // Attitude error is defined as in Brescianini, Dario, Markus Hehn, and
  // Raffaello D'Andrea. Nonlinear quadrocopter attitude control: Technical
  // report. ETH Zurich, 2013.

  Eigen::Vector4d ratecmd;
  Eigen::Vector4d qe, q_inv, inverse;
  Eigen::Matrix3d rotmat;
  Eigen::Vector3d zb;

  inverse << 1.0, -1.0, -1.0, -1.0;
  q_inv = inverse.asDiagonal() * curr_att;



  qe = quatMultiplication(q_inv, ref_att);

  ratecmd(0) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(1);
  ratecmd(1) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(2);
  ratecmd(2) = (1.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(3);
  rotmat = quat2RotMatrix(mavAtt_);
  zb = rotmat.col(2);
  ratecmd(3) =
      std::max(0.0, std::min(1.0, norm_thrust_const_ * ref_acc.dot(zb) +
                                      norm_thrust_offset_)); // Calculate thrust

  return ratecmd;
}

 2. 发布参考位置和姿态

 referencePosePub_ =
      nh_.advertise<geometry_msgs::PoseStamped>("reference/pose", 1);

void geometricCtrl::pubReferencePose(const Eigen::Vector3d &target_position,
                                     const Eigen::Vector4d &target_attitude) {
  geometry_msgs::PoseStamped msg;

  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = "map";
  msg.pose.position.x = target_position(0);
  msg.pose.position.y = target_position(1);
  msg.pose.position.z = target_position(2);
  msg.pose.orientation.w = target_attitude(0);
  msg.pose.orientation.x = target_attitude(1);
  msg.pose.orientation.y = target_attitude(2);
  msg.pose.orientation.z = target_attitude(3);
  referencePosePub_.publish(msg);
}

3. 发布控制指令

发布到/mavros/setpoint_raw/attitude话题，输入给无人机飞控（FCU）,无人机获取信息，并做出相应相应。到此，无人机就完成了一次从获取路径到路径跟随执行的完整过程。

void geometricCtrl::pubRateCommands(const Eigen::Vector4d &cmd) {
  mavros_msgs::AttitudeTarget msg;
  double maxVelocity = 6.0;
  geometry_msgs::TwistStamped velocity_msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = "map";
  msg.body_rate.x = cmd(0);
  msg.body_rate.y = cmd(1);
  msg.body_rate.z = cmd(2);
  msg.type_mask = 128; // Ignore orientation messages
  msg.thrust = cmd(3);

  // Publish velocity cmd (PI controller)
  const Eigen::Vector3d pos_error = targetPos_ - mavPos_;
  const Eigen::Vector3d vel_error = targetVel_ - mavVel_;
  Eigen::Vector3d v_fb = 0.1 * Kpos_.asDiagonal() * pos_error + 0.1 * Kvel_.asDiagonal() * vel_error; // feedback term for trajectory error
 
  angularVelPub_.publish(msg);
  // velocity_msg.twist.linear.x = satfunc(velocity_msg.twist.linear.x, maxVelocity);    //控制输出的最大线速度，当参考速度大于设置的最大速度，取设置速度
  // velocity_msg.twist.linear.y = satfunc(velocity_msg.twist.linear.y, maxVelocity);
  // velocity_msg.twist.linear.z = satfunc(velocity_msg.twist.linear.z, 0.7);
//  target_velocity_pub_.publish(velocity_msg);
}

 angularVelPub_ =
      nh_.advertise<mavros_msgs::AttitudeTarget>("command/bodyrate_command", 1);        //mavros/setpoint_raw/attitude

 到此，无人机路径规划到路径跟随的控制模块就告一段落，后续再继续对该算法作详细解释补充， 如有不对的地方，欢迎大家指正解答，谢谢^0^！