阅读说明：本技术 基于双移动声信标测距辅助的水下航行器协同定位方法 (Underwater vehicle cooperative positioning method based on double-mobile-acoustic-beacon ranging assistance ) 是由 王银涛 贾晓宝 韩正卿 严卫生 崔荣鑫 张守旭 李宏 王崇武 于 2020-12-27 设计创作，主要内容包括：本发明公开了一种基于双移动声信标测距辅助的水下航行器协同定位方法,用于提高水下航行器的定位精度,以解决水下导航定位难的问题。无人船携带USBL水声定位和通信设备,UUV上安装有航向、航速和航向角速度传感器以及OEM应答器。无人船周期性将自身的位置发送给UUV携带的应答器,USBL同时测得UUV相对于无人船USV的位置。当UUV接收到USV的位置坐标后,利用扩展卡尔曼滤波的方法进行自身定位,同时,UUV将定位结果通过应答器发送给两个USV,使两个USV以UUV作为领航者,随UUV一起编队运行。编队队形保持与UUV之间的距离为设定的安全距离,且两个USV与UUV之间的夹角为90°。本发明利用了无人船携带USBL辅助UUV定位以及编队控制算法,解决了水下航行器UUV定位精度不高的问题。(The invention discloses an underwater vehicle cooperative positioning method based on double-mobile-acoustic-beacon ranging assistance, which is used for improving the positioning precision of an underwater vehicle and solving the problem of difficult underwater navigation positioning. The unmanned ship carries USBL underwater sound positioning and communication equipment, and a course, navigational speed and course angular speed sensor and an OEM transponder are installed on the UUV. The unmanned ship periodically sends the position of the unmanned ship to a transponder carried by the UUV, and the USBL simultaneously measures the position of the UUV relative to the USV of the unmanned ship. After the UUV receives the position coordinates of the USV, the UUV carries out self-positioning by using an extended Kalman filtering method, and meanwhile, the UUV sends positioning results to the two USVs through the responder, so that the two USVs form a formation to operate together with the UUV by taking the UUV as a pilot. The formation keeps the distance between the formation and the UUV at a set safe distance, and the included angle between the two USVs and the UUV is 90 degrees. The invention utilizes the USBL carried by the unmanned ship to assist UUV positioning and formation control algorithm, and solves the problem of low positioning accuracy of the UUV.)

1. An underwater vehicle cooperative positioning method based on double-mobile-acoustic-beacon ranging assistance is characterized by comprising the following steps:

step 1: before two USV1 and USV2 provided with USBL and a UUV cloth provided with an OEM responder are placed in water, a GPS synchronous clock signal is utilized to carry out clock synchronization on the UUV, the USV1 and the USV 2;

step 2: placing USV1, USV2, and UUV cloths in water, the initial position vectors are defined as:

UUV：X(0)＝[x(0) y(0) ψ(0)]^T；

USV1：X₁(0)＝[x₁(0) y₁(0) ψ₁(0)]^T；

USV2：X₂(0)＝[x₂(0) y₂(0) ψ₂(0)]^T，

wherein x (0) and y (0) represent initial values of the position of the UUV in the horizontal plane, and x₁(0)、y₁(0) Indicating the initial value, x, of the position of USV1 in the horizontal plane₂(0)、y₂(0) Represents the initial value of the position of USV2 in the horizontal plane; psi (0), psi₁(0)、ψ₂(0) Respectively representing initial values of the course angles of the UUV, the USV1 and the USV 2;

and step 3: position vector X ═ X y ψ defining UUV]^TAnd setting the initial position filtering value X (0|0) of Kalman filtering as X (0), and setting the initial state error covariance matrix of UUVThe sampling time interval is T and the sampling time interval is T,the variances of the UUV in the x direction, the y direction and the z direction are preset respectively;

and 4, step 4: UUV, USV1 and USV2 start sailing; let k equal to 1;

and 5: at the kth sampling instant, the USBL mounted on USV1 and USV2 simultaneously emit acoustic locating signals, and the OEM mounted on UUV receivesSending a return signal after the two acoustic positioning signals arrive; the USBL on USV1 and USV2 receive return signals; meanwhile, a course sensor, a speed sensor and a course angular velocity sensor which are arranged on the UUV respectively measure a course psi (k), a navigational speed v (k) and a course angular velocity omega (k) of the UUV at the moment k; measure the noise covariance matrix as Respectively setting the variances of the course, the navigational speed and the course angular speed of the UUV;

step 6: predicting the state of the UUV by using an extended Kalman filtering algorithm;

step 6-1: calculating the position filtering predicted value of the UUV at the k moment by using the formula (1):

wherein X (k-1| k-1) represents an accurate value of UUV position filtering at the k-1 moment;

will be provided withExpressed as:

wherein the content of the first and second substances,andrespectively representing the predicted values of the UUV at the position filtering of the k moment in the x direction, the y direction and the course angle;

step 6-2: and (3) calculating the predicted value of the state error covariance matrix of the UUV at the k moment by using the formula (2):

wherein:

p (k-1| k-1) represents the exact value of the state error covariance matrix of the UUV at time k-1;

and 7: the USV1 and the USV2 obtain the position of the UUV through the USBL, send the UUV to the UUV and calculate the slope distance, and the steps are as follows:

the positions of the UUV measured by the USBL carried by the USV1 and the USV2 are respectively:

X_a1(k)＝[x_a1(k) y_a1(k) z_a1(k)]

X_a2(k)＝[x_a2(k) y_a2(k) z_a2(k)]

in the formula: x_a1(.) is the UUV position measured by USV1, X_a2(.) is the position of the UUV measured by USV 2; x is the number of_a1(.)、y_a1(.) and z_a1(.) are the coordinates of the position of the UUV measured by USV1 in the xyz three directions, x_a2(k)、y_a2(k) And z_a2(k) Coordinates of the position of the UUV measured by USV2 in the xyz three directions, respectively;

removal of X_a1(k) And X_a2(k) The depth information in (1) is respectively expressed as:

X_b1(k)＝[x_a1(k) y_a1(k)]

X_b2(k)＝[x_a2(k) y_a2(k)]

the relative positions of USV1, USV2 and UUV are calculated by equation (3):

ρ₁(k)＝X₁(k)-X_b1(k)

ρ₂(k)＝X₂(k)-X_b2(k) (3)

where ρ is₁(k)、ρ₂(k) Relative positions of USV1, USV2 and UUV, X₁(k)＝[x₁(k)，y₁(k)]、X₂(k)＝[x₂(k)，y₂(k)]The position of USV1 and USV2 at time k, respectively;

will | ρ₁(k)|、|ρ₂(k) | is sent as a skew distance to the UUV through the USBL, and a distance measurement error covariance matrix r (k) ═ diag [ 0.010.01 ] is set]；

And 8: the method for filtering the UUV state by using the distance measurement comprises the following steps:

step 8-1: and (3) calculating a Kalman filtering gain matrix of the UUV at the k moment by using the formula (4):

wherein the content of the first and second substances,

step 8-2: and (3) calculating the position filtering accurate value of the UUV at the k moment by using the formula (5):

wherein:

let X (k | k) be:

X(k|k)＝[x(k|k) y(k|k) ψ(k|k)]

wherein x (k | k), y (k | k), and ψ (k | k) represent the exact values of the positional filtering of the UUV at time k in the x-direction, y-direction, and heading angle, respectively;

step 8-3: and (3) calculating the accurate value of the state error covariance matrix of the UUV at the k moment by using the formula (6):

and step 9: the UUV sends the speed v (k) at the moment k, the heading angular speed omega (k) and the position filtered value X (k | k) to the USV1 and the USV2 through the USBL;

step 10: the USV1 and the USV2 take UUV as a pilot, and adopt the following master-slave formation control laws to realize formation navigation;

step 10-1: the distances between USV1, USV2, and UUV were calculated using equation (7), respectively:

step 10-2: and (3) respectively calculating included angles between the USV1, the USV2 and the UUV by using the formula (8):

step 10-3: the errors between the distances between USV1, USV2 and UUV and the minimum safe distance are calculated using equation (9):

in the formula, r_miniIs the minimum safe distance between the USV and the UUV;

step 10-4: the errors between the included angles between USV1, USV2 and UUV and the desired included angle are calculated using equation (10):

in the formula (I), the compound is shown in the specification,

step 10-5: heading angle deviations between the USV1, the USV2 and the UUV are calculated using equation (11), respectively:

ψ_i(k)＝ψ(k|k)-ψ_i(k)(i＝1，2) (11)

step 10-6: the speeds of USV1 and USV2 were controlled as:

in the formula, K₁，K₂∈R⁺Is a preset controller gain; psi₁(k)、ψ₂(k) Respectively, the heading of USV1 and USV2 at time k;

step 10-7: heading angular velocities for controlling USV1 and USV2 are:

in the formula (d)₁＞0，d₂> 0 and d₁And d₂The distance of the USBL of USV1 and USV2, respectively, from the center of gravity of the USV along the longitudinal axis;

step 11: and adding 1 to k, returning to the step 5, and starting a new cycle to realize continuous positioning of the UUV.

2. The underwater vehicle cooperative positioning method based on the double mobile acoustic beacon ranging assistance as claimed in claim 1, wherein the heading angles of the USV1, the USV2 and the UUV are measured by an SBG attitude heading sensor installed on the USV.

3. The dual mobile acoustic beacon ranging assistance-based underwater vehicle co-location method as claimed in claim 1, wherein the positions of the USV1 and USV2 are measured by their own installed RTKGPS rover device.

4. The method for the cooperative positioning of the underwater vehicles based on the range finding assistance of the double mobile acoustic beacons as claimed in claim 1, wherein epsilon is a constant value between 0 and 2 pi.

Technical Field

The invention belongs to the technical field of underwater vehicles, and particularly relates to a cooperative positioning method of an underwater vehicle.

Background

UUV (Unmanned Underwater Vehicle, UUV for short) is a new generation Underwater robot, has the advantages of large range of motion, good maneuverability, safety, intellectualization and the like, and plays an extremely important role in submarine detection data and naval mine fighting. However, the UUV accomplishes this task on the premise that it needs to know its own precise location information. The UUV generally navigates underwater, so that the underwater positioning of the UUV belongs to a three-dimensional problem, the algorithm design is difficult, but the UUV can be provided with a depth sensor, so that the depth of the UUV can be accurately measured, and the underwater three-dimensional positioning problem can be converted into a two-dimensional plane positioning problem.

An Ultra-Short Base Line (USBL) is an underwater acoustic positioning technology, is widely applied to the aspects of marine production development such as marine oil exploration and development and marine salvage, and is mainly used for determining the accurate positions of an ROV, an AUV, a diver and other underwater carriers. The ultra-short baseline positioning system consists of a transmitting transducer, an OEM transponder and a receiving array. The transmitting transducer and the receiving array are arranged on a ship, and the transponder is fixed on an underwater carrier UUV. The transmitting transducer sends out an acoustic pulse, the transponder sends back the acoustic pulse after receiving the acoustic pulse, the receiving array measures X, Y phase difference in two directions after receiving the acoustic pulse, and calculates the distance R from the underwater device to the array according to the arrival time of the acoustic wave, thereby calculating the position of the UUV on the plane coordinate and the depth of the underwater detector.

At present, the method can be used as a UUV underwater high-precision positioning method mainly. Mainly comprises the following steps:

(1) short baseline acoustic localization methods. This method requires a beacon to be fixed to the sea floor in advance and the position of the beacon to be measured. When the beacon is used, the revised position of the beacon is placed in the UUV in advance, and the UUV measures the distance between the UUV and the beacon in real time, so that the position of the UUV relative to the beacon can be calculated. However, the method UUV can be used only in a limited range covered by the beacon, and the beacon is arranged at high cost and is not easy to maintain. The method has poor observation performance, and positioning can be carried out only by maneuvering a UUV.

(2) A long baseline acoustic positioning method based on multiple fixed beacons. The method needs to fix 3-4 beacons on the sea floor in advance and measure the positions of the beacons. When the device is used, the revised beacon position is stored in the UUV, and the UUV can calculate the position of the UUV relative to each beacon by measuring the distance between the UUV and each beacon in real time. The method has the defects that the UUV can only carry out positioning in a limited area covered by the beacon, and the calibration and the arrangement process of the beacon are complicated.

Disclosure of Invention

In order to overcome the defects of the prior art, the invention provides an underwater vehicle cooperative positioning method based on double-mobile-acoustic-beacon ranging assistance, which is mainly used for improving the positioning accuracy of an underwater vehicle and solving the problem of difficult underwater navigation positioning. The unmanned ship carries USBL underwater sound positioning and communication equipment, and a course, navigational speed and course angular speed sensor and an OEM transponder are installed on the UUV. The unmanned ship periodically sends the position of the unmanned ship to a transponder carried by the UUV, and the USBL simultaneously measures the position of the UUV relative to the USV of the unmanned ship. After the UUV receives the position coordinates of the USV, the UUV carries out self-positioning by using an extended Kalman filtering method, and meanwhile, the UUV sends positioning results to the two USVs through the responder, so that the two USVs form a formation to operate together with the UUV by taking the UUV as a pilot. The formation keeps the distance between the formation and the UUV at a set safe distance, and the included angle between the two USVs and the UUV is 90 degrees. The invention utilizes the USBL carried by the unmanned ship to assist UUV positioning and formation control algorithm, and solves the problem of low positioning accuracy of the UUV.

The technical scheme adopted by the invention for solving the technical problem comprises the following steps:

step 1: before two USV1 and USV2 provided with USBL and a UUV cloth provided with an OEM responder are placed in water, a GPS synchronous clock signal is utilized to carry out clock synchronization on the UUV, the USV1 and the USV 2;

step 2: placing USV1, USV2, and UUV cloths in water, the initial position vectors are defined as:

UUV：X(0)＝[x(0) y(0) ψ(0)]^T；

USV1：X₁(0)＝[x₁(0) y₁(0) ψ₁(0)]^T；

USV2：X₂(0)＝[x₂(0) y₂(0) ψ₂(0)]^T，

wherein x (0) and y (0) represent initial values of the position of the UUV in the horizontal plane, and x₁(0)、y₁(0) Indicating the initial value, x, of the position of USV1 in the horizontal plane₂(0)、y₂(0) Represents the initial value of the position of USV2 in the horizontal plane; psi (0), psi₁(0)、ψ₂(0) Respectively representing initial values of the course angles of the UUV, the USV1 and the USV 2;

and step 3: position vector X ═ X y ψ defining UUV]^TAnd setting the initial position filtering value X (0|0) of Kalman filtering as X (0), and setting the initial state error covariance matrix of UUVThe sampling time interval is T and the sampling time interval is T,the variances of the UUV in the x direction, the y direction and the z direction are preset respectively;

and 4, step 4: UUV, USV1 and USV2 start sailing; let k equal to 1;

and 5: at the kth sampling moment, the USBL arranged on the USV1 and the USBL arranged on the USV2 simultaneously send out acoustic positioning signals, and the OEM arranged on the UUV sends out a return signal after receiving the two acoustic positioning signals; the USBL on USV1 and USV2 receive return signals; meanwhile, a course sensor, a speed sensor and a course angular velocity sensor which are arranged on the UUV respectively measure a course psi (k), a navigational speed v (k) and a course angular velocity omega (k) of the UUV at the moment k; measure the noise covariance matrix as Respectively setting the variances of the course, the navigational speed and the course angular speed of the UUV;

step 6: predicting the state of the UUV by using an extended Kalman filtering algorithm;

step 6-1: calculating the position filtering predicted value of the UUV at the k moment by using the formula (1):

wherein X (k-1| k-1) represents an accurate value of UUV position filtering at the k-1 moment;

will be provided withExpressed as:

wherein the content of the first and second substances,andrespectively representing the predicted values of the UUV at the position filtering of the k moment in the x direction, the y direction and the course angle;

step 6-2: and (3) calculating the predicted value of the state error covariance matrix of the UUV at the k moment by using the formula (2):

wherein:

p (k-1| k-1) represents the exact value of the state error covariance matrix of the UUV at time k-1;

and 7: the USV1 and the USV2 obtain the position of the UUV through the USBL, send the UUV to the UUV and calculate the slope distance, and the steps are as follows:

the positions of the UUV measured by the USBL carried by the USV1 and the USV2 are respectively:

X_a1(k)＝[x_a1(k) y_a1(k) z_a1(k)]

X_a2(k)＝[x_a2(k) y_a2(k) z_a2(k)]

in the formula: x_a1(.) is the UUV position measured by USV1, X_a2(.) is the position of the UUV measured by USV 2; x is the number of_a1(.)、y_a1(.) and z_a1(.) are the coordinates of the position of the UUV measured by USV1 in the xyz three directions, x_a2(k)、y_a2(k) And z_a2(k) Coordinates of the position of the UUV measured by USV2 in the xyz three directions, respectively;

removal of X_a1(k) And X_a2(k) The depth information in (1) is respectively expressed as:

X_b1(k)＝[x_a1(k) y_a1(k)]

X_b2(k)＝[x_a2(k) y_a2(k)]

the relative positions of USV1, USV2 and UUV are calculated by equation (3):

ρ₁(k)＝X₁(k)-X_b1(k)

ρ₂(k)＝X₂(k)-X_b2(k) (3)

where ρ is₁(k)、ρ₂(k) Relative positions of USV1, USV2 and UUV, X₁(k)＝[x₁(k)，y₁(k)]、X₂(k)＝[x₂(k)，y₂(k)]The position of USV1 and USV2 at time k, respectively;

will | ρ₁(k)|、|ρ₂(k) L is sent to UUV through USBL as slant distance, and distance measurement is setThe covariance matrix of magnitude errors R (k) diag [ 0.010.01]；

And 8: the method for filtering the UUV state by using the distance measurement comprises the following steps:

step 8-1: and (3) calculating a Kalman filtering gain matrix of the UUV at the k moment by using the formula (4):

wherein the content of the first and second substances,

step 8-2: and (3) calculating the position filtering accurate value of the UUV at the k moment by using the formula (5):

wherein:

let X (k | k) be:

X(k|k)＝[x(k|k)y(k|k)ψ(k|k)]

wherein x (k | k), y (k | k), and ψ (k | k) represent the exact values of the positional filtering of the UUV at time k in the x-direction, y-direction, and heading angle, respectively;

step 8-3: and (3) calculating the accurate value of the state error covariance matrix of the UUV at the k moment by using the formula (6):

and step 9: the UUV sends the speed v (k) at the moment k, the heading angular speed omega (k) and the position filtered value X (k | k) to the USV1 and the USV2 through the USBL;

step 10: the USV1 and the USV2 take UUV as a pilot, and adopt the following master-slave formation control laws to realize formation navigation;

step 10-1: the distances between USV1, USV2, and UUV were calculated using equation (7), respectively:

step 10-2: and (3) respectively calculating included angles between the USV1, the USV2 and the UUV by using the formula (8):

step 10-3: the errors between the distances between USV1, USV2 and UUV and the minimum safe distance are calculated using equation (9):

in the formula, r_{min i}Is the minimum safe distance between the USV and the UUV;

step 10-4: the errors between the included angles between USV1, USV2 and UUV and the desired included angle are calculated using equation (10):

in the formula (I), the compound is shown in the specification,

step 10-5: heading angle deviations between the USV1, the USV2 and the UUV are calculated using equation (11), respectively:

ψ_i(k)＝ψ(k|k)-ψ_i(k)(i＝1，2) (11)

step 10-6: the speeds of USV1 and USV2 were controlled as:

in the formula, K₁，K₂∈R⁺Is a preset controller gain; psi₁(k)、ψ₂(k) Respectively, the heading of USV1 and USV2 at time k;

step 10-7: heading angular velocities for controlling USV1 and USV2 are:

in the formula (d)₁＞0，d₂> 0 and d₁And d₂The distance of the USBL of USV1 and USV2, respectively, from the center of gravity of the USV along the longitudinal axis;

step 11: and adding 1 to k, returning to the step 5, and starting a new cycle to realize continuous positioning of the UUV.

Preferably, the heading angles of the USV1, USV2, and UUV are measured by SBG attitude heading sensors mounted on the USV.

Preferably, the positions of USV1 and USV2 are measured by RTK GPS rover equipment installed by itself.

Preferably, the epsilon is a constant value between 0 and 2 pi.

The invention has the following beneficial effects:

because the USBL is arranged on the movable USV, the USV is provided with the high-precision RTK GPS mobile station, and the two USVs take the UUV as a pilot and form a formation to sail along with the UUV, the navigation and the positioning in a larger range can be realized, the distances between the two USVs and the UUV are kept to be equal to the minimum safe distance, the included angle between the connecting lines of the two USVs and the UUV is kept to be 90 degrees, and the positioning precision of the UUV and the USV in the formation is superior to that of a common formation.

Drawings

FIG. 1 is a schematic diagram of the positioning of USBL according to the method of the present invention.

FIG. 2 is a schematic diagram of a UUV underwater positioning system principle based on double USBL.

FIG. 3 is a schematic diagram of UUV positioning based on double USBL in the method of the present invention.

FIG. 4 shows the relative kinematic relationship between UUV and USV according to the method of the present invention.

FIG. 5 is a diagram illustrating an optimal observation configuration positioning formation structure according to the method of the present invention.

FIG. 6 shows the motion state of UUV in the ground coordinate system.

FIG. 7 is a diagram showing the relationship between the distance between the USV and UUV and the positioning error in the method of the present invention.

FIG. 8 is a comparison diagram of positioning errors when the distances between the USV and the UUV are different in the method of the present invention.

FIG. 9 is a diagram showing the relationship between the angle between the connecting lines of two USVs and a UUV and the positioning error.

Fig. 10 is a comparison diagram of positioning errors when the included angles between the connecting lines of two USVs and UUVs are different according to the method of the present invention.

Detailed Description

The invention is further illustrated with reference to the following figures and examples.

The invention provides a UUV underwater positioning method based on double USBL, which is characterized in that two USBL devices are respectively fixed on two USVs (unmanned Surface vehicles), as shown in figure 2, the ultrashort baseline device can not only actively position the position of the UUV, but also be used as the position coordinate of a communication device for periodically interacting the USV and the distance between the USV and the UUV. The UUV is provided with a course, a navigation speed and a course angular speed sensor, and an OEM responder. As shown in fig. 3, after the UUV receives the acoustic positioning information and the position coordinates of the USV, the UUV performs underwater positioning by using an extended kalman filtering method; meanwhile, the UUV sends the positioning result to the two USVs through the OEM, so that the two USVs take the UUV as a pilot, form a queue to run along with the UUV, and keep the distance between the UUV and the two USVs as the minimum safety distance, and the distance between the UUV and the two USVs is the minimum safety distanceIncluded angle ofAs shown in fig. 5.

As shown in fig. 1, an underwater vehicle co-location method based on dual mobile acoustic beacon ranging assistance includes the following steps:

step 1: before two USV1 and USV2 provided with USBL and a UUV cloth provided with an OEM responder are placed in water, a GPS synchronous clock signal is utilized to carry out clock synchronization on the UUV, the USV1 and the USV 2;

step 2: placing USV1, USV2, and UUV cloths in water, the initial position vectors are defined as:

UUV：X(0)＝[x(0) y(0) ψ(0)]^T；

USV1：X₁(0)＝[x₁(0) y₁(0) ψ₁(0)]^T；

USV2：X₂(0)＝[x₂(0) y₂(0) ψ₂(0)]^T，

wherein x (0) and y (0) represent initial values of the position of the UUV in the horizontal plane, and x₁(0)、y₁(0) Indicating the initial value, x, of the position of USV1 in the horizontal plane₂(0)、y₂(0) Represents the initial value of the position of USV2 in the horizontal plane; psi (0), psi₁(0)、ψ₂(0) Respectively representing initial values of the course angles of the UUV, the USV1 and the USV 2;

and step 3: position vector X ═ X y ψ defining UUV]^TAnd setting the initial position filtering value X (0|0) of Kalman filtering as X (0), and setting the initial state error covariance matrix of UUVThe sampling time interval is T and the sampling time interval is T,the variances of the UUV in the x direction, the y direction and the z direction are preset respectively;

and 4, step 4: UUV, USV1 and USV2 start sailing; let k equal to 1;

and 5: at the kth sampling instant, install on USV1Simultaneously sending out acoustic positioning signals with the USBL on the USV2, and sending out a return signal after an OEM installed on the UUV receives the two acoustic positioning signals; the USBL on USV1 and USV2 receive return signals; meanwhile, a course sensor, a speed sensor and a course angular velocity sensor which are arranged on the UUV respectively measure a course psi (k), a navigational speed v (k) and a course angular velocity omega (k) of the UUV at the moment k; measure the noise covariance matrix as Respectively setting the variances of the course, the navigational speed and the course angular speed of the UUV;

step 6: predicting the state of the UUV by using an extended Kalman filtering algorithm;

step 6-1: calculating the position filtering predicted value of the UUV at the k moment by using the formula (1):

wherein X (k-1| k-1) represents an accurate value of UUV position filtering at the k-1 moment;

will be provided withExpressed as:

wherein the content of the first and second substances,andrespectively representing the predicted values of the UUV at the position filtering of the k moment in the x direction, the y direction and the course angle;

step 6-2: and (3) calculating the predicted value of the state error covariance matrix of the UUV at the k moment by using the formula (2):

wherein:

p (k-1| k-1) represents the exact value of the state error covariance matrix of the UUV at time k-1;

and 7: the USV1 and the USV2 obtain the position of the UUV through the USBL, send the UUV to the UUV and calculate the slope distance, and the steps are as follows:

the positions of the UUV measured by the USBL carried by the USV1 and the USV2 are respectively:

X_a1(k)＝[x_a1(k) y_a1(k) z_a1(k)]

X_a2(k)＝[x_a2(k) y_a2(k) z_a2(k)]

in the formula: x_a1(.) is the UUV position measured by USV1, X_a2(.) is the position of the UUV measured by USV 2; x is the number of_a1(.)、y_a1(.) and z_a1(.) are the coordinates of the position of the UUV measured by USV1 in the xyz three directions, x_a2(k)、y_a2(k) And z_a2(k) Coordinates of the position of the UUV measured by USV2 in the xyz three directions, respectively;

removal of X_a1(k) And X_a2(k) The depth information in (1) is respectively expressed as:

X_b1(k)＝[x_a1(k) y_a1(k)]

X_b2(k)＝[x_a2(k) y_a2(k)]

the relative positions of USV1, USV2 and UUV are calculated by equation (3):

ρ₁(k)＝X₁(k)-X_b1(k)

ρ₂(k)＝X₂(k)-X_b2(k) (3)

where ρ is₁(k)、ρ₂(k) Relative positions of USV1, USV2 and UUV, X₁(k)＝[x₁(k)，y₁(k)]、X₂(k)＝[x₂(k)，y₂(k)]The position of USV1 and USV2 at time k, respectively;

will | ρ₁(k)|、|ρ₂(k) | is sent as a skew distance to the UUV through the USBL, and a distance measurement error covariance matrix r (k) ═ diag [ 0.010.01 ] is set]；

And 8: the method for filtering the UUV state by using the distance measurement comprises the following steps:

step 8-1: and (3) calculating a Kalman filtering gain matrix of the UUV at the k moment by using the formula (4):

wherein the content of the first and second substances,

step 8-2: and (3) calculating the position filtering accurate value of the UUV at the k moment by using the formula (5):

wherein:

let X (k | k) be:

X(k|k)＝[x(k|k)y(k|k)ψ(k|k)]

wherein x (k | k), y (k | k), and ψ (k | k) represent the exact values of the positional filtering of the UUV at time k in the x-direction, y-direction, and heading angle, respectively;

step 8-3: and (3) calculating the accurate value of the state error covariance matrix of the UUV at the k moment by using the formula (6):

and step 9: the UUV sends the speed v (k) at the moment k, the heading angular speed omega (k) and the position filtered value X (k | k) to the USV1 and the USV2 through the USBL;

step 10: as shown in fig. 4, USV1 and USV2 use UUV as pilots, and respectively adopt the following master-slave formation control laws to realize formation navigation;

step 10-1: the distances between USV1, USV2, and UUV were calculated using equation (7), respectively:

step 10-2: and (3) respectively calculating included angles between the USV1, the USV2 and the UUV by using the formula (8):

step 10-3: the errors between the distances between USV1, USV2 and UUV and the minimum safe distance are calculated using equation (9):

in the formula, r_{min i}Is the minimum safe distance between the USV and the UUV;

step 10-4: the errors between the included angles between USV1, USV2 and UUV and the desired included angle are calculated using equation (10):

in the formula (I), the compound is shown in the specification,

step 10-5: heading angle deviations between the USV1, the USV2 and the UUV are calculated using equation (11), respectively:

ψ_i(k)＝ψ(k|k)-ψ_i(k)(i＝1，2) (11)

step 10-6: the speeds of USV1 and USV2 were controlled as:

in the formula, K₁，K₂∈R⁺Is a preset controller gain; psi₁(k)、ψ₂(k) Respectively, the heading of USV1 and USV2 at time k;

step 10-7: heading angular velocities for controlling USV1 and USV2 are:

in the formula (d)₁＞0，d₂> 0 and d₁And d₂The distance of the USBL of USV1 and USV2, respectively, from the center of gravity of the USV along the longitudinal axis; as shown in fig. 3;

step 11: and adding 1 to k, returning to the step 5, and starting a new cycle to realize continuous positioning of the UUV.

Preferably, the heading angles of the USV1, USV2, and UUV are measured by SBG attitude heading sensors mounted on the USV.

Preferably, the positions of USV1 and USV2 are measured by RTK GPS rover equipment installed by itself.

Preferably, the epsilon is a constant value between 0 and 2 pi.

The specific embodiment is as follows:

the first embodiment is as follows: and analyzing the influence of the distance between the USV and the UUV on the positioning performance. As shown in FIG. 6, suppose that UUV moves at 3 knots, starting at coordinates (20,20) to get the UUV moving speedThe course angle starts to move linearly. In addition, the movement speeds of the two USVs are assumed to be 3 sections, and the initial heading angle isAnd starting from different initial positions given in table 1, the included angle between the two USVs and the UUV is 90 degrees, and the distance between the USV and the UUV is increased from the minimum safe distance of 60 meters to 1000 meters. Hypothesis in the experimentQ＝diag([0.1 0.005 0.005])，P(0|0)＝diag([1000 1000 1000])，T＝0.5s，k₁＝5，k₂＝1，d₁＝d₂0.1 m. Table 1 shows the average positioning error of the positioning system under different initial motion positions of the USV, fig. 7 shows a positioning error graph when the distance between the USV and the UUV is increased from 80 to 1000m, and fig. 8 shows a positioning error comparison graph when the distance between the USV and the UUV is 400m and 80m, respectively.

TABLE 1 mean positioning error of positioning system under different initial position conditions of USV

As can be seen from table 1 and fig. 7 and 8, when the included angle between the two USVs and the UUV is kept at 90 degrees, the distance between the two USVs and the UUV is changed, and as the distance increases, the average positioning error increases.

Example two: and analyzing the influence of the included angle between the USV and the UUV on the positioning performance. Assuming that the UUV movement speed is 3 knots, sit downStarting at the point marked (20,20) to startThe course angle starts to move linearly. Firstly, the movement speeds of two USVs are assumed to be 3 sections, and the initial course angle isAt the moment, the included angle between the two USVs and the UUV isThe distance is the minimum safe distance. Then, starting from different initial positions given in table 2, the included angle between the two USVs and the UUV is changed. Hypothesis in the experimentQ＝diag([0.1 0.005 0.005])，P(0|0)＝diag([1000 1000 1000])，T＝0.5s，k₁＝5，k₂＝1，d₁＝d₂0.1 m. Table 2 shows the average positioning error of the positioning system under the condition of the included angle between the connecting lines of different USVs and UUVs, fig. 9 shows a positioning error curve graph when the included angle between the USV and the UUV increases from 40 degrees to 160 degrees, and fig. 10 shows a positioning error comparison graph when the included angle between the USV and the UUV is 90 degrees and 160 degrees, respectively.

TABLE 2 average positioning error of positioning system for different USV and UUV connecting line corner conditions

As can be seen from table 2 and fig. 9 and 10, when the distance between the two USVs and the UUV is kept at 400m, the angle between the two USVs and the UUV is changed, and the average positioning error is minimum at 90 degrees.

The UUV underwater positioning method based on the double-mobile-acoustic-beacon assistance has the optimal positioning performance.