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

权利要求书

1.一种标定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的非线性函数。

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

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

将所述非线性优化函数进行一阶泰勒展开；

根据一阶泰勒展开后的结果，确定出增量方程；

求解所述增量方程。

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

令x＝[β,δk]，将非线性优化函数f(x)进行一阶泰勒展开，

f(x+Δx)＝f(x)+J(x)Δx，

其中，J(x)为f(x)关于x的导数，J(x)称为雅克比矩阵。

5.根据权利要求4所述的方法，其特征在于，所述根据一阶泰勒展开后的结果，确定出增量方程，包括：

将寻找下降矢量Δx，使得||f(x+Δx)||²达到最小的问题，转换成求解一个线性的最小二乘问题：

经过上述转换，要优化的参数由x变为Δx，对目标函数的平方项进行展开：

求展开后的式子关于Δx的导数，并令其为0，得：

得到增量方程为：

把增量方程左边的系数定义为H，右边定义为g,那么所述上式变为：

HΔx＝g。

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

步骤1：给定初始值x₀；

步骤2：对于第k次迭代，求出当前的雅克比矩阵J(x_k)和误差f(x_k)；

步骤3：求解增量方程HΔx＝g，得到x_k；

步骤4：如果x_k小于预设值，或者，迭代次数达到设定的最大值，则x_k作为最优解，停止迭代；否则，令x_k+1＝x_k+Δx_k，返回步骤2继续迭代运行。