Automatic cruise path planning method for water pollution monitoring and treatment unmanned ship

Abstract

The invention provides an automatic cruise path planning method for a water pollution monitoring and treatment unmanned ship, and the method comprises the following steps: constructing a global virtual gridding two-dimensional space model, inputting a two-dimensional contour of a cruise water area into the global virtual gridding two-dimensional space model, and obtaining a global virtual grid array; corresponding grid types are defined for all virtual grids of the global virtual grid array according to the field cruise water area, wherein the virtual grid types comprise cruise grids, obstacle grids and obstacle avoidance cruise grids; and introducing a positioning reference coordinate system for the global virtual gridding two-dimensional space model, mapping actual geographic position coordinates with each infinitesimal element point in the global virtual grid array, and determining a water bank boundary envelope domain set of the cruise water area. The method can adapt to a complex water area environment, and a water pollution monitoring and treatment unmanned ship control compensation algorithm with obstacle avoidance and self-adaptive adjustment capabilities is provided, so that the safe operation efficiency and comprehensiveness of the water environment are improved.

Description

Technical Field

[0001] The present invention relates to the field of water pollution monitoring and control, and in particular to an automatic cruising path planning method for an unmanned ship for water pollution monitoring and control. Background Art

[0002] As water pollution becomes increasingly serious, efficient and continuous monitoring of water quality has become particularly important. To this end, unmanned ships for water pollution monitoring and control have emerged. Their automatic cruising function has greatly broadened the application scenarios of water quality monitoring; unmanned ships can autonomously cruise in the waters, covering vast areas that are difficult to reach with traditional monitoring methods, and collect water quality data in real time.

[0003] The "Design of Unmanned Ship Fixed-point Cruise System Based on ROS-Web Interaction" published in an existing journal reveals the use of IMU and GPS data fusion to achieve absolute positioning of the unmanned ship, and the use of error state Kalman filter algorithm and the interaction between the robot operating system and the Web server to control the unmanned ship. However, it mainly relies on the fusion positioning of GPS and IMU, and no navigation map is established. In some environments, such as dense cities, indoors or sheltered waters, there may be problems of reduced accuracy or signal loss;

[0004] The article "Research on Unmanned Boats for Seawater Quality Detection" published in an existing journal proposed a fuzzy PID trajectory tracking control algorithm, which calculates the target heading angle through the relative position deviation between the unmanned boat and the target point, and controls the speed difference of the motors on both sides of the unmanned boat to achieve precise control of the heading angle; that is, the control input of the unmanned boat is adjusted based on the error information between the current position and the expected path, so as to achieve accurate movement of the unmanned boat on the predetermined path.

[0005] Most traditional water pollution monitoring and control methods focus on monitoring fixed areas or simple route patrols, lacking adaptability to complex water environments and intelligent route planning capabilities, which in turn leads to incomplete water quality monitoring and inadequate implementation of control measures, thus affecting the restoration and protection of the water environment. Summary of the invention

[0006] In view of this, the present invention proposes an automatic cruising path planning method for an unmanned boat for water pollution monitoring and control, which can adapt to complex water environments and has a control compensation algorithm for an unmanned boat for water pollution monitoring and control with obstacle avoidance and adaptive adjustment capabilities. It is suitable for complex path planning and dynamic obstacle avoidance, ensuring that it can navigate according to a predetermined route, thereby having strong adaptability, improving the navigation stability, accuracy and efficiency of the unmanned boat, and enhancing its ability to adapt to complex navigation environments.

[0007] The technical solution of the present invention is implemented as follows: The present invention provides a method for automatic cruising path planning of an unmanned ship for water pollution monitoring and control, the method comprising the following steps:

[0008] S1, constructing a global virtual grid two-dimensional space model, inputting the two-dimensional contour of the cruising waters into the global virtual grid two-dimensional space model, and obtaining a global virtual grid array;

[0009] S2, defining corresponding grid types for each virtual grid of the global virtual grid array according to the actual cruising waters, wherein the virtual grid types include a navigable grid, an obstacle grid, and an obstacle-avoiding navigable grid;

[0010] S3, introduces a positioning reference coordinate system into the global virtual grid two-dimensional space model, maps the actual geographical location coordinates with each micro-element point in the global virtual grid array, and determines the waterside boundary envelope domain set of the cruising water area;

[0011] S4, subdividing the obstacle avoidance navigable grid into a number of sub-virtual grids and defining the corresponding grid types, obtaining a sub-virtual grid array, determining a set of obstacle boundary domains that cannot be navigable, and obtaining a set of navigable obstacle avoidance sub-boundary domains based on a set of waterside boundary envelope domains of the navigable waters;

[0012] S5, generating a global path planning graph for the unmanned ship based on a hybrid path planning algorithm according to the set of navigable obstacle avoidance sub-boundary regions;

[0013] S6, when the unmanned ship executes the cruising path, it uses the active obstacle avoidance heading azimuth pre-compensation correction algorithm and PID track vector correction algorithm to monitor and avoid obstacles in real time.

[0014] On the basis of the above technical solution, preferably, in step S1, a global virtual grid two-dimensional space model is constructed, and the two-dimensional contour of the cruising waters is input into the global virtual grid two-dimensional space model to obtain a global virtual grid array, including the following sub-steps:

[0015] S11, determine the cruising waters and set the gridding resolution;

[0016] S12, constructing a global virtual grid two-dimensional space model, inputting the two-dimensional contour of the cruising waters into the global virtual grid two-dimensional space model, and dividing the cruising waters into a number of uniform virtual grids according to a set grid resolution;

[0017] S13, assigning a unique feature access code to each virtual grid in the global virtual gridded two-dimensional space model, and combining several virtual grids to form a global virtual grid array;

[0018] The global virtual grid array expression is A(i, j), where i and j represent the row and column index values ​​of the grid in the global virtual grid array respectively.

[0019] On the basis of the above technical solution, preferably, in step S2, corresponding grid types are defined for each virtual grid of the global virtual grid array according to the actual cruising waters, and the virtual grid types include navigable grids, obstacle grids and obstacle avoidance navigable grids, wherein:

[0020] The definition expression of each virtual grid of the global virtual grid array is:

[0021]

[0022] On the basis of the above technical scheme, preferably, in step S3, a positioning reference coordinate system is introduced into the global virtual grid two-dimensional space model, and the actual geographical location coordinates are mapped to each micro-element point in the global virtual grid array to determine the waterside boundary envelope domain set of the cruising waters, wherein the basic geographical longitude and latitude coordinate reference system of the satellite navigation positioning system is used as the reference coordinate system of the global virtual grid model, and each micro-element point and each vertex in the virtual grid are mapped to the actual geographical location coordinate information, and the waterside boundary envelope domain of the cruising waters is formed by connecting a plurality of micro-element points in sequence;

[0023] The expression of the waterside boundary envelope domain set in the cruising waters is:

[0024] S[am(Em,Nm)]=S[al(E1,N1),a2(E2,N2),an(En,Nn)

[0025] In the formula, S∈A, m=1, 2, 3, …, n; am(Em, Nm) is the waterside boundary envelope of the cruising waters, (Em, Nm) is the geographic coordinate value of the micro-element point on the waterside boundary envelope of the cruising waters, E is the east longitude of the geographic location coordinate, and N is the north latitude of the geographic location coordinate.

[0026] On the basis of the above technical solution, preferably, the expression of the sub-virtual grid array in step S4 is: A′(x, y), where x and y represent the row and column index values ​​of the grid in the sub-virtual grid array respectively;

[0027] The expression of the set of navigable obstacle avoidance sub-boundary regions is: S′[b(Ed, Nd) c ], where, b(Ed, Nd) c is the boundary domain of the cth obstacle grid, c is a positive integer, and (Ed, Nd) is the geographic coordinate value of the infinitesimal point on the boundary domain of the obstacle grid.

[0028] On the basis of the above technical solution, preferably, when the unmanned ship in step S6 executes the cruising path, an active obstacle avoidance heading azimuth pre-compensation correction algorithm and a PID track vector correction algorithm are used to monitor and avoid obstacles in real time, including the following sub-steps:

[0029] S61, the unmanned ship starts loading the global path planning, obtains the current position of the unmanned ship according to the satellite navigation positioning system, and obtains the next cruising point from the global path planning map as the target position;

[0030] S62, according to the current position and the target position of the unmanned ship, calculating the absolute value of the angle between the vector pointing from the current position to the target position and the true north, to obtain the LOS line of sight angle;

[0031] S63, obtaining magnetic north sensor data at the current moment, wherein the magnetic north sensor is disposed on the unmanned ship, and the acquired data is the absolute value of the angle between the steady-state direction of the bow of the unmanned ship's centerline at the current moment and the zero azimuth of the north, to obtain the first heading magnetic north angle;

[0032] S64, adjusting the first heading magnetic north angle accordingly according to the steering mode set by the unmanned ship to obtain a second heading magnetic north angle;

[0033] S65, calculate the yaw angle according to the LOS sight angle and the second heading magnetic north angle, and the calculation expression is:

[0034] Δθ=β LOS -a

[0035] Where Δθ is the yaw angle, β LOS is the LOS sight angle, a is the magnetic north angle of the second heading;

[0036] S66, setting a steering strategy, steering according to the steering strategy based on the size of the yaw angle, and using a dynamic PID control algorithm to adjust the traveling direction posture of the unmanned ship.

[0037] On the basis of the above technical solution, preferably, in step S64, according to the steering mode set by the unmanned ship, the first heading magnetic north angle is adjusted accordingly to obtain the second heading magnetic north angle, wherein the bow and stern of the unmanned ship are respectively provided with steering gears for steering and thrust, respectively, and the positions of the steering steering gear and the thrust steering gear of the unmanned ship are determined. When the stern side steering gear is used for propulsion and the bow side steering gear is used for steering, the first heading magnetic north angle is not adjusted in angle. If the stern side steering gear is used for steering and the bow side steering gear is used for propulsion, the first heading magnetic north angle is adjusted in angle.

[0038] When the first heading magnetic north angle is greater than 180° and the stern side steering gear is used for steering, the first heading magnetic north angle is subtracted by 180° to obtain the second heading magnetic north angle;

[0039] When the first heading magnetic north angle is not greater than 180° and the stern side rudder position is used for steering, add 180° to the first heading magnetic north angle to obtain the second heading magnetic north angle.

[0040] On the basis of the above technical solution, preferably, the steering strategy is set in step S66, and steering is performed according to the steering strategy according to the size of the yaw angle, and the dynamic PID control algorithm is used to adjust the traveling pointing posture of the unmanned ship, wherein the steering strategy includes:

[0041] When Δθ=0°, the unmanned ship is in a forward straight-line state, the forward stern rudder pushes water outward to drive the ship to sail straight, and the dynamic PID control algorithm is used to maintain the heading;

[0042] When |Δθ|≤5°, no steering adjustment is made and the current navigation state is maintained;

[0043] When |Δθ|>5°, the steering adjustment is performed, and the dynamic PID is used to adjust the unmanned ship's heading attitude so that the yaw angle is equal to 0;

[0044] When 5°<Δθ<90°, the unmanned ship yaws clockwise, and the stern steering gear rotates counterclockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 before starting the drive.

[0045] When 90°≤Δθ≤180°, the unmanned ship yaws counterclockwise, and the steering gear at the bow rotates clockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 before starting the drive.

[0046] When 180°<Δθ≤270°, the unmanned ship yaws clockwise, and the stern steering gear rotates counterclockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 position before starting the drive.

[0047] When 270°<Δθ<360°, the unmanned ship yaws counterclockwise, and the servo at the bow rotates clockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 before starting the drive.

[0048] On the basis of the above technical solution, preferably, the dynamic PID control algorithm is used to adjust the traveling pointing posture of the unmanned ship, wherein the dynamic PID control algorithm includes P proportional control and I integral control,

[0049] P proportional control: According to the dynamic maximum value Δθ of the yaw angle max , the output increment is G*Δθ max Steering angle;

[0050] I Integral control: According to the dynamic maximum value of the yaw angle Δθ max , adjust the angular velocity Δr of the unmanned ship max , calculate the basic turning time, the expression is: t_base=Δθ max / Δrmax ;

[0051] Use the gain coefficient to adjust the steering time to get the actual steering time. The expression is: t_actual = PG*t_base, where PG is the gain coefficient.

[0052] Determine the PWM cycle, and calculate the duty cycle based on the steering time and PWM cycle. The expression is: duty_cycle = t_actual / T, where T is the PWM cycle;

[0053] The steering motor is alternately turned on and off during the PWM cycle according to the duty cycle.

[0054] On the basis of the above technical solution, preferably, step S6 further includes obtaining the current position coordinates of the unmanned ship, using the Euclidean distance formula to calculate the distance d between the current position and the target position, and comparing the distance d with the expected error radius R. If d≤R, the unmanned ship enters the error radius and stops the steering operation. The expression is:

[0055]

[0056] Where d is the distance between the current position and the target position, (x1, y1) is the current position coordinate, and (x0, y0) is the target position.

[0057] The automatic cruising path planning method for unmanned boats for water pollution monitoring and control of the present invention has the following beneficial effects compared with the prior art:

[0058] (1) By combining the global virtual grid method and the hybrid traversal algorithm, the global grid cruise path planning in the two-dimensional space of the water surface and the multiple planning method of the cruise path within the local grid can not only adapt to the complex water environment, realize autonomous obstacle avoidance, and automatically plan the optimal cruise path, but also improve the efficiency and comprehensiveness of water quality monitoring;

[0059] (2) The steering strategy is set to take into account the different behaviors of the unmanned ship under different yaw angles, and can be adjusted through the corresponding steering strategy to ensure that it can sail according to the predetermined route, thereby having strong adaptability, improving the navigation stability, accuracy and efficiency of the unmanned ship, and enhancing its ability to adapt to complex navigation environments;

[0060] (3) The dynamic PID control algorithm can adjust the control parameters in real time according to the change of the yaw angle, so as to adapt to different navigation environments and conditions, so that the unmanned ship can maintain a stable navigation posture in a complex and changeable marine environment, thereby enhancing the adaptability of the unmanned ship in a complex navigation environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0062] Figure 1 This is a flow chart of the method for automatic cruising path planning of an unmanned ship for water pollution monitoring and control according to the present invention;

[0063] Figure 2 It is a global virtual gridded two-dimensional space model of the automatic cruising path planning method of the water pollution monitoring and treatment unmanned ship of the present invention;

[0064] Figure 3 It is a schematic diagram of the active obstacle avoidance heading azimuth pre-compensation correction algorithm of the automatic cruising path planning method of the water pollution monitoring and treatment unmanned ship of the present invention;

[0065] Figure 4 This is a schematic diagram of the automatic cruising path planning method for an unmanned ship for water pollution monitoring and control according to the present invention;

[0066] Figure 5 It is a schematic diagram of the first state of the second heading magnetic north angle of the method for automatic cruising path planning of an unmanned ship for water pollution monitoring and control according to the present invention;

[0067] Figure 6 It is a schematic diagram of the second state of the second heading magnetic north angle of the method for automatic cruising path planning of an unmanned ship for water pollution monitoring and control according to the present invention;

[0068] Figure 7 It is a schematic diagram of the yaw angle of the automatic cruising path planning method of the water pollution monitoring and control unmanned ship of the present invention. DETAILED DESCRIPTION

[0069] The following will be combined with the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0070] like Figure 1 As shown, a method for automatic cruising path planning of an unmanned ship for water pollution monitoring and control of the present invention comprises the following steps:

[0071] S1, construct a global virtual grid two-dimensional space model, input the two-dimensional contour of the cruising waters into the global virtual grid two-dimensional space model, and obtain a global virtual grid array.

[0072] It should be noted that this step is the basis of path planning. By inputting the two-dimensional outline of the cruising waters into the global virtual gridded two-dimensional space model, an array composed of multiple virtual grids can be generated; these grids serve as the basic units for subsequent path planning, which is convenient for computer processing and analysis.

[0073] like Figure 2 As shown, step S1 includes the following sub-steps:

[0074] S11, determine the cruising waters and set the gridding resolution;

[0075] It is understandable that first of all, it is necessary to clarify the specific water area where the unmanned ship needs to cruise. This process is determined through water map or satellite image data. The grid resolution is called grid subdivision roughness, which is an adjustable parameter and is also applicable to the establishment of a global virtual grid model. The resolution setting directly affects the accuracy and calculation amount of path planning. Too high a resolution will increase the calculation complexity, while too low a resolution may not accurately reflect the complex situation of the water area. For the global virtual grid two-dimensional space model, it is generally suitable for large-scale grid subdivision, which is called a high-roughness virtual grid.

[0076] S12, constructing a global virtual grid two-dimensional space model, inputting the two-dimensional contour of the cruising waters into the global virtual grid two-dimensional space model, and dividing the cruising waters into a number of uniform virtual grids according to a set grid resolution;

[0077] It can be understood that the two-dimensional contour data of the cruising waters is input into the constructed global virtual gridded two-dimensional space model, and the cruising waters are divided into a number of uniform virtual grids according to the set gridding resolution; each grid is a rectangular or square area, and its size and shape are determined by the gridding resolution. When dividing the grids, it is ensured that the entire cruising waters are completely covered and there is no overlap or omission between the grids. In this embodiment, each grid is a square area.

[0078] S13, assigning a unique feature access code to each virtual grid in the global virtual gridded two-dimensional space model, and combining several virtual grids to form a global virtual grid array;

[0079] The global virtual grid array expression is A(i, j), where i and j represent the row and column index values ​​of the grid in the global virtual grid array respectively.

[0080] It can be understood that a unique feature access code is assigned to each virtual grid, and the code is generated based on the row and column index values ​​of the grid. All virtual grids are combined according to their positional relationship in the cruising waters to form a global virtual grid array.

[0081] S2, defining corresponding grid types for each virtual grid of the global virtual grid array according to the actual cruising waters, wherein the virtual grid types include a navigable grid, an obstacle grid, and an obstacle-avoiding navigable grid;

[0082] Among them, the definition expression of each virtual grid of the global virtual grid array is:

[0083]

[0084] It can be understood that the fully navigable grid is defined as 0, the grid obstacle grid is defined as 1; the obstacle avoidance cruising grid is defined as 0 / 1. If the actual obstacle shore edge envelope in an obstacle navigable grid is smaller than the boundary of a virtual grid, it is treated as an independent grid. Since the actual impact range of the obstacle is small and the grids within this range still allow local cruising, this part of the grid can be regarded as a relatively independent navigable area, which can simplify the path planning algorithm and reduce the amount of calculation, while still ensuring the safety of cruising.

[0085] like Figure 2 As shown, the matrix expression of the global virtual grid array in this embodiment is:

[0086]

[0087] S3, introduces a positioning reference coordinate system into the global virtual grid two-dimensional space model, maps the actual geographical location coordinates with each micro-element point in the global virtual grid array, and determines the waterside boundary envelope domain set of the cruising water area;

[0088] In step S3, the basic geographic longitude and latitude coordinate reference system of the satellite navigation and positioning system is used as the reference coordinate system of the global virtual grid model, and each micro-element point and each vertex in the virtual grid are mapped with the actual geographic location coordinate information. The waterside boundary envelope of the cruising waters is formed by connecting multiple micro-element points in sequence;

[0089] The expression of the waterside boundary envelope domain set in the cruising waters is:

[0090] S[am(Em,Nm)=S[al(El,N1),a2(E2,N2),an(En,Nn)

[0091] In the formula, S∈A, m=1, 2, 3, …, n; am(Em, Nm) is the waterside boundary envelope of the cruising waters, (Em, Nm) is the geographic coordinate value of the micro-element point on the waterside boundary envelope of the cruising waters, E is the east longitude of the geographic location coordinate, and N is the north latitude of the geographic location coordinate.

[0092] It can be understood that the basic geographic longitude and latitude coordinate reference system of the Beidou or GPS satellite navigation and positioning system is used as the reference coordinate system of the global virtual gridding model. This coordinate system provides a globally unified longitude and latitude standard, which can accurately describe the position of any point on the earth and define true north zero degrees as the zero degree direction of the reference coordinate system; each micro-element point and vertex of the grid in the virtual grid is mapped to the actual geographic location coordinates, and the external geographic information system can convert the coordinate system of the virtual grid into actual geographic coordinates.

[0093] S4, subdividing the obstacle avoidance navigable grid into a number of sub-virtual grids and defining the corresponding grid types, obtaining a sub-virtual grid array, determining a set of obstacle boundary domains that cannot be navigable, and obtaining a set of navigable obstacle avoidance sub-boundary domains based on a set of waterside boundary envelope domains of the navigable waters;

[0094] The expression of the sub-virtual grid array in step S4 is: A′(x, y), where x and y represent the row and column index values ​​of the grid in the sub-virtual grid array respectively;

[0095] The expression of the set of navigable obstacle avoidance sub-boundary domains is: S′[b(Ed, Nd)c], where b(Ed, Nd)c is the boundary domain of the cth obstacle grid, c is a positive integer, and (Ed, Nd) is the geographic coordinate value of the infinitesimal point on the boundary domain of the obstacle grid.

[0096] It should be noted that the original obstacle avoidance navigable grid area is first subdivided into multiple smaller sub-grids, each of which has a unique row index x and column index y in the array; the subdivision process can adjust the size of the grid as needed to more accurately process obstacle avoidance and cruising path planning, determine the position of each obstacle grid, and obtain its boundary domain. According to the waterside boundary envelope domain set of the cruising water area, the navigable obstacle avoidance sub-boundary domain set is obtained, and the sub-grid is located inside the waterside boundary envelope domain set of the cruising water area.

[0097] Assuming that the initial gridding resolution of the global virtual grid array is set to 100 meters * 100 meters, the resolution of the re-planned independent sub-virtual grid array can be subdivided into an independent sub-virtual grid array of K1*100 meters *K2*100 meters, where the grid subdivision roughness factor K1=K2<1, and an A′(x, y) sub-virtual grid array can be obtained. The resolution of the sub-virtual grid array is generally as close as possible to the actual geographic coordinate system resolution level of the Beidou / GPS system, which is called a low-roughness virtual grid.

[0098] In addition, the obstacle grid is a completely non-navigable grid. In order to facilitate the data operation and processing of the algorithm, the completely non-navigable obstacle grid only plays the role of row and column alignment operation in the corresponding operation matrix.

[0099] S5, generating a global path planning graph for the unmanned ship based on a hybrid path planning algorithm according to the set of navigable obstacle avoidance sub-boundary regions;

[0100] Among them, the obstacle area and the blank area are marked, the obstacle is marked as 1, the un-cruised area is marked as 2, and the cruised area is marked as 3. The unmanned ship adopts an inner spiral method, moving from one corner to the center of the spiral. When the unmanned ship encounters an operating dead point, it traverses to obtain the virtual grid of the un-cruised area closest to the point, obtains the shortest travel path, and uses the virtual grid point as the new cruise starting point. Then it follows the same inner spiral walking rules, and repeats in sequence until the entire area is cruised.

[0101] In this embodiment, based on the combination of the global virtual grid method and the hybrid traversal algorithm, the global grid cruise path planning for cruising in two-dimensional space on the water surface and the multiple planning method of the cruise path within the local grid can not only adapt to the complex water environment and realize autonomous obstacle avoidance, but also automatically plan the optimal cruise path, thereby improving the efficiency and comprehensiveness of water quality monitoring.

[0102] like Figure 3-4 As shown, S6, when the unmanned ship executes the cruising path, it adopts the active obstacle avoidance heading azimuth pre-compensation correction algorithm and the PID track vector correction algorithm to monitor and avoid obstacles in real time.

[0103] Step S6 includes the following sub-steps:

[0104] S61, the unmanned ship starts loading the global path planning, obtains the current position of the unmanned ship according to the satellite navigation positioning system, and obtains the next cruising point from the global path planning map as the target position;

[0105] S62, according to the current position and the target position of the unmanned ship, calculating the absolute value of the angle between the vector pointing from the current position to the target position and the true north, to obtain the LOS line of sight angle;

[0106] The polar coordinate system for calculating the navigation angle is defined, with the plane geometric center of the unmanned ship as the pole of the polar coordinate system, and the line that coincides with the current position of the planned cruise path and points to the planned predicted target point is a ray. The length of the unmanned ship is p*LP, where LP is the length of the unmanned ship in the length direction, and the value range of p is 2 to 5;

[0107] When the target point is located in the first quadrant of the polar coordinate system for navigation angle calculation, the calculation expression of the LOS sight angle is:

[0108]

[0109] In the formula, β LOS is the LOS line of sight angle, and the current position coordinate is (x k ,y k ), the target position coordinates are (x k+1 ,y k+1 );

[0110] When the target point is at the due north position of the polar coordinate system for navigation angle calculation, β LOS =0°;

[0111] When the target point is located in the third or fourth quadrant, the calculation expression of the LOS sight angle is:

[0112]

[0113] When the target point is located at the south position of the polar coordinate system for navigation angle calculation, β LOS =180°;

[0114] When the target point is in the second quadrant, the calculation expression of the LOS sight angle is:

[0115]

[0116] S63, obtaining magnetic north sensor data at the current moment, wherein the magnetic north sensor is disposed on the unmanned ship, and the acquired data is the absolute value of the angle between the steady-state direction of the bow of the unmanned ship's centerline at the current moment and the zero azimuth of the north, to obtain the first heading magnetic north angle;

[0117] Among them, the magnetic north sensor navigation coordinate system is the coordinate system defined by the magnetic north sensor carried by the unmanned ship. The zero degree value output by the magnetic north sensor is defined as the degree value of the magnetic north coordinate system. The output angle change value range is between 0-360°, and the clockwise change is the direction of increasing angle.

[0118] S64, adjusting the first heading magnetic north angle accordingly according to the steering mode set by the unmanned ship to obtain a second heading magnetic north angle;

[0119] like Figure 5-6 As shown, in this embodiment, the bow and stern of the unmanned ship are respectively provided with steering gears, which are used for steering and thrust respectively. The positions of the steering steering gear and the thrust steering gear of the unmanned ship are determined. When the stern side steering gear is used for propulsion and the bow side steering gear is used for steering, the angle of the magnetic north angle of the first heading is not adjusted. If the stern side steering gear is used for steering and the bow side steering gear is used for propulsion, the angle of the magnetic north angle of the first heading is adjusted.

[0120] When the first heading magnetic north angle is greater than 180° and the stern side steering gear is used for steering, the first heading magnetic north angle is subtracted by 180° to obtain the second heading magnetic north angle;

[0121] When the first heading magnetic north angle is not greater than 180° and the stern side rudder position is used for steering, add 180° to the first heading magnetic north angle to obtain the second heading magnetic north angle.

[0122] S65, calculate the yaw angle according to the LOS sight angle and the second heading magnetic north angle, and the calculation expression is:

[0123] Δθ=β LOS -a

[0124] Where Δθ is the yaw angle, β LOS is the LOS sight angle, a is the magnetic north angle of the second heading;

[0125] like Figure 7 As shown, S66, a steering strategy is set, steering is performed according to the steering strategy based on the size of the yaw angle, and a dynamic PID control algorithm is used to adjust the traveling direction posture of the unmanned ship.

[0126] The value of the yaw angle Δθ between the driving vector and the geomagnetic north azimuth determines the direction of rotation of the driving vector mechanism. The steering strategies include:

[0127] When Δθ=0°, the unmanned ship is in a forward straight-line state. The forward stern rudder pushes water outward to drive the ship to sail straight. The dynamic PID control algorithm is used to maintain the heading, reducing the heading deviation caused by external factors such as wind and waves, thereby improving the stability of navigation.

[0128] When |Δθ|≤5°, no steering adjustment is performed and the current navigation state is maintained; unnecessary steering operations are reduced, energy is saved, and potential damage to the structure of the unmanned ship caused by frequent steering is reduced;

[0129] When |Δθ|>5°, the steering adjustment is performed, and the dynamic PID is used to adjust the unmanned ship's traveling pointing attitude so that the yaw angle is equal to 0; ensuring that the unmanned ship can smoothly and accurately turn to the predetermined heading. The dynamic PID control algorithm can adjust the control parameters in real time according to the current yaw angle and its rate of change, thereby achieving precise control of the steering process;

[0130] When 5°<Δθ<90°, the unmanned ship yaws clockwise, and the stern steering gear rotates counterclockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 before starting the drive.

[0131] When 90°≤Δθ≤180°, the unmanned ship yaws counterclockwise, and the steering gear at the bow rotates clockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 before starting the drive.

[0132] When 180°<Δθ≤270°, the unmanned ship yaws clockwise, and the stern steering gear rotates counterclockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 position before starting the drive.

[0133] When 270°<Δθ<360°, the unmanned ship yaws counterclockwise, and the servo at the bow rotates clockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 before starting the drive.

[0134] Among them, by slowing down during the steering process and stopping the rotation when Δθ decreases to close to 0 and returning to 0 position to restart the drive, the energy consumption in the steering process can be further reduced and the navigation efficiency of the unmanned ship can be improved. The steering strategy takes into account the different behaviors of the unmanned ship under different yaw angles, and can be adjusted through the corresponding steering strategy to ensure that it can navigate according to the predetermined route, thereby having strong adaptability, improving the navigation stability, accuracy and efficiency of the unmanned ship, and enhancing its ability to adapt to complex navigation environments.

[0135] Among them, the dynamic PID control algorithm is used to adjust the traveling pointing attitude of the unmanned ship. The dynamic PID control algorithm includes P proportional control and I integral control.

[0136] P proportional control: According to the dynamic maximum value Δθ of the yaw angle max , the output increment is G*Δθ max The steering angle can be adjusted according to the actual situation of the unmanned ship.

[0137] I Integral control: According to the dynamic maximum value of the yaw angle Δθ max , adjust the angular velocity Δr of the unmanned ship max , calculate the basic turning time, the expression is: t_base=Δθ max / Δr max ;

[0138] Among them, I integral control adjusts the steering angular velocity of the unmanned ship by accumulating past yaw angle deviations, thereby helping to eliminate steady-state errors and ensuring that the unmanned ship maintains an accurate heading during long-term navigation.

[0139] Use the gain coefficient to adjust the steering time to get the actual steering time. The expression is: t_actual = PG*t_base, where PG is the gain coefficient.

[0140] The system can control the smoothness of the steering process by calculating the basic steering time and adjusting the actual steering time according to the gain factor PG. The gain factor PG allows fine-tuning of the steering speed to adapt to different navigation conditions and yaw angles.

[0141] Determine the PWM cycle, and calculate the duty cycle based on the steering time and the PWM cycle. The expression is: duty_cycle = t_actual / T, where T is the PWM cycle. According to the duty cycle, the steering motor is alternately turned on and off within the PWM cycle.

[0142] Using PWM technology to calculate the duty cycle based on the steering time and PWM period, and accordingly turning the steering motor on and off alternately within the PWM period, precise control of the steering motor output can be achieved. This control method can ensure that the steering motor operates at a predetermined angular velocity and duration, thereby achieving precise steering control.

[0143] The dynamic PID control algorithm in this embodiment can adjust the control parameters in real time according to the change of the yaw angle, so as to adapt to different navigation environments and conditions, so that the unmanned ship can maintain a stable navigation posture in a complex and changeable marine environment, and enhance the adaptability of the unmanned ship in a complex navigation environment.

[0144] Step S6 also includes obtaining the current position coordinates of the unmanned ship, using the Euclidean distance formula to calculate the distance d between the current position and the target position, and comparing the distance d with the expected error radius R. If d≤R, the unmanned ship enters the error radius and stops the steering operation. The expression is:

[0145]

[0146] Where d is the distance between the current position and the target position, (x1, y1) is the current position coordinate, and (x0, y0) is the target position.

[0147] When the unmanned boat enters within the error radius R of the expected point, and when the yaw angle approaches 0 and enters within the expected convergence error, the unmanned boat drives and accelerates, stops the steering operation, returns the rudder to the middle position, maintains the middle position directional propulsion to drive the unmanned boat forward, and continues to observe whether the yaw angle exceeds the expected convergence error. If so, repeat the above yaw angle correction and adjustment process.

[0148] In addition, when the cruising waters are river channels, it is safer and more reasonable to define the error radius R of the expected point as half the width of the river channel, which is recorded as R≤1 / 2 river width.

[0149] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for automatic cruising path planning of an unmanned ship for water pollution monitoring and control, characterized in that: The method comprises the following steps: S1, constructing a global virtual grid two-dimensional space model, inputting the two-dimensional contour of the cruising waters into the global virtual grid two-dimensional space model, and obtaining a global virtual grid array; S2, defining corresponding grid types for each virtual grid of the global virtual grid array according to the actual cruising waters, wherein the virtual grid types include a navigable grid, an obstacle grid, and an obstacle-avoiding navigable grid; S3, introduces a positioning reference coordinate system into the global virtual grid two-dimensional space model, maps the actual geographical location coordinates with each micro-element point in the global virtual grid array, and determines the waterside boundary envelope domain set of the cruising water area; S4, subdividing the obstacle avoidance navigable grid into a number of sub-virtual grids and defining the corresponding grid types, obtaining a sub-virtual grid array, determining a set of obstacle boundary domains that cannot be navigable, and obtaining a set of navigable obstacle avoidance sub-boundary domains based on a set of waterside boundary envelope domains of the navigable waters; S5, generating a global path planning graph for the unmanned ship based on a hybrid path planning algorithm according to the set of navigable obstacle avoidance sub-boundary regions; S6, when the unmanned ship executes the cruising path, it uses the active obstacle avoidance heading azimuth pre-compensation correction algorithm and PID track vector correction algorithm to monitor and avoid obstacles in real time.

2. The method for automatic cruising path planning of an unmanned ship for water pollution monitoring and control according to claim 1 is characterized in that: In step S1, a global virtual grid two-dimensional space model is constructed, and the two-dimensional contour of the cruising waters is input into the global virtual grid two-dimensional space model to obtain a global virtual grid array, which includes the following sub-steps: S11, determine the cruising waters and set the gridding resolution; S12, constructing a global virtual grid two-dimensional space model, inputting the two-dimensional contour of the cruising waters into the global virtual grid two-dimensional space model, and dividing the cruising waters into a number of uniform virtual grids according to a set grid resolution; S13, assigning a unique feature access code to each virtual grid in the global virtual gridded two-dimensional space model, and combining several virtual grids to form a global virtual grid array; The global virtual grid array expression is A(i, j), where i and j represent the row and column index values ​​of the grid in the global virtual grid array respectively.

3. The method for automatic cruising path planning of an unmanned ship for water pollution monitoring and control according to claim 2 is characterized in that: In step S2, corresponding grid types are defined for each virtual grid of the global virtual grid array according to the actual cruising waters, and the virtual grid types include navigable grids, obstacle grids, and obstacle-avoiding navigable grids, wherein: The definition expression of each virtual grid of the global virtual grid array is:

4. The method for automatic cruising path planning of an unmanned ship for water pollution monitoring and control according to claim 3 is characterized in that: In step S3, a positioning reference coordinate system is introduced into the global virtual grid two-dimensional space model, and the actual geographical location coordinates are mapped to each micro-element point in the global virtual grid array to determine the waterside boundary envelope domain set of the cruising waters, wherein the basic geographical longitude and latitude coordinate reference system of the satellite navigation positioning system is used as the reference coordinate system of the global virtual grid model, and each micro-element point and each vertex in the virtual grid are mapped to the actual geographical location coordinate information, and the waterside boundary envelope domain of the cruising waters is formed by connecting multiple micro-element points in sequence; The expression of the waterside boundary envelope domain set in the cruising waters is: S[am(Em,Nm)]=S[al(E1,N1),a2(E2,N2),...,an(En,Nn)] In the formula, S∈A, m=1, 2, 3, …, n; am(Em, Nm) is the waterside boundary envelope of the cruising waters, (Em, Nm) is the geographic coordinate value of the micro-element point on the waterside boundary envelope of the cruising waters, E is the east longitude of the geographic location coordinate, and N is the north latitude of the geographic location coordinate.

5. The method for automatic cruising path planning for an unmanned ship for water pollution monitoring and control according to claim 4 is characterized in that: The expression of the sub-virtual grid array in step S4 is: A′(x, y), where x and y represent the row and column index values ​​of the grid in the sub-virtual grid array respectively; The expression of the set of navigable obstacle avoidance sub-boundary regions is: S′[b(Ed, Nd) c ], where, b(Ed, Nd) c is the boundary domain of the cth obstacle grid, c is a positive integer, and (Ed, Nd) is the geographic coordinate value of the infinitesimal point on the boundary domain of the obstacle grid.

6. The method for automatic cruising path planning of an unmanned ship for water pollution monitoring and control according to claim 5 is characterized in that: In step S6, when the unmanned ship executes the cruising path, an active obstacle avoidance heading azimuth pre-compensation correction algorithm and a PID track vector correction algorithm are used to monitor and avoid obstacles in real time, including the following sub-steps: S61, the unmanned ship starts loading the global path planning, obtains the current position of the unmanned ship according to the satellite navigation positioning system, and obtains the next cruising point from the global path planning map as the target position; S62, according to the current position and the target position of the unmanned ship, calculating the absolute value of the angle between the vector pointing from the current position to the target position and the true north, to obtain the LOS line of sight angle; S63, obtaining magnetic north sensor data at the current moment, wherein the magnetic north sensor is disposed on the unmanned ship, and the acquired data is the absolute value of the angle between the steady-state direction of the bow of the unmanned ship's centerline at the current moment and the zero azimuth of the north, to obtain the first heading magnetic north angle; S64, adjusting the first heading magnetic north angle accordingly according to the steering mode set by the unmanned ship to obtain a second heading magnetic north angle; S65, calculate the yaw angle according to the LOS sight angle and the second heading magnetic north angle, and the calculation expression is: Δθ=β LOS -a Where Δθ is the yaw angle, β LOS is the LOS sight angle, a is the magnetic north angle of the second heading; S66, setting a steering strategy, steering according to the steering strategy based on the size of the yaw angle, and using a dynamic PID control algorithm to adjust the traveling direction posture of the unmanned ship.

7. The method for automatic cruising path planning for an unmanned ship for water pollution monitoring and control according to claim 6 is characterized by: In step S64, according to the steering mode set by the unmanned ship, the first heading magnetic north angle is adjusted accordingly to obtain the second heading magnetic north angle, wherein the bow and stern of the unmanned ship are respectively provided with steering gears for steering and thrust, respectively, and the positions of the steering steering gear and the thrust steering gear of the unmanned ship are determined. When the stern side steering gear is used for propulsion and the bow side steering gear is used for steering, the first heading magnetic north angle is not adjusted in angle. If the stern side steering gear is used for steering and the bow side steering gear is used for propulsion, the first heading magnetic north angle is adjusted in angle. When the first heading magnetic north angle is greater than 180° and the stern side steering gear is used for steering, the first heading magnetic north angle is subtracted by 180° to obtain the second heading magnetic north angle; When the first heading magnetic north angle is not greater than 180° and the stern side rudder position is used for steering, add 180° to the first heading magnetic north angle to obtain the second heading magnetic north angle.

8. The method for automatic cruising path planning for an unmanned ship for water pollution monitoring and control according to claim 7 is characterized by: In step S66, the steering strategy is set, and the steering is performed according to the steering strategy according to the size of the yaw angle, and the dynamic PID control algorithm is used to adjust the traveling direction posture of the unmanned ship, wherein the steering strategy includes: When Δθ=0°, the unmanned ship is in a forward straight-line state, the forward stern rudder pushes water outward to drive the ship to sail straight, and the dynamic PID control algorithm is used to maintain the heading; When |Δθ|≤5°, no steering adjustment is made and the current navigation state is maintained; When |Δθ|>5°, the steering adjustment is performed, and the dynamic PID is used to adjust the unmanned ship's heading attitude so that the yaw angle is equal to 0; When 5°<Δθ<90°, the unmanned ship yaws clockwise, and the stern steering gear rotates counterclockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 before starting the drive. When 90°≤Δθ≤180°, the unmanned ship yaws counterclockwise, and the steering gear at the bow rotates clockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 before starting the drive. When 180°<Δθ≤270°, the unmanned ship yaws clockwise, and the stern steering gear rotates counterclockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 position before starting the drive. When 270°<Δθ<360°, the unmanned ship yaws counterclockwise, and the servo at the bow rotates clockwise at the same angle as Δθ, slowing down when turning the rudder. When Δθ decreases to close to 0, it stops rotating and returns to 0 before starting the drive.

9. The method for automatic cruising path planning for an unmanned ship for water pollution monitoring and control according to claim 8 is characterized by: The dynamic PID control algorithm is used to adjust the traveling direction posture of the unmanned ship, wherein the dynamic PID control algorithm includes P proportional control and I integral control. P proportional control: According to the dynamic maximum value Δθ of the yaw angle max , the output increment is G*Δθ max Steering angle; I Integral control: According to the dynamic maximum value of the yaw angle Δθ max , adjust the angular velocity Δr of the unmanned ship max , calculate the basic turning time, the expression is: t_base=Δθ max / Δr max ; Use the gain coefficient to adjust the steering time to get the actual steering time. The expression is: t_actual = PG*t_base, where PG is the gain coefficient. Determine the PWM cycle, and calculate the duty cycle based on the steering time and PWM cycle. The expression is: duty_cycle = t_actual / T, where T is the PWM cycle; The steering motor is alternately turned on and off during the PWM cycle according to the duty cycle.

10. The method for automatic cruising path planning for an unmanned ship for water pollution monitoring and control as claimed in claim 9, characterized in that: The step S66 also includes obtaining the current position coordinates of the unmanned ship, using the Euclidean distance formula to calculate the distance d between the current position and the target position, and comparing the distance d with the expected error radius R. If d≤R, the unmanned ship enters the error radius and stops the steering operation. The expression is: Where d is the distance between the current position and the target position, (x1, y1) is the current position coordinate, and (x0, y0) is the target position.

Images