px4ctrl里calculateControl的代码解析

px4ctrl的代码里将目标加速度转成飞控的rpy的calculateControl代码

/* 
  compute u.thrust and u.q, controller gains and other parameters are in param_ 
*/
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;
      Kp << param_.gain.Kp0, param_.gain.Kp1, param_.gain.Kp2;
      Kv << param_.gain.Kv0, param_.gain.Kv1, param_.gain.Kv2;
      des_acc = des.a + Kv.asDiagonal() * (des.v - odom.v) + Kp.asDiagonal() * (des.p - odom.p);
      des_acc += Eigen::Vector3d(0,0,param_.gra);

      u.thrust = computeDesiredCollectiveThrustSignal(des_acc);
      double roll,pitch,yaw,yaw_imu;
      double yaw_odom = fromQuaternion2yaw(odom.q);
      double sin = std::sin(yaw_odom);
      double cos = std::cos(yaw_odom);
      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);
      // Eigen::Quaterniond q = Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())
      //   * Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX())
      //   * Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY());
      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;


  /* 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, double>(ros::Time::now(), u.thrust));
  while (timed_thrust_.size() > 100)
  {
    timed_thrust_.pop();
  }
  return debug_msg_;
}

第一部分的代码：

    Eigen::Vector3d des_acc(0.0, 0.0, 0.0);
    Eigen::Vector3d Kp,Kv;
    
//    读取参数
    Kp << param_.gain.Kp0, param_.gain.Kp1, param_.gain.Kp2;
    Kv << param_.gain.Kv0, param_.gain.Kv1, param_.gain.Kv2;
    
    // 依照目标位置和速度，对比当前位置和速度，调整目标直线加速度（距离远则加速度增加）
    des_acc = des.a + Kv.asDiagonal() * (des.v - odom.v) + Kp.asDiagonal() * (des.p - odom.p);
    des_acc += Eigen::Vector3d(0,0,param_.gra);
    // 油门的具体算法这里就不讨论了
    u.thrust = computeDesiredCollectiveThrustSignal(des_acc);
    

computeDesiredCollectiveThrustSignal 的解析请看

px4ctrl里estimateThrustModel代码详解-CSDN博客

第二部分的代码：本文重点讨论的部分

    double roll,pitch,yaw,yaw_imu;
    double yaw_odom = fromQuaternion2yaw(odom.q);//calculateControl
    double sin = std::sin(yaw_odom);
    double cos = std::cos(yaw_odom);
    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);//calculateControl
    // Eigen::Quaterniond q = Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())
    //   * Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX())
    //   * Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY());
    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;

这里要先介绍下三个坐标系：

ROS坐标系（也就是odom坐标系）：x向前，y向左，z向上（roll增加向左，pitch向上，yaw向左）

机体坐标系：x向前，y向右，z向下（roll增加向右，pitch向下，yaw向右）。

Eigen坐标系：

这个不太清楚，使用程序里的代码

Eigen::Quaterniond q = Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())
                       * Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())
                       * Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());

和 Quaternions - Visualisation 对照来看

是（roll增加向右，pitch向下，yaw向左），也就是说yaw和ROS的相同，roll和pitch的和机体坐标系的相同。

------------------------------------------------------------------------------------------------------

有了坐标系的知识后，以下要做的步骤是：

1. 将ROS坐标系的X，Y轴的加速度acc(0)和acc(1)转换成机体坐标系的roll,pitch

2. 将ROS坐标系的yaw和机体坐标系的roll，pitch带入

Eigen::Quaterniond q = Eigen::AngleAxisd(des.yaw,Eigen::Vector3d::UnitZ())
                       * Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())
                       * Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());

计算在ROS（odom）坐标系下的目标姿态

3. 使用 u.q = imu.q * odom.q.inverse() * q;的公式，计算将imu的姿态（飞控当前的姿态）转换成目标姿态。发给飞控。

--------------------------------------------------------------------------------------------

首先我们先看一下如何将ROS坐标系的X，Y轴的加速度acc(0)和acc(1)转换成机体坐标系的roll,pitch.

注：这里的acc和des_acc是相同的。

这里先了解下，对于机体坐标而言，roll增加向右，pitch增加向前，可以参考以下的网址

https://arklab.tw/codrone-lite-pro_arduino_getting-started_-part-3/

以ROS的yaw角在0-90度为例，acc(0) 为正时，提供一个向前和向右的力，

向右：最后roll是正值，此时 sin 是正值，所以 roll = acc(0) * sin

向前：最后pitch是正值，此时cos 是正值，所以 pitch = acc(0) * cos

acc(1) 为正时，提供一个向前和向左的力，所以

向左：最后roll是负值，此时 cos 是正值，所以 roll = - acc(1) * cos

向前：最后pitch是正值，此时sin 是正值，所以 pitch = acc(1) * sin

sin和cos是为了把x方向和y方向的力，分解成机体坐标系里朝前和朝右(机体坐标系y轴朝右)的力

所以合并后的公式就是

roll = (des_acc(0) * sin - des_acc(1) * cos )/ param_.gra;
pitch = (des_acc(0) * cos + des_acc(1) * sin )/ param_.gra;

但这里为什么要除以param_.gra呢？我问了chatgpt得到的公式是

可以看到，最后将直线加速度转成roll和pitch都要除以g

这里画一下图帮助理解

可以试着依照上述步骤验证其它三个象限，都会得到同样的公式，或是尝试解析下chatgpt里说的公式。

----------------------------------------------------------------

2. 将ROS坐标系的yaw和机体坐标系的roll，pitch带入

Eigen::Quaterniond q = Eigen::AngleAxisd(des.yaw,Eigen::Vector3d::UnitZ())
                       * Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())
                       * Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());

计算在ROS（odom）坐标系下的目标姿态

由于刚刚说了Eigen的坐标系yaw的方向和ROS相同，roll和pitch和机体坐标系相同，所以我们吧des.yaw和刚刚计算的roll和pitch带入。得到的就是在odom坐标系下的目标位姿态。

可以想象下，首先飞机朝ROS坐标系的前方，依照ROS的坐标系旋转yaw角后，依照机体坐标系的roll和pitch旋转机体，得到的就是在ROS坐标系下的目标姿态。

----------------------------------------------------------------

3. 使用 u.q = imu.q * odom.q.inverse() * q;的公式，计算将imu的姿态（飞控当前的姿态）转换成目标姿态。发给飞控。

这段代码可以这样理解 T_imu_target =  T_imu_world * T_world_odom * T_odom_target

这里的T_imu_world的意思是从世界坐标转到imu坐标的变换，由于这里只有姿态，所以就是imu.q,其余同理。

首先imu和odom开机的原点就是world，也就是世界坐标的原点，imu.q就是当前飞控的位姿对应初始位姿T_imu_world。 同理 odom.q = T_odom_world，而odom.q.inverse() 就是T_imu_world。q是目标姿态和当前odom姿态的变换，就是T_odom_target。

所以最后计算的结果就是目标q到imu（飞控）的姿态变换，把这个q和油门值传给飞控，飞控就能推导出四个操作参数 roll, pitch, yaw，油门的值，和遥杆的四个通道对应。

----------------------------------------------------------------

后面都是debug的代码就不解释了