Navigation strategy model training and exploration mapping method and device

Abstract

The present disclosure belongs to the field of robot mapping, and particularly relates to a navigation strategy model training method and an exploration mapping method and device. The navigation strategy model training method comprises the following steps: obtaining a target position, determining a current position and a position uncertainty of a robot at the current position, determining a first state of the robot at the current position based on the position uncertainty; obtaining and executing a current strategy to make the robot move to a second position, determining a second state of the robot at the second position and a reward, and the first state, the second state, the reward and the current strategy constitute an interaction sample, wherein the reward comprises an uncertainty penalty related to a change trend of the position uncertainty; repeating the above steps until the robot reaches the target position or meets a preset standard, and updating parameters of the navigation strategy model based on the interaction sample.

Description

TECHNICAL FIELD

[0001] The present disclosure belongs to the field of robot mapping, and particularly relates to a navigation strategy model training method and device and an exploration mapping method. BACKGROUND

[0002] SLAM (Simultaneous Localization and Mapping, SLAM for short) technology refers to that a robot simultaneously completes self-positioning and map construction in the absence of a prior map, and is one of core technologies of an autonomous mobile robot. Active SLAM technology further realizes the synchronization of navigation and positioning and mapping, and actively explores an environment and collects the most valuable sensor data to obtain a more accurate map more efficiently through autonomous planning of a motion path and optimization of an observation direction.

[0003] Existing active SLAM technology mainly focuses on the evaluation of an environment explorable area and information gain, and is usually based on the following assumption: as long as a high-information observation point is selected and a path to reach the observation point is planned, stable and reliable positioning and mapping results can be obtained.

[0004] However, this assumption is often difficult to establish in a dynamic environment. Because the existing scheme lacks evaluation of positioning uncertainty, it cannot actively adjust the motion and perception strategy according to the environment characteristics and task requirements. Therefore, when the robot passes through an area with a large number of dynamic obstacles, the adverse factors such as occlusion and noise introduced by dynamic objects cannot be effectively perceived and coped with in the existing framework, and are further converted into unpredictable cumulative errors, thereby reducing the accuracy and robustness of mapping. SUMMARY

[0005] Embodiments of the present disclosure propose an active SLAM scheme based on positioning uncertainty to solve the problem that existing active SLAM methods lack active adjustment of the motion and perception strategy based on positioning uncertainty, thereby reducing the accuracy and robustness of mapping in unstructured dynamic scenes.

[0006] A first aspect of embodiments of the present disclosure provides a navigation strategy model training method, comprising:

[0007] obtaining a target position, determining a current position and a position uncertainty of the robot at the current position, and determining a first state of the robot at the current position based on the position uncertainty;

[0008] acquiring and executing a current policy makes the robot move to a second position, determining a second state of the robot at the second position and a reward, the interaction sample is composed of the first state, the second state, the reward and the current policy, wherein the reward comprises an uncertainty penalty related to a change trend of the position uncertainty;

[0009] repeating the above steps until the robot reaches the target position or meets a preset standard, updating parameters of the navigation policy model based on the interaction sample.

[0010] In some embodiments, the determining the current position comprises:

[0011] acquiring omnidirectional lidar data of the robot on the surrounding environment, and determining the current position of the robot based on the lidar data.

[0012] In some embodiments, the determining the position uncertainty of the robot at the current position comprises:

[0013] performing example filtering on the lidar data, and determining a pose estimation covariance matrix of the robot at the current position based on the example filtering data according to the following formula,

[0014]

[0015] wherein Σ t is a pose estimation covariance matrix of the robot at the current position at t time, and respectively represent a weight and a relative pose deviation of an i-th particle maintained in the example filtering at t time;

[0016] based on the pose estimation covariance matrix, obtaining the position uncertainty of the robot at the current position at t time,

[0017] wherein is the position uncertainty of the robot at the current position at t time, ∑ t is a pose estimation covariance matrix of the robot at the current position at t time, and trace( ) is a trace operation.

[0018] In some embodiments, the determining the first state of the robot at the current position based on the position uncertainty comprises:

[0019] the first state of the robot at the current position is composed of the current position, the target position, the lidar data, and the position uncertainty of the robot at the current position.

[0020] In some embodiments, the reward comprises a target position reaching reward, a collision penalty and an uncertainty penalty, wherein:

[0021] the reaching target position reward refers to giving a positive reward when the distance between the robot and the target position is less than a preset threshold;

[0022] the collision penalty refers to giving a negative penalty when the robot collides with an obstacle;

[0023] the uncertainty penalty refers to applying a penalty to the change of the pose uncertainty of the robot at the current moment and the last moment according to the following formula to encourage the robot to select an action that can reduce the uncertainty,

[0024]

[0025] wherein, is the uncertainty penalty at time t, λ is a positive proportional coefficient, ∑ t and ∑ t-1 are the pose estimation covariance matrices of the robot at time t and time t-1 respectively, and trace( ) is a trace operation.

[0026] A second aspect of the embodiments of the present disclosure provides an exploration mapping method, comprising:

[0027] taking the current position as a root node, newly creating and initializing a rapidly-exploring random tree, adding a new position as a new node based on a preset strategy, evaluating the information gain of the new node, and repeatedly adding new nodes until a preset number is reached;

[0028] determining a target branch based on the sum of the information gains of all nodes of each branch of the rapidly-exploring random tree, and taking the end node of the target branch as a target position;

[0029] inputting the target position into a navigation strategy model trained according to the method of the first aspect of the embodiments of the present disclosure, outputting a navigation strategy, moving to the target position based on the navigation strategy, and expanding a map based on the target position.

[0030] In some embodiments, the evaluation of the information gain of the new node comprises:

[0031] evaluating the information gain of the new node based on a node evaluation function;

[0032] correcting the node evaluation function according to the following formula based on the density of dynamic objects,

[0033]

[0034] wherein, f(v) is a node evaluation function, I(v) is a geometric information gain, and C(v) is a path cost. wherein λ2 is a dynamic object penalty weight, n is a number of dynamic objects in a neighborhood of the node v, λ1 is a balance factor, and α is a balance factor.

[0035] In some embodiments, the dynamic object penalty weight of the node evaluation function is adaptively adjusted according to the number of observed dynamic objects according to the following formula:

[0036]

[0037] wherein λ2 is a dynamic object penalty weight, is an initial dynamic object penalty weight, n is a number of dynamic objects in a neighborhood of the node v, λ1 is a balance factor, and α is a balance factor. dyn is a cumulative number of observed dynamic objects, and κ is a decay rate.

[0038] A third aspect of the embodiments of the present disclosure provides a training device of a 3D target detection model, comprising:

[0039] A training device of a navigation strategy model, comprising:

[0040] A state determination module is configured to acquire a target position, determine a current position and a pose uncertainty of a robot at the current position, and determine a first state of the robot at the current position based on the pose uncertainty;

[0041] A sample preparation module is configured to acquire and execute a current strategy to move the robot to a second position, determine a second state of the robot at the second position and a reward, and compose an interaction sample based on the first state, the second state, the reward, and the current strategy, wherein the reward comprises an uncertainty penalty related to a change trend of the pose uncertainty.

[0042] A model training module is configured to repeat the above steps until the robot reaches the target position or meets a preset standard, and update parameters of the navigation strategy model based on the interaction sample.

[0043] A fourth aspect of the embodiments of the present disclosure provides an exploration mapping device, comprising:

[0044] A tree building module is configured to take a current position as a root node, newly build and initialize a rapidly-exploring random tree, add a new node based on a preset strategy, acquire dynamic object information within a preset range around the new node, correct information gain of the new node based on a density of the dynamic objects, and repeat adding new nodes until a preset number is reached.

[0045] A determination module is configured to determine a target branch based on a sum of information gain of all nodes of each branch of the rapidly-exploring random tree, and take an end node of the target branch as a target position.

[0046] The navigation module is configured to input the target position into the navigation strategy model trained by the method according to the first aspect of the embodiments of the present disclosure, output a navigation strategy, and move to the target position based on the navigation strategy and expand the map based on the target position.

[0047] In summary, the training method and device of the navigation strategy model and the exploration mapping method and device provided by the embodiments of the present disclosure realize active perception and control of the positioning quality by introducing the uncertainty information feedback loop of the SLAM process into the motion strategy. When the robot is located in an area with high positioning uncertainty, the learned strategy will automatically adopt a more conservative and robust motion mode; when the uncertainty is low, the strategy tends to explore unknown areas more actively. Compared with the motion decision based on geometric information alone, the uncertainty perception motion mode of the present disclosure can obtain more continuous and accurate trajectory estimation in a complex environment, thereby improving the mapping accuracy. Meanwhile, the dynamic object density information is integrated when the RRT is expanded, so that the observation points are naturally far away from the complex dynamic area. At the same time, the adaptive weight decay mechanism designed for dynamic objects can quickly avoid interference in the early exploration stage and resume exploration of the previous area in time in the later stage, thereby maximizing the coverage. Through the improvement of the two aspects, the present disclosure can significantly improve the mapping accuracy and robustness of active SLAM in unstructured dynamic scenes. BRIEF DESCRIPTION OF DRAWINGS

[0048] The features and advantages of the present disclosure will be more clearly understood through reference to the following drawings, which are presented as illustrative and should not be construed as limiting the present disclosure, in which:

[0049] Figure 1 is a schematic diagram of a computer system to which the present disclosure is applicable;

[0050] Figure 2 is a flowchart of a training method of a navigation strategy model according to some embodiments of the present disclosure;

[0051] Figure 3 is a training process in a simulated environment according to an embodiment of the present disclosure;

[0052] Figure 4 are motion trajectories of DRL strategies obtained with and without environment uncertainty perception in the movement process towards the same target observation point in a simulated environment;

[0053] Figure 5 is a flowchart of an exploration mapping method according to some embodiments of the present disclosure;

[0054] Figure 6 shows a dynamic construction process of an exploration map in an embodiment of the present disclosure;

[0055] Figure 7 This is a schematic diagram of a training device for a navigation strategy model according to some embodiments of the present disclosure;

[0056] Figure 8 This is a schematic diagram of a navigation device according to some embodiments of the present disclosure. Detailed Implementation

[0057] In the following detailed description, numerous specific details of this disclosure are set forth by way of example in order to provide a thorough understanding of the relevant disclosure. However, it will be apparent to those skilled in the art that this disclosure may be practiced without these details. It should be understood that the terms “system,” “apparatus,” “unit,” and / or “module” used in this disclosure are a method of distinguishing different parts, elements, sections, or components at different levels in a sequential arrangement. However, these terms may be replaced by other expressions if they can achieve the same purpose.

[0058] It should be understood that when a device, unit, or module is referred to as being "on," "connected to," or "coupled to" another device, unit, or module, it may be directly connected to or coupled to, or communicate with, other devices, units, or modules, or there may be intermediate devices, units, or modules present, unless the context explicitly indicates otherwise. For example, the term "and / or" as used in this disclosure includes any one and all combinations of one or more of the associated listed items.

[0059] The terminology used in this disclosure is for the purpose of describing particular embodiments only and is not intended to limit the scope of this disclosure. As shown in this specification and claims, unless the context clearly indicates otherwise, words such as "a," "an," "an," and / or "the" do not specifically refer to the singular and may include the plural. Generally speaking, the terms "comprising" and "including" only indicate the inclusion of explicitly identified features, integrals, steps, operations, elements, and / or components, and such expressions do not constitute an exclusive list, in which other features, integrals, steps, operations, elements, and / or components may also be included.

[0060] Referring to the following description and accompanying drawings, these and other features and characteristics, operating methods, functions of related structural elements, combinations of parts, and economics of manufacture of this disclosure can be better understood, wherein the description and drawings form part of the specification. However, it is clearly understood that the drawings are for illustrative and descriptive purposes only and are not intended to limit the scope of protection of this disclosure. It is understood that the drawings are not drawn to scale.

[0061] Various structural diagrams are used in the present disclosure to illustrate various variations of embodiments according to the present disclosure. It should be understood that the foregoing or the following structures are not intended to limit the present disclosure. The protection scope of the present disclosure is subject to the claims.

[0062] Figure 1 is a schematic diagram of a computer system to which the present disclosure is applicable. Figure 1 The system shown includes a moving object that simultaneously performs navigation, localization and mapping in an unknown place. Among them:

[0063] The unknown place refers to a place without a priori map.

[0064] The moving object can be an autonomous vehicle, in particular, a SLAM robot. The moving object contains a SLAM module, SLAM is the abbreviation of Simultaneous Localization And Mapping, which means simultaneous localization and mapping. The SLAM module can enable the moving object to operate autonomously in an unstructured environment without a priori map. The SLAM module in the present disclosure refers to an active SLAM module, which further realizes the synchronization of navigation and localization, mapping.

[0065] The moving object is deployed with one or more sensors. The sensor is used to collect the surrounding environment data of the moving object. Typically, the sensor is a laser radar, and the surrounding environment data is omnidirectional laser radar data.

[0066] The active SLAM module obtains the surrounding environment data from the sensor, and based on the surrounding environment data, autonomously plans a moving path and optimizes the deployment of the sensor to actively explore the environment and collect the most valuable sensor data for localization and mapping, thereby obtaining a more efficient and more accurate map. In some embodiments of the present disclosure, the active SLAM module obtains the surrounding moving object density, the current position and the position uncertainty of the current position based on the omnidirectional laser radar data.

[0067] Figure 2 is a flowchart of a training method of a navigation strategy model according to some embodiments of the present disclosure. In some embodiments, the training method of the navigation strategy model is executed by a model training server, and the training method of the navigation strategy model includes the following steps:

[0068] S210, obtaining a target position, determining a current position and a position uncertainty of the robot at the current position, and determining a first state of the robot at the current position based on the position uncertainty.

[0069] First, build the reinforcement learning environment. Use ROS to build a simulation environment, which should include a robot model, sensor models (laser radar, IMU), and dynamic obstacles (such as a moving pedestrian model).

[0070] Then define the state space:

[0071] At each decision-making time, collect the raw scan data of the robot's omnidirectional laser radar as the agent's observation of the surrounding environment. At each time, the robot's SLAM module performs particle filtering (Gmapping) based on laser radar data.

[0072] Particle filtering is a filtering algorithm widely used in robot and vehicle positioning. Its basic principle is to randomly select N points in the prediction domain, called particles. Calculate the predicted value, and calculate the probability in the measurement domain, that is, the weight. The weighted average is the optimal estimate. Then resample according to the weight ratio for the next iteration.

[0073] The disclosure will use the particle covariance matrix obtained based on Gmapping as a measure of the pose uncertainty of the robot at the current position.

[0074]

[0075] where ∑ t is the particle covariance matrix, and represent the weight and relative pose deviation of the u-th particle maintained in the Gmapping at time t, respectively.

[0076] The disclosure determines the position uncertainty of the current position based on the pose uncertainty of the current position determined by the example filtering

[0077]

[0078] where is the position uncertainty of the current position, ∑ t is the particle covariance matrix, and trace() is the trace operation.

[0079] The uncertainty-aware motion planning idea of the disclosure is not limited to Gmapping. In fact, as long as the SLAM backend can provide a quantitative indicator of the uncertainty of the pose estimate (such as the covariance matrix), it can be adapted with the reinforcement learning module of the disclosure.

[0080] At the same time, the SLAM module determines the current position based on the laser radar data and receives a set target position

[0081] from the laser radar data current position target position position uncertainty of the current position state data of the robot at the current position.

[0082] Therefore, the state definition of the present disclosure is:

[0083]

[0084] where s t is the state, is the lidar data, is the current position, is the target position.

[0085] S220, obtaining and executing the current policy to move the robot to the second position, determining the second state of the robot at the second position and the reward, and the interaction sample is composed of the first state, the second state, the reward and the current policy, wherein the reward includes an uncertainty penalty related to the trend of change of the position uncertainty.

[0086] Next, the action space is defined. The present disclosure adopts a discrete action space, that is, a set of combinations of linear velocity and angular velocity (v i ,ω j ) is predefined, and each action a t corresponds to a speed combination. The output of the policy network is the probability of each discrete action.

[0087] Finally, the reward function is designed. The reward function is used to guide the agent to learn the desired behavior, and the reward design in the present disclosure includes three parts:

[0088] arrival reward When the distance between the robot and the target point is less than a set threshold, a positive reward is given. The purpose of this reward is to encourage the robot to reach the target position as soon as possible.

[0089] collision penalty When the robot collides with the obstacle, a negative penalty is given. This reward is used to punish the behavior of the robot colliding with the obstacle.

[0090] positioning uncertainty penalty The current positioning uncertainty of the robot is punished for the change from the last time to encourage the robot to choose actions that can reduce the uncertainty.

[0091] The calculation formula is as follows:

[0092]

[0093] where λ is a positive proportionality coefficient, Σ t and Σ t-1 are the pose covariance matrices estimated by the particle filter SLAM algorithm at time t and t-1, respectively.

[0094] The reward function is represented as:

[0095] The robot obtains a current policy from the policy network, and moves from a first position to a second position based on the current policy.

[0096] A set of interaction data of the agent and the surrounding environment is composed of a first state of the robot at the first position, a second state of the robot at the second position, the current policy, and a reward for moving to the second position.

[0097] S230, repeating the above steps until the robot reaches the target position or meets a preset standard, updating the parameters of the navigation policy model based on the interaction samples.

[0098] Repeat S210-S220 until the robot reaches the target position or meets the maximum number of interaction steps. Train the policy network based on the interaction data collected in this process until convergence or other training end criteria are met to generate a navigation policy model.

[0099] One embodiment of the present disclosure tests the motion execution strategy of the present application in a simulation environment built by ROS and Gazebo. The experimental scene is an indoor environment of 800 square meters, including multiple rooms and corridors, and 6 moving obstacles are set to simulate dynamic pedestrians. The robot uses a differential drive model, with a maximum linear speed of 0.5 m / s and a maximum angular speed of 0.5 rad / s. The sensor configuration includes a 360° laser radar (noise standard deviation 0.1 m) and an IMU. The SLAM algorithm uses gmapping, and the particle filter uses 30 particles.

[0100] Figure 3 The training process in the simulation environment is shown.

[0101] Figure 4 The motion trajectories of the DRL strategy with and without environment uncertainty perception during movement towards the same target observation point are shown. The "X" points in the figure are the DRL obstacle avoidance navigation strategy with environment uncertainty perception. The "O" points in the figure are the general DRL obstacle avoidance strategy. The circular points in the figure are the trajectories of dynamic pedestrians. It can be seen that the strategy with mapping uncertainty perception moves more towards the area with rich map information, thereby maintaining the accuracy of the mapping.

[0102] Figure 5is a flow chart of an exploration mapping method according to some embodiments of the present disclosure, in some embodiments, the navigation method is performed by a SLAM module as shown in Figure 1 The navigation method comprises the following steps:

[0103] S510, a current position is taken as a root node, a rapidly expanding random tree is newly created and initialized, a new position is added as a new node based on a preset strategy, information gain of the new node is evaluated, and the new node is repeatedly added until a preset number is reached.

[0104] S520, a target branch is determined based on a sum of information gains of all nodes of each branch of the rapidly expanding random tree, and an end node of the target branch is taken as a target position.

[0105] The present disclosure provides a next best observation point selection mechanism fusing dynamic semantic information. Traditional observation point selection methods are mainly based on geometric information, such as new area, path cost, etc., and ignore the dynamic characteristics of different regions. The present application innovatively introduces a dynamic object penalty term to correct the node v evaluation function f(v) in the RRT expansion process:

[0106]

[0107] where I(v) is the geometric information gain, C(v) is the path cost, is an exponential function of the dynamic object density in the neighborhood of node v, and alpha, lambda1 and lambda2 are balance factors. The definition of f(v) is:

[0108]

[0109] where is the set of dynamic grids in the neighborhood of node v, p i and p v are the positions of dynamic grid i and node v respectively, and beta controls the spatial scale of the influence of dynamic objects.

[0110] When the density of dynamic objects in a certain region is high, the score f(v) of the node will be significantly reduced, so that the RRT expansion naturally avoids the dense area of people and preferentially explores the relatively stable and large information area in the environment. At the same time, in order to avoid excessive interference from dynamic objects due to insufficient information in the early exploration, the present application also introduces an adaptive weight adjustment mechanism:

[0111]

[0112] where is the initial dynamic object penalty weight, n dynκ controls the decay rate to accumulate the number of observed dynamic objects since the start of exploration. As exploration progresses, λ2 adaptively decreases, allowing previously suppressed areas to regain exploration opportunities. Through this dynamic perception-driven RRT extension strategy, the robot can actively avoid dynamic areas of the environment, reducing the interference of dynamic objects on localization and mapping.

[0113] The specific process is as follows:

[0114] Step 1. Initialize the Rapid Expanding Random Tree (RRT). Set the current robot position p r As the root node of RRT, and set the maximum number of iterations N to control the expansion depth of RRT.

[0115] Step 2. Repeat the following sub-steps N times:

[0116] 2.1 Random sampling point p rand A sampling point is randomly generated within the robot's perception range, serving as the direction for RRT expansion.

[0117] 2.2 Finding the nearest node p near Search for sampling point p in the current RRT. rand The node with the closest Euclidean distance is used as the starting point for expanding the tree.

[0118] 2.3 Obtaining the new node p using a motion model new From the nearest node p near Starting from point p, using the robot's motion model, the robot moves towards the sampling point p. rand Extend the direction by a certain distance to obtain a new node p. new .

[0119] 2.4 Calculate the dynamic occlusion probability. For a new node p new Based on the density of dynamic objects around it, the dynamic occlusion rate is calculated using formula (2).

[0120] (2-5) Evaluate the information gain of the new node. Based on the new node p new The position and orientation are used to calculate the information gain (Gain(p)) of the current map. new The path cost (Cost) to reach this node. near ,p new ) and the distance penalty Dist(p) from the robot's current position new ).

[0121] (2-6) Add the new node to RRT.

[0122] Step 3. Evaluate all branches of the RRT. Using Equation (1), calculate the total information gain for each branch of the RRT. The information gain, path cost, and dynamic object influence of each node are comprehensively considered.

[0123] Step 4. Selecting the optimal target point and path. Find the RRT branch with the maximum total gain The end node is taken as the next target observation point p *

[0124] S530, input the target position into a navigation strategy model trained according to the method, output a navigation strategy, and move to the target position based on the navigation strategy. Figure 2 The method trains a navigation strategy model.

[0125] Step 5. Output the target point p * The motion execution module guides the next exploration action of the robot.

[0126] Steps 1 to 5 are repeatedly performed, so that the robot can continuously perform autonomous exploration and mapping in a dynamic environment until the entire scene is covered.

[0127] One embodiment of the present disclosure tests the exploration and mapping strategy given in the present disclosure in a real environment. The experiment is performed in a 15m x 10m indoor environment containing multiple partitions and a large number of dynamic pedestrians. The robot is equipped with a 2D laser radar with a 360° field of view, and the SLAM algorithm uses Gmapping. The observation point selection is based on the improved RRT algorithm, which considers dynamic object information and path smoothness.

[0128] Figure 6 The dynamic construction process of the exploration map is shown. As the robot continuously explores the environment, the coverage range of the map (white area) gradually expands, and the originally unknown gray area gradually decreases. The light gray points in the figure are dynamic grid points, and the gray lines are mapping trajectories.

[0129] The present disclosure proposes a node evaluation mechanism that integrates dynamic semantic information under the RRT path planning framework. By introducing a dynamic object penalty term in the evaluation function, the expansion process tends to select areas with low dynamic object density as observation points. At the same time, an adaptive weight decay strategy is proposed to dynamically adjust the penalty strength as the exploration progresses, ensuring exploration efficiency while minimizing dynamic interference.

[0130] Figure 7 is a schematic diagram of a navigation strategy model training device according to some embodiments of the present disclosure. As shown in Figure 7 The navigation strategy model training device 700 includes a state determination module 710, a sample preparation module 720, and a model training module 730. In some embodiments of the present disclosure, the navigation strategy model training function can be performed by a model training server.

[0131] a state determining module 710, configured to obtain a target position, determine a current position and a pose uncertainty of the robot at the current position, and determine a first state of the robot at the current position based on the pose uncertainty;

[0132] a sample preparing module 720, configured to obtain and execute a current policy to move the robot to a second position, determine a second state of the robot at the second position and a reward, and compose an interaction sample based on the first state, the second state, the reward and the current policy, wherein the reward comprises an uncertainty penalty related to a change trend of the pose uncertainty;

[0133] a model training module 730, configured to repeat the above steps until the robot reaches the target position or meets a preset standard, and update parameters of the navigation policy model based on the interaction sample.

[0134] Figure 8 is a schematic diagram of a navigation device according to some embodiments of the present disclosure. As shown in Figure 8 , the navigation device 800 comprises a tree building module 810, a determining module 820 and a navigation module 830. In some embodiments of the present disclosure, the navigation function can be performed by the SLAM module as shown in Figure 1 .

[0135] wherein:

[0136] The tree building module 810 is configured to take the current position as a root node, newly create and initialize a rapidly-exploring random tree, add new nodes based on a preset policy, obtain dynamic object information within a preset range around the new nodes, correct the information gain of the new nodes based on the density of the dynamic objects, and repeat the adding of new nodes until a preset number is reached.

[0137] The determining module 820 is configured to determine a target branch based on the sum of the information gain of all nodes of each branch of the rapidly-exploring random tree, and take the end node of the target branch as a target position.

[0138] The navigation module 830 is configured to input the target position into a navigation policy model trained according to the method described in S210-S230 of Figure 2 , and output a navigation policy based on the navigation policy model, and move to the target position based on the navigation policy.

[0139] In summary, the training method and device of the navigation strategy model and the exploration mapping method and device provided by the embodiments of the present disclosure realize active perception and control of the positioning quality by introducing the uncertainty information feedback loop of the SLAM process in the motion strategy. When the robot is located in an area with high positioning uncertainty, the learned strategy will automatically adopt a more conservative and robust motion mode; and when the uncertainty is low, the strategy tends to explore unknown areas more actively. Compared with the motion decision based on geometric information, the uncertainty perception motion mode of the present disclosure can obtain more continuous and accurate trajectory estimation in a complex environment, thereby improving the mapping accuracy. Meanwhile, the dynamic object density information is integrated when the RRT is extended, so that the observation points are naturally away from the complex dynamic area. At the same time, the adaptive weight decay mechanism designed for dynamic objects can quickly avoid interference in the early exploration stage and timely resume the exploration of the previous area in the later stage, thereby maximizing the coverage. With the improvement of the two aspects, the present disclosure can significantly improve the mapping accuracy and robustness of active SLAM in an unstructured dynamic scene.

[0140] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the devices and modules described above can refer to the corresponding description in the foregoing device embodiments, which will not be described here.

[0141] Although the subject matter described herein is provided in the general context of computer-executable instructions of a program module being executed by a computer system on a computer-based platform, those skilled in the art will recognize that other implementations can be performed in combination with other types of program modules. Generally, program modules include routines, programs, components, data structures, and other types of structures that perform particular tasks or implement particular abstract data types. Those skilled in the art will appreciate that the subject matter described herein can be practiced with other computer system configurations, including hand-held devices, multiprocessor systems, microprocessor-based or programmable consumer electronics, minicomputers, mainframe computers, and the like. The subject matter can also be practiced in distributed computing environments where tasks are performed by remote processing devices that are linked through a communications network. In a distributed computing environment, program modules can be located in both local and remote memory storage devices.

[0142] Those of ordinary skill in the art can realize that the units and method steps of the examples described in combination with the embodiments disclosed herein can be realized in electronic hardware or in a combination of computer software and electronic hardware. Whether the functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present disclosure.

[0143] It should be understood that the foregoing detailed description of the disclosure, rather than limiting the disclosure, is intended to exemplify and elicit the principles of the disclosure. Accordingly, any modification, equivalent replacement, improvement, etc. made without departing from the spirit and scope of the disclosure should be included in the scope of protection of the disclosure. In addition, the claims of the disclosure are intended to cover all changes and modifications falling within the scope and boundary of the appended claims, or the equivalent form of such scope and boundary.

Claims

1. A method for training a navigation strategy model, characterized in that, The method comprises: obtaining a target position, determining a current position and a position uncertainty of the robot at the current position, determining a first state of the robot at the current position based on the position uncertainty; obtaining and executing a current policy to move the robot to a second position, determining a second state of the robot at the second position and a reward, and composing an interaction sample from the first state, the second state, the reward and the current policy, wherein the reward comprises an uncertainty penalty related to a trend of change of the position uncertainty; repeating the above steps until the robot reaches the target position or meets a preset standard, and updating parameters of the navigation policy model based on the interaction sample; wherein the reward comprises a target position arrival reward, a collision penalty and an uncertainty penalty, and wherein: the target position arrival reward refers to giving a positive reward when the distance between the robot and the target position is less than a preset threshold; the collision penalty refers to giving a negative penalty when the robot collides with an obstacle; the uncertainty penalty refers to applying a penalty to the change of the pose uncertainty of the robot at the current time and the last time according to the following formula to encourage the robot to select an action that can reduce the uncertainty, where, is the uncertainty penalty at time t, λ is a positive scaling factor, and t and t-1 are the pose estimation covariance matrices of the robot at time t and t - 1, respectively, and trace() is the trace operation.

2. The method of claim 1, wherein, the determination of the current position comprises: obtaining omnidirectional laser radar data of the robot on the surrounding environment, and determining the current position of the robot based on the laser radar data.

3. The method of claim 2, wherein, The determination of the position uncertainty of the robot at the current position comprises: performing example filtering on the laser radar data, and determining a pose estimation covariance matrix of the robot at the current position based on the example filtering data according to the following formula, where ∑ t is the pose estimation covariance matrix of the robot at the current position at time t, and wi and δpi represent the weight and the relative pose bias of the i-th particle maintained in the example filter at time t, respectively. based on the pose estimation covariance matrix, obtaining a position uncertainty of the robot at the current position at time t, wherein, is the position uncertainty of the robot at time t at the current position, ∑ t is the pose estimate covariance matrix of the robot at time t at the current position, and trace() is the trace operation.

4. The method of claim 3, wherein, the determination of the first state of the robot at the current position based on the position uncertainty comprises: composing the first state of the robot at the current position from the current position, the target position, the laser radar data and the position uncertainty of the robot at the current position.

5. An exploration mapping method characterized by, The method comprises: taking the current position as a root node, newly creating and initializing a rapidly expanding random tree, adding a new position as a new node based on a preset policy, evaluating the information gain of the new node, and repeatedly adding new nodes until a preset number is reached; determining a target branch based on the sum of the information gains of all nodes of each branch of the rapidly expanding random tree, and taking the end node of the target branch as a target position; inputting the target position into a navigation policy model trained according to the method of any one of claims 1-4, outputting a navigation policy, moving to the target position based on the navigation policy, and expanding a map based on the target position.

6. The method of claim 5, wherein, The evaluation of the information gain of the new node comprises: evaluating the information gain of the new node based on a node evaluation function; correcting the node evaluation function according to the following formula based on the density of dynamic objects, where f(v) is the node evaluation function, I(v) is the geometric information gain, C(v) is the path cost, is the density of dynamic objects in the neighborhood of node v, λ2is the dynamic object penalty weight, and α, λ1, are balancing factors.

7. The method of claim 6, wherein: the dynamic object penalty weight of the node evaluation function is adaptively adjusted according to the following formula according to the number of observed dynamic objects where λ2 is a dynamic object penalty weight, is an initial dynamic object penalty weight, n dyn is the cumulative number of observed dynamic objects, and κ is a decay rate.

8. A training device for a navigation strategy model, characterized in that, The method comprises: a state determination module configured to obtain a target position, determine a current position and a position uncertainty of the robot at the current position, and determine a first state of the robot at the current position based on the position uncertainty. a sample preparation module configured to obtain and execute a current policy to move the robot to a second position, determine a second state of the robot at the second position and a reward, and generate an interaction sample comprising the first state, the second state, the reward, and the current policy, wherein the reward comprises an uncertainty penalty related to a trend of change in the pose uncertainty; a model training module configured to repeat the above steps until the robot reaches the target position or meets a preset criterion, and update parameters of the navigation policy model based on the interaction sample; wherein the reward comprises a target position arrival reward, a collision penalty, and an uncertainty penalty, and wherein: the target position arrival reward refers to a positive reward given when the distance between the robot and the target position is less than a preset threshold; the collision penalty refers to a negative penalty given when the robot collides with an obstacle; the uncertainty penalty refers to a penalty applied to the change in the pose uncertainty of the robot at the current time and the last time according to the following formula, to encourage the robot to select an action that can reduce the uncertainty, where, is the uncertainty penalty at time t, λ is a positive scaling factor, and t and t-1 are the pose estimation covariance matrices of the robot at time t and t-1, respectively, and trace() is the trace operation.

9. An exploration mapping device, characterized by comprising: a tree building module configured to take the current position as a root node, newly build and initialize a rapidly-exploring random tree, add new nodes based on a preset policy, obtain dynamic object information within a preset range around the new nodes, correct the information gain of the new nodes based on the density of the dynamic objects, and repeat the adding of new nodes until a preset number is reached; a determination module configured to determine a target branch based on the sum of the information gain of all nodes of each branch of the rapidly-exploring random tree, and take the end node of the target branch as a target position; a navigation module configured to input the target position into a navigation policy model trained according to the method of any one of claims 1-4, output a navigation policy, move to the target position based on the navigation policy, and expand a map based on the target position.

Images