申请/专利权人：湖南大学无锡智能控制研究院;湖南大学

申请日：2022-03-03

公开（公告）日：2024-02-09

公开（公告）号：CN114459507B

主分类号：G01C25/00

分类号：G01C25/00

优先权：

专利状态码：有效-授权

法律状态：2024.02.09#授权;2022.05.27#实质审查的生效;2022.05.10#公开

摘要：本发明实施例公开了一种DVL安装误差标定方法、设备及系统，该方法包括：步骤一，建立DVL的测速模型，步骤二，以李代数表达载体姿态，利用非线性最小二乘法根据该测速模型建立目标函数；步骤三，利用目标函数不断迭代得到极小值，以所述极小值对应的李代数标定所述安装误差角方向余弦阵。本发明实施例中，采用李代数的方法表示旋转矩阵，不存在奇点，不会出现万向节锁死问题，可以实现合并连续变换，适用于一次与二次近似模型，并在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量。

主权项：1.一种DVL安装误差标定方法，其特征在于，包括：步骤1.建立DVL的测速模型，该DVL的测速模型为DVL测量的速度关于刻度因子误差和安装误差的表达式；步骤2.对刻度因子误差进行求解，将DVL测速模型简化为只包含安装误差一项误差的式子；步骤3.列出目标函数，包括：使用李代数将目标函数表示出来，使用李代数扰动模型，运用列文伯格—马夸尔特的非线性优化方法，对目标函数进行迭代，计算出目标最优值，即可得出安装误差方向余弦阵；步骤1中，建立DVL测速模型具体如下：在忽略杆臂矢量的基础上，DVL测速模型表示为： 式中，Vb为载体真实速度；表示DVL实际测量速度；为DVL安装误差角方向余弦阵，反映了DVL坐标系与载体坐标系之间的安装偏角；Kd为DVL刻度因子误差矩阵，步骤2.对刻度因子误差进行求解的方法包括：令DVLx、y、z三轴的刻度因子误差均为kd，则刻度因子误差矩阵Kd＝kdI3×3，将其代入上式后得：式中，I3×3代表3维单位矩阵，两边取模可得：则kd求解方程表示为：为降低速度噪声对kd求解结果的影响，先对多个速度测量值求和的方式滤除高频噪声，然后再求解刻度因子误差，即改进后的kd估计方程为 式中，N为用于求解kd的速度测量值数量；在之后的标定过程中只需要考虑安装误差的影响即可，将上式所求kd代入第二个式子，并令Vb＝kdVb，可得 步骤3中，迭代运算主要分为以下步骤：1设置目标函数；2李代数求导；3进行迭代运算，具体如下：1设置目标函数：利用非线性最小二乘法表达式1： 当等式左边取得最小值时，即与最为接近，即可求得的最优值，令将安装误差角方向余弦阵用李代数来表示，即令以李代数表达载体姿态时，目标函数可以写成： 式中，ξ表示李代数，i表示第i次量测，n为自然数，表示共有n次量测；该目标函数使用李代数代替了安装误差角方向余弦阵，在非线性优化中不断迭代，可以找到目标函数极小值，这个极小值就是全局最优值，通过该目标函数即可求得最优的安装误差角方向余弦阵；2李代数求导：对李代数左乘微小扰动，然后对该扰动求导，称为左扰动模型；运用左扰动模型对旋转矩阵进行扰动，即对expξ^进行左扰动Δexpξ^，看结果相对于扰动的变化率；设左扰动Δexpξ^对应的李代数为φ，当φ为小量时，expφ^＝I+φ^，对φ求导，即： 将李代数求导的结果带入到目标函数中，以x表示ξ，则得到 其中，-expξ^^为李代数的导数，Vb中不含李代数的相关项，所以f'x＝0+expξ^^Vb＝expξ^^Vb式4其中，Vb是可以通过测量得出的数值，对导数曲线形状不产生影响，要想求目标函数的最小值，就要先求fx导数为零时所对应的李代数的值；3进行迭代运算：将fx进行泰勒展开：fx+Δx≈fx+JxTΔx当前的目标是寻找下降增量Δx，使得||fx+Δx||2达到最小，为了求Δx，需要解一个最小二乘问题： 给定初始值x0，x0即第一次求得的安装误差角方向余弦阵对应的李代数的值，对于第k次迭代，求解： 式中，D为非负数对角矩阵，μ将增量限定在预设范围之内，该预设范围称为信赖区域，μ为信赖区域的半径，||DΔxk||2≤μ将增量限定在一个半径为μ的球中，即增量在信赖区域范围内；求解式5得到增量Δxk，判断算法是否收敛：当下降增量Δxk小于阈值时则认为算法收敛，否则继续迭代，直至算法收敛时结束；迭代结束时得到xk，使得f'x＝0，此时得到目标函数的最小值，xk为极小值对应的李代数，相应得到DVL安装误差角方向余弦阵，即确定DVL安装误差；其中，通过下述方式确定u的值：首先定义一个指标ρ来刻画近似的好坏程度，判断在展开点附近的泰勒近似展开效果： 式中，JxT＝expξ^^Vb，Δx为迭代增量，将李代数ξ作为x代入到该式中，即对计算得到的安装误差角方向余弦阵对应的李代数进行判断，对其近似的好坏程度进行判断，如果ρ接近于1，则说明近似结果是好的，如果ρ太小，说明实际减小的值远小于近似减小的值，则认为近似比较差，需要缩小近似范围，反之，如果ρ比较大，则说明实际下降的比预计的更大，放大近似范围；下降增量Δxk取到极小值时，算法收敛，即求Δx极值，根据极值条件将上式对Δx求导，并令导数为零，先展开其平方项： 求上式关于Δx的导数，并令其为零：Jxfx+JxJxTΔx＝0得到：JxJxTΔx＝-Jxfx令JxJxT＝H，-Jxfx＝g，则HΔx＝g，HΔx＝g为高斯牛顿法的增量方程，对迭代式子进行求解得到梯度，而列文伯格—马夸尔特的迭代式子是带着不等式约束的优化问题，用拉格朗日乘子把约束项D、μ放到目标函数中，构成拉格朗日函数： 式中，λ为拉格朗日乘子，令该拉格朗日函数关于Δx导数为零，计算增量线性方程：H+λDTDΔxk＝g当参数λ比较小时，H占主要地位，说明二次近似模型在该范围内是比较好的，列文伯格—马夸尔特方法更接近于高斯牛顿法；另一方面，当λ比较大时，λDTD占据主要地位，列文伯格—马夸尔特方法跟接近于一阶梯度下降法，这说明附近的二次近似不够好；在不断地迭代过程中，使增量Δx不断变小，当增量Δx非常小时，得到一个李代数的值，使得f'x为零，此时找到目标函数的最小值，即目标函数达到最优，此李代数的值即为最优解，从而解出最优的即可求出安装误差角方向余弦阵，完成DVL安装误差标定。

全文数据：

权利要求：

百度查询： 湖南大学无锡智能控制研究院;湖南大学 一种DVL安装误差标定方法、设备及系统

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD