CN104049636B - 一种融合相对高度和绝对高度获得导航高度的方法 - Google Patents

Abstract

一种融合相对高度和绝对高度获得导航高度的方法，它有五大步骤：一、采用相对高度测量计，获取飞行器距离地面的相对高度h_relat；二、采用绝对高度测量计，获取飞行器的绝对高度h_abs；三、根据相对高度h_relat的大小，确定导航高度来源；四、若上述步骤三确定导航高度来源为绝对高度h_abs，使用绝对高度h_abs计算导航高度h_Navi；五、若上述步骤三确定导航高度来源为相对高度h_relat，使用相对高度h_relat和地面海拔高度h_ground计算导航高度h_Navi，然后计算并记录导航高度h_Navi与绝对高度h_abs的差值h_offset。本发明提高了导航高度的可靠性和准确性；飞行器近地飞行时，跟随上升地形飞行，而跟随下降地形飞行时可及时切出并返回到期望高度，从而保障飞行安全。

Description

一种融合相对高度和绝对高度获得导航高度的方法

技术领域

本发明涉及一种融合相对高度和绝对高度获得导航高度的方法，它是用于获得无人飞行器导航高度的方法，通过融合相对高度和绝对高度，得到精确可靠的导航高度，主要用于无人飞行器导航控制技术领域。

背景技术

无人飞行器的飞行高度获取方法包括使用绝对高度测量计获取绝对高度，使用相对高度测量计获取相对高度。绝对高度测量计包括GPS(Global Positioning System)定位系统、气压高度计等仪器；相对高度测量计包括超声波高度测量计、激光高度测量计、无线电高度测量计等仪器。

绝对高度测量计测量范大，可在飞行器整个升限内使用。但是在靠近地面时，易受周围建筑物、树木、天气变化等因素影响而使测量精度降低甚至失效，因此使用绝对高度直接作为导航高度将给飞行控制系统带来极大地不确定性和不可靠性，尤其在无人飞行器的起飞和着陆阶段，绝对高度无法满足飞行控制系统对高度通道的控制要；而且，绝对高度无法反映地面海拔高度的变化情况，因此使用绝对高度导航，无人飞行器无法获得距离地面的相对高度数据，从而无法保障近地飞行时的飞行安全。

相对高度测量计可以获取飞行器距离地面的相对高度数据，但是测量范围小，在飞行器远离地面飞行时，相对高度会超出相对高度测量计的量程范围，造成相对高度无法作为导航高度使用。

综合上述原因，无人飞行器通常同时搭载绝对高度测量计和相对高度测量计，获取绝对高度和相对高度共同作为导航高度。但是绝对高度和相对高度适用于导航的高度范围不同，物理形式也不同，两者共同作为导航高度时，会给飞行控制系统带来以下问题。

一是导航高度来源切换的问题，无人飞行器从起飞到着陆，期间会执行一定的飞行任务，飞行控制系统需要根据飞行任务执行高度选择使用相对高度还是绝对高度作为导航高度。如果飞行任务执行高度距离地面较近，飞行过程中相对高度都在相对高度测量计量程范围以内，可以选用相对高度作为导航高度；如果飞行任务执行高度距离地面较远，飞行过程中相对高度在相对高度测量计量程范围以外，则必须选用绝对高度作为导航高度；如果飞行任务执行高度同时包括近地面和远地面，在飞行过程中相对高度既有在相对高度测量计量程范围内的阶段，也有在相对高度测量计量程范围外的阶段，则需要判断何时使用相对高度何时使用绝对高度。

二是相对高度和绝对高度的物理形式不相同的问题，相对高度是指飞行器相对地面的垂直距离，绝对高度是指飞行器相对平均海平面的垂直距离，也叫“海拔高度”，在数值上差距较大，因此在相对高度和绝对高度共同作为导航高度时，需要将两者的物理形式统一：如果导航高度统一采用绝对高度形式，应将用于导航的相对高度转换为绝对高度；如果导航高度统一采用相对高度形式，则应将用于导航的绝对高度转换为相对高度。否则将导致导航高度的物理形式不明确，使飞行控制系统使用导航高度时发生错误，引发飞行事故。例如，飞行控制系统中的飞行期望高度为相对高度形式，当前值为100m，如果导航高度为绝对高度形式，当前值为500m，那么飞行控制系统将做出当前飞行高度远大于飞行期望高度的错误判断，从而使无人飞行器高度快速下降，如果此时地面海拔高度大于100m，就会发生触地坠毁的事故。

三是导航高度在相对高度和绝对高度之间切换时存在高度跳变问题，不同的高度测量计测量精度和测量范围存在差别，因此，即使相对高度和绝对高度作为导航高度时转换为相同物理形式，但两者提供的高度数值仍存在不同。如果将来源于相对高度的导航高度和来源于绝对高度的导航高度直接切换，将引起导航高度值的跳变，从而使飞行控制系统输出的控制量发生跳变，引起飞行器的飞行高度和飞行姿态产生较大变化，甚至引发飞行事故。

本发明通过融合相对高度和绝对高度，解决了将相对高度和绝对高度共同作为导航高度时存在的上述问题。

首先，明确了导航高度的切换条件，在相对高度测量计量程范围内确定切换上限和切换下限，相对高度大于切换上限时，导航高度使用绝对高度，相对高度小于切换下限时，导航高度使用相对高度，切换上限和切换下限之间的区域作为缓冲区，相对高度位于此区间时，导航高度来源与进入该区间之前的高度来源相同，不发生切换，从而避免了因高度测量误差造成的导航高度的频繁切换。

其次，统一了导航高度的物理形式，导航高度来源无论是相对高度还是绝对高度，导航高度都为绝对高度的物理形式，其中导航高度来源为相对高度时，通过相对高度加地面海拔高度的方法使导航高度转换为绝对高度形式；因此，飞行控制系统中使用的其他高度(如期望高度)可以统一采用绝对高度形式，避免了因物理形式不同而发生的导航事故。

再次，设计了高度平滑功能，使导航高度来源切换时，导航高度可以平稳过渡，不会发生跳变。高度平滑功能通过适时调用高度平滑函数实现，在导航高度来源由相对高度切换为绝对高度时，调用高度平滑函数，逐步消除使用相对高度时的导航高度与绝对高度的差值，使切换平稳过渡；同时，在导航高度来源为相对高度时，若地面海拔高度被重新计算，同样调用高度平滑函数，逐步消除前后地面海拔高度的差值，防止导航高度跳变。

最后，设计了地面海拔高度的更新机制，在导航高度来源由绝对高度切换为相对高度的周期，计算地面海拔高度，用于导航高度的计算；同时在导航高度来源为相对高度时，如果当前地面海拔高度与用于导航高度计算的地面海拔高度的差值超过设定阈值，则重新计算地面海拔高度。以上两种条件下对地面海拔高度的计算统称为地面海拔高度更新机制，通过更新地面海拔高度，可使飞行器的导航高度以绝对高度为真值得到校正，从而有利于导航高度来源由相对高度向绝对高度切换时的快速平稳过渡；而且，地面海拔高度的更新可使飞行器及时停止跟随下降地形飞行，返回到期望高度。

发明内容

本发明要解决的技术问题是：提供一种融合相对高度和绝对高度获得导航高度的方法，使飞行控制系统使用导航高度导航时无需判断飞行器所处高度和飞行任务执行高度，也无需考虑不同高度来源时导航高度的物理形式不一致和高度跳变问题；同时，导航高度兼具相对高度和绝对高度的优点，提高导航高度的精确可靠性。

本发明解决其技术问题所采用的技术方案是：一种融合相对和绝对高度获得导航高度的方法，其特征在于，需要以下设备条件：

(1)相对高度测量计，用于获取飞行器距离地面的相对高度，相对高度测量计可以是超声波高度测量计、激光高度测量计、无线电高度测量计等任何可获得飞行器距离地面相对高度的传感器；

(2)绝对高度测量计，用于获取飞行器的绝对高度，绝对高度测量计可以是GPS(Global Positioning System)定位系统、气压高度计等任何可获得飞行器绝对高度的传感器；

(3)导航计算机，用于接收上述相对高度和绝对高度，并根据相对高度的大小确定导航高度来源，通过融合相对高度和绝对高度获得导航高度。

一种融合相对高度和绝对高度获得导航高度的方法，其特征在于，它包括以下步骤：

步骤一：采用相对高度测量计，获取飞行器距离地面的相对高度h_relat；

步骤二：采用绝对高度测量计，获取飞行器的绝对高度h_abs；

步骤三：根据相对高度h_relat的大小，确定导航高度来源，该步骤的实施需要以下条件：

在相对高度测量计的量程范围内，设定导航高度来源切换上限h_{max_limit}和切换下限h_{min_limit}；

确定导航高度来源的具体方法为：若相对高度h_relat大于导航高度来源切换上限h_{max_limit}，导航高度来源选择为绝对高度；若相对高度h_relat小于导航高度来源切换下限h_{min_limit}，导航高度来源选择为相对高度；若相对高度h_relat位于导航高度来源切换下限h_{min_limit}和切换上限h_{max_limit}之间，导航高度来源与上一周期导航高度来源相同，不发生变化；

步骤四：若上述步骤三确定导航高度来源为绝对高度h_abs，使用绝对高度h_abs计算导航高度h_Navi；

步骤五：若上述步骤三确定导航高度来源为相对高度h_relat，使用相对高度h_relat和地面海拔高度h_ground计算导航高度h_Navi，然后计算并记录导航高度h_Navi与绝对高度h_abs的差值h_offset。

其中，步骤四中所述的“使用绝对高度h_abs计算导航高度h_Navi”，其具体计算步骤为：

(4.1)判断上一周期的导航高度来源是否为相对高度h_relat；

(4.2)若步骤(4.1)确定上一周期的导航高度来源为相对高度h_relat，启用高度平滑函数，减小导航高度h_Navi与绝对高度h_abs的差值h_offset，将高度平滑函数返回值赋值给变量h_{smooth_offset}；否则，不启用高度平滑函数；这里应注意，“启用”高度平滑函数是指“开始调用”高度平滑函数，在变量h_{smooth_offset}为0时，停止调用高度平滑函数；

(4.3)使用绝对高度h_abs和变量h_{smooth_offset}计算导航高度h_Navi。

其中，步骤(4.2)中所述的“高度平滑函数”，其具体形式如下：

$\left\{\begin{array}{c}\mathrm{fh}=\mathrm{fhan}(x_1\left(k\right)-v\left(k\right),x_2\left(k\right),r,h_1)\\ x_1(k+1)=x_1\left(k\right)+h\·x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h\·\mathrm{fh}\end{array}\right.---\left(1\right)$

公式(1)中函数fhan(x₁(k)-v(k),x₂(k),r,h₁)的表达式如下：

$\left\{\begin{array}{c}d=r\·{h_1}^2\\ a_0=h_1\·x_2\\ y=x_1-v+a_0\\ a_1=\sqrt{d\·(d+8|y\left|\right)}\\ a_2=a_0+\mathrm{sign}\left(y\right)\·(a_1-d)/2\\ a=(a_0+y)\·\mathrm{fsg}(y,d)+a_2\·(1-\mathrm{fsg}(y,d))\\ \mathrm{fhan}=-r\·\left(\frac{a}{d}\right)\·\mathrm{fsg}(a,d)-r\·\mathrm{sign}\left(a\right)\·(1-\mathrm{fsg}(a,d))\end{array}\right.---\left(2\right)$

公式(2)中函数fsg(x,d)的表达式如下：

y＝fsg(x,d)＝(sign(x+d)-sign(x-d))/2 (3)

公式(2)(3)中函数sign(x)为符号函数，具体表达式如下：

$y=\mathrm{sign}\left(x\right)=\left\{\begin{array}{c}1,x\&GreaterEqual;0\\ -1,x<0\end{array}\right.---\left(4\right)$

公式(1)(2)(3)(4)引自《自抗扰控制技术》70-71页，韩京清著。其中，fh＝fhan(x₁(k)-v(k),x₂(k),r,h₁)是二阶离散系统

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+h\&CenterDot;x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h\&CenterDot;\mathrm{fh}\end{array}\right.$

的最速控制函数，作用是使变量x₁(k)以有限步从其初始值x₁(0)到达期望值v(k)并跟踪期望值v(k)，v(k)既可以是变量也可以是常量。公式(1)(2)中主要参数的定义如下：

h为离散系统的积分步长；

r为快速因子，r取值越大，变量x₁(k)对期望值v(k)的跟踪速度越快；

h₁为滤波因子，h₁取值越小，变量x₁(k)对期望值v(k)的跟踪速度越快；

公式(1)(2)的详细解释可参考《自抗扰控制技术》。公式(1)(2)用作高度平滑函数时，在第一次被调用时，将x₁(k)初始值x₁(0)赋值为步骤五中“计算并记录的导航高度h_Navi与绝对高度h_abs的差值h_offset”，期望值v(k)为常量0。因此调用高度平滑函数，将使导航高度h_Navi与绝对高度h_abs的差值h_offset以有限步减小到0，高度平滑函数的返回值为x₁(k)；

其中，步骤(4.3)中所述的“使用绝对高度h_abs和变量h_{smooth_offset}计算导航高度h_Navi”，其具体计算公式如下：

h_Navi＝h_abs+h_{smooth_offset}

式中，h_Navi为导航高度，h_abs为绝对高度，h_{smooth_offset}为高度平滑函数的返回值。

以上步骤四的实施过程中，高度平滑函数的调用将使导航高度来源由相对高度切换为绝对高度时，不会发生导航高度的跳变，有利于对无人飞行器高度的控制；

其中，步骤五中所述的“使用相对高度h_relat和地面海拔高度h_ground计算导航高度h_Navi”，其具体计算步骤为：

(5.1)判断上一周期的导航高度来源是否为绝对高度h_abs；

(5.2)若步骤(5.1)确定上一周期的导航高度来源为绝对高度h_abs，计算并记录地面海拔高度h_ground；否则，不计算地面海拔高度h_ground；

(5.3)判断相对高度h_relat不发生变化的周期数n_trace是否大于设定值N_trace；

(5.4)判断导航高度h_Navi与绝对高度h_abs的差值h_offset是否大于设定的阈值h_{offset_limit}；

(5.5)若步骤(5.3)确定相对高度h_relat不发生变化的周期数n_trace大于设定值N_trace并且步骤(5.4)确定导航高度h_Navi与绝对高度h_abs的差值h_offset大于设定的阈值h_{offset_limit}，计算并记录地面海拔高度h_ground；否则，不计算地面海拔高度h_ground；

(5.6)若步骤(5.5)计算并记录了地面海拔高度h_ground，启用高度平滑函数，减小本周期地面海拔高度与上一周期地面海拔高度的差值，该差值为步骤(5.5)中设定的导航高度h_Navi与绝对高度h_abs的差值阈值h_{offset_limit}，将高度平滑函数返回值赋值给变量h_{smooth_offset}；否则，不启用高度平滑函数；这里应注意，“启用”高度平滑函数是指“开始调用”高度平滑函数，在变量h_{smooth_offset}为0时，停止调用高度平滑函数；

(5.7)使用相对高度h_relat、地面海拔高度h_ground和变量h_{smooth_offset}计算导航高度h_Navi；

(5.8)计算并记录导航高度h_Navi与绝对高度h_abs的差值h_offset。

其中，步骤(5.2)中所述的“计算地面海拔高度h_ground”，其计算公式如下：

h_ground＝h_Navi-h_relat

式中，h_ground为地面海拔高度，h_Navi为导航高度，h_relat为相对高度；

其中，步骤(5.3)中所述的“判断相对高度h_relat不发生变化的周期数n_trace是否大于设定值N_trace”，其实施方法如下，首先设定判断相对高度h_relat发生变化的阈值h_{relat_limit}，若本周期相对高度与上一周期相对高度的差值的绝对值小于阈值h_{relat_limit}，认为相对高度h_relat不发生变化，变量n_trace加1；若本周期相对高度与上一周期相对高度的差值的绝对值大于等于阈值h_{relat_limit}，认为相对高度h_relat发生变化，变量n_trace置0；然后判断n_trace是否大于设定值N_trace；

其中，步骤(5.4)中所述的“导航高度h_Navi与绝对高度h_abs的差值h_offset”，其差值h_offset的计算公式如下：

h_offset＝h_Navi-h_abs

公式中，h_offset为导航高度与绝对高度的差值，h_Navi为导航高度，h_abs为绝对高度；

其中，步骤(5.5)中所述的“计算并记录地面海拔高度h_ground”，其地面海拔高度h_ground的计算公式如下：

h_ground＝h_abs-h_relat

式中，h_ground为地面海拔高度，h_abs为导航高度，h_relat为相对高度；

其中，步骤(5.6)中所述的“高度平滑函数”，其具体形式与步骤(4.2)中“高度平滑函数”相同，作用是使变量x₁(k)以有限步从其初始值x₁(0)到达期望值v(k)，第一次调用高度平滑函数时，x₁(k)的初始值x₁(0)赋值为本周期地面海拔高度与上一周期地面海拔高度的差值h_{offset_limit}，v(k)值为常量0；因此调用高度平滑函数，将使地面海拔高度的差值h_{offset_limit}以有限步减小到0，高度平滑函数的返回值为x₁(k)；

其中，步骤(5.7)中所述的“使用相对高度h_relat、地面海拔高度h_ground和变量h_{smooth_offset}计算导航高度h_Navi”，其具体计算公式如下：

h_Navi＝h_relat+h_ground+h_{smooth_offset}

式中，h_Navi为导航高度，h_relat为相对高度，h_ground为地面海拔高度，h_{smooth_offset}为高度平滑函数的返回值；

其中，步骤(5.8)中所述的“计算并记录导航高度h_Navi与绝对高度h_abs的差值h_offset”，其计算公式如下：

h_offset＝h_Navi-h_abs

公式中，h_offset为导航高度与绝对高度的差值，h_Navi为导航高度，h_abs为绝对高度；记录的导航高度与绝对高度的差值h_offset将用于导航高度来源由相对高度切换为绝对高度时，高度平滑函数的使用。

以上步骤五的实施过程中，导航高度来源由绝对高度切换为相对高度时，计算地面海拔高度h_ground，并用于导航高度的计算，该步骤的目的是使导航高度来源切换时导航高度不发生跳变；判断相对高度是否在超过N_trace个周期内不发生变化，是为判断无人飞行器是否在跟踪恒定的期望高度飞行，若是，说明无人飞行器正在跟踪恒定的期望高度飞行，此时，判断导航高度与绝对高度的值h_offset是否大于设定的阈值h_{offset_limit}，是为判断真实的地面海拔高度是否已小于上一次计算的地面海拔高度，若是，说明真实的地面海拔高度已小于上一次计算的地面海拔高度，因此重新计算地面海拔高度并用于导航高度的计算，以上步骤可使飞行器停止跟随下降地形并恢复到期望高度；以上地面海拔高度的计算统称为地面海拔高度更新机制，这种地面海拔高度更新机制可以保证导航高度来源切换时，导航高度平稳过渡，同时，还可以使飞行器及时停止跟随海拔高度下降的地面飞行并恢复到期望飞行高度；地面海拔高度更新时对高度平滑函数的调用，可以防止导航高度跳变，从而保证导航高度平稳过渡，有利于无人飞行器对高度的控制。

本发明的有益效果是：

(1)仅通过融合相对和绝对高度获得导航高度，不增加传感器数量，不增加无人飞行器的起飞重量。

(2)充分利用了绝对高度和相对高度在不同飞行高度的导航优势，绝对高度和相对高度的融合使用既拓宽了导航高度的适用范围，又提高了近地面时导航高度的精确性和可靠性，使导航高度兼具了绝对高度和相对高度的优点。

(3)明确了导航高度来源的切换条件，在相对高度测量计测量范围内确定切换下限和切换上限，相对高度小于切换下限时，导航高度来源为相对高度，相对高度大于切换上限时，导航高度来源为绝对高度，飞行控制系统可直接使用融合后的导航高度，无需考虑飞行器所处高度。

(4)设置了导航高度来源切换的缓冲区，相对高度处于切换下限和切换上限之间区域时，导航高度来源与进入该区间之前的来源相同，不发生切换，从而避免了因高度测量误差造成的导航高度来源的频繁切换。

(5)导航高度来源无论是相对高度还是绝对高度，导航高度都为绝对高度的物理形式，其中导航高度来源于相对高度时，通过相对高度加地面海拔高度的方法使相对高度转换为绝对高度，方便飞行控制系统使用。

(6)使用了高度平滑函数，使导航高度来源切换和地面海拔高度重新计算时，导航高度可以平稳过渡，不会发生跳变，有利于飞行控制系统对飞行器姿态和高度的控制。

(7)设计了地面海拔高度的更新方法，使飞行器跟踪期望高度飞行时，可以跟随上升地形飞跃凸起的地面或建筑物，跟随下降地形飞行时可及时从地形跟踪飞行中切出并返回到期望高度，从而既能跟踪预设期望高度，又能兼顾飞行安全。

附图说明

下面结合附图对本发明进一步说明。

图1是本发明的硬件设备示意图；

图2是本发明的导航高度来源切换上限和切换下限示意图；

图3是本发明的融合相对高度和绝对高度获得导航高度流程图；

图4是本发明的高度平滑流程图。

图2中，h_{max_limit}为导航高度来源切换上限，h_{min_limit}为导航高度来源切换下限；

图3中，n_trace作用是记录相对高度不发生变化的周期数；N_trace为相对高度不发生变化的周期数阈值；

图4中，x₁(k)为高度平滑函数的变量之一，作用是跟踪给定的高度平滑函数的期望值，并作为高度平滑函数的返回值；h_{smooth_offset}为高度平滑的变量之一，作用是记录高度平滑函数的返回值x₁(k)。

具体实施方式

现在结合附图对本发明作进一步详细的说明。这些附图均为简化的示意图，仅以示意方式说明本发明的设计思想。

如图1所示，一种融合相对高度和绝对高度获得导航高度的方法，需要以下设备条件：相对高度测量计、绝对高度测量计和导航计算机；

其中，相对高度测量计可以是超声波高度测量计、激光高度传测量计、无线电高度测量计等任何可测量相对高度的传感器，绝对高度测量计可以是GPS(Global PositioningSystem)定位系统、气压高度计等任何可测量绝对高度的传感器；

其中，从相对高度测量计获取飞行器距离地面的相对高度数据h_relat，从绝对高度测量计获取飞行器的绝对高度数据h_abs；导航计算机接受上述相对高度h_relat和绝对高度h_abs，并将相对高度和绝对高度融合获得导航高度h_Navi。

如图2所示，将相对高度和绝对高度融合之前，需要设定导航高度来源切换下限h_{min_limit}和切换上限h_{max_limit}，通过比较相对高度h_relat与导航高度来源切换下限和切换上限，确定本周期导航高度使用相对高度数据还是绝对高度数据。具体实施方法如下：

在相对高度测量计量程范围内设定导航高度来源切换下限和切换上限，相对高度小于切换下限时，导航高度来源为相对高度，相对高度大于切换上限时，导航高度来源为绝对高度，相对高度位于导航高度来源切换下限和切换上限之间区域时，导航高度来源不发生变化，与进入该区域之前的高度来源相同；

以超声波测距传感器作为相对高度测量计为例，超声波测距传感器的量程为0至8米，在该量程内，设置导航高度来源切换下限为5米，切换上限为7米。在本周期，若超声波测距传感器测得的超声波高度小于5米，导航高度来源选择为超声波高度；若测得的超声波高度大于7米，导航高度来源选择为绝对高度；若测得的超声波高度在5米和7米之间，导航高度来源与上一周期相同，不发生变化。这样，在距离地面5米至7米的高度区间，将形成缓冲区间，飞行器超声波高度位于该区间时，导航高度来源将不发生切换。

如图3所示，本发明一种融合相对高度和绝对高度获得导航高度的方法，具体步骤如下：

步骤一：从相对高度测量计获取飞行器距离地面的相对高度h_relat；

步骤二：从绝对高度测量计获取飞行器的绝对高度h_abs；

步骤三：比较相对高度h_relat与导航高度来源切换上限h_{max_limit}和切换下限h_{min_limit}，判断本周导航高度来源为相对高度还是绝对高度，其具体实施步骤如下：

相对高度h_relat小于切换下限h_{min_limit}时，导航高度来源为相对高度h_relat；相对高度h_relat大于切换上限h_{max_limit}时，导航高度来源为绝对高度h_abs；相对高度h_relat位于导航高度来源切换下限h_{min_limit}和切换上限h_{max_limit}之间区域时，导航高度来源与上一周期相同，不发生变化。

步骤四：若步骤三中确定本周期导航高度来源为绝对高度，使用绝对高度h_abs计算导航高度h_Navi，具体实施步骤如下：

(4.1)判断上一周期导航高度来源是否为相对高度，若是，进入步骤(4.2)；若否，进入步骤(4.3)；

(4.2)将高度平滑开始标志置1，进入步骤(4.3)；

(4.3)高度平滑，其主要作用是根据高度平滑开始标志和高度平滑结束标志选择是否调用高度平滑函数，高度平滑的具体实施步骤如图4所示：

(a)判断高度平滑开始标志是否为1或者高度平滑结束标志是否为0，若是，进入步骤(b)；若否，进入步骤(e)；

(b)调用高度平滑函数，将高度平滑函数的计算结果x₁(k)返回给变量h_{smooth_offset}，然后进入步骤(c)，高度平滑函数的其具体形式如下：

$\left\{\begin{array}{c}\mathrm{fh}=\mathrm{fhan}(x_1\left(k\right)-v\left(k\right),x_2\left(k\right),r,h_1)\\ x_1(k+1)=x_1\left(k\right)+h\&CenterDot;x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h\&CenterDot;\mathrm{fh}\end{array}\right.---\left(1\right)$

公式(1)中函数fhan(x₁(k)-v(k),x₂(k),r,h₁)的表达式如下：

$\left\{\begin{array}{c}d=r\&CenterDot;{h_1}^2\\ a_0=h_1\&CenterDot;x_2\\ y=x_1-v+a_0\\ a_1=\sqrt{d\&CenterDot;(d+8|y\left|\right)}\\ a_2=a_0+\mathrm{sign}\left(y\right)\&CenterDot;(a_1-d)/2\\ a=(a_0+y)\&CenterDot;\mathrm{fsg}(y,d)+a_2\&CenterDot;(1-\mathrm{fsg}(y,d))\\ \mathrm{fhan}=-r\&CenterDot;\left(\frac{a}{d}\right)\&CenterDot;\mathrm{fsg}(a,d)-r\&CenterDot;\mathrm{sign}\left(a\right)\&CenterDot;(1-\mathrm{fsg}(a,d))\end{array}\right.---\left(2\right)$

公式(2)中函数fsg(x,d)的表达式如下：

y＝fsg(x,d)＝(sign(x+d)-sign(x-d))/2 (3)

公式(2)(3)中函数sign(x)为符号函数，具体表达式如下：

$y=\mathrm{sign}\left(x\right)=\left\{\begin{array}{c}1,x\&GreaterEqual;0\\ -1,x<0\end{array}\right.---\left(4\right)$

公式(1)(2)(3)(4)引自《自抗扰控制技术》70-71页，韩京清著。其中，fh＝fhan(x₁(k)-v(k),x₂(k),r,h₁)是二阶离散系统

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+h\&CenterDot;x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h\&CenterDot;\mathrm{fh}\end{array}\right.$

的最速控制函数，作用是使变量x₁(k)以有限步从其初始值x₁(0)到达期望值v(k)并跟踪期望值v(k)，v(k)既可以是变量也可以是常量。公式(1)(2)中主要参数的定义如下：

h为离散系统的积分步长；

r为快速因子，r取值越大，变量x₁(k)对期望值v(k)的跟踪速度越快；

h₁为滤波因子，h₁取值越小，变量x₁(k)对期望值v(k)的跟踪速度越快；

公式(1)(2)的详细解释可参考《自抗扰控制技术》。公式(1)(2)用作高度平滑函数时，x₁(k)初始值x₁(0)为步骤(5.9)中记录的导航高度h_Navi与绝对高度h_abs的差值h_offset，v(k)值为常量0。因此调用高度平滑函数，将使导航高度h_Navi与绝对高度h_abs的差值h_offset以有限步减小到0，高度平滑函数的返回值为x₁(k)；

(c)将高度平滑开始标志置为0，进入步骤(d)；

(d)判断变量h_{smooth_offset}值是否为0，若是，将高度平滑结束标志置1；若否，将高度平滑结束标志置0；然后进入步骤(f)；

(e)将变量h_{smooth_offset}赋值为0，进入步骤(f)；

(f)结束；

(4.4)计算并输出导航高度h_Navi，导航高度h_Navi的计算公式如下：

h_Navi＝h_abs+h_{smooth_offset}

式中，h_Navi为导航高度，h_abs为绝对高度，h_{smooth_offset}为高度平滑函数的返回值。

其中，步骤(4.2)、(4.3)中所述的“平滑开始标志”，初始化为0，其作用是，当其为1时，开始调用高度平滑函数，并为高度平滑函数的变量x₁(k)赋初值；

其中，步骤(4.3)中所述的“平滑结束标志”，初始化为1，其作用是，当其为1时，高度平滑已结束，不再调用高度平滑函数；当其为0时，高度平滑未结束，继续调用高度平滑函数；

步骤五：若步骤三中确定本周期导航高度来源为相对高度，使用相对高度h_relat和地面海拔高度h_ground计算导航高度h_Navi，计算并记录导航高度h_Navi与绝对高度h_abs的差值h_offset，具体实施步骤如下：

(5.1)判断上一周期导航高度来源是否为绝对高度h_abs，若是，进入步骤(5.2)；若否，进入步骤(5.3)；

(5.2)计算地面海拔高度h_ground，然后进入步骤(5.7)，地面海拔高度h_ground计算公式如下：

h_ground＝h_Navi-h_relat

式中，h_ground为地面海拔高度，h_Navi为导航高度，h_relat为相对高度；

(5.3)判断相对高度h_relat是否发生变化，若是，变量n_trace加1；若否，变量n_trace置0，然后进入步骤(5.4)，判断相对高度h_relat是否发生变化的方法如下：

首先设定相对高度h_relat变化的阈值h_{relat_limit}，若本周期相对高度与上一周期相对高度的差值的绝对值小于阈值h_{relat_limit}，认为相对高度h_relat不发生变化；若本周期相对高度与上一周期相对高度的差值的绝对值大于或等于阈值h_{relat_limit}，认为相对高度h_relat发生变化；

(5.4)判断导航高度h_Navi与绝对高度h_abs的差值h_offset是否大于设定的阈值h_{offset_limit}以及变量n_trace是否大于设定的阈值N_trace，若是，进入步骤(5.5)；若否，进入步骤(5.7)，差值h_offset的计算公式如下：

h_offset＝h_Navi-h_abs

公式中，h_offset为导航高度与绝对高度的差值，h_Navi为导航高度，h_abs为绝对高度；

(5.5)计算地面海拔高度h_ground，并将变量n_trace置0，然后进入步骤(5.6)，地面海拔高度h_ground计算公式如下：

h_ground＝h_abs-h_relat

式中，h_ground为地面海拔高度，h_abs为绝对高度，h_relat为相对高度；

(5.6)将高度平滑开始标志置1，进入步骤(5.7)；

(5.7)高度平滑，其主要作用是根据高度平滑开始标志和高度平滑结束标志选择是否调用高度平滑函数，高度平滑的具体实施步骤如图4所示：

(a)判断高度平滑开始标志是否为1或者高度平滑结束标志是否为0，若是，进入步骤(b)；若否，进入步骤(e)；

(b)调用高度平滑函数，将高度平滑函数的计算结果x₁(k)返回给变量h_{smooth_offset}，然后进入步骤(c)，高度平滑函数的其具体形式如下：

$\left\{\begin{array}{c}\mathrm{fh}=\mathrm{fhan}(x_1\left(k\right)-v\left(k\right),x_2\left(k\right),r,h_1)\\ x_1(k+1)=x_1\left(k\right)+h\&CenterDot;x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h\&CenterDot;\mathrm{fh}\end{array}\right.---\left(1\right)$

公式(1)中函数fhan(x₁(k)-v(k),x₂(k),r,h₁)的表达式如下：

$\left\{\begin{array}{c}d=r\&CenterDot;{h_1}^2\\ a_0=h_1\&CenterDot;x_2\\ y=x_1-v+a_0\\ a_1=\sqrt{d\&CenterDot;(d+8|y\left|\right)}\\ a_2=a_0+\mathrm{sign}\left(y\right)\&CenterDot;(a_1-d)/2\\ a=(a_0+y)\&CenterDot;\mathrm{fsg}(y,d)+a_2\&CenterDot;(1-\mathrm{fsg}(y,d))\\ \mathrm{fhan}=-r\&CenterDot;\left(\frac{a}{d}\right)\&CenterDot;\mathrm{fsg}(a,d)-r\&CenterDot;\mathrm{sign}\left(a\right)\&CenterDot;(1-\mathrm{fsg}(a,d))\end{array}\right.---\left(2\right)$

公式(2)中函数fsg(x,d)的表达式如下：

y＝fsg(x,d)＝(sign(x+d)-sign(x-d))/2 (3)

公式(2)(3)中函数sign(x)为符号函数，具体表达式如下：

$y=\mathrm{sign}\left(x\right)=\left\{\begin{array}{c}1,x\&GreaterEqual;0\\ -1,x<0\end{array}\right.---\left(4\right)$

公式(1)(2)(3)(4)引自《自抗扰控制技术》70-71页，韩京清著。其中，fh＝fhan(x₁(k)-v(k),x₂(k),r,h₁)是二阶离散系统

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+h\&CenterDot;x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h\&CenterDot;\mathrm{fh}\end{array}\right.$

的最速控制函数，作用是使变量x₁(k)以有限步从其初始值x₁(0)到达期望值v(k)并跟踪期望值v(k)，v(k)既可以是变量也可以是常量。公式(1)(2)中主要参数的定义如下：

h为离散系统的积分步长；

r为快速因子，r取值越大，变量x₁(k)对期望值v(k)的跟踪速度越快；

h₁为滤波因子，h₁取值越小，变量x₁(k)对期望值v(k)的跟踪速度越快；

公式(1)(2)的详细解释可参考《自抗扰控制技术》。公式(1)(2)用作高度平滑函数时，x₁(k)初始值x₁(0)为本周期地面海拔高度与上一周期地面海拔高度的差值，该差值为步骤(5.4)中“导航高度h_Navi与绝对高度h_abs的差值阈值h_{offset_limit}”，v(k)值为常量0。因此调用高度平滑函数，将使地面海拔高度的差值h_{offset_limit}以有限步减小到0，高度平滑函数的返回值为x₁(k)；

(c)将高度平滑开始标志置为0；进入步骤(d)；

(d)判断变量h_{smooth_offset}值是否为0，若是，将高度平滑结束标志置1；若否，将高度平滑结束标志置0，然后进入步骤(f)；

(e)将变量h_{smooth_offset}赋值为0，进入步骤(e)；

(f)结束；

(5.8)计算并输出导航高度h_Navi，导航高度h_Navi的计算公式如下：

h_Navi＝h_relat+h_ground+h_{smooth_offset}

式中，h_Navi为导航高度，h_relat为相对高度，h_ground为地面海拔高度，h_{smooth_offset}为高度平滑函数的返回值。

(5.9)计算并记录导航高度h_Navi与绝对高度h_abs的差值h_offset，差值h_offset计算公式如下：

h_offset＝h_Navi-h_abs

公式中，h_offset为导航高度与绝对高度的差值，h_Navi为导航高度，h_abs为绝对高度。

其中，步骤(5.6)、(5.7)中所述的“平滑开始标志”，初始化为0，其作用是，当其为1时，开始调用高度平滑函数，并为高度平滑函数的变量x₁(k)赋初值；

其中，步骤(5.7)中所述的“平滑结束标志”，初始化为1，其作用是，当其为1时，高度平滑已结束，不再调用高度平滑函数；当其为0时，高度平滑未结束，继续调用高度平滑函数。

本发明的融合相对高度和绝对高度获得导航高度的方法在实际使用时具有以下优点：

(1)所使用的高度传感器和飞行控制计算机普通无人飞行器都会搭载，因此无需增加仪器设备，不增加飞行器的起飞重量。

(2)算法简单，逻辑清晰，所涉及的运算皆为基本的四则运算，不会增加飞行控制计算机的运行负担。

(3)通过融合相对高度和绝对高度获得的导航高度兼具相对高度和绝对高度的优点，在近地面导航高度来源于相对高度，可以排除地面建筑物、树木、天气等外界因素对导航高度的干扰，使导航高度的精确性、可靠性得以提高；其次，导航高度反映了飞行器相对地面的高度数据，因此可以保障飞行安全；再次，飞行器跟踪期望高度飞行时，亦会跟随上升地形飞跃凸起的地面或建筑物，在地面海拔高度降低时，因地面海拔高度的更新可停止跟随地形飞行，返回到期望高度，进一步保障了飞行安全。在飞行器远离地面飞行时，导航高度来源可自动切换为绝对高度，从而增大了导航高度的适用范围，保证飞行器在整个飞行升限内都可获得准确的导航高度数据。

(4)切换缓冲区的设计避免了导航高度来源地频繁切换，从而避免了导航高度值的频繁改变，有利于提高飞行器的控制性能和飞行品质。

(5)高度平滑函数的调用使导航高度来源切换和地面海拔高度更新时，导航高度缓慢、连续地变化，而不是跳变，从而有利于飞行控制系统对飞行器姿态和高度的控制。

以上述本发明的设计方案为启示，根据上述的说明内容，相关工作人员完全可以在不偏离本项发明技术思想的范围内，进行多样的变更以及修改。本项发明的技术性范围并不局限于说明书上的内容，必须根据权利要求范围来确定其技术性范围。

Claims (2)

1.一种融合相对高度和绝对高度获得导航高度的方法，其特征在于：它包括以下步骤：

步骤一：采用相对高度测量计，获取飞行器距离地面的相对高度h_relat；

步骤二：采用绝对高度测量计，获取飞行器的绝对高度h_abs；

步骤三：根据相对高度h_relat的大小，确定导航高度来源，该步骤的实施需要以下条件：

在相对高度测量计的量程范围内，设定导航高度来源切换上限h_{max_limit}和切换下限h_{min_limit}；

确定导航高度来源的具体方法为：若相对高度h_relat大于导航高度来源切换上限h_{max_limit}，导航高度来源选择为绝对高度；若相对高度h_relat小于导航高度来源切换下限h_{min_limit}，导航高度来源选择为相对高度；若相对高度h_relat位于导航高度来源切换下限h_{min_limit}和切换上限h_{max_limit}之间，导航高度来源与上一周期导航高度来源相同，不发生变化；

步骤四：若上述步骤三确定导航高度来源为绝对高度h_abs，使用绝对高度h_abs计算导航高度h_Navi；

步骤五：若上述步骤三确定导航高度来源为相对高度h_relat，使用相对高度h_relat和地面海拔高度h_ground计算导航高度h_Navi，然后计算并记录导航高度h_Navi与绝对高度h_abs的差值h_offset；

其中，步骤四中所述的“使用绝对高度h_abs计算导航高度h_Navi”，其具体计算步骤为：

(4.1)判断上一周期的导航高度来源是否为相对高度h_relat；

(4.2)若步骤(4.1)确定上一周期的导航高度来源为相对高度h_relat，启用高度平滑函数，减小导航高度h_Navi与绝对高度h_abs的差值h_offset，将高度平滑函数返回值赋值给变量h_{smooth_offset}；否则，不启用高度平滑函数；这里应注意，“启用”高度平滑函数是指“开始调用”高度平滑函数，在变量h_{smooth_offset}为0时，停止调用高度平滑函数；

所述的“高度平滑函数”，其具体形式如下：

$\left\{\begin{array}{c}fh=fhan\left(x_1\right(k)-v(k),x_2(k),r,h_1)\\ x_1(k+1)=x_1\left(k\right)+h\&CenterDot;x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h\&CenterDot;fh\end{array}\right.---\left(1\right)$

公式(1)中函数fhan(x₁(k)-v(k),x₂(k),r,h₁)的表达式如下：

$\left\{\begin{array}{c}d=r\&CenterDot;{h_1}^2\\ a_0=h_1\&CenterDot;x_2\\ y=x_1-v+a_0\\ a_1=\sqrt{d\&CenterDot;(d+8|y\left|\right)}\\ a_2=a_0+sign\left(y\right)\&CenterDot;(a_1-d)/2\\ a=(a_0+y)\&CenterDot;fsg(y,d)+a_2\&CenterDot;(1-fsg(y,d\left)\right)\\ fhan=-r\&CenterDot;\left(\frac{a}{d}\right)\&CenterDot;fsg(a,d)-r\&CenterDot;sign\left(a\right)\&CenterDot;(1-fsg(a,d\left)\right)\end{array}\right.---\left(2\right)$

公式(2)中函数fsg(x,d)的表达式如下：

y＝fsg(x,d)＝(sign(x+d)-sign(x-d))/2 (3)

公式(2)(3)中函数sign(x)为符号函数，具体表达式如下：

$y=sign\left(x\right)=\left\{\begin{array}{c}1,x\&GreaterEqual;0\\ -1,x<0\end{array}\right.---\left(4\right)$

其中，fh＝fhan(x₁(k)-v(k),x₂(k),r,h₁)是二阶离散系统

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+h\&CenterDot;x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h\&CenterDot;fh\end{array}\right.$

的最速控制函数，作用是使变量x₁(k)以有限步从其初始值x₁(0)到达期望值v(k)并跟踪期望值v(k)，v(k)既可以是变量也可以是常量；公式(1)(2)中主要参数的定义如下：

h为离散系统的积分步长；

r为快速因子，r取值越大，变量x₁(k)对期望值v(k)的跟踪速度越快；

h₁为滤波因子，h₁取值越小，变量x₁(k)对期望值v(k)的跟踪速度越快；

公式(1)(2)用作高度平滑函数时，在第一次被调用时，将x₁(k)初始值x₁(0)赋值为步骤五中“计算并记录的导航高度h_Navi与绝对高度h_abs的差值h_offset”，期望值v(k)为常量0；因此调用高度平滑函数，将使导航高度h_Navi与绝对高度h_abs的差值h_offset以有限步减小到0，高度平滑函数的返回值为x₁(k)；

(4.3)使用绝对高度h_abs和变量h_{smooth_offset}计算导航高度h_Navi；

其具体计算公式如下：

h_Navi＝h_abs+h_{smooth_offset}

式中，h_Navi为导航高度，h_abs为绝对高度，h_{smooth_offset}为高度平滑函数的返回值；

以上步骤四的实施过程中，高度平滑函数的调用将使导航高度来源由相对高度切换为绝对高度时，不会发生导航高度的跳变，有利于对无人飞行器高度的控制。

2.根据权利要求1所述的一种融合相对高度和绝对高度获得导航高度的方法，其特征在于：步骤五中所述的“使用相对高度h_relat和地面海拔高度h_ground计算导航高度h_Navi”，其具体计算步骤为：

(5.1)判断上一周期的导航高度来源是否为绝对高度h_abs；

(5.2)若步骤(5.1)确定上一周期的导航高度来源为绝对高度h_abs，计算并记录地面海拔高度h_ground；否则，不计算地面海拔高度h_ground；所述的“计算地面海拔高度h_ground”，其计算公式如下：

h_ground＝h_Navi-h_relat

式中，h_ground为地面海拔高度，h_Navi为导航高度，h_relat为相对高度；

(5.3)判断相对高度h_relat不发生变化的周期数n_trace是否大于设定值N_trace；其实施方法如下，首先设定判断相对高度h_relat发生变化的阈值h_{relat_limit}，若本周期相对高度与上一周期相对高度的差值的绝对值小于阈值h_{relat_limit}，认为相对高度h_relat不发生变化，变量n_trace加1；若本周期相对高度与上一周期相对高度的差值的绝对值大于等于阈值h_{relat_limit}，认为相对高度h_relat发生变化，变量n_trace置0；然后判断n_trace是否大于设定值N_trace；

(5.4)判断导航高度与绝对高度h_abs的差值h_offset是否大于设定的阈值h_{offset_limit}；其差值h_offset的计算公式如下：

h_offset＝h_Navi-h_abs

公式中，h_offset为导航高度与绝对高度的差值，h_Navi为导航高度，h_abs为绝对高度；

(5.5)若步骤(5.3)确定相对高度h_relat不发生变化的周期数n_trace大于设定值N_trace并且步骤(5.4)确定导航高度h_Navi与绝对高度h_abs的差值h_offset大于设定的阈值h_{offset_limit}，计算并记录地面海拔高度h_ground；否则，不计算地面海拔高度h_ground；所述的“计算并记录地面海拔高度h_ground”，其地面海拔高度h_ground的计算公式如下：

h_ground＝h_abs-h_relat

式中，h_ground为地面海拔高度，h_abs为导航高度，h_relat为相对高度；

(5.6)若步骤(5.5)计算并记录了地面海拔高度h_ground，启用高度平滑函数，减小本周期地面海拔高度与上一周期地面海拔高度的差值，该差值为步骤(5.5)中设定的导航高度h_Navi与绝对高度h_abs的差值阈值h_{offset_limit}，将高度平滑函数返回值赋值给变量h_{smooth_offset}；否则，不启用高度平滑函数；这里应注意，“启用”高度平滑函数是指“开始调用”高度平滑函数，在变量h_{smooth_offset}为0时，停止调用高度平滑函数；所述的“高度平滑函数”，其具体形式与步骤(4.2)中“高度平滑函数”相同，作用是使变量x₁(k)以有限步从其初始值x₁(0)到达期望值v(k)，第一次调用高度平滑函数时，x₁(k)的初始值x₁(0)赋值为本周期地面海拔高度与上一周期地面海拔高度的差值h_{offset_limit}，v(k)值为常量0；因此调用高度平滑函数，将使地面海拔高度的差值h_{offset_limit}以有限步减小到0，高度平滑函数的返回值为x₁(k)；

(5.7)使用相对高度h_relat、地面海拔高度h_ground和变量h_{smooth_offset}计算导航高度h_Navi；其具体计算公式如下：

h_Navi＝h_relat+h_ground+h_{smooth_offset}

式中，h_Navi为导航高度，h_relat为相对高度，h_ground为地面海拔高度，h_{smooth_offset}为高度平滑函数的返回值；

(5.8)计算并记录导航高度h_Navi与绝对高度h_abs的差值h_offset，其计算公式如下：

h_offset＝h_Navi-h_abs

公式中，h_offset为导航高度与绝对高度的差值，h_Navi为导航高度，h_abs为绝对高度；记录的导航高度与绝对高度的差值h_offset将用于导航高度来源由相对高度切换为绝对高度时，高度平滑函数的使用；

以上步骤五的实施过程中，导航高度来源由绝对高度切换为相对高度时，计算地面海拔高度h_ground，并用于导航高度的计算，该步骤的目的是使导航高度来源切换时导航高度不发生跳变；判断相对高度是否在超过N_trace个周期内不发生变化，是为判断无人飞行器是否在跟踪恒定的期望高度飞行，若是，说明无人飞行器正在跟踪恒定的期望高度飞行，此时，判断导航高度与绝对高度的差值h_offset是否大于设定的阈值h_{offset_limit}，是为判断真实的地面海拔高度是否已小于上一次计算的地面海拔高度，若是，说明真实的地面海拔高度已小于上一次计算的地面海拔高度，因此重新计算地面海拔高度并用于导航高度的计算，以上步骤使飞行器停止跟随下降地形并恢复到期望高度；以上地面海拔高度的计算统称为地面海拔高度更新机制，这种地面海拔高度更新机制保证导航高度来源切换时，导航高度平稳过渡，同时，还使飞行器及时停止跟随海拔高度下降的地面飞行并恢复到期望飞行高度；地面海拔高度更新时对高度平滑函数的调用，能防止导航高度跳变，从而保证导航高度平稳过渡，有利于无人飞行器对高度的控制。