lego_loam——featuerAssociation.cpp

最新推荐文章于 2024-07-20 22:49:13 发布

My.科研小菜鸡

最新推荐文章于 2024-07-20 22:49:13 发布

阅读量383

点赞数

分类专栏： # LEGO_LOAM 文章标签： c++

本文链接：https://blog.csdn.net/qq_41861406/article/details/125795691

版权

LEGO_LOAM 专栏收录该内容

3 篇文章 0 订阅

订阅专栏

IMU回调函数：

    //imu回调函数
    void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
    {
        //通过接收到的imuIn里面的四元素得到roll,pitch,yaw三个角
        double roll, pitch, yaw;
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(imuIn->orientation, orientation);
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
        
        // ! step 1 加速度去除重力影响，同时坐标轴进行变换从xyz转换到yzx
        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;
        
        //!step  2   将欧拉角，加速度，速度保存到循环队列中
        imuPointerLast = (imuPointerLast + 1) % imuQueLength;

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

        imuRoll[imuPointerLast] = roll;//横滚角
        imuPitch[imuPointerLast] = pitch;//俯仰角
        imuYaw[imuPointerLast] = yaw;//偏航

        imuAccX[imuPointerLast] = accX;//x轴加速度
        imuAccY[imuPointerLast] = accY;//y轴加速度
        imuAccZ[imuPointerLast] = accZ;//z轴加速度

        imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;//x轴角速度
        imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;//y轴角速度
        imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;//z轴角速度
        
        //! step 3对速度，角速度，加速度进行积分，得到位移，角度和速度
        AccumulateIMUShiftAndRotation();
    }

难点1：

 // ! step 1 加速度去除重力影响，同时坐标轴进行变换从xyz转换到yzx
        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;

本段代码涉及两个操作

1.坐标转换从xyz转换到yzx

imu原始观测值的坐标轴方向为右前上（xyz）
转换为前上右（xyz–>yzx）？？

 // ! step 1 加速度去除重力影响，同时坐标轴进行变换从xyz转换到zxy
        float accX = imuIn->linear_acceleration.y     
        float accY = imuIn->linear_acceleration.z     
        float accZ = imuIn->linear_acceleration.x

2.消除重力加速度对于测量的影响：

欧拉角表示的： $R_b^n ：$

在这里插入图片描述

导航坐标系下重力矢量： $g^n=[0,0,-9.81]$
将重力转换到导航坐标系： $R_b^n)^T*g^n$
原始观测值减去重力影响： $acc-(R_b^n)^T *g^n$

 // ! step 1 坐标轴进行变换从xyz转换到zxy，同时去除重力影响。
        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;