[发明专利]一种基于输出校正的容错水平阻尼方法

权利要求书

1.一种基于输出校正的容错水平阻尼方法，其特征在于，包括：

第1步：装订初始参数，包括装订无阻尼的初始导航参数；

第2步：纯惯导解算当前时刻无阻尼导航参数；

第3步：获取当前时刻外参考速度、无阻尼速度与上一时刻的外参考速度、无阻尼速度，判断是否符合判据并且，其中为预设的判据阈值；当前外参考速度不符合判据时，则将当前时刻的无阻尼导航参数赋值给当前时刻的阻尼后的导航参数并输出，然后跳至第2步直至导航解算结束；当当前时刻的外参考速度符合判据时，而上一时刻的数据不符合判据，进行水平速度阻尼，记录下当前时刻T_i为阻尼起始时间即T_D=T_i，得到当前时刻的阻尼后速度的方法是：

通过当前时刻的外参考速度、无阻尼速度，以及上两个时刻的外参考速度、，无阻尼速度、和阻尼后速度、，采用公式

得到当前时刻的阻尼后速度，其中num(1)、num(2)、num(3)、dum(1)、dum(2)、dum(3)为基于输出校正水平阻尼传递函数离散时间表达式系数，num(1)=1.0，num(2)=-1.99997367841154，num(3)=0.999975215058425，dum(1)=1.0，dum(2)=-1.998759231835443，dum(3)=0.998760768482328；

第4步：通过当前时刻的阻尼后速度采用位置增量方式解算当前时刻的阻尼后的位置；

第5步：通过当前时刻的阻尼后的位置解算阻尼后的姿态矩阵；

第6步：输出当前时刻的阻尼后速度、位置和姿态矩阵，跳至第2步直至导航解算结束。

2.根据权利要求1所述的基于输出校正的容错水平阻尼方法，其特征在于：第1步中初始导航参数包含东北天三个方向的初始速度

，还包含经度、纬度的初始位置和初始姿态矩阵。

3.根据权利要求1所述的基于输出校正的容错水平阻尼方法，其特征在于：第2步中，根据IMU惯性测量单元实时提供的旋转角速度、比力矢量和上一时刻的导航参数、、，按纯惯导算法解算得到当前时刻的无阻尼导航参数、、。

4.根据权利要求1、2或3所述的基于输出校正的容错水平阻尼方法，其特征在于：第3步中，取值区间为。

5.根据权利要求4所述的基于输出校正的容错水平阻尼方法，其特征在于：第3步中，通过水速计获取当前时刻外参考速度、无阻尼速度与上一时刻的外参考速度、无阻尼速度。

6.根据权利要求4所述的基于输出校正的容错水平阻尼方法，其特征在于：第4步中，判断速度连续阻尼的时间是否超过小时，即如果，则；如果，则根据当前时刻无阻尼的位置、阻尼后的速度和外参考速度以及上一时刻无阻尼的位置，阻尼后的位置、速度和外参考速度，采用位置增量计算公式

得到当前时刻的阻尼后位置，其中R_N和R_E分别是子午面曲率半径和横向曲率半径，h是海拔高度；、分别是时刻阻尼后的纬度、经度，、分别是时刻阻尼后的纬度、经度，、分别是时刻无阻尼的纬度、经度，，分别是时刻无阻尼的纬度、经度，、分别是时刻阻尼后惯导导航解算的北向、东向速度，、分别是时刻外参考速度在北向、东向上的分速度，、分别是时刻阻尼后惯导导航解算的北向、东向速度，、分别是时刻外参考速度在北向、东向上的分速度。

7.根据权利要求6所述的基于输出校正的容错水平阻尼方法，其特征在于：第4步中，取1.5。

8.根据权利要求1所述的基于输出校正的容错水平阻尼方法，其特征在于：第5步中，根据当前时刻无阻尼的位置计算出地球系至无阻尼导航系转移矩阵，根据当前时刻的阻尼后位置计算出地球系至阻尼导航系转移矩阵，阻尼后的姿态矩阵由公式计算得到。

9.根据权利要求8所述的基于输出校正的容错水平阻尼方法，其特征在于：第5步中，地球系至无阻尼导航系转移矩阵为：

；

地球系至阻尼导航系转移矩阵为：

。