A target following method for mobile robots based on SAC-PID

Abstract

The present invention relates to a target following method for a mobile robot based on SAC-PID. The method acquires and preprocesses robot perception data, uses a target detection network to obtain candidate regions for following objects, and after matching the following objects, collects state information for reinforcement learning. Using a trained SAC reinforcement learning model, the method outputs the target angular velocity and target linear velocity for the mobile robot's motion control, achieving end-to-end target following decision output for the mobile robot and smooth control of the mobile robot using a PID controller. The present invention has the beneficial effects of establishing a model for tracking targets, effectively distinguishing between background and follow-up targets, and allowing the follow-up method to continue to be applied even in the presence of occlusion. The method is simple to implement, has fast convergence speed, and achieves high target following accuracy. It can cope with complex application scenarios, possesses strong robustness, and achieves smooth control of the tracking process.

Description

Technical Field

[0001] The present invention relates to the technical field of calculation, estimation and counting, and in particular to a mobile robot target following method based on SAC-PID that combines SAC and PID control in the field of intelligent robots. Background Art

[0002] With the rapid development of technologies such as mobile robots and artificial intelligence, intelligent robots are gradually becoming a part of our work and life. Compared with traditional industrial robots, intelligent robots have a stronger perception of the external world and lower requirements for positioning systems, obstacle detection, and map information. They can complete flexible and precise operations in complex environments. Human-machine integration is also a key trend in the development of intelligent robots. In the logistics industry, following intelligent robots have the advantages of higher handling efficiency, greater handling capacity, and more friendly human-machine collaboration compared to traditional manual handling. Following tasks require the robot to complete the following motion task without collision after determining the object to be followed, while maintaining a safe distance from the object. This requires certain obstacle avoidance and target detection capabilities.

[0003] Existing robot following technologies can be primarily categorized into template matching-based following methods, estimator-based following methods, and classifier-based following methods. Template matching-based following methods select the target information in the tracking area of ​​the first frame as a matching template. In subsequent frames, the area with the smallest matching error with the template is selected as the matching result to achieve target following. These methods offer the advantages of simplicity and fast convergence, and have been widely used in the field of object tracking. Estimator-based following methods use a target following model to predict the target's position in the next frame and update the target's current state. Different types of estimators are designed for different scenarios to improve target following accuracy. Classifier-based target following methods use online learning to learn classification rules based on given categories and known training data, and then classify unknown data. These methods are capable of handling complex application scenarios and exhibit strong robustness. While these methods offer various advantages, they still face numerous challenges, such as establishing a target tracking model, distinguishing between background and target, resolving tracking failures under occlusion, and achieving smooth control of the tracking process.

[0004] Patent application number 201810980715.6 discloses a target object following method for a mobile robot based on a monocular vision sensor. This method obtains an image feature matrix of the following object based on the target area of ​​the target object, and then uses a tracking algorithm and the feature matrix to determine the center point of the target object to achieve target following. However, this method cannot effectively solve the problem of target loss during the following process. Patent application number 202010993247.3 discloses a vision-based robot following method and a following robot. This method obtains pedestrian detection results by inputting image information into a detection neural network for detection, and then controls the robot to follow according to the target results. However, this method only uses a single sensor to obtain environmental information, which is easily affected by environmental factors, and its robustness needs to be improved. Summary of the Invention

[0005] The present invention solves the problems existing in the prior art and provides an optimized mobile robot target following method based on SAC-PID. It adopts a deep learning target detection network to detect candidate following targets, uses a deep reinforcement learning algorithm SAC to realize the robot's target following decision, and realizes smooth action output through PID.

[0006] The technical solution adopted by the present invention is a target following method of a mobile robot based on SAC-PID, the method comprising the following steps:

[0007] Step 1: Use the calibrated RGBD camera and lidar to obtain a frame of real-time RGBD data during the mobile robot's target following process. RGBD and radar data R of N points N , will I RGBD Split into RGB image I RGB and depth image I D , in preparation for subsequent target detection and follow-up object matching, R N Perform k-fold downsampling to obtain radar data R N / k , reduce R N / k In subsequent steps, the occupation of the state space is reinforced;

[0008] Step 2: Use the target detection network trained with the human dataset to detect the RGB image I RGB Detect the humanoid target in the image and obtain the candidate region ROI of the following object t,i ; During initialization, if the follow object is not determined, the candidate region ROI with the largest pixel area will be selected. t,i Designate OBJ0 as the initial follow-up object as the benchmark for subsequent target matching and return to step 1, otherwise proceed to the next step; where t is the time index of the current frame and i is the candidate region index;

[0009] Here, the object detection network mainly follows human-shaped targets and is trained with the dataset. The confidence level is increased by adding some additional human-shaped samples taken by local cameras to the dataset.

[0010] Initial tracking target uncertainty refers to the process of determining the tracking object after the tracking algorithm is started and the first frame of image is obtained, which facilitates the subsequent locking of the tracking object. Based on this, since the target of tracking is generally a person, among the several human detection ROIs in the detection results of a frame of image, the frame with the largest pixel area is selected as the main tracking object in the camera.

[0011] Step 3: For all the candidate regions ROI obtained t,i Perform similarity judgment and select the area with the greatest similarity as the following object OBJ of the current frame t , get the follow object OBJ t In the RGB image I RGB Location information in t and the distance information d between it and the mobile robot t ;Solve the impact of suspected objects on the robot's target following during the following process;

[0012] Step 4: Collect the state information S of reinforcement learning based on steps 1 and 3 t ; The laser radar data R obtained in step 1 N / k Add it to the state space and follow the target object OBJ obtained in step 3 t Location information S l and distance information S d Also added to the state space;

[0013] Step 5: S t Input the trained SAC reinforcement learning model and output the target angular velocity ω of the mobile robot motion control target and target linear velocity v target , achieving end-to-end target following decision output of mobile robots;

[0014] Step 6: During the target following process transferred to the real world, the robot cannot instantly reach the target angular velocity ω output by the reinforcement learning policy network. target and target linear velocity v target , and the state observation will change greatly during the migration process, which may easily lead to the output of an action that is quite different from the previous frame. The separation between actions may easily lead to steering oscillation and sudden stop and start of the robot. Therefore, after sampling the action on the distribution output by the SAC strategy network, the PID controller is used to smoothly control the mobile robot based on the output target following decision.

[0015] Preferably, in step 1, the value range of k is {2, 3, 4}.

[0016] Preferably, step 3 comprises the following steps:

[0017] Step 3.1: All candidate regions ROI t,i All are scaled to the same size as the following object OBJ in the previous frame t-1 Same size, calculate each candidate region ROI separately t,i Follow the object OBJ in the previous frame t-1 The similarity Q i ;Q i Indicates the i-th candidate region ROI in the current frame t,i Follow the object OBJ in the previous frame t-1 The overall similarity of

[0018] Step 3.2: Q i The largest candidate region ROI t,i As the following object OBJ of the current frame t , and determine its RGB Location information in t , and according to Q i The largest candidate region is selected corresponding to d ROI,i As the following object OBJ of the current frame t The distance d from the robot t .

[0019] Preferably, Q i =α×Q distance +(1-α)×Q histogram , where Q distance Represents the candidate region ROI t,i Follow the object OBJ in the previous frame t-1 Similarity in terms of distance, Q histogram Represents the candidate region ROI t,i Follow the object OBJ in the previous frame t-1 Similarity in terms of histogram, α represents Q distance The weight coefficient of Q is in the range of [0.2, 0.5]; distance and Q histogram The value range of Q is between 0 and 1. distance The larger the value, the closer the distance between the object to be compared and the robot is to the distance between the following object and the robot in the previous frame. histogram The larger the value, the higher the similarity. When two images are exactly the same, the value is 1.

[0020] Preferably, In each candidate region ROIt,i The center position determines an m×n RGB pixel area, and then the RGB image I RGB With the depth image I D The corresponding relationship calculates the depth average of the m×n pixel area and uses it as the candidate region ROI t,i The distance d from the robot ROI,i , and will be based on the similarity Q i , select the one with the largest similarity from all (limited) candidate regions ROI,i The distance d between the robot and the target to be followed in the current frame t ;d t-1 Indicates the following object OBJ in the previous frame t-1 Distance from the mobile robot; d row,col Indicates the depth value corresponding to the position near the center coordinates of the candidate region in the depth image.

[0021] Preferably, H ox (z), H oy (c) are the following objects OBJ in the previous frame t-1 The values ​​of the projection histogram in the horizontal and vertical directions, Q histogram By calculating the current candidate region ROI from both horizontal and vertical directions t,i With the target object OBJ in the previous frame t-1 The projection histogram similarity of H is calculated; x (z), H y (c) The candidate regions ROI in the current frame after scaling t,i The values ​​of the projection histogram in the X-axis and Y-axis directions; z is the horizontal histogram component index, c is the vertical histogram component index; w and h represent the candidate region ROI respectively t,i width and height;

[0022] H x (z), H y (c) First, each candidate region ROI t,i The R, G, and B channel pixel values ​​of each pixel position are averaged, and then the candidate region ROI is calculated t,i In the vertical projection histogram in the horizontal X-axis and vertical Y-axis directions, in order to avoid the influence of the ROI area size on the histogram similarity, the cumulative sum is also averaged when calculating the projection histogram, R z,c , G z,c 、B z,c is the three-channel pixel value at row z and column c, H ox (z), H oyThe calculation method of (c) is the same as H x (z), H y (c).

[0023] Preferably, in step 4, the current state information S t ={R N / k ,S l ,S d}S t ={R N / k ,S l ,S d}, where S l ={l t-4 ,l t-3 ,l t-2 ,l t-1 ,l t}, when S l If the queue is not full, wait, S d That is, the following object OBJ in the current frame t The distance d from the mobile robot t ;

[0024] Here, the radar distance data R obtained in step 1 is N / k As part of the state space, at each radar distance data R N / k After a change occurs, the value of this part of the state space needs to be updated;

[0025] Determine the following object OBJ in the current frame through step 3 t Location information S l and distance information S d , for the follower object OBJ t Location status information S l The coordinate information of the following object position obtained in step 3 is obtained by combining the RGB images of the five frames before and after. t Obtained by combining.

[0026] Preferably, the SAC reinforcement learning model is trained with a juxtaposed Q-value network and a policy network and a set reward function;

[0027] In the policy network, the state information is used as input, and the mean and variance of the action distribution are output after passing through the fully connected neural network. The mean and variance are used to construct the Gaussian distribution of the action, and the final action information is obtained through sampling;

[0028] The goal of the policy network is to maximize the sum of expected reward and expected entropy under the premise of known state. That is, the training goal of the policy network is to maximize the state value. Q π(s,a) is the state-action value, α is the entropy regularization coefficient, and π(a|s) is the probability of taking action a in state s; the value of α can be set by those skilled in the art based on their needs.

[0029] In order to speed up the convergence of the policy network, the SAC algorithm adopts the reparameterization technique, taking the output of the policy network as the mean and variance to realize the Gaussian distribution of the output action, and adding random noise to it. Finally, the sampled actions are flattened to make the actions within the effective range. In the policy, the action selection formula is ξ~N(0,I), where a′(s,ξ) represents the action randomly sampled under a Gaussian distribution with noise, ξ is a Gaussian distribution with mean 0 and variance I, μ θ (s) represents the mean of the policy network output, σ θ (s) represents the standard deviation of the policy network output.

[0030] In the Q-value network, the state information and the action information are taken as input, and the two are concatenated and the Q value of the corresponding state-action pair is obtained through the neural network;

[0031] The learning of the Q-value network is achieved through the mean square Bellman error function, and the loss function is L v (D)=E (s,a,r,s',done)～D (Q v (s,a)-y) 2 ,v=1,2, where L v (D) is the loss function of the vth current Q value network, D is the experience pool, (s, a, r, s', done) is an experience sampled from the experience pool, which records the process of getting reward r after taking action a output by the policy network from a certain state s and reaching the next state s'. done indicates whether it is the flag bit of the terminal state (the terminal state has no next state); y represents the target Q value, Where a' is obtained by the policy network according to s', and the smaller Q is selected from the two target Q value networks. i (s′, a′), r is the current reward, and γ is the decay factor. The target value is constructed through the target Q-value network for the current Q-value network to update, and the current Q-value network guides the policy network to update, realizing the SAC training process.

[0032] Since the present invention constructs two current Q-value networks and two target Q-value networks and a policy network, the purpose of constructing two Q-value networks is to avoid over-estimation of the target Q-value. When constructing the target Q-value, the two networks select the one with the lower estimate, but use this lower yv to update the two networks. For the policy network, the smaller one (v=1,2) in the current Q network is selected for updating. Under the premise that two target Q-value networks are defined, the target Q-value is composed of the target Q network output and the reward r. Q(s,a) represents the estimate given by the current Q-value network based on the state s action a, and Q'(s',a') is the estimate given by the target network based on the next state s' and the policy network based on the action output a' of the state s'. γ mainly represents the correlation between the state action values, which is generally taken as 0.8 to 0.99.

[0033] Preferably, the reward function r=r collision +r d_obstacle +r d_object +r location The definition of reward function is mainly to help the intelligent agent to conduct effective training. In the mobile robot target following method, the size of the reward function is mainly composed of four parts: the penalty r for whether the robot collides with the obstacle, collision , the distance reward r between the robot and the nearest obstacle d_obstacle , the distance reward r between the robot and the following target d_object , the reward r for following the position of the object in the robot's field of view location ;

[0034] r collision Collision penalty refers to a certain penalty given to the robot after it collides with an obstacle or a wall during operation. The size of the penalty is inversely proportional to the distance between the robot and the obstacle. collision =-10×collision, where collision is the collision flag and takes the value 1 or 0, 1 represents collision and 0 represents no collision;

[0035] r d_obstacle The reward is the distance between the mobile robot and the nearest obstacle. It is used to prevent the robot from getting too close to obstacles or walls during the following process. The reward value is inversely proportional to the distance to the obstacle, expressed as d obstacle is the distance between the mobile robot and the nearest obstacle, d1 is the safe distance from the obstacle, which should be adjusted according to the size of the robot, in meters.

[0036]

[0037] r d_objectThe distance reward between the mobile robot and the object to be followed is to enable the robot to maintain an effective distance range with the object to be followed. Positive rewards can be obtained within the set range, and penalties will be incurred if the robot exceeds this range. The target tracking distance achieved by the present invention is d2~d3, with d t is the distance between the mobile robot and the following object, d max Is the maximum distance between the robot and the object being followed. If it is greater than this distance, the following is considered a failure.

[0038]

[0039] r location The reward for following an object in the field of view of the mobile robot is to ensure that the robot is always following the object and can make timely adjustments if there is any deviation.

[0040]

[0041] Where ω is the current angular velocity of the mobile robot, ω max is the maximum angular velocity that the mobile robot can achieve, w is the lateral resolution of the camera, and x is the center coordinate of the object being followed.

[0042] Preferably, in step 6, the discretized PID formula is: Where T is the regulation period, the proportional term P is expressed as K p , the integral term I is expressed as The differential term D is expressed as T i and T d They are integral time and differential time respectively, k is the number of executions, e k is the error between the actual value and the set value of the current frame, The accumulated error is summed to eliminate the static error. In the robot following task, the control goal of the PID controller is to smooth the action output of the current frame and the previous frame to avoid oscillation, so the error value e k Set it as the difference between the current speed and the action given by the reinforcement learning strategy network, that is, with the help of the proportional term for rapid adjustment, the integral term for eliminating the cumulative speed error, and the differential term for increasing system stability, the purpose of smoothly reaching the set value of the strategy network output action is achieved; the error between the actual speed and the target speed is recorded as v through the output of the discretized PID formula PID With ω PID , as the input of the robot actuator.

[0043] The present invention relates to a target following method for a mobile robot based on SAC-PID. The method obtains and preprocesses the robot's perception data, uses a target detection network to obtain candidate areas for following objects, collects reinforcement learning state information after matching the following objects, and uses a trained SAC reinforcement learning model to output the target angular velocity and target linear velocity of the mobile robot's motion control, thereby achieving end-to-end target following decision output for the mobile robot and smoothly controlling the mobile robot using a PID controller.

[0044] The beneficial effects of the present invention are that a model of the tracking target is established, the background and the tracking target are effectively distinguished, the tracking method can continue to be applied under occlusion conditions, the implementation is simple, the convergence speed is fast, the target tracking accuracy is high, it can cope with complex application scenarios, has strong robustness, and realizes smooth control of the tracking process. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a flowchart of the present invention;

[0046] Figure 2 This is a flow chart of target matching for following objects in the present invention;

[0047] Figure 3 Schematic diagram of the SAC decision neural network and terminal PID controller structure in the present invention. DETAILED DESCRIPTION

[0048] The present invention is further described in detail below with reference to the embodiments, but the protection scope of the present invention is not limited thereto.

[0049] The method is implemented using the Robot Operating System (ROS) platform. The robot's steering mechanism uses two differential wheels, and the sensors used primarily include an Intel RealSense D455 depth camera, a Delta-3A lidar, and a photoelectric encoder. The computing unit onboard the two-wheel differential robot is NVIDIA's Jeston Xavier NX AI edge computing platform, running Ubuntu 18.04 with ROS Melodic.

[0050] like Figure 1 As shown, a target following method for a mobile robot combining SAC and PID control includes the following steps:

[0051] (1) Obtain robot perception data and preprocess it;

[0052] (2) Use the target detection network to obtain candidate regions for following objects;

[0053] (3) follow-up target matching;

[0054] (4) Reinforcement learning state information collection;

[0055] (5)Robot target following decision;

[0056] (6) PID-based motion control.

[0057] Step (1) specifically includes:

[0058] The present invention uses a calibrated RGBD camera and a laser radar to obtain the state information of the mobile robot during the target following process, and uses the RGBD camera to collect a frame of real-time RGBD data I RGBD , and split it into RGB image I RGB and depth image I D , in preparation for subsequent target detection and tracking target matching; use the laser radar to scan the robot around a week to obtain the radar distance data R of N points N , and the radar data R of N points N Perform k-fold downsampling to obtain R N / k , the value range of k is {2,3,4}, in order to reduce R N / k In subsequent steps the occupation of the state space is reinforced.

[0059] Step (2) specifically includes:

[0060] Use the trained deep learning target detection network model to detect the RGB image I RGB Perform human target detection and obtain candidate regions (ROIs) of the following objects t,i , where t is the time index of the current frame and i is the candidate region index; during initialization, if the following object is not determined, the candidate region ROI with the largest pixel area will be directly selected. t,i Designate OBJ0 as the initial follow-up object as the benchmark for subsequent target matching and return to step (1); if the initial follow-up object has been determined, jump to the next step.

[0061] In order to get the following object in RGB image I RGB To obtain accurate location information in the image, this paper uses YOLOv5s as the target detection network and uses the benchmark dataset and data enhancement method released by Megvii to expand the detection dataset, so that the detection network can achieve better detection results. The specific steps of YOLOv5s detection network training are as follows:

[0062] (2-1) To accommodate the complexity of the robot's operating environment and the wide range of objects it follows, the CrowdHuman benchmark dataset, published by Megvii, was selected as the target detection dataset for this invention. This dataset contains a large amount of data and is richly annotated. Furthermore, to increase the variety and style of the dataset, Mosaic data augmentation technology was used to augment the data. This technology selects any four images from the dataset, performs flipping, scaling, color space conversion, and cropping operations, and then splices them into a single image to enhance the data's expressiveness.

[0063] (2-2) Train the YOLOv5s detection network using the CrowdHuman benchmark dataset and images processed by the Mosaic data augmentation method. Adjust the network parameters and train the model until the mean average precision (mAP) is greater than 95%. Then, use the RGB three-channel image obtained in step (1) for target detection. If no target object is detected in the current RGB image, skip to step (4). Otherwise, proceed to step (3).

[0064] Step (3) specifically includes:

[0065] In order to solve the influence of the suspected object on the robot during the following process, the following object OBJ of the previous frame is used t-1 The candidate region ROI detected in step (2) t,i Perform similarity judgment, the judgment process is as follows Figure 2 As shown, the area with the largest similarity is selected as the following object OBJ of the current frame t , the specific identification method is as follows:

[0066] (3-1) Use the candidate region ROI detected in step (2) t,i Calculate its domain and the target following object OBJ in the previous frame t-1 First, the candidate region ROI t,i Zoom to the same OBJ as the previous frame t-1 Same size, then calculate each candidate region ROI t,i Follow the object OBJ in the previous frame t-1 The specific calculation formula is as follows:

[0067] Q i =α×Q distance +(1-α)×Q histogram (1)

[0068] Among them, Q i Indicates the i-th candidate region ROI in the current frame t,i Follow the object OBJ in the previous frame t-1 The overall similarity of Qdistance Represents the candidate region ROI t,i Follow the object OBJ in the previous frame t-1 Similarity in terms of distance; Q histogram Represents the candidate region ROI t,i Follow the object OBJ in the previous frame t-1 Similarity in terms of histogram; α represents Q distance The proportion of the overall similarity comparison is in the range of α∈[0.2,0.5].

[0069] The distance similarity Q distance The calculation is first done in each candidate region ROI t,i The center position of the pixel area is determined as m × m, and in this implementation process, 3 × 3 is selected, and then the RGB image I is used to calculate the pixel area. RGB With the depth image I D The corresponding relationship calculates the depth average of the 3×3 pixel area and uses it as the candidate region ROI t,i The distance between the robot and the object in the previous frame is calculated. The specific calculation formula is as follows:

[0070]

[0071] Among them, d t-1 Indicates the following object OBJ in the previous frame t-1 Distance from the robot; d row,col It represents the depth value corresponding to the position near the center coordinate of the candidate area in the depth image. The m×m pixel area near the center can roughly represent the distance between the candidate area and the robot. The mean depth is recorded as d ROI,i ;Q distance The value range is between 0 and 1. The larger the value, the closer the distance between the object to be compared and the robot is to the distance between the following object and the robot in the previous frame.

[0072] The calculation of the histogram similarity is first to transform each candidate region ROI t,i The three-channel pixel values ​​are averaged, and then the current candidate region ROI is calculated from the X-axis and Y-axis directions t,i With the target object OBJ in the previous frame t-1 The specific calculation formula for the histogram similarity is as follows:

[0073]

[0074] Among them, Q histogramThe numerical range is [0,1]. The larger the value, the higher the similarity. When the two images are exactly the same, the value is 1. z is the horizontal projection histogram component index, c is the vertical projection histogram component index; w and h represent the candidate region ROI respectively. t,i Width and height; H x (z), H y (c) The candidate regions ROI in the current frame after scaling t,i The values ​​of the projection histogram in the X-axis and Y-axis directions are calculated according to formulas (4) and (5), respectively. ox (z), H oy (c) The calculation method is the same as H x (z), H y (c) Same:

[0075]

[0076]

[0077] (3-2) The similarity Q of each candidate region is calculated through the above steps i , select the candidate area with the highest similarity as the following object OBJ of the current frame t , and determine its RGB Location information in t , and then the d of the area with the maximum similarity is ROI,i As the following object OBJ in the current frame t The distance d from the robot t The location information l t Includes five frames of RGB images before and after I RGB The position coordinate information of the following object in the image obtained by detection.

[0078] Step (4) specifically includes:

[0079] The input state S of the SAC decision algorithm t Mainly from lidar data R N / k , target following object OBJ t Location information t and distance information d t The specific composition is shown in formula (6).

[0080] S t ={R N / k ,S l ,S d} (6)

[0081] Among them, S t Indicates the current state information; S lIndicates the current moment following the object OBJ t Location status information; S d Indicates the current moment of the robot and the following object OBJ t distance status information.

[0082] The specific steps for collecting status information are as follows:

[0083] (4-1) For the lidar data R N / k It is obtained in step (1) and used as part of the state space of the state SAC algorithm. This part of the state information is updated after each interaction between the robot and the environment;

[0084] (4-2) For the following object OBJ t Location status information S l The coordinate information of the following object position obtained in step (3) is obtained by taking the RGB images of the five frames before and after. t The specific expression is as shown in formula (7). When the cache queue is not full, it waits until the cache queue is full before starting again. If the detection object is lost, the position information of the previous frame is used.

[0085] S l ={l t-4 ,l t-3 ,l t-2 ,l t-1 ,l t} (7)

[0086] (4-3) For the following object OBJ t Distance status information S d This is done by obtaining the following object OBJ in step (3) t The corresponding candidate region with the highest similarity is d ROI,i As the distance d between the target and the robot in the current frame t If the detected object is lost, the position information of the previous frame will be used.

[0087] Step (5) specifically includes:

[0088] The present invention converts the agent state information S in step (4) into t The input is fed into the trained SAC reinforcement learning model, which outputs the target angular velocity ω and target linear velocity v of the robot motion control, thus achieving the end-to-end target action set value output of the robot. The specific training steps of the SAC reinforcement learning model are as follows:

[0089] The target following state information S of the mobile robot is obtained through step (4) t , and according to the state S tThe neural network and reward function of the SAC reinforcement learning model are designed based on the characteristics and target following control process. Finally, the SAC reinforcement learning model is trained to obtain the robot target following control model.

[0090] (5-1) The neural network structure of the SAC algorithm is as follows Figure 3 As shown in Figure 1, the network structure is primarily composed of a Q-value network and a policy network. The policy network inputs the state information collected by the robot's sensors. After passing through a three-layer fully connected network, it outputs the mean and variance of the action's Gaussian distribution. This is then used to construct the action's Gaussian distribution. The final action output is a sample of this Gaussian distribution and a Tanh function applied to keep the output action within a specified range. The Q-value network inputs the state information collected by the sensors and the action output by the policy network. Both are fed into the fully connected layers, where they are then concatenated. Finally, after passing through four fully connected layers, the Q-value is output.

[0091] (5-2) The definition of the reward function is mainly to help the intelligent agent to conduct effective training. In the mobile robot target following method, the size of the reward function is mainly composed of four parts: the penalty r for whether the robot collides with an obstacle, collision , the distance reward r between the robot and the nearest obstacle d_obstacle , the distance reward r between the robot and the following target d_object , the reward r for following the position of the object in the robot's field of view location , the specific calculation formula is as follows:

[0092] r=r collision +r d_obstacle +r d_object +r location (8)

[0093] Collision Penalty collision It refers to the penalty given to the robot after it collides with an obstacle or wall during operation. The size of the penalty is inversely proportional to the distance between the robot and the obstacle. The specific calculation formula is as follows:

[0094] r collision = -10×collision (9)

[0095] Among them, collision is the collision flag, which takes a value of 1 or 0, 1 represents collision, and 0 represents no collision.

[0096] The distance reward r between the robot and the nearest obstacle d_obstacle , is to prevent the robot from getting too close to obstacles or walls during the following process. The reward value is inversely proportional to the distance to the obstacle. The specific calculation formula is as follows:

[0097]

[0098] Among them, d obstacle is the distance between the robot and the nearest obstacle, and d1 is the set safety distance from the obstacle, which is 0.5 meters in this implementation.

[0099] The distance reward r between the robot and the following object d_object , set the target following distance between d2 and d3, select the distance according to the needs, keep following the target within this distance range, the agent can get positive rewards, and will be punished if it exceeds this range. The robot will eventually try to stay within this range. During the tracking process, a distance too far is not conducive to target detection, so d2 is generally taken as 1.5 and d3 is 2.5. The specific reward calculation formula is:

[0100]

[0101] Among them, d t is the distance between the robot and the following object; d max The maximum distance between the robot and the object being followed is usually 7.5 meters. If the distance is greater than this, the following attempt is considered a failure.

[0102] The reward r for following an object in the robot's field of view location , is to ensure that the robot always follows the object and can make timely adjustments if there is any deviation. The specific calculation formula is as follows:

[0103]

[0104] Where ω is the current angular velocity of the robot; ω max is the maximum angular velocity that the robot can achieve; w is the lateral resolution of the camera; x is the center coordinate of the follower object.

[0105] (5-3) The SAC strategy algorithm is trained based on the state information obtained in step (4) and the neural network and reward function designed in steps (5-1) and (5-2). Entropy regularization is added to the calculation of the state-action function. By increasing entropy, the randomness of the strategy is enhanced, thereby improving the speed of subsequent strategy learning and preventing the strategy from falling into a local optimum. In the SAC algorithm, a strategy network and two current Q-value networks need to be learned simultaneously. The learning of the Q network is achieved through the mean square Bellman error function. The specific loss function is as follows:

[0106] L v (D)=E (s,a,r,s',done)～D (Q v (s,a)-y) 2 ,v=1,2 (13)

[0107] Among them, L v (D) is the loss function of the vth Q-value network; D is the experience pool; y is the target Q-value, and (s, a, r, s', d) is an experience sampled from the experience pool. This experience records the process of going from a certain state s, taking action a, obtaining reward r, and reaching the next state s'. done indicates whether it is the terminal state. The specific calculation formula is as follows:

[0108]

[0109] Select the smaller Q from the two target Q value networks v '(s′,a′); r is the current reward; γ is the decay factor, and the target value is constructed through the target Q value network for the current Q value network to learn and update.

[0110] The goal of the policy network is to maximize the sum of the expected reward and the expected entropy under the premise of a known state, that is, to maximize the state value V, and guide the policy network to update through the current Q value network. The calculation formula of the state value V is:

[0111]

[0112] Among them, Q π (s,a) is the state-action value, α is the entropy regularization coefficient, and π(a|s) is the probability of taking action a in state s.

[0113] At the same time, in order to accelerate the convergence of the policy network, SAC uses the reparameterization technique to use the output of the policy network as the mean and variance to achieve the Gaussian distribution of the output action, and adds random noise to it. Finally, the sampled actions are flattened to make the actions within the effective range. Therefore, the action selection formula is:

[0114]

[0115] where a′(s,ξ) represents the action randomly sampled under a Gaussian distribution with noise, ξ is a Gaussian distribution with mean 0 and variance I, μ θ (s) represents the mean of the policy network output, σ θ (s) represents the standard deviation of the policy network output.

[0116] Step (6) specifically includes:

[0117] (6) PID-based motion control

[0118] To ensure that the robot's actuator can stably achieve the tracking task, a PID controller is added to the output end of the action execution. The target angular velocity ω can be obtained by inputting the observation information of the robot's scene obtained by the radar camera into the strategy network. target and target linear velocity v target The photoelectric encoder is used to obtain the rotation of the two wheels, and the current angular velocity ω is easily obtained by combining the two-wheel differential model. actual With the current linear velocity v actual After obtaining the set value and actual value, PID can be used for smooth control. The discretized PID formula is:

[0119]

[0120] The proportional term P can be expressed as K p ; The integral term I can be expressed as The differential term D can be expressed as k is the number of executions; e k Designed as the error between the actual speed of the current frame and the output action of the SAC policy network; is the cumulative error; in the implementation of the present invention, the adjustment time T is the time it takes to pass the pre-processed radar camera data into the SAC strategy network to obtain the action and output it to the robot actuator through the PID controller. The cycle remains basically unchanged, so the PID parameters can be regarded as fixed values ​​and manually adjusted to meet the needs of smooth control. The result of formula (17) is used as the input linear velocity v of the robot actuator in the current frame PID With the input angular velocity ω PID , to achieve the effect of robot following the target.

Claims

1. A target following method for a mobile robot based on SAC-PID, characterized by: The method comprises the following steps: Step 1: Use a calibrated RGBD camera and lidar to obtain a frame of real-time RGBD data during the mobile robot's target following process and radar data of N points ,Will Split into RGB images and depth image ,Will Perform k-fold downsampling to obtain radar data ; Step 2: Use the object detection network trained with the human dataset to detect the RGB image Detect the humanoid target in the image and obtain the candidate area of ​​the following object ; During initialization, if the following object is not determined, the candidate area with the largest pixel area will be Designate as initial follow object , as the benchmark for subsequent target matching, and return to step 1, otherwise proceed to the next step; where t is the time index of the current frame, and i is the candidate region index; Step 3: For all the candidate regions obtained Perform similarity judgment and select the area with the greatest similarity as the following object of the current frame , get the follow object In RGB image Location information in and the distance information between it and the mobile robot ; Step 4: Collect reinforcement learning status information based on steps 1 and 3 ; Step 5: Input the trained SAC reinforcement learning model and output the target angular velocity of the mobile robot motion control and target linear speed , achieving end-to-end target following decision output of mobile robots; Step 6: Based on the output target following decision, the mobile robot is smoothly controlled using a PID controller.

2. The mobile robot target following method based on SAC-PID according to claim 1, characterized in that: In step 1, the value range of k is {2, 3, 4}.

3. The mobile robot target following method based on SAC-PID according to claim 1, characterized in that: The step 3 comprises the following steps: Step 3.1: All candidate regions obtained Zoom to the same position as the follow object in the previous frame Same size, calculate each candidate region separately Follow the object in the previous frame Similarity ; Step 3.2: The largest candidate region As the follow object of the current frame , and determine its RGB image Location information in , and according to The largest candidate region is selected As the follow object of the current frame Distance to the robot .

4. The mobile robot target following method based on SAC-PID according to claim 3, characterized in that: ， in, Represents the candidate region Follow the object in the previous frame The similarity in terms of distance, Represents the candidate region Follow the object in the previous frame The similarity in terms of histograms, express The weight coefficient of is in the range of [0.2, 0.5]; and The value range is between 0 and 1.

5. The mobile robot target following method based on SAC-PID according to claim 4, characterized in that: ， In each candidate region The center position determines a RGB pixel area, and then through the RGB map With depth image The corresponding relationship is calculated The average depth of the pixel area is used as the candidate area Distance to the robot , and based on the similarity , select the one with the greatest similarity from all candidate regions The distance between the robot and the target being followed in the current frame ; Indicates the following object in the previous frame Distance to the mobile robot; Indicates the depth value corresponding to the position near the center coordinates of the candidate region in the depth image.

6. The mobile robot target following method based on SAC-PID according to claim 4, characterized in that: ， ， ， 、 They are the following objects in the previous frame The values ​​of the projection histogram in the horizontal and vertical directions; 、 They are the candidate regions in the current frame after scaling The values ​​of the projection histogram in the X-axis and Y-axis directions; is the horizontal histogram component index, and c is the vertical histogram component index; 、 Represent candidate regions width and height; 、 、 for OK The three-channel pixel value at the column position.

7. The mobile robot target following method based on SAC-PID according to claim 1, characterized in that: In step 4, the current state information ,in, ,when If the queue is not satisfied, wait and follow the object in the current frame. Distance to the mobile robot for .

8. The mobile robot target following method based on SAC-PID according to claim 1, characterized in that: The SAC reinforcement learning model is trained with a juxtaposed Q-value network and a policy network and a set reward function; In the policy network, the state information is used as input, and the mean and variance of the action distribution are output after passing through the fully connected neural network. The mean and variance are used to construct the Gaussian distribution of the action, and the final action information is obtained through sampling; The training goal of the policy network is to maximize the state value , ,in, is the state action value, is the entropy regularization coefficient, Status Take action probability; The action selection formula is ,in, represents the action randomly sampled under a Gaussian distribution with noise, is subject to a mean of 0 and a variance of Gaussian distribution, represents the mean of the policy network output, represents the standard deviation of the policy network output; In the Q-value network, the state information and the action information are taken as input, and the two are concatenated and the Q value of the corresponding state-action pair is obtained through the neural network; The learning of the Q-value network is achieved through the mean square Bellman error function, and the loss function is ,in, is the loss function of the vth current Q value network, D is the experience pool, (s, a, r, s', done) is an experience sampled from the experience pool, recording the experience from a certain state , take the action output by the policy network Afterwards received the reward , and reaches the next state process, A flag indicating whether it is a termination state; is the target Q value, ,in, By the policy network based on Get, select the smaller one from the two target Q value networks , r is the current reward, is the attenuation factor.

9. The mobile robot target following method based on SAC-PID according to claim 8, characterized in that: The reward function , For collision penalties, ,by Collision flag, 1 represents collision, 0 represents no collision; is the distance reward between the mobile robot and the nearest obstacle, is the distance between the mobile robot and the nearest obstacle, in meters, ； is the distance reward between the mobile robot and the following object, is the distance between the mobile robot and the following object, is the maximum distance between the robot and the object being followed, ； is the reward for following the object in the mobile robot’s field of view, ， in, is the current angular velocity of the mobile robot, is the maximum angular velocity that the mobile robot can achieve, is the horizontal resolution of the camera, are the center coordinates of the follower object.

10. The mobile robot target following method based on SAC-PID according to claim 1, characterized in that: In step 6, the discretized PID formula is: ， Among them, the proportional term is represented as , integral term is represented as , the differential term D is expressed as , is the error between the actual value and the set value of the current frame, is the cumulative error.

Images