Space-time joint trajectory planning method for unmanned vehicle in non-structural environment

Abstract

The invention discloses a space-time joint trajectory planning method for an unmanned vehicle in a non-structural environment, which belongs to the technical field of robot systems and comprises a mixed A * sampling step, a cost function design step, a constraint condition design step and a quadratic planning step. In the mixed A * sampling step, an initial planning track of the unmanned vehicle is obtained through a mixed A * algorithm, and sampling is carried out at equal time intervals, so that an initial solution of space-time joint planning of the unmanned vehicle in the unstructured environment is generated; the space-time joint trajectory planning step is used for constructing a space-time joint trajectory planning proposition of the unmanned vehicle in a non-structural environment, and the quadratic programming step is used for performing space-time joint optimization on the trajectory obtained in the mixed A * sampling step by using a quadratic programming solver so as to form a safe, executable and higher-quality unmanned vehicle trajectory.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot systems, and in particular relates to a spatiotemporal joint trajectory planning method for an unmanned vehicle in a non-structured environment. Background Art

[0002] Trajectory planning for driverless vehicles in unstructured environments is a hot topic in the field of robotics, which aims to find a safe and feasible trajectory for driverless vehicles, so that they can accurately avoid obstacles while ensuring efficient and comfortable driving. This requires the driverless vehicle to be able to quickly and accurately perceive various obstacles and factors in the surrounding environment and plan its trajectory based on this information.

[0003] At present, some researchers have proposed planning methods that use search or lateral and longitudinal decoupling. For example, the A* algorithm is used for three-dimensional state space search, but this method cannot guarantee the global optimal solution. For details, see Dolgov, Dmitri, et al. "Practical Search Techniques in Path Planning for Autonomous Driving." Proceedings of the First International Symposium on Search Techniques in Artificial Intelligence and Robotics (STAIR-08), 2008.; or, decoupling a three-dimensional state space planning problem into lateral and longitudinal planning problems greatly improves the solution speed, but the lateral and longitudinal planning method sacrifices the optimality of the planning solution. For details, see Xu, Wenda et al. "A Real-Time Motion Planner with Trajectory Optimization for Autonomous Vehicles." IEEE International Conference on Robotics and Automation, 2012. Based on the above background, the present invention discloses a spatiotemporal joint trajectory planning method for an unmanned vehicle in an unstructured environment. A preliminary search is performed through a hybrid A* algorithm to establish a spatiotemporal joint trajectory planning proposition for an unmanned vehicle in order to find a safer, executable, and higher quality trajectory. Summary of the invention

[0004] The purpose of the present invention is to provide a spatiotemporal joint trajectory planning method for an unmanned vehicle in an unstructured environment, so as to solve the problem that it is difficult to find a global optimal solution using a planning method using search or lateral and longitudinal decoupling in the prior art.

[0005] To achieve the above object, the present invention provides a method for spatiotemporal joint trajectory planning of an unmanned vehicle in an unstructured environment, comprising the following steps: Step 1: Obtain the initial planning trajectory of the unmanned vehicle through the hybrid A* algorithm and sample it at equal time intervals to generate the initial solution of the spatiotemporal joint trajectory planning of the unmanned vehicle in the unstructured environment; Step 2: Construct the cost function of the spatiotemporal joint trajectory planning proposition of the unmanned vehicle in the unstructured environment; Step 3: Construct the constraints of the spatiotemporal joint trajectory planning proposition of the unmanned vehicle in the unstructured environment, including continuity constraint, boundary constraint, kinematic constraint, and security constraint. Step 4: Use a quadratic programming solver to perform spatiotemporal joint optimization on the initial planning trajectory of the driverless car obtained in step 1 to form a trajectory of the driverless car.

[0006] Preferably, in step 1, the initial planning trajectory of the unmanned vehicle is obtained by a hybrid A* algorithm and sampled at equal time intervals to generate an initial solution for the spatiotemporal joint trajectory planning of the unmanned vehicle in an unstructured environment. The specific process is as follows: S21. Design the sampling parameters of hybrid A*, including the kinematic equations of the driverless car, the node expansion method and the heuristic function; S22. Obtaining the initial result of the driverless vehicle trajectory , the specific expression is as follows: ; In the formula, node represents the node obtained by the hybrid A* algorithm. Represents the nodes passing The curves are connected; S23. Define sampling interval ; S24. Obtain the initial solution of the spatiotemporal joint trajectory planning. The specific expression is as follows: ; In the formula, represents the final time of the hybrid A* algorithm planning, Represents the index subscript, represents the number of sampling intervals, Represents the call to the hybrid A* algorithm exist Always planned coordinate, Represents the call to the hybrid A* algorithm exist Always planned coordinate.

[0007] Preferably, the specific expressions of the kinematic equations, node expansion methods and heuristic functions of the driverless vehicle in S21 are as follows: The specific expression of the kinematic equation of the driverless car is as follows: ; In the formula Represents the coordinates of the driverless car in the Cartesian coordinate system, For the speed of the driverless car, is the azimuth of the driverless car, is the front wheel turning angle of the driverless car, Represents the wheelbase of the driverless car; For the node to be expanded, the node expansion method is defined as: exist 、 Under these conditions, up to six types of nodes can be expanded. Represents the maximum front wheel turning angle; The specific expression of the heuristic function is as follows: ; In the formula, represents the heuristic function, represents the shortest path that takes obstacles into account but is not necessarily kinematically feasible. Represents the length of the RS curve considering kinematic constraints but ignoring obstacles.

[0008] Preferably, the specific expression of the cost function cost constructed in step 2 is as follows: ; Where w1, w2, w3 represent the weight coefficients of velocity, acceleration, and jerk in the x-direction, w4, w5, w6 represent the weight coefficients of velocity, acceleration, and jerk in the y-direction, w7, w8 represent the weight coefficients of reference items in the x-direction and y-direction, respectively. , , , Represent the position, velocity, acceleration and jerk in the x direction respectively, , , , Represent the position, velocity, acceleration and jerk in the y direction respectively, i represents the index of the i-th planning point, Represents the total number of planning points.

[0009] Preferably, the specific expression of the continuity constraint constructed in step 3 is as follows: Under the condition that the jerk between sampling points is constant, the continuity constraint is defined as: ; Where i represents the index of the i-th planning point.

[0010] Preferably, the specific expression of the boundary constraint constructed in step 3 is as follows: Under the condition that the jerk between sampling points is constant, the boundary constraint is defined as: ; The trajectory planned by the driverless car needs to satisfy the initial and terminal states, where: , , , , , , , They represent the x-position, y-position, x-speed, y-speed, x-acceleration, y-acceleration, x-jerk and y-jerk of the planning result at the initial moment. , , , , , , , Respectively represent the x-direction position, y-direction position, x-direction velocity, y-direction velocity, x-direction acceleration, y-direction acceleration, x-direction acceleration and y-direction acceleration of the planning result at the end moment , , , , , , , They represent the x-position, y-position, x-velocity, y-velocity, x-acceleration, y-acceleration, x-jerk and y-jerk at the initial moment respectively. , , , , , , , They represent the x-position, y-position, x-velocity, y-velocity, x-acceleration, y-acceleration, x-jerk and y-jerk at the end moment respectively.

[0011] Preferably, the specific expression of the kinematic constraint constructed in step 3 is as follows: Under the condition that the jerk between sampling points is constant, the kinematic constraint is defined as: ; The trajectory planned by the driverless car needs to meet the kinematic constraints of the driverless car, where 、 、 Respectively represent Maximum speed, maximum acceleration, maximum jerk in a direction, 、 、 Respectively represent Maximum velocity, maximum acceleration, and maximum jerk in a direction.

[0012] Preferably, the specific expression of the security constraint constructed in step 3 is as follows: Under the condition that the acceleration between sampling points is constant, the security constraint is defined as: ; The obstacles in the environment are collided with the volume of the unmanned vehicle to ensure that the planned trajectory of the unmanned vehicle will not collide. 、 、 、 Respectively represent Minimum value in direction, The maximum value in the direction, Direction minimum, Maximum value in the direction.

[0013] Preferably, in step 4, the specific process of using a quadratic programming solver to perform spatiotemporal joint optimization on the initial planning trajectory of the unmanned vehicle obtained in step 1 to form the trajectory of the unmanned vehicle is as follows: S41. Establish a spatiotemporal joint trajectory planning proposition, specifically: ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; S42, solving by a quadratic programming solver, the quadratic programming problem is specifically: ; The quadratic programming solver uses the alternating direction multiplier method to decompose the original problem into multiple sub-problems, and finds the solution to the original problem by alternately solving the sub-problems. represents the decision variable, represents the objective function, represents the constrained inequality, Represents the lower bound, Represents the upper bound, represents the quadratic term matrix in the quadratic programming, Represents the matrix of linear terms in a quadratic program.

[0014] Therefore, the present invention adopts the above-mentioned spatiotemporal joint trajectory planning method for an unmanned vehicle in an unstructured environment, which has the following beneficial effects: (1) The method provided by the present invention takes into account the joint planning of unmanned vehicles in the temporal and spatial dimensions, and can better adapt to complex situations in unstructured environments; (2) The present invention can find feasible solutions more quickly and improve planning efficiency through the preliminary search of the hybrid A* algorithm.

[0015] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 It is an overall flow chart of a spatiotemporal joint trajectory planning method for an unmanned vehicle in an unstructured environment according to the present invention; Figure 2 2. It is a schematic diagram of comparison before and after optimization of the present invention; Figure 3 It is a schematic diagram of the trajectory of the driverless car after the spatiotemporal joint optimization of the present invention. DETAILED DESCRIPTION

[0017] The following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0018] See also Figure 1-3 , a spatiotemporal joint trajectory planning method for an unmanned vehicle in an unstructured environment, comprising the following steps: Step 1: Get the initial planning trajectory of the driverless car through the hybrid A* algorithm and sample it at equal time intervals to generate the initial solution of the spatiotemporal joint trajectory planning of the driverless car in the unstructured environment. The specific process is as follows: S21. Design the sampling parameters of hybrid A*, including the kinematic equation of the driverless car, the node expansion method and the heuristic function. The specific expressions are as follows: The specific expression of the kinematic equation of the driverless car is as follows: ; In the formula Represents the coordinates of the driverless car in the Cartesian coordinate system, For the speed of the driverless car, is the azimuth of the driverless car, is the front wheel turning angle of the driverless car, Represents the wheelbase of the driverless car; For the node to be expanded, the node expansion method is defined as: exist 、 Under these conditions, up to six types of nodes can be expanded. Represents the maximum front wheel turning angle; The specific expression of the heuristic function is as follows: ; In the formula, represents the heuristic function, represents the shortest path that takes obstacles into account but is not necessarily kinematically feasible. Represents considering kinematic constraints but ignoring obstacles Length of the curve.

[0019] S22. Obtaining the initial result of the driverless vehicle trajectory , the specific expression is as follows: ; In the formula, Represents the nodes obtained by the hybrid A* algorithm, Represents the nodes passing The curves are connected; S23. Define sampling interval ; S24. Obtain the initial solution of the spatiotemporal joint trajectory planning. The specific expression is as follows: ; In the formula, Represents the final time of the hybrid A* algorithm planning, Represents the index subscript, represents the number of sampling intervals, Represents the call to the hybrid A* algorithm exist Always planned coordinate, Represents the call to the hybrid A* algorithm exist Always planned coordinate.

[0020] Step 2: Construct the cost function of the spatiotemporal joint trajectory planning proposition of the unmanned vehicle in the unstructured environment. The specific expression is as follows: ; Where w1, w2, w3 represent the weight coefficients of velocity, acceleration, and jerk in the x-direction, w4, w5, w6 represent the weight coefficients of velocity, acceleration, and jerk in the y-direction, w7, w8 represent the weight coefficients of reference items in the x-direction and y-direction, respectively. , , , Represent the position, velocity, acceleration and jerk in the x direction respectively, , , , Represent the position, velocity, acceleration and jerk in the y direction respectively, i represents the index of the i-th planning point, Represents the total number of planning points.

[0021] Step 3: Construct the constraints of the spatiotemporal joint trajectory planning proposition of the unmanned vehicle in the unstructured environment; the constraints constructed include continuity constraint, boundary constraint, kinematic constraint, and security constraint. The specific expressions are as follows: Under the condition that the jerk between sampling points is constant, the continuity constraint is defined as: ; In the formula, Indicates The index of a planning point.

[0022] Boundary constraints are defined as: ; The trajectory planned by the driverless car needs to satisfy the initial and terminal states, where: , , , , , , , They represent the x-direction position, y-direction position, x-direction velocity, y-direction velocity, x-direction acceleration, y-direction acceleration, x-direction jerk, and y-direction jerk of the planning result at the initial moment. , , , , , , , Respectively represent the x-direction position, y-direction position, x-direction velocity, y-direction velocity, x-direction acceleration, y-direction acceleration, x-direction acceleration and y-direction acceleration of the planning result at the end moment , , , , , , , They represent the x-position, y-position, x-velocity, y-velocity, x-acceleration, y-acceleration, x-jerk and y-jerk at the initial moment respectively. , , , , , , , Respectively represent the x-direction position, y-direction position, x-direction velocity, y-direction velocity, x-direction acceleration, y-direction acceleration, x-direction jerk and y-direction jerk at the end moment; Kinematic constraint is defined as: ; The trajectory planned by the driverless car needs to meet the kinematic constraints of the driverless car, where 、 、 Represent the maximum velocity, maximum acceleration, and maximum jerk in the x direction, respectively. 、 、 Respectively represent the maximum velocity, maximum acceleration, and maximum jerk in the y direction; Security constraint is defined as: ; The obstacles in the environment are collided with the volume of the unmanned vehicle to ensure that the planned trajectory of the unmanned vehicle will not collide. 、 、 、 They represent the minimum value in the x direction, the maximum value in the x direction, the minimum value in the y direction, and the maximum value in the y direction respectively.

[0023] Step 4: Use the quadratic programming solver to perform spatiotemporal joint optimization on the initial planning trajectory of the driverless car obtained in step 1 to form the trajectory of the driverless car. The specific process is as follows: S41. Establish a spatiotemporal joint trajectory planning proposition, specifically: ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; S42, solving by a quadratic programming solver, the quadratic programming problem is specifically: ; The quadratic programming solver uses the alternating direction multiplier method to decompose the original problem into multiple sub-problems, and finds the solution to the original problem by alternately solving the sub-problems. In the formula, x represents the decision variable, represents the objective function, represents the constrained inequality, Represents the lower bound, represents the upper bound, H represents the quadratic term matrix in the quadratic programming, Represents the matrix of linear terms in a quadratic program.

[0024] Example The following is a simulation verification of a spatiotemporal joint trajectory planning method for an unmanned vehicle in an unstructured environment designed by the present invention.

[0025] Set the map size to 800 × 800 and the hybrid A* grid map resolution to 10. First, use the hybrid A* algorithm to find the initial search solution, and then use the quadratic programming solver to optimize the initial search solution to find a higher quality trajectory. The simulation results are shown in Figure 2. Figure 2 , Figure 3 shown.

[0026] Figure 2 The figure is a comparison diagram of the trajectory before and after optimization. During the optimization process, the trajectory after the quadratic programming process shows higher smoothness and quality compared with the sampling points obtained by the traditional hybrid A* algorithm. This shows that the spatiotemporal joint optimization algorithm can effectively reduce mutations and unnecessary twists in the trajectory, making the driverless car drive more smoothly and safely. Figure 3The schematic diagram of the unmanned vehicle trajectory after the joint optimization of time and space is further depicted. In this optimization strategy, the unmanned vehicle not only considers the spatial path planning, but also integrates the time factor to achieve the simultaneous optimization of driving speed and time.

[0027] Therefore, the present invention adopts the above-mentioned spatiotemporal joint trajectory planning method for unmanned vehicles in unstructured environments, performs preliminary search through the hybrid A* algorithm, establishes a spatiotemporal joint trajectory planning proposition for unmanned vehicles, and searches for a safer, executable, and higher-quality trajectory. Compared with traditional planning methods, the method of the present invention takes into account the joint planning of unmanned vehicles in the spatiotemporal dimensions, and can better adapt to complex situations in unstructured environments. At the same time, through the preliminary search of the hybrid A* algorithm, feasible solutions can be found more quickly, improving planning efficiency.

[0028] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A spatiotemporal joint trajectory planning method for an unmanned vehicle in an unstructured environment, characterized in that: The following steps are involved: Step 1: Obtain the initial planning trajectory of the unmanned vehicle through the hybrid A* algorithm and sample it at equal time intervals to generate the initial solution of the spatiotemporal joint trajectory planning of the unmanned vehicle in the unstructured environment; Step 2: Construct the cost function of the spatiotemporal joint trajectory planning proposition of the unmanned vehicle in the unstructured environment; Step 3: Construct the constraints of the spatiotemporal joint trajectory planning proposition of the unmanned vehicle in the unstructured environment, including continuity constraint, boundary constraint, kinematic constraint, and security constraint. Step 4: Use a quadratic programming solver to perform spatiotemporal joint optimization on the initial planning trajectory of the driverless car obtained in step 1 to form a trajectory of the driverless car.

2. The method for spatiotemporal joint trajectory planning of an unmanned vehicle in an unstructured environment according to claim 1, characterized in that: In step 1, the initial planning trajectory of the unmanned vehicle is obtained by the hybrid A* algorithm and sampled at equal time intervals to generate the initial solution of the spatiotemporal joint trajectory planning of the unmanned vehicle in the unstructured environment. The specific process is as follows: S21. Design the sampling parameters of hybrid A*, including the kinematic equations of the driverless car, the node expansion method and the heuristic function; S22. Obtaining the initial result of the driverless vehicle trajectory , the specific expression is as follows: ； In the formula, node represents the node obtained by the hybrid A* algorithm. Represents the nodes passing The curves are connected; S23. Define sampling interval ; S24. Obtain the initial solution of the spatiotemporal joint trajectory planning. The specific expression is as follows: ； In the formula, represents the final time of the hybrid A* algorithm planning, Represents the index subscript, represents the number of sampling intervals, Represents the call to the hybrid A* algorithm exist Always planned coordinate, Represents the call to the hybrid A* algorithm exist Always planned coordinate.

3. The method for spatiotemporal joint trajectory planning of an unmanned vehicle in an unstructured environment according to claim 2, characterized in that: The specific expressions of the kinematic equations, node expansion methods and heuristic functions of the driverless car in S21 are as follows: The specific expression of the kinematic equation of the driverless car is as follows: ； In the formula Represents the coordinates of the driverless car in the Cartesian coordinate system, For the speed of the driverless car, is the azimuth of the driverless car, is the front wheel turning angle of the driverless car, Represents the wheelbase of the driverless car; For the node to be expanded, the node expansion method is defined as: exist 、 Under these conditions, up to six types of nodes can be expanded. Represents the maximum front wheel turning angle; The specific expression of the heuristic function is as follows: ； In the formula, represents the heuristic function, represents the shortest path that takes obstacles into account but is not necessarily kinematically feasible. Represents the length of the RS curve considering kinematic constraints but ignoring obstacles.

4. The method for spatiotemporal joint trajectory planning of an unmanned vehicle in an unstructured environment according to claim 3, characterized in that: The specific expression of the cost function cost constructed in step 2 is as follows: ； Where w1, w2, w3 represent the weight coefficients of velocity, acceleration, and jerk in the x-direction, w4, w5, w6 represent the weight coefficients of velocity, acceleration, and jerk in the y-direction, w7, w8 represent the weight coefficients of reference items in the x-direction and y-direction, respectively. , , , Represent the position, velocity, acceleration and jerk in the x direction respectively, , , , Represent the position, velocity, acceleration and jerk in the y direction respectively, i represents the index of the i-th planning point, Represents the total number of planning points.

5. The method for spatiotemporal joint trajectory planning of an unmanned vehicle in an unstructured environment according to claim 4, characterized in that: The specific expression of the continuity constraint constructed in step 3 is as follows: Under the condition that the jerk between sampling points is constant, the continuity constraint is defined as: ； Where i represents the index of the i-th planning point.

6. The method for spatiotemporal joint trajectory planning of an unmanned vehicle in an unstructured environment according to claim 5, characterized in that: The specific expression of the boundary constraint constructed in step 3 is as follows: Under the condition that the jerk between sampling points is constant, the boundary constraint is defined as: ； The trajectory planned by the driverless car needs to satisfy the initial and terminal states, where: , , , , , , , They represent the x position, y position, x velocity, y velocity, x acceleration, y acceleration, x jerk and y jerk of the planning result at the initial moment respectively. , , , , , , , Respectively represent the x-position, y-position, x-velocity, y-velocity, x-acceleration, y-acceleration, x-jerk, and y-jerk of the planning result at the end moment , , , , , , , They represent the x-position, y-position, x-velocity, y-velocity, x-acceleration, y-acceleration, x-jerk and y-jerk at the initial moment respectively. They represent the x-position, y-position, x-velocity, y-velocity, x-acceleration, y-acceleration, x-jerk and y-jerk at the end moment respectively.

7. The method for spatiotemporal joint trajectory planning of an unmanned vehicle in an unstructured environment according to claim 6, characterized in that: The specific expression of the kinematic constraint constructed in step 3 is as follows: Under the condition of constant acceleration between sampling points, the kinematic constraint is defined as: ； The trajectory planned by the driverless car needs to meet the kinematic constraints of the driverless car, where 、 、 They represent the maximum velocity, maximum acceleration, and maximum jerk in the x-direction, respectively. 、 、 They represent the maximum velocity, maximum acceleration, and maximum jerk in the y-direction respectively.

8. The method for spatiotemporal joint trajectory planning of an unmanned vehicle in an unstructured environment according to claim 7, characterized in that: The specific expression of the security constraint constructed in step 3 is as follows: Under the condition that the jerk between sampling points is constant, the security constraint is defined as: ； The obstacles in the environment are collided with the volume of the unmanned vehicle to ensure that the planned trajectory of the unmanned vehicle will not collide. , , , They represent the minimum value in the x direction, the maximum value in the x direction, the minimum value in the y direction, and the maximum value in the y direction respectively.

9. The method for spatiotemporal joint trajectory planning of an unmanned vehicle in an unstructured environment according to claim 8, characterized in that: In step 4, the quadratic programming solver is used to perform spatiotemporal joint optimization on the initial planning trajectory of the driverless car obtained in step 1. The specific process of forming the trajectory of the driverless car is as follows: S41. Establish a spatiotemporal joint trajectory planning proposition, specifically: ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； ； S42, solving by a quadratic programming solver, the quadratic programming problem is specifically: ； The quadratic programming solver uses the alternating direction multiplier method to decompose the original problem into multiple sub-problems, and finds the solution to the original problem by alternately solving the sub-problems. In the formula, x represents the decision variable, represents the objective function, represents the constrained inequality, Represents the lower bound, represents the upper bound, H represents the quadratic term matrix in the quadratic programming, Represents the matrix of linear terms in a quadratic program.

Images