imu积分定位
imu积分定位的输入为imu传感器得到的线性加速度和角速度，输出为积分得到的位姿（位置和角度）。
具体公式为：
1.求旋转
设前一帧位姿的旋转用旋转矩阵表示为 R 1 R_{1} R1，当前帧位姿的旋转用旋转矩阵表示为 R 2 R_{2} R2
，假设对机器人进行了一次旋转，可以表示为如下：
R 2 = R 1 ∗ R 12 R_{2}=R_{1}*R_{12} R2=R1∗R12
用四元数计算如下
q 2 = q 1 ∗ q 12 q_{2}=q_{1}*q_{12} q2=q1∗q12
代码如下：
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
２.求平移
设前一帧位姿的平移表示为 t 1 t_{1} t1，当前帧位姿的平移表示为 t 2 t_{2} t2
，假设对机器人进行了一次平移，可以表示为如下：
求平移公式为
t 2 = t 1 + v ∗ t + 1 2 ∗ a ∗ t 2 t_{2}=t_{1}+v*t+\frac{1}{2}*a*t^2 t2=t1+v∗t+21∗a∗t2
代码如下
tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
３.vins中的偏移
vins中的将角速度偏移和线加速度的偏移作为优化量，因此预测过程（航迹推导过程）中需要加入偏移。
3.1角速度计算
设前一帧的角速度表示为 w 1 w_{1} w1，当前帧传感器获得的角速度表示为 w 2 w_{2} w2,传感器偏移为 b g b_{g} bg
则平均角速度为
w = 1 2 ( w 1 + w 2 ) − b g w=\frac{1}{2}(w_{1}+w_{2})-b_{g} w=21(w1+w2)−bg
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
则在ｔ时刻的旋转角度为：
β = w t \beta=wt β=wt
3.2加速度计算
设前一帧的局部坐标系的加速度表示为 a 1 a_{1} a1，加速度偏移为 b 1 a b1_{a} b1a,重力加速度为 g g g
前一帧在世界坐标系下的加速度 a 1 , g a_{1,g} a1,g
a 1 , g = q 1 ( a 1 − b 1 a ) − g a_{1,g}=q_{1}(a_{1}-b1_{a})-g a1,g=q1(a1−b1a)−g
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
设当前帧的局部坐标系的加速度表示为 a 2 a_{2} a2，加速度偏移为 b 2 a b2_{a} b2a,重力加速度为 g g g
当前帧在世界坐标系下的加速度 a 2 , g a_{2,g} a2,g
a 2 , g = q 2 ( a 2 − b 2 a ) − g a_{2,g}=q_{2}(a_{2}-b2_{a})-g a2,g=q2(a2−b2a)−g
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
imu积分定位实践代码如下
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
double t = imu_msg->header.stamp.toSec();
if (init_imu)
{
latest_time = t;
init_imu = 0;
return;
}
double dt = t - latest_time;
latest_time = t;
double dx = imu_msg->linear_acceleration.x;
double dy = imu_msg->linear_acceleration.y;
double dz = imu_msg->linear_acceleration.z;
Eigen::Vector3d linear_acceleration{dx, dy, dz};
double rx = imu_msg->angular_velocity.x;
double ry = imu_msg->angular_velocity.y;
double rz = imu_msg->angular_velocity.z;
Eigen::Vector3d angular_velocity{rx, ry, rz};
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
tmp_V = tmp_V + dt * un_acc;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
可用代码２
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
//********para of imu*******
Eigen::Vector3d tmp_P=Eigen::Vector3d(0, 0, 0); //t
Eigen::Quaterniond tmp_Q=Eigen::Quaterniond::Identity();//R
Eigen::Vector3d tmp_V=Eigen::Vector3d(0, 0, 0);
double g_last_imu_time = -1.0;
ros::Publisher g_imu_path_pub;
nav_msgs::Path g_imu_path;
using namespace std;
void ShowIMUPositionCallBack(Eigen::Vector3d imu_linear_acc, Eigen::Vector3d& imu_angular_vel,clock_t t)
{
std::cout<("imu_path",1, true);
g_imu_path.header.frame_id="world";
double imu_angular_vel_rz=0.1;
double const_imu_linear_acc_x = 0.0;
double const_imu_linear_acc_y =0.0;
Eigen::Vector3d imu_angular_vel={ 0, 0, imu_angular_vel_rz};
Eigen::Vector3d imu_linear_acc={const_imu_linear_acc_x, const_imu_linear_acc_y,0};
clock_t current_time,init_time;
init_time=clock();
current_time=clock();
bool init_flag=false;
tmp_V={0.1,0,0};
cv::RNG rng; // OpenCV随机数产生器
while (1)
{
if(false==init_flag)
{
current_time=init_time;
g_last_imu_time=current_time;
init_flag=true;
}else{
current_time = clock();
}
//add noise
// imu_linear_acc(0)+= rng.gaussian ( 0.0001);
cout<<"imu_angular_vel"<