Abstract
Path planning and obstacle avoidance are essential for autonomous driving cars. On the base of a self-constructed smart obstacle avoidance car, which used a LeTMC-520 depth camera and Jetson controller, this paper established a map of an unknown indoor environment based on depth information via SLAM technology. The Dijkstra algorithm is used as the global path planning algorithm and the dynamic window approach (DWA) as its local path planning algorithm, which are applied to the smart car, enabling it to successfully avoid obstacles from the planned initial position and reach the designated position. The tests on the smart car prove that the system can complete the functions of environment map establishment, path planning and navigation, and obstacle avoidance.
1. Introduction
In recent years, new industries such as cloud computing, big data, data center, virtual/augmented reality (VR/AR), 5G, artificial intelligence (AI), Internet of Things (IoT), and optical fiber sensing have emerged. These developments have changed our way of life and have simplified the completion of tasks that were difficult in the past [1]. Nowadays, Mobile Robot (MR) is widely used in various fields, such as military, industrial, agricultural, and many other applications [2]. They can replace humans to complete various difficult or dangerous operations in dangerous and special environments such as aviation, underwater, and tunnels. For example, MultiModal Mall Entertainment Robot (MuMMER) [3] can offer the public with an entertaining and engaging experience in an open-air plaza. Pepper [4] is a human-like robot, who can serve as a family companion robot for elder people. Atlas robot [5], developed by Boston Dynamics, can replace people to perform special tasks in both indoor and outdoor environments. Robots are required to work in more and more complex environments nowadays, such as shopping malls, city streets, hospitals, and train stations. Thus, to coexist harmoniously with people and other agents in these highly dynamic environments is one of the key problems. For autonomous driving robots, path planning and obstacle avoidance are the most basic functions. Traditional intelligent obstacle avoidance robots generally use binocular cameras or distance detectors to avoid obstacles. These robots have poor perception of the environment and high dependence on light and can only achieve obstacle avoidance in specific environments. In view of this, the new intelligent obstacle avoidance vehicle uses a depth camera, which combines machine image recognition and machine vision technology. It can fulfill lots of SLAM functions such as indoor mapping, automatic driving, and robotic arm grabbing on the robot operating system (ROS for short) [6]. The smart obstacle avoidance vehicle can be used in safety inspection and evaluation fields, such as road quality inspection, bridge maintenance, industrial pipeline leakage, and location search. It can also replace people to complete logistics and transportation work in dangerous environments that are not suitable for humans to work. It can also perform military tasks such as surveillance, security patrol, pollutant collection, and hazardous material disposal in a more subtle manner.
Path planning is one of the key techniques for guiding the robot in dynamic environments. The goal of the path planning is to control the robot from the starting point to the target point, subject to the constraints such that the robot does not touch any obstacle throughout the process [7]. According to the target range of path planning, it can be divided into local path planning and global path planning [8]. Local path planning refers to the smart car overall unknown or partially unknown environmental data, and the environmental information is collected by the sensors in real time to determine the distribution of local obstacles, and the optimal path from the starting point to the target point is selected, so it is also called dynamic planning. Global path planning is also known as static planning, which refers to the smart car knowing all the geographic data and planning corresponding feasible paths based on this environmental information, also known as static planning.
At present, local path planning mainly includes artificial potential field method and dynamic window approach. The artificial potential field method is an abstract artificial force field, which assumes that a virtual gravitational field is formed between the target point and the car, and the gravitational force is inversely proportional to the distance between the car and the target point; a virtual repulsion field is formed between the obstacle and the car; the repulsive force is inversely proportional to the distance between the car and the obstacle; the smart car is regarded as a particle. The path optimization can be achieved by using the form of gravitation and repulsion functions. The planned path has fast convergence speed, smooth path, good real-time algorithm, and small amount of calculation. However, when facing with a complex dynamic environment, there will be a situation where the combined force is zero and the car stops moving during the movement of the smart car, so the design of the gravitational field is the key to the success of the algorithm. The dynamic window approach is to combine the sampled multiple sets of speed with the motion constraints of speed and acceleration, then evaluate the simulated multiple sets of trajectories through an evaluation function, and select the optimal speed corresponding to the trajectory as the drive of the car. Compared with the artificial potential field method, this approach has the characteristics of fast planning, safe and reliable, and good real-time performance [8–10].
A lot of researches have been done on global path planning, and many efficient algorithms have been proposed. These algorithms can be divided into three categories, Graph Search Algorithms, Random Sampling Algorithms, and Intelligent Bionic Algorithms. The Graph Search Algorithms mainly include the Dijkstra algorithm, A algorithm, BFS algorithm (breadth-first search), and DFS algorithm (depth-first search). The Dijkstra algorithm and A algorithms are widely implemented in the ROS (robot operating system) [11]. Although these methods are improved by reducing the number of searching grids through a heuristic estimation, the planning efficiency is inevitably low when the environment is complex, however. Random Sampling Algorithms include BIT (batch informed trees), RABIT (regionally accelerated batch informed trees), RRT (rapidly exploring random tree), and Risk-DTRRT (risk-based dual-tree rapidly exploring random tree). These algorithms are more efficient and widely used in dynamic high-dimensional environments. Intelligent Bionic Algorithms mainly include GA (genetic algorithm), ACO (ant colony algorithm), ABC (artificial bee colony algorithm), and PSO (particle swarm optimization algorithm). These methods simulate the evolutionary and biomimetic insects’ behaviors. Wang et al. [12] proposed the optimization of the genetic algorithm-particle swarm optimization algorithm (OGA-PSO) to speed up the calculation and solve the shortcoming of locally optimal. Liu et al. [13] raised a combined algorithm with artificial potential field and geometric local optimization method, aiming to search a globally optimal path. Mac et al. [14] optimized constrained multiobjective PSO with an accelerated update methodology, which can shorten and smoothen the optimal global robot path.
This paper designs and implements the autonomous path planning of the smart car. The rest of this paper is organized as follows. In Section 2, we create a two-dimensional map of an unknown indoor environment by using the SLAM method based on depth information to simulate laser scanning data. The Dijkstra global path planning algorithm and DWA (dynamic window algorithm) local path planning algorithm are introduced in Section 3. Then, in Sections 4 and 5, we test our combined algorithm in a simulation environment and on practical self-built smart cars. The results show that the car can successfully avoid obstacles from the planned initial position and reach the designated position. Section 6 comes with our conclusion that both the algorithm and the system are proved to be effective and feasible.
2. Building a Map Based on Depth Information
SLAM technology is a very important part in the realization of intelligent obstacle avoidance vehicle technology. It is a technology for the intelligent vehicle to rely on various sensors to obtain information about the surrounding environment and establish a map of the environment, while using the created environment map to achieve reliable positioning. It mainly solves the problems such as the navigation of the vehicle in an unknown environment. This paper uses the SLAM method based on depth information to simulate laser scanning data to create a two-dimensional map of an unknown indoor environment [15].
First, obtain the color map and depth map of the indoor scene through the depth camera, and establish a geometric model of depth data, and then implement the function of simulating 2D laser scanning according to the geometric model of depth data, and create a 2D map of the surrounding environment.
Figure 1 is a schematic diagram of the geometric model of depth data. The pixel value of each pixel in the depth image of the environment collected by the depth camera has a corresponding depth value. This depth value is the vertical distance depth from the plane of the object to the plane of the depth camera, not the straight-line distance between the camera and the object. Therefore, only through the corresponding geometric transformation can obtain the distance information of the object.
The method of geometric transformation is shown in where the depth value corresponding to the pixel in the th row and th column of the depth image is expressed as and represents the angle in the geometric model coordinates of the coordinate of the pixel in the depth image [9].
The principle of converting depth map to laser data is shown in Figure 2. In the schematic diagram of the depth map to laser data, for any depth image point in the image (which is shown as red block in Figure 2), the steps to convert each pixel data into laser data are as follows: (1)Convert the point of the depth image into a point under the depth camera coordinate system(2)Calculate the angle between the line AO and CO (3) needs to be mapped into the corresponding laser data. Assuming that the minimum range and maximum range of the laser are known, the laser beam has equal divisions, that is, the array can be used to represent the laser data. The index of the point projected into the array laser can be calculated using the following formula:
The value of represents the distance between the point projected on the -axis of the point in the depth camera coordinate system and the optical center of the camera, that is:
3. Obstacle Avoidance Algorithm
Obstacle avoidance algorithm is the key to navigation, while path planning is the basic condition of obstacle avoidance algorithm. According to the target range of path planning, it can be divided into global path planning and local path planning. Global path planning refers to finding a good collision-free path among the known environment maps, and local path planning refers to planning of the intelligent car to realize its own positioning and dynamic obstacle avoidance through sensor information in a partially unknown or completely unknown environment map.
This paper uses the Dijkstra global path planning algorithm and DWA (dynamic window algorithm) local path planning algorithm. Based on the success of global path planning, that is, global path planning first plans a roughly feasible route and then maps the global path to the local map and then uses the DWA algorithm for local path planning according to the problem of unknown obstacles that the car may face. The local path planner selects or truncates the map mapped by the global path planner based on real-time information, then samples the velocity space, scores each generated trajectory to find the optimal trajectory, and then issues velocity instructions for path planning.
3.1. Global Path Planning Algorithm
Global path planning is to make the smart car plan a path smoothly from the starting point (where it is located) to the ending point (target point). In the process of global planning, first of all, the car is required to reach the target point accurately and cannot collide with obstacles in the environment, and secondly, it is necessary to choose the path that can reach the destination as quickly as possible. The global planner designed at this time uses the Dijkstra algorithm. After specifying a starting point and an ending point in the built environment map, the Dijkstra algorithm expands radially from the starting point to the outer layer until the target point and plans the shortest path between these two points.
When using the Dijkstra algorithm to plan the shortest path, it is usually necessary to specify the starting position of the car, then introduce two sets and . Set is used to record the vertices of which the shortest path has not been found, and the distance from the vertex to the starting point [16]. (1)Initially, the starting point in the map is regarded as the set , that is, the set contains only the starting point. The set contains vertices other than the starting point(2)According to the specified starting point, find the distance from other points to the initial point. If the point is adjacent to the starting point, is the edge weight (that is, the length between the two points); if the point is not adjacent to the starting point, is (3)Select the smallest from the set (that is, the vertex with the shortest distance from the starting point), and add this vertex to the set of vertices that have found the shortest path; at the same time, the vertex is removed from the set of points that have never found the shortest path(4)Update the distance from each vertex in the set to the starting point(5)Repeat steps (3) and (4) continuously until all vertices are searched; then, corresponding to the target point is the shortest path length. The flowchart of the Dijkstra algorithm is shown in Figure 3
3.2. Local Path Planning Algorithm
Local path planning is to let the smart vehicle successfully complete “how to reach the destination” during the navigation process. Many uncertain factors may exist in practical car driving, for example, the local actual situation does not match the global map, temporary dynamic obstacles, and so on. It will be very dangerous if navigation was performed according to the global map. Thus, local path planning is necessary to avoid those dynamic or abrupt obstacles. Local path planning is mainly to avoid obstacles for local unknown obstacles and return to the calculated global path after avoiding obstacles. This paper uses the DWA algorithm to solve the local path planning problem.
The DWA is also called the dynamic window method, and its core idea is to transform the path planning problem into a constrained optimization problem on the velocity vector space. Multiple sets of velocities in the velocity space , formed by the linear velocity and the rotational velocity of the car, are sampled, and then, the trajectory is simulated. After obtaining multiple sets of trajectories, these trajectories are evaluated, and the optimal trajectory is selected as the actual trajectory of the car. However, there are certain constraints when sampling in velocity space. In addition, the obstacle cannot be regarded as a particle only, and the situation where the contour of the car may collide with the obstacle needs to be considered. Therefore, the dynamic window approach is added to the calculation of the expansion radius of the obstacle. The expansion radius is equal to the farthest length of the contour particle of the car and the edge of the obstacle outline. The flowchart of the DWA is shown in Figure 4. The steps of the dynamic window method are as follows: (1)Sampling the data from the speed space to get the current state of the car(2)For each sampling speed, calculate the movement trajectory of the car at this speed for a period(3)Use the criteria of the evaluation function to score multiple routes and discard the infeasible routes(4)The optimal path is selected as the actual trajectory of the car according to the scoring situation [17]
Note that the number of speed is infinite theoretically; thus, it is necessary to limit the speed sampling range [18]:
The car’s velocity range in its own maximum and minimum speed
Due to the limited torque of the motor, the performance of the car will be affected by the motor drive module, so there is a maximum acceleration and deceleration limit. Therefore, in the simulation cycle of the car traveling, the speed in the dynamic window is the speed that the car can actually reach:
Among them, and are the current speed of the car, and correspond to the maximum acceleration, and and correspond to the maximum deceleration.
At the same time, in order to allow the car to stop before encountering an obstacle for safety consideration, the speed should have a range with the condition of maximum deceleration: where represents the minimum distance between the car and the obstacle, and and are the translation acceleration and rotation acceleration of the car, respectively. In the sampled speed group, there are several feasible trajectories. Therefore, each trajectory needs to be evaluated in the form of an evaluation function, so as to obtain the optimal value of the car at time in the speed vector space.
The standard objective function of the DWA is
The is a measure of progress towards the goal location. It is maximal if the car moves directly towards the target. When the angle between the car and the target point is 0 degree, the maximum value is
Among them, represents the angle between the car and the target point.
The is the distance to the closest obstacle on the trajectory. is the distance between the car’s own position and the target point at that moment. The larger the value of the function, the farther the distance to the target point, and the less likely to be a collision; the smaller the value of the function, the closer to the obstacle, the greater the probability of a collision, which also means the higher is the car’s desire to move around it.
The is the forward velocity of the car and supports fast movements. where is the average speed of the car and is the maximum speed of the car in the speed space.
4. Navigation and Obstacle Avoidance Simulation
In order to verify the obstacle avoidance performance of the Dijkstra algorithm and DWA, we have established a grid environment in MATLAB, as shown in Figures 5–7, and three groups of simulation experiments are carried out for navigation and avoidance of both static and dynamic obstacles. There are three different situations in each group of simulation, that is, the smart car meets dynamic obstacles in three ways, head-on, crossing, and overtaking.
(a)
(b)
(c)
(a)
(b)
(c)
(a)
(b)
(c)
The parameters in the simulation experiment are as follows: the maximum linear velocity and angular velocity of the smart car are set as 3.5 m/s and 40 rad/s; the maximum linear acceleration and angular acceleration are 0.35 m/s2 and 60 rad/s2; the linear velocity resolution is 0.01 m/s, and the angular velocity resolution is 1 rad/s. The three weighted coefficients , , and of the evaluation function are set as 0.05, 0.2, and 0.1. The starting position is indicated by a green dot, and its coordinates are (3,14); the target position is indicated by a red dot, and its coordinates are (15,8); the radius of dynamic obstacles is 0.3 m; the blue curves are the optimal paths obtained by the Dijkstra algorithm and DWA.
The simulation results of head-on situation are shown in Figure 5. The dynamic obstacle moves about 55° up to the left at a speed of 0.35 m/s from the initial position (6,12) and head-on to the car. When meeting the dynamic obstacle, as shown in Figures 5(a) and 5(b), the car will actively detour under the dynamic obstacle along the optimal trajectory evaluated by the DWA to avoid it. Then, the car continues to move towards the target position along the optimal trajectory. Finally, we can see from Figure 5(c) that the car reaches the target position safely and plans a smooth path.
The simulation results of crossing situation are shown in Figure 6; the dynamic obstacle moves horizontally to the left at a same speed from the initial position (7.5,12) and crosses the car. At this moment, the car will still actively detour under the dynamic obstacle along the optimal trajectory to avoid it as shown in Figures 6(a) and 6(b). Then, the car continues to move towards the target position along the optimal trajectory. Finally, we can also see from Figure 6(c) that the car reaches the target position safely and plans a smooth path, too.
The simulation results of overtaking situation are shown in Figure 7. The dynamic obstacle moves about 55° down to the right at a speed of 0.15 m/s from the initial position (4,13). When meeting the slow-moving dynamic obstacle, as shown in Figures 7(a) and 7(b), the car will actively detour to the left and surmount it to avoid it. Finally, we can still see from Figure 7(c) that the car reaches the target position safely and plans a smooth path.
We can find from the above three simulation results that the smart car can avoid dynamic obstacles well considering head-on, crossing, or overtaking situations, and the smooth final path the car planned still meets the global optimal performance.
5. Navigation and Obstacle Avoidance Tests
A self-built smart car was constructed as shown in Figure 8, mainly using Jetson Nano as the main controller, a motor with a reduction ratio of 30, a LeTMC-520 depth camera, and so on. Before the test starts, the angular velocity, linear velocity, and IMU of the car should be calibrated, and the PID parameters of the camera module and motor control module should be debugged. Various tests such as navigation and obstacle avoidance can be conducted only after they are confirmed to be correct and feasible. The working principle diagram of the car is shown in Figure 9.
5.1. Build Environment Map
When the depth camera and Rviz visual control platform are working, the grid map can be utilized and the keyboard can be used to control the car and collect environmental information. When using SLAM to model a new environment, it is necessary to manually control the rotation and forward speed of the intelligent car at medium level , so as to ensure the accuracy of created map.After collecting all the environmental information, save the established map in time to facilitate future experiments. The , , , , , and keys in the keyboard correspond to the left, straight, right, left turn, backward, and right turn of the control car, respectively.
Figure 10 is a map image collected when the car is just in its original position, showing the orientation of the car and the range of the environment currently being collected. Figure 11 is a built environment map of barrier-free information.
5.2. Obstacle Avoidance Tests
All the obstacle avoidance studies of this paper are based on the environment map. First, specify a starting position in the map, and then, use the global path planning and local path planning to make the car bypass the obstacle to reach the specified target position.
The test of the obstacle avoidance algorithm is divided into three situations, from simple to complex. To detect whether the obstacle avoidance algorithm is suitable for a variety of occasions, here are some representative detections as follows: (1)In the simple obstacle environment, only one book is placed on the road as an obstacle. The test shows that the car can easily bypass obstacles from the starting position to the specified target position(2)In a complicated environment, a book and two milk tanks are placed on the road, and which are placed in the road that are relatively close to each other enough for cars, to avoid obstacles to plan. The test also shows that the car can avoid obstacles and pass through the middle of these obstacles(3)In a more complex environment, pianos, TV cabinets, and tea tables are used as borders on the road, and books, storage boxes, trash cans, and stools are used as obstacles. They are staggered to form an S-shaped roadblock. The specific obstacle avoidance test is shown in Figure 12. The test still shows that the car can also successfully avoid obstacles. It is worth mentioning that an appropriate inflation radius needs to be chosen carefully if there are different sizes of obstacles. In this experimental test, the value of the obstacle radius was finally set to 0.3 meters after debugging. If there are obstacles with special shapes and sizes, the obstacle radius value needs to be adjusted accordingly
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)
(j)
(k)
(l)
The test process of the S-shaped roadblock in a complex environment is shown in Figure 12.
6. Conclusion
This paper designs and implements a smart obstacle avoidance car system, including hardware platform construction and software obstacle avoidance algorithm implementation. The main work can be summarized as follows: (1) A smart car was self-designed and self-built with a LeTMC-520 depth camera, a Jetson controller, four motors, and other modules. (2) Based on the ROS, SLAM technology was used to build a new unknown indoor environment map by converting depth map image to laser data; (3) the Dijkstra algorithm and DWA dynamic window method were used as the global path planning algorithm and local path planning algorithm, respectively. The validity and feasibility of the combined algorithm are approved by simulation results. The practical experiments were carried out and showed that the car can successfully avoid obstacles from the planned initial position and reach the designated position in a smooth planned path. The algorithm and the system are both proved to be effective and feasible.
Data Availability
Data are available on request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work is supported by the Initial Scientific Research Fund of FJUT (GY-Z12079 and GY-Z160124), Fujian Provincial Education Department Youth Fund (JAT170367 and JAT170369), Natural Science Foundation of Fujian Province (2019J01773, 2018J01640, and 2017J01728), and China Scholarship Council (201709360002).