【ROS&GAZEBO】多旋翼无人机仿真(八)——手把手编写四元数姿态控制器

【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 进行交流:
【ROS&GAZEBO】多旋翼无人机仿真(八)——手把手编写四元数姿态控制器_第1张图片

你可能感兴趣的:(无人机仿真,编程,自动驾驶,人工智能,机器学习)