UGV Rover PI ROS2 6. Auto Navigation
| ||
Before starting navigation, first make sure you have built an environment map named map. If you have not followed the previous tutorial, you need to follow UGV Rover PI ROS2 4. 2D Mapping Based on LiDAR or UGV Rover PI ROS2 5. 3D Mapping Based on Depth Camera to create a map.
After the mapping is completed, then start the navigation, we provide a variety of autonomous navigation modes, you can choose one of the following autonomous navigation modes for robot navigation.
1. AMCL algorithm
Adaptive Monte Carlo Localization (AMCL) is a particle filter-based positioning algorithm in ROS 2 that uses 2D lidar to estimate the position and direction (i.e. posture) of the robot in a given known map. AMCL is mainly used for mobile robot navigation. It matches existing maps with laser sensors (such as lidar) to calculate the robot's position and direction in the map. The core idea is to represent the possible position of the robot through a large number of particles, and gradually update these particles to reduce the uncertainty of the robot's pose.
Advantages of AMCL:
- Adaptive particle number: AMCL will dynamically adjust the number of particles based on the uncertainty of the robot's position.
- Suitable for dynamic environments: While AMCL assumes a static environment, it can handle a small number of dynamic obstacles, such as pedestrians and other moving objects, to a certain extent, which makes it more flexible in practical applications.
- Reliable positioning capability: AMCL's positioning effect in known maps is very reliable. Even if the robot's pose is initially uncertain, it can gradually converge to the correct pose.
AMCL assumes that the map is known and you cannot create the map yourself. It also relies on high-quality static maps that are matched to sensor data. If there is a big difference between the map and the real environment, the positioning effect will be affected. AMCL is often used for autonomous navigation of mobile robots. During the navigation process, the robot can determine its own pose through AMCL and rely on known maps for path planning and obstacle avoidance.
In the container, start navigation based on the AMCL algorithm. After successful startup, you can see the RViz screen of the previously built map:
ros2 launch ugv_nav nav.launch.py use_localization:=amcl use_rviz:=true
Then, you can determine the initial position of the robot based on 6.2 Initialize the robot's position.
2. EMCL algorithm
EMCL is an alternative Monte Carlo localization (MCL) package to AMCL. Unlike AMCL, KLD sampling and adaptive MCL are not implemented. Instead, extended resets and other features are implemented. EMCL does not rely entirely on adaptive particle filtering, but introduces methods such as extended reset to improve positioning performance. EMCL implements the extended reset strategy, a technique for improving the quality of particle sets to better handle uncertainty and drift in positioning.
Start the navigation based on the EMCL algorithm. After successful startup, you can see the RViz screen of the previously built map:
ros2 launch ugv_nav nav.launch.py use_localization:=emcl use_rviz:=true
Then, you can determine the initial position of the robot based on 6.2 Initialize the robot's position.
6.1.1.2 Pure positioning based on Cartographer
Cartographer is an open-source Google system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.
Cartographer system architecture overview: You can see that the optional inputs on the left include depth information, odometer information, IMU data, and fixed Frame attitude.
For more tutorials, please refer to Cartographer official document and project address.
Start pure positioning based on Cartographer. After successful startup, you can see the RViz screen of the previously built map:
Note: The navigation mode based on Cartographer's pure positioning can only be used after using Cartographer to build the map.
ros2 launch ugv_nav nav.launch.py use_localization:=cartographer use_rviz:=true
Then, you can determine the initial position of the robot based on 6.2 Initialize the robot's position.
6.1.1.3 Based on DWA and TEB algorithms
1. DWA algorithm Dynamic Window Approaches (DWA) is a suboptimal method based on predictive control theory, because it can safely and effectively avoid obstacles in an unknown environment, and has the characteristics of small computational effort, rapid response and strong operability. The DWA algorithm is a local path planning algorithm.
The core idea of this algorithm is to determine a sampling speed space that satisfies the mobile robot's hardware constraints in the speed space (v, ω) based on the current position and speed status of the mobile robot, and then calculate the trajectories of the mobile robot within a certain period of time under these speed conditions. trajectory, and evaluate the trajectories through the evaluation function, and finally select the speed corresponding to the trajectory with the best evaluation as the movement speed of the mobile robot. This cycle continues until the mobile robot reaches the target point.
Start the navigation based on the DWA algorithm. After successful startup, you can see the RViz screen of the previously built map:
ros2 launch ugv_nav nav.launch.py use_localplan:=dwa use_rviz:=true
Then, you can determine the initial position of the robot based on 6.2 Initialize the robot's position.
2. TEB algorithm
TEB stands for Time Elastic Band Local Planner. This method performs subsequent corrections on the initial global trajectory generated by the global path planner to optimize the robot's motion trajectory and belongs to local path planning. During the trajectory optimization process, the algorithm has a variety of optimization objectives, including but not limited to: overall path length, trajectory running time, distance from obstacles, passing intermediate way points, and compliance with robot dynamics, kinematics, and geometric constraints.
Start the navigation based on the TEB algorithm. After successful startup, you can see the RViz screen of the previously built map:
ros2 launch ugv_nav nav.launch.py use_localplan:=teb use_rviz:=true
Then, you can determine the initial position of the robot based on 6.2 Initialize the robot's position.
The map navigation mode introduced above is based on the 2D construction of LiDAR. For the 3D map built according to UGV Rover PI ROS2 5. Mapping Based on Depth Camera, please refer to the navigation startup method in this subsection.
Enable nav positioning:
ros2 launch ugv_nav rtabmap_localization_launch.py
You need to wait for the 3D data to be loaded, wait for a period of time, and then you can start navigation as shown in the figure below.
In a new terminal, turn on navigation and choose one of the two navigation modes:
- DWA algorithm
ros2 launch ugv_nav rtabmap_localization_launch.py
- TEB algorithm
ros2 launch ugv_nav nav_rtabmap.launch.py use_localplan:=teb use_rviz:=true
Choose a navigation mode based on the map created above to start the navigation, then proceed with the following content.
6.2 Initialize the robot's position
By default, when navigation is started, the robot initially has no idea where it is and the map waits for you to provide it with an approximate starting location.
First, find the robot's location on the map and check the actual location of your robot. Manually set the robot's initial pose in RViz. Click the 2D Pose Estimate button and indicate the robot's location on the map. The direction of the green arrow is the direction the robot pan-tilt is facing forward.
Keep the operation of the navigation terminal, set the approximate initial pose of the robot, and ensure that the navigation ensures that the actual position of the robot is roughly corresponding to the ground. You can also control the robot through the keyboard on a new terminal to simply move and rotate it to assist in initial positioning.
ros2 run ugv_tools keyboard_ctrl
6.3 Send target pose
Select a target location for the robot on the map. You can use the Nav2 Goal tool to send the target location and direction to the robot. Indicate the location (target point) that the robot wants to navigate to automatically on the RViz map. The direction of the green arrow is the direction the robot pan-tilt is facing forward.
Once the target pose is set, the navigation will find the global path and begin navigating to move the robot to the target pose on the map. Now you can see the robot moving towards the actual target location.
In the lower left corner of the RViz interface, there is a Nav2 RViz2 plug-in [Waypoint/Nav Through Poses Mode], which can switch the navigation mode. Click the [Waypoint/Nav Through Poses Mode] button to switch to the multi-point navigation mode.
Then use Nav2 Goal in the RViz2 toolbar to give multiple target points to move. After setting, click [Start Waypoint Following] in the lower left corner to start path planning navigation. The robot will move according to the order of the selected target points. After reaching the first target point, it will automatically go to the next target point without any further operation. The robot will stop if it reaches the last target point.
6.4 Change map name
According to the UGV Rover PI ROS2 4. 2D Mapping Based on LiDAR tutorial, the default names of the maps we build are map, so the maps called by the above files that start navigation are also map. However, if you change the name of the map when building the map, you need to synchronously change the name of the map called in the startup file before starting navigation.
Open the nav.launch.py script file in the directory /home/ws/ugv_ws/src/ugv_main/ugv_nav/launch. Change map.yaml to the name of the map you saved as shown below. Save and close after making changes.
Then recompile the nav package, enter the product workspace in the Docker container terminal to compile the ugv_nav package:
cd /home/ws/ugv_ws colcon build --packages-select ugv_nav --symlink-install source ~/.bashrc
If you change the name of the saved map when you use Cartographer to create a map, in addition to the above files, you also need to change the bringup_launch_cartographer.launch.py script file in the /home/ws/ugv_ws/src/ugv_main/ugv_nav/launch/nav_bringup/ directory. Change map.pbstream to the name of the map you saved as shown below. Save and close after making changes, then recompile the ugv_nav package again.
