A 6D pose grasping method for robotic arm based on neural network
Through the improved YOLOV5 algorithm and PVN3D-Tiny network, 6D pose information is calculated, combined with seven-degree polynomial trajectory planning and genetic algorithm, the problem of robotic arms grabbing and avoiding obstacles in complex scenarios is solved, and precise grasping and optimization are achieved The effect of the motion trajectory.
Patent Information
- Application Number
- CN202311277997.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-09-28
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2043-09-28
AI Technical Summary
The existing 6D pose estimation method based on YOLO has problems such as high accuracy but weak generalization ability and poor real-time performance in the robotic arm grasping task. The traditional method has failed to effectively combine the robotic arm trajectory obstacle avoidance planning, resulting in It is difficult to achieve accuracy in complex scenarios and obstacle avoidance.
The improved YOLOV5 algorithm network combined with the PVN3D-Tiny network is used to calculate 6D pose information through singular value decomposition, and combine seven-order polynomial trajectory planning and genetic algorithm to perform robotic arm obstacle avoidance planning, optimize the motion trajectory to achieve accurate grasping and obstacle avoidance. .
It improves the accuracy and speed of object detection, enhances the real-time performance of position estimation, and ensures that the robotic arm can accurately grasp objects and avoid obstacles in complex scenarios, maximizing the motion performance of the robotic arm.
Smart Images

Figure CN117315025B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the field of automation technology, and in particular relates to a 6D posture grasping method of a robot arm based on a neural network. Background Art
[0002] In recent years, target detection algorithms have developed rapidly. At present, the main target detection algorithms are divided into two categories: 1) two-stage target detection algorithms, including R-CNN, Fast R-CNN, Faster R-CNN and Mask R-CNN; 2) one-stage target detection methods, including RetinNet, SSD (Single Shot Multi-box Detector, SSD), YOLO (You Only LookOnce, YOLO) series. The YOLO target detection algorithm includes the YOLOV1~YOLOV8 series of algorithms and a series of improved YOLO target detection algorithms. It has attracted widespread attention from scholars due to its excellent performance and effective balance between accuracy and speed. However, there are still problems such as too large number of parameters, high computational cost and high rate of missed detection of small targets. Some scholars have proposed an improved YOLOV3 model to address the complex background problems in small target detection. This model increases the prediction scale from 3 to 4 and adjusts the input image size to 608x608 to retain more image details. With the development of target detection technology, many scholars have applied the YOLO algorithm to 6D pose estimation of targets. Some scholars have improved YOLOV2 and proposed a deep convolutional network framework, which directly predicts the 2D projection of the 3D bounding box vertices of the target object, and then uses the PnP algorithm to estimate the 6D pose information of the target object. Some scholars have proposed a two-stage method based on YOLOV3 and FFB6D and achieved the best results in 6D pose estimation accuracy, but there are still too many parameters and high computational cost.
[0003] 6D pose estimation methods based on deep learning can be divided into two categories: one-stage estimation methods and two-stage estimation methods. The one-stage estimation method directly obtains the 6D pose information of the target object from the input image based on regression. Some scholars proposed a 6D pose estimation network PoseCNN based on convolutional neural network, which determines the center position of the target object in the image through Hough voting, and then calculates the 3D translation and 3D rotation matrix of the target object, and finally uses the ICP (Iterative Closest Point, ICP) algorithm for optimization. However, this method is time-consuming and cannot be optimized end-to-end. Subsequently, some scholars proposed a deep learning network Deep-6D Pose that can be optimized end-to-end, which can obtain the 6D pose information of the target object from a single RGB image. Compared with the two-stage method, the one-stage method directly outputs 6D pose information by regression. However, for the rotation matrix in pose estimation, the two-stage method is more accurate. Some scholars predict the 2D image coordinates and 3D space coordinates of the target through RGB images, and then use the PnP algorithm to calculate the target pose information, which has good real-time performance. However, if the target is low-texture or low-resolution, its prediction accuracy will be greatly reduced. Some scholars apply convolutional neural networks (CNNs) to detected objects, predict the 3D bounding box vertices of the target object and project them in a 2D image, and then use the PnP algorithm to obtain 6D pose information. However, when the object is severely occluded, the prediction accuracy will be greatly reduced. In response to the problem of occlusion and truncation of key points, some scholars proposed a pixel-level voting network PVNet pose estimation method, which provides a new representation method for occluded or truncated key points, but its real-time performance is poor when applied to robotic arm grasping tasks.
[0004] In addition to accurately identifying the target object when the robot performs a task, another core issue is how to achieve autonomous obstacle avoidance of the robot in complex scenarios. Some scholars have proposed an obstacle avoidance method based on the cylindrical bounding box model. This method extracts the segmented description equation of the ideal robot trajectory, sets the intermediate unknown quantity of the segmented trajectory as the undetermined parameter, and achieves the purpose of obstacle avoidance of the robot through parameter optimization. Some scholars have proposed an improved inverse kinematics solution method combining analytical solution and numerical solution. This method models the robot and the obstacle in the form of an ellipsoid and calculates the minimum distance between the robot link and the obstacle. This method improves the solution accuracy and speed, but there is still a singular point problem. Some scholars use the line segment sphere envelope box model for collision detection and adopt the artificial field method to solve the problem that the robot is prone to fall into local minima when avoiding obstacles. The Gaussian function is introduced to establish a virtual gravitational potential field to deal with the local minima problem. In addition, genetic algorithms, fast random search tree algorithms, A* algorithms and other methods are also used for robot obstacle avoidance trajectory planning. Such methods can effectively avoid obstacles, but there are few studies that can take into account both obstacle avoidance and kinematic performance optimization.
[0005] In summary, the 6D pose estimation method based on YOLO has high accuracy and strong robustness, but it still has problems such as weak generalization ability and low real-time performance. In the robotic arm grasping task, the traditional 6D pose estimation method does not perform trajectory obstacle avoidance planning for the robotic arm after calculating the 6D pose information of the target. Therefore, in order to achieve the precise grasping of objects and obstacle avoidance tasks of the robotic arm in complex scenes, the present invention proposes an improved 6D pose grasping method for the robotic arm based on neural network. Summary of the invention
[0006] In view of the above technical problems, the present invention provides a 6D posture grasping method of a robotic arm based on a neural network.
[0007] The technical solution adopted by the present invention to solve the technical problem is:
[0008] A 6D posture grasping method of a robot arm based on a neural network, the method comprising the following steps:
[0009] S100: Acquire RGB video sequence and depth video sequence respectively through industrial depth camera;
[0010] S200: Build an improved YOLOV5 algorithm network, which includes a backbone network, a network layer and a prediction layer. The backbone network is used for feature extraction, the network layer is used for processing the features extracted by the backbone network, and the prediction layer is used for predicting the processed features to obtain an output feature map.
[0011] S300: training the improved YOLOV5 algorithm network and the preset PVN3D-Tiny network based on a preset loss function to obtain a trained improved YOLOV5 algorithm network and a preset trained PVN3D-Tiny network;
[0012] S400: Sending the RGB video sequence to the trained improved YOLOV5 algorithm network to obtain an output feature map, wherein the output feature map includes the ROI area of the target to be captured;
[0013] S500: Based on the ROI area of the target to be grasped in the output feature map and the depth video sequence, the preset trained PVN3D-Tiny network is used to obtain the predicted key points of the surface of the target to be grasped, and the singular value decomposition method is used to calculate the 6D pose information of the target to be identified;
[0014] S600: Obtain obstacle information, obtain a planned trajectory based on the 6D pose information of the target to be identified by combining a seventh-order polynomial trajectory planning algorithm and a genetic algorithm, perform obstacle avoidance with the robot arm according to the planned trajectory and obstacle information to obtain an optimal trajectory, and achieve obstacle avoidance and grasping of the target.
[0015] Preferably, the backbone network in S200 includes a CBS module, a first GhostConv module, a first G3Ghost module, a second GhostConv module, a second G3Ghost module, a third GhostConv module, a third G3Ghost module, a fourth GhostConv module, a CBAM attention mechanism module and an SPP module connected in sequence; the network layer includes two convolutional layers, and the prediction layer includes three parallel CBS modules.
[0016] Preferably, the first G3Ghost module includes a Ghost Bottleneck module, a connection module and three CBS modules, any half of the feature map in the input feature map is generated through a CBS module and Ghost Bottleneck to generate a feature map, and the remaining other half of the feature map in the input feature map is generated through another CBS module connected in parallel with a CBS module and Ghost Bottleneck to generate a feature map, and then the two parts of the feature map are spliced and passed through the last CBS module to obtain the output of the first G3Ghost module;
[0017] The Ghost bottlenecks module consists of two stacked GhostConvs. The first GhostConv acts as an expansion layer to increase the number of channels. The second GhostConv is used to reduce the number of channels of the output feature map to match the number of input channels and perform an addition operation with the input feature map transmitted from the residual edge.
[0018] Preferably, the loss function preset in S300 is specifically:
[0019] L=λ1L obj +λ2L reg +λ3L cls +λ4L key +λ5L pose
[0020] Where λ1, λ2, λ3, λ4 and λ5 are the scaling factors of each loss during training, and L obj , L cls and L reg They are target loss function, classification loss function and regression loss function respectively, L key is the key point selection loss function, L pose is the pose loss function, the target loss function L obj The Focal cross entropy loss function is used to measure the difference between it and the true value, which is defined as follows:
[0021]
[0022] Where N obj is the total number of video frame targets, ys is the label of the test sample s, the foreground is marked as 1, the background is marked as 0, and d s The probability of predicting the test sample s as a prospect;
[0023] Regression loss function L reg The CIoU loss function is used to measure the overlap area, center point distance and aspect ratio between the predicted box and the real box. The CIoU value range is [-1, 1]. The regression loss function L reg The definition is as follows:
[0024] L reg =1-CIoU
[0025] In order to judge the difference between the category to which the target belongs and the true value, the classification loss function uses the multi-classification cross entropy loss function to score the prediction of the category to which the target belongs, L cls The definition is as follows:
[0026]
[0027] Where N cls is the total number of video frame targets, M cls is the number of categories, y oc Symbol function, if the true category of the current frame target o is equal to c, y oc 1, otherwise 0, d oc is the predicted probability that the current frame target o belongs to the real category c. The key point selects the loss function L key The definition is as follows:
[0028]
[0029] Where N key is the total number of points detected, M key is the total number of key points, T i j is the translation offset from the i-th visible point to the j-th keypoint, is the distance from the i-th visible point to the j-th visible point * The true translation offset of the key points, the pose loss function L pose The definition is as follows:
[0030]
[0031] Where τ is the balance parameter that controls the rotation and translation regression errors, is the rotation angle obtained by regression, is the true value of the rotation angle label, is the translation vector obtained by regression, is the label truth data of the translation vector.
[0032] Preferably, S500 includes:
[0033] S510: Cropping the output feature map and the depth video sequence into a minimum square centered on and including the ROI area, using the nearest neighbor interpolation method to adjust the size of the cropped square image to a preset size, and after preprocessing, obtaining two RGB images and point cloud images of the preset size and sending them to the trained preset PVN3D-Tiny network;
[0034] S520: The trained preset PVN3D-Tiny network uses PointNet++ and PSPNet as the backbone network to extract RGB and point cloud feature information, and then uses DenseFusionNet to perform iterative dense fusion of global features on the obtained feature information to obtain a fused output feature map, and performs key point selection on the fused output feature map to obtain the key points of the target object and obtain the predicted key points;
[0035] S530: Determine the key points of prediction using singular value decomposition and the reference key points of the target object {kp i} is the 6D pose matrix Rt between them.
[0036] Preferably, in S520, key point selection is performed on the fused output feature map to obtain key points of the target object, and the predicted key points are obtained, including:
[0037] S521: Use the point-by-point segmentation method to filter out the background points of the target to be identified in the fused output feature map to obtain candidate key points {of i}∈R 3 ;
[0038] S522: Define candidate key points {of i} and the reference key point {kp i}∈R 3 The average prediction distance μ and standard deviation σ between them, at this stage, filter out any candidate key points whose difference from the average prediction distance μ exceeds the standard deviation σ, that is, when |of i When -μ|>σ, the candidate key point will be removed to obtain the predicted key point of the target object.
[0039] Preferably, S600 includes:
[0040] S610: Obtain the obstacle position, input the 6D pose information of the target to be identified, and generate a set of initial populations P by the genetic algorithm generation function iAccording to the initial parent K value combined with the seventh-order polynomial trajectory planning algorithm, the planned trajectory is obtained. The planned trajectory and obstacle information are combined to calculate the collision detection result f CO , the sum of the changes in the angles of each joint f Q , the sum of the trajectory lengths of the robot end f L The sum of the joint torques exceeds the set value f TO , and then find the fitness function f K ,
[0041] S620: Obtain offspring through crossover and mutation of genetic algorithm, and determine P after multiple iterations i With the fitness function f K Whether it is close or not, after reaching the termination condition, determine the optimal parameter K that is closest to the fitness, and finally substitute it into the seventh-order polynomial to obtain the optimal trajectory to achieve obstacle avoidance and grasping of the target.
[0042] Preferably, S620 further includes: when the termination condition is not met, adjusting the parent generation K value, and returning to S610 to obtain a planned trajectory in combination with a seventh-order polynomial trajectory planning algorithm, until the termination condition in S620 is met.
[0043] Preferably, the seventh-order polynomial is specifically:
[0044] θ(t)=c0+c1t+c2t 2 +c3t 3 +c4t 4 +c5t 5 +c6t 6 +Kt 7
[0045] Where K is an adjustable parameter, θ = [θ1, θ2, θ3, θ4, θ5, θ6] is a 6-dimensional vector, and θ 1~6 They are the six joint angles of the robot arm, and c0~c6 are the coefficients to be determined;
[0046] Assume that the robot starts at time t0 and ends at time t f The angular displacement, angular velocity, angular acceleration and angular impact degree are and Substituting the above 8 variables as initial conditions into the above formula and taking the derivative, we can find the trajectory functions of each joint at the start and end of the robot arm:
[0047]
[0048]
[0049]
[0050]
[0051]
[0052]
[0053]
[0054] When the parameter K is given, the unknown coefficients c0~c6 can be solved by the above formula, and then the motion trajectory of the robot arm with continuous angular displacement, angular velocity, angular acceleration and angular impact degree of each joint is planned.
[0055] Preferably, the fitness function in S610 is specifically:
[0056]
[0057] In the formula, f CO is the collision detection result, collision is 0, no collision is 1; η i is the weighting factor, f Q is the sum of the changes in the angles of each joint, f L is the sum of the trajectory lengths of the end of the robot arm, f TO The torque of each joint exceeds the sum of the set values;
[0058] The seventh-order polynomial trajectory planning algorithm divides the planned motion trajectory into i small segments. The spatial position coordinates of the end of the robot arm in the i-1th segment are (p i-1,x ,p i-1,y ,p i-1,z ), so the distance between each position coordinate is the end trajectory length f L , the calculation formula is:
[0059]
[0060] The sum of joint angle changes f Q It is the accumulation of the absolute value of the difference between the joint angles of the six joints in the i-th and i-1-th segments of the trajectory. The calculation formula is:
[0061]
[0062] Where q i,j is the jth joint angle value of the i-th trajectory;
[0063] The torque of each joint exceeds the sum of the set values f TO The calculation formula is:
[0064]
[0065] Where δ i,j is the i-th trajectory and j-th joint torque, is the torque limit of the jth joint.
[0066] The above-mentioned neural network-based 6D pose grasping method of a robotic arm, (1) in order to improve the target detection accuracy and speed, an improved YOLOV5 algorithm network is proposed to improve the network feature extraction capability; (2) in order to improve the pose estimation speed, the PVN3D-Tiny network is used to calculate the 6D pose information of the target object, and a new 3D key point selection method is proposed, which uses singular value decomposition (SVD) to find the 6D pose matrix between the target prediction key point and the reference key point; (3) in the process of the robotic arm grasping, in order to accurately grasp the object and avoid obstacles, the present invention proposes a robotic arm obstacle avoidance algorithm based on seventh-order polynomial trajectory planning, which ensures that the motion trajectory of the robotic arm is optimized to the maximum extent under the premise of avoiding obstacles. BRIEF DESCRIPTION OF THE DRAWINGS
[0067] Figure 1 This is a flow chart of a 6D posture grasping method of a robot arm based on a neural network in one embodiment of the present invention;
[0068] Figure 2 It is an overall structural block diagram of a 6D posture grasping method of a robot arm based on a neural network in one embodiment of the present invention;
[0069] Figure 3 It is a schematic diagram of the structure of the GhostBottleneck module and the C3Ghost module in one embodiment of the present invention;
[0070] Figure 4 A schematic diagram of adding a CBAM module to a backbone network in one embodiment of the present invention;
[0071] Figure 5 Schematic diagram of the CR3 robot arm connecting rod coordinate system in one embodiment of the present invention;
[0072] Figure 6 Schematic diagram of the minimum spherical envelope of an obstacle in one embodiment of the present invention;
[0073] Figure 7 The prediction results of the algorithm in the present invention on the self-made data set;
[0074] Figure 8 The figure is a graph showing the displacement, angular velocity and angular acceleration of each joint of the robot arm versus time in one embodiment of the present invention. DETAILED DESCRIPTION
[0075] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings.
[0076] In one embodiment, Figure 1As shown, a 6D pose grasping method of a robot arm based on a neural network, the method comprises the following steps:
[0077] S100: Acquire an RGB video sequence and a depth video sequence respectively through an industrial depth camera.
[0078] S200: Build an improved YOLOV5 algorithm network, and train the network in combination with a preset network loss function to obtain a trained network. The network includes a backbone network, a network layer, and a prediction layer. The backbone network is used for feature extraction, the network layer is used for processing the features extracted by the backbone network, and the prediction layer is used to predict the processed features to obtain an output feature map.
[0079] In one embodiment, the backbone network in S200 includes a CBS module, a first GhostConv module, a first G3Ghost module, a second GhostConv module, a second G3Ghost module, a third GhostConv module, a third G3Ghost module, a fourth GhostConv module, a CBAM attention mechanism module and an SPP module connected in sequence; the network layer includes two convolutional layers, and the prediction layer includes three parallel CBS modules.
[0080] Specifically, Figure 2 As shown in the figure, target detection is the key issue of robot vision grasping. The improved YOLOV5 algorithm network is mainly divided into three parts, namely the backbone network, the network layer and the prediction layer. The backbone network is responsible for feature extraction, the network layer processes the extracted features and improves the diversity of features, and the prediction layer uses 3 decoupling heads. In order to improve the real-time performance of robot target detection, this paper proposes a target detection algorithm of the YOLOV5-GhostNet network, uses the GhostNet network structure to replace the original CSPDarknet-53 of YOLOV5 as the backbone feature extraction network, and introduces the CBAM attention mechanism to focus on the feature information of the two dimensions of channel and space, ensuring accuracy while reducing the amount of network parameters.
[0081] In the backbone network part, the first layer uses 6×6 convolution to slice the input image, replaces the original convolution of YOLOV5 with GhostConv, replaces all C3 modules with the C3Ghost modules proposed in the present invention, and integrates the CBAM attention mechanism module to improve the network feature extraction capability.
[0082] GhostConv uses feature map similarity to generate a large number of feature maps through a small amount of calculation to solve the problem of feature map redundancy. Therefore, GhostConv can be divided into two parts, one of which is used for standard convolution, but its number is strictly controlled. Then the other part performs a series of simple linear operations on the inherent feature map generated by the first part to generate more feature maps, and finally splices the two parts of the feature map to generate the final feature map. In this way, the redundancy problem is eliminated without changing the size of the output feature map, and the computational cost is reduced to obtain a lighter model. In order to improve the model processing speed and the practicality of equipment deployment, the present invention uses 3x3 convolution kernels in all GhostConv modules. The theoretical speedup ratio of using the Ghost module instead of ordinary convolution is:
[0083]
[0084] Where c is the number of input channels, h and w are the height and width of the feature map, h′ and w′ are the height and width of the output data, n is the number of convolution kernels, k is the size of the convolution kernel, d is the size of the linear transformation convolution kernel, and s is the total mapping generated by each channel, where k×k and d×d are the same size and s< <c。
[0085] In one embodiment, the first G3Ghost module includes a Ghost Bottleneck module, a connection module and three CBS modules, any half of the feature map in the input feature map is generated through a CBS module and Ghost Bottleneck to generate a feature map, and the remaining other half of the feature map in the input feature map is generated through another CBS module connected in parallel with a CBS module and Ghost Bottleneck to generate a feature map, and then the two parts of the feature map are spliced and passed through the last CBS module to obtain the output of the first G3Ghost module;
[0086] The Ghost bottlenecks module consists of two stacked GhostConvs. The first GhostConv acts as an expansion layer to increase the number of channels. The second GhostConv is used to reduce the number of channels of the output feature map to match the number of input channels and perform an addition operation with the input feature map transmitted from the residual edge.
[0087] Specifically, based on the superiority of the Ghost module, the present invention further designs a C3Ghost module, the specific structure of which is as follows: Figure 3 shown.
[0088] Figure 3As shown in (a), each GhostBottleneck is mainly composed of two stacked GhostConvs. The first GhostConv is used as an expansion layer to increase the number of channels. The second GhostConv is used to reduce the number of channels of the output feature map to match the number of input channels and perform an addition operation with the input feature map transmitted from the residual edge.
[0089] The CSPNet (Cross Stage Partial Network, CSPNet) structure achieves richer gradient combinations by splitting shallow feature maps into two in the channel dimension. One part is propagated backwards through the feature extraction module, and the other part is directly merged with the output of the feature extraction module through a cross-stage hierarchy. It can reduce the number of network parameters by 10% to 20% while maintaining or improving the accuracy. Therefore, the CSPNet architecture was continued to be used when improving the YOLOV5 model. On the other hand, the traditional GhostNet uses the MobileNetV3 structure, which has a too deep network structure. Using this structure to extract features at multiple scales will increase the number of model parameters and the amount of computation. Taking both aspects into consideration, the C3Ghost module was designed. Figure 3 As shown in (b), the C3Ghost module generates feature maps for half of the feature information through the CBS module and GhostBottleneck, and the other part only generates feature maps through the CBS module, and then splices the two parts of the feature maps to maximize the difference in gradient combinations and reduce a large amount of gradient information. Among them, the CBS module includes convolution, regularization and activation functions.
[0090] The GhostConv module uses simple linear operations to generate feature maps, which is a grouped convolution method. Grouped convolution eliminates the correlation between channels. Although it reduces the amount of calculation, the model's ability to extract global features is reduced. In addition, there are many interference factors in the visual grasping of the robotic arm, such as illumination changes, background clutter, etc. Therefore, the present invention introduces the CBAM attention mechanism in the backbone network to improve the model's feature extraction capability and detection accuracy.
[0091] The present invention adds the CBAM attention mechanism module to the backbone network in three different ways, such as Figure 4 As shown, the first method is Figure 4 As shown in (a), named GhostNet_CBAM_V1, the input size of the backbone network is 640x640x3, and a feature map of size 320x320x64 is obtained after the CBS module. The CBAM module is added after this feature map; the second method is as follows Figure 4As shown in (b), named GhostNet_CBAM_V2, the feature map size is 20x20x1024 after passing the last layer of GhostConv, and the CBAM module is added after this feature map; the third method is as follows Figure 4 As shown in (c), named GhostNet_CBAM_V3, the method of adding the CBAM module in the third way combines the method in the first way and the method in the second way, and adds the CBAM module at the corresponding position at the same time. In this embodiment, the first way is selected.
[0092] S300: Based on a preset loss function, the improved YOLOV5 algorithm network and the preset PVN3D-Tiny network are trained to obtain a trained improved YOLOV5 algorithm network and a preset trained PVN3D-Tiny network.
[0093] In one embodiment, the loss function preset in S300 is specifically:
[0094] L=λ1L obj +λ2L reg +λ3L cls +λ4L key +λ5L pose
[0095] Where λ1, λ2, λ3, λ4 and λ5 are the scaling factors of each loss during training, and L obj , L cls and L reg They are target loss function, classification loss function and regression loss function respectively, L key is the key point selection loss function, L pose is the pose loss function, the target loss function L obj The Focal cross entropy loss function is used to measure the difference between it and the true value, which is defined as follows:
[0096]
[0097] Where N obj is the total number of video frame targets, y s is the label of the test sample s, the foreground is marked as 1, the background is marked as 0, and d s The probability of predicting the test sample s as a prospect;
[0098] Regression loss function L reg The CIoU loss function is used to measure the overlap area, center point distance and aspect ratio between the predicted box and the real box. The CIoU value range is [-1, 1]. The regression loss function L reg The definition is as follows:
[0099] L reg=1-CIoU
[0100] In order to judge the difference between the category to which the target belongs and the true value, the classification loss function uses the multi-classification cross entropy loss function to score the prediction of the category to which the target belongs, L cls The definition is as follows:
[0101]
[0102] Where N cls is the total number of video frame targets, M cls is the number of categories, y oc Symbol function, if the true category of the current frame target o is equal to c, y oc 1, otherwise 0, d oc is the predicted probability that the current frame target o belongs to the real category c. The key point selects the loss function L key The definition is as follows:
[0103]
[0104] Where N key is the total number of points detected, M key is the total number of key points, T i j is the translation offset from the i-th visible point to the j-th keypoint, is the distance from the i-th visible point to the j-th visible point * The true translation offset of the key points, the pose loss function L pose The definition is as follows:
[0105]
[0106] Where τ is the balance parameter that controls the rotation and translation regression errors, is the rotation angle obtained by regression, is the true value of the rotation angle label, is the translation vector obtained by regression, is the label truth data of the translation vector.
[0107] S400: Send the RGB video sequence to the trained improved YOLOV5 algorithm network to obtain an output feature map, wherein the output feature map includes the ROI area of the target to be captured.
[0108] S500: Based on the ROI area of the target to be grasped in the output feature map and the depth video sequence, the preset trained PVN3D-Tiny network is used to obtain the predicted key points of the surface of the target to be grasped, and the singular value decomposition method is used to calculate the 6D pose information of the target to be identified.
[0109] In one embodiment, S500 includes:
[0110] S510: Cropping the output feature map and the depth video sequence into a minimum square centered on and including the ROI area, using the nearest neighbor interpolation method to adjust the size of the cropped square image to a preset size, and after preprocessing, obtaining two RGB images and point cloud images of the preset size and sending them to the trained preset PVN3D-Tiny network;
[0111] S520: The trained preset PVN3D-Tiny network uses PointNet++ and PSPNet as the backbone network to extract RGB and point cloud feature information, and then uses DenseFusionNet to perform iterative dense fusion of global features on the obtained feature information to obtain a fused output feature map, and performs key point selection on the fused output feature map to obtain the key points of the target object and obtain the predicted key points;
[0112] S530: Determine the key points of prediction using singular value decomposition and the reference key points of the target object {kp i} is the 6D pose matrix Rt between them.
[0113] In one embodiment, in S520, key points are selected on the fused output feature map to obtain key points of the target object, and the predicted key points are obtained, including:
[0114] S521: Use the point-by-point segmentation method to filter out the background points of the target to be identified in the fused output feature map to obtain candidate key points {of i}∈R 3 ;
[0115] S522: Define candidate key points {of i} and the reference key point {kp i}∈R 3 The average prediction distance μ and standard deviation σ between them, at this stage, filter out any candidate key points whose difference from the average prediction distance μ exceeds the standard deviation σ, that is, when |of i When -μ|>σ, the candidate key point will be removed to obtain the predicted key point of the target object.
[0116] Specifically, YOLOV5-GhostNet is used to detect the target region of interest (ROI), and then PVN3D-Tiny is used to obtain the predicted key points on the object surface, and the singular value decomposition method is used to calculate the 6D pose information of the object.
[0117] like Figure 2As shown in the 6D pose estimation section, after obtaining the ROI area of the target to be captured by YOLOV5-Ghost, the RGB image and depth image output in the previous stage are cropped into the smallest square (such as 80x80, 160x160...) centered on and including the ROI area, and then the nearest neighbor interpolation method is used to adjust the size of the cropped square image to 80x80. After preprocessing, two RGB images and point cloud images of size 80x80 are obtained and sent to PVN3D-Tiny. Since the present invention adjusts the image to 80x80 size, PVN3D-Tiny only processes the data part related to the target object. When the number of sampling points is the same, the point cloud sampled from the cropped image will be denser, providing PVN3D-Tiny with richer geometric feature information.
[0118] PVN3D-Tiny uses PointNet++ and PSPNet as the backbone network to further extract RGB and point cloud feature information, and then uses DenseFusionNet to perform iterative dense fusion of global features to obtain the key points of the target object. In the key point selection stage, in order to obtain the final target pose, the point-by-point segmentation method is first used to filter out the background points of the target to be identified to obtain candidate key points {of i}∈R 3 In order to make the key point selection more certain, it is necessary to remove abnormal candidate key points. First, the candidate key points {of i} and the reference key point {kp i}∈R 3 The average predicted distance μ and standard deviation σ between them. At this stage, any candidate keypoints whose difference from the average predicted distance μ exceeds the standard deviation σ are filtered out, that is, when |of i When -μ|>σ, the candidate key point will be removed, so that the predicted key point of the target object is obtained. Finally, singular value decomposition (SVD) is used to find the key points of prediction and the reference key points of the target object {kp i} is the 6D pose matrix Rt between them.
[0119] S600: Obtain obstacle information, based on the 6D pose information of the target to be identified, combine the seventh-order polynomial trajectory planning algorithm and the genetic algorithm to obtain a planned trajectory, perform obstacle avoidance planning for the robot arm according to the planned trajectory and obstacle information to obtain the optimal trajectory, and achieve obstacle avoidance and grasping of the target.
[0120] Specifically, in order to improve the problems of joint jitter and motion trajectory deviation of the 6DOF robotic arm DOBOT CR3 during movement, a robotic arm obstacle avoidance algorithm based on seventh-order polynomial trajectory planning was proposed. The fitness function was established by comprehensively considering the collision detection results and kinematic indicators. The genetic algorithm was used for optimization, and a motion trajectory with no collision and small joint jitter was planned in the joint space to achieve precise grasping.
[0121] First, a simplified model of the robot arm needs to be established. The coordinate system of the CR3 robot arm link is as follows: Figure 5 As shown. The robot arm consists of 1 waist joint, 2 shoulder joints, and 3 wrist joints. According to the connecting rod coordinate system, the standard DH parameter table of the robot arm can be established, as shown in Table 1, where a is the length of the connecting rod, d is the distance between adjacent joints, θ is the rotation angle of adjacent joints, and α is the angle between the rotation axes of adjacent joints.
[0122] Table 1 CR3 robot arm standard DH parameter table
[0123]
[0124] Secondly, it is necessary to build a model for the obstacles. Since the shape of obstacles is uncertain, it is difficult to build an accurate mathematical model for all obstacles. Therefore, the present invention uses the smallest sphere to envelop the obstacles, such as Figure 6 As shown, the obstacle shape is converted into a sphere sequence described by the sphere center and sphere radius. Therefore, the collision judgment between the straight line segment of the robot arm link and the obstacle is converted into the distance judgment between the straight line segment and the sphere center point.
[0125] In one embodiment, S600 includes:
[0126] S610: Obtain the obstacle position, input the 6D pose information of the target to be identified, and generate a set of initial populations P by the genetic algorithm generation function i According to the initial parent K value combined with the seventh-order polynomial trajectory planning algorithm, the planned trajectory is obtained. The planned trajectory and obstacle information are combined to calculate the collision detection result f CO , the sum of the changes in the angles of each joint f Q , the sum of the trajectory lengths of the robot end f L The sum of the joint torques exceeds the set value f TO , and then find the fitness function f K ,
[0127] S620: Obtain offspring through crossover and mutation of genetic algorithm, and determine P after multiple iterations i With the fitness function f K Whether it is close or not, after reaching the termination condition, determine the optimal parameter K that is closest to the fitness, and finally substitute it into the seventh-order polynomial to obtain the optimal trajectory to achieve obstacle avoidance and grasping of the target.
[0128] In one embodiment, S620 further includes: when the termination condition is not met, adjusting the parent generation K value, and returning to S610 to obtain a planned trajectory in combination with a seventh-order polynomial trajectory planning algorithm, until the termination condition in S620 is met.
[0129] In one embodiment, the seventh-order polynomial is specifically:
[0130] θ(t)=c0+c1t+c2t 2 +c3t 3 +c4t 4 +c5t 5 +c6t 6 +Kt 7
[0131] Where K is an adjustable parameter, θ = [θ1, θ2, θ3, θ4, θ5, θ6] is a 6-dimensional vector, and θ 1~6 They are the six joint angles of the robot arm, and c0~c6 are the coefficients to be determined;
[0132] Assume that the robot starts at time t0 and ends at time t f The angular displacement, angular velocity, angular acceleration and angular impact degree are and Substituting the above 8 variables as initial conditions into the above formula and taking the derivative, we can find the trajectory functions of each joint at the start and end of the robot arm:
[0133]
[0134]
[0135]
[0136]
[0137]
[0138]
[0139]
[0140] When the parameter K is given, the unknown coefficients c0~c6 can be solved by the above formula, and then the motion trajectory of the robot arm with continuous angular displacement, angular velocity, angular acceleration and angular impact degree of each joint is planned.
[0141] Specifically, trajectory planning is generally expressed as a function of angular displacement, angular velocity, angular acceleration and other motion quantities with respect to time.
[0142] In one embodiment, the fitness function in S610 is specifically:
[0143]
[0144] In the formula, f CO is the collision detection result, collision is 0, no collision is 1; η i is the weighting factor, f Q is the sum of the changes in the angles of each joint, f L is the sum of the trajectory lengths of the end of the robot arm, f TO The torque of each joint exceeds the sum of the set values;
[0145] The seventh-order polynomial trajectory planning algorithm divides the planned motion trajectory into i small segments. The spatial position coordinates of the end of the robot arm in the i-1th segment are (p i-1,x ,p i-1,y ,p i-1,z ), so the distance between each position coordinate is the end trajectory length f L , the calculation formula is:
[0146]
[0147] The sum of joint angle changes f Q It is the accumulation of the absolute value of the difference between the joint angles of the six joints in the i-th and i-1-th segments of the trajectory. The calculation formula is:
[0148]
[0149] Where q i,j is the jth joint angle value of the i-th trajectory;
[0150] The torque of each joint exceeds the sum of the set values f TO The calculation formula is:
[0151]
[0152] Where δ i,j is the i-th trajectory and j-th joint torque, is the torque limit of the jth joint.
[0153] Specifically, the undetermined coefficient K of the seventh-order polynomial determines the performance of the motion trajectory. Since the genetic algorithm has good stability and robustness in solving high-dimensional nonlinear problems, the present invention uses the genetic algorithm to optimize the undetermined coefficient K. When the K value is too large or too small, it is easy to cause the mechanical arm joint angular torque to exceed the limit. Therefore, the present invention sets the value range of Ki to [-400, 400]. For energy saving and high efficiency considerations, the above adaptation function is defined to reduce unnecessary movement of the mechanical arm.
[0154] The optimization process of genetic algorithm for the given parameter K is as follows Figure 2 First, the target and obstacle positions are input, and a set of initial populations P is generated by the genetic algorithm generation function. i , and then calculate the collision detection result f CO , the sum of the changes in the angles of each joint f Q , the sum of the trajectory lengths of the robot end f L The sum of the joint torques exceeds the set value f TO , and then find the fitness function f K , through genetic algorithm crossover and mutation to obtain offspring, after multiple iterations to determine P i With fitness f K Whether it is close or not, after reaching the termination condition, find the optimal parameter K that is closest to the fitness, and finally substitute it into the seventh-order polynomial to get the optimal trajectory, and then the obstacle avoidance and grasping of the target can be achieved.
[0155] The above-mentioned 6D pose grasping method of a robot arm based on a neural network has the following advantages:
[0156] 1) In order to improve the accuracy and speed of target detection, a target detection algorithm based on the YOLOV5-GhostNet network is proposed, and the CBAM attention mechanism module is integrated to improve the network feature extraction capability.
[0157] 2) In order to improve the speed of pose estimation, the PVN3D-Tiny network is used to calculate the 6D pose information of the target object, and a new 3D key point selection method is proposed, which uses singular value decomposition (SVD) to find the 6D pose matrix between the target predicted key points and the reference key points.
[0158] 3) During the robotic arm grasping process, in order to accurately grasp objects and avoid obstacles, the present invention proposes a robotic arm obstacle avoidance algorithm based on seventh-order polynomial trajectory planning. This method ensures that the robotic arm motion trajectory is optimized to the maximum extent while avoiding obstacles.
[0159] In the detailed experimental process, the robot arm 6D posture grasping system mainly includes two platforms: deep learning platform and robot arm grasping control platform. The GPU of the deep learning platform is NVIDIA Geforce RTX2080ti, and the built-in software environment is Ubuntu18.04LTS system. The 6D posture information of the object is obtained by training the network through the deep learning platform. The single-arm end load of the robot arm grasping control platform is 3kg, the repeatability positioning accuracy is ±0.0.2mm, and the working radius is about 666mm. The main components include: realsense depth camera, robot body, electric two-finger gripper, NUC onboard microcomputer, etc. The robot operating system is ROS (Robot Operating System), which has good open source and scalability.
[0160] Qualitative analysis of experimental results: First, the effectiveness of the proposed algorithm is verified on the POSE4DS dataset. The POSE4DS dataset contains 4 target objects, all of which are symmetrical objects. Figure 7 These are the prediction results of the algorithm of the present invention at different viewing angles on the POSE4DS dataset. The surfaces of the two types of industrial parts are both reflective and have low texture, but the present invention can still predict the pose information of the object very well.
[0161] In order to intuitively evaluate the performance of the algorithm of the present invention, the present invention conducted quantitative and qualitative comparative analysis experiments on the YCB-Video dataset, and conducted network ablation experiments on the test set. Simulation experiments were also conducted on the motion trajectory and obstacle avoidance performance of the robot arm.
[0162] The YCB-Video dataset contains 6D poses of 21 objects in 92 videos, with a total of 133,827 frames. The objects in the dataset show different symmetries and are arranged in various postures and spatial configurations, resulting in serious occlusions between objects. Since the dataset is too large, this paper selects 8 common objects in life scenes (electric drill, wood block, scissors, marker, clamp, large clamp, can, red basin) from the YCB-Video dataset and some video frames with occlusion and stacking for experiments.
[0163] Experiments show that the clamp is in an unobstructed state in the 50th frame. Except for the algorithm of the present invention and the PointFusion algorithm, the other two algorithms fail to predict the 6D pose information of the clamp; at the 72nd frame, the PoseCNN algorithm and the PointFusion algorithm are affected by the change in illumination, and the prediction error of the clamp pose information is large, while the algorithm of the present invention can still accurately predict the clamp pose information; at the 437th and 491st frames, the red basin is in a serious occlusion state, and only the PointFusion algorithm and the algorithm of the present invention can still accurately predict the red basin pose information, indicating that the algorithm of the present invention has good anti-occlusion ability; at the 1488th frame, except for the PoseCNN algorithm, the other three algorithms can prepare to predict its pose information. The present invention uses the high-performance GhostNet feature extraction network under the YOLOV5 framework and the new 3D key point selection method to effectively improve the problem of target occlusion and cross occlusion.
[0164] To verify the effectiveness of adding the network and CBAM attention mechanism, the algorithm of the present invention is compared with five algorithms: 1) traditional YOLOV5 backbone network; 2) using GhostNet to replace the backbone network in the YOLOV5 framework; 3) using GhostNet_CBAM_V1 to replace the backbone network in the YOLOV5 framework; 4) using GhostNet_CBAM_V2 to replace the backbone network in the YOLOV5 framework; 5) using GhostNet_CBAM_V3 to replace the backbone network in the YOLOV5 framework. In order to quantitatively analyze the target detection performance, several common evaluation indicators are used, such as precision, recall, average accuracy (mAP), parameter quantity (Params, in M) and floating-point operations per second (in GHz). The results are shown in Table 2.
[0165] Table 2 Network ablation experiment
[0166]
[0167] As shown in Table 2, firstly, the performance indicators of the traditional YOLOV5 backbone network CSPDarknet53 and GhostNet in the YOLOV5 framework are compared. When the accuracy, recall rate and average accuracy rate are not much different, the number of parameters is greatly reduced from 7.20M to 3.59M, and the computational complexity is greatly reduced. Secondly, the GhostNet_CBAM_V1~V3 backbone networks in the YOLOV5 framework are compared. It can be seen that GhostNet_CBAM_V2 performs best. When the number of parameters is not much different, GhostNet_CBAM_V2 is 1.2% and 1.6% higher than V1 and V2 respectively. Therefore, when the GPU computing power of the robotic arm grasping platform is limited, the lightweight model GhostNet_CBAM_V2 is more suitable for application and robotic arm grasping platform.
[0168] In order to quantitatively analyze the proposed algorithm, comparative analysis experiments were conducted on PoseCNN, PVNet, PointFusion and PVN3D on the POSE4DS and YCB-Video datasets. The comparison with the recent methods is shown in Table 3.
[0169] Table 3 Test results of POSE4DS dataset under evaluation indicators
[0170]
[0171]
[0172] As shown in Table 3, the accuracy of the invention in workpiece 1, workpiece 2 and milk carton is 1.4%, 0.5% and 0.4% higher than that of the PVN3D algorithm respectively. The average prediction accuracy of the objects obtained by the invention is 98.4%. In the POSE4DS dataset, the accuracy of milk carton is as high as 99.1%. For low-texture objects such as industrial parts, the accuracy of the invention is the highest, which is 98.7% and 97.1% respectively.
[0173] The comparison results of different algorithms on the YCB-Video dataset are shown in Table 4.
[0174] Table 4 Test results of YCB-Video dataset under ADD-S indicator
[0175]
[0176] The present invention is 29.1%, 19.3% and 2.2% higher than PoseCNN, PointFusion and PVN3D-Tiny respectively under the ADD-S evaluation index. In the pose estimation stage, the present invention crops the ROI area, and the network only processes part of the data related to the target object. When the number of sampling points is the same, the sampled point cloud is denser, providing the network with richer geometric feature information.
[0177] In order to verify the performance of the robot arm obstacle avoidance algorithm based on seventh-order polynomial trajectory planning, the present invention uses MATLAB Robotics Toolbox and Optimtool toolbox to conduct simulation experiments. When performing obstacle avoidance simulation, it can be seen that the robot arm avoids obstacles along the trajectory from the starting position to the end position, which illustrates the effectiveness of the present invention in obstacle avoidance in three-dimensional space. The curves of the displacement, angular velocity, and angular acceleration of each joint of the robot arm over time are shown in Figure 1. Figure 8 shown.
[0178] It can be seen that the motion trajectory of the robot arm planned by the seventh-order polynomial is smooth and continuous without mutations in the angular velocity and angular acceleration curves of each joint. Therefore, an ideal motion trajectory without collision is planned while the kinematics, trajectory length, rotation angle and other requirements are coordinated and optimized.
[0179] The above is a detailed introduction to a neural network-based robotic arm 6D posture grasping method provided by the present invention. The present invention uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the core idea of the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, the present invention can also be improved and modified, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.
Claims
1. A 6D pose grasping method for a robotic arm based on a neural network, characterized in that: The method comprises the following steps: S100: Acquire RGB video sequence and depth video sequence respectively through industrial depth camera; S200: Build an improved YOLOV5 algorithm network, which includes a backbone network, a network layer and a prediction layer. The backbone network is used for feature extraction, the network layer is used for processing the features extracted by the backbone network, and the prediction layer is used for predicting the processed features to obtain an output feature map. S300: training the improved YOLOV5 algorithm network and the preset PVN3D-Tiny network based on a preset loss function to obtain a trained improved YOLOV5 algorithm network and a preset trained PVN3D-Tiny network; S400: Sending the RGB video sequence to the trained improved YOLOV5 algorithm network to obtain an output feature map, wherein the output feature map includes the ROI area of the target to be captured; S500: Based on the ROI area of the target to be grasped in the output feature map and the depth video sequence, the preset trained PVN3D-Tiny network is used to obtain the predicted key points of the surface of the target to be grasped, and the singular value decomposition method is used to calculate the 6D pose information of the target to be identified; S600: Obtain obstacle information, obtain a planned trajectory based on the 6D pose information of the target to be identified by combining a seventh-order polynomial trajectory planning algorithm and a genetic algorithm, perform obstacle avoidance with the robot arm according to the planned trajectory and obstacle information to obtain an optimal trajectory, and achieve obstacle avoidance and grasping of the target; The backbone network in S200 includes a CBS module, a first GhostConv module, a first C3Ghost module, a second GhostConv module, a second C3Ghost module, a third GhostConv module, a third C3Ghost module, a fourth GhostConv module, a CBAM attention mechanism module and an SPP module connected in sequence; the network layer includes two convolutional layers, and the prediction layer includes three parallel CBS modules; The first, second and third C3Ghost modules all include a Ghost Bottleneck module, a connection module and three CBS modules. Any half of the feature map in the input feature map is generated through a CBS module and Ghost Bottleneck. The remaining other half of the feature map in the input feature map is generated through another CBS module connected in parallel with a CBS module and Ghost Bottleneck. The two parts of the feature map are then spliced and passed through the last CBS module to obtain the output of the first, second and third C3Ghost modules. The Ghost bottlenecks module consists of two stacked GhostConvs. The first GhostConv is used as an expansion layer to increase the number of channels. The second GhostConv is used to reduce the number of channels of the output feature map to match the number of input channels and perform an addition operation with the input feature map transmitted from the residual edge. S500 includes: S510: Cropping the output feature map and the depth video sequence into a minimum square centered on and including the ROI area, using the nearest neighbor interpolation method to adjust the size of the cropped square image to a preset size, and after preprocessing, obtaining two RGB images and point cloud images of the preset size and sending them to the trained preset PVN3D-Tiny network; S520: The trained preset PVN3D-Tiny network uses PointNet++ and PSPNet as the backbone network to extract RGB and point cloud feature information, and then uses DenseFusionNet to perform iterative dense fusion of global features on the obtained feature information to obtain a fused output feature map, and performs key point selection on the fused output feature map to obtain the key points of the target object and obtain the predicted key points; S530: Determine the key points of prediction using singular value decomposition and the reference key points of the target object The 6D pose matrix between ; S600 includes: S610: Obtain the obstacle position, input the 6D pose information of the target to be identified, and generate a set of initial populations using the genetic algorithm generation function , according to the initial parent K value combined with the seventh-order polynomial trajectory planning algorithm to obtain the planned trajectory, combined with the planned trajectory and obstacle information, by calculating the collision detection result , the sum of the changes in the angles of each joint , the sum of the trajectory lengths of the robot end The sum of the joint torques exceeds the set value , and then find the fitness function ; S620: Get offspring through genetic algorithm crossover and mutation, and judge after multiple iterations And the fitness function Is it close? After reaching the termination condition, determine the optimal parameters closest to the fitness K , and finally substitute it into the seventh-order polynomial to obtain the optimal trajectory and achieve obstacle avoidance and grasping of the target.
2. The method according to claim 1, characterized in that The loss function preset in S300 is specifically: ; In the formula , , , and is the scaling factor for each loss during training, , and They are target loss function, classification loss function and regression loss function respectively. is the key point selection loss function, is the pose loss function, the target loss function The Focal cross entropy loss function is used to measure the difference between it and the true value, which is defined as follows: ; In the formula, is the total number of video frame targets, For test samples s The labels are 1 for the foreground and 0 for the background. For test samples s The probability of prediction being a prospect; Regression loss function The CIoU loss function is used to measure the overlap area, center point distance and aspect ratio between the predicted box and the real box. The CIoU value range is [-1, 1]. The regression loss function The definition is as follows: ; In order to judge the difference between the category to which the target belongs and the true value, the classification loss function uses the multi-classification cross entropy loss function to score the prediction of the category to which the target belongs. The definition is as follows: ; In the formula, is the total number of video frame targets, is the number of categories, Symbol function, if the current frame target o The true category is equal to c , If yes, it is 1, otherwise it is 0. The current frame target o Belongs to the real category c The prediction probability, key point selection loss function The definition is as follows: ; In the formula is the total number of points detected, is the total number of key points, It is from Visible points to The translation offset of the key points, It is from Visible points to The true translation offset of the key points, the pose loss function The definition is as follows: ; In the formula To control the balance parameter of rotation and translation regression error, is the rotation angle obtained by regression, is the true value of the rotation angle label, is the translation vector obtained by regression, is the label truth data of the translation vector.
3. The method according to claim 2, characterized in that In S520, key points are selected from the fused output feature map to obtain key points of the target object and obtain predicted key points, including: S521: Use the point-by-point segmentation method to filter out the background points of the target to be identified in the fused output feature map to obtain candidate key points ; S522: Define candidate key points With reference key points The average prediction distance between and standard deviation , at this stage filter out any The difference exceeds the standard deviation The candidate key points of When , the candidate key point will be removed and the predicted key point of the target object will be obtained. .
4. The method according to claim 3, characterized in that S620 also includes: when the termination condition is not met, adjusting the parent generation K value, and returning to S610 to obtain a planned trajectory in combination with a seventh-order polynomial trajectory planning algorithm, until the termination condition in S620 is met.
5. The method according to claim 4, characterized in that The seventh-order polynomial is specifically: ; In the formula K is an adjustable parameter, is a 6-dimensional vector, They are the 6 joint angles of the robot arm. is the coefficient to be determined; Assume that the robot arm starts at and the end moment The angular displacement, angular velocity, angular acceleration and angular impact degree are and , the above 8 variables are substituted into the above formula as initial conditions and the derivatives are taken to obtain the trajectory functions of each joint at the start and end time of the robot arm: ; When given parameters K When , the unknown coefficients can be solved from the above formula , and then plan the motion trajectory of each joint of the robot arm with continuous angular displacement, angular velocity, angular acceleration, and angular impact degree.
6. The method according to claim 5, characterized in that The fitness function in S610 is specifically: ; In the formula, is the collision detection result, collision is 0, no collision is 1; is the weighting factor, is the sum of the changes in the angles of each joint, is the sum of the trajectory lengths of the end of the robot arm, The torque of each joint exceeds the sum of the set values; The seventh-order polynomial trajectory planning algorithm divides the planned motion trajectory into Small paragraph, The spatial coordinates of the end of the segment robot arm are , so the distance between each position coordinate is the end trajectory length , the calculation formula is: ; The sum of joint angle changes For 6 joints in the Paragraph and Section The absolute value of the difference between the joint angles of the segment trajectory is accumulated, and the calculation formula is: ; In the formula For the Segment track, Joint angle values; The torque of each joint exceeds the sum of the set values The calculation formula is: ; In the formula For the Segment track, Joint torques, For the The torque limit of each joint.
Citation Information
Cited By
Methods and systems for landmark-based visual localization and pose estimation of large dynamic objects
WO2026207510A1
