【无人机自主导航9 自主导航】PX4仿真环境下的浙大VINS-Fusion+ego-Planner旋翼无人机自主导航

发布于 2021-04-03  3088 次阅读

前言

配置Ceres

Start by installing all the dependencies.

# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev libgflags-dev
# BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# Eigen3
sudo apt-get install libeigen3-dev
# SuiteSparse and CXSparse (optional)
sudo apt-get install libsuitesparse-dev

We are now ready to build, test, and install Ceres.

到官网下载：http://ceres-solver.org/installation.html

或是git clone https://ceres-solver.googlesource.com/ceres-solver

tar zxf ceres-solver-2.0.0.tar.gz
sudo mkdir ceres-bin &&  cd ceres-bin
sudo cmake ../ceres-solver-2.0.0
sudo make -j3
sudo make test

#可以选择安装Ceres，也可以使用CMake导出，
#CMake允许在不需要安装的情况下使用Ceres，更多信息请参阅文档#中的EXPORT BUILD DIR选项。

sudo make install

bin/simple_bundle_adjuster ../ceres-solver-2.0.0/data/problem-16-22106-pre.txt使用DENSE_SCHUR线性求解器，此Ceres最多可运行10次迭代。 输出应如下所示:


iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  4.185660e+06    0.00e+00    1.09e+08   0.00e+00   0.00e+00  1.00e+04       0    7.59e-02    3.37e-01
   1  1.062590e+05    4.08e+06    8.99e+06   5.36e+02   9.82e-01  3.00e+04       1    1.65e-01    5.03e-01
   2  4.992817e+04    5.63e+04    8.32e+06   3.19e+02   6.52e-01  3.09e+04       1    1.45e-01    6.48e-01
   3  1.899774e+04    3.09e+04    1.60e+06   1.24e+02   9.77e-01  9.26e+04       1    1.43e-01    7.92e-01
   4  1.808729e+04    9.10e+02    3.97e+05   6.39e+01   9.51e-01  2.78e+05       1    1.45e-01    9.36e-01
   5  1.803399e+04    5.33e+01    1.48e+04   1.23e+01   9.99e-01  8.33e+05       1    1.45e-01    1.08e+00
   6  1.803390e+04    9.02e-02    6.35e+01   8.00e-01   1.00e+00  2.50e+06       1    1.50e-01    1.23e+00

Ceres Solver v2.0.0 Solve Report
----------------------------------
                                     Original                  Reduced
Parameter blocks                        22122                    22122
Parameters                              66462                    66462
Residual blocks                         83718                    83718
Residual                               167436                   167436

Minimizer                        TRUST_REGION

Dense linear algebra library            EIGEN
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                     DENSE_SCHUR              DENSE_SCHUR
Threads                                     1                        1
Linear solver threads                       1                        1
Linear solver ordering              AUTOMATIC                22106, 16

Cost:
Initial                          4.185660e+06
Final                            1.803390e+04
Change                           4.167626e+06

Minimizer iterations                        6
Successful steps                            6
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                            0.261

  Residual evaluation                   0.082
  Jacobian evaluation                   0.412
  Linear solver                         0.442
Minimizer                               1.051

Postprocessor                           0.002
Total                                   1.357

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.769766e-09 <= 1.000000e-06)

VINS-Fusion

简介

VINS-Fusion是基于优化的多传感器状态估计器，可为自主应用（无人机，汽车和AR / VR）实现准确的自定位。 VINS-Fusion是VINS-Mono的扩展，它支持多种视觉惯性传感器类型（单摄像机+ IMU，立体摄像机+ IMU，甚至仅立体摄像机）即VINS-Fusion可以进行单目 + IMU, 双目 + IMU 或只有双目的多传感器融合定位，另外也可以融进GPS信息

编译

cp -r ~/XTDrone/sensing/slam/vio/VINS-Fusion ~/catkin_ws/src/
mkdir ~/catkin_ws/scripts/
cp ~/XTDrone/sensing/slam/vio/xtdrone_run_vio.sh ~/catkin_ws/scripts/
cd ~/catkin_ws
catkin_make #或catkin build，取决于自己的编译工具

quadrotor_msgs

cd ~
git clone https://github.com/HKUST-Aerial-Robotics/plan_utils.git
cp -r plan_utils/quadrotor_msgs catkin_ws/src/
cd catkin_ws
catkin_make
source devel/setup.bash

报错：

-- Could NOT find quadrotor_msgs (missing: quadrotor_msgs_DIR)
-- Could not find the required component 'quadrotor_msgs'. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found.
CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "quadrotor_msgs"
  with any of the following names:

    quadrotor_msgsConfig.cmake
    quadrotor_msgs-config.cmake

ego-Planner安装

cp -r  ~/XTDrone/motion_planning/3d/ego_planner ~/catkin_ws/src/
cd ~/catkin_ws/
catkin_make #或catkin build

无人机自主导航

ego_planner需要输入深度图+相机位姿或是点云，这里以深度图+相机位姿的组合为例进行仿真，深度图来源于realsense_camera，相机位姿由VINS-Fusion计算得到。
首先按照VINS-Fusion仿真教程启动仿真，将飞机用键盘控制起飞后悬停，关闭键盘控制。
转换相机位姿的坐标系方向

cd ~/XTDrone/motion_planning/3d
python ego_transfer.py iris 0

启动rviz

cd ~/XTDrone/motion_planning/3d
rviz -d ego_rviz.rviz

启动ego_planner

roslaunch ego_planner single_uav.launch

file

视频效果：