学习资料是深蓝学院的《从零开始手写VIO》课程，对课程做一些记录，方便自己以后查询，如有错误还请斧正。由于习惯性心算公式，所以为了加深理解，文章公式采用手写的形式。
IMU 传感器
旋转运动学
线速度与角速度
粒子在坐标系中 z = h 中的平面做圆周运动
坐标为: r = (a cos θ, a sin θ, h) ⊤对坐标求导得:
上式结果我觉得应该是：ｗ^×ｒ
其中 ω = θｚ，｜θ｜是角速度大小。
线速度公式：
旋转坐标系下的运动学
质量块在 body 坐标系下的坐标为:
旋转到惯性系下有:
对时间求导有:
对速度求导得到:
在旋转坐标系下观察,运动的物体(运动方向和旋转轴不为同一个轴时)会受到科氏力的作用。
欧拉力(Euler force)与科里奥利力(Coriolis Force)有何不同吗？这两个描述和成因都相差很远。科里奥利力是匀速旋转非惯性系中除了离心力【因为运动】受的额外力；欧拉力是非匀速旋转参考系中物体受处离心力所受由【非匀速旋转】（相对匀速）产生的惯性力。
IMU 测量模型及运动模型
MEMS 加速度计工作原理
测量原理可以用一个简单的质量块 + 弹簧 + 指示计来表示
加速度计测量值 a m 为弹簧拉力对应的加速度,
f 弹簧拉力,a 物体在惯性系下的加速度,g 重力加速度
MEMS 加速度计利用电容或者电阻桥来等原理来测量 a m
东北天坐标系 (ENU):
g = (0, 0, −9.81) ⊤
假设 IMU 坐标系就是 ENU 坐标系,R IB = I,
静止时有:a =0, a m = −g
自由落体时有:a = g, a m = 0
陀螺工作原理
陀螺仪主要用来测量物体的旋转角速度,按测量原理分有振动陀螺,光纤陀螺等。
低端 MEMS 陀螺上一般采用振动陀螺原理,通过测量 Coriolis force (科氏力)来间接得到角速度。
在旋转坐标系中,运动的物体受到科氏力作用
MEMS 陀螺仪:一个主动运动轴 + 一个敏感轴
音叉振动陀螺
叉子的中间为旋转轴,叉子左右两个质量块,做方向相反的正弦运动,质量块受到的科氏力方向相反。
陀螺仪的 G-sensitivity
实际上,两个质量块不可能完全一致,也就是说陀螺仪的测量可能会受到外部加速度的影响,即常称的 G-sensitivity。
IMU 误差模型
误差分类
加速度计和陀螺仪的误差可以分为:确定性误差,随机误差。
确定性误差可以事先标定确定,包括:bias, scale …
随机误差通常假设噪声服从高斯分布,包括:高斯白噪声,bias随机游走…
确定性误差
Bias
理论上,当没有外部作用时,IMU 传感器的输出应该为 0。但是,实际数据存在一个偏置 b。加速度计 bias 对位姿估计的影响:
Scale
scale 可以看成是实际数值和传感器输出值之间的比值。
Nonorthogonality/Misalignment Errors
多轴 IMU 传感器制作的时候,由于制作工艺的问题,会使得 xyz 轴可能不垂直,如下图所示。
其他确定性误差
Run-to-Run Bias/Scale Factor
In Run (Stability) Bias/Scale Factor
Temperature-Dependent Bias/Scale Factor
确定误差标定
六面法标定加速度
六面法标定加速度 bias 和 scale factor
六面法是指将加速度计的 3 个轴分别朝上或者朝下水平放置一段时间,采集 6 个面的数据完成标定。(想到了自己在装四旋翼的时候，校准陀螺仪了，也是用的六面法标定法)
如果各个轴都是正交的,那很容易得到 bias 和 scale:
其中,l 为加速度计某个轴的测量值,g 为当地的重力加速度。
考虑轴间误差的时候,实际加速度和测量值之间的关系为:
水平静止放置 6 面的时候,加速度的理论值为
对应的测量值矩阵 L :
利用最小二乘就能够把 12 个变量求出来
六面法标定陀螺仪
六面法标定陀螺仪 bias 和 scale factor
和加速度计六面法不同的是,陀螺仪的真实值由高精度转台提供,这里的 6 面是指各个轴顺时针和逆时针旋转。
温度相关的参数标定
目的: 这个标定的主要目的是对传感器估计的 bias 和 scale 进行温度补偿,获取不同温度时 bias 和 scale的值,绘制成曲线。
两种标定方法:
soak method: 控制恒温室的温度值,然后读取传感器数值进行标定。
ramp method:记录一段时间内线性升温和降温时传感器的数据来进行标定。
IMU 随机误差
随机误差部分没搞懂啥意思，也不知咋推出来的，还得再研究，就先把推导过程放这吧。
新的推导过程请看我的这篇博客（更新于2020年3月26日23时）
高斯白噪声
IMU 数据连续时间上受到一个均值为 0,方差为 σ,各时刻之间相互独立的高斯过程 n(t):
其中 δ() 表示狄拉克函数。
实际上,IMU 传感器获取的数据为离散采样,离散和连续高斯白噪声的方差之间存在如下转换关系:
即
其中
也就是说高斯白噪声的连续时间到离散时间之间差一个 1/ √∆t ，√∆t 是传感器的采样时间。
Bias 随机游走
通常用维纳过程 (wiener process)来建模 bias 随时间连续变化的过程,离散时间下称之为随机游走。
其中 w 是方差为 1 的白噪声
同样,离散和连续之间的转换:
即:
其中:
bias 随机游走的噪声方差从连续时间到离散之间需要乘以√∆t
IMU 随机误差的标定
艾伦方差标定 random walk noise
Allan 方差法是 20 世纪 60 年代由美国国家标准局的 David Allan 提出的,它是一种基于时域的分析方法。
具体的流程如下:
- 保持传感器绝对静止获取数据。
- 对数据进行分段,设定时间段的时长,如下图所示。
- 将传感器数据按照时间段进行平均。
- 计算方差,绘制艾伦曲线。得到的艾伦曲线如下图所示 :
具体标定过程请参考我的这篇博客
加速度计的误差模型
导航系 G 为东北天,g G = (0, 0, −9.81) ⊤ 。理论测量值为:
如果考虑高斯白噪声,bias,以及尺度因子,则为:
通常假设尺度因子为单位矩阵。
陀螺仪的误差模型
考虑尺度因子,高斯白噪声,以及 bias, 陀螺仪的误差模型如下:
低端传感器,考虑加速度对陀螺仪的影响,即 g-灵敏度:
陀螺仪受四种噪声的影响分别如下图所示
运动模型离散时间处理
VIO 中的 IMU 模型
忽略 scale 的影响,只考虑白噪声和 bias 随机游走:
上标 g 表示 gyro,a 表示 acc,w 表示在世界坐标系 world,b 表示imu 机体坐标系 body。IMU 的真实值为 ω, a, 测量值为 ω~, a~。
P(ose),V(elocity),Q(uaternion) 对时间的导数可写成:
连续时间下 IMU 运动模型
根据上面的导数关系,可以从第 i 时刻的 PVQ,通过对 IMU 的测量值进行积分,得到第 j 时刻的 PVQ:
运动模型的离散积分——欧拉法
使用欧拉法,即两个相邻时刻 k 到 k+1 的位姿是用第 k 时刻的测量值 a, ω 来计算。
其中,
代码描述：
MotionData imupose = imudata[i];
//delta_q = [1 , 1/2 * thetax , 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;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = Vw + acc_w * dt;
运行仿真数据结果如下图，发现存在误差：
运动模型的离散积分——中值法
使用 mid-point 方法,即两个相邻时刻 k 到 k+1 的位姿是用两个时刻的测量值 a, ω 的平均值来计算。
其中,
代码描述：
// imu 动力学模型 参考svo预积分论文
Eigen::Vector3d acc_w = Qwb * (imupose_pre.imu_acc) + gw; // aw = Rwb * ( acc_body - acc_bias ) + gw
Qwb = Qwb * dq; //获得Qwb_k+1
//Qwb+1
Eigen::Vector3d acc_w1 = Qwb * (imupose_now.imu_acc) + gw; // aw = Rwb * ( acc_body - acc_bias ) + gw
acc_w = (acc_w + acc_w1) / 2.0;
//a update
Vw = Vw + acc_w * dt;
//Pwb update
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
运行仿真数据结果如下图，发现存在误差，但误差比欧拉法要小：
欧拉角
inertial frame 下的一个点旋转到 body 坐标系,用欧拉角如何表示?仿真数据中旋转矩阵用欧拉角来表示很方便。
step 1. 绕着惯性坐标系的 z 轴旋转,得到新的坐标系 b 1 。
step 2. 绕着新坐标系 b 1 的 y 轴旋转得到坐标系 b 2
step 3. 绕着新坐标系 b 2 的 x 轴旋转得到坐标系 b 3 , b 3 就是我们的body 坐标系。
综合起来,得到:
代码描述;
// euler2Rotation: body frame to interitail frame
Eigen::Matrix3d euler2Rotation( Eigen::Vector3d eulerAngles)
{
double roll = eulerAngles(0);
double pitch = eulerAngles(1);
double yaw = eulerAngles(2);
double cr = cos(roll); double sr = sin(roll);
double cp = cos(pitch); double sp = sin(pitch);
double cy = cos(yaw); double sy = sin(yaw);
Eigen::Matrix3d RIb;
RIb<< cy*cp , cy*sp*sr - sy*cr, sy*sr + cy* cr*sp,
sy*cp, cy *cr + sy*sr*sp, sp*sy*cr - cy*sr,
-sp, cp*sr, cp*cr;
return RIb;
}
欧拉角速度和 body 角速度的转换
inertial frame 下的欧拉角速度怎么转到 body 坐标系下呢?
euler rate to body rate:
如何理解这个式子呢？由欧拉角速度变换到body坐标系下，z轴需要旋转两次，y轴需要旋转一次，x轴不需要旋转。
代码描述;
Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles)
{
double roll = eulerAngles(0);
double pitch = eulerAngles(1);
double cr = cos(roll); double sr = sin(roll);
double cp = cos(pitch); double sp = sin(pitch);
Eigen::Matrix3d R;
R<< 1, 0, -sp,
0, cr, sr*cp,
0, -sr, cr*cp;
return R;
}
公式 (31) 取逆就能得到,body rate to euler rate 的变换:
1653
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
