技术领域
[0001] 本
发明涉及一种飞行器在等高航路点间的定高度大圆飞行实现方法，一种在考虑地球
曲率情况下飞行器在等高航路点间的定高度大圆飞行的实现方法，属于飞行控制领域。
背景技术
[0002] 等高航路点间的定高度大圆飞行是指飞行器在相邻等高航路点间均沿给定高度进行大圆飞行的一种飞行方式，是以两个等高航路点间的定高度大圆飞行为
基础的。据此，本发明着重介绍两个预置等高航路点间的定高度大圆飞行实现方法。两个等高航路点间的飞行器定高度大圆飞行是指飞行器在由两个预置航路点和地心确定的平面内从第一个预置航路点开始沿定高度飞至第二个预置航路点的过程，示意图参见
附图1。定高度大圆飞行作为定高度飞行的最简单和最基本形式，对其进行深入研究有着重要的意义。基于实际需求(如研究定高度飞行火箭在地形摄影和探矿的工作效能、无人机在定高度掠地或掠海平飞时的作战效能等)，经常需要通过计算机仿真研究飞行器在定高度飞行时的工作效能。当所选飞行器的工作区域较小时，一般可将地球表面近似为平面进行处理；但是，如果工作区域较大，同样的近似处理将导致仿真处理结果失真。这时就需要一种高
精度的考虑地球曲率的定高度实现方法。传统的高度控制做法是基于
力矩控制，即通过改变飞行器力矩平衡来改变
俯仰角姿态，进而改变升力大小，实现对高度的控制。该方法需要建立较复杂的动力学模型，不便于仿真实现，本发明给出了一种便于实现的模型简单的定高度大圆飞行控制方法，很好的克服了传统实现方法的弊端。
发明内容
[0003] 要解决的技术问题
[0004] 为了避免
现有技术的不足之处，本发明提出一种飞行器在等高航路点间的定高度大圆飞行实现方法。
[0005] 技术方案
[0006] 一种飞行器在等高航路点间的定高度大圆飞行实现方法，其特征在于步骤如下：
[0007] 步骤1将经纬高坐标转换到″Rg2″
坐标系：
[0008] 首先利用下述公式将给出的预置航路点的经纬高坐标转换到″Rd″坐标系：
[0009] X＝(Re+H0)cosφ0cosλ0
[0010] Y＝(Re+H0)cosφ0sinλ0
[0011] Z＝(Re+H0)sinφ0
[0012] 其中Re＝6378137m，λ0，分别表示给定点的经度、纬度，单位为弧度；H0为给定点的高度；X，Y，Z分别表示给定点在″Rd″坐标系下对应的三坐标；
[0013] 所述″Rd″坐标系为地心系，定义为：坐标原点为地心，X轴在赤道平面内并指向首子午线方向，Z轴指向北极，Y轴通过右手法则确定；
[0014] 再转换到″Rg2″坐标系： 为给定点在″Rg2″坐标系下对应的坐标；
[0015] 其中：从″Rd″坐标系到″Rg2″坐标系的旋转矩阵
[0016] 其中：λ1、 Hn为两个预置等高航路点之一的经纬高；
[0017] 所述″Rg2″坐标系坐标定义为：原点为地心，X轴指向其正北方向，Y轴指向天，Z轴指向其正东方向；
[0018] 步骤2计算飞行器初始速度：令 为飞行器在″Rg2″坐标系下的实时速度矢量，以 和 表示两个预置航路点在″Rg2″坐标系下的坐标，通过 对 初始化，其中 为 对应的单位向量，″.″为点乘符号，″×″为叉乘符号；
[0019] 步骤3判断飞行器当前是否需要终止飞行：以 表示″Rg2″坐标系下从地心指向飞行器当前
位置的向量，当飞行器距第二个预置航路点(目标点)的距离小于飞行器在一个步长的最大飞行距离时， 则飞行器终止飞行；否则，执行下一步骤；
[0020] 步骤4计算飞行器实现定高度大圆飞行的实时过载：根据飞行器实际飞行航线与理想大圆航线之间的实时偏差来计算在″Rg2″坐标系下飞行器沿大圆定高飞行的实时过载 其中 为 对应的单位向量，εn为向心
加速度所对应的过载的大小，nH为高度方向偏差调节系数，nB为大圆法向偏差调节系数， 表示在″Rg2″坐标系下预定大圆飞行平面的法向量；
[0021] 所述
[0022] 所述
[0023] 所述
[0024] 其中K1、K2分别取0.025和0.07，g为
重力加速度，取9.8；ΔH和 分别为飞行器的当前海拔高度上的偏差和高度变化率；ΔB和 分别为大圆法向偏差和法向偏差的变化率；
[0025] 所述ΔH＝Ht-Hn，其中Ht为飞行器的当前海拔高度，
[0026] 所述
[0027] 所述
[0028] 所述
[0029] 步骤5：计算飞行器在″Rg2″坐标系下的实时速度 和实时矢径其中 对应的单位向量；
[0030] 步骤6计算飞行器在″Rg″坐标系下的实时坐标：令飞行器在″Rg″坐标系下的实时坐标为 则
[0031] 步骤7计算飞行器在″Rc″坐标系下的实时坐标：令飞行器在″Rc″坐标系下的实时坐标为 ″Rg″和″Rc″的坐标原点在″Rg2″坐标系下对应的坐标分别为 和″Rd″坐标系到″Rc″坐标系的转换矩阵为 则 其中， 表示从″Rg″坐标系到″Rc″坐标系的转换矩阵
[0032] ( 可通过式1.5计算得到，用λ′替换λ1、替换 )，λ′、 0为″Rc″坐标系原点的经纬高；
[0033] 所述″Rc″为战区坐标系，定义为由给定的、位于地面上的坐标原点确定的北天东坐标系；
[0034] 所述″Rg″为当地北天东坐标系，定义为坐标原点为位于地面上的
选定点，X轴指向其正北方向，Y轴指向天，Z轴指向其正东方向；
[0035] 所述 和 根据经纬高坐标到″Rg2″坐标系的转换步骤得到。
[0036] 有益效果
[0037] 本发明提出的一种飞行器在等高航路点间的定高度大圆飞行实现方法，通过计算实际飞行航线与理想大圆航线之间的实时偏差，给出了飞行器实现两等高点之间的定高度大圆飞行的具体实施步骤，进而可拓展至多等高航路点间的定高度大圆飞行。通过对该方法进行仿真验证可知，其通过对飞行器的实时高度和实时方位的调整很好的解决了飞行器沿大圆定高度飞行的实现问题，精度高，并且对飞行器的机
动能力要求较弱，所以可以应用于多种研究对象。
附图说明
[0038] 图1：为基于“两个预置等高航路点”的定高度大圆飞行示意图，其中航路点1和航路点2分别表示飞行器沿大圆等高度飞行的起点和终点，Hn表示预想的飞行高度；
[0039] 图2：为″Rd″坐标系、″Rg″坐标系和″Rc″坐标系之间的相对关系示意图，其中λ和 分别为当地北天东坐标系原点对应的经度和纬度(单位为弧度)，λ′和 分别为战区坐标系原点对应的经度和纬度(单位为弧度)；
[0040] 图3：为飞行器在两个预置等高航路点间的定高度大圆飞行实现
流程图；
[0041] 图4：给出了″Rc″坐标系下飞行器在两个预置等高航路点间定高度大圆飞行的轨迹图；
[0042] 图5：给出了飞行器飞行过程中的实时高度曲线，X轴代表飞行时间，Y轴代表实时高度；
[0043] 图6：给出了飞行器飞行过程中的实时过载大小曲线，X轴代表飞行时间，Y轴代表实时过载大小。
具体实施方式
[0044] 现结合
实施例、附图对本发明作进一步描述：
[0045] 首先将需要用到的坐标系说明如下：
[0046] 1)地心系(″Rd″)
[0047] 坐标原点为地心，X轴在赤道平面内并指向首子午线方向，Z轴指向北极，Y轴通过右手法则确定。
[0048] 2)当地北天东坐标系(″Rg″)
[0049] 坐标原点为位于地面上的选定点，X轴指向其正北方向，Y轴指向天，Z轴指向其正东方向。
[0050] 3)″Rg2″坐标系
[0051] ″Rg2″坐标系的坐标原点为地心，各轴指向与″Rg″的各轴指向对应相同。
[0052] 4)战区坐标系(″Rc″)
[0053] 坐标原点、各轴指向可根据需要
指定(各轴指向只需满足右手准则即可)。在此令″Rc″为由给定的坐标原点(位于地面上)确定的北天东坐标系。
[0054] ″Rd″、″Rg″和″Rc″的相对关系参见附图2。首先令两个预置等高航路点的经纬高分别为(λ1、 Hn)、(λ2、 Hn)、飞行器飞行速度的大小为Vn、飞行器所要稳定飞行的高度(相对于地面)为Hn、
算法仿真步长为dt且″Rc″坐标系原点的经纬高为(λ′、0)。以上所有经纬度均以弧度形式给出，下面详细介绍基于“两个预置等高航路点”的定高度大圆飞行的实现步骤：
[0055] 步骤1：将经纬高坐标转换到″Rg2″坐标系
[0056] 令″Rg″坐标系的原点为第一个预置航路点在地面上的投影。实时过载的计算是在″Rg2″坐标系下完成的，所以首先要将预置航路点的坐标转换到″Rg2″坐标系。可通过首先将给出的预置航路点的经纬高坐标转换到″Rd″坐标系，然后再转换到″Rg2″坐标系这两步来实现。
[0057] Step1：将经纬高坐标转换到″Rd″坐标系
[0058] 可以通过式1.1-1.3将经纬高坐标转换到″Rd″坐标系下表示：
[0059] X＝(Re+H0)cosφ0cosλ0 (0.1)
[0060] Y＝(Re+H0)cosφ0sinλ0 (0.2)
[0061] Z＝(Re+H0)sinφ0 (0.3)其中Re＝6378137m，λ0， 分别表示给定点的经度、纬度(单位：弧度)，H0为给定点的高度；
[0062] X，Y，Z分别表示给定点在″Rd″坐标系下对应的三坐标；
[0063] Step2：″Rd″坐标系到″Rg2″坐标系的转换
[0064] 取给定点在″Rg2″坐标系下对应的坐标为(X′Y′Z′)T，则
[0065]
[0066] 其中
[0067]
[0068] 表示从″Rd″坐标系到″Rg2″坐标系的旋转矩阵。
[0069] 步骤2：计算飞行器初始速度
[0070] 令 为飞行器在″Rg2″坐标系下的实时速度矢量。对 进行初始化时要求其模等于Vn，且为了获得平滑的大圆飞行轨迹，取飞行器初始速度的方向为由两个预置航路点和地心确定的大圆在起始航路点处的切向。以 和 表示两个预置航路点在″Rg2″坐标系下的坐标，则 可通过下式来初始化：
[0071]
[0072] 其中 为 对应的单位向量，″.″为点乘符号，″×″为叉乘符号。
[0073] 步骤3：判断飞行器当前是否需要终止飞行
[0074] 以 表示″Rg2″坐标系下从地心指向飞行器当前位置的向量，当飞行器距第二个预置航路点(目标点)的距离小于飞行器在一个步长的最大飞行距离时，即如果则飞行器终止飞行。否则，转到步骤4。
[0075] 步骤4：计算飞行器实现定高度大圆飞行的实时过载
[0076] 飞行器沿大圆定高飞行的实时过载在″Rg2″坐标系下的表示 可根据飞行器实际飞行航线与理想大圆航线之间的实时偏差来计算。 的计算公式如下：
[0077]
[0078] 其中 为 对应的单位向量，εn为
向心加速度所对应的过载的大小，nH为高度方向偏差调节系数，nB为大圆法向偏差调节系数， 为预定大圆飞行平面的法向量在″Rg2″坐标系下的表示。
[0079] 可依照以下步骤计算
[0080] Step1：计算ΔH和
[0081]
[0082] ΔH＝Ht-Hn (0.8)
[0083]
[0084] 其中Ht为飞行器的当前海拔高度，ΔH和 分别为高度上的偏差和高度变化率。
[0085] Step2：计算
[0086]
[0087] 和 分别为″Rg2″坐标系下地心指向预置航路点1和2的单位矢量，即为 对应的单位向量， 为 对应的单位向量。两个预置等高航路点和地心确定了大圆平面，同时也确定了大圆平面的法向量 所以无需每一步长均计算 当Step1执行完之后，首先判断是否第一次执行Step2，若是，则执行Step2，否则直接执行Step3。
[0088] Step3：计算ΔB和
[0089]
[0090]
[0091] ΔB和 分别为大圆法向偏差和法向偏差的变化率。
[0092] Step4：计算nH、nB和εn
[0093]
[0094]
[0095]
[0096] 其中K1、K2分别取0.025和0.07，g为重力加速度(取9.8)。
[0097] Step5.：计算
[0098] 将各相关参量代入式(1.7)计算
[0099] 步骤5：计算飞行器在″Rg2″坐标系下的实时速度和实时矢径
[0100]
[0101]
[0102] 其中 为 对应的单位向量，本发明假设飞行器在任意飞行时刻的速度大小恒定，但实质上该发明仍适用于变速度大小的定高大圆飞行。
[0103] 步骤6：计算飞行器在″Rg″坐标系下的实时坐标
[0104] 令飞行器在″Rg″坐标系下的实时坐标为 则
[0105]
[0106] 步骤7：计算飞行器在″Rc″坐标系下的实时坐标
[0107] 令飞行器在″Rc″坐标系下的实时坐标为 ″Rg″和″Rc″的坐标原点在″Rg2″坐标系下对应的坐标分别为 和 ( 和 可根据经纬高坐标到″Rg2″坐标系的转换步骤得到)，″Rd″坐标系到″Rc″坐标系的转换矩阵为 ( 可通过式1.5计算得到，用λ′替换λ1、替换 )，则
[0108]
[0109] 其中， 表示从″Rg″坐标系到″Rc″坐标系的转换矩阵，可通过下式计算得到：
[0110]
[0111] 和 只需在第一次执行步骤7时计算即可。
[0112] 下一部分仿真实现了飞行器在两个预置等高航路点间的定高度大圆飞行。假设飞行器匀速飞行的速度大小300m/s、两个预置等高航路点的经纬高坐标分别为(116，39，10000.0)(处于北京地区)、(121，31，10000.0)(处于上海地区)、预定飞行高度为10000m、仿真步长为0.1s、″Rc″坐标系的原点为(117，41，0)(处于北京地区)。
[0113] 实施步骤如下：
[0114] 步骤1：将经纬高坐标转换到″Rg2″坐标系
[0115] 按照初始条件可得：
[0116] λ1＝116*π/180 φ1＝39*π/180 H1＝10000
[0117] λ2＝121*π/180 φ2＝31*π/180 H2＝10000
[0118] λ′＝117*π/180 Hn＝10000
[0119] Vn＝300 dt＝0.1
[0120] Step1：将经纬高坐标转换到″Rd″坐标系
[0121] X1 ＝ (Re+H1)cosφ1cosλ1 ＝ -2176300.0772 Y1 ＝ (Re+H1)cosφ1sinλ1 ＝4462076.4086
[0122] Z1＝(Re+H1)sinφ1＝4020184.8749
[0123] X2＝(Re+H2)cosφ2cosλ2＝-2820195.0935
[0124] Y2＝(Re+H2)cosφ2sinλ2＝4693592.8303
[0125] Z2＝(Re+H2)sinφ2＝3290133.7827
[0126] Step2：″Rd″坐标系到″Rg2″坐标系的转换
[0127] 将λ1、φ1带入式1.5可得
[0128]
[0129] 则第一个预置航路点在″Rg2″坐标系下的坐标为
[0130]
[0131] 第二个预置航路点在″Rg2″坐标系下的坐标为
[0132]
[0133] 飞行器的初始位置在″Rg2″坐标系下的坐标
[0134]
[0135] 步骤2：计算飞行器初始速度T
[0136] 由 归一化为(-0.8781 0 0.4784) 可知，
[0137] 飞行器的初始速度为
[0138] 步骤3：判断飞行器当前是否需要终止飞行
[0139] 如果
[0140]
[0141] 即
[0142]
[0143] 则飞行器已经到达第二个预置航路点，终止飞行；否则，执行步骤4。
[0144] 步骤4：计算飞行器实现定高度大圆飞行的实时过载
[0145] Step1：计算ΔH和
[0146]
[0147]
[0148] Step2：计算
[0149] 如果第一次执行步骤4，则计算 其中
[0150]
[0151]
[0152]
[0153]
[0154] 否则，直接执行Step3。
[0155] Step3：计算ΔB和
[0156]
[0157]
[0158] Step4：计算nH、nB和εn
[0159]
[0160]
[0161]
[0162] Step5：计算
[0163]
[0164] 步骤5：计算飞行器在″Rg2″坐标系下的实时速度和实时矢径
[0165]
[0166]
[0167]
[0168] 步骤6：计算飞行器在″Rg″坐标系下的实时坐标
[0169]
[0170] 步骤7：计算飞行器在″Rc″坐标系下的实时坐标
[0171] 如果第一次执行步骤7，则首先计算 和
[0172]
[0173]
[0174]
[0175]
[0176]
[0177] 否则，直接计算
[0178]
[0179] 然后返回步骤3。
[0180] 步骤终
[0181] ″Rc″坐标系下的飞行器大圆飞行轨迹图、飞行器实时高度图和飞行器实时过载大小图分别见附图4、附图5和附图6。从附图5可以看出飞行器定高度大圆飞行过程中的海拔高度基本稳定在9999.980m处，非常准确的模拟了两预置等高度航路点间的定高度大圆飞行，并且飞行轨迹非常平滑，体现了算法的可行性和有效性。附图6表明飞行器飞行过程中的过载大小基本稳定在0.9985，
波动很小，说明该方法对于飞行器的机动能力的要求较弱，所以可广泛应用于飞行器定高度大圆飞行的研究中去。
