A path tracking control method, device, terminal and storage medium

Abstract

The present invention discloses a path tracking control method, device, terminal and storage medium. The present invention relates to the field of path tracking technology and solves the technical problem that a suitable target point cannot be determined according to the influence of different paths, thereby inappropriately selecting and affecting the overall tracking. The present invention obtains the expected path of the vehicle, identifies and analyzes the expected path, selects the target point based on the expected path of different situations, and when selecting the target point, identifies the curve point of the expected path, and combines the curve curvature radius of the curve point and the corresponding vehicle speed to select the target point. Finally, according to the lateral position deviation and heading position deviation corresponding to the selected target point, a comprehensive analysis is performed to determine the suitable target point, and further the azimuth deviation is calculated according to the determined target point, so as to perform path tracking control on the target vehicle.

Description

Technical Field

[0001] The present invention relates to the field of path tracking technology, and in particular to a path tracking control method, device, terminal and storage medium. Background Art

[0002] In the case of autonomous driving of cars and navigation control of wheeled robots, after planning an executable path, the vehicle or robot needs to be driven along the desired path, also known as tracking. This is the path tracking problem. The performance of the path tracking algorithm directly determines the actual driving conditions of the vehicle and is an important part of the autonomous driving system.

[0003] According to Chinese patent publication number CN116300955A, a path tracking method, apparatus, terminal device and storage medium are provided, including: obtaining a pre-planned path, the pre-planned path includes multiple path tracking points, controlling a self-mobile device to track each path tracking point in turn so that the self-mobile device moves along the pre-planned path, detecting a first distance between the current position of the self-mobile device and the next path tracking point, when the first distance is greater than a preset length threshold, determining a first target point closest to the self-mobile device from multiple path tracking points of the pre-planned path, controlling the self-mobile device to travel to the first target point, and after reaching the first target point, returning to execute the step of controlling the self-mobile device to track each path tracking point in turn.

[0004] When some existing path tracking control technologies are used, it is necessary to determine the corresponding target point when performing path tracking. However, for different paths, the determination of the target point will be affected by different factors. How to select a suitable target point becomes a difficult problem. If the target point is not selected appropriately, it will cause overall errors in the subsequent path tracking process, further affecting the control results. Summary of the invention

[0005] In view of the deficiencies in the prior art, the present invention provides a path tracking control method, device, terminal and storage medium, which solves the problem that a suitable target point cannot be determined according to the influence of different paths, thereby inappropriate selection affects the overall tracking.

[0006] To achieve the above objectives, the present invention is implemented by the following technical solutions: a path tracking control method, the method specifically comprising the following steps:

[0007] Step 1: determine the target vehicle and obtain the expected path of the target vehicle, generate vehicle information, and judge the expected path to generate path information, where the path information includes curve path information and straight path information;

[0008] Step 2: Analyze the straight path information in the path information, determine the target point in combination with the vehicle information, and generate the target point information;

[0009] Step 3: Analyze the curve path information in the path information, and determine the target point in combination with the vehicle information. When determining the target point, comprehensively analyze the road continuity and change trend corresponding to the expected path to generate the target point information;

[0010] Step 4: Select the standard target point in the expected path according to the generated target point information, and track and control the target vehicle according to the information of the standard target point and generate control information.

[0011] As a further solution of the present invention: the specific method of determining the expected path and generating the path information in step 1 is:

[0012] The target vehicle is determined, and then the expected path of the target vehicle within time t is obtained. At the same time, the vehicle position of the target vehicle is obtained and vehicle information is generated. Then, the expected path of the target vehicle is analyzed. If there is a curve in the expected path, the corresponding expected path is marked as a curve path, and the curve path information is generated. If there is no curve in the expected path, the corresponding expected path is marked as a straight path, and the straight path information is generated.

[0013] As a further solution of the present invention: the specific method of analyzing the straight line path information in step 2 to generate the target point information is:

[0014] The target point in the expected path is selected by a fixed distance method, and the specific selection method is: the distance of the expected path is obtained according to the vehicle information, recorded as S, and the real-time speed corresponding to the target vehicle is obtained, recorded as V, and then the time required for the target vehicle to pass the expected path distance S is calculated based on the real-time speed V, recorded as the driving time T;

[0015] At the same time, the ratio of the real-time vehicle speed V to the travel time T is calculated and recorded as a fixed ratio V / T, and the calculated fixed ratio value is used as the target point segmentation point. At the same time, the quotient of the distance S and the fixed ratio V / T is calculated, and the calculated quotient value is used as the target selection point, and the target point information is generated at the same time.

[0016] As a further solution of the present invention: the specific method of analyzing the curve path information in step 3 to generate the target point information is:

[0017] Obtain the expected path corresponding to the curve path information, obtain the number of curves in the expected path, and label the curve points as i, where i=1, 2, ..., j, where j represents the number of curve points, and then calculate the curve curvature radius corresponding to the curve point i and record it as Qi;

[0018] Then, the continuity of the curve of the expected path is analyzed, and the expected path is classified into continuous curves and discontinuous curves. The specific classification method is: obtain the coordinates corresponding to two adjacent numbered curve points (x i ,y i ) and (x i+1 ,y i+1 ), and calculate the distance value corresponding to the two curve points, and compare the calculated distance value with the preset value. When the distance value is greater than the preset value, it means that the two adjacent curve points are continuous curve points, and the expected path is classified as a continuous curve. When the distance value is less than the preset value, it means that the two adjacent curve points are discontinuous curve points, and the expected path is classified as a discontinuous curve.

[0019] As a further solution of the present invention: the specific method of analyzing the classified discontinuous curves in step 3 is:

[0020] The curve length corresponding to the discontinuous curve is obtained and recorded as D1. At the same time, the real-time speed V of the target vehicle is obtained, and the time to pass the expected path at the real-time speed V is predicted and recorded as T1. Then, the fixed ratio V / T1 of the real-time speed V and the time T1 is calculated, and the quotient of the fixed ratio V / T1 and the curve length D1 is calculated. The calculated quotient is used as the target selection point, and the target point information is generated at the same time.

[0021] As a further solution of the present invention: the specific method of analyzing the continuous curves obtained by classification in step 3 is:

[0022] Obtain the curve curvature radius Qi corresponding to all curve points i, obtain the first curve point i, and calculate the corresponding quotient based on the corresponding curve length, determine the first target point, then calculate the curve curvature radius difference between two adjacent curve points, and multiply the calculated curve curvature radius difference with the quotient corresponding to the target point to obtain the adjustment distance, and sum the adjustment distance and the quotient of the first target point to obtain the target point distance, and generate the target point information.

[0023] As a further solution of the present invention: the specific method of generating the control information in step 4 is:

[0024] Get all the target points and label them as n, where n=1, 2, ..., m, where m represents the number of target points. Get the lateral position deviation and heading position deviation corresponding to target point i at the same time. Then calculate the sum of the lateral position deviation and heading position deviation of target point i, and select the target point with the smallest sum as the standard target point.

[0025] Then, the information corresponding to the standard target point is obtained, including the coordinates of the standard target point (x c ,y c ), and obtain the driving direction of the target vehicle and the position coordinates of the target vehicle (x z ,y z ), and then obtain the vector of the target vehicle position coordinate pointing to the standard target point as , the target vehicle's driving direction is recorded as , substitute the obtained vector into the formula Calculate the corresponding azimuth deviation , and the azimuth deviation Generates control information for standards.

[0026] A path tracking control device, the device specifically comprises an acquisition module, a control module, a determination module and a return module;

[0027] An acquisition module obtains the expected path and target vehicle information;

[0028] A control module controls the mobile device to track each of the target points in sequence;

[0029] A determination module, which determines a standard target point in the mobile device;

[0030] The return module returns to execute the step of controlling the mobile device to track each of the target points in sequence.

[0031] A terminal includes a memory, a processor, and a computer program stored in the memory and executable on the processor.

[0032] A storage medium stores a computer program.

[0033] The present invention provides a path tracking control method, device, terminal and storage medium. Compared with the prior art, it has the following beneficial effects:

[0034] The present invention obtains the expected path of the vehicle, identifies and analyzes the expected path, selects the target point based on the expected path of different situations, and when selecting the target point, identifies the curve point of the expected path, and selects the target point in combination with the curve curvature radius of the curve point and the corresponding vehicle speed. Finally, a comprehensive analysis is performed based on the lateral position deviation and the heading position deviation corresponding to the selected target point to determine the appropriate target point, and further calculates the azimuth deviation based on the determined target point, so as to perform path tracking control on the target vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1is a flow chart of the method of the present invention;

[0036] Figure 2 It is a schematic diagram of the structure of the terminal device of the present invention. DETAILED DESCRIPTION

[0037] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0038] For example, see Figure 1 The present application provides a path tracking control method, which specifically includes the following steps:

[0039] Step 1: Determine the target vehicle and obtain the expected path of the target vehicle, generate vehicle information, and judge the expected path to generate path information, where the path information includes curve path information and straight path information, and the specific method of generating the path information is:

[0040] First, a group of target vehicles is determined, and then the expected path of the target vehicle within time t is obtained. The method for obtaining the expected path here includes a pre-planning method and a real-time perception method. At the same time, the pre-planning method can be specifically implemented through map data and path planning algorithms, while the real-time perception method can be implemented through visual sensors, lidars and sensor fusion. At the same time, the vehicle position of the target vehicle is obtained and vehicle information is generated. Then, the expected path of the target vehicle is analyzed. If there is a curve in the expected path, the corresponding expected path is marked as a curve path, and the curve path information is generated. If there is no curve in the expected path, the corresponding expected path is marked as a straight path, and the straight path information is generated.

[0041] For example, the desired path is obtained based on map data, using high-precision digital maps, such as those used by in-vehicle navigation systems. These maps contain information such as the location, shape, and curvature of the road, which can be used as the desired path. In autonomous driving, the vehicle can determine the best path from the current location to the destination based on the map data and track it as the desired path;

[0042] For example, the desired path is obtained based on the path planning algorithm. The path planning algorithm, such as the A* algorithm and the Dijkstra algorithm, is used to search for the optimal path on the map according to specific goals and constraints. For example, for a mobile robot, the path planning algorithm can be used to determine the shortest path or the safest path from the starting point to the target point according to the task requirements and environmental information;

[0043] For example, the desired path is obtained based on visual sensors. Visual sensors such as cameras are used to capture and analyze the surrounding environment in real time, identify road signs, lane lines and other information, and thus determine the desired path. In the intelligent driving assistance system, lane lines are identified by cameras and tracked as the desired path of the vehicle.

[0044] For example, the desired path is obtained based on LiDAR. LiDAR is used to scan the surrounding environment, obtain the distance and position information of objects, and determine the desired path by identifying road boundaries, obstacles, etc. In self-driving cars, LiDAR can accurately detect the shape and boundaries of the road and provide the vehicle with an accurate desired path.

[0045] For example, the desired path can be obtained based on sensor fusion. The data of multiple sensors, such as visual sensors, lidar, GPS, etc., can be integrated to improve the accuracy and reliability of the desired path. By fusing the data of cameras and lidar, visual information and distance information can be used at the same time to more accurately identify roads and obstacles and determine the desired path.

[0046] Step 2: Analyze the straight path information in the path information, and determine the target point in combination with the vehicle information and generate the target point information at the same time. The specific method of generating the target point information is:

[0047] When the path information is a straight line path information, the target point in the expected path is selected by the fixed distance method, and the specific selection method is: the distance of the expected path is obtained according to the vehicle information, recorded as S, and the real-time speed corresponding to the target vehicle is obtained, recorded as V, and the real-time speed here is expressed as the average speed of the target vehicle. For example, if the target vehicle travels 100 meters in 10 seconds, the corresponding unit average speed is 10m / s. The unit average speed is further converted to an average speed of 36 km / h, and use it as the real-time vehicle speed V. Then, the time required for the target vehicle to pass the expected path S is calculated based on the real-time vehicle speed V, and recorded as the driving time T. The specific calculation method here is T=S / V. At the same time, the ratio of the real-time vehicle speed V to the driving time T is calculated and recorded as a fixed ratio V / T. The calculated fixed ratio value is used as the target point segmentation point. At the same time, the quotient of the distance S and the fixed ratio V / T is calculated. The specific calculation method is: S÷(V / T), and the calculated quotient is used as the target selection point. For example, the real-time vehicle speed is 60 km / h, and the calculated driving time T value is 20s. The fixed ratio obtained by further calculation is 3, and the corresponding expected distance is 90 meters. 90 is further divided by 3 to obtain 30 meters. Therefore, a target point is set every 30 meters on the expected path, and the target point information is generated at the same time. The target point information here includes the coordinates corresponding to the target point, such as (X, Y).

[0048] Step 3: Analyze the curve path information in the path information, and determine the target point in combination with the vehicle information. When determining the target point, the target point information is generated by comprehensively analyzing the road continuity and change trend corresponding to the expected path. The specific method of generating the target point information is as follows:

[0049] Obtain the expected path corresponding to the curve path information, and obtain the number of curves in the expected path. At the same time, the curve points are labeled as i, and i=1, 2, ..., j, where j represents the number of curve points. Then the curve curvature radius corresponding to the curve point i is calculated and recorded as Qi. The curvature radius of the curve refers to the radius of the circle perpendicular to the tangent at a certain point on the curve. This circle is called the curvature circle of the curve at this point. The specific calculation method is as follows:

[0050] For plane curves, mathematical formulas can be used to calculate the radius of curvature. Generally speaking, it can be calculated using the function expression or parametric equation of the curve. For example, for a curve y=f(x) on a plane, the radius of curvature R at a certain point (x0, y0) can be calculated using the following formula:

[0051]

[0052] in are the first and second derivatives of the function f(x) at x0, respectively.

[0053] Then, the continuity of the curve of the expected path is analyzed, and the expected path is classified into continuous curves and discontinuous curves. The specific classification method is: obtain the coordinates corresponding to two adjacent numbered curve points (x i ,y i ) and (x i+1 ,y i+1 ), and calculate the distance value corresponding to the two curve points, and the calculation formula of the distance value is: , and the calculated distance value is compared with a preset value, and the specific value of the preset value is set by the operator. When the distance value is greater than the preset value, it means that the two adjacent curve points are continuous curve points, and the expected path is classified as a continuous curve. When the distance value is less than the preset value, it means that the two adjacent curve points are discontinuous curve points, and the expected path is classified as a discontinuous curve.

[0054] Analyze the discontinuous curves obtained by classification, obtain the curve length corresponding to the discontinuous curve, record it as D1, and obtain the real-time speed V of the target vehicle, and predict the time to pass the expected path at the real-time speed V, record it as T1, then calculate the fixed ratio V / T1 of the real-time speed V and the time T1, and calculate the quotient of the fixed ratio V / T1 and the curve length D1, and use the calculated quotient as the target selection point, and generate the target point information at the same time;

[0055] The classified continuous curves are analyzed to obtain the curve curvature radii Qi corresponding to all curve points i, and the first curve point i is obtained at the same time. The corresponding quotient is calculated based on the corresponding curve length, and the first target point is determined at the same time. The method for determining the first target point here is the same as the method for determining the above-mentioned discontinuous curve. Then, the difference in the curve curvature radii of two adjacent curve points is calculated, and the calculated difference in the curve curvature radii is multiplied by the quotient corresponding to the target point to obtain the adjustment distance, and the adjustment distance is summed with the quotient of the first target point to obtain the target point distance, and the target point information is generated by analogy. Specifically, first, the curve curvature radius difference and the quotient are multiplied, and the value obtained by the product is summed with the quotient. If the curve curvature radius difference is a negative number, a subtraction operation is performed, and if the curve curvature radius difference is a positive number, an addition operation is performed, and so on to determine the target points of all curve points.

[0056] Step 4: Select the standard target point in the expected path according to the generated target point information, and track and control the target vehicle according to the information of the standard target point and generate control information. The specific method of generating the control information is:

[0057] Obtain all target points and label them as n, where n=1, 2, …, m, where m represents the number of target points. At the same time, obtain the lateral position deviation and heading position deviation corresponding to the target point i. The lateral position deviation refers to the position offset of the vehicle or mobile body in the direction perpendicular to the expected path during the path tracking process. The heading position deviation refers to the angular difference between the actual heading of the vehicle or mobile body and the expected heading during the path tracking process. Then calculate the sum of the lateral position deviation and the heading position deviation of the target point i, and select the target point corresponding to the smallest sum as the standard target point.

[0058] Then, the information corresponding to the standard target point is obtained, including the coordinates of the standard target point (x c ,y c ), and obtain the driving direction of the target vehicle and the position coordinates of the target vehicle (x z ,y z ), and then obtain the vector of the target vehicle position coordinate pointing to the standard target point as , the target vehicle's driving direction is recorded as , substitute the obtained vector into the formula Calculate the corresponding azimuth deviation , and the azimuth deviation Generates control information for standards.

[0059] Embodiment 2, a path tracking control device, the device specifically includes an acquisition module, a control module, a determination module and a return module;

[0060] An acquisition module obtains the expected path and target vehicle information;

[0061] A control module controls the mobile device to track each of the target points in sequence;

[0062] A determination module, which determines a standard target point in the mobile device;

[0063] The return module returns to execute the step of controlling the mobile device to track each of the target points in sequence.

[0064] For example 3, please refer to Figure 2 A path tracking control terminal specifically includes: at least one processor, a memory, and a computer program stored in the memory and executable on at least one processor, wherein the processor executes the computer program to implement the steps in any of the above-mentioned path tracking method embodiments. The terminal device may be a computing device such as a desktop computer, a notebook, a PDA, and a cloud server. The terminal device may include, but is not limited to, a processor and a memory.

[0065] Embodiment 4 is a path tracking control storage medium, the storage medium stores a computer program, and when the computer program is executed by a processor, the steps in the above-mentioned method embodiments can be implemented.

[0066] Computer-readable media may include at least: any entity or device that can carry computer program code to a device / terminal device, a recording medium, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electric carrier signal, a telecommunication signal, and a software distribution medium. For example, a USB flash drive, a mobile hard disk, a magnetic disk, or an optical disk. In some jurisdictions, based on legislation and patent practice, computer-readable media cannot be electric carrier signals and telecommunication signals.

[0067] Some of the data in the above formulas are numerically calculated by removing their dimensions. Meanwhile, the contents not described in detail in this specification belong to the prior art known to those skilled in the art.

[0068] The above embodiments are only used to illustrate the technical method of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical method of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical method of the present invention.

Claims

1. A path tracking control method, characterized in that: The method specifically comprises the following steps: Step 1: determine the target vehicle and obtain the expected path of the target vehicle, generate vehicle information, and judge the expected path to generate path information, where the path information includes curve path information and straight path information; Step 2: Analyze the straight path information in the path information, determine the target point in combination with the vehicle information, and generate the target point information; Step 3: Analyze the curved path information in the path information, and determine the target point in combination with the vehicle information. When determining the target point, combine the road continuity and change trend corresponding to the expected path to generate the target point information through comprehensive analysis. The specific method of analyzing the curved path information to generate the target point information is as follows: Obtain the expected path corresponding to the curve path information, obtain the number of curves in the expected path, and label the curve points as i, where i=1, 2, ..., j, where j represents the number of curve points, and then calculate the curve curvature radius corresponding to the curve point i and record it as Qi; Then, the continuity of the curve of the expected path is analyzed, and the expected path is classified into continuous curves and discontinuous curves. The specific classification method is: obtain the coordinates corresponding to two adjacent numbered curve points (x i ,y i ) and (x i+1 ,y i+1 ), and calculate the distance value corresponding to the two curve points, and compare the calculated distance value with the preset value. When the distance value is greater than the preset value, it means that the two adjacent curve points are continuous curve points, and the expected path is classified as a continuous curve. When the distance value is less than the preset value, it means that the two adjacent curve points are discontinuous curve points, and the expected path is classified as a discontinuous curve. And the specific method of analyzing the classified discontinuous curves is: The length of the curve corresponding to the discontinuous curve is obtained and recorded as D1. At the same time, the real-time speed V of the target vehicle is obtained, and the time for passing the expected path at the real-time speed V is predicted and recorded as T1. Then, the fixed ratio V / T1 of the real-time speed V and the time T1 is calculated, and the quotient of the fixed ratio V / T1 and the curve length D1 is calculated. The calculated quotient is used as the target selection point, and the target point information is generated at the same time; The specific method for analyzing the classified continuous curves is: Obtain the curve curvature radius Qi corresponding to all curve points i, obtain the first curve point i at the same time, and calculate the corresponding quotient value based on the corresponding curve length, and the quotient value represents the quotient value of the fixed ratio corresponding to the continuous curve and the curve length, and determine the first target point at the same time, then calculate the curve curvature radius difference between two adjacent curve points, and multiply the calculated curve curvature radius difference with the quotient value corresponding to the target point to obtain the adjustment distance, and sum the adjustment distance and the quotient value of the first target point to obtain the target point distance, and generate the target point information; Step 4: Select the standard target point in the expected path according to the generated target point information, and track and control the target vehicle according to the information of the standard target point and generate control information.

2. A path tracking control method according to claim 1, characterized in that: The specific method of determining the expected path and generating path information in step 1 is: The target vehicle is determined, and then the expected path of the target vehicle within time t is obtained. At the same time, the vehicle position of the target vehicle is obtained and vehicle information is generated. Then, the expected path of the target vehicle is analyzed. If there is a curve in the expected path, the corresponding expected path is marked as a curve path, and the curve path information is generated. If there is no curve in the expected path, the corresponding expected path is marked as a straight path, and the straight path information is generated.

3. A path tracking control method according to claim 1, characterized in that: The specific method of analyzing the straight line path information to generate the target point information in step 2 is: The target point in the expected path is selected by a fixed distance method, and the specific selection method is: the distance of the expected path is obtained according to the vehicle information, recorded as S, and the real-time speed corresponding to the target vehicle is obtained, recorded as V, and then the time required for the target vehicle to pass the expected path distance S is calculated based on the real-time speed V, recorded as the driving time T; At the same time, the ratio of the real-time vehicle speed V to the travel time T is calculated and recorded as a fixed ratio V / T, and the calculated fixed ratio value is used as the target point segmentation point. At the same time, the quotient of the distance S and the fixed ratio V / T is calculated, and the calculated quotient value is used as the target selection point, and the target point information is generated at the same time.

4. A path tracking control method according to claim 1, characterized in that: The specific method of generating the control information in step 4 is: Get all the target points and label them as n, where n=1, 2, ..., m, where m represents the number of target points. Get the lateral position deviation and heading position deviation corresponding to target point i at the same time. Then calculate the sum of the lateral position deviation and heading position deviation of target point i, and select the target point with the smallest sum as the standard target point. Then, the information corresponding to the standard target point is obtained, including the coordinates of the standard target point (x c ,y c ), and obtain the driving direction of the target vehicle and the position coordinates of the target vehicle (x z ,y z ), and then obtain the vector of the target vehicle position coordinate pointing to the standard target point as , the target vehicle's driving direction is recorded as , substitute the obtained vector into the formula Calculate the corresponding azimuth deviation , and the azimuth deviation Generates control information for standards.

5. A path tracking control device, used to execute a path tracking control method according to any one of claims 1 to 4, characterized in that: The device specifically includes an acquisition module, a control module, a determination module and a return module; An acquisition module obtains the expected path and target vehicle information; A control module controls the mobile device to track each target point in sequence; A determination module, which determines a standard target point in the mobile device; The return module returns to execute the step of controlling the mobile device to track each of the target points in sequence.

6. A terminal comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the method according to any one of claims 1 to 4 is implemented.

7. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 4 is implemented.

Images