CN109699007A - 一种室内外无缝渐变导航过渡方法 - Google Patents

Abstract

本发明涉及导航定位领域，具体涉及一种室内外无缝渐变导航过渡方法，包括：步骤1，构建坐标基准框架；步骤2，室内外无缝渐变导航过渡。本发明所述室内外无缝渐变导航过渡方法，不采用在某一个点上切换，而是在一段区域里，逐渐变换，能实现室内与室外导航定位的平滑过渡，所以位置轨迹无断点，是连续光滑的；采用同一种解算方法，不但能解决室外与室内定位解算，而且也能实现室内外信号覆盖重叠区域的无缝导航定位的解算问题。

Description

一种室内外无缝渐变导航过渡方法

技术领域

本发明涉及导航定位领域，具体涉及一种室内外无缝渐变导航过渡方法。

背景技术

室外导航定位与室内导航定位过渡和连接时的一种经典的处理方法是切换的方法。2012 年美国苹果公司申请了一项专利，就是室外导航定位与室内导航定位无缝切换专利，指从一个导航***切换到另一个导航***时采用切换的方法达到无缝过渡连续导航的目的。这项专利后来由美国专利局批准授权。这一专利抢先发布，其目的是把会广泛应用的室内外连续导航切换方法封杀掉。但我们认为专利所述的切换方法并不是一种最好的无缝变换连续导航办法。因为所谓导航***的切换，实质上主要是利用的导航信号的变换,两个或多个导航***的信号相互间的覆盖界线犬牙交错，十分模糊，一般是没有明确分界的，所以会难于找到一个合适的切换点的，且切换的做法会产生时延，往往会使定位解的位置有跳跃，容易造成切换处断续，难于无缝连接，最后造成导航定位的位置轨迹不连贯、不连续、有突跳等现象。

如中国专利申请号为：CN201210290358.3的专利公布了一种基于代价函数的室内无线定位与室外无线定位的无缝切换方法，它涉及一种室内无线定位与室外无线定位无缝切换方法，具体涉及一种基于代价函数的室内无线定位与室外无线定位的无缝切换方法，以解决现有室内无线定位与室外无线定位不能无缝切换的问题，方法的具体步骤为：步骤一、设置交叠区；步骤二、选择参数：根据不同定位***选取不同的参数参与代价函数运算；步骤三、设定权重；步骤四、门限或滞留时间的确定；步骤五、代价函数的比较。

该专利与苹果专利不同的地方是设置了一个交叠的空间，对于不同的定位***选择不同的参数和权重来构成代价函数，参与运算的参数为定值，而权重系数为变量，使代价函数与空间位置间建立对应关系，改变权重系数，使代价函数在各定位系数服务区的边缘处具有较好的区分能力。当目标***的代价函数与当前***的代价函数值之差高于某一门限时，进行切换。究其专利的实质，仍然是多种异构定位***间的切换问题，区分室内外环境的物理量主要指GNSS卫星信号强度，GNSS可见星数量，室内定位节点信号强度，测试向量与指纹图的欧氏距离等。专利通过各***代价函数值的比较来判断定位服务向哪个定位***切换。所以仍然使用了切换手段，而且需要选择权重，并伴有乒乓效应。

又如中国专利申请号为：CN201510273112.9的专利公布了一种集成卫星导航与蓝牙技术的室内外无缝定位***及其方法，该专利所使用的应用终端设备由室外卫星定位模块、室内蓝牙定位模块，以及室内外定位融合切换模块以及电子地图显示模块组成。把导航定位区域分成三类区域：室外信号工作区、室内信号工作区以及两种信号共存区域。只有在两种信号共存区域里，它才采用卡尔曼滤波方法进行数据处理。这种隔离成三类区域的做法，是比较理想化的分割做法。专利所指的终端只能接收两种信号，即室外接收卫星导航信号，室内只有蓝牙信号，实际上现在趋势性的使用方案是多源融合导航定位，与现实不相符。

再如中国专利申请号：CN201810813783.3的发明提出了一种室内外导航无缝切换方法及控制***，该发明所使用的应用终端设备包括场景感知模块、场景变化分析模块及方法切换模块，在运行过程中，要反覆进行分析和判断，把一个不很复杂的处理搞得很繁琐复杂。

发明内容

本发明的目的是针对现有技术的不足，提供一种室内外无缝渐变导航过渡方法，适应室内外无缝导航定位的需求。

所述室内外无缝渐变导航过渡方法，包括如下步骤：

步骤1，构建坐标基准框架：

在采用地心地固坐标系框架的基础上，同时采用楼宇局部坐标系和站心坐标系框架，并采用终端设备进行定位,并通过附近基站进行修正；

步骤2，室内外无缝渐变导航过渡：

1)在室内外交界处，布置重叠信号覆盖区，重叠范围为20～50m；

2)绘制室内外交界处的重叠信号覆盖区的二维地图，并连接室内地图与室外地图；

3)采集定位信息，在从室外向室内过渡导航中，逐步放弃卫星导航信号，同时逐渐增加室内导航信号；在从室内向室外过渡导航中，逐步放弃室内导航信号，同时逐渐增加卫星导航信号；列出解算使用的室内外导航定位测量模型；

4)采用广义数据融合算法，处理重叠信号覆盖区内的信息，得到优化导航轨迹。

进一步地，步骤1中，采用楼宇局部坐标系的具体步骤包括：

1)在建筑物内设置坐标原点O；

2)以O点垂线方向为Z轴，指向天顶为正；子午线方向为X轴，向北为正；Y轴与X、 Z轴垂直，向东为正；构成左手直角坐标系；

3)通过坐标原点O的地球坐标，建立起室内楼宇坐标系与室外地心地固坐标系之间的转换关系如下：

已知坐标原点O的地心坐标为(X₀,Y₀,Z₀)，大地坐标经纬度为设空间中任一点P 的地心坐标(X_P,Y_P,Z_P)，那么点P在以点O为原点的楼宇局域坐标系下的空间坐标表示如下：

相反，若已知空间点P，在以点O为原点的楼宇局域坐标系下的坐标则点P的地心坐标为：

进一步地，步骤1中，终端设备中设置气压测高芯片,通过气压测高芯片与基站提供的气压基点修正信息，经差分计算获得绝对高程值。

进一步地，步骤1中，站心坐标系设置方式为在城市中心、商业密集处设置区域坐标系， Z轴与铅垂线方向重合指向天顶，X轴指向正北，Y轴垂直于X、Z轴指向正东，构成左手坐标系。

进一步地，步骤2的2)中，绘制室内外交界处的重叠信号覆盖区的三维地图。

进一步地，步骤2的3)中，建立室内外无缝导航定位的测量模型如下：

x_i＝f(y_i,z_j,ρ_l,r_k,δ_m) (3)

i＝1,2,.....n₁

j＝1,2,.....n₁

l＝1,2,.....n₁

k＝1,2,.....n₁

m＝1,2,.....n₁

Ω:y_i∩z_j＞0

式中，x_i表示终端待求位置坐标值；y_i为室外信号源的测量值；z_j为室内信号源的测量值；r_k为室内其它测量量；ρ_l为室外其它测量量；δ_m为全域其它测量量；f(.)为测量函数表达式；Ω：表示室内与室外信号的交集区域。

进一步地，步骤2的4)中，采用广义数据融合算法计算得到优化导航轨迹的具体步骤包括：

S1、构建求解问题模型：

建立求轨迹解(x_i,y_i,z_i),i＝1,2,3…n_e的模型：

模型(4)由两部分组成，上面部分是直接求解部分，其中ρ_j，η_l是绝对测量值或观测值，f₁(x_i,y_i,z_i,x_sj,y_sj,z_sj)为状态量求解函数关系式，g₁(x_i,y_i,z_i)为约束函数关系式，n_e为最终历元数，m为信号源数目，k为约束方程数量；这里组合约束方程的目的是通过组合压缩求解空间，提高解的精度；

下面部分是递推部分，其中ΔX_i,ΔY_i,ΔZ_i为低阶状态量，可以是状态变量的相对变化量，也可以是状态变量的导数值或微分值，f₂(Δx_i),f₃(Δy_i),f₄(Δz_i)分别为递推关系式， g₂(Δx_i,Δy_i,Δz_i)为低阶状态量之间的相关约束函数式，α_i表示各分量之间的约束关系；各分量值间增加相关约束后可以加强不同分量之间的耦合匹配，有利于获得相关解，实际上也有利于使其成为真正的矢量解；

S2、坐标变换：

若模型(4)中的两部分分别建立在不同的坐标系下，则进行坐标转换，将两部分统一到同一坐标系下进行解算；也可以先在不同坐标系下分别完成解算，然后把数据转换到同一个坐标系下，进行集成组合；

S3、求解绝对状态量的状态解算方程，直接获得状态量值(x_i,y_i,z_i)；

如果方程为非线形式，采用非线性直接求解算法求解如下非线性状态解算方程：

f(x_i)＝ρ_i+ν_i，i＝1,2,...,n_e (5)

式(5)中，f(x_i)为状态量求解关系式，一般为非线性函数关系，x_i为函数状态变量，ρ_i为测量量，v_i为随机噪声，i为历元序号，n_e为方程个数；

首先，建立如下的残差极小化优化模型，即：

求x，使

若有约束条件，求x，使

S.T.g(x_n)＝c+υ

式(7)中，g(x_n)为约束函数关系式，c为约束方程右端项，υ为随机噪声；式(7)可以采用单成形法、随机复形法等非线性直接求解算法进行求解，通过搜索、比较等步骤，使目标函数值一步一步地逼近最优点；

S4、建立状态量递推方程，获取递推后状态量的最优预估值

建立低阶状态量的递推方程，获取高阶状态量的新值，并与直接从解算方程求解获得的状态解算值组合，改善状态值解的精度及解的关联性；低阶状态量可以是与状态量相关的导数值或微分值，也可以是差分信息；当预估下一历元状态预估量时，本实施例采用在当前最优状态估计值的基础上，加上可观测的状态量的一阶导数值或高阶导数值与时间间隔的乘积，获得下一历元的高阶状态预估量；这一过程中采用的状态量递推公式表示如下：

式中，Z(·)表示递推关系式；

当递推关系符合运动学时，用如下关系式表示：

在式(8)和式(9)中，为t_n历元时状态变量的一阶导数值，为t_n历元时状态变量的二阶导数值，υ_n为t_n历元时的随机噪声，为t_n历元时状态量递推预估值，为t_n-1历元时状态量的最优估计值，Δt为历元间的间隔时间；

在融合起始阶段时，初始状态估计量尚未知道，可用前几个历元的状态测量值代替，或用经逼近处理后的逼近值替代；

S5、组合t_n历元时的直接求解状态值与最优预估状态值获得最优状态估计值

把上述两类获得的状态值进行集成融合求解，把t_n历元时的状态预估量和t_n历元时的直接求解状态值x_n模糊组合起来，从而求得t_n历元时的状态最优区间估计量

采用广义延拓逼近方法求解式(10)中的系数α₁，α₂，α₃，即求：

式(11)中，ω₀、ω₁、ω₂为权系数，是待求量α₁，α₂，α₃的约束区间数，对上述式(11)用优化算法求解，能获得广义延拓逼近多项式的系数α₁，α₂，α₃以及目标函数的极小化值；

同理，可以求解得到

S6、重复运用迭代求解获得一组优化解：

将i+1赋给i，用n+1代替n；用t_n+1代替t_n，重复步骤S3～S6，便可以求得在t_n+1历元时的最优状态估计值

上述迭代过程重复进行，递推求解，直至i＝n_e时结束，便能获得一组优化导航轨迹解

本发明的有益效果是：

1.本发明所述室内外无缝渐变导航过渡方法，不采用在某一个点上切换，而是在一段区域里，逐渐变换，能实现室内与室外导航定位的平滑过渡，所以位置轨迹无断点，是连续光滑的。

2.本发明所述室内外无缝渐变导航过渡方法采用同一种解算方法，不但能解决室外与室内定位解算，而且也能实现室内外信号覆盖重叠区域的无缝导航定位的解算问题。

具体实施方式

实施例1

所述室内外无缝渐变导航过渡方法，包括如下步骤：

步骤1，构建坐标基准框架：

在采用地心地固坐标系框架的基础上，同时采用楼宇局部坐标系和站心坐标系框架，并采用终端设备进行定位,并通过附近基站进行修正；终端设备中设置气压测高芯片,通过气压测高芯片与基站提供的气压基点修正信息，经差分计算获得绝对高程值。

采用楼宇局部坐标系的具体步骤包括：

1)在建筑物内设置坐标原点O；

2)以O点垂线方向为Z轴，指向天顶为正；子午线方向为X轴，向北为正；Y轴与X、 Z轴垂直，向东为正；构成左手直角坐标系；

3)通过坐标原点O的地球坐标，建立起室内楼宇坐标系与室外地心地固坐标系之间的转换关系如下：

已知坐标原点O的地心坐标为(X₀,Y₀,Z₀)，大地坐标经纬度为设空间中任一点P 的地心坐标(X_P,Y_P,Z_P)，那么点P在以点O为原点的楼宇局域坐标系下的空间坐标表示如下：

相反，若已知空间点P，在以点O为原点的楼宇局域坐标系下的坐标则点P的地心坐标为：

站心坐标系设置方式为在城市中心、商业密集处设置区域坐标系，Z轴与铅垂线方向重合指向天顶，X轴指向正北，Y轴垂直于X、Z轴指向正东，构成左手坐标系

步骤2，室内外无缝渐变导航过渡：

1)在室内外交界处，布置重叠信号覆盖区，重叠范围为20～50m；

2)绘制室内外交界处的重叠信号覆盖区的二维地图或三维地图，并连接室内地图与室外地图；重叠信号覆盖区的二维地图或三维地图连接室内地图与室外地图，能使室外导航定位轨迹过渡到重叠信号覆盖区地图上，也能使导航定位轨迹从重叠信号覆盖区地图上过渡至室内地图上；同样，能使室内导航定位轨迹过渡到重叠信号覆盖区地图上，再使导航定位轨迹从重叠信号覆盖区地图过渡至室外地图上。

3)采集定位信息，在从室外向室内过渡导航中，逐步放弃卫星导航信号，同时逐渐增加室内导航信号；在从室内向室外过渡导航中，逐步放弃室内导航信号，同时逐渐增加卫星导航信号；列出解算使用的室内外导航定位测量模型如下：

x_i＝f(y_i,z_j,ρ_l,r_k,δ_m) (3)

i＝1,2,.....n₁

j＝1,2,.....n₁

l＝1,2,.....n₁

k＝1,2,.....n₁

m＝1,2,.....n₁

Ω:y_i∩z_j＞0

式中，x_i表示终端待求位置坐标值；y_i为室外信号源的测量值；z_j为室内信号源的测量值；r_k为室内其它测量量；ρ_l为室外其它测量量；δ_m为全域其它测量量；f(.)为测量函数表达式；Ω：表示室内与室外信号的交集区域。

4)采用广义数据融合算法，处理重叠信号覆盖区内的信息，得到优化导航轨迹；

具体包括以下步骤：

S1、构建求解问题模型。

建立求轨迹解(x_i,y_i,z_i),i＝1,2,3…n_e的模型：

模型(4)由两部分组成，上面部分是直接求解部分，其中ρ_j，η_l是绝对测量值或观测值，f₁(x_i,y_i,z_i,x_sj,y_sj,z_sj)为状态量求解函数关系式，g₁(x_i,y_i,z_i)为约束函数关系式，n_e为最终历元数，m为信号源数目，k为约束方程数量。这里组合约束方程的目的是通过组合压缩求解空间，提高解的精度。

下面部分是递推部分，其中ΔX_i,ΔY_i,ΔZ_i为低阶状态量，可以是状态变量的相对变化量，也可以是状态变量的导数值或微分值，f₂(Δx_i),f₃(Δy_i),f₄(Δz_i)分别为递推关系式， g₂(Δx_i,Δy_i,Δz_i)为低阶状态量之间的相关约束函数式，α_i表示各分量之间的约束关系。各分量值间增加相关约束后可以加强不同分量之间的耦合匹配，有利于获得相关解，实际上也有利于使其成为真正的矢量解。

S2、坐标变换。

若模型(4)中的两部分分别建立在不同的坐标系下，则进行坐标转换，将两部分统一到同一坐标系下进行解算；也可以先在不同坐标系下分别完成解算，然后把数据转换到同一个坐标系下，进行集成组合。

S3、求解绝对状态量的状态解算方程，直接获得状态量值(x_i,y_i,z_i)。

如果方程为非线形式，采用非线性直接求解算法求解如下非线性状态解算方程：

f(x_i)＝ρ_i+ν_i，i＝1,2,...,n_e (5)

式(5)中，f(x_i)为状态量求解关系式，一般为非线性函数关系，x_i为函数状态变量，ρ_i为测量量，v_i为随机噪声，i为历元序号，n_e为方程个数。

首先，建立如下的残差极小化优化模型，即：

求x，使

若有约束条件，求x，使

S.T.g(x_n)＝c+υ

式(7)中，g(x_n)为约束函数关系式，c为约束方程右端项，υ为随机噪声。式(7)可以采用单成形法、随机复形法等非线性直接求解算法进行求解，通过搜索、比较等步骤，使目标函数值一步一步地逼近最优点。

S4、建立状态量递推方程，获取递推后状态量的最优预估值

建立低阶状态量的递推方程，获取高阶状态量的新值，并与直接从解算方程求解获得的状态解算值组合，改善状态值解的精度及解的关联性。低阶状态量可以是与状态量相关的导数值或微分值，也可以是差分信息。当预估下一历元状态预估量时，本实施例采用在当前最优状态估计值的基础上，加上可观测的状态量的一阶导数值或高阶导数值与时间间隔的乘积，获得下一历元的高阶状态预估量。这一过程中采用的状态量递推公式表示如下：

式中，Z(·)表示递推关系式；

当递推关系符合运动学时，用如下关系式表示：

在式(8)和式(9)中，为t_n历元时状态变量的一阶导数值，为t_n历元时状态变量的二阶导数值，υ_n为t_n历元时的随机噪声，为t_n历元时状态量递推预估值，为t_n-1历元时状态量的最优估计值，Δt为历元间的间隔时间。

在融合起始阶段时，初始状态估计量尚未知道，可用前几个历元的状态测量值代替，或用经逼近处理后的逼近值替代。

S5、组合t_n历元时的直接求解状态值与最优预估状态值获得最优状态估计值

把上述两类获得的状态值进行集成融合求解，把t_n历元时的状态预估量和t_n历元时的直接求解状态值x_n模糊组合起来，从而求得t_n历元时的状态最优区间估计量

采用广义延拓逼近方法求解式(10)中的系数α₁，α₂，α₃，即求：

式(11)中，ω₀、ω₁、ω₂为权系数，是待求量α₁，α₂，α₃的约束区间数，对上述式(11)用优化算法求解，能获得广义延拓逼近多项式的系数α₁，α₂，α₃以及目标函数的极小化值。

同理，可以求解得到

S6、重复运用迭代求解获得一组优化解。

将i+1赋给i，用n+1代替n；用t_n+1代替t_n，重复步骤S3～S6，便可以求得在t_n+1历元时的最优状态估计值

上述迭代过程重复进行，递推求解，直至i＝n_e时结束，便能获得一组优化导航轨迹解

本发明提出的室内外导航犬牙交错无缝渐变过渡方法的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种室内外无缝渐变导航过渡方法，其特征在于，包括如下步骤：

步骤1，构建坐标基准框架：

在采用地心地固坐标系框架的基础上，同时采用楼宇局部坐标系和站心坐标系框架，并采用终端设备进行定位,并通过附近基站进行修正；

步骤2，室内外无缝渐变导航过渡：

1)在室内外交界处，布置重叠信号覆盖区，重叠范围为20～50m；

2)绘制室内外交界处的重叠信号覆盖区的二维地图，并连接室内地图与室外地图；

3)采集定位信息，在从室外向室内过渡导航中，逐步放弃卫星导航信号，同时逐渐增加室内导航信号；在从室内向室外过渡导航中，逐步放弃室内导航信号，同时逐渐增加卫星导航信号；列出解算使用的室内外导航定位测量模型；

4)采用广义数据融合算法，处理重叠信号覆盖区内的信息，得到优化导航轨迹。

2.根据权利要求1所述的一种室内外无缝渐变导航过渡方法，其特征在于，所述步骤1中，采用楼宇局部坐标系的具体步骤包括：

1)在建筑物内设置坐标原点O；

2)以O点垂线方向为Z轴，指向天顶为正；子午线方向为X轴，向北为正；Y轴与X、Z轴垂直，向东为正；构成左手直角坐标系；

3)通过坐标原点O的地球坐标，建立起室内楼宇坐标系与室外地心地固坐标系之间的转换关系如下：

已知坐标原点O的地心坐标为(X₀,Y₀,Z₀)，大地坐标经纬度为设空间中任一点P的地心坐标(X_P,Y_P,Z_P)，那么点P在以点O为原点的楼宇局域坐标系下的空间坐标表示如下：

相反，若已知空间点P，在以点O为原点的楼宇局域坐标系下的坐标则点P的地心坐标为：

3.根据权利要求2所述的一种室内外无缝渐变导航过渡方法，其特征在于，所述步骤1中，终端设备中设置气压测高芯片,通过气压测高芯片与基站提供的气压基点修正信息，经差分计算获得绝对高程值。

4.根据权利要求1所述的一种室内外无缝渐变导航过渡方法，其特征在于，所述步骤1中，站心坐标系设置方式为在城市中心、商业密集处设置区域坐标系，Z轴与铅垂线方向重合指向天顶，X轴指向正北，Y轴垂直于X、Z轴指向正东，构成左手坐标系。

5.根据权利要求1所述的一种室内外无缝渐变导航过渡方法，其特征在于，所述步骤2的2)中，绘制室内外交界处的重叠信号覆盖区的三维地图。

6.根据权利要求1所述的一种室内外无缝渐变导航过渡方法，其特征在于，所述步骤2的3)中，建立室内外无缝导航定位的测量模型如下：

i＝1,2,.....n₁

j＝1,2,.....n₁

l＝1,2,.....n₁

k＝1,2,.....n₁

m＝1,2,.....n₁

Ω:y_i∩z_j＞0

式中，x_i表示终端待求位置坐标值；y_i为室外信号源的测量值；z_j为室内信号源的测量值；r_k为室内其它测量量；ρ_l为室外其它测量量；δ_m为全域其它测量量；f(.)为测量函数表达式；Ω：表示室内与室外信号的交集区域。

7.根据权利要求1所述的一种室内外无缝渐变导航过渡方法，其特征在于，所述步骤2的4)中，采用广义数据融合算法计算得到优化导航轨迹的具体步骤包括：

S1、构建求解问题模型：

建立求轨迹解(x_i,y_i,z_i),i＝1,2,3…n_e的模型：

模型(4)由两部分组成，上面部分是直接求解部分，其中ρ_j，η_l是绝对测量值或观测值，f₁(x_i,y_i,z_i,x_sj,y_sj,z_sj)为状态量求解函数关系式，g₁(x_i,y_i,z_i)为约束函数关系式，n_e为最终历元数，m为信号源数目，k为约束方程数量；这里组合约束方程的目的是通过组合压缩求解空间，提高解的精度；

下面部分是递推部分，其中ΔX_i,ΔY_i,ΔZ_i为低阶状态量，可以是状态变量的相对变化量，也可以是状态变量的导数值或微分值，f₂(Δx_i),f₃(Δy_i),f₄(Δz_i)分别为递推关系式，g₂(Δx_i,Δy_i,Δz_i)为低阶状态量之间的相关约束函数式，α_i表示各分量之间的约束关系；各分量值间增加相关约束后可以加强不同分量之间的耦合匹配，有利于获得相关解，实际上也有利于使其成为真正的矢量解；

S2、坐标变换：

若模型(4)中的两部分分别建立在不同的坐标系下，则进行坐标转换，将两部分统一到同一坐标系下进行解算；也可以先在不同坐标系下分别完成解算，然后把数据转换到同一个坐标系下，进行集成组合；

S3、求解绝对状态量的状态解算方程，直接获得状态量值(x_i,y_i,z_i)；

如果方程为非线形式，采用非线性直接求解算法求解如下非线性状态解算方程：

f(x_i)＝ρ_i+ν_i，i＝1,2,...,n_e (5)

式(5)中，f(x_i)为状态量求解关系式，一般为非线性函数关系，x_i为函数状态变量，ρ_i为测量量，v_i为随机噪声，i为历元序号，n_e为方程个数；

首先，建立如下的残差极小化优化模型，即：

求x，使

若有约束条件，求x，使

式(7)中，g(x_n)为约束函数关系式，c为约束方程右端项，υ为随机噪声；式(7)可以采用单成形法、随机复形法等非线性直接求解算法进行求解，通过搜索、比较等步骤，使目标函数值一步一步地逼近最优点；

S4、建立状态量递推方程，获取递推后状态量的最优预估值

建立低阶状态量的递推方程，获取高阶状态量的新值，并与直接从解算方程求解获得的状态解算值组合，改善状态值解的精度及解的关联性；低阶状态量可以是与状态量相关的导数值或微分值，也可以是差分信息；当预估下一历元状态预估量时，本实施例采用在当前最优状态估计值的基础上，加上可观测的状态量的一阶导数值或高阶导数值与时间间隔的乘积，获得下一历元的高阶状态预估量；这一过程中采用的状态量递推公式表示如下：

式中，Z(·)表示递推关系式；

当递推关系符合运动学时，用如下关系式表示：

在式(8)和式(9)中，为t_n历元时状态变量的一阶导数值，为t_n历元时状态变量的二阶导数值，υ_n为t_n历元时的随机噪声，为t_n历元时状态量递推预估值，为t_n-1历元时状态量的最优估计值，Δt为历元间的间隔时间；

在融合起始阶段时，初始状态估计量尚未知道，可用前几个历元的状态测量值代替，或用经逼近处理后的逼近值替代；

S5、组合t_n历元时的直接求解状态值与最优预估状态值获得最优状态估计值

把上述两类获得的状态值进行集成融合求解，把t_n历元时的状态预估量和t_n历元时的直接求解状态值x_n模糊组合起来，从而求得t_n历元时的状态最优区间估计量

采用广义延拓逼近方法求解式(10)中的系数α₁，α₂，α₃，即求：

式(11)中，ω₀、ω₁、ω₂为权系数，是待求量α₁，α₂，α₃的约束区间数，对上述式(11)用优化算法求解，能获得广义延拓逼近多项式的系数α₁，α₂，α₃以及目标函数的极小化值；

同理，可以求解得到

S6、重复运用迭代求解获得一组优化解：

将i+1赋给i，用n+1代替n；用t_n+1代替t_n，重复步骤S3～S6，便可以求得在t_n+1历元时的最优状态估计值

上述迭代过程重复进行，递推求解，直至i＝n_e时结束，便能获得一组优化导航轨迹解