[发明专利]一种罗盘与加速度计安装误差角的计算方法

摘要

本发明公开了一种罗盘与加速度计安装误差角的计算方法，包括以下步骤：第一步，坐标定义和坐标转换关系矩阵的定义；第二步，构建电子罗盘的测量模型；第三步，重力向量在导航坐标系下的归一化描述；第四步，根据公式以及加速度计的归一化输出；第五步，导航坐标系下，当地的地磁归一化矢量B；第六步，规定载体坐标系b下的归一化地磁矢量M；第七步：加速度计和电子罗盘的安装误差角Δσ的计算。

主权项

1.一种罗盘与加速度计安装误差角的计算方法，其特征在于，包括以下步骤：第一步：坐标定义和坐标转换关系矩阵的定义；采用两个坐标系统分别为导航坐标系n，定义为东北天坐标系；载体坐标系b，在载体坐标系b中，x_b沿多旋翼飞行器的机体指向右，y_b沿多旋翼飞行器的机体指向前，z_b沿多旋翼飞行器的机体垂直向上；从导航坐标系n到载体坐标系b的旋转关系矩阵从载体坐标系b到导航坐标系n的旋转关系矩阵旋转矩阵是旋转矩阵的转置和逆，数学描述公式(1)所示；第二步，构建电子罗盘的测量模型如式(2)所示；其中，mx为三轴电子罗盘的X轴输出数据；my为三轴电子罗盘的Y轴输出数据；mz为三轴电子罗盘的Z轴输出数据；sx为电子罗盘的X轴尺度因子；sy为电子罗盘的Y轴尺度因子；sz为电子罗盘的Z轴尺度因子；mxb为电子罗盘X轴的零位误差；myb为电子罗盘Y轴的零位误差；mzb为电子罗盘Z轴的零位误差；R为地磁场强；第三步，重力向量在导航坐标系下n的归一化描述如公式(3)所示；其中，g为导航坐标系下n的重力向量；第四步，根据公式(1)和(3)以及加速度计的归一化输出，即体坐标系下加速度的归一化输出，可得导航坐标系n下的归一化重力矢量和载体坐标系b下的归一化重力矢量关系如式(4)所示；其中，g为导航系下归一化的重力加速度常数矢量，为体坐标系向导航系的旋转矩阵；a为体坐标系下的归一化重力加速度，即加速度计的测量输出的归一化值；第五步：导航坐标系下，当地的地磁矢量B如式(5)所示；其中，bx是地磁场强常矢量在导航坐标系下北向的分量；by是地磁场强常矢量在导航坐标系下东向,即y轴正向的分量；bz是地磁场强常矢量在导航坐标系下地向，即垂直于x轴和y轴指向地的z轴正向分量；第六步，令载体坐标系b下的归一化地磁矢量M如式(6)所示；当地的地磁矢量B和载体坐标系b下的归一化地磁矢量M的变换关系如式(7)所示；其中为体坐标系到导航坐标系n的转换矩阵；第七步：加速度计和电子罗盘的安装误差角Δσ的计算：由于公式(7)中，B和g均为常数矢量，所以B和g的向量内积应为常数，如式(8)所示：其中为加速度计的载体坐标系b转换到导航坐标系n的转换矩阵；为罗盘的载体坐标系b转换到导航坐标系n的转换矩阵；当加速度计的载体坐标系b和罗盘的载体坐标系b存在夹角时，由式(8)可得gTB＝|g|*|B|*cosσ＝|a|*|M|*cos(σ+Δσ)   (9)其中σ为重力归一化矢量和地磁归一化矢量在导航坐标系b下的夹角；Δσ为加速度计和罗盘的安装误差角；第六步，求取加速度计和罗盘的安装误差角Δσ首先，通过式(9)解算获得σ；然后，通过式(9)解算加速度计和罗盘的安装误差角Δσ。