基于迭代EKF的SINS/USBL安装角估计方法

基于迭代ekf的sins/usbl安装角估计方法
技术领域
1.本发明属于水下机器人导航定位技术领域，具体涉及基于迭代ekf的sins/usbl安装角估计方法。


背景技术：

2.随着人类对资源的需求愈加剧烈，陆地资源已经无法满足人类的发展需要，海洋中蕴含着巨大的资源，是一座宝库。但是人工探索海洋成本高，危险性大，而水下自主潜航器（autonomous underwater vehicle, auv）的出现可以代替人工，对海洋环境进行探索开发。
3.auv的运行需要时刻了解自身的位置，速度，姿态信息，但在水下gps信号无法有效传递，而捷联惯性导航系统（strapdown inertial navigation system, sins）和超短基线定位系统（ultra-short baseline, usbl）可以帮助机器人进行水下的导航定位。由于sins和usbl的设备安装存在偏差，会导致导航系统的精度下降，甚至会威胁auv的航行安全，需要对二者的安装误差角进行估计，来提高导航系统的有效性，因此准确估计安装误差角是研究sins/usbl导航系统的重点。同时，水下机器人航行时的环境变化对安装角估计技术也会造成较大影响。


技术实现要素：

4.针对上述问题，本发明提出基于迭代ekf（扩展卡尔曼滤波）的sins/usbl安装角估计方法，进一步提高了算法的收敛性和估计精度。
5.为达到上述目的，本发明的技术方案如下：基于迭代ekf的sins/usbl安装角估计方法，该方法基于水上/水下载体、应答器、以及安装在所述载体上的一体化装置，所述一体化装置上安装有imu（惯性测量单元）、sins（捷联惯性导航系统）、usbl（超短基线系统）；其中usbl输出信息，和分别表示usbl输出的两个方位角信息，表示usbl输出的斜距信息；用上标表示导航坐标系，简称为系，应答器在系下的绝对位置通过长基线的方式获得；gps实时测量的卫星天线安装点在系下的绝对位置已知；载体姿态由imu测量得到， 、、分别表示横摇角、纵摇角和航向角；该方法具体包括如下步骤：s1.对水下机器人导航系统模型进行构建；s2.使用基于迭代的扩展卡尔曼滤波算法进行状态更新；s3.使用lm算法优化调整迭代的扩展卡尔曼滤波算法的预测协方差矩阵；s4.对量测噪声方差阵进行优化；s5.对系统噪声方差阵进行优化；s6.进行基于迭代的扩展卡尔曼滤波算法进行量测更新。
6.进一步地，步骤s1的具体方法是：
系统的状态方程为：，其中，是系统k时刻的状态，是系统k-1时刻的状态；为系统噪声矩阵，为系统噪声向量；为系统状态转移矩阵；；将安装误差角与杆臂误差作为状态量，则状态方程表示为，其中，，、、分别表示x、y、z三轴下的安装误差角；，、、分别表示x、y、z三轴下的杆臂误差；建立系统量测方程：，其中，为系统测量值，为量测的非线性函数矩阵,为量测噪声向量；由于标定误差的观测方程有如下的非线性关系：，上式显示了载体位置、应答器位置、载体姿态、标定误差()和usbl输出的方位角及斜距之间的关系；表示一体化装置的杆臂误差；用上标a表示声学基阵坐标系，简称a系，表示在a系下的三轴测量值；通过usbl解算的imu的位置表示为，，其中，用下标b表示载体坐标系，简称b系，表示b系到系的姿态转移矩阵；是a系到b系的姿态转移矩阵；是根据usbl定位原理解算出应答器在a系中的相对位置；是根据usbl定位原理解算出应答器在u系中的相对位置；u系表示超短基线基座坐标系；考虑标定误差后，应答器在a系下的位置表示为。
7.进一步地，步骤s2的具体方法是：基于迭代的扩展卡尔曼滤波算法的时间更新公式如下：
，其中，表示k-1时刻系统状态估计值，表示k时刻状态预测值；表示状态转移矩阵的一阶泰勒展开结果，为k-1时刻系统噪声方差阵；表示状态估计误差的均方误差阵；表示误差协方差矩阵的先验估计。
8.进一步地，步骤s3的具体方法是在步骤s2所述基于迭代的扩展卡尔曼滤波算法的每一次迭代过程中，寻找合适的阻尼参数来修正状态量的误差协方差矩阵，即对其进行修正的式子为：，，其中，为修正后的状态量的误差协方差矩阵；i为单位矩阵；然后再用这个经过修正后的状态量的误差协方差矩阵来参与后面的迭代更新。
9.进一步地，阻尼参数。
10.进一步地，步骤s4的具体方法是将sage-husa滤波原理中对量测噪声方差阵的优化思想引入到迭代的扩展卡尔曼滤波算法中，sage-husa自适应卡尔曼滤波算法的步骤如下：，，，其中，为k-1时刻量测噪声期望估计值；为k-1时刻量测噪声方差阵；为k时刻的观测矩阵；为残差；为量测噪声方差阵；为k时刻的量测噪声调节因子，且，为遗忘因子，且，上标t表示矩阵的转置。
11.进一步地，遗忘因子取值范围在之内。
12.进一步地，步骤s5所述对系统噪声方差阵进行优化，公式如下：，，，其中，为实际的运动状态协方差矩阵；为系统初始化的运动状态噪声协方差矩阵；为增量；表示卡尔曼滤波增益；为k时刻的观测矩阵；表示观测残差；为实际的误差协方差矩阵。
13.进一步地，步骤s6所述迭代的扩展卡尔曼滤波算法进行量测更新公式如下所示：
14.式中上角标i为量测更新迭代次数，、和 分别为第i次迭代的增益矩阵、状态估计值、状态估计协方差矩阵；表示第i+1次迭代的状态估计值；表示k时刻量测的非线性函数矩阵；对于非线性量测矩阵，为第i次迭代的量测矩阵，其表达式如下所示：，式中，表示量测非线性函数；表示状态向量。
15.与现有技术相比，本发明具有如下优点：(1) 本发明针对sins和usbl安装角估计问题，在传统ekf算法的基础上提出了迭代ekf方法，进一步提高了算法的估计精度;(2) 本发明针对算法的预测协方差矩阵收敛速度不够的问题，引入了lm算法，提高了算法的收敛性和估计精度;(3) 本发明针对滤波算法中噪声矩阵无法估计的问题，分别设计了量测噪声方差阵优化策略和系统噪声方差阵优化策略，进一步提高了复杂环境下系统的适应性。
附图说明
16.图1为本发明所描述的基于迭代ekf的sins/usbl安装角估计方法流程图。
具体实施方式
17.下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
18.如图1所示，本实施例的基于迭代ekf的sins/usbl安装角估计方法，该方法基于水上/水下载体、应答器、以及安装在所述载体上的一体化装置，所述一体化装置上安装有imu（微机电系统）、sins、usbl；其中usbl输出信息，和分别表示usbl输出的两个方位角信息，表示usbl输出的斜距信息；用上标表示导航坐标系，简称为系，应答器在系下的绝对位置通过长基线的方式获得；gps实时测量的卫星天线安装点在系下的绝对位置已知；载体姿态由imu测量得到， 、、分别表示横摇角、纵摇角和航向角；该方法具体包括如下步骤：s1.对水下机器人导航系统模型进行构建；
具体方法是：系统的状态方程为：，其中，是系统k时刻的状态，是系统k-1时刻的状态；为系统噪声矩阵，为系统噪声向量；为系统状态转移矩阵；；将安装误差角与杆臂误差作为状态量，则状态方程表示为，其中，，、、分别表示x、y、z三轴下的安装误差角；，、、分别表示x、y、z三轴下的杆臂误差；建立系统量测方程：，其中，为系统测量值，为量测的非线性函数矩阵,为量测噪声向量；由于标定误差的观测方程有如下的非线性关系：，上式显示了载体位置、应答器位置、载体姿态、标定误差()和usbl输出的方位角及斜距之间的关系；表示一体化装置的杆臂误差；用上标a表示声学基阵坐标系，简称a系，表示在a系下的三轴测量值；通过usbl解算的imu的位置表示为，，其中，用下标b表示载体坐标系，简称b系，表示b系到系的姿态转移矩阵；是a系到b系的姿态转移矩阵；是根据usbl定位原理解算出应答器在a系中的相对位置；是根据usbl定位原理解算出应答器在u系中的相对位置；u系表示超短基线基座坐标系；考虑标定误差后，应答器在a系下的位置表示为。
19.s2.使用基于迭代的扩展卡尔曼滤波算法进行状态更新；更新公式如下：
，其中，表示k-1时刻系统状态估计值，表示k时刻状态预测值；表示状态转移矩阵的一阶泰勒展开结果，为k-1时刻系统噪声方差阵；表示状态估计误差的均方误差阵；表示误差协方差矩阵的先验估计。
20.s3.使用lm算法优化调整迭代的扩展卡尔曼滤波算法的预测协方差矩阵；本实施例中采用lm算法来优化迭代ekf算法。利用lm算法优化调整迭代ekf算法的预测协方差矩阵，以保证算法的估算结果误差具有收敛性。
21.主要思想就是在迭代eke算法的每一次迭代过程中，寻找合适的阻尼参数来修正状态量的误差协方差矩阵。即对其进行修正的式子为：，，其中，为修正后的状态量的误差协方差矩阵；i为单位矩阵；然后再用这个经过修正后的状态量的误差协方差矩阵来参与后面的迭代更新。本实施例中阻尼参数。
22.s4.对量测噪声方差阵进行优化；本实施例将sage-husa滤波原理中对量测噪声方差阵的优化思想引入到迭代的扩展卡尔曼滤波算法中，sage-husa自适应卡尔曼滤波算法的步骤如下：，，，其中，为k-1时刻量测噪声期望估计值；为k-1时刻量测噪声方差阵；为k时刻的观测矩阵；为残差；为量测噪声方差阵；为k时刻的量测噪声调节因子，且，为遗忘因子，且，上标t表示矩阵的转置。本实施例中遗忘因子取值范围在之内。
23.s5.对系统噪声方差阵进行优化；考虑到系统状态噪声往往会随着运动位置的变化变得不确定。本实施例在上述量测噪声方差阵优化的基础上提出一种系统噪声方差阵的优化方法，优化公式如下：，，，其中，为实际的运动状态协方差矩阵；为系统初始化的运动状态噪声协方差矩
阵；为增量；表示卡尔曼滤波增益；为k时刻的观测矩阵；表示观测残差；为实际的误差协方差矩阵。
24.s6.进行基于迭代的扩展卡尔曼滤波算法进行量测更新，更新公式如下所示：，式中上角标i为量测更新迭代次数，、和 分别为第i次迭代的增益矩阵、状态估计值、状态估计协方差矩阵；表示第i+1次迭代的状态估计值；表示k时刻量测的非线性函数矩阵；对于非线性量测矩阵，为第i次迭代的量测矩阵，其表达式如下所示：，式中，表示量测非线性函数；表示状态向量。
25.为了验证本发明方法的有效性，开展基于迭代ekf的sins/usbl安装角估计方法实验。试验设备包括：试验船、imu设备、usbl设备等。试验结果如表1和表2所示。
26.表1不同方法的安装角估计结果
27.表2不同方法的杆臂估计结果
28.其中表1为不同方法的安装角估计结果，从表1中可以看出，在安装角估计结果来看，本发明方法在x, y, z三个方向的安装角估计结果均优于ekf方法，本发明的安装角估计结果更接近真值。表2为不同方法的杆臂估计结果，从表2中可以看出，在杆臂估计结果来看，本发明方法在x, y, z三个方向的杆臂估计结果均优于ekf方法，本发明的杆臂估计结果更接近真值。
29.需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

技术特征：
1.一种基于迭代ekf的sins/usbl安装角估计方法，该方法基于水上/水下载体、应答器、以及安装在所述载体上的一体化装置，所述一体化装置上安装有imu、sins、usbl；其中usbl输出信息 ，和分别表示usbl输出的两个方位角信息，表示usbl输出的斜距信息；用上标表示导航坐标系，简称为系，应答器在系下的绝对位置通过长基线的方式获得；gps实时测量的卫星天线安装点在系下的绝对位置已知；载体姿态，由imu测量得到， 、、分别表示横摇角、纵摇角和航向角；其特征在于，该方法具体包括如下步骤：s1.对水下机器人导航系统模型进行构建；s2.使用基于迭代的扩展卡尔曼滤波算法进行状态更新；s3.使用lm算法优化调整迭代的扩展卡尔曼滤波算法的预测协方差矩阵；s4.对量测噪声方差阵进行优化；s5.对系统噪声方差阵进行优化；s6.进行基于迭代的扩展卡尔曼滤波算法进行量测更新。2.根据权利要求1所述的基于迭代ekf的sins/usbl安装角估计方法，其特征在于，步骤s1的具体方法是：系统的状态方程为：，其中，是系统k时刻的状态，是系统k-1时刻的状态；为系统噪声矩阵，为系统噪声向量；为系统状态转移矩阵；；将安装误差角与杆臂误差作为状态量，则状态方程表示为，其中，，、、分别表示x、y、z三轴下的安装误差角；，、、分别表示x、y、z三轴下的杆臂误差；建立系统量测方程：，其中，为系统测量值，为量测的非线性函数矩阵, 为量测噪声向量；由于标定误差的观测方程有如下的非线性关系：，
上式显示了载体位置、应答器位置、载体姿态、标定误差()和usbl输出的方位角及斜距之间的关系；表示一体化装置的杆臂误差；用上标a表示声学基阵坐标系，简称a系，表示在a系下的三轴测量值；通过usbl解算的imu的位置表示为，，其中，用下标b表示载体坐标系，简称b系，表示b系到系的姿态转移矩阵；是a系到b系的姿态转移矩阵；是根据usbl定位原理解算出应答器在a系中的相对位置；是根据usbl定位原理解算出应答器在u系中的相对位置；u系表示超短基线基座坐标系；考虑标定误差后，应答器在a系下的位置表示为。3.根据权利要求1所述的基于迭代ekf的sins/usbl安装角估计方法，其特征在于，步骤s2的具体方法是：基于迭代的扩展卡尔曼滤波算法的时间更新公式如下：，其中，表示k-1时刻系统状态估计值，表示k时刻状态预测值；表示状态转移矩阵的一阶泰勒展开结果，为k-1时刻系统噪声方差阵；表示状态估计误差的均方误差阵；表示误差协方差矩阵的先验估计。4.根据权利要求3所述的基于迭代ekf的sins/usbl安装角估计方法，其特征在于，步骤s3的具体方法是在步骤s2所述基于迭代的扩展卡尔曼滤波算法的每一次迭代过程中，寻找合适的阻尼参数来修正状态量的误差协方差矩阵，即对其进行修正的式子为：，，其中，为修正后的状态量的误差协方差矩阵；i为单位矩阵；然后再用这个经过修正后的状态量的误差协方差矩阵来参与后面的迭代更新。5.根据权利要求4所述的基于迭代ekf的sins/usbl安装角估计方法，其特征在于，阻尼参数。6.根据权利要求2所述的基于迭代ekf的sins/usbl安装角估计方法，其特征在于，步骤s4的具体方法是将sage-husa滤波原理中对量测噪声方差阵的优化思想引入到迭代的扩展卡尔曼滤波算法中，sage-husa自适应卡尔曼滤波算法的步骤如下：
，，，其中，为k-1时刻量测噪声期望估计值；为k-1时刻量测噪声方差阵；为k时刻的观测矩阵；为残差；为量测噪声方差阵；为k时刻的量测噪声调节因子，且，为遗忘因子，且，上标t表示矩阵的转置。7.根据权利要求6所述的基于迭代ekf的sins/usbl安装角估计方法，其特征在于，遗忘因子取值范围在之内。8.根据权利要求6所述的基于迭代ekf的sins/usbl安装角估计方法，其特征在于，步骤s5所述对系统噪声方差阵进行优化，公式如下：，，，其中，为实际的运动状态协方差矩阵；为系统初始化的运动状态噪声协方差矩阵；为增量；表示卡尔曼滤波增益；为k时刻的观测矩阵；表示观测残差；为实际的误差协方差矩阵。9.根据权利要求8所述的基于迭代ekf的sins/usbl安装角估计方法，其特征在于，步骤s6所述迭代的扩展卡尔曼滤波算法进行量测更新公式如下所示：，式中上角标i为量测更新迭代次数，、和 分别为第i次迭代的增益矩阵、状态估计值、状态估计协方差矩阵；表示第i+1次迭代的状态估计值；表示k时刻量测的非线性函数矩阵；对于非线性量测矩阵，为第i次迭代的量测矩阵，其表达式如下所示：，式中，表示量测非线性函数；表示状态向量。

技术总结
本发明公开了一种基于迭代EKF（扩展卡尔曼滤波）的SINS/USBL安装角估计方法，属于水下机器人导航技术领域。本发明将迭代EKF算法用于SINS/USBL安装角估计方法中，整个过程主要步骤包括：模型构建、基于迭代EKF的状态更新、基于LM算法的预测协方差矩阵优化，量测噪声方差阵优化，系统噪声方差阵优化以及基于迭代EKF的量测更新。本发明可以解决由于水下机器人捷联惯性导航系统与超短基线定位系统同载体的坐标系不同，而导致水下机器人导航精度下降的问题，提高估计SINS/USBL安装角的能力。相比于传统SINS/USBL安装角估计方法，本方法具有较高的鲁棒性和估计精度。有较高的鲁棒性和估计精度。有较高的鲁棒性和估计精度。


技术研发人员：王迪 王冰 黄浩乾 王汝鹏 孙世安
受保护的技术使用者：河海大学
技术研发日：2023.08.16
技术公布日：2023/9/20