从零开始学习VIO笔记 --- 第二讲:IMU 数据仿真(分析+代码)
一.理论分析
机器人身上携带了相机与imu传感器
世界坐标系/惯性系为
w
w
w ; imu的坐标系作为body系
b
b
b; 相机坐标系为
c
c
c ;
b
b
b系与
c
c
c系的外参手动设定
R
b
c
R_{bc}
Rbc 和
t
b
c
t_{bc}
tbc
t
w
b
t_{wb}
twb 描述 机器人的位置与世界坐标系原点的关系;
R
w
b
R_{wb}
Rwb描述机器人坐标系与世界坐标系的旋转关系
需要:
1.1 机器人的位姿
机器人的位姿 t w b t_{wb} twb与 R w b R_{wb} Rwb
- 定义机器人运动轨迹 p = f ( t ) p = f(t) p=f(t) ,p点的坐标值是相对于世界坐标系的,即为 t w b t_{wb} twb
- 定义机器人运动的欧拉角方程
(
r
o
l
l
,
p
i
t
c
h
,
y
a
w
)
(roll,pitch,yaw)
(roll,pitch,yaw) 分别对应
x
,
y
,
z
x,y,z
x,y,z轴的旋转量,也是相对于世界坐标系的; (欧拉角方程描述了机器人坐标系与世界坐标系的旋转关系,更为直观,也更容易建立方程,然后我们需要将这个旋转关系转为旋转矩阵去描述)
世界坐标系下的一个点 X X X 通过三次旋转(分别绕 z , y , x z,y,x z,y,x)可以得到在 body坐标系下的坐标值:即
X b = R b w X X_b=R_{bw}X Xb=RbwX,即可以得到 R w b = R b w − 1 = R b w T R_{wb}=R_{bw}^{\;-1}=R_{bw}^{\;T} Rwb=Rbw−1=RbwT
1.2 imu的角速度 和线加速度
imu的角速度 w b w^b wb 和线加速度 a b a^b ab (imu的坐标系作为body系)
- 对欧拉角方程
ϑ
\vartheta
ϑ 求一阶导
d
ϑ
d
t
\frac{\operatorname d\vartheta}{\operatorname dt}
dtdϑ,得到欧拉角速度,然后通过变换(见下图)获得body系下的角速度
w
b
w^b
wb
- 通过对运动轨迹
p
=
f
(
t
)
p = f(t)
p=f(t) 对时间
t
t
t 求二阶导获得在世界坐标系下机器人的加速度
a
w
a^w
aw, 由于还有ENU/G(东北天/导航)坐标系下的重力加速度
g
G
=
[
0
,
0
,
−
9.81
]
T
g^G=[0,0,-9.81]^T
gG=[0,0,−9.81]T的影响,b系下的线加速度
a
b
a^b
ab 通过下式计算:
a b = R b w ( a w − g G ) a^b = R_{bw}(a^w-g^G) ab=Rbw(aw−gG)
1.3 对imu数据添加噪声
偏差噪声bias ,随机噪声(高斯白噪声)n (见上一博文中的 imu加速度计和陀螺仪的数学模型)
1.4 问题
假设我们将机器的运动轨迹描述为:在z=1的平面做半径为 r 的圆周运动,方程则为
(
c
o
s
(
t
)
,
s
i
n
(
t
)
,
1
)
(cos(t),sin(t),1)
(cos(t),sin(t),1),那么机器人坐标系的方向变换呢?即机器人坐标系与世界坐标系的旋转关系
R
w
b
R_{wb}
Rwb 如何定义呢?
也就是如何确定 随时间变换的欧拉角方程
(
r
o
l
l
,
p
i
t
c
h
,
y
a
w
)
(roll,pitch,yaw)
(roll,pitch,yaw)呢?
答:已知机器人在固定的平面做圆周运动,我们的body坐标系,那么 roll和 pitch 为0,yaw随时间变化,。。。 (以后再补充吧)
二.代码
2.1 模型建立与产生数据
1.机器人的运动轨迹 — 可以构造函数
f
(
t
)
f(t)
f(t), 函数值为 Postion=(x,y,z),函数自变量为时间 t
t
w
b
t_{wb}
twb 就是Postion
float ellipse_x = 15;
float ellipse_y = 20;
float z = 1; // z轴做sin运动
float K1 = 10; // z轴的正弦频率是x,y的k1倍
float K = M_PI/ 10; // 20 * K = 2pi 由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈,运动一周
Eigen::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 frame
double 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二阶导数
对运动轨迹进行对时间t的一阶求导 或得 机身速度
v
w
v^w
vw(是带方向的,为矢量)
对运动轨迹进行对时间t的二阶求导 或得 机身加速度
a
w
a^w
aw(是带方向的,为矢量)
2.定义欧拉角运动方程,对其求导获得欧拉角速度,然后再转到 body系下的角速度 w b w^b wb
double 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 的导数
// 欧拉角速度转到body坐标系下的角速度
Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates; // euler rates trans to body gyro
3. R w b R_{wb} Rwb 通过欧拉角方程确定
Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles); // body frame to world frame
4.需要imu的加速度数据 Vector3d imu_acc — 由于还有世界坐标系下的重力加速度的影响,需要将机身加速度
a
w
a^w
aw 转为 imu的加速度:
a
b
=
R
b
w
(
a
w
−
g
w
)
a^b = R_{bw}(a^w-g^w)
ab=Rbw(aw−gw)
Eigen::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 = gs
2.2 保存相机位姿
相机位姿(相机在世界坐标系下位姿) R w c = R w b R b c R_{wc}=R_{wb}R_{bc} Rwc=RwbRbc , t w c = t w b + R w b t b c t_{wc}=t_{wb}+R_{wb}t_{bc} twc=twb+Rwbtbc
cam.timestamp = imu.timestamp;
cam.Rwb = imu.Rwb * params.R_bc; // cam frame in world frame
cam.twb = imu.twb + imu.Rwb * params.t_bc; // Tcw = Twb * Tbc , t = Rwb * tbc + twb
camdata.push_back(cam);
文件行格式:time,四元数,位移;
void save_Pose(std::string filename, std::vector<MotionData> pose)
{
std::ofstream save_points;
save_points.open(filename.c_str());
for (int i = 0; i < pose.size(); ++i) {
MotionData data = pose[i];
double time = data.timestamp;
Eigen::Quaterniond q(data.Rwb);
Eigen::Vector3d t = data.twb;
// Eigen::Vector3d gyro = data.imu_gyro;
// Eigen::Vector3d acc = data.imu_acc;
save_points<<time<<" "
<<q.w()<<" "
<<q.x()<<" "
<<q.y()<<" "
<<q.z()<<" "
<<t(0)<<" "
<<t(1)<<" "
<<t(2)<<" "
// <<gyro(0)<<" "
// <<gyro(1)<<" "
// <<gyro(2)<<" "
// <<acc(0)<<" "
// <<acc(1)<<" "
// <<acc(2)<<" "
<<std::endl;
}
}
2.3 imu数据添加噪声
// noise
double gyro_bias_sigma = 1.0e-5;
double acc_bias_sigma = 0.0001;
double gyro_noise_sigma = 0.015; // rad/s
double acc_noise_sigma = 0.019; // m/(s^2)
注意噪声的更新
void IMU::addIMUnoise(MotionData& data)
{
std::random_device rd;
std::default_random_engine generator_(rd());
std::normal_distribution<double> noise(0.0, 1.0);
Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));
Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();
data.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 update
Eigen::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 update
Eigen::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_;
}
2.4 使用imu的动力学模型(离散积分得到位姿)验证数据以及模型的准确性
运动模型的离散积分——中值法 (见上一博文),中值法相比欧拉法更为准确
imuGen.init_velocity_ = imudata[0].imu_velocity;
imuGen.init_twb_ = imudata.at(0).twb;
imuGen.init_Rwb_ = imudata.at(0).Rwb;
//读取生成的imu数据并用imu动力学模型对数据进行计算,最后保存imu积分以后的轨迹,
//用来验证数据以及模型的有效性。
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 * theta_x , 1/2 * theta_y, 1/2 * theta_z]
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half = imupose.imu_gyro * dt /2.0;
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
/// imu 动力学模型 欧拉积分
Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw; // aw = Rwb * ( acc_body - acc_bias ) + gw
Qwb = Qwb * dq;
Vw = Vw + acc_w * dt;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w; //
/// 中值积分
// 按着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;
}