Arc length parameterization method and motion track planning method based on curve fitting

Abstract

The invention discloses an arc length parameterization method and a motion track planning method based on curve fitting. The method comprises the following steps: collecting a motion point position sequence; performing curve fitting on the motion point data to obtain a first motion curve of each motion point, and calculating the arc length of each point on the first motion curve to obtain a first motion curve set with parameterized arc lengths; a Cartesian space point location and a corresponding first motion curve are extracted; smoothing the first motion curve of the Cartesian space point locations to obtain a second motion curve, and calculating the arc length of each point on the second motion curve to obtain a second motion curve set with parameterized arc lengths; and determining motion parameters of the joint space point location according to the first motion curve and determining motion parameters of the joint space point location according to the second motion curve. Compared with the prior art, the method has the advantages that the mixed point locations can be processed, the tangent point density can be adaptively adjusted in the processing process, and the fitting precision and efficiency are high.

Description

Technical Field

[0001] This invention relates to the field of robot motion trajectory planning technology, and in particular to an arc length parameterization method and motion trajectory planning method based on curve fitting. Background Technology

[0002] Currently, the points input to the teach pendant for industrial robots are mostly discrete, and they combine data from joint space (the space that directly controls the angles of each joint of the robotic arm) and Cartesian space (the space that describes the position and orientation of the end effector in three-dimensional space). For trajectory planning in joint space, the core task is to construct a function of joint angles changing over time to directly guide the movement of each joint of the robotic arm, ensuring the smoothness and continuity of the motion trajectory. For trajectory planning in Cartesian space, it is first necessary to use inverse kinematics algorithms to map the target position in Cartesian space back to joint space, and then further construct the time function of joint angles. In motion trajectory planning, it is usually obtained from functions of motion variables such as displacement, velocity, acceleration, and jerk over time. Before obtaining the expected motion trajectory, it is necessary to fit the input discrete points into parametric curves for interpolation and trajectory planning. In practical applications, the input discrete points are often insufficient to directly describe complex motion trajectories; therefore, parametric techniques are needed to fit these discrete points into continuous parametric curves.

[0003] However, trajectory planning is extremely difficult because curves or surfaces often lack direct closed-form expressions in mathematics, making it extremely difficult to calculate any point and curve length directly in closed form. To solve this problem, the concept of arc length parameterization was introduced in the field of trajectory planning. By using the arc length of the curve as a natural parameter, the position of each point on the curve can be matched with its corresponding arc length value. In this process, the selection and generation of the tangent point is crucial and affects the accuracy and efficiency of trajectory planning.

[0004] Existing arc length parameterization processes typically select tangent points at equal intervals or at equal times, failing to adaptively adjust the density of tangent points based on curve curvature changes. The density of tangent points remains consistent regardless of whether the curve has high or low curvature. This not only affects the accuracy of subsequent trajectory planning but also increases unnecessary computational burden, leading to tangent point redundancy and reduced computational efficiency, ultimately resulting in decreased path planning efficiency. Summary of the Invention

[0005] In view of this, the purpose of this invention is to provide an arc length parameterization method and a motion trajectory planning method based on curve fitting, so as to solve the problem that the accuracy and efficiency of subsequent trajectory planning are low due to the inability to adaptively adjust the density of tangent points during the arc length parameterization process in the prior art.

[0006] To achieve the above objectives, one technical solution of the present invention provides an arc length parameterization method based on curve fitting, comprising the following steps:

[0007] The robot's motion point data is collected to form a motion point sequence, wherein the motion point data includes data of several joint space points of the robot in joint space and data of several Cartesian space points in Cartesian space;

[0008] Curve fitting is performed on the motion point data to obtain the first motion curve for each motion point, and the arc length of each point on the first motion curve is calculated to obtain the first motion curve set parameterized by arc length.

[0009] Extract the Cartesian space points and their corresponding first motion curves;

[0010] The first motion curve of the Cartesian space point is smoothed, and the Cartesian space point is converted to the joint space point to obtain the second motion curve of the corresponding joint space point. The arc length of each point on the second motion curve is calculated to obtain the set of second motion curves parameterized by arc length.

[0011] Based on the first motion curve, the motion parameters of the joint space point are determined, and based on the second motion curve, the motion parameters of the joint space point corresponding to the Cartesian space point are determined, thereby obtaining the correspondence between the motion point and the corresponding curve after arc length parameterization.

[0012] Furthermore, in the step of performing curve fitting on the motion point data to obtain the first motion curve for each motion point, and calculating the arc length of each point on the first motion curve to obtain the first motion curve set parameterized by arc length, the step of fitting the motion point data with Bezier curves to obtain the first motion curve includes the following sub-steps:

[0013] The motion point data in the motion point sequence is extracted according to the first preset window size, and a Bezier curve is obtained by fitting the extracted motion point data.

[0014] Integrate the Bezier curve to obtain the length of the Bezier curve, reparameterize the Bezier curve to obtain the first motion curve, and calculate the arc length of each point on the first motion curve to obtain the first motion curve parameterized by arc length.

[0015] The first window is slid according to the first preset step size, and the Bezier curve fitting and arc length are repeatedly performed until the first window completely covers the motion point sequence, resulting in a series of arc length parameterized first motion curves, forming a first motion curve set.

[0016] Furthermore, in the step of extracting motion point data from the motion point sequence according to the first preset window size, and fitting a Bezier curve based on the extracted motion point data, the Bezier curve is limited to a fourth-order Bezier curve. The specific method for fitting the fourth-order Bezier curve is as follows:

[0017] According to the first preset window size, the motion point data in the motion point sequence is extracted, wherein the motion point data is six-dimensional data, wherein the Cartesian space point data includes three translational degrees of freedom and three rotational degrees of freedom, and the joint space point data includes six joint angle data;

[0018] The motion point data is processed according to the number of control points required to fit the fourth-order Bezier curve to obtain the curve fitting control points.

[0019] A fourth-order Bezier curve is obtained by curve fitting based on the curve fitting control points.

[0020] Furthermore, in the steps of integrating the Bezier curve to obtain its length, reparameterizing the Bezier curve to obtain a first motion curve, and calculating the arc length at each point on the first motion curve to obtain the arc length-parameterized first motion curve, the specific method for obtaining the arc length-parameterized first motion curve is as follows:

[0021] Define a first integral function, wherein the first integral function is restricted to the Gauss-Legendal integral function;

[0022] Determine the range of values ​​for the fourth-order Bezier curve and the integration interval of the first integral function, and convert the range of values ​​into the integration interval;

[0023] The fourth-order Bezier curve is integrated over the integration interval using the Gauss-Legend integration function to obtain the length of the fourth-order Bezier curve. Based on the length of the fourth-order Bezier curve, the fourth-order Bezier curve is reparameterized to form the first parameter curve.

[0024] Calculate the arc length at each point on the first parameter curve to obtain the first motion curve parameterized by arc length.

[0025] Furthermore, in the steps of smoothing the first motion curve of the Cartesian space point and converting the Cartesian space point to the joint space point to obtain the corresponding second motion curve of the joint space point, and calculating the arc length of each point on the second motion curve to obtain the set of arc length parameterized second motion curves, the second motion curve is obtained by piecewise fitting of the first motion curve using a polynomial, including the following sub-steps:

[0026] Several sampling point data are sequentially collected on the first motion curve corresponding to the Cartesian space point, and inverse kinematics calculation is performed on the Cartesian space point to convert the Cartesian space point to the joint space point, forming a sampling point sequence.

[0027] The sampling point data in the sampling point sequence is extracted according to the second preset window size, and a polynomial curve is obtained by fitting the extracted sampling point data, wherein the extracted sampling points include a marker site.

[0028] Integrate the polynomial curve to obtain the length of the polynomial curve, reparameterize the polynomial curve to obtain the second motion curve, and calculate the arc length of each point on the second motion curve to obtain the second motion curve parameterized by arc length.

[0029] Determine whether the marker site is on the second motion curve. If it is, slide the second window according to the second preset step size and repeat the polynomial fitting backward until the second window completely covers the sampling point sequence and outputs a series of arc-length parameterized second motion curves. Otherwise, iterate forward to perform polynomial fitting until the determined marker site is located on the second motion curve or the set maximum number of iterations is reached.

[0030] Furthermore, in the step of extracting sampling point data from the sampling point sequence according to the second preset window size, and fitting a polynomial curve based on the extracted sampling point data, a third-order polynomial is used to perform piecewise fitting on the sampling point data. The specific method for fitting the polynomial curve is as follows:

[0031] The sampling point data in the sampling point sequence is extracted according to the second preset window size, and the midpoint between two adjacent sampling points is determined by the binary search method. The median of the extracted sampling point data is determined as the marker point, wherein the marker point does not participate in curve fitting.

[0032] Define a third-order polynomial function, wherein the third-order polynomial function includes four unknown coefficients;

[0033] The independent variable matrix of the third-order polynomial function is constructed based on the extracted sampling point data and midpoint, and the independent variable matrix is ​​scaled according to a preset scaling factor to obtain the scaled independent variable matrix.

[0034] The observation vector is determined based on the extracted sampling point data and midpoint, and the coefficients of the third-order polynomial function are solved based on the scaled independent variable matrix to obtain the third-order polynomial curve.

[0035] Furthermore, in the step of determining whether the marker site is on the second motion curve, the specific method is as follows:

[0036] Extract the arc length of the marker point, and determine the relative points with the same arc length on the second motion curve based on the arc length;

[0037] Determine whether the positional error between the marker point and the relative point is within a preset threshold range;

[0038] If the position error is within the preset threshold range, the second window is slid according to the second preset step size to extract a new set of sampling points, and polynomial fitting, arc length calculation and arc length judgment are repeatedly performed until the second window completely covers the sampling point sequence, and a series of arc length parameterized second motion curves are obtained to form a set of second motion curves.

[0039] If the position error is not within the preset threshold range, then the starting point and other sampling points, midpoints or marker points other than the ending point among the extracted sampling points are used as endpoints. New midpoints and marker points are re-determined based on the newly determined endpoints. Then, iterative fitting, arc length calculation and arc length judgment are performed using the new endpoints, midpoints and marker points until the determined marker point is located on the second motion curve or the set maximum number of iterations is reached.

[0040] Furthermore, the specific method for determining the motion parameters of the joint spatial points based on the first motion curve, and obtaining the correspondence between the joint spatial points and the corresponding curves after arc length parameterization, is as follows:

[0041] Extract the first motion curves corresponding to the joint space points from the first set of motion curves;

[0042] Calculate the first and second derivatives of the first motion curve;

[0043] The motion parameters of the joint space points in the joint space are obtained by normalizing the modulus of the first derivative of the first motion curve.

[0044] The correspondence between the joint spatial points and the corresponding curves after arc length parameterization is determined based on the joint spatial points and their motion parameters.

[0045] Furthermore, the specific method for determining the motion parameters of the joint space points corresponding to the Cartesian space points based on the second motion curve, and obtaining the correspondence between the joint space points after the Cartesian space point transformation and the corresponding curve after arc length parameterization, is as follows:

[0046] Extract the second motion curve;

[0047] Calculate the first and second derivatives of the second motion curve;

[0048] The motion parameters of the joint space points in the joint space are obtained by normalizing the magnitude of the first derivative of the second motion curve. The motion parameters of the joint space points are then scaled according to the preset scaling factor so that the motion parameters of the joint space points match the magnitude of the actual motion parameters of the robot.

[0049] The correspondence between the joint spatial points and the corresponding curves after arc length parameterization is determined based on the joint spatial points and their motion parameters.

[0050] To achieve the above objectives, another technical solution of the present invention provides a motion trajectory planning method, comprising the following steps:

[0051] The arc length parameterization method based on curve fitting, as described above, is used to obtain the correspondence between the moving point and the corresponding curve after arc length parameterization.

[0052] Based on the correspondence between the motion points and the corresponding curves after arc length parameterization, the robot's position, velocity, acceleration, and jerk are planned to obtain the motion data of each joint of the robot in time and fit the motion trajectory.

[0053] This invention combines Bezier curve fitting for the first motion curve of joint space points with a cubic polynomial fitting for the second motion of Cartesian space points, processing dimensions from coarse to fine. This not only ensures the continuity and smoothness of the robotic arm's joint movements but also significantly improves the accuracy of connecting mixed points in complex task spaces. Simultaneously, by combining Bezier curve and cubic polynomial curve fitting methods, the curves are parameterized for arc length. The processing method, which involves tangent points one iteration at a time, effectively reduces the number of tangent points in the front-end processing, thereby simplifying the calculation process and reducing computational complexity. Furthermore, this invention calculates the first and second derivatives of each point on the curve, establishing a correspondence between points and parameters, providing rich dynamic information for subsequent trajectory planning. Attached Figure Description

[0054] Figure 1 This is a flowchart of an arc length parameterization method based on curve fitting according to an embodiment of the present invention.

[0055] Figure 2 This is a flowchart of step S2.

[0056] Figure 3 This is a flowchart of step S4.

[0057] Figure 4 This is a schematic diagram of the sampling points during polynomial piecewise fitting in step S4.

[0058] Figure 5 This is a schematic diagram of a polynomial piecewise fitting curve, which is a specific example of the arc length parameterization method based on curve fitting of the present invention.

[0059] Figure 6 This is a schematic diagram of the final curve point location of the arc length parameterization method based on curve fitting of the present invention. Detailed Implementation

[0060] The following detailed description illustrates the specific implementation method:

[0061] Example

[0062] Please refer to Figure 1 This is a flowchart of an arc length parameterization method based on curve fitting according to an embodiment of the present invention. The arc length parameterization method based on curve fitting in this embodiment is mainly used to parameterize the arc length when the input points in the robot's trajectory planning process are a mixture of joint space points and Cartesian space points. First, a first motion curve is obtained by curve fitting the joint space points and Cartesian space points using Bezier curves, and the arc length corresponding to each point on the first motion curve is calculated, thus parameterizing the arc length of the first motion curve. Then, for the Cartesian space points, a polynomial is used again to perform piecewise fitting on each point on the first motion curve corresponding to the obtained Cartesian space points, resulting in... A smooth second motion curve with approximate arc length and small chord height difference is obtained, and the arc length corresponding to each point on the second motion curve is calculated. The second motion curve is then parameterized by arc length. Finally, based on the first motion curve parameterized by arc length, the first derivative (i.e., displacement or position) and second derivative (i.e., velocity) of each joint space point on the first motion curve are solved. Based on the second motion curve parameterized by arc length, the first derivative and second derivative of the joint space point corresponding to each Cartesian space point on the second motion curve are solved, and the corresponding relationship is obtained to facilitate the subsequent planning of motion trajectories with dynamic constraints.

[0063] Specifically, the arc length parameterization method based on curve fitting in this embodiment includes the following steps:

[0064] S1: Collect motion point data to form a motion point sequence.

[0065] The robot's motion point data is collected to form a motion point sequence. The motion point data includes data on several joint space points in joint space and several Cartesian space points in Cartesian space. The joint space points and Cartesian space points are mixed and discrete. In this embodiment, the motion point data is six-dimensional data. The joint space point data includes six joint angle data, and the Cartesian space point data includes three translational degrees of freedom and three rotational degrees of freedom data.

[0066] S2: Fit the first motion curve.

[0067] Curve fitting is performed on the motion point data to obtain the first motion curve for each motion point, and the arc length of each point on the first motion curve is calculated to obtain the first motion curve set parameterized by arc length. In this embodiment, Bezier curves are used to fit the motion point data to obtain the first motion curve. Bezier curves can process motion point data that mixes joint space points and Cartesian space points. In specific implementation, since the robot's motion includes linear motion and curvilinear motion, both joint space points and Cartesian space points contain points corresponding to linear and curvilinear motions. When performing Bezier curve fitting, Bezier curves should be applied to both the linear and curvilinear points in the joint space and Cartesian space points respectively to obtain the Bezier curve set corresponding to each motion point.

[0068] Furthermore, when performing Bezier curve fitting, for joint space points, the collected joint space point data can be directly fitted. However, since the Cartesian space point data contains the robot joint posture data, the quaternions of the Cartesian space point data need to be preprocessed before fitting, and then mapped to the joint space before fitting. The specific method for mapping the Cartesian space point data to the joint space can be implemented using existing technologies, which will not be elaborated here.

[0069] Please refer to Figure 2 Step S2 includes the following sub-steps:

[0070] S201: Fit the motion point data to a Bezier curve.

[0071] Motion point data is extracted from the motion point sequence according to a first preset window size, and a Bezier curve is fitted based on the extracted motion point data. In this embodiment, considering both the accuracy and efficiency of data processing, the Bezier curve is limited to a fourth-order Bezier curve. The specific method for fitting a fourth-order Bezier curve is as follows:

[0072] First, select the motion point data.

[0073] Extract motion point data from the motion point sequence according to the first preset window size, denoted as p. i1 This represents the i1th motion point data extracted according to the first preset window, where i1 = 0, 1, ..., I1-1, I1 is the number of extracted motion point data, which is also the size of the first window, and I1 is a positive integer. The three extracted motion point data can contain only joint space point data, only Cartesian space point data, or a mixture of joint space point data and Cartesian space point data, to achieve data processing for inputs that are a mixture of joint space point data and Cartesian space point data.

[0074] For example, the first preset window size is set to "3", that is, the motion point data is extracted starting from the first motion point data (i.e., p0) in the motion point sequence, and three motion point data are extracted at a time, denoted as p0, p1, p2.

[0075] Then, the motion point data is expanded.

[0076] The motion point data is processed according to the number of control points required for fitting the Bezier curve, and the curve fitting control points are obtained, denoted as P. i2 Let represent the i2th curve fitting control point, where i2 = 0, 1, ..., I2-1, and I2 is the number of control points required to fit the Bezier curve, i.e., the number of curve fitting control points, and I2 is a positive integer. Specifically, since this embodiment uses a fourth-order Bezier curve for fitting, five control points are required during fitting. However, in order to reduce the number of tangent points and improve trajectory planning efficiency, the number of motion point data extracted is usually less than the number of control points. In this case, it is necessary to expand the extracted motion point data so that the number of motion point data used for fitting matches the number of control points for fourth-order Bezier curve fitting.

[0077] For example, the extracted three motion point data points p0, p1, p2 are expanded into five curve fitting control points, denoted as P0, P1, P2, P3, P4. In this embodiment, the formula for expanding the three motion point data points p0, p1, p2 into five curve fitting control points P0, P1, P2, P3, P4 is as follows:

[0078]

[0079] Where w represents the dimension of the motion point data, w = 0, 1, ..., 5; P0[w], P1[w], P2[w], P3[w], P4[w] are the six-dimensional data corresponding to the five curve fitting control points P0, P1, P2, P3, P4 respectively; p0[w], p1[w], p2[w] are the six-dimensional data corresponding to the three motion point data p0, p1, p2 respectively.

[0080] It is understood that the above data expansion process is only one example of data expansion in this embodiment. In specific implementation, other methods can also be used, including but not limited to interpolation, data duplication and variation, model generation of new data, data merging, etc.

[0081] Finally, a fourth-order Bezier curve is fitted.

[0082] A fourth-order Bezier curve is obtained by curve fitting based on the curve fitting control points. In this embodiment, the fourth-order Bezier curve is represented as:

[0083] B(t)=(1-t) 4 P0+4(1-t) 3 tP1+6(1-t) 2 t 2 P2+4(1-t)t 3 P3+t 4 P4,t∈[a,b] (2)

[0084] Wherein, B(t) is the fitted fourth-order Bezier curve; t is the fitting parameter (or variable) of the fourth-order Bezier curve, and [a,b] is the range of values ​​for the fitting parameter t of the fourth-order Bezier curve. In this embodiment, for example, the range of values ​​for the fitting parameter t of the fourth-order Bezier curve is preferably [0,1], that is, a=0,b=1.

[0085] S202: Calculate the arc length by integrating the Bezier curve using the first integral function.

[0086] Integrating the Bezier curve yields its length. The Bezier curve is then reparameterized to obtain a first motion curve. The arc length at each point on this first motion curve is calculated to obtain an arc-length-parameterized first motion curve. In this embodiment, the specific method for obtaining the arc-length-parameterized first motion curve is as follows:

[0087] First, define the first integral function.

[0088] In this embodiment, the first integral function is limited to the Gauss-Legendal integral function. Using the Gauss-Legendal integral function allows for higher accuracy calculation results with fewer integration nodes, thereby improving the accuracy of arc length calculations at points on the first motion curve. For example, the general quadrature formula for the Gauss-Legendal integral function is expressed as:

[0089]

[0090] Where f(x) is the integrand of the Gauss-Legendé integral function, with an integration interval of [-1, 1]; x is the parameter of the integrand; x k Let A be the k-th integration node, also known as the Gaussian point; k Let K be the weight coefficient corresponding to the kth integration node, where k = 0, 1, ..., K-1, K is the number of integration nodes, and K is a positive integer.

[0091] To ensure the accuracy of the obtained curve length, this embodiment uses eight integration nodes, and the integration nodes and their corresponding weight coefficients are shown in Table 1 below.

[0092] Table 1

[0093]

[0094] Understandably, in some other embodiments, the first integration function may also be other integration functions, including but not limited to one or more functions selected from polynomials, trigonometric functions, exponential functions, and logarithmic functions.

[0095] Then, the range of values ​​of the fourth-order Bezier curve is mapped to the integration range of the first integral function.

[0096] The range of values ​​for the fourth-order Bezier curve and the integration interval of the first integral function are determined, and the range of values ​​is converted into the integration interval. In this embodiment, the range of values ​​for the fourth-order Bezier curve is the range of values ​​for the fitting parameter t of the Bezier curve, i.e., [a,b] (i.e., [0,1]), while the integration interval of the Gauss-Legend integral function is [-1,1]. Therefore, before integrating the fourth-order Bezier curve, its range of values ​​needs to be converted, mapping it to the integration interval of the Gauss-Legend integral function. The mapping relationship between the range of values ​​for the fourth-order Bezier curve and the integration interval of the Gauss-Legend integral function is as follows:

[0097]

[0098] Where t is the fitting parameter t of the Bezier curve, which varies within the range [a,b]; x is the parameter of the Gauss-Legendal integral function, which varies within the integration interval [-1,1].

[0099] Through the above mapping relationship, when x = -1, t = a, and when x = 1, t = b, so as to transform the range of values ​​of the fourth-order Bezier curve into the integration range of the Gauss-Legend integral function, ensuring that the range of values ​​of the fourth-order Bezier curve matches the integration range of the Gauss-Legend integral function.

[0100] Next, the fourth-order Bezier curve is integrated to calculate the curve length.

[0101] The length of the fourth-order Bezier curve is obtained by integrating the fourth-order Bezier curve over the integration interval using the Gauss-Legend integral function. Based on the length of the fourth-order Bezier curve, the curve is reparameterized to form the first parameter curve.

[0102] Specifically, based on the general quadrature formula of the Gauss-Legendé integral function and the mapping relationship between the range of values ​​of the fourth-order Bezier curve and the integration interval of the Gauss-Legendé integral function, the integrand of the Gauss-Legendé integral function when integrating over the fourth-order Bezier curve is determined, namely:

[0103]

[0104] The length of the fourth-order Bezier curve is obtained by integrating it over the integration interval. In this embodiment, the length of the fourth-order Bezier curve is expressed as:

[0105]

[0106] Where L is the length of the fourth-order Bezier curve.

[0107] The length of the fourth-order Bezier curve is calculated according to the above formula (6) so as to reparameterize the fourth-order Bezier curve.

[0108] For example, in this embodiment, the range of values ​​for the fitting parameter t of the Bezier curve is [a,b], which is [0,1]. Thus, the length of the fourth-order Bezier curve in this embodiment can be further expressed as:

[0109]

[0110] Finally, calculate the arc length at each point on the first parameter curve.

[0111] Calculate the arc length at each point on the first parameter curve to obtain the first motion curve parameterized by arc length. Specifically, according to the above formula (6), t is taken and solved in the range of values ​​to obtain the arc length at each motion point on the first parameter curve. The correspondence between the arc length at each motion point and the original fitting parameter t is established. The calculated arc length is used to replace the original parameter on the first parameter curve (that is, the arc length at each point is used to replace the fitting parameter t of the Bezier curve) to obtain the first motion curve parameterized by arc length.

[0112] S203: Repeated fitting to form the first set of motion curves.

[0113] The first window slides according to a first preset step size, repeatedly performing Bezier curve fitting and calculating arc length until the first window completely covers the motion point sequence, resulting in a series of arc-length parameterized first motion curves, forming a first motion curve set. For example, the first preset step size is set to "1", meaning the first window slides one data point backward in the motion point sequence, retrieves three more motion point data points, and repeats steps S201 and S202 to obtain another arc-length parameterized first motion curve, until the first window slides to the last motion point data point in the motion point sequence, obtaining the last arc-length parameterized first motion curve, and then stops. In this way, a series of segmented arc-length parameterized first motion curves are obtained, forming a first motion curve set.

[0114] S3: Extract the points in Cartesian space and their corresponding first motion curves.

[0115] Since the Cartesian space point data contains the robot's position and orientation data in Cartesian space, to further ensure the fitting accuracy of the curves, after fitting a series of segmented first motion curves, a smoothing process needs to be applied again to the first motion curves corresponding to the Cartesian space points and their sampling points to ensure fitting accuracy. Specifically, the Cartesian space points in the motion point sequence are determined, and the Cartesian space points and their corresponding first motion curves are extracted for use in the subsequent fitting of the second motion curves.

[0116] S4: Fit the second motion curve.

[0117] The first motion curve of the Cartesian space points is smoothed, and the Cartesian space points are transformed into joint space points to obtain the second motion curve of the joint space points corresponding to the Cartesian space points. The arc length of each point on the second motion curve is calculated to obtain the set of second motion curves parameterized by arc length. In this embodiment, a polynomial is used to piecewise fit the first motion curve to obtain the second motion curve. The polynomial fitting method can perform a second fitting of the first motion curve to improve the fitting accuracy. At the same time, during the fitting process, only a small number of sampling points are needed for polynomial iterative fitting. When the fitted polynomial does not meet the conditions, it is iterated once, and the cutting point is moved forward once simultaneously. Once the fitted polynomial meets the conditions, the iteration is completed and the cutting point is stopped. In this way, adaptive cutting points and fitting can be performed according to the curvature of the first motion curve.

[0118] Please refer to Figure 3 Step S4 includes the following sub-steps:

[0119] S401: Collect sampling point data to form a sampling point sequence.

[0120] The first motion curve corresponding to the extracted Cartesian space points is sampled. Specifically, starting from the endpoint (starting point) of the first motion curve, a series of sampling points are collected sequentially, and the data of the sampling points are obtained to form a sampling point sequence. In this embodiment, since the collected data is Cartesian space data, in order to unify the data during subsequent trajectory planning, the Cartesian space data needs to be subjected to inverse kinematics calculations to convert the Cartesian space data into the corresponding joint space data before polynomial fitting.

[0121] S402: Select sampling points and perform polynomial fitting.

[0122] Sampling point data is extracted from the sampling point sequence according to the second preset window size, and a polynomial curve is obtained by fitting the extracted sampling point data. In this embodiment, a third-order polynomial is used to fit the sampling point data piecewise. The specific method for fitting the polynomial curve is as follows:

[0123] First, sampling points are selected and then amplified.

[0124] Extract sampling point data from the sampling point sequence according to the second preset window size, denoted as c. i3 This represents the data of the i3th sampling point extracted according to the second preset window, where i3 = 0, 1, ..., I3-1, I3 is the number of extracted sampling points, which is also the size of the second window, and I3 is a positive integer. The selected sampling points are ordered as c0, c1, ..., c... I3-1 The midpoint between two adjacent sampling points is determined sequentially using the binary search method, denoted as . This represents the i-4th midpoint, which is also the... The sampling point and the first The midpoints of the sampling points are defined, where i4 = 0, 1, ..., I4-1, I4 is the number of midpoints, and I4 = I3-1, where I4 is a positive integer. Furthermore, the extracted sampling point data... The sampled data includes a marker site that does not participate in curve fitting but is used to verify the curve fitting results after the curve fitting is completed. In this embodiment, the extracted sampling point data... The median in the middle is determined as the marker locus, that is... It was identified as a marker site.

[0125] For example, the second preset window size is set to "3", meaning that sampling point data is extracted starting from the first sampling point data (i.e., c0) in the sampling point sequence, extracting three sampling point data at a time, denoted as c0, c1, and c2. Then, a binary search method is used to find the midpoint m0 ​​of c0 and c1 and the midpoint m1 of c1 and c2, respectively, obtaining the positional relationship between the sampling points and the midpoints as follows: Figure 4 As shown, the points are c0, m0, c1, m1, and c2 in sequence. After determining c1 as the marker site, in subsequent steps, a third-order polynomial fitting is performed on the four points c0, m0, m1, and c2.

[0126] Then, define a third-order polynomial function.

[0127] A third-order polynomial function has four unknown coefficients. The general expression for a third-order polynomial is:

[0128] y = Au 3 +Bu 2 +Cu+D (8)

[0129] Where y is a third-order polynomial; u is the independent variable of the third-order polynomial; A, B, C, and D are the unknown coefficients of the third-order polynomial, including a constant, namely D.

[0130] Next, construct the matrix of independent variables.

[0131] For example, for the sampled point data and the midpoint data c0,m0,m1,c2, the expression based on the third-order polynomial can be further expressed as follows:

[0132] Based on the above sampling point data and midpoint data, construct the independent variable matrix of the third-order polynomial function. The independent variable matrix is ​​represented as:

[0133]

[0134] Where X is a matrix containing the independent variable.

[0135] To reduce the matrix order when converting a system of polynomial equations into matrix operations, the independent variables need to be scaled to redefine them. In this embodiment, the scaling factor of the independent variable matrix is ​​determined based on the curve length data from the starting point to the ending point, i.e.:

[0136]

[0137] Where Scale is the scaling factor of the independent variable matrix. The length of the endpoint on the curve. is the length of the starting point on the curve.

[0138] The independent variables are scaled using the scaling factor Scale of the independent variable matrix. The scaled independent variables are represented as follows:

[0139]

[0140] Among them, u s The scaled-up independent variable.

[0141] Finally, a third-order polynomial curve is fitted.

[0142] The observation vector is determined based on the extracted sampling point data and midpoint, and the coefficients of the third-order polynomial function are solved based on the scaled independent variable matrix to obtain the third-order polynomial curve.

[0143] Specifically, the observation vector of the polynomial y is transformed by a scaling factor to obtain a polynomial coefficient matrix, which is represented as:

[0144]

[0145] The inverse matrix X of the independent variable matrix X is obtained by using the Vandermonde determinant. -1 The polynomial coefficient matrix and the inverse matrix X of the independent variable matrix X are... -1 After multiplication, we obtain a numerical value representing a 1×3 polynomial with mixed coefficients, expressed as:

[0146]

[0147] Where n0, n1, and n2 are numerical values ​​representing a mixture of polynomial coefficients.

[0148] Combining formulas (9) to (13), the unknown coefficients A, B, C, and D of the third-order polynomial are derived as follows:

[0149]

[0150] Substitute formula (14) into formula (8) and replace the independent variable u with the scaled independent variable u. s The third-order polynomial curve can then be obtained.

[0151] S403: Calculate the arc length by integrating the polynomial curve using the second integral function.

[0152] Integrating the polynomial curve yields its length. The polynomial curve is then reparameterized to obtain a second motion curve. The arc length at each point on the second motion curve is calculated to obtain an arc-length-parameterized second motion curve. In this embodiment, the specific method for obtaining the arc-length-parameterized second motion curve is the same as in step S202. The specific process can be found in step S202, where the arc-length is parameterized for the first motion curve; it will not be elaborated here.

[0153] S404: Determine whether the marker site is on the fitted second motion curve.

[0154] Specifically, the arc length of the marker point is extracted, and relative points with the same arc length are determined on the second motion curve based on the arc length. Since the marker point does not participate in polynomial fitting and is a point on the first motion curve, the arc length of the marker point can be calculated based on the first motion curve after arc length parameterization in step S202. Based on the arc length of the marker point, points with the same arc length as the marker point are found on the second motion curve after arc length parameterization as relative points.

[0155] To determine whether the positional error between the marker point and the relative point is within a preset threshold range, it is necessary to ascertain whether the third-order polynomial curve meets the fitting conditions. The positional error can be calculated based on the dimensions of the marker point and the relative point; the specific calculation process is existing technology and will not be elaborated here. The preset threshold range is customized according to the fitting requirements.

[0156] S405: The marker site is on the fitted second motion curve.

[0157] The second window slides according to the second preset step size, and polynomial fitting is repeated until the second window completely covers the sampling point sequence, outputting a series of arc-length parameterized second motion curves.

[0158] Specifically, if the marker point is on the fitted second motion curve, that is, the positional error between the marker point and the relative point is within a preset threshold range, then the second window slides according to the second preset step size, extracts a new set of sampling points, and repeats the polynomial fitting, arc length calculation, and arc length judgment processes in steps S402 to S404 until the second window completely covers the sampling point sequence, resulting in a series of arc length parameterized second motion curves, forming a set of second motion curves.

[0159] For example, the second preset step size is set to "1", that is, the second window slides one data point backward in the sampling point sequence and takes out three more sampling point data. That is, the first data is c0, c1, c2. After amplification, the obtained c0, m0, m1, c2 are fitted. If the curve fitting meets the conditions, the next data is c1, c2, c3. After amplification, the obtained c1, m1, m2, c3 are fitted to obtain another arc-length parameterized second motion curve. This is repeated until the second window slides to the last sampling point data in the sampling point sequence and the last arc-length parameterized second motion curve is obtained. In this way, a series of segmented arc-length parameterized second motion curves are obtained, forming a set of second motion curves.

[0160] S406: The marker site is not on the fitted second motion curve.

[0161] The process iterates forward using polynomial fitting until the identified marker point lies on the second motion curve or the maximum number of iterations is reached.

[0162] Specifically, if the marker point is not on the fitted second motion curve, that is, the positional error between the marker point and the relative point is not within the preset threshold range, then the starting point and other sampling points, midpoints, or marker points among the extracted sampling points are used as endpoints. New midpoints and marker points are re-determined based on the newly determined endpoints. Iterative fitting, arc length calculation, and arc length judgment are performed using the new endpoints, midpoints, and marker points until the determined marker point is located on the second motion curve or the set maximum number of iterations is reached. After outputting the second motion curve, the second window is slid according to the second preset step size, and polynomial fitting is repeated until the second window completely covers the sampling point sequence, and a series of arc length parameterized second motion curves are output.

[0163] For example, if the fitted c1 is not on the second motion curve, then c0, m0, and c1 are taken as the three points in step S402. The midpoint of c0 and m0 and the midpoint of m0 and c1 are taken, and m0 is determined as the marker point. The above iterative fitting, arc length calculation and arc length judgment processes are repeated until the determined marker point is located on the second motion curve or the set maximum number of iterations is reached, and the second motion curve is output.

[0164] S5: Differentiate the first and second motion curves.

[0165] The motion parameters of the joint space points are determined based on the first motion curve, and the motion parameters of the Cartesian space points (i.e., the transformed joint space points corresponding to the Cartesian space points) are determined based on the second motion curve. This yields the correspondence between the motion points (including the joint space points and the joint space points corresponding to the Cartesian space points) and the corresponding curves after arc length parameterization, which facilitates the planning of subsequent motion trajectories.

[0166] In this embodiment, the correspondence between joint space points and corresponding curves after arc length parameterization is determined using the following method:

[0167] First, extract the first motion curves corresponding to the joint space points from the first set of motion curves. Specifically, determine the joint space points, and then match and extract the corresponding first motion curves from the first set of motion curves based on these joint space points.

[0168] Then, the first and second derivatives of the first motion curve are calculated. Specifically, the first derivative of the first motion curve represents the position corresponding to the joint space point, and the second derivative represents the velocity at that position. This allows for the mapping of joint space points to position coordinates and velocities. The calculation of the first and second derivatives of the first motion curve is existing technology and will not be elaborated upon here.

[0169] Next, the motion parameters of the joint space points are obtained by normalizing the magnitude of the first derivative of the first motion curve. During normalization, the magnitude of the joint space point vectors corresponding to the first motion curve is typically scaled to a preset unit length to ensure that the magnitudes of all data are matched. The specific process can be implemented using existing technologies and will not be elaborated here.

[0170] Finally, the correspondence between the joint spatial points and the corresponding curves after arc length parameterization is determined based on the joint spatial points and their motion parameters, so as to facilitate the direct calling of the corresponding joint spatial points and their corresponding parameters during subsequent motion trajectory planning.

[0171] In this embodiment, the correspondence between the joint space points after Cartesian space point transformation and the corresponding curves after arc length parameterization is determined using the following method:

[0172] First, the second motion curve is extracted. Specifically, the joint spatial points are determined, and then the corresponding second motion curve is matched and extracted from the set of second motion curves based on these joint spatial points.

[0173] Then, the first and second derivatives of the second motion curve are calculated. Specifically, the first derivative of the second motion curve represents the position corresponding to the joint space point, and the second derivative represents the velocity at that position. This allows for the mapping of joint space points to position coordinates and velocities. The calculation of the first and second derivatives of the second motion curve is existing technology and will not be elaborated upon here.

[0174] Next, the motion parameters of the joint space points are obtained by normalizing the magnitude of the first derivative of the second motion curve. During normalization, the magnitude of the corresponding joint space point vector on the second motion curve is typically scaled to a preset unit length to ensure that the magnitudes of all data are matched. The specific process can be implemented using existing technologies and will not be elaborated here.

[0175] Since the independent variable was scaled during the fitting of the second motion curve, the motion parameters of the joint space points need to be scaled inversely according to the preset scaling factor (i.e., scaled up during curve fitting, and scaled down here, and vice versa) during normalization, so that the motion parameters of the joint space points match the magnitude of the robot's actual motion parameters.

[0176] Finally, the correspondence between the joint spatial points and the corresponding curves after arc length parameterization is determined based on the joint spatial points and their motion parameters, so as to facilitate the direct calling of the joint spatial points and their corresponding parameters in subsequent motion trajectory planning.

[0177] The following example further illustrates the arc length parameterization method based on curve fitting in this embodiment.

[0178] S1': Collect motion point data to form a motion point sequence.

[0179] This example inputs a sequence of nine points, a mixture of joint space points and Cartesian space points, forming a motion point sequence. The robot used is a six-joint robotic arm, EVO7L. The motion point sequence is shown in Table 2 below. In Table 2, the angle unit for the point data is °, and the length unit is mm.

[0180] Table 2

[0181]

[0182] In Table 2, the first, second, and ninth points are joint space points, and the third to eighth points are Cartesian space points; x / j1, y / j2, and z / j3 represent translational degrees of freedom data; a / j4, b / j5, and c / j6 represent rotational degrees of freedom data.

[0183] S2': Based on the fitting of points 1 to 9 in Table 2, the first motion curve is obtained, and the set of Bezier curves corresponding to each motion point is obtained.

[0184] S3': Extract the Cartesian space points of the 3rd to 8th points from Table 2, and the first motion curves corresponding to the Cartesian space points calculated in step S2'.

[0185] S4': Smooth the first motion curve of the Cartesian space points and convert the Cartesian space points into corresponding joint space points to obtain the second motion curve of the joint space points. Calculate the arc length of each point on the second motion curve to obtain the set of arc-length parameterized second motion curves. Based on the point fitting in Table 2, the curves are as follows: Figure 5 As shown.

[0186] S5': Determine the motion parameters of the joint spatial points based on the first motion curve and the motion parameters of the joint spatial points corresponding to the Cartesian spatial points based on the second motion curve, and obtain the corresponding relationship of the curves after arc length parameterization, so as to facilitate the planning of subsequent motion trajectories.

[0187] Please refer to Figure 6 This is a schematic diagram of the final curve points, i.e., a tangent point diagram, obtained by using the arc length parameterization method based on curve fitting in this embodiment. From Figure 6 It is evident that when the curve curvature is large, the points or tangents are dense, while when the curve curvature is small, the points or tangents are sparse. In this way, the adaptive adjustment of tangents is achieved during the arc length parameterization process, thereby improving the planning accuracy and planning efficiency.

[0188] This embodiment employs a curve-fitting-based arc-length parameterization method. By combining Bezier curves to fit the first motion curve of joint space points with cubic polynomials to fit the second motion curve of Cartesian space points, the processing dimension is refined from coarse to fine. This not only ensures the continuity and smoothness of the robotic arm's joint movements but also significantly improves the accuracy of connecting mixed points in complex task spaces. Simultaneously, by combining Bezier curve and cubic polynomial curve fitting methods to parameterize the arc length, and by iterating once per cut point during processing, the number of cut points in the front-end processing is effectively reduced, thereby simplifying the calculation process and lowering computational complexity. Furthermore, this embodiment calculates the first and second derivatives of each point on the curve to establish a correspondence between points and parameters, providing rich dynamic information for subsequent trajectory planning.

[0189] As a preferred embodiment, a motion trajectory planning method is also provided. Specifically, the motion trajectory planning method of this embodiment includes the following steps:

[0190] First, the arc length parameterization method based on curve fitting, as described in the above embodiment, is used to obtain the correspondence between the joint space points and Cartesian space points (specifically, the transformed joint space points) and the corresponding curves after arc length parameterization. The correspondence between the joint space points and the corresponding curves includes at least the joint space points and their corresponding positional, velocity, and acceleration relationships. Similarly, the correspondence between the transformed Cartesian space points and the corresponding curves includes at least the joint space points and their corresponding positional, velocity, and acceleration relationships.

[0191] Based on the correspondence between the joint space points after conversion from Cartesian space points and the corresponding curves after arc length parameterization, the robot's position, velocity, acceleration, and jerk are planned to obtain the motion data of each joint in time and fit the motion trajectory.

[0192] The motion trajectory planning method in this embodiment calculates the first and second derivatives of each point on the first and second motion curves to form a correspondence between points and parameters. This allows for direct data extraction during the motion trajectory planning process, ensuring a more efficient dynamic planning process, thereby shortening the overall planning time and improving real-time response capabilities.

Claims

1. An arc length parameterization method based on curve fitting, characterized in that, Includes the following steps: The robot's motion point data is collected to form a motion point sequence, wherein the motion point data includes data of several joint space points of the robot in joint space and data of several Cartesian space points in Cartesian space; Curve fitting is performed on the motion point data to obtain the first motion curve for each motion point, and the arc length of each point on the first motion curve is calculated to obtain the first motion curve set parameterized by arc length. Extract the Cartesian space points and their corresponding first motion curves; The first motion curve of the Cartesian space point is smoothed, and the Cartesian space point is converted to the joint space point to obtain the second motion curve of the corresponding joint space point. The arc length of each point on the second motion curve is calculated to obtain the set of second motion curves parameterized by arc length. Based on the first motion curve, the motion parameters of the joint space point are determined, and based on the second motion curve, the motion parameters of the joint space point corresponding to the Cartesian space point are determined, thereby obtaining the correspondence between the motion point and the corresponding curve after arc length parameterization.

2. The arc length parameterization method based on curve fitting according to claim 1, characterized in that, In the step of obtaining a first motion curve for each motion point by curve fitting the motion point data and calculating the arc length of each point on the first motion curve to obtain a first set of arc length parameterized motion curves, the first motion curve is obtained by fitting the motion point data with a Bezier curve, including the following sub-steps: The motion point data in the motion point sequence is extracted according to the first preset window size, and a Bezier curve is obtained by fitting the extracted motion point data. Integrate the Bezier curve to obtain the length of the Bezier curve, reparameterize the Bezier curve to obtain the first motion curve, and calculate the arc length of each point on the first motion curve to obtain the first motion curve parameterized by arc length. The first window is slid according to the first preset step size, and the Bezier curve fitting and arc length are repeatedly performed until the first window completely covers the motion point sequence, resulting in a series of arc length parameterized first motion curves, forming a first motion curve set.

3. The arc length parameterization method based on curve fitting according to claim 2, characterized in that, In the step of extracting motion point data from the motion point sequence according to the first preset window size, and fitting a Bezier curve based on the extracted motion point data, the Bezier curve is limited to a fourth-order Bezier curve. The specific method for fitting the fourth-order Bezier curve is as follows: According to the first preset window size, the motion point data in the motion point sequence is extracted, wherein the motion point data is six-dimensional data, wherein the Cartesian space point data includes three translational degrees of freedom and three rotational degrees of freedom, and the joint space point data includes six joint angle data; The motion point data is processed according to the number of control points required to fit the fourth-order Bezier curve to obtain the curve fitting control points. A fourth-order Bezier curve is obtained by curve fitting based on the curve fitting control points.

4. The arc length parameterization method based on curve fitting according to claim 3, characterized in that, In the steps of integrating the Bezier curve to obtain its length, reparameterizing the Bezier curve to obtain a first motion curve, and calculating the arc length at each point on the first motion curve to obtain the arc length-parameterized first motion curve, the specific method for obtaining the arc length-parameterized first motion curve is as follows: Define a first integral function, wherein the first integral function is restricted to the Gauss-Legendal integral function; Determine the range of values ​​for the fourth-order Bezier curve and the integration interval of the first integral function, and convert the range of values ​​into the integration interval; The fourth-order Bezier curve is integrated over the integration interval using the Gauss-Legend integration function to obtain the length of the fourth-order Bezier curve. Based on the length of the fourth-order Bezier curve, the fourth-order Bezier curve is reparameterized to form the first parameter curve. Calculate the arc length at each point on the first parameter curve to obtain the first motion curve parameterized by arc length.

5. The arc length parameterization method based on curve fitting according to claim 1, characterized in that, In the steps of smoothing the first motion curve of the Cartesian space points, converting the Cartesian space points to joint space points to obtain the corresponding second motion curve of the joint space points, and calculating the arc length of each point on the second motion curve to obtain the set of arc length parameterized second motion curves, the second motion curve is obtained by piecewise fitting of the first motion curve using a polynomial, including the following sub-steps: Several sampling point data are sequentially collected on the first motion curve corresponding to the Cartesian space point, and inverse kinematics calculation is performed on the Cartesian space point to convert the Cartesian space point to the joint space point, forming a sampling point sequence. The sampling point data in the sampling point sequence is extracted according to the second preset window size, and a polynomial curve is obtained by fitting the extracted sampling point data, wherein the extracted sampling points include a marker site. Integrate the polynomial curve to obtain the length of the polynomial curve, reparameterize the polynomial curve to obtain the second motion curve, and calculate the arc length of each point on the second motion curve to obtain the second motion curve parameterized by arc length. Determine whether the marker site is on the second motion curve. If it is, slide the second window according to the second preset step size and repeat the polynomial fitting backward until the second window completely covers the sampling point sequence and outputs a series of arc-length parameterized second motion curves. Otherwise, iterate forward to perform polynomial fitting until the determined marker site is located on the second motion curve or the set maximum number of iterations is reached.

6. The arc length parameterization method based on curve fitting according to claim 5, characterized in that, In the step of extracting sampling point data from the sampling point sequence according to the second preset window size, and fitting a polynomial curve based on the extracted sampling point data, a third-order polynomial is used to perform piecewise fitting on the sampling point data. The specific method for fitting the polynomial curve is as follows: The sampling point data in the sampling point sequence is extracted according to the second preset window size, and the midpoint between two adjacent sampling points is determined by the binary search method. The median of the extracted sampling point data is determined as the marker point, wherein the marker point does not participate in curve fitting. Define a third-order polynomial function, wherein the third-order polynomial function includes four unknown coefficients; The independent variable matrix of the third-order polynomial function is constructed based on the extracted sampling point data and midpoint, and the independent variable matrix is ​​scaled according to a preset scaling factor to obtain the scaled independent variable matrix. The observation vector is determined based on the extracted sampling point data and midpoint, and the coefficients of the third-order polynomial function are solved based on the scaled independent variable matrix to obtain the third-order polynomial curve.

7. The arc length parameterization method based on curve fitting according to claim 6, characterized in that, In the step of determining whether the marker site is on the second motion curve, the specific method is as follows: Extract the arc length of the marker point, and determine the relative points with the same arc length on the second motion curve based on the arc length; Determine whether the positional error between the marker point and the relative point is within a preset threshold range; If the position error is within the preset threshold range, the second window is slid according to the second preset step size to extract a new set of sampling points, and polynomial fitting, arc length calculation and arc length judgment are repeatedly performed until the second window completely covers the sampling point sequence, and a series of arc length parameterized second motion curves are obtained to form a set of second motion curves. If the position error is not within the preset threshold range, then the starting point and other sampling points, midpoints or marker points other than the ending point among the extracted sampling points are used as endpoints. New midpoints and marker points are re-determined based on the newly determined endpoints. Then, iterative fitting, arc length calculation and arc length judgment are performed using the new endpoints, midpoints and marker points until the determined marker point is located on the second motion curve or the set maximum number of iterations is reached.

8. The arc length parameterization method based on curve fitting according to claim 4, characterized in that, The specific method for determining the motion parameters of the joint spatial points based on the first motion curve, and obtaining the correspondence between the joint spatial points and the corresponding curves after arc length parameterization, is as follows: Extract the first motion curves corresponding to the joint space points from the first set of motion curves; Calculate the first and second derivatives of the first motion curve; The motion parameters of the joint space points in the joint space are obtained by normalizing the modulus of the first derivative of the first motion curve. The correspondence between the joint spatial points and the corresponding curves after arc length parameterization is determined based on the joint spatial points and their motion parameters.

9. The arc length parameterization method based on curve fitting according to claim 7, characterized in that, The specific method for determining the motion parameters of the joint space points corresponding to the Cartesian space points based on the second motion curve, and obtaining the correspondence between the joint space points after the Cartesian space point transformation and the corresponding curve after arc length parameterization, is as follows: Extract the second motion curve; Calculate the first and second derivatives of the second motion curve; The motion parameters of the joint space points in the joint space are obtained by normalizing the magnitude of the first derivative of the second motion curve. The motion parameters of the joint space points are then scaled according to the preset scaling factor so that the motion parameters of the joint space points match the magnitude of the actual motion parameters of the robot. The correspondence between the joint spatial points and the corresponding curves after arc length parameterization is determined based on the joint spatial points and their motion parameters.

10. A motion trajectory planning method, characterized in that, Includes the following steps: The arc length parameterization method based on curve fitting as described in any one of claims 1 to 9 is used to obtain the correspondence between the moving point and the corresponding curve after arc length parameterization; Based on the correspondence between the motion points and the corresponding curves after arc length parameterization, the robot's position, velocity, acceleration, and jerk are planned to obtain the motion data of each joint of the robot in time and fit the motion trajectory.

Images