一种基于牛顿迭代法的DVL误差标定方法及系统技术方案

本发明专利技术公开了一种基于牛顿迭代法的DVL误差标定方法及系统，其包括：步骤1，构建DVL误差损失函数f(Φ，δ)；步骤2，通过牛顿迭代法逼近损失极小值点获得Φ和δ的最优估计值，其具体包括：步骤21，令X＝[Φ，δ]，将所述非线性优化函数f(X)进行在初始点二阶泰勒展开；步骤22，根据所述步骤21二阶泰勒展开后的结果，确定迭代方程：确定迭代方程：步骤23，给定初始值通过i次迭代，求出每次迭代中不同的海森矩阵A(X)及其逆矩阵，从而得到X

【技术实现步骤摘要】
一种基于牛顿迭代法的DVL误差标定方法及系统


[0001]本专利技术涉及组合导航
，特别是关于一种基于牛顿迭代法的DVL(Doppler Velocity Log，多普勒计程仪)误差标定方法及系统。

技术介绍

[0002]DVL用安装在水下航行器上声学换能器阵发射声波，基于多普勒原理测量水下航行器对于水底或者水层/水面的相对速度，具有精度高的特点，是水下航行器导航系统关键设备。然而实际工程应用中，DVL安装误差与DVL刻度误差通常难以避免，且随着航行时间增加会产生累计误差，进而直接影响系统整体导航精度。因此在实现DVL测速导航定位之前，通常需要对其进行刻度系数和安装误差角标定，以提高载体自主导航精度。现有的标定方法主要采用航迹校准法，梯度下降优化法。航迹校准法耗时较长，需要标定人员具备一定调参经验，且不容易得到最优标定参数；梯度下降优化法法标定过程简单、环境依赖度低，但优化过程中只使用一阶偏导，即利用平面函数拟合损失函数曲面，但由于损失函数曲面多与平面存在较大差别，因此该方法需要多次迭代拟合才能逐步逼近损失函数最优解，导致误差估计效率较低。

技术实现思路

[0003]本专利技术的目的在于提供一种基于牛顿迭代法的DVL误差标定方法及系统，以实现提升DVL误差估计效率。
[0004]为实现上述目的，本专利技术提供一种DVL误差优化方法，其包括：
[0005]步骤1，构建DVL误差损失函数f(Φ,δ)；
[0006]步骤2，通过牛顿迭代法逼近损失极小值点获得Φ和δ的最优估计值，其具体包括：
[0007]步骤21，令X＝[Φ,δ]，将所述非线性优化函数f(X)进行在初始点二阶泰勒展开；
[0008]步骤22，根据所述步骤21二阶泰勒展开后的结果，确定迭代方程(3)，其中为A(X)逆矩阵：
[0009][0010]其中，为f(X)梯度，表示为式(4)；表示为式(5)；A和A(X)均为海森矩阵，表示为式(6)：
[0011][0012][0013][0014]式中，为偏导数。
[0015]步骤23，给定初始值通过i次迭代，求出每次迭代中不同的海森矩阵A(X)及其逆矩阵，从而得到X
i
，如果X
i
小于预设值，或者迭代次数达到设定的最大值，则X
i
作为最优解，停止迭代；否则返回方程(3)继续迭代运行。
[0016]进一步地，所述f(Φ,δ)具体包括被描述为式(2)：
[0017][0018]式中，Φ＝[φ1,φ2,φ3]T
，φ1表示SINS与DVL之间的安装方位失准角，φ2表示SINS与DVL之间的安装俯仰失准角，φ3表示SINS与DVL之间的安装滚转失准角，δ为DVL刻度因子误差，X
GPS,i
、Y
GPS,i
分别表示第i次航位推算时GPS数据在导航坐标系下的X和Y坐标值，X
DR,i
、Y
DR,i
分别表示第i次航位推算获得的航位推算位置数据在导航坐标系下的X和Y坐标值，i＝1,2,3
…
N，‖‖表示范数；
[0019]进一步地，所述步骤1具体包括：
[0020]步骤11，通过航位推算公式(1)，计算每一个航位推算位置数据：
[0021][0022]式中，Z
DR,i
表示第i次航位推算获得的所述航位推算位置数据在导航坐标系下的Z坐标值，X
DR,i
‑1、Y
DR,i
‑1、Z
DR,i
‑1分别表示第i
‑
1次航位推算获得的所述航位推算位置数据在导航坐标系下的X、Y和Z坐标值，表示DVL速度，为载体坐标系到导航坐标系的转换矩阵，表示SINS和DVL之间安装误差转换矩阵，I为单位矩阵，Φ
×
表示Φ的反对称矩阵，V
x
、V
y
、V
z
分别表示第i次航位推算时DVL采集到的在DVL坐标系三个方向的速度值，Δt为DVL的数据更新时间；
[0023]步骤12，将所述GPS数据从经纬度转换到导航坐标系下，得到[X
GPS,i
,Y
GPS,i
]T
；
[0024]步骤13，获取f(Φ,δ)。
[0025]本专利技术提供一种基于牛顿迭代法的DVL误差标定方法，其包括：
[0026]采用如上所述的DVL误差优化方法获得Φ和δ的最优估计值；
[0027]根据Φ和δ的最优估计值，完成标定过程。
[0028]本专利技术提供一种DVL误差优化装置，其包括：
[0029]DVL误差损失函数建模子单元，其用于DVL误差损失函数f(Φ,δ)；
[0030]DVL误差损失函数优化子单元，其用于通过牛顿迭代法逼近损失极小值点获得Φ和δ的最优估计值，其具体包括：
[0031]首先，令X＝[Φ,δ]，将所述非线性优化函数f(X)进行在初始点首先，令X＝[Φ,δ]，将所述非线性优化函数f(X)进行在初始点二阶泰勒展开；
[0032]其次，确定迭代方程(3)，其中为A(X)逆矩阵：
[0033][0034]其中，为f(X)梯度，表示为式(4)；表示为式(5)；A和A(X)均为海森矩阵，表示为式(6)：
[0035][0036][0037][0038]式中，为偏导数。
[0039]最后，给定初始值通过i次迭代，求出每次迭代中不同的海森矩阵A(X)及其逆矩阵，从而得到X
i
，如果X
i
小于预设值，或者迭代次数达到设定的最大值，则X
i
作为最优解，停止迭代；否则返回方程(3)继续迭代运行。
[0040]进一步地，所述f(Φ,δ)被描述为式(2)：
[0041][0042]式中，Φ＝[φ1,φ2,φ3]T
，φ1表示SINS与DVL之间的安装方位失准角，φ2表示SINS与DVL之间的安装俯仰失准角，φ3表示SINS与DVL之间的安装滚转失准角，δ为DVL刻度因子误差，X
GPS,i
、Y
GPS,i
分别表示第i次航位推算时GPS数据在导航坐标系下的X和Y坐标值，X
DR,i
、Y
DR,i
分别表示第i次航位推算获得的航位推算位置数据在导航坐标系下的X和Y坐标值，i＝1,2,3
…
N，‖‖表示范数；
[0043]进一步地，所述DVL误差损失函数建模子单元具体包括：
[0044]航位推算子模块，其用于通过航位推算公式(1)，计算每一个航位推算位置数据：
[0045][0046]式中，Z
DR,i
表示第i次航位推算获得的所述航位推算位置数据在导航坐标系下的Z坐标值，X
DR,i
‑1、Y
DR,i
‑1、Z
DR,i...

【技术保护点】

【技术特征摘要】
1.一种DVL误差优化方法，其特征在于，包括：步骤1，构建DVL误差损失函数f(Φ,δ)；步骤2，通过牛顿迭代法逼近损失极小值点获得Φ和δ的最优估计值，其具体包括：步骤21，令X＝[Φ,δ]，将所述非线性优化函数f(X)进行在初始点二阶泰勒展开；步骤22，根据所述步骤21二阶泰勒展开后的结果，确定迭代方程(3)，其中为A(X)逆矩阵：其中，为f(X)梯度，表示为式(4)；表示为式(5)；A和A(X)均为海森矩阵，表示为式(6)：为式(6)：为式(6)：式中，为偏导数；步骤23，给定初始值通过i次迭代，求出每次迭代中不同的海森矩阵A(X)及其逆矩阵，从而得到X
i
，如果X
i
小于预设值，或者迭代次数达到设定的最大值，则X
i
作为最优解，停止迭代；否则返回方程(3)继续迭代运行。2.如权利要求1所述的DVL误差优化方法，其特征在于，所述f(Φ，δ)具体包括被描述为式(2)：式中，Φ＝[φ1，φ2，φ3]
T
，φ1表示SINS与DVL之间的安装方位失准角，φ2表示SINS与DVL之间的安装俯仰失准角，φ3表示SINS与DVL之间的安装滚转失准角，δ为DVL刻度因子误差，X
GPS，i
、Y
GPS，i
分别表示第i次航位推算时GPS数据在导航坐标系下的X和Y坐标值，X
DR，i
、Y
DR，i
分别表示第i次航位推算获得的航位推算位置数据在导航坐标系下的X和Y坐标值，i＝1，2，3
…
N，|| ||表示范数。3.如权利要求1或2所述的DVL误差优化方法，其特征在于，所述步骤1具体包括：
步骤11，通过航位推算公式(1)，计算每一个航位推算位置数据：式中，Z
DR，i
表示第i次航位推算获得的所述航位推算位置数据在导航坐标系下的Z坐标值，X
DR，i
‑1、Y
DR，i
‑1、Z
DR，i
‑1分别表示第i
‑
1次航位推算获得的所述航位推算位置数据在导航坐标系下的X、Y和Z坐标值，表示DVL速度，为载体坐标系到导航坐标系的转换矩阵，表示SINS和DVL之间安装误差转换矩阵，I为单位矩阵，Φ
×
表示Φ的反对称矩阵，V
x
、V
y
、V
z
分别表示第i次航位推算时DVL采集到的在DVL坐标系三个方向的速度值，Δt为DVL的数据更新时间；步骤12，将所述GPS数据从经纬度转换到导航坐标系下，得到[X
GPS，i
，Y
GPS，i
]
T
；步骤13，获取f(Φ，δ)。4.一种基于牛顿迭代法的DVL误差标定方法，其特征在于，包括：采用如权利要求1
‑
3中任一项所述的DVL误差优化方法获得Φ和δ的最优估计值；根据Φ和δ的最优估计值，完成标定过程。5.一种DVL误差优化装置，其特征在于，包括：D...

【专利技术属性】
技术研发人员：秦洪懋，岳昊，王广才，崔庆佳，秦晓辉，胡满江，秦兆博，王晓伟，谢国涛，杨泽宇，徐彪，边有钢，丁荣军，
申请(专利权)人：湖南大学无锡智能控制研究院，
类型：发明
国别省市：