CN106197475A - 基于序贯滤波的陀螺与磁传感器联合标定方法 - Google Patents

Abstract

本发明提供了一种基于序贯滤波的陀螺与磁传感器联合标定方法，本发明在均匀磁场中充分变化姿态，并同步采集磁力仪与陀螺的测量值，由于磁力仪和陀螺的数据共同用于本发明的磁力仪内部参数、磁力仪外部参数以及陀螺零偏的联合标定，因此标定效果佳。可以实时标定出磁力仪内、外部参数以及陀螺零偏，而且不受任何加速度干扰的影响，因此实施过程中不需要设备保持静止状态。本发明还可用于磁传感器与惯性测量单元(内含陀螺和加速度计)的姿态配准。

Description

基于序贯滤波的陀螺与磁传感器联合标定方法

技术领域

本发明涉及传感器技术领域，具体地，涉及一种基于序贯滤波的陀螺与磁传感器联合标定方法。

背景技术

陀螺和磁传感器(后者又名磁力仪、磁强计)经常用于姿态确定或科学测量领域。陀螺敏感载体的角速度，磁力仪敏感环境磁场。当磁力仪处于铁磁材料附近时，磁力仪周围的磁场受到扭曲，不能正确测量出磁场强度。磁场干扰可分为硬磁效应和软磁效应两种。硬磁效应是由永磁铁或电流产生的加性磁干扰，而软磁效应由软磁材料诱导产生，处于背景磁场中的软磁材料会诱导产生出自身的磁场，并对背景磁场的强度和方向产生扭曲。除此之外，因制造工艺不完善，磁力仪还存在标度因数、敏感轴交叉耦合和偏置等误差，因此，在使用磁力仪之前，必须对以上误差进行标校，即进行磁力仪内部标定。

当磁力仪与陀螺一起使用时，还必须进行磁力仪外部标定，即标定磁力仪与陀螺之间的坐标系失准角。软磁效应不仅会引起磁力仪内部参数的变化，也会导致磁力仪与陀螺的坐标系失准角发生改变。因此，在使用之前，磁力仪内部标定和外部标定都需要进行。目前存在的磁力仪标定方法或要求磁力仪内部标定和外部标定先后分别进行，或要求在静态条件下采集加速度计信息。另一方面，低成本陀螺(如MEMS陀螺)的零偏误差较大，而且每次使用时都有所变化，若不做补偿直接使用陀螺测量值，将影响磁力仪外部标定的效果。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于序贯滤波的陀螺与磁传感器联合标定方法。

根据本发明提供的基于序贯滤波的陀螺与磁传感器联合标定方法，包括如下步骤：

步骤1：建立联合标定问题的状态空间模型；

步骤2：采用序贯滤波方法估计出状态空间模型的状态。

优选地，所述步骤1中联合标定问题的状态空间模型如下：

***方程：

${\stackrel{\·}{C}}_b^i=C_b^i({\mathrm{\ω}}_{ib}^b-\mathrm{\ϵ}-n_g)\×$

$\stackrel{\·}{\mathrm{\ϵ}}=n_{\mathrm{\ϵ}}$

$\stackrel{\·}{S}=0$

$\stackrel{\·}{h}=0$

${\stackrel{\·}{m}}^i=n_{mi}$

其中：陀螺/加速度计的体坐标系表示为b系，惯性坐标系表示为i系，即初始时刻的b系；

式中：表示惯性坐标系相对于体坐标系的姿态矩阵，表示的时间导数，表示陀螺测量的b系下的角速度矢量，ε表示陀螺零偏，表示ε的时间导数，n_g表示陀螺测量的高斯噪声，n_ε表示陀螺零偏的高斯噪声，矩阵S表示磁力仪参数矩阵，表示矩阵S的时间导数，向量h表示磁力仪零偏，表示h的时间导数，mⁱ为i系下表示的恒定磁场向量，表示mⁱ的时间导数，n_mi表示因地球自转引起的磁场向量误差；其中：运算a×表示由一个三维向量a＝[a₁ a₂ a₃]^T构成的叉乘矩阵，具体地，a×的展开式如下：

$a\×=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right];$

观测方程：

$y_m={\mathrm{SC}}_i^b{m}^i+h+n_m$

式中：y_m表示磁力仪的测量值，为姿态矩阵的转置，即上标T表示向量的转置运算，n_m表示磁力仪测量值的高斯噪声。

优选地，步骤1中的状态空间模型的状态包括姿态、磁力仪内部参数、磁力仪外部参数、陀螺零偏和磁场向量；磁场向量mⁱ的初始值包括：磁力仪的首次测量值；磁力仪参数矩阵S的初始值包括：单位矩阵或事先给定；陀螺零偏ε和磁力仪零偏h的初始值选为零；因状态可观性的关系，姿态的初始值确定为单位矩阵，对应的初始方差矩阵为零。

优选地，所述步骤2包括：采用序贯滤波方法估计出状态空间模型的状态，所述序贯滤波方法包括：扩展卡尔曼滤波，或者粒子滤波法；具体地，采用扩展卡尔曼滤波时，包括如下步骤：

步骤2.1：定义状态误差δx为估计值减去真值x，即δ表示对应状态的误差，而姿态估计值与姿态真值及姿态误差ψ的关系定义为

则相应的状态误差向量表示为δx≡[ψ^T δεT vec^T(δS) δh^T δm^iT]^T，vec(·)表示将矩阵按照列的顺序拼接起来的运算；ψ^T表示姿态误差ψ的转置，ε^T表示陀螺零偏向量ε的转置，h^T、m^iT分别表示h、mⁱ的转置；

状态误差的近似线性状态空间模型按如下给出

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{x}=F\mathrm{\δ}x+Gw\\ {\mathrm{\δy}}_m=H_m\mathrm{\δ}x+n_m\end{array}\right.$

其中：噪声

$F=\left[\begin{array}{ccc}{0}_{3\×3}& -C_b^i& 0_{3\×15}\\ 0_{18\×3}& 0_{18\×3}& 0_{18\×15}\end{array}\right],G=\left[\begin{array}{ccc}-C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_3& 0_{3\×3}\\ 0_{12\×3}& 0_{12\×3}& 0_{12\×3}\\ 0_{3\×3}& 0_{3\×3}& I_3\end{array}\right],$

$H_m=\[\begin{array}{ccccc}-{\mathrm{SC}}_i^b\left(m^i\right)\×& 0_{3\×3}& {\left(C_i^b{m}^i\right)}^T\⊗I_3& I_3& {\mathrm{SC}}_i^b\end{array}\]$

式中：表示δx的时间导数，矩阵F、G、H_m分别表示***矩阵、***输入矩阵、观测矩阵；I表示单位矩阵，I的下标表示单位矩阵的阶数；0表示零矩阵，0的下标表示零矩阵的行列数；符号表示矩阵与矩阵的Kronecker乘积；

步骤2.2：不失一般性，假定当地磁场向量的大小为1，对扩展卡尔曼滤波的结果进行如下尺度调整；调整公式如下：

S_rs＝||mⁱ||S，

mⁱ _rs＝mⁱ/||mⁱ||；

式中：S_rs表示尺度调整后的磁力仪矩阵，mⁱ _rs表示尺度调整后的磁场向量，||·||表示取模运算。

与现有技术相比，本发明具有如下的有益效果：

本发明中磁力仪与陀螺固定连接，在均匀磁场中充分变化姿态，并同步采集磁力仪与陀螺的测量值，由于磁力仪和陀螺的数据共同用于本发明的磁力仪内部参数、磁力仪外部参数以及陀螺零偏的联合标定，因此零标定效果佳。可以实时标定出磁力仪内、外部参数以及陀螺零偏，而且不受任何加速度干扰的影响，因此实施过程中不需要设备保持静止状态。本发明还可用于磁传感器与惯性测量单元(内含陀螺和加速度计)的姿态配准。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

根据本发明提供的基于序贯滤波的陀螺与磁传感器联合标定方法，包括如下步骤：

步骤1：建立联合标定问题的状态空间模型；

步骤2：采用序贯滤波方法估计出状态空间模型的状态。

所述步骤1中联合标定问题的状态空间模型如下：

***方程：

${\stackrel{\·}{C}}_b^i=C_b^i(h_{ib}^b-\mathrm{\ϵ}-n_g)\×$

$\stackrel{\·}{\mathrm{\ϵ}}=n_{\mathrm{\ϵ}}$

$\stackrel{\·}{S}=0$

$\stackrel{\·}{h}=0$

${\stackrel{\·}{m}}^i=n_{mi}$

式中：表示惯性坐标系相对于体坐标系的姿态矩阵，表示的时间导数，表示陀螺测量的b系下的角速度矢量，ε表示陀螺零偏，表示ε的时间导数，n_g表示陀螺测量的高斯噪声，n_ε表示陀螺零偏的高斯噪声，矩阵S表示磁力仪参数矩阵，表示矩阵S的时间导数，向量h表示磁力仪零偏，表示h的时间导数，mⁱ为i系下表示的恒定磁场向量，表示mⁱ的时间导数，n_mi表示因地球自转引起的磁场向量误差；其中：运算a×表示由一个三维向量a＝[a₁ a₂ a₃]^T构成的叉乘矩阵，具体地，a×的展开式如下：

$a\×=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right];$

观测方程：

$y_m={\mathrm{SC}}_i^b{m}^i+h+n_m$

式中：ym表示磁力仪的测量值，为姿态矩阵的转置，即上标T表示向量的转置运算，n_m表示磁力仪测量值的高斯噪声。

具体地，不是一般性，假定均匀磁场的磁场强度为1，即||mⁱ||＝1。以上模型的状态包括姿态、磁力仪内部参数、磁力仪外部参数、陀螺零偏和磁场向量。磁场向量mⁱ的初始值可选为磁力仪的首次测量值，磁力仪参数矩阵S的初始值可选为单位矩阵或事先给定，陀螺零偏ε和磁力仪参数矩阵h的初始值可选为零。因状态可观性的关系，姿态的初始值确定为单位矩阵，对应的初始方差矩阵为零。

基于以上模型，可采用序贯滤波方法(如扩展卡尔曼滤波、粒子滤波等)估计出状态空间模型的状态。为了满足||mⁱ||＝1的状态约束，需要对序贯滤波的结果进行如下尺度调整：S_rs＝||mⁱ||S，mⁱ _rs＝mⁱ/||mⁱ||，其中：S_rs表示尺度调整后的磁力仪矩阵，mⁱ _rs表示尺度调整后的磁场向量，||·||表示取模运算。

下面以扩展卡尔曼滤波为例扼要阐述具体实施过程：

定义状态误差δx为估计值减去真值x，即而姿态估计值与姿态真值及姿态误差ψ的关系定义为则相应的状态误差向量表示为δx＝[ψ^T δεTvec^T(δS) δh^T δm^iT]^T，上部T表示向量的转置运算，vec(·)表示将矩阵按照列的顺序拼接起来的运算；ψ^T表示姿态误差ψ的转置，ε^T表示陀螺零偏向量ε的转置，h^T、m^iT分别表示h、mⁱ的装置；

状态误差的线性近似状态空间模型按如下给出

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{x}=F\mathrm{\δ}x+Gw\\ \mathrm{\δ}{y}_m=H_m\mathrm{\δ}x+n_m\end{array}\right.$

其中噪声矩阵

$F=\left[\begin{array}{ccc}{0}_{3\×3}& -C_b^i& 0_{3\×15}\\ 0_{18\×3}& 0_{18\×3}& 0_{18\×15}\end{array}\right],G=\left[\begin{array}{ccc}-C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_3& 0_{3\×3}\\ 0_{12\×3}& 0_{12\×3}& 0_{12\×3}\\ 0_{3\×3}& 0_{3\×3}& I_3\end{array}\right],$

$H_m=\[\begin{array}{ccccc}-{\mathrm{SC}}_i^b\left(m^i\right)\×& 0_{3\×3}& {\left(C_i^b{m}^i\right)}^T\⊗I_3& I_3& {\mathrm{SC}}_i^b\end{array}\]$

式中：矩阵F、G、H_m分别表示***矩阵、***输入矩阵和观测矩阵；I₃表示三阶单位矩阵；符号表示矩阵与矩阵的Kronecker乘积；

对扩展卡尔曼滤波的结果进行如下尺度调整：S_rs＝||mⁱ||S，mⁱ _rs＝mⁱ/||mⁱ||。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (4)

1.一种基于序贯滤波的陀螺与磁传感器联合标定方法，其特征在于，包括如下步骤：

步骤1：建立联合标定问题的状态空间模型；

步骤2：采用序贯滤波方法估计出状态空间模型的状态。

2.根据权利要求1所述的基于序贯滤波的陀螺与磁传感器联合标定方法，其特征在于，所述步骤1中联合标定问题的状态空间模型如下：

***方程：

${\stackrel{\·}{C}}_b^i=C_b^i({\mathrm{\ω}}_{ib}^b-\mathrm{\ϵ}-n_g)\×$

$\stackrel{\·}{\mathrm{\ϵ}}=n_{\mathrm{\ϵ}}$

$\stackrel{\·}{S}=0$

$\stackrel{\·}{h}=0$

${\stackrel{\·}{m}}^i=n_{mi}$

其中：陀螺/加速度计的体坐标系表示为b系，惯性坐标系表示为i系，即初始时刻的b系；

式中：表示惯性坐标系相对于体坐标系的姿态矩阵，表示的时间导数，表示陀螺测量的b系下的角速度矢量，ε表示陀螺零偏，表示ε的时间导数，n_g表示陀螺测量的高斯噪声，n_ε表示陀螺零偏的高斯噪声，矩阵S表示磁力仪参数矩阵，表示矩阵S的时间导数，向量h表示磁力仪零偏，表示h的时间导数，mⁱ为i系下表示的恒定磁场向量，表示mⁱ的时间导数，n_mi表示因地球自转引起的磁场向量误差；其中：运算a×表示由一个三维向量a＝[a₁ a₂ a₃]^T构成的叉乘矩阵，具体地，a×的展开式如下：

$a\×=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right];$

观测方程：

$y_m={\mathrm{SC}}_i^b{m}^i+h+n_m$

式中：y_m表示磁力仪的测量值，为姿态矩阵的转置，即上标T表示矩阵或向量的转置运算，n_m表示磁力仪测量值的高斯噪声。

3.根据权利要求1所述的基于序贯滤波的陀螺与磁传感器联合标定方法，其特征在于，步骤1中的状态空间模型的状态包括姿态、磁力仪内部参数、磁力仪外部参数、陀螺零偏和磁场向量；磁场向量mⁱ的初始值包括：磁力仪的首次测量值；磁力仪参数矩阵S的初始值包括：单位矩阵或事先给定；陀螺零偏ε和磁力仪零偏h的初始值选为零；因状态可观性的关系，姿态的初始值确定为单位矩阵，对应的初始方差矩阵为零。

4.根据权利要求3所述的基于序贯滤波的陀螺与磁传感器联合标定方法，其特征在于，所述步骤2包括：采用序贯滤波方法估计出状态空间模型的状态，所述序贯滤波方法包括：扩展卡尔曼滤波，或者粒子滤波法；具体地，采用扩展卡尔曼滤波时，包括如下步骤：

步骤2.1：定义状态误差δx为估计值减去真值x，即δ表示对应状态的误差，而姿态估计值与姿态真值及姿态误差ψ的关系定义为则相应的状态误差向量表示为δx≡[ψ^T δε^T vec^T(δS) δh^T δm^iT]^T，vec(·)表示将矩阵按照列的顺序拼接起来的运算；ψ^T表示姿态误差ψ的转置，ε^T表示陀螺零偏向量ε的转置，h^T、m^iT分别表示h、mⁱ的转置；

状态误差的近似线性状态空间模型按如下给出

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{x}=F\mathrm{\δ}x+Gw\\ {\mathrm{\δy}}_m=H_m\mathrm{\δ}x+n_m\end{array}\right.$

其中：噪声

$F=\left[\begin{array}{ccc}{0}_{3\×3}& -C_b^i& 0_{3\×15}\\ 0_{18\×3}& 0_{18\×3}& 0_{18\×15}\end{array}\right],G=\left[\begin{array}{ccc}-C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_3& 0_{3\×3}\\ 0_{12\×3}& 0_{12\×3}& 0_{12\×3}\\ 0_{3\×3}& 0_{3\×3}& I_3\end{array}\right],$

$H_m=\[\begin{array}{ccccc}-{\mathrm{SC}}_i^b\left(m^i\right)\×& 0_{3\×3}& {\left(C_i^b{m}^i\right)}^T\⊗I_3& I_3& {\mathrm{SC}}_i^b\end{array}\]$

式中：表示δx的时间导数，矩阵F、G、H_m分别表示***矩阵、***输入矩阵和观测矩阵；I表示单位矩阵，I的下标表示单位矩阵的阶数；0表示零矩阵，0的下标表示零矩阵的行列数；符号表示矩阵与矩阵的Kronecker乘积；

步骤2.2：假定当地磁场向量的大小为1，对扩展卡尔曼滤波的结果进行如下尺度调整；调整公式如下：

S_rs＝||mⁱ||S，

mⁱ _rs＝mⁱ/||mⁱ||；

式中：S_rs表示尺度调整后的磁力仪矩阵，mⁱ _rs表示尺度调整后的磁场向量，||·||表示取模运算。