LIO-SAM是基于因子图构建的激光雷达惯性里程计，可以将大量的相对测量值、绝对测量值、回环等多种不同数据作为因子融入激光雷达惯性里程计系统中。IMU预积分的运动估计被用来去处激光雷达运动畸变，并为激光雷达惯性里程计的优化提供初值。获得的激光惯性里程计的结果反过来用作估计IMU的偏差。为了确保实时性与高性能，进行位姿优化时边缘化掉了一些旧的激光雷达数据，而不是将激光雷达点云与整个地图进行匹配。在局部范围而不是全局范围进行扫描匹配可以有效提高系统的实时性，选择性地引入关键帧和高效的滑动窗口也能提高实时性能。

局限性：LIO-SAM对输入数据精度较为敏感，由于采用局部范围扫描匹配，导致在局部过窄或特征不足时，出现建图偏移现象。

3. 什么是点云降采样

三维点云往往包含大量冗余数据，直接处理计算量大，消耗时间长，因此对其进行降采样是十分必要的。降采样同时也是点云预处理过程中的关键环节。采样算法里，比较常用的是体素网格下采样。

3.1 体素网格下采样原理

体素（Voxel）：将三维空间划分成一个个立体的方格，每个方格就叫一个体素。

在每个体素中可能存在几个点，也可能没有点。降采样的思路为：检查每个体素中是否有点存在，若有，则用一个点代替体素内的点集，通常，这个采样点可以是体素中所有点坐标的平均值（质心），也可以是中心点或者离中心点最近的点。

3.2 流程

计算点云的包围盒，将包围盒离散成小体素。体素的长宽高尺寸可以通过用户设定，也可以指定三个方向上的体素个数；
获取落在每个体素中的点集，在每个体素中取一个采样点代替原来的点集。

3.3 特点

效率高
采样点分布比较均匀
可以通过控制体素大小间接控制采样点的距离（采样后点云的稀疏程度）
采集到的点云数量不可控

3. 基于Autoware.AI的ndt

Autoware 建图用的坐标系之间的关系，Autoware 用的坐标系有四类：世界坐标系 world，地图坐标系 map，小车坐标系base_link，传感器坐标系 velodyne，它们之间的关系如图所示：

从 world 到 map 的坐标系转换与从 base_link 到 velodyne 的坐标系转换是固定的，用 ROS 的 TF 即可。从 map 到 base_link 的映射就需要正态分布变换（NDT）算法，autoware 在建图采用的是 ndt_mapping。

4. 建图环境

Lidar：雷神16线（格式与velodyne一致）
IMU：H30
底盘：WheelTech阿克曼底盘
OS：Ubuntu 20.04
Autoware：autoware.ai docker

二、准备工作

1. 编译Autoware.ai

源码编译只支持到ubuntu18，20.04版本直接上docker。

1.1 安装Docker

参考：docker入门

参考：Docker—将容器导出为镜像并push到hub仓库

1.2 部署Autoware.ai

为了方便，可以直接用这个已经适配好的镜像：

docker login
docker pull yanjingang/autoware:latest-melodic-cuda

也可以参考Installation手动部署官方镜像并进行修改：

# source code
git clone https://github.com/autowarefoundation/autoware_ai_docker.git autoware.ai.x86

# run docker
cd ~/autoware.ai.x86/generic
./run.sh  --cuda on          # use cuda

# 备份官方的Autoware代码
mv Autoware Autoware.bak

# 修改docker启动脚本，挂载本地的Autoware源码目录
vim ~/autoware.ai.x86/generic/run.sh
    SHARED_DOCKER_DIR=/home/autoware/autoware_shared_dir
    SHARED_HOST_DIR=$HOME/autoware_shared_dir

    AUTOWARE_DOCKER_DIR=/home/autoware/Autoware
    # AUTOWARE_HOST_DIR=$HOME/Autoware
    VOLUMES="--volume=$XSOCK:$XSOCK:rw
        --volume=$XAUTH:$XAUTH:rw
        # --volume=$AUTOWARE_HOST_DIR:$AUTOWARE_DOCKER_DIR:rw
        --volume=$SHARED_HOST_DIR:$SHARED_DOCKER_DIR:rw"

# 增加启动脚本
vim ~/autoware.ai.x86/open.sh
    bash -c "cd /home/work/autoware.ai/docker/generic/;sudo ./run.sh --cuda on"

容器内编译和运行Autoware：

# 进入容器
cd ~/autoware.ai.x86/
./open.sh

# 编译Autoware目录源码
cd ~/Autoware
catkin_make install

# 启动autoware
source ~/Autoware/install/setup.bash
roslaunch runtime_manager runtime_manager.launch

# 保存容器内的变化
docker ps
docker commit -a "yanjingang" -m "autoware.ai latest-melodic-cuda" 1fa529a5fa31 yanjingang/autoware:latest-melodic-cuda-v2

# 使用新镜像
vim ~/autoware.ai.x86/generic/run.sh
    IMAGE_NAME="yanjingang/autoware"
    TAG_PREFIX="latest"

2. 传感器数据

需要准备以下3个topic的输出

2.1 雷达：/points_raw

Autoware 默认使用 velodyne 雷达，默认订阅的雷达话题为/points_raw，雷达话题的 frame_id 为 velodyne。使用雷达驱动包时，雷达话题名称与 frame_id 必须与上述的一致。帧率20hz。

2.2 IMU：/imu_raw

默认订阅的IMU话题为/imu_raw，帧率200hz。

2.3 小车里程计：/vehicle/odom

Autoware 中 Localization 自定位模块使用正态变换算法来匹配雷达当前帧和3D 地图来实现定位，同时也可以勾选使用里程计来辅助定位，默认订阅里程计话题为/vehicle/odom 话题，使用到里程计定位时，需发布话题为/vehicle/odom 的里程计话题，帧率20hz。

2.4 RTK：/gps/fix

G70+4GDTU插入上网卡并配置千寻账号后，可以输入厘米级的RTK数据，发布的话题为/gps/fix，帧率5hz。

三、录制ROSBAG

1. 启动底盘+传感器+Autoware

# 1. 启动底盘+传感器
roslaunch turn_on_wheeltec_robot open_autoware.launch

# 检查topic帧率
rostopic hz /imu_raw /points_raw /vehicle/odom /gps/fix
# 检查topic时间戳是否正常
rostopic echo /imu_raw /gps/fix -p

# 2. 启动Autoware
cd ~/autoware.ai.x86/
./open.sh
source ~/Autoware/install/setup.bash
roslaunch runtime_manager runtime_manager.launch

2. 录制 ROSBAG

录制 ROSBAG 主要是为了后面的建图以及生成导航的路径点文件需要。Autoware 建图方式是利用 rosbag 包进行离线建图，因此我们先录数据（注：录制的过程中，适当控制车速，尽量避免压坑或车身剧烈抖动，会影响建图效果）。

需要录制的话题列表：

IMU话题 /imu_raw
雷达话题 /points_raw
里程计话题 /vehicle/odom
RTK话题 /gps/fix

在 Autoware 界面中，点击[ROSBAG]按钮，在弹出来的界面中点击[Ref]选择 ROSBAG 的存储路径（docker 安装方式的注意保存在 autoware_shared_dir 目录下面），然后勾选话题，点击“start”即可开始录制。

点击开始录制后，可以通过底盘遥控或ros键盘控制命令控制小车移动。
录制完成后点击[stop]按钮便可结束录制，录制的 bag 文件会自动保存。

录制后，必须关闭Autoware和底盘程序，否则会影响接下来的建图。

四、使用ROSBAG建图

1. 重启Autoware

播包不能与底盘topic同时输出，所以这里需要把底盘节点关闭掉，把Autoware节点重启一下：

# 重启Autoware
roslaunch runtime_manager runtime_manager.launch

2. 加载从base_link到Lidar坐标系的TF、小车模型

点击进入【Setup】菜单，分别设置Localizer、TF、Vehicle Model：

在【Localizer】中选择Velodyne。
在【Baselink to Localizer】中设置好各个参数，其中 x、y、z、yaw、pitch、roll表示真车雷达中心点与车身后轴中心点的 TF 位置关系，可根据雷达实际位置进行修改（对于镭神 16 线雷达z参数设置为0.38，yaw 参数设置成-1.57），设置好之后点击【TF】按钮。
docker版本的urdf小车模型在 autoware_shared_dir/urdf 目录下面，选择后点击【Vehicle Model】加载模型（如果Vehicle Model为空，那么会加载一个默认模型，在 rviz 显示时，如果有激光雷达数据，车辆会显示为黑色）。

3. 设置从 world 到 map 转换

点击【Map】菜单，加载 world 到 map 的坐标转换：

点击【TF】右侧的【ref】按钮，选择~/autoware_shared_dir/lgsvl-tf.launch 文件后
点击【TF】按钮加载该文件，以加载 world 到 map 的坐标转换

cat ~/autoware_shared_dir/lgsvl-tf.launch
    <launch>
        <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map 10" />
    </launch>

4. 配置并启动ndt-mapping（map到 base_link的转换）

在【Compulting】菜单栏找到【lidar_localizer】下的【ndt_mapping】选项，先点击其右侧的【app】设置一些参数（对 app 内的参数数据修改完成时，必须按回车键，左侧的滑动栏位置发生变化后数据才能保存成功）。

Ndt_Mapping 有一些参数设置需要注意一下：

Resolution：ndt 分辨率，最大建图精度，默认为 1 即可，室外范围较大时建议设为2
Step size：步长，默认为 0.1
Transformation epsilon：两次正态变换允许的最大值，默认为 0.01 米
Maximum iterations：最大迭代次数，默认为 30 次
Leaf size：点云降采样用的体素叶大小，室内可设置小点，室外可设置大点0.5-1
Minimum scan range：设置雷达最小范围，过滤较近处雷达点云，推荐设置小点：0.15 米
Maximum scan range：设置雷达最大范围，过滤较远处雷达点云，默认 200米
在[method type]栏中，勾选[pcl_anh]选项（选其中一个，最终节点能成功运行即可。运行失败可以尝试勾选其他选项。目前测试使用[pcl_anh_gpu]无法显示建图进度，用[pcl_anh]选项正常）。
如果要用到里程计或imu辅助定位的，把[use odometry]、[use imu]选项勾选上。

设置完参数点击【Close】按钮后，勾选【ndt_mapping】以启动ndt建图模块。

5. 加载点云数据.bag文件

点击Autoware的【Simulaton】 Tab页，点击界面右上方【Ref】按钮，加载上一步骤录制的 bag 文件。
点击【Play】按钮开始播放数据，此时右边会显示播包进度条，在命令行窗口也可以看到ndt的建图进度。

命令行最下边的建图进度部分，左边数字为已处理的数据，右侧的数字为总的数据。
如果是嵌入式设备，二者相差太大程序有可能未处理数据积攒太多导致卡死，推荐相差 200 左右就按下【Pause】按钮暂停播放数据，等待当前数据处理完成就再次点击【Pause】按钮继续播放数据即可。
如果系统负载很低则可以忽略，ndt算法看起来并行处理性能上不是很好，耐心等待建图完成即可。

7. 保存 3D 点云地图

7.1 检查建图进度

等待点云数据加载完毕，命令行停止打印相关日志时，建图过程即完成：

7.2 保存PCD地图

回到【Compulting】菜单栏的【ndt_mapping】选项，点击其右侧的【app】，在【ref】中选择点云地图保存的路径后，点击【PCD OUTPUT】按钮即可保存点云地图（注意：docker版本的需要保存在~/autoware_shared_dir目录下面）。

7.3 查看PCD地图

保存完地图之后，可以通过pcl_viewer查看当前建图效果。

cd ~/autoware_shared_dir/bag/map-2/
pcl_viewer autoware-241118.pcd

五、使用ROSBAG/PCD生成路径点

1. 重启Autoware

同上一章

2. 加载从base_link到Lidar坐标系的TF、小车模型

同上一章

3. 设置从 world 到 map 转换

同上一章

4. 加载点云地图 PCD 文件

打开【map】页面，点击【point cloud】右侧的【ref】按钮，选择上一步建图保存的3D点云地图.pcd文件
点击【point cloud】按钮，此时下方会出现一个进度条，当进度条显示加载了 100％并出现[OK]字样时，证明 3D 点云地图已经成功加载完毕

5. 点云降采样过滤

打开【sensing】页面，找到【points_downsampler】下的【voxel_grid_filter】选项，设置[app]的一些参数后，勾选[voxel_grid_filter]。 [voxel_grid_filter]有一些参数设置如下图：

Voxel Leaf size：体素降采样算法中的体素叶大小，室内可设置小点，室外可设置大点

6. 设置从 map 到 base_link 的转换（ndt_matching）

找到【Computing】左菜单栏下的【ndt_matching】选项，设置【app】里面的参数后，勾选【ndt_matching】，一些参数设置如下所示：

使用RTK时，【topic:/config/ndt】勾选【GNSS】。没有使用 GNSS时，【topic:/config/ndt】勾选【Initial_Pose】，x，y，z，roll，pitch，yaw 的值表示小车的初始位置（默认建图起点位置）。
在[method type]栏中，无 GPU 加速的选【pcl_generic】，有 GPU 加速的选[pcl_anh_gpu]。
用到里程计和 imu 辅助定位，需要把【use odometry】和【useimu】选项勾选上，其余参数参考默认值就可以了。

7. 启动[vel_pose_connect]

找到【Computing】左菜单栏下的【vel_pose_connect】
打开【app】并确保选项 [Simulation_Mode] 没有被勾选。
退出并勾选【vel_pose_connect】。

8. 设置路径点文件保存路径

找到【Computing】右菜单栏下的【waypoint_saver】，打开【app】。
点击【ref】选择保存的路径（注意： docker 版本的需要保存在~/autoware_shared_dir 目录下面），勾选上【save/current_velocity】。
退出并勾选【waypoint_saver】。

9. 加载点云数据.bag文件

点击Autoware的【Simulaton】 Tab页，点击界面右上方【Ref】按钮，加载上一步骤录制的 bag 文件。
点击【Play】按钮开始播放数据，等待数据包回放结束，路径点文件会自动保存在设置好的路径下。

11. rviz 查看路径点

在路径点生成过程中，源码安装方式的可以点击界面右下角的[rviz] 打开rviz，相关配置在~/autoware.ai/src/autoware/documentation/autoware_quickstart_examples/launch/rosbag_demo/default.rviz 中（docker 版本的需要在小车上运行 rviz 单独打开，相关配置在~/autoware_shared_dir/default.rviz 中）。此时我们可以看到 rviz 中逐渐出现一条路径：

六、总结

好了，今天带大家尝试了lidar+imu+rtk+ndt的建图过程，整体建图效果看起来还不错，下一次带大家使用这个地图进行定位和导航。如果没啥问题，我们就拉到公园里溜溜，看看特征点不明显的场景效果怎么样。

参考：

点云降采样（DownSampling）

建立地图专栏之5.2：三维全局地图的开源方案及对比–NDT、LOAM、LIO-SAM、ALOAM、FLOAM、Lego_loam、SC-Lego-LOAM

【Autoware入门教程】如何使用NDT构建点云地图

Autoware.AI

Autoware AI Generic-x86-Docker

docker pull失败，报connect: connection refused

ubuntu20.04镜像方法安装Autoware.ai

Autoware.AI ROSBAG 演示

欢迎关注下方“非著名资深码农“公众号进行交流～

一、概述

1. NDT算法原理

2. LIO-SAM算法原理

3. 什么是点云降采样

3.1 体素网格下采样原理

3.2 流程

3.3 特点

3. 基于Autoware.AI的ndt

4. 建图环境

二、准备工作

1. 编译Autoware.ai

1.1 安装Docker

1.2 部署Autoware.ai

2. 传感器数据

2.1 雷达：/points_raw

2.2 IMU：/imu_raw

2.3 小车里程计：/vehicle/odom

2.4 RTK：/gps/fix

三、录制ROSBAG

1. 启动底盘+传感器+Autoware

2. 录制 ROSBAG

四、使用ROSBAG建图

1. 重启Autoware

2. 加载从base_link到Lidar坐标系的TF、小车模型

3. 设置从 world 到 map 转换

4. 配置并启动ndt-mapping（map到 base_link的转换）

5. 加载点云数据.bag文件

7. 保存 3D 点云地图

7.1 检查建图进度

7.2 保存PCD地图

7.3 查看PCD地图

五、使用ROSBAG/PCD生成路径点

1. 重启Autoware

2. 加载从base_link到Lidar坐标系的TF、小车模型

3. 设置从 world 到 map 转换

4. 加载点云地图 PCD 文件

5. 点云降采样过滤

6. 设置从 map 到 base_link 的转换（ndt_matching）

7. 启动[vel_pose_connect]

8. 设置路径点文件保存路径

9. 加载点云数据.bag文件

11. rviz 查看路径点

六、总结

发表评论 取消回复

发表评论取消回复