A path planning and tracking control method and device based on ship collision avoidance

Abstract

The application provides a path planning and tracking control method and device based on ship collision avoidance, comprising: obtaining the positions of the ship, the destination and the obstacles; improving the artificial potential field algorithm according to the position of the ship, the position of the destination, the position of the obstacles and the four-element ship field; outputting the collision avoidance path point of the ship according to the improved artificial potential field algorithm; fitting the collision avoidance path point according to the least square method to obtain the collision avoidance path trajectory of the ship from the destination; controlling the ship according to the collision avoidance path trajectory to obtain the target heading and target position of the ship at each time, and tracking the ship according to the target heading and target position. The application improves the artificial potential field algorithm to obtain the collision avoidance path point, makes the collision avoidance path trajectory smooth and safe through the least square method, realizes the path planning of the ship collision avoidance, determines the target heading and target position of the ship, and realizes the tracking of the ship collision avoidance path.

Description

Technical Field

[0001] This invention relates to the field of ship path planning technology, specifically to a path planning and tracking control method and device based on ship collision avoidance. Background Technology

[0002] With the development of economic globalization, the shipping industry has become a pillar of global trade. In recent years, my country's maritime transportation industry has faced the challenge of digital and intelligent transformation. Among the research areas of ship intelligentization, the study of ship collision avoidance path planning and tracking control is of great significance. Research on ship collision avoidance path planning is mainly divided into traditional algorithms and intelligent algorithms. Intelligent algorithms are applicable to the complex environment of dynamic ship navigation and have good robustness and distributed computing capabilities, but they suffer from drawbacks such as requiring a large number of training samples and training time, and the curse of dimensionality. Traditional algorithms, while having shortcomings such as uneven path planning and a tendency to get trapped in local optima, also have advantages such as simple structure and good performance in real-time obstacle avoidance during path planning.

[0003] In existing technologies, research on ship trajectory tracking control mainly involves combining a control method with a ship motion model. The control methods include fuzzy control and PID control, while the ship motion models include the NOMOTO model and the MMG model. However, many current trajectory tracking methods suffer from the drawback of being prone to collisions with environmental obstacles.

[0004] Therefore, there is an urgent need to propose a path planning and tracking control method and device based on ship collision avoidance to solve the technical problem that existing trajectory tracking methods are prone to collision risks with obstacles in the environment. Summary of the Invention

[0005] In view of this, it is necessary to provide a path planning and tracking control method and device based on ship collision avoidance to solve the technical problem that existing trajectory tracking methods are prone to collision risks with obstacles in the environment.

[0006] On the one hand, the present invention provides a path planning and tracking control method based on ship collision avoidance, including: Obtain the positions of this vessel, the destination, and obstacles; The artificial potential field algorithm is improved based on the position of the vessel, the position of the destination, the position of the obstacle, and the four-dimensional vessel domain. The collision avoidance path points of the vessel are output based on the improved artificial potential field algorithm. The collision avoidance path points are fitted using the least squares method to obtain the collision avoidance path trajectory of the vessel from the destination; The vessel is controlled according to the collision avoidance path trajectory to obtain the target course and target position of the vessel at each time, and the vessel is tracked according to the target course and target position.

[0007] In some possible implementations, the artificial potential field algorithm includes a gravitational potential field function, a gravitational function, a repulsive potential field function, and a repulsive function; The improvement of the artificial potential field algorithm based on the ship's position, the destination's position, the obstacle's position, and the quaternary ship domain includes: A first distance is determined based on the positions of the vessel and the destination, and a second distance is determined based on the positions of the vessel and the obstacle. The gravitational potential field function and the gravitational function are improved based on the position of the ship, the position of the destination, and the first distance; The repulsive potential field function and the repulsive function are improved based on the position of the vessel, the position of the obstacle, the first distance, the second distance, and the quaternary vessel domain.

[0008] In some possible implementations, the step of outputting the collision avoidance path points of the vessel based on the improved artificial potential field algorithm includes: Calculate the resultant gravitational force acting on the ship based on the improved gravitational potential field function and the gravitational function; Calculate the resultant repulsive force acting on the vessel based on the improved repulsive potential field function and the repulsive function; Calculate the resultant force acting on the vessel based on the resultant force of the gravitational force and the resultant force of the repulsive force; Based on the resultant force, the collision avoidance path points are obtained as the vessel navigates toward the destination under the action of the resultant force and reaches the destination.

[0009] In some possible implementations, after calculating the resultant repulsive force on the vessel based on the improved repulsive potential field function and the repulsive function, the method further includes: When multiple obstacles exist during the navigation of the vessel, the resultant repulsive forces of each obstacle are added together to obtain the total resultant repulsive force of the vessel.

[0010] In some possible implementations, fitting the collision avoidance path points using the least squares method to obtain the collision avoidance path trajectory of the vessel from the destination includes: An initial function is preset based on the changing trend of the latitude and longitude coordinates of the collision avoidance path points; Based on the latitude and longitude coordinates of the collision avoidance path points and the initial function, an approximate function is determined; When the approximation function meets the preset conditions, the approximation function is determined as the collision avoidance path trajectory.

[0011] In some possible implementations, determining the approximate function as the collision avoidance path trajectory when the approximate function satisfies a preset condition includes: The ordinate values ​​of the initial function at each time point are calculated together with the ordinate values ​​of the approximation function at each time point to obtain the residual values ​​corresponding to each time point. The residual values ​​corresponding to each time point are calculated to obtain the sum of squared residual values; When the sum of squares residual value is less than a preset threshold, the approximate function is determined as the collision avoidance path trajectory.

[0012] In some possible implementations, after determining the approximate function as the collision avoidance path trajectory when the approximate function satisfies a preset condition, the method further includes: When the sum of squares residual value is greater than or equal to the preset threshold, the initial function is reset, and the process returns to the step of "setting the initial function according to the changing trend of the latitude and longitude coordinates of the collision avoidance path point".

[0013] In some possible implementations, controlling the vessel based on the collision avoidance path trajectory to obtain the target course and target position of the vessel at various times includes: Based on the coordinate system of the ship's four-degree-of-freedom MMG model and the PID control method, a motion model is constructed to calculate the heading of the ship during navigation. The heading of the collision avoidance path trajectory at each time point is determined as the target heading; Using the motion model, the ship's current course at the current moment is turned to the target course corresponding to the current moment, and the rudder angle and course difference corresponding to the current moment are obtained; Based on the rudder angle and the heading difference, the target position corresponding to the current moment is obtained; Determine whether the collision avoidance path trajectory has a target heading corresponding to the next moment; If so, then the next moment is determined as the current moment, and the step of "using the motion model to change the current course of the ship at the current moment to the target course corresponding to the current moment, and obtain the rudder angle and course difference corresponding to the current moment" is returned; If not, then the target heading and target position of the vessel at each time point are obtained.

[0014] In some possible implementations, the ship's four-degree-of-freedom MMG model coordinate system includes four degrees of freedom: sway, pitch, roll, and bow.

[0015] On the other hand, the present invention also provides a path planning and tracking control device based on ship collision avoidance, comprising: The location acquisition module is used to acquire the location of the vessel, the destination, and obstacles. The path point determination module is used to improve the artificial potential field algorithm based on the position of the vessel, the position of the destination, the position of the obstacle, and the four-dimensional vessel domain, and output the collision avoidance path point of the vessel based on the improved artificial potential field algorithm; The path trajectory determination module is used to fit the collision avoidance path points according to the least squares method to obtain the collision avoidance path trajectory of the vessel from the destination; The ship control module is used to control the ship according to the collision avoidance path trajectory, obtain the target heading and target position of the ship at each time, and track the ship according to the target heading and target position.

[0016] The beneficial effects of the above embodiments are as follows: The collision avoidance-based path planning and tracking control method provided by this invention, through an improved artificial potential field algorithm, obtains the collision avoidance path points during the process of the ship avoiding obstacles to reach its destination. This solves the problems in the prior art where the destination is too far from the ship, resulting in excessive gravity and the inability to autonomously return to the planned route after avoiding obstacles, thus improving the safety of the ship's navigation. Furthermore, by fitting the collision avoidance path points using the least squares method, the collision avoidance path trajectory is kept smooth and safe, thereby realizing the collision avoidance path planning for the ship. The ship is also controlled through the collision avoidance path trajectory to determine the target heading and target position of the ship at various times, thereby tracking the collision avoidance path. Since the collision avoidance path trajectory is the path trajectory of the ship avoiding obstacles, tracking the collision avoidance path can prevent the ship from colliding with obstacles, reducing the risk of collision and improving the safety of the ship. Attached Figure Description

[0017] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0018] Figure 1 A schematic flowchart of an embodiment of the path planning and tracking control method based on ship collision avoidance provided by the present invention; Figure 2 A schematic diagram of an embodiment of the positions of the vessel, destination, and obstacle provided by the present invention; Figure 3 A schematic flowchart illustrating an embodiment of the gravitational potential field under the gravity of a flight path provided by the present invention; Figure 4 A schematic diagram of a coordinate system for an embodiment of quaternary ship technology provided by this invention; Figure 5 A schematic diagram of an embodiment of the space conversion module provided by the present invention; Figure 6 A schematic diagram of a coordinate system for an embodiment of the least squares method fitting output trajectory points provided by the present invention; Figure 7 A schematic diagram of an embodiment of the ship coordinate system provided by the present invention; Figure 8 A schematic diagram of an embodiment of the basic principle of PID control provided by the present invention; Figure 9 This is a schematic diagram of an embodiment of the path planning and tracking control device based on ship collision avoidance provided by the present invention. Detailed Implementation

[0019] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0020] Some of the block diagrams shown in the accompanying drawings are functional entities and do not necessarily correspond to physically or logically independent entities. These functional entities can be implemented in software, in one or more hardware modules or integrated circuits, or in different network and / or processor systems and / or microcontroller systems.

[0021] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of the invention. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a mutually exclusive, independent, or alternative embodiment. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0022] This invention provides a path planning and tracking control method and apparatus based on ship collision avoidance, which will be described below.

[0023] Figure 1 This is a schematic flowchart of an embodiment of the path planning and tracking control method based on ship collision avoidance provided by the present invention, as shown below. Figure 1 As shown, the path planning and tracking control methods based on ship collision avoidance include: S101. Obtain the position of the vessel, the destination, and obstacles; S102. Based on the position of the vessel, the position of the destination, the position of the obstacle, and the four-dimensional vessel domain, the artificial potential field algorithm is improved, and the collision avoidance path points of the vessel are output according to the improved artificial potential field algorithm. S103. Fit the collision avoidance path points according to the least squares method to obtain the collision avoidance path trajectory of the vessel from the destination; S104. Control the vessel according to the collision avoidance path trajectory to obtain the target course and target position of the vessel at each time, and track the vessel according to the target course and target position.

[0024] It should be noted that obstacles are divided into dynamic obstacles and static obstacles. Dynamic obstacles can be other ships, etc., while static obstacles can be reefs, etc.

[0025] Compared with existing technologies, the collision avoidance-based path planning and tracking control method provided by this invention obtains the collision avoidance path points during the process of the vessel avoiding obstacles to reach its destination by improving the artificial potential field algorithm. This solves the problems in existing technologies where the destination is too far from the vessel, resulting in excessive gravity and the inability to autonomously return to the planned route after avoiding obstacles, thus improving the safety of the vessel's navigation. Furthermore, the collision avoidance path points are fitted using the least squares method to ensure that the collision avoidance path trajectory remains smooth and safe, thereby realizing the collision avoidance path planning for the vessel. The vessel is also controlled based on the collision avoidance path trajectory to determine the target heading and target position of the vessel at various times, thereby tracking the collision avoidance path. Since the collision avoidance path trajectory is the path the vessel takes to avoid obstacles, tracking the collision avoidance path can prevent the vessel from colliding with obstacles, reducing the risk of collision and improving the safety of the vessel.

[0026] In some embodiments of the present invention, the artificial potential field algorithm includes a gravitational potential field function, a gravitational function, a repulsive potential field function, and a repulsive function; The improvement of the artificial potential field algorithm based on the ship's position, the destination's position, the obstacle's position, and the quaternary ship domain includes: A first distance is determined based on the positions of the vessel and the destination, and a second distance is determined based on the positions of the vessel and the obstacle. The gravitational potential field function and the gravitational function are improved based on the position of the ship, the position of the destination, and the first distance; The repulsive potential field function and the repulsive function are improved based on the position of the vessel, the position of the obstacle, the first distance, the second distance, and the quaternary vessel domain.

[0027] It should be noted that, Figure 2 A schematic diagram illustrating an embodiment of the positions of the vessel, destination, and obstacles provided by the present invention, as shown below. Figure 2 As shown, the position of this vessel is P The destination location is G The location of the obstacle is T The gravitational pull of the destination on this vessel is The repulsive force of the obstacle is , working together The direction indicates the target course of the vessel. As the vessel's position in the virtual potential field is continuously updated, the resultant force... The direction and magnitude of the force will be continuously updated, enabling the vessel to avoid obstacles and eventually reach its destination. When the vessel is far from its destination, the gravity function is corrected by adjusting the destination distance threshold, which reduces the gravitational pull of the destination on the vessel, thus preventing the vessel from colliding with surrounding obstacles while only heading towards the destination. When the vessel is about to reach the destination distance threshold, the gravity on the vessel is restored to the gravity before the correction, allowing the vessel to smoothly sail to its destination under the pull of the resultant force.

[0028] The improved gravitational potential field function is shown in Equation 1: Formula 1 In the formula, It is the gravitational gain coefficient. It is the gravitational potential field of the destination on the ship. This is the ship's position. It is the destination location. It is the first distance from the ship to its destination. The destination distance threshold is the specific gravity gain coefficient and destination distance threshold, which can be set according to the actual situation. This embodiment of the invention does not impose any restrictions on them.

[0029] The improved gravitational function is shown in Equation 2: Formula 2 In the formula, It is the attraction of the destination to this vessel.

[0030] To address the problem of ships being unable to autonomously return to their course, the gravitational function can be modified. Figure 3 A schematic diagram of an embodiment of the gravitational potential field under the gravity of the flight path provided by the present invention is shown below. Figure 3 As shown, when this vessel OS(P) When a vessel deviates from its course due to the repulsive force of an obstacle T in the potential field space, the vessel... OS(P) The position is perpendicular to the route, with the baseline as the reference point (the vertical distance between this vessel and the route is...). x 1), this ship OS(P) Location and destination x The distance between points 2 is called the gravitational attraction segment of the route. Within this segment, the gravitational force of the route can attract the vessel. OS(P) Return route, destination gravity and the repulsive force of obstacles Generate resultant force , working together With the gravity of the route Generate new synergy Then this ship OS(P) Gravity at the destination , route gravity and the repulsive force of obstacles Under the combined effect of speed V os The ship sails towards its destination until it reaches it. The gravitational function of the route is shown in Equation 3: Formula 3 In the formula, It is the gravitational pull of the shipping route on the vessel. It is the gravitational gain coefficient of the flight path gravitational function. Indicates the vertical distance of this vessel from the route. Indicates the destination location. On the flight path and The location of a point between the points, wherein the gravitational gain coefficient of the specific flight path gravitational function can be set according to the actual situation, and the embodiments of the present invention are not limited thereto.

[0031] The gravitational force acting on this vessel during its voyage originates from two sources: the gravitational force from the destination and the gravitational force from the route. The resultant gravitational force acting on this vessel is shown in Formula 4: Formula 4 in, It is the attraction of the destination to the ship. It is the gravitational pull of the route on the ship.

[0032] In some embodiments of the present invention, the step of outputting the collision avoidance path points of the vessel according to the improved artificial potential field algorithm includes: Calculate the resultant gravitational force acting on the ship based on the improved gravitational potential field function and the gravitational function; Calculate the resultant repulsive force acting on the vessel based on the improved repulsive potential field function and the repulsive function; Calculate the resultant force acting on the vessel based on the resultant force of the gravitational force and the resultant force of the repulsive force; Based on the resultant force, the collision avoidance path points are obtained as the vessel navigates toward the destination under the action of the resultant force and reaches the destination.

[0033] It should be noted that traditional artificial potential field algorithms do not consider the ship's size and velocity. Therefore, based on the quaternary ship domain, the repulsive potential field function and repulsive function can be improved. First, a quaternary ship domain coordinate system is constructed, such as... Figure 4 As shown, in the quaternary ship domain coordinate system, let the coordinates of this ship be... The boundary equations for the quaternary ship domain are shown in Equation 5: Formula 5 In the formula, , , , These represent the radius lengths in the quaternary ship domain. For the sign determination function, determine the neighborhood radius length of each quadrant. This is the length of the vessel. This is the approach distance of this vessel. This is the initial diameter of the vessel's turning radius. It is the gain coefficient for the approach distance. It is the gain coefficient of the initial cyclotron diameter.

[0034] The approach distance of this vessel and ship's initial turning diameter Based on the ship's dimensions and speed, an empirical formula can be used for estimation, as shown in Formula 6: Formula 6 In the formula, It is the gain coefficient for the approach distance. It is the gain coefficient of the initial diameter of the cyclotron. This is the speed of the vessel.

[0035] As the size and speed of a vessel increase, the size of its quaternary domain also gradually increases, a characteristic consistent with observations of the distribution of vessel domains in maritime practice. For example... Figure 4As shown, the length from the center to the boundary of the quaternary vessel domain. The length can be obtained using the formula for the distance from the center of the ellipse to any point on the ellipse. The calculation is shown in Formula 7: Formula 7 In the formula, , The line connecting the center and a point on the boundary of a quaternary ship's domain and... The angle between them , The line connecting the center and a point on the boundary of a quaternary ship's domain and... The angle between them.

[0036] Assume the positions and velocities of the ship and the obstacle at the same time step are respectively expressed as: , , and ,like Figure 5 As shown, the safe distance for the vessel during collision avoidance is given by formulas 8 and 9: Formula 8 Formula 9 In the formula, This is the safe distance for this vessel. This is the critical distance range for the vessel to avoid collision with obstacles. This is the radius of this vessel's domain. This is the permissible safe distance between this vessel and the obstacle. This refers to the range of influence of obstacles set up by the vessel's operators, which is greater in conditions such as low visibility and open water.

[0037] Draw two tangent lines from this ship to the critical circle. Defined as any tangent line and its relative position vector point The angles between them are shown in Formulas 10 and 11: Formula 10 Formula 11 In the formula, It is any tangent line and the relative position vector point The angle between them This is the critical distance range for the vessel to avoid collision with obstacles. Indicates the position of the vessel at the same time step. This indicates the speed of the vessel at the same time step. This indicates the position of the obstacle at the same time step. This represents the velocity of the obstacle at the same time step. The extension line passes through a radius of When the critical circle is reached, it indicates a collision hazard. This is the distance between the vessel and the obstacle. This is the relative velocity vector of the ship. This is the ship's speed. It is the speed of the obstacle.

[0038] The improved repulsive potential field function is shown in Equation 12: Formula 12 The improved repulsive force function of this ship is shown in Formula 13: Formula 13 The parameters of the repulsion function are calculated as shown in Formula 14-23: Formula 14 Formula 15 Formula 16 Formula 17 Formula 18 Formula 19 Formula 20 Formula 21 Formula 22 Formula 23 In the formula, , These represent the scaling factors for dynamic obstacles and static obstacles, respectively. This indicates the proportion of the vessel's emergency collision avoidance maneuver. This is the distance between the vessel and the obstacle. This is the distance between the vessel and its destination. It is the speed of the obstacle. It is the modulus of the relative speed between the vessel and the obstacle. , and All of these are related to the relative positions of the vessel and the obstacle, and under the corresponding conditions, force the vessel to move away from the obstacle to complete the avoidance maneuver. Whether the vessel takes evasive action from port or starboard depends on the vector. Location Which side, Based on the crew's experience, the vessel changed its course to starboard within the safe zone, because the side the vessel passes on when encountering an obstacle is determined by the International Regulations for Preventing Collisions at Sea, 1972 (COLREGs). , and This forces the vessel to sail toward its destination.

[0039] In some embodiments of the present invention, after calculating the resultant repulsive force on the vessel based on the improved repulsive potential field function and the repulsive function, the method further includes: When multiple obstacles exist during the navigation of the vessel, the resultant repulsive forces of each obstacle are added together to obtain the total resultant repulsive force of the vessel.

[0040] It should be noted that during navigation, this vessel is not only affected by a single obstacle, but also obstructed by multiple obstacles. The repulsive force it experiences is the sum of the repulsive forces generated by each obstacle, as shown in Formula 24: Formula 24 in, It is the first The repulsive force exerted by the obstacle on the vessel It is the number of obstacles in the navigation area that exert a repulsive force on the vessel.

[0041] The force acting on this vessel during navigation is the resultant force of the gravitational force of the destination, the gravitational force of the route, and the repulsive force of multiple obstacles. Therefore, the resultant force acting on this vessel is as shown in Formula 25: Formula 25 in, This represents the sum of the gravitational forces acting on the ship. This represents the sum of the repulsive forces acting on the vessel.

[0042] The vessel works together within the navigation area. Under the influence of [the system], the ship sails towards its destination and eventually reaches it, thus achieving path planning.

[0043] In some embodiments of the present invention, the step of fitting the collision avoidance path points according to the least squares method to obtain the collision avoidance path trajectory of the vessel from the destination includes: An initial function is preset based on the changing trend of the latitude and longitude coordinates of the collision avoidance path points; Based on the latitude and longitude coordinates of the collision avoidance path points and the initial function, an approximate function is determined; When the approximation function meets the preset conditions, the approximation function is determined as the collision avoidance path trajectory.

[0044] It should be noted that, Figure 6 This is a schematic diagram of the coordinate system for an embodiment of the least squares method fitting output trajectory points. It is assumed that the latitude and longitude coordinates of the n trajectory points output by the improved artificial potential field algorithm are... By observing these discrete points, their changing trends can be set as an initial function. The initial function may be a linear function or a multivariate function of higher powers. Then, using the latitude and longitude coordinates of the trajectory points, an approximate function is obtained. To make it "best" close And it is not necessary to satisfy the interpolation principle, such as Figure 6 The curve in the middle, The resulting curve is the optimal collision avoidance navigation trajectory of the ship output by the algorithm.

[0045] In some embodiments of the present invention, determining the approximate function as the collision avoidance path trajectory when the approximate function satisfies a preset condition includes: The ordinate values ​​of the initial function at each time point are calculated together with the ordinate values ​​of the approximation function at each time point to obtain the residual values ​​corresponding to each time point. The residual values ​​corresponding to each time point are calculated to obtain the sum of squared residual values; When the sum of squares residual value is less than a preset threshold, the approximate function is determined as the collision avoidance path trajectory.

[0046] It should be noted that for curve fitting functions It is not required that the fitting function strictly passes through all trajectory points, that is, the fitting function exist Place and The deviation (also known as the residual) is not always strictly equal to zero. The formula for calculating the residual is shown in Formula 26: Formula 26 In the formula, It is a residual. The fitted function value (in) (The ordinate value corresponding to the fitted function at that point). It is an objectively existing function value (in) The objective existence of the function corresponds to the ordinate value, which is the ordinate value corresponding to the x-coordinate of the trajectory point itself.

[0047] To ensure that the approximate curve reflects the changing trend of the given trajectory points as closely as possible, the deviation must be minimized according to a certain metric, that is, the sum of squares of the residuals must be minimized. The formula for calculating the sum of squares of the residuals is shown in Formula 27: Formula 27 When the obtained sum of squares residual value is less than the preset threshold, it indicates that the fitting trajectory effect meets the requirements. Furthermore, the smaller the sum of squares residual value, the better the fitting trajectory effect. The preset threshold can be set according to the actual situation, and this embodiment of the invention does not impose any restrictions on it.

[0048] In some embodiments of the present invention, after determining the approximate function as the collision avoidance path trajectory when the approximate function satisfies a preset condition, the method further includes: When the sum of squares residual value is greater than or equal to the preset threshold, the initial function is reset, and the process returns to the step of "setting the initial function according to the changing trend of the latitude and longitude coordinates of the collision avoidance path point".

[0049] When the obtained sum of squares residual value is greater than or equal to the preset threshold, it indicates that the fitted trajectory does not meet the requirements. It is necessary to reset the initial function according to the changing trend of the latitude and longitude coordinates of the collision avoidance path points, and repeat the least squares trajectory fitting steps until the fitted trajectory meets the requirements.

[0050] In some embodiments of the present invention, controlling the vessel according to the collision avoidance path trajectory to obtain the target course and target position of the vessel at various times includes: Based on the coordinate system of the ship's four-degree-of-freedom MMG model and the PID control method, a motion model is constructed to calculate the heading of the ship during navigation. The heading of the collision avoidance path trajectory at each time point is determined as the target heading; Using the motion model, the ship's current course at the current moment is turned to the target course corresponding to the current moment, and the rudder angle and course difference corresponding to the current moment are obtained; Based on the rudder angle and the heading difference, the target position corresponding to the current moment is obtained; Determine whether the collision avoidance path trajectory has a target heading corresponding to the next moment; If so, then the next moment is determined as the current moment, and the step of "using the motion model to change the current course of the ship at the current moment to the target course corresponding to the current moment, and obtain the rudder angle and course difference corresponding to the current moment" is returned; If not, then the target heading and target position of the vessel at each time point are obtained.

[0051] It should be noted that the ship can be controlled using the PID-MMG model. First, a four-degree-of-freedom MMG model coordinate system for the ship is constructed, such as... Figure 7 As shown, O-XYZ is the geodetic fixed coordinate system, with the X-axis pointing due north, the Y-axis pointing due east, and the Z-axis pointing towards the Earth's center. o-xyz is the ship's fixed coordinate system, where the ox-axis points towards the bow, the oy-axis points towards the starboard side, and the oz-axis points towards the keel.

[0052] In some embodiments of the present invention, the ship four-degree-of-freedom MMG model coordinate system includes four degrees of freedom: sway, pitch, roll, and bow.

[0053] It should be noted that a four-DOF MMG model is constructed by considering four degrees of freedom: sway, pitch, roll, and bow. The specific formula of the model is shown in Formula 28. Formula 28 In the formula, This refers to the hull mass of the vessel. , Indicates in shaft and The added mass of the shaft, , Therefore , Moment of inertia around the axis , Therefore , Additional moment of inertia centered on the axis, , yes shaft and The velocity components of the shaft, , Representing the angular velocities of bow and roll respectively, the hydrodynamic forces and moments of bow, lateral, roll, and longitudinal forces of a ship are expressed as follows: , , , This indicates that the rudder, hull, and propeller are used for... , , express.

[0054] For ease of calculation, the Yuanliang diagram calculation method can be used to calculate the ship's additional mass and additional moment of inertia in Formula 28, as shown in Formula 29: Formula 29 In the formula, He is the captain of this vessel. This is the beam of the vessel. It is the square coefficient. This is the ship's draft.

[0055] During ship navigation, the hydrodynamic and moment forces of the hull in Formula 28 are calculated using the model shown in Formula 30: Formula 30 In the formula, For the density of water, It is the vertical length of the ship.

[0056] During ship navigation, the propeller thrust is expressed as shown in Formula 31 to calculate the propeller hydrodynamics in Formula 28: Formula 31 In the formula, This is the thrust reduction fraction of this vessel. , These are the propeller's rotational speed and diameter, respectively. This is the thrust coefficient of this ship. , Indicates the left and starboard sides of the ship, , and These parameters are obtained by fitting the open-water characteristic curve of the propeller. It is the advance coefficient. It is the effective wake fraction of thrust.

[0057] During the ship's navigation, considering the four degrees of freedom of rolling, the rudder force hydrodynamics in Formula 28 are as shown in Formula 32: Formula 32 In the formula, and It is the coefficient of influence of the rudder on the hydrodynamics of the ship. It is the longitudinal position of the rudder shaft. It is the longitudinal coordinate of the additional lateral force.

[0058] After constructing the coordinate system of the ship's four-degree-of-freedom MMG model, a motion model for calculating the ship's heading during navigation is built using PID control combined with the four-degree-of-freedom MMG model. This motion model can calculate the heading in real time. The principle of PID control is as follows: Figure 8 As shown, , These are the system's inputs and outputs. , It refers to the input and output of control.

[0059] The control equation for the PID controller is shown in Equation 33: Formula 33 The differential equation for PID control is shown in Equation 34: Formula 34 In the formula, , , These represent the proportional, integral, and derivative gain parameters, respectively. It is the differential time constant.

[0060] The four-degree-of-freedom MMG model of a ship addresses the nonlinear changes in the ship's rudder angle when the steering gear rotates to a specified rudder angle. However, the changes in the rudder angle are also nonlinear during the ship's navigation. Therefore, PID control is needed to control the ship's rudder angle and address the changes during the ship's navigation.

[0061] The heading of the collision avoidance path trajectory output by the least squares method is used as the target heading of the vessel. The vessel is then controlled by PID to change from the current heading to the target heading, thereby achieving real-time control of the vessel's motion, as shown in Equations 35 and 36: Formula 35 Formula 36 In the formula, refer to The ship's rudder angle at all times, For the time of the ship Poor heading Indicates the target course of this vessel. Indicates the current course of this vessel, where, It can be obtained by the angle formed by the line connecting the current trajectory point and the trajectory point at the next time moment, and the extension of the line pointing towards true north from the current trajectory point. The first current heading can be manually entered. Subsequent current headings are obtained by the angle between the extension of the true north line of the trajectory point at the previous moment and the line connecting the trajectory point at the previous moment to the trajectory point at the current moment.

[0062] After obtaining the rudder angle and heading difference of the vessel at the current moment using formulas 35 and 36, the target position corresponding to the vessel at the current moment can be obtained. Then, the above steps are repeated to obtain the target heading corresponding to the next moment. This process continues until the target headings at all moments on the collision avoidance path trajectory are processed. Then, the target heading and target position of the vessel at each moment can be obtained, thus realizing the tracking of the collision avoidance path trajectory of the vessel.

[0063] This invention combines determining the collision avoidance path trajectory with tracking the collision avoidance path trajectory, enabling the vessel to not only avoid dynamic and static obstacles while tracking the collision avoidance path trajectory, but also to quickly and safely reach the destination.

[0064] To better implement the collision avoidance-based path planning and tracking control method in this invention, this invention also provides a collision avoidance-based path planning and tracking control device, which includes: The location acquisition module is used to acquire the location of the vessel, the destination, and obstacles. The path point determination module is used to improve the artificial potential field algorithm based on the position of the vessel, the position of the destination, the position of the obstacle, and the four-dimensional vessel domain, and output the collision avoidance path point of the vessel based on the improved artificial potential field algorithm; The path trajectory determination module is used to fit the collision avoidance path points according to the least squares method to obtain the collision avoidance path trajectory of the vessel from the destination; The ship control module is used to control the ship according to the collision avoidance path trajectory, obtain the target heading and target position of the ship at each time, and track the ship according to the target heading and target position.

[0065] The path planning and tracking control device based on ship collision avoidance provided in the above embodiments can realize the technical solutions described in the above embodiments of the path planning and tracking control method based on ship collision avoidance. The specific implementation principles of each module or unit can be found in the corresponding content in the above embodiments of the path planning and tracking control method based on ship collision avoidance, and will not be repeated here.

[0066] like Figure 9 As shown, the present invention also provides a path planning and tracking control device 1000 based on ship collision avoidance. The path planning and tracking control device 1000 based on ship collision avoidance includes a processor 1001, a memory 1002, and a display 1003. Figure 9Only some components of the collision avoidance-based path planning and tracking control device 1000 are shown; however, it should be understood that implementation of all shown components is not required, and more or fewer components may be implemented instead.

[0067] In some embodiments, memory 1002 may be an internal storage unit of the collision avoidance path planning and tracking control device 1000, such as a hard disk or memory of the collision avoidance path planning and tracking control device 1000. In other embodiments, memory 1002 may also be an external storage device of the collision avoidance path planning and tracking control device 1000, such as a pluggable hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc., equipped on the collision avoidance path planning and tracking control device 1000.

[0068] Furthermore, the memory 1002 may include both internal storage units of the collision avoidance-based path planning and tracking control device 1000 and external storage devices. The memory 1002 is used to store application software and various types of data installed on the collision avoidance-based path planning and tracking control device 1000.

[0069] In some embodiments, processor 1001 may be a central processing unit (CPU), microprocessor, or other data processing chip, used to run program code stored in memory 1002 or process data, such as the path planning and tracking control method based on ship collision avoidance in this invention.

[0070] In some embodiments, display 1003 may be an LED display, a liquid crystal display, a touch-screen liquid crystal display, or an OLED (Organic Light-Emitting Diode) touchscreen. Display 1003 is used to display information from the ship collision avoidance-based path planning and tracking control device 1000 and to display a visual user interface. Components 1001-1003 of the ship collision avoidance-based path planning and tracking control device 1000 communicate with each other via a system bus.

[0071] In some embodiments of the present invention, when the processor 1001 executes the path planning and tracking control program based on ship collision avoidance in the memory 1002, the following steps can be implemented: Obtain the positions of this vessel, the destination, and obstacles; The artificial potential field algorithm is improved based on the position of the vessel, the position of the destination, the position of the obstacle, and the four-dimensional vessel domain. The collision avoidance path points of the vessel are output based on the improved artificial potential field algorithm. The collision avoidance path points are fitted using the least squares method to obtain the collision avoidance path trajectory of the vessel from the destination; The vessel is controlled according to the collision avoidance path trajectory to obtain the target course and target position of the vessel at each time, and the vessel is tracked according to the target course and target position.

[0072] It should be understood that when the processor 1001 executes the path planning and tracking control program based on ship collision avoidance in the memory 1002, in addition to the functions mentioned above, it can also perform other functions, as can be found in the description of the corresponding method embodiments above.

[0073] Furthermore, this embodiment of the invention does not specifically limit the type of the ship collision avoidance-based path planning and tracking control device 1000 mentioned. The ship collision avoidance-based path planning and tracking control device 1000 can be a portable ship collision avoidance-based path planning and tracking control device such as a mobile phone, tablet computer, personal digital assistant (PDA), wearable device, or laptop computer. Exemplary embodiments of portable ship collision avoidance-based path planning and tracking control devices include, but are not limited to, portable ship collision avoidance-based path planning and tracking control devices running iOS, Android, Microsoft, or other operating systems. The aforementioned portable ship collision avoidance-based path planning and tracking control device can also be other portable ship collision avoidance-based path planning and tracking control devices, such as laptop computers with touch-sensitive surfaces (e.g., touch panels). It should also be understood that in some other embodiments of the invention, the ship collision avoidance-based path planning and tracking control device 1000 may not be a portable ship collision avoidance-based path planning and tracking control device, but rather a desktop computer with a touch-sensitive surface (e.g., a touch panel).

[0074] Accordingly, this application also provides a computer-readable storage medium for storing computer-readable programs or instructions. When the programs or instructions are executed by a processor, they can implement the steps or functions of the path planning and tracking control method based on ship collision avoidance provided in the above-described method embodiments.

[0075] Those skilled in the art will understand that all or part of the processes of the methods described in the above embodiments can be implemented by a computer program instructing related hardware (such as a processor, controller, etc.), and the computer program can be stored in a computer-readable storage medium. The computer-readable storage medium may be a disk, optical disk, read-only memory, or random access memory, etc.

[0076] The above provides a detailed description of the path planning and tracking control method and device based on ship collision avoidance provided by the present invention. Specific examples have been used to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only for the purpose of helping to understand the method and core ideas of the present invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of the present invention. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A path planning and tracking control method based on ship collision avoidance, characterized in that, include: Obtain the positions of this vessel, the destination, and obstacles; The artificial potential field algorithm is improved based on the position of the vessel, the position of the destination, the position of the obstacle, and the four-dimensional vessel domain to obtain the improved artificial potential field algorithm. The collision avoidance path point of the vessel is output based on the improved artificial potential field algorithm. The collision avoidance path points are fitted using the least squares method to obtain the collision avoidance path trajectory of the vessel from the destination; The vessel is controlled according to the collision avoidance path trajectory to obtain the target course and target position of the vessel at each time, and the vessel is tracked according to the target course and target position; The artificial potential field algorithm includes a gravitational potential field function, a gravitational function, a repulsive potential field function, and a repulsive function; The improvement of the artificial potential field algorithm based on the ship's position, the destination's position, the obstacle's position, and the quaternary ship domain includes: A first distance is determined based on the positions of the vessel and the destination, and a second distance is determined based on the positions of the vessel and the obstacle. The gravitational potential field function and the gravitational function are improved based on the position of the ship, the position of the destination, and the first distance; The repulsive potential field function and the repulsive function are improved based on the position of the vessel, the position of the obstacle, the first distance, the second distance, and the quaternary vessel domain.

2. The path planning and tracking control method based on ship collision avoidance according to claim 1, characterized in that, The step of outputting the collision avoidance path points of the vessel based on the improved artificial potential field algorithm includes: Calculate the resultant gravitational force acting on the ship based on the improved gravitational potential field function and the gravitational function; Calculate the resultant repulsive force acting on the vessel based on the improved repulsive potential field function and the repulsive function; Calculate the resultant force acting on the vessel based on the resultant force of the gravitational force and the resultant force of the repulsive force; Based on the resultant force, the collision avoidance path points are obtained as the vessel navigates toward the destination under the action of the resultant force and reaches the destination.

3. The path planning and tracking control method based on ship collision avoidance according to claim 2, characterized in that, After calculating the resultant repulsive force on the vessel based on the improved repulsive potential field function and the repulsive function, the method further includes: When multiple obstacles exist during the navigation of the vessel, the resultant repulsive forces of each obstacle are added together to obtain the total resultant repulsive force of the vessel.

4. The path planning and tracking control method based on ship collision avoidance according to claim 1, characterized in that, The step of fitting the collision avoidance path points using the least squares method to obtain the collision avoidance path trajectory of the vessel from the destination includes: An initial function is preset based on the changing trend of the latitude and longitude coordinates of the collision avoidance path points; Based on the latitude and longitude coordinates of the collision avoidance path points and the initial function, an approximate function is determined; When the approximation function meets the preset conditions, the approximation function is determined as the collision avoidance path trajectory.

5. The path planning and tracking control method based on ship collision avoidance according to claim 4, characterized in that, The step of determining the approximate function as the collision avoidance path trajectory when the approximate function satisfies a preset condition includes: The ordinate values ​​of the initial function at each time point are calculated together with the ordinate values ​​of the approximation function at each time point to obtain the residual values ​​corresponding to each time point. The residual values ​​corresponding to each time point are calculated to obtain the sum of squared residual values; When the sum of squares residual value is less than a preset threshold, the approximate function is determined as the collision avoidance path trajectory.

6. The path planning and tracking control method based on ship collision avoidance according to claim 5, characterized in that, After determining the approximation function as the collision avoidance path trajectory when the approximation function meets the preset conditions, the method further includes: When the sum of squares residual value is greater than or equal to the preset threshold, the initial function is reset, and the process returns to the step of "presetting the initial function according to the changing trend of the latitude and longitude coordinates of the collision avoidance path point".

7. The path planning and tracking control method based on ship collision avoidance according to claim 1, characterized in that, The step of controlling the vessel based on the collision avoidance path trajectory to obtain the target course and target position of the vessel at various times includes: Based on the coordinate system of the ship's four-degree-of-freedom MMG model and the PID control method, a motion model is constructed to calculate the heading of the ship during navigation. The heading of the collision avoidance path trajectory at each time point is determined as the target heading; Using the motion model, the ship's current course at the current moment is turned to the target course corresponding to the current moment, and the rudder angle and course difference corresponding to the current moment are obtained; Based on the rudder angle and the heading difference, the target position corresponding to the current moment is obtained; Determine whether the collision avoidance path trajectory has a target heading corresponding to the next moment; If so, then the next moment is determined as the current moment, and the step of "using the motion model to change the current course of the ship at the current moment to the target course corresponding to the current moment, and obtain the rudder angle and course difference corresponding to the current moment" is returned; If not, then the target heading and target position of the vessel at each time point are obtained.

8. The path planning and tracking control method based on ship collision avoidance according to claim 7, characterized in that, The coordinate system of the ship's four-degree-of-freedom MMG model includes four degrees of freedom: sway, pitch, roll, and bow.

9. A path planning and tracking control device based on ship collision avoidance, characterized in that, include: The location acquisition module is used to acquire the location of the vessel, the destination, and obstacles. The path point determination module is used to improve the artificial potential field algorithm based on the position of the vessel, the position of the destination, the position of the obstacle, and the four-dimensional vessel domain, to obtain an improved artificial potential field algorithm, and output the collision avoidance path point of the vessel based on the improved artificial potential field algorithm; The path trajectory determination module is used to fit the collision avoidance path points according to the least squares method to obtain the collision avoidance path trajectory of the vessel from the destination; The ship control module is used to control the ship according to the collision avoidance path trajectory, obtain the target heading and target position of the ship at each time, and track the ship according to the target heading and target position; The artificial potential field algorithm includes a gravitational potential field function, a gravitational function, a repulsive potential field function, and a repulsive function; The improvement of the artificial potential field algorithm based on the ship's position, the destination's position, the obstacle's position, and the quaternary ship domain includes: A first distance is determined based on the positions of the vessel and the destination, and a second distance is determined based on the positions of the vessel and the obstacle. The gravitational potential field function and the gravitational function are improved based on the position of the ship, the position of the destination, and the first distance; The repulsive potential field function and the repulsive function are improved based on the position of the vessel, the position of the obstacle, the first distance, the second distance, and the quaternary vessel domain.

Images