Cleaning robot and dynamic real-time track planning algorithm for ground garbage grabbing of cleaning robot

Abstract

The invention discloses a cleaning robot and a dynamic real-time track planning algorithm for ground garbage grabbing of the cleaning robot, and relates to the technical field of cleaning robots, the cleaning robot comprises a movable cleaning robot, a six-degree-of-freedom mechanical arm is installed on a movable robot body, an observation camera is connected nearby a clamping jaw, the observation camera obtains real-time images, and the real-time images are displayed on the robot body. The real-time image information is processed through a garbage detection algorithm, garbage types and position information are output, target garbage information is processed through a multi-stage real-time trajectory planning algorithm, the algorithm is an RT-DETR-RTD-RRT * multi-stage processing algorithm, and the method relates to obtaining of target information to be grabbed and staged real-time mechanical arm trajectory planning according to the target information. The multi-stage real-time trajectory planning algorithm is used for processing the target garbage information, the mechanical arm is controlled to accurately and rapidly grab the garbage, the change of the target position information can be rapidly responded, a new mechanical arm executable trajectory is generated, and it is ensured that the robot can still work stably in the dynamic environment.

Description

Technical Field

[0001] The present invention relates to the technical field of cleaning robots, and particularly to a cleaning robot and a dynamic real-time trajectory planning algorithm for ground garbage grasping thereof. Background Art

[0002] When a cleaning robot performs ground garbage cleaning work, it needs to grasp garbage at different positions and of different types. In order to ensure work efficiency, it is necessary to judge in real time and generate a trajectory executable by the robotic arm. It is also required that the robot can perform trajectory planning and complete garbage grasping under dynamic conditions such as vibration. Traditional cleaning robots have insufficient accuracy in identifying ground garbage and cannot achieve dynamic real-time trajectory planning in combination with vision. Summary of the Invention

[0003] The cleaning robot and the dynamic real-time trajectory planning algorithm for ground garbage grasping proposed by the present invention solve the problem that traditional cleaning robots have insufficient accuracy in identifying ground garbage and cannot achieve dynamic real-time trajectory planning in combination with vision.

[0004] To achieve the above object, the present invention adopts the following technical solution: A dynamic real-time trajectory planning algorithm for ground garbage grasping of a cleaning robot includes a movable cleaning robot, and a robotic arm with six degrees of freedom is installed on the moving body. A two-finger gripper mechanism needs to be installed at the end of the robotic arm, and an observation camera is connected near the gripper installation. The observation camera acquires real-time images, and a garbage detection algorithm processes the real-time image information and outputs the garbage type and position information. A multi-stage real-time trajectory planning algorithm is used to process the target garbage information and output the trajectory to the cleaning robot and the robotic arm control system. The control system operates the moving body to approach the garbage, and at the same time controls the robotic arm to accurately and quickly grasp the garbage; the algorithm is an RT-DETR-RTD-RRT* multi-stage processing algorithm, which involves obtaining target information to be grasped and performing real-time robotic arm trajectory planning in stages according to the target information.

[0005] Preferably, the operation process and each stage of the RT-DETR-RTD-RRT* multi-stage processing algorithm are as follows:

[0006] S1. Garbage detection stage, in this stage, the position information of the garbage to be grasped is obtained quickly and accurately;

[0007] S2. Visual detection stage, processing to obtain the target garbage position coordinates with a higher confidence level;

[0008] S3. Trajectory planning stage, performing inverse kinematics solution of the position coordinate information based on the robotic arm kinematic model to obtain the target joint angles of the robotic arm as the target points for planning for trajectory planning.

[0009] Preferably, S1 includes:

[0010] ResNet50 Backbone: Used to extract features of images. Through convolutional layers, batch normalization layers, ReLU activation functions, max pooling layers, and four residual blocks, it extracts multi-scale features;

[0011] Transformer Encoder: Utilizes the multi-head self-attention mechanism to capture the global dependencies between features. Through the feed-forward neural network and layer normalization, it enhances the expressive power and stability of the features;

[0012] Position Embedding: Constructs 2D sine-cosine position embeddings to provide spatial position information for the Transformer;

[0013] Prediction Head: Through two linear layers, it predicts the category and bounding box coordinates of the target respectively. The bounding box prediction is normalized by the sigmoid function.

[0014] Preferably, the trajectory planning part in S3 uses real-time trajectory planning, which can quickly process the target point information and generate a reliable trajectory executable by the robotic arm; if the target position information changes, this stage can also quickly respond and generate a new trajectory.

[0015] Preferably, after obtaining the final path, after performing polynomial interpolation calculation and processing on the path, control the movements of the joints of the robotic arm so that the end gripper approaches the target point at an appropriate speed and posture and closes the gripper to grab the garbage on the ground.

[0016] Preferably, the above description is only one cycle of garbage grabbing. If there are multiple pieces of garbage to be grabbed, the above process is looped until there is no garbage in the camera observation area.

[0017] A cleaning robot, which applies the above-mentioned dynamic real-time trajectory planning algorithm for the cleaning robot to grab ground garbage.

[0018] The beneficial effects of the present invention are as follows: By combining the six-degree-of-freedom robotic arm and the two-finger gripper mechanism, the cleaning robot of the present invention can accurately grab ground garbage of different sizes, shapes and types, greatly improving the cleaning efficiency. The observation camera can obtain the position and type information of the garbage in real time, ensuring that the robotic arm can accurately and quickly locate and grab the target garbage, reducing the situations of mis-grabbing and missed-grabbing;

[0019] Process the target garbage information using a multi-stage real-time trajectory planning algorithm, and output the trajectory to the cleaning robot and the manipulator control system. The control system operates to move the fuselage close to the garbage, and at the same time controls the manipulator to accurately and quickly grasp the garbage. It can quickly respond to the change of the target position information, generate a new executable trajectory for the manipulator, and ensure that the robot can still work stably in a dynamic environment. The robot can perform trajectory planning and complete garbage grasping under dynamic conditions such as vibration. Brief Description of the Drawings

[0020] Figure 1 It is a schematic flow chart of the dynamic real-time trajectory planning algorithm for the cleaning robot to grasp ground garbage of the present invention. Detailed Embodiments

[0021] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments.

[0022] Embodiment 1

[0023] Refer to Figure 1 , the present invention provides the following technical solutions: a dynamic real-time trajectory planning algorithm for a cleaning robot to grasp ground garbage, including a movable cleaning robot, and a six-degree-of-freedom manipulator is installed on the movable fuselage. A two-finger gripper mechanism needs to be installed at the end of the manipulator, and an observation camera is connected near the gripper installation. The observation camera acquires real-time images, and the garbage detection algorithm processes the real-time image information and outputs the garbage type and position information. The target garbage information is processed using a multi-stage real-time trajectory planning algorithm, and the trajectory is output to the cleaning robot and the manipulator control system. The control system operates to move the fuselage close to the garbage, and at the same time controls the manipulator to accurately and quickly grasp the garbage;

[0024] The algorithm designed according to the above process is the RT-DETR-RTD-RRT* multi-stage processing algorithm, which mainly involves the acquisition of target information to be grasped, and the real-time manipulator trajectory planning in stages according to the target information. The operation process and pseudo-code of each stage of this algorithm are as follows:

[0025] (1) Garbage detection stage. This stage is mainly to quickly and accurately obtain the position information of the garbage to be grasped. The processing process and pseudo-code of this stage are as follows:

[0026] Input: Input Image, ResNet50 Backbone, Transformer Encoder, Prediction Head Output: Class Predictions, Bounding Box Predictions 1 Tf <- ResNet50Backbone(InputImage) / / Use ResNet50Backbone to extract features of the image

[0027] 2 Tr <- TransformerEncoder(Tf, PositionEmbedding) / / Use TransformerEncoder to encode the features

[0028] 3 Tpath <- PredictionHead(Tr) / / Use PredictionHead to generate the final prediction results

[0029] / / ResNet50Backbone / / Extract image features

[0030] Function ResNet50Backbone(Image):

[0031] x = ConvLayer(Image)

[0032] x = BatchNorm(x)

[0033] x = ReLU(x)

[0034] x = MaxPooling(x)

[0035] x = ResidualBlock1(x)

[0036] x = ResidualBlock2(x)

[0037] x = ResidualBlock3(x)

[0038] x = ResidualBlock4(x)

[0039] return x

[0040] / / TransformerEncoder / / Encode the features

[0041] Function TransformerEncoder(Features, PositionEmbedding):

[0042] encoded_features = MultiHeadSelfAttention(Features, PositionEmbedding) / / Use multi-head self-attention mechanism to capture global dependencies between features

[0043] encoded_features =

[0044] FeedForwardNetwork(encoded_features) / / Use feed-forward neural network to perform non-linear transformation on features

[0045] encoded_features = LayerNormalization(encoded_features) / / Use layer normalization to stabilize the training process

[0046] return encoded_features / / Return the encoded features

[0047] / / PositionEmbedding / / Position embedding

[0048] Function build_2d_sincos_position_embedding(W, H, hidden_dim = 256):

[0049] grid_w = arange(W)

[0050] grid_h = arange(H)

[0051] grid_w, grid_h = meshgrid(grid_w, grid_h)

[0052] pos_dim = hidden_dim / / 4

[0053] omega = arange(pos_dim) / pos_dim

[0054] omega = 1.0 / (10000.0 ** omega)

[0055] out_w = grid_w.flatten()[:, None] @ omega[None, :] / / Calculate the position encoding in the width direction

[0056] out_h = grid_h.flatten()[:, None] @ omega[None, :] / / Calculate the position encoding in the height direction

[0057] pos_embed = concat([sin(out_w), cos(out_w), sin(out_h), cos(out_h)], axis=1) # Concatenate the positional encodings in the width and height directions

[0058] return pos_embed.reshape(1, W * H, hidden_dim) # Reshape the positional embedding

[0059] / / PredictionHead / / Bounding box prediction

[0060] Function PredictionHead(EncodedFeatures):

[0061] class_predictions = LinearLayerClass(EncodedFeatures) # Predict the target class using a linear layer

[0062] bbox_predictions = LinearLayerBBox(EncodedFeatures) # Predict the target bounding box using a linear layer

[0063] return class_predictions, bbox_predictions.sigmoid() # Return the class predictions and the normalized bounding box predictions

[0064] The functions of each part are as follows:

[0065] ResNet50Backbone: Used to extract features from images. Through convolutional layers, batch normalization layers, ReLU activation functions, max pooling layers, and four residual blocks, multi-scale features are extracted.

[0066] TransformerEncoder: Utilizes the multi-head self-attention mechanism to capture the global dependencies between features. Through a feed-forward neural network and layer normalization, the expressive power and stability of the features are enhanced.

[0067] PositionEmbedding: Constructs 2D sine-cosine positional embeddings to provide spatial position information for the Transformer.

[0068] PredictionHead: Predicts the class and bounding box coordinates of the target through two linear layers. The bounding box predictions are normalized by the sigmoid function.

[0069] (2) In the visual detection stage, the coordinates of the target garbage position with a relatively high confidence are obtained. In the trajectory planning stage, the position coordinate information is used for inverse kinematic solution based on the manipulator kinematic model to obtain the target joint angles of the manipulator as the target points for planning for trajectory planning. The real-time trajectory planning is used in the trajectory planning part, which can quickly process the target point information and generate a reliable trajectory executable by the manipulator. If the target position information changes, this stage can also quickly respond to generate a new trajectory. The pseudo-code of the RTD-RRT* (Real Time Dynamic RRT*) algorithm in this stage is as follows:

[0070]

[0071]

[0072] After obtaining the final path, after performing polynomial interpolation calculation and processing on the path, control the movement of each joint of the manipulator to make the end gripper approach the target point at an appropriate speed and posture and close the gripper to grab the garbage on the ground.

[0073] The above is the main pseudo-code of each operation process part of this algorithm. The above description is only one cycle of garbage grabbing. If there are multiple garbage to be grabbed, the above process is looped until there is no garbage in the camera observation area.

[0074] Embodiment 2

[0075] A cleaning robot, which applies the dynamic real-time trajectory planning algorithm for grabbing ground garbage of the above cleaning robot. By writing the algorithm program in Embodiment 1 on the cleaning robot, the manipulator of the cleaning robot is combined with vision to grab ground garbage and perform dynamic real-time trajectory planning, thereby improving the cleaning efficiency of the robot.

[0076] The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution of the present invention and its inventive concept, makes equivalent substitutions or changes, and should be covered by the protection scope of the present invention.

Claims

1. Dynamic real-time trajectory planning algorithm for cleaning robot to grab ground garbage, characterized by: It includes a movable cleaning robot, and a six-degree-of-freedom robotic arm is installed on the mobile body. A two-finger gripper mechanism needs to be installed at the end of the robotic arm. An observation camera is connected near the gripper installation. The observation camera obtains real-time images. The garbage detection algorithm processes the real-time image information and outputs the garbage type and location information. A multi-stage real-time trajectory planning algorithm is used to process the target garbage information and output the trajectory to the cleaning robot and robotic arm control system. The control system operates the mobile body to approach the garbage and controls the robotic arm to grab the garbage accurately and quickly. The algorithm is a RT-DETR-RTD-RRT* multi-stage processing algorithm, which involves the acquisition of target information to be grabbed and the real-time robotic arm trajectory planning in stages according to the target information.

2. The dynamic real-time trajectory planning algorithm for grabbing ground garbage by a cleaning robot according to claim 1 is characterized in that: The operation process and stages of the RT-DETR-RTD-RRT* multi-stage processing algorithm include the following: S1, garbage detection stage, this stage is to quickly and accurately obtain the location information of the garbage to be captured; S2, visual detection stage, processing to obtain the target garbage location coordinates with high confidence; S3, trajectory planning stage, the position coordinate information is solved by inverse kinematics based on the robot arm kinematic model, and the target joint angle of the robot arm is obtained as the planning target point for trajectory planning.

3. The dynamic real-time trajectory planning algorithm for grabbing ground garbage by a cleaning robot according to claim 2 is characterized in that: The S1 includes: ResNet50Backbone: used to extract image features, through convolutional layers, batch normalization layers, ReLU activation functions and maximum pooling layers, and four residual blocks, to extract multi-scale features; TransformerEncoder: It uses a multi-head self-attention mechanism to capture the global dependencies between features and enhances the expressiveness and stability of features through feedforward neural networks and layer normalization. PositionEmbedding: Construct 2D sine-cosine position embedding to provide spatial position information for Transformer; PredictionHead: Through two linear layers, the target category and bounding box coordinates are predicted respectively, and the bounding box prediction is normalized by the sigmoid function.

4. The dynamic real-time trajectory planning algorithm for grabbing ground garbage by a cleaning robot according to claim 2 is characterized in that: The trajectory planning part in S3 uses real-time trajectory planning, which can quickly process target point information and generate reliable trajectory executable by the robot arm; if the target position information changes, this stage can also respond quickly to generate a new trajectory.

5. The dynamic real-time trajectory planning algorithm for grabbing ground garbage by a cleaning robot according to claim 2 is characterized in that: After obtaining the final path, the path is processed by polynomial interpolation calculation, and the movement of each joint of the robotic arm is controlled so that the end gripper approaches the target point at a suitable speed and posture and closes the gripper to grab the garbage on the ground.

6. The dynamic real-time trajectory planning algorithm for grabbing ground garbage by a cleaning robot according to claim 2 is characterized in that: The above is only a cycle of garbage grabbing. If there are multiple garbages to be grabbed, the above process will be executed repeatedly until there is no garbage in the camera observation area.

7. A cleaning robot, characterized in that: The cleaning robot applies the dynamic real-time trajectory planning algorithm for grabbing ground garbage by the cleaning robot as described in any one of claims 1 to 6 above.

Images