精华内容
下载资源
问答
  • Mahony
    千次阅读 多人点赞
    2019-01-04 11:17:56

    Mahony互补滤波器代码详解

    在博客文章《一起深入读懂Mahony互补滤波器》(https://blog.csdn.net/jessecw79/article/details/84668189) 的基础上,本文将对Mahony互补滤波器的Matlab代码进行详细解读。本文是Jesse Chen的原创文章,如果转载请注明出处。

    回顾《一起深入读懂Mahony互补滤波器》

    在上一篇文章中,我们介绍了Mahony互补滤波器的推导过程和公式。如图1是一个基本SO(3)的Mahony互补滤波器的框图。
    在这里插入图片描述
    图1 Block diagram of the passive complementary filter on SO(3)

    如果采用估计值 R ^ \hat{R} R^作为反馈,可以得到在估计参考坐标系下的互补滤波器方程(将陀螺仪的bias考虑在内):
    R ^ ˙ = R ^ ( Ω y − b ^ + ω ) × b ^ ˙ = − k b vex ⁡ ( π a ( R ~ ) ) , with  k b > 0 ω = k est vex ⁡ ( π a ( R ~ ) ) , with  k est > 0 R ~ = R ^ T R \begin{aligned} \dot{\hat{R}} &= \hat{R}\left(\Omega^y - \hat{\mathbf{b}} + \omega\right)_\times \\ \dot{\hat{\mathbf{b}}} & = -k_b\operatorname{vex}\left(\pi_a\left(\widetilde{R}\right)\right),\qquad\text{with }k_b > 0 \\ \omega &= k_{\text{est}}\operatorname{vex}\left(\pi_a\left(\widetilde{R}\right)\right),\qquad\text{with }k_\text{est} > 0 \\ \widetilde{R} &= \hat{R}^T R \end{aligned} R^˙b^˙ωR =R^(Ωyb^+ω)×=kbvex(πa(R )),with kb>0=kestvex(πa(R )),with kest>0=R^TR
    假设 k P = k est k_P = k_\text{est} kP=kest k I = k b k_I = k_b kI=kb,那么很容易得到参考文献[1],[2],[3]中的passive complementary filter:
    R ^ ˙ = R ^ ( Ω y − b ^ + k P ω ) × , R ^ ( 0 ) = R ^ 0 b ^ ˙ = − k I ω , b ^ ( 0 ) = b ^ 0 ω = vex ⁡ ( P a ( R ~ ) ) , R ~ = R ^ T R y \begin{aligned} \dot{\hat{R}} &= \hat{R}\left(\Omega^y - \hat{\mathbf{b}} + k_P\omega\right)_\times,\qquad\hat{R}\left(0\right) = \hat{R}_0 \\ \dot{\hat{\mathbf{b}}} &= -k_I\omega,\qquad\qquad\qquad\qquad\quad\hat{\mathbf{b}}\left(0\right) = \hat{\mathbf{b}}_0 \\ \omega &= \operatorname{vex}\left(\mathbb{P}_a\left(\widetilde{R}\right)\right),\qquad\qquad\qquad\widetilde{R} = \hat{R}^T R_y \end{aligned} R^˙b^˙ω=R^(Ωyb^+kPω)×,R^(0)=R^0=kIω,b^(0)=b^0=vex(Pa(R )),R =R^TRy
    其中 P a ( ⋅ ) = π a ( ⋅ ) \mathbb{P}_a\left(\cdot\right) = \pi_a\left(\cdot\right) Pa()=πa()

    IMU的测量值

    一般情况下,IMU可以是9轴,包括:

    • 3轴的陀螺仪;
    • 3轴的加速度计;
    • 3轴的磁力计;

    捷联式IMU的参考坐标系是和IMU固定在一起的(body fixed frame)坐标系 { B } \{\mathcal{B}\} {B},惯性参考坐标系为 { A } \{\mathcal{A}\} {A},旋转 R = B A R R = {}_\mathcal{B}^\mathcal{A}R R=BAR表示 { B } \{\mathcal{B}\} {B}相对于 { A } \{\mathcal{A}\} {A}的旋转。

    陀螺仪测量误差模型

    陀螺仪的测量值可以表示为:
    Ω y = Ω + b + μ   ∈ R 3 \Omega^y = \Omega + \mathbf{b} + \boldsymbol{\mu}\,\in\mathbb{R}^3 Ωy=Ω+b+μR3
    其中 Ω \Omega Ω是真实值, μ \boldsymbol{\mu} μ是加性高斯噪声,而 b : = b ( t ) \mathbf{b}:=\mathbf{b}\left(t\right) b:=b(t)是陀螺仪的bias。

    加速度计测量误差模型

    加速度计的测量值可以表示为:
    a = R T ( v ˙ − g 0 ) + b a + μ a \mathbf{a} = R^T\left(\dot{\mathbf{v}} - \mathbf{g}_0\right) + \mathbf{b}_a + \boldsymbol{\mu}_a a=RT(v˙g0)+ba+μa
    其中, b a \mathbf{b}_a ba是加速度计的bias, μ a \boldsymbol{\mu}_a μa是加性高斯噪声。如果我们规定重力加速度的方向为 e 3 \mathbf{e}_3 e3,那么 g 0 = ∣ g 0 ∣ e 3 \mathbf{g}_0 = \left\vert\mathbf{g}_0\right\vert\mathbf{e}_3 g0=g0e3,通常 ∣ g 0 ∣ = 9.8 \left\vert\mathbf{g}_0\right\vert = 9.8 g0=9.8

    通常使用
    v a = a ∣ a ∣ ≈ − R T e 3 \mathbf{v}_a = \frac{\mathbf{a}}{\left\vert\mathbf{a}\right\vert} \approx -R^T\mathbf{e}_3 va=aaRTe3
    作为惯性坐标系的 z z z轴在和IMU固定的坐标系(body fixed frame)中的一个低频估计。

    磁力计

    磁力计的测量值可以表示为:
    m = R T   A m + B m + μ b \mathbf{m} = R^T\,{}^A\mathbf{m} + \mathbf{B}_m + \boldsymbol{\mu}_b m=RTAm+Bm+μb
    其中, A m {}^A\mathbf{m} Am表示在惯性坐标系下表示的地球磁场, B m \mathbf{B}_m Bm表示本地磁力干扰, μ b \boldsymbol{\mu}_b μb是噪声。只有磁力计的方向和方向估计有关,因此我们通常使用如下的向量形式的测量
    v m = m ∣ m ∣ \mathbf{v}_m = \frac{\mathbf{m}}{\lvert\mathbf{m}\rvert} vm=mm

    向量 v a \mathbf{v}_a va v m \mathbf{v}_m vm可以用来构造 B A R   :   { B } → { A } {}_\mathcal{B}^\mathcal{A}R\,:\,\{\mathcal{B}\}\to\{\mathcal{A}\} BAR:{B}{A}的一个瞬时代数测量值
    R y = arg ⁡ min ⁡ R ∈ S O ( 3 ) ( λ 1 ∣ e 3 − R v a ∣ 2 + λ 2 ∣ v m ∗ − R v m ∣ 2 ) ≈ B A R R_y = \arg\min_{R\in SO\left(3\right)}\left(\lambda_1\lvert\mathbf{e}_3 - R\mathbf{v}_a\rvert^2 + \lambda_2\lvert\mathbf{v}_m^\ast - R\mathbf{v}_m\rvert^2\right) \approx {}_\mathcal{B}^\mathcal{A}R Ry=argRSO(3)min(λ1e3Rva2+λ2vmRvm2)BAR
    其中 v m ∗ \mathbf{v}_m^\ast vm是磁场的惯性方向。

    注意 v a \mathbf{v}_a va v m \mathbf{v}_m vm就是参考文献中所说的惯性方向。

    我想没人愿意按照上式在线估计 R y R_y Ry,Mahony等学者提出了直接使用IMU测量值得互补滤波器–explicit complementary filter

    Explicit Error Formulation of the Passive Complementary Filter

    v 0 i ∈ A ,   i = 1 , ⋯   , n \mathbf{v}_{0i}\in\mathcal{A},\,i=1,\cdots,n v0iA,i=1,,n表示 n n n个已知的惯性方向。而与之相对,我们所测量到的惯性方向都是在和IMU固定在一起(body fixed frame)的坐标系中的观测:
    v i = R T v 0 i + μ i , v i ∈ B \mathbf{v}_i = R^T\mathbf{v}_{0i} + \boldsymbol{\mu}_i,\qquad\mathbf{v}_i\in\mathcal{B} vi=RTv0i+μi,viB
    其中 μ i \boldsymbol{\mu}_i μi是噪声。因为我们只关心惯性方向的方向,因此我们可以做出归一化:
    ∣ v 0 i ∣ = 1 ∣ v i ∣ = 1 \begin{aligned} \lvert\mathbf{v}_{0i}\rvert &= 1 \\ \lvert\mathbf{v}_i\rvert &= 1 \end{aligned} v0ivi=1=1

    R ^ \hat{R} R^ R R R的一个估计,定义:
    v ^ i = R ^ T v 0 i \hat{\mathbf{v}}_i = \hat{R}^T\mathbf{v}_{0i} v^i=R^Tv0i
    是于 v 0 i \mathbf{v}_{0i} v0i对应的估计值。对于单个方向 v i \mathbf{v}_i vi,方向误差可以定义为:
    E i = 1 − cos ⁡ ( ∠ v i , v ^ i ) = 1 − ⟨ v i , v ^ i ⟩ E_i = 1 - \cos\left(\angle\mathbf{v}_i,\hat{\mathbf{v}}_i\right) = 1 - \langle\mathbf{v}_i,\hat{\mathbf{v}}_i\rangle Ei=1cos(vi,v^i)=1vi,v^i
    继而得到
    E i = 1 − tr ⁡ ( R ^ T v 0 i v 0 i T R ) = 1 − tr ⁡ ( R ~ R T v 0 i v 0 i T R ) E_i = 1 - \operatorname{tr}\left(\hat{R}^T\mathbf{v}_{0i}\mathbf{v}_{0i}^T R\right) = 1 - \operatorname{tr}\left(\widetilde{R}R^T\mathbf{v}_{0i}\mathbf{v}_{0i}^T R\right) Ei=1tr(R^Tv0iv0iTR)=1tr(R RTv0iv0iTR)
    对于多个测量 v i \mathbf{v}_i vi,考虑下面的误差:
    E mes = ∑ i k i E i = ∑ i = 1 n k i − tr ⁡ ( R ~ M ) , k i > 0 E_\text{mes} = \sum_ik_i E_i = \sum_{i=1}^nk_i - \operatorname{tr}\left(\widetilde{R}M\right),\qquad k_i > 0 Emes=ikiEi=i=1nkitr(R M),ki>0
    其中
    M = R T M 0 R with     M 0 = ∑ i = 1 n k i v 0 i v 0 i T M = R^T M_0 R\qquad\text{with }\,\,M_0 = \sum_{i=1}^n k_i\mathbf{v}_{0i}\mathbf{v}_{0i}^T M=RTM0Rwith M0=i=1nkiv0iv0iT
    假设惯性方向之间是线性不相关的,那么

    • n ≥ 3 n \geq 3 n3时,矩阵 M M M是正定的;
    • n ≤ 2 n \leq 2 n2时,矩阵 M M M是半正定的,且其中一个特征值为0。

    我们可以得到按照如下方程组描述的Explicit Complementary Filter:
    R ^ ˙ = R ^ ( ( Ω y − b ^ ) × + k P ( ω mes ) × ) , R ^ ( 0 ) = R ^ 0 b ^ ˙ = − k I   ω mes ω mes : = ∑ i = 1 n k i v i × v ^ i , k i > 0 \begin{aligned} \dot{\hat{R}} &= \hat{R}\left(\left(\Omega^y - \hat{\mathbf{b}}\right)_\times + k_P\left(\omega_\text{mes}\right)_\times\right),\qquad\hat{R}\left(0\right) = \hat{R}_0 \\ \dot{\hat{\mathbf{b}}} &= -k_I\,\omega_\text{mes} \\ \omega_\text{mes} :&= \sum_{i=1}^n k_i\mathbf{v}_i\times\hat{\mathbf{v}}_i,\qquad k_i > 0 \end{aligned} R^˙b^˙ωmes:=R^((Ωyb^)×+kP(ωmes)×),R^(0)=R^0=kIωmes=i=1nkivi×v^i,ki>0

    Passive Complementary Filter和Explicit Complementary Filter之间的关系

    下面我们来推导Explicit Complementary Filter和Passive Complementary Filter之间的关系。

    容易证明:
    ω mes = vex ⁡ ( ∑ i = 1 n k i ( v ^ i v i T − v i v ^ i T ) ) = − vex ⁡ ( ∑ i = 1 n k i ( v i v ^ i T − v ^ i v i T ) ) \omega_\text{mes} = \operatorname{vex}\left(\sum_{i=1}^n k_i\left(\hat{\mathbf{v}}_i\mathbf{v}_i^T - \mathbf{v}_i\hat{\mathbf{v}}_i^T\right)\right) = -\operatorname{vex}\left(\sum_{i=1}^n k_i\left(\mathbf{v}_i\hat{\mathbf{v}}_i^T - \hat{\mathbf{v}}_i\mathbf{v}_i^T\right)\right) ωmes=vex(i=1nki(v^iviTviv^iT))=vex(i=1nki(viv^iTv^iviT))

    本人反复计算过上式,也借助过计算机,仍然和参考文献中间的结论有2倍的差异,但是由于有一组可调系数 { k i } \{k_i\} {ki},因此不会影响参考文献结论的正确性。看到本文的同学,如果对这段计算感兴趣或者对我的结论有不同意见的,请联系754971421@qq.com。

    同时也容易证明:
    R ~ M = R ^ T M 0 R = ∑ i = 1 n k i v ^ i v i T \widetilde{R}M = \hat{R}^T M_0 R = \sum_{i=1}^n k_i\hat{\mathbf{v}}_i\mathbf{v}_i^T R M=R^TM0R=i=1nkiv^iviT
    显然
    ( ω mes ) × = P a ( R ~ M ) \left(\omega_\text{mes}\right)_\times = \mathbb{P}_a\left(\widetilde{R}M\right) (ωmes)×=Pa(R M)
    在一种特殊情况下:当 n = 3 n = 3 n=3 k i = 1 k_i = 1 ki=1,且测量的惯性方向是两两正交的( v i T v j = 0 ,   ∀   i ≠ j \mathbf{v}_i^T\mathbf{v}_j = 0,\,\forall\, i\neq j viTvj=0,i̸=j),显然 M 0 = I 3 M_0 = \mathbf{I}_3 M0=I3。那么
    M = R T M 0 R = R T R = I 3 M = R^T M_0 R = R^T R = \mathbf{I}_3 M=RTM0R=RTR=I3
    在这种情况下,
    ( ω mes ) × = P a ( R ~ ) ⇒ ω mes = vex ⁡ ( P a ( R ~ ) ) \begin{aligned} \left(\omega_\text{mes}\right)_\times &= \mathbb{P}_a\left(\widetilde{R}\right) \\ \Rightarrow\qquad\omega_\text{mes} &= \operatorname{vex}\left(\mathbb{P}_a\left(\widetilde{R}\right)\right) \end{aligned} (ωmes)×ωmes=Pa(R )=vex(Pa(R ))
    这就和开篇回顾的Mahony互补滤波器公式一致了。前不久,有一个工程师和我讲:他看参考文献[3]写技术报告,因为参考文献[3]是非线性的,而参考文献[4]中的互补滤波器是线性的。这种说法是不正确的,本文已经说明了explicit complementary filter就是Mahony Complementary Filter。

    Explicit Complementary Filter的四元数实现形式

    ω mes = ∑ i = 1 n k i v × v ^ i T , k i > 0 q ^ ˙ = 1 2 q ^ ⊗ p ( Ω y − b ^ + k P ω mes ) , k P > 0 b ^ ˙ = − k I ω mes , k I > 0 \begin{aligned} \omega_\text{mes} &= \sum_{i=1}^n k_i\mathbf{v}\times\hat{\mathbf{v}}_i^T,\qquad k_i > 0 \\ \dot{\hat{q}} &= \frac{1}{2}\hat{q}\otimes\mathbf{p}\left(\Omega^y - \hat{\mathbf{b}} + k_P\omega_\text{mes}\right),\qquad k_P > 0 \\ \dot{\hat{\mathbf{b}}} &= -k_I\omega_\text{mes},\qquad k_I > 0 \end{aligned} ωmesq^˙b^˙=i=1nkiv×v^iT,ki>0=21q^p(Ωyb^+kPωmes),kP>0=kIωmes,kI>0
    其中, p   :   R 3 → Q \mathbf{p}\,:\,\mathbb{R}^3\to\mathbb{Q} p:R3Q

    Mahony互补滤波器的Matlab代码详解

    下面,我们将解读Matlab版本的Mahony互补滤波器代码,这个版本是由Madgwick实现的。

    function obj = UpdateIMU(obj, Gyroscope, Accelerometer)
        q = obj.Quaternion; % short name local variable for readability
     
        % Normalise accelerometer measurement
        if(norm(Accelerometer) == 0), return; end   % handle NaN
        Accelerometer = Accelerometer / norm(Accelerometer);	% normalise magnitude
     
        % Estimated direction of gravity and magnetic flux
        v = [2*(q(2)*q(4) - q(1)*q(3))
            2*(q(1)*q(2) + q(3)*q(4))
            q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
     
        % Error is sum of cross product between estimated direction and measured direction of field
        e = cross(Accelerometer, v); 
        if(obj.Ki > 0)
            obj.eInt = obj.eInt + e * obj.SamplePeriod;   
        else
            obj.eInt = [0 0 0];
        end
                
        % Apply feedback terms
        Gyroscope = Gyroscope + obj.Kp * e + obj.Ki * obj.eInt;            
                
        % Compute rate of change of quaternion
        qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]);
     
        % Integrate to yield quaternion
        q = q + qDot * obj.SamplePeriod;
        obj.Quaternion = q / norm(q); % normalise quaternion
    end
    

    Note:

    • obj.eInt就是 b ^ \hat{\mathbf{b}} b^,这里是积分运算。
        if(obj.Ki > 0)
            obj.eInt = obj.eInt + e * obj.SamplePeriod;   
        else
            obj.eInt = [0 0 0];
        end
    
    • 关于代码中Estimated direction of gravity and magnetic flux的解释:首先来看unit quaternion转换为旋转矩阵的公式:The orthogonal matrix corresponding to a rotation by the unit quaternion z = a + b i + c j + d k \mathbf{z} = a + b\mathbf{i} + c\mathbf{j} + d\mathbf{k} z=a+bi+cj+dk (with ∣ z ∣ = 1 \vert\mathbf{z}\vert = 1 z=1,说明是unit quaternion) when post-multiplying with a column vector is given by
      R = ( a 2 + b 2 − c 2 − d 2 2 b c − 2 a d 2 b d + 2 a c 2 b c + 2 a d a 2 − b 2 + c 2 − d 2 2 c d − 2 a b 2 b d − 2 a c 2 c d + 2 a b a 2 − b 2 − c 2 + d 2 ) R=\left(\begin{array}{ccc} a^{2}+b^{2}-c^{2}-d^{2} & 2bc-2ad & 2bd+2ac \\ 2bc+2ad & a^{2}-b^{2}+c^{2}-d^{2} & 2cd-2ab \\ 2bd-2ac & 2cd+2ab & a^{2}-b^{2}-c^{2}+d^{2} \end{array}\right) R=a2+b2c2d22bc+2ad2bd2ac2bc2ada2b2+c2d22cd+2ab2bd+2ac2cd2aba2b2c2+d2
      那么代码中的这段
    v = [2*(q(2)*q(4) - q(1)*q(3))
         2*(q(1)*q(2) + q(3)*q(4))
         q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2];
    

    的计算就是
    v ^ i = R ^ T ( 0 0 1 ) \hat{\mathbf{v}}_i = \hat{R}^T\left(\begin{array}{c} 0 \\ 0 \\ 1 \end{array}\right) v^i=R^T001

    • 这段代码的中的Accelerometer就是 v i \mathbf{v}_i vi:
      v i = R T v 0 i + μ i , v i ∈ { B } \mathbf{v}_i = R^T\mathbf{v}_{0i} + \boldsymbol{\mu}_i,\qquad\qquad\mathbf{v}_i\in\{\mathcal{B}\} vi=RTv0i+μi,vi{B}

    参考文献

    [1] Tarek Hamel, Robert Mahony, Attitude Estimation on SO(3) Based on Direct Inertial Measurements, https://www.researchgate.net/profile/T_Hamel/publication/224635156_Attitude_estimation_on_SO3_based_on_direct_inertial_measurements/links/00b4951a50e818d6be000000/Attitude-estimation-on-SO3-based-on-direct-inertial-measurements.pdf
    [2] Robert Mahony, Tarek Hamel, A Study of Non-linear Complementary Filter Design for Kinematic Systems on the Special Orthogonal group, Internal report: ISRN I3S/RR-2006-36-FR.
    [3] Robert Mahony, Tarek Hamel, Jean-Michel Pflimlin, Non-linear Complementary Filters on the Special Orthogonal Group, IEEE Transactions on Automatic Control, Volume: 53 , Issue: 5 , June 2008. https://hal.archives-ouvertes.fr/hal-00488376/document
    [4] Robert Mahony, Tarek Hamel, Jean-Michel Pflimlin, Complementary filter design on the special orthogonal group SO(3), http://folk.ntnu.no/skoge/prost/proceedings/cdc-ecc05/pdffiles/papers/1889.pdf

    更多相关内容
  • 经典的四元数解算方法官方源码Madgwick 和 Mahony,里面有模拟示例数据和c和matlab类型的数据,特别适合开发所需要的姿态解算和惯性导航所需要的姿态解算。本人亲测代码可以使用。
  • Mahony算法代码.zip

    2020-10-21 15:04:40
    Mahony算法进行姿态解算。
  • 一个旨在演示 IMU 滤波器(互补滤波器、卡尔曼滤波器和 Mahony&Madgwick 滤波器)的项目,其中包含大量参考资料和教程。 参考 鼻子 教程和算法 在嵌入式应用中使用 IMU(加速度计和陀螺仪设备)的指南 IMU 指南算法...
  • 互补滤波以及梯度下降的实例化完整版,还有四元数库
  • Mahony字体

    2019-10-22 11:44:34
    Mahony字体是一款适用于广告设计方面的字体
  • 本文基于四元素利用mahony互补滤波然后通过粒子滤波来实现姿态解算
  • LSM9DS1-AHRS 用于Arduino和LSM9DS1传感器的Mahony AHRS和Tilt Compensated Compass,用于Arduino和LSM9DS1传感器,使用Arduino Pro Mini上的I2C连接为Adafruit LSM9DS1分支板编写和测试。 2021年3月更新:实施了一...
  • Mahony算法MPU9250数据解析和气压计MSE5611读取高度,可用读出横滚角,俯仰角、航向角、以及高度,亲测可用。
  • 本文在Mathematica的帮助下,获得了Kadomtsev-Petviashvili-Benjamin-Bona-Mahony(KP-BBM)方程的总解。 依次绘制一些具有不同行列式值的等高线图,以表明当x2 + y2→∞时,相应的块解趋于零。 特别地,作为说明性...
  • Madgwick Mahony.rar

    2019-10-11 13:30:24
    IMU 姿态解算 AHRS 官方源码,并且还附带了 matlab 仿真示例,数据真实,亲测可用
  • Mahony姿态解算

    2017-04-25 19:54:03
    相比于MPU6050自带的DMP姿态解算,硬件的Mahony姿态解算更快更好,这个压缩包里面有Mahony的源文件以及头文件,欢迎大家下载交流
  • Mahony姿态解算算法详解

    千次阅读 2021-11-28 17:07:10
    本文将介绍Mahony姿态解算算法,不涉及磁力计部分,程序源代码见:https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 姿态的表示方法 欧拉角 物体姿态最直观的表示方法就是欧拉角,一个坐标系相对于另一个...

    本文将介绍Mahony姿态解算算法,不涉及磁力计部分,程序源代码见:https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

    姿态的表示方法

    欧拉角

    物体姿态最直观的表示方法就是欧拉角,一个坐标系相对于另一个坐标系的转动,可以通过围绕三个正交轴的转动来表示,这三个转动角被称为一组欧拉角。常用的欧拉角组是横滚角( r o l l roll roll ϕ \phi ϕ)、俯仰角( p i t c h pitch pitch θ \theta θ)、偏航角( y a w yaw yaw ψ \psi ψ),分别表示绕导航坐标系中 X X X Y Y Y Z Z Z轴的旋转,导航坐标系满足右手坐标系,食指方向为 X X X轴正方向,中指为 Y Y Y轴正方向,大拇指为 Z Z Z轴正方向,这三个旋转角对应的旋转矩阵如下:
    R ( ϕ ) = [ 1 0 0 0 c o s ϕ s i n ϕ 0 − s i n ϕ c o s ϕ ] R ( θ ) = [ c o s θ 0 − s i n θ 0 1 0 s i n θ 0 c o s θ ] R ( ψ ) = [ c o s ψ s i n ψ 0 − s i n ψ c o s ψ 0 0 0 1 ] \begin{aligned} R(\phi)&=\begin{bmatrix}1&0&0\\ 0&cos\phi&sin\phi\\0&-sin\phi&cos\phi\end{bmatrix}\\ R(\theta)&=\begin{bmatrix}cos\theta&0&-sin\theta\\ 0&1&0\\sin\theta&0&cos\theta\end{bmatrix}\\ R(\psi)&=\begin{bmatrix}cos\psi&sin\psi&0\\-sin\psi&cos\psi&0\\0&0&1\end{bmatrix} \end{aligned} R(ϕ)R(θ)R(ψ)=1000cosϕsinϕ0sinϕcosϕ=cosθ0sinθ010sinθ0cosθ=cosψsinψ0sinψcosψ0001
    由于矩阵乘法不满足交换律,所以旋转顺序不同,得到的姿态也不同,一般按照" Z Y X ZYX ZYX"的顺序进行旋转,得到:
    R = R ( ψ ) R ( θ ) R ( ϕ ) = [ c o s θ c o s φ − c o s ϕ s i n ψ + s i n ϕ s i n θ c o s ψ s i n ϕ s i n ψ + c o s ϕ s i n θ c o s ψ c o s θ s i n ψ c o s ϕ c o s ψ + s i n ϕ s i n θ s i n ψ − s i n ϕ c o s ψ + c o s ϕ s i n θ s i n ψ − s i n θ s i n ϕ c o s θ c o s ϕ c o s θ ] \begin{aligned} R&=R(\psi)R(\theta)R(\phi)\\ &=\begin{bmatrix} cos\theta cos\varphi&-cos\phi sin\psi+sin\phi sin\theta cos\psi&sin\phi sin\psi +cos\phi sin\theta cos\psi\\ cos\theta sin\psi&cos\phi cos\psi+sin\phi sin\theta sin\psi&-sin\phi cos\psi+cos\phi sin\theta sin\psi\\ -sin\theta&sin\phi cos\theta &cos\phi cos\theta \end{bmatrix} \end{aligned} R=R(ψ)R(θ)R(ϕ)=cosθcosφcosθsinψsinθcosϕsinψ+sinϕsinθcosψcosϕcosψ+sinϕsinθsinψsinϕcosθsinϕsinψ+cosϕsinθcosψsinϕcosψ+cosϕsinθsinψcosϕcosθ
    虽然欧拉角的表示方式很直观,但是由于万向节死锁的存在,一般不用欧拉角来计算姿态。

    四元数

    四元数(Quaternion)是一种能表示三维物体转动的四维超复数。一般的复数可表示为 a + i b a+ib a+ib,而四元数包含一个实部,三个虚部,用 q 0 + q 1 i + q 2 j + q 3 k q_0+q_1i+q_2j+q_3k q0+q1i+q2j+q3k表示,用向量表示为:
    q = [ q 0 q 1 q 2 q 3 ] [ 1 i j k ] q=\begin{bmatrix}q_0&q_1&q_2&q_3\end{bmatrix}\begin{bmatrix}1\\i\\j\\k\end{bmatrix} q=[q0q1q2q3]1ijk
    假设坐标系 A A A,绕该坐标系中的一个轴 r ^ = [ r x , r y , r z ] \hat r=[r_x,r_y,r_z] r^=[rx,ry,rz]进行旋转,旋转角度为 θ \theta θ,得到坐标系 B B B,如下图:

    image-20210819171816213

    我们可以用如下四元数来表示从 A A A系到 B B B系的旋转,也即 A A A系绕 r ^ \hat r r^轴的正方向(右手准则)转动角度 θ \theta θ后与 B B B系重合。注:使用四元数来表示刚体旋转,必须使四元数的模等于1
    A B q = [ q 0 q 1 q 2 q 3 ] = [ c o s θ 2 r x s i n θ 2 r y s i n θ 2 r z s i n θ 2 ] _A^Bq =\begin{bmatrix}q_0&q_1&q_2&q_3\end{bmatrix} =\begin{bmatrix}cos\frac{\theta}{2}&r_xsin\frac{\theta}{2}&r_ysin\frac{\theta}{2}&r_zsin\frac{\theta}{2}\end{bmatrix} ABq=[q0q1q2q3]=[cos2θrxsin2θrysin2θrzsin2θ]
    相反的旋转可用 q B A q_B^A qBA表示:
    B A q = A B q − 1 = A B q ∗ ∣ ∣ A B q ∣ ∣ _B^Aq={_A^Bq^{-1}}=\frac{_A^Bq^*}{||{_A^Bq}||} BAq=ABq1=ABqABq
    其中 q ∗ q^* q表示共轭, ∣ ∣ q ∣ ∣ ||q|| q表示四元数的模,用于表示姿态的四元数模长固定为1,所以:
    B A q = A B q ∗ = [ q 0 − q 1 − q 2 − q 3 ] _B^Aq={_A^Bq^*}=\begin{bmatrix}q_0&-q_1&-q_2&-q_3\end{bmatrix} BAq=ABq=[q0q1q2q3]
    也即相反的旋转可以用四元数的共轭表示。

    使用 ⊗ \otimes 来表示四元数乘法,对于四元数 a a a b b b,四元数乘积为:
    a ⊗ b = [ a 0 a 1 a 2 a 3 ] ⊗ [ b 0 b 1 b 2 b 3 ] = [ a 0 b 0 − a 1 b 1 − a 2 b 2 − a 3 b 3 a 0 b 1 + a 1 b 0 + a 2 b 3 − a 3 b 2 a 0 b 2 − a 1 b 3 + a 2 b 0 + a 3 b 1 a 0 b 3 + a 1 b 2 − a 2 b 1 + a 3 b 0 ] = [ b 0 − b 1 − b 2 − b 3 b 1 b 0 b 3 − b 2 b 2 − b 3 b 0 b 1 b 3 b 2 − b 1 b 0 ] ⋅ [ a 0 a 1 a 2 a 3 ] = [ a 0 − a 1 − a 2 − a 3 a 1 a 0 − a 3 a 2 a 2 a 3 a 0 − a 1 a 3 − a 2 a 1 a 0 ] ⋅ [ b 0 b 1 b 2 b 3 ] \begin{aligned} a\otimes b&=\begin{bmatrix}a_0\\a_1\\a_2\\a_3\end{bmatrix}\otimes\begin{bmatrix}b_0\\b_1\\b_2\\b_3\end{bmatrix}\\ &=\begin{bmatrix}a_0b_0-a_1b_1-a_2b_2-a_3b_3\\ a_0b_1+a_1b_0+a_2b_3-a_3b_2\\ a_0b_2-a_1b_3+a_2b_0+a_3b_1\\ a_0b_3+a_1b_2-a_2b_1+a_3b_0\end{bmatrix}\\ &=\begin{bmatrix} b_0&-b_1&-b_2&-b_3\\ b_1&b_0&b_3&-b_2\\ b_2&-b_3&b_0&b_1\\ b_3&b_2&-b_1&b_0 \end{bmatrix}\cdot \begin{bmatrix}a_0\\a_1\\a_2\\a_3\end{bmatrix}\\ &=\begin{bmatrix} a_0&-a_1&-a_2&-a_3\\ a_1&a_0&-a_3&a_2\\ a_2&a_3&a_0&-a_1\\ a_3&-a_2&a_1&a_0 \end{bmatrix}\cdot \begin{bmatrix}b_0\\b_1\\b_2\\b_3\end{bmatrix} \end{aligned} ab=a0a1a2a3b0b1b2b3=a0b0a1b1a2b2a3b3a0b1+a1b0+a2b3a3b2a0b2a1b3+a2b0+a3b1a0b3+a1b2a2b1+a3b0=b0b1b2b3b1b0b3b2b2b3b0b1b3b2b1b0a0a1a2a3=a0a1a2a3a1a0a3a2a2a3a0a1a3a2a1a0b0b1b2b3
    a = [ a s , a v ] T a=[a_s, a_v]^T a=[as,av]T b = [ b s , b v ] T b=[b_s, b_v]^T b=[bs,bv]T,下标 s s s表示实部,下标 v v v表示虚部,则两者的乘积可表示为矩阵形式:
    a ⊗ b = [ b s − b v T b v b s I − ( b v × ) ] [ a s a v ] = [ a s − a v T a v a s I + ( a v × ) ] [ b s b v ] \begin{aligned} a\otimes b &=\begin{bmatrix}b_s&-b_v^T\\b_v&b_sI-(b_v\times)\end{bmatrix}\begin{bmatrix}a_s\\a_v\end{bmatrix}\\ &=\begin{bmatrix}a_s&-a_v^T\\a_v&a_sI+(a_v\times)\end{bmatrix}\begin{bmatrix}b_s\\b_v\end{bmatrix} \end{aligned} ab=[bsbvbvTbsI(bv×)][asav]=[asavavTasI+(av×)][bsbv]
    其中 v × v\times v×表示向量 v v v反对称矩阵,如下:
    v × = [ 0 − v 3 v 2 v 3 0 − v 1 − v 2 v 1 0 ] v\times=\begin{bmatrix}0&-v_3&v_2\\v_3&0&-v_1\\-v_2&v_1&0\end{bmatrix} v×=0v3v2v30v1v2v10
    假设 B A q _B^Aq BAq C B q _C^Bq CBq分别表示 B B B系→ A A A系、 C C C系→ B B B系的旋转,则 C C C系→ A A A系的旋转可表示为:
    C A q = B A q ⊗ C B q _C^Aq={_B^Aq}\otimes {_C^Bq} CAq=BAqCBq
    通过如下算子即可将 B B B系中的向量 B v ^Bv Bv转换为 A A A系中的向量 A v ^Av Av
    [ 0 A v ] = B A q ⊗ [ 0 B v ] ⊗ B A q ∗ \begin{bmatrix}0\\^Av\end{bmatrix}={_B^Aq}\otimes \begin{bmatrix}0\\{^Bv}\end{bmatrix} \otimes {_B^Aq^*} [0Av]=BAq[0Bv]BAq
    B A q = [ q 0 q 1 q 2 q 3 ] = [ q s q v ] {_B^Aq}=\begin{bmatrix}q_0\\q_1\\q_2\\q_3\end{bmatrix}=\begin{bmatrix}q_s\\q_v\end{bmatrix} BAq=q0q1q2q3=[qsqv] B A q ∗ = [ q 0 − q 1 − q 2 − q 3 ] = [ q s − q v ] {_B^Aq^*}=\begin{bmatrix}q_0\\-q_1\\-q_2\\-q_3\end{bmatrix}=\begin{bmatrix}q_s\\-q_v\end{bmatrix} BAq=q0q1q2q3=[qsqv],带入四元数乘法的矩阵表示形式,得到:
    A v = B A q ⊗ [ 0 A v ] ⊗ B A q ∗ = [ q s q v T − q v q s I + ( q v × ) ] [ q s − q v T q v q s I + ( q v × ) ] [ 0 A v ] = [ 1 0 T 0 q v q v T + q s 2 I + 2 q s ( q v × ) + ( q v × ) 2 ] [ 0 A v ] \begin{aligned} ^Av&={_B^Aq}\otimes \begin{bmatrix}0\\^Av\end{bmatrix} \otimes {_B^Aq^*}\\ &=\begin{bmatrix}q_s&q_v^T\\-q_v&q_sI+(q_v\times)\end{bmatrix} \begin{bmatrix}q_s&-q_v^T\\q_v&q_sI+(q_v\times)\end{bmatrix}\begin{bmatrix}0\\^Av\end{bmatrix}\\ &=\begin{bmatrix}1&0^T\\0&q_vq_v^T+q_s^2I+2q_s(q_v\times)+(q_v\times)^2\end{bmatrix}\begin{bmatrix}0\\^Av\end{bmatrix} \end{aligned} Av=BAq[0Av]BAq=[qsqvqvTqsI+(qv×)][qsqvqvTqsI+(qv×)][0Av]=[100TqvqvT+qs2I+2qs(qv×)+(qv×)2][0Av]
    其中,矩阵 R 3 × 3 = q v q v T + q s 2 I + 2 q s ( q v × ) + ( q v × ) 2 R_{3\times 3}=q_vq_v^T+q_s^2I+2q_s(q_v\times)+(q_v\times)^2 R3×3=qvqvT+qs2I+2qs(qv×)+(qv×)2就是旋转矩阵的四元数表示形式,展开如下:
    R = [ 2 ( q 0 2 + q 1 2 ) − 1 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) 2 ( q 0 2 + q 2 2 ) − 1 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) 2 ( q 0 2 + q 3 2 ) − 1 ] R=\begin{bmatrix} 2(q_0^2+q_1^2)-1&2(q_1q_2-q_0q_3)&2(q_1q_3+q_0q_2)\\ 2(q_1q_2+q_0q_3)&2(q_0^2+q_2^2)-1&2(q_2q_3-q_0q_1)\\ 2(q_1q_3-q_0q_2)&2(q_2q_3+q_0q_1)&2(q_0^2+q_3^2)-1 \end{bmatrix} R=2(q02+q12)12(q1q2+q0q3)2(q1q3q0q2)2(q1q2q0q3)2(q02+q22)12(q2q3+q0q1)2(q1q3+q0q2)2(q2q3q0q1)2(q02+q32)1
    向量 A v ^Av Av通过左乘 R R R,也能够转换为 B v ^Bv Bv,即: B v = R ⋅ A v ^Bv=R\cdot{^Av} Bv=RAv

    通过如下公式可将四元数转换为欧拉角:
    ϕ = A t a n 2 ( 2 q 2 q 3 − 2 q 0 q 1 , 2 q 0 2 + 2 q 3 2 − 1 ) θ = − s i n − 1 ( 2 q 1 q 3 + 2 q 0 q 2 ) ψ = A t a n 2 ( 2 q 1 q 2 − 2 q 0 q 3 , 2 q 0 2 + 2 q 1 2 − 1 ) \begin{aligned} \phi&=Atan2(2q_2q_3-2q_0q_1,2q_0^2+2q_3^2-1)\\ \theta&=-sin^{-1}(2q_1q_3+2q_0q_2)\\ \psi&=Atan2(2q_1q_2-2q_0q_3,2q_0^2+2q_1^2-1) \end{aligned} ϕθψ=Atan2(2q2q32q0q1,2q02+2q321)=sin1(2q1q3+2q0q2)=Atan2(2q1q22q0q3,2q02+2q121)
    其中

    • ϕ \phi ϕ:横滚角(roll)
    • θ \theta θ:俯仰角(pitch)
    • ψ \psi ψ:偏航角(yaw)

    姿态解算原理

    基于角速度积分得到的姿态

    对角速度进行积分,可以得到角度,四元数微分方程的推导过程较为复杂,本文直接给出,如下:

    b n q ˙ = 1 2 b n q ⊗ [ 0 b ω ] {_b^n\dot{q}}=\frac{1}{2}{_b^nq}\otimes \begin{bmatrix}0\\{^b\omega}\end{bmatrix} bnq˙=21bnq[0bω]
    其中,角标 b b b表示机体(body)坐标系,角标 n n n表示导航(navigation)坐标系, ω \omega ω表示角速度。使用前向差分法对其进行离散化:
    q ˙ = q t − q t − 1 △ t = 1 2 q t − 1 ⊗ [ 0 ω ] \dot q = \frac{q_t-q_{t-1}}{\triangle t}=\frac{1}{2}q_{t-1}\otimes \begin{bmatrix}0\\{\omega}\end{bmatrix} q˙=tqtqt1=21qt1[0ω]
    离散后的方程如下:
    b n q ω , t = b n q t − 1 + 1 2 b n q t − 1 ⊗ [ 0 b ω ] △ t \begin{aligned} {_b^nq}_{\omega,t}&={_b^nq}_{t-1} +\frac{1}{2}{_b^nq}_{t-1}\otimes \begin{bmatrix}0\\{^b\omega}\end{bmatrix}\triangle t \end{aligned} bnqω,t=bnqt1+21bnqt1[0bω]t

    b ω ^b\omega bω表示机体坐标系下的角速度,也就是陀螺仪的测量值。

    基于加速度计观测得到的姿态

    对角速度积分可以得到机体的姿态,但是这个姿态会随着时间漂移,还需要引入其他观测量来对积分得到的姿态进行校正,如使用加速度计测量得到的三轴加速度。

    前面说到,旋转矩阵也是一种姿态的表示方法,它是一个正交矩阵,即满足: R T = R − 1 R^T=R^{-1} RT=R1。除了使用四元数表示外,它还可以用欧拉角表示:
    KaTeX parse error: Expected 'EOF', got '&' at position 3: R&̲=\begin{bmatrix…
    其中: ϕ \phi ϕ表示横滚角(roll), θ \theta θ表示俯仰角(pitch), ψ \psi ψ表示偏航角(yaw)

    如果用 R b n R_b^n Rbn来表示 b b b系到 n n n系的旋转矩阵,则 b b b系中的向量 b v ^bv bv通过左乘 R b n R_b^n Rbn即可转换为 n n n系中的向量 n v ^nv nv,即:
    n v = R b n ⋅ b v ^nv=R_b^n\cdot{^bv} nv=Rbnbv
    那么如何用加速度计的三轴观测值得到机体的姿态呢?我们知道,当IMU在导航坐标系( n n n系)静止时,加速度计的输出为:
    n a = [ 0 0 g ] ^na=\begin{bmatrix}0\\0\\g\end{bmatrix} na=00g
    其中 g g g为重力加速度。此时加速度计在机体坐标系( b b b系)中的输出为:
    b a = [ a x a y a z ] ^ba=\begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix} ba=axayaz
    n a ^na na b a ^ba ba带入 { b a = R n b ⋅ n a R n b = R b n T \begin{cases}^ba=R_n^b\cdot{^na}\\R_n^b={R_b^n}^T\end{cases} {ba=RnbnaRnb=RbnT,得:
    [ a x a y a z ] = R n b ⋅ [ 0 0 g ] = [ − g s i n θ g c o s θ s i n ϕ g c o s θ c o s ϕ ] \begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}=R_n^b\cdot\begin{bmatrix}0\\0\\g\end{bmatrix}=\begin{bmatrix}-gsin\theta\\gcos\theta sin\phi\\gcos\theta cos\phi\end{bmatrix} axayaz=Rnb00g=gsinθgcosθsinϕgcosθcosϕ
    解得:
    ϕ = a r c t a n ( a y a z ) θ = a r c s i n ( − a x g ) \begin{aligned} \phi&=arctan(\frac{a_y}{a_z})\\ \theta&=arcsin(-\frac{a_x}{g}) \end{aligned} ϕθ=arctan(azay)=arcsin(gax)
    由于偏航角 ψ \psi ψ的旋转轴与重力方向平行,所以使用加速度计没办法获得偏航角 ψ \psi ψ,还需要其他的传感器,如磁力计,这里不展开叙述。

    以上只是说明使用加速度计测量值计算姿态的原理,但是在实际应用中并没有这么简单的,因为载体是会运动的,此时加速度计在 n n n系的输出值并不恒等于重力加速度

    互补滤波

    前面说到,陀螺仪解算得到的姿态具有良好的高频特性,但是会随着时间漂移,而加速度计解算得到的姿态具有良好的低频特性,不会随着时间漂移,但是载体剧烈运动时,往往不能解算出真实的姿态。这时可以将陀螺仪的高频特性和加速度计的低频特性相融合,得到高频、低频特性都很好的算法。

    当然这里的融合不是简单地将陀螺仪和加速度计解算得到的姿态进行相加求平均,而是要用互补滤波,以下将介绍Mahony互补滤波算法。

    误差定义

    互补滤波是一种基于误差反馈的滤波器,首先要定义误差,Mahony算法中,用向量叉积来表示误差。向量叉积的模长如下:
    ∣ a × b ∣ = ∣ a ∣ ∣ b ∣ s i n θ |a\times b|=|a||b|sin\theta a×b=absinθ
    a a a b b b均为单位向量,则 ∣ a × b ∣ = s i n θ |a\times b|=sin\theta a×b=sinθ,使用小角度假设,则:
    ∣ a × b ∣ = s i n θ ≈ θ |a\times b|=sin\theta ≈\theta a×b=sinθθ
    两个向量差距越大,它们的叉积模长也越大,因此可以用向量的叉积来表示向量间的误差。接下来需要构造两个向量。

    • 加速度归一化向量

    对加速度计的测量值进行归一化,得到:加速度的归一化向量
    a ′ = [ a x ′ a y ′ a z ′ ] = 1 ∣ a ∣ [ a x a y a z ] a'=\begin{bmatrix}a_x'\\a_y'\\a_z'\end{bmatrix}=\frac{1}{|a|}\begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix} a=axayaz=a1axayaz
    Mahony源码中的相应程序为:

    // 归一化加速度测量值
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    
    • 当前姿态估计得到重力方向向量

    上一节中,当载体处于静止状态时,加速度计在 n n n系的输出恒为 [ 0 0 g ] \begin{bmatrix}0\\0\\g\end{bmatrix} 00g。如果带入四元数形式的旋转矩阵,则有
    [ a x a y a z ] = R n b ⋅ [ 0 0 g ] = g [ 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) 2 ( q 0 2 + q 3 2 ) − 1 ] \begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}=R_n^b\cdot\begin{bmatrix}0\\0\\g\end{bmatrix}=g\begin{bmatrix}2(q_1q_3-q_0q_2)\\2(q_2q_3+q_0q_1)\\2(q_0^2+q_3^2)-1\end{bmatrix} axayaz=Rnb00g=g2(q1q3q0q2)2(q2q3+q0q1)2(q02+q32)1
    我们用 v = [ v x v y v z ] v=\begin{bmatrix}v_x\\v_y\\v_z\end{bmatrix} v=vxvyvz来表示重力方向向量:
    v = [ v x v y v z ] = [ 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) 2 ( q 0 2 + q 3 2 ) − 1 ] v=\begin{bmatrix}v_x\\v_y\\v_z\end{bmatrix}=\begin{bmatrix}2(q_1q_3-q_0q_2)\\2(q_2q_3+q_0q_1)\\2(q_0^2+q_3^2)-1\end{bmatrix} v=vxvyvz=2(q1q3q0q2)2(q2q3+q0q1)2(q02+q32)1
    相应程序如下:

    // 估计重力方向向量
    halfvx = q1 * q3 - q0 * q2;
    halfvy = q0 * q1 + q2 * q3;
    halfvz = q0 * q0 - 0.5f + q3 * q3;
    

    注意此处计算的向量是原始向量的一半,这样做的目的是减少乘法运算量,所以在程序开头的宏定义中,滤波系数 K p K_p Kp K i K_i Ki均为两倍的比例和积分增益。

    #define twoKpDef	(2.0f * 0.5f)	// 2 * 比例增益
    #define twoKiDef	(2.0f * 0.0f)	// 2 * 积分增益
    
    • 向量叉积得到误差

    误差可以表示为:
    E r r = a ′ × v Err = a'\times v Err=a×v
    相应的程序为:

    // 误差就是两个向量的叉积
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);
    

    误差补偿(补偿角速度)

    使用PI控制算法来进行误差补偿,补偿量就是角速度的测量偏差,如下:
    ω b i a s = K p ⋅ E r r + K i ⋅ ∫ E r r \omega_{bias}=K_p\cdot Err + K_i\cdot \int Err ωbias=KpErr+KiErr
    其中 K p K_p Kp为比例系数, K i K_i Ki为积分系数。 K p K_p Kp可以表示对传感器的信任程度, K p K_p Kp越大,越信任加速度计的测量值,反之 K p K_p Kp越小,越信任陀螺仪的数据。 K i K_i Ki用于消除静态误差,也就是消除陀螺仪的零偏。

    最后将补偿量加到角速度测量值上则可得到修正后的角速度。相应的程序为:

    // 计算误差补偿,并应用到角速度测量值上
    if(twoKi > 0.0f) {
        integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// 误差积分
        integralFBy += twoKi * halfey * (1.0f / sampleFreq);
        integralFBz += twoKi * halfez * (1.0f / sampleFreq);
        gx += integralFBx;	// 应用积分反馈项
        gy += integralFBy;
        gz += integralFBz;
    }
    else {
        integralFBx = 0.0f;	// 防止积分饱和
        integralFBy = 0.0f;
        integralFBz = 0.0f;
    }
    
    // 应用比例反馈项
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
    

    姿态递推

    通过前面的PI算法,可以得到修正后的角速度值。再根据前文提到的四元数微分方程对姿态进行递推即可得到融合后的姿态
    b n q ω , t = b n q t − 1 + 1 2 b n q t − 1 ⊗ b ω △ t = b n q t − 1 + 1 2 △ t [ 0 − ω x − ω y − ω z ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y − ω x 0 ] [ q 0 q 1 q 2 q 3 ] \begin{aligned} {_b^nq}_{\omega,t}&={_b^nq}_{t-1} +\frac{1}{2}{_b^nq}_{t-1}\otimes{^b\omega}\triangle t\\ &={_b^nq}_{t-1} +\frac{1}{2}\triangle t \begin{bmatrix} 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{bmatrix}\begin{bmatrix}q_0\\q_1\\q_2\\q_3\end{bmatrix} \end{aligned} bnqω,t=bnqt1+21bnqt1bωt=bnqt1+21t0ωxωyωzωx0ωzωyωyωz0ωxωzωyωx0q0q1q2q3
    相应的程序如下:

    // 利用四元数微分方程进行迭代
    gx *= (0.5f * (1.0f / sampleFreq));
    gy *= (0.5f * (1.0f / sampleFreq));
    gz *= (0.5f * (1.0f / sampleFreq));
    qa = q0;
    qb = q1;
    qc = q2;
    q0 += (-qb * gx - qc * gy - q3 * gz);
    q1 += (qa * gx + qc * gz - q3 * gy);
    q2 += (qa * gy - qb * gz + q3 * gx);
    q3 += (qa * gz + qb * gy - qc * gx); 
    
    // 四元数归一化,用于表示姿态的四元数,其模长必须为1
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
    

    此时姿态还是用四元数来表示的,不是很直观,通过前文提到的公式可以将四元数转换为欧拉角:
    ϕ = A t a n 2 ( 2 q 2 q 3 − 2 q 0 q 1 , 2 q 0 2 + 2 q 3 2 − 1 ) θ = − s i n − 1 ( 2 q 1 q 3 + 2 q 0 q 2 ) ψ = A t a n 2 ( 2 q 1 q 2 − 2 q 0 q 3 , 2 q 0 2 + 2 q 1 2 − 1 ) \begin{aligned} \phi&=Atan2(2q_2q_3-2q_0q_1,2q_0^2+2q_3^2-1)\\ \theta&=-sin^{-1}(2q_1q_3+2q_0q_2)\\ \psi&=Atan2(2q_1q_2-2q_0q_3,2q_0^2+2q_1^2-1) \end{aligned} ϕθψ=Atan2(2q2q32q0q1,2q02+2q321)=sin1(2q1q3+2q0q2)=Atan2(2q1q22q0q3,2q02+2q121)

    调用方法

    在调用Mahony解算程序前,建议先对陀螺仪和加速度计零偏进行估计。将IMU静止一段时间,这段时间角速度和加速度的均值就是陀螺仪和加速度计的零偏。程序如下:

    if(!ready) 
    {
        // 计算零偏
        gyro_offsets_sum[0] += imu.gyro[0];
        gyro_offsets_sum[1] += imu.gyro[1];
        gyro_offsets_sum[2] += imu.gyro[2];
        accel_offsets_sum[0] += imu.accel[0];
        accel_offsets_sum[1] += imu.accel[1];
        accel_offsets_sum[2] += imu.accel[2];
        offset_count++;
        if(offset_count > 3000)
        {
            gyroOffset[0] = gyro_offsets_sum[0] / offset_count;
            gyroOffset[1] = gyro_offsets_sum[1] / offset_count;
            gyroOffset[2] = gyro_offsets_sum[2] / offset_count;
            accelOffset[0] = accel_offsets_sum[0] / offset_count;
            accelOffset[1] = accel_offsets_sum[1] / offset_count;
            accelOffset[2] = accel_offsets_sum[2] / offset_count;
            accelOffset[2]-= 9.81; // 去除重力加速度常量
    
            offset_count = 0;
            gyro_offsets_sum[0] = 0;
            gyro_offsets_sum[1] = 0;
            gyro_offsets_sum[2] = 0;
            ready = true;
        }
        return;
    }
    
    // 去除陀螺仪零偏
    imu.gyro[0] -= gyroOffset[0];
    imu.gyro[1] -= gyroOffset[1];
    imu.gyro[2] -= gyroOffset[2];
    // 去除加速度计零偏
    imu.accel[0] -= accelOffset[0];
    imu.accel[1] -= accelOffset[1];
    imu.accel[2] -= accelOffset[2];
    
    // 调用Mahony解算程序
    MahonyAHRSupdateIMU(imu.gyro[0], imu.gyro[1], imu.gyro[2], 
                     	imu.accel[0], imu.accel[1], imu.accel[2]);
    

    参考

    https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

    https://www.cnblogs.com/WangHongxi/p/12357230.html

    展开全文
  • 该模块通过MEMS惯性传感器测量消防员足部运动的角速度和加速度,利用Mahony互补滤波算法进行姿态解算,并将载体坐标系加速度转化成世界坐标系加速度,对世界坐标系加速度进行二重积分计算,得出消防员在火灾现场中...
  • 以STM32F407为硬件平台,使用MPU9250结合mahony姿态融合算法,融合加速度计、磁力计、陀螺仪数据解算出表征姿态的四元数,并总结成全套资料,供大家学习
  • 目录传感器的方向源码Mahony_9.cMahony_9.h使用方法测试main.c效果 STC15F2K60S2 22.1184MHz Keil uVision V5.29.0.0 PK51 Prof.Developers Kit Version:9.60.0.0 上位机:Vofa+ 1.3.10 移植自MPU6050 获取角度...

    STC15F2K60S2 22.1184MHz
    Keil uVision V5.29.0.0
    PK51 Prof.Developers Kit Version:9.60.0.0
    上位机:Vofa+ 1.3.10


    移植自MPU6050 获取角度理论推导(三)—9轴融合算法 —— shao15232_1

    传感器的方向

    在这里插入图片描述

    源码

           所用MCU为STC15F2K60S2 使用内部RC时钟,22.1184MHz

           stdint.h【51单片机快速入门指南】1:基础知识和工程创建
           软件I2C程序见【51单片机快速入门指南】4: 软件 I2C
           串口部分见【51单片机快速入门指南】3.3:USART 串口通信
           MPU6050驱动程序见【51单片机快速入门指南】4.3: I2C读取MPU6050陀螺仪的原始数据
           HMC5883L/QMC5883L驱动程序见【51单片机快速入门指南】4.4:I2C 读取HMC5883L / QMC5883L 磁力计
           磁力计的椭球拟合校准见【51单片机快速入门指南】4.4.1:python串口接收磁力计数据并进行最小二乘法椭球拟合
           Kp和Ki要按需调整,我这里取10和0.008

    Mahony_9.c

    #include <math.h>
    #include "MPU6050.h"
    
    #define Kp 10.0f        // proportional gain governs rate of convergence to accelerometer/magnetometer
    #define Ki 0.008f       // integral gain governs rate of convergence of gyroscope biases 
    
    float halfT = 1;	// half the sample period采样周期的一半
    float GYRO_K = 1;
    
    void MPU6050_Mahony_Init(float loop_ms)
    {
    	halfT = loop_ms/1000./2;	//计算周期的一半,单位s
    	switch((MPU_Read_Byte(MPU_GYRO_CFG_REG) >> 3) & 3)
    	{
    		case 0:
    			GYRO_K = 1./131/57.3;
    			break;
    		case 1:
    			GYRO_K = 1./65.5/57.3;
    			break;
    		case 2:
    			GYRO_K = 1./32.8/57.3;
    			break;
    		case 3:
    			GYRO_K = 1./16.4/57.3;
    			break;
    	}
    }
    
    float Pitch = 0, Roll = 0, Yaw = 0;
    float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
    float exInt = 0, eyInt = 0, ezInt = 0;   // scaled integral error
    
    void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
    {
    	float norm;
    	float hx, hy, hz, bx, bz;
    	float wx, wy, wz;
    	float vx, vy, vz;
    	float ex, ey, ez;
    
    	// 先把这些用得到的值算好
    	float q0q0 = q0*q0;
    	float q0q1 = q0*q1;
    	float q0q2 = q0*q2;
    	float q0q3 = q0*q3;
    	float q1q1 = q1*q1;
    	float q1q2 = q1*q2;
    	float q1q3 = q1*q3;
    	float q2q2 = q2*q2;
    	float q2q3 = q2*q3;
    	float q3q3 = q3*q3;
    
    	if (ax*ay*az == 0)
    		return;
    
    	if (mx*my*mz == 0)
    		return;
    
    	//将陀螺仪AD值转换为 弧度/s
    	gx = gx * GYRO_K;
    	gy = gy * GYRO_K;
    	gz = gz * GYRO_K;
    
    	norm = sqrt(ax*ax + ay*ay + az*az);       //acc数据归一化
    	ax = ax / norm;
    	ay = ay / norm;
    	az = az / norm;
    
    	norm = sqrt(mx*mx + my*my + mz*mz);       //mag数据归一化
    	mx = mx / norm;
    	my = my / norm;
    	mz = mz / norm;
    
    	hx = 2 * mx *