最近在公众号上看到Prometheus无人机的资料，发现里面开源了很好的无人机的仿真环境，并且有很好的教程。而本人正好在上《Introduction to Aerial Robotics》的课程，正好搭建一下仿真环境来测试一下。写下本博文作为学习记录用。（PS：无任何商业或者宣传之用，仅仅为本人学习记录用，相关资料给出参考。先给出一个视频，汇总了本博文中本人做仿真的结果。
基于Prometheus自主无人机开源项目的学习与仿真
目录
Prometheus - 自主无人机开源项目
项目配置
PX4编译环境配置及固件代码安装
Gazebo仿真运行测试
代码框架的介绍
控制模块
进行轨迹跟踪的仿真
多机编队飞行
PID控制
运动规划模块
基于A*的全局路径规划
SLAM
RTAB Map建图
Octomap建图
参考资料
https://github.com/amov-lab/Prometheus
https://github.com/amov-lab/Prometheus/wiki/%E4%BB%BF%E7%9C%9F%E9%A3%9E%E8%A1%8C%E9%85%8D%E7%BD%AE
https://github.com/amov-lab/Prometheus/wiki/%E5%AE%89%E8%A3%85%E5%8F%8A%E7%BC%96%E8%AF%91
Prometheus项目支持软件仿真及机载飞行实验，目前只开源全部仿真代码及基础机载飞行代码。下面我们先看看如何配置仿真环境。
首先是Ubuntu 18.0与ros melodic这是最基本的
安装Mavros包
sudo apt-get install ros-melodic-mavros ros-melodic-mavros-extras安装GeographicLib(可以通过sudo chmod 777来获取某个文件的权限)
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
sudo sh ./install_geographiclib_datasets.sh
建图模块依赖项安装。安装rtabmap。之前博客《ROS实验笔记之——XBOX Kinect2 的使用及基于RTAB-MAP的建图》也已经用过了
sudo apt-get install ros-melodic-rtabmap*建图模块依赖项安装。安装Octomap
sudo apt-get install ros-melodic-octomap-*激光SLAM依赖项安装。安装cartographer
sudo apt-get install ros-melodic-cartographer*视觉检测依赖项安装（目前ROS仅支持python2，所以确保你的CUDA版本为10.1或9.2）
# 首先查看CUDA版本
nvidia-smi
# CUDA 10.1
pip2 install torch==1.4.0 torchvision==0.5.0
# CUDA 9.2
pip2 install torch==1.4.0+cu92 torchvision==0.5.0+cu92 -f https://download.pytorch.org/whl/torch_stable.html
# CPU only，如果没有Nvidia显卡，则安装CPU版本（影响检测性能）
pip2 install torch==1.4.0+cpu torchvision==0.5.0+cpu -f https://download.pytorch.org/whl/torch_stable.html关于pip2报错：pip升级后Import Error:cannot import name main解决方案_ZONGXP的博客-CSDN博客
关于如何使用vim：Linux vi/vim | 菜鸟教程
规划模块依赖项安装(安装非线性优化工具箱 NLopt，安装方法如下)
git clone git://github.com/stevengj/nlopt
cd nlopt
mkdir build
cd build
cmake ..
make
sudo make install Prometheus项目下载
git clone https://github.com/amov-lab/Prometheus
也可以使用国内Gitee源(我们会定期搬运更新!)
git clone https://gitee.com/amovlab/Prometheus.gitPrometheus项目编译。打开至Prometheus项目根目录，并执行对应的编译脚本
cd Prometheus
sudo chmod 777 ./compile_all.sh (第一次运行才需要执行此赋权命令)
./compile_all.sh应该成功啦？
Prometheus项目中的Gazebo仿真模块依赖PX4固件及sitl_gazebo包，因此需先配置PX4编译环境。
下载PX4固件代码，此处使用阿木实验室的Prometheus项目专用的PX4仓库：prometheus_px4，安装方法如下
git clone https://gitee.com/amovlab/prometheus_px4.git
cd prometheus_px4
git submodule update --init --recursive
pip3 install --user toml empy jinja2 packaging
make amovlab_sitl_default gazebo最后一句一直报错。试试把python3卸载，然后只用python2
sudo apt autoremove python3
然后奔溃了。。。。参考资料见：Ubuntu 16.04 卸载自带 Python3 引发的惨案_python学习者的博客-CSDN博客
由于ubuntu依赖于python，一删，系统几乎就是奔溃。。。重新装吧hhh
查看所有的python版本update-alternatives --list python
pip2 install --user toml empy jinja2 packaging尝试了一下下面的：
make clean
make px4_sitl gazebo
sudo apt-get install libprotobuf-dev libprotoc-dev protobuf-compiler libeigen3-dev libxml2-utils python-rospkg python-jinja2
sudo apt-get install libgstreamer-plugins-base1.0-dev gstreamer1.0-plugins-bad gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-ugly -y
好像还是不行，先运行下面的
PX4编译环境安装（第一次安装PX4环境需要运行）
cd prometheus_px4/Tools/setup
## 请在运行此脚本时注意终端的打印信息，保证每一项均顺利安装
## 若有某一项失败或一直卡住，请重新运行即可！
source ./ubuntu.sh
再运行一次
make amovlab_sitl_default gazebo还是不行。。。换台电脑，再重新跑上面的所有步骤。。。
然后好像还是不行，先
cd prometheus_px4/Tools/setup
source ./ubuntu.sh之后，再运行，好像久可以了？
此处安装成功的标志为：PX4固件能够编译，并能运行其自带的Gazebo仿真，即运行make px4_sitl gazebo能够正常运行Gazebo仿真
但好像还是不正常。。。。
*****************************************************************************************************************************************************
仿真用插件安装
安装3Dlidar插件
sudo apt-get install ros-melodic-velodyne-gazebo-plugins安装octomap在rviz中的插件
sudo apt-get install ros-melodic-octomap-rviz-plugins运行一下
make px4_sitl gazebo正常运行，但是什么都没有。。。。
然后添加如下路径
source ~/Prometheus/devel/setup.bash
export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:~/Prometheus/devel/lib
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/Prometheus/Simulator/gazebo_simulator/models
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/Prometheus/Simulator/gazebo_simulator/amov_models
source ~/prometheus_px4/Tools/setup_gazebo.bash ~/prometheus_px4 ~/prometheus_px4/build/amovlab_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/prometheus_px4
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/prometheus_px4/Tools/sitl_gazebo
Prometheus仿真功能包编译
确保Prometheus项目中其他相应的功能包均已编译通过，然后编译prometheus_gazebo功能包
cd Prometheus
./compile_gazebo.sh暂时正常
运行如下命令测试是否Gazebo仿真是否正确配置
roslaunch prometheus_gazebo sitl.launch此时，第一个终端同时运行了PX4仿真、Mavros、px4_pos_estimator、px4_pos_controller四个节点，第二个终端则运行了ground_station节点。因此，若第一个终端无报错，第二个终端显示[Connected]并能够查看到飞机状态，且Gazebo成功运行，代表成功运行。
运行如下指令启动终端控制节点，并根据终端提示输入指令
rosrun prometheus_control terminal_control正常！
运行下面命令启动控制节点
rosrun prometheus_control terminal_control一般情况下，首先输入999解锁并切换至offboard模式，然后输入1起飞，然后再根据提示输入其他指令（悬停、降落、机体系移动、惯性系移动、轨迹追踪等等）
注意：每次修改过代码后，都需要运行下面命令来对代码进行编译
cd Prometheus
sudo chmod 777 ./compile_all.sh (第一次运行才需要执行此赋权命令)
./compile_all.sh （或者单独的某个脚本）
| Modules | 各模块功能的源代码 |
| Simulator | 仿真飞行相关代码 |
| Experiment | 真实飞行相关代码 |
| Modules | 目录 | 功能 |
|---|---|---|
| 控制模块 | Modules/control | 底层控制 |
| 规划模块 | Modules/planning | 路径规划 |
| SLAM模块 | Modules/slam | 定位与建图 |
| 目标检测模块 | Modules/object_detection | 目标识别与检测 |
| 地面站模块 | Modules/ground_station | 地面监控与消息打印 |
| 任务模块 | Modules/mission | 上层任务 |
| 公用模块 | Modules/common | 各模块公用代码 |
| 自定义消息模块 | Modules/common/msgs | 本项目自定义消息 |
而在仿真环境下，关键的是在目录（/home/kwanwaipang/Prometheus/Simulator/gazebo_simulator）下的代码
| gazebo_simulator/amov_models | 特定模型文件 |
| gazebo_simulator/models | 通用模型文件 |
| gazebo_simulator/worlds | 地图文件 |
| gazebo_simulator/cpp_nodes | 辅助的cpp节点 |
| gazebo_simulator/py_nodes | 辅助的python节点 |
| gazebo_simulator/launch_xxxx | 各类启动文件 |
| gazebo_simulator/config | 仿真相关的配置文件 |
prometheus_control模块中（/home/kwanwaipang/Prometheus/Modules/control/src）,有px4_sender.cpp和px4_pos_controller.cpp 两大控制方式,其中px4_sender.cpp只是给飞控发送各种期望值(实际控制算法运行于飞控中),px4_pos_controller.cpp 发送期望姿态值给飞控(位置环控制算法位于Prometheus代码中),因此如果要对位置环代码进行二次开发,则需要使用px4_pos_controller.cpp .
在仿真中，可以控制位置。位置控制器的代码实现位于Modules/control/include/Position_Controller文件夹中，目前共有五种位置环控制器。因此可以通过仿写的方式，新增自定义位置环控制器，并在px4_pos_controller.cpp中进行调用。
/home/kwanwaipang/Prometheus/Modules/control/src/Utilities/terminal_control.cpp中的Controller_Test类进行位置环控制器性能测试，目前提供圆形、8字形、阶跃三种形式的轨迹追踪测试。
/home/kwanwaipang/Prometheus/Simulator/gazebo_simulator/config/prometheus_control_config/terminal_control.yaml中对轨迹的形状参数进行了设置
roslaunch prometheus_gazebo sitl_pos_control.launch/home/kwanwaipang/Prometheus/Simulator/gazebo_simulator/launch_control/sitl_pos_control.launch
无人机的轨迹为红色，真实的轨迹为绿色
上面视频中出现了圆形跟踪出错的问题，感觉是圆的半径设置太小了，设置大一些稍微好点，如下图所示
然后再运行8字轨迹跟踪如上图所示
有两种控制方式：
roslaunch prometheus_gazebo sitl_swarm_control_5uav.launch
Proportional Integral Derivative，实际上就是对调节器控制进行比例、积分、微分控制。
增大比例系数P将加快系统的响应，它的作用于输出值较快，但不能很好稳定在一个理想的数值，不良的结果是虽较能有效的克服扰动的影响，但有余差出现，过大的比例系数会使系统有比较大的超调，并产生振荡，使稳定性变坏。积分能在比例的基础上消除余差，它能对稳定后有累积误差的系统进行误差修整，减小稳态误差。微分具有超前作用，对于具有容量滞后的控制通道，引入微分参与控制，在微分项设置得当的情况下，对于提高系统的动态性能指标，有着显著效果，它可以使系统超调量减小，稳定性增加，动态误差减小。
PID控制详解_tingfenghanlei的博客-CSDN博客_pid控制
以飞行器为例子，加入个人的思考如下：
入下所示的控制器：
x为状态，u为控制输入；误差定义为：
整个控制系统应该如下所示：
对于控制器的公式，期望的状态值
可以看成是一个前馈的输入，而后面的误差则是期望输入与真实状态的差。
首先，对于比例系数Proportional而言，希望调节到的是状态差最小。比如有一个需要跟踪的路径（考虑一个一阶的系统，输入是速度，目标是位置），我们希望最终输出的结果跟要跟踪的路径，越接近越好。通过定义两个状态（位姿）的误差，可以使得两者as close as possible。那么会存在如下：
这个比例系数很好理解，就是我们飞行器目前的状态跟期望的状态有一个error，那么我们通过kp将飞行器拉到我们的期望位置。这个系数实现了how the control to react the error，the larger the error，the bigger the control signal to reduce the error。所以这个系数主要是处理超调以及不稳定的情况。直观理解就是，在某个directuion上有误差，那么就通过误差及比例系数，将error draw back。对于一个一阶系统而言，就是速度跟位置误差的关系。
但是实际上大部分都是一个二阶或者多阶系统，如下所示。考虑一个二阶的系统，输入是加速度，目标的状态是位置。
在上面的公式中，有三项。第一项是期望的加速度。属于前馈控制。也有比例项，比例项很好理解，就是如果我们的状态在某个direction下有error，我们通过比例项将将robot拉回来。而对于Derivative项，则是跟error的微分相关。那么直观理解就是，如果误差很大，但是如果误差不改变，那么这个微分项会是0，但如果这个error改变得很快，无论是增大很快还是减少很快，这个微分项还是很大，那么这样看来，这个微分项的作用，更像是让系统不要改变得太快。这样理解的话，应该是：比例系数让系统的状态误差尽可能小，而Derivative则是让系统变化不要太快，比如不要一下冲到Overshoot
到上面为止，实际上就是较为常用的PD控制，而PID控制，整体入下所示：
PID相当于一个三阶的反馈系统。而里面的积分项Integral，则是为了让系统的 steady-state error go to zero，换句话说，就是让稳态的误差尽可能的小
个人的理解是，如果你拥有一个Quadrotor在飞，然后有风力，这样的情况下，如果你只有PD control，就会出现一个很尴尬的情况就是，风力让你的无人机飞到一边，那么你通过PD control，D control使机器人的状态变化不要太大，P control使得你的无人机的状态回到原来的位置。但是当无人机回到原来的位置时，误差为0，没有比例的控制输入，同时微分项也是0，那么无人机又飞出去。那么最终，虽然还是会达到一个动态平衡的状态，但是需要有一个error来抵御风力。那么这种情况就是，只要风力存在，这个机器人就永远不会回到目标点，这就是所谓的稳态误差 steady-state error，这个error永远存在，而不会减少到0。而这个时候，通过引入一个I control，使得稳态误差减少到0.只要误差存在，只要这个error不是0，这个积分项就会一直增大，直到而当error为0的时候，这个积分项就会保持，从而保证了稳态误差为0.这里很重要的一点是，跟比例项不一样，当error为0，比例项就没有了，但是对于积分项，当erro慢慢减少到0的时候，积分项是一个常数。而这个常数就提供了一个稳态下的控制信号给无人机。
关于运动模块这里，编译一下发现有错误：
cd Prometheus
sudo chmod 777 ./compile_planning.sh (第一次运行才需要执行此赋权命令)
./compile_planning.sh （或者单独的某个脚本）
应该就是FastPlanner这个包有点问题，先采取不用它，注释掉用其他的吧～
对于路径规划，比较常用的应该是A*全局路径规划，而局部路径规划有APF、滑动窗口。
运行命令如下
roslaunch prometheus_gazebo sitl_astar_3dlidar.launch//3D LiDAR
roslaunch prometheus_gazebo sitl_astar_2dlidar.launch//2D LiDAR
roslaunch prometheus_gazebo sitl_astar_2dlidar_gps.launch//GPS
roslaunch prometheus_gazebo sitl_astar_rgbd.launch//RGBD
可以选择不同的传感器进行障碍物的检测？
下面是用rgbd摄像头的效果
基于APF（人工势场）的局部路径规划
人工势场法APF是局部路径规划的一种比较常用的方法。这种方法假设机器人在一种虚拟力场下运动。人工势场包括引力场合斥力场，其中目标点对物体产生引力，引导物体朝向其运动。障碍物对物体产生斥力，避免物体与之发生碰撞。物体在路径上每一点所受的合力等于这一点所有斥力和引力的和。这里的关键是如何构建引力场和斥力场。
在此仿真中，将会使用激光雷达作为传感器，运行代码如下
roslaunch prometheus_gazebo sitl_apf_3Dlidar.launch 发现并没有这个脚本。。。。
那试试RGBD camera的吧～
roslaunch prometheus_gazebo sitl_apf_rgbd.launch
之前博客《ROS实验笔记之——XBOX Kinect2 的使用及基于RTAB-MAP的建图》也介绍过RTAB Map建图
RTAB-Map | Real-Time Appearance-Based Mapping
RTAB Map(Real-Time Appearance-Based Mapping)是一种基于增量式外观特征进行回环检测的SLAM框架，可以支持包括RGB-D，双目和激光雷达在内的多种传感器进行定位和建图。RTAB-Map使用词袋的方式判断新接收到的图像有多大可能是来自一个新位置或是曾经到过的位置，以此来完成回环检测，当检测到回环时，新的约束会被加到地图中，然后通过图优化的方式来最小化误差。RTAB-Map会使用内存管理的方法，限制用于回环检测和图优化的位置数量以保证在大规模环境中的实时性。RTAB-Map可以使用手持的RGB-D相机、双目相机或三维激光雷达进行六自由度建图。
整个流程如下图所示
关于RTAB map在ROS的应用可以参考（rtabmap_ros - ROS Wiki）
这是RTAM-Map的ROS包的核心节点，包含了RTAB-Map的核心库，地图的增量式构建、地图优化和回环检测就是在这个节点内进行的。通过订阅cloud_map，grid_map（proj_map）的话题，可以获得三维点云和二维占据地图。另外，RTAB-Map的数据库存储在路径"~/.ros/rtabmap.db"下，可以通过指令"--delete_db_on_start"在每次启动时进行删除，否则将会加载前一次的数据库。
运行下面命令来测试建图的效果
roslaunch prometheus_gazebo sitl_rtabmap.launch点云地图具有占据空间大，更新不够方便的特点，这决定了其难以直接用于导航使用，而Octomap就是一种基于稠密点云的新的地图表达方式，相比点云而言，具有占据空间小，可以方便的更新的优点，可以用于机器人的导航。
roslaunch prometheus_gazebo sitl_octomap.launchhttps://github.com/amov-lab/Prometheus