Robot dynamic target searching and tracking method based on adaptive particle filter tree
By adopting mutual information estimation based on sigma points and adaptive path planning of Monte Carlo tree search in autonomous mobile robots, combined with the adaptive particle filter tree algorithm, the high complexity of mutual information calculation and path in dynamic target search and tracking are solved. The problem of difficult planning time domain setting has achieved efficient and accurate target positioning and smooth trajectory generation.
Patent Information
- Application Number
- CN202410547681.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-05-06
- Publication Date
- 2024-11-05
- Estimated Expiration
- 2044-05-06
AI Technical Summary
The prior art has problems in the process of dynamic target search and tracking of autonomous mobile robots with high complexity in the calculation of mutual information and low estimation accuracy. The time domain setting of path planning is difficult, resulting in the inability to effectively discover and locate the targets, and the generated trajectory is not possible. Non-smooth, low efficiency.
Using the mutual information estimation method based on sigma points and the adaptive path planning of Monte Carlo tree search, the adaptive particle filter tree algorithm is combined with UCB criteria and particle filtering technology to dynamically adjust the planning time domain to generate smooth and large information Movement trajectory.
Fast and accurate mutual information estimation is achieved, and smooth paths that conform to kinematic constraints are generated, which improves the accuracy of target positioning and search efficiency, reduces calculation time, and enhances the stability and efficiency of the path.
Smart Images

Figure CN118897541A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to a method for searching and tracking a dynamic target in an environment for an autonomous mobile robot, in particular to an adaptive particle filter tree method, belonging to the technical field of robot target searching and tracking. Background Art
[0002] Using autonomous mobile robots to search and track dynamic targets in the environment is a highly concerned issue in the field of robotics. It is widely used in scenarios such as environmental monitoring and disaster relief. The target search and tracking problem is mainly divided into two stages, namely target search and target tracking. The target search stage means that the robot does not know the specific location of the target at the beginning, and needs to use sensors with limited perception range, such as cameras, lidar, etc., to explore the environment to find the target; after finding the target, it immediately enters the target tracking stage, that is, the robot performs motion control to try to ensure that the target is within the robot's perception range and reduce the probability of losing the target's field of view.
[0003] Target search and tracking problems can usually be considered as information collection problems, that is, the robot autonomously performs motion planning based on the current known information to obtain more informative environmental observation results, thereby reducing the uncertainty of the robot's target estimation and improving the accuracy of target positioning. In the information collection problem, calculating the objective function and performing path planning are the two main contents, but there are still some defects in the existing work. First, in terms of the objective function, mutual information, as a measure of the degree of correlation of metric variables in information theory, is an effective objective function commonly used in information collection problems, but in its calculation process, it is usually necessary to calculate the entropy of the mixed distribution, which involves complex integral calculations and often has no analytical expression. Existing methods either estimate the integral through a large number of random sampling methods, which will introduce a high computational cost; or approximate the integral through a method based on Taylor expansion, but it will bring non-negligible estimation errors. The computational accuracy and efficiency of these objective functions will affect the subsequent planning stage. Secondly, in terms of path planning, in the case of a series of limitations of traditional planning schemes, planning schemes based on Monte Carlo tree search have become a widely used method in recent years. However, this type of method requires a fixed planning horizon in advance. In the case of a long planning horizon problem such as target search, it may encounter the dilemma of setting the planning horizon: if the planning horizon is too short, the robot may not be able to effectively discover and locate the target, while a long planning horizon will make it difficult to make quick decisions. In response to the above two problems, this solution proposes an adaptive particle filter tree algorithm, which proposes a mutual information estimation method based on sigma point sampling and an adaptive path planning method based on Monte Carlo tree search, combining the two aspects to obtain a complete solution that can solve the dynamic target search and tracking problem in real time and efficiently.
[0004] Among the commonly used methods, 1. The control method based on maximizing mutual information estimates the target position, thereby realizing the search and tracking of the target. This method first generates a series of trajectories through motion primitives, and predicts the future state of the target based on the target motion model, and obtains the optimal robot trajectory by maximizing the mutual information between the target state and the observation in the planning domain. In order to solve the problem that the mixed entropy in the mutual information cannot be explicitly calculated, it uses an integral estimation method based on Taylor series expansion to approximate the mutual information. Specifically, this estimation method performs a low-order Taylor series expansion on the logarithmic term in the entropy and estimates the integrand, so that the integral can be calculated analytically. The mutual information estimation method based on Taylor expansion used in this method has a non-negligible estimation error in the result, and the control law obtains an open-loop control strategy, that is, in the planning process, the target state is only predicted based on the target motion model, without considering the impact of possible future observation results on the planning effect. In addition, this method assumes that the robot can always obtain the observation results of the target, without considering the impact of the robot's limited sensing range.
[0005] 2. A motion planning method based on sampling is used to solve the problem of information collection. This method is based on the rapidly expanding random tree algorithm. It generates a tree structure by sampling in the feasible space and extracts a planning scheme with information that meets the constraints from the tree. Specifically, the current position of the robot is taken as the root node, and random sampling is performed in the task environment. New leaf nodes are added based on the sampling results and the existing tree structure, and the information benefits of the new nodes are calculated. The above steps are repeated continuously under the pre-specified budget constraints to establish a search tree, and finally a collision-free and effective path with the maximum amount of information is extracted from the tree based on the node information benefits. The path planning method based on sampling has a large randomness, and the algorithm effect is greatly affected by the number of samples and the sampling distribution, and the stability is poor. At the same time, the generated trajectory is tortuous and the smoothness is poor; there are many redundant points and the path efficiency is low.
[0006] 3. Use a greedy planning method to search and track the target. This method models the problem as a one-step, constrained optimization problem in the planning domain, considers maximizing the mutual information of the next step for motion planning, and uses a gradient descent-based optimization method to solve it. Greedy planning schemes can only obtain local optimal solutions, but cannot consider the planning effect in the long-term domain, and cannot obtain a global optimal solution. For example, in the target search stage, the amount of information obtained by each action in the initial search process is usually small and the difference is not large, which makes it difficult for the robot to find an effective path that can complete the task globally in the short-term planning. Summary of the invention
[0007] In view of the defects of high computational complexity and low estimation accuracy of the existing mutual information estimation methods in the above background technology, this method proposes a mutual information estimation method based on sigma points to solve the above problems. In view of the shortcomings of the existing path planning methods, such as short planning time domain, inability to obtain the global optimal solution, unstable planning effect, tortuous and uneven planning path, and low path efficiency, this method proposes an adaptive planning solution based on the Monte Carlo tree search method to solve the above problems.
[0008] The technical purpose of the present invention is to propose a robot dynamic target search and tracking method based on an adaptive particle filter tree. The method is based on the mutual information estimation method of sigma points and can quickly and accurately calculate mutual information. In the Monte Carlo tree search, motion primitives that conform to the kinematic model are used to form an action space to generate a smooth path that meets the kinematic constraints. The adaptive stopping strategy in the adaptive particle filter tree can dynamically adjust the planning time domain according to the reward information and reduce the operation time.
[0009] To achieve the above purpose, the technical solution adopted by this method is a robot dynamic target search and tracking method based on an adaptive particle filter tree, comprising the following steps:
[0010] Step 1: The mobile robot uses sensors with limited field of view, including cameras, radars, etc., at its current location to obtain information about the surrounding environment. Define the observation result of the mobile robot on the target through multiple sensors as z. If the target is within the observation range of the mobile robot, the mobile robot obtains the observation information of the target, that is, the distance and angle information of the target relative to the robot; otherwise, the observation result of the mobile robot on the target is empty. The mobile robot uses particle filtering technology to filter the observation result z and obtain the estimated result of the target position.
[0011] Step 2: According to the current particle-based target position estimation results and the mobile robot's own state, the tree is built based on the Monte Carlo tree search method, and the four steps of selection-expansion-simulation-backtracking are iteratively performed to incrementally build an adaptive particle filter tree, continuously add nodes to the adaptive particle filter tree and maintain and update the node information for subsequent mobile robot decision-making. The nodes in the adaptive particle filter tree are used to store the information of the mobile robot in the planning domain, including predicting the possible position and corresponding rewards that the mobile robot may reach in the planning domain. The adaptive particle filter tree is used to explore the possible trajectory of the mobile robot in the future and provide the mobile robot with the optimal motion decision-making plan. The adaptive particle filter tree contains two types of nodes: action nodes and state nodes. The two types of nodes are generated alternately according to the action-observation sequence to simulate the actions taken by the planning mobile robot during the movement process in the planning domain and the possible observation results. The tree building process includes four steps of selection-expansion-simulation-backtracking:
[0012] Step 2.1, selection: Based on the current policy tree information, the mobile robot weighs action exploration and utilization according to the UCB criterion and selects the most valuable planning path in the current tree, that is, a series of action-state nodes. The UCB criterion is used to select the optimal child node n in node n. * , the specific selection formula is as follows:
[0013]
[0014] Where Q is the cumulative reward of the node, N(n) is the number of times node n has been visited, N(n') is the number of times node n' has been visited, C is the set of child nodes of node n, and c is the parameter for weighing exploration and utilization. Starting from the root node, the mobile robot continuously selects the optimal child node through this criterion to find the current optimal planning path. Further, based on the selected series of action-state nodes, the particle estimation results are predicted and updated using their action-observation sequence and particle filtering technology.
[0015] Step 2.2, expansion: After the mobile robot finds the end point of the selected planned path, it samples the action-observation sequence from the action set and environmental information of the state node corresponding to the end point, calculates the reward brought by the action using the mutual information estimation method based on the sigma point, and uses the particle filter to update the particle estimation result to expand new action nodes and state nodes.
[0016] The calculation process of the mutual information calculation method based on sigma points is as follows:
[0017] The calculation process of mutual information involves the calculation of the entropy H(z) of the mobile robot's observation result z of the target, that is, the calculation
[0018]
[0019] When the observation noise is Gaussian, the probability distribution P(z) of the mobile robot's observation result z of the target satisfies a Gaussian mixture distribution where μ i and V i are the mean and variance of the ith Gaussian component. Define the sigma point and the corresponding weight as j=0,…,2m, where the dimension of the observation result is defined as m. The specific calculation formula is:
[0020]
[0021] Among them, λ is used to measure the dispersion of sigma points. Each Gaussian component is approximated according to the obtained sigma points and weights, that is,
[0022]
[0023] Thus, the formula for approximate calculation of the entropy of the Gaussian mixture model is obtained
[0024]
[0025] The entropy formula of the Gaussian mixture model is used to quickly calculate the mutual information result, which is used to measure the reward of the nodes in the planned path and lay the foundation for the subsequent mobile robot decision-making.
[0026] Furthermore, the state space is regularly divided into multiple small squares, and the particles in each square are weighted averaged to obtain new particles. This method can reduce the number of particles required for calculation, thereby improving the computational efficiency of the estimation method and achieving efficient and accurate estimation of mutual information.
[0027] Step 2.3, simulation: For the newly expanded state nodes in the expansion process, an adaptive rollout process is performed, that is, the action of the mobile robot of the newly expanded node is simulated according to the pre-set action strategy, and the cumulative discounted reward of the process is calculated until the terminal state or the preset time budget is reached, which is used to estimate the value of the new node. If the information gain obtained is greater than the pre-set threshold before reaching the terminal state or the preset time budget, the rollout process is terminated in advance to adaptively adjust the planning time domain.
[0028] Step 2.4, backtracking: backtrack all visited action nodes according to the reward information obtained by the adaptive rollout process in the simulation step, and update the node visit count and reward information. The specific process is to take the newly expanded node as the current node n', find its parent node n, and update the parent node n. The formula is as follows:
[0029] N(n)=N(n)+1
[0030]
[0031] Where r is the immediate reward, V is the total discounted reward in the planning time domain, and γ is the discount factor. After updating the information of node n, select its parent node as the current node and iteratively perform the above steps until backtracking to the root node. By repeatedly repeating the four steps of selection-expansion-simulation-backtracking, an update strategy tree is iteratively established for selecting the optimal strategy of the mobile robot.
[0032] Step 3: Based on the established adaptive particle filter tree, the mobile robot selects the child node with the largest reward in the root node of the tree, which is the optimal decision for the current location of the mobile robot. Subsequently, the mobile robot executes the motion control corresponding to the child node with the largest reward and generates a motion trajectory.
[0033] Step 4: After the mobile robot completes the motion trajectory and reaches the new position, repeat steps 1-3, that is, the mobile robot replans according to the newly obtained observation results until the target search and tracking task is completed. During the movement of the mobile robot, the target object in the environment is dynamic. Therefore, the mobile robot continuously obtains new environment and target information, estimates the target position, and performs trajectory planning to achieve real-time search and tracking of dynamic targets, ensuring the accuracy of target positioning.
[0034] Compared with the prior art, the adaptive particle filter tree technology proposed in the present invention is a planning method based on Monte Carlo tree search, which can balance decision exploration and utilization, and selectively search for future feasible paths; at the same time, its adaptive nature can dynamically adjust the planning time domain according to the current reward information, saving computing resources. Combined with the action space composed of motion primitives, the invention can efficiently generate a smooth path that takes into account the influence of future observations, has a long time domain, and conforms to the kinematic model, thereby efficiently completing the problem of target search and tracking. BRIEF DESCRIPTION OF THE DRAWINGS
[0035] Figure 1 This is an overview of the adaptive particle filter tree.
[0036] Figure 2 This is a diagram of the construction process of the adaptive particle filter tree.
[0037] Figure 3 It is a comparison chart of the performance indicators of different methods in target search and tracking tasks.
[0038] Figure 4 It is a trajectory comparison chart of different methods in target search and tracking tasks. DETAILED DESCRIPTION
[0039] The present invention is described in detail below with reference to the accompanying drawings and embodiments.
[0040] The technical solution adopted by this method is: a robot dynamic target search and tracking method based on an adaptive particle filter tree. This method is an informative path planning method that takes into account model uncertainty and limited observation field of view and is used to solve dynamic target search and tracking. Figure 1As shown in the figure, this method mainly includes two aspects of progress: First, a mutual information estimation method based on sigma points is proposed, which can quickly and accurately calculate mutual information in continuous state space and observation space, and in the case of non-Gaussian state distribution, providing an objective function basis for the real-time stable operation of the planning part. Second, based on the Monte Carlo tree search method, an adaptive particle filter tree technology is proposed, which can adaptively adjust the planning time domain according to the current known information, and can efficiently generate a robot motion path that meets kinematic constraints, actively avoids obstacles, and has information content, so as to better complete dynamic target search and tracking tasks.
[0041] This program includes the following steps:
[0042] Step 1: The mobile robot uses sensors with limited field of view, such as cameras and radars, to obtain information about the surrounding environment at its current location. Define the observation result of the mobile robot on the target through multiple sensors as z. If the target is within the observation range of the mobile robot at this time, the mobile robot will obtain observation information about the target, that is, the distance and angle information of the target relative to the robot; otherwise, the observation result of the mobile robot on the target is empty. Under the influence of inevitable sensor noise, the mobile robot uses particle filtering technology to filter the inaccurate observation result z and obtain the estimated result of the target position.
[0043] Step 2: According to the current particle-based target position estimation results and the mobile robot's own state, a tree is built based on the Monte Carlo tree search method, and the four steps of selection-expansion-simulation-backtracking are iteratively performed to incrementally build an adaptive particle filter tree, continuously add nodes to the adaptive particle filter tree and maintain and update the node information for subsequent mobile robot decision-making. The nodes in the adaptive particle filter tree are used to store the information of the mobile robot in the planning domain, including predicting the possible positions and corresponding rewards that the mobile robot may reach in the planning domain. Therefore, the adaptive particle filter tree is used to explore the possible trajectories of the mobile robot in the future and provide the mobile robot with the optimal motion decision plan. The adaptive particle filter tree contains two types of nodes: action nodes and state nodes. The two types of nodes are generated alternately according to the action-observation sequence to simulate the actions taken by the planning mobile robot and the possible observation results obtained during the movement process in the planning domain. Figure 2 As shown in the figure, the tree building process includes four steps: selection-expansion-simulation-backtracking:
[0044] Step 2.1 (selection): Based on the current policy tree information, the mobile robot weighs action exploration and exploitation according to the UCB (upper confidence bound) criterion and selects the most valuable planning path in the current tree, that is, a series of action-state nodes. The UCB criterion is used to select the optimal child node n in node n. *, the specific selection formula is as follows:
[0045]
[0046] Where Q is the cumulative reward of the node, N(n) is the number of times node n has been visited, N(n') is the number of times node n' has been visited, C is the set of child nodes of node n, and c is the parameter for weighing exploration and utilization. Starting from the root node, the mobile robot continuously selects the optimal child node through this criterion to find the current optimal planning path. Further, based on the selected series of action-state nodes, the particle estimation results are predicted and updated using their action-observation sequence and particle filtering technology.
[0047] Step 2.2 (Extension): After the mobile robot finds the end point of the selected planned path, it samples the action-observation sequence from the action set and environmental information of the state node corresponding to the end point, calculates the reward brought by the action using the mutual information estimation method based on the sigma point, and uses the particle filter to update the particle estimation result to expand new action nodes and state nodes. The calculation process of the mutual information calculation method based on the sigma point is as follows:
[0048] The calculation process of mutual information involves the calculation of the entropy H(z) of the mobile robot's observation result z of the target, that is, the calculation
[0049] H(z)=-∫P(z)logP(z)dz
[0050] Under the assumption that the observation noise is Gaussian, the probability distribution P(z) of the mobile robot's observation result z of the target satisfies a Gaussian mixture distribution where μ i and V i are the mean and variance of the ith Gaussian component. Define the sigma point and the corresponding weight as j=0,…,2m, where the dimension of the observation result is defined as m. The specific calculation formula is:
[0051]
[0052] Among them, λ is used to measure the dispersion of sigma points. Each Gaussian component is approximated according to the obtained sigma points and weights, that is,
[0053]
[0054] Thus, the formula for approximate calculation of the entropy of the Gaussian mixture model is obtained
[0055]
[0056] This formula can be used to explicitly and quickly calculate the mutual information results, which are used to measure the rewards of nodes in the planned path and lay the foundation for subsequent mobile robot decision-making.
[0057] Furthermore, the state space is divided into a plurality of small squares in a regular manner, and the particles in each square are weighted averaged to obtain new particles. In this way, the number of particles required for calculation can be reduced, thereby improving the calculation efficiency of the estimation method and achieving efficient and accurate estimation of mutual information. The sigma point mutual information calculation method proposed in the present invention uses a small amount of fixed position sampling to approximate the calculation of mutual information. It not only keeps the first-order moment and second-order moment information unchanged by utilizing the properties of the sigma point, but also ensures the estimation accuracy of the calculation result. At the same time, the small number of samples avoids the method from being too complex to calculate, and improves the algorithm operation efficiency.
[0058] Step 2.3 (Simulation): For the newly expanded state nodes in the expansion process, an adaptive rollout process is performed, that is, the action of the mobile robot of the newly expanded node is simulated according to the pre-set action strategy, and the cumulative discounted reward of the process is calculated until the terminal state or the preset time budget is reached, which is used to estimate the value of the new node. If the information gain obtained by the process is greater than the pre-set threshold before reaching the terminal state or the preset time budget, the rollout process is terminated in advance to adaptively adjust the planning time domain.
[0059] Step 2.4 (backtracking): Backtrack all visited action nodes according to the reward information obtained from the adaptive rollout process in the simulation step, and update the node visit count and reward information. The specific process is to take the newly expanded node as the current node n', find its parent node n, and update the parent node n. The formula is as follows:
[0060] N(n)=N(n)+1
[0061]
[0062] Where r is the immediate reward, V is the total discounted reward in the planning time domain, and γ is the discount factor. After updating the information of node n, select its parent node as the current node and iteratively perform the above steps until backtracking to the root node. By repeatedly repeating the four steps of selection-expansion-simulation-backtracking, an update strategy tree is iteratively established for selecting the optimal strategy of the mobile robot.
[0063] Step 3: Based on the established adaptive particle filter tree, the mobile robot selects the child node with the largest reward in the root node of the tree, which is the optimal decision for the current location of the mobile robot. Subsequently, the mobile robot executes the motion control corresponding to the child node with the largest reward and generates a motion trajectory.
[0064] Step 4: After the mobile robot completes the motion trajectory and reaches the new position, repeat steps 1-3, that is, the mobile robot replans according to the newly obtained observation results until the target search and tracking task is completed. Note that during the movement of the mobile robot, the target object in the environment is also dynamic. Under this scheme, the mobile robot can continuously obtain new environment and target information, estimate the target position, and perform trajectory planning to achieve real-time search and tracking of dynamic targets, ensuring the accuracy of target positioning.
[0065] The adaptive particle filter tree proposed in the present invention can adaptively adjust the planning time domain according to the current mode and information to adapt to different task stages, speed up the efficiency of algorithm operation while ensuring the planning trajectory effect, and achieve real-time and stable path planning to complete dynamic target search and tracking tasks.
[0066] The present invention will show the simulation verification results of mutual information estimation and target search and tracking. First, the results of mutual information estimation based on sigma point estimation method and Monte Carlo integration and Taylor expansion estimation method are compared, as shown in Table 1:
[0067] Table 1: Comparison of results of different mutual information estimation methods
[0068] method Absolute error Relative error (%) Calculation time(s) Monte Carlo integration - - 0.0614 Zero-order Taylor expansion estimate 0.4220 38.9 0.0070 Second-order Taylor expansion estimate 0.0993 8.66 0.3502 Sigma point estimate 0.0395 3.42 0.0302 Sigma point estimation + particle simplification 0.0533 4.69 0.0017
[0069] The present invention regards the result obtained by Monte Carlo integration as the true value, and calculates the absolute error, relative error and calculation time of the calculation results of each method. The sigma point-based estimation method proposed by the method of the present invention has the smallest calculation error. After combining with the particle simplification technology, the method has significantly improved the calculation speed while maintaining a high estimation accuracy, and its accuracy and efficiency have been verified. In addition, the method of the present invention is compared with the greedy method (NBV) and the sampling-based method (IIG) in dynamic target search and tracking tasks. The comparison indicators include the time difference in finding the target, the probability of target loss during tracking, and the target estimation error during tracking. At the same time, considering the cases where the initial target distribution is unimodal and multimodal, the quantitative results are as follows Figure 3 As shown in . It can be seen that no matter in the unimodal or multimodal initial target distribution, this method has a shorter search time, lower target loss probability and estimation error, which means that this method has higher search efficiency and more stable tracking performance, and can provide more efficient and reliable performance for dynamic target search and tracking tasks. The trajectory comparison results are shown in Figure 4 As shown, it can be seen that the method can find the target faster with a smooth trajectory and track the target stably under uncertain conditions, which reflects the efficiency, robustness and real-time performance of the method.
[0070] References
[0071] [1] Charrow B, Kumar V, Michael N. Approximate representations for multi-robot control policies that maximize mutual information[J]. Autonomous Robots, 2014, 37: 383 - 400.
[0072] [2] Ghaffari Jadidi M, Valls Miro J, Dissanayake G. Sampling-based incremental information gathering with applications to robotic exploration and environmental monitoring[J]. The International Journal of Robotics Research, 2019, 38(6): 658 - 685.
[0073] [3] Hoffmann G M, Tomlin C J. Mobile sensor network control using mutual information methods and particle filters[J]. IEEE Transactions on Automatic Control, 2009, 55(1): 32 - 47。
Claims
1. A robot dynamic target search and tracking method based on an adaptive particle filter tree, characterized in that: The following steps are involved: Step 1: The mobile robot uses sensors with limited field of view, including cameras, radars, etc., to obtain information about the surrounding environment at its current location; the observation result of the mobile robot on the target through multiple sensors is defined as z; if the target is within the observation range of the mobile robot, the mobile robot obtains the observation information of the target, that is, the distance and angle information of the target relative to the robot; otherwise, the observation result of the mobile robot on the target is empty; the mobile robot uses particle filtering technology to filter the observation result z and obtain the estimated result of the target position; Step 2: According to the current particle-based target position estimation result and the mobile robot's own state, the tree is built based on the Monte Carlo tree search method, and the four steps of selection-expansion-simulation-backtracking are iteratively performed to incrementally build an adaptive particle filter tree, continuously add nodes to the adaptive particle filter tree and maintain and update the node information for subsequent mobile robot decision-making; the nodes in the adaptive particle filter tree are used to store the information of the mobile robot in the planning domain, including predicting the possible position of the mobile robot in the planning domain and the corresponding reward. The adaptive particle filter tree is used to explore the possible trajectory of the mobile robot in the future and provide the mobile robot with the optimal motion decision-making plan; the adaptive particle filter tree contains two types of nodes: action nodes and state nodes. The two types of nodes are alternately generated according to the action-observation sequence to simulate the actions taken by the planning mobile robot in the planning domain and the possible observation results; Step 3: Based on the established adaptive particle filter tree, the mobile robot selects the child node with the largest reward in the root node of the tree, which is the optimal decision for the current location of the mobile robot; then, the mobile robot executes the motion control corresponding to the child node with the largest reward to generate a motion trajectory; Step 4: After the mobile robot completes the motion trajectory and reaches the new position, it repeats steps 1-3, that is, the mobile robot replans according to the newly obtained observation results until the target search and tracking tasks are completed; during the movement of the mobile robot, the target object in the environment is dynamic, so the mobile robot continuously obtains new environment and target information, estimates the target position, and performs trajectory planning to realize real-time search and tracking of dynamic targets, ensuring the accuracy of target positioning.
2. The robot dynamic target search and tracking method based on adaptive particle filter tree according to claim 1 is characterized in that: The tree building process includes four steps: selection-expansion-simulation-backtracking: Step 2.1, selection: Based on the current policy tree information, the mobile robot weighs action exploration and utilization according to the UCB criterion and selects the most valuable planning path in the current tree, that is, a series of action-state nodes; the UCB criterion is used to select the optimal child node n in node n * , the specific selection formula is as follows: Where Q is the cumulative reward of the node, N(n) is the number of times node n has been visited, N(n') is the number of times node n' has been visited, C is the set of child nodes of node n, and c is the parameter for weighing exploration and utilization; starting from the root node, the mobile robot continuously selects the optimal child node through this criterion to find the current optimal planning path; based on the selected series of action-state nodes, the particle estimation results are predicted and updated using their action-observation sequence and particle filtering technology; Step 2.2, expansion: After the mobile robot finds the end point of the selected planned path, it samples the action-observation sequence from the action set and environmental information of the state node corresponding to the end point, calculates the reward using the mutual information estimation method based on the sigma point, and uses the particle filter to update the particle estimation result to expand new action nodes and state nodes; Step 2.3, simulation: For the newly expanded state node in the expansion process, an adaptive rollout process is performed, that is, the action of the mobile robot of the newly expanded node is simulated according to the pre-set action strategy, and the cumulative discounted reward of the process is calculated until the terminal state or the preset time budget is reached, which is used to estimate the value of the new node; If the information gain obtained is greater than the pre-set threshold before reaching the termination state or the preset time budget, the rollout process is terminated in advance to adaptively adjust the planning time domain; Step 2.4, backtracking: backtrack all visited action nodes according to the reward information obtained by the adaptive rollout process in the simulation step, and update the node visit count and reward information; the specific process is to take the newly expanded node as the current node n', find its parent node n, and update the parent node n. The formula is as follows: N(n)=N(n)+1 Where r is the immediate reward, V is the total discounted reward in the planning domain, and γ is the discount factor. After updating the information of node n, select its parent node as the current node and iteratively perform the above steps until backtracking to the root node. By repeatedly repeating the four steps of selection-expansion-simulation-backtracking, an update strategy tree is iteratively established for selecting the optimal strategy for the mobile robot.
3. The robot dynamic target search and tracking method based on adaptive particle filter tree according to claim 2 is characterized in that: The calculation process of the mutual information calculation method based on sigma points is as follows: The calculation process of mutual information involves the calculation of the entropy H(z) of the mobile robot's observation result z of the target, that is, the calculation H(z)=-∫P(z)logP(z)dz When the observation noise is Gaussian, the probability distribution P(z) of the mobile robot's observation result z of the target satisfies a Gaussian mixture distribution where μ i and V i are the mean and variance of the i-th Gaussian component respectively; for the i-th Gaussian distribution Define the sigma point and the corresponding weight as The dimension of the observation results is defined as m; The specific calculation formula is: Among them, λ is used to measure the dispersion of sigma points; each Gaussian component is approximated according to the obtained sigma points and weights, that is, Thus, the formula for approximate calculation of the entropy of the Gaussian mixture model is obtained The entropy formula of the Gaussian mixture model is used to quickly calculate the mutual information result, which is used to measure the reward of the nodes in the planned path and lay the foundation for the subsequent mobile robot decision-making.
4. The robot dynamic target search and tracking method based on adaptive particle filter tree according to claim 1 is characterized in that: The state space is divided into multiple small squares in a regular manner, and the particles in each square are weighted averaged to obtain new particles, which reduces the number of particles required for calculation and improves the computational efficiency of the estimation method, thus achieving efficient and accurate estimation of mutual information.
Citation Information
Patent Citations
Underwater auxiliary oil recovery control system and dynamic positioning method thereof
CN109062230A
Swarm robot collaborative search method based on Monte Carlo tree search algorithm
CN110989352A
Light weight and real time slam for robots
US20220026920A1
Robot State Planning Method Based on Monte Carlo Tree Search Algorithm
US20230271318A1
Cited By
Robot self-positioning method and device based on self-adaptive extension reset and medium
CN120252733A
Petrochemical explosion-proof humanoid robot trajectory planning method, device and equipment
CN120909303A
A petrochemical explosion-proof humanoid robot trajectory planning method, device and equipment
CN120909303B
