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

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

【技术实现步骤摘要】

本专利技术属于惯导对准技术的方法，涉及一种基于逆向导航算法的捷联惯导与转速计组合对准方法的研究。
技术介绍
自主水下航行器((AutonomousUnderwaterVehicle，简称AUV)是执行海洋军事任务和进行海洋开发的重要工具。当AUV在进行任务前，惯导需要对系统进行初始对准，其中快速性和精确性是对系统要求的重要指标，一般来说二者的关系是互相矛盾、相互制约的。不同于静基座对准，行进间对准需要外部的辅助设备提供载体的运动信息，通过辅助设备量测的信息对惯导系统进行修正补偿。在非常时期航行器必须有快速机动，并精确导航，兼具水下隐蔽性等要求。因此解决对准的精确性和快速性问题成为AUV航行任务中的重要议题。然而传统的对准技术需要利用岸上平台进行传递对准，或利用价值昂贵的辅助设备进行组合对准，这给AUV在快速投入作战的需要和实际工程开发应用中带来巨大挑战，因此，必须对其进行适当改善，在解决此类问题的同时，适应时局的发展。在水下领域，作为比较成熟的组合导航方式，SINS/DVL(Strap-downInertialNavigationSystem,捷联惯性导航系统)组合导航系统利用DVL(DopplerVelocityLog,多普勒计程仪)提供的速度信息修正SINS的量测信息，以此抑制捷联惯导系统的误差积累，是目前应用比较广泛的水下组合导航技术。然而，由于DVL价值昂贵，并且会在工作时会向外发射声波，暴露自身的位置，所以不能很好的满足隐蔽性的要求。同时，DVL的有效测速度范围为±10m/s，当航行器处于相对速度较高的状态下，DVL亦不能保证有效的测量精度，因此限制了该方式的应用。转速计凭借其低廉的价格并且能够测得在高速运动下航行器轴向速度的特点，可以有效解决上述的问题。通过选取转速计作为捷联惯导系统的辅助设备，测量载体在高速运动下的速度信息，利用量测的速度信息修正惯导系统，并借助正向和逆向结合的导航算法对获取的数据信息反复处理，以加大对数据的利用，进而满足系统的对准精度和快速性的要求。因此，本文将逆向导航算法引入捷联惯导与转速计组合对准方法。通过将捷联惯导与转速计的速度信息进行信息融合，利用正逆向导航算法交替处理，在增大对获取的数据信息的同时，有效提高系统的对准精度，缩短系统的对准时间。
技术实现思路
为了避免现阶段对准技术的不足之处，本专利技术提出了捷联惯导与转速计组合对准的方法，并引入逆向导航算法，通过获取捷联惯导与转速计的采样数据，并将二者加以滤波融合，然后利用正逆向导航算法，对采样阶段的信息进行多次反复处理，得到更为精确的对准信息。这种方法在提高系统对准精确度的同时，还能有效缩短系统在对准过程的时间，进而提升系统的整体性能。一种基于逆向导航算法的捷联惯导与转速计组合的对准方法，其特征在于步骤如下：步骤1：建立捷联惯导与转速计对准模型：转态方程：观测方程：ZST＝HSTXST+vST上式中其中，FST为系统状态矩阵，HST为系统量测矩阵，系统噪声wST和量测噪声μST统为零均值高斯白噪声。VN、VU、VE为载体在北天东方向上的速度，λ、L和h分别表示载体的经纬度与高度，且高度信息可以通过深度传感器直接测量得到。建立正向导航算法微分方程，对其进行离散化：姿态方程：速度方程：位置方程：RM,RN分别为载体所在位置地球子午圈和卯酉圈曲率半径，其近似计算公式为：RM≈Re(1-2e+3esin2L)RN≈Re(1+esin2L)步骤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：设置时间循环(fork＝2:2:s_time*100)读取tr函数的角增量与比力增量，并对角增量与比力增量进行更新，对更新的角增量与比力本文档来自技高网...

【技术保护点】
一种基于逆向导航算法的捷联惯导与转速计组合的对准方法，其特征在于步骤如下：步骤1：建立捷联惯导与转速计对准模型：转态方程：观测方程：ZST＝HSTXST+vST上式中FST=F11F12F13-Cbn03×3F21F22F2303×3Cbn02×3F32F3302×302×306×14HST=0VE-VU-VE0VNI3×303×8VU-VN0]]>F11=0-VNRM+h-(ωiesin L+VEtan LRN+h)VNRM+h0ωiecos L+VERN+hωiesin L+VEtan LRN+h-(ωiecos L+VERN+h)0]]>F12=001RN+h00tan LRN+h-1RM+h00,F13=-ωiesin L0ωiecos L+VEsec2LRN+h000]]>F33=00VEsec L tan LRN+h0,F32=1RM+h0000sec LRN+h]]>F22=-VURM+h-VNRM+h-2(ωiesin L+VEtan LRN+h)2VNRM+h02(ωiecos L+VERN+h)2ωiesin L+VEtan LRN+h-(2ωiecos L+VERN+h)VNtan L-VURN+h]]> F23=-VE(2ωiecos L+VEsec2LRN+h)0-2VEωiesinL02ωie(VNcos L+VUsin L)+VEVNsec2LRN+h0,F21=0-fEfUfE0-fN-fUfN0]]>其中，FST为系统状态矩阵，HST为系统量测矩阵，系统噪声wST和量测噪声μST统为零均值高斯白噪声。VN、VU、VE为载体在北天东方向上的速度，λ、L和h分别表示载体的经纬度与高度，且高度信息可以通过深度传感器直接测量得到；建立正向导航算法微分方程，并进行离散化：姿态方程：速度方程：位置方程：RM,RN分别为载体所在位置地球子午圈和卯酉圈曲率半径，其计算公式为：RM≈Re(1‑2e+3esin2 L)RN≈Re(1+esin2 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...

【技术特征摘要】
1.一种基于逆向导航算法的捷联惯导与转速计组合的对准方法，其特征在于步骤如下：步骤1：建立捷联惯导与转速计对准模型：转态方程：观测方程：ZST＝HSTXST+vST上式中FST=F11F12F13-Cbn03×3F21F22F2303×3Cbn02×3F32F3302×302×306×14HST=0VE-VU-VE0VNI3×303×8VU-VN0]]>F11=0-VNRM+h-(ωiesinL+VEtanLRN+h)VNRM+h0ωiecosL+VERN+hωiesinL+VEtanLRN+h-(ωiecosL+VERN+h)0]]>F12=001RN+h00tanLRN+h-1RM+h00,F13=-ωiesinL0ωiecosL+VEsec2LRN+h000]]>F33=00VEsecLtanLRN+h0,F32=1RM+h0000secLRN+h]]>F22=-VURM+h-VNRM+h-2(ωiesinL+VEtanLRN+h)2VNRM+h02(ωiecosL+VERN+h)2ωiesinL+VEtanLRN+h-(2ωiecosL+VERN+h)VNtanL-VURN+h]]>F23=-VE(2ωiecosL+VEsec2LRN+h)0-2VEωiesinL02ωie(VNcosL+VUsinL)+VEVNsec2LRN+h0,F21=0-fEfUfE0-fN-fUfN0]]>其中，FST为系统状态矩阵，HST为系统量测矩阵，系统噪声wST和量测噪声μST统为零均值高斯白噪声。VN、VU、VE为载体在北天东方向上的速度，λ、L和h分别表示载体的经纬度与高度，且高度信息可以通过深度传感器直接测量得到；建立正向导航算法微分方程，并进行离散化：姿态方程：速度方程：位置方程：RM,RN分别为载体所在位置地球子午圈和卯酉圈曲率半径，其计算公式为：RM≈Re(1-2e+3esin2L)RN≈Re(1+esin2L)步骤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：设置时间循环(fork＝2:2...

【专利技术属性】
技术研发人员：张福斌，张立川，高剑，刘明雍，严卫生，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：陕西;61