一种融合相对高度和绝对高度获得导航高度的方法
【专利摘要】一种融合相对高度和绝对高度获得导航高度的方法，它有五大步骤：一、采用相对高度测量计，获取飞行器距离地面的相对高度hrelat；二、采用绝对高度测量计，获取飞行器的绝对高度habs；三、根据相对高度hrelat的大小，确定导航高度来源；四、若上述步骤三确定导航高度来源为绝对高度habs，使用绝对高度habs计算导航高度hNavi；五、若上述步骤三确定导航高度来源为相对高度hrelat，使用相对高度hrelat和地面海拔高度hground计算导航高度hNavi，然后计算并记录导航高度hNavi与绝对高度habs的差值hoffset。本发明提高了导航高度的可靠性和准确性；飞行器近地飞行时，跟随上升地形飞行，而跟随下降地形飞行时可及时切出并返回到期望高度，从而保障飞行安全。
【专利说明】一种融合相对高度和绝对高度获得导航高度的方法
【技术领域】
[0001] 本发明涉及一种融合相对高度和绝对高度获得导航高度的方法，它是用于获得无 人飞行器导航高度的方法，通过融合相对高度和绝对高度，得到精确可靠的导航高度，主要 用于无人飞行器导航控制【技术领域】。
【背景技术】
[0002] 无人飞行器的飞行高度获取方法包括使用绝对高度测量计获取绝对高度，使用相 对高度测量计获取相对高度。绝对高度测量计包括GPS (Global Positioning System)定位 系统、气压高度计等仪器；相对高度测量计包括超声波高度测量计、激光高度测量计、无线 电高度测量计等仪器。
[0003] 绝对高度测量计测量范大，可在飞行器整个升限内使用。但是在靠近地面时，易受 周围建筑物、树木、天气变化等因素影响而使测量精度降低甚至失效，因此使用绝对高度直 接作为导航高度将给飞行控制系统带来极大地不确定性和不可靠性，尤其在无人飞行器的 起飞和着陆阶段，绝对高度无法满足飞行控制系统对高度通道的控制要；而且，绝对高度无 法反映地面海拔高度的变化情况，因此使用绝对高度导航，无人飞行器无法获得距离地面 的相对高度数据，从而无法保障近地飞行时的飞行安全。
[0004] 相对高度测量计可以获取飞行器距离地面的相对高度数据，但是测量范围小，在 飞行器远离地面飞行时，相对高度会超出相对高度测量计的量程范围，造成相对高度无法 作为导航高度使用。
[0005] 综合上述原因，无人飞行器通常同时搭载绝对高度测量计和相对高度测量计，获 取绝对高度和相对高度共同作为导航高度。但是绝对高度和相对高度适用于导航的高度范 围不同，物理形式也不同，两者共同作为导航高度时，会给飞行控制系统带来以下问题。
[0006] -是导航高度来源切换的问题，无人飞行器从起飞到着陆，期间会执行一定的飞 行任务，飞行控制系统需要根据飞行任务执行高度选择使用相对高度还是绝对高度作为导 航高度。如果飞行任务执行高度距离地面较近，飞行过程中相对高度都在相对高度测量计 量程范围以内，可以选用相对高度作为导航高度；如果飞行任务执行高度距离地面较远，飞 行过程中相对高度在相对高度测量计量程范围以外，则必须选用绝对高度作为导航高度； 如果飞行任务执行高度同时包括近地面和远地面，在飞行过程中相对高度既有在相对高度 测量计量程范围内的阶段，也有在相对高度测量计量程范围外的阶段，则需要判断何时使 用相对高度何时使用绝对高度。
[0007] 二是相对高度和绝对高度的物理形式不相同的问题，相对高度是指飞行器相对地 面的垂直距离，绝对高度是指飞行器相对平均海平面的垂直距离，也叫"海拔高度"，在数值 上差距较大，因此在相对高度和绝对高度共同作为导航高度时，需要将两者的物理形式统 一：如果导航高度统一采用绝对高度形式，应将用于导航的相对高度转换为绝对高度；如 果导航高度统一采用相对高度形式，则应将用于导航的绝对高度转换为相对高度。否则将 导致导航高度的物理形式不明确，使飞行控制系统使用导航高度时发生错误，引发飞行事 故。例如，飞行控制系统中的飞行期望高度为相对高度形式，当前值为l〇〇m，如果导航高度 为绝对高度形式，当前值为500m，那么飞行控制系统将做出当前飞行高度远大于飞行期望 高度的错误判断，从而使无人飞行器高度快速下降，如果此时地面海拔高度大于l〇〇m，就会 发生触地坠毁的事故。
[0008] 三是导航高度在相对高度和绝对高度之间切换时存在高度跳变问题，不同的高度 测量计测量精度和测量范围存在差别，因此，即使相对高度和绝对高度作为导航高度时转 换为相同物理形式，但两者提供的高度数值仍存在不同。如果将来源于相对高度的导航高 度和来源于绝对高度的导航高度直接切换，将引起导航高度值的跳变，从而使飞行控制系 统输出的控制量发生跳变，引起飞行器的飞行高度和飞行姿态产生较大变化，甚至引发飞 行事故。
[0009] 本发明通过融合相对高度和绝对高度，解决了将相对高度和绝对高度共同作为导 航高度时存在的上述问题。
[0010] 首先，明确了导航高度的切换条件，在相对高度测量计量程范围内确定切换上限 和切换下限，相对高度大于切换上限时，导航高度使用绝对高度，相对高度小于切换下限 时，导航高度使用相对高度，切换上限和切换下限之间的区域作为缓冲区，相对高度位于此 区间时，导航高度来源与进入该区间之前的高度来源相同，不发生切换，从而避免了因高度 测量误差造成的导航高度的频繁切换。
[0011] 其次，统一了导航高度的物理形式，导航高度来源无论是相对高度还是绝对高度， 导航高度都为绝对高度的物理形式，其中导航高度来源为相对高度时，通过相对高度加地 面海拔高度的方法使导航高度转换为绝对高度形式；因此，飞行控制系统中使用的其他高 度（如期望高度）可以统一采用绝对高度形式，避免了因物理形式不同而发生的导航事故。
[0012] 再次，设计了高度平滑功能，使导航高度来源切换时，导航高度可以平稳过渡，不 会发生跳变。高度平滑功能通过适时调用高度平滑函数实现，在导航高度来源由相对高度 切换为绝对高度时，调用高度平滑函数，逐步消除使用相对高度时的导航高度与绝对高度 的差值，使切换平稳过渡；同时，在导航高度来源为相对高度时，若地面海拔高度被重新计 算，同样调用高度平滑函数，逐步消除前后地面海拔高度的差值，防止导航高度跳变。
[0013] 最后，设计了地面海拔高度的更新机制，在导航高度来源由绝对高度切换为相对 高度的周期，计算地面海拔高度，用于导航高度的计算；同时在导航高度来源为相对高度 时，如果当前地面海拔高度与用于导航高度计算的地面海拔高度的差值超过设定阈值，则 重新计算地面海拔高度。以上两种条件下对地面海拔高度的计算统称为地面海拔高度更新 机制，通过更新地面海拔高度，可使飞行器的导航高度以绝对高度为真值得到校正，从而有 利于导航高度来源由相对高度向绝对高度切换时的快速平稳过渡；而且，地面海拔高度的 更新可使飞行器及时停止跟随下降地形飞行，返回到期望高度。
【发明内容】
[0014] 本发明要解决的技术问题是：提供一种融合相对高度和绝对高度获得导航高度 的方法，使飞行控制系统使用导航高度导航时无需判断飞行器所处高度和飞行任务执行高 度，也无需考虑不同高度来源时导航高度的物理形式不一致和高度跳变问题；同时，导航高 度兼具相对高度和绝对高度的优点，提高导航高度的精确可靠性。
[0015] 本发明解决其技术问题所采用的技术方案是：一种融合相对和绝对高度获得导航 高度的方法，其特征在于，需要以下设备条件：
[0016] (1)相对高度测量计，用于获取飞行器距离地面的相对高度，相对高度测量计可以 是超声波高度测量计、激光高度测量计、无线电高度测量计等任何可获得飞行器距离地面 相对高度的传感器；
[0017] (2)绝对高度测量计，用于获取飞行器的绝对高度，绝对高度测量计可以是 GPS(Global Positioning System)定位系统、气压高度计等任何可获得飞行器绝对高度的 传感器；
[0018] (3)导航计算机，用于接收上述相对高度和绝对高度，并根据相对高度的大小确定 导航高度来源，通过融合相对高度和绝对高度获得导航高度。
[0019] 一种融合相对高度和绝对高度获得导航高度的方法，其特征在于，它包括以下步 骤：
[0020] 步骤一：采用相对高度测量计，获取飞行器距离地面的相对高度hMlat ;
[0021] 步骤二：采用绝对高度测量计，获取飞行器的绝对高度habs;
[0022] 步骤三：根据相对高度hMlat的大小，确定导航高度来源，该步骤的实施需要以下 条件：
[0023] 在相对高度测量计的量程范围内，设定导航高度来源切换上限hmax limit和切换下 限 hmin-iimit ;
[0024] 确定导航高度来源的具体方法为：若相对高度hMlat大于导航高度来源切换上限 hmax_limit，导航高度来源选择为绝对高度；若相对高度hMlat小于导航高度来源切换下限h min limit，导航高度来源选择为相对高度；若相对高度hMlat位于导航高度来源切换下限h min limit 和切换上限hmax limit之间，导航高度来源与上一周期导航高度来源相同，不发生变化；
[0025] 步骤四：若上述步骤三确定导航高度来源为绝对高度habs，使用绝对高度h abs计算 导航高度hNavi ;
[0026] 步骤五：若上述步骤三确定导航高度来源为相对高度hMlat，使用相对高度h Mlat和 地面海拔高度hgromd计算导航高度hNavi，然后计算并记录导航高度h Navi与绝对高度habs的差 值h offset °
[0027] 其中，步骤四中所述的"使用绝对高度habs计算导航高度hNavi"，其具体计算步骤 为：
[0028] (4. 1)判断上一周期的导航高度来源是否为相对高度hrelat ;
[0029] (4. 2)若步骤（4. 1)确定上一周期的导航高度来源为相对高度h,elat，启用高度平 滑函数，减小导航高度hNavi与绝对高度h abs的差值hf#，将高度平滑函数返回值赋值给变 量;否则，不启用高度平滑函数；这里应注意，"启用"高度平滑函数是指"开始调 用"高度平滑函数，在变量h s_th ()ffsrt为0时，停止调用高度平滑函数；
[0030] (4. 3)使用绝对高度habs和变量hs_th计算导航高度hNavi。
[0031] 其中，步骤（4. 2)中所述的"高度平滑函数"，其具体形式如下： //? ^ fhan(x^ (k) - v(k), x2 (/〇, /% h,)
[0032] < .v, (^ + 1) = .v, (k) + h- x2(k) (1) χ?(Ι? + \ ) = χ?(Ι?) + h · fh
[0033] 公式（1)中函数 fhanUi (k)-v(k)，x2 (k)，r, h)的表达式如下：
[0034]
【权利要求】
1. 一种融合相对高度和绝对高度获得导航高度的方法，其特征在于：它包括以下步 骤： 步骤一：采用相对高度测量计，获取飞行器距离地面的相对高度hMlat ; 步骤二：采用绝对高度测量计，获取飞行器的绝对高度habs; 步骤三：根据相对高度hMlat的大小，确定导航高度来源，该步骤的实施需要以下条件： 在相对高度测量计的量程范围内，设定导航高度来源切换上限hmaxlimit和切换下限hmin limit ? 确定导航高度来源的具体方法为：若相对高度hMlat大于导航高度来源切换上限hmax limit，导航高度来源选择为绝对高度；若相对高度hMlat小于导航高度来源切换下限hmin limit， 导航高度来源选择为相对高度；若相对高度hMlat位于导航高度来源切换下限h min limit和切 换上限hmax limit之间，导航高度来源与上一周期导航高度来源相同，不发生变化； 步骤四：若上述步骤三确定导航高度来源为绝对高度habs，使用绝对高度habs计算导航 咼度hNavi ; 步骤五：若上述步骤三确定导航高度来源为相对高度hMlat，使用相对高度hMlat和地面 海拔高度hgMund计算导航高度hNavi，然后计算并记录导航高度hNavi与绝对高度h abs的差值 h〇ffset ?
2. 根据权利要求1所述的一种融合相对高度和绝对高度获得导航高度的方法，其特征 在于：步骤四中所述的"使用绝对高度habs计算导航高度h Navi"，其具体计算步骤为： (4. 1)判断上一周期的导航高度来源是否为相对高度hMlat ; (4. 2)若步骤（4. 1)确定上一周期的导航高度来源为相对高度hMlat，启用高度平滑函 数，减小导航高度hNavi与绝对高度habs的差值，将高度平滑函数返回值赋值给变量 h s_thjfset ;否则，不启用高度平滑函数；这里应注意，"启用"高度平滑函数是指"开始调用" 高度平滑函数，在变量hs_ th ()ffsrt为0时，停止调用高度平滑函数； 所述的"高度平滑函数"，其具体形式如下：
(1) 公式⑴中函数fhan(x1(k)-v(k)，x2(k)，r，h1)的表达式如下 ： 公式（2)中函数fsg(x，d)的表达式如下： (2) (4) y = fsg(x, d) = (sign (x+d)-sign (x-d) )/2 (3) 公式（2) (3)中函数sign(x)为符号函数，具体表达式如下： 其中，fh = fhan h (k) -V (k)，x2 (k)，r, h)是二阶离散系统
的最速控制函数，作用是使变量^00以有限步从其初始值Xl(0)到达期望值v(k)并 跟踪期望值v(k)，v(k)既可以是变量也可以是常量；公式（1)(2)中主要参数的定义如下： h为尚散系统的积分步长； r为快速因子，r取值越大，变量Xl (k)对期望值v (k)的跟踪速度越快； 4为滤波因子，h取值越小，变量Xl(k)对期望值v(k)的跟踪速度越快； 公式（1) (2)用作高度平滑函数时，在第一次被调用时，将Xl(k)初始值Xl(0)赋值为步 骤五中"计算并记录的导航高度hNavi与绝对高度habs的差值，期望值v (k)为常量0 ; 因此调用高度平滑函数，将使导航高度hNavi与绝对高度habs的差值hf#以有限步减小到 〇,高度平滑函数的返回值为 Xl(k); (4. 3)使用绝对高度habs和变量h smooth-offset 计算导航高度hNavi ; 其具体计算公式如下： ^Navi habs+hsm00th-off set 式中，hNavi为导航高度，habs为绝对高度，hs_th为高度平滑函数的返回值； 以上步骤四的实施过程中，高度平滑函数的调用将使导航高度来源由相对高度切换为 绝对高度时，不会发生导航高度的跳变，有利于对无人飞行器高度的控制。
3.根据权利要求1所述的一种融合相对高度和绝对高度获得导航高度的方法，其特征 在于：步骤五中所述的"使用相对高度hMlat和地面海拔高度hgMund计算导航高度h Navi"，其 具体计算步骤为： (5. 1)判断上一周期的导航高度来源是否为绝对高度habs ; (5. 2)若步骤（5. 1)确定上一周期的导航高度来源为绝对高度habs，计算并记录地面海 拔高度hg_d ;否则，不计算地面海拔高度hg_d ;所述的"计算地面海拔高度hg_d"，其计算 公式如下： hground ^Navi ^relat 式中，hgMund为地面海拔高度，hNavi为导航高度，hMlat为相对高度； (5. 3)判断相对高度hMlat不发生变化的周期数nt_是否大于设定值Ntra其实施 方法如下，首先设定判断相对高度hMlat发生变化的阈值hMlat limit，若本周期相对高度与上 一周期相对高度的差值的绝对值小于阈值hMlat limit，认为相对高度hMlat不发生变化，变量 nt_加1 ;若本周期相对高度与上一周期相对高度的差值的绝对值大于等于阈值hMlatlimit， 认为相对高度h Mlat发生变化，变量置0 ;然后判断是否大于设定值; (5. 4)判断导航高度hNavi与绝对高度habs的差值是否大于设定的阈值; 其差值的计算公式如下： h〇ffset ^Navi ^abs 公式中，hf#为导航高度与绝对高度的差值，hNavi为导航高度，habs为绝对高度； (5. 5)若步骤（5. 3)确定相对高度hMlat不发生变化的周期数ntMM大于设定值Ntrara并 且步骤（5. 4)确定导航高度hNavi与绝对高度habs的差值大于设定的阈值 计算并记录地面海拔高度h gMund ;否则，不计算地面海拔高度hgMund ;所述的"计算并记录地 面海拔高度hgMund"，其地面海拔高度hgMund的计算公式如下： hground habs hreiat 式中，hg_d为地面海拔高度，habs为导航高度，hMlat为相对高度； (5.6)若步骤（5.5)计算并记录了地面海拔高度hgMund，启用高度平滑函数，减小本周 期地面海拔高度与上一周期地面海拔高度的差值，该差值为步骤（5. 5)中设定的导航高度 hNavi与绝对高度habs的差值阈值hQffset limit，将高度平滑函数返回值赋值给变量h-ffset ; 否则，不启用高度平滑函数；这里应注意，"启用"高度平滑函数是指"开始调用"高度平滑函 数，在变量hs_ th ()ffsrt为0时，停止调用高度平滑函数；所述的"高度平滑函数"，其具体形式 与步骤（4.2)中"高度平滑函数"相同，作用是使变量&(1〇以有限步从其初始值 Xl(0)到 达期望值v(k)，第一次调用高度平滑函数时，Xl(k)的初始值^(0)赋值为本周期地面海拔 高度与上一周期地面海拔高度的差值值为常量0 ;因此调用高度平滑函数， 将使地面海拔高度的差值limit以有限步减小到0,高度平滑函数的返回值为 Xl (k); (5. 7)使用相对高度hMlat、地面海拔高度hg_d和变量hs_ th ()ffsrt计算导航高度hNavi ; 其具体计算公式如下： ^Navi hr elat+hgr〇und+hsm〇〇th-0ff set 式中，hNavi为导航高度，hMlat为相对高度，hg_d为地面海拔高度，h s_th ()ffsrt为高度平 滑函数的返回值； (5. 8)计算并记录导航高度hNavi与绝对高度habs的差值，其计算公式如下： h〇ffset ^Navi ^abs 公式中，为导航高度与绝对高度的差值，hNavi为导航高度，habs为绝对高度；记录 的导航高度与绝对高度的差值hf#将用于导航高度来源由相对高度切换为绝对高度时， 高度平滑函数的使用； 以上步骤五的实施过程中，导航高度来源由绝对高度切换为相对高度时，计算地面海 拔高度hgMmd，并用于导航高度的计算，该步骤的目的是使导航高度来源切换时导航高度不 发生跳变；判断相对高度是否在超过N trara个周期内不发生变化，是为判断无人飞行器是否 在跟踪恒定的期望高度飞行，若是，说明无人飞行器正在跟踪恒定的期望高度飞行，此时， 判断导航高度与绝对高度的值hf#是否大于设定的阈值，是为判断真实的地面 海拔高度是否已小于上一次计算的地面海拔高度，若是，说明真实的地面海拔高度已小于 上一次计算的地面海拔高度，因此重新计算地面海拔高度并用于导航高度的计算，以上步 骤使飞行器停止跟随下降地形并恢复到期望高度；以上地面海拔高度的计算统称为地面海 拔高度更新机制，这种地面海拔高度更新机制保证导航高度来源切换时，导航高度平稳过 渡，同时，还使飞行器及时停止跟随海拔高度下降的地面飞行并恢复到期望飞行高度；地面 海拔高度更新时对高度平滑函数的调用，能防止导航高度跳变，从而保证导航高度平稳过 渡，有利于无人飞行器对高度的控制。
【文档编号】G05D1/04GK104049636SQ201410228158
【公开日】2014年9月17日 申请日期:2014年5月27日 优先权日:2014年5月27日
【发明者】蒙志君, 陈旭智, 林子豪, 周尧明 申请人:北京航空航天大学