Mobile robot formation control method and system based on lightweight improved YOLOv5

Abstract

The invention discloses a mobile robot formation control method and system based on lightweight improved YOLOv5. The method comprises the steps that firstly, an RGBD camera is configured and calibrated, RGB images are collected in real time through the RGBD camera and transmitted into a target detection neural network, and the position of a detection frame is obtained; secondly, predicting and tracking the state of the leader by using a Kalman filter based on a target detection result; and then a depth value of the leader is obtained, and the actual position of the leader relative to the camera coordinate system is solved to obtain the position of the leader under the map coordinate. Finally, according to the position of the leader under the map coordinates, a Bezier curve is drawn to serve as a moving track of the follower, the follower follows the leader to move according to the track through a model prediction control method, and if the leader is lost, the leader conducts detection again. According to the method, the tracking smoothness of followers can be kept, the tracking precision is improved, the following of one to two leader robots can be supported, and formation control is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot leader-follower formation navigation and control, and in particular to a mobile robot formation control method and system based on lightweight improved YOLOv5. Background Art

[0002] Robotics is increasingly being used, especially in the field of autonomous mobile robots, which are able to independently complete a variety of tasks without human intervention. In complex environments, robots must also be able to avoid obstacles while performing leader-following tasks. Traditional following algorithms can often only track the target object in front, but cannot identify and avoid obstacles, which may lead to collision accidents. On the other hand, although traditional obstacle avoidance algorithms can avoid obstacles, they often cannot continue to track the leader in front, which may lead to the loss of the leader. How to effectively avoid obstacles while following the leader is a major challenge currently faced.

[0003] At present, the development of simultaneous following and obstacle avoidance algorithms is still in the basic stage, and there are not many solutions that can meet the needs. According to the use of sensors, some use radar and camera for leading and following while avoiding obstacles, use camera for target recognition, and then use radar for obstacle avoidance, but this method requires two sensors, which increases the cost and complexity, and cannot provide three-dimensional obstacle information. Additional sensors are required for obstacles such as suspended obstacles. Some use sensors such as UWB for target detection, but users are required to carry corresponding sensors, which increases the difficulty of user use. If the sensor is lost, following cannot continue. Based on this, a method for leading and following movement of a non-complete mobile robot using only RGBD camera as a sensor is provided, which uses a low-cost RGBD camera, which is conducive to reducing the cost of the robot. Only the camera is used to track the leader.

[0004] Robot target tracking technology through camera images can be divided into two categories: generative model method and detection tracking method. The generative model method achieves tracking by building a model of the target. However, this algorithm mainly focuses on the target itself and ignores the background information, so the target is often lost. In order to consider both the target and the background information, the detection tracking method has received more and more attention. This method distinguishes the target and the background by building a classifier, and its tracking effect is generally better than the generative model method.

[0005] The target following algorithm for mobile robots based on deep learning has advantages such as strong robustness. Some researchers have proposed a target detector based on deep learning, introduced Kalman filtering to predict the target position, and added a re-identification module to deal with occlusion problems, but such algorithms need to obtain high-precision target detection results first. In view of the above problems, the JDE (Jointlylearns the Detector and Embedding model) detection model can be used to integrate the re-identification and detection branches to improve the accuracy of target detection. The YOLO series of algorithms is a type of target detection algorithm under the one-stage frame based on the JDE detection model, which has the advantages of high efficiency, flexibility and good generalization performance. The YOLO algorithm performs target detection and directly uses the regression method to detect and classify the coordinate frame. It uses an end-to-end simple network to realize coordinate regression and classification, which can greatly improve the detection speed of the target.

[0006] However, on low-computing power mobile robot platforms, there are defects such as low computing power and small memory. Compared with the YOLO algorithm in normal situations, the larger model and computing power cannot meet the real-time detection requirements on low-computing power platforms. Summary of the invention

[0007] Based on this, it is necessary to provide a mobile robot formation control method based on lightweight improved YOLOv5 to reduce the model size and computational complexity.

[0008] At the same time, a mobile robot formation control system based on lightweight improved YOLOv5 with reduced costs is provided.

[0009] A mobile robot formation control method based on lightweight improved YOLOv5, comprising:

[0010] S100: Configuration calibration and image data processing: Configure and calibrate the RGBD camera. Build the leader image dataset;

[0011] S200: Obtaining image position: using an RGBD camera to collect an RGB image in real time, passing the RGB image into a target detection neural network to obtain a target detection result, and obtaining a detection frame position;

[0012] S300: Image position tracking: Based on the target detection result, a Kalman filter is used to predict and track the leader state;

[0013] S400: Get depth value: get the leader depth value;

[0014] S500: Obtaining spatial position: combining the depth value, solving the actual position of the leader relative to the camera coordinate system, and then obtaining the position of the leader in the map coordinate system;

[0015] S600: trajectory planning: drawing a Bezier curve of the leader's moving trajectory according to the position of the leader in the map coordinates to obtain the follower's moving trajectory;

[0016] S700: Trajectory tracking: The follower moves along the trajectory following the leader;

[0017] S800: Redetection: If the leader is lost, step S200 performs leader redetection.

[0018] In the preferred embodiment S100, the configuration calibration and image data processing further includes:

[0019] Matching: Match the RGBD camera, match the RGB image with the depth map, and obtain the depth value of each pixel in the RGB image through linear transformation, so that the RGB image and the depth image maintain the same resolution. i ,v j ), get the coordinates (u) from the depth map i ,v j ) Depth value d ij .

[0020] Calibration: Calibrate the RGBD camera external parameters and obtain the coordinate transformation relationship from the camera coordinate system to the mobile robot base coordinate system according to the structural design

[0021] Data acquisition: Collect leader images from different angles, distances, time, and lighting, use the image annotation tool LabelImg to annotate labels for each training sample, and save the position information and category information of the annotated anchor box as a text file to build a leader mobile robot image dataset;

[0022] Data preprocessing: randomly divide the acquired data set into training set and validation set;

[0023] In the preferred embodiment S200, the step of obtaining the image position includes:

[0024] Building a target detection neural network: The target detection neural network introduces the Shuffleblock module, GSconv module, CA attention module and DWConvblock module based on the traditional YOLOv5 model, and uses ShapeIOU as the loss function of the target detection neural network for lightweight improvement; the Shuffleblock module is used to replace all modules of the backbone network in the traditional YOLOv5 network model, the GSconv module replaces the Conv module in the neck network in the traditional YOLOv5 network model, and the DWConvblock module replaces the CSP module in the neck network in the traditional YOLOv5 network model. At the same time, the CA attention module is added to the fifth layer of the backbone network, and the ShapeIoU loss function is used to replace the CIoU loss function in the head structure.

[0025] The Shuffleblock module includes two forms: a basic module and a downsampling module. The basic module first performs channel cutting on the input feature map, performs 1×1 convolution, 3×3 depth-separable convolution, and 1×1 convolution on half of the channel feature map in sequence, and then concatenates it with the other half of the channel. The spliced ​​feature map performs a Channel Shuffl channel shuffling operation to recombine the channel group of the input feature map, promote information exchange between different channels, enhance cross-channel communication of features, and obtain a feature map of the same size as the input feature map. The downsampling module divides the input feature map into two branches and performs 1×1 convolution, 3×3 depth-separable convolution, 1×1 convolution, 3×3 depth-separable point convolution, and 1×1 convolution respectively, then concatenates the two branches, performs a Channel Shuffl channel shuffling operation on the spliced ​​feature map to obtain the feature map after the input feature map is downsampled. In the lightweight YOLOv5 network model backbone, the downsampling module is first used to process the input feature map to obtain a 1 / 2 input feature map, and then the basic module is used multiple times to extract features from the input feature map.

[0026] The CA attention module pools the input feature map in the X and Y directions respectively, concatenates the pooled feature map, and then uses a 1×1 convolution kernel to reduce the dimension and activate it. After splitting the feature map, the 1×1 convolution kernel is used to increase the dimension, and then the attention vector is obtained in combination with the sigmoid activation function.

[0027] The CSconv module first performs ordinary convolution on the input feature map, then performs depth-separable convolution on the obtained feature map and concatenates it with the ordinary convolution output feature map, and then performs Channel Shuffl operation on the concatenated feature map to output the feature map.

[0028] The DWConvblock module performs depth-wise separable convolution and point-wise convolution on the input feature map in sequence and then outputs the feature map through the relu activation function.

[0029] The head structure uses the ShapeIoU loss function to replace the CIoU loss function.

[0030] Perform model training: train and verify the target detection neural network using the training set and verification set obtained by data preprocessing to obtain a trained lightweight improved YOLOv5 network model;

[0031] Deploy the model: deploy the trained object detection neural network to a follower mobile robot;

[0032] Target detection: The real-time image obtained by the RGBD camera is input into the target detection neural network deployed above to obtain the initial tracking area, obtain the position of the leader in the image at the current moment, and set the coordinates of the upper left and lower right of the target detection box in the image to be (u lt ,v lt )、(u rb ,v rb ), take the center point of the image as the leader image position, and define the leader image position (u t ,v t ),get:

[0033] u t =(u lt +u rb ) / 2

[0034] v t =(v lt +v rb ) / 2.

[0035] When there are two leaders in the camera field of view at the same time, the positions of the two leaders in the image at the current moment are obtained, and the coordinates of the upper left and lower right of the target detection frame in the image are (u 1lt ,v 1lt )、(u 1rb ,v 1rb ) and (u 2lt ,v 2lt )、(u 2rb ,v 2rb ), define the positions of the two leader images as (u 1t ,v 1t )、(u 2t ,v 2t ),get:

[0036] u 1t =(u 1lt +u1rb ) / 2

[0037] v 1t =(v 1lt +v 1rb ) / 2

[0038] u 2t =(u 2lt +u 2rb ) / 2

[0039] v 2t =(v 2lt +v 2rb ) / 2.

[0040] In the preferred embodiment S300, the image position tracking mainly includes tracking the position of the leader according to the center coordinate position of the leader in the image using a Kalman filter, tracking the image coordinates of the center pixel point, and using the detection results to update the Kalman filter in real time to ensure the tracking effect.

[0041] In the preferred embodiment S400, the step of obtaining the depth value includes: using the RGBD camera to obtain the RGB image and the depth image at the current moment, setting the size of the depth image to be consistent with the size of the RGB image, the position of the leader in the RGB image is the same as the position of the leader in the depth image, and the position of the leader in the depth image is also (u t ,v t ), the value of the coordinate point in the depth image at this time is the leader depth value, defined as d t .

[0042] In the preferred embodiment S500, obtaining the map coordinate position includes: converting the leader area depth value into the coordinates in the lens coordinate system, that is, obtaining the three-dimensional coordinates p of the leader in the lens coordinate system. i =[x i ,y i ,z i ,1] T , set the camera internal parameters (c x ,c y ,f x ,f y ), is converted as follows:

[0043]

[0044] Where s is the conversion coefficient, and the conversion relationship from the camera lens coordinate system to the robot base coordinate system is obtained Convert the leader to the base coordinate system:

[0045]

[0046] When there are two leaders, the positions of the leaders relative to the robot base coordinate system are:

[0047]

[0048] After conversion, the leader is converted to map coordinates:

[0049]

[0050] The transformation from the robot base coordinate system to the map coordinate system is:

[0051] When there are two leaders in the field of view, the distance between the leader and the follower is calculated, and the leader closest to it is selected to track its trajectory.

[0052] In the preferred embodiment S600, the trajectory planning includes:

[0053] Regularly save the leader's coordinates P in the map coordinate system i , where i = 0, 1, 2, 3. After obtaining the map coordinates of the four leaders, draw the Bezier curve to initialize the follow-up trajectory: P0 and P1, P1 and P2, P2 and P3 all constitute the first-order Bezier point p 1,1 (t), p 1,2 (t) and p 1,3 (t), represents the time scale t:

[0054]

[0055] Based on the three generated first-order points, two second-order Bezier points p can be generated 2,1 (t) and p 2,2 (t):

[0056]

[0057] Based on the two generated second-order points, the third-order Bessel point p3(t) can be generated:

[0058] p3(t)=(1-t)p 2,1 +tp 2,2

[0059] Right now:

[0060] p3(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3

[0061] Draw and splice subsequent Bezier coordinate points based on the leader map coordinates obtained in real time, draw a Bezier curve as the leader's moving trajectory, set the offset distance, and the following mobile robot generates a tracking trajectory for the Bezier curve based on the offset distance;

[0062] Trajectory tracking: The follower moves according to the trajectory of the leader. Using model predictive control (MPC), the follower mobile robot is controlled to track the trajectory.

[0063] The re-detection described in the preferred embodiment S800 means that after the leader is lost from the field of view of the follower's camera, the leader detection result cannot be obtained. At this time, the follower stops moving and rotates on the spot to re-acquire the leader's image. After successfully acquiring the leader's image, the above process is repeated to enable the follower to continue to follow the leader.

[0064] A mobile robot formation control system based on lightweight improved YOLOv5, including:

[0065] Configuration and calibration module: configure and calibrate the RGBD camera to ensure the accuracy of image data;

[0066] Detection module: The image acquired by the RGBD camera is passed into the target detection neural network to obtain the target detection result, the image position of the leading robot in the field of view, and the result is sent to the image tracking module;

[0067] Image tracking module: based on the target detection result, the Kalman filter is used to predict the tracking leader position and output it to the depth value acquisition module;

[0068] Depth value acquisition module: According to the RGB image position where the leader image is tracked, the value of the depth map at that position is obtained, and the result is sent to the spatial position acquisition module;

[0069] Spatial position acquisition module: convert the leader area depth value into the coordinates in the lens coordinate system, obtain the actual position of the leader relative to the camera coordinate system, and then obtain the position of the leader in the map coordinate system;

[0070] Trajectory planning module: Draws a Bezier curve as the follower's movement trajectory based on the leader's position in the map coordinates, and sends the trajectory to the path following module;

[0071] Path following module: follow the leader's movement according to the trajectory;

[0072] Re-detection module: After the leader is lost from the follower's camera field of view, the follower stops moving, rotates in place, and re-detects the leader's position.

[0073] The above-mentioned mobile robot formation control method and system based on lightweight improved YOLOv5 uses a low-cost RGBD camera, which is conducive to reducing the cost of the robot. The RGBD camera can provide three-dimensional information. The present invention is suitable for using the RGBD camera to follow the leader. The RGBD camera is already equipped, and no additional sensors are required to realize the following function. The lightweight YOLOv5 detection algorithm is used to reduce the amount of calculation required for deep learning image processing, and can be used on a low-computing platform. The navigation framework can continue to use an open source framework. The robot's navigation framework can use the navigation framework under ROS (Robot Operating System). If the method of the present invention is used to realize the same leader following function, the original navigation framework does not need to be modified. The Bezier trajectory generated according to the leader's position is used as the navigation target point input navigation, and the follower robot walks along the planned path, and the overall framework does not need to be changed. Add the processing logic of leader loss. For the follower in the following process, the leader may be lost. Generally, the loss may be caused by the leader being blocked by an obstacle or the follower robot turning too far around the obstacle. Adding the processing logic after the leader is lost helps to maintain the smoothness of the follower tracking, and can quickly re-identify the leader for tracking. From the perspective of product cost and power consumption, the processor computing power of a general service robot is not too high. Using an algorithm with too much computation obviously cannot meet the requirements of real-time tracking, and using an algorithm with too much computation for tracking will also affect the functions of other parts. Since RGB and depth are obtained from different lenses, the synchronization and registration problems between the two are considered, and registration is performed to obtain the precise position of the target and improve the tracking accuracy. In order to solve the tracking problem after the leader is lost, a leader prediction and re-detection method is proposed. The RGBD camera has sufficient close-range accuracy, provides three-dimensional obstacle information, and is cheap. The present invention does not require users to add additional sensors, which can reduce the difficulty of use. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 is a flow chart of the present invention;

[0075] Figure 2 This is a diagram of the lightweight YOLOv5 network architecture of the present invention;

[0076] Figure 3 This is an example diagram of the detection results of the lightweight YOLOv5 detection model obtained after training of the present invention;

[0077] Figure 4 It is a re-detection flow chart of the present invention. DETAILED DESCRIPTION

[0078] The following embodiments are provided to facilitate a better understanding of the present invention, but are not intended to limit the present invention.

[0079] like Figure 1 As shown, a lightweight improved YOLOv5 mobile robot formation control method of the present invention comprises:

[0080] Step S100, configuration calibration and image data processing: configure and calibrate the RGBD camera. Construct a leader image dataset;

[0081] Step S200, obtaining the image position: using an RGBD camera to collect an RGB image in real time, passing the RGB image into a target detection neural network to obtain a target detection result, and obtaining the detection frame position;

[0082] Step S300, image position tracking: tracking the leader state based on the target detection result and Kalman filter prediction;

[0083] Step S400, obtaining a depth value: obtaining a leader depth value;

[0084] Step S500, obtaining the spatial position: calculating the actual position of the leader relative to the camera coordinate system, and then obtaining the position of the leader in the map coordinate system;

[0085] Step S600, trajectory planning: drawing a Bezier curve of the leader's moving trajectory according to the leader's map position to obtain the follower's moving trajectory;

[0086] Step S700, path following: the follower moves along the leader according to the trajectory;

[0087] Step S800, re-detection: if the leader is lost, re-detection of the leader is performed.

[0088] RGBD camera refers to a camera that measures distance physically, and the result is an RGB image + a D depth image.

[0089] Matching: Match the RGBD camera, match the RGB image with the depth map, and obtain the depth value of each pixel in the RGB image through linear transformation, so that the RGB image and the depth image maintain the same resolution. i ,v j ), get the coordinates (u) from the depth map i ,v j ) Depth value d ij .

[0090] Calibration: Calibrate the RGBD camera external parameters and obtain the coordinate transformation relationship from the camera coordinate system to the mobile robot base coordinate system according to the structural design

[0091] Data acquisition: Collect leader images from different angles, distances, time, and lighting, use the image annotation tool LabelImg to annotate labels for each training sample, and save the position information and category information of the annotated anchor box as a YOLO format text file to build a leader mobile robot image dataset for subsequent training of a lightweight YOLOv5 detection model;

[0092] Data preprocessing: The acquired data set was randomly divided into training set and validation set in a ratio of 7:3;

[0093] Furthermore, the step of obtaining the image position in this embodiment includes:

[0094] Build a target detection neural network, such as Figure 2 As shown: a target detection neural network is built, wherein the lightweight YOLOv5 network model introduces the Shuffleblock module, the GSconv module, the CA attention module and the DWConvblock module based on the traditional model, and adopts ShapeIOU as the loss function of the target detection neural network, the Shuffleblock module replaces all the modules of the backbone network in the traditional YOLOv5 network model, the GSconv module replaces the Conv module in the neck network in the traditional YOLOv5 network model, the DWConvblock module replaces the CSP module in the neck network in the traditional YOLOv5 network model, and at the same time, the CA attention module is inserted in the fifth layer of the backbone network, and the head structure uses the ShapeIoU loss function to replace the CIoU loss function.

[0095] The present invention replaces all modules in the original backbone network with a lightweight module Shuffleblock, and reduces the original ten layers to eight layers. The Shuffleblock module is a standard module inside the ShuffleNetV2 network. The ShuffleNetV2 network meets the four criteria for an efficient network: balanced channel width, moderate group convolution, reduced fragmentation, and emphasis on element operations. Optimize memory access and parallel computing to improve the reasoning speed and accuracy of the model. At the same time, a CA attention module is inserted into the fifth layer of the backbone network. The CA attention module not only considers channel information, but also direction-related position information, and is flexible and lightweight enough to be simply inserted into the core module of the lightweight network to improve accuracy;

[0096] The present invention adopts a DWConvblock module to replace the CSP module in the neck network of the traditional YOLOv5 network model, and at the same time, adopts a GSConv module to replace the Conv module in the neck network of the traditional YOLOv5 network model, both of which can greatly optimize the parameter scale and computing resource consumption of the network without affecting the network training detection accuracy.

[0097] Perform model training: Use the training set and validation set obtained by data preprocessing to train the target detection neural network, set the training rounds to 400 rounds, and obtain the optimal weight model;

[0098] In order to verify the actual effect of the present invention, a comparative experiment was conducted on the YOLOv5n model and the improved model. The experimental results are shown in Table 1. The size of the improved model is 1.51MB, the number of model parameters is 654230, and the number of floating-point operations is 2.2GFLOPs. The size of the original YOLOv5n model is 3.63MB, the number of model parameters is 1760518, and the number of floating-point operations is 4.1GFLOPs. After the improvement, the model size is reduced by about 58%, the number of model parameters is reduced by about 62%, the number of floating-point operations is reduced by about 46%, and the average accuracy of the model is reduced by about 1%, achieving a lightweight improvement of the model and achieving the expected goal.

[0099] Table 1 Model comparison experiment

[0100]

[0101] Deployment model: deploy the trained target detection neural network to the follower mobile robot, and the robot framework uses the ROS robot framework;

[0102] Target detection: The real-time image obtained by the RGBD camera is input into the target detection neural network deployed above to obtain the initial tracking area and obtain the current position of the leader in the image. The detection results are as follows: Figure 3 As shown, the coordinates of the upper left and lower right of the target detection frame in the image are set to be (u lt ,v lt )、(u rb ,v rb ), take the center point of the image as the leader image position, and define the leader position (u t ,v t ),get:

[0103] u t =(u lt +u rb ) / 2

[0104] v t =(v lt +v rb ) / 2.

[0105] When there are two leaders in the camera field of view at the same time, the positions of the two leaders in the image at the current moment are obtained, and the coordinates of the upper left and lower right of the target detection frame in the image are (u 1lt ,v 1lt )、(u1rb ,v 1rb ) and (u 2lt ,v 2lt )、(u 2rb ,v 2rb ), define the positions of the two leader images as (u 1t ,v 1t )、(u 2t ,v 2t ),get:

[0106] u 1t =(u 1lt +u 1rb ) / 2

[0107] v 1t =(v 1lt +v 1rb ) / 2

[0108] u 2t =(u 2lt +u 2rb ) / 2

[0109] v 2t =(v 2lt +v 2rb ) / 2.

[0110] The image position tracking uses a Kalman filter to track the center coordinate position of the leader in the image. Kalman filter is an efficient autoregressive filter that can estimate the state of a dynamic system in the presence of a combination of information with many uncertainties. It is a powerful and highly versatile tool. Kalman filter tracking mainly includes two steps: prediction and tracking.

[0111] Prediction Phase:

[0112]

[0113] The leader's state space at time t is defined as x t ,y t is the coordinate of the center point of the detection box in the state space, a is the aspect ratio of the detection box in the state space, h is the height of the detection box in the state space, and dx, dy, da, dh are the corresponding first-order differences. t is the prediction matrix used to predict Fixed. t is the uncertainty interference, is the diagonal matrix and Related, the larger the bit element, the larger the value.

[0114]

[0115] Update phase:

[0116]

[0117] For the observed quantity a is the detection frame aspect ratio, and h is the detection frame height. t for The covariance matrix of is a diagonal matrix, with the corresponding elements is proportional to the size of H. t It is the mapping matrix, which realizes the mapping from state space to prediction space and is fixed by default.

[0118] The predicted state output by the Kalman filter is the result of the Kalman filter tracking the leader image position. The Kalman filter has a small memory footprint and is fast, making it an ideal choice for real-time problems and embedded systems. Using a Kalman filter to track the leader can reduce the frequency of target detection, further reducing the computing resources required for the entire following process.

[0119] Further, the step of obtaining the depth value of this embodiment includes: using the RGBD camera to obtain the RGB image and the depth image at the current moment, setting the size of the depth image to be consistent with the size of the RGB image, the position of the leader in the RGB image is the same as the position of the leader in the depth image, and the position of the depth information of the leader in the depth image is also (u t ,v t ), the value of the coordinate point in the depth image at this time is the leader depth value, defined as d t .

[0120] Furthermore, the step of obtaining the spatial position in this embodiment includes:

[0121] Convert the depth value of the target area to the coordinates in the lens coordinate system, that is, obtain the three-dimensional coordinates p of the target in the lens coordinate system i =[x i ,y i ,z i ,1] T , set the camera internal parameters (c x ,c y ,f x ,f y ), is converted as follows:

[0122]

[0123] Where s is the conversion coefficient, and the conversion relationship from the camera lens coordinate system to the robot base coordinate system is obtained Convert the leader to the base coordinate system:

[0124]

[0125] When there are two leaders, the positions of the leaders relative to the robot base coordinate system are:

[0126]

[0127] After conversion, the leader is converted to map coordinates:

[0128]

[0129] When there are two leaders in the field of view, the distance between the leader and the follower is calculated, and the leader closest to it is selected to track its trajectory.

[0130] Transformation from robot base coordinate system to map coordinate system

[0131] Trajectory planning: regularly save the leader's coordinates P in the map coordinate system i , where i = 0, 1, 2, 3. After obtaining the map coordinates of the four leaders, draw the Bezier curve to initialize the follow-up trajectory: P0 and P1, P1 and P2, P2 and P3 all constitute three first-order Bezier points:

[0132]

[0133] Based on the three generated first-order points, two second-order Bezier points can be generated:

[0134]

[0135] Based on the two generated second-order points, third-order Bezier points can be generated:

[0136] p3(t)=(1-t)p 2,1 +tp 2,2

[0137] Right now:

[0138] p3(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3 draws and splices subsequent Bezier coordinate points based on the leader map coordinates obtained in real time, makes a leader trajectory based on a series of Bezier coordinate points obtained, and generates a tracking trajectory by following the mobile robot and offsetting the leader's moving trajectory by a certain distance.

[0139] Path following: Use model predictive control to find the optimal control quantity to control the follower mobile robot to track the trajectory. Model predictive control uses modern control theory to build a prediction model to predict a series of state points after multiple sampling moments in the future, and compare the series of future predicted trajectories with the reference trajectory. Usually, the mean square error can be calculated. In this way, at this moment, the control quantity is changed, and a series of state points after the same sampling moment are predicted and evaluated. After multiple predictions, the set of control quantities with the best evaluation results are used as the control quantity input at the current moment. This is repeated to form a model predictive control method.

[0140] Using the model predictive control method, we must first build a kinematic model of the following mobile robot:

[0141] Follow the robot state vector x = [x, y, θ] T represents the position and orientation of the follower robot relative to the global coordinate system, where x and y are the coordinate positions and θ is the orientation of the follower robot;

[0142] Follow the robot control input u ω =[v l ,v r ] T Indicates the linear velocity of the left and right wheels of the following robot, u=[v,ω] T .

[0143] The kinematic equation of the following robot is expressed as:

[0144]

[0145] Select the state variable x = [x, y, θ] T , the control quantity is u=[v,ω] T , then for any reference point on the reference trajectory, represented by r, the above formula can be rewritten as:

[0146]

[0147] where x r =[x r ,y r ,θ r ] T ,u r =[v r ,ω r ] T . The above equation is expanded using Taylor series at the reference point and higher-order terms are ignored:

[0148]

[0149] right and To find the Jacobian matrix, we have:

[0150]

[0151] Then the change of state error is It is expressed as:

[0152]

[0153] Forward Euler discretization yields:

[0154]

[0155] Define the output equation:

[0156]

[0157] Construct a new state vector:

[0158]

[0159] Then the new state space expression is:

[0160]

[0161] in

[0162] The output equation is:

[0163]

[0164] After multiple steps of derivation of the state vector and the output equation, the output equation can be rewritten as:

[0165] Y=ψξ(k)+ΘΔU

[0166] in:

[0167]

[0168] At this time, if the current state quantity and control time domain N are known c The control increment within can also predict the future time domain N p system output.

[0169] Make the following robot move completely along the trajectory and define the reference value of the system output:

[0170] Y r =[η r (k+1) η r (k+2) … η r (k+N c ) … η r (k+Np )] T

[0171] =[0 0 … 0 … 0] T

[0172] Let E = ψξ(k), The optimization objective function is defined as:

[0173] J=(YY r ) T Q Q (YY r )+ΔU T R R ΔU

[0174] =ΔU T (Θ T Q Q Θ+R R )ΔU+2E T Q Q ΘΔU+E T Q Q EY r Q Q ΘΔU

[0175] +Y r T Q Q Y-2Y r T Q Q E

[0176] Let H = Θ T Q Q Θ+R R ,g=E T Q Q Θ, then the above formula can be rewritten as:

[0177]

[0178] For the control quantity and control increment, there are:

[0179]

[0180] The acceleration of the following robot is limited by the motors:

[0181]

[0182] In summary, the model predictive control problem is transformed into a standard quadratic programming problem:

[0183]

[0184] Solve the optimal input change, obtain the optimal input of the following mobile robot, and realize trajectory tracking.

[0185] like Figure 4 As shown, further re-detection in this embodiment includes:

[0186] Leaders follow;

[0187] Determine if the leader is lost in the camera's field of view;

[0188] If it is determined that the leader has not been lost, continue to follow;

[0189] If the leader is judged to be lost, it is also judged whether the loss exceeds the set time;

[0190] If the loss does not exceed the set time, the target prediction is started: the next leader position is predicted based on the previous target running state of the leader robot;

[0191] If the loss time exceeds the set time, the follower stops moving, rotates on the spot, and starts re-detection of the target; it determines whether the leader can be re-detected, and if so, it follows the leader;

[0192] If not, continue to stop moving and start target detection;

[0193] The target prediction includes: setting the leader position detected at the last moment and the last moment to be P respectively m,t-2 and P m,t-1 At this moment, the leader is out of sight, making it impossible to get the leader. Use the positions of the last two moments to predict the current position P m,t :

[0194] P m,t =2P m,t-1 -P m,t-2

[0195] Use the lightweight improved YOLOv5 target detection algorithm for detection. If the leader can be detected, repeat the above process to enable the follower to continue to follow the leader. If no result can be detected, continue to detect until the leader's position can be re-detected.

[0196] A mobile robot formation control system based on lightweight improved YOLOv5 according to an embodiment of the present invention is characterized by comprising:

[0197] Configuration and calibration module: configure and calibrate the RGBD camera to ensure the accuracy of image data;

[0198] Detection module: The camera image is passed into the target detection neural network to obtain the target detection result, leading the robot's image position within the field of view, and the result is sent to the image tracking module;

[0199] Image tracking module: based on the target detection result, the Kalman filter is used to predict the tracking leader position and output it to the depth value acquisition module;

[0200] Depth value acquisition module: According to the RGB image position where the leader image is tracked, the value of the depth map at that position is obtained, and the result is sent to the spatial position acquisition module;

[0201] Spatial position acquisition module: According to the camera pinhole model, the depth value of the leader area is converted into the coordinates in the lens coordinate system, and the actual position of the leader relative to the camera coordinate system is obtained, and then the position of the leader in the map coordinate system is obtained;

[0202] Trajectory planning module: Based on the position of the leader in the map coordinates obtained by the spatial position acquisition module, a Bezier curve is drawn as the follower's movement trajectory, and the trajectory is sent to the path following module;

[0203] Path following module: follow the leader's movement according to the trajectory;

[0204] Re-detection module: After the leader is lost from the follower's camera field of view, the follower stops moving, rotates in place, and re-detects the leader's position.

[0205] The present invention is based on a mobile robot formation control method and system of lightweight improved YOLOv5, using a low-cost RGBD camera, which is conducive to reducing the cost of the robot. The RGBD camera can provide three-dimensional information, and the present invention is suitable for RGBD cameras for following. The RGBD camera is already equipped, and no additional sensors are required to realize the following function. The lightweight YOLOv5 detection algorithm is used to reduce the amount of calculation required for deep learning image processing, and can be used on a platform with low computing power. The navigation framework can continue to use an open source framework. The robot's navigation framework can use the navigation framework under ROS (Robot Operating System). If the method of the present invention is used to realize the same following function, the original navigation framework does not need to be modified. The Bezier trajectory generated according to the leader's position is used as the navigation target point input navigation, and the follower robot walks along the planned path, and the overall framework does not need to be changed. The processing logic of leader loss is added. For the follower in the following process, the leader may be lost. Generally, the loss may be caused by the leader being blocked by an obstacle or the robot turning too far around the obstacle. Adding the processing logic after the leader is lost helps to maintain the smoothness of machine tracking and can quickly re-identify the leader for tracking.

[0206] The present invention is based on a mobile robot formation control method and system based on a lightweight improved YOLOv5 to reduce the computational load of the follower mobile robot. From the perspective of product cost and power consumption, the processor computing power on a general service robot is not too high. Using an algorithm with too much computational load obviously cannot meet the requirements of real-time tracking, and using an algorithm with too much computational load for tracking will also affect the functions of other parts. Since RGB and depth are obtained from different lenses, the synchronization and registration problems between the two are considered, and registration is performed to obtain the precise position of the target and improve the tracking accuracy. In order to solve the tracking problem after the leader is lost, a leader prediction and re-detection method is proposed. The RGBD camera has sufficient close-range accuracy, provides three-dimensional obstacle information, and is cheap. The present invention does not require users to add additional sensors, which can reduce the difficulty of use.

[0207] Based on the above ideal embodiments of this application, the relevant staff can make various changes and modifications without deviating from the technical concept of this application through the above description. The technical scope of this application is not limited to the content in the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A mobile robot formation control method based on lightweight improved YOLOv5, characterized in that: The following steps are involved: S1: Configure and calibrate the RGBD camera and build the leader image dataset; S2: Collect RGB images in real time through the RGBD camera, pass the RGB images into the target detection neural network to obtain the target detection results, and obtain the detection frame position; S3: Based on the target detection results, the Kalman filter is used to predict the tracking leader state; S4: Get the leader depth value; S5: Combine the depth value to calculate the actual position of the leader relative to the camera coordinate system, and obtain the position of the leader in the map coordinate system; S6: Draw a Bezier curve of the leader's moving trajectory according to the leader's position under the map coordinates to obtain the follower's moving trajectory; S7: The follower moves along the leader according to the trajectory; S8: If the leader is lost, return to step S2 to re-detect the leader.

2. The mobile robot formation control method based on lightweight improved YOLOv5 according to claim 1 is characterized in that: The step S1 specifically The process includes: Matching: Match the RGBD camera, match the RGB image with the depth map, obtain the depth value of each pixel in the RGB image through linear transformation, and keep the RGB image and the depth image with the same resolution; after matching, the known pixel coordinates of any RGB image (u i ,v j ), get the coordinates (u) from the depth map i ,v j ) Depth value d ij ; Calibration: Calibrate the RGBD camera external parameters and obtain the coordinate transformation relationship from the camera coordinate system to the mobile robot base coordinate system according to the structural design Data acquisition: Collect leader images from different angles, distances, time, and lighting, use image annotation tools to label each sample, and save the position information and category information of the annotated anchor box as a text file to build a leader mobile robot image dataset; Data preprocessing: The acquired data set is randomly divided into training set and validation set in proportion.

3. The mobile robot formation control method based on lightweight improved YOLOv5 according to claim 2 is characterized in that: The specific implementation process of the target detection neural network is as follows: Building a target detection neural network: The target detection neural network introduces the Shuffleblock module, GSconv module, CA attention module and DWConvblock module based on the YOLOv5 model, and uses ShapeIOU as the loss function of the target detection neural network for lightweight improvement; the Shuffleblock module is used to replace all modules of the backbone network in the traditional YOLOv5 network model, the GSconv module replaces the Conv module in the neck network in the traditional YOLOv5 network model, and the DWConvblock module replaces the CSP module in the neck network in the traditional YOLOv5 network model. At the same time, the CA attention module is added to the fifth layer of the backbone network, and the head structure uses the ShapeIoU loss function to replace the CIoU loss function; Perform model training: Use the training set and validation set to train and validate the target detection neural network; Deploy the model: Deploy the trained object detection neural network to the follower mobile robot; Target detection: The real-time image obtained by the RGBD camera is input into the target detection neural network deployed above to obtain the initial tracking area, obtain the position of the leader in the image at the current moment, and set the coordinates of the upper left and lower right of the target detection box in the image to be (u lt ,v lt )、(u rb ,v rb ), take the center point of the image as the leader image position, and define the leader image position (u t ,v t ): in t =(in lt +in rb ) / 2 v t =(v lt +v rb ) / 2 When there are two leaders in the camera field of view at the same time, the positions of the two leaders in the image at the current moment are obtained, and the coordinates of the upper left and lower right of the target detection frame in the image are (u 1lt ,v 1lt )、(u 1rb ,v 1rb ) and (u 2lt ,v 2lt )、(u 2rb ,v 2rb ), define the positions of the two leader images as (u 1t ,v 1t )、(u 2t ,v 2t ),get: in 1t =(in 1lt +in 1rb ) / 2 v 1t =(v 1lt +v 1rb ) / 2 in 2t =(in 2lt +in 2rb ) / 2 v 2t =(v 2lt +v 2rb ) / 2。 4. The mobile robot formation control method based on lightweight improved YOLOv5 according to claim 3 is characterized in that: The Shuffleblock module includes two forms: a basic module and a downsampling module; the basic module first performs channel cutting on the input feature map, performs 1×1 convolution, 3×3 depth-separable convolution, and 1×1 convolution on half of the channel feature map in sequence, and then performs Concate splicing with the other half of the channel, performs Channel Shuffl channel shuffling operation on the spliced ​​feature map to recombine the channel group of the input feature map to obtain a feature map of the same size as the input feature map; the downsampling module divides the input feature map into two branches and performs 1×1 convolution, 3×3 depth-separable convolution, 1×1 convolution operation and 3×3 depth-separable point convolution, 1×1 convolution respectively, performs Concate splicing on the two branches, performs Channel Shuffl channel shuffling operation on the spliced ​​feature map to obtain a feature map after the input feature map is downsampled; in the lightweight YOLOv5 network model backbone, the input feature map is first processed by the downsampling module to obtain a 1 / 2 input feature map, and then the basic module is used multiple times thereafter to extract features from the input feature map; The CA attention module pools the input feature map in the X and Y directions respectively, concatenates the pooled feature map, and then uses a 1×1 convolution kernel to reduce the dimension and activate it. After splitting the feature map, it uses a 1×1 convolution kernel to increase the dimension, and then combines the sigmoid activation function to obtain the attention vector. The CSconv module first performs ordinary convolution on the input feature map, then performs depth-separable convolution on the obtained feature map and concatenates it with the ordinary convolution output feature map, and then performs Channel Shuffl operation on the concatenated feature map to output the feature map; The DWConvblock module performs depth-wise separable convolution and point-wise convolution on the input feature map in sequence and then outputs the feature map through the relu activation function.

5. The mobile robot formation control method based on lightweight improved YOLOv5 according to claim 4 is characterized in that: The implementation process of step S3 includes: tracking the location of the leader according to the center coordinate position of the leader in the image using a Kalman filter, tracking the image coordinates of the center pixel point, and updating the Kalman filter in real time using the detection result.

6. The mobile robot formation control method based on lightweight improved YOLOv5 according to claim 5 is characterized in that: The step S4 is specifically implemented as follows: using the RGBD camera to obtain the RGB image and the depth image at the current moment, setting the size of the depth image to be consistent with the size of the RGB image, the position of the leader in the RGB image is the same as the position of the leader in the depth image, and the position of the leader in the depth image is also (u t ,v t ), the value of the coordinate point in the depth image at this time is the leader depth value, defined as d t .

7. The mobile robot formation control method based on lightweight improved YOLOv5 according to claim 6 is characterized in that: The specific implementation process of step S5 is as follows: Convert the leader area depth value to the coordinates in the lens coordinate system, that is, obtain the three-dimensional coordinates p of the leader in the lens coordinate system i =[x i ,y i ,z i ,1] T , set the camera internal parameters (c x ,c y ,f x ,f y ), is converted as follows: Where s is the conversion coefficient, and the conversion relationship from the camera lens coordinate system to the robot base coordinate system is obtained Convert the leader to the base coordinate system: When there are two leaders, the leader is relative to the robot bas e The positions of the coordinate systems are: After conversion, the leader is converted to map coordinates: The transformation from the robot base coordinate system to the map coordinate system is: When following two leaders, the distance between the leader and the follower is calculated, and the leader closest to it is selected to track its trajectory.

8. The mobile robot formation control method based on lightweight improved YOLOv5 according to claim 7 is characterized in that: The specific implementation process of step S6 is as follows: Regularly save the leader's coordinates P in the map coordinate system i , where i = 0, 1, 2, 3. After obtaining the map coordinates of the four leaders, draw the Bezier curve to initialize the follow-up trajectory: P0 and P1, P1 and P2, P2 and P3 all constitute three first-order Bezier points p 1，1 (t), p 1，2 (t) and p 1，3 (t), represents the time scale t: Based on the three generated first-order points, two second-order Bezier points p are generated. 2，1 (t) and p 2，2 (t): Based on the two generated second-order points, generate the third-order Bessel point p3(t): p3(t)=(1-t)p 2,1 +tP 2,2 Right now: p3(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3 Draw and splice subsequent Bezier coordinate points based on the leader map coordinates obtained in real time, draw a Bezier curve, set the offset distance, and follow the mobile robot to generate a tracking trajectory for the Bezier curve based on the offset distance; The model predictive control method MPC is used to generate the optimal control quantity, and the followers are controlled to move along the path following the leader.

9. A mobile robot formation control system based on lightweight improved YOLOv5, used to implement the mobile robot formation control method according to any one of claims 1 to 8, characterized in that: Includes the following modules: Configuration and calibration module: configure and calibrate the RGBD camera; Detection module: The image acquired by the RGBD camera is passed into the target detection neural network to obtain the target detection result, the image position of the leading robot in the field of view, and the result is sent to the image tracking module; Image tracking module: Based on the target detection results, the Kalman filter is used to predict the tracking leader position and output it to the depth value acquisition module; Depth value acquisition module: According to the RGB image position where the leader image is tracked, the value of the depth map at that position is obtained, and the result is sent to the spatial position acquisition module; Spatial position acquisition module: convert the leader area depth value into the coordinates in the lens coordinate system, obtain the actual position of the leader relative to the camera coordinate system, and obtain the position of the leader in the map coordinate system; Trajectory planning module: Draws a Bezier curve as the follower's movement trajectory based on the leader's position in the map coordinates, and sends the trajectory to the path following module; Path following module: follow the leader's movement according to the trajectory; Redetection module: After the leader is lost from the follower's camera field of view, the follower stops moving, rotates in place, and redetects the leader's position.

Images