Keywords

1 Introduction

Collision-avoidance path planning has been a major challenge for robotics researchers. Different proposals have been made to address individual, but contrasting, requirements, such as following the shortest or smoothest trajectory or minimizing processing time. Our system has the goal to quickly find a smooth path with a low computational cost without ensuring that it is the shortest trajectory.

RoboCup has reveal a strong and rapid need to develop efficient path planning algorithms for complex environments. It is a global initiative in which researchers around the world present their best developments in the topics of robotics, artificial intelligence and related areas [1]. Based on the RoboCup initiative, every year many tournaments are held in different countries around the world, therefore teams participate in various disciplines [2].

For the case of the Small Size League (SSL) [3], the challenge is a soccer contest in which full autonomous robots are able to cooperate to score and win a match. Its artificial vision system sends field images at a rate of \(60\) fps [4], so path planning and intelligence processing must be made within the span of 16 ms. Thus, the challenge involves a highly dynamic multi-agent environment which implies the need for obstacles avoidance and fast path planning algorithms.

Initially, we present a short review of related work, specifically describing the RRT [5] algorithm, some applications and results that this algorithm generates in situations with several obstacles. After that, we describe in detail the proposed algorithm and we suggest a set of benchmarking scenarios in the context of this league, including real game situations from the RoboCup 2013 competition.

Then, we show the results of our performance analysis for both algorithms over the proposed scenarios. We evaluated specific attributes such as path smoothness, distance traveled and the processing time. Finally, we include conclusions and future work sections.

2 Related Work

The problem of path planning under dynamic environments has been tackled by a variety of researchers. Most of them have focused on optimizing specific performance measures such as obtaining smooth paths in the less amount of time as possible. For example, Tsubouchi et al. analyze the behavior of a single robot within a multi-obstacle dynamic environment [6]. The navigation scheme in this work is based on a heuristic and assumes that obstacles move with a piecewise constant velocity.

On the Small Size League challenge, the robotic players have omnidirectional traction and the dynamic characteristic of the environment have to be carefully taken into account for a proper navigation. Han et al. studied the control of multiple non-holonomic robotic agents in which half the obstacles are non-controllable opponents whose dynamic patterns are unknown [7]. The algorithm in this work creates a set of halfway points between the initial robot position and its final destination. These points are calculated based on the evaluation of potential blockages in the route.

Kuffner et al. presented an algorithm based on Rapidly-exploring Random Trees (RRT) [8]. This algorithm is specifically suited to overcome the constraints that arise in dynamic environments. RRT creates the path that should be followed by an agent from its initial position to a target point by iteratively building search trees that quickly explore the environment. The general procedure is divided in \(5\) main processes as shown in Fig. 1. However, we have found that this approach can still be improved with regards to computational cost and path smoothness, in contexts like the RoboCup SSL environment.

Fig. 1.
figure 1

Diagram RRT

Full size image

The result of applying the RRT algorithm to an environment with a set of obstacles is shown in Fig. 2. Where the green region represents the obstacles, the white region is the obstacle-free configuration space, blue branches are the RRT, and the black line is the solution path between the starting configuration (blue) and the goal configuration (red).

Fig. 2.
figure 2

Example RRT (Color figure online)

Full size image

The RRT algorithm has been widely embraced in a variety of applications in different environments. This has been especially true for the teams that participate in the small size league of the RoboCup initiative. For instance, Bruce et al. showed a RRT-based planning system in simulation and its implementation on actual robots [9]. This algorithm is presented as ERRT and aims at reducing the cost of searching the nearest point to the path that is being build when compared to the original RRT. This feature increases the efficiency of the path planning procedure for real time applications even in dynamic and continuous environments.

Desaraju et al. presented a Decentralized Multi-Agent Rapidly-exploring Random Tree (DMA-RRT) algorithm [10]. This approach allows to perform an efficient planning by considering complex environments. It uses a coordination strategy to dynamically update the order in which the robots carry out their individual planning.

3 Proposal

After having reviewed path planning algorithms for dynamic environments, we have implemented RRT and validated it in simulated game situations. After that, we have measured the time taken by the algorithm to generate paths in complex situations. Finally we have proposed a new planning algorithm that is capable of dramatically reducing the time required to create the path and compared it with the RRT algorithm. Both are validated in real game situations of the RoboCup 2013.

The proposed algorithm is based on generating straight trajectories between an initial state and a goal state. To accomplish this, an initial straight trajectory between these points is defined and checked for collisions against all obstacles. If there is no obstacle, the selected route is returned, as shown in Fig. 3. Otherwise, a subgoal state is generated to avoid the obstacle. As a consequence, the original trajectory is split in two: one between the initial state and the subgoal, and another one between the subgoal and the goal state. Then, these new trajectories are recursively evaluated until the algorithm finds an obstacle-free path. Below we show the developed algorithm:

Pseudocode of the proposed path planning

figure a
Fig. 3.
figure 3

Path without obstacles

Full size image

The function Collides determines if there is any obstacle in the trajectory and if this is the case, the function returns the position of the closest obstacle in the trajectory.

The function SearchPoint assigns a new point (subgoal) at the side of this obstacle. This point is located from the obstacle to a distance equal to the robot diameter and \(90\) or \(-90\) degrees related to the path between the initial point to the obstacle (the sign is a function parameter). See Fig. 4a. In the case the new point collides with an obstacle, the function keeps moving the point one robot diameter in the same direction, until an obstacle free point is found.

Fig. 4.
figure 4

(a) Subgoal selection. (b) New paths created with subgoal

Full size image

The function GenerateSegment generates a straight path between two points. It is used to create two new paths. The first one between the initial state and the subgoal and the second one between the subgoal and the goal state. These new paths will be analyzed recursively as shown in Fig. 4b. Figure 5 shows all the steps of execution until an obstacle free path is found.

Fig. 5.
figure 5

The figures show the recursive steps of the algorithm. At each step, the black line shows the computed straight path and the magenta segments denote the alternative route to avoid the found obstacle. After computing an alternative route, the algorithm is applied over it, recursively (Color figure online).

Full size image

Finally, the returned paths should be joined; this is performed by the function JoinSegments. It returns the path to reach the target point avoiding obstacles. Figure 6 shows an example of a game situation in which the algorithm performed recursion twice.

Fig. 6.
figure 6

Example of the proposed algorithm with recursion

Full size image

As we mentioned before, two possible subgoal points may be returned by the function Collides, one at \(90\) degrees and another at \(-90\) degrees. This means that the obstacles will always be avoided in the same direction, either always in the \(90\) degree direction, or in the other one. In order to optimize the path length, both options are tested and the shortest path is chosen.

The Fig. 7a shows the two possible paths found by the algorithm in a scenario with multiple obstacles. Figure 7b shows a continuous line for the trajectory to be followed.

Fig. 7.
figure 7

(a) Possible paths (b) Path selection

Full size image

Finally, at Fig. 8 we present the solution found by the algorithm in a sample random scenario with multiple obstacles.

Fig. 8.
figure 8

Solution found by the algorithm in a random scenario with multiple obstacles

Full size image

4 Results and Evaluation

The evaluation and comparison of the algorithms involved three different metrics. Namely, processing time required for the path generation, path smoothness and total path length. Additionally, a weighted sum evaluation (Eq. 1) of the trajectories was made according to Xiao’s proposal [11] with some modifications, where \(W_{t}\),\(W_{s}\) and \(W_{d}\) are the weights assigned to each criterion according to the desired relevance of processing time, smoothness and distance traveled respectively.

$$\begin{aligned} Eval(p)=W_{t}time(p)+W_{d}dist(p)+W_{s}smooth(p) \end{aligned}$$
(1)

We define \(time\), \(smooth\) and \(dist\) as:

  • \(time(p)\), processing time

  • \(dist(p)= \sum _{i=1}^{n-1}d(m_{i},m_{i+1})\), total path length, where \(d(m_{i},m_{i+1})\) is the distance between two adjacent nodes \(m_{i}\) and \(m_{i+1}\).

  • \(smooth(p)\) measures the smoothness of the path, defined as

    $$ smooth(p) = \frac{\sum \theta _i}{dist(p)}$$

    where \(\theta _{i}\) is the angle between adjacent segments of the route.

According to Eq. 1, smaller \(Eval(p)\) values represent better performance measures. In addition, for the case of Small Size League, we assign more relevance to processing time \(W_{t}=0.5\), next in significance the smoothness \(W_{s}=0.3\) and finally the path length \(W_{d}=0.2\). As \(time\), \(smooth\) and \(dist\) variables must be normalized, we normalized each variable using the higher value obtained for it from both RRT and the proposed algorithm.

The algorithm evaluation process was performed in two parts. The first one involves static random scenarios and second one includes real game dynamic environments taken from the RoboCup 2013 contest.

4.1 Part 1: Static Scenario

We consider two different game cases and for each one we generate \(100\) paths using the RRT algorithm. From these paths, we take average values of each one of the three criteria and then we compare with the trajectory generated by the proposed algorithm in the same case. We did not run our algorithm several times, since it would generate the same result every time, as it is not randomized.

Case 1. Figure 9a shows the trajectories generated by both algorithms in the shortest time. According to criterion 1 (processing time), the path generated the faster by RRT algorithm took less time than our proposal’s. Figure 9b shows processing time for the \(100\) trajectories generated by RRT (green), their average (blue) and the time spent by the proposed algorithm (red). The obtained results are shown in Table 1.

Fig. 9.
figure 9

(a) Proposal and RRT algorithm (Path that took less time to create). (b) Results Case 1, Criterion 1 (Process time) (Color figure online)

Full size image
Table 1. Case 1 results: processing time analysis
Full size table

Regarding the second criterion (smoothness of the path), Fig. 10 and Table 2 exhibit the results obtained for the same case. Last, in Fig. 11 and Table 3 we show the results according to the third criterion (path length).

Fig. 10.
figure 10

(a) Proposal and RRT algorithm (smoother path). (b) Results Case 1, Criterion 2 (path smoothness)

Full size image
Table 2. Case 1 results: smoothness analysis of path
Full size table
Fig. 11.
figure 11

(a) Proposal and RRT algorithm (shortest path). (b) Results Case 1, Criterion 3 (Path length)

Full size image
Table 3. Case 1 results: path length analysis
Full size table

The performance evaluation of this case is shown in Table 4. The path generated by the proposed algorithm is \(3.4\) times better than the average of those generated by RRT.

Table 4. Case 1 results: weighted performance evaluation
Full size table

Case 2. Figure 12 shows the second case, with (a) the fastest generated path by RRT, (b) the smoothest and (c) the shortest in length. All the results for this case are shown in Table 5.

Fig. 12.
figure 12

Case 2. (a) Fastest generated path. (b) Smoother path. (c) Shortest path

Full size image
Table 5. Case 2 results
Full size table

The evaluation of this case is shown in Table 6. We observe that for this game situation, the proposed algorithm is \(3\) times better than RRT.

Table 6. Case 2 results: evaluation
Full size table

4.2 Part 2: Dynamic Scenario

For this case, we took data from a segment of one game of RoboCup 2013 (STOx’s vs CMDragons). There are \(281\) continuous scenarios in total and for each one we generated trajectories using both RRT and the proposed algorithm. Full data of this match can be obtained from [12] and a video showing the segment can be found in [13].

The obtained results are shown in Fig. 13. This graphic presents (a) processing times for the \(281\) trajectories for both algorithms, (b) the smoothness and (c) distance traveled. Table 7, consolidates results obtained from both algorithms.

Table 7. Dynamic scenario results
Full size table

Finally, Table 8 shows the evaluation of both algorithms for the dynamic scenario and results indicate that the average of the proposed algorithm performance is \(2.4\) times better than RRT’s.

Table 8. Dynamic scenario evaluation
Full size table
Fig. 13.
figure 13

Dynamic Scenario. (a) Time results. (b) Smooth results. (c) Dist results.

Full size image

5 Conclusions

We consider RRT to be a robust and flexible algorithm when applied to path planning. However, we observe that its generality can turn into its own weakness, when aspects like processing time or path smoothness are critical. Thus, we have proposed an ad-hoc heuristic for path planning in non-cluttered dynamic environments. We have tested our approach over a set of artificial and real RoboCup Small Size League situations, and we have found it finds smoother and shorter paths faster in the average case.

It is clear that the good performance of the proposed algorithm is related with the constraints of the environment where we apply it. However, we claim that the characteristics of the SSL league with regards to path planning are similar to many other planning situations, e.g. autonomous car driving.

When testing the proposed heuristic, we have found that it outperforms the RRT implemented version by approximately \(30\,\%\) on average, using the proposed metrics. Additionally, we have also observed that the proposal outperforms RRT in all three individual metrics, i.e. path length, smoothness and processing time.

After competition in RoboCup 2013, we can assert that the proposed algorithm generated paths avoiding obstacles and also processed all necessary data in real time, which was crucial to be among the top eight teams in the SSL.

6 Future Work

We propose to test the new algorithm on more complex environments to assess the validity of our claims regarding the applicability of our heuristic.

According with the outcomes of these tests, we will modify algorithm to make it more robust.

We also acknowledge the need to compare this algorithms with newer and improved versions of RRT-based path planners. Future work will include these comparisons.

Integrating the robots dynamics information into the planning process is also our priority. We believe that this is crucial to close the gap between the planned paths and the actual navigated ones, as well as to reach optimal plans.