LegoLoam(1)imu输入预处理

原创

大阳的冒险岛

修改于 2023-02-17 14:43:27

1.1K0

修改于 2023-02-17 14:43:27

文章被收录于专栏：大阳岛

1.概述

Lego-Loam的通信框架和Aloam相同，都是基于ros，其各个节点的运行数据流图如下:

imageProjection节点负责将原始点云转化为深度图，在深度图上快速进行的地面点分割和目标点聚类，对点云降维。
featureAssociation节点负责提取线面特征，并进行scan-to-scan的特征匹配，估计lidar的相对运动变换T^{k-1}_k 。
mapOptimization节点首先负责利用T^{k-1}_k 预测idar位姿T^w_k ，由scan-to-map的特征点云匹配对T^w_k 进一步优化；其次在并行线程中根据关键帧位置点间的距离检测回环，并采用ICP算法匹配构成回环的关键帧点云，最后进行全局位姿图优化。
transformFusion节点负责将高频运行的featureAssociation节点和低频运行的mapOptimization节点估计的位姿进行整合输出，保证高频的位姿输出。

2.代码

相比于Aloam，Lego-Loam的输入会多一个imu数据，这里首先介绍imu数据预处理

imu数据输入及预处理

imu数据主要用于做lidar点云去畸变以及odometry位姿估计；

首先对输入的imu数据去重力，坐标系遵循常规欧拉角物理定义，imu在世界坐标系下面的角度为\tilde{r} = \left[\begin{matrix} roll & pitch & yaw \end{matrix}\right]^T ，常采用先进行实际意义偏航yaw，再进行实际意义俯仰pitch，最后进行实际意义滚转roll的规则顺序来进行组合旋转构成。仅从转轴顺序上进行对应，则对应了z、y、x的坐标轴顺序，即可表示为：

R^w_I = R_z * R_y * R_x

R^w_I 表示imu坐标系到world坐标系的变换矩阵，即为imu坐标在world坐标系的姿态，所以imu坐标系下面的加速度为

V_I = {(R_z * R_y * R_x)}^T * V_w

令roll = \alpha ，pitch = \beta , yaw = \gamma , 那么有

R_x = \left[\begin{matrix}1 & 0 & 0 \\0 & \cos\alpha & -\sin\alpha \\0 & \sin\alpha & \cos\alpha \end{matrix} \right]\tag{2}

R_y = \left[\begin{matrix}\cos\beta & 0 & \sin\beta \\0 & 1 & 0 \\-\sin\beta & 0 & \cos\beta \end{matrix} \right]\tag{2}

R_z = \left[\begin{matrix}\cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & 0 \\0 & 0 & 1 \end{matrix} \right]\tag{2}

{(R_z \cdot R_y \cdot R_x)}^T = \left[\begin{matrix}\cos\gamma\sin\beta & \cos\beta\sin\gamma & -\sin\beta \\ \cos\gamma\sin\beta\sin\alpha - \cos\alpha\sin\gamma & \cos\gamma\cos\alpha + \sin\gamma\sin\beta\sin\alpha & \cos\beta\sin\alpha \\ \sin\gamma\sin\alpha + \cos\gamma\cos\alpha\sin\beta & \cos\alpha\sin\gamma\sin\beta - \cos\gamma\sin\alpha & \cos\beta\cos\alpha \end{matrix} \right]\tag{2}

imu的原始坐标系为tx朝前、ty朝左、tz朝上，如下图所示

而重力减速度在world坐标系下面的坐标为

V_w = \left[\begin{matrix} 0 & 0 & g \end{matrix}\right]^T

所以有

V_I = \left[\begin{matrix}-9.8 * \sin\beta \\ 9.8 * \cos\beta\sin\alpha \\ 9.8 * \cos\beta\cos\alpha \end{matrix} \right]\tag{2}

然后将imu测量的三轴加速度分别减去重力分量V_I ，得到去重力的加速度。

后来将传统的imu坐标系变换到和相机坐标轴朝向相同的坐标系，矫正后的坐标系为tz朝前、tx朝左、ty朝上，矫正后的坐标系如下图所示

显示在实车上如下图所示：

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
    {
        double roll, pitch, yaw;
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(imuIn->orientation, orientation);
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
        // 这里的roll、pitch、yaw是imu通过重力以及磁力计结算出来的三个欧拉角？？
        // 以下加速度去除重力影响，同时坐标轴进行变换
        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;

        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();
    }

最后通过函数AccumulateIMUShiftAndRotation实现将以上imu坐标系下面的加速度转换到world坐标系，并在world坐标系下面进行imu角速度、加速度的积分得到world坐标系下面的车体的平移和旋转，由于加速度是交换坐标轴后的加速度，因此

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];

        // 先绕Z轴(原x轴)旋转,下方坐标系示意imuHandler()中加速度的坐标轴交换
        //  z->Y
        //  ^  
        //  |    ^ y->X
        //  |   /
        //  |  /
        //  | /
        //  -----> x->Z
        //
        //     |cosrz  -sinrz  0|
        //  Rz=|sinrz  cosrz   0|
        //     |0       0      1|
        // [x1,y1,z1]^T=Rz*[accX,accY,accZ]
        // 因为在imuHandler中进行过坐标变换，
        // 下面的roll其实已经对应于新坐标系中(X-Y-Z)的yaw
        float x1 = cos(roll) * accX - sin(roll) * accY;
        float y1 = sin(roll) * accX + cos(roll) * accY;
        float z1 = accZ;

        // 绕X轴(原y轴)旋转
        // [x2,y2,z2]^T=Rx*[x1,y1,z1]
        //    |1     0        0|
        // Rx=|0   cosrx -sinrx|
        //    |0   sinrx  cosrx|
        float x2 = x1;
        float y2 = cos(pitch) * y1 - sin(pitch) * z1;
        float z2 = sin(pitch) * y1 + cos(pitch) * z1;

        // 最后再绕Y轴(原z轴)旋转
        //    |cosry   0   sinry|
        // Ry=|0       1       0|
        //    |-sinry  0   cosry|
        // 得到world坐标系下面的三个轴加速度 accX、accY、accZ
        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) {

            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;

            imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
            imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
            imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
            // 角速度积分得到imu在world坐标系下面相对初始imu坐标系的旋
            imuAngularRotationX[imuPointerLast] = imuAngularRotationX[imuPointerBack] + imuAngularVeloX[imuPointerBack] * timeDiff;
            imuAngularRotationY[imuPointerLast] = imuAngularRotationY[imuPointerBack] + imuAngularVeloY[imuPointerBack] * timeDiff;
            imuAngularRotationZ[imuPointerLast] = imuAngularRotationZ[imuPointerBack] + imuAngularVeloZ[imuPointerBack] * timeDiff;
        }
    }

原创声明：本文系作者授权腾讯云开发者社区发表，未经许可，不得转载。

如有侵权，请联系 cloudcommunity@tencent.com 删除。

对象存储

原创声明：本文系作者授权腾讯云开发者社区发表，未经许可，不得转载。

如有侵权，请联系 cloudcommunity@tencent.com 删除。

对象存储

登录后参与评论

0 条评论

热度