常用的积分方法讨论（数学表达与代码整理）（龙格－库塔、中值积分、欧拉积分）

最新推荐文章于 2022-09-23 14:39:31 发布

擦擦擦大侠

最新推荐文章于 2022-09-23 14:39:31 发布

阅读量1.7k

点赞数 1

本文链接：https://blog.csdn.net/chenshiming1995/article/details/105549526

版权

积分方法讨论（数学表达与代码整理）

数学原理

1.1 四元数与角速度的关系

在无人机或无人车的导航系统中常常采用四元数代替欧拉角来表示机体的旋转，因为欧拉角在计算过程中容易产生奇异，这与欧拉角的计算需要利用正弦、余弦公式相关。

机体姿态的计算中常常需要对四元数进行积分处理。博客¹中给出了四元数与机体角速度的关系，表示如下。下面的积分方法基于该公式。
$\dot{q}_{e}^{b}(t)=\frac{1}{2}\left[\begin{array}{cccc} 0 & -\omega_{x} & -\omega_{y} & -\omega_{z} \\ \omega_{x} & 0 & \omega_{z} & -\omega_{y} \\ \omega_{y} & -\omega_{z} & 0 & \omega_{x} \\ \omega_{z} & \omega_{y} & -\omega_{x} & 0 \end{array}\right] q_{e}^{b}(t)$

1.2 4阶龙格－库塔方法探讨（Runge-Kutta methods）²

算法如下：
$\begin{aligned} &\mathbf{q}_{k+1}=\mathbf{q}_{k}+\Delta t \frac{1}{6}\left(\mathbf{k}_{1}+2 \mathbf{k}_{2}+2 \mathbf{k}_{3}+\mathbf{k}_{4}\right)\\ &\begin{array}{ll} \mathbf{k}_{i}=\mathbf{f}\left(\mathbf{q}^{(i)}, t_{k}+c_{i} \Delta t\right) \\ \mathbf{q}^{(i)}=\mathbf{q}_{k}, & \text { for } i=1 \\ \mathbf{q}^{(i)}=\mathbf{q}_{k}+\Delta t \sum_{j=1}^{i-1} a_{i j} \mathbf{k}_{j}, & \text { for } i>1 \end{array} \end{aligned}$
其中需要的系数 $c_i$ 和 $a_ij$ 是：
$\begin{aligned} c_{1}=0, & c_{2}=\frac{1}{2}, \quad c_{3}=\frac{1}{2}, \quad c_{4}=1 \\ a_{21}=\frac{1}{2}, & a_{31}=0, \quad a_{41}=0 \\ a_{32}=\frac{1}{2}, & a_{42}=0, \quad a_{43}=1 \end{aligned}$
最后我们需要对四元数进行归一化处理。

1.3 欧拉积分³

1.4 中值积分

2 仿真部分

2.1 代码

1. 龙格－库塔

%% 
for k = 2:N
    wp = imu_data(4:6, k-1);
    w1 = imu_data(4:6, k);
    w = (wp+w1)/2;
    ap = imu_data(1:3, k-1);
    a1 = imu_data(1:3, k);
    Ow = [0     -w(1)   -w(2)    -w(3);...
          w(1)   0       w(3)    -w(2);...
          w(2)  -w(3)    0        w(1);...
          w(3)   w(2)   -w(1)     0  ];
    Fc = 0.5*Ow;
    F = eye(4) + Fc*dt;%%或者在这里改变积分的方法(对于矩阵的积分方法)
    quat1 = attitude_update_RK4(quat,dt,wp,w1);
    
    %%
    R_S_n = quat2dcm(quat');
    R_S_n_1 = quat2dcm(quat1');
    acc_w = (R_S_n' * ap + gw   +   R_S_n_1' * a1 + gw)/2;

%     quat = F* quat;
    quat = attitude_update_RK4(quat,dt,wp,w1);
    Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
    Vw = Vw + acc_w * dt;
%%
    quatAll(k,:) = quat';
    oula_new(k,:) = qtpoula(quat)';
    
    PwbSav(k,:) = Pwb';
end

function [Qk_plus1] = attitude_update_RK4(Qk,dt,gyro0,gyro1)
%% /*gyro0代表k-1时刻的角速度，gyro1代表k时刻的角速度*/
    % RK4
    % conference: A Robust and Easy to implement method for imu
    % calibration without External Equipments

    q_1=Qk;
    k1=(1/2)*omegaMatrix(gyro0)*q_1;
    q_2=Qk+dt*(1/2)*k1;
    k2=(1/2)*omegaMatrix((1/2)*(gyro0+gyro1))*q_2;
    q_3=Qk+dt*(1/2)*k2;
    k3=(1/2)*omegaMatrix((1/2)*(gyro0+gyro1))*q_3;
    q_4=Qk+dt*k3;
    k4=(1/2)*omegaMatrix(gyro1)*q_4;
    Qk_plus1=Qk+dt*(k1/6+k2/3+k3/3+k4/6);
    Qk_plus1=Qk_plus1/norm(Qk_plus1);
end
function [omega]=omegaMatrix(data)

    % wx=data(1)*pi/180;
    % wy=data(2)*pi/180;
    % wz=data(3)*pi/180;
    wx=data(1);
    wy=data(2);
    wz=data(3);

    omega=[0  , -wx , -wy , -wz ;...
        wx ,  0  ,  wz , -wy ;...
        wy , -wz ,  0  ,  wx ;...
        wz ,  wy , -wx ,  0   ];
end

2. 欧拉积分：

for k = 1:N
    w =   imu_data(4:6, k);
    a =   imu_data(1:3, k);
    Ow = [0     -w(1)   -w(2)    -w(3);...
          w(1)   0       w(3)    -w(2);...
          w(2)  -w(3)    0        w(1);...
          w(3)   w(2)   -w(1)     0  ];
    Fc = 0.5*Ow;
    F = eye(4) + Fc*dt;%%或者在这里改变积分的方法(对于矩阵的积分方法)

    %%
    R_S_n = quat2dcm(quat');%%Rbw
    acc_w = R_S_n' * a + gw;
    quat = F* quat;
    quat = quat/norm(quat);
    Vw = Vw + acc_w * dt;
    Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
%%
    quatAll(k,:) = quat';
    oula_new(k,:) = qtpoula(quat)';
    PwbSav(k,:) = Pwb';
end

3. 中值积分

for k = 2:N
    w = (imu_data(4:6, k-1)    +    imu_data(4:6, k))/2;
    a = imu_data(1:3, k-1);
    a1 = imu_data(1:3, k);
    Ow = [0     -w(1)   -w(2)    -w(3);...
          w(1)   0       w(3)    -w(2);...
          w(2)  -w(3)    0        w(1);...
          w(3)   w(2)   -w(1)     0  ];
    Fc = 0.5*Ow;
    F = eye(4) + Fc*dt;%%或者在这里改变积分的方法(对于矩阵的积分方法)
    quat1 = F* quat;
    
    
    %%
    R_S_n = quat2dcm(quat');
    R_S_n_1 = quat2dcm(quat1');
    acc_w = (R_S_n' * a + gw   +   R_S_n_1' * a1 + gw)/2;

    quat = F* quat;
    Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
    Vw = Vw + acc_w * dt;
%%
    quatAll(k,:) = quat';
    oula_new(k,:) = qtpoula(quat)';
    
    PwbSav(k,:) = Pwb';
end

2.2 结果

欧拉积分
中值积分
龙格－库塔

https://blog.csdn.net/chenshiming1995/article/details/105107578 ↩︎
A Robust and Easy to Implement Method for IMU Calibration without External Equipments ↩︎
参考贺一家「从0开始写VIO」 ↩︎