多旋翼无人机仿真 rotors_simulator 是如何悬停的（二）

前言

无人机仿真主要分为两类：硬件在环仿真（HITL）和软件在环仿真（SITL全称Software in the loop）。

无人机软件在环仿真是指完全用计算机来模拟出无人机飞行时的状态，而硬件在环仿真是指计算机连接飞控板来测试飞控软件是否可以流畅运行。一般来说硬件在环仿真若没有加上真实的转台进行测试的话，其与软件在环仿真没有很大的区别。

在无须解决在研发过程中的硬件问题带来的麻烦，并且可以直观的调试代码，搭建一套无人机仿真系统，对于研发来说好处是数不剩数的。

上一篇博客，解析到了
lee_position_controller_node.cpp

这个文件，向 LeePositionController 这个 类 中传递了 几个参数，并从这个类中获得了电机的转速，然后发布出去。将电机转速发布出去之后的事情暂时先不管，知道可以这个转速可以改变飞机的力矩从而改变飞机的位姿就可以了。

回顾一下上篇博客 lee_position_controller_node.cpp 中和 LeePositionController 这个类的交互有：

1、

LeePositionController lee_position_controller_;
1

LeePositionControllerNode类的私有变量实例化LeePositionController类

2、
调用

lee_position_controller_.InitializeParameters();
1

初始化参数

3、
调用

lee_position_controller_.SetTrajectoryPoint(commands_.front());
1

将轨迹的航点传入 lee_position_controller 类中

4、
调用

lee_position_controller_.SetOdometry(odometry);
1

将里程计值传入 lee_position_controller 类中

5、
调用

lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);
1

获得 lee_position_controller 计算的 电机转速数据

本篇博客就主要分析lee_position_controller.cpp 文件中的lee_position_controller这个类

也就是上面调用的那几个函数

位姿控制器

InitializeParameters()

这个函数的功能就是初始化参数

void LeePositionController::InitializeParameters() {
1

calculateAllocationMatrix(vehicle_parameters_.rotor_configuration_, &(controller_parameters_.allocation_matrix_));
1

根据旋翼的类型，得到动力混控的分配矩阵

  normalized_attitude_gain_ = controller_parameters_.attitude_gain_.transpose()
      * vehicle_parameters_.inertia_.inverse();
  normalized_angular_rate_gain_ = controller_parameters_.angular_rate_gain_.transpose()
      * vehicle_parameters_.inertia_.inverse();
1
2
3
4

使调谐独立于惯性矩阵

SetOdometry()

void LeePositionController::SetOdometry(const EigenOdometry& odometry) {
  odometry_ = odometry;
}
1
2
3

就是将外部的里程计数据传到了内部

SetTrajectoryPoint()

void LeePositionController::SetTrajectoryPoint(
    const mav_msgs::EigenTrajectoryPoint& command_trajectory) {
  command_trajectory_ = command_trajectory;
  controller_active_ = true;
}
1
2
3
4
5

将外部航点指令出入内部
控制器启动标志位 至 true

CalculateRotorVelocities()

上面的两个函数很简单，相当于向类的内部传入了 里程计数据和目标位置指令
那么在这个函数中就是通过这两者的信息，计算出来电机转速

来看如何实现

  if (!controller_active_) {
    *rotor_velocities = Eigen::VectorXd::Zero(rotor_velocities->rows());
    return;
  }
1
2
3
4

如果控制器没有启动，那么就直接返回电机转速为 0
接到第一命令后，控制器就启动了，之后执行完也部关的

  Eigen::Vector3d acceleration;
  ComputeDesiredAcceleration(&acceleration);
1
2

计算期望加速度

ComputeDesiredAcceleration()

下面则进到这个函数看，看下期望加速度是如何计算得到的

void LeePositionController::ComputeDesiredAcceleration(Eigen::Vector3d* acceleration) const {
  assert(acceleration);
1
2

  Eigen::Vector3d position_error;
  position_error = odometry_.position - command_trajectory_.position_W;
1
2

计算得到位置偏差

  const Eigen::Matrix3d R_W_I = odometry_.orientation.toRotationMatrix();
  Eigen::Vector3d velocity_W =  R_W_I * odometry_.velocity;
1
2

将里程计的速度转到世界坐标系下

  Eigen::Vector3d velocity_error;
  velocity_error = velocity_W - command_trajectory_.velocity_W;
1
2

计算速度偏差

Eigen::Vector3d e_3(Eigen::Vector3d::UnitZ());
1

UnitZ() 返回一个z轴的一个单位向量 0，0，1

  *acceleration = (position_error.cwiseProduct(controller_parameters_.position_gain_)
      + velocity_error.cwiseProduct(controller_parameters_.velocity_gain_)) / vehicle_parameters_.mass_
      - vehicle_parameters_.gravity_ * e_3 - command_trajectory_.acceleration_W;
1
2
3

将 位置偏差 和 速度偏差 和 加速度偏差 ，得到 期望的加速度

• position_error.cwiseProduct(controller_parameters_.position_gain_ 将位置偏差经过增益系数转成 的 期望加速度部分
• velocity_error.cwiseProduct(controller_parameters_.velocity_gain_)) / vehicle_parameters_.mass_ 将速度偏差经过增益系数转成 的 期望加速度部分还除以了质量
• vehicle_parameters_.gravity_ * e_3 重力加速度部分
• command_trajectory_.acceleration_W 指令加速度部分

继续回到 这个函数 CalculateRotorVelocities() 中

  Eigen::Vector3d angular_acceleration;
  ComputeDesiredAngularAcc(acceleration, &angular_acceleration);
1
2

通过计算的期望加速度读
计算期望角速度

ComputeDesiredAngularAcc()

来看是如何通过计算的期望加速度读计算期望角速度的

void LeePositionController::ComputeDesiredAngularAcc(const Eigen::Vector3d& acceleration,
                                                     Eigen::Vector3d* angular_acceleration) const {
  assert(angular_acceleration);
1
2
3

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

将飞机的姿态 四元数 转成 旋转矩阵

  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;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

得到期望的旋转矩阵

Eigen::Matrix3d angle_error_matrix = 0.5 * (R_des.transpose() * R - R.transpose() * R_des);
1

计算旋转矩阵的角度偏差

  Eigen::Vector3d angle_error;
  vectorFromSkewMatrix(angle_error_matrix, &angle_error);
1
2

将旋转矩阵的角度差的矩阵形式转成角度差的形式

  Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero());
  angular_rate_des[2] = command_trajectory_.getYawRate();
1
2

声明期望角速度
赋值指令的航向期望角速度

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

计算角速度的偏差

  *angular_acceleration = -1 * angle_error.cwiseProduct(normalized_attitude_gain_)
                           - angular_rate_error.cwiseProduct(normalized_angular_rate_gain_)
                           + odometry_.angular_velocity.cross(odometry_.angular_velocity); 
1
2
3

将角速度的偏差结合角度的偏差，转成期望角加速度

现在有了期望角加速度，继续回到 这个函数 CalculateRotorVelocities() 中

  double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));
1

这里将推力映射到z轴上

  Eigen::Vector4d angular_acceleration_thrust;
  angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
  angular_acceleration_thrust(3) = thrust;
1
2
3

将3个轴的加角速度 加上推力，就是最后的力

  *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();
1
2
3

通过混控器，将最后的力，分配到各个电机上，得到电机转速。

总结

致此则完成了，探究 多旋翼无人机仿真 rotors_simulator 是如何悬停的 梳理。

总结下 在第一个cpp中 设置了一个期望位置 0，0，1。并在该文件中获得了无人机的里程计数据。将期望位置和里程计数据 传入了一个类中。

传入的类是一个位姿控制功能的类。将期望位置和里程计中的位置的偏差计算得到期望加速度。将期望加速度，根据里程计中的角度，计算得到期望角角速度。

最终根据计算得到的期望角角速度，经过电机混合，得到电机的转速，返回给第一个cpp，然后将电机转速的值通过topic发布了出去。

通过以上分析，如果在gazebo中拖动无人机，那么无人机则会自动的回到原来0,0,1的位置上

测试效果：