一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法的制作方法

本发明属于极区导航技术领域，涉及一种适用于极区的捷联惯性导航系统(sins)阻尼算法，具体涉及一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法。

背景技术

由于极区特殊的地理环境，一般的导航方法如卫星导航、无线电导航、地磁导航都无法正常工作，捷联惯导系统以其具有自主性、连续性和全球性成为了极区导航的首选。随着纬度的升高，极区存在经线收敛过快和北向失去定义等问题，常用的指北方位惯导系统在极区无法正确解算出相关的导航信息。

为了解决这个问题，极区惯性导航系统，如游移方位惯导系统、自由方位惯导系统和横坐标系惯导系统等逐渐发展起来。其中游移方位惯导系统和自由方位惯导系统虽然能完成姿态和位置方向余弦矩阵计算，但是游移角和经度的提取仍然存在奇异值。横坐标系惯性导航系统通过重新定义横地球坐标和横地理坐标系，巧妙地解决了惯性导航系统在极区位置和姿态的解算问题。

捷联惯导系统在极区的惯导解算仍然包含三种周期性振荡：舒勒振荡、傅科振荡和地球振荡。对于长时间运行的载体而言，需要采用阻尼等方法来消除这些周期性振荡，以提高惯导解算精度。对于横坐标系捷联惯导系统，采用基于阻尼网络的全阻尼导航算法能够有效抑制舒勒振荡、傅科振荡和地球振荡。针对有外部信息定期进行校正的系统，采用基于阻尼网络的外水平阻尼算法来抑制舒勒周期振荡就能有效提高系统的导航精度。但是这种基于阻尼网络的算法一般存在两个主要问题：一是根据运动状态不同，阻尼网络参数难以确定，需要反复尝试；而是当外速度信息存在误差时，无阻尼切换到阻尼状态会打破系统平衡，带来较大的超调误差。

技术实现要素：

本发明的目的在于提供一种适用于极区的基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法。

本发明的目的是这样实现的：

本发明提出了一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法，通过以下步骤实现：

(1)建立横坐标系捷联惯导力学编排，获得横坐标系下的运动学方程：

其中，表示载体坐标系b到横导航坐标系的方向余弦矩阵；为载体的对地速度在横导航系下的投影；为载体在横坐标系下的位置矢量；为地球自转角速度在横导航系下的投影；为横导航系相对于横地球坐标系e的旋转角速率在横导航系下的投影；g为当地重力加速度大小；r为地球半径；rc为位置方向余弦矩阵；为捷联惯导系统陀螺仪输出；f^b为加速度计输出；

(2)根据运动学方程，针对舰船使用的横坐标系捷联惯导系统，建立横坐标系捷联惯导系统的状态方程和量测方程：

z＝hx+η

其中a为状态转移矩阵；状态变量为为系统噪声；b为系统噪声驱动阵；h为观测阵；η为量测噪声；

(3)建立多普勒计程仪速度信息有效性判据，判断外速度信息有效性是否发生改变；其中，多普勒计程仪为dopplervelocitylog，dvl；

(4)若步骤(3)判断外速度信息从有效变为无效，执行步骤(5)；

若步骤(3)判断外速度信息从无效变为有效，执行步骤(6)；

若步骤(3)判断外速度信息有效性状态不变，则系统继续执行当前状态；

(5)系统从阻尼切换至无阻尼状态，直接执行纯惯导下的姿态、速度和位置的解算；

(6)系统从无阻尼切换至阻尼状态，通过运行卡尔曼滤波器估计出水平姿态误差角，待滤波器稳定后，将估计的水平姿态误差角进行反馈校正，从而实现外水平阻尼。

对于一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法，所述的步骤(1)中

根据船舶的运动特性，忽略天向通道的影响，设h＝0，vz＝0；通过获得捷联惯导系统陀螺仪输出和加速度计输出f^b，完成横坐标系下捷联惯导系统的姿态、速度、位置的解算。

对于一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法，步骤(2)主要通过以下步骤实现：

(2.1)对捷联惯导系统中陀螺仪和加速度计的标度因数误差和安装误差进行补偿，针对舰船使用的横坐标系捷联惯导系统建立系统的误差方程：

其中：

为姿态误差角；由于舰船不考虑天向通道，速度误差表示为和分别为横纬度和横经度误差；陀螺漂移误差包括随机常值漂移ε^b和零均值的高斯白噪声εw两部分；加速度计误差δf^b包括随机常值零偏和零均值的高斯白噪声

(2.2)获取dvl提供的有效外速度信息，根据舰船的运动特性，忽略天向通道ozt，建立横坐标系下捷联惯导系统的状态方程和量测方程：

z＝hx+η

其中，a、b、h可以分别表示为：

其中f7×7通过上面给出的捷联惯导系统误差方程获得，上述子矩阵中(·)m,n表示对矩阵(·)取前m行n列。

对于一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法，所述的步骤(3)通过以下步骤实现：

(3.1)将dvl提供的外速度作为观测量，建立量测模型；

(3.2)判断dvl提供的外速度的信息有效性；收集横向捷联惯导解算的速度和计程仪提供的速度：

vdvl＝vtrue+dvdvl

其中vtrue为舰船的真实速度，和dvdvl分别为横坐标系捷联惯导解算的速度和外参考速度vdvl的速度误差；

(3.3)取t1时刻和邻近t2时刻的滤波器观测值：

其中，和分别为t1和t2时刻滤波器观测值；

对t1和t2时刻的观测值做差：

其中，由两个时刻横坐标系捷联惯导速度误差和两个时刻的计程仪速度误差组成；

(3.4)根据的变化趋势判断dvl提供的外速度的有效性；

当发生大的改变时，判定dvl的速度误差发生较大变化，即dvl提供的外速度无效，系统进入无阻尼状态；

若由剧烈变化的向量向小幅值向量转变，则判定dvl提供的外速度有效，则系统由无阻尼状态进入阻尼状态。

对于一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法，所述的步骤(6)通过以下步骤实现：

(6.1)对步骤(2)中建立的系统方程和量测方程进行离散化，通过离散卡尔曼滤波对系统误差进行估计；

(6.2)不断校正水平误差角，消除速度、位置、姿态中的舒勒振荡，实现水平阻尼。

本发明的有益效果在于：

本发明公开的一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法，由于水平姿态误差角可观测性好，能够用卡尔曼滤波估计出来，只要不断地校正水平误差角就可以从根源上消除速度、位置、姿态中的舒勒振荡，实现横坐标系捷联惯导系统外水平阻尼；

此外，当横坐标系捷联惯导系统从无阻尼切换到阻尼状态时，基于阻尼网络的传统阻尼算法产生较大的超调误差与dvl提供的外速度误差紧密相关。因此，当采用低精度的dvl时，该算法很难从原理上减小超调误差。与基于阻尼网络的阻尼算法相比，当采用低精度的dvl时，由于水平姿态误差角的可观测性较好，通过建立的卡尔曼滤波误差模型仍然可以估计出水平姿态误差角。待卡尔曼滤波稳定后完成反馈校正，即可一定程度上减小阻尼切换过程中的超调误差。

附图说明

图1为本发明中横坐标系的定义图；

图2为本发明中横坐标系捷联惯导系统卡尔曼滤波外水平阻尼原理框图；

图3为本发明中横东向外部参考速度误差；

图4为本发明中横坐标系下姿态误差曲线；

图5为本发明中横坐标系下速度误差曲线；

图6为本发明中横坐标系下位置误差曲线；

图7为本发明中仿真初始参数指标表。

具体实施方式

下面结合附图对本发明做进一步描述。

结合图1至图7，本发明公开的一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法，在本发明中，首先需要对多普勒计程仪提供的外速度信息进行有效性判断；然后根据外度度信息的可靠性决定能否从无阻尼切换至阻尼状态，直到外速度信有效时，横坐标系捷联惯导系统切换至卡尔曼滤波阻尼状态；最后待滤波器稳定后，将估计出的姿态误差角进行不断地反馈校正，实现卡尔曼滤波外水平阻尼。

图2为横坐标系捷联惯导系统的卡尔曼滤波阻尼原理框图。通常情况下，计程仪提供的外速度误差都是存在阶段性的常值误差的，使得估计出来速度误差与外速度误差耦合在一起，难以准确估计出速度误差，但是水平姿态误差角的可观测性很好，仍然准确地估计出来，因此可以通过估计的水平姿态误差角来校正系统的平台误差角。由于存在水平姿态误差角，使得水平方向引入重力加速度分量，构成二阶负反馈系统，从而表现为舒勒振荡，因此只要不断地校正水平误差角就可以从根源上消除速度、位置、姿态中的舒勒振荡，实现水平阻尼。本发明的具体实施步骤如下：

步骤一：建立横坐标系捷联惯导力学编排，获得横坐标系下的运动学方程：

根据船舶的运动特性，可以忽略天向通道的影响，即可以设h＝0，vz＝0。通过获得捷联惯导系统陀螺仪输出和加速度计输出f^b，就可以完成横坐标系下捷联惯导系统的姿态、速度、位置的解算。

步骤二：根据运动学方程，针对舰船使用的横坐标系捷联惯导系统，建立横坐标系捷联惯导系统的状态方程和量测方程：

z＝hx+η

其中a为状态转移矩阵；状态变量为为系统噪声；b为系统噪声驱动阵；h为观测阵；η为量测噪声。a、b、h可以分别表示为：

其中f7×7可以通过上面给出的捷联惯导系统误差方程获得，上述子矩阵中(·)m,n表示对矩阵(·)取前m行n列。

步骤三：建立dvl外速度信息有效性判据，判断外速度信息有效性是否发生改变；

则是由两个时刻横坐标系捷联惯导速度误差和两个时刻的计程仪速度误差组成，由于惯导系统的信息具有连续性的特点，在短时间内不会发生大的变化。但是当发生大的改变，可认为是dvl的速度误差发生大的变化，即dvl提供的外速度无效，则系统应该进入无阻尼状态。若由剧烈变化的向量向小幅值向量转变，即dvl提供的外速度有效，则系统由无阻尼状态向阻尼状态切换。

步骤四：当步骤三判断的外速度信息从有效变为无效时，执行步骤五，当步骤三判断外速度信息从无效变为有效时，执行步骤六，当步骤三中判断的外速度信息有效性状态不变时，系统继续执行当前状态；

步骤五：系统从阻尼切换至无阻尼状态，则根据步骤一中的运动方程直接执行纯惯导下的姿态、速度和位置的解算；

步骤六：系统从无阻尼切换至阻尼状态，通过运行卡尔曼滤波器估计出水平姿态误差角，待滤波器稳定后，将估计的水平姿态误差角进行反馈校正，从而实现外水平阻尼。以横东向水平回路为例，如图2所示。将卡尔曼滤波估计出来的东向姿态误差角反馈到系统，不断校正捷联惯导系统的水平姿态误差角，即可实现基于卡尔曼滤波的外水平阻尼，从而抑制舒勒振荡和被傅科周期振荡调制的舒勒振荡。

本发明还包括这样一些结构特征：

1.步骤一具体为：

横坐标系包括横地球坐标系和横地理坐标系它的定义与传统导航坐标系类似。以180°经线与赤道平面的交点为横北极点，以0°经线与赤道平面的交点为横南极点。如图1所示，横地球坐标系可以等效于将原地球坐标系oexeyeze经过两次旋转得到，其转动过程为：

将转动后得到的平面定义为横本初子午面，平面定义为横赤道平面。载体p点的位置可以由横经度横纬度和相对于水平面的高度h来描述，p点的横经度为经过p点的横子午面与横本初子午面之间的夹角，横纬度为p点和地心连线与横赤道平面之间的夹角。如图1所示，横地理坐标系以载体p的质心o为原点，为当地水平面，沿当地横经线方向指向横北极点，指向天向，轴满足右手定则。

本发明以横地理坐标系为导航坐标系，它的力学编排和误差方程与指北方位惯导系统相类似，主要区别是地球自转角度ωie在各自的导航系下的投影分量不同，因此可以获得载体在横坐标系下捷联惯导系统的运动学方程：

其中：

本发明中表示载体坐标系b到横导航坐标系的方向余弦矩阵；为载体的对地速度在横导航系下的投影；为载体在横坐标系下的位置矢量；为地球自转角速度在横导航系下的投影；为横导航系相对于横地球坐标系的旋转角速率在横导航系下的投影；g为当地重力加速度大小；r为地球半径；rc为位置方向余弦矩阵。根据船舶的运动特性，可以忽略天向通道的影响，即可以设h＝0，vz＝0。通过获得捷联惯导系统陀螺仪输出和加速度计输出f^b，就可以完成横坐标系下捷联惯导系统的姿态、速度、位置的解算。

2.步骤二具体为：

假设捷联惯导系统中陀螺仪和加速度计的标度因数误差和安装误差已经补偿，针对舰船使用的横坐标系捷联惯导系统可以建立系统的误差方程为：

其中：

为姿态误差角；由于舰船不考虑天向通道，速度误差可以表示为和分别表示横纬度和横经度误差；陀螺漂移误差由随机常值漂移ε^b和零均值的高斯白噪声εw两部分组成；加速度计误差δf^b由随机常值零偏和零均值的高斯白噪声组成。

当dvl提供外速度信息有效时，本发明针对舰船的运动特性，忽略天向通道建立横坐标系下捷联惯导系统的状态方程和量测方程为：

z＝hx+η

其中a为状态转移矩阵；状态变量为为系统噪声；b为系统噪声驱动阵；h为观测阵；η为量测噪声。a、b、h可以分别表示为：

其中f7×7可以通过上面给出的捷联惯导系统误差方程获得，上述子矩阵中(·)m,n表示对矩阵(·)取前m行n列。

3.步骤三具体为：

本发明将dvl提供的外速度作为观测量，建立了量测模型。当船舶做复杂的机动运动或当船舶航行到海湾或海沟等复杂的海洋环境当中时，dvl的测速误差通常很大，此时认为计程仪测得的速度不可用。对于长时间航行的舰船捷联惯导系统，通常工作在无阻尼和阻尼两种状态，并根据dvl提供的外速度的是否有效进行状态切换。因此有必要建立dvl外速度信息有效性判据，横向捷联惯导解算的速度和计程仪提供的速度可以表示为：

vdvl＝vtrue+dvdvl

其中vtrue为舰船的真实速度，和dvdvl分别为横坐标系捷联惯导解算的速度和外参考速度vdvl的速度误差。取t1时刻和邻近t2时刻的滤波器观测值如下：

上式中和分别为t1和t2时刻滤波器观测值。对t1和t2时刻的观测值做差得到：

则是由两个时刻横坐标系捷联惯导速度误差和两个时刻的计程仪速度误差组成，由于惯导系统的信息具有连续性的特点，在短时间内不会发生大的变化。但是当发生大的改变，可认为是dvl的速度误差发生大的变化，即dvl提供的外速度无效，则系统应该进入无阻尼状态。若由剧烈变化的向量向小幅值向量转变，即dvl提供的外速度有效，则系统由无阻尼状态向阻尼状态切换。

4.步骤六具体为：

将步骤二中建立的系统方程和量测方程进行离散化，可以通过离散卡尔曼滤波对系统误差进行估计。由于存在水平姿态误差角，使得水平方向引入重力加速度分量，构成二阶负反馈系统，从而表现为舒勒振荡，因此只要不断地校正水平误差角就可以从根源上消除速度、位置、姿态中的舒勒振荡，实现水平阻尼。以横东向水平回路为例，如图2所示。将卡尔曼滤波估计出来的东向姿态误差角反馈到系统，不断校正捷联惯导系统的水平姿态误差角，即可实现基于卡尔曼滤波的外水平阻尼，从而抑制舒勒振荡和被傅科周期振荡调制的舒勒振荡。

本实施例的实现条件与所得结果如下：

(1)实验仿真条件

为了验证所设计的横坐标系下捷联惯导系统的卡尔曼滤波阻尼算法性能，分别进行了10小时的无阻尼和有阻尼仿真测试。仿真的初始参数设置如图7所示。横东向参考速度误差如图3所示，当t＜2.5h时，速度随机误差较大，dvl提供的外速度信息视为无效，当t≥2.5h时，dvl提供的外速度误差相对较小，可以作为外速度参考。当dvl测得的外速度信息可用时，系统由无阻尼状态切换到阻尼状态，仿真中在t＝2.5h时，则可以进行阻尼切换。将横坐标系下捷联惯导系统在无阻尼、基于阻尼网络的传统阻尼和卡尔曼滤波阻尼下的导航误差进行对比，仿真结果如图4-6所示。

(2)实验仿真结果

当t＝2.5h时，dvl提供的外速度信息有效，基于阻尼网络的阻尼算法直接由无阻尼状态切换至阻尼状态。卡尔曼滤波阻尼算法进入切换准备状态，待滤波器稳定后(约10min左右)，将估计的水平姿态误差角不断地进行反馈校正，此时基于卡尔曼滤波的外水平阻尼算法正式进入阻尼状态。从图4看出，两种阻尼方法均能消除水平姿态角误差的舒勒周期振荡，只剩下由加速度计零偏引起的常值误差，而方位姿态角误差只存在地球周期振荡，因此两种阻尼算法的阻尼效果一样。从图5和图6看出，两种阻尼算法均能有效消除速度误差和位置误差中的舒勒振荡以及被傅科周期调制的舒勒振荡，但是地球周期振荡仍然存在。如图4-6所示，传统阻尼虽然能够对舒勒振荡进行阻尼，但在阻尼切换后存在较大的超调误差。采用卡尔曼滤波阻尼算法能够较平滑地从无阻尼切换到阻尼状态，实现水平阻尼。从仿真结果可以得出如下结论：(1)在高纬度地区的横坐标系捷联惯导系统中，基于阻尼网络的阻尼算法和本文设计的卡尔曼滤波阻尼算法均能够有效抑制舒勒周期振荡；(2)与基于阻尼网络的阻尼算法相比，所设计的卡尔曼滤波阻尼算法能够有效抑制从无阻尼切换到阻尼状态产生的超调误差。

综上，本发明公开了一种适用极区的横坐标系捷联惯导系统卡尔曼滤波阻尼算法。在该算法中，通过建立横坐标系下卡尔曼滤波误差模型，对水平姿态误差角进行估计，并利用反馈校正不断校正水平姿态误差角，从而抑制姿态、速度和位置误差中的舒勒振荡，实现卡尔曼滤波外水平阻尼。首先根据外速度有效性判据判断外速度信息的可靠性；然后根据外速度信息的判断结果确定当前捷联惯导系统的运行状态，直到外速度信息有效时，系统从无阻尼切换至阻尼状态；最后待卡尔曼滤波稳定后，将估计的水平姿态误差角不断地进行反馈校正，从而抑制姿态、位置和速度误差中的舒勒振荡，实现极区卡尔曼滤波外水平阻尼。与基于阻尼网络的传统阻尼方法相比，本发明提供的阻尼算法不仅能实现极区外水平阻尼，还能抑制阻尼切换过程产生的超调误差。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。