CN114610021A - Autonomous underwater vehicle homing path planning method and device - Google Patents
Autonomous underwater vehicle homing path planning method and device Download PDFInfo
- Publication number
- CN114610021A CN114610021A CN202210120059.9A CN202210120059A CN114610021A CN 114610021 A CN114610021 A CN 114610021A CN 202210120059 A CN202210120059 A CN 202210120059A CN 114610021 A CN114610021 A CN 114610021A
- Authority
- CN
- China
- Prior art keywords
- underwater vehicle
- autonomous underwater
- obtaining
- distance
- moment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 239000013598 vector Substances 0.000 claims abstract description 43
- 238000004891 communication Methods 0.000 claims abstract description 41
- 238000003032 molecular docking Methods 0.000 claims abstract description 26
- 238000001914 filtration Methods 0.000 claims abstract description 24
- 239000011159 matrix material Substances 0.000 claims abstract description 18
- 230000033001 locomotion Effects 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 238000011084 recovery Methods 0.000 description 17
- 230000008569 process Effects 0.000 description 7
- 238000005259 measurement Methods 0.000 description 6
- 230000007704 transition Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000005284 excitation Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000004064 recycling Methods 0.000 description 2
- 230000005236 sound signal Effects 0.000 description 2
- 239000000126 substance Substances 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000004146 energy storage Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 210000001503 joint Anatomy 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a homing path planning method and a homing path planning device for an autonomous underwater vehicle, wherein the homing path planning method comprises the following steps: acquiring first position communication information of a mother ship and second position information of an autonomous underwater vehicle at a first moment, and further acquiring a first direction vector between the autonomous underwater vehicle and the mother ship according to the first position communication information and the second position information; controlling the autonomous underwater vehicle to revolve and controlling the autonomous underwater vehicle to navigate linearly based on the first directional vector; obtaining a first state matrix of the autonomous underwater vehicle at a first moment according to the second position information and the course angle; thereby obtaining a system state equation of the autonomous underwater vehicle; based on a system state equation, obtaining the path estimation of the autonomous underwater vehicle at the second moment through an extended Kalman filtering algorithm; and when the distance between the autonomous underwater vehicle and the mother boat is less than the preset distance, the autonomous underwater vehicle is controlled to carry out docking tasks, so that the path planning of the autonomous underwater vehicle is realized.
Description
Technical Field
The invention relates to the technical field of underwater vehicle positioning, in particular to a homing path planning method and a homing path planning device for an autonomous underwater vehicle.
Background
With the development of people on ocean resources, autonomous underwater vehicles are more and more widely used, usage scenes of the autonomous underwater vehicles comprise underwater reconnaissance, seabed construction or a plurality of large-range long-range underwater tasks, the usage scenes have greater limitation on the communication capacity of the autonomous underwater vehicles, the recovery of the autonomous underwater vehicles is also an important link after the autonomous underwater vehicles complete the tasks, the autonomous underwater vehicles need to use high-precision sensors for docking and recovering, the sensors often need to realize high-precision navigation effect within a certain range or an opening angle, the high-precision docking can be kept only by limiting the movement speed of the autonomous underwater vehicles in the docking process, and the docking and recovering of the autonomous underwater vehicles need to ensure that the distance between the autonomous underwater vehicles and a mother boat is within a preset range, the angle is in the angle of predetermineeing just can have higher butt joint recovery efficiency. How to guide the autonomous underwater vehicle to navigate for docking and recovery by using fewer communication and navigation means before the autonomous underwater vehicle performs the docking and recovery process is not a good solution in the prior art.
Disclosure of Invention
The problem solved by the invention is how to accurately guide and plan the homing path of the autonomous underwater vehicle before the docking recovery process.
In order to solve the problems, the invention provides a homing path planning method for an autonomous underwater vehicle, which comprises the following steps:
acquiring first position communication information of a mother ship and second position information of an autonomous underwater vehicle at a first moment, and further acquiring a first direction vector between the autonomous underwater vehicle and the mother ship according to the first position communication information and the second position information; controlling the autonomous underwater vehicle to turn to a preset angle based on the first direction vector, and controlling the autonomous underwater vehicle to sail linearly, wherein the preset angle is determined according to an included angle between the autonomous underwater vehicle and the first direction vector; obtaining a first state matrix of the autonomous underwater vehicle at the first moment according to the second position information and a course angle, wherein the course angle is an included angle formed by the preset angle and a preset direction; obtaining a system state equation of the autonomous underwater vehicle according to the first state matrix, the measured forward synthetic motion speed and the measured yaw rate; based on the system state equation, obtaining a path estimation of the autonomous underwater vehicle at a second moment through an extended Kalman filtering algorithm; acquiring the distance between the autonomous underwater vehicle and the mother boat according to the path estimation at the second moment, and judging whether the distance is smaller than a preset distance; and if so, controlling the autonomous underwater vehicle to carry out docking tasks.
Compared with the prior art, the method and the device have the advantages that the position relation between the autonomous underwater vehicle and the mother boat at the first moment is obtained by utilizing the sonar of the mother boat, the sonar of the autonomous underwater vehicle and inertial navigation, the autonomous underwater vehicle is controlled to carry out primary recovery action relative to the mother boat, and then a first state matrix of the autonomous underwater vehicle at the first moment is obtained according to the course angle and the second position information, which are obtained by the autonomous underwater vehicle through inertial navigation equipment. The actual traveling path of the autonomous underwater vehicle is accurately estimated by acquiring the forward synthetic motion speed and the yaw rate of the autonomous underwater vehicle and calculating the predicted state of the autonomous underwater vehicle at the second moment by using the extended Kalman filtering. The distance between the autonomous underwater vehicle and the mother boat is obtained through path estimation, so that the autonomous underwater vehicle and the mother boat can start a docking recovery process within a range allowed by the function of the high-precision sensor, and planning and guiding of a homing path of the autonomous underwater vehicle before docking recovery are realized.
Optionally, the obtaining, by an extended kalman filter algorithm, a path estimate of the autonomous underwater vehicle at a second time based on the system state equation comprises:
obtaining, by the extended Kalman filtering algorithm, a first a priori estimated covariance of the autonomous underwater vehicle between the first time and the second time; updating Kalman coefficients in the extended Kalman filtering algorithm based on the first a priori estimated covariance; obtaining a first observation equation of the autonomous underwater vehicle at the second moment; and correcting the path estimation and the estimated covariance of the autonomous underwater vehicle at the second moment according to the updated Kalman coefficient and the first observation equation.
Therefore, the method is based on the extended Kalman filtering, is suitable for predicting a nonlinear system, and ensures that the prior estimation covariance is corrected according to the observation path of the autonomous underwater vehicle at the second moment to obtain more accurate path estimation of the autonomous underwater vehicle at the second moment; updating the estimated covariance at the second time instant is used in favor of the next iteration (i.e., the a priori estimated covariance for the autonomous underwater vehicle at the third time instant).
Optionally, the obtaining a first observation equation of the autonomous underwater vehicle at the second time instant according to the second time instant includes:
obtaining a first relative distance between the autonomous underwater vehicle and the mother boat according to the first position communication information and the second position information; acquiring third position communication information of the mother boat and fourth position information of the autonomous underwater vehicle at the second time; obtaining a second relative distance between the autonomous underwater vehicle and the mother boat and a moving vector of the autonomous underwater vehicle according to the third position communication information and the fourth position information, wherein the moving vector comprises a displacement and a direction vector of the autonomous underwater vehicle between the first time and the second time; obtaining the first observation equation based on the first relative distance, the second relative distance, the moving radius, the first location communication information, and the third location communication information.
Therefore, the homing path of the autonomous underwater vehicle can be accurately planned based on the observation equation.
Optionally, after the obtaining the distance between the autonomous underwater vehicle and the mother boat and judging whether the distance is smaller than a preset distance, the method further includes:
if the distance is greater than the preset distance, obtaining the path estimation of the autonomous underwater vehicle at a third moment according to the extended Kalman filtering algorithm; judging whether the distance between the autonomous underwater vehicle and the mother boat is smaller than the preset distance according to the path estimation at the third moment; if not, continuing to obtain the path estimation of the autonomous underwater vehicle at the later moment until the distance between the autonomous underwater vehicle and the mother boat is smaller than the preset distance.
Therefore, if the distance between the autonomous underwater vehicle and the mother boat is larger than the distance for docking and recycling by the high-precision sensor, the path estimation of the autonomous underwater vehicle at the later moment is obtained in an iteration mode, and the homing path of the autonomous underwater vehicle is planned until the distance for docking and recycling by the high-precision sensor is met.
Optionally, if the distance is greater than a preset distance, obtaining, according to the extended kalman filter algorithm, a path estimate of the autonomous underwater vehicle at a third time includes:
obtaining a second prior estimation covariance of a third moment according to the estimation covariance of the second moment, and updating the Kalman coefficient based on the second prior estimation covariance; acquiring a distance observation value between the autonomous underwater vehicle and the mother boat at the third time; and obtaining a path estimation of the third moment according to the updated Kalman coefficient and the observation value of the autonomous underwater vehicle at the third moment.
Therefore, the prior estimation is corrected through the observed data, and more accurate path estimation is guaranteed to be obtained.
Optionally, the controlling the autonomous underwater vehicle to slew to a preset angle based on the first directional vector and controlling the autonomous underwater vehicle to sail straight comprises: controlling the autonomous underwater vehicle to turn at a maximum rudder angle.
Therefore, the autonomous underwater vehicle is controlled to rotate at the fastest speed, redundant paths can be reduced, and the homing guiding efficiency of the autonomous underwater vehicle is increased.
Optionally, a preset time interval is set between the first time, the second time, and the third time.
Therefore, data transmission is carried out at preset time intervals, and the positioning information of the autonomous underwater vehicle can be obtained by smaller communication and navigation means.
Optionally, the system state equation further includes a first white gaussian noise when the forward synthetic motion velocity is measured and a second white gaussian noise when the yaw rate is measured, where the first white gaussian noise and the second white gaussian noise are zero-mean white gaussian noise independent of each other.
Thus, the precondition of the extended kalman filter is satisfied by assuming that the noise is gaussian white noise independent of each other.
Optionally, after the obtaining, by an extended kalman filter algorithm, a path estimate of the autonomous underwater vehicle at the second time based on the system state equation, the method further includes:
acquiring position information of the mother boat, and acquiring a second direction vector between the autonomous underwater vehicle and the mother boat according to the position information and the path estimation; and controlling the autonomous underwater vehicle to turn to a preset angle to sail straight on the basis of the second direction vector.
Therefore, after accurate path estimation of the autonomous underwater vehicle is obtained each time, the autonomous underwater vehicle is controlled to rotate to a preset angle and navigate in a straight line, and the autonomous underwater vehicle is controlled and guided to home in one step in an iteration mode.
In another aspect, the present invention further provides an autonomous underwater vehicle homing path planning apparatus, which includes a computer readable storage medium storing a computer program and a processor, wherein the computer program is read by the processor and executed to implement the autonomous underwater vehicle homing path planning method described above.
Compared with the prior art, the homing path planning device of the autonomous underwater vehicle has the advantages that the homing path planning method of the autonomous underwater vehicle is consistent with the homing path planning method of the autonomous underwater vehicle, and is not repeated herein.
Drawings
Fig. 1 is a schematic flow chart of an autonomous underwater vehicle homing path planning method according to an embodiment of the present invention;
fig. 2 is a schematic flow chart of the autonomous underwater vehicle homing path planning method of the embodiment of the present invention after step S500 is refined;
fig. 3 is a schematic flowchart of the autonomous underwater vehicle homing path planning method of the embodiment of the present invention after step S530 is refined;
fig. 4 is a schematic flow chart of the autonomous underwater vehicle homing path planning method according to the embodiment of the present invention after step S600;
fig. 5 is a flowchart illustrating the detailed procedure of step S610 of the homing path planning method of the autonomous underwater vehicle according to the embodiment of the present invention;
fig. 6 is a schematic flow chart of the autonomous underwater vehicle homing path planning method according to the embodiment of the present invention after step S500.
Detailed Description
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in detail below.
An Autonomous Underwater Vehicle (AUV) is a task controller integrating artificial intelligence and other advanced computing technologies, integrates high technologies such as a deep submersible vehicle, a sensor, an environmental effect, computer software, energy storage, conversion and propulsion, new materials and new processes, an underwater intelligent weapon and the like, and is used in the fields of anti-submarine warfare, mine warfare, reconnaissance and monitoring, logistics support and the like in military.
In order to realize accurate docking of an autonomous underwater vehicle and a mother boat, in the prior art, high-precision sensors are used for acquiring positioning information of the autonomous underwater vehicle, but the sensors usually need to enable the autonomous underwater vehicle and the mother boat to meet the requirements of distance and opening angle so as to realize a high-precision navigation effect. In order to realize high-precision docking recovery, the autonomous underwater vehicle must be guided to precisely home outside the distance of the sensor.
In order to solve the problem of guiding and homing before docking and recovering of an autonomous underwater vehicle, as shown in fig. 1, the invention provides a homing path planning method for the autonomous underwater vehicle, which comprises the following steps:
step S100, first position communication information of a mother ship and second position information of an autonomous underwater vehicle at a first moment are obtained, and then a first direction vector between the autonomous underwater vehicle and the mother ship is obtained according to the first position communication information and the second position information.
In an embodiment, the first position communication information sent by the mother boat is an underwater acoustic communication signal, wherein the underwater acoustic communication signal includes position information and a timestamp, and the launching time and the launching position of the mother boat can be obtained by acquiring the first position communication information.
In one embodiment, a plane rectangular coordinate system is established for the horizontal planes of the autonomous underwater vehicle and the mother boat, and the position information of the autonomous underwater vehicle and the mother boat is represented by using vectors. At a first moment, the mother boat transmits first position communication information to the autonomous underwater vehicle through a sonar, the first position communication information comprises a timestamp for recording the transmitting moment, the autonomous underwater vehicle determines the propagation time of a communication signal according to the timestamp in the signal after receiving the first position communication information, a one-way ranging mode is utilized, the relative distance between the autonomous underwater vehicle and the mother boat is obtained according to the underwater sound velocity and the propagation time, the obtained relative distance is an observation quantity, the observation quantity possibly has observation errors, and noise is input to the sonar of the autonomous underwater vehicle and the mother boat, so that the state of the autonomous underwater vehicle is predicted according to extended Kalman filtering, and the predicted value is corrected according to the observation value at the next moment.
The position information of the autonomous underwater vehicle and the position information of the mother boat are vector information, so that the position information of the mother boat and the position information of the autonomous underwater vehicle are subjected to difference, a first direction vector between the autonomous underwater vehicle and the mother boat can be obtained, and the first direction vector represents a direction vector between the autonomous underwater vehicle and the mother boat at a first moment.
In one embodiment, the first time is denoted as time k, and the position information in the first position communication information of the mother boat is
The second position information of the autonomous underwater vehicle is
Propagation time of tsignalAcoustic velocity of water vsignalThen the first direction vector is
Wherein
And S200, controlling the autonomous underwater vehicle to revolve to a preset angle based on the first direction vector, and controlling the autonomous underwater vehicle to sail straightly, wherein the preset angle is determined according to an included angle between the autonomous underwater vehicle and the first direction vector.
Optionally, the autonomous underwater vehicle is controlled to slew at maximum rudder angle.
In one embodiment, after the first direction vector is acquired, the autonomous underwater vehicle is controlled to rotate towards the angle of the first direction vector until the absolute value of the first angle difference between the orientation of the autonomous underwater vehicle and the first angle is 45 degrees. And recording the positive included angle between the first direction vector and the x-axis of the rectangular coordinate system as a first angle.
And after the orientation of the autonomous underwater vehicle is adjusted, controlling the autonomous underwater vehicle to perform straight line navigation until the underwater sound signal of the mother boat is obtained next time.
In the process, the orientation of the autonomous underwater vehicle is adjusted according to the relative position relation with the mother boat, and the autonomous underwater vehicle moves linearly towards a preset angle, so that the docking and recovery of the autonomous underwater vehicle are guided preliminarily.
And step S300, obtaining a first state matrix of the autonomous underwater vehicle at the first moment according to the second position information and a course angle, wherein the course angle is an included angle formed by the preset angle and a preset direction.
And obtaining a first state matrix, namely a two-dimensional motion state of the autonomous underwater vehicle in a rectangular plane coordinate system at the first moment through the abscissa and the ordinate of the autonomous underwater vehicle at the first moment and a course angle formed by the course of the autonomous underwater vehicle after the first moment and the x axis.
The first state matrix may be represented as
Wherein x iskRepresenting the x-axis coordinate, y, of the autonomous underwater vehicle in a coordinate systemkRepresenting the y-axis coordinates of the autonomous underwater vehicle in a coordinate system,
representing the heading angle of the autonomous underwater vehicle in a coordinate system.
And S400, obtaining a system state equation of the autonomous underwater vehicle according to the first state matrix, the measured forward synthetic motion speed and the measured yaw rate.
When the sampling period
The two-dimensional plane kinematics model of the autonomous underwater vehicle is as follows:
wherein v iskIs the forward resultant motion velocity; omegakIs the yaw rate. The model is established in an ideal state, and the input of the sensor in the actual model is interfered by noise.
If the noise is white Gaussian noise, the system input under the influence of the noise is
Wherein v ismkAnd omegamkAutonomous underwater vehicles respectively at tkA measure of velocity at time and yaw rate; w is avkAnd wωkAre independent zero mean Gaussian white noise.
The noise variance is then:
order to
The system state equation can be expressed as Xk+1＝f(Xk,uk)+Γ(wk). The system state equation is used for representing state data such as the position, the course angle and the like of the autonomous underwater vehicle in the plane rectangular coordinate system at the first moment, is used for carrying out prior prediction on the state at the next moment, and needs to be corrected by Kalman filtering because errors possibly exist in the system.
And S500, based on the system state equation, obtaining the path estimation of the autonomous underwater vehicle at the second moment through an extended Kalman filtering algorithm.
Because the Kalman filtering is mainly aiming at the processing of a linear discrete system and cannot be applied to a nonlinear system, the method selects the extended Kalman filtering as an algorithm to predict the path of the autonomous underwater vehicle.
Step S600, obtaining the distance between the autonomous underwater vehicle and the mother boat according to the path estimation at the second moment, and judging whether the distance is smaller than a preset distance.
Based on the principle of extended Kalman filtering, after the path of the autonomous underwater vehicle is predicted, the path estimation of the autonomous underwater vehicle is optimized through an observation value, and the accuracy of the path estimation can be improved, so that after the path of the autonomous underwater vehicle is estimated, more accurate position information and angle information can be obtained, the distance between the autonomous underwater vehicle and a mother boat can be further obtained by combining mother boat information, and whether the distance can be subjected to a docking task or not is judged.
And S700, if yes, controlling the autonomous underwater vehicle to carry out docking tasks.
When the distance between the autonomous underwater vehicle and the mother boat is close enough, the high-precision sensor on the autonomous underwater vehicle can complete the docking and recovery task, and then the autonomous underwater vehicle is controlled to perform the docking task.
Alternatively, as shown in fig. 2, step S500 includes:
step S510, obtaining a first prior estimated covariance of the autonomous underwater vehicle between the first time and the second time through the extended kalman filter algorithm.
And predicting the result of the second moment by the optimal estimation of the first moment, wherein the result is a priori estimation, is a filtering intermediate result and needs to be corrected by the observed value of the second moment.
The estimate of the second time instant can be expressed as:
wherein the content of the first and second substances,
representing an a priori estimate of the autonomous underwater vehicle between a first time and a second time,
representing the a posteriori path estimates for the first time instant.
Obtaining a prior estimated covariance at a second time based on the state transition matrix, the posterior estimated covariance at the first time, and the process excitation noise covariance, and expressed as:
wherein, Pk,k+1Representing the prior estimated covariance, F, of the second time instantk,k+1A state transition matrix is represented that represents the state transition,
transposed matrix, P, representing a state transition matrixkRepresenting the posteriori estimated covariance, Q, of the first time instantk+1Representing the process excitation noise covariance, used to represent the noise.
Step S520, updating Kalman coefficients in the extended Kalman filtering algorithm based on the first priori estimated covariance.
Because the system has errors, the observed value is not the true value of the system state, and the predicted value can be optimized through the observed value by the extended Kalman filtering algorithm, so that the prior predicted value is updated to the posterior predicted value, and the accuracy of the predicted value is higher than that of the observed value.
The updated kalman coefficient may be expressed as:
wherein, Kk+1Showing the updated Kalman Filter at the second moment, Hk+1A transition matrix representing state variables to observations, representing the relationship that links states and observations,
represents Hk+1Transposed matrix of (2), Rk+1Representing the measurement noise variance.
Step S530, obtaining a first observation equation of the autonomous underwater vehicle at the second time.
And S540, correcting the path estimation and the estimated covariance of the autonomous underwater vehicle at the second moment according to the updated Kalman coefficient and the first observation equation.
The path estimate for the second time instant may be expressed as:
wherein Z isk+1An observation matrix representing the second moment in time, i.e. the measurements of the autonomous underwater vehicle at the second moment in time,
and the residual errors representing actual observation and predicted observation and the Kalman coefficient are used for correcting the prior prediction result together to obtain the path estimation of the second moment.
The estimated covariance at the second time instant represents the a posteriori estimated covariance at the second time instant modified according to the observation equation, which can be expressed as:
Pk+1＝[I-Kk+1Hk+1]Pk,k+1，
wherein I is an identity matrix, Pk,k+1Representing the prior estimated covariance, K, of the second time instantk+1Kalman coefficient, H, representing the second momentk+1The Kalman filter is a linear relation and is responsible for converting the m-dimensional measurement value into the n-dimensional measurement value so as to enable the m-dimensional measurement value to be in accordance with the mathematical form of the state variable, and the Kalman filter is one of the preconditions of the filtering.
Alternatively, as shown in fig. 3, step S530 includes:
and step S531, obtaining a first relative distance between the autonomous underwater vehicle and the mother boat according to the first position communication information and the second position information.
The autonomous underwater vehicle can acquire the information sending time of the mother boat according to the timestamp in the received position information of the mother boat, so that the propagation time of the signal is acquired, and if the propagation time of the communication signal is tsignalAcoustic velocity of water vsignalThen the first relative distance can be expressed as
Step S532, acquiring third position communication information of the mother ship and fourth position information of the autonomous underwater vehicle at the second time.
And the autonomous underwater vehicle acquires third position communication information sent to the autonomous underwater vehicle by the mother vehicle at the second time, and acquires the time of sending the third position communication information from the timestamp of the third position communication information.
Step S533, obtaining a second relative distance between the autonomous underwater vehicle and the mother boat and a moving vector of the autonomous underwater vehicle according to the third position communication information and the fourth position information, where the moving vector includes a displacement and a direction vector of the autonomous underwater vehicle between the first time and the second time.
And obtaining the moving vector diameter of the autonomous underwater vehicle in a time period between the first moment and the second moment according to the coordinate information of the rectangular coordinate system of the plane where the mother boat and the autonomous underwater vehicle are located.
The location information in the third location communication may be expressed as
The fourth location information may be expressed as
The second relative distance can be expressed as
Coordinate changes of the autonomous underwater vehicle along the x axis and the y axis from the first moment to the second moment can be known by utilizing the inertial navigation information of the autonomous underwater vehicle
And
the moving radius can be expressed as
Wherein the content of the first and second substances,
the functions are represented separately.
Step S534, obtaining the first observation equation based on the first relative distance, the second relative distance, the moving radius, the first location communication information, and the third location communication information.
The observation equation can be expressed as:
wherein v iskFor noise measurement, we assume that it is zero-mean, independent white gaussian noise.
Optionally, as shown in fig. 4, after step S600, the method further includes:
and step S610, if the distance is greater than the preset distance, obtaining the path estimation of the autonomous underwater vehicle at the third moment according to the extended Kalman filtering algorithm.
And if the posterior path estimation of the second moment is predicted by using the extended Kalman filtering, the distance between the autonomous underwater vehicle and the mother boat at the second moment of the autonomous underwater vehicle is obtained, and if the distance is not enough to meet the requirement of docking recovery, the posterior path estimation of the autonomous underwater vehicle at the third moment is obtained.
Step S620, determining whether the distance between the autonomous underwater vehicle and the mother boat is less than the preset distance according to the path estimation at the third time.
And after the posterior path estimation of the autonomous underwater vehicle at the third moment is obtained, judging whether the distance between the autonomous underwater vehicle and the mother ship meets the docking recovery range required by the high-precision sensor in the autonomous underwater vehicle or not according to the posterior position of the autonomous underwater vehicle at the third moment.
Step S630, if the distance is greater than a preset distance, continuing to obtain a path estimation of the autonomous underwater vehicle at the later time until the distance between the autonomous underwater vehicle and the mother boat is less than the preset distance.
And if the butt recovery range is not met, continuing to obtain the posterior path estimation of the autonomous underwater vehicle at the fourth time, and repeating the processes of the steps S610 to S630 until the distance between the autonomous underwater vehicle and the mother boat meets the requirement of the over-precision sensor on the butt recovery distance.
Alternatively, as shown in fig. 5, step S610 includes:
step S611, obtaining a second apriori estimated covariance at a third time according to the estimated covariance at the second time, and updating the kalman coefficient based on the second apriori estimated covariance.
The prior estimated covariance at the third time is obtained by the a posteriori estimated covariance at the second time, and the predicted path estimate at this time is only related to the optimal estimated value at the second time (i.e. only related to the a posteriori path estimate).
Step S612, acquiring a distance observation value between the autonomous underwater vehicle and the mother boat at the third time.
Step S613, obtaining a path estimate of the third time according to the updated kalman coefficient and the observation value of the autonomous underwater vehicle at the third time.
And acquiring the position information of the autonomous underwater vehicle and the position communication information of the mother boat at the third moment, updating the Kalman coefficient, and performing posterior prediction on the path of the autonomous underwater vehicle at the third moment according to the updated Kalman coefficient, wherein the process is the same as the steps S300-S700, and is not repeated herein.
Optionally, a preset time interval is provided between the first time, the second time and the third time.
In one embodiment, a measured value and a prior estimated value are obtained at preset time intervals, path estimation at each moment, namely a posterior estimated value, is obtained according to extended Kalman filtering, and a new path planning direction is formulated according to the posterior estimated value until the distance between the autonomous underwater vehicle and the mother boat is smaller than the distance required by docking recovery.
Optionally, the system state equation further includes a first white gaussian noise when the forward synthetic motion velocity is measured and a second white gaussian noise when the yaw rate is measured, where the first white gaussian noise and the second white gaussian noise are zero-mean white gaussian noise independent of each other.
Optionally, after step S500, the method further includes:
step S530, obtaining the position information of the mother boat, and obtaining a second direction vector between the autonomous underwater vehicle and the mother boat according to the position information and the path estimation.
After the position information of the mother boat is acquired, a second direction position vector can be obtained according to the posterior path estimation of the autonomous underwater vehicle at the moment, and the second direction position vector is a vector of the position of the autonomous underwater vehicle and the connecting direction of the mother boat at the moment.
And S540, controlling the autonomous underwater vehicle to turn to a preset angle to sail straight on the basis of the second direction vector.
In one embodiment, after the position of the autonomous underwater vehicle and the position of the mother boat are determined, the autonomous underwater vehicle is controlled to rotate by taking the direction of a connecting line of the autonomous underwater vehicle and the mother boat as a reference.
And defining the positive direction of the second direction vector as the direction from the autonomous underwater vehicle to the mother boat, recording the included angle between the second direction vector and the x axis as a second angle, controlling the autonomous underwater vehicle to rotate until the absolute value of the angle difference between the first angle and the second angle is 45 degrees, and controlling the autonomous underwater vehicle to sail linearly until the underwater sound signal of the mother boat is received next time or the distance between the autonomous underwater vehicle and the mother boat is less than the distance required by docking recovery.
In another aspect, the present invention provides an autonomous underwater vehicle homing path planning apparatus, including a computer readable storage medium storing a computer program and a processor, where the computer program is read by the processor and executed to implement the autonomous underwater vehicle homing path planning method.
Compared with the prior art, the homing path planning device of the autonomous underwater vehicle has the advantages that the homing path planning method of the autonomous underwater vehicle is consistent with the homing path planning method of the autonomous underwater vehicle, and the homing path planning device of the autonomous underwater vehicle is not described again.
Although the present disclosure has been described above, the scope of the present disclosure is not limited thereto. Various changes and modifications may be effected therein by one of ordinary skill in the pertinent art without departing from the spirit and scope of the present disclosure, and these changes and modifications are intended to be within the scope of the present disclosure.
Claims (10)
1. An autonomous underwater vehicle homing path planning method is characterized by comprising the following steps:
acquiring first position communication information of a mother ship and second position information of an autonomous underwater vehicle at a first time, and further acquiring a first direction vector between the autonomous underwater vehicle and the mother ship according to the first position communication information and the second position information;
controlling the autonomous underwater vehicle to turn to a preset angle based on the first direction vector, and controlling the autonomous underwater vehicle to sail linearly, wherein the preset angle is determined according to an included angle between the autonomous underwater vehicle and the first direction vector;
obtaining a first state matrix of the autonomous underwater vehicle at the first moment according to the second position information and a course angle, wherein the course angle is an included angle formed by the preset angle and a preset direction;
obtaining a system state equation of the autonomous underwater vehicle according to the first state matrix, the measured forward synthetic motion speed and the measured yaw rate;
based on the system state equation, obtaining a path estimation of the autonomous underwater vehicle at a second moment through an extended Kalman filtering algorithm;
acquiring the distance between the autonomous underwater vehicle and the mother boat according to the path estimation at the second moment, and judging whether the distance is smaller than a preset distance;
and if so, controlling the autonomous underwater vehicle to carry out docking tasks.
2. The homing path planning method for the autonomous underwater vehicle of claim 1, wherein said obtaining a path estimate of the autonomous underwater vehicle at a second time by an extended kalman filter algorithm based on the system state equation comprises:
obtaining, by the extended Kalman filtering algorithm, a first a priori estimated covariance of the autonomous underwater vehicle between the first time and the second time;
updating Kalman coefficients in the extended Kalman filtering algorithm based on the first a priori estimated covariance;
obtaining a first observation equation of the autonomous underwater vehicle at the second moment;
and correcting the path estimation and the estimated covariance of the autonomous underwater vehicle at the second moment according to the updated Kalman coefficient and the first observation equation.
3. The homing path planning method for an autonomous underwater vehicle of claim 2, wherein said obtaining a first observation equation of said autonomous underwater vehicle at said second time comprises:
obtaining a first relative distance between the autonomous underwater vehicle and the mother boat according to the first position communication information and the second position information;
acquiring third position communication information of the mother boat and fourth position information of the autonomous underwater vehicle at the second time;
obtaining a second relative distance between the autonomous underwater vehicle and the mother boat and a moving vector of the autonomous underwater vehicle according to the third position communication information and the fourth position information, wherein the moving vector comprises a displacement and a direction vector of the autonomous underwater vehicle between the first time and the second time;
obtaining the first observation equation based on the first relative distance, the second relative distance, the moving radius, the first location communication information, and the third location communication information.
4. The homing path planning method for the autonomous underwater vehicle as claimed in claim 2 or 3, wherein after said obtaining the distance between the autonomous underwater vehicle and the mother boat and judging whether the distance is smaller than a preset distance, the homing path planning method further comprises:
if the distance is greater than the preset distance, obtaining the path estimation of the autonomous underwater vehicle at a third moment according to the extended Kalman filtering algorithm;
judging whether the distance between the autonomous underwater vehicle and the mother boat is smaller than the preset distance according to the path estimation at the third moment;
if not, continuing to obtain the path estimation of the autonomous underwater vehicle at the later moment until the distance between the autonomous underwater vehicle and the mother boat is smaller than the preset distance.
5. The homing path planning method for the autonomous underwater vehicle of claim 4, wherein said obtaining the path estimate of the autonomous underwater vehicle at the third time according to the extended kalman filter algorithm if the distance is greater than a preset distance comprises:
obtaining a second prior estimation covariance of a third moment according to the estimation covariance of the second moment, and updating the Kalman coefficient based on the second prior estimation covariance;
acquiring a distance observation value between the autonomous underwater vehicle and the mother boat at the third time;
and obtaining a path estimation of the third moment according to the updated Kalman coefficient and the observation value of the autonomous underwater vehicle at the third moment.
6. The homing path planning method for the autonomous underwater vehicle of claim 4, wherein said controlling the autonomous underwater vehicle to slew to a preset angle based on the first directional vector and to sail straight comprises:
controlling the autonomous underwater vehicle to turn at a maximum rudder angle.
7. The autonomous underwater vehicle homing path planning method of claim 4, wherein there is a preset time interval between said first time, said second time and said third time.
8. The autonomous underwater vehicle homing path planning method of claim 4, wherein the system state equation further comprises a first white gaussian noise when measuring the forward synthetic motion velocity and a second white gaussian noise when measuring the yaw angular velocity, wherein the first white gaussian noise and the second white gaussian noise are independent white gaussian noise with zero mean.
9. The homing path planning method for the autonomous underwater vehicle according to claim 2, further comprising, after said obtaining a path estimate of the autonomous underwater vehicle at a second time by an extended kalman filter algorithm based on the system state equation:
acquiring position information of the mother boat, and acquiring a second direction vector between the autonomous underwater vehicle and the mother boat according to the position information and the path estimation;
and controlling the autonomous underwater vehicle to turn to a preset angle to sail straight on the basis of the second direction vector.
10. An autonomous underwater vehicle homing path planning apparatus comprising a computer readable storage medium storing a computer program and a processor, the computer program being read and executed by the processor to implement the autonomous underwater vehicle homing path planning method of any of claims 1-9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210120059.9A CN114610021A (en) | 2022-02-07 | 2022-02-07 | Autonomous underwater vehicle homing path planning method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210120059.9A CN114610021A (en) | 2022-02-07 | 2022-02-07 | Autonomous underwater vehicle homing path planning method and device |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114610021A true CN114610021A (en) | 2022-06-10 |
Family
ID=81858377
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210120059.9A Pending CN114610021A (en) | 2022-02-07 | 2022-02-07 | Autonomous underwater vehicle homing path planning method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114610021A (en) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104075715A (en) * | 2014-07-07 | 2014-10-01 | 东南大学 | Underwater navigation and positioning method capable of combining terrain and environment characteristics |
| CN107167126A (en) * | 2017-03-31 | 2017-09-15 | 大鹏高科（武汉）智能装备有限公司 | A kind of autonomous type underwater robot Combinated navigation method and system |
| CN107200099A (en) * | 2017-05-23 | 2017-09-26 | 大连理工大学 | Underwater robot automatic recycling device and its method of work based on unmanned boat platform |
| CN107976188A (en) * | 2017-10-12 | 2018-05-01 | 浙江大学 | A kind of AUV led to based on ranging sound returns depressed place air navigation aid |
| CN109238291A (en) * | 2018-10-26 | 2019-01-18 | 河海大学 | A kind of planing method of water surface unmanned boat guiding cable recycling Autonomous Underwater Vehicle |
| CN111596333A (en) * | 2020-06-05 | 2020-08-28 | 鹏城实验室 | Underwater positioning navigation method and system |
| CN111982098A (en) * | 2019-05-21 | 2020-11-24 | 中国科学院沈阳自动化研究所 | Simultaneous positioning method for two moving AUVs |
| CN112241176A (en) * | 2020-10-16 | 2021-01-19 | 哈尔滨工程大学 | Path planning and obstacle avoidance control method of underwater autonomous vehicle in large-scale continuous obstacle environment |
| CN113984045A (en) * | 2021-10-13 | 2022-01-28 | 湖南航天机电设备与特种材料研究所 | Underwater robot mobile docking target motion state estimation method and system |
-
2022
- 2022-02-07 CN CN202210120059.9A patent/CN114610021A/en active Pending
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104075715A (en) * | 2014-07-07 | 2014-10-01 | 东南大学 | Underwater navigation and positioning method capable of combining terrain and environment characteristics |
| CN107167126A (en) * | 2017-03-31 | 2017-09-15 | 大鹏高科（武汉）智能装备有限公司 | A kind of autonomous type underwater robot Combinated navigation method and system |
| CN107200099A (en) * | 2017-05-23 | 2017-09-26 | 大连理工大学 | Underwater robot automatic recycling device and its method of work based on unmanned boat platform |
| CN107976188A (en) * | 2017-10-12 | 2018-05-01 | 浙江大学 | A kind of AUV led to based on ranging sound returns depressed place air navigation aid |
| CN109238291A (en) * | 2018-10-26 | 2019-01-18 | 河海大学 | A kind of planing method of water surface unmanned boat guiding cable recycling Autonomous Underwater Vehicle |
| CN111982098A (en) * | 2019-05-21 | 2020-11-24 | 中国科学院沈阳自动化研究所 | Simultaneous positioning method for two moving AUVs |
| CN111596333A (en) * | 2020-06-05 | 2020-08-28 | 鹏城实验室 | Underwater positioning navigation method and system |
| CN112241176A (en) * | 2020-10-16 | 2021-01-19 | 哈尔滨工程大学 | Path planning and obstacle avoidance control method of underwater autonomous vehicle in large-scale continuous obstacle environment |
| CN113984045A (en) * | 2021-10-13 | 2022-01-28 | 湖南航天机电设备与特种材料研究所 | Underwater robot mobile docking target motion state estimation method and system |
Non-Patent Citations (1)
| Title |
|---|
| CHEN PENG-YUN等: "Underwater Terrain Positioning Method Based on Least Squares Estimation for AUV", 《CHINA OCEAN ENGINEERING》, vol. 29, no. 6, 30 June 2015 (2015-06-30), pages 859 - 874, XP035736009, DOI: 10.1007/s13344-015-0060-9 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Wang et al. | Roboat II: A novel autonomous surface vessel for urban environments | |
| CN106444806B (en) | The drive lacking AUV three-dimensional track tracking and controlling method adjusted based on biological speed | |
| CN109765929B (en) | UUV real-time obstacle avoidance planning method based on improved RNN | |
| CN108445762B (en) | Dynamic positioning control method based on UKF filtering and constraint model predictive control | |
| Xu et al. | A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles | |
| CN112068440B (en) | AUV recovery butt joint power positioning control method based on model prediction control | |
| CN109946976B (en) | Wide-navigational-speed AUV motion control method | |
| Bo et al. | Cooperative localisation of AUVs based on Huber-based robust algorithm and adaptive noise estimation | |
| CN112710304B (en) | Underwater autonomous vehicle navigation method based on adaptive filtering | |
| CN115560759A (en) | Underwater multi-source navigation positioning method based on seabed oil and gas pipeline detection | |
| CN113467231A (en) | Unmanned ship path tracking method based on sideslip compensation ILOS guidance law | |
| Karras et al. | Localization of an underwater vehicle using an IMU and a laser-based vision system | |
| Noguchi et al. | Tracking omnidirectional surfaces using a low-cost autonomous underwater vehicle | |
| CN110333369B (en) | UUV DVL speed measurement system based on water surface GPS correction and self-adaptive denoising method | |
| CN114967714A (en) | Anti-interference motion control method and system for autonomous underwater robot | |
| CN114967702A (en) | Unmanned ship control system and path tracking method | |
| Yildiz et al. | State-of-the-art system solutions for unmanned underwater vehicles | |
| Pelletier et al. | AUV-assisted diver navigation | |
| Li et al. | Terrain correlation correction method for AUV seabed terrain mapping | |
| Horner et al. | AUV experiments in obstacle avoidance | |
| Annamalai et al. | A robust navigation technique for integration in the guidance and control of an uninhabited surface vehicle | |
| CN116540717A (en) | AUV local path planning method based on improved DWA | |
| CN113984045B (en) | Method and system for estimating motion state of movable butt-joint target of underwater robot | |
| CN114610021A (en) | Autonomous underwater vehicle homing path planning method and device | |
| Mousazadeh et al. | Experimental evaluation of a new developed algorithm for an autonomous surface vehicle and comparison with simulink results |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |