[发明专利]基于逆向导航算法的捷联惯导与转速计组合对准方法

摘要

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

主权项

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/2dwvm2＝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为系统一次完整的正向和逆向处理过程，当需要对系统进行多次正逆向处理时，只需对导航信息进行反复保存及加载，并修改系统仿真时间，参考上述方法，即可完成系统不同次数、不同时间的正逆向处理。