CN111895988A

CN111895988A - 无人机导航信息更新方法及装置

Info

Publication number: CN111895988A
Application number: CN201911326817.7A
Authority: CN
Inventors: 田方澍; 范宇; 吕竹勇; 姚德清; 吴嘉梁; 杨胜江; 刘超逸; 朱柏羊; 程阳
Original assignee: Beijing Kongtian Technology Research Institute
Current assignee: Beijing Kongtian Technology Research Institute
Priority date: 2019-12-20
Filing date: 2019-12-20
Publication date: 2020-11-06

Abstract

本申请实施例提供一种无人机导航信息更新方法及装置，方法包括：根据预获取的无人机的机体坐标系到北东地系的姿态转移矩阵确定对应的目标四元数；基于处于飞行状态中的所述无人机当前的磁强计测量数据、加速度计测量数据和GPS测量数据对该无人机的陀螺仪测量的角速度数据进行修正；应用修正后的角速度数据对所述目标四元数进行数值更新，并将经数值更新后的目标四元数转换为所述无人机的导航信息中的姿态信息；以及对所述导航信息中的位置和速度信息进行数值更新。本申请能够有效提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率。

Description

无人机导航信息更新方法及装置

技术领域

本申请涉及数据处理技术领域，具体涉及无人机导航信息更新方法及装置。

背景技术

微机电***MEMS已经应用到了航空航天、汽车电子技术等很多领域中，相较于高精度惯性传感器，MEMS传感器具有体积小、重量轻、噪声较高的特点。因此，MEMS传感器尤其适用于小型无人机，且由于MEMS在小小的芯片上可以融合力学、光学、电子、材料、信息技术等一系列学科技术，因此其作为交叉科学的新的研究领域具有极大的研究与应用价值，尤其可以应用MEMS传感器数据获取小型无人机的导航信息。

目前，小型无人机在进行导航解算时，由于所使用的MEMS传感器精度较差，往往无法从单个传感器获取准确的信息，也就是说，如果单独使用陀螺仪积分获得角度则会由于陀螺仪的常值偏差和随机漂移造成很大的误差，如果使用加速度计和磁强计获取姿态信息会由于加速度计中混有线加速度以及磁强计受到干扰，误差也会很大。

发明内容

针对现有技术中的问题，本申请提供一种无人机导航信息更新方法及装置，能够有效提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率。

为解决上述技术问题，本申请提供以下技术方案：

第一方面，本申请提供一种无人机导航信息更新方法，包括：

根据预获取的无人机的机体坐标系到北东地系的姿态转移矩阵确定对应的目标四元数；

基于处于飞行状态中的所述无人机当前的磁强计测量数据、加速度计测量数据和GPS测量数据对该无人机的陀螺仪测量的角速度数据进行修正；

应用修正后的角速度数据对所述目标四元数进行数值更新，并将经数值更新后的目标四元数转换为所述无人机的导航信息中的姿态信息；

以及，对所述导航信息中的位置和速度信息进行数值更新。

进一步地，所述对所述导航信息中的位置和速度信息进行数值更新，包括：

根据处于飞行状态中的所述无人机当前的加速度计测量数据、气压计测量数据和GPS测量数据，以及预设的扩展卡尔曼滤波方式，对所述无人机的所述导航信息中的位置和速度信息进行数值更新。

进一步地，所述根据预获取的无人机的机体坐标系到北东地系的姿态转移矩阵确定对应的目标四元数，包括：

对预获取的所述无人机的姿态转移矩阵进行归一化处理，并基于经归一化处理后的姿态转移矩阵确定对应的初始四元数；

应用用于表示磁偏角的四元数旋转矩阵对所述初始四元数进行修正，得到对应的目标四元数。

进一步地，在所述对预获取的所述无人机的姿态转移矩阵进行归一化处理之前，还包括：

基于所述无人机的加速度计测量数据和磁强计测量数据，获取所述无人机的机体坐标系到北东地系的地方向上的单位矢量、北方向上的单位矢量和东方向上的单位矢量；

应用所述无人机的机体坐标系到北东地系的地方向上的单位矢量、北方向上的单位矢量和东方向上的单位矢量，确定所述无人机的机体坐标系到北东地系的姿态转移矩阵。

进一步地，所述基于所述无人机的加速度计测量数据和磁强计测量数据，获取所述无人机的机体坐标系到北东地系的地方向上的单位矢量、北方向上的单位矢量和东方向上的单位矢量，包括：

对处于静止状态中的所述无人机的加速度计测量数据进行归一化处理，得到该无人机的机体坐标系到北东地系的地方向上的单位矢量；

根据所述地方向上的单位矢量和所述无人机的磁强计测量数据，确定所述无人机的机体坐标系到北东地系的北方向上的单位矢量；

基于所述地方向上的单位矢量和所述北方向上的单位矢量，确定所述无人机的机体坐标系到北东地系的东方向上的单位矢量。

进一步地，所述基于处于飞行状态中的所述无人机当前的磁强计测量数据、加速度计测量数据和GPS测量数据对该无人机的陀螺仪测量的角速度数据进行修正，包括：

基于所述无人机当前的磁强数据获取用于修正航向角的第一修正矢量；

应用所述无人机当前的加速度数据和GPS测量数据获取用于修正俯仰角和横滚角的第二修正矢量；

基于所述第一修正矢量和第二修正矢量确定用于修正所述陀螺仪的漂移量的目标修正矢量；

根据所述目标修正矢量对所述无人机的陀螺仪测量的角速度数据进行修正。

进一步地，所述应用修正后的角速度数据对所述目标四元数进行数值更新，并将经数值更新后的目标四元数转换为所述无人机的导航信息中的姿态信息，包括：

应用修正后的角速度数据对所述目标四元数进行数值更新，得到预设的下一时刻的所述目标四元数的值；所述姿态信息包括：无人机的俯仰角、横滚角和航向角；

基于预设的下一时刻的所述目标四元数的值，以及所述无人机的俯仰角、横滚角和航向角分别与所述目标四元数之间的对应关系，分别确定预设的下一时刻的所述无人机的俯仰角、横滚角和航向角的值。

进一步地，所述扩展卡尔曼滤波方式，包括：

预先建立的无人机运动学状态空间方程、卡尔曼滤波器预测方程和使用GPS 时的无人机的卡尔曼滤波器量测方程；

其中，所述无人机运动学状态空间方程用于表示所述无人机的机体坐标系到北东地系的姿态转移矩阵、所述加速度计测量数据转移到导航坐标系后的值以及状态向量之间的对应关系；

所述状态向量包括：所述无人机在北东地坐标系下以起飞位置为原点的相对位置向量、速度向量和所述无人机的加速度计的漂移向量。

第二方面，本申请提供一种无人机导航信息更新装置，包括：

目标四元数获取模块，用于根据预获取的无人机的机体坐标系到北东地系的姿态转移矩阵确定对应的目标四元数；

角速度修正模块，用于基于处于飞行状态中的所述无人机当前的磁强计测量数据、加速度计测量数据和GPS测量数据对该无人机的陀螺仪测量的角速度数据进行修正；

姿态更新模块，用于应用修正后的角速度数据对所述目标四元数进行数值更新，并将经数值更新后的目标四元数转换为所述无人机的导航信息中的姿态信息；

位置和速度更新模块，用于对所述导航信息中的位置和速度信息进行数值更新。

第三方面，本申请提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现权利要求 1至8任一项所述的无人机导航信息更新方法的步骤。

第四方面，本申请提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现无人机导航信息更新方法的步骤。

由上述技术方案可知，本申请提供的无人机导航信息更新方法及装置，针对如何准确地获取无人机的姿态信息的问题，设计了四元数形式的二阶互补滤波姿态解算算法。根据陀螺仪、加速度计、磁强计频率特性互补的原理，对三个传感器的输出进行数据融合，将陀螺仪作为主要测量元件，利用加速度计和磁强计对其进行修正，获取准确的姿态解算结果。接着，设计了基于扩展卡尔曼滤波器的位置、速度信息解算算法。利用加速度计、气压计、GPS对机体飞行过程中的位置、速度进行测量，并且通过扩展卡尔曼滤波算法对三者的信息进行融合，基于连续-离散时间***，引入加速度计的偏移量作为状态向量，算法简易，提高了解算速度，进而能够对多个传感器采集或测量的数据进行融合处理，有效提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，进而能够有效提高对无人机的航行控制的准确性和可靠性，以保证无人机的运行精确性。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例中的无人机导航信息更新方法的流程示意图。

图2为本申请实施例中的无人机导航信息更新方法中步骤100的具体流程示意图。

图3为本申请实施例中的无人机导航信息更新方法中步骤010和步骤020 的流程示意图。

图4为本申请实施例中的无人机导航信息更新方法中步骤010的具体流程示意图。

图5为本申请实施例中的无人机导航信息更新方法中步骤200的具体流程示意图。

图6为本申请实施例中的无人机导航信息更新方法中步骤300的具体流程示意图。

图7为本申请实施例中的无人机导航信息更新方法中扩展卡尔曼滤波方式的具体内容示意图。

图8为本申请应用实例中的互补滤波算法的***框图。

图9为本申请应用实例中的互补滤波算法实施流程示意图。

图10为本申请应用实例中的扩展卡尔曼滤波位置解算算法实施流程示意图。

图11为本申请实施例中的使用实验数据的姿态角曲线中的PX4与互补滤波解算后的俯仰角曲线示意图。

图12为本申请实施例中的使用实验数据的姿态角曲线中的互补滤波与传感器解算的俯仰角误差示意图。

图13为本申请实施例中的使用实验数据的姿态角曲线中的PX4与互补滤波解算后的横滚角曲线示意图。

图14为本申请实施例中的使用实验数据的姿态角曲线中的互补滤波与传感器解算的横滚角误差示意图。

图15为本申请实施例中的使用实验数据的姿态角曲线中的PX4与互补滤波解算后的偏航角曲线示意图。

图16为本申请实施例中的使用实验数据的姿态角曲线中的互补滤波与传感器解算的偏航角误差示意图。

图17为本申请实施例中的使用实验数据的速度曲线中的PX4与EKF解算后的北向速度曲线示意图。

图18为本申请实施例中的使用实验数据的速度曲线中的EKF与GPS解算的北向速度误差示意图。

图19为本申请实施例中的使用实验数据的速度曲线中的PX4与EKF解算后的东向速度曲线示意图。

图20为本申请实施例中的使用实验数据的速度曲线中的EKF与GPS解算的东向速度误差示意图。

图21为本申请实施例中的使用实验数据的速度曲线中的PX4与EKF解算后的地向速度曲线示意图。

图22为本申请实施例中的使用实验数据的速度曲线中的EKF与GPS解算的地向速度误差示意图。

图23为本申请实施例中的使用实验数据的高度曲线中的PX4与EKF解算后的北向位移曲线示意图。

图24为本申请实施例中的使用实验数据的高度曲线中的EKF与GPS解算的北向位移误差示意图。

图25为本申请实施例中的使用实验数据的高度曲线中的PX4与EKF解算后的东向位移曲线示意图。

图26为本申请实施例中的使用实验数据的高度曲线中的EKF与GPS解算的东向位移误差示意图。

图27为本申请实施例中的使用实验数据的高度曲线中的PX4与EKF解算后的高度曲线示意图。

图28为本申请实施例中的使用实验数据的高度曲线中的EKF与传感器解算的高度误差示意图。

图29为本申请实施例中的无人机导航信息更新装置的结构示意图。

图30为本申请实施例中的电子设备的结构示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

MEMS是微电子机械***的英文简称，1959年美国著名的物理学家RichardPfeynman首先提出了这个概念，一开始的思想是在低精度的生产条件下，生产出高精度的机电产品。随着微机械加工工艺等制造技术的发展，MEMS可以实现在一块微小的芯片上集成包括微机械、微传感器、微执行器等功能单元，可以完成测量、驱动等许多功能。

多传感器数据融合的实质就是把多个传感器的信息进行融合处理，进而得到更加准确可靠的结果。组合导航就是利用了多传感器融合技术，其算法大致可以分为两类，即线性和非线性。线性算法主要有卡尔曼滤波算法、分散化估计理论以及并行卡尔曼技术。非线性算法则是近些年逐渐发展起来的，在实际的工程中，很多***都是非线性的，因此非线性算法的应用范围更广。主要的非线性算法有小波分析、神经网络、模糊控制等。

本申请主要考虑在小型无人机进行导航解算时，由于所使用的MEMS传感器精度较差，往往无法从单个传感器获取准确的信息。本申请基于数据融合思想，利用互补滤波及卡尔曼滤波器，根据不同传感器的自身特性，对多个传感器进行数据融合，从而得到准确的导航信息。具体来说，本申请针对如何准确地获取无人机的姿态信息的问题，设计了四元数形式的二阶互补滤波姿态解算算法。根据陀螺仪、加速度计、磁强计频率特性互补的原理，对三个传感器的输出进行数据融合，将陀螺仪作为主要测量元件，利用加速度计和磁强计对其进行修正，获取准确的姿态解算结果。接着，设计了基于扩展卡尔曼滤波器的位置、速度信息解算算法。利用加速度计、气压计、GPS对机体飞行过程中的位置、速度进行测量，并且通过扩展卡尔曼滤波算法对三者的信息进行融合，基于连续-离散时间***，引入加速度计的偏移量作为状态向量，算法简易，提高了解算速度，进而能够对多个传感器采集或测量的数据进行融合处理，有效提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，进而能够有效提高对无人机的航行控制的准确性和可靠性，以保证无人机的运行精确性。

为了有效提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，本申请提供一种无人机导航信息更新方法的实施例，参见图 1，所述无人机导航信息更新方法具体包含有如下内容：

步骤100：根据预获取的无人机的机体坐标系到北东地系的姿态转移矩阵确定对应的目标四元数。

在步骤100中，无人机导航信息更新装置可以对预获取的所述无人机的姿态转移矩阵进行归一化处理，并基于经归一化处理后的姿态转移矩阵确定对应的初始四元数；应用用于表示磁偏角的四元数旋转矩阵对所述初始四元数进行修正，得到对应的目标四元数。

可以理解的是，所述目标四元数和本申请的一个或多个实施例中提及的初始四元数均为四元数，该四元数是简单的超复数。复数是由实数加上虚数单位组成。四元数是复数的不可交换延伸。如把四元数的集合考虑成多维实数空间的话，四元数就代表着一个四维空间，相对于复数为二维空间。

其中，机体坐标系是指固定在飞行器(无人机)或者飞机上的遵循右手法则的三维正交直角坐标系，其原点位于飞行器的质心，OX轴位于飞行器参考平面内平行于机身轴线并指向飞行器前方，OY轴垂直于飞行器参考面并指向飞行器右方，OZ轴在参考面内垂直于XOY平面，指向飞行器下方。

可以理解的是，所述北东地系是指北东地坐标系NED(North East Down)，也叫做导航坐标系，是在导航时根据导航***工作的需要而选取的用于导航解算的参考坐标系。所述NED坐标系各轴的定义为：N为北轴指向地球北；E为东轴指向地球东；D为地轴垂直于地球表面并指向下。

步骤200：基于处于飞行状态中的所述无人机当前的磁强计测量数据、加速度计测量数据和GPS测量数据对该无人机的陀螺仪测量的角速度数据进行修正。

具体来说，所述无人机导航信息更新装置可以根据应用磁强计修正后的陀螺仪获取准确的航向角，应用加速度计修正后的陀螺仪获取准确的俯仰角和横滚角，确定陀螺仪的角速度修正公式，得到较为准确的陀螺仪输出。

步骤300：应用修正后的角速度数据对所述目标四元数进行数值更新，并将经数值更新后的目标四元数转换为所述无人机的导航信息中的姿态信息。

在步骤300中，所述无人机导航信息更新装置可以应用最终得到的较为准确的陀螺仪输出对目标四元数进行更新，求出下一时刻的四元数值；并将更新后的目标四元数转化为无人机的姿态角，进而完成对无人机的姿态角的精确测量，所述姿态角包括：俯仰角、横滚角和航向角。

步骤400：对所述导航信息中的位置和速度信息进行数值更新。

在步骤400中，所述无人机导航信息更新装置在对姿态信息进行解算及数值更新后，也对所述导航信息中的位置和速度信息进行解算及数值更新，进保证无人机导航信息更新的全面性和可靠性。

为了提高对所述导航信息中的位置和速度信息进行数值更新的准确性，以进一步提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，在本申请的一种无人机导航信息更新方法的实施例中，所述无人机导航信息更新方法中的步骤400具体包含有如下内容：

可以通过设计基于扩展卡尔曼滤波器的位置、速度信息解算算法。利用加速度计、气压计、GPS对机体飞行过程中的位置、速度进行测量，并且通过扩展卡尔曼滤波算法对三者的信息进行融合，基于连续-离散时间***，引入加速度计的偏移量作为状态向量，算法简易，能够有效提高导航信息的解算速度。

为了提高目标四元数的获取准确性，以进一步提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，在本申请的一种无人机导航信息更新方法的实施例中，参见图2，所述无人机导航信息更新方法中的步骤100具体包含有如下内容：

步骤101：对预获取的所述无人机的姿态转移矩阵进行归一化处理，并基于经归一化处理后的姿态转移矩阵确定对应的初始四元数。

步骤102：应用用于表示磁偏角的四元数旋转矩阵对所述初始四元数进行修正，得到对应的目标四元数。

具体来说，所述无人机导航信息更新装置对所述无人机的机体坐标系到北东地系的地方向上的单位矢量k、所述北方向上的单位矢量i和东方向上的单位矢量j进行归一化处理，而后根据所述姿态转移矩阵确定初始四元数，应用用于表示磁偏角的四元数旋转矩阵对初始四元数进行修正，得到对应的目标四元数。

为了提高姿态转移矩阵的应用可靠性，以进一步提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，在本申请的一种无人机导航信息更新方法的实施例中，参见图3，所述无人机导航信息更新方法中的步骤100之前还具体包含有如下内容：

步骤010：基于所述无人机的加速度计测量数据和磁强计测量数据，获取所述无人机的机体坐标系到北东地系的地方向上的单位矢量、北方向上的单位矢量和东方向上的单位矢量。

步骤020：应用所述无人机的机体坐标系到北东地系的地方向上的单位矢量、北方向上的单位矢量和东方向上的单位矢量，确定所述无人机的机体坐标系到北东地系的姿态转移矩阵。

为了姿态转移矩阵的获取准确性，以进一步提高姿态转移矩阵的应用可靠性，并进一步提高无人机的导航信息的获取准确性和效率，在本申请的一种无人机导航信息更新方法的实施例中，参见图4，所述无人机导航信息更新方法中的步骤010具体包含有如下内容：

步骤011：对处于静止状态中的所述无人机的加速度计测量数据进行归一化处理，得到该无人机的机体坐标系到北东地系的地方向上的单位矢量。

步骤012：根据所述地方向上的单位矢量和所述无人机的磁强计测量数据，确定所述无人机的机体坐标系到北东地系的北方向上的单位矢量。

步骤013：基于所述地方向上的单位矢量和所述北方向上的单位矢量，确定所述无人机的机体坐标系到北东地系的东方向上的单位矢量。

具体来说，对无人机的加速度计在机体坐标系下的测量值进行归一化处理，得到该无人机的机体坐标系到北东地系的地方向上的单位矢量(竖直向下的单位矢量)k；根据所述地方向上的单位矢量k，以及所述无人机的磁强计的读数，确定所述无人机的机体坐标系到北东地系的北方向上的单位矢量i；基于所述地方向上的单位矢量k和所述北方向上的单位矢量i，确定所述无人机的机体坐标系到北东地系的东方向上的单位矢量j；应用所述无人机的机体坐标系到北东地系的地方向上的单位矢量k、所述北方向上的单位矢量i和东方向上的单位矢量 j获取所述无人机的机体坐标系到北东地系的姿态转移矩阵。

为了提高对陀螺仪测量的角速度数据进行修正的准确性和修正效率，以进一步提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，在本申请的一种无人机导航信息更新方法的实施例中，参见图5，所述无人机导航信息更新方法中的步骤200具体包含有如下内容：

步骤201：基于所述无人机当前的磁强数据获取用于修正航向角的第一修正矢量；

步骤202：应用所述无人机当前的加速度数据和GPS测量数据获取用于修正俯仰角和横滚角的第二修正矢量；

步骤203：基于所述第一修正矢量和第二修正矢量确定用于修正所述陀螺仪的漂移量的目标修正矢量；

步骤204：根据所述目标修正矢量对所述无人机的陀螺仪测量的角速度数据进行修正。

具体来说，所述无人机导航信息更新装置可以将磁强计的读数转换到北东地坐标系中，得到无人机对应的导航坐标系下的示数；应用所述目标四元数表示转换矩阵

进而得到所述姿态转移矩阵

的所述目标四元数的表达式；应用估算出的磁偏角与实际磁偏角之间的差值确定所述姿态转移矩阵

的误差值；根据预设的所述磁强计的权重、所述姿态转移矩阵

的误差值和所述姿态转移矩阵

的所述目标四元数的表达式，得到修正矢量K_m；应用所述修正矢量K_m修正所述无人机的陀螺仪测得的航向角。

而后应用加速度计修正陀螺仪获取准确的俯仰角和横滚角，即：将重力加速度方向上的单位矢量通过姿态转移矩阵

转换到体坐标系中，得到该重力加速度方向上的单位矢量在体坐标系中的投影e₁；应用GPS记录无人机在测量周期为Δt的时间段中的速度变化v₁-v₀，得到无人机在北东地下的运动线加速度a_p，再将a_p转换到体坐标系下；通过加速度计的实测加速度与线加速度

的和，得到估算到的重力加速度g_p；对重力加速度g_p归一化后得到竖直向下的单位向量 e₂；通过加速度计和磁强计确定用于对误差进行修正的修正矢量K；根据修正矢量K来修正陀螺仪的修正俯仰角和横滚角。

为了进一步提高无人机的导航信息中的姿态信息的更新准确性和效率，在本申请的一种无人机导航信息更新方法的实施例中，参见图6，所述无人机导航信息更新方法中的步骤300具体包含有如下内容：

步骤301：应用修正后的角速度数据对所述目标四元数进行数值更新，得到预设的下一时刻的所述目标四元数的值；所述姿态信息包括：无人机的俯仰角、横滚角和航向角。

步骤302：基于预设的下一时刻的所述目标四元数的值，以及所述无人机的俯仰角、横滚角和航向角分别与所述目标四元数之间的对应关系，分别确定预设的下一时刻的所述无人机的俯仰角、横滚角和航向角的值。

为了进一步提高对所述导航信息中的位置和速度信息进行数值更新的效率和准确性，在本申请的一种无人机导航信息更新方法的实施例中，参见图7，所述无人机导航信息更新方法中的步骤400中的所述扩展卡尔曼滤波方式具体包含有如下内容：

预先建立的无人机运动学状态空间方程、卡尔曼滤波器预测方程和使用GPS 时的无人机的卡尔曼滤波器量测方程；其中，所述无人机运动学状态空间方程用于表示所述无人机的机体坐标系到北东地系的姿态转移矩阵、所述加速度计测量数据转移到导航坐标系后的值以及状态向量之间的对应关系；所述状态向量包括：所述无人机在北东地坐标系下以起飞位置为原点的相对位置向量、速度向量和所述无人机的加速度计的漂移向量。

具体来说，所述无人机导航信息更新装置利用加速度计、气压计、GPS对机体飞行过程中的位置、速度进行测量，并且通过扩展卡尔曼滤波算法对三者的信息进行融合，基于连续-离散时间***，引入加速度计的偏移量作为状态向量。

(1)建立无人机运动学状态空间方程

所述无人机运动学状态空间方程用于表示该无人机的机体坐标系到导航坐标系的姿态转换矩阵

加速度计的测量值转移到导航坐标系后的值 [a_bx a_by a_bz]^T与状态向量中各向量之间的对应关系。

状态向量包括：机体在北东地坐标系下以起飞位置为原点的相对位置向量、机体北东地坐标系下的速度向量和加速度计的漂移向量。

(2)建立卡尔曼滤波器预测方程。

(3)建立使用GPS时的无人机***的卡尔曼滤波器量测方程。

为了进一步说明本方案，本申请还提供一种无人机导航信息更新方法的具体应用实例，首先设计了四元数形式的二阶互补滤波姿态解算算法。根据陀螺仪、加速度计、磁强计频率特性互补的原理，对三个传感器的输出进行数据融合，将陀螺仪作为主要测量元件，利用加速度计和磁强计对其进行修正，获取准确的姿态解算结果，该无人机导航信息更新方法的具体应用实例具体包含有如下内容：

第一步，当无人机静止时，第一次对无人机进行姿态测量。

设

为机体坐标系到北东地系的姿态转移矩阵，

为北东地系到机体坐标系的姿态转移矩阵，a＝[a_x a_y a_z]^T为加速度计在机体坐标系下的测量值，则在初始条件下有：

设i、j、k分别是姿态转移矩阵

的3个列向量，由方向余弦矩阵的相关知识可知，i、j、k分别是北东地坐标系中北、东、地三个方向上的单位向量在机体坐标系上的投影。因为k是竖直向下的单位矢量，而重力加速度与北东地系固联也是竖直向下的，而初始时加速度计的度数还均由重力加速度提供，所以有：

即将加速度计的初始值归一化后取负值就是k的值，这里取负值是由于加速度计的输出数值此时为正值。

第二步，获取磁强记读数并加以修正。

设磁强计的读数为b＝[b_x b_y b_z]^T，当不考虑磁偏角时，地磁场应该是一个只与北、地方向有夹角的向量，通过将已求出的竖直向下的单位向量k与地磁矢量b进行施密特正交化，可得到一个只指向北的单位矢量。该单位矢量就是导航坐标系中北向在机体坐标系中的投影，因此由方向余弦矩阵的知识可知该单位矢量即为i：

由旋转矩阵的性质，由已知的两个列向量进行叉乘可得到j：

j＝k×i (4)

由此可得初始的旋转矩阵

为了保证姿态矩阵

严格的正交性，在每次求出i、j、k后都要进行归一化处理，在得到了准确的姿态矩阵后，便可以获得初始四元数

若矩阵

的形式如下：

则四元数q'可由式(7)得到：

当然此时得到的四元数的值没有考虑到磁偏角的影响，即现在的导航坐标系的北向是沿着地磁北向的而不是地理的北极。利用四元数表示旋转的性质，以及四元数乘法的意义，在初始四元数左边乘上一个表示磁偏角的四元数旋转矩阵，即可得到更为准确的初始四元数了。设磁偏角为τ_d，利用四元数的定义就可以得到表示磁偏角误差的四元数q_τ：

式(8)中，由于补偿磁偏角误差的转动是在机体坐标系中进行的，所以等效旋转轴就是机体坐标系的Z轴。再将与初始四元数相乘就可以得到修正后的四元数，如式(9)所示：

第三步，使用磁强计修正陀螺仪获取准确的航向角。

首先将磁力计的读数b转换到北东地坐标系中，得到在导航坐标系下的示数 b_n：

利用上一步求得的四元数q来表示转换矩阵

在得到北东地系中的磁强计读数后，利用x与y方向磁力计读数的反正切函数即可估算出的磁偏角，用这个估算出的磁偏角减去实际的磁偏角τ_d，如果传感器及姿态矩阵不存在误差，那么这个值应该为零，但实际存在误差，所以用这个得到的差值ε_m就可以来衡量传感器及姿态矩阵的误差：

式(12)中b_nx、b_ny分别表示磁力计读数在北东地坐标系中的北向、东向分量。这个得到的误差角实际上就是一个以竖直方向为轴的旋转角，它与地平面是平行的，因此只能修正航向角。

由于使用该误差角来修正陀螺仪的读数，陀螺仪的读数都是在体坐标系中进行的，所以还需要再将误差转换到体坐标系中：

其中：

这样就可以通过

的值去修正陀螺仪测得的航向角，但是由于运用的是二阶互补滤波，需要引入PI环节，因此还要给

乘上一个系数w_m，把该系数称为磁强计的权重，其实w_m就是PI环节中的比例环节k_p，以传感器的权重的形式引入便于直观的理解，利用修正矢量K_m就可以来修正陀螺仪。其中，互补滤波算法的***框图参见图8，互补滤波算法实施流程参见图9。

第四步，使用加速度计修正陀螺仪获取准确的俯仰角和横滚角。

将重力加速度方向上的单位矢量通过旋转矩阵

转换到体坐标系中，可以得到其在体坐标系中的投影e₁，其方向理论上应仍为竖直向下的。

由加速度计测量原理可知，加速度计的测量值等于运动线加速度与重力加速度的矢量和，将加速度计测量到的值a减去机体的运动加速度值就可以得到重力加速度。这里的运动加速度是通过GPS得到的，通过GPS记录在测量周期为△t 的时间段中的速度变化v₁-v₀，再除以△t，就可以得到在北东地下的运动线加速度a_p，再将其转换到体坐标系下：

考虑到加速度的方向问题，通过加速度计的实测加速度与线加速度

的和就可以得到估算到的重力加速度g_p：

得到的加速度向量g_p理论上应该就是重力加速度，将其归一化后得到理论上竖直向下的单位向量e₂，理论上与上一步得到的同为竖直向下的单位向量e₁应该是重合的，但是因为加速度计和姿态矩阵存在误差，造成了两者实际上并不重合。两个矢量的误差可以通过矢量的叉乘来表示。将理论的重力方向与实际的方向叉乘，得到误差：

由于矢量e₁、e₂是单位向量，误差

在数值上实际上就是两个向量的夹角的正弦值，向量误差大则正弦值就大，向量误差小则正弦值也减小，当这个角度很小时正弦值可以约等于这个角度所以单位矢量的叉乘可以很好的表示矢量间的误差。因此可以用

来修正俯仰角和横滚角，且两个向量都是平行于竖直方向的，不能修正航向角。与磁力计修正时采用的方式相同，将误差角

再将误差值乘上加速度计的权重w_a就可以得到修正矢量K_g：

最终的修正矢量K：

这样我们就可以得到通过加速度计和磁强计对误差进行修正的修正矢量 K，通过修正矢量来修正陀螺仪的漂移量：

ω_bias＝∫K·w_gbdt＝K·w_gb·dt (22)

式(22)中ω_bias为陀螺仪角速度测量的偏移量，w_gb是陀螺仪偏移量的权重，积分时间dt即为陀螺仪的测量周期，在计算机中可以由程序中的计时器得到。

第五步，四元数的更新。

设ω_m为陀螺仪的输出值，则最终得到的角速度ω为：

由式(23)可知，该算法的原理就是将误差通过PI环节对陀螺仪的输出不断进行修正，其中w_m、w_a就相当于PI环节中比例环节的系数k_p，w_m·w_gb、w_a·w_gb就相当于积分环节的系数k_i。

将最终得到的较为准确的陀螺仪输出ω用来对四元数进行更新，由于机体在飞行中是不断运动的，机体的姿态也会随着时间不断变化，因此四元数也是一种关于时间的变量。当测量周期△t很小时：

由于已经求得较为准确的角速度值ω＝[ω_x ω_y ω_z]^T以及四元数 q＝[q₀ q₁ q₂q₃]^T，那么根据四元数的微分定理我们就可以求出式(24)中的

即：

这样就可以求出下一时刻的四元数值q(t+△t)，完成了对四元数的更新，该四元数可用于下一测量周期对四元数的更新。需要说明的是由于各种运算都可能使四元数的值出现“溢出”的现象即四元数模长大于1，因此每一次得到新的四元数后都要对四元数进行归一化处理，这样也保证了姿态矩阵的正交性。

最后将更新后的四元数根据式(26)转化为姿态角，以达到了对姿态角的精确测量的目的，式中θ、

ψ分别是无人机的三个姿态角，依次为：俯仰角、横滚角、航向角。

接着，参见图10，设计了基于扩展卡尔曼滤波器的位置、速度信息解算算法。利用加速度计、气压计、GPS对机体飞行过程中的位置、速度进行测量，并且通过扩展卡尔曼滤波算法对三者的信息进行融合，基于连续-离散时间***，引入加速度计的偏移量作为状态向量，算法简易，提高了解算速度。

具体如下：

第一步，建立无人机运动学状态空间方程。

状态向量X不仅要包括最后求解的位置与速度信息，还应该包括我们不需要但是过程中却必不可少的变量。本算法中取状态向量为：

X＝[x y z v_n v_e v_d b_x b_y b_z]^T (27)

其中[x y z]^T为机体在北东地坐标系下以起飞位置为原点的相对位置， [v_n v_ev_d]^T为机体北东地坐标系下的速度，[b_x b_y b_z]^T为加速度计的漂移。

由运动学的知识可知：

式(28)中

为机体坐标系到导航坐标系的转移矩阵，由姿态解算过程可以获得，[a_bx a_by a_bz]^T是加速度计的测量值转移到导航坐标系后的值，综合式 (28)可以得到***的状态空间方程为：

其中

为姿态矩阵

[a_nx a_ny a_nz]^T则相当于***的输入控制量，其具体表达式为：

式中[a_x a_y a_z]^T是加速度计的测量值，[r_ax r_ay r_az]^T是加速度计在机体坐标系的三个轴向上的测量噪声，该值一般可由加速度计的说明书中得到。 [0 0 g]^T为重力加速度，由于加速度计测量值中包含重力加速度，要获得运动加速度就需要补偿掉重力加速度，由于在北东地坐标系下重力加速度方向为正，所以要加上重力加速度。W(t)是***的过程噪声矩阵，不必具体了解噪声矩阵的值，但需要了解其协方差矩阵Q(t)的具体形式：

w_p、w_v、w_b分别是***的位置噪音密度、速度噪音密度和加速度计漂移噪音密度，其具体的值可以由工程经验和调试得到。

第二步，建立卡尔曼滤波器预测方程。

通过对***方程的构建可以看出***的状态方程是连续形式的，但由于量测方程是离散的，所以本申请设计的卡尔曼滤波算法比较特殊，它是一种连续-离散形式的扩展卡尔曼滤波(Continuous-Discrete Kalman Filter,CDEKF),该算法通过在线积分来对状态量和估计误差协方差进行预测，在采样时刻利用测量值再对预测数据进行更新。滤波器的原型公式如下：

Z(k)＝H(k)X(k)+V(k),V(t)～N(0,R(k))

设采样周期为h，则在kh(k＝1,2,3.....)处获取量测数据，在两次测量 [(h-1)k,hk]之间，状态变量以及误差协方差的预测方程满足：

由于该过程在计算机中其实也是离散的，所以为了便于理解将上式改写为离散形式：

如果在相邻两次的数据采集时间间隔内对上式进行积分来获取预测值，计算方式比较复杂，会影响解算的实时性，考虑到计算机的运算特点，在此采用四阶龙格库塔法(Runge-Kutta)对式(35)、(36)进行积分，具体方法如下：

估计误差协方差P(k|k-1)也可以由式(37)的形式求出，这里不再赘述。误差协方差矩阵的初值P(0|0)一般是由工程经验给出，需要对初始状况下的滤波误差进行一个大概的估计，初值P(0|0)越接近真实误差那么滤波的效果就越好。但由于误差协方差矩阵会随着滤波的过程不断地被迭代更新来接近真实误差，所以只要在一定的范围内给定初值即可，并不需要特别精确。

第三步，建立卡尔曼滤波器量测方程。

当使用GPS时，***的量测方程如下：

其中V_gps(k)为GPS的观测噪声矩阵，其噪声协方差矩阵为：

var_xy、var_z、var_vx、var_vy、var_vz表示的是GPS在测量位置、速度信息上的固有偏差，这个可以在GPS的使用手册中得到。

对于GPS虽然可以通过修正得到高度z，但是由于GPS在测量高度上误差很大，所以求出的这个z并没有用，真正获得的高度是由气压计修正得到的。当使用气压计测量高度时，***的量测方程为：

由于气压计测量到的是绝对高度，其输出值为正值，与北东地坐标系地向方向相反，因此测量值前要取负号。但是气压计对高度的测量精度是比较低的，其误差精度接近1m，得到量测数据后就可以在kh(k＝1,2,3.....)处对***的预测数据进行更新从而获得最优估计值，其更新过程与一般的线性卡尔曼滤波无异：

Kg(k)＝P(k|k-1)H^T(HP(k|k-1)H^T+R)^-1 (41)

P(k|k)＝(I-Kg(k)H)P(k|k-1) (42)

从上述描述可知，本申请应用实例提供的无人机导航信息更新方法，在短时间内对于角度的测量是以陀螺仪为主的，利用加速度计、磁强计长期测量结果比较准确的特点，不断地去修正陀螺仪的测量结果，同时引入GPS对机体运动加速度进行测量，使得在无人机在加速运动状态下加速度计仍可以进行修正。设计了一种基于连续-离散时间***的扩展卡尔曼滤波算法。为了避免对雅克比矩阵的计算，采用了四阶龙格库塔法来对积分运算进行求解，减少了计算量，对速度和水平位移解算的精度都很高。

依据图9和图10所示的实施流程，采集实际的飞行数据来对算法进行验证，以无人机自身飞控***的解算结果作为标准的姿态曲线进行对比，本申请验证使用的无人机搭载的是PX4飞控***。

设置解算频率为250Hz，陀螺仪、加速度计的更新频率为1000Hz，磁强计更新频率为200Hz；无人机初始姿态角

θ＝0°，ψ＝-80°，磁偏角为7°；仿真时间为300s。经调试w_a＝0.2、w_m＝0.1、w_gb＝0.1，得到姿态角曲线图如图11至图16所示，当无人机的姿态角变化不是非常快的时候，互补滤波算法可以对姿态角进行很好的跟踪，俯仰角和横滚角的误差均小于2°，且滤波融合后的角度误差要明显小于单独使用传感器进行解算时的误差，因此可以说明互补滤波算法对姿态角的解算具有比较高的精度。但是由图15和图16可以看出，在无人机做连续转弯的运动时，偏航角的误差明显要大一些，偏航角误差最大接近3°，此时主要是由陀螺仪进行测姿，由于其本身存在一定的误差，故解算误差相应的也会稍大一些。

接着对位置和速度解算算法进行验证，飞控解算频率为250Hz，GPS更新频率为10Hz，气压计更新频率为500Hz，无人机起始的位移和速度均为零，实际飞行时间为1600s，仿真时间为前300s。

卡尔曼滤波器相关参数初始设置：

初始状态向量即控制向量：X＝0，[a_nx a_ny a_nz]^T＝0；加速度计的测量噪声密度为：[r_ax r_ay r_az]^T＝[1.44×10^-4 1.44×10^-4 4×10^-4]^T；初始状态协方差矩阵：P(0|0)＝diag{4,4,4,1,1,1，10^-6，10^-6，10^-6}；过程噪声协方差矩阵： Q＝diag{0.1²,0.1²,0.1²,0.1²,0.1²,0.1²,0.001²,0.001²,0.001²}；GPS与气压计的量测噪声协方差矩阵：R_gps＝diag{1²,1²,3²,0.25²,0.25²,0.25²}，R_baro＝1²。

根据所设计的参数，经过卡尔曼滤波器后的无人机位置与速度的曲线如图 17至图28所示，可以看到，卡尔曼滤波算法对对机体的位置、速度变化跟踪效果非常的好，不管是速度基本不变的情况下还是速度变化比较快的情况，三维的速度误差基本不会超过0.2m/s，误差也小于单独使用GPS进行速度测量的误差，说明本申请设计的卡尔曼滤波器可以对机体的速度信息进行稳定和准确的测量，有效地抑制了传感器测量噪声对测量结果的影响。

为了有效提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，本申请提供一种用于实现无人机导航信息更新方法中全部或部分内容的无人机导航信息更新装置的实施例，参见图29，所述无人机导航信息更新装置具体包含有如下内容：

目标四元数获取模块10，用于根据预获取的无人机的机体坐标系到北东地系的姿态转移矩阵确定对应的目标四元数；

角速度修正模块20，用于基于处于飞行状态中的所述无人机当前的磁强计测量数据、加速度计测量数据和GPS测量数据对该无人机的陀螺仪测量的角速度数据进行修正；

姿态更新模块30，用于应用修正后的角速度数据对所述目标四元数进行数值更新，并将经数值更新后的目标四元数转换为所述无人机的导航信息中的姿态信息；

位置和速度更新模块40，用于对所述导航信息中的位置和速度信息进行数值更新。

本说明书提供的无人机导航信息更新装置的实施例具体可以用于执行上述无人机导航信息更新方法的实施例的处理流程，其功能在此不再赘述，可以参照上述无人机导航信息更新方法实施例的详细描述。

从上述描述可知，本申请实施例提供的无人机导航信息更新装置，针对如何准确地获取无人机的姿态信息的问题，设计了四元数形式的二阶互补滤波姿态解算算法。根据陀螺仪、加速度计、磁强计频率特性互补的原理，对三个传感器的输出进行数据融合，将陀螺仪作为主要测量元件，利用加速度计和磁强计对其进行修正，获取准确的姿态解算结果。接着，设计了基于扩展卡尔曼滤波器的位置、速度信息解算算法。利用加速度计、气压计、GPS对机体飞行过程中的位置、速度进行测量，并且通过扩展卡尔曼滤波算法对三者的信息进行融合，基于连续- 离散时间***，引入加速度计的偏移量作为状态向量，算法简易，提高了解算速度，进而能够对多个传感器采集或测量的数据进行融合处理，有效提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，进而能够有效提高对无人机的航行控制的准确性和可靠性，以保证无人机的运行精确性。

从硬件层面来说，为了提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，本申请提供一种用于实现所述无人机导航信息更新方法中的全部或部分内容的电子设备的实施例，所述电子设备具体包含有如下内容：

处理器(processor)、存储器(memory)、通信接口(Communications Interface)和总线；其中，所述处理器、存储器、通信接口通过所述总线完成相互间的通信；所述通信接口用于实现无人机导航信息更新装置与各类数据库、MEMS传感器、控制中心以及用户终端等相关设备之间的信息传输；该电子设备可以是台式计算机、平板电脑及移动终端等，本实施例不限于此。在本实施例中，该电子设备可以参照实施例中的无人机导航信息更新方法的实施例，以及，无人机导航信息更新装置的实施例进行实施，其内容被合并于此，重复之处不再赘述。

图30为本申请实施例的电子设备9600的***构成的示意框图。如图30所示，该电子设备9600可以包括中央处理器9100和存储器9140；存储器9140耦合到中央处理器9100。值得注意的是，该图30是示例性的；还可以使用其他类型的结构，来补充或代替该结构，以实现电信功能或其他功能。

一实施例中，无人机导航信息更新功能可以被集成到中央处理器9100中。其中，中央处理器9100可以被配置为进行如下控制：

从上述描述可知，本申请实施例提供的电子设备，针对如何准确地获取无人机的姿态信息的问题，设计了四元数形式的二阶互补滤波姿态解算算法。根据陀螺仪、加速度计、磁强计频率特性互补的原理，对三个传感器的输出进行数据融合，将陀螺仪作为主要测量元件，利用加速度计和磁强计对其进行修正，获取准确的姿态解算结果。接着，设计了基于扩展卡尔曼滤波器的位置、速度信息解算算法。利用加速度计、气压计、GPS对机体飞行过程中的位置、速度进行测量，并且通过扩展卡尔曼滤波算法对三者的信息进行融合，基于连续-离散时间***，引入加速度计的偏移量作为状态向量，算法简易，提高了解算速度，进而能够对多个传感器采集或测量的数据进行融合处理，有效提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，进而能够有效提高对无人机的航行控制的准确性和可靠性，以保证无人机的运行精确性。

在另一个实施方式中，无人机导航信息更新装置可以与中央处理器9100分开配置，例如可以将无人机导航信息更新装置配置为与中央处理器9100连接的芯片，通过中央处理器的控制来实现无人机导航信息更新功能。

如图30所示，该电子设备9600还可以包括：通信模块9110、输入单元9120、音频处理器9130、显示器9160、电源9170。值得注意的是，电子设备9600也并不是必须要包括图30中所示的所有部件；此外，电子设备9600还可以包括图 30中没有示出的部件，可以参考现有技术。

如图30所示，中央处理器9100有时也称为控制器或操作控件，可以包括微处理器或其他处理器装置和/或逻辑装置，该中央处理器9100接收输入并控制电子设备9600的各个部件的操作。

其中，存储器9140，例如可以是缓存器、闪存、硬驱、可移动介质、易失性存储器、非易失性存储器或其它合适装置中的一种或更多种。可储存上述与失败有关的信息，此外还可存储执行有关信息的程序。并且中央处理器9100可执行该存储器9140存储的该程序，以实现信息存储或处理等。

输入单元9120向中央处理器9100提供输入。该输入单元9120例如为按键或触摸输入装置。电源9170用于向电子设备9600提供电力。显示器9160用于进行图像和文字等显示对象的显示。该显示器例如可为LCD显示器，但并不限于此。

该存储器9140可以是固态存储器，例如，只读存储器(ROM)、随机存取存储器(RAM)、SIM卡等。还可以是这样的存储器，其即使在断电时也保存信息，可被选择性地擦除且设有更多数据，该存储器的示例有时被称为EPROM等。存储器9140还可以是某种其它类型的装置。存储器9140包括缓冲存储器9141 (有时被称为缓冲器)。存储器9140可以包括应用/功能存储部9142，该应用/ 功能存储部9142用于存储应用程序和功能程序或用于通过中央处理器9100执行电子设备9600的操作的流程。

存储器9140还可以包括数据存储部9143，该数据存储部9143用于存储数据，例如联系人、数字数据、图片、声音和/或任何其他由电子设备使用的数据。存储器9140的驱动程序存储部9144可以包括电子设备的用于通信功能和/或用于执行电子设备的其他功能(如消息传送应用、通讯录应用等)的各种驱动程序。

通信模块9110即为经由天线9111发送和接收信号的发送机/接收机9110。通信模块(发送机/接收机)9110耦合到中央处理器9100，以提供输入信号和接收输出信号，这可以和常规移动通信终端的情况相同。

基于不同的通信技术，在同一电子设备中，可以设置有多个通信模块9110，如蜂窝网络模块、蓝牙模块和/或无线局域网模块等。通信模块(发送机/接收机) 9110还经由音频处理器9130耦合到扬声器9131和麦克风9132，以经由扬声器 9131提供音频输出，并接收来自麦克风9132的音频输入，从而实现通常的电信功能。音频处理器9130可以包括任何合适的缓冲器、解码器、放大器等。另外，音频处理器9130还耦合到中央处理器9100，从而使得可以通过麦克风9132能够在本机上录音，且使得可以通过扬声器9131来播放本机上存储的声音。

本申请的实施例还提供能够实现上述实施例中的无人机导航信息更新方法中全部步骤的一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现上述实施例中的执行主体为服务器或客户端的无人机导航信息更新方法的全部步骤，例如，所述处理器执行所述计算机程序时实现下述步骤：

从上述描述可知，本申请实施例提供的计算机可读存储介质，针对如何准确地获取无人机的姿态信息的问题，设计了四元数形式的二阶互补滤波姿态解算算法。根据陀螺仪、加速度计、磁强计频率特性互补的原理，对三个传感器的输出进行数据融合，将陀螺仪作为主要测量元件，利用加速度计和磁强计对其进行修正，获取准确的姿态解算结果。接着，设计了基于扩展卡尔曼滤波器的位置、速度信息解算算法。利用加速度计、气压计、GPS对机体飞行过程中的位置、速度进行测量，并且通过扩展卡尔曼滤波算法对三者的信息进行融合，基于连续-离散时间***，引入加速度计的偏移量作为状态向量，算法简易，提高了解算速度，进而能够对多个传感器采集或测量的数据进行融合处理，有效提高无人机的导航信息的获取准确性，并能够有效提高无人机的导航信息的获取效率，进而能够有效提高对无人机的航行控制的准确性和可靠性，以保证无人机的运行精确性。

本领域内的技术人员应明白，本发明的实施例可提供为方法、装置、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(装置)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和 /或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims

1.一种无人机导航信息更新方法，其特征在于，包括：

以及，对所述导航信息中的位置和速度信息进行数值更新。

2.根据权利要求1所述的无人机导航信息更新方法，其特征在于，所述对所述导航信息中的位置和速度信息进行数值更新，包括：

3.根据权利要求1所述的无人机导航信息更新方法，其特征在于，所述根据预获取的无人机的机体坐标系到北东地系的姿态转移矩阵确定对应的目标四元数，包括：

4.根据权利要求3所述的无人机导航信息更新方法，其特征在于，在所述对预获取的所述无人机的姿态转移矩阵进行归一化处理之前，还包括：

5.根据权利要求4所述的无人机导航信息更新方法，其特征在于，所述基于所述无人机的加速度计测量数据和磁强计测量数据，获取所述无人机的机体坐标系到北东地系的地方向上的单位矢量、北方向上的单位矢量和东方向上的单位矢量，包括：

6.根据权利要求1所述的无人机导航信息更新方法，其特征在于，所述基于处于飞行状态中的所述无人机当前的磁强计测量数据、加速度计测量数据和GPS测量数据对该无人机的陀螺仪测量的角速度数据进行修正，包括：

7.根据权利要求1所述的无人机导航信息更新方法，其特征在于，所述应用修正后的角速度数据对所述目标四元数进行数值更新，并将经数值更新后的目标四元数转换为所述无人机的导航信息中的姿态信息，包括：

8.根据权利要求2所述的无人机导航信息更新方法，其特征在于，所述扩展卡尔曼滤波方式，包括：

预先建立的无人机运动学状态空间方程、卡尔曼滤波器预测方程和使用GPS时的无人机的卡尔曼滤波器量测方程；

9.一种无人机导航信息更新装置，其特征在于，包括：

10.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1至8任一项所述的无人机导航信息更新方法的步骤。