无人机仿真SLAM_gazebo&promethues

总体概述

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

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

本文将分别介绍PX4固件与机载电脑分别的作用与联系，以及如何在计算机中安装无人机的仿真环境。

本文是在amov-lab的prometheus项目的基础上编写，详情请参考Amov_Lab在github上的源码与wiki。

系统要求

无人机二次开发系统要求为ubuntu 18.04，或者在amov-lab提供的系统镜像中做开发。系统镜像地址为链接:https://pan.baidu.com/s/14jBJs-NwDi_3kJMayv7Oeg 提取码:wh4k

PX4固件

简介

这里主要介绍PX4的固件以及仿真系统中固件的意义。无人机的固件可以称作飞控系统，其中PX4是最出名的开源飞控系统之一。PX4项目为无人机开发人员提供了一套灵活的工具，以共享技术为目的，为开发人员开发无人机应用、创建量身定制的解决方案提供了广大的平台。PX4提供了无人机硬件支持和软件堆栈的标准，从而允许生态系统以可拓展的方式构建和维护硬件和软件。PX4 github主页：https://github.com/PX4/PX4-Autopilot

这里项目使用的PX4固件下载地址为https://gitee.com/amovlab/prometheus_px4。
教程请参考官方WIKI中仿真环境配置部分。

无人机固件整体框图

无人机软件框图

无人机各个模块在目录 prometheus_px4/src/modules/下。上图为PX4固件框架总览，蓝色方块是指飞控系统中的各个模块。
下面分别介绍各个模块的作用：

• Mavlink模块最为常用，可以调整为off_board模式以配合机载电脑，机载电脑可以将控制信息与飞控的姿态信息打包成MAVLink消息传给飞控
• 对于图中的位置控制与姿态控制模块，主要关注Offboard模式控制接口
• 对于图中的位置估计与姿态估计模块，主要关注外部测量值接口
• mixer模块主要负责将达到期望力矩与期望推力的油门值计算出来，并且发送到电机控制模块中

想要具体了解模块内部结构可以阅读相关模块的代码，参考文档px4控制相关资料。

无人机硬件模型

四旋翼无人机模型是无人机固件中位置控制与姿态控制模块的重要一环。对于模型具体结构和公式不需要记住其推导过程，只需要记住各个模型的输入输出是什么即可。

四旋翼无人机动力学模型推导可参考B站视频多旋翼飞行器设计与控制【北航-全权-中国MOCC】P29 多旋翼控制模型。
四旋翼无人机模型主要是在推导给无人机一个油门量之后，无人机的位置与姿态会如何变化。建立良好的无人机模型是保证无人机稳定飞行的前提。无人机模型有固定的推导公式，但是模型其中参数的确定需要经过一系列测量才能得到，比如无人机的质量、转动惯量、电机静态参数等。

四旋翼无人机模型具体可以分为:

1. 动力系统模型（电池、电调、电机及螺旋桨，输入：油门，输出：转速）
2. 控制效率模型（取决于构型，常见构型为四旋翼，输入：转速，输出：力和力矩）
3. 刚体动力学模型（受力情况，输入：力和力矩，输出：速度与角速度）
4. 刚体运动学模型（状态量关系，输入：速度和角速度，输出：位置和姿态）
   各个模型之间的关系如下图所示
   

四旋翼无人机闭环控制主要是根据当前位姿以及期望位姿，计算出当前的油门量。位置控制器与姿态控制器都最常见的还是串级PID控制器。
闭环控制控制框架可分为：

1. 位置控制（输入：期望位置，输出：期望姿态及期望升力）
2. 姿态控制（输入：期望姿态，输出：期望力矩）
3. 控制分配（输入：期望升力、力矩，输出：期望转速）
4. 电机控制（输入：期望转速，输出：期望油门）

四旋翼无人机闭环控制框架如下图所示

由于旋翼无人机的特性及其应用场景，其位置控制可以根据控制目标分为三类：位置追踪（定点）、速度追踪（定速）、轨迹追踪。
目前广泛应用于旋翼自动驾驶仪中的控制为串级PID控制器，可以实现位置追踪以及速度追踪，但是无法较好的实现轨迹追踪的需求。

Mavlink模块

无人机固件的Mavlink模块在下面的路径中prometheus_px4/src/modules/mavlink。

MAVLink全称是Micro Air Vehicle Message Marshalling Library，是一种轻量级的消息传输协议，主要用于无人机（以及板载无人机组件之间）的通信。在仿真中，外部模块给PX4固件发送的是MAVLink消息。

MAVLink消息是通过XML（Extensible Markup Language，可扩展标记语言）文件进行定义。MAVLink工具链可以通过指定的XML文件生成支持多种编程语言（C、C++、python、java等11种）的MAVLink库。由于MAVLink库支持多种语言，所以MAVLink消息可移植性很好，可以运行在多种平台上，甚至8位的单片机上也可以运行。对于MAVLink协议格式本文不做具体描述，若感兴趣可以参考https://mavlink.io/en/（中文网址为https://mavlink.io/zh/）

无人机中Mavlink模块负责MAVLink相关的参数设置，MAVLink消息的解压和收取、MAVLink消息的打包和发送。MAVLink模块中最主要三个文件的功能如下所示：

• mavlink_main.cpp为主cpp，包含了无人机的各个模式以及各个模式下需要收发的MAVLink消息
• mavlink_message.cpp包含了负责发送MAVLink消息的函数
• mavlink_receiver.cpp包含了负责接收MAVLink消息的函数

在amov-lab的prometheus项目中已经集成了一部分控制函数接口，包括起飞、待机、降落、定点飞行等，可以省去研究MAVLink协议的步骤，直接调用现有的函数给无人机飞控传递控制消息。其具体流程请参考下面的机载计算机程序中的控制模块。

位置估计与姿态估计模块

无人机本身会集成一些传感器，传感器的数据可以直接传给飞控。当无人机固件设置为不同模式时，飞控接收的就不是传感器传回的数据，而是经过其他机载组件运算之后得到的无人机的位姿。例如通过视觉SLAM计算之后得到的视觉里程计（用图像处理技术估计出来的相机的位姿）可以通过相关接口直接传回飞控，飞控接收视觉里程计之后通过滤波融合算法与其他传感器或者模块的到的数据融合，得到融合后的位置估计与姿态估计。

具体流程请参考下面的机载计算机程序中的估计模块。

安装与编译

对于无人机固件的编译请参考教程仿真环境配置。
推荐使用官方提供的系统镜像来安装，可以避免很多报错。下载地址在本文最上方。

二次开发

对于PX4的二次开发主要集中在无人机硬件的接口做一些改变。

机载计算机程序

下面简介来自Prometheus项目的github主页：

Prometheus是一套开源的自主无人机软件平台，为无人机的智能与自主飞行提供全套解决方案。本项目基于PX4开源飞控固件，旨在为PX4开发者配套成熟可用的机载电脑端程序，提供更加简洁快速的开发体验。目前已集成建图、定位、规划、控制及目标检测等模块，并配套有Gazebo仿真测试代码。

项目的下载地址为https://github.com/amov-lab/Prometheus。
教程请参考官方WIKI中Prometheus部分。推荐使用官方提供的系统镜像来安装，可以避免很多报错。镜像的下载地址在本文最上方。

控制模块

控制模块代码在路径Prometheus/Modules/control/src/px4_pos_controller.cpp下。
（等待更新）

估计模块

估计模块代码在路径Prometheus/Modules/control/src/px4_pos_estimator.cpp下。
（等待更新）

仿真模块

PX4中的仿真可以使用多种仿真平台来进行仿真，其中官方最推荐的一种仿真器就是Gazebo。具体可以参考PX4 User Guide，其中有更为具体的解释。

强烈建议使用此仿真器。
它具有功能强大的 3D 仿真环境, 特别适用于测试对象避障和计算机视觉。 它还可用于 多工具仿真，通常用于 ROS，这是一种用于自动控制的工具集。
Supported Vehicles: Quad (Iris and Solo, Hex (Typhoon H480), Generic quad delta VTOL, Tailsitter, Plane, Rover, Submarine

所有模拟器都使用 Simulator MAVLink API 与 PX4 进行通信。 该 API 定义了一组 MAVLink 消息，这些消息将仿真机的传感器数据提供给 PX4，并从将应用于仿真机的飞行代码返回电机和执行器值。

上图为PX4（Flight stack）与Gazebo（Simulator）直接的通信。无人机将姿态控制器最后的输出控制量传递给仿真器，而不是将电机最后输出值传给仿真器，这意味着混控没有在无人机中进行，而是在仿真器中计算出真正的油门值；仿真器接收无人机传递过来的姿态控制器的输出，再计算出无人机在仿真环境下的状态，将各个传感器的值再传回无人机。
所有仿真器与PX4的通讯都是通过MAVLink消息来进行的，SITL使用simulator模块中的simulator_mavlink.cpp来处理无人机传递过来的消息。

还有重要的一点就是PX4是通过UDP来进行这些消息通讯的。与仿真器的通讯是通过TCP端口4560进行通讯；与offboard模式的通讯是通过UDP端口14540进行通讯。若进行多个无人机的仿真时，需要考虑端口号不能重复。

总之仿真器、飞控、上层应用（如机载电脑）的任务如下：

1. 仿真器包含了传感器（视觉、激光雷达、GPS等）和执行结构（电机电调）的模型，经过仿真器的计算后，发送出传感器的数据以供订阅使用。
2. Offboard API以及QGC负责发送传输上层指令（位置期望点、速度期望值、以及vision模式下提供给无人机视觉里程计等）并且监控飞机状态。
3. PX4固件本身运行控制以及估计算法（即飞控核心算法）。

SLAM模块

如何安装ORB_SLAM3参考快速安装ORB_SLAM3。
在安装完成ORB_SLAM3之后需要修改文件ORB_SLAM3.0/Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc的代码。这里是为了修改坐标，添加一个坐标变换，使得坐标从ORB_SLAM坐标系变换到ROS的坐标系。详情请参考ORB_SLAM到ROS坐标转换。
将109行删掉

    mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());

改为

    cv::Mat Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image, cv_ptrD->image, cv_ptrRGB->header.stamp.toSec());
    if (Tcw.empty())
    {
        return;
    }
    geometry_msgs::PoseStamped pose;
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "map_slam";

    cv::Mat Rwc = Tcw.rowRange(0, 3).colRange(0, 3).t();

    cv::Mat Rmap = (cv::Mat_<float>(3, 3) << 0, 0, 1.0, -1.0, 0, 0, 0, -1.0, 0);
    // Rwc = Rmap * Rwc;
    cv::Mat twc = -Rwc * (Tcw.rowRange(0, 3).col(3));
    twc = Rmap * twc;
    vector<float> q = ORB_SLAM3::Converter::toQuaternion(Rwc);

    tf::Transform new_transform;
    // tf::Transform map_transform(tf::Matrix3x3(0, 0, 1.0, -1.0, 0, 0, 0, -1.0, 0));
    new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));
    tf::Quaternion quaternion(q[2], -q[0], -q[1], q[3]);

    new_transform.setRotation(quaternion);
    //new_transform.mult(map_transform, new_transform);
    tf::poseTFToMsg(new_transform, pose.pose);

    pose_pub.publish(pose);

这里的publish是为了向Prometheus项目中的px4_pos_estimator.cpp发送视觉里程计，然后再将坐标从ROS坐标系变换到PX4坐标系，再传给PX4的固件，起到视觉里程计的作用。

在修改了坐标转换代码之后，需要额外添加一些头文件才能使代码生效，

#include <geometry_msgs/PoseStamped.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include "../../../include/Converter.h"

修改话题，使得话题同一

//修改第66行中的rostopic话题名称为/cam0/image_raw，与数据集中topic话题名称统一
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/realsense_plugin/camera/color/image_raw", 100);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/realsense_plugin/camera/depth/image_raw", 100);

SLAM效果演示

修改Prometheus/Simulator/gazebo_simulator/launch_slam/sitl_drone_race.launch文件，将其中的

    <node pkg="ORB_SLAM2" type="RGBD_Publish" name="RGBD_Publish" args="$(find prometheus_slam)/config/Vocabulary/ORBvoc.txt $(find prometheus_slam)/config/Examples/RGB-D/D435i_simu.yaml">
	</node>	

节点名修改为ORB_SLAM3，包名修改为代码中的包名，参数修改为真实路径，我修改之后的代码是这样的

    <node pkg="ORB_SLAM3" type="RGBD" name="RGBD" args="/home/s/git/ORB_SLAM3/Vocabulary/ORBvoc.txt /home/s/git/ORB_SLAM3/Examples/ROS/ORB_SLAM3/Asus.yaml">
	</node>	

启动仿真即可看到无人机仿真界面

roslaunch prometheus_gazebo sitl_drone_race