Mobile robot target following method fused with local path planning algorithm

Abstract

The invention discloses a mobile robot target following method fused with a local path planning algorithm, and the method comprises the steps: carrying out the target detection of an environment image obtained by a mobile robot through a FastDet target detection network, and generating a detection frame which is used for representing the position and size of a target in the environment image; extracting the detection frame as an initial region through a KCF visual tracking algorithm, performing feature extraction in the initial region, and outputting center pixel coordinates of the target; converting the center pixel coordinate into a three-dimensional coordinate in a world coordinate system to obtain an actual position of the target in a three-dimensional space; and generating a plurality of candidate paths by using a DWA local path planning algorithm, and selecting an optimal path based on the actual position of the target and in combination with the current pose of the mobile robot. According to the mobile robot target following method fused with the local path planning algorithm, the problem that an existing mobile robot is difficult to meet real-time target tracking in a complex environment is solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot target following methods, and particularly relates to a mobile robot target following method integrating a local path planning algorithm. Background Technique

[0002] With the rapid development of artificial intelligence and computer vision technologies, robot target tracking technology has become a current research hotspot. In today's society, many scenarios require robots to be able to follow a target and move in a complex environment, such as in fields like supermarket automatic shopping cart following, golf carts, and power detection. For these complex scenarios, long-term and stable target tracking and smooth and reasonable path planning are crucial. However, when a dynamic target enters an unstructured environment, due to obstacle occlusion, there are problems of target loss and inaccurate positioning. In addition, the computing resources and capabilities of small mobile robots are very limited. Therefore, developing real-time, efficient, and robust tracking and path planning schemes is challenging.

[0003] Common target detection algorithms such as the YOLO series algorithms have high detection accuracy, but they have a complex network structure, a large number of model parameters, and consume a large amount of computing resources; and they have a long running time and are difficult to meet the real-time requirements of robot target tracking in complex environments. Summary of the Invention

[0004] The purpose of the present invention is to provide a mobile robot target following method integrating a local path planning algorithm, which solves the problem that existing mobile robots are difficult to meet the real-time target tracking in complex environments.

[0005] The technical solution adopted by the present invention is: a mobile robot target following method integrating a local path planning algorithm, including the following steps: Step 1, perform target detection on the environmental image obtained by the mobile robot through the FastestDet target detection network, and generate a detection box for representing the position and size of the target in the environmental image; Step 2, extract the detection box obtained in Step 1 as the initial region through the KCF visual tracking algorithm, and perform feature extraction within the initial region, and output the central pixel coordinates of the target; Step 3, convert the central pixel coordinates obtained in Step 2 into three-dimensional coordinates in the world coordinate system to obtain the actual position of the target in three-dimensional space; Step 4, use the DWA local path planning algorithm to generate multiple candidate paths, and select the optimal path based on the actual position of the target obtained in Step 3 and combined with the current pose of the mobile robot.

[0006] The characteristics of the present invention also lie in that Step 1 During target detection, the FastestDet target detection network receives the input of RGB environmental images, preprocesses the environmental images, including normalization, scale adjustment, and image enhancement, and then extracts image features through a convolutional neural network, and outputs a detection box representing the position and size of the target in the environmental image.

[0007] Step 2 Specifically includes the following steps: Step 2.1: Take the position and size of the detection box obtained in Step 1 as the initialization parameters of the KCF visual tracking algorithm, extract the features of the initial region, and construct an initial target feature template; Step 2.2: Extract features from the region where the target is located through the KCF visual tracking algorithm to form a feature vector of the target , and the feature vector of the circulant matrix is expressed as:

[0008] In the formula, is a constant matrix independent of , and represents the discrete Fourier transform of the generated vector; Step 2.3: Use the circulant matrix to find the weight coefficient of the linear classifier in the KCF visual tracking algorithm to minimize the square error between the sample and its regression target :

[0009] In the formula, is the complex conjugate matrix of , is the regularization parameter, is the identity matrix, is the vector composed of ; Step 2.4: Represent the weight coefficient by the non-linear combination of the minimized sample :

[0010] In the formula, is the mapping function; The vector composed of is

[0011] In the formula, is the kernel matrix, , is the mapping function, is the identity matrix. Using the Fourier diagonalization property of the circulant matrix, the solution of ridge regression is calculated as:

[0012] In the formula, represents the kernel matrix gets the discrete Fourier transform, and the classifier coefficients are obtained by solving . After that, the trained classifier is used to quickly detect the target; in the next frame, the response of the objective function to the test sample is calculated by the following formula:

[0013] In the formula, is and the kernel function of the test sample . The position of the target is estimated according to the response peak, is the two-dimensional response map, and the target area is output according to the response map , where is the pixel coordinate of the upper left corner of the target box, and represent the width and height of the target box respectively. Then the pixel coordinate of the center position of the target is expressed as:

[0014] .

[0015] Step 3 specifically includes the following steps: Step 3.1: Calibrate the camera of the mobile robot to obtain the internal and external parameters of the camera; Step 3.2: Convert the pixel coordinate of the center of the target to the coordinate in the camera coordinate system through the internal and external parameter matrices of the camera; Step 3.3: Use the projection matrix to convert the coordinate in the camera coordinate system to the three-dimensional coordinate in the world coordinate system where the mobile robot is located, and obtain the actual position of the target in the three-dimensional space :

[0016] In the formula, Z c is the depth distance along the camera optical axis direction; is a 3×3 rotation matrix; is a 3×1 translation vector; is the camera focal length, , is the actual physical size of the pixels on the photosensitive chip, , ; the origin of the image coordinate system, is an arbitrary point in the pixel plane; is the internal parameter matrix of the camera.

[0017] Step 4 specifically includes the following steps: Step 4.1, According to the current speed, acceleration limit and obstacle information of the mobile robot, use the DWA local path planning algorithm to generate multiple candidate paths in the set of feasible speeds of the mobile robot, is the intersection of three speed spaces, expressed as:

[0018] In the formula, is the speed space that the mobile robot can sample, is the speed space that can be sampled when considering acceleration, is the constraint condition that the mobile robot does not collide with surrounding obstacles at a certain moment; Step 4.2, Use the evaluation function to score each path in the multiple candidate paths generated in Step 4.1 respectively. The evaluation function is expressed as:

[0019] In the formula, is the speed sampling space of the mobile robot, is used to normalize the score of each trajectory, , , are the weights of the three scoring items, is the azimuth evaluation function; is the distance evaluation function; represents the current speed magnitude; Step 4.3, Select the path with the highest score as the optimal path of the mobile robot, and the mobile robot executes motion according to the optimal path.

[0020] The azimuth evaluation function is expressed as:

[0021] The direction angle of the target point relative to the current position of the mobile robot is determined by the geometric relationship between the two points, as shown in the following formula:

[0022] In the formula, Used to calculate the azimuth angle of the target point relative to the current position of the mobile robot; To evaluate the deviation between the current orientation of the mobile robot and the target direction, calculate the absolute value of the angle between the two, that is, the target orientation error :

[0023] Obtained After that, it can be known that The larger it is, the better the degree of the mobile robot facing the target. And Jointly affect Determine Size.

[0024] Distance evaluation function Is expressed as:

[0025] In the formula, Represents the closest distance between the predicted path and the obstacle; Is the maximum value of the obstacle score. If the distance between the predicted path and the obstacle exceeds Then the path is considered safe; Is the safe distance between the mobile robot and the obstacle. If the distance between the predicted path and the obstacle is less than Then it is considered that the path has a collision.

[0026] The calculation formula for the current pose of the mobile robot in step 4 is:

[0027] In the formula, Is the Axis coordinate of the mobile robot at Time, Is the Axis coordinate of the mobile robot at Time, Is the orientation angle of the mobile robot relative to the target at Time.

[0028] The beneficial effects of the present invention are as follows: The mobile robot target following method of the present invention integrating the local path planning algorithm effectively improves the robustness and continuity of target tracking, reduces the risk of target loss, and at the same time reduces the computational overhead by integrating target detection, visual tracking and path planning, and is applicable to robot platforms with limited computing resources. The present invention maintains a high target tracking accuracy and obstacle avoidance success rate in complex environments, realizes the function conversion from obstacle avoidance around an obstacle to autonomous obstacle avoidance during the dynamic target following process, and has strong practical application value. Brief Description of the Drawings

[0029] Figure 1 It is the structure diagram of the FastestDet model; Figure 2 (a) is the RGB image, Figure 2 (b) is the detection result image; Figure 3 It is the framework diagram of the object tracking system that fuses FastestDet-KCF with DWA; Figure 4 It is the schematic diagram of coordinate transformation; Figure 5 (a) is the pixel coordinate system and the image coordinate system, Figure 5 (b) is the camera coordinate system; Figure 6 It is the conversion diagram from the world coordinate system to the camera coordinate system; Figure 7 It is the schematic diagram of the relative position between the camera and the target pedestrian; Figure 8 It is the robot experimental platform; Figure 9 It is the schematic diagram of the representation of the robot and the tracking target in RVIZ; Figure 10 It is the schematic diagram of the robot following the target trajectory; Figure 11 It is the schematic diagram of the tracking effect from the robot's perspective of the FastestDet-KCF algorithm; Figure 12 It is the schematic diagram of the following and obstacle avoidance effect of the mobile robot. Specific implementation manners

[0030] The present invention will be described in detail below in conjunction with the accompanying drawings and specific implementation manners.

[0031] Example 1 The present invention provides a mobile robot target following method integrating a local path planning algorithm. The FastestDet network is used for target detection to generate detection boxes and initialize the KCF tracker, which is used to track the target in real time and output the central pixel coordinates of the target. Subsequently, the detected central pixel coordinates of the target are converted into three-dimensional coordinates in the world coordinate system, and combined with the current pose of the robot, the DWA local path planning algorithm is used to generate multiple candidate paths, and the optimal path is selected as the current motion trajectory of the robot. Finally, the robot can achieve stable target tracking and obstacle avoidance in a complex environment. The method of the present invention effectively improves the robustness and continuity of target tracking, reduces the risk of target loss, and at the same time reduces the computational overhead, and is applicable to robot platforms with limited computing resources. Through experimental verification, the present invention maintains a high target tracking accuracy and obstacle avoidance success rate in a complex environment, realizes the function conversion from obstacle bypassing to autonomous obstacle avoidance of the robot during the dynamic target following process, and has strong practical application value.

[0032] The present invention combines the FastestDet target detection network and the KCF visual tracking algorithm. In a complex environment, the target detection module can accurately locate the target, and the visual tracking module can stably track the target, avoiding tracking failures caused by target loss or mis-tracking. The DWA local path planning algorithm can generate an optimal motion trajectory according to the dynamic state of the robot and environmental information, ensuring that the robot can avoid obstacles in real time in a dynamic environment. While ensuring the safety of the path, the DWA algorithm also considers the proximity to the tracking target and the smoothness of the path, providing an efficient and stable motion control strategy.

[0033] Embodiment 2 The present invention provides a mobile robot target following method integrating a local path planning algorithm, including: Target detection: The FastestDet lightweight target detection algorithm is introduced to achieve the detection of human targets. This algorithm is designed to replace the yolo-fastest series of algorithms. Compared with existing lightweight target detection algorithms in the industry such as yolov5n and YOLO-Nano, FastestDet has an order-of-magnitude advantage in both speed and the number of parameters. Its main features are a single lightweight detection head, anchor-free, cross-network multi-candidate targets, dynamic positive and negative sample assignment, and simple data augmentation. Therefore, FastestDet is used to complete the initial detection of the following target or the re-detection task when the target is lost. The target is detected through the FastestDet target detection network, and the detection box of the target area is output. The FastestDet network is based on the convolutional neural network (CNN) architecture and uses deep learning technology to achieve real-time detection of targets in images and predict their position and category information. The detection box represents the position and size of the target in the image.

[0034] Target tracking: Initialize the KCF tracker based on the detection box. The KCF tracker uses kernel correlation filter technology to track the target in real time and output the central pixel coordinates of the target. The KCF algorithm uses kernel correlation filtering technology to calculate the correlation between the target and the background through HOG features and dynamically updates the filter weights to ensure continuous tracking of the target.

[0035] Coordinate transformation: Convert the central pixel coordinates of the target into three-dimensional coordinates in the world coordinate system. The conversion uses the internal and external parameter matrices of the camera and adopts the perspective transformation algorithm to map the target position in the image coordinate system to the world coordinate system where the robot is located.

[0036] Path planning: According to the three-dimensional coordinates and the current pose of the robot, use the DWA local path planning algorithm to generate multiple candidate paths. The DWA algorithm considers the current dynamic constraints of the robot (such as speed, acceleration) and the positions of obstacles in the environment to generate multiple possible trajectories. The candidate paths calculate the scores of each path by evaluating the safety, target proximity, and path smoothness of each path, and select the path with the highest score as the current motion trajectory of the robot.

[0037] Dynamic adjustment: Combine the target tracking results and the path planning results to dynamically adjust the motion trajectory of the robot. The dynamic adjustment ensures that the robot can avoid obstacles and maintain stable tracking of the target in a complex dynamic environment.

[0038] Embodiment 3 The present invention provides a mobile robot target following method integrating a local path planning algorithm, and preferably the following solutions on the basis of Embodiment 2: The target detection network is the FastestDet network. The network uses a convolutional neural network (CNN) architecture to extract the features of the image through multiple convolutional operations, directly outputs the detection results on the feature map, and outputs the target position according to the class prediction and confidence score. The FastestDet network has low computational overhead and is suitable for resource-constrained robot platforms.

[0039] The KCF visual tracking algorithm adopts a target matching method based on correlation filtering. The circulant matrix of this algorithm can be expressed as:

[0040] The KCF algorithm performs kernel method feature transformation on the target area to extract the texture features, color features, and shape features of the target to improve the robustness of tracking. Especially in the case of scale change, pose change, or partial occlusion of the target, the KCF algorithm can stably track the target.

[0041] The local path planning algorithm is the DWA (Dynamic Window Approach) algorithm. The DWA algorithm generates multiple candidate paths based on the current motion state of the robot (including speed, acceleration, etc.) and the obstacle information in the environment. By evaluating the safety (risk of collision), goal proximity (degree of approach to the goal on the path), and smoothness of each path, the optimal path is selected as the current motion trajectory of the robot.

[0042] The evaluation function performs a weighted scoring based on the safety, goal proximity, and path smoothness of the path. The scoring function comprehensively considers various metrics of the path, reflects the importance of each metric in path selection by setting different weight values, and finally selects the path with the highest score as the motion trajectory of the robot.

[0043] The path evaluation process includes the following steps: a) Azimuth evaluation function: Calculate the azimuth between each candidate path and the target position, and evaluate the change in azimuth of the path relative to the target. The azimuth evaluation function evaluates the goal proximity of the path by calculating the angular difference between the robot's orientation and the target orientation. The path with a higher goal proximity scores higher; b) Distance evaluation function: Calculate the Euclidean distance between each position on the path and the target position. The distance evaluation function evaluates the goal proximity at different stages of the path. The path with a shorter distance scores higher; c) Speed evaluation function: Evaluate the speed of each node on the path to ensure that the motion speed of the path is within the maximum speed and acceleration limits of the robot, avoid excessive speed changes, and ensure the smoothness and stability of the robot's motion.

[0044] The evaluation function in the DWA algorithm is a multi-objective optimization function. This function performs a weighted synthesis of multiple metrics such as goal proximity, safety, and smoothness of the path, obtains the total score of each path, and selects the path with the highest score as the current motion trajectory of the robot.

[0045] The DWA path planning algorithm can adapt to real-time changes in a dynamic environment and can quickly re-plan the path when the goal or environment changes, ensuring that the robot always maintains good tracking and obstacle avoidance effects in a real-time environment.

[0046] Embodiment 4 The present invention provides a mobile robot target following method integrating a local path planning algorithm, which mainly includes five core modules: a target detection module, a visual tracking module, a pose solution module, a positioning module, and a local path planning module. Each module works together to achieve efficient target tracking and obstacle avoidance functions.

[0047] The target detection module is based on the FastestDet object detection network and adopts the convolutional neural network (CNN) architecture to achieve efficient object detection and localization. The FastestDet network has high detection accuracy and low computational overhead, making it suitable for real-time applications.

[0048] In the robot system, the main task of the target detection module is to identify target objects from the environment and output their positions in the image. The FastestDet network processes the image through the convolutional neural network (CNN), generates the detection bounding boxes of the targets, and determines the categories of the targets. This module can handle various challenging problems such as occlusion, fast motion, and illumination changes, ensuring the reliability of the detection results.

[0049] The visual tracking module further introduces the KCF (Kernelized Correlation Filters) visual tracking algorithm on the basis of the target detection module as the core technology for target tracking. The KCF algorithm has high accuracy and real-time performance and can achieve continuous tracking after target detection.

[0050] Specifically, the target detection module generates the detection bounding boxes of the targets through FastestDet and uses these bounding boxes as the initial parameters of the KCF algorithm. The KCF algorithm uses the initial position detected by the target to construct the feature model of the target and tracks the target through the method of correlation filtering. The advantage of the KCF algorithm is that it improves the tracking accuracy in cases such as scale change and rotation through the kernel method, while reducing the computational burden, making it suitable for application on robot platforms with limited resources.

[0051] The function of the localization module is to convert the two-dimensional coordinates of the following target in the image into three-dimensional coordinates in the world coordinate system. These coordinates can be used as the end target position in the path planning of the DWA algorithm, thus realizing the fusion of KCF and DWA and achieving dynamic obstacle avoidance during the real-time following process of the robot.

[0052] To ensure that the robot can effectively avoid obstacles and continuously track the target in a dynamic environment, the present invention introduces the DWA (Dynamic Window Approach) local path planning algorithm. The DWA algorithm generates multiple candidate trajectories based on the current speed, acceleration of the robot, and the positions of the obstacles, and evaluates these trajectories through an evaluation function, and finally selects the optimal path as the current motion trajectory of the robot.

[0053] The core idea of the DWA algorithm is to generate a set of feasible motion trajectories at each time step by considering the current dynamic constraints of the robot and combining the obstacle information in the environment, and to evaluate each trajectory. This evaluation usually depends on factors such as the safety of the trajectory, the proximity to the target, and the smoothness of the path. In this way, the DWA algorithm can effectively avoid collisions between the robot and obstacles while ensuring the continuity of target tracking.

[0054] Embodiment 5 The present invention provides a mobile robot target following method integrating a local path planning algorithm. The target detection module, visual tracking module, pose solving module, positioning module, and local path planning module in Embodiment 4 cooperate with each other to form an efficient tracking system. The specific process is as follows: Target detection and initialization: The robot performs target detection through the FastestDet network, and the position and size of the detection box are used as the initialization parameters of the KCF tracker. At this time, the KCF algorithm will establish a feature model of the target based on the initial position of the target and perform subsequent tracking.

[0055] Real-time target tracking: As the robot moves, the KCF algorithm continuously tracks the target to ensure the accuracy of the target position. In the case of a large change in the target position, the FastestDet network will perform target detection again to correct the tracking result.

[0056] Coordinate transformation and path planning: After obtaining the real-time position of the target through the visual tracking module, the system will convert the pixel coordinates of the target into three-dimensional coordinates in the robot world coordinate system. Subsequently, the local path planning module combines the current pose of the robot and the obstacle information, generates multiple candidate paths through the DWA algorithm, and selects the optimal path as the current motion trajectory of the robot.

[0057] Motion control and execution: The robot executes motion according to the selected path, and the DWA algorithm dynamically adjusts the path to ensure that the robot can avoid obstacles and maintain tracking of the target. If the target is lost, the system will restart the target detection process to resume tracking.

[0058] Embodiment 6 The present invention provides a mobile robot target following method integrating a local path planning algorithm, which is specifically implemented according to the following steps: Step 1. Environmental data collection and sensor configuration: The robot is equipped with multiple sensors, including an RGB camera, a LiDAR, and a Kinect V1 sensor. These sensors work together to obtain information about the surrounding environment. The Kinect V1 camera is used to obtain visual information around the robot, especially the appearance features of the target object, and provides depth information to assist in target detection. The LiDAR (Light Detection and Ranging) is used to provide the three-dimensional structure of the environment to help the robot identify static and dynamic obstacles around it.

[0059] Step 1.1. FastestDet object detection network When the robot obtains an environmental image, the FastestDet object detection network is used to detect the target position in real time. The FastestDet network is based on the convolutional neural network (CNN) architecture and can efficiently and accurately identify and locate targets from images. The FastestDet network is optimized to run on resource-constrained hardware platforms while maintaining high detection accuracy. The structure of the FastestDet model is as Figure 1 shown.

[0060] During object detection, the FastestDet network receives an RGB image input, as Figure 2 shown in (a), preprocesses the image, including normalization, scale adjustment, and image enhancement; extracts multi-scale features through a lightweight convolutional neural network, and directly outputs the detection results on the feature map, as Figure 2 shown in (b), to obtain the output results of the target's category, location, and confidence score.

[0061] Step 1.2. The coordinates of the object detection bounding box generated by the FastestDet network are used as the initialization parameters of the KCF object tracking algorithm. The KCF tracker extracts the detection bounding box of the target as the initial region and extracts features within this region.

[0062] Step 2. KCF visual tracking: The KCF (Kernelized Correlation Filters) algorithm is the key algorithm for object tracking in the present invention. The KCF algorithm establishes a feature model of the target and uses convolutional operations to match the features of the target. The framework of the mobile robot target following system is as Figure 3 , and the main steps of the KCF algorithm include: Step 2.1. Initializing the target: Using the detection bounding box output by FastestDet, the KCF algorithm extracts the features in the detection bounding box and constructs an initial target feature template.

[0063] Step 2.2. Training Ridge Regression Classifier: In each frame of the image, the KCF algorithm extracts features from the region where the target is located, such as HOG (Histogram of Oriented Gradients), color histogram, etc., to form the feature vector of the target. . Feature vector 's circulant matrix can be expressed as: (1) In the formula, is a constant matrix independent of , and represents the discrete Fourier transform (DFT) of the generated vector. For the sample set , it is necessary to find a linear regression equation , and the classifier is to find the weight coefficient to minimize the squared error between the sample and its regression target : (2) is a regularization parameter to control overfitting, and the weight coefficient can be expressed as: (3) In the formula is the matrix composed of elements , is the vector composed of , and is an identity matrix.

[0064] Step 2.3. Linear Regression in the Kernel Space: Most problems in actual situations are non-linear problems. Usually, the non-linear transformation is converted into a linear problem in the kernel space, and the non-linear transformation is realized using a kernel function. If there is a function in the low-dimensional space that satisfies , then is called a kernel function. Equation (4) is the expression of the Gaussian kernel function used in the KCF algorithm: (4) In the linear regression equation , replace with the non-linear combination of : (5) Equation (5) converts the solution of into the solution of . Substituting Equation (5) back into Equation (3) gives: (6) Among them is the kernel matrix, , is the mapping function, is the identity matrix, is 's vector. Using the Fourier diagonalization property of the circulant matrix, the solution of ridge regression is calculated as: (7) Among them, represents the kernel matrix obtains the discrete Fourier transform. After solving equation (7) to obtain the classifier coefficient , the trained classifier can be used to quickly detect the target. is the test sample, then the objective function is: (8) Among them, represents the test sample and the training sample 's kernel function. In the next frame, the response of the objective function to the test sample can be calculated through equation (9): (9) Among them, can be learned in the model. According to the response peak, the position of the target can be accurately estimated. is the two-dimensional response map, which contains the test results of all test samples. According to the response map, the target area is output, where is the pixel coordinates of the upper left corner of the target box, and represent the width and height of the target box respectively. The pixel coordinates of the center position of the target can be expressed as: (10) (11) Step 3. The KCF tracker outputs the two-dimensional image coordinates (pixel coordinates) of the target. In order for the robot to perform path planning based on the position of the target, these pixel coordinates need to be converted into three-dimensional coordinates in the world coordinate system. The overall process of coordinate conversion is as Figure 4 shown.

[0065] Step 3.1. Camera calibration: First, calibrate the RGB camera to obtain the internal parameters (such as focal length, optical center, etc.) and external parameters (such as the position and orientation of the camera) of the camera. These parameters are obtained through methods such as calibration boards.

[0066] The conversion from the pixel coordinate system to the camera coordinate system is asFigure 5 As shown in Figure 5 Figure (a), there is a pixel coordinate system and an image coordinate system. In Figure 5 Figure (b), the internal and external camera parameter matrices obtained through calibration are used to convert the central pixel coordinates of the target into coordinates in the camera coordinate system : (12) In the formula, Z c is the depth distance along the camera optical axis direction, , are the actual physical sizes of the pixels on the photosensitive chip, is the camera focal length, , . is the origin of the image coordinate system, is an arbitrary point in the pixel plane.

[0067] Step 3.3: Then, convert the coordinates in the camera coordinate system into three-dimensional coordinates in the world coordinate system where the robot is located. As shown in Figure 6 Figure, this process uses a projection matrix to perform coordinate transformation to obtain the actual position of the target in three-dimensional space: (13) In the formula, through the position and attitude information of the visual odometer, the rotation matrix and the translation vector can be obtained. is a 3×3 rotation matrix, is a 3×1 translation vector. Thus, the entire process of converting coordinates from the pixel coordinate system to the world coordinate system is obtained, as shown in Equation (14): (14) In the formula, is the internal parameter matrix of the camera.

[0068] Step 4: The DWA (Dynamic Window Approach) algorithm is a local path planning method based on the dynamic constraints of the robot. In the present invention, the DWA algorithm is used to generate candidate trajectories for the movement of the robot and select the optimal path.

[0069] In the DWA and KCF target tracking fusion algorithm, a motion model of the robot is established by obtaining the real-time linear velocity and angular velocity of the robot. The kinematic equation of the robot is: (15) In the formula, is the robot at At a moment axis coordinate, is the axis coordinate of the robot at a moment ; is the orientation angle of the robot relative to the target at a moment. It describes a robot trajectory from a moment to a moment .

[0070] Step 4.1, Generate candidate paths: According to the current speed, acceleration limit and obstacle information of the robot, the DWA algorithm generates multiple candidate paths. The generation of each path takes into account the maximum speed, minimum turning radius and acceleration limit of the robot.

[0071] The DWA algorithm selects the best path by using an evaluation function in the speed sampling space. The speed sampling space of the robot is restricted by speed, acceleration and obstacles in the environment. There are boundaries for both linear velocity and angular velocity, forming a dynamic window. Finally, the set of feasible speeds of the robot is the intersection of three speed spaces: (16) where is the speed space that the robot can sample, is the speed space that can be sampled considering acceleration, is the constraint condition that the mobile robot does not collide with surrounding obstacles at a certain moment.

[0072] In the set of allowable speeds of the robot , there are multiple trajectories that meet the requirements. In the DWA algorithm, the evaluation function can select the optimal trajectory, which comprehensively considers the position relationship between the robot and the target point, obstacle information, robot speed information, etc. The evaluation function for evaluating each trajectory is: (17) where , , are the weights of three scoring items.

[0073] Step 4.2, Path evaluation: The specific meanings of the three evaluation functions are as follows: (1) is the azimuth angle evaluation function, which is used to evaluate the error of the angle between the line connecting the end position of the trajectory generated at the current sampled speed and the target point : (18) According to the characteristics of the dynamic target tracking task, the robot needs to face the pedestrian target to ensure that the pedestrian target is at the center of the camera's field of view during the tracking process. As Figure 7 shown, during the robot's tracking process, it is necessary to calculate the relative position of the human body to prevent the camera from failing to capture the target due to the target's movement, resulting in the failure of the tracking task.

[0074] The direction angle of the target point relative to the robot's current position is determined by the geometric relationship between the two points, as shown in Equation (19): (19) In the formula, is used to calculate the azimuth angle of the target point relative to the robot's current position.

[0075] To evaluate the deviation between the robot's current orientation and the target direction, it is necessary to calculate the absolute value of the angle between the two, that is, the target orientation error: (20) After obtaining From Equation (18), it can be seen that the larger, the better the degree of the robot facing the target. and jointly affect thus determining the size. The DWA algorithm optimizes and to maximize and make the robot move towards the target point as much as possible.

[0076] (2) is the distance evaluation function, indicating the closest distance between the simulated trajectory corresponding to the current speed and the obstacle: (21) Aims to select a trajectory with a greater distance from the obstacle to avoid collisions during the robot's movement. represents the closest distance between the predicted trajectory and the obstacle; is the maximum value of the obstacle score. If the distance between the predicted trajectory and the obstacle exceeds then the trajectory is considered safe; is the safety distance between the robot and the obstacle. If the distance between the predicted trajectory and the obstacle is less than then the trajectory is considered to have a collision.

[0077] (3) represents the current speed magnitude. The larger, the faster the speed on the planned trajectory and the higher the evaluation score.

[0078] Step 4.3. Select the optimal path: The DWA algorithm scores each path based on the above evaluation results and selects the path with the highest score as the motion trajectory of the robot.

[0079] Step 4.4. After selecting the optimal path, the robot will execute the motion according to this path. The motion control system generates corresponding control commands (such as speed, steering angle, etc.) based on the current speed, acceleration of the robot and the requirements of the trajectory.

[0080] Step 5. During the robot's movement, if the target is lost or the obstacle situation changes, the DWA algorithm will recalculate the path. If the KCF tracker makes an error or the target is completely lost, the system will restart the FastestDet target detection process, re - perform target localization, and then restart path planning to ensure that the robot can always track the target and avoid obstacles.

[0081] Figure 8 is the robot experimental platform. As Figure 9 shown, in RVIZ, the target position followed by the robot, the movement path of the target pedestrian, and the robot following path integrating the DWA algorithm are presented. The origin of the world coordinate system is the starting position of the robot.

[0082] Figure 10 The robot target tracking trajectories under the FastestDet - KCF algorithm integrating DWA and the separate FastestDet - KCF algorithm are compared. The dotted line represents the movement trajectory of the pedestrian target, the short - dashed line represents the robot trajectory where the KCF algorithm fails to avoid obstacles and collides during following, and the solid line represents the robot following target trajectory after integrating the DWA algorithm. The 2D lidar is used to identify obstacles and construct a cost map to prevent collisions, demonstrating that the mobile robot uses local path planning to avoid obstacles and follow the target pedestrian. This method can follow the pedestrian at a safe distance in a complex unknown environment, and at the same time uses the local path planning DWA algorithm for dynamic obstacle avoidance, fully verifying the effectiveness and robustness of the method of the present invention.

[0083] As Figure 11 shown, the tracking test results of the FastestDet - KCF algorithm integrating DWA are used to evaluate the performance of this algorithm on a small mobile robot platform. Target following experiments are carried out in an indoor environment. The following effect from the perspective of the robot camera is as Figure 11 shown, and the following and obstacle avoidance effect of the mobile robot is as Figure 12 shown. Figure 11 shown, when the target moves from a dimly lit area to a brightly lit area, the robot can still follow stably. Figure 11As shown, during the target following process, the robot updates the map information, target position, and robot trajectory in RVIZ in real time. This experiment demonstrates that the robot can still stably follow the target pedestrian in an environment with changing lighting conditions.

[0084] Figure 12 For the third-person view following effect, there are dynamic pedestrians in the environment. The robot uses lidar to detect the position information of obstacles in real time. And the DWA algorithm is used for local path planning. The DWA algorithm is based on the obstacle information sensed by the lidar, calculates the speed space of the robot in real time, predicts the trajectory in the next few seconds, and selects an optimal path that can both avoid obstacles and maintain following the target. This enables the robot to maintain a stable following effect in a dynamic environment, improving its robustness and safety in a dynamic environment.

Claims

1. A mobile robot target following method integrating a local path planning algorithm, characterized in that: The following steps are involved: Step 1: Use the FastestDet target detection network to perform target detection on the environment image acquired by the mobile robot, and generate a detection frame to represent the position and size of the target in the environment image; Step 2: Use the KCF visual tracking algorithm to extract the detection box obtained in step 1 as the initial area, perform feature extraction in the initial area, and output the center pixel coordinates of the target; Step 3: Convert the center pixel coordinates obtained in step 2 into three-dimensional coordinates in the world coordinate system to obtain the actual position of the target in the three-dimensional space; Step 4: Use the DWA local path planning algorithm to generate multiple candidate paths, and select the optimal path based on the actual position of the target obtained in step 3 and the current position of the mobile robot.

2. The mobile robot target following method integrating the local path planning algorithm as claimed in claim 1, characterized in that: In step 1, during target detection, the FastestDet target detection network receives RGB environment image input, preprocesses the environment image, including normalization, scale adjustment and image enhancement, extracts image features through a convolutional neural network, and outputs a detection box for representing the position and size of the target in the environment image.

3. The mobile robot target following method integrating the local path planning algorithm as claimed in claim 1, characterized in that: The step 2 specifically includes the following steps: Step 2.1: Use the position and size of the detection frame obtained in step 1 as the initialization parameters of the KCF visual tracking algorithm, extract the features of the initial area, and construct the initial target feature template; Step 2.2: Use the KCF visual tracking algorithm to extract features from the target area to form the target's feature vector , the feature vector The circulant matrix It is expressed as: In the formula, is a non-dependant The constant matrix of represents the discrete Fourier transform of the generated vector; Step 2.3: Using the Circular Matrix Find the weight coefficients of the linear classifier in the KCF visual tracking algorithm To minimize the sample and its regression target The squared error: In the formula, for The complex conjugate matrix of is the regularization parameter, is the identity matrix, Is The vector composed of Step 2.4: Set the weight coefficient Minimize the sample The nonlinear combination of is represented as: In the formula, is the mapping function; The vector composed of , which is the nonlinear classifier coefficient in the KCF visual tracking algorithm, is expressed as: In the formula, is the kernel matrix, , is the mapping function, is the unit matrix, and the solution of ridge regression is calculated by using the Fourier diagonalization property of the circulant matrix: In the formula, Represents the kernel matrix Get the discrete Fourier transform and solve for the classifier coefficients After that, the trained classifier is used to quickly detect the target; in the next frame, the target function is calculated by the following formula for the test sample Response: In the formula, for With test samples The kernel function estimates the position of the target based on the response peak. It is a two-dimensional response map, and the target area is output according to the response map ,in is the pixel coordinate of the upper left corner of the target box, and Represent the width and height of the target frame respectively, then the pixel coordinates of the center position of the target It is expressed as: 。 4. The mobile robot target following method integrating the local path planning algorithm as claimed in claim 3, characterized in that: The step 3 specifically comprises the following steps: Step 3.1, calibrate the camera of the mobile robot to obtain the intrinsic and extrinsic parameters of the camera; Step 3.2: Use the camera internal and external parameter matrix to convert the center pixel coordinates of the target Convert to the coordinates in the camera coordinate system; Step 3.3: Use the projection matrix to convert the coordinates in the camera coordinate system into three-dimensional coordinates in the world coordinate system of the mobile robot to obtain the actual position of the target in three-dimensional space. : In the formula, Z c is the depth distance along the camera optical axis; is a 3×3 rotation matrix; is a 3×1 translation vector; is the camera focal length, , is the actual physical size of the pixel on the photosensitive chip, , ; The origin of the image coordinate system, is any point in the pixel plane; is the intrinsic parameter matrix of the camera.

5. The mobile robot target following method integrating the local path planning algorithm as claimed in claim 4, characterized in that: The step 4 specifically comprises the following steps: Step 4.1: Based on the current speed, acceleration limit and obstacle information of the mobile robot, use the DWA local path planning algorithm to find the feasible speed set of the mobile robot. Generate multiple candidate paths in is the intersection of three velocity spaces, expressed as: In the formula, is the velocity space that the mobile robot can sample, is the velocity space that can be sampled when considering acceleration, It is a constraint condition that the mobile robot does not collide with surrounding obstacles at a certain moment; Step 4.2: Using the evaluation function Score each of the multiple candidate paths generated in step 4.1, and use the evaluation function It is expressed as: In the formula, is the velocity sampling space of the mobile robot, Used to normalize each trajectory score, , , are the weights of the three scoring items, is the azimuth evaluation function; is the distance evaluation function; Indicates the current speed; Step 4.3: Select the path with the highest score as the optimal path for the mobile robot, and the mobile robot performs movement according to the optimal path.

6. The mobile robot target following method integrating the local path planning algorithm as claimed in claim 5, characterized in that: The azimuth evaluation function It is expressed as: The orientation angle of the target point relative to the current position of the mobile robot is determined by the geometric relationship between the two points, as shown in the following formula: In the formula, Used to calculate the azimuth of the target point relative to the current position of the mobile robot; In order to evaluate the deviation between the current orientation of the mobile robot and the target orientation, the absolute value of the angle between the two is calculated, that is, the target orientation error : get Afterwards, we know The larger it is, the better the robot moves toward the target. and Common Impact Decide size.

7. The mobile robot target following method integrating the local path planning algorithm as claimed in claim 5, characterized in that: The distance evaluation function It is expressed as: In the formula, Indicates the shortest distance between the predicted path and the obstacle; The maximum obstacle score. If the distance between the predicted path and the obstacle exceeds The path is considered safe; is the safe distance between the mobile robot and the obstacle. If the distance between the predicted path and the obstacle is less than The paths are considered to collide.

8. The mobile robot target following method integrating the local path planning algorithm as claimed in claim 5, characterized in that: The calculation formula for the current position of the mobile robot in step 4 is: In the formula, For mobile robots Moment Axis coordinates, For mobile robots Moment Axis coordinates, For mobile robots The orientation angle relative to the target at any moment.

Images