CN114802616B - Small water surface cleaning unmanned ship, control method and positioning method thereof - Google Patents
Small water surface cleaning unmanned ship, control method and positioning method thereof Download PDFInfo
- Publication number
- CN114802616B CN114802616B CN202210396815.0A CN202210396815A CN114802616B CN 114802616 B CN114802616 B CN 114802616B CN 202210396815 A CN202210396815 A CN 202210396815A CN 114802616 B CN114802616 B CN 114802616B
- Authority
- CN
- China
- Prior art keywords
- control
- ship
- garbage collection
- cabin
- unmanned ship
- 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
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 title claims abstract description 68
- 238000000034 method Methods 0.000 title claims abstract description 41
- 238000004140 cleaning Methods 0.000 title claims abstract description 38
- 239000010813 municipal solid waste Substances 0.000 claims abstract description 59
- 230000033001 locomotion Effects 0.000 claims abstract description 27
- 241000195493 Cryptophyta Species 0.000 claims abstract description 19
- 239000007787 solid Substances 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 19
- 239000012528 membrane Substances 0.000 claims description 6
- 238000000926 separation method Methods 0.000 claims description 5
- 238000001471 micro-filtration Methods 0.000 claims description 4
- 239000012530 fluid Substances 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000012986 modification Methods 0.000 claims 1
- 230000004048 modification Effects 0.000 claims 1
- 238000005070 sampling Methods 0.000 description 8
- 239000003344 environmental pollutant Substances 0.000 description 6
- 231100000719 pollutant Toxicity 0.000 description 6
- 238000007476 Maximum Likelihood Methods 0.000 description 5
- 230000005540 biological transmission Effects 0.000 description 5
- 238000004891 communication Methods 0.000 description 5
- 238000013461 design Methods 0.000 description 4
- 238000005259 measurement Methods 0.000 description 4
- 238000012544 monitoring process Methods 0.000 description 4
- OKTJSMMVPCPJKN-UHFFFAOYSA-N Carbon Chemical compound [C] OKTJSMMVPCPJKN-UHFFFAOYSA-N 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
- 238000004364 calculation method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000007667 floating Methods 0.000 description 2
- 238000005374 membrane filtration Methods 0.000 description 2
- 239000000126 substance Substances 0.000 description 2
- 238000003911 water pollution Methods 0.000 description 2
- 241000196324 Embryophyta Species 0.000 description 1
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 239000003054 catalyst Substances 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000005202 decontamination Methods 0.000 description 1
- 230000003588 decontaminative effect Effects 0.000 description 1
- 230000004927 fusion 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
- 238000005192 partition Methods 0.000 description 1
- 239000012466 permeate Substances 0.000 description 1
- 239000011148 porous material Substances 0.000 description 1
- 238000005086 pumping Methods 0.000 description 1
- 238000000746 purification Methods 0.000 description 1
- 238000012887 quadratic function Methods 0.000 description 1
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)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Chemical & Material Sciences (AREA)
- Mechanical Engineering (AREA)
- Ocean & Marine Engineering (AREA)
- Aviation & Aerospace Engineering (AREA)
- Combustion & Propulsion (AREA)
- Health & Medical Sciences (AREA)
- Public Health (AREA)
- Environmental & Geological Engineering (AREA)
- Automation & Control Theory (AREA)
- Probability & Statistics with Applications (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 water surface cleaning unmanned ship, a control method and a positioning method thereof. The unmanned ship of small-size surface of water decontaminates includes: a garbage collection bin for garbage collection; the garbage collection cabin comprises a solid garbage collection cabin and an algae garbage treatment cabin; sensing means for sensing the position of the sensor; the sensing device comprises a depth camera, a laser radar and a UWB positioning tag; control means for controlling the movement of the hull; the control device comprises a motion controller and a main controller; propulsion means 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 forward steering of the ship body. The invention is suitable for small and medium-sized water tanks and can efficiently clean water plankton.
Description
Technical Field
The invention belongs to the technical field of water area environment protection, and particularly relates to a small water surface cleaning unmanned ship, a control method and a positioning method thereof.
Background
At present, the water garbage can not be diluted and degraded naturally, but can be cleaned in a mode of intercepting salvage. The unmanned ship is utilized to clean the garbage on the water, so that the garbage on the water is more efficient and convenient than manual salvage, and the labor force is liberated. At present, various companies produce special unmanned water pollution cleaning ships, but the unmanned water pollution cleaning ships are mostly used for large lakes, and small and medium-sized pools are rarely considered to be cleaned. In addition, these unmanned vessels often have the function of collecting garbage, cleaning aquatic weeds, etc., but rarely consider the cleaning of plankton (e.g., algae) in water.
Therefore, it is necessary to design a small water surface cleaning unmanned ship which is suitable for small and medium-sized water ponds and can efficiently clean water plankton, and a control method and a positioning method thereof.
For example, a water area decontamination device, system and method described in chinese patent document with application number CN202010247698.2 includes a shore-based monitoring system, a plurality of unmanned boats and a salvage net, wherein the shore-based monitoring system is used for receiving image information and position information transmitted by the unmanned boats, identifying the information of a polluted area on the water surface in front of the unmanned boats through an image fusion technology and a target recognition technology, and controlling the unmanned boats to decontaminate 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 plurality of unmanned boats are used for cooperatively cleaning, the cleaning efficiency can be improved; through the special structural design of the salvaging net, the all-round cleaning of the garbage on the water surface can be realized, and the omission of the garbage is avoided; the shore-passing-base monitoring system sends a course instruction according to image information sent by the unmanned boats, so that the automation degree of the trash cleaning system is improved, manual operation is not needed, and manpower and material resources are saved.
Disclosure of Invention
The invention provides a small water surface cleaning unmanned ship which is applicable to 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 water surface cleaning unmanned ship is not applicable to small and medium-sized water tanks and cannot efficiently clean water plankton in the prior art.
In order to achieve the aim of the invention, the invention adopts the following technical scheme:
small-size surface of water unmanned ship of decontaminating includes:
a garbage collection bin for garbage collection; 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 the position of the sensor; 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;
control means 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 the route, the speed and the inclination angle of the ship body 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 information;
propulsion means 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 forward steering of the ship body.
Preferably, the small water surface cleaning unmanned ship 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 is not more than 200 meters; the upper computer positioning module is used for setting a cruising track.
Preferably, the base station and the UWB positioning tag each comprise an antenna, a circuit board electrically connected with the antenna, a microcontroller arranged on the circuit board, a UWB positioning module and a clock module.
Preferably, the movable cabin door is also included; the movable cabin door is positioned at 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 water surface cleaning unmanned ship, wherein the motion controller adopts an LQR controller; the method also comprises the following steps:
full drive control:
s1, when an unmanned ship sails in a limited water area, the LQR controller generates a stable rudder angle to offset the suction force and the suction moment;
s2, setting a 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= -Kx into the cost function to obtain
Setting the presence of the constant matrix P such that
To minimize J, the optimal control is obtained by the minimum principle: 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 course offset angle, and eliminating steady-state transverse drift errors;
s3, according to a formula of the shore suction, referring to a database of parameters in the formula to obtain the magnitude of the shore suction; 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 speed, F is the hydrodynamic force received by the hull, M is the hydrodynamic moment received by the hull, C F Is a transverse force, C M The first 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 an imu sensor, feeding back and adjusting the numerical value of the state variable to enable the integral of the state variable to be drift error,
error oft 0 To start time, t 1 Is the current time;
s5, the unmanned ship continuously adjusts the heading through the state variable, so that the unmanned ship returns to a set route;
under-actuated control:
s6, defining an asymptotically stable plane S according to the state variables in the full-drive control, and defining a formal control law u;
s7, continuously keeping all system trajectories starting at the plane S on the plane S and sliding on the plane S until all system trajectories meet 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 a point on the plane S for a finite time.
The invention also provides a positioning method of the small water surface cleaning unmanned ship, which comprises the following steps:
s101, before ranging, UWB positioning labels communicate with each base station once, and inform in advance that ranging pulse signals are to be sent out at the time t 0;
s102, the base station receives time information in a communication mode, starts waiting, prepares to receive a time sequence of a ranging pulse signal at a time t0, and estimates sampling time delay of the ranging pulse signal through calculation;
s103, after capturing the ranging pulse signals, the base station calculates for a plurality of times by taking the time of the sampling point as an interval on the basis of roughly estimating the position of the unmanned ship to obtain the time offset of the sampling point, and finally calculates t1 of the first arrival time of the ranging pulse signals according to the sampling time delay estimation;
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; the 3 UWB positioning labels are respectively placed at the bow, the left rear and the right rear of the unmanned ship; the method also comprises the following steps:
s105, according to the relative positions of the 3 UWB positioning labels and an imu sensor arranged in the motion controller, obtaining the heading of the unmanned aerial vehicle;
s106, when the heading obtained according to the relative positions of the UWB positioning labels is inconsistent with the heading measured by the imu sensor, estimating the probability density of the positions of the 3 UWB positioning labels by utilizing maximum likelihood, obtaining the maximum probability position of each UWB positioning label, and correcting the position of the UWB positioning label;
s107, determining the specific range of the unmanned ship through 3 UWB positioning tags, and transmitting information to a main controller and an upper computer positioning module.
Compared with the prior art, the invention has the beneficial effects that: (1) The invention is suitable for medium-small water tanks, and can increase the function of cleaning micro plankton (such as algae) on the water surface; (2) The unmanned aerial vehicle cruise control system is provided with a high-precision UWB positioning system, can provide a centimeter-level accurate position for the unmanned aerial vehicle for the controller, and sets a cruise track of the unmanned aerial vehicle through an upper computer software module; (3) The track tracking in the invention adopts a full-driving underactuated method, the unmanned ship completes full-driving control by using a linear quadratic optimal control algorithm with integral feedback, the underactuated control is realized by sliding film control, and the unmanned ship can collect pollutants along the way by using a pollutant cleaning device during navigation.
Drawings
FIG. 1 is a front view of a small and medium sized unmanned surface vehicle for cleaning water;
FIG. 2 is a left side view of the small and medium sized unmanned surface vehicle for cleaning water according to the present invention;
FIG. 3 is a top view of a small and medium sized unmanned surface vehicle for cleaning water according to the present invention;
FIG. 4 is a schematic diagram of a base station and UWB positioning tag according to the present invention;
fig. 5 is a schematic diagram of a positioning method of a small and medium-sized unmanned surface cleaning boat according to the present invention.
In the figure: the device comprises a solid garbage collection cabin 1, an algae garbage treatment 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, specific embodiments of the present invention will be described below with reference to the accompanying drawings. It is evident that the drawings in the following description are only examples of the invention, from which other drawings and other embodiments can be obtained by a person skilled in the art without inventive effort.
Example 1:
a small water surface cleaning unmanned boat as shown in fig. 1 to 3, comprising:
a garbage collection bin for garbage collection; 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;
sensing means 3 for sensing the position of the sensor; 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 the route, the speed and the inclination angle of the ship body 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 information;
propulsion means for providing power; the propulsion device comprises a plurality of underwater propellers 5 positioned at the stern for the power responsible for the forward steering of the hull.
The water pump 6 for pumping water is further arranged in the small water surface cleaning unmanned ship, and a 6S lithium battery is adopted in a power supply part of the unmanned ship, so that power required by the water pump, the propulsion device and the control device can be simultaneously provided.
Furthermore, the small water surface cleaning unmanned ship 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 is not more than 200 meters; the upper computer positioning module is used for setting a cruising track.
The main functions of the upper computer positioning module are as follows:
1. the device is connected with the UWB positioning tag through Bluetooth;
2. reading a ranging result from the UWB positioning tag;
3. setting actual placement 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 lead in a PNG format map, and can set the ratio of pixel value to actual distance and coordinate position;
6. and setting an unmanned ship cruising track on the 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 provided 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 a base station is not more than two hundred meters, positioning is achieved through distance measurement between the UWB positioning tag and each base station, and each distance measurement result is summarized to a host station and then transmitted to a PC (personal computer) through a Bluetooth module.
Further, the small water surface cleaning unmanned ship also comprises a movable cabin door 12; the movable cabin door is positioned at the front side of the garbage collection cabin.
Further, 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 cleaning unmanned ship, wherein the motion controller adopts an LQR controller; the method also comprises the following steps:
full drive control:
s1, when an unmanned ship sails in a limited water area, the LQR controller generates a stable rudder angle to offset the suction force and the suction moment;
s2, setting a 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= -Kx into the cost function to obtain
Setting the presence of the constant matrix P such that
To minimize J, the optimal control is obtained by the minimum principle: 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 course offset angle, and eliminating steady-state transverse drift errors;
s3, according to a formula of the shore suction, referring to a database of parameters in the formula to obtain the magnitude of the shore suction; 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 speed, F is the hydrodynamic force received by the hull, M is the hydrodynamic moment received by the hull, C F Is a transverse force, C M The first 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 an imu sensor, feeding back and adjusting the numerical value of the state variable to enable the integral of the state variable to be drift error,
error oft 0 To start time, t 1 Is the current time;
s5, the unmanned ship continuously adjusts the heading through the state variable, so that the unmanned ship returns to a set route;
the full-driving control method selects a linear quadratic optimal control LQR method with integral feedback.
The LQR controller can effectively correct and control the motion trail of the ship through rudder swing and speed reduction, maintains 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 the object to be a state space form linear system, the objective function is a quadratic function of the object state and control input, and the gain matrix which enables the objective function value to be minimum is calculated, so that the time domain rudder angle change which meets the optimal control rule is obtained, and the optimal control on the ship motion is realized. When the ship is inclined to one side of the water channel and approaches to the wall of the bank, the ship body is subjected to transverse force absorbed to one side near the bank, namely the action of the suction force, and the suction phenomenon that the ship body is absorbed to the wall of the bank occurs. At the same time, the ship body is subjected to the action of the shoreside moment, so that the bow turns to the center of the channel, namely the shoreside phenomenon. When the ship sails in the limited water area, the LQR controller generates a stable rudder angle to offset the suction force and the suction moment. When the unmanned ship reaches a stable state, the ship body generates a certain transverse movement amount, and the original course is kept for continuous navigation, but the unmanned ship deviates from the original set course, so that the course is ensured to be stable, 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 according to a formula of the shore suction, referring to a database of related parameters to obtain the magnitude of the shore suction. And setting a state variable according to the magnitude of the shore suction force, and then sensing the ship body heading feedback and adjusting the state scalar value according to an imu sensor in the controller. The unmanned ship continuously adjusts the course through the state variable to enable the unmanned ship to return to the set course.
In addition, the gesture and the speed perceived by the imu sensor are calculated through integral operation, the numerical value of the track deviating from the preset route is calculated, and whether the unmanned ship returns to the preset route or not and the position deviating from the preset route is judged.
Under-actuated control:
s6, defining an asymptotically stable plane S according to the state variables in the full-drive control, and defining a formal control law u;
s7, continuously keeping all system trajectories starting at the plane S on the plane S and sliding on the plane S until all system trajectories meet 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 a point on the plane S for a finite time.
In order to enable the movement of the unmanned ship to be related to a time sequence, a sliding mode control method is selected for the underactuated control method.
In the sliding mode control method, an asymptotically stable plane S is defined in accordance with the state variables of the system, and a formal control law u is defined. All system trajectories starting 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 law needs to be modified so that the trajectory converges to a point on this plane for a finite time. The design of the sliding mode control law can be divided into two parts: designing a suitable sliding surface to limit the dynamics of the system to the sliding manifold, resulting in the desired behavior; a continuous control law is designed which forces the system trajectory to the sliding surface and remains thereon.
The invention designs a track tracking sliding mode control law aiming at a water surface unmanned ship system, and the thrust of two propellers is calculated by using two sliding planes.
The first sliding plane is a first order plane defined by the tracking error of the longitudinal movement.
The second sliding plane is a second order plane defined by tracking errors of lateral movement.
In the system, only the absolute position and heading angle of the vessel can be measured and fed back. Therefore, the absolute speed of the ship can only be estimated by a mathematical method, and the heave and heave speeds are calculated through the kinematic relation between the inertial reference system and the ship-following coordinate system.
Taking into account the impedance force model in the form of power law, sliding mode control is designed to track a continuously differentiable target trajectory. 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 a first-order plane which is stable according to an index defined by a ship longitudinal motion tracking error;
the second sliding surface is a second-order exponentially stable plane, defined in terms of tracking errors of lateral motions of the vessel.
As shown in fig. 5, the invention also provides a positioning method of the small water surface cleaning unmanned ship, and the positioning system consists of four base stations and labels carried by the unmanned ship. Each base station and the tag have the functions 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 the upper computer through Bluetooth. The tags may be distributed at any location within the location area.
The positioning system is prepared in advance as follows:
1. first, a map of the unmanned ship working scene is drawn, the number of base stations (4 in fig. 5) is selected according to the size of the map, and the placement positions of the base stations are selected. And the method ensures that any position in the working scene is covered by all base stations, and the more the covered base stations, the higher the positioning accuracy is.
2. 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 all the base stations 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. To improve the transmission capacity and resolution capacity of signals, a wide stop band of signals is realized through a multimode resonant filter.
The positioning method adopts a TOA method and specifically comprises the following steps:
s101, before ranging, UWB positioning labels communicate with each base station once, and inform in advance that ranging pulse signals are to be sent out at the time t 0;
s102, the base station receives time information in a communication mode, starts waiting, prepares to receive a time sequence of a ranging pulse signal at a time t0, and estimates sampling time delay of the ranging pulse signal through calculation;
s103, after capturing the ranging pulse signals, the base station calculates for a plurality of times by taking the time of the sampling point as an interval on the basis of roughly estimating the position of the unmanned ship to obtain the time offset of the sampling point, and finally calculates t1 of the first arrival time of the ranging pulse signals according to the sampling time delay estimation;
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.
After the distance is measured, the label position is calculated by using a maximum likelihood estimation method. The label coordinates are typically at the intersection of circles with each base station coordinate as the center and the base station to label distance as the radius. In actual measurement, however, the tag positions may not be located at the intersections of circles centered on the respective base station positions due to measurement errors of the distances. Therefore, the maximum likelihood estimation method is used for obtaining the approximate normal distribution of the ranging structure error allowance by carrying out statistical fitting, and then the maximum likelihood estimation principle is used for obtaining the position of the tag. And obtaining an estimation result, and transmitting information to the unmanned ship controller and the upper computer after the estimation result passes through a Kalman filter.
Further, the number of the UWB positioning tags is 3; the 3 UWB positioning labels are respectively placed at the bow, the left rear and the right rear of the unmanned ship; the method also comprises the following steps:
s105, according to the relative positions of the 3 UWB positioning labels and an imu sensor arranged in the motion controller, obtaining the heading of the unmanned aerial vehicle;
s106, when the heading obtained according to the relative positions of the UWB positioning labels is inconsistent with the heading measured by the imu sensor, estimating the probability density of the positions of the 3 UWB positioning labels by utilizing maximum likelihood, obtaining the maximum probability position of each UWB positioning label, and correcting the position of the UWB positioning label;
s107, determining the specific range of the unmanned ship through 3 UWB positioning tags, and transmitting information to a main controller and an upper computer positioning module.
The unmanned ship carries 3 labels for avoid the range error to influence positioning accuracy.
The invention discloses a garbage collection method, which 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 an angle of 135 degrees, so that more garbage is guided into the garbage collection cabin. When the unmanned ship advances, the garbage floating on the water surface on the path is collected in the cabin. In addition, the water pump at the stern pumps water, and the pumped water pumps more garbage into the cabin.
The garbage collection cabin in the ship cabin is divided into two parts by a partition net, wherein the front end is a solid garbage collection position, and the rear end is an algae pollutant treatment position. When the water flows into the garbage collection cabin together with the floating garbage, the larger solid garbage can be blocked at the solid garbage collection position due to the blocking of the separating net, and smaller garbage such as algae enters the algae pollutant treatment position through the separating net. The micro-filtration membrane is arranged at the water outlet of the stern, the water flow is sucked out of the cabin by the water pump, and the micro-garbage is remained in the cabin by a membrane filtration method. The membrane filtration method uses a selective permeable membrane with a micro pore size level as a separation medium and uses a pressure difference and a concentration difference as driving, so that substances in water flow selectively permeate the membrane to realize separation or purification. Besides, substances capable of adsorbing and decomposing algae such as activated carbon, biological ceramsite, biological catalyst IBC and the like are placed at the cleaning position of the algae garbage, so that the pressure of the micro-filter screen is reduced, and the micro-filter screen is prevented from being blocked due to the accumulation of a large amount of micro garbage.
The invention is suitable for medium-small water tanks, and can increase the function of cleaning micro plankton (such as algae) on the water surface; the unmanned aerial vehicle cruise control system is provided with a high-precision UWB positioning system, can provide a centimeter-level accurate position for the unmanned aerial vehicle for the controller, and sets a cruise track of the unmanned aerial vehicle through an upper computer software module; the track tracking in the invention adopts a full-driving underactuated method, the unmanned ship completes full-driving control by using a linear quadratic optimal control algorithm with integral feedback, the underactuated control is realized by sliding film control, and the unmanned ship can collect pollutants along the way by using a pollutant cleaning device during navigation.
The foregoing is only illustrative of the preferred embodiments and principles of the present invention, and changes in specific embodiments will occur to those skilled in the art upon consideration of the teachings provided herein, and such changes are intended to be included within the scope of the invention as defined by the claims.
Claims (3)
1. The control method of the small water surface cleaning unmanned ship is characterized by comprising the following steps of:
a garbage collection bin for garbage collection; 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 the position of the sensor; 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;
control means 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 the route, the speed and the inclination angle of the ship body 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 information;
propulsion means 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 forward steering of the ship body;
the small water surface cleaning unmanned ship 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 is not more than 200 meters; the upper computer positioning module is used for setting a cruising track;
the base station and the UWB positioning tag comprise an antenna, a circuit board electrically connected with the antenna, a microcontroller arranged on the circuit board, a UWB positioning module and a clock module;
the motion controller adopts an LQR controller;
the control method of the small water surface cleaning unmanned ship comprises the following steps:
full drive control:
s1, when an unmanned ship sails in a limited water area, the LQR controller generates a stable rudder angle to offset the suction force and the suction moment;
s2, setting a 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= -Kx into the cost function to obtain
Setting the presence of the constant matrix P such that
To minimize J, the optimal control is obtained by the minimum principle: k=r -1 B T P；
S3, according to a formula of the shore suction, referring to a database of parameters in the formula to obtain the magnitude of the shore suction; 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 speed, F is the hydrodynamic force received by the hull, M is the hydrodynamic moment received by the hull, C F Is a transverse force, C M The first 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 an imu sensor, feeding back and adjusting the numerical value of the state variable to enable the integral of the state variable to be drift error,
error oft 0 To start time, t 1 Is the current time;
s5, the unmanned ship continuously adjusts the heading through the state variable, so that the unmanned ship returns to a set route;
under-actuated control:
s6, defining an asymptotically stable plane S according to the state variables in the full drive control, and defining a formal control law U c ；
S7, continuously keeping all system trajectories starting at the plane S on the plane S and sliding on the plane S until all system trajectories meet at a point on the plane S; if there is a system trace that does not start on plane S, then control law U c The modification is performed until all system trajectories converge at a point on the plane S for a finite time.
2. The method of controlling a small water surface unmanned surface cleaning boat of claim 1, further comprising a movable hatch; the movable cabin door is positioned at the front side of the garbage collection cabin.
3. The control method of a small water surface cleaning unmanned boat according to any one of claims 1 to 2, wherein a separation net is provided 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.
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 CN114802616A (en) | 2022-07-29 |
| CN114802616B true 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) |
Families Citing this family (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 (9)
| 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 |
| 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 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11403955B2 (en) * | 2018-12-14 | 2022-08-02 | Brunswick Corporation | Marine propulsion control system and method with proximity-based velocity limiting |
-
2022
- 2022-04-15 CN CN202210396815.0A patent/CN114802616B/en active Active
Patent Citations (9)
| 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 |
| 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 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114802616A (en) | 2022-07-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10589829B2 (en) | Gliding robotic fish navigation and propulsion | |
| CN100443369C (en) | Deep-sea solar diving device | |
| CN113433933B (en) | Autonomous cruising method of intelligent water surface cleaning unmanned ship | |
| Dunbabin et al. | An autonomous surface vehicle for water quality monitoring | |
| US20200262529A1 (en) | Mobile Underwater Docking System and Autonomous Underwater Vehicle | |
| CN114802616B (en) | Small water surface cleaning unmanned ship, control method and positioning method thereof | |
| Mousazadeh et al. | Experimental evaluation of a hydrography surface vehicle in four navigation modes | |
| CN201872911U (en) | Near-surface automatic mobile monitoring station | |
| Al Maawali et al. | Design and implementation of an unmanned surface vehicle for oil spill handling | |
| CN110696979A (en) | Clean ship of solar energy intelligence based on binocular vision | |
| CN112960078A (en) | Unmanned sailing boat automatic driving system and 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 | |
| EP3696078B1 (en) | A method and system for piloting an unmanned surface vessel | |
| EP3501966A1 (en) | An unmanned marine surface vessel | |
| CN110667790A (en) | Aerodynamic water surface weeding and cleaning equipment and cleaning method | |
| Liu et al. | Unmanned airboat technology and applications in environment and agriculture | |
| Yaakob et al. | Development of unmanned surface vehicle (USV) for sea patrol and environmental monitoring | |
| CN109263814A (en) | A kind of multi-functional autonomous cruise boat system of Novel surface | |
| CN211978876U (en) | Unmanned ship for monitoring and tracking oil pollution source in collaborative water environment | |
| CN111332415A (en) | Autonomous cruise energy-saving water quality monitoring ship and monitoring method | |
| Zhang et al. | Design of low-cost unmanned surface vessel for water surface cleaning | |
| Paez et al. | Implementation of an unmanned surface vehicle for environmental monitoring applications | |
| Pu et al. | Development of an unmanned surface vehicle for the emergency response mission of the ‘Sanchi’oil tanker collision and explosion accident | |
| Simmerman et al. | Evaluation of the utility and performance of an autonomous surface vehicle for mobile monitoring of waterborne biochemical agents |
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 |