[学习记录]realsence d455 +vins-fusion+px4+ego_planner下无人机的悬停与控制

写在前面：持续更新修改......

my env：ubuntu20

my pixhawk：2.4.8

my px4 firmware：1.9.0 stablepx4fmu-v2-1.6.0.px4v

经济型科研方案（有个较好的网络环境）：

                     无人机端：250机架——38块钱或者可以更便宜、三叶桨——也很便宜、飞控——2.4.8——五百多、机载板子——jetsonnano（可以选择更便宜但是wifi模块还是要强）、电池也很便宜。

                     地面站：电脑一台

无人机端的机载板子主要用于通信，传输数据，全部算法都在电脑上跑（手动滑稽）。

关于带宽问题，使用压缩图即可

关于网络出错等问题，几乎都可以通过修改dns与/etc/hosts解决。

-------------------------------------------------------------分割符-----------------------------------------------------

整体步骤整理，整体的步骤来自（注意看里面的readme.txt）：https://github.com/ZJU-FAST-Lab/Fast-Drone-250https://github.com/ZJU-FAST-Lab/Fast-Drone-250

Step1: 安装 realsence2 驱动，把ros自带的和源码都安装了，并检测和imu数据与camera数据。

参考GitHub - IntelRealSense/realsense-ros: Intel(R) RealSense(TM) ROS Wrapper for D400 series, SR300 Camera and T265 Tracking ModuleIntel(R) RealSense(TM) ROS Wrapper for D400 series, SR300 Camera and T265 Tracking Module - GitHub - IntelRealSense/realsense-ros: Intel(R) RealSense(TM) ROS Wrapper for D400 series, SR300 Camera and T265 Tracking Modulehttps://github.com/IntelRealSense/realsense-ros

Step2：安装vins-fustion，并测试结果

参考GitHub - HKUST-Aerial-Robotics/VINS-Fusion: An optimization-based multi-sensor state estimatorhttps://github.com/HKUST-Aerial-Robotics/VINS-Fusion

如果遇到一些问题还能参考：＜解决方法＞ubuntu20+ros+opencv4运行vinfusion_fikimi的博客-CSDN博客

Step3：仿真环境安装，并解决px4 和机载板卡与笔记本的实机通信问题，连接方式等

ubuntu18+jetson nano +px4+ros ＜——＞QGC+ubuntu20+ros(关于仿真和实物运行的持续记录心得)_fikimi的博客-CSDN博客

notice：关于在px4 的sd卡写入marlink 命令或者QGC的mavlink控制台修改帧率问题：通过telem2 连接，硬件写/dev/ttyS2,通过telem1 连接，写/dev/ttyS1,通过usb 则写/dev/ttyACM0

mavlink stream -d /dev/ttyS2 -s ATTITUDE_QUATERNION -r 200
mavlink stream -d /dev/ttyS2 -s HIGHRES_IMU -r 200

Step4：（这里可以写个shell，省得一条一条按，关于shell运行ssh的方法可以查expect的用法）

drone0-terminal1：roslaunch px4_realsense_bridge mypx4.launch

drone0-terminal2：roslaunch realsense2_camera rs_camera.launch

ubuntu-terminal1：roscore

ubuntu-terminal2：roslaunch vins vins_fusion.launch

ubuntu-terminal3：roslaunch vins rviz.launch

ubuntu-terminal4：roslaunch px4ctrl run_ctrl.launch

ubuntu-terminal5：sh shfiles/takeoff.sh

其中，在遥控器（飞行模式上是通道5,都设置为自稳模式）上将模式转为Autohover(通道5)模式，，通道6拨动一下（根据遥控器可以进行修改），然后将油门推到中位，再在ubuntu-terminal5：中运行sh shfiles/takeoff.sh。

注意：Fast-lab的Fast-drone-250中包px4ctrl的readme.txt中需要详细阅读

-------------------------------------------------------------分割符-----------------------------------------------------------

（控制方法：除了使用px4ctrl，还可以借鉴px4 vio的官方的方法（或者把官方vio中的odom用vins-fusion替代注意话题的对应，tf树的转换）rosrun rqt_tf_tree rqt_tf_tree ）

controller.cpp部分代码注释（有误可以指出并交流）：       

//推力估计模型，计算thr2acc_
//输入参数：估计的加速度
bool 
LinearControl::estimateThrustModel(
    const Eigen::Vector3d &est_a,
    const Parameter_t ¶m)
{
  ros::Time t_now = ros::Time::now();
  while (timed_thrust_.size() >= 1)
  {
    // Choose data before 35~45ms ago
    std::pair t_t = timed_thrust_.front();
    double time_passed = (t_now - t_t.first).toSec();
    if (time_passed > 0.045) // 45ms
    {
      // printf("continue, time_passed=%f\n", time_passed);
      timed_thrust_.pop();
      continue;
    }
    if (time_passed < 0.035) // 35ms
    {
      // printf("skip, time_passed=%f\n", time_passed);
      return false;
    }

    /***********************************************************/
    /* Recursive least squares algorithm with vanishing memory */
    /***********************************************************/
    double thr = t_t.second;//取出后就弹出
    timed_thrust_.pop();
    //非精准推理模型:thr2acc = est_a(2)/thr
    /***********************************/
    /* Model: est_a(2) = thr2acc_ * thr */
    /***********************************/
    //kLQR，you can print it to see
    double gamma = 1 / (rho2_ + thr * P_ * thr);
    double K = gamma * P_ * thr;
    thr2acc_ = thr2acc_ + K * (est_a(2) - thr * thr2acc_);
    P_ = (1 - K * thr) * P_ / rho2_;

    //printf("%6.3f,%6.3f,%6.3f,%6.3f\n", thr2acc_, gamma, K, P_);
    //fflush(stdout);
    // debug_msg_.thr2acc = thr2acc_;
    return true;
  }
  return false;
}
/* 
  compute u.thrust and u.q, controller gains and other parameters are in param_ 
  u就是q和thrust等等，根据输入的odom和imu信息计算u
  debug
*/
quadrotor_msgs::Px4ctrlDebug
LinearControl::calculateControl(const Desired_State_t &des,
    const Odom_Data_t &odom,
    const Imu_Data_t &imu, 
    Controller_Output_t &u)
{
  /* WRITE YOUR CODE HERE */
      //compute disired acceleration
      Eigen::Vector3d des_acc(0.0, 0.0, 0.0);
      Eigen::Vector3d Kp,Kv;//增益，pid的p，比例增益
      Kp << param_.gain.Kp0, param_.gain.Kp1, param_.gain.Kp2;//先把参数中的增益加进去
      Kv << param_.gain.Kv0, param_.gain.Kv1, param_.gain.Kv2;
      //输入偏差x常数
      des_acc = des.a + Kv.asDiagonal() * (des.v - odom.v) + Kp.asDiagonal() * (des.p - odom.p);//asDiagonal对角化伪表达
      //增量
      des_acc += Eigen::Vector3d(0,0,param_.gra);
      //推力计算转换，z方向加速度占比计算
      u.thrust = computeDesiredCollectiveThrustSignal(des_acc);
      double roll,pitch,yaw,yaw_imu;
      double yaw_odom = fromQuaternion2yaw(odom.q);//根据四元数计算出欧拉角，ros的odom消息机制为ZYX，无人机的是ZXY
      double sin = std::sin(yaw_odom);
      double cos = std::cos(yaw_odom);
      //跟着把roll yaw pitch 算出来
      roll = (des_acc(0) * sin - des_acc(1) * cos )/ param_.gra;
      pitch = (des_acc(0) * cos + des_acc(1) * sin )/ param_.gra;
      //yaw = fromQuaternion2yaw(des.q);
      yaw_imu = fromQuaternion2yaw(imu.q);//一个道理，把imu的yaw算出来
      // Eigen::Quaterniond q = Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())
      //   * Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX())
      //   * Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY());

      //旋转向量（AngleAxisd）初始化，des为旋转角，Eigen::Vector3d::UnitZ()表示绕z轴转.Z-Y-X
      //odom.q 接受的话题是Z-Y-X 
      //odom.q.inverse()表示 odom.q的共轭，并且单位化
      Eigen::Quaterniond q = Eigen::AngleAxisd(des.yaw,Eigen::Vector3d::UnitZ())
        * Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());
      u.q = imu.q * odom.q.inverse() * q;// Align with FCU frame

  /* WRITE YOUR CODE HERE */
  //used for debug。可以自己添加
   debug_msg_.des_p_x = des.p(0);
   debug_msg_.des_p_y = des.p(1);
   debug_msg_.des_p_z = des.p(2);
 
  debug_msg_.des_v_x = des.v(0);
  debug_msg_.des_v_y = des.v(1);
  debug_msg_.des_v_z = des.v(2);
  
  debug_msg_.des_a_x = des_acc(0);
  debug_msg_.des_a_y = des_acc(1);
  debug_msg_.des_a_z = des_acc(2);
  
  debug_msg_.des_q_x = u.q.x();
  debug_msg_.des_q_y = u.q.y();
  debug_msg_.des_q_z = u.q.z();
  debug_msg_.des_q_w = u.q.w();
  
  debug_msg_.des_thr = u.thrust;
  
  // Used for thrust-accel mapping estimation 推力加速度映射估计
  timed_thrust_.push(std::pair(ros::Time::now(), u.thrust));
  while (timed_thrust_.size() > 100)
  {
    timed_thrust_.pop();
  }
  return debug_msg_;
}

-----------------------------------------------------------------分割符--------------------------------------------------------