阅读说明：本技术 一种基于多假设深度的水底固定目标的三维定位方法 (Three-dimensional positioning method for underwater fixed target based on multiple assumed depths ) 是由 齐滨 付进 梁国龙 王燕 苏钰 孙思博 王逸林 邹男 张光普 王晋晋 于 2019-10-24 设计创作，主要内容包括：本发明公开一种基于多假设深度的水底固定目标的三维定位方法。建立坐标系并存储当前时刻相关信息,通过门限条件筛选当前时刻信息和历史信息匹配成对并列出方程,解算方程得到结果,对同一深度的方程结果进行滑动平均,用滑动平均结果来剔除假设深度和对应定位结果不一致的数值,再检测是否有新的量测值,当有新的量测值时,重复进行上述过程,当没有新的量测值时,则定位结束。水声定位系统是通过对水下目标测距或者测向,从而在某一参照系下对目标进行定位的系统,在不同的目标环境中,用到的定位方法不同。(The invention discloses a three-dimensional positioning method for a water bottom fixed target based on multiple assumed depths. Establishing a coordinate system, storing relevant information of the current time, screening the current time information and historical information through a threshold condition, matching the current time information and the historical information in pairs, parallel-arranging equations, resolving the equations to obtain results, performing sliding averaging on the equation results at the same depth, using the sliding averaging results to eliminate numerical values with different assumed depths and corresponding positioning results, detecting whether new measurement values exist or not, repeating the process when new measurement values exist, and ending positioning when new measurement values do not exist. The underwater acoustic positioning system is a system for positioning an underwater target under a certain reference system by ranging or direction finding of the underwater target, and different positioning methods are used in different target environments.)

1. A three-dimensional positioning method for a water bottom fixed target based on multiple assumed depths is characterized by comprising the following steps:

step 1: selecting local longitude and latitude depth as an origin, and establishing a coordinate system;

step 2: storing platform information and measurement information at the current moment in a coordinate system;

and step 3: performing one-to-one traversal matching on the current moment information and history storage information of all other frames;

and 4, step 4: after the matching process is finished, returning to the step 2 when no matching pair meeting the threshold condition exists, and performing to the step 5 when a matching pair meeting the threshold condition exists;

and 5: collecting information pairs meeting the conditions, extracting a pair of matched platform information and measurement information, and listing an equation according to a geometric relationship;

step 6: the depth unknowns in the equation in the step 5 are respectively replaced by a plurality of assumed values according to the water depth condition of the specific test area, and the equation set is solved to obtain the abscissa and the ordinate of the target;

and 7: performing sliding average on all historical calculation results at the same depth to obtain an average abscissa and an average ordinate at the same depth, and performing sliding average on the result at each depth;

and 8: storing the positioning result after the sliding average of each assumed depth, and obtaining the abscissa and the ordinate of the corresponding depth from the positioning result;

and step 9: according to the abscissa and the ordinate obtained in step 8, verifying whether the depth is consistent with the corresponding hypothesis,

when the verification depth is consistent with the corresponding hypothesis, the hypothesis depth and the corresponding positioning result are retained,

when the verification depth is inconsistent with the corresponding hypothesis, rejecting the hypothesis depth and the corresponding positioning result;

step 10: the depth and positioning results of the remaining multiple hypotheses except for step 9 are retained;

step 11: whether a new measurement value exists or not is judged,

when a new measurement value exists, returning to the step 2 to repeat the process; when there is no new measurement value, the positioning is finished.

2. The method according to claim 1, wherein the information stored in step 2 from the detection of the target comprises detection attitude results, coordinate information and azimuth-and-attitude-meter information of the unmanned underwater vehicle UUV.

3. The method according to claim 1, wherein in step 3, the distance between the latest frame coordinate and the historical coordinate of the UUV, the included angle between the latest frame navigation direction and the historical navigation direction of the UUV, and the physical quantity to be tested are: the attitude angle of the unmanned underwater vehicle UUV in the latest frame and the historical attitude angle are both required to be within a set range. When matching and checking with historical information, there are two cases: first, there are one or more pairs of information that meet all of the above conditions; the second absence is a pair of information that meets all of the above conditions. When the first condition occurs, entering step 4; when the second situation occurs, the step 2 is continued to collect new information, then the step 4 is matched and checked until the first situation occurs, and then the step 5 is entered.

4. The method according to claim 1, wherein the threshold conditions in step 4 are specifically a distance, an included angle and an azimuth angle, the threshold of the distance is set to be 5% of the detection distance, and the satisfied range of the included angle is set to be 45-135 °; the satisfying range of the azimuth angle is set to 30-150 deg.

5. The method of claim 1, wherein step 5 results in two equations containing three unknowns, namely the abscissa, the ordinate and the depth of the object in the coordinate system.

6. The method according to claim 1, wherein in the step 6, two equations cannot solve three unknowns, so that a plurality of assumed values are provided for replacing depth unknowns in the three unknowns according to the water depth condition of a specific test area, the two equations solve the two unknowns, an equation set is solved to obtain a horizontal coordinate and a vertical coordinate of the target, and the step 7 is sequentially performed by traversing the depth in the step 6.

7. The method of claim 1, wherein the positioning results of all the assumed depths in step 7 are performed with a sliding average, and the averaged result is used as the final result at each time, and since there are multiple assumed depths, each assumed depth corresponds to one solved abscissa and ordinate at each time.

Technical Field

The invention belongs to the technical field of three-dimensional positioning of underwater targets; in particular to a three-dimensional positioning method of a water bottom fixed target based on multiple assumed depths.

Background

Disclosure of Invention

The underwater acoustic positioning system is a system for positioning an underwater target under a certain reference system by ranging or direction finding of the underwater target, and different positioning methods are used in different target environments; for passive long baseline positioning, if the target is non-cooperative, the used method has hyperbolic intersection based on relative time delay; if the targets are cooperative, the method is used for distance intersection based on absolute time delay, for an ultra-short base line, the targets are cooperative and can be positioned through time delay and direction finding, and the method is suitable for positioning the fixed targets at the water bottom with the approximately known depth.

The invention is realized by the following technical scheme:

a method for three-dimensional localization of a fixed target at a water bottom based on multiple assumed depths, the method comprising the steps of:

step 1: selecting local longitude and latitude depth as an origin, and establishing a coordinate system;

step 2: storing platform information and measurement information at the current moment in a coordinate system;

and step 3: performing one-to-one traversal matching on the current moment information and history storage information of all other frames;

and 4, step 4: after the matching process is finished, returning to the step 2 when no matching pair meeting the threshold condition exists, and performing to the step 5 when a matching pair meeting the threshold condition exists;

and 5: collecting information pairs meeting the conditions, extracting a pair of matched platform information and measurement information, and listing an equation according to a geometric relationship;

step 6: the depth unknowns in the equation in the step 5 are respectively replaced by a plurality of assumed values according to the water depth condition of the specific test area, and the equation set is solved to obtain the abscissa and the ordinate of the target;

and 7: performing sliding average on all historical calculation results at the same depth to obtain an average abscissa and an average ordinate at the same depth, and performing sliding average on the result at each depth;

and 8: storing the positioning result after the sliding average of each assumed depth, and obtaining the abscissa and the ordinate of the corresponding depth from the positioning result;

and step 9: according to the abscissa and the ordinate obtained in step 8, verifying whether the depth is consistent with the corresponding hypothesis,

when the verification depth is consistent with the corresponding hypothesis, the hypothesis depth and the corresponding positioning result are retained,

when the verification depth is inconsistent with the corresponding hypothesis, rejecting the hypothesis depth and the corresponding positioning result;

step 10: the depth and positioning results of the remaining multiple hypotheses except for step 9 are retained;

step 11: whether a new measurement value exists or not is judged,

when a new measurement value exists, returning to the step 2 to repeat the process; when there is no new measurement value, the positioning is finished.

Further, in the step 2, from the detection of the target, the stored information includes a detection attitude and heading angle result, coordinate information and azimuth and heading instrument information of the unmanned underwater vehicle UUV.

Further, in the step 3, the distance between the latest frame coordinate of the unmanned underwater vehicle UUV and the historical coordinate, and the included angle between the latest frame navigation direction of the unmanned underwater vehicle UUV and the historical navigation direction are the physical quantities to be checked: the attitude angle of the unmanned underwater vehicle UUV in the latest frame and the historical attitude angle are both required to be within a set range. When matching and checking with historical information, there are two cases: first, there are one or more pairs of information that meet all of the above conditions; the second absence is a pair of information that meets all of the above conditions. When the first condition occurs, entering the next step, namely step 4; when the second condition occurs, continuing to wait for step 2 to collect new information, then performing matching and checking in step four until the first condition occurs, and then entering the next step.

Further, the threshold conditions in the step 4 are specifically a distance, an included angle and an azimuth angle, the threshold of the distance is set to be 5% of the detection distance, and the satisfied range of the included angle is set to be 45-135 degrees; the satisfying range of the azimuth angle is set to 30-150 deg.

Further, step 5 obtains two equations, which include three unknowns, i.e., the abscissa, the ordinate, and the depth of the target in the coordinate system.

Further, the two equations in the step 6 cannot solve the three unknowns, so that a plurality of assumed values of the depth unknowns in the three unknowns are provided according to the water depth condition of the specific test area for respective substitution, the two equations solve the two unknowns, the equation set is solved to obtain the abscissa and the ordinate of the target, the depth in the step 6 is traversed, and the next step is sequentially performed.

Furthermore, in step 7, the positioning results of all the assumed depths are subjected to a sliding average, and the average result is taken as a final result at each time.

The invention has the beneficial effects that:

under the premise of roughly estimating the depth range of the water area, positioning the underwater fixed target. The positioning method adopts a multi-hypothesis depth method, can avoid positioning failure caused by slight difference between the actual water depth and the estimated water depth, has high fault tolerance rate, and can provide reference for positioning uneven water bottom; secondly, for the screening of available information before calculation, the practical situation is considered, and the acceptable error of the calculation result every time is ensured; and finally, adding a sliding average in the calculation, wherein the calculation at different moments is independent from each other and can be regarded as different events, and the error of the added average can be reduced from the angle of probability, and the reason that the average is the sliding is that the calculation result of the large error in the previous period is not expected to influence all moments because the error of the measurement information is large when the UUV is far away from the target, the error of the calculation result is possibly large, so that the calculation result of the large error in the previous period is n when the sliding average is calculated, when n is an even number, the sliding average is calculated by selecting data from n/2 to the last, and when n is an odd number, the sliding average is calculated by selecting data from (n +1)/2 to the last.

Drawings

FIG. 1 is a flow chart of the present invention.

Fig. 2 is a diagram of the matching process of the current time information and the historical time information according to the present invention.

FIG. 3 is a schematic diagram of the geometric relationship corresponding to the equations listed in the present invention.

Fig. 4 is a diagram of the relative situation of an unmanned underwater vehicle UUV and a target in simulation of the present invention-top view.

Fig. 5 is a diagram of the relative situation of an unmanned underwater vehicle UUV and a target in simulation, which is a three-dimensional diagram.

FIG. 6 shows the result of resolving x and y coordinates corresponding to a plurality of assumed depths in the simulation of the present invention.

FIG. 7 is a comparison graph of the error before and after the moving average of the x and y coordinates corresponding to the real depth.

FIG. 8 is a relative diagram of the multi-hypothesis depth-resolved target position under the flat seabed condition of the present invention.

Detailed Description

The technical solutions in the embodiments of the present invention will be described clearly and completely with reference to the accompanying drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

Assuming that the sea depth is 2000 m, the sea bottom is flat; the initial position of the unmanned underwater vehicle UUV is (0, 0, -1000), the navigation speed is 1.5 m/s, the course change rate is 1.5 degrees/s, and the unmanned underwater vehicle UUV keeps navigating at the same depth; the target is located at (1000, -1200, -2000) and is immobilized. The situation diagrams are shown in fig. 4 and fig. 5. The linear array is arranged on the unmanned underwater vehicle UUV and is consistent with the heading direction of the unmanned underwater vehicle UUV. In the present application, the azimuth angle and the course angle are both angles, and the main steps for realizing the positioning are shown in FIG. 1,

the multiple hypothesis depth is a plurality of hypothesis depth values.

A method for three-dimensional localization of a fixed target at a water bottom based on multiple assumed depths, the method comprising the steps of:

step 1: selecting local longitude and latitude depth as an origin, and establishing a coordinate system;

signals of the underwater target are received by a linear array in the unmanned underwater vehicle UUV, and are processed conventionally, so that an azimuth angle under an array coordinate can be obtained, and the azimuth angle is an azimuth angle containing three-dimensional information due to the fact that the target and the unmanned underwater vehicle UUV have depth difference. Storing the azimuth angle at the moment, namely a measurement value; and meanwhile, the coordinates and the heading angle (the angle from the right north direction to the heading direction clockwise) of the unmanned underwater vehicle UUV are also stored, and whether the detection target is positioned on a port or a starboard is detected.

Step 2: storing platform information and measurement information at the current moment in a coordinate system;

and step 3: performing one-to-one traversal matching on the current moment information and history storage information of all other frames;

and 4, step 4: after the matching process is finished, returning to the step 2 when no matching pair meeting the threshold condition exists, and performing to the step 5 when a matching pair meeting the threshold condition exists;

traversing and matching the current frame and all historical frames one by one, and when matching each time, using a threshold to see whether the frame meets the requirement, and when the matching of all frame information is finished, determining whether a matching pair meeting the requirement exists;

and 5: collecting information pairs meeting the conditions, extracting a pair of matched platform information and measurement information, and listing an equation according to a geometric relationship;

and after the current time information is stored, matching the current time information with the historical time information, and traversing all historical times. The matching process is shown in fig. 2, and the matching process is divided into three parts: firstly, the heading of the unmanned underwater vehicle UUV at two moments is considered, and the reason for considering the heading is that when the unmanned underwater vehicle UUV travels straight, the resolving error is too large, so the resolving is influenced by considering the heading. Through a large number of simulation verifications, when the course difference value is 90 degrees or 270 degrees, the error of the calculation result is minimum, meanwhile, the simulation finds that the calculation error is acceptable within a certain range around 90 degrees, and a large number of simulations indicate that the range is 45 degrees to 135 degrees or 225 degrees to 315 degrees; second, the distance between the coordinates of the unmanned underwater vehicle UUV at two times is considered because if the two points are too close, the coordinate error at the intersection will be relatively large in the geometric relationship. The simulation result shows that the maximum detection distance is 5%, the detection distance in the experiment is 3000 m, and the distance threshold can be set to be 150 m; thirdly, the azimuth angle at two times, i.e. the range of the measured value, is considered because in practical engineering, the linear array has a large error in the vicinity of 0 ° and 180 ° when the signal is processed normally, i.e. the angle in the vicinity of 0 ° and 180 ° is not very reliable. The desirable range of this amount is set from 30 to-150 in the present experiment, depending on the actual situation.

And a plurality of pairs of matching succeeds, and only one pair of matching successfully information needs to be taken.

The geometrical relationship is shown in fig. 3 according to the information column equation of two points successfully matched. The two points are respectively corresponding to an equation, the physical meaning of the equations is that the vector angle between the vector of the connecting line direction of the coordinates of the unmanned underwater vehicle UUV and the target coordinates and the heading direction of the unmanned underwater vehicle UUV at the moment is a detected azimuth angle, namely a measured value, and the equations are established according to the quantity product formula of the vectors. As shown in the geometric relationship in fig. 3, knowing that the coordinates of the unmanned underwater vehicle UUV at time t1 are (x1, y1, z1), the heading angle is heading1, the measured azimuth angle is θ 1, the coordinates of the unmanned underwater vehicle UUV at time t2 are (x2, y2, z2), the heading angle is heading2, and the measured azimuth angle is θ 2; the target coordinates are unknown and can be set to (x0, y0, z 0).

Equation establishment process at time t 1: firstly, solving a vector of the coordinates of the unmanned underwater vehicle UUV and the direction of a target connecting line at the moment as (x0-x1, y0-y1, z0-z 1); secondly, calculating vectors in the heading direction, namely (sin (heading1 pi/180), cos (heading1 pi/180) and 0); according to the measurement, the included angle between the two vectors is θ 1, and an equation is established according to the quantity product formula of the vectors, i.e. a | · | b |. cos (θ):

wherein (sin (leading 1 × π/180) means angle radian change, and calculating sine value

Similarly, the same equation is established at time t 2:

wherein (sin (leading 2 × π/180) means angle radian change, calculating sine value,

the two equations are combined and deformed to form a equation set:

step 6: the depth unknowns in the equation in the step 5 are provided with a plurality of assumed values according to the water depth condition of the specific test area to replace respectively, wherein 50 assumed values are arranged at intervals of 10 meters, and an equation set is solved to obtain the horizontal coordinate and the vertical coordinate of the target;

under the condition that the approximate water depth of the local water is known, since whether the water bottom is flat or not is not known, a plurality of water depths are assumed within a certain range of the given water depth, so that z0 of the equation becomes known, and the equation is solved.

The method for solving the equation is a quasi-Newton iteration method, and specifically comprises the following steps:

step 6.1: setting an initial value, regarding the measured azimuth as a plane angle at the time t1, and determining a direction according to port and starboard information, if the starboard information is the measured azimuth theta 1 plus heading angle heading 1; if the ship is port, 360 degrees is subtracted by the azimuth angle theta 1 and then added with heading1 to determine a direction; determining a straight line along the direction by the AUV coordinate point at the time t 1; similarly, a straight line can be determined at the time t 2; the x and y coordinates of the intersection point of the two straight lines are initial values.

Step 6.2: and (3) iteratively solving an optimal solution, and writing an equation set into a matrix form:

initial value matrix:

an iterative formula:

wherein:

wherein, XⁱThe meaning of this is the result of the ith iteration, F' (X)ⁱ) Meaning expressed is the partial derivative matrix of the F matrix to the result of the ith iteration, F (X)ⁱ) The meaning of the representation is the F matrix corresponding to the result of the ith iteration,

A_iis a partial derivative matrix, each element being a corresponding partial derivative; wherein f is₁Refers to the first element of the F matrix; f. of₂Refers to the second element of the F matrix; x is the number ofⁱRefers to the first element of the ith iteration result matrix; y isⁱRefers to the second element of the result matrix of the ith iteration.

Until the iteration is stable, the solution is the solution of the equation; according to a large number of simulations, the solution can be stable after iteration for a certain number of times, and a stable solution can be obtained after the solution is set to be iterated for 50 times.

And 7: performing sliding average on all historical calculation results at the same depth to obtain an average abscissa and an average ordinate at the same depth, and performing sliding average on the result at each depth;

for the same instant, there are multiple results, i.e., spatially multiple results; for the same depth, there are multiple results, i.e., multiple results in time. For the same depth, the calculation at different moments are independent, and the calculation results at different moments can be subjected to sliding average to obtain a final result. Note that the running average refers to a running average over time, each averaging using only the most recent 1/2 of the total number of current solution results. If the number of the resolving results is an even number a, the used number is a/2; if the number of the resolving results is odd, the number used is (a + 1)/2.

And performing a moving average operation at each moment of time for each of a plurality of assumed depths, and taking an average result as a final result. For the effect of the sliding average, as shown in fig. 7, the real depth calculation results are compared, and the calculation results are more stable after the average and have less error than before the average.

And 8: storing the positioning result after the sliding average of each assumed depth, and obtaining the abscissa and the ordinate of the corresponding depth from the positioning result;

and step 9: verifying whether the actual depth of the position is consistent with a corresponding hypothesis or not according to the abscissa and the ordinate obtained in the step 8, wherein the solved hypothesis depth with larger horizontal and vertical coordinate fluctuation is not considered from the aspect of time,

when the verification depth is consistent with the corresponding hypothesis, the hypothesis depth and the corresponding positioning result are retained,

the depth at x, y of a certain depth solution is used to verify whether the assumption is true, and refer to fig. 8.

For example, taking a flat water bottom as an example, the depth is 1600 m, the solved x and y coordinates are 1000 and 1000, if the depth measurement at the position corresponding to x and y is found to be 2000 m, this indicates that this assumption is not true, and when the result corresponding to this assumed depth is deleted; if the depth measurement at the position corresponding to x and y is found to be about 1600 meters, if the hypothesis is considered to be valid, the result corresponding to the hypothesis depth is retained.

When the verification depth is inconsistent with the corresponding hypothesis, rejecting the hypothesis depth and the corresponding positioning result;

step 10: the depth and positioning results of the remaining multiple hypotheses except for step 9 are retained;

step 11: whether a new measurement value exists or not is judged,

corresponding to the verification of a plurality of assumed depth results, the stability of the calculation result can be observed after a certain time (set for 10-100 times), as shown in fig. 6, the true depth of the target of the application is 2000 meters, it can be seen that the calculation result at the true depth is relatively stable, and the fluctuation of the calculation results of other depths is relatively large.

When a new measurement value exists, returning to the step 2 to repeat the process; when there is no new measurement value, the positioning is finished.

In conclusion, a real target positioning result can be obtained.

Further, in the step 2, from the detection of the target, the stored information includes a detection attitude and heading angle result, coordinate information and azimuth and heading instrument information of the unmanned underwater vehicle UUV.

Further, in the step 3, the distance between the latest frame coordinate of the unmanned underwater vehicle UUV and the historical coordinate, and the included angle between the latest frame navigation direction of the unmanned underwater vehicle UUV and the historical navigation direction are the physical quantities to be checked: the attitude angle of the unmanned underwater vehicle UUV in the latest frame and the historical attitude angle are both required to be within a set range. When matching and checking with historical information, there are two cases: first, there are one or more pairs of information that meet all of the above conditions; the second absence is a pair of information that meets all of the above conditions. When the first condition occurs, entering the next step, namely step 4; when the second condition occurs, continuing to wait for step 2 to collect new information, then performing matching and checking in step four until the first condition occurs, and then entering the next step.

Further, the threshold conditions in the step 4 are specifically a distance, an included angle and an azimuth angle, the threshold of the distance is set to be 5% of the detection distance, the satisfied range of the included angle is set to be 45-135 degrees, and 90 degrees is verified to be optimal; the azimuth angle satisfying range is set to be 30-150 degrees, because the front-end detection beam forming is considered, the result errors of the azimuth angles near 0 degrees and 180 degrees are too large due to the directivity of the array, the positioning is influenced, and therefore information with large errors is eliminated.

Further, step 5 obtains two equations, which include three unknowns, i.e., the abscissa, the ordinate, and the depth of the target in the coordinate system.

Further, the two equations in the step 6 cannot solve the three unknowns, so that a plurality of assumed values of the depth unknowns in the three unknowns are provided according to the water depth condition of the specific test area for respective substitution, the two equations solve the two unknowns, the equation set is solved to obtain the abscissa and the ordinate of the target, the depth in the step 6 is traversed, and the next step is sequentially performed.

Further, only the nearest 1/2 is averaged in step 7; the positioning results of all the assumed depths are subjected to moving average, the average result is taken as a final result at each moment, and because a plurality of assumed depths exist, each assumed depth corresponds to one solved horizontal coordinate and one solved vertical coordinate at each moment finally.

Further, the step 9 does not consider the assumed depth with large fluctuation of the horizontal and vertical coordinates in time, because the calculation result will not fluctuate too much if the target is near the assumed depth, as shown in fig. 6.