ROS2+cartographer+激光雷达+IMU里程计数据融合(robot_locazation) 建图

目录

• 写在前面
• 总体流程
• 分块解释
• • IMU数据接收和发布
  • 车轮编码器数据接收和发布
  • 数据融合------robot_localization
  • • 概括
    • 使用
  • cartographer订阅
• 效果

写在前面

之前写了一篇ROS2+cartorgrapher+激光雷达建图并保存，但是由于其只对激光雷达的数据进行订阅，这就导致了其建图在室内会有一个较好的效果(但是也会出现偏差)，在室外完全无法使用。究其原因，是因为只用激光雷达且没有比较明显的建筑障碍物的话，cartographer很难计算出一个比较精准的位置和朝向。

因此，为了达到一个更好的建图效果，我们使用了robot_localization包，对IMU和里程计的数据进行融合，并将其发布，使得cartographer的建图效果更上一层楼。且基本上不会出现漂移等问题。

我们也尝试了直接将编码器的值输出为odom让cartographer订阅来建图，但是那样的效果非常差劲，而融合了里程计和IMU数据之后，再建图的效果就非常好了
环境：

• Ubuntu22.04
• ROS2 humble
• 激光雷达：镭神激光雷达M10P 网口版
• cartographer robot_localization IMU 里程计

总体流程

在写具体的使用方法之前，先把我们整套系统的整体流程进行概括。
IMU数据 robot_localization 车轮编码器数据 转化为里程计数据 输出odom系融合数据 cartographer订阅并建图 激光雷达数据

整体概述而言，就是将两个传感器的数据利用robot_locazation包进行融合，让cartographer可以将其订阅，最终得到一个较好的建图效果。

因此，需要了解的是以下几点

1. IMU数据的接收和发布
2. 车轮编码器数据的接收并转换为里程计数据
3. 数据融合------robot_localization
4. cartographer订阅

分块解释

IMU数据接收和发布

IMU使用的是GY-95T，成本不到100元，三轴(yaw pitch roll)误差不大。底层数据接收驱动和在ROS2中的发布是自己手写的，具体可以参考之前写的这篇博客

ROS2手写接收IMU数据(Imu)代码并发布

车轮编码器数据接收和发布

我们的车是差速轮驱动+两全向轮，且重量较大。因此我们直接用差速轮的编码器值来建立里程计信息。车轮编码器底层的返回值这里不在赘述，这里主要写是如何从编码器的值转换成里程计信息并通过odom发布。

在网上直接找到了编码器转里程计发布的代码，链接如下

https://github.com/jfstepha/differential-drive/blob/master/scripts/diff_tf.py

我们直接将这个项目中的diff_ty.py文件拿出来稍微修改参数就可以使用，减少了工作量。我们可以设置其中的每米多少转，两轮宽距，发布话题名等参数。默认的发布话题名字为odom，但是由于我还需要将里程计的数据进行融合，因此，我将其发布的话题名字改为odom_diff

数据融合------robot_localization

概括

这个包是从ROS开发出来至今一直在广泛使用的包，效果也非常的不错。这里先把官方的wiki文档贴出:

https://docs.ros.org/en/melodic/api/robot_localization/html

既然是ROS2集成好的包，那自然就需要和nav2一样，先将其安装下来。

sudo apt install ros-humble-robot-localization

使用该包的方法也非常简单，只需要调用该包中的某些我们需要的节点即可。该算法其实能够融合非常多的传感器

官方给出该算法图片表明：其实该算法能融合IMU，相机，GPS，里程计和其他3D感知传感器得到的数据，由于我们成本有限，我们仅融合了IMU和里程计的数据。

使用

说完了概括，那么接下来就要来说明我用到的该包中的节点，robot_localization中有两个用于融合传感器数据的两种不同的算法节点ekf_localization_node和ukf_localization_node，它们的主要区别在于使用的滤波器算法不同。

• ekf_localization_node使用的是扩展卡尔曼滤波器（EKF），它是一种适用于非线性系统的滤波器。当机器人或车辆的运动模型和传感器模型呈现非线性时，EKF可以通过对非线性函数进行线性化来逼近真实状态，并将观测数据和运动模型结合起来进行状态估计。但是，EKF也存在一些缺点，例如需要事先了解运动模型和传感器模型的具体形式，并且在非线性程度较高的情况下可能存在不稳定性。

• ukf_localization_node使用的是无迹卡尔曼滤波器（UKF），它也是一种适用于非线性系统的滤波器，但是它不需要对非线性函数进行线性化。UKF通过使用一种称为无迹变换的方法，将概率分布从状态空间转换到高斯空间，从而避免了线性化过程中可能出现的不稳定性问题。

因此，ekf_localization_node和ukf_localization_node的主要区别在于它们使用的滤波器算法不同。对于非线性程度较低的情况，EKF通常可以提供较好的性能；对于非线性程度较高的情况，UKF可能更适合使用。我们使用的是扩展卡尔曼滤波器(UKF)。

因此，我们和使用其他包一样使用ros2 run package package_name即可，为了便于启动，我将其放入launch文件中

import launch_ros
import launch 

def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='your_package_name').find('your_package_name')

	robot_localization_node = launch_ros.actions.Node(
	         package='robot_localization',
	         executable='ekf_node',
	         name='ekf_filter_node',
	         output='screen',
	         parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': False}]
	    )
	return launch.LaunchDescription([
	robot_localization_node,
	# ...
	# other node
	])

了解完了使用的是哪一个节点之后，我们就需要配置该节点的启动和其对应的参数文件。ROS2中的参数文件一般都是用yaml文件形式给出的。根据上方launch文件给出的形式，我们将扩展卡尔曼滤波节点的配置参数放置到src/your_package_name/config/ekf.yaml当中，配置文件的示例如下：

### ekf config file ###
ekf_filter_node:
    ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
        frequency: 30.0
        
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
        two_d_mode: false

# Whether to publish the acceleration state. Defaults to false if unspecified.
        publish_acceleration: true

# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
        publish_tf: true
        
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame" 
#    to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark 
#    observations) then:
#     3a. Set your "world_frame" to your map_frame value
#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node 
#         from robot_localization! However, that instance should *not* fuse the global data.
        map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom           # Defaults to "odom" if unspecified
        base_link_frame: base_link  # Defaults to "base_link" if unspecified
        world_frame: odom           # Defaults to the value of odom_frame if unspecified
        # odom0: /odom_diff
        # odom0_config: [true,  true,  true,
        #                false, false, false,
        #                false, false, true,
        #                false, false, false,
        #                false, false, false]
        odom0: /odom_diff
        odom0_config: [false,  false,  false,
                       false, false, false,
                       true, true, true,
                       false, false, false,
                       false, false, false]

        imu0: /imu_data
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      false, false, false,
                      false, false, false]

后面的参数文件矩阵含义是我们可以在上面的话题中提供给robot_localization的数据，矩阵的各个配置值的含义为(X,Y,Z,roll,pitch,yaw,X˙,Y˙,Z˙,roll˙,pitch˙,yaw˙,X¨,Y¨,Z¨)。由于之前用到的diff_ty.py文件发布出来的数据是带有方向的XYZ速度值，因此需要将X˙,Y˙,Z˙这三个参数对应的矩阵设置为true，上面的话题odom0: /odom_diff是指robotlocalization需要订阅的话题名字，我们里程计数据是由diff_ty.py发布的odom_diff话题，因此在这里改为对应话题名字即可。

同理，对于IMU数据，我IMU驱动底层写的话题发布名字为imu_data，因此，我们也将其订阅的imu话题名字改为imu_data即可。而我们在这里只用到了IMU的roll pitch yaw三轴数据，因此是将第二排的三个参数改为true即可。

而上面的4个frame不需要进行修改，因为我们需要建立的就是map, odom, base_link三个系之间的关系。

其他注释可以跟着官方给出的注释并结合自己的实际情况来进行修改即可。

我们在进行完以上步骤之后，robot_localization就可以输出融合后的数据了，但是需要注意的一点是，robot_localization输出的话题叫做/odometry/filtered

cartographer订阅

我们已经可以输出融合后的数据了，现在就是修改cartographer的参数来让其订阅融合后的数据即可。我们需要修改的参数有

.lua配置参数文件中以下参数改为true

use_odometry = true

其他参数文件和我前面一篇博客的参数文件都一致

launch文件中，将remapping参数进行修改

    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        arguments = [
            '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files',
            '-configuration_basename', 'my_robot.lua'],
        remappings = [
            ('odom', '/odometry/filtered')],
        output = 'screen'
        )

其意思是remap标签将节点的输入话题/odom重映射到/odometry/filtered，以便Cartographer节点可以正确接收odom数据。

然后再重新启动即可

最终的rqt_tree如下图所示

效果

这是融合前后的里程计可视化的效果图，可以发现平滑了很多，且方向准确了很多。

这是融合了所有数据后的建图效果，把整个楼都建出来了，但是会有些角度上的漂移。即弯了。

但是相比于没有数据融合的建图效果，已经是好了太多了。