[发明专利]标定SINS/DVL组合导航系统误差的方法和装置

说明书

本发明涉及一种标定SINS/DVL组合导航系统误差的方法和装置，所述方法包括：建模非线性优化函数；所述非线性优化函数的参数包括：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的位置增量计算公式为：