网站项目合同,网站的根目录下是哪个文件夹,品牌营销策划书模板,优化资讯VIO第2讲#xff1a;IMU标定实验 文章目录 VIO第2讲#xff1a;IMU标定实验5 IMU标定实验5.1 仿真数据产生5.1.1 c代码分析5.1.2 生成ros包数据 5.2 Allan方差实验#xff08;港科大imu_utils#xff09;5.2.1 安装5.2.2 运行 5.3 Allan方差实验#xff08;matlab代码kali…VIO第2讲IMU标定实验 文章目录 VIO第2讲IMU标定实验5 IMU标定实验5.1 仿真数据产生5.1.1 c代码分析5.1.2 生成ros包数据 5.2 Allan方差实验港科大imu_utils5.2.1 安装5.2.2 运行 5.3 Allan方差实验matlab代码kalibr_allan5.3.1 安装5.3.2 bag转mat5.3.3 运行标定 5 IMU标定实验
5.1 仿真数据产生
参考代码
5.1.1 c代码分析 下面代码确实是高斯噪声连续到离散除以sqr(δt)偏差随机游走则是乘以sqr(δt)。 void IMU::addIMUnoise(MotionData data)
{std::random_device rd;std::default_random_engine generator_(rd());std::normal_distributiondouble noise(0.0, 1.0); // 均值为0标准差为1的正态分布// 将噪声添加到原始的陀螺仪数据Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));Eigen::Matrix3d gyro_sqrt_cov param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();// w w cov(3*3)*noise_gyro(3*1)/sqrt(t) bgdata.imu_gyro data.imu_gyro gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) gyro_bias_;Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));Eigen::Matrix3d acc_sqrt_cov param_.acc_noise_sigma * Eigen::Matrix3d::Identity();data.imu_acc data.imu_acc acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) acc_bias_;// gyro_bias updateEigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));gyro_bias_ param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;data.imu_gyro_bias gyro_bias_;// acc_bias updateEigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));acc_bias_ param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;data.imu_acc_bias acc_bias_;}运动模型利用p求出va通过欧拉角的导数求出角速度w根据时间变量t来产生数据。 MotionData IMU::MotionModel(double t)
{MotionData data;// paramfloat ellipse_x 15;float ellipse_y 20;float z 1; // z轴做sin运动float K1 10; // z轴的正弦频率是xy的k1倍float K M_PI/ 10; // 20 * K 2pi 由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈运动一周// translation // twb: body frame in world frame// p v aEigen::Vector3d position( ellipse_x * cos( K * t) 5, ellipse_y * sin( K * t) 5, z * sin( K1 * K * t ) 5);Eigen::Vector3d dp(- K * ellipse_x * sin(K*t), K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t)); // position导数 in world framedouble K2 K*K;Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t), -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t)); // position二阶导数// Rotationdouble k_roll 0.1;double k_pitch 0.2;Eigen::Vector3d eulerAngles(k_roll * cos(t) , k_pitch * sin(t) , K*t ); // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi]Eigen::Vector3d eulerAnglesRates(-k_roll * sin(t) , k_pitch * cos(t) , K); // euler angles 的导数// Eigen::Vector3d eulerAngles(0.0,0.0, K*t ); // roll ~ 0, pitch ~ 0, yaw ~ [0,2pi]
// Eigen::Vector3d eulerAnglesRates(0.,0. , K); // euler angles 的导数// 角速度是通过欧拉角求导来得到的Eigen::Matrix3d Rwb euler2Rotation(eulerAngles); // body frame to world frameEigen::Vector3d imu_gyro eulerRates2bodyRates(eulerAngles) * eulerAnglesRates; // euler rates trans to body gyroEigen::Vector3d gn (0,0,-9.81); // gravity in navigation frame(ENU) ENU (0,0,-9.81) NED(0,0,9,81)Eigen::Vector3d imu_acc Rwb.transpose() * ( ddp - gn ); // Rbw * Rwn * gn gsdata.imu_gyro imu_gyro;data.imu_acc imu_acc;data.Rwb Rwb;data.twb position;data.imu_velocity dp;data.timestamp t;return data;}5.1.2 生成ros包数据 GitHub同时提供了ros代码我们直接用这套代码生成相应的imu.bag
// ros还是cpp代码种原始设置的标准差都是下面数据// noise----离散时间噪声标准差double gyro_bias_sigma 1.0e-5; // δbg rad/sdouble acc_bias_sigma 0.0001; // δba m/(s^2)double gyro_noise_sigma 0.015; // rad/s δgdouble acc_noise_sigma 0.019; // m/(s^2) δa这里先直接给出利用imu_utils进行的一个标定结果对于高斯噪声数量级一致但对于偏置随机游走数量级差了10~100总而言之这种标定方法对于高斯白噪声即角度/速度随机游走的标定结果是可信的但是对于偏置还是没有那么可信
%YAML:1.0
---
type: IMU
name: vio_test
Gyr:unit: rad/savg-axis:gyr_n: 2.1017740504505478e-01 gyr_w: 1.1046695141140115e-03x-axis:gyr_n: 2.0373831670509726e-01gyr_w: 8.5917637284818333e-04y-axis:gyr_n: 2.0848823537157685e-01gyr_w: 1.1543541527689969e-03z-axis:gyr_n: 2.1830566305849022e-01gyr_w: 1.3004780167248545e-03
Acc:unit: m/s^2avg-axis:acc_n: 2.6608701268913315e-01acc_w: 5.0312842533421255e-03x-axis:acc_n: 2.6703857869563274e-01acc_w: 5.4894453768553463e-03y-axis:acc_n: 2.6561748859645246e-01acc_w: 5.1153714824778724e-03z-axis:acc_n: 2.6560497077531420e-01acc_w: 4.4890359006931569e-035.2 Allan方差实验港科大imu_utils
Github
5.2.1 安装
sudo apt-get install libdw-dev报错1 创建一个ROS空间把两个ROS包放进去然后catkin_make.如果这样做的话大概率会报下面的错误。本质就是imu_utils依赖于code_utils这个包但实际上我们之前没有按照那个包就会报下面的错其实可以在catkin_make之前显示的告诉它路径或者就是先编译成功code_utils再编译imu_utils。
-- processing catkin package: imu_utils
-- add_subdirectory(imu_utils)
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- Could NOT find code_utils (missing: code_utils_DIR)
-- Could not find the required component code_utils. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found.
CMake Error at /opt/ros/noetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):Could not find a package configuration file provided by code_utils withany of the following names:code_utilsConfig.cmakecode_utils-config.cmakeAdd the installation prefix of code_utils to CMAKE_PREFIX_PATH or setcode_utils_DIR to a directory containing one of the above files. Ifcode_utils provides a separate development package or SDK, be sure it hasbeen installed.
Call Stack (most recent call first):imu_utils/CMakeLists.txt:13 (find_package)-- Configuring incomplete, errors occurred!
See also /home/pj/pj/vio_with_only_eigen/imu_allan/build/CMakeFiles/CMakeOutput.log.
See also /home/pj/pj/vio_with_only_eigen/imu_allan/build/CMakeFiles/CMakeError.log.
Invoking cmake failed 编译出现的报错code_utils 报错2 报错显示没有那个头文件查看之后发现是有的说名再CMakeLists.txt中没有显示的引入头文件我们只需要添加即可。或者把#include backward.hpp改成#include code_utils/backward.hpp
[ 30%] Building CXX object code_utils/CMakeFiles/sumpixel_test.dir/src/sumpixel_test.cpp.o
/home/pj/pj/vio_with_only_eigen/imu_allan/src/code_utils/src/sumpixel_test.cpp:2:10: fatal error: backward.hpp: No such file or directory2 | #include backward.hpp| ^~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [code_utils/CMakeFiles/sumpixel_test.dir/build.make:63: code_utils/CMakeFiles/sumpixel_test.dir/src/sumpixel_test.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:480: code_utils/CMakeFiles/sumpixel_test.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
include_directories(${catkin_INCLUDE_DIRS}${CMAKE_SOURCE_DIR}/code_utils/include/code_utils # 添加这个这样就能找到了${EIGEN3_INCLUDE_DIR})报错3 报错显示没找到glog.so动态库文件但是这个环境前两天刚装的所以必然不是这个问题
make[2]: *** No rule to make target /usr/lib/x86_64-linux-gnu/libglog.so, needed by /home/pj/pj/vio_with_only_eigen/imu_allan/devel/lib/libpnp.so. Stop.
make[2]: *** Waiting for unfinished jobs....
[ 92%] Building CXX object code_utils/CMakeFiles/pnp.dir/src/cv_utils/pnp/nonlinearpnp.cpp.o
make[1]: *** [CMakeFiles/Makefile2:804: code_utils/CMakeFiles/pnp.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
Invoking make -j2 -l2 failed locate下发现库文件存在但上面找的不是这个
pjpj:~$ locate glog | grep so
...
/usr/lib/x86_64-linux-gnu/libglog.so.0
/usr/lib/x86_64-linux-gnu/libglog.so.0.0.0libglog.so和 libglog.so.0所以我们把这两个软链接起来就能解决问题
sudo ln -s /usr/lib/x86_64-linux-gnu/libglog.so.0 /usr/lib/x86_64-linux-gnu/libglog.so然后编译imu_utils就没有报错了
5.2.2 运行
要么就是自己录个数据至少2个小时以上
播放rosbag数据 rosbag play -r 200 imu_A3.bag启动对应得launch文件注意修改
roslaunch imu_utils A3.launch # 换成自己的launch文件launchnode pkgimu_utils typeimu_an nameimu_an outputscreen!--改成自己IMU的话题--param nameimu_topic typestring value /djiros/imu/!--这个和最后保存的文件名有关最后的文件名为A3_imu_param.yaml--param nameimu_name typestring value A3/!--标定结果存放的文件目录--param namedata_save_path typestring value $(find imu_utils)/data_my//!--修改标定时间单位是分钟需要根据数据包的时长来调整--param namemax_time_min typeint value 120/param namemax_cluster typeint value 100//node
/launch结果 会生成很多文件最终计算结果再yaml文件中可以利用脚本文件下的matlab函数去画出角速度和加速度的Allan曲线。 type: IMU
name: A3
Gyr:unit: rad/savg-axis: gyr_n: 1.0351286977809465e-04 # δggyr_w: 2.9438676109223402e-05 # δbgx-axis:gyr_n: 1.0312669892959053e-04gyr_w: 3.3765827874234673e-05y-axis:gyr_n: 1.0787155789128671e-04gyr_w: 3.1970693666470835e-05z-axis:gyr_n: 9.9540352513406743e-05gyr_w: 2.2579506786964707e-05
Acc:unit: m/s^2avg-axis:acc_n: 1.3985049290745563e-03acc_w: 6.3249251509920116e-04x-axis:acc_n: 1.1687799474421937e-03acc_w: 5.3044554054317266e-04y-axis:acc_n: 1.2050535351630543e-03acc_w: 6.0281218607825414e-04z-axis:acc_n: 1.8216813046184213e-03acc_w: 7.6421981867617645e-04github给出的—标定是离散参数
绘图结果—script—这下面的图像不是A3是生成的那个仿真数据图像
角速度allan曲线 加速度allan曲线 其实也能看出来关于角速度偏置随机游走肯定是误差最大的
5.3 Allan方差实验matlab代码kalibr_allan
5.3.1 安装 GitHub主要是装那个ros转换包matlab直接打开就行
mkdir -p datacen/src
catkin_make5.3.2 bag转mat 使用命令 rosrun bagconvert bagconvert xx.bag/imu0该命令的解释是rosrun bagconvert bagconvert [bag名字] [topic名字]这里如果topic输入不正确可能会得到一个177kb的错误mat文件 5.3.3 运行标定
修改SCRIPT_allan_matparallel.m
% Our bag information
%mat_path ../data/imu_mtig700.mat;
%mat_path ../data/imu_tango.mat;
% 改成上面bag转mat文件的路径可以在matlab命令行pwd像Ubuntu一样
mat_path I:\kalibr_allan-master\data\imu.mat;然后运行这个文件根据电脑的性能运行时间长短不一最终会在你的mat文件下生成一个result的mat文件
修改SCRIPT_process_results.m
titlestr ADIS16448 VI-Sensor;
% 修改路径
mat_path I:\kalibr_allan-master\data\results_20240106T181017.mat;结果展示
加速度allan曲线 角速度allan曲线目前不知道哪里有问题导致对应偏置随机游走无法计算 上面生成的数据对应方差// noise----离散时间噪声标准差double gyro_bias_sigma 1.0e-5; // δbg rad/sdouble acc_bias_sigma 0.0001; // δba m/(s^2)double gyro_noise_sigma 0.015; // rad/s δgdouble acc_noise_sigma 0.019; // m/(s^2) δa其实可以看出这种方差实验结果要比imu_utils要准确一点
