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

月照银海似蛟龙

分类：运动控制

发布时间 2022.10.14阅读数 2863 评论数 0

前言

无人机仿真主要分为两类：硬件在环仿真（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();

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

总结

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

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

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

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

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

测试效果：

在这里插入图片描述

四旋翼位置控制

原创文章作者：月照银海似蛟龙。如若转载，请注明出处：古月居 https://www.guyuehome.com/40212

打赏 0

上一篇：多旋翼无人机仿真 rotors_simulator 是如何悬停的（一）

下一篇：多旋翼无人机仿真 rotors_simulator：roll pitch yawrate thrust 控制器

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

月照银海似蛟龙

前言

位姿控制器

InitializeParameters()

SetOdometry()

SetTrajectoryPoint()

CalculateRotorVelocities()

ComputeDesiredAcceleration()

ComputeDesiredAngularAcc()

总结

为你推荐

【无人机系统】四轴飞行器及其UAV飞控系统 - 桂林电子科技大学信息科技学院 电子工程系（三）

【分享】从零开始在树莓派4B上搭建环境，使用ros控制dofbot机械臂（二）

10. SISO反馈控制器设计 (1)：状态反馈控制 State Feedback Control

专家PID仿真

机器人运动规划中的构形空间（Configuration Space）

机器人-编程-作业1-保持机械臂静止

评论（0）

关于作者

月照银海似蛟龙

153

0

165

7

A-LOAM :前端lidar点预处理部分代码解读

lio-sam框架：后端里程计、回环、gps融合

3d激光SLAM：LIO-SAM框架---IMU预积分功能数据初始化

相关推荐

车辆运动控制（7）考虑道路倾角和曲率

常见移动机器人运动学模型总结

轨迹优化06. 镇定、跟踪 & 调节

基于STM32F103C8Tx芯片实现的应答式串口数据管理器逻辑分析与程序实现

飞行机器人（三）DJI平台OSDK ROS 编译及使用

NLP非线性优化第二步CasAdi Matlab库导出与部署

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

3d 激光slam

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

【无人机系统】四轴飞行器及其UAV飞控系统 - 桂林电子科技大学信息科技学院电子工程系（三）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板