Tractor track iterative planning method based on improved search random tree algorithm

By applying the two-way fast search random tree algorithm and iterative solution strategy of Reeds-Shepp curve on the aircraft carrier deck, the efficiency and optimization problems of tractor trajectory planning in complex environments are solved, high-quality trajectories are generated, and the success of path planning is improved. rate and optimality.

CN119066986AActive Publication Date: 2024-12-03DALIAN UNIV OF TECH

Patent Information

Application Number
CN202411554132.9
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-11-04
Publication Date
2024-12-03
Estimated Expiration
2044-11-04

AI Technical Summary

Technical Problem

The complex environment and dense obstacles on the aircraft carrier deck make it difficult for existing path planning methods to generate efficient and optimized tractor trajectories, especially when the initial reference path quality is not high, affecting the success rate and optimization of path planning.

Method used

Using a bidirectional fast search random tree algorithm based on Reeds-Shepp curve, through a multi-stage planning strategy, first searching for a thick path that satisfies the maximum turning radius constraint, resampling to obtain the initial reference solution, and using the iterative solution strategy to use the nonlinear constraint as a penalty term Add objective functions to simplify obstacle avoidance constraints and generate safe channels to improve the efficiency of path planning.

Benefits of technology

Effectively generate high-quality tractor trajectories in complex environments, improving the success rate and optimization of path planning, reducing the impact of the initial rough path on the final trajectory, and ensuring the smoothness and safety of the trajectory.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119066986A_ABST
    Figure CN119066986A_ABST
Patent Text Reader

Abstract

The invention provides a tractor track iterative planning method based on an improved search random tree algorithm, and belongs to the field of equipment intelligent guarantee. The method comprises the following steps: firstly, preliminarily constructing an optimal control problem according to an allocation demand and a motor tractor motion characteristic; 2, representing a tractor body by using a characteristic circle according to the actual shape of the tractor, and equivalently converting an obstacle avoidance problem; and thirdly, obtaining a grid map according to the actual map and the radius of the characteristic circle. Fourthly, a bidirectional fast search random tree algorithm based on a Reeds-Shepp curve is adopted to obtain a coarse path; fifthly, re-sampling the rough path to obtain an initial reference solution; sixthly, constructing a safety channel, and simplifying the form of obstacle avoidance constraint conditions; and 7, based on the obstacle avoidance constraint and the motion constraint of the tractor, softening a nonlinear term constraint condition, establishing an optimal control problem, and performing iterative solution. According to the method, the secure channel can be continuously updated, the influence of the initial guess solution on the final solution result is weakened, and a planned path with good quality is obtained.
Need to check novelty before this filing date? Find Prior Art

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 , 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 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 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 tractor speed 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 planning process speed.

[0116] Fig.10 Steering angle change diagram 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 steering angular velocity changes 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 , 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 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 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 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 tractor speed 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] Formula (5) is softened 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.

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, the definition The center position of the rear axle of the tractor is recorded as, and the motion model of the tractor is approximately replaced by a bicycle model: (1) 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; For time; is the end time; The state variables of the tractor system are recorded as , the control variable is recorded as ; The constraints during the movement of the tractor are as follows: (2) in, , They represent the minimum and maximum acceleration allowed for the tractor respectively; , 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; Step 1-2: Describe the task requirements; The transportation task is described by boundary conditions, 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: (3) 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; At the terminal moment , the configuration of the tractor is recorded as: (4) (5) 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; Step 1-3: Describe obstacle avoidance constraints; 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 , and the passage area is recorded as , then the obstacle avoidance constraint is expressed as: (6) Step 1-4: Establish the objective function as follows: (7) 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; Step 1-5: Set up a preliminary optimal control problem , as shown below: (8)。 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 , the rear suspension length is recorded as , the wheelbase is , the body width is recorded as ; 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 , we get the following expression: (9) 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; Shrink the characteristic circle to the center of the circle and move the obstacle through 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 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 and ; define "grid ” represents the following two-dimensional region: ;in, Represents the index of the raster; Represent the coordinates of the grid respectively.

5. The tractor trajectory iterative planning method based on improved search random tree algorithm according to claim 4 is characterized in that: 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, and are recorded as and ; Add the starting point and end point to and ,Node information includes location information and orientation angle information; Step 4-2: Expand the tree ; Step 4-2-1: Semi-random sampling; Pick a random point , adopting a semi-random sampling strategy with 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 nearest point; Step 4-2-2: Find the tree Random point above The point with the smallest cost is recorded as the closest point ; Among them, the minimum cost refers to the point The Euclidean distance is the smallest; Step 4-2-3: Generate child nodes; When connecting two points, the path is smooth, meets the kinematic constraints of the tractor, and satisfies the minimum turning radius limit. and The Reeds-Shepp curve strategy between and 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 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 are directly connected and the search stops; 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 ; 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 ; Step 4-2-4: Detection and 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: Rewiring; Set a fixed length ,by Centered on A spherical neighborhood is formed with a radius of The set of all nodes on ; Traverse All nodes in the Will the Reeds-Shepp curve between the two points collide with the obstacle? Remove the points that collide with the obstacle, keep the points that do not collide with the obstacle, and find the points that collide with the obstacle in the remaining nodes. The point with the smallest distance between The parent node of Step 4-4: Expand the tree ; Step 4-4-1: Semi-random sampling; Pick a random point , adopting a semi-random sampling strategy with 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 nearest point; Step 4-4-2: Find the tree Random point above The point with the smallest cost is recorded as the closest point ; Step 4-4-3: Generate child nodes; Satisfy the kinematic constraints of the tractor and make the path meet the minimum turning radius limit. and The Reeds-Shepp curve strategy between and 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 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 are directly connected and the search is stopped; 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 ; 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 ; Step 4-4-4: Detection and Whether the connecting line collides with obstacles. If not, the tree expansion is considered successful, otherwise the expansion fails. Step 4-5: Rewiring; Set a fixed length ,by Centered on A spherical neighborhood is formed for the radius, within the spherical neighborhood, and in the tree The set of all nodes on ; Traverse All nodes in the Will the Reeds-Shepp curve between the two points collide with the obstacle? Remove the points that collide with the obstacle, keep the points that do not collide with the obstacle, and find the points that collide with the obstacle in the remaining nodes. The point with the smallest distance between The parent node of When two trees can be directly connected in step 4-2-3 or step 4-4-3, stop searching and return a rough path ; 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 .

6. The tractor trajectory iterative planning method based on improved search random tree algorithm according to claim 5 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 as , whose expression is ,in, Representation interval Any positive integer in ; Assuming that the acceleration 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, 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; It represents the sum of the maximum acceleration distance and the maximum deceleration distance; At this time The transport time of the segment path is calculated by formula (10), and the estimation formula of the total transport time is expressed as ; (10) Step 5-2: Resample at discrete points; 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 , No. 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 optimal time 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: (11) (12) 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 tractor speed at the time; Indicated in The steering angular velocity of the tractor at the moment; Indicated in The tractor's heading angle at the moment; Transportation time is adopted Initialize, at all discrete points, the state variables and control variables can be Initialize; record the resampled path as .

7. The tractor trajectory iterative planning method based on improved search random tree algorithm according to claim 6 is characterized in that: The step 6 is specifically as follows: according to And formula (9), respectively calculate the centers of the two characteristic circles and The discrete trajectories of and ; The generation process of the safe transportation corridor corresponding to the center of each characteristic circle is similar. Explain: How to construct a safe transportation corridor; Assumptions In the At discrete points ( ), the corresponding position is recorded as ,remember The corresponding safety channel is recorded as ; First, Initialize to point itself, treating it as a rectangle with a width of 0 and a length of 0; defining a 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 , in a circular form along The four directions in the middle are expanded in a fixed order to continuously update the data of the safe channel; the process of updating the safe channel data is as follows: The expansion of the safe channel is considered valid only when the following two conditions are met, and the safe channel information is updated accordingly: Condition 1, the safe channel must not overlap with the grid map Any obstacle grids in the grid overlap; 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 set Remove direction ; This expansion process continues until the set of exploration directions is empty; right The above processing is performed on all points. For the centers of the two characteristic circles, at any The collision constraint conversion is implemented at each sampling position as follows: (13)。 8. The tractor trajectory iterative planning method based on improved search random tree algorithm according to claim 7 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 in steps 1-5 The obstacle avoidance conditions in the equation constitute the following optimal control problem. : (14) right 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: (15) 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; 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: (16) Soften formula (5) to: (17) The above three softened equations are combined into: (18) This gives the composite cost function: (19) in, is the initial objective function defined by formula (7), is the penalty term coefficient; Optimal control problem after softening The composition is as follows: (20) Iterative solution of optimal control problem 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 .

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

Citation Information

Patent Citations

  • Mobile robot path planning system and method based on deep reinforcement learning algorithm

    CN118409584A

  • Route planning system and method for collaborative operation of special vehicles in dynamic environment

    CN118655885A

  • Motion planning and controlling method for tractor-trailer mobile robot in complex environment

    CN103941737A

  • Automatic parking path planning method based on vehicle four-wheel steering

    CN115903828A

  • Method for planning mooring trajectory of surface ship based on safe navigation channel

    CN116382260A

Cited By

  • Robot path planning method based on RRT improved algorithm

    CN119321769A

  • Shipborne special vehicle dispatching trajectory planning method considering driving habits

    CN120313632A

  • A shipborne special vehicle transportation trajectory planning method considering driving habits

    CN120313632B