具体实施方式

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

在工程应用上，只通过磁传感器的方式进行融合求解，极容易受到磁场干扰，如果干扰数据被带入了航向角的测算中，则会得到带有误差或错误的航向角，飞行器便无法进行导航，飞行性能将受到影响。

本发明实施例提供的航向角修正方法，能够在磁数据受干扰或异常时，不完全依赖磁力计来融合，加入GNSS运动修正数据来估算真实航向角用于修正，GNSS只需要简单的一个天线便可完成这个步骤，通过运动迭代计算后输出航向角度，因此，本发明同时利用磁力计传感器和GNSS数据对航向角进行测量，可以得到准确可靠的航向数据，且不怕磁场干扰的存在，对于特种环境的适应性、作业性、稳定性都得到了加强。

为实现上述目的，本发明实施例提供了一种航向角修正方法。

以下结合具体实施例对本发明实施例提供的航向角修正方法的具体实现进行详细描述。

飞行器一般只有在室外飞行时才会用到航向角，用于在机体坐标与导航坐标间的转换，而如果要实现导航飞行，GNSS的定位数据就必不可少。航向角是基于导航坐标的，和GNSS输出的数据处于同一坐标，可以利用飞行器运动过程中GNSS测算出来的速度或加速度来迭代测算飞行器的飞行指向角度，再通过机身加速度测算出基于机身坐标的方向角，两者进行对齐，便可以算出两者角度的偏差，从而利用GNSS的数据进行修正航向角。本发明实施例的实施条件是需要飞行器运动后才能感知到运动加速度，如果出现航向角偏差，则飞行器进行多次机动动作后便可取得修正值，从而保证航向角的准确性。

如图1所示，在本发明的一个实施例中，一种航向角修正方法，所述方法包括：

步骤S100：利用磁力计传感器对飞行器航向角进行测量，得到第一航向角；

具体的，在本发明实施例提供的步骤S100的具体实现中，在飞行器在初始启动时，利用磁力计传感器输出的磁数据进行对准；在所述对准过程中，将磁数据中地磁感应强度数值与设定范围进行比较；当起飞点地磁感应强度在设定范围以内，则以磁力计测量的磁航向作为第一航向角。

步骤S200：通过GNSS单天线输出的速度计算姿态，并根据所述速度和所述姿态计算飞行器的第一加速度；

其中，在本发明实施例提供的步骤S200的具体实现中，对GNSS单天线输出的速度进行差分获得飞行器在导航坐标系下的估计加速度，作为飞行器的第二加速度。

步骤S300：通过飞行器装载的惯性器件计算所述飞行器机身的第二加速度；

其中，在本发明实施例提供的步骤S300的具体实现中，利用姿态矩阵估计值将惯性器件输出的比力信号变换到导航坐标系下，并计算在导航坐标系下飞行器的加速度，作为无人机的第一加速度，其中，n代表导航坐标系；b代表飞行器坐标系。

步骤S400：根据所述第一加速度和所述第二加速度计算所述飞行器与所述GNSS的航向偏差；

其中，在本发明实施例提供的步骤S400的具体实现过程中，利用飞行器运动过程中GNSS测算出来的第一加速度来迭代测算飞行器的飞行指向角度；通过飞行器机身的第二加速度测算出基于机身坐标的方向角；将所述飞行指向角度和所述方向角进行对齐，基于所述飞行指向角度和所述方向角的角度偏差计算飞行器与GNSS的航向偏差；

步骤S500：基于第一航向角和所述航向偏差，对所述飞行器在导航坐标系下的航向角进行修正。

作为本发明优选实施例技术方案的进一步限定，所述利用磁力计传感器对飞行器航向角进行测量，得到第一航向角的步骤S100包括：

步骤S101:飞行器在初始启动时，利用磁力计传感器输出的磁数据进行对准；

步骤S102:在所述对准过程中，将磁数据中地磁感应强度数值与设定范围进行比较；

步骤S103:当起飞点地磁感应强度在设定范围以内，则以磁力计测量的磁航向作为第一航向角。

作为本发明优选实施例技术方案的进一步限定，在所述利用磁力计传感器对飞行器航向角进行测量的步骤之前，所述方法还包括利用磁数据进行对准的步骤，所述利用磁数据进行对准的步骤包括：以磁方位代替地理方位进行方位对准。

其中，在本发明提供的优选实施方式中，飞行器在初始启动时需要利用磁数据进行对准，其中，在所述对准过程中，如果地磁测量信息可用，通常可忽略小的磁偏角影响，直接以磁方位近似代替地理方位进行方位对准。当然，若已知当地磁偏角参数，为提高方位精度可作适当的补偿。

在具体实现中，假设经过加速度计水平对准之后，获得姿态矩阵真实姿态矩阵为它可分解为：

其中，是与方位有关的矩阵，可展开为：

后面将会看到，并不需要知道中符号的具体含义，只需求得其正/余弦值即可。

进一步的，在本发明实施例中，根据地磁场的测量关系：

即：

其中，记归一化地磁矢量

再将式(2)代入展开并只取x和y分量，得到：

由上式可解得：

或者

其中，atan2()为四象限反正切函数。至此，求得了方位校正矩阵再利用式(1)可完成方位对准。

两个矢量的测量转换关系如下：

另外，本发明的另一个优选实施例中，利用卫星导航进行方位对准的实施方式中，完成水平对准之后，如果GNSS导航信号可用，对于飞行器，比如旋翼无人机，其飞行速度方向具有任意性(即可沿载体任意方向飞行)，可以通过在水平方向上作直线加速度机动来实现，基本原理叙述如下。

对比力方程作如下近似和变换

其中，和的含义同式(1)。

加速度机动意味着不为零，它可通过两个时刻的GNSS导航速度差分求得，近似计算为：

相应地，对应于载体在时间段内的水平投影平均比力。

展开式(8)，仅取x和y分量，可得：

其中，记和由上式可解得：

不难看出，式(11)与式(5)在形式上完全一致，水平加速度机动的实质是在水平方向上提供了一个用于确定方位的观测量，这与地磁场的水平观测量作用完全一样。

式(11)的分母表达式显示，较大的水平加速度有利于可靠地求得和同理，在求得之后，再代入即完成姿态初始化。

其中，在本发明提供的优选实施例中，所述根据所述第一加速度和所述第二加速度计算所述飞行器与所述GNSS的航向偏差的步骤S400具体包括：

步骤S401：利用飞行器运动过程中GNSS测算出来的第一加速度来迭代测算飞行器的飞行指向角度；

步骤S402：通过飞行器机身的第二加速度测算出基于机身坐标的方向角；

步骤S403：将所述飞行指向角度和所述方向角进行对齐，基于所述飞行指向角度和所述方向角的角度偏差计算飞行器与GNSS的航向偏差。

其中，在本发明提供的优选实施例中，所述根据所述速度和所述姿态计算飞行器的第一加速度的步骤S200包括：对GNSS单天线输出的速度进行差分获得飞行器在导航坐标系下的估计加速度，作为飞行器的第二加速度。

其中，在本发明提供的优选实施例中，所述通过飞行器装载的惯性器件计算所述飞行器机身的第二加速度的步骤S300包括：利用姿态矩阵估计值将惯性器件输出的比力信号变换到导航坐标系下，并计算在导航坐标系下飞行器的加速度，作为无人机的第一加速度，其中，n代表导航坐标系；b代表飞行器坐标系。

本发明实施例提供的航向修正角修正方法，在磁数据受干扰或异常时，不完全依赖磁力计来融合，加入GNSS运动修正数据来估算真实航向角用于修正，GNSS只需要简单的一个天线便可完成这个步骤，通过运动迭代计算后输出航向角度。

在本发明提供的另一个实施例中，一种航向角修正系统，所述修正系统600包括：

测量模块601，用于利用磁力计传感器对飞行器航向角进行测量，得到第一航向角；

第一计算模块602，用于通过GNSS单天线输出的速度计算姿态，并根据所述速度和所述姿态计算飞行器的第一加速度；

第二计算模块603，用于通过飞行器装载的惯性器件计算所述飞行器机身的第二加速度；

第三计算模块604，用于根据所述第一加速度和所述第二加速度计算所述飞行器与所述GNSS的航向偏差；

修正模块605，用于基于第一航向角和所述航向偏差，对所述飞行器在导航坐标系下的航向角进行修正。

在本发明提供的再一个实施例中，一种计算机设备，本发明实施例提供的计算机设备700可以执行航向角修正方法实施例提供的处理流程。

所述计算机设备700包括存储器701、处理器702、计算机程序；其中，计算机程序存储在存储器701中，并被配置为由处理器702执行时实现所述航向角修正方法的步骤；

所述的航向角修正方法包括以下步骤：

利用磁力计传感器对飞行器航向角进行测量，得到第一航向角；

通过GNSS单天线输出的速度计算姿态，并根据所述速度和所述姿态计算飞行器的第一加速度；

通过飞行器装载的惯性器件计算所述飞行器机身的第二加速度；

根据所述第一加速度和所述第二加速度计算所述飞行器与所述GNSS的航向偏差；

基于第一航向角和所述航向偏差，对所述飞行器在导航坐标系下的航向角进行修正。

进一步的，在本发明提供的实时例中，所述的计算机设备700还可具有通讯接口703，用于接收控制指令。

在本发明提供的又一个实施例中，一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机可读指令，所述计算机可读指令被处理器执行时实现所述航向角修正方法的步骤。

其中，被处理器执行时实现所述航向角修正方法的步骤包括：

利用磁力计传感器对飞行器航向角进行测量，得到第一航向角；

通过GNSS单天线输出的速度计算姿态，并根据所述速度和所述姿态计算飞行器的第一加速度；

通过飞行器装载的惯性器件计算所述飞行器机身的第二加速度；

根据所述第一加速度和所述第二加速度计算所述飞行器与所述GNSS的航向偏差；

基于第一航向角和所述航向偏差，对所述飞行器在导航坐标系下的航向角进行修正。

与现有技术相比，本发明实施例提供的航向角修正方法的技术优势在于，在磁数据受干扰或异常时，不完全依赖磁力计来融合，加入GNSS运动修正数据来估算真实航向角用于修正，GNSS只需要简单的一个天线便可完成这个步骤，通过运动迭代计算后输出航向角度，因此，本发明同时利用磁力计传感器和GNSS数据对航向角进行测量，可以得到准确可靠的航向数据，且不怕磁场干扰的存在，对于特种环境的适应性、作业性、稳定性都得到了加强。

在本发明所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

在本发明实施例的一个典型的配置中，终端、服务网络的设备和计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flashRAM)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。

计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括非暂存电脑可读媒体(transitorymedia)，如调制的数据信号和载波。

本领域技术人员可以清楚地了解到，为描述的方便和简洁，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将装置的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。上述描述的装置的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本公开的其它实施方案。本公开的实施例旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本公开的真正范围和精神由下面的权利要求书指出。

应当理解的是，本公开并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本公开的范围仅由所附的权利要求书来限制。