A method, system, device and medium for dynamic trajectory planning of automatic parking

Abstract

The present invention provides a trajectory dynamic planning method, system, device and medium for automatic parking, establishes a vehicle dynamics model in automatic parking, solves vehicle kinematic constraints and boundary conditions and obstacle constraints in the motion process, establishes an automatic parking optimal trajectory planning model with a given initial position to a terminal position and a driving time or a driving mileage as an evaluation function, discretizes the state variables and control variables of the vehicle in the state space, and adopts the idea of ​​dynamic programming based on a cell mapping method to achieve global optimality of trajectory planning for the automatic parking trajectory, and obtains an optimal trajectory planning discrete table through offline calculation; wherein the idea of ​​dynamic programming can reduce the complexity of calculation and ensure the accuracy of the optimal planning solution, and the vehicle can make online decision planning for the vehicle according to the optimal trajectory planning discrete table calculated offline, so as to meet the real-time requirements and ensure the safety and reliability of its automatic parking process.

Description

Technical Field

[0001] The present invention belongs to the technical field of automatic parking in intelligent driving, and specifically relates to a method, system, device and medium for dynamic planning of automatic parking trajectories. Background Art

[0002] Intelligent driving vehicles can realize automatic parking, which involves a method for dynamic planning of the optimal trajectory of automatic parking at low speed. The optimal trajectory planning of a vehicle is to find the global optimal path from a given starting position to an end position while satisfying various constraints when the vehicle is driving, and provide the vehicle's motion state information at each moment and the optimal control input from a time series.

[0003] At present, the automatic parking trajectory planning method is still under improvement and perfection. Among them, the geometric line construction method can simply and quickly obtain the optimal trajectory of automatic parking, but the discontinuity of the trajectory greatly reduces the accuracy. The graph search method based on the Dijkstra algorithm takes too long in the search process and the search is extremely blind. The random sampling method based on the probabilistic road map or the fast search random tree can effectively solve the problem of the optimal trajectory of automatic parking in a wide environment, but it cannot solve the problem of optimizing the optimal trajectory of automatic parking in a local narrow environment due to the lack of sampling points.

[0004] With the rapid development of artificial intelligence technology and computer hardware, various intelligent algorithms are widely used in the optimization of the optimal trajectory of automatic parking, such as neural networks, fuzzy control, machine learning, genetic algorithms and particle swarm optimization, and have achieved good results. However, various intelligent algorithms cannot meet the real-time requirements of automatic parking due to the complex calculation process, and have great defects in the stability and safety issues that are extremely important in automatic parking. Therefore, it is extremely important and urgent to develop an optimal trajectory planning method for automatic parking that meets real-time, stability, safety and accuracy. Summary of the invention

[0005] In view of the problems existing in the prior art, the present invention provides a method, system, device and medium for dynamic trajectory planning of automatic parking, which can determine the optimal trajectory planning of automatic parking in real time, stably and accurately.

[0006] The present invention is achieved through the following technical solutions:

[0007] A method for dynamic trajectory planning of automatic parking, characterized in that it comprises the following steps:

[0008] S1: Establish the vehicle dynamics model in automatic parking, calculate the vehicle kinematic constraints and boundary conditions as well as the obstacle constraints during the motion process;

[0009] S2: Establishing an optimal trajectory planning model for automatic parking from a given initial position to a terminal position with driving time or mileage as the evaluation function;

[0010] S3: By discretizing the state variables and control variables of the vehicle in the state space, the dynamic programming method is used based on the cell mapping method to perform global optimal planning of the automatic parking trajectory, and the optimal trajectory planning discrete table is obtained through offline calculation. The vehicle completes online decision planning based on the optimal trajectory planning discrete table calculated offline.

[0011] Furthermore, the vehicle kinematic constraints in step S1 are road obstacles, including passing pedestrians and vehicles.

[0012] Furthermore, the vehicle kinematic constraints in step S1 are road obstacles, including passing pedestrians and vehicles.

[0013] The cell mapping method in step S3 comprises the following steps:

[0014] Discretize the state space into multiple evenly distributed square areas, each of which is a cell, and take all the points of each cell as the center point of the cell;

[0015] The center points of the cells in the square area that the vehicle needs to pass through are sequentially substituted into the dynamic equation and mapped to obtain the cell mapping relationship.

[0016] Furthermore, the vehicle kinematic constraints in step S1 are road obstacles, including passing pedestrians and vehicles.

[0017] The offline calculation in step S3 includes the following steps:

[0018] Discretize the state variables and decision variables in the vehicle dynamics equation to obtain its discretized dynamics equation and evaluation function; mark the cells where the initial position and the final position are located in the discrete state space, and define the trapped cells by the constraint equation;

[0019] Based on the vehicle's dynamic equation, each cell is mapped step by step in the discrete state space, and the corresponding cell mapping information is recorded;

[0020] Start searching from the cell where the termination position is located, and record the accumulated optimal cell mapping information of each cell, the accumulated optimal evaluation function value and the corresponding optimal planning solution;

[0021] Continue to search forward and update the cumulative optimal evaluation function value and the corresponding optimal trajectory planning solution;

[0022] Determine the end search condition. When the optimal trajectory planning discrete table is no longer updated or all cells are searched, the search ends and the optimal trajectory planning discrete table is obtained through integration.

[0023] Furthermore, the vehicle kinematic constraints in step S1 are road obstacles, including passing pedestrians and vehicles.

[0024] The trap cells defined by the constraint equations are not searched during the search and are regarded as obstacle areas.

[0025] Furthermore, the vehicle kinematic constraints in step S1 are road obstacles, including passing pedestrians and vehicles.

[0026] The cell mapping information includes a mapping relationship and a corresponding evaluation function value and a one-step planning solution.

[0027] Furthermore, the vehicle kinematic constraints in step S1 are road obstacles, including passing pedestrians and vehicles.

[0028] The process of online decision-making and planning for vehicles is as follows:

[0029] Find the cell where the vehicle's current state variables and decision variables are located, and determine whether the cell where the current state is located has changed. If the cell number has changed, query the optimal trajectory planning discrete table that has been calculated offline online, and find the one-step optimal trajectory planning solution in the table; if the cell number has not changed, the optimal trajectory planning solution remains unchanged; use the one-step optimal trajectory planning solution as the decision input for the vehicle's current state, and perform one-step trajectory planning online.

[0030] A trajectory dynamic planning system for automatic parking, comprising:

[0031] The first modeling and calculation module is used to establish the vehicle dynamics model in automatic parking, calculate the vehicle kinematic constraints and boundary conditions, and obstacle constraints during the movement process;

[0032] The second modeling and operation module is used to establish an automatic parking optimal trajectory planning model from a given initial position to a terminal position with driving time or driving mileage as an evaluation function;

[0033] Path planning module: It is used to discretize the vehicle's state variables and control variables in the state space, use dynamic programming based on the cell mapping method to perform global optimal planning of the automatic parking trajectory, and obtain the optimal trajectory planning discrete table through offline calculation. The vehicle makes decisions online based on the optimal trajectory planning discrete table calculated offline.

[0034] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of a method for dynamic trajectory planning of automatic parking are implemented.

[0035] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of a method for dynamic trajectory planning of automatic parking.

[0036] Compared with the prior art, the present invention has the following beneficial technical effects:

[0037] The present invention provides a trajectory dynamic planning method, system, device and medium for automatic parking, establishes a vehicle dynamics model in automatic parking, solves vehicle kinematic constraints and boundary conditions and obstacle constraints in the motion process, establishes an automatic parking optimal trajectory planning model with a given initial position to a terminal position and a driving time or a driving mileage as an evaluation function, discretizes the state variables and control variables of the vehicle in the state space, and adopts the idea of ​​dynamic programming based on a cell mapping method to achieve global optimality of trajectory planning for the automatic parking trajectory, and obtains an optimal trajectory planning discrete table through offline calculation; wherein the idea of ​​dynamic programming can reduce the complexity of calculation and ensure the accuracy of the optimal planning solution, and the vehicle can make online decision planning for the vehicle according to the optimal trajectory planning discrete table calculated offline, so as to meet the real-time requirements and ensure the safety and reliability of its automatic parking process. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 A flow chart of a dynamic trajectory planning method for automatic parking in a specific embodiment of the present invention;

[0039] Figure 2 It is a scene diagram of automatic parking trajectory planning in a specific embodiment of the present invention;

[0040] Figure 3 Schematic diagram of an automatic parking cell mapping method in a specific embodiment of the present invention;

[0041] Figure 4 It is a schematic diagram of an offline solution based on cell mapping dynamic programming in a specific embodiment of the present invention;

[0042] Figure 5 Schematic diagram of an online decision-making scheme for automatic parking trajectory planning in a specific embodiment of the present invention. DETAILED DESCRIPTION

[0043] The present invention is further described in detail below in conjunction with specific embodiments, which are intended to explain the present invention rather than to limit it.

[0044] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0045] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0046] The present invention provides a method for dynamic trajectory planning of automatic parking, such as Figure 1 As shown, the following steps are included:

[0047] S1: Establish the vehicle dynamics model in automatic parking, calculate the vehicle kinematic constraints and boundary conditions as well as the obstacle constraints during the motion process;

[0048] S2: Establishing an optimal trajectory planning model for automatic parking from a given initial position to a terminal position with driving time or mileage as the evaluation function;

[0049] S3: By discretizing the state variables and control variables of the vehicle in the state space, a dynamic programming method based on the cell mapping method is used to perform global optimal planning of the automatic parking trajectory, and an optimal trajectory planning discrete table is obtained through offline calculation. The vehicle makes decisions online based on the optimal trajectory planning discrete table calculated offline.

[0050] Specifically, Figure 2 As shown, the vehicle kinematic constraint in step S1 is a road obstacle, including passing pedestrians and vehicles. The automatic parking trajectory planning scenario of the present application is automatic parking, which is a trajectory planning scenario from the initial position to the end position under given constraints and satisfies the optimal evaluation function value (call time). The automatic parking is performed in a local narrow environment at a low speed, and the purpose of automatic parking is achieved while meeting the requirements of safety, comfort, convenience and speed.

[0051] Preferably, the cell mapping method in step S3 comprises the following steps:

[0052] Discretize the state space into multiple evenly distributed square areas, each of which is a cell, and take all the points of each cell as the center point of the cell;

[0053] The center points of the cells in the square area that the vehicle needs to pass through are sequentially substituted into the dynamic equation and mapped to obtain the cell mapping relationship.

[0054] Specifically, Figure 3 As shown in the figure, taking the two-dimensional state space as an example, the state space is discretized into small square areas, each of which represents a cell. All points of each cell will be regarded as the center point of the cell, that is, the center point of each cell represents all points in each cell area. The area where the initial state is located is in cell z0. The center point of z0 is substituted into the dynamic equation of the vehicle for one-step mapping to reach the area where cell z1 is located. Then, the center point of cell z1 is substituted into the dynamic equation of the vehicle for one-step mapping to reach the area where z2 is located. Thus, the mapping relationship from cell z0 to z1 and the mapping relationship from z1 to z2 are established. The cell mapping information includes not only the mapping pair relationship, but also the corresponding one-step mapping evaluation function value and the one-step mapping trajectory planning solution.

[0055] Preferably, the offline calculation in step S3 includes the following steps:

[0056] Discretize the state variables and decision variables in the vehicle dynamics equation to obtain its discretized dynamics equation and evaluation function; mark the cells where the initial position and the final position are located in the discrete state space, and define the trapped cells by the constraint equation;

[0057] Based on the vehicle's dynamic equation, each cell is mapped step by step in the discrete state space, and the corresponding cell mapping information is recorded;

[0058] Start searching from the cell where the termination position is located, and record the accumulated optimal cell mapping information of each cell, the accumulated optimal evaluation function value and the corresponding optimal planning solution;

[0059] Continue to search forward and update the cumulative optimal evaluation function value and the corresponding optimal trajectory planning solution;

[0060] Determine the end search condition. When the optimal trajectory planning discrete table is no longer updated or all cells are searched, the search ends and the optimal trajectory planning discrete table is obtained through integration.

[0061] Specifically, Figure 4 As shown in Figure 2, the offline solution based on cell mapping dynamic programming consists of five steps:

[0062] Step 1: Discretize the state variables and decision variables in the vehicle dynamics equation, and obtain its discretized dynamics equation and evaluation function; then mark the cells where the initial position and the final position are located in the discrete state space, and define the trapped cells by the constraint equation (the trapped cells are not searched during the search and are regarded as obstacle areas);

[0063] Step 2: Map each cell step by step in the discrete state space based on the vehicle's dynamic equations, and record the corresponding cell mapping information (mapping relationship and corresponding evaluation function value and one-step planning solution).

[0064] Step 3: Search forward from the cell where the termination position is located, and record the cumulative optimal cell mapping information of each cell (the cumulative optimal evaluation function value and the corresponding optimal planning solution);

[0065] Step 4: Continue to search forward and update the cumulative optimal evaluation function value and the corresponding optimal trajectory planning solution;

[0066] Step 5: Determine the end search condition. When the optimal trajectory planning discrete table is no longer updated or all cells are searched, the search ends.

[0067] Step 6: Integrate to obtain the optimal trajectory planning discrete table.

[0068] Preferably, the process of online decision-making planning for vehicles is:

[0069] Find the cell where the vehicle's current state variables and decision variables are located, and determine whether the cell where the current state is located has changed. If the cell number has changed, query the optimal trajectory planning discrete table that has been calculated offline online, and find the one-step optimal trajectory planning solution in the table; if the cell number has not changed, the optimal trajectory planning solution remains unchanged; use the one-step optimal trajectory planning solution as the decision input for the vehicle's current state, and perform one-step trajectory planning online.

[0070] The present invention provides a trajectory dynamic planning system for automatic parking, comprising:

[0071] The first modeling and calculation module is used to establish the vehicle dynamics model in automatic parking, calculate the vehicle kinematic constraints and boundary conditions, and obstacle constraints during the movement process;

[0072] The second modeling and operation module is used to establish an automatic parking optimal trajectory planning model from a given initial position to a terminal position with driving time or driving mileage as an evaluation function;

[0073] Path planning module: It is used to discretize the vehicle's state variables and control variables in the state space, use dynamic programming based on the cell mapping method to perform global optimal planning of the automatic parking trajectory, and obtain the optimal trajectory planning discrete table through offline calculation. The vehicle makes decisions online based on the optimal trajectory planning discrete table calculated offline.

[0074] In another embodiment of the present invention, a computer device is provided, the computer device including a processor and a memory, the memory is used to store a computer program, the computer program includes program instructions, and the processor is used to execute the program instructions stored in the computer storage medium. The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc., which are the computing core and control core of the terminal, which are suitable for implementing one or more instructions, and are specifically suitable for loading and executing one or more instructions in the computer storage medium to implement the corresponding method flow or corresponding function; the processor described in the embodiment of the present invention can be used for the operation of a trajectory dynamic planning method for automatic parking.

[0075] In another embodiment of the present invention, the present invention also provides a storage medium, specifically a computer-readable storage medium (Memory), which is a memory device in a computer device for storing programs and data. It is understandable that the computer-readable storage medium here can include both built-in storage media in a computer device and, of course, extended storage media supported by the computer device. The computer-readable storage medium provides a storage space, which stores the operating system of the terminal. In addition, one or more instructions suitable for being loaded and executed by a processor are also stored in the storage space, and these instructions can be one or more computer programs (including program codes). It should be noted that the computer-readable storage medium here can be a high-speed RAM memory or a non-volatile memory, such as at least one disk memory. The processor can load and execute one or more instructions stored in the computer-readable storage medium to implement the corresponding steps of the dynamic trajectory planning method for an automatic parking in the above embodiment.

[0076] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0077] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0078] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0079] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0080] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A dynamic trajectory planning method for automatic parking, characterized in that: The following steps are involved: S1: Establish the vehicle dynamics model in automatic parking, calculate the vehicle kinematic constraints and boundary conditions as well as the obstacle constraints during the motion process; S2: Establishing an optimal trajectory planning model for automatic parking from a given initial position to a terminal position with driving time or mileage as the evaluation function; S3: The state variables and control variables of the vehicle are discretized in the state space, and the global optimal planning of the automatic parking trajectory is performed by a dynamic programming method based on the cell mapping method, and the optimal trajectory planning discrete table is obtained by offline calculation. The vehicle completes the online decision planning of the vehicle according to the optimal trajectory planning discrete table calculated offline. The cell mapping method includes the following steps: Discretize the state space into multiple evenly distributed square areas, each of which is a cell, and take all the points of each cell as the center point of the cell; The center points of the cells in the square area that the vehicle needs to pass through are sequentially substituted into the dynamic equation and mapped to obtain the cell mapping relationship.

2. The method for dynamic trajectory planning of automatic parking according to claim 1, characterized in that: The vehicle kinematic constraints in step S1 are road obstacles, including passing pedestrians and vehicles.

3. The method for dynamic trajectory planning of automatic parking according to claim 1, characterized in that: The offline calculation in step S3 includes the following steps: Discretize the state variables and decision variables in the vehicle dynamics equation to obtain its discretized dynamics equation and evaluation function; mark the cells where the initial position and the final position are located in the discrete state space, and define the trapped cells by the constraint equation; Based on the vehicle's dynamic equation, each cell is mapped step by step in the discrete state space, and the corresponding cell mapping information is recorded; Start searching from the cell where the termination position is located, and record the accumulated optimal cell mapping information of each cell, the accumulated optimal evaluation function value and the corresponding optimal planning solution; Continue to search forward and update the cumulative optimal evaluation function value and the corresponding optimal trajectory planning solution; Determine the end search condition. When the optimal trajectory planning discrete table is no longer updated or all cells are searched, the search ends and the optimal trajectory planning discrete table is obtained through integration.

4. The method for dynamic trajectory planning of automatic parking according to claim 3, characterized in that: The trap cells defined by the constraint equations are not searched during the search and are regarded as obstacle areas.

5. The method for dynamic trajectory planning of automatic parking according to claim 3, characterized in that: The cell mapping information includes a mapping relationship and a corresponding evaluation function value and a one-step planning solution.

6. The method for dynamic trajectory planning of automatic parking according to claim 1, characterized in that: The process of online decision-making and planning for vehicles is as follows: Find the cell where the vehicle's current state variables and decision variables are located, and determine whether the cell where the current state is located has changed. If the cell number has changed, query the optimal trajectory planning discrete table that has been calculated offline online, and find the one-step optimal trajectory planning solution in the table; if the cell number has not changed, the optimal trajectory planning solution remains unchanged; use the one-step optimal trajectory planning solution as the decision input for the vehicle's current state, and perform one-step trajectory planning online.

7. A dynamic trajectory planning system for automatic parking, characterized in that: include: The first modeling and calculation module is used to establish the vehicle dynamics model in automatic parking, calculate the vehicle kinematic constraints and boundary conditions, and obstacle constraints during the movement process; The second modeling and operation module is used to establish an automatic parking optimal trajectory planning model from a given initial position to a terminal position with driving time or driving mileage as an evaluation function; Path planning module: It is used to discretize the state variables and control variables of the vehicle in the state space, and use the dynamic programming method based on the cell mapping method to perform global optimal planning of the automatic parking trajectory, and obtain the optimal trajectory planning discrete table through offline calculation. The vehicle makes decisions and plans online according to the optimal trajectory planning discrete table calculated offline. The cell mapping method includes the following steps: Discretize the state space into multiple evenly distributed square areas, each of which is a cell, and take all the points of each cell as the center point of the cell; The center points of the cells in the square area that the vehicle needs to pass through are sequentially substituted into the dynamic equation and mapped to obtain the cell mapping relationship.

8. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method for dynamic trajectory planning of automatic parking as described in any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method for dynamic trajectory planning of automatic parking as claimed in any one of claims 1 to 6 are implemented.

Images