Path planning method and system for autonomous berthing of unmanned ship

Abstract

A path planning method for autonomous berthing of an unmanned ship is characterized by comprising the following steps that S1, rasterizing is conducted on a feasible space, and each grid serves as a node; s2, setting a ship berthing position as a starting point, setting a current position as an end point, and searching through a mixed A * method to obtain a first path; s3, performing reverse operation on the first path to obtain an actual path; and S4, carrying out real-time collision detection on the actual path, and adjusting the actual path based on a collision detection result. Compared with a traditional ship docking path, the path generated by the reverse mixing A * method with the steering penalty is more suitable for actual operation requirements of the ship in a limited water area. By adding the steering penalty term, the path planning is more reasonable, unnecessary complex turning is avoided, and the applicability and safety of the path are improved. Meanwhile, the mixed A * method can reduce the search cost near a narrow parking end point.

Description

Technical Field

[0001] The present invention relates to the field of autonomous navigation control technology, and in particular to a path planning method and system for autonomous berthing of an unmanned boat. Background Art

[0002] In recent years, with the breakthroughs and progress of a series of technologies such as computer technology, communication technology, sensor technology and material technology, unmanned ship technology has also been developing continuously. Unmanned ship technology involves multiple disciplines and fields, and autonomous berthing and unberthing technology is an important research direction for unmanned ship technology. Autonomous berthing and unberthing technology is not only a key technology to realize unmanned navigation of ships, but also an important means to improve ship safety. Autonomous berthing and unberthing technology can improve the unmanned ship technology system and is of great significance to promoting the development of unmanned ship technology. The autonomous berthing and unberthing process not only controls the heading and speed of the ship, but also includes environmental perception, path planning, dynamic positioning, route tracking and many other contents.

[0003] At present, in the complex and critical scenario of ship docking, traditional path planning algorithms often seem to be unable to cope with the situation. These algorithms failed to fully incorporate the particularity of ship docking when they were designed, especially ignoring the key practical factor that the ship must ensure that its posture remains parallel to the shore when it finally docks. Therefore, even if these algorithms can generate a path from the starting point to the end point, this path often cannot meet the actual needs of the ship to dock safely and smoothly. During the docking process, the ship not only needs to accurately control its forward direction, but also needs to pay close attention to its heading angle, speed, and possible external interference to ensure that it can eventually approach and be parallel to the shore with the correct posture. Because traditional algorithms fail to fully take these complex factors into account, the paths they generate are often too idealistic, lack practicality and reliability, and are difficult to be effectively applied in actual operations. Summary of the invention

[0004] In view of the above-mentioned defects, the purpose of the present invention is to propose a path planning method and system for autonomous berthing of unmanned boats, so as to solve the problems that in the existing path planning, more resources are consumed when generating the terminal path and the applicability and safety of the path are low.

[0005] To achieve this purpose, the present invention adopts the following technical solution: a path planning method for autonomous berthing of an unmanned boat, characterized in that it includes the following steps:

[0006] Step S1: gridding the feasible space, wherein each grid is regarded as a node;

[0007] Step S2: setting the mooring position as the starting point, setting the current position as the end point, and obtaining a first path by searching through a hybrid A* method;

[0008] Step S3: Perform a reverse operation on the first path to obtain an actual path;

[0009] Step S4: Perform real-time collision detection on the actual path, and adjust the actual path based on the result of the collision detection.

[0010] Preferably, the specific steps of searching for the first path using the hybrid A* method in step S2 are as follows:

[0011] Step S21: construct a first collection and a second collection, store the starting point in the first collection, store other nodes in the second collection, and mark the starting point as the first node;

[0012] Step S22: Obtain penalty values ​​of neighboring nodes of the first node in the second collection;

[0013] Step S23: obtaining a node with the smallest penalty value as the second node, removing the second node from the second collection, and storing the second node in the first collection;

[0014] Step S24: determine whether the second node is the end point. If so, construct the first path according to the storage order of the nodes in the second collection. If not, replace the second node as the first node and re-execute steps S22 to S24.

[0015] Preferably, the penalty value in step S23 is obtained by the following formula:

[0016] f(s)=g(s)+h(s);

[0017] Where g(s) is the multi-constraint expansion cost function and h(s) is the heuristic function;

[0018]

[0019] s i is the i-th node, d(s i-1 ,s i ) represents the distance between two adjacent nodes, δ i represents the driving state at the i-th node, δ i =1 means forward travel and δ i =-1 reverse driving, w r represents the cost coefficient of reverse driving, w t The coefficient representing the steering cost, risk i represents the penalty term for the distance between the i-th node and the obstacle;

[0020] h(s)=max(h1(s i ), h2(s i ));

[0021] in

[0022] s i Indicates the state of the current node, s goal =(x goal ,y goal ,θ goal ), (x goal ,y goal ) represents the starting point, θ goal Indicates the direction of the starting point, d Reed-Shepp represents the path length calculated by the Reeds-Shepp curve, represents the set of all Reeds-Shepp curves, dx(τ) represents the length of the τth curve in the x-direction, and dy(τ) represents the length of the τth curve in the y-direction;

[0023]

[0024] in represents the set of all possible obstacle-free paths, p = {(x1,y1),(x2,y2),…,(x N ,y N )} is a discrete path, N is the total number of nodes, (x k ,y k ) represents the coordinates of the kth node.

[0025] Preferably, the unmanned boat follows the following two-dimensional kinematics:

[0026]

[0027] where k max represents the maximum curvature of the hull motion, L represents the length of the motion primitive, Δθ max It represents the maximum heading angle of the unmanned boat at the current moment, Δx represents the difference between the two nodes in the x direction, and Δy represents the difference between the two nodes in the y direction.

[0028] Preferably, before executing step S3, step A needs to be executed: obtaining the turning point and the starting point range area in the first path, and performing path optimization on the turning point and the starting point range area;

[0029] The method for identifying the turning point is as follows: obtaining the turning angle of the unmanned boat at the node, if the turning angle is greater than the set angle, it means that the node is a turning point;

[0030] The formula for obtaining the turning angle is as follows:

[0031]

[0032] Where P i+1 represents the coordinates of the next node of the i-th node, P i Represents the coordinates of the i-th node;

[0033] The specific path optimization for the turning point and the starting point range area is as follows:

[0034] Insert virtual control nodes at the turning point and the starting point range area;

[0035] After the virtual control node and the target node are inserted, a safe pipeline is generated for the path after the inserted node through the Euclidean distance information, and the subsequent path optimization is constrained not to be outside the safe pipeline.

[0036] Preferably, after executing step A, step B needs to be executed;

[0037] Wherein the step B is to use the conjugate gradient method to smooth the first path;

[0038] The conjugate gradient function is as follows:

[0039]

[0040] where |X i -O i | is the minimum obstacle distance, d min is the minimum obstacle safety distance value, ΔX i =x i -x i-1 is the vector from the i-1th node to the i-1th node, is the reciprocal of the Euclidean distance from the i-th node to the j-th target point, w o 、w κ 、w s 、w a are weight coefficients, Indicates the angle change between adjacent path points; X gj represents the coordinates of the jth target position. i Indicates the distance from the current node X i The location coordinates of the obstacle with the smallest distance.

[0041] Preferably, the specific steps of performing real-time collision detection on the actual path in step S4 are as follows:

[0042] Get the status of all static and dynamic objects at the current time;

[0043] Based on dynamic motion model v i and control input u i (t), prediction time interval T = [t0,t H) in the dynamic object trajectory;

[0044] Get the minimum Euclidean distance from the unmanned boat to each object. If the minimum Euclidean distance is 0, it means that the unmanned boat is about to collide.

[0045] Preferably, the step of adjusting the actual path based on the result of the collision detection in step S4 is specifically as follows:

[0046] If the result of the collision detection is that no collision occurs, the vehicle will continue to travel along the original actual path;

[0047] If the result of the collision detection is that a collision is about to occur, the collision time TTC is obtained;

[0048] Determine the obstacle avoidance cutoff time point of the path based on the collision time TTC and the maximum reserved obstacle avoidance time;

[0049] A series of optimized paths are generated from the obstacle avoidance cutoff time points using a quintic polynomial based on the Frenet coordinates generated by Dijkstra.

[0050] Design an evaluation function and select two optimization paths of different categories with the largest evaluation function as the second path;

[0051] Obstacle densities on the two second paths are obtained, and the second path with the smaller obstacle density is selected as the final adjustment path.

[0052] A path planning system for autonomous berthing of an unmanned boat, using the path planning method for autonomous berthing of an unmanned boat, comprising a rasterization module, a preliminary search module, a path generation module and a path local optimization module;

[0053] The rasterization module is used to rasterize the feasible space;

[0054] The preliminary search module is used to set the moored position as the starting point, the current position as the end point, and obtain the first path by searching through the hybrid A* method;

[0055] The path generation module is used to perform a reverse operation on the first path to obtain an actual path;

[0056] The path local optimization module is used to perform real-time collision detection on the actual path and adjust the actual path based on the result of the collision detection.

[0057] One of the above technical solutions has the following advantages or beneficial effects:

[0058] 1. Compared with the traditional ship docking path, the path generated by the reverse hybrid A* method with turning penalty in the present invention is more suitable for the actual operation requirements of ships in restricted waters. By adding the turning penalty term, the path planning is more reasonable, avoiding unnecessary complex turns, and improving the applicability and safety of the path. At the same time, the reverse hybrid A* method can reduce the search cost near the narrow berthing end.

[0059] 2. Inserting sequence points at the turns and end points of the generated initial path facilitates subsequent trajectory optimization to generate a more controllable path. Introducing safe pipeline constraints based on Euclidean distance can ensure path safety, and setting virtual target points can adjust the bow direction in advance for better access to berths. These designs make ship control more accurate and safe when docking.

[0060] 3. Traditional local obstacle avoidance algorithms are prone to falling into local optimality and have poor path flexibility. The proposed local trajectory repair method can avoid falling into local optimality, have more options in complex environments, and cope with a wider range of scenarios. Trajectory repair can return to the original global path, saving the cost of re-planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 is a flow chart of an embodiment of the method of the present invention.

[0062] Figure 2 It is a schematic diagram of the structure of an embodiment of the system of the present invention.

[0063] Figure 3 It is a schematic diagram of inserting a virtual control node according to an embodiment of the present invention.

[0064] Figure 4 It is a schematic diagram of generating a secure channel according to an embodiment of the present invention.

[0065] Figure 5 This is a path after smoothing according to an embodiment of the present invention.

[0066] Figure 6 It is a schematic diagram of generating an avoidance point according to an embodiment of the present invention.

[0067] Figure 7 It is a schematic diagram of generating an optimization path according to an embodiment of the present invention. DETAILED DESCRIPTION

[0068] The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals represent the same or similar elements or elements having the same or similar functions from beginning to end. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0069] In the description of the embodiments of the present invention, the terms "first" and "second" are used for descriptive purposes only and are not to be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0070] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, the meaning of "plurality" is two or more. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0071] like Figures 1 to 7 As shown, a path planning method for autonomous berthing of an unmanned boat includes the following steps:

[0072] Step S1: gridding the feasible space, wherein each grid is regarded as a node;

[0073] Step S2: setting the mooring position as the starting point, setting the current position as the end point, and obtaining a first path by searching through a hybrid A* method;

[0074] Step S3: Perform a reverse operation on the first path to obtain an actual path;

[0075] Step S4: Perform real-time collision detection on the actual path, and adjust the actual path based on the result of the collision detection.

[0076] In the present invention, the berthing position is set as the starting point, the current position is set as the end point, and then the first path is searched through the hybrid A* method. At this time, the generation order of the first path is equivalent to the reverse order of the normal path. Since the berthing position is used as the starting point at the beginning of the search, the redundant paths generated near the end point during the traditional forward search can be screened out, thereby improving the efficiency of the search. At the same time, compared with the traditional ship berthing path, the path generated by the reverse hybrid A* method with turning penalty in the present invention is more suitable for the actual operation requirements of ships in restricted waters. By adding the turning penalty item, the path planning is more reasonable, unnecessary complex turns are avoided, and the applicability and safety of the path are improved. At the same time, the hybrid A* method can reduce the search cost near the narrow berthing end point.

[0077] Preferably, the specific steps of searching for the first path using the hybrid A* method in step S2 are as follows:

[0078] Step S21: construct a first collection and a second collection, store the starting point in the first collection, store other nodes in the second collection, and mark the starting point as the first node;

[0079] Step S22: Obtain penalty values ​​of neighboring nodes of the first node in the second collection;

[0080] Step S23: obtaining a node with the smallest penalty value as the second node, removing the second node from the second collection, and storing the second node in the first collection;

[0081] Step S24: determine whether the second node is the end point. If so, construct the first path according to the storage order of the nodes in the second collection. If not, replace the second node as the first node and re-execute steps S22 to S24.

[0082] Preferably, the penalty value in step S23 is obtained by the following formula:

[0083] f(s)=g(s)+h(s);

[0084] Where g(s) is the multi-constraint expansion cost function and h(s) is the heuristic function;

[0085]

[0086] s i is the i-th node, d(s i-1 ,s i ) represents the distance between two adjacent nodes, δ i represents the driving state at the i-th node, δ i =1 means forward travel and δ i =-1 reverse driving, w r represents the cost coefficient of reverse driving, w t The coefficient representing the steering cost, risk i represents the penalty term for the distance between the i-th node and the obstacle;

[0087] In order to make the initial path planned by the unmanned boat smooth, safe, and less reversing, the constraints of the extended cost function are improved. The multi-constrained extended cost function g(s) in the present invention sets a driving state penalty term, that is, node s i-1 to i The expansion cost (cost(s i-1 ,s i )), by setting cost(s i-1 ,s i) can make the planned path less reversing and less changing of direction. This setting can effectively solve the control problems of unmanned boats caused by frequent reversing or turning in complex sea areas, and at the same time provide a suitable initial path for subsequent trajectory optimization.

[0088] h(s)=max(h1(s i ), h2(s i ));

[0089] in

[0090] s i Indicates the state of the current node, s goal =(x goal ,y goal ,θ goal ), (x goal ,y goal ) represents the starting point, θ goal Indicates the direction of the starting point, d Reed-Shepp represents the path length calculated by the Reeds-Shepp curve, represents the set of all Reeds-Shepp curves, dx(τ) represents the length of the τth curve in the x-direction, and dy(τ) represents the length of the τth curve in the y-direction;

[0091] The first heuristic function h1(st i ) Considering the non-completeness constraints of the unmanned boat, ignoring obstacles, its value is to satisfy the current node (x, y, θ) to reach the target point (x goal ,y goal ,θ goal ) is obtained by the Reeds-Shepp curve. The curve calculates the distance from the starting point to any point offline and obtains the heuristic value from any point to the target point by rotating and translating the real-time 3D vector.

[0092]

[0093] in represents the set of all possible obstacle-free paths, p = {(x1,y1),(x2,y2),…,(x N ,y N )} is a discrete path, N is the total number of nodes, (x k ,y k ) represents the coordinates of the kth node.

[0094] The second heuristic function h2(s i ) is obtained by considering the obstacle environment and ignoring the kinematics of the unmanned vehicle.

[0095] Preferably, the unmanned boat follows the following two-dimensional kinematics:

[0096]

[0097] where k max represents the maximum curvature of the hull motion, L represents the length of the motion primitive, Δθ max It represents the maximum heading angle of the unmanned boat at the current moment, Δx represents the difference between the two nodes in the x direction, and Δy represents the difference between the two nodes in the y direction.

[0098] The two-dimensional kinematics defines six directions in which a node expands when searching for a second node, namely, front and back, left front turn, right front turn, left rear turn, and right rear turn.

[0099] Preferably, before executing step S3, step A needs to be executed: obtaining the turning point and the starting point range area in the first path, and performing path optimization on the turning point and the starting point range area;

[0100] The method for identifying the turning point is as follows: obtaining the turning angle of the unmanned boat at the node, if the turning angle is greater than the set angle, it means that the node is a turning point;

[0101] The formula for obtaining the turning angle is as follows:

[0102]

[0103] Where P i+1 represents the coordinates of the next node of the i-th node, P i Represents the coordinates of the i-th node;

[0104] The specific path optimization for the turning point and the starting point range area is as follows:

[0105] like Figure 3 As shown, virtual control nodes are inserted at the turning point and the starting point range area;

[0106] like Figure 4 As shown in FIG. 5 , after the virtual control node is inserted, a safe pipeline is generated for the path after the node is inserted through the Euclidean Signed Distance Field (ESDF) information, which constrains the subsequent path optimization not to be outside the safe pipeline.

[0107] For the first generated path, the control node at the bend can generate a path that is more conducive to control in the subsequent trajectory optimization. The introduction of a safe pipeline constraint based on Euclidean distance can ensure the safety of the path. For the ship docking scenario, the virtual target point introduced can adjust the bow direction in advance so that it can enter the berth better. Compared with traditional path planning methods, these designs make the control of the ship docking more accurate and safe.

[0108] Preferably, after executing step A, step B needs to be executed;

[0109] Wherein the step B is to use the conjugate gradient method to smooth the first path;

[0110] The conjugate gradient function is as follows:

[0111]

[0112] where |X i -O i | is the minimum obstacle distance, d min is the minimum obstacle safety distance value, ΔX i =x i -x i-1 is the vector from the i-1th node to the i-1th node, is the reciprocal of the Euclidean distance from the i-th node to the j-th target point, w o 、w κ 、w s 、w a are weight coefficients, Indicates the angle change between adjacent path points; X gj represents the coordinates of the jth target position. i Indicates the distance from the current node X i The location coordinates of the obstacle with the smallest distance.

[0113] like Figure 5 As shown in the figure, the dotted line points are the path trajectories after smoothing. In the conjugate gradient function, the first item is the obstacle distance penalty item, which guides the unmanned boat to stay away from obstacles in various scenarios; the second item is the curvature change penalty item, which avoids large turns in the path; the third item is the smoothing item, which ensures the smoothness of the path; the fourth item is the inverse item of the end point distance; the fourth item is the end point gravity item. As the ship's posture at the berth needs to be parallel to the shore, adding the attraction item of the virtual end point is conducive to making the path bow adjustment in advance, which is more reasonable during the berthing process.

[0114] Preferably, the specific steps of performing real-time collision detection on the actual path in step S4 are as follows:

[0115] Get the status of all static and dynamic objects at the current time;

[0116] Based on dynamic motion model v i and control input u i (t), prediction time interval T = [t0,t H ) in the dynamic object trajectory; when performing dynamic object trajectory, a simple motion model can be created, the input is u(t) and the object state at time t (posture, shape), and the output is the derivative of the object state, i.e., velocity. By integrating it, the object trajectory within the predicted time interval can be predicted based on the state at this time. i (t) is the control input function, which can be set to a constant value by assuming that the dynamic obstacle follows a simple uniform linear motion.

[0117] Get the minimum Euclidean distance from the unmanned boat to each object. If the minimum Euclidean distance is 0, it means that the unmanned boat is about to collide.

[0118] After obtaining the trajectory of the dynamic object, the minimum Euclidean distance between it and the actual trajectory can be obtained. When the minimum Euclidean distance is 0, it means that there is a collision between the two dynamic object trajectories and the actual path. Similarly, since static objects do not move, it is only necessary to obtain the minimum Euclidean distance between the static object and the actual path to determine whether a collision may occur.

[0119] Preferably, the step of adjusting the actual path based on the result of the collision detection in step S4 is specifically as follows:

[0120] If the result of the collision detection is that no collision occurs, the vehicle will continue to travel along the original actual path;

[0121] If the result of the collision detection is that a collision is about to occur, the collision time TTC is obtained;

[0122] Since the position at the minimum Euclidean distance is known, the collision time TTC is immediately obtained through the current position, the position at the minimum Euclidean distance and the speed of the unmanned boat;

[0123] Since the unmanned boat needs to leave time for response (maximum reserved obstacle avoidance time) when changing the route and operating, the obstacle avoidance cutoff time point can be obtained by subtracting the maximum reserved obstacle avoidance time from the collision time TTC. It is worth mentioning that when the collision time TTC minus the maximum reserved obstacle avoidance time is a negative number, the current time can be regarded as the obstacle avoidance cutoff time point t cut .

[0124] A series of optimized paths are generated from the obstacle avoidance cutoff time points using a quintic polynomial based on the Frenet coordinates generated by Dijkstra.

[0125] In the Frenet coordinate system, the motion of the unmanned boat is decomposed into lateral motion s(t) along the curve and longitudinal motion d(t) perpendicular to the curve. The Dijkstra algorithm is used to create a path assuming that there are no new obstacles, and determine the obstacle avoidance cutoff time point t from which the trajectory needs to be repaired. cut The optimized path from the corresponding pose to the target area (next node). When generating the optimized path, the path is the longitudinal axis of the Frenet coordinates. A series of obstacle avoidance points are generated at both ends of the obstacle, such as Figure 6 As shown, it is prepared for the subsequent generation of optimized paths at both ends of the obstacle avoidance point. The generation rules of the obstacle avoidance point are as follows:

[0126] Avoidance point[s a ,d a ] T is from the first abnormal point (such as the first collision point with an obstacle) [s v ,d v ] T The specific formula is as follows:

[0127] s a =s v ;

[0128]

[0129] Among them, d min and d max are the minimum and maximum permissible lateral displacement, n d is the number of path candidates, which can be specified according to the width of the unmanned boat ω, for example s v , d v are the lateral and longitudinal motions of the first collision point, s a , d a They are the lateral and longitudinal movements of the avoidance point respectively.

[0130] In order to connect the position of the obstacle avoidance cutoff time point to the target area through a quintic polynomial, 12 unknown coefficients need to be determined; the polynomial forms of the longitudinal motion s(t) and the lateral motion d(t) are as follows:

[0131]

[0132] The remaining time from the obstacle avoidance cutoff point to the planned target area. h is the total time range of the planning time. The boundary conditions, i.e., the cutoff and target states, can be obtained by coordinate transformation, and the following coefficients can be directly determined:

[0133] Initial state boundary conditions (cut-off state):

[0134] For s(t): position: s(0) = s cut ;speed: Acceleration: s cut is the lateral motion at the obstacle avoidance cutoff time point, d cut Longitudinal motion at the time point of obstacle avoidance cutoff;

[0135] For d(t): position: d(0) = d cut ;speed: Acceleration:

[0136] Target state boundary conditions:

[0137] For s(t): Position: speed: Acceleration:

[0138] For d(t): Position: speed: Acceleration:

[0139] s g and d g lateral and longitudinal movements when reaching the target area, respectively;

[0140] Initial state boundary condition coefficients:

[0141] At t = 0:

[0142] s(0)=α5*0 5 +α4*0 4 +α3*0 3 +α2*0 2 +α1*0+α0=s cut

[0143] d(0)=β5*0 5 +β4*0 4 +β3*0 3 +β2*0 2 +β1*0+β0=d cut

[0144] Similarly, the velocity and acceleration of the initial state can be obtained:

[0145]

[0146] Target state boundary condition coefficient:

[0147] exist hour:

[0148]

[0149] Given that α0 = s cut , We can get:

[0150]

[0151] Similarly, the speed of the target state and the position of the obstacle avoidance point can be obtained:

[0152]

[0153] Using these conditions, the equations are organized into matrix form and solved for α3, α4, α5; β3, β4, β5.

[0154]

[0155]

[0156] in is the time at the avoidance point. By solving the polynomial parameters corresponding to each obstacle avoidance point, the corresponding curve can be drawn. Figure 7 shown.

[0157] Design an evaluation function and select two optimization paths of different categories with the largest evaluation function as the second path;

[0158] In the present invention, the evaluation function is specifically as follows:

[0159]

[0160] The first and second terms (integrated longitudinal acceleration and lateral acceleration) are used to ensure the smoothness of the path and reduce sharp acceleration and turning to improve the comfort and stability of the unmanned boat. p(π) is expressed as the inverse of the distance between each obstacle avoidance point and its smallest obstacle, and ω1, ω2, and ω3 are weights that need to be manually adjusted.

[0161] In the present invention, not only one optimized path is obtained, but two optimized paths of different wheel types are selected. The paths of different wheel types cannot be continuously deformed without passing through obstacles, so this can deal with unexpected events in dynamic scenes. If only one path is generated, it cannot be adjusted immediately in the cutoff state when the situation occurs and needs to be replanned. Generating two paths of different types can deal with this situation.

[0162] Obstacle densities on the two second paths are obtained, and the second path with the smaller obstacle density is selected as the final adjustment path.

[0163] To prevent falling into the local optimum, the two different paths generated at both ends can be switched to another different path when more obstacles are detected on one side, thereby jumping out of the local optimum. The formula for obtaining the obstacle density is as follows:

[0164]

[0165] μ can be calculated from the data of the laser radar. Where L represents the amount of data obtained by the full scan of the laser radar, and dr represents the measurement radius of the radar. When there is no obstacle, dj = dr. If there is an obstacle, dj is the distance between the unmanned boat and the obstacle. The smaller the μ value, the lower the obstacle density, and the larger the μ value, the higher the obstacle density.

[0166] A path planning system for autonomous berthing of an unmanned boat, using the path planning method for autonomous berthing of an unmanned boat, comprising a rasterization module, a preliminary search module, a path generation module and a path local optimization module;

[0167] The rasterization module is used to rasterize the feasible space;

[0168] The preliminary search module is used to set the moored position as the starting point, the current position as the end point, and obtain the first path by searching through the hybrid A* method;

[0169] The path generation module is used to perform a reverse operation on the first path to obtain an actual path;

[0170] The path local optimization module is used to perform real-time collision detection on the actual path and adjust the actual path based on the result of the collision detection.

[0171] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "illustrative embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0172] Although the embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the claims and their equivalents.

Claims

1. A path planning method for autonomous berthing of an unmanned boat, characterized in that: The steps include: Step S1: gridding the feasible space, wherein each grid is regarded as a node; Step S2: setting the mooring position as the starting point, setting the current position as the end point, and obtaining a first path by searching through a hybrid A* method; Step S3: Perform a reverse operation on the first path to obtain an actual path; Step S4: Perform real-time collision detection on the actual path, and adjust the actual path based on the result of the collision detection.

2. A path planning method for autonomous berthing of an unmanned boat according to claim 1, characterized in that: The specific steps of the hybrid A* method in step S2 to search and obtain the first path are as follows: Step S21: construct a first collection and a second collection, store the starting point in the first collection, store other nodes in the second collection, and mark the starting point as the first node; Step S22: obtaining penalty values ​​of neighboring nodes of the first node in the second collection; Step S23: obtaining a node with the smallest penalty value as the second node, removing the second node from the second collection, and storing the second node in the first collection; Step S24: determine whether the second node is the end point. If so, construct the first path according to the storage order of the nodes in the second collection. If not, replace the second node as the first node and re-execute steps S22 to S24.

3. A path planning method for autonomous berthing of an unmanned boat according to claim 2, characterized in that: The penalty value in step S23 is obtained by the following formula: f(s)=g(s)+h(s); Where g(s) is the multi-constraint expansion cost function and h(s) is the heuristic function; s i is the i-th node, d ( s i-1 ,s i) represents the distance between two adjacent nodes, δ i represents the driving state at the i-th node, δ i =1 means forward travel and δ i =-1 reverse driving, w r represents the cost coefficient of reverse driving, w t The coefficient representing the steering cost, risk i represents the penalty term for the distance between the i-th node and the obstacle; h(s)=max(h1(s i ),h2(s i )); in s i Indicates the state of the current node, s goal =(x goal ,y goal ,θ goal ), (x goal ,y goal ) represents the starting point, θ goal Indicates the direction of the starting point, d Reed-Shepp represents the path length calculated by the Reeds-Shepp curve, represents the set of all Reeds-Shepp curves, represents the length of the τth curve in the X direction, represents the length of the τth curve in the y direction; in represents the set of all possible obstacle-free paths, p = { ( x1,y 1) , ( x2,y 2) ,…,(x N ,y N )} is a discrete path, N is the total number of nodes, ( x k ,y k) Represents the coordinates of the kth node.

4. A path planning method for autonomous berthing of an unmanned boat according to claim 3, characterized in that: The unmanned boat follows the following two-dimensional kinematics: where k max represents the maximum curvature of the hull motion, L represents the length of the motion primitive, Δθ max It represents the maximum heading angle of the unmanned boat at the current moment, Δx represents the difference between the two nodes in the x direction, and Δy represents the difference between the two nodes in the y direction.

5. The path planning method for autonomous berthing of an unmanned boat according to claim 1, characterized in that: Before executing step S3, step A needs to be executed: obtaining the turning point and the starting point range area in the first path, and performing path optimization on the turning point and the starting point range area; The method for identifying the turning point is as follows: obtaining the turning angle of the unmanned boat at the node, if the turning angle is greater than the set angle, it means that the node is a turning point; The formula for obtaining the turning angle is as follows: Where P i+1 represents the coordinates of the next node of the i-th node, P i Represents the coordinates of the i-th node; The specific path optimization for the turning point and the starting point range area is as follows: Insert virtual control nodes at the turning point and the starting point range area; After the virtual control node is inserted, a safe pipeline is generated for the path after the node is inserted through the Euclidean distance information, which constrains the subsequent path optimization not to be outside the safe pipeline.

6. A path planning method for autonomous berthing of an unmanned boat according to claim 5, characterized in that: After executing step A, you also need to execute step B; Wherein the step B is to use the conjugate gradient method to smooth the first path; The conjugate gradient function is as follows: in | X i -O i| is the minimum obstacle distance, d min is the minimum obstacle safety distance value, ΔX i =x i -x i-1 is the vector from the i-1th node to the i-1th node, is the reciprocal of the Euclidean distance from the i-th node to the j-th target point, w o 、w κ 、w s 、w a are weight coefficients, Indicates the angle change between adjacent path points; X gj represents the coordinates of the jth target position, O i Indicates the distance from the current node X i The location coordinates of the obstacle with the smallest distance.

7. The path planning method for autonomous berthing of an unmanned boat according to claim 1, characterized in that: The specific steps of performing real-time collision detection on the actual path in step S4 are as follows: Get the status of all static and dynamic objects at the current time; Based on dynamic motion model v i and control input u i (t), prediction time interval T = [t0,t H ) in the dynamic object trajectory; Get the minimum Euclidean distance from the unmanned boat to each object. If the minimum Euclidean distance is 0, it means that the unmanned boat is about to collide.

8. The path planning method for autonomous berthing of an unmanned boat according to claim 1, characterized in that: The steps of adjusting the actual path based on the result of the collision detection in step S4 are specifically as follows: If the result of the collision detection is that no collision occurs, the vehicle will continue to travel along the original actual path; If the result of the collision detection is that a collision is about to occur, the collision time TTC is obtained; Determine the obstacle avoidance cutoff time point of the path based on the collision time TTC and the maximum reserved obstacle avoidance time; A series of optimized paths are generated from the obstacle avoidance cutoff time points using a quintic polynomial based on the Frenet coordinates generated by Dijkstra. Design an evaluation function and select two optimization paths of different categories with the largest evaluation function as the second path; Obstacle densities on the two second paths are obtained, and the second path with the smaller obstacle density is selected as the final adjustment path.

9. A path planning system for autonomous berthing of an unmanned boat, using a path planning method for autonomous berthing of an unmanned boat as claimed in any one of claims 1 to 8, characterized in that: It includes a rasterization module, a preliminary search module, a path generation module, and a path local optimization module; The rasterization module is used to rasterize the feasible space; The preliminary search module is used to set the moored position as the starting point, the current position as the end point, and obtain the first path by searching through the hybrid A* method; The path generation module is used to perform a reverse operation on the first path to obtain an actual path; The path local optimization module is used to perform real-time collision detection on the actual path and adjust the actual path based on the result of the collision detection.

Images