A path fitting method and system based on uniform B-spline

Abstract

The present invention provides a path fitting method and system based on uniform B-spline, the method comprising obtaining a rough solution of a front-end planned path; fitting according to the sampling values ​​of the independent variables and dependent variables of N functions stored in the rough solution, the node interval between any two adjacent sampling points of the independent variables and the custom B-spline number, and determining an initial control point sequence; optimizing the initial control point sequence to generate an optimized control point sequence; based on the optimized control point sequence, iteratively calculating in the definition domain length of the independent variables of the uniform B-spline curve according to the custom sampling interval, and obtaining a back-end refined solution generated by the rough solution based on the uniform B-spline curve. The method and system are fitted through the front-end rough solution, and the path can be easily smoothed, obstacle avoided, etc. according to the obtained control point sequence, and a better quality original initial value is provided for the back-end optimization, reducing the back-end optimization burden.

Description

Technical Field

[0001] The present invention relates to the field of vehicle automatic driving path planning, and more specifically, to a path fitting method and system based on uniform B-spline. 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 that the refined solutions obtained in the front-end path planning of unmanned vehicles in the prior art are in different forms, resulting in inconvenience in using a unified model to characterize the front-end refined solutions in back-end processing, the present invention provides a path fitting method and system based on uniform B-splines.

[0004] According to one aspect of the present invention, the present invention provides a path fitting method based on uniform B-spline, comprising:

[0005] Obtain a rough solution of the front-end planned path, wherein the rough solution is a sequence of several discrete points, each discrete point corresponds to a path point, and the basic information stored in each path point includes N dimensions representing the path point, accumulated mileage, and accumulated time;

[0006] Constructing N functions that use uniform B-spline curves to characterize the relationship between basic information of waypoints, wherein the independent variables of the N functions are accumulated mileage or accumulated time, and the dependent variables are N dimensions of the waypoints;

[0007] According to the sampling values ​​of the independent variables and dependent variables of the N functions stored in each discrete point sequence, the node interval between any two adjacent sampling points of the independent variable and the custom B-spline degree are characterized, and the path fitting is performed on each discrete point sequence to determine the initial control point sequence of the uniform B-spline curve constructed based on the rough solution;

[0008] Optimizing the initial control point sequence based on the B-spline property to generate an optimized control point sequence;

[0009] Based on the optimized control point sequence, iterative calculation is performed in the definition domain length of the independent variable of the uniform B-spline curve according to the user-defined sampling interval to obtain a back-end refined solution generated by the rough solution based on the uniform B-spline curve.

[0010] According to another aspect of the present invention, the present invention provides a path fitting system based on uniform B-spline, the system comprising:

[0011] A data acquisition module is used to obtain a rough solution of the front-end planned path, wherein the rough solution is a sequence of several discrete points, each discrete point corresponds to a path point, and the basic information stored in each path point includes N dimensions representing the path point, accumulated mileage and accumulated time;

[0012] A function construction module, used to construct N functions that use uniform B-spline curves to characterize the relationship between basic information of waypoints, wherein the independent variables of the N functions are accumulated mileage or accumulated time, and the dependent variables are N dimensions of the waypoints;

[0013] The first sequence module is used to perform path fitting on each discrete point sequence according to the sampling values ​​of the independent variables and dependent variables of the N functions stored in each discrete point sequence, the node interval and the custom B-spline order, and determine the initial control point sequence of the uniform B-spline curve constructed based on the rough solution;

[0014] A second sequence module is used to optimize the initial control point sequence based on the B-spline property to generate an optimized control point sequence;

[0015] The optimization path module is used to perform iterative calculations in the domain length of the independent variable of the uniform B-spline curve according to the user-defined sampling interval based on the optimization control point sequence to obtain a back-end refined solution generated by the rough solution based on the uniform B-spline curve.

[0016] 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.

[0017] 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.

[0018] The path fitting method and system based on uniform B-spline of the present invention comprises the following steps: obtaining a rough solution of a front-end planned path; constructing N functions using uniform B-spline curves to characterize the relationship between basic information of path points in the rough solution, wherein the independent variables of the N functions are accumulated mileage or accumulated time, and the dependent variables are N dimensions of the path points; performing path fitting on each discrete point sequence according to the sampling values ​​of the independent variables and dependent variables of the N functions stored in each discrete point sequence, characterizing the node interval and the custom B-spline number between any two adjacent sampling points of the independent variables, and determining an initial control point sequence of the uniform B-spline curve constructed based on the rough solution; optimizing the initial control point sequence based on the B-spline property to generate an optimized control point sequence; and performing iterative calculation based on the optimized control point sequence in the definition domain length of the independent variable of the uniform B-spline curve according to the custom sampling interval to obtain a back-end refined solution generated by the rough solution based on the uniform B-spline curve. The method and system, before back-end optimization, first fit the front-end rough solution based on the relevant properties of uniform B-splines, and according to the obtained control point sequence, not only can the path be easily smoothed, obstacle avoided, etc., but also a better quality original initial value is provided for the back-end optimization, thereby reducing the back-end optimization burden. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] A more complete understanding of exemplary embodiments of the present invention may be obtained by referring to the following drawings:

[0020] Figure 1 A flow chart of a path fitting method based on uniform B-spline according to a preferred embodiment of the present invention;

[0021] Figure 2 A schematic diagram of a rough solution of a front-end planning path according to a preferred embodiment of the present invention;

[0022] Figure 3 A schematic diagram of control points obtained according to a preferred embodiment of the present invention;

[0023] Figure 4 A schematic diagram of a back-end refinement according to a preferred embodiment of the present invention;

[0024] Figure 5 A schematic diagram of the structure of a path fitting system based on uniform B-spline according to a preferred embodiment of the present invention;

[0025] Figure 6 Schematic diagram of the structure of an electronic device according to a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0026] 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.

[0027] 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.

[0028] Exemplary Methods

[0029] Figure 1 FIG. 1 is a flow chart of a path fitting method based on uniform B-spline according to a preferred embodiment of the present invention. Figure 1 As shown, the path fitting method based on uniform B-spline described in this preferred embodiment starts from step 101.

[0030] In step 101, a rough solution of the front-end planned path is obtained, wherein the rough solution is a sequence of several discrete points, each discrete point corresponds to a path point, and the basic information stored in each path point includes N dimensions representing the path point, accumulated mileage and accumulated time.

[0031] For autonomous driving path planning in unstructured environments, algorithms such as Dijkstra are often used to perform front-end path planning to obtain a rough solution. Figure 2 FIG. 1 is a schematic diagram of a rough solution of a front-end planning path according to a preferred embodiment of the present invention. Figure 2 As shown in FIG. 1 , the path points of the rough solution obtained in this preferred embodiment are very sparse, resulting in the connected path being closer to a broken line, which cannot meet the vehicle's requirement for smoothness during driving. Figure 2 Even if the rough solution shown considers the kinematic constraint algorithm, such as hybrid A*, it can meet a certain smoothness, but it still needs to be processed by the back end. In addition, the rough solutions obtained by different front-end algorithms are in different forms. When optimizing the rough solution at the back end, it is not convenient to use a unified model to represent the path obtained by the front-end planning, which leads to reduced efficiency and excessive burden on the back end processing. Figure 2 The 13 path points in the rough solution are used as an example for explanation.

[0032] In step 102, N functions are constructed using uniform B-spline curves to characterize the relationship between basic information of waypoints, wherein the independent variables of the N functions are accumulated mileage or accumulated time, and the dependent variables are N dimensions of the waypoints.

[0033] In the rough solution of the front-end planning path, for each path point, the accumulated mileage, accumulated time, and multiple dimensions of data information representing the location of the path point are generally stored. Figure 2 The dimensions of the path points in are three-dimensional, namely x, y, and θ. Then, the accumulated mileage s is taken as the independent variable, and the three dimensions of the path points x, y, and θ are the dependent variables. The uniform B-spline can be used to represent the following functional relationship: x=f(s), y=f(s), θ=f(s). The principle of taking the accumulated time as the independent variable is the same.

[0034] In step 103, according to the sampling values ​​of the independent variables and dependent variables of the N functions stored in each discrete point sequence, the node interval between any two adjacent sampling points of the independent variable and the customized B-spline degree are characterized, and a path fitting is performed on each discrete point sequence to determine an initial control point sequence of the uniform B-spline curve constructed based on the rough solution.

[0035] Preferably, the step of fitting each discrete point sequence according to the sampling values ​​of the independent variables and dependent variables of the N functions stored in each discrete point sequence, the node interval between any two adjacent sampling points representing the independent variables, and the customized B-spline degree, and determining the discrete point sequence before the initial control point sequence of the uniform B-spline curve constructed based on the rough solution comprises:

[0036] When the difference between the sampling values ​​of any two adjacent discrete points of the independent variable is not equal, several discrete point sequences are fitted, and the fitting results are resampled according to the difference between the sampling values ​​of two adjacent sampling points to generate several discrete point sequences for path fitting.

[0037] For each discrete point sequence in the rough solution, nodes are used to represent the sampling points of the independent variables. If the difference in sampling values ​​between two adjacent nodes is the same, it means that the nodes have the same interval. Therefore, the difference in sampling values ​​between two adjacent nodes can be directly used as the node interval. On the contrary, if there are multiple differences in sampling values ​​between two adjacent nodes of the discrete point, it means that the intervals between the nodes of the discrete point sequence are uneven. In this case, the discrete point sequence in the rough solution must be refitted, and then resampled according to the same interval between adjacent nodes to generate a discrete point sequence for subsequent path fitting using uniform B-spline curves.

[0038] Preferably, the method of performing path fitting on each discrete point sequence according to the sampling values ​​of the independent variables and dependent variables of N functions stored in each discrete point sequence, characterizing the node interval and the custom B-spline degree between any two adjacent sampling points of the independent variables, and determining the initial control point sequence of the uniform B-spline curve constructed based on the rough solution includes:

[0039] Construct N data sequences A based on the sampled values ​​of the dependent variables of the N functions stored in each discrete point sequence at K sampling points;

[0040] According to the custom B-spline degree P, determine the minimum value I of the number of derivative sequences, where I=P-1, 1≤P≤4;

[0041] Generate N i-order derivative sequences corresponding to the N data sequences A using a differential algorithm, wherein 0≤i≤I, and when i=0, it means that no derivative sequence is generated;

[0042] Taking the first and last points of the N i-th order derivative sequences respectively to generate a data sequence B;

[0043] Determine, according to the custom degree vector correspondence table and the node interval, P P-dimensional row vectors corresponding to the custom B-spline degree P;

[0044] Assign values ​​to a transfer matrix C according to P P-dimensional row vectors, wherein the number of rows of the transfer matrix C is K+2P-2 and the number of columns is P+K-1;

[0045] Assign values ​​to the path matrix D according to the sampling values ​​of the dependent variables of the N functions stored in each discrete point sequence at the K sampling points and the data sequence B, wherein the number of rows of the path matrix D is K+2P-2 and the number of columns is N;

[0046] Based on the assigned transfer matrix C and path matrix D, the linear equation group CX=D is solved, where X is the initial control point matrix to be solved, which has P+K-1 rows and N columns.

[0047] Preferably, the P P-dimensional row vectors corresponding to the custom B-spline degree is P are determined according to the custom degree vector correspondence table and the node interval, wherein the degree vector correspondence table is:

[0048] When P=1, the corresponding 1-dimensional row vector is the first row vector 1*1*{1};

[0049] When P=2, the corresponding two 2-dimensional row vectors are the first row vectors *1*{1,1} and the second row vector * *{-2,2}, where ts is the node interval;

[0050] When P=3, the corresponding three 3D row vectors are the first row vectors. *1*{1,4,1}, the second row vector * *{-3,0,3} and the third row vector ;

[0051] When P=4, the corresponding four 4-dimensional row vectors are the first row vectors. *1*{1,11,11,1}, the second row vector * *{-4,-12,12,4}, the third row vector and the fourth row vector .

[0052] Preferably, assigning values ​​to the transfer matrix C according to P P-dimensional row vectors includes:

[0053] Let the initial values ​​of all elements of the transfer matrix C be 0;

[0054] The loop iterates K times. At the kth time, the first row vector is assigned to the kth row and kth column of the transfer matrix C, 1≤k≤K;

[0055] When P=2, place the second row vector at the K+1 row and 1 column position and the K+2 row and K column position respectively;

[0056] When P=3, the second row vector is placed at the K+1 row and 1 column position and the K+2 row and K column respectively, and the third row vector is placed at the K+3 row and 1 column position and the K+4 row and K column respectively;

[0057] When P=4, the second row vector is placed at the K+1 row and 1 column position and the K+2 row and K column respectively, the third row vector is placed at the K+3 row and 1 column position and the K+4 row and K column respectively; the fourth row vector is placed at the K+5 row and 1 column position and the K+6 row and K column respectively.

[0058] Preferably, assigning values ​​to the path matrix D according to the sampled values ​​of the dependent variables of the N functions stored in each discrete point sequence at the K sampling points and the data sequence B comprises:

[0059] The outer loop traverses N times, and at the nth time, creates the nth K+2P-2-dimensional column vector, where 1≤n≤N;

[0060] The inner loop traverses K times, and assigns the sampled values ​​of the dependent variable of the nth function stored in each discrete point sequence at the K sampling points to the first K elements of the nth K+2P-2 dimensional column vector in sequence; and

[0061] When P=2, the first and last points of the N first-order derivative sequences in the data sequence B are assigned to the last two elements of the corresponding K+2P-2 dimensional column vector;

[0062] When P=3, the first and last points of the N first-order derivative sequences and the second-order derivative sequences in the data sequence B are assigned to the last 4 elements of the corresponding K+2P-2 dimensional column vector;

[0063] When P=4, the first and last points of the N first-order derivative sequences, second-order derivative sequences and third-order derivative sequences in the data sequence B are assigned to the last 6 elements of the corresponding K+2P-2 dimensional column vector.

[0064] In this preferred embodiment, when using a differential algorithm to solve the i-th order derivative sequence of each data sequence, the central difference method is used to calculate the derivatives of points other than the first and last points of the data sequence. For the first and last points, the forward difference and backward difference methods are used for calculation.

[0065] Furthermore, an enable end point constraint flag can be set to indicate the path corresponding to the last discrete point sequence in the rough solution. When the discrete point sequence in the rough solution is not the last segment, the flag is set to false, otherwise, it is set to true. In the case of this flag, when the i-th order derivative sequence of each data sequence is solved by the differential algorithm, the end point of the i-th order derivative sequence of the Nth function is replaced by 0. In this preferred embodiment, when the dependent variables are x, y and θ, the end point of the i-th order derivative sequence corresponding to the function θ=f(s) in the last discrete point sequence is replaced by 0.

[0066] This preferred implementation takes N=3, custom B-spline degree P=3, K=13 as an example, and constructs a transfer matrix B according to the above definition, assigns the first row vector to the corresponding positions of rows 1 to 13, assigns the second row vector to the corresponding positions of rows 14 and 15, and assigns the third row vector to the corresponding positions of rows 16 to 17, and then the assigned transfer matrix C is obtained. At the same time, the values ​​of x, y and θ of the 13 path points are assigned to the first 13 elements of the column vector corresponding to the matrix D, and the first and last points of the first-order derivative sequence of x, y and θ are assigned to the 14th and 15th elements of the column vector corresponding to the matrix D, and the first and last points of the second-order derivative sequence of x, y and θ are assigned to the 16th and 17th elements of the column vector corresponding to the matrix D, thus completing the assignment of matrix D. Then, for a control point sequence of 15 rows and 3 columns, a system of equations of CX=D is constructed, and a mature linear algebra solver is used to obtain a full solution to the control point sequence, and 15 control points can be generated based on the control point sequence.

[0067] Figure 3 This is a schematic diagram of control points obtained according to a preferred embodiment of the present invention. Figure 3 As shown, Figure 2 The discrete point sequence in corresponds to the path points represented by 13 small dots. Figure 3 There are exactly 15 control points in the image, and except for the first and last two large dots, the 13 large dots in the middle are Figure 2 There are overlaps in the 13 small dots in the figure, which means that the control point sequence obtained by path fitting based on the rough solution path points has not deviated.

[0068] In step 104, the initial control point sequence is optimized based on the B-spline property to generate an optimized control point sequence.

[0069] Preferably, the optimizing process of the initial control point sequence based on the B-spline property to generate an optimized control point sequence comprises:

[0070] The initial control point sequence is subjected to at least one of smoothing processing, obstacle avoidance processing, guidance processing, endpoint processing, kinematic processing and key point processing.

[0071] In this preferred embodiment, there is no limitation on the technical means for performing curve smoothing, obstacle avoidance, guidance, endpoint processing, kinematic processing and key point processing.

[0072] In step 105, based on the optimized control point sequence, iterative calculation is performed in the domain length of the independent variable of the uniform B-spline curve according to the user-defined sampling interval to obtain a back-end refined solution generated by the rough solution based on the uniform B-spline curve.

[0073] Preferably, the method of performing iterative calculations in the domain length of the independent variable of the uniform B-spline curve according to a user-defined sampling interval based on the optimized control point sequence to obtain a back-end refined solution generated by the rough solution based on the uniform B-spline curve comprises:

[0074] Step 1: Based on the optimized control point sequence, the node interval ts and the custom B-spline degree P, a P-order uniform B-spline curve is generated, wherein when each discrete point sequence stores the sampling values ​​of the dependent variables of N functions at K sampling points, the number of independent variable nodes of the P-order uniform B-spline curve is K+2P, wherein:

[0075] For the node sequence u_seq consisting of K+2P independent variable nodes,

[0076] When j≤P, u_seq[j] = (-p +j) *ts

[0077] When P<j≤K+P-1, u_seq[j] = u_seq[j-1]+ts

[0078] When j>K+P-1, u_seq[j] = u_seq[j-1]+ts

[0079] Step 2: When the domain interval of the node sequence u_seq is u_seq[P] to u_seq[K+P-1], let its length be duration, then duration= u_seq[K+P-1]- u_seq[P];

[0080] Step 3: Let the custom sampling interval be Δts, and calculate the position W of the mth sampling point on the domain length on the node sequence u_seq. The calculation formula is:

[0081] W=m*Δts+ u_seq[P]

[0082] Where, 0≤m≤M, M is the total number of sampling points obtained by traversing the length of the definition domain according to the custom sampling interval Δts, and the initial value of m is 0;

[0083] Step 4: Clamp W. The expression is:

[0084] W=clamp(W, u_seq[P],u_seq[K+P-1])

[0085] Step 5, traverse the node sequence u_seq, and find the node interval u_seq[v] to u_seq[v+1] containing W;

[0086] Step 6: Select P+1 control points related to v in the optimized control point sequence, that is, store the vPth to vth control points in the container R;

[0087] Step 7, perform two-layer traversal update on the container R, wherein the outer loop traversal r increases from 1 to P, r represents the current outer loop layer number, the inner loop traversal z decreases from P to r, and calculate alpha = (W - u_seq[z+ v - p]) / (u_seq[z + 1 + v - r]- u_seq[z+ v - p]) and R[z] = (1 - alpha) * R [z - 1] + alpha * R[z];

[0088] Step 7: Output R[P] as the optimized path point corresponding to position X, and set m=m+1;

[0089] Step 8: When m≤M, return to step 3; when m>M, the iterative calculation ends, and a back-end refined solution generated by the rough solution based on the uniform B-spline curve is obtained according to the optimized path points.

[0090] In this preferred embodiment, according to Figure 1 When the 13 path points in the rough solution shown above determine that the corresponding uniform B-spline curve has 15 control points, according to the properties of B-spline, the uniform B-spline curve composed of 15 control points is divided into K-P+2=12 segments, which is equivalent to each segment of the curve being associated with P+1 control points, that is, 4 control points, and the number of nodes in the node sequence of the independent variable of the uniform B-spline curve should be K+2P=19, which is equivalent to u_seq[0] to u_seq

[18] dividing the node sequence into 18 segments. Since the segmentation of the node sequence corresponds to the segmentation of the curve, by setting a denser sampling interval in the node sequence than in the rough solution to traverse the sampling points, and then finding the control points associated with the sampling points, the corresponding points on the curve can be obtained, thereby restoring a denser path point. Figure 4 Schematic diagram of the back-end analysis according to the preferred embodiment of the present invention. Figure 4 As shown, in Figure 3 Based on the 15 control points represented by the large dots, a smaller sampling interval was set to traverse the sampling points, thus obtaining the same Figure 2 In comparison, the path points represented by denser small dots are equivalent to providing better quality original initial values ​​for back-end optimization, effectively reducing the burden of back-end optimization.

[0091] The path fitting method based on uniform B-splines described in this preferred embodiment first fits the front-end rough solution based on the relevant properties of uniform B-splines before the back-end optimization. According to the obtained control point sequence, not only can the path be easily smoothed and obstacle avoided, but also a better quality original initial value is provided for the back-end optimization, reducing the back-end optimization burden.

[0092] Exemplary Systems

[0093] Figure 5 FIG. 1 is a schematic diagram of a path fitting system based on uniform B-spline according to a preferred embodiment of the present invention. Figure 5 As shown, the path fitting system 500 based on uniform B-spline described in this preferred embodiment includes:

[0094] The data acquisition module 501 is used to obtain a rough solution of the front-end planned path, wherein the rough solution is a sequence of several discrete points, each discrete point corresponds to a path point, and the basic information stored in each path point includes N dimensions representing the path point, accumulated mileage and accumulated time;

[0095] A function construction module 502 is used to construct N functions that use uniform B-spline curves to characterize the relationship between basic information of waypoints, wherein the independent variables of the N functions are accumulated mileage or accumulated time, and the dependent variables are N dimensions of the waypoints;

[0096] The first sequence module 503 is used to perform path fitting on each discrete point sequence according to the sampling values ​​of the independent variables and dependent variables of the N functions stored in each discrete point sequence, characterize the node interval between any two adjacent sampling points of the independent variables and the custom B-spline degree, and determine the initial control point sequence of the uniform B-spline curve constructed based on the rough solution;

[0097] The second sequence module 504 is used to optimize the initial control point sequence based on the B-spline property to generate an optimized control point sequence;

[0098] The optimization path module 505 is used to perform iterative calculations in the domain length of the independent variable of the uniform B-spline curve according to the user-defined sampling interval based on the optimization control point sequence to obtain a back-end refined solution generated by the rough solution based on the uniform B-spline curve.

[0099] Preferably, the system further comprises a node spacing module for

[0100] When the difference between the sampling values ​​of any two adjacent sampling points of the independent variable is not equal, several discrete point sequences are fitted, and the fitting results are resampled according to the difference between the sampling values ​​of two adjacent sampling points to generate several discrete point sequences for path fitting.

[0101] Preferably, the first sequence module 504 performs path fitting on each discrete point sequence according to the sampling values ​​of the independent variables and dependent variables of the N functions stored in each discrete point sequence, the node interval and the custom B-spline order, and determines the initial control point sequence of the uniform B-spline curve constructed based on the rough solution, including:

[0102] Construct N data sequences A based on the sampled values ​​of the dependent variables of the N functions stored in each discrete point sequence at K sampling points;

[0103] According to the custom B-spline degree P, determine the minimum value I of the number of derivative sequences, where I=P-1, 1≤P≤4;

[0104] Generate N i-order derivative sequences corresponding to the N data sequences A using a differential algorithm, wherein 0≤i≤I, and when i=0, it means that no derivative sequence is generated;

[0105] Taking the first and last points of the N i-th order derivative sequences respectively to generate a data sequence B;

[0106] Determine, according to the custom degree vector correspondence table and the node interval, P P-dimensional row vectors corresponding to the custom B-spline degree P;

[0107] Assign values ​​to a transfer matrix C according to P P-dimensional row vectors, wherein the number of rows of the transfer matrix C is K+2P-2 and the number of columns is P+K-1;

[0108] Assign values ​​to the path matrix D according to the sampling values ​​of the dependent variables of the N functions stored in each discrete point sequence at the K sampling points and the data sequence B, wherein the number of rows of the path matrix D is K+2P-2 and the number of columns is N;

[0109] Based on the assigned transfer matrix C and path matrix D, the linear equation group CX=D is solved, where X is the initial control point matrix to be solved, which has P+K-1 rows and N columns.

[0110] Preferably, the first sequence module 504 determines P P-dimensional row vectors corresponding to the custom B-spline degree P according to the custom degree vector correspondence table and the node interval, wherein the degree vector correspondence table is:

[0111] When P=1, the corresponding 1-dimensional row vector is the first row vector 1*1*{1};

[0112] When P=2, the corresponding two 2-dimensional row vectors are the first row vectors *1*{1,1} and the second row vector * *{-2,2}, where ts is the node interval;

[0113] When P=3, the corresponding three 3D row vectors are the first row vectors. *1*{1,4,1}, the second row vector * *

[0114] {-3,0,3} and the third row vector ;

[0115] When P=4, the corresponding four 4-dimensional row vectors are the first row vectors. *1*{1,11,11,1}, the second row vector * *{-4,-12,12,4}, the third row vector and the fourth row vector .

[0116] Preferably, the first sequence module 504 assigns values ​​to the transfer matrix C according to P P-dimensional row vectors, including:

[0117] Let the initial values ​​of all elements of the transfer matrix C be 0;

[0118] The loop iterates K times. At the kth time, the first row vector is assigned to the kth row and kth column of the transfer matrix C, 1≤k≤K;

[0119] When P=2, place the second row vector at the K+1 row and 1 column position and the K+2 row and K column position respectively;

[0120] When P=3, the second row vector is placed at the K+1 row and 1 column position and the K+2 row and K column respectively, and the third row vector is placed at the K+3 row and 1 column position and the K+4 row and K column respectively;

[0121] When P=4, the second row vector is placed at the K+1 row and 1 column position and the K+2 row and K column respectively, the third row vector is placed at the K+3 row and 1 column position and the K+4 row and K column respectively; the fourth row vector is placed at the K+5 row and 1 column position and the K+6 row and K column respectively.

[0122] Preferably, the first sequence module 504 assigns values ​​to the path matrix D according to the sampled values ​​of the dependent variables of the N functions stored in each discrete point sequence at the K sampling points and the data sequence B, including:

[0123] The outer loop traverses N times, and at the nth time, creates the nth K+2P-2-dimensional column vector, where 1≤n≤N;

[0124] The inner loop traverses K times, and assigns the sampling values ​​of the dependent variable of the nth function stored in each discrete point sequence at K sampling points to the first K elements of the nth K+2P-2 dimensional column vector in sequence;

[0125] When P=2, the first and last points of the N first-order derivative sequences in the data sequence B are assigned to the last two elements of the corresponding K+2P-2 dimensional column vector;

[0126] When P=3, the first and last points of the N first-order derivative sequences and the second-order derivative sequences in the data sequence B are assigned to the last 4 elements of the corresponding K+2P-2 dimensional column vector;

[0127] When P=4, the first and last points of the N first-order derivative sequences, second-order derivative sequences and third-order derivative sequences in the data sequence B are assigned to the last 6 elements of the corresponding K+2P-2 dimensional column vector.

[0128] Preferably, the second sequence module 505 optimizes the initial control point sequence based on the B-spline property to generate an optimized control point sequence, including:

[0129] The initial control point sequence is subjected to at least one of smoothing processing, obstacle avoidance processing, guidance processing, endpoint processing, kinematic processing and key point processing.

[0130] Preferably, the optimization path module 506 performs iterative calculations in the domain length of the independent variable of the uniform B-spline curve according to a custom sampling interval based on the optimization control point sequence to obtain a back-end refined solution generated by the rough solution based on the uniform B-spline curve, including:

[0131] Step 1: Based on the optimized control point sequence, the node interval ts and the custom B-spline degree P, a P-order uniform B-spline curve is generated, wherein when each discrete point sequence stores the sampling values ​​of the dependent variables of N functions at K sampling points, the number of independent variable nodes of the P-order uniform B-spline curve is K+2P, wherein:

[0132] For the node sequence u_seq consisting of K+2P independent variable nodes,

[0133] When j≤P, u_seq[j] = (-p +j) *ts

[0134] When P<j≤K+P-1, u_seq[j] = u_seq[j-1]+ts

[0135] When K+P-1<j≤K+2P, u_seq[j] = u_seq[j-1]+ts

[0136] Step 2: When the domain interval of the node sequence u_seq is u_seq[P] to u_seq[K+P-1], let its length be duration, then duration= u_seq[K+P-1]- u_seq[P];

[0137] Step 3: Let the custom sampling interval be Δts, and calculate the position W of the mth sampling point on the domain length on the node sequence u_seq. The calculation formula is:

[0138] W=m*Δts+ u_seq[P]

[0139] Where, 0≤m≤M, M is the total number of sampling points obtained by traversing the length of the definition domain according to the custom sampling interval Δts, and the initial value of m is 0;

[0140] Step 4: Clamp W. The expression is:

[0141] W=clamp(W, u_seq[P],u_seq[K+P-1])

[0142] Step 5, traverse the node sequence u_seq, and find the node interval u_seq[v] to u_seq[v+1] containing W;

[0143] Step 6: Select P+1 control points related to v in the optimized control point sequence, that is, store the vPth to vth control points in the container R;

[0144] Step 7, perform two-layer traversal update on the container R, wherein the outer loop traversal r increases from 1 to P, r represents the current outer loop layer number, the inner loop traversal z decreases from P to r, and calculate alpha = (W - u_seq[z+ v - p]) / (u_seq[z + 1 + v - r]- u_seq[z+ v - p]) and R[z] = (1 - alpha) * R [z - 1] + alpha * R[z];

[0145] Step 7: Output R[P] as the optimized path point corresponding to position X, and set m=m+1;

[0146] Step 8: When m≤M, return to step 3; when m>M, the iterative calculation ends, and a back-end refined solution generated by the rough solution based on the uniform B-spline curve is obtained according to the optimized path points.

[0147] The path fitting system based on uniform B-splines described in this preferred embodiment and the path fitting method based on uniform B-splines perform path fitting based on uniform B-splines on the rough solution of the acquired front-end planned path to obtain the back-end refined solution in the same steps, and the technical effects achieved are also the same, which will not be repeated here.

[0148] Exemplary Electronic Devices

[0149] Figure 6 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 6 FIG. 1 is a block diagram of an electronic device according to an embodiment of the present disclosure. Figure 6 As shown, the electronic device includes one or more processors 601 and a memory 602 .

[0150] The processor 601 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.

[0151] The memory 602 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 601 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 603 and an output device 604, which are interconnected via a bus system and / or other forms of connection mechanisms (not shown).

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

[0153] The output device 604 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.

[0154] Of course, to simplify, Figure 6 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.

[0155] Exemplary computer program products and computer-readable storage media

[0156] 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 uniform B-spline-based path fitting method according to various embodiments of the present disclosure described in the above-mentioned "Exemplary Method" section of this specification.

[0157] 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.

[0158] 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, causes the processor to execute the steps of the path fitting method based on uniform B-splines according to various embodiments of the present disclosure described in the above “Exemplary Method” section of this specification.

[0159] 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.

[0160] 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.

[0161] 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.

[0162] 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.

[0163] 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.

[0164] 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.

[0165] 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 path fitting method based on uniform B-spline, characterized in that: The method comprises: Obtain a rough solution of the front-end planned path, wherein the rough solution is a sequence of several discrete points, each discrete point corresponds to a path point, and the basic information stored in each path point includes N dimensions representing the path point, accumulated mileage, and accumulated time; Constructing N functions that use uniform B-spline curves to characterize the relationship between basic information of waypoints, wherein the independent variables of the N functions are accumulated mileage or accumulated time, and the dependent variables are N dimensions of the waypoints; According to the sampling values ​​of the independent variables and dependent variables of the N functions stored in each discrete point sequence, the node interval between any two adjacent sampling points representing the independent variables, and the custom B-spline order, path fitting is performed on each discrete point sequence to determine the initial control point sequence of the uniform B-spline curve constructed based on the rough solution, including: Construct N data sequences A based on the sampled values ​​of the dependent variables of the N functions stored in each discrete point sequence at K sampling points; According to the custom B-spline degree P, determine the minimum value I of the number of derivative sequences, where I=P-1, 1≤P≤4; Generate N i-order derivative sequences corresponding to the N data sequences A using a differential algorithm, wherein 0≤i≤I, and when i=0, it means that no derivative sequence is generated; Taking the first and last points of the N i-th order derivative sequences respectively to generate a data sequence B; Determine, according to the custom degree vector correspondence table and the node interval, P P-dimensional row vectors corresponding to the custom B-spline degree P; Assign values ​​to a transfer matrix C according to P P-dimensional row vectors, wherein the number of rows of the transfer matrix C is K+2P-2 and the number of columns is P+K-1; Assign values ​​to the path matrix D according to the sampling values ​​of the dependent variables of the N functions stored in each discrete point sequence at the K sampling points and the data sequence B, wherein the number of rows of the path matrix D is K+2P-2 and the number of columns is N; Based on the assigned transfer matrix C and path matrix D, solve the linear equation system CX=D, where X is the initial control point matrix to be solved, which has P+K-1 rows and N columns; Optimizing the initial control point sequence based on the B-spline property to generate an optimized control point sequence; Based on the optimized control point sequence, iterative calculation is performed in the definition domain length of the independent variable of the uniform B-spline curve according to the user-defined sampling interval to obtain a back-end refined solution generated by the rough solution based on the uniform B-spline curve.

2. The method according to claim 1, characterized in that The method of fitting each discrete point sequence according to the sampling values ​​of the independent variables and dependent variables of the N functions stored in each discrete point sequence, the node interval between any two adjacent sampling points representing the independent variables, and the custom B-spline degree, and determining the initial control point sequence of the uniform B-spline curve constructed based on the rough solution before the discrete point sequence is also determined, includes: When the difference between the sampling values ​​of any two adjacent sampling points of the independent variable is not equal, several discrete point sequences are fitted, and the fitting results are resampled according to the difference between the sampling values ​​of two adjacent sampling points to generate several discrete point sequences for path fitting.

3. The method according to claim 1, characterized in that The P P-dimensional row vectors corresponding to the custom B-spline degree is determined according to the custom degree vector correspondence table and the node interval, wherein the degree vector correspondence table is: When P=1, the corresponding 1-dimensional row vector is the first row vector 1*1*{1}; When P=2, the corresponding two 2-dimensional row vectors are the first row vectors *1*{1,1} and the second row vector * *{-2,2}, where ts is the node interval; When P=3, the corresponding three 3D row vectors are the first row vectors. *1*{1,4,1}, the second row vector * *{-3,0,3} and the third row vector ; When P=4, the corresponding four 4-dimensional row vectors are the first row vectors. *1*{1,11,11,1}, the second row vector * *{-4,-12,12,4}, the third row vector and the fourth row vector .

4. The method according to claim 3, characterized in that The assigning values ​​to the transfer matrix C according to P P-dimensional row vectors includes: Let the initial values ​​of all elements of the transfer matrix C be 0; The loop iterates K times. At the kth time, the first row vector is assigned to the kth row and kth column of the transfer matrix C, 1≤k≤K; When P=2, place the second row vector at the K+1 row and 1 column position and the K+2 row and K column position respectively; When P=3, the second row vector is placed at the K+1 row and 1 column position and the K+2 row and K column respectively, and the third row vector is placed at the K+3 row and 1 column position and the K+4 row and K column respectively; When P=4, the second row vector is placed at the K+1 row and 1 column position and the K+2 row and K column respectively, the third row vector is placed at the K+3 row and 1 column position and the K+4 row and K column respectively; the fourth row vector is placed at the K+5 row and 1 column position and the K+6 row and K column respectively.

5. The method according to claim 1, characterized in that The assigning of values ​​to the path matrix D according to the sampling values ​​of the dependent variables of the N functions stored in each discrete point sequence at the K sampling points and the data sequence B comprises: The outer loop traverses N times, and at the nth time, creates the nth K+2P-2-dimensional column vector, where 1≤n≤N; The inner loop traverses K times, and assigns the sampling values ​​of the dependent variable of the nth function stored in each discrete point sequence at K sampling points to the first K elements of the nth K+2P-2 dimensional column vector in sequence; When P=2, the first and last points of the N first-order derivative sequences in the data sequence B are assigned to the last two elements of the corresponding K+2P-2 dimensional column vector; When P=3, the first and last points of the N first-order derivative sequences and the second-order derivative sequences in the data sequence B are assigned to the last 4 elements of the corresponding K+2P-2 dimensional column vector; When P=4, the first and last points of the N first-order derivative sequences, second-order derivative sequences and third-order derivative sequences in the data sequence B are assigned to the last 6 elements of the corresponding K+2P-2 dimensional column vector.

6. The method according to claim 1, characterized in that The step of optimizing the initial control point sequence based on the B-spline property to generate an optimized control point sequence includes: The initial control point sequence is subjected to at least one of smoothing processing, obstacle avoidance processing, guidance processing, endpoint processing, kinematic processing and key point processing.

7. The method according to claim 1, characterized in that The method of performing iterative calculations in the domain length of the independent variable of the uniform B-spline curve according to a user-defined sampling interval based on the optimized control point sequence to obtain a back-end refined solution generated by the rough solution based on the uniform B-spline curve comprises: Step 1: Based on the optimized control point sequence, the node interval ts and the custom B-spline degree P, a P-order uniform B-spline curve is generated, wherein when each discrete point sequence stores the sampling values ​​of the dependent variables of N functions at K sampling points, the number of independent variable nodes of the P-order uniform B-spline curve is K+2P, wherein: For the node sequence u_seq consisting of K+2P independent variable nodes, When j≤P, u_seq[j] = (-p +j) *ts When P<j≤K+P-1, u_seq[j] = u_seq[j-1]+ts When K+P-1<j≤K+2P, u_seq[j] = u_seq[j-1]+ts Step 2: When the domain interval of the node sequence u_seq is u_seq[P] to u_seq[K+P-1], let its length be duration, then duration= u_seq[K+P-1]- u_seq[P]; Step 3: Let the custom sampling interval be Δts, and calculate the position W of the mth sampling point on the domain length on the node sequence u_seq. The calculation formula is: W=m*Δts+ u_seq[P] Where, 0≤m≤M, M is the total number of sampling points obtained by traversing the length of the definition domain according to the custom sampling interval Δts, and the initial value of m is 0; Step 4: Clamp W. The expression is: W=clamp(W, u_seq[P], u_seq[K+P-1]) Step 5, traverse the node sequence u_seq, and find the node interval u_seq[v] to u_seq[v+1] containing W; Step 6: Select P+1 control points related to v in the optimized control point sequence, that is, store the vPth to vth control points in the container R; Step 7, perform two-layer traversal update on the container R, wherein the outer loop traversal r increases from 1 to P, r represents the current outer loop layer number, the inner loop traversal z decreases from P to r, and calculate alpha = (W - u_seq[z+ v - p]) / (u_seq[z + 1 + v - r] - u_seq[z+ v - p]) and R[z] = (1 - alpha) * R [z - 1] +alpha * R[z]; Step 7: Output R[P] as the optimized path point corresponding to position X, and set m=m+1; Step 8: When m≤M, return to step 3; when m>M, the iterative calculation ends, and a back-end refined solution generated by the rough solution based on the uniform B-spline curve is obtained according to the optimized path points.

8. A path fitting system based on uniform B-spline, characterized in that: The system comprises: A data acquisition module is used to obtain a rough solution of the front-end planned path, wherein the rough solution is a sequence of several discrete points, each discrete point corresponds to a path point, and the basic information stored in each path point includes N dimensions representing the path point, accumulated mileage and accumulated time; A function construction module, used to construct N functions that use uniform B-spline curves to characterize the relationship between basic information of waypoints, wherein the independent variables of the N functions are accumulated mileage or accumulated time, and the dependent variables are N dimensions of the waypoints; The first sequence module is used to perform path fitting on each discrete point sequence according to the sampling values ​​of the independent variables and dependent variables of the N functions stored in each discrete point sequence, the node interval between any two adjacent sampling points representing the independent variables, and the custom B-spline order, and determine the initial control point sequence of the uniform B-spline curve constructed based on the rough solution, including: Construct N data sequences A based on the sampled values ​​of the dependent variables of the N functions stored in each discrete point sequence at K sampling points; According to the custom B-spline degree P, determine the minimum value I of the number of derivative sequences, where I=P-1, 1≤P≤4; Generate N i-order derivative sequences corresponding to the N data sequences A using a differential algorithm, wherein 0≤i≤I, and when i=0, it means that no derivative sequence is generated; Taking the first and last points of the N i-th order derivative sequences respectively to generate a data sequence B; Determine, according to the custom degree vector correspondence table and the node interval, P P-dimensional row vectors corresponding to the custom B-spline degree P; Assign values ​​to a transfer matrix C according to P P-dimensional row vectors, wherein the number of rows of the transfer matrix C is K+2P-2 and the number of columns is P+K-1; Assign values ​​to the path matrix D according to the sampling values ​​of the dependent variables of the N functions stored in each discrete point sequence at the K sampling points and the data sequence B, wherein the number of rows of the path matrix D is K+2P-2 and the number of columns is N; Based on the assigned transfer matrix C and path matrix D, solve the linear equation system CX=D, where X is the initial control point matrix to be solved, which has P+K-1 rows and N columns; A second sequence module is used to optimize the initial control point sequence based on the B-spline property to generate an optimized control point sequence; The optimization path module is used to perform iterative calculations in the domain length of the independent variable of the uniform B-spline curve according to the user-defined sampling interval based on the optimization control point sequence to obtain a back-end refined solution generated by the rough solution based on the uniform B-spline curve.

9. 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 7.

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

Images