沉默杀手HomeArchivesTagsCategories
laser_scan_matcher
|激光SLAMICP和NDT|
Word count: 1.1k|Reading time: 4 min

laser_scan_matcher是一个增量式的scan配准工具. 在连续的sensor_msgs/LaserScan消息之间进行scan match, 把laser的估计位姿以geometry_msgs/Pose2D消息发布，产生tf变换。

激光里程计可以不需要轮式里程计和IMU，也可以使用二者加快配准速度。可接受sensor_msgs/LaserScan或者sensor_msgs/PointCloud2消息

当每次新的scan产生时（每次进入scan的回调函数时），如果为传感器当前的位姿提供初值(使用轮式里程计和IMU)，可以加快PL-ICP的配准速度。如果没有初值，则使用零速度模型。 源码在getPrediction函数，在delta_t时间内的相对运动x, y用轮式里程计的delta_x和delta_y表示。 如果用了IMU，delta_theta用IMU的相对yaw表示，否则还用轮式里程计。

包里的demo.bag是手持一个Hokuyo雷达的移动结果，当然没有里程计。


常数速度模型: 假设机器人基于估计的速度运行，速度估计来自传感器或 derivating and filtering scan match的结果. 需要的话题是vel. 使用Alpha-beta滤波器 tracking scan-matching 的预测值

零速度模型: 不使用预测，假设机器人停在原地。

我们可以使用轮式里程计+IMU，或者 IMU+alpha-beta tracking。 使用的优先级是：IMU > Odometry > Constant Velocity > Zero Velocity. 源码在getPrediction函数

IMU和轮式里程计(尤其IMU)可以显著提高收敛速度， 强烈建议添加IMU

scan matcher性能稳定时， Alpha-beta tracking 也可以加速收敛，但可能在动态变化环境或缺少特征的环境下出现weird behavior， 一般建议开启，出现异常再关闭。

Keyframes 和 frame-to-frame scan matching

激光里程计一般是frame-to-frame，每一帧scan和上一帧配准. scan中的噪声是难免的，即使机器人不动， incremental 变换也不是零，机器人位姿会慢慢漂移。

可以使用keyframe-based matching. 位姿变换是在当前scan 和 keyframe scan之间计算. keyframe scan只有在机器人移动一定距离后才更新，如果机器人不动，keyframe scan不变，位姿就不会漂移。 参数kf_dist_linear和kf_dist_angular(弧度制)作为阈值。 把两个阈值都改为0，就可以切换回frame-to-frame模式

话题和tf

订阅:

  • scan (sensor_msgs/LaserScan) Scans from a laser range-finder
  • cloud (sensor_msgs/PointCloud2) Scans in point cloud form
  • imu/data (sensor_msgs/Imu) 用于 theta prediction. 只有参数use_imu为true才有效
  • odom (nav_msgs/Odometry) 用于x-, y-的预测, 没有IMU时预测theta. 只有参数use_odom为true才有效

发布:

  • pose2D (geometry_msgs/Pose2D) The pose of the base frame, in some fixed (world) frame

  • 需要的tf: base_link——laser, the pose of the laser in the base frame.

  • 提供的tf: world——base_link, the pose of the robot base in the world frame. Only provided when publish_tf is enabled. world可以改为自定义的名称，一般为odom

参数

  • ~fixed_frame (string, default: “world”): the fixed frame，可以改为odom

  • ~base_frame (string, default: “base_link”): the base frame of the robot

  • ~use_imu (bool, default: true) Whether to use an imu for the theta prediction of the scan registration. Requires input on /imu/data topic.

  • ~use_odom (bool, default: true) Whether to use wheel odometry for the x-, y-, and theta prediction of the scan registration. Requires input on odom topic.
  • ~use_vel (bool, default: false) Whether to use constant velocity model for the x-, y-, and theta prediction of the scan registration. Requires input on vel topic.

官网的剩余ICP参数

在小强机器人的使用

注释StatusPublisher.cpp的部分代码，也就是屏蔽原来的odom：

1
2
3
4
5
mOdomPub = mNH.advertise<nav_msgs::Odometry>("xqserial_server/Odom", 1, true);

mOdomPub.publish(CarOdom);

br.sendTransform(tf::StampedTransform(transform, current_time.fromSec(base_time_), "odom", "base_footprint"));

注释xqserial.launch下面部分，因为我们的目的是odom-->base_link，目前base_footprint是link的parent了，base_link不能有两个parent:

1
<node pkg="tf" type="static_transform_publisher" name="baselink_broadcaster" args="0 0 0.15 0 0 0 1 base_footprint base_link 50">


编辑laser_scan_matcher中的rplidar.launch:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
<launch>
  <include file="$(find rplidar_ros)/launch/a2.launch" />
	<node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
	name="laser_scan_matcher_node"   output="screen">
    <param name="publish_tf" value = "true" />
    <param name="fixed_frame" value = "odom"/>
    <param name="max_iterations" value="10"/>
    <param name="publish_pose_with_covariance_stamped"  value="true" />
    <param name="publish_pose"  value="false"  />
    <param name="do_compute_covariance"  value="false"  />

    <param name="kf_dist_linear"  value="0.08"  />
    <param name="kf_dist_angular"  value="0.14"  />
    <remap from="/imu/data" to="/xqserial_server/IMU" />
    <remap from="pose_with_covariance_stamped" to="plicp_pose"  />
  </node>
</launch>

启动startup, roslaunch laser_scan_matcher rplidar.launch. 现在原来的轮式坐标系odom坐标系换成了 激光坐标系 odom

现在tf变换是: odom —-> base_link —-> laser

问题

process scan失败.png