XTDrone仿真平台示例解析（四）——三维激光SLAM

上一篇二维激光SLAM只能进行平面信息解析，做二维运动规划，若遇到更复杂的环境则无能为力，因此需要获取更多信息。三维激光SLAM可进一步感知无人机周边的空间环境信息，可以帮助无人机更精准地进行三维运动规划。

三维激光SLAM需要安装的依赖包与视觉SLAM类似，若还没跑通视觉SLAM则可以参考这篇文章：XTDrone仿真平台示例解析（二）——视觉SLAM

A-LOAM算法跑velodyneHDL32激光雷达

首先复制开源代码(A-LOAM算法代码以及激光雷达插件)

cp -r ~/XTDrone/sensing/slam/laser_slam/A-LOAM ~/catkin_xtdrone_ws/src/
cp -r ~/XTDrone/sitl_config/gazebo_plugin/velodyne/* ~/catkin_xtdrone_ws/src/

安装依赖包：ceres(点击此链接下载)

sudo apt-get install cmake
sudo apt-get install libgoogle-glog-dev libgflags-dev
sudo apt-get install libatlas-base-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libsuitesparse-dev
tar zxf ceres-solver-2.2.0.tar.gz
mkdir ceres-bin
cd ceres-bin
cmake ../ceres-solver-2.2.0
make -j
sudo make install

 修改A-LOAM代码

cd ~/catkin_xtdrone_ws/src/A-LOAM/src
#将src文件夹下所有.cpp/.hpp文件中出现的下列代码进行修改
#"/camera_init"改为"camera_init"
#"/camera"改为"camera"
#"#include <opencv/cv.h>"改为"#include <opencv2/opencv.hpp>"
#"CV_LOAD_IMAGE_GRAYSCALE"改为"cv::IMREAD_GRAYSCALE"
#"ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization();"改为"ceres::Manifold *q_parameterization = new ceres::EigenQuaternionManifold();"
#如果碰到Eigen错误则将"#include <eigen3/Eigen/Dense>"改为"#include <Eigen/Dense>"

cd ~/catkin_xtdrone_ws/src/A-LOAM
vim CMakeLists.txt
#将"set(CMAKE_CXX_FLAGS "-std=c++11")"改为"set(CMAKE_CXX_FLAGS "-std=c++14")"

改好之后进行编译

cd ~/catkin_xtdrone_ws
catkin build

修改PX4代码

cd ~/PX4_Firmware
cp launch/outdoor1.launch launch/outdoor1_1.launch
vim launch/outdoor1_1.launch
# 将无人机型号由typhoon_h480改为iris，并选择sdf模型为iris_3d_gpu_lidar

修改后的outdoor1_1.launch文件如下

<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor1_light.world"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="true"/>
    <arg name="paused" default="false"/>
    <!-- Gazebo sim -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="gui" value="$(arg gui)"/>
        <arg name="world_name" value="$(arg world)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
    </include>
     <!-- iris_0 -->
     <group ns="iris_0">
        <!-- MAVROS and vehicle configs -->
            <arg name="ID" value="0"/>
            <arg name="ID_in_group" value="0"/>
            <arg name="fcu_url" default="udp://:24540@localhost:34580"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="iris"/>
            <arg name="sdf" value="iris_3d_lidar"/>
            <arg name="mavlink_udp_port" value="18570"/>
            <arg name="mavlink_tcp_port" value="4560"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py  -->

运行px4

roslaunch px4 outdoor1_1.launch

运行A-LOAM算法

roslaunch aloam_velodyne aloam_velodyne_HDL_32.launch

运行效果如下图所示

Faster-LIO算法跑livox固态激光雷达

下载开源代码并解压

cd ~/catkin_xtdrone_ws/src
git clone https://github.com/Luchuanzhao/Livox_simulation_customMsg.git
git clone https://github.com/Livox-SDK/livox_ros_driver.git
git clone https://github.com/gaoxiang12/faster-lio.git
mv Livox_simulation_customMsg livox_laser_simulation

修改livox_laser_simulation代码路径

cd ~/catkin_xtdrone_ws/src/livox_laser_simulation/src
vim livox_points_plugin.cpp
#将第54行的内容改为自己电脑实际的csv文件路径
#笔者的路径为（/home/wangml71/catkin_xtdrone_ws/src/livox_laser_simulation/scan_mode/avia.csv）仅供参考

修改faster_lio配置文件

cd ~/catkin_xtdrone_ws/src/faster-lio/config/
vim avia.yaml

将avia.yaml的前半部分代码修改为

common:
  lid_topic: "/scan"
  imu_topic: "/iris_0/imu_gazebo"
  time_sync_en: false         # ONLY turn on when external time synchronization is really not possible

preprocess:
  lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
  scan_line: 6
  blind: 4
  time_scale: 1e-3

mapping:
  acc_cov: 0.4
  gyr_cov: 0.2
  b_acc_cov: 0.0005
  b_gyr_cov: 0.0001
  fov_degree: 90
  det_range: 200.0
  extrinsic_est_en: false      # true: enable the online estimation of IMU-LiDAR extrinsic
  # extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
  extrinsic_T: [ -0.05, 0, -0.205 ]
  extrinsic_R: [ 1, 0, 0,
                 0, 1, 0,
                 0, 0, 1 ]

改好后进行编译

cd ~/catkin_xtdrone_ws
catkin build

修改PX4代码

cd ~/PX4_Firmware
cp launch/outdoor3.launch launch/outdoor3_1.launch
vim launch/outdoor3_1.launch
# 将无人机型号改为iris，并选择sdf模型为iris_realsense_livox

改好后的outdoor3_1.launch如下

<?xml version="1.0"?>
<launch>
    <!-- MAVROS posix SITL environment launch script -->
    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
    <!-- vehicle model and world -->
    <arg name="est" default="ekf2"/>
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor3.world"/>
    <!-- gazebo configs -->
    <arg name="gui" default="true"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="paused" default="false"/>
    <!-- Gazebo sim -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="gui" value="$(arg gui)"/>
        <arg name="world_name" value="$(arg world)"/>
        <arg name="debug" value="$(arg debug)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="paused" value="$(arg paused)"/>
    </include>
     <!-- iris_0 -->
     <group ns="iris_0">
        <!-- MAVROS and vehicle configs -->
            <arg name="ID" value="0"/>
            <arg name="ID_in_group" value="0"/>
            <arg name="fcu_url" default="udp://:24540@localhost:34580"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="0"/>
            <arg name="z" value="1"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="iris"/>
            <arg name="sdf" value="iris_realsense_livox"/>
            <arg name="mavlink_udp_port" value="18570"/>
            <arg name="mavlink_tcp_port" value="4560"/>
            <arg name="ID" value="$(arg ID)"/>
            <arg name="ID_in_group" value="$(arg ID_in_group)"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py  -->

运行PX4

roslaunch px4 outdoor3_1.launch

运行faster-lio算法

roslaunch faster_lio mapping_avia.launch

运行效果如下

值得注意的一点，在~/catkin_xtdrone_ws/src/livox_laser_simulation/urdf/livox_avia.sdf文件的68行是采样数量，极其消耗计算资源，笔者的笔记本电脑是16G内存，1650显卡，intel-i7-9750H处理器，把采样数量降低到5000才勉强跑起来，在此提醒大家根据配置情况合理设置sample值。