An autonomous valet parking trajectory planning method for complex unstructured scenes
By using the concept of a driving corridor and a hybrid A* algorithm to generate coarse trajectories in complex unstructured scenarios, and then optimizing the trajectories using a nonlinear optimization solver, the inefficient autonomous valet parking trajectory planning problem in existing technologies is solved, achieving fast and accurate trajectory generation.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-04-03
- Publication Date
- 2026-03-31
AI Technical Summary
In complex, unstructured scenarios, existing autonomous valet parking trajectory planning methods are inefficient, prone to failure, and struggle to generate fast, accurate, and optimal trajectories, thus failing to meet real-time requirements.
The concept of driving corridor is used to determine the corridor type of discrete points. A coarse trajectory is generated by combining the traditional A* algorithm and the hybrid A* algorithm. The trajectory is then optimized by the nonlinear optimization solver IPOPT. The vehicle kinematic constraint equations are constructed to achieve fast and accurate trajectory planning.
It improves the accuracy and efficiency of trajectory generation in complex unstructured scenarios, addresses the accuracy issues of traditional methods, ensures the smooth progress of subsequent optimization processes, and generates the optimal trajectory.
Smart Images
Figure CN116466708B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of autonomous driving, and in particular to a method for autonomous valet parking trajectory planning in complex unstructured scenarios. Background Technology
[0002] Autonomous valet parking systems are crucial for developing safe, environmentally friendly, comfortable, and convenient intelligent mobility service systems. Typically, these systems mainly include modules such as perception, localization, prediction, planning, and control. Trajectory planning, as one of the core modules, aims to generate safe and comfortable trajectories for autonomous vehicles to complete driving tasks. Safety refers to avoiding collisions, and comfort refers to avoiding sudden changes in speed or acceleration. Appropriate trajectory planning methods play a vital role in improving the intelligence level of vehicles.
[0003] In real-world traffic environments, compared to structured roads, typical unstructured parking scenarios often lack reference lines to guide vehicles, requiring multiple maneuvers to park. Parking is even more challenging in complex environments with narrow passages formed by stationary vehicles and other irregular obstacles. Current autonomous parking trajectory planning primarily combines search and optimization methods. Search-based methods first generate a coarse initial solution on a global map, while optimization-based methods use this initial solution to accelerate the optimization process. However, current search-based methods are inefficient in complex environments with narrow passages, and may even fail, potentially causing subsequent optimization problems and undermining the real-time performance requirements of parking algorithms. Therefore, achieving efficient, high-quality, and collision-avoidant initial solution search in complex unstructured scenarios to accelerate numerical optimization and generate fast, accurate, and optimal trajectories is a pressing issue. Summary of the Invention
[0004] To address the shortcomings and deficiencies of existing technologies, this invention proposes an autonomous valet parking trajectory planning method for complex unstructured scenarios, achieving fast, accurate, and optimal trajectory generation.
[0005] To achieve the above objectives, the technical solution of the present invention is as follows:
[0006] An autonomous valet parking trajectory planning method for complex unstructured scenarios includes the following steps:
[0007] A. Initialize vehicle physical parameters and determine initial and target parking positions.
[0008] The vehicle shape is represented by a rectangle, and the geometric parameters related to the vehicle itself are initialized, including the wheelbase L. m Front overhang length L fRear overhang length L r Determine the vehicle's width W; determine the vehicle's initial position P. s and target location P f P s Including the x and y coordinates of the vehicle's rear axle center in the global map coordinate system. s and y s And the angle θ between the vehicle's forward direction and the positive x-axis. s ;P f Including the x and y coordinates of the vehicle's rear axle center in the global map coordinate system. f and y f and the angle θ between the vehicle's forward direction and the positive x-axis. f .
[0009] B. Construct an obstacle expansion map for the parking scene.
[0010] Using N c A rectangular vehicle body is covered by several disks of the same radius, and the center coordinates x of each disk are obtained. j (t),y j (t) and disk radius R d The specific calculation is shown in the following formula.
[0011]
[0012]
[0013]
[0014] In the formula, x(t) and y(t) represent the coordinates of the rear axle center of the vehicle at time t, where x... j (t), y j (t) represents the center coordinates of the j-th disk at time t.
[0015] Next, a map containing several stationary obstacles is obtained, and each disk in the map is shrunk to its central point, while each stationary obstacle is expanded radially by R. d Width, obtain the expanded map of obstacles in the parking scene.
[0016] C. Searching for a coarse trajectory
[0017] C1. Based on vehicle physical parameters, initial position, target position, and obstacle expansion map, the obstacle expansion map is rasterized to obtain a raster map. Then, the traditional A* algorithm is used on the raster map to quickly obtain a two-dimensional path discrete point X connecting the initial position and the target position. AStar .
[0018] C2, Based on the two-dimensional path discrete point X in step C1 AStarLay out a rectangular driveway and obtain the lengths of the four sides of the i-th rectangle. and For each rectangle, determine whether it has a wide or narrow corridor. If any side length is less than a threshold L, then... thre If the value is positive, it is determined to be a narrow corridor; otherwise, it is determined to be a wide corridor. When all corridors formed based on a continuous discrete point segment are either wide or narrow corridors, these continuous discrete points are regarded as a set of points, thereby obtaining a total of d discrete point segments in the complex obstacle scene.
[0019] C3. Based on the multiple discrete point sets in step C2, obtain the boundary point coordinate information χ for each set. bou , χ bou Including the x and y coordinates p of the vehicle's rear axle center in the global map coordinate system. x ,p y And initialize the direction angles at both ends of each set. Obtain several boundary points containing position and orientation angle information. Including the x and y coordinates p of the vehicle's rear axle center in the global map coordinate system. x And py, and the angle between the vehicle's forward direction and the positive x-axis.
[0020]
[0021] In the formula, arctan(·) is the arctangent function. Let x and y be the coordinates of the boundary point at the end of the d-th segment of the set in the global coordinate system. Let x and y represent the x and y coordinates of the discrete point preceding the boundary point of the d-th segment of the set in the global coordinate system.
[0022] C4. Boundary points generated based on step C3 A coarse trajectory x0 conforming to the vehicle's kinematics is quickly searched and obtained between each adjacent boundary point using a hybrid A* algorithm. Then, along the coarse trajectory x0, speed-related decision variables are appended using speed planning to obtain a coarse trajectory v0 containing complete position and speed information. The coarse trajectory x0 includes the x and y coordinates of the vehicle's rear axle center in the global map coordinate system. and and the angle θ between the vehicle's forward direction and the positive x-axis. 0 Three decision variables related to location information. The decision variables related to velocity information include velocity v. 0 Front wheel steering angle acceleration a 0 angular velocity w 0 and parking time
[0023] D. Set optimization goals and construct optimization problems.
[0024] The following criteria are used to optimize the parking trajectory planning task from start to finish:
[0025] minJ(u(t),x(t),t f )
[0026]
[0027] In the formula, J(·) represents the objective function to be minimized, x(t) and u(t) are the state variable and control variable of the vehicle at time t, respectively. start ,u start Let x(t) and u(t) represent the initial states of x(t) and u(t), respectively. final ,u final Let x(t) and u(t) represent the termination states of x(t) and u(t), respectively; x max ,u max Let x(t) and u(t) be the supremum and the supremum of x(t) and u(t), respectively. min ,u min Let f denote the infimum of x(t) and u(t) respectively. kinematics (·) is the vehicle kinematic constraint equation. and These represent the map space, obstacle space, and free space for vehicle movement, respectively.
[0028] E. Use the nonlinear optimization solver IPOPT to solve the optimization problem and obtain the optimal trajectory.
[0029] Furthermore, in step B, N c =2-4.
[0030] Compared with the prior art, the present invention has the following beneficial effects:
[0031] 1. This invention uses the concept of a driving corridor to determine the width (narrowness) corridor to which discrete points belong, which can improve the rationality and accuracy of boundary point selection.
[0032] 2. This invention is used for autonomous valet parking trajectory planning in complex unstructured scenarios. It can generate a fast, accurate and optimal trajectory in complex environments formed by obstacles of different shapes. Compared with traditional search-based methods, it can effectively improve the problems of low accuracy and easy failure, and more efficiently search for a high-quality coarse trajectory for the subsequent optimization process. Attached Figure Description
[0033] Figure 1 This is a flowchart illustrating the present invention.
[0034] Figure 2 This is a schematic diagram of the vehicle's kinematic geometry.
[0035] Figure 3 This is a schematic diagram of the disk contracting and the obstacle expanding.
[0036] Figure 4 This is a schematic diagram for quickly obtaining discrete points at the initial and target locations of a connection.
[0037] Figure 5 This is a schematic diagram of a driving corridor laid out along discrete points.
[0038] Figure 6 It is a schematic diagram of boundary points obtained for different discrete point sets.
[0039] Figure 7 This is a schematic diagram of the initial rough trajectory obtained between boundary points. Detailed Implementation
[0040] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings. Figure 1 As shown, an autonomous valet parking trajectory planning method suitable for complex unstructured scenarios includes the following steps:
[0041] A. Initialize vehicle physical parameters and determine initial and target parking positions.
[0042] The vehicle shape is represented by a rectangle, and the geometric parameters related to the vehicle itself are initialized, including the wheelbase L. m Front overhang length L f Rear overhang length L r And the width of the car, W, such as Figure 2 As shown; determine the initial position P of the vehicle. s and target location P f P s Including the x and y coordinates of the vehicle's rear axle center in the global map coordinate system. s ,y s And the angle θ between the vehicle's forward direction and the positive x-axis. s ;P f Including the x and y coordinates of the vehicle's rear axle center in the global map coordinate system. f ,y f And the angle θ between the vehicle's forward direction and the positive x-axis. f .
[0043] B. Construct an obstacle expansion map for the parking scene.
[0044] Using N c A rectangular car body is covered by a circular disc of the same radius, such as Figure 2 As shown, the center coordinates x of the disk are obtained respectively.j (t),y j (t) and disk radius R d The specific calculation is shown in the following formula.
[0045]
[0046]
[0047]
[0048] In the formula, x(t) and y(t) represent the coordinates of the rear axle center of the vehicle at time t, x j (t),y j (t) represents the center coordinates of the j-th disk at time t.
[0049] Next, a global map containing several stationary obstacles is obtained, and each disk in the map is shrunk to its central point, while the different stationary obstacles expand radially by R. d Width. In this embodiment of the invention, N c The value is set to 2, and a 40m*40m obstacle expansion map of the parking scene is obtained, such as... Figure 3 As shown.
[0050] C. Searching for a coarse trajectory
[0051] C1. Based on vehicle physical parameters, initial position, target position, and obstacle expansion map, the obstacle expansion map is rasterized to obtain a raster map. Then, the traditional A* algorithm is used on the raster map to quickly obtain a two-dimensional path discrete point X connecting the initial and target positions. AStar ,like Figure 4 As shown.
[0052] C2, Based on the two-dimensional path discrete point X in step C1 AStar Lay a rectangular carriageway, such as Figure 5 As shown, the lengths of the four sides of the i-th rectangle are obtained respectively. For each rectangle, determine whether it has a wide or narrow corridor. If any side length is less than a threshold L, then... thre If the value is positive, it is determined to be a narrow corridor; otherwise, it is determined to be a wide corridor. When all corridors formed based on a continuous discrete point segment are either wide or narrow corridors, these continuous discrete points are regarded as a set of points, thereby obtaining a total of d discrete point segments in the complex obstacle scene.
[0053] C3. Based on the multiple discrete point sets in step C2, obtain the boundary point coordinate information χ for each set. bou ,like Figure 6 As shown, χ bouIncluding the x and y coordinates p of the vehicle's rear axle center in the global map coordinate system. x ,p y And initialize the direction angles at both ends of each set. Obtain several boundary points containing position and orientation angle information. Specifically, this includes the x and y coordinates of the vehicle's rear axle center in the global map coordinate system. x ,p y and the angle between the vehicle's forward direction and the positive x-axis.
[0054]
[0055] In the formula, arctan(·) is the arctangent function. Let x and y be the coordinates of the boundary point at the end of the d-th segment of the set in the global coordinate system. Let x and y represent the x and y coordinates of the discrete point preceding the boundary point of the d-th segment of the set in the global coordinate system.
[0056] C4. Boundary points generated based on step C3 A coarse path X0 conforming to the vehicle's kinematics characteristics is quickly searched and obtained between each adjacent boundary point using a hybrid A* algorithm, such as... Figure 7 As shown, X0 specifically includes the x and y coordinates of the vehicle's rear axle center in the global map coordinate system. and the angle θ between the vehicle's forward direction and the positive x-axis. 0 In addition to the decision variables included in χ0. and θ 0 Furthermore, the decision variable D0 needs to be initialized along χ0, where D0 specifically includes the velocity v. 0 Front wheel steering angle acceleration a 0 angular velocity w 0 and parking time This yields a line containing and the initial rough trajectory of the D0 decision variable
[0057] D. Set optimization goals and construct optimization problems.
[0058] The parking trajectory planning task from start to finish can be described using the following standard optimization problem:
[0059]
[0060] In the formula, J(·) represents the objective function to be minimized, x(t) and u(t) are the state variable and control variable of the vehicle at time t, respectively. start ,u startLet x(t) and u(t) represent the initial states of x(t) and u(t), respectively. final ,u final Let x(t) and u(t) represent the termination states of x(t) and u(t), respectively; x max ,u max Let x(t) and u(t) be the supremum and the supremum of x(t) and u(t), respectively. min ,u min Let f denote the infimum of x(t) and u(t) respectively. kinematics (·) is the vehicle kinematic constraint equation. and These represent the map space, obstacle space, and free space for vehicle movement, respectively.
[0061] E. Solve the optimization problem using the nonlinear optimization solver IPOPT to obtain the optimal trajectory, such as... Figure 7 As shown.
[0062] The basic principles, main features, and advantages of this invention have been described above. Those skilled in the art should understand that this invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of this invention. Various changes and modifications can be made to this invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the invention as claimed.
Claims
1. A method for autonomous valet parking trajectory planning for complex unstructured scenarios, characterized in that: The steps include: A. Initialize vehicle physical parameters and determine the initial and target positions of parking The vehicle shape is represented by a rectangle, and the geometry parameters related to the vehicle itself are initialized, including wheelbase L m , front suspension length L f , rear suspension length L r , and vehicle width W; the initial position P s and target position P f of the vehicle are determined, P s including the x and y axis coordinates x s and y s of the center of the rear axle of the vehicle in the global coordinate system of the map, and the angle θ s between the forward direction of the vehicle and the positive direction of the x axis; P f including the x and y axis coordinates x f and y f of the center of the rear axle of the vehicle in the global coordinate system of the map, and the angle θ f between the forward direction of the vehicle and the positive direction of the x axis; B. Construct the parking scene obstacle inflation map N c discs with the same radius are used to cover the rectangular body, and the center coordinates x j (t), y j (t) and the disc radius R d are obtained respectively, and the specific calculation is shown in the following formula; In the formula, x(t), y(t) represent the coordinates of the center of the rear axle of the vehicle at time t, x j (t), y j (t) represent the coordinates of the center of the jth disc at time t. Then a map with several static obstacles is acquired, and each disc is contracted to the center particle in the map, and each static obstacle is expanded radially by R d Obtain the obstacle inflation map of the parking scene by expanding the width C. Search for a rough trajectory C1, based on the vehicle physical parameters, the initial position, the target position and the obstacle inflation map, rasterizing the obstacle inflation map to obtain a grid map, and using a traditional A* algorithm to quickly obtain a two-dimensional path discrete point X connecting the initial position and the target position on the grid map ASta r; C2, based on the two-dimensional path discrete points X in the C1 step AStar Laying a rectangular driving corridor, and obtaining the length of the four sides of the ith rectangle respectively and Determining the wide corridor or narrow corridor for each rectangle, when any one of the side lengths is less than the threshold value L thre , it is determined as a narrow corridor; otherwise, it is determined as a wide corridor; when the corridors formed based on a segment of continuous discrete points are all wide corridors or narrow corridors, the continuous discrete points are regarded as a point set, thereby obtaining a total of d discrete point sets in a complex obstacle scene; C3, based on the plurality of discrete point sets in the C2 step, obtaining boundary point coordinate position information of each set including the x and y axis coordinates p of the center of the rear axle of the vehicle in the global coordinate system of the map x ,p y and initializing the direction angle of each segment set obtaining a plurality of boundary points containing position and direction angle information including the x and y axis coordinates p of the center of the rear axle of the vehicle in the global coordinate system of the map x and p y and the angle between the forward direction of the vehicle and the positive direction of the x axis where arctan(·) is the inverse tangent function, respectively represent the x and y axis coordinates of the end boundary point of the dth segment set in the global coordinate system, respectively represent the x and y axis coordinates of the previous discrete point of the end boundary point of the dth segment set in the global coordinate system; C4, boundary points generated based on C3 step A rough trajectory that meets the kinematic characteristics of the vehicle is quickly searched and obtained between each adjacent boundary point using the hybrid A* algorithm And along the rough trajectory By adding decision variables related to speed information through speed planning, a rough trajectory containing complete position and speed information is obtained The rough trajectory Including the x and y axis coordinates of the center of the rear axle of the vehicle in the global coordinate system of the map And And the angle θ between the forward direction of the vehicle and the positive direction of the x axis 0 Three decision variables related to position information; the decision variables related to speed information include speed v 0 , front wheel steering angle Acceleration a 0 , angular velocity w 0 And parking time D. Set the optimization target and construct the optimization problem The parking trajectory planning task from the start to the end state is described by using the following standard optimization problem description: min J(u(t),x(t),t f ) In the formula, J(·) represents the objective function to be minimized, x(t) and u(t) are the state variable and control variable of the vehicle at time t, respectively. start ,u start Let x(t) and u(t) represent the initial states of x(t) and u(t), respectively. final ,u final Let x(t) and u(t) represent the termination states of x(t) and u(t), respectively; x max ,u max Let x(t) and u(t) be the supremum and the supremum of x(t) and u(t), respectively. min ,u min Let f denote the infimum of x(t) and u(t) respectively. kinematics (·) is the vehicle kinematic constraint equation. and These represent the map space, obstacle space, and free space for vehicle movement, respectively. E. Solve the optimization problem by using the nonlinear optimization solver IPOPT to obtain the optimal trajectory. 2.The method of claim 1, wherein: In step B, N c = 2-4.