1.设置IMU仿真代码中的不同参数，生成Allan方差标定曲线

1.1 imu_utils结果运行及分析

在默认参数下，imu_utils的输出信息如下，可以看出，输出信息中只显示了角度随机游走噪声和零偏稳定性噪声，没有角速率随机游走噪声。

waihekor@waihekor-G7-7588:~$ roslaunch imu_utils imu.launch
... logging to /home/waihekor/.ros/log/02c3a164-4d41-11ea-abad-181dea39b7e1/roslaunch-waihekor-G7-7588-9605.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://waihekor-G7-7588:33481/

SUMMARY
========

PARAMETERS
 * /imu_an/data_save_path: /home/waihekor/ca...
 * /imu_an/imu_name: imutest
 * /imu_an/imu_topic: /imu
 * /imu_an/max_cluster: 100
 * /imu_an/max_time_min: 120
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /
    imu_an (imu_utils/imu_an)

ROS_MASTER_URI=http://localhost:11311

process[imu_an-1]: started with pid [9622]
[ INFO] [1581475389.253698295]: Loaded imu_topic: /imu
[ INFO] [1581475389.255509997]: Loaded imu_name: imutest
[ INFO] [1581475389.257141539]: Loaded data_save_path: /home/waihekor/catkin_imu/src/imu_utils/new_data/
[ INFO] [1581475389.258722220]: Loaded max_time_min: 120
[ INFO] [1581475389.260228528]: Loaded max_cluster: 100
gyr x  num of Cluster 100
gyr y  num of Cluster 100
gyr z  num of Cluster 100
acc x  num of Cluster 100
acc y  num of Cluster 100
acc z  num of Cluster 100
wait for imu data.
gyr x  numData 796612
gyr x  start_t 1.58126e+09
gyr x  end_t 1.58127e+09
gyr x dt
-------------7203 s
-------------120.05 min
-------------2.00083 h
gyr x  freq 110.594
gyr x  period 0.00904205
gyr y  numData 796612
gyr y  start_t 1.58126e+09
gyr y  end_t 1.58127e+09
gyr y dt
-------------7203 s
-------------120.05 min
-------------2.00083 h
gyr y  freq 110.594
gyr y  period 0.00904205
gyr z  numData 796612
gyr z  start_t 1.58126e+09
gyr z  end_t 1.58127e+09
gyr z dt
-------------7203 s
-------------120.05 min
-------------2.00083 h
gyr z  freq 110.594
gyr z  period 0.00904205
Gyro X
C   2.05826   4146.45  -17.6949   5.70864 -0.110374
 Bias Instability 0.000796122 rad/s
 Bias Instability 0.000687128 rad/s, at 2370.33 s
 White Noise 760.997 rad/s
 White Noise 0.211588 rad/s
  bias 0.0854403 degree/s
-------------------
Gyro y
C   2.6287  4138.81  40.1669 -5.04356 0.193993
 Bias Instability 4.44366e-05 rad/s
 Bias Instability 0.00101889 rad/s, at 672.177 s
 White Noise 760.927 rad/s
 White Noise 0.211388 rad/s
  bias 0.151585 degree/s
-------------------
Gyro z
C 0.945991  4147.56  37.1063 -5.31173 0.296005
 Bias Instability 0.000600739 rad/s
 Bias Instability 0.00118061 rad/s, at 522.413 s
 White Noise 770.95 rad/s
 White Noise 0.214194 rad/s
  bias 0.179184 degree/s
-------------------
==============================================
==============================================
acc x  numData 796612
acc x  start_t 1.58126e+09
acc x  end_t 1.58127e+09
acc x dt
-------------7203 s
-------------120.05 min
-------------2.00083 h
acc x  freq 110.594
acc x  period 0.00904205
acc y  numData 796612
acc y  start_t 1.58126e+09
acc y  end_t 1.58127e+09
acc y dt
-------------7203 s
-------------120.05 min
-------------2.00083 h
acc y  freq 110.594
acc y  period 0.00904205
acc z  numData 796612
acc z  start_t 1.58126e+09
acc z  end_t 1.58127e+09
acc z dt
-------------7203 s
-------------120.05 min
-------------2.00083 h
acc z  freq 110.594
acc z  period 0.00904205
acc X
C -7.9812e-05   0.0262253 -0.00120074 0.000286063 1.46279e-07
 Bias Instability 0.00386438 m/s^2
 White Noise 0.267536 m/s^2
-------------------
acc y
C -9.59688e-05    0.0263255   -0.0012822  0.000332272   8.1427e-07
 Bias Instability 0.00425269 m/s^2
 White Noise 0.268581 m/s^2
-------------------
acc z
C -2.93949e-05    0.0258152 -0.000475965  8.92749e-05  4.12915e-06
 Bias Instability 0.00329462 m/s^2
 White Noise 0.266947 m/s^2
-------------------
[imu_an-1] process has finished cleanly
log file: /home/waihekor/.ros/log/02c3a164-4d41-11ea-abad-181dea39b7e1/imu_an-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done
waihekor@waihekor-G7-7588:~$

matlab脚本如下图所示：

clear 
close all
% dt = dlmread('../new_data/data_imutest_acc_t.txt');         
% data_x = dlmread('../new_data/data_imutest_acc_x.txt'); 
% data_y= dlmread('../new_data/data_imutest_acc_y.txt'); 
% data_z = dlmread('../new_data/data_imutest_acc_z.txt'); 

dt = dlmread('../new_data/data_imutest_gyr_t.txt');         
data_x = dlmread('../new_data/data_imutest_gyr_x.txt'); 
data_y= dlmread('../new_data/data_imutest_gyr_y.txt'); 
data_z = dlmread('../new_data/data_imutest_gyr_z.txt'); 

data_draw=[data_x data_y data_z];

data_sim_x= dlmread('../new_data/data_imutest_sim_gyr_x.txt'); 
data_sim_y= dlmread('../new_data/data_imutest_sim_gyr_y.txt'); 
data_sim_z= dlmread('../new_data/data_imutest_sim_gyr_z.txt'); 

% data_sim_x= dlmread('../new_data/data_imutest_sim_acc_x.txt'); 
% data_sim_y= dlmread('../new_data/data_imutest_sim_acc_y.txt'); 
% data_sim_z= dlmread('../new_data/data_imutest_sim_acc_z.txt'); 

data_sim_draw=[data_sim_x data_sim_y data_sim_z];


figure
loglog(dt, data_draw , 'o');
% loglog(dt, data_sim_draw , '-');
xlabel('time:sec');                   
ylabel('Sigma:deg/h');  
% legend('x','y','z');      
grid on;                           
hold on;                           
loglog(dt, data_sim_draw , '-');

运行matlab脚本，陀螺仪的艾伦曲线方差如下:

由于bias随机游走噪声很难从图中直观看出，所以这里只给出角度随机游走的看图计算结果。
从图中可以计算出高斯白噪声的标准差为：$69.67 \frac{d e g}{\sqrt{h}}$69.67h​deg​，和代码中给出的参数$51.57 \frac{d e g}{\sqrt{h}}$51.57h​deg​有些差异。
下图是加速度计的艾伦方差曲线：

从图中可以读出，加速度计的速度随机游走参数为$0.0254 \frac{m}{s \sqrt{h r}}$0.0254shr​m​，和代码中的标称值$0.019 \frac{m}{s \sqrt{h r}}$0.019shr​m​相差不太。

1.2 kalibr_allan结果运行及分析

在kalibr_allan中运行程序，由imu.bag生成imu.mat时遇到一个错误提示，vio群里有人说是matlab版本问题，我装的版本是2017，如果是2018，就没有上述问题，但是生成的数据是没有错的。

生成imu.mat文件之后，运行matlab文件中的脚本，需要注意的是要先运行SCRIPT_allan_matparallel.m脚本，生成数据，修改SCRIPT_process_results.m脚本文件中的mat文件路径，修改后的mat文件如下图所示：

最终的运行结果如下图所示：

输出的结果整理成下表所示，可以看到和真实的模拟结果相差较小。

噪声类型	数值	单位
Gyroscope “random walk”	0.000047	$\frac{r a d}{s^{2}} \frac{1}{\sqrt{H z}}$s2rad​Hz​1​
Accelerometer “random walk”	0.000479	$\frac{m}{s^{3}} \frac{1}{\sqrt{H z}}$s3m​Hz​1​
Gyroscope “white noise”	0.015159	$\frac{r a d}{s} \frac{1}{\sqrt{H z}}$srad​Hz​1​
Accelerometer “white noise”	0.019164	$\frac{m}{s^{2}} \frac{1}{\sqrt{H z}}$s2m​Hz​1​

1.3 其他结果运行及分析

下面是我简单修改了下VIO ROS版本的代码，使在生成bag的同时也能够生成txt文件，然后运行第三方的一个Allan matlab代码，生成Allan曲线，该代码可以生成艾伦曲线的全部5中噪声参数。
设置参数如下表所示，也就是默认参数。

噪声类型	数值	单位
Gyroscope “random walk”	0.00005	$\frac{r a d}{s^{2}} \frac{1}{\sqrt{H z}}$s2rad​Hz​1​
Accelerometer “random walk”	0.0005	$\frac{m}{s^{3}} \frac{1}{\sqrt{H z}}$s3m​Hz​1​
Gyroscope “white noise”	0.015	$\frac{r a d}{s} \frac{1}{\sqrt{H z}}$srad​Hz​1​
Accelerometer “white noise”	0.019	$\frac{m}{s^{2}} \frac{1}{\sqrt{H z}}$s2m​Hz​1​

陀螺仪的艾伦曲线如下所示：

Matlab输出结果如下图：

从图中可以读出陀螺仪的角度随机游走噪声$51.7070 \frac{d e g}{\sqrt{h}}=0.015041 \frac{r a d}{\sqrt{s}}$51.7070h​deg​=0.015041s​rad​（各轴平均）,上面参数中的gyro_noise_sigma$=0.015 \frac{r a d}{\sqrt{s}}=51.57 \frac{d e g}{\sqrt{h}}$=0.015s​rad​=51.57h​deg​，注意该参数时定义在连续域上的，可以看出参数比较接近。
由角速率随机游走的定义可得，其导数即角角速度服从高斯白噪声，所以对应代码中的bias随机游走噪声，代码中gyro_bias_sigma$=0.00005 \frac{r a d}{s \sqrt{s}}=618.79 \frac{d e g}{h \sqrt{h}}$=0.00005ss​rad​=618.79hh​deg​，和输出结果中的Z轴数据较为接近。

加速度计的艾伦方差曲线如下图所示：

输出结果如下：
角度随机游走 X轴：0.019249 Y轴：0.019564 Z轴：0.019570 单位:m/s/sqrt(hr)
零偏不稳定性 X轴：0.001018 Y轴：0.002117 Z轴：0.002128 单位：m/s^2
角速率游走 X轴：0.000402 Y轴：0.000682 Z轴：0.000649单位：m/s^2/ sqrt(hr)

加速度计中高斯白噪声参数为acc_noise_sigma$=0.019 \frac{m}{s \sqrt{h r}}$=0.019shr​m​ ，可以看出，和输出结果基本一致。
加速度计中速率随机游走噪声为acc_bias_sigma$=0.0005 \frac{m}{s^{2} \sqrt{h r}}$=0.0005s2hr​m​ ，可以看出，和输出结果基本一致,可以看出，和输出结果中的角速率随机游走相差不大。

噪声类型	数值	单位
Gyroscope “random walk”	0.000034	$\frac{r a d}{s^{2}} \frac{1}{\sqrt{H z}}$s2rad​Hz​1​
Accelerometer “random walk”	0.000578	$\frac{m}{s^{3}} \frac{1}{\sqrt{H z}}$s3m​Hz​1​
Gyroscope “white noise”	0.015041	$\frac{r a d}{s} \frac{1}{\sqrt{H z}}$srad​Hz​1​
Accelerometer “white noise”	0.019461	$\frac{m}{s^{2}} \frac{1}{\sqrt{H z}}$s2m​Hz​1​

2.将IMU仿真代码中的欧拉积分替换成中值积分

欧拉法的轨迹如下：

中值法的轨迹如下：

把欧拉法和中值法的轨迹画在一张图上，如下面两张图所示，其中imu_int为欧拉法的轨迹，imu_int_mid为中值法的轨迹，可以很清晰的看出中值法的轨迹几乎和gt重合，所以中值法的精度较高。


中值法的代码如下：

void IMU::testImu(std::string src, std::string dist)
{
    std::vector<MotionData>imudata;
    LoadPose(src,imudata);

    std::ofstream save_points;
    save_points.open(dist);

    double dt = param_.imu_timestep;
    Eigen::Vector3d Pwb = init_twb_;              // position :    from  imu measurements
    Eigen::Quaterniond Qwb(init_Rwb_);            // quaterniond:  from imu measurements
    Eigen::Vector3d Vw = init_velocity_;          // velocity  :   from imu measurements
    Eigen::Vector3d gw(0,0,-9.81);    // ENU frame
    Eigen::Vector3d temp_a;
    Eigen::Vector3d theta;
    for (int i = 1; i < imudata.size(); ++i) {

        MotionData imupose = imudata[i];

        //delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
        static Eigen::Vector3d imu_gyro_old;
        static Eigen::Vector3d imu_acc_old;
        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half;
        Eigen::Vector3d acc_w;
        /// 中值积分
        if(i==1)
        {
             dtheta_half =  imupose.imu_gyro * dt /2.0;
             acc_w = Qwb * (imupose.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
        }
        else
        {
            dtheta_half =  (imupose.imu_gyro + imu_gyro_old)/2* dt /2.0;
            acc_w = Qwb * (imupose.imu_acc + imu_acc_old)/2 + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
        }

        //非单位四元数
		dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();

        Qwb = Qwb * dq; //姿态 四元数乘法
        Vw = Vw + acc_w * dt; //速度
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w; //位移

        imu_gyro_old = imupose.imu_gyro;
        imu_acc_old = imupose.imu_acc;


        //　按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次
        save_points<<imupose.timestamp<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<Qwb.w()<<" "
                   <<Qwb.x()<<" "
                   <<Qwb.y()<<" "
                   <<Qwb.z()<<" "
                   <<Pwb(0)<<" "
                   <<Pwb(1)<<" "
                   <<Pwb(2)<<" "
                   <<std::endl;

    }

    std::cout<<"test　end"<<std::endl;

}

• 点赞
• 收藏
• 分享
• • 文章举报

waihekor 发布了8 篇原创文章 · 获赞 11 · 访问量 1377 私信 关注