Automatic driving path planning method, device, equipment and storage medium
By building a high-precision map and using the status raster algorithm to calculate the driving path, the problem of inaccurate positioning in the autonomous driving path planning is solved, and more accurate and feasible path planning is achieved.
Patent Information
- Application Number
- CN202111088984.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-09-16
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2041-09-16
AI Technical Summary
The existing autonomous driving path planning methods have cumulative errors in the point cloud positioning process, resulting in problems such as non-convergence of positioning and inaccurate positioning.
By obtaining the road information collected by lidar, a high-precision map is built, the current position and target position of the unmanned vehicle are determined, and multiple driving paths are calculated using the status grid algorithm, and the path with the least driving cost is selected as the target driving path, combined with movement Learn constraints to make path correction.
It reduces the randomness of path planning, can find a feasible path at a given starting point and end point coordinates, and reasonably selects the path when dynamic obstacles avoid obstacles, improving the accuracy and feasibility of path planning.
Smart Images

Figure CN113819917B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of autonomous driving technology. Specifically, the present application relates to an autonomous driving path planning method, device, equipment and storage medium. Background Art
[0002] With the full promotion of artificial intelligence technology, autonomous driving technology is developing rapidly, mainly reflected in the development of environmental perception, decision-making and planning, control and execution, high-precision maps and real-time positioning technologies. In recent years, autonomous driving technology has become the latest development direction of the global automotive industry. Compared with traditional cars, autonomous driving cars can effectively improve vehicle safety, traffic efficiency and comfort. Unmanned vehicles can not only reduce the incidence of traffic accidents, but also improve the efficiency of automobile travel. Therefore, they have received extensive attention from scientific research institutions, automobile manufacturers and Internet companies. Path planning, one of the core technologies of unmanned vehicles, has always been a difficult and hot spot in the field of unmanned driving technology.
[0003] The path planning algorithm plans an optimal path for the driverless vehicle under the known environment map information. The driverless car is in an unpredictable and highly dynamic road environment when it is moving forward. Obstacles are likely to appear on the planned path, and some obstacles may appear on the path dynamically during the process of moving forward. The autonomous vehicle must respond to these unpredictable events in some way.
[0004] Issues that need to be considered in path planning for autonomous vehicles. Autonomous vehicles need to plan a drivable path that meets the size and shape constraints of the vehicle itself in real time to cope with complex and changing traffic environments. However, existing path planning solutions have many shortcomings in solving the above problems. Summary of the invention
[0005] The purpose of this application is to solve at least one of the above-mentioned technical defects, especially the technical defect that the prior art has cumulative errors, resulting in non-convergence of positioning during the point cloud positioning process, and ultimately causing inaccurate positioning.
[0006] In a first aspect, a method for autonomous driving path planning is provided, characterized by comprising:
[0007] Obtain road information collected by LiDAR;
[0008] Building a high-precision map based on the road information;
[0009] Determine the current position of the unmanned vehicle in the high-precision map and the target position that the unmanned vehicle needs to reach;
[0010] Determine obstacles on the driving path, and use a state grid algorithm to calculate multiple driving paths from the current position to the target position;
[0011] The driving cost of each driving path is calculated using a preset method, and the driving path with the smallest driving cost is selected as the target driving path.
[0012] As a possible implementation of the present application, in this implementation, obtaining road information collected by a laser radar includes:
[0013] Acquire the position information of the unmanned vehicle, wherein the position information includes the global positioning coordinates, yaw angle, pitch angle and roll angle of the unmanned vehicle;
[0014] The constructing a high-precision map based on the road information includes:
[0015] The rotation matrix of the point cloud to rigid body transformation is determined based on the posture information, and the global positioning coordinates collected by the laser radar are transformed into the same coordinate system through the rotation matrix to construct a three-dimensional environmental map of the road.
[0016] As a possible implementation of the present application, in this implementation, the rotation matrix of the point cloud to rigid body transformation is determined based on the posture information, and the global positioning coordinates collected by the laser radar are transformed into the same coordinate system through the rotation matrix to construct the three-dimensional environment map of the road, including:
[0017] The point cloud data collected by the laser radar at time t is F t = {P t1 ,P t2 …P tj}, the point cloud map of the road is
[0018] Among them, R t is a 3×3 rotation matrix, t t is the translation matrix, P tj A vector consisting of point cloud coordinates.
[0019] As a possible implementation of the present application, in this implementation, the method further includes:
[0020] Using the pose information as input of a preset IPC registration algorithm to extract the trunk point cloud of the road;
[0021] The backbone point cloud is aligned to determine the high-precision map.
[0022] As a possible implementation of the present application, in this implementation, determining obstacles on the driving path and using a state grid algorithm to calculate multiple driving paths from the current position to the target position includes:
[0023] Initialize the road space, determine the current position point, the target position point, the number of sampling points, and the step length between sampling points;
[0024] randomly generating intermediate points in the road space;
[0025] Selecting a sampling point closest to the middle point from among the sampling points as the target middle point;
[0026] Intercepting an intermediate path of the step length from the sampling point to the target intermediate point;
[0027] When there is no obstacle in the middle path, the middle path is determined as a section of the driving path.
[0028] As a possible implementation of the present application, in this implementation, the method further includes:
[0029] Calculating the curvature of the unmanned vehicle on the driving path;
[0030] The target driving path is constructed by selecting a path whose curvature satisfies the kinematic constraints of the unmanned vehicle itself.
[0031] As a possible implementation of the present application, in this implementation, before calculating the curvature of the unmanned vehicle on the driving path, the method further includes:
[0032] The state grid algorithm is used to guide the expansion domain;
[0033] The RRT algorithm is used to search for a feasible path based on the current position point, the target position point, the number of sampling points, and the step length between sampling points.
[0034] In a second aspect, an automatic driving path planning device is provided, the device comprising:
[0035] An information acquisition module is used to obtain road information collected by the laser radar;
[0036] A map construction module, used to construct a high-precision map based on the road information;
[0037] A position determination module, used to determine the current position of the unmanned vehicle in the high-precision map and the target position that the unmanned vehicle needs to reach;
[0038] A path planning module, used to determine obstacles on the driving path and calculate multiple driving paths from the current position to the target position using a state grid algorithm;
[0039] The path determination module is used to calculate the driving cost of each driving path using a preset method, and select the driving path with the smallest driving cost as the target driving path.
[0040] According to a third aspect, an electronic device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-mentioned autonomous driving path planning method when executing the program.
[0041] In a fourth aspect, a computer-readable storage medium is provided, wherein the storage medium stores at least one instruction, at least one program, a code set or an instruction set, and the at least one instruction, the at least one program, the code set or the instruction set are loaded and executed by the processor to implement the above-mentioned autonomous driving path planning method.
[0042] The embodiment of the present application obtains road information through laser radar, and builds a high-precision map based on the road information, determines the current position of the unmanned vehicle in the high-precision map and the target position of the unmanned vehicle, can reduce the randomness of path planning, and can find a drivable path and derive the corresponding input information after the starting point and end point coordinates are given. When avoiding dynamic obstacles, the unmanned vehicle performs local path planning and can reasonably select a drivable path. According to kinematic constraints, such as speed, turning radius, path curvature, etc., the planned route is corrected. BRIEF DESCRIPTION OF THE DRAWINGS
[0043] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in describing the embodiments of the present application.
[0044] Figure 1 A flowchart of an autonomous driving path planning method provided in an embodiment of the present application;
[0045] Figure 2 A flowchart of a method for constructing a high-precision map provided in an embodiment of the present application;
[0046] Figure 3 A schematic diagram of vehicle modeling provided in an embodiment of the present application;
[0047] Figure 4 A flow chart of an RRT algorithm provided in an embodiment of the present application;
[0048] Figure 5 A state diagram provided for an embodiment of the present application;
[0049] Figure 6 A schematic diagram of the structure of an automatic driving path planning device provided in an embodiment of the present application;
[0050] Figure 7 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application.
[0051] The above and other features, advantages and aspects of the embodiments of the present application will become more apparent with reference to the following detailed description in conjunction with the accompanying drawings. Throughout the accompanying drawings, the same or similar reference numerals represent the same or similar elements. It should be understood that the drawings are schematic and the originals and elements are not necessarily drawn to scale. DETAILED DESCRIPTION
[0052] The embodiments of the present application are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application, and cannot be interpreted as limiting the present application.
[0053] It will be understood by those skilled in the art that, unless expressly stated, the singular expressions "a", "an", "said" and "the" used herein may also include the plural expressions. It should be further understood that the expression "comprising" used in the specification of the present application refers to the presence of the described features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof. It should be understood that when we refer to an element as being "connected" or "coupled" to another element, it may be directly connected or coupled to the other element, or there may be intermediate elements. In addition, the "connection" or "coupling" used herein may include wireless connection or wireless coupling. The expression "and / or" used herein includes all or any unit and all combinations of one or more associated listed items.
[0054] In order to make the objectives, technical solutions and advantages of the present application clearer, the implementation methods of the present application will be further described in detail below with reference to the accompanying drawings.
[0055] With the full promotion of artificial intelligence technology, autonomous driving technology is developing rapidly, mainly reflected in the development of environmental perception, decision-making and planning, control and execution, high-precision maps and real-time positioning technologies. In recent years, autonomous driving technology has become the latest development direction of the global automotive industry. Compared with traditional cars, autonomous driving cars can effectively improve vehicle safety, traffic efficiency and comfort. Unmanned vehicles can not only reduce the incidence of traffic accidents, but also improve the efficiency of automobile travel. Therefore, they have received extensive attention from scientific research institutions, automobile manufacturers and Internet companies. Path planning, one of the core technologies of unmanned vehicles, has always been a difficult and hot spot in the field of unmanned driving technology.
[0056] The path planning algorithm plans an optimal path for the driverless vehicle under the known environment map information. The driverless car is in an unpredictable and highly dynamic road environment when it is moving forward. Obstacles are likely to appear on the planned path, and some obstacles may appear on the path dynamically during the process of moving forward. The autonomous vehicle must respond to these unpredictable events in some way.
[0057] Issues that need to be considered in path planning for autonomous vehicles. Autonomous vehicles need to plan a drivable path that meets the size and shape constraints of the vehicle itself in real time to cope with complex and changing traffic environments. However, existing path planning solutions have many shortcomings in solving the above problems.
[0058] The method, device, electronic device and computer-readable storage medium based on autonomous driving path planning provided in this application are intended to solve the above technical problems of the prior art.
[0059] The technical solution of the present application and how the technical solution of the present application solves the above-mentioned technical problems are described in detail below with specific embodiments. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments. The embodiments of the present application will be described below in conjunction with the accompanying drawings.
[0060] An automatic driving path planning method is provided in an embodiment of the present application. Figure 1 As shown, the method includes:
[0061] Step S101, obtaining road information collected by a laser radar;
[0062] Step S102, constructing a high-precision map based on the road information;
[0063] Step S103, determining the current position of the unmanned vehicle in the high-precision map and the target position that the unmanned vehicle needs to reach;
[0064] Step S104, determining obstacles on the driving path, and using a state grid algorithm to calculate multiple driving paths from the current position to the target position;
[0065] Step S105 , using a preset method to calculate the driving costs of each driving path, and selecting the driving path with the minimum driving cost as the target driving path.
[0066] In the embodiment of the present application, the computing platform selected is NVIDIA Jetson agx xavier, and is installed with Ubuntu18.04 and machine operating system ROS. The hardware includes Velodyne 16-line mechanical laser radar, wire-controlled electric chassis, and integrated inertial navigation system (IMU). The remote-controlled driving vehicle collects road information, records the original point cloud map data, processes the original point cloud map data through the high-precision map software 3D unity, constructs a high-precision map with driving areas and lane lines, and imports the constructed high-precision map into the computing platform. For the construction of high-precision maps, road information is first collected through laser radar, and then a high-precision map is constructed based on the road information. The road information collected by the laser radar includes:
[0067] Acquire the position information of the unmanned vehicle, wherein the position information includes the global positioning coordinates, yaw angle, pitch angle and roll angle of the unmanned vehicle;
[0068] The constructing a high-precision map based on the road information includes:
[0069] The rotation matrix of the point cloud to rigid body transformation is determined based on the posture information, and the global positioning coordinates collected by the laser radar are transformed into the same coordinate system through the rotation matrix to construct a three-dimensional environmental map of the road.
[0070] In the embodiment of the present application, the construction of a high-precision point cloud map is usually constructed by using the position data provided by the combined inertial navigation system, and its posture data includes global positioning coordinates and three posture data of yaw angle, pitch angle and roll angle centered on the inertial navigation device. A set of posture data collected at time t is represented as Z t =(x t ,y t ,z t ,yaw t ,pitch t ,roll t ) T , where x t ,y t and z t They represent the global coordinates in the three-dimensional Cartesian coordinate system, yaw t 、pitch t and rollt Represent the yaw angle, pitch angle, and roll angle respectively. Among the common inertial navigation devices currently available, the positioning accuracy is usually 2-5 cm when the positioning signal is good, the accuracy of the attitude is usually less than 0.1 degrees in all directions, and the update frequency of the attitude is 100-200 Hz. However, considering the comprehensive cost, more devices with slightly lower accuracy are used. Therefore, in the process of high-precision map construction, the posture provided by the vehicle's inertial navigation system can be used for preliminary rough construction. In order to construct a high-precision point cloud map based on the posture, it is first necessary to obtain the rotation matrix R of the point cloud rigid body transformation based on the posture data. t and t t Then, by matching the point cloud data collected at time t with the pose data, all point cloud frames at different times are transformed to the same coordinates through the rotation matrix. Assume that the point cloud data frame collected by the laser radar at time t is F t = {P t1 ,P t2 …P tj}, the point cloud map composed of lidar can be expressed as:
[0071]
[0072] Among them, R t is a rotation matrix of size 3×3, which is obtained by yaw t 、pitch t and roll t Calculate and get. t =[x t ,y t ,z t ] T is the translation vector, P tj =[x j ,y j ,z j ] T is a vector consisting of the point cloud coordinates.
[0073] However, since the position and attitude provided by the inertial navigation system have certain deviations in positioning and attitude, even in the absence of positioning signals, the position and attitude can only be estimated by the inertial measurement unit, resulting in the accumulation of position and attitude errors. Therefore, the constructed coarse-precision point cloud map also needs to be registered between point cloud frames to reduce errors and further improve the accuracy of the map.
[0074] As a possible implementation of the present application, in this implementation, if Figure 2 As shown, the method also includes:
[0075] Step S201, using the posture information as input of a preset IPC registration algorithm to extract the trunk point cloud of the road;
[0076] Step S202, performing registration on the backbone point cloud to determine the high-precision map.
[0077] In the embodiment of the present application, the acquired pose data provided by the inertial navigation system is obtained, and the pose data is integrated into the ICP algorithm to extract the road trunk point cloud as the input of the improved ICP algorithm. After the road trunk point cloud is extracted, the ICP algorithm can be used for registration. The positioning point set P and the point cloud S to be registered are input. The improved ICP algorithm is used to extract the point cloud that meets the road trunk through formula (1) and realize the registration of the main road point cloud, and the registration matrix R is obtained. r With the translation vector t r , and update the transformation parameters at the same time. Let the updated rotation parameters be:
[0078]
[0079] Through the above-mentioned improved ICP registration method, the construction of high-precision point cloud map is finally achieved.
[0080] In the embodiment of the present application, it is necessary to model the vehicle, such as Figure 3 As shown, the vehicle kinematic equation is as follows:
[0081]
[0082] Linearize it and get the linear time-varying model:
[0083]
[0084] In the formula, the matrices and state variables are:
[0085]
[0086]
[0087] T is the sampling time.
[0088] As a possible implementation of the present application, in this implementation, determining obstacles on the driving path and using a state grid algorithm to calculate multiple driving paths from the current position to the target position includes:
[0089] Initialize the road space, determine the current position point, the target position point, the number of sampling points, and the step length between sampling points;
[0090] randomly generating intermediate points in the road space;
[0091] Selecting a sampling point closest to the middle point from among the sampling points as the target middle point;
[0092] Intercepting an intermediate path of the step length from the sampling point to the target intermediate point;
[0093] When there is no obstacle in the middle path, the middle path is determined as a section of the driving path.
[0094] As a possible implementation of the present application, in this implementation, the method further includes:
[0095] Calculating the curvature of the unmanned vehicle on the driving path;
[0096] The target driving path is constructed by selecting a path whose curvature satisfies the kinematic constraints of the unmanned vehicle itself.
[0097] In the embodiment of the present application, for the convenience of explanation, a specific embodiment is taken as an example. When searching based on the RRT algorithm, the algorithm flow chart is as follows: Figure 4 As shown, initialize the entire space, define the initial point, end point, number of sampling points, step length between points, etc.; randomly generate a point x in the space rand ; Find the point x closest to this random point in the point set of the known tree near ; in x near to x rand The straight line direction from x near Intercept point x with step size new ; Judging from x new to x near Is there an obstacle between them? If so, discard the point. Otherwise, new The point is added to the tree set. Multithreaded parallel calculations are used to find multiple paths at the same time, and the best path is selected according to the evaluation function. Because the algorithm itself has the characteristics of randomness, a better path can be found when calculating multiple routes without increasing too much calculation. The multiple curves obtained are respectively brought into the evaluation function to calculate the scores, and the output with the highest score is selected. The evaluation criteria are the total length of the curve, the maximum curvature change rate and the minimum turning radius. The evaluation function is as follows:
[0098]
[0099] Where Length represents the total route length from the starting point to the end point. It represents the maximum curvature of the curve, and min(r) represents the minimum turning radius of the curve.
[0100] In the embodiment of the present application, the RRT algorithm searches for trajectories without dynamic constraints when performing global path planning. Considering the dynamic constraints such as the actual speed and acceleration of the autonomous vehicle, it is necessary to use the state grid algorithm to optimize the current path to meet the requirements of the autonomous vehicle. The specific implementation method of the state grid algorithm is based on sampling of the control space: Assuming that the power model of the unmanned vehicle is Where s is the state variable and u is the control variable. Under the condition of the known controlled object, the input control variable u and the integral time T are fixed, and the forward integration can be performed to obtain the controlled object from any initial state s0 to the final state s f , forward integration can predict the state of the object after any time T, and obtain a series of state sets, which is the sampling in the control space. Since u and T are given arbitrarily, sampling in the control space often has no clear purpose, and the sampling results can only be distributed near certain states, and it is impossible to accurately sample a given state, such as Figure 5 As shown,
[0101] State variables
[0102] Input Vector
[0103] State transition matrix
[0104]
[0105] System equation: After u and T are given, the optimized trajectory equation can be obtained as:
[0106]
[0107] The metric function aims to find a point x on the random expansion tree near From sampling point x ran d is the closest one. In fact, it is to calculate the Euclidean distance between two points. It refers to the real distance between two points in m-dimensional space, or the natural length of a vector.
[0108] Euclidean distance in N dimensions:
[0109] The driving trajectory of the unmanned vehicle can be calculated using the above trajectory equation.
[0110] The embodiment of the present application obtains road information through laser radar, and builds a high-precision map based on the road information, determines the current position of the unmanned vehicle in the high-precision map and the target position of the unmanned vehicle, can reduce the randomness of path planning, and can find a drivable path and derive the corresponding input information after the starting point and end point coordinates are given. When avoiding dynamic obstacles, the unmanned vehicle performs local path planning and can reasonably select a drivable path. According to kinematic constraints, such as speed, turning radius, path curvature, etc., the planned route is corrected.
[0111] The present application embodiment provides an automatic driving path planning device, such as Figure 6 As shown, the positioning device 60 based on the three-dimensional point cloud may include: an information acquisition module 601, a map construction module 602, a position determination module 603, a path planning module 604, and a path determination module 605, wherein:
[0112] Information acquisition module 601, used to acquire road information collected by laser radar;
[0113] A map construction module 602, configured to construct a high-precision map based on the road information;
[0114] A position determination module 603 is used to determine the current position of the unmanned vehicle in the high-precision map and the target position that the unmanned vehicle needs to reach;
[0115] A path planning module 604, used to determine obstacles on the driving path and calculate multiple driving paths from the current position to the target position using a state grid algorithm;
[0116] The path determination module 605 is used to calculate the driving cost of each driving path using a preset method, and select the driving path with the minimum driving cost as the target driving path.
[0117] As a possible implementation of the present application, in this implementation, obtaining road information collected by a laser radar includes:
[0118] Acquire the position information of the unmanned vehicle, wherein the position information includes the global positioning coordinates, yaw angle, pitch angle and roll angle of the unmanned vehicle;
[0119] The constructing a high-precision map based on the road information includes:
[0120] The rotation matrix of the point cloud to rigid body transformation is determined based on the posture information, and the global positioning coordinates collected by the laser radar are transformed into the same coordinate system through the rotation matrix to construct a three-dimensional environmental map of the road.
[0121] As a possible implementation of the present application, in this implementation, the rotation matrix of the point cloud to rigid body transformation is determined based on the posture information, and the global positioning coordinates collected by the laser radar are transformed into the same coordinate system through the rotation matrix to construct the three-dimensional environment map of the road, including:
[0122] The point cloud data collected by the laser radar at time t is F t = {P t1 ,P t2 …P tj}, the point cloud map of the road is
[0123] Among them, R t is a 3×3 rotation matrix, t t is the translation matrix, P tj A vector consisting of point cloud coordinates.
[0124] As a possible implementation of the present application, in this implementation, the method further includes:
[0125] Using the pose information as input of a preset IPC registration algorithm to extract the trunk point cloud of the road;
[0126] The backbone point cloud is aligned to determine the high-precision map.
[0127] As a possible implementation of the present application, in this implementation, determining obstacles on the driving path and using a state grid algorithm to calculate multiple driving paths from the current position to the target position includes:
[0128] Initialize the road space, determine the current position point, the target position point, the number of sampling points, and the step length between sampling points;
[0129] randomly generating intermediate points in the road space;
[0130] Selecting a sampling point closest to the middle point from among the sampling points as the target middle point;
[0131] Intercepting an intermediate path of the step length from the sampling point to the target intermediate point;
[0132] When there is no obstacle in the middle path, the middle path is determined as a section of the driving path.
[0133] As a possible implementation of the present application, in this implementation, the method further includes:
[0134] Calculating the curvature of the unmanned vehicle on the driving path;
[0135] The target driving path is constructed by selecting a path whose curvature satisfies the kinematic constraints of the unmanned vehicle itself.
[0136] The embodiment of the present application obtains road information through laser radar, and builds a high-precision map based on the road information, determines the current position of the unmanned vehicle in the high-precision map and the target position of the unmanned vehicle, can reduce the randomness of path planning, and can find a drivable path and derive the corresponding input information after the starting point and end point coordinates are given. When avoiding dynamic obstacles, the unmanned vehicle performs local path planning and can reasonably select a drivable path. According to the kinematic constraints derived from the reverse, such as speed, turning radius, path curvature, etc., the unmanned vehicle completes the motion path planning.
[0137] An electronic device is provided in an embodiment of the present application, and the electronic device includes: a memory and a processor; at least one program, stored in the memory, for obtaining road information collected by a laser radar when executed by the processor; building a high-precision map based on the road information; determining the current position of an unmanned vehicle in the high-precision map and the target position that the unmanned vehicle needs to reach; determining obstacles on the driving path, and using a state grid algorithm to calculate multiple driving paths from the current position to the target position; using a preset method to calculate the driving cost of each of the driving paths, and selecting the driving path with the smallest driving cost as the target driving path.
[0138] Compared with the prior art, the following can be achieved: the embodiment of the present application obtains road information through laser radar, and builds a high-precision map based on the road information, determines the current position of the unmanned vehicle in the high-precision map and the target position of the unmanned vehicle, can reduce the randomness of path planning, and can find a drivable path and derive the corresponding input information after the starting point and end point coordinates are given. When avoiding dynamic obstacles, the unmanned vehicle performs local path planning and can reasonably select a drivable path. According to kinematic constraints, such as speed, turning radius, path curvature, etc., the planned route is corrected.
[0139] In an alternative embodiment, an electronic device is provided, such as Figure 7 As shown, Figure 7 The electronic device 4000 shown includes: a processor 4001 and a memory 4003. The processor 4001 and the memory 4003 are connected, such as through a bus 4002. Optionally, the electronic device 4000 may also include a transceiver 4004. It should be noted that in actual applications, the transceiver 4004 is not limited to one, and the structure of the electronic device 4000 does not constitute a limitation on the embodiments of the present application.
[0140] Processor 4001 may be a CPU (Central Processing Unit), a general-purpose processor, a DSP (Digital Signal Processor), an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array) or other programmable logic devices, transistor logic devices, hardware components or any combination thereof. It may implement or execute various exemplary logic blocks, modules and circuits described in conjunction with the disclosure of this application. Processor 4001 may also be a combination that implements computing functions, such as a combination of one or more microprocessors, a combination of a DSP and a microprocessor, etc.
[0141] The bus 4002 may include a path to transmit information between the above components. The bus 4002 may be a PCI (Peripheral Component Interconnect) bus or an EISA (Extended Industry Standard Architecture) bus. The bus 4002 may be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 7 Only one thick line is used in the diagram, but this does not mean that there is only one bus or only one type of bus.
[0142] The memory 4003 may be a ROM (Read Only Memory) or other types of static storage devices that can store static information and instructions, a RAM (Random Access Memory) or other types of dynamic storage devices that can store information and instructions, or an EEPROM (Electrically Erasable Programmable Read Only Memory), a CD-ROM (Compact Disc Read Only Memory) or other optical disk storage, optical disk storage (including compressed optical disk, laser disk, optical disk, digital versatile disk, Blu-ray disk, etc.), a magnetic disk storage medium or other magnetic storage device, or any other medium that can be used to carry or store the desired program code having instructions or data structures and can be accessed by a computer, but is not limited to these.
[0143] The memory 4003 is used to store the application code for executing the solution of the present application, and the execution is controlled by the processor 4001. The processor 4001 is used to execute the application code stored in the memory 4003 to implement the contents shown in the above method embodiment.
[0144] The embodiment of the present application provides a computer-readable storage medium, on which a computer program is stored, and when the computer is run on a computer, the computer can execute the corresponding content in the aforementioned method embodiment. Compared with the prior art, the embodiment of the present application obtains the combined navigation data and the point cloud data measured by the laser radar respectively, and processes the point cloud data and the combined navigation data respectively, first determines the current posture information of the vehicle based on the combined navigation data, and determines the transformation matrix and translation matrix in the point cloud positioning, and then determines the laser radar posture information of the vehicle based on the transformation matrix and translation matrix, and then determines the posture calculation equation of the vehicle based on the laser radar posture information and the posture information of the vehicle, and then uses the extended Kalman filter method to locate the vehicle based on the posture state calculation equation, which is more in line with the driving law of the vehicle and ensures the accuracy of vehicle positioning.
[0145] It should be understood that, although the steps in the flowchart of the accompanying drawings are displayed in sequence as indicated by the arrows, these steps are not necessarily executed in sequence in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least a part of the steps in the flowchart of the accompanying drawings may include multiple sub-steps or multiple stages, and these sub-steps or stages are not necessarily executed at the same time, but can be executed at different times, and their execution order is not necessarily sequential, but can be executed in turn or alternately with other steps or at least a part of the sub-steps or stages of other steps.
[0146] The above description is only a partial implementation method of the present application. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present application. These improvements and modifications should also be regarded as the scope of protection of the present application.
Claims
1. An automatic driving path planning method, characterized in that: include: Obtain road information collected by LiDAR; Building a high-precision map based on the road information; Determine the current position of the unmanned vehicle in the high-precision map and the target position that the unmanned vehicle needs to reach; Determine obstacles on the driving path, and use a state grid algorithm to calculate multiple driving paths from the current position to the target position; Calculating the driving cost of each driving path by using a preset method, and selecting the driving path with the smallest driving cost as the target driving path; The obtaining of road information collected by the laser radar includes: Acquire the position information of the unmanned vehicle, wherein the position information includes the global positioning coordinates, yaw angle, pitch angle and roll angle of the unmanned vehicle; The constructing a high-precision map based on the road information includes: Determine the rotation matrix of the point cloud to rigid body transformation based on the posture information, and transform the global positioning coordinates collected by the laser radar into the same coordinate system through the rotation matrix to construct a three-dimensional environmental map of the road; The method of determining a rotation matrix of a point cloud to rigid body transformation based on the posture information, and transforming the global positioning coordinates collected by the laser radar into the same coordinate system through the rotation matrix to construct a three-dimensional environment map of the road includes: The point cloud data collected by the laser radar at time t is , the point cloud map of the road is ; in, for The rotation matrix of is the translation matrix, is the vector formed by the point cloud coordinates; Using the pose information as input of a preset IPC registration algorithm to extract the trunk point cloud of the road; Performing registration on the backbone point cloud to determine the high-precision map; The determining of obstacles on the driving path and calculating a plurality of driving paths from the current position to the target position using a state grid algorithm includes: Initialize the road space, determine the current position point, the target position point, the number of sampling points, and the step length between sampling points; randomly generating intermediate points in the road space; Selecting a sampling point closest to the middle point from among the sampling points as the target middle point; Intercepting an intermediate path of the step length from the sampling point to the target intermediate point; When there is no obstacle in the middle path, determining the middle path as a section of the driving path; Calculating the curvature of the unmanned vehicle on the driving path; Selecting a path whose curvature satisfies the kinematic constraints of the unmanned vehicle to construct the target driving path; Before calculating the curvature of the unmanned vehicle on the driving path, the method further includes: The state grid algorithm is used to guide the expansion domain; The RRT algorithm is used to search for a feasible path based on the current position point, the target position point, the number of sampling points, and the step length between sampling points.
2. An automatic driving path planning device, characterized in that: include: An information acquisition module is used to obtain road information collected by the laser radar; A map construction module, used to construct a high-precision map based on the road information; A position determination module, used to determine the current position of the unmanned vehicle in the high-precision map and the target position that the unmanned vehicle needs to reach; A path planning module, used to determine obstacles on the driving path and calculate multiple driving paths from the current position to the target position using a state grid algorithm; A path determination module, used to calculate the driving cost of each driving path by a preset method, and select the driving path with the smallest driving cost as the target driving path; The obtaining of road information collected by the laser radar includes: Acquire the position information of the unmanned vehicle, wherein the position information includes the global positioning coordinates, yaw angle, pitch angle and roll angle of the unmanned vehicle; The constructing a high-precision map based on the road information includes: Determine the rotation matrix of the point cloud to rigid body transformation based on the posture information, and transform the global positioning coordinates collected by the laser radar into the same coordinate system through the rotation matrix to construct a three-dimensional environmental map of the road; The method of determining a rotation matrix of a point cloud to rigid body transformation based on the posture information, and transforming the global positioning coordinates collected by the laser radar into the same coordinate system through the rotation matrix to construct a three-dimensional environment map of the road includes: The point cloud data collected by the laser radar at time t is , the point cloud map of the road is ; in, for The rotation matrix of is the translation matrix, is the vector formed by the point cloud coordinates; Using the pose information as input of a preset IPC registration algorithm to extract the trunk point cloud of the road; Performing registration on the backbone point cloud to determine the high-precision map; The determining of obstacles on the driving path and calculating a plurality of driving paths from the current position to the target position using a state grid algorithm includes: Initialize the road space, determine the current position point, the target position point, the number of sampling points, and the step length between sampling points; randomly generating intermediate points in the road space; Selecting a sampling point closest to the middle point from among the sampling points as the target middle point; Intercepting an intermediate path of the step length from the sampling point to the target intermediate point; When there is no obstacle in the middle path, determining the middle path as a section of the driving path; Calculating the curvature of the unmanned vehicle on the driving path; Selecting a path whose curvature satisfies the kinematic constraints of the unmanned vehicle to construct the target driving path; Before calculating the curvature of the unmanned vehicle on the driving path, the method further includes: The state grid algorithm is used to guide the expansion domain; The RRT algorithm is used to search for a feasible path based on the current position point, the target position point, the number of sampling points, and the step length between sampling points.
3. An electronic 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 program, the automatic driving path planning method according to claim 1 is implemented.
4. A computer-readable storage medium, characterized in that: The storage medium stores at least one instruction, at least one program, a code set or an instruction set, and the at least one instruction, the at least one program, the code set or the instruction set are loaded and executed by the processor to implement the method based on automatic driving path planning as described in claim 1.
Citation Information
Patent Citations
Driving path planning method based on state grid method
CN109263639A
Lidar three-dimensional mapping method based on semantic point cloud registration
CN109345574A
Driverless smart car automatic collision avoidance method based on quantum wolf pack algorithm
CN110471426A
Unmanned vehicle path planning method and device
CN111857160A
