无人机自主飞行仿真(一) Gazebo+PX4+VINS+FastPlanner

maofaw

今年年后在学校做了一部分四旋翼无人机仿真的工作，仿真环境用的是Gazebo，主要利用港科大的VINS和Fast-Planner实现Px4无人机自主飞行，下面直接进入正题：
1.参考

港科大：
https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
https://github.com/HKUST-Aerial-Robotics/Fast-Planner
阿木实验室Prometheus：
https://github.com/amov-lab/Prometheus
肖昆的XTDrone：
https://gitee.com/robin_shaun/XTDrone
知乎张碧辉老师的一篇文章也对Fast-Planner进行了复现，并进行了实机的测试，在此感谢张老师的指导。
2.仿真系统介绍

(0) 系统结构及主要数据流向
本文所做的工作主要是对几个模块的整合，并根据参考中的代码框架手写了任务与控制模块，将无人机实时状态读取出来并在rviz中进行了可视化。系统的总体结构和主要数据流向如下图：


下面根据这个结构图大概介绍下每个模块：
(1) 四旋翼无人机仿真环境
无人机仿真采用Gazebo+Px4的方式，网上有很多参考，因此关于Px4仿真环境的搭建不做过多介绍。我在这里用的是带有深度相机的无人机模型，可以获取左右目RGB图像、深度图像和IMU数据(实际使用时可以使用d435i这类相机)。
(2) 定位与轨迹规划
关于这一部分做SLAM的朋友一定非常熟悉，采用了港科大沈老师组开源的VINS-Fusion和Fast-Planner，当然使用单目的VINS-Mono也是可以的。需要说明的就是VINS估计出来的位姿要通过"/mavros/vision_pose/pose"这个话题发布给无人机，并且PX4需要修改EKF配置采用视觉定位的方式。 根据上面的系统结构图，FastPlanner要接收的是VINS估计的位姿和深度图像，这里的位姿实际上包括两个，一个是body的pose，另一个是camera的pose(用于和深度图结合创建局部点云)。当然Planner也可以直接输入点云，但因为我后面想用d435i，所以这里就直接使用深度图加相机pose的输入方式，可以根据配置文件自己修改。
(3) 控制模块
该模块就是图中的Mission部分，主要处理Planner规划的轨迹，并转换成PX4可接收的Mavros消息给飞控。之所以叫Mission而不是直接叫Control，是模仿了阿木实验室的Prometheus项目，方便后面扩展其它功能的时候使用。但是阿木实验室的开源项目包含的内容太多，这里我只借鉴他们的代码框架，重新写了我需要的部分。
控制策略：Planner规划的轨迹包括了位置、速度和加速度以及偏航角指令，通过Mavros我们可以获取无人机的当前状态(包括位置速度等数据)。我采用的控制策略是将规划的期望速度作为前馈，期望位置与无人机当前位置作差后作为反馈，乘上一定系数并求和后作为期望速度发送给无人机。采用这种方式在仿真中看起来效果还可以，但由于一直忙于其他事，目前没有实机验证过，欢迎测试过的朋友提些建议给我或者交流下实验部分，感谢。
可以看到，我目前在仿真中采用的这种方式没有利用加速度数据，飞机动态性能肯定不会很好，后面有时间会考虑添加其它控制率进行仿真和实机的测试，感兴趣的朋友可以自己实现。想要实现效果比较好的轨迹追踪，一个思路是拿位置、速度做PID，结合加速度前馈得到期望加速度，最后转换成期望姿态和油门给无人机，不知道在Px4，"Mavros+Offboard"模式下效果怎么样，欢迎大佬和小伙伴们分享经验，再次感谢。
3.Gazbeo仿真演示

这不，前几天上英文专业课要做汇报，正巧就把这段仿真视频剪辑了一下放上了，第一次剪辑，有什么不到位的地方大家积极提意见。

Gazebo+Px4+VINS+FastPlanner
https://www.zhihu.com/video/1383575421993201664
4.存在的问题及后续工作
(1) 缺乏实机的飞行实验：虽然之前使用d435i进行了部分测试，但由于一直忙于其他事情，并没有在四旋翼上进行飞行测试，后续有时间的话考虑补充部分实验。
(2) 控制策略改进空间大：仅使用期望位置和速度数据肯定不能达到最优的效果，这部分的改进主要看后面测试结果，不过毕竟不是专门做控制的，我想自己不会花太多时间在这上面。
(3) 后续可能考虑加入动态物体的识别，追踪功能，不过可能还是要先看最终自己课题方向了。

cityboy

大佬，这个Gazbeo能仿真出飞机轨迹对应的imu、图片话题给vins-mono吗？

cccvip

可以的呀，可以用现成的IMU和相机插件，参考http://gazebosim.org/tutorials?tut=ros_gzplugins

yjwfgihc

你好 请问一下gazebo中的树木是怎么构建出来的？有相关方面的教程或者说明么？我现在想搭建一个复杂的丛林环境！

896953053

你好，我是直接下载的gazebo模型库，用的里面的模型，你可以直接搜 gazebo模型下载之类的 网上很多教程有问题再交流

sunflower2018

非常感谢您能够回复！我现在按照网上的教程下载了gazebo模型库，链接如下：gazebo模型下载以及配置_sunshine-CSDN博客_gazebo模型库，gazebo中能够正常加载模型，但是我仔细找了一下，好像没有树木模型，请问您gazebo中的环境是自己搭建的还是有一个完整的模型包就是上面视频中所示的环境？然后是不是我的模型包有缺失，所以没有树木模型？如果是不知道您是否方便把您下载的模型库链接发给我一下？万分感谢！

ytyt666

是之前下载的模型库不完整，不用麻烦您回复了，谢谢

zelon1234

解决问题了就好！

xubenben0313

你好，请问一下你搭建这些环境时主要参考啥，刚开始学还不太明白[大笑]

macheng0824

你好，你指的环境是gazebo仿真的环境还是整个运行环境？，仿真的环境如果不需要特别定制的话用gazebo自带的模型直接拖就可以，当然可能需要先下载。如果是整个代码运行环境的话参考ubuntu下ROS，SLAM，px4等基本环境搭建

ofuzlj

您好，请问您的前馈和反馈参数用的是多少？

maoxuesong

仿真的代码在学校台式机里[捂脸]，代码还不完善就没有上传，这个参考意义也不大吧

tangxin

您好，我想和您一样用自己的环境来做仿真，请问是在kino_replan.launch文件中进行相应的修改么？请问进行哪些方面的修改？[捂脸]

赋明通

官方有说明，我记得一个是里程计话题一个是深度图和相机的话题，就在文件最开始

AWWREJ

您好，请问仿真中的相机 内参外参 是怎么做的呢？

风履

能否尽快把代码共享下？不完善大家也可以帮忙修改的呢，这就是开源的意义嘛[大笑]

zelon1234

感觉跟图像仿真不太一样啊大佬

macheng0824

你好，请问您是如何处理fastplanner生成的轨迹来控制无人机的？

zhoulinchun01

博主您好，我看您用的是fastplanner中的topo_replan方法，不知道您试过kino_replan算法没有，我试了kino_replan算法，在gazebo中的轨迹弯弯曲曲的，不知道您遇到过这种情况没？

lihuaxun88

【无人机自主导航10 自主导航】PX4仿真环境下RGBD+港科大Fast-Planner旋翼无人机自主导航 – DGZC的小创元世纪  --------  我是根据这个网页内容搭建的环境