A multi-source information fusion industrial heavy-load robot-human collaboration safety assembly system and a control method thereof

Abstract

The application discloses a kind of multi-source information fusion's industrial heavy-duty robot man-machine cooperation safety assembly system and control method thereof, by host computer control system control AGV car from standby point movement to the workpiece placement point to be assembled, control robot control cabinet issues motion control instruction to industrial robot, clamping system receives host computer control system instruction, clamps the workpiece to be assembled, AGV car moves to assembly position point according to user instruction;Six-dimensional force sensor, acceleration sensor and vision sensor collect data, host computer control system processes and the result is fed back to robot control cabinet, control industrial robot drives clamping system and workpiece to be assembled motion, until completing assembly operation;Clamping system releases the workpiece to be assembled according to instruction, AGV car leaves assembly position point, returns standby point.The application can solve industrial robot in safe environment man-machine cooperation assembly, meet the assembly demand of high-precision large complex structure component under multiple complex scenes.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of industrial heavy-duty robot assembly, and particularly relates to a multi-source information fusion industrial heavy-duty robot man-machine cooperation safety assembly system and a control method thereof. BACKGROUND

[0002] In modern manufacturing, man-machine cooperation assembly tasks are increasingly complex and frequent, and high requirements are put forward for production efficiency and operation safety. Traditional assembly operation mode often simply relies on manual operation, which is not only low in efficiency, but also workers are exposed to high-intensity and repetitive labor environment for a long time, which is easy to cause fatigue and misoperation, leading to frequent safety accidents,

[0003] With the rapid development of automation technology, industrial robots are gradually introduced into the assembly link to reduce the labor burden and improve the precision and efficiency. However, the early man-machine separation type of automatic assembly strictly separates the robot working area and the human operation space, which limits the full play of the cooperative advantages of the two, and it is difficult to flexibly respond to the complex and variable assembly task requirements, and cannot realize the efficient and smooth cooperation process.

[0004] In recent years, the concept of man-machine cooperation has emerged, which builds a work mode of close cooperation and complementary advantages between humans and robots. However, this deep integration of collaborative scenarios brings new safety challenges. Since humans and robots interact in close proximity in the same space, the impact force generated by the high-speed motion of the robot, the accidental deviation of the motion trajectory, and the unintentional intrusion of the human body into the robot operation path may cause serious collision danger and cause irreversible physical harm to personnel.

[0005] To solve the above problems, the patent with publication number CN114434444A discloses an industrial heavy-duty robot assisted assembly safety space planning method, which establishes models for industrial heavy-duty robots, assembly workpieces and working environments respectively, and then establishes a virtual safety space environment, considering the interference relationship between the three. In addition, a safety space envelope surface and a safety space buffer zone for large load workpieces are established on this basis. This method has universality for automatic programming and man-machine cooperation tasks in the safety space, but the virtual space cannot completely and accurately simulate the real environment, and the real-time dynamic adjustment capability in the assembly process is limited, which is difficult to quickly adapt to the influence of actual working condition instantaneous changes. This affects the degree of human cooperation and safety in the man-machine cooperation process.

[0006] Patent publication number CN116423518A discloses a vision-based collaborative robot safety control technology. This method combines intelligent vision and robotic arm motion control to design and implement an intelligent vision system for collaborative robots. This method can effectively predict obstacles in advance and, to a certain extent, ensure the safety of operators during human-machine collaborative operations. However, visual recognition has its limitations. It is affected by factors such as visual obstruction, inaccurate recognition, and light changes. Detection accuracy and reliability are greatly reduced, and it is prone to misjudgment or missed judgment, which may directly affect the safety of operators.

[0007] In summary, while utilizing virtual safe spaces or single visual sensors for safe human-robot collaborative assembly can, to a certain extent, protect operators, this is insufficient in terms of safety, and technical shortcomings still exist, significantly limiting its application. Therefore, combining innovative control methods to construct a safe human-robot collaborative assembly system for industrial heavy-duty robots that integrates multi-source information is urgently needed to overcome the limitations of existing technologies and achieve the dual goals of efficient and safe human-robot collaborative assembly. Summary of the Invention

[0008] The technical problem to be solved by the present invention is to address the deficiencies of the above-mentioned prior art and to provide an industrial heavy-load robot human-machine collaborative safe assembly system and a control method thereof with multi-source information fusion.

[0009] In order to achieve the above technical objectives, the technical solution adopted by the present invention is:

[0010] A multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system includes an assembly object, a host computer control system, an AGV car, an industrial robot, a robot control cabinet, a multi-sensor system and a clamping system, wherein:

[0011] The assembly objects are divided into workpieces to be assembled and main bodies to be assembled, both of which are affixed with visual recognition target marking points. The main body to be assembled is a fixed part with a known assembly position. The workpiece to be assembled moves with the assembly system, and its posture is adjusted in real time during the assembly process until the assembly task is completed.

[0012] The host computer control system is installed and fixed on the rear half of the upper reference surface of the AGV car;

[0013] The industrial robot and the robot control cabinet are installed on the front half and the rear half of the upper reference part of the AGV vehicle;

[0014] The multi-sensor system includes a six-dimensional force sensor, an acceleration sensor and a visual sensor, wherein the six-dimensional force sensor is installed at the end of the industrial robot, the clamping system is installed on the six-dimensional force sensor, the acceleration sensor is installed on the clamping system, and the visual sensor is a binocular visual tracker, which is placed near the subject to be assembled, and its camera range covers the subject to be assembled, the industrial robot, and the workpiece to be assembled.

[0015] A control method for a multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system, the control method specifically comprising the following steps:

[0016] S1. The host computer control system controls the AGV to move from the standby point to the workpiece placement point according to user instructions. It then controls the robot control cabinet to issue motion control instructions to the industrial robot, controlling the movement of each joint of the industrial robot and moving the clamping system at the end to the specified position. The clamping system then receives instructions from the host computer control system and clamps the workpiece to be assembled. The AGV then moves to the specified assembly position according to user instructions.

[0017] S2, the six-dimensional force sensor, acceleration sensor, and vision sensor communicate with the host computer control system in real time. The host computer control system processes the data through an algorithm and feeds back the transition correction values ​​of the three translation axes and three rotation axes of the robot relative to the end to the robot control cabinet. The robot control cabinet controls the industrial robot to drive the clamping system and the workpiece to be assembled according to the feedback results until the workpiece to be assembled and the main body to be assembled complete the assembly operation;

[0018] S3. The clamping system releases the workpiece to be assembled according to the instruction of the host control system, and the AGV car leaves the assembly position and returns to the standby point.

[0019] To optimize the above technical solutions, specific measures taken also include:

[0020] The above-mentioned S2 includes the following steps:

[0021] Step S21: The host computer control system establishes transformation matrices of different coordinate systems based on the visual sensor, the marking points on the workpiece to be assembled, and the main body to be assembled, and tracks the postures of the workpiece to be assembled and the main body to be assembled in real time based on the transformation matrix. When the workpiece to be assembled and the main body to be assembled exceed the safety space range, the robot is controlled to stop suddenly;

[0022] Step S22: The host computer control system monitors the actual three-axis acceleration generated by the acceleration sensor. If the actual acceleration exceeds the threshold, it is considered that a large collision has occurred and the robot does not move;

[0023] Step S23, use the data collected by the six-dimensional force sensor and the rotation transformation matrix relative to the world coordinate system to solve the weight of the workpiece to be assembled, the clamping system and the acceleration sensor, and then calculate the actual three-axis force and actual three-axis torque generated by the six-dimensional force sensor, perform admittance control model analysis, time domain change processing and backward differential discretization method processing on it, and obtain the initial correction value of the robot relative to the end. After transition correction and total correction, the transition correction values ​​of the three translation axes and three rotation axes of the robot relative to the end are obtained, and fed back to the robot control cabinet to realize active and flexible control of human-machine collaboration.

[0024] The above step S22 specifically includes:

[0025] Step S221: construct a rotation transformation matrix of the acceleration sensor coordinate system relative to the world coordinate system. The formula is as follows:

[0026]

[0027] Where: is the rotation transformation matrix of the accelerometer coordinate system relative to the world coordinate system; is the rotation transformation matrix of the acceleration sensor coordinate system relative to the industrial robot end connection flange coordinate system, The rotation transformation matrix of the industrial robot end connection flange coordinate system relative to the industrial robot base coordinate system; is the rotation transformation matrix of the industrial robot base coordinate system relative to the world coordinate system;

[0028] Step S222: collect the posture data of at least six points obtained by the acceleration sensor, the posture data including the three-axis acceleration (a xi 、a yi 、a zi ), the coordinates of the industrial robot body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i , Z i 、A i 、B i 、C i );

[0029] Step S223, according to step S221 and step S222, solves the scale factor and zero drift value of each axis of the acceleration sensor in the acceleration sensor coordinate system, including:

[0030] First, calculate the components of gravity acceleration on each axis in the accelerometer coordinate system at point i:

[0031]

[0032] Where: g is the acceleration due to gravity; (gxi 、g yi 、g zi ) are the axis components of the gravitational acceleration in the accelerometer coordinate system at point i; is the coordinate of the industrial robot body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i , Z i 、A i 、B i 、C i ) in (A i 、B i 、C i ) calculated rotation transformation matrix;

[0033] Then, the scale factors and zero drift values ​​of each axis of the accelerometer in the accelerometer coordinate system are solved by the following formula:

[0034]

[0035] Step S224: Calculate the actual three-axis acceleration (a) generated by the acceleration sensor after excluding the gravity acceleration in real time based on the scale factors of each axis and the zero drift value obtained in step S223. x 、a y 、a z ), the formula is as follows:

[0036]

[0037] Where: (k x 、k y 、k z ) is the scale factor of each axis of the acceleration sensor in the acceleration sensor coordinate system; (f x 、f y 、f z ) is the zero drift value of each axis of the acceleration sensor in the acceleration sensor coordinate system;

[0038] Step S225, monitoring the actual three-axis acceleration (a x 、a y 、a z ), if the following equation is satisfied, it is considered a major collision, an alarm is given, and the robot stops moving:

[0039]

[0040] Among them, a xmax 、a ymax 、a zmax They are the three-axis acceleration thresholds respectively.

[0041] The above-mentioned step S23 specifically includes:

[0042] Step S231, constructing a rotation transformation matrix of the six-dimensional force sensor coordinate system relative to the world coordinate system The formula is as follows:

[0043]

[0044] In the formula: is a rotation transformation matrix of the six-dimensional force sensor coordinate system relative to the world coordinate system; is a rotation transformation matrix of the six-dimensional force sensor coordinate system relative to the industrial robot end connecting flange plate coordinate system; is a rotation transformation matrix of the industrial robot end connecting flange plate coordinate system relative to the industrial robot base coordinate system; is a rotation transformation matrix of the industrial robot base coordinate system relative to the world coordinate system;

[0045] Step S232, collecting pose data of at least six points from the six-dimensional force sensor, including three-axis forces (F xi , F yi , F zi ) and three-axis torques (T xi , T yi , T zi ) of the i-th point, and coordinates (X i , Y i , Z i , A i , B i , C i ) of the industrial robot body corresponding to the i-th point in the Cartesian coordinate system;

[0046] Step S233, calculating the coordinates of the center of gravity of the workpiece to be assembled, the clamping system, and the acceleration sensor in the six-dimensional force sensor coordinate system, the zero point drift values of the three-axis forces and the three-axis torques of the six-dimensional force sensor sensor according to the data collected in step S232, and then solving the weight G of the workpiece to be assembled, the clamping system, and the acceleration sensor to be solved;

[0047] Step S234, according to the rotation transformation matrix obtained in step S231 and the weight G obtained in step S233, combining the coordinates (X, Y, Z, A, B, C) of the industrial robot body in the Cartesian coordinate system obtained in real time, and combining the six-dimensional force sensor data, calculating the actual three-axis forces (F x , F y , F z ) and the actual three-axis torques (T x , T y , T z ) generated by the real-time external force on the components behind the six-dimensional force sensor.

[0048] Step S235, performing admittance control model analysis, time domain variation processing, and backward difference discretization processing on the actual three-axis forces and actual three-axis torques obtained in step S234 to obtain initial correction values ​​of the robot relative to the terminal;

[0049] Step S236: Define the operating range coefficient and correction coefficient of the six-axis force sensor, perform intermediate transition processing on the initial correction values ​​of the three translational and three rotational degrees of freedom of the robot relative to the terminal, and obtain safe relative terminal transition correction values. Further adjustment is performed based on the corresponding closing thresholds to finally obtain transition correction values ​​of the three translational and three rotational degrees of freedom of the robot relative to the terminal.

[0050] Step S237, based on the transition correction values ​​of the robot in the directions of the three translational axes of freedom relative to the terminal obtained in step S236, taking into account the control range of the robot operation, performing a total correction on the transition correction values ​​of the robot in the directions of the three translational axes of freedom relative to the terminal;

[0051] In step S238, according to the transition correction values ​​of the three rotation axis degrees of freedom obtained in step S236 and the total correction values ​​of the three translation axis degrees of freedom obtained in step S237, the upper computer control system sends a posture correction instruction based on the industrial robot tool coordinate system to the robot control cabinet, and continuously corrects the posture of the industrial robot end until the workpiece to be assembled is fitted with the completed surface of the main body to be assembled.

[0052] The above step S233 specifically includes:

[0053] First, calculate the coordinates of the center of gravity of the workpiece to be assembled, the clamping system, and the acceleration sensor in the six-dimensional force sensor coordinate system, and the zero-point drift values ​​of the three-axis force and three-axis torque of the six-dimensional force sensor. The formula is as follows:

[0054]

[0055] Where: (L x , L y , L z ) is the coordinate of the center of gravity of the workpiece to be assembled, the clamping system and the acceleration sensor in the six-dimensional force sensor coordinate system; (F x0 、F y0 、F z0 、T x0 、T y0 、T z0 ) is the zero drift value of the three-axis force and three-axis torque of the six-axis force sensor;

[0056] Then, the weight G of the workpiece to be assembled, the clamping system, and the acceleration sensor is solved as follows:

[0057]

[0058] Where: I 3×3 is a third-order unit matrix; G is the weight of the workpiece to be assembled, the clamping system, and the acceleration sensor; α and β are the roll angle and pitch angle of the base of the industrial robot relative to the world coordinate system, respectively; is the coordinate of the industrial robot body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i , Z i 、A i 、B i 、C i ) in (A i 、B i 、C i ) calculated rotation transformation matrix;

[0059] The above step S234 is based on the rotation transformation matrix obtained in step S231. The weight G obtained in step S233 is combined with the coordinates (X, Y, Z, A, B, C) of the industrial robot body in the Cartesian coordinate system obtained in real time, and then combined with the real-time six-dimensional force sensor data to calculate the actual three-axis force (F) generated by the real-time external force on the component behind the six-dimensional force sensor. x 、F y 、F z ) and actual triaxial moment (T x 、T y 、T z ), specifically including:

[0060] First, calculate the weight of the workpiece to be assembled, the clamping system, and the acceleration sensor in the six-dimensional force sensor coordinate system at any point i:

[0061]

[0062] Where: G x , G y , G z ) are the x-, y-, and z-axis components of the weight of the workpiece to be assembled, the clamping system, and the acceleration sensor in the acceleration sensor coordinate system at any point i; The rotation transformation matrix (A, B, C) calculated for the coordinates (X, Y, Z, A, B, C) of the real-time industrial robot body in the Cartesian coordinate system;

[0063] Then, the actual three-axis force (F) generated by the real-time external force on the components behind the six-axis force sensor is calculated based on the components of each axis. x 、F y 、F z ) and actual triaxial moment (Tx 、T y 、T z ), the formula is as follows:

[0064]

[0065] The above step S235 is specifically as follows:

[0066] (1) The actual three-axis force (F) generated by the real-time external force on the component after the six-axis force sensor x 、F y 、F z ) and actual triaxial moment (T x 、T y 、T z ) to analyze the admittance control model and obtain the corresponding one-dimensional mobile admittance control model, which is as follows:

[0067]

[0068] Where: F d is the actual force, that is, the actual triaxial force (F x 、F y 、F z ) in any one; m d is the virtual mass; b d is the virtual damping; is the expected acceleration; is the expected speed;

[0069] The corresponding one-dimensional rotation admittance control model is obtained as follows:

[0070]

[0071] Where: T d is the actual moment, that is, the actual triaxial force (T x 、T y 、T z ) in any one; ρ d is the virtual moment of inertia; σ d is the virtual rotation damping; is the expected angular acceleration; is the desired angular velocity;

[0072] (2) The same time domain variation processing is performed on the one-dimensional rotation admittance control and the one-dimensional moving admittance control models. The formula for the time domain variation processing of the one-dimensional moving admittance control model is as follows:

[0073]

[0074] Where: h is the time step at a certain moment; is the expected velocity in the time step at time h; is the expected acceleration in the time step at time h; F(h) is the actual force in the time step at time h;

[0075] (3) The model formula after time domain change processing is processed by backward difference discretization method to obtain the initial correction value of the robot relative end. The formula is as follows:

[0076]

[0077] x t (h)=λ0F(h)+λ1x t (h-1)+λ2x t (h-2)

[0078] Where: T is the sampling period; x t (h-1) is the initial correction value sent to the robot relative to the terminal at time h-1; x t (h-2) is the initial correction value sent to the robot relative end at time h-2; x t (h) is the initial correction value sent to the robot relative to the end at time h.

[0079] The above-mentioned step S236 specifically includes:

[0080] (1) Operating range coefficient β of the x-axis of the six-dimensional force sensor Fx The definition is as follows:

[0081]

[0082] Where: F cx is the actual force on the x-axis of the current six-dimensional force sensor, that is, the original data of the six-dimensional force sensor, F xmax is the maximum range of the six-dimensional force sensor in the x-axis direction;

[0083] Define the correction coefficient β' of the x-axis of the six-axis force sensor Fx , the formula is as follows:

[0084]

[0085] Where, F px It is the safety threshold of the six-dimensional force sensor;

[0086] (2) Perform intermediate transition processing on the initial correction value of the safety relative end in the x-axis direction sent to the robot relative end, and further define the safety relative end transition correction value x' in the x-axis direction of the relative end tx (h), the formula is as follows:

[0087] x' tx (h) = β' Fx ×xtx (h)

[0088] Where x tx (h) is the initial correction value of the robot relative to the end x-axis;

[0089] (3) When F cx When the six-axis force sensor is infinitely close to the maximum range, the safety relative end correction value is infinitely close to 0. At this time, it is necessary to adjust x' tx (h) is adjusted according to the following formula:

[0090]

[0091] Where x' tx (h) is the x-axis transition correction value of the robot relative to the end; ξ Fx It is the closing threshold of the force control mode relative to the end x-axis correction value, that is, when x' t (h)<ξ Fx When the force control is turned off, the system will sound an alarm to prevent the six-dimensional force sensor from being damaged by excessive external force.

[0092] The above-mentioned step S237 specifically includes:

[0093] (1) According to the envelope space of each link and the envelope space of each joint of the robot, the maximum operating range of the robot is determined, and then the operating range coefficient set of each joint is obtained. According to the operating range coefficient set of each joint, the operating range correction coefficient set of each joint is obtained, which is as follows:

[0094]

[0095] α' allj =[α' xj α' yj α' zj ] 1×3

[0096] Where, α xj , α yj , α zj are the operating range coefficients of the jth joint in the x, y, and z axes respectively; α′ xj , α′ yj , α′ zj are the correction coefficients of the j-th joint's operating range in the x, y, and z axes respectively; α allj is the coefficient set of the j-th joint;

[0097] Among them, α xj The formula is as follows:

[0098]

[0099] Where xxj is the coordinate of the j-th joint in the x-axis direction in the base coordinate system; α xj is the coefficient of the j-th joint's operating range on the x-axis; [x min ,x max ] is the operating range of the x-axis;

[0100] α′ xj The formula is as follows:

[0101]

[0102] Where x α is the safety threshold of all joints in the x-axis direction in the base coordinate system; α′ xj is the correction coefficient of the j-th joint in the x-axis operating range;

[0103] (2) The working range correction coefficients of each joint are processed to obtain the overall correction coefficients of the x, y, and z axis working ranges acting on the relative end tool coordinate system. The formula is as follows:

[0104]

[0105] Where μ is the overall control coefficient of the operating range; α x , α x , α z They are the overall correction coefficients of the x, y, and z axis action ranges that finally act on the base coordinate system;

[0106] is the rotation transformation matrix of the acceleration sensor coordinate system relative to the industrial robot end connection flange coordinate system; The rotation transformation matrix of the industrial robot end connection flange coordinate system relative to the industrial robot base coordinate system;

[0107] α′ x , α′ y , α′ z are the overall correction coefficients of the x, y, and z axis operating ranges acting on the relative end tool coordinate system;

[0108] (3) Multiply the overall correction coefficient of the x, y, and z axis operating range in the relative end tool coordinate system by the transition correction value of the robot relative end corresponding to the translation axis degree of freedom direction to obtain the total correction value of the robot relative end corresponding to the translation axis degree of freedom direction.

[0109] The present invention has the following beneficial effects:

[0110] 1) This invention proposes a multi-source information fusion industrial heavy-load robot human-robot collaborative safe assembly system. The system incorporates an AGV into the assembly system, and its power supply is integrated into the AGV's power supply system. Compared with traditional industrial robot assembly systems with ground-mounted bases, this significantly expands the spatial application range of assembly.

[0111] 2) The proposed control method for a multi-source information fusion industrial heavy-load robot human-robot collaborative safe assembly system fully considers human-robot safety through the fusion of visual, acceleration, and other information. It comprehensively perceives potential risks in the human-robot collaborative environment, can adapt to complex and changing working environments, and provides solid protection for personal safety and equipment integrity.

[0112] 3) The present invention proposes a control method for a multi-source information fusion industrial heavy-duty robot human-machine collaborative safe assembly system, which adopts an admittance control model and fully considers the range of force sensors and the human-machine safe operating range. In terms of ensuring safety, it greatly improves the overall collaboration efficiency and production efficiency.

[0113] The present invention can solve the problem of human-machine collaborative assembly of industrial robots in a safe environment and meet the needs of high-precision assembly of large and complex structural components in multiple complex scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0114] Figure 1 This is a module diagram of a multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system according to an embodiment of the present invention;

[0115] Figure 2 This is a schematic structural diagram of a multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system according to an embodiment of the present invention;

[0116] Figure 3 This is a schematic diagram of the entire process of a control method for a multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system according to an embodiment of the present invention;

[0117] Figure 4 This is a schematic diagram of the operation flow of a control method for a multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system according to an embodiment of the present invention;

[0118] Description of reference numerals:

[0119] 1—Assembly object; 11—Workpiece to be assembled; 12—Main body to be assembled; 2—Upper computer control system; 3—AGV; 4—Industrial robot; 5—Robot control cabinet; 6—Multi-sensor system; 61—Six-dimensional force sensor; 62—Acceleration sensor; 63—Vision sensor; 7—Clamping system. DETAILED DESCRIPTION

[0120] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0121] Although the steps in the present invention are arranged with numbers, they are not intended to limit the order of the steps. Unless the order of the steps is clearly stated or the execution of a step requires other steps as a basis, the relative order of the steps can be adjusted. It is understood that the term "and / or" used herein refers to and covers any and all possible combinations of one or more of the associated listed items.

[0122] Figure 1 FIG. 1 is a schematic diagram of a module of a multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system according to an embodiment of the present invention. Figure 1 As shown, this embodiment provides an industrial heavy-load robot human-machine collaborative safe assembly system with multi-source information fusion, the system includes an assembly object 1, a host computer control system 2, an AGV car 3, an industrial robot 4, a robot control cabinet 5, a multi-sensor system 6 and a clamping system 7, wherein:

[0123] The assembly object 1 is an object that the assembly system needs to assemble, and is divided into a workpiece 11 to be assembled and a main body 12 to be assembled. Corresponding visual recognition target points are affixed to each of them for visual recognition and tracking. The main body 12 to be assembled is a fixed part with a known assembly position, while the workpiece 11 to be assembled moves with the assembly system, and its posture is adjusted in real time during the assembly process until the assembly task is completed;

[0124] The host computer control system 2 communicates with the AGV trolley 3, the robot control cabinet 5, the multi-sensor system 6 and the clamping system 7. The host computer control system 2 sends instructions to the AGV trolley 3, plans and controls its motion trajectory, controls the clamping and release states of the clamping system 7, performs clamping and release operations on the workpiece 11 to be assembled, receives data collected by the multi-sensor system 6, obtains the posture data of the industrial robot 4 sent by the robot control cabinet 5, performs algorithmic processing on the data, and obtains the motion control instructions that need to be sent to the robot control cabinet 5 to control the movement of the industrial robot 4 and drive the workpiece 11 to be assembled to complete the specified motion to complete the assembly task;

[0125] The AGV trolley 3 can move the vehicle body forward, backward, turn, rotate in place, etc., and is equipped with a hydraulic system to complete the lifting of the vehicle body. It receives instructions from the host computer control system 2 to move the assembly system to the waiting assembly position and return to the standby position after the assembly is completed.

[0126] The industrial robot 4 and the robot control cabinet 5 are a large-range multi-degree-of-freedom industrial robot system. The industrial robot 4 is a standard six-axis industrial robot. The robot control cabinet 5 receives motion control instructions from the host computer control system 2 and controls the corresponding industrial robot 4 to move within the allowed Cartesian space.

[0127] The multi-sensor system 6 is used to obtain the current posture data of the industrial robot 4 and the current posture data and force data of the assembly object 1, and transmit the data to the host control system 2 in real time, including a six-dimensional force sensor 61, an acceleration sensor 62 and a visual sensor 63, wherein;

[0128] The clamping system 7 receives instructions from the host control system 2 and is used to clamp and remove the workpiece 11 to be assembled;

[0129] The multi-sensor system 6 includes a six-dimensional force sensor 61, an acceleration sensor 62 and a visual sensor 63, wherein;

[0130] The six-axis force sensor 61 is installed at the end of the industrial robot 4 and is used to detect the three-axis force and three-axis torque acting on the clamping system 7 and the workpiece 11 to be assembled;

[0131] The acceleration sensor 62 is installed on the clamping system 7 and is used to detect the three-axis acceleration and three-axis angular velocity at a specified position on the clamping system 7;

[0132] The visual sensor 63 is a binocular visual tracker, which is used to track the actual position state of the end of the industrial robot 4 in real time.

[0133] Figure 2 FIG. 1 is a schematic structural diagram of a multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system according to an embodiment of the present invention. Figure 2 As shown, the host computer control system 2 is installed and fixed on the upper reference surface of the AGV trolley 3. Considering the counterweight of the assembly system, the host computer control system 2 is installed and fixed on the rear half of the upper reference surface of the AGV trolley 3;

[0134] The industrial robot 4 and the robot control cabinet 5 are mounted and fixed on the upper reference surface of the AGV trolley 3. Considering the counterweight of the assembly system, the industrial robot 4 is mounted and fixed on the front half of the upper reference surface of the AGV trolley 3, and the robot control cabinet 5 is mounted and fixed on the rear half of the upper reference surface of the AGV trolley 3;

[0135] The six-dimensional force sensor 61 is fixed to the end of the industrial robot 4 through a connecting flange, and the coordinates +x, +y, +z of the six-dimensional force sensor 61 correspond to the flange coordinates +x, +y, +z of the industrial robot 4;

[0136] The clamping system 7 is fixed to the six-dimensional force sensor 61 via a connecting flange, and the clamp is opened and closed by a pneumatic device;

[0137] The acceleration sensor 62 is mounted and fixed on the clamping system 7. The acceleration sensor 62 can be installed at a certain position of the clamping system 7 according to user needs.

[0138] The workpiece 11 to be assembled can be clamped and removed by the pneumatic device on the clamping system 7;

[0139] The main body 12 to be assembled is fixed somewhere in the position to be assembled, and the distance from the predetermined assembly position can be random, but cannot exceed 1m;

[0140] The visual sensor 63 is placed near the main body to be assembled 12, but not too close to the assembly industrial robot 4 and the main body to be assembled 12. The main body to be assembled 12, the industrial robot 4, and the workpiece to be assembled 11 need to be included in the camera range of the visual sensor 63. The specific placement position can be appropriately adjusted according to the instruction file of the visual sensor 63.

[0141] Figure 3 FIG. 1 is a schematic diagram of the entire process of a control method for a multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system according to an embodiment of the present invention. Figure 3 As shown, this embodiment also provides a control method for a multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system. The upper computer control system 2 controls the AGV trolley 3 to move from the standby point to the placement point of the workpiece 11 to be assembled according to the user's instructions, and then controls the robot control cabinet 5 to issue motion control instructions to the industrial robot 4, so that the clamping system 7 fixedly installed at the end of the industrial robot 4 can clamp the workpiece 11 to be assembled. At the same time, the clamping system 7 receives the instructions from the upper computer control system 2 and clamps the workpiece 11 to be assembled. Then the AGV trolley 3 moves according to the user's instructions. Move to the designated assembly position, the six-dimensional force sensor 61, acceleration sensor 62 and visual sensor 63 communicate with the host control system 2 in real time through data cables and network cables. The host control system 2 processes the data through algorithms and feeds back the processed results to the robot control cabinet 5. The robot control cabinet 5 controls the industrial robot 4 to drive the clamping system 7 and the workpiece to be assembled 11 to move according to the feedback results until the workpiece to be assembled 11 and the main body to be assembled 12 complete the assembly operation. The clamping system 7 releases the workpiece to be assembled 11, and the AGV trolley 3 leaves the assembly position and returns to the standby point. The assembly task is completed. This control method specifically includes the following steps:

[0142] S1, the host computer control system 2 controls the AGV trolley 3 to move from the standby point to the placement point of the workpiece 11 to be assembled according to the user's instructions, and then controls the robot control cabinet 5 to issue motion control instructions to the industrial robot 4, controlling the movement of each joint of the industrial robot 4 to move the clamping system 7 at its end to the specified position. After that, the clamping system 7 receives the instructions from the host computer control system 2 and clamps the workpiece 11 to be assembled. Then, the AGV trolley 3 moves to the specified assembly position according to the user's instructions;

[0143] S2, the six-dimensional force sensor 61, the acceleration sensor 62, and the visual sensor 63 communicate with the host computer control system 2 in real time via data cables and network cables. The host computer control system 2 processes the data through an algorithm and feeds back the processed results to the robot control cabinet 5. The robot control cabinet 5 controls the industrial robot 4 to drive the clamping system 7 and the workpiece 11 to be assembled to move according to the feedback results until the workpiece 11 to be assembled and the main body 12 to be assembled complete the assembly operation;

[0144] S3: The clamping system 7 releases the workpiece 11 to be assembled, and the AGV 3 leaves the assembly position and returns to the standby point, completing the assembly task.

[0145] Figure 4 The control method of the multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system according to one embodiment of the present invention is a schematic diagram of the operation flow chart. Figure 4 As shown, to complete S2 in the above embodiment, the following steps are included:

[0146] Step S21: The visual sensor 63 tracks the posture of the workpiece 11 and the main body 12 in real time to ensure that the assembly is within the safe space. If the safe space is exceeded, the robot stops suddenly.

[0147] Step S22: The acceleration sensor 62 performs high-frequency data monitoring on the workpiece 11 to be assembled, and monitors the actual acceleration obtained. If the acceleration exceeds a certain threshold, it is considered that a major collision has occurred, and the robot does not move.

[0148] Step S23: The assembly process adopts a method of human-machine collaborative active compliant control combining the six-dimensional force sensor 61 and the Cartesian space envelope position of the robot.

[0149] The preferred implementation of step S21 is that the host computer control system 2 establishes a transformation matrix of different coordinate systems based on the visual sensor 63, the marking points on the workpiece to be assembled 11 and the main body to be assembled 12, and tracks the posture of the workpiece to be assembled 11 and the main body to be assembled 12 in real time based on the change matrix. When the workpiece to be assembled 11 and the main body to be assembled 12 exceed the safety space range, the robot is controlled to stop suddenly, specifically including:

[0150] Step S211: Track the posture of the workpiece 11 to be assembled in real time, monitor the spatial position of the workpiece 11 to be assembled, and perform safety monitoring. A rigid body model of the workpiece 11 to be assembled is established by attaching four marking points on the workpiece 11 to be assembled. The relative posture relationship between the marking points is fixed. The visual sensor 63 measures the position of each marking point using the triangulation principle described above, and calculates the posture of the rigid body model of the workpiece 11 to be assembled using the four fixed marking points.

[0151] The coordinates of the marking point are as follows:

[0152] J p i =[X i Y i Z i ] T

[0153] Where: J p i is the coordinate of the i-th marking point in the rigid body model coordinate system of the workpiece 11 to be assembled; (X i 、Y i 、Z i ) is the three-dimensional translation coordinate corresponding to the i-th marking point in the rigid body model coordinate system of the workpiece 11 to be assembled;

[0154] The rigid body model of the workpiece 11 to be assembled is established by four marking points. The position and posture relationship of the rigid body model of the workpiece 11 to be assembled relative to the coordinate system of the visual sensor 63 can be calculated using the following formula:

[0155]

[0156] Where: S p i A measurement coordinate system for the vision sensor 63; The transformation matrix of the measurement coordinate system of the visual sensor 63 relative to the coordinate system of the rigid model of the workpiece 11 to be assembled, including two parts of rotation and translation; The rotation transformation matrix of the measurement coordinate system of the visual sensor 63 relative to the coordinate system of the rigid body model of the workpiece 11 to be assembled; The translation transformation matrix of the measurement coordinate system of the visual sensor 63 relative to the coordinate system of the rigid model of the workpiece 11 to be assembled;

[0157] The posture of the rigid model coordinate system of the workpiece 11 to be assembled can be obtained through the parameters of the rotation transformation matrix. The position of the rigid model coordinate system of the workpiece 11 to be assembled can be calculated through the coordinates of the marking points. The visual sensor 63 can calculate the actual posture of the workpiece 11 to be assembled in real time through the marking points during tracking.

[0158] Step S212: According to step S211, referring to the real-time tracking of the pose of the workpiece 11 to be assembled, the pose of the main body 12 to be assembled is calculated in a similar manner, and the formula is as follows:

[0159]

[0160] In the formula: Z p i is the coordinate of the i-th marker point of the main body 12 to be assembled in the rigid model coordinate system; is the transformation matrix of the visual sensor 63 measurement coordinate system relative to the rigid model coordinate system of the main body 12 to be assembled, including rotation and translation;

[0161] Step S213: According to steps S211 and S212, the pose relationship of the rigid model of the main body 12 to be assembled relative to the rigid model of the workpiece 11 to be assembled can be calculated, and the formula is as follows:

[0162]

[0163] In the formula: is the transformation matrix of the rigid model of the main body 12 to be assembled relative to the rigid model coordinate system of the workpiece 11 to be assembled, including rotation and translation; is the rotation transformation matrix of the rigid model of the main body 12 to be assembled relative to the rigid model coordinate system of the workpiece 11 to be assembled; is the translation transformation matrix of the rigid model of the main body 12 to be assembled relative to the rigid model coordinate system of the workpiece 11 to be assembled;

[0164] Step S214: According to step S213, the host computer control system 2 tracks the poses of the workpiece 11 to be assembled and the main body 12 to be assembled in real time, and ensures that the assembly is within the safe space range. When it exceeds the safe space range, the robot stops urgently.

[0165] The preferred embodiment of step S22 is that the host computer control system 2 monitors the actual three-axis acceleration generated by the acceleration sensor 62. If the actual acceleration exceeds the threshold value, it is considered that a large collision has occurred, and the robot does not displace. Specifically, it includes:

[0166] Step S221: A rotation transformation matrix of the acceleration sensor 62 coordinate system relative to the world coordinate system is constructed, and the formula is as follows:

[0167]

[0168] In the formula: is the rotation transformation matrix of the acceleration sensor 62 coordinate system relative to the world coordinate system; is the rotation transformation matrix of the acceleration sensor 62 coordinate system relative to the end connecting flange plate coordinate system of the industrial robot 4, The rotation transformation matrix of the flange coordinate system of the end connection of the industrial robot 4 relative to the base coordinate system of the industrial robot 4; is the rotation transformation matrix of the industrial robot's 4-base coordinate system relative to the world coordinate system;

[0169] Step S222: The host computer control system 2 needs to perform preliminary data preprocessing, calibrate the scale factor and identify the zero drift parameters of the acceleration sensor 62;

[0170] Receive the data of more than six points of the acceleration sensor 62, where the three-axis acceleration (a xi 、a yi 、a zi ), where the coordinates of the industrial robot 4 body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i 、Z i 、A i 、B i 、C i );

[0171] Step S223: Process the data obtained by the acceleration sensor 62 according to step S222 and step S221. The formula is as follows:

[0172]

[0173] Where: g is the acceleration due to gravity; (g xi 、g yi 、g zi ) are the axis components of the acceleration due to gravity at the i-th point in the coordinate system of the acceleration sensor 62; is the coordinate of the industrial robot body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i 、Z i 、A i 、B i 、C i ) in (A i 、B i 、C i ) calculated rotation transformation matrix;

[0174] In order to solve the scale factors and zero drift values ​​of each axis of the acceleration sensor 62 in the acceleration sensor 62 coordinate system, the formula is as follows:

[0175]

[0176] Where: (k x 、k y 、k z) is the scale factor of each axis of the acceleration sensor 62 in the acceleration sensor 62 coordinate system; (f x 、f y 、f z ) is the zero drift value of each axis of the acceleration sensor 62 in the acceleration sensor 62 coordinate system;

[0177] Step S224, based on the parameters calculated in step S221, combined with steps S222 and S23, combined with the coordinates (X, Y, Z, A, B, C) of the industrial robot 4 body in the Cartesian coordinate system obtained in real time, and combined with the data of the acceleration sensor 62, calculate the actual three-axis acceleration (a) generated by the acceleration sensor 62 after excluding the gravity acceleration in real time. x 、a y 、a z ), the formula is as follows:

[0178]

[0179] In step S225, the actual acceleration obtained in step S224 is monitored. If the acceleration exceeds a certain threshold, it is considered that a major collision has occurred and the robot stops suddenly. The formula is as follows:

[0180]

[0181] Among them, a xmax 、a ymax 、a zmax They are the three-axis acceleration thresholds;

[0182] When the threshold is exceeded, an alarm is given and the robot does not move but stops and waits for the operator to cancel the prompt.

[0183] The preferred implementation of step S23 is to use the data collected by the six-dimensional force sensor 61 and the rotation transformation matrix relative to the world coordinate system to solve the weight of the workpiece to be assembled 11, the clamping system 7 and the acceleration sensor 62, and then calculate the actual three-axis force and actual three-axis torque generated by the six-dimensional force sensor 61. The admittance control model analysis, time domain change processing and backward difference discretization method are performed on them to obtain the initial correction value of the robot relative to the end. After transition correction and total correction, the transition correction values ​​of the three translation axes and three rotation axes of the robot relative to the end are obtained, and the transition correction values ​​are fed back to the robot control cabinet 5 to realize active and compliant control of human-machine collaboration. Specifically, it includes:

[0184] Step S231: construct a rotation transformation matrix of the six-dimensional force sensor 61 coordinate system relative to the world coordinate system. The formula is as follows:

[0185]

[0186] Where: is the rotation transformation matrix of the six-dimensional force sensor 61 coordinate system relative to the world coordinate system; is the rotation transformation matrix of the six-dimensional force sensor 61 coordinate system relative to the industrial robot 4 end connection flange coordinate system; The rotation transformation matrix of the flange coordinate system of the end connection of the industrial robot 4 relative to the base coordinate system of the industrial robot 4; is the rotation transformation matrix of the industrial robot's 4-base coordinate system relative to the world coordinate system;

[0187] Step S232: The host computer control system 2 needs to perform preliminary data preprocessing and perform gravity center parameter identification and zero drift parameter identification on the six-dimensional force sensor 61;

[0188] Receive the data of more than six points from the six-dimensional force sensor 61, where the three-axis force (F xi 、F yi 、F zi ) and the three-axis moment is (T xi 、T yi 、T zi ), where the coordinates of the industrial robot 4 body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i 、Z i 、A i 、B i 、C i );

[0189] Step S233: According to step S232, the data obtained by the six-dimensional force sensor 61 is processed. The formula is as follows:

[0190]

[0191] Where: (L x , L y , L z ) are the coordinates of the center of gravity of the workpiece 11 to be assembled, the clamping system 7 and the acceleration sensor 62 in the coordinate system of the six-dimensional force sensor 61; (F x0 、F y0 、F z0 、T x0 、T y0 、T z0 ) is the zero drift value of the six-dimensional force sensor 61 to be determined;

[0192] The weight of the workpiece 11 to be assembled, the clamping system 7 and the acceleration sensor 62 is solved as follows:

[0193]

[0194] Where: I 3×3 is a third-order unit matrix; G is the weight of the workpiece 11 to be assembled, the clamping system 7, and the acceleration sensor 62; α and β are the roll angle and pitch angle of the base of the industrial robot 4 relative to the world coordinate system, respectively; is the coordinate of the industrial robot body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i 、Z i 、A i 、B i 、C i ) in (A i 、B i 、C i ) calculated rotation transformation matrix;

[0195] Step S234, based on the parameters calculated in step S231, combined with steps S232 and S233, combined with the coordinates (X, Y, Z, A, B, C) of the industrial robot 4 body in the Cartesian coordinate system obtained in real time, and combined with the real-time six-dimensional force sensor 61 data, calculate the actual three-axis force (F) generated by the real-time external force on the component behind the six-dimensional force sensor 61. x 、F y 、F z ) and actual triaxial moment (T x 、T y 、T z ), the formula is as follows:

[0196]

[0197] Where: G x , G y , G z ) are the axial components of the weight of the workpiece 11 to be assembled, the clamping system 7 and the acceleration sensor 62 at any point i in the acceleration sensor 62 coordinate system; The rotation transformation matrix (A, B, C) calculated for the coordinates (X, Y, Z, A, B, C) of the real-time industrial robot body in the Cartesian coordinate system;

[0198]

[0199] The components after the above-mentioned six-dimensional force sensor refer to the objects on which the six-dimensional force sensor is subjected to force, and these components are the workpiece to be assembled, the clamping system and the acceleration sensor.

[0200] Step S235, according to step S234, the actual triaxial force, actual triaxial torque obtained, using the mobility controller, for better description, mobility control model is decoupled in each direction, using one-dimensional system mobility control model to express, at the same time considering that the desired pose is the current pose of the robot, therefore the one-dimensional system mobility control model is simplified, and only one-dimensional movement direction is analyzed, one-dimensional movement mobility control model formula is as follows:

[0201]

[0202] In the formula: F d is actual force, that is, the actual force generated by the external force on the component after the six-dimensional force sensor 61, that is, any one of actual triaxial force (F x , F y , F z ); m d is virtual mass; b d is virtual damping; is expected acceleration; is expected speed;

[0203] In order to describe the rotation direction in detail, one-dimensional rotation direction is analyzed, and one-dimensional rotation mobility control model formula is as follows:

[0204]

[0205] In the formula: T d is actual torque, that is, the actual torque generated by the external force on the component after the six-dimensional force sensor 61, that is, any one of actual triaxial torque (T x , T y , T z ); ρ d is virtual rotational inertia; σ d is virtual rotational damping; is expected angular acceleration; is expected angular velocity;

[0206] Considering that the mobility control model analysis of rotation and movement direction is similar, therefore only one-dimensional movement direction is further analyzed as follows:

[0207] The control of the industrial robot 4 is carried out in a fixed time step, therefore the one-dimensional movement mobility control model formula needs to be processed in time domain, and the formula is as follows:

[0208]

[0209] In the formula: h is the time step at a certain time; is the expected speed in the time step at h time; is the expected acceleration in the time step h; F(h) is the actual force in the time step h;

[0210] The formula after time domain change processing is processed by backward difference discrete method, and the formula is as follows:

[0211]

[0212] x t (h) = λ0F(h) + λ1x t (h-1) + λ2x t (h-2)

[0213] In the formula, T is a sampling period; x t (h-1) is the initial correction value of the robot relative to the end at h-1 time; x t (h-2) is the initial correction value of the robot relative to the end at h-2 time; x t (h) is the initial correction value of the robot relative to the end at h time, but considering safety, x t (h) needs to be processed, and the initial first and second time steps x t (h) are all set to 0.

[0214] In step S236, considering that the range of the six-dimensional force sensor is limited, in order to ensure safe assembly, a safety mechanism based on the range of the six-dimensional force sensor is added, and the working range coefficient of the six-dimensional force sensor is defined. Taking the x-axis of the six-dimensional force sensor as an example, the working range coefficient β Fx of the x-axis of the six-dimensional force sensor is defined, and the other axes are analyzed similarly, and the formula is as follows:

[0215]

[0216] In the formula, F cx is the real force in the x-axis direction of the current six-dimensional force sensor, that is, the original data of the six-dimensional force sensor, F xmax is the maximum range of the x-axis of the six-dimensional force sensor.

[0217] In order to prevent the six-dimensional force sensor from being damaged due to excessive external force, the correction coefficient β' Fx of the x-axis of the six-dimensional force sensor is further defined, and the formula is as follows:

[0218]

[0219] In the formula, F px is the safety threshold of the six-dimensional force sensor.

[0220] Thus, the initial correction value of the safety relative end in the x-axis direction sent to the robot relative end is processed in the intermediate transition, and the safety relative end transition correction value x' in the x-axis direction of the relative end is further defined. tx (h), the formula is as follows:

[0221] x' tx (h) = β' Fx ×x tx (h)

[0222] Where x tx (h) is the initial correction value of the robot relative to the end x-axis;

[0223] When F cx When the six-axis force sensor is infinitely close to the maximum range, the safety relative end correction value is infinitely close to 0. At this time, it is necessary to adjust x' tx (h) is adjusted according to the following formula:

[0224]

[0225] Where x' tx (h) is the x-axis transition correction value of the robot relative to the end; ξ Fx It is the closing threshold of the force control mode relative to the end x-axis correction value, that is, when x' t (h)<ξ Fx When the force control is turned off, the system will sound an alarm to prevent the six-dimensional force sensor from being damaged by excessive external force.

[0226] Similarly, the total range coefficient β of the other axes of the six-axis force sensor can be obtained all , total correction coefficient β' all And the force control mode closing threshold ξ all :

[0227]

[0228] Where, β Fx , β Fy , β Fz , β Rx , β Ry β Rz are the operating range coefficients of the six-dimensional force sensor x, y, z, a, b, and c axes; β' Fx ,β' Fy ,β' Fz ,β' Rx ,β' Ry ,β' Rz are the correction coefficients of the six-dimensional force sensor x, y, z, a, b, and c axes respectively; ξ Fx ,ξ Fy ,ξ Fz ,ξRx ,ξ Ry ,ξ Rz They are the closing thresholds of the force control mode relative to the end x, y, z, a, b, and c axis correction values;

[0229] Here, a transition correction is performed on the initial correction values ​​of the directions of the three translation axes and three rotation axes of the robot relative to the end;

[0230] Step S237: Considering the danger of the robot drifting during the human-robot collaboration, further consider controlling the robot's operating range. Determine the robot's maximum operating range based on the envelope space of each link and the envelope space of the joints. Taking the x-axis in the robot's base coordinate system as an example, the x-axis operating range is defined as [x min ,x max ], and define the robot's x-axis operating range coefficient α xj , the formula is as follows:

[0231]

[0232] Where x xj is the coordinate of the j-th joint in the x-axis direction in the base coordinate system; α xj is the operating range coefficient of the j-th joint in the x-axis;

[0233] In order to ensure the flexibility of operation in the safe space and give the operator reaction time when approaching the edge, the correction coefficient α′ is set xj , the formula is as follows:

[0234]

[0235] Where x α is the safety threshold of all joints in the x-axis direction in the base coordinate system; α′ xj is the correction coefficient of the j-th joint in the x-axis operating range;

[0236] Similarly, the operating range coefficient α of other joints can be obtained allj and the operating range correction coefficient α′ allj :

[0237]

[0238] Where, α xj , α yj , α zj are the operating range coefficients of the jth joint in the x, y, and z axes respectively; α′ xj , α′ yj , α′ zj are the correction coefficients of the j-th joint's operating range in the x, y, and z axes respectively; α alljis the coefficient set of the jth joint; a' allj is the correction coefficient set of the jth joint;

[0239] In order to reflect the work range correction coefficient to the robot relative end correction value, the correction coefficient of each joint is processed, and the formula is as follows:

[0240]

[0241] In the formula, μ is the work range overall control coefficient; a x , a x , a z are the overall correction coefficients of the x, y, and z axes of the last action in the base coordinate system, respectively;

[0242] In order to reflect the overall correction coefficient to the end coordinate system, further processing is required, and the formula is as follows:

[0243]

[0244] In the formula, a x , a y , a z are the overall correction coefficients of the x, y, and z axes of the last action in the relative end tool coordinate system, respectively;

[0245] The work range overall correction coefficient is added to the robot relative end x-axis transition correction value x' tx (h), to obtain the final robot relative end x-axis total correction value x" tx (h), and the formula is as follows:

[0246] x" tx (h) = a x x' tx (h)

[0247] Similarly, the total correction values of the three translational axis degrees of freedom directions of the robot relative end (x" tx (h), x" ty (h), and x" tz (h)) are considered, and only the transition correction values of the three translational axis degrees of freedom directions of the robot relative end are corrected, and the transition correction values of the three rotational axis degrees of freedom directions of the robot relative end are not corrected; that is, step S236 corrects the transition correction values of the three translational axis degrees of freedom directions and the three rotational axis degrees of freedom directions, and then step S237 combines the transition correction values of the three rotational axis degrees of freedom directions in step S236 to correct the total correction, and the transition correction values of the three rotational axis degrees of freedom directions are not processed in this step.

[0248] In step S238, the robot end correction is further processed based on the results calculated by the admittance control model and combined with the six-dimensional force sensor 61 and the Cartesian space envelope position of the robot to ensure safe assembly. The upper computer control system 2 sends a posture correction instruction based on the tool coordinate system of the industrial robot 4 to the robot control cabinet 5, and continuously corrects the posture of the end of the industrial robot 4 until the workpiece 11 to be assembled and the main body 12 to be assembled are completed.

[0249] In summary, the present invention proposes a multi-source information fusion industrial heavy-duty robot human-machine collaborative safe assembly system and its control method, which can solve the problem of industrial robots performing human-machine collaborative assembly in a safe environment and meet the needs of high-precision assembly of large and complex structural components in multiple complex scenarios.

[0250] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above and that the invention can be embodied in other specific forms without departing from the spirit or essential characteristics of the invention. Therefore, the embodiments should be considered in all respects as illustrative and non-restrictive, and the scope of the invention is defined by the appended claims, not the foregoing description, and all variations within the meaning and range of equivalents of the claims are intended to be included therein. Any reference sign in a claim should not be construed as limiting the claim to which it relates.

[0251] In addition, it should be understood that although this specification is described in terms of implementation methods, not every implementation method contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

Claims

1. A control method for a multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system, characterized in that: The control method specifically includes the following steps: S1. The host computer control system controls the AGV to move from the standby point to the workpiece placement point according to user instructions. It then controls the robot control cabinet to issue motion control instructions to the industrial robot, controlling the movement of each joint of the industrial robot and moving the clamping system at the end of the robot to the specified position. The clamping system then receives instructions from the host computer control system and clamps the workpiece to be assembled. The AGV then moves to the specified assembly position according to user instructions. S2, the six-dimensional force sensor, the acceleration sensor and the visual sensor communicate real-time data with the host computer control system. The host computer control system processes the data through an algorithm and feeds back the transition correction value results of the three translation axes and three rotation axes of the robot relative to the end to the robot control cabinet. The robot control cabinet controls the industrial robot to drive the clamping system and the workpiece to be assembled to move according to the feedback results until the workpiece to be assembled and the main body to be assembled complete the assembly operation; the six-dimensional force sensor is installed at the end of the industrial robot, the clamping system is installed on the six-dimensional force sensor, and the acceleration sensor is installed on the clamping system; S2 includes the following steps: Step S21: The host computer control system establishes transformation matrices of different coordinate systems based on the visual sensor, the marking points on the workpiece to be assembled, and the main body to be assembled, and tracks the posture of the workpiece to be assembled and the main body to be assembled in real time based on the transformation matrix. When the workpiece to be assembled and the main body to be assembled exceed the safety space range, the robot is controlled to stop suddenly; Step S22: The host computer control system monitors the actual three-axis acceleration generated by the acceleration sensor. If the actual acceleration exceeds the threshold, it is considered that a large collision has occurred and the robot does not move; Step S23: Use the data collected by the six-dimensional force sensor and the rotation transformation matrix relative to the world coordinate system to solve the weight of the workpiece to be assembled, the clamping system, and the acceleration sensor, and then calculate the actual three-axis force and actual three-axis torque generated by the six-dimensional force sensor. Perform admittance control model analysis, time domain change processing, and backward difference discretization method processing on them to obtain the initial correction value of the robot relative to the end. After transition correction and total correction, obtain the transition correction values ​​of the three translation axes and three rotation axes of the robot relative to the end. Feedback to the robot control cabinet to achieve active and compliant control of human-machine collaboration; S3. The clamping system releases the workpiece to be assembled according to the instruction of the host control system, and the AGV car leaves the assembly position and returns to the standby point.

2. The control method according to claim 1, characterized in that: The multi-source information fusion industrial heavy-load robot human-machine collaborative safe assembly system includes an assembly object, a host computer control system, an AGV car, an industrial robot, a robot control cabinet, a multi-sensor system and a clamping system, wherein: The assembly objects are divided into workpieces to be assembled and main bodies to be assembled, both of which are affixed with visual recognition target marking points. The main body to be assembled is a fixed part with a known assembly position. The workpiece to be assembled moves with the assembly system, and its posture is adjusted in real time during the assembly process until the assembly task is completed. The host computer control system is installed and fixed on the rear half of the upper reference surface of the AGV car; The industrial robot and the robot control cabinet are installed on the front half and the rear half of the upper reference part of the AGV vehicle; The multi-sensor system includes a six-dimensional force sensor, an acceleration sensor and a visual sensor, wherein the visual sensor is a binocular visual tracker placed near the subject to be assembled, and its camera range covers the subject to be assembled, the industrial robot, and the workpiece to be assembled.

3. The control method according to claim 1, wherein: Step S22 specifically includes: Step S221: construct a rotation transformation matrix of the acceleration sensor coordinate system relative to the world coordinate system. The formula is as follows: Where: is the rotation transformation matrix of the accelerometer coordinate system relative to the world coordinate system; is the rotation transformation matrix of the acceleration sensor coordinate system relative to the industrial robot end connection flange coordinate system, The rotation transformation matrix of the industrial robot end connection flange coordinate system relative to the industrial robot base coordinate system; is the rotation transformation matrix of the industrial robot base coordinate system relative to the world coordinate system; Step S222: collect the posture data of at least six points obtained by the acceleration sensor, the posture data including the three-axis acceleration (a xi 、a yi 、a zi ), the coordinates of the industrial robot body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i , Z i 、A i 、B i 、C i ); Step S223, according to step S221 and step S222, solves the scale factor and zero drift value of each axis of the acceleration sensor in the acceleration sensor coordinate system, including: First, calculate the components of gravity acceleration on each axis in the accelerometer coordinate system at point i: Where: g is the acceleration due to gravity; (g xi 、g yi 、g zi ) are the axis components of the gravitational acceleration in the accelerometer coordinate system at point i; is the coordinate of the industrial robot body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i , Z i 、A i 、B i 、C i ) in (A i 、B i 、C i ) calculated rotation transformation matrix; Then, the scale factors and zero drift values ​​of each axis of the accelerometer in the accelerometer coordinate system are solved by the following formula: Step S224: Calculate the actual three-axis acceleration (a) generated by the acceleration sensor after excluding the gravity acceleration in real time based on the scale factors of each axis and the zero drift value obtained in step S223. x 、a y 、a z ), the formula is as follows: Where: (k x 、k y 、k z ) is the scale factor of each axis of the acceleration sensor in the acceleration sensor coordinate system; (f x 、f y 、f z ) is the zero drift value of each axis of the acceleration sensor in the acceleration sensor coordinate system; Step S225, monitoring the actual three-axis acceleration (a x 、a y 、a z ), if the following equation is satisfied, it is considered a major collision, an alarm is given, and the robot stops moving: Among them, a xmax 、a ymax 、a zmax They are the three-axis acceleration thresholds respectively.

4. The control method according to claim 1, wherein: Step S23 specifically includes: Step S231: Construct the rotation transformation matrix of the force sensor coordinate system relative to the world coordinate system The formula is as follows: Where: is the rotation transformation matrix of the six-dimensional force sensor coordinate system relative to the world coordinate system; is the rotation transformation matrix of the six-dimensional force sensor coordinate system relative to the industrial robot end connection flange coordinate system; The rotation transformation matrix of the industrial robot end connection flange coordinate system relative to the industrial robot base coordinate system; is the rotation transformation matrix of the industrial robot base coordinate system relative to the world coordinate system; Step S232: collect the posture data of at least six points from the six-dimensional force sensor, including the three-axis force (F xi 、F yi 、F zi ) and the three-axis moment is (T xi 、T yi 、T zi ), the coordinates of the industrial robot body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i , Z i 、A i 、B i 、C i ); Step S233: Calculate the coordinates of the center of gravity of the workpiece to be assembled, the clamping system, and the acceleration sensor in the six-dimensional force sensor coordinate system, and the zero-point drift values ​​of the three-axis force and three-axis torque of the six-dimensional force sensor based on the data collected in step S232, and then solve for the weight G of the workpiece to be assembled, the clamping system, and the acceleration sensor. Step S234, the rotation transformation matrix obtained in step S231 The weight G obtained in step S233 is combined with the coordinates (X, Y, Z, A, B, C) of the industrial robot body in the Cartesian coordinate system obtained in real time, and then combined with the real-time six-dimensional force sensor data to calculate the actual three-axis force (F) generated by the real-time external force on the component behind the six-dimensional force sensor. x 、F y 、F z ) and actual triaxial moment (T x 、T y 、T z ); Step S235, performing admittance control model analysis, time domain variation processing, and backward difference discretization processing on the actual three-axis forces and actual three-axis torques obtained in step S234 to obtain initial correction values ​​of the robot relative to the terminal; Step S236: Define the operating range coefficient and correction coefficient of the six-axis force sensor, perform intermediate transition processing on the initial correction values ​​of the three translational and three rotational degrees of freedom of the robot relative to the terminal, and obtain safe relative terminal transition correction values. Further adjustment is performed based on the corresponding closing thresholds to finally obtain transition correction values ​​of the three translational and three rotational degrees of freedom of the robot relative to the terminal. Step S237, based on the transition correction values ​​of the robot's three translational axis degrees of freedom relative to the terminal obtained in step S236, taking into account the control of the robot's operating range, performing a total correction on the robot's three translational axis degrees of freedom relative to the terminal; In step S238, according to the transition correction values ​​of the three rotation axis degrees of freedom obtained in step S236 and the total correction values ​​of the three translation axis degrees of freedom obtained in step S237, the upper computer control system sends a posture correction instruction based on the industrial robot tool coordinate system to the robot control cabinet, and continuously corrects the posture of the industrial robot end until the workpiece to be assembled is fitted with the completed surface of the main body to be assembled.

5. The control method according to claim 4, characterized in that: Step S233 specifically includes: First, calculate the coordinates of the center of gravity of the workpiece to be assembled, the clamping system, and the acceleration sensor in the six-dimensional force sensor coordinate system, and the zero-point drift values ​​of the three-axis force and three-axis torque of the six-dimensional force sensor. The formula is as follows: Where: (L x 、L y 、L z ) is the coordinate of the center of gravity of the workpiece to be assembled, the clamping system and the acceleration sensor in the six-dimensional force sensor coordinate system; (F x0 、F y0 、F z0 、T x0 、T y0 、T z0 ) is the zero drift value of the three-axis force and three-axis torque of the six-axis force sensor; Then, the weight G of the workpiece to be assembled, the clamping system, and the acceleration sensor is solved as follows: Where: I 3×3 is a third-order unit matrix; G is the weight of the workpiece to be assembled, the clamping system, and the acceleration sensor; α and β are the roll angle and pitch angle of the base of the industrial robot relative to the world coordinate system, respectively; is the coordinate of the industrial robot body corresponding to the i-th point in the Cartesian coordinate system (X i 、Y i , Z i 、A i 、B i 、C i ) in (A i 、B i 、C i ) to calculate the rotation transformation matrix.

6. The control method according to claim 4, characterized in that: Step S234 is based on the rotation transformation matrix obtained in step S231. The weight G obtained in step S233 is combined with the coordinates (X, Y, Z, A, B, C) of the industrial robot body in the Cartesian coordinate system obtained in real time, and then combined with the real-time six-dimensional force sensor data to calculate the actual three-axis force (F) generated by the real-time external force on the component behind the six-dimensional force sensor. x 、F y 、F z ) and actual triaxial moment (T x 、T y 、T z ), specifically including: First, calculate the weight of the workpiece to be assembled, the clamping system, and the acceleration sensor in the six-dimensional force sensor coordinate system at any point i: Where: (G x , G y , G z ) are the x-, y-, and z-axis components of the weight of the workpiece to be assembled, the clamping system, and the acceleration sensor in the real-time six-dimensional force sensor coordinate system; The rotation transformation matrix (A, B, C) calculated for the coordinates (X, Y, Z, A, B, C) of the real-time industrial robot body in the Cartesian coordinate system; Then, the actual three-axis force (F) generated by the real-time external force on the components behind the six-axis force sensor is calculated based on the components of each axis. x 、F y 、F z ) and actual triaxial moment (T x 、T y 、T z ), the formula is as follows:

7. The control method according to claim 4, characterized in that: Step S235 is specifically as follows: (1) The actual three-axis force (F) generated by the real-time external force on the component after the six-axis force sensor x 、F y 、F z ) and actual triaxial moment (T x 、T y 、T z ) to analyze the admittance control model and obtain the corresponding one-dimensional mobile admittance control model, which is as follows: Where: F d is the actual force, that is, the actual triaxial force (F x 、F y 、F z ) in any one; m d is the virtual mass; b d is the virtual damping; is the expected acceleration; is the expected speed; The corresponding one-dimensional rotation admittance control model is obtained as follows: Where: T d is the actual moment, that is, the actual triaxial force (T x 、T y 、T z ) in any one; ρ d is the virtual moment of inertia; σ d is the virtual rotation damping; is the expected angular acceleration; is the desired angular velocity; (2) The same time domain variation processing is performed on the one-dimensional rotation admittance control and one-dimensional moving admittance control models. The time domain variation processing formula of the one-dimensional moving admittance control model formula is as follows: Where: h is the time step at a certain moment; is the expected velocity in the time step at time h; is the expected acceleration in the time step at time h; F(h) is the actual force in the time step at time h; (3) The model formula after time domain change processing is processed by backward difference discretization method to obtain the initial correction value of the robot relative end. The formula is as follows: x t (h)=λ0F(h)+λ1x t (h-1)+λ2x t (h-2) Where: T is the sampling period; x t (h-1) is the initial correction value sent to the robot relative to the terminal at time h-1; x t (h-2) is the initial correction value sent to the robot relative end at time h-2; x t (h) is the initial correction value sent to the robot relative to the end at time h.

8. The control method according to claim 4, characterized in that: Step S236 specifically includes: (1) Operating range coefficient β of the x-axis of the six-dimensional force sensor Fx The definition is as follows: Where: F cx is the actual force on the x-axis of the current six-dimensional force sensor, that is, the original data of the six-dimensional force sensor, F xmax is the maximum range of the six-dimensional force sensor in the x-axis direction; Define the correction coefficient β' of the x-axis of the six-axis force sensor Fx , the formula is as follows: Where, F px It is the safety threshold of the six-dimensional force sensor; (2) Perform intermediate transition processing on the initial correction value of the safety relative end in the x-axis direction sent to the robot relative end, and further define the safety relative end transition correction value x' in the x-axis direction of the relative end tx (h), the formula is as follows: x' tx (h)=β' Fx ×x tx (h) Where x tx (h) is the initial correction value of the robot relative to the end x-axis; (3) When F cx When the six-axis force sensor is infinitely close to the maximum range, the safety relative end correction value is infinitely close to 0. At this time, it is necessary to adjust x' tx (h) is adjusted as follows: Where x' tx (h) is the x-axis transition correction value of the robot relative to the end; ξ Fx It is the closing threshold of the force control mode relative to the end x-axis correction value, that is, when x' t (h)<ξ Fx When the force control is turned off, the system will sound an alarm to prevent the six-dimensional force sensor from being damaged by excessive external force.

9. The control method according to claim 4, characterized in that: Step S237 specifically includes: (1) According to the envelope space of each link and the envelope space of each joint of the robot, the maximum operating range of the robot is determined, and then the operating range coefficient set of each joint is obtained. According to the operating range coefficient set of each joint, the operating range correction coefficient set of each joint is obtained, which is as follows: Where, α xj , α yj , α zj are the operating range coefficients of the jth joint in the x, y, and z axes respectively; α′ xj , α′ yj , α′ zj are the correction coefficients of the j-th joint's operating range in the x, y, and z axes respectively; α allj is the coefficient set of the jth joint; α′ allj The set of correction coefficients for the jth joint; Among them, α xj The formula is as follows: Where x xj is the coordinate of the j-th joint in the x-axis direction in the base coordinate system; α xj is the coefficient of the j-th joint's operating range on the x-axis; [x min ,x max ] is the operating range of the x-axis; α′ xj The formula is as follows: Where x α is the safety threshold of all joints in the x-axis direction in the base coordinate system; α′ xj is the correction coefficient of the j-th joint in the x-axis operating range; (2) The working range correction coefficients of each joint are processed to obtain the overall correction coefficients of the x, y, and z axis working ranges acting on the relative end tool coordinate system. The formula is as follows: Where μ is the overall control coefficient of the operating range; α x , α x , α z They are the overall correction coefficients of the x, y, and z axis action ranges that finally act on the base coordinate system; is the rotation transformation matrix of the acceleration sensor coordinate system relative to the industrial robot end connection flange coordinate system; The rotation transformation matrix of the industrial robot end connection flange coordinate system relative to the industrial robot base coordinate system; α′ x , α′ y , α′ z are the overall correction coefficients of the x, y, and z axis operating ranges acting on the relative end tool coordinate system; (3) Multiply the overall correction coefficient of the x, y, and z axis operating range in the relative end tool coordinate system by the transition correction value of the robot relative end corresponding to the translation axis degree of freedom direction to obtain the total correction value of the robot relative end corresponding to the translation axis degree of freedom direction.

Images