关于lego-loam的总结（一）

最新推荐文章于 2025-04-26 20:42:35 发布

虚函数机器人

最新推荐文章于 2025-04-26 20:42:35 发布

阅读量7.2k

点赞数 12

分类专栏：建图算法文章标签： slam

本文链接：https://blog.csdn.net/qq_31775031/article/details/116054444

版权

建图算法同时被 2 个专栏收录

4 篇文章

订阅专栏

无人驾驶

3 篇文章

订阅专栏

最近在测试legoloam的时候，总是会碰到雷达坐标系剧烈漂移的情况，即使现实中雷达静止，在rviz中的显示也漂移的很厉害。建的地图也十分糟糕。

首先明确，legoloam中真正实现功能的代码只有三个部分：image.cpp map.cpp feature.cpp 特征识别和雷达、IMU位置变换都集中在了feature.cpp文件中。legoloam的整体结构如下所示（盗的图）

系统接收来自3D激光雷达的输入并输出6个DOF姿势估计。整个系统分为五个模块。首先是segmentation，使用单次扫描的点云，并将其投影到范围图像上进行分段（线）。然后将分段的点云发送到feature extraction模块。然后，激光雷达测距仪使用从前一模块中提取的特征来找到与连续扫描相关的变换。这些特征在lidar mapping中进一步处理，将它们注册到全局点云图。最后，transform integration模块融合了激光雷达测距和激光雷达测绘的姿态估计结果，并输出最终的姿态估计。

一、定位及位姿变换部分

1.雷达位姿估计

legoloam不加IMU传感器也可以很好的实现定位和自身位姿估计等功能。但是有些情形下，雷达会出现坐标严重漂移的情况，导致无法建图。

那么雷达的坐标为什么会漂移，雷达的位置信息是如何计算和确定的？继续查找，我发现雷达的位置信息发布被写在了featureAssociation中的PublishOdometry函数中，具体变量为transform数组！

void publishOdometry(){
        geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]);
         
        laserOdometry.header.stamp = cloudHeader.stamp;
        laserOdometry.pose.pose.orientation.x = -geoQuat.y;//四元数
        laserOdometry.pose.pose.orientation.y = -geoQuat.z;
        laserOdometry.pose.pose.orientation.z = geoQuat.x;
        laserOdometry.pose.pose.orientation.w = geoQuat.w;
        laserOdometry.pose.pose.position.x =transformSum[3];//这里改了雷达坐标就变了
        laserOdometry.pose.pose.position.y =transformSum[4];
        laserOdometry.pose.pose.position.z =transformSum[5];
        pubLaserOdometry.publish(laserOdometry);

        laserOdometryTrans.stamp_ = cloudHeader.stamp;
        laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
        laserOdometryTrans.setOrigin(tf::Vector3(transformSum[3], transformSum[4], transformSum[5]));
        tfBroadcaster.sendTransform(laserOdometryTrans);
    }

transformSum数组的信息更新函数如下integrateTransformation, 该函数不断的通过旋转矩阵更新雷达的位姿状态：

void integrateTransformation(){
        float rx, ry, rz, tx, ty, tz;
        AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
                           -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);

        float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX) 
                 - sin(rz) * (transformCur[4] - imuShiftFromStartY);
        float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX) 
                 + cos(rz) * (transformCur[4] - imuShiftFromStartY);
        float z1 = transformCur[5] - imuShiftFromStartZ;

        float x2 = x1;
        float y2 = cos(rx) * y1 - sin(rx) * z1;
        float z2 = sin(rx) * y1 + cos(rx) * z1;

        tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
        ty = transformSum[4] - y2;
        tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);

        PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, 
                          imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);

        transformSum[0] = rx;
        transformSum[1] = ry;
        transformSum[2] = rz;
        transformSum[3] = tx;
        transformSum[4] = ty;
        transformSum[5] = tz;
    }

那么雷达是如何估算自身的位姿的？由于featrure.cpp文件进行了大量的特征匹配，比如面特征匹配，棱边匹配。和ICP匹配一样，在进行配准之后相应的就得到了位姿的变换矩阵，这是legoloam进行定位的基本原理，对应成代码的话，integrateTransformation函数之前需要加入一个计算变换矩阵的代码结构，就是updateTransformation函数，该函数利用前后帧匹配计算两帧之间的相对位姿变换,计算两帧点云的变换矩阵,函数内部调用了calculateTransformationSurf等特征匹配函数。

void updateTransformation(){

        if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
            return;

        for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
            laserCloudOri->clear();
            coeffSel->clear();

            findCorrespondingSurfFeatures(iterCount1);

            if (laserCloudOri->points.size() < 10)
                continue;
            if (calculateTransformationSurf(iterCount1) == false)
                break;
        }

        for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {

            laserCloudOri->clear();
            coeffSel->clear();

            findCorrespondingCornerFeatures(iterCount2);

            if (laserCloudOri->points.size() < 10)
                continue;
            if (calculateTransformationCorner(iterCount2) == false)
                break;
        }
    }

特征函数一共两个一个是点到线的对应特征findCorrespondingCornerFeatures，一种是点到面的对应特征findCorrespondingSurfFeatures。总结来说，雷达的位姿就是通过点云的特征匹配来求得变换矩阵，再进行位姿更新的。

在legoloam的头文件中，对雷达的机械参数进行了定义，这一部分要参考对应雷达的技术手册，我以镭神16线为例，设定的参数对应如下：

extern const int N_SCAN = 16;
extern const int Horizon_SCAN = 2000;
extern const float ang_res_x = 0.18;
extern const float ang_res_y = 2.0;
extern const float ang_bottom = 15.0;
extern const int groundScanInd = 10;

N_SCAN是线数，Horizon_SCAN是点云数 ang_res_x/ang_res_y分别是水平和垂直分辨率，ang_bottom是垂直视场角，在调试过程中发现 ang_res_x ang_res_y ang_bottom的细小数据差异都会对建图效果产生巨大的影响。猜测是因为在imageproject没款中引入了这三个参数并进行了点云图像化处理，所以参数的设定会影响最后所形成的地图以及定位效果。

2.IMU位姿计算

legoloam有一个特点，不需要IMU也可以进行很好的定位，但是在legoloam中也进行了/imu/data话题的订阅，在调试时，当我订阅IMU的话题时，雷达坐标系漂移的十分剧烈，直接飞了，在这里先不讨论IMU的矩阵坐标转换，直接看IMU的数据解析部分，首先看IMU的回调函数：

   void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
    {
        ROS_INFO("IMU hae been start!");
        double roll, pitch, yaw;
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(imuIn->orientation, orientation);
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
        //加速度去除重力影响，同时坐标轴进行变换(室内无姿态角，先设置为0)

        float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
        float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
        float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
        


       
        imuPointerLast = (imuPointerLast + 1) % imuQueLength;

        imuTime[imuPointerLast] = imuIn->header.stamp.toSec();

        imuRoll[imuPointerLast] = roll;
        imuPitch[imuPointerLast] = pitch;
        imuYaw[imuPointerLast] = yaw;


        imuRoll[imuPointerLast] = roll;
        imuPitch[imuPointerLast] = pitch;
        imuYaw[imuPointerLast] = yaw;

        imuAccX[imuPointerLast] = accX;
        imuAccY[imuPointerLast] = accY;
        imuAccZ[imuPointerLast] = accZ;

        imuAngularVeloX[imuPointerLast] =imuIn->angular_velocity.x;
        imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;
        imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;

        AccumulateIMUShiftAndRotation();
    }

回调函数对IMU里取得的xyz三个方向上的加速度进行了去重力处理。然后将姿态角和加速度，角速度进行了储存。函数最后调用了AccumulateIMUShiftAndRotation函数，这个函数使用IMU姿态计算的矩阵将IMU的测量值从b-frame投影至n-frame (从导航的角度是n系，或者叫global IMU frame) 此坐标系其实是当地地理系(导航的角度看)。之后在此固定坐标系（global IMU frame）下进行积分运算，计算得到0.1s间隔内的位置变换（而且位置、速度变换只能在这个坐标系下n系下完成）。

void AccumulateIMUShiftAndRotation()
    {
        float roll = imuRoll[imuPointerLast];
        float pitch = imuPitch[imuPointerLast];
        float yaw = imuYaw[imuPointerLast];
        float accX = imuAccX[imuPointerLast];
        float accY = imuAccY[imuPointerLast];
        float accZ = imuAccZ[imuPointerLast];

        float x1 = cos(roll) * accX - sin(roll) * accY;
        float y1 = sin(roll) * accX + cos(roll) * accY;
        float z1 = accZ;

        float x2 = x1;
        float y2 = cos(pitch) * y1 - sin(pitch) * z1;
        float z2 = sin(pitch) * y1 + cos(pitch) * z1;

        accX = cos(yaw) * x2 + sin(yaw) * z2;
        accY = y2;
        accZ = -sin(yaw) * x2 + cos(yaw) * z2;

        int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
        double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
        if (timeDiff < scanPeriod) {

            //imushift就是此刻imu的位置信息

            imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;
            imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;
            imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;


	 
            ROS_INFO("x: %f,y: %f,z: %f",imuShiftX[imuPointerLast],imuShiftY[imuPointerLast],imuShiftZ[imuPointerLast]);

            imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
            imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
            imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;

            imuAngularRotationX[imuPointerLast] = imuAngularRotationX[imuPointerBack] + imuAngularVeloX[imuPointerBack] * timeDiff;
            imuAngularRotationY[imuPointerLast] = imuAngularRotationY[imuPointerBack] + imuAngularVeloY[imuPointerBack] * timeDiff;
            imuAngularRotationZ[imuPointerLast] = imuAngularRotationZ[imuPointerBack] + imuAngularVeloZ[imuPointerBack] * timeDiff;
        }
    }

这里可以看到legoloam对IMU的位姿信息计算是很草率的，对于位移信息直接引用的加速度计算位移公式 $X=x+1/2*a*t^2$ 但是对于大部分的IMU来讲，它的内部加速计是十分灵敏的，就算它处于静止状态，加速度也会有一定的值（可能是0.002这种量级的），但是随着时间的累加，位置的误差累计也是越来越大的。但是最新的LIO_SLAM好像解决了这个问题。个人觉得在IMU部分最好引入预积分处理，不能单纯的用加速度位移公式来计算。并且legoloam中没有将IMU的坐标可视化的部分，这些都是可以优化的点。