CN115061483B - Underwater target state cooperative estimation method based on detection configuration - Google Patents

Abstract

The utility model provides a detection configuration-based underwater target state collaborative estimation method, which relates to the technical field of underwater unmanned aircraft formation control and target state estimation, and aims at solving the problem that a single submarine can have larger positioning error in continuous underwater long voyage in the prior art. According to the method, the centralized communication topological structure is utilized, pure azimuth information of a plurality of UUV detection nodes is concentrated and expanded, the defect that the observability of a single UUV node observation equation is not strong is overcome, the problem that a single submarine can have larger positioning error when continuously sailing underwater is solved, estimation precision is improved, and convergence time is shortened.

Description

Translated fromChinese

一种基于探测构型的水下目标状态协同估计方法A collaborative estimation method for underwater target state based on detection configuration

技术领域Technical Field

本发明涉及水下无人航行器编队控制及目标状态估计技术领域，具体为一种基于探测构型的水下目标状态协同估计方法。The present invention relates to the technical field of underwater unmanned vehicle formation control and target state estimation, and in particular to a method for collaborative estimation of underwater target states based on a detection configuration.

背景技术Background Art

随着对海洋的探测和开发技术日益增强，水下无人航行器对海洋探测发挥着重要作用。由于水下环境的复杂性，单个潜艇在水下持续长航会存在较大的定位误差，无法满足某些任务需求。因此，潜艇和多UUV的协同合作可以完成需要水下持续长航探测及跟踪目标的任务。As the ocean exploration and development technology becomes increasingly powerful, underwater unmanned vehicles play an important role in ocean exploration. Due to the complexity of the underwater environment, a single submarine will have a large positioning error when it continues to travel underwater for a long time, and cannot meet the requirements of certain missions. Therefore, the cooperation of submarines and multiple UUVs can complete tasks that require underwater continuous long-distance detection and tracking of targets.

近年来，随着水下目标的隐蔽性能的提高、机动性能的增强，潜艇的探测装备会存在不确定性的偏差。潜艇和多UUV组成的协同系统，能够扩大潜艇的探测范围。该协同系统以多UUV为前出力量，潜艇可以实现对近程全方位的警戒侦察，有效清除潜艇威胁。面对潜艇的防御作战时，UUV可以作为欺骗目标或佯动目标，掩护潜艇的行动，UUV还可以模拟潜艇声学特性直接产生潜艇的模拟声信号，达到混淆敌方探测设备，保护潜艇的安全目的。潜艇作为后方遥控指挥，通过通信设备传输控制指令给UUV。潜艇和UUV组成的多节点协同定位及跟踪系统具有适应能力强、探测效率高等优点，在军事、海洋开发等方面具有很多显著优点，因此其发展受到世界各国的重视。In recent years, with the improvement of the concealment performance and maneuverability of underwater targets, there will be uncertainty deviations in the detection equipment of submarines. The cooperative system composed of submarines and multiple UUVs can expand the detection range of submarines. With multiple UUVs as the forward force, the submarine can realize short-range all-round warning and reconnaissance, and effectively eliminate the threat of submarines. In the face of submarine defense operations, UUVs can be used as deceptive targets or feint targets to cover the actions of submarines. UUVs can also simulate the acoustic characteristics of submarines to directly generate simulated acoustic signals of submarines, so as to confuse enemy detection equipment and protect the safety of submarines. Submarines act as rear remote control commands and transmit control instructions to UUVs through communication equipment. The multi-node cooperative positioning and tracking system composed of submarines and UUVs has the advantages of strong adaptability and high detection efficiency. It has many significant advantages in military and marine development, so its development has been valued by countries around the world.

发明内容Summary of the invention

本发明的目的是：针对现有技术中单个潜艇在水下持续长航会存在较大的定位误差的问题，提出一种基于探测构型的水下目标状态协同估计方法。The purpose of the present invention is to propose a method for collaboratively estimating the state of underwater targets based on detection configuration in order to solve the problem in the prior art that a single submarine may have large positioning errors during long-term underwater navigation.

本发明为了解决上述技术问题采取的技术方案是：The technical solution adopted by the present invention to solve the above technical problems is:

一种基于探测构型的水下目标状态协同估计方法，所述协同估计方法包括编队步骤和估计步骤；A method for collaboratively estimating the state of an underwater target based on a detection configuration, the method comprising a formation step and an estimation step;

所述编队步骤具体为：The formation steps are specifically as follows:

步骤1：利用声呐采集目标的方位，并根据目标的方位建立多UUV编队控制模型；Step 1: Use sonar to collect the target's position and establish a multi-UUV formation control model based on the target's position;

步骤2：获取编队期望队形Δ_i以及率先发现目标的UUV的位置，然后根据率先发现目标的UUV的位置、编队期望队形Δ_i和多UUV编队控制模型设计编队控制器，并根据编队控制器对多UUV进行编队；Step 2: Obtain the expected formation Δ_i of the formation and the position of the UUV that first discovers the target, then design a formation controller based on the position of the UUV that first discovers the target, the expected formation Δ_i of the formation and the multi-UUV formation control model, and form multiple UUVs according to the formation controller;

所述估计步骤具体为：The estimation step is specifically as follows:

步骤一：建立目标的运动模型；Step 1: Establish the target motion model;

步骤二：设计扩张状态观测器，然后利用扩张状态观测器对目标进行扰动估计，得到目标的扰动估计结果；Step 2: Design an extended state observer, and then use the extended state observer to perform disturbance estimation on the target to obtain the disturbance estimation result of the target;

步骤三：利用目标的扰动估计结果对目标的运动模型进行估计，得到目标的准确估计结果；Step 3: Use the disturbance estimation result of the target to estimate the motion model of the target and obtain an accurate estimation result of the target;

步骤四：根据目标的准确估计结果得到目标运动状态；Step 4: Obtain the target motion state based on the accurate estimation result of the target;

步骤五：采用平方根容积卡尔曼滤波方法对目标的方位进行估计；Step 5: Use the square root volumetric Kalman filter method to estimate the target's position;

步骤六：获取噪声，并根据目标运动状态、估计后的目标的方位以及噪声构建观测方程；Step 6: Obtain the noise and construct the observation equation based on the target motion state, the estimated target position and the noise;

步骤七：根据观测方程并采用集中式的通讯拓扑结构，得到拓展的观测方程，进而完成协同估计。Step 7: Based on the observation equation and using a centralized communication topology, the extended observation equation is obtained to complete the collaborative estimation.

进一步的，所述多UUV编队控制模型表示为：Furthermore, the multi-UUV formation control model is expressed as:

其中，X_i、V_i、A_i分别表示第i个UUV在三维空间的期望位置、速度和加速度，

表示期望位置X_i的导数，

表示速度V_i的导数。Where_Xi ,_Vi , and_Ai represent the expected position, velocity, and acceleration of the i-th UUV in three-dimensional space, respectively.

represents the derivative of the desired position_Xi ,

represents the derivative of velocity_Vi .

进一步的，所述编队控制器表示为：Further, the formation controller is expressed as:

其中，下标m表示率先发现目标的UUV编号，γ表示反馈控制增益，K_v表示速度误差控制增益，K_p表示位置误差控制增益，a_ij表示通信拓扑权重矩阵系数参数，c表示一致性协议参数，N_i表示UUV节点的集合，X_j表示第J个UUV节点的位置，Δ_j表示第J个UUV节点的期望队形，V_j表示第J个UUV节点的速度。Among them, the subscript m represents the number of the UUV that first discovers the target, γ represents the feedback control gain,_Kv represents the speed error control gain,_Kp represents the position error control gain,_aij represents the communication topology weight matrix coefficient parameter, c represents the consistency protocol parameter,_Ni represents the set of UUV nodes,_Xj represents the position of the Jth UUV node,_Δj represents the expected formation of the Jth UUV node, and_Vj represents the speed of the Jth UUV node.

进一步的，所述目标的运动模型表示为：Furthermore, the motion model of the target is expressed as:

其中，X_T、V_T、A_T分别表示三维空间中的位置、速度、加速度，

表示目标的位置X_T的导数，

表示目标速度V_T的导数。Among them, X_T , V_T , and A_T represent the position, velocity, and acceleration in three-dimensional space, respectively.

represents the derivative of the target position_XT ,

Represents the derivative of the target velocity_VT .

进一步的，所述扩张状态观测器表示为：Furthermore, the extended state observer is expressed as:

其中，β₁、β₂和β₃表示增益参数，

表示对

的估计，

表示目标速度V_T的估计值，

表示目标位置X_T的估计值，

表示目标加速度A_T的估计值，g₁()、g₂()、g₃()表示观测器稳定的函数。Where β₁ , β₂ and β₃ represent gain parameters,

Express

The estimate,

represents the estimated value of the target speed V_T ,

represents the estimated value of the target position_XT ,

represents the estimated value of the target acceleration_AT , and_g1 (),_g2 (), and_g3 () represent observer stability functions.

进一步的，所述利用目标的扰动估计结果对目标的运动模型进行估计表示为：Furthermore, the estimation of the target's motion model using the target's disturbance estimation result is expressed as:

进一步的，所述估计后的目标的方位表示为：Furthermore, the estimated target position is expressed as:

其中，q_εi表示高低角，q_βi表示方位角，x_T、y_T、h_T表示目标的三维坐标中的位置，x_si、y_si、h_si表示潜艇或UUV在三维坐标中的位置。Among them, q_εi represents the elevation angle, q_βi represents the azimuth angle, x_T , y_T , h_T represent the position of the target in three-dimensional coordinates, and x_si , y_si , h_si represent the position of the submarine or UUV in three-dimensional coordinates.

进一步的，所述观测方程表示为：Furthermore, the observation equation is expressed as:

其中，

表示高低角的白噪声，

表示方位角的白噪声，h_i(X_T)表示目标的位置函数。in,

represents white noise at high and low angles,

represents the white noise of the azimuth angle, and_hi (_XT ) represents the position function of the target.

进一步的，所述拓展的观测方程表示为：Furthermore, the extended observation equation is expressed as:

其中，η表示所有UUV节点探测到目标的位置函数的集合。Among them, η represents the set of position functions of all UUV nodes detecting the target.

本发明的有益效果是：The beneficial effects of the present invention are:

本申请所提出的由潜艇(中心节点)和多个水下无人航行器(UUV)节点构成的协同估计方法，潜艇发送控制指令使各UUV完成期望的编队队形，各UUV利用方位测量信息对敌方目标进行协同估计，实现对水下动态目标状态的有效估计，为协同系统完成水下任务提供必要的条件。本申请利用集中式的通讯拓扑结构，通过将多个UUV探测节点的纯方位信息进行集中扩展，解决了单UUV节点观测方程可观性不强的缺点，进而避免了单个潜艇在水下持续长航会存在较大的定位误差的问题，提高了估计精度，缩短了收敛时间。The collaborative estimation method proposed in this application is composed of a submarine (central node) and multiple underwater unmanned vehicle (UUV) nodes. The submarine sends control instructions to enable each UUV to complete the desired formation. Each UUV uses azimuth measurement information to collaboratively estimate the enemy target, achieve effective estimation of the underwater dynamic target state, and provide the necessary conditions for the collaborative system to complete underwater tasks. This application uses a centralized communication topology structure to centrally expand the pure azimuth information of multiple UUV detection nodes, thereby solving the shortcoming of the weak observability of the observation equation of a single UUV node, thereby avoiding the problem of large positioning errors when a single submarine continues to sail underwater for a long time, improving the estimation accuracy and shortening the convergence time.

附图说明BRIEF DESCRIPTION OF THE DRAWINGS

图1为本申请的流程图；FIG1 is a flow chart of the present application;

图2为本申请的系统结构图；FIG2 is a system structure diagram of the present application;

图3为多UUV编队变换图；Figure 3 is a diagram of the transformation of multiple UUV formations;

图4为编队期望队形示意图；Figure 4 is a schematic diagram of the expected formation of the formation;

图5为目标位置的估计值示意图；FIG5 is a schematic diagram of the estimated value of the target position;

图6为目标速度的估计值示意图；FIG6 is a schematic diagram of the estimated value of the target speed;

图7为目标加速度的估计值示意图。FIG. 7 is a schematic diagram of estimated values of target acceleration.

具体实施方式DETAILED DESCRIPTION

需要特别说明的是，在不冲突的情况下，本申请公开的各个实施方式之间可以相互组合。It should be particularly noted that, in the absence of conflict, the various embodiments disclosed in this application can be combined with each other.

具体实施方式一：参照图1具体说明本实施方式，本实施方式所述的一种基于探测构型的水下目标状态协同估计方法，所述协同估计方法包括编队步骤和估计步骤；Specific implementation method 1: Referring to FIG. 1 , this implementation method is described in detail. This implementation method is a method for collaboratively estimating the state of an underwater target based on a detection configuration. The collaborative estimation method includes a formation step and an estimation step.

所述编队步骤具体为：The formation steps are specifically as follows:

步骤1：利用声呐采集目标的方位，并根据目标的方位建立多UUV编队控制模型；Step 1: Use sonar to collect the target's position and establish a multi-UUV formation control model based on the target's position;

步骤2：获取编队期望队形Δ_i以及率先发现目标的UUV的位置，然后根据率先发现目标的UUV的位置、编队期望队形Δ_i和多UUV编队控制模型设计编队控制器，并根据编队控制器对多UUV进行编队；Step 2: Obtain the expected formation Δ_i of the formation and the position of the UUV that first discovers the target, then design a formation controller based on the position of the UUV that first discovers the target, the expected formation Δ_i of the formation and the multi-UUV formation control model, and form multiple UUVs according to the formation controller;

所述估计步骤具体为：The estimation step is specifically as follows:

步骤一：建立目标的运动模型；Step 1: Establish the target motion model;

步骤二：设计扩张状态观测器，然后利用扩张状态观测器对目标进行扰动估计，得到目标的扰动估计结果；Step 2: Design an extended state observer, and then use the extended state observer to perform disturbance estimation on the target to obtain the disturbance estimation result of the target;

步骤三：利用目标的扰动估计结果对目标的运动模型进行估计，得到目标的准确估计结果；Step 3: Use the disturbance estimation result of the target to estimate the motion model of the target and obtain an accurate estimation result of the target;

步骤四：根据目标的准确估计结果得到目标运动状态；Step 4: Obtain the target motion state based on the accurate estimation result of the target;

步骤五：采用平方根容积卡尔曼滤波方法对目标的方位进行估计；Step 5: Use the square root volumetric Kalman filter method to estimate the target's position;

步骤六：获取噪声，并根据目标运动状态、估计后的目标的方位以及噪声构建观测方程；Step 6: Obtain the noise and construct the observation equation based on the target motion state, the estimated target position and the noise;

步骤七：根据观测方程并采用集中式的通讯拓扑结构，得到拓展的观测方程，进而完成协同估计。Step 7: Based on the observation equation and using a centralized communication topology, the extended observation equation is obtained to complete the collaborative estimation.

本申请能够自主完成期望编队队形、在隐藏环境下对目标定位和跟踪精度高、扩大对目标的探测范围并且可以有效降低成本的水下目标协同估计及多UUV编队控制研究方法，协助潜艇完成水下作战任务。This application is able to autonomously complete the desired formation, locate and track targets with high accuracy in hidden environments, expand the detection range of targets, and effectively reduce costs for underwater target collaborative estimation and multi-UUV formation control research methods, assisting submarines in completing underwater combat missions.

实施例：Example:

结合图1和图2多水下无人航行器跟踪及编队控制系统结构包括：1艘潜艇和2个UUV。潜艇和UUV组成的协同系统包含中央处理单元、探测模块、通信模块、执行模块。探测模块通过传感器收集自身状态和周围环境信息，并发送给位于潜艇中的中央处理单元进行信息融合；通信模块负责潜艇和各UUV之间进行信息交互；中央处理单元是潜艇根据探测模块给出的传感器信息和模块给出的协同要素给出相应的执行指令；执行模块是各个UUV根据中央处理单元给出的编队和控制指令进行相应的动作响应。Combined with Figure 1 and Figure 2, the structure of the multi-underwater unmanned vehicle tracking and formation control system includes: 1 submarine and 2 UUVs. The collaborative system composed of submarines and UUVs includes a central processing unit, a detection module, a communication module, and an execution module. The detection module collects its own status and surrounding environment information through sensors, and sends it to the central processing unit located in the submarine for information fusion; the communication module is responsible for information exchange between the submarine and each UUV; the central processing unit is the submarine that gives corresponding execution instructions based on the sensor information given by the detection module and the collaborative elements given by the module; the execution module is that each UUV responds to the corresponding actions according to the formation and control instructions given by the central processing unit.

结合图3和图4可知率先发现目标的UUV保证能持续探测到目标。同时将潜艇作为编队队形的几何中心，以此为参照给出其余UUV的期望位置，当潜艇和另一个UUV到达期望位置以后，最后调整率先发现目标的UUV位置，至此，编队队形形成，潜艇和UUV均能探测到目标。图3以1艘潜艇和2个UUV为对象，1号节点为潜艇，2号和3号节点是UUV，分别展示2、3号节点率先发现目标，潜艇控制另一个UUV形成编队队形的过程，此过程要求率先发现目标的UUV最后进行位置调整。最后结合多智能体的一致性理论来解决编队控制问题，编队控制器(1)可以实现编队的保持控制。Combining Figures 3 and 4, it can be seen that the UUV that first discovers the target is guaranteed to continue to detect the target. At the same time, the submarine is used as the geometric center of the formation, and the expected position of the remaining UUVs is given with reference to it. When the submarine and another UUV reach the expected position, the position of the UUV that first discovers the target is finally adjusted. At this point, the formation is formed, and both the submarine and the UUV can detect the target. Figure 3 takes a submarine and two UUVs as objects,node 1 is the submarine, andnodes 2 and 3 are UUVs. It shows the process in whichnodes 2 and 3 first discover the target, and the submarine controls another UUV to form a formation. This process requires the UUV that first discovers the target to adjust its position last. Finally, the formation control problem is solved by combining the consistency theory of multi-agents. The formation controller (1) can achieve formation maintenance control.

目标状态估计的前提是建立与目标实际机动相匹配的目标机动模型，需要对目标当前进行的机动进行假设。本设计将目标的加速度视为引起目标位置和速度变化的扰动项，采用扩张状态观测器技术来估计这个扰动。因此把扩张状态观测器建立在目标跟踪模型之中，可以不对目标的机动情况进行假设，从而使该模型具备通用性。The premise of target state estimation is to establish a target maneuver model that matches the actual maneuver of the target, which requires assumptions about the current maneuver of the target. This design regards the acceleration of the target as a disturbance that causes changes in the target position and velocity, and uses the extended state observer technology to estimate this disturbance. Therefore, by building the extended state observer into the target tracking model, it is possible to make no assumptions about the maneuver of the target, making the model universal.

目标的运动模型的通用表达式为式(2)。The general expression of the target motion model is formula (2).

式中X_T、V_T、A_T分别表示三维空间中的位置，速度，加速度。为了估计目标的运动状态，需要根据式(2)来建立滤波模型。由于目标的加速度A_T形式多变，通常单个模型难以匹配实际A_T从而产生模型不匹配的问题，因此采用多模型的解决方案。多模型方案虽然一定程度上解决了模型匹配的问题，同样带来计算量大，多模型之间信息融合等问题，形式复杂。本方案采用扩张状态观测器来辅助建立滤波模型，使其具有多模型的实际效果。由于加速度A_T形式多变，故将其视为扰动，并使用扩张状态观测器来对其进行估计。对系统(2)建立一个扩张状态观测器如式(3)所示。Where X_T , V_T , and_AT represent the position, velocity, and acceleration in three-dimensional space, respectively. In order to estimate the motion state of the target, it is necessary to establish a filter model according to formula (2). Since the acceleration_AT of the target varies in form, it is usually difficult for a single model to match the actual_AT , resulting in a model mismatch problem. Therefore, a multi-model solution is adopted. Although the multi-model solution solves the problem of model matching to a certain extent, it also brings problems such as large amount of calculation, information fusion between multiple models, and complex form. This solution uses an extended state observer to assist in establishing the filter model, so that it has the actual effect of multiple models. Since the acceleration_AT varies in form, it is regarded as a disturbance and is estimated using an extended state observer. An extended state observer is established for system (2) as shown in formula (3).

观测器(3)中，

表示对·的估计，增益参数β₁～β₃及函数g₁()～g₃()的选取需要保证观测器是稳定的。观测器(3)可以对原系统中的A_T实现良好的估计，因此通过联立(2)和(3)便可以得到一个闭环的模型(4)。观测器输出

和真实的A_T之间虽然存在偏差，一方面由于观测器是稳定的，偏差会逐渐收敛，越来越小，模型越来越精准；另一方面，偏差也被当成噪声的一部分，利用观测值来进行提高估计精度。In observer (3),

represents the estimation of ·. The selection of gain parameters β₁ ~β₃ and functions g₁ () ~g₃ () needs to ensure that the observer is stable. Observer (3) can achieve a good estimation of_AT in the original system. Therefore, by combining (2) and (3), a closed-loop model (4) can be obtained. The observer output

Although there is a deviation between the true AT and the actual_AT , on the one hand, since the observer is stable, the deviation will gradually converge and become smaller and smaller, and the model will become more and more accurate; on the other hand, the deviation is also regarded as part of the noise, and the observation value is used to improve the estimation accuracy.

结合图5、图6以及图7由于目标的状态(位置，速度，加速度)不是直接测量得到的，而是只能测量方位信息。纯方位信息使得方程的可观测性不强。因此首先需要根据纯方位信息以及节点信息来增加观测信息，增强可观性。由于观测方程是非线性的，因此需要使用非线性估计方法来估计目标的状态。基于目标跟踪算法采用平方根容积卡尔曼滤波方法，可以避免对矩阵求逆操作，且不要求噪声特性，比较适用于非线性强，状态维度大的场景。Combined with Figures 5, 6 and 7, the state of the target (position, velocity, acceleration) is not directly measured, but only the orientation information can be measured. Pure orientation information makes the equation less observable. Therefore, it is first necessary to increase the observation information based on the pure orientation information and node information to enhance observability. Since the observation equation is nonlinear, a nonlinear estimation method is required to estimate the state of the target. The square root volume Kalman filter method based on the target tracking algorithm can avoid the matrix inversion operation and does not require noise characteristics. It is more suitable for scenarios with strong nonlinearity and large state dimensions.

式(5)定义了第i个UUV对目标的方位测量(高低角q_εi和方位角q_βi)。可以看出高低角和方位角是关于目标X_T＝[x_T y_T h_T]^T和UUV位置X_s＝[x_s y_s h_s]^T的非线性函数。假设测量噪声为高斯白噪声。可以得到新的观测方程如式(6)所示。Equation (5) defines the azimuth measurement of the target by the i-th UUV (elevation angle q_εi and azimuth angle q_βi ). It can be seen that the elevation angle and azimuth angle are nonlinear functions of the target X_T = [x_T y_T h_T ]^T and the UUV position X_s = [x_s y_s h_s ]^T. Assume that the measurement noise is Gaussian white noise. The new observation equation can be obtained as shown in equation (6).

观测方程(6)中，凭借单个UUV很难完成对目标的状态估计。考虑到潜艇与UUV的通讯结构为集中式，根据集中式的特点，本研究方案提出如下的增强观测方程可观性的方法。In the observation equation (6), it is difficult to complete the state estimation of the target with a single UUV. Considering that the communication structure between the submarine and the UUV is centralized, according to the characteristics of the centralized structure, this research proposal proposes the following method to enhance the observability of the observation equation.

采用集中式的通讯拓扑结构，所有的UUV节点向中心节点发送目标的方位信息。设第i个UUV节点探测到目标的高低角、方位角分别为q_εi和q_βi。那么拓展的观测方程为Using a centralized communication topology, all UUV nodes send the target's position information to the central node. Suppose the elevation angle and azimuth angle of the target detected by the i-th UUV node are q_εi and q_βi respectively. Then the expanded observation equation is

式中，m为能探测到目标的UUV编号。可以看出式(7)易拓展，拓展后的观测噪声矩阵表达式为ρ＝diag[R₁ R₂ … R_m]。Where m is the number of the UUV that can detect the target. It can be seen that formula (7) is easy to expand, and the expression of the observation noise matrix after expansion is ρ = diag[R₁ R₂ … R_m ].

至此，利用集中式的通讯拓扑结构，通过将多个UUV探测节点的纯方位信息进行集中扩展，解决了单UUV节点观测方程可观性不强的缺点，这对提高估计精度，缩短收敛时间是有益的。At this point, by utilizing the centralized communication topology and centrally expanding the pure bearing information of multiple UUV detection nodes, the shortcoming of weak observability of the observation equation of a single UUV node has been solved, which is beneficial to improving the estimation accuracy and shortening the convergence time.

需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围内。It should be noted that the specific implementation is only an explanation and description of the technical solution of the present invention, and cannot be used to limit the scope of protection of the rights. Any partial changes made according to the claims and description of the present invention should still fall within the scope of protection of the present invention.

Claims (6)

Translated fromChinese

1.一种基于探测构型的水下目标状态协同估计方法，其特征在于所述协同估计方法包括编队步骤和估计步骤；1. A method for collaborative estimation of underwater target states based on detection configuration, characterized in that the collaborative estimation method comprises a formation step and an estimation step;所述编队步骤具体为：The formation steps are specifically as follows:步骤1：利用声呐采集目标的方位，并根据目标的方位建立多UUV编队控制模型；Step 1: Use sonar to collect the target's position and establish a multi-UUV formation control model based on the target's position;步骤2：获取编队期望队形Δ_i以及率先发现目标的UUV的位置，然后根据率先发现目标的UUV的位置、编队期望队形Δ_i和多UUV编队控制模型设计编队控制器，并根据编队控制器对多UUV进行编队；Step 2: Obtain the expected formation Δ_i of the formation and the position of the UUV that first discovers the target, then design a formation controller based on the position of the UUV that first discovers the target, the expected formation Δ_i of the formation and the multi-UUV formation control model, and form multiple UUVs according to the formation controller;所述估计步骤具体为：The estimation step is specifically as follows:步骤一：建立目标的运动模型；Step 1: Establish the target motion model;步骤二：设计扩张状态观测器，然后利用扩张状态观测器对目标进行扰动估计，得到目标的扰动估计结果；Step 2: Design an extended state observer, and then use the extended state observer to perform disturbance estimation on the target to obtain the disturbance estimation result of the target;步骤三：利用目标的扰动估计结果对目标的运动模型进行估计，得到目标的准确估计结果；Step 3: Use the disturbance estimation result of the target to estimate the motion model of the target and obtain an accurate estimation result of the target;步骤四：根据目标的准确估计结果得到目标运动状态；Step 4: Obtain the target motion state based on the accurate estimation result of the target;步骤五：采用平方根容积卡尔曼滤波方法对目标的方位进行估计；Step 5: Use the square root volumetric Kalman filter method to estimate the target's position;步骤六：获取噪声，并根据目标运动状态、估计后的目标的方位以及噪声构建观测方程；Step 6: Obtain the noise and construct the observation equation based on the target motion state, the estimated target position and the noise;步骤七：根据观测方程并采用集中式的通讯拓扑结构，得到拓展的观测方程，进而完成协同估计；Step 7: Based on the observation equation and using a centralized communication topology, the extended observation equation is obtained, and then the collaborative estimation is completed;所述估计后的目标的方位表示为：The estimated target position is expressed as:

其中，q_εi表示高低角，q_βi表示方位角，x_T、y_T、h_T表示目标的三维坐标中的位置，x_si、y_si、h_si表示潜艇或UUV在三维坐标中的位置；Among them, q_εi represents the elevation angle, q_βi represents the azimuth angle, x_T , y_T , h_T represent the position of the target in three-dimensional coordinates, x_si , y_si , h_si represent the position of the submarine or UUV in three-dimensional coordinates;所述观测方程表示为：The observation equation is expressed as:

其中，V_qεi表示高低角的白噪声，

表示方位角的白噪声，h_i(X_T)表示目标的位置函数；Among them,_Vqεi represents the white noise of high and low angles,

represents the white noise of the azimuth,_hi (_XT ) represents the position function of the target;所述拓展的观测方程表示为：The extended observation equation is expressed as:

其中，η表示所有UUV节点探测到目标的位置函数的集合。Among them, η represents the set of position functions of all UUV nodes detecting the target.2.根据权利要求1所述的一种基于探测构型的水下目标状态协同估计方法，其特征在于所述多UUV编队控制模型表示为：2. The underwater target state collaborative estimation method based on detection configuration according to claim 1, characterized in that the multi-UUV formation control model is expressed as:

其中，X_i、V_i、A_i分别表示第i个UUV在三维空间的期望位置、速度和加速度，

表示期望位置X_i的导数，

表示速度V_i的导数。Where_Xi ,_Vi , and_Ai represent the expected position, velocity, and acceleration of the i-th UUV in three-dimensional space, respectively.

represents the derivative of the desired position_Xi ,

represents the derivative of velocity_Vi .3.根据权利要求2所述的一种基于探测构型的水下目标状态协同估计方法，其特征在于所述编队控制器表示为：3. The underwater target state collaborative estimation method based on detection configuration according to claim 2, characterized in that the formation controller is expressed as:

其中，下标m表示率先发现目标的UUV编号，γ表示反馈控制增益，K_v表示速度误差控制增益，K_p表示位置误差控制增益，a_ij表示通信拓扑权重矩阵系数参数，c表示一致性协议参数，N_i表示UUV节点的集合，X_j表示第J个UUV节点的位置，Δ_j表示第J个UUV节点的期望队形，V_j表示第J个UUV节点的速度。Among them, the subscript m represents the number of the UUV that first discovers the target, γ represents the feedback control gain,_Kv represents the speed error control gain,_Kp represents the position error control gain,_aij represents the communication topology weight matrix coefficient parameter, c represents the consistency protocol parameter,_Ni represents the set of UUV nodes,_Xj represents the position of the Jth UUV node,_Δj represents the expected formation of the Jth UUV node, and_Vj represents the speed of the Jth UUV node.4.根据权利要求3所述的一种基于探测构型的水下目标状态协同估计方法，其特征在于所述目标的运动模型表示为：4. The method for collaboratively estimating the underwater target state based on detection configuration according to claim 3, characterized in that the motion model of the target is expressed as:

其中，X_T、V_T、A_T分别表示三维空间中的位置、速度、加速度，

表示目标的位置X_T的导数，

表示目标速度V_T的导数。Among them, X_T , V_T , and A_T represent the position, velocity, and acceleration in three-dimensional space, respectively.

represents the derivative of the target position_XT ,

Represents the derivative of the target velocity_VT .5.根据权利要求4所述的一种基于探测构型的水下目标状态协同估计方法，其特征在于所述扩张状态观测器表示为：5. The underwater target state collaborative estimation method based on detection configuration according to claim 4, characterized in that the extended state observer is expressed as:

其中，β₁、β₂和β₃表示增益参数，

表示对

的估计，

表示目标速度V_T的估计值，

表示目标位置X_T的估计值，

表示目标加速度A_T的估计值，g₁()、g₂()、g₃()表示观测器稳定的函数。Where β₁ , β₂ and β₃ represent gain parameters,

Express

The estimate,

represents the estimated value of the target speed V_T ,

represents the estimated value of the target position_XT ,

represents the estimated value of the target acceleration_AT , and_g1 (),_g2 (), and_g3 () represent observer stability functions.6.根据权利要求5所述的一种基于探测构型的水下目标状态协同估计方法，其特征在于所述利用目标的扰动估计结果对目标的运动模型进行估计表示为：6. The method for collaboratively estimating the state of an underwater target based on a detection configuration according to claim 5, characterized in that the estimation of the target's motion model using the target's disturbance estimation result is expressed as:

Images