上海高端网站建设高端网站建设,自己在家搭建服务器,政务公开加强网站规范化建设,企业小程序开发费用本文介绍一篇 关于IMU 标定的经典论文#xff0c;论文收录于 ICRA14#xff0c;在论文中作者介绍了如何不适用外部设备标定 IMU 加速度和角速度偏差、尺度系数、轴偏移参数。
论文链接#xff1a;https://readpaper.com/paper/2021503353、https://readpaper.com/paper/221… 本文介绍一篇 关于IMU 标定的经典论文论文收录于 ICRA14在论文中作者介绍了如何不适用外部设备标定 IMU 加速度和角速度偏差、尺度系数、轴偏移参数。
论文链接https://readpaper.com/paper/2021503353、https://readpaper.com/paper/2211578699
项目链接https://github.com/JzHuai0108/imu_tk_matlab 1. Sensor Error Model
首先介绍传感器误差模型令 aO\mathbf{a}^{O}aO 表示为理想情况下的加速度数据aS\mathbf{a}^{S}aS 表示为实际的加速度数据Ta[1−αyzαzy01−αzx001]\mathbf{T}^{a}\left[\begin{array}{ccc}1 -\alpha_{y z} \alpha_{z y} \\ 0 1 -\alpha_{z x} \\ 0 0 1\end{array}\right]Ta⎣⎡100−αyz10αzy−αzx1⎦⎤ 表示从 aS\mathbf{a}^{S}aS 到 aO\mathbf{a}^{O}aO 的旋转变换。ba[bxabyabza]\mathbf{b}^{a}\left[\begin{array}{l}b_{x}^{a} \\ b_{y}^{a} \\ b_{z}^{a}\end{array}\right]ba⎣⎡bxabyabza⎦⎤ 为加速度偏差Ka[sxa000sya000sza]\mathbf{K}^{a}\left[\begin{array}{ccc}s_{x}^{a} 0 0 \\ 0 s_{y}^{a} 0 \\ 0 0 s_{z}^{a}\end{array}\right]Ka⎣⎡sxa000sya000sza⎦⎤ 为加速度尺度系数νa\boldsymbol{\nu}^{a}νa 为加速度测量噪声则可以得到加速度误差模型 aOTaKa(aSbaνa)\mathbf{a}^{O}\mathbf{T}^{a} \mathbf{K}^{a}\left(\mathbf{a}^{S}\mathbf{b}^{a}\boldsymbol{\nu}^{a}\right) aOTaKa(aSbaνa)
同样也可以得到角速度误差模型令 Tg[1−γyzγzyγxz1−γzx−γxyγyx1]\mathbf{T}^{g}\left[\begin{array}{ccc}1 -\gamma_{y z} \gamma_{z y} \\ \gamma_{x z} 1 -\gamma_{z x} \\ -\gamma_{x y} \gamma_{y x} 1\end{array}\right]Tg⎣⎡1γxz−γxy−γyz1γyxγzy−γzx1⎦⎤Kg[sxg000syg000szg]\mathbf{K}^{g}\left[\begin{array}{ccc}s_{x}^{g} 0 0 \\ 0 s_{y}^{g} 0 \\ 0 0 s_{z}^{g}\end{array}\right]Kg⎣⎡sxg000syg000szg⎦⎤bg[bxgbygbzg]\mathbf{b}^{g}\left[\begin{array}{l}b_{x}^{g} \\ b_{y}^{g} \\ b_{z}^{g}\end{array}\right]bg⎣⎡bxgbygbzg⎦⎤νg\boldsymbol{\nu}^{g}νg 为角速度测量噪声则角速度误差模型为
ωOTgKg(ωSbgνg)\boldsymbol{\omega}^{O}\mathbf{T}^{g} \mathbf{K}^{g}\left(\boldsymbol{\omega}^{S}\mathbf{b}^{g}\boldsymbol{\nu}^{g}\right) ωOTgKg(ωSbgνg) 2. Basic Calibration Framework
加速度标定我们需要估计的未知参数为 θacc[αyz,αzy,αzx,sxa,sya,sza,bxa,bya,bza]\theta^{a c c}\left[\alpha_{y z}, \alpha_{z y}, \alpha_{z x}, s_{x}^{a}, s_{y}^{a}, s_{z}^{a}, b_{x}^{a}, b_{y}^{a}, b_{z}^{a}\right]θacc[αyz,αzy,αzx,sxa,sya,sza,bxa,bya,bza]
此时我们可以忽略测量噪声则加速度误差模型简化为 aOTaKa(aSba)\mathbf{a}^{O}\mathbf{T}^{a} \mathbf{K}^{a}\left(\mathbf{a}^{S}\mathbf{b}^{a}\right) aOTaKa(aSba)
正如在传统的多位置策略中我们将 IMU 置于 MMM 个不同的位置在每一个静止周期内读取加速度测量值 akS\mathbf{a}^{S}_{k}akS我们可以使用以下损失函数来估计加速度误差模型参数 L(θacc)∑k1M(∥g∥2−∥h(akS,θacc)∥2)2\mathbf{L}\left(\boldsymbol{\theta}^{a c c}\right)\sum_{k1}^{M}\left(\|\mathbf{g}\|^{2}-\left\|h\left(\mathbf{a}_{k}^{S}, \boldsymbol{\theta}^{a c c}\right)\right\|^{2}\right)^{2} L(θacc)k1∑M(∥g∥2−∥∥h(akS,θacc)∥∥2)2
其中∣∣g∥||\mathbf{g}\|∣∣g∥ 是当地重力加速度幅值。损失函数程序为
function [res_vector] accCostFunctLSQNONLIN(E, a_hat, magnitude)misalignmentMatrix [1, -E(1), E(2); 0, 1, -E(3); 0, 0, 1];scalingMtrix diag([E(4), E(5), E(6)]);a_bar misalignmentMatrix*scalingMtrix*(a_hat (diag([E(7), E(8), E(9)])*ones(3, size(a_hat,2))));% Magnitude taken from tables if(nargin3)magnitude 9.81744;endresiduals zeros(length(a_bar(1,:)), 1);for i 1:length(a_bar(1,:))residuals(i,1) (magnitude^2 - (a_bar(1,i)^2 a_bar(2,i)^2 a_bar(3,i)^2))^2;endres_vector residuals;end我们使用同样的静止周期来标定陀螺仪。在这里我们通过对初始静止时刻角速度值求平均来得到角速度偏差。这样我们需要求解的参数简化为 θgyro[γyz,γzy,γxz,γzx,γxy,γyx,sxg,syg,szg]\boldsymbol{\theta}^{g y r o}\left[\gamma_{y z}, \gamma_{z y}, \gamma_{x z}, \gamma_{z x}, \gamma_{x y}, \gamma_{y x}, s_{x}^{g}, s_{y}^{g}, s_{z}^{g}\right] θgyro[γyz,γzy,γxz,γzx,γxy,γyx,sxg,syg,szg]
我们使用标定后的加速度数据作为参考给定一个初始的重力向量对角速度数据进行积分我们可以估计最终的重力向量则损失函数可以写为 L(θgyro)∑k2M∥ua,k−ug,k∥2\mathbf{L}\left(\boldsymbol{\theta}^{g y r o}\right)\sum_{k2}^{M}\left\|\mathbf{u}_{a, k}-\mathbf{u}_{g, k}\right\|^{2} L(θgyro)k2∑M∥ua,k−ug,k∥2
其中ua,k\mathbf{u}_{a, k}ua,k 是标定后的加速度向量ug,k\mathbf{u}_{g, k}ug,k 是估计后的重力向量。角速度积分这里使用的是 RK4不过目前 IMU 的采样频率都很高了一般很少再使用了。 3. Calibration Procedure
A. Static Detector
IMU 标定的准确性非常依赖于静止和运动时间间隔的准确区分为了标定加速度计我们使用静止周期标定陀螺仪我们使用两个静态周期之间的动态时间间隔。我们这里使用基于方差的静止检测器对于时间周期长度 twaitt_{wait}twait 秒我们有加速度 (axt,ayt,azt)\left(\mathbf{a}_{x}^{t}, \mathbf{a}_{y}^{t}, \mathbf{a}_{z}^{t}\right)(axt,ayt,azt)然后我们计算标准差 ς(t)[vartw(axt)]2[vartw(ayt)]2[vartw(azt)]2\varsigma(t)\sqrt{\left[\operatorname{var}_{t_{w}}\left(\mathbf{a}_{x}^{t}\right)\right]^{2}\left[\operatorname{var}_{t_{w}}\left(\mathbf{a}_{y}^{t}\right)\right]^{2}\left[\operatorname{var}_{t_{w}}\left(\mathbf{a}_{z}^{t}\right)\right]^{2}} ς(t)[vartw(axt)]2[vartw(ayt)]2[vartw(azt)]2
我们通过比较方标准差ς(t)\varsigma(t)ς(t) 是否大于某一阈值来区分静止和运动状态。我们将初始方差 ςinit\varsigma_{init}ςinit 扩大整数倍来作为阈值。下图是静止检测器的检测结果这里整数倍为6倍。 B. Runge-Kutta Integration
下面简单介绍下四阶龙格库塔法这里主要用在陀螺仪的标定。四元数微分方程为 f(q,t)q˙12Ω(ω(t))q\mathbf{f}(\mathbf{q}, t)\dot{\mathbf{q}}\frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t)) \mathbf{q} f(q,t)q˙21Ω(ω(t))q
其中Ω(ω)\Omega(\boldsymbol{\omega})Ω(ω) 是一个反对称矩阵形式为 Ω(ω)[0−ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0]\boldsymbol{\Omega}(\boldsymbol{\omega})\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] Ω(ω)⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0⎦⎥⎥⎤
四阶龙格库塔法原理为 qk1qkΔt16(k12k22k3k4)kif(q(i),tkciΔt),for i1q(i)qk,for i1q(i)qkΔt∑j1i−1aijkj,\begin{array}{ll} \mathbf{q}_{k1}\mathbf{q}_{k}\Delta t \frac{1}{6}\left(\mathbf{k}_{1}2 \mathbf{k}_{2}2 \mathbf{k}_{3}\mathbf{k}_{4}\right) \\ \mathbf{k}_{i}\mathbf{f}\left(\mathbf{q}^{(i)}, t_{k}c_{i} \Delta t\right), \text { for } i1 \\ \mathbf{q}^{(i)}\mathbf{q}_{k}, \text { for } i1 \\ \mathbf{q}^{(i)}\mathbf{q}_{k}\Delta t \sum_{j1}^{i-1} a_{i j} \mathbf{k}_{j}, \end{array} qk1qkΔt61(k12k22k3k4)kif(q(i),tkciΔt),q(i)qk,q(i)qkΔt∑j1i−1aijkj, for i1 for i1
各个参数为 c10,c212,c312,c41a2112,a310,a410a3212,a420,a431\begin{gathered} c_{1}0, \quad c_{2}\frac{1}{2}, \quad c_{3}\frac{1}{2}, \quad c_{4}1 \\ a_{21}\frac{1}{2}, \quad a_{31}0, \quad a_{41}0 \\ a_{32}\frac{1}{2}, \quad a_{42}0, \quad a_{43}1 \end{gathered} c10,c221,c321,c41a2121,a310,a410a3221,a420,a431
最终得到积分后的四元数还需要再转化为单位四元数整个RK4 程序为
function [R] rotationRK4(omega, dt)omega_x omega(1,:);omega_y omega(2,:);omega_z omega(3,:);num_samples length(omega_x);q_k fromOmegaToQ([omega_x(1); omega_y(1); omega_z(1)], [dt]);q_next_k q_k; % was [0; 0; 0; 0]; changed by Huaifor i 1:num_samples - 1% first Runge-Kutta coefficientq_i_1 q_k;OMEGA_omega_t_k ...[0 -omega_x(i) -omega_y(i) -omega_z(i);omega_x(i) 0 omega_z(i) -omega_y(i);omega_y(i) -omega_z(i) 0 omega_x(i);omega_z(i) omega_y(i) -omega_x(i) 0 ];k_1 (1/2)*OMEGA_omega_t_k*q_i_1;% second Runge-Kutta coefficientq_i_2 q_k dt*(1/2)*k_1;OMEGA_omega_t_k_plus_half_dt ...[0 -(omega_x(i) omega_x(i 1))/2 -(omega_y(i) omega_y(i 1))/2 -(omega_z(i) omega_z(i 1))/2;(omega_x(i) omega_x(i 1))/2 0 (omega_z(i) omega_z(i 1))/2 -(omega_y(i) omega_y(i 1))/2;(omega_y(i) omega_y(i 1))/2 -(omega_z(i) omega_z(i 1))/2 0 (omega_x(i) omega_x(i 1))/2;(omega_z(i) omega_z(i 1))/2 (omega_y(i) omega_y(i 1))/2 -(omega_x(i) omega_x(i 1))/2 0 ];k_2 (1/2)*OMEGA_omega_t_k_plus_half_dt*q_i_2;% third Runge-Kutta coefficientq_i_3 q_k dt*(1/2)*k_2;OMEGA_omega_t_k_plus_half_dt ...[0 -(omega_x(i) omega_x(i 1))/2 -(omega_y(i) omega_y(i 1))/2 -(omega_z(i) omega_z(i 1))/2;(omega_x(i) omega_x(i 1))/2 0 (omega_z(i) omega_z(i 1))/2 -(omega_y(i) omega_y(i 1))/2;(omega_y(i) omega_y(i 1))/2 -(omega_z(i) omega_z(i 1))/2 0 (omega_x(i) omega_x(i 1))/2;(omega_z(i) omega_z(i 1))/2 (omega_y(i) omega_y(i 1))/2 -(omega_x(i) omega_x(i 1))/2 0 ];k_3 (1/2)*OMEGA_omega_t_k_plus_half_dt*q_i_3;% forth Runge-Kutta coefficientq_i_4 q_k dt*1*k_3;OMEGA_omega_t_k_plus_dt ...[0 -omega_x(i 1) -omega_y(i 1) -omega_z(i 1);omega_x(i 1) 0 omega_z(i 1) -omega_y(i 1);omega_y(i 1) -omega_z(i 1) 0 omega_x(i 1);omega_z(i 1) omega_y(i 1) -omega_x(i 1) 0 ];k_4 (1/2)*OMEGA_omega_t_k_plus_dt*q_i_4;q_next_k q_k dt*((1/6)*k_1 (1/3)*k_2 (1/3)*k_3 (1/6)*k_4);q_next_k q_next_k/norm(q_next_k);q_k q_next_k;endR inv(fromQtoR(q_next_k));endC. Complete Procedure
为了避免标定参数估计中的不可观察性至少需要收集IMU9个不同姿态的数据姿态数越多标定结果越准确。整个标定算法如下需要知道采集好的加速度数据 aS\mathbf{a}^{S}aS 和角速度数据 ωS\boldsymbol{\omega}^{S}ωS初始静止时间 TinitT_{init}Tinit以及运动后的静止时间 twaitt_{wait}twait。
首先根据初始时间计算陀螺仪偏差 bg\boldsymbol{b}^gbg根据计算后的陀螺仪偏差得到无偏角速度数据 ωbiasfreeS\boldsymbol{\omega}^{S}_{biasfree}ωbiasfreeS计算初始协方差 ςinit\varsigma_{init}ςinit ;设 i1:ki1:ki1:k 根据等待时间 twaitt_{wait}twait 和阈值计算静止间隔、再根据静止间隔 twaitt_{wait}twait 和加速度数据得到估计参数最后选取残差最小对应的参数为加速度标定参数然后再使用同样的静止周期计算陀螺仪标定参数
