当前位置：首页 > news >正文

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

news 来源：原创 2024/11/19 2:05:16

rotors_simulation 研究笔记

前言
位姿控制器
- InitializeParameters()
- SetOdometry()
- SetTrajectoryPoint()
- CalculateRotorVelocities()
- - ComputeDesiredAcceleration()
  - ComputeDesiredAngularAcc()
总结

前言

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

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

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

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

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

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

1、

LeePositionController lee_position_controller_;

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

2、
调用

lee_position_controller_.InitializeParameters();

初始化参数

3、
调用

lee_position_controller_.SetTrajectoryPoint(commands_.front());

将轨迹的航点传入 lee_position_controller 类中

4、
调用

lee_position_controller_.SetOdometry(odometry);

将里程计值传入 lee_position_controller 类中

5、
调用

lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);

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

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

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

位姿控制器

InitializeParameters()

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

void LeePositionController::InitializeParameters() {

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

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

  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();

使调谐独立于惯性矩阵

SetOdometry()

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

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

SetTrajectoryPoint()

void LeePositionController::SetTrajectoryPoint(
    const mav_msgs::EigenTrajectoryPoint& command_trajectory) {
  command_trajectory_ = command_trajectory;
  controller_active_ = true;
}

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

CalculateRotorVelocities()

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

来看如何实现

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

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

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

计算期望加速度

ComputeDesiredAcceleration()

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

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

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

计算得到位置偏差

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

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

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

计算速度偏差

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

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;

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

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);

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

ComputeDesiredAngularAcc()

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

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

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

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

  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;

得到期望的旋转矩阵

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);

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

  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);

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

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

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

这里将推力映射到z轴上

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

将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();