Target tracking method based on acousto-optic fusion of multiple submersible vehicles

Abstract

The invention discloses a target tracking method based on acousto-optic fusion of multiple submersible vehicles, and the method comprises the following steps: S1, carrying out the initialization and sensor calibration operation of each submersible vehicle; s2, each submersible vehicle cooperative system observes a target by using own equipment to obtain measurement data; s3, preprocessing optical measurement data; s4, when the acoustic ranging information is updated, executing an acousto-optic fusion algorithm, fusing the acoustic ranging information and the optical azimuth information in combination with cross positioning and a three-ball intersection method, and constructing a least square formula to obtain target position estimation; if not, executing a high-frequency optical target tracking algorithm, performing distance correction by using high-frequency optical observation, and obtaining target position estimation through a deviation compensation pseudo-linear Kalman filtering method; s5, when the target tracking task is completed, the tracking task is stopped; and if not, returning to S2 until the target tracking task is completed. According to the invention, starting from an acousto-optic measurement mechanism, the real-time performance and accuracy of target tracking are effectively improved by utilizing acousto-optic information fusion.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater target tracking, and particularly to a target tracking method based on acoustic-optical fusion of multiple underwater vehicles. Background Art

[0002] In underwater applications, target tracking, i.e., the continuous estimation of the target position, is one of the indispensable basic technologies for underwater vehicles. However, the harsh underwater environment, such as limited communication and measurement uncertainty, poses many challenges to improving the target tracking accuracy. In addition, underwater targets are often uncooperative and highly maneuverable, making it difficult to ensure the continuity and stability of position estimation. Underwater target tracking methods are usually designed based on acoustic measurements. However, the inherent limitations of underwater acoustics, such as slow propagation speed, low update frequency, and narrow transmission bandwidth, limit the improvement of target tracking accuracy, especially in small-scale and high-dynamic underwater scenarios. To alleviate the limitations of traditional acoustic target tracking, designing multi-source fusion algorithms has become a key approach to improving target tracking performance. Inertial navigation components and Doppler velocity loggers have been widely used in the cooperative positioning of multiple underwater vehicles but are not directly applicable to target tracking. In addition, due to the significant attenuation of underwater electromagnetic and infrared signals, radar and infrared technologies often fail in underwater applications. Although the range of optical information is limited, its high resolution and high update frequency make it an important information source. Therefore, in a small-scale and high-dynamic underwater environment, how to fuse optical measurements with high update frequency and low communication dependence and acoustic signals with low attenuation and large range to improve the tracking accuracy and timeliness of highly maneuverable underwater targets is the key to enhancing target tracking performance.

[0003] In the prior art, for the underwater multi-aUV target tracking method, the application number is CN202410875772.3, and the title is "Multi-aUV Cooperative Localization and 3D Target Localization Method Based on Azimuth Measurement". By using acoustic measurement information and combining the interactive multiple model method and the belief propagation algorithm, it realizes the target localization under variable motion states. However, the limitations of acoustics itself restrict the further improvement of tracking accuracy. For the acoustic-optic fusion technology, the application number is CN202411899037.2, and the title is "An Underwater and Above-Water Target Detection and Localization Method Based on Acoustic-Optic Fusion". By fusing the above-water optical image information with the underwater sonar information and combining the inertial guidance information, it detects and locates the target. The application number is CN202411219303.2, and the title is "An Autonomous Acoustic-Optic Fusion Target Detection Method and System for Amphibious Unmanned Platforms". By synchronously collecting underwater environment information through the optical camera and forward-looking sonar of the amphibious unmanned platform, establishing a support matrix after segmentation and target recognition, and obtaining the target localization result through optimal weighted fusion. The above-mentioned many methods explore the underwater multi-source fusion target localization method, but focus on the fusion of acoustics and optics at the image level, without considering the differences in the acoustic-optic measurement mechanisms.

[0004] Therefore, how to design a method that starts from the acoustic-optic measurement mechanism and uses acoustic-optic information fusion for underwater target tracking is an urgent problem to be solved. The technical personnel in this field are committed to developing a target tracking method based on the acoustic-optic fusion of multi-aUVs. Summary of the Invention

[0005] In view of the above-mentioned defects of the prior art, the technical problems to be solved by the present invention are: to break through the traditional target tracking technology that only relies on underwater acoustics with great limitations, improve the accuracy of aUVs in tracking underwater targets and the real-time performance of tracking high-maneuver targets; reduce the influence of multiplicative noise in the optical observation of underwater targets and obtain a higher-precision target position estimate.

[0006] To achieve the above object, the present invention provides a target tracking method based on the acoustic-optic fusion of multi-aUVs, and the method includes the following steps:

[0007] S1: Each aUV performs initialization and sensor calibration operations, and then goes underwater to execute the navigation task;

[0008] S2: The cooperative system of each aUV detects the target, performs target tracking, and uses its own equipment to observe the target to obtain measurement data;

[0009] S3: Preprocess the optical measurement data;

[0010] S4: When there is an update in the acoustic ranging information, execute the acoustic-optical fusion algorithm, combine the cross-location and three-sphere intersection methods to fuse the acoustic ranging information and the optical azimuth information, and construct a least-squares formula to obtain the target position estimate; when there is no update, execute the high-frequency optical target tracking algorithm, use the high-frequency optical observations for distance correction, and obtain the target position estimate through the bias-compensated pseudo-linear Kalman filtering method;

[0011] S5: When the target tracking task is completed, stop the tracking task; when it is not completed, return to S2 until the target tracking task is completed;

[0012] Furthermore, the preprocessing of the optical measurement data in step S3 includes:

[0013] Define the true position of the target at time k as q k =[q k，x , q k，y , q k,z T , and define the true position of the i-th submersible at time k as p i，k =[p i，k，x , p i，k，y , p i，k，z T , and the azimuth vector of the target relative to the i-th submersible is Composed of the azimuth angles φ i，k and θ i，k of the target relative to the i-th submersible in the horizontal and vertical directions:

[0014] λ i，k =[cosθ i，k cosφ i，k , cosθ i，k sinφ i，k , sinθ i，k T

[0015] where the azimuth angles can be obtained from the position of the target in the image plane,

[0016]

[0017] v i，k，x and v i，k，y are the horizontal and vertical coordinates of the target in the image plane respectively, and f is the camera focal length;

[0018] The azimuth of the target measured by each submersible has multiplicative noise:

[0019]

[0020] where R ρ (ε) represents the rotation of ε about the ρ axis, ​​​To measure noise, the rotation axis ρ is a unit vector defined as where is an arbitrary unit vector perpendicular to λ i，k and can be expressed as:

[0021]

[0022] where, satisfies a uniform distribution;

[0023] Using the Rodrigues rotation formula, R ρ (ε) and can be expressed as:

[0024]

[0025] Furthermore, the acoustic-optical fusion algorithm described in step S4 includes the following steps:

[0026] S411: Use the cross-location method to fuse the azimuth measurement information;

[0027] S412: Preprocess the acoustic data and use the three-sphere intersection method to fuse the distance measurement information;

[0028] S413: Use the least squares method to fuse the acoustic ranging information and the optical azimuth information;

[0029] Furthermore, step S411 includes:

[0030] Regard the azimuth information as rays, and take the point with the minimum sum of distances to the rays as the estimated position of the target. Define each estimated point as the perpendicular point of the fused estimated point to each ray,

[0031]

[0032] The optimization goal is to minimize the sum of the distances between the fused estimated point and each ray, that is, to minimize the distance between the fused estimated point and each estimated point.

[0033]

[0034] By optimizing the objective function, the following system of equations can be obtained,

[0035]

[0036] Furthermore, step S412 includes:

[0037] The i-th submersible uses the acoustic ranging device to measure the relative distance of the target at time k which is expressed as:

[0038]

[0039] where is Gaussian noise, and ||·||2 represents the Euclidean norm;

[0040] Use the three - sphere intersection method to construct a system of equations:

[0041]

[0042] Furthermore, step S413 includes:

[0043] Use the least - squares method to fuse the acoustic ranging information and the optical azimuth information to estimate the target position, expressed as:

[0044]

[0045] where the expressions of A = [A(1), A(2), A(3)] and B are as follows:

[0046]

[0047]

[0048] The estimated target position is obtained by obtained, which is a pseudo - inverse calculation;

[0049] Furthermore, the high - frequency optical target tracking algorithm in step S4 includes the following steps:

[0050] S421: Use high - frequency optical observations for distance correction;

[0051] S422: Linearize the non - linear optical observation information;

[0052] S423: Use pseudo - linear Kalman filtering to obtain the target position estimate;

[0053] S424: Perform bias compensation for the influence of multiplicative noise on the target position estimate;

[0054] Furthermore, step S421 includes:

[0055] Set the ratio of the update frequencies of acoustic and optical information to be r f , if the relative distance of the target in the first frame captured by the optical camera is d1, obtained by acoustic ranging; subsequent relative distances are obtained by analyzing the changes of the target in the image plane between optical frames. The relative distance ratio of the second frame estimate is d2, and the relative distance ratio can be obtained by the following formula:

[0056]

[0057] Among them, h1 and w1 are the height and width of the target ROI on the image plane of the first frame, l1 is the height of the target xOy plane on the image plane, c1 is the width of the target xOz plane on the image plane, α is the angle of rotation of the target in the second frame relative to the previous frame, and on the image plane of the second frame, a is the length of the vertex of the target yOz plane on the z-axis from the ROI frame, b is the length of the midline of the target yOz plane in the z-axis direction, and the height h2 and width w2 of the ROI can be expressed as:

[0058]

[0059] By analyzing the relationship between the relative orientation of the target and the target position between frames, the expression is obtained:

[0060]

[0061]

[0062] where r w is the ratio of the length to the width of the target, r h is the ratio of the target's length to its height,

[0063] Combining the above equations, we can estimate the relative distance of the target at the second frame:

[0064]

[0065] The expression of a is:

[0066]

[0067] Further, step S422 includes:

[0068] Introducing the orthogonal projection operator

[0069]

[0070] The orthogonal projection operator indicates that for any vector In vertical The orthogonal projection on the plane is

[0071] because Available

[0072] where ν i，k is the measurement noise vector, expressed as:

[0073]

[0074] The distance d i，kObtained by updating the high-frequency optical distance in step S421

[0075] Let z i，k be the measurement vector, and the non-linear measurement model is:

[0076] z i，k = H i，k x i，k + v i，k ,

[0077] where is the relative state vector, x q，k is the state vector of the target, is the state vector of the i-th submersible,

[0078] x q，k = F k-1 x q，k-1 + ω k-1 ,

[0079]

[0080] where, u k is the control input of the submersible, represents the process noise of a zero-mean Gaussian distribution, and

[0081]

[0082] where is the variance of the process noise, T s is the sampling time, equivalent to the time interval between two adjacent optical observations;

[0083] Furthermore, step S423 includes:

[0084] From the non-linear measurement model in step S422, we can get z i，k ≈ 0 3×1 , and the measurement information is hidden in the measurement matrix H i，k , and the recursive estimation of x i，k can be expressed as:

[0085]

[0086] where is the predicted state vector of x i，k , P k|k-1，i is the corresponding error covariance matrix, and the prediction stage of the pseudo-linear Kalman filter is:

[0087]

[0088] The correction stage of the pseudo-linear Kalman filter is:

[0089]

[0090] where the measurement covariance is R i，k varies with H i，k and is expressed as:

[0091]

[0092] where is the covariance matrix of v i，k Since the value of ε is usually very small, using the approximations sinε≈ε and cosε≈1 - ε 2 / 2, we get:

[0093]

[0094] Therefore, the covariance representation formula of v i，k is:

[0095]

[0096] Since the covariance representation formula is simplified to:

[0097]

[0098] According to we get:

[0099]

[0100] Therefore, the covariance matrix of v i，k is:

[0101]

[0102] where diag{·} represents a diagonal matrix;

[0103] Step S424 includes:

[0104] According to the matrix inversion theorem, the posterior error covariance and the Kalman gain can be rewritten as:

[0105]

[0106] The estimation error is:

[0107]

[0108] can be divided into three parts e i，k (1), e i，k (2), e i，k (3):

[0109]

[0110] Since is related to ψ that satisfies a uniform distribution, the estimation bias is obtained by taking the expectation. i，k

[0111]

[0112] Because e i，k (1) Only transmitting the bias from the previous moment to the current moment is not the main reason for the estimation bias. The non - cooperation of the target leads to being weakly correlated with ω k-1 Let

[0113] Since the noise is random and unknown, for e i，k (3) Perform bias compensation:

[0114]

[0115] Convert to calculate

[0116]

[0117] Among them,

[0118]

[0119] Therefore, in the prediction stage of update step S423:

[0120]

[0121] A target tracking method based on the acoustic - optical fusion of multiple sub - vehicles proposed by the present invention uses an acoustic - optical fusion algorithm to estimate the target position with high precision by using acoustic ranging information and optical azimuth information, improving the real - time performance and accuracy of target position estimation; designs a bias - compensation pseudo - linear Kalman filtering method to effectively reduce the influence of multiplicative noise in azimuth measurement information and improve the accuracy of target tracking; realizes real - time distance estimation of the target through a high - frequency optical target tracking algorithm, improving the real - time performance of target tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0122] Figure 1 is a schematic diagram of the architecture of the target tracking method of the present invention;

[0123] Figure 2 is a schematic diagram of the architecture of the acoustic - optical fusion algorithm of the present invention;

[0124] Figure 3 is a schematic diagram of the architecture of the high - frequency optical target tracking algorithm of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0125] The following introduces several preferred embodiments of the present invention with reference to the accompanying drawings of the specification to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the protection scope of the present invention is not limited to the embodiments mentioned in the text.

[0126] In the drawings, components with the same structure are denoted by the same numerical labels, and components with similar structures or functions everywhere are denoted by similar numerical labels. The size and thickness of each component shown in the drawings are arbitrarily shown, and the present invention does not limit the size and thickness of each component. To make the illustration clearer, the thickness of some parts in the drawings is appropriately exaggerated.

[0127] The present invention aims at the problem of underwater target tracking, and designs a target tracking method based on the acoustic-optical fusion of multiple underwater vehicles, generally considering aspects such as the non-cooperativeness and high maneuverability of the target, the acoustic-optical measurement mechanism, the target tracking accuracy, the update frequency, etc. Different from the existing methods of using acoustic-optical fusion for target tracking, the present invention considers the significant characteristic differences between acoustic and optical measurements in terms of update frequency, operating range, etc. Starting from the acoustic-optical measurement mechanism, the present invention utilizes high-frequency optical observations to alleviate the lag effect of traditional acoustic tracking on highly maneuverable targets, and improves the tracking accuracy and real-time performance. The present invention designs an acoustic-optical fusion target tracking framework, which consists of the following two parts: In the "high-frequency optical target tracking algorithm", the inter-frame change of the target in the image plane is utilized to reduce the influence of measurement delay. Aiming at the multiplicative noise in optical observations, the present invention designs a bias compensation method to reduce the estimation error; in the "acoustic-optical fusion algorithm", the present invention fuses the acoustic and optical measurement information from multiple underwater vehicles, overcomes the limitations of individual measurements, and ensures more reliable tracking in a dynamic underwater environment.

[0128] In a more specific implementation, the present invention provides a target tracking method based on the acoustic-optical fusion of multiple underwater vehicles, and the schematic diagram of the architecture of the target tracking method is as Figure 1 shown, including the following steps:

[0129] S1: Each underwater vehicle performs initialization and sensor calibration operations, and then goes underwater to execute the navigation task;

[0130] S2: The cooperative system of each underwater vehicle detects the target, performs target tracking, and observes the target using its own equipment to obtain measurement data;

[0131] S3: Preprocess the optical measurement data;

[0132] S4: When there is an update in the acoustic ranging information, execute the acoustic-optical fusion algorithm, combine the cross-location and three-sphere intersection methods to fuse the acoustic ranging information and the optical azimuth information, and construct a least-squares formula to obtain the target position estimate; when there is no update, execute the high-frequency optical target tracking algorithm, use high-frequency optical observations for distance correction, and obtain the target position estimate through the bias compensation pseudo-linear Kalman filtering method;

[0133] S5: When the target tracking task is completed, stop the tracking task; when it is not completed, return to S2 until the target tracking task is completed.

[0134] In a more specific embodiment, the preprocessing of the optical measurement data in step S3 includes:

[0135] Assume that the true position of the target at time k is defined as q k =[q k，x ,q k，y ,q k，z T ,the true position of the i-th submersible at time k is defined as p i，k =[p i，k，x ,p i，k，y ,p i，k，z T ,the azimuth vector of the target relative to the i-th submersible is Composed of the azimuth angles φ i，k and θ i，k of the target relative to the i-th submersible in the horizontal and vertical directions:

[0136] λ i，k =[cosθ i，k cosφ i，k ,cosθ i，k sinφ i，k ,sinθ i，k T ,

[0137] where the azimuth angle can be obtained from the position of the target in the image plane:

[0138]

[0139] v i，k，x and v i，k，y are the horizontal and vertical coordinates of the target in the image plane respectively, and f is the camera focal length.

[0140] The azimuth of the target measured by the submersible has multiplicative noise,

[0141]

[0142] where R ρ (ε) represents the rotation of ε about the ρ axis,​​​ For measuring noise, the rotation axis ρ is a unit vector defined as

[0143] where where is an arbitrary unit vector perpendicular to λ i，k and can be expressed as:

[0144]

[0145] where satisfies a uniform distribution.

[0146] Using the Rodrigues rotation formula, R ρ (ε) and can be expressed as:

[0147]

[0148] In a more specific embodiment, the acousto-optic fusion algorithm in step S4 includes the following steps:

[0149] Step S411: Use the cross-location method to fuse the azimuth measurement information. Regard the azimuth information as rays, and take the point with the minimum sum of distances to the rays as the estimated position of the target. This step completes the fusion at one moment, and all variables omit the subscript k. Define each estimated point as the fusion estimated point which is the perpendicular point from the fusion estimated point

[0150]

[0151] The optimization objective is to minimize the sum of the distances between the fusion estimated point and each ray, that is, to minimize the distances between the fusion estimated point and each estimated point.

[0152]

[0153] By optimizing the objective function, the following system of equations can be obtained.

[0154]

[0155] Step S412: Preprocess the acoustic data and use the three-sphere intersection method to fuse the distance measurement information of multiple submersibles to the target. The i-th submersible measures the relative distance of the target at time k using an acoustic ranging device which is expressed as:

[0156]

[0157] where is Gaussian noise, and ||·||2 represents the Euclidean norm. Use the three-sphere intersection method to construct a system of equations:

[0158]

[0159] Step S413: Use the least squares method to fuse the acoustic ranging information and the optical azimuth information to estimate the target position, expressed as:

[0160]

[0161] where the expressions of A = [A(1), A(2), A(3)] and B are as follows:

[0162]

[0163]

[0164] The target estimated position is obtained by obtained, which is the pseudo-inverse calculation. At this time, if the target tracking task is completed, stop the tracking task; if not, return to S2 until the target tracking task is completed.

[0165] In a more specific embodiment, the high-frequency optical target tracking algorithm in step S4 includes the following steps:

[0166] Step S421: Use high-frequency optical observations for distance correction. Set the ratio of the update frequencies of acoustic and optical information as r f , if the relative distance of the target in the first frame captured by the optical camera is d1, obtained by acoustic ranging; subsequent ones are obtained by analyzing the change of the target in the image plane between optical frames. Here, taking the second frame as an example, the estimated relative distance of the target is d2. According to the perspective principle, the size of the target on the image plane is inversely proportional to the distance from the target to the camera. However, due to the relative movement of the target and the submersible, both the viewing angle and the distance change, and the area of the region of interest (ROI) of the target on the image plane is not necessarily proportional to the relative distance. However, the line segments on the image plane still maintain this property. It can be considered that the lengths and heights of the same target surface between two frames are inversely proportional to the distance respectively. Therefore, the ratio of relative distances can be obtained by the following formula:

[0167]

[0168] where h1 and w1 are the height and width of the target ROI on the first-frame image plane respectively, l1 is the height of the target xOy plane on the image plane, c1 is the width of the target xOz plane on the image plane, α is the angle of rotation of the target in the second frame relative to the previous frame. On the image plane of the second frame, a is the length of the vertex of the target yOz plane from the ROI frame on the z-axis, b is the length of the midline of the target yOz plane in the z-axis direction, and the height h2 and width w2 of the ROI can be expressed as:

[0169]

[0170] By analyzing the relationship between the relative orientation of the target and the target position between frames, the expression is obtained:

[0171]

[0172]

[0173] where r w is the ratio of the length to the width of the target, r h It is the ratio of the length to the height of the target.

[0174] Combining the above equations, we can estimate the relative distance of the target at the second frame:

[0175]

[0176] The expression of a is:

[0177]

[0178] Each submersible uses its own high-frequency optical observation information to estimate the target position, specifically:

[0179] Step S422: Linearize the nonlinear optical observation information and introduce the orthogonal projection operator

[0180]

[0181] The orthogonal projection operator indicates that for any vector In vertical The orthogonal projection on the plane is because You can get:

[0182]

[0183] where v i，k is the measurement noise vector, expressed as:

[0184]

[0185] The distance d i，k Obtained by updating the high-frequency optical distance in step S421. i，k For the measurement vector, the nonlinear measurement model is:

[0186] z i，k =H i，k x i，k +v i，k ,

[0187] in is the relative state vector, x q，k is the state vector of the target, is the state vector of the i-th submersible,

[0188] x q，k = F k-1 x q，k-1 + ω k-1 ,

[0189]

[0190] where u k is the control input of the submersible, represents the process noise of a zero-mean Gaussian distribution, and

[0191]

[0192] where is the variance of the process noise, T s is the sampling time, equivalent to the time interval between two adjacent optical observations.

[0193] Step S423: Each submersible obtains the target position estimate using the pseudo-linear Kalman filter. From the non-linear measurement model in step S422, we can get z i，k ≈ 0 3×1 , and the measurement information is implicit in the measurement matrix H i，k . The recursive estimation of x i，k can be expressed as:

[0194]

[0195] where is the predicted state vector of x i，k , P k|k-1，i is the corresponding error covariance matrix. The prediction stage of the pseudo-linear Kalman filter is:

[0196]

[0197] The correction stage of the pseudo-linear Kalman filter is:

[0198] where the measurement covariance R i，k varies with H i，k and is expressed as

[0199]

[0200] where is for v i，kThe covariance matrix. Since the value of ε is usually very small, using the approximations sinε≈ε and cosε≈1 - ε 2 / 2, we get:

[0201]

[0202] Therefore, v i，k The covariance is expressed as:

[0203]

[0204] Since Simplify the above formula to:

[0205]

[0206] According to We get:

[0207]

[0208] Therefore, v i，k The covariance matrix is:

[0209]

[0210] where diag{·} represents a diagonal matrix.

[0211] Step S424: Each submersible performs bias compensation for the influence of multiplicative noise on position estimation. According to the matrix inversion theorem, the posterior error covariance and the Kalman gain can be rewritten as:

[0212]

[0213] The estimation error is:

[0214]

[0215] It can be divided into three parts e i，k (1), e i，k (2), e i，k (3):

[0216]

[0217] Since is related to ψ i，k which satisfies a uniform distribution, the estimation bias is obtained by taking the expectation,

[0218]

[0219] Because e i，k(1) Only passing the deviation from the previous moment to the current moment is not the main reason for estimating the deviation. The non - cooperation of the target leads to being weakly correlated with ω k-1 and can be set as

[0220] Since the noise is random and unknown, for e i，k (3) Conduct deviation compensation:

[0221]

[0222] Convert it for calculation

[0223]

[0224] where

[0225]

[0226] Therefore, in the prediction phase of update step S423:

[0227]

[0228] At this time, if the target tracking task is completed, stop the tracking task; if not, return to S2 until the target tracking task is completed.

[0229] The present invention is used in the scenario of multi - AUV cooperative target tracking. Each AUV needs to be equipped with an inertial measurement unit, a depth sensor, a control and power module, an underwater acoustic communication module and an underwater acoustic measurement module to achieve the attitude and motion control of the AUV itself and the communication and relative measurement between AUVs. At the same time, the AUV needs to have the ability of target detection and measurement to support the perception and measurement of the target position, including an acoustic ranging module and an optical camera. The above requirements are all basic hardware requirements in the multi - AUV cooperative scenario. Deploying the algorithm proposed by the present invention on this hardware basis can achieve the acoustic - optical fusion target tracking of multi - AUVs, reflecting the practicability and easy expandability of the present invention.

[0230] The preferred specific embodiments of the present invention have been described in detail above. It should be understood that those of ordinary skill in the art can make many modifications and variations according to the concept of the present invention without creative work. Therefore, all technical solutions that can be obtained by those skilled in the art in the technical field based on the concept of the present invention through logical analysis, reasoning or limited experiments on the basis of the existing technology should be within the protection scope determined by the claims.

Claims

1. A target tracking method based on multi-submersible acoustic and optical fusion, characterized in that: The method comprises the following steps: S1: Each submersible performs initialization and sensor calibration operations and is launched into the water to perform navigation tasks; S2: Each submersible cooperates with the system to detect the target, perform target tracking, observe the target using its own equipment, and obtain measurement data; S3: optical measurement data preprocessing; S4: When the acoustic ranging information is updated, the acoustic-optical fusion algorithm is executed, the acoustic ranging information and the optical orientation information are fused by combining the cross positioning and three-ball intersection method, and the least squares formula is constructed to obtain the target position estimate; when there is no update, the high-frequency optical target tracking algorithm is executed, and the distance correction is performed using high-frequency optical observation, and the target position estimate is obtained by the deviation compensation pseudo-linear Kalman filtering method; S5: When the target tracking task is completed, stop the tracking task; if it is not completed, return to S2 until the target tracking task is completed.

2. The target tracking method based on multi-submersible acoustic-optical fusion as claimed in claim 1, characterized in that: The optical measurement data preprocessing in step S3 includes: The actual position of the target at time k is defined as The actual position of the i-th submersible at time k is defined as p i，k =[p i，k，x , p i，k，y , p i，k，z ] T , the azimuth vector of the target relative to the i-th submersible is The azimuth angle φ of the target relative to the i-th submersible in the horizontal and vertical directions i，k and θ i，k composition: l i,k =[cosθ i，k cosφ i，k ,cosθ i，k sinφ i,k ,sinth i，k ] T , The azimuth angle can be obtained from the position of the target on the image plane. v i，k，x and v i，k，y are the horizontal and vertical coordinates of the target on the image plane, respectively, and f is the focal length of the camera; The target position measured by each submersible has multiplicative noise: Where R ρ (ε) represents the rotation of ε on the ρ axis, To measure noise, the rotation axis ρ is a unit vector defined as in is a perpendicular i，k Any unit vector can be expressed as: in, Satisfy uniform distribution; Using the Rodrigues rotation formula, R ρ (ε) and It can be expressed as:

3. The target tracking method based on multi-submersible acoustic-optical fusion as claimed in claim 2, characterized in that: The sound and light fusion algorithm in step S4 includes the following steps: S411: using a cross positioning method to fuse the azimuth measurement information; S412: preprocessing the acoustic data and fusing the distance measurement information using the three-sphere intersection method; S413: Use the least squares method to fuse the acoustic ranging information and the optical orientation information.

4. The target tracking method based on multi-submersible acoustic-optical fusion as claimed in claim 3, characterized in that: Step S411 includes: The orientation information is regarded as a ray, and the point with the smallest sum of distances to the ray is taken as the estimated position of the target. is the fusion estimation point To the perpendicular point of each ray, The optimization goal is to minimize the sum of the distances from the fused estimated point to each ray, that is, the distance from the fused estimated point to each estimated point is minimized. By optimizing the objective function, we can obtain the following set of equations:

5. The target tracking method based on multi-submersible acoustic-optical fusion as claimed in claim 4, characterized in that: Step S412 includes: The i-th submersible uses the acoustic ranging equipment to measure the relative distance of the target at time k It is expressed as: in is Gaussian noise, ||·||2 represents the Euclidean norm; Use the three-sphere intersection method to construct the system of equations:

6. The target tracking method based on multi-submersible acoustic-optical fusion as claimed in claim 5, characterized in that: Step S413 includes: The target position is estimated by fusing acoustic ranging information and optical orientation information using the least squares method, which is expressed as: Where A = [A(1), A(2), A(3)] and B is expressed as follows: The estimated target position is given by get, Pseudo-inverse calculation.

7. The target tracking method based on multi-submersible acoustic-optical fusion as claimed in claim 2, characterized in that: The high-frequency optical target tracking algorithm in step S4 includes the following steps: S421: Use high-frequency optical observation for distance correction; S422: Linearizing nonlinear optical observation information; S423: Obtain target position estimation using pseudo linear Kalman filtering; S424: Perform deviation compensation for the influence of multiplicative noise on the target position estimation.

8. The target tracking method based on multi-submersible acoustic-optical fusion as claimed in claim 7, characterized in that: Step S421 includes: Set the ratio of acoustic and optical information update frequency to r f , if the relative distance of the target in the first frame captured by the optical camera is d1, obtained by acoustic ranging; then by analyzing the changes of the target on the image plane between optical frames, the relative distance of the target estimated in the second frame is d2, and the ratio of the relative distances can be obtained by the following formula: Among them, h1 and w1 are the height and width of the target ROI on the image plane of the first frame, l1 is the height of the target xOy plane on the image plane, c1 is the width of the target xOz plane on the image plane, α is the angle of rotation of the target in the second frame relative to the previous frame, and on the image plane of the second frame, a is the length of the vertex of the target yOz plane on the z-axis from the ROI frame, b is the length of the midline of the target yOz plane in the z-axis direction, and the height h2 and width w2 of the ROI can be expressed as: By analyzing the relationship between the relative orientation of the target and the target position between frames, the expression is obtained: where r w is the ratio of the length to the width of the target, r h is the ratio of the target's length to its height, Combining the above equations, we can estimate the relative distance of the target at the second frame: The expression of a is:

9. The target tracking method based on multi-submersible acoustic-optical fusion as claimed in claim 8, characterized in that: Step S422 includes: Introducing the orthogonal projection operator The orthogonal projection operator indicates that for any vector In vertical The orthogonal projection on the plane is because Available where v i，k is the measurement noise vector, expressed as: The distance d i，k Obtained through the high-frequency optical distance update in step S421, Assume z i，k For the measurement vector, the nonlinear measurement model is: z i,k =H i，k x i,k +ν i，k in is the relative state vector, x q，k is the state vector of the target, is the state vector of the ith submersible, x q，k =F k-1 x q，k-1 +ω k-1 , Among them, u k is the control input of the submersible, represents the process noise with zero-mean Gaussian distribution, and in is the variance of the process noise, T s is the sampling time, which is equivalent to the time interval between two adjacent optical observations.

10. The target tracking method based on multi-submersible acoustic-optical fusion as claimed in claim 9, characterized in that: Step S423 includes: From the nonlinear measurement model in step S422, we can get z i，k ≈0 3×1 , the measurement information is implicit in the measurement matrix H i，k In, x i，k The recursive estimation of can be expressed as: in is x i，k The predicted state vector, P k|k-1，i is the corresponding error covariance matrix, and the prediction stage of the pseudo-linear Kalman filter is: The correction stage of the pseudo-linear Kalman filter is: The measurement covariance R i，k Follow H i，k Changes with changes, expressed as: in v i，k The covariance matrix of , since the value of ε is usually small, uses the approximate values ​​sinε≈ε and cosε≈1-ε 2 / 2, we get: Therefore, v i，k The covariance expression formula is: because The covariance expression formula is simplified as follows: according to get: Therefore, v i，k The covariance matrix of is: Where diag{·} represents a diagonal matrix; Step S424 includes: According to the matrix inversion theorem, the posterior error covariance and Kalman gain can be rewritten as: The estimated error is: Can be divided into three parts i，k (1)e i，k (2)e i，k (3): because and ψ that satisfies uniform distribution i，k The estimated deviation is obtained by taking the expectation, Because e i，k (1) The transfer of bias from the previous moment to the current moment is not the main cause of estimation bias. The non-cooperative nature of the target leads to With ω k-1 Weak correlation, assuming Since the noise is random and unknown, i，k (3) Perform deviation compensation: Convert to Calculation in, Therefore, the prediction stage of step S423 is updated:

Images