Path planning and control method and system of intelligent vehicle and intelligent vehicle equipment

Abstract

The invention relates to a path planning and control method and system of an intelligent vehicle and intelligent vehicle equipment, and belongs to the technical field of control engineering.The method comprises the steps that an obtained starting point and an obtained target point serve as a starting node and a target node, and an improved # imgabs0 # algorithm is adopted to explore path nodes of the intelligent vehicle, carrying out cost value calculation on the explored nodes based on the constructed heuristic function considering the expansion radius of the obstacle, taking the node with the minimum cost value as a path node, and obtaining a global optimal path based on the path node; taking the global optimal path as a reference path, performing local path planning on the intelligent vehicle by adopting a moving window method to obtain a local optimal path, taking the local optimal path as a driving path of the intelligent vehicle, and controlling the intelligent vehicle based on the constructed motion chassis model to enable the intelligent vehicle to reach a target point. And the intelligent vehicle path planning efficiency and accuracy are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of control engineering, and particularly to a path planning and control method, system and intelligent vehicle device for an intelligent vehicle. Background Art

[0002] In recent years, the development of driverless technology has become increasingly mature, and the development of science and technology has been constantly advancing. Autonomous driving technology has become an important research direction in the field of transportation. The autonomous navigation ability of intelligent vehicles not only concerns the intelligent level of the vehicle itself, but also has a profound impact on the development of future intelligent transportation systems. As the core link for intelligent vehicles to achieve safe and efficient driving, path planning and control technology has gradually received more and more attention.

[0003] Although traditional path planning algorithms perform well in static environments, they often seem inadequate when faced with dynamic obstacles and real-time requirements. This limits the navigation ability of intelligent vehicles in complex environments and makes it difficult to meet the needs of practical applications.

[0004] Therefore, when using traditional path planning algorithms to plan paths for intelligent vehicles in complex environments, there are problems of low efficiency and accuracy. Summary of the Invention

[0005] In view of this, it is necessary to provide a path planning and control method, system and intelligent vehicle device for an intelligent vehicle to solve the technical problems of low path planning efficiency and accuracy of intelligent vehicles in complex environments.

[0006] To solve the above problems, in a first aspect, the present invention provides a path planning and control method for an intelligent vehicle, including: Taking the obtained starting point and target point as the starting node and target node, and using an improved algorithm to explore the path nodes of the intelligent vehicle. Among them, based on the constructed heuristic function considering the obstacle expansion radius, the cost value of the explored nodes is calculated, and the node with the minimum cost value is used as the path node. Based on the path node, a globally optimal path is obtained; Taking the globally optimal path as the reference path, using the moving window method to perform local path planning for the intelligent vehicle to obtain a locally optimal path; Taking the locally optimal path as the driving path of the intelligent vehicle, and controlling the intelligent vehicle based on the constructed motion chassis model to make the intelligent vehicle reach the target point.

[0007] In a possible implementation manner, the improved algorithm includes an open list and a closed list; taking the obtained starting point and target point as the starting node and target node, and using the improved The algorithm explores the path nodes of the intelligent vehicle. Among them, based on the constructed heuristic function considering the obstacle expansion radius, the cost value of the explored nodes is calculated, and the node with the minimum cost value is used as the path node. Based on the path node, a globally optimal path is obtained, including: Step 1: Initialize the environment map, divide the environment map into a grid map, and determine the starting point, target point, and obstacles of the intelligent vehicle in the grid map, and use the starting point and target point as the starting node and target node; Step 2: Construct an improved heuristic function of the algorithm considering Euclidean distance, obstacle expansion radius, and cost growth rate; Step 3: Determine the nodes to be explored based on the grid map, store the nodes to be explored in the open list, and store the starting node and the target node in the closed list. Among them, the nodes to be explored include the starting node, the target node, and the obstacles; Step 4: Determine the exploration direction based on the starting node and the target node, and set exploration rules based on the exploration direction. Among them, the exploration rule is five-direction exploration; Step 5: Use the starting node as the first parent node, and based on the exploration rules, use the improved algorithm to expand the nodes, obtain multiple expanded nodes, determine whether the multiple expanded nodes are obstacles. When the multiple expanded nodes are not obstacles, add the multiple expanded nodes to the open list, calculate the cost values of the multiple expanded nodes through the heuristic function, and determine the expanded node with the minimum cost value, and store the expanded node with the minimum cost value in the closed list; Step 6: Use the expanded node with the minimum cost value as the second parent node, repeat Step 5 until the target node is explored, then stop the exploration to obtain multiple parent nodes; Step 7: Obtain the global path nodes based on the multiple parent nodes, and optimize the global path nodes to obtain the globally optimal path.

[0008] In a possible implementation manner, the heuristic function is: Among them, is the heuristic function, is the actual cost from the starting point to the current node, is the maximum cost, is the Euclidean distance, is the obstacle expansion radius, is the obstacle expansion radius, is the cost growth rate.

[0009] In a possible implementation, optimizing the global path nodes to obtain the globally optimal path includes: When three adjacent nodes in the global path nodes form a straight line, after removing the middle node among the three adjacent nodes, globally optimal path nodes are obtained, and the globally optimal path is obtained based on the globally optimal path nodes.

[0010] In a possible implementation, using the globally optimal path as a reference path and adopting a moving window method to perform local path planning for the intelligent vehicle to obtain the locally optimal path includes: Determine the speed range of the intelligent vehicle. Taking the globally optimal path as a reference path, based on the speed range of the intelligent vehicle, set a dynamic window through the moving window method, sample the speed of the intelligent vehicle in the dynamic window, and obtain multiple local obstacle avoidance paths; Use an evaluation function to evaluate the multiple local obstacle avoidance paths to obtain the total evaluation value of the local obstacle avoidance paths; Determine the optimal trajectory based on the total evaluation value, and obtain the locally optimal path based on the optimal trajectory.

[0011] In a possible implementation, the evaluation function is: , where is the evaluation function, is the yaw angle evaluation sub-function, is the safety factor evaluation sub-function, is the speed evaluation function, is the smoothing function, , , are the weighting coefficients, is the linear acceleration, is the angular acceleration, is the th trajectory.

[0012] In a possible implementation, using the locally optimal path as the driving path of the intelligent vehicle and controlling the intelligent vehicle to reach the target point based on the constructed motion chassis model includes: Construct a kinematic model of a pure path tracking algorithm based on the motion chassis model; Determine the current position of the intelligent vehicle. Based on the current position of the intelligent vehicle, dynamically select a preview point on the locally optimal path using the pure path tracking algorithm, and calculate the target steering angle between the current position of the intelligent vehicle and the preview point through the kinematic model; Obtain the actual steering angle through the steering servo encoder of the intelligent vehicle, determine the steering angle error based on the target steering angle and the actual steering angle, perform PID operation on the steering angle error using the PID algorithm to generate a PWM control signal, and control the steering servo of the intelligent vehicle based on the PWM control signal to make the intelligent vehicle reach the target point.

[0013] In a possible implementation manner, the motion chassis model is: , where, is the longitudinal speed of the vehicle's rear wheels, is the lateral speed of the vehicle's rear wheels, is the yaw angular velocity of the vehicle, is the yaw angle of the intelligent vehicle, is the cosine value of the vehicle's yaw angle, is the sine value of the vehicle's yaw angle, is the tangent value of the front wheel steering angle, is the front wheel steering angle, is the combined speed of the vehicle's rear wheels; The PID algorithm is: , where, is the steering angle of the steering servo, is the target steering angle, is the previous steering angle, is the proportional coefficient, is the differential coefficient.

[0014] In a second aspect, the present invention also provides a path planning and control system for an intelligent vehicle, including: A global path planning module, which is used to explore the path nodes of the intelligent vehicle by using an improved algorithm with the obtained starting point and target point as the starting node and target node. Among them, the cost value of the explored nodes is calculated based on the constructed heuristic function considering the obstacle expansion radius, and the node with the minimum cost value is used as the path node, and the global optimal path is obtained based on the path node; A local path planning module, which is used to perform local path planning on the intelligent vehicle by using the moving window method with the global optimal path as the reference path to obtain the local optimal path; An intelligent vehicle control module, which is used to control the intelligent vehicle based on the constructed motion chassis model with the local optimal path as the driving path of the intelligent vehicle to make the intelligent vehicle reach the target point.

[0015] In a third aspect, the present invention also provides an intelligent vehicle device, including: a processor and a memory; A computer-readable program executable by the processor is stored on the memory; When the processor executes the computer-readable program, the steps in the path planning and control method of the intelligent vehicle as described above are implemented.

[0016] The beneficial effects of the present invention are: An improved algorithm is used to explore the path nodes of the intelligent vehicle to obtain the globally optimal path. Through an improved algorithm, the obstacle expansion coefficient is fused to smooth the area near the obstacle, making the planned path safer. Through an improved algorithm for improving the exploration rules, the efficiency of path planning is improved. Taking the globally optimal path as the reference path, the local path planning of the intelligent vehicle is carried out by using the moving window method. Through local planning, the path is more in line with the global planning. The improved algorithm is fused with the moving window method to plan the path of the intelligent vehicle, enabling the intelligent vehicle to maintain global optimality during driving and also achieve the effect of dynamic obstacle avoidance, improving the efficiency and accuracy of path planning for the intelligent vehicle in a complex environment. Taking the locally optimal path as the driving path of the intelligent vehicle, the intelligent vehicle is controlled based on the constructed motion chassis model, achieving accurate position control and greatly improving the safety performance of the intelligent vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required for description in the embodiments. Obviously, the following described drawings are only some embodiments of the present invention. For those skilled in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0018] Figure 1 It is a flowchart of an embodiment of the path planning and control method of the intelligent vehicle provided by the present invention; Figure 2 It is a flowchart of an embodiment of the global path planning of the path planning and control method of the intelligent vehicle provided by the present invention; Figure 3 It is a schematic diagram of the node movement direction of the path planning and control method of the intelligent vehicle provided by the present invention; Figure 4 It is a schematic diagram of the motion chassis model of the path planning and control method of the intelligent vehicle provided by the present invention; Figure 5 It is a schematic diagram of the kinematic model of the path planning and control method of the intelligent vehicle provided by the present invention; Figure 6 It is a schematic diagram of the map of the path planning and control method of the intelligent vehicle provided by the present invention; Figure 7 Schematic diagram of the motion trajectory of the intelligent vehicle for the path planning and control method of the intelligent vehicle provided by the present invention; Figure 8 Schematic diagram of the structure of an embodiment of the path planning and control system of the intelligent vehicle provided by the present invention; Figure 9 Schematic diagram of the structure of an embodiment of the intelligent vehicle device provided by the present invention. Detailed implementation manners

[0019] The following will specifically describe the preferred embodiments of the present invention in conjunction with the accompanying drawings. The accompanying drawings form a part of this application and are used together with the embodiments of the present invention to explain the principles of the present invention, rather than to limit the scope of the present invention.

[0020] Referring to "embodiment" herein means that the specific features, structures or characteristics described in conjunction with the embodiment may be included in at least one embodiment of the present invention. The phrase appears in various positions in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment mutually exclusive with other embodiments. Those skilled in the art explicitly and implicitly understand that the embodiments described herein can be combined with other embodiments.

[0021] Before presenting the embodiments, the following terms will be explained first.

[0022] Algorithm: A classic heuristic search algorithm, mainly used to solve the shortest path planning problem. By introducing a heuristic function to guide the search direction, it can greatly improve the search efficiency while ensuring the optimality of the path.

[0023] Dynamic Window Approach (DWA): The dynamic window method is a local path planning algorithm, an autonomous obstacle avoidance algorithm derived from the analysis of the robot kinematic model.

[0024] Pure Path Tracking Algorithms: A class of algorithms used to control a mobile robot or vehicle to strictly follow a preset path. Its core idea is to obtain the deviation between the current pose and the target path through sensors or positioning systems, and generate control instructions to eliminate the deviation, achieving precise path tracking, focusing on the stable following of local paths, and applicable to structured environments or the execution of a pre-planned global path.

[0025] The present invention discloses a path planning and control method, system and intelligent vehicle device for an intelligent vehicle, which can be used in a computer. The method, device or computer-readable storage medium involved in the present invention can either be integrated with the above devices or be relatively independent.

[0026] A specific embodiment of the present invention discloses a path planning and control method for an intelligent vehicle, which can be executed by a computer, specifically by one or more processors of the computer. As Figure 1 shown, the path planning and control method for the intelligent vehicle includes: S101. Using the obtained starting point and target point as the starting node and target node, and adopting an improved algorithm to explore the path nodes of the intelligent vehicle. Among them, based on the constructed heuristic function considering the expansion radius of obstacles, the cost value of the explored nodes is calculated, and the node with the minimum cost value is used as the path node. Based on the path nodes, a globally optimal path is obtained; It should be noted that through the environmental map, the starting point, target point, and obstacles are determined; considering the expansion radius of the obstacles and the cost growth rate of the ROS intelligent vehicle to improve the heuristic function of the algorithm, and improving the exploration direction of the algorithm to construct an improved algorithm. Using the starting point and target point as the starting node and target node, the improved algorithm is used to perform global path planning for the intelligent vehicle, improving the efficiency of path planning.

[0027] S102. Using the globally optimal path as the reference path, adopting the moving window method to perform local path planning for the intelligent vehicle to obtain a locally optimal path; It should be noted that on the basis of global path planning, the moving window method is used to perform local path planning for the intelligent vehicle, making the local plan make the path more conform to the global plan.

[0028] S103. Using the locally optimal path as the driving path of the intelligent vehicle, and controlling the intelligent vehicle based on the constructed motion chassis model to make the intelligent vehicle reach the target point; It should be noted that on the basis of local planning, based on the constructed motion chassis model, the pure path tracking algorithm and the PID algorithm are used to perform cooperative control on the intelligent vehicle, realizing accurate position control.

[0029] In some embodiments, in step S101, using the obtained starting point and target point as the starting node and target node, and adopting an improved algorithm to explore the path nodes of the intelligent vehicle. Among them, based on the constructed heuristic function considering the expansion radius of obstacles, the cost value of the explored nodes is calculated, and the node with the minimum cost value is used as the path node. Based on the path nodes, a globally optimal path is obtained. The improved algorithm includes an open list and a closed list. For the flowchart of using the improved algorithm to perform global path planning for the intelligent vehicle, please refer to Figure 2 , including: S1011, Step 1, Initialize the environmental map, divide the environmental map into a grid map, determine the starting point, target point and obstacles of the intelligent vehicle in the grid map, and use the starting point and target point as the starting node and target node; Initialize the environmental map, divide the environmental map into a grid map, determine the position coordinates of the starting point, target point and obstacles of the intelligent vehicle in the grid map, and use the starting point and target point of the intelligent vehicle as the starting node and target node for path planning.

[0030] S1012, Step 2, Construct an improved heuristic function of the algorithm considering Euclidean distance, obstacle inflation radius and cost growth rate; Construct an improved heuristic function of the algorithm considering Euclidean distance, obstacle inflation radius and cost growth rate; The heuristic function of the traditional algorithm is: where is the actual cost, is the cost function. Since the heuristic function of the traditional algorithm is Euclidean distance, and Euclidean distance does not specifically handle obstacles or other non-linear factors and cannot be used in complex environments. By optimizing the cost function from the current position to the target point, the improvement of the algorithm is realized. The heuristic function of its improved algorithm is: where is the heuristic function, is the actual cost from the starting point to the current node, is the maximum cost, is the Euclidean distance, is the obstacle inflation radius, adding a safety distance through the obstacle inflation radius, is the cost growth rate.

[0031] S1013, Step 3, Determine the nodes to be explored based on the grid map, store the nodes to be explored in the open list, and store the starting node and target node in the closed list. Among them, the nodes to be explored include the starting node, target node and obstacles; Each grid in the grid map represents a node to be explored. Store all the nodes to be explored in the grid map in the open list, and store the starting node and target node in the closed list. Among them, the nodes to be explored also include the starting node, target node and obstacles.​​

[0032] S1014, Step 4: Determine the exploration direction based on the starting node and the target node, and set the exploration rules based on the exploration direction. Among them, the exploration rule is five-direction exploration; Traditional When performing algorithm path planning, it will expand the 8 grids around the node. That is, the movement direction of the node is eight directions. For the schematic diagram of the movement direction of the node, please refer to Figure 3 , such as Figure 3 As shown, an O randomly selected in the grid map is used as the parent node, and the adjacent grid map is the movable direction, which are the directions of the 8 child nodes m1 - m8 respectively; In the improved When performing path planning by the algorithm, the five-direction exploration rule is adopted, that is, the search directions of five points are retained. As shown in Figure 3 , taking m2 and m7 as the Y-axis, and m4 and m5 as the X-axis, it is divided into two parts, that is, the right side of the X-axis or the left side of the X-axis; and the left side of the Y-axis or the right side of the Y-axis. When the target point is at any position, the 3 search points behind the origin are deleted. That is, when the direction from the parent node O to the child node m2 is the exploration direction, the 3 search points m6 - m8 are deleted, which can reduce the number of search points to optimize the search efficiency.

[0033] S1015, Step 5: Taking the starting node as the first parent node, based on the exploration rules, use the improved algorithm to expand the node, obtain multiple expanded nodes, determine whether the multiple expanded nodes are obstacles. When the multiple expanded nodes are not obstacles, add the multiple expanded nodes to the open list, calculate the cost values of the multiple expanded nodes through the heuristic function, and determine the expanded node with the minimum cost value, and store the expanded node with the minimum cost value in the closed list; Taking the starting node as the first parent node, that is, the initial node for exploration by the improved algorithm. Based on the initial node, expand the node according to the five-direction exploration rule to obtain expanded nodes, determine whether the expanded nodes are obstacles. When the expanded nodes are not obstacles, add the expanded nodes to the open list, and calculate the cost values of the expanded nodes through the heuristic function of the improved algorithm, and save the node with the minimum cost value in the closed list.

[0034] S1016, Step 6: Taking the expanded node with the minimum cost value as the second parent node, repeat Step 5 until the target node is explored, then stop the exploration to obtain multiple parent nodes; After calculating the cost values of the expanded nodes through the heuristic function, taking the node with the minimum cost value as the second parent node, taking the second parent node as the base node of the node to be expanded, using the improved After the algorithm expands the nodes, the cost value of the expanded nodes is calculated through the heuristic function. The node with the smallest cost value among the expanded nodes is used as the third parent node. Repeat step 5 to obtain the fourth parent node, the fifth parent node, and so on until the target node is explored, at which point the exploration stops to obtain multiple parent nodes.

[0035] S1017, Step 7: Obtain global path nodes based on the multiple parent nodes, and optimize the global path nodes to obtain the globally optimal path; Obtain global path nodes based on multiple parent nodes, and optimize the global path nodes to obtain the globally optimal path. Since there are redundant points in the global path nodes, the global path nodes are optimized. Traverse all the nodes on the path. If there are extra nodes on a straight line, keep the first and last nodes and delete the rest. The optimization process is as follows: when three adjacent nodes in the global path nodes are on a straight line, after removing the middle node among the three adjacent nodes, obtain the globally optimal path nodes, and obtain the globally optimal path based on the globally optimal path nodes.

[0036] Improvement The specific process of the global path planning of the algorithm is as follows: First, initialize the environmental map, divide the map into a grid, determine the position coordinates of the starting node and the target node of the ROS intelligent vehicle, and scan the obstacle position information through a 2D radar, etc.; create two empty lists OPEN for the starting node and the target node respectively, namely the starting node list OPEN and the target node list OPEN, to store the nodes to be explored, and add the starting node and the target node to the starting node list OPEN and the target node list OPEN respectively; at the same time, create two empty lists CLOSED for the starting node and the target node respectively, namely the starting node list CLOSED and the target node list CLOSED, to store the nodes that have been explored; through the improved The search points of the algorithm expand nodes with the starting node as the parent node, and determine whether the expanded node is an obstacle. If it is an obstacle, it is an infeasible node. If not, the expanded node is added to the corresponding OPEN list; calculate the cost value of the expanded node according to the heuristic function, and select the expanded node with the minimum cost value in the OPEN list and add it to the corresponding CLOSED list, and traverse its adjacent nodes again with this expanded node as the parent node. This process is looped until the starting node and the target node exist in the target node list CLOSED and the starting node list CLOSED respectively, which means the path is found and the search process ends. Trace back the parent nodes of the nodes to obtain the global path nodes; then through the searched nodes, if it is found that three nodes are on the same straight line, keep the first and the last nodes in the OPEN list. The core idea is that if a node has been expanded and no better path has been found, then this node will not be further expanded, thus avoiding redundant calculations.

[0037] In some embodiments, in step S102, with the globally optimal path as the reference path, the local path planning of the intelligent vehicle is performed by using the moving window method to obtain the locally optimal path; determine the speed range of the intelligent vehicle, with the globally optimal path as the reference path, obtain the starting point, the target point and the moving obstacles through the reference path, based on the speed range of the intelligent vehicle, set the dynamic window by using the moving window method, sample the speed of the intelligent vehicle in the dynamic window to obtain multiple local obstacle avoidance paths, sample within the dynamic window to obtain multiple sets of sampled speeds. For each set of sampled speeds, simulate the motion trajectory of the intelligent vehicle from the current position to a future period of time at these speeds, and predict the position and moving trajectory of the dynamic obstacles within the planning period to obtain the sampled trajectories, that is, multiple local obstacle avoidance paths. Use the evaluation function to evaluate the multiple local obstacle avoidance paths to obtain the total evaluation value of the local obstacle avoidance paths. The moving window method only considers the linear speed and angular speed of the intelligent vehicle, and uses the cost function to evaluate the sampled trajectories, which also considers the linear speed and angular speed of the intelligent vehicle. Its evaluation function is: , where, is the evaluation function, is the deflection angle evaluation sub-function, is the direction between the current target point and the trajectory target point, is the safety factor evaluation sub-function, is the Euclidean distance between the trajectory and the obstacle, is the speed evaluation function, is the speed of the intelligent vehicle, is the smoothing function, , , is the weighting coefficient, is the linear acceleration, is the angular acceleration, is the th trajectory; Normalize the yaw angle evaluation sub-function, safety factor evaluation sub-function, and speed evaluation function in the evaluation function: , , , where, is the total number constraint condition of the sampled trajectories. Determine the optimal trajectory based on the total evaluation value, obtain the local optimal path based on the optimal trajectory. After evaluating all local obstacle avoidance paths through the evaluation function, select the trajectory with the lowest total evaluation value as the optimal trajectory. When the robot successfully reaches the target point according to the selected optimal trajectory, the path planning stops, and its optimal trajectory is used as the driving path of the intelligent vehicle; due to the uncertainty of dynamic obstacles, in order to enable the intelligent vehicle to stably avoid obstacles, set a safety distance obstacle within a certain range, that is, set a dilation coefficient within a certain range, and its dilation coefficient is set according to the moving speed of the obstacle.

[0038] In some embodiments, in step S103, use the local optimal path as the driving path of the intelligent vehicle, and control the intelligent vehicle based on the constructed motion chassis model to make the intelligent vehicle reach the target point. Construct the motion chassis model of the intelligent vehicle according to the Ackermann vehicle chassis model of the ROS intelligent vehicle. For the schematic diagram of its motion chassis model, please refer to Figure 4 , and its motion chassis model is: , , , where, is the longitudinal speed of the vehicle's rear wheels, is the lateral speed of the vehicle's rear wheels, is the yaw angular velocity of the vehicle, is the yaw angle of the intelligent vehicle, is the cosine value of the vehicle's yaw angle, is the sine value of the vehicle's yaw angle, is the tangent value of the front wheel steering angle, is the front wheel steering angle, is the combined speed of the vehicle's rear wheels, is the distance between the front and rear axles of the intelligent vehicle, is the distance between the two rear wheels of the intelligent vehicle, is the vehicle front wheel corner value, is the linear velocity of the left rear wheel of the intelligent vehicle, is the linear velocity of the right rear wheel of the intelligent vehicle, is the linear velocity of the intelligent vehicle, is the angular velocity of the intelligent vehicle; Based on the motion chassis model, a kinematic model of the pure path tracking algorithm is constructed. For the schematic diagram of the kinematic model, please refer to Figure 5 to determine the current position of the intelligent vehicle. Based on the current position of the intelligent vehicle, a preview point on the locally optimal path is dynamically selected using the pure path tracking algorithm The preview point is a point on the locally optimal path, usually located at the look-ahead distance of the intelligent vehicle. The look-ahead distance is the preview distance The preview distance is obtained by calculating the Euclidean distance between the position of the intelligent vehicle and the preview point. The preview distance can be dynamically adjusted at different positions at different times. Usually, the higher the speed, the larger the preview distance. The target steering angle between the current position of the intelligent vehicle and the preview point is calculated through the kinematic model As Figure 5 shown, the intelligent vehicle moves in a two-dimensional plane and rotates around the Z-axis, is the steering angle of the intelligent vehicle, is the wheelbase of the intelligent vehicle, is the turning radius, is the angle between the vector connecting the rear axle of the intelligent vehicle and the preview point and the heading angle; The steering angle is: , , where, is the curvature, The actual steering angle is obtained through the steering servo encoder of the intelligent vehicle. Based on the target steering angle and the actual steering angle, the steering angle error is determined. The PID algorithm is used to perform PID operation on the steering angle error to generate a PWM control signal. Based on the PWM control signal, the steering servo of the intelligent vehicle is controlled to make the intelligent vehicle reach the target point. The PID algorithm is: , where, is the steering angle of the steering servo, is the target steering angle, is the previous steering angle, is the proportional coefficient, is the differential coefficient.

[0039] After calculating the steering angle of the intelligent vehicle through the aiming point of the pure path tracking algorithm, the steering angle of the steering gear of the intelligent vehicle is controlled by PID, so that the intelligent vehicle reaches the target point. The PID control includes the position PID and the incremental PID. The rotational angular velocity of the intelligent vehicle is controlled by the position PID and the incremental PID. Only the PD control steering output is applied. The proportional control uses the position PD to adjust the current error output, while the differential term adjusts the output according to the change of the error of the incremental PD. The position PID controller calculates a control output value according to the error between the current position and the target position (preview point), which is used to adjust the system to move towards the target position. For the control output value, a limit processing is performed according to the set target speed to ensure that the control signal will not be too large. The control signal after the limit processing is subjected to position integration to better track the change of the target position; the incremental PID controller corresponds to the increment ∆Uk of the controlled object, rather than the actual control quantity size. Therefore, the steering angle of the steering gear is controlled by the incremental PD. Finally, the single-chip microcomputer outputs a PWM signal, and the control system executes the corresponding actions to achieve position control. In this way, combining the position PD algorithm and the incremental PD algorithm can control the intelligent vehicle to achieve accurate position control. At the end of each control cycle, the target position of the current cycle is regarded as the starting position of the next cycle, so as to achieve continuous position control, and then the intelligent vehicle is stably driven towards the target position.

[0040] The improved algorithm combines the dynamic window method and the pure path tracking algorithm with the PID control algorithm and applies it to the ROS intelligent vehicle to conduct a random obstacle avoidance experiment to verify the effectiveness of the algorithm. The map is constructed according to the environmental map. For the schematic diagram of the map, please refer to Figure 6 For the schematic diagram of the motion trajectory of the intelligent vehicle, please refer to Figure 7 After setting the target point in the map, the global path from the starting point to the target point is planned. When the intelligent vehicle is at the starting point position, random obstacles within the range of the radar characteristics can be scanned. The improved algorithm plans a safer path, smooths the area near the obstacles in the path planning, and makes the path more conform to the global planning through local planning. The intelligent vehicle can avoid obstacles in time for the obstacles through the improved algorithm and the control algorithm, can safely bypass the obstacle area, and conform to the global path planning.

[0041] In summary, the path planning and control method of the intelligent vehicle provided by the present invention takes the obtained starting point and target point as the starting node and the target node, and adopts the improved The algorithm explores the path nodes of the intelligent vehicle. Among them, based on the constructed heuristic function considering the expansion radius of obstacles, the cost value of the explored nodes is calculated, and the node with the minimum cost value is used as the path node. Based on the path nodes, the global optimal path is obtained. Using the global optimal path as the reference path, the local path planning of the intelligent vehicle is carried out by the moving window method to obtain the local optimal path. Using the local optimal path as the driving path of the intelligent vehicle, the intelligent vehicle is controlled based on the constructed motion chassis model, so that the intelligent vehicle reaches the target point, improving the efficiency and accuracy of the path planning of the intelligent vehicle.

[0042] In order to better implement the path planning and control method of the intelligent vehicle in the embodiments of the present invention, correspondingly, based on the path planning and control method of the intelligent vehicle, as Figure 8 shown, the embodiments of the present invention also provide a path planning and control system for an intelligent vehicle. The path planning and control system 800 of the intelligent vehicle includes: A global path planning module 801, which is used to take the obtained starting point and target point as the starting node and target node, and use an improved algorithm to explore the path nodes of the intelligent vehicle. Among them, based on the constructed heuristic function considering the expansion radius of obstacles, the cost value of the explored nodes is calculated, and the node with the minimum cost value is used as the path node. Based on the path nodes, the global optimal path is obtained; A local path planning module 802, which is used to take the global optimal path as the reference path and use the moving window method to perform local path planning on the intelligent vehicle to obtain the local optimal path; An intelligent vehicle control module 803, which is used to take the local optimal path as the driving path of the intelligent vehicle and control the intelligent vehicle based on the constructed motion chassis model, so that the intelligent vehicle reaches the target point.

[0043] As Figure 9 shown, the present invention also correspondingly provides an intelligent vehicle device 900. The intelligent vehicle device 900 can be a computing device such as a mobile terminal, a desktop computer, a notebook, a palm computer, and a server. The intelligent vehicle device 900 includes a processor 901, a memory 902, and a display 903. Figure 9 Only some components of the intelligent vehicle device 900 are shown, but it should be understood that it is not required to implement all the shown components, and more or fewer components can be alternatively implemented.

[0044] The memory 902 can be an internal storage unit of the intelligent vehicle device 900 in some embodiments, such as the hard disk or memory of the intelligent vehicle device 900. The memory 902 can also be an external storage device of the intelligent vehicle device 900 in other embodiments, such as a plug-in hard disk equipped on the intelligent vehicle device 900, a Smart Media Card (SMC), a Secure Digital (SD) card, a Flash Card, etc. Further, the memory 902 can also include both the internal storage unit of the intelligent vehicle device 900 and the external storage device. The memory 902 is used to store the application software installed on the intelligent vehicle device 900 and various types of data, such as the program code installed on the intelligent vehicle device 900. The memory 902 can also be used to temporarily store the data that has been output or will be output. In one embodiment, a path planning and control program of the intelligent vehicle is stored on the memory 902, and the path planning and control program of the intelligent vehicle can be executed by the processor 901, so as to implement the path planning and control method of the intelligent vehicle in various embodiments of the present invention.

[0045] The processor 901 can be a central processing unit (CPU), a microprocessor or other data processing chips in some embodiments, and is used to run the program code stored in the memory 902 or process data, such as the path planning and control method of the intelligent vehicle.

[0046] The display 903 can be an LED display, a liquid crystal display, a touch liquid crystal display, and an OLED (Organic Light-Emitting Diode) toucher, etc. in some embodiments. The display 903 is used to display the identification information of the path planning and control program of the intelligent vehicle and to display a visual user interface. The components 901-903 of the intelligent vehicle device 900 communicate with each other through the system bus.

[0047] In some embodiments, when the processor 901 executes the path planning and control program of the intelligent vehicle in the memory 902, each step in the path planning and control method of the intelligent vehicle as described in the above embodiments is implemented. Since the path planning and control method of the intelligent vehicle has been described in detail above, it will not be repeated here.

[0048] Correspondingly, the present invention also provides a computer-readable storage medium. The computer-readable storage medium is used to store computer-readable programs or instructions. When the programs or instructions are executed by a processor, the steps or functions in the path planning and control method of the intelligent vehicle provided in the above method embodiments can be implemented.

[0049] Those skilled in the art can understand that all or part of the processes of implementing the methods in the above embodiments can be completed by instructing relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium. Among them, the computer-readable storage medium is a disk, an optical disc, a read-only memory, a random access memory, etc.

[0050] As mentioned above, the above are only the preferred specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A path planning and control method for an intelligent vehicle, characterized in that: include: The starting point and target point are taken as the starting node and target node, and the improved The algorithm explores the path nodes of the smart car, wherein the cost value of the explored nodes is calculated based on the constructed heuristic function considering the expansion radius of the obstacle, and the node with the smallest cost value is used as the path node, and the global optimal path is obtained based on the path node; Taking the global optimal path as a reference path, a moving window method is used to perform local path planning on the smart car to obtain a local optimal path; The local optimal path is used as the driving path of the smart car, and the smart car is controlled based on the constructed motion chassis model to make the smart car reach the target point.

2. The path planning and control method of the intelligent vehicle according to claim 1, characterized in that: The improved The algorithm includes an open list and a closed list; the starting point and the target point are taken as the starting node and the target node, and the improved The algorithm explores the path nodes of the smart car, wherein the cost value of the explored nodes is calculated based on the constructed heuristic function considering the expansion radius of the obstacle, and the node with the smallest cost value is used as the path node, and the global optimal path is obtained based on the path node, including: Step 1: Initialize the environment map, divide the environment map into a grid map, determine the starting point, target point and obstacles of the smart car in the grid map, and use the starting point and target point as the starting node and target node; Step 2: Construct an improved The algorithm's heuristic function; Step 3: determining nodes to be explored based on the grid map, storing the nodes to be explored in an open list, and storing the start node and the target node in a closed list, wherein the nodes to be explored include the start node, the target node, and obstacles; Step 4: determining an exploration direction based on the starting node and the target node, and setting an exploration rule based on the exploration direction, wherein the exploration rule is a five-direction exploration; Step 5: Taking the starting node as the first parent node, based on the exploration rule, adopt the improved The algorithm expands the node to obtain multiple expanded nodes, determines whether the multiple expanded nodes are obstacles, and when the multiple expanded nodes are not obstacles, adds the multiple expanded nodes to the open list, calculates the cost values ​​of the multiple expanded nodes through the heuristic function, determines the expanded node with the smallest cost value, and stores the expanded node with the smallest cost value in the closed list; Step 6: Take the extended node with the smallest cost value as the second parent node, repeat step 5 until the target node is explored, and stop exploring to obtain multiple parent nodes; Step 7: Obtain a global path node based on the multiple parent nodes, optimize the global path node, and obtain a global optimal path.

3. The path planning and control method of the intelligent vehicle according to claim 2, characterized in that: The heuristic function is: ， in, is the heuristic function, is the actual cost from the starting point to the current node, For the maximum cost, is the Euclidean distance, is the obstacle expansion radius, At the expense of growth rate.

4. The path planning and control method of the smart car according to claim 2, characterized in that: The step of optimizing the global path nodes to obtain a global optimal path includes: When three adjacent nodes in the global path node are directly connected, after removing the middle nodes in the three adjacent nodes, a global optimal path node is obtained, and a global optimal path is obtained based on the global optimal path node.

5. The path planning and control method of the intelligent vehicle according to claim 2, characterized in that: The method of taking the global optimal path as a reference path and adopting a moving window method to perform local path planning on the smart car to obtain a local optimal path includes: Determine the speed range of the smart car, take the global optimal path as a reference path, set a dynamic window based on the speed range of the smart car by a moving window method, sample the speed of the smart car in the dynamic window, and obtain multiple local obstacle avoidance paths; Using an evaluation function to evaluate the multiple local obstacle avoidance paths to obtain a total evaluation value of the local obstacle avoidance paths; An optimal trajectory is determined based on the total evaluation value, and a local optimal path is obtained based on the optimal trajectory.

6. The path planning and control method of the intelligent vehicle according to claim 5, characterized in that: The evaluation function is: ， in, is the evaluation function, is the deflection angle evaluation subfunction, is the safety factor evaluation subfunction, is the speed evaluation function, is a smooth function, , , is the weighting coefficient, is the linear acceleration, is the angular acceleration, For the A trajectory.

7. The path planning and control method of the intelligent vehicle according to claim 5, characterized in that: The method of taking the local optimal path as the driving path of the smart car and controlling the smart car based on the constructed motion chassis model so that the smart car reaches the target point includes: Construct a kinematic model of a pure path-following algorithm based on a kinematic chassis model; Determine the current position of the smart car, dynamically select a preview point on the local optimal path using a pure path tracking algorithm based on the current position of the smart car, and calculate the target steering angle between the current position of the smart car and the preview point using the kinematic model; The actual steering angle is obtained through the servo encoder of the smart car, the steering angle error is determined based on the target steering angle and the actual steering angle, the steering angle error is PID operated by a PID algorithm to generate a PWM control signal, and the servo of the smart car is controlled based on the PWM control signal to enable the smart car to reach the target point.

8. The path planning and control method of the intelligent vehicle according to claim 7, characterized in that: The sports chassis model is: ， in, is the longitudinal velocity of the rear wheels of the vehicle, is the lateral velocity of the rear wheels of the vehicle, is the yaw rate of the vehicle, is the yaw angle of the smart car, is the cosine value of the vehicle's yaw angle, is the sine value of the vehicle yaw angle, is the tangent of the front wheel steering angle, is the front wheel steering angle, is the resultant speed of the rear wheels of the vehicle; The PID algorithm is: ， in, is the steering angle of the servo, is the target steering angle, is the last steering angle, is the proportionality coefficient, is the differential coefficient.

9. A path planning and control system for an intelligent vehicle, characterized in that: include: The global path planning module is used to obtain the starting point and target point as the starting node and target node, using the improved The algorithm explores the path nodes of the smart car, wherein the cost value of the explored nodes is calculated based on the constructed heuristic function considering the expansion radius of the obstacle, and the node with the smallest cost value is used as the path node, and the global optimal path is obtained based on the path node; A local path planning module is used to use the global optimal path as a reference path and adopt a moving window method to perform local path planning on the smart car to obtain a local optimal path; The smart car control module is used to use the local optimal path as the driving path of the smart car and control the smart car based on the constructed motion chassis model to make the smart car reach the target point.

10. A smart car device, characterized in that: including memory and processor; The memory stores a computer-readable program executable by the processor; When the processor executes the computer-readable program, the steps in the path planning and control method of the intelligent vehicle as described in any one of claims 1-8 are implemented.

Images