CN106482749B - 基于逆向导航算法的捷联惯导与转速计组合对准方法 - Google Patents

Abstract

本发明涉及一种基于逆向导航算法的捷联惯导与转速计组合的对准方法。对准技术是自主水下航行器(AUV)编队航行任务中的重要议题，同时快速性与精确性也是当今作战的主要任务需要。针对该问题，本发明提出了基于逆向导航算法的捷联惯导与转速计组合对准的研究方法。该方法将转速计测得的AUV的轴向速度与捷联惯导测得的导航信息进行有效结合。通过对转速计和捷联惯导系统的速度输出进行解算，减少二者的速度增量差；同时对对准阶段的采样数据进行反演解算，增大对信息量的利用，进而快速获得高精度的对准结果。

Description

基于逆向导航算法的捷联惯导与转速计组合对准方法

技术领域

本发明属于惯导对准技术的方法，涉及一种基于逆向导航算法的捷联惯导与转速计组合对准方法的研究。

背景技术

自主水下航行器((Autonomous Underwater Vehicle，简称AUV)是执行海洋军事任务和进行海洋开发的重要工具。当AUV在进行任务前，惯导需要对系统进行初始对准，其中快速性和精确性是对系统要求的重要指标，一般来说二者的关系是互相矛盾、相互制约的。不同于静基座对准，行进间对准需要外部的辅助设备提供载体的运动信息，通过辅助设备量测的信息对惯导系统进行修正补偿。在非常时期航行器必须有快速机动，并精确导航，兼具水下隐蔽性等要求。因此解决对准的精确性和快速性问题成为AUV航行任务中的重要议题。然而传统的对准技术需要利用岸上平台进行传递对准，或利用价值昂贵的辅助设备进行组合对准，这给AUV在快速投入作战的需要和实际工程开发应用中带来巨大挑战，因此，必须对其进行适当改善，在解决此类问题的同时，适应时局的发展。

在水下领域，作为比较成熟的组合导航方式，SINS/DVL(Strap-down InertialNavigation System,捷联惯性导航系统)组合导航系统利用DVL(Doppler Velocity Log,多普勒计程仪)提供的速度信息修正SINS的量测信息，以此抑制捷联惯导系统的误差积累，是目前应用比较广泛的水下组合导航技术。然而，由于DVL价值昂贵，并且会在工作时会向外发射声波，暴露自身的位置，所以不能很好的满足隐蔽性的要求。同时，DVL的有效测速度范围为±10m/s，当航行器处于相对速度较高的状态下，DVL亦不能保证有效的测量精度，因此限制了该方式的应用。

转速计凭借其低廉的价格并且能够测得在高速运动下航行器轴向速度的特点，可以有效解决上述的问题。通过选取转速计作为捷联惯导系统的辅助设备，测量载体在高速运动下的速度信息，利用量测的速度信息修正惯导系统，并借助正向和逆向结合的导航算法对获取的数据信息反复处理，以加大对数据的利用，进而满足系统的对准精度和快速性的要求。

因此，本文将逆向导航算法引入捷联惯导与转速计组合对准方法。通过将捷联惯导与转速计的速度信息进行信息融合，利用正逆向导航算法交替处理，在增大对获取的数据信息的同时，有效提高系统的对准精度，缩短系统的对准时间。

发明内容

为了避免现阶段对准技术的不足之处，本发明提出了捷联惯导与转速计组合对准的方法，并引入逆向导航算法，通过获取捷联惯导与转速计的采样数据，并将二者加以滤波融合，然后利用正逆向导航算法，对采样阶段的信息进行多次反复处理，得到更为精确的对准信息。这种方法在提高系统对准精确度的同时，还能有效缩短系统在对准过程的时间，进而提升系统的整体性能。

一种基于逆向导航算法的捷联惯导与转速计组合的对准方法，其特征在于步骤如下：

步骤1：建立捷联惯导与转速计对准模型：

转态方程：

观测方程：Z_ST＝H_STX_ST+v_ST

上式中

其中，F_ST为系统状态矩阵，H_ST为系统量测矩阵，系统噪声w_ST和量测噪声μ_ST统为零均值高斯白噪声。V_N、V_U、V_E为载体在北天东方向上的速度，λ、L和h分别表示载体的经纬度与高度，且高度信息可以通过深度传感器直接测量得到。

建立正向导航算法微分方程，对其进行离散化：

姿态方程：

速度方程：

位置方程：

R_M,R_N分别为载体所在位置地球子午圈和卯酉圈曲率半径，其近似计算公式为：

R_M≈R_e(1-2e+3esin²L)

R_N≈R_e(1+esin²L)

步骤2：卡尔曼滤波初始化。

一：首先，对捷联惯导与转速计组合系统的状态变量初值进行初始化，确定其在误差干扰下的状态初值x0＝[phi；dvn；dpos；ed；db]，其中平台失准角误差phi＝[10；60；-10]*(pi/180/60)，位置误差dpos＝[10/6378160；10/6378160]、速度误差dvn＝[0.5；0.5；0.5]，陀螺随机漂移误差eb＝[0.02；0.02；0.02]*(pi/180/3600)与加速度计零位误差db＝[100；100；100]*(0.000001*9.78)，估计陀螺测量白噪声web＝[0.02；0.02；0.02]*(pi/180/3600)，加速度计的测量白噪声wdb＝[50；50；50]*(0.000001*9.78)，

设webq＝[web(1,1)^2；web(2,1)^2；web(3,1)^2]，

wdbq＝[wdb(1,1)^2；wdb(2,1)^2；wdb(3,1)^2]。

可得系统方程中白噪声均方差为w＝[web；wdb；webq；wdbq]，其中观测方程中捷联惯导SINS与转速计的量测量之间的白噪声均方差为v＝[0.01；0.01；0.01]。

然后，对状态初值及相应误差函数分别赋值给相应参数。phi＝x0(1:3)；dvn＝x0(4:6)；Qtd＝w(7:12)；Qt_d＝diag(Qtd)；Rk_d＝diag(v)^2；Pkd＝x0(1:14)；Pk_d＝10*diag(Pkd)^2；Xk_d＝zeros(14,1)；eb＝x0(9:11)；db＝x0(12:14)；web＝w(1:3)；wdb＝x0(4:6)。

其中，Qt_d、Rk_d和Pk_d分别为系统噪声协方差驱动矩阵、量测噪声协方差阵和一步预测均方差阵。

二：对系统的姿态角、速度与位置进行初始化。载体初始姿态角att＝[0；0；0]*(pi/180)；载体初始速度vb＝[8；0；0]；载体的初始位置pos＝[24*(pi/180)+35*(pi/180/60)；120*(pi/180)+58*(pi/180/60)；-10]。

然后对载体的轨迹进行设置后保存到tr(1:15)函数，该函数保存的是不含误差的真实值，包括载体的三个姿态角分量(横滚角、航向角及俯仰角)、三个速度分量(北向、天向及东向)、三个位置分量(经纬度及高度)、三个角速率分量个三个比力分量。

三：捷联惯导解算参数初始化。将载体真实的姿态角矩阵转换为四元数形式，然后加入平台误差角，即qnb＝tr(1:3)+phi，且系统的状态变量初始值Xk_d＝zeros(14,0)；将载体真实的速度信息加入速度误差，即vnm＝tr(4:6)+dvn；将载体真实的位置信息加入位置误差，即posm＝tr(7:9)+[x0(7)；x0(8)；0.1]；最后，将载体输出的陀螺仪与加速度计信息保存到wvm2函数，即wvm2＝tr(10:15)。

步骤3：步骤2完成后，对载体的姿态、速度与位置信息进行组合滤波。首先对误差估计函数进行初始化置0，inse(1,:)＝zeros(14,1)；然后将载体真实的姿态信息tr(1:3)与加入误差的实际值qnb中提取姿态误差信息，具体步骤为：先将qnb转化为共轭形式，再与姿态阵tr(1:3)的四元数形式做差乘得到姿态误差信息；其次，将载体的实际速度值与真实值做差，求得速度误差信息，即vnm-tr(4:6)；最后将载体实际的位置信息与真实值做差，求得位置误差值，即posm(1)-tr(7),posm(2)-tr(8)。

步骤4：设置时间循环(for k＝2:2:s_time*100)读取tr函数的角增量与比力增量，并对角增量与比力增量进行更新，对更新的角增量与比力增量信息保存到扩维后的tr函数中。即wvm0＝wvm2,wvm1＝tr(10:15),wvm2＝tr((15+10):(15+15))。其中，s_time为系统仿真时间。

获取角增量与比力增量，加入误差，并保存：

dwvm1＝wvm1-wvm0+([eb；db]+[web；wdb].randn(6,1))*Tm/2

dwvm2＝wvm2-wvm1+([eb；db]+[web；wdb].randn(6,1))*Tm/2

其中，Tm＝0.02为捷联惯导解算周期。

步骤5：以获取角增量与比力增量对陀螺仪增量与加速度计增量扩维，进行二子样解算，即

wm＝[dwvm1(1:3,:),dwvm2(1:3,:)]

vm＝[dwvm1(4:6,:),dwvm2(4:6,:)]

对载体的深度用深度传感器测量得到，加入相应误差，即posm(3,1)＝tr(9)+0.05*randn(1)

步骤6:将步骤5得到含有误差的实际增量值进行惯导解算，根据上一时刻的姿态四元素、速度、位置、陀螺仪与加速度计的测量值，计算当前时刻的姿态四元数、速度与位置。

一：计算地球自转角速度引起的陀螺感应值

wnie＝wie*[cos(posm_1(1))；sin(posm_1(1))；0]，其中wie＝0.0000729215为地球自转角速率；posm_1为前一时刻载体的位置信息。

二：计算载体在导航系下由北向与东向速度引起的陀螺感应值

wnen＝[vnm_1(3)/(Re*(1+e*sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))；

vnm_1(3)*(sin(posm_1(1))/cos(posm_1(1)))/(Re*(1+e*sin(posm_1(1))*

sin(posm_1(1)))+posm_1(3))；-vnm_1(1)/(Re*(1-2*e+3*e*

sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))]

其中，posm_1(3)为前一时刻载体的高度信息；Re＝6378160为地球半径；e为地球椭圆率；vnm_1(3)为载体东向航行速度。

三：由上述两个步骤，可得由载体运动速度和地球自转引起的导航系下的陀螺角速率wnin＝wnie+wnen。

四：采用二子样方法，计算陀螺仪与加速度计的输出增量

sumw＝wm(:,1)+wm(:,2)

sumv＝vm(:,1)+vm(:,2)

对sumw进行保存，即phim＝sumw。

五：计算二子样等效旋转矢量，更新phim。即：

phim＝phim+2/3*cross(wm(:,1),wm(:,2))

六：计算划桨效应补偿dvsculm与速度旋转效应补偿dvrotm。

dvsculm＝2/3*(cross(wm(:,1),vm(:,2))+cross(vm(:,1),wm(:,2)))

dvrotm＝1/2*cross(sumw,sumv)

七：根据上式对速度进行更新。

vnm＝vnm_1+Cnn*Cnb*(dvm+dvrotm+dvsculm)+gn-cross(2*wnie+wnen,vnm_1))*Tm

其中，gn重加速度；Cnn为在导航系下载体的陀螺角速率由旋转矢量转化为矩阵形式；Cnb为载体的姿态矩阵。

步骤7:求解步骤1的系统的状态矩阵Ft_d与观测矩阵Hk_d，并进行离散化，结果分别保存到Fk_d与Qk_d。

步骤8：模拟转速计的测量值V_Tacho＝(Att2Mat(tr(1:3)))'*tr(4:6)+0.01*randn(3,1)，该测量值为载体坐标系下的测量值，同时将该测量值转化到导航系下，并将结果保存至V_nTacho。其中Att2Mat(tr(1:3))表示将载体真实的姿态角转换成矩阵形式。

步骤9：由步骤6和步骤7的结果计算系统测量值Zk_d＝vnm-V_nTacho。

步骤10：采用卡尔曼滤波对状态变量Xk_d进行滤波更新。

卡尔曼增益：K_k＝P_{k_d}H_k_d'*inv(H_{k_d}P_{k_d}H_{k_d}'+R_{k_d})

误差协方差阵：

P_{k_d}＝(eye(length(Xk_d))-Kk*Hk_d)*Pk_d*(eye(length(Xk_d))-Kk*Hk_d)'+Kk*Rk_d*Kk'

其中，Xk_d＝Fk_d*Xk_d；Pk_d＝Fk_d*Pk_d*Fk_d'+Qk_d。

状态更新：Xk_d＝Xk_d+Kk*(Zk_d-Hk_d*Xk_d)

步骤11：反馈修正。

一：分别对捷联惯导的姿态、速度与位置进行修正。即

qnb＝qnb-Xk_d(1:3)

vnm＝vnm-Xk_d(4:6)

posm(1)＝posm(1)-Xk_d(7)

posm(2)＝posm(2)-Xk_d(8)

二：反馈校正后的状态变量，得到Xk_d＝[zeros(8,1)；Xk_d(9:14)]。

步骤12：调用save函数，对系统状态变量及导航信息进行保存，包括Pk_d、Xk_d、Kk、qnb、vnm与posm。

步骤13：建立系统逆向导航算法。

一：首先调用load函数，加载步骤12保存的所有数据，并修改步骤4的时间循环顺序为逆序(for k＝s_time*100:(-2):2)。

二：然后对步骤6中的陀螺仪输出取反，有wb_nb＝-(phim-Cnb'*wnin*Tm)。

三：最后对对步骤6中的速度取反，有

vnm＝vnm_1-Cnn*Cnb*(dvm+dvrotm+dvsculm)-(gn-cross(2*wnie+wnen,vnm_1))*Tm

四：数据保存。调用save函数，对步骤12中的信息进行保存。

步骤1-步骤13为系统一次完整的正向和逆向处理过程，当需要对系统进行多次正逆向处理时，只需对导航信息进行反复保存及加载，并修改系统仿真时间，参考上述方法，即可完成系统不同次数、不同时间的正逆向处理。

本发明提出的一种基于逆向导航算法的捷联惯导与转速计组合的对准方法。对准技术是自主水下航行器(AUV)编队航行任务中的重要议题，同时快速性与精确性也是当今作战的主要任务需要。针对该问题，本发明提出了基于逆向导航算法的捷联惯导与转速计组合对准的研究方法。该方法将转速计测得的AUV的轴向速度与捷联惯导测得的导航信息进行有效结合。通过对转速计和捷联惯导系统的速度输出进行解算，减少二者的速度增量差；同时对对准阶段的采样数据进行反演解算，增大对信息量的利用，进而快速获得高精度的对准结果。

仿真实验结果表明，在捷联惯导与转速计组合的方式下，该方法可以实现快速对准，同时满足对准精度的需要，故所提方法在理论和工程实践中具有较好的指导价值和应用价值。

附图说明

图1捷联惯导与转速计组合对准方法流程图；

图2前30秒采样数据的常规导航解算过程；

图3前200秒采样数据的常规导航解算过程；

图4前300秒采样数据的常规导航解算过程；

图5前200秒采样数据正逆向交替解算5次；

图6前300秒采样数据正逆向交替解算5次。

图7前200秒采样数据正逆向交替解算10次。

图8前300秒采样数据正逆向交替解算10次。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

步骤1：建立捷联惯导与转速计对准模型：

转态方程：

观测方程：Z_ST＝H_STX_ST+v_ST

上式中

其中，F_ST为系统状态矩阵，H_ST为系统量测矩阵，系统噪声w_ST和量测噪声μ_ST统为零均值高斯白噪声。V_N、V_U、V_E为载体在北天东方向上的速度，λ、L和h分别表示载体的经纬度与高度，且高度信息可以通过深度传感器直接测量得到。

建立正向导航算法微分方程，对其进行离散化：

姿态方程：

速度方程：

位置方程：

R_M,R_N分别为载体所在位置地球子午圈和卯酉圈曲率半径，其近似计算公式为：

R_M≈R_e(1-2e+3esin²L)

R_N≈R_e(1+esin²L)

步骤2：卡尔曼滤波初始化。

一：首先，对捷联惯导与转速计组合系统的状态变量初值进行初始化，确定其在误差干扰下的状态初值x0＝[phi；dvn；dpos；ed；db]，其中平台失准角误差phi＝[10；60；-10]*(pi/180/60)，位置误差dpos＝[10/6378160；10/6378160]、速度误差dvn＝[0.5；0.5；0.5]，陀螺随机漂移误差eb＝[0.02；0.02；0.02]*(pi/180/3600)与加速度计零位误差db＝[100；100；100]*(0.000001*9.78)，估计陀螺测量白噪声web＝[0.02；0.02；0.02]*(pi/180/3600)，加速度计的测量白噪声wdb＝[50；50；50]*(0.000001*9.78)，

设webq＝[web(1,1)^2；web(2,1)^2；web(3,1)^2]，

wdbq＝[wdb(1,1)^2；wdb(2,1)^2；wdb(3,1)^2]。

可得系统方程中白噪声均方差为w＝[web；wdb；webq；wdbq]，其中观测方程中捷联惯导SINS与转速计的量测量之间的白噪声均方差为v＝[0.01；0.01；0.01]。

然后，对状态初值及相应误差函数分别赋值给相应参数。phi＝x0(1:3)；dvn＝x0(4:6)；Qtd＝w(7:12)；Qt_d＝diag(Qtd)；Rk_d＝diag(v)^2；Pkd＝x0(1:14)；Pk_d＝10*diag(Pkd)^2；Xk_d＝zeros(14,1)；eb＝x0(9:11)；db＝x0(12:14)；web＝w(1:3)；wdb＝x0(4:6)。

其中，Qt_d、Rk_d和Pk_d分别为系统噪声协方差驱动矩阵、量测噪声协方差阵和一步预测均方差阵。

二：对系统的姿态角、速度与位置进行初始化。载体初始姿态角att＝[0；0；0]*(pi/180)；载体初始速度vb＝[8；0；0]；载体的初始位置

pos＝[24*(pi/180)+35*(pi/180/60)；120*(pi/180)+58*(pi/180/60)；-10]。

然后对载体的轨迹进行设置后保存到tr(1:15)函数，该函数保存的是不含误差的真实值，包括载体的三个姿态角分量(横滚角、航向角及俯仰角)、三个速度分量(北向、天向及东向)、三个位置分量(经纬度及高度)、三个角速率分量个三个比力分量。

三：捷联惯导解算参数初始化。将载体真实的姿态角矩阵转换为四元数形式，然后加入平台误差角，即qnb＝tr(1:3)+phi，且系统的状态变量初始值Xk_d＝zeros(14,0)；将载体真实的速度信息加入速度误差，即vnm＝tr(4:6)+dvn；将载体真实的位置信息加入位置误差，即posm＝tr(7:9)+[x0(7)；x0(8)；0.1]；最后，将载体输出的陀螺仪与加速度计信息保存到wvm2函数，即wvm2＝tr(10:15)。

步骤3：步骤2完成后，对载体的姿态、速度与位置信息进行组合滤波。首先对误差估计函数进行初始化置0，inse(1,:)＝zeros(14,1)；然后将载体真实的姿态信息tr(1:3)与加入误差的实际值qnb中提取姿态误差信息，具体步骤为：先将qnb转化为共轭形式，再与姿态阵tr(1:3)的四元数形式做差乘得到姿态误差信息；其次，将载体的实际速度值与真实值做差，求得速度误差信息，即vnm-tr(4:6)；最后将载体实际的位置信息与真实值做差，求得位置误差值，即posm(1)-tr(7),posm(2)-tr(8)。

步骤4：设置时间循环(for k＝2:2:s_time*100)读取tr函数的角增量与比力增量，并对角增量与比力增量进行更新，对更新的角增量与比力增量信息保存到扩维后的tr函数中。即wvm0＝wvm2,wvm1＝tr(10:15),wvm2＝tr((15+10):(15+15))。其中，s_time为系统仿真时间。

获取角增量与比力增量，加入误差，并保存：

dwvm1＝wvm1-wvm0+([eb；db]+[web；wdb].randn(6,1))*Tm/2

dwvm2＝wvm2-wvm1+([eb；db]+[web；wdb].randn(6,1))*Tm/2

其中，Tm＝0.02为捷联惯导解算周期。

步骤5：以获取角增量与比力增量对陀螺仪增量与加速度计增量扩维，进行二子样解算，即

wm＝[dwvm1(1:3,:),dwvm2(1:3,:)]

vm＝[dwvm1(4:6,:),dwvm2(4:6,:)]

对载体的深度用深度传感器测量得到，加入相应误差，即posm(3,1)＝tr(9)+0.05*randn(1)

步骤6:将步骤5得到含有误差的实际增量值进行惯导解算，根据上一时刻的姿态四元素、速度、位置、陀螺仪与加速度计的测量值，计算当前时刻的姿态四元数、速度与位置。

一：计算地球自转角速度引起的陀螺感应值

wnie＝wie*[cos(posm_1(1))；sin(posm_1(1))；0]，其中wie＝0.0000729215为地球自转角速率；posm_1为前一时刻载体的位置信息。

二：计算载体在导航系下由北向与东向速度引起的陀螺感应值

wnen＝[vnm_1(3)/(Re*(1+e*sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))；

vnm_1(3)*(sin(posm_1(1))/cos(posm_1(1)))/(Re*(1+e*sin(posm_1(1))*

sin(posm_1(1)))+posm_1(3))；-vnm_1(1)/(Re*(1-2*e+3*e*

sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))]

其中，posm_1(3)为前一时刻载体的高度信息；Re＝6378160为地球半径；e为地球椭圆率；vnm_1(3)为载体东向航行速度。

三：由上述两个步骤，可得由载体运动速度和地球自转引起的导航系下的陀螺角速率wnin＝wnie+wnen。

四：由于采用二子样方法，计算陀螺仪与加速度计的输出增量

sumw＝wm(:,1)+wm(:,2)

sumv＝vm(:,1)+vm(:,2)

对sumw进行保存，即phim＝sumw。

五：计算二子样等效旋转矢量，更新phim。即：

phim＝phim+2/3*cross(wm(:,1),wm(:,2))

六：计算划桨效应补偿dvsculm与速度旋转效应补偿dvrotm。

dvsculm＝2/3*(cross(wm(:,1),vm(:,2))+cross(vm(:,1),wm(:,2)))

dvrotm＝1/2*cross(sumw,sumv)

七：根据上式对速度进行更新。

vnm＝vnm_1+Cnn*Cnb*(dvm+dvrotm+dvsculm)+gn-cross(2*wnie+wnen,vnm_1))*Tm其中，gn重加速度；Cnn为在导航系下载体的陀螺角速率由旋转矢量转化为矩阵形式；Cnb为载体的姿态矩阵。

步骤7:求解步骤1的系统的状态矩阵Ft_d与观测矩阵Hk_d，并进行离散化，结果分别保存到Fk_d与Qk_d。

步骤8：模拟转速计的测量值V_Tacho＝(Att2Mat(tr(1:3)))'*tr(4:6)+0.01*randn(3,1)，该测量值为载体坐标系下的测量值，同时将该测量值转化到导航系下，并将结果保存至V_nTacho。其中Att2Mat(tr(1:3))表示将载体真实的姿态角转换成矩阵形式。

步骤9：由步骤6和步骤7的结果计算系统测量值Zk_d＝vnm-V_nTacho。

步骤10：采用卡尔曼滤波对状态变量Xk_d进行滤波更新。

卡尔曼增益：K_k＝P_{k_d}H_k_d'*inv(H_{k_d}P_{k_d}H_{k_d}'+R_{k_d})

误差协方差阵：

P_{k_d}＝(eye(length(Xk_d))-Kk*Hk_d)*Pk_d*(eye(length(Xk_d))-Kk*Hk_d)'+Kk*Rk_d*Kk'

其中，Xk_d＝Fk_d*Xk_d；Pk_d＝Fk_d*Pk_d*Fk_d'+Qk_d。

状态更新：Xk_d＝Xk_d+Kk*(Zk_d-Hk_d*Xk_d)

步骤11：反馈修正。

一：分别对捷联惯导的姿态、速度与位置进行修正。即

qnb＝qnb-Xk_d(1:3)

vnm＝vnm-Xk_d(4:6)

posm(1)＝posm(1)-Xk_d(7)

posm(2)＝posm(2)-Xk_d(8)

二：反馈校正后的状态变量，得到Xk_d＝[zeros(8,1)；Xk_d(9:14)]。

步骤12：调用save函数，对系统状态变量及导航信息进行保存，包括Pk_d、Xk_d、Kk、qnb、vnm与posm。

步骤13：建立系统逆向导航算法。

一：首先调用load函数，加载步骤12保存的所有数据，并修改步骤4的时间循环顺序为逆序(for k＝s_time*100:(-2):2)。

二：然后对步骤6中的陀螺仪输出取反，有wb_nb＝-(phim-Cnb'*wnin*Tm)。

三：最后对对步骤6中的速度取反，有

vnm＝vnm_1-Cnn*Cnb*(dvm+dvrotm+dvsculm)-(gn-cross(2*wnie+wnen,vnm_1))*Tm

四：数据保存。调用save函数，对步骤12中的信息进行保存。

步骤1-步骤13为系统一次完整的正向和逆向处理过程，当需要对系统进行多次正逆向处理时，只需对导航信息进行反复保存及加载，并修改系统仿真时间，参考上述方法，即可完成系统不同次数、不同时间的正逆向处理。该算法流程图如图1所示。

在实验分析部分，首先对常规的正向导航算法与正逆向结合的导航算法性能进行对比，通过对不同时间、不同处理次数的对准过程进行对比，验证该方法的有效性及优越性。

图2是对系统前30秒采样数据的常规导航解算过程。可以看出，在30s内对系统进行常规正向导航解算时，对准发散；在200s和300s内对系统进行常规正向导航解算时，对准角误差收敛，并且300s的对准效果优于200s，见图3和图4；分别对系统前200秒和前300s的时间内采样数据正逆向交替解算5次，可以看出，此时对准精度明显优于常规的正向导航解算方式，同时前300s的对准效果优于200s，见图5和图6；分别对系统前200秒和前300s的时间内采样数据正逆向交替解算10次，此时与系统通过正逆向交替解算5次后的结果相比，失准角误差更小更平稳，见图7和图8。实验结果表明，利用此种方法能够获得较高的对准精度以及更短的对准时间，进而提高系统的整体性能，故所使用的方法是正确有效的。

Claims (1)

1.一种基于逆向导航算法的捷联惯导与转速计组合的对准方法，其特征在于步骤如下：

步骤1：建立捷联惯导与转速计对准模型：

状态方程：

观测方程：Z_ST＝H_STX_ST+v_ST

上式中

其中，F_ST为系统状态矩阵，H_ST为系统量测矩阵，系统噪声w_ST和量测噪声v_ST统为零均值高斯白噪声，V_N、V_U、V_E为载体在北天东方向上的速度，λ、L和h分别表示载体的经纬度与高度，且高度信息通过深度传感器直接测量得到；

建立正向导航算法微分方程，并进行离散化：

姿态方程：

速度方程：

位置方程：

R_M,R_N分别为载体所在位置地球子午圈和卯酉圈曲率半径，其计算公式为：

R_M≈R_e(1-2e+3esin²L)

R_N≈R_e(1+esin²L)

步骤2：卡尔曼滤波初始化

一：首先，对捷联惯导与转速计组合系统的状态变量初值进行初始化，确定其在误差干扰下的状态初值x0＝[phi；dvn；dpos；eb；db]，其中平台失准角误差phi＝[10；60；-10]*(pi/180/60)，位置误差dpos＝[10/6378160；10/6378160]、速度误差dvn＝[0.5；0.5；0.5]，陀螺随机漂移误差eb＝[0.02；0.02；0.02]*(pi/180/3600)与加速度计零位误差db＝[100；100；100]*(0.000001*9.78)，估计陀螺测量白噪声web＝[0.02；0.02；0.02]*(pi/180/3600)，加速度计的测量白噪声wdb＝[50；50；50]*(0.000001*9.78)，设webq＝[web(1,1)^2；web(2,1)^2；web(3,1)^2]，wdbq＝[wdb(1,1)^2；wdb(2,1)^2；wdb(3,1)^2]；

得系统方程中白噪声均方差为w＝[web；wdb；webq；wdbq]，其中观测方程中捷联惯导SINS与转速计的量测量之间的白噪声均方差为v＝[0.01；0.01；0.01]；

然后，对状态初值及相应误差函数分别赋值给相应参数，phi＝x0(1:3)；dvn＝x0(4:6)；Qtd＝w(7:12)；Qt_d＝diag(Qtd)；Rk_d＝diag(v)^2；Pkd＝x0(1:14)；Pk_d＝10*diag(Pkd)^2；Xk_d＝zeros(14,1)；eb＝x0(9:11)；db＝x0(12:14)；web＝w(1:3)；wdb＝x0(4:6)；

其中，Qt_d、Rk_d和Pk_d分别为系统噪声协方差驱动矩阵、量测噪声协方差阵和一步预测均方差阵；

二：对系统的姿态角、速度与位置进行初始化：载体初始姿态角att＝[0；0；0]*(pi/180)；载体初始速度vb＝[8；0；0]；载体的初始位置

pos＝[24*(pi/180)+35*(pi/180/60)；120*(pi/180)+58*(pi/180/60)；-10]；

然后对载体的轨迹进行设置后保存到tr(1:15)函数，包括载体的横滚角、航向角及俯仰角三个姿态角分量、北向、天向及东向三个速度分量，经纬度及高度三个位置分量，三个角速率分量和三个比力分量；

三：捷联惯导解算参数初始化：将载体真实的姿态角矩阵转换为四元数形式，然后加入平台误差角，即qnb＝tr(1:3)+phi，且系统的状态变量初始值Xk_d＝zeros(14,1)；将载体真实的速度信息加入速度误差，即vnm＝tr(4:6)+dvn；将载体真实的位置信息加入位置误差，即posm＝tr(7:9)+[x0(7)；x0(8)；0.1]；最后，将载体输出的陀螺仪与加速度计信息保存到wvm2函数，即wvm2＝tr(10:15)；

步骤3：步骤2完成后，对载体的姿态、速度与位置信息进行组合滤波：

首先对误差估计函数进行初始化置0，inse(1,:)＝zeros(14,1)；然后将载体真实的姿态信息tr(1:3)与加入误差的实际值qnb中提取姿态误差信息，具体步骤为：

先将qnb转化为共轭形式，再与姿态阵tr(1:3)的四元数形式做差乘得到姿态误差信息；其次，将载体的实际速度值与真实值做差，求得速度误差信息，即vnm-tr(4:6)；最后将载体实际的位置信息与真实值做差，求得位置误差值，即posm(1)-tr(7),posm(2)-tr(8)；

步骤4：设置时间循环for k＝2:2:s_time*100读取tr函数的角增量与比力增量，并对角增量与比力增量进行更新，对更新的角增量与比力增量信息保存到扩维后的tr函数中；即wvm0＝wvm2,wvm1＝tr(10:15),wvm2＝tr((15+10):(15+15))；其中，s_time为系统仿真时间；

获取角增量与比力增量，加入误差，并保存：

dwvm1＝wvm1-wvm0+([eb；db]+[web；wdb]·randn(6,1))*Tm/2

dwvm2＝wvm2-wvm1+([eb；db]+[web；wdb]·randn(6,1))*Tm/2

其中，Tm＝0.02为捷联惯导解算周期；

步骤5：以获取角增量与比力增量对陀螺仪增量与加速度计增量扩维，进行二子样解算，即

wm＝[dwvm1(1:3,:),dwvm2(1:3,:)]

vm＝[dwvm1(4:6,:),dwvm2(4:6,:)]

对载体的深度用深度传感器测量得到，加入相应误差，即posm(3,1)＝tr(9)+0.05*randn(1)

步骤6:将步骤5得到含有误差的实际增量值进行惯导解算，根据上一时刻的姿态四元数、速度、位置、陀螺仪与加速度计的测量值，计算当前时刻的姿态四元数、速度与位置；

一：计算地球自转角速度引起的陀螺感应值

wnie＝wie*[cos(posm_1(1))；sin(posm_1(1))；0]，其中wie＝0.0000729215为地球自转角速率；posm_1为前一时刻载体的位置信息；

二：计算载体在导航系下由北向与东向速度引起的陀螺感应值

wnen＝[vnm_1(3)/(Re*(1+e*sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))；

vnm_1(3)*(sin(posm_1(1))/cos(posm_1(1)))/(Re*(1+e*sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))；-vnm_1(1)/(Re*(1-2*e+3*e*sin(posm_1(1))*sin(posm_1(1)))+posm_1(3))]

其中，posm_1(3)为前一时刻载体的高度信息；Re＝6378160为地球半径；e为地球椭圆率；vnm_1(3)为载体东向航行速度；

三：由上述步骤6的步骤一和步骤二，可得由载体运动速度和地球自转引起的导航系下的陀螺角速率wnin＝wnie+wnen；

四：采用二子样方法，计算陀螺仪与加速度计的输出增量

sumw＝wm(:,1)+wm(:,2)

sumv＝vm(:,1)+vm(:,2)

对sumw进行保存，即phim＝sumw；

五：计算二子样等效旋转矢量，更新phim，即：

phim＝phim+2/3*cross(wm(:,1),wm(:,2))

六：计算划桨效应补偿dvsculm与速度旋转效应补偿dvrotm：

dvsculm＝2/3*(cross(wm(:,1),vm(:,2))+cross(vm(:,1),wm(:,2)))

dvrotm＝1/2*cross(sumw,sumv)

七：根据上式对速度进行更新；

vnm＝vnm_1+Cnn*Cnb*(dvm+dvrotm+dvsculm)+gn-cross(2*wnie+wnen,vnm_1))*Tm

其中，gn重加速度；Cnn为在导航系下载体的陀螺角速率由旋转矢量转化为矩阵形式；

Cnb为载体的姿态矩阵；

步骤7:求解步骤1的系统的状态矩阵F_ST与观测矩阵H_ST，并进行离散化，结果分别保存到Fk_d与Hk_d；

步骤8：模拟转速计的测量值V_Tacho＝(Att2Mat(tr(1:3)))'*tr(4:6)+0.01*randn(3,1)，该测量值为载体坐标系下的测量值，同时将该测量值转化到导航系下，并将结果保存至V_nTacho；其中Att2Mat(tr(1:3))表示将载体真实的姿态角转换成矩阵形式；

步骤9：由步骤6和步骤8的结果计算系统测量值Zk_d＝vnm-V_nTacho；

步骤10：采用卡尔曼滤波对状态变量Xk_d进行滤波更新；

卡尔曼增益：K_k＝P_{k_d}Hk_d'*inv(Hk_dP_{k_d}Hk_d'+R_{k_d})

误差协方差阵：

P_{k_d}＝(eye(length(Xk_d))-Kk*Hk_d)*Pk_d*(eye(length(Xk_d))-Kk*Hk_d)'+Kk*Rk_d*Kk'

其中，Xk_d＝Fk_d*Xk_d；Pk_d＝Fk_d*Pk_d*Fk_d'+Hk_d；

状态更新：Xk_d＝Xk_d+Kk*(Zk_d-Hk_d*Xk_d)

步骤11：反馈修正：

一：分别对捷联惯导的姿态、速度与位置进行修正，即

qnb＝qnb-Xk_d(1:3)

vnm＝vnm-Xk_d(4:6)

posm(1)＝posm(1)-Xk_d(7)

posm(2)＝posm(2)-Xk_d(8)

二：反馈校正后的状态变量，得到Xk_d＝[zeros(8,1)；Xk_d(9:14)]；

步骤12：调用save函数，对系统状态变量及导航信息进行保存，包括Pk_d、Xk_d、Kk、qnb、vnm与posm；

步骤13：建立系统逆向导航算法：

一：首先调用load函数，加载步骤12保存的所有数据，并修改步骤4的时间循环顺序为逆序for k＝s_time*100:(-2):2；

二：然后对步骤6中的陀螺仪输出取反，有wb_nb＝-(phim-Cnb'*wnin*Tm)；

三：最后对对步骤6中的速度取反，有

vnm＝vnm_1-Cnn*Cnb*(dvm+dvrotm+dvsculm)-(gn-cross(2*wnie+wnen,vnm_1))*Tm

四：数据保存；调用save函数，对步骤12中的信息进行保存；

步骤1-步骤13为系统一次完整的正向和逆向处理过程，当需要对系统进行多次正逆向处理时，只需对导航信息进行反复保存及加载，并修改系统仿真时间，参考上述方法，即可完成系统不同次数、不同时间的正逆向处理。