[发明专利]一种DVL安装误差标定方法、设备及系统

权利要求书

1.一种DVL安装误差标定方法，其特征在于，包括：

步骤一，建立DVL的测速模型，

其中，V_b为载体真实速度；表示DVL实际测量速度；为DVL安装误差角方向余弦阵；

步骤二，令以李代数表达载体姿态，利用非线性最小二乘法根据式(1)建立目标函数：

其中，ξ表示李代数，i表示第i次量测，n为自然数；

步骤三，利用式(2)不断迭代得到极小值，以所述极小值对应的李代数标定所述安装误差角方向余弦阵。

2.根据权利要求1所述的方法，其特征在于，步骤一之前还包括：

建立DVL测速模型为：

其中，K_d为DVL刻度因子误差矩阵；

通过预设方式得到K_d的值，令将式(3)变更为式(1)。

3.根据权利要求2所述的方法，其特征在于，通过下述方式得到所述K_d的值：

令DVL x、y、z三轴的刻度因子误差均为k_d，则刻度因子误差矩阵K_d＝k_dI_3×3，将其代入式(3)后得

其中，I_3×3代表3维单位矩阵；上式两边取模可得

根据向量与方向余弦阵相乘不改变自身模值，将省去后依然成立，得到

即

其中，N为用于求解k_d的速度测量值数量。

4.根据权利要求1所述的方法，其特征在于，对式(2)中的李代数求导，得到：

f'(x)＝(exp(ξ^))^V_b 式(4)

其中，x＝ξ，以f'(x)＝0时的x取值作为所述极小值对应的李代数。

5.根据权利要求4所述的方法，其特征在于，通过迭代方式获取f'(x)＝0时的x取值：

给定初始值x₀，以及初始优化半径μ，对于第k次迭代，求解：

其中，Δx_k为第k次迭代的迭代增量，J(x)^T＝(exp(ξ^))^V_b，D为非负数对角矩阵，μ是预设值，根据式(5)得到Δx_k：

若Δx_k满足预设阈值条件，则以x_k为所述f'(x)＝0时的x取值，结束迭代；否则令x_k+1＝x_k+Δx_k，继续迭代。

6.根据权利要求5所述的方法，其特征在于，还包括：

定义指标ρ：

对于第k次迭代的结果计算ρ，如果ρ满足预设阈值条件，则令x_k+1＝x_k+Δx_k；否则，若ρ预设极大值，则增加μ的值，若ρ预设极小值，则减小u的值，并再次执行第k次迭代。

7.根据权利要求6所述的方法，其特征在于，若ρ预设极大值，则设置μ＝2μ；若ρ预设极小值，则设置μ＝0.5μ。

8.一种DVL安装误差标定设备，其特征在于，包括：

模型建立单元，用于建立DVL的测速模型，

其中，V_b为载体真实速度；表示DVL实际测量速度；为DVL安装误差角方向余弦阵；

安装误差确定单元，用于:

以李代数表达载体姿态，使用所述李代数表示所述DVL安装误差角方向余弦阵，根据式(1)建立目标函数：

其中，ξ表示所述李代数，i表示第i次量测，n为自然数；

利用式(2)不断迭代得到极小值，以所述极小值对应的李代数标定所述安装误差角方向余弦阵。