型旋翼无人机高度测量方法，其特 征是，还包括垂直加速度的偏置补偿步骤：经过坐标系变换后获得的垂直加速度包含一定 的测量偏置，采用互补滤波器对该测量偏置进行消除，系统动力学描述如下：
其中，Az为无人机垂直加速度输入，ba为所估计出的垂直加速度偏置，hSN&to为声纳测 量高度与气压测量高度的输入，Vz、h分别为垂直速度与高度，^分别为Vz、h的时间导 数，Ki、K2、K3为待定系数，配置系统的闭环极点为si= -1/t，s2 = -1/t，s3 = -1/t，T为根据实际情况选择的系数，对应的滤波器增益为： Kj= 3/ X , K 2= 3/ T 2, K3= 1/ t3〇
5. 如权利要求1所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特 征是，还包括气压计测量高度与垂直加速度的融合步骤：以补偿后的垂直加速度和气压计 高度测量值作为输入，应用改进后的卡尔曼滤波器获得对应的垂直速度与高度输出： 无人机垂直方向的运动学描述用状态空间表达式表述如下：
其中，vB、hB分别为气压计测量高度与加速度融合所对应的垂直速度与高度输出，^为 系统状态x的时间导数，Azc是补偿后的垂直加速度输入，系统的离散化一阶近似如下：
u(k)为无人机在k时刻的z轴方向加速度，x(k)为对应k时刻的无人机状态，Ts为选 定的时间步长，（（k)为系统过程激励噪声，S卩补偿后的垂直加速度的零均值噪声，其对应 一个协方差矩阵Q，I是单位矩阵，对于高度测量的测量模型： y(k+l) =Hx(k) +ii(k) H = [0 1] 其中，y(k+l)为高度的测量输入，H为观测矩阵，y(k)为气压计高度测量噪声，其均值 为零，对应协方差矩阵记为R。
6. 如权利要求1所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特 征是，基于离散卡尔曼滤波器的气压高度与垂直加速度的融合实现如下： 时间更新：
状态更新：
其中，i+对应k时刻的状态估计，UgSk-1时刻的系统垂直加速度输入，是先验协 方差，Pk是后验协方差，Kk是卡尔曼增益，上标T表示转置，这样就实现了卡尔曼滤波器的 更新循环。
7. 如权利要求1所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特 征是，还包括声纳测量高度与垂直加速度的融合步骤：在互补滤波器的基础上，去除了输入 补偿的环节。以校正后的垂直加速度4和声纳测量高度hSN为输入，进行声纳测量高度与 垂直加速度的融合，算法系统的动力学描述如下：
其中，匕为声纳与加速度融合对应的高度估计输出，VZs为声纳与加速度融合对应的垂 直速度输出，分别为hs、VZs的时间导数，系数KrUf3/t，K2= 3/t2; 针对输入hSN在某些时段是不可靠的错误值，解决方法如下：当无人机离开声纳正常工 作范围时，暂停该高度融合的进行；当无人机返回声纳的正常工作范围时，启动该高度融合 过程的同时进行以下操作：使用卡尔曼滤波器的垂直速度输出更新垂直速度状态VZs;使用 超声波传感器测量信息更新高度状态匕与其对应的状态缓存；暂停垂直加速度偏置补偿过 程中的气压传感器测量信息的输入，仅开放超声波传感器测量信息的输入。
8. 如权利要求1所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特 征是，还包括高度融合信息的输出步骤：对卡尔曼滤波器与互补滤波器的输出按照各自对 应的权重进行线性组合，获得最终的高度与垂直速度输出信息；互补滤波器的输出即声纳 与垂直加速度的融合数据的权重随着高度的增加而减小，卡尔曼滤波器的输出即气压计与 垂直加速度的融合数据的权重相反。
9. 如权利要求1所述的基于多传感器信息融合的小型旋翼无人机高度测量方法，其特 征是，针对气压瞬态扰动解决办法是：在扰动发生时，通过增大设定的观测噪声协方差以降 低传感器测量的置信度，当扰动作用结束后，恢复观测噪声协方差设定值以恢复其对传感 器测量的置信度，具体实现算法：Baro与Az分别为卡尔曼滤波器中，气压传感器测量高度 与校正后的垂直加速度输入，&为其高度输出，QIn。为在扰动发生时，所设定的测量噪声增 量，QBase为所设定的观测噪声基础量，Q为卡尔曼滤波器设定的观测噪声，Ka、Km为大于零的 系数；工作原理说明如下：对于卡尔曼滤波器工作于理想条件时的情况：气压计测量高度 h&=h，il，il为其测量噪声；滤波器输出高度hB?hpb为高度真值，当瞬态气压扰动发 生时，由于测量更新对状态的校正是一个逐步的过程，高度输出匕的变化在时间上滞后于 气压传感器测量值的变化，假设扰动的发生过程是在0时刻瞬间发生的，则： hBr=hr+u+Ah 其中，Ah为扰动造成的测量偏置，则测量噪声增量： QInc =sath{ /(KJhr-hB+Ah+u| -Km)dt}
sath(x)为自定义的单边饱和函数，XMax为设定的饱和值，以上过程即是根据&与1的 偏离程度确定QIn。，通过选择比例合适的系数Ka、Km，使得该扰动未发生时，测量噪声增量在 QIn。很大概率上为零，而当瞬态扰动发生后，QIn。很快达到饱和上限值，当扰动作用结束后， QIn。退出饱和并回到原先状态，这样，就降低了扰动发生时，传感器测量对高度估计的影响， 从而在一定程度上实现了对扰动的抵抗， 另外，为了防止Q长期处于一个较高的饱和上限值，对QIn。的饱和上限值进行处理，每 当发生一次瞬态扰动时，减小QIn。的饱和值上限。
【专利摘要】本发明属于小型旋翼无人机自主飞行控制研究领域，为提出一套适应户内、外多种环境的小型旋翼无人机高度测量方法，为无人机提供实时、精准、可靠的高度信息，本发明采取的技术方案是，基于多传感器信息融合的小型旋翼无人机高度测量方法，利用安装在旋翼无人机底部的声纳采集其与地面间的距离作为声纳测量高度；通过加速度计获取无人机对象坐标系下的三轴加速度，获得大地参考系下的垂直加速度；气压计测量无人机所处高度的气压信息，比较地面的气压信息，代入标准大气模型获得通过基于气压测量的高度测量信息；利用以上输入，应用多传感器信息融合的高度测量方法，得到一个高频的实时高度与垂直速度信息。本发明主要应用于无人机自主飞行控制。
【IPC分类】G01C5-06, G01C5-00
【公开号】CN104567799
【申请号】CN201410709091
【发明人】鲜斌, 郑国周
【申请人】天津大学
【公开日】2015年4月29日
【申请日】2014年11月28日