CN103196445B - 基于匹配技术的地磁辅助惯性的载体姿态测量方法 - Google Patents

Abstract

一种基于匹配技术的地磁辅助惯性的载体姿态测量方法，包括：1)从地磁数据库中读出载体所在位置的地磁矢量信息，建立空间直角坐标系，计算出每个姿态下磁传感器的理论值；2)在一定时间内，从惯性器件得到运动载体的N个测量姿态，从磁强计得到N个磁场测量值；3)根据步骤2得到的载体N个测量姿态，在步骤1所建立的姿态角坐标系中，找到对应的N个点，结合磁传感器N个磁场测量值，采用匹配算法进行匹配，得到载体姿态的匹配结果；4)把匹配结果作为载体的真实姿态来修正惯性器件的输出结果；5)在下一测量时刻，重复执行步骤2-4，实现连续测姿。本发明能够有效的矫正惯性器件的积累误差，实现长时间、高精度的载体姿态测量。

Description

基于匹配技术的地磁辅助惯性的载体姿态测量方法

技术领域

本发明涉及一种飞行器姿态测量方法，具体涉及一种基于匹配技术的地磁辅助惯性的载体姿态测量方法。

背景技术

姿态测量是飞行器控制中不或缺的一部分。目前的姿态测量方法较多，大致可以分为使用惯性器件、无线电信号和自然环境信息三类方法。第一类方法包括捷联惯导和平台惯导，其特点是独立性强，无需测量外界信息，具有较强的抗干扰能力，但是具有误差累积的缺点；第二类方法包括用GPS和地面无线电基站信号测姿，其特点是飞行器需要不断的接收外界信号，误差不积累，但是容易受干扰，复杂环境条件下生存能力弱；第三类方法采用星敏感器、红外地平仪、磁强计等传感器测量自然环境信息来导航，具有不易受干扰，误差不累积等特点。本专利研究的是地磁辅助惯性测姿技术，即以第三类方法辅助第一类方法为应用背景。

惯导系统都具有抗干扰性强，短时精度高，但是长时间工作误差累积精度较低的特点。而地磁测姿具有抗干扰性强、体积小、质量轻、误差不随时间积累等优点。所以地磁辅助惯性测姿，一方面可以根据地磁测姿信息及时对惯性器件进行修正，消除惯导系统运行过程中的累积误差，另一方面可以根据惯导系统提供的姿态信息将地磁匹配区域限定在一定的范围内，提高地磁匹配的可靠性和精度，从而实现两种测姿方式的优势互补，提高运动载体的组合测姿精度。

目前地磁测姿的方案较多，但是由于三维磁传感器不能提供三个独立的测量方程，因此需要其他输入信号作为补充确定载体三维姿态。常用的思路是通过其他传感器或者估计来给出至少一个姿态角，然后再用地磁信息进行解算。解算算法主要有：欧拉角算法，方向余弦法，四元数法，等效转动矢量法。

经检索文献发现，中国发明专利申请号：201010231211.8，专利名称为：一种基于微惯性和地磁技术的自适应三维姿态定位方法，该专利利用三轴微陀螺传感器来预测并用三轴微加速度传感器和三轴磁场传感器的数据更正获取姿态信息。该方法是用三轴磁传感器，使用的是迭代算法。

中国发明专利申请号：201010176997.8，专利名称为：一种基于陀螺、地磁传感器的惯性测量装置，该专利用三轴MEMS陀螺给出角速度信号，再用两个薄膜式地磁传感器给出的旋转角速度作为姿态测量基准，对MEMS的信号进行修正，但磁传感器只能在载体绕纵轴高速旋转时才能给出角速度，因而该装置提供的方法只能应用于载体高速旋转场合。

文献“基于磁传感器组合的旋转弹体姿态测试方法研究”(南京理工大学博士论文，2009.3.1)中，文中给出了全磁传感器的特殊配置方法与磁传感器和陀螺/加速度计组合旋转弹测姿方法，还给出了零交叉和极值比较法等计算方法，但是都需要以弹体偏航角恒定，滚转角速度变化缓慢为前提，因而该文献提供的方法不适用与载体机动场合，应用条件受到严格限制。

发明内容

基于以上不足之处，本发明提供一种基于匹配技术的地磁辅助惯性的载体姿态测量方法，该方法同时利用多个点的地磁测量数据和这些点之间的姿态变化信息，通过匹配算法来测姿。本方法能够有效的矫正惯性器件的累积误差，具体方法包括如下步骤：

步骤1：从地磁数据库中读出载体所在位置的地磁矢量信息，以载体的滚转角、偏航角和俯仰角为坐标轴建立空间直角坐标系，并计算出每个姿态下磁传感器的理论值；

按照如下步骤建立以载体的滚转角、偏航角和俯仰角为坐标轴的空间直角坐标系，首先从地磁数据库中读出载体所在地理位置的地磁矢量信息M，由(1)式所示，下标xyz分别指向北东地方向：

$M=\left[\begin{array}{c}{M}_X\\ M_Y\\ M_Z\end{array}\right]---\left(1\right)$

以载体的滚转角γ、俯仰角θ和偏航角为坐标轴建立空间直角坐标系，由(2)式计算载体在任意姿态时，与载体捷联的单轴磁强计的理论测量值B：

$B=L\left(2\right)L\left(1\right)L\left(0\right)\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]---\left(2\right)$

其中，

$L\left(0\right)=\left[\begin{array}{ccc}0& 1& 0\\ 1& 0& 0\\ 0& 0& -1\end{array}\right]---\left(3\right)$

L(0)把地磁数据库中磁场值从北东地坐标系转换到东北天坐标系，即地理坐标系；L(1)是方向余弦矩阵，用于把地理坐标系下的地磁分量转换成载体坐标系下的磁场分量；L(2)为三维行向量，表示单轴磁强计在载体坐标系下的方位角，用于把地理坐标系下的地磁分量转换到单轴磁强计的轴线上，其具体数值在传感器装配好后，对装配方向进行标定得到；

步骤2：在t-N+1到t的N个时刻，从惯性器件得到运动载体的N个测量姿态A，从磁强计得到N个磁场测量值M_b，如下所示：

M_b＝[m₁m₂…m_N](6)

步骤3：根据步骤2得到的载体N个测量姿态，在步骤1所建立的姿态角坐

标系中，找到对应的N个点，并将该N个点作为参考点集，结合磁传感器N个磁

场测量值，采用匹配算法进行匹配，得到载体姿态的匹配结果；

步骤4：把匹配结果作为载体的真实姿态来修正惯性器件的输出结果；

步骤5：在下一测量时刻t＝t+1，重复执行步骤2～4，从而实现连续测姿。

本发明还具有如下特征：

1、如上所述的步骤(3)采用固定点数的滑动窗口方式进行匹配，每次匹配点数为N点，在新的磁场测量值m_i到来后，将m_i加到匹配序列中，并将m_i-N从该序列中剔除，留下m_i-N+1到m_i共N个测量值作为新的匹配序列，从而实现连续的姿态测量。

2、如上所述的匹配准则采用MAD、MSD、NPROD或Hausdorff距离准则；匹配方法采用ICP算法、遗传算法或模拟退火算法。

3、如上所述的步骤(3)，采用ICP匹配算法和MSD相似性准则匹配过程如下：

(a)对每个测量点，从B中找出磁传感器实测值m_i(i＝1，2，…，N)所在的等值面s_i，并在s_i上找到一点使其距离a_i最近，由此得到最近点点集坐标C如式(7)所示：

(b)求点集A和C的重心和

$\left\{\begin{array}{c}\tilde{A}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{a}_i\\ \tilde{C}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{c}_i\end{array}\right.---\left(8\right)$

(c)根据点集A和C的重心和求取协方差矩阵S：

$S=\frac{1}{N}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(a_i-\tilde{A}){(c_i-\tilde{C})}^T$

(9)

$=\frac{1}{N}\left[\begin{array}{ccc}{S}_{11}& S_{12}& S_{13}\\ S_{21}& S_{22}& S_{23}\\ S_{31}& S_{32}& S_{33}\end{array}\right]$

(d)根据上面矩阵S的元素值，构造W矩阵：

$W=\left[\begin{array}{cccc}{S}_{11}+S_{22}+S_{33}& S_{32}-S_{23}& S_{13}-S_{31}& S_{21}-S_{12}\\ S_{32}-S_{23}& S_{11}-S_{22}-S_{33}& S_{12}+S_{21}& S_{13}+S_{31}\\ S_{13}-S_{31}& S_{12}+S_{21}& -S_{11}+S_{22}-S_{33}& S_{23}+S_{32}\\ S_{21}-S_{12}& S_{13}+S_{31}& S_{23}+S_{32}& -S_{11}+S_{22}-S_{33}\end{array}\right]---\left(10\right)$

(e)求出矩阵W的最大特征值对应的特征向量Q＝[q₀q₁q₂q₃]，其中Q作为一个四元素；

(f)根据Q求出旋转矩阵R和平移向量t，如下所示：

$R=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 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)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)---\left(11\right)$

$t=\tilde{A}-R\tilde{C}---\left(12\right)$

(g)把R和t作用于参考点集A得到新的参考点集如式(13)所示：

$\stackrel{-}{A}=\mathrm{RA}+t---\left(13\right)$

(h)判断是否满足终止迭代条件。若满足，则终止迭代，输出参考位置作为匹配结果；若不满足，则令转入步骤(a)，重复执行步骤(a)～(h)。

终止条件有三个，满足其中任意一项则终止迭代：①迭代次数达到预设次数；②迭代结果收敛到基本不变，即||(RA+t)-A||接近于零；③A与C两点集MSD相关性达到阈值要求，即式(14)小于阈值。

$\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{(a_i-c_i)}^2---\left(14\right)$ 。

为了提高测姿系统的冗余以及提高匹配精度，单轴磁强计可以由三轴磁强计替代，此时三轴磁强计的每个轴都可以通过上述方法匹配得到载体的姿态角，然后采用数据融合算法将三个匹配结果进行数据融合得到可靠性更强、精度更高的匹配结果，最后用该匹配结果修正惯性器件。

本发明的有益效果是：本发明采用单轴磁强计与惯性器件组合测量载体姿态，与传统采用三轴磁强计相比，降低了系统的成本与体积；本发明采用匹配算法实现载体姿态测量，匹配算法原理简单，并可以根据惯导系统提供的姿态信息将地磁匹配区域限定在一定的范围内，提姿态测量的可靠性和精度；本发明能够实现长时间、高精度、连续的载体姿态测量；本发明也可以采用三轴磁强计来提高系统的冗余度以及匹配精度。

附图说明

图1是本发明的测量方法流程图。

具体实施方式

下面结合实例和附图对本发明进一步详细说明。

本发明采用与载体捷联的单轴磁强计和惯性器件通过匹配算法实现载体的姿态测量。以ICP匹配算法和MSD相似性准则为例说明本发明的具体实现步骤如下：

步骤1：从地磁数据库中读出载体所在位置的地磁矢量信息，以载体的滚转角、偏航角和俯仰角为坐标轴建立空间直角坐标系，并计算出每个姿态下磁传感器的理论值。

从地磁数据库中读出载体所在地理位置的地磁矢量信息记为M，如(15)式所示，下标xyz分别指向北东地方向。

$M=\left[\begin{array}{c}{M}_X\\ M_Y\\ M_Z\end{array}\right]---\left(15\right)$

以载体的滚转角γ、俯仰角θ和偏航角为坐标轴建立空间直角坐标系，由(16)式计算载体在任意姿态时，与载体捷联的单轴磁强计的理论测量值B：

$B=L\left(2\right)L\left(1\right)L\left(0\right)\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]---\left(16\right)$

其中，

$L\left(0\right)=\left[\begin{array}{ccc}0& 1& 0\\ 1& 0& 0\\ 0& 0& -1\end{array}\right]---\left(17\right)$

L(0)把地磁数据库中磁场值从北东地坐标系转换到东北天坐标系，即地理坐标系；L(1)是方向余弦矩阵，用于把地理坐标系下的地磁分量转换成载体坐标系下的磁场分量；L(2)为三维行向量，表示单轴磁强计在载体坐标系下的方位角，用于把地理坐标系下的地磁分量转换到单轴磁强计的轴线上，其具体数值在传感器装配好后，对装配方向进行标定得到。

步骤2：在t-N+1到t的N个时刻，从惯性器件得到运动载体的N个测量姿态A，从磁强计得到N个磁场测量值M_b，如下所示：

M_b＝[m₁m₂…m_N](20)

步骤3：根据步骤2得到的载体N个测量姿态，在步骤1所建立的姿态角坐标系中，找到对应的N个点，并将该N个点作为参考点集，结合磁传感器N个磁场测量值，采用匹配算法进行匹配，得到载体姿态的匹配结果。

下面以ICP匹配算法和MSD相似性准则为例说匹配过程。

(a)对每个测量点，从B中找出磁传感器实测值m_i(i＝1，2，…，N)所在的等值面s_i，并在s_i上找到一点使其距离a_i最近，由此得到最近点点集坐标C如式(21)所示：

(b)求点集A和C的重心和

$\left\{\begin{array}{c}\tilde{A}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{a}_i\\ \tilde{C}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{c}_i\end{array}\right.---\left(22\right)$

(c)根据点集A和C的重心和求取协方差矩阵S：

$S=\frac{1}{N}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(a_i-\tilde{A}){(c_i-\tilde{C})}^T$

(23)

$=\frac{1}{N}\left[\begin{array}{ccc}{S}_{11}& S_{12}& S_{13}\\ S_{21}& S_{22}& S_{23}\\ S_{31}& S_{32}& S_{33}\end{array}\right]$

(d)根据上面矩阵S的元素值，构造W矩阵：

$W=\left[\begin{array}{cccc}{S}_{11}+S_{22}+S_{33}& S_{32}-S_{23}& S_{13}-S_{31}& S_{21}-S_{12}\\ S_{32}-S_{23}& S_{11}-S_{22}-S_{33}& S_{12}+S_{21}& S_{13}+S_{31}\\ S_{13}-S_{31}& S_{12}+S_{21}& -S_{11}+S_{22}-S_{33}& S_{23}+S_{32}\\ S_{21}-S_{12}& S_{13}+S_{31}& S_{23}+S_{32}& -S_{11}+S_{22}-S_{33}\end{array}\right]---\left(24\right)$

(e)求出矩阵W的最大特征值对应的特征向量Q＝[q₀q₁q₂q₃]，其中Q可以看做是一个四元素；

(f)根据Q求出旋转矩阵R和平移向量t，如下所示：

$R=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 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)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)---\left(25\right)$

$t=\tilde{A}-R\tilde{C}---\left(26\right)$

(g)把R和t作用于参考点集A得到新的参考点集如式(27)所示：

$\stackrel{-}{A}=\mathrm{RA}+t---\left(27\right)$

(h)判断是否满足终止迭代条件。若满足，则终止迭代，输出参考位置作为匹配结果；若不满足，则令转入步骤(a)，重复执行步骤(a)～(h)。

终止条件有三个，满足其中任意一项则终止迭代：①迭代次数达到预设次数；②迭代结果收敛到基本不变，即||(RA+t)-A||接近于零；③A与C两点集MSD相关性达到阈值要求，即式(28)小于阈值。

$\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{(a_i-c_i)}^2---\left(28\right)$

步骤4：把匹配结果作为载体的真实姿态来修正惯性器件的输出结果；

步骤5：在下一测量时刻t＝t+1，重复执行步骤2～4，从而实现连续测姿。

匹配准则可以采用MAD、MSD、NPROD或Hausdorff距离等相似性准则。匹配方法可以采用ICP算法、遗传算法或模拟退火算法。

用固定点数的滑动窗口方式进行匹配。假设每次匹配点数为N点，在新的磁场测量值m_i到来后，将m_i加到匹配序列中，并将m_i-N从该序列中剔除，留下m_i-N+1到m_i共N个测量值作为新的匹配序列，从而实现连续的姿态测量。

为提高测姿系统的冗余以及提高匹配精度，单轴磁强计可以由三轴磁强计替代，此时三轴磁强计的每个轴都可以通过上述方法匹配得到载体的姿态角，然后采用数据融合算法将三个匹配结果进行数据融合得到可靠性更强、精度更高的匹配结果，最后用该匹配结果修正惯性器件。

Claims (4)

1.一种基于匹配技术的地磁辅助惯性的载体姿态测量方法，其特征在于包括如下步骤：

步骤1：从地磁数据库中读出载体所在位置的地磁矢量信息，以载体的滚转角、偏航角和俯仰角为坐标轴建立空间直角坐标系，并计算出每个姿态下磁强计的理论值；

按照如下步骤建立以载体的滚转角、偏航角和俯仰角为坐标轴的空间直角坐标系，首先从地磁数据库中读出载体所在地理位置的地磁矢量信息M，由(1)式所示，下标xyz分别指向北东地方向：

$M=\left[\begin{array}{c}{M}_X\\ M_Y\\ M_Z\end{array}\right]---\left(1\right)$

以载体的滚转角γ、俯仰角θ和偏航角为坐标轴建立空间直角坐标系，由(2)式计算载体在任意姿态时，与载体捷联的磁强计的理论测量值B：

$B=L\left(2\right)L\left(1\right)L\left(0\right)\left[\begin{array}{c}{M}_x\\ M_y\\ M_z\end{array}\right]---\left(2\right)$

其中，

$L\left(0\right)=\left[\begin{array}{ccc}0& 1& 0\\ 1& 0& 0\\ 0& 0& -1\end{array}\right]---\left(3\right)$

L(0)把地磁数据库中磁场值从北东地坐标系转换到东北天坐标系，即地理坐标系；L(1)是方向余弦矩阵，用于把地理坐标系下的地磁分量转换成载体坐标系下的磁场分量；L(2)为三维行向量，表示磁强计在载体坐标系下的方位角，用于把地理坐标系下的地磁分量转换到磁强计的轴线上，其具体数值在磁强计装配好后，对装配方向进行标定得到；

步骤2：在t-N+1到t的N个时刻，从惯性器件得到运动载体的N个测量姿态A，从磁强计得到N个磁场测量值M_b，如下所示：

M_b＝[m₁m₂…m_N](6)

步骤3：根据步骤2得到的载体N个测量姿态，在步骤1所建立的空间直角坐标系中，找到对应的N个点，并将该N个点作为参考点集，结合磁强计的N个磁场测量值，采用匹配算法进行匹配，得到载体姿态的匹配结果；

步骤4：把匹配结果作为载体的真实姿态来修正惯性器件的输出结果；

步骤5：在下一测量时刻t＝t+1，重复执行步骤2～4，从而实现连续测姿。

2.根据权利要求1所述的一种基于匹配技术的地磁辅助惯性的载体姿态测量方法，其特征在于：所述的步骤3采用固定点数的滑动窗口方式进行匹配，每次匹配点数为N点，在新的磁场测量值m_i到来后，将m_i加到匹配序列中，并将m_i-N从该序列中剔除，留下m_i-N+1到m_i共N个测量值作为新的匹配序列，从而实现连续的姿态测量，其中i＝1，2，……，N。

3.根据权利要求1或2所述的一种基于匹配技术的地磁辅助惯性的载体姿态测量方法，其特征在于：所述的匹配采用MAD、MSD、NPROD或Hausdorff距离准则；匹配方法采用ICP算法、遗传算法或模拟退火算法。

4.根据权利要求1所述的一种基于匹配技术的地磁辅助惯性的载体姿态测量方法，其特征在于：所述的步骤3，采用ICP匹配算法和MSD相似性准则匹配过程如下：

(a)对每个测量点，从B中找出磁强计的实测值m_i，i＝1，2，……，N，所在的等值面s_i，并在s_i上找到一点使其距离a_i最近，由此得到最近点点集坐标C如式(7)所示：

(b)求点集A和C的重心和

$\left\{\begin{array}{c}\tilde{A}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{a}_i\\ \tilde{C}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{c}_i\end{array}\right.---\left(8\right)$

(c)根据点集A和C的重心和求取协方差矩阵S：

$\begin{array}{c}S=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(a_i-\tilde{A}){(c_i-\tilde{C})}^T\\ =\frac{1}{N}\left[\begin{array}{ccc}{S}_{11}& S_{12}& S_{13}\\ S_{21}& S_{22}& S_{23}\\ S_{31}& S_{32}& S_{33}\end{array}\right]\end{array}---\left(9\right)$

(d)根据上面矩阵S的元素值，构造W矩阵：

$W=\left[\begin{array}{cccc}{S}_{11}+S_{22}+S_{33}& S_{32}-S_{23}& S_{13}-S_{31}& S_{21}-S_{12}\\ S_{32}-S_{23}& S_{11}-S_{22}-S_{33}& S_{12}+S_{21}& S_{13}+S_{31}\\ S_{13}-S_{31}& S_{12}+S_{21}& {-S}_{11}+S_{22}-S_{33}& S_{23}+S_{32}\\ S_{21}-S_{12}& S_{13}+S_{31}& S_{23}+S_{32}& -S_{11}+S_{22}-S_{33}\end{array}\right]---\left(10\right)$

(e)求出矩阵W的最大特征值对应的特征向量Q＝[q₀q₁q₂q₃]，其中Q作为一个四元素；

(f)根据Q求出旋转矩阵R和平移向量t₁，如下所示：

$R=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 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)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)---\left(11\right)$

$t_1=\tilde{A}-R\tilde{C}---\left(12\right)$

(g)把R和t₁作用于参考点集A得到新的参考点集如式(13)所示：

$\stackrel{\‾}{A}=\mathrm{RA}+t_1---\left(13\right)$

(h)判断是否满足终止迭代条件，若满足，则终止迭代，输出参考位置作为匹配结果；若不满足，则令转入步骤(a)，重复执行步骤(a)～(h)；

终止条件有三个，满足其中任意一项则终止迭代：①迭代次数达到预设次数；②迭代结果收敛到基本不变，即||(RA+t₁)-A||接近于零；③A与C两点集MSD相关性达到阈值要求，即式(14)小于阈值，