一种基于法向量位置模型的惯性导航综合校正方法

1.本发明涉及惯性导航技术领域，尤其涉及一种基于法向量位置模型的惯性导航综合校正方法。


背景技术：

2.惯性导航具有连续性好、自主性高、隐蔽性强的特点，是船舶极区航行最基本的导航手段。由于高纬度地区子午线汇聚，常用的指北方位惯性导航力学编排并不适用于极区。另外当船舶出入极区时，需要将传统导航算法与极区算法进行切换，切换过程会影响算法内部过程的连续性与一致性。
3.对于长航时的惯性导航系统，惯性指示位置和航向角的主要误差源是陀螺仪未被补偿的漂移误差。陀螺漂移除了产生周期振荡传播的误差外，还产生随时间增长而不断累积的误差。为抑制惯导系统随时间积累的误差，对于长期水下航行的载体，可以通过偶尔上浮水面，采用卫星导航、天文导航等外部参考信息校正惯导系统误差。为避免频繁上浮，根据稀疏的观测信息对惯导系统进行综合校正具有重要的现实意义。对于目前普遍使用的光学陀螺航海惯导系统，单轴旋转调制惯导系统只需考虑方位陀螺常值漂移误差，而双轴旋转调制光学陀螺航海惯导系统的绝大部分方位陀螺常值漂移误差虽然得到了抑制，仍会有残存的等效方位陀螺常值漂移误差。
4.传统的惯性导航综合校正技术是把漂移角与陀螺漂移之间的关系方程投影到o系(qep坐标系)上。o系下漂移角与陀螺漂移之间的关系方程存在解析解的前提是假设载体在地球表面位移运动的角速度远小于地球自转角速度，同时假设陀螺漂移角速度误差εo近似为常值，由于又因为
[0005][0006]
由于方向余弦矩阵仅与纬度l相关，且陀螺仪与载体固连，εb可以近似为常值，因此εo近似为常值的条件就是近似为常值。即对于传统的捷联式惯导系统而言，传统的综合校正方法要求载体低速、近似等纬度航行，载体的姿态保持不变，实用性受到了较大限制。
[0007]
另一方面，纬度l和经度λ的位置误差向量与漂移角之间的关系方程为：
[0008][0009]
由于此系数矩阵与纬度l的正切值相关，因此传统综合校正方法在高纬度地区的观测方程系数矩阵是病态的，也就是说并不适用于极区。
[0010]
同理，当横纬度l
t
＝90
°
，即载体位于赤道(横向地球坐标系极点)时，在横坐标系
下的极区校正方法同样是不适用的，不具备全球适用性。
[0011]
另外，由于o系的e轴指向东，在极点附近e轴指向将发生较快变化，使得传统综合校正受到一定影响。
[0012]
综上，传统的惯性导航综合校正技术存在以下缺陷：
[0013]
传统的惯性导航综合校正技术是在qep坐标系下编排的，要求载体低速且近似等纬度航行的条件，实用性受到了较大限制；
[0014]
传统的综合校正方法中位置误差与漂移角之间的关系方程是在传统当地水平导航坐标系下实现的，并不适用于极区；
[0015]
适用于极区的横坐标系、格网坐标系下的惯性导航综合校正方法同样不具备全球适用性；
[0016]
在穿越极区的应用场景中，由于需要与传统的地理坐标系算法进行切换，这样会带来积分过程的变化，对于综合校正算法，切换过程会影响内部算法的连续性与一致性，同时还极大地增加算法复杂度。


技术实现要素：

[0017]
本发明的目的在于提供一种基于法向量位置模型的惯性导航综合校正方法。
[0018]
为实现上述发明目的，本发明提供一种基于法向量位置模型的惯性导航综合校正方法，包括：
[0019]
s1.接收来自imu惯性测量单元的实时旋转角速度矢量和比力矢量，并装订初始导航参数，其中，所述初始导航参数包括：初始经纬度、初始指示高度h(t0)、初始导航速度vn(t0)、初始导航姿态矩阵基于所述初始导航参数获取航行器的初始法向量位置矢量η(t0)、初始四元组位置矢量p
η
(t0)、初始方向余弦矩阵以及获取地心地固坐标系下的初始速度和初始姿态矩阵
[0020]
s2.根据前一时刻地心地固坐标系下的前刻速度前刻姿态矩阵前刻四元组位置矢量p
η
(t
i-1
)，以及所述imu惯性测量单元中陀螺仪和加速度计的测量值和方位陀螺漂移εu(t0)，采用纯惯导解算的递推公式得到当前时刻的当前速度当前姿态矩阵当前四元组位置矢量p
η
(ti)；
[0021]
s3.根据外参考速度采用水平阻尼的递推公式得到当前时刻阻尼后的当前阻尼速度和当前时刻阻尼后的当前阻尼四元组位置矢量p
η_damp
(ti)，并计算法向量积分项int
η
(ti)；
[0022]
s4.判断是否需要综合校正且成功接收来自外部测量设备的位置，如果不需要校正则转至s2；如果需要综合校正且成功接收来自外部测量设备的实时经纬度并得到外参考位置p
ηr
(t
mk
)时，则基于观测次数k选取重调所述外参考位置p
ηr
(t
mk
)的方式，其中，如果观测次数k等于1，则将输出水平位置重调为外参考位置p
ηr
(t
mk
)，记录当次成功观测的时间t
mk
并且将法向量积分项int
η
(t
mk
)置零；
[0023]
s5.如果观测次数k大于1，将输出的水平位置重调为外参考位置p
ηr
(t
mk
)，根据法向量位置误差矢量δη(t
mk
)，与前一次观测的时间间隔δt
mk
和法向量积分项int
η
(t
mk
)，按照
综合校正公式得到漂移角和方位陀螺漂移根据漂移角将姿态矩阵输出校正为而方位陀螺漂移将用于之后纯惯导的导航解算，最后记录当次成功观测的时间t
mk
并且将法向量积分项int
η
(t
mk
)置零；
[0024]
s6.重复执行步骤s2至s5，直至导航结束。
[0025]
根据本发明的一个方面，步骤s1中，基于所述初始导航参数获取航行器的初始法向量位置矢量η(t0)、初始四元组位置矢量p
η
(t0)、初始方向余弦矩阵的步骤中，包括：
[0026]
基于所述初始经纬度计算所述航行器的初始法向量位置矢量η(t0)和初始四元组位置矢量p
η
(t0)；
[0027]
基于所述初始法向量位置矢量η(t0)计算所述初始方向余弦矩阵
[0028]
根据本发明的一个方面，步骤s1中，基于所述初始经纬度计算所述航行器的初始法向量位置矢量η(t0)和初始四元组位置矢量p
η
(t0)的步骤中，所述初始法向量位置矢量η(t0)的计算公式表示为：
[0029][0030]
其中，l(t0)、λ(t0)为初始的地理纬度和地理经度，η
x
(t0)、ηy(t0)和ηz(t0)分别为初始法向量位置矢量η(t0)在x、y和z方向的三个分量；
[0031]
所述初始四元组位置矢量p
η
(t0)的计算方法表示为:
[0032]
p
η
(t0)＝[η
x
(t0)ηy(t0)ηz(t0)h(t0)]
t
[0033]
其中，h(t0)为初始指示高度。
[0034]
根据本发明的一个方面，步骤s1中，基于所述初始法向量位置矢量η(t0)计算所述初始方向余弦矩阵的步骤中，所述初始方向余弦矩阵为地心地固坐标系与当地水平指北地理坐标系的方向余弦矩阵，其计算方法表示为：
[0035][0036]
根据本发明的一个方面，步骤s1中，获取地心地固坐标系下的初始速度和初始姿态矩阵的步骤中，基于所述初始导航速度vn(t0)、所述初始导航姿态矩阵得到所述初始速度和所述初始姿态矩阵其中，所述初始速度的计算公式表示为：
[0037][0038]
所述初始姿态矩阵的计算公式表示为：
[0039][0040]
根据本发明的一个方面，步骤s2中，采用纯惯导解算的递推公式得到当前时刻的当前速度当前姿态矩阵当前四元组位置矢量p
η
(ti)的步骤中，包括：
[0041]
采用地心地固坐标系下的速度递推公式获得所述当前时刻的当前速度其表示为：
[0042][0043]
其中，t为采样间隔，为地球自转角速度矢量，fb(ti)为所述imu惯性测量单元中加速度计实时获得的比力矢量，为前一时刻的重力矢量在地心地固坐标系下的投影，为的斜对称矩阵；
[0044]
根据确定的所述当前速度更新所述前刻四元组位置矢量p
η
(t
i-1
)，以获得所述当前四元组位置矢量p
η
(ti)，即四元组位置递推公式表示为：
[0045][0046]
其中，re为卯酉圈半径，rn为子午圈半径；
[0047]
所述卯酉圈半径re和所述子午圈半径rn表示为：
[0048][0049]
其中，r0＝6378137为地球半径，e＝0.081819191为地球第一偏心率；
[0050]
根据所述当前四元组位置矢量p
η
(ti)和所述imu惯性测量单元中陀螺仪测量的实时旋转角速度矢量更新姿态，即获取所述当前姿态矩阵则姿态矩阵递推公式为：
[0051][0052][0053]
其中，i3为3阶单位矩阵，为陀螺仪实时获得的旋转角速度矢量，εu(t
mk
)为
第k次校正后得到的方位陀螺漂移，其初始值为0。
[0054]
根据本发明的一个方面，在步骤s3中，根据外参考速度采用水平阻尼的递推公式得到当前时刻阻尼后的当前阻尼速度和当前时刻阻尼后的当前阻尼四元组位置矢量p
η_damp
(ti)，并计算法向量积分项int
η
(ti)的步骤中，所述水平阻尼的递推公式为：
[0055][0056]
其中，vh(ti)为当前时刻的水平速度矢量；
[0057][0058]
其中，v
hr
(ti)为当前时刻的外参考水平速度矢量。v
h_damp
(ti)为当前时刻阻尼后的水平速度矢量；
[0059]
基于所述水平阻尼的递推公式获得所述当前阻尼速度其表示为：
[0060][0061]
其中，η(ti)表示当前法向量位置矢量，vh(ti)表示当前垂直指示速度，num与dum均为相位滞后-超前串联校正网络的阻尼系数，即：
[0062]
num(1)＝1.0，num(2)＝-1.99997367841154，num(3)＝0.999975215058425
[0063]
dum(1)＝1.0，dum(2)＝-1.99875923183544，dum(3)＝0.998760768482328；
[0064]
基于所述水平阻尼的递推公式构建法向量位置的递推公式，其表示为：
[0065][0066]
基于所述法向量位置的递推公式获得所述当前阻尼四元组位置矢量p
η_damp
(ti)，其表示为：
[0067][0068]
其中，h(ti)为当前指示高度；
[0069]
所述法向量积分项int
η
(ti)的递推公式为：
[0070][0071]
根据本发明的一个方面，步骤s5中，所述综合校正公式为：
[0072][0073]
其中，
[0074][0075]
则所述综合校正公式简化为：
[0076][0077]
其中，β
ε
为马尔科夫过程相关时间常数。
[0078]
根据本发明的一个方面，步骤s4和s5中，将输出的水平位置重调为外参考位置ηr(t
mk
)的步骤中，用于水平位置重调的位置重调公式表示为：
[0079][0080]
根据漂移角将姿态输出校正为的步骤中，姿态输出校正的计算公式表示为：
[0081][0082]
其中，[
×
]表示对应的斜对称矩阵。
[0083]
根据本发明的一个方面，步骤s5中，所述法向量位置误差矢量δη(t
mk
)表示为：
[0084][0085]
其中，η(t
mk
)表示第k次校正时刻位置重调之前的法向量位置矢量。
[0086]
根据本发明的一种方案，本发明以船舶的极区惯性导航为背景，在水平阻尼条件下，基于法向量位置模型设计实现了综合校正方法。该方法避免了出入极区时算法切换带来的复杂性和不连续的问题，具有全球适用性；避免了对船舶的运动轨迹和运行状态的限制，具有较强的实用性；准确估计了方位陀螺常值漂移，有效抑制了长航时积累的导航误差，改善了导航定位精度。
附图说明
[0087]
图1是示意性表示根据本发明的一种实施方式的惯性导航综合校正方法的步骤框图；
[0088]
图2是示意性表示根据本发明的一种实施方式的惯性导航综合校正方法的流程
图。
具体实施方式
[0089]
为了更清楚地说明本发明实施方式或现有技术中的技术方案，下面将对实施方式中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施方式，对于本领域普通技术人员而言，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0090]
在针对本发明的实施方式进行描述时，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”所表达的方位或位置关系是基于相关附图所示的方位或位置关系，其仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此上述术语不能理解为对本发明的限制。
[0091]
下面结合附图和具体实施方式对本发明作详细地描述，实施方式不能在此一一赘述，但本发明的实施方式并不因此限定于以下实施方式。
[0092]
结合图1和图2所示，根据本发明的一种实施方式，本发明的一种基于法向量位置模型的惯性导航综合校正方法，包括：
[0093]
s1.接收来自imu惯性测量单元的实时旋转角速度矢量和比力矢量，并装订初始导航参数，其中，初始导航参数包括：初始经纬度、初始指示高度h(t0)、初始导航速度vn(t0)(其为东北天坐标系下的初始速度)、初始导航姿态矩阵以及，基于初始导航参数获取航行器的初始法向量位置矢量η(t0)、初始四元组位置矢量p
η
(t0)、初始方向余弦矩阵以及获取地心地固坐标系下的初始速度和初始姿态矩阵
[0094]
s2.根据前一时刻地心地固坐标系下的前刻速度前刻姿态矩阵前刻四元组位置矢量p
η
(t
i-1
)，以及imu惯性测量单元中陀螺仪和加速度计的测量值(即imu惯性测量单元输出的实时旋转角速度和比力矢量)和方位陀螺漂移εu(t0)，采用纯惯导解算的递推公式得到当前时刻的当前速度当前姿态矩阵当前四元组位置矢量p
η
(ti)；
[0095]
s3.根据外参考速度采用水平阻尼的递推公式得到当前时刻阻尼后的当前阻尼速度和当前时刻阻尼后的当前阻尼四元组位置矢量p
η_damp
(ti)，并计算法向量积分项int
η
(ti)；其中，外部参考速度可采用外部测量速度的设备获得，例如水速计、空速计、多普勒计程仪等；
[0096]
s4.判断是否需要综合校正且成功接收来自外部测量设备的位置，如果不需要校正则转至s2；如果需要综合校正且成功接收来自外部测量设备的实时经纬度并得到外参考位置p
ηr
(t
mk
)时，则基于观测次数k选取重调外参考位置p
ηr
(t
mk
)的方式，其中，如果观测次数k等于1，则将输出的水平位置重调为外参考位置p
ηr
(t
mk
)，记录当次成功观测的时间t
mk
并且将法向量积分项int
η
(t
mk
)置零；其中，外部测量设备是指外部测量位置的设备，例如卫星测量设备；外参考位置p
ηr
(t
mk
)由外部测量设备提供的实时经纬度获得，公式与初始四元组位置矢量计算公式一致；此外，输出的水平位置为惯性指示位置，即惯性系统经过纯惯导计算得到的位置，重调为外参考位置就是将外参考位置设置为惯性指示位置；
[0097]
s5.如果观测次数k大于1，将输出的水平位置重调为外参考位置p
ηr
(t
mk
)，根据法向量位置误差矢量δη(t
mk
)，与前一次观测的时间间隔δt
mk
和法向量积分项int
η
(t
mk
)，按照综合校正公式得到漂移角和方位陀螺漂移根据漂移角将姿态矩阵输出校正为而方位陀螺漂移将用于之后纯惯导的导航解算，最后记录当次成功观测的时间t
mk
并且将法向量积分项int
η
(t
mk
)置零；
[0098]
s6.重复执行步骤s2至s5，直至导航结束。
[0099]
根据本发明的一种实施方式，步骤s1中，基于初始导航参数获取航行器的初始法向量位置矢量η(t0)、初始四元组位置矢量p
η
(t0)、初始方向余弦矩阵的步骤中，包括：
[0100]
基于初始经纬度计算航行器的初始法向量位置矢量η(t0)和初始四元组位置矢量p
η
(t0)；
[0101]
基于初始法向量位置矢量η(t0)计算初始方向余弦矩阵
[0102]
根据本发明的一种实施方式，步骤s1中，基于初始经纬度计算航行器的初始法向量位置矢量η(t0)和初始四元组位置矢量p
η
(t0)的步骤中，初始法向量位置矢量η(t0)的计算公式表示为：
[0103][0104]
其中，l(t0)、λ(t0)为初始的地理纬度和地理经度，η
x
(t0)、ηy(t0)和ηz(t0)分别为初始法向量位置矢量η(t0)在x、y和z方向的三个分量；
[0105]
初始四元组位置矢量p
η
(t0)的计算方法表示为:
[0106]
p
η
(t0)＝[η
x
(t0)ηy(t0)ηz(t0)h(t0)]
t
[0107]
其中，h(t0)为初始指示高度。
[0108]
根据本发明的一种实施方式，步骤s1中，基于初始法向量位置矢量η(t0)计算初始方向余弦矩阵的步骤中，初始方向余弦矩阵为地心地固坐标系与当地水平指北地理坐标系的方向余弦矩阵，其计算方法表示为：
[0109][0110]
根据本发明的一种实施方式，步骤s1中，获取地心地固坐标系下的初始速度和初始姿态矩阵的步骤中，基于初始导航速度vn(t0)、初始导航姿态矩阵得
到初始速度和初始姿态矩阵其中，初始速度的计算公式表示为：
[0111][0112]
初始姿态矩阵的计算公式表示为：
[0113][0114]
根据本发明的一种实施方式，步骤s2中，采用纯惯导解算的递推公式得到当前时刻的当前速度当前姿态矩阵当前四元组位置矢量p
η
(ti)的步骤中，包括：
[0115]
采用地心地固坐标系下的速度递推公式获得当前时刻的当前速度(ti)，其表示为：
[0116][0117]
其中，t为采样间隔，为地球自转角速度矢量，fb(ti)为imu惯性测量单元中加速度计实时获得的比力矢量，为前一时刻的重力矢量在地心地固坐标系下的投影，为的斜对称矩阵；
[0118]
根据确定的当前速度更新前刻四元组位置矢量p
η
(t
i-1
)，以获得当前四元组位置矢量p
η
(ti)，即四元组位置递推公式表示为：
[0119][0120]
其中，re为卯酉圈半径，rn为子午圈半径；
[0121]
卯酉圈半径re和子午圈半径rn表示为：
[0122][0123]
其中，r0＝6378137为地球半径，e＝0.081819191为地球第一偏心率；
[0124]
根据当前四元组位置矢量p
η
(ti)和imu惯性测量单元中陀螺仪测量的实时旋转角速度矢量更新姿态，即获取当前姿态矩阵则姿态矩阵递推公式为：
[0125][0126]
[0127]
其中，i3为3阶单位矩阵，为陀螺仪实时获得的旋转角速度矢量，εu(t
mk
)为第k次校正后得到的方位陀螺漂移，其初始值为0。
[0128]
根据本发明的一种实施方式，在步骤s3中，根据外参考速度采用水平阻尼的递推公式得到当前时刻阻尼后的当前阻尼速度和当前时刻阻尼后的当前阻尼四元组位置矢量p
η_damp
(ti)，并计算法向量积分项int
η
(ti)的步骤中，水平阻尼的递推公式为：
[0129][0130]
其中，vh(ti)为当前时刻的水平速度矢量。
[0131][0132]
其中，v
hr
(ti)为当前时刻的外参考水平速度矢量。v
h_damp
(ti)为当前时刻阻尼后的水平速度矢量。
[0133]
基于水平阻尼的递推公式获得当前阻尼速度其表示为：
[0134][0135]
其中，η(ti)表示当前法向量位置矢量，vh(ti)表示当前垂直指示速度，num与dum均为相位滞后-超前串联校正网络的阻尼系数，即：
[0136]
num(1)＝1.0，num(2)＝-1.99997367841154，num(3)＝0.999975215058425
[0137]
dum(1)＝1.0，dum(2)＝-1.99875923183544，dum(3)＝0.998760768482328；
[0138]
基于水平阻尼的递推公式构建法向量位置的递推公式，其表示为：
[0139][0140]
基于法向量位置的递推公式获得当前阻尼四元组位置矢量p
η_damp
(ti)，其表示为：
[0141][0142]
其中，h(ti)为当前指示高度；
[0143]
法向量积分项int
η
(ti)的递推公式为：
[0144][0145]
根据本发明的一种实施方式，步骤s5中，综合校正公式为：
[0146][0147]
其中，
[0148][0149]
则综合校正公式简化为：
[0150][0151]
其中，β
ε
为马尔科夫过程相关时间常数。
[0152]
根据本发明的一种实施方式，步骤s4和s5中，将输出的水平位置重调为外参考位置ηr(t
mk
)的步骤中，用于水平位置重调的位置重调公式表示为：
[0153][0154]
根据漂移角将姿态输出校正为的步骤中，姿态输出校正的计算公式表示为：
[0155][0156]
其中，[
×
]表示对应的斜对称矩阵。
[0157]
根据本发明的一种实施方式，步骤s5中，法向量位置误差矢量δη(t
mk
)表示为：
[0158][0159]
其中，η(t
mk
)表示第k次校正时刻位置重调之前的法向量位置矢量。
[0160]
根据本发明，阻尼后的法向量位置误差向量δη与e系下漂移角误差向量的关系方程为：
[0161][0162]
由于此系数矩阵[η
×
]仅与法向量相关，与纬度l或横纬度l
t
的正切值不再相关，因此本发明的综合校正方法是全球适用的。
[0163]
此外，e系下漂移角的误差微分方程为：
[0164]
[0165]
由于旋转调制惯导系统的水平方向陀螺漂移影响已得到较充分的抑制，因此本发明只考虑残存的等效方位陀螺漂移作为主要的误差影响因素。由于法向量方向是当地垂线方向，因此：
[0166][0167]
其中，εu为等效方位陀螺漂移，wg为陀螺角度随机游走误差。结合上式，将εu建模为缓慢变化的马尔科夫过程，可以得到：
[0168][0169]
其中，β
ε
为马尔科夫过程相关时间常数，w
ε
为相关高斯白噪声。
[0170]
进一步的，设综合校正系统的滤波状态为：
[0171][0172]
当t
k，k-1
＝t
k-t
k-1
为小量时，短时间内陀螺漂移以及法向量近似为常值。可得滤波时间更新方程为：
[0173]
xk＝f(tk,t
k-1
)x
k-1
+w
k-1
[0174]
其中，
[0175][0176][0177][0178][0179][0180]
其中，w
gdk-1
、w
εdk-1
分别为与wg、w
ε
相对应的离散化噪声。
[0181]
由于上述推导过程没有载体运动速度较慢和陀螺漂移角速度误差ε0近似为常值的假设，因此避免了传统方法对对船舶的运动轨迹和运行状态的限制。
[0182]
进一步的，如果选取船用惯性导航系统输出法向量η
ins
与卫星导航参考位置法向
量η
gnss
的差值z
gnss
作为观测量，则观测方程可以表示为：
[0183][0184]
其中，ν
gnss
为卫星导航信息定位法向量的观测高斯白噪声。
[0185]
因此观测矩阵为：
[0186]hgnss
(tk)＝[[η(tk)
×
] 03×1]
[0187]
综上可知，本发明的综合校正算法避免了出入极区时算法切换带来的复杂性和不连续的问题，具有全球适用性；避免了对船舶的运动轨迹和运行状态的限制，具有较强的实用性；准确估计了方位陀螺常值漂移，有效抑制了长航时积累的导航误差，改善了导航定位精度。
[0188]
另外，涉及的坐标系定义如下:
[0189]
i系：地心惯性坐标系；
[0190]
e系：地心地固坐标系；
[0191]
n系：当地导航坐标系，本文中选用东北天(enu)地理坐标系；
[0192]
b系：运载体坐标系；
[0193]
o系：qep坐标系。o系的坐标原点位于载体所处位置,q轴平行于赤道平面与本地子午面的交线,由地轴指向外,e轴与等纬度圈相切指向东,p轴平行于极轴构成右手坐标系。
[0194]
上述内容仅为本发明的具体方案的例子，对于其中未详尽描述的设备和结构，应当理解为采取本领域已有的通用设备及通用方法来予以实施。
[0195]
以上所述仅为本发明的一个方案而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种基于法向量位置模型的惯性导航综合校正方法，其特征在于，包括：s1.接收来自imu惯性测量单元的实时旋转角速度矢量和比力矢量，并装订初始导航参数，其中，所述初始导航参数包括：初始经纬度、初始指示高度h(t0)、初始导航速度v
n
(t0)、初始导航姿态矩阵以及，基于所述初始导航参数获取航行器的初始法向量位置矢量η(t0)、初始四元组位置矢量p
η
(t0)、初始方向余弦矩阵以及获取地心地固坐标系下的初始速度和初始姿态矩阵s2.根据前一时刻地心地固坐标系下的前刻速度前刻姿态矩阵前刻四元组位置矢量p
η
(t
i-1
)，以及所述imu惯性测量单元中陀螺仪和加速度计的测量值和方位陀螺漂移ε
u
(t0)，采用纯惯导解算的递推公式得到当前时刻的当前速度当前姿态矩阵当前四元组位置矢量p
η
(t
i
)；s3.根据外参考速度采用水平阻尼的递推公式得到当前时刻阻尼后的当前阻尼速度和当前时刻阻尼后的当前阻尼四元组位置矢量p
η_damp
(t
i
)，并计算法向量积分项int
η
(t
i
)；s4.判断是否需要综合校正且成功接收来自外部测量设备的位置，如果不需要校正则转至s2；如果需要综合校正且成功接收来自外部测量设备的实时经纬度并得到外参考位置p
ηr
(t
mk
)时，则基于观测次数k选取重调所述外参考位置p
ηr
(t
mk
)的方式，其中，如果观测次数k等于1，则将输出的水平位置重调为外参考位置p
ηr
(t
mk
)，记录当次成功观测的时间t
mk
并且将法向量积分项int
η
(t
mk
)置零；s5.如果观测次数k大于1，将输出的水平位置重调为外参考位置p
ηr
(t
mk
)，根据法向量位置误差矢量δη(t
mk
)，与前一次观测的时间间隔δt
mk
和法向量积分项int
η
(t
mk
)，按照综合校正公式得到漂移角和方位陀螺漂移根据漂移角将姿态矩阵输出校正为而方位陀螺漂移将用于之后纯惯导的导航解算，最后记录当次成功观测的时间t
mk
并且将法向量积分项int
η
(t
mk
)置零；s6.重复执行步骤s2至s5，直至导航结束。2.根据权利要求1所述的惯性导航综合校正方法，其特征在于，步骤s1中，基于所述初始导航参数获取航行器的初始法向量位置矢量η(t0)、初始四元组位置矢量p
η
(t0)、初始方向余弦矩阵的步骤中，包括：基于所述初始经纬度计算所述航行器的初始法向量位置矢量η(t0)和初始四元组位置矢量p
η
(t0)；基于所述初始法向量位置矢量η(t0)计算所述初始方向余弦矩阵3.根据权利要求2所述的惯性导航综合校正方法，其特征在于，步骤s1中，基于所述初始经纬度计算所述航行器的初始法向量位置矢量η(t0)和初始四元组位置矢量p
η
(t0)的步骤中，所述初始法向量位置矢量η(t0)的计算公式表示为：
其中，l(t0)、λ(t0)为初始的地理纬度和地理经度，η
x
(t0)、η
y
(t0)和η
z
(t0)分别为初始法向量位置矢量η(t0)在x、y和z方向的三个分量；所述初始四元组位置矢量p
η
(t0)的计算方法表示为:p
η
(t0)＝[η
x
(t0)η
y
(t0)η
z
(t0)h(t0)]
t
其中，h(t0)为初始指示高度。4.根据权利要求3所述的惯性导航综合校正方法，其特征在于，步骤s1中，基于所述初始法向量位置矢量η(t0)计算所述初始方向余弦矩阵(t0)的步骤中，所述初始方向余弦矩阵为地心地固坐标系与当地水平指北地理坐标系的方向余弦矩阵，其计算方法表示为：5.根据权利要求4所述的惯性导航综合校正方法，其特征在于，步骤s1中，获取地心地固坐标系下的初始速度和初始姿态矩阵的步骤中，基于所述初始导航速度v
n
(t0)、所述初始导航姿态矩阵得到所述初始速度和所述初始姿态矩阵其中，所述初始速度的计算公式表示为：所述初始姿态矩阵的计算公式表示为：6.根据权利要求5所述的惯性导航综合校正方法，其特征在于，步骤s2中，采用纯惯导解算的递推公式得到当前时刻的当前速度当前姿态矩阵当前四元组位置矢量p
η
(t
i
)的步骤中，包括：采用地心地固坐标系下的速度递推公式获得所述当前时刻的当前速度其表示为：其中，t为采样间隔，为地球自转角速度矢量，f
b
(t
i
)为所述imu惯性测量单元中加速
度计实时获得的比力矢量，为前一时刻的重力矢量在地心地固坐标系下的投影，为的斜对称矩阵；根据确定的所述当前速度更新所述前刻四元组位置矢量p
η
(t
i-1
)，以获得所述当前四元组位置矢量p
η
(t
i
)，即四元组位置递推公式表示为：其中，r
e
为卯酉圈半径，r
n
为子午圈半径；所述卯酉圈半径r
e
和所述子午圈半径r
n
表示为：其中，r0＝6378137为地球半径，e＝0.081819191为地球第一偏心率；根据所述当前四元组位置矢量p
η
(t
i
)和所述imu惯性测量单元中陀螺仪测量的实时旋转角速度矢量更新姿态，即获取所述当前姿态矩阵则姿态矩阵递推公式为：则姿态矩阵递推公式为：其中，i3为3阶单位矩阵，为陀螺仪实时获得的旋转角速度矢量，ε
u
(t
mk
)为第k次校正后得到的方位陀螺漂移，其初始值为0。7.根据权利要求6所述的惯性导航综合校正方法，其特征在于，在步骤s3中，根据外参考速度采用水平阻尼的递推公式得到当前时刻阻尼后的当前阻尼速度和当前时刻阻尼后的当前阻尼四元组位置矢量p
η_damp
(t
i
)，并计算法向量积分项int
η
(t
i
)的步骤中，所述水平阻尼的递推公式为：其中，v
h
(t
i
)为当前时刻的水平速度矢量；
其中，v
hr
(t
i
)为当前时刻的外参考水平速度矢量，v
h_damp
(t
i
)为当前时刻阻尼后的水平速度矢量；基于所述水平阻尼的递推公式获得所述当前阻尼速度其表示为：其中，η(t
i
)表示当前法向量位置矢量，v
h
(t
i
)表示当前垂直指示速度，num与dum均为相位滞后-超前串联校正网络的阻尼系数，即：num(1)＝1.0，num(2)＝-1.99997367841154，num(3)＝0.999975215058425dum(1)＝1.0，dum(2)＝-1.99875923183544，dum(3)＝0.998760768482328；基于所述水平阻尼的递推公式构建法向量位置的递推公式，其表示为：基于所述法向量位置的递推公式获得所述当前阻尼四元组位置矢量p
η_damp
(t
i
)，其表示为：其中，h(t
i
)为当前指示高度；所述法向量积分项int
η
(t
i
)的递推公式为：8.根据权利要求7所述的惯性导航综合校正方法，其特征在于，步骤s5中，所述综合校正公式为：其中，
则所述综合校正公式简化为：其中，β
ε
为马尔科夫过程相关时间常数。9.根据权利要求8所述的惯性导航综合校正方法，其特征在于，步骤s4和s5中，将输出的水平位置重调为外参考位置η
r
(t
mk
)的步骤中，用于水平位置重调的位置重调公式表示为：根据漂移角将姿态输出校正为的步骤中，姿态输出校正的计算公式表示为：其中，[
×
]表示对应的斜对称矩阵。10.根据权利要求9所述的惯性导航综合校正方法，其特征在于，步骤s5中，所述法向量位置误差矢量δη(t
mk
)表示为：其中，η(t
mk
)表示第k次校正时刻位置重调之前的法向量位置矢量。

技术总结
本发明涉及一种基于法向量位置模型的惯性导航综合校正方法，包括：接收实时旋转角速度矢量和比力矢量，装订初始导航参数，获取初始法向量位置矢量、初始四元组位置矢量、初始方向余弦矩阵，获取初始速度和初始姿态矩阵；获得当前速度、当前姿态矩阵、当前四元组位置矢量；获得当前阻尼速度和当前阻尼四元组位置矢量，计算法向量积分项；判断是否需要综合校正，若是，基于观测次数重调外参考位置；若观测次数>1，将水平位置重调为外参考位置，获得漂移角和方位陀螺漂移，根据漂移角校正姿态矩阵输出，而方位陀螺漂移将用于导航解算，记录当次成功观测的时间并将法向量积分项置零；重复前述步骤，直至导航结束。直至导航结束。直至导航结束。


技术研发人员：吴文启 刘潺 冯国虎 王茂松
受保护的技术使用者：中国人民解放军国防科技大学
技术研发日：2023.04.03
技术公布日：2023/8/24