一种惯性多普勒全参量高精度标定方法及装置与流程

本发明涉及导航
技术领域：
，具体涉及一种惯性多普勒全参量高精度标定方法及装置。
背景技术：
：相控阵多普勒可通过声波原理测量航行器航行的速度信息，其与惯导结合构成惯性/多普勒组合导航系统可有效提高惯导的定位定向精度，并具有自主性好、不易受外界干扰等优点。但多普勒存在安装误差和标度因数误差，在使用前若不能进行准确的标定则系统导航精度将答复下降。此外，海底平面环境恶劣导致测量噪声和野值较大等特殊情况，易导致组合精度变差甚至滤波发散。本方案中涉及的坐标系定义如下：导航参考坐标系(t系)：坐标系原点在经纬度位置，x轴指东(e)，y轴指北(n)，z轴指天(u)；惯性平台坐标系(p系)：惯导基于姿态解算跟踪、复现导航参考坐标系所实际得到的坐标系；惯导计算坐标系(c系)：基于惯导解算经纬度所确定的东北天坐标系，原点在计算经纬度位置，x轴指东(e)，y轴指北(n)，z轴指天(u)；载体坐标系(b系)：固联在运载体上的参考坐标系，x轴沿运载体右侧，y轴指向运载体前向，z轴指向运载体顶部；载体初始惯性坐标系(b0系)：初始时刻与载体坐标系重合，并与惯性坐标系相对静止；多普勒坐标系(d系)：固联在运载体上的参考坐标系，x轴指向右侧，y轴指向前向，z轴与x轴、y轴构成右手正交坐标系。技术实现要素：本发明针对现有技术中存在的技术问题，提供一种惯性/多普勒全参量高精度标定方法及装置。该方法充分利用卫导、多普勒信息实现了航行器航行状态下多普勒三维安装误差和三维标度因子误差的标定，解决了现有算法标定精度低或对航行机动要求严格等的问题；在卫导有效的条件下通过多传感器组合导航对惯导陀螺漂移、加表漂移以及航行过程中多普勒标度因数误差进行在线标定；在卫导丢失后利用准确标定后的多普勒信息辅助惯导导航，可保持高精度的速度组合导航性能；在整个标定过程中还可利用组合导航后的高精度导航参数信息对多普勒信息进行故障检测，保证了系统的稳定性与容错性。本发明解决上述技术问题的技术方案如下：第一方面，本发明提供一种惯性/多普勒全参量高精度标定方法，包括以下步骤：s1，利用卫星导航信息对惯导系统进行初始对准；s2，对多普勒三维标度因数误差和惯导安装三维误差进行建模，基于惯导系统误差方程以及多普勒误差方程，建立关于导航系统误差、惯性器件误差以及多普勒三维标度因数和安装误差的高阶数学模型，即卡尔曼滤波器；s3，同步采集卫导水平位置信息及多普勒导航信息，根据卫导信息有效状态，结合所述卡尔曼滤波器，构造相应的卡尔曼滤波方程；s4，以深度计信息作为观测量进行组合导航滤波，以组合后的高度信息对地球重力参数信息进行解算，完成深度计与惯导之间的深度信息融合；s5，在滤波稳定后，系统进入在线反馈状态，滤波器利用状态变量估计值对系统各项误差进行实时修正。本发明的有益效果是：本发明采用卫导、多普勒等多种传感器作为辅助导航手段，构成惯性/卫星/多普勒组合导航系统能够在卫导有效的条件下对多普勒三维标度因数误差、安装误差进行高精度在线全参量标定，多种外部信息辅助惯导进行航行对准，可实现航行器航行状态下的高精度对准。在卫导丢失后，由于陀螺零偏、加表零偏、多普勒标度因数误差以及安装误差都得到了准确的估计补偿，系统精度仍可以得到保证。进一步的，所述步骤s1，包括：s101，利用动态解析对准算法快速确定惯导粗略水平姿态及航向，获得航行器初始航姿；s102，同步采集卫导水平位置信息，以惯导位置误差作为观测量进行卡尔曼滤波，完成初始对准。进一步的，所述步骤s1，还包括：s103，在对惯导系统完成初始对准后，对卡尔曼滤波参数进行重置，转入惯性/多普勒全参量标定状态。进一步的，所述的根据卫导信息有效状态构造相应的卡尔曼滤波方程，包括：在卫导系统有效状态下，利用卫导位置信息及多普勒速度信息，计算惯导位置误差以及速度误差，并以所述惯导位置误差以及速度误差作为观测量，构建卫导系统有效状态下的卡尔曼滤波方程；在卫导系统失效状态下，利用多普勒速度信息，以惯导速度误差误差作为观测量，将所述卫导系统有效状态下的卡尔曼滤波方程进行离散化，得到卫导系统失效状态下的卡尔曼滤波方程。进一步的，在所述步骤s4之前还包括：以组合导航速度信息为基准对多普勒速度信息的有效性进行判断，若当前系统在第一时间阈值内处于无效状态，则滤波器进入预测状态；若当前系统超过第二时间阈值处于无效状态，则系统退出组合导航状态；其中第二时间阈值大于第一时间阈值。进一步的，所述的以组合导航速度信息为基准对多普勒速度信息的有效性进行判断，包括：对多普勒两次测量信息进行计算得到速度增量信息；对组合导航后的速度信息进行统计处理得到速度增量信息基准值；当测量值与基准值差值超过设定门限值后，则多普勒当前信息不可用；当测量值与基准值差值小于设定门限值后，则多普勒当前信息可用。进一步的，所述步骤s4还包括：利用连续稳定输出的组合导航高度信息对深度计信息进行有效性判断。本发明的惯性/多普勒全参量高精度标定方法综合使用卫导、多普勒、深度计等多种传感器完成对准、导航与标定。对准阶段结合动态解析对准及卡尔曼滤波对准，实现了惯导全程处于航行状态下的初始对准。在多传感器组合模式下，滤波器可观测性较高，可对系统各误差源进行在线标定补偿，使得在卫导信息无效时能够保持高精度的导航。利用导航参数对外部观测信息进行有效性判断，可准确判断出有效信息，减少了无效信息对滤波器的影响。第二方面，本发明提供一种惯性/多普勒全参量高精度标定系统，包括：初始对准模块，利用卫星导航信息对惯导系统进行初始对准；滤波器建立模块，对多普勒三维标度因数误差和惯导安装三维误差进行建模，基于惯导系统误差方程以及多普勒误差方程，建立关于导航系统误差、惯性器件误差以及多普勒三维标度因数和安装误差的高阶数学模型，即卡尔曼滤波器；滤波方程构造模块，同步采集卫导水平位置信息及多普勒导航信息，根据卫导信息有效状态，结合所述卡尔曼滤波器，构造相应的卡尔曼滤波方程；深度信息融合模块，以深度计信息作为观测量进行组合导航滤波，以组合后的高度信息对地球重力参数信息进行解算，完成深度计与惯导之间的深度信息融合；反馈修正模块，在滤波稳定后，系统进入在线反馈状态，滤波器利用状态变量估计值对系统各项误差进行实时修正。第三方面，本发明提供一种电子设备，包括：存储器，用于存储计算机软件程序；处理器，用于读取并执行所述存储器存储的计算机软件程序，用于实现本发明第一方面所述的一种惯性/多普勒全参量高精度标定方法。第四方面，本发明提供一种非暂态计算机可读存储介质，其特征在于，所述存储介质内存储有用于实现本发明第一方面所述的一种惯性/多普勒全参量高精度标定方法的计算机软件程序。附图说明图1为本发明实施例提供的一种惯性/多普勒全参量高精度标定方法流程图；图2为本发明实施例提供的一种惯性/多普勒全参量高精度标定方法实施原理图；图3为本发明实施例提供的该方法的试验过程中航程点位误差曲线图；图4为本发明实施例提供的该方法的试验过程中航程轨迹曲线图。具体实施方式以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。实施例本发明实施例提供一种惯性/多普勒全参量高精度标定方法，如图1图2所示，包括以下步骤：s1，利用卫星导航信息对惯导系统进行初始对准。具体的，包括以下内容：s101，利用动态解析对准算法快速确定惯导粗略水平姿态及航向，获得航行器初始航姿。s102，同步采集卫导水平位置信息，以惯导位置误差作为观测量进行卡尔曼滤波，完成初始对准。s103，在对惯导系统完成初始对准后，对卡尔曼滤波参数进行重置，转入惯性/多普勒全参量标定状态。s2，对多普勒三维标度因数误差和惯导安装三维误差进行建模，基于惯导系统误差方程以及多普勒误差方程，建立关于导航系统误差、惯性器件误差以及多普勒三维标度因数和安装误差的高阶数学模型，即卡尔曼滤波器。具体的，惯导系统进行惯性解算获得导航参数，构造卡尔曼滤波状态转移矩阵f(t)，并计算离散状态转移矩阵φ(k,k-1)，利用公式计算观测矩阵z(t)。在卫导系统可用状态下，同步采集卫导位置信息与多普勒速度信息，计算惯导位置误差以及速度误差作为观测量，基于惯性误差方程、多普勒误差方程建立系统状态方程以及观测方程。系统误差方程如下：姿态误差模型：速度误差方程：位置误差方程：公式(1)(2)(3)中，的含义为地球坐标系e相对于惯性坐标系i在导航参考坐标系t的投影值，地球自转角速度误差，同理，的含义为导航坐标系t相对于地球坐标系e在导航参考坐标系t的投影值，载体运动引起的角速度误差。多普勒误差方程如下：基于惯性系统误差方程与多普勒误差方程，选取惯导姿态误差φ、速度误差δv、位置误差(δλ,δl,δh)、陀螺漂移ε、加表漂移多普勒航向安装误差αθ、纵摇安装误差αψ、横摇安装误差αφ以及三维标度因数误差δkd等21个状态变量建立卡尔曼滤波器：s3，同步采集卫导水平位置信息及多普勒导航信息，根据卫导信息有效状态，结合所述卡尔曼滤波器，构造相应的卡尔曼滤波方程。其中，g(t)为滤波模型所对应系统噪声分布矩阵；w(t)为滤波模型所对应系统噪声分布矩阵；v(t)为系统激励噪声；f(t)为根据惯导误差方程以及多普勒误差方程所建立的描述状态变量之间耦合关系的状态矩阵；z(t)为系统观测变量；h(t)为系统观测矩阵。观测矩阵需要根据观测量的不同进行变换，在卫导可用的条件下，滤波器利用卫导位置信息及多普勒速度信息，计算惯导位置误差z1以及速度误差z2作为观测量z。来自于卫导的惯导位置误差观测量：z1＝[δl,δλ,δh]t；来自于里程计的速度误差观测量：z2＝[δve,δvn,δvu]t；建立观测方程如下：z1＝h1·x+v1＝[03×6i3×303×12]x+v1(7)其中，dsins纯惯性导航位置增量，dsdr为里程计计算的位置增量，δvins为惯性导航速度误差，δvdr为里程计速度误差，k为观测点序号，观测方程观测噪声在卫导系统失效状态下，利用多普勒速度信息，以惯导速度误差误差作为观测量，通过改变系统观测方程对滤波器状态进行切换，进入惯性/多普勒组合模式。惯性/多普勒组合模式下系统状态方程不改变，系统观测方程切换为如下状态：对公式(5)所示连续系统状态方程进行离散化可得到(10)所示离散模型：其中fi为状态矩阵f(t)的离散化表示，tn为离散化周期。s4，以组合导航速度信息为基准对多普勒速度信息的有效性进行判断，减少无效信息对滤波器造成的影响。在判断的过程中主要依照以下三个准则：(1)、多普勒速度远大于航行器前进速度；(2)多普勒速度远小于航行器前进速度；(3)航行器运动与多普勒速度之间的关联性将发生变化，惯性测量加速度与多普勒测量加速度存在较大差别。传统算法的主要思路是对多普勒信息进行微分求速度、加速度信息，并在组合滤波过程中根据滤波器相关参数信息对多普勒有效性进行判断。该方法会导致以下两种问题：第一、微分运算将产生较大微分误差，可能导致判断失误；第二、滤波器会受到无效信息的污染。本发明采用速度、加速度信息在滤波前对有效性进行判断，减少无效信息对滤波器的影响。具体的，这里所述的以组合导航速度信息为基准对多普勒速度信息的有效性进行判断，包括：对多普勒两次测量信息进行计算得到速度增量信息；对组合导航后的速度信息进行统计处理得到速度增量信息基准值；当测量值与基准值差值超过设定门限值后，则多普勒当前信息不可用；当测量值与基准值差值小于设定门限值后，则多普勒当前信息可用。若当前系统在第一时间阈值内处于无效状态，则滤波器进入预测状态；若当前系统超过第二时间阈值处于无效状态，则系统退出组合导航状态；其中第二时间阈值大于第一时间阈值。s5，以深度计信息作为观测量进行组合导航滤波，并利用连续稳定输出的组合导航高度信息对深度计信息进行有效性判断，以组合后的高度信息对地球重力参数信息进行解算，完成深度计与惯导之间的深度信息融合。s6，在滤波稳定后，系统进入在线反馈状态，滤波器利用状态变量估计值对系统各项误差进行实时修正。带有反馈控制回路的卡尔曼滤波方程如下：w(t)为滤波模型所对应系统噪声分布矩阵；v(t)为系统激励噪声；f(t)为根据惯导误差方程以及多普勒误差方程所建立的描述状态变量之间耦合关系的状态矩阵；z(t)为系统观测变量；h(t)为系统观测矩阵。利用位置和速度进行组合导航，滤波器可对21个状态变量进行估计，其中，状态变量x(16)、x(17)、x(18)构成安装误差修正矩阵，对多普勒安装误差按如下方法进行修正：d'表示安装误差坐标系。同理，对滤波估计的状态变量x(19:21)对里程计标度因数kd按如下方法进行修正：kd＝kd·(1+x(19:21))(14)本发明采用卫导、多普勒等多种传感器作为辅助导航手段，构成惯性/卫星/多普勒组合导航系统能够在卫导有效的条件下对多普勒三维标度因数误差、安装误差进行高精度在线全参量标定，多种外部信息辅助惯导进行航行对准，可实现航行器航行状态下的高精度对准。在卫导丢失后，由于陀螺零偏、加表零偏、多普勒标度因数误差以及安装误差都得到了准确的估计补偿，系统精度仍可以得到保证。本发明的惯性/多普勒全参量高精度标定方法综合使用卫导、多普勒、深度计等多种传感器完成对准、导航与标定。对准阶段结合动态解析对准及卡尔曼滤波对准，实现了惯导全程处于航行状态下的初始对准。在多传感器组合模式下，滤波器可观测性较高，可对系统各误差源进行在线标定补偿，使得在卫导信息无效时能够保持高精度的导航。利用导航参数对外部观测信息进行有效性判断，可准确判断出有效信息，减少了无效信息对滤波器的影响。利用本发明的方法在青岛某海域进行了航行试验，采用陀螺精度为0.05度/小时的50型激光惯导。航行启动对准，匀速直航20秒，动态对准8分钟后转入全参量标定状态，1小时后完成全参数误差标定，标定完成后，断开卫导信息，自主导航运行5小时，航程近55km，形成百分比优于0.1％d。试验结果如表1所示，图3图4为试验曲线图。表1组合导航试验结果统计多普勒组合水平位置误差50(最大值/米)总路程55(公里)水平位置误差0.1％d(误差/总路程)航行轨迹青岛某海域实施例二本发明实施例提供一种惯性/多普勒全参量高精度标定系统，如图5所示，包括：初始对准模块，利用卫星导航信息对惯导系统进行初始对准；滤波器建立模块，对多普勒三维标度因数误差和惯导安装三维误差进行建模，基于惯导系统误差方程以及多普勒误差方程，建立关于导航系统误差、惯性器件误差以及多普勒三维标度因数和安装误差的高阶数学模型，即卡尔曼滤波器；滤波方程构造模块，同步采集卫导水平位置信息及多普勒导航信息，根据卫导信息有效状态，结合所述卡尔曼滤波器，构造相应的卡尔曼滤波方程；深度信息融合模块，以深度计信息作为观测量进行组合导航滤波，以组合后的高度信息对地球重力参数信息进行解算，完成深度计与惯导之间的深度信息融合；反馈修正模块，在滤波稳定后，系统进入在线反馈状态，滤波器利用状态变量估计值对系统各项误差进行实时修正。需要说明的是，实施例一中所述的方法可以通过计算机软件程序实现，基于此，本发明实施例还提供一种电子设备，包括：存储器，用于存储计算机软件程序；处理器，用于读取并执行所述存储器存储的计算机软件程序，用于实现实施例一提供的一种惯性/多普勒全参量高精度标定方法。同时还需说明的是，计算机软件程序中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。当前第1页12