A path trajectory correction method, device, equipment and medium

Abstract

The application discloses a path trajectory correction method, device, equipment and medium. In the method, the coordinate set of the initial path point of the to-be-corrected path is acquired, B-spline fitting is performed to obtain the position of each control point of the to-be-corrected path, for each control point, the angle difference between the line direction of the control point and the last control point and the line direction of the last control point and the control point before the last control point is determined according to the position of the control point and the positions of the adjacent two control points before the control point, and / or the distance of the nearest obstacle corresponding to the control point is determined according to the pre-stored ESDF map, the updated position of the control point is determined through angle optimization and / or obstacle distance optimization, and the position of each path point after trajectory modification is obtained through B-spline distance sampling, so that the problems of path close to obstacles, danger and / or large angle deflection leading to position deviation are solved, and the operation efficiency of the forklift AGV is improved.

Description

Technical Field

[0001] The present invention relates to the field of wireless technology, and in particular to a path trajectory correction method, device, equipment and medium. Background Art

[0002] Industrial robots have developed rapidly in recent years, and forklift-type automated guided vehicles (AGVs) play a vital role in cargo handling in the warehousing and logistics industries. Because forklift-type AGVs are more complex than other types of industrial robots and have more stringent path requirements, traditional path planning algorithms sometimes produce inefficient paths for forklift-type AGVs. For example, paths can be dangerously close to obstacles and / or have excessive angular deflections, leading to positional deviations and thus inefficient operation. Therefore, how to correct the trajectory of forklift-type AGVs to improve their efficiency has become a pressing issue. Summary of the Invention

[0003] The present invention provides a path trajectory correction method, device, equipment and medium to solve the problem of low operating efficiency of forklift-type AGV in the prior art.

[0004] The present invention provides a path trajectory correction method, the method comprising:

[0005] Obtaining the input coordinate set of the initial path points of the path to be corrected, and performing B-spline fitting to obtain the position of each control point of the path to be corrected;

[0006] For each control point, determining, based on the control point and the two adjacent control points before the control point, a first orientation angle value of the direction of the line connecting the control point and the previous control point, and a second orientation angle value of the direction of the line connecting the previous control point and the previous control point, and determining an angle difference between the second orientation angle value and the first orientation angle value, and / or determining, based on the position of the control point and a pre-saved Euclidean Signed Distance Field (ESDF) map, the distance to the nearest obstacle corresponding to the control point, and performing angle optimization and / or obstacle distance optimization based on the angle difference and / or distance corresponding to the control point to determine the updated position of the control point;

[0007] According to the updated position of each control point, B-spline distance sampling is performed to obtain the position of each path point after the path trajectory to be corrected is modified.

[0008] Furthermore, determining the distance to the nearest obstacle corresponding to the control point according to the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map includes:

[0009] According to the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map, determining the first target voxel corresponding to the position of the control point in the ESDF map, and determining the stored distance corresponding to the first target voxel as the distance to the nearest obstacle corresponding to the control point; or

[0010] According to the position of the control point and the Euclidean Signed Distance Field (ESDF) map, a first target voxel corresponding to the position of the control point is determined in the ESDF map; based on a pre-stored automatic guided vehicle (AGV) model and a set number of preset sphere models, the position of the motion center of the AGV model is placed at the first target voxel of the ESDF map; each second target voxel corresponding to the center position of each sphere of each preset sphere model on the AGV model is obtained; and based on the stored distance corresponding to each second target voxel, a minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point; or,

[0011] According to the position of the control point and the Euclidean signed distance field (ESDF) map, the first target voxel corresponding to the position of the control point is determined in the ESDF map. According to the pre-saved automatic guided vehicle (AGV) model and each preset surface point of the AGV model, the position of the motion center of the AGV model is placed at the first target voxel, and each third target voxel point corresponding to each preset surface point of the AGV model is obtained. According to the saved distance corresponding to each third target voxel, the minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point.

[0012] Furthermore, performing angle optimization to determine the updated position of the control point according to the angle difference corresponding to the control point includes:

[0013] If the angle difference is greater than a preset angle threshold, the difference between the angle difference and the preset angle threshold is determined, the product of the square of the difference and a preset weight value is determined as the target cost value of the control point, and the product vector of the target cost value and a preset standard gradient vector is determined as the target gradient vector of the control point; if the angle difference is not greater than the preset angle threshold, the preset cost value is determined as the target cost value of the control point, and the preset gradient vector is determined as the target gradient vector of the control point;

[0014] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization. According to the position of the control point after angle optimization, the angle optimization is repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0015] Furthermore, performing obstacle distance optimization to determine the updated position of the control point according to the distance corresponding to the control point includes:

[0016] If the distance is greater than a preset distance threshold, determining a difference between the distance and the preset distance threshold, determining the product of the square of the difference and a preset weight value as the target cost value of the control point, and determining the product vector of the target cost value and a preset standard gradient vector as the target gradient vector of the control point; if the distance is not greater than the preset distance threshold, determining the preset cost value as the target cost value of the control point, and determining the preset gradient vector as the target gradient vector of the control point;

[0017] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after obstacle distance optimization. According to the position of the control point after obstacle distance optimization, the obstacle distance optimization is repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0018] Furthermore, performing angle optimization and obstacle distance optimization to determine the updated position of the control point based on the angle difference and distance corresponding to the control point includes:

[0019] According to the angle difference and distance corresponding to the control point, the angle difference is determined as the first value for angle optimization of the control point, and the distance is determined as the second value for obstacle distance optimization of the control point;

[0020] For each value, if the value is greater than the corresponding preset value threshold, the difference between the value and the corresponding preset value threshold is determined, the product of the square of the difference and the preset weight value is determined as the cost value of the control point, and the product vector of the cost value and the preset standard gradient vector is determined as the gradient vector of the control point; if the value is not greater than the corresponding preset value threshold, the preset cost value is determined as the cost value of the control point, and the preset gradient vector is determined as the gradient vector of the control point;

[0021] The sum of the first cost value corresponding to the first value of the control point and the second cost value corresponding to the second value is determined as the target cost value for angle optimization and obstacle distance optimization; the sum of the first gradient vector corresponding to the first value of the control point and the second gradient vector corresponding to the second value is determined as the target gradient vector for angle optimization and obstacle distance optimization;

[0022] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization and obstacle distance optimization. According to the position of the control point after angle optimization and obstacle distance optimization, the angle optimization and obstacle distance optimization are repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0023] Furthermore, before determining the product of the square of the difference and the preset weight value as the cost value of the control point, the method further includes:

[0024] Determine the target preset weight value corresponding to the control point based on the target sequence number corresponding to the control point determined in order according to the path, and a pre-stored functional relationship between the control point of each sequence number and the preset weight value, wherein the preset weight value of the starting point and the end point of each control point is greater than the preset weight value of the intermediate point;

[0025] The step of determining the product of the square of the difference value and the preset weight value as the cost value of the control point comprises:

[0026] The cost value of the control point is determined by multiplying the square of the difference value and the target preset weight value.

[0027] Furthermore, before performing B-spline distance sampling to obtain the position of each path point after modification of the path trajectory to be corrected based on the updated position of each control point, the method further includes:

[0028] For each control point except the first control point, determine the distance between the control point and the previous control point based on the updated position of the control point and the control point before the control point, and determine the quotient of the distance and the preset interval distance as the number of intervals between the control point and the previous control point; search the coefficients corresponding to the control point and the previous control point in a pre-stored coefficient table, and evenly divide the coefficients corresponding to the previous control point and the control point according to the number of intervals to obtain each sub-coefficient of the evenly divided coefficients between the coefficients corresponding to the previous control point and the control point, where the number of each sub-coefficient is the difference between the number of intervals minus 1;

[0029] Adding each corresponding sub-coefficient between the coefficients corresponding to each two adjacent control points in the coefficient table in numerical order to obtain a target coefficient table after adding each sub-coefficient;

[0030] The performing B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified, comprising:

[0031] For each coefficient in the target coefficient table, based on the coefficient and the updated position of each control point, determine the sum of the product values ​​of the coefficient and the updated position of each control point, and determine the sum as the position of the path point corresponding to the coefficient.

[0032] Accordingly, the present invention provides a path trajectory correction device, the device comprising:

[0033] An acquisition module is used to obtain a coordinate set of the initial path points of the input path to be corrected, and perform B-spline fitting to obtain the position of each control point of the path to be corrected;

[0034] an optimization module for determining, for each control point, a first orientation angle value of a line connecting the control point and a previous control point, and a second orientation angle value of a line connecting the previous control point and the next previous control point based on the control point and the two adjacent control points before the control point, and determining an angular difference between the second orientation angle value and the first orientation angle value, and / or determining a distance to the nearest obstacle corresponding to the control point based on the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map, and performing angle optimization and / or obstacle distance optimization to determine an updated position of the control point based on the angular difference and / or distance corresponding to the control point;

[0035] The sampling module is used to perform B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified.

[0036] Furthermore, the optimization module is specifically used to determine the first target voxel corresponding to the position of the control point in the ESDF map according to the position of the control point and a pre-saved Euclidean signed distance field ESDF map, and determine the saved distance corresponding to the first target voxel as the distance to the nearest obstacle corresponding to the control point; or, according to the position of the control point and the Euclidean signed distance field ESDF map, determine the first target voxel corresponding to the position of the control point in the ESDF map, and according to a pre-saved automatic guided vehicle AGV model and a set number of each preset spherical model, place the position of the motion center of the AGV model at the first target voxel of the ESDF map to obtain the distance of each preset spherical model on the AGV model. For each second target voxel corresponding to each sphere center position of the model, according to the corresponding saved distance of each second target voxel, the minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point; or, according to the position of the control point and the Euclidean signed distance field ESDF map, the first target voxel corresponding to the position of the control point is determined in the ESDF map, and according to the pre-saved automatic guided vehicle AGV model and each preset surface point of the AGV model, the position of the motion center of the AGV model is placed at the first target voxel to obtain each third target voxel point corresponding to each preset surface point of the AGV model, and according to the corresponding saved distance of each third target voxel, the minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point.

[0037] Furthermore, the optimization module is specifically used to determine the difference between the angle difference and the preset angle threshold if the angle difference is greater than a preset angle threshold, determine the product of the square of the difference and a preset weight value as the target cost value of the control point, and determine the product vector of the target cost value and a preset standard gradient vector as the target gradient vector of the control point; if the angle difference is not greater than the preset angle threshold, determine the preset cost value as the target cost value of the control point, and determine the preset gradient vector as the target gradient vector of the control point; input the target cost value and the target gradient vector into a pre-saved optimizer to obtain the output position of the control point after angle optimization, and repeat the angle optimization according to the position of the control point after angle optimization until the number of optimizations meets the optimization termination condition, and determine the updated position of the control point.

[0038] Furthermore, the optimization module is specifically configured to, if the distance is greater than a preset distance threshold, determine the difference between the distance and the preset distance threshold, determine the product of the square of the difference and a preset weight value as the target cost value of the control point, and determine the product vector of the target cost value and a preset standard gradient vector as the target gradient vector of the control point; if the distance is not greater than the preset distance threshold, determine the preset cost value as the target cost value of the control point, and determine the preset gradient vector as the target gradient vector of the control point; input the target cost value and the target gradient vector into a pre-saved optimizer to obtain the output position of the control point after obstacle distance optimization; and repeat the obstacle distance optimization according to the position of the control point after obstacle distance optimization until the number of optimizations meets the optimization termination condition, and then determine the updated position of the control point.

[0039] Furthermore, the optimization module is specifically used to determine the angle difference as the first value of the control point for angle optimization and the distance as the second value of the control point for obstacle distance optimization according to the angle difference and distance corresponding to the control point; for each value, if the value is greater than the corresponding preset value threshold, the difference between the value and the corresponding preset value threshold is determined, and the product value of the square of the difference and the preset weight value is determined as the cost value of the control point, and the product vector of the cost value and the preset standard gradient vector is determined as the gradient vector of the control point; if the value is not greater than the corresponding preset value threshold, the preset cost value is determined as the cost value of the control point, and the preset gradient vector is determined is the gradient vector of the control point; the sum of the first cost value corresponding to the first value of the control point and the second cost value corresponding to the second value is determined as the target cost value for angle optimization and obstacle distance optimization, and the sum of the first gradient vector corresponding to the first value of the control point and the second gradient vector corresponding to the second value is determined as the target gradient vector for angle optimization and obstacle distance optimization; the target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization and obstacle distance optimization, and according to the position of the control point after angle optimization and obstacle distance optimization, the angle optimization and obstacle distance optimization are repeated until the number of optimizations meets the optimization termination condition, and the updated position of the control point is determined.

[0040] Furthermore, the optimization module is also used to determine the target preset weight value corresponding to the control point according to the corresponding target serial number determined by the control point in the order of the path, and the pre-saved functional relationship between the control point of each serial number and the preset weight value before determining the product value of the square of the difference value and the preset weight value as the cost value of the control point, wherein the preset weight value of the starting point and the end point of each control point is greater than the preset weight value of the middle point; specifically, it is used to determine the cost value of the control point by multiplying the square of the difference value and the target preset weight value.

[0041] Furthermore, the sampling module is further configured to perform B-spline distance sampling to obtain the position of each path point after the modification of the path trajectory to be corrected based on the updated position of each control point, and for each control point except the first control point, determine the distance between the control point and the previous control point based on the updated position of the control point and the updated position of the previous control point of the control point, and determine the quotient of the distance and the preset interval distance as the number of intervals between the control point and the previous control point; search for the coefficient corresponding to the control point and the previous control point in a pre-stored coefficient table, and calculate the coefficient corresponding to the control point and the previous control point based on the number of intervals. , and obtain each sub-coefficient equally divided between the coefficients corresponding to the previous control point and the control point, wherein the number of each sub-coefficient is the difference between the number of intervals minus 1; in the coefficient table, each sub-coefficient is added between the coefficients corresponding to every two adjacent control points in the coefficient table in numerical order to obtain a target coefficient table after adding each sub-coefficient; specifically, for each coefficient in the target coefficient table, according to the coefficient and the updated position of each control point, determine the sum of the product values ​​of the coefficient and the updated position of each control point, and determine the sum as the position of the path point corresponding to the coefficient.

[0042] Accordingly, the present invention provides an electronic device, comprising: a processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory communicate with each other via the communication bus;

[0043] The memory stores a computer program, and when the program is executed by the processor, the processor is caused to execute the steps of any one of the above-mentioned path trajectory correction methods.

[0044] Accordingly, the present invention provides a computer-readable storage medium storing a computer program, wherein the computer program, when executed by a processor, implements the steps of any of the above-mentioned path trajectory correction methods.

[0045] The present invention provides a path trajectory correction method, device, equipment and medium. In this method, a coordinate set of initial path points of a path to be corrected is obtained, and B-spline fitting is performed to obtain the position of each control point of the path to be corrected. For each control point, based on the positions of the control point and the two adjacent control points before the control point, the angle difference between the direction of the line connecting the control point and the previous control point and the direction of the line connecting the previous control point and the next previous control point is determined, and / or based on a pre-stored Euclidean signed distance field (ESDF) map, the distance to the nearest obstacle corresponding to the control point is determined, thereby performing angle optimization and / or obstacle distance optimization to determine the updated position of the control point; and based on the updated position of each control point, B-spline distance sampling is performed to obtain the position of each path point after the trajectory is modified; since the control points of the path to be corrected are optimized in angle and / or obstacle distance, the problems of the danger of the path being close to an obstacle and / or the position deviation caused by excessive angle deflection are solved, thereby improving the operating efficiency of the forklift-type AGV. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, 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 invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0047] Figure 1 A schematic diagram of a path trajectory correction process provided by an embodiment of the present invention;

[0048] Figure 2 A schematic diagram of an ESDF map generation process provided by an embodiment of the present invention;

[0049] Figure 3 A schematic diagram of an ESDF map update process provided by an embodiment of the present invention;

[0050] Figure 4 A top view of an AGV model including a set number of spherical models provided in an embodiment of the present invention;

[0051] Figure 5 A side view of an AGV model including a set number of spherical models provided by an embodiment of the present invention;

[0052] Figure 6 A schematic diagram of angle optimization provided by an embodiment of the present invention;

[0053] Figure 7 A schematic diagram of a process of angle optimization and obstacle distance optimization provided by an embodiment of the present invention;

[0054] Figure 8 A schematic diagram of a functional relationship between each numbered control point and a preset weight value provided by an embodiment of the present invention;

[0055] Figure 9 A schematic diagram of B-spline distance sampling provided by an embodiment of the present invention;

[0056] Figure 10 A schematic diagram of a B-spline distance uniform sampling provided by an embodiment of the present invention;

[0057] Figure 11 A schematic diagram of a process of a path trajectory correction method provided by an embodiment of the present invention;

[0058] Figure 12 A schematic structural diagram of a path trajectory correction device provided by an embodiment of the present invention;

[0059] Figure 13 This is a schematic diagram of the structure of an electronic device provided in this application. DETAILED DESCRIPTION

[0060] To make the objectives, technical solutions, and advantages of the present invention more apparent, the present invention will be further described in detail below with reference to the accompanying drawings. It is apparent that the embodiments described are only some, not all, of the present invention. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without creative effort are intended to fall within the scope of protection of the present invention.

[0061] An existing AGV path planning method and device in a warehouse environment determines the optimal trajectory as the final path based on the total score of five aspects: Euclidean distance from the starting and end points, heading angle, obstacle distance, linear speed, and Manhattan distance from the end point. However, this solution incorporates angle and obstacle constraints during the initial path search. The excessive number of constraints can easily lead to a slow search speed. In addition, the specific calculation methods for distance and angle items are not specified, and obstacles are calculated based on two-dimensional distance.

[0062] The existing paper FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Areal Robots proposes the concept of Euclidean signed distance field maps. Specifically, this approach divides three-dimensional space into voxels of equal size and calculates the distance from each voxel to its nearest obstacle. This distance is then used to determine the distance from the generated initial path to the obstacle, allowing optimization to avoid obstacles. However, its main drawback is its application to drones, which can be treated as a sphere to calculate the distance from the drone to the obstacle (i.e., the distance from the center of the circle to the obstacle). AGV models have irregular shapes, and using a sphere for calculations can waste space for obstacle avoidance.

[0063] The existing paper "Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight" mainly uses B-splines to optimize the initial path after searching for it based on the smoothness term, ESDF distance term, and deviation from the original path term. This ensures that the final path is away from obstacles while maintaining its original shape and smoothness. However, its main drawback is that it is also applied to drones. Drones have no steering requirements and can turn 360 degrees, but AGV models are prone to positioning errors if they turn too far in place.

[0064] In order to improve the operating efficiency of a forklift-type AGV, embodiments of the present invention provide a path trajectory correction method, apparatus, device, and medium.

[0065] Example 1:

[0066] Figure 1 A schematic diagram of a path trajectory correction method provided by an embodiment of the present invention includes the following steps:

[0067] S101: Obtain an input coordinate set of initial path points of a path to be corrected, and perform B-spline fitting to obtain the position of each control point of the path to be corrected.

[0068] In order to improve the operating efficiency of forklift-type AGVs, a path trajectory correction method provided in an embodiment of the present invention is applied to electronic devices, wherein the electronic device can be a host, a tablet computer, a laptop computer, a smart phone and other intelligent terminal devices, or a server, wherein the server can be a local server or a cloud server, and the embodiment of the present invention does not impose any restrictions on this.

[0069] The electronic device obtains a coordinate set of the input initial path points of the path to be corrected, wherein the path to be corrected is a traversable path obtained by searching in advance based on the grayscale map of the starting point, end point and obstacles using an existing path search algorithm. The coordinate set of the initial path points is list(x, y, z), where x represents the x-axis coordinate of the initial path point, y represents the y-axis coordinate of the initial path point, and z represents the z-axis coordinate of the initial path point. List represents the coordinate set of each initial path point, wherein since the AGV can only move on a plane, the z-axis coordinates of the initial path points are all 0.

[0070] Specifically, the electronic device may obtain a coordinate set input by a user through an input device such as a keyboard or a touch screen, or the electronic device may be connected to a network of other devices to obtain and send a coordinate set of initial path points of the path to be corrected searched by other devices.

[0071] The electronic device calculates the coordinates of each control point on the path to be corrected based on the coordinates of each initial path point in the coordinate set using the existing basic B-spline formula, and uses these coordinates as the position of each control point. The control points have a convex hull property, meaning that the path generated by the control points must be within the range enclosed by the control points. Therefore, ensuring the safety of the control points ensures the safety of the path points.

[0072] S102: For each control point, determine, based on the control point and the two adjacent control points before the control point, a first orientation angle value of the direction of the line connecting the control point and the previous control point, and a second orientation angle value of the direction of the line connecting the previous control point and the previous control point, and determine an angular difference between the second orientation angle value and the first orientation angle value, and / or determine, based on the position of the control point and a pre-stored Euclidean Signed Distance Functions (ESDF) map, the distance to the nearest obstacle corresponding to the control point, and perform angle optimization and / or obstacle distance optimization based on the angle difference and / or distance corresponding to the control point to determine the updated position of the control point.

[0073] In order to solve the problem of dangerous paths approaching obstacles and / or position deviations caused by excessive angle deflections, the electronic device optimizes the angles of the control points and / or the obstacle distances. The purpose of the angle optimization is to ensure that when the AGV model runs according to the path points, the angle deflection of each section will not be too large. Therefore, the angle difference between adjacent control points needs to be optimized and reduced.

[0074] In order to solve the problem of position deviation caused by excessive angle deflection, the electronic device determines, for each control point, the first connecting line between the control point and the previous control point, and the second connecting line between the previous control point and the previous control point based on the positions of the control point and the two adjacent control points before the control point, and determines the angle value of the angle between the first connecting line and the positive direction of the horizontal coordinate axis as the first orientation angle value, and determines the angle value of the angle between the second connecting line and the positive direction of the horizontal coordinate axis as the second orientation angle value.

[0075] Specifically, based on the positions of the control point and the previous control point of the control point, a first ratio of the difference between the ordinates and the horizontal coordinates of the control point and the previous control point is determined, and the first ratio is input into the inverse tangent function to obtain a first orientation angle value; based on the positions of the previous control point and the previous control point, a first ratio of the difference between the ordinates and the horizontal coordinates of the previous control point and the previous control point is determined, and the first ratio is input into the inverse tangent function to obtain a first orientation angle value; based on the first orientation angle value and the second orientation angle value, the angle difference between the second orientation angle value and the first orientation angle value is determined.

[0076] Specifically, the calculation function of the orientation angle value of each control point is Where py represents the ordinate of the current control point, px represents the abscissa of the current control point, p-1.y represents the ordinate of the previous control point of the current control point, and p-1.x represents the abscissa of the previous control point of the current control point.

[0077] To address the danger of obstacles approaching the path, the electronic device determines the first target voxel corresponding to the control point's location in the ESDF map based on the control point's location and a pre-stored ESDF map. The distance stored in the first target voxel is then used as the distance to the nearest obstacle corresponding to the control point. The ESDF map divides the three-dimensional space into multiple voxels, each of which is equivalent to a three-dimensional grid and stores a pre-calculated distance value to its nearest obstacle.

[0078] Figure 2 A schematic diagram of an ESDF map generation process provided by an embodiment of the present invention is as follows Figure 2 As shown, the following steps are included: S201: Obtain input obstacle map information; wherein the obstacle map is a grayscale image, and the obstacle map information only has two values: obstacle and non-obstacle; S202: Calculate the distance value of each voxel from its nearest obstacle according to the obstacle map information, and establish an ESDF map.

[0079] As a possible implementation method, in an embodiment of the present invention, after the ESDF map is pre-generated and saved, the ESDF map can be updated at preset intervals. Figure 3 A schematic diagram of an ESDF map update process provided by an embodiment of the present invention is as follows Figure 3 As shown, the following steps are included: S301: Obtain obstacle coordinate information at every preset time interval; S302: Add the obstacle coordinate information to the pre-saved ESDF map and update the ESDF map.

[0080] According to the angle difference and / or distance corresponding to the control point, angle optimization and / or obstacle distance optimization are performed to determine the updated position of the control point. Specifically, the angle difference and / or distance are input into a pre-saved optimizer for optimization, thereby obtaining the updated position of the control point.

[0081] S103: performing B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified.

[0082] Based on the basic principle of B-spline, the coefficients of each control point in the coefficient table are evenly distributed. Therefore, according to the number of each control point, the coefficient corresponding to each control point is evenly divided within the preset numerical range, where the preset numerical range is 0 to 1. For example, when the number of control points is 4, the coefficients corresponding to each control point obtained by even division are 0, 1 / 3, 2 / 3 and 1.

[0083] As a possible implementation, in that embodiment of the present invention, the coefficients corresponding to the starting point and the end point in the control point can be repeated three times. For example, the coefficients corresponding to each control point are 0, 0, 0, 1 / 3, 2 / 3, 1, 1 and 1. The three repetitions are to allow the final path to pass through the first and last control points.

[0084] According to the coefficients corresponding to each two adjacent control points, the average value of the coefficients corresponding to each two adjacent control points is determined, and the average value is added between the coefficients corresponding to each two adjacent control points in the coefficient table to obtain the target coefficient table. For each coefficient in the target coefficient table, the coefficient and the updated position of each control point are input into the pre-saved B-spline distance sampling position determination function to obtain the position of the path point corresponding to the coefficient. According to the position of the path point corresponding to each coefficient, the path after trajectory correction is obtained.

[0085] Specifically, the position determination function of B-spline distance sampling is Where t represents the number of the coefficient in the target coefficient table, i represents the sequence number of the control point, and C(t) is the position corresponding to the t-th coefficient in the path obtained by B-spline sampling. i,deg (t) represents the t-th coefficient, deg represents the order, P iThe method of using B-spline distance sampling to determine the position of each path point belongs to the prior art and will not be described in detail in the embodiment of the present invention.

[0086] In an embodiment of the present invention, the method obtains a coordinate set of initial path points of the path to be corrected, performs B-spline fitting to obtain the position of each control point of the path to be corrected, and for each control point, determines the angle difference between the direction of the line connecting the control point and the previous control point and the direction of the line connecting the previous control point and the next previous control point based on the positions of the control point and the two adjacent control points before the control point, and / or determines the distance to the nearest obstacle corresponding to the control point based on a pre-saved Euclidean signed distance field (ESDF) map, thereby performing angle optimization and / or obstacle distance optimization to determine the updated position of the control point; and performs B-spline distance sampling based on the updated position of each control point to obtain the position of each path point after the trajectory is modified; since the control points of the path to be corrected are optimized in angle and / or obstacle distance, the problems of dangerous path proximity to obstacles and / or position deviation caused by excessive angle deflection are solved, thereby improving the operating efficiency of the forklift AGV.

[0087] Example 2:

[0088] In order to determine the distance to the nearest obstacle corresponding to the control point, based on the above embodiment, in an embodiment of the present invention, determining the distance to the nearest obstacle corresponding to the control point according to the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map includes:

[0089] According to the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map, determining the first target voxel corresponding to the position of the control point in the ESDF map, and determining the stored distance corresponding to the first target voxel as the distance to the nearest obstacle corresponding to the control point; or

[0090] According to the position of the control point and the Euclidean Signed Distance Field (ESDF) map, a first target voxel corresponding to the position of the control point is determined in the ESDF map; based on a pre-stored automatic guided vehicle (AGV) model and a set number of preset sphere models, the position of the motion center of the AGV model is placed at the first target voxel of the ESDF map; each second target voxel corresponding to the center position of each sphere of each preset sphere model on the AGV model is obtained; and based on the stored distance corresponding to each second target voxel, a minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point; or,

[0091] According to the position of the control point and the Euclidean signed distance field (ESDF) map, the first target voxel corresponding to the position of the control point is determined in the ESDF map. According to the pre-saved automatic guided vehicle (AGV) model and each preset surface point of the AGV model, the position of the motion center of the AGV model is placed at the first target voxel, and each third target voxel point corresponding to each preset surface point of the AGV model is obtained. According to the saved distance corresponding to each third target voxel, the minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point.

[0092] In order to determine the closest distance to an obstacle corresponding to a control point, in an embodiment of the present invention, the electronic device determines the first target voxel corresponding to the position of the control point in the ESDF map based on the position of the control point and a pre-saved ESDF map, wherein each voxel in the ESDF map stores the distance to the nearest obstacle of the voxel itself, and determines that the distance to the nearest obstacle of the first target voxel itself stored corresponding to the first target voxel is the distance to the nearest obstacle corresponding to the control point.

[0093] According to the position of the control point and the pre-saved ESDF map, the first target voxel corresponding to the position of the control point is determined in the ESDF map. According to the pre-saved automatic guided vehicle AGV model and a set number of preset spherical models, wherein the set number of each spherical model is located at a fixed position of the AGV model and includes a partial area of ​​the AGV model, the set number can be 1 or any value greater than 1, the position of the motion center of the AGV model is placed at the first target voxel in the ESDF map, and each second target voxel corresponding to each sphere center position of each preset spherical model on the AGV model in the ESDF map is determined. According to the distance saved corresponding to each second target voxel, the minimum distance in each distance is determined, and the minimum distance is determined as the distance to the nearest obstacle corresponding to the control point.

[0094] Figure 4 A top view of an AGV model including a set number of spherical models provided in an embodiment of the present invention, such as Figure 4 As shown, the dotted circle represents the top view of the spherical model, and the solid line represents the top view of the AGV model. There are two spherical models on the AGV model, one spherical model contains the forklift body of the AGV model, and the other spherical model contains the fork ruler of the AGV model.

[0095] Figure 5 A side view of an AGV model including a set number of spherical models provided by an embodiment of the present invention, such as Figure 5As shown, the dotted circle represents the top view of the spherical model, and the solid line represents the top view of the AGV model. The spherical model containing the forklift body on the AGV model also includes a part of the cargo area, and the spherical model containing the fork ruler on the AGV model also includes a part of the cargo area.

[0096] When a single sphere model is used to include the entire AGV model, obstacle avoidance misjudgment may occur. That is, the vehicle can actually pass through, but the distance to the nearest obstacle determined by the single sphere model is not passable, thus wasting obstacle avoidance space. Compared with the solution of using a single sphere model, the use of multiple sphere models can save more obstacle avoidance space and reduce the occurrence of obstacle avoidance misjudgment. In addition, the more multiple sphere models there are, the more precise the distance to the nearest obstacle can be determined.

[0097] According to the position of the control point and the pre-saved ESDF map, the first target voxel corresponding to the position of the control point is determined in the ESDF map. According to the pre-saved automatic guided vehicle AGV model and each preset surface point of the AGV model, the position of the motion center of the AGV model is placed at the first target voxel in the ESDF map. Each third target voxel point corresponding to each preset surface point on the AGV model in the ESDF map is determined. According to the saved distance corresponding to each third target voxel, the minimum distance is determined, and the minimum distance is determined as the distance to the nearest obstacle corresponding to the control point.

[0098] In order to achieve three-dimensional obstacle avoidance, the distance value stored for each voxel to its nearest obstacle has Z-axis information.

[0099] Example 3:

[0100] In order to determine the updated position of the control point after the angle optimization, based on the above embodiments, in an embodiment of the present invention, performing angle optimization to determine the updated position of the control point according to the angle difference corresponding to the control point includes:

[0101] If the angle difference is greater than a preset angle threshold, the difference between the angle difference and the preset angle threshold is determined, the product of the square of the difference and a preset weight value is determined as the target cost value of the control point, and the product vector of the target cost value and a preset standard gradient vector is determined as the target gradient vector of the control point; if the angle difference is not greater than the preset angle threshold, the preset cost value is determined as the target cost value of the control point, and the preset gradient vector is determined as the target gradient vector of the control point;

[0102] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization. According to the position of the control point after angle optimization, the angle optimization is repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0103] In order to achieve angle optimization, a preset angle threshold is saved in an embodiment of the present invention, wherein the preset angle threshold is used to determine whether the problem of excessive angle deflection causing position deviation will occur. If it is greater than the preset angle threshold, it is determined that the angle deflection is large; if it is not greater than the preset angle threshold, it is determined that the angle deviation is small. The preset angle threshold is set in advance by the user. If you want to reduce the problem of position deviation caused by excessive angle deflection, you can set the preset angle threshold to be smaller. If you want to reduce the number of control points for angle optimization, you can set the preset angle threshold to be larger.

[0104] Based on the angle difference between the determined second orientation angle value and the first orientation angle value, determine whether the angle difference is greater than the preset angle threshold. If the angle difference is greater than the preset angle threshold, it is determined that the angle deflection is large. Therefore, the difference between the angle difference and the preset angle threshold is determined, and the product of the square of the difference and the preset weight value is determined as the target cost value of the control point; the preset weight value is pre-set by the user. If you want to improve the precision of the angle difference judgment, you can set the preset weight value larger. If you want to lower the priority of angle optimization and give priority to other optimization items, you can set the preset weight value smaller.

[0105] Specifically, according to the angle difference between the second orientation angle value and the first orientation angle value, and the preset angle threshold, the angle difference and the preset angle threshold are input into the target cost value determination function cost=k×(value-value0) 2 , where cost represents the target cost value, k represents the preset weight value, value is the angle difference between the second orientation angle value and the first orientation angle value, and value0 is the preset angle threshold. The smaller the cost, the more ideal the angle optimization effect is and the more in line with expectations it is.

[0106] According to the target cost value and the pre-saved preset standard gradient vector, the product vector of the target cost value and the preset standard gradient vector is determined, and the product vector is determined as the target gradient vector of the control point, where the structure of the preset standard gradient vector is (x, y, z), which represents the preset standard gradient directions of the x-axis, y-axis, and z-axis, respectively.

[0107] Specifically, according to the angle difference between the second orientation angle value and the first orientation angle value, the preset angle threshold and the preset standard gradient vector, the angle difference and the preset angle threshold are input into the target gradient vector determination function gradient = k × grad × (value - value0) 2 , where gradient represents the target gradient vector, grad represents the preset standard gradient vector, value represents the angle difference between the second orientation angle value and the first orientation angle value, and value0 represents the preset angle threshold. This is equivalent to multiplying the target cost value by the preset standard gradient vector to obtain the target gradient vector. The structure of Gradient is also (x, y, z), representing the gradient directions of the x-axis, y-axis, and z-axis, respectively. That is, during optimization, the coordinates of the control point should be moved in the gradient direction of the target gradient vector to reduce the target cost value.

[0108] If the angle difference is not greater than the preset angle threshold, it means that no angle optimization is required. Therefore, the preset cost value is determined as the target cost value of the control point, and the preset gradient vector is determined as the target gradient vector of the control point, where the preset cost value is 0 and the preset gradient vector is also (0, 0, 0).

[0109] In order to achieve angle optimization, the target cost value and target gradient vector of the control point are input into the pre-saved optimizer, and the position of the control point after angle optimization is obtained from the optimizer. According to the position of the control point after angle optimization, the above steps are repeated to perform angle optimization until the number of optimizations meets the optimization termination condition. The position of the control point after angle optimization output by the optimizer for the last time is determined as the updated position of the control point.

[0110] As a possible implementation method, it can be determined that the optimization termination condition is met when the changes in the target cost value after the angle optimization of the control point for the last set number of times are small; it can also be determined that the optimization termination condition is met when the changes in the position after the angle optimization of the control point for the last set number of times are small.

[0111] Figure 6 A schematic diagram of an angle optimization provided by an embodiment of the present invention is shown as follows: Figure 6 As shown, p1, p2, and p3 are three adjacent control points. Figure 5 The direction of control point p2 is Line1, the direction of control point p3 is Line2, and p4 is the position of control point p3 after angle optimization. If the angle difference between the direction of Line1 and the direction of Line2 is greater than the preset angle threshold, the target cost value and target gradient vector of control point p3 are determined, and the direction of the target gradient vector is Figure 5Specifically, Line 1 is extended by the length of p2p3 to obtain point p4 so that p2p4=p2p3. Line 3 is obtained by connecting p3 and p4. The control point p3 is moved along the direction of Line 3. The angle difference between Line 2 and Line 1 will be reduced, thereby achieving angle optimization.

[0112] Example 4:

[0113] In order to determine the updated position of the control point after the obstacle distance is optimized, based on the above embodiments, in an embodiment of the present invention, performing obstacle distance optimization based on the distance corresponding to the control point to determine the updated position of the control point includes:

[0114] If the distance is greater than a preset distance threshold, determining a difference between the distance and the preset distance threshold, determining the product of the square of the difference and a preset weight value as the target cost value of the control point, and determining the product vector of the target cost value and a preset standard gradient vector as the target gradient vector of the control point; if the distance is not greater than the preset distance threshold, determining the preset cost value as the target cost value of the control point, and determining the preset gradient vector as the target gradient vector of the control point;

[0115] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after obstacle distance optimization. According to the position of the control point after obstacle distance optimization, the obstacle distance optimization is repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0116] In order to optimize the obstacle distance, a preset distance threshold is stored in an embodiment of the present invention, wherein the preset distance threshold is used to determine whether there will be a problem of the path being close to the obstacle and being dangerous. If it is not greater than the preset distance threshold, it is determined that there will be a problem of the path being close to the obstacle and being dangerous. If it is greater than the preset distance threshold, it is determined that there will be no problem of the path being close to the obstacle and being dangerous. The preset distance threshold is set in advance by the user. If it is desired to obtain a more efficient control point so that the corrected path is shorter within a certain range, the preset distance threshold can be set to a smaller value. If it is desired to obtain a safer control point, the preset distance threshold can be set to a larger value.

[0117] According to the distance corresponding to the control point, determine whether the distance is greater than the preset distance threshold. If the distance is greater than the preset distance threshold, it is determined that there will be a problem of the path being close to an obstacle. Therefore, obstacle distance optimization is performed, and the difference between the distance and the preset distance threshold is determined. The square of the difference and the product of the preset weight value are determined as the target cost value of the control point; the preset weight value is pre-set by the user. If you want to improve the precision of distance judgment, you can set the preset weight value to be larger. If you want to reduce the priority of obstacle distance optimization and give priority to meeting other optimization items, you can set the preset weight value to be smaller.

[0118] Specifically, according to the distance corresponding to the control point and the preset distance threshold, the distance and the preset distance threshold are input into the target cost value determination function cost = k × (value - value0) 2 , where cost represents the target cost value, k represents the preset weight value, value represents the distance, and value0 represents the preset distance threshold. The smaller the cost, the more ideal the obstacle distance optimization effect is and the more in line with expectations.

[0119] According to the target cost value and the pre-saved preset standard gradient vector, the product vector of the target cost value and the preset standard gradient vector is determined, and the product vector is determined as the target gradient vector of the control point, where the structure of the preset standard gradient vector is (x, y, z), which represents the preset standard gradient directions of the x-axis, y-axis, and z-axis, respectively.

[0120] Specifically, according to the distance, the preset distance threshold and the preset standard gradient vector, the distance and the preset distance threshold are input into the target gradient vector to determine the function gradient = k × grad × (value - value0) 2 , where gradient represents the target gradient vector, grad represents the preset standard gradient vector, value represents the distance, and value0 represents the preset distance threshold. This is equivalent to multiplying the target cost by the preset standard gradient vector to obtain the target gradient vector. The structure of Gradient is also (x, y, z), representing the gradient directions of the x-axis, y-axis, and z-axis, respectively. That is, when optimizing the obstacle distance, the coordinates of the control point should move in the gradient direction of the target gradient vector to reduce the target cost.

[0121] If the distance is not greater than the preset distance threshold, it means that obstacle distance optimization is not required, so the preset cost value is determined as the target cost value of the control point, and the preset gradient vector is determined as the target gradient vector of the control point, where the preset cost value is 0 and the preset gradient vector is also (0, 0, 0).

[0122] In order to achieve obstacle distance optimization, the target cost value and target gradient vector of the control point are input into the pre-saved optimizer, and the position of the control point after obstacle distance optimization is obtained from the optimizer output. Based on the position of the control point after obstacle distance optimization, the above steps are repeated to optimize the obstacle distance until the number of optimizations meets the optimization termination condition. The position of the control point after obstacle distance optimization output by the optimizer for the last time is determined as the updated position of the control point.

[0123] As a possible implementation method, it can be determined that the optimization termination condition is met when the changes in the target cost value after the obstacle distance optimization of the control point for the last set number of times are small; it can also be determined that the optimization termination condition is met when the changes in the position after the obstacle distance optimization of the control point for the last set number of times are small.

[0124] Example 5:

[0125] In order to determine the updated position of the control point after the angle optimization and the obstacle distance optimization, based on the above embodiments, in an embodiment of the present invention, performing angle optimization and obstacle distance optimization to determine the updated position of the control point according to the angle difference and distance corresponding to the control point includes:

[0126] According to the angle difference and distance corresponding to the control point, the angle difference is determined as the first value for angle optimization of the control point, and the distance is determined as the second value for obstacle distance optimization of the control point;

[0127] For each value, if the value is greater than the corresponding preset value threshold, the difference between the value and the corresponding preset value threshold is determined, the product of the square of the difference and the preset weight value is determined as the cost value of the control point, and the product vector of the cost value and the preset standard gradient vector is determined as the gradient vector of the control point; if the value is not greater than the corresponding preset value threshold, the preset cost value is determined as the cost value of the control point, and the preset gradient vector is determined as the gradient vector of the control point;

[0128] The sum of the first cost value corresponding to the first value of the control point and the second cost value corresponding to the second value is determined as the target cost value for angle optimization and obstacle distance optimization; the sum of the first gradient vector corresponding to the first value of the control point and the second gradient vector corresponding to the second value is determined as the target gradient vector for angle optimization and obstacle distance optimization;

[0129] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization and obstacle distance optimization. According to the position of the control point after angle optimization and obstacle distance optimization, the angle optimization and obstacle distance optimization are repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0130] In order to achieve angle optimization and obstacle distance optimization, in an embodiment of the present invention, the electronic device determines the angle difference as the first value for angle optimization of the control point and determines the distance as the second value for obstacle distance optimization of the control point based on the angle difference and distance corresponding to the control point.

[0131] For each value in the first value and the second value, the electronic device pre-saves a preset value threshold corresponding to each value, wherein the first value corresponds to the preset angle threshold, and the second value corresponds to the preset distance threshold, and determines whether the value is greater than the corresponding preset value threshold. If the value is greater than the corresponding preset value threshold, it means that optimization corresponding to the value is required. If the value is not greater than the corresponding preset value threshold, it means that optimization corresponding to the value is not required. For example, if the value is the first value, it means that angle optimization is required. If the value is the second value, it means that obstacle distance optimization is required.

[0132] If the value is greater than the corresponding preset value threshold, determine the difference between the value and the corresponding preset value threshold, determine the product of the square of the difference and the preset weight value, and determine the product value as the cost value corresponding to the control point. The preset weight value is pre-set by the user. If you want to improve the precision of distance judgment or angle difference judgment, you can set the preset weight value to be larger. If you want to lower the priority of the optimization item corresponding to the value and give priority to other optimization items, you can set the preset weight value to be smaller.

[0133] Specifically, when the angle difference corresponding to the control point is used as the value value, the corresponding preset angle threshold is used as the preset value threshold, and the angle difference corresponding to the control point and the corresponding preset angle threshold are input into the target cost value determination function cost = k × (value - value0) 2, where cost represents the target cost value, k represents the preset weight value, value represents the angle difference corresponding to the control point, and value0 is the preset angle threshold; when the distance corresponding to the control point is used as the value value, the corresponding preset distance threshold is used as the preset value threshold, and the distance corresponding to the control point and the preset distance threshold are input into the target cost value determination function cost = k × (value - value0) 2 , where cost represents the target cost value, k represents the preset weight value, value represents the distance, and value0 represents the preset distance threshold. The smaller the cost, the more ideal the optimization effect and the more in line with expectations.

[0134] According to the cost value and the pre-stored preset standard gradient vector, the product vector of the cost value and the preset standard gradient vector is determined, and the product vector is determined as the gradient vector of the control point; specifically, according to the angle difference, the preset angle threshold and the preset standard gradient vector, the angle difference and the preset angle threshold are input into the target gradient vector determination function gradient = k × grad × (value - value0) 2 , where gradient represents the target gradient vector, grad represents the preset standard gradient vector, value is the angle difference between the second orientation angle value and the first orientation angle value, and value0 is the preset angle threshold; according to the distance, the preset distance threshold and the preset standard gradient vector, the distance and the preset distance threshold are input into the target gradient vector to determine the function gradient = k × grad × (value - value0) 2 , where gradient represents the target gradient vector, grad represents the preset standard gradient vector, value represents the distance, and value0 represents the preset distance threshold. This is equivalent to multiplying the target cost by the preset standard gradient vector to obtain the target gradient vector. The structure of Gradient is also (x, y, z), representing the gradient directions of the x-axis, y-axis, and z-axis, respectively. That is, during optimization, the coordinates of the control point should be moved in the gradient direction of the target gradient vector to reduce the target cost.

[0135] If the value is not greater than the corresponding preset value threshold, it means that there is no need to perform corresponding optimization on the control point. Therefore, the preset cost value is determined as the cost value of the control point, and the preset gradient vector is determined as the gradient vector of the control point, where the preset cost value is 0 and the preset gradient vector is also (0, 0, 0).

[0136] According to the first generation value corresponding to the first value of the control point and the second generation value corresponding to the second value, determine the sum of the first generation value and the second cost value, and determine the sum as the target cost for angle optimization and obstacle distance optimization; according to the first gradient vector corresponding to the first value of the control point and the second gradient vector corresponding to the second value, determine the sum vector of the first gradient vector and the second gradient vector, and determine the sum vector as the target gradient vector for angle optimization and obstacle distance optimization.

[0137] In order to achieve angle optimization and obstacle distance optimization, the target cost value and target gradient vector of the control point are input into the pre-saved optimizer, and the position of the control point after angle optimization and obstacle distance optimization is obtained from the optimizer output. According to the position of the control point after angle optimization and obstacle distance optimization, the above steps are repeated to perform angle optimization and obstacle distance optimization until the number of optimizations meets the optimization termination condition. The position of the control point after angle optimization and obstacle distance optimization output by the optimizer for the last time is determined as the updated position of the control point.

[0138] As a possible implementation method, the third-generation value and third gradient vector of the control point for smoothing term optimization can be determined based on the position of the control point, the sum of the first-generation value, the second-generation value and the third cost value can be determined as the target cost value for angle optimization, obstacle distance optimization and smoothing term optimization, the sum vector of the first gradient vector, the second gradient vector and the third gradient vector can be determined, and the sum vector can be determined as the target gradient vector for angle optimization, obstacle distance optimization and smoothing term optimization.

[0139] As a possible implementation method, it can also be determined that the optimization termination condition is met when the changes in the target cost value after the last set number of angle optimizations and obstacle distance optimizations of the control point are both small; it can also be determined that the optimization termination condition is met when the changes in the position after the last set number of angle optimizations and obstacle distance optimizations of the control point are both small.

[0140] Figure 7 A schematic diagram of the process of angle optimization and obstacle distance optimization provided by an embodiment of the present invention is shown as follows: Figure 7 As shown, the process includes the following steps:

[0141] S701: Obtain the position of each control point.

[0142] S702: For each control point, based on the control point and the two adjacent control points before the control point, determine the first orientation angle value of the direction of the line connecting the control point and the previous control point, and the second orientation angle value of the direction of the line connecting the previous control point and the previous control point, and determine the angle difference between the second orientation angle value and the first orientation angle value, and determine the angle difference as the first value for angle optimization of the control point. Based on the position of the control point and the pre-saved Euclidean Signed Distance Field (ESDF) map, determine the distance to the nearest obstacle corresponding to the control point, and determine the distance as the second value for obstacle distance optimization of the control point.

[0143] S703: For each value, determine whether the value is greater than the corresponding preset value threshold. If so, proceed to S704; if not, proceed to S705.

[0144] S704: Determine the difference between the value and the corresponding preset value threshold, determine the product of the square of the difference and the preset weight value as the cost value of the control point, determine the product vector of the cost value and the preset standard gradient vector as the gradient vector of the control point, and proceed to S706.

[0145] S705: Determine a preset cost value as the cost value of the control point, and determine a preset gradient vector as the gradient vector of the control point.

[0146] S706: Determine the sum of the first cost value corresponding to the first value of the control point and the second cost value corresponding to the second value as the target cost value for angle optimization, and determine the sum of the first gradient vector corresponding to the first value of the control point and the second gradient vector corresponding to the second value as the target gradient vector for angle optimization.

[0147] S707: Optimize the angle and obstacle distance of the control point.

[0148] Example 6:

[0149] In order to improve the accuracy of determining the target cost value, based on the above embodiments, in an embodiment of the present invention, before determining the product of the square of the difference value and the preset weight value as the cost value of the control point, the method further includes:

[0150] Determine the target preset weight value corresponding to the control point based on the target sequence number corresponding to the control point determined in order according to the path, and a pre-stored functional relationship between the control point of each sequence number and the preset weight value, wherein the preset weight value of the starting point and the end point of each control point is greater than the preset weight value of the intermediate point;

[0151] The step of determining the product of the square of the difference value and the preset weight value as the cost value of the control point comprises:

[0152] The cost value of the control point is determined by multiplying the square of the difference value and the target preset weight value.

[0153] In order to improve the accuracy of determining the target cost value, in an embodiment of the present invention, each control point is sorted in order of the path to obtain the serial number of each control point, and a functional relationship between the control point of each serial number and the preset weight value is pre-saved, wherein the preset weight values ​​of the starting point and the end point of each control point are greater than the preset weight value of the middle point.

[0154] Figure 8 A schematic diagram of a functional relationship between a control point of each sequence number and a preset weight value provided by an embodiment of the present invention, such as Figure 8 As shown, i represents the serial number of the control point, k represents the preset weight value, and k near the starting point and end point is greater than k at the middle point. Therefore, the starting point and end point in the control point need more angle optimization and / or obstacle distance optimization, and the starting point and end point in the control point have greater constraints, thereby avoiding the problem of the path being close to the obstacle at the starting point and end point, and / or the angle deflection being too large to cause position deviation.

[0155] According to the corresponding target serial number determined by the control point in the order of the path and the pre-saved functional relationship, the target weight value corresponding to the target serial number is determined, and the cost value of the control point is determined by multiplying the square value of the difference and the target preset weight value.

[0156] Example 7:

[0157] In order to make the distance between each path point after the trajectory is modified uniform, based on the above embodiments, in an embodiment of the present invention, before performing B-spline distance sampling based on the updated position of each control point to obtain the position of each path point after the trajectory of the to-be-corrected path is modified, the method further includes:

[0158] For each control point except the first control point, determine the distance between the control point and the previous control point based on the updated position of the control point and the control point before the control point, and determine the quotient of the distance and the preset interval distance as the number of intervals between the control point and the previous control point; search the coefficients corresponding to the control point and the previous control point in a pre-stored coefficient table, and evenly divide the coefficients corresponding to the previous control point and the control point according to the number of intervals to obtain each sub-coefficient of the evenly divided coefficients between the coefficients corresponding to the previous control point and the control point, where the number of each sub-coefficient is the difference between the number of intervals minus 1;

[0159] Adding each corresponding sub-coefficient between the coefficients corresponding to each two adjacent control points in the coefficient table in numerical order to obtain a target coefficient table after adding each sub-coefficient;

[0160] The performing B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified, comprising:

[0161] For each coefficient in the target coefficient table, based on the coefficient and the updated position of each control point, determine the sum of the product values ​​of the coefficient and the updated position of each control point, and determine the sum as the position of the path point corresponding to the coefficient.

[0162] When B-spline distance sampling is used in the prior art to determine the position of each path point, the distance between each path point is not uniform; Figure 9 A schematic diagram of B-spline distance sampling provided by an embodiment of the present invention is shown in FIG. Figure 9 As shown, Figure 9 Each dotted circle in the table represents the position of a path point, and each solid circle represents the position of a control point, which also represents the position of a path point. The coefficients corresponding to each path point in the coefficient table are evenly distributed, but the distances between each path point are not necessarily evenly distributed. The distances between path points near the starting point and the end point are closer, while the distances between path points in the middle are farther. Figure 9 The distances between two adjacent path points, Distance1 and Distance2, are very different.

[0163] In order to achieve uniform sampling of B-spline distances and make the distance between each path point after the trajectory is modified uniform, the electronic device determines the distance between each control point except the first control point based on the position of the control point and the previous control point of the control point. The specific method for determining the distance between two control points is existing technology and will not be described in detail in the embodiment of the present invention; the electronic device pre-stores a preset interval distance, determines the quotient of the distance between the control point and the previous control point and the preset interval distance, and determines the quotient as the number of intervals between the control point and the previous control point.

[0164] The electronic device pre-stores a coefficient table, which includes coefficients corresponding to each control point. The coefficient table is searched to determine the coefficients corresponding to the control point and the previous control point, and the coefficients are evenly divided between the two coefficients according to the coefficients corresponding to the control point and the previous control point and the number of intervals, to obtain each sub-coefficient that is evenly divided between the coefficients corresponding to the previous control point and the control point.

[0165] Specifically, determine the difference between the coefficient corresponding to the control point and the coefficient corresponding to the previous control point, determine the ratio of the difference to the number of intervals, add the ratio to the coefficient corresponding to the previous control point to obtain a sub-coefficient, add the ratio to the sub-coefficient to obtain another sub-coefficient, add the ratio to each sub-coefficient until the sum of the last sub-coefficient and the ratio is equal to the coefficient corresponding to the control point, thereby obtaining each sub-coefficient evenly divided between the coefficient corresponding to the previous control point and the control point, where the number of each sub-coefficient is the difference after the number of intervals is reduced by 1.

[0166] After determining each sub-coefficient between the coefficients corresponding to each two adjacent control points, each corresponding sub-coefficient is added between the coefficients corresponding to each two adjacent control points in the coefficient table, specifically by adding according to the numerical value, to obtain the target coefficient table after adding each sub-coefficient.

[0167] For each coefficient in the target coefficient table, based on the coefficient and the updated position of each control point, determine the product value of the coefficient and the updated position of each control point, add each product value to obtain the sum value, and determine the sum value as the position of the path point corresponding to the coefficient.

[0168] Figure 10 A schematic diagram of a B-spline distance uniform sampling provided by an embodiment of the present invention is shown as follows: Figure 10 As shown, Figure 10 Each dotted circle in the table represents the position of a path point, and each solid circle represents the position of both a control point and a path point. The coefficients corresponding to each path point in the coefficient table are evenly distributed, and the distances between each path point are also evenly distributed. The distances Distance1 and Distance2 between two adjacent path points are the same.

[0169] As a possible implementation method, in an embodiment of the present invention, after determining the updated position of each control point, a simulation can be performed to verify whether each control point is safe based on the updated position of each control point. If so, the subsequent step of uniform B-spline distance sampling is performed. If not, the optimization is determined to have failed, and navigation is performed according to the path to be corrected.

[0170] Example 8:

[0171] The path trajectory correction method of the present invention is described below through a specific embodiment. Figure 11 A schematic diagram of a path trajectory correction method provided by an embodiment of the present invention is shown in FIG. Figure 11 As shown, the process includes the following steps:

[0172] S1101: Obtain an input coordinate set of initial path points of a path to be corrected, and perform B-spline fitting to obtain the position of each control point of the path to be corrected.

[0173] S1102: For each control point, determine a first orientation angle value of the direction of the line connecting the control point and the previous control point, and a second orientation angle value of the direction of the line connecting the previous control point and the next previous control point based on the control point and the two adjacent control points before the control point, and determine the angle difference between the second orientation angle value and the first orientation angle value, and / or determine the distance to the nearest obstacle corresponding to the control point based on the position of the control point and a pre-saved Euclidean signed distance field (ESDF) map, and perform angle optimization and / or obstacle distance optimization based on the angle difference and / or distance corresponding to the control point to determine the optimized position of the control point.

[0174] S1103: Determine whether the optimization termination condition is met based on the number of optimizations. If so, proceed to S1104; if not, proceed to S1107.

[0175] S1104: Based on the updated position of each control point, simulate and check whether the control point is safe. If so, proceed to S1105; if not, proceed to S1106.

[0176] S1105: Based on the updated position of each control point, B-spline distance sampling is performed to obtain the position of each path point after the path trajectory to be corrected is modified.

[0177] S1106: Determine that the optimization has failed, and navigate according to the path to be corrected.

[0178] S1107: Obtain the optimized position of the control point and perform S1102 again.

[0179] Example 9:

[0180] Figure 12 A schematic diagram of the structure of a path trajectory correction device provided by an embodiment of the present invention is shown in FIG. Figure 12 As shown, the device includes:

[0181] An acquisition module 1201 is configured to acquire a coordinate set of initial path points of a path to be corrected, and perform B-spline fitting to obtain the position of each control point of the path to be corrected;

[0182] An optimization module 1202 is configured to determine, for each control point, a first orientation angle value of a line connecting the control point and a previous control point, and a second orientation angle value of a line connecting the previous control point and the next previous control point based on the control point and the two adjacent control points before the control point, determine an angle difference between the second orientation angle value and the first orientation angle value, and / or determine a distance to the nearest obstacle corresponding to the control point based on the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map, and perform angle optimization and / or obstacle distance optimization based on the angle difference and / or distance corresponding to the control point to determine an updated position of the control point;

[0183] The sampling module 1203 is configured to perform B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified.

[0184] Furthermore, the optimization module 1202 is specifically used to determine the first target voxel corresponding to the position of the control point in the ESDF map according to the position of the control point and a pre-saved Euclidean signed distance field ESDF map, and determine the saved distance corresponding to the first target voxel as the distance to the nearest obstacle corresponding to the control point; or, according to the position of the control point and the Euclidean signed distance field ESDF map, determine the first target voxel corresponding to the position of the control point in the ESDF map, and according to a pre-saved automatic guided vehicle AGV model and a set number of each preset sphere model, place the position of the motion center of the AGV model at the first target voxel of the ESDF map, and obtain the distance of each preset sphere model on the AGV model. For each second target voxel corresponding to each sphere center position of the body model, the minimum distance is determined as the distance to the nearest obstacle corresponding to the control point according to the corresponding saved distance of each second target voxel; or, according to the position of the control point and the Euclidean signed distance field ESDF map, the first target voxel corresponding to the position of the control point is determined in the ESDF map, and according to the pre-saved automatic guided vehicle AGV model and each preset surface point of the AGV model, the position of the motion center of the AGV model is placed at the first target voxel to obtain each third target voxel point corresponding to each preset surface point of the AGV model, and according to the corresponding saved distance of each third target voxel, the minimum distance is determined as the distance to the nearest obstacle corresponding to the control point.

[0185] Furthermore, the optimization module 1202 is specifically configured to determine, if the angle difference is greater than a preset angle threshold, the difference between the angle difference and the preset angle threshold, determine the product of the square of the difference and a preset weight value as the target cost value of the control point, and determine the product vector of the target cost value and a preset standard gradient vector as the target gradient vector of the control point; if the angle difference is not greater than the preset angle threshold, determine the preset cost value as the target cost value of the control point, and determine the preset gradient vector as the target gradient vector of the control point; input the target cost value and the target gradient vector into a pre-saved optimizer to obtain the output position of the control point after angle optimization, and repeat the angle optimization according to the position of the control point after angle optimization until the number of optimizations meets the optimization termination condition, and then determine the updated position of the control point.

[0186] Furthermore, the optimization module 1202 is specifically configured to determine, if the distance is greater than a preset distance threshold, the difference between the distance and the preset distance threshold, determine the product of the square of the difference and a preset weight value as the target cost value of the control point, and determine the product vector of the target cost value and a preset standard gradient vector as the target gradient vector of the control point; if the distance is not greater than the preset distance threshold, determine the preset cost value as the target cost value of the control point, and determine the preset gradient vector as the target gradient vector of the control point; input the target cost value and the target gradient vector into a pre-saved optimizer to obtain the output position of the control point after obstacle distance optimization; and repeat the obstacle distance optimization according to the position of the control point after obstacle distance optimization until the number of optimizations meets the optimization termination condition, and then determine the updated position of the control point.

[0187] Furthermore, the optimization module 1202 is specifically configured to determine, based on the angle difference and distance corresponding to the control point, the angle difference as the first value for the angle optimization of the control point, and the distance as the second value for the obstacle distance optimization of the control point; for each value, if the value is greater than the corresponding preset value threshold, determine the difference between the value and the corresponding preset value threshold, determine the product of the square of the difference and the preset weight value as the cost value of the control point, determine the product vector of the cost value and the preset standard gradient vector as the gradient vector of the control point, and if the value is not greater than the corresponding preset value threshold, determine the preset cost value as the cost value of the control point, and the preset gradient vector Determine the gradient vector of the control point; determine the sum of the first cost value corresponding to the first value of the control point and the second cost value corresponding to the second value as the target cost value for angle optimization and obstacle distance optimization, and determine the sum of the first gradient vector corresponding to the first value of the control point and the second gradient vector corresponding to the second value as the target gradient vector for angle optimization and obstacle distance optimization; input the target cost value and the target gradient vector into a pre-saved optimizer to obtain the output position of the control point after angle optimization and obstacle distance optimization, and repeat the angle optimization and obstacle distance optimization according to the position of the control point after angle optimization and obstacle distance optimization until the number of optimizations meets the optimization termination condition, and then determine the updated position of the control point.

[0188] Furthermore, the optimization module 1202 is also used to determine the target preset weight value corresponding to the control point according to the corresponding target serial number determined by the control point in the order of the path, and the pre-saved functional relationship between the control point of each serial number and the preset weight value before determining the product value of the square of the difference value and the preset weight value as the cost value of the control point, wherein the preset weight value of the starting point and the end point of each control point is greater than the preset weight value of the middle point; specifically, it is used to determine the cost value of the control point by multiplying the square of the difference value and the target preset weight value.

[0189] Furthermore, the sampling module 1203 is also used for performing B-spline distance sampling to obtain the position of each path point after the modification of the path trajectory to be corrected according to the updated position of each control point, and for each control point except the first control point, determining the distance between the control point and the previous control point according to the updated position of the control point and the updated position of the previous control point of the control point, and determining the quotient of the distance and the preset interval distance as the number of intervals between the control point and the previous control point; searching for the coefficients corresponding to the control point and the previous control point in a pre-stored coefficient table, and performing the B-spline distance sampling to obtain the position of each path point after the modification of the path trajectory to be corrected according to the updated position of the control point and the previous control point of the control point, and determining the quotient of the distance and the preset interval distance as the number of intervals between the control point and the previous control point; The corresponding coefficients are evenly divided to obtain each sub-coefficient equally divided between the coefficients corresponding to the previous control point and the control point, wherein the number of each sub-coefficient is the difference after the number of intervals is reduced by 1; each corresponding sub-coefficient is added between the coefficients corresponding to every two adjacent control points in the coefficient table in order of numerical size to obtain a target coefficient table after adding each sub-coefficient; specifically, for each coefficient in the target coefficient table, according to the coefficient and the updated position of each control point, the sum of the product values ​​of the coefficient and the updated position of each control point is determined, and the sum is determined as the position of the path point corresponding to the coefficient.

[0190] Example 10:

[0191] Figure 13 This is a schematic diagram of the structure of an electronic device provided by this application. Based on the above embodiments, the embodiment of the present invention further provides an electronic device, such as Figure 13 As shown, it includes: a processor 1301 , a communication interface 1302 , a memory 1303 and a communication bus 1304 , wherein the processor 1301 , the communication interface 1302 , and the memory 1303 communicate with each other via the communication bus 1304 .

[0192] The memory 1303 stores a computer program. When the program is executed by the processor 1301, the processor 1301 performs the following steps:

[0193] Obtaining the input coordinate set of the initial path points of the path to be corrected, and performing B-spline fitting to obtain the position of each control point of the path to be corrected;

[0194] For each control point, determining, based on the control point and the two adjacent control points before the control point, a first orientation angle value of the direction of the line connecting the control point and the previous control point, and a second orientation angle value of the direction of the line connecting the previous control point and the previous control point, and determining an angle difference between the second orientation angle value and the first orientation angle value, and / or determining, based on the position of the control point and a pre-saved Euclidean Signed Distance Field (ESDF) map, the distance to the nearest obstacle corresponding to the control point, and performing angle optimization and / or obstacle distance optimization based on the angle difference and / or distance corresponding to the control point to determine the updated position of the control point;

[0195] According to the updated position of each control point, B-spline distance sampling is performed to obtain the position of each path point after the path trajectory to be corrected is modified.

[0196] Furthermore, the processor 1301 is specifically configured to determine the distance to the nearest obstacle corresponding to the control point according to the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map, including:

[0197] According to the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map, determining the first target voxel corresponding to the position of the control point in the ESDF map, and determining the stored distance corresponding to the first target voxel as the distance to the nearest obstacle corresponding to the control point; or

[0198] According to the position of the control point and the Euclidean Signed Distance Field (ESDF) map, a first target voxel corresponding to the position of the control point is determined in the ESDF map; based on a pre-stored automatic guided vehicle (AGV) model and a set number of preset sphere models, the position of the motion center of the AGV model is placed at the first target voxel of the ESDF map; each second target voxel corresponding to the center position of each sphere of each preset sphere model on the AGV model is obtained; and based on the stored distance corresponding to each second target voxel, a minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point; or,

[0199] According to the position of the control point and the Euclidean signed distance field (ESDF) map, the first target voxel corresponding to the position of the control point is determined in the ESDF map. According to the pre-saved automatic guided vehicle (AGV) model and each preset surface point of the AGV model, the position of the motion center of the AGV model is placed at the first target voxel, and each third target voxel point corresponding to each preset surface point of the AGV model is obtained. According to the saved distance corresponding to each third target voxel, the minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point.

[0200] Furthermore, the processor 1301 is specifically configured to optimize the angle according to the angle difference corresponding to the control point to determine the updated position of the control point, including:

[0201] If the angle difference is greater than a preset angle threshold, the difference between the angle difference and the preset angle threshold is determined, the product of the square of the difference and a preset weight value is determined as the target cost value of the control point, and the product vector of the target cost value and a preset standard gradient vector is determined as the target gradient vector of the control point; if the angle difference is not greater than the preset angle threshold, the preset cost value is determined as the target cost value of the control point, and the preset gradient vector is determined as the target gradient vector of the control point;

[0202] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization. According to the position of the control point after angle optimization, the angle optimization is repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0203] Furthermore, the processor 1301 is specifically configured to optimize the obstacle distance according to the distance corresponding to the control point to determine the updated position of the control point, including:

[0204] If the distance is greater than a preset distance threshold, determining a difference between the distance and the preset distance threshold, determining the product of the square of the difference and a preset weight value as the target cost value of the control point, and determining the product vector of the target cost value and a preset standard gradient vector as the target gradient vector of the control point; if the distance is not greater than the preset distance threshold, determining the preset cost value as the target cost value of the control point, and determining the preset gradient vector as the target gradient vector of the control point;

[0205] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after obstacle distance optimization. According to the position of the control point after obstacle distance optimization, the obstacle distance optimization is repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0206] Furthermore, the processor 1301 is specifically configured to perform angle optimization and obstacle distance optimization according to the angle difference and distance corresponding to the control point to determine the updated position of the control point, including:

[0207] According to the angle difference and distance corresponding to the control point, the angle difference is determined as the first value for angle optimization of the control point, and the distance is determined as the second value for obstacle distance optimization of the control point;

[0208] For each value, if the value is greater than the corresponding preset value threshold, the difference between the value and the corresponding preset value threshold is determined, the product of the square of the difference and the preset weight value is determined as the cost value of the control point, and the product vector of the cost value and the preset standard gradient vector is determined as the gradient vector of the control point; if the value is not greater than the corresponding preset value threshold, the preset cost value is determined as the cost value of the control point, and the preset gradient vector is determined as the gradient vector of the control point;

[0209] The sum of the first cost value corresponding to the first value of the control point and the second cost value corresponding to the second value is determined as the target cost value for angle optimization and obstacle distance optimization; the sum of the first gradient vector corresponding to the first value of the control point and the second gradient vector corresponding to the second value is determined as the target gradient vector for angle optimization and obstacle distance optimization;

[0210] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization and obstacle distance optimization. According to the position of the control point after angle optimization and obstacle distance optimization, the angle optimization and obstacle distance optimization are repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0211] Furthermore, the processor 1301 is further configured to, before determining the product of the square of the difference value and the preset weight value as the cost value of the control point, further include:

[0212] Determine the target preset weight value corresponding to the control point based on the target sequence number corresponding to the control point determined in order according to the path, and a pre-stored functional relationship between the control point of each sequence number and the preset weight value, wherein the preset weight value of the starting point and the end point of each control point is greater than the preset weight value of the intermediate point;

[0213] The step of determining the product of the square of the difference value and the preset weight value as the cost value of the control point comprises:

[0214] The cost value of the control point is determined by multiplying the square of the difference value and the target preset weight value.

[0215] Furthermore, the processor 1301 is specifically configured to perform B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified. The method further includes:

[0216] For each control point except the first control point, determine the distance between the control point and the previous control point based on the updated position of the control point and the control point before the control point, and determine the quotient of the distance and the preset interval distance as the number of intervals between the control point and the previous control point; search the coefficients corresponding to the control point and the previous control point in a pre-stored coefficient table, and evenly divide the coefficients corresponding to the previous control point and the control point according to the number of intervals to obtain each sub-coefficient of the evenly divided coefficients between the coefficients corresponding to the previous control point and the control point, where the number of each sub-coefficient is the difference between the number of intervals minus 1;

[0217] Adding each corresponding sub-coefficient between the coefficients corresponding to each two adjacent control points in the coefficient table in numerical order to obtain a target coefficient table after adding each sub-coefficient;

[0218] The performing B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified, comprising:

[0219] For each coefficient in the target coefficient table, based on the coefficient and the updated position of each control point, determine the sum of the product values ​​of the coefficient and the updated position of each control point, and determine the sum as the position of the path point corresponding to the coefficient.

[0220] The communication bus mentioned in the electronic device mentioned above may be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus. This communication bus can be divided into an address bus, a data bus, a control bus, etc. For ease of illustration, only one thick line is used in the figure, but this does not mean that there is only one bus or only one type of bus.

[0221] The communication interface 1302 is used for communication between the electronic device and other devices.

[0222] The memory may include random access memory (RAM) or non-volatile memory (NVM), such as at least one disk memory. Alternatively, the memory may be at least one storage device located away from the processor.

[0223] The above-mentioned processor can be a general-purpose processor, including a central processing unit, a network processor (NP), etc.; it can also be a digital signal processing processor (DSP), an application-specific integrated circuit, a field programmable gate array or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component, etc.

[0224] Example 11:

[0225] Based on the above embodiments, the present application further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program executable by a processor. When the program is executed on the processor, the processor implements the following steps:

[0226] Obtaining the input coordinate set of the initial path points of the path to be corrected, and performing B-spline fitting to obtain the position of each control point of the path to be corrected;

[0227] For each control point, determining, based on the control point and the two adjacent control points before the control point, a first orientation angle value of the direction of the line connecting the control point and the previous control point, and a second orientation angle value of the direction of the line connecting the previous control point and the previous control point, and determining an angle difference between the second orientation angle value and the first orientation angle value, and / or determining, based on the position of the control point and a pre-saved Euclidean Signed Distance Field (ESDF) map, the distance to the nearest obstacle corresponding to the control point, and performing angle optimization and / or obstacle distance optimization based on the angle difference and / or distance corresponding to the control point to determine the updated position of the control point;

[0228] According to the updated position of each control point, B-spline distance sampling is performed to obtain the position of each path point after the path trajectory to be corrected is modified.

[0229] Furthermore, determining the distance to the nearest obstacle corresponding to the control point according to the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map includes:

[0230] According to the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map, determining the first target voxel corresponding to the position of the control point in the ESDF map, and determining the stored distance corresponding to the first target voxel as the distance to the nearest obstacle corresponding to the control point; or

[0231] According to the position of the control point and the Euclidean Signed Distance Field (ESDF) map, a first target voxel corresponding to the position of the control point is determined in the ESDF map; based on a pre-stored automatic guided vehicle (AGV) model and a set number of preset sphere models, the position of the motion center of the AGV model is placed at the first target voxel of the ESDF map; each second target voxel corresponding to the center position of each sphere of each preset sphere model on the AGV model is obtained; and based on the stored distance corresponding to each second target voxel, a minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point; or,

[0232] According to the position of the control point and the Euclidean signed distance field (ESDF) map, the first target voxel corresponding to the position of the control point is determined in the ESDF map. According to the pre-saved automatic guided vehicle (AGV) model and each preset surface point of the AGV model, the position of the motion center of the AGV model is placed at the first target voxel, and each third target voxel point corresponding to each preset surface point of the AGV model is obtained. According to the saved distance corresponding to each third target voxel, the minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point.

[0233] Furthermore, performing angle optimization to determine the updated position of the control point according to the angle difference corresponding to the control point includes:

[0234] If the angle difference is greater than a preset angle threshold, the difference between the angle difference and the preset angle threshold is determined, the product of the square of the difference and a preset weight value is determined as the target cost value of the control point, and the product vector of the target cost value and a preset standard gradient vector is determined as the target gradient vector of the control point; if the angle difference is not greater than the preset angle threshold, the preset cost value is determined as the target cost value of the control point, and the preset gradient vector is determined as the target gradient vector of the control point;

[0235] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization. According to the position of the control point after angle optimization, the angle optimization is repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0236] Furthermore, performing obstacle distance optimization to determine the updated position of the control point according to the distance corresponding to the control point includes:

[0237] If the distance is greater than a preset distance threshold, determining a difference between the distance and the preset distance threshold, determining the product of the square of the difference and a preset weight value as the target cost value of the control point, and determining the product vector of the target cost value and a preset standard gradient vector as the target gradient vector of the control point; if the distance is not greater than the preset distance threshold, determining the preset cost value as the target cost value of the control point, and determining the preset gradient vector as the target gradient vector of the control point;

[0238] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after obstacle distance optimization. According to the position of the control point after obstacle distance optimization, the obstacle distance optimization is repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0239] Furthermore, performing angle optimization and obstacle distance optimization to determine the updated position of the control point based on the angle difference and distance corresponding to the control point includes:

[0240] According to the angle difference and distance corresponding to the control point, the angle difference is determined as the first value for angle optimization of the control point, and the distance is determined as the second value for obstacle distance optimization of the control point;

[0241] For each value, if the value is greater than the corresponding preset value threshold, the difference between the value and the corresponding preset value threshold is determined, the product of the square of the difference and the preset weight value is determined as the cost value of the control point, and the product vector of the cost value and the preset standard gradient vector is determined as the gradient vector of the control point; if the value is not greater than the corresponding preset value threshold, the preset cost value is determined as the cost value of the control point, and the preset gradient vector is determined as the gradient vector of the control point;

[0242] The sum of the first cost value corresponding to the first value of the control point and the second cost value corresponding to the second value is determined as the target cost value for angle optimization and obstacle distance optimization; the sum of the first gradient vector corresponding to the first value of the control point and the second gradient vector corresponding to the second value is determined as the target gradient vector for angle optimization and obstacle distance optimization;

[0243] The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization and obstacle distance optimization. According to the position of the control point after angle optimization and obstacle distance optimization, the angle optimization and obstacle distance optimization are repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

[0244] Furthermore, before determining the product of the square of the difference and the preset weight value as the cost value of the control point, the method further includes:

[0245] Determine the target preset weight value corresponding to the control point based on the target sequence number corresponding to the control point determined in order according to the path, and a pre-stored functional relationship between the control point of each sequence number and the preset weight value, wherein the preset weight value of the starting point and the end point of each control point is greater than the preset weight value of the intermediate point;

[0246] The step of determining the product of the square of the difference value and the preset weight value as the cost value of the control point comprises:

[0247] The cost value of the control point is determined by multiplying the square of the difference value and the target preset weight value.

[0248] Furthermore, before performing B-spline distance sampling to obtain the position of each path point after modification of the path trajectory to be corrected based on the updated position of each control point, the method further includes:

[0249] For each control point except the first control point, determine the distance between the control point and the previous control point based on the updated position of the control point and the control point before the control point, and determine the quotient of the distance and the preset interval distance as the number of intervals between the control point and the previous control point; search the coefficients corresponding to the control point and the previous control point in a pre-stored coefficient table, and evenly divide the coefficients corresponding to the previous control point and the control point according to the number of intervals to obtain each sub-coefficient of the evenly divided coefficients between the coefficients corresponding to the previous control point and the control point, where the number of each sub-coefficient is the difference between the number of intervals minus 1;

[0250] Adding each corresponding sub-coefficient between the coefficients corresponding to each two adjacent control points in the coefficient table in numerical order to obtain a target coefficient table after adding each sub-coefficient;

[0251] The performing B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified, comprising:

[0252] For each coefficient in the target coefficient table, based on the coefficient and the updated position of each control point, determine the sum of the product values ​​of the coefficient and the updated position of each control point, and determine the sum as the position of the path point corresponding to the coefficient.

[0253] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

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

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

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

[0257] Obviously, those skilled in the art may make various changes and modifications to this application without departing from the spirit and scope of this application. Thus, if these modifications and variations of this application fall within the scope of the claims of this application and their equivalents, this application is intended to include these modifications and variations.

Claims

1. A path trajectory correction method, characterized in that: The method comprises: Obtaining the input coordinate set of the initial path points of the path to be corrected, and performing B-spline fitting to obtain the position of each control point of the path to be corrected; For each control point, based on the control point and the two adjacent control points before the control point, determine a first orientation angle value of the direction of the line connecting the control point and the previous control point, and a second orientation angle value of the direction of the line connecting the previous control point and the next previous control point, and determine the angle difference between the second orientation angle value and the first orientation angle value, and / or determine the distance to the nearest obstacle corresponding to the control point based on the position of the control point and a pre-saved Euclidean signed distance field (ESDF) map, and perform angle optimization and / or obstacle distance optimization to determine the updated position of the control point based on the angle difference and / or distance corresponding to the control point; wherein the pre-saved Euclidean signed distance field (ESDF) map divides the three-dimensional space into multiple voxels, and for each voxel, a pre-calculated distance to its nearest obstacle is stored; Performing B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified; Before performing B-spline distance sampling to obtain the position of each path point after modification of the path trajectory to be corrected based on the updated position of each control point, the method further includes: For each control point except the first control point, search the coefficient corresponding to the control point and the previous control point in a pre-stored coefficient table, and divide the coefficients corresponding to the previous control point and the control point equally according to the number of intervals to obtain each sub-coefficient of the coefficients corresponding to the previous control point and the control point, where the number of each sub-coefficient is the difference between the number of intervals minus 1; Adding each corresponding sub-coefficient between the coefficients corresponding to each two adjacent control points in the coefficient table in numerical order to obtain a target coefficient table after adding each sub-coefficient; The performing B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified, comprising: For each coefficient in the target coefficient table, based on the coefficient and the updated position of each control point, determine the sum of the product values ​​of the coefficient and the updated position of each control point, and determine the sum as the position of the path point corresponding to the coefficient.

2. The method according to claim 1, characterized in that Determining the distance to the nearest obstacle corresponding to the control point based on the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map includes: According to the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map, determining the first target voxel corresponding to the position of the control point in the ESDF map, and determining the stored distance corresponding to the first target voxel as the distance to the nearest obstacle corresponding to the control point; or According to the position of the control point and the Euclidean Signed Distance Field (ESDF) map, a first target voxel corresponding to the position of the control point is determined in the ESDF map; based on a pre-stored automatic guided vehicle (AGV) model and a set number of preset sphere models, the position of the motion center of the AGV model is placed at the first target voxel of the ESDF map; each second target voxel corresponding to the center position of each sphere of each preset sphere model on the AGV model is obtained; and based on the stored distance corresponding to each second target voxel, a minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point; or, According to the position of the control point and the Euclidean signed distance field (ESDF) map, the first target voxel corresponding to the position of the control point is determined in the ESDF map. According to the pre-saved automatic guided vehicle (AGV) model and each preset surface point of the AGV model, the position of the motion center of the AGV model is placed at the first target voxel, and each third target voxel corresponding to each preset surface point of the AGV model is obtained. According to the saved distance corresponding to each third target voxel, the minimum distance is determined to be the distance to the nearest obstacle corresponding to the control point.

3. The method according to claim 1, characterized in that The performing angle optimization to determine the updated position of the control point according to the angle difference corresponding to the control point includes: If the angle difference is greater than a preset angle threshold, the difference between the angle difference and the preset angle threshold is determined, the product of the square of the difference and a preset weight value is determined as the target cost value of the control point, and the product vector of the target cost value and a preset standard gradient vector is determined as the target gradient vector of the control point; if the angle difference is not greater than the preset angle threshold, the preset cost value is determined as the target cost value of the control point, and the preset gradient vector is determined as the target gradient vector of the control point; The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization. According to the position of the control point after angle optimization, the angle optimization is repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

4. The method according to claim 1, wherein The step of optimizing the obstacle distance according to the distance corresponding to the control point to determine the updated position of the control point includes: If the distance is greater than a preset distance threshold, determining a difference between the distance and the preset distance threshold, determining the product of the square of the difference and a preset weight value as the target cost value of the control point, and determining the product vector of the target cost value and a preset standard gradient vector as the target gradient vector of the control point; if the distance is not greater than the preset distance threshold, determining the preset cost value as the target cost value of the control point, and determining the preset gradient vector as the target gradient vector of the control point; The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after obstacle distance optimization. According to the position of the control point after obstacle distance optimization, the obstacle distance optimization is repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

5. The method according to claim 1, wherein The determining of the updated position of the control point by performing angle optimization and obstacle distance optimization according to the angle difference and distance corresponding to the control point includes: According to the angle difference and distance corresponding to the control point, the angle difference is determined as the first value for angle optimization of the control point, and the distance is determined as the second value for obstacle distance optimization of the control point; For each value, if the value is greater than the corresponding preset value threshold, the difference between the value and the corresponding preset value threshold is determined, the product of the square of the difference and the preset weight value is determined as the cost value of the control point, and the product vector of the cost value and the preset standard gradient vector is determined as the gradient vector of the control point; if the value is not greater than the corresponding preset value threshold, the preset cost value is determined as the cost value of the control point, and the preset gradient vector is determined as the gradient vector of the control point; The sum of the first cost value corresponding to the first value of the control point and the second cost value corresponding to the second value is determined as the target cost value for angle optimization and obstacle distance optimization; the sum of the first gradient vector corresponding to the first value of the control point and the second gradient vector corresponding to the second value is determined as the target gradient vector for angle optimization and obstacle distance optimization; The target cost value and the target gradient vector are input into a pre-saved optimizer to obtain the output position of the control point after angle optimization and obstacle distance optimization. According to the position of the control point after angle optimization and obstacle distance optimization, the angle optimization and obstacle distance optimization are repeated until the number of optimizations meets the optimization termination condition, and then the updated position of the control point is determined.

6. The method according to any one of claims 3 to 5, characterized in that: Before determining the product of the square of the difference value and the preset weight value as the cost value of the control point, the method further includes: Determine the target preset weight value corresponding to the control point based on the target sequence number corresponding to the control point determined in order according to the path, and a pre-stored functional relationship between the control point of each sequence number and the preset weight value, wherein the preset weight value of the starting point and the end point of each control point is greater than the preset weight value of the intermediate point; The step of determining the product of the square of the difference value and the preset weight value as the cost value of the control point comprises: The cost value of the control point is determined by multiplying the square of the difference value and the target preset weight value.

7. The method according to claim 1, characterized in that The process of determining the number of intervals includes: For each control point except the first control point, the distance between the control point and the previous control point is determined based on the updated position of the control point and the previous control point, and the quotient of the distance and the preset interval distance is determined as the interval between the control point and the previous control point.

8. A path trajectory correction device, characterized in that: The device comprises: An acquisition module is used to obtain a coordinate set of the initial path points of the input path to be corrected, and perform B-spline fitting to obtain the position of each control point of the path to be corrected; An optimization module is configured to determine, for each control point, a first orientation angle value of the direction of the line connecting the control point and the previous control point, and a second orientation angle value of the direction of the line connecting the previous control point and the next previous control point based on the control point and the two adjacent control points before the control point, and determine an angle difference between the second orientation angle value and the first orientation angle value, and / or determine the distance to the nearest obstacle corresponding to the control point based on the position of the control point and a pre-stored Euclidean Signed Distance Field (ESDF) map, and perform angle optimization and / or obstacle distance optimization to determine the updated position of the control point based on the angle difference and / or distance corresponding to the control point; wherein the pre-stored Euclidean Signed Distance Field (ESDF) map divides the three-dimensional space into a plurality of voxels, and stores a pre-calculated distance to the nearest obstacle for each voxel; A sampling module, configured to perform B-spline distance sampling based on the updated position of each control point to obtain the position of each path point after the path trajectory to be corrected is modified; The sampling module is also used for performing B-spline distance sampling according to the updated position of each control point to obtain the position of each path point after the modification of the path trajectory to be corrected. For each control point except the first control point, the coefficient corresponding to the control point and the previous control point is searched in a pre-saved coefficient table, and the coefficients corresponding to the previous control point and the control point are evenly divided according to the number of intervals to obtain each sub-coefficient evenly divided between the coefficients corresponding to the previous control point and the control point, wherein the number of each sub-coefficient is the difference after the number of intervals is reduced by 1; each sub-coefficient is added between the coefficients corresponding to each two adjacent control points in the coefficient table in numerical order to obtain a target coefficient table after each sub-coefficient is added; specifically, for each coefficient in the target coefficient table, according to the coefficient and the updated position of each control point, the sum of the product values ​​of the coefficient and the updated position of each control point is determined, and the sum is determined as the position of the path point corresponding to the coefficient.

9. An electronic device, characterized in that: include: A processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory communicate with each other via the communication bus; A computer program is stored in the memory, and when the program is executed by the processor, the processor executes the steps of the path trajectory correction method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that It stores a computer program executable by a processor. When the program runs on the processor, the processor executes the steps of the path trajectory correction method according to any one of claims 1 to 7.

Images