Two-arm collaborative end tracking method fusing global view angle information

Abstract

The invention discloses a two-arm collaborative tail end tracking method fusing global view angle information, which comprises the following steps of: uniformly modeling state information of two mechanical arms into a joint state vector, and introducing an acceleration attenuation item and a coupling item to describe dynamic relevance between tail ends of the two arms; and a centralized Kalman filtering algorithm is used, a global observation matrix is established, position measurement of different visual angles is fused, and finally mechanical arm tail end tracking is completed according to fused information. Compared with the prior art, the limitation of a traditional independent filtering framework is broken through, the motion correlation between the tail ends of the two arms in the cooperative working state can be deeply excavated and utilized to a certain extent by influencing error covariance, the tail end tracking stability is improved, the tail end tracking error is reduced under a global unified coordinate system, and the tracking efficiency is improved. And the two-arm collaborative end tracking algorithm can be applied to a task scene with a higher dynamic performance requirement.

Description

Technical Field

[0001] This invention relates to the field of robotic arm motion planning methods, specifically to a dual-arm collaborative end-effector tracking method that integrates global perspective information. Background Technology

[0002] In the evolution of industrial automation towards intelligence, multi-robotic arm collaborative operation has become a core technology in complex assembly and flexible manufacturing scenarios. However, multi-robotic arm collaborative operation relies on visual data, and dynamic occlusion, visual blurring caused by high-speed motion, and visual sensor noise severely restrict the continuous tracking accuracy of the target at the end of the robotic arm. This essentially stems from the inherent limitations of a single-view vision system; the narrowness of the observation field causes the target visibility to dynamically change with the robotic arm's pose. Kalman filtering, as the theoretical cornerstone of dynamic system state estimation, continues to play a crucial role in the field of robot perception. This algorithm achieves optimal fusion of sensor observations and dynamic models through a time-domain recursive framework, demonstrating excellent noise suppression capabilities and computational efficiency in tasks such as target tracking, pose estimation, and multi-sensor fusion. In robotic arm visual servo control scenarios, traditional methods typically rely on a single visual sensor for target localization. However, when facing challenges such as multi-machine collaboration, high-speed motion, and partial occlusion in complex industrial environments, the limitations of a single sensor will significantly reduce the reliability and continuity of state estimation. Specifically, a limited field of view can easily lead to target loss, and motion blur reduces the accuracy of image feature extraction. The combined effect of these problems severely restricts the operational performance of robotic arms in highly dynamic scenarios. To overcome the performance bottleneck of single sensors, multi-source heterogeneous information fusion technology has gradually become a research focus in the field of robot perception. Existing solutions are mainly evolving in two directions: one is to track the consistency of state estimation of the system through redundant configuration of multiple sensors, such as fusing visual, inertial navigation, and joint encoder data; the other is to construct cooperative motion models to capture the state coupling characteristics between robotic arms. However, how to effectively coordinate the spatiotemporal asynchronicity of multi-source data and how to establish accurate cooperative motion representations remain urgent technical challenges to be solved. Summary of the Invention

[0003] Purpose of the invention: In order to solve the problems of joint modeling and observation fusion of multiple view sensors in the process of dual-arm collaborative operation, we propose a dual-arm collaborative end-effector tracking method that fuses global view information.

[0004] In this invention, a global-view camera is set up in a dual-arm collaborative environment. The data from the global-view camera is fused with the target position information obtained from the local-view camera of each arm. By introducing acceleration attenuation and coupling terms, the dynamic correlation between the end effectors of the two arms is characterized. A joint motion state model of the two arm end effectors based on a uniform acceleration motion model is constructed. A centralized Kalman filter algorithm is used to achieve the fusion observation of multi-view information, thereby realizing position tracking of the dual-arm end effectors. The improved centralized Kalman filter algorithm specifically includes the following steps:

[0005] Step 1, Construct initial state estimate Initial covariance Process noise covariance Observation noise covariance Time step .

[0006] Step 2, in the time step According to the recursive algorithm of Kalman filtering Perform state prediction to obtain Time Prior Estimation ,in, for The system state vector at any given time; for The state transition matrix at each time step represents the dynamic characteristics of the system; This is process noise.

[0007] Step 3, according to the state transition equation

[0008] ,

[0009] Calculate the Jacobian matrix The block matrix is ​​as follows:

[0010] ,

[0011]

[0012] Step 4: Linearize the propagation equation based on the Jacobian matrix. predict Prior covariance matrix at time step ,in, for The posterior covariance at time.

[0013] Step 5, based on the block diagonal structure of the observation matrix

[0014] ,

[0015] Obtain the observation matrix ,in, This represents the selective observation of the position components, from which the observation equation can be derived. ,in for Observation vector at time, To observe the noise covariance matrix.

[0016] Step 6: Calculate the Kalman gain equation. Update Kalman gain .

[0017] Step 7: Correct the equations based on the state estimation. Perform a state update and obtain Post-hoc estimation ,in, This is the observation matrix.

[0018] Step 8: Update the equation based on covariance. ,renew Posterior covariance matrix at time step .

[0019] Step 9: Repeat steps 2-8 to finally obtain the motion state estimation sequence of the dual robotic arms' end effectors. .

[0020] An electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the aforementioned dual-arm cooperative end-effector tracking method that integrates global perspective information.

[0021] A computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the aforementioned dual-arm cooperative end-effector tracking method that integrates global perspective information.

[0022] Compared with the prior art, the advantages of the present invention are as follows:

[0023] To address the challenges of joint modeling of end-effector motion and observation fusion from multiple viewpoint sensors, a centralized end-effector tracking method is proposed, improving the position tracking performance of two motion-correlated end-effectors. This method, based on the Kalman filter algorithm, models the state information of the two manipulators into a unified joint state vector, introducing acceleration decay and coupling terms to characterize the dynamic correlation between the end-effectors. This modeling approach allows the system to estimate the end-effector motion state by leveraging the cooperative relationship between the two manipulators, thereby improving the overall system tracking performance. Compared to traditional independent modeling methods, this method can, to some extent, exploit and utilize the motion correlation between the two end-effectors in a cooperative working state by influencing the error covariance, thus improving the stability of end-effector tracking.

[0024] In the simulation experiment, the optimal parameter combination is selected. , It has achieved such Figure 5 The tracking performance of the complete model shown is analyzed based on the distribution of the centroid position tracking error of target 1. It is evident that the dual-view visual positioning information fusion method achieves a smaller expected error value compared to a single visual sensor and reduces the maximum measurement error. As shown in Table 1, comparing the parameter degradation models, the complete model exhibits comprehensive advantages, with its tracking performance for target 1 and target 2 being significantly better. They reached 15.1 respectively. and 14.4 Compared to traditional Kalman filtering, this represents an improvement of 6.2% and 9.8%. Experiments show that introducing an acceleration attenuation mechanism and a cooperative coupling model can reduce the target tracking mean square error by 13.8%-17.7%, compressing the maximum error to within 50mm. Parameter scanning experiments reveal the attenuation coefficient... With coupling coefficient The nonlinear compensation characteristics, and its optimal combination in , The area presents a strip-shaped section, providing a theoretical basis for engineering deployment. Attached Figure Description

[0025] Figure 1 This is a schematic diagram of the system architecture of the method of the present invention;

[0026] Figure 2 This is a schematic diagram of the environment of the method of the present invention;

[0027] Figure 3 This is a comparison of the tracking error distribution diagrams of the single-view Kalman filter and the dual-view fusion model simulated by the method of this invention;

[0028] Figure 4 This is a graph showing the parameter scanning experimental results simulated by the method of this invention;

[0029] Figure 5 This is the tracking result of the simulation model of the method of this invention. Detailed Implementation

[0030] The following description, with reference to the specification, illustrates a preferred embodiment of the present invention to make its technical content clearer and easier to understand. The present invention can be embodied in many different embodiments, and the scope of protection of the present invention is not limited to the embodiments mentioned herein.

[0031] Example: This embodiment of the invention provides a dual-arm collaborative end-effector tracking method that integrates global perspective information. This embodiment applies the method to a dual-arm collaborative scenario. First, it constructs an end-effector target tracking model that incorporates acceleration decay and acceleration coupling during collaboration. Then, it integrates data from a global monitoring camera and local perspective cameras mounted on the robotic arms as target position observations. Finally, it uses a centralized Kalman filter algorithm to perform end-effector target tracking for dual-arm collaborative tasks. The specific steps are as follows:

[0032] Step 1: As Figure 2 The dual-robotic arm environment is set up as shown, and the global view camera data is fused with the target position information obtained by the camera equipped on each robotic arm. The specific implementation process of step 1 is as follows:

[0033] Step 1-1: Install a global view camera at the top of the robotic arm's operating space.

[0034] Steps 1-2: The camera 1 equipped on the robotic arm 1 can only see the auxiliary positioning device at the end of the robotic arm 2, thus obtaining the position of the target 2 in the coordinate system of camera 1. With time Corresponding data .

[0035] Steps 1-3: Using the external parameters of the camera obtained from the joint sensors of the robotic arm, calculate the coordinates of the end target 2 in the world coordinate system.

[0036] Steps 1-4: The global view camera can simultaneously observe the positions of two targets. The corresponding data between the obtained end target position and the timestamp of the shooting completion time is as follows: .

[0037] Steps 1-5: The global camera is fixedly mounted above the robotic arm's operating space. Its external parameters are known, so the position obtained from the global camera is transformed to the world coordinate system to obtain... .

[0038] Step 2: Considering the cooperative motion of the end effector targets of the two robotic arms, establish a continuous-time model that includes acceleration decay and interactive coupling. For the continuous model in... Perform analytical integration over the interval, let To maintain a fixed sampling period, the continuous-time decay dynamics model and the Euclidean model of the robotic arm's end effector acceleration are precisely embedded into a discrete filtering architecture to establish a discretized dynamics model. The specific implementation process of step 2 is as follows:

[0039] Step 2-1: Considering the cooperative motion of the end effector target of the two robotic arms, establish a continuous-time model that includes acceleration decay and interactive coupling, and define the robotic arms. The continuous-time state vector is

[0040]

[0041] in For location, For speed, Let the acceleration be . The system dynamics are described by the following differential equation:

[0042]

[0043] in, The acceleration attenuation coefficient reflects the damping characteristics of the robotic arm drive system. The cooperative coupling strength characterizes the dynamic influence between robotic arms; For continuous process noise, Noise intensity.

[0044] Step 2-2: For continuous models in Perform analytical integration over the interval, let To fix the sampling period, firstly, the acceleration components are discretized, and the integral factor method is applied to equation (3).

[0045] (4)

[0046] Assumption exist The inner approximation is a constant. Then it is discretized into

[0047] (5)

[0048] in Discrete noise covariance is

[0049] (6)

[0050] Next, the velocity components are discretized and obtained through integral (2).

[0051] (7)

[0052] Substituting equation (5) into the equation and performing integration, we obtain...

[0053] (8)

[0054] in, covariance For velocity noise covariance

[0055] (9)

[0056] At this point, the position components can be discretized, and the integral formula (1) can be used.

[0057] (10)

[0058] Substituting equation (8) into the equation and performing a double integral, we get...

[0059] (11)

[0060] in, covariance Location noise covariance

[0061] (12)

[0062] This model preserves the decay characteristics of the continuous system through rigorous integration, and its coefficient terms... and The physical meaning can be verified through Taylor expansion: when When, its coefficient term and The model degenerates into a classical uniform acceleration model.

[0063] Step 3: Based on the discrete model in Step 2, and according to the physical constraints of the target motion at the robotic arm's end effector, establish an improved Kalman filter framework to construct the initial state estimate. Initial covariance Process noise covariance Observation noise covariance Time step Among them, the process noise covariance matrix Covariance matrix of observation noise The specific form is as follows:

[0064]

[0065]

[0066] in, For robotic arms Location noise covariance, For robotic arms velocity noise covariance For robotic arms acceleration noise covariance, Robotic arm for global camera observation The noise covariance parameter, For robotic arms Independent perspective camera observation of robotic arm The noise covariance parameter. For each time step The specific implementation process of step 3 is as follows:

[0067] Step 3-1: At the time step According to the recursive algorithm of Kalman filtering Perform state prediction to obtain Time Prior Estimation ,in, for The system state vector at any given time; for The state transition matrix at each time step represents the dynamic characteristics of the system; This is process noise.

[0068] Step 3-2: According to the state transition equation

[0069] ,

[0070] Calculate the Jacobian matrix The block matrix is

[0071] ,

[0072]

[0073] Step 3-3: Linearize the propagation equation based on the Jacobian matrix. predict Prior covariance matrix at time step ,in, for The posterior covariance at time.

[0074] Steps 3-4: Based on the block diagonal structure of the observation matrix

[0075] ,

[0076] Obtain the observation matrix ,in, This represents the selective observation of the position components, from which the observation equation can be derived. ,in for Observation vector at time, To observe the noise covariance matrix.

[0077] Steps 3-5: Calculate the Kalman gain equation Update Kalman gain .

[0078] Steps 3-6: Correct the equations based on the state estimation Perform a state update and obtain Post-hoc estimation ,in, This is the observation matrix.

[0079] Steps 3-7: Update the equation based on covariance ,renew Posterior covariance matrix at time step .

[0080] Step 4: Obtain the motion state estimation sequence of the dual robotic arms' end effectors based on the results of centralized Kalman filtering. A dual-arm collaborative end-effector tracking method that integrates global perspective information is implemented.

[0081] To further illustrate the technical effects of the dual-arm collaborative end-effector tracking method that integrates global perspective information proposed in this invention in dual-arm collaborative operation scenarios, a dual-arm collaborative simulation environment was constructed based on the aforementioned implementation method, and the method of this invention was simulated and verified.

[0082] The simulation is based on the Webots simulation platform, which constructs a dual-robotic arm cooperative tracking experimental system. The core configuration is as follows: Figure 2 As shown, the system comprises two UR5e collaborative robotic arms, each with a high-reflectivity cubic marker mounted on its end effector as a visual detection target. Each arm's wrist is equipped with an Orbbec Astra depth camera, and the coordinate transformation relationship between the camera and the robotic arm base is established through eye-in-hand calibration. A Kinect v1 global observation camera is mounted on top of the system, forming a multi-view tracking system. The simulated global coordinates of the target are obtained through the Webots Supervisor module. To establish an algorithm evaluation benchmark, the positioning performance is quantified using the following metrics.

[0083]

[0084] in for State estimate at time step The total number of valid observation samples. Characterizes the positioning error under the worst operating conditions.

[0085] The parameter scanning experiment results show that the attenuation coefficient With coupling coefficient There may be a dynamic compensation mechanism, such as Figure 4 As shown. When The optimal value is found when the value increases in the range of 0.3–2.8. The value exhibits a non-monotonic trend of first decreasing and then increasing, such as Figure 4 As shown in a. This phase lag phenomenon may stem from the following factors, Range, higher The value may compensate for the response delay caused by rapid decay by enhancing the interaction between robotic arms; when When the value is increased to 1.4–2.2, the optimal value is achieved. As the value decreases to 0.15–0.45, the system enters a stable region dominated by acceleration decay; High attenuation region A rise to 0.25–0.4 may be used to balance the cooperative error caused by excessive decay. Statistical data shows that... and The optimal combination exhibits nonlinear characteristics, indicating a significant but nonlinear negative correlation between the two. , Minimum synthesis obtained at point This is approximately 13.8% lower than the single decay model. ), a 17.7% reduction compared to the single-coupled model ( ).

[0086] In the optimal parameter combination , Under these conditions, the tracking performance of the complete model is as follows: Figure 5 As shown in the distribution diagram of the centroid position tracking error of target 1, the method of dual-view visual positioning information fusion can obtain a smaller expected error value than a single visual sensor and can reduce the maximum measurement error. As shown in Table 1, the complete model exhibits comprehensive advantages, with its target 1 and target 2... Respectively reach and This represents an improvement of 6.2% and 9.8% compared to traditional Kalman filtering. The slight fluctuation in the maximum error may originate from a special motion phase; when the coupling strength between the robotic arms falls below a critical value, the coupling correction mechanism may temporarily fail.

[0087] Table 1 Comparison of localization performance of multiple algorithms

[0088]

[0089] By simulating typical cooperative trajectories in a simulation environment, a mapping relationship between dual-view visual observations and the target's true position was established. The system verified the effectiveness of the algorithm in suppressing coupling noise and improving state estimation accuracy. Experiments show that introducing an acceleration decay mechanism and a cooperative coupling model can reduce the root mean square error of target tracking by 13.8%-17.7%, and compress the maximum error to [missing value]. Within. Parameter scanning experiments revealed the attenuation coefficient. With coupling coefficient The nonlinear compensation characteristics, and its optimal combination in , The area presents a strip-shaped section, providing a theoretical basis for engineering deployment.

[0090] In a collaborative dual-arm environment, this method delves deeper into and utilizes the motion correlation between the end effects of the two arms in a collaborative working state, thereby improving the stability of end-effector tracking and reducing the end-effector tracking error in a globally unified coordinate system. This enables the dual-arm collaborative end-effector tracking algorithm to be applied to task scenarios with stronger dynamic performance requirements.

[0091] It should be noted that the above embodiments are not intended to limit the scope of protection of the present invention. Equivalent transformations or substitutions made based on the above technical solutions all fall within the scope of protection of the claims of the present invention.

Claims

1. A dual-arm collaborative end-effector tracking method that integrates global perspective information, characterized in that, A global view camera is set up in a dual-arm collaborative environment. The data from the global view camera is fused with the target position information obtained from the local view camera of each arm. By introducing acceleration decay term and coupling term, the dynamic correlation between the ends of the two arms is characterized. A joint model of the motion state of the ends of the two arms based on the uniform acceleration motion model is constructed. The fusion observation of multi-view information is realized by using a centralized Kalman filter algorithm, so as to realize the position tracking of the ends of the two arms.

2. The dual-arm collaborative end-effector tracking method according to claim 1, characterized in that, A collaborative scenario involving two robotic arms is constructed, with each arm equipped with a camera to acquire visual information from its own perspective. The cameras on the robotic arms are mounted in a "eye-to-hand" manner, meaning they can only see the auxiliary positioning device at the end effector of the other robotic arm. Simultaneously, a global-view camera is positioned at the top of the robotic arm's operating space to assist in target tracking. This way, each robotic arm's end effector has two cameras as observation sources for target tracking, and an acceleration attenuation coefficient is specifically introduced. With cooperative coupling strength Reflecting the coordinated motion relationship of the end effector targets of the two robotic arms: The acceleration decay function reflects the damping characteristics of the robotic arm drive system. The cooperative coupling strength characterizes the dynamic influence between robotic arms. Define robotic arm The continuous-time state vector is in For location, For speed, If the acceleration is constant, then the dynamics of the system can be described by the following differential equation: Analytical integration is performed on the continuous model to further establish a discretized dynamic model of the coupling motion state of the robotic arm end effector in the dual-arm collaborative scenario: By constructing a state vector containing the pose, velocity, and acceleration of the two robotic arms' end effectors, the motion correlation between the robotic arms is explicitly represented, thus achieving mathematical modeling of their cooperative characteristics. in, ; For robotic arms At time step The terminal acceleration, For robotic arms At time step The terminal acceleration, For robotic arms At time step End-effector velocity, For robotic arms At time step End position; For a fixed sampling period; ; For robotic arms Discrete acceleration noise; For robotic arms velocity discrete noise; For robotic arms The location discrete noise; where, For acceleration noise covariance; For velocity noise covariance; For location noise covariance; It is the identity matrix; Noise intensity.

3. The dual-arm collaborative end-effector tracking method according to claim 2, characterized in that, Based on the physical constraints of the target motion at the end of the robotic arm, a continuous-time decay dynamics model and a Euclidean model of the robotic arm's end-effector acceleration are precisely embedded into a discrete filtering architecture, forming an improved centralized Kalman filter framework. The improved centralized Kalman filter algorithm specifically includes the following steps: Step 1, Construct initial state estimate Initial covariance Process noise covariance Observation noise covariance Time step , Step 2, in the time step According to the recursive algorithm of Kalman filtering Perform state prediction to obtain Time Prior Estimation ,in, for The system state vector at any given time; for The state transition matrix at each time step represents the dynamic characteristics of the system; For process noise, Step 3, according to the state transition equation ， Calculate the Jacobian matrix The block matrix is ​​as follows: ， Step 4: Linearize the propagation equation based on the Jacobian matrix. predict Prior covariance matrix at time step ,in, for The posterior covariance at time 1, Step 5, based on the block diagonal structure of the observation matrix ， Obtain the observation matrix ,in, This represents the selective observation of the position components, from which the observation equation can be derived. ,in for Observation vector at time, To observe the noise covariance matrix, Step 6: Calculate the Kalman gain equation. Update Kalman gain , Step 7: Correct the equations based on the state estimation. Perform a state update and obtain Post-hoc estimation ,in, For the observation matrix, Step 8: Update the equation based on covariance. ,renew Posterior covariance matrix at time step , Step 9: Repeat steps 2-8 to finally obtain the motion state estimation sequence of the dual robotic arms' end effectors. .

4. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements a dual-arm cooperative end-effector tracking method that integrates global perspective information as described in any one of claims 1 to 3 above.

5. A computer-readable storage medium storing computer instructions thereon, characterized in that, When the computer instruction is executed by the processor, it implements a dual-arm cooperative end-effector tracking method that integrates global perspective information as described in any one of claims 1-3.

Images