CN105607106B - 一种低成本高精度bd/mems融合姿态测量方法 - Google Patents

Abstract

本发明涉及一种低成本高精度BD/MEMS融合姿态测量方法，属于导航技术领域。MEMS惯性传感器模块包括一个三轴加速度计，一个三轴陀螺仪和一个三轴磁力计。BD接收机模块包括2个天线，一个集成BD导航芯片。在该方法中，根据MEMS惯性传感器信息求解出载体姿态角，然后辅助AFM(模糊度函数法)算法求解整周模糊度，最后通过扩展卡尔曼滤波器实现BD/MEMS融合姿态测量。本方法解决了在BD信号被障碍物遮情况下，观测到的卫星颗数不足导致卫星测姿系统无法正常工作的问题，解决了信号失锁带来的整周模糊度求解困难，也解决了陀螺仪、加速度计累计误差的问题，该方法相比于传统方法实时性、可靠性等性能更强，精度更高。

Description

一种低成本高精度BD/MEMS融合姿态测量方法

技术领域

本发明属于导航技术领域，涉及一种低成本高精度BD/MEMS融合姿态测量方法。

背景技术

载体的姿态信息是重要导航参数之一。这些参数可以帮助了解实时载体运动状态，在军事、民用等方面都有着重要的作用。诸如火炮、雷达、导弹都需要快速精确的姿态和方位结果，否则对其命中精度就会有很大的影响。

随着载波相位差分技术的快速发展，利用BD系统实现姿态测量成为可能。与惯性导航系统相比，利用BD实现姿态测量，具有无漂移、误差不累计、体积小等特点。但是BD信号容易收到外界干扰，比如天气、地形或者其他客观因素，从而导致无法给予实时准确的姿态信息。而基于微机电系统MEMS(micro-electro-mechanic system,MEMS)惯性传感器实现的姿态测量，具有全自主性，且具有较强的隐蔽性，但无法避免陀螺仪的漂移问题。因此本发明能充分利用BD和MEMS惯性传感器的优点实现基于BD和MEMS惯性传感器的融合姿态测量，能在复杂环境下姿态测量系统的可靠性和准确性。在融合算法方面，组合测姿主要集中在神经网络、扩展卡尔曼滤波和粒子滤波。三种算法对非线性系统都具有良好的性能，但神经网络在事先需要进行参数学习，实现时极为不便；粒子滤波虽然鲁棒性好，但算法复杂度高，若采样时造成样本的有效性和多样性丢失，将导致样本贫化；本发明采用的扩展卡尔曼滤波在算法性能、复杂度等方面都比较适中。

整周模糊度求解算法对于一个单基线的BD姿态测量系统非常重要。目前主要的求解方法有最小二乘搜索算法，优化Cholesky分解，最小二乘降相关平差(Least squaresambiguity decor-relation adjustment,LAMBDA)算法以及将基线长度等作为约束条件的带约束的最小二乘降相关平差算法(Constrained LAMBDA，CLAMBDA)。然而这些算法主要是在高精度浮点解的基础上提高姿态解算的正确率，并没有解决在低精度浮点解情况下姿态解算错误的问题。本发明利用传感器测量得到的姿态信息辅助整周模糊度求解，减小AFM算法的计算量提高整周模糊度的求解成功率解决了AFM算法运算量大无法保证实时解算出整周模糊度的应用瓶颈。

发明内容

有鉴于此，本发明的目的在于提供一种低成本高精度BD/MEMS融合姿态测量方法，该方法首先利用传感器测量得到的姿态信息辅助整周模糊度求解，减小AFM算法的计算量提高整周模糊度的求解成功率，并通过扩展卡尔曼滤波器实现BD/MEMS融合姿态测量。本方法解决了在BD信号被障碍物遮情况下，观测到的卫星颗数不足导致卫星测姿系统无法正常工作的问题，解决了信号失锁带来的整周模糊度求解困难，也解决了陀螺仪、加速度计累计误差的问题。

为达到上述目的，本发明提供如下技术方案：

一种低成本高精度BD/MEMS融合姿态测量方法，其特征在于：包括以下步骤：

步骤1、采集MEMS惯性传感器(三轴陀螺仪、三轴加速度计和三轴磁力计)记录的数据；

步骤2、对步骤1中采集的原始数据进行预处理；

步骤3、利用处理过的加速度计与磁力计原始数据计算载体的姿态信息；

步骤4、根据BD接收机接收到的星历数据，建立双差载波相位观测方程；

步骤5、利用步骤3中计算出来的载体姿态信息辅助AFM算法求解双差整周模糊度；

步骤6、将步骤5中双差整周模糊度带入双差载波相位观测方程求解出姿态角；

步骤7、通过扩展卡尔曼滤波器将步骤6中利用BD信息解算出来的姿态角信息和处理过的MEMS惯性传感器数据进行融合求出最佳的姿态角信息。

进一步，在步骤1中，由STM32对MEMS惯性传感器进行驱动并通过定时器精确控制传感器输出频率。每次采集到的2个传感器数据封装为一帧数据。数据封装完成后通过蓝牙接口向外界输出。

进一步，在步骤2中，将MEMS传感器传回的数据做预处理包括根据数据手册对原始数据进行单位转换和中值滤波。

进一步，在步骤4中，根据卫星星历数据中的载波相位，建立双差载波相位观测方程为：

λ(▽Δφ^ij+N)＝Ab_k+ε

其中λ表示载波波长，▽Δφ^ij卫星i,j的载波相位双差，N表示双差整周模糊度，A为接收机到卫星的单位矢量构成的设计矩阵,b_k为基线向量，ε表示测量误差。

进一步，在步骤5中，根据解算出来的航向角y和俯仰角p，MEMS传感器解算出来的姿态角存在一定的误差Δy_mems和Δp_mems(均大于0)，因此可以确定载体航向角和俯仰角搜索范围为(y_mems-Δy_mems,y_mems+Δy_mems)，(p_mems-Δp_mems,p_mems+Δp_mems)根据姿态角范围求出基线向量范围，进而通过最小二乘算法求出对应的双差整周模糊度浮点解，然后将浮点解带入

AFM算法的适应度函数筛选出双差整周模糊度的最优解。在此范围进行角度搜索可以剔除很多错误解大大减小AFM算法的计算量，提高了运算效率。

进一步，在步骤6中，将步骤5中求出的整周模糊度最优解带入载波相位双差方程通过最小二乘算法求得对应的基线向量b_LS＝(A^TA)^-1A^Tλ(▽Δφ^ij+N)进而求出利用BD信息解算的姿态角(y_BD,p_BD)。

将加速度计数据与BDS测量得到的姿态角作为观测量构建EKF测量方程。

为了让计算更加方便，对该式进行线性化求雅可比矩阵可得测量方程系数矩阵H。

根据建立的状态方程和观测方程构成扩展卡尔曼滤波器，实时估测出载体的最优姿态角所对应的四元数。

本发明的有益效果在于：本发明提供的姿态测量方法解决了在BD信号被障碍物遮情况下BD测姿系统观测到的卫星颗数不足无法正常工作的问题。同时也解决了MEMS惯性传感器系统中陀螺仪、加速度计累计误差的问题，该方法相比于传统方法实时性、可靠性等性能更强，精度更高。

附图说明

为了使本发明的目的、技术方案和有益效果更加清楚，本发明提供如下附图进行说明：

图1为本发明的一种低成本高精度BD/MEMS融合姿态测量流程图；

图2为本发明MEMS辅助AFM算法流程图；

图3为本发明基于EKF的BD/MEMS融合测姿算法流程图；

具体实施方式

现结合附图，对本发明的优选实施例进行详细的描述。

图1为本发明的一种低成本高精度BD/MEMS融合姿态测量流程图。如图所示，在本发明中，MEMS传感器模块包括一个三轴加速度计负责输出比力信息，一个三轴陀螺仪输出角速度信号，一个三轴磁力计负责输出三维磁场强度，BD测姿模块包括两个天线和一个集成BD导航测姿芯片。

步骤A1，MEMS惯性传感器模块和BD接收机模块数据采集。

具体的，由STM32对MEMS惯性传感器进行驱动并通过定时器精确控制传感器输出频率。每次采集到的2个传感器数据封装为一帧数据，其中，帧头和帧尾分别占1个字节，用于确定一帧的起始和结束；陀螺仪、加速度和磁力计分别包括了三个轴的数据，每个轴占用2个字节，即每个传感器的数据占用6个字节，一帧数据共20字节。数据封装完成后通过蓝牙接口向外界输出。

BD接收机模块通过两个天线构成单基线测姿系统同时采集星历数据传回集成BD测姿芯片。

步骤A2，对采集的MEMS传感器原始数据进行预处理。

具体的，将MEMS传感器传回的数据做预处理包括根据数据手册对原始数据进行单位转换和中值滤波。根据手册给定的参数，转换方法如下：

其中，表示由传感器传回的原始数据，scale表示刻度因子，χ表示经过单位转换后的值。在MEMS硬件电路中由于电气特性等原因，传感器容易受到脉冲噪声的影响，输出的数据容易存在突跳，为了剔除突跳数据选择了使用中值滤波的方法。将序列中某一点的数据x_i′用其领域窗口n内的所有点进行排序然后取中值x_med代替即：

步骤A3，利用处理过的加速度计与磁力计原始数据计算载体的姿态信息。

具体的，将三轴加速度计和磁力计的Y轴与载体的Y轴重合，可由加速度计测量得到载体的俯仰角p和横滚角r，由磁力计可以计算得到载体的航向角y。

不同的坐标系通过绕坐标轴连续旋转三次就可以从一坐标系旋转到另一个坐标系，每次旋转可以用一个旋转矩阵表示。在右手坐标系中，假定旋转角以右旋为正，沿x，y和z轴旋转的旋转矩阵可分别表示如下：

载体的三个姿态角：航向角、横滚角、俯仰角的大小分别为y，r，p。使用最广泛的一种旋转的顺序，旋转矩阵为：

由于航向角的大小不影响加速度计的输出，因此先假设y＝0。则姿态矩阵变为：

在地理坐标系下，加速度计的输出应该为：fⁿ＝[0 0 g]^T

由姿态矩阵的定义得

由此可得俯仰角和倾斜角分别为：

再由三角函数关系可得航向角y为：

步骤A4，建立载波相位双差方程。

具体的，根据卫星星历数据中的载波相位，建立双差载波相位观测方程为：

λ(▽Δφ^ij+N)＝Ab_k+ε

其中λ表示载波波长，▽Δφ^ij卫星i,j的载波相位双差，N表示双差整周模糊度，A为接收机到卫星的单位矢量构成的设计矩阵,b_k为基线向量，ε表示测量误差。

步骤A5，利用步骤A3中MEMS传感器计算出来的姿态角建立整周模糊度的搜索空间。

具体的，针对y和p的角度范围，选定合适的步长τ，则适应度函数的最大值所对应的(y,p)即为正确的姿态角。为了缩小收缩的空间，可以利用MEMS惯性传感器提供的姿态信息(y_mems,p_mems)其对应的误差分别为Δy_mems和Δp_mems(均大于0)。则航向角搜索范围为(y_mems-Δy_mems,y_mems+Δy_mems)，俯仰角搜索范围为(p_mems-Δp_mems,p_mems+Δp_mems)。在此范围进行角度搜索可以剔除很多错误解大大减小AFM算法的计算量，提高了运算效率。

步骤A6，根据步骤A5中的搜索空间利用AFM算法求出整周模糊度，并求出基线向量和姿态角。

具体的，由于基线长度为固定值L，搜索空间(y_mems-Δy_mems,y_mems+Δy_mems)和(p_mems-Δp_mems,p_mems+Δp_mems)，基线向量可表示为：b_k＝[Lcospsiny Lcospcosy Lsinp],同样的接收机与卫星i的单位向量也可以用卫星的方位角β和仰角α表示sⁱ＝[cosαⁱ sinβⁱ cosαⁱcosβⁱ sinαⁱ]，则双差整周模糊度的浮点解为将双差整周模糊度的浮点解代入AFM算法的观测值适应度函数其中n表示观测到的卫星总颗数。

尽管减小了搜索空间，但是仍然会有几组相似的适应度函数值接近于最大值1。因此我们选取适应度函数取值大于0.9所对应浮点解，再将这些浮点解取整。为了确定最精确的整周模糊度我们需要通过几个约束条件进行筛选。首先将取整后的整周模糊度带入载波相位双差方程通过最小二乘算法求得对应的基线向量进而求出姿态角以及最小二乘残差再根据以下三个约束条件逐渐减少候选解得个数，最终求得最佳值。约束条件：

(2)应该非常接近真值，他们之间的误差不超过搜索步长；

(3)整周模糊度对应的最小二乘残差最小。

最终双差求得整周模糊度

步骤A7，计算基线向量和姿态

具体的，根据步骤A6中的求得的双差整周模糊度带入载波相位双差观测方程，通过最小二乘算法求得基线向量b_BD＝[x_n y_n z_n]求得航向角和俯仰角：

步骤A8，建立BD/MEMS融合姿态测量扩展卡尔曼滤波器

将加速度计数据与BDS测量得到的姿态角作为观测量构建EKF测量方程。

为了让计算更加方便，对该式进行线性化求雅可比矩阵可得测量方程系数矩阵H。

根据建立的状态方程和观测方程构成扩展卡尔曼滤波器，实时估测出载体的最优姿态角所对应的四元数并求得载体坐标系至本地坐标系的转换矩阵

步骤A9，将EKF求得的最优状态量四元数信息转化为最优姿态角

具体的，由载体坐标系至本地坐标系的转换矩阵为姿态矩阵。

显然，这个变换矩阵的元素是载体姿态角的函数也是关于四元数的函数，根据四元数值求出这个姿态矩阵的元素值，就可以计算出载体的姿态角，即：

俯仰角：p＝arcsinT₃₂,p∈(-90°,90°)

在本发明实施例中，与传统的BD测姿系统中消去伪距精度不高对姿态角精度的影响，同时减小整周模糊度的搜索空间大大减小传统AFM算法的计算量。采用EKF将BD姿态角信息和MEMS传感器信息进行融合测姿，提高了系统在BD信号不良的情况下测姿系统的稳定性和精度。

图2为发明MEMS辅助AFM算法流程图。如图所示，其中步骤应该包括：

步骤B1，建立载波相位双差方程。

具体的，通过星历数据中的载波相位信息建立载波相位双差方程。

步骤B2，求得载体到卫星间的单位矢量

具体的，根据星历数据中卫星的仰角β和方位角α求得载体到卫星间的单位矢量

sⁱ＝[cosαⁱ sinβⁱ cosαⁱ cosβⁱ sinαⁱ]

步骤B3，根据MEMS姿态角信息确定搜索范围。

具体的，针对y和p的角度范围，选定合适的步长τ，则适应度函数的最大值所对应的(y,p)即为正确的姿态角。为了缩小收缩的空间，可以利用MEMS惯性传感器提供的姿态信息(y_mems,p_mems)其对应的误差分别为Δy_mems和Δp_mems(均大于0)。则航向角搜索范围为(y_mems-Δy_mems,y_mems+Δy_mems)，俯仰角搜索范围为(p_mems-Δp_mems,p_mems+Δp_mems)。在此范围进行角度搜索可以剔除很多错误解大大减小AFM算法的计算量，提高了运算效率。

步骤B4，求出对应的整周模糊度浮点解。

步骤B5带入AFM算法的适应度函数

具体的，将双差整周模糊度的浮点解代入AFM算法的观测值适应度函数其中n表示观测到的卫星总颗数。

步骤B6根据适应度函数值和约束条件进行筛选

(2)应该非常接近真值，他们之间的误差不超过搜索步长；

(3)整周模糊度对应的最小二乘残差最小。

最终双差求得整周模糊度

步骤B7求出对应基线向量和姿态角

具体的，将双差整周模糊度带入载波相位双差观测方程，通过最小二乘算法求得基线向量b_BD＝[x_n y_n z_n]求得航向角和俯仰角：

在本发明实施例中，不同于传统的AFM算法，航向角搜索空间为0度到360度，俯仰角搜索空间为负90度到正90度，如此候选的姿态角组合数量庞大从而AFM算法的计算量也非常大无法保证测姿系统的实时性。本发明利用MEMS解算出来的姿态角建立搜索空间大大减少了候选姿态角组合的数量，提高了BD测姿系统的实时性。

参见图3，图3是本发明中基于EKF的BD/MEMS融合测姿算法流程图。如图所示，其中步骤应该包括：

步骤C1，建立扩展卡尔曼状态方程。

步骤C2，建立扩展卡尔曼观测方程

具体的，首先根据姿态角[y_BD p_BD r_MEMS]按下式转换得到对应的四元素

然后将加速度计数据与BDS测量得到的姿态角作为观测量构建EKF测量方程。

为了让计算更加方便，对该式进行线性化求雅可比矩阵可得测量方程系数矩阵H。

步骤C3，进行滤波的到最优的四元数

具体的，在前面数学模型的基础上，采用扩展卡尔曼滤波器EKF方法进行信息融合，获得载体姿态对应四元数最优估计值

状态一步预测方程：

一步预测均方误差方程：P_k/k-1＝F_k/k-1P_k/k-1F_k/k-1 ^T+W

滤波增益方程：K_k＝P_k/k-1H^T(HP_k/k-1H^T+R)^-1

步骤C4，更新载体的姿态角。具体的，将扩展卡尔曼求得的最优四元素转换成姿态角。

r＝arcsin(2(q₁q₃-q₀q₁))

最后说明的是，以上优选实施例仅用以说明本发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离本发明权利要求书所限定的范围。

Claims (3)

1.一种低成本高精度BD/MEMS融合姿态测量方法，其特征在于：包括以下步骤：

步骤1、采集MEMS惯性传感器记录的数据，其中，MEMS惯性传感器包括三轴陀螺仪、三轴加速度计和三轴磁力计；

步骤2、对步骤1中采集的原始数据进行预处理；

步骤3、利用处理过的加速度计与磁力计原始数据计算载体的姿态信息；

步骤4、根据BD接收机接收到的星历数据，建立双差载波相位观测方程；

步骤5、利用步骤3中计算出来的载体姿态信息辅助AFM算法求解双差整周模糊度；

步骤6、将步骤5中双差整周模糊度带入双差载波相位观测方程求解出姿态角；

步骤7、通过扩展卡尔曼滤波器将步骤6中利用BD信息解算出来的姿态角信息和处理过的MEMS惯性传感器数据进行融合求出最佳的姿态角信息；

在步骤1中，由STM32对MEMS惯性传感器进行驱动并通过定时器精确控制传感器输出频率；每次采集到的2个传感器数据封装为一帧数据；数据封装完成后通过蓝牙接口向外界输出；

在步骤2中，将MEMS传感器传回的数据做预处理包括根据数据手册对原始数据进行单位转换和中值滤波；

在步骤3中，加速度计、磁力计和陀螺仪的三轴分别沿着载体坐标系的三轴安装，分别测得加速度计输出在载体坐标系三轴上的投影分量记为：分别测得磁力计输出在载体坐标系三轴上的投影分量记为：分别测得陀螺仪输出在载体坐标系三轴上的投影分量记为：根据加速度计/磁力计组合姿态解算原理求解出载体初始姿态角[y p r]；

在步骤4中，根据卫星星历数据中的载波相位，建立双差载波相位观测方程为：

λ(▽Δφ^ij+N)＝Ab_k+ε

其中λ表示载波波长，▽Δφ^ij为卫星i,j的载波相位双差，N表示双差整周模糊度，A为接收机到卫星的单位矢量构成的设计矩阵,b_k为基线向量，ε表示测量误差；

在步骤5中，根据解算出来的航向角y和俯仰角p，MEMS传感器解算出来的姿态角存在一定的误差Δy_mems和Δp_mems，均大于0，因此可以确定载体航向角和俯仰角搜索范围为(y-Δy_mems,y+Δy_mems)，(p-Δp_mems,p+Δp_mems)根据姿态角范围求出基线向量范围，进而通过最小二乘算法求出对应的双差整周模糊度浮点解，然后将浮点解带入AFM算法的适应度函数筛选出双差整周模糊度的最优解；在此范围进行角度搜索可以剔除很多错误解大大减小AFM算法的计算量，提高了运算效率。

2.根据权利要求1所述的一种低成本高精度BD/MEMS融合姿态测量方法，其特征在于：在步骤6中，将步骤5中求出的整周模糊度最优解带入载波相位双差方程通过最小二乘算法求得对应的基线向量b_LS＝(A^TA)^-1A^Tλ(▽Δφ^ij+N)进而求出利用BD信息解算的姿态角(y_BD,p_BD)。

3.根据权利要求2所述的一种低成本高精度BD/MEMS融合姿态测量方法，其特征在于：在步骤7中，将四元数作为扩展卡尔曼滤波的状态变量，根据旋转四元数更新姿态的离散时间模型作为状态方程其中，ω_k为k时刻陀螺的测量矢量值，w_k为过程噪声矢量其对应的协方差矩阵为W；

将加速度计数据与BDS测量得到的姿态角作为观测量构建EKF测量方程：

其中，为四元数更新得到的姿态旋转矩阵，为BD在k时刻测量得到的姿态角转换得到的四元数；为当地重力加速度归一化矢量测量噪声的协方差矩阵为

根据建立的状态方程和观测方程构成扩展卡尔曼滤波器，实时估测出载体的最优姿态角所对应的四元数。