End path fairing method and device based on Lie group manifold

Abstract

The invention discloses a Lie group manifold-based tail end path fairing method, which comprises the following steps of: resolving a start-stop path position pose entering and leaving a corner fairing area through a numerical method under the constraint of the fairing area, and mapping and transforming a corresponding tail end pose and a tail end pose path to a Lie group manifold space; the method comprises the following steps: establishing a Lie group manifold, solving 0-2 order parameter continuity conditions at starting and ending points, further introducing slack variables to slack the slack variables into geometric continuity conditions on the Lie group manifold, constructing a Bezier curve connecting the starting and ending points on the Lie group manifold based on the slack variables, and evaluating the optimality of the curve as a fairing path according to a defined objective function. Therefore, the optimal slack variable value can be solved through nonlinear optimization, the optimal slack variable value is substituted to generate an optimal Bezier curve fairing path, and finally, the optimal slack variable value is inversely mapped into the position attitude representation of the tail end to obtain a final fairing path. According to the method, under the constraint of geometric continuity, the corner fairing transition path is optimized by combining a construction method of a Bezier curve on the Lie group manifold.

Description

Technical Field

[0001] The present invention relates to a method and device for smoothing an end path, and particularly to a method and device for smoothing an end path based on a Lie group manifold. Background Art

[0002] At present, with the advancement of industrial automation and intelligent manufacturing, industrial robots and high-end five-axis machine tools, as core equipment, have become the key to improving production efficiency and high-precision manufacturing quality with their flexible, high-speed, and high-precision processing methods. In particular, breakthroughs in high-value-added strategic fields such as aerospace and automotive contribute to the improvement of the international competitiveness of the manufacturing industry under the background of the transformation of Industry 4.0. In the complex operation tasks and environments of intelligent manufacturing, a smooth, stable end trajectory is the core requirement for achieving high-precision, high-quality processing and operation. It directly determines the processing accuracy, equipment reliability, and production efficiency, and is the core technical challenge for achieving high quality, high flexibility, and high response ability. The end trajectory planning problem is the core of robot motion control, which is mainly divided into three parts: path smoothing, optimal velocity-time law planning, and interpolation instruction generation. As the front-end input of motion control, if the end key path points generated by the CAM algorithm, graph search algorithm, or error sampling method are directly used for velocity-time law planning, it will result in a non-smooth trajectory, severe velocity fluctuations, and affect the smoothness and safety during processing. Therefore, in order to ensure the high-quality trajectory planning effect, the corner path smoothing operation before velocity-time law planning is a necessary step to ensure natural and smooth motion, and it is also a research hotspot in the field currently.

[0003] In the related technologies of end path smoothing, the core task is still to construct a smooth transition and smoothing path under the control of continuity and corner deviation constraints to meet the optimality under various evaluation methods. Currently, related research is mainly divided into three categories: global smoothing, local smoothing, and sliding window smoothing according to the processing range of discrete paths.

[0004] Among them, global smoothing aims to construct all path points into a smooth path curve. By interpolation or fitting, curves such as Akima, NURBS, or PH are selected as curve models. Under constraints such as chord error, data point error, or Hausdorff distance, optimization methods such as feature point selection method, least square method, or genetic algorithm are used to smooth the global path points. When processing attitude paths, this type of method mainly uses spatial transformation methods such as hyperbola, Euler angle space, or joint task space, and uses the same path smoothing method in the transformation space, and maps the angle and error constraints simultaneously to achieve the control of smoothing error. The end pose path smoothed by the global smoothing method has the advantages of good data compression, high smoothness, and parameter continuity. However, it requires iterative solution for all path points, resulting in high computational complexity, poor stability, and difficult error control.

[0005] Local fairing aims to smoothly splice adjacent paths using parametric curves at the corners, and select classic curves such as arcs, NURBS, PH, or Clothoid as models through internal or external corner transitions, and symmetric or asymmetric transition paths. Under the same error constraints as global fairing and continuity constraints at the splicing points with the original paths, construction methods such as the control point assignment method or the error approximation method are used to generate smooth transition curves for local adjacent paths. When dealing with attitude paths, such methods also use the same spatial transformation method to map the attitude to the extended space and adopt similar local fairing methods for the attitude paths in the extended space. Thus, alternately combining the original path and the transition path constitutes the overall end pose path smoothed by the local fairing method. This method has the advantages of high computational efficiency, low algorithm complexity, and easy error control. However, due to the mixed expression of each curve segment, problems such as curve parameter fluctuations and large data storage amounts are caused.

[0006] As a harmonic method, the sliding window fairing method mainly constructs a smooth curve from all path points within the sliding window and completes the smoothing of the overall path by sliding the window. However, related research has all been implemented based on the preview speed time law within the sliding window and is not sufficiently relevant to the end path fairing. Additionally, there is an ordered click fairing method in related research that treats the end path points as point cloud data and smooths them. The processing methods include wavelet transform, convolutional neural network, angle filtering, curvature energy correction, etc. However, the maturity of such methods is low, there is little related research, and the application is not widespread.

[0007] Although the above-mentioned related technologies have achieved the fairness of the end position path and ensured the smoothness of the movement along the position curve, they all use the method of non-linearly mapping to the extended space to handle the change of attitude. This makes the attitude curve in the extended space fair, but the movement of non-linearly mapping back to the end attitude path is not guaranteed to be smooth. Additionally, the above-mentioned related technologies all use the method of separately fairing the position and attitude paths and then coordinating and integrating them, which causes fluctuations in the end pose movement due to incoordination. Summary of the Invention

[0008] The present invention provides an end path fairing method and device based on Lie group manifolds to solve the technical problems existing in the known technology.

[0009] The technical solution adopted by the present invention to solve the technical problems existing in the known technology is:

[0010] An end path fairing method based on Lie group manifolds, the method comprising the following steps:

[0011] Step 1: Obtain the discrete points of the initial end - pose path in the Euclidean space, and set the path smoothing region constraint and the path smoothing objective function;

[0012] Step 2: Connect the discrete points of the initial end - pose path in the Euclidean space in sequence to form an initial end - pose path curve, and the initial end - pose path curve is divided into several path segments by the discrete points;

[0013] Step 3: According to the path smoothing region constraint, for each path discrete point located at a corner, set the start and end points of the path part to be smoothed on its adjacent two path segments; the path part to be smoothed is called the to - be - smoothed path segment; solve the end - poses of the start and end points of each to - be - smoothed path segment;

[0014] Step 4: Map the initial end - pose path curve in the Euclidean space and the end - poses of the start and end points of each to - be - smoothed path segment to the Lie group manifold space to obtain the end - pose path parameter curve on the Lie group manifold and the Lie group elements of the end - poses corresponding to the start and end points of each to - be - smoothed path segment;

[0015] Step 5: Solve the 0 - 2 order parameter continuity conditions at the start and end points of a certain to - be - smoothed path segment in the Lie group manifold space;

[0016] Step 6: By introducing slack variables, relax the parameter continuity condition to the geometric continuity condition corresponding to the Lie group manifold;

[0017] Step 7: Under the constraint of satisfying the geometric continuity condition, construct a Bezier curve connecting the start and end points of the to - be - smoothed path segment in the Lie group manifold space;

[0018] Step 8: Based on the path smoothing objective function, evaluate whether the Bezier curve makes the path smoothing objective function reach the optimal value; if it does not reach the optimal value, optimize and adjust the slack variables, and then turn to Step 6; if it reaches the optimal value, select the Bezier curve as the smoothed path between the start and end points of the to - be - smoothed path segment; then proceed to Step 9;

[0019] Step 9: Smooth the remaining to - be - smoothed path segments one by one according to the methods of Steps 5 to 8 to obtain a complete smoothed end - pose path curve; transform the complete smoothed end - pose path curve to the Euclidean space to obtain the final smoothed end - pose path.

[0020] Furthermore, in Step 1, set the path smoothing region constraint as: the points on the smoothed end - path are located within a sphere centered at the path discrete point at the corner within the to - be - smoothed path segment, and let the radius of the sphere be ε; minimize one parameter or a linear combination of several parameters among the four curve parameters of curve curvature, curve torsion, integral of the modulus of each order differential of the curve, and curve length as the objective function.

[0021] Further, in step 3, according to the path fairing region constraint, the end poses of the start and end points of each path segment to be faired are solved by the following method:

[0022] According to the adjacent end pose path discrete points and the corresponding motion modes, construct a path segment connecting two adjacent path discrete points in the Cartesian space;

[0023] Construct a non-linear equation with the distance between the points on the path segments on both sides of the path discrete point and the path discrete point being the fairing constraint, and find the roots by numerical solution to determine the poses of the start and end points of the path segments to be faired on both sides of the path discrete point.

[0024] Further, in step 4, represent the initial end pose path discrete points and the end poses of the start and end points of each path segment to be faired in the Euclidean space by direction vectors and / or Euler angles;

[0025] Represent the end pose path parameter curve in the Lie group manifold space and the Lie group elements of the end poses of the start and end points of each path segment to be faired by rotation matrices, quaternions, homogeneous matrices, and / or dual quaternions.

[0026] Further, step 5 includes the following sub-steps:

[0027] Step 5-1, according to each path segment to be faired in the Lie group manifold space and the end poses of its start and end points, construct a vector tangent space at the start and end points;

[0028] Step 5-2, based on the vector tangent space, calculate the 0-2 order differentials of the end pose path parameter curve on the corresponding Lie group manifold at the start and end points of each path segment to be faired, represented by the corresponding Lie group or Lie algebra elements;

[0029] Step 5-3, organize and summarize the 0-2 order differential values at the start and end points into a set representing the parameter continuity conditions.

[0030] Further, step 6 includes the following sub-steps:

[0031] Step 6-1, according to the principle of invariant Cartesian space points, tangent vectors, and curvatures, construct a relaxation rule for converting the 0-2 order parameter continuity conditions in the Lie group manifold space into relaxed geometric continuity conditions;

[0032] Step 6-2, based on this relaxation rule, convert the Lie group elements, tangent vectors, and pseudo-curvatures in the Lie group manifold space into relaxed geometric continuity conditions, and determine the set of geometric continuity conditions at the start and end points of each path segment to be faired on the Lie group manifold according to this rule and the relaxation variables.

[0033] Further, let the i-th path point be a corner point, d iDenote the \(i\)-th path point, where \(i = 2, 3, \ldots, z - 1\); \(i\) represents the path point sequence number; \(z\) is the number of path points;

[0034] In the Lie group manifold space, the following relaxation rules are established under the conditions of Lie group elements, tangent vectors, and pseudo-curvature invariance:

[0035]

[0036] According to this relaxation principle, the corresponding geometric continuity conditions are constructed as follows:

[0037]

[0038]

[0039] Taking \(\alpha\) i,s , \(\beta\) i,s , \(\alpha\) i,e , \(\beta\) i,e as independent variables, the following relational expressions are further obtained:

[0040]

[0041] In the formula:

[0042] \(H\) C0 , \(H\) C1 , \(H\) C2 respectively represent the 0th, 1st, and 2nd order parameter continuity condition vectors of the end pose path parameter curve;

[0043] corresponds to the unit vector of the 1st order parameter continuity condition vector of the end pose path parameter curve;

[0044] \((H\) C1 \times H\) C2 ) ∧ represents the unit vector of the cross product of the 1st and 2nd order parameter continuity condition vectors of the end pose path parameter curve;

[0045] \(\kappa\) represents the pseudo-curvature of the end pose path parameter curve;

[0046] \(\alpha\), \(\beta\) represent the undetermined relaxation variable parameters of the end pose path parameter curve;

[0047] \(H\) G0 , \(H\) G1 , \(H\) G2 respectively represent the 0th, 1st, and 2nd order geometric continuity condition vectors of the end pose path;

[0048] respectively correspond to the corner point \(d\) of the end pose path parameter curve iThe 0th, 1st, and 2nd order parametric continuity condition vectors of the starting point of the path segment to be fairing at

[0049] Denote the corner point d of the end - pose path parameter curve i The unit vector of the 1st order parametric continuity condition vector of the starting point of the path segment to be fairing at

[0050] Denote the corner point d of the end - pose path parameter curve i The unit vector of the cross - product of the 1st and 2nd order parametric continuity condition vectors of the starting point of the path segment to be fairing at

[0051] κ i,s Denote the corner point d of the end - pose path parameter curve i The pseudo - curvature of the starting point of the path segment to be fairing at

[0052] α i,s 、β i,s Denote the corner point d of the end - pose path parameter curve i The undetermined relaxation variable parameter of the starting point of the path segment to be fairing at

[0053] Successively correspond to denote the 0th, 1st, and 2nd order geometric continuity condition vectors of the starting point of the path segment to be fairing at the corner point d of the end - pose path i

[0054] Successively correspond to denote the 0th, 1st, and 2nd order parametric continuity condition vectors of the starting point of the path segment to be fairing at the corner point d of the end - pose path parameter curve i

[0055] Denote the corner point d of the end - pose path parameter curve i The unit vector of the 1st order parametric continuity condition vector of the starting point of the path segment to be fairing at

[0056] Denote the corner point d of the end - pose path parameter curve i The unit vector of the cross - product of the 1st and 2nd order parametric continuity condition vectors of the starting point of the path segment to be fairing at

[0057] κ i,e Denote the corner point d of the end - pose path parameter curve i The pseudo - curvature of the starting point of the path segment to be fairing at

[0058] α i,e 、β i,e Denote the corner point d of the end - pose path parameter curve i ​​The undetermined relaxation variable parameter at the starting point of the path segment to be fairing;

[0059] Successively corresponding to represent the corner point d of the end pose path i The 0th, 1st, and 2nd order geometric continuity condition vectors at the starting point of the path segment to be fairing;

[0060] Indicating the geometric continuity condition function of the end pose path at the corner point d i Geometric continuity condition function at the place;

[0061] X i Indicating the corner point d i The relaxation variable composed of the undetermined relaxation variable parameters.

[0062] Furthermore, in step 7, set the optimization objective as the path fairing objective function; set the constraint as the empirical limit range of the relaxation variable; optimize and select the relaxation variable on the Lie group manifold according to one of the following methods:

[0063] Method 1: Derive the calculation formula of the objective function convenient for computer operation through the analytical method, substitute multiple groups of relaxation variables to calculate the objective function, and obtain a group of relaxation variables with the optimal corresponding objective function value;

[0064] Method 2: Form a linear equation system with the relaxation variable as the parameter to be solved, and use the numerical solution method to calculate and solve the characteristic parameters of each order of the Bessel curve that meets the path fairing optimization objective; obtain the optimal group of relaxation variable solutions;

[0065] Method 3: Use the gradient descent method or the genetic algorithm to select the relaxation variable, evaluate the Bessel curve constructed by the selected relaxation variable using the path fairing objective function, and select a group of better relaxation variables according to the evaluation results;

[0066] Method 4: Use the iterative algorithm to iterate the relaxation variable, and calculate the new relaxation variable according to the current set of relaxation variables each time; after each iteration, evaluate the Bessel curve constructed by the current relaxation variable using the path fairing objective function, and iterate repeatedly until the Bessel curve constructed by the current relaxation variable makes the path fairing objective function reach the optimal value, or the iteration value reaches the set threshold.

[0067] Furthermore, in step 8, the method for evaluating whether the Bessel curve meets the fairing objective requirements includes the following method steps:

[0068] Let the i-th path point be the corner point, d i Represents the i-th path point, i = 2, 3,..., z - 1; i represents the path point serial number; z is the number of path points;

[0069] Set the path smoothing objective function as follows: the integral of the norm of the third - order differential of the path smoothing curve is minimized; then, the minimum of the integral of the norm of the third - order differential is used as the objective function to evaluate whether the B - spline curve is optimal.

[0070] Let denote the B - spline curve for the turning path at the turning point d i ; F(X i ) denote the objective function for evaluating the optimality of the B - spline curve; without changing the optimality, convert the norm into the form of an inner product, then we have:

[0071]

[0072] According to the definition of the inner product on the Lie group manifold, expand and simplify this objective function to:

[0073]

[0074] In the above formulas:

[0075] u B denotes the B - spline curve parameter;

[0076] X i denotes the slack variable composed of the undetermined slack variable parameters at the turning point d i ;

[0077] denotes the B - spline curve the third - order differential of the B - spline curve with respect to the B - spline curve parameter u B ;

[0078] ω i,B denotes the vector of the lengths of the components of the first - order derivative of the B - spline curve on the basis vectors of the Lie algebra tangent space; ;

[0079] denotes the vector of the lengths of the components of the second - order derivative of the B - spline curve on the basis vectors of the Lie algebra tangent space; ;

[0080] denotes the vector of the lengths of the components of the third - order derivative of the B - spline curve on the basis vectors of the Lie algebra tangent space; ;

[0081] Use the composite Simpson's method or Romberg extrapolation method to numerically integrate and solve the value of this objective function, and evaluate the optimality of the smoothing path of the B - spline curve.

[0082] The present invention also provides a device for the end path smoothing method based on Lie group manifold, including a memory and a processor. The memory is used for storing a computer program. The processor is used for executing the computer program and realizing the steps of the end path smoothing method based on Lie group manifold as described above when executing the computer program.

[0083] The advantages and positive effects of the present invention are as follows: By receiving the initial discrete end pose path, solving the pose of the start and end path points entering and leaving the corner smoothing area through numerical method under the constraint of the smoothing area, mapping the corresponding position, attitude and end pose path to the Lie group manifold space, and solving the 0-2 order parameter continuity conditions at the start and end points, and then relaxing them into the geometric continuity conditions on the Lie group manifold by introducing slack variables, and constructing a Bezier curve connecting the start and end points on the Lie group manifold based on this. The optimality of this curve as a smoothing path can be evaluated according to the defined objective function. Thus, the optimal numerical value of the slack variable can be solved through nonlinear optimization and substituted into generate the optimal Bezier curve smoothing path, and finally inverse mapped to the end position and attitude representation to obtain the final smoothing path, thereby effectively smoothing the overall movement of the end attitude or pose path. Therefore, the problems in the related art that using the nonlinear mapping method to process the end attitude results in uneven movement of the end attitude path and using the method of separate processing and then parameter reconciliation to deal with the coordination of the end position and attitude, resulting in uncoordinated movement of the end pose path are solved.

[0084] In order to realize the overall movement smoothing of the end attitude or pose path, the original input path is transformed into a smooth Lie group manifold space. Under the constraint of geometric continuity, combined with the construction method of Bezier curve on the Lie group manifold, the corner smoothing transition path is optimized, the optimal curve smoothing path satisfying the given optimality evaluation function is solved, and the continuity and smoothness of the overall path are realized through the cutting and splicing with the original path. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] Figure 1 is the working flow chart of an end path smoothing method based on Lie group manifold of the present invention.

[0086] In the figure:

[0087] S1 to S9 respectively represent step 1 to step 9;

[0088] ε represents the constraint radius of the smoothing area;

[0089] p1, p2, p3, p4 represent the discrete pose of the initial end pose path;

[0090] F(X) represents the objective function for evaluating the optimality of the smoothing path;

[0091] Si (v) represents the end - pose path curve generated by linearly interpolating the i - th path point in the Euclidean space;

[0092] p i,s represents the corner point d in the Euclidean space i and the pose of the starting point of the path segment to be smoothed at this point;

[0093] p i,e represents the corner point d in the Euclidean space i and the pose of the ending point of the path segment to be smoothed at this point.

[0094] v s represents the curve parameter corresponding to the starting point of the path segment to be smoothed in the end - pose path curve;

[0095] v e represents the curve parameter corresponding to the ending point of the path segment to be smoothed in the end - pose path curve;

[0096] represents the pose of the starting point of the path segment to be smoothed in the Lie group manifold space;

[0097] represents the pose of the starting point of the path segment to be smoothed in the Lie group manifold space;

[0098] represents the curve on the Lie group manifold corresponding to the end - attitude curve;

[0099] correspondingly represents the 0 - th, 1 - st, and 2 - nd order parameter continuity condition vectors of the starting point of the smoothing of the end - pose path parameter curve in sequence;

[0100] correspondingly represents the 0 - th, 1 - st, and 2 - nd order parameter continuity condition vectors of the ending point of the smoothing of the end - pose path parameter curve in sequence;

[0101] H C represents the set characterizing the parameter continuity conditions;

[0102] H G represents the set of geometric continuity conditions of the end - pose path;

[0103] X represents the slack variable composed of undetermined slack variable parameters;

[0104] X * represents the optimal slack variable composed of optimized slack variable parameters;

[0105] represents the optimal Bezier curve on the Lie group manifold corresponding to the optimal slack variable;

[0106] S oIt represents the optimal fairing path in the finally obtained Euclidean space.

[0107] h(X) represents the constraint formula for the upper and lower bounds of the values of the optimized slack variables. Detailed implementation manners

[0108] The present invention will be described in detail below with reference to the accompanying drawings and in conjunction with embodiments. It should be understood that the preferred embodiments described herein are only used to illustrate and explain the present invention and are not used to limit the present invention.

[0109] Please refer to Figure 1 , a method for fairing the end path based on Lie group manifolds, characterized in that the method includes the following steps:

[0110] Step 1: Obtain discrete points of the initial end pose path in the Euclidean space, and set path fairing region constraints and a path fairing objective function.

[0111] Step 2: Connect the discrete points of the initial end pose path in the Euclidean space in sequence to form an initial end pose path curve, and the initial end pose path curve is divided into several path segments by the discrete points.

[0112] Step 3: According to the path fairing region constraints, for each path discrete point located at a corner, set the start and end points of the part of the path to be faired corresponding to its adjacent two path segments; the part of the path to be faired is called the path segment to be faired; solve the end poses of the start and end points of each path segment to be faired.

[0113] Step 4: Map the initial end pose path curve in the Euclidean space and the end poses of the start and end points of each path segment to be faired to the Lie group manifold space to obtain the end pose path parameter curve on the Lie group manifold and the Lie group elements of the end poses corresponding to the start and end points of each path segment to be faired.

[0114] Step 5: Solve the 0-2 order parameter continuity conditions at the start and end points of a certain path segment to be faired in the Lie group manifold space.

[0115] Step 6: By introducing slack variables, relax the parameter continuity conditions into geometric continuity conditions corresponding to the Lie group manifold.

[0116] Step 7: Under the constraint of satisfying the geometric continuity conditions, construct a Bessel curve connecting the start and end points of the path segment to be faired in the Lie group manifold space.

[0117] Step 8: Based on the path fairing objective function, evaluate whether the Bessel curve makes the path fairing objective function reach the optimal value; if it does not reach the optimal value, optimize and adjust the slack variables, and then turn to Step 6; if it reaches the optimal value, select the Bessel curve as the fairing path between the start and end points of the path segment to be faired; then perform Step 9.

[0118] Step 9: Smooth each remaining path segment to be smoothed one by one according to the method in Steps 5 to 8 to obtain a completely smoothed path curve of the end pose; transform the completely smoothed path curve of the end pose into the Euclidean space to obtain the final smoothed path of the end pose.

[0119] Preferably, in Step 1, the path smoothing region constraint can be set as follows: the points on the smoothed end path are located within a sphere centered at the path discrete points at the corners within the path segment to be smoothed, and the radius of the sphere is set as ε; the minimization of one parameter or a linear combination of several parameters among the four curve parameters of curve curvature, curve torsion, integral of the modulus length of each order differential of the curve, and curve length can be used as the objective function. For example, the path smoothing objective function can be set as: the integral of the norm of the N - th order differential of the smoothed end path curve is minimized; N ≥ 3.

[0120] Preferably, in Step 3, according to the path smoothing region constraint, the end poses of the start and end points of each path segment to be smoothed can be solved by the following method.

[0121] Construct a path segment connecting two adjacent path discrete points in the Cartesian space according to the adjacent end pose path discrete points and the corresponding motion modes.

[0122] Construct a non - linear equation with the distance between the points on the path segments on both sides of the path discrete point and the path discrete point as the smoothing constraint, and find the roots by numerical methods to determine the poses of the start and end points of the path segments to be smoothed on both sides of the path discrete point.

[0123] Preferably, in Step 4, the initial end pose path discrete points in the Euclidean space and the end poses of the start and end points of each path segment to be smoothed can be represented by the direction vector and / or Euler angles.

[0124] The end pose path parameter curve mapped to the Lie group manifold space and the Lie group elements of the end poses of the start and end points of each path segment to be smoothed can be represented by the rotation matrix, quaternion, homogeneous matrix, and / or dual quaternion.

[0125] Preferably, Step 5 can include the following sub - steps:

[0126] Step 5 - 1: Construct a vector tangent space at the start and end points according to each path segment to be smoothed in the Lie group manifold space and the end poses of its start and end points.

[0127] Step 5 - 2: Based on the vector tangent space, calculate the 0 - 2 order differentials of the end pose path parameter curve on the corresponding Lie group manifold at the start and end points of each path segment to be smoothed, which are represented by the corresponding Lie group or Lie algebra elements.

[0128] Step 5-3: Organize and summarize the 0-2 order differential values at the starting and ending points into a set of characterization parameter continuity conditions.

[0129] Preferably, Step 6 may include the following sub-steps:

[0130] Step 6-1: Construct a relaxation rule for converting the 0-2 order parameter continuity conditions in the Lie group manifold space into relaxed geometric continuity conditions according to the principle of invariant Cartesian space points, tangent vectors, and curvature.

[0131] Step 6-2: Based on this relaxation rule, convert the Lie group elements, tangent vectors, and pseudo-curvature in the Lie group manifold space into relaxed geometric continuity conditions, and determine the set of geometric continuity conditions at the starting and ending points of each path segment to be smoothed on the Lie group manifold according to this rule and the relaxation variables.

[0132] Preferably, let the i-th path point be a corner point, d i represents the i-th path point, i = 2, 3, …, z-1; i represents the path point serial number; z is the number of path points.

[0133] In the Lie group manifold space, establish the following relaxation rule under the conditions of invariant Lie group elements, tangent vectors, and pseudo-curvature:

[0134]

[0135] According to this relaxation principle, construct the corresponding geometric continuity conditions as follows:

[0136]

[0137] Take α i,s , β i,s , α i,e , β i,e as independent variables, and further obtain the following relational expressions:

[0138]

[0139] In the formula:

[0140] H C0 , H C1 , H C2 correspondingly represent the 0th, 1st, and 2nd order parameter continuity condition vectors of the end pose path parameter curve in sequence;

[0141] correspondingly represent the unit vector of the 1st order parameter continuity condition vector of the end pose path parameter curve;

[0142] (H C1 ×H C2 ) ∧The unit vector representing the cross product of the 1st and 2nd order parameter continuity condition vectors of the end - pose path parameter curve;

[0143] κ represents the pseudo - curvature of the end - pose path parameter curve;

[0144] α, β represent the undetermined relaxation variable parameters of the end - pose path parameter curve;

[0145] H G0 、H G1 、H G2 Correspondingly represent the 0th, 1st, and 2nd order geometric continuity condition vectors of the end - pose path in sequence;

[0146] Correspondingly represent the 0th, 1st, and 2nd order parameter continuity condition vectors of the starting point of the path segment to be fair - faced at the corner point d i of the end - pose path parameter curve;

[0147] Represents the unit vector of the 1st order parameter continuity condition vector of the starting point of the path segment to be fair - faced at the corner point d i of the end - pose path parameter curve;

[0148] Represents the unit vector of the cross product of the 1st and 2nd order parameter continuity condition vectors of the starting point of the path segment to be fair - faced at the corner point d i of the end - pose path parameter curve;

[0149] κ i,s Represents the pseudo - curvature of the starting point of the path segment to be fair - faced at the corner point d i of the end - pose path parameter curve;

[0150] α i,s 、β i,s Represents the undetermined relaxation variable parameters of the starting point of the path segment to be fair - faced at the corner point d i of the end - pose path parameter curve;

[0151] Correspondingly represent the 0th, 1st, and 2nd order geometric continuity condition vectors of the starting point of the path segment to be fair - faced at the corner point d i of the end - pose path in sequence;

[0152] Correspondingly represent the 0th, 1st, and 2nd order parameter continuity condition vectors of the starting point of the path segment to be fair - faced at the corner point d i of the end - pose path parameter curve;

[0153] Represents the corner point d iThe unit vector of the first-order parameter continuity condition vector at the starting point of the path segment to be fairing;

[0154] Denote the corner point d of the path parameter curve of the end pose i The unit vector of the cross product of the first- and second-order parameter continuity condition vectors at the starting point of the path segment to be fairing;

[0155] κ i,e Denote the corner point d of the path parameter curve of the end pose i The pseudo-curvature at the starting point of the path segment to be fairing;

[0156] α i,e , β i,e Denote the corner point d of the path parameter curve of the end pose i The undetermined relaxation variable parameter at the starting point of the path segment to be fairing;

[0157] Successively correspond to denote the 0th-, 1st-, and 2nd-order geometric continuity condition vectors at the starting point of the path segment to be fairing at the corner point d of the end pose path i ;

[0158] Denote the geometric continuity condition function at the corner point d of the end pose path i ;

[0159] X i Denote the corner point d i The relaxation variable composed of the undetermined relaxation variable parameters.

[0160] ‖‖ represents the 2-norm of a vector, that is, the vector norm. For example: Denote the 2-norm of the vector, that is the vector norm.

[0161] Preferably, in step 7, set the optimization objective as the path fairing objective function; set the constraint as the empirical limit range of the relaxation variable; the relaxation variable can be optimized and selected on the Lie group manifold according to one of the following methods.

[0162] Method 1: Derive the calculation formula of the objective function convenient for computer operation through the analytical method, substitute multiple groups of relaxation variables to calculate the objective function, and obtain a group of relaxation variables with the optimal corresponding objective function value.

[0163] Method 2: Form a linear equation system with the relaxation variable as the parameter to be solved, and use the numerical solution method to calculate and solve the characteristic parameters of each order of the Bessel curve that meets the path fairing optimization objective; obtain an optimal group of relaxation variable solutions.

[0164] Method 3: The gradient descent method or genetic algorithm is used to select the relaxation variables. The path smoothing objective function is used to evaluate the Bézier curve constructed by the selected relaxation variables, and a set of better relaxation variables is selected according to the evaluation results.

[0165] Method 4: An iterative algorithm is used to iterate the relaxation variables. Each iteration calculates new relaxation variables based on the current set of relaxation variables; after each iteration, the path smoothing objective function is used to evaluate the Bézier curve constructed by the current relaxation variables, and the iteration is repeated until the Bézier curve constructed by the current relaxation variables makes the path smoothing objective function reach the optimal value or the iteration value reaches the set threshold.

[0166] Preferably, in step 8, the method for evaluating whether the Bézier curve meets the requirements of the smoothing objective may include the following method steps:

[0167] The i-th path point can be set as the corner point, and d i represents the i-th path point, where i = 2, 3, …, z - 1; i represents the path point sequence number; and z is the number of path points.

[0168] The following path smoothing objective function can be set: the integral of the norm of the third derivative of the path smoothing curve is minimized; then the minimum of the integral of the norm of the third derivative is used as the objective function for evaluating whether the Bézier curve is optimal.

[0169] It can be set that represents the Bézier curve for smoothing the turning angle path at the corner point d i ; F(X i ) represents the objective function for evaluating the optimality of the Bézier curve; without changing the optimality, the norm is transformed into the form of an inner product, then there is:

[0170]

[0171] According to the definition of the inner product on the Lie group manifold, the objective function is expanded and simplified to:

[0172]

[0173] In the above formulas:

[0174] u B represents the Bézier curve parameter;

[0175] X i represents the relaxation variables composed of the undetermined relaxation variable parameters at the corner point d i ;

[0176] represents the Bézier curve the third derivative of the Bézier curve with respect to the Bézier curve parameter u B ;

[0177] ω i,B denotes the vector of the lengths of the components of the first - order derivative of the Bézier curve on the basis vectors of the Lie - algebra tangent space;

[0178] denotes the vector of the lengths of the components of the second - order derivative of the Bézier curve on the basis vectors of the Lie - algebra tangent space;

[0179] denotes the vector of the lengths of the components of the third - order derivative of the Bézier curve on the basis vectors of the Lie - algebra tangent space;

[0180] The value of the objective function is solved by using the composite Simpson's method or Romberg extrapolation method for numerical integration, and the optimality of the smoothing path of the Bézier curve is evaluated.

[0181] The present invention also provides a device for the end - path smoothing method based on Lie - group manifold, including a memory and a processor. The memory is used for storing a computer program; the processor is used for executing the computer program and realizing the steps of the end - path smoothing method based on Lie - group manifold as described above when executing the computer program.

[0182] The working process and working principle of the present invention are further described below by taking a preferred embodiment of the present invention:

[0183] As Figure 1 shown, an end - path smoothing method based on Lie - group manifold, characterized in that the method includes the following steps:

[0184] Step 1: Obtain the discrete points of the initial end - pose path in the Euclidean space, and set the path - smoothing region constraint and the path - smoothing objective function.

[0185] Step 2: Connect the discrete points of the initial end - pose path in the Euclidean space in sequence to form an initial end - pose path curve, and the initial end - pose path curve is divided into several path segments by the discrete points.

[0186] Step 3: According to the path - smoothing region constraint, for each path discrete point located at a corner, set the start and end points of the path part to be smoothed on its adjacent two path segments; the path part to be smoothed is called the to - be - smoothed path segment; solve the end - poses of the start and end points of each to - be - smoothed path segment.

[0187] Step 4: Map the initial end - pose path curve in the Euclidean space and the end - poses of the start and end points of each to - be - smoothed path segment to the Lie - group manifold space to obtain the end - pose path parameter curve on the Lie - group manifold and the Lie - group elements of the end - poses corresponding to the start and end points of each to - be - smoothed path segment. ​​​

[0188] Step 5: Solve the 0-2 order parametric continuity conditions at the starting and ending points of a to-be-smoothed path segment in the Lie group manifold space.

[0189] Step 6: By introducing slack variables, relax the parametric continuity conditions into geometric continuity conditions corresponding to the Lie group manifold.

[0190] Step 7: Under the constraint of satisfying the geometric continuity conditions, construct a Bézier curve in the Lie group manifold space connecting the starting and ending points of the to-be-smoothed path segment.

[0191] Step 8: Based on the path smoothing objective function, evaluate whether the Bézier curve makes the path smoothing objective function reach the optimal value; if not, optimize and adjust the slack variables, and then turn to Step 6; if it reaches the optimal value, select the Bézier curve as the smoothed path between the starting and ending points of the to-be-smoothed path segment; then proceed to Step 9.

[0192] Step 9: Smooth the remaining to-be-smoothed path segments one by one according to the methods in Steps 5 to 8 to obtain a complete smoothed path curve of the end pose; transform the complete smoothed path curve of the end pose into the Euclidean space to obtain the final smoothed path of the end pose.

[0193] In Step 1, receive the initial discrete end pose path in the Euclidean space, the size constraint of the path smoothing region, and the objective function for evaluating the optimality of the smoothed path as inputs.

[0194] It can be understood that the embodiments of the present invention can receive the initial discrete end pose path in the Euclidean space, such as G code, position and Euler angle sequence, homogeneous transformation matrix sequence, rotation matrix sequence, etc.; the size constraint of the path smoothing region, such as the allowable range radius of the smoothing region and error at the path discrete points; the objective function for evaluating the optimality of the smoothed path, such as minimum curvature, minimum integral of the differential modulus of each order, etc. as the three input elements, and use this as the basis for carrying out the following steps.

[0195] Taking three initial end pose path discrete points as an example, let d1, d2, and d3 be three initial end pose path discrete points arranged in sequence. Let p1, p2, and p3 correspond to the poses of d1, d2, and d3 respectively. The connection of points d1, d2, and d3 forms an initial end pose path curve containing a corner point. d2 is the corner point of the initial end pose path curve.

[0196] Represent three discrete end - pose paths containing a corner point in the form of an Euler - angle sequence. Set the initial end - pose path smoothing region constraint as follows: points on the smoothed end - path are located within a sphere centered on the discrete points within the path segment to be smoothed, and let the radius of this sphere be ε. Set the path - smoothing objective function as: minimizing the integral of the norm of the third - order differential of the smoothed end - path curve.

[0197] The initial discrete end - pose path s in Euclidean space can be the following set of discrete - point sequences of the initial end - pose path:

[0198] s = [p1, p2, p 3, …, p z ;

[0199] where p1, p2, p 3, …, p z correspond to the poses of the 1st to the z - th discrete points of the initial end - pose path respectively. s is the initial discrete end - pose path in Euclidean space.

[0200] Let the smoothing - region constraint radius of an end - pose target - path point be ε, and set the path - smoothing objective function as: minimizing the integral of the norm of the third - order differential of the smoothed end - path curve.

[0201] In step 2, connect the discrete points of the initial end - pose path in Euclidean space in sequence to form an initial end - pose path curve, and the initial end - pose path curve is divided into several path segments by the discrete points.

[0202] Suppose there are two adjacent path segments with an intersection angle less than 180 degrees. Let these two adjacent path segments be obtained by connecting the discrete points d1, d2, d3 of the initial end - pose path in sequence. The intersection angle between the d1d2 path segment and the d2d3 path segment is less than 180 degrees, and d2 is the corner point. The initial end - pose path curve formed by d1, d2, d3 needs to be smoothed at the corner point d2.

[0203] In step 3, based on the initial end - pose path and the smoothing constraint, solve the poses of the start and end path points entering and leaving the corner - smoothing region by numerical methods.

[0204] Solve for the start and end path points that need to enter and leave the corner - smoothing constraint region. The start and end path points entering and leaving the corner - smoothing constraint region are the start and end points of the path segment to be smoothed. The main method for solving them is to find the intersection points of the pose path and the smoothing - constraint region according to the end - pose path points, the motion mode between path points, and the radius constraint of the smoothing region, that is, take the points on the initial end - pose path curve whose distance from the path discrete point at the corner is equal to the constraint radius as the start and end points of the path segment to be smoothed.

[0205] Based on the initial end - pose path and the smoothing constraint, the poses of the starting and ending points of the path segment to be smoothed are solved by numerical methods, including: constructing the end - pose path connecting adjacent path points in the Cartesian space according to adjacent end - pose path points and the corresponding motion modes; setting the distance between the points on the end - pose path and the target path point as the smoothing constraint, constructing a non - linear smoothing - constraint equation, and finding the roots through numerical methods to determine the poses of the starting and ending points of the part of the path to be smoothed on both sides of the target path point.

[0206] The discrete - point sequences of three initial end - pose paths containing a corner point can be used to construct the end - pose path curve between adjacent path discrete points in the Cartesian space according to the set motion mode, such as constructing the end - pose path curve by using the Euler - angle linear interpolation method. Further, a distance formula between the path points on this end - pose path curve and the target path point is constructed and made equal to the smoothing - region constraint radius to construct a non - linear equation. Finally, the roots of the non - linear equation can be solved by numerical methods such as the trust - region algorithm and substituted into the end - pose path curve to obtain the end - poses of the starting and ending points of the path segment to be smoothed.

[0207] For example, from the discrete - point sequences of three initial end - pose paths and the given motion mode of Euler - angle linear interpolation, let the i - th path point be the corner point, d i denote the i - th path point, p i denote the pose of the i - th path point, where i = 2, 3, …, z - 1; i represents the path - point serial number; z is the number of path points;

[0208] The end - pose path curve between three points can be constructed, and the expression of the end - pose path curve between three points is as follows:

[0209]

[0210] In the formula:

[0211] S i (v) represents the end - pose path curve generated by linearly interpolating the i - th path point in the Euclidean space;

[0212] v represents the curve parameter of S i (v);

[0213] v i represents the curve parameter corresponding to the i - th path point;

[0214] The curve S i (v) is a discontinuous end - pose path curve at the corner point v = v i .

[0215] The angular distance between two points on the end - pose path curves can be defined by the axis - angle representation method as follows:

[0216]

[0217] In the formula:

[0218] d(g, h) represents the angular distance between any two points on the end - pose path curve;

[0219] g, h represent any two points on the end - pose path curve;

[0220] Tr() represents the function for solving the trace of a matrix;

[0221] Rot() represents the conversion function for converting the end - pose to a rotation matrix.

[0222] It can make the angular distance between the points on the end - pose path curve and the target path points equal to the fairing region constraint radius ε. And thus construct the following non - linear equation for the corresponding corner point d i :

[0223] d(S i (v), p i ) - ε = 0;

[0224] Finally, taking - 0.99 and 0.99 as the initial iteration values respectively, solve the roots of the non - linear equation as v i,s and v i,e , and substitute them into the end - pose path curve to obtain the start - pose p i,s of the path segment to be fairing and the end - pose p i,e of the path segment to be fairing.

[0225] Where:

[0226] v i,s and v i,e represent the roots of the non - linear equation for the corresponding corner point d i ;

[0227] p i,s represents the pose of the start point of the path segment to be fairing at the corner point d i in the Euclidean space;

[0228] p i,e represents the pose of the end point of the path segment to be fairing at the corner point d i in the Euclidean space.

[0229] In step 4, map the initial end - pose path curve in the Euclidean space and the end - poses of the start and end points of each path segment to be fairing obtained by the above method into the Lie group manifold space to obtain the corresponding Lie group elements and the parametric curve on the manifold.

[0230] The initial end - pose path curve in Euclidean space and the end - poses of the start and end points of each path segment to be smoothed are represented by various methods such as direction vectors and Euler angles, and are mapped to the Lie - group elements of the initial end - pose path parameter curve in the Lie - group manifold space and the end - poses of the start and end points of each path segment to be smoothed through representation by rotation matrices, quaternions, homogeneous matrices, and / or dual quaternions. By constructing the corresponding mapping relationship, the initial end - pose path curve in Euclidean space and the end - poses of the start and end points of each path segment to be smoothed are uniformly transformed into Lie - group elements in the Lie - group manifold space.

[0231] For example, by constructing the mapping relationship between the Euler - angle representation in Euclidean space and the quaternion in Lie - group elements, the start - and - end path points and the end - pose curve obtained in the above steps are transformed into the corresponding points and parameter curves on the Lie - group manifold.

[0232] For example, let the \(i\) - th path point be a corner point, \(d\) i denotes the \(i\) - th path point, and \(p\) i denotes the pose of the \(i\) - th path point, where \(i = 2,3,\cdots,z - 1\); \(i\) represents the path - point serial number; and \(z\) is the number of path points.

[0233] In the embodiment of the present invention, according to the Euler - angle representation method of the information obtained in the above steps, let the pose of the point on the end - pose curve in Euclidean space represented by Euler angles be \(p\), and the mapping relationship between it and the pose of the point on the end - pose parameter curve on the Lie - group manifold represented by quaternions is \(Quat(p)\). Thus, the poses \(p\) i,s , \(p\) i,e of the start and end points of the path segment to be smoothed in Euclidean space and the end - pose path curve \(S\) i (v) can be transformed into the poses of the corresponding points on the Lie - group manifold as and the end - pose parameter curve That is and

[0234] JIn the formula:

[0235] denotes the pose of the start point of the path segment to be smoothed at the corner point \(d\) i in the Lie - group manifold space;

[0236] denotes the pose of the end point of the path segment to be smoothed at the corner point \(d\) i in the Lie - group manifold space;

[0237] denotes the end - pose path parameter curve obtained by mapping \(S\) i (v) to the Lie - group manifold space;

[0238] u represents the curve parameter;

[0239] Quat() represents a function that maps the end pose in Euclidean space to the corresponding quaternion Lie group element.

[0240] In step 5, the 0-2 order continuity conditions with respect to the curve parameter at the Lie group elements of the starting and ending path points on the curve can be solved based on the parameter curve of the end pose on the Lie group manifold.

[0241] The method for solving the 0-2 order parameter continuity conditions at the starting and ending points in the Lie group manifold space includes the following sub-steps:

[0242] Step 5-1, construct the vector tangent space at the starting and ending points according to each path segment to be smoothed in the Lie group manifold space and their end poses at the starting and ending points.

[0243] Step 5-2, based on the vector tangent space, calculate the 0-2 order differentials of the parameter curve of the end pose path on the corresponding Lie group manifold at the starting and ending points of each path segment to be smoothed, represented by the corresponding Lie group or Lie algebra elements.

[0244] Step 5-3, organize and summarize the 0-2 order differential values at the starting and ending points into a set representing the parameter continuity conditions.

[0245] First, a general representation method of the tangent space on the Lie group manifold such as the quaternion manifold can be established, such as the Lie algebra And the basis of the related tangent space is established. Further, based on the tangent space, the 0-2 order differentials represented by the basis of the tangent space corresponding to the starting and ending points of each path segment to be smoothed are solved through the differential formula of the Lie group manifold. Finally, it can be summarized into a set representing the parameter continuity conditions.

[0246] According to the three-dimensional quaternion Lie group manifold S of the points on the parameter curve of the end pose path on the Lie group manifold 3 , establish the related tangent space on the manifold as the Lie algebra space The basis of this space is the pure quaternion, that is:

[0247] E1 = [1 0 0] T , E2 = [0 1 0] T , E3 = [0 0 1] T .

[0248] E j represents the j-th basis vector of the Lie algebra tangent space ; E1, E2, E3 respectively represent the 1st, 2nd, and 3rd basis vectors of the Lie algebra tangent space ; j = 1, 2, 3.

[0249] Based on this tangent space, the 0-2 order differentials of the end-effector pose path parameter curve with respect to the curve parameter can be solved through the differential formula of the Lie group manifold, and we have:

[0250]

[0251] In the formula,

[0252]

[0253] where Conj(q) is the solution function of the conjugate quaternion and

[0254] Thus, the parameter coordinates of the start and end points of the path segment to be smoothed can be substituted to solve the 0-2 order parameter continuity, that is, the solution of the differential is:

[0255]

[0256] Finally, it can be summarized as a set representing the parameter continuity conditions

[0257] In the above formulas:

[0258] X i (u) represents the end-effector pose path parameter curve on the Lie group manifold the 0th order differential with respect to the curve parameter u;

[0259] V i (u) represents the end-effector pose path parameter curve on the Lie group manifold the 1st order differential with respect to the curve parameter u;

[0260] A i (u) represents the end-effector pose path parameter curve on the Lie group manifold the 2nd order differential with respect to the curve parameter u;

[0261] u i,s represents the curve parameter corresponding to the start point of the path segment to be smoothed on the end-effector pose path parameter curve ;

[0262] u i,e represents the curve parameter corresponding to the end point of the path segment to be smoothed on the end-effector pose path parameter curve ;

[0263] represents the Lie algebra tangent space corresponding to the quaternion manifold;

[0264] ω i,j (u) represents the length of the component of the 1st order derivative of the end-effector pose path parameter curve on the jth basis vector of the Lie algebra tangent space;

[0265] Denote the length of the component of the second - order derivative of the end - pose path parameter curve on the j - th basis vector of the Lie - algebra tangent space;

[0266] Denote the first - order differential of the end - pose path parameter curve with respect to the curve parameter u;

[0267] Denote the second - order differential of the end - pose path parameter curve with respect to the curve parameter u;

[0268] M q () represents the constructor of the matrix representation of quaternion multiplication;

[0269] Conj() represents the function for solving the conjugate quaternion;

[0270] q represents the input parameter of the conjugate - quaternion - solving function, that is, a general quaternion;

[0271] q1, q2, q3, q4 represent the 1st, 2nd, 3rd, and 4th components of the four - dimensional general quaternion vector q;

[0272] Denote the 0 - th order parameter continuity condition vector corresponding to the start point of the path segment to be smoothed of the end - pose path parameter curve;

[0273] Denote the 1 - st order parameter continuity condition vector corresponding to the start point of the path segment to be smoothed of the end - pose path parameter curve;

[0274] Denote the 2 - nd order parameter continuity condition vector corresponding to the start point of the path segment to be smoothed of the end - pose path parameter curve;

[0275] Denote the 0 - th order parameter continuity condition vector corresponding to the end point of the path segment to be smoothed of the end - pose path parameter curve;

[0276] Denote the 1 - st order parameter continuity condition vector corresponding to the end point of the path segment to be smoothed of the end - pose path parameter curve;

[0277] Denote the 2 - nd order parameter continuity condition vector corresponding to the end point of the path segment to be smoothed of the end - pose path parameter curve.

[0278] In step 6, by introducing slack variables, the parametric continuity condition is relaxed to a geometric continuity condition on the Lie group manifold.

[0279] According to the connection principle between parametric continuity and geometric continuity, under certain constraints, by introducing undetermined slack variable factors, the parametric continuity condition can be relaxed to a geometric continuity condition, so as to achieve the effect of regulating the parametric curve through slack variables under geometric continuity constraints.

[0280] By introducing slack variables, the parametric continuity condition is relaxed to a geometric continuity condition on the Lie group manifold, including: constructing a relaxation principle for relaxing the 0-2 order parametric continuity condition to a geometric continuity condition in the Lie group manifold space according to the principle of invariant Cartesian space points, tangents, and curvatures; taking this relaxation principle as the relaxation principle for the Lie group elements, tangent vectors, and pseudo-curvatures in the Lie group manifold space to be invariant to the geometric continuity condition, and determining the set of geometric continuity conditions at the starting and ending points on the Lie group manifold according to this relaxation principle and slack variables.

[0281] First, according to the parametric continuity conditions of the starting and ending points on the Lie group manifold obtained in the above steps, such as the 0-2 order parametric continuity conditions of the corresponding points on the quaternion manifold, a regulation relaxation principle of invariant points, tangent space directions, and curvatures on the manifold is established. Further, according to this relaxation principle, specific slack variables are substituted, and the parametric continuity condition on the Lie group manifold is transformed into a geometric continuity condition corresponding to a function of the slack variables.

[0282] Step 6 includes the following method steps:

[0283] Let the i-th path point be a corner point, d i denote the i-th path point, where i = 2, 3, …, z - 1; i represents the path point sequence number; z is the number of path points;

[0284] In the Lie group manifold space, the following relaxation rules are established under the conditions of invariant Lie group elements, tangent vectors, and pseudo-curvatures:

[0285]

[0286] According to this relaxation principle, the corresponding geometric continuity conditions are constructed as follows:

[0287]

[0288] Taking α i,s , β i,s , α i,e , β i,e as independent variables, the following relational expressions are further obtained:

[0289]

[0290] In the formula:

[0291] H C0 、H C1 、H C2 respectively correspond to representing the 0th, 1st, and 2nd order parameter continuity condition vectors of the end - pose path parameter curve;

[0292] correspond to representing the unit vector of the 1st order parameter continuity condition vector of the end - pose path parameter curve;

[0293] (H C1 ×H C2 ) ∧ represents the unit vector of the cross - product of the 1st and 2nd order parameter continuity condition vectors of the end - pose path parameter curve;

[0294] κ represents the pseudo - curvature of the end - pose path parameter curve;

[0295] α, β represent the undetermined relaxation variable parameters of the end - pose path parameter curve;

[0296] H G0 、H G1 、H G2 respectively correspond to representing the 0th, 1st, and 2nd order geometric continuity condition vectors of the end - pose path;

[0297] respectively correspond to representing the 0th, 1st, and 2nd order parameter continuity condition vectors of the starting point of the path segment to be fairing at the inflection point d i of the end - pose path parameter curve;

[0298] represents the unit vector of the 1st order parameter continuity condition vector of the starting point of the path segment to be fairing at the inflection point d i of the end - pose path parameter curve;

[0299] represents the unit vector of the cross - product of the 1st and 2nd order parameter continuity condition vectors of the starting point of the path segment to be fairing at the inflection point d i of the end - pose path parameter curve;

[0300] κ i,s represents the pseudo - curvature of the starting point of the path segment to be fairing at the inflection point d i of the end - pose path parameter curve;

[0301] α i,s 、β i,s represent the undetermined relaxation variable parameters of the starting point of the path segment to be fairing at the inflection point d i of the end - pose path parameter curve;

[0302] correspondingly represent the 0th, 1st, and 2nd order geometric continuity condition vectors of the starting point of the path segment to be smoothed at the corner point d of the end pose path i ;

[0303] correspondingly represent the 0th, 1st, and 2nd order parametric continuity condition vectors of the starting point of the path segment to be smoothed at the corner point d of the parametric curve of the end pose path i ;

[0304] represent the unit vector of the 1st order parametric continuity condition vector of the starting point of the path segment to be smoothed at the corner point d of the parametric curve of the end pose path i ;

[0305] represent the unit vector of the cross product of the 1st and 2nd order parametric continuity condition vectors of the starting point of the path segment to be smoothed at the corner point d of the parametric curve of the end pose path i ;

[0306] κ i,e represent the pseudo-curvature of the starting point of the path segment to be smoothed at the corner point d of the parametric curve of the end pose path i ;

[0307] α i,e , β i,e represent the undetermined relaxation variable parameters of the starting point of the path segment to be smoothed at the corner point d of the parametric curve of the end pose path i ;

[0308] correspondingly represent the 0th, 1st, and 2nd order geometric continuity condition vectors of the starting point of the path segment to be smoothed at the corner point d of the end pose path i ;

[0309] represent the geometric continuity condition function of the end pose path at the corner point d i ;

[0310] X i represent the relaxation variable composed of the undetermined relaxation variable parameters at the corner point d i ;

[0311] In step 7, under the constraint of satisfying the geometric continuity condition, construct a Bezier curve connecting the starting and ending points on the Lie group manifold.

[0312] According to the geometric continuity conditions on the Lie group manifold obtained by the above steps, construct a transition curve according to the construction principle of the Bezier curve that satisfies the continuity constraints at the starting and ending points, and give the calculation expression of this curve. Further, for the Bezier curve, constructing the curve means constructing the control points on the manifold of the curve according to the continuity constraint conditions, and the calculation expression of the corresponding curve is the recurrence calculation formula for the control points.

[0313] The control points of the Bezier curve of the corresponding order can be constructed according to the geometric continuity conditions of the starting and ending points on the Lie group manifold, such as the 0-2 order geometric continuity conditions of the corresponding points on the quaternion manifold. For example, the 0-2 order continuity corresponds to 6 control points of the 5th order Bezier curve. Further, based on the obtained control points, the general expression of the corresponding Bezier curve is solved by the recurrence method, such as the recurrence expression of the 5th order Bezier curve based on 6 control points.

[0314] The 0-2 order end pose path geometric continuity condition function obtained according to the above steps First, construct 6 control points corresponding to the 5th order Bezier curve, and set the corresponding corner point as d i The poses of the 6 control points are successively The calculation formula for the control point pose is as follows:

[0315]

[0316] Among them,

[0317]

[0318] and are the corresponding right addition and subtraction operations on the Lie group manifold.

[0319] is the corresponding right addition operation rule on the Lie group manifold.

[0320] is the corresponding right subtraction operation rule on the Lie group manifold.

[0321] In the above formulas:

[0322] m and n represent two general tangent vectors in the tangent space at the given Lie group element on the Lie group manifold;

[0323] t represents a general real independent variable;

[0324] represents a tangent vector τ in the tangent space at the given Lie group element on the Lie group manifold;

[0325] Represents a general Lie group element on the Lie group manifold;

[0326] Represents a general Lie group element on the Lie group manifold;

[0327] (dExp x ) mn Represents the function of the derivative of the exponential map Exp() at the given Lie group element x on the Lie group manifold acting on the tangent vector n on the tangent vector m;

[0328] Represents the multiplication of the corresponding group elements on the Lie group manifold;

[0329] Q i () represents the constructor of the control point vector of the Bézier curve corresponding to the corner point d i on the Lie group manifold;

[0330] Correspondingly represents the 6 control points of the 5th-order Bézier curve corresponding to the corner point d i on the Lie group manifold;

[0331] w i,s ,w i,e Represents two intermediate variables in the calculation process of the control points corresponding to the corner point d i on the Lie group manifold;

[0332] Exp() represents the exponential map between the corresponding Lie algebra element and the Lie group element on the Lie group manifold;

[0333] Log() represents the logarithmic map between the corresponding Lie algebra element and the Lie group element on the Lie group manifold.

[0334] Furthermore, the recurrence formula for constructing a kth-order Bézier curve using the de Casteljau algorithm (Casteljau method) is as follows:

[0335]

[0336] where, and μ ∈ [0, 1]. When based on the six control points on the quaternion manifold obtained above, a 5th-order Bézier curve under the geometric continuity constraint with respect to the slack variable parameter can be constructed as follows:

[0337]

[0338] In the above formulas:

[0339] γ k Represents the recurrence formula of the kth-order Bézier curve on the Lie group manifold;

[0340] Correspondingly represent the control points of the Bessel curve on the 0th to kth Lie group manifolds;

[0341] k represents the order of the Bessel curve, k = number of control points - 1;

[0342] μ represents the parameter of the kth-order Bessel curve;

[0343] Represents the generalized Bessel smoothing path curve of the corner path at the corresponding corner point d in the Lie group manifold space i at the turning angle;

[0344] u B Represents the curve parameter.

[0345] Thus, the Bessel smoothing path curve of the corner path under the geometric continuity constraint on the Lie group manifold is constructed.

[0346] In step 8, based on the objective function, evaluate whether the Bessel curve is the optimal smoothing path.

[0347] According to the objective function for evaluating the optimality of the smoothing path, evaluate the optimality of the Bessel curve on the Lie group manifold satisfying the continuity constraint as the smoothing path, and use this as the criterion for evaluating and optimizing the corner smoothing path.

[0348] The method for evaluating whether the Bessel curve is the optimal smoothing path includes: solving the characteristic parameters of each order of the Bessel curve required to satisfy the objective function; judging whether the calculation formula of the objective function convenient for computer operation can be derived by the analytical method. If so, substitute the relevant parameters to calculate the optimality of the curve path. If not, use the numerical solution method to calculate the value of the objective function as the optimality of the curve path.

[0349] According to the objective function for evaluating the optimality of the smoothing path, such as the objective function with the minimum integral of the norm of the third-order differential, evaluate the optimality of the Bessel smoothing path curve at the corner on the Lie group manifold obtained in the above steps as the smoothing path, such as the optimality of the 5th-order Bessel smoothing path curve on the quaternion manifold as the smoothing path. First, solve the characteristic parameters of each order of the corresponding Bessel curve required according to the objective function, and then judge whether the value of the objective function can be solved by the analytical method. If it can, substitute and solve. If not, solve by the numerical method. Finally, the evaluation value of the optimality of the curve can be obtained.

[0350] According to the generalized Bessel smoothing path curve of the corner path at the corresponding corner point d in the Lie group manifold space obtained in the above steps i at the turning angle Perform an evaluation with the minimum integral of the norm of the third-order differential as the objective function, then there is:

[0351] The objective function is minF(X i ).

[0352] Take F(X i ) as the objective function to evaluate the optimality of the Bezier curve; use F(X i ) to evaluate the optimality of the Bezier smooth path curve .

[0353] Solve the third-order differential parameters corresponding to the Bezier smooth path curve of this corner path. For the parametric curve on the Lie group manifold, there are:

[0354]

[0355] In the formula:

[0356] Correspondingly represent the 1st, 2nd, and 3rd order differentials of the curve with respect to the curve parameter u B ;

[0357] ω i,B,j (u B ), ω i,B,l (u B ) Correspondingly represent the lengths of the components of the first-order derivative of the smooth path parametric curve on the j-th and l-th basis vectors of the Lie algebra tangent space;

[0358] Represents the length of the component of the second-order derivative of the smooth path parametric curve on the j-th basis vector of the Lie algebra tangent space;

[0359] Represents the length of the component of the third-order derivative of the smooth path parametric curve on the j-th basis vector of the Lie algebra tangent space;

[0360] j, l represent the serial numbers of the basis vectors of the Lie algebra tangent space ; j, l = 1, 2, 3;

[0361] E j , E l Correspondingly represent the j-th and l-th basis vectors of the Lie algebra tangent space .

[0362] In the formula, [E j , E l = E j E l - E j E lis the corresponding Lie bracket on the Lie group manifold. According to the definition of the Bezier curve on the Lie group manifold obtained by the above steps, the differentials of the basic first-order Bezier curve are as follows:

[0363]

[0364] where:

[0365] correspondingly represent the 1st, 2nd, and 3rd differentials of the basic first-order Bezier curve;

[0366] denote the 1st and 2nd differentials with respect to the curve parameter μ;

[0367] ω γ1 is the vector representing the length of the component of the 1st derivative of the first-order Bezier curve on the basis vectors of the Lie algebra tangent space;

[0368] is the vector representing the length of the component of the 2nd derivative of the first-order Bezier curve on the basis vectors of the Lie algebra tangent space;

[0369] is the vector representing the length of the component of the 3rd derivative of the first-order Bezier curve on the basis vectors of the Lie algebra tangent space;

[0370] ω γ1,l represents the length of the component of the 1st derivative of the first-order Bezier curve on the l-th basis vector of the Lie algebra tangent space;

[0371] represents the length of the component of the 2nd derivative of the first-order Bezier curve on the j-th basis vector of the Lie algebra tangent space;

[0372] E represents the basis vectors of the Lie algebra tangent space ;

[0373] [,] represents the corresponding Lie bracket operation on the Lie group manifold;

[0374] ω γ1,1 、ω γ1,2、 ω γ1,3 correspondingly represent the lengths of the components of the 1st derivative of the first-order Bezier curve on the 1st, 2nd, and 3rd basis vectors of the Lie algebra tangent space.

[0375] Thus, the characteristic parameters of the corresponding Bezier curve can be obtained by recursive solution. Next, the optimality of the Bezier smoothing path curve as the turning angle path can be evaluated according to the objective function of minimizing the integral of the norm of the set third-order differential . Without changing the optimality, the norm can be transformed into the form of an inner product, so there is:

[0376]

[0377] According to the definition of the inner product on the Lie group manifold, the objective function can be expanded and simplified to:

[0378]

[0379] In the above expressions:

[0380] u B represents the Bessel curve parameter;

[0381] X i represents the corner point d i the slack variable composed of the undetermined slack variable parameters;

[0382] represents the Bessel curve For the Bessel curve parameter u B the third-order differential;

[0383] ω i,B represents the Bessel curve the vector of the length of the component of the first-order derivative on the basis vector of the Lie algebra tangent space;

[0384] represents the Bessel curve the vector of the length of the component of the second-order derivative on the basis vector of the Lie algebra tangent space;

[0385] represents the Bessel curve the vector of the length of the component of the third-order derivative on the basis vector of the Lie algebra tangent space.

[0386] Since the complexity of the formula corresponding to the objective function is relatively high and the integrated formula cannot be obtained by analytical methods, it is necessary to use numerical integration methods to solve the value of the objective function, such as using the composite Simpson method or the Romberg extrapolation method for numerical integration. Thus, the evaluation of the optimality of the Bessel curve as a fairing path is completed.

[0387] In step 8, the optimal numerical value of the slack variable is solved through nonlinear optimization and substituted to generate the optimal Bessel curve fairing path.

[0388] The above steps can be summarized as the solution of the optimality evaluation index for the given slack variable, and then it is constructed as the objective function and optimization variable of a nonlinear optimization problem to solve the optimal value of the slack variable that satisfies the given evaluation function, and substitute it into the curve function to obtain the optimal corner transition fairing curve path.

[0389] The optimal value of the relaxation variable is solved through nonlinear optimization and substituted to generate the optimal Bézier curve fairing path. The optimization problem includes: the optimization objective is the optimality of the Bézier curve, the optimization variable is the value of the relaxation variable, and the constraint is the empirical limit range of the relaxation variable; the optimization methods include: relevant optimization algorithms for finding the minimum value of a constrained nonlinear multivariable function, such as the interior point method, the trust region method, the sequential quadratic programming method, etc.

[0390] The steps of constructing a Bézier curve and performing optimality under the geometric continuity constraint by introducing a relaxation variable as described above can be integrated into an objective function for the value of the relaxation variable. The relaxation variable is used as the independent variable for optimization, and its value range is limited by the empirical method. Further, the optimal value of the relaxation variable is obtained through optimization using relevant optimization algorithms for finding the minimum value of a constrained nonlinear multivariable function, such as the interior point method. Finally, substitute into the above steps to construct the transition fairing curve of the optimal corner path on the Lie group manifold.

[0391] For example, let and X be used as the upper and lower bounds of the value of the optimization variable respectively. Within the upper and lower bounds of the value of the optimized relaxation variable, the relaxation variable is incremented or decremented by a set step size or its value is taken according to the empirical method, and a mapping F(X i ) from the relaxation variable to the curve optimality evaluation value is obtained, which is used as the optimization objective function. The relaxation variable X i = [α i,s , β i,s , α i,e , β i,e T is used as the independent variable for optimization, and a nonlinear optimization problem is constructed as follows:

[0392]

[0393] In the formula:

[0394] represents the upper bound of the value of the relaxation variable used for optimization.

[0395] X represents the lower bound of the value of the relaxation variable used for optimization.

[0396] Further, solve this nonlinear optimization problem through the interior point method to obtain the optimal value of the optimization variable, that is, the optimal relaxation variable

[0397] The remaining path segments to be fairing are fairing one by one according to the methods in step 5 to step 8. The Bézier fairing path curve of the corner path on the quaternion manifold under the optimal geometric continuity constraint can be obtained

[0398] In the formula:​

[0399] denotes the corner point d obtained by non - linear optimization solution i The corresponding optimal slack variable value;

[0400] denotes the corner point d obtained after substituting the optimal slack variable i The optimal generalized Bezier fairing path curve of the corner path at this point;

[0401] denotes the generalized Bezier fairing path curve representing the corner path of the i - th corner point obtained by non - linear optimization solution;

[0402] u * denotes the curve curve parameter of.

[0403] In step 9, the optimal generalized Bezier fairing path curve representing the corner path is transformed back from the Lie group manifold space to the Euclidean space to obtain the final fairing path.

[0404] The optimal fairing curve path obtained in the above steps is inversely transformed back from the Lie group manifold space to the Cartesian pose space to maintain the unified representation with the initial end - pose path curve, and finally combined to obtain the final fairing path.

[0405] Through the inverse transformation of the transformation in step 4, the optimal fairing curve path obtained above is inversely transformed back from the Lie group manifold space to the Cartesian pose space, and then the original path curve is intercepted by the positions of the start and end points obtained in step 3, and finally combined with the optimal fairing curve path segment to obtain the final fairing path.

[0406] Step 9 includes the following method steps:

[0407] According to the inverse transformation of the transformation in step 4, the generalized Bezier fairing path curve representing the corner path obtained by non - linear optimization solution above is inversely transformed from the quaternion space to the attitude space represented by the following Euler angles:

[0408]

[0409] The positions of the start and end points of the path segment to be fairing are used to intercept the original path curve, and let the part of the path that does not need to be fairing be the remaining curve S r (v r ) Then there is:

[0410]

[0411] S a (v a) = S2(v) ∪ … ∪ S i (v) ∪ … ∪ S z-1 (v), v ∈ [v i-1 , v i+1

[0412] S s (v s ) = S2(v) ∪ … ∪ S i (v) ∪ … ∪ S z-1 (v), v ∈ [v i,s , v i,e

[0413] Finally, combine the path parts that do not need to be smoothed with the curve after smoothing the path segments to be smoothed to obtain the final smoothed path as S o :

[0414]

[0415] In the above formulas:

[0416] represents the generalized Bezier smoothed path curve of the turning path at the corner point d i solved by non - linear optimization;

[0417] Quat -1 () represents the function that maps the inverse of the quaternion Lie group element to the end - pose in the corresponding Euclidean space;

[0418] represents the optimal smoothed path curve at the corner point d i in the Euclidean space;

[0419] v * represents the curve parameter of;

[0420] S r (v r ) represents the remaining curve after removing the path segments to be smoothed from the initial end - pose path curve;

[0421] v r represents the curve parameter of S r (v r );

[0422] S s (v s ) represents the set of all path segments to be smoothed on the initial end - pose path curve;

[0423] v s represents the curve parameter of S s (v s ​​The curve parameters of ();

[0424] S a (v a ) represents the curve generated by linearly interpolating all corner points in Euclidean space;

[0425] v a represents the curve parameters of S a (v a )

[0426] represents the curve S a (v a ) with respect to the curve S s (v s ) complement operation;

[0427] S o represents the optimal fairing path in the final obtained Euclidean space.

[0428] Thus, all steps of the end-path fairing method based on Lie group manifold are completed, and a smooth end pose path is obtained.

[0429] In summary, in order to achieve the overall motion smoothness of the end attitude or pose path in the embodiments of the present invention, the original input path is transformed into a smooth Lie group manifold space. Under the constraint of geometric continuity, combined with the construction method of Bezier curves on Lie group manifold, the corner fairing transition path is optimized, the optimal curve fairing path satisfying the given optimal evaluation function is solved, and the continuity and smoothness of the whole path are realized by cutting and splicing with the original path.

[0430] A method for fairing the end path based on Lie group manifold of the present invention receives an initial discrete end pose path, numerically calculates the pose of the starting and ending path points entering and leaving the corner fairing area under the constraint of the fairing area, maps and transforms the corresponding position, attitude and end pose path to the Lie group manifold space, and solves the 0-2 order parameter continuity conditions at the starting and ending points. Then, by introducing slack variables, it is relaxed into the geometric continuity condition on the Lie group manifold, and a Bezier curve connecting the starting and ending points on the Lie group manifold is constructed based on this. The optimality of this curve as a fairing path can be evaluated according to the defined objective function. Thus, the optimal numerical value of the slack variable can be solved by non-linear optimization and substituted into generate the optimal Bezier curve fairing path, and finally inverse mapped to the end position and attitude representation to obtain the final fairing path, thereby effectively smoothing the overall motion of the end attitude or pose path. Thus, the present invention solves the following technical problems: the problem that the non-linear mapping method is used to process the end attitude, resulting in uneven motion of the end attitude path, and the problem that the method of separately processing and then parameter reconciliation is used to handle the coordination of the end position and attitude, resulting in uncoordinated motion of the end pose path.

[0431] Any of the above gradient descent method, genetic algorithm, numerical solution method, iterative algorithm, interior point method, trust region method or sequential quadratic programming method can adopt the algorithms or software modules in the prior art, or adopt the algorithms or functional modules in the prior art, and construct the algorithms or software modules by using conventional technical means.

[0432] The above embodiments are only used to illustrate the technical idea and features of the present invention, and the purpose is to enable those skilled in the art to understand the content of the present invention and implement it accordingly. The patent scope of the present invention cannot be limited only by these embodiments. That is, any equivalent changes or modifications made according to the spirit disclosed in the present invention still fall within the patent scope of the present invention.

Claims

1. An end - path smoothing method based on Lie group manifold, characterized in that, The method includes the following steps: Step 1: Obtain the discrete points of the initial end - pose path in Euclidean space, and set the path smoothing region constraint and the path smoothing objective function. Step 2: Connect the discrete points of the initial end - pose path in Euclidean space in sequence to form an initial end - pose path curve, and the initial end - pose path curve is divided into several path segments by the discrete points. Step 3: According to the path smoothing region constraint, for each path discrete point located at a corner, set the start and end points of the path part to be smoothed on its adjacent two path segments; the path part to be smoothed is called the to - be - smoothed path segment; solve the end - pose of the start and end points of each to - be - smoothed path segment. Step 4: Map the initial end - pose path curve in Euclidean space and the end - pose of the start and end points of each to - be - smoothed path segment to the Lie group manifold space, to obtain the end - pose path parameter curve on the Lie group manifold and the Lie group elements of the end - pose corresponding to the start and end points of each to - be - smoothed path segment. Step 5: Solve the 0 - 2 order parameter continuity conditions at the start and end points of a certain to - be - smoothed path segment in the Lie group manifold space. Step 6: By introducing slack variables, relax the parameter continuity condition to the geometric continuity condition corresponding to the Lie group manifold. Step 7: Under the constraint of satisfying the geometric continuity condition, construct a Bezier curve connecting the start and end points of the to - be - smoothed path segment in the Lie group manifold space. Step 8: Based on the path smoothing objective function, evaluate whether the Bezier curve makes the path smoothing objective function reach the optimal value; if it does not reach the optimal value, optimize and adjust the slack variables, and then turn to Step 6; if it reaches the optimal value, select the Bezier curve as the smoothed path between the start and end points of the to - be - smoothed path segment; then proceed to Step 9. Step 9: Smooth the remaining to - be - smoothed path segments one by one according to the methods of Steps 5 to 8 to obtain a complete smoothed end - pose path curve; transform the complete smoothed end - pose path curve to Euclidean space to obtain the final smoothed end - pose path.

2. The end path smoothing method based on Lie group manifold according to claim 1, wherein In Step 1, the path smoothing region constraint is set as: the points on the smoothed end - path are located within a sphere centered at the path discrete point at the corner within the to - be - smoothed path segment, and the radius of the sphere is set as ε; the minimization of one parameter or a linear combination of several parameters among the four curve parameters of curve curvature, curve torsion, integral of the differential modulus of each order of the curve, and curve length is used as the objective function.

3. The end path smoothing method based on Lie group manifold according to claim 1, wherein In Step 3, according to the path smoothing region constraint, solve the end - pose of the start and end points of each to - be - smoothed path segment by the following method: Construct a path segment connecting two adjacent path discrete points in Cartesian space according to the adjacent end - pose path discrete points and the corresponding motion mode. Construct a non - linear equation with the distance between the points on the path segments on both sides of the path discrete point and the path discrete point being the smoothing constraint, and find the root by numerical solution to determine the poses of the start and end points of the to - be - smoothed path segments on both sides of the path discrete point.

4. The end path smoothing method based on Lie group manifold according to claim 1, characterized in that, In Step 4, represent the discrete points of the initial end - pose path in Euclidean space and the end - pose of the start and end points of each to - be - smoothed path segment by direction vectors and / or Euler angles. Represent the end - pose path parameter curve mapped to the Lie - group manifold space and the Lie - group elements of the end - poses of the start and end points of each path segment to be smoothed using rotation matrices, quaternions, homogeneous matrices, and / or dual quaternions.

5. The end path smoothing method based on Lie group manifold according to claim 1, characterized in that Step 5 includes the following sub - steps: Step 5 - 1: Construct a vector tangent space at the start and end points according to each path segment to be smoothed in the Lie - group manifold space and its start and end - pose end - poses. Step 5 - 2: Based on the vector tangent space, calculate the 0 - 2 order differentials of the end - pose path parameter curve on the corresponding Lie - group manifold at the start and end points of each path segment to be smoothed, represented by the corresponding Lie - group or Lie - algebra elements. Step 5 - 3: Organize and summarize the 0 - 2 order differential values at the start and end points into a set representing the parameter continuity conditions.

6. The end path smoothing method based on Lie group manifold according to claim 1, wherein Step 6 includes the following sub - steps: Step 6 - 1: Construct a relaxation rule for converting the 0 - 2 order parameter continuity conditions in the Lie - group manifold space to relaxed geometric continuity conditions according to the principle of invariant Cartesian - space points, tangent vectors, and curvatures. Step 6 - 2: Based on this relaxation rule, convert the Lie - group elements, tangent vectors, and pseudo - curvatures in the Lie - group manifold space to relaxed geometric continuity conditions, and determine the set of geometric continuity conditions at the start and end points of each path segment to be smoothed on the Lie - group manifold according to this rule and the relaxation variables.

7. The end path smoothing method based on Lie group manifold according to claim 6, wherein Let the i-th path point be a corner point, d i represents the i-th path point, where i = 2, 3, …, z - 1; i represents the path point serial number; z is the number of path points; In the Lie - group manifold space, establish the following relaxation rule under the conditions of invariant Lie - group elements, tangent vectors, and pseudo - curvatures: According to this relaxation principle, construct the corresponding geometric continuity conditions as follows: Taking α i,s , β i,s , α i,e , β i,e as independent variables, the following relational expressions are further obtained: In the formula: H C0 、H C1 、H C2 correspond to the 0th, 1st, and 2nd order parameter continuity condition vectors of the end pose path parameter curve in sequence; A unit vector corresponding to a first-order parametric continuity condition vector representing an end pose path parameter curve; (H C1 ×H C2 ) ∧ represents the unit vector of the cross product of the 1st and 2nd order parameter continuity condition vectors of the end pose path parameter curve; κ represents the pseudo - curvature of the end - pose path parameter curve; α, β represent the undetermined relaxation - variable parameters of the end - pose path parameter curve; H G0 、H G1 、H G2 correspond to the 0th, 1st, and 2nd order geometric continuity condition vectors of the end pose path in sequence; correspondingly represent the corner points d of the end pose path parameter curve in sequence i the 0th, 1st, and 2nd order parameter continuity condition vectors of the starting point of the path segment to be smoothed at the position Denote the unit vector of the first-order parametric continuity condition vector of the starting point of the path segment to be fairing at the corner point d of the end pose path parameter curve i ; Indicates the unit vector of the cross product of the first- and second-order parameter continuity condition vectors at the starting point of the path segment to be smoothed at the corner point d of the end pose path parameter curve; i ​ κ i,s Denotes the pseudo-curvature of the starting point of the path segment to be fairing at the corner point d of the end pose path parameter curve i ; α i,s and β i,s represent the undetermined relaxation variable parameters of the starting point of the path segment to be fairing at the corner point d i of the end pose path parameter curve; correspondingly represent the corner points d of the end pose path in sequence i the 0th, 1st, and 2nd order geometric continuity condition vectors of the starting point of the path segment to be fairing at correspondingly represent the corner points d of the end pose path parameter curve in sequence i the 0th, 1st, and 2nd order parameter continuity condition vectors of the starting point of the path segment to be fairing at Indicates the unit vector of the first-order parameter continuity condition vector of the starting point of the path segment to be smoothed at the corner point d of the end pose path parameter curve i ; Denote the unit vector of the cross product of the first- and second-order parametric continuity condition vectors at the starting point of the path segment to be fairing at the corner point d i of the end pose path parameter curve; κ i,e Indicates the pseudo-curvature of the starting point of the path segment to be fairing at the corner point d of the end pose path parameter curve i ; α i,e 、β i,e represent the undetermined relaxation variable parameters of the starting point of the path segment to be fairing at the corner point d i of the end pose path parameter curve; correspondingly represent the corner points d of the end pose path in sequence i the 0th, 1st, and 2nd order geometric continuity condition vectors of the starting point of the path segment to be fairing at Indicates the geometric continuity condition function of the end pose path at the corner point d i ; X i represents the corner point d i The slack variable consists of the slack variable parameters to be determined.

8. The end path smoothing method based on Lie group manifold according to claim 1, characterized in that In step 7, set the optimization objective as the path - smoothing objective function; set the constraint as the empirical limit range of the relaxation variables; optimize and select the relaxation variables on the Lie - group manifold according to one of the following methods: Method 1: Derive a calculation formula for the objective function that is convenient for computer operation through an analytical method, substitute multiple groups of relaxation variables to calculate the objective function, and obtain a group of relaxation variables with the optimal corresponding objective - function value. Method 2: Form a linear equation system with the relaxation variables as the parameters to be solved, and use a numerical solution method to calculate and solve the characteristic parameters of each order of the B - spline curve that meets the path - smoothing optimization objective; obtain an optimal set of relaxation - variable solutions. Method 3: Use the gradient - descent method or genetic algorithm to select the relaxation variables, evaluate the B - spline curve constructed by the selected relaxation variables using the path - smoothing objective function, and select a set of better relaxation variables according to the evaluation results. Method 4: Use an iterative algorithm to iterate the relaxation variables, calculate new relaxation variables according to the current set of relaxation variables in each iteration; after each iteration, evaluate the B - spline curve constructed by the current relaxation variables using the path - smoothing objective function, and iterate repeatedly until the B - spline curve constructed by the current relaxation variables makes the path - smoothing objective function reach the optimal value or the iteration value reaches the set threshold.

9. The end path smoothing method based on Lie group manifold according to claim 8, characterized in that, In step 8, the method for evaluating whether the B - spline curve meets the requirements of the smoothing objective includes the following method steps: Let the i-th path point be a corner point, d i represents the i-th path point, where i = 2, 3, …, z - 1; i represents the path point sequence number; z is the number of path points; Set the following path - smoothing objective function: the integral of the norm of the third - order differential of the path - smoothing curve is the smallest; then take the minimum integral of the norm of the third - order differential as the objective function for evaluating whether the B - spline curve is optimal. Let represent the Bezier curve for smoothing the corner point d i at the turning path; F(X i ) represents the objective function for evaluating the optimality of the Bezier curve; without changing the optimality, converting the norm into the form of inner product, we have: According to the definition of the inner product on the Lie group manifold, expand and simplify the objective function to: In the above formulas: u B represents the Bessel curve parameter; X i Indicates the corner point d i The slack variable composed of the undetermined slack variable parameters; Represents a Bessel curve For the Bessel curve parameter u B The third-order differential of; ω i,B represents the vector of the lengths of the components of the first-order derivative of the Bézier curve on the basis vectors of the Lie algebra tangent space; Indicates the vector of the length of the component of the second-order derivative of the Bézier curve on the basis vectors of the Lie algebra tangent space; ​ Denote the Bézier curve The vector of the lengths of the components of the third-order derivative on the basis vectors of the Lie algebra tangent space; Use the composite Simpson's method or Romberg extrapolation method to perform numerical integration to solve the value of the objective function, and evaluate the optimality of the fairing path of the Bessel curve.

10. An apparatus for an end - path smoothing method based on Lie group manifolds, comprising a memory and a processor, characterized in that, The memory is used to store a computer program; the processor is used to execute the computer program and implement the steps of the end path fairing method based on the Lie group manifold as described in any one of claims 1 to 9 when executing the computer program.

Images