CN112630813B - 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法 - Google Patents

Abstract

本发明公开了一种基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法，用于解决现有无人机姿态测量存在量测的姿态信息误差随着时间累积不断增大的问题，包括以下步骤：基于无人机捷联惯性导航系统中的三个传感器，分别测量无人机载体的角速度、比力和磁场强度；用四元数龙格库塔法处理三轴光纤陀螺仪的角速度数据，初步解算得到三个姿态角的估计值；再用三个方向的磁场值测量得到另一个姿态参数偏航角；本发明利用卡尔曼滤波方法对三轴光纤陀螺仪测得的姿态信息进行初步校正，通过引用北斗卫星测量的速度信息，差分后对无人机惯性器件进行互补滤波补偿，使得无人机在没有GPS导航信息的条件下仍然可以获得准确的姿态估计值。

Description

基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法

技术领域

本发明涉及无人机技术领域，具体为基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法。

背景技术

无人机(UAV)造价低廉且不会造成人员伤亡，非常适合人类无法参与的高危险、高难度任务，而无人机自主飞行的首要条件是要获得高精度、高可靠性的飞行姿态。

目前对于旋翼无人机精准姿态测量已有一些解决方案，如CN110793515A提出一种基于单天线GPS和IMU的大机动条件下无人机姿态估计方法，尽管GPS在以往姿态测量中起到重要作用，但如果单纯依靠GPS，将会处处受制于人甚至在关键时刻失效，而我国自主研制的北斗卫星导航系统，具有完全独立的自主性，是继GPS和GLONASS之后第三个成熟的卫星导航系统。同时，捷联式惯性导航系统(SINS)完全依靠无人机自身搭载的惯性器件如加速度计、陀螺仪等完成姿态测量，具有不依赖任何外界信息、数据更新率高的优点，但其量测的姿态信息误差随着时间累积不断增大。

发明内容

本发明的目的就在于为了解决现有无人机姿态测量存在量测的姿态信息误差随着时间累积不断增大的问题，而提出一种基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法。

本发明的目的可以通过以下技术方案实现：基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法，包括以下步骤：

步骤一：基于无人机捷联惯性导航系统中的三个传感器，分别测量无人机载体的角速度、比力和磁场强度；对三个传感器标定及误差补偿，并对北斗卫星导航系统测量值进行差分后得到线性加速度来补偿三轴加速度计数据；

步骤二：用四元数龙格库塔法处理三轴光纤陀螺仪的角速度数据，初步解算得到三个姿态角的估计值；同时用两个方向的三轴加速度计测量得到俯仰角和滚转角，再用三个方向的磁场值测量得到另一个姿态参数偏航角；得到两组姿态测量值；

步骤三：使用卡尔曼滤波法进行信息融合和姿态校正，用三轴加速度计和三轴磁强计所得到的姿态测量值校正光纤三轴光纤陀螺仪解算得到的每个时刻的姿态估计值，测量输出初步校正后的无人机三个姿态角；

步骤四：使用北斗导航系统测得的差分值补偿三轴加速度计的测量值，利用四元数法求解更新姿态转移矩阵从而更新三轴光纤陀螺仪姿态数据，得到无人机姿态角，并在运动时利用北斗卫星导航信息对偏航角进行校正。

优选的，三个传感器分别为三轴光纤陀螺仪、三轴加速度计和三轴磁强计。

优选的，三个姿态角分别为俯仰角θ、滚转角φ和偏航角γ。

优选的，步骤二中得到三个姿态角的估计值的具体步骤为：

S21：对三轴光纤陀螺仪进行标定和误差补偿后，用四元数龙格库塔法对三轴光纤陀螺仪进行姿态解算和姿态更新；

S22：选取姿态四元素作为状态向量

S23：构造姿态四元数转换矩阵:

S24：解算得四元数表示的初始姿态角为：

θ₀＝arcsin(2(q₂q₃+q₀q₁))

即得到三个姿态角的估计值；

S25：同时，三轴加速度计测量无人机运动状态下的三轴线加速度：

三轴磁力计测量多旋翼无人机运动状态下的三个磁场分量：

m_b＝[m_x，m，m_z]；

S26：利用三阶最优差分器对北斗卫星导航系统速度测量值进行差分得到线性运动加速度

S27：最后利用一阶低通滤波器对三轴加速度计、三轴磁强计和北斗卫星导航系统进行融合，通过计算得到三轴加速度计和三轴磁强计测得的姿态角信息。

优选的，使用卡尔曼滤波法进行信息融合和姿态校正的具体步骤为：获得状态一步预测

进行状态一步预测均方误差矩阵更新：

滤波增益更新：

线性加权平均状态估计：

得到状态估计均方误差矩阵更新：

P_k＝(I-K_kH_k)P_k/k-1。

优选的，得到无人机姿态角的具体步骤为：

S41：对经过卡尔曼滤波初步校正后的三轴光纤陀螺仪数据采用四阶龙格库塔法更新四元数；

S42：将北斗卫星导航系统差分结果补偿三轴加速度计测量值进行互补滤波之后解算的偏航角γ_BDS融入四元数中；

S43：将四元数归一化后，转化为最终测得的无人机姿态角：

q₀＝q₀/norm

q₁＝q₁/norm

q₂＝q₂/norm

q₃＝q₃/norm

θ＝arcsin(2(q₀q₂+q₁q₃))

与现有技术相比，本发明的有益效果是：

1、本发明利用卡尔曼滤波方法对三轴光纤陀螺仪测得的姿态信息进行初步校正，通过引用北斗卫星测量的速度信息，差分后对无人机惯性器件进行互补滤波补偿，使得无人机在没有GPS导航信息的条件下仍然可以获得准确的姿态估计值。

附图说明

为了便于本领域技术人员理解，下面结合附图对本发明作进一步的说明。

图1为本发明的原理框图；

图2为本发明的捷联惯导系统姿态测量示意图。

具体实施方式

下面将结合实施例对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

请参阅图1-2所示，基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法，首先基于无人机捷联惯性导航系统中的三个传感器，即三轴光纤陀螺仪、三轴加速度计和三轴磁强计分别测量无人机载体的角速度、比力和磁场强度，用四元数龙格库塔法处理三轴光纤陀螺仪的角速度数据，初步解算得到三个姿态角(俯仰角、滚转角、偏航角)的估计值；使用卡尔曼滤波法进行信息融合和姿态校正，测量系统最后输出初步校正后的无人机三个姿态角；

其次基于北斗卫星导航系统测量的速度信息，对三轴加速度计、三轴磁强计和三轴光纤陀螺仪进行互补滤波，从而补偿捷联惯性导航测量的姿态漂移误差，使得无人机在缺乏准确GPS导航信息的机动条件下，仍然可以获得准确的姿态估计值，并在运动时利用北斗卫星导航信息对偏航角进行校正；首先，对SINS中的传感器标定及误差补偿，并对北斗卫星导航系统测量值进行差分后得到线性加速度来补偿三轴加速度计数据；然后基于四元数龙库塔法对三轴光纤陀螺仪测得姿态进行解算得到三个姿态角的估计值；同时用两个方向的三轴加速度计测量得到俯仰角和滚转角，再用三个方向的磁场值测量得到另一个姿态参数偏航角；

其次，得到两组姿态测量值后，使用卡尔曼滤波法进行信息融合和姿态校正，用三轴加速度计和三轴磁强计所得到的姿态测量值校正光纤三轴光纤陀螺仪解算得到的每个时刻的姿态估计值，以完成三轴加速度计和三轴磁强计对三轴光纤陀螺仪漂移的补偿，输出无人机在捷联惯导下的三个姿态角；

最后，对补偿后的三轴光纤陀螺仪数据积分，将北斗卫星导航系统得到的偏航角测量值转化为姿态四元数，与卡尔曼滤波得到的四元数进行融合，最终转化为对应的无人机姿态角；

实施例：

对三轴光纤陀螺仪进行标定和误差补偿后，用四元数龙格库塔法对三轴光纤陀螺仪进行姿态解算和姿态更新；选取姿态四元素作为状态向量：

构造姿态四元数转换矩阵：

解算得四元数表示的初始姿态角为：

θ₀＝arcsin(2(q₂q₃+q₀q₁))

同时，三轴加速度计测量无人机运动状态下的三轴线加速度：

a_b＝[a_x，a_y，a_z]

三轴磁强计测量多旋翼无人机运动状态下的三个磁场分量：

m_b＝[m_x，m，m_z]

利用三阶最优差分器对北斗卫星导航系统速度测量值进行差分得到线性运动加速度；

最后利用一阶低通滤波器

对三轴加速度计、三轴磁强计和北斗卫星导航系统进行融合，通过计算可以得到三轴加速度计和磁力计测得的姿态角信息；

对得到的两组姿态测量值采用间接法进行卡尔曼滤波，即以姿态角误差为状态量，实际要得到的是姿态角误差的状态方程，即姿态角误差的随时间更新的传播方程与对应的观测方程；根据卡尔曼滤波原理，基于三轴加速度计与三轴磁强计测量值姿态校正的卡尔曼滤波流程为：首先获得状态一步预测：

进行状态一步预测均方误差矩阵更新：

滤波增益更新：

线性加权平均状态估计：

最后得到状态估计均方误差矩阵更新：

P_k＝(I-K_kH_k)P_k/k-1

最后使用北斗导航系统测得的差分值补偿三轴加速度计的测量值，利用四元数法求解更新姿态转移矩阵从而更新三轴光纤陀螺仪姿态数据；具体过程为：首先，对经过卡尔曼滤波初步校正后的三轴光纤陀螺仪数据采用四阶龙格库塔法更新四元数：

其次，将北斗卫星导航系统差分结果补偿三轴加速度计测量值进行互补滤波之后解算的偏航角γ_BDS融入四元数中：

最后，将四元数归一化后，转化为最终测得的姿态角：

q₀＝q₀/norm

q₁＝q₁/norm

q₂＝q₂/norm

q₃＝q₃/norm

θ＝arcsin(2(q₀q₂+q₁q₃))

本发明在使用时，基于无人机捷联惯性导航系统中的三个传感器，分别测量无人机载体的角速度、比力和磁场强度，用四元数龙格库塔法处理三轴光纤陀螺仪的角速度数据，初步解算得到三个姿态角的估计值；使用卡尔曼滤波法进行信息融合和姿态校正，测量系统最后输出初步校正后的无人机三个姿态角；基于北斗卫星导航系统测量的速度信息，对三轴加速度计、三轴磁强计和三轴光纤陀螺仪进行互补滤波，从而补偿捷联惯性导航测量的姿态漂移误差，使得无人机在缺乏准确GPS导航信息的机动条件下，仍然可以获得准确的姿态估计值，并在运动时利用北斗卫星导航信息对偏航角进行校正。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。

Claims (4)

1.基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法，其特征在于，包括以下步骤：

使用卡尔曼滤波法进行信息融合和姿态校正，用三轴加速度计和三轴磁强计所得到的姿态测量值校正光纤三轴光纤陀螺仪解算得到的每个时刻的姿态估计值，测量输出初步校正后的无人机三个姿态角；

使用北斗导航系统测得的差分值补偿三轴加速度计的测量值，利用四元数法求解更新姿态转移矩阵从而更新三轴光纤陀螺仪姿态数据，得到无人机姿态角，并在运动时利用北斗卫星导航信息对偏航角进行校正；

使用卡尔曼滤波法进行信息融合和姿态校正之前还包括：基于无人机捷联惯性导航系统中的三个传感器，分别测量无人机载体的角速度、比力和磁场强度；对三个传感器标定及误差补偿，并对北斗卫星导航系统测量值进行差分后得到线性加速度来补偿三轴加速度计数据；

用四元数龙格库塔法处理三轴光纤陀螺仪的角速度数据，初步解算得到三个姿态角的估计值；同时用两个方向的三轴加速度计测量得到俯仰角和滚转角，再用三个方向的磁场值测量得到另一个姿态参数偏航角；得到两组姿态测量值；

使用卡尔曼滤波法进行信息融合和姿态校正的具体步骤为：获得状态一步预测；然后进行状态一步预测均方误差矩阵更新、滤波增益更新和线性加权平均状态估计，得到状态估计均方误差矩阵更新；

得到无人机姿态角的具体步骤为：

S41：对经过卡尔曼滤波初步校正后的三轴光纤陀螺仪数据采用四阶龙格库塔法更新四元数；

S42：将北斗卫星导航系统差分结果补偿三轴加速度计测量值进行互补滤波之后解算的偏航角γ_BDS融入四元数中；

S43：将四元数归一化后，转化为最终测得的无人机姿态角。

2.根据权利要求1所述的基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法，其特征在于，三个传感器分别为三轴光纤陀螺仪、三轴加速度计和三轴磁强计。

3.根据权利要求1所述的基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法，其特征在于，三个姿态角分别为俯仰角θ、滚转角φ和偏航角γ。

4.根据权利要求1所述的基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法，其特征在于，得到三个姿态角的估计值的具体步骤为：

S21：对三轴光纤陀螺仪进行标定和误差补偿后，用四元数龙格库塔法对三轴光纤陀螺仪进行姿态解算和姿态更新；

S22：选取姿态四元素作为状态向量；

S23：构造姿态四元数转换矩阵；

S24：解算得四元数表示的初始姿态角即得到三个姿态角的估计值；

S25：同时，三轴加速度计测量无人机运动状态下的三轴线加速度，三轴磁力计测量多旋翼无人机运动状态下的三个磁场分量；

S26：利用三阶最优差分器对北斗卫星导航系统速度测量值进行差分得到线性运动加速度；

S27：最后利用一阶低通滤波器对三轴加速度计、三轴磁强计和北斗卫星导航系统进行融合，通过计算得到三轴加速度计和三轴磁强计测得的姿态角信息。