陀螺与磁传感器联合标定方法
【技术领域】
[0001] 本发明涉及传感器技术领域，具体而言，涉及一种陀螺与磁传感器的联合标定方 法。
【背景技术】
[0002] 陀螺和磁传感器(后者又名磁力仪、磁强计)经常用于姿态确定或科学测量领域。 陀螺敏感载体的角速度，磁力仪敏感环境磁场。当磁力仪处于铁磁材料附近时，磁力仪周围 的磁场受到扭曲，不能正确测量出磁场强度。磁场干扰可分为硬磁效应和软磁效应两种。硬 磁效应是由永磁铁或电流产生的加性磁干扰，而软磁效应由软磁材料诱导产生，处于背景 磁场中的软磁材料会诱导产生出自身的磁场，并对背景磁场的强度和方向产生扭曲。除此 之外，因制造工艺不完善，磁力仪还存在标度因数、敏感轴交叉耦合和偏置等误差，因此，在 使用磁力仪之前，必须对以上误差进行标校。这里所说的标定是指磁力仪内部标定。
[0003]当磁力仪与陀螺一起使用时，还必须进行磁力仪外部标定，即需要标定磁力仪与 陀螺之间的坐标系失准角。软磁效应不仅会引起磁力仪内部参数的变化，也会导致磁力仪 与陀螺的坐标系失准角发生改变。因此，在使用之前，需要进行磁力仪内部标定以及磁力仪 与其他传感器间的坐标系失准角标定。常用的磁力仪标定方法利用了当地的磁场强度与磁 力仪姿态无关这样一个事实，优点是不需要外部辅助设备，缺点是不能标定磁力仪与其他 传感器的坐标系失准角。另一方面，低成本陀螺(如MEMS陀螺)的零偏误差较大，而且每次使 用时都有所变化，如果不做补偿直接使用陀螺测量值，将影响磁力仪与陀螺的坐标系失准 角标定效果。
【发明内容】
[0004] 针对现有技术中的缺陷，本发明的目的是提供一种解决上述技术问题的陀螺与磁 传感器联合标定方法。
[0005] 为解决上述技术问题，本发明提供的一种陀螺与磁传感器联合标定方法，包括如 下步骤：
[0006] 步骤1，获得磁传感器与陀螺的关联测量模型，在所述关联测量模型中包括标定参 数；
[0007] 步骤2,采用基于所述标定参数的约束非线性最小二乘估计得到所述标定参数的 确定值。
[0008] 优选地，所述标定参数包括坐标系失准角和陀螺零偏。
[0009] 优选地，所述关联测量模型为：
[0010]
[0011] 其中，mm(tk+1)表示第k+1个时刻tk+1时磁传感器坐标系m下的磁场向量，m m(tk)表示 第k个时刻tk时磁传感器坐标系m下的磁场向量，k为正整数，m m(t)表示时刻t时磁传感器坐 标系m下的磁场向量，表示磁传感器与陀螺坐标系失准姿态矩阵，ω；；^)表示时刻t时陀 螺坐标系b的角速度向量，ε表示陀螺零偏向量;Vec (·)表示将矩阵按照列的顺序拼接起 来；
[0012] m?(t)X表示由三维向量nTUXnTUh mm(t)2 mm(t)3]%成的叉乘矩阵，即 0 -m'" ( /), m"' (r), rd {τ)χ^ ?rul 0 -，(厂乂，其中，(七^表示时刻t时磁传感器坐标系m下的磁 -mm (r)2 mm (r)t ο 场向量的X方向上的分量;其中mm(t) 2表不时刻t时磁传感器坐标系m下的磁场向量的Y方向 上的分量;其中mm (t) 3表示时刻t时磁传感器坐标系m下的磁场向量的Ζ方向上的分量。
[0013]优选地，所述约束非线性最小二乘估计由所述关联测量模型导出：
[0014]
[0015]其中，S3表示模为1的四维向量;λ为拉格朗日系数;q=[q0 qi q2 q3]T为磁传感器 与陀螺坐标系失准姿态矩阵ft对应的四元数，q()，q1，q2，q3分别表示四元数q的四个分量； εm 表示在磁传感器坐标系下的陀螺零偏，^ = ;
[0016]
[0017] VeC(C(q))表示将矩阵C(q)按照列的顺序拼接起来；
[0018] 磁传感器与陀螺坐标系失准姿态矩阵的系数矩阵Wk为：
[0019] ^=1" 〇> ；：'(〇?(m,!,(/)x)t//
[0020] 陀螺零偏的系数矩阵Mk为：
[0021] = -
[0022] mm(t)X表示由三维向量mlOztnTUh mm(t)2 !^(〇3]7构成的叉乘矩阵，即 0 ~mm (r)3 nf (r)2 mw(〇x= 0 ，其中示时刻t时磁传感器坐标系m下的磁 -m'n (?)2 nin 0： 场向量的X方向上的分量;其中mm(t) 2表不时刻t时磁传感器坐标系m下的磁场向量的Υ方向 上的分量;其中mm (t) 3表示时刻t时磁传感器坐标系m下的磁场向量的Ζ方向上的分量。
[0023] 优选地，利用线性最小二乘方法从所述关联测量模型中得到磁传感器与陀螺坐标 系失准姿态矩阵的初始值C；：(w)和陀螺零偏的初始值ε?%
[0024]
[0025] 从最小二乘解中提取出对应的<：；：(?)和正交化并利用姿态矩阵与四元 数的变换关系得到四元数的初始值q(<))，拉格朗日系数初始值λ(())设为零。
[0026] 优选地，迭代计算直至满足收敛条件：
[0027] Χ(Μ) = # - [Η|#-1J|、,，，/ = 0丄….
[0028] 其中，χ(1+1)表示待估参数X的第i+1次迭代值，χ(1)表示待估参数X的第i次迭代值，J 为雅可比矩阵导数向量，Η为海森矩阵，待估参数X为：
[0029] X~[qr &? if.
[0030] 其中，所述雅可比矩阵导数向量J和所述海森矩阵Η为：
[0031] J = [JJ jJ Η ? Ηβ/
[0032] H= ( Η% 03xl _ 〇L ο
[0033] 其中，
[0034] J?=2M + 2JX(V沈=2ΣΜ?α?，·^ = qVl， k k
[0035]
[0
[0
[0 % % ~Liy -fs' f2 ft ~(h (62 ft) Ll\ % .<h % %
[0039] a(/(r^(C(q)))-2 qa -qx q2 -? -fi i〇 q: -?2 ^ ~f〇 9\ §1 % ft f2 _ f〇： ~°i\ ~9l % _，.
[0040] ? 0 0 D ? 「0 1 Θ 0:] [ :0 0 -1 0] 「Ο Ο Ο -Ιο 0 0 -1 0 0 1 0 0 1 0 0 -1 0 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0 1 0 0 Q 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 Qo = 1 0 0 0 -1 :0 .0 =. .0 .0 1. .0，Q3 = 0. .0 0 -1 D -1 0 0 -1 0 0 0 Q 0 Q 1 0 a 1 0 0 0-10 Θ 0 0 1 -1 0 0 0 0 1 0 0 0 1 Q 0 1 0 0 0 0 Q 01 0 0 1 0 1 0 q 0」 [_0 -1 0 0」 [_ 0 0 -1 0」 LQ 0 0 1。
[0041 ]与现有技术相比，本发明的有益效果如下：
[0042] 1、既可以标定磁传感器与陀螺之间的失准角，还可以同时标定陀螺零偏；
[0043] 2、过程中磁场越稳定，标定效果越好，而且不受任何加速度干扰的影响，因此实施 过程中不需要设备保持静止状态；
[0044] 3、可用于磁传感器与惯性测量单元（内含陀螺和加速度计)的姿态配准。
【具体实施方式】
[0045] 下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术 人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术 人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明 的保护范围。
[0046] 在稳定磁场中，三轴磁传感器测量值的变化完全是由于姿态的变化引起的。基于 此事实，本发明提供了一种三轴磁力仪与三轴陀螺之间的坐标系失准角以及陀螺零偏的联 合标定方法。磁力仪与陀螺固定连接，充分变化姿态并同步采集磁力仪与陀螺的测量。磁力 仪数据可用于磁力仪内部标定，磁力仪和陀螺的数据共同用于本发明的磁力仪与陀螺之间 的坐标系失准角以及陀螺零偏的联合标定。以下假定已经实现了磁力仪内部标定。
[0047] 本发明陀螺与磁传感器联合标定方法包括：获得磁传感器与陀螺的关联测量模 型，所述测量模型中包括坐标系失准角和陀螺零偏等参数;采用基于所述标定参数的约束 非线性最小二乘估计得到所述标定参数确定值。
[0048] 其中，关联测量模型为：
[0049]
[0050] 其中，mm(tk+1)表示第k+Ι个时刻tk+1时磁传感器坐标系m下的磁场向量，m m(tk)表示 第k个时刻tk时磁传感器坐标系m下的磁场向量，k为正整数，m m(t)表示时刻t时磁传感器坐 标系m下的磁场向量，表示磁传感器与陀螺坐标系失准姿态矩阵，表示时刻t时陀 螺坐标系b的角速度向量，ε表示陀螺零偏向量;Vec (·)表示将矩阵按照列的顺序拼