Water cutting cooperative double-mechanical-arm safety protection method based on machine vision

Abstract

The invention discloses a water cutting cooperative double-mechanical-arm safety protection method based on machine vision. The water cutting cooperative double-mechanical-arm safety protection method comprises the following steps that 1, a mechanical arm kinematic model is established; step 2, acquiring a key point coordinate of the mechanical arm; step 3, acquiring a target image, calibrating a camera, and shooting the target image; 4, the image is processed, and a water jet outer contour image is obtained; step 5, acquiring coordinates of a target point P2; 6, the shortest distance between the water jet axis and the slave mechanical arm is calculated; and step 7, contact risk assessment. The method has the beneficial effects that high-precision calculation of the spatial position of the divergent water jet axis point can be realized through a high-precision image acquisition and processing technology of machine vision. The machine vision system can collect and process images in real time, real-time calculation of the spatial position of the divergent water jet axis point is achieved, and the method is suitable for dynamically changing divergent water jet scenes.

Description

Technical Field

[0001] The present invention relates to a dual-robot arm safety protection method, in particular to a water jet cutting collaborative dual-robot arm safety protection method based on machine vision, belonging to the technical field of robot arm collision detection. Background Art

[0002] The waterjet dual-arm collaborative robot is an important equipment for automatic cutting operations. In the process of performing automatic cutting tasks and monitoring the operating status, the real-time collision detection of the dual arms constitutes the core element of its technical architecture. Specifically, when the robot is working, the clamping device at the end of the slave arm is responsible for stabilizing the object being cut, while the end of the main arm is equipped with a water gun to perform precise water cutting operations on the target. It is worth noting that during the water cutting process, the high-speed and divergent water jet will approach the robotic arm. However, the current technical field lacks a mature method to accurately detect the spatial posture of this divergent water jet, thereby avoiding equipment damage caused by it and ensuring the continuity and stability of the water cutting operation.

[0003] Chinese patent CN109773785A discloses an industrial robot anti-collision method, which first refreshes the geometric model of the robot and the workpiece according to the predicted robot motion state, performs intersection detection of the robot and workpiece geometric model, and after detecting that there is no collision risk between the robot and the workpiece, then establishes an AABB bounding box model based on the refreshed geometric model of the robot and the workpiece, performs intersection detection with the simplified workpiece AABB bounding box model, and finally performs intersection detection between the robot, the workpiece, and the internal and external tooling in the intersection area. This method needs to be tested with multiple models. Although it can prevent collisions, the geometric models of the workpiece and the robot are known, and collision detection cannot be performed on changing objects.

[0004] Chinese patent CN103192413A discloses a sensorless robot collision detection device and method, in which a computing module calculates the difference between the robot's motion state at the next moment predicted by an observation module and the robot's actual planned state; a judgment module compares the difference to see if it exceeds a set threshold; when the difference is greater than the set threshold, an execution module drives the robot to stop. The training of the neural network in this method requires a large number of samples and time costs, and is not suitable for industrial robots that need to frequently change tools in flexible manufacturing systems. In addition, this method only stops the robot when a collision occurs, and cannot prevent the occurrence of collisions.

[0005] Chinese patent CN116604557A discloses a collaborative collision avoidance planning optimization method for dual-arm robots in unstructured environments. A depth camera is used to obtain three-dimensional point cloud information of the surrounding environment, and a swept sphere method is used to establish a safety envelope for the collaborative operation of robots in a constrained environment. A computational graphics algorithm is used to obtain the distance between the safety envelopes. A mathematical model and a dynamic planning method are established according to different tasks and goals to obtain the optimal trajectory.

[0006] The existing collision detection methods mainly include offline simulation based on path planning and real-time detection based on torque feedback. Offline simulation requires pre-programming and cannot handle different working conditions. It is difficult to modify the model for real-time working conditions. The real-time detection method based on torque feedback mainly analyzes the abnormal torque fluctuations when the collision occurs to achieve the purpose of collision detection and timely stop loss and stop. This type of method cannot actually avoid the occurrence of collisions, and often causes false alarms due to excessive detection sensitivity.

[0007] In practical applications, water jets often present a non-uniform divergent shape due to environmental factors and physical properties. Traditional machine vision technology mainly focuses on the detection of objects with fixed geometric features. When faced with dynamic and ever-changing divergent water jets, its positioning accuracy is often difficult to guarantee. Summary of the invention

[0008] Purpose of the invention: The purpose of the present invention is to provide a water jet cutting collaborative dual-arm safety protection method based on machine vision. When the collaborative dual-arm robot is working, the clamping device on the end of the slave arm fixes the target, and the main arm uses the water jet at the end of the robot arm to emit a high-pressure water jet to cut the working object. The method of the present invention can achieve real-time spatial positioning of key points on the axis of the divergent water jet released by the main arm water jet through machine vision technology, and then obtain the spatial posture information of its axis. In the collaborative dual-arm operation task, the method of the present invention can prevent the divergent water jet emitted by the main arm water gun from contacting the clamping device of the slave arm, causing damage to the equipment, so as to improve the safety and efficiency of the system operation.

[0009] Technical solution: A water jet cutting collaborative dual-manipulator safety protection method based on machine vision, comprising the following steps:

[0010] Step 1: Establish a kinematic model of the robotic arm, construct a fixed global world coordinate system and a local coordinate system of each link of the robotic arm, and perform kinematic modeling of the collaborative dual robotic arms;

[0011] Step 2: Obtain the key point coordinates of the robot arm and transform the coordinates to associate the local coordinate system of each link of the robot arm established in step 1 with the global world coordinate system, and obtain the key point P on the end effector of the main robot arm in the global world coordinate system. 1The three-dimensional coordinate information of point M, the center point of the end effector of the robot arm;

[0012] Step 3: Acquire the target image, calibrate the camera, and shoot the target image;

[0013] Step 4: Process the image to obtain the outer contour image of the water jet;

[0014] Step 5: Get the target point P 2 Coordinates, extract the target point P on the water jet axis 2 , calculate and determine the target point P 2 The three-dimensional spatial position in the global world coordinate system;

[0015] Step 6: Calculate the shortest distance between the water jet axis and the slave robot arm based on the P obtained in steps 2 and 5. 1 and P 2 Point, calculate the point through P in the global world coordinate system 1 and P 2 The mathematical equation of the straight line L is obtained, and the shortest distance d between the straight line L and the axis of the water jet is calculated;

[0016] Step 7: Contact risk assessment. Set a safety threshold distance r. Compare the shortest distance d with the safety distance threshold r. When the shortest distance d is greater than the safety distance threshold r, return to step 3. When the shortest distance d is less than or equal to the safety distance threshold r, determine that there is a contact risk and take protective measures.

[0017] In the actual working environment, the water jet is often regarded as a regular cylinder under ideal conditions. However, when the robot performs a cutting task, the water jet will show a variety of divergent phenomena, and its axis vector position and length will change dynamically. Faced with such a complex and changeable scene, traditional positioning methods are powerless in capturing and locating the position and direction of the divergent water jet. The present invention combines the principles of kinematics with machine vision algorithms to determine the spatial posture of the divergent water jet, especially for the real-time changing and unpredictable divergent form of the water jet. This technology can quickly and accurately capture its dynamic characteristics. By positioning the divergent water jet in real time, it is possible to determine whether the device is in a dangerous state, thereby avoiding high-speed jet cutting robotic arms and achieving safe and reliable operations.

[0018] Preferably, in order to accurately derive the three-dimensional spatial position coordinates of a specific target point on the dual robotic arm end effector, the method for establishing the robotic arm kinematic model described in step 1 is as follows:

[0019] 1.1. Use DH method to establish the dual robot arm link coordinate system

[0020] a i = Along X iAxis, from Z i Move to Z i+1 The distance

[0021] α i = around X i Axis, from Z i Rotate to Z i+1 Angle,

[0022] d i = Along Z i Axis, from X i-1 Move to X i The distance

[0023] θ i = around Z i Axis, from X i-1 Rotate to X i Angle

[0024] 1.2. Establish the robot arm link coordinate system according to the DH parameters and robot arm size.

[0025] By constructing a fixed global world coordinate system and the local coordinate systems of each link of the robotic arm, the kinematic modeling of the collaborative dual robotic arms is carried out. The Denavit-Hartenberg (DH) parameter method is used for kinematic analysis, and the three-dimensional spatial position coordinates of specific target points on the end effectors of the dual robotic arms are accurately derived.

[0026] Preferably, the method for obtaining the coordinates of the key points of the robot arm in step 2 is as follows:

[0027] 2.1. According to the robot arm link coordinate system established in step 1, the key point P is obtained through coordinate transformation 1 (X 1 ,Y 1 ,Z 1 ) and key point M(X m ,Y m ,Z m ) in the world coordinate system,

[0028] Key Points 1 The specific position is the intersection of the axis of the water gun and the axis of the last arm of the robot arm.

[0029] The position of the key point M is at the center of the spherical bounding box of the clamping device;

[0030] 2.2. Obtain the forward kinematics equation of the robot arm, and obtain the transformation matrix between the connecting rods by matrix multiplication. i T j represents the transformation matrix of coordinate system {j} relative to coordinate system {i}. The overall transformation matrix of the manipulator terminal operation mechanism compared to the base is expressed as:

[0031] 0 T 6 = 0 T 1 1 T 2 2 T 3 3 T 4 4 T 5 5 T 6 (1).

[0033] According to the relationship between the established coordinate systems, the coordinate transformation technology is used to obtain the three-dimensional coordinate information of the key point P1 on the end effector of the main robot arm and the center point M of the end effector of the slave robot arm in the global world coordinate system.

[0034] Preferably, in order to realize the positioning of the camera, the method of calibrating the camera in step 3 is as follows:

[0035] According to the camera pinhole imaging principle, the relationship between the image in the pixel coordinate system, image coordinate system and camera coordinate system is determined. The transformation relationship between the three coordinate systems is described as shown in formula (2):

[0036]

[0037] Assume that p is a point in space, and its coordinates in the world coordinate system are Its projection coordinates in the pixel coordinate system are (u, v); is the intrinsic parameter matrix of the camera, is the extrinsic matrix of the camera. The intrinsic matrix and the extrinsic matrix obtained by the camera calibration method are collectively called the camera matrix;

[0038] The camera calibration method is Zhang Zhengyou's camera calibration method, and the specific method is as follows:

[0039] S1. Prepare a chessboard and shoot it at different angles with a camera to obtain a set of images;

[0040] S2, detecting the corner points of the calibration plate in the image, obtaining the pixel coordinate values ​​of the corner points of the calibration plate, and then calculating the physical coordinate values ​​of the corner points of the calibration plate;

[0041] S3, solving the internal parameter matrix and the external parameter matrix;

[0042] S4. Use LM (Levenberg-Marquardt) algorithm to optimize the above parameters.

[0043] Camera calibration is the premise and foundation for the successful application of machine vision in practice. Only by accurately calibrating the camera can the effective three-dimensional position information of the object be accurately calculated based on the collected two-dimensional image sequence of the object.

[0044] Preferably, in order for the machine vision system to collect and process images in real time, realize the real-time calculation of the spatial position of the axis point of the divergent water jet, and be applicable to the dynamically changing divergent water jet scene, the method for processing the image in step 4 is as follows:

[0045] 4.1. Through image binarization processing, the outer contour information of the water jet is extracted from the water jet cutting robot arm operation image captured by the camera; a threshold T is set to separate the two types of pixels. Any point (x, y) in the image with f(x, y)>T is the target point, otherwise the point is the background point. The target pixel and the background pixel are distinguished, the messy background information is eliminated, and the image characteristics of the high-speed water jet are retained;

[0046] 4.2. Edge detection: perform edge detection on the binarized image to obtain the required water jet contour and remove non-edge pixels;

[0047] From the water jet robot arm operation images captured by the camera, given that the high-pressure water jet appears as a bright white area due to light scattering, its grayscale value is usually low. In order to accurately extract the outer contour information of the water jet and effectively eliminate the messy background information, while retaining the image features of the high-speed water jet as completely as possible, we adopt an image binarization processing strategy.

[0048] Due to the influence of background and light, only image binarization cannot directly separate the water jet contour from the background. Therefore, it is necessary to perform edge detection on the binarized image to obtain the required water jet outer contour and remove some non-edge pixels.

[0049] The edge detection method is the Canny edge detection algorithm, and the specific method is as follows:

[0050] Step 1: First, perform Gaussian filtering and denoising on the input image;

[0051] Step 2: On the smoothed grayscale image, use the Sobel operator to calculate the gradient amplitude Gx(x,y) and Gy(x,y) in the horizontal and vertical directions;

[0052] Step 3: Non-maximum suppression, after calculating the gradient amplitude and direction, remove non-edge points and non-edges;

[0053] Step 4: Use the Otsu algorithm to implement the adaptive selection of high and low thresholds for the Canny algorithm.

[0054] Optimize the options to accurately obtain the target point P2 Coordinates, the target point P is obtained in step 5 2 The coordinate method is as follows:

[0055] 5.1. Extract straight lines. According to the characteristic that the contour line of the water jet is a straight line, the image is detected for straight lines to obtain the complete outer contour line of the water jet;

[0056] 5.2. After extracting the four corner points of the target and obtaining the outer contour of the water jet, extract the four corner points of the outer contour to calculate the position of a point P on the axis of the water jet in the image coordinate system;

[0057] 5.3. Get the target point P 2 The three-dimensional coordinates of the image processing object are divergent water jets, which are similar to truncated cones in space. The corner points obtained after image processing are regarded as the four vertices of a convex quadrilateral. The center positions of the four points are on the axis. The center positions of these four points are calculated to obtain a point on the axis.

[0058] Through the binocular stereo vision positioning principle and coordinate transformation, the three-dimensional coordinates P of the center point p in the world coordinate system are obtained. 2 (X 2 ,Y 2 ,Z 2 ), the desired P 2 It is a point on the axis of the divergent water jet.

[0059] Preferably, in order to obtain the outer contour of the complete water jet, the method for extracting the straight line is to extract the straight line by Hough transform, and the specific method is as follows:

[0060] The first step is to obtain all points with a value of 1 in the binary image as target points, perform Hough transform on each target point and map it to the parameter space, and quantize the P and θ parameters to M P and N θ equal parts;

[0061] The second step is to use the discretized M P and N θ Create a two-dimensional accumulator A with an initial value of 0;

[0062] The third step is to calculate the coordinate value of each point in the Cartesian coordinate system in polar coordinates, and add 1 to the accumulator for each additional sinusoidal intersection curve.

[0063] The fourth step is to select a suitable threshold for judgment. When the value in the accumulator is greater than the threshold, the matched line is the required straight line. The points on the same line appear as peak points in polar coordinates. The length of the straight line is then calculated and returned to the Cartesian coordinates to complete the extraction of the straight line.

[0064] After the edge of the image is obtained, the edge is not a complete straight line at this time. According to the characteristic that the contour line of the water jet is a straight line, the image can be detected for straight lines to obtain the complete outer contour line of the water jet.

[0065] Preferably, in order to accurately obtain the positions of the four corner points of the target, the method for extracting the four corner points of the target is Harris corner point detection, and the specific method is as follows:

[0066] After obtaining the outer contour of the water jet, it is necessary to extract the four corner points of the outer contour to calculate the position of a point P on the axis of the water jet in the image coordinate system.

[0067] The high-pressure water jet from a water gun is ideally cylindrical, but in reality, it will diverge, similar to a truncated cone. Regardless of the state, when performing corner point detection on an image, the center points of the four corner points detected in the end, that is, the centroid of the quadrilateral, are all on the axis of the water jet, which meets the purpose of the detection step. The center points of corner points (1, 2, 3, 4) and corner points (1, 2, 5, 6) are the same point and are on the central axis of the water jet.

[0068] The grayscale transformation generated by moving the local window on the image determines whether it is a corner point by the degree of change of the window in each direction; the image window (u, v) is translated to generate grayscale change E(u, v).

[0069] Get the image coordinates of the four corner points, obtain the centroid coordinates of the water jet image in the image coordinate system, and use formula (2) to get the coordinates P in the global world coordinate system: 2

[0070] E(u,v)=∑ x,y w(x,y)[I(x+u,y+v)-I(x,y)] 2 (3)

[0071] The above formula is written as

[0072]

[0073] In the formula,

[0074]

[0075] The w function is the window function, and the M matrix is ​​the partial derivative matrix, where I x ,I y They are the partial derivative functions of the image in the horizontal and vertical directions respectively. According to the eigenvalue calculation method, two eigenvalues ​​are generated. These two eigenvalues ​​represent the intensity of the transformation of each pixel. In the actual calculation process, the response value R of each corner point is calculated through the corner point response function, and then the R value is used to determine whether it is a corner point;

[0076] If the set threshold Tr < R, then the pixel is considered a corner point in the image; otherwise, it is a non-corner point in the image.

[0077] The mathematical definition of the corner response function is as follows:

[0078] R = det(M) - k(traceM) 2 (6)

[0079] Where k is an empirical constant, and the value range of k is between 0.04 and 0.06.

[0080] Preferred option. To accurately obtain the three-dimensional coordinates of the target point P 2 The method for obtaining the three-dimensional coordinates of the target point P 2 is as follows:

[0081] Since the object of image processing is a divergent water jet, which is similar to a frustum of a cone in space, the corner points obtained after image processing are regarded as the four vertices A i (u i , v i ) of a convex quadrilateral. The central position of the four points is on the axis. Calculating the central position of these four points finds a point on the axis:

[0082]

[0083] Through the binocular stereo vision positioning principle and coordinate transformation, the three-dimensional coordinates P 2 (X 2 , Y 2 , Z 2 ) of the center point p in the world coordinate system are obtained. The required P 2 is a point on the axis of the divergent water jet.

[0084] Preferred option. The method for calculating the shortest distance between the water jet axis and the manipulator in step 6 is as follows:

[0085] According to the obtained P 1 (X 1 , Y 1 , Z 1 ) and P 2 (X 2 , Y 2 , Z 2 ), a space straight line L parallel to the axis of the divergent water jet is obtained;

[0086] The two-point form equation of the straight line L:

[0087]

[0088] From the M(X m,Y m ,Z m ), calculate the shortest distance d between M and L;

[0089] The value of d is calculated from the distance from the point to the line. Take a point P on the line 1 ,get

[0090]

[0091] Direction vector of line L

[0092]

[0093] but

[0094]

[0095] Where a=(Y 1 -Y m )(Z 2 -Z 1 )-(Y 2 -Y 1 )(Z 1 -Z m ), b=(X 1 -X m )(Z 2 -Z 1 )-(X 2 -X 1 )(Z 1 -Z m ), c=(X 1 -X m )(Y 2 -Y 1 )-(X 2 -X 1 )(Y 1 -Y m );

[0096] Get the shortest distance d between M and L:

[0097]

[0098] Beneficial effects: The kinematic modeling and analysis of the robot arm of the present invention: the Denavit-Hartenberg (DH) parameter method is used for kinematic analysis to accurately derive the three-dimensional spatial position coordinates of specific target points on the end effector of the dual robot arm.

[0099] Image acquisition system deployment: Utilize high-resolution machine vision sensors to continuously and accurately capture images of the water jet. The vision sensor is installed between the two robotic arms to ensure that the diffusion pattern and key features of the water jet can be clearly recorded.

[0100] Image preprocessing and feature extraction: The collected image data is preprocessed, including noise suppression, image binarization and other steps to improve image quality. Subsequently, advanced image processing techniques such as edge detection, line fitting and corner point detection are used to extract the contour boundary and corner point information of the water jet.

[0101] Axis point positioning: Based on the extracted corner point information, a geometric analysis method is used to determine the precise position of the key points on the divergent water jet axis.

[0102] Calculation of three-dimensional spatial coordinates: Combining the camera calibration parameters with the image processing results and using the binocular stereo vision principle, the three-dimensional spatial coordinates of a specific point on the axis of the divergent water jet are calculated through triangulation.

[0103] Collision detection and judgment mechanism: Based on the spatial position information of the key points on the end effector of the robot arm and the calculated coordinates of the key points on the axis of the water jet, a mathematical model of the shortest distance between a straight line and a point is constructed. By comparing the shortest distance with the preset safety threshold, it is determined whether there is a potential contact risk between the water jet and the clamping device.

[0104] The present invention can realize high-precision calculation of the spatial position of the axis point of the divergent water jet through the high-precision image acquisition and processing technology of machine vision. The machine vision system can acquire and process images in real time to realize the real-time calculation of the spatial position of the axis point of the divergent water jet, which is suitable for dynamically changing divergent water jet scenes. The present invention is not only suitable for the measurement of divergent water jets, but can also be extended to the spatial position calculation of other liquids or solid cylinders, and has wide applicability. BRIEF DESCRIPTION OF THE DRAWINGS

[0105] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0106] Figure 1 It is a schematic diagram of the collision detection process of the present invention;

[0107] Figure 2 It is a schematic diagram of DH parameters of the present invention;

[0108] Figure 3A schematic diagram of the robot arm connecting rod coordinate system established for the present invention;

[0109] Figure 4 This is a schematic diagram of the collaborative dual-manipulator system model of the present invention;

[0110] Figure 5 It is a schematic diagram of conversion between coordinate systems of the present invention;

[0111] Figure 6 This is a flow chart of the Zhang Zhengyou calibration method of the present invention;

[0112] Figure 7 This is a schematic diagram of an image before image processing of the present invention;

[0113] Figure 8 This is a schematic diagram of an image after image binarization according to the present invention;

[0114] Fig. 9 This is a schematic diagram of corner point extraction of a divergent water jet image according to the present invention;

[0115] Fig.10 It is a schematic diagram of the image processing process of the present invention;

[0116] Fig.11 This is a schematic diagram of the binocular stereoscopic vision positioning principle of the present invention;

[0117] Fig.12 It is a mathematical model diagram of the straight line and the distance to point M of the present invention. DETAILED DESCRIPTION

[0118] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0119] In the description of the present invention, it is necessary to understand that the terms "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship are based on the orientation or position relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0120] In the present invention, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may include that the first and second features are in direct contact, or may include that the first and second features are not in direct contact but are in contact through another feature between them. Moreover, a first feature being "above", "above" and "above" a second feature includes that the first feature is directly above and obliquely above the second feature, or simply indicates that the first feature is higher in level than the second feature. A first feature being "below", "below" and "below" a second feature includes that the first feature is directly below and obliquely below the second feature, or simply indicates that the first feature is lower in level than the second feature.

[0121] like Figure 1 As shown, a water jet cutting collaborative dual-manipulator safety protection method based on machine vision includes the following steps:

[0122] Step 1: Establish a kinematic model of the robotic arm, construct a fixed global world coordinate system and a local coordinate system of each link of the robotic arm, and perform kinematic modeling of the collaborative dual robotic arms;

[0123] Step 2: Obtain the key point coordinates of the robot arm and transform the coordinates to associate the local coordinate system of each link of the robot arm established in step 1 with the global world coordinate system, and obtain the key point P on the end effector of the main robot arm in the global world coordinate system. 1 The three-dimensional coordinate information of point M, the center point of the end effector of the robot arm;

[0124] Step 3: Acquire the target image, calibrate the camera, and shoot the target image;

[0125] Step 4: Process the image to obtain the outer contour image of the water jet;

[0126] Step 5: Get the target point P 2 Coordinates, extract the target point P on the water jet axis 2 , calculate and determine the target point P 2 The three-dimensional spatial position in the global world coordinate system;

[0127] Step 6: Calculate the shortest distance between the water jet axis and the slave robot arm based on the P obtained in steps 2 and 5. 1 and P 2 Point, calculate the point through P in the global world coordinate system 1 and P 2 The mathematical equation of the straight line L is obtained, and the shortest distance d between the straight line L and the axis of the water jet is calculated;

[0128] Step 7: Contact risk assessment. Set a safety threshold distance r. Compare the shortest distance d with the safety distance threshold r. When the shortest distance d is greater than the safety distance threshold r, return to step 3. When the shortest distance d is less than or equal to the safety distance threshold r, determine that there is a contact risk and take protective measures.

[0129] The system of the present invention consists of three parts: a computer control unit, a visual perception system, and a robotic arm execution system. The visual system collects image information and feeds it back to the computer. The computer control unit uses image processing algorithms and data analysis to accurately identify and locate the spatial position of the target object. The computer then sends instructions to control the movement of the robotic arm based on the current position of the robotic arm.

[0130] like Figure 2 and 3 As shown, the DH method is used to establish the dual-arm link coordinate system.

[0131] In the figure: a i =Along X i Axis, from Z i Move to Z i+1 distance;

[0132] α i = around X i Axis, from Z i Rotate to Z i+1 Angle

[0133] d i = Along Z i Axis, from X i-1 Move to X i distance;

[0134] θ i = around Z i Axis, from X i-1 Rotate to X i angle.

[0135] according to Figure 2 DH parameters and robot arm size can be established Figure 3 Link coordinate system.

[0136] Since the two robotic arms are of the same type but different sizes, the kinematic modeling methods of the two robotic arms are the same, and only the kinematic modeling of the main arm is shown in the figure.

[0137] like Figure 4 As shown in the figure, the RGB-D camera is 1, the main arm of the robot is 2, the water jet is 3, the clamping mechanism is 4, and the slave arm of the robot is 5.

[0138] According to the robot arm link coordinate system established in step 1, the coordinate transformation is used to obtain Figure 4 Medium1 (X 1 ,Y 1 ,Z 1 ) and M(X m ,Y m ,Z m ) in the world coordinate system. Key point P 1 The specific position is the intersection of the axis of the water gun and the axis of the last arm of the robot arm. The position of the key point M is at the center point of the spherical bounding box of the clamping device.

[0139] The forward kinematics equation of the robot arm can be obtained by matrix multiplication to obtain the transformation matrix between the links. i T j Represents the transformation matrix of coordinate system {j} relative to coordinate system {i}. The overall transformation matrix of the manipulator terminal operation mechanism compared to the base can be expressed as:

[0140] 0 T 6 = 0 T 1 1 T 2 2 T 3 3 T 4 4 T 5 5 T 6 (1)

[0141] like Figure 5 As shown, coordinate transformation and camera calibration in binocular vision:

[0142] Camera calibration is the premise and foundation for the successful application of machine vision in practice. Only by accurately calibrating the camera can the effective three-dimensional position information of the object be accurately calculated based on the collected two-dimensional image sequence of the object. First, according to the camera pinhole imaging principle, the relationship between the image in the pixel coordinate system, the image coordinate system, and the camera coordinate system can be determined. The conversion relationship between these three coordinate systems can be described as shown in formula (2).

[0143]

[0144] Assume that p is a point in space, and its coordinates in the world coordinate system are Its projection coordinates in the pixel coordinate system are (u, v); is the intrinsic parameter matrix of the camera, is the extrinsic parameter matrix of the camera. The intrinsic parameter matrix and the extrinsic parameter matrix are collectively referred to as the camera matrix, and both need to be obtained through the camera calibration method.

[0145] like Figure 6As shown, the camera calibration method is Zhang Zhengyou's camera calibration method, and the specific method is as follows:

[0146] S1. Prepare a chessboard and shoot it at different angles with a camera to obtain a set of images;

[0147] S2, detecting the corner points of the calibration plate in the image, obtaining the pixel coordinate values ​​of the corner points of the calibration plate, and then calculating the physical coordinate values ​​of the corner points of the calibration plate;

[0148] S3, solving the internal parameter matrix and the external parameter matrix;

[0149] S4. Use LM (Levenberg-Marquardt) algorithm to optimize the above parameters.

[0150] like Figure 7 and 8 As shown, the image is binarized:

[0151] From the water jet robot arm operation images captured by the camera, given that the high-pressure water jet appears as a bright white area due to light scattering, its grayscale value is usually low. In order to accurately extract the outer contour information of the water jet and effectively eliminate the messy background information, while retaining the image features of the high-speed water jet as completely as possible, we adopt an image binarization processing strategy.

[0152] Image binarization is a technology that converts an image into a pixel containing only two pixel values, usually 0 and 255, representing black and white respectively. To separate the water jet contour pixels and background pixels, a threshold T for segmenting the two pixels can be set. Any point (x, y) in the image where f(x, y)>T is the target point, otherwise the point is the background point. In this way, the target pixel can be distinguished from the background pixel. Figure 8 Figure 6 is the binarized image of the water jet.

[0153] Edge detection: perform edge detection on the binarized image to obtain the required water jet contour and remove non-edge pixels;

[0154] The edge detection method is the Canny edge detection algorithm. Due to the influence of background and light, only image binarization cannot directly separate the water jet contour from the background. Therefore, it is necessary to perform edge detection on the binarized image to obtain the required water jet outer contour and remove some non-edge pixels.

[0155] The specific method is as follows:

[0156] Step 1: First, perform Gaussian filtering and denoising on the input image;

[0157] Step 2: On the smoothed grayscale image, use the Sobel operator to calculate the gradient amplitude Gx(x,y) and Gy(x,y) in the horizontal and vertical directions;

[0158] Step 3: Non-maximum suppression, after calculating the gradient amplitude and direction, remove non-edge points and non-edges;

[0159] Step 4: Use the Otsu algorithm to implement the adaptive selection of high and low thresholds for the Canny algorithm.

[0160] Hough transform is used to extract straight line steps.

[0161] After the edge of the image is obtained, the edge is not a complete straight line at this time. According to the characteristic that the contour line of the water jet is a straight line, the image can be detected for straight lines to obtain the complete outer contour line of the water jet.

[0162] Step 1: Get all the points with a value of 1 in the binary image as target points, perform Hough transform on each target point and map it to the parameter space, and quantize the P and θ parameters into MP and Nθ.

[0163] Step 2: Establish a two-dimensional accumulator A with an initial value of 0 based on the discretized MP and Nθ.

[0164] Step 3: Count the coordinate values ​​of each point in the Cartesian coordinate system in polar coordinates, and add 1 to the accumulator for each additional sinusoidal intersection curve.

[0165] Step 4: Select a suitable threshold for judgment. When the value in the accumulator is greater than the threshold, it proves that the matched line is the required straight line. The points on the same line appear as peak points in polar coordinates. Then calculate the length of the straight line and return it to the Cartesian coordinates to complete the extraction of the straight line.

[0166] like Fig. 9 As shown, Harris corner detection is used to extract the four corner points of the target:

[0167] After obtaining the outer contour of the water jet, it is necessary to extract the four corner points of the outer contour to calculate the position of a point P on the axis of the water jet in the image coordinate system.

[0168] The high-pressure water jet from a water gun is ideally cylindrical, but in reality, it will diverge, similar to a truncated cone, such as Fig. 9 As shown. Regardless of the state, when the image is detected for corner points, the center points of the four corner points detected in the end, that is, the centroid of the quadrilateral, and the center points are all on the axis of the water jet, which meets the purpose of the detection step. The center points of corner points (1, 2, 3, 4) and corner points (1, 2, 5, 6) are the same point and are on the central axis of the water jet.

[0169] Harris corner detection is the gray-scale transformation generated by moving a local window on an image. Whether a point is a corner is determined by the degree of change of the window in various directions. Translate the image window (u, v) to generate the gray-scale change E(u, v).

[0170] Once the image coordinates of the four corners are obtained, the centroid coordinates of the water jet image in the image coordinate system can be acquired. Using formula (2), the coordinates of point P in the world coordinate system can be obtained as P 2 。

[0171] E(u, v) = ∑ x,y w(x, y)[I(x + u, y + v) - I(x, y)] 2 (3)

[0172] The above formula can be written as

[0173]

[0174] In the formula,

[0175]

[0176] The w function is the window function, and the M matrix is the partial derivative matrix. Among them, Ix and Iy are the partial derivative functions of the image in the horizontal and vertical directions respectively. According to the eigenvalue calculation method, two eigenvalues will be generated, and these two eigenvalues represent the intensity of the transformation of each pixel point. In the actual calculation process, the response value R of each corner is calculated through the corner response function, and then whether it is a corner is judged according to the R value.

[0177] If the set threshold Tr < R, then the pixel point is considered to be a corner in the image; otherwise, it is a non-corner in the image.

[0178] The mathematical definition of the corner response function is specifically as follows:[[]]

[0179] R = det(M) - k(traceM) 2 (6)

[0180] In the formula, k is an empirical constant, and the value range of k is between 0.04 and 0.06.

[0181] As Fig.10 and 11 shown, the three-dimensional coordinates of the target point P 2 are obtained as follows:[[]]

[0182] Since the object of image processing is a divergent water jet, which is similar to a frustum of a cone in space, the corners obtained after image processing can be regarded as the four vertices A i (u i,v i ),like Fig.10 As shown. The center positions of the four points are on the axis. By calculating the center positions of these four points, we can find a point on the axis.

[0183]

[0184] Through binocular stereo vision positioning principle, such as Fig.11 As shown, the coordinate transformation obtains the three-dimensional coordinates P of the center point p in the world coordinate system 2 (X 2 ,Y 2 ,Z 2 ), the desired P 2 It is a point on the axis of the divergent water jet.

[0185] like Fig.12 As shown, according to the obtained three-dimensional information of the target, the distance between the water jet and the center point of the end effector of the main machine arm is calculated;

[0186] In the picture: P 1 is the key point on the water jet axis, P 2 is the key point on the water jet axis, L is the water jet axis, d is the shortest path from point M to straight line L, and M is the key point in the center of the clamping device.

[0187] Depend on Fig.12 As shown, according to the obtained P 1 (X 1 ,Y 1 ,Z 1 ), P 2 (X 2 ,Y 2 ,Z 2 ) can get a straight line L in space with the divergent water jet axis. The two-point equation of the straight line L is:

[0188]

[0189] M(X) obtained in step 1 m ,Y m ,Z m ), we can calculate the shortest distance d between M and L. The value of d can be calculated from the distance from the point to the line. Take a point P on the line 1 ,available

[0190]

[0191] Direction vector of line L

[0192]

[0193] but

[0194]

[0195] Where a=(Y 1 -Y m )(Z 2 -Z 1 )-(Y 2 -Y 1 )(Z 1 -Z m ),

[0196] b=(X 1 -X m )(Z 2 -Z 1 )-(X 2 -X 1 )(Z 1 -Z m ),

[0197] c=(X 1 -X m )(Y 2 -Y 1 )-(X 2 -X 1 )(Y 1 -Y m ).

[0198] The shortest distance d between M and L can be obtained:

[0199]

[0200] Step 7: Contact risk assessment to determine whether contact is established

[0201] When d>r, the divergent water jet and the gripper at the end of the slave arm are considered to be at a safe distance and no collision occurs; when d≤r, the divergent water jet and the gripper at the end of the slave arm are considered to be at a dangerous distance and a collision may occur. At this time, measures can be taken to avoid collision accidents. Considering the accuracy of visual system measurement and possible errors in other links, the value of r can be appropriately adjusted to prevent collisions.

[0202] In the actual working environment, the water jet is often regarded as a regular cylinder under ideal conditions. However, when the robot performs the cutting task, the water jet will show a variety of divergent phenomena, and its axis vector position and length will change dynamically. Faced with such a complex and changeable scene, traditional positioning methods are powerless to capture and locate the position and direction of the divergent water jet. The vision-based water jet cutting collaborative dual-arm safety protection method combines kinematic principles with machine vision algorithms to determine the spatial posture of the divergent water jet, especially for those real-time changing and unpredictable water jet divergent forms. This technology can quickly and accurately capture its dynamic characteristics. By locating the divergent water jet in real time, it is possible to determine whether the device is in a dangerous state, thereby avoiding high-speed jet cutting robotic arms and achieving safe and reliable operation.

[0203] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0204] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A water jet cutting collaborative dual-manipulator safety protection method based on machine vision, characterized in that , including the following steps: Step 1: Establish a kinematic model of the robotic arm, construct a fixed global world coordinate system and a local coordinate system of each link of the robotic arm, and perform kinematic modeling of the collaborative dual robotic arms; Step 2, obtain the coordinates of the key points of the robot arm, transform the coordinates to associate the local coordinate system of each link of the associated robot arm established in step 1 with the global world coordinate system, and obtain the three-dimensional coordinate information of the key point P1 on the end effector of the main robot arm and the center point M of the end effector of the slave robot arm in the global world coordinate system; Step 3: Acquire the target image, calibrate the camera, and shoot the target image; Step 4: Process the image to obtain the outer contour image of the water jet; Step 5, obtain the coordinates of the target point P2, extract the target point P2 on the water jet axis, and calculate and determine the three-dimensional spatial position of the target point P2 in the global world coordinate system; Step 6, calculate the shortest distance between the water jet axis and the slave robot arm, calculate the mathematical equation of the straight line L passing through P1 and P2 in the global world coordinate system based on the points P1 and P2 obtained in steps 2 and 5, and calculate the shortest distance d between the straight line L and the water jet axis; Step 7: Contact risk assessment. Set a safety threshold distance r. Compare the shortest distance d with the safety distance threshold r. When the shortest distance d is greater than the safety distance threshold r, return to step 3. When the shortest distance d is less than or equal to the safety distance threshold r, determine that there is a contact risk and take protective measures.

2. The waterjet cutting collaborative dual-manipulator safety protection method based on machine vision according to claim 1 is characterized in that: The method for establishing the kinematic model of the robotic arm described in step 1 is as follows: 1.

1. Use DH method to establish the dual robot arm link coordinate system a i = Along X i Axis, from Z i Move to Z i+1 The distance α i = around X i Axis, from Z i Rotate to Z i+1 Angle, d i = Along Z i Axis, from X i-1 Move to X i The distance θ i = around Z i Axis, from X i-1 Rotate to X i Angle 1.

2. Establish the robot arm link coordinate system according to the DH parameters and robot arm size.

3. The waterjet cutting collaborative dual-manipulator safety protection method based on machine vision according to claim 2 is characterized in that: The method for obtaining the key point coordinates of the robot arm in step 2 is as follows: 2.

1. According to the robot arm link coordinate system established in step 1, the key point P1 (X1, Y1, Z1) and the key point M (X m ,Y m ,Z m ) in the world coordinate system, The specific position of the key point P1 is the intersection of the axis of the water gun and the axis of the last arm of the robot arm. The position of the key point M is at the center of the spherical bounding box of the clamping device; 2.

2. Obtain the forward kinematics equation of the robot arm, and obtain the transformation matrix between the connecting rods by matrix multiplication. i T j represents the transformation matrix of coordinate system {j} relative to coordinate system {i}. The overall transformation matrix of the manipulator terminal operation mechanism compared to the base is expressed as: <h2 style=";text-align:left;direction:ltr"> 0 <h2 style=";text-align:left;direction:ltr"> T6=<h2 style=";text-align:left;direction:ltr"> 0 <h2 style=";text-align:left;direction:ltr"> T1<h2 style=";text-align:left;direction:ltr"> 1 <h2 style=";text-align:left;direction:ltr"> T2<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> T3<h2 style=";text-align:left;direction:ltr"> 3 <h2 style=";text-align:left;direction:ltr"> T4<h2 style=";text-align:left;direction:ltr"> 4 <h2 style=";text-align:left;direction:ltr"> T5<h2 style=";text-align:left;direction:ltr"> 5 <h2 style=";text-align:left;direction:ltr"> T6(1)。 4. The waterjet cutting collaborative dual-manipulator safety protection method based on machine vision according to claim 1 is characterized in that: The method for calibrating the camera in step 3 is as follows: According to the camera pinhole imaging principle, the relationship between the image in the pixel coordinate system, image coordinate system and camera coordinate system is determined. The transformation relationship between the three coordinate systems is described as shown in formula (2): Assume that p is a point in space, and its coordinates in the world coordinate system are Its projection coordinates in the pixel coordinate system are (u, v); is the intrinsic parameter matrix of the camera, is the extrinsic matrix of the camera. The intrinsic matrix and the extrinsic matrix obtained by the camera calibration method are collectively called the camera matrix; The camera calibration method is Zhang Zhengyou's camera calibration method, and the specific method is as follows: S1. Prepare a chessboard and shoot it at different angles with a camera to obtain a set of images; S2, detecting the corner points of the calibration plate in the image, obtaining the pixel coordinate values ​​of the corner points of the calibration plate, and then calculating the physical coordinate values ​​of the corner points of the calibration plate; S3, solving the internal parameter matrix and the external parameter matrix; S4. Use LM (Levenberg-Marquardt) algorithm to optimize the above parameters.

5. The waterjet cutting collaborative dual-manipulator safety protection method based on machine vision according to claim 1 is characterized in that: The method for processing the image in step 4 is as follows: 4.

1. Through image binarization processing, the outer contour information of the water jet is extracted from the water jet cutting robot arm operation image captured by the camera; a threshold T is set to separate the two types of pixels. Any point (x, y) in the image with f(x, y)>T is the target point, otherwise the point is the background point. The target pixel and the background pixel are distinguished, the messy background information is eliminated, and the image characteristics of the high-speed water jet are retained; 4.2 Edge detection: Perform edge detection on the binarized image to obtain the required water jet outer contour and remove non-edge pixels. The method of the edge detection is the Canny edge detection algorithm, and the specific method is as follows: The first step: First, perform Gaussian filtering denoising on the input image. The second step: On the smoothed grayscale image, use the Sobel operator to calculate the gradient amplitudes Gx(x, y) and Gy(x, y) in the horizontal and vertical directions. The third step: Non-maximum suppression. After calculating the gradient amplitude and direction, remove non-edge points and remove non-edges. The fourth step: Use the Otsu algorithm to adaptively select the high and low thresholds of the Canny algorithm.

6. The waterjet cutting collaborative dual-manipulator safety protection method based on machine vision according to claim 3 is characterized in that: The method for obtaining the coordinates of the target point P2 in step 5 is as follows: 5.1 Extract a straight line: According to the characteristic that the water jet contour line is a straight line, perform straight line detection on the image to obtain the complete water jet outer contour line. 5.2 Extract the four corner points of the target: After obtaining the water jet outer contour line, extract the four corner points of the outer contour to calculate the position of a point P on the water jet axis in the image coordinate system. 5.3 Obtain the three-dimensional coordinates of the target point P2: The object of image processing is a divergent water jet, which is similar to a frustum of a cone in space. The corner points obtained after image processing are regarded as the four vertices of a convex quadrilateral, and the central position of the four points is on the axis. Calculate the central position of these four points to obtain a point on the axis. Through the binocular stereo vision positioning principle and coordinate transformation, obtain the three-dimensional coordinates P2(X2, Y2, Z2) of the center point p in the world coordinate system. The required P2 is a point on the axis of the divergent water jet.

7. The waterjet cutting collaborative dual-manipulator safety protection method based on machine vision according to claim 6 is characterized in that: The method for extracting the straight line is to extract the straight line by Hough transform, and the specific method is as follows: The first step is to obtain all points with a value of 1 in the binary image as target points, perform Hough transform on each target point and map it to the parameter space, and quantize the P and θ parameters to M P and N θ equal parts; The second step is to use the discretized M P and N θ Create a two-dimensional accumulator A with an initial value of 0; The third step: Statistically obtain the coordinate values of each point in the Cartesian coordinate system in the polar coordinate system. For each additional sine intersection curve transformed, add 1 to the accumulator. The fourth step: Select an appropriate threshold for determination. When it is determined that the value in the accumulator is greater than the threshold, the matched line is the required straight line. The points on the same line appear in the form of peak points in the polar coordinate system. Then calculate the length of the straight line and return it to the Cartesian coordinate system to complete the extraction of the straight line.

8. The waterjet cutting collaborative dual-manipulator safety protection method based on machine vision according to claim 6 is characterized in that: The method for extracting the four corner points of the target is Harris corner detection, and the specific method is as follows: The gray level transformation generated by the local window moving on the image is used to judge whether it is a corner point according to the change degree of the window in each direction; translate the image window (u, v) to generate the gray level change E(u, v). Obtain the image coordinates of the four corner points, obtain the centroid coordinates of the water jet image in the image coordinate system, and use formula (2) to obtain the coordinates P2 of P in the global world coordinate system. E(u,v)=∑ x,y w(x,y)[I(x+u,y+v)-I(x,y)] 2 (3) The above formula is written as In the formula, The w function is the window function, and the M matrix is ​​the partial derivative matrix, where I x ,I y They are the partial derivative functions of the image in the horizontal and vertical directions respectively. According to the eigenvalue calculation method, two eigenvalues ​​are generated. These two eigenvalues ​​represent the intensity of the transformation of each pixel. In the actual calculation process, the response value R of each corner point is calculated through the corner point response function, and then the R value is used to determine whether it is a corner point; If the set threshold Tr < R, it is considered that the pixel point is a corner point in the image, otherwise it is a non-corner point in the image. The mathematical definition of the corner response function is specifically as follows: R=det(M)-k(traceM) 2 (6) In the formula, k is an empirical constant, and the value range of k is between 0.04 and 0.

06.

9. The waterjet cutting collaborative dual-manipulator safety protection method based on machine vision according to claim 6 is characterized in that: The method for obtaining the three-dimensional coordinates of the target point P2 is as follows: The corner points obtained after image processing are regarded as the four vertices A of the convex quadrilateral i (u i ,v i ), the center positions of the four points are on the axis, and calculating the center positions of these four points will find a point on the axis: Through the binocular stereo vision positioning principle and coordinate transformation, the three-dimensional coordinates P2 (X2, Y2, Z2) of the center point p in the world coordinate system are obtained. The required P2 is a point on the axis of the divergent water jet.

10. The waterjet cutting collaborative dual-manipulator safety protection method based on machine vision according to claim 9 is characterized in that: The method for calculating the shortest distance between the water jet axis and the slave robot arm in step 6 is as follows: According to the obtained P1(X1, Y1, Z1) and P2(X2, Y2, Z2), a spatial straight line L is obtained which is in line with the axis of the divergent water jet; The two-point equation of line L is: M(X) obtained in step 1 m ,Y m ,Z m ), calculate the shortest distance d between M and L; The value of d is calculated from the distance from the point to the line. Taking a point P1 on the line, we get Direction vector of line L but where a = (Y1 - Y m )(Z2 - Z1) - (Y2 - Y1)(Z1 - Z m ), b = (X1 - X m )(Z2 - Z1) - (X2 - X1)(Z1 - Z m ), c = (X1 - X m )(Y2 - Y1) - (X2 - X1)(Y1 - Y m ); Get the shortest distance d between M and L:

Images