Wheeled robot navigation method for complex indoor environment

Abstract

The invention discloses a wheeled robot navigation method oriented to a complex indoor environment, solves the problem that obstacle avoidance actions of a DWA algorithm and a TEB algorithm are not natural and smooth enough in a limited scene with dense pedestrians, and belongs to the technical field of robots. The method comprises the steps that mapping is conducted through three-dimensional laser SLAM and binocular vision SLAM, a point cloud map and a vision pose map are obtained, and when machine positioning is conducted, positioning results of the point cloud map and the vision pose map are fused, and global positioning is completed; planning a global path of the wheeled robot; a local map with the wheeled robot as the center is obtained through three-dimensional laser SLAM, a next path point of the wheeled robot is generated according to the global path, and the local map with the wheeled robot as the center, tracking information of pedestrians needing to be bypassed and the next path point are input into an obstacle avoidance model; and the obstacle avoidance model outputs an obstacle avoidance action strategy of the wheeled robot.

Description

Technical Field

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

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

[0003] In terms of global positioning, similar to the gmapping algorithm, the AMCL (Adaptive Monte Carlo Localization) algorithm only uses the spatial distribution information of objects at a specific height in the environmental information. When the environmental features are relatively poor or there are many dynamic obstacles, there is less effective information in the single-line laser point cloud frame, which may lead to positioning failure. At the same time, the running stability of the AMCL algorithm based on particle filtering is very dependent on a good artificial estimated initial value, and the algorithm convergence efficiency is low when the map features are more complex.

[0004] In terms of global path planning, A * The operating efficiency of the A Star algorithm or the Dijkstra algorithm is greatly affected by the map granularity and map scale. When the map granularity is fine or the scale is large, it may not meet the real-time requirements of robot path planning. At the same time, the planning indicators of these two algorithms are mainly the shortest path length, and the path smoothness needs to be optimized, which is reflected in the unnatural feeling of the actual walking trajectory of the robot in human cognition.

[0005] In terms of local path planning and obstacle avoidance, the DWA (Dynamic Window Approach) algorithm and the TEB (Time-Elastic-Band) algorithm both make static assumptions about obstacles. When planning the robot's speed, they always assume the invariance of the obstacle distribution in space, and adapt to low-speed dynamic obstacles by updating the local cost map and local path at a high frequency. Due to the lack of prediction of dynamic obstacles, the two algorithms perform generally in restricted scenes with dense pedestrians. The actual planned robot's real-time speed is prone to jitter, and the smoothness of the local path is also poor, which is reflected in the robot's unnatural and unsmooth movements when avoiding pedestrians, and it is prone to sudden turns and sudden stops. Summary of the invention

[0006] In view of 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 a complex indoor environment of 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 positioned, the positioning results of the point cloud map and the visual pose map are fused to complete the 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 according to the global path, inputting 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, and the obstacle avoidance model outputs the obstacle avoidance action strategy of 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, the feature vector 2, and the feature vector 3 are simultaneously input into the strategy network; the strategy network connects the feature vector 1, the feature vector 2, and the feature vector 3 and processes them using a multi-layer perceptron 4, and then inputs the processing result 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 strategies of the wheeled robot include a forward movement strategy, a strategy of bypassing the pedestrian on the left side from the left side, a strategy of bypassing the pedestrian on the left side from the right side, a strategy of bypassing the pedestrian on the right side from the left side, a strategy of bypassing the pedestrian on the left side from the right side, and a waiting strategy.

[0013] As a preferred method, the obstacle avoidance model is implemented using the reinforcement learning ORCA algorithm. The reward function of the obstacle avoidance model during training 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 has not ended and it needs to continue learning. When it 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 comprises:

[0020] The global positioning map includes a point cloud map and a visual pose map. The 3D laser SLAM uses the Faster-LIO algorithm, which uses a direct method to process the 3D point cloud. The filtering method is used to fuse the processed 3D point cloud and the inertial measurement unit data to obtain the fused point cloud data. The incremental sparse voxel data structure is used to store the fused point cloud data to complete the construction of the point cloud map.

[0021] Stereoscopic vision SLAM uses the DVDS algorithm, which eliminates interference from dynamic objects through a detection and elimination mechanism, uses discrete Transformer modules to enhance feature aggregation, and introduces a dense bundle adjustment layer to iteratively calculate the posture and inverse depth of the stereo camera to complete the construction of the visual posture 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 according to 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 according to 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, compress and project the point cloud map to obtain a two-dimensional grid map;

[0026] Then, a navigation line is generated according to the starting point and the end point on the two-dimensional grid map, and the contour point set of the basic obstacle is obtained through the navigation line. 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. When the scene is large and the environmental structure is complex, it can also meet the real-time operation requirements and the moving trajectory is smoother. In scenes with many dynamic obstacles (pedestrians), the strategy adopted for obstacle avoidance is more intelligent, and collisions can be avoided 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, the invariance of the obstacle distribution in space is always assumed, and low-speed dynamic obstacles are adapted by 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 real-time speed of the wheeled robot actually planned is prone to jitter, and the smoothness of the local path is also poor, which is reflected in the fact that the wheeled robot is not natural and smooth enough when avoiding pedestrians, and 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 It is a 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 It is a schematic diagram of the principle of the obstacle avoidance model of the present invention. DETAILED DESCRIPTION

[0033] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. 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 creative work are 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 in conjunction with 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, 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 As shown, including:

[0037] Step 1 Mapping and positioning based on multimodal fusion: 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 positioned, the positioning results of the point cloud map and the visual pose map are fused to complete the global positioning:

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

[0039] Specifically, 3D laser SLAM uses Faster-LIO, which adopts direct method for point cloud processing, filtering method to fuse 3D point cloud and inertial measurement unit data, and incremental sparse voxel data structure to store point cloud, ultimately achieving high computing speed, high precision, and real-time point cloud map establishment and global positioning that can be used for high-speed motion scenes.

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

[0041] The above two algorithms are used to locate the point cloud map and the visual pose map respectively, and the confidence of the two positioning results is judged. The coordinate transformation relationship between the point cloud map and the visual map is determined according to 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 implementation is as follows Figure 3 As shown, it includes two parts: establishing a navigation map and using the improved D*Lite algorithm for planning.

[0044] For A * The algorithm and Dijkstra algorithm cannot meet the problem of real-time path planning when the map granularity is fine or the scale is large, and the map needs to be optimized. First, the point cloud map established by three-dimensional laser SLAM 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, and the contour point set of the basic obstacle is obtained through the navigation line. The convex hull optimization technology is used to delete unnecessary intermediate points in the set. 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 view of the fact that the starting point and end point may be 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 point and end point, and the adjacency relationship between the points is established to generate an optimized navigation map.

[0045] For A * The problem of poor path smoothness in the planning of the D* algorithm and the Dijkstra algorithm requires adding corresponding path smoothness constraints during planning. The D*Lite algorithm combines the dynamic programming characteristics of the D* algorithm and the incremental search characteristics of the LPA* algorithm, and can adapt well to path planning in unknown environments. The parameters related to the path turning angle are used to calculate the path smoothness and path length, and the path smoothness is incorporated into the planning cost function of the algorithm. The path length and smoothness will be considered simultaneously during planning. The improved D*Lite algorithm will sacrifice the path length to a certain extent to improve the path smoothness, making the movement trajectory of the wheeled robot more natural and smooth.

[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 according to 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, and the obstacle avoidance model outputs the wheeled robot action strategy:

[0047] In order to solve 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, pedestrian tracking technology is used to obtain the positions of pedestrians around the wheeled robot and estimate their speed. The obstacle avoidance action strategy of the ORCA (Optimal Reciprocal Collision Avoidance) algorithm is improved by introducing deep reinforcement learning technology, which can effectively improve 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 dynamic pedestrian scenes can be trained, so that the wheeled robot can 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 the figure, the obstacle avoidance model includes two parts: feature encoder and strategy network. In the feature encoder, for the local grid map centered on the wheeled robot, multiple convolutional layers and multi-layer perceptron 1 are used to encode it into feature vector 1; for all pedestrian position and speed information generated by the pedestrian tracking module of the wheeled robot, self-attention module and multi-layer perceptron 2 are used to encode it into feature vector 2; for the next path point generated by the global path planning, multi-layer perceptron 3 is directly used to encode it into feature vector 3. In the strategy network, the above three feature vectors are connected and processed by multi-layer perceptron 4. During the training process, the result processed by multi-layer perceptron 4 is input into the critic network part of the PPO framework to obtain the current value of the network (value), which is input into the actor network of the PPO framework. Combined with the manually designed action mask to constrain it, the wheeled robot action strategy (action) is obtained. During the training process, the current value (value) is used to evaluate the actor network.

[0049] In terms of the output of the action strategy, compared with directly outputting the wheeled robot speed, outputting the high-level strategy is safer and more reliable, and is easier to converge during training. The obstacle avoidance action strategy of the wheeled robot in this embodiment 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.

[0050] To improve learning efficiency, the output action is preprocessed using an action mask. Based on the current state of the wheeled robot (position, obstacle information, pedestrian information), its next state can be easily obtained after it performs an action. If the next state is not feasible (such as colliding with a wall), the output layer will prevent this action and let the wheeled robot choose from the remaining actions.

[0051] Regarding the reward function setting of reinforcement learning, a dense reward function is defined to address the sparse reward situation where it is difficult for a wheeled robot to obtain positive rewards by reaching the destination 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 only avoids one pedestrian at each moment. When the wheeled robot runs continuously, it will change its obstacle avoidance target in real time 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 building the training set, when choosing to bypass pedestrians, it is necessary to find the pedestrians to be bypassed. For example, to bypass the pedestrian on the left from the left, the wheeled robot first sorts the pedestrians in the nearby range, and finds the pedestrian closest to the left as the bypass target, calculates the corresponding bypass radius, and obtains the initial speed direction. Based on this speed direction as the optimization initial value of the ORCA algorithm, the final real-time speed of the wheeled robot is optimized according to the speed barrier theory.

[0053] The wheeled robot uses the next waypoint instead of the final target point in local navigation. The obstacle image uses local information, and the pedestrian information is relative. These inputs do not depend on the global map. The wheeled robot can learn to reach the next waypoint based on the pedestrians and environment information nearby. Therefore, when the global map changes, the method is still effective as long as the waypoints are selected properly.

[0054] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely examples of the principles and applications of the present invention. It should therefore be understood that many modifications may be made to the exemplary embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in a manner different from that described in the original claims. It should also be understood that features described in conjunction with individual embodiments may be used in 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 positioned, the positioning results of the point cloud map and the visual pose map are fused to complete the global positioning; S2, planning the global path of the wheeled robot; S3, using 3D laser SLAM to obtain a local map centered on the wheeled robot, generating the next path point of the wheeled robot according to the global path, inputting 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, and the obstacle avoidance model outputs the obstacle avoidance action strategy of 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 to obtain a feature vector 3. The feature vector 1, the feature vector 2, and the feature vector 3 are simultaneously input into the strategy network; the strategy network connects the feature vector 1, the feature vector 2, and the feature vector 3 and processes them using a multi-layer perceptron 4, and then inputs the processing result 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 movement strategy, bypassing the pedestrian on the left from the left, bypassing the pedestrian on the left from the right, bypassing the pedestrian on the right from the left, bypassing the pedestrian on the left from the right, and waiting in place strategy.

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 obstacle avoidance action strategy of the wheeled robot is: sort all pedestrians generated by the pedestrian tracking module of the wheeled robot, 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 has not ended and it needs to continue learning. When it 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: The S1 includes: The global positioning map includes a point cloud map and a visual pose map. The 3D laser SLAM uses the Faster-LIO algorithm, which uses a direct method to process the 3D point cloud. The filtering method is used to fuse the processed 3D point cloud and the inertial measurement unit data to obtain the fused point cloud data. The incremental sparse voxel data structure is used to store the fused point cloud data to complete the construction of the point cloud map. Stereoscopic vision SLAM uses the DVDS algorithm, which eliminates interference from dynamic objects through a detection and elimination mechanism, uses discrete Transformer modules to enhance feature aggregation, and introduces a dense bundle adjustment layer to iteratively calculate the posture and inverse depth of the stereo camera to complete the construction of the visual posture 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 according to 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: S2 includes establishing a navigation map. The D*Lite algorithm calculates the path length and path smoothness according to the navigation map, incorporates the path smoothness into the planning cost function, and plans the global path of the wheeled robot in combination with the path length.

7. The wheeled robot navigation method for complex indoor environments according to claim 6, characterized in that: Methods for building navigation maps include: First, compress and project the point cloud map to obtain a two-dimensional grid map; Then, a navigation line is generated according to the starting point and the end point on the two-dimensional grid map, and the contour point set of the basic obstacle is obtained through the navigation line. 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.

8. 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 a complex indoor environment are implemented as claimed in any one of claims 1 to 7.

9. 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 executable 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 as described in any one of claims 1 to 7.

10. 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 described in any one of claims 1 to 7 are implemented.

Images