【ROS&GAZEBO】多旋翼无人机仿真（一）——搭建仿真环境
【ROS&GAZEBO】多旋翼无人机仿真（二）——基于rotors的仿真
【ROS&GAZEBO】多旋翼无人机仿真（三）——自定义多旋翼模型
【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理
【ROS&GAZEBO】多旋翼无人机仿真（五）——位置控制器
【ROS&GAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器
【ROS&GAZEBO】多旋翼无人机仿真（七）——四元数姿态控制
【ROS&GAZEBO】多旋翼无人机仿真（八）——手把手编写四元数姿态控制器

前言
因为一些乱七八糟的事情(ˉ▽ˉ；)…，大概有三周没有更新了，这周终于有时间来更新一下四元数姿态控制了，o(￣▽￣)ブ。上一篇讲了四元数控制的原理，详情请移步此处，
废话不多说，这一篇我们来编写四元数控制器，将其应用于rotors中。

改写rotors姿态控制器
为了省事，本人就没有去从头开始写姿态控制器插件，而是在rotors的姿态控制器上进行修改。当然感兴趣的朋友可以仿造rotors插件的写法，从零开始写（就是会比较麻烦，还可能会面对ros和gazebo中杂七杂八的bug）。

下面我们正式开始，先打开lee_position_controller.cpp文件，（如果对rotors的层次不理解的话，建议回到我前面的文章学习【ROS&GAZEBO】多旋翼无人机仿真（四）——探索控制器原理）

找到其中的CalculateRotorVelocities函数，这里就是最核心的位置和姿态控制代码。这里我也贴出来了，并且用中文进行注释了，方便大家学习：

void LeePositionController::CalculateRotorVelocities(Eigen::VectorXd* rotor_velocities) const {
  assert(rotor_velocities);
  assert(initialized_params_);

  rotor_velocities->resize(vehicle_parameters_.rotor_configuration_.rotors.size());
  // Return 0 velocities on all rotors, until the first command is received.
  if (!controller_active_) {
    *rotor_velocities = Eigen::VectorXd::Zero(rotor_velocities->rows());
    return;
  }

  // 计算期望加速度
  Eigen::Vector3d acceleration;
  ComputeDesiredAcceleration(&acceleration);

  // 计算期望角加速度
  Eigen::Vector3d angular_acceleration;
  ComputeDesiredAngularAcc(acceleration, &angular_acceleration);

  // 映射油门到机体z轴
  double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));

  // 组合角加速度和油门输出
  Eigen::Vector4d angular_acceleration_thrust;
  angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
  angular_acceleration_thrust(3) = thrust;

  // angular_acc_to_rotor_velocities_： 控制分配矩阵
  *rotor_velocities = angular_acc_to_rotor_velocities_ * angular_acceleration_thrust;
  *rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
  *rotor_velocities = rotor_velocities->cwiseSqrt();
}

而姿态控制器在此次：ComputeDesiredAngularAcc，所以我们要改掉这里，这里我的做法是复制ComputeDesiredAngularAcc这个函数，将其重命名为：ComputeDesiredAngularAcc_Q，接口和原生的一样，这也方便我们去修改。

void LeePositionController::ComputeDesiredAngularAcc_Q(const Eigen::Vector3d& acceleration,
                                                     Eigen::Vector3d* angular_acceleration) const {
}

接下来，进入主题：

这里首先模仿rotors的写法，做个断言，判断一下angular_acceleration有没有：

  assert(angular_acceleration);

下面获取当前的四元数和期望的航向角：

  // 获取当前四元数
  Eigen::Quaterniond quat = odometry_.orientation;

  // 获取期望航向角
  double yaw_d = command_trajectory_.getYaw();

然后获取期望的加速度，需要注意的一点是：由位置控制器计算期望加速度的时候是误差-期望，和我们平常的逻辑是反的，因此需要取反：

  // 归一化加速度
  Eigen::Vector3d accel_norm;

  // 由于计算期望加速度是误差-期望，因此，这里需要取反
  accel_norm = -acceleration / acceleration.norm();

然后计算期望的滚转和俯仰角，我这里用了简化的方法进行计算，需要注意的是俯仰滚转和XY位置的对应关系：

  // 简化的方法计算期望滚转和俯仰
  double pitch_d = -accel_norm.x() / acceleration.z();
  double roll_d = accel_norm.y() / acceleration.z();

然后将期望的角度转化为期望四元数：

  Eigen::Quaterniond quat_d = Eigen::AngleAxisd(yaw_d, Eigen::Vector3d::UnitZ()) *
                              Eigen::AngleAxisd(pitch_d, Eigen::Vector3d::UnitY()) *
                              Eigen::AngleAxisd(roll_d, Eigen::Vector3d::UnitX());

这一步可能会比较难理解，这里计算过程用的是轴角的方法，具体的轴角是什么大家也可以移步至我前面的文章，先不管具体的细节，反正通过这一步得到了期望四元数。

归一化一下期望四元数：

  quat_d.normalize();

如果看了我上一篇文章，下面就很好理解了，看一下具体过程就行了，这里的k是四元数的增益，根据实际情况进行调节：

  // 四元数误差增益
  double k = 4.0;

  // 计算四元数误差
  Eigen::Quaterniond quat_err = quat_d * quat.inverse();

  // 计算期望角速度
  Eigen::Vector3d angle_error = k * sgn(quat_err.w()) * quat_err.vec();

这里是角速度误差计算期望角加速度的步骤，用了最简单的误差，想得到更好的效果，可以换成PID等方法，这里留给大家自由发挥了。

  // 计算角速度误差
  Eigen::Vector3d angular_rate_error = angle_error - odometry_.angular_velocity;

  // 计算期望角加速度
  *angular_acceleration = angular_rate_error.cwiseProduct(normalized_angular_rate_gain_);

整个过程就完成了，具体代码如下：

void LeePositionController::ComputeDesiredAngularAcc_Q(const Eigen::Vector3d& acceleration,
                                                     Eigen::Vector3d* angular_acceleration) const {
  assert(angular_acceleration);

  // 获取当前四元数
  Eigen::Quaterniond quat = odometry_.orientation;

  // 获取期望航向角
  double yaw_d = command_trajectory_.getYaw();

  // 归一化加速度
  Eigen::Vector3d accel_norm;

  // 由于计算期望加速度是误差-期望，因此，这里需要取反
  accel_norm = -acceleration / acceleration.norm();

  // 简化的方法计算期望滚转和俯仰
  double pitch_d = -accel_norm.x() / acceleration.z();
  double roll_d = accel_norm.y() / acceleration.z();

  Eigen::Quaterniond quat_d = Eigen::AngleAxisd(yaw_d, Eigen::Vector3d::UnitZ()) *
                              Eigen::AngleAxisd(pitch_d, Eigen::Vector3d::UnitY()) *
                              Eigen::AngleAxisd(roll_d, Eigen::Vector3d::UnitX());
  quat_d.normalize();

  // 四元数误差增益
  double k = 4.0;

  // 计算四元数误差
  Eigen::Quaterniond quat_err = quat_d * quat.inverse();

  // 计算期望角速度
  Eigen::Vector3d angle_error = k * sgn(quat_err.w()) * quat_err.vec();

  // 计算角速度误差
  Eigen::Vector3d angular_rate_error = angle_error - odometry_.angular_velocity;

  // 计算期望角加速度
  *angular_acceleration = angular_rate_error.cwiseProduct(normalized_angular_rate_gain_);
}

移植rotors_control
另外，我在这里将rotors_control移植到了reed_control包，将LeePositionController改成了 QuatPositionController，这样做的目的是为了使整个工程代码逻辑更清晰和更容易移植。此处代码就不贴出来了，代码我上传至了gitee仓库：https://gitee.com/liaoluweillw/reed_simulator.git

结尾
到处为止，四元数姿态控制器就写完了。欢迎感兴趣的小伙伴添加微信 Reed_UAV 进行交流：
在这里插入图片描述