FAST LIO是目前较为通用的雷达与IMU紧耦合导航定位算法，对于四足机器人来说高精度的导航有利于局部高程图的构建并实现自主越障和落足点选择，高精度的里程计也为自主导航提供了可靠的反馈数据，原始的FastLIO不具有回环功能的，这里运行的是由ETH RSL实验室修改的版本，其实现回环检测因此可以更加利于室内狭窄区域内的自主机动：
代码

依赖：

  • Ubuntu 18.04 and ROS Melodic
  • PCL >= 1.8 (default for Ubuntu 18.04)
  • Eigen >= 3.3.4 (default for Ubuntu 18.04)
  • GTSAM >= 4.0.0(tested on 4.0.0-alpha2)

1. 仿真安装与测试
首先在虚拟机中完成数据集的测试，FastLIO安装很简单下载代码后建立工作空间完成catkin_make即可，但是仍然需要提前安装几个依赖。

（1）错误1：GTSAM安装

Could not find a package configuration file provided by "GTSAM" with any of
  the following names:

    GTSAMConfig.cmake
    gtsam-config.cmake

下载代码https://github.com/borglab/gtsam/archive/4.0.0-alpha2.zip，进行编译和安装：

mkdir build && cd build
cmake ..
cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON ..
sudo make install

（2）错误2：tf2-sensor
安装：sudo apt-get install ros-melodic-tf2-sensor-msgs

（3）Livox依赖：

下载git clone https://github.com/Livox-SDK/Livox-SDK.git，进行编译

（4）fatal error: livox_ros_driver/CustomMsg.h: No such file or directory

由于源码中包含了多个文件，建议先完成livox_ros_driver的编译后进行source在进行其他部分的编译，或者多次进行catkin_make编译

下面进行测试，首先需要下载数据集：
链接：https://pan.baidu.com/s/1ZG9DM1u1VZd-0FrGyy0r7A
提取码：pi70
—来自百度网盘超级会员V3的分享

另外数据集发布的IMU话题不一致，需要修改如下：

之后就行按例子运行：

roslaunch fast_lio mapping_velodyne.launch
roslaunch aloam_velodyne fastlio_velodyne_VLP_16.launch
rosbag play  T3F2-2021-08-02-15-00-12.bag

测试结果如下，不知道是不是由于虚拟机原因在一开始的时候会出现一些问题，但是在首次回环后正常：

测试问题：
如下图所示雷达定位虽然具有较好的的鲁棒性，但是图中出现了Z轴的数据跳跃，整体轨迹连续性也一般，Z轴数据跳跃最终将会反馈到局部高程图出现高低的噪声，如何解决这个问题还需要进一步学习，另外足式机器人越障机动大，需要融合足式里程计才能保证长时间的导航一致性和轨迹平滑精度！

  1. 实物测试
    采用实车进行了测试，底盘采用即将推出的开源无人车辆底盘，采用前驱动后万向轮设计，采用微雪的直驱电机，实现扭矩闭环控制，雷达采用VLP16雷达，IMU采用WheelTec N100，这样坐标系不需要进行配置，底盘采用24V供电，雷达默认供电为12V，IMU通过JetsonNX完成采集，并安装ROS Noetic运行算法：

测试结果如下：视频

实验结果：采用Fastlio相比LioSam等算法在系统配置和标定方面大大简化，只要坐标系和偏差不是完全错误，基本可以实现非常准确的定位，借助LC回环检测可以实现较大范围内的高精度定位，同时也保障了Z轴高度不会发生较大的偏差。下图为VLP16雷达与N100 IMU的坐标系定义，但是在配置文件中并不需要修改变换关系：