激光SLAM:激光雷达运动畸变补偿--轮速里程计辅助方法_基于激光雷达的运动补偿方法

前言

在之前博客中梳理了 ALOAM:激光雷达的运动畸变补偿 链接

在ALOAM中由于仅用激光雷达作为传感器,所以只能用帧间匹配的位姿,去补偿下一帧的数据.
举个例子:通过帧间匹配 得到了 k-1 帧到k帧的位姿变换.那么当k+1帧来的时候,就把那个位姿变换认为是k+1帧的起始点和结束点之间的位姿变换,然后通过每个点的时间去线性插值每个点的位姿,最后做补偿计算.

直接匹配的算法就要做一个假设,机器人是匀速运动的.否则凭什么上一时刻帧间的位姿,是该帧的起始点和结束点的位姿呢.

当机器人真的做匀速运动,或者激光雷达更新很快时,这样的假设是合理的.

当机器人上有其它传感器,例如轮速里程计或者IMU,那么就可以利用其它传感器辅助的方式去进行激光雷达的运动畸变补偿.

这篇博客主要讲解下 如何使用轮速里程计进行激光雷达的运动畸变补偿
有理论的讲解和代码的实例

基础理论

机器人在运动的时候激光雷达数据会受影响.发生畸变,数据不能反应真是情况.再用数据进行匹配时则会出错.

产生的原因:

• 激光点数据不是瞬时获得
• 激光测量时伴随着机器人的运动
• 激光帧率较低时,机器人的运动不能忽略

去除畸变方法:

• 纯估计方法
• 里程计辅助方法
• 融合方法

轮速里程计辅助方法去除激光雷达运动畸变

传感器辅助方法(Odom/IMU)优点:

• 极高的位姿更新频率(200Hz),可以比较准确的反应运动情况
• 较高精度的局部位姿估计
• 跟状态估计完全解耦

辅助传感器轮速里程计与IMU的对比:

轮式里程计—直接测量机器人的位移和角度;具有较高的局部角度测量精度;具有较高的局部位置测量精度;更新速度较高(100Hz~200Hz).

惯性测量单元(IMU)—直接测量角速度和线加速度;具有较高的角速度测量精度;测量频率极高(1kHz~8kHz);线加速度精度太差,二次积分在局部的精度依然很差.

所以如果是在地面机器人上,可以使用轮速里程计,那么要比IMU要好.
在不能使用轮速里程计的情况下,例如无人机上,那么可以通过IMU与GPS融合的方式,先进行位置的解算,然后再进行辅助方法去除激光雷达运动畸变

下面介绍通过轮式里程计进行激光雷达畸变去除
已知数据

• 当前帧激光起始时间ts,te
• 里程计数据按照时间顺序存储在一个队列中,队首的时间最早最早的里程计数据的时间戳< ts 最晚的里程计数据的时间戳>te

目标
求解当前帧激光数据中每一个激光点对应的机器人位姿
根据求解的位姿把所有激光点转换到同一坐标系下
重新封装成一帧激光数据,发布出去

步骤
求解ts和te 时刻的位姿ps和pe
如果里程计队列正好和激光数据同步,假设第i和第j个数据的时刻分别为ts , te

但是这种情况基本不存在
更多的情况是在ts和te时刻没有对应的位姿
在ts时刻没有对应的里程计位姿,则进行线性插值。设在l,k时刻有位姿,且l < s < k,则:

按照上面这种线性插值的方式得到ps和pe后,那么再根据当前帧激光每个点的时间可以线性插值得到每个点的位姿,但是整体的线性插值则意味这机器人在ts和te直接做匀速运动

在一帧激光数据之间,如果认为机器人做匀加速运动s=s0+v0t+0.5at*t。那么做二次插值则更为合理
假设机器人的位姿是关于时间t的二次函数

tm是ts和te的中间时刻,那么可以用上面线性插值的方法得到pm

已知ps\pm\pe,可以计算出这条假设的二次曲线,然后曲线上的每个点就都可以得到了

但是,为更接近实际情况,机器人的运动也不是二次曲线,加速度也会改变,那么可以在ts和te直接通过上面里程计线性插值的方式计算m个时刻的位姿

然后,激光雷达每个点对应的位姿则可以在这已知的m+2个位姿里面分段进行线性插值

坐标系统一&激光数据发布
上面求出了一帧激光数据的所有点的位姿
之后把每个点的数据转换到一个坐标系下,然后发布出去即可

Code

int main(int argc,char ** argv)
{
    ros::init(argc,argv,"LidarMotionCalib");//初始化节点

    tf::TransformListener tf(ros::Duration(10.0));//创建tf监听者

    LidarMotionCalibrator tmpLidarMotionCalib(&tf);//实例化激光雷达畸变校正类

    ros::spin();
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11

main函数. 很简单
初始化节点
创建tf监听者 轮速里程计的数据会以tf的形式发布
实例化激光雷达畸变校正的类 校正的功能在类中实现
下面来看这个类
+++++++++++++++++++++++++++++++++++++++++++++++++++

/* 定义 激光雷达畸变校正的类  */
class LidarMotionCalibrator
{
public:
    /*构造函数  传入tf监听者 */
    LidarMotionCalibrator(tf::TransformListener* tf)
    {
        tf_ = tf;//赋值类的私有成员变量  tf的监听者  获得里程计位姿
        scan_sub_ = nh_.subscribe("champion_scan", 10, &LidarMotionCalibrator::ScanCallBack, this);//订阅激光雷达数据
    }

    /*析构函数 */
    ~LidarMotionCalibrator()
    {
        if(tf_!=NULL)
            delete tf_;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

类的名称为LidarMotionCalibrator
构造函数 传入tf监听者
在构造函数中

• 赋值类的私有成员变量 tf的监听者 获得里程计位姿
• 订阅激光雷达数据

private:
    tf::TransformListener* tf_;  //类构造函数传入的tf监听者
    ros::NodeHandle nh_;//节点nh
    ros::Subscriber scan_sub_;//订阅激光雷达句柄
    pcl::PointCloud<pcl::PointXYZRGB> visual_cloud_;//pcl显示点云
1
2
3
4
5

类的私有成员变量就这四个
功能已注释

然后就是类的各个公有函数,在构造函数中 订阅激光雷达数据 数据来了之后进入回调函数
下来看回调函数
+++++++++++++++++++++++++++++++++++++++++++++++++++

    // 拿到原始的激光数据来进行处理
    void ScanCallBack(const champion_nav_msgs::ChampionNavLaserScanPtr& scan_msg)
    {
        //转换到矫正需要的数据
        ros::Time startTime, endTime;//声明该帧雷达的 起始时间ts与结束时间te
        startTime = scan_msg->header.stamp;//获取该帧雷达的起始时间

        champion_nav_msgs::ChampionNavLaserScan laserScanMsg = *scan_msg;//取出该帧雷达数据

        //得到最终点的时间
        int beamNum = laserScanMsg.ranges.size();//获取该帧雷达的激光线束
        endTime = startTime + ros::Duration(laserScanMsg.time_increment * (beamNum - 1));//计算雷达的结束时间  laserScanMsg.time_increment就是激光雷达线束间的dt
1
2
3
4
5
6
7
8
9
10
11
12

这块主要的功能是获取该帧雷达激光束的起始时间和计算该帧激光束的结束时间
起始时间就按消息包的时间戳即可
然后获取激光线束\线束间dt
计算 结束时间 = 起始时间 + 线束间dt*(线束-1)

        // 将数据复制出来
        std::vector<double> angles,ranges;//声明雷达数据向量 角度和距离
        for(int i = beamNum - 1; i >= 0; --i)
        {   
            double lidar_dist = laserScanMsg.ranges[i];//取出该激光束的距离
            double lidar_angle = laserScanMsg.angles[i];//取出该激光束的角度

            //去除距离过小的点与nan点 设置为0
            if(lidar_dist < 0.05 || std::isnan(lidar_dist) || std::isinf(lidar_dist))
                lidar_dist = 0.0;

            ranges.push_back(lidar_dist);//保存数据
            angles.push_back(lidar_angle);//保存数据
        }
1
2
3
4
5
6
7
8
9
10
11
12
13
14

将数据复制出来
去除掉 距离过小的点与nan/inf点
把距离和角度分别对应保存在 两个向量变量中 angles,ranges

        tf::Stamped<tf::Pose> visualPose;// 声明一个tf形式的pose  
        if(!getLaserPose(visualPose, startTime, tf_))//获取该帧雷达开始时刻的位姿(世界坐标系下)
        {
            //获取不到则报错
            ROS_WARN("Not visualPose,Can not Calib");
            return ;
        }

        double visualYaw = tf::getYaw(visualPose.getRotation());//提取世界坐标系下雷达的航向角

        visual_cloud_.clear();//清空显示点云

        /*将原始点云转为世界坐标系下,压入显示点云*/
        for(int i = 0; i < ranges.size();i++)//遍历每个点
        {

            if(ranges[i] < 0.05 || std::isnan(ranges[i]) || std::isinf(ranges[i]))//不处理坏点
                continue;

            double x = ranges[i] * cos(angles[i]);//计算雷达坐标系下的x
            double y = ranges[i] * sin(angles[i]);//计算雷达坐标系下的x

            /*转为世界坐标系*/
            pcl::PointXYZRGB pt;//世界坐标系下的点
            pt.x = x * cos(visualYaw) - y * sin(visualYaw) + visualPose.getOrigin().getX();
            pt.y = x * sin(visualYaw) + y * cos(visualYaw) + visualPose.getOrigin().getY();
            pt.z = 1.0;

            // pack r/g/b into rgb
            unsigned char r = 255, g = 0, b = 0;    //red color
            unsigned int rgb = ((unsigned int)r << 16 | (unsigned int)g << 8 | (unsigned int)b);
            pt.rgb = *reinterpret_cast<float*>(&rgb);

            visual_cloud_.push_back(pt);//将原始点云在世界坐标系下的点 压入显示点云
        }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

获得该帧开始时刻的位姿
然后在for循环里 把该帧的每个点,以开始时刻的位姿转换到世界坐标系下将点的颜色设为红色

        //进行矫正
        Lidar_Calibration(ranges,angles,
                          startTime,
                          endTime,
                          tf_);
1
2
3
4
5

进行矫正的函数.这个之后再详细看.

        /*将校正后的点云转为世界坐标系下,压入显示点云*/
        for(int i = 0; i < ranges.size();i++)//遍历每个点
        {

            if(ranges[i] < 0.05 || std::isnan(ranges[i]) || std::isinf(ranges[i]))//不处理坏点
                continue;

            double x = ranges[i] * cos(angles[i]);//计算雷达坐标系下的x
            double y = ranges[i] * sin(angles[i]);//计算雷达坐标系下的x

            /*转为世界坐标系*/
            pcl::PointXYZRGB pt;
            pt.x = x * cos(visualYaw) - y * sin(visualYaw) + visualPose.getOrigin().getX();
            pt.y = x * sin(visualYaw) + y * cos(visualYaw) + visualPose.getOrigin().getY();
            pt.z = 1.0;

            unsigned char r = 0, g = 255, b = 0;    // green color
            unsigned int rgb = ((unsigned int)r << 16 | (unsigned int)g << 8 | (unsigned int)b);
            pt.rgb = *reinterpret_cast<float*>(&rgb);

            visual_cloud_.push_back(pt);//将校正后的点云在世界坐标系下的点 压入显示点云
        }

        //进行显示
         g_PointCloudView.showCloud(visual_cloud_.makeShared());
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

和上上块的代码类似
在for循环里 把矫正后的该帧的每个点,以开始时刻的位姿转换到世界坐标系下将点的颜色设为绿色

整体的流程就是这样,下面看矫正功能的实现

+++++++++++++++++++++++++++++++++++++++++++++++++++

    //激光雷达数据　分段线性进行插值
    //这里会调用Lidar_MotionCalibration()
    /**
     * @name Lidar_Calibration()
     * @brief 激光雷达数据　分段线性进行差值　分段的周期为5ms
     * @param ranges 激光束的距离值集合
     * @param angle　激光束的角度值集合
     * @param startTime　第一束激光的时间戳
     * @param endTime　最后一束激光的时间戳
     * @param *tf_
    */
    void Lidar_Calibration(std::vector<double>& ranges,
                           std::vector<double>& angles,
                           ros::Time startTime,
                           ros::Time endTime,
                           tf::TransformListener * tf_)
    {
        //统计激光束的数量
        int beamNumber = ranges.size();

        if(beamNumber != angles.size())//距离和角度数量必须一致
        {
            ROS_ERROR("Error:ranges not match to the angles");
            return ;
        }

        // 5ms来进行分段
        int interpolation_time_duration = 5 * 1000;

        tf::Stamped<tf::Pose> frame_start_pose;//起始时刻的位姿
        tf::Stamped<tf::Pose> frame_mid_pose;//分段的中间时刻的位姿
        tf::Stamped<tf::Pose> frame_base_pose;//基准坐标系的位姿  就是起始时刻的位姿
        tf::Stamped<tf::Pose> frame_end_pose;//结束时刻的位姿

        
        double start_time = startTime.toSec() * 1000 * 1000;//起始时间 us
        double end_time = endTime.toSec() * 1000 * 1000;//结束时间 us
        double time_inc = (end_time - start_time) / (beamNumber - 1); // 每束激光数据的时间间隔

        //当前插值的段的起始坐标
        int start_index = 0;

        //起始点的位姿 这里要得到起始点位置的原因是　起始点就是我们的base_pose
        //所有的激光点的基准位姿都会改成我们的base_pose
        // ROS_INFO("get start pose");

        if(!getLaserPose(frame_start_pose, ros::Time(start_time /1000000.0), tf_))//获得起始时刻的位姿
        {
            ROS_WARN("Not Start Pose,Can not Calib");
            return ;
        }

        if(!getLaserPose(frame_end_pose,ros::Time(end_time / 1000000.0),tf_))//获得结束时刻的位姿
        {
            ROS_WARN("Not End Pose, Can not Calib");
            return ;
        }

        int cnt = 0;
        //基准坐标就是第一个位姿的坐标
        frame_base_pose = frame_start_pose;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61

统计激光束的数量
设置用来进行分段的时间 这里设置5ms
定义各个位姿 已注释
计算起始时间\结束时间\线束间时间
计算起始位姿\结束位姿
将基准坐标就是第一个位姿的坐标
然后在for循环里遍历每个点,通过时间进行分段,当满足分段时间要求后,进入该段的校正处理
Lidar_MotionCalibration 就是针对一段的校正处理
处理完该段后,更新下段的开始时间\下段起始下标\下段的开始时刻位姿

下面来看Lidar_MotionCalibration 针对一段的校正处理

+++++++++++++++++++++++++++++++++++++++++++++++++++

    /**
     * @brief Lidar_MotionCalibration
     *        激光雷达运动畸变去除分段函数;
     *        在此分段函数中，认为机器人是匀速运动；
     * @param frame_base_pose       标定完毕之后的基准坐标系
     * @param frame_start_pose      本分段第一个激光点对应的位姿
     * @param frame_end_pose        本分段最后一个激光点对应的位姿
     * @param ranges                激光数据－－距离
     * @param angles                激光数据－－角度
     * @param startIndex            本分段第一个激光点在激光帧中的下标
     * @param beam_number           本分段的激光点数量
     */
    void Lidar_MotionCalibration(
            tf::Stamped<tf::Pose> frame_base_pose,
            tf::Stamped<tf::Pose> frame_start_pose,
            tf::Stamped<tf::Pose> frame_end_pose,
            std::vector<double>& ranges,
            std::vector<double>& angles,
            int startIndex,
            int& beam_number)
    {
       //TODO

        tf::Quaternion start_q = frame_start_pose.getRotation();//起始姿态
        tf::Quaternion end_q = frame_end_pose.getRotation();//结束姿态
        tf::Vector3 start_xy(frame_start_pose.getOrigin().getX(), frame_start_pose.getOrigin().getY(), 1);//起始位置
        tf::Vector3 end_xy(frame_end_pose.getOrigin().getX(), frame_end_pose.getOrigin().getY(), 1);//结束位置
 
        for (size_t i = startIndex; i < startIndex + beam_number; i++) {
            tf::Vector3 mid_xy = start_xy.lerp(end_xy, (i - startIndex) / (beam_number - 1));//位置线性插值
            tf::Quaternion mid_q = start_q.slerp(end_q, (i - startIndex) / (beam_number - 1));//姿态线性插值
            tf::Transform mid_frame(mid_q, mid_xy);//插值结果组合成位姿变换矩阵
            double x = ranges[i] * cos(angles[i]);//当前帧坐标系下的x
            double y = ranges[i] * sin(angles[i]);//当前帧坐标系下的y
            tf::Vector3 calib_point = frame_base_pose.inverse() * mid_frame * tf::Vector3(x, y, 1);//转成基础坐标系下的x,y
            ranges[i] = sqrt(calib_point[0] * calib_point[0] + calib_point[1] * calib_point[1]);//计算距离
            angles[i] = atan2(calib_point[1], calib_point[0]);//计算角度
        }

       //end of TODO
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

这里就是通过点的序号,和该段的起始位姿\结束位姿,来线性插值出该点的位姿
然后将该点转换到基础坐标系下,然后计算距离和角度即可.
注意位置的插值用lerp 四元数的插值用slerp.这个在之前的博客提到了

完毕,这就是整个功能的代码
+++++++++++++++++++++++++++++++++++++++++++++++++++

Result

编译完成后,运行该节点.然后通过rosbag播放数据,可以在pcl的可视化窗口看到校正前的数据与校正后的数据



红色是校正前的数据,绿色是校正后的数据.