Real-time three-dimensional trajectory tracking method for robot end-effector operating point based on asynchronous fusion

Abstract

This invention discloses a real-time 3D trajectory tracking method for robot end effector points based on asynchronous fusion, relating to the fields of high-precision motion control and multi-sensor information fusion technology. Its key feature is the acquisition of image frames and high-frequency component data from the robot under test, followed by the calculation of visual and high-frequency 3D position observations. Using these observations as input, visual and high-frequency local state estimates are output, respectively. The optimal weighting matrix for both visual and high-frequency data is then obtained. A weighted sum of the visual and high-frequency local state estimates yields the optimal estimate of the real-time 3D trajectory of the robot's end effector points. The advantage is that this method achieves optimal fusion of heterogeneous and asynchronous 3D data from binocular vision and high-frequency sensors under asynchronous sampling conditions, improving the accuracy and robustness of the 3D trajectory estimation for the robot's end effector points.

Description

Technical Field

[0001] This invention relates to the field of high-precision motion control and multi-sensor information fusion technology, and in particular to a method for real-time three-dimensional trajectory tracking of robot end effector points based on asynchronous fusion. Background Technology

[0002] With the rapid development of intelligent manufacturing, precision inspection, micro-nano manipulation, and medical robotics technologies, robot systems are playing an increasingly important role in fields such as semiconductor manufacturing, precision optical assembly, laser micro-nano processing, cell manipulation, minimally invasive medical surgery, and high-end industrial automated inspection. In these application scenarios, robots typically need to complete high-speed, high-precision, and high-stability motion tasks within a limited space. The real-time measurement accuracy of the motion trajectory of its end effector directly affects the robot's control accuracy, operational safety, and task execution reliability.

[0003] In actual robot operation, the movement of the robot's end effector typically occurs in three-dimensional space. Its trajectory includes not only planar displacement but also depth displacement, out-of-plane offset, and spatial positional deviations caused by changes in end effector posture. Especially in tasks such as minimally invasive injection, cell manipulation, precision assembly, and micro / nano fabrication, the objects that actually need to be controlled and observed are often not single visual markers, but rather tool tips, microneedle tips, gripper ends, or actual working points rigidly connected to the robot's end effector. When there are slight posture changes or structural offsets in the robot's end effector, a significant difference may occur between the position of the visual marker and the actual working point. If the true three-dimensional spatial trajectory of the working point cannot be obtained, it will directly affect the robot's operational accuracy and safety.

[0004] Current methods for acquiring robot motion information mainly rely on single-type sensors such as vision sensors or high-frequency sensors. However, a single sensor cannot simultaneously meet the requirements of 3D spatial measurement, high sampling rate, and high real-time performance. Binocular stereo vision systems can obtain the 3D spatial information of a target through the parallax of the left and right images, making them suitable for providing global position observation of the robot's end effector. However, they require image acquisition, feature matching, parallax calculation, and 3D reconstruction, and are susceptible to data delays under high-speed motion conditions due to factors such as image acquisition frame rate, exposure time, and the computational load of the vision algorithm. High-frequency sensors, such as laser displacement sensors, eddy current sensors, or inertial measurement units, while possessing high sampling rates and fast response speeds, typically can only acquire local displacement or local state information along a single measurement axis, making it difficult to independently reconstruct the complete 3D spatial trajectory of the robot's end effector.

[0005] To obtain the 3D trajectory of a robot's end effector during high-speed motion, relying solely on a binocular stereo vision system is easily limited by the visual frame rate and 3D reconstruction time, while relying solely on a high-frequency sensor is insufficient to provide complete spatial position information. Therefore, it is necessary to fuse the global 3D observation capability of a binocular stereo vision system with the high-speed response capability of a high-frequency sensor. This allows the visual sensor to provide a 3D spatial reference, while the high-frequency sensor provides high-frequency dynamic compensation, thereby simultaneously improving the accuracy, real-time performance, and stability of 3D trajectory estimation.

[0006] Meanwhile, binocular stereo vision systems and high-frequency sensors typically operate on asynchronous data streams. Binocular stereo vision systems have a lower output frequency and suffer from image processing delays, while high-frequency sensors usually output local measurement data continuously at a higher sampling frequency. Simply assuming that the two are synchronized, or performing interpolation, prediction, or weighting based solely on a fixed sampling rate relationship, makes it difficult to accurately reflect the actual temporal relationship between visual observation, high-frequency observation, and prediction states under high-speed 3D robot motion conditions. This can easily introduce phase lag, abrupt changes in depth direction error, and distortion in 3D trajectory estimation. Summary of the Invention

[0007] The technical problem to be solved by the present invention is to provide a real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion. Under asynchronous sampling conditions, it can effectively fuse binocular vision information and high-frequency sensor data to obtain the real-time motion trajectory of the robot end effector points in three-dimensional space, thus meeting the requirements for high-precision, real-time three-dimensional trajectory tracking in complex dynamic environments.

[0008] The technical solution adopted by this invention to solve the above-mentioned technical problems is: a real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion, comprising the following specific steps: S1. Transform the coordinate system of the binocular stereo vision system to the same coordinate system as the high-frequency sensor, and establish a fixed geometric relationship between the robot end effector point and the robot end effector. S2, a binocular stereo vision system, and a high-frequency sensor respectively acquire image frames and high-frequency component data of the robot under test. Then, optical flow is used to predict the estimated position of feature points on the end effector of the robot under test in the current image frame and to establish a dynamic region of interest. Then, at least three non-collinear feature points are reconstructed in three dimensions within the dynamic region of interest to calculate the visual three-dimensional position observation value of the robot's end effector. At the same time, the high-frequency component data is used to calculate the high-frequency three-dimensional position observation value of the robot's end effector through the three-dimensional system sensitivity matrix and its inverse matrix. S3. Establish a two-way parallel binocular vision local estimator and a high-frequency local estimator, taking the visual three-dimensional position observation value and the high-frequency three-dimensional position observation value as inputs respectively, and outputting the visual local state estimate and visual local error covariance matrix, as well as the high-frequency local state estimate and high-frequency local error covariance matrix at the current discrete sampling time. S4. Set multiple different asynchronous observation states, and determine the asynchronous observation state to which the two outputs in step S3 belong at the current discrete sampling time. Then, establish the cross-covariance matrix between the visual local error covariance and the high-frequency local error covariance according to the asynchronous observation state. S5. Use the Lagrange multiplier method to establish the Lagrange auxiliary function, then introduce unbiased estimation constraints, and solve to obtain the visual optimal weighting matrix and the high-frequency optimal weighting matrix. S6. Using the optimal visual weighting matrix and the optimal high-frequency weighting matrix as weights, the visual local state estimate and the high-frequency local state estimate at the current discrete sampling time are weighted and summed to obtain the optimal estimate of the real-time three-dimensional trajectory of the end-effector of the robot under test.

[0009] Furthermore, in step S1, the high-frequency sensor is either a laser displacement sensor or an eddy current sensor. At least three high-frequency sensors are installed non-coplanarly. The process for establishing the spatial measurement relationship of the high-frequency sensors is as follows: The unit direction vector of the effective measurement axis of each high-frequency sensor in the robot's reference coordinate system is obtained through calibration, and a full-rank three-dimensional system sensitivity matrix and its inverse matrix are constructed based on each unit direction vector.

[0010] Furthermore, in step S1, the binocular stereo vision system consists of two high-speed industrial cameras arranged side-by-side, and its coordinate system transformation method is as follows: Two high-speed industrial cameras are used for binocular calibration, with the left camera coordinate system as the camera coordinate system. Then, the rigid body transformation matrix from the camera coordinate system to the robot reference coordinate system is obtained through hand-eye calibration, realizing the transformation between the camera coordinate system and the robot reference coordinate system, so that the binocular stereo vision system and the high-frequency sensor are in the same robot reference coordinate system.

[0011] Furthermore, in step S1, the process of establishing the fixed geometric relationship between the robot's end effector point and the robot's end effector is as follows: A robot end-effector coordinate system is established at the robot end-effector, which moves together with the robot end-effector. At least three non-collinear feature points are set at the robot end-effector, and each feature point is rigidly connected to the robot end-effector. The fixed three-dimensional coordinates of each feature point in the robot end-effector coordinate system are calibrated offline. The relative geometric relationship between each feature point remains unchanged during the robot's movement. At the same time, the fixed position vector of the robot end-effector operation point in the robot end-effector coordinate system is calibrated offline. The relative positional relationship between the robot end-effector operation point and each feature point also remains unchanged.

[0012] Furthermore, in step S2, the method for establishing the dynamically generated region of interest is as follows: First, using the actual coordinates of the feature points on the end effector of the robot under test in the previous image frame, the pixel displacement vector of the feature point between consecutive image frames is traced using the Lucas-Kanade optical flow algorithm. Then, based on the pixel displacement vector and the sampling time interval between two consecutive image frames... The instantaneous velocity of the feature point in the image coordinate system is calculated in real time, and then the estimated position of the feature point in the current image frame is predicted based on the instantaneous velocity. pred v pred ): , Where: u pred v represents the estimated horizontal coordinates of a feature point on the end effector of the robot under test in the current image frame. pred This represents the estimated vertical coordinates of a feature point on the end effector of the robot under test in the current image frame, where t represents the sampling time of the current image frame, t-1 represents the sampling time of the previous image frame, and u... t-1 This represents the actual horizontal coordinates of the feature point on the end effector of the robot under test in the previous image frame; v t-1 v represents the actual vertical coordinates of a feature point on the end effector of the robot under test in the previous image frame. u v represents the instantaneous velocity component of the pixel displacement vector in the horizontal direction in the image coordinate system. v This represents the instantaneous velocity component of the pixel displacement vector in the vertical direction within the image coordinate system. This represents the sampling time interval between two consecutive image frames, i.e., the single-frame sampling period of a binocular stereo vision system. Finally, the estimated position of this feature point in the current image frame (u) is used. pred v pred Using as the geometric center, a dynamic region of interest of fixed size is automatically generated.

[0013] Furthermore, in step S2, the process of calculating the visual three-dimensional position observation value of the robot's end effector is as follows: In the dynamic region of interest, at least three non-collinear feature points are reconstructed in 3D to obtain their 3D coordinates in the camera coordinate system. Then, based on the rigid body transformation matrix from the camera coordinate system to the robot reference coordinate system, each feature point is transformed to the robot reference coordinate system to obtain its 3D coordinates. Next, based on the fixed 3D coordinates of each feature point in the robot end effector coordinate system and the 3D coordinates of each feature point in the robot reference coordinate system, the spatial pose matrix of the robot end effector coordinate system relative to the robot reference coordinate system is solved. The spatial pose matrix is ​​used to transform the 3D coordinates in the robot end effector coordinate system to the robot reference coordinate system. Finally, based on the spatial pose matrix and the fixed position vector of the robot end effector in the robot end effector coordinate system, the visual 3D position observation value of the robot end effector in the robot reference coordinate system is calculated.

[0014] Furthermore, in step S3, the process of setting the current discrete sampling time k is as follows: The moment when visual 3D position observations and high-frequency 3D position observations are first simultaneously input into the corresponding binocular visual local estimator and high-frequency local estimator is defined as the initial discrete sampling moment, i.e., k=0. Then, the fixed sampling period of the high-frequency sensor is used as the recursive step size. Starting from the initial discrete sampling moment, the discrete sampling moment is recursively pushed from k to k+1 after each fixed sampling period of the high-frequency sensor.

[0015] Furthermore, in step S3, before establishing the binocular vision local estimator and the high-frequency local estimator, the end-effector state vector of the robot under test at the current discrete sampling time k is defined as X(k), and the discrete state transition equation of the end-effector state vector X(k) is established: , Where: X(k+1) represents the end state vector of the end point of the robot under test at the next discrete sampling time k+1, F(k) represents the state transition matrix of the end point of the robot under test at the current discrete sampling time, which describes the evolution relationship of the end state vector X(k) of the end point of the robot under test with the discrete sampling time, and ω(k) represents the process noise vector with the same dimension as the end state vector at the current discrete sampling time. When the binocular vision local estimator receives visual 3D position observations at the current discrete sampling time, it outputs the visual local state estimate at the current discrete sampling time through a Kalman filter recursive algorithm. and the visual local error covariance matrix P c (k); When there is no visual 3D position observation input at the current discrete sampling time, the output is predicted by the discrete state transition equation of the end effector point of the robot under test, and the prediction result is used as the visual local state estimate at the current discrete sampling time. and the visual local error covariance matrix P c (k); When the high-frequency local estimator receives high-frequency three-dimensional position observations at the current discrete sampling time, it outputs the high-frequency local state estimate for the current discrete sampling time through a Kalman filter recursive algorithm. and the high-frequency local error covariance matrix P l (k); When there is no high-frequency three-dimensional position observation input at the current discrete sampling time, the output is predicted by the discrete state transition equation of the end effector point of the robot under test, and the prediction result is used as the high-frequency local state estimate at the current discrete sampling time. and the high-frequency local error covariance matrix P l (k).

[0016] Furthermore, in step S4, the asynchronous observation state is divided into four types, specifically: a. Dual-channel pure prediction state: When neither the visual 3D position observations nor the high-frequency 3D position observations are input to the corresponding binocular visual local estimator and high-frequency local estimator at the current discrete sampling time, only the time update at the discrete sampling time is performed. The cross-covariance matrix is ​​composed of the state transition matrix F(k) and the process noise covariance matrix Q. l The recursion is as follows: , in: This represents the cross-covariance matrix between the visual local error covariance and the high-frequency local error covariance predicted by the discrete state transition equation at the current discrete sampling time. This represents the cross-covariance matrix updated by measurement at the previous discrete sampling time. The superscript T indicates the transpose of the matrix, and Q... l The process noise covariance matrix represents the end effector point of the robot under test, and is used to express the uncertainty of the motion model of the robot under test caused by external environmental interference, friction, or unmodeled dynamics of the robot under test. b. Visual Independent Update State: When the visual 3D position observations at the current discrete sampling time are input to the binocular visual local estimator, but the high-frequency 3D position observations are not input to the high-frequency local estimator, the binocular visual local estimator updates its measurement based on the visual 3D position observations, while the high-frequency local estimator makes predictions based on the discrete state transition equations. In this case, the visual Kalman gain matrix K is used. c After one-sided correction of the cross-covariance matrix, the recursive formula for the cross-covariance matrix is ​​as follows: , in: K represents the cross-covariance matrix updated by measurement at the current discrete sampling time, where I represents the identity matrix matching the end-effector state vector X(k) of the robot under test; c H represents the visual Kalman gain matrix of the binocular vision local estimator at the current discrete sampling time. c This represents the observation matrix of the binocular vision local estimator, whose physical function is to extract and map the corresponding spatial location observation variables from the end state vector X(k); c. High-Frequency Independent Update State: When the visual 3D position observation is not input to the binocular vision local estimator at the current discrete sampling time, but the high-frequency 3D position observation is input to the high-frequency local estimator, the high-frequency local estimator updates its measurement based on the high-frequency 3D position observation, while the binocular vision local estimator makes predictions based on the discrete state transition equation; at this time, the high-frequency Kalman gain matrix K is used. l After one-sided correction of the cross-covariance matrix, the recursive formula for the cross-covariance matrix is ​​as follows: , Where: K l H represents the high-frequency Kalman gain matrix of the high-frequency local estimator at the current discrete sampling time. l This represents the observation matrix of the high-frequency local estimator; d. Dual-channel synchronous update state: When the visual 3D position observations are input to the binocular visual local estimator at the current discrete sampling time, and the high-frequency 3D position observations are input to the high-frequency local estimator, the binocular visual local estimator updates its measurement based on the visual 3D position observations, and the high-frequency local estimator updates its measurement based on the high-frequency 3D position observations. At this time, the visual Kalman gain matrix K is used simultaneously. c With the high-frequency Kalman gain matrix K l After two-sided correction of the cross covariance matrix, the recursive formula for the cross covariance matrix is ​​as follows: .

[0017] Furthermore, in step S5, the relationship of the Lagrange auxiliary function is: , , Where: L(k) is the Lagrange auxiliary function established at the current discrete sampling time, W c (k) represents the visual optimal weighting matrix to be solved, W l (k) represents the high-frequency optimal weighting matrix to be solved, P f (k) represents the global error covariance matrix. The trace operator represents a matrix, used to sum the elements along the main diagonal of the matrix; The introduced Lagrange multiplier matrix serves to mathematically integrate the unbiased constraints into the Lagrange auxiliary function; P cl (k) represents the cross-covariance matrix obtained in step S4 at the current discrete sampling time based on the asynchronous observation state, denoted here as P. cl (k), P lc (k) denotes the transpose of the cross covariance matrix, i.e. ; The optimal weighting matrix W for vision c (k) and the high-frequency optimal weighting matrix W l The solution process for (k) is as follows: Let the Lagrange auxiliary function L(k) be applied to the visually optimal weighting matrix W respectively. c (k) High-frequency optimal weighting matrix W l (k) and Lagrange multiplier matrix Find the partial derivatives and set each partial derivative to zero, then combine this with the unbiased estimation constraint W. c (k)+W l (k)=I, thus obtaining the visually optimal weighting matrix W. c (k) is: , Among them: superscript Represents the matrix inversion operation; Based on the unbiased estimation constraints, the high-frequency optimal weighting matrix W is obtained. l (k) is: W l (k)=I-W c (k).

[0018] Furthermore, the specific implementation process of step S6 is as follows: S6.1 First, calculate the visually optimal weighted matrix W. c (k) and the high-frequency optimal weighting matrix W l (k) serves as the weight for the visual local state estimation at the current discrete sampling time. and high-frequency local state estimation By performing a weighted summation, the globally optimal state estimate of the robot's end effector point at the current discrete sampling time is obtained. The relation is: ; S6.2. At the next discrete sampling time k+1, estimate the global optimal state at the current discrete sampling time. and the global error covariance matrix P f(k) serves as the basis for the recursion of the next discrete sampling time. Steps S3 to S6.1 are repeated cyclically to obtain the global optimal state estimate and global error covariance matrix of the next discrete sampling time. As the discrete sampling time continues to advance, the continuously obtained global optimal state estimates are seamlessly spliced ​​together to obtain the optimal estimate of the real-time three-dimensional trajectory of the end operation point of the robot under test.

[0019] Compared with the prior art, the advantages of the present invention are: (1) This method is for real-time three-dimensional trajectory tracking of robot end-effectors. It uses a binocular stereo vision system to reconstruct three-dimensional features of at least three non-collinear feature points of the robot end-effectors and calculates the visual three-dimensional position observation of the robot end-effectors based on the rigid geometric relationship between the feature points. This method can obtain the real trajectory information of the robot end-effectors in three-dimensional space and avoid the spatial position deviation caused by trajectory estimation based on a single marker point or planar projection information. (2) This method introduces a dynamic region of interest (ROI) generation mechanism based on optical flow. It performs 3D reconstruction of feature points only within the predicted dynamic region of interest, transforming global search into local processing. This significantly reduces the computational load of visual operators and improves real-time performance. At the same time, it reduces latency and ensures high weight of visual data in the fusion algorithm, avoiding the problem of high-precision visual data being down-weighted due to time delay in traditional algorithms. (3) Based on the actual input of visual three-dimensional position observations and high-frequency three-dimensional position observations at the current discrete sampling time, this method divides the observations into four asynchronous observation states: dual-channel pure prediction, visual independent update, high-frequency independent update, and dual-channel synchronous update. The cross-covariance matrix is ​​then recursively derived to fully cover the different input states of visual observation and high-frequency observation. This enables the fusion process to predict or update based on the actual observation at the current discrete sampling time, thereby improving the adaptability and stability of asynchronous fusion. (4) This method describes visual local errors, high-frequency local errors and the cross-error between them in the three-dimensional state space. It can simultaneously consider planar direction errors, depth direction errors and the propagation of coupling errors between the depth direction and the planar direction, thereby improving the completeness and stability of the three-dimensional trajectory estimation of the robot's end-effector. (5) This method uses the Lagrange multiplier method to obtain the optimal weighted matrix for vision and the optimal weighted matrix for high frequency, so that the trace of the fused global error covariance matrix is ​​minimized, thereby realizing the optimal fusion of heterogeneous and asynchronous 3D data from binocular vision and high frequency sensors, and further improving the accuracy and robustness of 3D trajectory estimation of robot end-point operation points. Attached Figure Description

[0020] Figure 1 This is a flowchart of the present invention; Figure 2 This is a comparison diagram of trajectory estimates obtained by the present invention and the random weighted estimation fusion method; Figure 3 This is a comparison chart of the displacement errors obtained by the present invention and the random weighted estimation fusion method. Detailed Implementation

[0021] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0022] The high-frequency sensor involved in the method of this embodiment is at least three non-coplanar laser displacement sensors. The binocular stereo vision system consists of two high-speed industrial cameras arranged side by side. The object under test is a robot. This method is used to obtain the real-time three-dimensional trajectory of the robot's end-effector operation point. The robot's end-effector operation point can be the center point of the robot's end-effector, the tip of a tool, the tip of a microneedle, the end of a gripper, or other actual working points rigidly connected to the robot's end-effector.

[0023] like Figure 1 As shown, the real-time 3D trajectory tracking method for robot end effector points based on asynchronous fusion includes the following specific steps: S1. Transform the coordinate system of the binocular stereo vision system to the same coordinate system as the high-frequency sensor, and establish a fixed geometric relationship between the robot's end effector point and the robot's end effector, specifically: S1.1. Calibrate and obtain the unit direction vector of the effective measurement axis of each high-frequency sensor in the robot's reference coordinate system, and construct a full-rank three-dimensional system sensitivity matrix and its inverse matrix based on each unit direction vector to establish the spatial measurement relationship of the high-frequency sensors. S1.2 Perform binocular calibration on two high-speed industrial cameras, and use the left camera coordinate system as the camera coordinate system. Then, obtain the rigid body transformation matrix from the camera coordinate system to the robot reference coordinate system through the hand-eye calibration method to realize the transformation between the camera coordinate system and the robot reference coordinate system, so that the binocular stereo vision system and the high-frequency sensor are in the same robot reference coordinate system. S1.3 Establish a robot end-effector coordinate system that moves with the robot end-effector. Set at least three non-collinear feature points on the robot end-effector. Each feature point is rigidly connected to the robot end-effector. Offline calibrate the fixed three-dimensional coordinates of each feature point in the robot end-effector coordinate system. The relative geometric relationship between each feature point remains unchanged during the robot's movement. At the same time, offline calibrate the fixed position vector of the robot end-effector operation point in the robot end-effector coordinate system. The relative positional relationship between the robot end-effector operation point and each feature point also remains unchanged. S2, a binocular stereo vision system, and a high-frequency sensor respectively acquire image frames and high-frequency component data of the robot under test. Then, optical flow is used to predict the estimated position of feature points on the end effector of the robot in the current image frame. Specifically: First, using the actual coordinates of the feature points on the end effector of the robot under test in the previous image frame, the pixel displacement vector of the feature point between consecutive image frames is traced using the Lucas-Kanade optical flow algorithm. Then, based on the pixel displacement vector and the sampling time interval between two consecutive image frames... The instantaneous velocity of the feature point in the image coordinate system is calculated in real time, and then the estimated position of the feature point in the current image frame is predicted based on the instantaneous velocity. pred v pred ): , Where: u pred v represents the estimated horizontal coordinates of a feature point on the end effector of the robot under test in the current image frame. pred This represents the estimated vertical coordinates of a feature point on the end effector of the robot under test in the current image frame, where t represents the sampling time of the current image frame, t-1 represents the sampling time of the previous image frame, and u... t-1 This represents the actual horizontal coordinates of the feature point on the end effector of the robot under test in the previous image frame; v t-1 v represents the actual vertical coordinates of a feature point on the end effector of the robot under test in the previous image frame. u v represents the instantaneous velocity component of the pixel displacement vector in the horizontal direction in the image coordinate system. v This represents the instantaneous velocity component of the pixel displacement vector in the vertical direction within the image coordinate system. This represents the sampling time interval between two consecutive image frames, i.e., the single-frame sampling period of a binocular stereo vision system. Then, the estimated position of this feature point in the current image frame (u) pred v pred Using as the geometric center, a dynamic region of interest of fixed size is automatically generated; Then, 3D reconstruction is performed on at least three non-collinear feature points within the dynamic region of interest to obtain the 3D coordinates of each feature point in the camera coordinate system. Next, based on the rigid body transformation matrix from the camera coordinate system to the robot reference coordinate system, each feature point is transformed to the robot reference coordinate system, obtaining its 3D coordinates. Then, based on the fixed 3D coordinates of each feature point in the robot end effector coordinate system and its 3D coordinates in the robot reference coordinate system, the spatial pose matrix of the robot end effector coordinate system relative to the robot reference coordinate system is solved. This spatial pose matrix is ​​used to transform the 3D coordinates from the robot end effector coordinate system to the robot reference coordinate system. Finally, based on the spatial pose matrix and the fixed position vector of the robot end effector in the robot end effector coordinate system, the visual 3D position observation value of the robot end effector in the robot reference coordinate system is calculated. Meanwhile, based on the spatial measurement relationship of the high-frequency sensor, the high-frequency component data is solved by the full-rank three-dimensional system sensitivity matrix and its inverse matrix to obtain the high-frequency three-dimensional position observation value of the robot end effector in the robot reference coordinate system. S3. First, set the current discrete sampling time k, specifically as follows: The moment when visual 3D position observations and high-frequency 3D position observations are first simultaneously input into the corresponding binocular visual local estimator and high-frequency local estimator is defined as the initial discrete sampling moment, i.e., k=0. Then, the fixed sampling period of the high-frequency sensor is used as the recursive step size. Starting from the initial discrete sampling moment, the discrete sampling moment is recursively pushed from k to k+1 after each fixed sampling period of the high-frequency sensor. Define the end-effector state vector of the robot under test at the current discrete sampling time k as X(k), and establish the discrete state transition equation of the end-effector state vector X(k): , Where: X(k+1) represents the end state vector of the end point of the robot under test at the next discrete sampling time k+1, F(k) represents the state transition matrix of the end point of the robot under test at the current discrete sampling time, which describes the evolution relationship of the end state vector X(k) of the end point of the robot under test with the discrete sampling time, and ω(k) represents the process noise vector with the same dimension as the end state vector at the current discrete sampling time. Then, a two-way parallel binocular vision local estimator and a high-frequency local estimator are established, with visual 3D position observations and high-frequency 3D position observations as inputs, respectively: When the binocular vision local estimator receives visual 3D position observations at the current discrete sampling time, it outputs the visual local state estimate at the current discrete sampling time through a Kalman filter recursive algorithm. and the visual local error covariance matrix Pc (k); When there is no visual 3D position observation input at the current discrete sampling time, the output is predicted by the discrete state transition equation of the end effector point of the robot under test, and the prediction result is used as the visual local state estimate at the current discrete sampling time. and the visual local error covariance matrix P c (k); When the high-frequency local estimator receives high-frequency three-dimensional position observations at the current discrete sampling time, it outputs the high-frequency local state estimate for the current discrete sampling time through a Kalman filter recursive algorithm. and the high-frequency local error covariance matrix P l (k); When there is no high-frequency three-dimensional position observation input at the current discrete sampling time, the output is predicted by the discrete state transition equation of the end effector point of the robot under test, and the prediction result is used as the high-frequency local state estimate at the current discrete sampling time. and the high-frequency local error covariance matrix P l (k); S4. Based on the input of visual 3D position observations and high-frequency 3D position observations at the current discrete sampling time, four different asynchronous observation states are set, and the asynchronous observation state to which the two outputs from step S3 belong at the current discrete sampling time is determined. Then, based on the corresponding asynchronous observation state, a cross-covariance matrix between the visual local error covariance and the high-frequency local error covariance is established, specifically as follows: a. Dual-channel pure prediction state: When neither the visual 3D position observations nor the high-frequency 3D position observations are input to the corresponding binocular visual local estimator and high-frequency local estimator at the current discrete sampling time, only the time update at the discrete sampling time is performed. The cross-covariance matrix is ​​composed of the state transition matrix F(k) and the process noise covariance matrix Q. l The recursion is as follows: , in: This represents the cross-covariance matrix between the visual local error covariance and the high-frequency local error covariance predicted by the discrete state transition equation at the current discrete sampling time. This represents the cross-covariance matrix updated by measurement at the previous discrete sampling time. The superscript T indicates the transpose of the matrix, and Q... l The process noise covariance matrix represents the end effector point of the robot under test, and is used to express the uncertainty of the motion model of the robot under test caused by external environmental interference, friction, or unmodeled dynamics of the robot under test. b. Visual Independent Update State: When the visual 3D position observations at the current discrete sampling time are input to the binocular visual local estimator, but the high-frequency 3D position observations are not input to the high-frequency local estimator, the binocular visual local estimator updates its measurement based on the visual 3D position observations, while the high-frequency local estimator makes predictions based on the discrete state transition equations. In this case, the visual Kalman gain matrix K is used. c After one-sided correction of the cross-covariance matrix, the recursive formula for the cross-covariance matrix is ​​as follows: , in: K represents the cross-covariance matrix updated by measurement at the current discrete sampling time, where I represents the identity matrix matching the end-effector state vector X(k) of the robot under test; c H represents the visual Kalman gain matrix of the binocular vision local estimator at the current discrete sampling time. c This represents the observation matrix of the binocular vision local estimator, whose physical function is to extract and map the corresponding spatial location observation variables from the end state vector X(k); c. High-Frequency Independent Update State: When the visual 3D position observation is not input to the binocular vision local estimator at the current discrete sampling time, but the high-frequency 3D position observation is input to the high-frequency local estimator, the high-frequency local estimator updates its measurement based on the high-frequency 3D position observation, while the binocular vision local estimator makes predictions based on the discrete state transition equation; at this time, the high-frequency Kalman gain matrix K is used. l After one-sided correction of the cross-covariance matrix, the recursive formula for the cross-covariance matrix is ​​as follows: , Where: K l H represents the high-frequency Kalman gain matrix of the high-frequency local estimator at the current discrete sampling time. l This represents the observation matrix of the high-frequency local estimator; d. Dual-channel synchronous update state: When the visual 3D position observations are input to the binocular visual local estimator at the current discrete sampling time, and the high-frequency 3D position observations are input to the high-frequency local estimator, the binocular visual local estimator updates its measurement based on the visual 3D position observations, and the high-frequency local estimator updates its measurement based on the high-frequency 3D position observations. At this time, the visual Kalman gain matrix K is used simultaneously. c With the high-frequency Kalman gain matrix K l After two-sided correction of the cross covariance matrix, the recursive formula for the cross covariance matrix is ​​as follows: ; S5. Using the Lagrange multiplier method, establish the Lagrange auxiliary function, whose relation is: , , Where: L(k) is the Lagrange auxiliary function established at the current discrete sampling time, W c (k) represents the visual optimal weighting matrix to be solved, W l (k) represents the high-frequency optimal weighting matrix to be solved, P f (k) represents the global error covariance matrix. The trace operator represents a matrix, used to sum the elements along the main diagonal of the matrix; The introduced Lagrange multiplier matrix serves to mathematically integrate the unbiased constraints into the Lagrange auxiliary function; P cl (k) represents the cross-covariance matrix obtained in step S4 at the current discrete sampling time based on the asynchronous observation state, denoted here as P. cl (k), P lc (k) denotes the transpose of the cross covariance matrix, i.e. ; Then, the unbiased estimation constraint W is introduced. c (k)+W l (k)=I, for the visually optimal weighting matrix W c (k) and the high-frequency optimal weighting matrix W l (k) is solved: Let the Lagrange auxiliary function L(k) be applied to the visually optimal weighting matrix W respectively. c (k) High-frequency optimal weighting matrix W l (k) and Lagrange multiplier matrix By taking the partial derivatives and setting each partial derivative to zero, and combining this with the unbiased estimation constraint, we obtain the visually optimal weighting matrix W. c (k) is: , Among them: superscript Represents the matrix inversion operation; Based on the unbiased estimation constraints, the high-frequency optimal weighting matrix W is obtained. l (k) is: W l (k)=I-W c (k); S6.1 First, calculate the visually optimal weighted matrix W. c (k) and the high-frequency optimal weighting matrix W l (k) serves as the weight for the visual local state estimation at the current discrete sampling time. and high-frequency local state estimation By performing a weighted summation, the globally optimal state estimate of the robot's end effector point at the current discrete sampling time is obtained. The relation is: ; S6.2. At the next discrete sampling time k+1, estimate the global optimal state at the current discrete sampling time. and the global error covariance matrix P f (k) serves as the basis for the recursion of the next discrete sampling time. Steps S3 to S6.1 are repeated cyclically to obtain the global optimal state estimate and global error covariance matrix of the next discrete sampling time. As the discrete sampling time continues to advance, the continuously obtained global optimal state estimates are seamlessly spliced ​​together to obtain the optimal estimate of the real-time three-dimensional trajectory of the end operation point of the robot under test.

[0024] To verify the trajectory estimation effect of this invention, the real-time three-dimensional trajectory estimation results of the same end-effector operation point of the robot under test were obtained by comparing the existing random weighted estimation fusion method with the real-time three-dimensional trajectory tracking method of this invention. Figure 2 As shown in the figure, the 3D estimated trajectory obtained by this invention basically coincides with the real trajectory, and it can still fit the real trajectory well in the local magnified area, while the random weighted estimation fusion result has a certain deviation. Furthermore, from... Figure 3 The displacement error comparison results show that, within the test time of 0–1 s, the displacement error of the 3D trajectory of the end effector point of the robot under test obtained by the random weighted estimation fusion method is mainly distributed in the range of approximately 15–35 μm, with a local peak exceeding 40 μm; while the displacement error of the 3D trajectory of the end effector point of the robot under test obtained by the present invention is mainly distributed in the range of approximately 3–12 μm, with a local peak of approximately 20 μm. Therefore, compared with the existing random weighted estimation fusion method, the present invention can significantly reduce the estimation error of the 3D trajectory, reduce error fluctuation, and improve the accuracy and stability of the optimal estimation of the real-time 3D trajectory of the end effector point of the robot under test.

[0025] The scope of protection of this invention includes, but is not limited to, the above embodiments. The scope of protection is defined by the claims. Any substitutions, modifications, or improvements to this technology that are easily conceived by those skilled in the art fall within the scope of protection of this invention.

Claims

1. A method for real-time 3D trajectory tracking of robot end effector points based on asynchronous fusion, characterized in that... The specific steps include the following: S1. Transform the coordinate system of the binocular stereo vision system to the same coordinate system as the high-frequency sensor, and establish a fixed geometric relationship between the robot end effector point and the robot end effector. S2, a binocular stereo vision system, and a high-frequency sensor respectively acquire image frames and high-frequency component data of the robot under test. Then, optical flow is used to predict the estimated position of feature points on the end effector of the robot under test in the current image frame and to establish a dynamic region of interest. Then, at least three non-collinear feature points are reconstructed in three dimensions within the dynamic region of interest to calculate the visual three-dimensional position observation value of the robot's end effector. At the same time, the high-frequency component data is used to calculate the high-frequency three-dimensional position observation value of the robot's end effector through the three-dimensional system sensitivity matrix and its inverse matrix. S3. Establish a two-way parallel binocular vision local estimator and a high-frequency local estimator, taking the visual three-dimensional position observation value and the high-frequency three-dimensional position observation value as inputs respectively, and outputting the visual local state estimate and visual local error covariance matrix, as well as the high-frequency local state estimate and high-frequency local error covariance matrix at the current discrete sampling time. S4. Set multiple different asynchronous observation states, and determine the asynchronous observation state to which the two outputs in step S3 belong at the current discrete sampling time. Then, establish the cross-covariance matrix between the visual local error covariance and the high-frequency local error covariance according to the asynchronous observation state. S5. Use the Lagrange multiplier method to establish the Lagrange auxiliary function, then introduce unbiased estimation constraints, and solve to obtain the visual optimal weighting matrix and the high-frequency optimal weighting matrix. S6. Using the optimal visual weighting matrix and the optimal high-frequency weighting matrix as weights, the visual local state estimate and the high-frequency local state estimate at the current discrete sampling time are weighted and summed to obtain the optimal estimate of the real-time three-dimensional trajectory of the end-effector of the robot under test.

2. The real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion as described in claim 1, characterized in that: In step S1, the high-frequency sensor uses a laser displacement sensor or an eddy current sensor. At least three high-frequency sensors are used and are not coplanarly installed. The process for establishing the spatial measurement relationship of the high-frequency sensors is as follows: The unit direction vector of the effective measurement axis of each high-frequency sensor in the robot's reference coordinate system is obtained through calibration, and a full-rank three-dimensional system sensitivity matrix and its inverse matrix are constructed based on each unit direction vector.

3. The real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion as described in claim 2, characterized in that: In step S1, the binocular stereo vision system consists of two high-speed industrial cameras arranged side-by-side, and its coordinate system transformation method is as follows: Two high-speed industrial cameras are used for binocular calibration, with the left camera coordinate system as the camera coordinate system. Then, the rigid body transformation matrix from the camera coordinate system to the robot reference coordinate system is obtained through hand-eye calibration, realizing the transformation between the camera coordinate system and the robot reference coordinate system, so that the binocular stereo vision system and the high-frequency sensor are in the same robot reference coordinate system.

4. The real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion as described in claim 3, characterized in that: In step S1, the process of establishing the fixed geometric relationship between the robot's end effector point and the robot's end effector is as follows: A robot end-effector coordinate system is established at the robot end-effector, which moves together with the robot end-effector. At least three non-collinear feature points are set at the robot end-effector, and each feature point is rigidly connected to the robot end-effector. The fixed three-dimensional coordinates of each feature point in the robot end-effector coordinate system are calibrated offline. The relative geometric relationship between each feature point remains unchanged during the robot's movement. At the same time, the fixed position vector of the robot end-effector operation point in the robot end-effector coordinate system is calibrated offline. The relative positional relationship between the robot end-effector operation point and each feature point also remains unchanged.

5. The real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion as described in claim 4, characterized in that: In step S2, the method for establishing the dynamically generated region of interest is as follows: First, using the actual coordinates of the feature points on the end effector of the robot under test in the previous image frame, the pixel displacement vector of the feature point between consecutive image frames is traced using the Lucas-Kanade optical flow algorithm. Then, based on the pixel displacement vector and the sampling time interval between two consecutive image frames... The instantaneous velocity of the feature point in the image coordinate system is calculated in real time, and then the estimated position of the feature point in the current image frame is predicted based on the instantaneous velocity. pred v pred ): ， Where: u pred v represents the estimated horizontal coordinates of a feature point on the end effector of the robot under test in the current image frame. pred This represents the estimated vertical coordinates of a feature point on the end effector of the robot under test in the current image frame, where t represents the sampling time of the current image frame, t-1 represents the sampling time of the previous image frame, and u... t-1 This represents the actual horizontal coordinates of the feature point on the end effector of the robot under test in the previous image frame; v t-1 v represents the actual vertical coordinates of a feature point on the end effector of the robot under test in the previous image frame. u v represents the instantaneous velocity component of the pixel displacement vector in the horizontal direction in the image coordinate system. v This represents the instantaneous velocity component of the pixel displacement vector in the vertical direction within the image coordinate system. This represents the sampling time interval between two consecutive image frames, i.e., the single-frame sampling period of a binocular stereo vision system. Finally, the estimated position of this feature point in the current image frame (u) is used. pred v pred Using as the geometric center, a dynamic region of interest of fixed size is automatically generated.

6. The real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion as described in claim 5, characterized in that: In step S2, the process of calculating the visual three-dimensional position observations of the robot's end effector is as follows: In the dynamic region of interest, at least three non-collinear feature points are reconstructed in 3D to obtain their 3D coordinates in the camera coordinate system. Then, based on the rigid body transformation matrix from the camera coordinate system to the robot reference coordinate system, each feature point is transformed to the robot reference coordinate system to obtain its 3D coordinates. Next, based on the fixed 3D coordinates of each feature point in the robot end effector coordinate system and the 3D coordinates of each feature point in the robot reference coordinate system, the spatial pose matrix of the robot end effector coordinate system relative to the robot reference coordinate system is solved. The spatial pose matrix is ​​used to transform the 3D coordinates in the robot end effector coordinate system to the robot reference coordinate system. Finally, based on the spatial pose matrix and the fixed position vector of the robot end effector in the robot end effector coordinate system, the visual 3D position observation value of the robot end effector in the robot reference coordinate system is calculated.

7. The real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion as described in claim 1, characterized in that: In step S3, the process of setting the current discrete sampling time k is as follows: The moment when visual 3D position observations and high-frequency 3D position observations are first simultaneously input into the corresponding binocular visual local estimator and high-frequency local estimator is defined as the initial discrete sampling moment, i.e., k=0. Then, the fixed sampling period of the high-frequency sensor is used as the recursive step size. Starting from the initial discrete sampling moment, the discrete sampling moment is recursively pushed from k to k+1 after each fixed sampling period of the high-frequency sensor.

8. The real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion as described in claim 7, characterized in that: In step S3, before establishing the binocular vision local estimator and the high-frequency local estimator, the end-effector state vector of the robot under test at the current discrete sampling time k is defined as X(k), and the discrete state transition equation of the end-effector state vector X(k) is established: ， Where: X(k+1) represents the end state vector of the end point of the robot under test at the next discrete sampling time k+1, F(k) represents the state transition matrix of the end point of the robot under test at the current discrete sampling time, which describes the evolution relationship of the end state vector X(k) of the end point of the robot under test with the discrete sampling time, and ω(k) represents the process noise vector with the same dimension as the end state vector at the current discrete sampling time. When the binocular vision local estimator receives visual 3D position observations at the current discrete sampling time, it outputs the visual local state estimate at the current discrete sampling time through a Kalman filter recursive algorithm. and the visual local error covariance matrix P c (k); When there is no visual 3D position observation input at the current discrete sampling time, the output is predicted by the discrete state transition equation of the end effector point of the robot under test, and the prediction result is used as the visual local state estimate at the current discrete sampling time. and the visual local error covariance matrix P c (k); When the high-frequency local estimator receives high-frequency three-dimensional position observations at the current discrete sampling time, it outputs the high-frequency local state estimate for the current discrete sampling time through a Kalman filter recursive algorithm. and the high-frequency local error covariance matrix P l (k); When there is no high-frequency three-dimensional position observation input at the current discrete sampling time, the output is predicted by the discrete state transition equation of the end effector point of the robot under test, and the prediction result is used as the high-frequency local state estimate at the current discrete sampling time. and the high-frequency local error covariance matrix P l (k).

9. The real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion as described in claim 8, characterized in that: In step S4, the asynchronous observation state is divided into four types, specifically: a. Dual-channel pure prediction state: When neither the visual 3D position observations nor the high-frequency 3D position observations are input to the corresponding binocular visual local estimator and high-frequency local estimator at the current discrete sampling time, only the time update at the discrete sampling time is performed. The cross-covariance matrix is ​​composed of the state transition matrix F(k) and the process noise covariance matrix Q. l The recursion is as follows: ， in: This represents the cross-covariance matrix between the visual local error covariance and the high-frequency local error covariance predicted by the discrete state transition equation at the current discrete sampling time. This represents the cross-covariance matrix updated by measurement at the previous discrete sampling time. The superscript T indicates the transpose of the matrix, and Q... l The process noise covariance matrix represents the end effector point of the robot under test, and is used to express the uncertainty of the motion model of the robot under test caused by external environmental interference, friction, or unmodeled dynamics of the robot under test. b. Visual Independent Update State: When the visual 3D position observations at the current discrete sampling time are input to the binocular visual local estimator, but the high-frequency 3D position observations are not input to the high-frequency local estimator, the binocular visual local estimator updates its measurement based on the visual 3D position observations, while the high-frequency local estimator makes predictions based on the discrete state transition equations. In this case, the visual Kalman gain matrix K is used. c After one-sided correction of the cross-covariance matrix, the recursive formula for the cross-covariance matrix is ​​as follows: ， in: K represents the cross-covariance matrix updated by measurement at the current discrete sampling time, where I represents the identity matrix matching the end-effector state vector X(k) of the robot under test; c H represents the visual Kalman gain matrix of the binocular vision local estimator at the current discrete sampling time. c This represents the observation matrix of the binocular vision local estimator, whose physical function is to extract and map the corresponding spatial location observation variables from the end state vector X(k); c. High-Frequency Independent Update State: When the visual 3D position observation is not input to the binocular vision local estimator at the current discrete sampling time, but the high-frequency 3D position observation is input to the high-frequency local estimator, the high-frequency local estimator updates its measurement based on the high-frequency 3D position observation, while the binocular vision local estimator makes predictions based on the discrete state transition equation; at this time, the high-frequency Kalman gain matrix K is used. l After one-sided correction of the cross-covariance matrix, the recursive formula for the cross-covariance matrix is ​​as follows: ， Where: K l H represents the high-frequency Kalman gain matrix of the high-frequency local estimator at the current discrete sampling time. l This represents the observation matrix of the high-frequency local estimator; d. Dual-channel synchronous update state: When the visual 3D position observations are input to the binocular visual local estimator at the current discrete sampling time, and the high-frequency 3D position observations are input to the high-frequency local estimator, the binocular visual local estimator updates its measurement based on the visual 3D position observations, and the high-frequency local estimator updates its measurement based on the high-frequency 3D position observations. At this time, the visual Kalman gain matrix K is used simultaneously. c With the high-frequency Kalman gain matrix K l After two-sided correction of the cross covariance matrix, the recursive formula for the cross covariance matrix is ​​as follows: 。 10. The real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion as described in claim 9, characterized in that: In step S5, the relationship of the Lagrange auxiliary function is: ， ， Where: L(k) is the Lagrange auxiliary function established at the current discrete sampling time, W c (k) represents the visual optimal weighting matrix to be solved, W l (k) represents the high-frequency optimal weighting matrix to be solved, P f (k) represents the global error covariance matrix. The trace operator represents a matrix, used to sum the elements along the main diagonal of the matrix; The introduced Lagrange multiplier matrix serves to mathematically integrate the unbiased constraints into the Lagrange auxiliary function; P cl (k) represents the cross-covariance matrix obtained in step S4 at the current discrete sampling time based on the asynchronous observation state, denoted here as P. cl (k), P lc (k) denotes the transpose of the cross covariance matrix, i.e. ; The optimal weighting matrix W for vision c (k) and the high-frequency optimal weighting matrix W l The solution process for (k) is as follows: Let the Lagrange auxiliary function L(k) be applied to the visually optimal weighting matrix W respectively. c (k) High-frequency optimal weighting matrix W l (k) and Lagrange multiplier matrix Find the partial derivatives and set each partial derivative to zero, then combine this with the unbiased estimation constraint W. c (k)+W l (k)=I, thus obtaining the visually optimal weighting matrix W. c (k) is: ， Among them: superscript Represents the matrix inversion operation; Based on the unbiased estimation constraints, the high-frequency optimal weighting matrix W is obtained. l (k) is: W l (k)=I-W c (k).

11. The real-time three-dimensional trajectory tracking method for robot end effector points based on asynchronous fusion as described in claim 10, characterized in that: The specific implementation process of step S6 is as follows: S6.1 First, calculate the visually optimal weighted matrix W. c (k) and the high-frequency optimal weighting matrix W l (k) serves as the weight for the visual local state estimation at the current discrete sampling time. and high-frequency local state estimation By performing a weighted summation, the globally optimal state estimate of the robot's end effector point at the current discrete sampling time is obtained. The relation is: ； S6.

2. At the next discrete sampling time k+1, estimate the global optimal state at the current discrete sampling time. and the global error covariance matrix P f (k) serves as the basis for the recursion of the next discrete sampling time. Steps S3 to S6.1 are repeated cyclically to obtain the global optimal state estimate and global error covariance matrix of the next discrete sampling time. As the discrete sampling time continues to advance, the continuously obtained global optimal state estimates are seamlessly spliced ​​together to obtain the optimal estimate of the real-time three-dimensional trajectory of the end operation point of the robot under test.

Images