一种提高机载航姿仪解算载体三维姿态角精度的方法与流程

本发明涉及机载航空仪技术领域，尤其涉及一种提高机载航姿仪解算载体三维姿态角精度的方法。

背景技术：

机载航姿仪常采用imu和磁力计为传感器来实现载体三维姿态角的解算，其中加速度计输出比力对水平姿态角提供修正信息。加速度计对水平姿态角的修正在载体机动时必须断开，否则将引起很大的姿态误差，常采用系统运动状态自检验的方法判断出载体是否处于机动状态。如载体在空中做长时间的盘旋机动时，将产生向心加速度，传统机载航姿仪算法中的系统运动状态自检验算法将判断出载体处于机动状态，从而断开加速度计信息对载体水平姿态的修正，载体水平姿态角由陀螺仪数据单独解算，由于陀螺仪误差的存在，会造成姿态误差随时间累积；当载体做加减速运动时，将产生线加速度度，传统机载航姿仪算法中的系统运动状态自检验算法将判断出载体处于机动状态，从而断开加速度计信息对载体水平姿态的修正，载体水平姿态角解算由陀螺仪数据单独完成，由于陀螺仪数据存在漂移，且当载机处于长时间盘旋机动时，将造成载体水平姿态误差累积的问题，以及动态下噪声变大，精度降低，从而影响姿态解算精度。

技术实现要素：

本发明的目的在于提供一种提高机载航姿仪解算载体三维姿态角精度的方法，从而解决现有技术中存在的前述问题。

为了实现上述目的，本发明采用的技术方案如下：

一种提高机载航姿仪解算载体三维姿态角精度的方法，所述方法包括如下步骤，

s1、根据卫星导航接收机获取的载体的东向速度和北向速度，获取载体在右前上载体坐标系下的向心加速度；

s2、根据载体当前周期的地速以及上一周期的地速，获取载体在右前上载体坐标系下的线加速度；

s3、利用载体在右前上载体坐标系下的向心加速度和线加速度，对机载航姿仪中三轴加速度计输出的比力数据进行机动加速度补偿，获取补偿后的加速度；所述机动加速度即为向心加速度和线加速度；

s4、利用补偿后的加速度，计算载体的水平姿态角；

s5、根据载体的水平姿态角、磁航向和机载航姿仪解算得到的三维姿态角，获取三维姿态角误差，使用姿态角误差和惯导的误差方程进行卡尔曼滤波计算，获取惯导误差，并将惯导误差反馈补偿到惯导的捷联解算中；所述三维姿态角即为水平姿态角和磁航向，所述水平姿态角包括俯仰角和横滚角。

优选的，步骤s1具体包括如下内容，

s11、根据载体的东向速度和北向速度，获取载体的地速和航迹角，计算公式为；

其中，ve为载体的东向速度；vn为载体的北向速度；gs为载体的地速；为载体的航迹角；

s12、利用载体的航迹角以及上一周期载体的航迹角计算载体的航迹角变化率，计算公式为；

其中，为上一周期航迹角；为航迹角变化量；时，时，为航迹角变化率；t为地速和航迹角的更新周期；

s13、根据航迹角变化率获取载体的向心加速度，计算公式为；

其中，ac为载体的向心加速度；

s13、利用载体的向心加速度分别计算载体的东向向心加速度和北向向心加速度，计算公式为，

其中，ac_e为载体的东向向心加速度；ac_n为载体的北向向心加速度；

s14、根据载体的东向向心加速度和北向向心加速度，获取载体在右前上载体坐标系下的向心加速度，计算公式为，

其中，ac_x、ac_y、ac_z分别为载体在右前上载体坐标系下x、y、z方向的向心加速度；为导航坐标系n到右前上载体坐标系b的坐标变换矩阵。

优选的，步骤s2具体包括如下内容，

s21、根据载体的地速和上一周期的地速，获取载体的线加速度，计算公式为，

al＝(gs-gslast)/t

其中，al为载体的线加速度；gslast为载体的上一周期的地速；

s22、根据载体的线加速度，获取载体的东向线加速度和北向线加速度，计算公式为，

其中，al_e为载体的东向线加速度；al_n为载体的北向线加速度；

s23、根据载体的东向线加速度和北向线加速度，获取载体在右前上载体坐标系下的线加速度，计算公式为，

其中，al_x、al_y、al_z分别为载体在右前上载体坐标系下x、y、z方向的线加速度。

优选的，步骤s3具体为，对机载航姿仪中的三轴加速度计输出的比力数据ax、ay、az，对其进行机动加速度补偿，补偿后的加速度分别为

优选的，步骤s4具体为使用补偿后的加速度，计算载体的水平姿态角，载体的水平姿态角包括载体的俯仰角和横滚角，

俯仰角计算公式为

当绝对值大于等于1m/s²时，横滚角计算公式为：

当绝对值小于1m/s²时，横滚角计算公式为：

其中，θa为载体的俯仰角；γa为载体的横滚角。

优选的，步骤s5具体为，利用载体的水平姿态角和磁航向，与机载航姿仪捷联解算得到的三维姿态角作差，获取三维姿态角误差；并将三维姿态角误差作为卡尔曼滤波的量测信息，惯导的误差方程作为卡尔曼滤波状态方程，通过卡尔曼滤波计算，得到惯导误差，并将惯导误差反馈补偿到惯导的捷联解算中。。

本发明的有益效果是：利用机载卫星导航接收机的数据解算出载体的机动加速度；在机载航姿仪算法中，将加速度计数据扣除机动加速度后用于载体水平姿态的修正；相较于传统算法中在载体机动时断开加速度计数据对载体水平姿态的修正，无论载体是否机动，都可以使用加速度计数据对载体水平姿态角进行修正，避免了断开加速度计数据修正后陀螺仪导致的载体水平姿态误差漂移问题。

附图说明

图1是本发明实施例中方法的原理示意图；

图2时本发明实施例中载体的机动加速度的计算与补偿过程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不用于限定本发明。

如图1和图2所示，本实施例中，提供了一种提高机载航姿仪解算载体三维姿态角精度的方法，所述方法包括如下步骤，

s1、根据卫星导航接收机获取的载体的东向速度和北向速度，获取载体在右前上载体坐标系下的向心加速度；

s2、根据载体当前周期的地速以及上一周期的地速，获取载体在右前上载体坐标系下的线加速度；

s3、利用载体在右前上载体坐标系下的向心加速度和线加速度，对机载航姿仪中三轴加速度计输出的比力数据进行机动加速度补偿(即将三轴加速度计输出的比力数据扣除向心加速度和线加速度)，获取补偿后的加速度；所述机动加速度即为向心加速度和线加速度；

s4、利用补偿后的加速度，计算载体的水平姿态角；

s5、根据载体的水平姿态角(即s4中通过加速度计数据计算得到的水平姿态角)、磁航向和机载航姿仪解算得到的三维姿态角，获取三维姿态角误差，使用姿态角误差和惯导的误差方程进行卡尔曼滤波计算，获取惯导误差，并将惯导误差反馈补偿到惯导的捷联解算中；所述三维姿态角即为水平姿态角和磁航向，所述水平姿态角包括俯仰角和横滚角。

本实施例中，所述机载航姿仪包括imu和磁力计；其中imu(惯性测量组合)由三轴正交安装的陀螺仪和三轴正交安装的加速度计构成。本发明的原理为：imu中的加速度计输出比力ax、ay、az，根据卫星导航接收机输出的地速gs和航迹角计算出载体的机动加速度，比力数据扣除机动加速度后用于计算载体水平姿态角θa和γa；再利用θa、γa和磁航向ψm，与imu导航解算得到的三维姿态角ψi、θi和γi作差，得到姿态角误差，作为卡尔曼滤波的量测信息，而惯导的误差方程作为卡尔曼滤波状态方程，通过卡尔曼滤波计算，得到惯导误差，并反馈补偿到惯导的捷联解算中，从而实现载体三维姿态角的准确解算。因为有机动加速度的补偿，因此加速度信息无论在载体机动还是非机动状态，都对水平姿态进行修正，避免了传统算法中在载体机动时段开加速度修正导致的姿态误差漂移问题。

本实施例中，针对方法流程中的各个步骤分别详细说明：

一、步骤s1具体包括如下内容，

s11、根据载体的东向速度和北向速度，获取载体的地速和航迹角，计算公式为；

其中，ve为载体的东向速度；vn为载体的北向速度；gs为载体的地速；为载体的航迹角；

卫星导航接收机可配置输出载体地速和航迹角，假如卫星导航接收机只输出载体的东向速度和北向速度，则就可以利用上述公式计算输出载体的地速和航迹角。其中航迹角应转化为北偏东为正0～2π范围内。

s12、利用载体的航迹角以及上一周期载体的航迹角计算载体的航迹角变化率，计算公式为；

其中，为上一周期航迹角；为航迹角变化量；时，时，t为地速和航迹角的更新周期；

航迹角变化率应根据载体具体情况进行限幅和降噪处理；

s13、根据航迹角变化率获取载体的向心加速度，计算公式为；

其中，ac为载体的向心加速度；

s13、利用载体的向心加速度分别计算载体的东向向心加速度和北向向心加速度，计算公式为，

其中，ac_e为载体的东向向心加速度；ac_n为载体的北向向心加速度；

s14、根据载体的东向向心加速度和北向向心加速度，获取载体在右前上载体坐标系下的向心加速度，计算公式为，

其中，ac_x、ac_y、ac_z分别为载体在右前上载体坐标系下x、y、z方向的向心加速度；为导航坐标系n到右前上载体坐标系b的坐标变换矩阵。

二、步骤s2具体包括如下内容，

s21、根据载体的地速和上一周期的地速，获取载体的线加速度，计算公式为，

al＝(gs-gslast)/t

其中，为载体的线加速度；gslast为载体的上一周期的地速；

载体的线加速度al应根据载机具体情况进行限幅和降噪处理；

s22、根据载体的线加速度，获取载体的东向线加速度和北向线加速度，计算公式为，

其中，al_e为载体的东向线加速度；al_n为载体的北向线加速度；

s23、根据载体的东向线加速度和北向线加速度，获取载体在右前上载体坐标系下的线加速度，计算公式为，

其中，al_x、al_y、al_z分别为载体在右前上载体坐标系下x、y、z方向的线加速度。

三、步骤s3具体为，对机载航姿仪中imu内的三轴加速度计输出的比力数据ax、ay、az，对其进行机动加速度补偿，补偿后的加速度分别为

四、步骤s4具体为使用补偿后的加速度，计算载体的水平姿态角，载体的水平姿态角包括载体的俯仰角和横滚角，

俯仰角计算公式为

当绝对值大于等于1m/s²时，横滚角计算公式为：

当绝对值小于1m/s²时，横滚角计算公式为：

其中，θa为载体的俯仰角；γa为载体的横滚角。

五、步骤s5具体为，利用载体的水平姿态角θa和γa以及磁航向ψm，与机载航姿仪捷联解算得到的三维姿态角θi和γi以及ψm作差，获取三维姿态角误差；并将三维姿态角误差作为卡尔曼滤波的量测信息，惯导的误差方程作为卡尔曼滤波状态方程，通过卡尔曼滤波计算，得到惯导误差，并将惯导误差反馈补偿到惯导的捷联解算中。

下面具体解释步骤s5：

1、捷联结算具体为：利用陀螺仪输出角速度数据，扣除地球自转角速度分量后，采用等效旋转矢量算法实现姿态更新，并用加速度计数据实现速度更新，得到航向角ψi、俯仰角θi、横滚角γi。其中，计算地球自转角速度分量所需要的纬度值由初始纬度值代替。

2、卡尔曼滤波状态方程如下：

卡尔曼滤波状态变量为

f8×1＝[φeφnφuδveδvnεrxεryεrz]

其中，φe、φn、φe为数学平台误差角；δve、δvn分别为东向和北向速度误差；εx、εy、εz为三个陀螺的漂移误差。

惯性仪表误差模型：陀螺漂移误差ε考虑随机游走和随机白噪声，加速度计误差仅考虑白噪声：

其中ωg为陀螺仪白噪声，εr为陀螺随机游走，ωa为加速度计白噪声。

各误差状态变量模型如下：

其中，εe、εn、εu为东北天地理坐标系内的等效陀螺漂移；r为地球半径；ωie为地球自转角速度；ve、vn分别为东向和北向速度；fe、fn、fu为加速度计输出在东北天地理坐标系下的投影；ωrx、ωry、ωrz为随机游走的驱动白噪声。

根据误差状态变量模型，将卡尔曼滤波状态方程表示为：

其中，x(t)为t时刻系统的状态向量；f(t)和g(t)分别为系统状态矩阵和噪声矩阵；w(t)为系统噪声向量，包括加速度计白噪声ωa、陀螺仪白噪声ωg和陀螺仪随机游走驱动白噪声ωr。

3、卡尔曼滤波量测方程为：

利用三维姿态误差构建卡尔曼滤波量测方程，量测方程具体为，

式中，z(t)为t时刻系统的量测向量；h(t)为系统量测矩阵；v(t)为系统量测噪声向量；ψm为来自磁力计传感器的磁航向。

量测矩阵h(t)：

根据状态方程和量测方程，离散化后，按照如下公式进行卡尔曼滤波：

4、姿态补偿的具体过程为，利用卡尔曼滤波得到的数学平台误差角对姿态四元数进行补偿，从而实现姿态和航向的修正。由数学平台误差角得到矩阵

计算修正后的姿态矩阵

其中矩阵为捷联解算得到的姿态矩阵。

得到修正后的姿态矩阵后即可得到最终的姿态角和航向角，然后根据姿态角重新计算姿态四元数。

本实施例中，本发明通过采用机载卫星导航接收机输出的地速和航迹角数据，解算出载体的机动加速度，包括向心加速度和线加速度，将加速度计输出的比力数据扣除机动加速度后，再参与载体水平姿态角的误差修正，从而保证了载体在机动和非机动状态下的水平姿态角的解算精度。

通过采用本发明公开的上述技术方案，得到了如下有益的效果：

本发明提供了一种提高机载航姿仪解算载体三维姿态角精度的方法，利用机载卫星导航接收机的数据解算出载体的机动加速度；在机载航姿仪算法中，将加速度计数据扣除机动加速度后用于载体水平姿态的修正；相较于传统算法中在载体机动时断开加速度计数据对载体水平姿态的修正，无论载体是否机动，都可以使用加速度计数据对载体水平姿态角进行修正，避免了断开加速度计数据修正后陀螺仪导致的载体水平姿态误差漂移问题。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视本发明的保护范围。