Alignment method of double mechanical arms and alignment system of double mechanical arms

Abstract

The invention discloses a double-mechanical-arm alignment method and a double-mechanical-arm alignment system. The robot includes: a first robot arm; a second mechanical arm; a camera; a first feature point; and the first mechanical arm or / and the second mechanical arm are fixedly installed relative to the construction environment. Comprising the following steps: a camera position calculation step: calculating the position of a camera relative to the tail end of a first mechanical arm according to the position of a first feature point relative to the camera obtained through the camera and the position of the tail end of the first mechanical arm relative to a base of the first mechanical arm; calculating the position of a second mechanical arm, the position of the camera relative to the tail end of the first mechanical arm is obtained in the camera position calculation step according to the position, obtained through the camera, of a second feature point relative to the camera; and calculating the position of the second robot arm relative to the first robot arm according to the position of the tail end of the first robot arm relative to the base of the first robot arm and the position of the tail end of the second robot arm relative to the base of the second robot arm.

Description

Technical Field

[0001] The invention relates to a dual-mechanical-arm alignment method and a dual-mechanical-arm alignment system. Background Art

[0002] At present, with the continuous development of automated operations, when planning the path of a robotic arm, it is necessary to calibrate the robotic arm with the help of a visual system such as a camera.

[0003] Patent document 1 below discloses a system and method for calibrating a robot arm, wherein the system includes a visual measurement system, a robot arm is arranged within the field of view of the visual measurement system, and also includes a movable light pen or detection target. The robot arm calibration method in patent document 1 is applicable to the calibration of a single robot arm.

[0004] Prior art literature

[0005] Patent Literature

[0006] Patent document 1: CN2021055264.8 Summary of the invention

[0007] Problem that the invention aims to solve

[0008] When constructing high-rise buildings and large bridges, multiple robotic arms can work together to perform various tasks such as pouring concrete, which can not only improve construction efficiency and project quality, but also reduce the risk of work-related injuries during construction. In addition, through the collaborative operation of multiple robotic arms, it is possible to carry and install longer or heavier objects that cannot be completed by a single robotic arm. However, when multiple robotic arms work together, how to align the multiple robotic arms so that the accuracy of the coordinated construction of multiple robotic arms meets the high-precision construction requirements is a difficult problem.

[0009] In view of this, the purpose of the present invention is to provide a dual robotic arm alignment method and dual robotic arm alignment system for calibrating the coordinate systems between two robotic arms when multiple robotic arms work together, so that the accuracy of the coordinated construction of multiple robotic arms meets the high-precision construction requirements.

[0010] Technical means of solving problems

[0011] In the dual robotic arm alignment method of the present invention, the dual robotic arm comprises: a first robotic arm; a second robotic arm; a camera, which is fixed to the end of the first robotic arm; a first feature point, which is fixed relative to the base of the first robotic arm; and a second feature point, which is fixed to the end of the second robotic arm. The first robotic arm and / or the second robotic arm are fixedly installed relative to the construction environment. The dual robotic arm alignment method includes: a camera position calculation step, which calculates the position of the camera relative to the end of the first robotic arm based on the position of the first feature point relative to the camera obtained by the camera, and the position of the end of the first robotic arm relative to the base of the first robotic arm; and a second robotic arm position calculation step, which calculates the position of the second robotic arm relative to the first robotic 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 end of the first robotic arm obtained in the camera position calculation step, the position of the end of the first robotic arm relative to the base of the first robotic arm, and the position of the end of the second robotic arm relative to the base of the second robotic arm.

[0012] The dual-arm alignment system of the present invention comprises: a first robotic arm; a second robotic arm; a camera fixed to the end of the first robotic arm; a first feature point fixed relative to the base of the first robotic arm; and a second feature point fixed to the end of the second robotic arm. The first robotic arm and / or the second robotic arm are fixedly installed relative to the construction environment. The dual-arm alignment system also comprises: a camera position calculation unit, which calculates the position of the camera relative to the end of the first robotic arm based on the position of the first feature point relative to the camera obtained by the camera and the position of the end of the first robotic arm relative to the base of the first robotic arm; and a second robotic arm position calculation unit, which calculates the position of the second robotic arm relative to the first robotic 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 end of the first robotic arm obtained in the camera position calculation unit, the position of the end of the first robotic arm relative to the base of the first robotic arm, and the position of the end of the second robotic arm relative to the base of the second robotic arm.

[0013] Effects of the Invention

[0014] The present invention can provide a dual robotic arm alignment method and dual robotic arm alignment system which can calibrate coordinate systems between two robotic arms when multiple robotic arms work in collaboration, so that the accuracy of coordinated construction of the multiple robotic arms meets high-precision construction requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 It is a schematic diagram showing the structure of the dual robot arm of the present invention.

[0016] Figure 2 It is a flow chart showing the dual robot arm alignment method of the present invention.

[0017] FIG. 3( a ) is a schematic diagram showing the parallel movement of the first robot arm along the X-axis according to the present invention.

[0018] FIG. 3( b ) is a schematic diagram showing the parallel movement of the first robot arm along the Y-axis of the present invention.

[0019] FIG. 4( a ) is a schematic diagram showing the first robot arm of the present invention rotating along the X-axis.

[0020] FIG. 4( b ) is a schematic diagram showing the first robot arm of the present invention rotating along the Z axis.

[0021] FIG. 5( a ) is a schematic diagram showing the parallel movement of the second robot arm along the X-axis according to the present invention.

[0022] FIG5(b) is a schematic diagram showing the parallel movement of the second robot arm along the Y-axis of the present invention.

[0023] Figure 6 It is a schematic diagram showing the rotation of the second robot arm of the present invention along the Z axis. DETAILED DESCRIPTION

[0024] Hereinafter, a dual-robot alignment method and a dual-robot alignment system according to the present invention will be described with reference to the accompanying drawings.

[0025] In addition, the present disclosure is only an example, and the invention is not limited to the contents described in the following embodiments. Deformations that can be easily made by those skilled in the art are of course included in the scope of the present disclosure. In order to explain more clearly, in the drawings, the size, shape, etc. of each part are sometimes changed relative to the actual embodiment and schematically represented. In multiple drawings, the same figure numbers are marked on corresponding elements, and detailed descriptions are sometimes omitted.

[0026] Figure 1 Schematic diagram showing the structure of the dual robot arm alignment system of the present invention. Figure 1As shown, the dual robotic arms of this embodiment are dual robotic arms configured in the up and down directions for operation in the shaft 10. The dual robotic arm alignment system of this embodiment comprises: a first robotic arm 1 fixedly mounted on a suspension mechanism suspended in the shaft 10, a work platform 9 fixedly mounted on the inner wall of the shaft 10, a second robotic arm 2 fixedly mounted on the work platform 9, a camera 3 fixed at the end of the first robotic arm 1, a first feature point 4 fixedly mounted on the suspension mechanism suspended in the shaft 10, a second feature point 5 fixed at the end of the second robotic arm 2, and a control unit. The control unit comprises: a camera position calculation unit that performs the following camera position calculation step; a second robotic arm position calculation unit that performs the following second robotic arm position calculation step; and a position change compensation unit that performs the following position change compensation step. Among them, the first feature point 4 and the second feature point 5 are identification objects or marks that can be easily recognized by the camera, and are not limited to color, shape, form and size.

[0027] The dual robot arm of this embodiment further has an acceleration sensor 6 fixed on the base 7 of the first robot arm 1. The acceleration sensor 6 may also be replaced by other devices capable of detecting position changes, such as a displacement sensor.

[0028] In addition to the dual robotic arms configured in the up-and-down directions for operation in the shaft 10 as in the present embodiment, the dual robotic arms of the present invention may also be configured in the horizontal direction or at a certain angle in accordance with the operating environment, operating requirements, etc. In addition to the first robotic arm 1 being movable relative to the shaft 10 and the second robotic arm 2 being fixedly installed relative to the shaft 10 as in the present embodiment, the first robotic arm 1 may also be fixedly installed relative to the shaft 10 and the second robotic arm 2 may be movable relative to the shaft 10, or both the first robotic arm 1 and the second robotic arm 2 may be fixedly installed relative to the shaft 10. In the case where the first robotic arm 1 is fixedly installed relative to the shaft 10 and the second robotic arm 2 is movable relative to the shaft 10, a device for detecting position changes, such as an acceleration sensor, etc., may be fixedly installed on the base 8 of the second robotic arm 2. In the case where both the first robotic arm 1 and the second robotic arm 2 are fixedly installed relative to the shaft 10, the installation of a device for detecting position changes, such as an acceleration sensor, etc., may be omitted.

[0029] Figure 2 3(a) is a schematic diagram showing the parallel movement of the first robot arm of the present invention along the X-axis. FIG3(b) is a schematic diagram showing the parallel movement of the first robot arm of the present invention along the Y-axis. FIG4(a) is a schematic diagram showing the rotation of the first robot arm of the present invention along the X-axis. FIG4(b) is a schematic diagram showing the rotation of the first robot arm of the present invention along the Z-axis. FIG5(a) is a schematic diagram showing the parallel movement of the second robot arm of the present invention along the X-axis. FIG5(b) is a schematic diagram showing the parallel movement of the second robot arm of the present invention along the Y-axis. Figure 6It is a schematic diagram showing the rotation of the second robot arm of the present invention along the Z axis.

[0030] In the camera position calculation step S21, the position of the camera 3 relative to the end of the first robotic 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 end of the first robotic arm 1 relative to the base 7 of the first robotic arm 1.

[0031] Preferably, the position of the camera 3 relative to the end of the first robotic arm 1 can be calculated based on the positions of two groups of first feature points 4 obtained by the camera 3 when the first robotic arm 1 is moved relative to the camera 3, and the positions of two groups of ends of the first robotic arm 1 corresponding to the positions of the two groups of first feature points 4 relative to the camera 3 relative to the base 7 of the first robotic arm 1.

[0032] The position of the first feature point 4 relative to the camera 3 refers to the position of the first feature point 4 in the camera coordinate system. The position of the end of the first robotic arm 1 relative to the base 7 of the first robotic arm 1 refers to the position and attitude angle value of the end of the first robotic arm 1 in the base coordinate system of the first robotic arm 1. The position of the camera 3 relative to the end of the first robotic arm 1 refers to the position and attitude angle value of the camera 3 in the end coordinate system of the first robotic arm 1.

[0033] As shown in Figures 3(a), 3(b), 4(a), and 4(b), the camera coordinate system is based on the center of the camera 3 as the origin, the direction parallel to the paper plane along the upper edge of the horizontal plane as the x direction, the direction perpendicular to the paper plane as the y direction, and the direction perpendicular to the horizontal plane as the z direction. The base coordinate system of the first robotic arm 1 is based on the center of the base of the first robotic arm 1 as the origin, the direction parallel to the paper plane along the upper edge of the horizontal plane as the x direction, the direction perpendicular to the paper plane as the y direction, and the direction perpendicular to the horizontal plane as the z direction. The end coordinate system of the first robotic arm 1 is based on the end of the first robotic arm 1 as the origin, the direction parallel to the paper plane along the upper edge of the horizontal plane as the x direction, the direction perpendicular to the paper plane as the y direction, and the direction perpendicular to the horizontal plane as the z direction.

[0034] As shown in FIG3(a), the first robotic arm 11 and the camera 31 are schematic diagrams of the new positions of the first robotic arm 1 and the camera 3 after the first robotic arm 1 is moved parallel to the X-axis, preferably after 1 second of parallel movement. Before the first robotic arm 1 is moved parallel to the X-axis, the coordinate value of the first feature point 4 in the camera coordinate system collected by the camera 3 is xc1, and the coordinate value of the end of the first robotic arm 1 in the base coordinate system of the first robotic arm 1 is obtained by reading the first robotic arm 1, which is p1=(x1, y1, z1), and the attitude angle value is rxyz1. After the first robotic arm 1 is moved parallel to the X-axis, the coordinate value of the first feature point 4 in the camera coordinate system collected by the camera 3 is xc2, and the coordinate value of the end of the first robotic arm 1 in the base coordinate system of the first robotic arm 1 is obtained by reading the first robotic arm 1, which is p2=(x2, y2, z2), and the attitude angle value is rxyz2.

[0035] As shown in FIG3(b), the first robot arm 12 and the camera 32 are schematic diagrams of the new positions of the first robot arm 1 and the camera 3 after the first robot arm 1 is moved parallel to the Y axis, preferably after 1 second of parallel movement. Before the first robot arm 1 is moved parallel to the Y axis, the coordinate value of the first feature point 4 in the camera coordinate system is collected by the camera 3 as xc3, and the coordinate value of the end of the first robot arm 1 in the base coordinate system of the first robot arm 1 is obtained by reading the first robot arm 1, which is p3 = (x3, y3, z3), and the attitude angle value is rxyz3. After the first robot arm 1 is moved parallel to the Y axis, the coordinate value of the first feature point 4 in the camera coordinate system is collected by the camera 3 as xc4, and the coordinate value of the end of the first robot arm 1 in the base coordinate system of the first robot arm 1 is obtained by reading the first robot arm 1, which is p4 = (x4, y4, z4), and the attitude angle value is rxyz4.

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

[0037] First, the posture angle transformation determinant of the tip posture with respect to the base coordinate system of the first robot arm 1 is as follows.

[0038] (rx1,ry1,rz1)→R e1 ,(rx2,ry2,rz2)→R e2

[0039] (rx3,ry3,rz3)→R e3 ,(rx4,ry4,rz4)→R e4

[0040] Next, the coordinate calculation formula of the first feature point 4 in the base coordinate system of the first robot arm 1 before and after the X-axis parallel movement and the Y-axis movement is as follows.

[0041] X obj =R e1 ×(R×xc1+T)+p1 (1)

[0042] X obj =R e2 ×(R×xc2+T)+p2 (2)

[0043] X obj =R e3 ×(R×xc3+T)+p3 (3)

[0044] X obj =R e4 ×(R×xc4+T)+p4 (4)

[0045] Before and after the first robot arm 1 moves in parallel, the position of the first feature point 4 in the base coordinate system of the first robot arm 1 remains fixed, so equation (1) is equal to equation (2), equation (3) is equal to equation (4), and the equation is constructed.

[0046] R e1 ×(R×xc1+T)+p1=R e2 ×(R×xc2+T)+p2(1)=(2)

[0047] R e3 ×(R×xc3+T)+p3=R e4 ×(R×xc4+T)+p4(3)=(4)

[0048] Solve the above equations. First, due to the translation process, R e1 ×T,R e2 ×T,R e3 ×T and R e4 ×T, we can eliminate the terms and simplify it into the following formula.

[0049] R×(xc1-xc2)=R e1 -1 ×(p2-p1) (5)

[0050] R×(xc3-xc4)=R e3 -1 ×(p4-p3) (6)

[0051] Next, construct the vector relationship.

[0052] v1=xc1-xc2、v2=R e1 -1×(p2-p1), v3=xc3-xc4, v4=R e3 -1 ×(p4-p3)

[0053] R×v1=v2

[0054] R×v3=v4

[0055] Use the rotation transformation method to find R.

[0056] eta1=nomal(v1), eta2=nomal(v1×v3), eta3=eta1×eta2

[0057] eta4=nomal(v2), eta5=nomal(v2×v4), eta6=eta4×eta5

[0058] A=[η1, η2, η3], B=[η4, η5, η6]

[0059] R=B -1 ×A

[0060] As shown in FIG4(a), the first mechanical arm 13 and the camera 33 are schematic diagrams of the new positions of the first mechanical arm 1 and the camera 3 after the first mechanical arm 1 is rotated along the X-axis, preferably after 1 second of rotation. Before the first mechanical arm 1 is rotated along the X-axis, the coordinate value of the first feature point 4 in the camera coordinate system collected by the camera 3 is xc1, and the coordinate value of the end of the first mechanical arm 1 in the base coordinate system of the first mechanical arm 1 is obtained by reading the first mechanical arm 1, which is p1=(x1, y1, z1), and the attitude angle value is rxyz1. After the first mechanical arm 1 is rotated along the X-axis, the coordinate value of the first feature point 4 in the camera coordinate system collected by the camera 3 is xc2, and the coordinate value of the end of the first mechanical arm 1 in the base coordinate system of the first mechanical arm 1 is obtained by reading the first mechanical arm 1, which is p2=(x2, y2, z2), and the attitude angle value is rxyz2.

[0061] As shown in FIG4(b), the first robot arm 14 and the camera 34 are schematic diagrams of the new positions of the first robot arm 1 and the camera 3 after the first robot arm 1 is rotated along the Z axis, preferably after 1 second of rotation. Before the first robot arm 1 is rotated along the Z axis, the coordinate value of the first feature point 4 in the camera coordinate system is collected by the camera 3 as xc3, and the coordinate value of the end of the first robot arm 1 in the base coordinate system of the first robot arm 1 is obtained by reading the first robot arm 1, which is p3=(x3, y3, z3), and the attitude angle value is rxyz3. After the first robot arm 1 is rotated along the Z axis, the coordinate value of the first feature point 4 in the camera coordinate system is collected by the camera 3 as xc4, and the coordinate value of the end of the first robot arm 1 in the base coordinate system of the first robot arm 1 is obtained by reading the first robot arm 1, which is p4=(x4, y4, z4), and the attitude angle value is rxyz4.

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

[0063] First, a posture angle transformation determinant of the end posture of the first robot arm 1 with respect to the base coordinate system is created.

[0064] (rx1,ry1,rz1)→R e1 ,(rx2,ry2,rz2)→R e2

[0065] (rx3,ry3,rz3)→R e3 ,(rx4,ry4,rz4)→R e4

[0066] Next, calculation formulas for the position coordinates of 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-axis and the Z-axis are prepared respectively.

[0067] X obj =R e1 ×(R×xc1+T)+p1 (1)

[0068] X obj =R e2 ×(R×xc2+T)+p2 (2)

[0069] X obj =R e3 ×(R×xc3+T)+p3 (3)

[0070] X obj =R e4×(R×xc4+T)+p4 (4)

[0071] Before and after the first robot arm 1 rotates along the X-axis and the Z-axis, the position of the first feature point 4 in the base coordinate system of the first robot arm 1 remains fixed. Therefore, equation (1) is equal to equation (2), and equation (3) is equal to equation (4). The constructed equations are as follows.

[0072] R e1 ×(R×xc1+T)+p1=R e2 ×(R×xc2+T)+p2(1)=(2)

[0073] R e3 ×(R×xc3+T)+p3=R e4 ×(R×xc4+T)+p4(3)=(4)

[0074] Finally, align T to the left side of the equation and set the X-axis equation and Z-axis rotation equation as the calculation formula for T.

[0075] R e1 ×(R×xc1+T)+p1=R e2 ×(R×xc2+T)+p2(5)

[0076] (R e1 -R e2 )×T=(R e2 ×R×xc2+p2)-(R e1 ×R×xc1+p1) (6)

[0077] R e3 ×(R×xc3+T)+p3=R e4 ×(R×xc4+T)+p4(7)

[0078] (R e3 -R e4 )×T=(R e4 ×R×xc4+p4)-(R e3 ×R×xc3+p3) (8)

[0079] Construct A×T=B and solve the position formula as follows.

[0080] A=ER x1 -1 ·R x2

[0081] B=R e1 -1 ((R e2 ×R×xc2+p2)-(R e1 ×R×xc1+p1))

[0082] T=A -1 ·B

[0083] 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 obtained by the camera relative to the camera 3, the position of the camera 3 relative to the end of the first robot arm 1 obtained in the camera position calculation step S21, the position of the end of the first robot arm 1 relative to the base 7 of the first robot arm 1, and the position of the end of the second robot arm 2 relative to the base 8 of the second robot arm 2.

[0084] Preferably, the position of the second robotic arm 2 relative to the first robotic arm 1 can be calculated based on the positions of two groups of second feature points 5 obtained by the camera 3 by moving the second robotic arm 2 relative to the camera 3, the position of the camera 3 relative to the end of the first robotic arm 1 obtained in the camera position calculation step S21, the position of the end of the first robotic arm 1 relative to the base of the first robotic arm 1, and two groups of positions of the end of the second robotic arm 2 relative to the base 8 of the second robotic arm 2 corresponding to the positions of the two groups of second feature points 5 relative to the camera 3.

[0085] The position of the second feature point 5 relative to the camera 3 refers to the position of the second feature point 5 in the camera coordinate system. The position of the end of the second robotic arm 2 relative to the base 8 of the second robotic arm 2 refers to the position and attitude angle value of the end of the second robotic arm 2 in the base coordinate system of the second robotic arm 2. The position of the second robotic arm 2 relative to the first robotic arm 1 refers to the position and attitude angle value of the base 8 of the second robotic arm 2 in the base coordinate system of the first robotic arm 1.

[0086] As shown in Figure 5(a), Figure 5(b), Figure 6 The base coordinate system of the second robot arm 2 shown has the center of the base 8 of the second robot arm 2 as the origin, the direction along the horizontal plane parallel to the paper surface as the x direction, the direction perpendicular to the paper surface as the y direction, and the direction perpendicular to the horizontal plane as the z direction.

[0087] As shown in Fig. 5(a), 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 is moved parallel to the X axis, preferably after 1 second of parallel movement. Before and after the second robot arm 2 is moved parallel to the X axis, the coordinate values ​​of the second feature point 5 in the camera coordinate system collected by the camera 3 are xc1 and xc2; the terminal posture determinant of the first robot arm 1 is obtained by reading the readings of the first robot arm and the second robot arm, which is R e1 , R e2, the coordinate values ​​are p1 and p2, the coordinate values ​​of the end of the second robot arm 2 in the base coordinate system of the second robot arm 2 are up1 and up2, and the posture determinant is UR e1 =(0,0,Rz1),UR e2 =(0,0,Rz2).

[0088] As shown in Fig. 5(b), 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 is moved parallel to the Y axis, preferably after 1 second of parallel movement. Before and after the second robot arm 2 is moved parallel to the Y axis, the coordinate values ​​of the second feature point 5 in the camera coordinate system collected by the camera 3 are xc3 and xc4; the terminal posture determinant of the first robot arm 1 is obtained by reading the readings of the first robot arm and the second robot arm, which is R e3 , R e4 , the coordinate values ​​are p3 and p4, the coordinate values ​​of the end of the second robot arm 2 in the base coordinate system of the second robot arm 2 are up3 and up4, and the posture determinant is UR e1 =(0,0,Rz3),UR e2 =(0,0,Rz4).

[0089] The equations before and after the second robot arm 2 moves are constructed as follows. Wherein, R is the calculation result in step S21, that is, the rotation matrix corresponding to the posture angle of the camera 3 in the end coordinate system of the first robot arm 1, T is the calculation result in step S21, that is, the column vector of the offset position value of the camera 3 in the end coordinate system of the first robot arm 1, R u is the rotation matrix of the posture of the base coordinate system of the second robot 2 in the base coordinate system of the first robot 1, T u is the offset position column vector of the origin of the second robot 2 in the base coordinate system of the first robot 1, xb1 is the coordinate of the second feature point 5 in the base coordinate system of the first robot 1 before the second robot 2 is moved parallel to the X axis, xb2 is the coordinate of the second feature point 5 in the base coordinate system of the first robot 1 after the second robot 2 is moved parallel to the X axis, xb3 is the coordinate of the second feature point 5 in the base coordinate system of the first robot 1 before the second robot 2 is moved parallel to the Y axis, and xb4 is the coordinate of the second feature point 5 in the base coordinate system of the first robot 1 after the second robot 2 is moved parallel to the Y axis.

[0090] Then the calculation formulas for xb1, xb2, xb3, and xb4 are as follows.

[0091] xb1=R e1 ×(R×xc1+T)+p1 (11)

[0092] xb2=R e2×(R×xc2+T)+p2 (12)

[0093] xb3=R e3 ×(R×xc3+T)+p3 (13)

[0094] xb4=R e4 ×(R×xc4+T)+p4 (14)

[0095] First, according to the above equations (11), (12), (13), and (14), the position calculation formula of the second feature point 5 in the coordinate system of the end of the second robot arm 2 is as follows.

[0096] Obj end =UR e1 -1 ×(R u ×xb1+T u -up1) (1)

[0097] Obj end =UR e2 -1 ×(R u ×xb2+T u -up2) (2)

[0098] UR e1 =UR e2 UR e3 =UR e4

[0099] Obj end =UR e3 -1 ×(R u ×xb3+T u -up3) (3)

[0100] Obj end =UR e4 -1 ×(R u ×xb4+T u -up4) (4)

[0101] Next, before and after the second robot arm 2 moves parallel, the position of the second feature point 5 in the end coordinate system of the second robot arm 2 remains fixed. Therefore, equation (1) is equal to equation (2), equation (3) is equal to equation (4), and the constructed equations are as follows.

[0102] UR e1 -1 ×(R u ×xb1+T u -up1)=URe2 -1 ×(R u ×xb2+T u -up2)(5)

[0103] UR e3 -1 ×(R u ×xb3+T u -up3)=UR e4 -1 ×(R u ×xb4+T u -up4)(6)

[0104] Since the end posture matrix of the second robot arm 2 is fixed during parallel movement, the UR e1 -1 ×T u , UR e2 -1 ×T u , UR e3 -1 ×T u and UR e4 -1 ×T u , we get equations (5) and (6) for the parallel movement of the second robot arm 2 along the X-axis and the Y-axis as follows.

[0105] R u ×(xb1-xb2)=up2-up1 (7)

[0106] R u ×(xb3-xb4)=up4-up3 (8)

[0107] Solve the equation. First, construct the vector relationship as follows.

[0108] v1=xb1-xb2, v2=up2-up1, v3=xb3-xb4, v4=up4-up3

[0109] R u ×v1=v2

[0110] R u ×v3=v4

[0111] Use the rotation transformation method to find R u as follows.

[0112] eta1=nomal(v1), eta2=nomal(v1×v3), eta3=eta1×eta2

[0113] eta4=nomal(v2), eta5=nomal(v2×v4), eta6=eta4×eta5

[0114] A=[η1, η2, η3], B=[η4, η5, η6]

[0115] R u =B -1 ×A

[0116] like Figure 6 As shown, the second mechanical arm 23 and the second characteristic point 53 are schematic diagrams of the new positions of the second mechanical arm 2 and the second characteristic point 5 after the second mechanical arm 2 is rotated along the Z axis, preferably after rotating for 1 second. Before and after the second mechanical arm 2 is rotated along the Z axis, the coordinate values ​​of the second characteristic point 5 in the camera coordinate system collected by the camera 3 are xc1 and xc2; the terminal posture determinant of the first mechanical arm 1 obtained by reading the readings of the first mechanical arm and the second mechanical arm is R e1 , R e2 , the coordinate values ​​are p1 and p2, the coordinate values ​​of the end of the second robot arm 2 in the base coordinate system of the second robot arm 2 are up1 and up2, and the posture determinant is UR e1 =(0,0,Rz1),UR e2 =(0,0,Rz2).

[0117] The equations before and after the second robot 2 rotates along the Z axis are constructed as follows. Wherein, R is the calculation result in step S21, that is, the rotation matrix corresponding to the posture angle of the camera 3 in the end coordinate system of the first robot 1, T is the calculation result in step S21, that is, the column vector of the offset position value of the camera 3 in the end coordinate system of the first robot 1, R u is the rotation matrix of the posture of the base coordinate system of the second robot 2 in the base coordinate system of the first robot 1, T u is the offset position column vector of the origin of the second robot 2 in the base coordinate system of the first robot 1, xb1 is the coordinate of the second feature point 5 in the base coordinate system of the first robot 1 before the second robot 2 is moved parallel to the X axis, xb2 is the coordinate of the second feature point 5 in the base coordinate system of the first robot 1 after the second robot 2 is moved parallel to the X axis, xb3 is the coordinate of the second feature point 5 in the base coordinate system of the first robot 1 before the second robot 2 is moved parallel to the Y axis, xb4 is the coordinate of the second feature point 5 in the base coordinate system of the first robot 1 after the second robot 2 is moved parallel to the Y axis, and E is the replacement unit matrix.

[0118] Then the calculation formulas for xb1, xb2, xb3, and xb4 are as follows.

[0119] xb1=R e1 ×(R×xc1+T)+p1 (11)

[0120] xb2=R e2 ×(R×xc2+T)+p2 (12)

[0121] xb3=R e3 ×(R×xc3+T)+p3 (13)

[0122] xb4=R e4 ×(R×xc4+T)+p4 (14)

[0123] First, according to the above equations (11), (12), (13), and (14), the calculation equation for the second feature point 5 in the base coordinate system of the second robot arm 2 is as follows.

[0124] Obj end =UR e1 -1 ×(R u ×xb1+T u -up1) (1)

[0125] Obj end =UR e2 -1 ×(R u ×xb2+T u -up2) (2)

[0126] Next, before and after the second robot 2 rotates along the Z axis, since the position of the second feature point 5 in the base coordinate system of the second robot 2 is fixed, equation (1) and equation (2) are equal, and the constructed equation is as follows.

[0127] UR e1 -1 ×(R u ×xb1+T u -up1)=UR e2 -1 ×(R u ×xb2+T u -up2)(1)=(2)

[0128] Finally, T U Align to the left side of the equation and set the Z axis rotation equation to T U Calculation formula.

[0129] (E-UR e1 ×UR e2 -1 )×T U =UR e1 ×UR e2 -1 ×((R u×xb2-up2)-(R u ×xb1-up1))

[0130] Construct A×T U =B, the solution position is as follows.

[0131] A=E-UR e1 ×UR e2 -1

[0132] B=UR e1 ×UR e2 -1 ×((R u ×xb2-up2)-(R u ×xb1-up1))

[0133] A×T U =B

[0134] T U =A -1 ·B

[0135] As shown in Figure 5(a), Figure 5(b), Figure 6 The vibrations generated during the operation such as the operation shown will cause the position of the second robot arm 2 in the base coordinate system of the first robot arm 1 to change. In order to ensure the alignment accuracy of the dual robot arms, the change amount needs to be compensated in real time. In the position change compensation step S23, the position of the second robot arm 2 relative to the first robot arm 1 is compensated according to the position change of the first robot arm 1 obtained by the acceleration sensor 6. The acceleration sensor 6 measures the position change value of the first robot arm 1 as p=(x, y, z), and then the coordinate value of the compensated first robot arm 1 is obtained by adding the coordinate reading of the first robot arm 1 to the position change amount of the first robot arm 1 p=(x, y, z). In the case where the first robot arm 1 and the second robot arm 2 are both fixedly installed relative to the shaft 10, the device for detecting the position change, such as the acceleration sensor, can be omitted, and the position change compensation step S23 for compensating for the position change can be omitted.

[0136] According to the dual robotic arm alignment method and dual robotic arm alignment system described above, the coordinate systems between the robotic arms can be calibrated when multiple robotic arms work in collaboration, so that the accuracy of the coordinated construction of multiple robotic arms meets the high-precision construction requirements.

[0137] Although some embodiments of the present invention have been described, these embodiments are given as examples and are not intended to limit the scope of the invention. These embodiments can be implemented in various other ways, and various omissions, substitutions, and changes can be made without departing from the scope of the invention. These embodiments and their variations are included in the invention recorded in the claims and their equivalents, as are those included in the scope and purpose of the invention.

Claims

1. A dual-arm alignment method, characterized in that: The dual robotic arms have: The first robotic arm; The second robotic arm; A camera, which is fixed to the end of the first mechanical arm; a first feature point fixed relative to a base of the first robotic arm; as well as a second feature point, which is fixed on the end of the second robotic arm, The first mechanical arm and / or the second mechanical arm are fixedly installed relative to the construction environment. The dual robotic arm alignment method comprises: a camera position calculation step, which calculates the position of the camera relative to the end of the first robotic arm according to the position of the first feature point relative to the camera obtained by the camera and the position of the end of the first robotic arm relative to the base of the first robotic arm; as well as A second robotic arm position calculation step, which calculates the position of the second robotic arm relative to the first robotic 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 end of the first robotic arm obtained in the camera position calculation step, the position of the end of the first robotic arm relative to the base of the first robotic arm, and the position of the end of the second robotic arm relative to the base of the second robotic arm.

2. The dual robot arm alignment method according to claim 1, characterized in that: The first mechanical arm can move relative to the construction environment, and the second mechanical arm is fixedly installed relative to the construction environment. The dual robotic arm also has a position change detection device, which is fixed on the base of the first robotic arm. The dual-manipulator alignment method further comprises: A position change compensation step is performed to compensate the position of the second robot arm relative to the first robot arm according to the position change of the first robot arm obtained by the position change detection device.

3. The dual robot arm alignment method according to claim 1, characterized in that: The first mechanical arm is fixedly installed relative to the construction environment, and the second mechanical arm can move relative to the construction environment. The dual robotic arm also has a position change detection device, which is fixed on the base of the second robotic arm. The dual-manipulator alignment method further comprises: A position change compensation step is performed to compensate the position of the second robot arm relative to the first robot arm according to the position change of the second robot arm obtained by the position change detection device.

4. The dual robot arm alignment method according to claim 1, characterized in that: In the camera position calculation step, the position of the camera relative to the end of the first robotic arm is calculated based on the positions of two groups of the first feature points acquired by the camera by moving the first robotic arm relative to the camera, and the positions of two groups of the ends of the first robotic arm corresponding to the positions of the two groups of the first feature points relative to the camera relative to the base of the first robotic arm.

5. The dual robot arm alignment method according to claim 4, characterized in that: In the camera position calculation step, The position of the first feature point relative to the camera refers to the position of the first feature point in the camera coordinate system. The position of the end of the first mechanical arm relative to the base of the first mechanical arm refers to the position and attitude angle value of the end of the first mechanical arm in the base coordinate system of the first mechanical arm. The position of the camera relative to the end of the first robotic arm refers to the position and attitude angle value of the camera in the end coordinate system of the first robotic arm.

6. The dual robot arm alignment method according to claim 5, characterized in that: In the step of calculating the position of the second robotic arm, the position of the second robotic arm relative to the first robotic arm is calculated based on the positions of two groups of the second feature points acquired by the camera by moving the second robotic arm relative to the camera, the position of the camera relative to the end of the first robotic arm obtained in the camera position calculation step, the position of the end of the first robotic arm relative to the base of the first robotic arm, and two groups of positions of the end of the second robotic arm relative to the base of the second robotic arm corresponding to the positions of the two groups of the second feature points relative to the camera.

7. The dual robot arm alignment method according to claim 6, characterized in that: In the second robot arm position calculation step, The position of the second feature point relative to the camera refers to the position of the second feature point in the camera coordinate system. The position of the end of the second robotic arm relative to the base of the second robotic arm refers to the position and attitude angle value of the end of the second robotic arm in the base coordinate system of the second robotic arm. The position of the second robotic arm relative to the first robotic arm refers to the position and attitude angle value of the base of the second robotic arm in the base coordinate system of the first robotic arm.

8. A dual-arm alignment system, characterized in that: The dual-manipulator alignment system has: The first robotic arm; The second robotic arm; A camera, which is fixed to the end of the first mechanical arm; a first feature point fixed relative to a base of the first robotic arm; as well as a second feature point, which is fixed on the end of the second robotic arm, The first mechanical arm and / or the second mechanical arm are fixedly installed relative to the construction environment. The dual-manipulator alignment system also has a control unit, which has: a camera position calculation unit, which calculates the position of the camera relative to the end of the first robotic arm according to the position of the first feature point relative to the camera obtained by the camera and the position of the end of the first robotic arm relative to the base of the first robotic arm; as well as a second robotic arm position calculation unit, which calculates the position of the second robotic arm relative to the first robotic 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 end of the first robotic arm obtained in the camera position calculation unit, the position of the end of the first robotic arm relative to the base of the first robotic arm, and the position of the end of the second robotic arm relative to the base of the second robotic arm.

9. The dual-manipulator alignment system according to claim 8, characterized in that: The first mechanical arm can move relative to the construction environment, and the second mechanical arm is fixedly installed relative to the construction environment. The dual robotic arm also has a position change detection device, which is fixed on the base of the first robotic arm. The control unit also has: A position variation compensating unit compensates for a position of the second robot arm relative to the first robot arm according to the position variation of the first robot arm obtained by the position variation detecting device.

10. The dual-manipulator alignment system according to claim 8, characterized in that: The first mechanical arm is fixedly installed relative to the construction environment, and the second mechanical arm can move relative to the construction environment. The dual robotic arm also has a position change detection device, which is fixed on the base of the second robotic arm. The control unit also has: A position variation compensating unit compensates for the position of the second robot arm relative to the first robot arm according to the position variation of the second robot arm obtained by the position variation detecting device.

Images