一种用于非完备测量条件的异构多源导航信息反演方法

1.本发明涉及一种用于非完备测量条件的异构多源导航信息反演方法，属于定位与导航技术领域。


背景技术：

2.针对卫星拒止、干扰等多复杂环境中，现代导航系统广泛采用异构多源传感器作为导航信号源进行信息智能融合，输出导航信息，采用异构多源传感器的导航系统因其对复杂环境适应性强，输出载体导航状态信息准确，受到了国内外学者广泛关注。然而在一些成本比较昂贵难以重复进行或者一次性的多源导航实验测试中，如果部分器件发生故障，会导致导航系统部分数据缺失进而影响总体实验效果及后续实验分析。传统的导航信息反演方法一般为开环的信息反演，输出的反演信息的精度没有进行评估及迭代修正，导致输出的导航信息精度低与可靠度低，且反演的传感器类型比较单一，数量较少。
3.本发明方法利用陀螺仪及加速度计无噪声输出值反演出来的航路点数据与真实航路点数据进行对比评估，若精度不满足，则进行反馈迭代修正，直至满足设定反演精度；同时本发明基于有限航路点数据反演出多种导航传感器信息的输出，具有良好的运用前景与工程价值。


技术实现要素：

4.发明目的：提出一种用于非完备测量条件的异构多源导航信息反演方法，克服了在非完备测量条件下部分异构多源导航数据无法获取的问题，可以在一定程度上反演出多导航传感器实验输出数据，使其在实际测试中得到良好的运用。
5.技术方案：一种用于非完备测量条件的异构多源导航信息反演方法，包括以下步骤：
6.一种用于非完备测量条件的异构多源导航信息反演方法，包括以下步骤：
7.步骤(1)，获取载体运动过程中航路点数据；
8.步骤(2)，依据航路点数据类型来选择相对应方案进行数据预处理；
9.步骤(3)，使用插值法对预处理的航路点数据进行扩充；
10.步骤(4)，计算载体运动加速度及角速度；
11.步骤(5)，计算反演陀螺仪与加速度计无噪声输出；
12.步骤(6)，计算载体反演航路点坐标；
13.步骤(7)，评估反演数据是否满足精度；
14.步骤(8)，反演多种导航传感器仿真输出数据。
15.进一步的，所述步骤(1)中，载体航路点数据类型为经纬高数据或当地坐标系坐标数据，获取载体运动过程中经过第1～n个航路点的数据wp1、wp2、wp3、
…
、wpn，若为当地坐标系数据，还应获取当地坐标系原点经纬高坐标o，其表达式为：
16.o＝(l0,λ0,h0)
17.wp1＝(t1,x1,y1,z1,ψ1)
t
18.wp2＝(t2,x2,y2,z2,ψ2)
t
[0019][0020]
wpn＝(tn,xn,yn,zn,ψn)
t
[0021]
其中，(l0,λ0,h0)分别为当地坐标系原点纬度、经度与高度坐标，t1、t2、
…
、tn分别为载体经过第1～n航路点的时刻，(x1,y1,z1)、(x2,y2,z2)、
…
、(xn,yn,zn)分别为载体经过第1～n航路点时的三维坐标，ψ1、ψ2、
…
、ψn分别为载体经过第1～n航路点时的偏航角。
[0022]
进一步的，所述步骤(2)包括如下具体步骤：
[0023]
步骤(2-1)，根据步骤(1)航路点数据类型来判断执行的操作，若航路点数据为当地坐标系坐标数据，则执行步骤(2-2)，否则执行步骤(2-3)；
[0024]
步骤(2-2)，根据步骤(1)获得的数据，将所有航路点当地坐标系坐标转化为经纬高坐标；
[0025]
步骤(2-3)，根据步骤(1)获得的数据，将所有航路点坐标转化为经纬高坐标；
[0026]
步骤(2-4)，将所有的航路点数据均存放在航迹矩阵path中。
[0027]
进一步的，所述步骤(3)包括如下具体步骤：
[0028]
步骤(3-1)，根据步骤(2-4)获得的离散数据点通过三次样条插值算法分别找到时间与经度的关系式、时间与纬度的关系式、时间与高度的关系式、时间与偏航角的关系式；
[0029]
步骤(3-2)，设置一个合适的足够小的初始采样周期t，单位为秒；
[0030]
步骤(3-3)，根据步骤(3-1)获得的时间与经度、时间与纬度、时间与高度、时间与偏航角变量的函数关系式(f1,f2,f3,f4)与步骤(3-2)获得的采样周期t，对当前时刻两个相邻的航路点数据之间进行插值扩充。
[0031]
进一步的，所述步骤(4)包括如下具体步骤：
[0032]
步骤(4-1)，根据步骤(3-3)获取的相邻航路点i与i+1之间经过采样周期t离散后经度λ与纬度l数据，计算二维平面载体车头方向的运动速度；
[0033]
步骤(4-2)，根据步骤(4-1)获取的二维平面载体车头方向的运动速度计算二维平面载体车头方向的运动加速度，其表达式为：
[0034][0035]
步骤(4-3)，根据步骤(3-3)获取的载体高度方向数据，计算载体天向运动速度，其表达式为：
[0036][0037]
步骤(4-4)，根据步骤(4-3)获取的载体天向运动速度计算二维平面载体天向运动加速度，其表达式为：
[0038][0039]
步骤(4-5)，根据步骤(3-3)获取的载体偏航角数据，计算载体偏航角角速率，其表
达式为：
[0040][0041]
进一步的，所述步骤(5)包括如下具体步骤：
[0042]
步骤(5-1)，根据步骤(3-3)获得的航路点之间插值的偏航角变量ψ，计算载体系相对于地理系姿态转换矩阵，其表达式为：
[0043][0044]
其中，(γ,θ,ψ)分别为载体横滚、俯仰、航向角变量，γ＝0,θ＝0；
[0045]
步骤(5-2)，根据步骤(5-1)获得的姿态转化矩阵步骤(4-1)获得的载体车头速度ve，步骤(4-3)获得的载体天向速度vu，计算载体的地理速度其表达式为：
[0046][0047][0048][0049]
其中，为地理系相对于地球系的速度在地理系上的东北天方向上的分量,为地理系相对地球系的速度在载体系轴向的分量构成的列矢量；
[0050]
步骤(5-3)，根据步骤(5-2)获得的载体的地理速度步骤(5-1)获得的姿态转化矩阵步骤(4-5)获得的偏航角角速率ωb，步骤(3-3)获得的载体偏航角ψ，计算陀螺仪无噪声输出其表达式为：
[0051][0052]
其中，为地球系相对惯性系的角速度在载体系轴向的分量构成的列矢量，为地理系相对地球系的角速度在载体系轴向的分量构成的列矢量，为载体系相对地理系的角速度在载体系轴向的分量构成的列矢量，为地球系相对惯性系的角速度在地理系
轴向的分量构成的列矢量，为地理系相对地球系的角速度在地理系轴向的分量构成的列矢量；(γ,θ,ψ)分别为载体横滚、俯仰、航向角变量，γ＝0,θ＝0，分别为载体的偏航角、俯仰角、横滚角角速率，ω
ie
为地球自转角速率，取值为7.292115147
×
10-5
rad/s；
[0053]
步骤(5-4)，根据步骤(4-1)获得的载体车头方向速度ve，根据步骤(4-2)获得的载体车头方向加速度ae，根据步骤(4-3)获得的载体天向速度vu，根据步骤(4-4)获得的载体天向速度au，根据步骤(5-3)获得的角速度步骤(5-2)获得的载体的地理速度步骤(5-1)获得的姿态转化矩阵计算加速度计测得的比力fb，其表达式为：
[0054][0055]
其中，
[0056][0057]
g为重力加速度，fn为比力在导航系中的分量构成的列矢量，为速度的微分。
[0058]
进一步的，所述步骤(6)包括如下具体步骤：
[0059]
步骤(6-1)，根据步骤(5-2)获得的载体的地理速度计算载体经纬高位置坐标，其表达式为：
[0060][0061][0062][0063]
其中，与分别为相邻航路点i与i+1之间经过采样周期t离散后两个相邻离散航路点坐标，单位为度，度，米；符号上标表示此数据为第i个航路点与第i+1个航路点之间插值出来的离散航路点数据，符号下标表示此数据为第i个航路点与第i+1个航路点之间插值出来的第j个离散航路点数据；
[0064]
步骤(6-2)，根据步骤(6-1)获得的航路点坐标推算公式，由推算得到的第i个航路点坐标数据来连续推算得到第i+1个航路点坐标，其表达式为：
[0065]
h(
i+1
)
t
＝h
it
+δhi[0066]
l(
i+1
)
t
＝l
it
+δli[0067]
λ
(i+1)t
＝λ
it
+δλi[0068]
其中，(l
(i+1)t
，λ
(i+1)t
，h
(i+1)t
)与(l
it
，λ
it
，h
it
)分别为推算得到的第i+1、i个航路点
坐标数据，(δli，δλi，δhi)为由推算得到的第i个航路点坐标数据来连续推算第i+1个航路点经纬高方向坐标增量。
[0069]
进一步的，所述步骤(7)包括如下具体步骤：
[0070]
步骤(7-1)，根据步骤(6-2)获得的第i+1个推算航路点坐标与步骤(2-4)得到的第i+1个真实航路点坐标，将它们对比来评估反演数据是否满足精度要求，若反演数据满足精度要求，则直接进入步骤(8)；反之，则进入步骤(7-2)，反演数据精度判决表达式为：
[0071][0072]
其中，r为设定的反演数据精度评估值，单位为米，re为地球半径，取值为6378137.0米；
[0073]
步骤(7-2)，重新设定采样周期t，返回步骤(3-3)重新进行插值计算，其表达式为：
[0074][0075]
其中，t
old
为上一采样周期值。
[0076]
有益效果：本发明公布了一种用于非完备测量条件的异构多源导航信息反演方法，属于定位与导航技术领域。对稀疏的航路点数据使用三次样条插值法进行了插值扩充，使得载体的航路点数据更加平滑；对航路点数据进行处理分析，计算载体的速度、加速度及角速度等原始物理变量；将反演得到的陀螺仪及加速度计无噪声输出值来推算载体航路点数据，然后与真实航路点数据进行比较与评估，若不满足精度要求则进行反馈迭代修正；反之进行多导航传感器仿真数据反演。本发明可以在基于有限航路点数据的情况下在一定程度上反演出多导航传感器输出，适合实际应用。
附图说明
[0077]
图1为本发明方法的原理流程示意图；
[0078]
图2为分别采用本发明方法与传统方法反演载体航路点坐标定位误差值对比图；
[0079]
图3为在采用本发明方法反演出来的多传感器数据融合推算轨迹、载体真实运动轨迹、传统方法反演出来的多传感器数据融合推算轨迹对比图；
[0080]
图4为在采用本发明方法反演出来的多传感器数据融合推算轨迹、载体真实运动轨迹、传统方法反演出来的多传感器数据融合推算轨迹对比图的俯视图；
[0081]
图5为采用本发明方法反演出来的多传感器数据融合推算轨迹、载体真实运动轨迹、传统方法反演出来的多传感器数据融合推算轨迹的经度、纬度与高度数值对比图；
[0082]
图6～图17为采用本发明方法反演出来的多种导航传感器数据；
[0083]
图18、19、20分别为使用本发明方法与传统方法反演得到的多传感器数据融合后解算的载体位置、速度、姿态与载体真实位置、速度、姿态误差曲线对比图。
具体实施方式
[0084]
下面结合附图对本发明做更进一步的解释。
[0085]
本发明方法通过对稀疏的航路点数据使用三次样本插值方法进行扩充，对插值后的数据进行分析处理来计算多导航传感器输出数据，使得解决在多源导航实验测试中因部分传感器故障导致实验数据无法获取的问题，在一定程度上反演出多导航传感器实验输出数据。
[0086]
如图1所示，一种用于非完备测量条件的异构多源导航信息反演方法包括以下步骤：
[0087]
一种用于非完备测量条件的异构多源导航信息反演方法，包括以下步骤：
[0088]
步骤(1)，获取载体运动过程中航路点数据；
[0089]
所述步骤(1)中，载体航路点数据类型为经纬高数据或当地坐标系坐标数据，获取载体运动过程中经过第1～n个航路点的数据wp1、wp2、wp3、
…
、wpn，若为当地坐标系数据，还应获取当地坐标系原点经纬高坐标o，其表达式为：
[0090]
o＝(l0,λ0,h0)
[0091]
wp1＝(t1,x1,y1,z1,ψ1)
t
[0092]
wp2＝(t2,x2,y2,z2,ψ2)
t
[0093][0094]
wpn＝(tn,xn,yn,zn,ψn)
t
[0095]
其中，(l0,λ0,h0)分别为当地坐标系原点纬度、经度与高度坐标，t1、t2、
…
、tn分别为载体经过第1～n航路点的时刻，(x1,y1,z1)、(x2,y2,z2)、
…
、(xn,yn,zn)分别为载体经过第1～n航路点时的三维坐标，ψ1、ψ2、
…
、ψn分别为载体经过第1～n航路点时的偏航角。
[0096]
步骤(2)，依据航路点数据类型来选择相对应方案进行数据预处理；
[0097]
步骤(2-1)，根据步骤(1)航路点数据类型来判断执行的操作，若航路点数据为当地坐标系坐标数据，则执行步骤(2-2)，否则执行步骤(2-3)；
[0098]
步骤(2-2)，根据步骤(1)获得的数据，将所有航路点当地坐标系坐标转化为经纬高坐标，其表达式为：
[0099]hi
＝h0+zi[0100][0101][0102]
其中，rm为地球卯酉圈曲率半径，rn为地球子午圈曲率半径，(l0,λ0,h0)为当地坐标系原点纬度、经度与高度坐标，(xi,yi,zi)为第i个航路点在当地坐标系坐标，(li,λi,hi)为转化后第i个航路点纬度、经度与高度坐标，π为圆周率；
[0103]
步骤(2-3)，根据步骤(1)获得的数据，将所有航路点坐标转化为经纬高坐标，其表达式为：
[0104]hi
＝zi[0105]
li＝yi[0106]
λi＝xi[0107]
其中，(xi,yi,zi)为第i个航路点三维坐标，(li,λi,hi)为赋值后第i个航路点纬度、
经度与高度坐标；
[0108]
步骤(2-4)，将所有的航路点数据均存放在航迹矩阵path中，其表达式为：
[0109][0110]
步骤(3)，使用插值法对预处理的航路点数据进行扩充；
[0111]
步骤(3-1)，根据步骤(2-4)获得的离散数据点通过三次样条插值算法分别找到时间与经度的关系式、时间与纬度的关系式、时间与高度的关系式、时间与偏航角的关系式,其表达式为：
[0112]
λ＝f1(t)
[0113]
l＝f2(t)
[0114]
h＝f3(t)
[0115]
ψ＝f4(t)
[0116]
其中，(λ,l,h,ψ)分别代表经度、纬度、高度与偏航角变量，t代表时间变量，(f1,f2,f3,f4)分别代表时间与经度、时间与纬度、时间与高度、时间与偏航角变量的函数关系式；
[0117]
步骤(3-2)，设置一个合适的足够小的初始采样周期t，单位为秒；
[0118]
步骤(3-3)，根据步骤(3-1)获得的时间与经度、时间与纬度、时间与高度、时间与偏航角变量的函数关系式(f1,f2,f3,f4)与步骤(3-2)获得的采样周期t，对当前时刻两个相邻的航路点数据之间进行插值扩充，其表达式为：
[0119][0120][0121][0122][0123]
其中，m为相邻航路点i与i+1(i＝1～n-1)之间离散点个数，(ti、ti+t、ti+2t、
…
、t
i+1-t、t
i+1
)为相邻航路点i与i+1之间经过采样周期t离散后的时间，(j＝1～m)分别为相邻航路点i与i+1之间经过采样周期t离散后的第j个经度、纬度、高度与偏航角变量。
[0124]
步骤(4)，计算载体运动加速度及角速度；
[0125]
步骤(4-1)，根据步骤(3-3)获取的相邻航路点i与i+1之间经过采样周期t离散后经度λ与纬度l数据，计算二维平面载体车头方向的运动速度，其表达式为：
[0126][0127]
其中，
[0128][0129]
与分别为相邻航路点i与i+1之间经过采样周期t离散后两个相邻离散航路点纬度与经度坐标，与分别为相邻航路点i与i+1之间经过采样周期t离散后两个相邻离散航路点之间距离与载体的速度；
[0130]
步骤(4-2)，根据步骤(4-1)获取的二维平面载体车头方向的运动速度计算二维平面载体车头方向的运动加速度，其表达式为：
[0131][0132]
步骤(4-3)，根据步骤(3-3)获取的载体高度方向数据，计算载体天向运动速度，其表达式为：
[0133][0134]
步骤(4-4)，根据步骤(4-3)获取的载体天向运动速度计算二维平面载体天向运动加速度，其表达式为：
[0135][0136]
步骤(4-5)，根据步骤(3-3)获取的载体偏航角数据，计算载体偏航角角速率，其表达式为：
[0137][0138]
步骤(5)，计算反演陀螺仪与加速度计无噪声输出；
[0139]
步骤(5-1)，根据步骤(3-3)获得的航路点之间插值的偏航角变量ψ，计算载体系相对于地理系姿态转换矩阵，其表达式为：
[0140][0141]
其中，(γ,θ,ψ)分别为载体横滚、俯仰、航向角变量，γ＝0,θ＝0；
[0142]
步骤(5-2)，根据步骤(5-1)获得的姿态转化矩阵步骤(4-1)获得的载体车头速度ve，步骤(4-3)获得的载体天向速度vu，计算载体的地理速度其表达式为：
[0143]
[0144][0145][0146]
其中，为地理系相对于地球系的速度在地理系上的东北天方向上的分量,为地理系相对地球系的速度在载体系轴向的分量构成的列矢量；
[0147]
步骤(5-3)，根据步骤(5-2)获得的载体的地理速度步骤(5-1)获得的姿态转化矩阵步骤(4-5)获得的偏航角角速率ωb，步骤(3-3)获得的载体偏航角ψ，计算陀螺仪无噪声输出其表达式为：
[0148][0149]
其中，为地球系相对惯性系的角速度在载体系轴向的分量构成的列矢量，为地理系相对地球系的角速度在载体系轴向的分量构成的列矢量，为载体系相对地理系的角速度在载体系轴向的分量构成的列矢量，为地球系相对惯性系的角速度在地理系轴向的分量构成的列矢量，为地理系相对地球系的角速度在地理系轴向的分量构成的列矢量；(γ,θ,ψ)分别为载体横滚、俯仰、航向角变量，γ＝0,θ＝0，分别为载体的偏航角、俯仰角、横滚角角速率，ω
ie
为地球自转角速率，取值为7.292115147
×
10-5
rad/s；
[0150]
步骤(5-4)，根据步骤(4-1)获得的载体车头方向速度ve，根据步骤(4-2)获得的载体车头方向加速度ae，根据步骤(4-3)获得的载体天向速度vu，根据步骤(4-4)获得的载体天向速度au，根据步骤(5-3)获得的角速度步骤(5-2)获得的载体的地理速度步骤(5-1)获得的姿态转化矩阵计算加速度计测得的比力fb，其表达式为：
[0151]
[0152]
其中，
[0153][0154]
g为重力加速度，fn为比力在导航系中的分量构成的列矢量，为速度的微分。
[0155]
步骤(6)，计算载体反演航路点坐标；
[0156]
步骤(6-1)，根据步骤(5-2)获得的载体的地理速度计算载体经纬高位置坐标，其表达式为：
[0157][0158][0159][0160]
其中，与分别为相邻航路点i与i+1之间经过采样周期t离散后两个相邻离散航路点坐标，单位为度，度，米；符号上标表示此数据为第i个航路点与第i+1个航路点之间插值出来的离散航路点数据，符号下标表示此数据为第i个航路点与第i+1个航路点之间插值出来的第j个离散航路点数据；
[0161]
步骤(6-2)，根据步骤(6-1)获得的航路点坐标推算公式，由推算得到的第i个航路点坐标数据来连续推算得到第i+1个航路点坐标，其表达式为：
[0162]
h(
i+1
)
t
＝h
it
+δhi[0163]
l(
i+1
)
t
＝l
it
+δli[0164]
λ
(i+1)t
＝λ
it
+δλi[0165]
其中，(l
(i+1)t
，λ
(i+1)t
，h
(i+1)t
)与(l
it
，λ
it
，h
it
)分别为推算得到的第i+1、i个航路点坐标数据，(δli，δλi，δhi)为由推算得到的第i个航路点坐标数据来连续推算第i+1个航路点经纬高方向坐标增量。
[0166]
步骤(7)，评估反演数据是否满足精度；
[0167]
步骤(7-1)，根据步骤(6-2)获得的第i+1个推算航路点坐标与步骤(2-4)得到的第i+1个真实航路点坐标，将它们对比评估反演数据是否满足精度要求，若反演数据满足精度要求，则直接进入步骤(8)；反之则进入步骤(7-2)，反演数据精度判决表达式为：
[0168][0169]
其中，r为设定的反演数据精度评估值，单位为米，re为地球半径，取值为6378137.0米；
[0170]
步骤(7-2)，重新设定采样周期t，返回步骤(3-3)重新进行插值计算，其表达式为：
[0171][0172]
其中，t
old
为上一采样周期值。
[0173]
步骤(8)，反演多种导航传感器仿真输出数据；
[0174]
步骤(8-1)，根据步骤(5-4)获得的加速度计测得的比力fb，计算加速度计仿真输出其表达式为：
[0175][0176]
其中，为加速度计漂移，其误差模型为为的微分，ta为相关时间，ωa为测量白噪声。
[0177]
步骤(8-2)，根据步骤(5-3)获得的陀螺仪无噪声输出计算陀螺仪仿真输出其表达式为：
[0178][0179]
其中，ε为陀螺漂移，(εb、εr、ωg)分别为陀螺仪随机常数、一阶马尔可夫过程与测量白噪声，陀螺仪随机常数模型为陀螺仪一阶马尔可夫过程为tr为相关时间，ωr为驱动白噪声；
[0180]
步骤(8-3)，根据步骤(3-3)获得的载体插值出来的经纬高变量，根据步骤(5-2)获得的载体地理速度变量，计算gnss仿真输出的载体位置及速度数据，其表达式为：
[0181][0182][0183][0184][0185][0186][0187]
其中，为反演的载体经纬高坐标，(l，λ，h)为相邻航路点i与i+1之间经过采样周期t离散后离散航路点坐标变量，(v
xgnss
，v
ygnss
，v
zgnss
)分别为gnss经度、纬度与高度方向上的测量噪声误差，单位为：米，米，米；为反演的载体在地理系东北天方向速度，为gnss在地理系东北天方向上速度测量
噪声误差，单位为：m/s，m/s，m/s；
[0188]
步骤(8-4)，根据步骤(3-3)设定的时间变量计算计时器仿真输出数据，其表达式为：
[0189][0190]
其中，t为载体经过航路点i的时间变量，v
tm
为计时器上的时间偏差，单位为秒；
[0191]
步骤(8-5)，根据步骤(5-1)获得的姿态转化矩阵来计算磁传感器仿真输出数据，其表达式为：
[0192][0193]
其中，(h
x
，hy，hz)为已知的地理系东北天三个轴向的地磁分量，单位为：gs，gs，gs；分别为磁传感器在载体东北天方向上的测量噪声误差，单位为：gs，gs，gs；
[0194]
步骤(8-6)，根据步骤(4-1)获取的二维平面载体车头方向的运动速度变量ve来计算里程计位移仿真输出数据，单位为米，其表达式为：
[0195][0196]
其中，v
odo
为里程计位移的测量噪声误差，单位为米；
[0197]
步骤(8-7)，根据步骤(4-1)获取的二维平面载体车头方向的运动速度变量ve来计算在时间周期t内步进传感器脉冲输出个数其表达式为：
[0198][0199]vstep
为步进传感器线位移的测量噪声误差，单位为米，δs
p
为单位脉冲线位移，单位为米；
[0200]
步骤(8-8)，根据步骤(3-3)获得的载体插值出来的高度变量h来计算气压高度计仿真输出数据，其表达式为：
[0201][0202]
其中，v
bh
分别为气压高度计高度方向上的测量噪声误差，单位为米；
[0203]
步骤(8-9)，根据步骤(3-3)获得的载体插值出来的高度变量h来计算激光高度计仿真输出数据，其表达式为：
[0204][0205]
其中，h
lh
为已知的当地地形高度值，单位为米，v
la
分别为激光高度计高度方向上的测量噪声误差，单位为米；
[0206]
步骤(8-10)，计算倾角罗盘仿真输出数据，其表达式为：
[0207][0208]
[0209]
其中，分别为倾角罗盘对载体横滚、俯仰角的测量值，(γ，θ)为对应于载体横滚、俯仰角插值变量，γ＝0,θ＝0，(v
comp_dr
，v
comp_dp
)分别为倾角罗盘对载体横滚、俯仰角的测量噪声误差，单位为：度，度；
[0210]
步骤(8-11)，根据步骤(3-3)获得的载体插值出来的载体偏航角变量ψ来计算回转罗盘仿真输出数据，单位为度，其表达式为：
[0211][0212]
其中，为回转罗盘对载体航向角的测量值，v
comp_gy
为回转罗盘对载体航向角的测量噪声误差，单位为度；
[0213]
步骤(8-12)，根据步骤(3-3)获得的载体插值出来的载体偏航角变量ψ来计算偏振光罗盘仿真输出数据，其表达式为：
[0214][0215]
其中，为偏振光罗盘航向角的测量值输出数据，v
comp_pl
为偏振光罗盘对载体航向角的测量噪声误差，单位为度；
[0216]
步骤(8-13)，根据步骤(3-3)获得的载体插值出来的载体偏航角变量ψ来计算视觉传感器仿真输出数据，其表达式为：
[0217][0218][0219][0220]
其中，分别为视觉传感器对载体横滚、俯仰、航向角的测量值，(γ，θ，ψ)为对应于载体横滚、俯仰、航向角插值变量，(v
vs_r
，v
vs_p
，v
vs_y
)分别为视觉传感器对载体横滚、俯仰、航向角的测量噪声误差，单位为：度，度，度；
[0221]
步骤(8-14)，根据步骤(3-3)获得的载体插值出来的经纬高变量来计算停止标记传感器仿真输出数据，若载体与标志点之间距离小于设定值d
st
，则停止标记传感器输出为1，反之为0，距离判决表达式为：
[0222][0223]
其中，(l，λ，h)为相邻航路点i与i+1之间经过采样周期t离散后离散航路点坐标变量，(l
st
，λ
st
，h
st
)为停止标记传感器测量的标志点经纬高坐标，v
st
为停止标记传感器距离测量噪声误差，单位为米；
[0224]
步骤(8-15)，根据步骤(3-3)获得的载体插值出来的经纬高变量来计算超声传感器距离仿真输出数据，单位为米，其表达式为：
[0225]
[0226]
其中，(l，λ，h)为相邻航路点i与i+1之间经过采样周期t离散后离散航路点坐标变量，(l
us
，λ
us
，h
us
)为超声传感器测量的标志点经纬高坐标，v
us
为超声传感器距离测量噪声误差，单位为米；
[0227]
步骤(8-16)，根据步骤(3-3)获得的载体插值出来的经纬高变量来计算红外传感器距离仿真输出数据，单位为米，其表达式为：
[0228][0229]
其中，(l，λ，h)为相邻航路点i与i+1之间经过采样周期t离散后离散航路点坐标变量，(l
is
，λ
is
，h
is
)为红外传感器测量的标志点经纬高坐标，v
is
为红外传感器距离测量噪声误差，单位为米；
[0230]
步骤(8-17)，根据步骤(3-3)获得的载体插值出来的经纬高变量来计算二维激光雷达距离仿真输出数据，单位为米，其表达式为：
[0231][0232]
其中，(l
t
，λ
t
)为激光雷达在时间为t时的二维经纬坐标变量，单位为：度，度；表示为在时间t时二维平面转角为时激光雷达发射器所扫到的目标点二维经纬坐标，单位为：度，度；v
ld
为激光雷达传感器距离测量噪声误差，单位为米；
[0233]
步骤(8-18)，根据步骤(3-3)获得的载体插值出来的经纬高变量来计算蓝牙传感器仿真输出数据，单位为米，其表达式为：
[0234][0235]
其中，(l，λ，h)为相邻航路点i与i+1之间经过采样周期t离散后离散航路点坐标变量，(l
bt
，λ
bt
，h
bt
)为蓝牙基站的经纬高坐标，v
bt
为蓝牙传感器距离测量噪声误差，单位为米；
[0236]
步骤(8-19)，根据步骤(3-3)获得的载体插值出来的经纬高变量来计算wifi仿真输出数据，单位为米，其表达式为：
[0237][0238]
其中，(l，λ，h)为相邻航路点i与i+1之间经过采样周期t离散后离散航路点坐标变量，(l
wifi
，λ
wifi
，h
wifi
)为wifi基站的经纬高坐标，v
wifi
为wifi距离测量噪声误差，单位为米；
[0239]
步骤(8-20)，根据步骤(3-3)获得的载体插值出来的经纬高变量来计算uwb仿真输出数据，单位为米，其表达式为：
[0240][0241]
其中，(l，λ，h)为相邻航路点i与i+1之间经过采样周期t离散后离散航路点坐标变量，(l
uwb
，λ
uwb
，h
uwb
)为uwb基站的经纬高坐标，v
uwb
为uwb距离测量噪声误差，单位为米。
[0242]
为了验证本发明所提出的一种用于非完备测量条件的异构多源导航信息反演方法的有效性，进行数字仿真分析。通过本发明方法反演出来的陀螺仪与加速度计无偏差数据正向推算载体的运动轨迹，来说明仿真出来数据的准确性。图2为分别采用本发明方法与传统方法反演载体航路点坐标定位误差值对比图；图3为在采用本发明方法反演出来的多传感器数据融合推算轨迹、载体真实运动轨迹、传统方法反演出来的多传感器数据融合推算轨迹对比图；图4为在采用本发明方法反演出来的多传感器数据融合推算轨迹、载体真实运动轨迹、传统方法反演出来的多传感器数据融合推算轨迹对比图的俯视图；图5为采用本发明方法反演出来的多传感器数据融合推算轨迹、载体真实运动轨迹、传统方法反演出来的多传感器数据融合推算轨迹的经度、纬度与高度数值对比图；图6～图17为采用本发明方法反演出来的多种导航传感器数据；图18、19、20分别为使用本发明方法与传统方法反演得到的多传感器数据融合后解算的载体位置、速度、姿态与载体真实位置、速度、姿态误差曲线对比图。
[0243]
由图2可以看出，采用本发明方法反演的载体航路点坐标定位误差值明显小于传统方法反演的载体航路点坐标定位误差值，说明了本发明方法经过反馈修正迭代过的imu无噪声输出反演的航路点坐标接近于真实航路点坐标数据。
[0244]
由图3、图4与图5可以看出，采用传统方法反演的多传感器数据融合计算出来的载体运动轨迹无法较好拟合载体真实运动轨迹，而使用本发明方法反演的多传感器数据融合计算出来的载体运动轨迹可以较好拟合载体真实运动轨迹，从而反映出使用本发明方法反演出来的载体多导航传感器数据的准确性。
[0245]
由图18、19、20可以看出，经过传统方法反演的多传感器信息融合得到的导航状态量误差值明显大于本发明方法反演的多传感器信息融合得到的导航状态量误差值，这说明了本发明的有效性。同时，本发明方法得到的位置、速度、姿态误差均较小，在规定范围之内，符合实际需求。
[0246]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。