Robot motion control method based on real-time perception of multi-ranging sensors and application

Abstract

The application discloses a kind of robot motion control method and application based on real-time sensing of multiple ranging sensors, for the mechanical arm control of industrial robot, by controlling two-stage mechanical arm coordinated action to run to workpiece approximate measurement point, according to the real-time sensing posture and distance of workpiece of front ranging sensor, real-time control arm makes that front end execution head gradually approaches workpiece, reaches predetermined distance and starts suction cup to adsorb workpiece, arm switches to free mode, subsequently carries out measurement.The application realizes the real-time posture adjustment and distance control of mechanical arm execution head and measured workpiece by using the continuous point position control function of mechanical arm, combined with the front end distance sensing of execution head, so that the automatic coating quality detection of industrial detection robot on production line under the condition of complex large workpiece hanging swing state becomes possible, mechanical arm cascade control, system installation is simple and reliable, reduces the space occupation of motion module.

Description

Technical Field

[0001] This invention relates to a robot motion control method and application based on real-time sensing by multiple ranging sensors, belonging to the field of automatic control technology for robotic arms in industrial robots. Background Technology

[0002] The coating production line is a workshop specifically for spraying workpieces. Currently, spraying robots suitable for automated operations are quite common. However, post-coating quality inspection is mainly done manually. The spraying workshop environment is poor, the labor intensity is high, and the inspection data is difficult to update and upload in real time, which is not conducive to the factory's data-driven production management.

[0003] Because large and complex workpieces are mainly coated using a suspended spraying method, and these suspended workpieces are not stationary, they will sway during movement on the spraying production line. While spraying robots can automate the coating process to some extent even with slight positional shifts in the suspended workpieces, they do not need to contact them. However, for coating quality inspection (coating thickness, gloss, color difference, orange peel, etc.), current mature testing instruments require close contact with the surface of the object being tested. If an automated method is used to inspect moving suspended workpieces on the production line, the complex surface of the workpiece, its swaying position, and inherent positional deviations make it impossible for existing robotic arms to achieve contact with the workpiece by moving to fixed points. The irregular swaying of the workpiece prevents the use of traditional methods like vision to guide the robotic arm at a single point or to accurately bring the arm's end close to the surface being inspected by modifying the arm's position at a single point. Furthermore, since the workpiece's positional change occurs throughout the entire space, it is also impossible to achieve non-destructive approach to the workpiece surface by installing a unidirectional floating joint at the end of the robotic arm. Summary of the Invention

[0004] The technical problem solved by this invention is: for existing inspection robots that are difficult to adapt to swaying suspended workpieces, this invention provides a robot motion control method and application based on real-time sensing by multiple ranging sensors.

[0005] This invention is achieved using the following technical solution:

[0006] This invention discloses a robot motion control method based on real-time sensing by multiple ranging sensors, comprising the following steps:

[0007] S1. By calibrating the position coordinates of the ranging sensor on the robot's end-effector plane and the corresponding measurement point coordinates of the ranging sensor on the workpiece's measured surface, a first coordinate system where the robot's end-effector plane is located and a second coordinate system where the workpiece's measured surface is located are constructed respectively, and the expression relationship of the second coordinate system relative to the first coordinate system is obtained.

[0008] S2. In the first coordinate system, obtain the difference matrix between the current workpiece posture and the workpiece required locking posture. Based on the interpolation matrix and the expression relationship between the second coordinate system and the first coordinate system, convert to obtain the target rotation angle for the robot end effector plane to maintain parallel locking with the measured surface of the workpiece.

[0009] S3. Based on the distance measured by the distance sensor from the workpiece surface to the robot end effector plane, and the locking distance between the robot end effector plane and the workpiece surface, obtain the target position coordinates of the robot end effector plane moving to approach the workpiece.

[0010] S4. Based on the real-time posture of the workpiece swaying, adjust the target rotation angle and target position coordinates of the robot's end effector plane and control the robot's end effector unit to move closer to the workpiece.

[0011] Specifically, in the robot motion control method based on real-time perception using multiple ranging sensors described above, in step S1, at least three sets of ranging sensors are provided on the robot's end effector plane. A first coordinate system is constructed using the robot's end effector plane, and the position coordinates of the ranging sensors in the first coordinate system are respectively calibrated as Q1(x1,y1,z). 1初 Q2(x2,y2,z) 2初 ), ..., Q n (x n ,y n ,z n初 ), where n is the number of ranging sensors. Input the initial point coordinates collected by the ranging sensors into the host computer and establish real-time communication with the host computer.

[0012] The distances d1, d2, ..., d1, d2, ..., d3, d4, d5, d6, d7, d8, d9, d1, d2, ... ... n The coordinates of the measurement point on the workpiece's surface in the first coordinate system are obtained as P1(x1,y1,d1-z). 1初 P2(x2,y2,d2-z) 2初 ), ..., P n (x n ,y n ,d n -z n初 Based on the coordinates of any three points, fit a plane ax+by+cz+d=0, and construct a second coordinate system with this fitting plane. The x-axis and y-axis of the second coordinate system are the projections of the x-axis and y-axis of the first coordinate system onto the fitting plane of the workpiece's measured surface, respectively, and the z-axis is the normal of the fitting plane.

[0013] Furthermore, the ranging sensor is a laser displacement sensor, and the laser sensing emission direction of the laser displacement sensor is set perpendicular to the execution plane of the robot end effector.

[0014] Specifically, in the robot motion control method based on real-time perception using multiple ranging sensors described above, in step S2, the current workpiece posture rotation matrix R of the workpiece in the first coordinate system is obtained. 工件 Based on the parallel locking posture between the robot end effector and the wobbling workpiece, the cross product of the vectors of any two measurement points in the fitting plane ax+by+cz+d=0 of the measured surface of the workpiece is used to obtain the locking rotation matrix R of the workpiece relative to the required locking posture of the first coordinate system. 锁定 Calculate the difference matrix ΔR between the current workpiece posture and the required locked posture of the workpiece, where ΔR = R 锁定 -R 工件 ;

[0015] Using the parameters of the fitted plane on the measured surface of the workpiece as a reference, the expression p of the current robot end effector plane posture change is obtained. 旋转 [a 当前 ,b 当前 ,c 当前 ], a 当前 ,b 当前 ,c 当前 The parameter values ​​a, b, and c of the fitted plane ax + by + cz + d = 0 on the measured surface of the workpiece are transformed into rotation matrices for the X, Y, and Z axes of the second coordinate system using the following formula.

[0016]

[0017]

[0018]

[0019] α, β, and γ are the rotation angles of the second coordinate system relative to the X, Y, and Z axes of the first coordinate system, respectively. R x (α), R y (β), R z (γ) are the rotation matrices of the target position coordinates adjusted according to the rotation angle for the X, Y, and Z axes of the second coordinate system, respectively. The rotation matrix R from the second coordinate system to the world coordinate system is then constructed. 旋转 for:

[0020]

[0021] Calculate the target rotation matrix R of the robot's end effector plane. 目标 R 目标 =R 旋转 ×ΔR,

[0022] Through the target rotation matrix R 目标 Reverse computation of the target rotation angle p of the robot's end effector plane 角度目标 [a 目标 b 目标 c 目标 ].

[0023] Specifically, in the robot motion control method based on real-time perception of multiple ranging sensors described above in this invention, in step S3, the locking distance d of the workpiece relative to the first coordinate system to achieve the required locked posture is set. 锁定 The distance d from the robot's end effector plane to the fitted plane of the workpiece's measured surface is measured using a distance sensor. 原点-工件 Obtain the interpolated distance Δd between the locked distance and the current distance, where Δd = d 锁定 -d 原点-工件 ;

[0024] Obtain the expression p of the current position change of the robot's end effector plane. 位置 [x 当前 ,y 当前 ,z 当前 The interpolated distance is converted into the target distance d for the robot end effector to track the workpiece. 目标 d 目标 =p 位置 +Δd, to obtain the target position coordinate expression p of the robot's end effector plane. 位置目标 [x 目标 y 目标 , z 目标 ].

[0025] Specifically, in the robot motion control method based on real-time perception using multiple ranging sensors described above in this invention, in step S4, the target position coordinates p of the robot's end effector plane are expressed as... 位置目标 [x 目标 y 目标 , z 目标 [p] is expressed as the rotation angle of the target. 角度目标 [a 目标 b 目标 c 目标 By combining these elements, the complete target point P on the final robot end effector plane is obtained. 目标 [x 目标 y 目标 , z 目标 a 目标 b 目标 c 目标 The robot's end effector moves to the target point, approaching the surface of the workpiece being measured. During the approach, the robot's end effector adjusts the target point position as the workpiece sways to follow the workpiece.

[0026] As a preferred embodiment, in the robot motion control method based on real-time perception of multiple ranging sensors described above, the target point of the robot end effector plane is adjusted according to the workpiece swaying. When the position deviation is large, the response speed of the robot end effector unit is accelerated, and when the position deviation is small, the response speed of the robot end effector unit is slowed down.

[0027] As another preferred embodiment, in the robot motion control method based on real-time sensing by multiple ranging sensors described above, the robot end effector plane approaches the workpiece at a constant speed V. When controlling the robot end effector unit to move and approach the measured surface of the workpiece, the time interval t between the start time and the current control point is recorded. 时间 Calculate the target distance d from the current control point on the robot's end effector plane. 目标t d 目标t =V*t 时间 , use d 目标t Replace d 目标 Obtain the target position coordinates of the robot's end effector plane.

[0028] This invention also discloses the application of the above-mentioned robot motion control method in a robotic arm, wherein the robotic arm is a detection robot. The detection robot uses the above-mentioned robot motion control method of this invention to control the robotic arm execution flange to move and approach the workpiece. The plane of the robotic arm execution flange is the end effector plane of the robot. The robotic arm execution flange is provided with a pneumatic suction cup that is adsorbed and fixed to the surface of the workpiece to be measured. When the robotic arm execution flange moves to the target position coordinates that approach the surface of the workpiece to be measured, it is adsorbed and fixed to the surface of the workpiece to be measured by the pneumatic suction cup.

[0029] Furthermore, in the robotic arm described above in this invention, the robotic arm is the robotic arm of a spraying quality inspection robot, and the robotic arm's execution flange is equipped with at least one spraying inspection instrument selected from film thickness gauge, color difference meter, gloss meter, IQ meter, and orange peel meter. The above instruments are mounted on the robotic arm's execution flange through an elastic structure.

[0030] The present invention adopts the above-mentioned technical solution and utilizes the continuous point-to-point control function of the robotic arm, combined with the front-end distance sensing of the actuator head, to realize real-time posture adjustment and distance control between the robotic arm actuator head and the workpiece being tested. This makes it possible to automate coating quality inspection on the production line under the condition of complex large workpieces being suspended and swaying. The robotic arm system is simple and reliable to install and reduces the space occupied by the motion module.

[0031] This invention achieves automated inspection of coating quality using a robotic arm. The current material number is obtained via RFID or PLC communication. The material number indicates whether measurement is required. If measurement is required, the corresponding measurement program is retrieved. The host computer controls the two-stage robotic arm to coordinate its movements to the approximate measurement point of the workpiece according to the measurement program. The front-end distance sensor detects the posture and distance of the workpiece in real time and controls the robotic arm to gradually approach the workpiece. Once the predetermined distance is reached, the suction cup is activated to adsorb the workpiece, and the robotic arm switches to free mode for subsequent inspection.

[0032] In summary, the robot motion control method based on real-time perception by multiple ranging sensors provided by this invention enables the robotic arm to automatically detect swaying suspended workpieces, improves the automation rate of assembly line production, and makes it possible to automatically inspect the coating quality of complex and large workpieces under swaying conditions on the production line. It is applicable to the automatic production inspection of the surface of various processed workpieces.

[0033] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. Attached Figure Description

[0034] Figure 1 This is a schematic diagram of the overall structure of the two-stage cascaded robotic arm of the spraying quality inspection robot in the embodiment.

[0035] Figure 2 , 3 This is a schematic diagram of the structure on both sides of the end effector head of the robotic arm of the spraying quality inspection robot in the embodiment.

[0036] Figure 4 This is a flowchart of the robot motion control method based on real-time sensing by multiple ranging sensors according to the present invention.

[0037] The numbers in the diagram are: 100-supporting robotic arm, 200-following robotic arm, 300-end effector of robotic arm, 301-end effector flange of robotic arm, 302-range sensor, 303-pneumatic suction cup, 304-IQ tester, 305-color difference meter, 306-film thickness gauge. Detailed Implementation

[0038] Example

[0039] See Figure 1The robotic arm shown in the figure is a specific embodiment of the present invention. This robotic arm is a robotic arm for a coating quality inspection robot, used for inspecting the coating quality of large suspended workpieces. The robotic arm shown is a two-stage cascaded robotic arm, including a supporting robotic arm 100, a follower robotic arm 200, and a robotic arm end effector 300. The supporting robotic arm 100 is fixedly connected to the robotic arm base and includes two main arms with at least three joint degrees of freedom, used for rapid movement of the entire robotic arm between a safe position and a designated inspection position. The follower robotic arm 200 is cascaded with the supporting robotic arm 100 and has at least four joint degrees of freedom, used for the approach movement of the robotic arm end effector to the suspended workpiece and for follower state control. The robotic arm end effector 300 is located at the end of the follower robotic arm 200 and, after adhering to the inspection surface of the suspended workpiece, inspects the coating quality of the workpiece surface.

[0040] The robotic arm in this embodiment is a spray coating quality inspection robotic arm. The end effector 300 of the robotic arm is provided with an end effector flange 301, such as... Figure 2 and Figure 3 As shown, a pair of pneumatic suction cups 303 are provided on the flange 301 of the end effector head of the robotic arm. The pneumatic suction cups 303 are used to fix the end effector head of the robotic arm to the surface of the suspended workpiece for spraying and inspection by negative pressure adsorption. Pneumatic suction cups are a mature robotic arm accessory, and the air circuit and control system of the pneumatic suction cups will not be described in detail in this embodiment.

[0041] In addition, the end effector flange 301 of the robotic arm also integrates an IQ meter 304, a colorimeter 305, and a film thickness gauge 306. These instruments, after being adsorbed and fixed to the workpiece surface by the end effector, respectively measure the surface finish, color difference, and film thickness of the workpiece coating. These instruments are mounted on the end effector flange via a damping elastic structure, giving them a certain degree of deformation and recovery capability to meet the requirements of inspecting uneven workpiece surfaces. In practical applications, other coating quality testing instruments, such as an orange peel analyzer, can be added according to testing needs.

[0042] During the automated conveying process on the production line, the suspended workpiece will swing. To enable the end effector of the robotic arm to quickly and accurately approach the surface of the swinging workpiece at the spraying and inspection station, this embodiment also integrates a distance sensor 302 on the end effector 300 of the robotic arm to monitor the real-time distance to the workpiece surface during the approach process. Figure 2 As shown, the ranging sensor 302 installed on the robotic arm in this embodiment includes four sets of laser displacement sensors arranged at the four corners of the end effector flange of the robotic arm.

[0043] The following provides a detailed description of the control method for the end effector of the robotic arm to approach the surface of the suspended workpiece for detection.

[0044] like Figure 4 As shown, the motion control method for a spraying quality inspection robot based on real-time sensing by multiple ranging sensors in this embodiment includes the following steps:

[0045] S1. Taking the mounting plane of the flange 301 of the end effector of the robotic arm as the end effector plane of the spraying quality inspection robot, the end effector of the robotic arm is the end effector unit of the spraying quality inspection robot. By calibrating the position coordinates of the distance sensor on the end effector plane of the robot and the corresponding measurement point coordinates of the distance sensor on the surface of the workpiece being measured, the first coordinate system where the end effector plane of the robot is located and the second coordinate system where the surface of the workpiece being measured are located are respectively constructed, and the expression relationship of the second coordinate system relative to the first coordinate system is obtained.

[0046] Specifically, the spraying quality inspection robot approaches the workpiece inspection surface through its end effector plane. A first coordinate system is constructed using the end effector plane, which is the coordinate plane containing the x and y axes of the first coordinate system. The position coordinates of four sets of ranging sensors positioned on the end effector plane within the first coordinate system are respectively calibrated as Q1(x1, y1, z). 1初 Q2(x2,y2,z) 2初 Q3(x3,y3,z) 3初 Q4(x4,y4,z) 4初 The initial coordinates of the ranging sensor are input into the host computer on the production line, and real-time communication is established with the host computer. The communication cycle is required to be less than 2ms, and the distance information measured by the current ranging sensor is obtained in real time.

[0047] Generally, setting three sets of distance measuring sensors that are not on the same straight line is sufficient to satisfy the condition of a uniquely determined robot end-effector plane. In practical applications, considering that there may be obstacles between the workpiece and the robot end-effector plane, the results obtained by the three sets of distance measuring sensors may be incorrect. Therefore, in this embodiment, four sets of distance measuring sensors are arranged at the four corners of the end-effector flange of the robotic arm, which can play an obstacle avoidance role and solve the problem of calculation errors caused by obstacles.

[0048] The distance between corresponding points on the workpiece surface is detected in real time by a distance measuring sensor. The coordinates of the corresponding detection points on the workpiece surface in the first coordinate system are obtained as P1(x1,y1,z1), P2(x2,y2,z2), P3(x3,y3,z3), and P4(x4,y4,z4). Based on the coordinates of any three points, a plane ax+by+cz+d=0 is fitted. A second coordinate system is constructed using this fitted plane. The x-axis and y-axis of the second coordinate system are the projections of the x-axis and y-axis of the first coordinate system onto the fitted plane on the workpiece surface, respectively. The z-axis is the normal to the fitted plane. The z-axis coordinates are obtained in real time by the distance measuring sensor.

[0049] The construction of the first and second coordinate systems and their interrelationships are as follows:

[0050] Based on the first coordinate system, the installation space position of the ranging sensor relative to the first coordinate system is calibrated. The calibration adopts the four-point calibration method in the coordinate system calibration method of industrial robot manipulator tools. Before calibration, real-time data communication with the ranging sensor needs to be established.

[0051] First, locate a planar region within the robot arm's travel range that is fixed relative to the robot arm's mounting position. Move the robot arm until the distance sensor on the robot's end effector plane can detect the plane. At this point, record the real-time position d of the distance sensor and the position p of the origin of the robot's end effector plane in the world coordinate system. 标1 The points measured by the ranging sensor are marked on the plane. The robot's end effector plane is then randomly moved, gradually shifting the measuring point of the ranging sensor until it aligns with the previous measurement point, while simultaneously ensuring that the distance measured by the ranging sensor remains consistent with d. At this point, the position p of the origin of the robot's end effector plane in the world coordinate system is recorded. 标2 Using the same method, move, rotate, or swing the robot's end effector plane again to create a certain tilt angle or rotation compared to the previous movement. Adjust the robotic arm again to ensure the distance measured by the ranging sensor is the same as before, thus obtaining P. 标3 and p 标4 .

[0052] According to p 标1 p 标2 p 标3 p 标4 By using the tool coordinate calculation method of the robotic arm, the coordinates of the ranging sensor in the first coordinate system, Q1(x1, y1, z1), can be calculated. Using the same calibration method, the coordinate positions of the other three ranging sensors in the first coordinate system can be obtained: Q2(x2, y2, z2), Q3(x3, y3, z3), and Q4(x4, y4, z4). Note that the z-axis information of these positions is useless at this point; only the xy-plane information is needed.

[0053] For the Z-axis position calibration method of the ranging sensor installation, prepare a flat plate and attach it to the robot's end effector plane, at which point the flat plate is flush with the robot's end effector plane. Record the measurement position d of each ranging sensor. 1初 d 2初 d 3初 d n初 The initial z-axis value of each ranging sensor's position coordinate in the first coordinate system is used as the initial z-axis value. 1初 z 2初 z 3初 z 4初 .

[0054] The ranging sensor is a laser displacement sensor, and the laser emission direction of the laser displacement sensor is perpendicular to the execution plane of the robot's end effector. These calibration values ​​are closely related to the installation of the ranging sensor; if the installation of the ranging sensor changes, the corresponding position needs to be recalibrated.

[0055] After calibrating the position coordinates of the ranging sensor on the robot's end effector plane, the distances d1, d2, d3, and d4 measured by the ranging sensor to the corresponding measurement points on the workpiece's surface are then used for further calibration. n The coordinates of the measurement point on the workpiece's surface in the first coordinate system are obtained as P1(x1,y1,d1-z). 1初 P2(x2,y2,d2-z) 2初 P3(x3,y3,d3-z) 3初 P4(x4,y4,d4-z) 4初 Based on the coordinates of any three points, fit a plane ax+by+cz+d=0, and construct a second coordinate system with this fitting plane. The x-axis and y-axis of the second coordinate system are the projections of the x-axis and y-axis of the first coordinate system onto the fitting plane of the workpiece's measured surface, respectively, and the z-axis is the normal of the fitting plane.

[0056] First, control the end effector of the robotic arm to move in normal point-to-point motion mode to a position in front of the swaying workpiece. Set a point as the advance teaching point. Just make sure that the swaying workpiece will not collide with the robotic arm. During the process of the robot's end effector performing planar motion to the advance teaching point, the distance sensor has already detected the obstacle. When it detects that the obstacle in front is below the warning distance, it will sound an alarm and stop the robotic arm to prevent a collision.

[0057] After the robot's end effector moves to the pre-taught point in front of the workpiece, it activates the workpiece attitude sensing via multi-point ranging sensors and switches the robot arm's motion mode to continuous position adjustment mode. It acquires the distance information d1, d2, d3, and d4 measured by each ranging sensor. Since the initial distance has been calibrated and the mounting position information of the ranging sensors relative to the robot's end effector plane is known, the actual distance is d1-z. 1初 d2-z 2初 d3-z 3初 d4-z 4初 Since the laser sensing emission direction of the ranging sensors is perpendicular to the execution plane of the robot's end effector, it is equivalent to being able to determine the coordinates of the four ranging sensors on the workpiece surface relative to the points in the first coordinate system as P1(x1,y1,d1-z). 1初 P2(x2,y2,d2-z) 2初 P3(x3,y3,d3-z) 3初P4(x4,y4,d4-z) 4初 Based on the coordinates of any three points, a plane ax + by + cz + d = 0 is fitted. A second coordinate system is constructed using this fitted plane. The x-axis and y-axis of the second coordinate system are the projections of the x-axis and y-axis of the first coordinate system onto the fitted plane on the workpiece's measured surface, respectively. The z-axis is the normal to the fitted plane. The expression vectors of each axis of the second coordinate system relative to the robotic arm flange coordinate system can be calculated. This constructs the expression matrix of the workpiece coordinate system relative to the robotic arm flange coordinate system, thus realizing the perception of the workpiece relative to the first coordinate system.

[0058] S2. In the first coordinate system, obtain the difference matrix between the current workpiece posture and the workpiece required locking posture. Based on the interpolation matrix and the expression relationship between the second coordinate system and the first coordinate system, convert to obtain the target rotation angle for the robot end effector plane to maintain parallel locking with the measured surface of the workpiece.

[0059] Specifically, first, obtain the current workpiece orientation rotation matrix R of the workpiece in the first coordinate system. 工件 When the workpiece wobbles and its posture changes, the distance between the measurement points on the workpiece's surface detected by the ranging sensors on the robot's end effector plane changes. The current workpiece posture rotation matrix R can be obtained by converting these real-time distance changes between the measurement points on the workpiece's surface using the ranging sensors. 工件 Assuming the robot's end effector plane and the fitted plane of the wobbling workpiece's measured surface are parallel and locked together, in this posture, all ranging sensors detect equal distances between measurement points on the workpiece's measured surface. The cross product of the vectors of any two measurement points in the fitted plane ax+by+cz+d=0 on the workpiece's measured surface yields the locking rotation matrix R of the workpiece relative to the required locked posture in the first coordinate system. 锁定 Calculate the difference matrix ΔR between the current workpiece posture and the required locked posture of the workpiece, where ΔR = R 锁定 -R 工件 .

[0060] Then, obtain the position of the current robot end effector plane, and represent it as P[x] using the rx rotation angle notation. 当前 ,y 当前 ,z 当前 ,a 当前 ,b 当前 ,c 当前 The point is divided into two parts for processing, p 位置 [x 当前 ,y 当前 ,z 当前 ] and p 旋转 [a 当前 ,b 当前 ,c 当前The former focuses on the spatial position changes of the robot's end effector plane, while the latter focuses on the orientation changes of the robot's end effector plane.

[0061] In this step, the parameters of the fitted plane of the workpiece's measured surface are used as a reference to obtain the expression p of the current robot end effector's plane posture change. 旋转 [a 当前 ,b 当前 ,c 当前 ], a 当前 ,b 当前 ,c 当前 To convert the parameter values ​​a, b, and c of the fitting plane ax + by + cz + d = 0 for the real-time measurement points on the workpiece surface detected by the ranging sensor into rotation matrices for the X, Y, and Z axes of the second coordinate system using the following formula,

[0062]

[0063]

[0064]

[0065] α, β, and γ are the rotation angles of the second coordinate system relative to the X, Y, and Z axes of the first coordinate system, respectively. R x (α), R y (β), R z (γ) are the rotation matrices of the target position coordinates adjusted according to the rotation angle for the X, Y, and Z axes of the second coordinate system, respectively. The rotation matrix R from the second coordinate system to the world coordinate system is then constructed. 旋转 for:

[0066]

[0067] Calculate the target rotation matrix R of the robot's end effector plane. 目标 R 目标 =R 旋转 ×ΔR,

[0068] Through the target rotation matrix R 目标 Reverse computation of the target rotation angle p of the robot's end effector plane 角度目标 [a 目标 b 目标 c 目标 By inverting the above rotation matrix, the rotation angle of the robot's end effector plane XYZ axis adjusted to the target posture can be obtained.

[0069] S3. Based on the distance measured by the distance sensor from the workpiece surface to the robot end effector plane, and the locking distance between the robot end effector plane and the workpiece surface, the target position coordinates of the robot end effector plane moving to approach the workpiece are obtained.

[0070] Specifically, firstly, the locking distance d of the workpiece relative to the first coordinate system is set to the required locking posture. 锁定 The locking distance d here 锁定 This refers to the distance from the center of the robot's end effector plane to the workpiece surface after the robotic arm's actuator head approaches the workpiece's measured surface. It is calculated using a distance sensor to measure and calculate the current distance d from the origin of the first coordinate system constructed with the robot's end effector plane to the fitted plane of the workpiece's measured surface. 原点-工件 Since the planar representation of the workpiece's measured surface relative to the first coordinate system, ax + by + cz + d = 0, has already been fitted and calculated, the distance d from the origin of the current robot end effector plane center to that plane can be calculated. 原点-工件 Obtain the interpolated distance Δd between the locked distance and the current distance, where Δd = d. 锁定 -d 原点-工件 .

[0071] Obtain the expression p of the current position change of the robot's end effector plane. 位置 [x 当前 ,y 当前 ,z 当前 The interpolated distance is converted into the target distance d for the robotic arm to track the workpiece. 目标 d 目标 =p 位置 +Δd, to obtain the target position coordinate expression p of the robot's end effector plane. 位置目标 [x 目标 y 目标 , z 目标 After controlling the robot's end-effector to be parallel to the workpiece's measured surface fitting plane by controlling the target rotation angle of the robot's end-effector, it is only necessary to perform distance interpolation calculation on the Z-axis coordinate of the origin of the first coordinate system relative to the second coordinate system, and move the locked posture of the robot's end-effector parallel to the workpiece's measured surface fitting plane to approach the target position coordinates of the workpiece.

[0072] S4. Based on the real-time posture of the workpiece swaying, adjust the target rotation angle and target position coordinates of the control robot's end effector plane and control the robotic arm to move the flange closer to the workpiece.

[0073] The target position coordinates of the robot's end effector plane are expressed as p. 位置目标 [x 目标 y 目标 , z 目标 [p] is expressed as the rotation angle of the target.角度目标 [a 目标 b 目标 c 目标 By combining these elements, the complete target point P on the final robot end effector plane is obtained. 目标 [x 目标 y 目标 , z 目标 a 目标 b 目标 c 目标 Then, based on the relationship of the robotic arm, the adjustment target angle of each joint of the robotic arm is calculated in reverse. The robotic arm is controlled to execute the flange movement to the target position and approach the surface of the workpiece to be measured. During the approach process, the robotic arm adjusts the target position with the workpiece to follow the workpiece.

[0074] Finally, the robotic arm is controlled to move to the target position. Since the suspended workpiece is constantly swaying, the target position calculated above is constantly changing and adjusting. In order to speed up the response speed of the robotic arm to move to the target position, a PID algorithm is used to control the robotic arm to follow the workpiece adjustment in real time. When the swaying of the workpiece causes a large deviation in the target position, the response of the robotic arm is automatically accelerated, and the response is slowed down when the deviation is small.

[0075] Another control adjustment requires the robot's end effector plane to approach the workpiece at a constant speed V. Introducing speed V necessitates considering the time variable. In this case, the target distance requirement of the robotic arm's actuator head changes linearly with time. During control, speed V is converted into the corresponding target distance at different time points, and the time interval t between the start time and the current control point is recorded. 时间 Calculate the target distance d from the current control point on the robot's end effector plane. 目标t , there is d 目标t =V*t 时间 When controlling, d is used. 目标t Replace d 目标 The target position coordinates of the robot's end effector plane are calculated, and the subsequent control method is similar to that described above, using PID control to adapt to the swaying of the suspended workpiece.

[0076] The specific inspection process for spray quality inspection using the spray inspection robotic arm in this embodiment is as follows:

[0077] Step 1: After spraying the paint, inspect the workpieces hanging in the cooling chamber. When the workpieces on the production line arrive at the inspection station, the robotic arm inspection system communicates with the production line control system. It obtains the workpiece number information to be inspected through RFID or communication means from the production line system. The robotic arm inspection system determines whether inspection is required based on the workpiece number and simultaneously searches the system for a corresponding inspection motion program.

[0078] Step 2: If no inspection is required, directly notify the production line control system to end the measurement. The suspended workpiece will then move to the next workpiece area with the production line, and no further measurement is needed. If inspection of the workpiece is required, retrieve the corresponding workpiece's inspection motion program. Simultaneously, control the robotic arm to move in front of the measurement point of the workpiece to be inspected. The 3D camera on the end effector of the robotic arm will scan the pre-inspection plane of the workpiece to confirm that the inspection platform can detect it. If the 3D camera scan confirms that the workpiece's inspection plane cannot be inspected, control the robotic arm to return to the safe position, and the suspended workpiece will stop moving to the next workpiece area with the production line.

[0079] Step 3: After confirming that the workpiece detection plane is detectable, activate the workpiece posture and distance sensors on the robotic arm. Control the robotic arm end effector to adjust its posture in real time. Using the robotic arm approach method described above, control the flange of the robotic arm end effector to approach the workpiece surface being measured until the robotic arm end effector and the workpiece surface reach the set approach distance. Then, activate the robotic arm's free mode. In this free mode, the robotic arm is controlled by the robotic arm control system to provide only a weak driving force, which can only maintain the posture of the robotic arm. In free mode, the robotic arm's posture can be changed by external forces. This allows the robotic arm end effector to adapt to minor positional changes caused by the suspended workpiece. Simultaneously with the activation of free mode, activate the electric suction cup on the robotic arm end effector to bring it into contact with the workpiece surface being measured, allowing the robotic arm end effector to swing along with the suspended workpiece. The approach distance between the robotic arm end effector flange and the workpiece surface being measured must ensure that the suction force generated by the electric suction cup can firmly adhere and fix the robotic arm end effector to the workpiece surface being measured.

[0080] Step 4: After bonding, the end effector of the robotic arm moves with the workpiece, and the paint quality inspection instrument on the end effector of the robotic arm is activated to inspect the surface of the workpiece.

[0081] Step 5: After the inspection is completed, turn off the electric suction cup in the free mode of the robotic arm, disengage the end effector of the robotic arm from the workpiece, and switch the robotic arm back to normal mode. In normal mode, the robotic arm quickly controls the robotic arm to retract a safe distance through the motors of each joint and then runs a fixed process program to make the robotic arm return to the safe position.

[0082] Step 6: Upload the detection data to the upper-level MOM system.

[0083] In this document, the terms "upper," "lower," "front," "back," "left," "right," "top," "bottom," "inner," "outer," "vertical," and "horizontal," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only used for the clarity of expressing the technical solution and for the convenience of description, and therefore should not be construed as limiting the present invention.

[0084] In this document, the terms “comprising,” “including,” or any other variations thereof are intended to cover non-exclusive inclusion, which includes not only the elements listed but also other elements not expressly listed.

[0085] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A robot motion control method based on real-time sensing by multiple ranging sensors, used for the robot end effector to move and approach a workpiece, characterized in that... Includes the following steps: S1. By calibrating the position coordinates of the ranging sensor on the robot's end-effector plane and the corresponding measurement point coordinates of the ranging sensor on the workpiece's measured surface, a first coordinate system where the robot's end-effector plane is located and a second coordinate system where the workpiece's measured surface is located are constructed respectively, and the expression relationship of the second coordinate system relative to the first coordinate system is obtained. The ranging sensor is a laser displacement sensor. The laser emission direction of the laser displacement sensor is perpendicular to the robot's end effector plane. At least three sets of ranging sensors are installed on the robot's end effector plane. A first coordinate system is constructed with the robot's end effector plane as the reference. The position coordinates of the ranging sensors in the first coordinate system are respectively labeled as Q1(x1,y1,z). 1初 Q2(x2,y2,z) 2初 ), ..., Q n (x) n ,y n ,z n初 (n represents the number of distance measuring sensors). The initial point coordinates collected by the distance measuring sensors are input into the host computer, and real-time communication is established with the host computer. The distances d1, d2, ..., d1, d2, ..., d3, d4, d5, d6, d7, d8, d9, d1, d2, ..., d1, d2 ... n The coordinates of the measurement point on the workpiece surface in the first coordinate system are obtained as P1(x1,y1,d1-z). 1初 P2(x2,y2,d2-z) 2初 ), ..., P n (x) n ,y n ,d n -z n初 Based on the coordinates of any three points, fit the plane ax+by+cz+d=0, and construct a second coordinate system with the fitting plane. The x-axis and y-axis of the second coordinate system are the projections of the x-axis and y-axis of the first coordinate system onto the fitting plane on the measured surface of the workpiece, respectively, and the z-axis is the normal of the fitting plane. S2. In the first coordinate system, obtain the difference matrix between the current workpiece posture and the workpiece required locking posture. Based on the interpolation matrix and the expression relationship between the second coordinate system and the first coordinate system, convert to obtain the target rotation angle for the robot end effector plane to maintain parallel locking with the measured surface of the workpiece. S3. Based on the distance measured by the distance sensor from the workpiece surface to the robot end effector plane, and the locking distance between the robot end effector plane and the workpiece surface, obtain the target position coordinates of the robot end effector plane moving to approach the workpiece. S4. Based on the real-time posture of the workpiece swaying, adjust the target rotation angle and target position coordinates of the robot's end effector plane and control the robot's end effector unit to move closer to the workpiece.

2. The robot motion control method based on real-time sensing by multiple ranging sensors according to claim 1, characterized in that: In step S2, the current workpiece posture rotation matrix R of the workpiece in the first coordinate system is obtained. 工件 Based on the parallel locking posture between the robot end effector and the wobbling workpiece, the cross product of the vectors of any two measurement points in the fitting plane ax+by+cz+d=0 of the measured surface of the workpiece is used to obtain the locking rotation matrix R of the workpiece relative to the required locking posture of the first coordinate system. 锁定 Calculate the difference matrix ΔR between the current workpiece posture and the required locked posture of the workpiece, where ΔR = R 锁定 -R 工件 ; Using the parameters of the fitted plane on the measured surface of the workpiece as a reference, the expression p of the current robot end effector plane posture change is obtained. 旋转 [a 当前 ,b 当前 ,c 当前 ], a 当前 ,b 当前 ,c 当前 The parameter values ​​a, b, and c of the fitted plane ax+by+cz+d=0 on the measured surface of the workpiece are transformed into rotation matrices for the X, Y, and Z axes of the second coordinate system using the following formula. 、 、 ， α, β, and γ are the rotation angles of the second coordinate system relative to the X, Y, and Z axes of the first coordinate system, respectively. R x (α), R y (β), R z (γ) are the rotation matrices of the target position coordinates adjusted according to the rotation angle for the X, Y, and Z axes of the second coordinate system, respectively. The rotation matrix R from the second coordinate system to the world coordinate system is then constructed. 旋转 for: ； Calculate the target rotation matrix R of the robot's end effector plane. 目标 R 目标 = R 旋转 ×ΔR, Through the target rotation matrix R 目标 Reverse computation of the target rotation angle p of the robot's end effector plane 角度目标 [a 目标 b 目标 c 目标 ].

3. The robot motion control method based on real-time sensing by multiple ranging sensors according to claim 2, characterized in that: In step S3, the locking distance d of the workpiece relative to the first coordinate system is set to the required locking posture. 锁定 The distance d from the robot's end effector plane to the fitted plane of the workpiece's measured surface is measured using a distance sensor. 原点-工件 Obtain the interpolated distance Δd between the locked distance and the current distance, Δd = d 锁定 -d 原点-工件 ; Obtain the expression p of the current position change of the robot's end effector plane. 位置 [x 当前 ,y 当前 ,z 当前 The interpolated distance is converted into the target distance d for the robot end effector to track the workpiece. 目标 d 目标 =p 位置 +Δd, to obtain the target position coordinate expression p of the robot's end effector plane. 位置目标 [x 目标 y 目标 , z 目标 ].

4. The robot motion control method based on real-time sensing by multiple ranging sensors according to claim 3, characterized in that: In step S4, the target position coordinates p of the robot's end effector plane are expressed as... 位置目标 [x 目标 y 目标 , z 目标 [p] is expressed as the rotation angle of the target. 角度目标 [a 目标 b 目标 c 目标 By combining these elements, the complete target point P on the final robot end effector plane is obtained. 目标 [x 目标 y 目标 , z 目标 a 目标 b 目标 c 目标 The robot's end effector moves to the target point, approaching the surface of the workpiece being measured. During the approach, the robot's end effector adjusts the target point position as the workpiece sways to follow the workpiece.

5. The robot motion control method based on real-time sensing by multiple ranging sensors according to claim 4, characterized in that: The target position of the robot's end effector plane is adjusted according to the workpiece's shaking. When the position deviation is large, the response speed of the robot's end effector unit is accelerated, and when the position deviation is small, the response speed of the robot's end effector unit is slowed down.

6. The robot motion control method based on real-time sensing by multiple ranging sensors according to claim 4, characterized in that: The robot's end effector plane approaches the workpiece at a constant speed V. When controlling the robot's end effector unit to move and approach the surface of the workpiece being measured, the time interval t between the start time and the current control point is recorded. 时间 Calculate the target distance d from the current control point on the robot's end effector plane. 目标t d 目标t = V*t 时间 , use d 目标t Replace d 目标 Obtain the target position coordinates of the robot's end effector plane.

7. A robotic arm, characterized in that: The robotic arm is the robotic arm of the inspection robot. The inspection robot uses the robot motion control method described in claim 5 or 6 to control the robotic arm execution flange to move and approach the workpiece. The plane of the robotic arm execution flange is the end effector plane of the robot. The robotic arm execution flange is provided with a pneumatic suction cup that is adsorbed and fixed to the surface of the workpiece to be measured. When the robotic arm execution flange of the end effector unit moves to the target position coordinates that are close to the surface of the workpiece to be measured, it is adsorbed and fixed to the surface of the workpiece to be measured by the pneumatic suction cup.

8. A robotic arm according to claim 7, characterized in that, The robotic arm is the robotic arm of the spraying quality inspection robot. The robotic arm's execution flange is equipped with at least one spraying inspection instrument selected from film thickness gauge, color difference meter, gloss meter, IQ meter, and orange peel meter. The above instruments are mounted on the robotic arm's execution flange through an elastic structure.

Images