CN-121995316-A - Underwater positioning method based on Sage-Husa self-adaptive Kalman filtering

CN121995316ACN 121995316 ACN121995316 ACN 121995316ACN-121995316-A

Abstract

An underwater positioning method based on Sage-Husa self-adaptive Kalman filtering comprises the following steps of setting a plane ternary array to collect water surface ship signals, calculating a pitch angle and an azimuth angle of the water surface ship to the plane ternary array by adopting a time delay estimation method, calculating three-axis coordinates and speed of the water surface ship under a ground coordinate by adopting a coordinate rotation method, correcting the coordinate position and speed by adopting the Sage-Husa self-adaptive Kalman filtering to obtain stable ship position and axial speed, calculating a ship track in advance by adopting a least square method, and predicting the minimum distance between the water surface ship and an observation point before the water surface ship passes the observation point. The method effectively prolongs the working time of the system under water by reducing the number of the array elements, simultaneously reduces the loss of precision caused by the reduction of the number of the array elements by a time delay estimation algorithm, reduces the influence of external noise by a time-varying self-adaptive Kalman filtering method based on Sage-Huge, and greatly improves the tracking capability when the observation parameters are obviously changed.

Inventors

WANG XINGYU
QIU TAO
SHAO MINGQI
ZHAO AIGANG

Assignees

宜昌测试技术研究所

Dates

Publication Date: 20260508
Application Date: 20251224

Claims (7)

1. An underwater positioning method based on Sage-Husa self-adaptive Kalman filtering is characterized by comprising the following steps: s1, setting a plane ternary array, and respectively acquiring target ship signals; s2, calculating the pitch angle and the azimuth angle of the target ship relative to the plane ternary array by adopting a time delay estimation method; s3, calculating the three-axis x, y and z coordinates and the x and y-axis speeds of the water surface ship under the geodetic coordinates by adopting a coordinate rotation method; S4, correcting the coordinate positions and speeds of the x axis and the y axis by adopting self-adaptive Kalman filtering based on Sage-Husa to obtain stable ship positions and axial speeds.
2. The method of claim 1, wherein the three array elements of the planar ternary array are in a regular triangular distribution.
3. The method according to claim 1, wherein the specific method of step S2 comprises: When the T position of the target ship is far greater than the interval between the array elements, the target ship is regarded as a point sound source, and spherical waves are emitted to the three points of the plane ternary array; Let B be the signal arrival time delay relative to A , Is the sound velocity; When the signal sampling period is When the signal of the point B relative to the point A reaches the final time delay: in the formula, The signal arrival time delay of the point B relative to the point A when the acquired A, B two-point received signal correlation function obtains the maximum value; in the formula, As the peak-to-peak value of the correlation function, 、 The two adjacent point values are about the peak value of the correlation function; The same method is used for acquiring the final time delay of the signal between other two points; with plane ternary array central points As the origin of coordinates, if the distances between the array elements A, B, C and the center point are r, the azimuth angle α of the target ship is: ; The pitch angle beta of the target ship is as follows: in the formula, , Is the signal arrival delay of B relative to C.
4. The method according to claim 1, wherein the specific method of step S3 comprises: measuring the angle of a compass observation plane coordinate system relative to a geodetic coordinate system on a measurement platform, namely a pitch angle Swing angle Azimuth angle ; Based on the principle of space coordinate system conversion, a transformation matrix from an observation platform coordinate system to a geodetic coordinate system is obtained: Is recorded as Solving to obtain the actual pitch angle of the target ship in the geodetic coordinate system And azimuth angle The method comprises the following steps: Wherein the method comprises the steps of , ; After the height of the observation platform from the water surface is determined, the position and the speed of the target ship under the geodetic coordinate system are determined through the actual pitch angle and the azimuth angle.
5. The method according to claim 1, wherein the specific method of step S4 includes: marking the position of a target ship under a geodetic coordinate system as an observation variable: the ship position coordinates and axial velocity are recorded as state variables: , the following observation equation is based on the adaptive Kalman filter of the Saga-Husa: The sensor precision and the underwater noise are white noise, and the observation equation meets the following conditions: Wherein the method comprises the steps of As a mean value of the process noise, In order to measure the mean value of the noise, The prediction process comprises the following steps: Wherein, the In order to predict the state vector of the object, A covariance matrix of the prediction error; the Kalman filtering updating process comprises the following steps: Wherein the method comprises the steps of Is a matrix of units which is a matrix of units, Is a filtering gain matrix; Wherein the method comprises the steps of As the weight factor of the weight factor, Is forgetting factor and 。
6. The method according to any one of claims 1-5, further comprising the step of: s5, predicting the minimum distance between the surface ship and the observation point in advance based on the current data.
7. The method according to claim 6, wherein the specific method of step S5 includes: After stable geodetic coordinates and axial speed output are obtained through time-varying Kalman filtering, estimating a target track by adopting a least square linear fitting method, and predicting the minimum distance between a target and an observation point: the least square linear fitting method assumes the target to move linearly at a uniform speed, and the motion equation is The observed quantity obtained at time t is: Wherein n is the fitting point number; Least squares estimation parameters: the minimum distance is 。

Description

Underwater positioning method based on Sage-Husa self-adaptive Kalman filtering Technical Field The invention relates to the technical field of target detection, in particular to an underwater positioning method based on Sage-Husa self-adaptive Kalman filtering. Background Environmental perception is a primary link for realizing interaction between mines and surrounding environment, and array element arrangement and algorithm selection have great influence on perception precision and perception speed. The environment sensing system obtains useful information through various technical means through the original data of a plurality of sensors so as to obtain the current and future states of a target ship on the water surface, and provides accurate target positions and motion parameters for mines. Since the mine needs to operate underwater in a small volume, it is necessary to simplify the structure of its external sensor as much as possible and reduce the load of the power module by the multiple sensors. The acoustic signal has problems of background noise and multipath propagation under water, so that deviation can occur in a detection space, the direction finding and speed measuring information acquired by an underwater acoustic array can be caused to have larger fluctuation, even the condition of a wild value occurs, acquisition errors need to be counteracted by a multi-sensor array, or the interference of noise to a system is reduced by adopting a high-precision data optimization method. The existing acoustic signal sensing field utilizes array element arrangement modes with multiple symmetries such as five-element cross array, eight-element circular array and the like to improve detection precision and detection distance, but simultaneously increases the load of a power supply system and conflicts with the miniaturization of mines. Disclosure of Invention The invention provides an underwater positioning method based on Sage-Husa self-adaptive Kalman filtering, which adopts a minimum positioning array element, namely a ternary triangular array to detect a water surface ship, aims at solving the problem of insufficient precision of the ternary triangular array, obtains the distance and the azimuth of a ship target and a detection platform under a geodetic coordinate system through time delay estimation and coordinate system conversion, improves the detection precision of ternary detection, aims at solving the problem that an acoustic signal has underwater background noise, and provides a positioning data processing method based on Sage-Husa self-adaptive Kalman filtering, which can effectively improve the detection precision of the detection platform to the target ship, reduce the problem of large-amplitude spatial offset of the target caused by noise and obtain a more accurate water surface positioning result. The method mainly comprises the following steps: s1, setting a plane ternary array, and respectively acquiring target ship signals; s2, calculating the pitch angle and the azimuth angle of the target ship relative to the plane ternary array by adopting a time delay estimation method; s3, calculating the three-axis x, y and z coordinates and the x and y-axis speeds of the water surface ship under the geodetic coordinates by adopting a coordinate rotation method; S4, correcting the coordinate positions and speeds of the x axis and the y axis by adopting self-adaptive Kalman filtering based on Sage-Husa to obtain stable ship positions and axial speeds, and the stable ship positions and axial speeds are used for predicting the ship positions in a short time, so that the striking precision is improved. Further, three array elements of the planar ternary array are distributed in an equilateral triangle. Further, the specific method in step S2 includes: When the T position of the target ship is far greater than the interval between the array elements, the target ship is regarded as a point sound source, and spherical waves are emitted to the three points of the plane ternary array; Let B be the signal arrival time delay relative to A ,Is the sound velocity; When the signal sampling period is When the signal of the point B relative to the point A reaches the final time delay: in the formula, The signal arrival time delay of the point B relative to the point A when the acquired A, B two-point received signal correlation function obtains the maximum value; in the formula, As the peak-to-peak value of the correlation function,、The two adjacent point values are about the peak value of the correlation function; The same method is used for acquiring the final time delay of the signal between other two points; with plane ternary array central points As the origin of coordinates, if the distances between the array elements A, B, C and the center point are r, the azimuth angle α of the target ship is: ; The pitch angle beta of the target ship is as follows: in the formula, ,Is the signal arrival delay of B relative