A method for generating ship berthing reference trajectory based on interpolation path planning and iterative discretization

Abstract

The present invention inputs the technical field of ship path planning, and proposes a method for generating a reference trajectory for ship berthing based on interpolation path planning and iterative discretization. This generation method includes two stages. The first stage completes global path planning based on the polynomial curve interpolation method, and the second stage completes reference trajectory generation through iterative discretization path. The reference trajectory generation method provided by the present invention and applied to autonomous berthing control of ships avoids the problems of high computing resource requirements and poor real-time performance of common path planning algorithms, and takes into account the constraints of ship motion capabilities when discretizing the path. The reference trajectory is an important reference input signal in the control process and is a necessary basis for realizing autonomous berthing of ships based on trajectory tracking control methods. Through the execution of this trajectory generation algorithm, it is possible to achieve reasonable planning of the berthing path when facing different berthing scenarios, and generate a continuous, smooth, and real-time navigation path.

Description

Technical Field

[0001] The present invention relates to the technical field of ship path planning, and in particular to a method for generating a ship berthing reference trajectory based on interpolation path planning and iterative discretization. Background Art

[0002] In the task of approaching a berth from an anchorage (referred to as berthing), trajectory tracking control is suitable because the speed in the port area is extremely low, the berthing path is mostly a continuous smooth curve, and the control accuracy of position and speed is high. However, in order to apply trajectory tracking control to the ship, at least one reference trajectory is first required to provide the controller as a reference input signal. The reference trajectory, also known as the expected trajectory, is the basic premise for realizing trajectory tracking control. The commonly used η d (t) represents a sequence of desired position coordinates and headings.

[0003] Among the existing trajectory tracking control technology solutions, the following solutions are commonly used: using permutations and combinations of sine functions, cosine functions, and linear functions about time t; using shapes that are easy to parameterize, such as circles; specifying the desired velocity vector ν d =[u d ,v d ,r d ] T , and then use the kinematic equation to get η d Although the above existing solutions are easy to generate reference trajectories, they rarely consider realistic factors, that is, it is difficult to obtain such an idealized reference trajectory in reality.

[0004] Although there are schemes that use third-order polynomials to generate parameterized reference trajectories, the polynomial coefficients are all assigned manually, and there is a lack of a path planning process based on the berthing scenario. Moreover, this generation scheme is not designed for the ship berthing process, so it cannot meet the berthing regulations well. In addition, there is a real-time planning method for berthing trajectories based on model prediction, but this scheme requires real-time solution of the multi-decision variable nonlinear constrained optimization problem it constructs, so it also faces great difficulties in engineering applications.

[0005] In summary, all of the above existing trajectory tracking controllers need to solve the problem of how to obtain a reference trajectory that complies with berthing regulations using an algorithm that is easy to implement in engineering before being applied on actual ships. Summary of the invention

[0006] In order to solve the problem of generating reference trajectory for unmanned ship berthing in the prior art, the present invention proposes a method for generating reference trajectory for ship berthing based on interpolation path planning and iterative discretization, which provides necessary information for berthing trajectory tracking control and ultimately serves the autonomous berthing of ships.

[0007] The technical solution is as follows:

[0008] A method for generating a ship berthing reference trajectory based on interpolation path planning and iterative discretization comprises the following steps:

[0009] Step S1: Berthing environment modeling: Based on the rules related to the ship's autonomous berthing control, the coordinates of the control points are calculated using the ship's anchorage position, posture, and angle information with the berth, and the agreed berthing angle. The control points include: the ship's initial position point I, the bow point A, the stern point B, the stabilization point S, and the berthing angle control point T;

[0010] Step S2: Berthing scenario classification: judging the berthing scenario according to the initial state of the ship and the current berthing environment, and selecting the control point generation strategy and polynomial order based on the berthing scenario;

[0011] Step S3: Global path planning: using the control point generation strategy and polynomial order described in step S2, combined with the polynomial curve method in the interpolation method, to achieve the acquisition of the parameterized path y=g(x);

[0012] Step S4: Determine the end point of the trajectory: the distance from point S to the end point of the path in the parameterized path is less than a set value as a traversal condition, and within the parameterized path, use a traversal algorithm to advance from point S to point I in the reverse direction to determine the end point of the path on the parameterized path that meets the traversal condition as the reference trajectory end point E;

[0013] Step S5: Iterative discretization: set the initial position of the ship as the first trajectory point in the sequence; gradually discretize the existing path based on the iterative algorithm: regard the latest trajectory point in the sequence as the old trajectory point, perform speed planning and curvature constraints based on the direction angle and curvature of the old trajectory point, calculate the coordinates of the new trajectory point on the existing path and store it in the sequence as the old trajectory point for the next iteration, repeat the iteration to update the sequence until the horizontal coordinate of the new trajectory point is greater than or equal to the horizontal coordinate of point E.

[0014] Preferably, step S1 comprises:

[0015] Step S11, defining a reference coordinate system for plane motion according to the shoreline and the berth, wherein the X-axis is set to coincide with the shoreline, and the Y-axis is set to be the horizontal distance 4L between the berth and the anchorage, and L represents the length of the ship;

[0016] Step S12, calculate the initial position of the ship and the coordinates of related points: record point I as the starting point, i.e. the initial position of the ship, point A as the bow, and point B as the stern, calculate and record the coordinates of I, A, and B in the reference system, regard point I, point A, and point B as control points, and record the initial heading ψ0 of the ship;

[0017] Step S13, calculate the coordinates of the stabilization point: according to the relevant agreement of automatic berthing, a point outside the berth with a vertical distance of 1.5L and the same horizontal coordinate as the berth is defined as the stabilization point, i.e., point S. The coordinates of point S are calculated and used as the target point of path planning, and point S is used as the control point at the same time;

[0018] Step S14, calculating the berthing angle control point T according to the ideal berthing angle: according to the relevant conventions of automatic berthing, the ideal berthing angle θ≤30 degrees, setting the control point T between point S and point I, the angle between line segment TS and the shoreline is equal to θ, and the length of ST needs to be determined based on the actual ship maneuverability through simulation or actual ship test;

[0019] Specifically, considering the margin, θ=27 degrees is set in the embodiment; it is recommended that L≤|ST|≤(x S -x I ) / 5, the embodiment is taken as 2L;

[0020] Step S15, calculate the heading of the line segment IS: the angle between the line segment IS and the shoreline X-axis is denoted as θ IS , calculate θ IS ; Also note that IS heading ψ IS With θ IS They are mutually alternate interior angles, and combined with the horizontal coordinate system definition, we have ψ IS =-θ IS ≤0.

[0021] Preferably, step S2 comprises:

[0022] Step S21, retrieve the initial position of the ship and the coordinate information of the stabilization point, and set a smaller distance threshold R;

[0023] Step S22, determine (y I -y S )≤R, where y I ,y S They are the Y coordinates of point I and point S respectively:

[0024] If yes, output the classification result that the current arrival scene is scene 5, and give the planned path y = y S ; The scene 5 indicates that the initial position is very close to the y coordinate of the target point, that is, (y I -y S )≤R holds true, the ship can sail parallel to the coastline to point S, and the path planned for the ship is a horizontal straight line y=y S ;

[0025] If not, execute step S23;

[0026] Step S23, determine (x I -x S)≤R is true:

[0027] If yes, output the classification result of the current scene as scene 6, and give the planned path x=x S ;

[0028] If not, execute step S24;

[0029] The scene 6 shows that the initial position is very close to the x coordinate of the target point, that is, (x I -x S )≤R holds true, the ship can sail perpendicular to the coastline to point S, and the planned path is a vertical straight line x=x S ;

[0030] Step S24, simultaneously determine θ and θ IS The size relationship of IS The relationship with the size of ψ0, the branch and its steps include:

[0031] If θ IS ≤θ, and ψ0>ψ IS , execute step S241;

[0032] If θ IS ≤θ, and ψ0≤ψ IS , execute step S242;

[0033] If θ IS >θ, and ψ0>ψ IS , execute step S243;

[0034] If θ IS >θ, and ψ0≤ψ IS , execute step S244;

[0035] Step S241, outputting the classification result of the current scene as scene 1, and recording the control points that the path needs to pass through as BIATS in sequence; the strategy in the scene 1 is to hope that the ship will slowly reduce the heading in the process of heading to the target, and there is no need to add or delete the control points obtained in step S1;

[0036] Step S242: Output the classification result of the current scene as scene 2, and find the middle point M between point A and point S. The coordinates of point M are x M =(x S +x A ) / 2,y M =y S , and then replace point T with point M, and record the control points that the path needs to pass through as BIAMS in sequence. In scenario 2, point T is an unnecessary control point. The path planning strategy in scenario 2 looks for the middle point M between point A and point S, and hopes that the final heading of the ship is 0 degrees;

[0037] Step S243, outputting the classification result of the current scene as scene 3, and adding control points, finding the middle point M between point T and point A, point M is the midpoint of line segment TA, and recording the control points that the path needs to pass through as BIAMTS in sequence;

[0038] Step S244, outputting the classification result that the current scene is scene 4, and recording the control points that the path needs to pass through as BIATS in sequence; the scene 4 adopts the same control points as scene 1, and the path also passes through BIATS in sequence;

[0039] Step S25, according to the number of control points c, the polynomial order n is given, n=c-2.

[0040] Preferably, step S3 comprises:

[0041] Step S31, judging whether it is one of scenes 1, 2, 3 or 4 according to the scene category obtained in S2:

[0042] If yes, execute step S32;

[0043] If not, skip step S32 and execute step S33;

[0044] Step S32, based on the control point coordinate sequence obtained in S2 and the polynomial order, the least squares method is used to perform polynomial fitting calculation, and the polynomial coefficient vector p is obtained and stored, that is, the parameterized path expression is obtained:

[0045] Where n+1 is the length of vector p, and p(i) is the i-th element of vector p;

[0046] Step S33, visualization of planning results: based on the parameterized path expression, the curve / straight line of the path is plotted into a graph; in addition to the path curve / straight line itself, the graph includes: scene type, starting point I position, target point S position, berthing path planning information and control point coordinates.

[0047] Preferably, step S4 comprises:

[0048] Step S41, given the input of the traversal algorithm: path expression y = g(x), traversal step length dx, preset distance threshold SE between point S and point E; initialize the intermediate variable: x = x S , arc length I = 0;

[0049] Step S42, determine whether I≤SE holds:

[0050] If yes, execute step S43;

[0051] If not, end the traversal and output the traversal algorithm result, that is, the coordinates of point E (x E ,g(xE )), where x E Equal to the current x value;

[0052] Step S43, save the coordinates (x past ,y past )=(x,g(x)), and treat x-dx as x in the new round of traversal process and input it into the algorithm to simulate the reverse advance along the path, that is, the search traversal process from point S to point I, and save the new coordinates (x now ,y now )=(x,g(x));

[0053] Step S44, calculate (x past ,y past ) and (x now ,y now ), this distance is recorded as dI;

[0054] Step S45, set I=I+dI, and then jump to S42.

[0055] Preferably, step S5 comprises:

[0056] Step S51, iterative algorithm initialization: point I (x I ,y I ) is added to the sequence as the first trajectory point; find the first-order differential g'(x) and second-order differential g"(x) of the path g(x); given the input of the algorithm: ideal longitudinal velocity u i , maximum allowable bow angular velocity r max ;

[0057] Step S52, calculate the direction angle and curvature: the coordinate x of the latest trajectory point in the sequence past , calculate the tangent direction angle and path curvature of the path at this point; let the intermediate variable k1 = g'(x past ),k2=g”(x past ), then the required direction angle ψ i The calculation formula is ψ i =tan -1 (k1), the required curvature κ is calculated as Need to i Deposit sequence;

[0058] Step S53, motion planning: planning speed is given u i , when κ is too large, u i will be reduced to meet the curvature constraint; the ship's bow angular velocity r = κ·u, if κ·u occurs at the current point i >r max , then the ideal longitudinal speed is adjusted to u i =rmax / κ; otherwise, u i The given value of S51 will be followed; Step S54, generate new trajectory points: the update of trajectory points is based on the existing old trajectory points, which is an iterative process; according to u i With t h , the distance between the new and old trajectory points is h = u i ·t h ; and because of the direction angle ψ i Knowing that the coordinates of the new trajectory points can be obtained; the new trajectory points need to be stored in a sequence;

[0059] Step S55, determine x new ≥x E Is it established:

[0060] If yes, terminate the iteration and output a sequence containing all trajectory points and direction angles (i.e., the generated reference trajectory);

[0061] If not, continue iterating and jump to S52;

[0062] x new Reach or exceed x E The algorithm terminates when .

[0063] Beneficial effects:

[0064] The present invention proposes a method for generating a reference trajectory for ship berthing based on interpolation path planning and iterative discretization. The generation method includes two stages. The first stage completes global path planning based on polynomial curve interpolation method, and the second stage completes reference trajectory generation by iterative discretization path. The reference trajectory generation method provided by the present invention and applied to autonomous berthing control of ships avoids the problems of high computing resource requirements and weak real-time performance of common path planning algorithms, and considers the ship's motion capability constraints when discretizing the path. The reference trajectory is an important reference input signal in the control process and is a necessary basis for realizing autonomous berthing of ships based on trajectory tracking control methods. Through the execution of the trajectory generation algorithm, it is possible to reasonably plan the berthing path when facing different berthing scenarios, and generate a continuous, smooth and real-time navigation path. Furthermore, the method performs motion planning to form a berthing control reference trajectory that meets the motion capability constraints. Using the reference trajectory obtained by the present invention, it is easy to obtain its differential with respect to time, and it can adapt to the input requirements of individual trajectory tracking controllers for the reference trajectory to the time derivative signal. The technical solution also allows certain adjustments in the input parameters to adapt to the maneuverability and controller input requirements of real ships. In addition, the method proposed in the present invention also provides relevant technical support for the development of ship motion control algorithms in complex scenarios, narrow waters, and limited time conditions, especially the simulation verification test of the effectiveness of the controller.

[0065] Specifically, the present invention mainly includes five aspects of innovation:

[0066] Environmental modeling is used to numerically represent the necessary environmental information that must be considered in the path planning stage of this method, determine several basic control points required for subsequent interpolation, and mathematically describe the positional relationship between the anchorage position and a certain point outside the agreed berth (stabilization point).

[0067] Scenario classification is used to determine the most reasonable way for a ship to approach the berth, configure multiple control points required for subsequent interpolation, and also determine the appropriate polynomial order. The basis for classifying scenarios is the initial heading of the ship, the relationship between the anchorage position and the stabilization point position, the agreed approach angle, etc.

[0068] Path interpolation is used to receive the coordinates and order information of existing control points, and obtain a global path that connects the control points, is continuous and smooth, can be expressed using polynomials, and can extract polynomial coefficients for storage through a fitting algorithm. The small number of polynomial coefficients significantly reduces the difficulty of path parameterization, which is very conducive to the subsequent generation of trajectories.

[0069] End point determination is used to determine the timing of issuing a stop order in trajectory tracking control. The given expected distance threshold is received, and the end point coordinates of the reference trajectory are calculated so that the distance along the path between the end point and the stabilization point meets the given expected value. If the stop order is issued after reaching the stabilization point, due to the large inertia of the ship, the ship will still slide forward and exceed the stabilization point, which is not conducive to subsequent maneuvering into the berth. The key point of this design is to deal with this problem and end the trajectory tracking process before reaching the stabilization point.

[0070] Iterative discretization is used to finally obtain the reference trajectory and ensure that the reference trajectory meets the constraints of the ship's motion capability. Different from simple equidistant discretization, this design receives the expected speed, the maximum allowable angular velocity, and the existing polynomial parameters, dynamically determines the interval between two expected positions, and uses step-by-step iteration and forward movement along the path to obtain the reference position sequence. The reference trajectory can be obtained until the iteration reaches the boundary, that is, the end point of the trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 This is a flowchart of a reference trajectory generating method in a preferred embodiment of the present invention.

[0072] Figure 2 These are example diagrams of the six types of berthing scenarios introduced in step S2 of the generation method.

[0073] Figure 3 It is a specific flow chart of step S1 in the generation method.

[0074] Figure 4Schematic diagram of the principle of step S1 in the generation method.

[0075] Figure 5 It is a specific flow chart of step S2 in the generation method.

[0076] Figure 6 It is a specific flow chart of step S24 in the generation method.

[0077] Figure 7 It is a specific flow chart of step S3 in the generation method.

[0078] Figure 8 It is a specific flow chart of step S4 in the generation method.

[0079] Fig. 9 It is a specific flow chart of step S5 in the generation method.

[0080] Fig.10 The following are example diagrams of the results of the generation method in berthing scenarios 3 and 4. DETAILED DESCRIPTION

[0081] In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention.

[0082] In a preferred embodiment of the present invention, based on the above problems existing in the prior art, a method for generating a reference trajectory of a ship berthing based on interpolation path planning and iterative discretization is provided. Figure 1 As shown, including:

[0083] Step S1: Berthing environment modeling: Based on the rules related to the ship's autonomous berthing control, the coordinates of the control points are calculated using the ship's anchorage position, posture, and angle information with the berth, and the agreed berthing angle. The control points include: the ship's initial position point I, the bow point A, the stern point B, the stabilization point S, and the berthing angle control point T;

[0084] Step S2: Berthing scenario classification: judging the berthing scenario according to the initial state of the ship and the current berthing environment, and selecting the control point generation strategy and polynomial order based on the berthing scenario;

[0085] In the embodiment, this embodiment is based on Figure 2 Taking the example of S2, six different docking scenarios introduced by S2 are demonstrated to help technical personnel understand the meaning of scenarios 1 to 6, the situations they correspond to, and the characteristics of their docking paths.

[0086] Step S3: Global path planning: using the control point generation strategy and polynomial order described in step S2, combined with the polynomial curve method in the interpolation method, to achieve the acquisition of the parameterized path y=g(x);

[0087] Step S4: Determine the end point of the reference trajectory: take the distance from point S to a certain point in the parameterized path less than a set value as a traversal condition, and use a traversal algorithm to advance from point S to point I in the parameterized path in the reverse direction to find the first point on the parameterized path that does not meet the traversal condition, and determine it as the end point E of the reference trajectory;

[0088] Step S5: Iterative discretization: Set the initial position of the ship as the first trajectory point in the sequence; gradually discretize the existing path based on the iterative algorithm: regard the latest trajectory point in the sequence as the old trajectory point, perform speed planning and curvature constraints based on the direction angle and curvature of the old trajectory point, calculate the coordinates of the new trajectory point on the existing path and store it in the sequence as the old trajectory point for the next iteration, repeat the iteration to update the sequence until the horizontal coordinate of the new trajectory point is greater than or equal to the horizontal coordinate of point E. In a preferred embodiment of the present invention, Figure 3 As shown, step S1 includes:

[0089] Step S11, defining a reference coordinate system for plane motion according to the shoreline and the berth, wherein the X-axis is set to coincide with the shoreline, and the Y-axis is set to be between the berth and the anchorage and at a horizontal distance of 4L from the berth;

[0090] Specifically, in the description of this solution, L always represents the "hull length" or "ship length", in meters; the intersection of the X-axis and the Y-axis is the origin;

[0091] Step S12, calculate the initial position of the ship and the coordinates of related points: record point I as the starting point, i.e. the initial position of the ship (Initial), point A as the bow (Ahead), point B as the stern (Behind), calculate and record the coordinates of I, A, B in the reference system, and record the initial heading ψ0 of the ship;

[0092] Step S13, calculate the coordinates of the stable point: According to the relevant agreement of automatic berthing, a point outside the berth with a vertical distance of 1.5L and the same horizontal coordinate as the berth is defined as the stable point (S) or point S. Calculate the coordinates of point S and use it as the target point of path planning;

[0093] Step S14, calculate the control point T according to the ideal berthing angle: according to the relevant conventions of automatic berthing, the ideal berthing angle θ≤30 degrees, calculate a control point T between point S and point I, requiring that the angle between line segment TS and the shoreline is equal to θ, and the length of line segment ST needs to be appropriately selected to make the path smooth;

[0094] Specifically, considering the safety margin of the berthing angle, θ is set to 27 degrees in the embodiment; in this embodiment, L≤|ST|≤(x S -x I ) / 5 is the feasible range and is always taken as 2L in subsequent examples;

[0095] Step S15, calculate the heading of the line segment IS: the angle between the line segment IS and the shoreline X-axis is denoted as θ IS , calculate θ IS . Also note that the IS heading ψ IS With θ IS They are mutually alternate interior angles, and combined with the horizontal coordinate system definition, we have ψ IS =-θ IS ≤0.

[0096] In an embodiment, Figure 4 Contains some of the coordinate points and angles involved in the work content of the above step S1.

[0097] As a supplement to step S1, the most important task in implementing the polynomial curve interpolation method is the selection of control points. Whether the control points are selected reasonably directly determines the success or failure of planning and the path shape. The points B, I, A, T, and S obtained by executing step S1 can be used as control points. The subsequent step S2 needs to distinguish different scenarios and appropriately add or delete control points to make the path curve smoother.

[0098] In a preferred embodiment of the present invention, Figure 5 As shown, step S2 includes:

[0099] Step S21, retrieve the initial position of the ship and the coordinate information of the stabilization point, and set a smaller distance threshold R;

[0100] Specifically, the threshold R needs to be adjusted according to the actual maneuverability of the ship and should be as small as possible. In the embodiment, R=L / 10;

[0101] Step S22, determine (y I -y S )≤R is true:

[0102] If yes, output the classification result of the current scene as scene 5, and give the planned path y=y S ;

[0103] If not, execute step S23;

[0104] In the embodiment, scene 5 indicates that the initial position is very close to the y coordinate of the target point, that is, (y I -y S )≤R holds true, the ship can sail parallel to the coastline to point S, and the path planned for the ship is a horizontal straight line y=y S ;

[0105] Step S23, determine (x I -x S )≤R is true:

[0106] If yes, output the classification result of the current scene as scene 6, and give the planned path x=x S ;

[0107] If not, execute step S24;

[0108] In the embodiment, similar to scenario 5, scenario 6 indicates that the initial position is very close to the x coordinate of the target point, that is, (x I -x S )≤R holds true, the ship can sail perpendicular to the coastline to point S, and the planned path is a vertical straight line x=x S ;

[0109] As a supplementary explanation to steps S22 and S23, the paths of scenarios 5 and 6 are along the XY axis direction, which is relatively special and idealized and rare in the real world; and the acquisition of its parameterized path is direct, and there is no need to implement the polynomial curve interpolation method; therefore, scenarios 5 and 6 are given here only to comprehensively analyze the berthing scenario and realize the integrity of the global path planning method;

[0110] Step S24, simultaneously determine θ and θ IS The size relationship of IS The relationship with the size of ψ0, the branch and its steps include:

[0111] If θ IS ≤θ, and ψ0>ψ IS , execute step S241;

[0112] If θ IS ≤θ, and ψ0≤ψ IS , execute step S242;

[0113] If θ IS >θ, and ψ0>ψ IS , execute step S243;

[0114] If θ IS >θ, and ψ0≤ψ IS , execute step S244;

[0115] Specifically, in this embodiment, S241, S242, S243, and S244 are independent of each other;

[0116] In the embodiment, the present design proposes a specific basis for a more detailed division of the berthing scenes (except scenes 5 and 6), namely, the two groups of angle comparisons described in S24 above. Based on the comparison results, the scenes are divided into four types and different operations are performed;

[0117] Step S241, outputting the classification result that the current scene is scene 1, and recording the control points that the path needs to pass through in sequence as BIATS;

[0118] In the embodiment, the strategy in scenario 1 is relatively simple, and it is desired that the ship slowly reduce the heading while heading toward the target, and there is no need to add or delete the control points obtained in step S1.

[0119] Step S242: Output the classification result of the current scene as scene 2, and find the middle point M between point A and point S. The coordinates of point M are x M =(x S +x A ) / 2,y M =y S , and then replace point T with point M, and record the control points that the path needs to pass through as BIAMS.

[0120] In the embodiment, the original design of point T is to ensure that the angle between the rear section of the path and the coastline is approximately θ. However, in scenario 2, point T becomes an unnecessary control point: on the one hand, the ship slowly increases the heading until 0 degrees while heading towards the target, which can make the angle between the rear section of the path and the coastline less than θ, and this path is legal (that is, it complies with the berthing operation agreement); on the other hand, assuming that the control point still includes point T, then in order to cover the TS section, the path must have an additional turn before point T, and the ship's heading needs to increase first and then decrease, which consumes more energy. For the above reasons, the path planning strategy in scenario 2 looks for the middle point M between points A and S, and hopes that the final heading of the ship is 0 degrees. The complexity of this scenario is moderate;

[0121] Step S243, output the classification result of the current scene as scene 3, and add control points, find the middle point M between point T and point A, point M is the midpoint of line segment TA, and record the control points that the path needs to pass through as BIAMTS in sequence.

[0122] In the embodiment, in scenario 3, the path is the most complex, so it is necessary to add control points. The specific reason is: the front section of the path requires the ship to reduce the heading to point S; but in the back section, the heading needs to be increased to meet the rule constraints on the berthing angle; so the heading has to be reduced first and then increased, resulting in an inflection point on the path; this inflection point should be located between point T and point A, so the midpoint M of TA is selected to add the control point. It is worth pointing out that, unlike S242, point T is still needed to play its role at this time, and M cannot replace T.

[0123] Step S244, outputting the classification result that the current scene is scene 4, and recording the control points that the path needs to pass through in sequence as BIATS;

[0124] In the embodiment, scenario 4 is very similar to scenario 1. The ship can slowly increase the heading while heading towards the target, and the path is relatively simple. The same control points as scenario 1 are adopted, and the path also passes through BIATS in sequence;

[0125] Step S25, according to the number of control points c, the polynomial order n is given, n=c-2;

[0126] In the embodiment, in the polynomial curve interpolation method, after the control points are determined, the selection of the order n is particularly important. If n is too high, the curve may oscillate due to the influence of high-order terms, causing the path to fail to meet the ship's executable constraints or generate unnecessary energy consumption. On the contrary, it may not be possible to smoothly connect all control points. In view of the fact that some berthing paths require inflection points, cubic and quartic polynomials are more reasonable choices. In addition, considering that the number of control points c is 5 or 6, this solution proposes that the setting of n=c-2 should be adopted.

[0127] Table 1 shows the basis for judging different scenarios:

[0128] Table 1

[0129] Scenario Criteria 1 <![CDATA[θ IS ≤θ and ψ0>ψ IS ]]> 2 <![CDATA[θ IS ≤θ and ψ0≤ψ IS ]]> 3 <![CDATA[θ IS >θandψ0>ψ IS ]]> 4 <![CDATA[θ IS >θandψ0≤ψ IS ]]> 5 <![CDATA[(y I -and S )≤R]]> 6 <![CDATA[(x I -x S )≤R]]>

[0130] In a preferred embodiment of the present invention, Figure 7 As shown, step S3 includes:

[0131] Step S31, judging whether it is one of scenes 1 / 2 / 3 / 4 according to the scene category obtained in S2:

[0132] If yes, execute step S32;

[0133] If not, skip step S32 and execute step S33;

[0134] Step S32, based on the control point coordinate sequence obtained in S2 and the polynomial order, the least square method is used to perform polynomial fitting calculation, and the polynomial coefficient vector p is obtained and stored, that is, the parameterized path expression of the path is obtained:

[0135] Where n+1 is the length of vector p, and p(i) is the i-th element of vector p;

[0136] Step S33, visualization of planning results. Based on the path parameterization expression, the curve / straight line of the path is plotted into a graph to facilitate the verification work of the technicians; in addition to the path curve / straight line itself, the graph must also contain important information of the berthing path planning such as the scene type, the starting point I position, the target point S position, etc.; it may also contain several control points for interpolation basis;

[0137] In an embodiment, the planning results are visualized to facilitate technicians to check the current scene and verify the rationality of the berthing path obtained by the planning algorithm;

[0138] As a supplementary explanation of step S3, the examples of the six different berthing scenarios given above ( Figure 2), which is actually the visualization output result of S3 in different scenarios.

[0139] In a preferred embodiment of the present invention, Figure 8 As shown, step S4 includes:

[0140] Step S41, given the input of the algorithm: path expression y = g(x), traversal step length dx, preset distance threshold SE between point S and point E; initialize the intermediate variable: x = x S , arc length I = 0;

[0141] In the embodiment, step S4 searches for a point on the path that meets the requirements and uses it as the end point of the desired trajectory, and this point is recorded as point E (End);

[0142] In the embodiment, considering the large scale of the ship, the traversal step length dx does not need to be particularly fine, and in this embodiment, dx=0.5m is taken; the setting of SE needs to be combined with the maneuverability of the actual ship, especially the inertial stop and sliding distance at a specific speed, and in this embodiment, SE=L is taken;

[0143] In one embodiment, the algorithm traverses x in reverse order, starting from x S Start calculating the arc length I in reverse along the path, initialized as above;

[0144] Step S42, determine whether I≤SE holds:

[0145] If yes, execute step S43;

[0146] If not, output the algorithm result, that is, the coordinates of point E (x E ,g(x E )), where x E Equal to the current x value;

[0147] In the embodiment, step S4 as a whole uses a traversal algorithm to determine point E: in this process, dI is calculated and accumulated to obtain the current I, the arc length I is compared with SE, and once I exceeds the threshold, the traversal is terminated, and the current x is used as the horizontal coordinate x of E. E Therefore, step S42 is actually used to determine whether the traversal is completed;

[0148] Step S43, save the coordinates (x past ,y past )=(x,g(x)), and treat x-dx as x in the new round of traversal and input it into the algorithm, saving the new coordinates (x now ,y now )=(x,g(x));

[0149] In the embodiment, the scheme adopts a loop and reduces x by a step size dx to simulate the search traversal process of advancing in the reverse direction along the path (from point S to point I). In each round of the loop, since x and the path expression y = g(x) are known, it is easy to obtain the current x now and x past The corresponding specific coordinates;

[0150] Step S44, calculate (x past ,y past ) and (x now ,y now ), this distance is recorded as dI;

[0151] In the embodiment, the two coordinates obtained by S43 can be easily obtained as the Euclidean distance between the two coordinates, which approximately replaces the differential of the arc length;

[0152] Step S45, set I=I+dI, then jump to S42;

[0153] In the embodiment, the algorithm approximates the arc length based on numerical integration, which saves more computing resources and is easier to implement than the method based on the arc length formula, and the accuracy is acceptable relative to the ship scale;

[0154] As a supplementary explanation to step S4, the aforementioned point S (stabilization point outside the berth) in this scheme is the end point of the path, but not the end point of the reference trajectory. Considering that the ship has a large inertia, it will still slide for a distance after the stop order. If the trajectory tracking is stopped at point S, the actual stop position will exceed S, which is extremely unfavorable for subsequent parallel approach to berthing. Therefore, it is necessary to stop at point S in advance, that is, it is necessary to determine the coordinates of the end point E of the reference trajectory through S4.

[0155] In a preferred embodiment of the present invention, Fig. 9 As shown, step S5 includes:

[0156] Step S51, algorithm initialization. Set point I (x I ,y I ) is added to the sequence as the first trajectory point; find the expression of g'(x) and g”(x); given the input of the algorithm: ideal longitudinal velocity u i , maximum allowable bow angular velocity r max ;

[0157] In an embodiment, the reference trajectory is calculated from the initial position (x I ,y I ), that is, point I is the first track point in the sequence. This design is based on the fact that the selection of the ship reference route in navigation practice is usually related to the initial ship position;

[0158] In an embodiment, g'(x) and g"(x) are first-order differential and second-order differential expressions of path g(x), respectively;

[0159] In the embodiment, when berthing in the port area, the ideal speed of the ship is u i Generally, it does not exceed 4 kn (where 1 kn = 0.5144 m / s). In this embodiment, it is taken as u i =3kn; According to the actual ship maneuverability (especially the bow turning capability limitation, it is necessary to pay attention to the phenomenon of reduced rudder efficiency at low speed), the angular velocity limit r is given max , in this embodiment, r max =0.0174533rad / s (i.e. 1deg / s);

[0160] Step S52, calculate the direction angle and curvature. past , calculate the tangent direction angle and path curvature of the path at this point;

[0161] Specifically, let the intermediate variable k1 = g'(x past ),k2=g”(x past ), then the required direction angle ψ i The calculation formula is ψ i =tan -1 (k1), the required curvature κ is calculated as Need to i Deposit sequence;

[0162] In an embodiment, the direction angle ψ i It is both the tangent direction at the current point and the heading of the ship when it is assumed to move strictly along the trajectory; the curvature κ quantifies the curvature of the resulting path;

[0163] Step S53, motion planning. The planned speed is usually the given u i , but when κ is too large, u i will be reduced, let u i =r max / κ to satisfy the curvature constraint.

[0164] Specifically, according to the kinematic principle and derivation, the relationship is obtained: the ship's bow angular velocity r = κ·u. If κ·u occurs at the current point i >r max , then the ideal longitudinal speed is adjusted to u i =r max / κ; otherwise, u i The given value of S51 will be followed;

[0165] Step S54: Generate new trajectory points. The update of trajectory points is based on the existing old trajectory points, which is an iterative process. i With t h , the distance between the new and old trajectory points is h = ui ·t h ; and because of the direction angle ψ i Knowing that the coordinates of the new trajectory points can be obtained; the new trajectory points need to be stored in a sequence;

[0166] Specifically, in this embodiment, the update interval t h =5s; the calculation expression of the new trajectory point coordinates is:

[0167]

[0168] In an embodiment, t h It does not need to be too short, because: the speed is very low during the berthing process (u i ≤4kn), the time interval for updating the expected position during motion planning should be appropriately relaxed so that there is a significant distinction between two adjacent expected positions.

[0169] Step S55, determine x new ≥x E Is it established:

[0170] If yes, terminate the iteration and output a sequence containing all trajectory points and direction angles (i.e., the generated reference trajectory);

[0171] If not, continue iterating and jump to S52;

[0172] In the embodiment, since point E is agreed to be the end point of the reference trajectory, x E >x I , so x new Reach or exceed x E The algorithm terminates when

[0173] As a supplement to step S5, S5 completes motion planning, which actually specifies how the ship moves along the path, and is divided into two aspects: speed planning and curvature constraint. The former determines the longitudinal speed u of the ship. i The latter is based on the maximum bow angular velocity r allowed for a ship to turn. max According to the motion planning results, S5 discretizes the existing path through the forward iterative method, thereby generating a reference trajectory suitable for trajectory tracking control requirements.

[0174] As a supplementary explanation of this embodiment, the reference trajectory generation method is tested in scenario 3 and scenario 4 respectively. The test results are as follows: Fig.10 As shown. Among them, Fig.10 (a) represents the test reference trajectory generation method (scenario 3), Fig.10(b) Test reference trajectory generation method (Scenario 4) The curve is the landing path, the green circle close to S is point E, and the small circles densely arranged on the path are the expected position sequence in the reference trajectory (x i ,y i ). After further analysis of the data, it was found that in both scenarios, the SE distance was extremely close to L, and the bow angular velocity r did not exceed r max The test results verify the effectiveness of the proposed generation method.

[0175] The present invention is not limited to the embodiment, and other embodiments may also belong to the scope of the present invention as long as they conform to the gist of the present invention.

Claims

1. A method for generating a ship berthing reference trajectory based on interpolation path planning and iterative discretization, characterized in that: The following steps are involved: Step S1: Berthing environment modeling: Based on the rules related to the ship's autonomous berthing control, the coordinates of the control points are calculated using the ship's anchorage position, posture, and angle information with the berth, and the agreed berthing angle. The control points include: the ship's initial position point I, the bow point A, the stern point B, the stabilization point S, and the berthing angle control point T; Step S2: Berthing scenario classification: judging the berthing scenario according to the initial state of the ship and the current berthing environment, and selecting the control point generation strategy and polynomial order based on the berthing scenario; Step S3: Global path planning: using the control point generation strategy and polynomial order described in step S2, combined with the polynomial curve method in the interpolation method, to achieve the acquisition of the parameterized path y=g(x); Step S4: Determine the end point of the reference trajectory: take the distance from point S to a certain point in the parameterized path less than a set value as a traversal condition, and use a traversal algorithm to advance from point S to point I in the parameterized path in the reverse direction to find the first point on the parameterized path that does not meet the traversal condition, and determine it as the end point E of the reference trajectory; Step S5: Iterative discretization: set the initial position of the ship as the first trajectory point in the sequence; gradually discretize the existing path based on the iterative algorithm: regard the latest trajectory point in the sequence as the old trajectory point, perform speed planning and curvature constraints based on the direction angle and curvature of the old trajectory point, calculate the coordinates of the new trajectory point on the existing path and store it in the sequence as the old trajectory point for the next iteration, repeat the iteration to update the sequence until the horizontal coordinate of the new trajectory point is greater than or equal to the horizontal coordinate of point E.

2. The method for generating a ship berthing reference trajectory based on interpolation path planning and iterative discretization according to claim 1 is characterized in that: Step S1 includes: Step S11, defining a reference coordinate system for plane motion according to the shoreline and the berth, wherein the X-axis is set to coincide with the shoreline, and the Y-axis is set to be the horizontal distance 4L between the berth and the anchorage, and L represents the length of the ship; Step S12, calculate the initial position of the ship and the coordinates of related points: record point I as the starting point, i.e. the initial position of the ship, point A as the bow, and point B as the stern, calculate and record the coordinates of I, A, and B in the reference system, regard point I, point A, and point B as control points, and record the initial heading ψ0 of the ship; Step S13, calculate the coordinates of the stabilization point: according to the relevant agreement of automatic berthing, a point outside the berth with a vertical distance of 1.5L and the same horizontal coordinate as the berth is defined as the stabilization point, i.e., point S. The coordinates of point S are calculated and used as the target point of path planning, and point S is used as the control point at the same time; Step S14, calculating the berthing angle control point T according to the ideal berthing angle: according to the relevant conventions of automatic berthing, the ideal berthing angle θ≤30 degrees, setting the control point T between point S and point I, the angle between line segment TS and the shoreline is equal to θ, and the length of ST needs to be determined based on the actual ship maneuverability through simulation or actual ship test; Step S15, calculate the heading of the line segment IS: the angle between the line segment IS and the shoreline X-axis is denoted as θ IS , calculate θ IS ; Also note that the IS heading ψ IS With θ IS They are mutually alternate interior angles, and combined with the horizontal coordinate system definition, we have ψ IS =-θ IS ≤0.

3. The method for generating a ship berthing reference trajectory based on interpolation path planning and iterative discretization according to claim 1 is characterized in that: Step S2 includes: Step S21, retrieve the initial position of the ship and the coordinate information of the stabilization point, and set the distance threshold R; Step S22, determine (y I -y S )≤R, where y I ,y S They are the Y coordinates of point I and point S respectively: If yes, output the classification result that the current arrival scene is scene 5, and give the planned path y = y S ; The scene 5 indicates that the initial position is very close to the y coordinate of the target point, that is, (y I -y S )≤R holds true, the ship can sail parallel to the coastline to point S, and the path planned for the ship is a horizontal straight line y=y S ; If not, execute step S23; Step S23, determine (x I -x S )≤R, where x I ,x S They are the X coordinates of point I and point S respectively: If yes, output the classification result of the current scene as scene 6, and give the planned path x=x S ; If not, execute step S24; The scene 6 shows that the initial position is very close to the x coordinate of the target point, that is, (x I -x S )≤R holds true, the ship can sail perpendicular to the coastline to point S, and the planned path is a vertical straight line x=x S ; Step S24, simultaneously determine θ and θ IS The size relationship of IS The relationship with the size of ψ0, the branch and its steps include: If θ IS ≤θ, and ψ0>ψ IS , execute step S241; If θ IS ≤θ, and ψ0≤ψ IS , execute step S242; If θ IS >θ, and ψ0>ψ IS , execute step S243; If θ IS >θ, and ψ0≤ψ IS , execute step S244; Step S241, outputting the classification result of the current scene as scene 1, and recording the control points that the path needs to pass through as BIATS in sequence; the strategy in the scene 1 is to hope that the ship will slowly reduce the heading in the process of heading to the target, and there is no need to add or delete the control points obtained in step S1; Step S242: Output the classification result that the current scene is scene 2, and find the middle point M between point A and point S. The coordinates of point M (x M ,y M ) is x M =(x S +x A ) / 2,y M =y S , and then replace point T with point M, and record the control points that the path needs to pass through as BIAMS; Step S243, outputting the classification result of the current scene as scene 3, and adding control points, finding the middle point M between point T and point A, point M is the midpoint of line segment TA, and recording the control points that the path needs to pass through as BIAMTS in sequence; Step S244, outputting the classification result that the current scene is scene 4, and recording the control points that the path needs to pass through as BIATS in sequence; the scene 4 adopts the same control points as scene 1, and the path also passes through BIATS in sequence; Step S25, according to the number of control points c, the polynomial order n is given, n=c-2.

4. The method for generating a ship berthing reference trajectory based on interpolation path planning and iterative discretization according to claim 3 is characterized in that: In step S242, in the scenario 2, point T is an unnecessary control point. The path planning strategy in scenario 2 searches for the middle point M between point A and point S, and hopes that the final heading of the ship is 0 degrees.

5. The method for generating a ship berthing reference trajectory based on interpolation path planning and iterative discretization according to claim 1 is characterized in that: Step S3 includes: Step S31, judging whether it is one of scenes 1-4 according to the scene category obtained in S2: If yes, execute step S32; If not, skip step S32 and execute step S33; Step S32, based on the control point coordinate sequence obtained in S2 and the polynomial order n, the least squares method is used to perform polynomial fitting calculation to obtain and store the polynomial coefficient vector p, where the coefficients in p are arranged in descending order, to obtain the parameterized path expression: Where n+1 is the length of vector p, and p(i) is the i-th element of vector p; Step S33, visualization of planning results: based on the parameterized path expression, the curve / straight line of the path is plotted into a graph; in addition to the path curve / straight line itself, the graph includes: scene type, starting point I position, target point S position, berthing path planning information and control point coordinates; 6. The method for generating a ship berthing reference trajectory based on interpolation path planning and iterative discretization according to claim 1 is characterized in that: Step S4 includes: Step S41, given the input of the traversal algorithm: parameterized path expression y = g(x), traversal step length dx, and a preset distance threshold SE between point S and point E; point E is a point on the parameterized path, i.e., the end point of the desired trajectory, and the intermediate variable x = x is initialized. S , arc length I = 0, x S is the X coordinate of point S; Step S42, determine whether I≤SE is established: If yes, execute step S43; If not, end the traversal and output the traversal algorithm result, that is, the coordinates of point E (x E ,g(x E )), where x E Equal to the current x value; Step S43, save the coordinates (x past ,y past )=(x,g(x)), and treat x-dx as x in the new round of traversal process and input it into the algorithm to simulate the reverse advance along the path, that is, the search traversal process from point S to point I, and save the new coordinates (x now ,y now )=(x,g(x)); Step S44, calculate (x past ,y past ) and (x now ,y now ), this distance is recorded as dI; Step S45, set I=I+dI, and then jump to S42.

7. The method for generating a ship berthing reference trajectory based on interpolation path planning and iterative discretization according to claim 1 is characterized in that: Step S5 includes: Step S51, iterative algorithm initialization: point I (x I ,y I ) is added to the sequence as the first trajectory point; find the first-order differential g'(x) and second-order differential g"(x) of the path g(x); given the input of the algorithm: ideal longitudinal velocity u i , maximum allowable bow angular velocity r max ; Step S52, calculate the direction angle and curvature: To implement iterative calculation, it is necessary to regard the latest existing trajectory point in the sequence as the old trajectory point, and use the coordinate x of the old trajectory point past , calculate the tangent direction angle and path curvature of the path at this point; let the intermediate variable k1 = g'(x past ),k2=g”(x past ), then the required direction angle ψ i The calculation formula is ψ i =tan -1 (k1), the required curvature κ is calculated as Need to i Deposit sequence; Step S53, motion planning: the distance between the new and old trajectory points depends on the planned speed; since the ship's bow angular velocity r = κ·u, if the current point has κ·u i >r max , then the ideal longitudinal velocity u i will be reduced, let u i =r max / κ, to satisfy the curvature constraint; otherwise, u i The given value of S51 will be followed; Step S54, generate new trajectory points: according to u i With t h , the distance between the new and old trajectory points is h = u i ·t h ; Based on the direction angle ψ i , spacing h, coordinates of old trajectory points and g(x), find the coordinates of new trajectory points (x new ,y new ) and store the new trajectory point into the sequence as the latest existing trajectory point in the reference trajectory; Step S55, determine x new ≥x E Is it established: If yes, the iteration is terminated and a sequence of all trajectory points and direction angles is output, which is the reference trajectory. If not, continue iterating and jump to S52; x new Reach or exceed x E The algorithm terminates when .

Images