VIO中的IMU积分

猫叔Rex

发布于 2021-03-17 10:15:21

1.2K0

发布于 2021-03-17 10:15:21

文章被收录于专栏：科学计算科学计算

VIO中的IMU积分

一、数值积分原理

对于一个给定的微分方程

\dot y(t)=f(t, y(t))

，假设已经知道了初值

y(t_0)

，则其

\Delta t

时刻后的数值积分为：

y(t+\Delta t)=y(t_0)+\int_{t_0}^{t_0+\Delta t} \dot y(t)dt

实际当中我们通常无法获得

\dot y(t)

的表达式，只能对其进行离散采样，然后使用离散积分逼近真实的连续积分。通常近似的思路为：认为

到

t+\Delta t

的微分

\dot y(t)

用恒定常数来表示，则积分表达式可改写为：

y_{n+1}=y_n+\Delta t \cdot k

从上述公式可以看出，减少积分误差的主要方式有两种：

1. 减少采样时间间隔

\Delta t

，时间间隔越小，则离散积分就越接近于连续积分，采样时间间隔取决于sensor的采样率，所以采样率越高积分精度越高；

2. 计算精确的恒定常数

，针对

的通常有三种积分方法：欧拉积分、中值积分和4阶龙格-库塔积分。

二、积分方法

2.1 欧拉积分

欧拉积分假设在倒数区间内的斜率是恒定的，其取

t_n

时刻的斜率作为

t_n

至

t_{n+1}

时间段的斜率，即：

k=\dot y(t_n)=f(t_n,y_n)

从公式可以看出，欧拉积分是最简单的一种积分方式，其逼近误差较大，但计算量很小。

2.2 中值积分

中值积分是在欧拉积分的基础上进行改善。先使用欧拉积分逼近时间间隔

\Delta t

的中点，即

t_n+ \frac{1}{2}\Delta t

的斜率，然后使用中点斜率作为整个时间段内的近似斜率。

首先使用欧拉积分来近似时间段内的中点斜率：

k_1=f(t_n,y_n) \\ y(t_n+\frac{1}{2}\Delta t)=y_n+\frac{1}{2}\Delta t \cdot k_1

然后我们使用得到的时间段中点斜率进一步近似整个时间段的斜率：

k = \dot y(t_n + \frac{1}{2}\Delta t)=f(t_n+\frac{1}{2}\Delta t,y(t_n+\frac{1}{2}\Delta t))=f(t_n+\frac{1}{2}\Delta t, y_n+\frac{1}{2}\Delta t k_1)

显而易见，中值积分比欧拉积分更合理一些。

2.2 4阶龙格-库塔积分

我们直接给出4阶龙格-库塔积分的公式：

k_1=f(t_n,f_n)

k_2=f(t_n+\frac{1}{2}\Delta t,y_n+\frac{1}{2}\Delta t \cdot k_1)

k_3=f(t_n+\frac{1}{2}\Delta t,y_n+\frac{1}{2}\Delta t \cdot k_2)

k_4=f(t_n+\Delta t, y_n+\Delta t \cdot k_3)

k=\frac{1}{6}(k_1+2k_2+2k_3+k_4)

k_1

是起点的斜率，

k_2

是中点的斜率，

k_3

是中点的中点斜率，

k_4

是终点的斜率。实际上4阶龙格-库塔积分就是斜率的加权结果，

k_2

与

k_3

的斜率权重为2，其余为1。显而易见，这种方法的近似精度是最高的。其中

k_1

就是欧拉积分当中的斜率，

k_2

就是中值积分当中的斜率。

4阶龙格-库塔积分的代码如下：

//Indirect Kalaman Filter for 3D Attitude Estimation的第11页
// Note we use Vector4f to represent quaternion instead of quaternion
// because it is better do not normalize q during integration
inline Eigen::Vector4f XpQuaternionDerivative(
    const Eigen::Vector4f &q,
    const Eigen::Vector3f &omega) {

  return -0.5 * XpComposeOmega(omega) * q;
}

//k时刻角速度,k+1时刻角速度,k时刻旋转,dalta_t,k+1时刻旋转
void IntegrateQuaternion(
    const Eigen::Vector3f &omega_begin, const Eigen::Vector3f &omega_end,
    const XpQuaternion &q_begin, const float dt,
    XpQuaternion *q_end,
    XpQuaternion *q_mid,
    XpQuaternion *q_fourth,
    XpQuaternion *q_eighth) {
  CHECK_NOTNULL(q_end);
  XpQuaternion q_0 = q_begin;

  // divide dt time interval into num_segment segments
  // TODO(mingyu): Reduce to 2 or 4 segments as 8 segments may overkill
  const int num_segment = 8;//步数

  // the time duration in each segment
  const float inv_num_segment = 1.0 / num_segment;//步长
  const float dt_seg = dt * inv_num_segment;

  Eigen::Vector3f delta_omega = omega_end - omega_begin;
  for (int i = 0; i < num_segment; ++i) {
    // integrate in the region: [i/num_segment, (i+1)/num_segment]
   //这里利用陀螺仪的测量RK4积分算出前后帧一个imu测量出的旋转,JPL格式四元数
   // 那么XpQuaternionDerivative(q_0, omega_tmp)应该是 - 0.5 * omega(w) × qcur_pre(t)
    Eigen::Vector3f omega_tmp = omega_begin + (i * inv_num_segment) * delta_omega;//当前步长内的imu角速度
    Eigen::Vector4f k1 = XpQuaternionDerivative(q_0, omega_tmp);
    omega_tmp = omega_begin + 0.5 * (2 * i + 1) * inv_num_segment * delta_omega;
    Eigen::Vector4f k2 = XpQuaternionDerivative(q_0 + 0.5 * k1 * dt_seg, omega_tmp);
    Eigen::Vector4f k3 = XpQuaternionDerivative(q_0 + 0.5 * k2 * dt_seg, omega_tmp);
    omega_tmp = omega_begin + (i + 1) * inv_num_segment * delta_omega;
    Eigen::Vector4f k4 = XpQuaternionDerivative(q_0 + k3 * dt_seg, omega_tmp);
    (*q_end) = q_0 + (dt_seg / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);

    // store the start point the next region of integration
    q_0 = (*q_end);

    // store the intermediate attitude as output
    if (i == 0 && q_eighth) {
      (*q_eighth) = (*q_end);
    } else if (i == 1 && q_fourth) {
      (*q_fourth) = (*q_end);
    } else if (i == 3 && q_mid) {
      (*q_mid) = (*q_end);
    }
  }
}

本文参与腾讯云自媒体分享计划，分享自微信公众号。

原始发表：2021-03-14，如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自傅里叶的猫微信公众号，前往查看

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

本文参与腾讯云自媒体分享计划，欢迎热爱写作的你一起参与！

登录后参与评论

0 条评论

热度

VIO中的IMU积分

VIO中的IMU积分

VIO中的IMU积分

一、数值积分原理

二、积分方法

2.1 欧拉积分

2.2 中值积分

2.2 4阶龙格-库塔积分

社区

活动

资源

关于

腾讯云开发者

热门产品

热门推荐

更多推荐