A multi-aquatic robot distributed cooperative positioning method and system

Abstract

The disclosure provides a kind of multi underwater robot distributed cooperative positioning method and system, it is related to underwater robot technical field, including determining target underwater robot and the monitoring node of target underwater robot, the measurement information of target underwater robot is collected;Underwater robot kinematics model, depth measurement model, attitude measurement model and distance measurement model under global coordinate system are established;The measurement information of target underwater robot is utilized by monitoring node, according to the extended Kalman filter algorithm of revision, local filter is constructed, the pose state of underwater robot is predicted;The estimation information of neighbor node pose state is fused, based on the approximate calculation of innovation matrix and interaction covariance matrix, regional filter is constructed, and the pose state of target underwater robot is obtained.The system can well eliminate the influence of time lag and unknown colored uncertainty, and more accurate raw data is obtained.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the technical field of underwater robots, in particular to a multi-underwater robot distributed cooperative positioning method and system based on a modified extended Kalman filter. BACKGROUND

[0002] The statements in this section merely provide background information related to the present disclosure and do not necessarily constitute the prior art.

[0003] The ocean accounts for 71% of the earth's surface and is a treasure trove of resources, but the current development is very limited. In fact, human energy exploitation is mainly above 300 meters deep underwater. If you want to continue to explore downward, you will face the problem of serious shortage of exploration means, and underwater robots can dive into the water to replace humans to complete complex tasks. For this reason, in recent years, autonomous underwater vehicles (AUVs) as the most important means of exploring the ocean have received widespread attention.

[0004] Due to the complex and changeable underwater environment, the noise interference of the surrounding environment of the AUV is inevitable when working underwater. In order to reduce the influence of noise on the positioning accuracy of the AUV, filter positioning methods have been widely used, such as Kalman filter (KF), extended Kalman filter (EKF), unscented Kalman filter (UKF), and particle filter positioning (PF) methods. The famous Kalman filter is considered to be an effective solution for linear systems affected by Gaussian noise, and the parameters and statistical information of the system are accurately known. The core of the Kalman filter algorithm is to design a recursive filter to minimize the local error variance. However, considering that the system may not have accurate knowledge of the system parameters, for example, the system is prone to parameter uncertainty due to unmodeled dynamics. In this regard, the analytical solution of the error variance is difficult to calculate, so the Kalman algorithm is no longer applicable. To solve the state estimation problem of nonlinear systems, the famous extended Kalman filter algorithm is proposed based on the linearization idea of nonlinear models. As a classic nonlinear filtering algorithm, the extended Kalman filter algorithm has been widely used and developed since its inception due to its clear ideas and simple implementation. As a widely used nonlinear filtering algorithm in the field of underwater navigation, the extended Kalman filter algorithm has a simple form and is a commonly used method in the positioning research and application of underwater robots. However, the extended Kalman filter method is a kind of approximate linearization method, which involves the solution of the Jacobian matrix (matrix partial derivative) and the matrix inversion problem in the solution process, which leads to a relatively high complexity of the algorithm. And the traditional method leads to high-dimensional calculation and poor real-time performance, the system cannot well eliminate the influence of time lag and unknown colored uncertainty, and cannot effectively obtain more accurate original data. SUMMARY

[0005] To solve the above problems, the present disclosure provides a multi-underwater robot distributed cooperative positioning method and system, which avoids the problems of Jacobian matrix solving and matrix inversion by approximately solving the filter error covariance matrix, the innovation covariance matrix and the interaction covariance matrix. Compared with the traditional extended Kalman filter method, the method has lower computational complexity and better real-time performance.

[0006] According to some embodiments, the present disclosure adopts the following technical solutions:

[0007] A multi-underwater robot distributed cooperative positioning method comprises:

[0008] Determining a target underwater robot and a monitoring node for the target underwater robot, and collecting measurement information of the target underwater robot;

[0009] Establishing a kinematic model, a depth measurement model, an attitude measurement model and a distance measurement model of the underwater robot in a global coordinate system;

[0010] Using the measurement information of the target underwater robot by the monitoring node, constructing a local filter according to a modified extended Kalman filter algorithm, and predicting the pose state of the underwater robot;

[0011] Fusing the estimated information of the neighbor node pose state, constructing a regional filter based on the approximate calculation of the innovation matrix and the interaction covariance matrix, and obtaining the pose state of the target underwater robot.

[0012] According to some embodiments, the present disclosure adopts the following technical solutions:

[0013] A preprocessing module is configured to determine a target underwater robot and a monitoring node for the target underwater robot, and collect measurement information of the target underwater robot;

[0014] A kinematic modeling module is configured to establish a kinematic model, a depth measurement model, an attitude measurement model and a distance measurement model of the underwater robot in a global coordinate system;

[0015] A local estimator and perception module is configured to use the measurement information of the target underwater robot by the monitoring node, construct a local filter according to a modified extended Kalman filter algorithm, and predict the pose state of the underwater robot;

[0016] A regional estimator and information fusion module is configured to fuse the estimated information of the neighbor node pose state, construct a regional filter based on the approximate calculation of the innovation matrix and the interaction covariance matrix, and obtain the pose state of the target underwater robot.

[0017] According to some embodiments, the present disclosure adopts the following technical solutions:

[0018] A computer readable storage medium, wherein a plurality of instructions are stored, the instructions being adapted to be loaded by a processor of a terminal device and to execute the multi-underwater robot distributed cooperative positioning method.

[0019] According to some embodiments, the present disclosure adopts the technical solutions as follows:

[0020] A terminal device, comprising a processor and a computer readable storage medium, the processor being configured to implement instructions; and the computer readable storage medium being configured to store a plurality of instructions, the instructions being adapted to be loaded by the processor and to execute the multi-underwater robot distributed cooperative positioning method.

[0021] Compared with the prior art, the present disclosure has the beneficial effects that:

[0022] The method of the present disclosure addresses the modeling uncertainty of measurement information and measurement time delay existing in a non-ideal network environment, proposes a modeling method for unknown uncertain measurement information, a reconstruction method for measurement and innovation, and solves the high-dimensional calculation and real-time cross problems caused by traditional methods; proposes a design method of coupled optimal estimator, so that the system can well eliminate the influence of time delay and unknown colored uncertainty, and obtain more accurate original data.

[0023] The present disclosure proposes a kind of multi-underwater robot distributed cooperative positioning method based on modified extended Kalman filter, by the approximate solution of filter error covariance matrix, innovation covariance matrix, interaction covariance matrix, solve the problem of Jacobian matrix solution and matrix inversion, compared with the traditional extended Kalman filter method, the method has lower computational complexity, and has better real-time performance in practical application. BRIEF DESCRIPTION OF DRAWINGS

[0024] The drawings accompanying the specification of the present disclosure serve to provide a further understanding of the present disclosure, and the illustrative embodiments of the present disclosure and their descriptions serve to explain the present disclosure, and do not constitute an improper limitation on the present disclosure.

[0025] Figure 1 A multi-underwater robot system of the embodiments of the present disclosure is shown in the figure;

[0026] Figure 2 A method flowchart of the embodiments of the present disclosure is shown in the figure;

[0027] Figure 3 A three-dimensional space positioning effect diagram of the underwater robot system of the embodiments of the present disclosure is shown in the figure;

[0028] Figure 4 A two-dimensional plane positioning effect diagram of the underwater robot system of the embodiments of the present disclosure is shown in the figure;

[0029] Figure 5 This is a diagram showing the convergence effect of the positioning error of the underwater robot system according to an embodiment of this disclosure. Detailed Implementation

[0030] The present disclosure will be further described below with reference to the accompanying drawings and embodiments.

[0031] It should be noted that the following detailed descriptions are illustrative and intended to provide further explanation of this disclosure. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure pertains.

[0032] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments according to this disclosure. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms “comprising” and / or “including” are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0033] Example 1

[0034] One embodiment of this disclosure provides a distributed cooperative localization method for underwater robots, including:

[0035] Step 1: Identify the target underwater robot and the monitoring nodes for the target underwater robot, and collect measurement information of the target underwater robot;

[0036] Step 2: Establish the kinematic model, depth measurement model, attitude measurement model, and distance measurement model of the underwater robot in the global coordinate system;

[0037] Step 3: Using the measurement information of the target underwater robot from the monitoring nodes, construct a local filter based on the modified extended Kalman filter algorithm to predict the pose state of the underwater robot;

[0038] Step 4: Fuse the estimated pose state information of neighboring nodes, construct a region filter based on the approximate calculation of the innovation matrix and the interaction covariance matrix, and obtain the pose state of the target underwater robot.

[0039] Further, in step one, due to the restriction of complex underwater environment and communication environment on the underwater robot (AUV), the disclosure proposes a distributed cooperative positioning method, considers isomorphic AUV, and sets a target underwater robot. The underwater beacon nodes and other AUVs within the communication range of the target underwater robot can be regarded as perception sensor nodes with computing and communication capabilities. Other underwater robots within the communication range of the target underwater robot are called monitoring nodes. The purpose of distributed cooperative positioning of the target underwater robot includes: reading the pose information of the target underwater robot from the beacon nodes or other underwater robots within the communication range of the target node, reducing the computational complexity, and improving the real-time performance of cooperative positioning.

[0040] As an embodiment, in step two, the process of establishing the kinematic model of the multiple underwater robots in the global coordinate system includes:

[0041] Considering the miniaturized target underwater robot, a discrete nonlinear model with noise disturbance is adopted, and the position information and attitude information of the target underwater robot are taken as the state information to be estimated. The attitude information includes roll angle, pitch angle and yaw angle.

[0042] Specifically, the AUV kinematic model and the measurement model of the monitoring node are established respectively.

[0043] 1) Establish the AUV kinematic model:

[0044] Take the position information s k and the attitude information (roll angle, pitch angle and yaw angle) in the global coordinate system as the state information to be estimated wherein,

[0045]

[0046] wherein, x k is the distance of the target AUV in the x-axis direction of the fixed coordinate system, y k is the distance in the y-axis direction of the fixed coordinate system, z k is the distance in the z-axis direction of the fixed coordinate system; φ k is the roll angle, θ k is the pitch angle, and ψ k is the yaw angle.

[0047] The control input is

[0048] v k =[h k ,l k ,t k ] T ,ω k =[pk q k r k T

[0049] where v k and ω k are the linear and angular velocities of the AUV in the body frame. h k l k t k are the linear velocities in three directions in the body frame, and p k q k r k are the angular velocities in three directions in the body frame.

[0050] Assuming the sampling time is ΔT, the kinematic model of the AUV can be expressed as the following nonlinear discrete-time system:

[0051] x k+1 = f(x k , u k ) + w k = x k + ΔT J(x k )u k + w k

[0052] The conversion matrix J(x) has the following form

[0053]

[0054] where,

[0055]

[0056]

[0057] Since u k cannot be perceived without a DVL, it is set as a constant here; the external disturbance w k is assumed to be Gaussian white noise with zero mean and covariance matrix Q k .

[0058] 2) Establish the monitoring node measurement signal model

[0059] ​The process of establishing the depth measurement information model of the multi-underwater robot in the global coordinate system comprises: the target underwater robot periodically broadcasts its unknown through an acoustic modem, the beacon nodes and underwater robots within the communication range thereof estimate the distance through the time of arrival, the state information of the beacon nodes and monitoring nodes is taken as the reference unknown to construct the distance measurement information, the measured depth information is interacted with the monitoring nodes through broadcasting to construct the depth measurement information; the target underwater robot interacts the IMU measurement information with the monitoring nodes through broadcasting to construct the attitude measurement information model.

[0060] In the presence of the acoustic modem, the IMU and the pressure sensor, the measurements about the target AUV include three types of distance measurement z r , attitude measurement z a and depth measurement z c .

[0061] (a) Distance measurement

[0062] The target AUV periodically broadcasts its position through the acoustic modem so that the beacon nodes and AUVs within the communication range thereof estimate the distance through the time of arrival. The reference position of the beacon nodes and monitoring nodes at time k is denoted as (wherein i = 1, …, m is the number of beacon nodes and monitoring nodes within the communication range of the target AUV), and the distance measurement of the monitoring nodes to the target AUV is

[0063]

[0064] wherein x i,k , y i,k and z i,k are the distance information of the monitoring nodes in the fixed coordinate system, respectively, and the disturbance is a zero-mean Gaussian white noise with a variance of

[0065] (b) Attitude measurement

[0066] The target AUV interacts the IMU measurement information for estimating the attitude with the monitoring nodes through broadcasting, and the following model is adopted:

[0067]

[0068] wherein the disturbance is a zero-mean Gaussian white noise with a variance of

[0069] (c) Depth measurement

[0070] The target AUV measures the depth information through the pressure sensor, which is interacted with the monitoring nodes through broadcasting, and the following model is adopted:

[0071]

[0072] where the disturbance is a zero-mean Gaussian white noise with variance

[0073] Based on the above three types of measurements, the measurement information of the monitoring node about the state information x k of the target AUV can be defined as:

[0074]

[0075] where the disturbance is a zero-mean Gaussian white noise with variance R i .

[0076] As an embodiment, in step three, the specific process of constructing the local filter according to the measurement information of the monitoring node on the target underwater robot is as follows:

[0077] According to the measurement information of the monitoring node on the target underwater robot, the prediction estimate of the pose information of the target underwater robot is constructed based on the extended Kalman filter algorithm.

[0078] Specifically, the design method of the local state estimator is to design the local state estimator of the target AUV by using the local measurement information and the Taylor expansion.

[0079] The one-step prediction value of the target AUV at k+1 time is:

[0080]

[0081] where E represents mathematical expectation.

[0082] The state equation of the target AUV is expanded about the filter value at k time, and the true state at k+1 time can be approximated by the matrix Taylor expansion as follows:

[0083]

[0084] and the first-order truncation of the Taylor expansion is

[0085]

[0086] As an embodiment, in step four, the specific process of constructing the regional filter is to fuse the estimated information of the neighbor nodes based on the local filter, to design the gain matrix of the regional state filter and the information fusion weighting matrix under a given fusion strategy through the approximate calculation of the innovation covariance matrix, the interaction innovation covariance matrix and the state covariance matrix, and to obtain the regional estimation of the target AUV pose information.

[0087] A regional estimation information and a modified extended Kalman filter method are used to design the regional state estimator of the target AUV.

[0088] The neighbor information of the i-th node is further fused to design the AUV regional pose estimator as follows:

[0089]

[0090]

[0091]

[0092] wherein, is the regional one-step prediction, is the weighted fused pose estimation information, is the filter gain matrix to be solved; and W ij is the weighting matrix to be optimized, and satisfies According to the above regional estimator structure, the structure of the modified local estimator estimation error is as follows:

[0093]

[0094] wherein, The local interaction covariance matrix between different monitoring nodes is:

[0095]

[0096] The regional interaction covariance matrix between different monitoring nodes is:

[0097]

[0098] In order to optimize the filter gain matrix and the information fusion weighting matrix W ij , the interaction covariance matrix and the innovation covariance matrix Similar to the local filter, three types of covariance matrix calculation methods can be proposed by using the modification of the extended Kalman filter algorithm. However, using the extended Kalman method, the matrix partial derivative (Jacobian matrix) and the matrix inversion need to be calculated, and such a mode has high computational intensity. The disclosure approximates the calculation of the three types of covariance matrices by vector decomposition, expecting to avoid the calculation of the matrix partial derivative.

[0099] where the approximate solution of the interaction covariance matrix is: the disclosure adopts the method of vector decomposition to propose an approximate solution method of the interaction covariance matrix.

[0100] (a) Approximate calculation of the interaction covariance matrix

[0101] In the disclosure, the calculation of the interaction covariance matrix takes the following form:

[0102]

[0103] where,

[0104]

[0105]

[0106] The parameter a is used to scale or construct a discrete vector set of state estimation values, and is used to scale the high-order moments of the nonlinear process model in the opposite direction.

[0107] (b) Approximate calculation of the interaction covariance matrix

[0108] In the disclosure, the calculation of the interaction covariance matrix takes the following form:

[0109]

[0110] where, is a one-step prediction of the measurement, is the innovation,

[0111] (c) Approximate calculation of the interaction covariance matrix

[0112] In the disclosure, the calculation of the interaction covariance matrix takes the following form:

[0113]

[0114] where

[0115] (d) Filter gain​​​ and the weighting coefficient W ij The optimization method of the weighting coefficient W

[0116] The local filtering error is a known matrix, and the approximation of the cross-covariance matrix is The local filtering error is minimized, and thus,

[0117]

[0118] The weighting matrix W ij is fused by the optimization calculation of the regional filtering error, and the local filtering error covariance matrix is a known matrix, and thus W ij can be obtained by the following optimization calculation,

[0119]

[0120] wherein, is the i-th row of the matrix W ij The specific calculation process is as follows by the above steps 1-4:

[0121] (1) According to the initial state estimation of the i-th (i=1,…,N) target AUV A and the local filtering error matrix and the initial weighting matrix , the regional filtering covariance matrix at the initial time is calculated

[0122] (2) According to the approximation calculation method in step 4, the state one-step prediction cross-covariance matrix is calculated

[0123] (3) According to the calculation result in step 4, the innovation covariance matrix is further calculated

[0124] (4) According to the estimation value of the regional filtering and the one-step prediction estimation value of the region in step 4, the cross-covariance matrix of the filtering error and the innovation is calculated;

[0125] (5) Based on the calculation results in steps (2), (3), and (4), the optimization in step 4 is used to obtain the filtering gain gain and the weighting coefficient W ij , and the pose information of the AUV at the current time is obtained;

[0126] (6) Let k=k+1, recalculate (1)-(5) until the positioning task is completed.

[0127] Example 2

[0128] In one embodiment of the present disclosure, it is realized by means of simulation tool Matlab. The effect of the present disclosure can be further illustrated by the following experimental simulation.

[0129] The target AUV is a moving target in three-dimensional space, so the pose information of the AUV can be obtained according to the local state estimation, the regional state estimation and the further weighted information fusion to obtain real-time pose information.

[0130] In the present embodiment, considering the ring topology of four AUVs, it is assumed that the four underwater robots have the same structure, and the state space equation is as follows,

[0131] x k+1 =x k +△TJ(x k )u k +w k

[0132]

[0133] wherein,

[0134]

[0135] The covariance of the white noise w k and is set to Q=0.1*I and R=0.5*I respectively. The initial positions of the four AUVs are given as follows,

[0136]

[0137]

[0138]

[0139]

[0140] After the calculation of the distributed cooperative positioning method proposed in the present disclosure, the multi-AUV system clearly shows the trajectory of the AUV in the three-dimensional space by using its own measurement of the target, Figure 3 in the three-dimensional space. Figure 4 The angle diagram of the four underwater robots in the two-dimensional space is shown. Figure 5 It is shown that the error covariance of the distributed cooperative positioning estimation error tends to converge over time.

[0141] Embodiment 3

[0142] In one embodiment of the present disclosure, a multi-underwater robot distributed cooperative positioning system is provided, comprising:

[0143] A preprocessing module is configured to determine a target underwater robot and a monitoring node of the target underwater robot, and collect measurement information of the target underwater robot.

[0144] A kinematic modeling module is configured to establish a kinematic model, a depth measurement model, a pose measurement model, and a distance measurement model of the underwater robot in a global coordinate system.

[0145] A local estimator and a perception module are configured to use the measurement information of the target underwater robot by the monitoring node, construct a local filter according to a modified extended Kalman filter algorithm, and predict a pose state of the underwater robot.

[0146] A regional estimator and an information fusion module are configured to fuse estimated information of a neighbor node pose state, construct a regional filter based on an innovation matrix and an approximate calculation of an interaction covariance matrix, and obtain a pose state of the target underwater robot.

[0147] Specifically, in the local estimator and the perception module, the measurement information about the target AUV is output by using the information perceived by the depth measurement sensor, the IMU, and the underwater sonar sensor.

[0148] Embodiment 4

[0149] In an embodiment of the present disclosure, a computer-readable storage medium is provided, wherein a plurality of instructions are stored in the computer-readable storage medium, and the instructions are adapted to be loaded and executed by a processor of a terminal device to implement a plurality of steps of a distributed cooperative positioning method for multiple underwater robots.

[0150] Embodiment 5

[0151] In an embodiment of the present disclosure, a terminal device is provided, which includes a processor and a computer-readable storage medium. The processor is configured to implement instructions. The computer-readable storage medium is configured to store a plurality of instructions, and the instructions are adapted to be loaded and executed by the processor to implement a plurality of steps of a distributed cooperative positioning method for multiple underwater robots.

[0152] The present disclosure is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present disclosure. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and combinations of flows and / or blocks in the flowcharts and / or block diagrams can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing apparatus to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing apparatus generate a means for implementing the functions specified in the flowcharts and / or block diagrams. Figure 1 The functions specified in one flow or multiple flows and / or blocks Figure 1 The functions specified in one flow or multiple flows and / or blocks

[0153] These computer program instructions can also be loaded into a computer or other programmable data processing devices, so that a series of operational steps are performed on the computer or other programmable data processing devices to generate a computer implemented process, so that the instructions executed on the computer or other programmable data processing devices provide a process for implementing the flowchart Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0154] Although the specific embodiments of the present disclosure are described above with reference to the drawings, the description is not a limitation on the scope of protection of the present disclosure, and those skilled in the art should understand that various modifications or changes made on the basis of the technical solutions of the present disclosure without creative labor are still within the scope of protection of the present disclosure.

Claims

1. A distributed cooperative localization method for multiple underwater robots, characterized in that, include: Identify the target underwater robot and its monitoring nodes, and collect measurement information of the target underwater robot; Establish kinematic models, depth measurement models, attitude measurement models, and distance measurement models for the underwater robot in a global coordinate system; By utilizing the measurement information of the target underwater robot from the monitoring nodes, a local filter is constructed based on the modified extended Kalman filter algorithm to predict the pose state of the underwater robot. The estimated pose states of neighboring nodes are fused, and a region filter is constructed based on the approximate calculation of the innovation matrix and the interaction covariance matrix to obtain the pose state of the target underwater robot.

2. The distributed cooperative localization method for multiple underwater robots as described in claim 1, characterized in that, The process of establishing the kinematic model of multiple underwater robots in the global coordinate system includes: Considering the miniaturized target underwater robot, a discretized nonlinear model with noisy perturbations is adopted, and the position and attitude information of the target underwater robot are used as the state information to be estimated.

3. The distributed cooperative localization method for multiple underwater robots as described in claim 2, characterized in that, The attitude information includes roll angle, pitch angle, and yaw angle.

4. The distributed cooperative localization method for multiple underwater robots as described in claim 1, characterized in that, The process of establishing a depth measurement information model for multiple underwater robots in a global coordinate system includes: The target underwater robot periodically broadcasts its unknowns via an acoustic modem, enabling beacon nodes and the underwater robot within its communication range to estimate distances based on arrival time. It uses the status information of the beacon nodes and monitoring nodes as reference unknowns to construct distance measurement information. It also interacts with the monitoring nodes via broadcast to construct depth measurement information. The target underwater robot interacts with the monitoring nodes via broadcast to construct an attitude measurement information model.

5. The distributed cooperative localization method for multiple underwater robots as described in claim 1, characterized in that, The specific process of constructing the local filter is as follows: based on the measurement information of the target underwater robot by the monitoring node, the pose information of the target underwater robot is predicted and estimated based on the extended Kalman filter algorithm, and the local state estimator of the target AUV is designed using local measurement information and Taylor expansion.

6. The distributed cooperative localization method for multiple underwater robots as described in claim 1, characterized in that, The specific process of constructing the region filter is as follows: Based on the local filter, the estimated information of neighboring nodes is fused. Under the given fusion strategy, the gain matrix of the region state filter and the information fusion weighting matrix are designed through the approximate calculation of the innovation covariance matrix, the interaction innovation covariance matrix, and the state covariance matrix, so as to obtain the region estimate of the pose information of the target underwater robot.

7. The distributed cooperative localization method for multiple underwater robots as described in claim 1, characterized in that, The monitoring nodes are other underwater robots within the communication range of the target underwater robot.

8. A distributed cooperative positioning system for multiple underwater robots, characterized in that, include: The preprocessing module is used to determine the target underwater robot and the monitoring nodes of the target underwater robot, and to collect the measurement information of the target underwater robot; The kinematics modeling module is used to build the kinematic model, depth measurement model, attitude measurement model and distance measurement model of the underwater robot in the global coordinate system. The local estimator and perception module are used to construct a local filter based on the measurement information of the target underwater robot by the monitoring node and the modified extended Kalman filter algorithm to predict the pose state of the underwater robot. The region estimator and information fusion module are used to fuse the estimated information of the pose state of neighboring nodes. Based on the approximate calculation of the innovation matrix and the interaction covariance matrix, a region filter is constructed to obtain the pose state of the target underwater robot.

9. A computer-readable storage medium, characterized in that, It stores multiple instructions, which are adapted to be loaded and executed by the processor of the terminal device as a distributed cooperative localization method for multiple underwater robots according to any one of claims 1-7.

10. A terminal device, characterized in that, It includes a processor and a computer-readable storage medium, the processor being used to implement various instructions; the computer-readable storage medium being used to store multiple instructions, the instructions being adapted to be loaded by the processor and executed as described in any one of claims 1-7, a distributed cooperative localization method for multiple underwater robots.

Images