Path planning method based on maritime collision avoidance rules

Abstract

The present invention discloses a path planning method based on maritime collision avoidance rules, which belongs to the technical field of intelligent ship path planning. The method comprises the following steps: S10, setting a global route for a ship's autonomous cruising mission; S20, obtaining ship positioning and environmental perception prediction data; S30, constructing a reference path and reference line generator unit based on the global route; S40, constructing a Cartesian coordinate and Frenet coordinate system converter unit based on the reference line; S50, obtaining the current decision result of the ship through a maritime collision avoidance rule behavior decider module; S60, constructing a path replanning decision unit; S70, constructing a path boundary decision unit based on maritime collision avoidance rules; S80, constructing a path optimizer unit based on maritime ship dynamic constraints; the present invention makes autonomous decisions and plans collision avoidance paths based on real-time perceived environmental information in a complex and changeable maritime environment without human intervention, thereby improving the safety and efficiency of navigation.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent ship path planning, and in particular relates to a path planning method based on maritime collision avoidance rules. Background Art

[0002] With the continuous increase in maritime traffic, especially the prevalence of large ships and unmanned vessels, maritime traffic safety has become one of the major issues in the global shipping sector. To reduce maritime traffic accidents and ensure the safe navigation of ships, the International Maritime Organization (IMO) has formulated the International Regulations for Preventing Collisions at Sea, which provide basic norms and standards for ships to avoid collisions in complex navigation environments.

[0003] However, faced with the increasingly complex marine traffic environment and the development of autonomous maritime navigation technology, traditional manual or semi-automatic collision avoidance operations can no longer meet the requirements of efficiency, safety and real-time performance. Therefore, a path planning method based on maritime collision avoidance rules is needed to solve the above problems. Summary of the Invention

[0004] The purpose of the present invention is to provide a path planning method based on maritime collision avoidance rules to solve the problems raised in the above background technology.

[0005] To achieve the above object, the present invention provides the following technical solution: a path planning method based on maritime collision avoidance rules, comprising the following steps:

[0006] S10. Setting the global route of the ship's autonomous cruise mission;

[0007] S20, obtaining ship positioning and environment perception prediction data;

[0008] S30, constructing a reference path and reference line generator unit based on the global route;

[0009] S40, constructing a Cartesian coordinate and Frenet coordinate system converter unit based on the reference line;

[0010] S50, obtaining the current decision result of the ship through the maritime collision avoidance rules behavior decision module;

[0011] S60, constructing a path replanning decision unit;

[0012] S70, constructing a path boundary decision unit based on maritime collision avoidance rules;

[0013] S80, constructs a path optimizer unit based on marine ship dynamics constraints;

[0014] S90, the speed planning module performs speed planning based on the path result;

[0015] S100. Establish a monitoring and continuous path planning operation mechanism.

[0016] As a preferred solution, in "step S10", setting the global route of the ship autonomous cruise mission includes the following steps:

[0017] S101, marking the cruise mission route of the intelligent ship on the electronic map in sequence, and recording the positions on the marked cruise mission route to obtain a point queue of the cruise mission route;

[0018] S102: Convert the point queue of the cruise mission route into a Cartesian coordinate system through a latitude, longitude and Cartesian coordinate conversion module;

[0019] S103 , performing waypoint compensation on two adjacent points in the global route point queue in the Cartesian coordinate system using a sample point interpolation algorithm to obtain a desired point queue, and using the desired point queue as the global route.

[0020] As a preferred solution, in "step S20", obtaining ship positioning and environment perception prediction data includes the following steps:

[0021] S201. The vessel obtains the current GPS longitude and latitude in real time through its own global positioning system, and converts it into a Cartesian coordinate system through a latitude and longitude & Cartesian coordinate system conversion module;

[0022] S202: The vessel obtains its current speed in real time through its own configured speed calculation module;

[0023] S203, the vessel obtains the current heading angle in real time through its own heading calculation module;

[0024] S204. The ship obtains obstacle boundary data through the perception prediction module, including two types of data: static obstacle boundary line data and obstacle data.

[0025] As a preferred solution, in "step S30", constructing a reference line and reference path generator unit based on the global route includes the following steps:

[0026] S301, construct a path accumulation S matrix calculation module. Specifically, the S matrix is ​​a one-dimensional matrix, in which each element represents a route point To the starting point The cumulative path length between includes the following steps:

[0027] S3011. Calculate the distance on the path using the Euclidean distance calculation formula. Index Point To the starting point The cumulative length of s ;

[0028] S3012, calculating the cumulative path length s from each path point to the starting point;

[0029] S302: Constructing a path clipping calculation module, including the following steps:

[0030] S3021, calculating the cumulative S matrix corresponding to the path through the path cumulative S matrix calculation module in step S301;

[0031] S3022, set the path end trimming length parameter to , calculate the index value in the path S matrix matched by the parameter;

[0032] S3023, clipping and slicing the path to obtain a clipped path coordinate point set;

[0033] S303: Constructing a reference path generator based on the global route, including the following steps:

[0034] S3031. Smoothing the global route through a route smoother module;

[0035] S3032, calculate the current position of the ship using the Euclidean distance calculation formula Project the index position in the smoothed global path;

[0036] S3033: Projecting the index position of the ship in the global route based on the above steps , slicing the starting part of the smooth global route to obtain the current route point set, and then using the path clipping calculation module in step S302 to set the reference path length threshold, clipping the end of the route to obtain the reference path coordinate data;

[0037] S304: Constructing a reference line generator based on the reference path, including the following steps:

[0038] S3041. Reference line definition: The reference line is mainly used for conversion between Cartesian coordinate system and Frenet coordinate system. It consists of a series of continuous points and should be smooth. Therefore, the reference line must meet certain curvature constraints.

[0039] S3042. Calculate the maximum curvature of the reference path using a two-dimensional plane curve curvature calculation module;

[0040] S3043. Calculate the coordinate matrix of the points under the Cartesian coordinates of the reference line;

[0041] S3044, calculating the reference line orientation matrix, including the following steps:

[0042] S30441. Select a reference point;

[0043] S30442, determine adjacent points;

[0044] S30443, calculate direction vector;

[0045] S30444, reference line Each sample point Execute the above steps to obtain the reference line orientation angle matrix;

[0046] S3045, calculating the reference line cumulative S matrix through the path cumulative S matrix calculation module in step S301;

[0047] S3046. Calculate the reference line curvature kappa matrix and the curvature derivative dkappa matrix through the two-dimensional plane curve curvature calculation module.

[0048] As a preferred solution, in “step S40”, constructing a Cartesian coordinate and Frenet coordinate system converter unit based on the reference line includes the following steps:

[0049] S401, Cartesian coordinate system and Frenet coordinate system description:

[0050] S4011. The Cartesian coordinate system is a rectangular coordinate system that describes the position of an object (x, y) relative to the origin of the coordinate system. The Frenet coordinate system is based on a reference line and uses the tangent vector of the reference line. and the normal vector Establish a coordinate system, namely the Frenet coordinate system;

[0051] S4012, Frenet coordinate system describes the position of a vessel relative to a reference line;

[0052] S402. Target point status representation in Cartesian coordinate system and Frenet coordinate system:

[0053] S4021, target point in Cartesian coordinate system express: ;

[0054] S4022, target point in Frenet coordinate system express: ;

[0055] S403, find the matching point of the Cartesian coordinate system and the Frenet coordinate system, for the target point in the Cartesian coordinate system , calculate the Euclidean distance with each discrete point of the reference path, and the point with the shortest distance is the matching point;

[0056] S404. Calculate the target point in the Cartesian coordinate system The corresponding projection point is calculated based on the matching points obtained in the above steps. , including the following steps:

[0057] S4041. Calculate the vector between the target point and the matching point , and the tangent vector of the matching point ;

[0058] S4042, the target point The unit orthogonal vector at a certain point on the actual trajectory is defined as and , the unit orthogonal vector at the corresponding projection point on the reference line is defined as and ;

[0059] S405. Calculate the target point in the Cartesian coordinate system Convert to Frenet coordinate system target point , including the following steps:

[0060] S4051, by accumulating and summing the discrete points before the projection point on the reference line, the projection point in the Frenet coordinate system is obtained. value;

[0061] S4052, calculate horizontal coordinates ;

[0062] S4053, calculate lateral velocity ;

[0063] S4054, calculate longitudinal speed ;

[0064] S4055. Calculate the first derivative of the horizontal coordinate with respect to the vertical coordinate ;

[0065] S4056, calculate the second derivative of the horizontal coordinate with respect to the vertical coordinate ;

[0066] S4057, calculate longitudinal acceleration ;

[0067] S406. Calculate the target point in the Frenet coordinate system Convert the target point to the Cartesian coordinate system .

[0068] As a preferred solution, in "step S50", obtaining the current decision result of the ship through the maritime collision avoidance rules behavior decision module includes the following steps:

[0069] S501: Reference path data calculated based on step S30 , the collision avoidance risk hazard degree of the reference path is detected through the maritime collision avoidance rules behavior decision module, and the path decision result is obtained;

[0070] S502: If the decision result is If it is true, it means that the ship needs to overtake and bypass at this time. According to the International Regulations for Preventing Collisions at Sea, when two power-driven vessels meet on opposite or nearly opposite courses and there is a risk of collision, each should turn right, so that each passes the other vessel's port side. Therefore, it should be overtaking and bypassing to the right; if the decision result of whether to overtake is False, indicating that the path should remain on the original course.

[0071] As a preferred solution, in "step S60", constructing a path replanning decision unit includes the following steps:

[0072] S601, setting whether to re-plan the path decision condition 1: determine the current position of the ship Is the lateral deviation from the reference path too large?

[0073] S602: Setting the path re-planning decision condition 2: Determining the current angle of the ship Is the deviation from the reference path too large?

[0074] S603, setting the path re-planning decision condition three: based on the decision result of the maritime collision avoidance rules behavior decision module in step 50, whether to overtake , when whether to overtake When true, set the path replanning decision result to true;

[0075] S604: If the above conditions are not met, set the path re-planning decision result to false, then set the reference path Path planning results for the current frame.

[0076] As a preferred solution, in "step S70", constructing a path boundary decision maker unit based on maritime collision avoidance rules includes the following steps:

[0077] S701: When the result of path replanning in step S60 is false, step S70 does not need to be executed and the execution is exited;

[0078] S702, defining the path boundary The process of the path boundary generation unit is performed in the Frenet coordinate system, also known as the SL coordinate system;

[0079] S703, projecting the boundary line and obstacle data obtained in step S20 into the Frenet coordinate system to generate an SL map;

[0080] S704: Calculate path boundaries Direction boundary matrix;

[0081] S705, path-based Boundary, for each s coordinate, set the current maximum Upper boundary parameter and minimum Lower boundary parameter;

[0082] S706. Calculate the path in the SL coordinate system Boundary matrix, corresponding to the set of L boundary nodes;

[0083] S707, using the depth-first search algorithm to find the path The boundary matrix corresponds to the L boundary node matrix to perform path boundary search, and finally obtain multiple groups of path boundary sets;

[0084] S708: Determine a set of optimal path boundaries from multiple sets of path boundaries.

[0085] As a preferred solution, in “step S80”, constructing a path optimizer unit based on marine ship dynamics constraints includes the following steps:

[0086] S801. Construct the quadratic programming problem of path optimization in the Frenet coordinate system;

[0087] S802, defining optimization variables for the path optimization quadratic programming problem;

[0088] S803. Design the objective function of the quadratic programming problem for path optimization;

[0089] S804. Design path optimization quadratic programming problem constraints;

[0090] S805, solving the path optimization problem using an OSQP solver;

[0091] S806 , converting the path s position matrix and l position matrix obtained by the optimization in the above steps into an xy coordinate matrix through a Cartesian coordinate system and a Frenet coordinate system converter.

[0092] As a preferred solution, in "step S90", the speed planning module performs speed planning based on the path result, including the following steps:

[0093] S901, performing speed planning on the Cartesian coordinate path data obtained in the above steps through a speed planning module, and finally forming the current frame trajectory data;

[0094] S902: Send the trajectory data obtained from the current frame planning to the ship control module to control the navigation of the ship;

[0095] In "Step S100", establishing a monitoring continuous path planning operation mechanism includes the following steps: establishing a monitoring continuous path planning operation program, continuously looping from step S20 to step S90, and adjusting the path planning results in real time until the autonomous cruise global route navigation is completed.

[0096] Compared with the prior art, the present invention has the following beneficial effects:

[0097] The present invention makes autonomous decisions and plans collision avoidance paths based on real-time perceived environmental information in complex and changeable maritime environments without human intervention, thereby improving navigation safety and efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0098] Figure 1 is a flow chart of the present invention;

[0099] Figure 2 Schematic diagram of the Cartesian coordinate system of the present invention;

[0100] Figure 3 Schematic diagram of the Frenet coordinate system of the present invention;

[0101] Figure 4 This is a flow chart of step S10 of the present invention;

[0102] Figure 5 This is a flow chart of step S20 of the present invention;

[0103] Figure 6 This is a flow chart of step S30 of the present invention;

[0104] Figure 7 This is a flowchart of step S40 of the present invention;

[0105] Figure 8 This is a flow chart of step S50 of the present invention;

[0106] Figure 9 This is a flowchart of step S60 of the present invention;

[0107] Figure 10 This is a flowchart of step S70 of the present invention;

[0108] Figure 11 This is a flow chart of step S80 of the present invention;

[0109] Figure 12 This is a flowchart of step S90 of the present invention;

[0110] Figure 13 This is a flow chart of step S100 of the present invention. DETAILED DESCRIPTION

[0111] The present invention will be further described below with reference to the embodiments.

[0112] The following examples are intended to illustrate the present invention but are not intended to limit the scope of protection of the present invention. The conditions in the examples may be further adjusted according to specific conditions. Simple improvements to the method of the present invention within the scope of the present invention are also within the scope of protection claimed in the present invention.

[0113] See also Figure 1-13 The present invention provides a path planning method based on maritime collision avoidance rules, including S10, setting a global route for a ship autonomous cruising mission;

[0114] S20, obtaining ship positioning and environment perception prediction data;

[0115] S30, constructing a reference path and reference line generator unit based on the global route;

[0116] S40, constructing a Cartesian coordinate and Frenet coordinate system converter unit based on the reference line;

[0117] S50, obtaining the current decision result of the ship through the maritime collision avoidance rules behavior decision module;

[0118] S60, constructing a path replanning decision unit;

[0119] S70, constructing a path boundary decision unit based on maritime collision avoidance rules;

[0120] S80, constructs a path optimizer unit based on marine ship dynamics constraints;

[0121] S90, the speed planning module performs speed planning based on the path result;

[0122] S100. Establish a monitoring and continuous path planning operation mechanism.

[0123] In step S10, setting the global route of the ship autonomous cruise mission includes the following steps:

[0124] S101, marking the cruise mission route of the intelligent ship on the electronic map in sequence, and recording the positions on the marked cruise mission route to obtain a point queue of the cruise mission route;

[0125] S102: Convert the cruise mission route point queue into a Cartesian coordinate system through a latitude, longitude and Cartesian coordinate conversion module;

[0126] S103, performing waypoint compensation on two adjacent points in the global route point queue in the Cartesian coordinate system using a sample point interpolation algorithm to obtain a desired point queue, and using the desired point queue as the global route, that is, .

[0127] In "step S20", obtaining ship positioning and environment perception prediction data includes the following steps:

[0128] S201, the ship obtains the current GPS latitude and longitude in real time through its own global positioning system, recorded as , converted to the Cartesian coordinate system through the latitude and longitude & Cartesian coordinate system conversion module, recorded as ;

[0129] S202: The ship obtains the current speed in real time through its own configured speed calculation module, which is recorded as ;

[0130] S203, the ship obtains the current heading angle in real time through its own heading calculation module, which is recorded as ;

[0131] S204. The ship obtains obstacle boundary data through the perception prediction module, including two types of data: static obstacle boundary line data and obstacle data:

[0132] Static obstacle boundary line data consists of line geometry, specifically ;

[0133] Obstacle data includes static obstacles and dynamic obstacles, which are composed of polygonal geometry: ;

[0134] The data on movement disorders are as follows: All obstacle boundaries at all times , obstacle speed , obstacle direction .

[0135] In step S30, constructing a reference line and a reference path generator unit based on the global route includes the following steps:

[0136] S301, construct a path accumulation S matrix calculation module. Specifically, the S matrix is ​​a one-dimensional matrix, in which each element represents a route point To the starting point The cumulative path length between includes the following steps:

[0137] S3011. Calculate the distance on the path using the Euclidean distance calculation formula. Index Point To the starting point The cumulative length of s : , where i represents the i-th index path point, Indicates the cumulative length from the i-th index path point to the starting point.

[0138] S3012, traverse each path point , calculate the cumulative path length s value from each path point to the starting point, add the s value to the S matrix, and finally get the S matrix: .

[0139] S302: Constructing a path clipping calculation module, including the following steps:

[0140] S3021. Set of known path coordinate points: ;

[0141] The path cumulative S matrix calculation module in step S301 calculates the cumulative S matrix corresponding to the path: ;

[0142] S3022, set the path end trimming length parameter to , calculate the index value in the path S matrix matched by the parameter: , where is the index value of the end point of the path S matrix, is the cumulative s value of each point in the path, Trim length parameter for path ends The corresponding path end clipping index value;

[0143] S3023: The path end clipping index value obtained through the above steps , clip and slice the path, and get the clipped path coordinate point set as: ;

[0144] S303: Constructing a reference path generator based on the global route, including the following steps:

[0145] S3031. Smooth the global route using a route smoother module to obtain smoothed global route data: ;

[0146] S3032, calculate the current position of the ship using the Euclidean distance calculation formula Global route after smoothing Projection index position: , where is the global route tail index value, and The index on the global route is Corresponding point coordinate; and is the current position coordinate of the ship, The index value is the shortest distance between the ship position and the global route point. That is, the projected index position of the ship in the global route;

[0147] S3033: Projecting the index position of the ship in the global route based on the above steps , the starting part of the smooth global route is sliced ​​to obtain the current route point set, and then the path clipping calculation module in step S302 sets the reference path length threshold, clips the end of the route to obtain the reference path coordinate data: , where Indicates the mapping point of the ship in the reference path, that is, the starting point of the reference path;

[0148] S304: Constructing a reference line generator based on the reference path, including the following steps:

[0149] S3041. Reference line definition: The reference line is mainly used for conversion between Cartesian coordinate system and Frenet coordinate system. It consists of a series of continuous points and should be smooth. Therefore, the reference line must meet certain curvature constraints. The maximum curvature constraint of the reference line is set as: ;

[0150] S3042. Calculate the maximum curvature of the reference path using a two-dimensional plane curve curvature calculation module: , where is the reference path end point index value, Indicates that the index is The curvature value of the path point is is the maximum curvature value of the reference path;

[0151] S3043. Calculate the coordinate matrix of the reference line Cartesian coordinates: Dynamically set the reference line point coordinate matrix based on the maximum curvature value of the reference path. If the maximum curvature value of the reference path is: , then the reference path The first and last points are connected, and the linear interpolation calculation module is used to interpolate the points at equal intervals to obtain the Cartesian coordinate matrix of the reference line; otherwise, the reference path is set. is the coordinate matrix of the point under the Cartesian coordinates of the reference line. Since the curvature of the reference path meets the curvature requirements of the reference line, the coordinate matrix of the point under the Cartesian coordinates of the reference line is: ;

[0152] S3044, calculating the reference line orientation matrix, including the following steps:

[0153] S30441. Select reference point: The time point is used as the position where the tangential angle will be calculated;

[0154] S30442, determine adjacent points: The time point is taken as the adjacent point of the reference point, i.e. the next point on the reference line;

[0155] S30443. Calculate direction vector: Use adjacent coordinates to calculate the direction vector (vector), which represents the direction from the reference point to the next point. The calculation method of the direction vector is: ,in are the coordinates of the reference point, are the coordinates of adjacent points; calculate the orientation angle: use the inverse tangent function to calculate the tangent angle: ;

[0156] S30444, reference line Each sample point Execute the above steps to get the reference line orientation angle matrix: ;

[0157] S3045. Calculate the reference line cumulative S matrix through the path cumulative S matrix calculation module in step S301:

[0158] ;

[0159] S3046. Calculate the reference line curvature kappa matrix and the curvature derivative dkappa matrix using the two-dimensional plane curve curvature calculation module: ;

[0160] .

[0161] In step S40, constructing a Cartesian coordinate and Frenet coordinate system converter unit based on the reference line includes the following steps:

[0162] S401, Cartesian coordinate system and Frenet coordinate system description:

[0163] S4011. The Cartesian coordinate system is a rectangular coordinate system that describes the position of an object (x, y) relative to the origin of the coordinate system. The Frenet coordinate system is based on a reference line (usually a reference path, a series of discrete points) and uses the tangent vector of the reference line. and the normal vector Establish a coordinate system, namely the Frenet coordinate system.

[0164] S4012, Frenet coordinate system describes the position of the ship relative to the reference line. In this coordinate system, the ship itself is the origin, and the coordinate axes are perpendicular to each other. direction (i.e., along the reference line, usually called the longitudinal direction, represents the distance along the road, called the ordinate) and Direction (i.e. the current normal of the reference line, called the horizontal direction, This ensures that at every point on the reference line, the horizontal and vertical axes are perpendicular. The vertical axis represents the distance traveled on the reference line, and the horizontal axis represents the distance the ship deviates from the reference line.

[0165] S402. Target point status representation in Cartesian coordinate system and Frenet coordinate system:

[0166] S4021, target point in Cartesian coordinate system express: ,

[0167] in is the position coordinate, is the heading angle, For speed, is the acceleration, is the curvature of the actual trajectory at the current position;

[0168] S4022, target point in Frenet coordinate system express: ,

[0169] in: is the longitudinal coordinate value in the Frenet coordinate system, that is, the distance of the projection point along the reference path;

[0170] , The derivative with respect to time, that is, the velocity on the ordinate in the Frenet coordinate system;

[0171] , The derivative with respect to time, that is, the acceleration on the ordinate in the Frenet coordinate system;

[0172] is the lateral coordinate value in the Frenet coordinate system, that is, the lateral deviation between the target point and the reference line;

[0173] , The derivative with respect to time, i.e. the transverse velocity in the Frenet coordinate system;

[0174] , The derivative with respect to time, i.e. the lateral acceleration in the Frenet coordinate system;

[0175] , the first derivative of the horizontal coordinate with respect to the vertical coordinate in Frenet coordinates;

[0176] , the second derivative of the horizontal coordinate with respect to the vertical coordinate in Frenet coordinates;

[0177] S403, find the matching point of the Cartesian coordinate system and the Frenet coordinate system, for the target point in the Cartesian coordinate system , calculate the Euclidean distance with each discrete point of the reference path, and the point with the shortest distance is the matching point, recorded as ;

[0178] S404. Calculate the target point in the Cartesian coordinate system The corresponding projection point is calculated based on the matching points obtained in the above steps. , including the following steps:

[0179] S4041. Calculate the vector between the target point and the matching point , and the tangent vector of the matching point :

[0180] , ;

[0181] Then the projection point Approximately: ;

[0182] ; ;

[0183] S4042, the target point The unit orthogonal vector at a certain point on the actual trajectory is defined as and , the unit orthogonal vector at the corresponding projection point on the reference line is defined as and , which can be expressed as the corresponding heading angle:

[0184] ;

[0185] ;

[0186] S405. Calculate the target point in the Cartesian coordinate system Convert to Frenet coordinate system target point , including the following steps:

[0187] S4051, by accumulating and summing the discrete points before the projection point on the reference line, the projection point in the Frenet coordinate system is obtained. The value is: ;

[0188] S4052, calculate horizontal coordinates ;

[0189] The position vectors of the target point and the projection point in the Cartesian coordinate system are expressed as , we can get: ,but ;

[0190] S4053, calculate lateral velocity ;

[0191] , in the Cartesian coordinate system, The arc length of the target point trajectory during time is recorded as , the arc length of the movement corresponding to the projection point on the reference path is recorded as ,when hour, The modulus length is infinitely close to , the direction approaches :

[0192] ;

[0193] Similarly, we can get:

[0194] ;

[0195] ;

[0196] Finally, we have:

[0197] ;

[0198] S4054, calculate longitudinal speed

[0199] right Derivatives of both sides with respect to time: , put it into the above formula, and we get: , multiply both sides , the left and right sides are: ; ;

[0200] It turns out that: ;

[0201] S4055. Calculate the first derivative of the horizontal coordinate with respect to the vertical coordinate

[0202] because ,make get: ;

[0203] S4056, calculate the second derivative of the horizontal coordinate with respect to the vertical coordinate ;

[0204] ; ;

[0205] According to the definition of curvature: , , and because , we get: , and finally we can get:

[0206] ;

[0207] S4057, calculate longitudinal acceleration ;

[0208] because , according to the above formula, we can get:

[0209] ;

[0210] S4058, calculate lateral acceleration ;

[0211] ;

[0212] S406. Calculate the target point in the Frenet coordinate system Convert the target point to the Cartesian coordinate system , including the following steps:

[0213] ;

[0214] ;

[0215] ;

[0216] ;

[0217] ;

[0218] ;

[0219] in, is the position coordinate, is the heading angle, For speed, is the acceleration, is the curvature of the current position.

[0220] In step S50, obtaining the current decision result of the ship through the maritime collision avoidance rules behavior decision module includes the following steps:

[0221] S501: Reference path data calculated based on step S30 , the reference path collision avoidance risk is detected through the maritime collision avoidance rules behavior decision module, and the path decision result is obtained, specifically: whether to overtake the decision result, recorded as , and the decision results of each obstacle encounter scenario are: ignore, encounter, overtake, and cross;

[0222] S502: If the decision result is If it is true, it means that the ship needs to overtake and bypass at this time. According to the International Regulations for Preventing Collisions at Sea, when two power-driven vessels meet on opposite or nearly opposite courses and there is a risk of collision, each should turn right, so that each passes the other vessel's port side. Therefore, it should be overtaking and bypassing to the right; if the decision result of whether to overtake is False, indicating that the path should remain on the original course.

[0223] In step S60, building a path replanning decision unit includes the following steps:

[0224] S601, setting whether to re-plan the path decision condition 1: determine the current position of the ship Check whether the lateral deviation from the reference path is too large. The specific steps are as follows:

[0225] Calculate the current position of the ship using the Euclidean distance formula With reference path Lateral offset :

[0226] , where is the starting point of the reference path, i.e. the point where the ship is mapped on the reference path;

[0227] Set the maximum lateral deviation threshold between the ship and the reference path to ,when When , set the path replanning decision result to true;

[0228] S602: Setting the path re-planning decision condition 2: Determining the current angle of the ship Check whether the deviation from the reference path is too large. The specific steps are as follows:

[0229] Calculate the current angle of the ship With reference path Angle offset :

[0230] , where is the starting direction of the reference path;

[0231] Set the maximum angle deviation threshold between the ship and the reference path to ,when When , set the path replanning decision result to true;

[0232] S603, setting the path re-planning decision condition three: based on the decision result of the maritime collision avoidance rules behavior decision module in step 50, whether to overtake , when whether to overtake When true, set the path replanning decision result to true;

[0233] S604: If the above conditions are not met, set the path re-planning decision result to false, then set the reference path Path planning results for the current frame.

[0234] In step S70, constructing a path boundary decision maker unit based on maritime collision avoidance rules includes the following steps:

[0235] S701: When the result of path replanning in step S60 is false, step S70 does not need to be executed and the execution is exited;

[0236] S702, defining the path boundary The process of the path boundary generation unit is performed in the Frenet coordinate system, also known as the SL coordinate system;

[0237] S703: Projecting the boundary line and obstacle data obtained in step S20 into the Frenet coordinate system to generate an SL map, including the following steps:

[0238] S7031, set obstacle soft constraint buffer parameters , that is, the path detour and the lateral distance parameter of the obstacle. This parameter is generally larger than the hard constraint buffer parameter of the decision module to detect the collision risk. To be big;

[0239] S7032, Buffer parameters based on obstacle soft constraints , the boundary line data is calculated by the line buffer calculation module Calculate the soft constraint polygon of each boundary line, recorded as ;

[0240] S7033, based on the decision result of each dynamic obstacle encounter scenario obtained in step S50, traverse the obstacle data When the obstacle encounter scene is ignored, the obstacle soft constraint polygon is not generated; when the obstacle is a static obstacle or the encounter scene is crossed, the obstacle is calculated by the polygon buffer module. The obstacle boundary generates the soft constraint polygon of the obstacle at all times; when the obstacle is a dynamic obstacle and the encounter scene is overtaking or encountering, the obstacle is calculated by the polygon buffer module. to The obstacle boundary generates the soft constraint polygon of the obstacle at all times. According to the International Regulations for Preventing Collisions at Sea, when the ship and the obstacle ship meet the encounter or overtaking relationship, the ship should bypass it. represents the tail time of the moving obstacle trajectory that the ship should avoid and circumvent; based on this, all soft constraint polygon sets generated by the obstacle are obtained, which are recorded as: ;

[0241] S7034, soft constraint polygon set of boundary line Soft constraint polygon set with obstacles , in step S40, the Cartesian coordinate and Frenet coordinate system converter unit converts the Cartesian point coordinates of each polygon in the soft constraint polygon set into:

[0242] ;

[0243] Convert to Frenet coordinates:

[0244] ;

[0245] Each soft constraint polygon The boundaries are stored in the SL graph structure;

[0246] S704, calculate the path boundary S direction boundary matrix, with reference line The initial s position is set, and the sampling interval is set , to sample until the end of the reference line , which is the path boundary Directional boundary matrix:

[0247] ;

[0248] S705, path-based Boundary, for each s coordinate, set the current maximum Upper boundary parameter , minimum Lower boundary parameter ;

[0249] S706. Calculate the path in the SL coordinate system Boundary Matrix correspond The boundary node collection includes the following steps:

[0250] S7061, Definition The coordinates correspond to a Boundary node meaning: Indicates that At the coordinates, a passable The boundary interval is recorded as: ;

[0251] S7062, build The coordinates are accessible Boundary node set The calculation module includes the following steps:

[0252] S70621, define the polygon linear interpolation calculation module in SL coordinate system, give Coordinates, determine the current Whether the coordinates are in the S-axis coordinates of all points in the polygon If it is not within the range, there is no interpolation point; if it exists, it is calculated by the linear interpolation calculation module The polygon interpolation result is usually two points, sorted from small to large along the l coordinate axis: ;

[0253] S70622, Definition The coordinates correspond to the obstacle boundary point The coordinate value set is:

[0254] ;

[0255] in, For the maximum Upper boundary parameter, is the minimum Lower boundary parameter;

[0256] Traverse all obstacle soft constraint polygon SL coordinate boundary points in the SL graph, based on Coordinates, calculated by the polygon linear interpolation calculation module under the SL coordinate system in the above steps Is there an interpolation point at the coordinate? If so, interpolate the point obtained by interpolation. Coordinate values ​​added to In the collection; The coordinate value is the lower boundary value interpolated from each obstacle Sort from small to large and finally get The coordinates correspond to the obstacle boundary point Coordinate value set:

[0257] ;

[0258] S70623, calculation Coordinates, obstacle boundary point The coordinate value set corresponds to the index value obtained by the above steps The coordinates correspond to the obstacle boundary point Coordinate value set:

[0259] ;

[0260] right Calculate the lower and upper boundaries of each obstacle boundary intersection interpolation in the collection The values ​​correspond to index values, as follows:

[0261] ;

[0262] Among them, 0 means The corresponding index value, 1, 2 means the interpolation obtained by intersecting with a certain obstacle boundary Corresponding index value, for The corresponding index value;

[0263] S70624, calculation Coordinates, Set of traversable boundary nodes ; Starting with index 0, traverse ,calculate ,in, Indicates the width of the safe passage gap between the upper and lower boundaries of each two different obstacles; Caused by an obstacle Upper boundary, Represents an even index value, ; The next obstacle lower boundary, Indicates an odd index value, ;like If the l gap interval is impassable, it is discarded, otherwise the passable safe gap interval is initialized to Corresponding safe passage boundaries:

[0264] ;

[0265] Add to Set of traversable boundary nodes In the end, we get Coordinates, Set of traversable boundary nodes:

[0266] .

[0267] S7063, traverse the path in the SL coordinate system Boundary Matrix , calculate each through the above step S7052 The corresponding coordinates The boundary node set finally obtains the L boundary node matrix:

[0268] ;

[0269] in, express The corresponding coordinates The set of boundary nodes, express The corresponding coordinates A collection of boundary nodes.

[0270] S707, using the depth-first search algorithm to find the path Boundary Matrix Corresponding L boundary node matrix Perform path boundary search and finally obtain multiple sets of path boundary sets, which are expressed as:

[0271] ;

[0272] in, Represents a set of traversable path boundary structures, specifically including:

[0273] S boundary matrix in SL coordinate system:

[0274] ;

[0275] L lower boundary matrix in SL coordinate system:

[0276] ;

[0277] L upper boundary matrix in SL coordinate system:

[0278] ;

[0279] And S boundary matrix The number and length of the L lower boundary matrix and the L upper boundary matrix are equal;

[0280] S7071. The depth-first search (DFS) mentioned in the above steps is a graph or tree traversal algorithm that can be used to find specific nodes, backtrack, generate state space graphs, and other problems. DFS starts from a starting node and searches as deeply as possible along the path until it is unable to continue or reaches the target node. It then backtracks to the previous node and continues searching other unexplored paths until all nodes have been visited. The following are the basic steps of a depth-first search:

[0281] Initialization: Select a starting node as the current node, mark it as visited, and add it to the traversal path;

[0282] Explore adjacent nodes: Starting from the current node, select an unvisited adjacent node, make it the new current node, and mark it as visited. Add the new current node to the traversal path;

[0283] Recursive exploration: For the new current node, repeat step 2 and explore its unvisited adjacent nodes in depth. If there are no unvisited adjacent nodes, backtrack to the previous node.

[0284] Termination condition: When no further exploration is possible or the target node has been reached, the search ends;

[0285] Repeat the above steps until all nodes have been visited.

[0286] S708, at the boundary collection of multiple groups of paths In , deciding a set of optimal path boundaries includes the following steps:

[0287] S7081. Establish a path boundary evaluation cost function as follows:

[0288] ;

[0289] in is the path boundary S boundary matrix Length cost, Path boundary Offset cost, is the path boundary direction cost; Cost of length The weight value of for Offset cost The weight value of Direction cost The weight value of

[0290] S7082, build path boundary length cost :The shorter the length of the design path boundary S boundary matrix, the greater the cost The larger the rule, the more likely it is:

[0291] ;

[0292] in, The maximum s reference value of the S axis in the SL diagram, The length of the boundary matrix S of the current path;

[0293] S7083, build path boundary Offset cost , including the following steps:

[0294] Calculate the maximum value among the upper and lower L boundaries of the current path Upper boundary parameter , minimum Lower boundary parameter The maximum absolute value of the offset outside:

[0295] ;

[0296] design The larger the offset, the greater the cost. The larger the rule, the more likely it is:

[0297] ;

[0298] S7084, build path boundary direction cost , including the following steps:

[0299] Calculate the current path boundary direction. If the lower boundary of L The maximum value is greater than 0 and the upper boundary of L If the minimum value is also greater than 0, the path boundary direction is considered If it is, it will go to the left. Otherwise, it will go to the right.

[0300] According to the International Regulations for Preventing Collisions at Sea, if there is a collision or overtaking scenario in the current decision-making meeting, it should be bypassed to the right. Therefore, when the direction of the path boundary is bypassed to the right, the cost is The smaller, generally:

[0301] ;

[0302] Among them, when the path boundary direction is to detour to the right, is 0, otherwise .

[0303] S7085. Calculate the path boundary cost for multiple groups of path boundary sets, and select the one with the smallest cost as the current optimal path boundary result.

[0304] In step S80, constructing a path optimizer unit based on marine ship dynamics constraints includes the following steps:

[0305] S801. Construct a quadratic programming problem for path optimization in the Frenet coordinate system. The standard form of the quadratic programming problem is: ,in, is the optimization variable, and the objective function is represented by the semi-positive definite matrix and vector Definition, linear constraints are given by the matrix The sum vector l and vector u are defined as:

[0306] , , ;

[0307] S802. Define the optimization variables for the path optimization quadratic programming problem: define the optimization variables as the horizontal coordinate l in the Frenet coordinate system, the first-order derivative of the horizontal coordinate with respect to the vertical coordinate , the second derivative of the horizontal coordinate with respect to the vertical coordinate , here it is considered Approximately the lateral velocity, Approximate to lateral acceleration; based on the path boundary length result, the number of optimization samples is set to n, and the optimized state quantity is as follows:

[0308] ;

[0309] S803, designing the objective function of the path optimization quadratic programming problem, including the following steps:

[0310] S8031, the path is more in line with the reference line and the reference expected path:

[0311] ,

[0312] ;

[0313] S8032, smoother path, minimize lateral velocity, lateral acceleration, and lateral jerk:

[0314] ,

[0315] ,

[0316] ;

[0317] S8033, ensure that the path is close to the reference end point:

[0318] ;

[0319] S8034. Calculate the final optimization objective function as follows:

[0320] ;

[0321] in, , , Expressed as the lateral position optimization objective, , For the lateral speed optimization goal, , is the lateral acceleration optimization target, it is known that , is the lateral acceleration optimization target, is the lateral position slack variable, is the lateral velocity relaxation variable, is the lateral acceleration relaxation variable.

[0322] S804. Designing constraint conditions for the path optimization quadratic programming problem includes the following steps:

[0323] S8041, horizontal position Must be within the path boundaries and cannot collide with obstacles:

[0324] ;

[0325] S8042: To meet the ship's dynamic constraints, the transverse velocity, transverse acceleration, and transverse jerk have specific dynamic limits:

[0326] ,

[0327] ,

[0328] ;

[0329] S8043. Basic physical principles must be met:

[0330] ,

[0331] ,

[0332] ;

[0333] S8044, slack variables , Soft constraints:

[0334] ;

[0335] ;

[0336] ;

[0337] ;

[0338] ;

[0339] ;

[0340] S805. Solving the path optimization problem using an OSQP solver includes the following steps:

[0341] S8051,OSQP is an open source numerical optimization software for solving QP problems;

[0342] S8052. Set the objective function solution weight parameters and set the OSQP solution parameters;

[0343] S8053: Based on the path boundary data obtained in step S70, the reference path is converted into an s1 coordinate matrix through the Cartesian coordinate system and the Frenet coordinate converter, and the initial state is initialized to the starting s1 state point of the reference path. 、 , calculate the QP problem P,Q,l,A,u coefficient matrix through the QP matrix calculation module;

[0344] S8054. Construct an OSQP solver and obtain the optimization results. The optimized path data is as follows:

[0345] Path s position matrix:

[0346] ,

[0347] Path l position matrix:

[0348] ,

[0349] path matrix:

[0350] ,

[0351] path matrix:

[0352] .

[0353] S806, convert the path position matrix obtained by the above steps into a Cartesian coordinate system and a Frenet coordinate system converter. and l position matrix , converted to an xy coordinate matrix:

[0354] .

[0355] In step S90, the speed planning module performs speed planning based on the path result, including the following steps:

[0356] S901, the speed planning module calculates the Cartesian coordinate path data of the above steps. Perform speed planning and finally form the current frame trajectory data;

[0357] S902: Send the trajectory data obtained from the current frame planning to the ship control module to control the navigation of the ship;

[0358] In "Step S100", establishing a monitoring continuous path planning operation mechanism includes the following steps: establishing a monitoring continuous path planning operation program, continuously looping from step S20 to step S90, and adjusting the path planning results in real time until the autonomous cruise global route navigation is completed.

[0359] While 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 these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A path planning method based on maritime collision avoidance rules, characterized by: The following steps are involved: S10. Setting the global route of the ship's autonomous cruise mission; S20, obtaining ship positioning and environment perception prediction data; S30, constructing a reference path and reference line generator unit based on the global route; In step S30, constructing a reference path and a reference line generator unit based on the global route includes the following steps: S301, constructing a path cumulative S matrix calculation module; S302, constructing a path clipping calculation module; S303, constructing a reference path generator based on the global route; S304. Construct a reference line generator based on the reference path. The reference line is used for conversion between the Cartesian coordinate system and the Frenet coordinate system. It consists of a series of continuous points and should be smooth. The reference line should meet certain curvature constraints. S40, constructing a Cartesian coordinate and Frenet coordinate system converter unit based on the reference line; S50, obtaining the current decision result of the ship through the maritime collision avoidance rules behavior decision module; In "step S50", obtaining the current decision result of the ship through the maritime collision avoidance rules behavior decision module includes the following steps: S501, performing collision avoidance risk detection on a reference path through a maritime collision avoidance rule behavior decision module, and obtaining a path decision result; S502: If the overtaking decision result is true, it indicates that the vessel needs to overtake and make a detour; if the overtaking decision result is false, it indicates that the vessel should maintain the original course; S60, constructing a path replanning decision unit; S70, constructing a path boundary decision unit based on maritime collision avoidance rules; In step S70, constructing a path boundary decision maker unit based on maritime collision avoidance rules includes the following steps: S701: When the result of path replanning in step S60 is false, step S70 does not need to be executed and the execution is exited; S702, defining the path boundary The process of the path boundary generation unit is performed in the Frenet coordinate system, also known as the SL coordinate system; S703, projecting the boundary line and obstacle data obtained in step S20 into the Frenet coordinate system to generate an SL map; S704, calculate the path boundary S direction boundary matrix, with reference line is the initial s position; S705: Based on the path boundary S, set the current maximum Upper boundary parameter and minimum Lower boundary parameter; S706. Calculate the path boundary S matrix in the SL coordinate system, corresponding to the set of l boundary nodes; S707, performing a path boundary search on the path boundary S matrix corresponding to the boundary node matrix l by a depth-first search algorithm, and finally obtaining multiple groups of path boundary sets; S708. Determine a set of optimal path boundaries from multiple sets of path boundaries. S80, constructs a path optimizer unit based on marine ship dynamics constraints; S90, the speed planning module performs speed planning based on the path result; S100. Establish a monitoring and continuous path planning operation mechanism.

2. The path planning method based on maritime collision avoidance rules according to claim 1, characterized in that: In "step S10", setting the global route of the ship autonomous cruise mission includes the following steps: S101, marking the cruise mission route of the intelligent ship on the electronic map in sequence, and recording the positions on the marked cruise mission route to obtain a point queue of the cruise mission route; S102: Convert the cruise mission route point queue into a Cartesian coordinate system through a latitude, longitude and Cartesian coordinate conversion module; S103 , performing waypoint compensation on two adjacent points in the global route point queue in the Cartesian coordinate system using a sample point interpolation algorithm to obtain a desired point queue, and using the desired point queue as the global route.

3. The path planning method based on maritime collision avoidance rules according to claim 1, characterized in that: In "step S20", obtaining ship positioning and environment perception prediction data includes the following steps: S201. The vessel obtains the current GPS longitude and latitude in real time through its own global positioning system, and converts it into a Cartesian coordinate system through a latitude and longitude & Cartesian coordinate system conversion module; S202: The vessel obtains its current speed in real time through its own configured speed calculation module; S203, the vessel obtains the current heading angle in real time through its own heading calculation module; S204: The ship obtains obstacle boundary data through the perception prediction module.

4. The path planning method based on maritime collision avoidance rules according to claim 1, characterized in that: In step S40, constructing a Cartesian coordinate and Frenet coordinate system converter unit based on the reference line includes the following steps: S401, Cartesian coordinate system and Frenet coordinate system description; S402, target point status representation in Cartesian coordinate system and Frenet coordinate system; S403, searching for a matching point between the Cartesian coordinate system and the Frenet coordinate system; S404. Calculate the target point in the Cartesian coordinate system The corresponding projection point; S405, calculating the target point in the Cartesian coordinate system and converting it to the target point in the Frenet coordinate system; S406: Calculate and convert the target point in the Frenet coordinate system to the target point in the Cartesian coordinate system.

5. The path planning method based on maritime collision avoidance rules according to claim 1, characterized in that: In step S60, constructing a path replanning decision unit includes the following steps: S601, setting a first condition for determining whether to re-plan the path: determining whether the lateral offset between the current position of the vessel and the reference path is too large. When the offset exceeds a threshold, setting the result of the decision on whether to re-plan the path to true; S602, setting the second decision condition for whether to re-plan the path: determining whether the current angle of the ship deviates too much from the reference path heading. When the deviation exceeds a threshold, setting the decision result for whether to re-plan the path to true; S603, setting the path replanning decision condition three: based on the decision result of the maritime collision avoidance rules behavior decision module in step 50, whether to overtake, if the overtaking is true, setting the path replanning decision result to true; S604: When the above conditions are not met, the decision result of whether to re-plan the path is set to false, and the reference path is set to the path planning result of the current frame.

6. The path planning method based on maritime collision avoidance rules according to claim 1, characterized in that: In step S80, constructing a path optimizer unit based on marine vessel dynamics constraints includes the following steps: S801. Construct the quadratic programming problem of path optimization in the Frenet coordinate system; S802, defining optimization variables for the path optimization quadratic programming problem; S803. Design the objective function of the quadratic programming problem for path optimization; S804. Design path optimization quadratic programming problem constraints; S805, solving the path optimization problem using an OSQP solver; S806 , converting the optimized path s position matrix and l position matrix into an xy coordinate matrix through a Cartesian coordinate system and a Frenet coordinate system converter.

7. The path planning method based on maritime collision avoidance rules according to claim 1, characterized in that: In step S90, the speed planning module performs speed planning based on the path result, including the following steps: S901, performing speed planning on the path data under Cartesian coordinates through a speed planning module, and finally forming the current frame trajectory data; S902: Send the trajectory data obtained from the current frame planning to the ship control module to control the navigation of the ship; In "step S100", establishing a monitoring and continuous path planning operation mechanism includes the following steps: establishing a monitoring and continuous path planning operation program, continuously looping from step S20 to step S90, and adjusting the path planning results in real time until the autonomous cruise global route navigation is completed.

Images