Robot autonomous mobile grasping method in large-range dense block object environment

Abstract

The application discloses a kind of robot autonomous mobile grabbing method, comprising the following steps: S1, the initial position and global image of all workpieces on workbench are acquired;S2, all the workpieces to be grabbed on workbench are positioned;S3, the pose of all the workpieces to be grabbed relative to camera coordinate system is calculated by normal vector solving method;S4, the actual grabbing pose of each workpiece by the end of mechanical arm is calculated, and it is established as the total information of workpiece pose;S5, the rotation angle of each joint of mechanical arm is calculated;S6, control mechanical arm to grab workpiece, and delete the workpiece pose information in the total information of workpiece pose in workpiece pose;S7, the position required to move the remaining workpieces to be grabbed is solved by combining greedy algorithm and genetic algorithm;S8, the sorting of path point, and control mechanical arm to grab all workpieces of each path point inverse solution completed.The application solves the problem that the existing robot positioning and grabbing method cannot be applied in the case of various poses of the objects to be grabbed.

Description

Autonomous movement and grasping methods for robots in environments with large-scale, densely packed blocks of objects Technical Field

[0001] This invention belongs to the field of robot vision technology, specifically, it relates to a method for autonomous robot movement and grasping in a large-scale, dense blocky object environment. Background Technology

[0002] The demand for low-cost, multi-station flexible analysis is becoming increasingly urgent, making autonomous mobile robots combining robotic arms and AGVs increasingly valuable in industrial and medical fields. However, due to the relatively low positioning accuracy of autonomous mobile robots and the limitations of the robotic arm's length, they cannot find a suitable position to grasp large and densely packed materials. Currently, tasks such as grasping and placing large amounts of material mainly rely on a combination of AGV navigation and positioning and machine vision compensation technology.

[0003] For example, Chinese patent publication number CN114089767A discloses a method for locating and grasping bottle-shaped objects in a mobile composite robot application. This method first determines a standard photographic pose and identifies obstacles based on a shape template matching method; it calculates the correspondence between robot movement and image feature changes by comparing the feature changes in the images before and after movement with the robot's movement changes; based on this correspondence, it eliminates the positioning error when the robot moves to the taught position and performs target grasping by the robotic arm. It can be used directly without calibration, and boasts high positioning accuracy and good real-time performance.

[0004] The drawback of existing patents is that the robot's positioning and grasping are based on a pre-defined teaching position, which is only applicable to objects with a small layout. It is not suitable for objects with a large layout (where there are some object positions that the robotic arm cannot reach); at the same time, it is not suitable for situations where the position of the object to be grasped is diverse. Summary of the Invention

[0005] To address the problem that existing robot localization and grasping methods are not applicable to large objects or objects with diverse poses when placed in a calibrated teaching position, this invention provides a robot autonomous movement and grasping method for environments with large-scale dense blocky objects.

[0006] To achieve the above-mentioned technical objectives, the technical solution adopted by the present invention is as follows:

[0007] A method for autonomous robot movement and grasping in environments with large-scale, densely packed blocks of objects, including the following steps:

[0008] S1. By controlling the robotic arm of the autonomous mobile robot to move with a camera, the initial position and global image of all workpieces on the workbench are obtained;

[0009] S2. Input the global image into the YOLOv5 network for target recognition and locate all workpieces to be gripped on the worktable;

[0010] S3. Based on the positioning information of the workpieces to be grasped obtained in S2, calculate the pose of all workpieces to be grasped relative to the camera coordinate system using the normal vector solution method.

[0011] S4. Based on the pose information of all workpieces to be grasped and the hand-eye transformation matrix, calculate the actual grasping pose of the robotic arm end for each workpiece, record all the actual grasping pose information of the workpieces, and establish it as the total workpiece pose information.

[0012] S5. Solve the inverse kinematics for the actual gripping pose of each workpiece and calculate the rotation angle of each joint of the robotic arm.

[0013] S6. Control the robotic arm to grasp the workpiece for which the inverse solution was obtained in step S5, and delete the workpiece pose information after grasping in the workpiece pose information.

[0014] S7. Solve the position that needs to be moved to grab the remaining workpiece by combining greedy algorithm and genetic algorithm, sort them according to priority, and store all the solved path points;

[0015] S8. Based on the sorting of path points in S7, perform path planning between points and control the robotic arm to grab all workpieces for which the inverse solution of each path point has been completed.

[0016] Furthermore, in step S2, the YOLOv5 network is a pre-trained network structure, and its training steps include:

[0017] S201. Take multiple images of the workpiece, perform positioning annotations, and obtain the annotated images and positioning label information;

[0018] S202. Cropping and data augmentation of the workpiece image to create a positioning dataset;

[0019] S203. Input the images and location label information in the location dataset into the YOLOV5 network to train the network weight parameters, obtain the optimized YOLOV5 network, and perform bounding box location and category recognition on the unknown workpiece image.

[0020] Furthermore, the detailed steps for calculating the pose of all workpieces to be grasped relative to the camera coordinate system in step S3 include:

[0021] S301. Obtain the pixel coordinate information of all workpieces, as well as the depth information in the camera coordinate system;

[0022] S302. Convert the pixel coordinate information of all workpieces into coordinate information in the camera coordinate system and record the position information of each workpiece.

[0023] S303. Using the X and Y vectors as the X-axis and Y-axis directions of the workpiece respectively, and taking the center coordinates as the origin of the workpiece, calculate the difference between the coordinates of the lower left corner and the upper left corner, and the coordinates of the upper right corner and the upper left corner of each workpiece.

[0024] S303. Calculate the Z-axis direction of the workpiece using the method of solving the normal vector;

[0025] S304. Solve for the rotation matrix and displacement of the workpiece relative to the camera coordinate system.

[0026] Furthermore, in step S4, based on the posture information of all workpieces and the hand-eye transformation matrix, the actual grasping pose formula of the robotic arm end effector for each workpiece is calculated:

[0027]

[0028] in This represents the pose transformation from the camera to the robotic arm base. Obtained through forward kinematics calculation; This represents the pose transformation of workpiece i to the end effector of the robotic arm;

[0029] Set the range of the worktable relative to the robot's Cartesian coordinate system as h0≤x≤h1, w0≤y≤w1.

[0030] Furthermore, the method described in step S7 for combining the greedy algorithm and the genetic algorithm to find the optimal path point after grabbing the workpiece on the workbench includes the following steps:

[0031] S701. Define chromosome: The displacement and angle changes of an autonomous mobile robot from its initial position to another position are represented as chromosome (x, y, θ), where x represents the displacement change relative to the X-axis of the robot's Cartesian coordinate system, y represents the displacement change relative to the Y-axis of the robot's Cartesian coordinate system, and θ represents the change in the robot's current direction and the angle change when moving to the next position.

[0032] S702. Define the fitness function: Define the fitness function as the number of workpieces the robot can grasp when it moves to the next position;

[0033] S703. Initialize the population: Randomly generate 100 chromosomes as the initial state of the population;

[0034] S704, Binary Tournament Selection: Randomly sample two chromosomes from the population, input each chromosome into the fitness function to obtain its fitness value, and select the one with the best fitness value as the parent.

[0035] S705. Repeat step S704 to select two parent individuals;

[0036] S706. Set the crossover probability to 0.6, randomly generate a decimal between 0 and 1, and determine whether the decimal is less than the crossover probability. If the decimal is less than the crossover probability, then perform a two-point crossover on the two parent generations selected in step S705.

[0037] If the decimal number is greater than the crossover probability, then the two parent generations will not be crossed, and the two parent generations will be added to the offspring population.

[0038] S707. Set the mutation probability to 0.3, extract one offspring from the offspring population, randomly generate a decimal between 0 and 1, and determine whether the decimal is less than the mutation probability. If the decimal is less than the mutation probability, then the offspring undergoes single-point mutation.

[0039] If the decimal number is greater than the mutation probability, then the offspring will not be mutated.

[0040] S708. Repeat the mutation operation of S707 until all offspring in the offspring population have been traversed.

[0041] S709. Repeat steps S704-S708 until the offspring population size reaches the initial population size.

[0042] S710, Update Population: Replace the original population with the newly generated offspring population;

[0043] S711, Repeat steps S704-S710 100 times;

[0044] S712. Substitute all chromosomes of the latest population into the fitness function, solve for the chromosome with the highest fitness, and record the information of that chromosome.

[0045] S713. Delete the workpiece pose information that the robot can grasp after reaching the position indicated by the chromosome from the total workpiece pose information;

[0046] S714. Repeat steps S701-S713 until the total workpiece pose information is empty.

[0047] Furthermore, the fitness function calculation method in step S702 includes:

[0048] Assuming any chromosome is (x, y, θ), calculate the rotation matrix and displacement of the robot to the position indicated by that chromosome relative to the robot's initial position:

[0049]

[0050] but

[0051]

[0052] Calculate the actual gripping pose of the robotic arm's end effector on the workpiece:

[0053]

[0054] in This indicates the pose transformation of the object and the camera after the robot moves to the position indicated by the chromosome. This indicates the pose transformation of the object and the end effector of the robotic arm after the robot moves to the position indicated by the chromosome;

[0055] The actual grasping pose information of each workpiece in the next position of the robot is obtained by calculating all workpieces according to the calculation method in step S2.

[0056] Perform step S5 and record the number of workpieces that can be grabbed.

[0057] Furthermore, the two-point intersection step in step S705 includes:

[0058] Generate two unique integers between 1 and 3.

[0059] Swap the values ​​of the two parent chromosomes at these two integer positions;

[0060] Two new chromosomes are obtained, and these two chromosomes are added to the offspring population;

[0061] The two parent generations were also added to the offspring population.

[0062] Furthermore, the single-point mutation step in the offspring in step S707 includes:

[0063] A random integer between 1 and 3 is generated to represent the mutated gene locus;

[0064] If the mutated gene position is 1 or 2, its value is randomly changed within the range of h0≥x≥h0-Δ1 or Δ1+h1≥x≥h1, w0≥y≥w0-Δ2 or Δ2+w1≥y≥w1; otherwise, the value of the gene position is randomly changed; a new offspring is obtained and replaces the current offspring.

[0065] Compared with the prior art, the present invention has the following advantages:

[0066] The YOLOv5 network model is used for target detection of workpieces. This algorithm is a single-stage target detection method, which is faster in real-time and has excellent results. The algorithm identifies all workpieces in the image by drawing bounding boxes and detects their positions in the pixel coordinate system.

[0067] A simple mathematical model is used to estimate the pose of the workpiece. The mathematical model includes solving for the X-axis direction vector, Y-axis direction vector and normal vector of the workpiece.

[0068] By combining greedy and genetic algorithms, the system effectively searches for locations that can capture the maximum number of objects, enabling efficient and rapid capture in environments with a large number of dense target objects, thus improving the efficiency of mobile capture. Attached Figure Description

[0069] Figure 1 shows the overall process of the robotic arm moving and grasping in an embodiment of the present invention;

[0070] Figure 2 is a flowchart of the initial position capture process in an embodiment of the present invention;

[0071] Figure 3 is a schematic diagram of the robot's initial position and the workbench in an embodiment of the present invention;

[0072] Figure 4 is a schematic diagram of five coordinates of workpiece pose information in an embodiment of the present invention;

[0073] Figure 5 is a schematic diagram of the annotation of the Z-axis normal vector of the workpiece side view in an embodiment of the present invention;

[0074] Figure 6 is a visualization of the training results in the YOLOv5 network in an embodiment of the present invention.

[0075] The markings in the diagram are as follows: L1 - top left corner coordinates, L2 - bottom left corner coordinates, L3 - top right corner coordinates, L4 - bottom right corner coordinates, L5 - center coordinates, L - recognition box. Detailed Implementation

[0076] To facilitate understanding by those skilled in the art, the present invention will be further described below with reference to embodiments and accompanying drawings. The content mentioned in the embodiments is not intended to limit the present invention.

[0077] As shown in Figures 1, 2, and 3, this embodiment provides a method for autonomous robot movement and grasping in a large-scale, densely packed block environment, including:

[0078] S1. Control the movement of the autonomous mobile robot and its built-in robotic arm so that the camera at the end of the robotic arm can capture the complete position of all workpieces on the worktable, establish it as the initial position, capture the image, and establish the image as the global image.

[0079] S2. Input the global image into the YOLOv5 network for target recognition and locate all workpieces to be gripped on the worktable;

[0080] S3. Using the workpiece positioning information obtained in S2, estimate the pose of all workpieces to be grasped relative to the camera coordinate system by solving the normal vector mathematical method.

[0081] S4. Based on the posture information of all workpieces and the hand-eye transformation matrix, calculate the actual grasping posture of the robotic arm end for each workpiece, record all the actual grasping posture information of the workpieces, and establish it as the total workpiece posture information.

[0082] S5. Solve the inverse kinematics for the actual gripping pose of each workpiece and calculate the rotation angle of each joint of the robotic arm; if the inverse kinematics for the actual gripping pose of the workpiece cannot be solved, record the pose information of the workpiece.

[0083] S6. Control the robotic arm to grab all workpieces for which inverse kinematics can be solved and place them in the storage box of the autonomous mobile robot, and remove the workpiece pose information of the grabbed workpieces from the total workpiece pose information.

[0084] S7. Combine the greedy algorithm and the genetic algorithm to solve the position that needs to be moved to grab the remaining workpieces and sort them according to priority, so that the autonomous mobile robot can achieve the current maximum number of grabs every time it moves. Record all the solved path points.

[0085] S8. According to the order of the path points in S7, perform path planning between the points, and at each path point, control the robotic arm to grab the workpiece that can be solved inversely.

[0086] The YOLOv5 network is a pre-trained network structure, and the training steps include:

[0087] S201. Take multiple images of the workpiece, perform positioning annotations, and obtain the annotated images and positioning label information;

[0088] S202. Cropping and data augmentation of the workpiece image to create a positioning dataset;

[0089] S203. Input the images and location label information in the location dataset into the YOLOV5 network for training, train the network weight parameters, and obtain the optimized YOLOV5 network. The trained network is used to perform bounding box location and category recognition on images of unknown workpieces.

[0090] The above location markings are as follows:

[0091] Outline the outer contour of the workpiece in the image;

[0092] Note the location tag information;

[0093] The method for estimating the pose of all workpieces to be grasped relative to the camera coordinate system, as described in S3, comprises the following steps:

[0094] S301. Obtain the pixel coordinate information of all workpieces, as well as the depth information in the camera coordinate system; the pixel coordinate information of the workpieces includes: the pixel coordinates of the four vertices and the center of the workpiece recognition box; the depth information in the camera coordinate system includes: the depth information of the four vertices and the center of the workpiece recognition box.

[0095] S302. Convert the pixel coordinates of all workpieces into coordinates in the camera coordinate system and record the position information of each workpiece: the coordinates of the upper left corner L1 of the workpiece recognition box L are... The coordinates of the lower left corner L2 are The coordinates of the upper right corner L3 are The coordinates of the bottom right corner L4 are The center coordinate L5 is i represents the workpiece number;

[0096] S303. Calculate the difference between the coordinates of the lower left corner L2 and the upper left corner L1, and the coordinates of the upper right corner L3 and the upper left corner L1 for each workpiece.

[0097] The X and Y vectors are taken as the X-axis and Y-axis directions of the workpiece, respectively, and the center coordinates are taken as the origin of the workpiece.

[0098] S303 calculates the Z-axis direction of the workpiece using a mathematical method that solves for the normal vector. Assume the Z-axis direction of a certain workpiece is...

[0099]

[0100] but

[0101]

[0102] The unit vector of the workpiece in the Z-axis direction can be obtained by solving the above equations;

[0103] S304. Solve for the rotation matrix and displacement of the workpiece relative to the camera coordinate system.

[0104]

[0105] but

[0106]

[0107] In step S4, based on the posture information of all workpieces and the hand-eye transformation matrix, the actual grasping pose of the robotic arm end effector for each workpiece is calculated:

[0108]

[0109] in This represents the pose transformation from the camera to the robotic arm base. Obtained through forward kinematics calculation; This represents the pose transformation of workpiece i to the end effector of the robotic arm;

[0110] Set the range of the worktable relative to the robot's Cartesian coordinate system as h0≤x≤h1, w0≤y≤w1;

[0111] The method described in step S7, which combines the greedy algorithm and the genetic algorithm to find the optimal path point after grabbing the workpiece on the workbench, is as follows:

[0112] S701. Define chromosome: The displacement and angle changes of an autonomous mobile robot from its initial position to another position are represented as chromosome (x, y, θ), where x represents the displacement change relative to the X-axis of the robot's Cartesian coordinate system, y represents the displacement change relative to the Y-axis of the robot's Cartesian coordinate system, and θ represents the change in the robot's current direction and the angle change when moving to the next position.

[0113] S702. Define the fitness function: The fitness function is used to evaluate the quality of a chromosome. The fitness function is defined as the number of workpieces the robot can grasp when it moves to the next position. Specifically, for each chromosome, calculate the number of workpieces the robotic arm can grasp after the robot reaches the position indicated by that chromosome.

[0114] The fitness function defined above is calculated as follows:

[0115] 1. Assuming any chromosome is (x, y, θ), calculate the rotation matrix and displacement of the robot to the position indicated by that chromosome relative to the robot's initial position:

[0116]

[0117] but

[0118]

[0119] 2. Calculate the actual gripping posture of the robotic arm's end effector on the workpiece:

[0120]

[0121] in This indicates the pose transformation of the object and the camera after the robot moves to the position indicated by the chromosome. This indicates the pose transformation of the object and the end effector of the robotic arm after the robot moves to the position indicated by the chromosome;

[0122] 3. Obtain the actual grasping pose information of each workpiece after the robot moves to the new position by calculating all workpieces according to the calculation method in step 2;

[0123] 4. Execute step S5 and record the number of workpieces that can be grabbed;

[0124] S703. Initialize the population: Randomly generate 100 chromosomes as the initial state of the population. Here, randomness refers to randomness within certain constraints, namely: h0≥x≥h0-Δ1 or Δ1+h1≥x≥h1, w0≥y≥w0-Δ2 or Δ2+w1≥y≥w1, where Δ1 and Δ2 are the movement constraints of the robot;

[0125] S704, Binary Tournament Selection: Randomly sample two chromosomes from the population (sampling with replacement), input each chromosome into the fitness function to obtain its fitness value, and then select the best one as the parent;

[0126] S705. Repeat step S704 to select two parent individuals;

[0127] S706. Set the crossover probability to 0.6. Randomly generate a decimal between 0 and 1. If the decimal is less than the crossover probability, then perform a two-point crossover on the parent generation 1 (x1, y1, θ1) and parent generation 2 (x2, y2, θ2) selected in step S705. The steps are as follows:

[0128] Generate two unique integers between 1 and 3.

[0129] Swap the values ​​of the two parent chromosomes at these two integer positions;

[0130] Two new chromosomes are obtained, and these two chromosomes are added to the offspring population;

[0131] Add the two parent generations to the offspring population;

[0132] If the decimal number is greater than the crossover probability, then the two parent generations will not be crossed, and the two parent generations will be added to the offspring population.

[0133] S707. Set the mutation probability to 0.3, extract one offspring from the offspring population, and randomly generate a decimal between 0 and 1. If this decimal is less than the mutation probability, then the offspring undergoes single-point mutation. The steps are as follows:

[0134] A random integer between 1 and 3 is generated to represent the mutated gene locus;

[0135] If the mutated gene position is 1 or 2, then its value is randomly changed within the range of h0≥x≥h0-Δ1 or Δ1+h1≥x≥h1, w0≥y≥w0-Δ2 or Δ2+w1≥y≥w1; otherwise, the value of the gene position is randomly changed.

[0136] Obtain new offspring and replace the current offspring;

[0137] If the decimal number is greater than the mutation probability, then the offspring will not be mutated.

[0138] S708 repeats the mutation operation of S707 until all offspring in the offspring population have been traversed.

[0139] S709 repeats steps S704-S708 until the offspring population size reaches the initial population size;

[0140] S710 Update Population: Replaces the original population with the newly generated offspring population;

[0141] S711, Repeat steps S704-S710 100 times;

[0142] S712. Substitute all chromosomes of the latest population into the fitness function, solve for the chromosome with the highest fitness, and record the information of that chromosome.

[0143] S713. Remove the workpiece pose information that the robot can grasp after reaching the position indicated by the chromosome from the total workpiece pose information;

[0144] S714. Repeat steps S701-S712 until the total workpiece pose information is empty.

[0145] This paper presents a deep learning-based object detection algorithm for workpiece recognition and detection. A vision-based grasping algorithm is used in a scenario with many block-shaped objects on a workbench and an autonomous mobile robot (with a robotic arm). The algorithm takes the visual information of the scene as input, calculates the optimal grasping pose for each workpiece from the robot's initial position, and determines the number of workpieces that can be grasped at the current position by inverting the rotation angles of the robotic arm joints. Since the robot cannot grasp all workpieces from its current position, a combination of greedy and genetic algorithms is used to find the minimum number of movement paths for the robot. The robot then grasps the workpieces that can be grasped at each path until all workpieces are grasped. A detailed explanation follows.

[0146] Target detection section:

[0147] Self-made object detection dataset: Take multiple photos of the object to be captured using a camera, with a total of 300 photos. Use Labelme software to annotate the workpieces in the photos and add corresponding labels. After the annotation is completed, export the dataset, which includes annotated images and a text file corresponding to each annotated image, and convert the data in the text files into YOLO format.

[0148] Model training: 10% of the labeled dataset is allocated to the validation set, and the remaining portion is allocated to the training set. The training and validation sets are used for training. After 300 epochs of training, the trained weight files and some training process files can be found in the runs folder.

[0149] As shown in Figure 6, objectness and valobjectness represent the mean object detection loss of the training set and validation set, respectively; classification and valclassification represent the mean classification loss of the training set and validation set, respectively; GIOU and valGIOU represent the mean GIOU loss function of the training set and validation set, respectively; and precision and recall represent the precision and recall of the training set, respectively.

[0150] After 300 training iterations, the model achieved convergence. During model validation, its accuracy reached a high level. The average confidence level for target detection on workpiece images reached 0.9, and the overall training effect met expectations.

[0151] After the model is successfully trained, two weight files will be generated in the weights folder. The best weight file is best.pt, and the most recently trained weight file is last.pt.

[0152] Testing the training model: Modify the `detect.py` file, adding a path to save the test results. The test results can then be found in the `images` folder, and the saved test results can be viewed in the `output` folder. The test results are shown in Figure 2; the selected area contains the workpiece to be detected.

[0153] The block-shaped workpieces are neatly placed on the worktable, and RGBD images are captured using a RealSense D435i depth camera to obtain the position and depth information of all workpieces, which are then saved in a txt file.

[0154] Workpiece pose information estimation part:

[0155] As shown in Figure 4, the coordinates L1 of the upper left corner of the workpiece i recognition box L are: The coordinates of the lower left corner L2 are The coordinates of the upper right corner L3 are The coordinates of the bottom right corner L4 are The center coordinate L5 is i represents the workpiece number;

[0156] Calculate the differences between the coordinates of the lower left corner (L2) and the upper left corner (L1) for each workpiece, and between the coordinates of the upper right corner (L3) and the upper left corner (L1).

[0157] The X and Y vectors are taken as the X-axis and Y-axis directions of the workpiece, respectively, and the center coordinates are taken as the origin of the workpiece.

[0158] As shown in Figure 5, the Z-axis direction of the workpiece is calculated using a mathematical method to solve for the normal vector. Assume the Z-axis direction of a certain workpiece is...

[0159]

[0160] but

[0161]

[0162] The unit vector of the workpiece in the Z-axis direction can be obtained by solving the above equations;

[0163] Solve for the rotation matrix and displacement of the workpiece relative to the camera coordinate system.

[0164]

[0165] but

[0166]

[0167] The robot's grasping pose calculation at the initial position:

[0168] Based on the posture information of all workpieces and the hand-eye transformation matrix, the actual grasping pose of the robotic arm end effector for each workpiece is calculated:

[0169]

[0170] in This represents the pose transformation from the camera to the robotic arm base. Obtained through forward kinematics calculation; This represents the pose transformation from workpiece i to the end effector of the robotic arm. In ROS, the Moveit open-source package can be used to perform hand-eye calibration on the robotic arm and camera. By inputting the workpiece's pose information into this package, the actual grasping pose of the workpiece can be calculated.

[0171] The aubo robotic arm is used to grasp workpieces within its reach at the current position. The grasping process is shown in Figure 3. The autonomous mobile robot and its onboard robotic arm are controlled to move, ensuring the camera at the end of the robotic arm can capture a complete view of all workpieces on the worktable. After starting the robotic arm and camera, the RealSense D435i camera and robotic arm are initialized, the grasping algorithm is initialized, and images are captured and established as the global image. The global image is then input into the YOLOv5 network for target recognition, locating all workpieces to be grasped on the worktable. The grasping algorithm is used to estimate the actual grasping pose of each workpiece, and the grasping pose information of each workpiece is saved. In the overall workpiece pose information, the inverse solution of the actual grasping pose of each workpiece is calculated. In ROS, the Moveit open-source package can be used to calculate the inverse solution of the actual grasping pose of the workpiece, and the workpiece information that can be inverted is extracted. The robotic arm is controlled to grasp the workpieces that can be grasped and place them in the robot's storage box. Then, the workpiece information that has been grasped is deleted from the overall workpiece pose information, and the robot returns to the initial pose to prepare to grasp the next workpiece, until all the workpieces that can be grasped are grasped.

[0172] Obtain the optimal route for mobile crawling:

[0173] The overall process of the robotic arm's movement and grasping is shown in Figure 4. After the robot has grasped all the workpieces it can from the initial position, as shown in Figure 5, the range of the worktable relative to the current robot's Cartesian coordinate system is set as h0≤x≤h1, w0≤y≤w1; where h0, h1, w0, and w1 are the distances of the worktable's closest edge to the robot relative to the y-axis of the robot's Cartesian coordinate system, the distance of the worktable's farthest edge from the robot relative to the y-axis of the robot's Cartesian coordinate system, the distance of the worktable's closest edge to the robot relative to the x-axis of the robot's Cartesian coordinate system, and the distance of the worktable's farthest edge from the robot relative to the x-axis of the robot's Cartesian coordinate system, respectively. A genetic algorithm is used to optimize the movement and grasping path points until the iteration termination condition is met, obtaining the optimal path point position with the optimal fitness value. Then, a greedy algorithm is used again, and the genetic algorithm is used to find the next optimal path point until all the workpieces on the worktable are grasped. The obtained path points are sorted according to the order obtained by the greedy algorithm, as shown in Table 1 below.

[0174] Table 1

[0175] Path point number xyθ1305026010303204090480100 surface

[0176] The AGV is controlled by a handle to quickly build a map. This involves scanning the surrounding environment with a LiDAR scanner, using the FastSLAM algorithm to construct an indoor environment map model, and using the Movebase open-source package in ROS to plan the path for the 1-2-3-4 path points. After reaching each path point, the robotic arm grabs the workpieces that can be grabbed. After grabbing all the workpieces, the robot returns to its initial position.

[0177] The above provides a detailed description of a robot's autonomous movement and grasping method in a large-scale, densely packed block environment. The specific embodiments described are merely for the purpose of helping to understand the method and its core ideas. It should be noted that those skilled in the art can make various improvements and modifications to this application without departing from its principles, and these improvements and modifications also fall within the protection scope of the claims.

Claims

1. A method for autonomous robot movement and grasping in environments with large-scale, densely packed blocky objects, characterized in that, The steps include: S1, controlling the robotic arm of the autonomous mobile robot to move with a camera to acquire the initial position and global image of all workpieces on the worktable; S2, inputting the global image into the YOLOv5 network for target recognition to locate all workpieces to be grasped on the worktable. S3. Based on the positioning information of the workpieces to be grasped obtained in S2, calculate the pose of all workpieces relative to the camera coordinate system using the normal vector solution method, including: S301. Obtaining the pixel coordinate information of all workpieces and their depth information in the camera coordinate system; S302. Converting the pixel coordinate information of all workpieces into coordinate information in the camera coordinate system, and recording the position information of each workpiece: the coordinates of the upper left corner L1 of the workpiece recognition box L are... The coordinates of the lower left corner L2 are The coordinates of the upper right corner L3 are The coordinates of the lower right corner L4 are The center coordinates L5 are ， Indicates the workpiece number; S303, has: Using the X and Y vectors as the X-axis and Y-axis directions of the workpiece, respectively, and taking the center coordinates as the origin of the workpiece, calculate the differences between the coordinates of the lower left and upper left corners, and the coordinates of the upper right and upper left corners of each workpiece; S304. Calculate the Z-axis direction of the workpiece using the normal vector solution method; S305. Solve for the rotation matrix and displacement of the workpiece relative to the camera coordinate system; S4. Based on the pose information of all workpieces to be grasped and the hand-eye transformation matrix, calculate the actual grasping pose of the robotic arm end effector for each workpiece, calculated as follows: in This represents the pose transformation from the camera to the robotic arm base. Obtained through forward kinematics calculation; Indicates workpiece Pose transformation to the end effector of the robotic arm; setting the range of the worktable relative to the robot's Cartesian coordinate system as follows: Record all actual workpiece grasping pose information and establish it as the total workpiece pose information; S5, calculate the inverse solution of the actual grasping pose of each workpiece and calculate the rotation angle of each joint of the robotic arm; S6, control the robotic arm to grasp the workpiece obtained from the inverse solution in step S5, and delete the grasped workpiece pose information from the total workpiece pose information; S7, solve the position required to grasp the remaining workpieces by combining greedy algorithm and genetic algorithm, sort them according to priority, and store all the solved path points, including: S701, define chromosome: the displacement change and angle change of the autonomous mobile robot from the initial position to another position are used as chromosomes, represented as ,in This represents the change in displacement relative to the X-axis of the robot's Cartesian coordinate system. This represents the change in displacement relative to the robot's Cartesian coordinate system along the Y-axis. S702. Define the fitness function: Define the fitness function as the number of workpieces the robot can grasp when it moves to the next position; S703. Initialize the population: Randomly generate 100 chromosomes as the initial state of the population; S704. Binary tournament selection: Randomly sample two chromosomes from the population, input each chromosome into the fitness function to obtain its fitness value, and select the one with the best fitness value as the parent; S705. Repeat step S704 to select two parent individuals; S706. Set the crossover probability to 0.6, and randomly generate a decimal between 0 and 1. S707. Determine if the decimal number is less than the crossover probability. If it is, perform a two-point crossover on the two parents selected in step S705. If the decimal number is greater than the crossover probability, do not crossover these two parents and add them to the offspring population. S708. Set the mutation probability to 0.3, extract one offspring from the offspring population, randomly generate a decimal number between 0 and 1, and determine if the decimal number is less than the mutation probability. If it is, perform a single-point mutation on the offspring. If the decimal number is greater than the mutation probability, do not mutate the offspring. S709. Repeat the mutation operation in S701 until all offspring in the offspring population have been traversed. Offspring; S709, Repeat steps S704-S708 until the offspring population size reaches the initial population size; S710, Update the population: Replace the original population with the newly generated offspring population; S711, Repeat steps S704-S710 100 times; S712, Substitute all chromosomes of the latest population into the fitness function, solve for the chromosome with the highest fitness, and record the information of this chromosome; S713, Delete the workpiece pose information that the robot can grasp after reaching the position indicated by this chromosome from the total workpiece pose information; S714, Repeat steps S701-S713 until the total workpiece pose information is empty; S8, According to In step S7, the path points are sorted, path planning between points is performed, and the robotic arm is controlled to grasp all workpieces that have completed the inverse solution of each path point. In step S2, the YOLOV5 network is a pre-trained network structure. Its training steps include: S201, taking multiple workpiece images, performing localization annotation, and obtaining the annotated images and localization label information; S202, cropping and data augmenting the workpiece images to create a localization dataset; S203, inputting the images and localization label information in the localization dataset into the YOLOV5 network to train the network weight parameters, obtaining the optimized YOLOV5 network, and performing bounding box localization and category recognition on unknown workpiece images.

2. The robot autonomous movement and grasping method according to claim 1, characterized in that, The fitness function calculation method in step S702 includes: assuming any chromosome is... Calculate the rotation matrix and displacement of the robot to the position indicated by the chromosome relative to the robot's initial position: but Calculate the actual gripping pose of the robotic arm's end effector on the workpiece: in This indicates the pose transformation of the object and the camera after the robot moves to the position indicated by the chromosome. This indicates the pose transformation of the object and the end effector of the robotic arm after the robot moves to the position indicated by the chromosome; obtain the actual grasping pose information of each workpiece in the next position of the robot according to the calculation method in step S2; execute step S5 and record the number of workpieces that can be grasped.

3. The robot autonomous movement and grasping method according to claim 1, characterized in that, The two-point crossover step in step S705 includes: randomly generating two unique integers from 1 to 3; swapping the values ​​of the two parent chromosomes at these two integer positions; obtaining two new chromosomes; adding these two chromosomes to the offspring population; and adding the two parent chromosomes to the offspring population.

4. The robot autonomous movement and grasping method according to claim 1, characterized in that, Step S707, the single-point mutation step in the offspring, includes: randomly generating an integer between 1 and 3 as the mutated gene locus; if the mutated gene locus is 1 or 2, then... The value is randomly changed within a certain range; otherwise, the value of the gene locus is randomly changed. and Limit the robot's movement range; obtain new offspring and replace the current offspring.

Images