Track correction method, electronic equipment and computer readable storage medium

Abstract

The invention discloses a trajectory correction method, electronic equipment and a computer readable storage medium. The method comprises the following steps: acquiring a plurality of control points corresponding to an original path of a vehicle model in a map; wherein the map at least comprises coordinate information of the obstacle; acquiring a current control point, and determining a nearest barrier corresponding to the current control point and an actual distance between the current control point and the nearest barrier based on the current control point and the coordinate information of the barrier; based on the current control point, a historical control point before the current control point and coordinate information of a nearest obstacle corresponding to the current control point, an orientation angle value between the vehicle model and the nearest obstacle is obtained, and a safety threshold matched with the orientation angle value is obtained; and optimizing each control point based on the safety threshold and the actual distance, and generating a correction path by using all optimized control points. According to the scheme, the operation efficiency and safety of the automatic guided vehicle can be improved.

Description

Technical Field

[0001] The present application relates to the technical field of path planning, and in particular to a trajectory correction method, an electronic device, and a computer-readable storage medium. Background Art

[0002] With the continuous development of industrial robots, automated guided vehicles (AGVs) are increasingly being used in the warehousing and logistics industry. Compared with other types of industrial robots, automated guided vehicles have stricter requirements on paths. For automated guided vehicles, the paths obtained by traditional path planning algorithms are sometimes too far away from obstacles, resulting in poor results and low operating efficiency. Sometimes they are too close to obstacles, causing collision risks and low safety. In view of this, how to improve the operating efficiency and safety of automated guided vehicles has become an urgent problem to be solved. Summary of the invention

[0003] The main technical problem solved by the present application is to provide a trajectory correction method, an electronic device and a computer-readable storage medium, which can improve the operating efficiency and safety of an automatic guided vehicle.

[0004] To solve the above technical problems, the first aspect of the present application provides a trajectory correction method, comprising: obtaining multiple control points corresponding to the original path of a vehicle model in a map; wherein the map at least includes coordinate information of obstacles; obtaining a current control point, and based on the coordinate information of the current control point and the obstacle, determining the nearest obstacle corresponding to the current control point and the actual distance between the current control point and the nearest obstacle; based on the current control point, historical control points before the current control point, and the coordinate information of the nearest obstacle corresponding to the current control point, obtaining a heading angle value between the vehicle model and the nearest obstacle, and obtaining a safety threshold matching the heading angle value; optimizing each of the control points based on the safety threshold and the actual distance, and generating a corrected path using all the optimized control points.

[0005] To solve the above technical problem, the second aspect of the present application provides an electronic device, comprising a memory and a processor coupled to each other, wherein the memory stores program instructions, and the processor is used to execute the program instructions to implement the method described in the first aspect.

[0006] In order to solve the above technical problem, the third aspect of the present application provides a computer-readable storage medium, which stores program instructions that can be executed by a processor, and the program instructions are used to implement the method described in the first aspect above.

[0007] The above scheme obtains multiple control points corresponding to the original path of the vehicle model in the map, obtains the current control point of the vehicle model, and determines the nearest obstacle corresponding to the current control point based on the coordinate information of the current control point and the obstacle, and determines the actual distance between the current control point and the nearest obstacle. Based on the current control point, the historical control points before the current control point, and the coordinate information of the nearest obstacle corresponding to the current control point, the orientation angle value between the vehicle model of the automatic guided vehicle and the corresponding nearest obstacle is obtained, and a safety threshold matching the orientation angle value is obtained. Each control point is optimized based on the safety threshold and the actual distance, and a corrected path is generated using all the control points after the optimization. Since a safety threshold matching the orientation angle value is obtained when optimizing the control point, the obstacle distance optimization item of the automatic guided vehicle is more accurate, the probability of the automatic guided vehicle colliding with the obstacle, and the inefficiency caused by excessive deviation from the obstacle are reduced, thereby improving the operating efficiency and safety of the automatic guided vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0008] 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 the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work. Among them:

[0009] Figure 1 It is a flow chart of an implementation method of a trajectory correction method of the present application;

[0010] Figure 2 It is a schematic diagram of the vehicle model and obstacles of this application in the ESDF map;

[0011] Figure 3 It is a structural schematic diagram of an implementation method of the outer contour of a vehicle model of the present application;

[0012] Figure 4 It is a structural schematic diagram of an embodiment of the electronic device of the present application;

[0013] Figure 5 It is a structural schematic diagram of an implementation method of a computer-readable storage medium of the present application. DETAILED DESCRIPTION

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

[0015] The terms "system" and "network" are often used interchangeably in this article. The term "and / or" in this article is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this article generally indicates that the associated objects before and after are in an "or" relationship. In addition, "many" in this article means two or more than two.

[0016] See also Figure 1 , Figure 1 It is a flow chart of an implementation method of the trajectory correction method of the present application.

[0017] The method includes:

[0018] S101: Acquire a plurality of control points corresponding to an original path of a vehicle model in a map; wherein the map at least includes coordinate information of obstacles.

[0019] Specifically, a plurality of control points corresponding to an original path of a vehicle model of an automated guided vehicle in a map are obtained, wherein the map at least includes coordinate information of obstacles.

[0020] Optionally, the map may be an Euclidean Signed Distance Functions (ESDF) map, or other types of maps including coordinate information of obstacles, and the present application does not impose any specific restrictions on this. For ease of explanation, the following description will be based on an ESDF map.

[0021] It should be noted that the trajectory correction method provided in the present application is applicable to any irregular vehicle model of an automated guided vehicle and is not limited to using a circle to represent the vehicle model.

[0022] In one application, a B-spline curve is used to fit the original path of the vehicle model in the ESDF map to obtain corresponding multiple control points.

[0023] In another application mode, a Bezier curve is used to fit the original path of the vehicle model in the ESDF map to obtain corresponding multiple control points.

[0024] In an application scenario, the original path includes multiple original path points, and the coordinate set of the original path points of the original path is obtained, wherein the original path is a traversable path that is searched in advance based on the grayscale map of the starting point, the end point and the obstacles using the existing path search algorithm, and the coordinate set of the original path points is list(x, y, z), wherein x represents the x-axis coordinate of the original path point, y represents the y-axis coordinate of the original path point, z represents the z-axis coordinate of the original path point, and list represents the coordinate set of each original path point. It can be understood that since the automatic guided vehicle can only move within a plane, the z-axis coordinate of the original path point is 0. Furthermore, according to the coordinates of each original path point in the coordinate set, the basic formula of the existing B-spline curve is used to fit the corresponding multiple control points.

[0025] S102: Acquire the current control point, and determine the nearest obstacle corresponding to the current control point and the actual distance between the current control point and the nearest obstacle based on the coordinate information of the current control point and the obstacle.

[0026] Specifically, the current control point of the vehicle model is obtained, and based on the coordinate information of the current control point and the obstacle, the nearest obstacle corresponding to the current control point is determined, and the actual distance between the current control point and the nearest obstacle is determined.

[0027] In one application method, the current control point and the corresponding coordinates of the vehicle model of the automatic guided vehicle are obtained, and based on the coordinate information of the current control point and all obstacles in the ESDF map, the nearest obstacle corresponding to the current control point is determined, and the coordinates of the nearest obstacle are obtained. According to the coordinates of the current control point and the corresponding coordinates of the nearest obstacle, the actual distance between the current control point and the nearest obstacle is calculated.

[0028] In another application, the ESDF map divides the three-dimensional space into multiple voxels, each voxel is equivalent to a three-dimensional network, and each voxel stores a pre-calculated distance value to its nearest obstacle. In order to improve the calculation speed, the current control point is obtained, and the target voxel corresponding to the current control point in the ESDF map is determined, and the distance value corresponding to the target voxel is determined as the actual distance between the current control point and the nearest obstacle.

[0029] S103: Based on the coordinate information of the current control point, the historical control points before the current control point, and the nearest obstacle corresponding to the current control point, obtain the orientation angle value between the vehicle model and the nearest obstacle, and obtain the safety threshold matching the orientation angle value.

[0030] Specifically, based on the coordinate information of the current control point, the historical control points before the current control point, and the nearest obstacle corresponding to the current control point, the orientation angle value between the vehicle model of the automatic guided vehicle and the corresponding nearest obstacle is obtained, and a safety threshold matching the orientation angle value is obtained.

[0031] It should be noted that the historical control point is usually the previous control point before the current control point. The line connecting the previous control point and the current control point is used as the orientation of the current control point. The angle between the line connecting the current control point and the center point of the nearest obstacle and the orientation of the current control point is used as the orientation angle between the vehicle model of the automatic guided vehicle and the nearest obstacle.

[0032] In one application, the heading angle value is obtained by constructing a vector and calculating the angle, obtaining the vector from the current control point to the historical control point and the vector from the current control point to the nearest obstacle, calculating the angle between the two vectors to obtain the corresponding heading angle value, and obtaining a safety threshold that matches the heading angle value.

[0033] In another application method, the angle difference is directly calculated through geometric relationships to obtain the orientation angle value, the direction vector from the current control point to the historical control point and the direction vector from the current control point to the nearest obstacle are obtained, the angle difference between the two direction vectors is calculated to obtain the corresponding orientation angle value, and the safety threshold matching the orientation angle value is obtained.

[0034] In an application scenario, assuming that the current control point is C(x c ,y c ), the previous control point is P(x c-1 ,y c-1 ), the center point of the nearest obstacle is O(x o ,y o ), at this time, the vector from the current control point to the previous control point The vector from the current control point to the nearest obstacle Calculate the angle between two vectors using the dot product formula The value of θ is the orientation angle value.

[0035] In another application scenario, assuming that the current control point is C(x c ,y c ), the previous control point is P(x c-1 ,y c-1 ), the center point of the nearest obstacle is O(x o ,y o ), at this time, the direction vector from the current control point to the previous control point The direction vector from the current control point to the nearest obstacle Use inverse trigonometric functions to calculate the angle difference between two vectors θ = arctan2(y c -y c-1 , x c -x c-1 )-arctan2(y o -y c , x o -x c ), the value of θ is the orientation angle value.

[0036] S104: Optimize each control point based on the safety threshold and the actual distance, and generate a corrected path using all optimized control points.

[0037] Specifically, each control point is optimized based on the safety threshold and the actual distance, and a corrected path is generated using all optimized control points.

[0038] In one application, based on the difference between the safety threshold and the actual distance, the optimization direction and size corresponding to each control point are obtained, and a correction path is generated using all the optimized control points.

[0039] In another application, based on the difference between the safety threshold and the actual distance, the optimization direction and size corresponding to each control point are obtained, and all control points after the final optimization are obtained in combination with the smoothing term optimization, thereby improving the accuracy of the generated correction path.

[0040] The above scheme obtains multiple control points corresponding to the original path of the vehicle model in the map, obtains the current control point of the vehicle model, and determines the nearest obstacle corresponding to the current control point based on the coordinate information of the current control point and the obstacle, and determines the actual distance between the current control point and the nearest obstacle. Based on the current control point, the historical control points before the current control point, and the coordinate information of the nearest obstacle corresponding to the current control point, the orientation angle value between the vehicle model of the automatic guided vehicle and the corresponding nearest obstacle is obtained, and a safety threshold matching the orientation angle value is obtained. Each control point is optimized based on the safety threshold and the actual distance, and a corrected path is generated using all the control points after the optimization. Since a safety threshold matching the orientation angle value is obtained when optimizing the control point, the obstacle distance optimization item of the automatic guided vehicle is more accurate, the probability of the automatic guided vehicle colliding with the obstacle, and the inefficiency caused by excessive deviation from the obstacle are reduced, thereby improving the operating efficiency and safety of the automatic guided vehicle.

[0041] In one embodiment, the map further includes coordinate information of non-obstacles. Before step S102, the method further includes: determining coordinate information corresponding to the current control point in the map based on the coordinate information of the current control point and the non-obstacles.

[0042] Specifically, the coordinate information of non-obstacles is also stored in advance in the ESDF map. Based on the coordinate information of the current control point and the non-obstacles, the coordinate information corresponding to the current control point in the ESDF map can be determined more accurately.

[0043] In one implementation scenario, step S102 specifically includes: obtaining the coordinate information of the nearest obstacle corresponding to the current control point based on the coordinate information corresponding to the current control point in the map and the coordinate information of the obstacle; obtaining the actual distance based on the coordinate information corresponding to the control point in the map and the coordinate information of the nearest obstacle corresponding to the current control point.

[0044] Specifically, based on the coordinate information corresponding to the current control point in the ESDF map and the coordinate information corresponding to all obstacles in the ESDF map, the coordinate information of the nearest obstacle corresponding to the current control point is determined. Based on the coordinate information corresponding to the current control point in the map and the coordinate information of the nearest obstacle corresponding to the current control point, the actual distance between the current control point and the corresponding nearest obstacle can be more accurately calculated.

[0045] In one implementation scenario, in step S103, obtaining the orientation angle value between the vehicle model and the nearest obstacle based on the coordinate information of the current control point, the historical control point before the current control point, and the nearest obstacle corresponding to the current control point specifically includes: determining the coordinate information corresponding to the historical control point in the map based on the coordinate information corresponding to the historical control point in the map; determining the orientation of the current control point and the coordinate information corresponding to the coordinate point in the orientation in the map based on the coordinate information corresponding to the current control point in the map and the coordinate information corresponding to the historical control point in the map; wherein the coordinate point is any point on the extension line of the line connecting the current control point and the historical control point in the orientation; obtaining the orientation angle value between the vehicle model and the nearest obstacle based on the coordinate information corresponding to the current control point in the map, the coordinate information corresponding to the coordinate point in the map, and the coordinate information of the nearest obstacle corresponding to the current control point.

[0046] Specifically, based on the coordinate information of historical control points and non-obstacles in the ESDF map, the coordinate information corresponding to the historical control points in the ESDF map is determined, and based on the coordinate information corresponding to the current control point in the ESDF map and the coordinate information corresponding to the historical control points in the ESDF map, the orientation of the current control point and the coordinate information corresponding to the coordinate point in the orientation in the ESDF map are determined; wherein the coordinate point is any point on the extension line of the line connecting the current control point and the historical control point in the orientation of the current control point, based on the coordinate information corresponding to the current control point in the ESDF map, the coordinate information corresponding to the coordinate point in the ESDF map and the coordinate information of the nearest obstacle corresponding to the current control point, the orientation angle value between the vehicle model of the automatic guided vehicle and the nearest obstacle can be obtained more accurately by calculation, thereby improving the accuracy of the corresponding safety threshold obtained by subsequent queries.

[0047] In a specific implementation scenario, please refer to Figure 2 , Figure 2 It is a schematic diagram of the vehicle model and obstacles in the ESDF map of this application, where P is the previous control point of the vehicle model, C is the current control point of the vehicle model, the gray area is the voxel occupied by the nearest obstacle corresponding to the current control point C, and point O is the center of the voxel occupied by the nearest obstacle. Assuming that the coordinates of point P are (x1, y1) and the coordinates of point C are (x2, y2), the coordinates of point V are (2x2-x1, 2y2-y1), and the coordinates of point O are (x3, y3). The coordinates of points V, C, and O are used to calculate the orientation angle between the vehicle model of the automatic guided vehicle and the nearest obstacle.

[0048] In one implementation scenario, the safety threshold is obtained by querying a distance query table, which is obtained based on the following steps: obtaining the outer contour of the vehicle model; wherein the outer contour is a closed curve and the vehicle model includes a motion center; based on the motion center, determining a straight line, and intersecting the straight line with the outer contour in different directions to obtain an intersection point; using the distance between the motion center and the intersection point as a safety threshold for different directions and saving it to obtain a distance query table.

[0049] Specifically, the outer contour of the vehicle model of the automatic guided vehicle is obtained, the vehicle model includes a motion center, and the motion center is the current control point of the automatic guided vehicle. A straight line is determined based on the motion center, and the straight line is intersected with the outer contour of the vehicle model according to different orientations to obtain intersection points. The distance between the motion center and each intersection point is used as the safety threshold for different orientations of the automatic guided vehicle, and is combined with the corresponding orientation angle and saved together to obtain a distance query table, thereby improving the efficiency of subsequently obtaining the safety threshold through the orientation angle value.

[0050] In a specific implementation scenario, please refer to Figure 3 , Figure 3 It is a structural diagram of an implementation method of the outer contour of the vehicle model of the present application. Define the direction of the vehicle head as 0°, counterclockwise rotation as positive, step length a, and a is a customizable constant value. Divide the plane 360° according to the step length a to obtain a set of calculated angles (0, a, 2a, 3a…(360-a) / a*a)°. For example, if a=10, the angle group to be calculated is (0, 10, 20…340, 350)°, and the distance from the center of motion to the farthest contour in this direction is calculated as Dist, which is the safety threshold. Each angle and distance are combined and saved, for example [(0°, 15cm), (10°, 25cm),…, (350°, 5cm)] to obtain a distance query table, so as to query the corresponding safety threshold according to the angle, wherein the calculation method of the safety threshold Dist is: integrate the outer contour of the vehicle model of the automatic guided vehicle into a closed curve, such as Figure 3 As shown, point O is the center of motion, one of the angles is taken as the OA direction, BC is the edge contour, and the intersection of OA and BC is point S. OA is intersected with each edge contour to obtain the intersection point, and it is determined whether the intersection point S is on the BC edge, and then whether OS is in the same direction as OA. If the above two conditions are met, point S is retained and the length of the corresponding OS is calculated. After traversing all edge contours, the longest OS is taken as the value of the safety threshold Dist in this direction.

[0051] In one embodiment, step S104 specifically includes: in response to the actual distance being greater than a safety threshold, taking the current control point as an optimized control point; in response to the actual distance being less than the safety threshold, obtaining the difference between the actual distance and the safety threshold, and optimizing the current control point based on the difference to obtain the optimized control point; in response to the number of optimizations satisfying the optimization termination condition, determining the updated position of each control point, and generating a corrected path based on the positions of all updated control points.

[0052] Specifically, when the actual distance is greater than the safety distance, there is no need to optimize the current control point, and the current control point is used as the optimized control point. When the actual distance is less than the safety distance, the current control point needs to be optimized to obtain the difference between the actual distance and the safety distance. Based on the difference, the current control point is optimized to obtain the optimized control point. When the number of optimizations meets the optimization termination condition, the updated position of each control point is determined, and a corrected path is generated based on the positions of all updated control points. Since the safety threshold on each control point is recalculated during each optimization, the obstacle distance optimization item of the automatic guided vehicle is more accurate, reducing the probability of the automatic guided vehicle colliding with an obstacle, as well as the inefficiency caused by excessive deviation from the obstacle, thereby improving the operating efficiency and safety of the automatic guided vehicle.

[0053] In one implementation scenario, optimizing the current control point based on the difference to obtain the optimized control point includes: obtaining a cost value corresponding to the current control point based on the difference; and optimizing the current control point based on the cost value to obtain the optimized control point.

[0054] Specifically, the cost value corresponding to the current control point is obtained based on the difference calculation. The cost value is the cost of the distance corresponding to the nearest obstacle when the current control point is optimized. The current control point is optimized based on the calculated cost value to obtain the optimized control point, so as to improve the accuracy of the subsequently generated correction path, thereby further improving the operating efficiency and safety of the automated guided vehicle.

[0055] Optionally, the safety threshold may also include a user-defined expected value, which may be regarded as a desired distance between the edge of the automated guided vehicle and the obstacle, such as 0.5 m, and this application does not impose any specific limitation on this.

[0056] In a specific implementation scenario, the formula for optimizing the distance term of the current control point is:

[0057] d safe (i) = d exp +Dist(θ i ) (1)

[0058]

[0059] Among them, i represents the i-th control point in a round of optimization, d safe Represents the safe distance from the center of the vehicle model to the nearest obstacle, which is used to calculate the distance term cost and gradient; θ i Dist(θ i ) represents the vehicle threshold, d exp is a custom expected value, which can be regarded as the distance between the edge of the automated guided vehicle and the obstacle. d ' indicates the cost of calculating the distance between point i and the nearest obstacle during optimization, and dist(i) indicates the actual distance between point i and the nearest obstacle.

[0060] Optionally, when the change in the cost value after the last set number of distance optimization of the control point is small, it is determined that the optimization termination condition is met, so that the position of the control point optimized last time is determined as the updated position of the control point.

[0061] In other implementation scenarios, the control points may also be optimized through different optimization methods according to the difference between the actual distance and the safety threshold, and this application does not make any specific restrictions on this.

[0062] In one implementation scenario, obtaining multiple control points corresponding to the original path of the vehicle model in the map in step S101 includes: obtaining a coordinate set of multiple original path points in the original path; performing B-spline fitting on the coordinate set to obtain multiple control points corresponding to the original path.

[0063] Specifically, the original path includes multiple original path points, and the coordinate set of the original path points of the original path is obtained, wherein the original path is a passable path that is searched in advance according to the grayscale map of the starting point, the end point and the obstacles using the existing path search algorithm, and the coordinate set of the original path points is list (x, y, z), wherein x represents the x-axis coordinate of the original path point, y represents the y-axis coordinate of the original path point, z represents the z-axis coordinate of the original path point, and list represents the coordinate set of each original path point. It can be understood that since the automatic guided vehicle can only move within a plane, the z-axis coordinate of the original path point is 0. Further, according to the coordinates of each original path point in the coordinate set, the basic formula of the existing B-spline curve is used to fit the corresponding multiple control points.

[0064] Furthermore, after obtaining the updated position of each control point, the basic formula of the existing B-spline curve is used to perform distance sampling according to the updated position of each control point to obtain the position of each path point after the original path trajectory is modified, thereby generating a corrected path.

[0065] See also Figure 4 , Figure 44 is a schematic diagram of the structure of an embodiment of an electronic device of the present application, wherein the electronic device 40 comprises a memory 401 and a processor 402 coupled to each other, wherein the memory 401 stores program data (not shown), and the processor 402 calls the program data to implement the method in any of the above embodiments, and the description of the relevant contents can be found in the detailed description of the above method embodiments, which will not be repeated here. Specifically, the electronic device 40 includes but is not limited to: a desktop computer, a laptop computer, a tablet computer, a server, etc., which are not limited here. In addition, the processor 402 can also be referred to as a CPU (Center Processing Unit). The processor 402 may be an integrated circuit chip with signal processing capabilities. The processor 402 may also be a general-purpose processor, a digital signal processor (Digital Signal Processor, DSP), an application-specific integrated circuit (Application Specific Integrated Circuit, ASIC), a field-programmable gate array (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc. In addition, the processor 402 may be implemented by an integrated circuit chip.

[0066] See also Figure 5 , Figure 5 It is a structural diagram of an embodiment of a computer-readable storage medium of the present application. The computer-readable storage medium 50 stores program data 500. When the program data 500 is executed by a processor, the method in any of the above embodiments is implemented. For descriptions of related contents, please refer to the detailed description of the above method embodiments, which will not be repeated here.

[0067] It should be noted that the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the present implementation scheme.

[0068] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0069] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) or a processor (processor) to perform all or part of the steps of each implementation method of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, ROM), random access memory (Random Access Memory, RAM), disk or optical disk and other media that can store program codes.

[0070] The above description is only an implementation method of the present application, and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly used in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A trajectory correction method, characterized in that: include: Acquire a plurality of control points corresponding to the original path of the vehicle model in a map; wherein the map at least includes coordinate information of obstacles; Acquire a current control point, and determine the nearest obstacle corresponding to the current control point and the actual distance between the current control point and the nearest obstacle based on the coordinate information of the current control point and the obstacle; Based on the coordinate information of the current control point, the historical control points before the current control point, and the nearest obstacle corresponding to the current control point, a heading angle value between the vehicle model and the nearest obstacle is obtained, and a safety threshold value matching the heading angle value is obtained; Each of the control points is optimized based on the safety threshold and the actual distance, and a corrected path is generated using all the optimized control points.

2. The method according to claim 1, characterized in that The map also includes coordinate information of non-obstacles. Before obtaining the current control point and determining the nearest obstacle corresponding to the current control point and the actual distance between the current control point and the nearest obstacle based on the coordinate information of the current control point and the obstacle, the method further includes: Based on the coordinate information of the current control point and the non-obstacle, the coordinate information corresponding to the current control point in the map is determined.

3. The method according to claim 2, characterized in that The determining, based on the coordinate information of the current control point and the obstacle, the nearest obstacle corresponding to the current control point and the actual distance between the current control point and the nearest obstacle includes: Based on the coordinate information corresponding to the current control point in the map and the coordinate information of the obstacle, obtaining the coordinate information of the nearest obstacle corresponding to the current control point; The actual distance is obtained based on the coordinate information corresponding to the current control point in the map and the coordinate information of the nearest obstacle corresponding to the current control point.

4. The method according to claim 2, characterized in that: The obtaining the orientation angle value between the vehicle model and the nearest obstacle based on the coordinate information of the current control point, the historical control points before the current control point, and the nearest obstacle corresponding to the current control point includes: Based on the coordinate information of the historical control point and the non-obstacle, determining the coordinate information corresponding to the historical control point in the map; Based on the coordinate information corresponding to the current control point in the map and the coordinate information corresponding to the historical control point in the map, determine the direction of the current control point and the coordinate information corresponding to the coordinate point in the direction in the map; wherein the coordinate point is any point on the extension line of the line connecting the current control point and the historical control point in the direction; Based on the coordinate information corresponding to the current control point in the map, the coordinate information corresponding to the coordinate point in the map, and the coordinate information of the nearest obstacle corresponding to the current control point, a heading angle value between the vehicle model and the nearest obstacle is obtained.

5. The method according to claim 1, characterized in that The safety threshold is obtained by querying a distance query table, and the distance query table is obtained based on the following steps: Acquire the outer contour of the vehicle model; wherein the outer contour is a closed curve, and the vehicle model includes a motion center; Based on the motion center, a straight line is determined, and the straight line is intersected with the outer contour according to different directions to obtain an intersection point; The distance between the movement center and the intersection point is used as a safety threshold for different orientations and saved to obtain the distance query table.

6. The method according to claim 1, characterized in that The step of optimizing each of the control points based on the safety threshold and the actual distance, and generating a corrected path using all the optimized control points, comprises: In response to the actual distance being greater than the safety threshold, taking the current control point as an optimized control point; In response to the actual distance being less than the safety threshold, obtaining a difference between the actual distance and the safety threshold, and optimizing the current control point based on the difference to obtain an optimized control point; In response to the optimization times satisfying the optimization termination condition, the updated position of each control point is determined, and the correction path is generated based on the positions of all updated control points.

7. The method according to claim 6, characterized in that The optimizing the current control point based on the difference to obtain an optimized control point includes: Based on the difference, obtaining a cost value corresponding to the current control point; The current control point is optimized based on the cost value to obtain an optimized control point.

8. The method according to claim 1, characterized in that The step of obtaining a plurality of control points corresponding to the original path of the vehicle model in the map includes: Obtaining a coordinate set of multiple original path points in the original path; B-spline fitting is performed on the coordinate set to obtain a plurality of control points corresponding to the original path.

9. An electronic device, characterized in that: The method comprises a memory and a processor coupled to each other, wherein the memory stores program instructions, and the processor is used to execute the program instructions to implement the method according to any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that: Program instructions that can be executed by a processor are stored, and the program instructions are used to implement the method according to any one of claims 1 to 8.

Images