A shipborne special vehicle transportation trajectory planning method considering driving habits

Abstract

A method for planning shipborne special vehicle transport trajectories that takes driving habits into account belongs to the field of intelligent equipment support. The method first establishes an optimal control problem for the transport trajectory planning problem; second, generates an expansion map to transform collision avoidance requirements; third, uses an improved RRT algorithm to obtain a coarse path; fourth, resamples the coarse path to obtain an initial reference solution; fifth, generates a safe transport channel and simplifies the form of collision avoidance constraints; sixth, transforms nonlinear constraints, applies a forward guidance penalty function, establishes a final optimal control problem, and iteratively solves it. The present invention can quickly generate a high-quality initial guess, increase trajectory planning efficiency, and can continuously guide shipborne special vehicles to travel in the forward direction and update the safe channel through iteration, increasing the proportion of forward travel in the trajectory and reducing the impact of the initial guess on the final solution, thereby obtaining a high-quality transport trajectory that conforms to driving habits.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent equipment support and relates to a shipborne special vehicle transportation trajectory planning method taking driving habits into consideration, in particular to a shipborne special vehicle transportation trajectory planning method combining an RRT* algorithm within a bidirectional ellipse with kinematic constraints and an artificial potential field and a forward guidance penalty function. Background Art

[0002] Unlike land-based airports, aircraft carrier decks are smaller, and the number and complexity of aircraft and equipment on board are high. This results in a narrower deck environment for shipborne special vehicles, complicating the planning of their maneuvering trajectories. Currently, more mature trajectory planning methods typically employ a multi-stage framework. Given the complex and densely populated aircraft carrier deck environment, an efficient initial coarse path planning method should be employed to ensure a kinematically feasible initial reference path can be quickly planned, thereby reducing the overall planning process time. For example, the hybrid A* algorithm used in the Chinese invention patent "A Method for Aircraft Carrier Deck Taxiing Trajectory Planning Based on a Safe Maneuvering Corridor" (CN115328165A) is biased towards endpoint searches due to the influence of a heuristic function. This results in excessive and redundant searches at the edge of obstacles, further increasing planning time. At the same time, the existing research on shipborne special vehicle transportation trajectory planning algorithms often ignores the fact that excessive vehicle reversing is inconsistent with driving habits. For example, the Chinese invention patent "A tractor trajectory iterative planning method based on an improved search random tree algorithm" (CN119066986A) does not take the tractor's driving habits into consideration. It is necessary to propose a forward guidance strategy to enable shipborne special vehicles to maintain forward driving as much as possible.

[0003] In view of the above problems, it is necessary to propose an efficient optimal control problem model and solution framework and a reasonable forward guidance strategy to efficiently generate a high-precision, collision-free, high-quality and driving-habit-compliant transportation trajectory for shipborne special vehicles. Summary of the Invention

[0004] In order to solve the technical problems encountered above, the present invention proposes a shipborne special vehicle transportation trajectory planning method that takes driving habits into consideration, and adopts a three-stage planning strategy. First, an RRT* algorithm within a bidirectional ellipse that considers kinematic constraints and combines an artificial potential field is used to search for a coarse path that meets the constraints of the shipborne special vehicle, ensuring that a high-quality trajectory can be found within the traversable space in a complex obstacle environment. Secondly, the coarse path is resampled to obtain the initial guess of the optimal control problem, and a safe channel is constructed with the help of the sampling points to achieve an efficient expression of the obstacle avoidance constraints and improve the overall solution efficiency. Finally, the nonlinear constraints in the optimal control problem are incorporated into the objective function as a penalty term to reduce the difficulty of solving the problem. At the same time, a forward-guided penalty function is introduced into the objective function, which can enable the solved trajectory to maintain forward driving as much as possible.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is:

[0006] A method for planning the transport trajectory of shipborne special vehicles taking into account driving habits, the method comprising the following steps:

[0007] Step 1: Establish the optimal control problem of the transportation trajectory planning problem;

[0008] Step 1-1: Describe the kinematic constraints of shipborne special vehicles;

[0009] Define the various parameters of shipborne special vehicles, and the rear axle center position is recorded as , It's speed, is the front wheel turning angle, is the attitude angle, is the acceleration, is the steering angular velocity, represents the wheelbase. The kinematic constraints of the shipborne special vehicle are described as the following differential equations:

[0010] (1)

[0011] in, For the end time, For shipborne special vehicles The horizontal coordinate of the center of the posterior circle at the moment, For shipborne special vehicles The vertical coordinate of the center of the posterior circle at the moment, For shipborne special vehicles The speed of time, For shipborne special vehicles The front wheel angle at the moment, For shipborne special vehicles The posture angle of the moment, For shipborne special vehicles The acceleration of time, For shipborne special vehicles The steering angular velocity at the moment.

[0012] The state variables of shipborne special vehicles are recorded as , the control variable is recorded as The differential equation shown in formula (1) can be simplified as . express right Derivative, function In addition, according to the mechanical characteristics of shipborne special vehicles, the following constraints must be met during their driving process:

[0013] (2)

[0014] in, 、 、 、 Respectively represent the minimum acceleration, speed, steering angular velocity and steering angle allowed for shipborne special vehicles; 、 、 、 They respectively represent the maximum acceleration, speed, steering angular velocity and steering angle allowed for shipborne special vehicles.

[0015] Step 1-2: Describe the transportation task;

[0016] The starting and ending configurations of the transport task determine the boundary conditions of the optimal control problem. At the initial moment , the configuration of shipborne special vehicles is recorded as:

[0017] (3)

[0018] in, 、 、 、 、 、 and They respectively represent the horizontal coordinate of the rear axle center point, the vertical coordinate of the rear axle center point, the heading angle, the speed, the steering angle, the acceleration and the steering angular velocity of the shipborne special vehicle at the starting moment.

[0019] At the terminal moment ( is also a variable to be optimized), the configuration of the shipborne special vehicle is recorded as:

[0020] (4)

[0021] (5)

[0022] in, 、 、 、 、 、 and They respectively represent the horizontal coordinate of the rear axle center point, the vertical coordinate of the rear axle center point, the heading angle, the speed, the steering angle, the acceleration and the steering angular velocity of the shipborne special vehicle at the terminal moment.

[0023] Steps 1-3: Describe collision avoidance constraints;

[0024] The space containing all shipborne special vehicle configurations is recorded as configuration space , all configurations where shipborne special vehicles collide with obstacles are recorded as impassable spaces , so the traversable space is recorded as . Using vector-valued functions The state variable Mapping to traversable space On the subspace of , the collision avoidance constraint is expressed as:

[0025] (6)

[0026] Step 1-4: Establish the objective function;

[0027] In the process of dispatching shipborne special vehicles, it is necessary to consider various requirements such as time, energy consumption and stability. Energy consumption and stability are reflected by the control variables and state variables. The objective function is represented by the weighted sum of the terminal time and the integral terms of the control variables and state variables:

[0028] (7)

[0029] in, is the integral term weight coefficient, and are the weight matrices of state and control variables, is the objective function of the optimal control problem, is the state variable of the shipborne special vehicle, is the transpose of the state variables of the shipborne special vehicle, is the control variable of the shipborne special vehicle, is the transpose of the control variables of the shipborne special vehicle.

[0030] Steps 1-5: Establish the initial optimal control problem;

[0031] In summary, the initial optimal control problem is established , its specific form is:

[0032] (8)

[0033] Step 2: Generate expansion map;

[0034] Step 2-1: Convert the representation of shipborne special vehicles;

[0035] The geometric parameters of shipborne special vehicles include: front suspension length , rear suspension length , wheelbase , body width Take the two quarter points of the longitudinal axis of the shipborne special vehicle and record them as and , the calculation formula of the two circle centers is as follows:

[0036] (9)

[0037] The radii of the two characteristic circles are equal, denoted as ; Among them, time, and Represent the abscissa and ordinate of the front characteristic circle respectively; and They represent the abscissa and ordinate of the posterior characteristic circle respectively; and They represent the horizontal and vertical coordinates of the center point of the rear axle of the tractor respectively.

[0038] Shrink the characteristic circle to the center and move the obstacle to the Expand, and the expanded map is called the expanded map, recorded as This achieves the transformation of the collision avoidance constraints, that is, making the center of the characteristic circle and the expanded obstacle not coincident. This transformation helps to generate a box-shaped safe passage and further simplify the obstacle avoidance constraints.

[0039] Step 2-2: Convert the expanded map to a raster map;

[0040] Expand the map Convert to a raster map and record the raster map as . Set the resolution to , take any point on the aircraft carrier deck as the origin of the two-dimensional rectangular coordinate system, and determine the map boundaries along the x-axis and y-axis according to the length and width of the aircraft carrier deck, which are recorded as and ,in and Respectively represent the minimum and maximum coordinates in the x-axis direction, and Represents the minimum and maximum coordinates of the y-axis direction respectively. Define the grid Represents a two-dimensional rectangular area ,in is the index of the grid in the x-axis direction, is the index of the grid in the y-axis direction; Indicates the coordinates contained in the raster.

[0041] For each grid, if it is consistent with the expansion map If there are overlapping parts of the expanded obstacles in the grid, it is considered as an obstacle grid; otherwise, it is considered as a passable grid.

[0042] Step 3: Use the improved RRT* algorithm to obtain the coarse path;

[0043] Step 3-1: Initialization;

[0044] For the bidirectional search strategy, two trees are maintained, which are recorded as and Initialize the two trees and Initialize as starting point ,Will Initialization endpoint , the node information includes position information and orientation angle information. For target offset sampling within the ellipse, the range of the ellipse and target offset must be determined. and The Euclidean distance between , the major axis of the ellipse is defined as ,in is a constant; the minor axis is defined as The two foci of the ellipse are and , the range within the ellipse is .

[0045] Take a radius of ,for , the target bias range is With The range is within the circle with radius ;for , the target bias range is With The range is within the circle with radius .

[0046] Step 3-2: Compare the sizes of the two trees;

[0047] statistics and The number of nodes on and .Compare and The size of If it is smaller, go to step 3-4; otherwise, go to step 3-3.

[0048] Step 3-3: Expand the tree ;

[0049] Step 3-3-1: target bias sampling within the ellipse;

[0050] Set a fixed probability , pick a random number ,if , then in Get a random two-dimensional point in Get a random two-dimensional point in . The obtained two-dimensional point is recorded as , which can guide the tree to expand in the target direction to a certain extent, while reducing invalid sampling on the basis of ensuring a certain degree of randomness;

[0051] Step 3-3-2: Find the tree Random point on the distance The point with the smallest Euclidean distance is recorded as the closest point ;by Activate the tree for the root node growth;

[0052] Step 3-3-3: Generate child nodes;

[0053] Step 3-3-3-1: Integrate to generate a set of child nodes to be selected;

[0054] When generating child nodes, the path between the root node and the child nodes should be smooth to meet the kinematic constraints of the shipborne special vehicle and to reduce the burden of back-end optimization. According to the mechanical constraints of the shipborne special vehicle in formula (2), Get a random front wheel angle from Get a random speed, repeat times, get A combination of front wheel angle and speed .

[0055] For each set of front wheel angle and speed, The horizontal axis , vertical axis and orientation angle is the initial state, with a time step is the integration step, and according to formula (10), we get The child nodes to be selected are denoted as , the set of paths corresponding to each child node to be selected is recorded as .

[0056] (10)

[0057] Step 3-3-3-2: Perform collision avoidance detection on the selected sub-nodes;

[0058] Pair Collection All paths in the set are collided and the paths that collide are deleted from the set of paths. The resulting set of collision-free paths is recorded as ; The corresponding node from The resulting collision-free node set is recorded as

[0059] Step 3-3-3-3: Calculate the comprehensive cost and determine the child nodes;

[0060] right Calculate the comprehensive cost of all nodes in the, comprehensive cost includes: (1) change cost If the transport entity is moving forward, the speed direction is recorded as 1, otherwise it is -1. One thing to note is that since the possibility of moving forward and backward is the same when expanding from the first node of each tree, the speed direction of the starting node of each tree is recorded as 0. For other nodes, if the speed direction between the selected child node and its parent node is the same, then the reversing cost ,otherwise ; (2) Obstacle repulsion cost , its calculation formula is shown in equation (11), where is the distance from the selected child node to the obstacle, Is a threshold that limits the range affected by obstacles, where the threshold The value of is usually an integer multiple of the wheelbase length of the shipborne special vehicle; (3) The gravitational cost of the target point ,in is the distance from the selected child node to the target point; (4) the distance cost between the random sampling point , that is, the child node to be selected to the random sampling point The distance between them.

[0061] (11)

[0062] Combining the above four parts of the cost, the weighted sum is obtained to obtain the comprehensive cost , its formula is shown in (12), where , , , , which represent the weight coefficients of the reversing cost, the obstacle repulsion cost, the target point's gravitational cost, and the distance cost between the random sampling point. The child node with the lowest comprehensive cost is selected and recorded as .

[0063] (12)

[0064] Step 3-3-4: rewiring;

[0065] Set the rewiring threshold length to ;by As the center of the circle, As the radius, determine a circle, recorded as ; the tree All on the circle The set of nodes in .

[0066] for All nodes in the The Reeds-Shepp curve is used to determine whether it will collide with an obstacle. All points that collide with obstacles are removed, and then the one with the smallest length is found in the remaining Reeds-Shepp curves. The nodes in The parent node of .

[0067] Step 3-4: Expand the tree ;

[0068] Step 3-4-1: target bias sampling within the ellipse;

[0069] Set a fixed probability , pick a random number ,if , then in Get a random two-dimensional point in Get a random two-dimensional point from . The two-dimensional point obtained above is recorded as , which can guide the tree to expand in the target direction to a certain extent, while reducing invalid sampling on the basis of ensuring a certain degree of randomness;

[0070] Step 3-4-2: Find the tree Random point on the distance The point with the smallest Euclidean distance is recorded as the closest point ;by Activate the tree for the root node growth;

[0071] Step 3-4-3: Generate child nodes;

[0072] Step 3-4-3-1: Integrate to generate a set of child nodes to be selected;

[0073] When generating child nodes, the path between the root node and the child nodes should be smooth to meet the kinematic constraints of the shipborne special vehicle and to reduce the burden of back-end optimization. According to the mechanical constraints of the shipborne special vehicle in formula (2), Get a random front wheel angle from Get a random speed, repeat times, get A combination of front wheel angle and speed For each set of front wheel angle and speed, The horizontal axis , vertical axis and orientation angle is the initial state, with a time step is the integration step, and according to formula (10), we get The child nodes to be selected are denoted as , the set of paths corresponding to each child node to be selected is recorded as .

[0074] Step 3-4-3-2: Perform collision avoidance detection on the selected sub-nodes;

[0075] Pair Collection All paths in the set are collided and the paths that collide are deleted from the set of paths. The resulting set of collision-free paths is recorded as ; The corresponding node from The resulting collision-free node set is recorded as

[0076] Step 3-4-3-3: Calculate the comprehensive cost and determine the child nodes;

[0077] right All nodes in the calculation of their comprehensive cost, the comprehensive cost includes: (1) the cost of change If the transport entity is moving forward, the speed direction is recorded as 1, otherwise it is -1. One thing to note is that since the possibility of moving forward and backward is the same when expanding from the first node of each tree, the speed direction of the starting node of each tree is recorded as 0. For other nodes, if the speed direction between the selected child node and its parent node is the same, then the reversing cost ,otherwise ; (2) Obstacle repulsion cost , the calculation formula is shown in equation (11); (3) The gravitational cost of the target point ,in is the distance from the selected child node to the target point; (4) the distance cost between the random sampling point , that is, the child node to be selected to the random sampling point The distance between them.

[0078] Combining the above four parts of the cost, the weighted sum is obtained to obtain the comprehensive cost , its formula is shown in (12), where , , , , which represent the weight coefficients of the reversing cost, the obstacle repulsion cost, the target point's gravitational cost, and the distance cost between the random sampling point. The child node with the lowest comprehensive cost is selected and recorded as .

[0079] Step 3-4-4: rewiring;

[0080] Set the rewiring threshold length to ;by As the center of the circle, As the radius, determine a circle, recorded as ; the tree All on the circle The set of nodes in .for All nodes in the The Reeds-Shepp curve is used to determine whether it will collide with an obstacle. All points that collide with obstacles are removed, and then the one with the smallest length is found in the remaining Reeds-Shepp curves. The nodes in The parent node of .

[0081] Step 3-4: Try to connect the two trees;

[0082] Find the two closest points on the two trees and record them as 、 , generating a Reeds-Shepp curve between the two , perform collision detection. If the Reeds-Shepp curve does not collide with the obstacle, stop searching and go to step 3-5; otherwise, go to step 3-2.

[0083] Step 3-5: Backtrack to the parent node and return a path;

[0084] from Start backtracking to the parent node until , recorded as ;from Start backtracking to the parent node until , and then arrange the points on the path in reverse order, recorded as ; Return a rough path

[0085] In summary, a RRT* algorithm in a bidirectional ellipse with kinematic constraints combined with an artificial potential field is used to consider the kinematic constraints of the shipborne special vehicle and the rear axle center of the shipborne special vehicle is Plan a rough path .remember The number of nodes in , among which The nodes are , whose coordinates are defined as .

[0086] Step 4: Resample the coarse path to get the initial guess;

[0087] Step 4-1: Estimate transportation time;

[0088] The length of the coarse path is defined as the sum of the Euclidean distances between all adjacent nodes, denoted as ; Define The length of the segment path is ;but .in, For the The horizontal coordinate of the starting point of the segment path; For the The horizontal coordinate of the end point of the segment path; For the The vertical coordinate of the starting point of the segment path; For the The horizontal coordinate of the end point of the segment path.

[0089] Assume that the transport trajectory of shipborne special vehicles meets the time optimization principle, that is, the variable speed movement always adopts the maximum or minimum acceleration, and the uniform speed movement runs at the maximum or minimum speed; when the shipborne special vehicle moves forward, the maximum speed, the maximum acceleration of acceleration, and the maximum acceleration of deceleration are defined as 、 and ; When the shipborne special vehicle is reversing, the maximum speed, the maximum acceleration of acceleration and the maximum acceleration of deceleration are defined as , and At the same time, define the maximum acceleration distance , maximum deceleration distance The sum of the maximum acceleration distance and the maximum deceleration distance . No. The transport time of the segment path is recorded as , the estimation formula is (13), then the estimation formula of the total transportation time is expressed as .

[0090] (13)

[0091] Step 4-2: Resample at discrete points;

[0092] Will The time period is divided into interval, thus we get time nodes, the length of each interval is , No. The time corresponding to the time node is .exist At time t, the state variables of the shipborne special vehicle are recorded as The speed and acceleration are estimated based on the time optimal principle; the steering angle of the front wheel of the shipborne special vehicle and steering angular velocity , according to the kinematic constraint estimation, the calculation formula is as follows:

[0093] (14)

[0094] (15)

[0095] in, 、 、 and Indicates Front wheel steering angle, heading angle, speed and angular velocity of shipborne special vehicles at all times; and Indicates The front wheel steering angle and heading angle of the shipborne special vehicle at the moment.

[0096] End Time Initialized to , at all time nodes, the state variables and control variables are initialized to The resampled path is recorded as .

[0097] Step 5: Generate a safe transportation channel based on the resampled path points and transform the obstacle avoidance constraints;

[0098] Step 5-1: Put in Separate them and combine them with formula (9) to calculate the centers of the two characteristic circles corresponding to each path point, which are recorded as and .

[0099] Step 5-2: Separately and The construction methods of the two are the same. Take as an example to illustrate the method of generating a safe transport corridor. No. points The corresponding safe transportation channel is recorded as .

[0100] First, initialize for point , treat it as a rectangle with a width of 0 and a length of 0. Define the exploration direction set . With a fixed step size , in order to The left, right, bottom and top boundaries of the safe transportation channel obtained by the previous step are defined as ,along The four directions in the direction are expanded in turn. When the above expansion is performed, the safe transport channel data is updated according to the following rules: Only when the following two conditions are met, it is considered a valid expansion and the safe transport channel information is updated accordingly: 1) The safe transport channel must not overlap with the grid map. Any obstacle grid overlaps; 2) The safe transportation channel is in the direction The length of the upper extension does not exceed the extension threshold Otherwise, the expansion is considered to have failed and the exploration direction is set Delete direction This expansion process continues until the set of exploration directions Empty.

[0101] right Each point is expanded to obtain The left, right, bottom and top boundaries are .

[0102] Step 5-3: Perform step 5-2 to get each point The left, right, bottom and top boundaries are . Implement the following collision constraint transformations as shown below:

[0103] (16)

[0104] Step 6: Transform the nonlinear constraints, apply the forward guidance penalty function, establish a lightweight optimal control problem and solve it iteratively;

[0105] Step 6-1: Add the penalty function other than the nonlinear constraint to the objective function;

[0106] The initial optimal control problem in steps 1-5 as shown in formula (8) is The obstacle avoidance constraint in is replaced by the description form in formula (16), forming the optimal control problem , as shown in formula (17):

[0107] (17)

[0108] In order to reduce the dependence of the optimal control problem on the initial guess, and because building a safe transport channel will inevitably waste some feasible space, an iterative solution strategy for the optimal control problem is adopted, and the trajectory obtained is used as the initial guess for the next solution. Based on the initial guess, the safe transport channel is regenerated using the method for generating a safe transport channel mentioned in step 5. There are still some nonlinear constraints in the present invention, so the present invention needs to Further transformation. The nonlinear constraints in are softened by external penalty functions, including the kinematic equality constraints shown in formula (1) and formula (9), and the terminal boundary conditions shown in formula (5), which are merged into the cost function shown in formula (7). Specifically:

[0109] Step 6-1-1: First, transform the kinematic constraints as shown in formula (1). According to the above mentioned step 1-1 It can be deduced that:

[0110] (18)

[0111] in, Indicates the time points; express State variables at the moment; express State variables at the moment;

[0112] Move the right side of the equal sign of the first equation in formula (18) to the left side, and record it as:

[0113] (19)

[0114] Then the external penalty function of the kinematic constraint shown in formula (1) is expressed as: ;

[0115] Step 6-1-2: Next, transform formula (9) by moving the left side of the equation to the right side of the equation, and express it as the following vector-valued function:

[0116] (20)

[0117] Then the external penalty function of the kinematic constraint shown in formula (9) is expressed as: ; The external penalty function of the boundary constraint shown in formula (5) is expressed as: .

[0118] Step 6-1-3: Combine the above three external penalty functions into: . This results in a cost function with a penalty term :

[0119] (twenty one)

[0120] in, is the objective function defined by formula (7), It is the weight coefficient of the external penalty function and should be set to a large number, usually an integer multiple of 100, to ensure that the penalty for violating the transformed nonlinear constraints dominates.

[0121] Step 6-1-4: Finally, we get the lightweight optimal control problem , as shown in formula (22):

[0122] (twenty two)

[0123] Step 6-2: Introduce the forward guidance penalty function into the objective function shown in formula (22);

[0124] According to the general driving habits of shipborne special vehicles, it is always hoped that the shipborne special vehicles can be driven forward as much as possible while ensuring the shortest possible time. Therefore, the reversing state of the shipborne special vehicles can be penalized to reduce the proportion of reversing driving as much as possible. in Take it out separately and map it using the sigmoid function: ,in is a constant used to control the smoothness of the function. The sigmoid function Mapped to This is a monotonically decreasing function, and its minimum value is at The maximum value of the function is obtained at Therefore, the penalty function of the forward guidance is as follows: . Then the objective function with the forward guidance penalty function is:

[0125] (twenty three)

[0126] in, is the weight coefficient of the forward guidance penalty function. One thing to note is that the coefficient should be selected so that and If the coefficient is too small, the forward guidance penalty will be ineffective. If the coefficient is too large, the dispatching entity will pay too much attention to the forward driving during the driving process, which will greatly increase the driving time. In summary, it is set to: .in, is the estimated time obtained by resampling, is the number of resampled nodes. Setting the penalty coefficient in this way can make the forward guidance penalty adaptively change according to the transportation time, so that the transportation time and the forward guidance penalty are on an equal footing. The optimal control problem with the forward guidance penalty function is recorded as , the formula is as follows:

[0127] (twenty four)

[0128] Step 6-3: Iteratively solve the optimal control problem with the forward guidance penalty function;

[0129] First iteration solution The initial guess of is obtained in step 4, and the safe transport channel of the first iteration is obtained in step 5; the trajectory obtained after the first iteration is used as the initial guess of the next iteration, and the corresponding safe channel is obtained in step 5; the termination condition of the iteration is a strict equality constraint that requires the kinematic constraints shown in formula (1) and formula (9) and the boundary constraints shown in formula (5) to be satisfied. Therefore, in each iterative solution process, the improvement , until the obtained Less than the allowable error is 10 -6 ~10 -4 .

[0130] The innovations of this invention include: proposing an improved bidirectional RTT method that considers kinematics, including: using elliptical target bias sampling during random sampling to limit the sampling range to an ellipse bounded by the start and end points, while simultaneously guiding the two trees to expand toward each other with a certain probability, thereby reducing invalid sampling and accelerating convergence; employing an integral drive strategy when generating new nodes, ensuring that the searched path satisfies the kinematic constraints of shipborne special vehicles, thereby obtaining a smooth path and reducing the burden on back-end optimization; and introducing the concept of an artificial potential field to reduce invalid sampling at the edges of obstacles during the tree expansion process. Furthermore, a forward guidance penalty function is proposed, which enables, when the initial guess contains a high proportion of reverses in the entire transport process, iteratively solving the optimal control problem with the forward guidance penalty function to obtain the final trajectory, while ensuring the principle of time optimization, significantly reducing the high proportion of reverses in the entire transport process.

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

[0132] This invention achieves the conversion of obstacle avoidance constraints through a safe maneuvering channel, significantly reducing the difficulty of solving the problem and ensuring efficient generation of shipborne special vehicle maneuvering trajectories. By combining an artificial potential field with a kinematically constrained bidirectional ellipse, the RRT* algorithm efficiently generates a coarse trajectory in large-scale, densely obstacle-filled deck environments. This coarse path resampling process provides a high-quality initial guess for the optimal control problem, and the resampling results are combined to generate a safe channel. By representing the shipborne special vehicle body with two characteristic circles, the geometry of the tractor is accurately described, fully utilizing the feasible space. A forward guidance penalty function, combined with an iterative strategy, continuously guides the shipborne special vehicle forward and updates the safe channel. This mitigates the impact of the initial guess on the optimality of the final solution and the proportion of reversing time to the total maneuvering time. Even in situations where the initial guess is poor and the proportion of reversing time to the total maneuvering time is high, a high-quality maneuvering trajectory that conforms to driving habits can be obtained. BRIEF DESCRIPTION OF THE DRAWINGS

[0133] Figure 1 Flowchart of the present invention.

[0134] Figure 2 This is a picture of the car model.

[0135] Figure 3 The graph is represented by two characteristic circles.

[0136] Figure 4 Schematic diagram of obstacle avoidance method.

[0137] Figure 5 Schematic diagram of obstacle avoidance method.

[0138] Figure 6 This is a map of an aircraft carrier deck.

[0139] Figure 7 A path planning diagram.

[0140] Figure 8 This is a diagram showing the speed change of the planning process.

[0141] Figure 9 This is the steering angle change diagram during the planning process.

[0142] Figure 10 It is the diagram of the change of the orientation angle during the planning process.

[0143] Figure 11 This is the acceleration change diagram of the planning process.

[0144] Figure 12 The diagram shows the change of the steering angular velocity during the planning process. DETAILED DESCRIPTION

[0145] The present invention will be further described below with reference to specific embodiments.

[0146] Considering the problem of planning the transportation of a certain type of shipborne special vehicle on the deck of an aircraft carrier, the relevant parameters of the shipborne special vehicle are shown in Table 1, the relevant parameters of the algorithm are shown in Table 2, and the aircraft carrier deck map is shown in Figure 5 Consider the initial boundary conditions and terminal target points in Table 3.

[0147] Table 1, Tractor parameters

[0148]

[0149] Table 2. Algorithm parameters

[0150]

[0151] Table 3 Initial boundary conditions and terminal target points of the tractor

[0152]

[0153] A shipborne special vehicle transportation trajectory planning algorithm considering driving habits includes the following steps:

[0154] Step 1: Establish the optimal control problem of the transportation trajectory planning problem;

[0155] Step 1-1: Describe the kinematic constraints of shipborne special vehicles;

[0156] Figure 2 The various parameters of shipborne special vehicles are shown, and the center position of the rear axle is recorded as , It's speed, is the front wheel turning angle, is the attitude angle, is the acceleration, is the steering angular velocity, represents the wheelbase. The kinematic constraints of shipborne special vehicles can be described as the following differential equation as shown in formula (1).

[0157] The state variables of the shipborne special vehicle are recorded as , the control variable is recorded as The differential equation shown in formula (1) can be simplified as . express right Derivative, function In addition, according to the mechanical characteristics of the shipborne special vehicle, the following constraints shown in formula (2) must be met during its driving process.

[0158] Step 1-2: Describe the transportation task;

[0159] The starting and ending configurations of the transport task determine the boundary conditions of the optimal control problem. At the initial moment , the configuration of shipborne special vehicles is recorded as:

[0160] (3)

[0161] in, 、 、 、 、 、 and They respectively represent the horizontal coordinate of the rear axle center point, the vertical coordinate of the rear axle center point, the heading angle, the speed, the steering angle, the acceleration and the steering angular velocity of the shipborne special vehicle at the starting moment.

[0162] At the terminal moment ( is also a variable to be optimized), the configuration of the shipborne special vehicle is recorded as:

[0163] (4)

[0164] (5)

[0165] in, 、 、 、 、 、 and They respectively represent the horizontal coordinate of the rear axle center point, the vertical coordinate of the rear axle center point, the heading angle, the speed, the steering angle, the acceleration and the steering angular velocity of the shipborne special vehicle at the terminal moment.

[0166] Steps 1-3: Describe collision avoidance constraints;

[0167] The space containing all shipborne special vehicle configurations is recorded as configuration space , all configurations where shipborne special vehicles collide with obstacles are recorded as impassable spaces , so the traversable space is recorded as . Using vector-valued functions The state variable Mapping to traversable space On the subspace of , the collision avoidance constraint is expressed as shown in formula (6).

[0168] Step 1-4: Establish the objective function;

[0169] In the process of dispatching shipborne special vehicles, it is necessary to consider various requirements such as time, energy consumption and stability. Energy consumption and stability are reflected by the control variables and state variables. The objective function is represented by the weighted sum of the terminal time and the integral terms of the control variables and state variables:

[0170] (7)

[0171] in, is the integral term weight coefficient, and are the weight matrices of state and control variables, is the objective function of the optimal control problem. is the state variable of the shipborne special vehicle, is the transpose of the state variables of the shipborne special vehicle, is the control variable of the shipborne special vehicle, is the transpose of the control variables of the shipborne special vehicle.

[0172] Steps 1-5: Establish the initial optimal control problem;

[0173] In summary, the initial optimal control problem is established , its specific form is:

[0174] (8)

[0175] Step 2: Generate expansion map;

[0176] Step 2-1: Convert the representation of shipborne special vehicles;

[0177] exist Figure 3 The geometric parameters of shipborne special vehicles include: front suspension length , rear suspension length , wheelbase , body width .like Figure 3 As shown, take the two quarter points of the longitudinal axis of the shipborne special vehicle and record them as and , the calculation formula of the two circle centers is shown in formula (9). In formula (9), the radii of the two characteristic circles are equal, which is recorded as .

[0178] Shrink the characteristic circle to the center and move the obstacle to the Expand, and the expanded map is called the expanded map, recorded as This achieves the transformation of the collision avoidance constraints, that is, making the center of the characteristic circle and the expanded obstacle not coincident. This transformation helps to generate a box-shaped safe passage and further simplify the obstacle avoidance constraints.

[0179] Step 2-2: Convert the expanded map to a raster map;

[0180] Expand the map Convert to a raster map and record the raster map as . Set the resolution to , take any point on the aircraft carrier deck as the origin of the two-dimensional rectangular coordinate system, and determine the map boundaries along the x-axis and y-axis according to the length and width of the aircraft carrier deck, which are recorded as and ,in and Respectively represent the minimum and maximum coordinates in the x-axis direction, and Represents the minimum and maximum coordinates of the y-axis direction respectively. Define the grid Represents a two-dimensional rectangular area ,in is the index of the grid in the x-axis direction, is the index of the grid in the y-axis direction; Indicates the coordinates contained in the raster.

[0181] For each grid, if it is consistent with the expansion map If there are overlapping parts of the expanded obstacles in the grid, it is considered as an obstacle grid; otherwise, it is considered as a passable grid.

[0182] Step 3: Use the improved RRT* algorithm to obtain the coarse path;

[0183] Step 3-1: Initialization;

[0184] For the bidirectional search strategy, two trees are maintained, which are recorded as and Initialize the two trees and Initialize as starting point ,Will Initialization endpoint , the node information includes position information and orientation angle information. For target offset sampling within the ellipse, the range of the ellipse and target offset must be determined. and The Euclidean distance between , the major axis of the ellipse is defined as , the minor axis is defined as The two foci of the ellipse are and , the range within the ellipse is . Take a radius of ,for The target bias range is With The range is within the circle with radius ;for The target bias range is With The range is within the circle with radius .

[0185] Step 3-2: Compare the sizes of the two trees;

[0186] statistics and The number of nodes on and .Compare and The size of If it is smaller, go to step 3-4; otherwise, go to step 3-3.

[0187] Step 3-3: Expand the tree ;

[0188] Step 3-3-1: target bias sampling within the ellipse;

[0189] Set a fixed probability , pick a random number ,if , then in Randomly select a two-dimensional point in Randomly select a two-dimensional point from . The above two-dimensional point is recorded as , which can guide the tree to expand in the target direction to a certain extent, while reducing invalid sampling on the basis of ensuring a certain degree of randomness;

[0190] Step 3-3-2: Find the tree Random point on the distance The point with the smallest Euclidean distance is recorded as the closest point ;by Activate the tree for the root node growth;

[0191] Step 3-3-3: Generate child nodes;

[0192] Step 3-3-3-1: Integrate to generate a set of child nodes to be selected;

[0193] When generating child nodes, the path between the root node and the child nodes should be smooth to meet the kinematic constraints of the shipborne special vehicle and to reduce the burden of back-end optimization. According to the mechanical constraints of the shipborne special vehicle in formula (2), Get a random front wheel angle from Get a random speed, repeat times, get A combination of front wheel angle and speed .

[0194] For each set of front wheel angle and speed, The horizontal axis , vertical axis and orientation angle is the initial state, with a time step is the integration step, and according to formula (10), we get The child nodes to be selected are denoted as , the set of paths corresponding to each child node to be selected is recorded as .

[0195] Step 3-3-3-2: Perform collision avoidance detection on the selected sub-nodes;

[0196] Pair Collection All paths in the set are collided and the paths that collide are deleted from the set of paths. The resulting set of collision-free paths is recorded as ; The corresponding node from The resulting collision-free node set is recorded as .

[0197] Step 3-3-3-3: Calculate the comprehensive cost and determine the child nodes;

[0198] right All nodes in the calculation of their comprehensive cost, the comprehensive cost includes: (1) the cost of change If the transport entity is moving forward, the speed direction is recorded as 1, otherwise it is -1. One thing to note is that since the possibility of moving forward and backward is the same when expanding from the first node of each tree, the speed direction of the starting node of each tree is recorded as 0. For other nodes, if the speed direction between the selected child node and its parent node is the same, then the reversing cost ,otherwise ; (2) Obstacle repulsion cost , its calculation formula is shown in equation (11), where is the distance from the selected child node to the obstacle, is a threshold that limits the range affected by obstacles; (3) the gravitational cost of the target point ,in is the distance from the selected child node to the target point; (4) the distance cost between the random sampling point , that is, the child node to be selected to the random sampling point The distance between them.

[0199] Combining the above four parts of the cost, the weighted sum is obtained to obtain the comprehensive cost , its formula is shown in (12), where , , , , which represent the weight coefficients of the reversing cost, the obstacle repulsion cost, the target point's gravitational cost, and the distance cost between the random sampling point. The child node with the lowest comprehensive cost is selected and recorded as .

[0200] (12)

[0201] Step 3-3-4: rewiring;

[0202] Set the rewiring threshold length to ;by As the center of the circle, As the radius, determine a circle, recorded as ; the tree All on the circle The set of nodes in .for All nodes in the The Reeds-Shepp curve is used to determine whether it will collide with an obstacle. All points that collide with obstacles are removed, and then the one with the smallest length is found in the remaining Reeds-Shepp curves. The nodes in The parent node of .

[0203] Step 3-4: Expand the tree ;

[0204] Step 3-4-1: target bias sampling within the ellipse;

[0205] Set a fixed probability , pick a random number ,if , then in Randomly select a two-dimensional point in Randomly select a two-dimensional point from . The two-dimensional point obtained above is recorded as , which can guide the tree to expand in the target direction to a certain extent, while reducing invalid sampling on the basis of ensuring a certain degree of randomness;

[0206] Step 3-4-2: Find the tree Random point on the distance The point with the smallest Euclidean distance is recorded as the closest point ;by Activate the tree for the root node growth;

[0207] Step 3-4-3: Generate child nodes;

[0208] Step 3-4-3-1: Integrate to generate a set of child nodes to be selected;

[0209] When generating child nodes, the path between the root node and the child nodes should be smooth to meet the kinematic constraints of the shipborne special vehicle and to reduce the burden of back-end optimization. According to the mechanical constraints of the shipborne special vehicle in formula (2), Randomly select a front wheel angle from Randomly select a speed and repeat times, get A combination of front wheel angle and speed For each set of front wheel angle and speed, The horizontal axis , vertical axis and orientation angle is the initial state, with a time step is the integration step, and according to formula (10), we get The child nodes to be selected are denoted as , the set of paths corresponding to each child node to be selected is recorded as .

[0210] Step 3-4-3-2: Perform collision avoidance detection on the selected sub-nodes;

[0211] Pair Collection All paths in the set are collided and the paths that collide are deleted from the set of paths. The resulting set of collision-free paths is recorded as ; The corresponding node from The resulting collision-free node set is recorded as

[0212] Step 3-4-3-3: Calculate the comprehensive cost and determine the child nodes;

[0213] right All nodes in the calculation of their comprehensive cost, the comprehensive cost includes: (1) the cost of change If the transport entity is moving forward, the speed direction is recorded as 1, otherwise it is -1. One thing to note is that since the possibility of moving forward and backward is the same when expanding from the first node of each tree, the speed direction of the starting node of each tree is recorded as 0. For other nodes, if the speed direction between the selected child node and its parent node is the same, then the reversing cost ,otherwise ; (2) Obstacle repulsion cost , its calculation formula is shown in equation (11), where is the distance from the selected child node to the obstacle, is a threshold that limits the range affected by obstacles; (3) the gravitational cost of the target point ,in is the distance from the selected child node to the target point; (4) the distance cost between the random sampling point , that is, the child node to be selected to the random sampling point The distance between them.

[0214] Combining the above four parts of the cost, the weighted sum is obtained to obtain the comprehensive cost , its formula is shown in (12), where , , , , which represent the weight coefficients of the reversing cost, the obstacle repulsion cost, the target point's gravitational cost, and the distance cost between the random sampling point. The child node with the lowest comprehensive cost is selected and recorded as .

[0215] Step 3-4-4: rewiring;

[0216] Set the rewiring threshold length to ;by As the center of the circle, As the radius, determine a circle, recorded as ; the tree All on the circle The set of nodes in .for All nodes in the The Reeds-Shepp curve is used to determine whether it will collide with an obstacle. All points that collide with obstacles are removed, and then the one with the smallest length is found in the remaining Reeds-Shepp curves. The nodes in The parent node of .

[0217] Step 3-4: Try to connect the two trees;

[0218] Find the two closest points on the two trees and record them as , , generating a Reeds-Shepp curve between the two , perform collision detection. If the Reeds-Shepp curve does not collide with the obstacle, stop searching and go to step 3-5; otherwise, go to step 3-2.

[0219] Step 3-5: Backtrack to the parent node and return a path;

[0220] from Start backtracking to the parent node until , recorded as ;from Start backtracking to the parent node until , and then arrange the points on the path in reverse order, recorded as ; Return a rough path

[0221] In summary, a RRT* algorithm in a bidirectional ellipse with kinematic constraints combined with an artificial potential field is used to consider the kinematic constraints of the shipborne special vehicle and the rear axle center of the shipborne special vehicle is Plan a rough path .remember The number of nodes in , among which The nodes are , whose coordinates are defined as .

[0222] Step 4: Resample the coarse path to get the initial guess;

[0223] Step 4-1: Estimate transportation time;

[0224] The length of the coarse path is defined as the sum of the Euclidean distances between all adjacent nodes, denoted as ; Define The length of the segment path is ;but .in, For the The horizontal coordinate of the starting point of the segment path; For the The horizontal coordinate of the end point of the segment path; For the The vertical coordinate of the starting point of the segment path; For the The horizontal coordinate of the end point of the segment path.

[0225] Assume that the transport trajectory of shipborne special vehicles meets the time optimization principle, that is, the variable speed movement always adopts the maximum or minimum acceleration, and the uniform speed movement runs at the maximum or minimum speed; when the shipborne special vehicle moves forward, the maximum speed, the maximum acceleration of acceleration, and the maximum acceleration of deceleration are defined as 、 and ; When the shipborne special vehicle is reversing, the maximum speed, the maximum acceleration of acceleration and the maximum acceleration of deceleration are defined as , and At the same time, define the maximum acceleration distance , maximum deceleration distance The sum of the maximum acceleration distance and the maximum deceleration distance . No. The transport time of the segment path is recorded as , the estimation formula is (13), then the estimation formula of the total adjustment time is expressed as .

[0226] Step 4-2: Resample at discrete points;

[0227] Will The time period is divided into interval, thus we get time nodes, the length of each interval is , No. The time corresponding to the time node is .exist At time t, the state variables of the shipborne special vehicle are recorded as The speed and acceleration are estimated based on the time optimal principle; the steering angle and steering angular velocity of the front wheel of the shipborne special vehicle are estimated based on the kinematic constraints, and the calculation formulas are as follows: Formula (14) and Formula (15).

[0228] End Time Initialized to , at all time nodes, the state variables and control variables are initialized to The resampled path is recorded as .

[0229] Step 5: Generate a safe transportation channel based on the resampled path points and transform the obstacle avoidance constraints;

[0230] Step 5-1: Put in Separate them and combine them with formula (9) to calculate the centers of the two characteristic circles corresponding to each path point, which are recorded as and .

[0231] Step 5-2: Separately and The construction methods of the two are the same. Take as an example to illustrate the method of generating a safe transport corridor. No. points The corresponding safe transportation channel is recorded as .

[0232] First, initialize for point , treat it as a rectangle with a width of 0 and a length of 0. Define the exploration direction set . With a fixed step size , in order to The left, right, bottom and top boundaries of the safe transportation channel obtained by the previous step are defined as ,along The four directions in the direction are expanded in turn. When the above expansion is performed, the safe transport channel data is updated according to the following rules: Only when the following two conditions are met, it is considered a valid expansion and the safe transport channel information is updated accordingly: 1) The safe transport channel must not overlap with the grid map. Any obstacle grid overlaps; 2) The safe transportation channel is in the direction The length of the upper extension does not exceed the extension threshold Otherwise, the expansion is considered to have failed and the exploration direction is set Delete direction This expansion process continues until the set of exploration directions Empty.

[0233] right Each point is expanded to obtain The left, right, bottom and top boundaries are .

[0234] Step 5-3: Perform step 5-2 to get each point The left, right, bottom and top boundaries are The following collision constraint transformation is implemented, as shown in formula (16).

[0235] Step 6: Transform the nonlinear constraints, apply the forward guidance penalty function, establish a lightweight optimal control problem and solve it iteratively;

[0236] Step 6-1: Add the penalty function other than the nonlinear constraint to the objective function;

[0237] The initial optimal control problem in steps 1-5 as shown in formula (8) is The obstacle avoidance constraint in is replaced by the description form in formula (16), forming the optimal control problem , as shown in formula (17):

[0238] (17)

[0239] In order to reduce the dependence of the optimal control problem on the initial guess, and because building a safe transport channel will inevitably waste some feasible space, an iterative solution strategy for the optimal control problem is adopted, and the trajectory obtained is used as the initial guess for the next solution. Based on the initial guess, the safe transport channel is regenerated using the method for generating a safe transport channel mentioned in step 5. There are still some nonlinear constraints in the present invention, so the present invention needs to Further transformation. The nonlinear constraints in are softened by external penalty functions, including the kinematic equality constraints shown in formula (1) and formula (9), and the terminal boundary conditions shown in formula (5), which are merged into the cost function shown in formula (7). Specifically:

[0240] Step 6-1-1: First, transform the kinematic constraints shown in formula (1). Formula (18) can be derived. By moving the right-hand side of the equal sign of the first equation in formula (18) to the left, we can obtain formula (19).

[0241] Then the external penalty function of the kinematic constraint shown in formula (1) is expressed as: .

[0242] Step 6-1-2: Secondly, transform formula (9) and move the left side of the equation to the right side of the equation, which is expressed as a vector value function as shown in formula (20); then the external penalty function of the kinematic constraint shown in formula (9) is expressed as: ; The external penalty function of the boundary constraint shown in formula (5) is expressed as: .

[0243] Step 6-1-3: Combine the above three external penalty functions into: . This results in a cost function with a penalty term :

[0244] (twenty one)

[0245] in, is the objective function defined by formula (7), is the weight coefficient of the external penalty function, which is set to 100 in the first iteration to ensure that the penalty for violating the transformed nonlinear constraints dominates.

[0246] Step 6-1-4: Finally, we get the lightweight optimal control problem , as shown in formula (22):

[0247] (twenty two)

[0248] Step 6-2: Introduce the forward guidance penalty function into the objective function shown in formula (22);

[0249] According to the general driving habits of shipborne special vehicles, it is always hoped that the shipborne special vehicles can be driven forward as much as possible while ensuring the shortest possible time. Therefore, the reversing state of the shipborne special vehicles can be penalized to reduce the proportion of reversing driving as much as possible. in Take it out separately and map it using the sigmoid function: ,in is a constant used to control the smoothness of the function. The sigmoid function Mapped to This is a monotonically decreasing function, and its minimum value is at The maximum value of the function is obtained at Therefore, the penalty function of the forward guidance is as follows: . Then the objective function with the forward guidance penalty function is:

[0250] (twenty three)

[0251] in, is the weight coefficient of the forward guidance penalty function. One thing to note is that the coefficient should be selected so that and If the coefficient is too small, the forward guidance penalty will be ineffective. If the coefficient is too large, the dispatching entity will pay too much attention to the forward driving during the driving process, which will greatly increase the driving time. In summary, it is set to: .in, is the estimated time obtained by resampling, is the number of resampled nodes. Setting the penalty coefficient in this way can make the forward guidance penalty adaptively change according to the transportation time, so that the transportation time and the forward guidance penalty are on an equal footing. The optimal control problem with the forward guidance penalty function is recorded as , the formula is as follows:

[0252] (twenty four)

[0253] Step 6-3: Iteratively solve the optimal control problem with the forward guidance penalty function;

[0254] First iteration solution The initial guess of is obtained by step 4, and the safe transportation channel for the first iteration is obtained by step 5. Set to 100; the trajectory obtained after the first iteration is used as the initial guess for the next iteration, and the corresponding safe channel is obtained by step 5; the termination condition of the iteration is a strict equality constraint that requires the kinematic constraints shown in formulas (1) and (9) and the boundary constraints shown in formula (5) to be satisfied. Therefore, in each iterative solution process, the improvement , until the obtained Less than the allowable error is 10 -6 , and obtain the final trajectory.

[0255] exist Figure 7 The transport trajectory from the initial point to the target point is shown in Figures 8 to 12 The figure shows the changes in five variables: the tractor's speed, steering angle, heading angle, acceleration, and steering angular velocity. This shows that this method can effectively complete the path planning task.

[0256] The above-described embodiments merely express the implementation methods of the present invention, but should not be understood as limiting the scope of the patent of the present invention. It should be pointed out that for those skilled in the art, several variations and improvements can be made without departing from the concept of the present invention, and these all fall within the scope of protection of the present invention.

Claims

1. A method for planning shipborne special vehicle transportation trajectories taking into account driving habits, characterized in that: The method for planning the transport trajectory of shipborne special vehicles comprises the following steps: Step 1: Establish the optimal control problem of the transportation trajectory planning problem; Step 2: Generate an expansion map to transform the collision avoidance requirements; Step 3: Use an improved RRT* algorithm, that is, the RRT* algorithm within a bidirectional ellipse that considers kinematic constraints and combines an artificial potential field, to obtain a rough path; the details are as follows: Step 3-1: Initialization; The bidirectional search strategy maintains two trees, denoted as and ; Initialize the two trees and Initialize as starting point ,Will Initialization endpoint , the node information includes position information and orientation angle information; for the target offset sampling in the ellipse, determine the range of the ellipse and target offset; record and The Euclidean distance between , the major axis of the ellipse is defined as ,in constant; the minor axis is defined as The two foci of the ellipse are and , the range within the ellipse is ; Take a radius of ,for , the target bias range is With The range is within the circle with radius ;for , the target bias range is With The range is within the circle with radius ; Step 3-2: Compare the sizes of the two trees; statistics and The number of nodes on and ;Compare and The size of If it is smaller, go to step 3-4; otherwise, go to step 3-3; Step 3-3: Expand the tree ; Step 3-3-1: target bias sampling within the ellipse; Set a fixed probability , pick a random number ,if , then in Get a random two-dimensional point in Get a random two-dimensional point in ; record the obtained two-dimensional point as ; Step 3-3-2: Find the tree Random point on the distance The point with the smallest Euclidean distance is recorded as the closest point ;by Activate the tree for the root node growth; Step 3-3-3: Generate child nodes; Step 3-3-3-1: Integrate to generate a set of child nodes to be selected; According to the constraints of formula (2), Get a random front wheel angle from Get a random speed, repeat times, get A combination of front wheel angle and speed ; （2）， in, 、 、 、 Respectively represent the minimum acceleration, speed, steering angular velocity and steering angle allowed for shipborne special vehicles; 、 、 、 They represent the maximum acceleration, speed, steering angular velocity and steering angle allowed for shipborne special vehicles respectively; For each set of front wheel angle and speed, The horizontal axis , vertical axis and orientation angle is the initial state, with a time step is the integration step, and according to formula (10), we get The child nodes to be selected are denoted as , the set of paths corresponding to each child node to be selected is recorded as ; （10）， Step 3-3-3-2: Perform collision avoidance detection on the selected sub-nodes; Pair Collection All paths in the set are collided and the paths that collide are deleted from the set of paths. The resulting set of collision-free paths is recorded as ; The corresponding node from The resulting collision-free node set is recorded as ; Step 3-3-3-3: Calculate the comprehensive cost and determine the child nodes; Step 3-3-4: rewiring; Set the rewiring threshold length to ;by As the center of the circle, As the radius, determine a circle, recorded as ; the tree All on the circle The set of nodes in ; for All nodes in the The Reeds-Shepp curve is used to determine whether it collides with an obstacle. The point that collides with the obstacle is removed, and the point with the smallest length is found in the remaining Reeds-Shepp curves. The nodes in The parent node of Step 3-4: Expand the tree ; Step 3-4-1: target bias sampling within the ellipse; Set a fixed probability , pick a random number ,if , then in Get a random two-dimensional point in Get a random two-dimensional point in the ; record the two-dimensional point obtained above as ; Step 3-4-2: Find the tree Random point on the distance The point with the smallest Euclidean distance is recorded as the closest point ;by Activate the tree for the root node growth; Step 3-4-3: Generate child nodes; Step 3-4-3-1: Integrate to generate a set of child nodes to be selected; According to formula (2) Get a random front wheel angle from Get a random speed, repeat times, get A combination of front wheel angle and speed ; For each set of front wheel angle and speed, The horizontal axis , vertical axis and orientation angle is the initial state, with a time step is the integration step, and according to formula (10), we get The child nodes to be selected are denoted as , the set of paths corresponding to each child node to be selected is recorded as ; Step 3-4-3-2: Perform collision avoidance detection on the selected sub-nodes; Pair Collection All paths in the set are collided and the paths that collide are deleted from the set of paths. The resulting set of collision-free paths is recorded as ; The corresponding node from The resulting collision-free node set is recorded as ; Step 3-4-3-3: Calculate the comprehensive cost and determine the child nodes; Step 3-4-4: rewiring; Set the rewiring threshold length to ;by As the center of the circle, As the radius, determine a circle, recorded as ; the tree All on the circle The set of nodes in ; for All nodes in the The Reeds-Shepp curve is used to determine whether it collides with an obstacle. The point that collides with the obstacle is removed, and then the point with the smallest length is found in the remaining Reeds-Shepp curves. The nodes in The parent node of Step 3-4: Try to connect the two trees; Find the two closest points on the two trees and record them as 、 , generating a Reeds-Shepp curve between the two , perform collision detection. If the Reeds-Shepp curve does not collide with the obstacle, stop searching and go to step 3-5; otherwise, go to step 3-2; Step 3-5: Backtrack to the parent node and return a path; from Start backtracking to the parent node until , recorded as ;from Start backtracking to the parent node until , and then arrange the points on the path in reverse order, recorded as ; Return a rough path , and finally the rear axle center of the shipborne special vehicle Plan a rough path ;remember The number of nodes in , among which The nodes are , whose coordinates are defined as , Step 4: Resample the rough path to obtain the initial reference solution; specifically: Step 4-1: Estimate transportation time; The length of the coarse path is defined as the sum of the Euclidean distances between all adjacent nodes, denoted as ; Define The length of the segment path is ;but ;in, For the The horizontal coordinate of the starting point of the segment path; For the The horizontal coordinate of the end point of the segment path; For the The vertical coordinate of the starting point of the segment path; For the The horizontal coordinate of the end point of the segment path; Assume that the transport trajectory of the shipborne special vehicle meets the time optimization principle; when the shipborne special vehicle moves forward, the maximum speed, the maximum acceleration of acceleration and the maximum acceleration of deceleration are defined as 、 and ; When the shipborne special vehicle is reversing, the maximum speed, the maximum acceleration of acceleration and the maximum acceleration of deceleration are defined as , and ; At the same time, define the maximum acceleration distance , maximum deceleration distance The sum of the maximum acceleration distance and the maximum deceleration distance ;No. The transport time of the segment path is recorded as , the estimation formula is (13), then the estimation formula of the total transportation time is expressed as ; （13）， Step 4-2: Resample at discrete points; Will The time period is divided into interval, thus we get time nodes, the length of each interval is , No. The time corresponding to the time node is ;exist At time t, the state variables of the shipborne special vehicle are recorded as ; Speed ​​and acceleration are estimated based on the time-optimal principle; Steering angle of the front wheels of shipborne special vehicles and steering angular velocity ; End Time Initialized to , at all time nodes, the state variables and control variables are initialized to ; The resampled path is recorded as ; Step 5: Generate a safe transportation channel based on the resampled path points and simplify the form of collision avoidance constraints; Step 6: Transform the nonlinear constraints, apply the forward guidance penalty function, establish the final optimal control problem and solve it iteratively.

2. The method for planning shipborne special vehicle transportation trajectory considering driving habits according to claim 1 is characterized in that: The step 1 is specifically as follows: Step 1-1: Describe the kinematic constraints of shipborne special vehicles; Define the various parameters of shipborne special vehicles, and the rear axle center position is recorded as , It's speed, is the front wheel turning angle, is the attitude angle, is the acceleration, is the steering angular velocity, represents the wheelbase; the kinematic constraints of the shipborne special vehicle are described as the following equations: （1）， in, For the end time, For shipborne special vehicles The horizontal coordinate of the rear axle center at time For shipborne special vehicles The vertical coordinate of the rear axle center at time For shipborne special vehicles The speed of time, For shipborne special vehicles The front wheel angle at the moment, For shipborne special vehicles The posture angle of the moment, For shipborne special vehicles The acceleration of time, For shipborne special vehicles Steering angular velocity at the moment; The state variables of the shipborne special vehicle are recorded as , the control variable is recorded as ; The differential equation shown in formula (1) can be abbreviated as ; express right Derivative, function Expresses a differential equation; and the following constraints must be met during the movement of shipborne special vehicles: （2）， in, 、 、 、 Respectively represent the minimum acceleration, speed, steering angular velocity and steering angle allowed for shipborne special vehicles; 、 、 、 They represent the maximum acceleration, speed, steering angular velocity and steering angle allowed for shipborne special vehicles respectively; Step 1-2: Describe the transportation task; At the initial moment , the configuration of shipborne special vehicles is recorded as: （3）， in, 、 、 、 、 、 and Respectively represent the abscissa of the rear axle center point, the ordinate of the rear axle center point, the heading angle, the speed, the steering angle, the acceleration and the steering angular speed of the shipborne special vehicle at the starting time; At the terminal moment , the configuration of shipborne special vehicles is recorded as: （4）， （5）， in, 、 、 、 、 、 and Respectively represent the abscissa of the rear axle center point, the ordinate of the rear axle center point, the heading angle, the speed, the steering angle, the acceleration and the steering angular speed of the shipborne special vehicle at the terminal moment; Steps 1-3: Describe collision avoidance constraints; The space containing all shipborne special vehicle configurations is recorded as configuration space , all configurations where shipborne special vehicles collide with obstacles are recorded as impassable spaces , then the traversable space is recorded as ; The expression of collision avoidance constraint is: （6）， Step 1-4: Establish the objective function; The objective function is represented by the weighted sum of the terminal time, the integral terms of the control variables and the state variables: （7）， in, is the integral term weight coefficient, and are the weight matrices of state and control variables, is the objective function of the optimal control problem, is the state variable of the shipborne special vehicle, is the transpose of the state variables of the shipborne special vehicle, is the control variable of the shipborne special vehicle, is the transposition of the control variables of the shipborne special vehicle; Steps 1-5: Establish the initial optimal control problem , its specific form is: （8）。 3. The method for planning shipborne special vehicle transportation trajectory considering driving habits according to claim 2 is characterized in that: The step 2 is specifically as follows: Step 2-1: Convert the representation of shipborne special vehicles; The geometric parameters of shipborne special vehicles include the front suspension length , rear suspension length , wheelbase , body width ; Take the two quarter points of the longitudinal axis of the shipborne special vehicle and record them as and ; The radii of the two characteristic circles are equal, denoted as ; Among them, time, and Represent the abscissa and ordinate of the front characteristic circle respectively; and They represent the abscissa and ordinate of the posterior characteristic circle respectively; and Respectively represent the horizontal and vertical coordinates of the center point of the rear axle of the tractor; Shrink the characteristic circle to the center and move the obstacle to the The expanded map is called the expanded map, which is recorded as , realizing the transformation of collision avoidance constraints; Step 2-2: Convert the expanded map to a raster map; Inflate the map Converted into a raster map, recorded as ; For each grid, if it is consistent with the expansion map If there are overlapping parts of the expanded obstacles in the grid, it is considered as an obstacle grid; otherwise, it is considered as a passable grid.

4. The method for planning shipborne special vehicle transportation trajectory considering driving habits according to claim 3 is characterized in that: In step 2: In the step 2-1, and The calculation formula for the two circle centers is as follows: （9）， In step 2-2: Set the resolution to , take any point on the aircraft carrier deck as the origin of the two-dimensional rectangular coordinate system, and determine the map boundaries along the x-axis and y-axis according to the length and width of the aircraft carrier deck, which are recorded as and ,in and Respectively represent the minimum and maximum coordinates in the x-axis direction, and Represents the minimum and maximum coordinates in the y-axis direction respectively; defines the grid Represents a two-dimensional rectangular area ,in is the index of the grid in the x-axis direction, is the index of the grid in the y-axis direction; Indicates the coordinates contained in the raster.

5. The method for planning shipborne special vehicle transportation trajectory considering driving habits according to claim 1 is characterized in that: In step 3: The step 3-3-3-3 is: right Calculate the comprehensive cost of all nodes in the, comprehensive cost includes: (1) change cost If the transport entity is moving forward, the speed direction is recorded as 1, otherwise it is -1. The speed direction of the starting node of each tree is recorded as 0. For other nodes, if the speed direction between the selected child node and its parent node is the same, then the reversing cost is ,otherwise ; (2) Obstacle repulsion cost , its calculation formula is shown in equation (11), where is the distance from the selected child node to the obstacle, is the threshold, which is an integer multiple of the wheelbase length of the shipborne special vehicle; (3) the gravitational cost of the target point ,in is the distance from the selected child node to the target point; (4) the distance cost between the random sampling point , that is, the child node to be selected to the random sampling point the distance between them; （11）， Combining the above four parts of the cost, the weighted sum is obtained to obtain the comprehensive cost , its formula is shown in (12), where , , , , which represent the weight coefficients of the reversing cost, the obstacle repulsion cost, the target point’s gravitational cost, and the distance cost between the random sampling point; the child node with the minimum comprehensive cost is selected and recorded as ; （12）， The steps 3-4-3-3 are: right Calculate the comprehensive cost of all nodes in the, comprehensive cost includes: (1) change cost If the transport entity is moving forward, the speed direction is recorded as 1, otherwise it is -1. The speed direction of the starting node of each tree is recorded as 0. For other nodes, if the speed direction between the selected child node and its parent node is the same, then the reversing cost is ,otherwise ; (2) Obstacle repulsion cost , the calculation formula is shown in equation (11); (3) The gravitational cost of the target point ,in is the distance from the selected child node to the target point; (4) the distance cost between the random sampling point , that is, the child node to be selected to the random sampling point the distance between them; Combining the above four parts of the cost, the weighted sum is obtained to obtain the comprehensive cost , its formula is shown in (12), where , , , , which represent the weight coefficients of the reversing cost, the obstacle repulsion cost, the target point’s gravitational cost, and the distance cost between the random sampling point; the child node with the minimum comprehensive cost is selected and recorded as .

6. The method for planning shipborne special vehicle transportation trajectory considering driving habits according to claim 5 is characterized in that: The step 5 is specifically as follows: Step 5-1: Put in Separate them and combine them with formula (9) to calculate the centers of the two characteristic circles corresponding to each path point, which are recorded as and ; Step 5-2: Separately and Construct a safe transportation corridor. The construction methods of the two are the same. To explain; No. points The corresponding safe transportation channel is recorded as , and get each point The left, right, bottom and top boundaries are ; Step 5-3: Perform step 5-2 to get each point The left, right, bottom and top boundaries are ; Implement the following collision constraint conversions as shown below: （16）。 7. The method for planning shipborne special vehicle transportation trajectory considering driving habits according to claim 6 is characterized in that: In step 5: In the step 5-2, The methods for constructing a safe transportation corridor are: First, initialize for point , treat it as a rectangle with a width of 0 and a length of 0; define the exploration direction set ; with a fixed step size , in order to Expand in the direction of ; define the left, right, bottom and top boundaries of the safe transportation channel obtained by the previous step as ,along The four directions in the direction are expanded in turn; when When the above expansion is performed, the safe transport channel data is updated according to the following rules: Only when the following two conditions are met, it is considered a valid expansion and the safe transport channel information is updated accordingly: 1) The safe transport channel must not overlap with the grid map. Any obstacle grid overlaps; 2) The safe transportation channel is in the direction The length of the upper extension does not exceed the extension threshold ; Otherwise, the expansion is considered to have failed and the exploration direction is collected. Delete direction ; Continue this expansion process until the exploration direction set is empty; right Each point is expanded to obtain The left, right, bottom and top boundaries are .

8. The method for planning shipborne special vehicle transportation trajectory considering driving habits according to claim 6 is characterized in that: The step 6 is specifically as follows: Step 6-1: Add the penalty function other than the nonlinear constraint to the objective function; The initial optimal control problem in steps 1-5 as shown in formula (8) is The obstacle avoidance constraint in is replaced by the description form in formula (16), forming the optimal control problem , as shown in formula (17): （17）， Adopting the strategy of iteratively solving the optimal control problem, the solved trajectory is used as the initial guess for the next solution, and the safe transportation channel is regenerated based on the initial guess; right The nonlinear constraints in are softened by external penalty functions, specifically: Step 6-1-1: Transform the kinematic constraints as shown in formula (1); according to the above mentioned step 1-1 Derived: （18）， in, Indicates the time points; express State variables at the moment; express State variables at the moment; Move the right side of the equal sign of the first equation in formula (18) to the left side, and record it as: （19）， Then the external penalty function of the kinematic constraint shown in formula (1) is expressed as: ; Step 6-1-2: Transform formula (9) by moving the left side of the equation to the right side of the equation, and express it as the following vector-valued function: （20）， Then the external penalty function of the kinematic constraint shown in formula (9) is expressed as: ; The external penalty function of the boundary constraint shown in formula (5) is expressed as: ; Step 6-1-3: Combine the above three external penalty functions into: ; Thus we get the cost function with penalty term : （21）， in, is the objective function defined by formula (7), is the weight coefficient of the external penalty function; Step 6-1-4: Finally, we get the lightweight optimal control problem , as shown in formula (22): （22）， Step 6-2: Introduce the forward guidance penalty function into the objective function shown in formula (22); Punish the reversing state of shipborne special vehicles; in Take it out separately and use sigmoid function to Map to ; The penalty function for forward guidance is as follows: , the objective function with the forward guidance penalty function is: （23）， in, is the weight coefficient of the forward guidance penalty function; Taking all the above into consideration, set it to: ;in, is the estimated time obtained by resampling, is the number of resampled nodes; then the optimal control problem with the forward guidance penalty function is recorded as , the formula is as follows: （24）， Step 6-3: Iteratively solve the optimal control problem with the forward guidance penalty function.

Images