Robot path planning method in unknown environment based on visual slam

Abstract

The application discloses a kind of unknown environment road under robot path planning method based on visual SLAM, it is related to robot autonomous navigation technical field.The method obtains environmental image data, constructs the probability topological map including key frame node and connected edge, and configures the path uncertainty cost based on the position estimation covariance calculation of map point for edge;On the map, global path is planned based on the composite cost function that fuses path length, safety cost and path uncertainty cost;By solving nonlinear model predictive control optimization problem in rolling, global path is converted into smooth control instruction to drive robot navigation.The application couples the uncertainty of SLAM process to path planning and dynamic control significantly, and the reliability and trajectory smoothness of robot in unknown environment navigation are improved.

Description

Technical Field

[0001] This invention relates to the field of robot autonomous navigation technology, specifically to a robot path planning method, system, electronic device, and storage medium based on visual SLAM (Simultaneous Localization and Mapping, SLAM) in unknown environments. More specifically, it relates to a method for real-time robot path planning and dynamic adjustment using visual simultaneous localization and mapping technology in unknown or dynamically changing environments. Background Technology

[0002] With the rapid development of robotics and artificial intelligence technologies, the need for mobile robots to perform autonomous exploration, search and rescue, and inspection tasks in unknown environments is becoming increasingly urgent. The core technological challenge lies in the fact that robots must, without prior maps, rely solely on their own sensors to perceive and construct maps of the environment in real time, and then plan a safe and efficient path based on these maps. Visual SLAM technology, due to its low-cost and information-rich sensors, has become the mainstream solution for achieving this goal. This technology allows robots to process image sequences captured by cameras, simultaneously estimate their own pose, and construct a map of the environment. Based on this map, combined with path planning algorithms, the robot can achieve autonomous navigation from its starting point to its destination.

[0003] Existing technical solutions mainly revolve around two dimensions: map representation and planning strategies. For example, patent document CN202210771312.7 proposes a scheme that integrates LiDAR and visual data to construct a grid map and combines Rapid Random Tree Exploration (RRT) and boundary exploration algorithms to select navigation target points. Another type of solution focuses on the safety and efficiency of planning. For instance, patent document CN202110711034.1 discloses a strategy that employs geometric topology path point search and a conservative adaptive trajectory replanning strategy, emphasizing that trajectories are generated only within known safe areas. Furthermore, patent document CN202511203557.X proposes a monitoring system that triggers dynamic replanning when the path deviation exceeds a threshold. Although the above-mentioned existing technologies have achieved certain application effects in specific scenarios, they still have profound technical shortcomings.

[0004] First, the decoupling of map representation from the SLAM process leads to information loss. For example, using grid maps crudely binarizes the rich geometric and uncertainty information generated by the visual SLAM process, completely ignoring the confidence level (i.e., covariance) of the SLAM system's estimation of the location of each landmark in the map. This causes the planner to be unable to distinguish the reliability of the map, and may choose a path that seems feasible but is actually located in a high-risk positioning area.

[0005] Secondly, the planning strategy is too conservative or lacks a balance between exploration and exploitation. For example, a strategy of "generating trajectories only within known safe areas" will cause the robot to move slowly or even stop in the early stages when the environment is completely unknown, greatly sacrificing exploration efficiency. On the other hand, boundary point-based exploration strategies often have heuristically designed gain functions that lack consideration for the reliability of the path itself, which may cause the robot to be attracted to areas where perception is difficult or where localization is prone to drift.

[0006] Third, the dynamic replanning mechanism is slow to respond and lacks optimality. The replanning mechanism based on the "deviation threshold" is a passive response mode. When the robot deviates from the path due to map updates or encountering dynamic obstacles, the system only starts to recalculate. This "stop-replanning-execution" mode will cause the robot's behavior to lag and it is difficult to guarantee the optimality of the new path in terms of global and dynamic feasibility.

[0007] In summary, existing technologies generally treat "SLAM mapping" and "path planning" as two independent steps. The planning layer only calls the final map result and does not perceive the difference in confidence level of the mapping, resulting in poor reliability of the planning results. There is an urgent need to propose a new technical solution. Summary of the Invention

[0008] I. Technical Solution

[0009] To achieve the above objectives, the present invention adopts the following technical solution: a robot path planning method based on visual SLAM in unknown environments, applied to a robot control system, comprising: The system acquires environmental image data collected by a visual sensor and constructs a probabilistic topology map based on the environmental image data. The probabilistic topology map includes nodes representing keyframes and edges representing the connectivity between keyframes. The edges are configured with path uncertainty costs, which are calculated based on the position estimation covariance of map points associated with the keyframes. On the probabilistic topology map, a global path is planned based on a composite cost function that integrates path length, security cost, and path uncertainty cost. The nonlinear model predictive control optimization problem is solved by rolling over the finite prediction time domain, and the global path is transformed into a smooth control command. The optimization problem aims to minimize the deviation between the predicted trajectory and the global path, as well as the energy consumption of the control input. The control commands are output to drive the mobile robot to perform navigation.

[0010] Furthermore, the path uncertainty cost is calculated using the location estimation covariance of map points within the path's vicinity, and the calculation formula is as follows: , In the formula, To connect keyframe nodes With keyframe nodes The path uncertainty cost corresponding to the edge; This represents the total number of map points in the area near the path between two keyframes. Index map points within the area near the path; For the first The covariance matrix of the location estimates of each map point; Represents the trace operation of a matrix.

[0011] Furthermore, the composite cost function is obtained by fusing multi-dimensional cost calculations, and the calculation formula is as follows: , In the formula, The total cost of a path segment extending from an existing node in the path tree to a newly generated node; The distance between the existing node and the newly generated node is the Euclidean distance. This represents the security cost corresponding to this path segment; This represents the cost of path uncertainty corresponding to this path segment; , , These are preset weighting coefficients, used to balance the cost proportions of path length, path security, and path reliability, respectively.

[0012] Furthermore, the nonlinear model predictive control optimization problem is achieved by solving the following objective function: , In the formula, Let be the objective function to be minimized; The control input sequence to be solved; The current moment; The length of the finite prediction time domain; To predict time variables in the time domain; For the robot's predicted state trajectory; This is a reference trajectory generated based on a global path; , These are the state deviation penalty weight matrix and the control input penalty weight matrix, respectively. This is the terminal cost function.

[0013] Furthermore, the location estimation covariance matrix of the map points is obtained by extracting the bundle adjustment optimization results from the backend of the visual synchronous positioning and mapping system.

[0014] Furthermore, during the global path planning process, a hybrid sampling strategy is adopted to generate new nodes for path expansion; the hybrid sampling strategy samples at a preset probability in the boundary region between the known free space and the unknown space, guiding the path tree to expand into the unknown region.

[0015] Furthermore, the process of transforming the global path into smooth control commands also includes a dynamic update mechanism: when the probabilistic topology map is updated due to loop closure detection, or when the visual sensor detects new dynamic obstacles, the reference trajectory or obstacle avoidance constraints in the nonlinear model predictive control optimization problem are updated in real time, and the updated optimization problem is solved in the next control cycle.

[0016] Furthermore, the safety cost is inversely proportional to the distance from the path segment to the nearest obstacle; when the distance is less than a preset safety threshold, the safety cost of the path segment is set to infinity, and the path extension direction is excluded.

[0017] Furthermore, the global path planning is implemented using a fast exploratory random tree star schema algorithm that integrates path uncertainty costs and a hybrid sampling strategy.

[0018] Furthermore, the present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the above-described visual SLAM-based robot path planning method in an unknown environment.

[0019] Compared with the prior art, the present invention has the following beneficial effects:

[0020] 1. Significantly improved navigation reliability: This invention retains and utilizes the covariance information from the SLAM process when building the map, and introduces it as a cost of path uncertainty into the cost function of path planning. This enables the planner to actively avoid areas with low SLAM positioning accuracy, fundamentally reducing the risk of path planning failure or collision due to positioning drift, and significantly improving the success rate of path execution in unknown environments.

[0021] 2. Intelligent Balance Between Exploration and Utilization: The fast exploration random tree star schema algorithm employed in this invention, with its composite cost function and hybrid sampling strategy, allows the robot to intrinsically "tend" to explore areas that reduce global map uncertainty while pursuing its objective, forming an intelligent exploration-utilization trade-off mechanism. Compared to heuristic boundary exploration methods, this method can effectively shorten the total path length and improve navigation efficiency when achieving the same exploration coverage.

[0022] 3. Dynamic Response Capability and Trajectory Smoothness: This invention employs NMPC-based dynamic replanning, unifying global path tracking and local obstacle avoidance within a single optimization framework. The robot can adjust its trajectory with millisecond-level response speeds to avoid suddenly appearing obstacles, and the entire process is continuous and smooth without noticeable pauses. Compared to the traditional "stop-replanning" method, this significantly improves the robot's average speed and trajectory curvature change rate in dynamic environments, resulting in smoother and more natural movement.

[0023] 4. Environmental Adaptability and Scalability: The topology map-based representation method of this invention saves more memory than grid maps and is more suitable for exploring large-scale environments. Furthermore, the framework of this invention has good modularity and scalability, allowing for easy integration of different types of SLAM systems and robot dynamics models, thus possessing strong industrial applicability. Attached Figure Description

[0024] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below.

[0025] Figure 1 This is an overall flowchart of a path planning method for an uncharted robot in an unknown environment based on visual SLAM, provided in an embodiment of the present invention.

[0026] Figure 2 The flowchart illustrates the rapid exploration random tree star topology algorithm in a path planning method for robots in unknown environments based on visual SLAM, as provided in this embodiment of the invention.

[0027] Figure 3 The flowchart of the NMPC dynamic replanning algorithm in a path planning method for robots in unknown environments based on visual SLAM provided in an embodiment of the present invention. Detailed Implementation

[0028] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention. Example 1

[0029] This embodiment provides a visual SLAM-based path planning system for robots in unknown environments, applied to robot control systems, such as a radioactive survey robot in a nuclear power plant decommissioning environment. In this scenario, the robot needs to enter an unknown environment with strong radioactivity, complex structure, and partially damaged areas to perform survey tasks, placing extremely high demands on the safety and reliability of its navigation. (Refer to...) Figure 1 The method may specifically include the following steps: Step S101: Acquire environmental image data and construct a probabilistic topology map.

[0030] Specifically, the robot control system acquires real-time image data of the environment through onboard vision sensors (such as RGB-D cameras) and runs a visual SLAM system (such as ORB-SLAM2). After optimizing the camera pose and the positions of 3D map points using bundle adjustment (BA) at the back end of the SLAM system, it not only saves the 3D coordinates of the map points but also extracts and stores the covariance matrix of each map point's estimated position. Subsequently, a topology graph is constructed based on the keyframe sequence generated by SLAM. Nodes in this topology graph represent keyframes, and edges represent the co-visibility relationships between keyframes. Crucially, a path uncertainty cost is configured for each edge. The path uncertainty cost is calculated by weighted averaging of the traces of the covariance matrices of map points in the vicinity region of the path connecting two keyframes. This step allows the constructed map model to quantify the positioning reliability of each region, overcoming the information loss problem of traditional grid maps and providing a crucial information foundation for subsequent reliable path planning. The rule for determining the vicinity region is as follows: a rectangular area centered on the pose line connecting the two keyframes, with a width twice the width of the robot chassis, is included in the uncertainty cost calculation for map points falling within this area.

[0031] Step S201: Plan the global path based on the composite cost function.

[0032] Specifically, upon receiving the global target point, the control system uses the Uncertainty Coupled Fast Exploration Random Tree Star (UC-RRT*) algorithm to search for the global optimal path on the probabilistic topology map constructed in step S101. (Refer to...) Figure 2 The core of this algorithm lies in its composite cost function, which considers the existing nodes in the path tree. Extend to newly generated nodes At that time, path length was taken into account. Security costs and path uncertainty cost By introducing uncertainty costs, the planner can intrinsically strike a balance between exploration efficiency and navigation reliability. For example, it might intentionally avoid a wide area with severe ground reflection (leading to unstable SLAM feature points and high uncertainty costs) and instead choose a slightly narrower corridor with rich wall textures (low uncertainty costs). Furthermore, to improve exploration efficiency, a hybrid sampling strategy can be employed, sampling with a certain probability in the leading edge region (the boundary between known and unknown space) to guide the path tree to expand into the unknown area. For example, sampling can be done with probability p at the boundary between known and unknown space (the leading edge region), and random sampling can be done with probability 1-p in the known free space; the initial value of p is 0.7, dynamically adjusted downwards as the environmental exploration coverage increases.

[0033] Specifically, the security cost S = k / d obs k is the preset safety factor, d obs This represents the distance from the path segment to the nearest obstacle. When the distance is less than the safety threshold, the safety cost is set to infinity, and the path segment is directly determined to be infeasible.

[0034] Step S301: Transform the global path into smooth control commands using nonlinear model predictive control (NMPC). Specifically, the control system uses the global path point sequence generated in step S201 as a reference to construct an NMPC optimization problem. (Refer to...) Figure 3 This problem, within a finite prediction time domain, seeks to find the optimal control input sequence (such as linear velocity and angular velocity) by continuously solving an objective function that minimizes the deviation of the predicted trajectory from the reference trajectory and the energy consumption of the control inputs. This optimization problem is simultaneously constrained by the robot's (e.g., differential robot) dynamics model, control input limitations, and real-time obstacle avoidance constraints. This approach enables continuous and smooth responses to environmental changes, avoiding the pauses and stutters of traditional replanning methods. For example, during navigation, if a piece of debris falls onto the path ahead, the visual sensor detects this new obstacle, and the obstacle avoidance constraints of the NMPC are immediately updated. In the next control cycle (e.g., 20 milliseconds), the system automatically calculates a fine-tuned trajectory that smoothly bypasses the debris, without requiring the robot to pause throughout the entire process.

[0035] Step S401: Apply control commands to drive the mobile robot to perform navigation.

[0036] Specifically, in each control cycle, the control system sends only the first control command of the calculated control sequence to the lower-level controller for execution. Simultaneously, when the SLAM system updates the map due to loop closure detection, the uncertainty of a large number of map points is significantly reduced, and the reference trajectory in the NMPC is updated accordingly. This makes subsequent trajectories more accurate and synchronously updates the path uncertainty cost of all edges in the probabilistic topology map. Global path planning is then re-executed, and the NMPC's reference trajectory is updated. This closed-loop feedback mechanism ensures that the robot can continuously, efficiently, and safely complete navigation tasks in unknown environments.

[0037] Through the above steps, this invention ensures highly reliable navigation for the surveying robot in environments with high uncertainty. By actively avoiding high-risk areas, the possibility of mission failure due to getting lost or colliding is greatly reduced. The application of NMPC enables the robot to respond flexibly and safely to sudden obstacles, ensuring the safety of expensive equipment. Ultimately, the robot efficiently and safely completes the surveying task of the target area, reducing the task time by approximately 30% compared to traditional methods, and requiring no human intervention throughout the process. Example 2

[0038] This embodiment provides a path planning system for robots in unknown environments based on visual SLAM. This system can be a hardware implementation of the method described in Embodiment 1. (Refer to...) Figure 1 This system is applied in robot control systems and may specifically include: The probabilistic topology map construction unit 101 is used to execute step S101 in Embodiment 1. Specifically, this unit is configured to acquire environmental image data collected by a visual sensor and run a visual SLAM system based on the data to construct a probabilistic topology map. The core function of this unit is to extract the location estimation covariance matrix of map points from the SLAM backend optimization results, and use the covariance information to calculate the path uncertainty cost of each edge in the topology map, thereby generating a map model that can quantify the reliability of positioning.

[0039] The path planning unit 102 is used to perform step S201 in Embodiment 1. Specifically, this unit is configured to plan a global path on the map generated by the probabilistic topology map construction unit 101 using the UC-RRT* algorithm. This unit evaluates the path using a composite cost function that integrates path length, security cost, and path uncertainty cost, thereby explicitly avoiding areas with high positioning uncertainty while ensuring safety and efficiency. The implementation of this unit results in a planned path that is both reliable and efficient.

[0040] The dynamic replanning unit 103 is used to execute step S301 in Embodiment 1. Specifically, this unit is configured to use the NMPC framework to transform the discrete path points output by the path planning unit 102 into smooth and dynamically feasible continuous control commands. By performing rolling optimization within a finite prediction time domain, this unit can unify the processing of global path tracking, robot dynamics constraints, and real-time obstacle avoidance. When a map update or a new dynamic obstacle is detected, this unit can update the reference trajectory or constraints in the optimization problem in real time and seamlessly calculate the new optimal control command in the next control cycle, thereby achieving a continuous and smooth response to environmental changes.

[0041] The navigation control unit is used to execute step S401 in Embodiment 1. This unit is configured to receive control commands generated by the dynamic replanning unit 103 and drive the actuators (such as motors) of the mobile robot to complete navigation actions.

[0042] This embodiment may also include an electronic device, which includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it implements the functions of the aforementioned units, that is, all the steps of the method described in Embodiment 1. The electronic device may be a robot controller, an onboard computer, or an embedded system with corresponding computing capabilities.

[0043] This embodiment may further include a computer-readable storage medium on which a computer program implementing the above method is stored. The medium may be any physical carrier capable of storing program code, such as a hard disk, solid-state drive, USB flash drive, or optical disk.

[0044] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A path planning method for robots in unknown environments based on visual SLAM, applied to robot control systems, characterized in that... Includes the following steps: The system acquires environmental image data collected by a visual sensor and constructs a probabilistic topology map based on the environmental image data. The probabilistic topology map includes nodes representing keyframes and edges representing the connectivity between the keyframes. The system also configures the edges with path uncertainty costs calculated based on the location estimation covariance of map points associated with the keyframes. On the probabilistic topology map, a global path is planned based on a composite cost function that integrates path length, security cost, and path uncertainty cost. The global path is transformed into a smooth control command by rolling a nonlinear model predictive control optimization problem within a finite prediction time domain. The optimization problem aims to minimize the deviation of the predicted trajectory from the global path and the energy consumption of the control input. The control commands are used to drive the mobile robot to perform navigation.

2. The method according to claim 1, characterized in that, The specific steps for calculating the path uncertainty cost are as follows: ， in, Indicates connection to keyframe nodes and keyframe nodes The cost of path uncertainty for edges; To connect the keyframe nodes and The total number of map points in the area near the path; This refers to the set of map points within the area near the path. The index of the map points in the set of map points; Represents the trace operation of a matrix; Indicates the first The location covariance matrix of each map point.

3. The method according to claim 1, characterized in that, The composite cost function is calculated using the following formula: ， in, This indicates that there are already nodes in the path tree. Extend to newly generated nodes The cost of the path segment; The Euclidean distance between the existing node and the newly generated node; This represents the safety cost between the path segment and the obstacle; The path uncertainty cost for the path segment; , and These are preset weighting coefficients used to balance path length, security, and reliability.

4. The method according to claim 1, characterized in that, The nonlinear model predictive control optimization problem is specifically achieved by solving the following objective function: ， in, Let be the objective function to be minimized; The control input sequence to be solved; The current moment; The length of the finite prediction time domain; The time variable within the prediction time domain; For the robot's predicted state trajectory; This is a reference trajectory generated based on the global path; and These are the weight matrices for penalizing state deviation and controlling input, respectively; This is the terminal cost function.

5. The method according to claim 2, characterized in that, The location estimation covariance matrix This was obtained by extracting the bundle adjustment optimization results from the backend of the visual synchronous localization and mapping system.

6. The method according to claim 3, characterized in that, The steps for planning a global path also include: The newly generated node is generated using a hybrid sampling strategy. The hybrid sampling strategy samples at a preset probability in the boundary region between the known free space and the unknown space to guide the path tree to expand into the unknown region.

7. The method according to claim 4, characterized in that, The step of converting the global path into smooth control commands further includes: The reference trajectory in the nonlinear model predictive control optimization problem is updated in real time when the probabilistic topology map is updated due to loop closure detection, or when the visual sensor detects a new dynamic obstacle. Alternatively, obstacle avoidance constraints may be applied, and the updated optimization problem is solved in the next control cycle.

8. The method according to claim 3, characterized in that, The security cost The safety cost is inversely proportional to the distance from the path segment to the nearest obstacle, and when the distance is less than a preset safety threshold, the safety cost is set to infinity.

9. The method according to claim 1, characterized in that, The step of planning a global path is implemented using a fast exploratory random tree star schema algorithm that integrates path uncertainty cost and a hybrid sampling strategy.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method as described in any one of claims 1-9.

Images