CN102426016B - 一种基于惯性导航综合航路管理的实现方法 - Google Patents

Abstract

本发明属于飞行管理技术领域，涉及一种基于惯性导航综合航路管理的实现方法。本发明提出了一种全面、详细、自动化程度高、可操作性强的综合航路管理的实现方法，按照该方法计算飞行参数并控制飞机状态，可保证飞机安全、平稳地完成各项飞行任务，提供从飞机起飞到进近着陆的横向最优飞行剖面与垂直最优飞行剖面，飞机将按优化飞行轨迹从起飞机场到达目的地机场。

Description

一种基于惯性导航综合航路管理的实现方法

技术领域

本发明属于飞行管理技术领域，涉及一种基于惯性导航综合航路管理的实现方法。

背景技术

综合航路管理是根据飞机当前所在的位置、飞机性能参数、目的地机场位置与跑道信息、各航路点参数以及等待航线、进近程序等信号或数据进行综合分析运算，以确定飞机的应飞航向、速度以及爬高、下降角和升降速度、阶梯爬高和下降等指令。

国防工业出版社2002年出版的刘慧英、周勇编著的《空中交通管理系统导论》，公开了一种针对民航客机与大型运输机的飞行规划、性能优化、制导等方面的基本原理、功能等，但是，对于飞机的综合航路管理的具体的实现方案没有公开。

发明内容

本发明的目的是提出一种控制准确、功能齐全、可操作性强的一种基于惯性导航综合航路管理的实现方法。本发明的技术解决方案是，根据惯性导航系统的输出，准确计算飞机在飞行中的即时位置，按照飞机目前所处状态与控制指令，确定最优导航模式，连续计算并输出航迹操纵模式、航线操纵模式、时间导航、返航与着陆等需要的控制参数，实现全自动导航。综合航路管理的实现方法包括航迹操纵、航线操纵两种操纵方式和时间导航、返航、着陆三种导航方法，其中

(一)航迹操纵方式实现步骤：

(1)接收惯性导航系统输出的数据：即时经度λp、纬度Φp、真航向ψ_T、东向速度V_E、北向速度V_N；

(2)根据惯性导航系统预先装订的数据，获得当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1；

(3)按照接收惯性导航系统输出的东向速度V_E、北向速度V_N，计算航迹角ψ_TK：

ψ_TK＝tan^-1(V_E/V_N)；         (1)

(4)按照接收惯性导航系统输出的真航向ψ_T与航迹角ψ_TK，计算偏流角δ：

δ＝ψ_TK-ψ_T；               (2)

(5)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1，计算预航迹角ψ_DTK： ${\mathrm{\ψ}}_{\mathrm{DTK}}={\cos }^{-1}({\stackrel{\→}{r}}_{N-P}\·{\stackrel{\→}{r}}_{i+1,i});---\left(3\right)$

其中，为即时位置当地真北的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和到达下一个航路点当地铅垂线的单位矢量，按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{N,p}\·{\stackrel{\→}{r}}_{i+1,i}=M_p\×M_N\×[-{\left(C_{\mathrm{zy}}\right)}_p\×p_1+{\left(C_{\mathrm{zx}}\right)}_p\×p_2]$

$M_N^{-1}=\sqrt{{\left(C_{\mathrm{zy}}\right)}_p^2+{\left(C_{\mathrm{zx}}\right)}_p^2}$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i+1×(C_zz)_i-(C_zz)_i+1×(C_zy)_i

p₂＝(C_zz)_i+1×(C_zx)_i-(C_zx)_i+1×(C_zz)_i

p₃＝(C_zx)_i+1×(C_zy)_i-(C_zy)_i+1×(C_zx)_i

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(6)根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T：

ψ′_T＝ψ_DTK-δ；                      (4)

(7)根据接收惯性导航系统输出的真航向ψ_T及应飞航向ψ′_T，计算航向偏差Δ

ψ：Δψ＝ψ_T-ψ′_T；                  (5)

(8)令横偏距X与高度偏差ΔH为0，即：X≡0，ΔH≡0；

(9)输出航向偏差Δψ、横偏距X、高度偏差ΔH；

(二)航线操纵方式实现步骤：

(1)接收惯性导航系统输出的数据：即时经度λp、纬度Φp、真航向ψ_T、东向速度V_E、北向速度V_N；

(2)根据惯性导航系统预先装订的数据，获得当前航路点经度λi、纬度Φi，上一个航路点经度λi-1、纬度Φi-1；

(3)按照接收惯性导航系统输出的东向速度V_E、北向速度V_N，计算航迹角ψ_TK，

ψ_TK＝tan^-1(V_E/V_N)；                        (6)

(4)按照接收惯性导航系统输出的真航向ψ_T与航迹角ψ_TK，计算偏流角δ，

δ＝ψ_TK-ψ_T；                              (7)

(5)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，上一个航路点经度λi-1、纬度Φi-1，计算飞机距预定航线距离，即横偏距X：

$X=-R\×{\sin }^{-1}({\stackrel{\→}{r}}_{i,i-1}\·{\stackrel{\→}{r}}_p)\×\frac{\mathrm{\π}}{180}---\left(8\right)$

其中：R为地球半径；

为即时位置当地铅垂线的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和上一个航路点当地铅垂线的单位矢量；按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i,i-1}\·{\stackrel{\→}{r}}_p=M_p\×[p_1\×{\left(C_{\mathrm{zx}}\right)}_p+p_2\×{\left(C_{\mathrm{zy}}\right)}_p+p_3\×{\left(C_{\mathrm{zz}}\right)}_p]$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i×(C_zz)_i-1-(C_zz)_i×(C_zy)_i-1

p₂＝(C_zz)_i×(C_zx)_i-1-(C_zx)_i×(C_zz)_i-1

p₃＝(C_zx)_i×(C_zy)_i-1-(C_zy)_i×(C_zx)_i-1

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(6)根据计算得到的飞机距预定航线的横偏距X判断飞机处于四个区域的哪个区，判别条件如下：

其中：R_p为飞机转弯半径；

L、L₁、L₂为根据飞机性能和控制精度要求确定的各区域宽度参数；

(7)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，上一个航路点经度λi-1、纬度Φi-1，计算当前区域的预航迹ψ_DL：

${\mathrm{\ψ}}_{\mathrm{DL}}={\cos }^{-1}({\stackrel{\→}{r}}_{N,P}\·{\stackrel{\→}{r}}_{i,i-1})---\left(9\right)$

其中：为即时位置当地真北的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和上一个航路点当地铅垂线的单位矢量；

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{N,p}\·{\stackrel{\→}{r}}_{i,i-1}=M_p\×M_N\×[-{\left(C_{\mathrm{zy}}\right)}_p\×p_1+{\left(C_{\mathrm{zx}}\right)}_p\×p_2]$

$M_N^{-1}=\sqrt{{\left(C_{\mathrm{zy}}\right)}_p^2+{\left(C_{\mathrm{zx}}\right)}_p^2}$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i×(C_zz)_i-1-(C_zz)_i×(C_zy)_i-1

p₂＝(C_zz)_i×(C_zx)_i-1-(C_zx)_i×(C_zz)_i-1

p₃＝(C_zx)_i×(C_zy)_i-1-(C_zy)_i×(C_zx)_i-1

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(8)根据计算得到的预航迹ψ_DL、横偏距X，计算应飞航迹ψ_DT：

ψ_DT＝ψ_DL+ψ×sign(X)                    (10)

其中：

其中：X′＝|X|-L₁；

R_P为飞机转弯半径；

$\mathrm{Rd}=R_P\×\frac{L^2}{(L-L_1)\×\left|X\right|};$

ψ为应飞航迹和预航迹之间的夹角；

(9)根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T，

ψ′_T＝ψ_DTK-δ；                         (11)

(10)根据接收惯性导航系统输出的真航向ψ_T及应飞航向ψ′_T，计算航向偏差

Δψ：Δψ＝ψ_T-ψ′_T；                   (12)

(11)令高度偏差为0，即：ΔH≡0；

(12)输出航向偏差Δψ、横偏距X、高度偏差ΔH；

(三)时间导航方法实现步骤

(1)接收惯性导航系统输出的数据：即时经度λp、纬度Φp、时间t_即时；

(2)根据惯性导航系统预先装订的数据，获得下一个航路点经度λi+1、纬度Φi+1、到达时间t_目标；

(3)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，计算即时位置到目标航路点的距离D：

$D=R\×\frac{\mathrm{\π}}{180}\×{\cos }^{-1}({\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p)---\left(13\right)$

其中：R为地球半径；

为下一个航路点当地铅垂线的单位矢量；

为即时位置当地铅垂线的单位矢量。

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p={\left(C_{\mathrm{zx}}\right)}_{i+1}\×{\left(C_{\mathrm{zx}}\right)}_p+{\left(C_{\mathrm{zy}}\right)}_{i+1}\×{\left(C_{\mathrm{zy}}\right)}_p+{\left(C_{\mathrm{zz}}\right)}_{i+1}\×{\left(C_{\mathrm{zz}}\right)}_p$

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(4)按照接收惯性导航系统输出的时间t_即时和惯性导航系统预先装订的到达时间t_目标，以及计算的距离D，计算应飞速度V_应飞：

t_应飞＝t_目标-t_即时

其中：Vmax为飞机最大巡航速度；

Vmin为飞机最小巡航速度；

(5)输出应飞速度V_应飞；

(四)返航方法实现步骤

(1)输入机场位置(φ_r、λ_r)与高度H，并设置为目标点；

(2)输入切入距离D1、切入航向ψ₁，返航点高度H_FAF，返航距离LD；

(3)接收惯性导航系统输出的数据：即时经度λp、纬度Φp、真航向ψ_T、东向速度V_E、北向速度V_N、惯性气压高度Hc；

(4)根据惯性导航系统预先装订的数据，获得当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1；

(5)按照接收惯性导航系统输出的东向速度V_E、北向速度V_N，计算航迹角ψ_TK：

ψ_TK＝tan^-1(V_E/V_N)；                (15)

(6)按照接收惯性导航系统输出的真航向ψ_T与航迹角ψ_TK，计算偏流角δ：

δ＝ψ_TK-ψ_T；                      (16)

(7)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1，计算飞机距预定航线距离，即横偏距X：

$X=-R\×{\sin }^{-1}({\stackrel{\→}{r}}_{i,i-1}\·{\stackrel{\→}{r}}_p)\×\frac{\mathrm{\π}}{180}---\left(17\right)$

其中：R为地球半径；

为即时位置当地铅垂线的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和下一个航路点当地铅垂线的单位矢量；

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i,i-1}\·{\stackrel{\→}{r}}_p=M_p\×[p_1\×{\left(C_{\mathrm{zx}}\right)}_p+p_2\×{\left(C_{\mathrm{zy}}\right)}_p+p_3\×{\left(C_{\mathrm{zz}}\right)}_p]$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i+1×(C_zz)_i-(C_zz)_i+1×(C_zy)_i

p₂＝(C_zz)_i+1×(C_zx)_i-(C_zx)_i+1×(C_zz)_i

p₃＝(C_zx)_i+1×(C_zy)_i-(C_zy)_i+1×(C_zx)_i

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(8)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，计算即时位置到目标航路点的待飞距D：

$D=R\×\frac{\mathrm{\π}}{180}\×{\cos }^{-1}({\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p)---\left(18\right)$

其中：R为地球半径；

为即时位置当地铅垂线的单位矢量；

为下一个航路点当地铅垂线的单位矢量。

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p={\left(C_{\mathrm{zx}}\right)}_{i+1}\×{\left(C_{\mathrm{zx}}\right)}_p+{\left(C_{\mathrm{zy}}\right)}_{i+1}\×{\left(C_{\mathrm{zy}}\right)}_p+{\left(C_{\mathrm{zz}}\right)}_{i+1}\×{\left(C_{\mathrm{zz}}\right)}_p$

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(9)当待飞距D≥LD时，按以下步骤计算应飞航向ψ′_T：

第一步，按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1，计算预航迹角ψ_DTK： ${\mathrm{\ψ}}_{\mathrm{DTK}}={\cos }^{-1}({\stackrel{\→}{r}}_{N-P}\·{\stackrel{\→}{r}}_{i+1,i});---\left(19\right)$

其中，为即时位置当地真北的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和到达下一个航路点当地铅垂线的单位矢量。

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{N,p}\·{\stackrel{\→}{r}}_{i+1,i}=M_p\×M_N\×[-{\left(C_{\mathrm{zy}}\right)}_p\×p_1+{\left(C_{\mathrm{zx}}\right)}_p\×p_2]$

$M_N^{-1}=\sqrt{{\left(C_{\mathrm{zy}}\right)}_p^2+{\left(C_{\mathrm{zx}}\right)}_p^2}$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i+1×(C_zz)_i-(C_zz)_i+1×(C_zy)_i

p₂＝(C_zz)_i+1×(C_zx)_i-(C_zx)_i+1×(C_zz)_i

p₃＝(C_zx)_i+1×(C_zy)_i-(C_zy)_i+1×(C_zx)_i

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

第二步，根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T：

ψ′_T＝ψ_DTK-δ；        (20)

(10)当待飞距D≤LD时，按以下步骤计算应飞航向ψ′_T：

第一步，根据惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，切入距离D1、切入航向ψ1和计算的横偏距X，计算相切圆的位置(φ₀、λ₀)：

${\mathrm{\φ}}_0={\mathrm{\φ}}_{i+1}+\frac{\mathrm{OP}\×\cos {\mathrm{\ψ}}^{\′}}{R}$

${\mathrm{\λ}}_0={\mathrm{\λ}}_{i+1}\frac{\mathrm{OP}\×\sin {\mathrm{\ψ}}^{\′}}{R\×\cos {\mathrm{\φ}}_{i+1}}---\left(21\right)$

$\mathrm{OP}=\sqrt{{D1}^2+R_p^2}$

其中：R为地球半径；

${\mathrm{\ψ}}^{\′}=\mathrm{\ψ}1+\mathrm{sign}\left(X\right)\×\arcsin \frac{R_p}{\mathrm{OP}}+180$

R_p为飞机转弯半径；

(ψ1、D1)：切入航向、切入距离。

第二步，根据惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，切入距离D1、切入航向ψ1和计算的横偏距X，计算切入点位置(φ_Q、λ_Q)：

${\mathrm{\φ}}_Q={\mathrm{\φ}}_{i+1}+\frac{D1\×\cos {\mathrm{\ψ}}_1}{R}$

${\mathrm{\λ}}_Q={\mathrm{\λ}}_{i+1}+\frac{D1\×\sin {\mathrm{\ψ}}^{\′}}{R\×\cos {\mathrm{\φ}}_{i+1}}---\left(22\right)$

其中：R为地球半径；

${\mathrm{\ψ}}^{\′}=\mathrm{\ψ}1+\mathrm{sign}\left(X\right)\×\arcsin \frac{R_p}{\mathrm{OP}}+180$

R_p为飞机转弯半径；

(ψ1、D1)：切入航向、切入距离。

第三步，按照接收惯性导航系统输出的即时经度λp、纬度Φp，计算的相切圆的位置(φ₀、λ₀)、切入点位置(φ_Q、λ_Q)和横偏距X计算应飞航迹ψ_DTK：

${\mathrm{\ψ}}_3=\arctan \frac{({\mathrm{\λ}}_Q-{\mathrm{\λ}}_p)\cos {\mathrm{\φ}}_p}{{\mathrm{\φ}}_Q-{\mathrm{\φ}}_p}$

${\mathrm{\ψ}}_4=\arctan \frac{({\mathrm{\λ}}_{i+1}-{\mathrm{\λ}}_Q)\cos {\mathrm{\φ}}_Q}{{\mathrm{\φ}}_{i+1}-{\mathrm{\φ}}_Q}$

$\mathrm{\ΔTK}=\arccos \frac{R_p}{L_{\mathrm{NO}}}$

$L_{\mathrm{NO}}=\sqrt{{({\mathrm{\λ}}_p-{\mathrm{\λ}}_o)}^2{\cos }^2{\mathrm{\φ}}_o+{({\mathrm{\φ}}_p-{\mathrm{\φ}}_o)}^2}\×180/\mathrm{\π}\×60\×1852$

当L_NO≤R_p时，判断飞机到达相切圆。

其中：R_p为飞机转弯半径。

第四步，根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T：

ψ′_T＝ψ_DTK-δ；                  (24)

(10)根据接收惯性导航系统输出的真航向ψ_T及应飞航向ψ′_T，计算航向偏差

Δψ：Δψ＝ψ_T-ψ′_T；            (25)

(11)按照输入的切入距离D1计算d₂、θ₂：

$d_2=\sqrt{R_p^2+{D1}^2}$

${\mathrm{\θ}}_2=\arctan \frac{R_p}{D1}---\left(26\right)$

其中：R_p为飞机转弯半径。

(12)按照输入的机场位置(φ_r、λ_r)、切入航向ψ1和计算的横偏距X、d₂、θ₂，计算φ₀、λ₀：

${\mathrm{\φ}}_0={\mathrm{\φ}}_r+\frac{d_2\×\cos {\mathrm{\ψ}}^{\′}}{R}$

${\mathrm{\λ}}_0={\mathrm{\λ}}_r+\frac{d_2\×\sin {\mathrm{\ψ}}^{\′}}{R\cos {\mathrm{\φ}}_0}---\left(27\right)$

ψ′＝ψ1+sign(X)·θ₂+180°

其中：R为地球半径。

(13)根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的φ₀、λ₀，计算d₁、θ₁：

$d_1=\sqrt{{({\mathrm{\λ}}_p-{\mathrm{\λ}}_o)}^2{\cos }^2{\mathrm{\φ}}_o+{({\mathrm{\φ}}_p-{\mathrm{\φ}}_o)}^2}\×180/\mathrm{\π}\×60\×1852$

${\mathrm{\θ}}_1=\arcsin \frac{R_p}{d_1}---\left(28\right)$

其中：R_p为飞机转弯半径。

(14)根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的横偏距X、φ₀、λ₀，d₁、θ₁，计算ψ_P→M：

ψ_P→M＝ψ_P→0+θ₁×sign(X)            (29)

${\mathrm{\ψ}}_{p\→o}=\arctan \frac{({\mathrm{\λ}}_o-{\mathrm{\λ}}_p)\cos {\mathrm{\φ}}_p}{{\mathrm{\φ}}_o-{\mathrm{\φ}}_p}$

(15)根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的d₁、θ₁、ψ_P→M，计算φ₃、λ₃：

${\mathrm{\φ}}_3={\mathrm{\φ}}_p+\frac{d_1\×\cos {\mathrm{\ψ}}_{P\→M}}{R}---\left(30\right)$

${\mathrm{\λ}}_3={\mathrm{\λ}}_p+\frac{d_1\×\sin {\mathrm{\ψ}}_{P\→M}}{R\cos {\mathrm{\φ}}_3}$

其中：R为地球半径。

(16)根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的φ₃、λ₃，计算距离S₃：

$S_3=\sqrt{{({\mathrm{\λ}}_p-{\mathrm{\λ}}_3)}^2{\cos }^2{\mathrm{\φ}}_3+{({\mathrm{\φ}}_p-{\mathrm{\φ}}_3)}^2}\×180/\mathrm{\π}\×60\×1852---\left(31\right)$

(17)根据计算的距离S₃确定应飞高度H_p：

当距离S₃满足条件：H_FAF+S₃×tanγ-(H_c0-H_机场)≥0

其中：H_c0为接到返航指令时刻飞机的惯性气压高度；

H_机场为目标机场的海拔高度；

H_FAF为FAF点相对高度；

γ为返航下滑道坡度。

则给定高度H_p为：

$H_p=\left\{\begin{array}{cc}H\max & H_{c0}>H\max \\ H_{c0}& H_{c0}\≤H\max \end{array}\right.---\left(32\right)$

其中：Hmax为最大返航高度；

当距离S₃满足条件：H_c0＝H_FAF+S₃×tanγ+H_机场

则给定高度为：H_p＝H_FAF+S₃×tanγ+H_机场            (33)

当距离S₃满足条件：H_FAF+S₃×tanγ-(H_c0-H_机场)≤0

则判断为飞机在不允许的高度上飞行，此时应飞高度为：

H_p＝H_FAF+S₃×tanγ+ΔH₀+H_机场                     (34)

其中：ΔH₀＝H_c0-H_p0

H_p0为接到返航指令时飞机的应飞高度，单位：m，

(18)计算高度差ΔH：ΔH＝H_c-H_p，                  (35)

(19)输出航向偏差Δψ、横偏距X、高度偏差ΔH，

(五)着陆方法的实现步骤

(1)输入返航点高度H_FAF；

(2)接收无线电高度表输出的高度H_C；

(3)根据输入的返航点高度H_FAF和接收的无线电高度表输出的高度H_C，计算高度偏差ΔH：ΔH＝H_FAF-H_c                            (36)

(4)输出高度偏差ΔH。

本发明具有的优点和有益效果是，提出了一种全面、详细、自动化程度高、可操作性强的综合航路管理的实现方法，按照该方法计算飞行参数并控制飞机状态，可保证飞机安全、平稳地完成各项飞行任务，提供从飞机起飞到进近着陆的横向最优飞行剖面与垂直最优飞行剖面，飞机将按优化飞行轨迹从起飞机场到达目的地机场。

附图说明

图1是本发明控制流程示意图；

图2是本发明航迹操纵方式流程图；

图3是本发明航线操纵方式流程图；

图4是本发明时间导航方法流程图；

图5是本发明返航过程方法流程图；

图6是本发明着陆方法流程图。

具体实施方式

按照不同的飞行阶段与飞行轨迹的控制模式要求，分为航迹操纵、航线操纵两种操纵方式；在各操纵方式下，具有时间导航、返航、着陆三种导航方法，提供各种导航模式的水平和垂直飞行规划。

各操纵方式与导航方法的控制流程见图1

(1)航迹操纵方式

在航迹操纵方式下，按照预先装订的航路点顺序，计算并输出飞机由即时位置飞向目标航路点的有关参数和飞机操纵信息，帮助飞行员控制偏航角为零。

(2)航线操纵方式

在航线操纵方式下，根据选择的航路点序号确定预定航线和预定航迹角，保证飞机准确地按预定航线飞行。当飞机偏离航迹线后，系统输出偏航信号，指示飞机向预定航迹线靠近。根据飞机偏航距的大小，分别以不同的控制规律计算并输出导航参数。

(3)时间导航

为进行空中集结，系统根据距待飞目标点的距离和要求的待飞时间，确定应飞速度。时间导航功能可叠加在航线、航迹、综合三种导航方式上。

(4)返航

返航过程中，系统根据飞行员指定的机场点信息，输出飞机相对预定返航航线的应飞航向、航迹误差角、横偏距与高度偏差等导航参数。

(5)着陆

着陆分为水平面控制与垂直面控制，用于引导飞机在指定的机场着陆。

下面结构附图对本发明中的航迹操纵、航线操纵两种操纵方式及在各操纵方式下，具有时间导航、返航、着陆的三种导航方法，作详细说明。

(一)航迹操纵方式

航迹操纵方式用于保证飞机沿最短的飞行轨迹向给定的航路点飞行。它使飞机的飞行速度矢量始终对准航路点方向。

航迹操纵方式计算流程见图2，计算步骤如下：

(1)接收惯性导航系统输出的数据：即时经度λp、纬度Φp、真航向ψ_T、东向速度V_E、北向速度V_N；

(2)根据惯性导航系统预先装订的数据，获得当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1；

(3)按照接收惯性导航系统输出的东向速度V_E、北向速度V_N，计算航迹角ψ_TK：

ψ_TK＝tan^-1(V_E/V_N)；                       (1)

(4)按照接收惯性导航系统输出的真航向ψ_T与航迹角ψ_TK，计算偏流角δ：

δ＝ψ_TK-ψ_T；                             (2)

(5)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1，计算预航迹角 ${\mathrm{\Ψ}}_{\mathrm{DTK}}:{\mathrm{\ψ}}_{\mathrm{DTK}}={\cos }^{-1}({\stackrel{\→}{r}}_{N-P}\·{\stackrel{\→}{r}}_{i+1,i});$

其中，为即时位置当地真北的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和到达下一个航路点当地铅垂线的单位矢量。

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{N,p}\·{\stackrel{\→}{r}}_{i+1,i}=M_p\×M_N\×[-{\left(C_{\mathrm{zy}}\right)}_p\×p_1+{\left(C_{\mathrm{zx}}\right)}_p\×p_2]$

$M_N^{-1}=\sqrt{{\left(C_{\mathrm{zy}}\right)}_p^2+{\left(C_{\mathrm{zx}}\right)}_p^2}$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i+1×(C_zz)_i-(C_zz)_i+1×(C_zy)_i

p₂＝(C_zz)_i+1×(C_zx)_i-(C_zx)_i+1×(C_zz)_i

p₃＝(C_zx)_i+1×(C_zy)_i-(C_zy)_i+1×(C_zx)_i

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(6)根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T：

ψ′_T＝ψ_DTK-δ；               (4)

(7)根据接收惯性导航系统输出的真航向ψ_T及应飞航向ψ′_T，计算航向偏差Δ

ψ：Δψ＝ψ_T-ψ′_T；           (5)

(8)令横偏距X与高度偏差ΔH为0，即：X≡0，ΔH≡0；

(9)输出航向偏差Δψ、横偏距X、高度偏差ΔH。

(二)航线操纵方式

航线操纵方式用于保证飞机严格地按给定的航线飞行。接到航线操纵方式指令后，根据选择的航路点序号确定预定航线和预定航迹角，保证飞机准确地按预定航线飞行。

根据偏航距的大小，分四个区域以不同的控制规律计算并输出导航参数。

I区：此时飞机偏离预定航迹线的距离较大，系统将选择垂直于预定航迹线的方向作为应飞航向，飞行方向与预定航迹线相垂直，于是以最近的方向靠近预定航迹线。

II区：当飞机靠近航迹线时，为使其平缓地接近航迹线，考虑到飞机的惯性，让飞机做圆周运动，在该区域中飞机的应飞航向角为圆弧切线方向与真北的夹角，其圆弧半径为飞机的转弯半径。

III区：当飞机进一步接近航迹线时，飞机进入第三区域，此时飞机以半径Rd转弯，平缓地进入航迹线，使应飞航向等于预定航向，在以半径Rd转弯时，应飞航向角为圆弧切线方向与真北的夹角。

IV区：当飞机基本沿航迹线飞行时，飞机进入第四区域，此时飞机保持当前飞行状态即可。

航线操纵方式计算流程见图3，计算步骤如下：

(1)接收惯性导航系统输出的数据：即时经度λp、纬度Φp、真航向ψ_T、东向速度V_E、北向速度V_N；

(2)根据惯性导航系统预先装订的数据，获得当前航路点经度λi、纬度Φi，上一个航路点经度λi-1、纬度Φi-1；

(3)按照接收惯性导航系统输出的东向速度V_E、北向速度V_N，计算航迹角ψ_TK，

ψ_TK＝tan^-1(V_E/V_N)；               (6)

(4)按照接收惯性导航系统输出的真航向ψ_T与航迹角ψ_TK，计算偏流角δ，

δ＝ψ_TK-ψ_T；                      (7)

(5)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，上一个航路点经度λi-1、纬度Φi-1，计算飞机距预定航线距离，即横偏距X：

$X=-R\×{\sin }^{-1}({\stackrel{\→}{r}}_{i,i-1}\·{\stackrel{\→}{r}}_p)\×\frac{\mathrm{\π}}{180}---\left(8\right)$

其中：R为地球半径；

为即时位置当地铅垂线的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和上一个航路点当地铅垂线的单位矢量。按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i,i-1}\·{\stackrel{\→}{r}}_p=M_p\×[p_1\×{\left(C_{\mathrm{zx}}\right)}_p+p_2\×{\left(C_{\mathrm{zy}}\right)}_p+p_3\×{\left(C_{\mathrm{zz}}\right)}_p]$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)i×(C_zz)_i-1-(C_zz)_i×(C_zy)_i-1

p₂＝(C_zz)_i×(C_zx)_i-1-(C_zx)_i×(C_zz)_i-1

p₃＝(C_zx)_i×(C_zy)_i-1-(C_zy)_i×(C_zx)_i-1

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(6)根据计算得到的飞机距预定航线的横偏距X判断飞机处于四个区域的哪个区，判别条件如下：

其中：R_p为飞机转弯半径；

L、L₁、L₂为根据飞机性能和控制精度要求确定的各区域宽度参数。

(7)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，上一个航路点经度λi-1、纬度Φi-1，计算当前区域的预航迹ψ_DL：

${\mathrm{\ψ}}_{\mathrm{DL}}={\cos }^{-1}({\stackrel{\→}{r}}_{N,P}\·{\stackrel{\→}{r}}_{i,i-1})---\left(9\right)$

其中：为即时位置当地真北的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和上一个航路点当地铅垂线的单位矢量。

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{N,p}\·{\stackrel{\→}{r}}_{i,i-1}=M_p\×M_N\×[-{\left(C_{\mathrm{zy}}\right)}_p\×p_1+{\left(C_{\mathrm{zx}}\right)}_p\×p_2]$

$M_N^{-1}=\sqrt{{\left(C_{\mathrm{zy}}\right)}_p^2+{\left(C_{\mathrm{zx}}\right)}_p^2}$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i×(C_zz)_i-1-(C_zz)_i×(C_zy)_i-1

p₂＝(C_zz)_i×(C_zx)_i-1-(C_zx)_i×(C_zz)_i-1

p₃＝(C_zx)_i×(C_zy)_i-1-(C_zy)_i×(C_zx)_i-1

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(8)根据计算得到的预航迹ψ_DL、横偏距X，计算应飞航迹ψ_DT：

ψ_DT＝ψ_DL+ψ×sign(X)                        (10)

其中：

其中：X′＝|X|-L₁；

R_P为飞机转弯半径；

$\mathrm{Rd}=R_P\×\frac{L^2}{(L-L_1)\×\left|X\right|};$

ψ为应飞航迹和预航迹之间的夹角；

(9)根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T，

ψ′_T＝ψ_DTK-δ；                  (11)

(10)根据接收惯性导航系统输出的真航向ψ_T及应飞航向ψ′_T，计算航向偏差

Δψ：Δψ＝ψ_T-ψ′_T；            (12)

(11)令高度偏差为0，即：ΔH≡0；

(12)输出航向偏差Δψ、横偏距X、高度偏差ΔH。

(三)时间导航

时间导航不是独立的导航方式，它可叠加于航线操纵方式、航迹操纵方式之上。在以上各导航方式下，系统增加输出应飞速度参数。

时间导航计算流程见图4，计算步骤如下：

(1)接收惯性导航系统输出的数据：即时经度λp、纬度Φp、时间t_即时；

(2)根据惯性导航系统预先装订的数据，获得下一个航路点经度λi+1、纬度Φi+1、到达时间t_目标；

(3)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，计算即时位置到目标航路点的距离D：

$D=R\×\frac{\mathrm{\π}}{180}\×{\cos }^{-1}({\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p)---\left(13\right)$

其中：R为地球半径；

为下一个航路点当地铅垂线的单位矢量；

为即时位置当地铅垂线的单位矢量。

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p={\left(C_{\mathrm{zx}}\right)}_{i+1}\×{\left(C_{\mathrm{zx}}\right)}_p+{\left(C_{\mathrm{zy}}\right)}_{i+1}\×{\left(C_{\mathrm{zy}}\right)}_p+{\left(C_{\mathrm{zz}}\right)}_{i+1}\×{\left(C_{\mathrm{zz}}\right)}_p$

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(4)按照接收惯性导航系统输出的时间t_即时和惯性导航系统预先装订的到达时间t_目标，以及计算的距离D，计算应飞速度V_应飞：

t_应飞＝t_目标-t_即时

其中：Vmax为飞机最大巡航速度；

Vmin为飞机最小巡航速度。

(5)输出应飞速度V_应飞。

(四)返航

返航用于引导飞机到达目标机场。返航过程中，水平和高度通道单独控制，计算流程见图5。

(四.一)高度通道

整个返航过程根据高度分为两个阶段：

第一阶段，水平飞行阶段：将程序设定高度H_设定与现实高度进行比较，当接通返航状态时的高度H_接通≥Hmax(最大返航高度)时，H_设定＝Hmax，当H_接通≤Hmax时，H_设定＝H_接通。

第二阶段，下降飞行阶段：进入下降轨迹线飞行，H_设定按一定规律变化，使飞机沿下滑线飞行，使飞机平缓地进入着陆状态。

(四.二)水平通道

水平轨迹控制根据待飞距的大小分为两个阶段：

a)当待飞距离D≥LD(返航距离)时按航迹操纵方式向目标点飞行；

b)当待飞距离D≤LD时，飞行航线的控制过程又分为两个部分：

第一部分以航迹操纵方式飞行，形成到以飞机最小转弯半径为圆转换点的应飞航向和预定航迹角，圆的位置由航路点的切入方向和切入距离来确定。

第二部分以航线操纵方式飞行，在飞越航路点后自动地按下一个航路点计算导航参数，保持操纵方式不变。

(四.三)计算步骤

(1)输入机场位置(φ_r、λ_r)与高度H，并设置为目标点；

(2)输入切入距离D1、切入航向ψ₁，FAF点高度H_FAF，返航距离LD；

(3)接收惯性导航系统输出的数据：即时经度λp、纬度Φp、真航向ψ_T、东向速度V_E、北向速度V_N；

(4)根据惯性导航系统预先装订的数据，获得当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1；

(5)按照接收惯性导航系统输出的东向速度V_E、北向速度V_N，计算航迹角ψ_TK：

ψ_TK＝tan^-1(V_E/V_N)；                            (15)

(6)按照接收惯性导航系统输出的真航向ψ_T与航迹角ψ_TK，计算偏流角δ：

δ＝ψ_TK-ψ_T；                                  (16)

(7)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1，计算飞机距预定航线距离，即横偏距X：

$X=-R\×{\sin }^{-1}({\stackrel{\→}{r}}_{i,i-1}\·{\stackrel{\→}{r}}_p)\×\frac{\mathrm{\π}}{180}---\left(17\right)$

其中：R为地球半径；

为即时位置当地铅垂线的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和下一个航路点当地铅垂线的单位矢量。

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i,i-1}\·{\stackrel{\→}{r}}_p=M_p\×[p_1\×{\left(C_{\mathrm{zx}}\right)}_p+p_2\×{\left(C_{\mathrm{zy}}\right)}_p+p_3\×{\left(C_{\mathrm{zz}}\right)}_p]$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i+1×(C_zz)_i-(C_zz)_i+1×(C_zy)_i

p₂＝(C_zz)_i+1×(C_zx)_i-(C_zx)_i+1×(C_zz)_i

p₃＝(C_zx)_i+1×(C_zy)_i-(C_zy)_i+1×(C_zx)_i

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(8)按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，计算即时位置到目标航路点的待飞距D：

$D=R\×\frac{\mathrm{\π}}{180}\×{\cos }^{-1}({\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p)---\left(18\right)$

其中：R为地球半径；

为即时位置当地铅垂线的单位矢量；

为下一个航路点当地铅垂线的单位矢量。

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p={\left(C_{\mathrm{zx}}\right)}_{i+1}\×{\left(C_{\mathrm{zx}}\right)}_p+{\left(C_{\mathrm{zy}}\right)}_{i+1}\×{\left(C_{\mathrm{zy}}\right)}_p+{\left(C_{\mathrm{zz}}\right)}_{i+1}\×{\left(C_{\mathrm{zz}}\right)}_p$

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

(9)当待飞距D≥LD时，按以下步骤计算应飞航向ψ′_T：

第一步，按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1，计算预航迹角ψ_DTK： ${\mathrm{\ψ}}_{\mathrm{DTK}}={\cos }^{-1}({\stackrel{\→}{r}}_{N-P}\·{\stackrel{\→}{r}}_{i+1,i});---\left(19\right)$

其中，为即时位置当地真北的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和到达下一个航路点当地铅垂线的单位矢量。

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{N,p}\·{\stackrel{\→}{r}}_{i+1,i}=M_p\×M_N\×[-{\left(C_{\mathrm{zy}}\right)}_p\×p_1+{\left(C_{\mathrm{zx}}\right)}_p\×p_2]$

$M_N^{-1}=\sqrt{{\left(C_{\mathrm{zy}}\right)}_p^2+{\left(C_{\mathrm{zx}}\right)}_p^2}$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i+1×(C_zz)_i-(C_zz)_i+1×(C_zy)_i

p₂＝(C_zz)_i+1×(C_zx)_i-(C_zx)_i+1×(C_zz)_i

p₃＝(C_zx)_i+1×(C_zy)_i-(C_zy)_i+1×(C_zx)_i

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

第二步，根据计算得到的偏流角δ和预航迹角ψDTK，计算应飞航向ψ′_T：ψ′_T＝ψ_DTK-δ；                     (20)

(10)当待飞距D≤LD时，按以下步骤计算应飞航向ψ′_T：

第一步，根据惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，切入距离D1、切入航向ψ1和计算的横偏距X，计算相切圆的位置(φ₀、λ₀)：

${\mathrm{\φ}}_0={\mathrm{\φ}}_{i+1}+\frac{\mathrm{OP}\×\cos {\mathrm{\ψ}}^{\′}}{R}$

${\mathrm{\λ}}_0={\mathrm{\λ}}_{i+1}\frac{\mathrm{OP}\×\sin {\mathrm{\ψ}}^{\′}}{R\×\cos {\mathrm{\φ}}_{i+1}}---\left(21\right)$

$\mathrm{OP}=\sqrt{{D1}^2+R_p^2}$

其中：R为地球半径；

${\mathrm{\ψ}}^{\′}=\mathrm{\ψ}1+\mathrm{sign}\left(X\right)\×\arcsin \frac{R_p}{\mathrm{OP}}+180$

R_p为飞机转弯半径；

(ψ1、D1)：切入航向、切入距离。

第二步，根据惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，切入距离D1、切入航向ψ1和计算的横偏距X，计算切入点位置(φ_Q、λ_Q)：

${\mathrm{\φ}}_Q={\mathrm{\φ}}_{i+1}+\frac{D1\×\cos {\mathrm{\ψ}}_1}{R}$

${\mathrm{\λ}}_Q={\mathrm{\λ}}_{i+1}+\frac{D1\×\sin {\mathrm{\ψ}}^{\′}}{R\×\cos {\mathrm{\φ}}_{i+1}}---\left(22\right)$

其中：R为地球半径；

${\mathrm{\ψ}}^{\′}=\mathrm{\ψ}1+\mathrm{sign}\left(X\right)\×\arcsin \frac{R_p}{\mathrm{OP}}+180$

R_p为飞机转弯半径；

(ψ1、D1)：切入航向、切入距离。

第三步，按照接收惯性导航系统输出的即时经度λp、纬度Φp，计算的相切圆的位置(φ₀、λ₀)、切入点位置(φ_Q、λ_Q)和横偏距X计算应飞航迹ψ_DTK：

${\mathrm{\ψ}}_3=\arctan \frac{({\mathrm{\λ}}_Q-{\mathrm{\λ}}_p)\cos {\mathrm{\φ}}_p}{{\mathrm{\φ}}_Q-{\mathrm{\φ}}_p}$

${\mathrm{\ψ}}_4=\arctan \frac{({\mathrm{\λ}}_{i+1}-{\mathrm{\λ}}_Q)\cos {\mathrm{\φ}}_Q}{{\mathrm{\φ}}_{i+1}-{\mathrm{\φ}}_Q}$

$\mathrm{\ΔTK}=\arccos \frac{R_p}{L_{\mathrm{NO}}}$

$L_{\mathrm{NO}}=\sqrt{{({\mathrm{\λ}}_p-{\mathrm{\λ}}_o)}^2{\cos }^2{\mathrm{\φ}}_0+{({\mathrm{\φ}}_p-{\mathrm{\φ}}_o)}^2}\×180/\mathrm{\π}\×60\×1852$

当L_NO≤R_p时，判断飞机到达相切圆。

其中：R_p为飞机转弯半径。

第四步，根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T：

ψ′_T＝ψ_DTK-δ；                        (24)

(10)根据接收惯性导航系统输出的真航向ψ_T及应飞航向ψ′_T，计算航向偏差

Δψ：Δψ＝ψ_T-ψ′_T；                  (25)

(11)按照输入的切入距离D1计算d₂、θ₂：

$d_2=\sqrt{R_p^2+{D1}^2}$

${\mathrm{\θ}}_2=\arctan \frac{R_p}{D1}---\left(26\right)$

其中：R_p为飞机转弯半径。

(12)按照输入的机场位置(φ_r、λ_r)、切入航向ψ₁和计算的横偏距X、d₂、θ₂，计算φ₀、λ₀：

${\mathrm{\φ}}_0={\mathrm{\φ}}_r+\frac{d_2\×\cos {\mathrm{\ψ}}^{\′}}{R}$

${\mathrm{\λ}}_0={\mathrm{\λ}}_r+\frac{d_2\×\sin {\mathrm{\ψ}}^{\′}}{R\cos {\mathrm{\φ}}_0}---\left(27\right)$

ψ′＝ψ1+sign(X)·θ₂+180°

其中：R为地球半径。

(13)根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的φ₀、λ₀，计算d₁、θ₁：

$d_1=\sqrt{{({\mathrm{\λ}}_p-{\mathrm{\λ}}_o)}^2{\cos }^2{\mathrm{\φ}}_o+{({\mathrm{\φ}}_p-{\mathrm{\φ}}_o)}^2}\×180/\mathrm{\π}\×60\×1852$

${\mathrm{\θ}}_1=\arcsin \frac{R_p}{d_1}---\left(28\right)$

其中：R_p为飞机转弯半径。

(14)根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的横偏距X、φ₀、λ₀，d₁、θ₁，计算ψ_P→M：

ψ_P→M＝ψ_P→0+θ₁×sign(X)                      (29)

${\mathrm{\ψ}}_{p\→o}=\arctan \frac{({\mathrm{\λ}}_o-{\mathrm{\λ}}_p)\cos {\mathrm{\φ}}_p}{{\mathrm{\φ}}_o-{\mathrm{\φ}}_p}$

(15)根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的d₁、θ₁、ψ_P→M，计算φ₃、λ₃：

${\mathrm{\φ}}_3={\mathrm{\φ}}_p+\frac{d_1\×\cos {\mathrm{\ψ}}_{P\→M}}{R}---\left(30\right)$

${\mathrm{\λ}}_3={\mathrm{\λ}}_p+\frac{d_1\×\sin {\mathrm{\ψ}}_{P\→M}}{R\cos {\mathrm{\φ}}_3}$

其中：R为地球半径。

(16)根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的φ₃、λ₃，计算距离S₃：

$S_3=\sqrt{{({\mathrm{\λ}}_p-{\mathrm{\λ}}_3)}^2{\cos }^2{\mathrm{\φ}}_3+{({\mathrm{\φ}}_p-{\mathrm{\φ}}_3)}^2}\×180/\mathrm{\π}\×60\×1852---\left(31\right)$

(17)根据计算的距离S₃确定应飞高度H_p：

当距离S₃满足条件：H_FAF+S₃×tanγ-(H_c0-H_机场)≥0

其中：H_c0为接到返航指令时刻飞机的惯性气压高度；

H_机场为目标机场的海拔高度；

H_FAF为FAF点相对高度；

γ为返航下滑道坡度。

则给定高度H_p为：

$H_p=\left\{\begin{array}{cc}H\max & H_{c0}>H\max \\ H_{c0}& H_{c0}\≤H\max \end{array}\right.---\left(32\right)$

其中：Hmax为最大返航高度。

当距离S₃满足条件：H_c0＝H_FAF+S₃×tanγ+H_机场

则给定高度为：H_p＝H_FAF+S₃×tanγ+H_机场        (33)

当距离S₃满足条件：H_FAF+S₃×tanγ-(H_c0-H_机场)≤0

则判断为飞机在不允许的高度上飞行，此时应飞高度为：

H_p＝H_FAF+S₃×tanγ+ΔH₀+H_机场                 (34)

其中：ΔH₀＝H_c0-H_p0

H_p0为接到返航指令时飞机的应飞高度，单位：m。

(18)计算高度差ΔH：＝H_c-H_p。                  (35)

(19)输出航向偏差Δψ、横偏距X、高度偏差ΔH。

(五)着陆

着陆用于引导飞机在目标机场着陆，水平面控制与所需参数同返航过程。

着陆计算流程见图6，计算步骤如下：

(1)输入返航点高度H_FAF；

(2)接收无线电高度表输出的高度H_C；

(3)根据输入的返航点高度H_FAF和接收的无线电高度表输出的高度H_C，计算高度偏差ΔH：ΔH＝H_FAF-H_c                        (36)

(4)输出高度偏差ΔH。

Claims (1)

1.一种基于惯性导航综合航路管理的实现方法，其特征是，综合航路管理的实现方法包括航迹操纵、航线操纵两种操纵方式和时间导航、返航、着陆三种导航方法，其中

（一）航迹操纵方式实现步骤：

（1）接收惯性导航系统输出的数据：即时经度λp、纬度Φp、真航向ψ_T、东向速度V_E、北向速度V_N；

（2）根据惯性导航系统预先装订的数据，获得当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1；

（3）按照接收惯性导航系统输出的东向速度V_E、北向速度V_N，计算航迹角ψ_TK：ψ_TK＝tan^-1(V_E/V_N)；   （1）

（4）按照接收惯性导航系统输出的真航向ψ_T与航迹角ψ_TK，计算偏流角δ：δ＝ψ_TK-ψ_T；   （2）

（5）按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1，计算预航迹角ψ_DTK： ${\mathrm{\ψ}}_{\mathrm{DTK}}={\cos }^{-1}({\stackrel{\→}{r}}_{N-P}\·{\stackrel{\→}{r}}_{i+1,i});---\left(3\right)$

其中，为即时位置当地真北的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和到达下一个航路点当地铅垂线的单位矢量，

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{N,p}\·{\stackrel{\→}{r}}_{i+1,i}=M_p\×M_N\×[-{\left(C_{\mathrm{zy}}\right)}_p\×p_1+{\left(C_{\mathrm{zx}}\right)}_p\×p_2]$

$M_N^{-1}=\sqrt{{\left(C_{\mathrm{zy}}\right)}_p^2+{\left(C_{\mathrm{zx}}\right)}_p^2}$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i+1×(C_zz)_i-(C_zz)_i+1×(C_zy)_i

p₂＝(C_zz)_i+1×(C_zx)_i-(C_zx)_i+1×(C_zz)_i

p₃＝(C_zx)_i+1×(C_zy)_i-(C_zy)_i+1×(C_zx)_i

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

（6）根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T：ψ′_T＝ψ_DTK-δ；   (4)

（7）根据接收惯性导航系统输出的真航向ψ_T及应飞航向ψ′_T，计算航向偏差Δψ：Δψ＝ψ_T-ψ′_T；   (5)

（8）令横偏距X与高度偏差ΔH为0，即：X≡0，ΔH≡0；

（9）输出航向偏差Δψ、横偏距X、高度偏差ΔH；

（二）航线操纵方式实现步骤：

（1）接收惯性导航系统输出的数据：即时经度λp、纬度Φp、真航向ψ_T、东向速度V_E、北向速度V_N；

（2）根据惯性导航系统预先装订的数据，获得当前航路点经度λi、纬度Φi，上一个航路点经度λi-1、纬度Φi-1；

（3）按照接收惯性导航系统输出的东向速度V_E、北向速度V_N，计算航迹角ψ_TK，ψ_TK＝tan^-1(V_E/V_N)；   （6）

（4）按照接收惯性导航系统输出的真航向ψ_T与航迹角ψ_TK，计算偏流角δ，δ＝ψ_TK-ψ_T；   （7）

（5）按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，上一个航路点经度λi-1、纬度Φi-1，计算飞机距预定航线距离，即横偏距X：

$X=-R\×{\sin }^{-1}({\stackrel{\→}{r}}_{i,i-1}\·{\stackrel{\→}{r}}_p)\×\frac{\mathrm{\π}}{180}---\left(8\right)$

其中：R为地球半径；

为即时位置当地铅垂线的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和上一个航路点当地铅垂线的单位矢量；

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i,i-1}\·{\stackrel{\→}{r}}_p=M_p\×[p_1\×{\left(C_{\mathrm{zx}}\right)}_p+p_2\×{\left(C_{\mathrm{zy}}\right)}_p+p_3\×{\left(C_{\mathrm{zz}}\right)}_p]$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i×(C_zz)_i-1-(C_zz)_i×(C_zy)_i-1

p₂＝(C_zz)_i×(C_zx)_i-1-(C_zx)_i×(C_zz)_i-1

p₃＝(C_zx)_i×(C_zy)_i-1-(C_zy)_i×(C_zx)_i-1

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

（6）根据计算得到的飞机距预定航线的横偏距X判断飞机处于四个区域的哪个区，判别条件如下：

其中：R_p为飞机转弯半径；

L、L₁、L₂为根据飞机性能和控制精度要求确定的各区域宽度参数；

（7）按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，上一个航路点经度λi-1、纬度Φi-1，计算当前区域的预航迹ψ_DL：

${\mathrm{\ψ}}_{\mathrm{DL}}={\cos }^{-1}({\stackrel{\→}{r}}_{N,P}\·{\stackrel{\→}{r}}_{i,i-1})---\left(9\right)$

其中：为即时位置当地真北的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和上一个航路点当地铅垂线的单位矢量；

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{N,p}\·{\stackrel{\→}{r}}_{i,i-1}=M_p\×M_N\×[-{\left(C_{\mathrm{zy}}\right)}_p\×p_1+{\left(C_{\mathrm{zx}}\right)}_p\×p_2]$

$M_N^{-1}=\sqrt{{\left(C_{\mathrm{zy}}\right)}_p^2+{\left(C_{\mathrm{zx}}\right)}_p^2}$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i×(C_zz)_i-1-(C_zz)_i×(C_zy)_i-1

p₂＝(C_zz)_i×(C_zx)_i-1-(C_zx)_i×(C_zz)_i-1

p₃＝(C_zx)_i×(C_zy)_i-1-(C_zy)_i×(C_zx)_i-1

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

（8）根据计算得到的预航迹ψ_DL、横偏距X，计算应飞航迹ψ_DT：

ψ_DT＝ψ_DL+ψ×sign(X)   (10)

其中：

其中：X′＝|X|-L₁；

R_P为飞机转弯半径；

$\mathrm{Rd}=R_P\×\frac{L^2}{(L-L_1)\×\left|X\right|};$

ψ为应飞航迹和预航迹之间的夹角；

（9）根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T，ψ′_T＝ψ_DTK-δ；   (11)

（10）根据接收惯性导航系统输出的真航向ψ_T及应飞航向ψ′_T，计算航向偏差Δψ：Δψ＝ψ_T-ψ′_T；   (12)

（11）令高度偏差为0，即：ΔH≡0；

（12）输出航向偏差Δψ、横偏距X、高度偏差ΔH；

（三）时间导航方法实现步骤

（1）接收惯性导航系统输出的数据：即时经度λp、纬度Φp、时间t_即时；

（2）根据惯性导航系统预先装订的数据，获得下一个航路点经度λi+1、纬度Φi+1、到达时间t_目标；

（3）按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，计算即时位置到目标航路点的距离D：

$D=R\×\frac{\mathrm{\π}}{180}\×{\cos }^{-1}({\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p)---\left(13\right)$

其中：R为地球半径；

为下一个航路点当地铅垂线的单位矢量；

为即时位置当地铅垂线的单位矢量；

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p={\left(C_{\mathrm{zx}}\right)}_{i+1}\×{\left(C_{\mathrm{zx}}\right)}_p+{\left(C_{\mathrm{zy}}\right)}_{i+1}\×{\left(C_{\mathrm{zy}}\right)}_p+{\left(C_{\mathrm{zz}}\right)}_{i+1}\×{\left(C_{\mathrm{zz}}\right)}_p$

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

（4）按照接收惯性导航系统输出的时间t_即时和惯性导航系统预先装订的到达时间t_目标，以及计算的距离D，计算应飞速度V_应飞：

t_应飞＝t_目标-t_即时

其中：Vmax为飞机最大巡航速度；

Vmin为飞机最小巡航速度；

（5）输出应飞速度V_应飞；

（四）返航方法实现步骤

（1）输入机场位置（φ_r、λ_r）与高度H，并设置为目标点；

（2）输入切入距离D1、切入航向ψ1，返航点高度H_FAF，返航距离LD；

（3）接收惯性导航系统输出的数据：即时经度λp、纬度Φp、真航向ψ_T、东向速度V_E、北向速度V_N、惯性气压高度Hc；

（4）根据惯性导航系统预先装订的数据，获得当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1；

（5）按照接收惯性导航系统输出的东向速度V_E、北向速度V_N，计算航迹角ψ_TK：ψ_TK＝tan^-1(V_E/V_N)；   (15)

（6）按照接收惯性导航系统输出的真航向ψ_T与航迹角ψ_TK，计算偏流角δ：δ＝ψ_TK-ψ_T；   (16)

（7）按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1，计算飞机距预定航线距离，即横偏距X：

$X=-R\×{\sin }^{-1}({\stackrel{\→}{r}}_{i+1,i}\·{\stackrel{\→}{r}}_p)\×\frac{\mathrm{\π}}{180}---\left(17\right)$

其中：R为地球半径；

为即时位置当地铅垂线的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和下一个航路点当地铅垂线的单位矢量；

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i+1,i}\·{\stackrel{\→}{r}}_p=M_p\×[p_1\×{\left(C_{\mathrm{zx}}\right)}_p+p_2\×{\left(C_{\mathrm{zy}}\right)}_p+p_3\×{\left(C_{\mathrm{zz}}\right)}_p]$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i+1×(C_zz)_i-(C_zz)_i+1×(C_zy)_i

p₂＝(C_zz)_i+1×(C_zx)_i-(C_zx)_i+1×(C_zz)_i

p₃＝(C_zx)_i+1×(C_zy)_i-(C_zy)_i+1×(C_zx)_i

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

（8）按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，计算即时位置到目标航路点的待飞距D：

$D=R\×\frac{\mathrm{\π}}{180}\×{\cos }^{-1}({\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p)---\left(18\right)$

其中：R为地球半径；

为即时位置当地铅垂线的单位矢量；

为下一个航路点当地铅垂线的单位矢量；

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{i+1}\·{\stackrel{\→}{r}}_p={\left(C_{\mathrm{zx}}\right)}_{i+1}\×{\left(C_{\mathrm{zx}}\right)}_p+{\left(C_{\mathrm{zy}}\right)}_{i+1}\×{\left(C_{\mathrm{zy}}\right)}_p+{\left(C_{\mathrm{zz}}\right)}_{i+1}\×{\left(C_{\mathrm{zz}}\right)}_p$

C_zx＝cosφ×cosλ

Cz_y＝cosφ×sinλ

C_zz＝sinφ

（9）当待飞距D≥LD时，按以下步骤计算应飞航向ψ′_T：

第一步，按照接收惯性导航系统输出的即时经度λp、纬度Φp和惯性导航系统预先装订的当前航路点经度λi、纬度Φi，下一个航路点经度λi+1、纬度Φi+1，计算预航迹角ψ_DTK： ${\mathrm{\ψ}}_{\mathrm{DTK}}={\cos }^{-1}({\stackrel{\→}{r}}_{N-P}\·{\stackrel{\→}{r}}_{i+1,i});---\left(19\right)$

其中，为即时位置当地真北的单位矢量；

为垂直于由和组成平面的单位矢量；

和分别为当前航路点和到达下一个航路点当地铅垂线的单位矢量；

按照标准球面坐标计算公式，有：

${\stackrel{\→}{r}}_{N,p}\·{\stackrel{\→}{r}}_{i+1,i}=M_p\×M_N\×[-{\left(C_{\mathrm{zy}}\right)}_p\×p_1+{\left(C_{\mathrm{zx}}\right)}_p\×p_2]$

$M_N^{-1}=\sqrt{{\left(C_{\mathrm{zy}}\right)}_p^2+{\left(C_{\mathrm{zx}}\right)}_p^2}$

$M_p^{-1}=\sqrt{p_1^2+p_2^2+p_3^2}$

p₁＝(C_zy)_i+1×(C_zz)_i-(C_zz)_i+1×(C_zy)_i

p₂＝(C_zz)_i+1×(C_zx)_i-(C_zx)_i+1×(C_zz)_i

p₃＝(C_zx)_i+1×(C_zy)_i-(C_zy)_i+1×(C_zx)_i

C_zx＝cosφ×cosλ

C_zy＝cosφ×sinλ

C_zz＝sinφ

第二步，根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T：ψ′_T＝ψ_DTK-δ；   (20)

（10）当待飞距D<LD时，按以下步骤计算应飞航向ψ′_T：

第一步，根据惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，切入距离D1、切入航向ψ1和计算的横偏距X，计算相切圆的位置

${\mathrm{\&phi;}}_0={\mathrm{\&phi;}}_{i+1}+\frac{\mathrm{OP}\&times;\cos {\mathrm{\&psi;}}^{\&prime;}}{R}$

${\mathrm{\&lambda;}}_0={\mathrm{\&lambda;}}_{i+1}+\frac{\mathrm{OP}\&times;\sin {\mathrm{\&psi;}}^{\&prime;}}{R\&times;\cos {\mathrm{\&phi;}}_{i+1}}---\left(21\right)$

$\mathrm{OP}=\sqrt{{D1}^2+R_p^2}$

其中：R为地球半径；

${\mathrm{\&psi;}}^{\&prime;}=\mathrm{\&psi;}1+\mathrm{sign}\left(X\right)+\arcsin \frac{R_p}{\mathrm{OP}}+180$

R_p为飞机转弯半径；

（ψ1、D1）：切入航向、切入距离；

第二步，根据惯性导航系统预先装订的下一个航路点经度λi+1、纬度Φi+1，切入距离D1、切入航向ψ1和计算的横偏距X，计算切入点位置

${\mathrm{\&phi;}}_Q={\mathrm{\&phi;}}_{i+1}+\frac{D1\&times;\cos \mathrm{\&psi;}1}{R}$

${\mathrm{\&lambda;}}_Q={\mathrm{\&lambda;}}_{i+1}+\frac{D1\&times;\sin {\mathrm{\&psi;}}^{\&prime;}}{R\&times;\cos {\mathrm{\&phi;}}_{i+1}}---\left(22\right)$

其中：R为地球半径；

${\mathrm{\&psi;}}^{\&prime;}=\mathrm{\&psi;}1+\mathrm{sign}\left(X\right)\&times;\arcsin \frac{R_p}{\mathrm{OP}}+180$

R_p为飞机转弯半径；

（ψ1、D1）：切入航向、切入距离；

第三步，按照接收惯性导航系统输出的即时经度λp、纬度Φp，计算的相切圆的位置切入点位置和横偏距X计算应飞航迹ψ_DTK：

${\mathrm{\&psi;}}_3=\arctan \frac{({\mathrm{\&lambda;}}_Q-{\mathrm{\&lambda;}}_p)\cos {\mathrm{\&phi;}}_p}{{\mathrm{\&phi;}}_Q-{\mathrm{\&phi;}}_p}$

${\mathrm{\&psi;}}_4=\arctan \frac{({\mathrm{\&lambda;}}_{i+1}-{\mathrm{\&lambda;}}_Q)\cos {\mathrm{\&phi;}}_Q}{{\mathrm{\&phi;}}_{i+1}-{\mathrm{\&phi;}}_Q}$

$\mathrm{\&Delta;TK}=\arccos \frac{R_p}{L_{\mathrm{NO}}}$

$L_{\mathrm{NO}}=\sqrt{{({\mathrm{\&lambda;}}_p-{\mathrm{\&lambda;}}_o)}^2{\cos }^2{\mathrm{\&phi;}}_o+{({\mathrm{\&phi;}}_p-{\mathrm{\&phi;}}_o)}^2}\&times;180/\mathrm{\&pi;}\&times;60\&times;1852$

当L_NO≤R_p时，判断飞机到达相切圆；

其中：R_p为飞机转弯半径；

第四步，根据计算得到的偏流角δ和预航迹角ψ_DTK，计算应飞航向ψ′_T：ψ′_T＝ψ_DTK-δ；   (24)

（10）根据接收惯性导航系统输出的真航向ψ_T及应飞航向ψ′_T，计算航向偏差Δψ：Δψ＝ψ_T-ψ′_T；   (25)

（11）按照输入的切入距离D1计算d₂、θ₂：

$d_2=\sqrt{R_p^2+{D1}^2}$

${\mathrm{\&theta;}}_2=\arctan \frac{R_p}{D1}---\left(26\right)$

其中：R_p为飞机转弯半径；

（12）按照输入的机场位置（φ_r、λ_r）、切入航向ψ和计算的横偏距X、d₂、θ₂，计算φ₀、λ₀：

${\mathrm{\&phi;}}_0={\mathrm{\&phi;}}_r+\frac{d_2\&times;\cos {\mathrm{\&psi;}}^{\&prime;}}{R}$

${\mathrm{\&lambda;}}_0={\mathrm{\&lambda;}}_r+\frac{d_2\&times;\sin {\mathrm{\&psi;}}^{\&prime;}}{R\cos {\mathrm{\&phi;}}_0}---\left(27\right)$

ψ′＝ψ+sign(X)·θ₂+180°

其中：R为地球半径；

（13）根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的φ₀、λ₀，计算d₁、θ₁：

$d_1=\sqrt{{({\mathrm{\&lambda;}}_p-{\mathrm{\&lambda;}}_o)}^2{\cos }^2{\mathrm{\&phi;}}_o+{({\mathrm{\&phi;}}_p-{\mathrm{\&phi;}}_o)}^2}\&times;180/\mathrm{\&pi;}\&times;60\&times;1852$

${\mathrm{\&theta;}}_1=\arcsin \frac{R_p}{d_1}---\left(28\right)$

其中：R_p为飞机转弯半径；

（14）根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的横偏距X、φ₀、λ₀，d₁、θ₁，计算ψ_P→M：

ψ_P→M＝ψ_P→0+θ₁×sign(X)   (29)

${\mathrm{\&psi;}}_{p\&RightArrow;o}=\arctan \frac{({\mathrm{\&lambda;}}_o-{\mathrm{\&lambda;}}_p)\cos {\mathrm{\&phi;}}_p}{{\mathrm{\&phi;}}_o-{\mathrm{\&phi;}}_p}$

（15）根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的d₁、θ₁、ψ_P→M，计算φ₃、λ₃：

${\mathrm{\&phi;}}_3={\mathrm{\&phi;}}_p+\frac{d_1\&times;\cos {\mathrm{\&psi;}}_{P\&RightArrow;M}}{R}---\left(30\right)$

${\mathrm{\&lambda;}}_3={\mathrm{\&lambda;}}_p+\frac{d_1\&times;\sin {\mathrm{\&psi;}}_{P\&RightArrow;M}}{R\cos {\mathrm{\&phi;}}_3}$

其中：R为地球半径；

（16）根据接收惯性导航系统输出的即时经度λp、纬度Φp和计算的φ₃、λ₃，计算距离S₃：

$S_3=\sqrt{{({\mathrm{\&lambda;}}_p-{\mathrm{\&lambda;}}_3)}^2{\cos }^2{\mathrm{\&phi;}}_3+{({\mathrm{\&phi;}}_p-{\mathrm{\&phi;}}_3)}^2}\&times;180/\mathrm{\&pi;}\&times;60\&times;1852$

（17）根据计算的距离S₃确定应飞高度H_p：

当距离S₃满足条件：H_FAF+S₃×tanγ-(H_c0-H_机场）≥0

其中：H_c0为接到返航指令时刻飞机的惯性气压高度；

H_机场为目标机场的海拔高度；

H_FAF为FAF点相对高度；

γ为返航下滑道坡度；

则给定高度H_p为：

$H_p=\left\{\begin{array}{cc}H\max & H_{c0}>H\max \\ H_{c0}& H_{c0}\&le;H\max \end{array}\right.---\left(32\right)$

其中：Hmax为最大返航高度；

当距离S₃满足条件：H_c0＝H_FAF+S₃×tanγ+H_机场

则给定高度为：H_p＝H_FAF+S₃×tanγ+H_机场   （33）

当距离S₃满足条件：H_FAF+S₃×tanγ-(H_c0-H_机场）＜0

则判断为飞机在不允许的高度上飞行，此时应飞高度为：

H_p＝H_FAF+S₃×tanγ+ΔH₀+H_机场   （34）

其中：ΔH₀＝H_c0-H_p0

H_p0为接到返航指令时飞机的应飞高度，单位：m，

（18）计算高度差ΔH：ΔH＝H_c-H_p，   （35）

（19）输出航向偏差Δψ、横偏距X、高度偏差ΔH，

（五）着陆方法的实现步骤

（1）输入返航点高度H_FAF；

（2）接收无线电高度表输出的高度H_C；

（3）根据输入的返航点高度H_FAF和接收的无线电高度表输出的高度H_C，计算高度偏差ΔH：ΔH＝H_FAF-H_c   （36）

（4）输出高度偏差ΔH。