其他分享
首页 > 其他分享> > 基于VINS与FastPlanner的无人机自主飞行Gazebo仿真

基于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就可以看到点云，如下：

效果如下：

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


 

参考文章：

  1. https://www.e-learn.cn/topic/3906130   从零实现vins-mono+fast-planner+M100无人机实验在现实场景中的应用
  2. https://blog.csdn.net/weixin_44580210/article/details/89789416 从零开始使用Realsense D435i运行VINS-Mono
  3. https://blog.csdn.net/AnChenliang_1002/article/details/109535355 从零实现vins-mono+fast-planner+M100无人机实验在现实场景中的应用
  4. https://blog.csdn.net/weixin_48068495/article/details/115515100 Fast-Planner安装

标签：cmake,local,sudo,glog,opencv,FastPlanner,VINS,Gazebo,usr
来源： https://blog.csdn.net/qq_38167930/article/details/118343026