CN114802616A - Small-sized unmanned surface cleaning boat and control method and positioning method thereof - Google Patents
Small-sized unmanned surface cleaning boat and control method and positioning method thereof Download PDFInfo
- Publication number
- CN114802616A CN114802616A CN202210396815.0A CN202210396815A CN114802616A CN 114802616 A CN114802616 A CN 114802616A CN 202210396815 A CN202210396815 A CN 202210396815A CN 114802616 A CN114802616 A CN 114802616A
- Authority
- CN
- China
- Prior art keywords
- uwb positioning
- ship
- control
- unmanned
- water
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000004140 cleaning Methods 0.000 title claims abstract description 28
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 60
- 239000010813 municipal solid waste Substances 0.000 claims abstract description 48
- 230000033001 locomotion Effects 0.000 claims abstract description 28
- 241000195493 Cryptophyta Species 0.000 claims abstract description 19
- 239000007787 solid Substances 0.000 claims abstract description 10
- 239000002699 waste material Substances 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 19
- 238000005070 sampling Methods 0.000 claims description 12
- 238000000926 separation method Methods 0.000 claims description 8
- 230000005540 biological transmission Effects 0.000 claims description 7
- 238000007476 Maximum Likelihood Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 6
- 239000012528 membrane Substances 0.000 claims description 6
- 238000001471 micro-filtration Methods 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 5
- 238000012545 processing Methods 0.000 claims description 4
- 239000012530 fluid Substances 0.000 claims description 3
- 239000002910 solid waste Substances 0.000 claims 1
- 238000005202 decontamination Methods 0.000 abstract description 7
- 230000003588 decontaminative effect Effects 0.000 abstract description 7
- 239000003344 environmental pollutant Substances 0.000 description 6
- 231100000719 pollutant Toxicity 0.000 description 6
- 238000012544 monitoring process Methods 0.000 description 5
- 238000013461 design Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 238000007667 floating Methods 0.000 description 3
- OKTJSMMVPCPJKN-UHFFFAOYSA-N Carbon Chemical compound [C] OKTJSMMVPCPJKN-UHFFFAOYSA-N 0.000 description 2
- 238000010521 absorption reaction Methods 0.000 description 2
- 108010066057 cabin-1 Proteins 0.000 description 2
- 108010066114 cabin-2 Proteins 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000005374 membrane filtration Methods 0.000 description 2
- 239000010865 sewage Substances 0.000 description 2
- 239000000126 substance Substances 0.000 description 2
- 241000196324 Embryophyta Species 0.000 description 1
- 102000004190 Enzymes Human genes 0.000 description 1
- 108090000790 Enzymes Proteins 0.000 description 1
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000002776 aggregation Effects 0.000 description 1
- 238000004220 aggregation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000011942 biocatalyst Substances 0.000 description 1
- -1 biological ceramsite Substances 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000008676 import Effects 0.000 description 1
- 229910052744 lithium Inorganic materials 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012067 mathematical method Methods 0.000 description 1
- 238000001208 nuclear magnetic resonance pulse sequence Methods 0.000 description 1
- 239000012466 permeate Substances 0.000 description 1
- 238000000746 purification Methods 0.000 description 1
- 238000012887 quadratic function Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
- B63B—SHIPS OR OTHER WATERBORNE VESSELS; EQUIPMENT FOR SHIPPING
- B63B35/00—Vessels or similar floating structures specially adapted for specific purposes and not otherwise provided for
- B63B35/32—Vessels or similar floating structures specially adapted for specific purposes and not otherwise provided for for collecting pollution from open water
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B11/00—Measuring arrangements characterised by the use of optical techniques
- G01B11/22—Measuring arrangements characterised by the use of optical techniques for measuring depth
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0278—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves involving statistical or probabilistic considerations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/06—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A20/00—Water conservation; Efficient water supply; Efficient water use
- Y02A20/20—Controlling water pollution; Waste water treatment
- Y02A20/204—Keeping clear the surface of open water from oil spills
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Health & Medical Sciences (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Probability & Statistics with Applications (AREA)
- Environmental & Geological Engineering (AREA)
- Public Health (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Mechanical Engineering (AREA)
- Ocean & Marine Engineering (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention belongs to the technical field of water area environment protection, and particularly relates to a small unmanned surface vehicle for water surface decontamination, and a control method and a positioning method thereof. Unmanned ship of small-size surface of water cleaning includes: a waste collection bin for collecting waste; the garbage collection cabin comprises a solid garbage collection cabin and an algae garbage treatment cabin; sensing means for sensing a position of the user; the sensing device comprises a depth camera, a laser radar and a UWB positioning tag; a control device for controlling the movement of the hull; the control device comprises a motion controller and a main controller; a propulsion device for providing power; the propulsion device comprises a plurality of underwater propellers positioned at the stern and is used for taking charge of the power of advancing and steering the ship body. The invention is suitable for small and medium size water pools, and can efficiently clean plankton on water.
Description
Technical Field
The invention belongs to the technical field of water area environment protection, and particularly relates to a small unmanned surface vehicle for water surface decontamination, and a control method and a positioning method thereof.
Background
At present, the garbage on water cannot be naturally diluted and degraded, and can only be cleaned in an interception and salvage mode. And utilize unmanned ship clearance rubbish on water to salvage more high-efficient convenient than the manual work, help liberation labour. At present, various companies produce a plurality of special unmanned ships for cleaning water, but the unmanned ships are mostly used for large lakes, and the cleaning of small and medium size ponds is seldom considered. In addition, the unmanned ships mostly have the functions of collecting garbage, cleaning aquatic weeds and the like, but the cleaning of plankton (such as algae) in water is rarely considered.
Therefore, it is necessary to design a small-sized unmanned surface-cleaning boat which is suitable for small and medium-sized ponds and can efficiently clean floating organisms on water, and a control method and a positioning method thereof.
For example, chinese patent application No. CN202010247698.2 describes a device, a system, and a method for cleaning a water area, which includes a shore-based monitoring system, a plurality of unmanned boats, and a salvage net, wherein the shore-based monitoring system is configured to receive image information and position information transmitted by the unmanned boats, identify polluted area information of a water surface in front of the unmanned boats through an image fusion technology and a target identification technology, and control the unmanned boats to clean the polluted area; the unmanned ship comprises a ship body, and a control module, a communication module, a power module, a positioning module and a monitoring module which are arranged on the ship body. Although the multiple unmanned boats are used for cleaning in coordination, the cleaning efficiency can be improved; the fishing net has the advantages that through the special structural design of the fishing net, the garbage on the water surface can be cleaned in all directions, and the omission of the garbage is avoided; the shore-based monitoring system sends course instructions according to image information sent by the unmanned boats, so that the automation degree of the sewage disposal system is improved, manual operation is not needed, manpower and material resources are saved, but the shore-based monitoring system has the defects that the unmanned boats are needed to realize all-dimensional cleaning of water surface garbage, the cost is overhigh, and the whole system is not suitable for small and medium size water tanks.
Disclosure of Invention
The invention provides a small-sized unmanned water surface cleaning boat which can be suitable for small and medium-sized water tanks and can efficiently clean water plankton, a control method and a positioning method thereof, and aims to solve the problems that the existing unmanned water surface cleaning boat is not suitable for small and medium-sized water tanks and cannot efficiently clean the water plankton.
In order to achieve the purpose, the invention adopts the following technical scheme:
unmanned ship of small-size surface of water decontamination includes:
a waste collection bin for collecting waste; the garbage collection cabin is positioned in the center of the ship body; the garbage collection cabin comprises a solid garbage collection cabin and an algae garbage treatment cabin;
sensing means for sensing a position of the user; the sensing device is positioned on the upper layer of the ship head; the sensing device comprises a depth camera, a laser radar and a UWB positioning tag;
a control device for controlling the movement of the hull; the control device is positioned at the lower layer of the bow; the control device comprises a motion controller and a main controller; the motion controller is used for detecting a course line, a navigational speed and a ship body inclination angle and driving the motor; the main controller is used for connecting the depth camera, the laser radar and the UWB positioning tag and is used for processing information and transmitting the information;
a propulsion device for providing power; the propulsion device comprises a plurality of underwater propellers positioned at the stern and is used for taking charge of the power of advancing and steering the ship body.
Preferably, the small unmanned surface vehicle further comprises a UWB positioning system; the UWB positioning system comprises a micro-control module and an upper computer positioning module; the micro-control module comprises at least four base stations; the position of each base station is fixed, and the distance between each base station does not exceed 200 meters; the upper computer positioning module is used for setting a cruise track.
Preferably, the base station and the UWB positioning tag each comprise an antenna, a circuit board electrically connected to the antenna, a microcontroller disposed on the circuit board, a UWB positioning module, and a clock module.
Preferably, the device further comprises a movable cabin door; the movable cabin door is positioned on the front side of the garbage collection cabin.
Preferably, a separation net is arranged between the solid garbage collection cabin and the algae garbage treatment cabin; and a microfiltration membrane is arranged at the water outlet of the algae garbage treatment cabin.
The invention also provides a control method of the small unmanned surface vehicle for water surface decontamination, wherein the motion controller adopts an LQR controller; also comprises the following steps:
full-drive control:
s1, when the unmanned ship sails in a limited water area, the LQR controller generates a stable rudder angle to counteract the shore suction force and the shore suction torque;
s2, setting the system state equation as
Wherein x is a state vector of the system; u is a control vector; d is an interference matrix of the system; a is a control object matrix derived from a system motion equation; b is a control matrix;
setting a cost function
Wherein x is a state vector of the system, u is a control vector, Q is a state weight matrix, and R is a control weight matrix;
substituting u as-Kx into the cost function to obtain
Setting the existence of the constant matrix P such that
To minimize J, optimal control is obtained from the minimum principle as: k ═ R -1 B T P；
Adding a state variable y into a system state equation to enable y to be equal to an instantaneous heading offset angle and used for eliminating steady-state lateral drift errors;
s3, according to the formula of the shore suction force, consulting the database of parameters in the formula to obtain the magnitude of the shore suction force; the formula of the shore suction force is as follows:
wherein L is the length of the ship, B is the width of the ship, T is the draft, ρ is the fluid density, V is the navigational speed, F is the hydrodynamic force experienced by the hull, M is the hydrodynamic moment experienced by the hull, C F As a transverse force, C M Is the yaw moment;
s4, estimating a state variable y (t) according to the magnitude of the shore suction force, sensing the heading of the ship body according to the imu sensor and feeding back and adjusting the numerical value of the state variable to enable the integral of the state variable to the time to be a drift error,
error of the measurement
t 0 To start time, t 1 Is the current time;
s5, continuously adjusting the heading of the unmanned ship through the state variable to enable the unmanned ship to return to the set route;
under-actuated control:
s6, defining a asymptotically stable plane S according to the state variables in the full-drive control, and defining a formal control law u;
s7, all system trajectories starting from the plane S are continuously kept on the plane S and are slid on the plane S until all system trajectories converge at a point on the plane S; if there are system trajectories that do not start on the plane S, the control law u is modified until all system trajectories converge on the plane S in a finite time.
The invention also provides a positioning method of the small unmanned surface vehicle for water surface decontamination, which comprises the following steps:
s101, before ranging, the UWB positioning tag firstly communicates with each base station once, and informs that a ranging pulse signal is sent at t0 in advance;
s102, the base station communicates with the received time information, starts to wait, prepares to receive a time sequence of the ranging pulse signal at the time t0, and estimates the sampling time delay of the ranging pulse signal by calculation;
s103, after capturing the ranging pulse signal, the base station performs multiple times of calculation by taking sampling point time as an interval on the basis of rough estimation of the position of the unmanned ship to obtain the time offset of the sampling point, and finally, the t1 of the first arrival moment of the ranging pulse signal is calculated according to sampling time delay estimation;
and S104, obtaining the transmission time of the ranging pulse signal according to t1-t0, and multiplying the obtained transmission time by the propagation speed of the electromagnetic wave to obtain the distance between the base station and the UWB positioning tag.
Preferably, the number of the UWB positioning tags is 3; 3 UWB positioning tags are respectively placed at the bow, the left rear and the right rear of the unmanned ship; also comprises the following steps:
s105, obtaining the course of the unmanned ship according to the relative positions of the 3 UWB positioning tags and an imu sensor arranged in the motion controller;
s106, when the course obtained according to the relative positions of the UWB positioning tags is inconsistent with the course measured by the imu sensor, the maximum probability position of each UWB positioning tag is obtained by utilizing the maximum likelihood estimation to reversely deduce the probability density of 3 UWB positioning tag positions through the course measured by the imu sensor, and the UWB positioning tag positions are corrected;
s107, the specific range of the unmanned ship is determined through the 3 UWB positioning tags, and information is transmitted to the main controller and the upper computer positioning module.
Compared with the prior art, the invention has the beneficial effects that: (1) the invention is suitable for small and medium-sized pools, and can increase the function of cleaning tiny plankton (such as algae) on the water surface; (2) the invention is provided with a high-precision UWB positioning system, can provide a centimeter-level precise position of the unmanned boat for the controller, and sets the cruising track of the unmanned boat through an upper computer software module; (3) the track tracking in the invention adopts a method of full-drive and under-drive, the unmanned ship utilizes a linear quadratic optimal control algorithm with integral feedback to complete full-drive control, the under-drive control is realized through sliding mode control, and pollutants along the way can be collected by a pollutant cleaning device when the unmanned ship sails.
Drawings
Fig. 1 is a front view of the small unmanned surface vehicle for cleaning water;
FIG. 2 is a left side view of the small unmanned surface vehicle for water surface cleaning according to the present invention;
FIG. 3 is a top view of the small unmanned surface vehicle;
FIG. 4 is a schematic diagram of a base station and UWB positioning tag according to the invention;
fig. 5 is a schematic positioning diagram of the positioning method of the small water surface sewage disposal unmanned ship in the invention.
In the figure: the device comprises a solid garbage collection cabin 1, an algae garbage processing cabin 2, a sensing device 3, a control device 4, a propeller 5, a water pump 6, an antenna 7, a circuit board 8, a microcontroller 9, a UWB positioning module 10, a clock module 11 and a movable cabin door 12.
Detailed Description
In order to more clearly illustrate the embodiments of the present invention, the following description will explain the embodiments of the present invention with reference to the accompanying drawings. It is obvious that the drawings in the following description are only some examples of the invention, and that for a person skilled in the art, other drawings and embodiments can be derived from them without inventive effort.
Example 1:
the small unmanned surface vehicle as shown in fig. 1 to 3 includes:
a waste collection bin for collecting waste; the garbage collection cabin is positioned in the center of the ship body; the garbage collection cabin comprises a solid garbage collection cabin 1 and an algae garbage treatment cabin 2;
a sensing device 3 for sensing the position of itself; the sensing device is positioned on the upper layer of the ship head; the sensing device comprises a depth camera, a laser radar and a UWB positioning tag;
a control device 4 for controlling the movement of the hull; the control device is positioned at the lower layer of the bow; the control device comprises a motion controller and a main controller; the motion controller is used for detecting a course line, a navigational speed and a ship body inclination angle and driving the motor; the main controller is used for connecting the depth camera, the laser radar and the UWB positioning tag and is used for processing information and transmitting the information;
a propulsion device for providing power; the propulsion device comprises a plurality of underwater propellers 5 positioned at the stern and is used for taking charge of the power of advancing and steering the ship body.
Still be equipped with the water pump 6 that is used for drawing water in the unmanned ship of small-size surface of water decontamination, the power supply part of unmanned ship adopts the 6S lithium cell, can provide water pump, advancing device and the required electric power of controlling means simultaneously.
Furthermore, the small unmanned surface vehicle for cleaning water also comprises a UWB positioning system; the UWB positioning system comprises a micro-control module and an upper computer positioning module; the micro control module comprises four base stations; the position of each base station is fixed, and the distance between each base station does not exceed 200 meters; the upper computer positioning module is used for setting a cruise track.
The main functions realized by the upper computer positioning module are as follows:
1. the UWB positioning tag is connected with the UWB positioning tag through Bluetooth;
2. reading a ranging result from the UWB positioning tag;
3. setting the actual placing positions of all base stations in a plan view;
4. displaying the distance between the UWB positioning tag and the base station, calculating the position (XYZ coordinates) of the UWB positioning tag, and displaying the position on a map in real time;
5. map display, which can import a map in PNG format and set the ratio of pixel value to actual distance and coordinate position;
6. and arranging the cruise track of the unmanned boat on a map.
Further, as shown in fig. 4, the base station and the UWB positioning tag each include an antenna 7, a circuit board 8 electrically connected to the antenna, a microcontroller 9 disposed on the circuit board, a UWB positioning module 10, and a clock module 11. The UWB positioning tag is carried by a positioning target (unmanned ship), the distance between the UWB positioning tag and the base station is not more than two hundred meters, positioning is realized through ranging with each base station, and ranging results are gathered on the host station and then transmitted to a PC (personal computer) through the Bluetooth module.
Further, the small unmanned surface vehicle for cleaning water also comprises a movable cabin door 12; the movable cabin door is positioned on the front side of the garbage collection cabin.
Furthermore, a separation net is arranged between the solid garbage collection cabin and the algae garbage treatment cabin; and a microfiltration membrane is arranged at the water outlet of the algae garbage treatment cabin.
Based on the embodiment 1, the invention also provides a control method of the small water surface pollution cleaning unmanned ship, wherein the motion controller adopts an LQR controller; also comprises the following steps:
full-drive control:
s1, when the unmanned ship sails in a limited water area, the LQR controller generates a stable rudder angle to counteract the shore suction force and the shore suction torque;
s2, setting the system state equation as
Wherein x is a state vector of the system; u is a control vector; d is an interference matrix of the system; a is a control object matrix derived from a system motion equation; b is a control matrix;
setting a cost function
Wherein x is a state vector of the system, u is a control vector, Q is a state weight matrix, and R is a control weight matrix;
substituting u as-Kx into the cost function to obtain
Setting the existence of the constant matrix P such that
To minimize J, optimal control is obtained from the minimum principle as: k ═ R -1 B T P；
Adding a state variable y into a system state equation to enable y to be equal to an instantaneous heading offset angle and used for eliminating steady-state lateral drift errors;
s3, according to the formula of the shore suction force, consulting the database of parameters in the formula to obtain the magnitude of the shore suction force; the formula of the shore suction force is as follows:
wherein L is the length of the ship, B is the width of the ship, T is the draft, ρ is the fluid density, V is the navigational speed, F is the hydrodynamic force experienced by the hull, M is the hydrodynamic moment experienced by the hull, C F As a transverse force, C M Is the yaw moment;
s4, estimating a state variable y (t) according to the magnitude of the shore suction force, sensing the heading of the ship body according to the imu sensor and feeding back and adjusting the numerical value of the state variable to enable the integral of the state variable to the time to be a drift error,
error of the measurement
t 0 To start toTime, t 1 Is the current time;
s5, continuously adjusting the heading of the unmanned ship through the state variable to enable the unmanned ship to return to the set route;
the full-drive control method adopts a linear quadratic form optimal control LQR method with integral feedback.
The LQR controller can effectively correct and control the movement track of the ship through rudder steering and speed reduction, keeps the stability of the ship route, and provides a good control strategy for reducing the risk of mutual collision of the ship.
The LQR controller controls an object to be a state space form linear system, an objective function is a quadratic function of object state and control input, and time domain rudder angle change meeting an optimal control rule is obtained by calculating a gain matrix enabling the objective function value to be minimum, so that optimal control of ship motion is achieved. When the ship sails by deviating to one side of the water channel and approaching the shore wall, the ship body is subjected to a transverse force which is absorbed to one side of the near shore, namely the effect of shore absorption, and the phenomenon of shore absorption of the ship body to the shore wall occurs. Meanwhile, the hull is subjected to the action of shoreside push moment, so that the bow turns to the center of the channel, namely the shoreside push phenomenon. When the ship sails in a limited water area, the LQR controller generates a stable rudder angle to counteract the shore suction force and the shore suction torque. When the unmanned ship reaches a stable state, the ship body generates a certain amount of transverse movement, and then the unmanned ship keeps the original fixed course to continue navigation, but deviates from the original fixed course, so that the course stability is ensured, but the stability of the course cannot be ensured. To eliminate this steady state lateral drift error, a state variable is added to the state space equation. And looking up a database of related parameters according to a formula of the shore suction force to obtain the magnitude of the shore suction force. And setting a state variable according to the magnitude of the shore suction force, and sensing the heading feedback of the ship body according to a built-in imu sensor of the controller to adjust the state scalar numerical value. And the unmanned ship continuously adjusts the course through the state variable to enable the unmanned ship to return to the set route.
And calculating the numerical value of the flight path deviated from the set air route by integrating the attitude and the flight speed sensed by the imu sensor, judging whether the unmanned ship returns to the set air route or not and feeding back the position deviated from the set air route.
Under-actuated control:
s6, defining a asymptotically stable plane S according to the state variables in the full-drive control, and defining a formal control law u;
s7, all system trajectories starting from the plane S are continuously kept on the plane S and are slid on the plane S until all system trajectories converge at a point on the plane S; if there are system trajectories that do not start on the plane S, the control law u is modified until all system trajectories converge on the plane S in a finite time.
In order to enable the motion of the unmanned ship to be related to a time sequence, a sliding mode control is selected as an under-actuated control method.
In the sliding mode control method, an asymptotically stable plane S is defined according to the state variables of the system, and a formal control law u is defined. All system trajectories that start at this plane continue to remain on this plane and slide on this plane until they slide to the desired destination at the intersection. If there is a system trajectory that does not start at this plane, the control laws need to be modified so that the trajectory converges to a point on this plane for a finite period of time. The design of the sliding mode control law can be divided into two parts: designing a proper sliding surface to limit the system dynamic to a sliding manifold to generate the expected behavior; a continuous control law is designed which forces the system to track and remain on the sliding surface.
The invention designs a trajectory tracking sliding mode control law for a water surface unmanned ship system, and calculates the thrust of two propellers by using two sliding planes.
The first sliding plane is a first order plane defined by the tracking error in terms of longitudinal motion.
The second sliding plane is a second order plane defined by the tracking error in terms of lateral motion.
In the system, only the absolute position and heading angle of the ship can be measured and fed back. Therefore, the absolute speed of the ship can only be estimated by a mathematical method, and the surging and swaying speeds are calculated through the kinematic relation between the inertial reference system and the ship-following coordinate system.
And (3) considering an impedance force model in a power law form, and designing sliding mode control to track a continuous differentiable target track. This trajectory is defined in terms of global positioning variables x and y in two planes using a set of two ordinary differential equations.
The first sliding plane is an exponentially stable first-order plane defined according to the tracking error of the longitudinal motion of the ship;
the second slip plane is a second-order exponentially stable plane defined by the vessel lateral motion tracking error.
As shown in fig. 5, the invention also provides a positioning method of the small unmanned surface vehicle for water surface decontamination, and a positioning system consists of four base stations and tags carried by the unmanned surface vehicle. Each base station and the label play the roles of sending and receiving signals, and the communication range is within 200 meters. The first base station is used as a main base station and can communicate with an upper computer through Bluetooth. The tags may be distributed anywhere within the location area.
The positioning system is prepared in the early stage as follows:
1. firstly, drawing a working scene map of the unmanned ship, selecting the number of base stations (4 in fig. 5) according to the size of the map, and selecting the placement positions of the base stations. And ensuring that any position in the working scene is covered by all base stations, wherein the more base stations are covered, the higher the positioning accuracy is.
2. And uploading the drawn map to an upper computer software module, setting the ratio of the actual size of the map to the pixel value, and marking the coordinate values of each base station according to the specific position and the north-south orientation.
3. And placing the base stations in the working area according to the map setting, and placing the unmanned ship in the positioning range.
The positioning scheme flow is as follows:
the base station and the tag realize UWB communication through pulse radio, and transmit signals by using a time domain pulse sequence formed by single pulse signals. In order to improve the transmission capability and the resolution capability of signals, a wide stop band of the signals is realized through the multi-mode resonant filter.
The positioning method adopts a TOA method, and specifically comprises the following steps:
s101, before ranging, the UWB positioning tag firstly communicates with each base station once, and informs that a ranging pulse signal is sent at t0 in advance;
s102, the base station communicates with the received time information, starts to wait, prepares to receive a time sequence of the ranging pulse signal at the time t0, and estimates the sampling time delay of the ranging pulse signal by calculation;
s103, after capturing the ranging pulse signal, the base station performs multiple times of calculation by taking sampling point time as an interval on the basis of rough estimation of the position of the unmanned ship to obtain the time offset of the sampling point, and finally, the t1 of the first arrival moment of the ranging pulse signal is calculated according to sampling time delay estimation;
and S104, obtaining the transmission time of the ranging pulse signal according to t1-t0, and multiplying the obtained transmission time by the propagation speed of the electromagnetic wave to obtain the distance between the base station and the UWB positioning tag.
And after the distance is measured, calculating the position of the label by adopting a maximum likelihood estimation method. The label coordinates are usually the intersection points of circles with the coordinates of each base station as the center of the circle and the distance from the base station to the label as the radius. In actual measurement, the tag position may not be at the intersection of circles centered on the respective base station positions due to measurement errors of the distances. Therefore, the approximate normal distribution of the error margin of the ranging structure is obtained by carrying out statistical fitting on the error margin by using a maximum likelihood estimation method, and then the position of the label is obtained by using a maximum likelihood estimation principle. And obtaining an estimation result, and transmitting information to the unmanned ship controller and the upper computer after passing through the Kalman filter.
Further, the number of the UWB positioning tags is 3; 3 UWB positioning tags are respectively placed at the bow, the left rear and the right rear of the unmanned ship; also comprises the following steps:
s105, obtaining the course of the unmanned ship according to the relative positions of the 3 UWB positioning tags and an imu sensor arranged in the motion controller;
s106, when the course obtained according to the relative positions of the UWB positioning tags is inconsistent with the course measured by the imu sensor, the maximum probability position of each UWB positioning tag is obtained by utilizing the maximum likelihood estimation to reversely deduce the probability density of 3 UWB positioning tag positions through the course measured by the imu sensor, and the UWB positioning tag positions are corrected;
s107, the specific range of the unmanned ship is determined through the 3 UWB positioning tags, and information is transmitted to the main controller and the upper computer positioning module.
Unmanned ship carries 3 labels for avoid range error to influence positioning accuracy.
The garbage collection method specifically comprises the following steps:
the front side of the unmanned ship is provided with a movable cabin door, and when the unmanned ship works, the two cabin doors are opened to form 135-degree angle and are opened, so that more garbage is guided into the garbage collection cabin. When the unmanned ship advances, garbage floating on the water surface on the path is collected into the cabin. In addition, a water pump at the stern pumps water, and the pumped water drives more garbage to enter the cabin.
The garbage collection cabin in the cabin is divided into two parts by a separation net, the front end is a solid garbage collection part, and the rear end is an algae pollutant treatment part. After the water flow carries the floating garbage to flow into the garbage collection chamber together, the larger solid garbage can be blocked at the solid garbage collection position due to the blocking of the separation net, and the smaller garbage such as algae enters the algae pollutant treatment position through the separation net. The tail water outlet of the ship is provided with a microfiltration membrane, water flow is sucked out of the cabin by a water pump, and tiny garbage is left in the cabin by a membrane filtration method. The membrane filtration method uses a selective permeation membrane with a micro-aperture level as a separation medium and uses pressure difference and concentration difference as driving, so that substances in a water flow selectively permeate the membrane to realize separation or purification. In addition, substances capable of adsorbing and decomposing algae, such as activated carbon, biological ceramsite, biocatalyst IBC and the like, are placed in the algae garbage cleaning position, the pressure of the microfiltration net is reduced, and the situation that the microfiltration net is blocked due to aggregation of a large amount of tiny garbage is avoided.
The invention is suitable for small and medium-sized pools, and can increase the function of cleaning tiny plankton (such as algae) on the water surface; the invention is provided with a high-precision UWB positioning system, can provide a centimeter-level precise position of the unmanned boat for the controller, and sets the cruising track of the unmanned boat through an upper computer software module; the track tracking in the invention adopts a method of full-drive and under-drive, the unmanned ship utilizes a linear quadratic optimal control algorithm with integral feedback to complete full-drive control, the under-drive control is realized through sliding mode control, and pollutants along the way can be collected by a pollutant cleaning device when the unmanned ship sails.
The foregoing has outlined rather broadly the preferred embodiments and principles of the present invention and it will be appreciated that those skilled in the art may devise variations of the present invention that are within the spirit and scope of the appended claims.
Claims (8)
1. Unmanned ship of small-size surface of water cleaning, its characterized in that includes:
a waste collection bin for collecting waste; the garbage collection cabin is positioned in the center of the ship body; the garbage collection cabin comprises a solid garbage collection cabin and an algae garbage treatment cabin;
sensing means for sensing a position of the user; the sensing device is positioned on the upper layer of the ship head; the sensing device comprises a depth camera, a laser radar and a UWB positioning tag;
a control device for controlling the movement of the hull; the control device is positioned at the lower layer of the bow; the control device comprises a motion controller and a main controller; the motion controller is used for detecting a course line, a navigational speed and a ship body inclination angle and driving the motor; the main controller is used for connecting the depth camera, the laser radar and the UWB positioning tag and is used for processing information and transmitting the information;
a propulsion device for providing power; the propulsion device comprises a plurality of underwater propellers positioned at the stern and is used for taking charge of the power of advancing and steering the ship body.
2. The small unmanned surface vehicle of claim 1, further comprising a UWB positioning system; the UWB positioning system comprises a micro-control module and an upper computer positioning module; the micro-control module comprises at least four base stations; the position of each base station is fixed, and the distance between each base station does not exceed 200 meters; the upper computer positioning module is used for setting a cruise track.
3. The small unmanned surface-cleaning boat of claim 2, wherein the base station and the UWB positioning tag each comprise an antenna, a circuit board electrically connected to the antenna, a microcontroller disposed on the circuit board, a UWB positioning module, and a clock module.
4. The small unmanned surface vehicle of claim 1, further comprising a mobile hatch; the movable cabin door is positioned on the front side of the garbage collection cabin.
5. The small unmanned surface vehicle as claimed in any one of claims 1 to 4, wherein a separation net is arranged between the solid waste collection chamber and the algae waste treatment chamber; and a microfiltration membrane is arranged at the water outlet of the algae garbage treatment cabin.
6. The control method of the small unmanned surface vehicle for water surface cleaning is characterized in that the motion controller is an LQR controller; also comprises the following steps:
full-drive control:
s1, when the unmanned ship sails in a limited water area, the LQR controller generates a stable rudder angle to counteract the shore suction force and the shore suction torque;
s2, setting the system state equation as
Wherein x is a state vector of the system; u is a control vector; d is an interference matrix of the system; a is a control object matrix derived from a system motion equation; b is a control matrix;
setting a cost function
Wherein x is a state vector of the system, u is a control vector, Q is a state weight matrix, and R is a control weight matrix;
substituting u as-Kx into the cost function to obtain
Setting the existence of the constant matrix P such that
To minimize J, optimal control is obtained from the minimum principle as: r is -1 B T P；
Adding a state variable y into a system state equation to enable y to be equal to an instantaneous heading offset angle and used for eliminating steady-state lateral drift errors;
s3, according to the formula of the shore suction force, consulting the database of parameters in the formula to obtain the magnitude of the shore suction force; the formula of the shore suction force is as follows:
wherein L is the length of the ship, B is the width of the ship, T is the draft, ρ is the fluid density, V is the navigational speed, F is the hydrodynamic force experienced by the hull, M is the hydrodynamic moment experienced by the hull, C F As a transverse force, C M Is the yawing moment;
s4, estimating a state variable y (t) according to the magnitude of the shore suction force, sensing the heading of the ship body according to the imu sensor and feeding back and adjusting the numerical value of the state variable to enable the integral of the state variable to the time to be a drift error,
error of the measurement
t 0 To start time, t 1 Is the current time;
s5, continuously adjusting the heading of the unmanned ship through the state variable to enable the unmanned ship to return to the set route;
under-actuated control:
s6, defining a asymptotically stable plane S according to the state variables in the full-drive control, and defining a formal control law u;
s7, all system trajectories starting from the plane S are continuously kept on the plane S and are slid on the plane S until all system trajectories converge at a point on the plane S; if there are system trajectories that do not start on the plane S, the control law u is modified until all system trajectories converge on the plane S in a finite time.
7. The method for positioning the small unmanned surface vehicle for water surface cleaning according to claim 3, comprising the following steps:
s101, before ranging, the UWB positioning tag firstly communicates with each base station once, and informs that a ranging pulse signal is sent at t0 in advance;
s102, the base station communicates with the received time information, starts to wait, prepares to receive a time sequence of the ranging pulse signal at the time t0, and estimates the sampling time delay of the ranging pulse signal by calculation;
s103, after capturing the ranging pulse signal, the base station performs multiple times of calculation by taking sampling point time as an interval on the basis of rough estimation of the position of the unmanned ship to obtain the time offset of the sampling point, and finally, the t1 of the first arrival moment of the ranging pulse signal is calculated according to sampling time delay estimation;
and S104, obtaining the transmission time of the ranging pulse signal according to t1-t0, and multiplying the obtained transmission time by the propagation speed of the electromagnetic wave to obtain the distance between the base station and the UWB positioning tag.
8. The method for positioning the small unmanned surface vehicle as claimed in claim 7, wherein the number of the UWB positioning tags is 3; 3 UWB positioning tags are respectively placed at the bow, the left rear and the right rear of the unmanned ship; also comprises the following steps:
s105, obtaining the course of the unmanned ship according to the relative positions of the 3 UWB positioning tags and an imu sensor arranged in the motion controller;
s106, when the course obtained according to the relative positions of the UWB positioning tags is inconsistent with the course measured by the imu sensor, the maximum probability position of each UWB positioning tag is obtained by utilizing the maximum likelihood estimation to reversely deduce the probability density of 3 UWB positioning tag positions through the course measured by the imu sensor, and the UWB positioning tag positions are corrected;
s107, the specific range of the unmanned ship is determined through the 3 UWB positioning tags, and information is transmitted to the main controller and the upper computer positioning module.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210396815.0A CN114802616B (en) | 2022-04-15 | 2022-04-15 | Small water surface cleaning unmanned ship, control method and positioning method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210396815.0A CN114802616B (en) | 2022-04-15 | 2022-04-15 | Small water surface cleaning unmanned ship, control method and positioning method thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114802616A true CN114802616A (en) | 2022-07-29 |
| CN114802616B CN114802616B (en) | 2023-08-04 |
Family
ID=82537269
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210396815.0A Active CN114802616B (en) | 2022-04-15 | 2022-04-15 | Small water surface cleaning unmanned ship, control method and positioning method thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114802616B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115792799A (en) * | 2022-11-30 | 2023-03-14 | 上海船舶运输科学研究所有限公司 | Ship model track mapping method and system |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105180942A (en) * | 2015-09-11 | 2015-12-23 | 安科智慧城市技术（中国）有限公司 | Autonomous navigation method and device for unmanned ship |
| CN105852817A (en) * | 2015-01-20 | 2016-08-17 | 中国人民解放军第二军医大学 | Crew maritime intra-cabin positioning, vital sign monitoring and searching-rescuing method |
| CN105898698A (en) * | 2016-04-26 | 2016-08-24 | 上海交通大学 | UWB-based ship berthing assist method and system |
| CN107719593A (en) * | 2017-09-11 | 2018-02-23 | 上海电机学院 | A kind of Water surface refuse collecting boat for possessing floating type salvaging transport structure |
| CN207972758U (en) * | 2018-01-12 | 2018-10-16 | 中国科学院合肥物质科学研究院 | A kind of hybrid power robot for water surface cleaner |
| CN110185013A (en) * | 2019-05-09 | 2019-08-30 | 上海海事大学 | Water surface cleaning ship device and water surface clean method based on Machine Vision Recognition |
| CN111290395A (en) * | 2020-03-10 | 2020-06-16 | 上海航天控制技术研究所 | Unmanned ship autonomous recovery method based on mother-son ships |
| US20200193840A1 (en) * | 2018-12-14 | 2020-06-18 | Brunswick Corporation | Marine Propulsion Control System and Method with Proximity-Based Velocity Limiting |
| CN111452924A (en) * | 2020-03-19 | 2020-07-28 | 哈尔滨工业大学 | Water surface garbage cleaning robot |
| CN112693571A (en) * | 2021-01-26 | 2021-04-23 | 中电建水环境科技有限公司 | Intelligent water surface float-cleaning ship and intelligent water surface float-cleaning method |
-
2022
- 2022-04-15 CN CN202210396815.0A patent/CN114802616B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105852817A (en) * | 2015-01-20 | 2016-08-17 | 中国人民解放军第二军医大学 | Crew maritime intra-cabin positioning, vital sign monitoring and searching-rescuing method |
| CN105180942A (en) * | 2015-09-11 | 2015-12-23 | 安科智慧城市技术（中国）有限公司 | Autonomous navigation method and device for unmanned ship |
| CN105898698A (en) * | 2016-04-26 | 2016-08-24 | 上海交通大学 | UWB-based ship berthing assist method and system |
| CN107719593A (en) * | 2017-09-11 | 2018-02-23 | 上海电机学院 | A kind of Water surface refuse collecting boat for possessing floating type salvaging transport structure |
| CN207972758U (en) * | 2018-01-12 | 2018-10-16 | 中国科学院合肥物质科学研究院 | A kind of hybrid power robot for water surface cleaner |
| US20200193840A1 (en) * | 2018-12-14 | 2020-06-18 | Brunswick Corporation | Marine Propulsion Control System and Method with Proximity-Based Velocity Limiting |
| CN110185013A (en) * | 2019-05-09 | 2019-08-30 | 上海海事大学 | Water surface cleaning ship device and water surface clean method based on Machine Vision Recognition |
| CN111290395A (en) * | 2020-03-10 | 2020-06-16 | 上海航天控制技术研究所 | Unmanned ship autonomous recovery method based on mother-son ships |
| CN111452924A (en) * | 2020-03-19 | 2020-07-28 | 哈尔滨工业大学 | Water surface garbage cleaning robot |
| CN112693571A (en) * | 2021-01-26 | 2021-04-23 | 中电建水环境科技有限公司 | Intelligent water surface float-cleaning ship and intelligent water surface float-cleaning method |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115792799A (en) * | 2022-11-30 | 2023-03-14 | 上海船舶运输科学研究所有限公司 | Ship model track mapping method and system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114802616B (en) | 2023-08-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10589829B2 (en) | Gliding robotic fish navigation and propulsion | |
| CN100443369C (en) | Deep-sea solar diving device | |
| Caccia et al. | Basic navigation, guidance and control of an unmanned surface vehicle | |
| US8418642B2 (en) | Unmanned submersible vehicles and methods for operating the same in a body of liquid | |
| CN113433933B (en) | Autonomous cruising method of intelligent water surface cleaning unmanned ship | |
| Dunbabin et al. | An autonomous surface vehicle for water quality monitoring | |
| CN108312151B (en) | Drift detection underwater robot device and control method | |
| Ferri et al. | The HydroNet ASV, a small-sized autonomous catamaran for real-time monitoring of water quality: From design to missions at sea | |
| Wood et al. | Autonomous underwater gliders | |
| CN112960078A (en) | Unmanned sailing boat automatic driving system and method thereof | |
| Mousazadeh et al. | Experimental evaluation of a hydrography surface vehicle in four navigation modes | |
| CN114802616B (en) | Small water surface cleaning unmanned ship, control method and positioning method thereof | |
| KR101806393B1 (en) | Movable water purification apparatus capable of performing unmanned autonomous voyage | |
| Francis et al. | Development of a highly portable unmanned surface vehicle for surf zone bathymetric surveying | |
| Al Maawali et al. | Design and implementation of an unmanned surface vehicle for oil spill handling | |
| EP3696078B1 (en) | A method and system for piloting an unmanned surface vessel | |
| EP3501966A1 (en) | An unmanned marine surface vessel | |
| Yaakob et al. | Development of unmanned surface vehicle (USV) for sea patrol and environmental monitoring | |
| CN110667790A (en) | Aerodynamic water surface weeding and cleaning equipment and cleaning method | |
| Liu et al. | Unmanned airboat technology and applications in environment and agriculture | |
| CN109263814A (en) | A kind of multi-functional autonomous cruise boat system of Novel surface | |
| CN212082961U (en) | Water quality sampling monitoring system based on unmanned ship | |
| CN111332415A (en) | Autonomous cruise energy-saving water quality monitoring ship and monitoring method | |
| CN211978876U (en) | Unmanned ship for monitoring and tracking oil pollution source in collaborative water environment | |
| Sanfilippo et al. | The aquatic surface robot (AnSweR), a lightweight, low cost, multipurpose unmanned research 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 |