Double-robot automatic calibration method based on adaptive point planning in confined space
By constructing a 3D simulation model and adaptive point planning on the Rviz platform, and combining Moveit collision detection and Tsai-Lenz hand-eye calibration algorithm, the problem of low efficiency in traditional dual-robot calibration methods is solved, achieving efficient and accurate dual-robot relative pose calibration, adapting to different working environments.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- NANKAI UNIV
- Filing Date
- 2025-08-25
- Publication Date
- 2026-08-04
AI Technical Summary
Traditional methods for relative pose calibration of dual robots are inefficient, susceptible to human error, and have limited applicability, making it difficult to meet the demands of modern industrial production for high-precision and high-efficiency collaborative operations.
An adaptive point planning method is adopted to build a 3D simulation model on the Rviz platform. The Moveit collision detection module is used to generate collision-free joint angle vectors. Combined with the RTDE interface and the Tsai-Lenz hand-eye calibration algorithm, calibration is performed by motion capture camera measurement and robot forward kinematics Cartesian coordinates to achieve multi-stage error screening and optimization.
It improves calibration efficiency and accuracy, enables dynamic adjustment of calibration points in confined spaces, reduces on-site debugging costs, ensures the reliability and accuracy of calibration results, and adapts to different operating environments.
Smart Images
Figure CN120941389B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of dual-robot calibration technology, and in particular relates to an automatic calibration method for dual robots based on adaptive point planning in a confined space. Background Technology
[0002] In collaborative operations between two robots in industrial manufacturing, aerospace, and other fields, the accuracy of their relative pose is crucial for ensuring operational precision. Whether it's the docking and assembly of precision parts or the collaborative handling of large components, deviations in relative pose can lead to workpiece damage, assembly failure, or even safety accidents. However, during manufacturing, installation, and long-term operation, factors such as machining errors, assembly deviations, and joint wear can cause deviations in the relative pose of the two robots, severely impacting the effectiveness of collaborative operations. Therefore, efficient and accurate calibration of the position and attitude relationship between the two robots is of paramount importance.
[0003] Traditional methods for calibrating the relative pose of two robots have many limitations. For example, calibration methods based on manual measurement are not only inefficient but also susceptible to human error, leading to unstable measurement accuracy. Furthermore, these methods require significant manpower and time, making them unsuitable for the fast and efficient demands of modern industrial production. While some methods based on fixed calibration boards can improve calibration accuracy to some extent, their applicability is limited, the calibration process is complex, and they are highly dependent on environmental conditions. If the calibration environment changes, or if the robot's range of motion exceeds the effective range of the calibration board, the accuracy and reliability of the calibration results will be significantly compromised.
[0004] With the rapid development of industrial automation and intelligence, the limitations of traditional dual-robot relative pose calibration methods are becoming increasingly apparent. In order to meet the demands of modern industrial production for high-precision and high-efficiency collaborative operations, there is an urgent need to develop a more intelligent, efficient, and adaptable automatic calibration technology. Summary of the Invention
[0005] The problem this invention aims to solve is to provide an automatic calibration method for dual robots based on adaptive point planning in a confined space.
[0006] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is: an automatic calibration method for dual robots based on adaptive point planning in a confined space, comprising the following steps:
[0007] S1. For dual-robot scanning scenarios, construct a 3D simulation model of the dual-robot workspace on the Rviz platform;
[0008] S2, within the set joint angle range [θ] min ,θ maxRandom sampling within the system generates joint angle vectors for the two robots. The Moveit collision detection module is used to determine whether a collision will occur. If a collision occurs, the vectors are discarded; otherwise, they are retained, until 30 sets of collision-free pre-calibration points are generated.
[0009] Where, θ max θ is the maximum angle of the joint. min This represents the minimum angle of the joint.
[0010] S3. Use the RTDE interface to drive the movement of the two robots through communication. Synchronously collect the rigid body center of mass pose data measured by the motion capture camera and the robot's forward kinematic Cartesian coordinates. Each time, add five sets of calibration point data to the current n valid calibration points and input them into the Tsai-Lenz hand-eye calibration algorithm. Optimize the calibration results using the least squares method, and transform the rigid body center of mass pose data measured by the motion capture camera through the calibration results to obtain the end-effector position P of the two robots. l =(x l ,y l ,z l ),P r =(x r ,y r ,z r ), where P l P represents the end-effector position of the robot equipped with the launch tube, obtained from the motion capture camera data transformation. r This represents the end position of the robot equipped with a receiving tablet, obtained from the data transformation of the motion capture camera data; that is, the right robot's end position.
[0011] Set the end position P of the left robot l =(x l ,y l ,z l ) and the end position P of the right robot r =(x r ,y r ,z r The end-effector position P′ obtained from the forward kinematics of the left robot l =(x′) l ,y′ l ,z′ l The end-effector position P′ obtained from the forward kinematics of the right robot. r =(x′) r ,y′ r ,z′ r (Compare)
[0012] Then, the Euclidean distance between the robot's end-effector position obtained from the motion capture camera data transformation and the robot's end-effector position obtained from the robot's forward kinematics is calculated with calibration accuracy, i.e., e. l =|P l -P′l |,e r =|P r -P′ r |, where e l e represents the calibration accuracy of the left robot. r This represents the calibration accuracy of the right robot. When the average calibration accuracy of all current calibration points of both robots reaches the standard, the addition of five sets of calibration points is considered valid. This process continues until the number of valid calibration points reaches 20, at which point the coordinate system is precisely aligned, and the calibration of the two robots is complete.
[0013] Further, in step S1, under the ROS architecture, a 3D simulation model for a dual-robot scanning scenario is constructed on the Rviz platform based on the URDF file, fully reproducing the scanning work space. The 3D simulation model of the scanning scenario is for a dual-robot scanning system. The 3D simulation model of the scanning scenario includes dual robots, dual-robot bases, a transmitting tube, a receiving plate, an operating table, a scanning phantom, and a collision avoidance wall. The dual robots include a left robot and a right robot, and the dual-robot base includes a left robot base and a right robot base.
[0014] The left robot is mounted on the left robot base, the right robot is mounted on the right robot base, the transmitting tube is mounted at the end of the left robot, the receiving plate is mounted at the end of the right robot, the operating table is placed between the left and right robots, and the scanning phantom is placed at the front end of the operating table.
[0015] By roughly measuring the distance between the bases of the two robots in a real working environment, as well as the distance between the operating table and each of the two robots, the corresponding model can be placed in a simulation environment.
[0016] Furthermore, the precise deployment of the anti-collision walls within the dual-robot workspace is crucial for maximizing their protective effectiveness. Determining their location requires comprehensive consideration of multiple factors, including the dual robots' operational coverage area, the distribution patterns of potential obstacles, and the effective capture range of the motion capture system. By placing anti-collision walls around the front, rear, left, right, and below the dual robots, the movement range during calibration is limited to the optimal measurement area of the motion capture camera and avoids some potential obstacles. Their positions can be adjusted according to different workspaces to adapt to different environments.
[0017] Further, step S2 includes the following steps:
[0018] S21. Limit the joint values of the two robots to [θ]. min ,θ max Within the specified range, a set of dual robot joint vectors is randomly generated;
[0019] S22. Call the collision detection function of the Moveit detection module, input a set of planned dual-robot joint vectors and a set of randomly generated dual-robot joint vectors, and determine whether the dual robots will collide under the set of randomly generated dual-robot joint vectors. If a collision occurs, discard the set of joint vectors. If no collision occurs, retain the set of joint vectors until 30 sets of collision-free dual-robot joint vectors are generated, i.e., N=30 pre-calibration point data.
[0020] S23. The 30 sets of dual-robot calibration points are visualized in the Rviz platform. This not only clearly displays the spatial distribution of each calibration point but also dynamically recreates the motion trajectory of the dual robots throughout the calibration process. Through this visual preview, operators can intuitively observe whether the dual robots' trajectories intersect or collide with the surrounding environment model during movement, thus identifying potential motion interference risks in advance. This process verifies the rationality of the calibration point planning and the safety of the motion path without deploying the actual robots, providing accurate preview references for subsequent calibration operations with the actual robots. This effectively reduces on-site debugging costs and collision risks, improving the reliability and efficiency of the calibration process.
[0021] Furthermore, in step S3, the calculation formula for the calibration result obtained through least squares optimization is as follows:
[0022] For the left robot, when it is at position P 1l At that time, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its end point. It can be represented as:
[0023]
[0024] in, Representing the left robot in P 1l The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the left robot in P 1l The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the left robot in P 1l The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system.
[0025] When the left robot is at position P 2l Similarly, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its endpoints can be obtained.
[0026]
[0027] in, Representing the left robot in P 2lThe transformation matrix from the position time base coordinate system to the end coordinate system. Representing the left robot in P 2l The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the left robot in P 2l The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system.
[0028] Since the relative pose between the robot's end effector coordinate system and the calibration board coordinate system remains constant during the robot's movement, the following formula holds true:
[0029]
[0030] remember Since the relative pose relationship between the world coordinate system and the robot's base coordinate system remains constant at any position, let's call it... Therefore, it can be simplified to the form AX = XB, where X is the hand-eye pose matrix to be solved, that is, the transformation matrix between the world coordinate system and the left robot base coordinate system, and then the Tsai-Lenz method can be used to solve it.
[0031] Similarly, for the right robot, when it is at position P 1r At that time, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its end point. It can be represented as:
[0032]
[0033] in, Representing the right robot in P 1r The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the right robot in P 1r The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the right robot in P 1r The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system.
[0034] When the right robot is at position P 2r Similarly, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its endpoints can be obtained.
[0035]
[0036] in, Representing the right robot in P 2r The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the right robot in P 2rThe position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the right robot in P 2r The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system.
[0037] Since the relative pose between the robot's end effector coordinate system and the calibration board coordinate system remains constant during the robot's movement, the following formula holds true:
[0038]
[0039] remember Since the relative pose relationship between the world coordinate system and the robot's base coordinate system remains constant at any position, let's call it... Therefore, it can be simplified to the form CY=YD, where Y is the hand-eye pose matrix to be solved, that is, the transformation matrix between the world coordinate system and the right robot base coordinate system, and then the Tsai-Lenz method can be used to solve it.
[0040] Step S3 includes the following steps:
[0041] S31. Send N=1 to 5 sets of calibration points to the dual robots for movement. When the dual robots reach the designated points, synchronously record the pose data of the rigid body center of mass measured by the motion capture camera and the forward kinematic Cartesian coordinates of the robot.
[0042] S32. Input the data from step S1 into the Tsai-Lenz hand-eye calibration algorithm to obtain calibration results for N = 1 to 5 sets of calibration points, including:
[0043] Transformation matrix from world coordinate system to left robot base coordinate system From world coordinate system to right robot base coordinate system From the left robot's rigid body center of mass coordinate system to the end effector coordinate system From the robot's rigid body coordinate system to the robot's end effector coordinate system
[0044] S33. The pose data of the rigid body center of mass of the left and right robots measured by the motion capture cameras at the N=1 to N5 calibration points are sequentially transformed through the calibration results to obtain the end-effector positions of the left and right robots. The calculation formula is as follows:
[0045]
[0046] in, This represents the transformation matrix from the rigid body center of mass coordinate system to the end effector coordinate system of the left and right robots at the Nth calibration point. This represents the transformation matrix from the world coordinate system to the rigid body's center of mass coordinate system for the left and right robots at the Nth calibration point. This represents the transformation matrix from the base coordinate system to the world coordinate system for the left and right robots at the Nth calibration point;
[0047] The end position obtained by combining it with the positive kinematics of the two robots at the Nth position The calibration accuracy of the left and right robots at the Nth position is then calculated by comparing the two, using the following formula:
[0048] e Nl =|P Nl -P′ Nl |
[0049] e Nr =|P Nr -P′ Nr |
[0050] Then, the average value of the calibration accuracy of the calibration points of the left and right robots (N=1 to 5 groups) is calculated.
[0051] S34. Based on the average value of the calibration point data, filter the calibration point data of N=1 to 5 groups: if the average value exceeds the preset standard, discard the data of that group and send only the data of N=6 to 10 groups to the robot, and control it to repeat steps S31 to S33; if the average value meets the preset standard, retain the calibration points of N=1 to 5 groups, send the data of N=6 to 10 groups to the robot, repeat step S31, and then integrate the rigid body center of mass pose data measured by the motion capture camera in the calibration points of N=1 to 10 groups, and substitute it together with the Cartesian coordinate data calculated by the robot's forward kinematics into steps S32 to S33 to calculate the new calibration result and calibration error, and make a judgment again.
[0052] The above process is repeated until the number of valid calibration points reaches 20 sets. The calibration result obtained from these 20 sets of data that meets the error standard is the final result. If all 30 sets of data have been sent to the dual robots, but the average error still does not meet the standard, the calibration is considered a failure, and the calibration process can be restarted after selecting new calibration points.
[0053] The more calibration points there are, the smaller the calibration error usually is. Therefore, the standard for determining whether to retain the average error of calibration points needs to be set differently at different stages of the calibration process. In the initial stage (e.g., N = 1 to 5 sets of data), the standard can be appropriately lenient, and the accuracy requirement should be gradually increased as the number of valid calibration points increases. The specific settings are as follows:
[0054] If n is the number of valid calibration points, then the piecewise function form of the standard error mean is:
[0055]
[0056] S35. Use 20 sets of calibration points to obtain the transformation matrix from the world coordinate system to the left robot base coordinate system. From world coordinate system to right robot base coordinate system From the left robot's rigid body center of mass coordinate system to the end effector coordinate system From the robot's rigid body coordinate system to the robot's end effector coordinate system The transformation matrix from the right robot base coordinate system to the left robot base coordinate system can be calculated. The calculation formula is as follows:
[0057]
[0058] The specific effects of this invention are as follows:
[0059] This invention constructs a three-dimensional simulation model of the dual-robot workspace using URDF files, then uses the Moveit collision detection module to generate 30 sets of calibration points composed of collision-free dual-robot joint angle vectors; the robot motion is driven through RTDE (Real-Time Data Exchange) interface communication, and motion capture camera data and robot forward kinematics Cartesian coordinates are collected synchronously, and the calibration results are obtained through least squares optimization.
[0060] As can be seen, this invention utilizes the Rviz platform to construct a 3D model of the dual-robot working environment, providing a foundation for calibration that closely matches the actual scenario. Simultaneously, it allows for visual modeling and pre-simulation of dual-robot calibration points and motion trajectories on this platform. Compared to traditional calibration relying on external precision equipment, visual pre-simulation in the Rviz environment is more intuitive and can proactively avoid collision risks. Since the selection of dual-robot calibration points needs to balance accuracy and accessibility, direct planning is quite difficult. By designing an adaptive point planning algorithm, the complex calibration point selection problem can be transformed into a dynamic optimization problem based on error feedback, simplifying the calibration process, improving calibration efficiency, and still satisfying the kinematic constraints and environmental collision constraints of the dual robots during the optimization process. The multi-stage error screening mechanism will progressively tighten the criteria for selecting valid calibration points while ensuring calibration accuracy, ensuring that the error threshold can be dynamically adjusted based on the current number of valid points at any stage of the calibration process, thereby verifying the reliability and accuracy of the calibration results.
[0061] This invention enables dynamic, rapid, and accurate calibration of the relative pose of two robots when factors such as machining errors, assembly deviations, and joint wear affect the pose relationship between them. Attached Figure Description
[0062] The present invention will be described in detail below with reference to the accompanying drawings and examples. The advantages and implementation methods of the present invention will become more apparent from this description. The accompanying drawings are for illustrative purposes only and do not constitute any limitation on the present invention. In the accompanying drawings:
[0063] Figure 1 This is a flowchart illustrating the present invention.
[0064] Figure 2 This is a flowchart of step S3 of the present invention.
[0065] Figure 3 This is a schematic diagram of the three-dimensional simulation environment of the dual-robot scanning operation space of the present invention.
[0066] Figure 4 This is the front view of the three-dimensional simulation model of the present invention.
[0067] Figure 5 This is a side view of the three-dimensional simulation model of the present invention.
[0068] Figure 6 This is a top view of the three-dimensional simulation model of the present invention.
[0069] Figure 7 This is a schematic diagram of the dual-robot calibration points in the simulation visualization part of this invention.
[0070] 1. Transmitting tube; 2. Receiving plate; 3. Operating table; 4. Scanning phantom; 5. Anti-collision wall; 6. Left robot; 7. Right robot; 8. Left robot base; 9. Right robot base. Detailed Implementation
[0071] like Figures 1 to 7 As shown, the present invention provides an automatic calibration method for dual robots based on adaptive point planning in a confined space, comprising the following steps:
[0072] S1. For dual-robot scanning scenarios, construct a 3D simulation model of the dual-robot workspace on the Rviz platform;
[0073] S2, within the set joint angle range [θ] min ,θ max Random sampling within the system generates joint angle vectors for the two robots. The Moveit collision detection module is used to determine whether a collision will occur. If a collision occurs, the vectors are discarded; otherwise, they are retained, until 30 sets of collision-free pre-calibration points are generated.
[0074] Where, θ max θ is the maximum angle of the joint. min This represents the minimum angle of the joint.
[0075] S3. Use the RTDE (Real-Time Data Exchange) interface to drive the movement of the two robots through communication. Synchronously collect the rigid body center of mass pose data measured by the motion capture camera and the robot's forward kinematic Cartesian coordinates. Each time, add five sets of calibration point data to the current n valid calibration points and input them into the Tsai-Lenz hand-eye calibration algorithm. Optimize the calibration results using the least squares method. Transform the rigid body center of mass pose data measured by the motion capture camera through the calibration results to obtain the end-effector position P of the two robots. l =(x l ,y l ,z l ),P r =(x r ,y r ,z r ), where P l P represents the end-effector position of the robot equipped with the launch tube 1 (hereinafter referred to as the left robot 6), obtained by transforming motion capture camera data. r This represents the end position of the robot equipped with receiving tablet 2 (hereinafter referred to as the right robot 7), obtained by transforming data from motion capture cameras.
[0076] Position P at the end of the left robot 6 l =(x l ,y l ,z l The end position P of the right robot 7 and the right robot 7 r =(x r ,y r ,z r The end-effector position P′ obtained from the positive kinematics of the left robot 6 l =(x′) l ,y′ l ,z′ l The end-effector position P′ obtained from the forward kinematics of the right robot 7 and the right robot 7 r =(x′) r ,y′ r ,z′ r (Compare)
[0077] Then, the Euclidean distance between the robot's end-effector position obtained from the motion capture camera data transformation and the robot's end-effector position obtained from the robot's forward kinematics is calculated with calibration accuracy, i.e., e. l =|P l -P′ l |,e r =|P r -P′ r |, where e l Represents the calibration accuracy of the left robot 6, e rThis represents the calibration accuracy of the right robot (7). When the average calibration accuracy of all current calibration points of both robots reaches the standard, the addition of five new calibration points is considered valid. This process continues until the number of valid calibration points reaches 20, at which point the coordinate system is precisely aligned, and the calibration of the two robots is complete.
[0078] In step S1, under the ROS architecture, a 3D simulation model for a dual-robot scanning scenario is constructed on the Rviz platform based on the URDF file, fully reproducing the scanning work space. The 3D simulation model of the scanning scenario is for a dual-robot scanning system. The 3D simulation model of the scanning scenario includes dual robots, dual-robot bases, a transmitter tube 1, a receiver plate 2, an operating table 3, a scanning phantom 4, and a crash barrier 5. The dual robots include a left robot 6 and a right robot 7, and the dual-robot bases include a left robot base 8 and a right robot base 9. The left robot 6 is mounted on the left robot base 8, and the right robot 7 is mounted on the right robot base 9. The transmitter tube 1 is mounted at the end of the left robot 6, the receiver plate 2 is mounted at the end of the right robot 7, the operating table 3 is placed between the left robot 6 and the right robot 7, and the scanning phantom 4 is placed at the front end of the operating table 3.
[0079] By roughly measuring the distance between the bases of the two robots in a real working environment, as well as the distance between the operating table 3 and each of the two robots, the corresponding model can be placed in the simulation environment.
[0080] The precise deployment of the anti-collision wall 5 within the dual-robot workspace is crucial for maximizing its protective effectiveness. Determining its location requires comprehensive consideration of multiple factors, including the dual robots' operational coverage area, the distribution patterns of potential obstacles, and the effective capture range of the motion capture system. By placing anti-collision walls 5 in front, behind, to the left, right, and below the dual robots, the movement range of the dual robots during calibration is limited to the optimal measurement area of the motion capture camera and avoids some potential obstacles. Their positions can be adjusted according to different workspaces to adapt to different environments.
[0081] Step S2 includes the following steps:
[0082] S21. Limit the joint values of the two robots to [θ]. min ,θ max Within the specified range, a set of dual robot joint vectors is randomly generated;
[0083] S22. Call the collision detection function of the Moveit detection module, input a set of planned dual-robot joint vectors and a set of randomly generated dual-robot joint vectors, and determine whether the dual robots will collide under the set of randomly generated dual-robot joint vectors. If a collision occurs, discard the set of joint vectors. If no collision occurs, retain the set of joint vectors until 30 sets of collision-free dual-robot joint vectors are generated, i.e., N=30 pre-calibration point data.
[0084] S23. The 30 sets of dual-robot calibration points are visualized in the Rviz platform. This not only clearly displays the spatial distribution of each calibration point but also dynamically recreates the motion trajectory of the dual robots throughout the calibration process. Through this visual preview, operators can intuitively observe whether the dual robots' trajectories intersect or collide with the surrounding environment model during movement, thus identifying potential motion interference risks in advance. This process verifies the rationality of the calibration point planning and the safety of the motion path without deploying the actual robots, providing accurate preview references for subsequent calibration operations with the actual robots. This effectively reduces on-site debugging costs and collision risks, improving the reliability and efficiency of the calibration process.
[0085] Furthermore, in step S3, the calculation formula for the calibration result obtained through least squares optimization is as follows:
[0086] For the left robot 6, when it is at position P 1l At that time, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its end point. It can be represented as:
[0087]
[0088] in, Representing the left robot 6 in P 1l The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the left robot 6 in P 1l The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the left robot 6 in P 1l The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system.
[0089] When the left robot 6 is at position P 2l Similarly, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its endpoints can be obtained.
[0090]
[0091] in, Representing the left robot 6 in P 2l The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the left robot 6 in P 2l The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the left robot 6 in P 2l The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system.
[0092] Since the relative pose between the robot's end effector coordinate system and the calibration board coordinate system remains constant during the robot's movement, the following formula holds true:
[0093]
[0094] remember Since the relative pose relationship between the world coordinate system and the robot's base coordinate system remains constant at any position, let's call it... Therefore, it can be simplified to the form AX = XB, where X is the hand-eye pose matrix to be solved, that is, the transformation matrix between the world coordinate system and the left robot base coordinate system, and then the Tsai-Lenz method can be used to solve it.
[0095] Similarly, for the right robot 7, when it is at position P 1r At that time, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its end point. It can be represented as:
[0096]
[0097] in, Representing the right robot 7 in P 1r The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the right robot 7 in P 1r The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the right robot 7 in P 1r The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system.
[0098] When the right robot 7 is at position P 2r Similarly, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its endpoints can be obtained.
[0099]
[0100] in, Representing the right robot 7 in P 2r The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the right robot 7 in P 2r The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the right robot 7 in P 2r The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system.
[0101] Since the relative pose between the robot's end effector coordinate system and the calibration board coordinate system remains constant during the robot's movement, the following formula holds true:
[0102]
[0103] remember Since the relative pose relationship between the world coordinate system and the robot's base coordinate system remains constant at any position, let's call it... Therefore, it can be simplified to the form CY=YD, where Y is the hand-eye pose matrix to be solved, that is, the transformation matrix between the world coordinate system and the right robot base coordinate system, and then the Tsai-Lenz method can be used to solve it.
[0104] Step S3 includes the following steps:
[0105] S31. Send N=1 to 5 sets of calibration points to the dual robots for movement. When the dual robots reach the designated points, synchronously record the pose data of the rigid body center of mass measured by the motion capture camera and the forward kinematic Cartesian coordinates of the robot.
[0106] S32. Input the data from step S1 into the Tsai-Lenz hand-eye calibration algorithm to obtain calibration results for N = 1 to 5 sets of calibration points, including:
[0107] Transformation matrix from world coordinate system to left robot base coordinate system From world coordinate system to right robot base coordinate system From the left robot's rigid body center of mass coordinate system to the end effector coordinate system From the robot's rigid body coordinate system to the robot's end effector coordinate system
[0108] S33. The pose data of the rigid body center of mass of the left and right robots measured by the motion capture cameras at the N=1 to N5 calibration points are sequentially transformed through the calibration results to obtain the end-effector positions of the left and right robots. The calculation formula is as follows:
[0109]
[0110] in, This represents the transformation matrix from the rigid body center of mass coordinate system to the end effector coordinate system of the left and right robots at the Nth calibration point. This represents the transformation matrix from the world coordinate system to the rigid body's center of mass coordinate system for the left and right robots at the Nth calibration point. This represents the transformation matrix from the base coordinate system to the world coordinate system for the left and right robots at the Nth calibration point;
[0111] The end position obtained by combining it with the positive kinematics of the two robots at the Nth position The calibration accuracy of the left and right robots at the Nth position is then calculated by comparing the two, using the following formula:
[0112] e Nl =|P Nl -P′ Nl |
[0113] e Nr =|P Nr -P′ Nr |
[0114] Then, the average value of the calibration accuracy of the calibration points of the left and right robots (N=1 to 5 groups) is calculated.
[0115] S34. Based on the average value of the calibration point data, filter the calibration point data of N=1 to 5 groups: if the average value exceeds the preset standard, discard the data of that group and send only the data of N=6 to 10 groups to the robot, and control it to repeat steps S31 to S33; if the average value meets the preset standard, retain the calibration points of N=1 to 5 groups, send the data of N=6 to 10 groups to the robot, repeat step S31, and then integrate the rigid body center of mass pose data measured by the motion capture camera in the calibration points of N=1 to 10 groups, and substitute it together with the Cartesian coordinate data calculated by the robot's forward kinematics into steps S32 to S33 to calculate the new calibration result and calibration error, and make a judgment again.
[0116] The above process is repeated until the number of valid calibration points reaches 20 sets. The calibration result obtained from these 20 sets of data that meets the error standard is the final result. If all 30 sets of data have been sent to the dual robots, but the average error still does not meet the standard, the calibration is considered a failure, and the calibration process can be restarted after selecting new calibration points.
[0117] The more calibration points there are, the smaller the calibration error usually is. Therefore, the standard for determining whether to retain the average error of calibration points needs to be set differently at different stages of the calibration process. In the initial stage (e.g., N = 1 to 5 sets of data), the standard can be appropriately lenient, and the accuracy requirement should be gradually increased as the number of valid calibration points increases. The specific settings are as follows:
[0118] If n is the number of valid calibration points, then the piecewise function form of the standard error mean is:
[0119]
[0120] S35. Use 20 sets of calibration points to obtain the transformation matrix from the world coordinate system to the left robot base coordinate system. From world coordinate system to right robot base coordinate system From the left robot's rigid body center of mass coordinate system to the end effector coordinate system From the robot's rigid body coordinate system to the robot's end effector coordinate system The transformation matrix from the right robot base coordinate system to the left robot base coordinate system can be calculated. The calculation formula is as follows:
[0121]
[0122] Experimental results:
[0123] A comparative analysis of the actual trajectory and the theoretically planned path showed a high degree of agreement, further validating the effectiveness and accuracy of the motion planning and obstacle avoidance scheme. The actual trajectory was also found to be largely consistent with the theoretically predicted path.
[0124] Using 20 sets of data, the hand-eye matrix of the left robot 6 with the launch tube 1 at its end can be obtained, as well as the coordinate system from the rigid body's center of mass to the end-effector coordinate system:
[0125]
[0126] Then, using the same method, the right robot was calibrated, and the hand-eye matrix of the right robot with the receiving plate 2 at the end was obtained, as well as the coordinate system from the rigid body center of mass to the end-effector coordinate system:
[0127]
[0128] according to The relative pose relationship matrix between the two robot base systems can be obtained as follows:
[0129]
[0130] Tables 1 and 2 show the calibration error results for the 20 sets of calibration points calculated based on the above results:
[0131] Table 1. Self-calibration error of the left robot with a launch tube at its end.
[0132] 1 1.3809 3.3068 0.7864 3.6688 2 0.0000 0.0000 0.0000 0.0000 3 2.9140 2.8468 2.1584 4.6102 4 0.1254 2.4734 0.5300 2.5326 5 4.8006 4.0012 4.3674 7.6243 6 7.4582 0.9314 2.5245 7.9287 7 0.6983 3.5312 2.1389 4.1871 8 4.9732 1.2118 2.0525 5.5149 9 3.3527 1.4995 2.3525 4.3616 10 1.7255 4.7622 0.1472 5.0673 11 3.9813 3.2756 2.8437 5.8878 12 1.2414 3.4623 2.3120 4.3444 13 2.0259 2.9342 4.3885 5.6544 14 3.5673 4.1066 1.8483 5.7451 15 0.8608 2.2257 3.4253 4.1746 16 0.7571 2.1119 1.6513 2.7857 17 7.3345 1.7091 2.5989 7.9668 18 4.6004 2.8060 0.8020 5.4480 19 2.3929 2.5570 2.6279 4.3783 20 1.8836 0.1923 2.3476 3.0160 average value 2.8037 2.4972 2.0952 4.7448
[0133] Table 2 Self-calibration error of the right robot with a receiving plate at the end.
[0134]
[0135]
[0136] As shown in Table 3, traditional manual calibration is time-consuming, while after three tests, the calibration time of the dual-robot automatic calibration was less than 10 minutes each time.
[0137] Table 3. Test results of automatic calibration time for dual robots
[0138]
[0139] The embodiments of the present invention have been described in detail above, but the content described is only a preferred embodiment of the present invention and should not be considered as limiting the scope of the present invention. All equivalent changes and improvements made within the scope of the present invention should still fall within the scope of the present invention.
Claims
1. An automatic calibration method for dual robots based on adaptive point planning in confined spaces, characterized by: Includes the following steps: S1. For dual-robot scanning scenarios, construct a 3D simulation model of the dual-robot workspace on the Rviz platform; S2, within the set joint angle range [θ] min ,θ max Random sampling within the system generates joint angle vectors for the two robots. The Moveit collision detection module is used to determine whether a collision will occur. If a collision occurs, the vectors are discarded; otherwise, they are retained, until 30 sets of collision-free pre-calibration points are generated. Where, θ max θ is the maximum angle of the joint. min This is the minimum angle of the joint; S3. Use the RTDE interface to drive the movement of the two robots through communication. Synchronously collect the rigid body center of mass pose data measured by the motion capture camera and the robot's forward kinematic Cartesian coordinates. Each time, add five sets of calibration point data to the current n valid calibration points and input them into the Tsai-Lenz hand-eye calibration algorithm. Optimize the calibration results using the least squares method, and transform the rigid body center of mass pose data measured by the motion capture camera through the calibration results to obtain the end-effector position P of the two robots. l =(x l ,y l ,z l ),P r =(x r ,y r ,z r ), where P l P represents the end-effector position of the robot equipped with the launch tube, obtained from the motion capture camera data transformation. r This represents the end position of the robot equipped with a receiving tablet, obtained from the data transformation of the motion capture camera; that is, the right robot's end position. Set the end position P of the left robot l =(x l ,y l ,z l ) and the end position P of the right robot r =(x r ,y r ,z r The end-effector position P′ obtained from the forward kinematics of the left robot l =(x′) l ,y′ l ,z′ l The end-effector position P′ obtained from the forward kinematics of the right robot. r =(x′) r ,y′ r ,z′ r ) for comparison; Then, the Euclidean distance between the robot's end-effector position obtained from the motion capture camera data transformation and the robot's end-effector position obtained from the robot's forward kinematics is calculated with a calibration accuracy of e. l =|P l -P′ l |,e r =|P r -P′ r |, where e l e represents the calibration accuracy of the left robot. r This represents the calibration accuracy of the right robot. When the average calibration accuracy of all current calibration points of the dual robots reaches the standard, the addition of five sets of calibration points is deemed valid. The coordinate system is precisely aligned when the number of valid calibration points reaches 20, thus completing the calibration of the dual robots.
2. The automatic calibration method for dual robots based on adaptive point planning in a confined space according to claim 1, characterized in that: In step S1, under the ROS architecture, a 3D simulation model for a dual-robot scanning scenario is constructed on the Rviz platform based on the URDF file, fully reproducing the scanning work space. The 3D simulation model of the scanning scenario is for a dual-robot scanning system. The 3D simulation model of the scanning scenario includes dual robots, dual-robot bases, a transmitting tube, a receiving plate, an operating table, a scanning phantom, and a collision avoidance wall. The dual robots include a left robot and a right robot, and the dual-robot bases include a left robot base and a right robot base. The left robot is mounted on the left robot base, the right robot is mounted on the right robot base, the transmitting tube is mounted at the end of the left robot, the receiving plate is mounted at the end of the right robot, the operating table is placed between the left and right robots, and the scanning phantom is placed at the front end of the operating table.
3. The automatic calibration method for dual robots based on adaptive point planning in a confined space according to claim 2, characterized in that: The crash barriers are installed in front, behind, to the left, right, and below the two robots.
4. The automatic calibration method for dual robots based on adaptive point planning in a confined space according to claim 1, characterized in that: Step S2 includes the following steps: S21. Limit the joint values of the two robots to [θ]. min ,θ max Within the specified range, a set of dual robot joint vectors is randomly generated; S22. Call the collision detection function of the Moveit detection module, input a set of planned dual-robot joint vectors and a set of randomly generated dual-robot joint vectors, and determine whether the dual robots will collide under the set of randomly generated dual-robot joint vectors. If a collision occurs, discard the set of joint vectors. If no collision occurs, retain the set of joint vectors until 30 sets of collision-free dual-robot joint vectors are generated, i.e., N=30 pre-calibration point data. S23. Visualize 30 sets of dual-robot calibration points in the Rviz platform.
5. The automatic calibration method for dual robots based on adaptive point planning in a confined space according to claim 1, characterized in that: In step S3, the calibration result obtained through least squares optimization is calculated as follows: For the left robot, when it is at position P 1l At that time, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its end point. It can be represented as: in, Representing the left robot in P 1l The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the left robot in p 1l The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the left robot in P 1l The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system; When the left robot is at position p 2l Similarly, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its endpoints can be obtained. in, Representing the left robot in P 2l The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the left robot in P 2l The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the left robot in P 2l The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system; Since the relative pose between the robot's end effector coordinate system and the calibration board coordinate system remains constant during the robot's movement, the following formula holds true: remember Since the relative pose relationship between the world coordinate system and the robot's base coordinate system remains constant at any position, let's call it... Therefore, it can be simplified to the form AX = XB, where X is the hand-eye pose matrix to be solved, that is, the transformation matrix between the world coordinate system and the left robot base coordinate system, and then the Tsai-Lenz method can be used to solve it. Similarly, for the right robot, when it is at position P 1r At that time, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its end point. It can be represented as: in, Representing the right robot in P 1r The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the right robot in P 1r The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the right robot in P 1r The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system; When the right robot is at position P 2r Similarly, the transformation matrix between the coordinate system of the rigid body's center of mass and the coordinate system of its endpoints can be obtained. in, Representing the right robot in P 2r The transformation matrix from the position time base coordinate system to the end coordinate system. Representing the right robot in P 2r The position is the transformation matrix from the world coordinate system to the base coordinate system. Representing the right robot in P 2r The position is the transformation matrix from the rigid body's center-of-mass coordinate system to the world coordinate system; Since the relative pose between the robot's end effector coordinate system and the calibration board coordinate system remains constant during the robot's movement, the following formula holds true: remember Since the relative pose relationship between the world coordinate system and the robot's base coordinate system remains constant at any position, let's call it... Therefore, it can be simplified to the form CY=YD, where Y is the hand-eye pose matrix to be solved, that is, the transformation matrix between the world coordinate system and the right robot base coordinate system, and then the Tsai-Lenz method can be used to solve it.
6. The automatic calibration method for dual robots based on adaptive point planning in a confined space according to claim 5, characterized in that: Step S3 includes the following steps: S31. Send N=1 to 5 sets of calibration points to the two robots for movement. When the two robots reach the designated points, synchronously record the rigid body center of mass pose data measured by the motion capture camera and the robot's forward kinematic Cartesian coordinates. S32. Input the data from step S1 into the Tsai-Lenz hand-eye calibration algorithm to obtain calibration results for N = 1 to 5 sets of calibration points, including: Transformation matrix from world coordinate system to left robot base coordinate system From world coordinate system to right robot base coordinate system From the left robot's rigid body center of mass coordinate system to the end effector coordinate system From the robot's rigid body coordinate system to the robot's end effector coordinate system S33. The pose data of the rigid body center of mass of the left and right robots measured by the motion capture cameras at the N=1 to N5 calibration points are sequentially transformed through the calibration results to obtain the end positions of the left and right robots. The calculation formula is as follows: in, This represents the transformation matrix from the rigid body center of mass coordinate system to the end effector coordinate system of the left and right robots at the Nth calibration point. This represents the transformation matrix from the world coordinate system to the rigid body's center of mass coordinate system for the left and right robots at the Nth calibration point. This represents the transformation matrix from the base coordinate system to the world coordinate system for the left and right robots at the Nth calibration point; The end position obtained by combining it with the positive kinematics of the two robots at the Nth position The calibration accuracy of the left and right robots at the Nth position is then calculated by comparing the two, using the following formula: e Nl =|P Nl -P′ Nl | e Nr =|P Nr -P′ Nr | Then, the average calibration accuracy of the calibration points for the left and right robots (N=1 to 5 groups) is calculated. S34. Based on the average value of the calibration point data, filter the calibration point data of N=1 to 5 groups: if the average value exceeds the preset standard, discard the data of that group and send only the data of N=6 to 10 groups to the robot, and control it to repeat steps S31 to S33; if the average value meets the preset standard, retain the calibration points of N=1 to 5 groups, send the data of N=6 to 10 groups to the robot, repeat step S31, and then integrate the rigid body center of mass pose data measured by the motion capture camera in the calibration points of N=1 to 10 groups, and substitute it together with the Cartesian coordinate data calculated by the robot's forward kinematics into steps S32 to S33 to calculate the new calibration result and calibration error, and make a judgment again; The above process is repeated until the number of valid calibration points reaches 20 sets. At this point, the calibration result obtained from these 20 sets of data that meets the error standard is the final result. If all 30 sets of data have been sent to the dual robots, but the average error still does not meet the standard, the calibration is determined to be a failure. The calibration process can be restarted after selecting new calibration points. S35. Use 20 sets of calibration points to obtain the transformation matrix from the world coordinate system to the left robot base coordinate system. From world coordinate system to right robot base coordinate system From the left robot's rigid body center of mass coordinate system to the end effector coordinate system From the robot's rigid body coordinate system to the robot's end effector coordinate system The transformation matrix from the right robot base coordinate system to the left robot base coordinate system can be calculated. The calculation formula is as follows:
7. The automatic calibration method for dual robots based on adaptive point planning in a confined space according to claim 6, characterized in that: In step S34, the standard for determining whether to retain the calibration point needs to be set differently at different stages of the calibration process. The specific settings are as follows: If n is the number of valid calibration points, then the piecewise function form of the standard error mean is: