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

前言

RotorS 是一个MAV gazebo 仿真系统。

提供了几种多旋翼仿真模型，例如

• AscTec Hummingbird
• AscTec Pelican
• AscTec Firefly

但是仿真系统不限于使用这几种模型

AscTec 是 德国Ascending Technologies公司的缩写。

是很早的无人机了，实物张下面这个样子：

仿真系统中包含很多种仿真传感器，都可以安装在无人机上，例如：

• IMU
• 里程计
• 视觉惯导相机

功能包中包含了几种控制器，包含位置控制，游戏手柄控制灯

github的地址为：https://github.com/ethz-asl/rotors_simulator

在之前的博客总分析 rotors_simulator 的位置控制器 是如何使无人机在 gazebo里面悬停的

如果无人机仅能悬停那么也没什么用啊，在同样的launch文件下，发现了mav_with_keyboard.launch 文件
通过文件的名字可以推断是通过键盘来控制无人机飞行

运行一下发现，运行不了，具体的原因会在下面给出

但是通过这个功能，发现了其中的一个控制接口 roll pitch yawrate thrust
本篇文章主要分析这个控制器是如何实现的。

了解清楚这个控制器，在之后的博客中，会通过应用更普遍的PID控制器，利用这个接口实现无人机的位置。

mav_with_keyboard.launch

这个启动文件对应的是启动 roll_pitch_yawrate_thrust_controller_node 的 节点。
但是 这个 文件有点问题

    <include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
      <arg name="mav_name" value="$(arg mav_name)" />
      <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo" />
      <arg name="enable_logging" value="$(arg enable_logging)" />
      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
      <arg name="log_file" value="$(arg log_file)"/>
    </include>
1
2
3
4
5
6
7

这个启动了gazebo，加载了无人机

<node name="key_joystick" pkg="rotors_joy_interface" type="key_joystick.py" />
1

启动这个的时候，会提示以下几个问题，都好解决

• ModuleNotFoundError: No module named uinput ----缺少python的uinput模块 通过指令 pip指令按照就行
• No matching distribution found for python-pygame----缺少python的pygame模块 通过指令 pip指令按照就行
• /usr/bin/env: “python”: 没有那个文件或目录----ubuntu20.04是对应的python3 通过 创建一个链接符号到 python 命令:sudo ln -s /usr/bin/python3 /usr/bin/python

但是启动了，会自己闪退，这个是launch文件运行不成功的原因之一

<node name="rotors_joy_interface" pkg="rotors_joy_interface" type="rotors_joy_interface" />
1

这个是启动 rotors_joy_interface 功能包下面 joy.cpp 对应生成的可执行文件

<node name="joy_node" pkg="joy" type="joy_node" />
1

这个功能包，及对应的执行文件就没有。
这个是launch文件启动不成功的原因之二

    <node name="roll_pitch_yawrate_thrust_controller_node" pkg="rotors_control" type="roll_pitch_yawrate_thrust_controller_node" output="screen">
      <rosparam
        command="load" file="$(find rotors_gazebo)/resource/roll_pitch_yawrate_thrust_controller_$(arg mav_name).yaml" />
      <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
      <remap from="odometry" to="odometry_sensor1/odometry" />
    </node>
1
2
3
4
5
6

启动 rotors_control 功能包的 roll_pitch_yawrate_thrust_controller_node.cpp 对应生产的可执行文件

由于只是分析 其 控制的原理与接口，所以只需要分析其代码就ok了，launch文件的问题就不去解决了。

roll_pitch_yawrate_thrust_controller_node.cpp

是一个执行文件的代码，所以从main看起

int main(int argc, char** argv) {
  ros::init(argc, argv, "roll_pitch_yawrate_thrust_controller_node");
1
2

初始化节点

  rotors_control::RollPitchYawrateThrustControllerNode roll_pitch_yawrate_thrust_controller_node;
  ros::spin();
  return 0;
}
1
2
3
4

实例化 类

下面来看这个类的内容

RollPitchYawrateThrustControllerNode

构造函数

RollPitchYawrateThrustControllerNode::RollPitchYawrateThrustControllerNode() {
1

  InitializeParams();
  ros::NodeHandle nh;
1
2

初始化参数
声明节点

  cmd_roll_pitch_yawrate_thrust_sub_ = nh.subscribe(kDefaultCommandRollPitchYawrateThrustTopic, 1,
                                     &RollPitchYawrateThrustControllerNode::RollPitchYawrateThrustCallback, this);
1
2

订阅控制指令

  odometry_sub_ = nh.subscribe(kDefaultOdometryTopic, 1,
                               &RollPitchYawrateThrustControllerNode::OdometryCallback, this);
1
2

订阅里程计

  motor_velocity_reference_pub_ = nh.advertise<mav_msgs::Actuators>(
      kDefaultCommandMotorSpeedTopic, 1);
1
2

发布电机转速

初始化参数就不看了，就是初始化的各个方向 角度和角速度的增益

来看订阅的两个回调函数

RollPitchYawrateThrustCallback

控制指令的回调函数

void RollPitchYawrateThrustControllerNode::RollPitchYawrateThrustCallback(
    const mav_msgs::RollPitchYawrateThrustConstPtr& roll_pitch_yawrate_thrust_reference_msg) {
1
2

  mav_msgs::EigenRollPitchYawrateThrust roll_pitch_yawrate_thrust;
  mav_msgs::eigenRollPitchYawrateThrustFromMsg(*roll_pitch_yawrate_thrust_reference_msg, &roll_pitch_yawrate_thrust);
1
2

转成eigen的格式

  roll_pitch_yawrate_thrust_controller_.SetRollPitchYawrateThrust(roll_pitch_yawrate_thrust);
1

对应格式的 控制指令 送入控制器中

OdometryCallback

里程计回调函数

void RollPitchYawrateThrustControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {
1

  EigenOdometry odometry;
  eigenOdometryFromMsg(odometry_msg, &odometry);
1
2

转成eigen 的里程计信息格式

  roll_pitch_yawrate_thrust_controller_.SetOdometry(odometry);
1

将eigen的里程计信息格式送入 控制器中

  Eigen::VectorXd ref_rotor_velocities;
  roll_pitch_yawrate_thrust_controller_.CalculateRotorVelocities(&ref_rotor_velocities);
1
2

获得控制器 最终 计算的 电机转速

  mav_msgs::ActuatorsPtr actuator_msg(new mav_msgs::Actuators);
1

构造 电机转速的 变量

  actuator_msg->angular_velocities.clear();
  for (int i = 0; i < ref_rotor_velocities.size(); i++)
    actuator_msg->angular_velocities.push_back(ref_rotor_velocities[i]);
  actuator_msg->header.stamp = odometry_msg->header.stamp;
1
2
3
4

赋值电机转速

motor_velocity_reference_pub_.publish(actuator_msg);
1

发布电机转速

下面来看在控制器里具体执行的操作，是如何计算得到电机转速的

RollPitchYawrateThrustController

构造函数

RollPitchYawrateThrustController::RollPitchYawrateThrustController()
    : initialized_params_(false),
      controller_active_(false) {
  InitializeParameters();
}
1
2
3
4
5

构造函数 初始化参数标志位为0 控制器激活标志位为0
初始化参数

SetOdometry

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

获得里程计数据
赋值给类的成员变量

SetRollPitchYawrateThrust

获得控制指令

void RollPitchYawrateThrustController::SetRollPitchYawrateThrust(
    const mav_msgs::EigenRollPitchYawrateThrust& roll_pitch_yawrate_thrust) {
  roll_pitch_yawrate_thrust_ = roll_pitch_yawrate_thrust;
  controller_active_ = true;
}
1
2
3
4
5

获得控制指令，赋值给类的成员变量
控制器激活

CalculateRotorVelocities

计算电机转速

  assert(rotor_velocities);
  assert(initialized_params_);
1
2

判断是否有这两个参数

  rotor_velocities->resize(vehicle_parameters_.rotor_configuration_.rotors.size());
1

电机转速清零

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

如果没有控制指令，那么控制器就没有激活 所有电机的转速为0

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

计算期望的角加速度

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

将3个方向的力，加上推力方向的力

 angular_acceleration_thrust(3) = roll_pitch_yawrate_thrust_.thrust.z();
1

最后是直接赋值了推力的大小

  *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

根据电机混控参数 分配各电机转速

ComputeDesiredAngularAcc

计算期望角加速度

void RollPitchYawrateThrustController::ComputeDesiredAngularAcc(Eigen::Vector3d* angular_acceleration) const {
1

  assert(angular_acceleration);
1

判断有这个变量

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

获得无人机的姿态旋转矩阵

double yaw = atan2(R(1, 0), R(0, 0));
1

根据旋转矩阵求得yaw角

  Eigen::Matrix3d R_des;
  R_des = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())  // yaw
        * Eigen::AngleAxisd(roll_pitch_yawrate_thrust_.roll, Eigen::Vector3d::UnitX())  // roll
        * Eigen::AngleAxisd(roll_pitch_yawrate_thrust_.pitch, Eigen::Vector3d::UnitY());  // pitch
1
2
3
4

计算得到期望的旋转矩阵

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

通过旋转矩阵 计算角度偏差

Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero());
1

声明并清零期望角速度值

angular_rate_des[2] = roll_pitch_yawrate_thrust_.yaw_rate;
1

把控制指令里面的期望航向角速度 赋值 到向量中

  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

根据角度偏差、角速度偏差、实际角速度 计算得到 期望的角加速度

总结

其中的整个节点逻辑和之前的位置控制类似
在node cpp中获得控制指令和里程计数据，转成 控制器需要的数据格式

控制器中的关键部分就是 期望角角速度的计算
核心思想是：
通过旋转矩阵计算得到 角度偏差 和角速度偏差
根据 角度偏差、角速度偏差、实际角速度 计算得到 期望角加速度