Abstract
Wheel-track hybrid mobile robot has been widely used because of its high autonomous mobility and its ability to adapt to complex and changeable outdoor environment. Map construction and path planning are the main conditions to achieve its autonomous mobility. Therefore, a wheel-track hybrid robot path planning has been designed and implemented. Simultaneous Localization and Mapping (SLAM) algorithms are used to process the road images captured by the robot’s camera and other sensors. A rapid raster map with Robot Operating System (ROS) was constructed by rapid recognition of key elements from the images with the help of the Oriented Fast and Rotated Brief (ORB) feature vector. The ant colony algorithm with strong optimization ability, high optimization efficiency, and flexible algorithm is used for path planning, and the problem of unfixed location of outdoor obstacles is solved at the same time. The simulation results show that the global performance and convergence speed of the improved algorithm are increased.
1. Introduction
With the rapid development of computer technology and robot technology, the application of mobile robot has been gradually expanded from the single indoor factory and production workshop to the complex and changeable outdoor environment. At present, the most common moving modes of outdoor mobile robots are wheeled and tracked. Wheeled mobile robots are simple in control and flexible in movement but weak in environmental adaptability. Tracked mobile robots have relatively strong environmental adaptability and good obstacle jumping and climbing performance but slow walking speed and easiness to slide [1, 2]. In order to better adapt to a variety of environments, wheel-track hybrid mobile robots with the advantages of both emerged.
In order to complete the specified task, wheel-track mobile robot needs to move from a starting position to a target position and bypass the existing obstacles, and how to carry out effective map construction and path planning is the key problem to be solved.
In map construction and path planning [3], the robot is completely independent to complete the collection of route images and carry out path planning according to the images. Through the coordination of lidar and RGB-D camera, Simultaneous Localization and Mapping (SLAM) system can be constructed to generate environmental map for path planning.
Traditional path planning algorithms rely on the presentation of obstacles in the environment; that is, obstacles in the environment need to be described. For example, a Bug algorithm needs to perform “boundary tracking” operations on obstacles encountered during operation. Raster-based algorithms require the construction of an environment raster map. The vector field histogram algorithm needs to construct the polar coordinate system according to the density of obstacles. Although various algorithms have slightly different ways of expressing the environment, they all depend on the environment, which makes the traditional algorithm greatly constrained by the environment. For example, the Bug algorithm only works in two-dimensional environments, and when the dimensions of the environment increase, the Bug algorithm will not be able to perform “boundary tracking operations.” The structure of traditional algorithms is simple, which makes the calculation speed of most algorithms faster. In general, the simpler the calculation method, the faster the convergence rate. Both raster method and Voronoi method are graphic-based algorithms. These algorithms often need to build path graph, which occupies a certain amount of computing resources. Therefore, compared with artificial potential field method, Bug algorithm, and vector field histogram method, graphic-based algorithm has slower convergence. Although the convergence speed of grid method and Voronoi method is slower, their path optimization ability is superior, the path is often lower cost, and the ability to adapt to the environment has been improved, with stronger robustness. The higher the environment dimension is, the more obvious this feature is. Algorithms with simple structure have poor adaptability to environment. Artificial potential field method in special environment (e.g., gravity and repulsion cancel each other), Bug algorithm, and vector field histogram algorithm in complex environment (e.g., maze) are prone to fall into local minimum, which leads to planning failure. Although the graph-based algorithm sacrifices the planning efficiency, its relatively complex structure improves its performance, solves the local minimum problem to some extent, and improves its adaptability to the environment to some extent.
Compared with the traditional algorithm, the structure of intelligent algorithm is more complex, and the intelligent algorithm has strong learning ability, which makes it very adaptable to the environment, reduces the constraints of the environment on the algorithm, and avoids the local minimum problem to a great extent. Ant colony algorithm, genetic algorithm, and neural network algorithm have many parameters and often there is a need to adjust parameters to improve the performance of the algorithm. The ant colony relies on the change of pheromone concentration to realize the gradual optimization of the path. When the initial solution is suboptimal, the ant colony algorithm is prone to local optimization. Genetic algorithm overcomes this shortcoming by means of crossover and mutation operation. However, the crossover and mutation of genetic algorithm need a lot of calculation, so the convergence speed of the algorithm is reduced while the optimization performance is obtained. The structure of neural network algorithm is more complex, and the algorithm incrementally optimizes model parameters by simulating human brain mechanism [4].
At present, many scholars use neural network, artificial field of view method, genetic algorithm, and other methods to carry out path planning for robots, but there are some problems such as low efficiency to a certain extent, and the algorithm is too complex, which leads to the increase of the complexity of path planning or inaccurate planning.
In this paper, an improved ant colony algorithm based on raster map is applied to the path planning of outdoor flat ground for wheel-track robot. The improved ORB-SLAM algorithm was adopted to quickly construct raster map for the road images captured by the robot's camera and 360-degree omnidirectional scanning lidar, and the flexible ant colony algorithm is used to realize the fast path planning of the robot.
2. Introduction to Map Generation Algorithm
The first step in the path planning of the robot is to obtain the location information of its own environment through sensors (cameras, radars, etc.), which requires the use of captured pictures to construct a map. The map generation algorithm—ORB-SLAM algorithm, is explained below.
2.1. ORB-SLAM Algorithm
2.1.1. SLAM Algorithm
SLAM is the abbreviation of Simultaneous Localization and Mapping. SLAM is the core technology of 3D vision, which is widely used in frontier and popular fields such as AR, autonomous driving, intelligent robots, and drones [5]. It can be said that any agent with certain action ability has some form of SLAM system. When a mobile device (such as a robot) starts from an unknown location in an unknown environment, it needs to observe and locate its position, attitude, and motion trajectory through sensors in real time during the movement process and then build an incremental map based on its own position, so as to achieve the purpose of simultaneous positioning and map construction. Positioning and mapping are two complementary processes. Maps can provide better positioning and positioning can further expand the map. SLAM places great emphasis on the unknown environment and is the core technology of autonomous mobile robots [6].
2.1.2. ORB Algorithm
ORB, short for Oriented Fast and Rotated Brief, can be used to quickly create feature vectors for key points in an image, and these feature vectors can be used to identify objects in an image. Among them, fast and brief are the feature detection algorithm and the vector creation algorithm, respectively. ORB first looks for special regions in the image, called keypoints. Keypoints are small prominent regions in an image, such as corners, which have a sharp change in pixel value from light to dark. The ORB then computes the corresponding feature vector for each keypoint. The feature vectors created by the ORB algorithm contain only 1 and 0, which is called binary feature vectors. The order of 1 and 0 changes based on a particular keypoint and the pixel area around it. This vector represents intensity patterns around keypoints, so multiple feature vectors can be used to identify larger areas or even specific objects in an image.
ORB is superfast and somewhat immune to noise points and image transformations such as rotation and scaling.
2.1.3. ORB-SLAM Algorithm
The ORB-SLAM algorithm is based on the SLAM algorithm. Feature points (key points) are extracted from the collected images by using the scale invariance, rotation invariance, illumination invariance, and noise invariance properties of ORB technology. The algorithm can locate and map the robot's working environment and has the advantage of being fast [7].
ORB-SLAM is a feature point-based real-time monocular SLAM system that can operate in large-scale, small-scale, indoor, and outdoor environments [8]. The system is robust to vigorous motion and supports wide-baseline closed-loop detection and relocalization, including fully automatic initialization. The system includes modules common to all SLAM systems: Tracking, Mapping, Relocalization, and Loop Closing. Since the ORB-SLAM system is a SLAM system based on feature points, it can calculate the trajectory of the camera in real time and generate sparse 3D reconstruction results of the scene. Based on ORB-SLAM, ORB-SLAM2 also supports calibrated binocular cameras and RGB-D cameras.
ORB-SLAM is mainly divided into three threads, namely, Tracking, Local Mapping, and Loop Closing.
(1) Tracking. The main work of this part is to extract ORB features from the image, perform pose estimation based on the previous frame, or initialize the pose through global relocation and then track the reconstructed local map, optimize the pose, and then determine new key frame according to some rules.
(2) Mapping. This part mainly completes the local map construction. It includes inserting keyframes, verifying and filtering the recently generated map points, then generating new map points, using local bundle adjustment (local BA), and finally filtering the inserted keyframes to remove redundant keyframes.
(3) Loop Closing. This part is mainly divided into two processes, namely, closed-loop detection and closed-loop correction. The closed-loop detection first uses WOB for detection and then calculates the similarity transformation through the Sim3 algorithm. Closed-loop correction mainly refers to closed-loop fusion and graph optimization of Essential Graph.
3. Model Construction
3.1. Initialization of the ORB-SLAM Algorithm
The ORB-SLAM algorithm uses the automatic initialization method [9]. Two models are calculated simultaneously: the homography matrix H for the planar scene and the fundamental matrix F for the nonplanar scene, and then a scoring rule is used to select the appropriate model to restore the camera's rotation matrix R and translation vector t.
3.1.1. Find the Initial Corresponding Point
The ORB feature points are extracted from the acquired current frame Fc and matched with the reference frame Fr. If the number of matching points is too small, the reference frame is reset. This step is included in the Tracking::Monocular Initialization function in Tracking.cc.
3.1.2. Simultaneously Calculate the Two Models
After finding the corresponding point, the Initializer::Initialized function in Initializer.cc is called for initialization. In order to select the most suitable model for 2D and 3D scenes, ORB-SLAM starts two threads to calculate R and t at the same time and calculates the homography matrix Hcr and the fundamental matrix Fcr, respectively.
Here, and , respectively, satisfy the following relations:
The thread calculates the homography matrix by calling the Initializer::FindHomography function, using the normalized direct linear transformation (normalized DLT). The thread F calls the Initializer::FindFundamental function to calculate the fundamental matrix , using the normalized 8-point method. In order to evaluate which model is more suitable, we use and to calculate the respective scores. The calculation method is as follows, where uniformly represents and :where and represent symmetrical conversion errors, which are the conversion error from the current frame to the reference frame and the conversion error from the reference frame to the current frame, respectively. Here,
3.1.3. Model Selection
We believe that when the scene is a plane or approximately plane, the homography matrix is used, and when the fundamental matrix is used to restore motion, the scene needs to be a nonplanar scene with large parallax, so now we use the evaluation mechanism to estimate the advantages and disadvantages of the two models:
When is greater than a certain value, the motion is restored from the homography transformation matrix. Once the model is selected, you can resume running.
3.1.4. Motion Restore
(1) Restore from the Homography Transformation Matrix. After obtaining the homography change , this paper uses the method of FAUGERAS [10] to extract 8 motion hypotheses. This method tests the selection of reasonable solutions by visualizing constraints. But if, at low parallax, the point cloud would run in front or behind the camera, the test would fail and choose a wrong solution. In this paper, 8 schemes of direct triangulation are used to check whether most of the cloud points can be seen in the case of low parallax of the view when there is less reprojection error in front of the two cameras. If none of the solutions are suitable, no initialization is performed and the first step is started again. This method is more robust to the initialization procedure in the case of low parallax and two intersecting views. The program calls the Initializer::ReconstructH function to resume motion.
(2) Restore from the Fundamental Matrix. When the fundamental matrix is obtained and the camera internal parameter K is always obtained, the essential matrix can be calculated, and then the solution of the four motion hypotheses can be recovered using the method in [11]. The formula for obtaining the essential matrix from the fundamental matrix is as follows:
Similarly, only one of these 4 solutions is reasonable and can be selected using visualization constraints. This paper uses the same method as the homography matrix for sfm, that is, triangulates all 4 solutions, and then selects the most suitable one. The Initializer::ReconstructF function is used here.
3.1.5. Cluster Adjustment
Finally, using global adjustment will initialize all the points collected on a plane, so as to get a preliminary constructed map. This part is in the CreateInitialMapMonocular() function in Tracking.cc, using the following statement: Optimizer:GlobalBundleAdjustment (mpMap, 20).
As shown in Figure 1, PTAM and LSD_SLAM in this dataset will initialize all points on a plane, while ORB-SLAM will wait until there is enough parallax before using the fundamental matrix to get the most correct initialization. Since ORB-SLAM has high requirements on initialization, a feature-rich scene can be selected during initialization, and the moving camera can provide it with sufficient parallax. In addition, since the coordinate system is attached to the position of the successful initialization frame, each initialization cannot be guaranteed to be in the same position.
(a)
(b)
(c)
(d)
(e)
(f)
After ORB-SLAM algorithm is used, path planning is further studied by raster map construction algorithm.
3.2. Raster Map Construction Algorithm
Raster images refer to images that have been discretized in space and brightness. We can consider a raster image as a matrix. Any element in the matrix corresponds to a point in the image, and the corresponding value corresponds to the gray level of the point. The elements in the digital matrix are called pixels.
Raster map is a map divided into rasters. Among them, some obstacles are occupied, and robot navigation needs to avoid these rasters, that is, to avoid these areas. Each raster is given a possible value, indicating the probability that the raster is occupied. Roster map navigation has the advantages of simple construction, unique location representation, and convenient short-path planning.
A raster map divides an environment into separate rasters of equal size, with each raster representing the area contained in the environment. as shown in Figure 2.
In a raster map, it is assumed that each independent raster has two states: idle and occupied. Use P(S) to represent the probability that each raster is occupied by obstacles, P(S = 1) to represent the occupied state, and P(S = 0) to represent the idle state, and the sum of the idle and occupied probabilities is 1. Assuming that the occupancy probabilities of each raster do not interfere with each other, the global occupancy probability is the product of the occupancy probabilities of all rasters.
Then the occupancy probability for any independent raster can be obtained by the following formula:where x1:t represents the state sequence of the robot and z1:t represents the sensor observation sequence of the robot.
Suppose that if there is no observed value zt, then the robot state xt at time t does not contain independent regional information in the raster map:
Then the idle probability of the raster is
On the basis of the existing raster map probability, at a certain time t, the raster probability is updated based on the robot pose and observation value.
In the two-dimensional raster map, assuming that Xr = (x, y) is the coordinate under the world coordinate system OwXwYw and the edge length of the raster is r, the resolution of the raster map is 1/r; that is, the coordinate value of the coordinate point in the raster map iswhere x/r and y/r are rounded up, respectively.
Through the above raster map conversion formula, the coordinates of the obstacles detected by the laser points of the radar in the raster map can be obtained. If the position of the robot is at this time, the angle between the sampling point and the robot is , and the distance between the obstacle and the sampling point is ; then the coordinates of the obstacle in the environment in reality are
Thus, the coordinates of the robot and the obstacle can be calculated:
At this time, the Bresenham algorithm can be used to obtain the set of passing idle rasters, as shown in Figure 3.
Using the raster map update algorithm, the raster maps of the initial moment and the moment after the exercise are combined to realize the update of the raster map and obtain the updated global raster map.
3.3. Path-Planning-Ant Colony Algorithm
As a key issue in the autonomous motion of mobile robots, path planning plays a very important role in the research on related technologies of mobile robots. Path planning is to avoid obstacles in the environment in the process of moving to the target point and plan an optimal or suboptimal path based on indicators such as distance, time, and energy consumption. Ant colony optimization (ACO) was originally proposed by Italian scholar Dorigo [12] after analyzing and simulating the foraging behavior of ant colonies in nature.
The study found that ants can leave a substance called pheromone on the path they pass during the foraging process, and ants can sense the existence and intensity of this substance during the foraging process and guide themselves accordingly. They tend to move in the direction of higher intensity of the material [13]. Therefore, the collective foraging behavior composed of a large number of ants shows a positive information feedback phenomenon: the shorter a path is, the more ants walk along the path, the higher the pheromone intensity left behind is, and the higher the probability of latecomers choosing this path is [14]. It is through this information exchange that individual ants choose the shortest path and reach the purpose of searching for food. Ant colony optimization is an optimization algorithm that simulates the foraging behavior of ant colony.
In the path planning of the robot, the traditional ant colony algorithm is used to search for the optimal path, which often has problems such as low efficiency and poor convergence. This paper proposes improvement measures on the basis of the traditional method.
4. Algorithm Improvement and Simulation Verification
4.1. Preprocessing of Raster Map
In the above paper, the raster map is obtained by modeling the map environment. In order to prevent the ant colony from entering the blind area of the environment, the method of concentrated thinking is adopted, and the connecting area is regarded as an obstacle, and if it encounters a concave area, it is automatically filled. The processing diagrams before and after raster map optimization are shown in Figure 4.
(a)
(b)
4.2. Extraction of Advantage Points
After the preprocessing of the raster, during the path search, the points of the obstacles passed in each path planning are defined as the dominant path points, and the dominant path points are extracted by the geometry-based template matching method. The identified path points are compared with the template that realizes the construction, and the dominant path points are obtained, as shown in Figure 5.
The extraction method of the dominant path points in this paper is carried out by using the template of geometric features. In this method, the extracted path points to be identified are compared with the template data to determine the dominant path points. When the area within the range of the raster obstacle is a free area, the convex range of the dominant path can be determined, as shown in Figure 6.
4.3. Searching Paths
After completing the preprocessing and extracting the dominant points in the raster, the algorithm starts to search for the path, and the raster to be reached in the next step can be obtained according to the following formula:
After calculation, it can be judged whether the target point is reached in the specified number of steps according to the previous template configuration; if so, record the path and length; if not, exclude this path.
4.4. Target Point Judgment
Whether the number of steps taken by the ant to reach the target point is consistent with the specified number of steps is judged. If it is consistent, the path and length of the ant will be recorded. Otherwise, the ant will be removed from the ant sequence.
Pheromone concentration is the key to ant colony cooperation [15]. Initially, the global pheromone concentrations are all equal. With the continuous exploration and walking, the ants will leave pheromone related to the path length on the route they traveled, guiding the subsequent ants to choose the dominant path. Therefore, after all ants complete one cycle, the pheromone content on each path is updated.
The pheromone update not only improves the convergence speed of the algorithm in the path construction but also ensures the continuous exploration of the current optimal solution by the algorithm in the update method. The update of pheromone concentration is only carried out on the optimal path, regardless of whether it is the operation of sending or incrementing the pheromone. After the ants have constructed the path, they begin to update the pheromone.
In the outdoor environment, obstacles often change. In order to solve this problem, the global path planning and local path planning are combined. Firstly, the global raster map of outdoor environment is established similar to the feature point map of environment. Then, ant colony algorithm is used for path planning, and artificial potential field method is used to avoid obstacles with changing outdoor environment. The specific algorithm steps are as follows: Step 1: create an environmental raster map for the outdoor environment. In the process of environment modeling, the driving path of the robot is S-shaped. When the distance of the obstacle d < 50 cm, the robot will drive in the opposite direction of the obstacle according to the robot posture until d > 50 cm. Step 2: according to the established raster map, global path planning is carried out, and an optimal path from the starting point to the destination is planned by the improved ant colony algorithm. Step 3: for obstacles in the moving path of the robot or obstacles within the perception range of the robot, the artificial potential field method is used to avoid obstacles.
The path planning and obstacle avoidance experiment of the wheel-track hybrid mobile robot uses the improved ant colony algorithm for global path planning and uses the global environment map established in Section 2.2. For obstacles that are not in the global map, the artificial potential field method is used for local obstacle avoidance within a certain distance. In the simulation experiment, the starting position and the end position are firstly set, the entire moving trajectory of the robot is observed through the host computer, and the obstacles are added in the experimental scene to detect obstacle avoidance.
The ant colony algorithm has the same defects as genetic algorithm, such as slow convergence speed and easiness to fall into local minimum value. Therefore, in order to improve the global search ability of the ant colony algorithm and improve its search speed, we will make the following improvements to the ant colony algorithm:(1)Keep the optimal solution At the end of each loop, find the optimal solution and keep it.(2)Adaptively change the value of
When the scale of the problem is relatively large, due to the existence of the volatility coefficient of the amount of information, the amount of information on the solutions that have never been searched will be reduced to be close to 0, which reduces the global search ability of the algorithm, and when is too large, with the increase of the information of solutions, the possibility of the previously searched solutions being selected is too large, which will also affect the global search ability of the algorithm. Although the global search ability of the algorithm can be improved by reducing , it will reduce the convergence speed of the algorithm; therefore, in this paper, we will adaptively change the value of .
The algorithm steps of this strategy are as follows.
|
4.5. Simulation Verification
The parameters that need to be set in the ant system are , , and the number of ants. The size of indicates the importance of the amount of information on each route. The larger the value is, the more likely the ant will choose the route previously selected. If the value of is too large, the search will fall into the local minimum point prematurely. The size of indicates the importance of heuristic information.
The greater the number of ants, the stronger the global search ability of the algorithm. But the increase of the number of ants will slow down the convergence speed of the algorithm, and, under the same number of ants, with the increase of the problem scale, the global search ability of the algorithm decreases. The number of ants is the same as the number of network nodes. Tables 1 and 2 show the simulation results for the Oliver 30 problem in this paper.
It can be seen from the simulation results in Tables 1 and 2 that the worst results obtained by the improved ant colony algorithm under different parameters are smaller than the optimal results of the basic ant colony algorithm. The required evolutionary generation is also shortened from more than 300 generations of the basic ant colony algorithm to less than 200 generations. For comparison, this paper lists a summary table of mainstream path planning algorithms, as shown in Table 3 [16].
Table 3 shows that different algorithms have different advantages and disadvantages and different application scenarios. The proposed algorithm is based on ant colony optimization + grid map + SLAM-modified, which combines traditional programming algorithm with intelligent programming algorithm. The algorithm in this paper has the following advantages:(1)Rapid raster map construction with Robot Operating System (ROS) was achieved with rapid recognition of key elements in images by feature vectors of ORB (Oriented Fast and Rotated Brief).(2)By using ants to move to the place with high pheromone, the robustness is high.
Therefore, the globality and convergence speed of the algorithm in this paper are improved, and it is an effective improved algorithm.
5. Summary and Outlook
The path planning of the robot plays a particularly important role in practical application. If there is a large deviation in the path planning, the robot cannot reach the predetermined goal and complete the corresponding task. In this paper, the improved SLAM algorithm is used to generate the map from the image collected by robot. Combined with the quick calculation of ORB operator and the construction of raster map, the path planning is carried out by using ant colony algorithm with strong optimization ability, high optimization efficiency, and flexible algorithm. At the same time, in order to solve the problem that the location of outdoor obstacles is not fixed, this paper integrates local path planning with global path planning, uses local artificial potential field method to avoid obstacles within the range, and uses improved ant colony algorithm to carry out global path planning. Compared with traditional path planning, the efficiency and accuracy of this algorithm are significantly improved. Compared with other intelligent algorithms such as neural network, the training and model of the proposed algorithm are simple, but the performance of the algorithm is very efficient.
Due to limited research time and research energy, the detection of complex obstacles in the environment and the construction of environmental maps have not been studied in depth. This paper only studies the path planning problem in the two-dimensional space, and the path planning problem for the robot in the three-dimensional space needs further research. Although this paper proposes an implementation scheme for robot mobile obstacle avoidance and path planning and conducts simulation verification experiments on the algorithm, due to limited time, the actual environment verification is not carried out. In the future, the content studied in this paper will be implemented on the robot to verify the reliability of the algorithm in the actual environment.
Data Availability
The data used to support the findings of this study are included within the article.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This project was supported by the Gansu Youth Science and Technology Fund Program (20JR5RA048) and the Innovation Ability Improvement Project of Colleges and Universities in Gansu Province (2019A-200).