ORB_SLAM3 IMU预积分PreintegrateIMU

2023-10-17 08:30

本文主要是介绍ORB_SLAM3 IMU预积分PreintegrateIMU,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

这篇博文主要分享ORB_SLAM3Tracking::PreintegrateIMU(),其主要包括几个部分:

  • 获得两帧之间的IMU数据
  • 中值积分
  • IMU状态更新

关于IMU的理论推导参考:

  • ORB_SLAM3_IMU预积分理论推导(预积分项)
  • ORB_SLAM3_IMU预积分理论推导(噪声分析)
  • ORB_SLAM3_IMU预积分理论推导(更新)
  • ORB_SLAM3_IMU预积分理论推导(残差)

1.获得两帧之间的IMU数据

主要是为了获取当前帧和前一帧之间的IMU数据,以便用于后续的IMU预积分
在这里插入图片描述

    while(true){// 数据还没有时,会等待一段时间,直到mlQueueImuData中有imu数据.一开始不需要等待bool bSleep = false;{unique_lock<mutex> lock(mMutexImuQueue);if(!mlQueueImuData.empty()){// 拿到第一个imu数据作为起始数据IMU::Point* m = &mlQueueImuData.front();cout.precision(17);// imu起始数据会比当前帧的前一帧时间戳早,如果相差0.001则舍弃这个imu数据if(m->t<mCurrentFrame.mpPrevFrame->mTimeStamp-mImuPer){mlQueueImuData.pop_front();}// 同样最后一个的imu数据时间戳也不能理当前帧时间间隔多余0.001else if(m->t<mCurrentFrame.mTimeStamp-mImuPer){mvImuFromLastFrame.push_back(*m);mlQueueImuData.pop_front();}else{// 得到两帧间的imu数据放入mvImuFromLastFrame中,得到后面预积分的处理数据mvImuFromLastFrame.push_back(*m);break;}}else{break;bSleep = true;}}if(bSleep)usleep(500);}

2.中值积分

在这里插入图片描述

  • 第一帧:线性插值得到 a s t a r t a_{start} astart
    1 2 ( a i + a i + 1 − a i + 1 − a i t a b ⋅ t i n i ) \frac{1}{2} \left (a_{i}+a_{i+1} - \frac{a_{i+1}-a_{i}}{t_{ab}}\cdot t_{ini} \right ) 21(ai+ai+1tabai+1aitini)
  • 中间帧
    1 2 ( a i + a i + 1 ) \frac{1}{2} \left (a_{i}+a_{i+1} \right ) 21(ai+ai+1)
  • 最后帧:线性插值得到 a e n d a_{end} aend
    1 2 ( a i + a i + 1 − a i + 1 − a i t a b ⋅ t e n d ) \frac{1}{2} \left (a_{i}+a_{i+1} - \frac{a_{i+1}-a_{i}}{t_{ab}}\cdot t_{end} \right ) 21(ai+ai+1tabai+1aitend)

这里讲解下上述公式,以第一帧为例:

  • 首先,线性插值得到 a s t a r t a_{start} astart,也就是第一个红点处:
    a 1 − a 0 t 1 − t 0 = a 0 − a t 0 − t ⇒ a = a 0 − a 1 − a 0 t 1 − t 0 ⋅ ( t 0 − t ) \begin{align} \frac{a_{1}-a_{0}}{t_{1}-t_{0}} & = \frac{a_{0}-a}{t_{0}-t} \\ \Rightarrow a & = a_{0} - \frac{a_{1}-a_{0}}{t_{1}-t_{0}} \cdot \left(t_{0}-t\right) \end{align} t1t0a1a0a=t0ta0a=a0t1t0a1a0(t0t)
  • 然后,取a与 a 1 a_{1} a1的平均值,得到

a 1 + a 0 − a 1 − a 0 t 1 − t 0 ⋅ ( t 0 − t ) a_{1} + a_{0} - \frac{a_{1}-a_{0}}{t_{1}-t_{0}} \cdot \left(t_{0}-t\right) a1+a0t1t0a1a0(t0t)

    IMU::Preintegrated* pImuPreintegratedFromLastFrame = new IMU::Preintegrated(mLastFrame.mImuBias,mCurrentFrame.mImuCalib);// 针对预积分位置的不同做不同中值积分的处理/***  根据上面imu帧的筛选,IMU与图像帧的时序如下:*  Frame---IMU0---IMU1---IMU2---IMU3---IMU4---------------IMUx---Frame---IMUx+1*  T_------T0-----T1-----T2-----T3-----T4-----------------Tx-----_T------Tx+1*  A_------A0-----A1-----A2-----A3-----A4-----------------Ax-----_T------Ax+1*  W_------W0-----W1-----W2-----W3-----W4-----------------Wx-----_T------Wx+1*  T_和_T分别表示上一图像帧和当前图像帧的时间戳,A(加速度数据),W(陀螺仪数据),同理*/for(int i=0; i<n; i++){float tstep;Eigen::Vector3f acc, angVel;// 第一帧数据但不是最后两帧,imu总帧数大于2if((i==0) && (i<(n-1))){// 获取相邻两段imu的时间间隔float tab = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t;// 获取当前imu到上一帧的时间间隔float tini = mvImuFromLastFrame[i].t-mCurrentFrame.mpPrevFrame->mTimeStamp;// 设当前时刻imu的加速度a0,下一时刻加速度a1,时间间隔tab 为t10,tini t0p// 正常情况下时为了求上一帧到当前时刻imu的一个平均加速度,但是imu时间不会正好落在上一帧的时刻,需要做补偿,要求得a0时刻到上一帧这段时间加速度的改变量// 有了这个改变量将其加到a0上之后就可以表示上一帧时的加速度了。其中a0 - (a1-a0)*(tini/tab) 为上一帧时刻的加速度再加上a1 之后除以2就为这段时间的加速度平均值// 其中tstep表示a1到上一帧的时间间隔,a0 - (a1-a0)*(tini/tab)这个式子中tini可以是正也可以是负表示时间上的先后,(a1-a0)也是一样,多种情况下这个式子依然成立acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a-(mvImuFromLastFrame[i+1].a-mvImuFromLastFrame[i].a)*(tini/tab))*0.5f;// 计算过程类似加速度angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w-(mvImuFromLastFrame[i+1].w-mvImuFromLastFrame[i].w)*(tini/tab))*0.5f;tstep = mvImuFromLastFrame[i+1].t-mCurrentFrame.mpPrevFrame->mTimeStamp;}else if(i<(n-1)){// 中间的数据不存在帧的干扰,正常计算acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a)*0.5f;angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w)*0.5f;tstep = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t;}// 直到倒数第二个imu时刻时,计算过程跟第一时刻类似,都需要考虑帧与imu时刻的关系else if((i>0) && (i==(n-1))){float tab = mvImuFromLastFrame[i+1].t-mvImuFromLastFrame[i].t;float tend = mvImuFromLastFrame[i+1].t-mCurrentFrame.mTimeStamp;acc = (mvImuFromLastFrame[i].a+mvImuFromLastFrame[i+1].a-(mvImuFromLastFrame[i+1].a-mvImuFromLastFrame[i].a)*(tend/tab))*0.5f;angVel = (mvImuFromLastFrame[i].w+mvImuFromLastFrame[i+1].w-(mvImuFromLastFrame[i+1].w-mvImuFromLastFrame[i].w)*(tend/tab))*0.5f;tstep = mCurrentFrame.mTimeStamp-mvImuFromLastFrame[i].t;}// 就两个数据时使用第一个时刻的,这种情况应该没有吧,,回头应该试试看else if((i==0) && (i==(n-1))){acc = mvImuFromLastFrame[i].a;angVel = mvImuFromLastFrame[i].w;tstep = mCurrentFrame.mTimeStamp-mCurrentFrame.mpPrevFrame->mTimeStamp;}// Step 3.依次进行预积分计算// 应该是必存在的吧,一个是相对上一关键帧,一个是相对上一帧if (!mpImuPreintegratedFromLastKF)cout << "mpImuPreintegratedFromLastKF does not exist" << endl;mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc,angVel,tstep);pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep);}

3.IMU状态更新

在此过程中,视IMU的bias不变,更新的顺序如下,主要和每个变量相互之间的依赖关系有关,其依赖关系如图所示
在这里插入图片描述

更新预积分测量值更新中的dPdV

  • dP包含上一次的dVdR
  • dV包含上一次的dR
Δ v ~ i j − 1 → Δ v ~ i j \Delta \tilde{v}_{ij-1} \to \Delta \tilde{v}_{ij} Δv~ij1Δv~ij

Δ v ~ i j ≜ ∑ k = i j − 1 [ Δ R ~ i k ⋅ ( f ~ k − b i a ) ⋅ Δ t ] ≜ ∑ k = i j − 2 [ Δ R ~ i k ⋅ ( f ~ k − b i a ) ⋅ Δ t ] + Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ⋅ Δ t ≜ Δ v ~ i j − 1 + Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ⋅ Δ t \begin{array}{l} \Delta \tilde{\mathbf{v}}_{i j} \triangleq \sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\right] \\ \triangleq \sum_{k=i}^{j-2}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\right] + \Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\\ \triangleq \Delta \tilde{\mathbf{v}}_{i j-1}+ \Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t \end{array} Δv~ijk=ij1[ΔR~ik(f~kbia)Δt]k=ij2[ΔR~ik(f~kbia)Δt]+ΔR~ij1(f~j1bia)ΔtΔv~ij1+ΔR~ij1(f~j1bia)Δt

Δ p ~ i j − 1 → Δ p ~ i j \Delta \tilde{p}_{ij-1} \to \Delta \tilde{p}_{ij} Δp~ij1Δp~ij

Δ p ~ i j ≜ ∑ k = i j − 1 [ Δ v ~ i k Δ t + 1 2 Δ R ~ i k ⋅ ( f ~ k − b i a ) Δ t 2 ] ≜ ∑ k = i j − 2 [ Δ v ~ i k Δ t + 1 2 Δ R ~ i k ⋅ ( f ~ k − b i a ) Δ t 2 ] + Δ v ~ i j − 1 Δ t + 1 2 Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) Δ t 2 ≜ Δ p ~ i j − 1 + Δ v ~ i j − 1 Δ t + 1 2 Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) Δ t 2 \begin{array}{l} \Delta \tilde{\mathbf{p}}_{i j} \triangleq \sum_{k = i}^{j-1}\left[\Delta \tilde{\mathbf{v}}_{i k} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2}\right] \\ \triangleq \sum_{k = i}^{j-2}\left[\Delta \tilde{\mathbf{v}}_{i k} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2}\right] + \Delta \tilde{\mathbf{v}}_{i j-1} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right) \Delta t^{2} \\ \triangleq \Delta \tilde{\mathbf{p}}_{i j-1} + \Delta \tilde{\mathbf{v}}_{i j-1} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right) \Delta t^{2} \end{array} Δp~ijk=ij1[Δv~ikΔt+21ΔR~ik(f~kbia)Δt2]k=ij2[Δv~ikΔt+21ΔR~ik(f~kbia)Δt2]+Δv~ij1Δt+21ΔR~ij1(f~j1bia)Δt2Δp~ij1+Δv~ij1Δt+21ΔR~ij1(f~j1bia)Δt2

    dP = dP + dV * dt + 0.5f * dR * acc * dt * dt;dV = dV + dR * acc * dt;

更新噪声更新中的AB,主要是与 Δ R ~ i j − 1 \Delta \tilde{\mathbf{R}}_{i j-1} ΔR~ij1有关部分

η i j Δ = [ Δ R ~ j j − 1 0 0 − Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ∧ Δ t I 0 − 1 2 Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ∧ Δ t 2 Δ t I I ] ⏟ A j − 1 η i j − 1 Δ ⋯ + [ J r j − 1 Δ t 0 0 Δ R ~ i j − 1 Δ t 0 1 2 Δ R ~ i j − 1 Δ t 2 ] ⏟ B j − 1 [ ( η j − 1 g d ) T ( η j − 1 a d ) T ] ⏟ η j − 1 d \begin{aligned} \boldsymbol{\eta}_{i j}^{\Delta}= &{\underbrace{ {\left[\begin{array}{ccc} \Delta \tilde{\mathbf{R}}_{j j-1} & \mathbf{0} & \mathbf{0} \\ -\Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \Delta t & \mathbf{I} & \mathbf{0} \\ -\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \Delta t^{2} & \Delta t \mathbf{I} & \mathbf{I} \end{array}\right]}} _{A_{j-1} }\boldsymbol{\eta}_{i j-1}^{\Delta} \cdots } \\ & +\underbrace{\left[\begin{array}{cc} \mathbf{J}_{r}^{j-1} \Delta t & \mathbf{0} \\ \mathbf{0} & \Delta \tilde{\mathbf{R}}_{i j-1} \Delta t \\ \mathbf{0} & \frac{1}{2} \Delta \tilde{\mathbf{R}}_{i j-1} \Delta t^{2} \end{array}\right]}_{B_{j-1}}\underbrace{ \begin{bmatrix} \left(\eta_{j-1}^{gd}\right)^T\\ \left(\eta_{j-1}^{ad}\right)^T \end{bmatrix} } _{\boldsymbol{\eta}_{j-1}^{d}} \end{aligned} ηijΔ=Aj1 ΔR~jj1ΔR~ij1(f~j1bia)Δt21ΔR~ij1(f~j1bia)Δt20IΔtI00I ηij1Δ+Bj1 Jrj1Δt000ΔR~ij1Δt21ΔR~ij1Δt2 ηj1d (ηj1gd)T(ηj1ad)T

    Eigen::Matrix<float, 3, 3> Wacc = Sophus::SO3f::hat(acc);A.block<3, 3>(3, 0) = -dR * dt * Wacc;A.block<3, 3>(6, 0) = -0.5f * dR * dt * dt * Wacc;A.block<3, 3>(6, 3) = Eigen::DiagonalMatrix<float, 3>(dt, dt, dt);B.block<3, 3>(3, 3) = dR * dt;B.block<3, 3>(6, 3) = 0.5f * dR * dt * dt;

更新Jacobian更新中的JPaJPgJVaJVg

  • ∂ Δ v ‾ i j − 1 ∂ b ‾ g → ∂ Δ v ‾ i j ∂ b ‾ g \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} \to \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} bgΔvij1bgΔvij
    ∂ Δ v ‾ i j ∂ b ‾ g = − ∑ k = i j − 1 ( Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t ) = − ∑ k = i j − 2 ( Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t ) − Δ R ‾ i j − 1 ⋅ ( f ~ j − 1 − b ‾ i a ) ∧ ∂ Δ R ‾ i j − 1 ∂ b ‾ g Δ t = ∂ Δ v ‾ i j − 1 ∂ b ‾ g − Δ R ‾ i j − 1 ⋅ ( f ~ j − 1 − b ‾ i a ) ∧ ∂ Δ R ‾ i j − 1 ∂ b ‾ g Δ t \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} =-\sum_{k=i}^{j-1}\left(\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right) \\ =-\sum_{k=i}^{j-2}\left(\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right) - \Delta \overline{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} \Delta t \\ = \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}}-{\color{Green} \Delta \overline{\mathbf{R}}_{i j-1}} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} {\color{Purple} \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}}} \Delta t \end{array} bgΔvij=k=ij1(ΔRik(f~kbia)bgΔRikΔt)=k=ij2(ΔRik(f~kbia)bgΔRikΔt)ΔRij1(f~j1bia)bgΔRij1Δt=bgΔvij1ΔRij1(f~j1bia)bgΔRij1Δt
  • ∂ Δ v ‾ i j − 1 ∂ b ‾ a → ∂ Δ v ‾ i j ∂ b ‾ a \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}} \to \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} baΔvij1baΔvij

∂ Δ v ‾ i j ∂ b ‾ a = − ∑ k = i j − 1 ( Δ R ‾ i k Δ t ) = − ∑ k = i j − 2 ( Δ R ‾ i k Δ t ) − Δ R ‾ i j − 1 Δ t = ∂ Δ v ‾ i j − 1 ∂ b ‾ a − Δ R ‾ i j − 1 Δ t \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} =-\sum_{k=i}^{j-1}\left(\Delta \overline{\mathbf{R}}_{i k} \Delta t\right) \\ =-\sum_{k=i}^{j-2}\left(\Delta \overline{\mathbf{R}}_{i k} \Delta t\right)-\Delta \overline{\mathbf{R}}_{i j-1} \Delta t \\ =\frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}} -{\color{Green} \Delta \overline{\mathbf{R}}_{i j-1} } \Delta t \end{array} baΔvij=k=ij1(ΔRikΔt)=k=ij2(ΔRikΔt)ΔRij1Δt=baΔvij1ΔRij1Δt

  • ∂ Δ p ‾ i j − 1 ∂ b ‾ g → ∂ Δ p ‾ i j ∂ b ‾ g \frac{\partial \Delta \overline{\mathbf{p}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} \to \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} bgΔpij1bgΔpij

∂ Δ p ‾ i j ∂ b ‾ g = ∑ k = i j − 1 [ ∂ Δ v ‾ i k ∂ b ‾ g Δ t − 1 2 Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t 2 ] = ∑ k = i j − 2 [ ∂ Δ v ‾ i k ∂ b ‾ g Δ t − 1 2 Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t 2 ] + ∂ Δ v ‾ i j − 1 ∂ b ‾ g Δ t − 1 2 Δ R ‾ i j − 1 ⋅ ( f ~ j − 1 − b ‾ i a ) ∧ ∂ Δ R ‾ i j − 1 ∂ b ‾ g Δ t 2 = ∂ Δ p ‾ i j − 1 ∂ b ‾ g + ∂ Δ v ‾ i j − 1 ∂ b ‾ g Δ t − 1 2 Δ R ‾ i j − 1 ⋅ ( f ~ j − 1 − b ‾ i a ) ∧ ∂ Δ R ‾ i j − 1 ∂ b ‾ g Δ t 2 \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} =\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right] \\ =\sum_{k=i}^{j-2}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right] + {\color{Red} \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}}} \Delta t-\frac{1}{2} {\color{Green} \Delta \overline{\mathbf{R}}_{i j-1}} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} {\color{Purple} \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} } \Delta t^{2} \\ =\frac{\partial \Delta \overline{\mathbf{p}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} + {\color{Red} \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}}} \Delta t-\frac{1}{2} {\color{Green} \Delta \overline{\mathbf{R}}_{i j-1}} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} {\color{Purple} \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} } \Delta t^{2} \end{array} bgΔpij=k=ij1[bgΔvikΔt21ΔRik(f~kbia)bgΔRikΔt2]=k=ij2[bgΔvikΔt21ΔRik(f~kbia)bgΔRikΔt2]+bgΔvij1Δt21ΔRij1(f~j1bia)bgΔRij1Δt2=bgΔpij1+bgΔvij1Δt21ΔRij1(f~j1bia)bgΔRij1Δt2

  • ∂ Δ p ‾ i j − 1 ∂ b ‾ a → ∂ Δ p ‾ i j ∂ b ‾ a \frac{\partial \Delta \overline{\mathbf{p}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}} \to \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} baΔpij1baΔpij

∂ Δ p ‾ i j ∂ b ‾ a = ∑ k = i j − 1 [ ∂ Δ v ‾ i k ∂ b ‾ a Δ t − 1 2 Δ R ‾ i k Δ t 2 ] = ∑ k = i j − 2 [ ∂ Δ v ‾ i k ∂ b ‾ a Δ t − 1 2 Δ R ‾ i k Δ t 2 ] + ∂ Δ v ‾ i j − 1 ∂ b ‾ a Δ t − 1 2 Δ R ‾ i j − 1 Δ t 2 = ∂ Δ p ‾ i j − 1 ∂ b ‾ a + ∂ Δ v ‾ i j − 1 ∂ b ‾ a Δ t − 1 2 Δ R ‾ i j − 1 Δ t 2 \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} =\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \Delta t^{2}\right] \\ =\sum_{k=i}^{j-2}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \Delta t^{2}\right] + {\color{Red} \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}}} \Delta t-\frac{1}{2} {\color{Green} \Delta \overline{\mathbf{R}}_{i j-1} } \Delta t^{2} \\ =\frac{\partial \Delta \overline{\mathbf{p}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}} + {\color{Red} \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}}} \Delta t-\frac{1}{2} {\color{Green} \Delta \overline{\mathbf{R}}_{i j-1} } \Delta t^{2} \end{array} baΔpij=k=ij1[baΔvikΔt21ΔRikΔt2]=k=ij2[baΔvikΔt21ΔRikΔt2]+baΔvij1Δt21ΔRij1Δt2=baΔpij1+baΔvij1Δt21ΔRij1Δt2

    JPa = JPa + JVa * dt - 0.5f * dR * dt * dt;JPg = JPg + JVg * dt - 0.5f * dR * dt * dt * Wacc * JRg;JVa = JVa - dR * dt;JVg = JVg - dR * dt * Wacc * JRg;

更新dRi

由罗德里格公式计算 Δ R ~ j − 1 j \Delta \tilde{R} _{j-1j} ΔR~j1j

IntegratedRotation::IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias, const float &time)
{// 得到考虑偏置后的角度旋转const float x = (angVel(0) - imuBias.bwx) * time;const float y = (angVel(1) - imuBias.bwy) * time;const float z = (angVel(2) - imuBias.bwz) * time;// 计算旋转矩阵的模值,后面用罗德里格公式计算旋转矩阵时会用到const float d2 = x * x + y * y + z * z;const float d = sqrt(d2);Eigen::Vector3f v;v << x, y, z;// 角度转成叉积的矩阵形式Eigen::Matrix3f W = Sophus::SO3f::hat(v);// eps = 1e-4 是一个小量,根据罗德里格斯公式求极限,后面的高阶小量忽略掉得到此式if (d < eps){deltaR = Eigen::Matrix3f::Identity() + W;rightJ = Eigen::Matrix3f::Identity();}else{deltaR = Eigen::Matrix3f::Identity() + W * sin(d) / d + W * W * (1.0f - cos(d)) / d2;rightJ = Eigen::Matrix3f::Identity() - W * (1.0f - cos(d)) / d2 + W * W * (d - sin(d)) / (d2 * d);}
}

更新预积分测量值更新中的dR

  • Δ R ~ i j − 1 → Δ R ~ i j \Delta \tilde{R} _{ij-1}\to \Delta \tilde{R}_{ij} ΔR~ij1ΔR~ij
    Δ R ~ i j = ∏ k = i j − 1 Exp ⁡ ( ( ω ~ k − b i g ) Δ t ) = ∏ k = i j − 2 Exp ⁡ ( ( ω ~ k − b i g ) Δ t ) ⋅ Exp ⁡ ( ( ω ~ j − 1 − b i g ) Δ t ) = Δ R ~ i j − 1 ⋅ Δ R ~ j − 1 j \begin{array}{l} \Delta \tilde{\mathbf{R}}_{i j}=\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \\ =\prod_{k=i}^{j-2} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right)\cdot \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{j-1}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \\ =\Delta \tilde{\mathbf{R}}_{i j-1}\cdot\Delta \tilde{\mathbf{R}}_{j-1 j} \end{array} ΔR~ij=k=ij1Exp((ω~kbig)Δt)=k=ij2Exp((ω~kbig)Δt)Exp((ω~j1big)Δt)=ΔR~ij1ΔR~j1j
dR = NormalizeRotation(dR * dRi.deltaR);

更新噪声更新中的AB

η i j Δ = [ Δ R ~ j j − 1 0 0 − Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ∧ Δ t I 0 − 1 2 Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ∧ Δ t 2 Δ t I I ] ⏟ A j − 1 η i j − 1 Δ ⋯ + [ J r j − 1 Δ t 0 0 Δ R ~ i j − 1 Δ t 0 1 2 Δ R ~ i j − 1 Δ t 2 ] ⏟ B j − 1 [ ( η j − 1 g d ) T ( η j − 1 a d ) T ] ⏟ η j − 1 d \begin{aligned} \boldsymbol{\eta}_{i j}^{\Delta}= &{\underbrace{ {\left[\begin{array}{ccc} \Delta \tilde{\mathbf{R}}_{j j-1} & \mathbf{0} & \mathbf{0} \\ -\Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \Delta t & \mathbf{I} & \mathbf{0} \\ -\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \Delta t^{2} & \Delta t \mathbf{I} & \mathbf{I} \end{array}\right]}} _{A_{j-1} }\boldsymbol{\eta}_{i j-1}^{\Delta} \cdots } \\ & +\underbrace{\left[\begin{array}{cc} \mathbf{J}_{r}^{j-1} \Delta t & \mathbf{0} \\ \mathbf{0} & \Delta \tilde{\mathbf{R}}_{i j-1} \Delta t \\ \mathbf{0} & \frac{1}{2} \Delta \tilde{\mathbf{R}}_{i j-1} \Delta t^{2} \end{array}\right]}_{B_{j-1}}\underbrace{ \begin{bmatrix} \left(\eta_{j-1}^{gd}\right)^T\\ \left(\eta_{j-1}^{ad}\right)^T \end{bmatrix} } _{\boldsymbol{\eta}_{j-1}^{d}} \end{aligned} ηijΔ=Aj1 ΔR~jj1ΔR~ij1(f~j1bia)Δt21ΔR~ij1(f~j1bia)Δt20IΔtI00I ηij1Δ+Bj1 Jrj1Δt000ΔR~ij1Δt21ΔR~ij1Δt2 ηj1d (ηj1gd)T(ηj1ad)T

    A.block<3, 3>(0, 0) = dRi.deltaR.transpose();B.block<3, 3>(0, 0) = dRi.rightJ * dt;

更新Jacobian更新中的JRg

  • ∂ Δ p ‾ i j − 1 ∂ b ‾ a → ∂ Δ p ‾ i j ∂ b ‾ a \frac{\partial \Delta \overline{\mathbf{p}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}} \to \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} baΔpij1baΔpij

∂ Δ p ‾ i j ∂ b ‾ a = ∑ k = i j − 1 [ ∂ Δ v ‾ i k ∂ b ‾ a Δ t − 1 2 Δ R ‾ i k Δ t 2 ] = ∑ k = i j − 2 [ ∂ Δ v ‾ i k ∂ b ‾ a Δ t − 1 2 Δ R ‾ i k Δ t 2 ] + ∂ Δ v ‾ i j − 1 ∂ b ‾ a Δ t − 1 2 Δ R ‾ i j − 1 Δ t 2 = ∂ Δ p ‾ i j − 1 ∂ b ‾ a + ∂ Δ v ‾ i j − 1 ∂ b ‾ a Δ t − 1 2 Δ R ‾ i j − 1 Δ t 2 \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} =\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \Delta t^{2}\right] \\ =\sum_{k=i}^{j-2}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \Delta t^{2}\right] + {\color{Red} \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}}} \Delta t-\frac{1}{2} {\color{Green} \Delta \overline{\mathbf{R}}_{i j-1} } \Delta t^{2} \\ =\frac{\partial \Delta \overline{\mathbf{p}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}} + {\color{Red} \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}}} \Delta t-\frac{1}{2} {\color{Green} \Delta \overline{\mathbf{R}}_{i j-1} } \Delta t^{2} \end{array} baΔpij=k=ij1[baΔvikΔt21ΔRikΔt2]=k=ij2[baΔvikΔt21ΔRikΔt2]+baΔvij1Δt21ΔRij1Δt2=baΔpij1+baΔvij1Δt21ΔRij1Δt2

  • ∂ Δ R ‾ i j − 1 ∂ b ‾ g → ∂ Δ R ‾ i j ∂ b ‾ g \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} \to \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} bgΔRij1bgΔRij

∂ Δ R ‾ i j ∂ b ‾ g = ∑ k = i j − 1 ( − Δ R ‾ k + 1 j T J r k Δ t ) = ∑ k = i j − 2 ( − Δ R ‾ k + 1 j T J r k Δ t ) − Δ R ‾ j j T J r k Δ t = ∑ k = i j − 2 ( − ( Δ R ‾ k + 1 j − 1 Δ R ‾ j − 1 j ) T J r k Δ t ) − Δ R ‾ j j T J r k Δ t = − Δ R ‾ j − 1 j T ⋅ ∑ k = i j − 2 ( Δ R ‾ k + 1 j − 1 T J r k Δ t ) − Δ R ‾ j j T J r k Δ t = − Δ R ‾ j − 1 j T ⋅ ∂ Δ R ‾ i j − 1 ∂ b ‾ g − Δ R ‾ j j T ⏟ I J r k Δ t \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}}=\sum_{k=i}^{j-1}\left(-\Delta \overline{\mathbf{R}}_{k+1 j}^{T} \mathbf{J}_{r}^{k} \Delta t\right) \\ =\sum_{k=i}^{j-2}\left(-\Delta \overline{\mathbf{R}}_{k+1 j}^{T} \mathbf{J}_{r}^{k} \Delta t\right) -\Delta \overline{\mathbf{R}}_{j j}^{T} \mathbf{J}_{r}^{k} \Delta t \\ =\sum_{k=i}^{j-2}\left(-\left(\Delta \overline{\mathbf{R}}_{k+1 j-1}\Delta \overline{\mathbf{R}}_{j-1 j} \right)^{T}\mathbf{J}_{r}^{k} \Delta t\right) -\Delta \overline{\mathbf{R}}_{j j}^{T} \mathbf{J}_{r}^{k} \Delta t \\ =-\Delta \overline{\mathbf{R}}_{ j-1 j}^{T}\cdot \sum_{k=i}^{j-2}\left(\Delta \overline{\mathbf{R}}_{k+1 j-1}^{T} \mathbf{J}_{r}^{k} \Delta t\right) -\Delta \overline{\mathbf{R}}_{j j}^{T} \mathbf{J}_{r}^{k} \Delta t \\ = -\Delta \overline{\mathbf{R}}_{ j-1 j}^{T}\cdot \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} -\underbrace{\Delta \overline{\mathbf{R}}_{j j}^{T}}_{I} \mathbf{J}_{r}^{k} \Delta t \end{array} bgΔRij=k=ij1(ΔRk+1jTJrkΔt)=k=ij2(ΔRk+1jTJrkΔt)ΔRjjTJrkΔt=k=ij2((ΔRk+1j1ΔRj1j)TJrkΔt)ΔRjjTJrkΔt=ΔRj1jTk=ij2(ΔRk+1j1TJrkΔt)ΔRjjTJrkΔt=ΔRj1jTbgΔRij1I ΔRjjTJrkΔt

JRg = dRi.deltaR.transpose() * JRg - dRi.rightJ * dt;

完整代码

void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt)
{// 保存imu数据,利用中值积分的结果构造一个预积分类保存在mvMeasurements中mvMeasurements.push_back(integrable(acceleration, angVel, dt));// Position is updated firstly, as it depends on previously computed velocity and rotation.// Velocity is updated secondly, as it depends on previously computed rotation.// Rotation is the last to be updated.// Matrices to compute covariance// Step 1.构造协方差矩阵// 噪声矩阵的传递矩阵,这部分用于计算i到j-1历史噪声或者协方差Eigen::Matrix<float, 9, 9> A;A.setIdentity();// 噪声矩阵的传递矩阵,这部分用于计算j-1新的噪声或协方差,这两个矩阵里面的数都是当前时刻的,计算主要是为了下一时刻使用Eigen::Matrix<float, 9, 6> B;B.setZero();// 考虑偏置后的加速度、角速度Eigen::Vector3f acc, accW;acc << acceleration(0) - b.bax, acceleration(1) - b.bay, acceleration(2) - b.baz;accW << angVel(0) - b.bwx, angVel(1) - b.bwy, angVel(2) - b.bwz;// 记录平均加速度和角速度avgA = (dT * avgA + dR * acc * dt) / (dT + dt);avgW = (dT * avgW + accW * dt) / (dT + dt);// Update delta position dP and velocity dV (rely on no-updated delta rotation)// 根据没有更新的dR来更新dP与dV  eq.(38)dP = dP + dV * dt + 0.5f * dR * acc * dt * dt;dV = dV + dR * acc * dt;// Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)// 根据η_ij = A * η_i,j-1 + B_j-1 * η_j-1中的A矩阵和B矩阵对速度和位移进行更新Eigen::Matrix<float, 3, 3> Wacc = Sophus::SO3f::hat(acc);A.block<3, 3>(3, 0) = -dR * dt * Wacc;A.block<3, 3>(6, 0) = -0.5f * dR * dt * dt * Wacc;A.block<3, 3>(6, 3) = Eigen::DiagonalMatrix<float, 3>(dt, dt, dt);B.block<3, 3>(3, 3) = dR * dt;B.block<3, 3>(6, 3) = 0.5f * dR * dt * dt;// Update position and velocity jacobians wrt bias correction// 因为随着时间推移,不可能每次都重新计算雅克比矩阵,所以需要做J(k+1) = j(k) + (~)这类事,分解方式与AB矩阵相同// 论文作者对forster论文公式的基础上做了变形,然后递归更新,参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212JPa = JPa + JVa * dt - 0.5f * dR * dt * dt;JPg = JPg + JVg * dt - 0.5f * dR * dt * dt * Wacc * JRg;JVa = JVa - dR * dt;JVg = JVg - dR * dt * Wacc * JRg;// Update delta rotation// Step 2. 构造函数,会根据更新后的bias进行角度积分IntegratedRotation dRi(angVel, b, dt);// 强行归一化使其符合旋转矩阵的格式dR = NormalizeRotation(dR * dRi.deltaR);// Compute rotation parts of matrices A and B// 补充AB矩阵A.block<3, 3>(0, 0) = dRi.deltaR.transpose();B.block<3, 3>(0, 0) = dRi.rightJ * dt;// 小量delta初始为0,更新后通常也为0,故省略了小量的更新// Update covariance// Step 3.更新协方差,frost经典预积分论文的第63个公式,推导了噪声(ηa, ηg)对dR dV dP 的影响C.block<9, 9>(0, 0) = A * C.block<9, 9>(0, 0) * A.transpose() + B * Nga * B.transpose();  // B矩阵为9*6矩阵 Nga 6*6对角矩阵,3个陀螺仪噪声的平方,3个加速度计噪声的平方// 这一部分最开始是0矩阵,随着积分次数增加,每次都加上随机游走,偏置的信息矩阵C.block<6, 6>(9, 9) += NgaWalk;// Update rotation jacobian wrt bias correction// 计算偏置的雅克比矩阵,r对bg的导数,∂ΔRij/∂bg = (ΔRjj-1) * ∂ΔRij-1/∂bg - Jr(j-1)*t// 论文作者对forster论文公式的基础上做了变形,然后递归更新,参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212// ? 为什么先更新JPa、JPg、JVa、JVg最后更新JRg? 答:这里必须先更新dRi才能更新到这个值,但是为什么JPg和JVg依赖的上一个JRg值进行更新的?JRg = dRi.deltaR.transpose() * JRg - dRi.rightJ * dt;// Total integrated time// 更新总时间dT += dt;
}

这篇关于ORB_SLAM3 IMU预积分PreintegrateIMU的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!


原文地址:https://blog.csdn.net/He3he3he/article/details/131341125
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.chinasem.cn/article/224197

相关文章

微积分-积分应用5.4(功)

术语“功”在日常语言中用来表示完成一项任务所需的总努力量。在物理学中,它有一个依赖于“力”概念的技术含义。直观上,你可以将力理解为对物体的推或拉——例如,一个书本在桌面上的水平推动,或者地球对球的向下拉力。一般来说,如果一个物体沿着一条直线运动,位置函数为 s ( t ) s(t) s(t),那么物体上的力 F F F(与运动方向相同)由牛顿第二运动定律给出,等于物体的质量 m m m 与其

变速积分PID控制算法

变速积分PID控制算法 变速积分PID控制算法:变速积分PID的基本思想:变速积分的PID积分项表达式: 注:本文内容摘自《先进PID控制MATLAB仿真(第4版)》刘金琨 编著,研读此书受益匪浅,感谢作者! 变速积分PID控制算法: 在普通的PID控制算法中,由于积分系数 k i k_i ki​是常数,所以在整个控制过程中,积分增量不变。而系统对积分项的要求是,系统偏差大

梯形积分PID控制算法

梯形积分PID控制算法 梯形积分PID控制算法: 注:本文内容摘自《先进PID控制MATLAB仿真(第4版)》刘金琨 编著,研读此书受益匪浅,感谢作者! 梯形积分PID控制算法: 在PID控制律中积分项的作用是消除余差,为了减小余差,应提高积分项的运算精度,为此,可将矩形积分改为梯形积分。梯形积分的计算公式: ∫ 0 t e ( t ) d t = ∑ i = 0 k e

抗积分饱和PID控制算法

抗积分饱和PID控制算法 抗积分饱和PID控制算法:1.积分饱和现象:2.抗积分饱和算法: 注:本文内容摘自《先进PID控制MATLAB仿真(第4版)》刘金琨 编著,研读此书受益匪浅,感谢作者! 抗积分饱和PID控制算法: 1.积分饱和现象: 所谓积分饱和现象是指若系统存在一个方向偏差,PID控制器的输出由于积分作用的不断累加而加大,从而导致执行机构到达极限位置 X m

积分分离PID控制算法

积分分离PID控制算法 积分分离PID控制:积分分离控制基本思路:积分分离控制算法表示:积分分离式PID控制算法程序流程图: 注:本文内容摘自《先进PID控制MATLAB仿真(第4版)》刘金琨 编著,研读此书受益匪浅,感谢作者! 积分分离PID控制: 在普通的PID控制中引入积分环节的目的,主要为了消除静差,提高控制精度。但在过程启动、结束或大幅度增减设定时,短时间内系统输出

IMU腕带评估轮椅用户运动健康

近期,美国的研究团队利用惯性测量单元(IMU)和机器学习来准确评估手动轮椅使用者的运动健康状况,这在康复训练和慢性病管理领域具有广阔的应用前景。 研究小组将运用高性能的IMU传感器固定到轮椅使用者佩戴的手腕带上,用来监测并记录轮椅推进过程中的运动数据。实验设置了不同强度的六分钟推力测试,结果证实仅使用IMU传感器就能准确捕捉到轮椅使用者的速度、距离和节奏变化,为心血管健康评估提供

IMU助力JAXA空间站机器人

近日,日本宇宙航空研究开发机构(JAXA)宣布,在国际空间站(ISS)实验舱“希望号”(Kibo)上部署的一款移动摄像机器人将采用Epson M-G370系列惯性测量单元(IMU)。IMU是一种能够检测物体运动状态的装置,通过测量加速度和角速度来确定物体的空间位置和姿态。这种技术对于在缺乏固定参照物的空间环境中尤为重要。 自2023年6月以来,“内部球形相机2号”(Internal Ball C

c++求积分算法

//c++ 作业:用两重菜单显示 积分函数和积分方法#include<iostream>#include<cmath>using namespace std;class Function{public:virtual double operator()(double x) const=0; };class Function1:public Function{publi

Azure_Kinect_ROS版编译及IMU参数标定.

Azure_Kinect_ROS版编译 参照官方文档:https://github.com/microsoft/Azure_Kinect_ROS_Driver/blob/melodic/docs/building.md 编译. 如果是ubuntu18.04等通过命令行安装SDK的,应该可以轻松编译成功.但是如果是在16.04上安装的(安装方法参照https://blog.csdn.net/u01

编译imu_utils时遇到的问题

https://blog.csdn.net/fang794735225/article/details/92804030 中提到了部分问题, 此外还有eigen找不到等问题,这些都好解决. 还有部分可能遇到的非常恶心的问题是: gcc , g++版本问题导致的ros link失败, 会出现如undefined reference ros::init等问题. 将其版本升级的5以上可解决: sud