Twin-arm alignment method and twin-arm alignment system

Abstract

This invention provides a twin-arm alignment method that enables the coordinated operation of multiple robotic arms to meet high-precision construction requirements. [Solution] The system includes a camera position calculation step that calculates the position of the camera relative to the tip of the first robot arm based on the position of a first feature point relative to the camera obtained by the camera and the position of the tip of the first robot arm relative to the base of the first robot arm, and a second robot arm position calculation step that calculates the position of the second robot arm relative to the first robot arm based on the position of a second feature point relative to the camera obtained by the camera, the position of the camera relative to the tip of the first robot arm obtained in the camera position calculation step, the position of the tip of the first robot arm relative to the base of the first robot arm, and the position of the tip of the second robot arm relative to the base of the second robot arm.

Description

[Technical Field]

[0001] The present invention relates to a twin-arm alignment method and a twin-arm alignment system. [Background technology]

[0002] Currently, with the advancement of automation, it is necessary to use a visual system such as a camera to calibrate the robot arm when planning its path.

[0003] Patent Document 1 discloses a system and calibration method for calibrating a robot arm, which includes a visual measurement system, the robot arm being positioned within the field of view of the visual measurement system, and further includes a movable cartridge or probe target. The robot arm calibration method of Patent Document 1 is applicable to the calibration of a single robot arm. [Prior art documents] [Patent Documents]

[0004] [Patent Document 1] Chinese Patent Application Publication No. 2021055264.8 Specification [Overview of the Initiative] [Problems that the invention aims to solve]

[0005] When constructing high-rise buildings or large bridges, the collaborative use of multiple robotic arms for various tasks such as concrete pouring can improve construction efficiency and quality, as well as reduce the risk of workplace accidents during construction. Furthermore, the collaborative work of multiple robotic arms allows for the transport and installation of long or heavy objects, which would be impossible with a single robotic arm. However, when multiple robotic arms work collaboratively, a challenge lies in how to align them to ensure that the combined precision of the robotic arms meets the high-precision construction requirements.

[0006] Therefore, the object of the present invention is to provide a twin-arm alignment method and a twin-arm alignment system that perform coordinate system calibration between two robot arms when multiple robot arms work together, thereby adjusting the accuracy of the combined work of multiple robot arms to meet high-precision construction requirements. [Means for solving the problem]

[0007] The twin-arm alignment method of the present invention comprises a twin arm comprising a first robot arm, a second robot arm, a camera fixed to the tip of the first robot arm, a first feature point fixed to the base of the first robot arm, and a second feature point fixed to the tip of the second robot arm, wherein the first robot arm and / or the second robot arm are fixedly mounted to the construction environment, and the twin-arm alignment method comprises a camera position calculation step of calculating the position of the camera relative to the tip of the first robot arm based on the position of the first feature point relative to the camera obtained by the camera and the position of the tip of the first robot arm relative to the base of the first robot arm, and a second robot arm position calculation step of calculating the position of the second robot arm relative to the first robot arm based on the position of the second feature point relative to the camera obtained by the camera, the position of the camera relative to the tip of the first robot arm obtained in the camera position calculation step, the position of the tip of the first robot arm relative to the base of the first robot arm, and the position of the tip of the second robot arm relative to the base of the second robot arm.

[0008] The present invention relates to a twin-arm alignment system, the twin-arm alignment system comprising a first robot arm, a second robot arm, a camera fixed to the tip of the first robot arm, a first feature point fixed to the base of the first robot arm, and a second feature point fixed to the tip of the second robot arm, wherein the first robot arm and / or the second robot arm are fixedly mounted to the construction environment, and the twin-arm alignment system further comprises a control unit, the control unit comprising: a camera position calculation unit that calculates the position of the camera relative to the tip of the first robot arm based on the position of the first feature point relative to the camera obtained by the camera and the position of the tip of the first robot arm relative to the base of the first robot arm; and a second robot arm position calculation unit that calculates the position of the second robot arm relative to the first robot arm based on the position of the second feature point relative to the camera obtained by the camera, the position of the camera relative to the tip of the first robot arm obtained by the camera position calculation unit, the position of the tip of the first robot arm relative to the base of the first robot arm, and the position of the tip of the second robot arm relative to the base of the second robot arm. [Effects of the Invention]

[0009] The present invention provides a twin-arm alignment method and twin-arm alignment system that perform coordinate system calibration between two robot arms when multiple robot arms are working together, thereby meeting the high-precision construction requirements when multiple robot arms work together. [Brief explanation of the drawing]

[0010] [Figure 1] A schematic diagram showing the configuration of the twin arm according to the present invention. [Figure 2] A flowchart illustrating the twin-arm alignment method according to the present invention. [Figure 3A] A schematic diagram showing the translational movement of the first robot arm along the X-axis according to the present invention. [Figure 3B]A schematic diagram showing the parallel movement of the first robot arm along the Y-axis according to the present invention. [Figure 4A] A schematic diagram showing the rotation of the first robot arm along the X-axis according to the present invention. [Figure 4B] A schematic diagram showing the rotation of the first robot arm along the Z-axis according to the present invention. [Figure 5A] A schematic diagram showing the parallel movement of the second robot arm along the X-axis according to the present invention. [Figure 5B] A schematic diagram showing the parallel movement of the second robot arm along the Y-axis according to the present invention. [Figure 6] A schematic diagram showing the rotation of the second robot arm along the Z-axis according to the present invention. [Modes for carrying out the invention]

[0011] The twin-arm alignment method and twin-arm alignment system according to the present invention will be described below with reference to the drawings.

[0012] It should be noted that the present invention is merely an example and is not limited to the embodiments described below. Modifications that are easily made by those skilled in the art are, of course, included within the scope of this disclosure. For clearer explanation, the drawings may schematically show the dimensions, shapes, etc., of parts of actual embodiments with modifications. In some cases, the same reference numerals are used for corresponding elements in multiple drawings, and detailed explanations are omitted.

[0013] Figure 1 is a schematic diagram showing the configuration of the twin-arm alignment system of the present invention. As shown in Figure 1, the twin arms of this embodiment are twin arms arranged vertically to work in a hoistway 10. The twin-arm alignment system of this embodiment includes a first robot arm 1 fixedly attached to a suspension mechanism suspended in the hoistway 10, a work platform 9 fixedly attached to the inner wall of the hoistway 10, a second robot arm 2 fixedly attached to the work platform 9, a camera 3 fixed to the tip of the first robot arm 1, a first feature point 4 fixed to a suspension mechanism suspended in the hoistway 10, a second feature point 5 fixed to the tip of the second robot arm 2, and a control unit. The control unit has a camera position calculation unit that performs a camera position calculation step described later, a second robot arm position calculation unit that performs a second robot arm position calculation step described later, and a position fluctuation correction unit that performs a position fluctuation correction step described later. The first feature point 4 and the second feature point 5 are objects or markers of the form of signs that can be easily recognized by the camera, and their color, shape, form, and size are not limited.

[0014] The twin arms in this embodiment are further equipped with an acceleration sensor 6 fixed to the base 7 of the first robot arm 1. The acceleration sensor 6 may be replaced with another sensor capable of detecting positional changes, such as a displacement sensor.

[0015] In addition to the twin arms arranged vertically to work in the elevator shaft 10 as in this embodiment, the twin arms of the present invention may also be arranged horizontally or at a fixed angle depending on the work environment, work requirements, etc. In addition to the first robot arm 1 being movable relative to the elevator shaft 10 and the second robot arm 2 being fixedly attached to the elevator shaft 10 as in this embodiment, it is also possible for the first robot arm 1 to be fixedly attached to the elevator shaft 10 and the second robot arm 2 to be movable relative to the elevator shaft 10, or for both the first robot arm 1 and the second robot arm 2 to be fixedly attached to the elevator shaft 10. When the first robot arm 1 is fixedly attached to the elevator shaft 10 and the second robot arm 2 is movable relative to the elevator shaft 10, means for detecting position changes, such as an acceleration sensor, are fixedly attached to the base 8 of the second robot arm 2. When the first robot arm 1 and the second robot arm 2 are fixedly attached to the elevator shaft 10, means for detecting position changes, such as an acceleration sensor, can be omitted.

[0016] Figure 2 is a flowchart illustrating the twin-arm alignment method according to the present invention. Figure 3A is a schematic diagram showing the translation of the first robot arm along the X-axis according to the present invention. Figure 3B is a schematic diagram showing the translation of the first robot arm along the Y-axis according to the present invention. Figure 4A is a schematic diagram showing the rotation of the first robot arm along the X-axis according to the present invention. Figure 4B is a schematic diagram showing the rotation of the first robot arm along the Z-axis according to the present invention. Figure 5A is a schematic diagram showing the translation of the second robot arm along the X-axis according to the present invention. Figure 5B is a schematic diagram showing the translation of the second robot arm along the Y-axis according to the present invention. Figure 6 is a schematic diagram showing the rotation of the second robot arm along the Z-axis according to the present invention.

[0017] In the camera position calculation step S21, the position of the camera 3 relative to the tip of the first robot arm 1 is calculated based on the position of the first feature point 4 obtained by the camera 3 relative to the camera 3 and the position of the tip of the first robot arm 1 relative to the base 7 of the first robot arm 1.

[0018] Preferably, the position of the camera 3 relative to the tip of the first robot arm 1 can be calculated based on the positions of two sets of first feature points 4 acquired by the camera 3 as the first robot arm 1 moves, relative to the camera 3, and the positions of the tips of the two sets of first robot arm 1 relative to the base 7 of the first robot arm 1, which correspond to the positions of the two sets of first feature points 4 relative to the camera 3.

[0019] The position of the first feature point 4 relative to camera 3 is the position of the first feature point 4 in the camera coordinate system. The position of the tip of the first robot arm 1 relative to the base 7 of the first robot arm 1 is the position and orientation angle of the tip of the first robot arm 1 in the base coordinate system of the first robot arm 1. The position of camera 3 relative to the tip of the first robot arm 1 is the position and orientation angle of camera 3 in the tip coordinate system of the first robot arm 1.

[0020] As shown in Figures 3A, 3B, 4A, and 4B, the camera coordinate system has the center of camera 3 as the origin, with the x-direction being parallel to the plane of the paper on the horizontal plane, the y-direction being perpendicular to the plane of the paper, and the z-direction being perpendicular to the horizontal plane. The base coordinate system of the first robot arm 1 has the center of the base of the first robot arm 1 as the origin, with the x-direction being parallel to the plane of the paper on the horizontal plane, the y-direction being perpendicular to the plane of the paper, and the z-direction being perpendicular to the horizontal plane. The tip coordinate system of the first robot arm 1 has the tip of the first robot arm 1 as the origin, with the x-direction being parallel to the plane of the paper on the horizontal plane, the y-direction being perpendicular to the plane of the paper, and the z-direction being perpendicular to the horizontal plane.

[0021] As shown in Figure 3A, the first robot arm 11 and camera 31 are schematic diagrams of the new positions of the first robot arm 1 and camera 31 after the first robot arm 1 has been translated along the X-axis, preferably after 1 second of translation. Before the first robot arm 1 is translated along the X-axis, the coordinate value of the first feature point 4 acquired by camera 3 in the camera coordinate system is xc1, and by reading the first robot arm 1, the coordinate value p1=(x1,y1,z1) and attitude angle value rxyz1 of the tip of the first robot arm 1 in the base coordinate system of the first robot arm 1 are obtained. After the first robot arm 1 is translated along the X-axis, the coordinate value of the first feature point 4 acquired by camera 3 in the camera coordinate system is xc2, and by reading the first robot arm 1, the coordinate value p2=(x2,y2,z2) and attitude angle value rxyz2 of the tip of the first robot arm 1 in the base coordinate system of the first robot arm 1 are obtained.

[0022] As shown in Figure 3B, the first robot arm 12 and camera 32 are schematic diagrams of the new positions of the first robot arm 1 and camera 32 after the first robot arm 1 has been translated along the Y-axis, preferably after 1 second of translation. Before the first robot arm 1 is translated along the Y-axis, the coordinate value of the first feature point 4 acquired by camera 3 in the camera coordinate system is xc3, and by reading the first robot arm 1, the coordinate value p3=(x3,y3,z3) and attitude angle value rxyz3 of the tip of the first robot arm 1 in the base coordinate system of the first robot arm 1 are obtained. After the first robot arm 1 is translated along the Y-axis, the coordinate value of the first feature point 4 acquired by camera 3 in the camera coordinate system is xc4, and by reading the first robot arm 1, the coordinate value p4=(x4,y4,z4) and attitude angle value rxyz4 of the tip of the first robot arm 1 in the base coordinate system of the first robot arm 1 are obtained.

[0023] The equation of the base coordinate system of the first robotic arm 1 is constructed as follows. Here, R is the rotation matrix corresponding to the posture angle in the tip coordinate system of the first robotic arm 1 of the camera 3, and T is the column vector of the offset position value in the tip coordinate system of the first robotic arm 1 of the camera 3.

[0024] First, the posture angle conversion matrix of the tip posture with respect to the base coordinate system of the first robotic arm 1 is as follows.

[0025] (rx1, ry1, rz1) → R e1 , (rx2, ry2, rz2) → R e2 (rx3, ry3, rz3) → R e3 , (rx4, ry4, rz4) → R e4 Next, before and after the X-axis parallel movement and Y-axis movement, the position coordinate calculation formula of the first feature point 4 in the base coordinate system of the first robotic arm 1 is as follows.

[0026] X obj = R e1 × (R × xc1 + T) + p1(1) X obj = R e2 × (R × xc2 + T) + p2(2) X obj = R e3 × (R × xc3 + T) + p3(3) X obj = R e4 × (R × xc4 + T) + p4(4) Before and after the first robotic arm 1 moves in parallel, since the position of the first feature point 4 in the base coordinate system of the first robotic arm 1 is fixed, Equation (1) and Equation (2) are equal, Equation (3) and Equation (4) are equal, and the equations are constructed as follows.

[0027] R e1 × (R × xc1 + T) + p1 = R e2 × (R × xc2 + T) + p2(1) = (2) R e3 × (R × xc3 + T) + p3 = R e4×(R×xc4+T)+p4(3)=(4) Solve the above equation. First, in the process of translation, R e1 ×T,R e2 ×T,R e3 ×T and R e4 Since ×T is the same, we can delete the term and simplify it to the following equation.

[0028] R × (xc1 - xc2) = R e1 -1 ×(p2-p1)(5) R × (xc3 - xc4) = R e3 -1 ×(p4-p3)(6) Next, we construct a vector relationship.

[0029] v1 = xc1 - xc2, v2 = R e1 -1 ×(p2-p1), v3 = xc3 - xc4, v4 = R e3 -1 ×(p4-p3) R × v1 = v2 R × v3 = v4 R is determined using the rotational transformation method.

[0030] η1=nomal(v1), η2=nomal(v1×v3), η3=η1×η2 η4=nomal(v2), η5=nomal(v2×v4), η6=η4×η5 A=[η1,η2,η3], B=[η4,η5,η6] R=B -1 ×A As shown in Figure 4A, the first robot arm 13 and camera 33 are schematic diagrams of the new positions of the first robot arm 1 and camera 33 after the first robot arm 1 has been rotated along the X-axis, preferably for 1 second. Before the first robot arm 1 is rotated along the X-axis, the coordinate value of the first feature point 4 acquired by camera 3 in the camera coordinate system is xc1, and by reading the first robot arm 1, the coordinate value p1=(x1,y1,z1) and attitude angle value rxyz1 of the tip of the first robot arm 1 in the base coordinate system of the first robot arm 1 are obtained. After the first robot arm 1 is rotated along the X-axis, the coordinate value of the first feature point 4 acquired by camera 3 in the camera coordinate system is xc2, and by reading the first robot arm 1, the coordinate value p2=(x2,y2,z2) and attitude angle value rxyz2 of the tip of the first robot arm 1 in the base coordinate system of the first robot arm 1 are obtained.

[0031] As shown in Figure 4B, the first robot arm 14 and camera 34 are schematic diagrams of the new positions of the first robot arm 1 and camera 34 after the first robot arm 1 has been rotated along the Z-axis, preferably for 1 second. Before the first robot arm 1 is rotated along the Z-axis, the coordinate value of the first feature point 4 acquired by camera 3 in the camera coordinate system is xc3, and by reading the first robot arm 1, the coordinate value p3=(x3,y3,z3) and attitude angle value rxyz3 of the tip of the first robot arm 1 in the base coordinate system of the first robot arm 1 are obtained. After the first robot arm 1 is rotated along the Z-axis, the coordinate value of the first feature point 4 acquired by camera 3 in the camera coordinate system is xc4, and by reading the first robot arm 1, the coordinate value p4=(x4,y4,z4) and attitude angle value rxyz4 of the tip of the first robot arm 1 in the base coordinate system of the first robot arm 1 are obtained.

[0032] The equations for the base coordinate system of the first robot arm 1 are constructed as follows: Here, R is the rotation matrix corresponding to the attitude angle of the first robot arm 1 in the tip coordinate system of camera 3, and T is the column vector of the offset position values ​​in the tip coordinate system of camera 3.

[0033] First, the attitude angle transformation matrix for the tip position of the first robot arm 1 relative to the base coordinate system is as follows:

[0034] (rx1,ry1,rz1) → R e1 , (rx2,ry2,rz2)→R e2 (rx3,ry3,rz3)→R e3 , (rx4,ry4,rz4)→R e4 Next, we create position coordinate calculation formulas for the first feature point 4 located in the base coordinate system of the first robot arm 1, before and after the first robot arm 1 rotates along the X and Z axes.

[0035] X obj =R e1 ×(R×xc1+T)+p1(1) X obj =R e2 ×(R×xc²+T)+p²(2) X obj =R e3 ×(R×xc3+T)+p3(3) X obj =R e4 ×(R×xc4+T)+p4(4) Since the position of the first feature point 4 in the base coordinate system of the first robot arm 1 is fixed before and after the first robot arm 1 rotates along the X and Z axes, equations (1) and (2) are equal, and equations (3) and (4) are equal, and the equations are constructed as follows.

[0036] R e1 ×(R×xc1+T)+p1=R e2 ×(R×xc²+T)+p²(1)=(2) R e3×(R×xc3+T)+p3=R e4 ×(R×xc4+T)+p4(3)=(4) Finally, align T to the left side of the equation and set the equation for the X-axis and the rotation equation for the Z-axis as the formulas for calculating T.

[0037] R e1 ×(R×xc1+T)+p1=R e2 ×(R×xc²+T)+p²(5) (R e1 -R e2 ) × T = (R e2 ×R×xc²+p²)-(R e1 ×R×xc1+p1)(6) R e3 ×(R×xc3+T)+p3=R e4 ×(R×xc4+T)+p4(7) (R e3 -R e4 ) × T = (R e4 ×R×xc4+p4)-(R e3 ×R×xc3+p3)(8) By structuring it as A × T = B, we can find the position equation as follows.

[0038] A=ER x1 -1 ·R x2 B=R e1 -1 ((R e2 ×R×xc²+p²)-(R e1 ×R×xc1+p1)) T=A -1 ·B In the second robot arm position calculation step S22, the position of the second robot arm 2 relative to the first robot arm 1 is calculated based on the position of the second feature point 5 relative to camera 3 acquired from the camera, the position of camera 3 relative to the tip of the first robot arm 1 acquired in the camera position calculation step S21, the position of the tip of the first robot arm 1 relative to the base 7 of the first robot arm 1, and the position of the tip of the second robot arm 2 relative to the base 8 of the second robot arm 2.

[0039] Preferably, the position of the second robot arm 2 relative to the first robot arm 1 is calculated based on the positions of the second feature points 5 relative to the two sets of cameras 3 acquired by moving the second robot arm 2, the position of the camera 3 relative to the tip of the first robot arm 1 acquired in the camera position calculation step S21, the position of the tip of the first robot arm 1 relative to the base of the first robot arm 1, and the positions of the tips of the two sets of second robot arm 2 relative to the base 8 of the second robot arm 2, which correspond to the positions of the second feature points 5 relative to the two sets of cameras 3.

[0040] The position of the second feature point 5 relative to camera 3 refers to the position of the second feature point 5 in the camera coordinate system. The position of the tip of the second robot arm 2 relative to the base 8 of the second robot arm 2 refers to the position and attitude angle values ​​of the tip of the second robot arm 2 in the base coordinate system of the second robot arm 2. The position of the second robot arm 2 relative to the first robot arm 1 refers to the position and attitude angle values ​​of the base 8 of the second robot arm 2 in the base coordinate system of the first robot arm 1.

[0041] As shown in Figures 5A, 5B, and 6, the base coordinate system of the second robot arm 2 has the center of the base 8 of the second robot arm 2 as the origin, with the x-direction being the direction parallel to the plane of the paper, the y-direction being the direction perpendicular to the plane of the paper, and the z-direction being the direction perpendicular to the horizontal plane.

[0042] As shown in Figure 5A, the second robot arm 21 and the second feature point 51 are schematic diagrams of the new positions of the second robot arm 2 and the second feature point 5 after the second robot arm 2 has been translated along the X-axis, preferably after 1 second of translation. Before and after the second robot arm 2 has been translated along the X-axis, the coordinate values ​​of the second feature point 5 in the camera coordinate system acquired by the camera 3 are xc1 and xc2, and from the readings of the first robot arm and the second robot arm, the attitude matrix of the tip of the first robot arm 1 is R e1 , R e2The coordinate values ​​are p1 and p2, the coordinate values ​​of the tip of the second robot arm 2 in the base coordinate system of the second robot arm 2 are up1 and up2, and the attitude matrix is ​​UR e1 =(0,0,Rz1), UR e2 Get (0,0,Rz2).

[0043] As shown in Figure 5B, the second robot arm 22 and the second feature point 52 are schematic diagrams of the new positions of the second robot arm 2 and the second feature point 5 after the second robot arm 2 has been translated along the Y-axis, preferably for 1 second. Before and after the second robot arm 2 has been translated along the Y-axis, the coordinate values ​​of the second feature point 5 in the camera coordinate system acquired by the camera 3 are xc3 and xc4. From the readings of the first and second robot arms, the attitude determinants of the tip of the first robot arm 1 are Re3 and Re4, and the coordinate values ​​are p3 and p4. The coordinate values ​​of the tip of the second robot arm 2 in the base coordinate system of the second robot arm 2 are up3 and up4, and the attitude determinant is UR. e1 =(0,0,Rz3), UR e2 Get (0,0,Rz4).

[0044] The equations for the movement of the second robot arm 2 before and after are constructed as follows. Here, R is the calculation result in step S21, i.e., the rotation matrix corresponding to the attitude angle of camera 3 in the tip coordinate system of the first robot arm 1, T is the calculation result in step S21, i.e., the column vector of the offset position value of camera 3 in the tip coordinate system of the first robot arm 1, R u The base coordinate system of the second robot arm 2 is the rotation matrix of the attitude in the base coordinate system of the first robot arm 1, T uHere, \(\mathbf{x}_{b1}\) is the coordinate of the second feature point 5 in the base coordinate system of the first robotic arm 1 before the second robotic arm 2 is translated parallel to the X-axis, \(\mathbf{x}_{b2}\) is the coordinate of the second feature point 5 in the base coordinate system of the first robotic arm 1 after the second robotic arm 2 is translated parallel to the X-axis, \(\mathbf{x}_{b3}\) is the coordinate of the second feature point 5 in the base coordinate system of the first robotic arm 1 before the second robotic arm 2 is translated parallel to the Y-axis, and \(\mathbf{x}_{b4}\) is the coordinate of the second feature point 5 in the base coordinate system of the first robotic arm 1 after the second robotic arm 2 is translated parallel to the Y-axis.

[0045] Therefore, the calculation formulas for \(\mathbf{x}_{b1}\), \(\mathbf{x}_{b2}\), \(\mathbf{x}_{b3}\), and \(\mathbf{x}_{b4}\) are as follows.

[0046] \(\mathbf{x}_{b1}=\mathbf{R}\) e1 \(\times(\mathbf{R}\times\mathbf{x}_{c1}+\mathbf{T})+\mathbf{p}_1\ (11)\) \(\mathbf{x}_{b2}=\mathbf{R}\) e2 \(\times(\mathbf{R}\times\mathbf{x}_{c2}+\mathbf{T})+\mathbf{p}_2\ (12)\) \(\mathbf{x}_{b3}=\mathbf{R}\) e3 \(\times(\mathbf{R}\times\mathbf{x}_{c3}+\mathbf{T})+\mathbf{p}_3\ (13)\) \(\mathbf{x}_{b4}=\mathbf{R}\) e4 \(\times(\mathbf{R}\times\mathbf{x}_{c4}+\mathbf{T})+\mathbf{p}_4\ (14)\) First, from the above formulas (11), (12), (13), and (14), the position calculation formula of the second feature point 5 in the end coordinate system of the second robotic arm 2 is as follows.

[0047] \(\text{Obj}\) end \(=\mathbf{U}\mathbf{R}\) e1 -1 \(\times(\mathbf{R}\) u \(\times\mathbf{x}_{b1}+\mathbf{T}\) u \(-\mathbf{u}_{p1})\ (1)\) \(\text{Obj}\) end \(=\mathbf{U}\mathbf{R}\) e2 -1 \(\times(\mathbf{R}\) u \(\times\mathbf{x}_{b2}+\mathbf{T}\) u \(-\mathbf{u}_{p2})\ (2)\) \(\mathbf{U}\mathbf{R}\) e1 \(=\mathbf{U}\mathbf{R}\) e2 \(\mathbf{U}\mathbf{R}\) e3 \(=\mathbf{U}\mathbf{R}\) e4 \(\text{Obj}\) end=UR e3 -1 ×(R u ×xb3 + T u -up3)(3) Obj end =UR e4 -1 ×(R u ×xb4 + T u -up4)(4) Next, before and after the second robot arm 2 translates, since the position of the second feature point 5 in the tip coordinate system of the second robot arm 2 is constant, Equation (1) and Equation (2) are equal, Equation (3) and Equation (4) are equal, and the equations are constructed as follows.

[0048] UR e1 -1 ×(R u ×xb1 + T u -up1) = UR e2 -1 ×(R u ×xb2 + T u -up2)(5) UR e3 -1 ×(R u ×xb3 + T u -up3) = UR e4 -1 ×(R u ×xb4 + T u -up4)(6) Since the tip pose matrix of the second robot arm 2 during translation is constant, UR e1 -1 ×T u 、UR e2 -1 ×T [[ID=J]] u 、UR e3 -1 ×T u 、UR e4 -1 ×T u are deleted, and the equations (5) and (6) for translation along the X and Y axes of the second robot arm 2 are obtained as follows.

[0049] R u ×(xb1 - xb2) = up2 - up1(7) R It should be noted that there seems to be an error in the original text where "UR e2 -1 u ×T " has an incorrect closing tag "J" which has been corrected in the translation for better readability. If this is not an error in the original, please clarify.u ×(xb3-xb4)=up4-up3(8) Solve the equation. First, construct the vector relationship as follows.

[0050] v1=xb1-xb2, v2=up2-up1, v3=xb3-xb4, v4=up4-up3 R u ×v1=v2 R u ×v3=v4 Using the rotational transformation method, R u They sought it.

[0051] η1=nomal(v1), η2=nomal(v1×v3), η3=η1×η2 η4=nomal(v2), η5=nomal(v2×v4), η6=η4×η5 A=[η1,η2,η3], B=[η4,η5,η6] R u =B -1 ×A As shown in Figure 6, the second robot arm 23 and the second feature point 53 are schematic diagrams of the new positions of the second robot arm 2 and the second feature point 5 after the second robot arm 2 has been rotated along the Z-axis, preferably for 1 second. Before and after the second robot arm 2 is rotated along the Z-axis, the coordinate values ​​of the second feature point 5 acquired by the camera 3 in the camera coordinate system are xc1 and xc2, and from the readings of the first robot arm and the second robot arm, the attitude matrix of the tip of the first robot arm 1 is R e1 , R e2 The coordinate values ​​are p1 and p2, the coordinate values ​​of the tip of the second robot arm 2 in the base coordinate system of the second robot arm 2 are up1 and up2, and the attitude matrix is ​​UR e1 =(0,0,R z1 ), UR e2 =(0,0,R z2 ) obtain.

[0052] The equations for rotating the second robot arm 2 along the Z-axis are constructed as follows: Here, R is the calculation result in step S21, i.e., the rotation matrix corresponding to the attitude angle of camera 3 in the tip coordinate system of the first robot arm 1, T is the calculation result in step S21, i.e., the column vector of the offset position value of camera 3 in the tip coordinate system of the first robot arm 1, R u T is the rotation matrix of the orientation of the first robot arm 1 in the base coordinate system of the base coordinate system of the second robot arm 2. u xb1 is the offset position column vector of the origin of the second robot arm 2 in the base coordinate system of the first robot arm 1 before translating the second robot arm 2 along the X-axis, xb2 is the coordinate of the second feature point 5 in the base coordinate system of the first robot arm 1 after translating the second robot arm 2 along the X-axis, xb3 is the coordinate of the second feature point 5 in the base coordinate system of the first robot arm 1 before translating the second robot arm 2 along the Y-axis, xb4 is the coordinate of the second feature point 5 in the base coordinate system of the first robot arm 1 after translating the second robot arm 2 along the Y-axis, and E is the permutation identity matrix.

[0053] Therefore, the formulas for xb1, xb2, xb3, and xb4 are as follows:

[0054] xb1=R e1 ×(R×xc1+T)+p1(11) xb²=R e2 ×(R×xc²+T)+p²(12) xb3=R e3 ×(R×xc3+T)+p3(13) xb4=R e4 ×(R×xc4+T)+p4(14) First, from equations (11), (12), (13), and (14) above, the formula for calculating the second feature point 5 in the base coordinate system of the second robot arm 2 is as follows.

[0055] Obj end =UR e1-1 ×(R u ×xb1+T u -up1)(1) Obj end =UR e2 -1 ×(R u ×xb²+T u -up2)(2) Next, since the position of the second feature point 5 in the base coordinate system of the second robot arm 2 remains constant before and after the second robot arm 2 rotates along the Z axis, equation (1) and equation (2) become equal, and the equation is constructed as follows.

[0056] UR e1 -1 ×(R u ×xb1+T u -up1)=UR e2 -1 ×(R u ×xb²+T u -up2)(1)=(2) Finally, T U Align it to the left side of the equation, and the Z-axis rotation equation is T U Set this as the calculation formula.

[0057] (E-UR e1 ×UR e2 -1 )×T U =UR e1 ×UR e2 -1 ×((R u ×xb2-up2)-(R u ×xb1-up1)) A×T U Structure it as =B, and find the position as follows.

[0058] A=E-UR e1 ×UR e2 -1 B=UR e1 ×UR e2 -1 ×((R u ×xb2-up2)-(R u ×xb1-up1)) A×TU =B T U =A -1 ·B Vibrations generated during operations such as those shown in Figures 5A, 5B, and 6 cause the position of the second robot arm 2 in the base coordinate system of the first robot arm 1 to fluctuate. To ensure the alignment accuracy of the twin arms, it is necessary to correct the amount of fluctuation in real time. In the position fluctuation correction step S23, the position of the second robot arm 2 relative to the first robot arm 1 is corrected based on the position fluctuation of the first robot arm 1 obtained from the acceleration sensor 6. If the position fluctuation value of the first robot arm 1 detected by the acceleration sensor 6 is p=(x,y,z), the corrected coordinate value of the first robot arm 1 is calculated by adding the position fluctuation amount p=(x,y,z) of the first robot arm 1 to the coordinate reading of the first robot arm 1. Note that if both the first robot arm 1 and the second robot arm 2 are fixedly mounted to the elevator shaft 10, the means for detecting position fluctuations such as the acceleration sensor can be omitted, and the position fluctuation correction step S23 for correcting position fluctuations can be omitted.

[0059] According to the twin-arm alignment method and twin-arm alignment system described above, when multiple robot arms work together, coordinate system calibration is performed between the two robot arms, and the accuracy of the combined work of multiple robot arms is made to meet high-precision construction requirements.

[0060] While several embodiments of the present invention have been described, these embodiments are presented as examples and are not intended to limit the scope of the invention. These embodiments can be carried out in various other forms, and various omissions, substitutions, and modifications can be made without departing from the spirit of the invention. These embodiments and their variations are included in the scope of the invention and its equivalents as described in the claims, as are included in the scope and spirit of the invention. [Explanation of Symbols]

[0061] 1, 11-14: First robotic arm 2. 21-23: Second robotic arm 3, 31-34: Camera 4: First characteristic point 5: Second characteristic point 6: Accelerometer 7, 8: Bass 9: Work Platform 10: Elevator

Claims

1. This is a method for aligning twin arms. The aforementioned twin arms The first robotic arm and The second robotic arm and A camera fixed to the tip of the first robot arm, A first feature point fixed to the base of the first robot arm, The second robot arm comprises a second feature point fixed to the tip of the second robot arm, The first robot arm and / or the second robot arm are fixedly mounted to the construction environment. The alignment method for the twin arms is as follows: A camera position calculation step that calculates the position of the camera relative to the tip of the first robot arm based on the position of the first feature point relative to the camera obtained by the camera and the position of the tip of the first robot arm relative to the base of the first robot arm, The system includes a second robot arm position calculation step which calculates the position of the second robot arm relative to the first robot arm based on the position of the second feature point relative to the camera obtained by the camera, the position of the camera relative to the tip of the first robot arm obtained in the camera position calculation step, the position of the tip of the first robot arm relative to the base of the first robot arm, and the position of the tip of the second robot arm relative to the base of the second robot arm. How to align a twin-arm suspension.

2. The first robotic arm is movable relative to the construction environment, and the second robotic arm is fixedly mounted relative to the construction environment. The twin arms further include position change detection means fixed to the base of the first robot arm, The alignment method for the twin arms The system further includes a position variation correction step, which corrects the position of the second robot arm relative to the first robot arm based on the position variation of the first robot arm obtained by the position variation detection means. The twin-arm alignment method according to claim 1.

3. The first robotic arm is fixedly mounted to the construction environment, and the second robotic arm is movable relative to the construction environment. The twin arms further include position change detection means fixed to the base of the second robot arm, The alignment method for the twin arms The system further includes a position change correction step, which corrects the position of the second robot arm relative to the first robot arm based on the position change of the second robot arm obtained by the position change detection means. The twin-arm alignment method according to claim 1.

4. In the camera position calculation step, the position of the camera relative to the tip of the first robot arm is calculated based on the positions of the two sets of first feature points for the camera acquired from the camera by moving the first robot arm, and the positions of the tips of the first robot arms relative to the bases of the two sets of first robot arms corresponding to the positions of the first feature points for the two sets of cameras. The twin-arm alignment method according to claim 1.

5. In the camera position calculation step, The position of the first feature point relative to the camera is the position of the first feature point in the camera coordinate system. The position of the tip of the first robot arm relative to the base of the first robot arm is the position and orientation angle value of the tip of the first robot arm in the base coordinate system of the first robot arm. The position of the camera relative to the tip of the first robot arm is the position and orientation angle value of the camera in the tip coordinate system of the first robot arm. The twin-arm alignment method according to claim 4.

6. In the second robot arm position calculation step, the position of the second robot arm relative to the first robot arm is calculated based on the positions of the second feature points relative to the two sets of cameras obtained by moving the second robot arm, the position of the camera relative to the tip of the first robot arm obtained in the camera position calculation step, the position of the tip of the first robot arm relative to the base of the first robot arm, and the positions of the tips of the second robot arms relative to the bases of the two sets of second robot arms corresponding to the positions of the second feature points relative to the two sets of cameras. The twin-arm alignment method according to claim 5.

7. In the second robot arm position calculation step, The position of the second feature point relative to the camera is the position of the second feature point in the camera coordinate system. The position of the tip of the second robot arm relative to the base of the second robot arm is the position and orientation angle value of the tip of the second robot arm in the base coordinate system of the second robot arm. The position of the second robot arm relative to the first robot arm is the position and orientation angle of the base of the second robot arm in the base coordinate system of the first robot arm. The twin-arm alignment method according to claim 6.

8. It is a twin-arm alignment system, The aforementioned twin-arm alignment system is The first robotic arm and The second robotic arm and A camera fixed to the tip of the first robot arm, A first feature point fixed to the base of the first robot arm, The second robot arm comprises a second feature point fixed to the tip of the second robot arm, The first robot arm and / or the second robot arm are fixedly mounted to the construction environment. The twin-arm alignment system further comprises a control unit, and the control unit is A camera position calculation unit calculates the position of the camera relative to the tip of the first robot arm based on the position of the first feature point relative to the camera obtained by the camera and the position of the tip of the first robot arm relative to the base of the first robot arm. The system includes a second robot arm position calculation unit that calculates the position of the second robot arm relative to the first robot arm based on the position of the second feature point relative to the camera obtained by the camera, the position of the camera relative to the tip of the first robot arm obtained by the camera position calculation unit, the position of the tip of the first robot arm relative to the base of the first robot arm, and the position of the tip of the second robot arm relative to the base of the second robot arm. Twin-arm alignment system.

9. The first robotic arm is movable relative to the construction environment, and the second robotic arm is fixedly mounted relative to the construction environment. The twin arms further include position change detection means fixed to the base of the first robot arm, The control unit The system further includes a position change correction unit that corrects the position of the second robot arm relative to the first robot arm based on the position change of the first robot arm obtained by the position change detection means. The twin-arm alignment system according to claim 8.

10. The first robotic arm is fixedly mounted to the construction environment, and the second robotic arm is movable relative to the construction environment. The twin arms further include position change detection means fixed to the base of the second robot arm, The control unit The system further includes a position change correction unit that corrects the position of the second robot arm relative to the first robot arm based on the position change of the second robot arm obtained by the position change detection means. The twin-arm alignment system according to claim 8.

Images