CN114894187B - Unmanned ship navigation method - Google Patents
Unmanned ship navigation method Download PDFInfo
- Publication number
- CN114894187B CN114894187B CN202210591518.1A CN202210591518A CN114894187B CN 114894187 B CN114894187 B CN 114894187B CN 202210591518 A CN202210591518 A CN 202210591518A CN 114894187 B CN114894187 B CN 114894187B
- Authority
- CN
- China
- Prior art keywords
- angle
- navigation
- moment
- unmanned ship
- imu
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 27
- 230000008569 process Effects 0.000 claims abstract description 16
- 238000001914 filtration Methods 0.000 claims abstract description 12
- 230000007704 transition Effects 0.000 claims description 24
- 238000005259 measurement Methods 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 8
- 238000012546 transfer Methods 0.000 claims description 7
- 238000002474 experimental method Methods 0.000 abstract description 3
- 238000000926 separation method Methods 0.000 abstract description 2
- 230000004927 fusion Effects 0.000 description 6
- 230000001276 controlling effect Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000011835 investigation Methods 0.000 description 2
- 239000002184 metal Substances 0.000 description 2
- 238000001228 spectrum Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005562 fading Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/024—Guidance services
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/025—Services making use of location information using location based information parameters
- H04W4/026—Services making use of location information using location based information parameters using orientation information, e.g. compass
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/029—Location-based management or tracking services
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
Abstract
The invention discloses a navigation method of an unmanned ship, which uses an angle fed back by an IMU to carry out closed-loop control on a course angle of the unmanned ship, introduces UWB positioning data, enables the unmanned ship to navigate at a preset current angle for a time interval, records position coordinates before and after the time interval, solves a true angle value of navigation through two groups of coordinates, fuses the gyroscope data of the IMU and the angle value of a coordinate point in a Kalman filtering mode, and finally uses a cascade integral separation PID algorithm to carry out course speed control of the unmanned ship, so that the unmanned ship navigation process is accurate and stable. According to the invention, the drifting heading angle value of the IMU and the accurate position value of the UWB are fused through the Kalman filter, so that the problem of difficult navigation of the small unmanned ship due to heading angle drifting under the condition of a pool is solved. The hardware implementation cost of the invention is extremely low, and the invention is a better choice for the unmanned ship pool navigation experiment with low cost.
Description
Technical Field
The invention belongs to the technical field of robot positioning and navigation, and relates to an unmanned ship navigation method, in particular to an unmanned ship navigation method based on UWB and IMU fusion.
Background
The unmanned ship is a water surface movement platform with certain autonomous capability, can independently navigate and complete operation tasks under unmanned conditions, has the characteristics of low cost, concealed action, flexibility and the like, can execute various tasks such as environmental investigation, search and rescue, material transportation and the like, and is widely applied to marine investigation and research.
An Inertial Measurement Unit (IMU) is a device that measures the three-axis attitude angle (or angular rate) and acceleration of an object. Are often used on devices requiring motion control, such as automobiles and robots. When unmanned ship navigation control is carried out in the pond, inertial Measurement Unit (IMU) installs on unmanned ship, because accelerometer fuses the filtering to the gyroscope and can't offset the static error of IMU course angle, along with unmanned ship's roll pitching, IMU's course angle drift is serious, can't independently satisfy unmanned ship's course feedback demand in the pond debugging process, GPS positioning signal is also difficult to obtain in the pond simultaneously, and magnetic compass receives the metal structure thing magnetic field interference in the pond and also can't effectively use, these problems all lead to unmanned ship heading to acquire difficulty.
Ultra Wide Band (UWB) technology is a wireless carrier communication technology, which uses non-sinusoidal narrow pulses of nanosecond order to transmit data, so that the spectrum occupied by the technology is Wide. The method has the advantages of low system complexity, low power spectrum density of the transmitted signal, insensitivity to channel fading, low interception capability, high positioning accuracy and the like. Is widely applied to small-range high-precision positioning.
Disclosure of Invention
Aiming at the problems and the defects of the prior art, the invention aims to provide an unmanned ship navigation method based on the fusion of UWB and IMU, which solves the problems that GPS positioning signals are difficult to obtain in a pool experiment, and the heading of an unmanned ship is difficult to obtain due to the fact that a magnetic compass is interfered by a metal structure magnetic field in a pool and an independent IMU course angle has drift.
In order to solve the technical problems, the unmanned ship navigation method provided by the invention comprises the following steps:
step one: on the premise of only IMU angle data, a PID control algorithm for integrating and separating the course angle of the unmanned aerial vehicle is used for respectively setting a threshold epsilon and setting parameters kp, kd and ki for an angle controller and a speed controller and controlling the unmanned aerial vehicle to carry out direct navigation in a closed loop state;
Step two: giving a navigation point set to the unmanned aerial vehicle, wherein the point set comprises a plurality of target position points which the unmanned aerial vehicle needs to pass through when sailing, the coordinates of the position points are (x n,yn), and n=1, 2,3, … N and N are the number of the target position points in the navigation point set;
Step three: taking a stage target position point (x n,yn) in the navigation point set, if the step three is executed for the first time, initializing n=1;
Step four: the unmanned aerial vehicle records a current position point coordinate (x i,yi) and a current heading angle psi imu_1 output by the IMU through the UWB module, takes the current heading angle psi imu_1 as a target heading angle, carries out direct navigation control under the unmanned aerial vehicle closed-loop state, and carries out direct navigation time interval T, wherein the zero offset stability of the heading angle data of the IMU is smaller than a set precision factor in the time T; after the direct voyage is finished, obtaining a current position point coordinate (x j,yj) after the time T through a UWB module, and obtaining a real voyage heading psi r in the moment T of the unmanned ship through the two position point coordinates;
Step five: calculating a direction vector angle psi t formed by a target position point (x n,yn) and a current position point (x j,yj) at the stage, and obtaining a current required rotation angle delta psi t of the unmanned ship from a real sailing heading psi r and the direction vector angle psi t;
Step six: inputting a rotation angle delta phi t as a command into a motion controller of the unmanned ship, enabling the real heading phi r of the unmanned ship to be aligned with a stage target position point (x n,yn), recording the current real heading phi r as a target angle phi t of a directional controller, and starting directional direct navigation;
Step seven: in the direct navigation process, a course angle of the current moment k is obtained by utilizing Kalman filtering
Step eight: calculating a distance d nk between a hull k time location point and a stage target location point (x n,yn);
step nine: and D, obtaining the course angle obtained in the step seven Inputting a distance d nk between a position point of the hull at the moment k obtained in the step eight and a target point as a command to the angle controller, and controlling the unmanned ship to navigate;
Step ten: judging whether d nk is zero or not, if so, reaching a target point, and executing the step eleventh; otherwise, returning to the step seven;
Step eleven: judging whether n=n is true, and if so, ending the navigation process; otherwise, let n=n+1, return to step three.
Further, in the seventh step, the heading angle of the current moment k is obtained by Kalman filteringComprising the following steps:
Obtaining a k-1 moment position point coordinate (x k-1,yk-1) and a current k moment position point coordinate (x k,yk) through a UWB module, calculating a motion vector direction according to the two moment position point coordinates, and considering the motion direction as a heading angle observed value of the hull under the condition that the hull directly navigates without a drift angle, and marking the heading angle observed value as alpha k; let the course angle of the boat body output by the filter be And if the angular speed of the course output by the IMU in the time interval delta t is omega k-1, the state transfer equation of the course angle of the boat body is as follows:
wherein, For the course state transfer of the ship body at the moment k according to the prediction of the angular velocity output by the IMU, delta t is the time interval between the moment k-1 and the moment k;
the covariance equation for state transition is:
wherein, For the covariance predicted according to the state transition equation, A is the state transition matrix, 1 delta t is taken, and Q is the covariance of the state transition process;
The Kalman gain K k is calculated as:
Wherein H is a transition matrix from a state space to a measurement space, R is measured heading angle measurement noise according to double standard position values of the UWB module, namely a k-1 moment position point coordinate and a k moment position point coordinate, an estimated value and a state transition covariance are updated, and an update equation is as follows:
wherein, And filtering the obtained course angle for the moment k.
Further, the direct-endurance interval T satisfies:
wherein V is the navigation speed, and L is the coxswain.
The invention has the beneficial effects that: the IMU output data is used for carrying out basic motion control on the unmanned ship, and on the basis of the course angle control of the IMU with drift, different time difference data of UWB are added for fusion, so that the aim of correcting the course angle and further completing accurate navigation is achieved. And performing closed-loop control on the course angle of the unmanned ship by using the angle fed back by the IMU, wherein the closed-loop control comprises constant-angle navigation and constant-angle rotation. On the basis, UWB positioning data are introduced, so that the unmanned aerial vehicle sails at a current angle for a time interval, position coordinates before sailing and after the time interval are recorded, real angle values of sailing are solved through two groups of coordinates, the gyroscope data of the IMU and the angle values of coordinate points are fused in a Kalman filtering mode, and finally, the cascade integral separation PID algorithm is used for carrying out course speed control on the unmanned aerial vehicle, so that the unmanned aerial vehicle sails accurately and stably. Compared with the prior art, the invention solves the problem of difficult navigation of the small unmanned ship caused by course angle drift under the condition of a pool by fusing the drifting course angle value of the IMU and the accurate position value of UWB through the Kalman filter. The hardware implementation cost of the invention is extremely low, and the invention is a better choice for the unmanned ship pool navigation experiment with low cost.
Drawings
FIG. 1 is a schematic illustration of navigation using UWB and IMU fusion navigation;
FIG. 2 is a schematic diagram of an unmanned boat motion control algorithm;
FIG. 3 is a flow chart of a fusion navigation algorithm.
Detailed Description
The invention is further described below with reference to the drawings and examples.
The invention comprises the following steps:
step one: on the premise of only IMU angle data, a PID control algorithm for integrating and separating the course angle of the unmanned aerial vehicle is used for respectively setting a threshold epsilon and setting parameters kp, kd and ki for an angle controller and a speed controller and controlling the unmanned aerial vehicle to carry out direct navigation in a closed loop state;
Step two: giving a navigation point set to the unmanned aerial vehicle, wherein the point set comprises a plurality of target position points which the unmanned aerial vehicle needs to pass through when sailing, the coordinates of the position points are (x n,yn), and n=1, 2,3, … N and N are the number of the target position points in the navigation point set;
Step three: taking a stage target position point (x n,yn) in the navigation point set, and initializing n=1;
Step four: the unmanned aerial vehicle records a current position point coordinate (x i,yi) and a current heading angle psi imu_1 output by the IMU through the UWB module, takes the current heading angle psi imu_1 as a target heading angle, carries out direct navigation control under the unmanned aerial vehicle closed-loop state, and carries out direct navigation time interval T, wherein the zero offset stability of the heading angle data of the IMU is smaller than a set precision factor in the time T; after the direct voyage is finished, obtaining a current position point coordinate (x j,yj) after the time T through a UWB module, and obtaining a real voyage heading psi r in the moment T of the unmanned ship through the two position point coordinates;
Step five: calculating a direction vector angle psi t formed by a target position point (x n,yn) and a current position point (x j,yj) at the stage, and obtaining a current required rotation angle delta psi t of the unmanned ship from a real sailing heading psi r and the direction vector angle psi t;
Step six: inputting a rotation angle delta phi t as a command into a motion controller of the unmanned ship, enabling the real heading phi r of the unmanned ship to be aligned with a stage target position point (x n,yn), recording the current real heading phi r as a target angle phi t of a directional controller, and starting directional direct navigation;
Step seven: in the direct navigation process, a course angle of the current moment k is obtained by utilizing Kalman filtering Obtaining a k-1 moment position point coordinate (x k-1,yk-1) and a current k moment position point coordinate (x k,yk) through a UWB module, calculating a motion vector direction according to the two moment position point coordinates, and considering the motion direction as a heading angle observed value of the hull under the condition that the hull directly navigates without a drift angle, and marking the heading angle observed value as alpha k; let the course angle of the boat body output by the filter be/>And if the angular speed of the course output by the IMU in the time interval delta t is omega k-1, the state transfer equation of the course angle of the boat body is as follows:
wherein, For the course state transfer of the ship body at the moment k according to the prediction of the angular velocity output by the IMU, delta t is the time interval between the moment k-1 and the moment k;
the covariance equation for state transition is:
wherein, For the covariance predicted according to the state transition equation, A is the state transition matrix, 1 delta t is taken, and Q is the covariance of the state transition process;
The Kalman gain K k is calculated as:
wherein H is a transition matrix from a state space to a measurement space, R is heading angle measurement noise measured according to a double standard position value of the UWB module, an estimated value and a state transition covariance are updated, and an update equation is as follows:
wherein, And filtering the obtained course angle for the moment k.
Step eight: calculating a distance d nk between a hull k time location point and a stage target location point (x n,yn);
step nine: and D, obtaining the course angle obtained in the step seven Inputting a distance d nk between a position point of the hull at the moment k obtained in the step eight and a target point as a command to the angle controller, and controlling the unmanned ship to navigate;
Step ten: judging whether d nk is zero or not, if so, reaching a target point, and executing the step eleventh; otherwise, returning to the step seven;
Step eleven: judging whether n=n is true, and if so, ending the navigation process; otherwise, let n=n+1, return to step three.
The examples are given in connection with specific parameters.
With reference to fig. 1, 2 and 3, the present invention comprises the steps of:
Step one: and (3) on the premise of only IMU angle data, integrating and separating the heading angle of the unmanned ship, setting a threshold epsilon according to actual conditions, and setting parameters kp, kd and ki.
As shown in fig. 2, a PID control algorithm for integrating and separating the heading angle and the distance of the unmanned ship sets a threshold epsilon for an angle controller and a speed controller respectively, and when the deviation of the controlled angle or distance is smaller than the threshold epsilon, the PID controller is used to adjust three controller parameters of k_p, k_i and k_d so as to eliminate the static difference of the system and improve the control precision. When the controlled angle or distance deviation is larger than the threshold epsilon, the PD controller is used, and three controller parameters of k_p, k_i and k_d are regulated, so that excessive overshoot is avoided, and the system has faster control response. And inputting the angle controller and the speed controller into a propeller of the unmanned ship to complete the angle and speed control of the unmanned ship. These two controllers will also be the execution preconditions for the subsequent steps of the algorithm.
Step two: on the basis of the closed-loop control effect, a navigation point set is given to the unmanned aerial vehicle, wherein the point set comprises a plurality of target position point coordinates (x n,yn) (n=1, 2,3, … N) required by the unmanned aerial vehicle to navigate, and N is the number of points in the navigation point set.
Step three: the stage target position point coordinates (x n,yn) in the navigation point set are taken.
Step four: the unmanned ship records the current position point coordinates (x i,yi) and the heading angle psi imu_1 output by the current IMU, takes the heading angle psi imu_1 at the moment as a target heading angle, and carries out direct navigation control under the unmanned ship closed-loop state, and the time interval T of direct navigation.
The upper navigation speed V is related to the boat size L, and the heading angle data of the IMU is basically accurate in the time T. After the direct voyage is finished, the coordinates (x j,yj) of the current position point after the time T are recorded. And obtaining the real sailing heading phi r in the moment of the unmanned ship T through the coordinates of the two position points. The position point coordinates are obtained by the UWB module.
Step five: and calculating a direction vector angle phi t formed by two points of the target position point coordinate (x n,yn) and the current position point coordinate (x j,yj) of the unmanned ship. The current required rotation angle delta phi t of the unmanned ship can be obtained by the real sailing heading phi r and the direction vector angle phi t,
Step six: the motion controller of the unmanned boat is instructed to rotate by an angle Δψ t so that the target point position (x n,yn) is in the radial direction of the real heading ψ r of the unmanned boat. Recording the current real heading phi r as a target angle phi t of the directional controller, and starting the directional direct navigation.
Step seven: in the direct voyage process, the motion vector direction of the position coordinate (x k-1,yk-1) at the moment k-1 and the position point coordinate (x k,yk) at the moment k is calculated periodically, and under the condition that the direct voyage of the hull does not have a drift angle, the motion direction is considered to be a heading angle observation value of the hull and is marked as alpha t. Let the course angle of the boat body output by the filter beAt a certain time interval Δt, if the angular velocity of the heading output by the IMU is ω k-1, the state transfer equation of the hull heading angle is:
wherein, And the ship course state is transferred according to the prediction of the angular speed output by the IMU. And the covariance equation of the state transition is:
wherein, In order to predict covariance according to the state transition equation, A is a state transition matrix, the algorithm takes [1 delta t ], Q is covariance of the state transition process, and [0.01 ] is initially taken.
The kalman gain K k is thus found to be:
Wherein H is a transition matrix from a state space to a measurement space, [ 10 ] is taken, R is heading angle measurement noise measured according to a double standard position value of UWB, 1 degree is initially taken, and after gain is obtained, an estimated value and a state transition covariance can be updated. The update equation is as follows:
Course angle obtained by filtering The angle used in the sailing process of the unmanned boat is used for fine adjustment of the heading when the unmanned boat moves to the target point.
Step eight: and calculating the distance d nt between the current position point of the hull and the target point.
Step nine: and step four to seven, combining the heading fusion result performed by the unmanned ship IMU and the UWB with the unmanned ship position deviation amount information of step eight, and inputting the information into an angle deviation controller and a distance deviation controller, thereby completing the navigation control process of the unmanned ship on the navigation points.
Step ten: and step three to step nine are circularly executed until the navigation point set is completed by the unmanned ship to navigate, and the navigation process is finished.
Claims (3)
1. The unmanned ship navigation method is characterized by comprising the following steps of:
step one: on the premise of only IMU angle data, a PID control algorithm for integrating and separating the course angle of the unmanned aerial vehicle is used for respectively setting a threshold epsilon and setting parameters kp, kd and ki for an angle controller and a speed controller and controlling the unmanned aerial vehicle to carry out direct navigation in a closed loop state;
Step two: giving a navigation point set to the unmanned aerial vehicle, wherein the point set comprises a plurality of target position points which the unmanned aerial vehicle needs to pass through when sailing, the coordinates of the position points are (x n,yn), and n=1, 2,3, … N and N are the number of the target position points in the navigation point set;
Step three: taking a stage target position point (x n,yn) in the navigation point set, if the step three is executed for the first time, initializing n=1;
Step four: the unmanned aerial vehicle records a current position point coordinate (x i,yi) and a current heading angle psi imu_1 output by the IMU through the UWB module, takes the current heading angle psi imu_1 as a target heading angle, carries out direct navigation control under the unmanned aerial vehicle closed-loop state, and carries out direct navigation time interval T, wherein the zero offset stability of the heading angle data of the IMU is smaller than a set precision factor in the time T; after the direct voyage is finished, obtaining a current position point coordinate (x j,yj) after the time T through a UWB module, and obtaining a real voyage heading psi r in the moment T of the unmanned ship through the two position point coordinates;
Step five: calculating a direction vector angle psi t formed by a target position point (x n,yn) and a current position point (x j,yj) at the stage, and obtaining a current required rotation angle delta psi t of the unmanned ship from a real sailing heading psi r and the direction vector angle psi t;
Step six: inputting a rotation angle delta phi t as a command into a motion controller of the unmanned ship, enabling the real heading phi r of the unmanned ship to be aligned with a stage target position point (x n,yn), recording the current real heading phi r as a target angle phi t of a directional controller, and starting directional direct navigation;
Step seven: in the direct navigation process, a course angle of the current moment k is obtained by utilizing Kalman filtering
Step eight: calculating a distance d nk between a hull k time location point and a stage target location point (x n,yn);
step nine: and D, obtaining the course angle obtained in the step seven Inputting a distance d nk between a position point of the hull at the moment k obtained in the step eight and a target point as a command to the angle controller, and controlling the unmanned ship to navigate;
Step ten: judging whether d nk is zero or not, if so, reaching a target point, and executing the step eleventh; otherwise, returning to the step seven;
Step eleven: judging whether n=n is true, and if so, ending the navigation process; otherwise, let n=n+1, return to step three.
2. The unmanned aerial vehicle navigation method of claim 1, wherein: step seven, obtaining the course angle of the current moment k by using Kalman filteringComprising the following steps:
Obtaining a k-1 moment position point coordinate (x k-1,yk-1) and a current k moment position point coordinate (x k,yk) through a UWB module, calculating a motion vector direction according to the two moment position point coordinates, and considering the motion direction as a heading angle observed value of the hull under the condition that the hull directly navigates without a drift angle, and marking the heading angle observed value as alpha k; let the course angle of the boat body output by the filter be And if the angular speed of the course output by the IMU in the time interval delta t is omega k-1, the state transfer equation of the course angle of the boat body is as follows:
wherein, For the course state transfer of the ship body at the moment k according to the prediction of the angular velocity output by the IMU, delta t is the time interval between the moment k-1 and the moment k;
the covariance equation for state transition is:
wherein, For the covariance predicted according to the state transition equation, A is the state transition matrix, 1 delta t is taken, and Q is the covariance of the state transition process;
The Kalman gain K k is calculated as:
Wherein H is a transition matrix from a state space to a measurement space, R is measured heading angle measurement noise according to double standard position values of the UWB module, namely a k-1 moment position point coordinate and a k moment position point coordinate, an estimated value and a state transition covariance are updated, and an update equation is as follows:
wherein, And filtering the obtained course angle for the moment k.
3. An unmanned aerial vehicle navigation method according to claim 1 or 2, wherein: the direct time interval T satisfies:
wherein V is the navigation speed, and L is the coxswain.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210591518.1A CN114894187B (en) | 2022-05-27 | 2022-05-27 | Unmanned ship navigation method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210591518.1A CN114894187B (en) | 2022-05-27 | 2022-05-27 | Unmanned ship navigation method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114894187A CN114894187A (en) | 2022-08-12 |
| CN114894187B true CN114894187B (en) | 2024-04-19 |
Family
ID=82726854
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210591518.1A Active CN114894187B (en) | 2022-05-27 | 2022-05-27 | Unmanned ship navigation method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114894187B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105785999A (en) * | 2016-04-27 | 2016-07-20 | 大连海事大学 | Unmanned surface vehicle course motion control method |
| CN109239709A (en) * | 2018-08-02 | 2019-01-18 | 哈尔滨工程大学 | A kind of autonomous construction method of local environment map of unmanned boat |
-
2022
- 2022-05-27 CN CN202210591518.1A patent/CN114894187B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105785999A (en) * | 2016-04-27 | 2016-07-20 | 大连海事大学 | Unmanned surface vehicle course motion control method |
| CN109239709A (en) * | 2018-08-02 | 2019-01-18 | 哈尔滨工程大学 | A kind of autonomous construction method of local environment map of unmanned boat |
Non-Patent Citations (2)
| Title |
|---|
| An AUV localization and path planning algorithm for terrain-aided navigation;Ma Teng et al.;ISA Transactions;20200831;全文 * |
| 智能无人水下航行器水下回收对接技术综述;孙叶义 等;哈尔滨工程大学学报;20191231;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114894187A (en) | 2022-08-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Fossen et al. | Line-of-sight path following for dubins paths with adaptive sideslip compensation of drift forces | |
| CN107065898B (en) | Navigation control method and system for underwater unmanned ship | |
| Nađ et al. | Navigation, guidance and control of an overactuated marine surface vehicle | |
| CN111324132B (en) | Intelligent ship automatic berthing control method based on coordinate compensation | |
| WO2015134759A1 (en) | Sailing user interface systems and methods | |
| CN109946976B (en) | Wide-navigational-speed AUV motion control method | |
| Yang et al. | An improved stanley guidance law for large curvature path following of unmanned surface vehicle | |
| Acuna et al. | Vision-based UAV landing on a moving platform in GPS denied environments using motion prediction | |
| Figueiredo et al. | Vision-based localization and positioning of an AUV | |
| CN114740859A (en) | Automatic ship hovering method and system | |
| CN114995409A (en) | Unmanned ship autonomous cruise control system and cruise method | |
| CN113296505B (en) | Unmanned ship multi-mode path tracking control method based on speed change LOS | |
| CN117389312B (en) | Model-based anti-roll three-dimensional tracking control method for underwater vehicle | |
| CN114894187B (en) | Unmanned ship navigation method | |
| Wang et al. | Dynamic model-aided localization of underwater autonomous gliders | |
| CN113093804A (en) | Unmanned ship formation control method and control system based on inversion sliding mode control | |
| CN112486168A (en) | Mobile docking track planning method based on revolution circle | |
| Kula et al. | Control system of training ship keeping the desired path consisting of straight-lines and circular arcs | |
| CN111947654A (en) | Navigation and control integrated chip and control method thereof | |
| Grenestedt et al. | LORCA: A high performance USV with applications to surveillance and monitoring | |
| CN116679693A (en) | Unmanned ship active disturbance rejection control method integrating propeller hydrodynamic force | |
| CN110426958A (en) | Unmanned ships and light boats navigation control method, system, storage medium and computer equipment | |
| CN113984045B (en) | Method and system for estimating motion state of movable butt-joint target of underwater robot | |
| CN114791735A (en) | Unmanned ship deviation rectifying method and system | |
| CN114879651A (en) | Power positioning method for under-actuated unmanned surface vessel |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |