CN107576977B - 基于多源信息自适应融合的无人机导航系统及方法 - Google Patents

Abstract

本发明提供了一种基于多源信息自适应融合的无人机导航系统及方法，其中的方法包括利用多个传感器检测无人机多个参考位置信息；计算每个传感器检测的参考位置信息与估计位置信息之间的差值，得到位置误差。针对同一方向对应有多个传感器的情况，自适应分配出该方向上每个传感器的信息融合权重，对每个传感器在该方向上的位置误差按照权重进行加权求和得到该方向上的位置总误差；基于位置误差或位置总误差，利用速度‑位移预测模型和三阶锁相环滤波器计算当前时刻估计的速度信息及估计位置信息。该方法既能够快速准确地从外部多源位置信息中估计出最优位置和速度，同时又减少了单个传感器信息突变带来的定位误差，提升了无人机跟踪的精确性。

Description

基于多源信息自适应融合的无人机导航系统及方法

技术领域

本发明涉及无人机导航技术领域，特别是涉及基于多源信息自适应融合的无人机导航系统及方法。

背景技术

不管是多旋翼无人机还是固定翼无人机的控制系统都需要提供高精度而且实时的位置、速度及姿态估计，高精度测量是高精度控制的前提，实时性则是为了控制性能的需要。

由于低成本的要求，惯性测量单元大多采用微机械陀螺与微机械加速度计。由于器件的精度较差，导致导航误差会很快发散。为了抑制误差的增长，通常利用惯性测量单元以外的绝对测量方法对惯性导航的误差进行校正，如3轴磁力计，气压高度计，卫星导航接收机，其特点是误差不随时间发散。

随着技术的进步，出现了差分方式的高精度相对定位技术，比如RTK—(Real-timekinematic)载波相位差分技术，可以得到厘米级的相对定位精度。但是遇到可靠性稍微差些的RTK接收机也有可能出现高程突变的情况，也有可能在RTK基站与RTK移动站之间的数传电台出现问题，导致RTK移动站的解算出现错误的情况。所以，一般会采取使用移动站接收机输出的单点定位信息进行定位，起到冗余备份的作用。

另外，在高程方向，也有同时采用气压高度计，超声波高度计，雷达高度计的备份方式，来进一步在高程方向进行数据冗余处理。

针对上述多源信息融合的问题，一般采取固定权重的方法进行处理。以高程方向的数据融合为例，飞控软件中的位置估计模块一般采取固定权重的做法，即GPS的权重为0.005，气压高度计的权重为0.5。上述固定权重带来的问题比较明显：在飞行期间，由于气压高度计的精度变差，导致无人机掉高现象非常严重。

发明内容

鉴于上述问题，本发明的一个目的是要提供一种克服上述问题或者至少部分地解决上述问题的基于多源信息自适应融合的无人机导航系统及方法。

本发明一个进一步的目的是提高无人机速度和位置的跟踪精度和减少单个传感器信息突变带来的定位误差。

根据本发明的一个方面，本发明提供了一种基于多源信息自适应融合的无人机导航方法，包括：

利用多个传感器分别检测的当前时刻无人机在X轴、Y轴、Z轴三个方向上的多个参考位置信息，X轴方向、Y轴方向、Z轴方向均对应有一个或多个传感器，且X轴、Y轴、Z轴三个方向中至少一个方向对应有多个传感器；

分别计算X轴、Y轴、Z轴三个方向上每个传感器检测的参考位置信息与估计位置信息之间的差值，得到X轴、Y轴、Z轴三个方向上每个传感器检测的每个参考位置信息与估计位置信息的位置误差；

针对同一方向对应有多个传感器的情况，根据每个传感器对应的位置误差自适应分配出该方向上每个传感器的信息融合权重，对同一方向对应有多个传感器中的每个传感器在该方向上的位置误差按照权重进行加权求和得到该方向上的位置总误差；

设计X轴、Y轴、Z轴三个方向对应的三阶锁相环滤波器；

针对同一方向对应有一个传感器的情况，利用三阶锁相环滤波器，根据该方向对应的位置误差计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；针对同一方向对应有多个传感器的情况，利用三阶锁相环滤波器，根据该方向对应的位置总误差计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；

设计速度-位移预测模型，利用速度-位移预测模型，根据估计加速度信息及估计的前一时刻的速度信息计算当前时刻估计的速度信息及估计位置信息。

可选地，根据每个传感器对应的位置误差自适应分配出该方向上每个传感器的信息融合权重，对同一方向上具有的多个传感器中的每个传感器在该方向上的位置误差按照权重进行加权求和得到该方向上的位置总误差的步骤具体包括：

对每个位置误差的绝对值求倒数，即为

其中，err_Dm为具有多个传感器的D方向对应的第m个位置误差，D方向为X轴、Y轴、或Z轴对应的方向；

根据每个位置误差的绝对值倒数以及每个位置误差对应的传感器的先验信息权重自适应分配出相应传感器的信息融合权重，记为K_Dm，计算公式如下：

其中，K_Dm为具有i个传感器的D方向对应的第m个传感器在D方向上的权重，i等于或大于2，R_Dm为D方向对应的第m个传感器的先验信息权重；

具有多个传感器的D方向的位置总误差的计算公式为：

err_posD＝K_D1err_D1+K_D2err_D2+KK_Dmerr_Dm+K+K_Dierr_Di。

可选地，若位置误差的绝对值的倒数大于预设阈值时，将位置误差的绝对值的倒数记为预设阈值。

可选地，每个三阶锁相环滤波器的三个支路的系数分别为C₁、C₂、C₃，其中，C₁支路的输出为out1_pll＝err_pos×C₁，C₂支路的输出为out2_pll＝err_pos×C₂，C₃支路的输出为

其中er_rpos表示位置误差或位置总误差。

可选地，无人机导航方法还包括：

利用加速度计传感器测量无人机加速度信息，并扣除零偏后，得到无人机体坐标系下的比力；

通过姿态转换矩阵得到地理坐标系下的比力，将地理坐标系下的比力扣除重力影响得到地理坐标系下的运动加速度信息；

计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息的步骤具体包括：

对C₂、C₃支路的输出进行求和得到锁相环路估计的加速度；

将运动加速度信息与锁相环路估计的加速度进行叠加，获得估计加速度信息；

将估计加速度信息进行积分，并与C1支路的输出求和得到估计的前一时刻的速度信息。

可选地，速度-位移预测模型为：

当前时刻速度预测模型，vel(n)＝vel(n-1)+acc×T；

当前时刻位置预测模型，

其中，pos(n-1)为估计的前一时刻的位置信息，vel(n-1)为估计的前一时刻的速度信息，acc为估计加速度信息，T为时间周期。

可选地，三阶锁相环滤波器的三个支路的系数C₁＝2.4ω₀，

其中

B_n为三阶锁相环滤波器的带宽，ω₀为频率。

根据本发明另一个方面，还提供了一种无人机导航系统，包括：

多个传感器，配置为获取无人机在X轴、Y轴、Z轴三个方向上的多个参考位置信息，X轴方向、Y轴方向、Z轴方向均对应有一个或多个传感器，且X轴、Y轴、Z轴三个方向中至少一个方向对应有多个传感器；

第一计算单元，配置为计算X轴、Y轴、Z轴三个方向上每个传感器检测的参考位置信息与估计位置信息之间的差值，得到X轴、Y轴、Z轴三个方向上每个传感器检测的每个参考位置信息与估计位置信息的位置误差；

第二计算单元，配置为针对同一方向上具有的多个传感器的情况，根据每个传感器对应的位置误差自适应分配出该方向上每个传感器的信息融合权重，对同一方向上具有的多个传感器中的每个传感器在该方向上的位置误差按照权重进行加权求和得到该方向上的位置总误差；

三个三阶锁相环滤波器，分别对应于X轴、Y轴、Z轴三个方向，三阶锁相环滤波器配置为：针对同一方向对应有一个传感器的情况，根据该方向对应的位置误差计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；三阶锁相环滤波器还配置为：针对同一方向对应有多个传感器的情况，利用三阶锁相环滤波器，根据该方向对应的位置总误差计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；

速度-位移预测单元，配置为根据估计加速度信息及估计的前一时刻的速度信息计算当前时刻估计的速度信息及估计位置信息。

可选地，无人机导航系统还包括：

加速度计传感器，配置为获取无人机的加速度信息；

第三计算单元，配置为通过姿态转换矩阵得到地理坐标系下的比力，将地理坐标系下的比力扣除重力影响得到地理坐标系下的运动加速度信息；

三阶锁相环滤波器还配置为对C₂、C₃支路的输出进行求和得到锁相环路估计的加速度，将运动加速度信息与锁相环路估计的加速度进行叠加，获得估计加速度信息，将估计加速度信息进行积分，并与C1支路的输出求和得到估计的前一时刻的速度信息。

可选地，传感器包括：

高精度时差分定位测量单元，配置为获取无人机在X轴方向、Y轴和Z轴方向的参考位置信息；

GPS，配置为获取无人机在X轴方向、Y轴和Z轴方向的参考位置信息；以及

高度计，配置为获取无人机在Z轴方向的参考位置信息。

本发明的基于多源信息自适应融合的无人机导航方法，利用多个传感器获得无人机的多个位置信息，针对同一方向对应有多个所述传感器的情况，通过信息融合权重的方式得到位置总误差，并利用三阶锁相环滤波器和速度-位移预测模型得到无人机的估计的速度信息及估计位置信息，该方法既能够快速准确地从外部多源位置信息中估计出最优位置和速度，同时又减少了单个传感器信息突变带来的定位误差，提升了无人机跟踪的精确性。

进一步地，本发明的基于多源信息自适应融合的无人机导航方法中，利用特别设计的信息融合权重的方法获得每个位置误差的权重，进一步减少了单个传感器信息突变带来的定位误差。

根据下文结合附图对本发明具体实施例的详细描述，本领域技术人员将会更加明了本发明的上述以及其他目的、优点和特征。

附图说明

后文将参照附图以示例性而非限制性的方式详细描述本发明的一些具体实施例。附图中相同的附图标记标示了相同或类似的部件或部分。本领域技术人员应该理解，这些附图未必是按比例绘制的。附图中：

图1是根据本发明一个实施例的基于多源信息自适应融合的无人机导航方法的流程图；

图2是根据本发明实施例1的Z轴方向三阶锁相环滤波器的原理图；

图3是根据本发明实施例2的Z轴方向三阶锁相环滤波器的原理图；

图4是根据本发明实施例3的X轴方向三阶锁相环滤波器的原理图；

图5是根据本发明实施例3的Y轴方向三阶锁相环滤波器的原理图；以及

图6是根据本发明实施例3的Z轴方向三阶锁相环滤波器的原理图。

具体实施方式

本实施例首先提供了一种无人机导航系统。无人机导航系统，包括多个传感器。多个传感器配置为获取无人机在X轴、Y轴、Z轴三个方向上的多个参考位置信息，X轴方向、Y轴方向、Z轴方向均对应有一个或多个传感器，且X轴、Y轴、Z轴三个方向中至少一个方向对应有多个传感器。

传感器可包括高精度时差分定位测量单元(RTK)、GPS和高度计。高精度时差分定位测量单元配置为获取无人机在X轴方向、Y轴和Z轴方向的参考位置信息，GPS配置为获取无人机在X轴方向、Y轴和Z轴方向的参考位置信息，高度计配置为获取无人机在Z轴方向的参考位置信息。也即是：水平方向(X轴、Y轴)可具有RTK高精度测量信息、单点GPS信息，高程方向(Z轴)可具有RTK高精度测量信息、单点GPS信息、高度计信息(比如气压高度计、超声波测距传感器、雷达高度计等)。

特别地，导航系统还包括第一计算单元、第二计算单元、三个三阶锁相环滤波器和速度-位移预测单元。

第一计算单元配置为计算X轴、Y轴、Z轴三个方向上每个传感器检测的参考位置信息与估计位置信息之间的差值，得到X轴、Y轴、Z轴三个方向上每个传感器检测的每个参考位置信息与估计位置信息的位置误差。

第二计算单元配置为针对同一方向上具有的多个传感器的情况，根据每个传感器对应的位置误差自适应分配出该方向上每个传感器的信息融合权重，对同一方向上具有的多个传感器中的每个传感器在该方向上的位置误差按照权重进行加权求和得到该方向上的位置总误差；

三个三阶锁相环滤波器分别对应于X轴、Y轴、Z轴三个方向，三阶锁相环滤波器配置为：针对同一方向对应有一个传感器的情况，根据该方向对应的位置误差计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；三阶锁相环滤波器还配置为：针对同一方向对应有多个传感器的情况，利用三阶锁相环滤波器，根据该方向对应的位置总误差计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；

速度-位移预测单元配置为根据估计加速度信息及估计的前一时刻的速度信息计算当前时刻估计的速度信息及估计位置信息。

其中，第二计算单元还配置为对每个位置误差的绝对值求倒数，并根据每个位置误差的绝对值倒数以及每个位置误差对应的传感器的先验信息权重自适应分配出相应传感器的信息融合权重。

第二计算单元还配置为在位置误差的绝对值的倒数大于预设阈值时，将位置误差的绝对值的倒数记为预设阈值。例如，若位置误差的绝对值的倒数大于100，则取100作为位置误差的绝对值的倒数。

每个三阶锁相环滤波器的三个支路的系数分别为C₁、C₂、C₃，其中，C₁支路的输出为out1_pll＝err_pos×C₁，C₂支路的输出为out2_pll＝err_pos×C₂，C₃支路的输出为out3_pll＝∫(err_pos×C₃)dt，其中err_pos表示上述中的位置误差或位置总误差。

无人机导航系统还包括配置为获取无人机的加速度信息的加速度计传感器和第三计算单元。第三计算单元配置为通过姿态转换矩阵得到地理坐标系下的比力，将地理坐标系下的比力扣除重力影响得到地理坐标系下的运动加速度信息。

三阶锁相环滤波器还配置为对C₂、C₃支路的输出进行求和得到锁相环路估计的加速度，将运动加速度信息与锁相环路估计的加速度进行叠加，获得估计加速度信息，将估计加速度信息进行积分，并与C1支路的输出求和得到估计的前一时刻的速度信息。

基于上述的无人机导航系统，本实施例提供了一种基于多源信息自适应融合的无人机导航方法，图1是根据本发明一个实施例的基于多源信息自适应融合的无人机导航方法的流程图，无人机导航方法包括：

步骤S102，利用多个传感器分别检测的当前时刻无人机在X Y Z三个方向上的多个参考位置信息，X轴方向、Y轴方向、Z轴方向均对应有一个或多个传感器，且X轴、Y轴、Z轴三个方向中至少一个方向对应有多个传感器。

例如，水平方向(X轴、Y轴)可具有RTK高精度测量信息、单点GPS信息，高程方向(Z轴)可具有RTK高精度测量信息、单点GPS信息、高度计信息(比如气压高度计、超声波测距传感器、雷达高度计等)。

步骤S104，分别计算X Y Z三个方向上每个传感器检测的参考位置信息与估计位置信息之间的差值，得到X Y Z三个方向上每个传感器检测的每个参考位置信息与估计位置信息的位置误差。

每个传感器在X、Y、Z方向的位置误差分别为err_Xi、err_Yi、err_Zi。以err_Xi为例，err_Xi代表第i个传感器在X方向的参考位置信息减去X方向的估计位置信息，估计位置信息为利用速度-位移预测模型估计出来的位置。

在水平X、Y方向，以集成了RTK高精度测量信息，单点GPS信息为例进行说明，即水平X、Y方向只有2源信息。在X方向则有err_X1，err_X2，err_X1代表传感器RTK在X方向的位置误差，err_X2代表单点GPS在X方向的位置误差。

步骤S106，针对同一方向对应有多个传感器的情况，根据每个传感器对应的位置误差自适应分配出该方向上每个传感器的信息融合权重，对同一方向上对应有多个传感器中的每个传感器在该方向上的所述位置误差按照权重进行加权求和得到该方向上的位置总误差。

步骤S108，设计X Y Z三个方向对应的三阶锁相环滤波器；

步骤S110，针对同一方向对应有一个传感器的情况，利用三阶锁相环滤波器，根据该方向对应的位置误差计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；针对同一方向对应有多个传感器的情况，利用三阶锁相环滤波器，根据该方向对应的位置总误差计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息。

步骤S112，设计速度-位移预测模型，利用速度-位移预测模型，根据估计加速度信息及所述估计的前一时刻的速度信息计算当前时刻估计的速度信息及估计位置信息。

该无人机导航方法还包括：

利用加速度计传感器测量无人机加速度信息，并扣除零偏后，得到无人机体坐标系下的比力。

体坐标系下X轴、Y轴、Z轴三个方向的加速度计的测量值记为acc_measure(3维列矢量)，零偏记为acc_bias(3维列矢量)，体坐标系下的比力记为

(3维列矢量)，则有：

其中，

acc_measure、acc_bias都是3维列矢量，3个分量分别对应X轴、Y轴、Z轴。

通过姿态转换矩阵得到地理坐标系下的比力，将地理坐标系下的比力扣除重力影响得到地理坐标系下的运动加速度信息。

将加速度计传感器的测量值信息扣除零偏后得到体坐标系下的比力，通过姿态转换矩阵得到地理坐标系下的比力，采用传统成熟方法利用陀螺信息得到姿态转换矩阵记为

地理坐标系下的比力记为

则有：

是3维列矢量，3个分量分别对应X轴、Y轴、Z轴。

重力矢量在NED地理坐标系中表示为：G＝[0 0 g]^T，将地理坐标系下的比力扣除重力影响后得到地理坐标系下的运动加速度信息，记为accⁿ，则有：

accⁿ是3维列矢量，3个分量分别对应X轴、Y轴、Z轴，假设accⁿ的三个分量表示为：accⁿ＝[accx accy accz]^T。

速度-位移预测模型对应的公式如下：

vel(n)＝vel(n-1)+acc×T；

其中，acc为上述的估计加速度信息，vel(n-1)是估计的前一时刻的速度信息，vel(n)是当前时刻估计速度信息，pos(n-1)是估计的前一时刻的位置信息，pos(n)是当前时刻估计位置信息，T为时间周期。

X轴、Y轴、Z轴三个方向分别对应有一个三阶锁相环滤波器，三阶锁相环滤波器分别对相应方向的位置信息进行跟踪。每个三阶锁相环滤波器的三个支路的系数分别为C₁、C₂、C₃，其中，C₁支路的输出为out1_pll＝err_pos×C₁，C₂支路的输出为out2_pll＝err_pos×C₂，C₃支路的输出为out3_pll＝∫(err_pos×C₃)dt，其中err_pos表示上述的位置误差或上述的位置总误差。对C₂、C₃支路的输出进行求和：

代表了三阶锁相环滤波器估计的上一时刻至本时刻期间的平均加速度，即为锁相环路估计的加速度，

将上述计算得到的运动加速度信息accⁿ的对应分量叠加到锁相环路估计的加速度，即，如图2、3、4、5、6所示，在A点处叠加运动加速度信息accⁿ。获得上述的估计加速度信息，即

假设accⁿ的三个分量表示为：accⁿ＝[accx accy accz]^T，则X轴的估计加速度信息：

Y轴的估计加速度信息：

Z轴的估计加速度信息：

将上述获得的估计加速度信息进行积分，并与C1支路的输出求和得到上述的估计的前一时刻的速度信息。将上述获得的估计的加速度信息及获得的估计的前一时刻的速度信息作为速度-位移预测模型的输入，经速度-位移预测模型的处理，输出当前时刻估计的速度信息及估计位置信息。

上述三阶锁相环滤波器的三个支路系数根据三阶锁相环滤波器的带宽确定，首先，根据需要的带宽B_n确定频率ω₀，B_n＝0.748ω₀，

再根据频率ω₀确定三个支路的系数，C₁＝2.4ω₀，

在进行无人机的位置及速度跟踪时，首先根据无人机初始时间周期内的初始位置、速度及加速度获得初始时间无人机的位置信息，同时，多个传感器获取无人机此刻的多个参考位置信息，将此刻无人机的多个参考位置信息与初始位置作差，获得位置误差。

针对同一方向对应有一个传感器的情况，将该位置误差作为三阶锁相环滤波器的输入，经过三阶锁相环滤波器和速度-位移预测模型的上述计算过程，输出当前时刻估计的速度信息及估计位置信息。再返回步骤S102，即将该估计位置信息与多个传感器获得的当前参考位置信息作差，获得参考位置信息与估计位置信息之间的位置误差，再进行上述步骤S104、S106、S108、S110、S112，依次循环，实现对无人机的速度及位置的跟踪。

针对同一方向对应有多个传感器的情况，首先按照步骤S106得到某一方向(X、Y、Z)上的位置总误差，将该位置总误差作为三阶锁相环滤波器的输入，经过三阶锁相环滤波器和速度-位移预测模型的上述计算过程，输出当前时刻估计的速度信息及估计位置信息。再返回步骤S102，即将该估计位置信息与多个传感器获得的当前参考位置信息作差，获得参考位置信息与估计位置信息之间的位置误差，再进行上述步骤S104、S106、S108、S110、S112，依次循环，实现对无人机的速度及位置的跟踪。

具体地，步骤S106中，根据每个传感器对应的位置误差自适应分配出该方向上每个传感器的信息融合权重，对同一方向上具有的多个所述传感器中的每个传感器在该方向上的位置误差按照权重进行加权求和得到该方向上的位置总误差的步骤具体包括：

对每个位置误差的绝对值求倒数，即为

其中，err_Dm为具有多个传感器的D方向对应的第m个位置误差，D方向为X轴、Y轴、或Z轴对应的方向；

根据每个位置误差的绝对值倒数以及每个位置误差对应的传感器的先验信息权重自适应分配出相应传感器的信息融合权重，记为K_Dm，计算公式如下：

其中，K_Dm为具有i个传感器的D方向对应的第m个传感器在D方向上的权重，i等于或大于2，R_Dm为D方向对应的第m个传感器的先验信息权重。传感器的先验信息权重可根据信息源的精度确定出。

具有多个传感器的D方向的位置总误差的计算公式为：

err_posD＝K_D1err_D1+K_D2err_D2+KK_Dmerr_Dm+K+K_Dierr_Di。

为便于更加清楚理解上述所述的位置总误差的计算过程，以下对该计算过程举例进行说明。

实施例1，参见图2，针对典型的单点GPS+超声波测高或者气压高度计的无人机导航系统，在水平方向(X方向和Y方向)只有一个单点GPS获取的参考信息源，高程方向则有单点GPS获得参考信息源和超声波测高或者气压高度计(本实施例以超声波测高为例进行说明)获取的参考信息源，共两个参考信息源。因此，水平方向(X方向和Y方向)以位置误差作为三阶锁相环的输入，高程方向(Z方向)以位置总误差作为三阶锁相环的输入，结合速度-位移模型得到无人机的当前时刻估计的速度信息及估计位置信息。

高程方向(Z方向)位置总误差的计算过程如下：

步骤T1：分别计算单点GPS、超声波测高二个传感器在Z方向的参考位置信息与速度-位移预测模型得到的估计位置信息之间的差值，获得每个参考位置信息与估计位置信息之间的位置误差。

每个传感器在Z方向的位置误差分别为err_Zi。err_Zi代表第i个传感器在Z方向的位置信息减去速度-位移预测模型在Z方向估计出来的位置。

步骤T2：对Z方向上每个参考信息源对应的位置误差的绝对值求倒数，得到

对于超过预设阈值的，

取预设阈值。

步骤T3：根据GPS获得参考信息源和超声波测高获取的参考信息源的精度确定出GPS和超声波测高传感器的先验信息权重。例如，单点GPS的先验信息权重可以设置为R_Z1＝1，超声波测高传感器的先验信息权重可以设置为R_Z2＝5。

步骤T4：依据位置误差绝对值倒数自适应分配出GPS和超声波测高传感器的信息融合权重。权重K_Z1、K_Z2计算公式如下：

步骤T5：对GPS和超声波测高传感器的误差信息按照权重进行加权求和得到Z方向的位置总误差。即：err_posZ＝K_Z1err_Z1+K_Z2err_Z2。

实施例2，参见图3，针对典型的单点GPS+超声波测高+气压高度计的无人机导航系统，在水平方向只有一个单点GPS获取的参考信息源，高程方向则有单点GPS获得参考信息源、超声波测高和气压高度计获取的参考信息源，共三个参考信息源。因此，水平方向(X方向和Y方向)以位置误差作为三阶锁相环的输入，高程方向(Z方向)以位置总误差作为三阶锁相环的输入，结合速度-位移模型得到无人机的当前时刻估计的速度信息及估计位置信息。

高程方向(Z方向)位置总误差的计算过程如下：

步骤T1：分别计算单点GPS、超声波测高和气压高度计三个传感器在Z方向的参考位置信息与速度-位移预测模型得到的估计位置信息之间的差值，获得每个参考位置信息与估计位置信息之间的位置误差。

每个传感器在Z方向的位置误差分别为err_Zi。err_Zi代表第i个传感器在Z方向的位置信息减去速度-位移预测模型在Z方向估计出来的位置。

步骤T2：对Z方向上每个参考信息源对应的位置误差的绝对值求倒数，得到

对于超过预设阈值的，

取预设阈值。

步骤T3：根据GPS获得参考信息源、超声波测高和气压高度计获取的参考信息源的精度确定出GPS、超声波测高传感器和气压高度计的先验信息权重。例如，单点GPS的先验信息权重可以设置为R_Z1＝1，超声波测高传感器的先验信息权重可以设置为R_Z2＝5，气压高度计测高传感器的先验信息权重可以设置为R_Z3＝0.4。

步骤T4：依据位置误差绝对值倒数自适应分配出GPS的信息融合权重K_Z1、超声波测高传感器的信息融合权重K_Z2和气压高度计测高传感器的信息融合权重K_Z3。权重K_Z1、K_Z2、K_Z3的计算公式如下：

步骤T5：对GPS、超声波测高传感器和气压高度计测高传感器的误差信息按照权重进行加权求和得到Z方向的位置总误差。即：err_Z＝K_Z1err_Z1+K_Z2err_Z2+K_Z3err_Z3。

实施例3，参见图4、图5和图6，针对复杂的RKT高精度定位信息、单点GPS、超声波测高、气压高度计测高的无人机导航系统，在水平方向(X方向和Y方向)都有两个参考信息源：RKT高精度定位信息和单点GPS，高程方向则有四个参考信息源：RKT高精度定位信息、单点GPS、超声波测距高度计和气压高度计。因此，水平方向(X方向和Y方向)以总位置误差作为三阶锁相环的输入，高程方向(Z方向)以位置总误差作为三阶锁相环的输入，结合速度-位移模型得到无人机的当前时刻估计的速度信息及估计位置信息。

水平方向(X方向和Y方向)和高程方向(Z方向)的位置总误差的计算过程如下：

步骤T1：分别计算RKT、单点GPS二个传感器在水平方向(X方向和Y方向)的参考位置信息与速度-位移预测模型得到的估计位置信息之间的差值，获得每个参考位置信息与估计位置信息之间的位置误差。分别计算RKT高精度定位信息、单点GPS、超声波测距高度计和气压高度计四个传感器在高程方向(Z方向)的参考位置信息与速度-位移预测模型得到的估计位置信息之间的差值，获得每个参考位置信息与估计位置信息之间的位置误差。

每个传感器在X、Y、Z方向的位置误差分别为err_Xi,err_Yi,err_Zi。以err_Xi为例，err_Xi代表第i个传感器在X方向的位置信息减去速度-位移预测模型在X方向估计出来的位置。

步骤T2：对X、Y、Z方向上每个参考信息源对应的位置误差的绝对值求倒数，得到

对于超过预设阈值的，

取对应的预设阈值。

步骤T3：根据各个信息源的精度确定出各个传感器的先验信息权重。例如，RKT高精度定位信息权重可以设置为R_X1＝10，R_Y1＝10，R_Z1＝10，单点GPS的先验信息权重可以设置为R_X2＝1，R_Y2＝1，R_Z2＝1，超声波测高传感器的先验信息权重可以设置为R_Z3＝5，气压高度计测高传感器的先验信息权重可以设置为R_Z4＝0.4。

步骤T4：依据位置误差绝对值倒数自适应分配出每个传感器的信息融合权重。X方向的位置误差权重：

Y方向的位置误差权重：

在Z方向，由于存在四种信息源，分别是传感器1—RTK、传感器2—单点GPS、传感器3—超声波测距高度计与传感器4—气压高度计，因此存在四个误差err_Z1，err_Z2，err_Z3，err_Z4。权重K_Z1、K_Z2、K_Z3、K_Z4计算公式如下：

步骤T5：对每个传感器的误差信息按照权重进行加权求和得到位置总误差。

X、Y、Z方向的位置总误差分别为：

err_X＝K_X1err_X1+K_X2err_X2；

err_Y＝K_Y1err_Y1+K_Y2err_Y2；

err_Z＝K_Z1err_Z1+K_Z2err_Z2+K_Z3err_Z3+K_Z4err_Z4。

本实施例的基于多源信息自适应融合的无人机导航方法，利用多个传感器获得无人机的多个位置信息，针对同一方向对应有多个所述传感器的情况，通过信息融合权重的方式得到位置总误差，并利用三阶锁相环滤波器和速度-位移预测模型得到无人机的估计的速度信息及估计位置信息，该方法既能够快速准确地从外部多源位置信息中估计出最优位置和速度，同时又减少了单个传感器信息突变带来的定位误差，提升了无人机跟踪的精确性。

进一步地，本实施例的基于多源信息自适应融合的无人机导航方法中，利用特别设计的信息融合权重的方法获得每个位置误差的权重，进一步减少了单个传感器信息突变带来的定位误差。

至此，本领域技术人员应认识到，虽然本文已详尽示出和描述了本发明的多个示例性实施例，但是，在不脱离本发明精神和范围的情况下，仍可根据本发明公开的内容直接确定或推导出符合本发明原理的许多其他变型或修改。因此，本发明的范围应被理解和认定为覆盖了所有这些其他变型或修改。

Claims (9)

1.一种基于多源信息自适应融合的无人机导航方法，包括：

利用多个传感器分别检测的当前时刻无人机在X轴、Y轴、Z轴三个方向上的多个参考位置信息，X轴方向、Y轴方向、Z轴方向均对应有一个或多个所述传感器，且X轴、Y轴、Z轴三个方向中至少一个方向对应有多个所述传感器；

分别计算X轴、Y轴、Z轴三个方向上每个所述传感器检测的所述参考位置信息与估计位置信息之间的差值，得到X轴、Y轴、Z轴三个方向上每个所述传感器检测的每个所述参考位置信息与所述估计位置信息的位置误差；

针对同一方向对应有多个所述传感器的情况，根据每个所述传感器对应的所述位置误差自适应分配出该方向上每个所述传感器的信息融合权重，对同一方向上对应有多个所述传感器中的每个所述传感器在该方向上的所述位置误差按照所述权重进行加权求和得到该方向上的位置总误差；

设计X轴、Y轴、Z轴三个方向对应的三阶锁相环滤波器；

针对同一方向对应有一个所述传感器的情况，利用所述三阶锁相环滤波器，根据该方向对应的所述位置误差计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；针对同一方向对应有多个所述传感器的情况，利用所述三阶锁相环滤波器，根据该方向对应的所述位置总误差计算所述无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；

设计速度-位移预测模型，利用所述速度-位移预测模型，根据所述估计加速度信息及所述估计的前一时刻的速度信息计算当前时刻估计的速度信息及所述估计位置信息；

其中，根据每个所述传感器对应的所述位置误差自适应分配出该方向上每个所述传感器的信息融合权重，对同一方向上具有的多个所述传感器中的每个所述传感器在该方向上的所述位置误差按照所述权重进行加权求和得到该方向上的位置总误差的步骤具体包括：

对每个所述位置误差的绝对值求倒数，即为

其中，err_Dm为具有多个所述传感器的D方向对应的第m个位置误差，D方向为X轴、Y轴、或Z轴对应的方向；

根据所述每个位置误差的绝对值倒数以及所述每个位置误差对应的所述传感器的先验信息权重自适应分配出相应传感器的信息融合权重，记为K_Dm，计算公式如下：

其中，K_Dm为具有i个所述传感器的D方向对应的第m个所述传感器在D方向上的权重，i等于或大于2，R_Dm为D方向对应的第m个所述传感器的先验信息权重；

具有多个所述传感器的D方向的位置总误差的计算公式为：

err_posD＝K_D1err_D1+K_D2err_D2+KK_Dmerr_Dm+K+K_Dierr_Di。

2.根据权利要求1所述的无人机导航方法，其中

若所述位置误差的绝对值的倒数大于预设阈值时，将所述位置误差的绝对值的倒数记为所述预设阈值。

3.根据权利要求1所述的无人机导航方法，其中

每个所述三阶锁相环滤波器的三个支路的系数分别为C₁、C₂、C₃，其中，C₁支路的输出为out1_pll＝err_pos×C₁，C₂支路的输出为out2_pll＝err_pos×C₂，C₃支路的输出为out3_pll＝∫(err_pos×C₃)dt，其中err_pos表示所述位置误差或所述位置总误差。

4.根据权利要求3所述的无人机导航方法，还包括：

利用加速度计传感器测量无人机加速度信息，并扣除零偏后，得到无人机体坐标系下的比力；

通过姿态转换矩阵得到地理坐标系下的比力，将地理坐标系下的比力扣除重力影响得到地理坐标系下的运动加速度信息；

计算所述无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息的步骤具体包括：

对C₂、C₃支路的输出进行求和得到锁相环路估计的加速度；

将所述运动加速度信息与所述锁相环路估计的加速度进行叠加，获得所述估计加速度信息；

将所述估计加速度信息进行积分，并与C1支路的输出求和得到所述估计的前一时刻的速度信息。

5.根据权利要求1所述的无人机导航方法，其中，所述速度-位移预测模型为：

当前时刻速度预测模型，vel(n)＝vel(n-1)+acc×T；

当前时刻位置预测模型，

其中，pos(n)是当前时刻所述估计位置信息，pos(n-1)为估计的前一时刻的位置信息，vel(n)是当前时刻所述估计速度信息，vel(n-1)为所述估计的前一时刻的速度信息，acc为所述估计加速度信息，T为时间周期。

6.根据权利要求3所述的无人机导航方法，其中

所述三阶锁相环滤波器的三个支路的系数C₁＝2.4ω₀，

其中

B_n为所述三阶锁相环滤波器的带宽，ω₀为频率。

7.一种无人机导航系统，包括：

多个传感器，配置为获取无人机在X轴、Y轴、Z轴三个方向上的多个参考位置信息，X轴方向、Y轴方向、Z轴方向均对应有一个或多个所述传感器，且X轴、Y轴、Z轴三个方向中至少一个方向对应有多个所述传感器；

第一计算单元，配置为计算X轴、Y轴、Z轴三个方向上每个所述传感器检测的所述参考位置信息与估计位置信息之间的差值，得到X轴、Y轴、Z轴三个方向上每个所述传感器检测的每个所述参考位置信息与所述估计位置信息的位置误差；

第二计算单元，配置为针对同一方向上具有的多个所述传感器的情况，根据每个所述传感器对应的所述位置误差自适应分配出该方向上每个所述传感器的信息融合权重，对同一方向上具有的多个所述传感器中的每个所述传感器在该方向上的所述位置误差按照所述权重进行加权求和得到该方向上的位置总误差；具体包括：

对每个所述位置误差的绝对值求倒数，即为

其中，err_Dm为具有多个所述传感器的D方向对应的第m个位置误差，D方向为X轴、Y轴、或Z轴对应的方向；

根据所述每个位置误差的绝对值倒数以及所述每个位置误差对应的所述传感器的先验信息权重自适应分配出相应传感器的信息融合权重，记为K_Dm，计算公式如下：

其中，K_Dm为具有i个所述传感器的D方向对应的第m个所述传感器在D方向上的权重，i等于或大于2，R_Dm为D方向对应的第m个所述传感器的先验信息权重；

具有多个所述传感器的D方向的位置总误差的计算公式为：

err_posD＝K_D1err_D1+K_D2err_D2+KK_Dmerr_Dm+K+K_Dierr_Di；

三个三阶锁相环滤波器，分别对应于X轴、Y轴、Z轴三个方向，所述三阶锁相环滤波器配置为：针对同一方向对应有一个所述传感器的情况，根据该方向对应的所述位置误差计算无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；所述三阶锁相环滤波器还配置为：针对同一方向对应有多个所述传感器的情况，利用所述三阶锁相环滤波器，根据该方向对应的所述位置总误差计算所述无人机在该方向上的估计加速度信息及估计的前一时刻的速度信息；

速度-位移预测单元，配置为根据所述估计加速度信息及所述估计的前一时刻的速度信息计算当前时刻的速度信息及所述估计位置信息。

8.根据权利要求7所述的无人机导航系统，还包括：

加速度计传感器，配置为获取所述无人机的加速度信息；

第三计算单元，配置为通过姿态转换矩阵得到地理坐标系下的比力，将地理坐标系下的比力扣除重力影响得到地理坐标系下的运动加速度信息；

所述三阶锁相环滤波器还配置为对C₂、C₃支路的输出进行求和得到锁相环路估计的加速度，将所述运动加速度信息与所述锁相环路估计的加速度进行叠加，获得所述估计加速度信息，将所述估计加速度信息进行积分，并与C1支路的输出求和得到所述估计的前一时刻的速度信息。

9.根据权利要求7所述的无人机导航系统，其中，所述传感器包括：

高精度时差分定位测量单元，配置为获取所述无人机在X轴方向、Y轴和Z轴方向的所述参考位置信息；

GPS，配置为获取无人机在X轴方向、Y轴和Z轴方向的所述参考位置信息；以及

高度计，配置为获取所述无人机在Z轴方向的所述参考位置信息。

Images