一种基于多传感器融合的无人机定高算法

技术领域

本发明及无人机测高技术领域，具体涉及一种基于多传感器融合的无人机定高算法。

背景技术

旋翼无人机是一个典型的欠驱动系统，具有多变量、强耦合、非线性等特性的系统，而且涉及学科众多，领域十分广泛，飞行过程中状态复杂。综合上述问题，旋翼无人机的测高具有很大困难。

现有的无人机测高传感器系统多是利用GPS，气压计，激光、加速度计等传感器中的一种获取高度信息。但由于各传感器均具有自己的短处，从而获取的无人机高度数据并不准确。无人机的高度信息可以通过气压计测量当前气压获取，也可以通过GPS直接读取，还可以通过加速度计的双重积分获取。然而，在起飞和降落的低空阶段，存在一定的气流，会导致气压计测高存在比较大的绝对误差，精度不高。 GPS是被动定位的，当GPS的系统关闭时，定位系统就会出现故障，加速度计的双重积分在短时间内是准确的，但时间一长，就会出现积分漂移现象，因此融合单一传感器所得高度信息并不准确，需要采用合理的算法，将各个传感器的信息进行融合，进而将各个传感器的优缺点进行结合，从而得到可靠性高的高度数据。

发明内容

本发明通过以下技术方案实现：

一种基于多传感器融合的无人机定高算法,包括无人机本体和设置在无人机本体中的控制装置，所述的控制装置包括电源、微处理器、存储器、信号处理及接口模块和多个用于测高度的传感器；将多个用于测高度的传感器测试的数据进行数据融合，融合时自动剔除不可用高度数据，将可用高度数据代入公式进行融合计算，得到更为精确可靠的高度信息；具体的操作步骤如下：

步骤1：设置高度传感器，且在正常状况下，通过各高度传感器获取对应移动设备的有效高度数据；

步骤2：结合传感器自身不足计算置信度，确定最终可用传感器高度数据；

步骤3：由于基准面不同，将所得可用传感器数据进行高度对齐；

步骤4：将所得数据代入公式进行融合计算，得出更为精确的高度信息。

进一步的，步骤1所述的设置高度传感器，是确定高度传感器在正常状况下获取的有效高度数据。

进一步的，所述多个用于测高度的传感器包括：高度计雷达、气压计以及GPS高度传感器，对应有效高度包括雷达高度、气压高度和 GPS高度；所述的高度计雷达需先结合姿态传感器进行高度修正，才能得到准确的雷达高度。

进一步的，所述的高度计雷达在进行有效性判断之前需结合姿态传感器进行高度修正：结合高度计雷达已知性能参数设定角度阈值为 20°；

基于所得姿态角对高度计雷达所得测量值进行修正，公式如下：

其中，H表示雷达测高值，h表示测量值，θ表示波束宽度，γ表示滚转角；θ、γ亦皆根据高度计雷达直接得到。

进一步的，步骤1所述的获取有效高度数据包括：高度计雷达在发射功率、射频均正常状态下，探测范围满足预设条件，数据传输无异常的正常状况下获取有效的高度数据；获取GPS高度传感器在GPS 搜星数量大于或等于设定阈值的正常状况下获取有效的高度数据；获取气压计高度传感器在气压计结构无损伤、高度传输无异常的正常状况下获取有效的高度数据。

进一步的，步骤2所述的计算置信度包括计算气压计的置信度和计算GPS的置信度；

计算气压计的置信度的公式如下：

其中，|V|为移动设备当前运动速度；Vx、Vy、Vz分别为XYZ三轴对应的运动速度；Vmax为移动设备最大运动速度；K为系数，取值范围为0-100，δbaro值越小表示气压计的数据可信度越高；

计算GPS的置信度包括计算GPS垂直速度置信度εGPS和GPS高度置信度δGPS；具体的公式如下：

其中，sacc为垂直速度精度因子，单位mm/s；vdop为垂直精度因子，取值范围为0.5-99.9；vacc为垂直精度，单位mm；tdop 为GPS钟差几何因子；vdop、tdop、vacc、sacc均可从GPS中读取，且δGPS和εGPS的值越小，则表示GPS数据的置信度越高。

进一步的，步骤3所述的将所得可用传感器数据进行高度对齐；

通过步骤一、二两次判定换算后，可采用下列公式计算出对齐后的雷达高度X1、对齐后的GPS高度X2和对齐后的气压高度X3；具体的公式如下：

X1＝H1+△h1 (5)

X2＝H2+△h2 (6)

X3＝H3 (7)

其中，H1是雷达高度，H2是GPS高度，H3是气压高度；△h1是气压高度H3和雷达高度H1之间的偏移量；△h2是气压高度H3和GPS 高度H2之间的偏移量，此处的偏移量为本身传感器已知连系参数；此时三个对齐后的高度数据处于同一基准面，可用于后续数据融合计算；

另外，当传感器高度数据从无效变为有效时，需对高度进行对齐，测得当前高度数据为H，知融合高度为

进一步的，步骤A：先假定系统状态空间模型，该模型根据具体所用系统结构已知：包括假定系统状态方程，用来描述状态随时间的变化的系统状态方程和用来描述每个状态下观测到的噪声的测量方程，具体的模型为：

其中，状态模型为f(·)，观测模型为h(·)，ε

步骤B：在t＝0时刻，进行初始化采样

步骤C：重要性采样：根据观测值序列确定重要性函数 q(x

步骤D：根据采样结果调整权值，并归一化：

步骤E：重新采样：根据归一化后的权值

步骤F：输出：得到更接近后验分布的t时刻状态估计值：

步骤G：操作完成，循环执行步骤步骤C至步骤F。

进一步的，当所述的传感器为多个时，则含有多个可用的传感数据时，其观测空间y

而对于传感器子系统Y

最大熵标准选择出传感器子系统：

一种基于多传感器融合的无人机，包括无人机本体和设置在无人机本体中的控制装置，所述的控制装置包括微处理器，以及与微处理器连接的电源，存储器，信号处理及接口模块和多个高度传感器；所述的信号处理及接口模块中的测高单元包括有效性判断单元、高度数据获取单元、高度对齐单元和融合计算单元；

所述多个用于测高度的传感器包括：高度计雷达、气压计以及 GPS高度传感器，对应有效高度包括雷达高度、气压高度和GPS高度；所述的高度计雷达需先结合姿态传感器通过修正单元进行高度修正，才能得到准确的雷达高度；

所述的修正单元与高度计雷达和姿态传感器连接，对测到的雷达高度进行修正；

所述的有效性判断单元与各个高度传感器连接，接收各个高度传感器的数据，并判断高度传感器是否在正常状况下取得，在正常状况下取得的多个高度传感器的数据为有效数据；

所述的高度数据获取单元为设置在有效性判断单元后侧、用于接收有效性判断单元的数据，并结合传感器自身不足计算数据的置信度，根据传感器可信范围再次确定可用高度传感器的高度数据；

所述的高度对齐单元设置在高度数据获取单元后侧，用于接收高度数据获取单元的数据，并对数据进行高度对齐计算；

所述的融合计算单元设置在高度对齐单元后侧，与高度对齐单元双向连接，进行信息交互；融合计算单元将对齐后的各个高度传感器高度代入公式进行融合计算。

有益效果

本发明提出的一种基于多传感器融合的无人机定高算法，与传统的采用GPS和气压计的无人机定高方法相比较，其具有以下有益效果：

(1)本技术方案克服了现有技术测量误差大、编程复杂等局限性，具有所用知识点原理及装置易理解、实时性强等优势，在无人机测高领域应用价值明显。

(2)本技术方案相比较采用单一的高度传感器，本发明可以才数据采集时候就可剔除不可用的高度数据(明细显偏离高度真实值的高度数据)，得到无人机的高度数据会更加真实，误差不至于过大。

(3)本技术方案采用了粒子滤波算法来融合无人机各传感器的高度数据，得到的无人机高度数据更加精确和可靠，高度数据的稳定性更好，会对无人机的高度信息逐步完善，最终得到的无人机高度信息会更加科学。

附图说明

图1是本发明中无人机整体结构的示意框图。

图2是本发明中传感器数据融合的流程示意图。

图3是本发明中无人机定高算法流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

实施例1

如图1所示，一种基于多传感器融合的无人机，包括无人机本体和设置在无人机本体中的控制装置，所述的控制装置包括微处理器，以及与微处理器连接的电源，存储器，信号处理及接口模块和多个高度传感器；所述的信号处理及接口模块中的测高单元包括有效性判断单元、高度数据获取单元、高度对齐单元和融合计算单元；

所述多个用于测高度的传感器包括：高度计雷达、气压计以及 GPS高度传感器，对应有效高度包括雷达高度、气压高度和GPS高度；所述的高度计雷达需先结合姿态传感器通过修正单元进行高度修正，才能得到准确的雷达高度；

所述的修正单元与高度计雷达和姿态传感器连接，对测到的雷达高度进行修正；

所述的有效性判断单元与各个高度传感器连接，接收各个高度传感器的数据，并判断高度传感器是否在正常状况下取得，在正常状况下取得的多个高度传感器的数据为有效数据；

所述的高度数据获取单元为设置在有效性判断单元后侧、用于接收有效性判断单元的数据，并结合传感器自身不足计算数据的置信度，根据传感器可信范围再次确定可用高度传感器的高度数据；

所述的高度对齐单元设置在高度数据获取单元后侧，用于接收高度数据获取单元的数据，并对数据进行高度对齐计算；

所述的融合计算单元设置在高度对齐单元后侧，与高度对齐单元双向连接，进行信息交互；融合计算单元将对齐后的各个高度传感器高度代入公式进行融合计算。

实施例2

如图2-3所示，一种基于多传感器融合的无人机定高算法,包括无人机本体和设置在无人机本体中的控制装置，所述的控制装置包括多个用于测高度的传感器。

本实施例采用三个高度传感器(高度计雷达、气压计以及GPS高度传感器)进行说明，传感器型号不定，市面上常见的该类型传感器皆可使用，但不同型号传感器误差不同，可根据实际情况使用。

将三个用于测高度的传感器(高度计雷达、气压计以及GPS高度传感器)测试的数据进行数据融合，融合时自动剔除不可用高度数据，将可用高度数据代入公式进行融合计算，得到更为精确可靠的高度信息，具体的操作步骤如下：

步骤1：设置高度传感器，且在正常状况下，通过各高度传感器获取对应移动设备的有效高度数据；是确定高度计雷达、气压计以及GPS在正常状况下获取的有效高度数据，对应有效高度包括：雷达高度、气压高度、GPS高度；

具体的，获取的有效高度数据包括：

气压计数据有效是指气压计在正常状况下获取的高度数据有效，其中正常状况包括气压计结构无损伤，数据传输无异常等，此时数据显示灯为绿色并向后传输数据，否则显示灯为红色截止传输；

GPS数据有效是指GPS在正常状况下获取的高度数据有效，其中正常状况是指GPS搜星数量大于或等于设定阈值(具体阈值根据实际需要确定，在此不做限定)，此时数据显示灯为绿色并向后传输数据，否则显示灯为红色截止传输；

述的高度计雷达在进行有效性判断之前需结合姿态传感器进行高度修正：结合高度计雷达已知性能参数设定角度阈值为20°；

基于所得姿态角对高度计雷达所得测量值进行修正，公式如下：

其中，H表示雷达测高值，h表示测量值，θ表示波束宽度，γ表示滚转角；θ、γ亦皆根据高度计雷达直接得到。

步骤2：结合传感器自身不足计算置信度，其中置信度的范围即二次确定传感器高度数据是否可用，需结合各自高度传感器本身性能参数设定；确定最终可用传感器高度数据；

在正常状况下获取各传感器有效数据，并结合传感器自身不足计算置信度确定最终可用传感器高度数据：

由于气压计受气流影响较大，所以需要根据移动设备的运动速度计算气压计的置信度，计算气压计的置信度的公式如下：

其中，|V|为移动设备当前运动速度；Vx、Vy、Vz分别为XYZ三轴对应的运动速度；Vmax为移动设备最大运动速度；K为系数，取值范围为0-100，δbaro值越小表示气压计的数据可信度越高；

计算GPS的置信度包括计算GPS垂直速度置信度εGPS和GPS高度置信度δGPS；具体的公式如下：

其中，sacc为垂直速度精度因子，单位mm/s；vdop为垂直精度因子，取值范围为0.5-99.9；vacc为垂直精度，单位mm；tdop 为GPS钟差几何因子；vdop、tdop、vacc、sacc均可从GPS中读取，且δGPS和εGPS的值越小，则表示GPS数据的置信度越高。

传感器置信度单元亦有显示装置，置信度在可用范围显示绿灯，否则为红灯截止传输。

步骤3：由于基准面不同，将所得可用传感器数据进行高度对齐；

由于高度计雷达是雷达到地面的距离，气压高度是移动设备机体相对地面的高度，而GPS高度是海拔高度，其各自基准不同，因此需要将获取到的可用高度数据进行高度对齐；

在本实施例中选用的高度传感器，至此其中所得可用传感器数据即是经过两次判定，气压计、GPS高度传感器经过有效性判断和置信度计算判定，高度计雷达经过修正单元和有效性判断。

对于高度对齐方面，由于各传感器的基准面不同，气压高度是移动设备机体相对地面的高度(在地面上的高度为0)，故以气压高度为参照；

通过步骤一、二两次判定换算后，可采用下列公式计算出对齐后的雷达高度X1、对齐后的GPS高度X2和对齐后的气压高度X3；具体的公式如下：

X1＝H1+△h1 (5)；

X2＝H2+△h2 (6)；

X3＝H3 (7)；

其中，H1是雷达高度，H2是GPS高度，H3是气压高度(此处三个传感器高度数据是经过两次判定后所得)；△h1是气压高度H3和雷达高度H1之间的偏移量；△h2是气压高度H3和GPS高度H2之间的偏移量，此处的偏移量为本身传感器已知连系参数；此时三个对齐后的高度数据处于同一基准面，可用于后续数据融合计算。另外，当传感器高度数据从无效变为有效时，需对高度进行对齐，测得当前高度数据为H，知融合高度为

步骤4：将所得数据代入公式进行融合计算，得出更为精确的高度信息；在得到高度对齐后的传感器数据后，将可用的高度数据代入融合计算单元利用粒子滤波算法得到更为精确的高度数据。粒子滤波算法常采用序贯蒙特卡洛方法来实现，即一组随机或已知某种的样本来描述估计量的概率分布(结合所用系统设定)，再根据得到的测量值，通过重要性函数对各样本点的权值的样本序列来逼近真实的后验概率分布，从而序贯更新状态。具体的融合方式如下：

步骤A：先假定系统状态空间模型，该模型根据具体所用系统结构已知：包括假定系统状态方程，用来描述状态随时间的变化的系统状态方程和用来描述每个状态下观测到的噪声的测量方程，具体的模型为：

其中，状态模型为f(·)，观测模型为h(·)，ε

步骤B：在t＝0时刻，进行初始化采样

步骤C：重要性采样：根据观测值序列确定重要性函数 q(x

步骤D：根据采样结果调整权值，并归一化：

步骤E：重新采样：根据归一化后的权值

步骤F：输出：得到更接近后验分布的t时刻状态估计值：

步骤G：操作完成，循环执行步骤步骤C至步骤F。

当所述的传感器为多个时，则含有多个可用的传感数据时，其观测空间y

而对于传感器子系统Y

最大熵标准选择出传感器子系统：