二维激光SLAM

---

二维激光 SLAM 学习

一、激光雷达数据效果对比

1、激光雷达的技术指标

• 数据的最大最小距离以及扫描的角度范围（视野范围）。决定了应用场景
• 2个数据点间的角度（角度分辨率，点数）。点越多效果越好但计算成本更高
• 发出数据的频率。频率越高，估计的距离越小，定位越准
• 发出数据的能量强度。有的厂商可能无法返回数据
• 点的跳动程度（精度）等等

2、激光雷达的测试结果

• 不同雷达对光强的适应程度不同
• 部分雷达在大角度下没有数据，有的雷达能看到更大的角度
• 不同的频率会产生不同的数据畸变，30Hz以上的雷达几乎无影响
• 频率越低，移动速度或旋转速度越高，产生的畸变越严重，对建图的影响也越大

二、了解雷达数据

如果无法产生可执行文件，1、选择新建ws；2、检查CMakeLists.txt文件中的编译选项

1、代码解读

这段代码将本节点 ( /lesson1_laser_scan_node ) 的回调函数与订阅的 topic ( laser_scan ) 进行绑定，每当有新消息到达就调用 ScanCallback 此回调函数

laser_scan_subscriber_ = node_handle_.subscribe("laser_scan", 1, &LaserScan::ScanCallback, this);

2、结果分析

[ INFO] [1606545575.110606737]: seqence: 4131, time stamp: 1606455444.278184417, frame_id: front_laser_link, angle_min: -3.14159, angle_max: 3.14159, angle_increment: 0.00436332, time_increment: 7.15627e-05, scan_time: 0.102979, range_min: 0.01, range_max: 25, range size: 1440, intensities size: 1440
[ INFO] [1606545575.110772238]: range = 2.6, angle = -3.12414, x = -2.5996, y = -0.0453758

• 坐标系：frame_id = front_laser_link
• 角度范围：angle_min: -3.14159, angle_max: 3.14159，水平视角为 360°
• 距离：range_min: 0.01, range_max: 25，雷达的盲区为1cm
• 数据点：range size: 1440，扫描一周返回 1440 个数据点

3、坐标转换

ranges 字段中储存的只有极坐标系下的距离值，每个数据点对应欧几里得坐标需要将极坐标进行转换

第i个数据点的距离值为 ranges[i]：

• 对应的角度为 angle = angle_min + angle_increment * i
• 对应的x坐标为 ranges[i] * cos(angle)
• 对应的y坐标为 ranges[i] * sin(angle)

三、使用单线雷达实现 LIO-SAM 中的特征点提取

1、遇到的问题

1.1 显示 C++ 的版本问题

显示版本出问题的解决办法

1.2 配置环境

配置环境时要和launch文件配对，这两个文件添加依赖时要细心

add_executable(${PROJECT_NAME}_laser_scan_node src/laser_scan_node.cc)
add_dependencies(${PROJECT_NAME}_laser_scan_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME}_laser_scan_node ${catkin_LIBRARIES})

add_executable(${PROJECT_NAME}_feature_detection_node src/feature_detection.cc)
add_dependencies(${PROJECT_NAME}_feature_detection_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME}_feature_detection_node ${catkin_LIBRARIES})

2、代码解读

// 将雷达的回调函数与订阅的topic进行绑定
laser_scan_subscriber_ = node_handle_.subscribe("laser_scan", 1, &LaserScan::ScanCallback, this);
// 将提取后的点发布到 feature_scan 这个topic
feature_scan_publisher_ = node_handle_.advertise<sensor_msgs::LaserScan>("feature_scan", 1, this);

该节点通过回调函数订阅 /aser_scan 并发布到 /feature_scan

3、补充知识

• INF表示“无穷大”，是infinite的缩写。
• NAN表示“无效数字”，是Not a number的缩写

如墙面与雷达射线的夹角太大，雷达一条线打到桌子的边缘，导致返回强度很低等等等等。这些情况下，雷达的驱动包里会将这个值赋值成inf或者nan

四、使用 PCL 进行雷达的消息类型转换

顺便复习查看包依赖的命令

$ rospack depends1 lesson2

1、代码解读

该节点将回调函数和 /laser_scan 绑定，接收来自雷达的 Sensor_msgs/LaserScan，并初始化为一个 PointCloudT 的 publisher，发布Pcl::PointCloud< T >类型的数据，并由 ROS 自动转化为 Sensor_msgs::PointCloud2 类型的消息

五、基于 ICP 的帧间匹配

调用ICP算法进行相邻两帧雷达数据间坐标变换的计算

1、代码解读

1 帧代表激光雷达转一圈收集到的所有信息

通过回调函数将 scan_msg 的信息存到 *current_pointcloud_ 这个指针下，处理前注意对第一个点进行特殊处理。

ICP 的部分：

void ScanMatchICP::ScanMatchWithICP(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    // ICP 输入数据,输出数据的设置,还可以进行参数配置,这里使用默认参数
    icp_.setInputSource(last_pointcloud_);
    icp_.setInputTarget(current_pointcloud_);
    // 开始迭代计算
    pcl::PointCloud unused_result;
    icp_.align(unused_result);
    // 如果迭代没有收敛,不进行输出
    if (icp_.hasConverged() == false)
    {
        std::cout << "not Converged" << std::endl;
        return;
    }
    else
    {
        // 收敛了之后, 获取坐标变换
        Eigen::Affine3f transfrom;
        transfrom = icp_.getFinalTransformation();
        // 将Eigen::Affine3f转换成x, y, theta, 并打印出来
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(transfrom, x, y, z, roll, pitch, yaw);
        std::cout << "transfrom: (" << x << ", " << y << ", " << yaw * 180 / M_PI << ")" << std::endl;
    }
}

六、基于 PL-ICP 的帧间匹配

使用 ICP 的改进算法 PL-ICP 算法来计算相邻帧间的坐标变换

1、PL-ICP

PL-ICP(Point to Line ICP) 使用点到线距离最小的方式进行ICP的计算，收敛速度快很多，同时精度也更高一些

作者将实现pl-icp的代码命名为csm( Canonical Scan Matcher)

2、scan_tools

2.1 wiki 地址

here

2.2 功能简介

• laser_ortho_projector: 将切斜的雷达数据投影到平面上．
• laser_scan_matcher: 基于pl-icp的扫描匹配的实现，并进行了位姿累加
• laser_scan_sparsifier: 对雷达数据进行稀疏处理
• laser_scan_splitter: 将一帧雷达数据分段，并发布出去
• ncd_parser: 读取New College Dataset，转换成ros的scan 与 odometry 发布出去
• polar_scan_matcher: 基于Polar Scan Matcher的扫描匹配器的ros实现
• scan_to_cloud_converter: 将 sensor_msgs/LaserScan 数据转成 sensor_msgs/PointCloud2 的数据格式．

3、代码解读

void ScanMatchPLICP::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    // 如果是第一帧数据，首先进行初始化
    if (!initialized_)
    {
        // 将雷达各个角度的sin与cos值保存下来，以节约计算量
        CreateCache(scan_msg);
        // 将 prev_ldp_scan_,last_icp_time_ 初始化
        LaserScanToLDP(scan_msg, prev_ldp_scan_);
        last_icp_time_ = scan_msg->header.stamp;
        initialized_ = true;
    }
    // 进行数据类型转换
    start_time_ = std::chrono::steady_clock::now();
    LDP curr_ldp_scan;
    LaserScanToLDP(scan_msg, curr_ldp_scan);
    end_time_ = std::chrono::steady_clock::now();
    time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
    std::cout << "\n转换数据格式用时: " << time_used_.count() << " 秒。" << std::endl;
    // 使用PL-ICP计算雷达前后两帧间的坐标变换
    start_time_ = std::chrono::steady_clock::now();
    ScanMatchWithPLICP(curr_ldp_scan, scan_msg->header.stamp);
    end_time_ = std::chrono::steady_clock::now();
    time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
    std::cout << "PLICP计算用时: " << time_used_.count() << " 秒。" << std::endl;
}

七、栅格地图的构建

scan-to-map的计算位姿变换的方式

1、栅格地图

二维激光SLAM构建的地图为二维栅格地图，三维激光SLAM与视觉SLAM构建的地图一般都是三维点云地图, 一副完全由离散的三维空间点组成的地图。

栅格地图就是用一个个栅格组成的网格来代表地图. 栅格里可以存储不同的数值, 代表这个栅格的不同含义：

• ROS的栅格地图使用白色代表空闲，也就是可通过区域，其存储的值为 0
• 黑色代表占用，也就是不可通过区域，其存储的值为 100
• 灰色代表未知，就是说目前还不清楚这个栅格是否可以通过，其存储的值为 -1
• 栅格地图由于其占用与空闲的表示方法，在ROS中又被称为占用地图

下面查看栅格地图的消息类型

$ rosmsg show nav_msgs/OccupancyGrid 
std_msgs/Header header          # 数据的消息头
  uint32 seq                    # 数据的序号
  time stamp                    # 数据的时间戳
  string frame_id               # 地图的坐标系
nav_msgs/MapMetaData info       # 地图的一些信息
  time map_load_time            # 加载地图的时间
  float32 resolution            # 地图的分辨率，一个格子代表着多少米，一般为0.05，[m/cell]
  uint32 width                  # 地图的宽度，像素的个数, [cells]
  uint32 height                 # 地图的高度，像素的个数, [cells]
  geometry_msgs/Pose origin     # 地图左下角的格子对应的物理世界的坐标，[m, m, rad]
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
# 地图数据，优先累加行，从（0,0）开始。占用值的范围为[0,100]，未知为-1。
int8[] data    

通过这里可以知道，地图本身是只有像素坐标的，其像素坐标系为坐下角为 (0,0) 的坐标系．通过左下角栅格对应的世界坐标以及 分辨率，再使用（像素 * 分辨率 + origin）, 将像素坐标转成世界坐标，使用这个方法确定了整个栅格地图的内容

2、代码解读

使用 OccupancyGrid() 函数构造地图，然后以1Hz的频率重复调用 PublishMap() 函数发布地图信息

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lesson4_make_occupancy_grid_map");
    OccupancyGrid occupancy_grid;
    ros::Rate rate(1);
    while (ros::ok())
    {
        ROS_INFO("publish occupancy map");
        occupancy_grid.PublishMap();
        rate.sleep();
    }
    return (0);
}

八、基于 GMapping 的栅格地图的构建

将激光雷达的数据构建成栅格地图

GMapping是基于粒子滤波算法实现的SLAM，通过里程计数据获取粒子群的先验位姿，再通过雷达数据与地图的匹配程度对所有粒子进行打分，通过分数高的粒子群来近似机器人的真实位姿

1、代码解读

1.1 回调函数

• 记录上次时间
• 对第一帧进行特殊判断，记录各个角度的sin与cos值
• 记录时间起点
• 调用 publishmap() 函数计算地图并发布出去

1.2 PublishMap()

这里的ScanMatcherMap为GMapping中存储地图的数据类型，先声明了一个ScanMatcherMap的对象gmapping_map_，然后通过ComputeMap()为gmapping_map_赋值，然后再将gmapping_map_中存储的值赋值到ros的栅格地图的数据类型中

• 记录地图的中点
• 声明 gmapping_map_
• ComputeMap(gmapping_map_, scan_msg) 使用 scan_msg 更新 gmapping_map_ 中的数据
• 将 gmapping_map_ 中的存储的栅格值赋值到 ros 的 map 中
• 添加当前时间戳
• 发布

1.3 ComputeMap()

将位姿 lp（地图坐标系下的激光雷达坐标系的位姿）转换成地图坐标系下的位置

第一部分：

• 声明地图有效区域 activeArea
• 排除错误的激光点
• 根据激光雷达在地图坐标系中的位姿计算激光点在地图坐标系下的坐标（使用距离 d 和第一帧中记录的角度）
• 使用 bresenham 算法来计算从激光位置到激光点要经过的栅格的坐标（就是 p0 到 p1 的线）
• 遍历线上的点，将空点的点加入到有效区域 activeArea 中
• 如果 d < m_usableRange 需要把击中点加入 hit_lists_ 中，以备后用；如果 d = max_use_range_ 那么说明这个值只用来进行标记空闲区域，不记录该点
• 为 activeArea 分配内存

第二部分：

• 遍历有效击中点，若线上有障碍物，则更新这个 hit 点

二、GMapping

主要使用部分在 ComputeMap() 函数的第二部分更新数值中

举个例子说明一下栅格地图的占用值更新的方式．

例如：当雷达扫到人腿时，会对击中点的栅格更新一次占用，这时在ROS地图下这个点将被表示为障碍物．如果人腿离开了这个位置，在之后的过程中，有4次在这个栅格更新了空闲，就会将ROS地图中这个栅格的状态更新到空闲，也就是没有障碍物．

原因：第一次更新地图时，这个格子的占用值为 1/1 = 1 > 0.25 ，就会将ROS地图中这个格子设置为100，表示障碍物．
如果之后4次更新地图，这个栅格都被更新空闲了，则这个格子的占用值将变为 1/5 = 0.2 <0.25了，所以就会将ROS地图中对应的栅格设置为0，变成可通过区域了．即 GMapping 是通过击中次数与观测次数的比值作为栅格地图中每个格子的值

九、基于 Hector 的栅格地图的构建

Hector 是2011年开源的二维激光 SLAM 的项目，非常创新地使用scan-to-map的匹配方式来进行进行单帧的地图构建

1、代码解读

1.1 回调函数

激光的回调函数里有4个步骤：

• 对 hector_map_ 进行初始化与内存空间分配
• ROSLaserScanToDataContainer() 将雷达的数据类型转成 hector 需要的格式
• ComputeMap() 将雷达的数据类型写成 hector 的地图
• PublishMap() 将hector的地图转换成 ROS 中的地图格式并发布出去

1.2 ComputeMap()

调用 updateByScanJustOnce() 进行 hector 地图的生成，其中数据来自 laserScan_container_，这是由上一步转化而成的符合 hector 的数据

1.3 PublishMap()

将hector格式的地图转换成ROS格式的地图，将 memset() 函数将所有值设置为 -1 ，可以只考虑占用与空闲两种情况

二、Hector 中的数据类型

将更新占用值的因子设置为 0.6，将更新空闲值的因子设置为0.4，分别使用 probToLogOdds() 函数进行转换之后得到:
占用值更新量 logOddsOccupied = 0.405465
空闲值更新量 logOddsFree = -0.405465
最后这两个值一正一负．
如果是更新占用，就将这个格子的值加上 logOddsOccupied , 也就是加上 0.405465 ;
如果是更新空闲，就将这个格子的值加上 logOddsFree，也就是减去 -0.405465；
最后通过累加后的值是否大于零来决定是否作为障碍物

三、思考

传感器和算法是存在误差的．这会导致不能通过一次两次的更新之后的值，来代表这个栅格是占用还是空闲的，因为这个状态十分有可能是错的。只有通过长时间的累计之后，最后通过这个值是否大于零，来近似地将这个栅格设置为是否是障碍物，这时候依然不能百分百地确定这个栅格是否就是障碍物。因为，有可能是人腿走过留下的痕迹。

现在的机器人建图与定位算法基本都是与概率相关，只能按照概率最大的值作为最好值，而不是计算出真实值。这也说明了 SLAM 的本质就是将不同时刻的 scan 在正确的位置上写成占据地图，通过定位来在这个位置上将雷达数据写成占据地图，反过来如果每次的建图都很准确，那么就可以说是进行了精确的定位并且构建了完美的地图。

十、重写 Hector SLAM

1、代码解读

1.1 构造函数

• 首先调用 InitParams 进行了 ROS 参数的初始化
• 对 HectorSlamProcessor 对象进行初始化.

再之后对多层地图进行了初始化，并调用 setMapInfo() 进行 ROS地图的内存的分配．

最后，查找从 base_link 到 雷达坐标系间的坐标变换.

1.2 回调函数

• 首先进行了雷达数据格式的转换. 先通过 laser_geometry 这个包将 LaserScan 转换成 PointCloud 格式
• 将雷达数据从点云形式转换到Hector自己的雷达数据存储格式．
• 进行扫描匹配与地图更新．
• 描匹配计算完成之后再发布里程计 topic 与 tf，tf将根据不同的配置决定是发布 map -> base_link 还是 map -> odom -> base_link.

void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan &scan)
{
    start_time_ = std::chrono::steady_clock::now();
    ros::WallTime startTime = ros::WallTime::now();
    // 将 scan 转换成 点云格式
    projector_.projectLaser(scan, laser_point_cloud_, 30.0);
    Eigen::Vector3f startEstimate(Eigen::Vector3f::Zero());
    // 将雷达数据的点云格式改成 hector 内部的数据格式
    if (rosPointCloudToDataContainer(laser_point_cloud_, laserTransform_, laserScanContainer, slamProcessor->getScaleToMap()))
    {
        // 首先获取上一帧的位姿，作为初值
        startEstimate = slamProcessor->getLastScanMatchPose();
        // 进入扫描匹配与地图更新
        slamProcessor->update(laserScanContainer, startEstimate);
    }
    end_time_ = std::chrono::steady_clock::now();
    time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
    std::cout << "数据转换与扫描匹配用时: " << time_used_.count() << " 秒。" << std::endl;
    if (p_timing_output_)
    {
        ros::WallDuration duration = ros::WallTime::now() - startTime;
        ROS_INFO("HectorSLAM Iter took: %f milliseconds", duration.toSec() * 1000.0f);
    }
    // 更新存储的位姿, 这里的位姿是 base_link 在 map 下的位姿
    poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance(), scan.header.stamp, p_map_frame_);

    // 发布 odom topic
    if (p_pub_odometry_)
    {
        nav_msgs::Odometry tmp;
        tmp.pose = poseInfoContainer_.getPoseWithCovarianceStamped().pose;
        tmp.header = poseInfoContainer_.getPoseWithCovarianceStamped().header;
        tmp.child_frame_id = p_base_frame_;
        odometryPublisher_.publish(tmp);
    }

    if (pub_map_to_baselink_tf_)
    {
        // pub map -> odom -> base_link tf
        if (p_pub_map_odom_transform_)
        {
            tfB_->sendTransform(tf::StampedTransform(map_to_odom_, scan.header.stamp, p_map_frame_, p_odom_frame_));
            tfB_->sendTransform(tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_odom_frame_, p_tf_map_scanmatch_transform_frame_name_));
        }
        // pub map -> base_link tf
        else
        {
            tfB_->sendTransform(tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_map_frame_, p_tf_map_scanmatch_transform_frame_name_));
        }
    }
}

1.3 rosPointCloudToDataContainer()

将雷达数据转换到 base_link 中，再乘地图的分辨率 0.05 放入 dataContainer 中（此时已经符合 hector 的格式）

// 将点云数据转换成Hector中雷达数据的格式
bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud &pointCloud, const tf::StampedTransform &laserTransform, hectorslam::DataContainer &dataContainer, float scaleToMap)
{
    size_t size = pointCloud.points.size();
    dataContainer.clear();
    tf::Vector3 laserPos(laserTransform.getOrigin());
    // 将 base_link 到 laser_link 的坐标转换乘以地图分辨率当成这帧数据的 origo
    dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y()) * scaleToMap);
    for (size_t i = 0; i < size; ++i)
    {
        const geometry_msgs::Point32 &currPoint(pointCloud.points[i]);
        float dist_sqr = currPoint.x * currPoint.x + currPoint.y * currPoint.y;
        if ((dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_))
        {
            if ((currPoint.x < 0.0f) && (dist_sqr < 0.50f)) continue;
            // 距离太远的点跳动太大，如果距离大于使用距离(20m)，就跳过
            if (dist_sqr > p_use_max_scan_range_ * p_use_max_scan_range_) continue;
            // 这里使用了 TF 的带时间戳的变换：tf::StampedTransform，这是一个 4X4 的矩阵，能够实现平移和旋转变换
			// 这种方法一般用来实现雷达到机器人的坐标变换
            // 点的坐标左乘 base_link->laser_link 的 tf 将得到该点在 base_link 下的 xy 坐标, 但是 z 坐标是不正确
            tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z));
            // 通过再减去 base_link->laser_link 的 tf 的 z 的值，得到该点在 base_link 下正确的 z 坐标
            float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z();
            if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_)
            {
                // 将雷达数据的 x y 都乘地图的分辨率 0.05 再放入 dataContainer 中
                // 就是将雷达坐标放入 Hector 的容器中
                dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(), pointPosBaseFrame.y()) * scaleToMap);
            }
        }
    }
    return true;
}

1.4 update()

base link 在 map 下的坐标

update() 分为两步：

• scan_match 返回当前 scan 在与地图进行扫描匹配后的 map 坐标系下的位姿
• updateByScan() 进行地图更新

hector world 坐标系与 hector map 坐标系：初始位姿在 world 坐标系下，Hector 中将实际物理坐标表示为 world 坐标，以左上角为原点，可以理解为此时坐标的单位是 m。而 hector map 坐标就是用 world 坐标再除以地图的分辨率 0.05，再加上 map 坐标系的原点得到像素坐标，可以理解为将 m 除以 m/cell，再加上 map 的坐标原点得到坐标点在第几个像素中。

将雷达数据从物理坐标系都转化到map坐标系（像素坐标，rviz那种的样子？）中，通过多算几次得到位姿（雷达的），再转化到物理坐标系下完成更新

1.5 matchData()

进行扫描匹配的地方（位姿估计），其中调用了estimateTransformationLogLh()函数对坐标系进行计算

    /*
     * @param beginEstimateWorld  位姿初值
     * @param gridMapUtil         网格地图工具,这里主要是用来做坐标变换
     * @param dataContainer       激光数据
     * @param covMatrix           协方差矩阵
     * @param maxIterations       最大迭代次数
     * @return
     */
    Eigen::Vector3f matchData(const Eigen::Vector3f &beginEstimateWorld,
                              ConcreteOccGridMapUtil &gridMapUtil, 
                              const DataContainer &dataContainer,
                              Eigen::Matrix3f &covMatrix, int maxIterations)
    {
        if (dataContainer.getSize() != 0)
        {
            // 1. 初始pose转换为地图尺度下的pose --仿射变换,包括位姿的尺度和偏移，旋转在 X Y 同尺度变化时保持不变
            Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));
            Eigen::Vector3f estimate(beginEstimateMap);
            // 2. 第一次迭代
            estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
            int numIter = maxIterations;
            /** 3. 多次迭代求解 **/
            for (int i = 0; i < numIter; ++i)
            {
                estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
            }
            // 角度正则化
            estimate[2] = util::normalize_angle(estimate[2]);
            covMatrix = Eigen::Matrix3f::Zero();
            // 使用Hessian矩阵近似协方差矩阵
            covMatrix = H;
            // 结果转换回物理坐标系下 -- 转换回实际尺度
            return gridMapUtil.getWorldCoordsPose(estimate);
        }
        return beginEstimateWorld;
    }

十一、IMU 与轮速计(里程计)进行运动畸变校正

1、IMU

1.1 IMU 简介

IMU 全称 Inertial Measurement Unit，惯性测量单元，主要用来检测和测量加速度与旋转运动的传感器

1.2 IMU 消息格式

IMU 在 ROS 中的消息格式如下，有些 IMU 会提供积分后的姿态值，也就是orientation，有些不会提供，就需要自己通过对角速度进行积分来获取姿态值

$ rosmsg show sensor_msgs/Imu
std_msgs/Header header		# 消息头
  uint32 seq
  time stamp				# 时间戳
  string frame_id			# 坐标系
geometry_msgs/Quaternion orientation		# IMU的当前姿态，4元数的形式表示
  float64 x
  float64 y
  float64 z
  float64 w
float64[9] orientation_covariance			# 姿态的协方差
geometry_msgs/Vector3 angular_velocity		# IMU的3轴角速度
  float64 x
  float64 y
  float64 z
float64[9] angular_velocity_covariance		# IMU的3轴角速度的协方差
geometry_msgs/Vector3 linear_acceleration	# IMU的3轴加速度
  float64 x
  float64 y
  float64 z
float64[9] linear_acceleration_covariance	# IMU的3轴加速度的协方差

2、轮速计(里程计)

2.1 轮速计简介

轮速计就是安装在电机上的编码器，通过电机旋转的圈数来计算机器人所走过的距离与角度，在 ROS 中称为 Odometry，译为里程计。随着 SLAM 的发展，里程计代表的意思多了起来，如激光雷达里程计，视觉里程计等等，这些都是通过各种方式，获取雷达或者相机这段时间内移动的距离与角度。

2.2 轮速计消息格式

$ rosmsg show nav_msgs/Odometry 
std_msgs/Header header
  uint32 seq
  time stamp				# 时间戳
  string frame_id			# 里程计坐标系名字，一般为odom
string child_frame_id		# 里程计坐标系指向的子坐标系名字，一般为base_link或者footprint
geometry_msgs/PoseWithCovariance pose		# 带协方差的位姿
  geometry_msgs/Pose pose
    geometry_msgs/Point position			#　位置，x y z
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation	#　四元数表示的姿态
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist		# 带协方差的速度值
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear			#　线速度
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular			#　角速度
      float64 x
      float64 y
      float64 z
  float64[36] covariance

大多数情况下，优先使用 IMU 的角速度，与轮速计的距离值

2、代码解读

2.1 回调函数

由于使用了 3 个线程，所以在保存 imu 与 odom 数据时，需要先上锁，以防止同一时间有 2 个地方对 imu_queue_（角速度） 与 odom_queue_（距离值） 进行更改。
在 scan 的回调函数中完成三个任务：①：缓存雷达数据；②：使用 imu 时对 imu 进行修剪，进行时间同步，并计算雷达数据时间内的旋转；③：使用 odom 时对 odom 进行修剪，进行时间同步，并计算雷达数据时间内的平移；④：对雷达的数据点进行去畸变；⑤：发布；⑥：参数重置

2.2 缓存雷达数据 CacheLaserScan()

有两帧雷达数据时才能继续向下进行，为了下两步都能有时间数据

2.3 IMU 时间同步与角度积分

由于 IMU 与雷达的频率不同所以需要进行时间同步，先将imu的双端队列中时间太早的数据删去，之后找到IMU的时间与当前帧雷达的起始时间早的第一个IMU数据。以这个 IMU 数据作为起点，使用之后的 IMU 数据进行积分，分别获得 2 个 IMU 数据之间的旋转，imu_rot_x_ 保存的是当前时刻相对于第一帧 IMU 数据时刻，转动的角度值．

重复这个操作，直到IMU的时间戳比当前帧雷达数据旋转一周之后的时间要晚．也就是这些IMU数据的时间是包含了雷达数据转一圈的时间。解释：IMU 数据是当前时间节点的 x，y，z 三个方向上的角度，新的角度 = 上个角度 + 角速度 * 两帧之间的时间间隔，对三个方向上的角度求上面这个式子。

2.4 轮速计的时间同步与平移计算

轮速计的时间同步方式与 IMU 的基本一致，只是由于轮速计是固定坐标系下的位姿变换，所以直接找到雷达数据开始前的一帧 odom 数据，与雷达旋转一圈之后的一帧 odom 数据，求这两个 odom 数据之间的坐标变换，即可得到雷达旋转一圈时间附近的总平移量。

3、畸变校正

3.1 畸变原因

激光雷达发射第一个点时激光雷达的位置是在(1, 0)处，碰到物体反射回来，测得的距离值为1.3m．由于激光雷达处于前进的状态，激光器旋转一周后，发射最后一个点时激光雷达的位置时假设变成了(1.1, 0)，再次碰到上述物体反射回来，测得的距离值就变为了1.2m．

3.2 校正方法

只需要找到每个激光点对应时刻的激光雷达坐标系，相对于发射第一个点时刻的激光雷达坐标系，间的坐标变换，就可以将每个激光点都变换到发射第一个点时刻的激光雷达坐标系下，就完成了畸变的校正．

• 首先，假设雷达发射第一个点的时刻为 time_start，这时的雷达坐标系为 frame_start．
• 雷达其余点的时刻为 time_point，这时的雷达坐标系为 frame_point，其余点在 frame_point 坐标系下的坐标为 point．
• 只需要找到 frame_start 与 frame_point 间的坐标变换，就可以将 其余点的坐标 point 通过坐标变换变换到 frame_start 坐标系下。对所有点都进行上述操作后，得到的点的坐标，就是去畸变后的坐标了．