Prometheus自主无人机入门笔记_prometheus与gazebo通信机制

本部分介绍自主无人机组成框架、自主无人机关键技术

1.1 自主无人机的组成框架

1.1.1 硬件框架

自主无人机硬件组成有机架、动力系统（包括电机、电调和浆）、指挥系统（机载电脑、飞控板）、通信系统（数传、图传、遥控器）、传感器（摄像头、激光雷达等）电池等。

1.1.2自主无人机功能框架

在这直接以“复杂环境下的自主导航定位与通用目标识别任务”为例：

单目摄像头和云台等硬件属于感知模块，感知模块和决策模块相互可传递消息，外部任务指令是给到决策模块，决策模块紧接着传递给规划模块，规划模块进行路径规划或者建图，同时它也需要接受来自定位导航模块的信息，定位导航模块例如激光雷达、相机、IMU、GPS等硬件也将信息传递给规划模块和控制模块，最后控制模块给出控制指令到Mavros，从而最后控制飞控。

1.2 四旋翼无人机模型及参数的确定

1.2.1 四旋翼无人机建模

建模就是为了理解事物而对事物做出的一种抽象，对于事物的一种无歧义的书面描述。而建模是研究系统的重要手段和前提。而模型描述系统的手段和方法也是多种多样的。可以通过对系统本身运动规律的分析进行建模也可以根据事物的机理来建模等等各种方法。

而对于四旋翼无人机建模的意思就是通过数学表达式将四旋翼无人机的运动机理用数学表达式表示出来。例如可以表示出四旋翼的飞行性能等，所以对于四旋翼的模型不仅仅是运动学模型，还包括电调模型、电机模型、电池模型等等一起组成的。

1.2.2 四旋翼无人机参数的确定

有关飞机平台参数测量主要有质量、转动惯量、电机静态参数、静态模型辨识结果、动态模型辨识结果、辨识结果验证。无人机参数的确定，是为了前面我们所介绍的系统建模所用，通过参数才可以更好的选型，选好型后才能建立可靠的模型，这样在后续才能更好的研究飞行性能等信息去评判自己的设计的算法是否可靠。

1.3 消息的传递

一般在自主无人机内部有三种消息的存在：uORB消息、MAVLink消息、ROS话题。

uORB是PX4内部的消息格式，MAVLink是用于飞控与外部的沟通，而ROS话题是ROS中通信的消息格式。

1.3.1  MAVLink通信协议

MAVLink全称为Micro Air Vehicle Message Marshalling Library，是一种轻量级的消息传输协议，用于机载组件之间的协议。其实MAVLink通信协议在前面的PX4开发也有提到过相关内容，它不存在与任何一个结构中，但是它是飞控与外界联系的通信方式。MAVLink支持二次开发，也支持多种语言，比如C，C++，PYTHON，JAVA等，在这些编程语言里基本都含有Mavlink库，都遵循MIT协议。更为详细的内容可以去查找有关PX4开发里有关通信协议的介绍。

1.3.2 ROS

如果你想要学习无人机、机器人或者无人机系统，那么ROS必定是需要掌握的一门技术。ROS全称为Robot Operating System，是一种得到广泛使用的机器人操作与控制系统软件框架。简单来说ROS就是一个平台，不仅提供了各种开源代码，还提供了开源代码之间通信机制。如果你想实现各个程序之间的通信，只需要下载这些功能包，然后写一个简单的启动脚本（即写launch文件），就能运行起来一个机器人系统。例如搭建一个机器人，电机、力传感器还有摄像头等等设备的底层驱动ROS都可以提供，同时ROS还可以提供PID控制代码、SLAM算法等等，都可以添加到里面。

1.3.3 MAVROS功能包

MAVROS就是MAVLink+ROS，MAVROS是PX4官方提供的一个运行于ROS下收发MAVLink消息的工具。利用MAVROS就可以发送MAVLink消息给飞机，即传递控制信息给飞机；同时还可以从飞控中接收数据，比如位置速度还有IMU数据等等。

关于飞控和Prometheus之间的消息流向：

从PX4——>Prometheus，消息一般是状态量，比如位置、速度、IMU等消息。它的消息流向为：从某一个PX4模块- PX4 mavlink模块- mavros plugin - prometheus

从Prometheus——>PX4，消息一般为控制指令，如期望速度、速度、解/上锁等消息。它的消息流向为：prometheus - mavros plugin - PX4 mavlink模块-某一个PX4的模块

2. Prometheus简介

Prometheus是一套开源的自主无人机软件平台，可以为无人机的智能与自主飞行提供全套解决方案。整个平台都是基于PX4开源飞控固件建立的，再加上成熟可用的机载电脑端程序，可以让人快速上手感受简洁快速的开发体验。

常用的自主无人机应用：无GPS环境的自主导航；基于视觉的目标跟踪；基于二维码的自主降落等。

项目网址：GitHub - amov-lab/Prometheus: Open source software for autonomous drones. 

2.1 Prometheus学习路线

1）安装和编译，系统要求

Ubuntu 18.04或Ubuntu20.04.

2）基础知识的学习

无人机建模控制、PX4、ROS等相关基础理论知识的学习。

3）Prometheus源码的学习：

4）仿真开发

在实物操作之前最好先进行仿真测试，没有问题，再进行工程实现。

5）实物开发。

2.2 Prometheus框架

控制模块在最底层，传递一个drone_state话题代表无人机当前状态，定位导航模块生成定位和建图信息给规划模块，规划模块生成轨迹或者路径给任务模块，如果有目标检测功能，也通过目标检测把信息给任务模块，任务模块经过一系列计算或判断任务决策执行后，通过prometheus/control_command话题给控制模块，最后驱动MAVROS给到飞控。

可在wiki与源码学习，在Prometheus源码中，根目录下主要有三个文件夹，分别是：Modules、Simulator、Experiment。

Modules模块是功能模块，每个模块都是一个标准的ROS工具包，每个小功能都是一个ROS节点。

Simulator是仿真器，目前使用Gazebo仿真器。

experiment文件夹主要放有关真机实验的相关模块。

如下表是modules功能模块的详细介绍：分别有共用模块、控制模块、SLAM模块、规划模块、目标检测模块，任务模块。

2.2.1共用模块

Prometheus项目自定义消息

ControlCommand.msg

DroneState.msg

Message.msg

共用的库函数

2.2.2控制模块

依赖Mavros功能包及prometheus_msgs功能包，重要代码:config文件夹中为控制器参数配置文件;px4_pos_controller.cpp; px4_pos_estimator.cpp; ground_station.cpp;…

读取飞控中的各项状态，并发布为话题“/prometheus/drone_state”

丰富了px4原有的offboard控制方式，提供以下控制接口用于任务模块开发（订阅话题“/prometheus/control_command”）:怠速、起飞、悬停、降落、移动(包括定点,定速及其复合模式,以及轨迹追踪）；可进行位置环控制器设计及分布式阵型控制器设计。

2.2.3 建图定位模块

视觉定位部分外部依赖ORB-SLAM2，重要代码： ros_rgbd_publish.cc； ros_stereo_publish.cc；ros_mono_publish.cc；elas_stereo_node.cpp；… 。激光定位外部依赖cartographer_ros包，视觉及激光定位结果与PX4信息融合，发布无人机里程计信息；建图部分依赖RTAB-Map及Octomap，建图包括RGBD建图、双目相机建图和激光建图。建图部分发布全局点云信息供规划模块使用，后续会加入视觉惯性里程计算法，如VINS-Fusion，maplab等。

2.2.4 规划模块

现有功能：全局规划（A*）、局部规划（APF、VFH）、轨迹优化（Fastplanner） Ø 订阅地图（局部/全局点云）、目标点、无人机当前位置等话题；发布规划结果至任务模块/Prometheus/planning/desired_vel；/Prometheus/planning/a_star /desired_path；/Prometheus/planning/fastplanner/desired_trajectory 。重要代码apf.cpp；vfh.cpp； A_star.cpp；bspline_optimizer.cpp ；后续会加入更多的规划类算法demo

2.2.5 目标识别模块

现有功能：二维码识别、椭圆识别、KCF追踪、数字识别（基于pytorch）、通用目标识别（YOLO v3 tiny）、Siam-RPN追踪（基于pytorch）等

订阅摄像头数据： “/prometheus/camera/rgb/image_raw”

发布识别结果，即目标在相机坐标系的位置

“/prometheus/target”

重要代码：config文件夹中为相机配置文件；pytorch_mnist_det.py； siam_rpn.py；ellipse_det.cpp；kcf_tracker.cpp；YoloObjectDetector.cpp…

2.2.6任务模块

订阅来自规划、目标检测模块的处理结果，发布统一的控制指令至控制模块 位置：“/prometheus/control_command”。处理任务逻辑，生成统一的控制指令，重要代码：autonomous_landing.cpp；circle_crossing.cpp；number_detection.cpp；indoor_competition.cpp

2.4. 仿真

仿真的原因：对于大型复杂系统，直接实验的费用往往是十分昂贵的，而仿真仅仅只是需要其成本的1/10。出于安全考虑，对于某些系统(如载人飞行器、核电装置等)，直接实验往往存在很大危险，甚至是不允许的；而采用仿真实验可以有效降低危险程度，对系统的研究起到保障作用。因为其不需要依靠硬件的支持，只需要下载仿真器在电脑上，改变代码端口连接，就可以做到模拟现实应用一样的效果，而一旦代码出现出问题在实体机上可能会炸机摧毁机体，但是仿真就可以避免这种问题。所以进行真机实验之前需要做仿真实验。

用Prometheus进行的仿真分为硬件在环仿真和软件在环仿真。

HITL（Hardware in the loop）硬件在环仿真，又称半实物仿真，是将需要仿真的部分系统硬件直接放到仿真回路中的仿真系统，它不仅弥补了纯数字仿真中的许多缺陷，提高了整个模型的置信度，而且可以大大减轻编程的工作量。硬件在环仿真是以实时处理器运行仿真模型来模拟受控对象的运行状态，通过I/0接口与被测对象相连接。

SITL（Software in the loop）软件在环仿真主要测试控制器模型与控制器模型代码实现之间的一致性。目前软件在环仿真一共有jMAVSim、Gazebo、AirSim三种仿真器。jMAVSim属于一种PX4原生轻量级仿真器，支持飞机类型只有四轴，可以支持多机仿真；AirSim是以Unreal虚幻引擎的插件开发的，有着目前来说最为强大的仿真效果，但是同时也是最吃电脑硬件的一个，对显卡，CPU及内存要求均最高，且安装过程中“坑”是最多的，目前github上更新维护较慢，所以AirSim在无人机仿真的应用是比较少的。

在Prometheus中我们用到的仿真器是Gazebo，它支持旋翼、固定翼、倾转、小车等，是所有仿真器中支持平台最多的仿真器。而硬件在环仿真（hardware in the loop），又称半实物仿真，是将需要仿真的部分系统硬件直接放到仿真回路中的仿真系统，它不仅弥补了纯数字仿真中的许多缺陷，提高了整个模型的置信度，而且可以大大减轻编程的工作量。

所有飞控PX4和仿真器Gazebo之间的通信都是通过MAVLink消息来进行的。而SITL使用simulator模块中的simulator_mavlink.cpp来处理这些信息的；HITL是通过mavlink模块中的mavlink_receiver.cpp来处理信息的。

2.5. 功能介绍

现在Prometheus可以实现的功能有无人机控制、多编队控制、建图、ORB-SLAM、深度学习等功能。在无人机控制中可以对无人机位置换控制器二次开发，Prometheus中内置多个实例开发程序同时也提供了测试工具，可供大家一起学习。多编队控制主要是指无人机之间分布式通信和集群之间的控制方式。在建图中传感器可选择双目，RGBD相机，激光雷达，使用的是RTAB-Map或Octomap。深度学习板块中，可实现KCF框选追踪和数字识别功能。

3. Prometheus自主无人机的开发流程

3.1 开发流程

1）硬件的选择

整个开发流程先从硬件选择开始。这就需要我们前面所提到的知识，对于自主无人机组成构架是否有一个清楚地认知，你才能够选择合适的机架、机载电脑和传感器等等来组成属于你自己的无人机。

2）软件（功能）选型

对于软件的选型就是对飞控软件、机载处理软件的选型。机载电脑处理软件这里除了包括ROS，Mavros的功能包还需要Prometheus及其他的功能包。

3.二次开发

一般是不需要用到这一块内容的，目前Prometheus开发出的功能已经很详尽了，可以直接供大家使用。但如果想要了解，需要先行懂得嵌入式开发和Prometheus所有功能，再来进行研究。

4.仿真实验

以上都做好后，我们可以先进行仿真实验，先对代码进行功能测试，测试是否可行，之后进行实物实验，可以节约时间成本。

5.最后一步就是进行实物实验