标定SINS/DVL组合导航系统误差的方法和装置制造方法及图纸

本发明专利技术涉及一种标定SINS/DVL组合导航系统误差的方法和装置，所述方法包括：建模非线性优化函数；所述非线性优化函数的参数包括：DVL速度的标度因数误差，以及，SINS与DVL之间的方位安装误差角；求解所述非线性优化函数，以完成对两个误差的标定。本发明专利技术所述的一种标定SINS/DVL组合导航系统误差的方法通过针对以上两个参数建模非线性优化函数，并利用高斯牛顿算法求解所述非线性优化函数，精确标定出这两个误差，经过和枚举法进行比较，测试本方法的标定精度能够达到最优解。本发明专利技术所述的标定SINS/DVL组合导航系统误差的方法能够提升SINS/DVL组合导航的精度，具有非常高的实用价值。

【技术实现步骤摘要】
标定SINS/DVL组合导航系统误差的方法和装置
本专利技术涉及惯性导航
，具体涉及一种标定SINS/DVL组合导航系统误差的方法和装置。
技术介绍
在水下环境中，GNSS信号不可用，而纯惯导系统往往误差发散较快，因而SINS/DVL组合导航是水下导航的常用方式。其中SINS和DVL之间安装误差角、DVL刻度系数误差对水下SINS/DVL组合导航精度的影响非常大。如能够准确标定出这两个误差，则对SINS/DVL组合导航精度会产生非常大的提升。针对SINS/DVL组合导航系统误差的标定主要有3种方案，都是借助GNSS的速度信息或者位置信息，作为真值，方案A使用“反复试探”的方法，通过观察SINS/DVL导航轨迹和GNSS轨迹的重合度，来不断修改误差值，最终得到一个和GNSS轨迹重合度较高的标定参数，该方案耗时较长，需要不断测试更新后的标定误差，人力消耗大，且需要标定人员具备一定的调参经验，且不容易得到最优的标定参数。方案B使用了最小二乘法的方法，但是由于没有考虑机器人的运动模型，会导致标定精度的下降，并且该方案只标定了SINS和DVL之间的安装误差，没有标定DVL标度因子，进一步导致导航精度的下降。方案C同时标定安装误差和标定因子，但是使用了梯度下降方法，该方法有可能陷入局部最小，造成寻不到最优解的问题。
技术实现思路
有鉴于此，本专利技术的目的在于克服现有技术的不足，提供一种标定SINS/DVL组合导航系统误差的方法和装置。为实现以上目的，本专利技术采用如下技术方案：一种标定SINS/DVL组合导航系统误差的方法，包括：建模非线性优化函数；所述非线性优化函数的参数包括：DVL速度的标度因数δk，以及，SINS与DVL之间的方位安装误差角β；求解所述非线性优化函数，以完成对DVL速度的标度因数δk，以及，SINS与DVL之间的方位安装误差角β的标定。可选的，所述建模非线性优化函数，包括：DVL自身载体坐标系为m系，惯导坐标系为b系，导航坐标系为n系，DVL探底模式下得到的DVL速度为DVL数据更新时间为则航位推算公式如下：其中，θq＝θy+β；θy表示SINS的方位失准角，θp表示俯仰失准角，θr表示滚转失准角，θq表示m系向导航系转换过程中的方位偏差角，表示SINS和DVL之间的安装误差转换矩阵，表示惯导和导航系之间的转换矩阵；获取每一个DVL数据所对应的GPS数据；将GPS数据中的经纬度转换到东北天坐标系下的位置，表示为：GPS的位置增量计算公式为：其中，为第一个GPS数据，航位推算位置数据和GPS数据均为增量数据，并且第一个DVL数据和GPS数据经过线性插值之后是对应的；最终的优化函数为：f(β，δk)为关于β和δk的非线性函数。可选的，所述求解所述非线性优化函数，包括：采用高斯牛顿方法求解所述非线性优化函数。可选的，所述采用高斯牛顿方法求解所述非线性优化函数，包括：将所述非线性优化函数进行一阶泰勒展开；根据一阶泰勒展开后的结果，确定出增量方程；求解所述增量方程。可选的，所述将所述非线性优化函数进行一阶泰勒展开，包括：令x＝[β,δk]，将非线性优化函数f(x)进行一阶泰勒展开，f(x+Δx)＝f(x)+J(x)Δx，其中，J(x)为f(x)关于x的导数，J(x)称为雅克比矩阵。可选的，所述根据一阶泰勒展开后的结果，确定出增量方程，包括：将寻找下降矢量Δx，使得||f(x+Δx)||2达到最小的问题，转换成求解一个线性的最小二乘问题：经过上述转换，要优化的参数由x变为Δx，对目标函数的平方项进行展开：求展开后的式子关于Δx的导数，并令其为0，得：得到增量方程为：把增量方程左边的系数定义为H，右边定义为g,那么所述上式变为：HΔx＝g。可选的，所述采用高斯牛顿方法求解所述非线性优化函数，包括：步骤1：给定初始值x0；步骤2：对于第k次迭代，求出当前的雅克比矩阵J(xk)和误差f(xk)；步骤3：求解增量方程HΔx＝g，得到xk；步骤4：如果xk小于预设值，或者，迭代次数达到设定的最大值，则xk作为最优解，停止迭代；否则，令xk+1＝xk+Δxk，返回步骤2继续迭代运行。本专利技术还提供了一种标定SINS/DVL组合导航系统误差的装置，包括：建模模块，用于建模非线性优化函数；所述非线性优化函数的参数包括：DVL速度的标度因数δk，以及，SINS与DVL之间的方位安装误差角β；求解模块，用于求解所述非线性优化函数，以完成对DVL速度的标度因数δk，以及，SINS与DVL之间的方位安装误差角β的标定。本专利技术还提供了一种SINS/DVL组合导航系统，其特征在于，包括：如前面所述的一种标定SINS/DVL组合导航系统误差的装置。此外，本专利技术还提供了一种控制器，用于执行前面任一项所述标定SINS/DVL组合导航系统误差的方法。本专利技术采用以上技术方案，所述一种标定SINS/DVL组合导航系统误差的方法，包括：建模非线性优化函数；所述非线性优化函数的参数包括：DVL速度的标度因数误差，以及，SINS与DVL之间的方位安装误差角；求解所述非线性优化函数，以完成对DVL速度的标度因数误差，以及，SINS与DVL之间的方位安装误差角的标定。本专利技术所述的一种标定SINS/DVL组合导航系统误差的方法通过针对以上两个参数建模非线性优化函数，并利用高斯牛顿算法求解所述非线性优化函数，精确标定出这两个误差，经过和枚举法进行比较，测试本方法的标定精度能够达到最优解。本专利技术所述的标定SINS/DVL组合导航系统误差的方法能够提升SINS/DVL组合导航的精度，具有非常高的实用价值。附图说明为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。图1是本专利技术一种标定SINS/DVL组合导航系统误差的方法提供的流程示意图；图2是没有进行标定误差的航位推算结果示意图；图3是通过本专利技术所述方法标定误差之后的航位推算结果示意图；图4是没有经过误差标定的轨迹示意图；图5是经过误差标定的轨迹示意图；图6是本专利技术一种标定SINS/DVL组合导航系统误差的装置提供的结构示意图。图中：1、建模模块；2、求解模块。具体实施方式为使本专利技术的目的、技术方案和优点更加清楚，下面将对本专利技术的技术方案进行详细的描述。显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。基于本本文档来自技高网...

【技术保护点】
1.一种标定SINS/DVL组合导航系统误差的方法，其特征在于，包括：/n建模非线性优化函数；所述非线性优化函数的参数包括：DVL速度的标度因数误差δk，以及，SINS与DVL之间的方位安装误差角β；/n求解所述非线性优化函数，以完成对DVL速度的标度因数误差δk，以及，SINS与DVL之间的方位安装误差角β的标定。/n

【技术特征摘要】
1.一种标定SINS/DVL组合导航系统误差的方法，其特征在于，包括：
建模非线性优化函数；所述非线性优化函数的参数包括：DVL速度的标度因数误差δk，以及，SINS与DVL之间的方位安装误差角β；
求解所述非线性优化函数，以完成对DVL速度的标度因数误差δk，以及，SINS与DVL之间的方位安装误差角β的标定。


2.根据权利要求1所述的方法，其特征在于，所述建模非线性优化函数，包括：
DVL自身载体坐标系为m系，惯导坐标系为b系，导航坐标系为n系，DVL探底模式下得到的DVL速度为DVL数据更新时间为则航位推算公式如下：



其中，θq＝θy+β；
θy表示SINS的方位失准角，θp表示俯仰失准角，θr表示滚转失准角，θq表示m系向导航系转换过程中的方位偏差角，表示SINS和DVL之间的安装误差转换矩阵，表示惯导和导航系之间的转换矩阵；









获取每一个DVL数据所对应的GPS数据；
将GPS数据中的经纬度转换到东北天坐标系下的位置，表示为：



GPS的位置增量计算公式为：
其中，为第一个GPS数据，航位推算位置数据和GPS数据均为增量数据，并且第一个DVL数据和GPS数据经过线性插值之后是对应的；
最终的优化函数为：



f(β，δk)为关于β和δk的非线性函数。


3.根据权利要求1或2所述的方法，其特征在于，所述求解所述非线性优化函数，包括：采用高斯牛顿方法求解所述非线性优化函数。


4.根据权利要求3所述的方法，其特征在于，所述采用高斯牛顿方法求解所述非线性优化函数，包括：
将所述非线性优化函数进行一阶泰勒展开；
根据一阶泰勒展开后的结果，确定出增量方程；
求解所述增量方程。


5.根据权利要求4所述的方法，其特征在于，所述将所述非线性优化函数进行一阶泰勒展开，包括：
令x＝[β,...

【专利技术属性】
技术研发人员：熊明磊，陈龙冬，刘兵，
申请(专利权)人：博雅工道北京机器人科技有限公司，
类型：发明
国别省市：北京;11