前言

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>

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

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

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

• 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" />

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

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

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

    <node name="roll_pitch_yawrate_thrust_controller_node" pkg="rotors_control" type="roll_pitch_yawrate_thrust_controller_node" output="screen"><rosparamcommand="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>

启动 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");

初始化节点

  rotors_control::RollPitchYawrateThrustControllerNode roll_pitch_yawrate_thrust_controller_node;ros::spin();return 0;
}

实例化 类

下面来看这个类的内容

RollPitchYawrateThrustControllerNode

构造函数

RollPitchYawrateThrustControllerNode::RollPitchYawrateThrustControllerNode() {

  InitializeParams();ros::NodeHandle nh;

初始化参数
声明节点

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

订阅控制指令

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

订阅里程计

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

发布电机转速

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

来看订阅的两个回调函数

RollPitchYawrateThrustCallback

控制指令的回调函数

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

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

转成eigen的格式

  roll_pitch_yawrate_thrust_controller_.SetRollPitchYawrateThrust(roll_pitch_yawrate_thrust);

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

OdometryCallback

里程计回调函数

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

  EigenOdometry odometry;eigenOdometryFromMsg(odometry_msg, &odometry);

转成eigen 的里程计信息格式

  roll_pitch_yawrate_thrust_controller_.SetOdometry(odometry);

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

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

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

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

构造 电机转速的 变量

  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;

赋值电机转速

motor_velocity_reference_pub_.publish(actuator_msg);

发布电机转速

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

RollPitchYawrateThrustController

构造函数

RollPitchYawrateThrustController::RollPitchYawrateThrustController(): initialized_params_(false),controller_active_(false) {InitializeParameters();
}

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

SetOdometry

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

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

SetRollPitchYawrateThrust

获得控制指令

void RollPitchYawrateThrustController::SetRollPitchYawrateThrust(const mav_msgs::EigenRollPitchYawrateThrust& roll_pitch_yawrate_thrust) {roll_pitch_yawrate_thrust_ = roll_pitch_yawrate_thrust;controller_active_ = true;
}

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

CalculateRotorVelocities

计算电机转速

  assert(rotor_velocities);assert(initialized_params_);

判断是否有这两个参数

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

电机转速清零

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

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

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

计算期望的角加速度

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

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

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

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

  *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

计算期望角加速度

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

  assert(angular_acceleration);

判断有这个变量

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

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

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

根据旋转矩阵求得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

计算得到期望的旋转矩阵

  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] = roll_pitch_yawrate_thrust_.yaw_rate;

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

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

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

总结

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

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

多旋翼无人机仿真 rotors_simulator：roll pitch yawrate thrust 控制器相关推荐

1. 多旋翼无人机仿真 rotors_simulator：用键盘控制无人机飞行

   多旋翼无人机仿真 rotors_simulator:用键盘控制无人机飞行 前言 书接上文 接口测试 键盘指令发布 指令转换与发布 修改 rotors_simulator 的控制接口节点 测试 前言 R ...

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

   多旋翼无人机仿真 rotors_simulator 是如何悬停的(一) 前言 mav_hovering_example.launch mav_generic_odometry_sensor.gazeb ...

3. 【rotors】多旋翼无人机仿真（三）——SE3控制

   [rotors]多旋翼无人机仿真(一)--搭建rotors仿真环境 [rotors]多旋翼无人机仿真(二)--设置飞行轨迹 [rotors]多旋翼无人机仿真(三)--SE3控制 [rotors]多旋翼 ...

4. 【rotors】多旋翼无人机仿真（四）——参数补偿和PID控制

   [rotors]多旋翼无人机仿真(一)--搭建rotors仿真环境 [rotors]多旋翼无人机仿真(二)--设置飞行轨迹 [rotors]多旋翼无人机仿真(三)--SE3控制 [rotors]多旋翼 ...

5. 【rotors】多旋翼无人机仿真（二）——设置飞行轨迹

   [rotors]多旋翼无人机仿真(一)--搭建rotors仿真环境 [rotors]多旋翼无人机仿真(二)--设置飞行轨迹 [rotors]多旋翼无人机仿真(三)--SE3控制 [rotors]多旋翼 ...

6. 【rotors】多旋翼无人机仿真（一）——搭建rotors仿真环境

   [rotors]多旋翼无人机仿真(一)--搭建rotors仿真环境 [rotors]多旋翼无人机仿真(二)--设置飞行轨迹 [rotors]多旋翼无人机仿真(三)--SE3控制 [rotors]多旋翼 ...

7. 【ROSGAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器

   [ROS&GAZEBO]多旋翼无人机仿真(一)--搭建仿真环境https://blog.csdn.net/qq_37680545/article/details/123185002 [ROS& ...

8. 四旋翼无人机仿真之hector_quadrotor无人机（ROS + Gazebo）（三）传感器数据读取与复现（IMU、GPS）

   系列文章目录 文章1:四旋翼无人机仿真之hector_quadrotor无人机(ROS + Gazebo) 文章2:四旋翼无人机仿真之hector_quadrotor(二)键盘teleop_twist ...

9. 四旋翼无人机仿真之hector_quadrotor无人机（ROS + Gazebo）（一）

   这里写自定义目录标题 应用环境 hector_quadrotor 功能包结构简介 hector_quadrotor 功能包安装 1. 安装所需依赖库 2. 下载.安装hector_quadrotor ...

最新文章

1. 匿名黑客Anonymous实施的8次最强攻击
2. Yii2 解决2006 MySQL server has gone away问题
3. linux I/O--IO原理和几种零拷贝机制（五）
4. zhcon解决Ubuntu命令行模式乱码问题
5. 网关Spring Cloud Gateway科普
6. Java开发趋势：2019年展望
7. xampp默认mysql数据库root密码的修改
8. HDU2161 Primes
9. 康众平板探测器_2020-2025年数字化X线探测器行业市场深度调研及投资前景预测分析报告 数字化设备成为市场主流...
10. Android-- Dialog对话框的使用方法
11. 剑指offer——面试题4：替换空格
12. 上传分片切片大文件 XLSX/CSV/TXT
13. System.ConfigurationManager类用于对配置文件的读取
14. PJAX，站点加速之翼
15. 如何使用fiddler抓取APP接口
16. 主流的商业智能BI工具推荐，学会数据分析没难度
17. 经典兔子问题有一对兔子从出生后第3个月起每个月都生一对兔子
18. Pomodoro Technique
19. 计算机ppt培训心得体会,制作ppt学习心得体会
20. uni-app z-index无效的解决办法（遮罩层）

热门文章

1. 关于不使用firefly补丁对系统进行美化的探讨(转)
2. eclipse3.6 太阳神版 中文汉化插件
3. OPengl学习（二）——opengl环境搭建
4. 上网行为安全之深信服用户认证技术和用户、组管理
5. 无影云服务器没有网络怎么办？
6. 富人越富，穷人越穷，我为什么反对PoS
7. 2017寒假作业 计科1501 李俊01
8. 五子棋java毕业设计论文_基于Java的五子棋游戏的设计毕业论文,课程设计,研究生文献...
9. arduino笔记15：按键开关输入
10. TCP穿透主流商用NAT产品的主要技术研究