CN116702479A - 一种水下航行器未知输入与位置估计方法及系统 - Google Patents

Abstract

本申请公开了一种水下航行器未知输入与位置估计方法及系统，方法包括：获取水下航行器的航行数据，并基于航行数据构建系统状态空间模型；对系统状态空间模型中的量测方程进行第一次优化，得到优化后量测方程，并基于优化后量测方程构建延时递推框架；基于延时递推框架，搭建未知非线性控制输入和延时到达观测信息之间的递推关联；基于递推关联得到未知非线性控制输入估计值；基于未知非线性控制输入估计值，得到位置状态后验估计表达式和第一估计误差协方差表达式；对后验估计表达式进行第二次优化，得到最终位置状态表达式和第二估计误差协方差表达式。本申请缓解观测值中的延时偏差对未知输入估计及位置估计的影响，提高水下航行器的定位精度。

Description

一种水下航行器未知输入与位置估计方法及系统

技术领域

本申请属于水下航行器协同定位技术领域，具体涉及一种水下航行器未知输入与位置估计方法及系统。

背景技术

受全球定位系统信号在水下的快速衰减以及水下恶劣的声学信道条件的影响，自主水下航行器自身位置估计一直是一个艰巨的任务。与传统的惯性系统和声基线系统相比，协同定位系统具有精度高、航程远、成本低、鲁棒性好的优点，可以大大增强多水下航行器系统的协同作业能力。然而，信息处理与传输所消耗的时间导致协同定位所需观测信息存在时变时间延时，此外，协同定位系统传感器如罗经、多普勒计程仪等受自身结构、工作原理及水下环境因素如海洋生物障碍、海底强吸声地质、深海沟等影响可能出现故障或失效。以上恶劣条件的出现均可能导致定位精度降低甚至估计结果无效。

罗经和多普勒计程仪故障都会导致无法获得有效的控制输入，从而导致先验信息的不准确，这必然会影响定位精度。目前，现有算法可以在实时外部观测的辅助下完成对控制输入的估计，如经典的增广卡尔曼滤波器、经过状态解耦优化的滤波器等。然而，这些方法依赖于相对准确的观测信息，而水下通信延时的存在导致观测数据存在延时偏差，此偏差的存在使得非线性控制输入估计的准确性无法得到保证，进而对位置估计产生不利影响。

发明内容

本申请旨在解决现有技术的不足，提出一种水下航行器未知输入与位置估计方法及系统，能够有效缓解观测值中的延时偏差对未知输入估计及位置估计的影响，提高水下航行器在非理想作业环境中的定位精度。

为实现上述目的，本申请提供了如下方案：

一种水下航行器未知输入与位置估计方法，包括以下步骤：

获取水下航行器的航行数据，并基于所述航行数据构建系统状态空间模型；

对所述系统状态空间模型中的量测方程进行第一次优化，得到优化后量测方程，并基于所述优化后量测方程构建延时递推框架；

基于所述延时递推框架，搭建未知非线性控制输入和延时到达观测信息之间的递推关联；

基于所述递推关联得到未知非线性控制输入估计值；

基于所述未知非线性控制输入估计值，得到位置状态后验估计表达式和第一估计误差协方差表达式；

对所述后验估计表达式进行第二次优化，得到最终位置状态表达式和第二估计误差协方差表达式。

优选的，所述航行数据包括：位置数据、速度数据和航向数据。

优选的，所述系统状态空间模型的构建方法包括：

基于所述位置数据、所述速度数据和所述航向数据构建离散时间模型：

其中，和/>分别为前向和右向速度，/>为航向，χ_m，[k]＝[a_m，[k]，b_m，[k]]为领航者在k时刻的位置坐标，χ_[k]＝[a_[k]，b_[k]]^T为跟随者在k时刻的位置坐标，γ_[k]为领航者与跟随者之间的相对距离，(·)^T为转置操作，Δt为采样周期，q_[k-l]＝[q_x，[k-l]，q_y，[k-l]]^T为过程噪声，r_[k]为量测噪声；

基于所述离散时间模型构建所述系统状态空间模型：

其中，状态转移矩阵Φ_[k-1]为二维单位矩阵，Γ_[k-1]＝diag(Δt，Δt)和u_[k-1]＝[u_x，u_y]^T分别为控制输入，且有和为量测方程。

优选的，所述优化后量测方程为：

其中，τ为延时采样时刻，C_[k-τ]＝(P_{xy，[k-τ，k-τ-1]})^T(P_{yy，[k-τ，k-τ-1]})^-1为观测矩阵，γ_[k-τ]为k-τ时刻相对距离量测值，为k-τ时刻相对距离预测值，/>为k-τ时刻跟随者位置的一步预测估计值；

η_[k-τ]＝r_[k-τ]+l_[k-τ]包含量测噪声与线性化误差两部分，l_[k-τ]为k-τ时刻的统计线性化误差，线性化误差的协方差表示为：

Θ_[k-τ]＝E(l_[k-τ]l_[k-τ] ^T)

＝P_{yy，[k-τ，k-τ-1]}-(P_{xy，[k-τ，k-τ-1]})^T(P_{xx，[k-τ，k-τ-1]})^-1P_{xy，[k-τ，k-τ-1]}-R_[k-τ]

其中，P_{yy，[k-τ，k-τ-1]}和P_{xy，[k-τ，k-τ-1]}分别为自协方差与互协方差矩阵，R_[k-τ]＝E(r_[k-τ]r_[k-τ] ^T)为k-τ时刻量测噪声协方差矩阵。

优选的，所述延时递推框架为：

其中，

为延时转化为的偏差：

Υ_[k]中的延时带来的附加项

优选的，所述递推关联包括：

其中，

进一步得到：

其中，P_{xx，[k，k-1]}为状态的一步预测误差协方差，

κ_[k]＝E(η_[k-τ]η_[k-τ] ^T)＝R_[k-τ]+Θ_[k-τ]，

L_[k]为T_[k]经过Cholesky分解后的下三角矩阵。

优选的，所述未知非线性控制输入估计值的计算方法包括：

基于所述递推关联构建递推关联的线性回归表达式：

基于所述线性回归表达式计算所述未知非线性控制输入估计值：

其中，

优选的，所述后验估计表达式为：

其中，K_[k]为增益矩阵；

所述第一估计误差协方差表达式为：

优选的，所述第二次优化的方法包括：舍去所述后验估计表达式中与未知输入相关项，得到所述最终位置状态表达式和所述第二估计误差协方差表达式；

所述最终位置状态表达式为：

所述第二估计误差协方差表达式为：

其中，

本申请还提供了一种水下航行器未知输入与位置估计系统，包括：模型构建模块、框架构建模块、关联构建模块、估计模块、后验状态模块和位置估计模块；

所述模型构建模块用于获取水下航行器的航行数据，并基于所述航行数据构建系统状态空间模型；

所述框架构建模块用于对所述系统状态空间模型中的量测方程进行第一次优化，得到优化后量测方程，并基于所述优化后量测方程构建延时递推框架；

所述关联构建模块用于基于所述延时递推框架，搭建未知非线性控制输入和延时到达观测信息之间的递推关联；

所述估计模块用于基于所述递推关联得到未知非线性控制输入估计值；

所述后验状态模块用于基于所述未知非线性控制输入估计值，得到位置状态后验估计表达式和第一估计误差协方差表达式；

所述位置估计模块用于对所述后验估计表达式进行第二次优化，得到最终位置状态表达式和第二估计误差协方差表达式。

与现有技术相比，本申请的有益效果为：

(1)本申请将航向角、前向速度和右向速度建模为非线性单元，降低了估计模型的复杂性；

(2)本申请通过重构观测矩阵和测量噪声方差矩阵对测量方程进行修正，建立了基于线性回归的延迟框架，从模型角度削弱了非实时观测对未知输入估计的影响；

(3)本申请构建了延迟到达量测与非线性控制输入估计之间的递推关联，并进一步推导了不受非线性输入干扰的后验估计表达形式，有效削弱未知输入对位置估计的影响，提高了系统的鲁棒性。

附图说明

为了更清楚地说明本申请的技术方案，下面对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例的方法流程示意图；

图2为本申请实施例的系统结构示意图；

图3为本申请实施例的协同定位系统多潜航器实际航行轨迹图；

图4为本申请实施例的定位误差比较图；

图5为本申请实施例的未知输入估计误差比较图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

为使本申请的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本申请作进一步详细的说明。

实施例一

在本实施例中，如图1所示，一种水下航行器未知输入与位置估计方法，其包括以下步骤：

S1.获取水下航行器的航行数据，并基于航行数据构建系统状态空间模型。

其中，航行数据包括：位置数据、速度数据和航向数据。

系统状态空间模型的构建方法包括：

基于位置数据、速度数据和航向数据构建离散时间模型：

其中，为速度向量，/>和/>分别为前向和右向速度，/>为航向，χ_m，[k]＝[a_m，[k]，b_m，[k]]为领航者在k时刻的位置坐标，χ_[k]＝[a_[k]，b_[k]]^T为跟随者在k时刻的位置坐标，γ_[k]为领航者与跟随者之间的相对距离，(·)^T为转置操作，Δt为采样周期，q_[k-l]＝[q_x，[k-l]，q_y，[k-l]]^T为过程噪声，r_[k]为量测噪声；

基于离散时间模型构建系统状态空间模型：

其中，状态转移矩阵Φ_[k-1]为二维单位矩阵，Γ_[k-1]＝diag(Δt，Δt)和u_[k-1]＝[u_x，u_y]^T分别为控制输入，且有和为量测方程，且E(q_[k]r_[l] ^T))＝0。

S2.对系统状态空间模型中的量测方程进行第一次优化，得到优化后量测方程，并基于优化后量测方程构建延时递推框架。

优化后量测方程为：

其中，τ为延时采样时刻，C_[k-τ]＝(P_{xy，[k-τ，k-τ-1]})^T(P_{yy，[k-τ，k-τ-1]})^-1为观测矩阵，Υ_[k-τ]为k-τ时刻相对距离量测值，为k-τ时刻相对距离预测值，/>为k-τ时刻跟随者位置的一步预测估计值；

η_[k-τ]＝r_[k-τ]+l_[k-τ]包含量测噪声与线性化误差两部分，l_[k-τ]为k-τ时刻的统计线性化误差，线性化误差的协方差表示为：

Θ_[k-τ]＝E(l_[k-τ]l_[k-τ] ^T)

＝P_{yy，[k-τ，k-τ-1]}-(P_{xy，[k-τ，k-τ-1]})^T(P_{xx，[k-τ，k-τ-1]})^-1P_{xy，[k-τ，k-τ-1]}-R_[k-τ]

其中，P_{yy，[k-τ，k-τ-1]}和P_{xy，[k-τ，k-τ-1]}分别为自协方差与互协方差矩阵，其表达形式与容积卡尔曼滤波中相同，R_[k-τ]＝E(r_[k-τ]r_[k-τ] ^T)为k-τ时刻量测噪声协方差矩阵。

延时递推框架为：

其中，为修正后的量测噪声协方差矩阵，其中包含了/>

为延时转化为的偏差：

γ_[k]中的延时带来的附加项

S3.基于延时递推框架，搭建未知非线性控制输入和延时到达观测信息之间的递推关联。

递推关联包括：

其中，

进一步得到：

其中，P_{xx，[k，k-1]}为状态的一步预测误差协方差，

κ_[k]＝E(η_[k-τ]η_[k-τ] ^T)＝R_[k-τ]+Θ_[k-τ]，

L_[k]为T_[k]经过Cholesky分解后的下三角矩阵。

S4.基于递推关联得到未知非线性控制输入估计值。

未知非线性控制输入估计值的计算方法包括：

基于递推关联构建递推关联的线性回归表达式：

基于线性回归表达式计算未知非线性控制输入的加权最小二乘估计值，即未知非线性控制输入估计值：

其中，

S5.基于未知非线性控制输入估计值，得到位置状态后验估计表达式和第一估计误差协方差表达式。

后验估计表达式为：

其中，K_[k]为增益矩阵；

第一估计误差协方差表达式为：

S6.对后验估计表达式进行第二次优化，得到最终位置状态表达式和第二估计误差协方差表达式。

第二次优化的方法包括：舍去后验估计表达式中与未知输入相关项，得到最终位置状态表达式和第二估计误差协方差表达式；

最终位置状态表达式为：

第二估计误差协方差表达式为：

其中，

实施例二

在本实施例中，如图2所示，一种水下航行器未知输入与位置估计系统，包括：模型构建模块、框架构建模块、关联构建模块、估计模块、后验状态模块和位置估计模块；

模型构建模块用于获取水下航行器的航行数据，并基于航行数据构建系统状态空间模型；

框架构建模块用于对系统状态空间模型中的量测方程进行第一次优化，得到优化后量测方程，并基于优化后量测方程构建延时递推框架；

关联构建模块用于基于延时递推框架，搭建未知非线性控制输入和延时到达观测信息之间的递推关联；

估计模块用于基于递推关联得到未知非线性控制输入估计值；

后验状态模块用于基于未知非线性控制输入估计值，得到位置状态后验估计表达式和第一估计误差协方差表达式；

位置估计模块用于对后验估计表达式进行第二次优化，得到最终位置状态表达式和第二估计误差协方差表达式。

实施例三

在本实施例中，如图3所示，为协同定位系统多潜航器实际航行轨迹图，此处考虑2个领航者1个跟随者的经典场景，在协同定位过程中，通过水声设备每隔一段时间测量领航者与跟随者之间的相对距离。两个领航者分别位于随从者两侧，这种队形可以提高系统的可观测性。图4和图5分别为协同定位误差比较图和未知输入估计误差比较图，可以看出，当量测信息存在延时偏差时，RCKF、RCKF、MBCDCKF定位精度不佳，这是由于三者在估计过程中需要相对准确的观测值，而DUICKF在搭建起非实时观测值与未知输入之间的递推关联后，有效的削弱了未知输入估计对观测信息实时性的依赖程度，提高了算法在观测值存在延时偏差下的未知输入估计精度，使得位置估计精度得以提高。

以上所述的实施例仅是对本申请优选方式进行的描述，并非对本申请的范围进行限定，在不脱离本申请设计精神的前提下，本领域普通技术人员对本申请的技术方案做出的各种变形和改进，均应落入本申请权利要求书确定的保护范围内。

Claims (10)

1.一种水下航行器未知输入与位置估计方法，其特征在于，包括以下步骤：

获取水下航行器的航行数据，并基于所述航行数据构建系统状态空间模型；

对所述系统状态空间模型中的量测方程进行第一次优化，得到优化后量测方程，并基于所述优化后量测方程构建延时递推框架；

基于所述延时递推框架，搭建未知非线性控制输入和延时到达观测信息之间的递推关联；

基于所述递推关联得到未知非线性控制输入估计值；

基于所述未知非线性控制输入估计值，得到位置状态后验估计表达式和第一估计误差协方差表达式；

对所述后验估计表达式进行第二次优化，得到最终位置状态表达式和第二估计误差协方差表达式。

2.根据权利要求1所述一种水下航行器未知输入与位置估计方法，其特征在于，所述航行数据包括：位置数据、速度数据和航向数据。

3.根据权利要求2所述一种水下航行器未知输入与位置估计方法，其特征在于，所述系统状态空间模型的构建方法包括：

基于所述位置数据、所述速度数据和所述航向数据构建离散时间模型：

其中，和/>分别为前向和右向速度，/>为航向，χ_m，[k]＝[a_m，[k]，b_m，[k]]为领航者在k时刻的位置坐标，χ_[k]＝[a_[k]，b_[k]]^T为跟随者在k时刻的位置坐标，γ_[k]为领航者与跟随者之间的相对距离，(·)^T为转置操作，Δt为采样周期，q_[k-1]＝[q_x，[k-1]，q_y，[k-1]]^T为过程噪声，r_[k]为量测噪声；

基于所述离散时间模型构建所述系统状态空间模型：

其中，状态转移矩阵Φ_[k-1]为二维单位矩阵，Γ_[k-1]＝diag(Δt，Δt)和u_[k-1]＝[u_x，u_y]^T分别为控制输入，且有和为量测方程。

4.根据权利要求3所述一种水下航行器未知输入与位置估计方法，其特征在于，所述优化后量测方程为：

其中，τ为延时采样时刻，C_[k-τ]＝(P_{xy，[k-τ，k-τ-1]})^T(P_{yy，[k-τ，k-τ-1]})^-1为观测矩阵,Υ_[k-τ]为k-τ时刻相对距离量测值，为k-τ时刻相对距离预测值，/>为k-τ时刻跟随者位置的一步预测估计值；

η_[k-τ]＝r_[k-τ]+l_[k-τ]包含量测噪声与线性化误差两部分，l_[k-τ]为k-τ时刻的统计线性化误差，线性化误差的协方差表示为：

Θ_[k-τ]＝E(l_[k-τ]l_[k-τ] ^T)

＝P_{yy，[k-τ，k-τ-1]}-(P_{xy，[k-τ，k-τ-1]})^T(P_{xx，[k-τ，k-τ-1]})^-1P_{xy，[k-τ，k-τ-1]}-R_[k-τ]

其中,P_{yy，[k-τ，k-τ-1]}和P_{xy，[k-τ，k-τ-1]}分别为自协方差与互协方差矩阵，R_[k-τ]＝E(r_[k-τ]r_[k-τ] ^T)为k-τ时刻量测噪声协方差矩阵。

5.根据权利要求4所述一种水下航行器未知输入与位置估计方法，其特征在于，所述延时递推框架为：

其中，

为延时转化为的偏差：

Υ_[k]中的延时带来的附加项

6.根据权利要求5所述一种水下航行器未知输入与位置估计方法，其特征在于，所述递推关联包括：

其中，

进一步得到：

其中，P_xx,[k,k-1]为状态的一步预测误差协方差，

κ_[k]＝E(η_[k-τ]η_[k-τ] ^T)＝R_[k-τ]+Θ_[k-τ]，

L_[k]为T_[k]经过Cholesky分解后的下三角矩阵。

7.根据权利要求6所述一种水下航行器未知输入与位置估计方法，其特征在于，所述未知非线性控制输入估计值的计算方法包括：

基于所述递推关联构建递推关联的线性回归表达式：

基于所述线性回归表达式计算所述未知非线性控制输入估计值：

其中，

8.根据权利要求7所述一种水下航行器未知输入与位置估计方法，其特征在于，所述后验估计表达式为：

其中，K_[k]为增益矩阵；

所述第一估计误差协方差表达式为：

9.根据权利要求8所述一种水下航行器未知输入与位置估计方法，其特征在于，所述第二次优化的方法包括：舍去所述后验估计表达式中与未知输入相关项，得到所述最终位置状态表达式和所述第二估计误差协方差表达式；

所述最终位置状态表达式为：

所述第二估计误差协方差表达式为：

其中，

10.一种水下航行器未知输入与位置估计系统，其特征在于，包括：模型构建模块、框架构建模块、关联构建模块、估计模块、后验状态模块和位置估计模块；

所述模型构建模块用于获取水下航行器的航行数据，并基于所述航行数据构建系统状态空间模型；

所述框架构建模块用于对所述系统状态空间模型中的量测方程进行第一次优化，得到优化后量测方程，并基于所述优化后量测方程构建延时递推框架；

所述关联构建模块用于基于所述延时递推框架，搭建未知非线性控制输入和延时到达观测信息之间的递推关联；

所述估计模块用于基于所述递推关联得到未知非线性控制输入估计值；

所述后验状态模块用于基于所述未知非线性控制输入估计值，得到位置状态后验估计表达式和第一估计误差协方差表达式；

所述位置估计模块用于对所述后验估计表达式进行第二次优化，得到最终位置状态表达式和第二估计误差协方差表达式。