CN104567799B - 基于多传感器信息融合的小型旋翼无人机高度测量方法 - Google Patents

Abstract

本发明属于小型旋翼无人机自主飞行控制研究领域，为提出一套适应户内、外多种环境的小型旋翼无人机高度测量方法，为无人机提供实时、精准、可靠的高度信息，本发明采取的技术方案是，基于多传感器信息融合的小型旋翼无人机高度测量方法，利用安装在旋翼无人机底部的声纳采集其与地面间的距离作为声纳测量高度；通过加速度计获取无人机对象坐标系下的三轴加速度，获得大地参考系下的垂直加速度；气压计测量无人机所处高度的气压信息，比较地面的气压信息，代入标准大气模型获得通过基于气压测量的高度测量信息；利用以上输入，应用多传感器信息融合的高度测量方法，得到一个高频的实时高度与垂直速度信息。本发明主要应用于无人机自主飞行控制。

Description

基于多传感器信息融合的小型旋翼无人机高度测量方法

技术领域

本发明属于小型旋翼无人机自主飞行控制研究领域。针对旋翼无人机在垂直方向上的运动控制需求，提出了一套基于多传感器数据融合的小型无人机高度测量方法。

技术背景

近年来，小型旋翼无人机因其在诸多领域的广泛应用，成为控制领域的一个研究热点。其中，尤为四旋翼无人机一类的带有多转子的可垂直起降的无人机倍受关注。其垂直起降等多种机动飞行能力使其能够在狭小、难以到达的恶劣环境下执行特定任务。其应用涵盖了军用侦查、基础设施维护、自然灾害预警等诸多领域。

高度信息作为飞行的一个重要参数，是无人机安全有效飞行以及地面操作和指挥人员安全的重要保障。而现今存在的可用于无人机高度测量的传感器存在各自各样的局限性，如声纳在测量距离上存在限制，基于气压测量的高度测量对空气中的气压变化敏感等。

目前国内外已有多种基于GPS、激光、气压、计算机视觉的无人机高度测量方法。瑞士苏黎世联邦理工学院提出使用一个朝下的单眼相机，运用视觉SLAM(同步定位与建图算法)，实现了在一个无GPS信号区域的定位和建图(会议：the 2010IEEE InternationalConference on Robotics and Automation；著者：Michael Blosch D Sea,StephanWeiss；出版年月：2010年；文章题目：Vision Based MAV Navigation in Unknown andUnstructured Environments；页码：21-28)。西班牙拉古那大学J.Toledo设计了一套由3个陀螺仪、2个加速度计、4个声纳组成的无人机高度测量系统(期刊：IET Control Theoryand Application；著者：J.Toledo,L.Acosta,M.Sigut et al；出版年月：2009年；文章题目：Stabilisation and Altitude Tracking of a Four-rotor Microhelicopter usingthe Lifting Operators；页码：452-464)。南京航空航天大学提出由气压计、GPS模块、加速度计组成的无人机高度测量系统(会议：the 2008Control and Decision Conference；著者：Hu Yong z Z,ZhishengW；出版年月：2008年；文章题目：Federated Filter Basedmulti-Sensor Fault-tolerant Altitude Determination system for UAV；页码：2030-2034)。

上述方法在无人机的高度测量上取得了一定成果，但仍存在各自方面的不足。其中，计算机视觉方法需要巨大的数据运算处理，对微处理器的运算性能提出苛刻要求。为了小型无人机的高度测量而单独架设一套计算机视觉系统并不现实。基于声纳的高度测量存在工作高度受限的问题。南京航空航天大学提出的方法在高度测量精度上略显不足。

发明内容

为了克服现有技术的不足，发明旨在提出一套适应户内、外多种环境的小型旋翼无人机高度测量方法，为无人机提供实时、精准、可靠的高度信息。方法的实例实现中，所用的元器件具有体积小型、重量轻、成本低廉的优点。为此，本发明采取的技术方案是，基于多传感器信息融合的小型旋翼无人机高度测量方法，利用安装在旋翼无人机底部的声纳采集其与地面间的距离作为声纳测量高度；通过加速度计获取无人机对象坐标系下的三轴加速度，结合无人机的姿态角信息进行坐标系变换，获得大地参考系下的垂直加速度；气压计测量无人机所处高度的气压信息，比较地面的气压信息，代入标准大气模型获得通过基于气压测量的高度测量信息；利用以上的声纳测量高度、无人机垂直加速度、气压测量高度作为输入，应用多传感器信息融合的高度测量方法，得到一个高频的实时高度与垂直速度信息。

还包括声纳高度测量信息的错误检测步骤：

声纳测距工作频率随距离的增大而减小，视其随距离增大呈线性衰减关系如下：

f_SN＝f_Max-kh_SN

其中f_SN为声纳在所在高度的工作频率，f_Max为其在探测距离下限时所对应的最大工作频率，k为其工作频率随距离变化的衰减系数，h_SN为声纳处于可靠工作状态时测得的无人机高度，考虑无人机的垂直速度V_Z与声纳工作频率，则声纳两次高度测量值之差Δ_h满足如下等式：

考虑最终的垂直速度输出V_Out，加入一定的判断裕量Δ_hBase，若声纳两次的高度测量差满足以下式子，则视更新的声纳测量高度值为不可靠的错数，将其舍弃，

还包括气压计高度测量漂移的补偿步骤：

在声纳工作范围内时，用声纳测量高度补偿气压测量高度中由于空气气压变化导致的测量漂移，采用如下所示的算法：

该算法为显性互补滤波器的构造，其中h_Br为气压传感器测量高度，是消除漂移量后的气压传感器高度测量量，e为测量漂移造成的测量偏差，T_i为选定的时间系数，h_SN是经过错误检测后的超声波传感器高度测量值，是估算出的气压高度测量漂移量，的变化相当缓慢，在对应时间段内可视为近似的恒定值，则通过该漂移补偿算法输出的高度值近似视为高度真实值与高斯白噪声的叠加。

还包括垂直加速度的偏置补偿步骤：

经过坐标系变换后获得的垂直加速度包含一定的测量偏置，采用互补滤波器对该测量偏置进行消除，动力学描述如下：

其中，A_Z为无人机垂直加速度输入，b_a为所估计出的垂直加速度偏置，h_SN&Br为声纳测量高度与气压测量高度的输入，V_Z、h分别为垂直速度与高度，分别为V_Z、h的时间导数，K₁、K₂、K₃为待定系数，配置系统的闭环极点为s1＝-1/τ，s2＝-1/τ，s3＝-1/τ,τ为根据实际情况选择的系数，对应的滤波器增益为：

K₁＝3/τ,K₂＝3/τ²,K₃＝1/τ³。

还包括气压计测量高度与垂直加速度的融合步骤：

以补偿后的垂直加速度和气压计高度测量值作为输入，应用改进后的卡尔曼滤波器获得对应的垂直速度与高度输出：

无人机垂直方向的运动学描述用状态空间表达式表述如下：

其中，v_B、h_B分别为气压计测量高度与加速度融合所对应的垂直速度与高度输出，为系统状态x的时间导数，A_Zc是补偿后的垂直加速度输入，系统的离散化一阶近似如下：

x(k+1)≈(I+AT_S)x(k)+T_SBu(k)+ζ(k)

＝Γx(k)+Ψu(k)+ζ(k)

u(k)为无人机在k时刻的z轴方向加速度，x(k)为对应k时刻的无人机状态，T_S为选定的时间步长，ζ(k)为系统过程激励噪声，即补偿后的垂直加速度的零均值噪声，其对应一个协方差矩阵Q，I是单位矩阵，对于高度测量的测量模型：

y(k+1)＝Hx(k)+μ(k)

H＝[0 1]

其中，y(k+1)为高度的测量输入，H为观测矩阵，μ(k)为气压计高度测量噪声，其均值为零，对应协方差矩阵记为R，

基于离散卡尔曼滤波器的气压高度与垂直加速度的融合实现如下：

时间更新：

状态更新：

其中，对应k时刻的状态估计，u_k-1为k-1时刻的系统垂直加速度输入，是先验协方差，P_k是后验协方差，K_k是卡尔曼增益，上标T表示转置，这样就实现了卡尔曼滤波器的更新循环。

还包括声纳测量高度与垂直加速度的融合步骤：

在互补滤波器的基础上，去除了输入补偿的环节，以校正后的垂直加速度A_Z和声纳测量高度h_SN为输入，进行声纳测量高度与垂直加速度的融合。算法系统的动力学描述如下：

其中，h_s为声纳与加速度融合对应的高度估计输出，V_Zs为声纳与加速度融合对应的垂直速度输出，分别为h_s、V_Zs的时间导数，系数K₁、K₂：K₁＝3/τ,K₂＝3/τ²。

针对输入h_SN在某些时段是不可靠的错误值，解决方法如下：当无人机离开声纳正常工作范围时，暂停该高度融合的进行；当无人机返回声纳的正常工作范围时，启动该高度融合过程的同时进行以下操作：使用卡尔曼滤波器的垂直速度输出更新垂直速度状态V_Zs；使用超声波传感器测量信息更新高度状态h_s与其对应的状态缓存；暂停垂直加速度偏置补偿过程中的气压传感器测量信息的输入，仅开放超声波传感器测量信息的输入。

还包括高度融合信息的输出步骤：

对卡尔曼滤波器与互补滤波器的输出按照各自对应的权重进行线性组合，获得最终的高度与垂直速度输出信息；互补滤波器的输出即声纳与垂直加速度的融合数据的权重随着高度的增加而减小，卡尔曼滤波器的输出即气压计与垂直加速度的融合数据的权重相反。

针对气压瞬态扰动解决办法是：在扰动发生时，通过增大设定的观测噪声协方差以降低传感器测量的置信度，当扰动作用结束后，恢复观测噪声协方差设定值以恢复其对传感器测量的置信度，具体实现算法：Baro与A_Z分别为卡尔曼滤波器中，气压传感器测量高度与校正后的垂直加速度输入，h_B为其高度输出，Q_Inc为在扰动发生时，所设定的测量噪声增量，Q_Base为所设定的观测噪声基础量，Q为卡尔曼滤波器设定的观测噪声，K_a、K_m为大于零的系数；工作原理说明如下：对于卡尔曼滤波器工作于理想条件时的情况：气压计测量高度h_Br＝h_r+μ，μ为其测量噪声。滤波器输出高度h_B≈h_r，h_r为高度真值，当瞬态气压扰动发生时，由于测量更新对状态的校正是一个逐步的过程，高度输出h_B的变化在时间上滞后于气压传感器测量值的变化，假设扰动的发生过程是在0时刻瞬间发生的，则：

h_Br＝h_r+μ+Δ_h

其中，Δ_h为扰动造成的测量偏置，则测量噪声增量：

Q_Inc＝sath{∫(K_a|h_r-h_B+Δ_h+μ|-K_m)dt}

sath(x)为自定义的单边饱和函数，X_Max为设定的饱和值，以上过程即是根据h_B与h_Br的偏离程度确定Q_Inc，通过选择比例合适的系数K_a、K_m，使得该扰动未发生时，测量噪声增量在Q_Inc很大概率上为零，而当瞬态扰动发生后，Q_Inc很快达到饱和上限值，当扰动作用结束后，Q_Inc退出饱和并回到原先状态，这样，就降低了扰动发生时，传感器测量对高度估计的影响，从而在一定程度上实现了对扰动的抵抗，

另外，为了防止Q长期处于一个较高的饱和上限值，对Q_Inc的饱和上限值进行处理，每当发生一次瞬态扰动时，减小Q_Inc的饱和值上限。

与已有技术相比，本发明的技术特点与效果：

本发明融合了多传感器的测量信息，实现了在多种环境、不同高度下的实时无人机高度与垂直速度的获取，提高了高度与垂直速度测量的精度与频率。当无人机处于较低高度时，声纳与加速度计的信息融合可以提供一个精度高、可靠性好的融合信息。当无人机逐渐离开声纳工作范围高度时，高度融合能够平稳、逐渐过渡到气压计与加速度计的信息融合。无人机从声纳工作高度范围外返回时，高度融合能够平稳、逐渐过渡到声纳与加速度计的信息融合。此外，提出的瞬态气压扰动抵抗机制可以有效地抵抗瞬态气压扰动对高度融合(基于卡尔曼滤波器的气压计与加速度计信息的高度融合)造成的影响。以融合信息进行无人机的高度镇定实验，或得了良好的实验效果。

附图说明

图1是本发明的基于多传感器信息融合的高度测量方法框图；

图2是本发明中采用互补滤波器算法结构图；

图3是本发明中瞬态气压扰动抵抗机制的气压测量高度与垂直加速度的融合算法框图；

图4是四悬翼无人机静止时的高度与垂直速度估计；

图5是四旋翼无人机在室内1.7m高度基于声纳与加速度计融合的高度镇定实验。子图1为视为真值的声纳测量高度(红线)与高度估计(蓝线)，子图2为高度真值与估计值误差，子图3为垂直速度的估计。

图6是四旋翼无人机在室外2.8m高度基于三传感器融合的高度镇定实验；

图7是四旋翼无人机在室外无声纳检测作用时的高度镇定实验；

图8是四悬翼无人机在密闭室内基于气压计与加速度计融合的进行高度镇定时，加入了开关门动作所造成瞬态气压扰动，前后进行了对比验证实验，图8为试验结果示意图。

图9是模式选择示意图。

具体实施方式

本发明采用的技术方案是：利用安装在旋翼无人机底部的声纳采集其与地面间的距离作为声纳测量高度；通过加速度计获取无人机对象坐标系下的三轴加速度，结合无人机的姿态角信息进行坐标系变换，获得大地参考系下的垂直加速度；气压计测量无人机所处高度的气压信息，比较地面的气压信息，代入标准大气模型获得通过基于气压测量的高度测量信息。利用以上的声纳测量高度、无人机垂直加速度、气压测量高度作为输入，应用该多传感器信息融合的高度测量方法，得到一个高频的实时高度与垂直速度信息。方法过程框图如图1所示，包括如下步骤。

1.声纳高度测量信息的错误检测

声纳在高度测量时会由于多种因素使测量高度读入中存在尖刺错误。声纳测距工作频率随距离的增大而减小，视其随距离增大呈线性衰减关系如下：

f_SN＝f_Max-kh_SN

其中f_SN为声纳在所在高度的工作频率，f_Max为其在探测距离下限时所对应的最大工作频率，k为其工作频率随距离变化的衰减系数，h_SN为声纳处于可靠工作状态时测得的无人机高度。考虑无人机的垂直速度V_Z与声纳工作频率，则声纳两次高度测量值之差Δ_h满足如下等式：

考虑最终的垂直速度融合输出V_Out，加入一定的判断裕量Δ_hBase，若声纳两次的高度测量差满足以下式子，则视更新的声纳测量高度值为不可靠的错数，将其舍弃。

2.气压计高度测量漂移的补偿

在声纳工作范围内时，用声纳测量高度补偿气压测量高度中由于空气气压变化导致的测量漂移。采用如下所示的算法。

该算法为显性互补滤波器的构造。其中h_Br为气压传感器测量高度，是消除漂移量后的气压传感器高度测量量，e为测量漂移造成的测量偏差，T_i为选定的时间系数，h_SN是经过错误检测后的超声波传感器高度测量值，是估算出的气压高度测量漂移量。实际中，θ(t)的变化相当缓慢，在对应时间段内可视为近似的恒定值。则通过该漂移补偿算法输出的高度值可近似视为高度真实值与高斯白噪声的叠加。

3：垂直加速度的偏置补偿

经过坐标系变换后获得的垂直加速度包含一定的测量偏置。采用互补滤波器对该测量偏置进行消除，该滤波器结构如图2所示。系统动力学描述如下：

其中，A_Z为无人机垂直加速度输入，b_a为所估计出的垂直加速度偏置，h_SN&Br为声纳测量高度与气压测量高度的输入，V_Z、h分别为垂直速度与高度，分别为V_Z、h的时间导数，K₁、K₂、K₃为待定系数。配置系统的闭环极点为s1＝-1/τ，s2＝-1/τ，s3＝-1/τ,τ为根据实际情况选择的系数：传感器的测量噪声越大，则τ取值越大。对应的滤波器增益为：

K₁＝3/τ,K₂＝3/τ²,K₃＝1/τ³

4：气压计测量高度与垂直加速度的融合

以补偿后的垂直加速度和气压计高度测量值作为输入，应用改进后的卡尔曼滤波器获得对应的垂直速度与高度输出。

无人机垂直方向的运动学描述用状态空间表达式表述如下：

其中，v_B、h_B分别为气压计测量高度与加速度融合所对应的垂直速度与高度输出，为系统状态x的时间导数，A_Zc是补偿后的垂直加速度输入。系统的离散化一阶近似如下：

x(k+1)≈(I+AT_S)x(k)+T_SBu(k)+ζ(k)

＝Γx(k)+Ψu(k)+ζ(k)

u(k)为无人机在k时刻的z轴方向加速度，x(k)为对应k时刻的无人机状态，ζ(k)是系统过程激励噪声，即补偿后的垂直加速度的零均值噪声，对应一个协方差矩阵Q，I是单位矩阵。对于高度测量的测量模型：

y(k+1)＝Hx(k)+μ(k)

H＝[0 1]

其中，y(k+1)为高度的测量输入，H为观测矩阵，μ(k)为气压计高度测量噪声，其均值为零，对应协方差矩阵记为R。

基于离散卡尔曼滤波器的气压高度与垂直加速度的融合实现如下：

时间更新：

状态更新：

其中，对应k时刻的状态估计，u_k-1为k-1时刻的系统垂直加速度输入，是先验协方差，P_k是后验协方差，K_k是卡尔曼增益,上标T表示转置。这样就实现了卡尔曼滤波器的更新循环。

实际应用中，空气中存在着作用短暂的瞬态气压变量，如在室外环境阵风的作用下，密闭室内由于开、关门造成的气压扰动。针对该扰动设计了气压瞬态扰动抵抗机制。该方法的对抗瞬态气压扰动的基本思想是：在扰动发生时，通过增大设定的观测噪声协方差以降低传感器测量的置信度，当扰动作用结束后，恢复观测噪声协方差设定值以恢复其对传感器测量的置信度。

具体实现算法如图2所示。其中，Baro与A_Z分别为卡尔曼滤波器中，气压传感器测量高度与校正后的垂直加速度输入。h_B为其高度输出。Q_Inc为在扰动发生时，所设定的测量噪声增量。Q_Base为所设定的观测噪声基础量。Q为卡尔曼滤波器设定的观测噪声。K_a、K_m为大于零的系数。

工作原理说明如下：对于卡尔曼滤波器工作于理想条件时的情况：气压计测量高度h_Br＝h_r+μ，μ为其测量噪声。滤波器输出高度h_B≈h_r，h_r为高度真值。当瞬态气压扰动发生时，由于测量更新对状态的校正是一个逐步的过程，高度输出h_B的变化在时间上滞后于气压传感器测量值的变化。假设扰动的发生过程是在0时刻瞬间发生的，则：

h_Br＝h_r+μ+Δ_h

其中，Δ_h为扰动造成的测量偏置。则测量噪声增量：

Q_Inc＝sath{∫(K_a|h_r-h_B+Δ_h+μ|-K_m)dt}

sath(x)为自定义的单边饱和函数，X_Max为定义的饱和值上界。以上过程即是根据h_B与h_Br的偏离程度确定Q_Inc。通过选择比例合适的系数K_a、K_m，可使得该扰动未发生时，测量噪声增量在Q_Inc很大概率上为零。而当瞬态扰动发生后，Q_Inc很快达到饱和上限值。当扰动作用结束后，Q_Inc退出饱和并回到原先状态。这样，就降低了扰动发生时，传感器测量对高度估计的影响，从而在一定程度上实现了对扰动的抵抗。

另外，为了防止Q长期处于一个较高的饱和上限值，对Q_Inc的饱和上限值进行处理。每当发生一次瞬态扰动时，减小Q_Inc的饱和值上限。

5：声纳测量高度与垂直加速度的融合

在互补滤波器的基础上，去除了输入补偿的环节。以校正后的垂直加速度A_Z和声纳测量高度h_SN为输入，进行声纳测量高度与垂直加速度的融合。算法系统的动力学描述如下：

其中，h_s为声纳与加速度融合对应高度估计输出，V_Zs为声纳与加速度融合对应的垂直速度输出，分别为h_s、V_Zs的时间导数。系数K₁、K₂：K₁＝3/τ,K₂＝3/τ²。

声纳在大于一定距离后，便无法正常工作。即算法中的输入h_SN在某些时段是不可靠的错误值。对此设计了一套解决方案，实现了该高度融合在声纳不同工作状态来回时的平稳过渡。

基本思想为以下两点：当无人机离开声纳正常工作范围时，暂停该高度融合的进行；当无人机返回声纳的正常工作范围时，启动该高度融合过程的同时进行以下操作：使用卡尔曼滤波器的垂直速度输出更新垂直速度状态V_Zs；使用超声波传感器测量信息更新高度状态h_s与其对应的状态缓存；暂停垂直加速度偏置补偿过程中的气压传感器测量信息的输入，仅开放超声波传感器测量信息的输入。

6：高度融合信息的输出

对卡尔曼滤波器与互补滤波器的输出按照各自对应的权重进行线性组合，获得最终的高度输出H_Out与垂直速度输出V_Out。互补滤波器的输出(声纳与垂直加速度的融合数据)的权重k_s随着高度的增加而减小，卡尔曼滤波器的输出(气压计与垂直加速度的融合数据)的权重相反。具体实现如下：

H_Out＝k_sh_s+(1-k_s)h_B

V_Out＝k_sV_Zs+(1-k_s)v_B

其中，k_s满足如下关系：

k_s＝(H_Max-H_SN)/H_SN if(h_B≤H_Max)

k_s＝0 if(h_B＞H_Max)

下面结合附图和具体实施方式进一步详细说明本发明。

本发明属于微小型旋翼式无人飞行器自主飞行控制研究领域，主要针对旋翼式如无人机的高度与垂直速度的实时检测。

本发明采用气压计、加速度计、声纳作为高度检测元件，设计了一套高度检测方法，成功地实现了精确、实时的高度信息的获取。包含以下具体几个步骤：

1)对声纳的测量高度进行有效性检测和错误检测

当无人机高度较高，离开了声纳的有效工作范围时，声纳测量获得的高度为一个不稳定的错误值，需要对该情况进行判断并取消此时的声纳高度测量数据的输入使能。判断逻辑方法如下所示：

if(H_Out＞H_Max)||(H_SN＞H_Max)

SnInputUnable←True

其中，H_Max为所声纳有效工作距离极限，H_Out为最终的高度融合输出，H_SN为声纳获得的高度测量，SnInputUnable为声纳高度测量数据的有效性标志。

另外，针对读入的测量尖刺错误采用如下方法进行检测。当满足以下条件时取消此时的测量数据输入使能：

其中，Δ_h为当前测量值与上一次测量值之差，V_Out为垂直速度的融合输出，f_SN为根据无人机高度近似获得声纳工作频率，Δ_hBase为所加入的判断裕量。

2)气压计测量漂移的补偿

利用气压计进行高度测量时，空气气压变化造成高度测量漂移可达到1m左右。当无人机高度较低，声纳处于有效工作状态时，相对于声纳的测量高度，气压计的测量高度参考价值较低。在声纳工作处于有效工作状态时对气压计测量漂移进行补偿能解决这一问题，同时为无人机的高度过渡提供了方便。算法如下式所示：

其中h_Br为气压传感器测量高度，是消除漂移量后的气压传感器高度测量量，e为测量漂移造成的测量偏差，T_i为选定的时间系数，是经过错误检测后的超声波传感器高度测量值，是估算出的气压高度测量漂移量。垂直加速度的偏置补偿

通过互补滤波器对垂直加速度进行补偿。算法结构图如图2所示。

3)气压计测量高度与垂直加速度的融合

气压计测量高度与垂直加速度的融合采用卡尔曼滤波器的形式实现。

无人机垂直方向的运动学描述用状态空间表达式表述如下：

其中，v_B、h_B分别为气压计测量高度与加速度融合所对应的垂直速度与高度输出，为系统状态x的时间导数。A_Zc是补偿后的垂直加速度输入。系统的离散化一阶近似如下：

x(k+1)≈(I+AT_S)x(k)+T_SBu(k)+ζ(k)

＝Γx(k)+Ψu(k)+ζ(k)

u(k)为无人机在k时刻的z轴方向加速度，x(k)为对应k时刻的无人机状态，T_S为选定的时间步长，ζ(k)是系统过程激励噪声，I是单位矩阵，即补偿后的垂直加速度的零均值噪声，对应一个协方差矩阵Q，对于高度测量的测量模型：

y(k+1)＝Hx(k)+μ(k)

H＝[0 1]

其中，y(k+1)为高度的测量输入，H为观测矩阵，μ(k)为气压计高度测量噪声，其均值为零，对应协方差矩阵记为R。

基于离散卡尔曼滤波器的气压高度与垂直加速度的融合实现如下：

时间更新：

状态更新：

其中，对应k时刻的状态估计，u_k-1为k-1时刻的系统垂直加速度输入，是先验协方差，P_k是后验协方差，K_k是卡尔曼增益,上标T表示转置。这样就实现了卡尔曼滤波器的更新循环。

针对瞬态气压扰动提出了基于观测噪声调节的抵抗机制，算法如图3所示。

4)声纳测量高度与垂直加速度的融合

采用简化的互补滤波器对声纳测量高度与垂直加速度进行融合，融合算法结构图如图4所示。

6)传感器测量延迟的处理

加速度计获得一个高频的近乎无延迟的加速度信息。气压计和声纳的工作频率较低，且由于较低的数据处理优先级，其测量信息包含了一定的时间延迟，而非当前时刻的真实值。

针对高度状态估计设立缓冲区，存储历史高度估计信息。实例中，垂直加速度频率为1kHz。该缓冲区中相邻间隔的数据对应的时间差为加速度信息更新的时间间隔，即1ms。假设在k时刻应用气压计和声纳高度测量信息对状态进行校正时，测量量分别包含了n和m毫秒的时间延迟，则分别参照(k-n)和(k-m)时刻对应的状态对当前时刻状态进行校正

下面给出具体的实例：

一、系统硬件连接及配置

本发明的基于多传感器信息融合的小型旋翼无人机高度测量系统，主要由加速度计、气压计、声纳、微处理器组成，所搭建的实验平台包括四旋翼无人机本体、地面站、遥控器等。

加速度计采用了MPU6000运动处理传感器，气压计采用MEAS的MS5611微型气压计，声纳选用型号为URM37V2.0(有效测量距离范围为4m)，微处理器选用ST推出的STM32F405芯片。

二、实验结果

本实施例对上述实验平台进行了多组基于融合信息的无人机高度镇定实验。

基于融合输出信息(超声波传感器与加速度的融合输出占了最终融合输出信息的全部比重)，在室内1.7m高度进行无人机的高度镇定实验。实验结果如图5所示：子图1红线对应声纳高度测量值，蓝线对应高度融合输出；子图2为垂直速度的估计输出。高度镇定精度达到±3cm。

基于融合输出信息(超声波传感器与加速度的融合输出占了最终融合输出信息约一半的比重)，在户外2.8m处进行无人机高度镇定实验，结果如图6所示。高度镇定精度达到±8cm。

基于融合输出信息(高度信息融合中，屏蔽超声波传感器测量输入)，在户外进行无人机高度镇定实验，结果如图7所示。室外无超声波传感器作用时，高度镇定实验也能达到一个较好的效果。

针对瞬态气压扰动的抵抗方法，进行了如下实验：在密闭室内，基于融合输出信息(高度信息融合中，屏蔽超声波传感器测量输入)进行无人机高度镇定实验，并在实验中加入开、关门造成的气压瞬态扰动。实验结果如图8所示。图中，上、下两部分分别为融合输出的高度信息与气压传感器高度测量信息。其中上线波动大的时刻即为加入开、关门气压瞬态扰动的时刻。图9为模式的选择：其值为1时，在其对应时间段的高度融合中加入了瞬态抗扰机制。可见，该瞬态扰动抵抗机制能显著地抵抗瞬态气压扰动对基于气压的高度测量的影响。

Claims (9)

1.一种基于多传感器信息融合的小型旋翼无人机高度测量方法，其特征是，利用安装在旋翼无人机底部的声纳采集其与地面间的距离作为声纳测量高度；通过加速度计获取无人机对象坐标系下的三轴加速度，结合无人机的姿态角信息进行坐标系变换，获得大地参考系下的垂直加速度；气压计测量无人机所处高度的气压信息，比较地面的气压信息，代入标准大气模型获得通过基于气压测量的高度测量信息；利用以上的声纳测量高度、无人机垂直加速度、气压计测量高度作为输入，应用多传感器信息融合的高度测量方法，并通过声纳高度测量信息的错误检测，气压计高度测量的漂移补偿，垂直加速度的偏置补偿，声纳测量高度与垂直加速度的融合，气压计测量高度与垂直加速度的融合，得到一个高频的实时高度与垂直速度信息。

2.如权利要求1所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特征是，还包括声纳高度测量信息的错误检测步骤：

声纳测距工作频率随距离的增大而减小，视其随距离增大呈线性衰减关系如下：

f_SN＝f_Max-kh_SN

其中f_SN为声纳在所在高度的工作频率，f_Max为其在探测距离下限时所对应的最大工作频率，k为其工作频率随距离变化的衰减系数，h_SN为声纳处于可靠工作状态时测得的无人机高度，考虑无人机的垂直速度V_Z与声纳工作频率，则声纳两次高度测量值之差△_h满足如下等式：

$\left|{\mathrm{\Δ}}_h\right|\≈\left|\frac{V_Z}{f_{SN}}\right|$

考虑最终的垂直速度融合输出V_Out，加入一定的判断裕量△_hBase，若声纳两次的高度测量差满足以下式子，则视更新的声纳测量高度值为不可靠的错数，将其舍弃，

$\left|{\mathrm{\Δ}}_h\right|>\left|\frac{V_{Out}}{f_{SN}}\right|+{\mathrm{\Δ}}_{h}Base.$

3.如权利要求1所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特征是，还包括气压计高度测量漂移的补偿步骤：在声纳工作范围内时，用声纳测量高度补偿气压测量高度中由于空气气压变化导致的测量漂移，采用如下所示的算法：

$e=\widehat{h_{Br}}-h_{SNC}$

$\widehat{\mathrm{\θ}}\left(t\right)=\frac{1}{T_i}\∫edt$

${\hat{h}}_{Br}=h_{Br}-\widehat{\mathrm{\θ}}\left(t\right)$

该算法为显性互补滤波器的构造，其中h_Br为气压传感器测量高度，是消除漂移量后的气压传感器高度测量量，e为测量漂移造成的测量偏差，T_i为选定的时间系数，h_SNC是经过错误检测后的声纳传感器高度测量值，是估算出的气压高度测量漂移量，的变化相当缓慢，在对应时间段内可视为近似的恒定值，则通过前述显性互补滤波器的构造的算法输出的高度值近似视为高度真实值与高斯白噪声的叠加。

4.如权利要求1所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特征是，还包括垂直加速度的偏置补偿步骤：经过坐标系变换后获得的垂直加速度包含一定的测量偏置，采用互补滤波器对该测量偏置进行消除，系统动力学描述如下：

其中，A_Z为无人机垂直加速度输入，b_a为所估计出的垂直加速度偏置，h_SN&Br为声纳测量高度与气压测量高度的输入，V_Z、h分别为垂直速度与高度，分别为V_Z、h的时间导数，K₁、K₂、K₃为待定系数，配置系统的闭环极点为s1＝-1/τ，s2＝-1/τ，s3＝-1/τ,τ为根据实际情况选择的系数，对应的滤波器增益为：

K₁＝3/τ,K₂＝3/τ²,K₃＝1/τ³。

5.如权利要求1或4所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特征是，还包括气压计测量高度与垂直加速度的融合步骤：以补偿后的垂直加速度和气压计高度测量值作为输入，应用改进后的卡尔曼滤波器获得对应的垂直速度与高度输出：

无人机垂直方向的运动学描述用状态空间表达式表述如下：

$\stackrel{\·}{x}=Ax+Bu$

$\begin{array}{cccc}x=\left[\begin{array}{c}{v}_B\\ h_B\end{array}\right]& A=\left[\begin{array}{cc}0& 0\\ 1& 0\end{array}\right]& B=\left[\begin{array}{c}1\\ 0\end{array}\right]& u=A_Z-b_a\end{array}$

其中，v_B、h_B分别为气压计测量高度与加速度融合所对应的垂直速度与高度输出，为系统状态x的时间导数，A_Z-b_a是补偿后的垂直加速度输入，系统的离散化一阶近似如下：

x(k+1)≈(I+AT_S)x(k)+T_SBu(k)+ζ(k)

＝Γx(k)+Ψu(k)+ζ(k)

$\begin{array}{cc}\mathrm{\Γ}=\left[\begin{array}{cc}1& 0\\ T_S& 1\end{array}\right]& \mathrm{\Ψ}=\left[\begin{array}{c}{T}_S\\ 0\end{array}\right]\end{array}$

u(k)为无人机在k时刻的z轴方向加速度，x(k)为对应k时刻的无人机状态，T_S为选定的时间步长，ζ(k)为系统过程激励噪声，即补偿后的垂直加速度的零均值噪声，其对应一个协方差矩阵Q，I是单位矩阵，对于高度测量的测量模型：

y(k+1)＝Hx(k)+μ(k)

H＝[0 1]

其中，y(k+1)为高度的测量输入，H为观测矩阵，μ(k)为气压计高度测量噪声，

其均值为零，对应协方差矩阵记为R。

6.如权利要求5所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特征是，基于卡尔曼滤波器的气压计测量高度与垂直加速度的融合实现如下：

时间更新：

${\hat{x}}_k=\mathrm{\Γ}{\hat{x}}_{k-1}+{\mathrm{\Ψu}}_{k-1}$

$P_k^{-}={\mathrm{\ΓP}}_{k-1}{\mathrm{\Γ}}^T+Q$

状态更新：

$K_k=P_k^{-}{H}^T{({\mathrm{HP}}_k^{-}{H}^T+R)}^{-1}$

${\hat{x}}_k={\hat{x}}_k+K_k(y_k-H{\hat{x}}_k)$

$P_k=(I-K_{k}H)P_k^{-1}$

其中，对应k时刻的状态估计，u_k-1为k-1时刻的系统垂直加速度输入，是先验协方差，P_k是后验协方差，K_k是卡尔曼增益，上标T表示转置，这样就实现了卡尔曼滤波器的更新循环。

7.如权利要求4所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特征是，还包括声纳测量高度与垂直加速度的融合步骤：在互补滤波器的基础上，去除了输入补偿的环节，以补偿后的垂直加速度A_Z-b_a和经过错误检测后的声纳高度测量信息h_SNC为输入，进行声纳测量高度与垂直加速度的融合，算法系统的动力学描述如下：

${\stackrel{\·}{h}}_s=V_{Zs}-K_1(h_s-h_{SNC})$

${\stackrel{\·}{V}}_{Zs}=A_Z-b_a-K_2(h_s-h_{SNC})$

其中，h_s为声纳测量高度与垂直加速度融合对应的高度估计输出，V_Zs为声纳测量高度与垂直加速度融合对应的垂直速度输出，分别为h_s、V_Zs的时间导数，系数K₁、K₂：K₁＝3/τ,K₂＝3/τ²，τ为根据实际情况选择的系数；

针对输入h_SNC在某些时段是不可靠的错误值，解决方法如下：当无人机离开声纳正常工作范围时，暂停声纳测量高度与垂直加速度融合的进行；当无人机返回声纳的正常工作范围时，启动声纳测量高度与垂直加速度融合过程的同时进行以下操作：使用卡尔曼滤波器的垂直速度输出更新垂直速度状态V_Zs；使用声纳传感器测量信息更新高度状态h_s与其对应的状态缓存；暂停垂直加速度偏置补偿过程中的气压计传感器测量信息的输入，仅开放声纳传感器测量信息的输入。

8.如权利要求5所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特征是，还包括高度融合信息的输出步骤：对卡尔曼滤波器与互补滤波器的输出按照各自对应的权重进行线性组合，获得最终的高度与垂直速度输出信息；互补滤波器的输出即声纳测量高度与垂直加速度的融合数据的权重随着高度的增加而减小，卡尔曼滤波器的输出即气压计测量高度与垂直加速度的融合数据的权重相反。

9.如权利要求6所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特征是，针对气压瞬态扰动解决办法是：在扰动发生时，通过增大设定的观测噪声协方差以降低传感器测量的置信度，当扰动作用结束后，恢复观测噪声协方差设定值以恢复其对传感器测量的置信度，具体实现算法：h_B为所述卡尔曼滤波器的高度融合输出，R_Inc为在扰动发生时，所设定的测量噪声增量，R_Base为所设定的观测噪声基础量，R为卡尔曼滤波器设定的观测噪声，K_a、K_m为大于零的系数；工作原理说明如下：对于卡尔曼滤波器工作于理想条件时的情况：气压计测量高度h_Br＝h_r+μ，μ为测量噪声；滤波器输出高度h_B≈h_r，h_r为高度真值，当瞬态气压扰动发生时，由于测量更新对状态的校正是一个逐步的过程，高度输出h_B的变化在时间上滞后于气压传感器测量值的变化，假设扰动的发生过程是在0时刻瞬间发生的，则：

h_Br＝h_r+μ+δ_h

其中，δ_h为扰动造成的测量偏置，则测量噪声增量：

R_Inc＝sath{∫(K_a|h_r-h_B+δ_h+μ|-K_m)dt}

$sath\left(x\right)=\left\{\begin{array}{ccc}{X}_{Max}& if& (x\&GreaterEqual;x_{Max})\\ x& if& (0<x<X_{Max})\\ 0& if& (x\&le;0)\end{array}\right.$

sath(x)为自定义的单边饱和函数，X_Max为设定的饱和值上界，以上过程即是根据h_B与h_Br的偏离程度确定R_Inc，通过选择比例合适的系数K_a、K_m，使得该扰动未发生时，测量噪声增量R_Inc为零，而当瞬态扰动发生后，R_Inc很快达到饱和上限值，当扰动作用结束后，R_Inc退出饱和并回到原先状态，这样，就降低了扰动发生时，传感器测量对高度估计的影响，从而在一定程度上实现了对扰动的抵抗，

另外，为了防止R_Inc长期处于一个较高的饱和上限值，对R_Inc的饱和上限值进行处理，每当发生一次瞬态扰动时，减小R_Inc的饱和值上限。