A method and system for optimizing a turn point of a pose trajectory

Abstract

The application relates to the technical field of attitude trajectory optimization, in particular to an attitude trajectory turning point optimization method and system. Projection point measurement data is acquired, and attitude information is obtained by processing the projection point measurement data; the attitude information is subjected to linear space processing to obtain linear space information; a large-curvature point in a linear space information trajectory is identified; the information of the large-curvature point is removed, the linear space information is reconstructed to obtain reconstructed space information, the accuracy of the attitude trajectory is improved, and the execution efficiency of the trajectory is improved.

Description

Technical Field

[0001] The present application relates to the technical field of posture trajectory optimization, and in particular to a posture trajectory turning point optimization method and system. Background Art

[0002] The combination of 3D vision and robotic arm control has become widespread in industrial scenarios. To reduce costs, robot manufacturers and integrators often need to adopt more economical 3D perception solutions. For a common posture perception task, which involves acquiring the posture sequence corresponding to the projection trajectory of a given spatial position trajectory onto a workpiece, the use of low-cost sensors is often accompanied by extremely unstable communication cycles and significant signal noise. Even after filtering, these noisy signal trajectories still contain gesture reversals. During trajectory planning, these localized reversals will slow down the trajectory planning process and ultimately affect trajectory execution efficiency.

[0003] For trajectories accompanied by severe noise, the approach is usually to optimize the estimation. For example, a Kalman filter is designed to fuse the data acquired in each cycle based on a certain prior and obtain an updated posterior estimate. Although this method can suppress noise in the attitude data and improve the smoothness of the collected data, considering the nonlinear characteristics of the attitude space transfer function, the filter may consider using a ukf or ekf to obtain a more accurate posterior probability. However, the return segment optimization method based on the filtering idea will introduce unnecessary loss of attitude accuracy throughout the trajectory segment, increase the processing cost of the return problem, affect the accuracy of the attitude trajectory, and thus affect the execution efficiency of the trajectory. These problems need to be solved. Summary of the Invention

[0004] In order to improve the accuracy of the posture trajectory and enhance the execution efficiency of the trajectory, this application provides a posture trajectory turning point optimization method and system, which adopts the following technical solutions:

[0005] In a first aspect, the present application provides a method for optimizing a turning point of a posture trajectory, comprising:

[0006] Obtaining projection point measurement data, and processing the projection point measurement data to obtain posture information;

[0007] Perform linear space processing on the posture information to obtain linear space information;

[0008] Identify points of large curvature in linear spatial information trajectories;

[0009] The information of large curvature points is removed, and the linear space information is reconstructed to obtain the reconstructed space information.

[0010] Preferably, the specific steps of acquiring the projection point measurement data and processing the projection point measurement data to obtain the posture information are:

[0011] The measurement data of three laser rangefinders are obtained, including a first geometric space position point, a second geometric space position point, and a third geometric space position point, respectively. A right-handed coordinate system satisfying an orthogonal relationship is established based on the first geometric space position point, the second geometric space position point, and the third geometric space position point, and the posture information corresponding to the three measurement data in the right-handed coordinate system is obtained.

[0012] Preferably, the specific steps of identifying the large curvature points in the linear space information trajectory are:

[0013] Obtaining front point information and back point information of the linear space information, and calculating first vector information and second vector information according to the linear space information, the front point information and the back point information;

[0014] Constructing a curvature value according to the first vector information and the second vector information;

[0015] By comparing several curvature values, the points with the largest curvature of the trajectory are obtained, and the large curvature points are obtained.

[0016] Preferably, the specific steps of removing the information of the large curvature points are:

[0017] Obtain several large curvature points, remove them from the original trajectory, and obtain the remaining point sequence information.

[0018] Preferably, the specific steps of reconstructing the linear spatial information to obtain the reconstructed spatial information are:

[0019] A fitting model is constructed, and the least squares method is used to obtain several polynomial coefficients, which are then substituted into the fitting model for trajectory fitting.

[0020] Preferably, reconstructing the linear spatial information to obtain the reconstructed spatial information further comprises:

[0021] Resample the linear space information trajectory according to the fitting model to obtain the replacement curvature point;

[0022] The replacement curvature points correspond to the large curvature points in the linear spatial information trajectory to obtain the reconstructed spatial information.

[0023] Preferably, it also includes:

[0024] The reconstructed spatial information is formatted to obtain the optimized posture trajectory.

[0025] Preferably, the linear space information is the motion trajectory information of the Lie algebra in the linear space that changes with time.

[0026] In a second aspect, the present application provides a posture trajectory turning point optimization system, comprising:

[0027] An acquisition module is used to acquire the projection point measurement data and obtain the posture information according to the projection point measurement data;

[0028] The format conversion module is used to perform linear space processing on the posture information to obtain linear space information;

[0029] Recognition module, used to identify large curvature points in linear space information trajectory;

[0030] The reconstruction module is used to remove the information of large curvature points and reconstruct the linear space information to obtain reconstructed space information.

[0031] Preferably, it also includes:

[0032] The format restoration module is used to convert the format of the reconstructed spatial information to obtain the optimized posture trajectory.

[0033] In summary, compared with the prior art, the technical solution provided by this application has at least the following beneficial effects:

[0034] This application obtains posture information by processing the acquired projection point measurement data, identifies the return segment of the posture in Lie algebraic space based on the concept of curvature, performs linear space processing to obtain linear space information, and after identifying the large curvature point, reconstructs the large curvature point after removing it to obtain reconstructed space information. The processed trajectory can pass through the path point with posture return at a faster speed and acceleration, thereby improving the accuracy of the posture trajectory and improving the execution efficiency of the trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is a flow chart of a posture trajectory turning point optimization method described in an embodiment of the present application.

[0036] Figure 2 This is a schematic diagram of the trajectory comparison before and after the data test optimization described in the embodiment of the present application.

[0037] Figure 3 It is a schematic diagram of the optimization effect of the trajectory curvature described in the embodiment of the present application.

[0038] Figure 4 This is a module diagram of a posture trajectory turning point optimization system described in an embodiment of the present application.

[0039] Description of reference numerals:

[0040] 1. Acquisition module; 2. Format conversion module; 3. Recognition module; 4. Reconstruction module. DETAILED DESCRIPTION

[0041] The following combination Figure 1-Figure 4To further explain the present application in detail, the terms used in the embodiments of the present application are only for the purpose of describing specific embodiments and are not intended to be limiting.

[0042] Reference Figure 1 , a posture trajectory turning point optimization method involved in this application specifically includes:

[0043] Step S1: Acquire projection point measurement data, and obtain posture information based on the projection point measurement data;

[0044] Step S2: Perform linear space processing on the posture information to obtain linear space information;

[0045] Step S3: identifying large curvature points in the linear spatial information trajectory;

[0046] Step S4: remove the information of the large curvature points, reconstruct the linear space information, and obtain reconstructed space information.

[0047] Specifically, the embodiment of the present application obtains posture information by processing the obtained projection point measurement data, performs linear space processing to obtain linear space information, and after identifying the large curvature point, removes the large curvature point and reconstructs it to obtain reconstructed space information. Based on the concept of curvature, a method for identifying the return segment of the posture in Lie algebraic space is proposed, and based on the idea of ​​local fitting, points with excessive curvature are deleted and the local trajectory is reconstructed. Through actual experiments, the processed trajectory can pass through the path points with posture return at a faster speed and acceleration. The overall trajectory has a higher execution efficiency than before processing.

[0048] As one implementation method, the specific steps of obtaining projection point measurement data and processing the projection point measurement data to obtain posture information are as follows:

[0049] The measurement data of three laser rangefinders are obtained, including a first geometric space position point, a second geometric space position point, and a third geometric space position point, respectively. A right-handed coordinate system satisfying an orthogonal relationship is established based on the first geometric space position point, the second geometric space position point, and the third geometric space position point, and the posture information corresponding to the three measurement data in the right-handed coordinate system is obtained.

[0050] Specifically, several infrared ranging sensors are used to collect projection point measurement data, with each infrared ranging sensor collecting data from a geometric space position. The measurement data of three laser rangefinders are acquired in sequence within a control cycle, denoted as P1, P2, and P3. For the posture R(t) calculation process, the three measured distances are used to calculate two direction vectors, and a right-handed coordinate system that satisfies the orthogonal relationship is established. Among them:

[0051]

[0052]

[0053] e y =e x ×e z ,

[0054] R(t)=(e x (t) e y (t) e z (t))

[0055] The posture information is processed in linear space to obtain linear space information. The purpose is to convert the posture representation from a nonlinear geometric space to a linear space for easy processing. First, the posture representation of the posture space at time t is R t , which is converted into the value of the Lie algebra ξ in a given reference frame ref ξ(t) is usually chosen as the posture of the workpiece reference system, or the starting point of the trajectory. Then the motion trajectory of the Lie algebra in the linear space that changes with time is:

[0056]

[0057] Ref represents the reference frame selection. Ref can be a variety of reference frames, such as the workpiece to be measured, the robot coordinate system as a reference, or a specific task coordinate system.

[0058] Here, log is a mapping method in the Lie algebra that maps a relative coordinate on the Lie group to an axis of rotation around the space and the corresponding angle of rotation. V maps the coordinates in tensor space back to linear space. The combination of log and V results in an element on the Lie group being mapped to an element in linear space, allowing operations on Lie group elements to be performed by performing calculations in linear space.

[0059] As one implementation method, the specific steps of identifying the large curvature points in the linear space information trajectory are:

[0060] Obtaining front point information and back point information of the linear space information, and calculating first vector information and second vector information according to the linear space information, the front point information and the back point information;

[0061] Constructing a curvature value according to the first vector information and the second vector information;

[0062] By comparing several curvature values, the points with the largest curvature of the trajectory are obtained, and the large curvature points are obtained.

[0063] Specifically, curvature is a geometric concept used to measure the degree of curvature of a straight line in space. The mean curvature represents the angle of deflection of the yaw angle θ when moving a distance s along the straight line. The instantaneous curvature represents the angle of deflection when Lim |s| = 0. In the global coordinate system, the curvature has the following expression, where and Respectively represent the first-order and second-order vector information of the motion vector along the trajectory, namely the first vector information and the second vector information.

[0064]

[0065] The obtained κ represents the curvature value corresponding to each discrete point in space. For discrete systems, the first-order and second-order information of ξ cannot be obtained directly, so it is inaccurate to use the differential value of the sequence to approximate the local curvature point. In order to obtain first-order and second-order vector information, the embodiment of the present application adopts a compact expression. The mathematical models include quadratic models and circles. In this embodiment, since we want to fit the turning point, we choose to use a quadratic model. In the local three-point model of ξ(t) at time t, the previous point ξ(t-1) and the next point ξ(t+1) are included, which are respectively recorded as ξ(t) = [x1, y1, z1], ξ(t-1) = [x2, y2, z2], ξ(t+1) = [x3, y3, z3]. Optionally, a quadratic polynomial model is used to fit the trajectories in each direction, and the coefficients of the fitted quadratic polynomial model are used to represent the current velocity acceleration. The curvature is thus calculated. The calculation process is as follows:

[0066]

[0067] With the following initial and final value conditions

[0068]

[0069] (x,y,z)| t＝0 =(x²,y²,z²)

[0070]

[0071] By solving the equations, we can get the trajectory polynomial coefficients $(a1,a2,a3)$,$(b1,b2,b3)$,$(c1,c2,c3)$, and substitute them into the following formula to get the first-order and second-order derivative information at the point $t$ to be solved

[0072]

[0073]

[0074]

[0075]

[0076]

[0077]

[0078] Therefore, the first-order and second-order derivative information is obtained:

[0079]

[0080]

[0081] A curvature value is constructed based on the obtained first vector information and the second vector information.

[0082] As one implementation method, the specific steps of removing the information of large curvature points are as follows:

[0083] Obtain several large curvature points, remove them from the original trajectory, and obtain the remaining point sequence information.

[0084] Specifically, the point number is i, and the corresponding curvature is κ i , delete the k points with the largest curvature, and then use a low-order simple curve to fit the remaining points. Finally, resample and replace the original points. For the trajectory of the Lie algebra space sequence, do the following: there is a sequence of n points (i, x i ), the range of n is selected to be 11-15, and the curvature corresponding to each point is κ i The goal is to reconstruct the trajectory segment with local low curvature. Then select the largest k, usually a curvature value of 3-5. These points with the largest curvature are marked as large curvature points in the trajectory segment. Remove these k curvature points from the original sequence to obtain the remaining point sequence (j, y j ).

[0085] As one implementation method, the linear spatial information is reconstructed to obtain the reconstructed spatial information in the following specific steps:

[0086] A fitting model is constructed, and the least squares method is used to obtain several polynomial coefficients, which are then substituted into the fitting model for trajectory fitting.

[0087] Specifically, curve fitting is performed on the remaining point sequence to reconstruct the local trajectory. A cubic polynomial P(j) is selected as the fitting model. Using the least squares method or other suitable fitting methods, the polynomial coefficients a0, a1, a2, and a3 are found so that the polynomial P(j) = a0 + a1j + a2j. 2 +a3j 3Optimally fit the sequence of remaining points, where j is the index of the remaining trajectory points. This function uses the SVD method for least squares calculation. The solution process is as follows: construct a matrix A, where each row corresponds to the index j of a remaining point, and the columns contain the various powers of the polynomial. The form of this matrix A is as follows:

[0088]

[0089]

[0090] Use the SVD method to solve the linear equation system Ax = b, that is, A T Ax=A T b, where x is a column vector containing the polynomial coefficients a0, a1, a2, and a3. After solving, the least squares solution of the coefficient vector x is obtained, that is, the estimated values ​​of a0, a1, a2, and a3, which are used to fit the remaining point sequence.

[0091] As one implementation method, reconstructing the linear spatial information to obtain the reconstructed spatial information further includes:

[0092] Resample the linear space information trajectory according to the fitting model to obtain the replacement curvature point;

[0093] The replacement curvature points correspond to the large curvature points in the linear spatial information trajectory to obtain the reconstructed spatial information.

[0094] Specifically, the curve is resampled using the fitted polynomial P(j) to generate a new set of points (i, x′ i ), which can be resampled uniformly or to the desired resolution. The resampled sequence of points replaces the original curvature points to obtain a reconstructed local low-curvature trajectory. This process allows the reconstruction of local low-curvature trajectory segments by removing high-curvature points, fitting the remaining points with a low-order polynomial, and resampling the curve. The end result is a smooth trajectory where the high-curvature sections are smoothed to suit the needs of a specific application.

[0095] As one implementation method, it also includes:

[0096] The reconstructed spatial information is formatted to obtain the optimized posture trajectory.

[0097] Specifically, finally turn the optimized posture trajectory back:

[0098] R(t)=exp(ξ(t))*R ref

[0099] As one of the implementation methods, refer to Figure 2 and Figure 3The method of the embodiment of the present application suppresses posture path noise. In this embodiment, a test case of a noisy posture trajectory is implemented to test the method. From the local details before and after the trajectory correction, it can be seen that the required local path curvature smoothness is achieved. The corrected trajectory has a lower posture accuracy than before correction, and the overall deviation from the middle finger is smaller. Secondly, the trajectory diagram shows that this correction is only local. After optimization, the sampling points basically maintain the spacing distribution pattern of the original trajectory. Comparing the control points of the two trajectories before and after optimization, the control points of the curves before and after correction can still be basically distributed according to the given distribution ratio according to the spacing before sampling, and basically obey the same sampling distribution in the curve coordinate system. This characteristic can ensure that the position and posture are matched before and after correction, and there will be no significant loss of posture at specific positions. The trajectory eliminates noise while basically maintaining path accuracy. Comparing the trajectory diagrams before and after control point optimization, the trajectory can basically cover the area covered by the original posture curve, indicating that the optimized trajectory path can basically reach the trajectory point taught by the user. For a test case with posture noise, the optimized trajectory improves efficiency by approximately 16.2704%.

[0100] Reference Figure 4 , an embodiment of the present application provides a posture trajectory turning point optimization system, the system comprising:

[0101] Acquisition module 1, used to acquire projection point measurement data and obtain posture information based on the projection point measurement data;

[0102] Format conversion module 2, used for performing linear space processing on the posture information to obtain linear space information;

[0103] Identification module 3, used to identify large curvature points in the linear space information trajectory;

[0104] The reconstruction module 4 is used to remove the information of the large curvature points and reconstruct the linear space information to obtain the reconstructed space information.

[0105] Specifically, it also includes a format restoration module for converting the format of the reconstructed spatial information to obtain an optimized posture trajectory.

[0106] An embodiment of the present application provides a posture trajectory turning point optimization device, including a memory and a processor, wherein the memory stores a computer program, and the processor is configured to run the computer program to execute the posture trajectory turning point optimization method as described above.

[0107] An embodiment of the present application provides a computer-readable storage medium, in which a computer program is stored. The computer program is configured to execute the above-mentioned posture trajectory turning point optimization method when running.

[0108] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described devices and products can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0109] In several embodiments provided in this application, it should be understood that the disclosed methods, systems, devices, and program products may be implemented in other ways.

[0110] In addition, the functional units in the various embodiments of the present application may be integrated into a single processing unit, or each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0111] As described above, the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A posture trajectory turning point optimization method, characterized in that: include: Obtaining projection point measurement data, and processing the projection point measurement data to obtain posture information; Based on the concept of curvature, the return segment of the posture is identified in the Lie algebra space, and the posture information is processed in a linear space to obtain linear space information, which is the motion trajectory information of the Lie algebra in the linear space that changes with time; Identify points of large curvature in linear spatial information trajectories; The information of the large curvature points is removed, and the linear space information is reconstructed to obtain the reconstructed space information; The reconstructed spatial information is formatted to obtain the optimized posture trajectory.

2. The posture trajectory turning point optimization method according to claim 1, characterized in that: The specific steps of obtaining the projection point measurement data and processing the projection point measurement data to obtain the posture information are as follows: The measurement data of three laser rangefinders are obtained, including a first geometric space position point, a second geometric space position point, and a third geometric space position point, respectively. A right-handed coordinate system satisfying an orthogonal relationship is established based on the first geometric space position point, the second geometric space position point, and the third geometric space position point, and the posture information corresponding to the three measurement data in the right-handed coordinate system is obtained.

3. The posture trajectory turning point optimization method according to claim 1, characterized in that: The specific steps of identifying the large curvature points in the linear space information trajectory are: Obtaining front point information and back point information of the linear space information, and calculating first vector information and second vector information according to the linear space information, the front point information and the back point information; Constructing a curvature value according to the first vector information and the second vector information; By comparing several curvature values, the points where the trajectory has a large degree of curvature are obtained, and the large curvature points are obtained.

4. The posture trajectory turning point optimization method according to claim 1, characterized in that: The specific steps of removing the information of large curvature points are: Obtain several large curvature points, remove them from the original trajectory, and obtain the remaining point sequence information.

5. The posture trajectory turning point optimization method according to claim 4, characterized in that: The specific steps of reconstructing the linear spatial information to obtain the reconstructed spatial information are: A fitting model is constructed, and the least squares method is used to obtain several polynomial coefficients, which are then substituted into the fitting model for trajectory fitting.

6. The posture trajectory turning point optimization method according to claim 5, characterized in that: The reconstructing the linear spatial information to obtain the reconstructed spatial information further includes: Resample the linear space information trajectory according to the fitting model to obtain the replacement curvature point; The replacement curvature points correspond to the large curvature points in the linear spatial information trajectory to obtain the reconstructed spatial information.

7. A posture trajectory turning point optimization system, characterized in that: include: An acquisition module is used to acquire the projection point measurement data and obtain the posture information according to the projection point measurement data; A format conversion module is used to identify the return segment of the posture in the Lie algebra space based on the concept of curvature, and perform linear space processing on the posture information to obtain linear space information, wherein the linear space information is the motion trajectory information of the Lie algebra in the linear space that changes with time; Recognition module, used to identify large curvature points in linear space information trajectory; The reconstruction module is used to remove the information of large curvature points and reconstruct the linear space information to obtain the reconstructed space information; The format restoration module is used to convert the format of the reconstructed spatial information to obtain the optimized posture trajectory.

Images