有关于IMU的标定。
此外,IMU的标定在github中也有介绍,并且有matlab代码,CSDN。从这个帖子出发,又看到IMU模块的姿态校正,具体代码参看文件。
matlab仿真,有关于IMU的,不过代码量较大。
C++
代码,编的比较好,可以仿照。已经根据其代码完成了相关的“卡尔曼滤波”的重新设计,并且写完C++
代码。
从零开始的 IMU 状态模型推导
matlab仿真比较好,简化了一部分,需要理解他的逻辑是什么,对应博客与csdn,有时间可以看一下他的博客。
好像是比较了多种(EKF或者其他滤波的方法)。该作者做了几种标定方式对比的实验,其中有mahony滤波、HLP、EKF等,内容比较完善。
下图取自代码
Gait-Tracking-With-x-IMU开源项目,该项目完成了轨迹追踪的标定。在代码中,应该主要采用的是manoy互补滤波器的方法。
有必要看一下,貌似设计滤波器进行处理数据了(简单的处理,实际上并没有太多作用)
基于上面的项目,开启下面的项目。matlab代码写的很清晰。(并且用了上面的数据)
这里主要的做法是基于上面的数据与对加速度计数据的预处理,进行卡尔曼滤波的设计。卡尔曼滤波的设计主要是基于观测值“v 等于0m/s”的设计。
项目,代码
同样是基于“声科技”的代码。
问题是不能运行,现在需要搞清楚,做标定实验时,需要怎么样的数据?必须要进行有规律的运动?
阅读这个人的博客
《视觉SLAM进阶:从零开始手写VIO》第二讲作业,讲到用工具进行惯组的标定工作(利用utils工具包)
这个人的博客做了很多IMU+视觉SLAM的工作,**有时间可以重点看一下,**参考下面:
我询问小觅相机的技术人员后,直接甩给我文件imu.params:
%YAML:1.0
---
in_accel:
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
drift 飘移: [ 0.0, 0.0, 0.0 ]
noise噪声: [ 0.016925432397973516, 0.016735310195561025, 0.017452487504590969 ]
bias 偏差: [ 0.00019031356589714596, 0.00016996777864587261, 0.00054490537096493644 ]
in_gyro:
scale: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
drift: [ 0., 0., 0. ]
noise: [ 0.0010848026158819934, 0.0012466367883501759, 0.0011003229919806443 ]
bias: [ 0.000023404834136742844, 0.000023596771567764949, 0.000014970418056326829 ]
ex_left_to_imu:()
rotation: [ -0.0064662, -0.99994994, -0.00763565, 0.99997909, -0.00646566, -0.00009558, 0.0000462, -0.00763611, 0.99997084 ]
translation: [ 0.00533646, -0.04302922, 0.02303124 ]
imu_utlis
自身的问题。后面会用kalibr_allan
工具标定一下。博客采用imu_utils和imu_tk标定imu的确定性误差和随机误差,值得借鉴。
IMU-TK提供简单的函数和数据结构,用于校准基于MEMS的惯性导航单元,以及处理和显示IMU数据。IMU-TK实现了一种多位置校准方法,该方法不需要任何参数调整,只需要手动移动传感器并放置在一组不同的,自动检测的静态位置。IMU-TK还提供了一系列数据集成功能。
引用博客的结果,生成两个标定文件,分别为陀螺仪和加速度计的旋转矩阵,刻度因数和零偏。完成标定。
IMU-TK需要以下工具和库:CMake,Eigen3,Ceres Solver,OpenGL,QT和Gnuplot。要在Ubuntu上安装这些必需的软件包。
安装错误参考2
C++
),基于ROS,代码在这里。唯一的问题是手中没有ROS平台。(来自于旷世科技)如果姿态补偿正确,那么利用加速度进行积分,其误差就不是很大了,但是一样会产生漂移,如下图:
上面可以看到,还是有一定的发散。
但是如何计算一个正确的“角度”呢?仅仅只是一个粗略的计算?下面主要对角度处理,不使用加速度数据补偿角度,那么会有下面做法:(利用四元数与机体角速度之间的关系)
可以看到,即使不采用加速度的数据进行角度的补偿,其结果也是可用的。那么如果直接对惯组角速度进行积分呢?有如下结果,可见,除了存在相位偏差之外,数据还是可以大致使用的。
惯性元件一般指的是加速度计和陀螺仪(有时也包括磁力计)3,它能够感知 3 个正交方向上的线性加速度和旋转角速度。一般地,影响惯性导航性能的主要误 差源包括惯性元件精度和系统误差,例如正交轴的对齐误差、传感器比例因子、 非正交性以及随机噪声等等,将详细的误差源列出如下表格 所示。
为了评估惯性导航系统的性能,目前存在多种方式,其中易于实现的方法为 将惯性测量单元(Inertial Measurement Unit,IMU)静止一段时间,分析随着时间 增长惯性导航位置漂移量的大小。当然位置估计假设导航误差仅仅由加速度计偏 差引起。实际上位置估计的误差由许多因素造成,包括陀螺仪偏移稳定性、加速度度计比例因子不确定性以及其他因素,但主要的贡献因子是由没有校正的加速度 计偏差引起的。例如在加速度计水平轴上的任何偏差都会导致物体姿态(Roll、 Pitch)估计的误差,姿态矩阵的误差直接关系坐标变换到同一参考系下的加速度 值与实际重力值之差,此偏差(运动加速度)经积分传递到位置误差中,这样加速 度计偏差导致的位置误差将随时间成平方的增长。
因此,惯性元件看似微小的误差对导航输出的干扰不容忽视。学术上,常常 将惯性元件的误差分为两类:确定性误差和随机误差。确定性误差,也叫静态误差,因能被精确建模和表示,可以通过校正或参数标定的方式将其消除。一 般地,在惯性元件使用之前,必须对其校正以消除静态误差影响。
为什么bias(固定偏差)会随机游走?
角速度与角度之间的关系如下:
Θ ˙ = W b ω \dot{\Theta}=W^{b} \omega Θ˙=Wbω
= [ 1 tan θ sin ϕ tan θ cos ϕ 0 cos ϕ − sin ϕ 0 sin ϕ / cos θ cos ϕ / cos θ ] b ω =\left[\begin{array}{ccc}1 & \tan \theta \sin \phi & \tan \theta \cos \phi \\ 0 & \cos \phi & -\sin \phi \\ 0 & \sin \phi / \cos \theta & \cos \phi / \cos \theta\end{array}\right]^{b} \omega =⎣⎡100tanθsinϕcosϕsinϕ/cosθtanθcosϕ−sinϕcosϕ/cosθ⎦⎤bω
有如下关系:
Θ ˙ = W b ω = [ 1 tan θ sin ϕ tan θ cos ϕ 0 cos ϕ − sin ϕ 0 sin ϕ / cos θ cos ϕ / cos θ ] b ω \dot{\Theta}=W^{b} \omega =\left[\begin{array}{ccc}1 & \tan \theta \sin \phi & \tan \theta \cos \phi \\ 0 & \cos \phi & -\sin \phi \\ 0 & \sin \phi / \cos \theta & \cos \phi / \cos \theta\end{array}\right]^{b} \omega Θ˙=Wbω=⎣⎡100tanθsinϕcosϕsinϕ/cosθtanθcosϕ−sinϕcosϕ/cosθ⎦⎤bω
旋转矩阵与机体角速度的关系:
R ˙ b e = R b e [ b ω ] × \dot{R}_{b}^{e}=R_{b}^{e}\left[^{b} \omega\right]_{\times} R˙be=Rbe[bω]×
四元数与机体角速度之间的关系:
q ˙ e b ( t ) = 1 2 [ 0 − ω x − ω y − ω z ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y − ω x 0 ] q e b ( t ) \dot{q}_{e}^{b}(t)=\frac{1}{2}\left[\begin{array}{cccc}0 & -\omega_{x} & -\omega_{y} & -\omega_{z} \\ \omega_{x} & 0 & \omega_{z} & -\omega_{y} \\ \omega_{y} & -\omega_{z} & 0 & \omega_{x} \\ \omega_{z} & \omega_{y} & -\omega_{x} & 0\end{array}\right] q_{e}^{b}(t) q˙eb(t)=21⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0⎦⎥⎥⎤qeb(t)
或者寻找三维的(非四元数,只是角速度)的角速度与角度之间的关系,如下面:
旋转矩阵的更新方程为:
R ˙ ( t ) = R ( t ) Ω ( t ) Ω ( t ) = [ 0 − ω b z ( t ) ω b y ( t ) ω b z ( t ) 0 − ω b x ( t ) − ω b y ( t ) ω b x ( t ) 0 ] \begin{aligned} \dot{R}(t) &=R(t) \Omega(t) \\ \Omega(t) &=\left[\begin{array}{ccc}0 & -\omega_{b z}(t) & \omega_{b y}(t) \\ \omega_{b z}(t) & 0 & -\omega_{b x}(t) \\ -\omega_{b y}(t) & \omega_{b x}(t) & 0\end{array}\right] \end{aligned} R˙(t)Ω(t)=R(t)Ω(t)=⎣⎡0ωbz(t)−ωby(t)−ωbz(t)0ωbx(t)ωby(t)−ωbx(t)0⎦⎤
离散化之后:
R ( t + δ t ) = R ( t ) ⋅ exp ( ∫ t t + δ t Ω ( t ) d t ) = R ( t ) ( I + sin σ σ B + 1 − cos σ σ 2 B 2 ) \begin{aligned} R(t+\delta t) &=R(t) \cdot \exp \left(\int_{t}^{t+\delta t} \Omega(t) d t\right) \\ &=R(t)\left(I+\frac{\sin \sigma}{\sigma} B+\frac{1-\cos \sigma}{\sigma^{2}} B^{2}\right) \end{aligned} R(t+δt)=R(t)⋅exp(∫tt+δtΩ(t)dt)=R(t)(I+σsinσB+σ21−cosσB2)
B = ∫ t t + δ t Ω ( t ) d t = ( 0 − ω b z ( t ) δ t ω b y ( t ) δ t ω b z ( t ) δ t 0 − ω b x ( t ) δ t − ω b y ( t ) δ t ω b x ( t ) δ t 0 ) \begin{aligned} B &=\int_{t}^{t+\delta t} \Omega(t) d t \\ &=\left(\begin{array}{ccc}0 & -\omega_{b z}(t) \delta t & \omega_{b y}(t) \delta t \\ \omega_{b z}(t) \delta t & 0 & -\omega_{b x}(t) \delta t \\ -\omega_{b y}(t) \delta t & \omega_{b x}(t) \delta t & 0\end{array}\right) \end{aligned} B=∫tt+δtΩ(t)dt=⎝⎛0ωbz(t)δt−ωby(t)δt−ωbz(t)δt0ωbx(t)δtωby(t)δt−ωbx(t)δt0⎠⎞
取一阶近似:
R ( t + δ t ) = R ( t ) ( I + B ) R(t+\delta t)=R(t)(I+B) R(t+δt)=R(t)(I+B)
在代码文件5中已经描述的非常清楚。
加速度计的误差通过两次积分带入位置估计,gyro 的误差也同样会带入位置估计。旋转矩阵将 acc 投影到 global 坐标系。姿态的误差将会导致 acc 投影偏差,在垂直方向上无法准确减去 重力加速度 g。
几乎所有 INS 计算中,gyro 的误差都是位置误差的主要来源。因为,大部分应用场景中,重力加速度 g 都远大于运动加速度。
q ^ ˙ = 1 2 q ^ ⊗ P ( Ω ˉ + δ ) \dot{\hat{q}}=\frac{1}{2} \hat{q} \otimes \mathbf{P}(\bar{\Omega}+\delta) q^˙=21q^⊗P(Ωˉ+δ)
δ = k P ⋅ e + k I ⋅ ∫ e \delta=k_{P} \cdot e+k_{I} \cdot \int e δ=kP⋅e+kI⋅∫e
e = v ˉ × v ^ e=\bar{v} \times \hat{v} e=vˉ×v^
其引用的是无人机开源项目PX4的代码,基于互补滤波器。
/Users/chenshiming/Downloads/imu_tk_matlab-master ↩︎
http://www.mamicode.com/info-detail-2623544.html ↩︎
基于UWBIMU融合的室内定位与导航技术研究.pdf ↩︎
https://blog.csdn.net/luoshi006/article/details/51513580 ↩︎
zupt.m ↩︎