A berthing path planning method for unmanned boat based on Bezier curve

Abstract

The present invention discloses a berthing path planning method for an unmanned boat based on a Bezier curve. Firstly, whether berthing can be started and specific information of a target berth are determined based on perception information. Then, a starting control point of the Bezier curve is set based on the above information. Then, the remaining unknown control points are calculated in combination with the characteristics of the Bezier curve and the kinematic characteristics of the unmanned boat navigation. Finally, after all control points are determined, a berthing path planned based on the Bezier curve can be obtained. This berthing path planning method based on the Bezier curve can effectively reduce the control difficulty and is convenient for considering constraint conditions that adapt to the motion characteristics of the unmanned boat.

Description

Technical Field

[0001] The present invention relates to the technical field of route planning, and in particular to a method for planning a berthing path for an unmanned boat based on a Bezier curve. Background Art

[0002] Autonomous berthing is the "last mile" to achieve autonomous navigation, and is of great significance for achieving long-distance autonomous navigation from berth to berth. Existing trajectory planning methods based on Bezier curves are mostly used in the field of unmanned vehicles. Such methods only consider the strict curvature continuity of the vehicle's dynamic constraints and the fitted Bezier curve, and do not consider the effect of large inertia. The difficulty of unmanned boat berthing is: (1) there are many constraints, the berthing information is complex and changeable, and the vehicle berth generally has a relatively standardized size and shape; (2) the final expected speed of berthing is 0kn, at this time the speed is low and the steering effect is poor, and the control difficulty is large. In contrast, the vehicle can still have better control ability at low speed; (3) it relies on inertial navigation or Beidou and other equipment for positioning and navigation, which has its own errors; (4) the berth is small and requires high control accuracy. Existing berthing path planning methods have problems such as curvature discontinuity, which will lead to angular velocity discontinuity. The angular velocity of the unmanned boat cannot jump, so it is easy to produce tracking errors, which will cause the unmanned boat to deviate from the expected route. Studies have shown that Bezier curves have the characteristics of curvature continuity and easy consideration of constraints. Therefore, a berthing path planning method based on Bezier curves for unmanned boats is studied to reduce the control difficulty of unmanned boat berthing. Summary of the invention

[0003] The purpose of the present invention is to ensure that the unmanned boat can safely and effectively plan its path when berthing, and provide a path suitable for control.

[0004] The technical solution of the present invention is to provide a method for planning a berthing path of an unmanned boat based on a Bezier curve, the method comprising the following steps:

[0005] S1. Plan the first Bezier curve according to the berth information, and set the desired parking position in the berth as the control point P4. The position coordinates of the control point P4 are (x p4 ,y p4 ), take the control point P4 as the starting point and take a distance from the berth to the outside to set it as the control point P3. The position coordinates of the control point P3 are (x g ,y g ), control point P3 and control point P4 form a first-order Bezier curve, which can be expressed as

[0006] S2. Set the initial position of the unmanned boat to the control point P0, and the position coordinates of the control point P0 are (x0, y0); plan the second Bezier curve and calculate the remaining control points P1 and P2 required, where the position coordinates of the control point P1 are expressed as (x p1 ,y p1 ), the position coordinates of the control point P2 are expressed as (x p2 ,y p2 ), the third-order Bezier curve uniquely determined by the control points P0 to P3 is expressed as:

[0007]

[0008] Calculate the kinematic constraints of the unmanned boat ρ(t) ≥ ρ min The position coordinates of the control points P1 and P2, where ρ(t) is the radius of curvature of any third-order Bezier curve, ρ min is the minimum turning radius of the unmanned boat at the corresponding position;

[0009] S3. Determine whether the calculation of control points P1 and P2 has been completed. If not, return to step S2. If completed, form a second segment of third-order Bezier curve with control points P0 to P3. At this point, the path planning is completed. The Bezier curves obtained in step S1 and step S3 are combined to form the planned path.

[0010] In any of the above technical solutions, further, in step S1, the distance between the control point P4 and the control point P3 is one time the ship length.

[0011] In any of the above technical solutions, further, calculating the position coordinates of the control point P1 and the control point P2 in step S2 specifically includes:

[0012] S21. Find the relationship between the position coordinates of the control points P1 and P2 and the position coordinates of the control points P0 and P3, and find the second-order derivative of the third-order Bezier curve expression:

[0013]

[0014] When the unmanned boat is at the control point P0, it has not started to steer. When it is at the control point P3, the rudder angle is restored to zero. The drift angles at both points are 0, and the speed direction is consistent with the heading. Therefore, the starting point P0 and the end point P3 of the third-order Bezier curve are subject to non-holonomic constraints:

[0015]

[0016] The control points P1 and P2 that satisfy the above constraints are expressed as:

[0017]

[0018] Among them, ψ0 is the heading of the unmanned boat when it starts to berth, ψ g is the bow direction of the unmanned boat when it is moored at the berth, d1 is the distance between control point P0 and control point P1, and d2 is the distance between control point P2 and control point P3;

[0019] S22. Calculate d1 and d2, and express the radius of curvature of any point of the third-order Bezier curve as:

[0020]

[0021] Discretize d1 and d2, determine the initial value and discrete interval, and determine the discrete interval and number of iterations of the curve parameter variable t. Calculate ρ(t) corresponding to each d1, d2 and t in turn according to the ρ(t) calculation formula to determine whether ρ(t) satisfies the condition ρ(t)≥ρ min If the conditions are met, the inner loop enters the next iteration, the number of iterations increases by 1, and the corresponding discrete interval is added to t. If the conditions are not met, the number of iterations is not counted. After the inner loop calculation reaches the upper limit of the number of iterations, it enters the middle loop. In the middle loop, d2 is gradually increased to the upper limit according to the discrete interval. For each d2, the corresponding inner loop calculation is performed. After the calculation of d2 in the middle loop is completed, the outermost loop is entered to perform the iterative calculation of d1. In the outermost loop, d1 is gradually increased to the upper limit according to the discrete interval, and the corresponding middle and inner loop calculations are performed for each d1.

[0022] After the inner, middle and outermost loops are completed, d1 and d2 with sufficient accuracy are obtained, and the position coordinates of control points P1 and P2 can be determined based on control points P0, d1, control points P3 and d2.

[0023] In any of the above technical solutions, further, the first-order Bezier curve obtained in step S1 and the third-order Bezier curve obtained in step S3 are connected at the control point P3.

[0024] In any of the above technical solutions, further, the selection of control points P1 and P2 in step S2 cannot exceed the scope of the harbor waters.

[0025] In any of the above technical solutions, further, before step S1, it also includes:

[0026] S0. Confirm the initial position of the unmanned boat, sense the navigation status of other ships in the berthing area through the laser radar, and continue when it is determined that there are no obstacles in the berthing area, otherwise terminate the planning and continue to wait; determine the berth information based on the laser radar, cluster the point cloud information returned by the laser radar, and obtain the location information of the berth.

[0027] The beneficial effects of the present invention are:

[0028] 1) The Bezier curve is used in the berthing path planning of the unmanned boat to make the planned berthing path smoother. In the process of calculating the control points, the unmanned boat in the water cannot quickly change the direction of the front wheels by rotating the body relative to the rear wheels through the friction between the tires and the ground like a land vehicle, so that the fitting parking curve is easier to meet the parking conditions of the car. Compared with the unmanned boat, the method of the present invention proposes to meet the kinematic constraint condition ρ(t)≥ρ in the water min , that is, the radius of curvature of any point of the generated third-order Bezier curve must be greater than or equal to the minimum turning radius of the unmanned boat at the corresponding position, which effectively reduces the control difficulty of the unmanned boat berthing condition and improves the berthing success rate and efficiency;

[0029] 2) The method of the present invention also combines specific berth information to calculate control points, dividing the path into two parts, one of which uses the desired parking position control point P4 and another control point P3 calculated from the control point P4 to form a first-order Bezier curve, avoiding frequent steering and low-speed control difficulties caused by tracking the Bezier curve, while taking into account the buffer distance required for parking. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] The advantages of the above and additional aspects of the present invention will become apparent and easily understood in the description of the embodiments in conjunction with the following drawings, in which:

[0031] Figure 1 is a schematic diagram of path planning of an unmanned boat berthing path planning method based on Bezier curve according to an embodiment of the present invention;

[0032] Figure 2 is a flow chart of a method for planning a berthing path for an unmanned boat based on a Bezier curve according to an embodiment of the present invention;

[0033] Figure 3 The figure is a schematic diagram of an example calculation of a method for planning a berthing path of an unmanned boat based on a Bezier curve according to an embodiment of the present invention. DETAILED DESCRIPTION

[0034] In order to more clearly understand the above-mentioned purpose, features and advantages of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that the embodiments of the present invention and the features in the embodiments can be combined with each other without conflict.

[0035] In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those described herein. Therefore, the protection scope of the present invention is not limited to the specific embodiments disclosed below.

[0036] like Figure 1 and Figure 2 As shown, this embodiment provides a method for planning a berthing path for an unmanned boat based on a Bezier curve, the method comprising:

[0037] S0. Confirm the initial position of the unmanned boat, and use the laser radar to sense the navigation status of other ships in the berthing area. If it is determined that there are no obstacles in the berthing area, continue to proceed, otherwise terminate the planning and continue to wait.

[0038] Among them, taking into account the uncertainty of the position of the unmanned boat entering the port, it is currently difficult to avoid collisions in the harbor, and it is necessary to stop the ship to observe and wait until it is safe before berthing. The position where the ship is stopped for observation is the initial position of the unmanned boat; at the initial position, the unmanned boat uses lidar to scan the berthing area. When the radar senses other ships or obstacles sailing in the berthing area, the unmanned boat continues to wait at the initial position until it can be safely moored, and then continues to plan the path.

[0039] The berth information is determined based on the laser radar, and the point cloud information returned by the laser radar is clustered to obtain the location information of the berth.

[0040] Planning a Bezier curve requires determining the control points of the curve, where the beginning and end of the path are each a control point, so the specific location information of the berth is required.

[0041] S1. Plan the first Bezier curve according to the berth information, such as Figure 1 As shown in the figure, the desired parking position in the berth is set as the control point P4, and the position coordinates of the control point P4 are (x p4 ,y p4 ), take control point P4 as the starting point and set the position one time the ship length outward from the berth as control point P3. The position coordinates of control point P3 are (x g ,y g ), control point P3 and control point P4 form a first-order Bezier curve, which can be expressed as

[0042] Specifically, according to experience, the terminal speed of the unmanned boat when it reaches the berth is 1kn, and the rudder effect is almost zero at this time. In order to avoid the frequent ruddering and low-speed control problems caused by the tracking of the Bezier curve, and considering the buffer distance required for parking, a first-order Bezier curve is formed perpendicular to the berth with control point P4 as the starting point, and the other end of the curve is control point P3. In this way, the first-order Bezier curve formed by control points P3 and P4, that is, the straight line P3P4, does not need to frequently adjust the bow direction for tracking control, and the speed adjustment is also relatively easy, and the control difficulty is relatively low.

[0043] S2. Set the initial position of the unmanned boat to the control point P0, and the position coordinates of the control point P0 are (x0, y0). Plan the second Bezier curve and calculate the required control points P1 and P2, where the position coordinates of the control point P1 are expressed as (x p1 ,y p1 ), the position coordinates of the control point P2 are expressed as (x p2 ,y p2 ), the specific calculation steps include:

[0044] S21. The third-order Bezier curve uniquely determined by the control points P0 to P3 is expressed as:

[0045]

[0046] Taking the second-order derivative of the above formula, we get:

[0047]

[0048] When the unmanned boat is at the control point P0, it has not started to steer. When it is at the control point P3, the rudder angle is restored to zero. The drift angles at both points are 0, and the speed direction is consistent with the heading. Therefore, the starting point P0 and the end point P3 of the third-order Bezier curve are subject to non-holonomic constraints:

[0049]

[0050] The control points P1 and P2 that satisfy the above constraints are expressed as:

[0051]

[0052] Among them, ψ0 is the heading of the unmanned boat when it starts to berth, ψ g is the bow direction of the unmanned boat when it is moored at the berth, that is, the opposite direction of the berth opening, d1 is the distance between control point P0 and control point P1, and d2 is the distance between control point P2 and control point P3. S22. Calculate the curvature radius of any point of the second segment of the third-order Bezier curve:

[0053]

[0054] Considering the motion characteristics of the unmanned boat, ρ(t) should satisfy ρ(t)≥ρ min , where ρ min It is the minimum turning radius of the unmanned boat.

[0055] Specifically, considering that the curve has a convex hull, the Bezier curve will always be inside the smallest convex polygon that includes all control points, so that the corresponding limit ranges of d1 and d2 can be obtained.

[0056] Discretize d1 and d2, select appropriate initial values ​​and discrete intervals, and determine the discrete interval and number of iterations of the curve parameter variable t. Calculate ρ(t) corresponding to each d1, d2 and t in turn according to the ρ(t) calculation formula to determine whether ρ(t) satisfies the condition ρ(t)≥ρ min If the condition is met, the inner loop enters the next iteration, the number of iterations increases by 1, and the corresponding discrete interval is increased for t. If the condition is not met, the number of iterations is not counted.

[0057] After the inner loop calculation reaches the upper limit of the number of iterations, it enters the middle loop. In the middle loop, d2 is gradually increased to the upper limit according to discrete intervals, and the corresponding inner loop calculation is performed for each d2. After the calculation of d2 in the middle loop is completed, it enters the outermost loop for iterative calculation of d1. In the outermost loop, d1 is gradually increased to the upper limit according to discrete intervals, and the corresponding middle and inner loop calculations are performed for each d1. After the inner, middle and outermost loops are completed, d1 and d2 with sufficient accuracy are obtained, and the control point P1 and control point P2 can be determined based on the control point P0, d1, control point P3 and d2.

[0058] From the definition of the third-order Bezier curve and the calculation formula of ρ(t), we know that the control point has a great influence on the curvature of the curve segment near it. Considering that the task of heading adjustment should not be completed when approaching the target berth, the curvature of the curve near the end, that is, the control point P0 and the control point P3 should not be too large, so d1 and d2 should be appropriately small. Therefore, the values ​​of d1 and d2 should vary from small to large.

[0059] S3. Determine whether the calculation of control points P1 and P2 has been completed. If not, return to step S2. If completed, form a second segment of third-order Bezier curve with control points P0 to P3. At this point, the path planning is completed. The Bezier curves obtained in step S1 and step S3 are combined to form the planned path.

[0060] In another embodiment of the present invention, the above method is actually tested at a fixed berth at a certain port.

[0061] like Figure 3 As shown, the berth information obtained is as follows: the fixed berth is a rectangular berth with a length of 10m×10m and one side open. The positions of the four vertices are described by longitude and latitude coordinates, and points are numbered 0, 1, 2, and 3 counterclockwise starting from the open end, as shown in Table 1 below.

[0062] Table 1 Berth vertex coordinates

[0063] Vertex Number longitude latitude 0 119.554292 35.402221 1 119.554273 35.402310 2 119.554165 35.402294 3 119.554183 35.402205

[0064] In addition, the unmanned boat is 15m long and 4.8m wide. The midpoint of the line connecting vertices 0 and 3 is taken as control point P4, which can ensure that the unmanned boat is finally moored in the center of the berth, while maintaining a gap of 2.6m on the left and right, and a gap of 2.5m in front. Then, take control point P4 as the starting point and take a distance of one ship length vertically outward from the berth, that is, 30m away, as control point P3.

[0065] In this berthing process, the expected speed of the unmanned boat in the straight section is 1 kn, the initial state of the berthing condition is (119.555999E, 35.400849N, 312.17°, 0 kn), the control point P0 is on the coordinates of this initial state, the target state is (119.554236E, 35.402212N, 350.13°, 1 kn), and the control point P3 is on the coordinates of this target state. The expected speed of the unmanned boat in the curved section is 3 kn, and the maximum rudder angle is 15°. Substituting it into the control response model obtained by parameter identification, the minimum turning diameter is 24.76 m, that is, the minimum curvature radius ρ min =12.38m. Then, the iterative algorithm is applied to find d1 and d2. Considering that the control points of the Bezier curve limit the curve to the inside of the convex polygon, the selection of control points cannot exceed the scope of the harbor water area. The longest longitudinal side of the harbor water area is 227m, so 0<d1≤227m, 0<d2≤227m; the discretization interval of d1 is 5m, that is, the values ​​of d1 are 5, 10, 15…225m. In order to reduce the number of cycles, the discretization interval of d2 is 10m, that is, the values ​​of d2 are 10, 20, 30…220m.

[0066] The value of t is discretized into 1000 parts, that is, the value of t is 0.001, 0.002, 0.003...1, and then the ρ(t) corresponding to each d1, d2 and t is calculated in turn according to the ρ(t) calculation formula to determine whether ρ(t) satisfies the kinematic constraint condition of the unmanned boat ρ(t)≥ρ min If the condition is met, the inner loop enters the next iteration, the number of iterations increases by 1, and t increases by 0.001. If the condition is not met, the number of iterations is not counted.

[0067] Output the final d1 and d2, write the coordinates of control points P1 and P2, use the obtained control points P0 to P3 to substitute into the third-order Bezier curve, and control points P3 and P4 to substitute into the first-order Bezier curve, combine the two curves, and get the required berthing path.

[0068] The technical solution of the present invention is described in detail above in conjunction with the accompanying drawings. The present invention proposes a method for planning a berthing path for an unmanned boat based on a Bezier curve. After determining the control point P4, the control point P3 is obtained, and then the control points P1 and P2 are calculated through the control points P0 and P3.

[0069] A first-order Bezier curve is planned through two control points P3 and P4, and a third-order Bezier curve is planned through four control points P0 to P3. The combination of the two curves is the berthing path.

[0070] The steps in the present invention can be adjusted in order, combined, or deleted according to actual needs.

[0071] Although the present invention is disclosed in detail with reference to the accompanying drawings, it should be understood that the above embodiments are only for illustrating the technical ideas of the present invention and cannot be used to limit the scope of protection of the present invention. The scope of protection of the present invention is defined by the appended claims and may include various modifications, alterations and equivalents made to the invention without departing from the scope and spirit of the present invention.

Claims

1. A method for planning the berthing path of an unmanned boat based on Bezier curve, characterized in that: The steps of the method include: S1. Plan the first Bezier curve according to the berth information, and set the desired parking position in the berth as the control point P4. The position coordinates of the control point P4 are (x p4 ,y p4 ), take the control point P4 as the starting point and take a distance from the berth to the outside to set it as the control point P3. The position coordinates of the control point P3 are (x g ,y g ), control point P3 and control point P4 form a first-order Bezier curve, which can be expressed as S2. Set the initial position of the unmanned boat to the control point P0, and the position coordinates of the control point P0 are (x0, y0); plan the second Bezier curve and calculate the remaining control points P1 and P2 required, where the position coordinates of the control point P1 are expressed as (x p1 ,y p1 ), the position coordinates of the control point P2 are expressed as (x p2 ,y p2 ), the third-order Bezier curve uniquely determined by the control points P0 to P3 is expressed as: Calculate the kinematic constraints of the unmanned boat ρ(t) ≥ ρ min The position coordinates of the control points P1 and P2, where ρ(t) is the radius of curvature of any third-order Bezier curve, ρ min is the minimum turning radius of the unmanned boat at the corresponding position; S3, determine whether the calculation of control points P1 and P2 is completed. If not, return to step S2. If completed, form a second segment of third-order Bezier curve with control points P0 to P3. At this point, the path planning is completed. The Bezier curves obtained in step S1 and step S3 are combined to form the planned path. Calculating the position coordinates of the control point P1 and the control point P2 in step S2 specifically includes: S21. Find the relationship between the position coordinates of the control points P1 and P2 and the position coordinates of the control points P0 and P3, and find the second-order derivative of the third-order Bezier curve expression: When the unmanned boat is at the control point P0, it has not started to steer. When it is at the control point P3, the rudder angle is restored to zero. The drift angles at both points are 0, and the speed direction is consistent with the heading. Therefore, the starting point P0 and the end point P3 of the third-order Bezier curve are subject to non-holonomic constraints: The control points P1 and P2 that satisfy the above constraints are expressed as: Among them, ψ0 is the heading of the unmanned boat when it starts to berth, ψ g is the bow direction of the unmanned boat when it is moored at the berth, d1 is the distance between control point P0 and control point P1, and d2 is the distance between control point P2 and control point P3; S22. Calculate d1 and d2, and express the radius of curvature of any point of the third-order Bezier curve as: Discretize d1 and d2, determine the initial value and discrete interval, and determine the discrete interval and number of iterations of the curve parameter variable t. Calculate ρ(t) corresponding to each d1, d2 and t in turn according to the ρ(t) calculation formula to determine whether ρ(t) satisfies the condition ρ(t)≥ρ min If the conditions are met, the inner loop enters the next iteration, the number of iterations increases by 1, and the corresponding discrete interval is added to t. If the conditions are not met, the number of iterations is not counted. After the inner loop calculation reaches the upper limit of the number of iterations, it enters the middle loop. In the middle loop, d2 is gradually increased to the upper limit according to the discrete interval. For each d2, the corresponding inner loop calculation is performed. After the calculation of d2 in the middle loop is completed, the outermost loop is entered to perform the iterative calculation of d1. In the outermost loop, d1 is gradually increased to the upper limit according to the discrete interval, and the corresponding middle and inner loop calculations are performed for each d1. After the inner, middle and outermost loops are completed, d1 and d2 with sufficient accuracy are obtained, and the position coordinates of control points P1 and P2 can be determined based on control points P0, d1, control points P3 and d2.

2. The unmanned boat berthing path planning method based on Bezier curve according to claim 1, characterized in that: In step S1, the distance between the control point P4 and the control point P3 is one ship length.

3. The unmanned boat berthing path planning method based on Bezier curve according to claim 1, characterized in that: The first-order Bezier curve obtained in step S1 and the third-order Bezier curve obtained in step S3 are connected at the control point P3.

4. The unmanned boat berthing path planning method based on Bezier curve according to claim 1, characterized in that: The selection of control points P1 and P2 in step S2 cannot exceed the scope of the harbor waters.

5. The unmanned boat berthing path planning method based on Bezier curve according to claim 1, characterized in that: Before step S1, the method further includes: S0. Confirm the initial position of the unmanned boat, sense the navigation status of other ships in the berthing area through the laser radar, and continue when it is determined that there are no obstacles in the berthing area, otherwise terminate the planning and continue to wait; determine the berth information based on the laser radar, cluster the point cloud information returned by the laser radar, and obtain the location information of the berth.

Images