一、引言

1、AGV需要同时具备定位、避障与导航的功能，其中避障对于雷达本身的分辨率、精度要求并不是很高，只需要能够根据预设定的雷达扫描范围准确避开障碍物即可，故本文以TIM240（SICK激光类雷达）为例介绍实现多雷达时空标定的问题。
2、多个避障雷达可能会被安装在车体各个位置，并且不一定有重叠区域，所以通过提取特征点再进行ICP或NDT配准的方法获取相对位姿变换关系的方式不可行，由于机械结构本身已经按照设计图纸预先设置好雷达的安装位置，所以雷达之间的相对位置关系相对准确，故可以直接利用已有参数可以进行雷达空间标定，将两个雷达数据统一到同一个坐标系下进行，需要用到PCL库。
3、坐标变换实际使用过程当中确实可以用tf进行静态坐标变换，但是相比较直接手动写代码进行转换延迟较高，故采用手动根据相对位置关系进行坐标变换的方式。
4、进行时空标定步骤主要分为：（1）驱动安装及雷达参数配置（2）雷达时间同步（3）雷达空间同步（Rp+T，这里的旋转矩阵和平移矩阵的定义后文会给出）

二、整体思路与流程

1、SICK激光雷达驱动安装
新买的SICK雷达不仅需要安装驱动，还需要进配置文件修改参数才能正常工作
（1）安装驱动

mkdir -p ./sick_scan_ws
cd ./sick_scan_ws

mkdir ./src
pushd ./src
git clone https://ghproxy.com/https://github.com/SICKAG/libsick_ldmrs.git
git clone https://ghproxy.com/https://github.com/SICKAG/msgpack11.git
git clone https://ghproxy.com/https://github.com/SICKAG/sick_scan_xd.git
popd

mkdir -p ./build/msgpack11
pushd ./build/msgpack11
cmake -G "Unix Makefiles" -D CMAKE_CXX_FLAGS=-fPIC -D CMAKE_BUILD_TYPE=Release -D MSGPACK11_BUILD_TESTS=0 ../../src/msgpack11
make
sudo make install
popd

source /opt/ros/melodic/setup.bash # replace noetic by your ros distro
catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev
source ./devel_isolated/setup.bash

（2）雷达配置文件修改
1）首先需要设置PC机的以太网ip端口号如：192.168.0.2
2）SICK官网下载sopas软件，在Windows端使用（用Win10系统，Win11有问题）
3）进入软件，修改登录权限为授权的用户，密码是client

4）修改雷达ip（与PC机同一网段即可，192.168.0.1以及Cola dialect改为cola binary格式。此处修改雷达ip是为了保证两个雷达能同时使用

（3）硬件组装
需要交换机、电源模块、电源线（普通的家庭供电线就可以，火线为棕灰色、地线为黄绿双色、零线为蓝色，分别对应接到电源模块的L端、FG端、N端）、供电线、网线，如下图连接示意图

（4）修改雷达启动文件（launch），需要创建两个启动文件，分别对应不同ip的雷达，我这里设置一个ip为192.168.0.1，一个为192.168.0.3，因为PC的ip为192.168.0.2

<?xml version="1.0"?>
<!--
     **********************************************
     Launch File for TIM 240 scanner
     **********************************************
     Start and stop angle is given in [rad]

     Check IP-address, if you scanner is not found after roslaunch.
-->

<!-- You can launch a TIM_240-scanner on a specific ip address (e.g. 192.68.0.71) using the following example call:

     roslaunch sick_scan sick_tim_240.launch hostname:=192.168.0.71

-->
<!-- Using node option required="true" will close roslaunch after node exits -->

<launch>
    <arg name="hostname" default="192.168.0.1"/>
    <arg name="port" default="2111"/>
    <arg name="cloud_topic" default="cloud1"/>
    <arg name="frame_id" default="cloud1"/>
    <arg name="sw_pll_only_publish" default="true"/>
    <arg name="nodename" default="sick_tim_240_1"/>
    <arg name="add_transform_xyz_rpy" default="0,0,0,0,0,0"/>
    <arg name="add_transform_check_dynamic_updates" default="false"/> <!-- Note: dynamical updates of parameter add_transform_xyz_rpy can decrease the performance and is therefor deactivated by default -->
    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
        <param name="scanner_type" type="string" value="sick_tim_240"/>
        <!-- datagram shows values from -90 ... 120 (211 values) -->
        <!-- -120 deg -->
        <param name="min_ang" type="double" value="-2.094395102"/>
        <!-- 120 deg -->
        <param name="max_ang" type="double" value="+2.094395102"/>
        <!-- corresponds to 1.0/(14.4 * 360) -->
        <param name="time_increment" type="double" value="0.00019290123"/>
        <param name="use_binary_protocol" type="bool" value="True"/>
        <param name="intensity" type="bool" value="true"/>
        <param name="intensity_resolution_16bit" type="bool" value="true"/>
        <param name="hostname" type="string" value="$(arg hostname)"/>
        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
        <param name="frame_id" type="str" value="$(arg frame_id)"/>
        <param name="port" type="string" value="$(arg port)"/>
        <param name="timelimit" type="int" value="5"/>
        <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
        <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->

        
        <!-- Optional range filter configuration: If the range of a scan point is less than range_min or greater than range_max, the point can be filtered. -->
        <!-- Depending on parameter range_filter_handling, the following filter can be applied for points with a range not within [range_min, range_max],   -->
        <!-- see enumeration RangeFilterResultHandling in range_filter.h:                                           -->
        <!--   0: RANGE_FILTER_DEACTIVATED,  do not apply range filter (default)                                    -->
        <!--   1: RANGE_FILTER_DROP,         drop point, if range is not within [range_min, range_max]              -->
        <!--   2: RANGE_FILTER_TO_ZERO,      set range to 0, if range is not within [range_min, range_max]          -->
        <!--   3: RANGE_FILTER_TO_RANGE_MAX, set range to range_max, if range is not within [range_min, range_max]  -->
        <!--   4: RANGE_FILTER_TO_FLT_MAX,   set range to FLT_MAX, if range is not within [range_min, range_max]    -->
        <!--   5: RANGE_FILTER_TO_NAN        set range to NAN, if range is not within [range_min, range_max]        -->
        <!-- Note: Range filter applies only to Pointcloud messages, not to LaserScan messages.                     -->
        <!-- Using range_filter_handling 4 or 5 requires handling of FLT_MAX and NAN values in an application.      -->
        <param name="range_min" type="double" value="0.0"/>
        <param name="range_max" type="double" value="100.0"/>
        <param name="range_filter_handling" type="int" value="0"/>
        
        <!-- Apply an additional transform to the cartesian pointcloud, default: "0,0,0,0,0,0" (i.e. no transform) -->
        <!-- Note: add_transform_xyz_rpy is specified by 6D pose x, y, z, roll, pitch, yaw in [m] resp. [rad] -->
        <!-- It transforms a 3D point in cloud coordinates to 3D point in user defined world coordinates: --> 
        <!-- add_transform_xyz_rpy := T[world,cloud] with parent "world" and child "cloud", i.e. P_world = T[world,cloud] * P_cloud -->
        <!-- The additional transform applies to cartesian lidar pointclouds and visualization marker (fields) -->
        <!-- It is NOT applied to polar pointclouds, radarscans, ldmrs objects or other messages -->
        <param name="add_transform_xyz_rpy" type="string" value="$(arg add_transform_xyz_rpy)" /> 
        <param name="add_transform_check_dynamic_updates" type="bool" value="$(arg add_transform_check_dynamic_updates)" />

        <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
        <param name="message_monitoring_enabled" type="bool" value="True" />      <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
        <param name="read_timeout_millisec_default" type="int" value="5000"/>     <!-- 5 sec read timeout in operational mode (measurement mode), default: 5000 milliseconds -->
        <param name="read_timeout_millisec_startup" type="int" value="120000"/>   <!-- 120 sec read timeout during startup (sensor may be starting up, which can take up to 120 sec.), default: 120000 milliseconds -->
        <param name="read_timeout_millisec_kill_node" type="int" value="150000"/> <!-- 150 sec pointcloud timeout, ros node will be killed if no point cloud published within the last 150 sec., default: 150000 milliseconds -->
        <param name="client_authorization_pw" type="string" value="F4724744"/>    <!-- Default password for client authorization -->

        <!-- Configuration of ROS quality of service: -->
        <!-- On ROS-1, parameter "ros_qos" sets the queue_size of ros publisher -->
        <!-- On ROS-2, parameter "ros_qos" sets the QoS of ros publisher to one of the following predefined values: -->
        <!-- 0: rclcpp::SystemDefaultsQoS(), 1: rclcpp::ParameterEventsQoS(), 2: rclcpp::ServicesQoS(), 3: rclcpp::ParametersQoS(), 4: rclcpp::SensorDataQoS() -->
        <!-- See e.g. https://docs.ros2.org/dashing/api/rclcpp/classrclcpp_1_1QoS.html#ad7e932d8e2f636c80eff674546ec3963 for further details about ROS2 QoS -->
        <!-- Default value is -1, i.e. queue_size=10 on ROS-1 resp. qos=rclcpp::SystemDefaultsQoS() on ROS-2 is used.-->
        <param name="ros_qos" type="int" value="-1"/>  <!-- Default QoS=-1, i.e. do not overwrite, use queue_size=10 on ROS-1 resp. qos=rclcpp::SystemDefaultsQoS() on ROS-2 -->

    </node>
</launch>

2、SICK激光雷达时间同步
主要使用ros官方提供的软同步方式（message_filters）
使用方式如下：

ros::Publisher pointcloud_pub;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> testSyncPolicy;

void callback(const sensor_msgs::PointCloud2ConstPtr &point1, const sensor_msgs::PointCloud2ConstPtr &point2)  //回调中包含多个消息
{
	//需要处理的内容//
}

int main (int argc, char **argv)
{
    ros::init (argc, argv, "lidar2base");
    ros::NodeHandle n;
    pointcloud_pub = n.advertise<sensor_msgs::LaserScan>("/scan", 1000);
    //发布的话题
    message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud1_sub(n, "/cloud1", 1);// 需要同步的topic1
    message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud2_sub(n, "/cloud2", 1);// 需要同步的topic2 
    message_filters::Synchronizer<testSyncPolicy> sync(testSyncPolicy(10), pointcloud1_sub, pointcloud2_sub);// 同步
    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::spin();
    ros::shutdown();
    return 0;
}

3、SICK激光雷达空间同步
主要是通过雷达之间的旋转和平移参数将两个雷达数据统一到一个坐标系下再使用PCL库对雷达数据进行融合，这里编写了相关函数

//坐标变换将激光雷达坐标系下的点转换到小车坐标系下
void transPoint(pcl::PointCloud<pcl::PointXYZ>::Ptr lidarCloud,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr carCloud,
                 double yaw,double pitch,double roll,double x,double y,double z)
{
    Eigen::Matrix4f transform=Eigen::Matrix4f::Identity();
    Eigen::Matrix4f transformYaw;
    Eigen::Matrix4f transformPitch;
    Eigen::Matrix4f transformRoll;
    
    //航向角
    transformYaw<<cos(yaw),-sin(yaw),0,0,\
            sin(yaw),cos(yaw),0,0,\
            0,0,1,0,\
            0,0,0,1;
    //俯仰角
    transformPitch<<cos(pitch),0,sin(pitch),0,\
            0,1,0,0,\
            -sin(pitch),0,cos(pitch),0,\
            0,0,0,1;
    //横滚角
    transformRoll<<1,0,0,0,\
            0,cos(roll),-sin(roll),0,\
            0,sin(roll),cos(roll),0,\
            0,0,0,1;
            
    //旋转矩阵
    transform=transformRoll*transformPitch*transformYaw;
    
    //平移矩阵
    transform(0,3)=x;
    transform(1,3)=y;
    transform(2,3)=z;
    
    //坐标转换
    pcl::transformPointCloud(*lidarCloud,*carCloud,transform);
}

如下图：
需求是通过上述函数将P点从激光雷达坐标系变换到车体坐标系，函数的输入分别为偏航角yaw, pitch, roll，x，y，z

偏航角yaw的获取方式是：以车坐标系为右手基准坐标系，大拇指指向为z轴正方向，激光雷达系为目标坐标系，顺时针旋转z轴的角度分量为yaw的值，图中x1Oy1为车体坐标系。
同理俯仰角pictch为绕y轴旋转角度分量，横滚角roll为绕x轴的分量。
平移分量（x, y, z）是激光雷达相对于车体坐标原点的x，y，z的坐标。
4、雷达数据融合
需要将ros的PointCloud2类型数据转换为PCL数据类型，而后将PCL数据类型转为ros的LaserScan数据类型。
1）数据融合主要在SICK激光雷达时间同步这一部分的回调函数中写：

    pcl::PointCloud<pcl::PointXYZ>::Ptr colorcloud1(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*point1, *colorcloud1);
    pcl::PointCloud<pcl::PointXYZ>::Ptr colorcloud2(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*point2, *colorcloud2);
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_points1(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_points2(new pcl::PointCloud<pcl::PointXYZ>);

    transPoint(colorcloud1, output_points1, yaw1, pitch1, roll1, x1, y11, z1);
    transPoint(colorcloud2, output_points2, yaw2, pitch2, roll2, x2, y2, z2);
    pcl::PointCloud<pcl::PointXYZ> output = *output_points1 + *output_points2;
    
    
    //pcl::toROSMsg((*output_points1 + *output_points2), output);
    //以实时发布的方式发布
    
    sensor_msgs::LaserScan output2 = PointCloudToLaserscan(output);
    output2.header.frame_id = "base_link";
    pointcloud_pub.publish(output2);

2）PCL转LaserScan单独写了一个函数

sensor_msgs::LaserScan PointCloudToLaserscan(pcl::PointCloud<pcl::PointXYZ>& _pointcloud)
{
  float angle_min, angle_max, range_min, range_max, angle_increment;
  
  //需要自行调整的参数
  angle_min = -2.094395102;
  angle_max =  2.094395102;
  range_min = 0.05;
  range_max = 10;
  //角度分辨率，分辨率越小，转换后的误差越小
  angle_increment = 0.0174444444;

  //计算扫描点个数
  unsigned int beam_size = ceil((angle_max - angle_min) / angle_increment);

  sensor_msgs::LaserScan output;
  output.header.stamp = ros::Time::now();
  output.header.frame_id = "base_link";
  output.angle_min = angle_min;
  output.angle_max = angle_max;
  output.range_min = range_min;
  output.range_max = range_max;
  output.angle_increment = angle_increment;
  output.time_increment = 0.0;
  output.scan_time = 0.0;
  
  //先将所有数据用nan填充
  output.ranges.assign(beam_size, std::numeric_limits<float>::quiet_NaN());
  //output.intensities.assign(beam_size, std::numeric_limits<float>::quiet_NaN());

  for (auto point : _pointcloud.points)
  {
    float range = hypot(point.x, point.y);
    float angle = atan2(point.y, point.x);
    int index = (int)((angle - output.angle_min) / output.angle_increment);
    if (index >= 0 && index < beam_size)
    {
      //如果当前内容为nan，则直接赋值
      if (isnan(output.ranges[index]))
      {
        output.ranges[index] = range;
      }
      //否则，只有距离小于当前值时，才可以重新赋值
      else
      {
        if (range < output.ranges[index])
        {
          output.ranges[index] = range;
        }
      }
      //output.intensities[index] = point.intensity;
    }
  }
  return output;
}

5、创建启动脚本
首先启动两个雷达，其次启动坐标转换的脚本，即可实现两个雷达的时空同步

#!/bin/bash
gnome-terminal --tab -- bash -c "roslaunch sick_scan sick_tim_240_1.launch"
sleep 1s
gnome-terminal --tab -- bash -c "roslaunch sick_scan sick_tim_240_2.launch "
sleep 2s
gnome-terminal --tab -- bash -c "source /home/lixushi/catkin_ws/devel/setup.bash; roslaunch lidar2base lidar2base.launch"

如下图为雷达实际摆放位置

转换前的点云图

转换后的点云图

三、参考文献

[1] https://blog.csdn.net/m0_68312479/article/details/128266483
[2] https://blog.csdn.net/hltt3838/article/details/123067620
[3] https://blog.csdn.net/m0_68312479/article/details/128266483
[4] https://adamshan.blog.csdn.net/article/details/105930565?spm=1001.2014.3001.5502
[5] https://mp.weixin.qq.com/s?__biz=MzU1NjEwMTY0Mw==&mid=2247556040&idx=1&sn=fa1f7cc63f7aeeaed69242add86efd75&chksm=fbc862acccbfebba12fd29cef32b7a34ac296f54e8ef6f19485aff81ebddd794a3364419c90b&scene=27