CN-122008210-A - Robot track closed-loop detection and pose smoothing method, device, equipment and medium

CN122008210ACN 122008210 ACN122008210 ACN 122008210ACN-122008210-A

Abstract

The application discloses a robot track closed-loop detection and pose smoothing method, a device, equipment and a medium, belonging to the field of robot control, wherein the method converts a first pose sequence track sequence in the robot action process into a first Cartesian position vector sequence and a first special orthogonal group rotation matrix sequence; the method comprises the steps of determining a closed loop region index interval according to a first Cartesian position vector sequence through principal component dimension reduction projection, converting a first special orthogonal group rotation matrix sequence into a linear first Lifex vector sequence, carrying out window average smoothing processing on the first Cartesian position vector sequence and the first Lifex vector sequence, restoring the smoothed first Lifex vector sequence back to the rotation matrix through exponential mapping, and obtaining a smoothed second pose track sequence through combining the smoothed first Cartesian position vector sequence and the smoothed restored first Lifex vector sequence. The application can avoid the attitude loss when the attitude is processed to turn back.

Inventors

HUANG YANWEI
ZHANG PENG
ZHANG GUOPING
WANG GUANGNENG

Assignees

广东华沿机器人股份有限公司

Dates

Publication Date: 20260512
Application Date: 20260204

Claims (10)

1. A robot track closed loop detection and pose smoothing method is characterized by comprising the following steps: Receiving a first pose track sequence in the action process of a robot, and converting the first pose track sequence into an isomorphic transformation matrix sequence, wherein the isomorphic transformation matrix sequence comprises a first Cartesian position vector sequence and a first special orthogonal group rotation matrix sequence; mapping the first Cartesian position vector sequence to a two-dimensional plane through main component dimension reduction projection, and determining a closed loop region index interval by combining line segment intersection detection and polygon area judgment based on projection coordinates; converting the first special orthogonal group rotation matrix sequence into a linear first Li algebraic vector sequence through logarithmic mapping, and respectively carrying out window average smoothing on the first Cartesian position vector sequence and the first Li algebraic vector sequence by combining a plurality of front and back expansion points of the closed loop region index interval to obtain a second Cartesian position vector sequence and a second Li algebraic vector sequence; Restoring the second lie algebra vector sequence back to a rotation matrix through exponential mapping to obtain a second special orthogonal group rotation matrix sequence; and obtaining a smoothed second pose track sequence by combining the second Cartesian position vector sequence and the second special orthogonal group rotation matrix sequence.
2. The method for closed loop detection and pose smoothing of a robot trajectory of claim 1, wherein mapping the first cartesian position vector sequence to a two-dimensional plane by principal component dimension-reduction projection comprises: dividing the first Cartesian position vector sequence into a plurality of window matrixes based on a preset window size; for each window matrix, calculating principal component feature vectors corresponding to three dimensions corresponding to the window matrix and feature values corresponding to the principal component feature vectors; For each window matrix, selecting two optimal first principal component feature vectors from all principal component feature vectors corresponding to the window matrix according to the feature values; and forming a projection point sequence of the first Cartesian position vector sequence on a two-dimensional plane according to the first principal component feature vectors corresponding to all the window matrixes.
3. The method for closed loop detection and pose smoothing of a robot trajectory as claimed in claim 2, wherein said calculating principal component eigenvectors corresponding to three dimensions corresponding to said window matrix and eigenvalues corresponding to each of said principal component eigenvectors comprises: Calculating a first average value of data points in the window matrix, and subtracting the first average value from each data point in the window matrix to obtain a corresponding centralized data matrix; And calculating a covariance matrix according to the centralized data matrix, and carrying out eigenvalue decomposition on the covariance matrix to obtain principal component eigenvectors corresponding to three dimensions and eigenvalues corresponding to the principal component eigenvectors.
4. The method for closed loop detection and pose smoothing of robot trajectory as claimed in claim 2, wherein said combining line segment intersection detection with projected coordinate-based polygon area determination, determining a closed loop region index interval, comprises: Generating corresponding connecting line segments according to all front and back adjacent projection points in the projection point sequence, taking the connecting line segments formed by the last two projection points as a first line segment and taking the connecting line segments formed by other projection points as a second line segment; When a third line segment intersecting with the first line segment exists in the second line segment, calculating an intersection point of the third line segment and the first line segment, and generating a polygon vertex sequence according to the intersection point and a projection point which is positioned behind the intersection point in the projection point sequence; And when the three-dimensional projection area corresponding to the polygon vertex sequence is larger than a first preset threshold value, determining the closed-loop region index interval according to the polygon vertex sequence.
5. The method for closed loop detection and pose smoothing of robot trajectories of claim 1, wherein said converting said first special orthogonal group rotation matrix sequence into a linear first lie algebraic vector sequence by logarithmic mapping comprises: calculating a first relative rotation matrix of each rotation matrix in the first special orthogonal group rotation matrix sequence relative to a preset reference rotation matrix; Mapping each first relative rotation matrix to a lie algebra space through logarithmic mapping to obtain an initial lie algebra vector corresponding to each first relative rotation matrix; For each initial lie algebra vector, if the difference between the first modular length and the circumferential rate of the initial lie algebra vector is smaller than a second preset threshold value, reconstructing the initial lie algebra vector according to the first relative rotation matrix and the first modular length corresponding to the initial lie algebra vector; And forming the first Li algebraic vector sequence according to each processed initial Li algebraic vector.
6. The method for closed loop detection and pose smoothing of robot trajectories as claimed in claim 5, wherein said reducing said second sequence of lie algebraic vectors back to a rotation matrix by exponential mapping to obtain a second sequence of special orthogonal group rotation matrices comprises: calculating a second modulo length of the lie algebraic vector for each lie algebraic vector in the second sequence of lie algebraic vectors; For the lie algebra vector with the second modulus being greater than or equal to a third preset threshold value, calculating an antisymmetric matrix corresponding to the lie algebra vector according to a lode-reed-solomon formula, and multiplying the antisymmetric matrix with the preset reference rotation matrix to obtain an absolute rotation matrix; and constructing the second special orthogonal group rotation matrix sequence according to each absolute rotation matrix.
7. The method for closed loop detection and pose smoothing of a robot trajectory as claimed in claim 1, wherein said performing window average smoothing on said first cartesian position vector sequence and said first lie algebraic vector sequence with respect to a plurality of front and rear extension points of said closed loop region index section, respectively, comprises: Expanding the closed loop region index interval according to the front and back expansion points to obtain a control point interval; and in the control point interval, respectively carrying out window average smoothing processing on the first Cartesian position vector sequence and the first Li algebra vector sequence according to windows with the number of extension points being the size.
8. The robot track closed-loop detection and pose smoothing device is characterized by comprising a data conversion module, a local closed-loop detection module, a smoothing module, a conversion module and a track reduction module; The data conversion module is used for receiving a first pose track sequence in the action process of the robot and converting the first pose track sequence into an isomorphic transformation matrix sequence, wherein the isomorphic transformation matrix sequence comprises a first Cartesian position vector sequence and a first special orthogonal group rotation matrix sequence; The local closed loop detection module is used for mapping the first Cartesian position vector sequence to a two-dimensional plane through main component dimension reduction projection, and determining a closed loop region index interval by combining line segment intersection detection and polygon area judgment based on projection coordinates; the smoothing module is configured to convert the first special orthogonal group rotation matrix sequence into a linear first lie algebraic vector sequence through logarithmic mapping, and perform window average smoothing on the first cartesian position vector sequence and the first lie algebraic vector sequence by combining a plurality of front and rear expansion points of the closed-loop region index interval to obtain a second cartesian position vector sequence and a second lie algebraic vector sequence; the conversion module is used for restoring the second lie algebra vector sequence back to the rotation matrix through exponential mapping to obtain a second special orthogonal group rotation matrix sequence; The track reduction module is configured to obtain a smoothed second pose track sequence by combining the second cartesian position vector sequence and the second special orthogonal group rotation matrix sequence.
9. A terminal device comprising a processor, a memory and a computer program stored in the memory and configured to be executed by the processor, the processor implementing a robot trajectory closed loop detection and pose smoothing method according to any one of claims 1 to 7 when executing the computer program.
10. A computer readable storage medium, characterized in that the computer readable storage medium comprises a stored computer program, wherein the computer program when run controls a device in which the computer readable storage medium is located to perform a robot trajectory closed loop detection and pose smoothing method according to any one of claims 1 to 7.

Description

Robot track closed-loop detection and pose smoothing method, device, equipment and medium Technical Field The application relates to the field of robot control, in particular to a method, a device, equipment and a medium for closed-loop detection and pose smoothing of a robot track. Background The combination of three-dimensional vision and robotic arm control has become very popular in industrial settings. In order to reduce the cost, the robot product is pursued to be more economical in three-dimensional perception scheme in integration. For a general gesture sensing task (including a gesture sequence requiring acquisition of a projected trajectory of a given spatial position trajectory on a workpiece), low cost sensors (such as infrared sensors) are often accompanied by extremely unstable communication periods and large signal noise. These noise signal tracks may still have position and attitude closed loops even after filtering, which in track planning will lead to a slow down of the track planning and will ultimately affect the execution efficiency of the track. The prior art processes trajectories with severe noise, usually starting from the idea of optimal estimation. For example, a kalman filter is designed to fuse the data acquired for each cycle based on a certain prior and obtain updated posterior estimates. Although the method can perform noise suppression on the attitude data, improve the smoothness of the acquired data, and consider the transfer function of the nonlinear characteristic of the attitude space, the filter may consider to use ukf and ekf to obtain more accurate posterior probability. However, the fold-back section optimization means based on the filtering thought can introduce unnecessary attitude precision loss in the whole track section, and the processing cost for the fold-back problem is too great. Therefore, in the three-dimensional vision and the mechanical arm control process, how to avoid unnecessary loss of gesture precision when processing gesture turn-back is a technical problem that needs to be solved at present. Disclosure of Invention The application provides a robot track closed-loop detection and pose smoothing method, device, equipment and medium, which can solve the problem of how to avoid unnecessary pose precision loss when processing pose turning back in the three-dimensional vision and mechanical arm control process in the prior art. Some embodiments of the present application provide a method for closed loop detection and pose smoothing of a robot track, including: Receiving a first pose track sequence in the action process of a robot, and converting the first pose track sequence into an isomorphic transformation matrix sequence, wherein the isomorphic transformation matrix sequence comprises a first Cartesian position vector sequence and a first special orthogonal group rotation matrix sequence; mapping the first Cartesian position vector sequence to a two-dimensional plane through main component dimension reduction projection, and determining a closed loop region index interval by combining line segment intersection detection and polygon area judgment based on projection coordinates; converting the first special orthogonal group rotation matrix sequence into a linear first Li algebraic vector sequence through logarithmic mapping, and respectively carrying out window average smoothing on the first Cartesian position vector sequence and the first Li algebraic vector sequence by combining a plurality of front and back expansion points of the closed loop region index interval to obtain a second Cartesian position vector sequence and a second Li algebraic vector sequence; Restoring the second lie algebra vector sequence back to a rotation matrix through exponential mapping to obtain a second special orthogonal group rotation matrix sequence; and obtaining a smoothed second pose track sequence by combining the second Cartesian position vector sequence and the second special orthogonal group rotation matrix sequence. Compared with the prior art, the method has the beneficial effects that the pose sequence is decoupled into the position and rotation matrix sequence, and the accurate identification of the local abnormal closed loop is realized by utilizing the principal component analysis dimension reduction projection and the geometric area judgment. Compared with the traditional Kalman filtering and other optimal estimation methods for introducing precision loss in the whole section, the method converts the nonlinear gesture into the linear lie algebraic vector through logarithmic mapping, combines the identified index interval to perform local directional smoothing processing, and only executes restoration on the abnormal region, thereby effectively eliminating foldback noise, eliminating track closed loop, simultaneously keeping the gesture precision of the normal section to the maximum extent and avoiding unnecessary precision loss. In addition, the rotation s