Active SLAM method based on generalized non-Markov strategy gradient
By constructing an environmental prior graph in the active SLAM method and introducing a generalized non-Markov strategy gradient optimization robot trajectory, the problems of insufficient prior information utilization and algorithm stability in the existing methods are solved, and more efficient and accurate autonomous exploration is achieved.
Patent Information
- Application Number
- CN202510449159.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-04-10
- Publication Date
- 2025-07-11
AI Technical Summary
The existing active SLAM method ignores a priori information and relies on Markov strategy to lead to insufficient suboptimal trajectory selection and algorithm stability, especially in sparse reward scenarios, high variance and difficulty in convergence.
The environment prior map is constructed based on the prior information of the structured environment, and the objective function is constructed based on the connectivity of the robot pose map and the trajectory distance. The generalized non-Markov strategy gradient is used to optimize the robot trajectory, and an adaptive baseline is introduced to reduce the gradient variance.
The exploration efficiency and mapping accuracy of the active SLAM method are improved, and the stability and convergence of the algorithm in sparse reward scenarios are improved.
Smart Images
Figure CN120295314A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of robot autonomous navigation and SLAM (Simultaneous Localization and Mapping), and particularly relates to an active SLAM method based on generalized non-Markov policy gradient. Background Art
[0002] The current active SLAM methods can be divided into four methods: search-based methods, sampling-based methods, information-theoretic methods, and reinforcement learning methods. The existing active SLAM methods enable the robot to actively form closed loops, thereby reducing the influence of cumulative errors during the autonomous exploration process.
[0003] However, the existing active SLAM methods have the following defects:
[0004] (1) Ignoring prior information: The existing active SLAM methods rely on real-time sensor data and do not utilize prior topological information such as building floor plans, resulting in repeated exploration and error accumulation;
[0005] (2) Markovianity limitation: The existing active SLAM methods adopt Markov-based policies and cannot handle closed-loop decisions that depend on historical trajectories, resulting in suboptimal trajectory selection;
[0006] (3) Insufficient algorithm stability: The reinforcement learning policy gradients adopted by the existing active SLAM methods have high variance in sparse reward scenarios and are difficult to converge. Summary of the Invention
[0007] In view of the above deficiencies in the prior art, the present invention provides an active SLAM method based on generalized non-Markov policy gradient.
[0008] In order to achieve the above invention objective, the technical solution adopted by the present invention is as follows:
[0009] Provide an active SLAM method based on generalized non-Markov policy gradient, including the following steps:
[0010] S1. Construct an environmental prior map based on the prior information of the structured environment;
[0011] S2. Construct a robot pose map based on the environmental prior map, generate an initial robot trajectory, and construct an objective function by combining the connectivity of the robot pose map and the total travel distance of the robot trajectory;
[0012] S3. Optimize the initial robot trajectory based on the objective function and the generalized non-Markov policy gradient to generate an optimized robot trajectory;
[0013] S4. Decompose the optimized robot trajectory into a sequence of navigation points, and control the robot through the sequence of navigation points to achieve active SLAM.
[0014] Furthermore, in step S1, the expression of the environmental prior map is as follows:
[0015]
[0016] Where: is the environmental prior map, is the node set of the real positions in the structured environment, ε is the edge set describing the node connectivity, and d(·) is the Euclidean distance between two nodes.
[0017] Furthermore, in step S2, the expression of the robot pose map is as follows:
[0018]
[0019] τ = (<s0, a0>, <s1, a1>,..., <s T-1 , a T-1 >, s T )
[0020] Where: is the pose map of the robot at trajectory τ, S is the pose set corresponding to the real position nodes in the structured environment, A is the action set corresponding to the edges describing the node connectivity, s0 is the pose corresponding to the real position node v0 in the structured environment, a0 is the action corresponding to the edge (v0, v1) describing the node connectivity, s1 is the pose corresponding to the real position node v1 in the structured environment, a1 is the action corresponding to the edge (v1, v2) describing the node connectivity, s T-1 is the pose corresponding to the real position node v T-1 in the structured environment, a T-1 is the action corresponding to the edge (v T-1 , v T ) describing the node connectivity, s T is the pose corresponding to the real position node v T in the structured environment, and T is the number of nodes in the trajectory τ.
[0021] Furthermore, in step S2, constructing the objective function by combining the connectivity of the robot pose map and the total travel distance of the robot trajectory includes the following steps:
[0022] A1. Calculate the connectivity of the robot pose map using the weighted Laplacian matrix;
[0023] A2. Calculate the total travel distance of the robot trajectory using the Euclidean distance of the robot trajectory;
[0024] A3. Construct the objective function based on the connectivity of the robot pose map and the total travel distance of the robot trajectory.
[0025] Furthermore, the expression of the objective function is as follows:
[0026]
[0027] Wherein: is the trajectory corresponding to the maximum value of the function F(τ), n is the number of real position nodes in the structured environment, log is the logarithmic function, det is the determinant symbol, and L ω is the weighted Laplacian matrix, ω is the covariance matrix weighting symbol, indicating that the actions corresponding to each edge describing the connectivity of nodes in the robot pose graph are weighted by the covariance matrix, is the pose graph of the robot at trajectory τ, β is the balance parameter, used to balance the two indicators of the connectivity of the robot pose graph and the total travel of the robot trajectory, and d(τ) is the total travel of the robot at trajectory τ.
[0028] Furthermore, step S3 includes the following steps:
[0029] S31. Construct an expected return model of the trajectory based on the objective function, and calculate the expected return of the initial trajectory of the robot using the expected return model of the trajectory;
[0030] S32. Calculate the gradient of the expected return with respect to the policy update based on the expected return of the initial trajectory of the robot and the generalized non-Markov policy gradient;
[0031] S33. Update the policy parameters using the gradient of the expected return with respect to the policy update, and obtain an optimized policy using the updated policy parameters, and execute the optimized policy to generate an optimized trajectory of the robot.
[0032] Furthermore, in step S31, the expression of the expected return model of the trajectory is:
[0033]
[0034] Wherein: J(θ) is the expected return of the robot at trajectory τ, t is the node number of trajectory τ, T is the number of nodes of trajectory τ, and π θ (a t |τ 0：t ) is the policy dependent on the historical trajectory τ 0:t The policy P(s t+1 |s t , a t ) is the pose transition probability, s t+1 is the pose corresponding to the real position node v t+1 in the structured environment, s t is the pose corresponding to the real position node v t in the structured environment, and a t is the edge (v describing the connectivity of nodest , v t+1 ) corresponding action, F(τ) is the objective function of the robot at trajectory τ, is the policy π θ The expected return corresponding to the generated trajectory τ.
[0035] Further, in step S32, the expression for calculating the gradient of the expected return with respect to the policy update is:
[0036]
[0037] Where: is the gradient of the expected return of the robot at trajectory τ with respect to the policy θ update, is the policy π θ The expected return corresponding to the generated trajectory τ, t is the node number of trajectory τ, T is the number of nodes of trajectory τ, is the gradient of the policy θ, log is the logarithmic function, π θ (a t |τ 0：t ) is the policy that depends on the historical trajectory τ 0：t , F(τ) is the objective function of the robot at trajectory τ, b(τ) is the adaptive baseline of the robot at trajectory τ.
[0038] Further, the expression of the adaptive baseline is:
[0039]
[0040] Where: g(τ) is the parameter of the baseline calculation process of the robot at trajectory τ.
[0041] The present invention has the following beneficial effects:
[0042] (1) The present invention constructs an environmental prior map according to the prior information of the structured environment, then constructs a robot pose map using the environmental prior map, and generates an initial robot trajectory. This process makes full use of the prior information of the structured environment and finally improves the exploration efficiency of the active SLAM method;
[0043] (2) The present invention optimizes the initial robot trajectory by adopting the generalized non-Markov policy gradient to generate an optimized robot trajectory, which can effectively handle the closed-loop decision depending on the historical trajectory, and finally improves the mapping accuracy of the active SLAM method;
[0044] (3) In the process of calculating the gradient of the expected return with respect to the policy update, the present invention overcomes the problems of high variance and difficult convergence of the reinforcement learning policy gradient adopted by the existing active SLAM methods by proposing an adaptive baseline subtraction, and finally improves the algorithm stability. Description of the Drawings
[0045] Figure 1 It is a schematic flow chart of an active SLAM method based on generalized non - Markov policy gradient;
[0046] Figure 2 It is a schematic diagram of the process of the present invention executing an optimization policy to generate an optimized trajectory of the robot;
[0047] Figure 3 It is an overall execution logic framework diagram of the method of the present invention;
[0048] Figure 4 They are schematic diagrams of three structured environments in the simulation experiment;
[0049] Figure 5 It is a comparison chart of variance results with and without an adaptive baseline in the simulation experiment;
[0050] Figure 6 It is a comparison chart of convergence results with and without an adaptive baseline in the simulation experiment;
[0051] Figure 7 They are learning curve diagrams of three structured environments in the simulation experiment;
[0052] Figure 8 It is a comparison chart of optimized path results output by different methods in the simulation experiment;
[0053] Figure 9 They are schematic diagrams of the occupancy map and the actual trajectory explored by the robot. Detailed Implementation Manner
[0054] The following describes the detailed implementation manner of the present invention to facilitate those skilled in the art of the present technology to understand the present invention. However, it should be clear that the present invention is not limited to the scope of the detailed implementation manner. For those of ordinary skill in the art of the present technology, as long as various changes are within the spirit and scope of the present invention defined and determined by the appended claims, these changes are obvious, and all inventions and creations using the concept of the present invention are within the scope of protection.
[0055] As Figure 1 shown, this solution provides an active SLAM method based on generalized non - Markov policy gradient, which includes steps S1 - S4, specifically as follows:
[0056] S1. Construct an environmental prior map based on the prior information of the structured environment.
[0057] In an alternative embodiment of the present invention, the expression of the environmental prior map is:
[0058]
[0059] Wherein: is the environmental prior map, is the node set of the true position in the structured environment, ε is the edge set describing the node connectivity, and d(·) is the Euclidean distance between two nodes.
[0060] S2. Construct a robot pose graph based on the environmental prior map, generate an initial robot trajectory, and construct an objective function by combining the connectivity of the robot pose graph and the total travel distance of the robot trajectory.
[0061] In an alternative embodiment of the present invention, the expression of the robot pose graph is:
[0062]
[0063] τ = (<s0, a0>, <s1, a1>,..., <s T-1 , a T-1 】, s T ) Wherein: is the pose graph of the robot at trajectory τ, S is the pose set corresponding to the true position nodes in the structured environment, A is the action set corresponding to the edges describing the node connectivity, s0 is the pose corresponding to the true position node v0 in the structured environment, a0 is the action corresponding to the edge (v0, v1) describing the node connectivity, s1 is the pose corresponding to the true position node v1 in the structured environment, a1 is the action corresponding to the edge (v1, v2) describing the node connectivity, s T-1 is the pose corresponding to the true position node v T-1 in the structured environment, a T-1 is the action corresponding to the edge (v T-1 , v T ) in the structured environment, s T is the pose corresponding to the true position node v T in the structured environment, and T is the number of nodes in trajectory τ.
[0064] The objective of the present invention is to find an optimized trajectory for the robot to quickly cover the structured environment while maintaining a reliable SLAM pose estimation by enhancing the connectivity of the robot pose graph. Therefore, the present invention considers two metrics, namely the connectivity of the robot pose graph and the total travel distance of the robot trajectory, when constructing the objective function.
[0065] The present invention constructs an objective function by combining the connectivity of the robot pose graph and the total travel distance of the robot trajectory, including the following steps:
[0066] A1. Calculate the connectivity of the robot pose graph using the weighted Laplacian matrix.
[0067] Specifically, the present invention calculates the weighted Laplacian matrix of the robot pose graph ω is the covariance matrix weighting symbol, indicating that the actions corresponding to each edge describing the connectivity of nodes in the robot pose graph are weighted by the covariance matrix. The determinant value of the Laplacian matrix of the robot pose graph is related to the connectivity of the robot pose graph. For the robot pose graph, its weighted Laplacian matrix is positive semi-definite and has at least one zero eigenvalue (corresponding to the all-ones eigenvector). As the connectivity of the robot pose graph increases, the non-zero eigenvalues of the weighted Laplacian matrix generally also increase, resulting in an increase in the determinant value. In the objective function, the present invention takes the determinant value of the weighted Laplacian matrix of the robot pose graph takes the logarithm and divides by the number n of true position nodes in the structured environment, thereby normalizing the determinant value of the weighted Laplacian matrix of the robot pose graph into a quantization index (D-optimal) independent of the size of the robot pose graph. This quantization index is used to evaluate the connectivity of the robot pose graph. The determinant value of the weighted Laplacian matrix of the robot pose graph is larger, the better the connectivity of the robot pose graph.
[0068] A2. Calculate the total travel distance of the robot trajectory using the Euclidean distance of the robot trajectory.
[0069] A3. Construct an objective function based on the connectivity of the robot pose graph and the total travel distance of the robot trajectory:
[0070]
[0071] where: is the trajectory corresponding to maximizing the function F(τ), n is the number of true position nodes in the structured environment, log is the logarithmic function, det is the determinant symbol, L ω is the weighted Laplacian matrix, w is the covariance matrix weighting symbol, indicating that the actions corresponding to each edge describing the connectivity of nodes in the robot pose graph are weighted by the covariance matrix, is the pose graph of the robot at trajectory τ, β is the balance parameter used to balance the two metrics of the connectivity of the robot pose graph and the total travel of the robot trajectory, and d(τ) is the total travel of the robot at trajectory τ.
[0072] S3. Optimize the initial robot trajectory based on the objective function and the generalized non-Markov policy gradient to generate an optimized robot trajectory.
[0073] In an alternative embodiment of the present invention, step S3 includes the following steps:
[0074] S31. Construct an expected return model of the trajectory based on the objective function, and calculate the expected return of the initial robot trajectory using the expected return model of the trajectory.
[0075] The expression of the expected return model of the trajectory is as follows:
[0076]
[0077] Where: J(θ) is the expected return of the robot at trajectory τ, t is the node number of trajectory τ, T is the number of nodes of trajectory τ, π θ (a t |τ 0:t ) is the policy that depends on the historical trajectory τ 0:t P(s t+1 |s t ,a t ) is the attitude transition probability, s t+1 is the attitude corresponding to the real position node v t+1 in the structured environment, s t is the attitude corresponding to the real position node v t in the structured environment, a t is the action corresponding to the edge (v t , v t+1 ) that describes the node connectivity, F(τ) is the objective function of the robot at trajectory τ, is the expected return corresponding to the trajectory τ generated by the policy π θ .
[0078] S32. Calculate the gradient of the expected return with respect to the policy update based on the expected return of the robot's initial trajectory and the generalized non-Markov policy gradient;
[0079] The expression for calculating the gradient of the expected return with respect to the policy update in the present invention is:
[0080]
[0081] Where: is the gradient of the expected return of the robot at trajectory τ with respect to the policy θ update, is the expected return corresponding to the trajectory τ generated by the policy π θ , t is the node number of trajectory τ, T is the number of nodes of trajectory τ, is the gradient of the policy θ, log is the logarithmic function, π θ (a t |τ 0：t ) is the policy that depends on the historical trajectory τ 0：t , F(τ) is the objective function of the robot at trajectory τ, b(τ) is the adaptive baseline of the robot at trajectory τ.
[0082] Specifically, the present invention first establishes a primary expression for calculating the gradient of the expected return with respect to the policy update:
[0083]
[0084] Then, the present invention considers the problems of high variance and difficult convergence of the reinforcement learning policy gradient adopted by existing active SLAM methods in sparse reward scenarios. Because only when the robot pose graph related to the trajectory τ is connected, the determinant value of the weighted Laplacian matrix of the robot pose graph is non-zero, that is, only when the robot reaches a complete trajectory can it obtain a reward. Therefore, the gradient has a high variance and cannot be eliminated through causality. So, the present invention reduces the variance by introducing an adaptive baseline. For any random trajectory function independent of the action, that is, the adaptive baseline, there is the following relational expression:
[0085]
[0086] Based on the above relational expression, the present invention can subtract the adaptive baseline from the primary expression of calculating the gradient of the expected reward with respect to the policy update without changing the gradient, and obtain the expression of calculating the gradient of the expected reward with respect to the policy update.
[0087] In the present invention, the policy that depends on the historical trajectory τ 0:t has the following expression:
[0088]
[0089] where: EXP is the exponential function, is the policy parameter related to the action a t , is the transpose symbol, φ(τ 0:t ) is the feature vector of the historical trajectory, derived from a multi-layer perceptron (MLP), and θ a is the policy parameter related to the action a.
[0090] The expression of the adaptive baseline is:
[0091]
[0092] where: g(τ) is the baseline calculation process parameter of the robot at the trajectory τ.
[0093] Specifically, in order to obtain the above adaptive baseline, the present invention calculates the derivative of the variance with respect to b(τ), and the expression is:
[0094]
[0095] where: Var is the variance symbol.
[0096] S33. Update the policy parameters using the gradient of the expected return with respect to the policy update, and obtain an optimized policy using the updated policy parameters, and execute the optimized policy to generate an optimized trajectory for the robot.
[0097] The expression for updating the policy parameters using the gradient of the expected return with respect to the policy update in the present invention is:
[0098]
[0099] where: θ′ is the updated policy parameter, and α is the learning rate.
[0100] The present invention obtains an optimized policy using the updated policy parameters. Specifically: The present invention uses a linear softmax function to map the initial trajectory of the robot to the probability of action selection based on the updated policy parameters to obtain an optimized policy.
[0101] As Figure 2 shown, the present invention executes the optimized policy to generate an optimized trajectory for the robot. The left part shows the construction of an environmental prior map based on the prior information of the structured environment, where the red dots represent the robot poses and the green lines represent the actions between two nodes. In the upper right part, the present invention formulates the problem as a non-Markov decision process by determining the current node as the robot pose and defining the edge connecting the two executed nodes as the action. The robot forms a yellow exploration route according to the trained green optimized trajectory of the robot on the environmental prior map, and then in the lower right part, the complete optimized trajectory of the robot is finally formed.
[0102] S4. Decompose the optimized trajectory of the robot into a sequence of navigation points, and control the robot through the sequence of navigation points to achieve active SLAM.
[0103] In an optional embodiment of the present invention, the present invention integrates an online navigator, which has functions of prior exploration, pose graph update, and local replanning. The present invention uses the online navigator to decompose the optimized trajectory of the robot into a sequence of navigation points, and controls the robot through the sequence of navigation points to achieve active SLAM.
[0104] As Figure 3 shown, it provides an overall execution logic framework diagram of the method of the present invention.
[0105] Simulation experiment:
[0106] As Figure 4As shown, the proposed method of the present invention was experimented in three structured environments env.1, env.2, and env.3 with different sizes and structures. The generalized non-Markov policy gradient in the present invention was implemented in Python 3.12 on a 64-bit Windows 10 desktop equipped with an i9-14900 CPU and 32GB of RAM. To evaluate the performance of exploration, the proposed method of the present invention was integrated into an online navigator and simulation experiments were conducted in the corresponding environment on an Ubuntu 20.04 system with ROS noetic. The present invention compared this method with several state-of-the-art algorithms: the Frontier-based method, the TSP (Traveling Salesman)-based method, and the SLAM-aware exploration (SAE) method. The Frontier-based method is a representative algorithm that does not require a prior map; the TSP-based method solves the TSP problem on the prior map to obtain a path that quickly covers the entire environment; SAE is a recently proposed method based on the selection information closed-loop of TSP.
[0107] A. Efficiency Verification of the Present Invention
[0108] The present invention set the learning rate α to 2×10-4 and the balance parameter β to 0.1. It is assumed that each edge in the environmental prior map has a constant measurement covariance map (0.1m, 0.1m, 0.001rad). The present invention defined the action space as adjacent edges of a pose, and its size is the maximum degree of nodes in the environmental prior map. To mask invalid actions, the corresponding parameter components were set to negative infinity, so the probabilities of these action selections became zero.
[0109] The present invention first evaluated the effectiveness of the adaptive baseline in a simple environmental prior map. Figure 5 Describes the sample variance during the learning process. From the figure, the present invention learned that the adaptive baseline effectively reduced the variance. Figure 6 Shows the convergence of GNP (Generalized Non-Markov Policy Gradient) with and without a baseline, which also proves that the adaptive baseline can enhance the stability of the algorithm.
[0110] Figure 7 Shows the learning curves under different environmental prior map inputs. The results show that the method proposed by the present invention can finally reach the respective optimal exploration strategies in the three structured environments. Figure 8 Is the optimized trajectory generated by the method of the present invention and the TSP-based method. Table 1 records the dpath (total travel distance of the robot trajectory) and D-opt (connectivity of the robot pose graph) corresponding to the optimized trajectories generated by the TSP-based method and GNP (the method of the present invention), showing the pose reliability of the improvement ratio (Ratio) as follows:
[0111] Table 1
[0112]
[0113] From Figure 8 As can be seen from Table 1, the optimized trajectory generated by the method of the present invention can enhance the reliability of SLAM.
[0114] B. Performance Comparison in the Benchmark Environment
[0115] The present invention uses an adaptive baseline in a simulation environment to benchmark the performance of the method of the present invention. Figure 9 It shows the occupancy map and the actual trajectory explored by the robot. The red line represents the robot pose graph, and the blue line represents loop closure. The online navigator allows the robot to explore the boundaries around each vertex and replan the robot's trajectory locally according to the connectivity of the actual robot pose graph.
[0116] Table 2 records the simulation results of the method of the present invention and other methods as follows:
[0117] Table 2
[0118]
[0119] In all environments, compared with other methods, the method of the present invention has the smallest absolute pose error (APE) and satisfactory exploration efficiency. Figure env.1 has the smallest topology and the largest free space, resulting in insignificant differences in pose error among the four algorithms, while the SAE-based method and the method of the present invention still introduce additional distance costs. In addition, due to the small size of Figure env.2, the cumulative pose errors of the TSP-based method, the SAE-based method, and the method of the present invention are still at a low level. However, due to the lack of prior information, the boundary-based method performs the worst in terms of connectivity. In contrast, the method of the present invention shows greater advantages in Figure env.3. The TSP-based method only considers the total travel distance and does not consider connectivity, resulting in a large pose error. The SAE-based method selects additional loop closures on the basis of the TSP-based method to improve the accuracy of pose estimation. However, the method of the present invention uses a generalized non-Markov policy gradient to learn the optimized path and significantly reduces the pose error.
[0120] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It should be understood that each flow and / or block in the flowchart and / or block diagram, and combinations of flows and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing device to produce a machine, such that the instructions executed by the processor of the computer or other programmable data processing device generate means for implementing the functions specified in one or more of the flows Figure 1 one or more of the flows and / or blocks Figure 1 or means for implementing the functions specified in one or more of the blocks.
[0121] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to operate in a particular manner, such that the instructions stored in the computer-readable memory produce a manufacture including instruction means that implement the functions specified in one or more of the flows Figure 1 one or more of the flows and / or blocks Figure 1 or means for implementing the functions specified in one or more of the blocks.
[0122] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operational steps are performed on the computer or other programmable device to produce a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more of the flows Figure 1 one or more of the flows and / or blocks Figure 1 or means for implementing the functions specified in one or more of the blocks.
[0123] Specific embodiments are used in the present invention to elaborate on the principles and implementation manners of the present invention. The description of the above embodiments is only used to help understand the method and its core idea of the present invention; at the same time, for those of ordinary skill in the art, according to the idea of the present invention, there will be changes in the specific implementation manners and application scopes. In summary, the content of this specification should not be construed as a limitation to the present invention.
[0124] Those of ordinary skill in the art will realize that the embodiments described herein are for helping the reader understand the principles of the present invention, and it should be understood that the protection scope of the present invention is not limited to such specific statements and embodiments. Those of ordinary skill in the art can make various other specific deformations and combinations that do not depart from the essence of the present invention based on the technical revelations disclosed in the present invention, and these deformations and combinations are still within the protection scope of the present invention.
Claims
1. An active SLAM method based on generalized non-Markov policy gradient, characterized in that It includes the following steps: S1. Construct an environmental prior map based on the prior information of the structured environment; S2. Construct a robot pose map based on the environmental prior map, generate an initial robot trajectory, and construct an objective function by combining the connectivity of the robot pose map and the total travel distance of the robot trajectory; S3. Optimize the initial robot trajectory based on the objective function and the generalized non-Markov policy gradient to generate an optimized robot trajectory; S4. Decompose the optimized robot trajectory into a sequence of navigation points, and control the robot through the sequence of navigation points to achieve active SLAM.
2. The active SLAM method based on the generalized non-Markov policy gradient according to claim 1, wherein In step S1, the expression of the environmental prior map is: Wherein: is the environmental prior map, is the node set of the true positions in the structured environment, ε is the edge set describing the node connectivity, and d(·) is the Euclidean distance between two nodes.
3. The active SLAM method based on the generalized non-Markov policy gradient according to claim 1, wherein In step S2, the expression of the robot pose map is: τ = (<s0, a0>, <s1, a1>,..., <s T-1 , a T-1 】>, s T ) Wherein: is the attitude graph of the robot at trajectory τ, S is the set of attitudes corresponding to the real position nodes in the structured environment, A is the set of actions corresponding to the edges describing the node connectivity, s0 is the attitude corresponding to the real position node v0 in the structured environment, a0 is the action corresponding to the edge (v0, v1) describing the node connectivity, s1 is the attitude corresponding to the real position node v1 in the structured environment, a1 is the action corresponding to the edge (v1, v2) describing the node connectivity, s T-1 is the real position node v in the structured environment T-1 corresponding attitude, a T-1 is the edge (v T-1 , v T ) corresponding action, s T is the real position node v in the structured environment T corresponding attitude, T is the number of nodes in trajectory τ.
4. The active SLAM method based on the generalized non-Markov policy gradient according to claim 1, characterized in that, In step S2, constructing the objective function by combining the connectivity of the robot pose map and the total travel distance of the robot trajectory includes the following steps: A1. Calculate the connectivity of the robot pose map using the weighted Laplacian matrix; A2. Calculate the total travel distance of the robot trajectory using the Euclidean distance of the robot trajectory; A3. Construct an objective function based on the connectivity of the robot pose map and the total travel distance of the robot trajectory.
5. The active SLAM method based on the generalized non-Markov policy gradient according to claim 1, characterized in that, The expression of the objective function is: Wherein: is to obtain the trajectory corresponding to the maximum value of the function F(τ), n is the number of nodes at the true position in the structured environment, log is the logarithmic function, det is the determinant symbol, and L ω is the weighted Laplacian matrix, ω is the covariance matrix weighting symbol, indicating that the actions corresponding to each edge describing the connectivity of nodes in the robot pose graph are weighted by the covariance matrix, is the pose graph of the robot at the trajectory τ, β is the balance parameter, used to balance the two indicators of the connectivity of the robot pose graph and the total travel of the robot trajectory, and d(τ) is the total travel of the robot at the trajectory τ.
6. The active SLAM method based on the generalized non-Markov policy gradient according to claim 1, wherein Step S3 includes the following steps: S31. Construct an expected return model of the trajectory based on the objective function, and calculate the expected return of the initial robot trajectory using the expected return model of the trajectory; S32. Calculate the gradient of the expected return with respect to the policy update based on the expected return of the initial robot trajectory and the generalized non-Markov policy gradient; S33. Update the policy parameters using the gradient of the expected return with respect to the policy update, obtain an optimized policy using the updated policy parameters, and execute the optimized policy to generate an optimized robot trajectory.
7. The active SLAM method based on the generalized non-Markov policy gradient according to claim 6, wherein, In step S31, the expression of the expected return model of the trajectory is: where: \(j(\theta)\) is the expected return of the robot at trajectory \(\tau\), \(t\) is the node number of trajectory \(\tau\), \(T\) is the number of nodes in trajectory \(\tau\), \(\pi\) θ (a t |\(\tau\) 0：t ) is the policy that depends on the historical trajectory \(\tau\) 0：t , \(P(s\) t+1 |s t , \(a\) t ) is the pose transition probability, \(s\) t+1 is the pose corresponding to the real position node \(v\) t+1 in the structured environment, \(s\) t is the pose corresponding to the real position node \(v\) t in the structured environment, \(a\) t is the action corresponding to the edge \((v\) t , \(v\) t+1 ) that describes the node connectivity, \(F(\tau)\) is the objective function of the robot at trajectory \(\tau\), is the expected return corresponding to the trajectory \(\tau\) generated by the policy \(\pi\) θ .
8. The active SLAM method based on the generalized non-Markov policy gradient according to claim 6, characterized in that In step S32, the expression for calculating the gradient of the expected return with respect to the policy update is: Wherein: is the gradient of the expected return of the robot at trajectory τ with respect to the update of policy θ, is the expected return corresponding to the trajectory τ generated by policy π θ , t is the node number of trajectory τ, and T is the number of nodes of trajectory τ, is the gradient of policy θ, log is the logarithmic function, π θ (a t |τ 0：t ) is the policy that depends on the historical trajectory τ 0：t , F(τ) is the objective function of the robot at trajectory τ, and b(τ) is the adaptive baseline of the robot at trajectory τ.
9. The active SLAM method based on the generalized non-Markov policy gradient according to claim 8, wherein, The expression of the adaptive baseline is: where: g(τ) is the parameter of the baseline calculation process when the robot is at trajectory τ.