【ROS&GAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器

【ROS&GAZEBO】多旋翼无人机仿真（一）——搭建仿真环境https://blog.csdn.net/qq_37680545/article/details/123185002
【ROS&GAZEBO】多旋翼无人机仿真（二）——基于rotors的仿真https://blog.csdn.net/qq_37680545/article/details/123213054
【ROS&GAZEBO】多旋翼无人机仿真（三）——自定义多旋翼模型https://blog.csdn.net/qq_37680545/article/details/123243313

【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理https://blog.csdn.net/qq_37680545/article/details/123310001
【ROS&GAZEBO】多旋翼无人机仿真（五）——位置控制器https://blog.csdn.net/qq_37680545/article/details/123390601

姿态角控制问题

这一篇我们介绍rotors中的姿态控制算法。

上篇说到，lee_position_controller_node.cpp中包含了位置和姿态控制器，位置控制器我们已经讲过了如何实现，这篇我们来讲姿态控制器是怎样实现的。

在介绍姿态控制之前，我们先直观的来看姿态控制如何实现。按照直观的理解，姿态控制器就是滚转、俯仰、偏航的控制，外环计算误差姿态角，内环计算误差角速度，就可以很好的实现控制。

但是这种方式有一个极大的漏洞——只适用于小角度。为什么这么说呢？因为欧拉角是和旋转顺序有关的，并且存在万向节死锁，这两个bug使得欧拉角控制只能适应小角度下的近似姿态控制。怎么来理解小角度假设呢？

我们来看下面两种旋转顺序：
1）欧拉角定义为3-1-2旋转，（即航向角-俯仰角-滚转角）

假设旋转角度为小角度：

 对上面的旋转矩阵小角度处理，得到如下结果：

2）欧拉角定义为3-2-1旋转，（即航向角-滚转角-俯仰角）

对上面的旋转矩阵小角度处理，得到如下结果：

可以看出，3-1-2和3-2-1两种旋转方式；基于小角度的旋转矩阵是相同的；同时，可以验证出，只要经过三次基本旋转（X、Y、Z轴各转动仅且一次），最终得到的方向余弦矩阵是一样的。但是这些所有结果都是在小角度条件下的。

若不在小角度条件下，不同旋转顺序导致的旋转矩阵结果不相同，这就导致姿态控制存在了问题，姿态控制必须要按旋转顺序进行控制才能回到初始姿态。例如按3-1-2顺序控制，需要先旋转俯仰角、再旋转滚转角、最后再旋转航向角才能回到正确的姿态。而对于飞行器来说，姿态控制频率都非常高，常见的ardupilot、px4等都在1KHz，若按顺序依次进行控制，可能飞机早已经炸机了。

那还有没有能够同时实现三维姿态控制的方法呢？一种就是上面说的小角度假设控制，飞机不进行较大角度的机动，弊端也很明显了，如果想玩花式特技怎么办呢，那结果必然是一次性的表演。。。，第二种方式就是采用四元数控制，这种方法我们留在后面再讲，第三种是几何控制，也就是今天我要介绍的一种控制方法——用姿态矩阵进行控制，目前作者已知就这三种控制方法，不知道会不会有其他更好的方法。

几何控制

几何控制器将多旋翼无人机的动力学全局表示在特殊欧几里德群SE(3)的构型流形上。  该方法已应用于李群上的完全驱动刚体动力学，有全局的渐近稳定性。几何控制器的整体结构如下：

平动动力学计算出总的推力和用于稳定平动动力学的轴向量，但是这样还差一个自由度航向还未计算出来，因此需要确定航向的轴向量，再根据航向、、确定组成一个正交轴系。

 怎么确定呢，采用叉乘公式，得到和、正交的：

 

这时我们已经获得了和轴了，但是此时轴和还不是正交的，同样的道理，我们再将轴投影到正交轴上：

 再重新组合的正交轴系就是我们的期望姿态矩阵，

如下图所示：

 计算误差姿态：

 根据旋转矩阵计算误差角度公式如下：

 

代表的意思是从反对称矩阵转换成向量。误差角速度计算如下：

有了误差姿态和角速度，就可以很容易的计算期望力矩了。

期望力矩由误差姿态、误差角速度、和当前的力矩组成。形式上和期望加速度差不多。

代码：

有了上面的理论分析，现在再回到rotors的代码就很好理解了。对代码的说明已经放在注释里面了。

// Implementation from the T. Lee et al. paper
// Control of complex maneuvers for a quadrotor UAV using geometric methods on SE(3)
void LeePositionController::ComputeDesiredAngularAcc(const Eigen::Vector3d& acceleration,
                                                     Eigen::Vector3d* angular_acceleration) const {
  assert(angular_acceleration);

  Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();


  //========================================================================
  //计算期望姿态矩阵
  // Get the desired rotation matrix.
  Eigen::Vector3d b1_des;
  double yaw = command_trajectory_.getYaw();
  b1_des << cos(yaw), sin(yaw), 0;

  Eigen::Vector3d b3_des;
  b3_des = -acceleration / acceleration.norm();

  Eigen::Vector3d b2_des;
  b2_des = b3_des.cross(b1_des);
  b2_des.normalize();

  Eigen::Matrix3d R_des;
  R_des.col(0) = b2_des.cross(b3_des);
  R_des.col(1) = b2_des;
  R_des.col(2) = b3_des;

  //========================================================================
  //计算误差角度
  // Angle error according to lee et al.
  Eigen::Matrix3d angle_error_matrix = 0.5 * (R_des.transpose() * R - R.transpose() * R_des);
  Eigen::Vector3d angle_error;
  vectorFromSkewMatrix(angle_error_matrix, &angle_error);

  //========================================================================
  //计算误差角速度
  // TODO(burrimi) include angular rate references at some point.
  Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero());
  angular_rate_des[2] = command_trajectory_.getYawRate();

  Eigen::Vector3d angular_rate_error = odometry_.angular_velocity - R_des.transpose() * R * angular_rate_des;

  //========================================================================
  //计算期望角加速度
  *angular_acceleration = -1 * angle_error.cwiseProduct(normalized_attitude_gain_)
                           - angular_rate_error.cwiseProduct(normalized_angular_rate_gain_)
                           + odometry_.angular_velocity.cross(odometry_.angular_velocity); // we don't need the inertia matrix here
}

对rotors的几何控制就分析到这里。

感兴趣的朋友可以点个赞,关注微信公众号交流：Reed UAV