角速度的推导:
左=ω∧\omega^{\wedge}ω∧,而ω\omegaω是在z轴方向运动,=θ′[0,0,1]T\theta^{\prime}[0,0,1]^Tθ′[0,0,1]T
两边取模后得到结论: 线速度大小=半径 * 角速度大小
其中,对旋转矩阵求导根据第一章的结论:
还有绿色箭头的公式,下面的推导看的不是很明白
实际上是把R的导数变成求极限的形式,但是这个极限怎么求的我不是很理解,然后就是下面这个公式
Rω∧=(Rω)∧RR\omega^\wedge=(R\omega)^\wedge RRω∧=(Rω)∧R
a=Rib∗aba = R_{ib}*a^ba=Rib∗ab表示body下的加速度在III系下的表示,仍是body下的加速度,只是表示在III系。
知道了这个科氏力之后,测量出科氏力以及运动的速度就能知道角速度了,这就是gyro的一个基本原理。
光纤陀螺仪一般较贵,原理是:光跑一圈路程是2πr2\pi r2πr,但如果在旋转,那就是2πr+x2\pi r+x2πr+x,测量出来这个x,用光速再进行相关计算就得到此时的角速度。
陀螺仪测角速度要两个轴:一个主动运动轴,一个敏感轴,敏感轴用于预测量科氏力
音叉振动陀螺
音叉两端左方向相反的正弦运动(什么叫正弦运动?音叉不是固定的吗? ),同一时刻其速度相反,±v,受到的科氏力大小相同方向相反F,整个音叉收到向右的力为f,左右也都为f,把受到的力相减,抵消之后,就能测出两倍科氏力2F,同时,知道自然块运动的速度,就能算出科氏力。
科氏力为2v∗ω2v*\omega2v∗ω,
这个G-sensitivity是灵敏度系数,比如机械振动会不会对IMU测量数据造成影响,如果不那么灵敏,就不会影响,如果比较灵敏,就需要考虑IMU减震等。
加速度计是否需要考虑科氏力影响?
不需要。
因为
1.即使开始加速时质量块会加速,但最终会达到平衡,速度v=0,最终的科氏力为0,
2.加计不是主动驱动的高速运动,会很缓慢地动,最终速度为0。
确定性误差有bias和scale,
scale是尺度,Misalignment是轴偏,如yz轴投影到x轴上的轴偏。
不考虑bias时,测量出的lax=sxx∗ax+mxy∗ay+mxz∗azl_{ax}=s_{xx}*a_x + m_{xy}*a_y + m_{xz}*a_zlax=sxx∗ax+mxy∗ay+mxz∗az,尺度轴偏矩阵主对角线为尺度,其他为轴偏。
其他确定性误差还有
六面法标定bias和scale,分别将xzy三个轴朝上或者下放置,测出的应该是±g,但是会受到bias影响.于是b=lup+ldown2b = \frac{l^{up}+l^{down}}{2}b=2lup+ldown就是两倍bias的均值,反之,相减绝对值就是2g2g2g,一除就是尺度scale。
l1l_1l1 ~ l6l_6l6是加速度测量值,S,b是待标定的尺度轴偏矩阵和bias,a1a_1a1 ~ a6a_6a6是加速度的理论值,其中g=9.81g=9.81g=9.81是标量。如此可以标定出SSS和bbb。
L=S[a1,a2,a3,a4,a5,a6]+bL=S[a_1, a_2,a_3,a_4,a_5,a_6]+bL=S[a1,a2,a3,a4,a5,a6]+b
最小二乘法能够求出S和b共12个元素。
同理,标定gyro也可以使用这种方法,需要有一个角速度较为精确的转台,也用6面,相对加计,此时我们知道较为精确的角速度ω1\omega_1ω1~ω6\omega_6ω6,即可标出gyro的尺度、轴偏、bias。
一般采用soak method,精度较高。
bias的导数满足高斯分布n(t)n(t)n(t)(注意是导数而不是bias本身),这个bias的分布被称为随机游走(random walk)。
ADC采样时间段内认为数据是常数,采集的数据=理想数据+bias和随机游走的噪声带来的部分,这个不是常数,所以需要进行积分
仅考虑高斯白噪声时(假设bias和n(t)是相互独立的)的协方差计算推导:
这里的τ\tauτ实际上就是一个自变量,可以是x,y,zx,y,zx,y,z等任意一个,是因为ttt在这里要代表时间,所以使用了τ\tauτ作为这里的自变量。本身高斯白噪声是满足高斯分布n(t)n(t)n(t)的,就如(13)式所定义。
协方差相关补充:
方差是协方差的特殊情况,方差实际上是对自身的协方差,即D(X)=Cov(X,X)D(X)=Cov(X,X)D(X)=Cov(X,X),也就是X的二阶原点矩。
补充对于一二阶矩的定义:(一阶矩是期望E(X)E(X)E(X),二阶原点矩是E(X2)E(X^2)E(X2),二阶非原点矩E(((X−E(X))2)E(\quad((X-E(X))^2\quad)E(((X−E(X))2),平方是因为如果E(X)E(X)E(X)不为有0时,若出现了负值,则会使整体二阶矩偏大,如果加了平方就相当于加上了绝对值,就更能体现偏离均值的范围。
这里的协方差标准写法应该是Cov(nd2[k])Cov(n_d^2[k])Cov(nd2[k]),写法做了省略:Cov(nd[k],nd[k])=D(nd[k])=E(nd2[k])−E(nd[k])2=E(nd2[k])Cov(n_d[k],n_d[k])=D(n_d[k])=E(n_d^2[k])-E(n_d[k])^2=E(n_d^2[k])Cov(nd[k],nd[k])=D(nd[k])=E(nd2[k])−E(nd[k])2=E(nd2[k])(因为这里nd[k]n_d[k]nd[k]是均值为0的高斯分布,所以E(nd[k])=0\bm{E(n_d[k])=0}E(nd[k])=0,方差=平方的期望-期望的平方),不好理解,内部展开就是下面项目的相乘
假设高斯白噪声是独立的,n(τ)n(t)n(\tau)n(t)n(τ)n(t)只有特定项(时间相差为1时)相乘才会有值,是狄拉克函数δ(t1−t2)=1(当且仅当t1−t2=1时)\delta(t_1-t_2)=1(当且仅当t_1-t_2=1时)δ(t1−t2)=1(当且仅当t1−t2=1时)
右因为前面有:
在tΔtt~\Delta tt Δt时间内只有一个时刻能使狄拉克函数为1,所以内层积分为1,外层积分为 (t+Δt)−Δt=Δt(t+\Delta t)-\Delta t=\Delta t(t+Δt)−Δt=Δt ,消掉分母即得协方差σ2Δt\frac{\sigma^2}{\Delta t}Δtσ2。
同理,下面的协方差写法也是做了省略,省略过程见上。
看起来像是开方的,σ\sigmaσ那一项相当于是服从一个N(0,1)N(0,1)N(0,1)分布。
结论:bias随即游走噪声方差从连续到离散之间需要∗Δt*\sqrt{\Delta t}∗Δt。(这个推导最后的开放有些看不懂,具体更详细的需要看论文
主要是标定协方差,用于IMU选型,确定使用什么灵敏度类型的IMU
random walk noise的标定
艾伦方差的标定的论文:
斜率-0.5处,t=1时的值是高斯白噪声方差的大小,斜率0.5,t=3处值是bias random walk的方差的大小,具体为什么,需要去看论文。(功率谱,靶向量?)
数据仿真部分:
(主要是标定Acc和Gyro的bias的random walk的方差,使用Kalibr_allan)
尺度因子如果标定的话会对精度有小幅提升(MSCKF什么黎明杨做的?VINS-MONO用过)
加计数学模型:加速度计的测量数据由尺度轴偏矩阵,重力分量,高斯白噪声,bias等构成。
陀螺仪数学模型:
下面这篇论文对MSCKF进行了非常详细的建模,如果有兴趣可以看。
如何使用这些数据获得pose:
这里需要强调,重力加速度gwg^wgw前面的符号是跟坐标系定义有关的,如果是东北天,那就是-,如果是北东地就是+,整体上自洽即可。
基于导数,四元数导数的⊗\bm{\otimes}⊗是四元数乘法,可以积分计算位置,速度,旋转:
欧拉法进行近似,假设在每个积分间隔内,被积函数值保持不变,即k~k+1时刻的积分使用k时刻的值:
关于四元数的更新:
qωbk+1=qωbk+q′Δt=q⊗[10]+q⊗[012ωΔt](q提出来)=q⊗[112ωΔt]\begin{align} q_{\omega b_{k+1}} &= q_{\omega b_k}+q^{\prime}\Delta t \\ &=q \otimes \begin{bmatrix} 1 \\ 0 \end{bmatrix}+ q \otimes \begin{bmatrix} 0 \\ \frac{1}{2}\omega \Delta t \end{bmatrix} \\ (q提出来)&=q \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\omega \Delta t \end{bmatrix} \\ \end{align} qωbk+1(q提出来)=qωbk+q′Δt=q⊗[10]+q⊗[021ωΔt]=q⊗[121ωΔt]
相较于欧拉法,中值法使用了k&k+1时刻的积分的均值,稍微准一点。
由于k->k+1时间较短,可以使用第k时刻的bias来矫正第k+1时刻的acc和gyro的值
两种方式产生IMU仿真数据:
由于四元数导数
所以四元数更新如上。
SO(3)更新也如上,更直观的方式是转换成欧拉角,需要将body系下的角速度转换为欧拉角速度。
粗略的理解:
旋转顺序是固定的(先绕哪个轴,再绕哪个轴)
最后转的x轴,所以x轴的角速度就是dψdt\frac{d\psi}{dt}dtdψ,
绕y轴转完之后还要绕x轴转个ψ\psiψ,所以y轴的角速度要乘一个R(ψ)R(\psi)R(ψ)
绕z轴转完之后还绕yx分别转了θ\thetaθ和ψ\psiψ,所以z方向的角速度要多乘个R(ψ)R(θ)R(\psi)R(\theta)R(ψ)R(θ)
所以就得到了Inertial->body系下的角速度转换矩阵,求逆就得从body->Inernial下的转换。
2014 ICRA:A Robust and Easy to Implement Method for IMU Calibration without
External Equipments