A tractor trajectory iterative planning method based on improved search random tree algorithm

Abstract

The present invention provides a tractor trajectory iterative planning method based on an improved search random tree algorithm, which belongs to the field of intelligent equipment support. The method firstly constructs an optimal control problem based on the transportation demand and the motion characteristics of the tractor; secondly, the vehicle body is represented by a characteristic circle according to the actual shape of the tractor, and the obstacle avoidance problem is equivalently transformed; thirdly, a grid map is obtained according to the actual map and combined with the radius of the characteristic circle. Fourthly, a bidirectional fast search random tree algorithm based on the Reeds-Shepp curve is used to obtain a coarse path; fifthly, the coarse path is resampled to obtain an initial reference solution; sixthly, a safe channel is constructed to simplify the form of obstacle avoidance constraints; seventhly, based on the obstacle avoidance constraints and the motion constraints of the tractor, the nonlinear term constraints are softened, and the optimal control problem is established and solved iteratively. The present invention can continuously update the safe channel, weaken the influence of the initial guess solution on the final solution result, and obtain a high-quality planning path.

Description

Technical Field

[0001] The invention belongs to the field of intelligent equipment support, and relates to a tractor trajectory iterative planning method based on an improved search random tree algorithm, in particular to a tractor trajectory iterative planning method based on a bidirectional fast search random tree algorithm of a Reeds-Shepp curve. Background Art

[0002] Compared with land airports, the space on aircraft carrier decks is smaller and the environment is more complex. In addition, the aircraft carrier decks are used to park multiple fighters, fuel trucks, maintenance equipment, etc., which further reduces the available space on the aircraft carrier decks. A suitable method for representing the shape of the tractor is needed to leave as large a feasible area as possible to increase the probability of successfully searching for a feasible path. Due to the complex shapes of obstacles on the aircraft carrier deck, it is difficult to divide the obstacles on the aircraft carrier deck into multiple obstacles that are easy to detect collisions. And it is necessary to convert obstacle avoidance constraints into efficient constraints to improve planning efficiency. Due to the complex environment and dense obstacles on the aircraft carrier deck, a high success rate initial reference path planning method is needed to improve the success rate of planning a feasible initial reference path, and to ensure that the initial reference path should be smooth enough to reduce the burden of back-end optimization. For example, the bidirectional search random tree algorithm in the Chinese invention patent "A mobile robot path planning system and method based on deep reinforcement learning algorithm" (CN118409584A) does not consider the kinematic constraints of the path, which will increase the burden of back-end optimization. When the obstacle environment is very complex, the quality of the initial reference path will decrease, which will affect the optimality of the solution. It is necessary to propose an efficient solution framework to ensure the success rate and optimality of path planning. For example, the optimal control problem in the Chinese invention patent "System and method for collaborative operation of special vehicles under dynamic environment" (CN118655885A) requires that all nonlinear constraints must be strictly satisfied. When the obstacle environment is very complex, the probability of successful problem solving will be greatly reduced.

[0003] In view of the above problems, it is necessary to propose an optimal control problem configuration and solution framework with high precision and efficiency to ensure that a trajectory that allows the tractor to safely complete the transportation task can be accurately and efficiently planned. Summary of the invention

[0004] In order to solve the above-mentioned technical problems, the present invention proposes a tractor trajectory iterative planning method based on an improved search random tree algorithm, specifically a tractor trajectory iterative planning method based on a bidirectional fast search random tree algorithm of the Reeds-Shepp curve. In the present invention, a multi-stage planning strategy is adopted. First, a coarse path that satisfies the maximum turning radius constraint is searched out using a bidirectional fast random search tree algorithm based on the Reeds-Shepp curve to ensure that a high-quality homotopic trajectory can be found through the path in the traversable space in a complex obstacle environment. Secondly, the initial guess solution of the optimal control problem is obtained by resampling the coarse path, and a safe channel is generated according to the sampling points to realize an efficient representation method of obstacle avoidance constraints, so as to improve the overall problem solving efficiency. Finally, an iterative solution strategy is adopted, and the nonlinear constraints in the optimal control problem are added as penalty terms to the objective function of the optimal control problem, which can significantly speed up the solution speed of each step, regenerate a safe channel with the results obtained in each step, make full use of the traversable space, and reduce the influence of the initial coarse path on the final solution trajectory.

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

[0006] A tractor trajectory iterative planning method based on an improved search random tree algorithm, wherein the tractor trajectory iterative planning method, first, preliminarily constructs an optimal control problem based on the transportation demand and the motion characteristics of the tractor; second, based on the actual shape of the tractor, two characteristic circles are used to represent the body, and the obstacle avoidance problem is equivalently transformed; third, a grid map is obtained based on the actual map and combined with the radius of the characteristic circle; fourth, a rough path is obtained using a bidirectional fast search random tree algorithm based on the Reeds-Shepp curve; fifth, the rough path is resampled to obtain an initial reference solution; sixth, a safe channel is constructed based on the resampling result to simplify the form of the obstacle avoidance constraint condition; seventh, based on the simplified obstacle avoidance constraint and the motion constraint of the tractor, the nonlinear term constraint is penalized and added to the objective function to achieve "softening" of the nonlinear term constraint condition, establish the final optimal control problem and solve it iteratively. The following steps are included:

[0007] Step 1: Establish a preliminary optimal control problem based on the transportation requirements and the motion characteristics of the tractor;

[0008] Step 1-1: Describe the kinematic equations of the tractor;

[0009] When the tractor is traveling at a low speed, the definition Denote the center position of the rear axle of the tractor. The motion model of the tractor can be approximately replaced by a bicycle model:

[0010] d d t [ x ( t ) y ( t ) v ( t ) ϕ ( t ) θ ( t ) ] = [ v ( t ) ⋅ c o s θ ( t ) v ( t ) ⋅ s i n θ ( t ) a ( t ) ω ( t ) v ( t ) ⋅ t a n ϕ ( t ) / L W ] , t ∈ [ 0 , T ] (1)

[0011] in, is the steering angle of the tractor; is the steering angular velocity of the tractor; is the speed of the tractor; is the acceleration of the tractor; is the heading angle of the tractor; Indicates the wheelbase of the tractor (e.g. Figure 2 shown); For time; For the end time.

[0012] The state variables of the tractor system are recorded as z ( t ) = [ x ( t ) , y ( t ) , θ ( t ) , v ( t ) , ϕ ( t ) ] T , the control variable is recorded as u ( t ) = [ a ( t ) , ω ( t ) ] T .

[0013] In addition, the following constraints are considered during the movement of the tractor:

[0014] [ a m i n v m i n − Ω m a x − Φ m a x ] ≤ [ a ( t ) v ( t ) ω ( t ) ϕ ( t ) ] ≤ [ a m a x v m a x Ω m a x Φ m a x ] , t ∈ [ 0 , T ] (2)

[0015] in, , Respectively represent the minimum and maximum acceleration allowed for the tractor; , Respectively represent the minimum and maximum speeds allowed for the towing vehicle; , They represent the minimum and maximum steering angle speeds allowed by the tractor respectively; , Respectively represent the minimum and maximum steering angles allowed for the tractor.

[0016] Step 1-2: Describe the task requirements;

[0017] The transportation task is described by boundary conditions, that is, the state of the tractor at the beginning and the expected state of the tractor at the end. , the configuration of the tractor is recorded as:

[0018] [ x ( 0 ) , y ( 0 ) , θ ( 0 ) , v ( 0 ) , ϕ ( 0 ) , a ( 0 ) , ω ( 0 ) ] = [ x 0 , y 0 , θ 0 , v 0 , ϕ 0 , a 0 , w 0 ] (3)

[0019] in, Indicates the horizontal coordinate of the center point of the rear axle of the tractor at the start time; Indicates the ordinate of the center point of the rear axle of the tractor at the start time; Indicates the heading angle of the tractor at the start time; Indicates the speed of the tractor at the start time; Indicates the steering angle of the tractor at the start time; represents the acceleration of the tractor at the initial moment; Represents the steering angular velocity at the initial moment.

[0020] At the terminal moment ( is also a variable to be optimized), the configuration of the tractor is recorded as:

[0021] [ x ( T ) , y ( T ) , v ( T ) , ϕ ( T ) , a ( T ) , ω ( T ) ] = [ x F , y F , v F , ϕ F , a F , w F ] (4)

[0022] (5)

[0023] in, Represents the horizontal coordinate of the center point of the rear axle of the tractor at the end moment; Indicates the ordinate of the center point of the rear axle of the tractor at the end moment; Indicates the speed of the tractor at the end moment; Indicates the steering angle of the tractor at the end moment; represents the acceleration of the tractor at the terminal moment; The steering angle acceleration at the end moment is shown; Indicates the heading angle of the tractor at the end moment.

[0024] Step 1-3: Describe obstacle avoidance constraints;

[0025] The entire configuration space is denoted as , using the function The state variable Mapped to the configuration space, the space where the tractor collides with the obstacle is recorded as the obstacle area , therefore, the traversable area can be recorded as , then the obstacle avoidance constraint is expressed as:

[0026] (6)

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

[0028] In the process of tractor transportation, time consumption, energy consumption and stability are important factors to consider. The running time can be directly obtained from the terminal time. Reflection, while considering economic factors should be minimized energy consumption, should improve the smoothness of the trajectory to ensure sufficient stability and comfort, so the following hybrid objective function is obtained:

[0029] (7)

[0030] in, and Represent the weight matrices of state quantity and control quantity respectively, is the weight coefficient, is the objective function of the entire optimal control problem.

[0031] Step 1-5: Establish a preliminary optimal control problem;

[0032] In summary, the preliminary optimal control problem is established , its specific form is:

[0033] (8)

[0034] Step 2: Based on the actual shape of the tractor, two characteristic circles are used to represent the vehicle body to simplify the obstacle avoidance constraints;

[0035] exist Figure 2 In the figure, the front suspension length of the tractor is recorded as , the rear suspension length is recorded as , the wheelbase is , the body width is recorded as .like Figure 3 As shown, two characteristic circles are used to represent the vehicle body. The centers of the two characteristic circles are located at the two quarter points of the vehicle body, respectively, and are denoted as and , and we get the following expression:

[0036] (9)

[0037] The two characteristic circles specifically include a front characteristic circle and a rear characteristic circle. The radius of each characteristic circle is equal and is recorded as , the expression is ; Among them, time, represents the abscissa of the front characteristic circle; Represents the ordinate of the front characteristic circle; Represents the abscissa of the posterior characteristic circle; Represents the ordinate of the rear characteristic circle; Represents the horizontal coordinate of the center point of the rear axle of the tractor; Indicates the ordinate of the center point of the rear axle of the tractor.

[0038] Nominal obstacle avoidance requires that each feature circle does not overlap with an obstacle. This is consistent with the center of the feature circle and the obstacle. Therefore, we can perform an equivalent transformation, shrinking the characteristic circle to the center of the circle and moving the obstacle through This transformation helps to generate a box-shaped safe passage and simplify the obstacle avoidance constraints.

[0039] Step 3: Obtain a grid map based on the actual map and the characteristic circle radius;

[0040] Based on the dilation method described in step 2, the resolution Constructing a grid map of an aircraft carrier deck For each grid, if it overlaps with the expanded obstacle area, it is considered as an obstacle grid; otherwise, it is considered as a passable grid. The grid map boundaries in the X and Y directions are [ X min , X max ] and [ Y min , Y max ] . Define the grid ” represents the following two-dimensional region: . Figure 4 and Figure 5 This paper shows the collision detection method in grid map. Represents the index of the raster; Represent the coordinates of the grid respectively.

[0041] Step 4: Obtain a rough path on the grid map using a bidirectional fast search random tree algorithm based on the Reeds-Shepp curve;

[0042] Step 4-1: Initialize the tree;

[0043] To use bidirectional fast search for random numbers, two trees need to be initialized. The two trees expand outward from the starting point and the end point of the transfer, respectively, and are recorded as and . Add the starting point and the end point to and ,Node information includes location information and orientation angle information.

[0044] Step 4-2: Expand the tree ;

[0045] Step 4-2-1: Semi-random sampling;

[0046] Pick a random point In order to speed up the convergence of the tree, a semi-random sampling strategy is adopted, that is, with a certain probability Randomly select a point in space. The point information includes three parts: horizontal coordinate, vertical coordinate and orientation angle. Probability selection tree Upper distance tree The nearest point, which can limit the expansion direction of the tree to a certain extent, but without losing the randomness of the expansion;

[0047] Step 4-2-2: Find the tree Random point above The point with the smallest cost is recorded as the closest point ; The minimum cost is the point Euclidean distance;

[0048] Step 4-2-3: Generate child nodes;

[0049] When connecting two points, the path should be as smooth as possible to meet the kinematic constraints of the tractor and the minimum turning radius limit, which can be used to reduce the burden of back-end optimization and generate and The Reeds-Shepp curve strategy between and Get the Reeds-Shepp curve .

[0050] According to step 4-2-1, there are two situations when semi-random sampling occurs:

[0051] The first case: When the The probability of selecting Upper distance When the nearest point is reached, judge and Will the Reeds-Shepp curve between the two trees collide with an obstacle? If there is no collision with an obstacle, the two trees can be directly connected to stop the search. If there is a collision with an obstacle, a fixed step size is set. , select the curve Up Distance is The point is taken as a child node, denoted as ;

[0052] The second situation: When the When randomly selecting points in the probability space of , select the curve Up Distance is The point is taken as a child node, denoted as ;

[0053] Step 4-2-4: Detection and Whether the connecting line between them collides with an obstacle. If not, the tree expansion is considered successful and step 4-3 is executed; otherwise, the expansion fails and go to step 4-4.

[0054] Step 4-3: Rewiring;

[0055] Set a fixed length ,by Centered on As the radius, put in this spherical neighborhood, and in the tree The set of all nodes on . Traverse All nodes in the first place, determine these nodes and Will the Reeds-Shepp curve between the two vertices collide with the obstacle? Remove all the points that collide with the obstacle, keep the points that do not collide with the obstacle, and then find the points that collide with the obstacle among the remaining nodes. The point with the smallest distance between The parent node of .

[0056] Step 4-4: Expand the tree ;

[0057] Step 4-4-1: Semi-random sampling;

[0058] Pick a random point In order to speed up the convergence of the tree, a semi-random sampling strategy is adopted, that is, with a certain probability Randomly select a point in space. The point information includes three parts: horizontal coordinate, vertical coordinate and orientation angle. Probability selection tree Upper distance tree The nearest point, which can limit the expansion direction of the tree to a certain extent, but without losing the randomness of the expansion;

[0059] Step 4-4-2: Find the tree Random point above The point with the smallest cost is recorded as the closest point ; The minimum cost is the point Euclidean distance;

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

[0061] When connecting two points, the path should be as smooth as possible to meet the kinematic constraints of the tractor and the minimum turning radius limit, which can be used to reduce the burden of back-end optimization and generate and The Reeds-Shepp curve strategy between and Get the Reeds-Shepp curve .

[0062] According to step 4-2-1, there are two situations when semi-random sampling occurs:

[0063] The first case: When the The probability of selecting Upper distance When the nearest point is reached, judge and Will the Reeds-Shepp curve between the two trees collide with an obstacle? If there is no collision with an obstacle, the two trees can be directly connected to stop the search. If there is a collision with an obstacle, a fixed step size is set. , select the curve Up Distance is The point is taken as a child node, denoted as ;

[0064] The second situation: When the When randomly selecting points in the probability space of , select the curve Up Distance is The point is taken as a child node, denoted as ;

[0065] Step 4-4-4: Detection and Whether the connecting line collides with an obstacle. If there is no collision with an obstacle, the tree expansion is considered successful, otherwise the expansion fails.

[0066] Step 4-5: Rewiring;

[0067] Set a fixed length ,by Centered on As the radius, put in this spherical neighborhood, and in the tree The set of all nodes on . Traverse All nodes in the first place, determine these nodes and Will the Reeds-Shepp curve between the two vertices collide with the obstacle? Remove all the points that collide with the obstacle, keep the points that do not collide with the obstacle, and then find the points that collide with the obstacle among the remaining nodes. The point with the smallest distance between The parent node of .

[0068] When two trees can be directly connected in step 4-2-3 or step 4-4-3, stop searching and return a rough path .

[0069] In summary, the bidirectional fast search random tree algorithm based on the Reeds-Shepp curve is used, considering the minimum turning radius constraint of the tractor, which is the center of the rear axle of the tractor Planning a rough path .remember The number of nodes in is , among which The nodes are , whose coordinates are defined as .

[0070] Step 5: Resample the coarse path to obtain the initial reference solution;

[0071] Step 5-1: Estimation of transportation time;

[0072] The length of the path after resampling the coarse path is defined as the sum of the distances between all adjacent nodes, denoted as , its expression can be written as ,in, Representation interval [ 2 , N P ] Assuming that the acceleration motion in the path always uses the maximum or minimum acceleration, the acceleration during the sampling process can be expressed as: , , when the system is moving forward, defines the maximum forward speed , the maximum acceleration of acceleration , Maximum acceleration of deceleration ; Otherwise, define the maximum reverse speed , Maximum acceleration for accelerating in reverse , Maximum acceleration for deceleration and reverse . Set three limits at the same time , as well as .in, Indicates the maximum acceleration distance; Indicates the maximum deceleration distance; Indicates the sum of the maximum acceleration distance and the maximum deceleration distance.

[0073] At this time The transport time of the segment path is estimated by formula (10), and the estimation formula of the total transport time is expressed as .

[0074] (10)

[0075] Step 5-2: Resample at discrete points;

[0076] Divide the time period into equal parts discrete time intervals, thus we get discrete points, then the time interval between every two adjacent discrete points is , then The discrete points correspond approximately to the time .exist At time , the state information of the tractor is recorded as The speed and acceleration are obtained based on the time optimal assumption. As for the steering angle and steering angular velocity of the front wheels of the tractor, they are determined based on the kinematic constraints shown below:

[0077] (11)

[0078] (12)

[0079] in, Indicated in The steering angle of the tractor's front wheels at all times; Indicated in Always keep the tractor towards the angle; Indicated in The tractor's heading angle at the moment; Indicated in The speed of the tractor at the time; Indicated in The steering angular velocity of the tractor at the moment; Indicated in The tractor is facing the angle at the moment.

[0080] The transportation time can be used Initialize, at all discrete points, the state variables and control variables can be Initialize. The resampling path is recorded as .

[0081] Step 6: Generate a safe channel based on the resampling results and simplify the form of obstacle avoidance constraints;

[0082] according to And formula (9), can calculate the center points of the two characteristic circles respectively and The discrete trajectories of and The generation process of the safe transportation corridor corresponding to the center of each characteristic circle is similar. As an example, the construction method of the safe transportation corridor is explained. In the At discrete points ( ), the corresponding position is recorded as ,remember The corresponding safety channel is recorded as .

[0083] First, Initialize to point It can be considered as a rectangle with a width of 0 and a length of 0. Define the set of trial directions =[up, left, down, right]. with a fixed step size , in a circular form Each direction in is explored in a fixed order. The left, right, bottom and top boundaries of the safe channel expanded in the previous iteration are defined as [ x ¯ f, n , x ¯ f, n , y ¯ f, n , y ¯ f, n ] , in a circular form along The data of the safety channel is continuously updated in a fixed order in the four directions. The process of updating the safety channel data is as follows: The expansion of the safety channel is considered valid only when the following two conditions are met, and the safety channel information is updated accordingly: Condition 1. The safety channel must not overlap with the grid map. Any obstacle grid overlaps; Condition 2: The safe passage is in the direction The length of the upper extension does not exceed the preset maximum extension length If any of the above conditions is not met, then the exploration direction set Remove direction This expansion process will continue until the set of exploration directions is empty.

[0084] right The above processing is performed on all points, so that the centers of the two characteristic circles can be The following collision constraint conversion is implemented for each sampling position, as shown below:

[0085] (13)

[0086] Step 7: "Soften" the nonlinear constraints, establish the final optimal control problem and select the iterative solution strategy;

[0087] Use the transformed constraints in formula (13) to replace the optimal control problem in steps 1-5 The obstacle avoidance conditions in the equation constitute the following optimal control problem. :

[0088] (14)

[0089] In order to ensure that the optimal control problem of each step of the iteration can be successfully solved during the iteration process, 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), formula (9), and the terminal boundary conditions shown in formula (5), which are merged into the cost function shown in formula (7). Formula (1) is softened as:

[0090] (15)

[0091] in, Indicated in hour, The specific form is to constrain the kinematic equation (1) to Integrate at each moment to get State quantity at the moment , then and Take the difference and find the second norm, then square it, which is called the error term. Sum the error terms.

[0092] Move the left side of the equation in formula (9) to the right side of the equation, and record it as , the softened form is:

[0093] (16)

[0094] Formula (5) is softened to:

[0095] (17)

[0096] The above three softened equations are combined into:

[0097] (18)

[0098] This gives the composite cost function:

[0099] (19)

[0100] in, is the initial objective function defined by formula (7), is the penalty term coefficient, which should be set to a larger number to ensure that the penalty for violating the softened nonlinear constraints dominates.

[0101] The final optimal control problem after softening The composition is as follows:

[0102] (20)

[0103] Solving the optimal control problem , and send its solution back to step 5 as the coarse path and improve , until the obtained Less than the allowable error For 10 -4 ~10 -5 .

[0104] The innovation of the present invention is: an improved two-way RTT algorithm is proposed, which includes: a semi-random strategy is adopted in the random sampling process to guide the two trees to expand toward each other with a certain probability, thereby accelerating convergence; a Reeds-Shepp curve is introduced when generating new nodes, so that the searched path can meet the constraint of the minimum turning radius of the tractor, thereby smoothing the path and reducing the burden on the back-end optimization. A framework for iteratively solving the optimal control problem of path planning is also proposed, which can make up for the situation that when the quality of the initial reference path is not high, the generated safe channel does not contain a feasible solution or an optimal solution, resulting in the feasibility and optimality of the problem solution being difficult to guarantee. The safe channel is continuously updated through iteration, so that the planned path is no longer completely dependent on the initial rough path, and the feasibility and optimality of the solution are guaranteed.

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

[0106] The present invention realizes the conversion of obstacle avoidance constraints through a safe channel, greatly simplifies the difficulty of solving the problem, and ensures that the tractor's transportation trajectory is generated efficiently and reliably. A bidirectional fast search random tree algorithm based on the Reeds-Shepp curve can efficiently generate an initial rough trajectory on a complex deck environment, and combined with the subsequent resampling process, a safe channel is generated, which also serves as the initial solution to the optimal control problem. By using two characteristic circles to represent the tractor body, the shape of the tractor can be represented as accurately as possible to ensure that the transportation trajectory can be successfully generated within a narrow track. Through a lightweight iterative framework, the safe channel can be continuously updated through iteration, which weakens the impact of the initial guess solution on the final solution result. Even when the quality of the initial guess solution is not high, a planning path with better quality can be obtained. BRIEF DESCRIPTION OF THE DRAWINGS

[0107] Figure 1 It is a flow chart of the present invention.

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

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

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

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

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

[0113] Figure 7 The initial reference path graph is searched using a bidirectional fast search random tree algorithm based on the Reeds-Shepp curve.

[0114] Figure 8 A path planning diagram.

[0115] Fig. 9 Graph of changes in speed of the planning process.

[0116] Fig.10 Diagram of steering angle changes during the planning process.

[0117] Fig.11 Diagram of the change in the planning process direction angle.

[0118] Fig.12 Graph of acceleration changes during the planning process.

[0119] Fig.13 Graph of the change in steering angular velocity during the planning process. DETAILED DESCRIPTION

[0120] The present invention is further described below in conjunction with specific embodiments.

[0121] Considering the problem of planning the transportation of a certain type of tractor on the deck of an aircraft carrier, the relevant parameters of the tractor 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.

[0122] Table 1 Tractor parameters

[0123]

[0124] Table 2 Algorithm parameters

[0125]

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

[0127]

[0128] A tractor trajectory iterative planning method based on improved bidirectional RRT* algorithm initialization specifically includes the following steps:

[0129] Step 1: Establish a preliminary optimal control problem based on the transportation requirements and the motion characteristics of the tractor;

[0130] Step 1-1: Describe the kinematic equations of the tractor;

[0131] When the tractor is traveling at a low speed, the definition Denote the center position of the rear axle of the tractor. The motion model of the tractor can be approximately replaced by a bicycle model:

[0132] d d t [ x ( t ) y ( t ) v ( t ) ϕ ( t ) θ ( t ) ] = [ v ( t ) ⋅ c o s θ ( t ) v ( t ) ⋅ s i n θ ( t ) a ( t ) ω ( t ) v ( t ) ⋅ t a n ϕ ( t ) / L W ] , t ∈ [ 0 , T ] (1)

[0133] in, is the steering angle of the tractor; is the steering angular velocity of the tractor; is the speed of the tractor; is the acceleration of the tractor; is the heading angle of the tractor; Indicates the wheelbase of the tractor (e.g. Figure 2 shown); For time; For the end time.

[0134] The state variables of the tractor system are recorded as z ( t ) = [ x ( t ) , y ( t ) , θ ( t ) , v ( t ) , ϕ ( t ) ] T , the control variable is recorded as u ( t ) = [ a ( t ) , ω ( t ) ] T .

[0135] In addition, the following constraints are considered during the movement of the tractor:

[0136] [ a m i n v m i n − Ω m a x − Φ m a x ] ≤ [ a ( t ) v ( t ) ω ( t ) ϕ ( t ) ] ≤ [ a m a x v m a x Ω m a x Φ m a x ] , t ∈ [ 0 , T ] (2)

[0137] in, , Respectively represent the minimum and maximum acceleration allowed by the tractor, and the values ​​are -1 m / s 2 and 1 m / s 2 ; , They represent the minimum and maximum speeds allowed by the tractor, and are -2 m / s and 2 m / s respectively; , They represent the minimum and maximum steering angle speeds allowed by the tractor, respectively -0.5 deg / s and 0.5 deg / s; , They represent the minimum and maximum steering angles allowed for the tractor, and their values ​​are -0.6 deg and 0.6 deg respectively.

[0138] Step 1-2: Describe the task requirements;

[0139] The transportation task is described by boundary conditions, that is, the state of the tractor at the beginning and the expected state of the tractor at the end. , the configuration of the tractor is recorded as:

[0140] [ x ( 0 ) , y ( 0 ) , θ ( 0 ) , v ( 0 ) , ϕ ( 0 ) , a ( 0 ) , ω ( 0 ) ] = [ x 0 , y 0 , θ 0 , v 0 , ϕ 0 , a 0 , w 0 ] (3)

[0141] in, Indicates the horizontal coordinate of the center point of the rear axle of the tractor at the starting time; Indicates the ordinate of the center point of the rear axle of the tractor at the start time; Indicates the heading angle of the tractor at the start time; Indicates the speed of the tractor at the start time; Indicates the steering angle of the tractor at the start time; represents the acceleration of the tractor at the initial moment; Represents the steering angular velocity at the initial moment. The values ​​are 75 m, 50 m, -1.57 deg, 0 m / s, 0 deg, 0 m / s 2 , 0 deg / s.

[0142] At the terminal moment ( is also a variable to be optimized), the configuration of the tractor is recorded as:

[0143] [ x ( T ) , y ( T ) , v ( T ) , ϕ ( T ) , a ( T ) , ω ( T ) ] = [ x F , y F , v F , ϕ F , a F , w F ] (4)

[0144] (5)

[0145] in, Represents the horizontal coordinate of the center point of the rear axle of the tractor at the end moment; Indicates the ordinate of the center point of the rear axle of the tractor at the end moment; Indicates the heading angle of the tractor at the end moment; Indicates the speed of the tractor at the end moment; Indicates the steering angle of the tractor at the end moment; represents the acceleration of the tractor at the terminal moment; The steering angular acceleration at the end moment is shown. The values ​​are 200 m, 25 m, -1.57 deg, 0 m / s, 0 deg, 0 m / s 2 , 0 deg / s.

[0146] Step 1-3: Describe obstacle avoidance constraints;

[0147] The entire configuration space is denoted as , using the function The state variable Mapped to the configuration space, the space where the tractor collides with the obstacle is recorded as the obstacle area , therefore, the traversable area can be recorded as , then the obstacle avoidance constraint can be expressed as follows:

[0148] (6)

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

[0150] In the process of tractor transportation, time consumption, energy consumption and stability are important factors to consider. The running time can be directly obtained from the terminal time. Reflection, while considering economic factors should be minimized energy consumption, should improve the smoothness of the trajectory to ensure sufficient stability and comfort, so the following hybrid objective function is obtained:

[0151] (7)

[0152] in, and Represent the weight matrices of state quantity and control quantity respectively, is the weight coefficient, is the objective function of the entire optimal control problem, and , ,

[0153] Step 1-5: Establish a preliminary optimal control problem;

[0154] In summary, the preliminary optimal control problem is established , its specific form is:

[0155] (8)

[0156] Step 2: Based on the actual shape of the tractor, two characteristic circles are used to represent the vehicle body to simplify the obstacle avoidance constraints;

[0157] exist Figure 2 In the figure, the front suspension length of the tractor is recorded as , the rear suspension length is recorded as , the wheelbase is , the body width is recorded as In this case, we take , , , .like Figure 3 As shown, two characteristic circles are used to represent the vehicle body. The centers of the two characteristic circles are located at the two quarter points of the vehicle body, respectively, and are denoted as and , and we get the following expression:

[0158] (9)

[0159] The radius of each characteristic circle is recorded as , the expression is .

[0160] Nominal obstacle avoidance requires that each feature circle does not overlap with an obstacle. This is consistent with the center of the feature circle and the obstacle. and above are equivalent. Therefore, we can perform an equivalent transformation, shrinking the disk to the center of the circle and moving the obstacle through This transformation helps to generate a box-shaped safe passage and thus simplify the obstacle avoidance constraints.

[0161] Step 3: Obtain a grid map based on the actual map and the characteristic circle radius;

[0162] Based on the dilation method described in step 2, the resolution Constructing a grid map of an aircraft carrier deck For each grid, if it overlaps with the expanded obstacle area, it is considered as an obstacle grid; otherwise, it is considered as a passable grid. The grid map boundaries in the X and Y directions are [ X min , X max ] and [ Y min , Y max ] . According to the data in the case , , , . Let "Grid ” represents the following two-dimensional region: . Define the grid ” represents the following two-dimensional region: . Figure 4 and Figure 5 Demonstrates collision detection methods in grid maps. Figure 6 This is a deck map of an aircraft carrier. Represents the index of the raster; Represent the coordinates of the grid respectively.

[0163] Step 4: Obtain a rough path on the grid map using a bidirectional fast search random tree algorithm based on the Reeds-Shepp curve;

[0164] Step 4-1: Initialize the tree;

[0165] To use bidirectional fast search for random numbers, two trees need to be initialized. The two trees expand outward from the starting point and the end point of the transfer, respectively, and are recorded as and . Add the starting point and the end point to and ,Node information includes location information and orientation angle information.

[0166] Step 4-2: Expand the tree ;

[0167] Step 4-2-1: Semi-random sampling;

[0168] Pick a random point In order to speed up the convergence of the tree, a semi-random sampling strategy is adopted, that is, Randomly select a point in space. The point information includes three parts: horizontal coordinate, vertical coordinate and orientation angle. Probability selection tree Upper distance tree The nearest point, which limits the expansion direction of the tree to a certain extent, but does not lose the randomness of the expansion;

[0169] Step 4-2-2: Find the tree Random point above The point with the smallest cost is recorded as the closest point ; The minimum cost is the point The Euclidean distance is the smallest;

[0170] Step 4-2-3: Generate child nodes;

[0171] When connecting two points, the path should be as smooth as possible to meet the kinematic constraints of the tractor and the minimum turning radius limit, which can be used to reduce the burden of back-end optimization and generate and The Reeds-Shepp curve strategy between and Get the Reeds-Shepp curve .

[0172] According to step 4-2-1, there are two situations when semi-random sampling occurs:

[0173] The first case: When the The probability of selecting Upper distance When the nearest point is reached, judge and Will the Reeds-Shepp curve between the two trees collide with an obstacle? If there is no collision with an obstacle, the two trees can be directly connected to stop the search. If there is a collision with an obstacle, a fixed step size is set. , select the curve Up Distance is The point is taken as a child node, denoted as ;

[0174] The second situation: When the When randomly selecting points in the probability space of , select the curve Up Distance is The point is taken as a child node, denoted as ;

[0175] Step 4-2-4: Detection and Whether the connecting line between them collides with an obstacle. If not, the tree expansion is considered successful and step 4-3 is executed; otherwise, the expansion fails and go to step 4-4.

[0176] Step 4-3: Rewiring;

[0177] Set a fixed length ,by Centered on As the radius, put in this spherical neighborhood, and in the tree The set of all nodes on . Traverse All nodes in the first place, determine these nodes and Will the Reeds-Shepp curve between the two vertices collide with the obstacle? Remove all the points that collide with the obstacle, keep the points that do not collide with the obstacle, and then find the points that collide with the obstacle among the remaining nodes. The point with the smallest distance between The parent node of .

[0178] Step 4-4: Expand the tree ;

[0179] Step 4-4-1: Semi-random sampling;

[0180] Pick a random point In order to speed up the convergence of the tree, a semi-random sampling strategy is adopted, that is, Randomly select a point in space. The point information includes three parts: horizontal coordinate, vertical coordinate and orientation angle. Probability selection tree Upper distance tree The nearest point, which can limit the expansion direction of the tree to a certain extent, but without losing the randomness of the expansion;

[0181] Step 4-4-2: Find the tree Random point above The point with the smallest cost is recorded as the closest point ; The minimum cost is the point The Euclidean distance is the smallest;

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

[0183] When connecting two points, the path should be as smooth as possible to meet the kinematic constraints of the tractor and the minimum turning radius limit, which can be used to reduce the burden of back-end optimization and generate and The Reeds-Shepp curve strategy between and Get the Reeds-Shepp curve .

[0184] According to step 4-2-1, there are two situations when semi-random sampling occurs:

[0185] The first case: When the The probability of selecting Upper distance When the nearest point is reached, judge and Will the Reeds-Shepp curve between the two trees collide with an obstacle? If there is no collision with an obstacle, the two trees can be directly connected to stop the search. If there is a collision with an obstacle, a fixed step size is set. , select the curve Up Distance is The point is taken as a child node, denoted as ;

[0186] The second situation: When the When randomly selecting points in the probability space of , select the curve Up Distance is The point is taken as a child node, denoted as ;

[0187] Step 4-4-4: Detection and Whether the connecting line collides with an obstacle. If there is no collision with an obstacle, the tree expansion is considered successful, otherwise the expansion fails.

[0188] Step 4-5: Rewiring;

[0189] Set a fixed length ,by Centered on As the radius, put in this spherical neighborhood, and in the tree The set of all nodes on . Traverse All nodes in the first place, determine these nodes and Will the Reeds-Shepp curve between the two vertices collide with the obstacle? Remove all the points that collide with the obstacle, keep the points that do not collide with the obstacle, and then find the points that collide with the obstacle among the remaining nodes. The point with the smallest distance between The parent node of .

[0190] When two trees can be directly connected in step 4-2-3 or step 4-4-3, stop searching and return a rough path .

[0191] In summary, the bidirectional fast search random tree algorithm based on the Reeds-Shepp curve is used, considering the minimum turning radius constraint of the tractor, which is the center of the rear axle of the tractor Planning a rough path .remember The number of nodes in is , among which The nodes are , whose coordinates are defined as . Figure 7 The process and results of searching the initial reference path using a bidirectional fast search random tree algorithm based on the Reeds-Shepp curve are demonstrated.

[0192] Step 5: Resample the coarse path to obtain the initial reference solution;

[0193] Step 5-1: Estimation of transportation time;

[0194] The path length after resampling the coarse path is defined as the sum of the distances between all adjacent nodes, denoted as , its expression can be written as ,in, Representation interval [ 2 , N P ] For any positive integer in the, the path length obtained is Assuming that the shortest time motion mode is adopted in the path, the acceleration motion during the sampling process is always set to the maximum or minimum acceleration, the acceleration during the sampling process can be expressed as: , , when the system is moving forward, defines the maximum forward speed , the maximum acceleration of the acceleration ,, Maximum acceleration of deceleration ; Otherwise, define the maximum reverse speed , Maximum acceleration for accelerating in reverse , maximum acceleration for deceleration and reverse . Set three limits at the same time , as well as .in, Indicates the maximum acceleration distance; Indicates the maximum deceleration distance; Indicates the sum of the maximum acceleration distance and the maximum deceleration distance.

[0195] At this time The segment path transfer time can be estimated by formula (10). The total transfer time can be expressed as .

[0196] (10)

[0197] In practice, the resampling .

[0198] Step 5-2: Resample at discrete points;

[0199] Divide the time period into equal parts discrete time intervals, thus we get discrete points, then the time interval between every two adjacent discrete points is , then The discrete points correspond approximately to the time .exist At time , the state information of the tractor is recorded as The speed and acceleration are obtained based on the time optimal assumption. As for the steering angle and steering angular velocity of the front wheels of the tractor, they are determined based on the kinematic constraints shown below:

[0200] (11)

[0201] (12)

[0202] in, Indicated in The steering angle of the tractor's front wheels at all times; Indicated in Always keep the tractor towards the angle; Indicated in The tractor's heading angle at the moment; Indicated in The speed of the tractor at the time; Indicated in The steering angular velocity of the tractor at the moment; Indicated in The tractor is facing the angle at the moment.

[0203] The transportation time can be used Initialize, at all discrete points, the state variables and control variables can be Initialize. The resampling path is recorded as .

[0204] Step 6: Generate a safe channel based on the resampling results and simplify the form of obstacle avoidance constraints;

[0205] according to And formula (9), can calculate the center points of the two characteristic circles respectively and The discrete trajectories of and The generation process of the safe transportation corridor corresponding to the center of each characteristic circle is similar. As an example, the construction method of the safe transportation corridor is explained. In the At discrete points ( ), the corresponding position is recorded as ,remember The corresponding safety channel is recorded as .

[0206] First, Initialize to point It can be considered as a rectangle with a width of 0 and a length of 0. Define the set of trial directions =[up, left, down, right]. with a fixed step size , in a circular form Each direction in is explored in a fixed order. The left, right, bottom and top boundaries of the safe channel expanded in the previous iteration are defined as [ x ¯ f, n , x ¯ f, n , y ¯ f, n , y ¯ f, n ] , in a circular form along The data of the safety channel is continuously updated in a fixed order in the four directions. The process of updating the safety channel data is as follows: The expansion of the safety channel is considered valid only when the following two conditions are met, and the safety channel information is updated accordingly: Condition 1. The safety channel must not overlap with the grid map. Any obstacle grid overlaps; Condition 2: The safe passage is in the direction The length of the upper extension does not exceed the preset maximum extension length If any of the above conditions is not met, then the exploration direction set Remove direction This expansion process will continue until the set of exploration directions is empty.

[0207] right The above processing is performed on all points, so that the centers of the two characteristic circles can be The following collision constraint transformation is implemented for each sampling position:

[0208] (13)

[0209] Step 7: "Soften" the nonlinear constraints, establish the final optimal control problem and select the iterative solution strategy;

[0210] Use the transformed constraints in formula (13) to replace the optimal control problem The obstacle avoidance conditions in the equation constitute the following optimal control problem. :

[0211] (14)

[0212] In order to ensure that the optimal control problem of each step of the iteration can be successfully solved during the iteration process, 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), formula (9), and the terminal boundary conditions shown in formula (5), which are merged into the cost function shown in formula (7). Formula (1) is softened as:

[0213] (15)

[0214] in, Indicated in hour, The specific form is to constrain the kinematic equation (1) to Integrate at each moment to get State quantity at the moment , then and Take the difference and find the second norm, then square it, which is called the error term. Sum the error terms.

[0215] Move the left side of the equation in formula (9) to the right side of the equation, and record it as , the softened form is:

[0216] (16)

[0217] Soften formula (5) to:

[0218] (17)

[0219] The above three softened equations are combined into:

[0220] (18)

[0221] This gives the composite cost function

[0222] (19)

[0223] in, is the initial objective function defined by formula (7), It is set to 1 and multiplied by 10 at each iteration to ensure that the penalty for violating the softened nonlinear constraints dominates.

[0224] The final optimal control problem after softening The composition is as follows:

[0225] (20)

[0226] The information obtained by resampling As an initial solution, solve the optimal control problem , and send its solution back to step 5 as the coarse path and improve , until the obtained Less than the allowable error Get the motion trajectory ,like Figure 7 shown.

[0227] exist Figure 8 The transport trajectory from the initial point to the target point is shown in Figures 9 to 13 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 shown in Figure 1. It can be seen that this method can complete the path planning task well.

[0228] The above-described embodiments merely express the implementation methods of the present invention, but they cannot 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 modifications and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention.

Claims

1. A tractor trajectory iterative planning method based on an improved search random tree algorithm, characterized in that: The tractor trajectory iterative planning method comprises the following steps: Step 1: Preliminarily construct the optimal control problem based on the transportation demand and the motion characteristics of the tractor; Step 2: Based on the actual shape of the tractor, two characteristic circles are used to represent the vehicle body to simplify the obstacle avoidance constraints. Step 3, obtaining a grid map based on the actual map and the characteristic circle radius; Step 4, using a bidirectional fast search random tree algorithm based on the Reeds-Shepp curve to obtain a coarse path on the grid map; Step 5, resample the coarse path to obtain an initial reference solution; Step 6: Generate a safe channel based on the resampling result and simplify the form of obstacle avoidance constraints; Step 7, soften the nonlinear constraints, establish the final optimal control problem and select the iterative solution strategy; The step 4 is specifically as follows: Step 4-1: Initialize the tree; To use bidirectional fast search for random numbers, two trees need to be initialized. The two trees expand outward from the starting point and the end point of the transfer, respectively recorded as Tree1 and Tree2; the starting point and the end point are added to Tree1 and Tree2 respectively, and the node information includes position information and orientation angle information; Step 4-2: Expand tree Tree1; Step 4-2-1: Semi-random sampling; Select a random node rand,1 , adopt a semi-random sampling strategy, randomly select a point in space with probability P1, the point information includes three parts: horizontal coordinate, vertical coordinate and orientation angle, and select the point on tree Tree2 that is closest to tree Tree1 with probability (1-P1); Step 4-2-2: Find the node on Tree1 that is closest to the random point rand,1 The point with the smallest cost is recorded as the nearest point node nearest,1 ; Among them, the minimum cost refers to the point node rand,1 The Euclidean distance is the smallest; Step 4-2-3: Generate child nodes; When connecting two points, the path is smooth, satisfying the kinematic constraints of the tractor, and making the path meet the minimum turning radius limit. rand,1 and node nearest,1 The Reeds-Shepp curve strategy between nodes rand,1 and node nearest,1 Get the Reeds-Shepp curve Γ; According to step 4-2-1, there are two situations when semi-random sampling occurs: The first case: When the point on Tree2 closest to Tree1 is selected with a probability of (1-P1), determine node rand,1 and node nearest,1 Will the Reeds-Shepp curve between them collide with an obstacle? If not, the two trees are directly connected and the search is stopped. If it collides with an obstacle, a fixed step size step is set and the node on the curve Γ that is closest to node nearest,1 The point with a distance of step is taken as a child node, denoted as node new,1 ; The second case: When a point is randomly selected in the probability space of P1, a fixed step size step is set and the point on the curve Γ that is away from node nearest,1 The point with a distance of step is taken as a child node, denoted as node new,1 ; Step 4-2-4: Detect node nearest,1 and node new,1 Whether the connecting line collides with an obstacle. If not, the tree expansion is considered successful and step 4-3 is executed; otherwise, the expansion fails and step 4-4 is executed; Step 4-3: Tree1 rewiring; Set a fixed length R rewire , with node new,1 Centered on R rewire A spherical neighborhood is formed with a radius of . In this spherical neighborhood, the set of all nodes on the tree Tree1 is recorded as X rewire,1 ; Traverse X rewire,1 All nodes in the first place, determine these nodes and node new,1 Will the Reeds-Shepp curve between them collide with obstacles? Remove the points that collide with obstacles, keep the points that do not collide with obstacles, and find the points that collide with node in the remaining nodes. new,1 The point with the smallest distance between them is taken as the node new,1 The parent node of Step 4-4: Expand tree Tree2; Step 4-4-1: Semi-random sampling; Select a random node rand,2 , adopt a semi-random sampling strategy, randomly select a point in space with probability P1, the point information includes three parts: horizontal coordinate, vertical coordinate and orientation angle, and select the point on tree Tree1 that is closest to tree Tree2 with probability (1-P1); Step 4-4-2: Find the node on Tree2 that is closest to the random point rand,2 The point with the smallest cost is recorded as the nearest point node nearest,2 ; Step 4-4-3: Generate child nodes; Satisfy the kinematic constraints of the tractor, make the path meet the minimum turning radius limit, and take the generated node rand,2 and node nearest,2 The Reeds-Shepp curve strategy between nodes rand,2 and node nearest,2 Get the Reeds-Shepp curve Γ; According to step 4-2-1, there are two situations when semi-random sampling occurs: The first case: When the point on Tree1 closest to Tree2 is selected with a probability of (1-P1), determine node rand,2 and node nearest,2 Will the Reeds-Shepp curve between them collide with an obstacle? If it does not collide with an obstacle, the two trees are directly connected and the search is stopped. If it collides with an obstacle, a fixed step size step is set and the node on the curve Γ that is closest to node nearest,2 The point with a distance of step is taken as a child node, denoted as node new,2 ; The second case: When a point is randomly selected in the probability space of P1, a fixed step size step is set and the point on the curve Γ that is away from node nearest,2 The point with a distance of step is taken as a child node, denoted as node new,2 ; Step 4-4-4: Detect node nearest,2 and node new,2 Whether the connecting line collides with obstacles. If not, the tree expansion is considered successful, otherwise the expansion fails. Step 4-5: Tree2 rewiring; Set a fixed length R rewire , with node new,2 Centered on R rewire A spherical neighborhood is formed with radius, and the set of all nodes in the spherical neighborhood and on the tree Tree2 is recorded as X rewire,2 ; Traverse X rewire,2 All nodes in the first place, determine these nodes and node new,2 Will the Reeds-Shepp curve between them collide with obstacles? Remove the points that collide with obstacles, keep the points that do not collide with obstacles, and find the points that collide with node in the remaining nodes. new,2 The point with the smallest distance between them is taken as the node new,2 The parent node of When two trees can be directly connected in step 4-2-3 or step 4-4-3, the search stops and a rough path Path is returned. P ; In summary, a bidirectional fast search random tree algorithm based on the Reeds-Shepp curve is used to plan a rough path Path for the rear axle center P(x,y) of the tractor considering the minimum turning radius constraint of the tractor. P ; Remember Path P The number of nodes in is N P , where the kth node is node k (k=1,2,…,N P ), whose coordinates are defined as 2. The tractor trajectory iterative planning method based on the improved search random tree algorithm according to claim 1 is characterized in that: The step 1 is specifically as follows: Step 1-1: Describe the kinematic equations of the tractor; When the tractor is traveling at a low speed, P(x,y) is defined as the center position of the rear axle of the tractor, and the motion model of the tractor is approximately replaced by a bicycle model: Where, φ is the steering angle of the tractor; ω is the steering angular velocity of the tractor; v is the speed of the tractor; a is the acceleration of the tractor; θ is the heading angle of the tractor; L W represents the wheelbase of the tractor; t is the time; T is the terminal time; The state variables of the tractor system are recorded as z(t) = [x(t), y(t), θ(t), v(t), φ(t)] T , the control variable is recorded as u(t)=[a(t),ω(t)] T ; The constraints during the movement of the tractor are as follows: Among them, a min 、a max Respectively represent the minimum and maximum acceleration allowed by the tractor; v min 、v max Respectively represent the minimum and maximum speeds allowed by the tractor; -Ω max ,Ω max Respectively represent the minimum and maximum steering angle speeds allowed by the tractor; -Φ max , Φ max Respectively represent the minimum and maximum steering angles allowed for the tractor; Step 1-2: Describe the task requirements; The transport task is described by boundary conditions, the state of the tractor at the beginning and the desired state of the tractor at the end. For the initial time t = 0, the configuration of the tractor is recorded as: [x(0),y(0),θ(0),v(0),φ(0),a(0),ω(0)]=[x0,y0,θ0,v0,φ0,a0,w0] (3) Among them, x0 represents the horizontal coordinate of the center point of the rear axle of the tractor at the starting time; y0 represents the vertical coordinate of the center point of the rear axle of the tractor at the starting time; θ0 represents the heading angle of the tractor at the starting time; v0 represents the speed of the tractor at the starting time; φ0 represents the steering angle of the tractor at the starting time; a0 represents the acceleration of the tractor at the initial time; w0 represents the steering angular velocity at the initial time; At the terminal time t = T, the configuration of the tractor is recorded as: [x(T),y(T),v(T),φ(T),a(T),ω(T)]=[x F ,y F ,v F ,φ F ,a F ,w F ](4) sinθ(T)=sin(θ F ),cosθ(T)=cos(θ F (5) Among them, x F Represents the horizontal coordinate of the center point of the rear axle of the tractor at the end moment; y F Indicates the ordinate of the center point of the rear axle of the tractor at the end moment; v F Indicates the speed of the tractor at the end moment; φ F Indicates the steering angle of the tractor at the end moment; a F represents the acceleration of the tractor at the end moment; w F The steering angle acceleration at the end moment; θ F Indicates the heading angle of the tractor at the end moment; Step 1-3: Describe obstacle avoidance constraints; The entire configuration space is denoted as Υ, and the function g is used gend Map the state variable z to the configuration space, and record the space where the tractor collides with the obstacle as the obstacle area Υ obs , and the pass area is denoted as Υ free =Υ\Υ obs , then the expression of obstacle avoidance constraint is: Step 1-4: Establish the objective function as follows: Among them, P and Q represent the weight matrices of state quantity and control quantity respectively, w is the weight coefficient, and J is the objective function of the entire optimal control problem; Step 1-5: Establish a preliminary optimal control problem OCP init , as shown below:

3. The tractor trajectory iterative planning method based on the improved search random tree algorithm according to claim 2 is characterized in that: The step 2 is specifically as follows: Define the front suspension length of the tractor as L F , the rear suspension length is recorded as L R , wheelbase is L W , the body width is recorded as L B ; Use two characteristic circles to represent the vehicle body. The centers of the two characteristic circles are located at the two quarter points of the vehicle body, respectively denoted as P f =(x f ,y f ) and P r =(x r ,y r ), we get the following expression: The radius of each characteristic circle is equal and is recorded as R C , the expression is Among them, at time t, x f (t) represents the horizontal coordinate of the front characteristic circle; y f (t) represents the ordinate of the front characteristic circle; x r (t) represents the horizontal coordinate of the rear characteristic circle; y r (t) represents the ordinate of the rear characteristic circle; x(t) represents the abscissa of the center point of the rear axle of the tractor; y(t) represents the ordinate of the center point of the rear axle of the tractor; Shrink the characteristic circle to the center of the circle and move the obstacle through R C To expand.

4. The tractor trajectory iterative planning method based on the improved search random tree algorithm according to claim 3 is characterized in that: The step 3 is specifically as follows: Based on the dilation method described in step 2, the resolution D res Construct a grid map Λ of the aircraft carrier deck; for each grid, if it overlaps with the expanded obstacle area, it is considered as an obstacle grid; otherwise, it is considered as a passable grid; the grid map boundaries in the X and Y directions are [X min ,X max ] and [Y min ,Y max ]; define "grid (i,j)" to represent the following two-dimensional area: {(X,Y)|X min +(i-1)D res <X≤X min +iD res ,Y min +(j-1)D res <Y≤Y min +jD res }; Among them, (i, j) represents the index of the grid; (X, Y) represents the coordinates of the grid respectively.

5. The tractor trajectory iterative planning method based on improved search random tree algorithm according to claim 1 is characterized in that: The step 5 is specifically as follows: Step 5-1: Estimation of transportation time; The length of the path after resampling the coarse path is defined as the sum of the distances between all adjacent nodes, denoted by L path , whose expression is Among them, k represents the interval [2,N P ] is any positive integer; assuming that the accelerated motion in the path always uses the maximum or minimum acceleration to move, the acceleration during the sampling process is expressed as: When the system is moving forward, define the maximum forward speed Maximum acceleration Maximum acceleration while decelerating a =|a min |; Otherwise, define the maximum reverse speed Maximum acceleration when accelerating in reverse Maximum acceleration when decelerating and reversing a =|a max |; Set three limits at the same time and L lim =L lim1 +L lim2 Among them, L lim1 Indicates the maximum acceleration distance; L lim2 Indicates the maximum deceleration distance; L lim It represents the sum of the maximum acceleration distance and the maximum deceleration distance; At this time, the transportation time of the nth segment of the route is calculated by formula (10), and the estimation formula of the total transportation time is expressed as Step 5-2: Resample at discrete points; Divide the time period into N equal parts OCP discrete time intervals, thus we get (N OCP +1) discrete points, then the time interval between two adjacent discrete points is ΔT = t S / N OCP , the nth discrete point corresponds approximately to the time T n =(n+1)ΔT,n=0,1,...N OCP ; in T n At this moment, the state information of the tractor is recorded as (x n ,y n ,v n ,φ n ,θ n ); the speed and acceleration are obtained according to the time optimal assumption, and the steering angle and steering angular velocity of the front wheels of the tractor are determined by the kinematic constraints using the following formula: Among them, φ n Indicates that in T n The steering angle of the front wheels of the tractor at the moment; θ n Indicates that in T n The tractor's heading angle at the moment; θ n+1 Indicates that in T n+1 The tractor's heading angle at the moment v n Indicates that in T n The tractor speed at the time; ω n Indicates that in T n The steering angular velocity of the tractor at the moment; φ n+1 Indicates that in T n+1 The tractor's heading angle at the moment; The transportation time is t S Initialize, at all discrete points, the state variables and control variables can be obtained by (x n ,y n ,v n ,φ n ,θ n ,a n ,ω n ) to initialize; the resampled path is recorded as Path sam .

6. The tractor trajectory iterative planning method based on improved search random tree algorithm according to claim 5 is characterized in that: The step 6 is specifically as follows: According to Path sam And formula (9), respectively calculate the center points P corresponding to the two characteristic circles f =(x f ,y f ) and P r =(x r ,y r ), denoted as Traj f and Traj r ; The generation process of the safe transportation corridor corresponding to the center of each characteristic circle is similar, with P f Explanation: The construction method of the safe transportation corridor; Assume that P f At the nth discrete point (n=0,1,…,N OCP ), the corresponding position is recorded as Note: f,n The corresponding security channel is recorded as SDC f,n ; First, the SDC f,n Initialize to point P f,n itself, and regard it as a rectangle with a width of 0 and a length of 0; define the set of trial directions Δ = [up, left, down, right]; with a fixed step size Δs, try each direction in Δ in a fixed order in a cyclic form; define the left, right, bottom, and top boundaries of the safe channel expanded in the previous iteration process as The data of the safety channel is continuously updated in a fixed order along the four directions in Δ in a cyclic form; the process of updating the safety channel data is as follows: the extension of the safety channel is considered valid only when the following two conditions are met, and the safety channel information is updated accordingly: Condition 1, the safety channel shall not overlap with any obstacle grid in the grid map Λ; Condition 2, the length of the safety channel extended in the direction λ does not exceed the preset maximum extension length L max ; If any of the above conditions is not met, then delete direction λ from the exploration direction set Δ; this expansion process continues until the exploration direction set is empty; Yes (N OCP +1) points are processed as above, and the collision constraint transformation is realized at any nth sampling position for the centers of the two characteristic circles, as shown below:

7. The tractor trajectory iterative planning method based on improved search random tree algorithm according to claim 6 is characterized in that: The step 7 softens the nonlinear constraints, establishes the final optimal control problem and selects an iterative solution strategy; specifically, as follows: Use the transformed constraints in formula (13) to replace the optimal control problem OCP in steps 1-5 init The obstacle avoidance conditions in the equation constitute the following optimal control problem OCP: MID : About OCP MID 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); formula (1) is softened as: Among them, ||f(z(τ),u(τ))|| 2 Indicates that in z I,i+1 When f(z(τ),u(τ)) deviates from 0; the specific form is to constrain the kinematic equation (1) for the i-th (i=1,...N OCP ) moments and integrate to get the state quantity z at moment i+1 I,i+1 , then and z i+1 Take the difference and find the second norm, then square it, call it the error term, and N OCP Sum the error terms; Move the term on the left side of formula (9) to the right side of the equation, and record it as g(z * (τ))=0, the softened form is: Soften formula (5) to: terminal =(sinθ(T)-sin(θ F )) 2 +(cosθ(T)-cos(θ F )) 2 (17) The above three softened equations are combined into: ψ infeasibility =J kinematics +J configuration +J terminal (18) This gives the composite cost function: FINAL =J+w penalty ·ψ infeasibility (19) Where J is the initial objective function defined by formula (7), w penalty is the penalty term coefficient; Softened optimal control problem OCP FIN The composition is as follows: Iterative solution of optimal control problem OCP FIN Generate a new safety channel, update the optimal control problem, end the iteration by setting the error threshold that violates the softened nonlinear constraints, and obtain the motion trajectory Traj FIN .

8. The tractor trajectory iterative planning method based on improved search random tree algorithm according to claim 7 is characterized in that: During the iterative solution process, the termination condition is the ψ obtained during the iteration process. infeasibility Less than the allowable error ε, the error ε range is 10 -4 ~10 -5 .

Images