IMU的数学模型与误差标定问题

最新推荐文章于 2024-08-26 18:27:29 发布

skycrygg

最新推荐文章于 2024-08-26 18:27:29 发布

阅读量4k

点赞数 8

分类专栏： SLAM

原文链接：https://xhy3054.github.io/imu-calibration/

版权

SLAM 专栏收录该内容

33 篇文章 41 订阅

订阅专栏

IMU数学模型

加速度计

首先，对于世界坐标系，一般我们会使用最常见的东北天(ENU)坐标系G（无关远点位置，只与姿态有关）。

在这个坐标系中，重力加速度为

此时，假设IMU坐标系就是ENU坐标系，则，静止时有（其中是测量值）：

所以，不静止时：（此处对a和g符号不做区分标记，因为假设body系与Global系一样）

由上可知，其实在物体做自由落体时imu测量的加速度才是0，静止时反而是，这个是由加速度计的测量原理决定的。

上面讲的是在IMU坐标系也是ENU坐标系时的情况（此时位置无关，只关乎姿态）。大多数实际应用中，IMU坐标系（Body）一般是与ENU坐标系有一个姿态的变化的。此时，得到的理论测量值为：

此处是将Global坐标转换到Body坐标姿态的旋转矩阵。此处可以看出，global坐标系的位置与body坐标系的位置与在两个系下测量的加速度大小无关。但是，与姿态有关。

陀螺仪

相比较于加速度计，陀螺仪相对简单。如果不考虑误差，则

我们会发现此处并没有像加速度计一样，将global坐标系下的角速度转换到陀螺仪的测量值，而是直接使用body系下的角速度。这是因为旋转叠加时（比如四元数和旋转矩阵表示姿态时），全局姿态是直接乘以body系下的更新量的就可以得到新的全局姿态的。

恢复运动轨迹

imu最后输出的是一个离散的加速度、角速度序列。我们想做的是利用这些恢复出运动的轨迹（也就是一个位姿的序列）。

下面会介绍两种离散积分的方法。欧拉法与中值法。

这两种方法，都是已知了时刻的位姿，时刻与时刻的测量值（加速度与角速度）。目的是求得时刻的位姿。

欧拉法

欧拉法，是直接使用时刻的测量值来积分。

其中

中值法

中值法，使用两个相邻时刻到的位姿是用两个时刻的测量值的平均值来离散积分。

其中

代码

 for (int i = 0; i < imudata.size()-1; ++i) {

        MotionData imupose = imudata[i];
        MotionData imupose1 = imudata[i+1];
/*
        // 欧拉积分
        //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half =  imupose.imu_gyro * dt /2.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = acc_w * dt + Vw;
        Qwb = Qwb * dq;
        Qwb.normalize();
*/

        /// 中值积分
        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half = (imupose.imu_gyro + imupose1.imu_gyro)*dt/4.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        Eigen::Quaterniond Qwb1 = Qwb * dq;
        Qwb1.normalize();
        Eigen::Vector3d acc_w = (Qwb * imupose.imu_acc + Qwb1 * imupose1.imu_acc)*0.5 + gw;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = acc_w * dt + Vw;  
        Qwb = Qwb1;

        //存储位姿
        save_points<<imupose.timestamp<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<std::endl;
    }

此处推荐一个生成imu数据、加噪声与测试的工具，这里中值积分与欧拉积分可以参考上面的我的代码，因为工具里可能不一定提供了。

旋转积分

上面恢复姿态轨迹的积分用了四元数的形式，下面我想多介绍几个

此处是imu的测量结果，也就是局部角速度。因为这个旋转积分的结果是姿态，因此下面会分别介绍使用四元数、SO3还有欧拉角下的积分方式。

四元数的形式：

形式：

欧拉角形式：

我们可以发现，这三种方法，对于全局姿态的变化，旋转矩阵与四元数都是直接使用imu的测量（也就是局部角速度）进行更新的。只有在欧拉角的形式里，表示将IMU body坐标系下的角速度转化成欧拉角速度。此处可以推导出，这个是三种方法中唯一一个需要转换的地方。原因的话，我觉得这是因为累加与累乘的区别，欧拉角不支持乘法。如果使用角度来表示旋转矩阵与四元数，此时对角度的更新也得使用加法（原来角度加上角速度与时间的积即为新的角度），但是四元数与旋转矩阵本身是只支持乘法的，它们是直接乘以局部扰动即可。