A multi-modal data fusion based robot arm perception method

Abstract

The application relates to the technical field of computer vision, and discloses a mechanical arm perception method based on multi-modal data fusion, which comprises the following steps: camera calibration and hand-eye calibration are performed; a camera is driven by ROS to acquire an image and a depth map, and a YOLOv5 image recognition algorithm is used to calculate the center point and pose matrix of a target object; a coordinate system transfer algorithm is designed to transfer the homogeneous matrix of the target object in the camera coordinate system to the homogeneous matrix in the base coordinate system of the mechanical arm; a motion control algorithm is designed to realize the motion control of the mechanical arm and complete the given task. The application fuses the data of multiple modes such as an RGB image, a depth image and a tactile system of a dexterous hand to identify the target object, calculates the pose information of the object center point, operates the target object through the dexterous hand on the mechanical arm, improves the accuracy of the dexterous hand tactile system in perceiving and operating the target object, and realizes accurate grasping of the target object by the mechanical arm.

Description

Technical Field

[0001] This invention relates to the field of computer vision technology, and in particular to a robotic arm perception method based on multimodal data fusion. Background Technology

[0002] With the continuous development of computer science, control theory, machine vision sensors, and other fields, robotics technology has entered a stage of rapid development. Robots use robotic arms to perform more meaningful tasks and functions, thereby improving production efficiency and quality of life. For example, in factories, robots can handle tasks such as cargo handling and loading / unloading; in welding workshops, robots can assist people with tasks such as welding and screwing. When performing various tasks, robots need vision guidance to bring the end effector on the robotic arm to a designated position to effectively complete the assigned tasks. Robots use RGB image information from cameras, depth map information, and multimodal sensor data such as dexterous hands to acquire environmental information, identify the category of target objects, and estimate the pose of the target objects. This guides the end effector of the robotic arm to the target object, enabling it to perform various tasks, making robots more intelligent. RGB images can reflect the characteristics of the real world, while depth images can reflect key information such as the size of the real world. By fusing RGB and depth images, the advantages of both can be combined, making them more suitable for many practical application scenarios.

[0003] RGB images, using the traditional YOLOv5, can identify the center point of pixels on the surface of an object. The 3D coordinates of the object's surface can be calculated from the depth information, making it suitable for regular objects. However, YOLOv5 cannot identify the center point of the entire object. Furthermore, traditional perception methods have a relatively limited modality, reducing the accuracy of using the tactile system of a dexterous hand to perceive and manipulate targets, and making it impossible for robotic arms to accurately grasp targets. Summary of the Invention

[0004] This invention provides a robotic arm perception method based on multimodal data fusion. It integrates data from multiple modalities, such as RGB images, depth images, and the tactile system of a dexterous hand, to identify the target object, calculate the pose information of the object's center point, and manipulate the target object through the dexterous hand on the robotic arm. This improves the accuracy of perceiving and manipulating the target object using the dexterous hand's tactile system, enabling the robotic arm to accurately grasp the target object.

[0005] This invention provides a robotic arm perception method based on multimodal data fusion, comprising:

[0006] Camera calibration and hand-eye calibration are performed. The camera calibration is performed using the OpenCV calibrateCamera function, and the hand-eye calibration is performed using the OpenCV calibrateHandEye function.

[0007] The ROS-driven camera acquires images and depth maps, and the YOLOv5 image recognition algorithm is used to calculate the center point and pose matrix of the target object.

[0008] Design a coordinate system transfer algorithm to transfer the homogeneous matrix of the target object in the camera coordinate system to the homogeneous matrix in the robot arm base coordinate system;

[0009] Design motion control algorithms to realize the motion control of the robotic arm and complete the predetermined tasks.

[0010] Furthermore, the steps of designing the motion control algorithm to realize the motion control of the robotic arm and complete the predetermined task include:

[0011] The YOLOv5 image recognition algorithm is used to determine whether the target object has been identified. If not, the robot body is moved and the target object identification continues. If it is, the object pose is obtained through depth information and camera intrinsic parameters.

[0012] Determine whether it is necessary to identify the center point of the object. If yes, start VoteNet 3D point cloud recognition to obtain the pose of the center point of the object, and combine it with YOLOv5 pixel center point weighted calculation to calculate the center point of the pixel on the object surface, and then perform coordinate system transfer algorithm; if no, directly perform coordinate system transfer algorithm.

[0013] The robot arm motion control algorithm is used to determine whether the robot arm can reach the position. If not, the process returns to the step of moving the robot body to identify the target object. If so, the robot arm is run and the dexterous hand is used to grasp or manipulate the target object until the task is completed.

[0014] Furthermore, the step of using ROS-driven cameras to acquire images and depth maps, and using the YOLOv5 image recognition algorithm to calculate the center point and pose matrix of the target object, includes:

[0015] Compile the ROS camera driver and drive the camera, call the topic of camera RGB image and depth information, convert the RGB image into a Mat matrix of the cv library through cv_bridge, and store the image as a jpg file;

[0016] Load the YOLOv5 deep learning model and perform recognition, calculate the pixel of the center point of each target pixel, and store the recognition results as a structure type;

[0017] VoteNet 3D point cloud recognition loads the model for recognition, determines the 3D coordinates of the center of each target object, and assists the YOLOv5 image recognition algorithm to obtain the surface pixel position.

[0018] The depth camera acquires the depth information of the pixel's location. Based on the intrinsic parameter matrix, the 3D coordinates (x, y, z) of the target object's center point are obtained, with the depth information being d. The coordinates of the target object's center pixel are known to be (u, v), and the intrinsic parameter matrix is ​​as follows: Three-dimensional coordinates: z = d; x = (uc) x )*z / f x ;y=(uc y )*z / f y .

[0019] Furthermore, the design coordinate system transfer algorithm, which transfers the homogeneous matrix of the target object in the camera coordinate system to the homogeneous matrix in the robot arm base coordinate system, includes the following steps:

[0020] YOLOv5 can be used to identify objects, obtain the object's translation vector (x, y, z), and the object's rotation vector is assumed to be (0, 0, 0). This is then converted into a rotation matrix, and the rotation matrix and translation vector are concatenated to form a 4x4 pose matrix M. target2cam ;

[0021] The hand-eye calibration matrix M is obtained through hand-eye calibration. cam2gripper ;

[0022] The homogeneous matrix M of the robotic arm's end effector in the robotic arm's base coordinate system can be obtained through the robotic arm's interface. gripper2base The homogeneous matrix M of the robotic arm's end effector in the base coordinate system is calculated using the formula. target2base Its formula is M target2base =M gripper2base *M cam2gripper *M target2cam ;

[0023] Calculate the homogeneous pose matrix M that the robotic arm should actually reach. gripperWithTool2base The transformation matrix M of the tool end in the end-effector coordinate system of the robotic arm TCP is used to determine the transformation of the tool end. tool2gripper This includes: setting the homogeneous matrix of the target object in the base coordinate system to be equal to the homogeneous matrix of the tool tip in the base coordinate system, i.e., M. target2base =M tool2base Based on the coordinate system transformation relationship, the homogeneous matrix of the tool end in the robot arm's end-effector coordinate system is transformed to the robot arm's polar coordinate system M. tool2base =M gripperWithTool2base *M tool2gripper ;

[0024] Based on the two formulas in the above steps, the homogeneous matrix M of the position that the robotic arm's end effector needs to reach in the robotic arm's base coordinate system can be calculated. gripperWithTool2base =M target2base *(M tool2gripper ) -1 ;

[0025] Through matrix decomposition, the 4*4 homogeneous matrix is ​​decomposed into a 3*3 rotation matrix and translation vector Location: (x,y,z). The rotation matrix is ​​then converted into a rotation vector (Rx,Ry,Rz). The translation and rotation vectors are provided to the robot arm's moveL interface for linear motion to the specified position.

[0026] Furthermore, in the steps of camera calibration and hand-eye calibration, the intrinsic parameter coefficients of the camera are first obtained, including:

[0027] After installing the camera driver and ROS and starting the camera, you can view the camera's intrinsic parameters and distortion coefficients by checking the camera_info topic message.

[0028] The camera is calibrated by taking multiple pictures of the calibration board in different poses. The three-dimensional actual coordinates of each corner point of the calibration board correspond one-to-one with the two-dimensional pixel coordinates on the picture, so the correspondence between the actual world coordinates and the pixel coordinates can be obtained.

[0029] The camera is calibrated using OpenCV's calibrateCamera function. The inputs are the 2D and 3D coordinates of the calibration board and the size of the calibration board mesh. The outputs are the camera intrinsic parameter matrix, distortion coefficients, and extrinsic parameter coefficients, including rotation and translation vectors.

[0030] Furthermore, the camera calibration process specifically includes:

[0031] Prepare an 11×8 grid calibration plate and fix it to the wall;

[0032] Keeping the position of the robotic arm's base coordinate system unchanged, use the robotic arm teach pendant to move the robotic arm's end effector, and the camera will move with the robotic arm's end effector;

[0033] The robotic arm's end effector moves to a position, takes a photo, and simultaneously records the position and orientation information displayed on the teach pendant.

[0034] Move the end effector of the robotic arm to another position, take and record the images repeatedly, taking a total of 14 pictures from different positions and angles, and recording 14 sets of data.

[0035] The program was written using VScode and C++. The parameters for the number of images, the size of each chessboard square on the calibration board, and the number of corner points in each row and column on the calibration board were set. 88 corner points on the calibration board were extracted and saved. Then, sub-pixel precision was performed on each corner point. The purpose was to refine the previously coarsely extracted corner points and save the sub-pixel corner points.

[0036] Assuming the calibration board is placed on the plane with Z=0 in the world coordinate system, initialize the three-dimensional coordinates of each corner point on the calibration board, and use the OpenCV's calibrateCamera function to calibrate the camera. The input is the three-dimensional coordinates and corresponding two-dimensional coordinates of each corner point, and the output is the camera's intrinsic parameter matrix, distortion coefficients, and the rotation and translation vectors of each image.

[0037] The calibration results are evaluated. Using the obtained camera intrinsic and extrinsic parameters, the three-dimensional coordinates of each corner point are reprojected to obtain new projection points. Then, the error is estimated by comparing the new projection points with the previous two-dimensional projection points to obtain the average pixel error of each image.

[0038] Furthermore, the steps for hand-eye alignment specifically include:

[0039] During camera calibration, 14 sets of extrinsic parameter matrices corresponding to the images are obtained, namely rotation vectors and translation vectors. The representation of the attitude rotation vector is converted into the representation of the rotation matrix.

[0040] The pose matrix of the robotic arm end in the base coordinate system of the robotic arm corresponding to each image. At this time, the pose matrix is ​​composed of translation vectors and Euler angles. In order to use the pose matrix as the input of OpenCV to calculate the hand-eye calibration matrix, the Euler angle representation is converted into the rotation matrix representation.

[0041] The OpenCV calibrateHandEye function is used. The inputs are the rotation matrix and translation vector of the end of the robot arm in the robot arm coordinate system and the rotation matrix and translation vector of the calibration plate in the camera coordinate system. The output is the hand-eye calibration matrix, which is composed of the rotation matrix and translation matrix and is converted into a 4×4 hand-eye calibration homogeneous matrix.

[0042] Hand-eye calibration accuracy test: Since the relative position of the calibration plate and the robot arm base coordinate system remains unchanged, the calibration effect is verified. The homogeneous matrix of the robot arm end in the robot arm base coordinate system, the hand-eye calibration homogeneous matrix, and the homogeneous matrix of the calibration plate in the camera coordinate system corresponding to each image are multiplied together, and the translation matrix is ​​extracted from the result.

[0043] Error analysis, using 14 sets of translation matrices, allows observation of the calibration effect, obtaining the maximum errors in the X, Y, and Z directions to determine whether the requirements are met.

[0044] Furthermore, the movement process of the robotic arm includes:

[0045] Log in to the robotic arm and initialize the initial angles of its six joints to ensure that the camera at the end of the robotic arm can observe the target object;

[0046] ROS initialization, subscribing to the camera's RGB topic messages and depth information, performing image recognition, and returning recognition information, including the category and the pixel of the center point of the recognition box;

[0047] A state machine is established to determine the operating state of the robotic arm and whether the camera has detected the target object. The operating states of the robotic arm include: the robotic arm is in standby state, in which the robot can walk normally; the robotic arm is in an abnormal state, in which the robot stops waiting; the robotic arm is in working state, in which the robot stops waiting; and the camera at the end of the robotic arm does not detect the target object.

[0048] Once the target object is identified, determine whether the distance between the target object and the robot arm's base coordinate system exceeds the robot arm's control range.

[0049] A new thread is created to publish ROS topic messages in real time, sending the real-time operating status of the robotic arm to the robot controller;

[0050] Based on the pixel and depth information of the target object's center, as well as the camera's intrinsic parameters, the position of the target object in the robot arm's base coordinate system is calculated, and the initial pose is given.

[0051] The robotic arm moves to the front of the target object, grabs the target object, and then uses the robotic arm's SDK to design a reasonable path plan to run the robotic arm. After the robotic arm finishes running, it returns to the initial state.

[0052] The robotic arm end effector uses a robotic arm dexterous hand with a tactile system to grasp or manipulate a target object. Through the tactile system, it provides feedback to determine whether the object has been successfully grasped or manipulated.

[0053] The beneficial effects of this invention are as follows:

[0054] 1. Establish visual guidance for hand-eye coordination. Through hand-eye calibration, calculate the hand-eye calibration matrix and obtain camera intrinsic parameters. Obtain pose information in the camera coordinate system through RGB and depth images, and then transmit the pose information to the controller. Through corresponding calculations, obtain the pose information of the target object in the robot arm base coordinate system, and then run the robot arm to reach the designated position. This position is the initial position for performing the task. The accuracy of the pose information obtained by the camera is a prerequisite for the robot arm to accurately reach the designated position.

[0055] 2. An improved YOLOv5-based target detection method combining Votenet 3D point cloud is proposed, which integrates 2D RGB images and 3D point cloud images and constructs a visual guidance system to achieve target object grasping and manipulation tasks.

[0056] 3. A multimodal data perception method based on 2D RGB images, depth images, point clouds, and dexterous hand tactile senses is proposed. The advantages of each modality are combined, and an improved YOLOv5 is used for image recognition. The depth information of the depth camera is combined with the bounding box of Votenet 3D point cloud recognition to enhance the accuracy of YOLOv5 image recognition pixels. The accuracy of sensing the target object by the dexterous hand's tactile system is achieved, enabling the robotic arm to accurately grasp the target object.

[0057] 4. RGB images can extract geometric, semantic, and texture information, enhancing the features of point clouds; an improved Votenet 3D point cloud target detection method is used to obtain the center point of the bounding box of the object, and the pixel center points identified by YOLOv5 are combined for weighted calculation to improve the accuracy of center point pixel coordinate recognition. Attached Figure Description

[0058] Figure 1 This is a flowchart illustrating the method of the present invention.

[0059] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0060] It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0061] Traditional visual pose estimation methods are based on Aruco codes, which require placing the Aruco codes near the target object, making them conspicuous and aesthetically unappealing. This invention, based on deep learning, first uses YOLOv5 to identify the target object, obtaining its bounding box and category. Then, it aligns the 2D RGB image and depth map to obtain the depth information of the target object's center pixel, thereby determining the target object's position. The specific process consists of the following steps:

[0062] like Figure 1 As shown, this invention provides a robotic arm perception method based on multimodal data fusion, comprising:

[0063] S1. Perform camera calibration and hand-eye calibration. The camera calibration is performed using the OpenCV calibrateCamera function, and the hand-eye calibration is performed using the OpenCV calibrateHandEye function.

[0064] S1.1 First, obtain the camera's intrinsic parameter coefficients. The main methods for obtaining the camera's intrinsic parameter coefficients are as follows:

[0065] S1.1.1 Install the camera driver and ROS. After starting the camera, check the camera's topic message camera_info to view the camera's intrinsic parameters and distortion coefficients.

[0066] S1.1.2. Use Zhang Zhengyou's calibration method to calibrate the camera. Take multiple pictures of the calibration board in different poses using the camera. The three-dimensional actual coordinates of each corner point of the calibration board and the two-dimensional pixel coordinates on the picture are in one-to-one correspondence. The correspondence between the actual coordinates and the pixel coordinates can be obtained.

[0067] S1.1.3. Use OpenCV's calibrateCamera function to calibrate the camera. The input is the 2D and 3D point coordinates of the calibration board and the size of the calibration board mesh. The output is the camera intrinsic parameter matrix, distortion coefficients, and extrinsic parameter coefficients. The extrinsic parameter coefficients include rotation vectors and translation vectors.

[0068] S1.2 Specific calibration methods for camera calibration and hand-eye calibration:

[0069] S1.2.1 Camera Calibration: The camera calibration method described in step S1.1.3 is selected because it is necessary to obtain the rotation and translation vectors of each calibration board image to provide input data for subsequent calculation of the hand-eye calibration matrix. The obtained intrinsic parameter matrix, distortion coefficients, and extrinsic parameter coefficients are shown in the figure. The specific implementation method consists of the following steps:

[0070] Step 1: Prepare an 11×8 grid calibration plate and fix it to the wall;

[0071] Step 2: Keep the position of the robotic arm's base coordinate system unchanged, and use the robotic arm teach pendant to move the end effector of the robotic arm. Because the eye-on-hand solution is used, the camera is fixed at the end effector of the robotic arm, so the camera will move with the end effector of the robotic arm.

[0072] Step 3: Move the robotic arm end effector to a position, take a photo, and record the position and posture information displayed on the teach pendant;

[0073] Step 4: Move the end effector of the robotic arm to another position, repeat step 3 to take and record a total of 14 pictures from different positions and angles, and record 14 sets of data.

[0074] Step 5: Use VScode programming software and C++ programming language to program the code, set the number of images, the size of each chessboard square on the calibration board, the number of corner points in each row and column on the calibration board, and other parameters. Extract 88 corner points on the calibration board and save them. Then, perform sub-pixel refinement on each corner point. The purpose is to refine the previously coarsely extracted corner points and save the sub-pixel corner points.

[0075] Step 6: Assuming the calibration board is placed on the plane with Z=0 in the world coordinate system, initialize the three-dimensional coordinates of each corner point on the calibration board, and use the OpenCV's calibrateCamera function to calibrate the camera. The input is the three-dimensional coordinates and corresponding two-dimensional coordinates of each corner point, and the output is the camera's intrinsic parameter matrix, distortion coefficients, and the rotation and translation vectors of each image.

[0076] Step 7: Evaluate the calibration results. Using the obtained camera intrinsic and extrinsic parameters, reproject the 3D coordinates of each corner point to obtain new projection points. Then, estimate the error between the new projection points and the previous 2D projection points to obtain the average pixel error of each image.

[0077] Step 8: Save the calibration results.

[0078] S1.2.2 Hand-Eye Calibration: The purpose of hand-eye calibration is to design an algorithm to obtain the hand-eye calibration matrix. A scheme where the eye is on the hand is adopted. Once the hand-eye calibration matrix is ​​obtained, the transformation relationship between the robotic arm's end effector coordinate system and the camera coordinate system is determined. The specific implementation method is as follows:

[0079] Step 1: During the camera calibration process described above, 14 sets of extrinsic parameter matrices corresponding to the images were obtained, namely rotation vectors and translation vectors. Then, the representation of the attitude rotation vectors was converted into the representation of the rotation matrix.

[0080] Step 2: The pose matrix of the robotic arm end effector in the base coordinate system of the robotic arm corresponding to each image. At this time, the pose matrix consists of translation vectors and Euler angles. In order to use the pose matrix as the input for OpenCV to calculate the hand-eye calibration matrix, the Euler angle representation needs to be converted into the rotation matrix representation.

[0081] Step 3: Use OpenCV's calibrateHandEye function. The inputs are the rotation matrix and translation vector of the robotic arm end in the robotic arm coordinate system and the rotation matrix and translation vector of the calibration plate in the camera coordinate system. The output is the hand-eye calibration matrix. At this time, the calibration matrix is ​​composed of rotation and translation matrices and needs to be converted into a 4x4 homogeneous hand-eye calibration matrix.

[0082] Step 4: Hand-eye calibration accuracy test. Since the relative positions of the calibration board and the robot arm's base coordinate system remain unchanged, this is used to verify the calibration effect. The homogeneous matrix of the robot arm's end effector in the robot arm's base coordinate system, the homogeneous matrix of hand-eye calibration, and the homogeneous matrix of the calibration board in the camera coordinate system corresponding to each image are multiplied together, and the translation matrix is ​​extracted from the result.

[0083] Step 5: Error Analysis. The calibration effect can be observed using 14 sets of translation matrices. In the X direction, the minimum error is -1.120m, the maximum is -1.196m, and the maximum error is 0.004m; in the Y direction, the minimum error is 0.121m, the maximum is 0.132m, and the maximum error is 0.011m; in the Z direction, the minimum error is -0.102m, the maximum is -0.095m, and the maximum error is 0.007m. The maximum error in all X, Y, and Z directions is 1.1cm, which meets the requirements.

[0084] S2. The ROS-driven camera acquires images and depth maps, and the YOLOv5 image recognition algorithm is used to calculate the center point and pose matrix of the target object; the specific method is as follows:

[0085] S2.1 Compile the ROS camera driver to drive the camera; command: roslaunch orbbec_cameragemini2.launch.

[0086] S2.2, Topics for calling camera RGB image and depth information: / camera / color / image_raw and / camera / depth / image_raw. The RGB image is converted into a Mat matrix of the cv library through cv_bridge and the image is stored as a jpg file;

[0087] S2.3 Load the YOLOv5 deep learning model and perform recognition, calculate the pixel of the center point of each target pixel, and store the recognition result as a structure type;

[0088] In YOLOv5 image recognition, a dataset is collected by taking images of the target object at different positions and poses. The images are then labeled using labelIMG. The labeled images are then fed into YOLOv5 for training to generate a model. The pyTorch-based pt model is then converted into an ONNX intermediate model. Using the Atlas 200DK developer kit, after deploying the environment, the ATC tool is used to convert the original model into a model (.om) suitable for Ascend hardware. The converted model is copied to the model folder, and inference is performed on the images to be recognized, identifying the target object and providing its 2D pixel coordinates. Based on the camera's intrinsic parameters and depth information at the pixels, the 3D coordinates of the target object in the camera coordinate system are calculated. By default, the robot faces the object directly. The initial pose of the camera on the robotic arm can be adjusted to face the object directly, so the default initial pose is (0, 0, 0). If the robot is not facing the target object, the pose needs to be calculated using the angles between the robot body and the target object.

[0089] S2.4, VoteNet 3D point cloud recognition, load the model for recognition, determine the 3D coordinates of the center of each target object, and assist the YOLOv5 image recognition algorithm to obtain the surface pixel position.

[0090] S2.5 The depth camera acquires the depth information of the pixel location, and obtains the three-dimensional coordinates (x, y, z) of the target object's center point based on the intrinsic parameter matrix, with the depth information being d;

[0091] Given the coordinates of the center pixel of the target object as (u, v), and the intrinsic parameter matrix:

[0092] Three-dimensional coordinates: z = d; x = (uc) x )*z / f x ;y=(uc y )*z / f y .

[0093] S3. Design a coordinate system transfer algorithm to transfer the homogeneous matrix of the target object in the camera coordinate system to the homogeneous matrix in the robot arm's base coordinate system; the specific method is as follows:

[0094] S3.1. YOLOv5 can be used to identify the target object, obtain the target object's translation vector (x, y, z), and the target object's rotation vector is assumed to be (0, 0, 0). This is then converted into a rotation matrix, and the rotation matrix and translation vector are concatenated to form a 4*4 pose matrix M. target2cam ;

[0095] S3.2 Obtain the hand-eye calibration matrix M through hand-eye calibration. cam2gripper ;

[0096] S3.3. The homogeneous matrix M of the robotic arm's end effector in the robotic arm's base coordinate system can be obtained through the robotic arm's interface. gripper2base The homogeneous matrix M of the robotic arm's end effector in the base coordinate system is calculated using the following formula. target2base :

[0097] M target2base =M gripper2base *M cam2gripper *M target2cam

[0098] S3.4 Considering that a tool, such as a dexterous hand, needs to be installed at the end effector of the robotic arm, it is necessary to calculate the homogeneous pose matrix M that the robotic arm should actually reach. gripperWithTool2base The transformation matrix M of the tool end in the end-effector coordinate system of the robotic arm TCP is used to determine the transformation of the tool end. tool2gripper ;

[0099] Let the homogeneous matrix of the target object in the base coordinate system be equal to the homogeneous matrix of the tool tip in the base coordinate system, that is:

[0100] M target2base =M tool2base

[0101] Based on the coordinate system transformation relationship, the homogeneous matrix of the tool end in the robot arm's end-effector coordinate system is transferred to the robot arm's polar coordinate system:

[0102] M tool2base =M gripperWithTool2base *M tool2gripper

[0103] S3.5. Based on the two formulas in step S3.4, the homogeneous matrix of the position that the end effector of the robotic arm needs to reach in the base coordinate system can be calculated:

[0104] M gripperWithTool2base =M target2base *(M tool2gripper ) -1

[0105] Through matrix decomposition, the 4*4 homogeneous matrix is ​​decomposed into a 3*3 rotation matrix and translation vector Location: (x,y,z). The rotation matrix is ​​then converted into a rotation vector (Rx,Ry,Rz). The translation and rotation vectors are provided to the robot arm's moveL interface for linear motion to the specified position.

[0106] S4. Design a motion control algorithm to realize the motion control of the robotic arm and complete the predetermined task.

[0107] like Figure 1 As shown, completing the grasping or manipulation of the target object includes:

[0108] S41. Using the YOLOv5 image recognition algorithm, determine whether the target object has been identified; if not, move the robot body and continue to identify the target object; if yes, obtain the object pose through depth information and camera intrinsic parameters.

[0109] S42. Determine whether it is necessary to identify the center point of the object. If yes, start VoteNet 3D point cloud recognition to obtain the pose of the center point of the object, and combine it with YOLOv5 pixel center point weighted calculation to calculate the center point of the pixel on the surface of the object, and then perform coordinate system transfer algorithm; if no, directly perform coordinate system transfer algorithm.

[0110] S43. Perform the robotic arm motion control algorithm to determine whether the robotic arm can reach the position; if not, return to the step of moving the robot body to identify the target object; if yes, run the robotic arm and use the dexterous hand to grasp or manipulate the target object until the task is completed.

[0111] The calculation of the object's pose matrix involves algorithm design using VS Code programming software and the C++ programming language. The specific implementation method includes the following steps:

[0112] Step 1: Observe the object in real time with the camera and take a picture;

[0113] Step 2: Using the YOLOv5 image recognition algorithm, depth image information, and camera intrinsic parameter matrix described above, the pose matrix of the target object's center point in the camera coordinate system is obtained.

[0114] Step 3: Design an algorithm to convert the object's pose matrix into a 4×4 homogeneous matrix using VScode software and C++ programming, which facilitates matrix operations.

[0115] Step 4: Obtain the pose matrix of the end effector in the base coordinate system of the robotic arm from the robotic arm controller, and then convert the Euler angle and translation matrix representation into the homogeneous matrix representation.

[0116] Step 5: Add the homogeneous hand-eye calibration matrix calculated earlier. Now you have three homogeneous matrices. Multiply the three matrices by matrix multiplication to obtain the homogeneous matrix of the target object in the robot arm's base coordinate system. Then, convert the homogeneous matrix into rotation vectors and translation vectors.

[0117] Step 6: Obtain the position and orientation of the end effector from the end effector hand to the end effector of the robotic arm through the robotic arm TCP calibration, and transfer the pose of the end effector of the robotic arm in the robotic arm base coordinate system to the pose of the end effector hand in the base coordinate system.

[0118] This invention utilizes the UR5 robotic arm, a flexible robot ideally suited for optimizing lightweight collaborative processes such as picking, placement, and testing. The UR5 is easy to program and allows for rapid setup, achieving an ideal balance between machine size and power, making it a perfect choice for automating light manufacturing tasks. The UR robot can simulate the range of motion of a human arm and can be dragged and taught; waypoints can be set simply by moving the robot to the target location. The robotic arm rotates or translates based on rotation and translation vectors to reach the designated position. The robotic arm's movement process is as follows:

[0119] a. Log in to the robotic arm. Initialize the initial angles of the six joints of the robotic arm to ensure that the camera at the end of the robotic arm can observe the target object;

[0120] b. ROS initialization: Subscribe to the camera's RGB topic messages and depth information, provide the RGB image topic messages to the Atlas200DK developer kit for image recognition, and return recognition information, including the category and the pixel of the center point of the recognition box;

[0121] c. Establish a state machine to determine the operating status of the robotic arm and whether the camera has detected the target object. There are 4 states: (1) The robotic arm is in standby state, at which time the robot can walk normally; (2) The robotic arm is in abnormal state, at which time the robot stops waiting; (3) The robotic arm is in working state, at which time the robot stops waiting; (4) The camera on the end of the robotic arm does not observe the target object.

[0122] d. Identify the target object and determine whether the distance between the target object and the robot arm's base coordinate system exceeds the robot arm's control range;

[0123] e. Create a new thread to publish ROS topic messages in real time, sending the real-time operating status of the robotic arm to the robot controller.

[0124] f. Based on the pixel and depth information of the target object's center, as well as the camera's intrinsic parameters, the position of the target object in the robot arm's base coordinate system can be calculated, and the initial posture can be given.

[0125] g. The robotic arm moves to the front of the target object, grasps the target object, and then uses the robotic arm's SDK to design a reasonable path plan, runs the robotic arm, and returns to the initial state after the robotic arm has finished running.

[0126] h. The robotic arm end effector uses a robotic arm dexterous hand with a tactile system to grasp or manipulate a target object. Through the tactile system, it provides feedback to determine whether the target object has been successfully grasped or manipulated.

[0127] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, apparatus, article, or method that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, apparatus, article, or method. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, apparatus, article, or method that includes that element.

[0128] The above description is merely a preferred embodiment of the present invention and does not limit the patent scope of the present invention. Any equivalent structural or procedural transformations made based on the content of the present invention's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of the present invention.

Claims

1. A method for robot perception based on multi-modal data fusion, the method comprising: The application relates to a robot target object recognition and operation method and device. Camera calibration and hand-eye calibration are performed, the camera calibration is performed by using a calibrateCamera function of opencv, and the hand-eye calibration is performed by using a calibrateHandEye function of opencv; ROS is used to drive a camera to obtain an image and a depth map, and a YOLOv5 image recognition algorithm is used to calculate a center point and a pose matrix of a target object; A coordinate system transfer algorithm is designed to transfer a homogeneous matrix of the target object in a camera coordinate system to a homogeneous matrix in a base coordinate system of a robot arm; A motion control algorithm is designed to realize motion control of the robot arm and complete a given task; Specifically, the YOLOv5 image recognition algorithm is used to determine whether a target object is recognized; if not, the robot body is moved and target object recognition is continued; if yes, object pose is obtained through depth information and camera intrinsic parameters; It is determined whether object center points need to be recognized; if yes, VoteNet three-dimensional point cloud recognition is started to obtain object center point pose, and object surface pixel center points are calculated by combining YOLOv5 pixel center points, and then the coordinate system transfer algorithm is performed; if not, the coordinate system transfer algorithm is directly performed; The robot arm motion control algorithm is performed to determine whether the robot arm can reach a position; if not, the step of moving the robot body is returned to perform target object recognition; if yes, the robot arm is run, and the target object is grasped or operated by using a dexterous hand until a task is completed. The step of using ROS to drive the camera to obtain the image and the depth map and using the YOLOv5 image recognition algorithm to calculate the center point and the pose matrix of the target object comprises the following steps:

2. The multi-modal data fusion based robotic arm perception method of claim 1, wherein, ROS camera driving is compiled and the camera is driven, a camera RGB image and depth information topics are called, the RGB image is converted into a cv library Mat matrix through a cv_bridge, and the picture is stored as a jpg file; A deep learning YOLOv5 model is loaded and recognition is performed, and pixel of each target object pixel center point is calculated, and the recognition result is stored as a structure type; VoteNet three-dimensional point cloud recognition is performed, a model is loaded and recognition is performed, three-dimensional coordinates of each target object center are determined, and surface pixel positions are obtained by assisting the YOLOv5 image recognition algorithm. The step of designing the coordinate system transfer algorithm to transfer the homogeneous matrix of the target object in the camera coordinate system to the homogeneous matrix in the base coordinate system of the robot arm comprises the following steps: The depth camera acquires depth information of a position where a pixel is located, and obtains three-dimensional coordinates (x, y, z) of a center point of a target object according to an intrinsic matrix, and the depth information is d; a known target object center pixel point coordinate is (u, v), and the intrinsic matrix is: Three-dimensional coordinates: z=d; x=(u-c x )*z / f x ; y=(u-c y )*z / f y .

3. The multi-modal data fusion based robotic arm perception method of claim 1, wherein, A 4*4 homogeneous matrix is decomposed into a 3*3 rotation matrix and a translation vector Location: (x, y, z) through matrix decomposition, the rotation matrix is converted into a rotation vector (Rx, Ry, Rz), and the translation vector and the rotation vector are provided to a moveL interface of the robot arm to realize linear motion to a specified position. The YOLOv5 can identify the target object, obtain a translation vector (x, y, z) of the target object, a default rotation vector of the target object is (0, 0, 0), and the rotation matrix is converted into a rotation matrix, and then the rotation matrix and the translation vector are spliced into a 4*4 pose matrix M target2cam ; A hand-eye calibration matrix M is obtained by hand-eye calibration cam2gripper ; The homogeneous matrix M of the end of the mechanical arm in the base coordinate system of the mechanical arm can be obtained through the interface of the mechanical arm gripper2base The homogeneous matrix M of the end of the mechanical arm in the base coordinate system of the mechanical arm is calculated through a formula target2base The formula is M target2base =M gripper2base *M cam2gripper *M target2cam ; A pose homogeneous matrix M that the robot arm should actually reach is calculated gripperWithTool2base , by a conversion matrix M of a tool end under a robot arm TCP robot arm end coordinate system tool2gripper ; including: let the homogeneous matrix of the target object under the base coordinate system equal the homogeneous matrix of the tool end under the base coordinate system, that is, M target2base =M tool2base ; according to the coordinate system conversion relationship, the homogeneous matrix of the tool end under the robot arm end coordinate system is transferred to M tool2base =M gripperWithTool2base *M tool2gripper ; According to the two formulas of the above steps, the homogeneous matrix M of the position required to be reached by the end of the robot arm in the robot arm base coordinate system can be calculated gripperWithTool2base = M target2base *(M tool2gripper ) -1 ; In the step of performing camera calibration and hand-eye calibration, first, intrinsic parameters of the camera are obtained, comprising the following steps:

4. The multi-modal data fusion based robotic arm perception method of claim 1, wherein, After the camera driver and ROS are installed and the camera is started, a topic message camera_info of the camera is viewed, and the intrinsic parameters and distortion coefficients of the camera can be viewed. ​ The camera is calibrated, a plurality of pictures of different poses are taken by using the camera to shoot the calibration board, the three-dimensional actual coordinates of each corner point of the calibration board and the two-dimensional pixel coordinates on the picture are one-to-one corresponding, so that the corresponding relationship between the world actual coordinates and the pixel coordinates can be obtained; The camera is calibrated by using the calibrateCamera function of opencv, the input is the 2D point coordinates and 3D point coordinates of the calibration board and the size of the calibration board grid, and the output is the camera intrinsic matrix and the distortion coefficient and the external parameter coefficient, the external parameter coefficient includes the rotation vector and the translation vector.

5. The multi-modal data fusion based robotic arm perception method of claim 4, wherein, The steps of camera calibration include: Prepare a 11x8 grid calibration board and fix it on the wall; Keep the position of the robot base coordinate system unchanged, move the robot end using the robot teach pendant, and move the camera with the robot end; Move the robot end to a position, take a picture, and record the position and attitude information displayed on the teach pendant; Move the robot end to another position, repeat the shooting and recording, a total of 14 pictures of different positions and angles are taken, and 14 groups of data are recorded; Program using VScode programming software and C++ programming language, set the number of pictures, the size of each chessboard on the calibration board, the number of corner points of each row and column on the calibration board, extract 88 corner points on the calibration board and save, then perform sub-pixel refinement on each corner point, the purpose is to refine the previously extracted corner points, and save the sub-pixel corner points; Assume that the calibration board is placed on the plane of the world coordinate system with Z=0, initialize the three-dimensional coordinates of each corner point on the calibration board, and calibrate the camera using the calibrateCamera function of opencv, the input is the three-dimensional coordinates of each corner point and the corresponding two-dimensional coordinates, and the output is the intrinsic matrix of the camera, the distortion coefficient, and the rotation vector and the translation vector of each picture; The calibration results are evaluated, the three-dimensional coordinates of each corner point are calculated by re-projecting the obtained camera internal and external parameters, new projection points are obtained, and then error estimation is performed with the previous two-dimensional projection points to obtain the average pixel error of each image.

6. The multi-modal data fusion based robotic arm perception method of claim 5, wherein, The steps of hand-eye calibration include: The external parameter matrix corresponding to the 14 groups of images is obtained during camera calibration, that is, the rotation vector and the translation vector, and the representation form of the attitude rotation vector is converted into the representation form of the rotation matrix; The pose matrix of the robot end in the robot base coordinate system corresponding to each image, the pose matrix at this time is composed of the translation vector and the Euler angle, in order to use the pose matrix as the input of the opencv calculation hand-eye calibration matrix in the subsequent process, the representation form of the Euler angle is converted into the representation form of the rotation matrix; The calibrateHandEye function of opencv is used, the input is the rotation matrix and the translation vector of the robot end in the robot coordinate system and the rotation matrix and the translation vector of the calibration board in the camera coordinate system, and the output is the hand-eye calibration matrix, the calibration matrix at this time is composed of the rotation matrix and the translation matrix, which is converted into a 4x4 hand-eye calibration homogeneous matrix. Hand-eye calibration precision test, because the relative position of the calibration board and the robot base coordinate system remains unchanged, the calibration effect is verified, the homogeneous matrix of the robot end in the robot base coordinate system corresponding to each image, the homogeneous matrix of the hand-eye calibration, the homogeneous matrix of the calibration board in the camera coordinate system, the three matrices are multiplied, and the translation matrix is extracted from the result; Error analysis, 14 groups of translation matrix can observe the calibration effect, get the maximum error in X, Y, Z direction to determine whether to meet the requirements.

7. The multi-modal data fusion based robotic arm perception method of claim 1, wherein, The motion process of the robot arm includes: Login robot arm, the robot arm initializes the initial angle of 6 joints to ensure that the camera on the robot arm end can observe the target object; ROS initialization, subscribe to the RGB topic message and depth information of the camera, perform image recognition, and return the recognition information including the category and the pixel of the recognition box center point; Establish a state machine to determine the running state of the robot arm and whether the camera recognizes the target object; wherein the running state of the robot arm includes: the robot arm is in standby state, at this time the robot can walk normally; the robot arm is in abnormal state, at this time the robot stops waiting; the robot arm is in working state, at this time the robot stops waiting; the camera on the robot arm end does not observe the target object; Recognize the target object, judge whether the distance between the target object and the robot base coordinate system exceeds the control range of the robot arm; Open a new thread, publish the ROS topic message in real time, and send the real-time robot running state to the robot master; According to the pixel and depth information of the target object center and the camera intrinsic parameter, the position of the target object in the robot base coordinate system is calculated, and the initial attitude is given; The robot arm runs to the front of the target object, grabs the target object, and then uses the SDK of the robot arm to design a reasonable path planning, runs the robot arm, and restores to the initial state after the robot arm runs; The robot arm end tool uses a robot hand with a tactile system to grab or operate the target object, and feeds back information through the tactile system to determine whether the target object is successfully grabbed or the target object is successfully operated.

Images