Control point sequence optimization method and system for path planning

Abstract

The invention provides a control point sequence optimization method and system for path planning, and the method comprises the steps: obtaining an initial control point sequence of a uniform B-spline curve constructed based on a rough solution of a front-end planning path, and then carrying out the optimization of the initial control point sequence according to a configured optimization algorithm, optimization items and parameters of related iterative optimization, and generating an optimized control point sequence. According to the method and the system, before back-end optimization, based on related properties of a uniform B spline, fitting is performed on a front-end rough solution, and smoothing, obstacle avoidance and other processing can be conveniently performed on a path according to an obtained initial control point sequence, so that an original initial value with better quality is provided for back-end processing, and the back-end processing burden is reduced.

Description

Technical Field

[0001] The present invention relates to the field of vehicle automatic driving path planning, and more specifically, to a control point sequence optimization method and system for path planning. Background Art

[0002] For the path planning of autonomous driving of unmanned vehicles in unstructured environments, algorithms such as Dijkstra are often used for front-end path planning to obtain rough solutions. These rough solutions are usually broken lines and cannot meet the vehicle's requirements for smoothness during driving. In addition, even if algorithms that consider kinematic constraints such as hybrid A* are used, although they can meet a certain degree of smoothness, they still need to be processed at the back end. The rough solutions obtained by different front-end algorithms are in different forms. When optimizing the rough solutions at the back end, it is not convenient to use a unified model to represent the path obtained by the front-end planning, which makes it inconvenient to do further processing. Summary of the invention

[0003] In order to solve the technical problem in the prior art that the refined solutions obtained in the front-end path planning are in different forms, resulting in inconvenience in using a unified model to represent the front-end rough solution when optimizing the rough solution at the back-end, the present invention provides a control point sequence optimization method and system for path planning.

[0004] According to one aspect of the present invention, the present invention provides a control point sequence optimization method for path planning, comprising: Get the initial control point sequence of the uniform B-spline curve constructed based on the rough solution of the front-end planning path and the custom B-spline degree; Start the solution time timing, and calculate the optimization vector and optimization boundary of the first iteration according to the initial control point sequence, the B-spline number, the configured initial optimization deviation vector and the endpoint constraint optimization item configuration flag, wherein the endpoint constraint optimization item configuration flag is used to determine whether the configured optimization item includes the endpoint constraint item; Adopting the configured optimization algorithm for optimizing the front-end planning path, performing iterative operations based on the optimization vector and optimization boundary of the first iteration according to the configured optimization items, the weight value, cost and gradient calculation function of each optimization item, and determining the optimal optimization vector that meets the configured iteration termination condition; The solution time is reset to zero, and an optimized control point sequence is generated according to the optimal optimization vector and the initial control point sequence.

[0005] According to another aspect of the present invention, the present invention provides a control point sequence optimization system for path planning, the system comprising: A data acquisition module is used to obtain an initial control point sequence of a uniform B-spline curve constructed based on a rough solution of the front-end planning path and a custom B-spline degree; A first calculation module is used to start the solution time timing, and calculate the optimization vector and optimization boundary of the first iteration according to the initial control point sequence, the B-spline number, the configured initial optimization deviation vector and the endpoint constraint optimization item configuration flag, wherein the endpoint constraint optimization item configuration flag is used to determine whether the configured optimization item includes the endpoint constraint item; A second calculation module is used to adopt a configured optimization algorithm for optimizing the front-end planning path, perform iterative operations based on the optimization vector and optimization boundary of the first iteration according to the configured optimization items, the weight value, cost and gradient calculation function of each optimization item, and determine the optimal optimization vector that meets the configured iteration termination condition; The result output module is used to solve the time timing and return to zero, and generate an optimized control point sequence according to the optimal optimization vector and the initial control point sequence.

[0006] According to another aspect of the present invention, the present invention provides a computer-readable storage medium, wherein the storage medium stores a computer program, and the computer program is used to execute the method described in any one of the above aspects of the present invention.

[0007] According to another aspect of the present invention, an electronic device is provided, comprising: a processor; a memory for storing instructions executable by the processor; the processor is configured to read the executable instructions from the memory and execute the instructions to implement the method described in any one of the above aspects of the present invention.

[0008] The control point sequence optimization method and system for path planning described in the present invention include obtaining an initial control point sequence of a uniform B-spline curve constructed based on a rough solution of a front-end planned path, and then optimizing the initial control point sequence according to the configured optimization algorithm, optimization items, and parameters of related iterative optimization to generate an optimized control point sequence. Before back-end optimization, the method and system first fit the front-end rough solution based on the relevant properties of the uniform B-spline, and the path can be easily smoothed, obstacle avoided, etc. according to the obtained initial control point sequence, thereby providing a better quality original initial value for back-end processing and reducing the back-end processing burden. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] A more complete understanding of exemplary embodiments of the present invention may be obtained by referring to the following drawings: Figure 1 A flowchart of a control point sequence optimization method for path planning according to a preferred embodiment of the present invention; Figure 2 A schematic diagram of a control point sequence for a front-end planning path according to a preferred embodiment of the present invention; Figure 3A schematic diagram of the structure of a control point sequence optimization system for path planning according to a preferred embodiment of the present invention; Figure 4 Schematic diagram of the structure of an electronic device according to a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0010] Now, exemplary embodiments of the present invention are described with reference to the accompanying drawings. However, the present invention can be implemented in many different forms and is not limited to the embodiments described herein. These embodiments are provided to disclose the present invention in detail and completely and to fully convey the scope of the present invention to those skilled in the art. The terms used in the exemplary embodiments shown in the accompanying drawings are not intended to limit the present invention. In the accompanying drawings, the same units / elements are marked with the same reference numerals.

[0011] Unless otherwise specified, the terms (including technical terms) used herein have the commonly understood meanings to those skilled in the art. In addition, it is understood that the terms defined in commonly used dictionaries should be understood to have the same meanings as those in the context of the relevant fields, and should not be understood as idealized or overly formal meanings.

[0012] Exemplary Methods Figure 1 FIG. 4 is a flow chart of a control point sequence optimization method for path planning according to a preferred embodiment of the present invention. Figure 1 As shown, the control point sequence optimization method for path planning described in this preferred embodiment starts from step 101.

[0013] In step 101, an initial control point sequence of a uniform B-spline curve constructed based on a rough solution of a front-end planned path and a user-defined B-spline degree is obtained.

[0014] For autonomous driving path planning in unstructured environments, the rough solution obtained by using the algorithm for front-end path planning is a discrete path point sequence. In order to convert the path point sequence into a uniform B-spline curve that is easy to optimize, the rough solution of the front end can be fitted based on the properties of B-spline, according to the number of path points in the rough solution, the dimension of the path points is characterized, and the custom B-spline degree is used to fit the rough solution of the front end to obtain the initial control point sequence of the B-spline curve corresponding to the rough solution. The number of rows of the initial control point sequence represents the number of control points associated with the path points in the rough solution, and the number of columns represents the dimension used to characterize the control points. For example, for a discrete path point sequence, there are 13 path points, and each path point is represented by three dimensions x, y and theta in the space. Then, when the B-spline degree is 3, according to the properties of B-spline, the number of control points in the initial control point sequence is 15, and the dimension is also 3.

[0015] In step 102, the solution time is started, and the optimization vector and optimization boundary of the first iteration are calculated according to the initial control point sequence, the B-spline number, the configured initial optimization deviation vector and the endpoint constraint optimization item configuration flag, wherein the endpoint constraint optimization item configuration flag is used to determine whether the configured optimization item includes the endpoint constraint item.

[0016] In this preferred embodiment, when optimizing the initial control point sequence, it is necessary to first configure the optimization algorithm, optimization items, etc. according to the selected optimization tool. For example, the optimization tool may be the nonlinear optimization tool nlopt, the configured optimization algorithm is LD_LBFGS, and the optimization items may be multiple options selected in a form, such as smoothing constraints, obstacle avoidance constraints, guidance constraints, end point constraints, kinematic constraints, key point constraints, etc. Furthermore, an open interface may be configured for adding other constraints. In this preferred embodiment, the example of configuring smoothing constraints, obstacle avoidance constraints, guidance constraints, and end point constraints at the same time is used for explanation.

[0017] Preferably, the calculation of the optimization vector and optimization boundary of the first iteration according to the initial control point sequence, the B-spline order, the configured initial optimization deviation vector and the endpoint constraint optimization item configuration flag includes: Determining the number of optimization variables according to the number of rows and columns of the initial control point sequence, the B-spline order, and the endpoint constraint optimization item configuration flag; Generate an optimization vector for the first iteration according to the number of optimization variables and the initial control point sequence; The optimization boundary of the first iteration is determined according to the optimization vector of the first iteration and the initial optimization deviation vector.

[0018] Preferably, determining the number of optimization variables according to the number of rows and columns of the initial control point sequence, the B-spline degree, and the endpoint constraint optimization item configuration flag includes: When the endpoint constraint optimization item configuration flag indicates that the configured optimization item includes the endpoint constraint, the expression for the number of optimization variables var_num is: var_num = max(0, pt_dim * (pt_num - 1 * order)) When the endpoint constraint optimization item configuration flag indicates that the configured optimization item does not include the endpoint constraint, the expression for the number of optimization variables var_num is: var_num = max(0, pt_dim * (pt_num - 2 * order)) In the formula, pt_dim is the number of columns of the initial control point sequence, representing the dimension of the control points, pt_num is the number of rows of the initial control point sequence, representing the number of control points, and order is the B-spline order.

[0019] In this embodiment, the value of pt_num is 15, and the values ​​of order and pt_dim are 3. When the configured optimization items include the endpoint constraint, the number of optimization variables var_num is 36.

[0020] Preferably, generating the optimization vector of the first iteration according to the number of optimization variables and the initial control point sequence includes: Create a new optimization vector q1[m] with a length of var_num, where var_num is the number of optimization variables in the optimization vector q1[m], and 1≤m≤var_num; The control point information participating in the iterative optimization in the initial control point sequence is written into the corresponding optimization variables in the optimization vector q1[m] according to the outer traversal point j from order+1 to end_dix and the inner traversal point k from 1 to pt_dim. The expression is: q1[m]= q1[pt_dim * (j – order-1) + k]= ctrl_pts[j, k] Among them, ctrl_pts is the initial control point sequence. When the end point constraint optimization item configuration flag indicates that the configured optimization item includes the end point constraint, end_dix=pt_num; when the end point constraint optimization item configuration flag indicates that the configured optimization item does not include the end point constraint, end_dix=pt_num-order.

[0021] In this embodiment, the optimization vector for the first iteration is determined based on the value of the initial control point sequence, which is essentially to flatten the control point information to be optimized in a matrix of 15 rows and 3 columns into a row vector including 36 elements.

[0022] Preferably, determining the optimization boundary of the first iteration according to the optimization vector of the first iteration and the initial optimization deviation vector comprises: Generate the first iteration optimization deviation vector bound1 with a length of var_num according to the optimization variable number var_num; Create an upper optimization boundary vector lb1[m] and a lower optimization boundary vector ub1[m] of length var_num; According to the information of the optimization variables in the optimization vector q1[m] of the first iteration and the initial optimization deviation vector, the upper optimization boundary vector ib1[m] and the lower optimization boundary vector ub1[m] are assigned values, where: lb1[m] = q1[m]- bound1 ub1[m] = q1[m]+bound1 In the formula, 1≤m≤var_num.

[0023] In this embodiment, when the optimization vector obtained based on the initial control point sequence is iterated multiple times, a constraint is required on the value of each element in the optimization vector. Therefore, the initial optimization deviation vector is centered on the value of each element in the optimization vector and is offset up and down.

[0024] In step 103, the configured optimization algorithm for optimizing the front-end planning path is adopted, and according to the configured optimization items, the weight value, cost and gradient calculation function of each optimization item, an iterative operation is performed based on the optimization vector and optimization boundary of the first iteration to determine the optimal optimization vector that meets the configured iteration termination condition.

[0025] Preferably, the optimization algorithm for optimizing the front-end planning path using the configuration performs iterative operations based on the optimization vector and optimization boundary of the first iteration according to the configured optimization items, the weight value, cost and gradient calculation function of each optimization item, and determines the optimal optimization vector that meets the configured iteration termination condition, including: Calculate the cost and gradient vector of the nth iteration according to the optimization vector of the nth iteration, the configured optimization items, the weight value of each optimization item, the cost and the gradient calculation function, where n is a positive integer, 1≤n≤N; Based on the optimization algorithm, determining an optimization vector for the n+1th iteration according to the gradient vector for the nth iteration and the optimization boundary for the nth iteration; Determine an optimization boundary of the n+1th iteration according to the optimization vector of the nth iteration and the optimization vector of the n+1th iteration; When the cost of the nth iteration is less than the minimum cost after the n-1th iteration, the minimum cost after the nth iteration is made the cost of the nth iteration, and the optimal optimization vector after the nth iteration is made the optimization vector of the nth iteration, otherwise, the minimum cost and the optimal optimization vector after the nth iteration remain unchanged, wherein, when n=1, the minimum cost after the n-1th iteration is the initial value of the configured minimum cost; The optimal optimization vector after the nth iteration that meets the configured iteration termination condition is used as the output optimal optimization vector, wherein the iteration termination condition is n>N, N is the configured maximum number of iterations; or the solution time is greater than the configured maximum solution time; or the absolute value of the difference between the cost of the nth iteration and the minimum cost after the n-1th iteration is not greater than the configured minimum convergence threshold, otherwise, let n=n+1.

[0026] In this preferred embodiment, after each iteration is completed, it is necessary to compare the calculated integrated cost with the minimum cost obtained in the previous iteration to determine the convergence of the optimization vector. Only when the integrated cost calculated based on the optimization vector of this iteration is less than the minimum cost of the previous iteration, the minimum cost and the optimal optimization vector are updated.

[0027] Preferably, the cost and gradient vector of the nth iteration are calculated according to the optimization vector of the nth iteration, the configured optimization items, the weight value of each optimization item, the cost and the gradient calculation function, including: Step 2-1, generating an optimization matrix for the nth iteration according to the number of optimization variables, the initial control point sequence and the optimization vector for the nth iteration; Step 2-2, calculating the cost and gradient vector of the ith optimization item of the nth iteration according to the optimization matrix of the nth iteration and the cost and gradient calculation function of the ith optimization item, wherein the input of the cost and gradient calculation function of the ith optimization item is the optimization matrix of the nth iteration, and the output is the cost and gradient vector of the ith optimization item of the nth iteration, 1≤i≤I, and I is the total number of configured optimization items; Step 2-3, calculate the integrated cost and gradient vector of the i-th optimization item of the n-th iteration according to the weight value of the i-th optimization item, the cost and gradient vector of the i-th optimization item of the n-th iteration; Step 2-4: When i<I, let i=i+1, return to step 2-2, and when i=I, use the integrated cost and gradient vector of the I-th optimization item of the n-th iteration as the cost and gradient vector of the n-th iteration.

[0028] Preferably, generating the optimization matrix of the nth iteration according to the number of optimization variables, the initial control point sequence and the optimization vector of the nth iteration includes: The parameter feas_pt_num is determined according to the number of optimization variables var_num and the control point dimension pt_dim, and its calculation formula is: feas_pt_num = var_num / pt_dim According to the outer traversal point j from 1 to order, the inner traversal point from k from 1 to pt_dim, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimization matrix q_mat of the nth iteration n , whose expression is: q_mat n [j, k] = ctrl_pts[j,k] According to the outer traversal point j from 1 to feas_pt_num, the inner traversal point k from 1 to pt_dim, the optimization vector q of the nth iteration nWrite the optimization matrix q_mat of the nth iteration n , whose expression is: q_mat n [j + order,k] =q n [(pt_dim-1) * j+k] When the endpoint constraint optimization item configuration flag indicates that the configured optimization item does not include the endpoint constraint, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimization matrix q_mat of the nth iteration according to the outer traversal point j from 1 to order and the inner traversal point k from 1 to pt_dim. n , whose expression is: q_mat n [order + feas_pt_num + j,k]= ctrl_pts[pt_num – or der + j, k] Among them, ctrl_pts is the initial control point sequence, and order is the B-spline order.

[0029] In this preferred embodiment, the one-dimensional optimization vector is converted into an optimization matrix because the ultimate purpose of the present invention is to optimize the initial control point matrix. Therefore, the optimization matrix used to calculate the cost of evaluating the iterative optimization effect needs to include both the control points to be optimized and the control points that do not participate in the iterative optimization. However, the optimization vector may change in each iteration, and the control point information that does not participate in the iterative optimization is unchanged. Therefore, after determining the optimization vector of each iteration, it is necessary to generate the corresponding optimization matrix based on the iterated optimization vector of the information of the initial control point sequence. In this embodiment, the end point constraint is configured, so the feas_pt_num value is 12, and the information of the first 3 rows in the optimization matrix is ​​determined based on the information of the first 3 control points of the initial control point sequence, while the information of the following 12 rows is determined based on the information of the one-dimensional optimization vector.

[0030] Preferably, the integrated cost and gradient vector of the i-th optimization item of the n-th iteration are calculated according to the weight value of the i-th optimization item, the cost and gradient vector of the i-th optimization item of the n-th iteration, wherein: The cost of the i-th optimization item integration at the n-th iteration cost_int n,i The expression is: cost_int n,i = cost_int n,i-1 + w i * cost n,i Among them, when i=1, cost_int n,i-1 The value of w is 0, iis the weight value of the i-th optimization item, cost n,i is the cost of the i-th optimization item in the n-th iteration; According to the weight value of the i-th optimization item, the gradient vector gradn,i of the i-th optimization item of the n-th iteration is generated according to the outer traversal point j from 1 to feas_pt_num and the inner traversal point k from 1 to pt_dim to generate the integrated gradient vector grad_int of the i-th optimization item of the n-th iteration. n,i , whose expression is: grad_int n,i [pt_dim*j ,k]= grad_int n,i-1 [pt_dim*j ,k] + w i *grad n,i [order+j,k] Among them, when i=1, grad_int n,i-1 The value of [pt_dim*j ,k] is 0.

[0031] In this embodiment, since the cost and gradient calculation function of each optimization item have been configured in advance, after the optimization matrix of each iteration is determined, it is substituted into the cost and gradient calculation function of the corresponding optimization item to calculate the corresponding cost and gradient vector. The smoothness constraint, obstacle avoidance constraint, guidance constraint and end point constraint selected in this embodiment are used as examples for explanation. Before calculating the cost and gradient vector of the first optimization item in each iteration, the initial value of the integrated cost and gradient vector is set to 0. Then, the cost and gradient vector are calculated in sequence according to the configured optimization items, and the integrated cost is clouded according to the integration formula. For the nth iteration, the cost and gradient vector of the four optimization items are calculated in sequence, and the integrated cost and gradient are updated according to the iterative calculation formula of the integrated cost and gradient to generate the final integrated cost and gradient of the nth iteration.

[0032] Assume that the cost and gradient calculation function of the smoothness constraint is CalcSmoothnessCost(q_mat, cost_s,grad_s_mat), where q_mat, cost_s and grad_s_mat correspond to the input optimization matrix, the output cost and gradient vector respectively. The method for solving the cost and gradient vector is as follows: Traverse j from 1 to (pt_num-order) and calculate jerk in a differential manner. The expression is: jerk = q_mat[j + 3] - 3 *q_mat[j + 2] + 3 * q_mat[j + 1]- q_mat[j] The cost and gradient vector of the smooth constraint are calculated according to the jerk, and the calculation formula is: cost_s = cost_s0 + cur_cost_s grad_s_mat[j + 0] =grad_s_mat0[j+ 0] -(2 * jerk) grad_s_mat[j+ 1] = grad_s_mat0[j+ 1] + 3 * (2 * jerk) grad_s_mat[j + 2] =grad_s_mat0[j+ 2] - 3 * (2 * jerk) grad_s_mat[j + 3] =grad_s_mat0[j+ 3] + (2 * jerk) Among them, cur_cost_s is defined as: cur_cost_s = a * (jerk[0]^2 +jerk[1]^2) + b * jerk[2]^2 In the formula, a and b represent custom weights, which represent the weights of the x and y dimensions, and the weight of the theta dimension respectively. jerk[0] and jerk[1] represent the jerks in the x and y dimensions, jerk[2] represents the jerks in the theta dimension, and the values ​​of the elements of cost_s0 and grad_s_mat0 are all 0.

[0033] Assume that the cost and gradient calculation function of the obstacle avoidance constraint is CalcDistanceCost(q_mat, cost_d,grad_d_mat). Similarly, q_mat, cost_d, and grad_d_mat correspond to the input optimization matrix, the output cost, and the gradient vector, respectively. When the end point constraint is configured, the method for solving the cost and gradient vector is as follows: Traverse j from order to pt_num, and query the distance dist and the corresponding gradient vector dist_grad from the grid at position q_mat[j, 1] in the x dimension and q_mat[j, 2] in the y dimension to the nearest obstacle through the real-time updated local Euclidean distance field map esdf, where pt_num is the number of control points in the initial control point sequence and order is the order of the uniform B-spline; Normalize the gradient vector dist_grad to obtain the normalized gradient vector dist_grad_int; Update the cost_d and gradient vector grad_d_mat of the barrier constraint according to the distance dist and the custom safety distance safe_dist, where: cost_d = (dist - safe_dist)^2 grad_d_mat[j]=grad_d_mat0[j] + 2 * (dist - safe_dist) * dist_grad_int In the formula, the values ​​of the elements of grad_d_mat0 are all 0.

[0034] The esdf in this preferred embodiment first constructs a local occupancy grid map based on the environment of the current planned path, and then generates a map representing the Euclidean distance from any position in the map to the nearest obstacle and the corresponding gradient direction based on the local occupancy grid map.

[0035] Suppose the cost and gradient calculation function of the guide constraint is CalcGuideCost(q_mat, cost_g, grad_g_mat). Similarly, q_mat, cost_g, grad_g_mat correspond to the input optimization matrix, the output cost and gradient vector respectively. The method for solving the cost and gradient vector is as follows: According to the input optimization matrix and the custom guide point sequence guide_pts, traverse j from order to (pt_num-order) and calculate delta. The calculation formula is: delta= q_mat[j] - guide_pts[j] The cost cost_g and gradient vector grad_g_mat of the guidance constraint are calculated according to the delta, and the calculation formula is: cost_g = cost_g0 + cur_cost_g grad_g_mat[j] =grad_g_mat0[j]+2* cur_cost_g Among them, cur_cost_g is defined as: cur_cost_g = a * (delta[1]^2 + delta[2]^2) + b * delta[3]^2 In the formula, a and b represent custom weights, which represent the weights of dimension x and dimension y, and the weight of dimension theta, respectively. delta[1] and delta[2] represent the delta in the x and y dimensions, delta[3] represents the delta in the theta dimension, and the values ​​of elements of cost_g0 and grad_g_mat0 are all 0.

[0036] In this preferred embodiment, the guide point sequence can be set as the initial control point sequence to ensure that the deviation of the optimized control point sequence from the initial control point sequence is within a controllable range, but the guide point sequence can also be designed by other algorithms and input from the outside.

[0037] Suppose the cost and gradient calculation function of the endpoint constraint is CalcEndpointCost(q_mat, cost_e, grad_e_mat). Similarly, q_mat, cost_e, and grad_e_mat correspond to the input optimization matrix, the output cost and gradient vector, respectively. The method for solving the cost and gradient vector is as follows: The following vectors are read from the initial control point sequence and the optimization matrix: cp_3 = ctrl_pts[pt_num - 3], cp_2 = ctrl_pts[pt_num - 2], cp_1 = ctrl_pts[pt_num - 1]; q_3 = q_mat[pt_num - 3], q_2 =q_mat[pt_num - 2], q_1 = q_mat[pt_num -1] The vectors ref and opt are calculated according to the vectors cp_3, cp_2, cp_1, q_3, q_2 and q_1, and the calculation formula is: ref = 1 / 6.0 * (cp_3 + 4 * cp_3 + cp_1) opt = 1 / 6.0 * (q_3 + 4 * q_2 + q_1) The vector delta is calculated according to the vector ref and opt, and the calculation formula is: delta = opt - ref The cost cost_e and the gradient vector grad_e_mat are calculated according to the vector delta, and the calculation formula is: cost_e = cost_e0 + cur_cost_e cur_cost_e = a * (delta[1]^2 +delta[2]^2) + b * delta[3]^2 grad_e_mat[pt_num - 3] =grad_e_mat0[pt_num - 3] + 2 * delta * (1 / 6.0) grad_e_mat[pt_num - 2] =grad_e_mat0[pt_num - 2] + 2 * delta * (4 / 6.0) grad_e_mat[pt_num - 1] =grad_e_mat0[pt_num - 1] + 2 * delta * (1 / 6.0) Where a and b represent custom weights, representing the weights of dimensions x, y, and theta, respectively. delta[1] and delta[2] represent deltas in the x and y dimensions, delta[3] represents delta in the theta dimension, and the elements in vectors cost_e0 and grad_e_mat0 are all 0.

[0038] In this preferred embodiment, since each iteration needs to calculate the cost and gradient of the optimization items one by one and then update the integrated cost and gradient, the optimization vector for the next iteration can be updated only after all the configured optimization items are calculated.

[0039] It should be noted that the smoothness constraints, obstacle avoidance constraints, guidance constraints and endpoint constraints in this preferred embodiment are all designed based on the control dimension of the initial control point sequence being three-dimensional. In practical applications, when the control dimension of the initial control point sequence is different, or the meaning of the dimensional representation is different, it can be redesigned as needed. The present invention does not limit the design of the cost and gradient function of each optimization item.

[0040] Preferably, the step of determining the optimization vector of the n+1th iteration based on the optimization algorithm according to the gradient vector of the nth iteration and the optimization boundary of the nth iteration comprises: Based on the optimization algorithm, solving the initial vector of the n+1th iteration according to the gradient vector of the nth iteration; When an element in the initial vector of the n+1th iteration is greater than a corresponding element in the optimization boundary of the nth iteration, the corresponding element in the optimization boundary of the nth iteration is written into the initial vector of the n+1th iteration to generate an optimization vector of the n+1th iteration.

[0041] In this preferred method, the update of the optimization vector must also meet the constraints of the optimization boundary. Therefore, when the optimization vector is updated based on the updated gradient vector, it is also necessary to determine whether the updated optimization vector exceeds the optimization boundary. When it exceeds the optimization boundary, the value should be based on the optimization boundary.

[0042] Preferably, determining the optimization boundary of the n+1th iteration according to the optimization vector of the nth iteration and the optimization vector of the n+1th iteration comprises: Determine, according to the optimization vector of the nth iteration and the optimization vector of the n+1th iteration, the change amplitude of each element in the optimization vector of the n+1th iteration relative to the corresponding element in the optimization vector of the nth iteration; Determine an optimized deviation vector for the (n+1)th iteration according to the change amplitude and the optimized deviation vector for the nth iteration; The optimization boundary of the (n+1)th iteration is determined according to the optimization deviation vector and the optimization vector of the (n+1)th iteration.

[0043] In this preferred implementation, there is no restriction on the configuration of the optimization deviation vector, as long as its change trend is ensured to be the same as the change trend of the optimization vector updated in each iteration.

[0044] In step 104, the solution time is reset to zero, and an optimized control point sequence is generated according to the optimal optimization vector and the initial control point sequence.

[0045] Preferably, generating an optimized control point sequence according to the optimal optimization vector and the initial control point sequence comprises: According to the outer traversal point j from 1 to order, the inner traversal point from k from 1 to pt_dim, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimized control point sequence ctrl_pts_opt, and its expression is: ctrl_pts_opt[j,k]= ctrl_pts[j,k] According to the outer traversal point j from 1 to feas_pt_num, the inner traversal point k from 1 to pt_dim will be the optimal optimization vector best_var after n-1 iterations n-1 Write the optimized control point sequence ctrl_pts_opt for the nth iteration, which is expressed as: ctrl_pts_opt[j + order,k]=best_var n-1 [(pt_dim-1) * j+k] When the endpoint constraint optimization item configuration flag indicates that the configured optimization item does not include the endpoint constraint, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimization control point sequence ctrl_pts_opt according to the outer traversal point j from 1 to order and the inner traversal point k from 1 to pt_dim, and its expression is: ctrl_pts_opt[order + feas_pt_num + j,k]= ctrl_pts[pt_num – or der +j, k] Among them, pt_num and pt_dim are the number of control points and the dimension of control points respectively.

[0046] In this preferred embodiment, after the best optimization vector is determined according to the optimization vectors of multiple iterations, the optimization control point sequence can be obtained by performing reverse generation according to the steps of generating the optimization vector from the initial control point sequence.

[0047] Figure 2 Schematic diagram of a control point sequence for a front-end planning path according to a preferred embodiment of the present invention. Figure 2 As shown, the blue path point sequence is the front-end rough solution, and the red path point sequence is obtained based on the optimized control point sequence. The direction of travel of the above two color path point sequences is from upper right to lower left. By comparison, it can be seen that the initial three path points are basically kept overlapping because no iterative optimization is performed, while the path points including the end point are obviously adjusted and are far away from the protruding roadblocks. It can be seen that the optimization method described in this preferred implementation can provide a better path point precise solution.

[0048] The control point sequence optimization method for path planning described in this preferred embodiment first obtains an initial control point sequence obtained by fitting the front-end rough solution based on the relevant properties of uniform B-splines before back-end optimization, and then conveniently performs optimization processing such as smoothing and obstacle avoidance on the path based on the initial control point sequence, thereby providing a better quality original initial value for the back-end optimization and reducing the back-end optimization burden.

[0049] Exemplary Systems Figure 3 FIG. 1 is a schematic diagram of the structure of a control point sequence optimization system for path planning according to a preferred embodiment of the present invention. Figure 3 As shown, the control point sequence optimization system 300 for path planning described in this preferred embodiment includes: The data acquisition module 301 is used to acquire an initial control point sequence of a uniform B-spline curve constructed based on a rough solution of the front-end planning path and a user-defined B-spline degree; The first calculation module 302 is used to start the solution time timing, and calculate the optimization vector and optimization boundary of the first iteration according to the initial control point sequence, the B-spline number, the configured initial optimization deviation vector and the endpoint constraint optimization item configuration flag, wherein the endpoint constraint optimization item configuration flag is used to determine whether the configured optimization item includes the endpoint constraint item; The second calculation module 303 is used to adopt the configured optimization algorithm for optimizing the front-end planning path, perform iterative calculations based on the optimization vector and optimization boundary of the first iteration according to the configured optimization items, the weight value, cost and gradient calculation function of each optimization item, and determine the optimal optimization vector that meets the configured iteration termination condition; The result output module 304 is used to solve the time timing reset and generate an optimized control point sequence according to the optimal optimization vector and the initial control point sequence.

[0050] Preferably, the first calculation module 302 calculates the optimization vector and optimization boundary of the first iteration according to the initial control point sequence, the B-spline order, the configured initial optimization deviation vector and the endpoint constraint optimization item configuration flag, including: Determining the number of optimization variables according to the number of rows and columns of the initial control point sequence, the B-spline order, and the endpoint constraint optimization item configuration flag; Generate an optimization vector for the first iteration according to the number of optimization variables and the initial control point sequence; The optimization boundary of the first iteration is determined according to the optimization vector of the first iteration and the initial optimization deviation vector.

[0051] Preferably, the first calculation module 302 determines the number of optimization variables according to the number of rows and columns of the initial control point sequence, the B-spline degree, and the endpoint constraint optimization item configuration flag, including: When the endpoint constraint optimization item configuration flag indicates that the configured optimization item includes the endpoint constraint, the expression for the number of optimization variables var_num is: var_num = max(0, pt_dim * (pt_num - 1 * order)) When the endpoint constraint optimization item configuration flag indicates that the configured optimization item does not include the endpoint constraint, the expression for the number of optimization variables var_num is: var_num = max(0, pt_dim * (pt_num - 2 * order)) In the formula, pt_dim is the number of columns of the initial control point sequence, representing the dimension of the control points, pt_num is the number of rows of the initial control point sequence, representing the number of control points, and order is the B-spline order.

[0052] Preferably, the first calculation module 302 generates the optimization vector of the first iteration according to the number of optimization variables and the initial control point sequence, including: Create a new optimization vector q1[m] with a length of var_num, where var_num is the number of optimization variables in the optimization vector q1[m], and 1≤m≤var_num; The control point information participating in the iterative optimization in the initial control point sequence is written into the corresponding optimization variables in the optimization vector q1[m] according to the outer traversal point j from order+1 to end_dix and the inner traversal point k from 1 to pt_dim. The expression is: q1[m]= q1[pt_dim * (j – order-1) + k]= ctrl_pts[j, k] Among them, ctrl_pts is the initial control point sequence. When the end point constraint optimization item configuration flag indicates that the configured optimization item includes the end point constraint, end_dix=pt_num; when the end point constraint optimization item configuration flag indicates that the configured optimization item does not include the end point constraint, end_dix=pt_num-order.

[0053] Preferably, the first calculation module 302 determines the optimization boundary of the first iteration according to the optimization vector of the first iteration and the initial optimization deviation vector, including: Generate the first iteration optimization deviation vector bound1 with a length of var_num according to the optimization variable number var_num; Create an upper optimization boundary vector lb1[m] and a lower optimization boundary vector ub1[m] of length var_num; According to the information of the optimization variables in the optimization vector q1[m] of the first iteration and the initial optimization deviation vector, the upper optimization boundary vector ib1[m] and the lower optimization boundary vector ub1[m] are assigned values, where: lb1[m] = q1[m]- bound1 ub1[m] = q1[m]+bound1 In the formula, 1≤m≤var_num.

[0054] Preferably, the second calculation module 303 uses a configured optimization algorithm for optimizing the front-end planning path, performs iterative operations based on the optimization vector and optimization boundary of the first iteration according to the configured optimization items, the weight value, cost and gradient calculation function of each optimization item, and determines the optimal optimization vector that meets the configured iteration termination condition, including: Calculate the cost and gradient vector of the nth iteration according to the optimization vector of the nth iteration, the configured optimization items, the weight value of each optimization item, the cost and the gradient calculation function, where n is a positive integer, 1≤n≤N; Based on the optimization algorithm, determining an optimization vector for the n+1th iteration according to the gradient vector for the nth iteration and the optimization boundary for the nth iteration; Determine an optimization boundary of the n+1th iteration according to the optimization vector of the nth iteration and the optimization vector of the n+1th iteration; When the cost of the nth iteration is less than the minimum cost after the n-1th iteration, the minimum cost after the nth iteration is made the cost of the nth iteration, and the optimal optimization vector after the nth iteration is made the optimization vector of the nth iteration, otherwise, the minimum cost and the optimal optimization vector after the nth iteration remain unchanged, wherein, when n=1, the minimum cost after the n-1th iteration is the initial value of the configured minimum cost; The optimal optimization vector after the nth iteration that meets the configured iteration termination condition is used as the output optimal optimization vector, wherein the iteration termination condition is n>N, N is the configured maximum number of iterations; or the solution time is greater than the configured maximum solution time; or the absolute value of the difference between the cost of the nth iteration and the minimum cost after the n-1th iteration is not greater than the configured minimum convergence threshold, otherwise, let n=n+1.

[0055] Preferably, the second calculation module 303 calculates the cost and gradient vector of the nth iteration according to the optimization vector of the nth iteration, the configured optimization items, the weight value of each optimization item, the cost and the gradient calculation function, including: Step 2-1, generating an optimization matrix for the nth iteration according to the number of optimization variables, the initial control point sequence and the optimization vector for the nth iteration; Step 2-2, calculating the cost and gradient vector of the ith optimization item of the nth iteration according to the optimization matrix of the nth iteration and the cost and gradient calculation function of the ith optimization item, wherein the input of the cost and gradient calculation function of the ith optimization item is the optimization matrix of the nth iteration, and the output is the cost and gradient vector of the ith optimization item of the nth iteration, 1≤i≤I, and I is the total number of configured optimization items; Step 2-3, calculate the integrated cost and gradient vector of the i-th optimization item of the n-th iteration according to the weight value of the i-th optimization item, the cost and gradient vector of the i-th optimization item of the n-th iteration; Step 2-4: When i<I, let i=i+1, return to step 2-2, and when i=I, use the integrated cost and gradient vector of the I-th optimization item of the n-th iteration as the cost and gradient vector of the n-th iteration.

[0056] Preferably, the second calculation module 303 generates an optimization matrix for the nth iteration according to the number of optimization variables, the initial control point sequence and the optimization vector for the nth iteration, including: The parameter feas_pt_num is determined according to the number of optimization variables var_num and the control point dimension pt_dim, and its calculation formula is: feas_pt_num = var_num / pt_dim According to the outer traversal point j from 1 to order, the inner traversal point from k from 1 to pt_dim, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimization matrix q_mat of the nth iteration n , whose expression is: q_mat n [j, k] = ctrl_pts[j,k] According to the outer traversal point j from 1 to feas_pt_num, the inner traversal point k from 1 to pt_dim, the optimization vector q of the nth iteration n Write the optimization matrix q_mat of the nth iteration n , whose expression is: q_mat n [j + order,k] =q n [(pt_dim-1) * j+k] When the endpoint constraint optimization item configuration flag indicates that the configured optimization item does not include the endpoint constraint, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimization matrix q_mat of the nth iteration according to the outer traversal point j from 1 to order and the inner traversal point k from 1 to pt_dim. n , whose expression is: q_mat n [order + feas_pt_num + j,k]= ctrl_pts[pt_num – or der + j, k] Among them, ctrl_pts is the initial control point sequence, and order is the B-spline order.

[0057] Preferably, the second calculation module 303 calculates the integrated cost and gradient vector of the i-th optimization item of the n-th iteration according to the weight value of the i-th optimization item, the cost and gradient vector of the i-th optimization item of the n-th iteration, wherein: The cost of the i-th optimization item integration at the n-th iteration cost_int n,i The expression is: cost_int n,i = cost_intn,i-1 + w i * cost n,i Among them, when i=1, cost_int n,i-1 The value of w is 0, i is the weight value of the i-th optimization item, cost n,i is the cost of the i-th optimization item in the n-th iteration; According to the weight value of the i-th optimization item, the gradient vector gradn,i of the i-th optimization item of the n-th iteration is generated according to the outer traversal point j from 1 to feas_pt_num and the inner traversal point k from 1 to pt_dim to generate the integrated gradient vector grad_int of the i-th optimization item of the n-th iteration. n,i , whose expression is: grad_int n,i [pt_dim*j ,k]= grad_int n,i-1 [pt_dim*j ,k] + w i *grad n,i [order+j,k] Among them, when i=1, grad_int n,i-1 The value of [pt_dim*j ,k] is 0.

[0058] Preferably, the second calculation module 303 determines the optimization vector of the n+1th iteration based on the optimization algorithm according to the gradient vector of the nth iteration and the optimization boundary of the nth iteration, including: Based on the optimization algorithm, solving the initial vector of the n+1th iteration according to the gradient vector of the nth iteration; When an element in the initial vector of the n+1th iteration is greater than a corresponding element in the optimization boundary of the nth iteration, the corresponding element in the optimization boundary of the nth iteration is written into the initial vector of the n+1th iteration to generate an optimization vector of the n+1th iteration.

[0059] Preferably, the second calculation module 303 determines the optimization boundary of the n+1th iteration according to the optimization vector of the nth iteration and the optimization vector of the n+1th iteration, including: Determine, according to the optimization vector of the nth iteration and the optimization vector of the n+1th iteration, the change amplitude of each element in the optimization vector of the n+1th iteration relative to the corresponding element in the optimization vector of the nth iteration; Determine an optimized deviation vector for the (n+1)th iteration according to the change amplitude and the optimized deviation vector for the nth iteration; The optimization boundary of the (n+1)th iteration is determined according to the optimization deviation vector and the optimization vector of the (n+1)th iteration.

[0060] Preferably, the second calculation module 303 generates an optimized control point sequence according to the optimal optimization vector and the initial control point sequence, including: According to the outer traversal point j from 1 to order, the inner traversal point from k from 1 to pt_dim, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimized control point sequence ctrl_pts_opt, and its expression is: ctrl_pts_opt[j,k]= ctrl_pts[j,k] According to the outer traversal point j from 1 to feas_pt_num, the inner traversal point k from 1 to pt_dim will be the optimal optimization vector best_var after n-1 iterations n-1 Write the optimized control point sequence ctrl_pts_opt for the nth iteration, which is expressed as: ctrl_pts_opt[j + order,k]=best_var n-1 [(pt_dim-1) * j+k] When the endpoint constraint optimization item configuration flag indicates that the configured optimization item does not include the endpoint constraint, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimization control point sequence ctrl_pts_opt according to the outer traversal point j from 1 to order and the inner traversal point k from 1 to pt_dim, and its expression is: ctrl_pts_opt[order + feas_pt_num + j,k]= ctrl_pts[pt_num – or der +j, k] Among them, pt_num and pt_dim are the number of control points and the dimension of control points respectively.

[0061] The control point sequence optimization system for path planning described in this preferred embodiment and the control point sequence optimization method for path planning optimize the initial control point sequence obtained by fitting the path based on uniform B-spline to obtain the rough solution of the front-end planning path, and the steps for generating the optimized control point sequence are the same, and the technical effects achieved are also the same, which will not be repeated here.

[0062] Exemplary Electronic Devices Figure 4 The electronic device according to the preferred embodiment of the present invention is a schematic diagram of the structure of the electronic device. The electronic device can be any one or both of the first device and the second device, or a stand-alone device independent of them, and the stand-alone device can communicate with the first device and the second device to receive the collected input signal from them. Figure 4 FIG. 1 is a block diagram of an electronic device according to an embodiment of the present disclosure. Figure 4 As shown, the electronic device includes one or more processors 401 and a memory 402 .

[0063] The processor 401 may be a central processing unit (CPU) or other forms of processing units having data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device to perform desired functions.

[0064] The memory 402 may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may include, for example, random access memory (RAM) and / or cache memory (cache), etc. The non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 401 may run the program instructions to implement the energy consumption anomaly diagnosis method based on the enterprise energy consumption space of the various embodiments disclosed above and / or other desired functions. In one example, the electronic device may also include: an input device 403 and an output device 404, which are interconnected via a bus system and / or other forms of connection mechanisms (not shown).

[0065] In addition, the input device 403 may also include, for example, a keyboard, a mouse, and the like.

[0066] The output device 404 can output various information to the outside, and can include, for example, a display, a speaker, a printer, a communication network and a remote output device connected thereto.

[0067] Of course, to simplify, Figure 4 Only some of the components related to the present disclosure in the electronic device are shown, and components such as a bus, an input / output interface, etc. are omitted. In addition, according to specific application situations, the electronic device may further include any other appropriate components.

[0068] Exemplary computer program products and computer-readable storage media In addition to the above-mentioned methods and devices, an embodiment of the present disclosure may also be a computer program product, which includes computer program instructions, which, when executed by a processor, enable the processor to execute the steps of the control point sequence optimization method for path planning according to various embodiments of the present disclosure described in the above-mentioned "Exemplary Method" section of this specification.

[0069] The computer program product may be written in any combination of one or more programming languages ​​to write program code for performing the operations of the disclosed embodiments, including object-oriented programming languages ​​such as Java, C++, etc., and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as a separate software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0070] In addition, an embodiment of the present disclosure may also be a computer-readable storage medium having computer program instructions stored thereon, which, when executed by a processor, enables the processor to execute the steps of the control point sequence optimization method for path planning according to various embodiments of the present disclosure described in the above “Exemplary Method” section of this specification.

[0071] The computer readable storage medium can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can include, for example, but is not limited to, a system, device or device of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination of the above. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0072] The basic principles of the present disclosure are described above in conjunction with specific embodiments. However, it should be noted that the advantages, strengths, effects, etc. mentioned in the present disclosure are only examples and not limitations, and it cannot be considered that these advantages, strengths, effects, etc. are required by each embodiment of the present disclosure. In addition, the specific details disclosed above are only for the purpose of illustration and ease of understanding, and are not limitations. The above details do not limit the present disclosure to the necessity of adopting the above specific details to be implemented.

[0073] Each embodiment in this specification is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system embodiment, since it basically corresponds to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0074] The block diagrams of the devices, apparatuses, equipment, and systems involved in this disclosure are only illustrative examples and are not intended to require or imply that they must be connected, arranged, and configured in the manner shown in the block diagrams. As will be appreciated by those skilled in the art, these devices, apparatuses, equipment, and systems can be connected, arranged, and configured in any manner. Words such as "including," "comprising," "having," and the like are open words, referring to "including but not limited to," and can be used interchangeably therewith. The words "or" and "and" used herein refer to the words "and / or," and can be used interchangeably therewith, unless the context clearly indicates otherwise. The word "such as" used herein refers to the phrase "such as but not limited to," and can be used interchangeably therewith.

[0075] The apparatus and method of the present disclosure may be implemented in many ways. For example, the apparatus and method of the present disclosure may be implemented by software, hardware, firmware, or any combination of software, hardware, and firmware. The above order of steps for the method is for illustration only, and the steps of the method of the present disclosure are not limited to the order specifically described above, unless otherwise specifically stated. In addition, in some embodiments, the present disclosure may also be implemented as a program recorded in a recording medium, which includes machine-readable instructions for implementing the method according to the present disclosure. Therefore, the present disclosure also covers a recording medium storing a program for executing the method according to the present disclosure.

[0076] It should also be noted that in the apparatus, equipment and method of the present disclosure, each component or each step can be decomposed and / or recombined. These decompositions and / or recombinations should be regarded as equivalent schemes of the present disclosure. The above description of the disclosed aspects is provided to enable any technician in the field to make or use the present disclosure. Various modifications to these aspects are very obvious to those skilled in the art, and the general principles defined herein can be applied to other aspects without departing from the scope of the present disclosure. Therefore, the present disclosure is not intended to be limited to the aspects shown here, but to the widest scope consistent with the principles and novel features disclosed herein.

[0077] The above description has been given for the purpose of illustration and description. In addition, this description is not intended to limit the embodiments of the present disclosure to the forms disclosed herein. Although multiple example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, changes, additions and sub-combinations thereof.

Claims

1. A control point sequence optimization method for path planning, characterized in that: The method comprises: Get the initial control point sequence of the uniform B-spline curve constructed based on the rough solution of the front-end planning path and the custom B-spline degree; Start the solution time timing, and calculate the optimization vector and optimization boundary of the first iteration according to the initial control point sequence, the B-spline number, the configured initial optimization deviation vector and the endpoint constraint optimization item configuration flag, wherein the endpoint constraint optimization item configuration flag is used to determine whether the configured optimization item includes the endpoint constraint item; Adopting the configured optimization algorithm for optimizing the front-end planning path, performing iterative operations based on the optimization vector and optimization boundary of the first iteration according to the configured optimization items, the weight value, cost and gradient calculation function of each optimization item, and determining the optimal optimization vector that meets the configured iteration termination condition; The solution time is reset to zero, and an optimized control point sequence is generated according to the optimal optimization vector and the initial control point sequence.

2. The method according to claim 1, characterized in that The step of calculating the optimization vector and optimization boundary of the first iteration according to the initial control point sequence, the B-spline order, the configured initial optimization deviation vector and the endpoint constraint optimization item configuration flag includes: Determining the number of optimization variables according to the number of rows and columns of the initial control point sequence, the B-spline order, and the endpoint constraint optimization item configuration flag; Generate an optimization vector for the first iteration according to the number of optimization variables and the initial control point sequence; The optimization boundary of the first iteration is determined according to the optimization vector of the first iteration and the initial optimization deviation vector.

3. The method according to claim 2, characterized in that The step of determining the number of optimization variables according to the number of rows and columns of the initial control point sequence, the number of B-spline orders, and the end point constraint optimization item configuration flag comprises: When the endpoint constraint optimization item configuration flag indicates that the configured optimization item includes the endpoint constraint, the expression for the number of optimization variables var_num is: var_num = max(0, pt_dim * (pt_num - 1 * order)) When the endpoint constraint optimization item configuration flag indicates that the configured optimization item does not include the endpoint constraint, the expression for the number of optimization variables var_num is: var_num = max(0, pt_dim * (pt_num - 2 * order)) In the formula, pt_dim is the number of columns of the initial control point sequence, representing the dimension of the control points, pt_num is the number of rows of the initial control point sequence, representing the number of control points, and order is the B-spline order.

4. The method according to claim 2, characterized in that: The step of generating the optimization vector for the first iteration according to the number of optimization variables and the initial control point sequence includes: Create a new optimization vector q1[m] with a length of var_num, where var_num is the number of optimization variables in the optimization vector q1[m], and 1≤m≤var_num; The control point information participating in the iterative optimization in the initial control point sequence is written into the corresponding optimization variables in the optimization vector q1 [m] according to the outer traversal point j from order+1 to end_dix and the inner traversal point k from 1 to pt_dim. The expression is: q1 [m]= q1 [pt_dim * (j – order-1) + k] = ctrl_pts[j, k] Among them, ctrl_pts is the initial control point sequence. When the end point constraint optimization item configuration flag indicates that the configured optimization item includes the end point constraint, end_dix=pt_num; when the end point constraint optimization item configuration flag indicates that the configured optimization item does not include the end point constraint, end_dix=pt_num-order.

5. The method according to claim 2, characterized in that: The step of determining the optimization boundary of the first iteration according to the optimization vector of the first iteration and the initial optimization deviation vector comprises: Generate the first iteration optimization deviation vector bound1 with a length of var_num according to the optimization variable number var_num; Create an upper optimization boundary vector lb1[m] and a lower optimization boundary vector ub1[m] of length var_num; According to the information of the optimization variables in the optimization vector q1[m] of the first iteration and the initial optimization deviation vector, the upper optimization boundary vector ib1[m] and the lower optimization boundary vector ub1[m] are assigned values, where: lb1[m] = q1[m] - bound1 ub1[m] = q1[m] +bound1 In the formula, 1≤m≤var_num.

6. The method according to claim 2, characterized in that The optimization algorithm for optimizing the front-end planning path using the configuration, performing iterative operations based on the optimization vector and optimization boundary of the first iteration according to the configured optimization items, the weight value, cost and gradient calculation function of each optimization item, and determining the optimal optimization vector that meets the configured iteration termination condition, includes: Calculate the cost and gradient vector of the nth iteration according to the optimization vector of the nth iteration, the configured optimization items, the weight value of each optimization item, the cost and the gradient calculation function, where n is a positive integer, 1≤n≤N; Based on the optimization algorithm, determining an optimization vector for the n+1th iteration according to the gradient vector for the nth iteration and the optimization boundary for the nth iteration; Determine an optimization boundary of the n+1th iteration according to the optimization vector of the nth iteration and the optimization vector of the n+1th iteration; When the cost of the nth iteration is less than the minimum cost after the n-1th iteration, the minimum cost after the nth iteration is made the cost of the nth iteration, and the optimal optimization vector after the nth iteration is made the optimization vector of the nth iteration, otherwise, the minimum cost and the optimal optimization vector after the nth iteration remain unchanged, wherein, when n=1, the minimum cost after the n-1th iteration is the initial value of the configured minimum cost; The optimal optimization vector after the nth iteration that meets the configured iteration termination condition is used as the output optimal optimization vector, wherein the iteration termination condition is n>N, N is the configured maximum number of iterations; or the solution time is greater than the configured maximum solution time; or the absolute value of the difference between the cost of the nth iteration and the minimum cost after the n-1th iteration is not greater than the configured minimum convergence threshold, otherwise, let n=n+1.

7. The method according to claim 6, characterized in that The cost and gradient vector of the nth iteration are calculated according to the optimization vector of the nth iteration, the configured optimization items, the weight value of each optimization item, the cost and the gradient calculation function, including: Step 2-1, generating an optimization matrix for the nth iteration according to the number of optimization variables, the initial control point sequence and the optimization vector for the nth iteration; Step 2-2, calculating the cost and gradient vector of the ith optimization item of the nth iteration according to the optimization matrix of the nth iteration and the cost and gradient calculation function of the ith optimization item, wherein the input of the cost and gradient calculation function of the ith optimization item is the optimization matrix of the nth iteration, and the output is the cost and gradient vector of the ith optimization item of the nth iteration, 1≤i≤I, and I is the total number of configured optimization items; Step 2-3, calculate the integrated cost and gradient vector of the i-th optimization item of the n-th iteration according to the weight value of the i-th optimization item, the cost and gradient vector of the i-th optimization item of the n-th iteration; Step 2-4: When i<I, let i=i+1, return to step 2-2, and when i=I, use the integrated cost and gradient vector of the I-th optimization item of the n-th iteration as the cost and gradient vector of the n-th iteration.

8. The method according to claim 7, characterized in that The step of generating an optimization matrix for the nth iteration according to the number of optimization variables, the initial control point sequence and the optimization vector for the nth iteration comprises: The parameter feas_pt_num is determined according to the number of optimization variables var_num and the control point dimension pt_dim, and its calculation formula is: feas_pt_num = var_num / pt_dim According to the outer traversal point j from 1 to order, the inner traversal point from k from 1 to pt_dim, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimization matrix q_mat of the nth iteration n , whose expression is: q_mat n [j, k] = ctrl_pts[j,k] According to the outer traversal point j from 1 to feas_pt_num, the inner traversal point k from 1 to pt_dim, the optimization vector q of the nth iteration n Write the optimization matrix q_mat of the nth iteration n , whose expression is: q_mat n [j + order,k] = q n [(pt_dim-1) * j+k] When the endpoint constraint optimization item configuration flag indicates that the configured optimization item does not include the endpoint constraint, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimization matrix q_mat of the nth iteration according to the outer traversal point j from 1 to order and the inner traversal point k from 1 to pt_dim. n , whose expression is: q_mat n [order + feas_pt_num + j,k] = ctrl_pts[pt_num – or der + j, k] Among them, ctrl_pts is the initial control point sequence, and order is the B-spline order.

9. The method according to claim 7, characterized in that: The integrated cost and gradient vector of the i-th optimization item of the n-th iteration are calculated according to the weight value of the i-th optimization item, the cost and gradient vector of the i-th optimization item of the n-th iteration, wherein: The cost of the i-th optimization item integration at the n-th iteration cost_int n,i The expression is: cost_int n,i = cost_int n,i-1 + w i * cost n,i Among them, when i=1, cost_int n,i-1 The value of w is 0, i is the weight value of the i-th optimization item, cost n,i is the cost of the i-th optimization item in the n-th iteration; According to the weight value of the i-th optimization item, the gradient vector gradn,i of the i-th optimization item of the n-th iteration is generated according to the outer traversal point j from 1 to feas_pt_num and the inner traversal point k from 1 to pt_dim to generate the integrated gradient vector grad_int of the i-th optimization item of the n-th iteration. n,i , whose expression is: grad_int n,i [pt_dim*j ,k]= grad_int n,i-1 [pt_dim*j ,k] + w i *grad n,i [order+ j,k] Among them, when i=1, grad_int n,i-1 The value of [pt_dim*j ,k] is 0.

10. The method according to claim 6, characterized in that The step of determining the optimization vector of the n+1th iteration based on the optimization algorithm and the gradient vector of the nth iteration and the optimization boundary of the nth iteration includes: Based on the optimization algorithm, solving the initial vector of the n+1th iteration according to the gradient vector of the nth iteration; When an element in the initial vector of the n+1th iteration is greater than a corresponding element in the optimization boundary of the nth iteration, the corresponding element in the optimization boundary of the nth iteration is written into the initial vector of the n+1th iteration to generate an optimization vector of the n+1th iteration.

11. The method according to claim 6, characterized in that The step of determining the optimization boundary of the n+1th iteration according to the optimization vector of the nth iteration and the optimization vector of the n+1th iteration includes: Determine, according to the optimization vector of the nth iteration and the optimization vector of the n+1th iteration, the change amplitude of each element in the optimization vector of the n+1th iteration relative to the corresponding element in the optimization vector of the nth iteration; Determine an optimized deviation vector for the (n+1)th iteration according to the change amplitude and the optimized deviation vector for the nth iteration; The optimization boundary of the (n+1)th iteration is determined according to the optimization deviation vector and the optimization vector of the (n+1)th iteration.

12. The method according to claim 1, characterized in that The step of generating an optimized control point sequence according to the optimal optimization vector and the initial control point sequence comprises: According to the outer traversal point j from 1 to order, the inner traversal point from k from 1 to pt_dim, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimized control point sequence ctrl_pts_opt, and its expression is: ctrl_pts_opt[j,k] = ctrl_pts[j,k] According to the outer traversal point j from 1 to feas_pt_num, the inner traversal point k from 1 to pt_dim will be the optimal optimization vector best_var after n-1 iterations n-1 Write the optimized control point sequence ctrl_pts_opt for the nth iteration, which is expressed as: ctrl_pts_opt[j + order,k] =best_var n-1 [(pt_dim-1) * j+k] When the endpoint constraint optimization item configuration flag indicates that the configured optimization item does not include the endpoint constraint, the control point information that does not participate in the iterative optimization in the initial control point sequence is written into the optimization control point sequence ctrl_pts_opt according to the outer traversal point j from 1 to order and the inner traversal point k from 1 to pt_dim, and its expression is: ctrl_pts_opt[order + feas_pt_num + j,k] = ctrl_pts[pt_num – or der + j,k] Among them, pt_num and pt_dim are the number of control points and the dimension of control points respectively.

13. A control point sequence optimization system for path planning, characterized in that: The system comprises: A data acquisition module is used to obtain an initial control point sequence of a uniform B-spline curve constructed based on a rough solution of the front-end planning path and a custom B-spline degree; A first calculation module is used to start the solution time timing, and calculate the optimization vector and optimization boundary of the first iteration according to the initial control point sequence, the B-spline number, the configured initial optimization deviation vector and the endpoint constraint optimization item configuration flag, wherein the endpoint constraint optimization item configuration flag is used to determine whether the configured optimization item includes the endpoint constraint item; A second calculation module is used to adopt a configured optimization algorithm for optimizing the front-end planning path, perform iterative operations based on the optimization vector and optimization boundary of the first iteration according to the configured optimization items, the weight value, cost and gradient calculation function of each optimization item, and determine the optimal optimization vector that meets the configured iteration termination condition; The result output module is used to solve the time timing and return to zero, and generate an optimized control point sequence according to the optimal optimization vector and the initial control point sequence.

14. A computer-readable storage medium, characterized in that: The storage medium stores a computer program, and the computer program is used to execute the method according to any one of claims 1 to 12.

15. An electronic device, characterized in that: The electronic device comprises: processor; a memory for storing instructions executable by the processor; The processor is configured to read the executable instructions from the memory and execute the instructions to implement the method described in any one of claims 1 to 12.

Images