[发明专利]一种低成本高精度BD/MEMS融合姿态测量方法

权利要求书

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时刻测量得到的姿态角转换得到的四元数；为当地重力加速度归一化矢量测量噪声的协方差矩阵为

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