基于VINS与FastPlanner的无人机自主飞行Gazebo仿真
2021/7/2 23:52:59
本文主要是介绍基于VINS与FastPlanner的无人机自主飞行Gazebo仿真，对大家解决编程问题具有一定的参考价值，需要的程序猿们随着小编来一起学习吧！
项目来源及展示：
https://www.bilibili.com/video/BV1WK4y1V7um?from=search&seid=12548150687335659873
基本思路：采用Gazebo+PX4进行仿真，定位导航使用港科大的VINS和FastPlanner方案。
其实目前有两个思路：一个是将《从零实现vins-mono+fast-planner+M100无人机实验在现实场景中的应用》改编成vins-mono+fast-planner；另一个思路是做仿真，基于GAAS做仿真，使用vins-fusion+fast-planer完成项目。
目录
1. Fast-Planner简单安装试用
2. VINS试用
2.1 安装测试librealsense SDK 2.0
2.2 安装测试realsense
2.3 安装测试VINS-Mono
1、卸载glog
2、安装OpenCV
2.4 在D435i上运行VINS-Mono
卸载melodic
3. GAAS试用
3.1
1. Fast-Planner简单安装试用
代码地址：https://github.com/HKUST-Aerial-Robotics/Fast-Planner#4-run-simulations
Fast-Planner的下载安装;
sudo apt-get install libarmadillo-dev ros-melodic-nlopt
cd ${YOUR_WORKSPACE_PATH}/src
git clone https://github.com/HKUST-Aerial-Robotics/Fast-Planner.git
cd ../
catkin_make
*注：ros-${ROS_VERSION_NAME}-nlopt，不同版本名字不同，这里用的是melodic。
编译后，开始可视化：
source devel/setup.bash roslaunch plan_manage rviz.launch
然后开始模拟（打开新终端运行）：
source devel/setup.bash roslaunch plan_manage kino_replan.launch
会在Rviz找到随机生成的地图和无人机，此时，可以使用2D Nav Goal工具为无人机选择要达到的目标。点击2D Nav Goal，然后在地图中点击一个点时，一个新的轨迹将立即生成并由无人机执行。
执行结果如下：
给定2D Nav Goal后：
2. VINS试用
代码地址：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
2.1 安装测试librealsense SDK 2.0
安装步骤在：https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md
这里将自己的安装步骤写下来进行记录：
（1）升级：注意下面的第三部分不需要使用，使用的话会造成内核升级！！！
sudo apt-get update && sudo apt-get upgrade # && sudo apt-get dist-upgrade
（2）选择源码编译方式，通过下面命令下载源码：
git clone https://github.com/IntelRealSense/librealsense.git
（3）准备linux后端环境，我的系统是ubuntu18.04：
sudo apt-get install libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev at
（4）在librealsense文件夹下，运行下面的命令：
./scripts/setup_udev_rules.sh
（5）编译内核补丁：
./scripts/patch-realsense-ubuntu-lts.sh
（6）编译librealsense源码：
mkdir build && cd build cmake ../ -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=true
（7）安装：
make -j8 && sudo make install
（8）在终端中输入：
realsense-viewer
即可打开软件，安装版本可以在界面打开时看见：
2.2 安装测试realsense
这个realsense指的是对应的realsense相机的ROS包，官方教程如下ROS Wrapper for Intel® RealSense™ Devices
（1）这里就很简单啦，先进入你的ROS工作空间：
cd ~/catkin_ws/src git clone https://github.com/intel-ros/realsense.git cd .. catkin_make rospack profile source devel/setup.sh
（2）运行（不加filters:=pointcloud好像没有点云的节点）
roslaunch realsense2_camera rs_camera.launch filters:=pointcloud
（3）打开rviz，添加PointCloud2的mudule就可以看到点云，如下：
- 选定Fixed Frame：camera_depth_frame；
- 添加PointCloud2作为显示选项，选定/camera/depth/color/points作为话题Topic。
- 也可以再添加image作为显示选项，选定/camera/color/image_row作为话题Topic。
效果如下：
2.3 安装测试VINS-Mono
这一步先用数据集测试VINS-Mono，同样，进入你的ROS工作空间：
cd ~/catkin_ws/src git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git cd .. catkin_make source ~/catkin_ws/devel/setup.bash
这里编译可能需要安装ceres，安装ceres的步骤自行百度就好，编译源码的同时可以把数据集给下了，测试用的bag文件会比较方便，数据集地址。
按照理想的步骤，下面应该是进行愉快的测试，使用如下命令进行测试：
roslaunch vins_estimator euroc.launch roslaunch vins_estimator vins_rviz.launch rosbag play MH_01_easy.bag
但是安装过程中，发现编译通过，但是无法运行，问题出在VINS-Mono与opencv上，安装的opecv版本不对，glog也有问题。最终通过卸载glog，安装opencv3.3.1版本成功跑通：
1、卸载glog
（1）安装和卸载glog
//安装 sudo apt-get install libgoogle-glog-dev //卸载 sudo apt-get remove libgoogle-glog-dev
（2）源码安装glog：源码安装较为简单，在build目录下，去https://github.com/google/glog下载源码。Solution： Installation methods using this link:glog cmake installation method，安装步骤是：
cd glog cmake -H. -Bbuild -G "Unix Makefiles" cmake --build build cmake --build build --target test # 这一步不需要执行 sudo cmake --build build --target install
这里安装的glog可能造成opencv有undefined symbol错误。由于glog并没有提供uninstall命令，所以我的思路是去安装路径下把对应的include和lib都删除掉。
示例：
sudo rm -rf /usr/local/include/glog/ sudo rm -rf /usr/local/lib/libglog*
如果不知道glog安装位置，不妨用用locate命令：
~$ locate glog /usr/local/include/glog /usr/local/include/glog/config.h /usr/local/include/glog/log_severity.h /usr/local/include/glog/logging.h /usr/local/include/glog/raw_logging.h /usr/local/include/glog/stl_logging.h /usr/local/include/glog/vlog_is_on.h /usr/local/lib/libglog.a /usr/local/lib/cmake/glog
2、安装OpenCV
下载OpenCV-3.3.1版本，去https://github.com/opencv/opencv/releases/tag/下载OpenCV源码，使用下面的命令进行安装：
mkdir build cd build cmake .. make -j8 sudo make install
之后所有的步骤都可以跑通。结果如下图：
有趣的是，数据集里面可以看到测试人员开始的时候拿着摄像头前后左右晃了晃，是为了初始化IMU与相机的外参用的。
2.4 在D435i上运行VINS-Mono
前面都测试好之后就可以再D435i上运行VINS-Mone了，这里特地感谢下博客如何用Realsense D435i运行VINS-Mono等VIO算法 获取IMU同步数据的作者Manii，让这个过程变得非常简单，如果大家需要对这个过程有一个更加详细的了解可以去参考那个博客，研究出来下面的步骤还是挺费时间的，这里我简单说下流程：
1、修改realsense包里的rs_camera.launch文件
第一处，修改unite_imu_method如下，这里是让IMU的角速度和加速度作为一个topic输出：
<arg name="unite_imu_method" default="copy"/>
第二处，修改enable_sync参数为true，这里是开机相机和IMU的同步：
<arg name="enable_sync" default="true"/>
第三处，允许相机发布陀螺仪和加速度计消息，这里不修改，VINS-Mono中右侧没有轨迹运动：
<arg name="enable_gyro" default="true"/> <arg name="enable_accel" default="true"/>
2、修改VINS-Mono包里的realsense_color_config.yaml文件
第一处，修改订阅的topic：
imu_topic: "/camera/imu" image_topic: "/camera/color/image_raw"
第二处，修改相机内参，这里先再次打开运行realsesne包，运行如下命令：
roslaunch realsense2_camera rs_camera.launch
通过如下命令获取相机内参：
rostopic echo /camera/color/camera_info
结果打印如下：
zlc@jm:~/catkin_ws/src/realsense$ rostopic echo /camera/color/camera_iofo
header:
seq: 0
stamp:
secs: 1625229784
nsecs: 82322359
frame_id: "camera_color_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [905.2868041992188, 0.0, 634.6292724609375, 0.0, 906.0779418945312, 367.53692626953125, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [905.2868041992188, 0.0, 634.6292724609375, 0.0, 0.0, 906.0779418945312, 367.53692626953125, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
---
header:
seq: 1
第三处，IMU到相机的变换矩阵，这里我根据注释的提示修改成2：
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
#If you choose 0 or 1, you should write down the following matrix.
第四处，IMU参数，这里我全部修改注释给的参数：
#imu parameters The more accurate parameters you provide, the better performance acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2 gyr_n: 0.05 # gyroscope measurement noise standard deviation. #0.05 acc_w: 0.02 # accelerometer bias random work noise standard deviation. #0.02 gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 g_norm: 9.80 # gravity magnitude
第五处，是否需要在线估计同步时差，根据上述博主的建议这里选择不需要：
#unsynchronization parameters estimate_td: 0 # online estimate time offset between camera and imu td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
第六处，相机曝光改成全局曝光：
#rolling shutter parameters rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
第七处，更改图像大小：
image_width: 1280 image_height: 720
这里不更改。若遇到运行roslaunch vins_estimator realsense_color.launch报OpenCV的错误，错误如下：
OpenCV Error: Assertion failed (_mask.empty() || (_mask.type() == CV_8UC1 && _mask.sameSize(_image))) in goodFeaturesToTrack,
则十有八九是roslaunch realsense2_camera rs_camera.launch提供的图像height,wight和vins-mono需要的height,wight不一致，一定要注意到这一点。仔细查看上面打印的数据，实际上发现D435i的height和width是720和1280，需要在vins-mono的realsense_color_config.yaml中改。
第八处，更改输出目录：
output_path: "/home/zlc/catkin_ws/src/VINS-Mono/output/"
3、打开摄像头，运行VINS-Mono
roslaunch realsense2_camera rs_camera.launch roslaunch vins_estimator realsense_color.launch roslaunch vins_estimator vins_rviz.launch
结果如下：
下面是一些零零碎碎的环境安装遇到的问题，也顺带进行记录，方便日后查看：
ubuntu下的opencv版本切换：
1、假如安装了opencv以下两个版本：
opencv-2.4.13
opencv-3.2.0
2、安装目录分别为：
/usr/local/opencv-2.4.13
/usr/local/opencv-3.2.0
3、opencv-2.4.13切换为opencv-3.2.0
默认版本为/usr/local/opencv-2.4.13
4、打开终端，输入以下命令：
（1）打开文件：
sudo gedit ~/.bashrc
（2）在文件末尾添加以下内容，然后保存：
export PKG_CONFIG_PATH=/usr/local/opencv-3.2.0/lib/pkgconfig
export LD_LIBRARY_PATH=/usr/local/opencv-3.2.0/lib
（3）输入命令：
source ~/.bashrc
（4）继续输入：
pkg-config --modversion opencv pkg-config --cflags opencv pkg-config --libs opencv
以上步骤完成，即为替换完成，如果想从opencv-3.2.0切换回去，步骤同上。
查看opencv版本：
（1）python下查看：
python
import cv2
cv2.__version__
（2）查看其他用opencv版本：
pkg-config --modversion opencv
（3）在下面文件中查看具体使用的opecv版本，这个影响最大：
cat /usr/local/include/opencv2/core/version.hpp
顺便补充以下查看Eigen版本的方法：
/usr/local/include/eigen3/Eigen/src/Core/util/Macros.h
在这个文件中可以看见用的eigen版本。
Eigen是一个高层次的C ++库，有效支持线性代数，矩阵和矢量运算，数值分析及其相关的算法
Glog是google提供的一个开源的C++日志库.
gflags：google开源的gflags是一套命令行参数解析工具，还支持从环境变量、配置文件读取参数（可用gflags代替配置文件）。
ceres solver：google开源项目，可以求解两类问题：1、带有边界约束的非线性最小二乘问题。2、一般的无约束优化问题。https://ceres-solver.googlesource.com/ceres-solver/+/1.13.0 点击tgz下载，我用opencv官网链接中git方式获取的ceres solver 配置失败，用此连接下载的配置成功
卸载opencv：
（1）首先要找到当初安装opencv的build目录（如果没有需要重新编译），进入该build目录执行卸载操作：
sudo make uninstall cd .. sudo rm -r build
（2）卸载其余：
sudo rm -r /usr/local/include/opencv2 /usr/local/include/opencv /usr/include/opencv /usr/include/opencv2 /usr/local/share/opencv /usr/local/share/OpenCV /usr/share/opencv /usr/share/OpenCV /usr/local/bin/opencv* /usr/local/lib/libopencv
time_out 时间更改：
首先查看延时：
ping raw.githubusercontent.com
然后更改时间：
sudo vim /usr/lib/python2.7/dist-packages/rosdep2/sources_list.py
常见错误：
1、
CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by
"jsk_recognition_msgs" with any of the following names:jsk_recognition_msgsConfig.cmake
jsk_recognition_msgs-config.cmake/
解决方案：
sudo apt-get install ros-melodic-jsk-recognition-msgs & sudo apt-get install ros-melodic-jsk-rviz-plugins
2、
CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by "move_base" with
any of the following names:move_baseConfig.cmake
move_base-config.cmake
解决方案：
sudo apt install ros-melodic-navigation
3、
CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by
"ddynamic_reconfigure" with any of the following names:ddynamic_reconfigureConfig.cmake
解决方案：
sudo apt-get install ros-melodic-ddynamic-reconfigure
卸载melodic
sudo apt-get remove ros-melodic-*
3. GAAS试用
代码地址：https://github.com/generalized-intelligence/GAAS
3.1
在运行过程中会遇到下面的错误：
Could not find a package configuration file provided by "glog" with any of the following names:
glogConfig.cmake
glog-config.cmake
解决办法：安装glog
I went tohttps://github.com/google/glogDownload Library, and installed in accordance write home installation method. We found that the problem is not solved.
Solution： Installation methods using this link:glog cmake installation method
Concrete is:
cd glog cmake -H. -Bbuild -G "Unix Makefiles" cmake --build build cmake --build build --target test # 这一步不需要执行 sudo cmake --build build --target install
参考文章：
- https://www.e-learn.cn/topic/3906130 从零实现vins-mono+fast-planner+M100无人机实验在现实场景中的应用
- https://blog.csdn.net/weixin_44580210/article/details/89789416 从零开始使用Realsense D435i运行VINS-Mono
- https://blog.csdn.net/AnChenliang_1002/article/details/109535355 从零实现vins-mono+fast-planner+M100无人机实验在现实场景中的应用
- https://blog.csdn.net/weixin_48068495/article/details/115515100 Fast-Planner安装
这篇关于基于VINS与FastPlanner的无人机自主飞行Gazebo仿真的文章就介绍到这儿，希望我们推荐的文章对大家有所帮助，也希望大家多多支持为之网！
- 2024-01-24手把手教你使用MDK仿真调试
- 2024-01-10基于“小数据”的机器学习
- 2024-01-08扩展卡尔曼滤波：提高机器学习性能的利器
- 2023-12-26各种二端口滤波器网络仿真遇到的问题
- 2023-12-14机器学习-搜索技术：从技术发展到应用实战的全面指南
- 2023-12-12机器学习 - 决策树：技术全解与案例实战
- 2023-12-05机器学习-学习率：从理论到实战，探索学习率的调整策略
- 2023-12-04解锁机器学习-梯度下降：从技术到实战的全面指南
- 2023-11-30回归算法全解析！一文读懂机器学习中的回归模型
- 2023-11-30机器学习 - 似然函数：概念、应用与代码实例
