Wheeled robot navigation method for complex indoor environments

Abstract

A wheeled robot navigation method for complex indoor environments addresses the problem of unnatural and unsmooth obstacle avoidance in densely populated and restricted scenes using the DWA and TEB algorithms, belonging to the field of robotics. The method comprises: utilizing three-dimensional laser SLAM and binocular vision SLAM to construct maps, respectively, to obtain a point cloud map and a visual pose map; during machine positioning, the positioning results of the point cloud map and the visual pose map are integrated to complete global positioning; planning the global path of the wheeled robot; utilizing three-dimensional laser SLAM to obtain a local map centered on the wheeled robot; generating the wheeled robot's next path point based on the global path; inputting the local map centered on the wheeled robot, tracking information about pedestrians to be bypassed, and the next path point into an obstacle avoidance model; and outputting the wheeled robot's obstacle avoidance action strategy from the obstacle avoidance model.

Description

Technical Field

[0001] The invention relates to a wheeled robot navigation method for complex indoor environments, belonging to the technical field of robots. Background Art

[0002] In terms of map creation, the gmapping algorithm based on single-line lidar only utilizes the spatial distribution information of objects at specific heights in the environmental information. The lack of sensor information leads to very limited algorithm mapping accuracy. Due to the lack of loop detection, the operation results may be unstable in complex environments.

[0003] In terms of global positioning, similar to the gmapping algorithm, the Adaptive Monte Carlo Localization (AMCL) algorithm only utilizes the spatial distribution of objects at specific heights in the environment. When the environment is feature-poor or there are many dynamic obstacles, the effective information in a single-line laser point cloud frame is insufficient, which may lead to positioning failure. Furthermore, the stability of the particle filter-based AMCL algorithm is highly dependent on a good initial value estimate, and the algorithm converges less efficiently when the map features are complex.

[0004] In terms of global path planning, A * The efficiency of the A Star algorithm or the Dijkstra algorithm is significantly affected by map granularity and scale. When the map granularity is fine or the map scale is large, they may not meet the real-time requirements of robot path planning. Furthermore, the planning metrics of these two algorithms primarily focus on minimizing path length, and path smoothness needs to be optimized, which results in the robot's actual walking trajectory appearing unnatural to human perception.

[0005] In terms of local path planning and obstacle avoidance, both the Dynamic Window Approach (DWA) and Time-Elastic-Band (TEB) algorithms make static assumptions about obstacles. When planning the robot's speed, they always assume the invariance of the spatial distribution of obstacles. They adapt to low-speed dynamic obstacles by frequently updating the local cost map and local path. Due to their lack of predictions for dynamic obstacles, these two algorithms perform poorly in confined scenarios with dense pedestrian traffic. The actual planned real-time robot speed is prone to jitter, and the smoothness of the local path is also poor. This results in the robot's movements being unnatural and fluid when avoiding pedestrians, and prone to sudden turns and stops. Summary of the Invention

[0006] To address the problem that the obstacle avoidance actions of the DWA algorithm and the TEB algorithm are not natural and smooth enough in restricted scenes with dense pedestrians, the present invention provides a wheeled robot navigation method for complex indoor environments.

[0007] A wheeled robot navigation method for complex indoor environments according to the present invention comprises:

[0008] S1. Use 3D laser SLAM and binocular vision SLAM to build maps respectively, and obtain point cloud maps and visual pose maps. When the machine is positioning, the positioning results of the point cloud map and visual pose map are integrated to complete global positioning;

[0009] S2, planning the global path of the wheeled robot;

[0010] S3. Using 3D laser SLAM to obtain a local map centered on the wheeled robot, generating the next path point of the wheeled robot based on the global path, inputting the local map centered on the wheeled robot, all pedestrian position and velocity information generated by the pedestrian tracking module of the wheeled robot, and the next path point into an obstacle avoidance model, which outputs an obstacle avoidance action strategy for the wheeled robot;

[0011] The obstacle avoidance model includes a feature encoder and a strategy network. The local map centered on the wheeled robot, the position and speed information of all pedestrians, and the next path point are simultaneously input into the feature encoder. The feature encoder uses multiple convolutional layers and a multi-layer perceptron 1 to encode the local map centered on the wheeled robot to obtain a feature vector 1. The feature encoder uses a self-attention module and a multi-layer perceptron 2 to encode the tracking information of the pedestrians to be bypassed to obtain a feature vector 2. The feature encoder uses a multi-layer perceptron 3 to encode to obtain a feature vector 3. The feature vector 1, feature vector 2, and feature vector 3 are simultaneously input into the strategy network; the strategy network connects the feature vector 1, feature vector 2, and feature vector 3 and processes them using a multi-layer perceptron 4, and then inputs the processing results into the actor network of the PPO framework, and the actor network outputs the obstacle avoidance action strategy of the wheeled robot.

[0012] Preferably, the obstacle avoidance action strategy of the wheeled robot includes a forward strategy, a strategy of bypassing the pedestrian on the left from the left, a strategy of bypassing the pedestrian on the left from the right, a strategy of bypassing the pedestrian on the right from the left, a strategy of bypassing the pedestrian on the left from the right, and a strategy of waiting in place.

[0013] As a preferred method, the obstacle avoidance model is implemented using the reinforcement learning ORCA algorithm. The reward function during the training process of the obstacle avoidance model includes:

[0014] (1) Collision penalty: When a collision occurs, the current round ends immediately and the environment is reset;

[0015] (2) Rewards when reaching the target point or sub-target point: The wheeled robot always takes the next path point as its target. When the wheeled robot reaches the sub-target point, the current round is not over and learning needs to continue. When the wheeled robot reaches the target point, the current round ends and the environment is reset;

[0016] (3) Approach reward: When the wheeled robot is closer to the next waypoint at the current moment than at the previous moment, the reward is positive, otherwise it is negative;

[0017] (4) Time penalty: a negative constant.

[0018] Preferably, during the training process, the method for selecting obstacle-avoiding pedestrians in the obstacle avoidance action strategy of the wheeled robot is as follows: all pedestrians generated by the pedestrian tracking module of the wheeled robot are sorted, the pedestrian closest to the wheeled robot is found as the detour target, the corresponding detour radius is calculated, and the initial velocity direction is obtained.

[0019] Preferably, the S1 includes:

[0020] The global positioning map includes a point cloud map and a visual pose map. 3D laser SLAM uses the Faster-LIO algorithm, which processes 3D point clouds directly and fuses the processed 3D point clouds with inertial measurement unit data using a filtering method to obtain fused point cloud data. This fused point cloud data is stored using an incremental sparse voxel data structure to complete the construction of the point cloud map.

[0021] Binocular vision SLAM uses the DVDS algorithm, which eliminates interference from dynamic objects through a detection and rejection mechanism, uses discrete Transformer modules to enhance feature aggregation, and introduces a dense bundle adjustment layer to iteratively calculate the pose and inverse depth of the binocular camera to complete the construction of the visual pose map.

[0022] During global positioning, the confidence of the positioning results of the point cloud map and the visual pose map is judged, and the coordinate transformation relationship between the point cloud map and the visual pose map is determined based on the sensor calibration information. The positioning results of the point cloud map and the visual pose map are integrated to obtain the global positioning of the wheeled robot.

[0023] Preferably, S2 includes establishing a navigation map, using the D*Lite algorithm to calculate the path length and path smoothness based on the navigation map, incorporating the path smoothness into the planning cost function, and planning the global path of the wheeled robot in combination with the path length.

[0024] Preferably, the method for establishing a navigation map includes:

[0025] First, the point cloud map is compressed and projected to obtain a two-dimensional grid map;

[0026] Then, a navigation line is generated according to the starting point and end point on the two-dimensional grid map. The contour point set of the basic obstacle is obtained through the navigation line, and the unnecessary intermediate points in the contour point set are deleted using the convex hull optimization technology.

[0027] Finally, the simplified contour points of the extended obstacle are obtained according to the contour point set and the starting and ending points, the adjacency relationship between the points is established, and the optimized navigation map is generated.

[0028] The beneficial effects of the present invention are that the mapping and positioning accuracy of the present invention is higher, the stability and versatility are better, and the risk of positioning loss can be effectively reduced. Even when the scene is large and the environmental structure is relatively complex, it can meet the real-time operation requirements and the movement trajectory is smoother. In scenes with many dynamic obstacles (pedestrians), the obstacle avoidance strategy is more intelligent and can avoid collisions with a smoother trajectory. In terms of local path planning and obstacle avoidance, both the DWA algorithm and the TEB algorithm make static assumptions about obstacles. When planning the speed of the wheeled robot, they always assume the invariance of the obstacle distribution in space, and adapt to low-speed dynamic obstacles through high-frequency updates of the local cost map and local path. Due to the lack of prediction of dynamic obstacles, the two algorithms perform generally in restricted scenes with dense pedestrians. The actual planned real-time speed of the wheeled robot is prone to jitter, and the smoothness of the local path is also poor, which is reflected in the fact that the wheeled robot's movements are not natural and smooth when avoiding pedestrians, and it is prone to sudden turns and sudden stops. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 This is a schematic diagram of the ROS navigation system based on the move_base framework;

[0030] Figure 2 Schematic diagram of the principle of the navigation method of the present invention;

[0031] Figure 3 This is a schematic diagram of the principle of global path planning;

[0032] Figure 4 Schematic diagram of the principle of the obstacle avoidance model of the present invention. DETAILED DESCRIPTION

[0033] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0034] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0035] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but they are not intended to limit the present invention.

[0036] The existing ROS navigation method based on the move_base framework includes four parts: mapping, positioning, global path planning and local path planning. Figure 1 As shown in FIG, the move_base framework of ROS is used to unify the interfaces between various algorithms and combine them into a navigation system to realize the navigation of wheeled robots. The wheeled robot navigation method for complex indoor environments in this embodiment is as follows: Figure 2 Shown, including:

[0037] Step 1 Mapping and positioning based on multimodal fusion: 3D laser SLAM and binocular vision SLAM are used to build maps respectively to obtain point cloud maps and visual pose maps. During machine positioning, the positioning results of the point cloud map and visual pose map are fused to complete global positioning:

[0038] Three-dimensional laser SLAM and binocular vision SLAM are used to respectively build maps and locate, and then multi-modal positioning results are fused. Compared with the Gmapping algorithm and AMCL algorithm, it can effectively utilize the spatial structure information and surface texture information within a larger height range in the environment, greatly improving the accuracy and stability of mapping and positioning.

[0039] Specifically, 3D laser SLAM uses Faster-LIO, which uses a direct method for point cloud processing, a filtering method to fuse 3D point clouds and inertial measurement unit data, and an incremental sparse voxel data structure to store point clouds. Ultimately, it achieves high computing speed, high precision, and real-time point cloud map creation and global positioning that can be used in high-speed motion scenarios.

[0040] Binocular visual SLAM uses DVDS (Deep Visual Dynamic SLAM). This algorithm eliminates interference from dynamic objects through a detection and rejection mechanism, uses discrete Transformer modules to enhance feature aggregation, and introduces a dense bundle adjustment layer to iteratively calculate the camera's pose and inverse depth. Ultimately, it achieves high-precision visual pose map establishment and global positioning in complex dynamic environments.

[0041] The above two algorithms are used to respectively locate the point cloud map and the visual pose map, judge the confidence of the two positioning results, and determine the coordinate transformation relationship between the point cloud map and the visual map based on the sensor calibration information. The positioning results of the two modal maps are integrated to obtain stable and reliable high-precision global positioning.

[0042] Step 2: Plan the global path of the wheeled robot:

[0043] The global path planning principle of this embodiment is as follows Figure 3 As shown in the figure, it includes two parts: building a navigation map and using the improved D*Lite algorithm for planning.

[0044] Target A * The algorithm and Dijkstra algorithm cannot meet the real-time requirements of path planning when the map granularity is fine or the scale is large, so map optimization is required. First, the point cloud map created by 3D laser SLAM is compressed and projected to obtain a 2D grid map. Then, navigation lines are generated based on the starting and ending points on the 2D grid map. The contour point set of the basic obstacle is obtained through the navigation lines, and unnecessary intermediate points in the set are deleted using convex hull optimization technology. Considering the situation where concave polygons cause the encirclement dilemma, constraints are added to the process of simplifying the contour points of the wheeled robot in order to prevent the starting and ending points of the wheeled robot from being inside the concave polygon. Finally, the simplified contour points of the extended obstacle are further obtained based on the simplified contour points of the basic obstacle and the starting and ending points. The adjacency relationship between the points is established to generate an optimized navigation map.

[0045] Target A * The poor path smoothness of the D* algorithm and the Dijkstra algorithm requires the addition of corresponding path smoothness constraints during planning. The D*Lite algorithm combines the dynamic programming characteristics of the D* algorithm with the incremental search characteristics of the LPA* algorithm, making it well-suited for path planning in unknown environments. Path smoothness and path length are characterized by calculations based on parameters related to path turning angles, and path smoothness is incorporated into the algorithm's planning cost function. Both path length and smoothness are considered during planning. The improved D*Lite algorithm sacrifices path length to improve path smoothness, making the wheeled robot's trajectory more natural and fluid.

[0046] Step 3: Use 3D laser SLAM to obtain a local map centered on the wheeled robot. Generate the next path point of the wheeled robot based on the global path. Input the local map centered on the wheeled robot, all pedestrian position and speed information generated by the pedestrian tracking module of the wheeled robot, and the next path point into the obstacle avoidance model. The obstacle avoidance model outputs the wheeled robot's action strategy:

[0047] To address the problem that the DWA and TEB algorithms' obstacle avoidance actions are not natural and smooth in confined scenes with dense pedestrians, pedestrian tracking technology is used to obtain the positions of pedestrians around the wheeled robot and estimate their speeds. Deep reinforcement learning technology is then introduced to improve the obstacle avoidance action strategy of the ORCA (Optimal Reciprocal Collision Avoidance) algorithm, effectively improving the intelligence of the wheeled robot's obstacle avoidance behavior. The deep reinforcement learning PPO (Proximal Policy Optimization) framework will be introduced to abstract the wheeled robot's obstacle avoidance action strategy into a Markov decision process. By setting a reasonable reward function, the wheeled robot's obstacle avoidance behavior decision-making ability in dense and dynamic pedestrian scenes will be trained, enabling the wheeled robot to make intelligent obstacle avoidance strategies in real time.

[0048] The specific structure of the deep reinforcement learning network used to generate obstacle avoidance action strategies is as follows Figure 4 As shown in Figure 1, the obstacle avoidance model consists of two parts: a feature encoder and a policy network. In the feature encoder, the local grid map centered on the wheeled robot is encoded into feature vector 1 using multiple convolutional layers and a multi-layer perceptron 1. All pedestrian position and velocity information generated by the wheeled robot's pedestrian tracking module is encoded into feature vector 2 using a self-attention module and a multi-layer perceptron 2. The next path point generated by global path planning is directly encoded into feature vector 3 using a multi-layer perceptron 3. In the policy network, the three feature vectors are concatenated and processed by a multi-layer perceptron 4. During training, the result of this processing is input into the critic network of the PPO framework to obtain the current value of the network. This is then input into the actor network of the PPO framework and, when constrained by a manually designed action mask, the wheeled robot's action policy is obtained. During training, the current value is used to evaluate the actor network.

[0049] Compared to directly outputting the wheeled robot's speed, outputting a high-level strategy is safer and more reliable, and it also facilitates convergence during training. The wheeled robot's obstacle avoidance strategies in this embodiment include a forward movement strategy, a strategy for bypassing a pedestrian on the left from the left, a strategy for bypassing a pedestrian on the left from the right, a strategy for bypassing a pedestrian on the right from the left, a strategy for bypassing a pedestrian on the left from the right, and a strategy for waiting in place.

[0050] To improve learning efficiency, the output actions are preprocessed using an action mask. Based on the wheeled robot's current state (position, obstacle information, pedestrian information), its next state can be easily determined after it performs an action. If the next state is infeasible (such as colliding with a wall), the output layer prevents the action, allowing the wheeled robot to choose from the remaining actions.

[0051] Regarding the reward function setting for reinforcement learning, a dense reward function is defined to address the sparse reward situation where wheeled robots find it difficult to obtain positive rewards by reaching their destinations during long-distance navigation. The function consists of four parts: (1) Collision penalty: When a collision occurs, the current round ends immediately and the environment is reset; (2) Reward when reaching the target point or sub-target point: The wheeled robot always takes the next waypoint as its target. When the wheeled robot reaches the sub-target point, the current round does not end and learning needs to continue. When the wheeled robot reaches the target point, the current round ends and the environment is reset; (3) Approach reward: When the wheeled robot is closer to the next waypoint at this moment than at the previous moment, the reward is positive, otherwise it is negative; (4) Time penalty: It is a negative constant that penalizes the navigation time of the wheeled robot, so that the wheeled robot reaches the destination as soon as possible and prevents the wheeled robot from standing still and falling into an infinite loop.

[0052] The wheeled robot avoids only one pedestrian at a time. As the wheeled robot continues to operate, it will change its obstacle avoidance target in real time, allowing it to navigate in an environment with multiple pedestrians. When the output is forward, the direction of the required speed in ORCA points to the next waypoint. During the training process, when constructing the training set, when choosing to bypass a pedestrian, it is necessary to find the pedestrian to be bypassed. For example, to bypass a pedestrian on the left, the wheeled robot first sorts the pedestrians in the nearby area and finds the pedestrian closest to the left as the bypass target. The corresponding bypass radius is calculated to obtain the initial speed direction. This speed direction is used as the initial optimization value of the ORCA algorithm, and the final real-time speed of the wheeled robot is obtained through optimization based on the speed barrier theory.

[0053] For local navigation, the wheeled robot uses the next waypoint rather than the final destination. Obstacle images use local information, and pedestrian information is relative. These inputs are independent of the global map. The wheeled robot learns to reach the next waypoint based on information about nearby pedestrians and the environment. Therefore, even when the global map changes, the method remains effective as long as the waypoints are chosen appropriately.

[0054] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely illustrative of the principles and applications of the invention. It should be understood that many modifications may be made to the illustrative embodiments, and that other arrangements may be devised, without departing from the spirit and scope of the invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in ways other than those described in the original claims. It should also be understood that features described in conjunction with individual embodiments may be employed in conjunction with other described embodiments.

Claims

1. A wheeled robot navigation method for complex indoor environments, characterized in that: The method comprises: S1. Use 3D laser SLAM and binocular vision SLAM to build maps respectively, and obtain point cloud maps and visual pose maps. When the machine is positioning, the positioning results of the point cloud map and visual pose map are integrated to complete global positioning; S2, planning the global path of the wheeled robot; S2 includes establishing a navigation map, using the D* Lite algorithm to calculate the path length and path smoothness based on the navigation map, incorporating the path smoothness into the planning cost function, and planning the global path of the wheeled robot in combination with the path length; S3. Using 3D laser SLAM to obtain a local map centered on the wheeled robot, generating the next path point of the wheeled robot based on the global path, inputting the local map centered on the wheeled robot, all pedestrian position and velocity information generated by the pedestrian tracking module of the wheeled robot, and the next path point into an obstacle avoidance model, which outputs an obstacle avoidance action strategy for the wheeled robot; The obstacle avoidance model includes a feature encoder and a strategy network. The local map centered on the wheeled robot, the position and speed information of all pedestrians, and the next path point are simultaneously input into the feature encoder. The feature encoder uses multiple convolutional layers and a multi-layer perceptron 1 to encode the local map centered on the wheeled robot to obtain a feature vector 1. The feature encoder uses a self-attention module and a multi-layer perceptron 2 to encode the tracking information of the pedestrians to be bypassed to obtain a feature vector 2. The feature encoder uses a multi-layer perceptron 3 to encode the next path point generated by the global path planning to obtain a feature vector 3. The feature vector 1, feature vector 2, and feature vector 3 are simultaneously input into the strategy network; the strategy network connects the feature vector 1, feature vector 2, and feature vector 3 and processes them using a multi-layer perceptron 4, and then inputs the processing results into the actor network of the PPO framework, and the actor network outputs the obstacle avoidance action strategy of the wheeled robot.

2. The wheeled robot navigation method for complex indoor environments according to claim 1, characterized in that: The obstacle avoidance action strategies of the wheeled robot include forward strategy, strategy of bypassing the pedestrian on the left from the left, strategy of bypassing the pedestrian on the left from the right, strategy of bypassing the pedestrian on the right from the left, strategy of bypassing the pedestrian on the left from the right, strategy of bypassing the pedestrian on the left from the right, and strategy of waiting in place.

3. The wheeled robot navigation method for complex indoor environments according to claim 2, characterized in that: During the training process, when constructing the training set, the method for selecting obstacle-avoiding pedestrians in the wheeled robot's obstacle avoidance action strategy is: sort all pedestrians generated by the wheeled robot's pedestrian tracking module, find the pedestrian closest to the wheeled robot as the detour target, calculate the corresponding detour radius, and obtain the initial velocity direction.

4. The wheeled robot navigation method for complex indoor environments according to claim 1, characterized in that: The reward function in the obstacle avoidance model training process includes: (1) Collision penalty: When a collision occurs, the current round ends immediately and the environment is reset; (2) Rewards when reaching the target point or sub-target point: The wheeled robot always takes the next path point as its target. When the wheeled robot reaches the sub-target point, the current round is not over and learning needs to continue. When the wheeled robot reaches the target point, the current round ends and the environment is reset; (3) Approach reward: When the wheeled robot is closer to the next waypoint at the current moment than at the previous moment, the reward is positive, otherwise it is negative; (4) Time penalty: a negative constant.

5. The wheeled robot navigation method for complex indoor environments according to claim 1, characterized in that: Said S1 comprises: The global positioning map includes a point cloud map and a visual pose map. 3D laser SLAM uses the Faster-LIO algorithm, which processes 3D point clouds directly and fuses the processed 3D point clouds with inertial measurement unit data using a filtering method to obtain fused point cloud data. This fused point cloud data is stored using an incremental sparse voxel data structure to complete the construction of the point cloud map. Binocular vision SLAM uses the DVDS algorithm, which eliminates interference from dynamic objects through a detection and rejection mechanism, uses discrete Transformer modules to enhance feature aggregation, and introduces a dense bundle adjustment layer to iteratively calculate the pose and inverse depth of the binocular camera to complete the construction of the visual pose map. During global positioning, the confidence of the positioning results of the point cloud map and the visual pose map is judged, and the coordinate transformation relationship between the point cloud map and the visual pose map is determined based on the sensor calibration information. The positioning results of the point cloud map and the visual pose map are integrated to obtain the global positioning of the wheeled robot.

6. The wheeled robot navigation method for complex indoor environments according to claim 1, characterized in that: Methods for building navigation maps include: First, the point cloud map is compressed and projected to obtain a two-dimensional grid map; Then, a navigation line is generated according to the starting point and end point on the two-dimensional grid map. The contour point set of the basic obstacle is obtained through the navigation line, and the unnecessary intermediate points in the contour point set are deleted using the convex hull optimization technology. Finally, the simplified contour points of the extended obstacle are obtained according to the contour point set and the starting and ending points, the adjacency relationship between the points is established, and the optimized navigation map is generated.

7. A computer-readable storage device storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the wheeled robot navigation method for complex indoor environments as claimed in any one of claims 1 to 6 are implemented.

8. A wheeled robot navigation device for complex indoor environments, comprising a storage device, a processor, and a computer program stored in the storage device and running on the processor, characterized in that: The processor executes the computer program to implement the steps of the wheeled robot navigation method for complex indoor environments according to any one of claims 1 to 6.

9. A computer program product comprising a computer program, characterized in that When the computer program is executed by a processor, the steps of the wheeled robot navigation method for a complex indoor environment as claimed in any one of claims 1 to 6 are implemented.

Images