devbox
代码探险家
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：代码探险家 | 2024-07-16 18:37:02

赞

踩

geometric controller

引言

本篇博文意在整体梳理一下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

  1. enum FlightState {
  2.     WAITING_FOR_HOME_POSE,
  3.     TAKING_OFF,
  4.     MISSION_EXECUTION,
  5.     MISSION_HOLD,
  6.     LANDING,
  7.     LANDED
  8.   } node_state;

飞行状态详解

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

  1. void geometricCtrl::cmdloopCallback(const ros::TimerEvent &event) { 
  2.   switch (node_state) {
  3.       case WAITING_FOR_HOME_POSE:   
  4.       case TAKING_OFF:
  5.       case MISSION_EXECUTION:
  6.       case MISSION_HOLD:
  7.       case LANDING:
  8.       case LANDED:
  9.   }
  10. }

1. WAITING_FOR_HOME_POSE

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

  1.  mavposeSub_ = nh_.subscribe("mavros/local_position/pose", 1,
  2.                               &geometricCtrl::mavposeCallback, this,
  3.                               ros::TransportHints().tcpNoDelay());
  1. void geometricCtrl::mavposeCallback(const geometry_msgs::PoseStamped &msg) {
  2.   if (!received_home_pose) {
  3.     received_home_pose = true;
  4.     home_pose_ = msg.pose;
  5.     ROS_INFO_STREAM("Home pose initialized to: " << home_pose_);
  6.   }
  7.   mavPos_ = toEigen(msg.pose.position);
  8.   mavAtt_(0) = msg.pose.orientation.w;
  9.   mavAtt_(1) = msg.pose.orientation.x;
  10.   mavAtt_(2) = msg.pose.orientation.y;
  11.   mavAtt_(3) = msg.pose.orientation.z;
  12. }

 执行case：WAITING_FOR_HOME_POSE

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

  1. case WAITING_FOR_HOME_POSE:
  2.     waitForPredicate(&received_home_pose, "Waiting for home pose...");
  3.     ROS_INFO("Got pose! Drone Ready to be armed.");
  4.     // node_state = MISSION_EXECUTION;
  5.     node_state = TAKING_OFF;
  6.     break;
  1. void waitForPredicate(const T *pred, const std::string &msg,
  2.                         double hz = 2.0) {
  3.     ros::Rate pause(hz);
  4.     ROS_INFO_STREAM(msg);
  5.     while (ros::ok() && !(*pred)) {
  6.       ros::spinOnce();
  7.       pause.sleep();
  8.     }
  9.   }

2. TAKING_OFF

执行case：TAKING_OFF

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

  1.   nh_private_.param<double>("init_pos_x", initTargetPos_x_, 0.0);
  2.   nh_private_.param<double>("init_pos_y", initTargetPos_y_, 0.0);
  3.   nh_private_.param<double>("init_pos_z", initTargetPos_z_, 1.0);
  1.  case TAKING_OFF: {
  2.     geometry_msgs::PoseStamped takingoff_msg;
  3.     takingoff_msg.header.stamp = ros::Time::now();
  4.     takingoff_msg.pose.position.x = initTargetPos_x_;
  5.     takingoff_msg.pose.position.y = initTargetPos_y_;
  6.     takingoff_msg.pose.position.z = initTargetPos_z_;
  7.     target_pose_pub_.publish(takingoff_msg);
  8.     ros::spinOnce();
  9.     break;
  10.   }
  1.  target_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>(
  2.       "mavros/setpoint_position/local", 10);

3. MISSION_EXCUTION

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

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

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

  1. void geometricCtrl::quadmsgCallback(
  2.     const quadrotor_msgs::PositionCommand::ConstPtr &cmd) {
  3.   node_state = MISSION_EXECUTION;
  4.   reference_request_last_ = reference_request_now_;
  5.  
  6.   targetPos_prev_ = targetPos_;
  7.   targetVel_prev_ = targetVel_;
  8.  
  9.   reference_request_now_ = ros::Time::now();
  10.   cmdpos_time_last_ = reference_request_now_;
  11.  
  12.   reference_request_dt_ =
  13.       (reference_request_now_ - reference_request_last_).toSec();
  14.  
  15.   targetPos_ =
  16.       Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
  17.   targetVel_ =
  18.       Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
  19.   targetAcc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y,
  20.                                cmd->acceleration.z);
  21.   targetJerk_ = Eigen::Vector3d::Zero();
  22.   targetSnap_ = Eigen::Vector3d::Zero();
  23.   mavYaw_ = double(cmd->yaw);
  24.   // cmdBodyRate_[2] = cmd->yaw_dot;
  25. }

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

1. 计算机身速度

2. 发布参考位姿

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

4. 添加历史点

5. 发布历史点

  1.  case MISSION_EXECUTION:
  2.   {
  3.  
  4.       std::cout << "mission execution  " << dtttt << std::endl;
  5.       if (!feedthrough_enable_)
  6.       computeBodyRateCmd(cmdBodyRate_, targetPos_, targetVel_, targetAcc_);
  7.       pubReferencePose(targetPos_, q_des);
  8.       pubRateCommands(cmdBodyRate_);
  9.       appendPoseHistory();
  10.       pubPoseHistory();
  11.       ros::spinOnce();
  12.     //  }
  13.     break;
  14.   }

1. 计算机身速度

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

  1. void geometricCtrl::computeBodyRateCmd(Eigen::Vector4d &bodyrate_cmd,
  2.                                        const Eigen::Vector3d &target_pos,
  3.                                        const Eigen::Vector3d &target_vel,
  4.                                        const Eigen::Vector3d &target_acc) {
  5.   /// Compute BodyRate commands using differential flatness
  6.   /// Controller based on Faessler 2017
  7.   const Eigen::Vector3d a_ref = target_acc;
  8.   if (velocity_yaw_) {
  9.     mavYaw_ = getVelocityYaw(mavVel_);
  10.   }
  11.   const Eigen::Vector4d q_ref = acc2quaternion(a_ref - g_, mavYaw_);
  12.   const Eigen::Matrix3d R_ref = quat2RotMatrix(q_ref);
  13.  
  14.   const Eigen::Vector3d pos_error = mavPos_ - target_pos;
  15.   const Eigen::Vector3d vel_error = mavVel_ - target_vel;
  16.   std::cout << "the position error is: " << pos_error(2) << std::endl;
  17.   Eigen::Vector3d a_fb =
  18.       Kpos_.asDiagonal() * pos_error +
  19.       Kvel_.asDiagonal() * vel_error; // feedforward term for trajectory error
  20.   if (a_fb.norm() > max_fb_acc_)
  21.     a_fb = (max_fb_acc_ / a_fb.norm()) *
  22.            a_fb; // Clip acceleration if reference is too large
  23.  
  24.   const Eigen::Vector3d a_rd =
  25.       R_ref * D_.asDiagonal() * R_ref.transpose() * target_vel; // Rotor drag
  26.   const Eigen::Vector3d a_des = a_fb + a_ref - a_rd - g_;
  27.  
  28.   q_des = acc2quaternion(a_des, mavYaw_);
  29.  
  30.  
  31.   if (ctrl_mode_ == ERROR_GEOMETRIC) {
  32.     bodyrate_cmd =
  33.         geometric_attcontroller(q_des, a_des, mavAtt_); // Calculate BodyRate
  34.   } else {
  35.     bodyrate_cmd = attcontroller(q_des, a_des, mavAtt_); // Calculate BodyRate
  36.   }
  37. }

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

  1. Eigen::Vector4d geometricCtrl::attcontroller(const Eigen::Vector4d &ref_att,
  2.                                              const Eigen::Vector3d &ref_acc,
  3.                                              Eigen::Vector4d &curr_att) {
  4.   // Geometric attitude controller
  5.   // Attitude error is defined as in Brescianini, Dario, Markus Hehn, and
  6.   // Raffaello D'Andrea. Nonlinear quadrocopter attitude control: Technical
  7.   // report. ETH Zurich, 2013.
  8.  
  9.   Eigen::Vector4d ratecmd;
  10.   Eigen::Vector4d qe, q_inv, inverse;
  11.   Eigen::Matrix3d rotmat;
  12.   Eigen::Vector3d zb;
  13.  
  14.   inverse << 1.0, -1.0, -1.0, -1.0;
  15.   q_inv = inverse.asDiagonal() * curr_att;
  16.  
  17.  
  18.  
  19.   qe = quatMultiplication(q_inv, ref_att);
  20.  
  21.   ratecmd(0) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(1);
  22.   ratecmd(1) = (2.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(2);
  23.   ratecmd(2) = (1.0 / attctrl_tau_) * std::copysign(1.0, qe(0)) * qe(3);
  24.   rotmat = quat2RotMatrix(mavAtt_);
  25.   zb = rotmat.col(2);
  26.   ratecmd(3) =
  27.       std::max(0.0, std::min(1.0, norm_thrust_const_ * ref_acc.dot(zb) +
  28.                                       norm_thrust_offset_)); // Calculate thrust
  29.  
  30.   return ratecmd;
  31. }

 2. 发布参考位置和姿态

  1.  referencePosePub_ =
  2.       nh_.advertise<geometry_msgs::PoseStamped>("reference/pose", 1);
  1. void geometricCtrl::pubReferencePose(const Eigen::Vector3d &target_position,
  2.                                      const Eigen::Vector4d &target_attitude) {
  3.   geometry_msgs::PoseStamped msg;
  4.  
  5.   msg.header.stamp = ros::Time::now();
  6.   msg.header.frame_id = "map";
  7.   msg.pose.position.x = target_position(0);
  8.   msg.pose.position.y = target_position(1);
  9.   msg.pose.position.z = target_position(2);
  10.   msg.pose.orientation.w = target_attitude(0);
  11.   msg.pose.orientation.x = target_attitude(1);
  12.   msg.pose.orientation.y = target_attitude(2);
  13.   msg.pose.orientation.z = target_attitude(3);
  14.   referencePosePub_.publish(msg);
  15. }

3. 发布控制指令

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

  1. void geometricCtrl::pubRateCommands(const Eigen::Vector4d &cmd) {
  2.   mavros_msgs::AttitudeTarget msg;
  3.   double maxVelocity = 6.0;
  4.   geometry_msgs::TwistStamped velocity_msg;
  5.   msg.header.stamp = ros::Time::now();
  6.   msg.header.frame_id = "map";
  7.   msg.body_rate.x = cmd(0);
  8.   msg.body_rate.y = cmd(1);
  9.   msg.body_rate.z = cmd(2);
  10.   msg.type_mask = 128; // Ignore orientation messages
  11.   msg.thrust = cmd(3);
  12.  
  13.   // Publish velocity cmd (PI controller)
  14.   const Eigen::Vector3d pos_error = targetPos_ - mavPos_;
  15.   const Eigen::Vector3d vel_error = targetVel_ - mavVel_;
  16.   Eigen::Vector3d v_fb = 0.1 * Kpos_.asDiagonal() * pos_error + 0.1 * Kvel_.asDiagonal() * vel_error; // feedback term for trajectory error
  17.  
  18.   angularVelPub_.publish(msg);
  19.   // velocity_msg.twist.linear.x = satfunc(velocity_msg.twist.linear.x, maxVelocity);    //控制输出的最大线速度，当参考速度大于设置的最大速度，取设置速度
  20.   // velocity_msg.twist.linear.y = satfunc(velocity_msg.twist.linear.y, maxVelocity);
  21.   // velocity_msg.twist.linear.z = satfunc(velocity_msg.twist.linear.z, 0.7);
  22. //  target_velocity_pub_.publish(velocity_msg);
  23. }
  1.  angularVelPub_ =
  2.       nh_.advertise<mavros_msgs::AttitudeTarget>("command/bodyrate_command", 1);        //mavros/setpoint_raw/attitude

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/代码探险家/article/detail/835647
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号