Shipborne special vehicle dispatching trajectory planning method considering driving habits

Abstract

The invention discloses a shipborne special vehicle dispatching trajectory planning method considering driving habits, and belongs to the field of equipment intelligent guarantee. The method comprises the following steps: 1, establishing an optimal control problem of an allocation trajectory planning problem; 2, generating an expansion map to convert collision avoidance requirements; thirdly, obtaining a coarse path by using an improved RRT algorithm; fourthly, re-sampling the rough path to obtain an initial reference solution; 5, generating a safe dispatching channel, and simplifying the form of collision avoidance constraint; and 6, converting a nonlinear constraint condition, applying a forward guidance penalty function, establishing a final optimal control problem, and carrying out iterative solution. According to the method, the high-quality initial guess can be quickly generated, the trajectory planning efficiency is improved, the ship-borne special vehicle can be continuously guided to run forwards through iteration, the safety channel is updated, the proportion of forward running in the trajectory is increased, the influence of the initial guess on the final solving result is weakened, and the trajectory planning efficiency is improved. And a high-quality dispatching track conforming to the driving habit is obtained.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent equipment support, and relates to a method for planning the transportation trajectory of shipborne special vehicles considering driving habits, in particular to an RRT* algorithm within a two-way ellipse considering kinematic constraints combined with an artificial potential field and a method for planning the transportation trajectory of shipborne special vehicles with a positive guidance penalty function. Background Art

[0002] Different from land airports, the space on the aircraft carrier deck is small, and there are a large number of carrier-based aircraft and various equipment with complex shapes on it. This results in a narrow area available for shipborne special vehicles to pass on the deck environment. Therefore, it increases the difficulty of planning the transportation trajectory of shipborne special vehicles. Currently, the relatively mature trajectory planning method is usually a multi-stage framework. Given the complex aircraft carrier deck environment and dense obstacles, an efficient initial rough path planning method should be adopted to ensure that a kinematically feasible initial reference path can be quickly planned, thereby reducing the time of the entire planning process. For example, the hybrid A* algorithm used in a Chinese invention patent, a method for planning the taxiing trajectory of carrier-based aircraft on the aircraft carrier deck based on a safe transportation corridor (CN115328165A), is affected by the heuristic function and will tend to search towards the end point, resulting in excessive redundant searches at the edge of obstacles, thus increasing the planning time. At the same time, in the existing research on the algorithm for planning the transportation trajectory of shipborne special vehicles, the situation that excessive reverse driving of the vehicle does not conform to driving habits is usually ignored. For example, in a Chinese invention patent, a method for iterative planning of the tractor trajectory based on an improved search random tree algorithm (CN119066986A), the driving habits of the tractor are not considered, and a positive guidance strategy needs to be proposed to make the shipborne special vehicle maintain forward driving as much as possible.

[0003] Considering the above problems, it is necessary to propose an efficient optimal control problem model and solution framework and a reasonable positive 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] To solve the above-mentioned technical problems, the present invention proposes a method for planning the transportation trajectory of shipborne special vehicles considering driving habits, adopting a three-stage planning strategy. First, use an RRT* algorithm within a two-way ellipse considering kinematic constraints combined with an artificial potential field to search for a rough path that meets the constraints of shipborne special vehicles, ensuring that in a complex obstacle environment, a high-quality trajectory can be found within the passable space. Secondly, resample the rough path to obtain an initial guess for the optimal control problem, and construct a safety corridor with the sampled points to efficiently express the obstacle avoidance constraints and improve the overall solution efficiency. Finally, incorporate the non-linear constraints in the optimal control problem as penalty terms into the objective function to reduce the difficulty of solving the problem, and introduce a positive guiding penalty function in the objective function to enable the obtained trajectory to maintain forward driving as much as possible.

[0005] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0006] A method for planning the transportation trajectory of shipborne special vehicles considering driving habits, the method for planning the transportation trajectory of shipborne special vehicles includes 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 parameters of the shipborne special vehicle, and record the position of the rear axle center as , is the speed, is the front wheel steering angle, is the attitude angle, is the acceleration, is the steering angular velocity, represents the wheelbase. Then, the kinematic constraints of the shipborne special vehicle are described by the following differential equations:

[0010] (1)

[0011] Among them, is the end time, is the abscissa of the rear axle center of the shipborne special vehicle at moment, is the ordinate of the rear axle center of the shipborne special vehicle at moment, is the speed of the shipborne special vehicle at moment, is the front wheel steering angle of the shipborne special vehicle at moment, is the attitude angle of the shipborne special vehicle at moment is the acceleration of the shipborne special vehicle at moment, is the steering angular velocity of the shipborne special vehicle at moment.

[0012] The state variables of the shipborne special vehicle are denoted as , and the control variables are denoted as . The differential equation shown in formula (1) can be abbreviated as . represents deriving with respect to, and the function represents the differential equation. In addition, according to the mechanical characteristics of the shipborne special vehicle, the following constraint conditions need to be satisfied during its driving process:

[0013] (2)

[0014] Among them, , , , respectively represent the minimum acceleration, speed, steering angular velocity, and steering angle allowed for the shipborne special vehicle; , , , respectively represent the maximum acceleration, speed, steering angular velocity, and steering angle allowed for the shipborne special vehicle.

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

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

[0017] (3)

[0018] Among them, , , , , , and respectively represent the abscissa of the center point of the rear axle, the ordinate of the center point of the rear axle, the orientation angle, the speed, the steering angle, the acceleration, and the steering angular velocity of the shipborne special vehicle at the start time.

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

[0020] (4)

[0021] (5)

[0022] Among them, , , , , , and respectively represent the abscissa of the center point of the rear axle, the ordinate of the center point of the rear axle, the orientation angle, the speed, the steering angle, the acceleration, and the steering angular velocity of the shipborne special vehicle at the terminal time.

[0023] Step 1-3: Describe the collision avoidance constraint;

[0024] Denote the space containing all configurations of the shipborne special vehicle as the configuration space , and denote all configurations where the shipborne special vehicle collides with obstacles as the non-passable space . Therefore, the passable space is denoted as . Using the vector-valued function to map the state variable to a subspace of the passable space , the expression form of the collision avoidance constraint is:

[0025] (6)

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

[0027] During the transportation process of the shipborne special vehicle, various requirements such as duration, energy consumption, and smoothness need to be considered. The duration is reflected by the terminal time , and the energy consumption and smoothness are reflected by the control variables and state variables. The objective function is represented by the weighted sum of the integral terms of the terminal time and the control variables and state variables:

[0028] (7)

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

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

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

[0032] (8)

[0033] Step 2: Generate the inflated map;

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

[0035] The geometric parameters of shipborne special vehicles include: front suspension length , rear suspension length , wheelbase , body width . Take two quarter points on the longitudinal axis of the shipborne special vehicle, denoted as and respectively. The calculation formulas for the two centers are as follows:

[0036] (9)

[0037] The radii of the two characteristic circles are equal, denoted as ; among them, at moment, and represent the abscissa and ordinate of the front characteristic circle respectively; and represent the abscissa and ordinate of the rear characteristic circle respectively; and represent the abscissa and ordinate of the center point of the rear axle of the tractor respectively.

[0038] Shrink the characteristic circle to the center, and at the same time inflate the obstacle with a radius of . The inflated map is called the inflated map, denoted as . In this way, the transformation of the collision avoidance constraint is realized, that is, the center of the characteristic circle does not coincide with the inflated obstacle. This transformation helps to generate a box-shaped safety passage subsequently, and thus simplifies the obstacle avoidance constraint.

[0039] Step 2-2: Convert the inflated map into a grid map;

[0040] Convert the inflated map into a grid map, and denote the grid map as . Set the resolution to . Arbitrarily take a point on the aircraft carrier deck as the origin of the two-dimensional rectangular coordinate system, and determine the map boundaries of the x-axis and y-axis upward according to the length and width of the aircraft carrier deck, denoted as and , where and respectively represent the minimum and maximum coordinates in the x-axis direction, and respectively represent the minimum and maximum coordinates in the y-axis direction. Define the grid to represent the two-dimensional rectangular area , where is the index of the grid in the x-axis direction, is the index of the grid in the y-axis direction; represents the coordinates contained in the grid.

[0041] For each grid, if it overlaps with the inflated obstacles in the inflated map , then it is regarded as an obstacle grid; otherwise, it is regarded as a passable grid.

[0042] Step 3: Obtain the rough path using the improved RRT* algorithm;

[0043] Step 3-1: Initialize;

[0044] For the bidirectional search strategy, two trees need to be maintained, denoted as and . Initialize the two trees, initialize as the starting point , and initialize as the ending point . The node information includes position information and orientation angle information. For the target-biased sampling within the ellipse, the range of the ellipse and the target bias needs to be determined. Denote the Euclidean distance between and as . The major axis of the ellipse is defined as , where is a constant; the minor axis is defined as . The two foci of the ellipse are and respectively. Denote the range within the ellipse as .

[0045] Take a circle with a radius of . For , its target-biased range is within the circle with as the center and as the radius. Denote this range as ; for , its target-biased range is within the circle with as the center and as the radius. Denote this range as .

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

[0047] Count and the number of nodes on, and denote them as and . Compare and . When 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 offset sampling within the ellipse;

[0050] Set a fixed probability , randomly pick a random number . If , then randomly obtain a two-dimensional point in ; otherwise, randomly obtain a two-dimensional point in . Denote the obtained two-dimensional point as . This can guide the expansion of the tree towards the target direction to a certain extent, while reducing ineffective sampling on the basis of ensuring a certain degree of randomness;

[0051] Step 3-3-2: Find the point on the tree with the minimum Euclidean distance from the random point , and denote it as the nearest point ; Activate the growth of the tree with as the root node;

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

[0053] Step 3-3-3-1: Integrate to generate a group of candidate child nodes;

[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, so as to reduce the burden of backend optimization. According to the mechanical constraints of the shipborne special vehicle in formula (2), randomly obtain a front wheel steering angle from , randomly obtain a speed from , repeat times to obtain combinations of front wheel steering angles and speeds .

[0055] For each group of front wheel steering angles and speeds, with the abscissa of , the ordinate and the orientation angle is the initial state, with a time step is the integration step, and integrate according to formula (10) to obtain The candidate child nodes are denoted as , and the set composed of the paths corresponding to each candidate child node is denoted as .

[0056] (10)

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

[0058] Perform collision detection on all paths in the set , delete the collided paths from the path set, and the obtained collision-free path set is denoted as ; The corresponding nodes are deleted from , and the obtained collision-free node set is denoted as

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

[0060] Calculate the comprehensive cost for all nodes in . The comprehensive cost includes: (1) The cost of changing direction . If the transportation entity is moving forward, the speed direction is denoted as 1, otherwise it is -1. It should be noted that since the possibilities of moving forward and backward are the same when expanding from the first node of each tree, the speed direction of the starting node of each tree is denoted as 0. For other nodes, if the speed direction between the candidate child node and its parent node is the same, then the reverse cost , otherwise ; (2) The repulsive cost of obstacles , and its calculation formula is shown in equation (11), where is the distance from the candidate child node to the obstacle, is a threshold that limits the range affected by the obstacle, and the threshold usually takes an integer multiple of the wheelbase length of the shipborne special vehicle; (3) The gravitational cost of the target point , where is the distance from the candidate child node to the target point; (4) The distance cost between the candidate child node and the randomly sampled point , that is, the distance between the candidate child node and the randomly sampled point .

[0061] (11)

[0062] Combining the above four parts of the cost, perform weighted summation to obtain the comprehensive cost , and its formula is as shown in formula (12), where , , , , respectively representing the weight coefficients of the reverse cost, the obstacle repulsion cost, the gravitational cost of the target point, and the distance cost from the randomly sampled point. Select the child node with the lowest comprehensive cost and denote it as .

[0063] (12)

[0064] Step 3-3-4: Re-wire for Re-wire;

[0065] Set the threshold length for re-wiring as ; with as the center and as the radius, determine a circle, denoted as ; Denote the set of all nodes on the tree that are inside the circle as .

[0066] Generate a Reeds-Shepp curve from all nodes in to and determine whether it will collide with obstacles. Remove all the points that collide with obstacles, and then find the curve with the minimum length among the remaining Reeds-Shepp curves. Use the nodes in the corresponding as the parent nodes of .

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

[0068] Step 3-4-1: Target-biased sampling inside the ellipse;

[0069] Set a fixed probability , randomly pick a random number . If , then randomly obtain a two-dimensional point in ; otherwise, randomly obtain a two-dimensional point in . Denote the obtained two-dimensional point as . This can guide the expansion of the tree towards the target direction to a certain extent, and at the same time, reduce invalid sampling while ensuring a certain degree of randomness;

[0070] Step 3-4-2: Find the point on the tree with the minimum Euclidean distance from the random point , denoted as the nearest point ; With Activate the growth of the root node tree ;

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

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

[0073] When generating child nodes, the path between the root node and the child nodes should be smooth to meet the kinematic constraints of shipborne special vehicles, which is used to reduce the burden of backend optimization. According to the mechanical constraints of shipborne special vehicles in formula (2), randomly obtain a front wheel steering angle from and a speed from . Repeat times to obtain combinations of front wheel steering angles and speeds . For each set of front wheel steering angles and speeds, with the abscissa , ordinate and orientation angle as the initial state, and a time step as the integration step size, integrate according to formula (10) to obtain candidate child nodes denoted as . The set composed of the paths corresponding to each candidate child node is denoted as .

[0074] Step 3-4-3-2: Perform collision avoidance detection on candidate child nodes

[0075] Perform collision detection on all paths in the set . Delete the paths that collide from the path set. The collision-free path set obtained is denoted as ; The corresponding nodes are deleted from . The collision-free node set obtained is denoted as

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

[0077] Calculate the comprehensive cost of all nodes in . The comprehensive cost includes: (1) Direction change cost . If the transfer entity is moving forward, the speed direction is recorded as 1, otherwise -1. It should be noted 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 candidate child node and its parent node is the same, then the reverse cost , otherwise ​; (2) Obstacle repulsion cost , and its calculation formula is shown in Equation (11); (3) Gravitational cost of the target point , where is the distance from the candidate child node to the target point; (4) Distance cost between the candidate child node and the randomly sampled point , that is, the distance from the candidate child node to the randomly sampled point .

[0078] Combining the above four parts of the cost, a weighted sum is performed on them to obtain the comprehensive cost , and its formula is shown in Equation (12), where , , , respectively represent the weight coefficients of the reverse cost, the obstacle repulsion cost, the gravitational cost of the target point, and the distance cost between the candidate child node and the randomly sampled point. Select the child node with the lowest comprehensive cost and denote it as .

[0079] Step 3-4-4: Re-wire;

[0080] Set the threshold length of re-wiring as ; With as the center and as the radius, determine a circle, denoted as ; Denote the set of all nodes on the tree that are inside the circle as . Generate a Reeds-Shepp curve from all nodes in to and determine whether it will collide with obstacles. Remove all the points that collide with obstacles, and then find the one with the minimum length among the remaining Reeds-Shepp curves. Use the nodes in the corresponding as the parent nodes of .

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

[0082] Find the two closest points on the two trees, denoted as , respectively, and generate a Reeds-Shepp curve between them. Perform collision detection. If the Reeds-Shepp curve does not collide with obstacles, stop the search and go to Step 3-5; otherwise, go to Step 3-2.

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

[0084] Backtrack from the parent node until , denoted as ; Backtrack from the parent node until , then reverse the order of the points on this path, denoted as ; Return a rough path

[0085] In summary, use a bidirectional RRT* algorithm within an ellipse that combines artificial potential fields and considers kinematic constraints. Considering the kinematic constraints of shipborne special vehicles, plan a rough path for the rear axle center of the shipborne special vehicle . Denote the number of nodes in as , where the th node is , and its coordinates are defined as

[0086] Step 4: Resample the rough path to obtain an initial guess;

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

[0088] Define the length of the rough path as the sum of the Euclidean distances between all adjacent nodes, denoted as ; Define the length of the th segment of the path as ; Then . Among them, is the abscissa of the starting point of the th segment of the path; is the abscissa of the ending point of the th segment of the path; is the ordinate of the starting point of the th segment of the path; is the abscissa of the ending point of the th segment of the path.

[0089] Assume that the transportation trajectory of the shipborne special vehicle satisfies the time-optimal principle, that is, the variable-speed motion always uses the maximum or minimum acceleration, and the uniform motion runs at the maximum or minimum speed; when the shipborne special vehicle moves forward, define the maximum speed, the maximum acceleration during acceleration, and the maximum acceleration during deceleration as , and respectively; when the shipborne special vehicle moves backward, define the maximum speed, the maximum acceleration during acceleration, and the maximum acceleration during deceleration as , and respectively. At the same time, define the maximum acceleration distance , the maximum deceleration distance and the sum of the maximum acceleration distance and the maximum deceleration distance . The transportation time of the -th path segment is denoted as , and the estimation formula is (13), then the estimation formula for the total transportation time is expressed as .

[0090] (13)

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

[0092] Divide the time period into intervals, and thus obtain time nodes. Then the length of each interval is , and the time corresponding to the -th time node is . At the moment, the state variables of the shipborne special vehicle are denoted as . The speed and acceleration are estimated according to the time-optimal principle; the steering angle and the steering angular velocity of the front wheels of the shipborne special vehicle are estimated according to the kinematic constraints, and the calculation formulas are as follows:

[0093] (14)

[0094] (15)

[0095] where , , and represent the steering angle, the heading angle, the speed and the angular velocity of the front wheels of the shipborne special vehicle at the moment; and represent the steering angle and the heading angle of the front wheels of the shipborne special vehicle at the moment.

[0096] The end time is initialized to , and at all time nodes, the state variables and the control variables are initialized to . Denote the resampled path 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: Take the in Separated and combined with formula (9), the centers of the two feature circles corresponding to each path point are calculated respectively, denoted as and .

[0099] Step 5-2: and Construct safety transportation corridors, and the construction methods of the two are the same. Taking as an example, the generation method of the safety transportation corridor is described. Denote the safety transportation channel corresponding to the th point as . .

[0100] First, initialize as the point , and regard it as a rectangle with a width of 0 and a length of 0. Define the exploration direction set . Expand in the directions in

[0101] .

[0102] .

[0101] For points, expand them all to obtain the left, right, lower, and upper boundaries of the corresponding to each point, which are respectively .

[0102] Step 5-3: Perform the processing in Step 5-2 on to obtain the left, right, lower, and upper boundaries of the corresponding to each point, which are respectively . Implement the following collision constraint transformation as follows:

[0103] (16)

[0104] Step 6: Transform the non - linear constraints, apply the positive - guiding penalty function, establish a lightweight optimal control problem and solve it iteratively;

[0105] Step 6 - 1: Add the non - linear constraints in the form of external penalty function to the objective function;

[0106] Replace the obstacle - avoidance constraint in the initial optimal control problem as shown in formula (8) in Steps 1 - 5 with the description form in formula (16) to form an optimal control problem , as shown in formula (17):

[0107] (17)

[0108] To weaken the dependence of the optimal control problem on the initial guess, and at the same time, since constructing a safe transportation channel will inevitably waste some feasible space, therefore, adopt the strategy of iteratively solving the optimal control problem. Take the obtained trajectory as the initial guess for the next solution, and regenerate the safe transportation channel based on the method of generating a safe transportation channel mentioned in Step 5 based on this initial guess. Since there are still some non - linear constraints in it, so the present invention needs to further transform . Soften the non - linear constraints in through an external penalty function, including the kinematic equality constraints as shown in formula (1) and formula (9), and the end - point boundary conditions as shown in formula (5), and merge them into the cost function as shown in formula (7). Specifically:

[0109] Step 6 - 1 - 1: First, transform the kinematic constraint as shown in formula (1). According to what is mentioned in Step 1 - 1 it can be deduced that:

[0110] (18)

[0111] where, represents the th time point; represents the state variable at time represents the state variable at time

[0112] Move the right - hand side term of the equal sign in the first equation in formula (18) to the left side and denote it as:

[0113] (19)

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

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

[0116] (20)

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

[0118] Step 6-1-3: Combine the above three external penalty functions into: . Thus, the cost function with penalty terms is obtained :

[0119] (21)

[0120] where is the objective function defined by Equation (7), is the weight coefficient of the external penalty function, and a relatively large number should be set, usually an integer multiple of 100, to ensure that the penalty for violating the transformed non-linear constraint conditions dominates.

[0121] Step 6-1-4: Finally, the lightweight optimal control problem is obtained, as shown in Equation (22):

[0122] (22)

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

[0124] According to the general driving habits of shipborne special vehicles, it is always desired to make the shipborne special vehicles drive forward as much as possible while ensuring the shortest possible time consumption. Therefore, the state of the shipborne special vehicles in reverse can be penalized so that the shipborne special vehicles can reduce the proportion of reverse driving as much as possible. Take in the state variable and make a mapping of it using the sigmoid function: , where is a constant used to control the smoothness of the function. This sigmoid function maps to . This is a monotonically decreasing function, and the minimum value of the function is obtained at , and the maximum value of the function is obtained at . Therefore, the positive guidance penalty function is as follows: Then the objective function with the positive guidance penalty function is as follows:

[0125] (23)

[0126] where is the weight coefficient of the positive guidance penalty function. It should be noted that the selection of this coefficient should make and maintain the same order of magnitude. If this coefficient is too small, the positive guidance penalty will fail. If this coefficient is too large, the transportation entity will overly focus on forward driving during the driving process, resulting in a significant increase in the driving time. Considering the above, it is set as: where is the estimated time obtained by resampling, is the number of nodes of resampling. Setting the penalty coefficient in this way can make the positive guidance penalty adaptively change according to the transportation time, so that the transportation time and the positive guidance penalty are in an equal position. Then the optimal control problem with the positive guidance penalty function is denoted as , and the formula is as follows:

[0127] (24)

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

[0129] The initial guess for the first iterative solution is obtained from Step 4, and the safe transportation channel for the first iteration is obtained from Step 5; the trajectory obtained after the first iteration is used as the initial guess for the next iteration, and the corresponding safe channel is obtained from Step 5; the termination condition of the iteration is that the kinematic constraints shown in formulas (1) and (9) and the boundary constraints shown in formula (5) are all satisfied strict equality constraints. Therefore, in each iterative solution process, improve , until the obtained during the iterative process is less than the allowable error is 10 -6 ~10 -4 .

[0130] The innovation points of the present invention are as follows: An improved two-way RTT method considering kinematics is proposed, which includes: adopting target biasing sampling within an ellipse during the random sampling process, restricting the sampling range within an ellipse limited by the starting point and the ending point, and guiding the two trees to expand towards each other with a certain probability, thereby reducing invalid sampling and accelerating convergence; adopting an integral driving strategy when generating new nodes, enabling the searched path to satisfy the kinematic constraints of the shipborne special vehicle, thereby obtaining a smooth path and reducing the burden on the backend optimization. At the same time, the idea of artificial potential field is introduced, enabling the reduction of invalid sampling at the obstacle edges during the process of expanding the tree. A forward guiding penalty function is also proposed, which can make the proportion of reverse driving in the initial guess relatively high in the entire transportation process. The final trajectory obtained by iteratively solving the optimal control problem with the forward guiding penalty function can, on the basis of ensuring the principle of time optimality, greatly reduce the proportion of reverse driving in the entire transportation process.

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

[0132] The present invention realizes the conversion of obstacle avoidance constraints through a safe transportation channel, greatly reducing the difficulty of problem solving to a large extent and ensuring the efficient generation of the transportation trajectory of the shipborne special vehicle. The two-way RRT* algorithm considering kinematic constraints in an ellipse combined with the artificial potential field can efficiently generate a rough trajectory in a large-scale and obstacle-densely distributed deck environment. The resampling process of this rough path provides a high-quality initial guess for the optimal control problem and generates a safe channel in combination with the resampling results. By representing the body of the shipborne special vehicle with two characteristic circles, the geometric shape of the tractor can be accurately described, and the feasible space can be fully utilized. Through a forward guiding penalty function and combined with an iterative strategy, continuously guiding the shipborne special vehicle to drive forward and updating the safe channel, weakening the influence of the initial guess on the optimality of the final solution result and the proportion of reverse driving time in the total transportation time. Even when the quality of the initial guess is not high and the proportion of reverse driving time in the total transportation time is very high, a transportation trajectory with good quality and conforming to driving habits can still be obtained. Brief Description of the Drawings

[0133] Figure 1 is the flowchart of the present invention.

[0134] Figure 2 is the small car model diagram.

[0135] Figure 3 is the representation diagram with two characteristic circles.

[0136] Figure 4 is the schematic diagram of the obstacle avoidance method.

[0137] Figure 5 is the schematic diagram of the obstacle avoidance method.

[0138] Figure 6 It is a map of a certain aircraft carrier deck.

[0139] Figure 7 It is a path planning diagram.

[0140] Figure 8 It is a diagram of the speed change during the planning process.

[0141] Figure 9 It is a diagram of the steering angle change during the planning process.

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

[0143] Figure 11 It is a diagram of the acceleration change during the planning process.

[0144] Figure 12 It is a diagram of the steering angular velocity change during the planning process. Detailed implementation method

[0145] The present invention will be further described below in conjunction with specific embodiments.

[0146] Considering the problem of the transfer planning of a certain type of shipborne special vehicle on an aircraft carrier deck, 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 as Figure 5 shown. Considering 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 transfer trajectory planning algorithm for shipborne special vehicles considering driving habits specifically includes the following steps:

[0154] Step 1: Establish an optimal control problem for the transfer trajectory planning problem;

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

[0156] Figure 2 Shows the various parameters of the shipborne special vehicle, and the position of the rear axle center is denoted as , is the speed, is the front wheel steering 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 by the following differential equations as shown in Equation (1).

[0157] Denote the state variables of shipborne special vehicles as , and the control variables as . The differential equations shown in Equation (1) can be abbreviated as . represents taking the derivative with respect to , and the function represents the differential equation. In addition, according to the mechanical characteristics of shipborne special vehicles, the following constraint conditions shown in Equation (2) need to be satisfied during their driving process.

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

[0159] The starting and ending configurations of the transportation task determine the boundary conditions of the optimal control problem. At the initial time , the configuration of the shipborne special vehicle is denoted as:

[0160] (3)

[0161] where , , , , , and represent the abscissa of the center point of the rear axle, the ordinate of the center point of the rear axle, the orientation angle, the speed, the steering angle, the acceleration, and the steering angular velocity of the shipborne special vehicle at the start time, respectively.

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

[0163] (4)

[0164] (5)

[0165] where , , , , , and respectively represent the abscissa of the center point of the rear axle of the shipborne special vehicle at the terminal moment, the ordinate of the center point of the rear axle, the orientation angle, the speed, the steering angle, the acceleration, and the steering angular velocity.

[0166] Step 1-3: Describe the collision avoidance constraint;

[0167] Denote the space containing all configurations of the shipborne special vehicle as the configuration space , and denote all configurations where the shipborne special vehicle collides with obstacles as the non-passable space . Therefore, the passable space is denoted as . Use the vector-valued function to map the state variables to a subspace of the passable space , then the expression form of the collision avoidance constraint is as shown in formula (6).

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

[0169] During the transportation process of the shipborne special vehicle, various requirements such as duration, energy consumption, and smoothness need to be considered. The duration is reflected by the terminal time , and the energy consumption and smoothness are reflected by the control variables and state variables. The objective function is represented by the weighted sum of the integral terms of the terminal time and the control variables and state variables:

[0170] (7)

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

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

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

[0174] (8)

[0175] Step 2: Generate the inflated map;

[0176] Step 2-1: Transform the representation form of the shipborne special vehicle;

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

[0178] Shrink the characteristic circle to its center, and at the same time expand the obstacle with a radius . Denote the expanded map as . In this way, the conversion of the collision avoidance constraint is realized, that is, the center of the characteristic circle does not coincide with the expanded obstacle. This conversion helps to generate a box-shaped safety channel subsequently, thereby simplifying the obstacle avoidance constraint.

[0179] Step 2-2: Convert the expanded map into a grid map;

[0180] Convert the expanded map into a grid map, and denote the grid map as . Set the resolution to . Arbitrarily take a point on the aircraft carrier deck as the origin of the two-dimensional rectangular coordinate system, and determine the map boundaries in the x-axis and y-axis directions according to the length and width of the aircraft carrier deck, denoted as and respectively, where and represent the minimum and maximum coordinates in the x-axis direction respectively, and represent the minimum and maximum coordinates in the y-axis direction respectively. Define the grid to represent the two-dimensional rectangular area , where is the index of the grid in the x-axis direction, is the index of the grid in the y-axis direction; represents the coordinates included in the grid.

[0181] For each grid, if it overlaps with the expanded obstacle in the expanded map , then it is regarded as an obstacle grid; otherwise, it is regarded as a passable grid.

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

[0183] Step 3-1: Initialize;

[0184] For the bidirectional search strategy, two trees need to be maintained, denoted as and . Initialize the two trees, initialize as the starting point , and initialize as the end point . The node information includes position information and orientation angle information. For the target bias sampling within the ellipse, the ranges of the ellipse and the target bias need to be determined. Denote the Euclidean distance between and as . The major axis of the ellipse is defined as , the minor axis is defined as . The two foci of the ellipse are and respectively. Denote the range within the ellipse as . Take a circle with a radius of . For , the target bias range is within the circle centered at with a radius of . Denote this range as ; for , the target bias range is within the circle centered at with a radius of . Denote this range as .

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

[0186] Count the number of nodes on and , and denote them as and respectively. Compare the sizes of and . When 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 . Randomly take a random number . If , then randomly take a two-dimensional point in ; otherwise, randomly take a two-dimensional point in . Denote the obtained two-dimensional point as , which can guide the tree to expand in the target direction to a certain extent, and at the same time, reduce invalid sampling while ensuring a certain degree of randomness;

[0190] Step 3-3-2: Find the point on the tree with the smallest Euclidean distance from the random point , and denote it as the nearest point ; Activate the growth of the tree with as the root node;

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

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

[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, which is used to reduce the burden of backend optimization. According to the mechanical constraints of the shipborne special vehicle in formula (2), randomly obtain a front wheel steering angle from , and randomly obtain a speed from , repeat times to obtain combinations of front wheel steering angles and speeds .

[0194] For each set of front wheel steering angles and speeds, with as the abscissa , ordinate and orientation angle as the initial state, and with a time step as the integration step, integrate according to formula (10) to obtain candidate child nodes denoted as , and the set of paths corresponding to each candidate child node is denoted as .

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

[0196] Perform collision detection on all paths in the set , delete the paths that collide from the path set, and the collision-free path set obtained is denoted as ; The corresponding nodes are deleted from , and the collision-free node set obtained is denoted as .

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

[0198] For All nodes in it calculate their comprehensive cost, and the comprehensive cost includes: (1) Direction-changing cost , if the transportation entity is moving forward, the speed direction is recorded as 1, otherwise it is -1. It should be noted that since the possibilities of moving forward and backward are 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 candidate child node and its parent node is the same, then the reverse cost , otherwise ; (2) Obstacle repulsion cost , and its calculation formula is shown in Equation (11), where is the distance from the candidate child node to the obstacle, is a threshold that limits the range affected by the obstacle; (3) Gravitational cost of the target point , where is the distance from the candidate child node to the target point; (4) Distance cost from the random sampling point , that is, the distance between the candidate child node and the random sampling point .

[0199] Combining the above four parts of the cost, a weighted sum is performed on them to obtain the comprehensive cost , and its formula is shown in Equation (12), where , , , respectively represent the weight coefficients of the reverse cost, the obstacle repulsion cost, the gravitational cost of the target point, and the distance cost from the random sampling point. Select the child node with the lowest comprehensive cost and denote it as .

[0200] (12)

[0201] Step 3-3-4: Re-wire for ;

[0202] Set the threshold length for re-wiring as ; Taking as the center and as the radius, determine a circle, denoted as ; Denote the set of all nodes on the tree that are inside the circle as . Generate a Reeds-Shepp curve from all nodes inside to and determine whether it will collide with obstacles. Remove all the points that collide with obstacles, and then find the one with the minimum length among the remaining Reeds-Shepp curves, and the corresponding The node in is used as the parent node of

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

[0204] Step 3-4-1: Target offset sampling within the ellipse;

[0205] Set a fixed probability , randomly select a random number , if , then randomly select a two-dimensional point in ; otherwise, randomly select a two-dimensional point in . Denote the obtained two-dimensional point as . This can guide the expansion of the tree towards the target direction to a certain extent, and at the same time, reduce invalid sampling while ensuring a certain degree of randomness;

[0206] Step 3-4-2: Find the point on the tree with the minimum Euclidean distance from the random point , and denote it as the nearest point ; Activate the growth of the tree with as the root node;

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

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

[0209] When generating child nodes, the path between the root node and the child nodes should be smoothed to meet the kinematic constraints of the shipborne special vehicle, so as to reduce the burden of backend optimization. According to the mechanical constraints of the shipborne special vehicle in formula (2), randomly select a front wheel steering angle from , randomly select a speed from , repeat times to obtain combinations of front wheel steering angles and speeds . For each set of front wheel steering angles and speeds, with the abscissa of , the ordinate and the orientation angle as the initial state, with a time step as the integration step size, integrate according to formula (10) to obtain candidate child nodes denoted as , and denote the set composed of the paths corresponding to each candidate child node as .

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

[0211] Perform collision detection on all paths in the set and delete the paths that collide from the path set. The collision-free path set obtained is denoted as ; The corresponding nodes are deleted from and the collision-free node set obtained is denoted as

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

[0213] Calculate the comprehensive cost for all nodes in . The comprehensive cost includes: (1) The cost of changing direction . If the transportation entity is moving forward, the speed direction is recorded as 1, otherwise it is -1. It should be noted that since the possibilities of moving forward and backward are 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 candidate child node and its parent node is the same, then the reversing cost , otherwise ; (2) The repulsive cost of obstacles , and its calculation formula is shown in Equation (11), where is the distance from the candidate child node to the obstacle, is a threshold that limits the range affected by the obstacle; (3) The gravitational cost of the target point , where is the distance from the candidate child node to the target point; (4) The distance cost from the random sampling point , that is, the distance between the candidate child node and the random sampling point .

[0214] Combining the above four parts of the cost, perform a weighted sum to obtain the comprehensive cost , and its formula is shown in Equation (12), where , , , represent the weight coefficients of the reversing cost, the repulsive cost of obstacles, the gravitational cost of the target point, and the distance cost from the random sampling point respectively. Select the child node with the lowest comprehensive cost and denote it as .

[0215] Step 3-4-4: Re-wire;

[0216] Set the threshold length for re-wiring as ; With as the center and Taking [radius] as the radius, determine a circle, denoted as ; Denote the set of all nodes on the tree that are inside the circle as . For all nodes inside , generate a Reeds-Shepp curve to and determine whether it will collide with obstacles. Remove all the points that collide with obstacles, and then find the Reeds-Shepp curve with the minimum length among the remaining curves. Take the nodes in the corresponding as the parent nodes of .

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

[0218] Find the two closest points on the two trees, denoted as , respectively, generate the Reeds-Shepp curve between them, and perform collision detection. If the Reeds-Shepp curve does not collide with obstacles, stop the search and go to Step 3-5; otherwise, go to Step 3-2.

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

[0220] Start backtracking the parent nodes from until , denoted as ; Start backtracking the parent nodes from until , and then reverse the order of the points on this path, denoted as ; Return a rough path

[0221] In summary, use a bidirectional RRT* algorithm inside an ellipse that combines artificial potential fields and considers kinematic constraints, taking into account the kinematic constraints of shipborne special vehicles, to plan a rough path for the center of the rear axle of the shipborne special vehicle . Denote the number of nodes in as , where the -th node is , and its coordinates are defined as . .

[0222] Step 4: Resample the rough path to obtain an initial guess;

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

[0224] Define the length of the rough path as the sum of the Euclidean distances between all adjacent nodes, denoted as ; Define the length of the -th path segment as ; Then . Among them, is the abscissa of the starting point of the -th path segment; is the abscissa of the ending point of the -th path segment; is the ordinate of the starting point of the -th path segment; is the abscissa of the ending point of the -th path segment.

[0225] Assume that the transportation trajectory of the shipborne special vehicle satisfies the principle of time optimality, that is, the variable-speed motion always uses the maximum or minimum acceleration, and the uniform motion runs at the maximum or minimum speed; when the shipborne special vehicle moves forward, define the maximum speed, the maximum acceleration during acceleration, and the maximum acceleration during deceleration as , and ; when the shipborne special vehicle moves backward, define the maximum speed, the maximum acceleration during acceleration, and the maximum acceleration during deceleration as , and . At the same time, define the maximum acceleration distance , the maximum deceleration distance and the sum of the maximum acceleration distance and the maximum deceleration distance . The transportation time of the -th path segment is denoted as , and the estimation formula is (13), then the estimation formula for the total transportation time is expressed as .

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

[0227] Divide the time period into intervals, and thus obtain time nodes, then the length corresponding to each interval is , and the moment corresponding to the -th time node is . At the moment, the state variables of the shipborne special vehicle are denoted as . The speed and acceleration are estimated according to the principle of time optimality; the steering angle and steering angular velocity of the front wheels of the shipborne special vehicle are estimated according to the kinematic constraints, and the calculation formulas are as shown in formulas (14) and (15).

[0228] Terminal time is initialized to , and at all time nodes, the state variables and control variables are initialized to Denote the resampled path 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: Separate the in , and combine with formula (9) to calculate the centers of two feature circles corresponding to each path point, denoted as and .

[0231] Step 5-2: Construct safe transportation corridors for and respectively. The construction methods of the two are the same. Taking as an example, illustrate the generation method of the safe transportation corridor. Denote the safe transportation channel corresponding to the th point of as .

[0232] First, initialize as point , which is regarded as a rectangle with a width of 0 and a length of 0. Define the exploration direction set . Expand in the directions in sequentially at a fixed step size. Define the left, right, bottom, and top boundaries of the safe transportation channel obtained from the previous expansion as , and expand sequentially in the four directions in . When expanding in the direction , update the safe transportation channel data according to the following rules: It is only regarded as a valid expansion and the information of the safe transportation channel is updated accordingly when the following two conditions are met: 1) The safe transportation channel does not overlap with any obstacle grid in the grid map ; 2) The length of the safe transportation channel expanded in the direction does not exceed the expansion threshold ; otherwise, it is regarded as a failed expansion, and the direction is deleted from the exploration direction set . Continue this expansion process until the exploration direction set is empty. Expand for all

[0233] points to obtain the left, right, bottom, and top boundaries of each point as . .

[0234] Step 5-3: For Perform the processing of step 5-2 to obtain The left, right, lower, and upper boundaries of are as follows. Implement the following collision constraint transformation as shown in formula (16).

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

[0236] Step 6-1: Add the non-linear constraint in the form of an external penalty function to the objective function;

[0237] Replace the obstacle avoidance constraint in the initial optimal control problem as shown in formula (8) in steps 1-5 with the description form in formula (16) to form an optimal control problem as shown in formula (17):

[0238] (17)

[0239] To weaken the dependence of the optimal control problem on the initial guess, and at the same time, since constructing a safe transportation channel will inevitably waste some feasible space, therefore, adopt the strategy of iteratively solving the optimal control problem, use the obtained trajectory as the initial guess for the next solution, and regenerate the safe transportation channel based on the method of generating a safe transportation channel mentioned in step 5 based on this initial guess. Since there are still some non-linear constraints in further transformation is required for . Soften the non-linear constraints in

[0240] through an external penalty function, including the kinematic equality constraints as shown in formula (1) and formula (9), and the end boundary conditions as shown in formula (5), and incorporate them into the cost function as shown in formula (7). Specifically: It can be deduced that formula (18), move the right-hand term of the equal sign in the first equation in formula (18) to the left side to obtain formula (19).

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

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

[0243] Step 6-1-3: Combine the above three external penalty functions into: . Thus, the cost function with penalty terms is obtained :

[0244] (21)

[0245] where is the objective function defined by formula (7), is the weight coefficient of the external penalty function, which is set to 100 for the first iteration to ensure that the penalty for violating the transformed non-linear constraint conditions dominates.

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

[0247] (22)

[0248] Step 6-2: Introduce a positive 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 desired to make the shipborne special vehicles drive forward as much as possible while ensuring the shortest possible time consumption. Therefore, the state of the shipborne special vehicles in reverse can be penalized so that the proportion of reverse driving of the shipborne special vehicles is reduced as much as possible. Take in the state variable alone and make a mapping of it using the sigmoid function: , where is a constant used to control the smoothness of the function. This sigmoid function maps to . This is a monotonically decreasing function, and the minimum value of the function is obtained at , and the maximum value of the function is obtained at . Therefore, the positive guidance penalty function is as follows: . Then the objective function with the positive guidance penalty function is:

[0250] (23)

[0251] where is the weight coefficient of the positive guidance penalty function. It should be noted that the selection of this coefficient should make and The order of magnitude remains consistent. If this coefficient is too small, the forward guidance penalty will become ineffective. If this coefficient is too large, the transportation entity will overly focus on forward driving during the driving process, resulting in a significant increase in the driving time. Considering all these factors, it is set as: . Among them, is the estimated time obtained by resampling, is the number of nodes for resampling. 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 penalty of forward guidance are in an equal position. Then the optimal control problem with the forward guidance penalty function is denoted as , and the formula is as follows:

[0252] (24)

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

[0254] The first iterative solution The initial guess of is obtained from Step 4, and the safe transportation channel for the first iteration is obtained from Step 5, is 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 from Step 5; the termination condition for the iteration is that the kinematic constraints shown in Formulas (1) and (9) and the boundary constraints shown in Formula (5) are all satisfied with strict equality constraints. Therefore, in each iterative solution process, increase , until the obtained during the iterative process is less than the allowable error which is 10 -6 , and the final trajectory is obtained.

[0255] In Figure 7 , the transportation trajectory from the initial point to the target point is shown. In Figures 8 to 12 , the changes of five variables, namely the speed, steering angle, heading angle, acceleration, and steering angular velocity of the tractor under the transportation scenario, are respectively shown. It can be seen that this method can better complete the path planning task.

[0256] The above-described embodiments only represent the implementation manners of the present invention, but should not be construed as limiting the scope of the patent of the present invention. It should be noted that for those skilled in the art, without departing from the concept of the present invention, several deformations and improvements can still be made, and these all belong to the protection scope of the present invention.

Claims

1. A method for planning the transportation trajectory of shipborne special vehicles considering driving habits, characterized in that, The method for planning the transportation trajectory of shipborne special vehicles includes the following steps: Step 1, establish an optimal control problem for the transportation trajectory planning problem; Step 2, generate an inflated map to transform the collision avoidance requirements; Step 3, use an improved RRT* algorithm, that is, the RRT* algorithm within a two-way ellipse considering kinematic constraints combined with the artificial potential field, to obtain a rough path; Step 4, resample the rough path to obtain an initial reference solution; Step 5, generate a safe transportation channel based on the resampled path points and simplify the form of the collision avoidance constraints; Step 6, transform the non-linear constraint conditions, apply a positive guidance penalty function, establish the final optimal control problem and solve it iteratively.

2. The method for planning the transportation trajectory of a shipborne special vehicle considering driving habits according to claim 1, characterized in that The specific content of Step 1 is as follows; Step 1-1: Describe the kinematic constraints of shipborne special vehicles; Define the parameters of the shipborne special vehicle, and denote the position of the rear axle center as , is the speed, is the front wheel steering angle, is the attitude angle, is the acceleration, is the steering angular velocity, represents the wheelbase; then the kinematic constraints of the shipborne special vehicle are described by the following equations: （1） Wherein, is the end time, is the abscissa of the rear axle center of the shipborne special vehicle at moment, is the ordinate of the rear axle center of the shipborne special vehicle at moment, is the speed of the shipborne special vehicle at moment, is the front wheel steering angle of the shipborne special vehicle at moment, is the attitude angle of the shipborne special vehicle at moment, is the acceleration of the shipborne special vehicle at moment, is the steering angular velocity of the shipborne special vehicle at moment; Denote the state variables of the shipborne special vehicle as , and the control variables as ; The differential equation shown in Equation (1) is abbreviated as ; denotes derivative with respect to , and the function represents the differential equation; Moreover, during the driving process of the shipborne special vehicle, the following constraint conditions need to be satisfied: （2） Among them, , , , respectively represent the minimum acceleration, speed, steering angular velocity, and steering angle allowed for shipborne special vehicles; , , , respectively represent the maximum acceleration, speed, steering angular velocity, and steering angle allowed for shipborne special vehicles; Step 1-2: Describe the transportation task; At the initial moment , the configuration of the shipborne special vehicle is denoted as: （3） Among them, , , , , , and respectively represent the abscissa, ordinate, orientation angle, speed, steering angle, acceleration, and steering angular velocity of the center point of the rear axle of the shipborne special vehicle at the start time; At the terminal moment , the configuration of the shipborne special vehicle is denoted as: （4） （5） Among them, , , , , , and respectively represent the abscissa of the center point of the rear axle of the shipborne special vehicle at the terminal moment, the ordinate of the center point of the rear axle, the orientation angle, the speed, the steering angle, the acceleration, and the steering angular velocity; Step 1-3: Describe the collision avoidance constraints; Denote the space containing all shipborne special vehicle configurations as the configuration space , and denote all configurations where the shipborne special vehicle collides with obstacles as the non-passable space , then the passable space is denoted as ; The expression form of the 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 and the integral terms of the control variables and state variables: （7） Among them, is the weight coefficient of the integral term, and are the weight matrices of the state variable and the control variable respectively, is the objective function of the optimal control problem, is the state variable of the shipborne special vehicle, is the transpose of the state variable of the shipborne special vehicle, is the control variable of the shipborne special vehicle, is the transpose of the control variable of the shipborne special vehicle; Step 1-5: Establish the initial optimal control problem , and its specific form is as follows: （8）。 3. A method for planning the transportation trajectory of a shipborne special vehicle considering driving habits according to claim 2, characterized in that The specific content of Step 2 is as follows; Step 2-1: Transform the representation form of shipborne special vehicles; The geometric parameters of shipborne special vehicles include the front suspension length , the rear suspension length , the wheelbase , the body width ; Take two quarter points on the longitudinal axis of the shipborne special vehicle, denoted as and ; The radii of the two characteristic circles are equal, denoted as ; among which, at moment, and respectively represent the abscissa and ordinate of the front characteristic circle; and respectively represent the abscissa and ordinate of the rear characteristic circle; and respectively represent the abscissa and ordinate of the center point of the rear axle of the tractor; Shrink the characteristic circle to its center, and at the same time expand the obstacle with a radius The expanded map is called the inflated map, denoted as , realizing the transformation of the collision avoidance constraint; Step 2-2: Convert the inflated map into a grid map; Convert the inflated map into a raster map, denoted as ; for each raster, if it overlaps with the inflated obstacles in the inflated map , then it is regarded as an obstacle raster; otherwise, it is regarded as a passable raster.

4. A method for planning the transfer trajectory of a shipborne special vehicle considering driving habits according to claim 3, characterized in that In Step 2: In the said step 2-1, and The calculation formulas for the centers of the two circles are as follows: （9） In step 2-2: Set the resolution to , arbitrarily select a point on the aircraft carrier deck as the origin of the two-dimensional rectangular coordinate system, and determine the map boundaries of the x-axis and y-axis upward according to the length and width of the aircraft carrier deck, denoted as and , respectively, where and represent the minimum coordinate and the maximum coordinate in the x-axis direction, respectively, and represent the minimum coordinate and the maximum coordinate in the y-axis direction, respectively; define the grid to represent the two-dimensional rectangular area , where is the index of the grid in the x-axis direction, is the index of the grid in the y-axis direction; represents the coordinates included in the grid.

5. A method for planning the transfer trajectory of a shipborne special vehicle considering driving habits according to claim 3, characterized in that The specific content of Step 3 is as follows: Step 3-1: Initialize; The bidirectional search strategy maintains two trees, denoted as and respectively; initialize the two trees, set as the starting point , and set as the initialized end point ; the node information includes position information and orientation angle information. For the target offset sampling within the ellipse, determine the ranges of the ellipse and the target offset; denote and the Euclidean distance between them as , define the major axis of the ellipse as , where is a constant; define the minor axis as , the two foci of the ellipse are respectively and , denote the range within the ellipse as ; Take a circle with a radius of For its target offset range is within a circle centered at with a radius of Denote this range as ; For its target offset range is within a circle centered at with a radius of Denote this range as ; Step 3-2: Compare the scales of the two trees; Statistics and the number of nodes on, which are respectively denoted as and ; Compare and for their magnitudes. When is relatively small, 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 , randomly select a random number . If , then randomly obtain a two-dimensional point in ; otherwise, randomly obtain a two-dimensional point in ; Denote the obtained two-dimensional point as ; Step 3-3-2: Find the tree The point on the with the minimum Euclidean distance from the random point, denoted as the nearest point ; Activate the growth of the tree with as the root node; Step 3-3-3: Generate child nodes; Step 3-3-3-1: Integrate to generate a group of candidate child nodes; According to the constraint conditions of formula (2), randomly obtain a front wheel steering angle from , and randomly obtain a speed from . Repeat times to obtain combinations of front wheel steering angles and speeds ; For each set of front wheel angles and speeds, using as the abscissa , the ordinate and the orientation angle as the initial state, with a time step as the integration step size, integrating according to formula (10), obtaining candidate child nodes to be denoted as , and the set composed of the paths corresponding to each candidate child node is denoted as ; （10） Step 3-3-3-2: Perform collision avoidance detection on the candidate child nodes; Perform collision detection on all paths in the set , delete the paths that collide from the path set, and denote the collision-free path set obtained as ; delete the corresponding nodes from , and denote the collision-free node set obtained as , Step 3-3-3-3: Calculate the comprehensive cost and determine the child nodes; Step 3-3-4: For Re-wire; Set the threshold length for rerouting to be ; With as the center and as the radius, determine a circle, denoted as ; Denote the set of all nodes on the tree that are inside the circle as ; Generate a Reeds-Shepp curve from all nodes within to and determine if it collides with an obstacle. Remove the points that collide with the obstacle, find the one with the minimum length among the remaining Reeds-Shepp curves, and use the corresponding node as the parent node; Step 3-4: Expand the tree ; Step 3-4-1: Target bias sampling within the ellipse; Set a fixed probability , randomly select a random number . If , then randomly obtain a two-dimensional point in ; otherwise, randomly obtain a two-dimensional point in ; Denote the obtained two-dimensional point as ; Step 3-4-2: Find the tree the point on the with the smallest Euclidean distance to the random point, denoted as the nearest point ; Activate the growth of the tree with as the root node; Step 3-4-3: Generate child nodes; Step 3-4-3-1: Integrate to generate a group of candidate child nodes; Randomly obtain a front-wheel steering angle from and a speed from . Repeat times to obtain combinations of front-wheel steering angles and speeds ; for each set of front-wheel steering angle and speed, with the abscissa , ordinate and orientation angle as the initial state, and with a time step as the integration step size, integrate according to formula (10) to obtain candidate child nodes denoted as . Denote the set composed of the paths corresponding to each candidate child node as ;​ Step 3-4-3-2: Perform collision avoidance detection on the candidate child nodes; Perform collision detection on all paths in the set , delete the paths that collide from the path set, and denote the collision-free path set obtained as ; delete the corresponding nodes from , and denote the collision-free node set obtained as , Step 3-4-3-3: Calculate the comprehensive cost and determine the child nodes; Step 3-4-4: Re-wire; Set the threshold length for re - wiring as ; With as the center and as the radius, determine a circle, denoted as ; Denote the set of all nodes on the tree that are inside the circle as ; Generate a Reeds-Shepp curve from all nodes within to and determine whether it collides with obstacles. Remove the points that collide with obstacles, then find the one with the minimum length among the remaining Reeds-Shepp curves, and use the corresponding nodes as the parent nodes; Step 3-4: Try to connect the two trees; Find the two closest points on the two trees, denoted as , respectively, and generate the Reeds-Shepp curve between them . Conduct collision detection. If the Reeds-Shepp curve does not collide with the obstacle, stop the search and go to step 3-5; otherwise, go to step 3-2; Step 3-5: Backtrack the parent node and return a path; From Start backtracking the parent nodes until , denoted as ; From Start backtracking the parent nodes until , then reverse the order of the points on this path, denoted as ; Return a rough path , and finally it is the center of the rear axle of the shipborne special vehicle Plan a rough path ; Denote The number of nodes in as , where the th node is .

6. A method for planning the transfer trajectory of a shipborne special vehicle considering driving habits according to claim 5, characterized in that, In Step 3: Step 3-3-3-3 is: For all nodes in, calculate their comprehensive cost, which includes: (1) Direction change cost . If the transportation 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 candidate child node and its parent node is the same, then the reverse cost , otherwise ; (2) Obstacle repulsion cost , whose calculation formula is shown in Equation (11), where is the distance from the candidate child node to the obstacle, is the threshold value, which is an integer multiple of the wheelbase length of the shipborne special vehicle; (3) Gravitational cost of the target point , where is the distance from the candidate child node to the target point; (4) Distance cost between the candidate child node and the randomly sampled point , that is, the distance between the candidate child node and the randomly sampled point ; （11） Combining the above four parts of costs, a weighted sum is performed on them to obtain the comprehensive cost , and its formula is shown in Equation (12), where , , , , respectively represent the weight coefficients of the reverse cost, the obstacle repulsion cost, the gravitational cost of the target point, and the distance cost from the randomly sampled point; select the child node with the lowest comprehensive cost and denote it as ; （12） Step 3-4-3-3 is: For all nodes in, calculate their comprehensive cost, where the comprehensive cost includes: (1) Direction change cost . If the transportation 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 candidate child node and its parent node is the same, then the reverse cost , otherwise ; (2) Obstacle repulsion cost , and its calculation formula is shown in Equation (11); (3) Gravitational cost of the target point , where is the distance from the candidate child node to the target point; (4) Distance cost between the candidate child node and the randomly sampled point , that is, the distance between the candidate child node and the randomly sampled point . Combining the above four parts of costs and performing a weighted sum on them to obtain the comprehensive cost , and its formula is shown in Equation (12), where , , , , respectively represent the weight coefficients of the reverse cost, the obstacle repulsion cost, the gravitational cost of the target point, and the distance cost between the random sampling points; the child node with the lowest comprehensive cost is selected and denoted as .

7. A method for planning the transportation trajectory of a shipborne special vehicle considering driving habits according to claim 5, characterized in that, In Step 4: Step 4-1: Estimate the transportation time; Define the length of the coarse path as the sum of the Euclidean distances between all adjacent nodes, denoted as ; Define the length of the -th segment of the path as ; Then ; where is the abscissa of the starting point of the -th segment of the path; is the abscissa of the ending point of the -th segment of the path; is the ordinate of the starting point of the -th segment of the path; is the abscissa of the ending point of the -th segment of the path; Assume that the transportation trajectory of shipborne special vehicles satisfies the principle of time optimality; when the shipborne special vehicle is moving forward, define the maximum speed, the maximum acceleration during acceleration, and the maximum acceleration during deceleration as , and respectively; when the shipborne special vehicle is moving backward, define the maximum speed, the maximum acceleration during acceleration, and the maximum acceleration during deceleration as , and respectively; at the same time, define the maximum acceleration distance , the maximum deceleration distance and the sum of the maximum acceleration distance and the maximum deceleration distance ; the transportation time of the th section of the path is denoted as , and the estimation formula is (13), then the estimation formula for the total transportation time is expressed as ; （13） Step 4-2: Resample at discrete points; Divide the time period into equal intervals, and thus obtain time nodes. Then the length corresponding to each interval is , and the moment corresponding to the th time node is ; at the moment, the state variables of the shipborne special vehicle are denoted as ; the speed and acceleration are estimated according to the time-optimal principle; the steering angle and steering angular velocity of the front wheels of the shipborne special vehicle End time Initialize to , at all time nodes, the state variables and control variables are initialized to ; Denote the resampled path as .

8. A method for planning the transportation trajectory of a shipborne special vehicle considering driving habits according to claim 7, characterized in that, The specific content of Step 5 is: Step 5-1: Separate from , and combined with formula (9), calculate the centers of the two feature circles corresponding to each path point, denoted as and respectively; Step 5-2: Respectively and construct a safe transportation corridor. The construction methods of the two are the same. Take as an example to illustrate; Denote the safe transportation channel corresponding to the th point as , and obtain the left, right, lower, and upper boundaries of each point's as ; Step 5-3: For perform the processing of Step 5-2 to obtain the left, right, bottom, and top boundaries of each point are respectively ; Implement the following transformation of the collision constraint, as follows: （16）。 9. A method for planning the transportation trajectory of a shipborne special vehicle considering driving habits according to claim 8, characterized in that, In Step 5: In the said step 5-2, The method for constructing a safe transportation corridor is as follows: First, initialize as a point , which is regarded as a rectangle with a width of 0 and a length of 0; define the set of exploration directions ; with a fixed step size , expand in the directions in in sequence; define the left, right, bottom, and top boundaries of the safe transportation channel obtained by expansion in the previous step as , and expand in the four directions in in sequence; when expanding in the direction , update the safe transportation channel data according to the following rules: It is only regarded as a valid expansion and the information of the safe transportation channel is updated accordingly when the following two conditions are met: 1) The safe transportation channel shall not overlap with any obstacle grid in the grid map ; 2) The length of the safe transportation channel expanded in the direction does not exceed the expansion threshold ; Otherwise, it is regarded as an extension failure, and the direction is deleted from the exploration direction set ; This extension process is continuously carried out until the exploration direction set is empty; For each point is expanded to obtain the left, right, bottom, and top boundaries of each point are respectively .

10. A method for planning the transportation trajectory of a shipborne special vehicle considering driving habits according to claim 8, characterized in that, The specific content of Step 6 is: Step 6-1: Add the penalty function form other than the non-linear constraint to the objective function; Replace the obstacle avoidance constraint in the initial optimal control problem shown in formula (8) in steps 1 - 5 with the description form in formula (16) to form an optimal control problem as shown in formula (17): ​ （17） Adopt the strategy of iteratively solving the optimal control problem, use the obtained trajectory as the initial guess for the next solution, and regenerate a safe transportation channel based on this initial guess; For the non - linear constraints in are softened by an external penalty function. Specifically: Step 6-1-1: Transform the kinematic constraints as shown in formula (1); According to what is mentioned in step 1-1 It is derived that: （18） Among them, represents the th time point; represents the state variable at the represents the state variable at the Move the right-hand term of the equal sign in the first equation of formula (18) to the left side and denote it as: （19） The external penalty function form of the kinematic constraint shown in Equation (1) is expressed as: ; Step 6-1-2: Transform formula (9), move the left side of the equation in formula (9) to the right side of the equation, and denote it as the following vector-valued function: （20） Then, the external penalty function form of the kinematic constraint as shown in Equation (9) is expressed as: ; the external penalty function form of the boundary constraint as shown in Equation (5) is expressed as: ; Step 6-1-3: Combine the above three exterior penalty functions into: ; thus obtaining the cost function with a penalty term : （21） Among them, is the objective function defined by formula (7), is the weight coefficient of the external penalty function; Step 6-1-4: Finally, obtain the lightweight optimal control problem , as shown in Equation (22): （22） Step 6-2: Introduce a positive guidance penalty function into the objective function shown in formula (22); Punish the reverse state of shipborne special vehicles; the state variable in is taken out separately, and the sigmoid function is used to map to ; the penalty function for forward guidance is as follows: , and the objective function with the forward guidance penalty function is: （23） Among them, is the weight coefficient of the positive guiding penalty function; Taking all the above into consideration, set it as: ; where is the estimated time obtained by resampling, is the number of nodes for resampling; then the optimal control problem with a positive guidance penalty function is denoted as , and the formula is as follows: （24） Step 6-3: Iteratively solve the optimal control problem with the positive guidance penalty function.

Images