3K Method: Time-optimal Path Planning for Field Robot

Nowadays, the usage of robots in production services is constantly increasing. This trend extends even to domestic applications, such as vacuum cleaners and lawnmowers. The expanding range of fields where autonomous robots can be employed presents us with new challenges. Widely known are autonomous cars, which are specifically adapted to road environments. However, off-road areas are much more prevalent on Earth. Tasks such as rescue actions, emergency transportation, patrol, and exploration in natural disaster areas often occur in regions with limited or nonexistent transportation infrastructure. Therefore, robust navigation methods that consider travel time constraints are crucial for further development in these types of terrains. Path planning and obstacle avoidance methods are often based on graph algorithms such as A* [22, 1], Dijkstra [14], or D* [20]. The path obtained by those methods has to be smoothed before applying it to a controller [5]. Another approach is to use artificial neural networks (ANN) [3, 19, 21, 25]. An alternative way is to use optimization methods. Biology-inspired genetic algorithms [10, 7, 9, 14], ant colony optimization [22], particle swarm optimization [24], chicken swarm optimization [6], cuckoo search optimization [2], grey wolf algorithm [16], whale optimization algorithm [17]. Other noteworthy approaches are potential fields [11] and fuzzy-based potential fields [4]. An interesting field of path planning are methods based on the Dubins path. Worth mentioning are especially methods using the coverage path planning approach, which can be used for both a priori known [12] and unknown [13] environments.

When it comes to avoiding an obstacle, the solution is trivial. The robot can ride on its left or right side. The problem becomes more complex when there are multiple obstacles. But if we want to go from point A to point B as fast as possible, the task becomes very complex. When a robot knows only about obstacles in its surroundings it does not have enough knowledge to choose the best path in the general context. On the other hand, if it follows a path, that was found using only previously mapped obstacles, it cannot omit a new object in the working area. The 3K method combines two strategies: it finds a global path using a priori known obstacles from maps and optimizes it under the travel time criterion; then, during the ride, it uses the global path as a set of way-points and corrects the trajectory by finding a local obstacle-free path. These local obstacles are detected autonomously by the robot. The local path is also selected under the shortest travel time condition.

The characteristic features of the 3K method in comparison to the other ones are obstacles modeled as circles, division of the problem to the local and the global scales and introduction of the velocity reduction area around obstacles for more precise navigation in the immediate vicinity of obstacles, especially those not known a priori, for safety reasons.

The main algorithm is presented in Fig. 1. The current robot position is considered a start position, while the ordered travel point is considered a destination point. At first, the map of a priori known obstacles is loaded. Then the global path is found, and the travel begins. The robot moves along the sequence of the global path waypoints. If at least one obstacle is detected, the local path between the current robot position and one of the more distant global waypoints is determined, and the robot starts following the local path. The local path computation is repeated as long as at least one local obstacle is in the robot’s view range. When the robot has avoided all the obstacles, it continues following the global waypoints. The 3K method ends when the last global way-point, the destination point, is reached.

2.3.

Collision detection

A collision between the B-spline and a circle can be detected by computing a set of equidistant curve points S and testing, if any of them lies inside the currently considered circle (Fig. 3). In the case of obstacle avoidance, however, where an optimization algorithm is used, the binary information is not enough. A smoother solution is required. Hence, the collision depth is adopted instead. It is computed as follows. Firstly, the closest point p_c of the S set to the obstacle center O_C is selected. Then the collision depth is the difference between the obstacle radius O_r and the distance between the obstacle center O_C and the p_c point, divided by the radius O_r. Then the collision depth value is always in the range [0; 1]. It is shown in Fig. 4 and given by the formula (1). (1) mk=Oγ−OCpCoγ,OCpC≤Or,Or≠0 [{m_k} = {{{O_\gamma } - \left| {{O_C}{p_C}} \right|} \over {{o_\gamma }}},\,\left| {{O_C}{p_C}} \right| \le {O_r},{O_r} \ne 0

Fig. 1.

Flowchart of 3K method

Fig. 2.

Example of map model

Fig. 3.

Example of collision detection between B-spline and circle-shaped obstacles

Fig. 4.

Visualization of the collision depth

To find a path of the shortest travel time, it is necessary to know the path travel time itself. To estimate it, the velocity profile of a path is needed. The travel, however, has to fulfill certain constraints: −

the robot must not exceed the given maximum velocity,

The acceleration and deceleration rate in the 3K method are considered as constant in time. The difference between the constant rates and the real ones is negligible because of many other factors present in a real environment, which can greatly affect the robot during the journey.

The computation of the velocity profile consists of the following steps:

computation of the maximum permissible velocities on all the segments of the path (between waypoints) – the maximum velocities profile,

The main factor limiting the robot’s velocity is the friction between the robot wheels and the ground. The speed must be low enough to prevent the robot from getting out of a track due to the centrifugal force. The formula for the maximum velocity v_max on the particular path segment (4) is derived from the formulae for the friction T (2) and the centrifugal F_c (3) forces. The maximum velocity is a velocity of the state where both forces are equal, i.e. T = F_c. (2) T=FC.T=μN=μmg [\matrix{ {T = {F_C}.} \hfill \cr {T = \mu N = \mu mg} \hfill \cr } (3) Fc=mv2r [{F_c} = {{m{v^2}} \over r} (4) v max =μqr [{v_{\;\max \;}} = \sqrt {\mu qr} where m is the mass of the robot [kg], g is standard gravity acceleration [m/s²], v - velocity [m/s], r - radius of the path segment [m]. In the case of a straight segment the maximum velocity is infinite, which is in fact the maximum velocity allowed for the robot to reach.

In the velocity reduction steps, the new velocity for the i + 1-th waypoint is computed from the formula (5). The start and the end point velocities are set at the beginning of the algorithm and are equal to 0. They remain intact during the whole process. (5) vi+1=2adi+vi2,i∈[0,n−2],a≤0 [{v_{i + 1}} = \sqrt {2a{d_i} + v_i^2} ,i \in [0,n - 2],\,\,a \le 0 where a - a set acceleration or deceleration [m/s²], d_i - Euclidean distance between i-th and i + 1-th waypoint, n - number of way-points. This formula is derived from the equation (6) [8]. (6) v2=v02+2ax−x0 [{v^2} = v_0^2 + 2a\left( {x - {x_0}} \right)

The acceleration and deceleration steps are computed in the same manner, although as the deceleration is a reverse operation to the acceleration, the reduction process has to proceed from the last waypoint to the first one.

The partial travel times, which mean times between each pair of the following waypoints, is computed by formula (8) which is derived from (7) [8]. (7) x−x0=12v0+vt [\left( {x - {x_0}} \right) = {1 \over 2}\left( {{v_0} + v} \right)t (8) ti=2divi+vi+1,i∈0,n−2 [{t_i} = {{2{d_i}} \over {{v_i} + {v_{i + 1}}}},i \in \left[ {0,\,\,n - 2} \right] where in (7): x - current distance, x₀ - initial distance, v₀ - initial velocity, v - current velocity, t - travel time; in (8): t_i - travel time between i-th and i + 1-th waypoints, d_i - distance between i-th and i + 1-th waypoints, v_i velocity at i-th waypoint, v_{i + 1} velocity at i + 1-th waypoint, n – number of waypoints.

Total travel time t_c [s] is a sum of all the partial times (9). (9) tc=∑i=0n−2ti [{t_c} = \mathop \sum \nolimits_{i = 0}^{n - 2} {t_i}

The global path method is based on a genetic algorithm (GA). It is used for finding a collision free path, whose shape is optimized to minimize the travel time. The features of the used GA are: −

roulette parents selection operator,

The collision criterion is measured with a collision depth value which is introduced by the formula (1). It is a sum of the collision depths between the considered path and all the obstacles. The travel time is computed for the considered path by the velocity profiler. Sets of the fitness values, that mean the collision and the travel time, of the all individuals are then remapped to a new domain [0; 1] separately. Then they are weighted and summed. It is shown by the equation (10). (10) F=Mkcm+Tcp;cm+cp=1;cm,cp>0 [F = {M_k}{c_m} + T{c_p};{c_m} + {c_p} = 1;{c_m},{c_p} > 0 where F is a vector of the fitness values of the whole population, M_k is a vector of the remapped collision criteria, T is a vector of the remapped travel times, c_m and c_m are coefficients of the collision criterion and the travel time respectively.

The initial population is generated by random modifications of the curve, that is built of equidistant control points, which are located on the straight line between the first and the last control point. The first point is equal to the robot start position and the last point is equal to the destination point. The robot start position and the destination point are converted from the ECEF coordinates system to the local coordinates system of the robot. The initial straight line variant of the curve is always included in the initial population. The modification range is limited to the area around known obstacles. The algorithm stops when the differences between the individuals become smaller than a set threshold.

When the algorithm stops, the result path is returned, that means the path with the lowest fitness value in the last generation of the individuals. Before use, it is converted to a set of the following waypoints.

The local path is computed in a geometrical way. The general idea of the method is presented in the Fig. 5. The map and obstacle models are the same as described in section 2. The method looks for a path constructed with two straight line segments between the local start P_S and the local destination P_K points. At first, rays, l_s,i from P_S and l_k,i from P_K , tangent to each obstacle are computed. It gives two pairs of the rays for each obstacle. To this set, a straight path between P_S and P_K points is added. Next, the intersection points D_i between the rays are calculated. This process is shown in the Fig. 6. The paths are created from segments of intersecting rays from the points P_S and P_K, which ends at their intersection point. Each path is then tested for a collision. If it is detected for at least one obstacle, the path is removed from the set.

Fig. 5.

Flowchart of the local path finding method

The same process is done for the velocity reduction areas instead of the obstacles. Collisions, however, still are computed for the obstacles.

For the remaining paths, approximate travel times are computed. The approximate travel time t_j for the j-th path is described by the formula (11). (11) tj=lj−cjv max +cjv min [{t_j} = {{{l_j} - {c_j}} \over {{v_{{\rm{\;max\;}}}}}} + {{{c_j}} \over {{v_{{\rm{\;min\;}}}}}} where l_j is the length of the j-th path, c_j is the sum of the chord lengths in the velocity reduction areas i.e. these parts of the path, that lie in the circles of the velocity reduction areas, v_max is the maximum allowed velocity for the robot, v_min is the maximum allowed velocity in the velocity reduction areas.

When the approximated travel times of every path are known, then the path of the shortest time is selected from the set as a result path and the local path algorithm ends.

Fig. 6.

Visualization of local path computation

The robot navigation method joins the results of the global path method (sec. 4) and the local path method (sec. 5). It consists of three layers: movement along the global path, deviation from the global path to avoid local obstacles (movement along the local path), return to the global path.

The 3K method described in the paper divides the path planning problem into two cases: the global context and the local context. Two distinct methods to both global and local path finding were developed in order to achieve necessary performance for successfully controlling the robot.

The global method is based on a genetic algorithm, and it uses the map of a priori known obstacles to produce a path. The local method is based on the geometrical path planning approach with numerous simplifications of the problem model. It uses the data about the obstacles that are detected during the run.

The third method was designed to control the robot using data obtained from the global and the local path finding methods. It leads the robot along the global path, but when an unknown obstacle is found, the local path is used to avoid it.

The main purpose of the 3K method is to minimize travel time of the robot in partially unknown environments. The results of the experiments in simulations and in the real world are described in a following paper.