Improving the Hybrid A* method for a non-holonomic wheeled robot

Abstract

This article proposes and completely describes a modification of the Hybrid A* method used for navigation of a non-holonomic mobile wheeled robot. Our modification allows straightforward multi-criterial adjustment of the algorithm according to the desired behavior considering not only traveled distance but also time, changing of direction, stopping, going backwards while avoiding obstacles. The obstacle avoidance algorithm evaluates the danger of collision smoothly (not-binarily) using danger fields. Such behavior reflects human-like sensing of danger—the closer to the obstacle the robot is, the higher is the danger of collision. A modified uniform state expansion method has been used to cover the state space of the robot more uniformly providing the possibility of precise near-target navigation. A greed factor has been introduced to decrease the computational time and improve the real-time performance of the algorithm.

Introduction

The navigation of the mobile robot is a dynamic control process during which the control algorithm computes actions which lead the mobile robot to the target. The position of the robot and the target can be defined by three degrees of freedom (3 DoF)—Cartesian coordinates x, y, and direction ψ. The non-holonomic robot has non-zero minimal turning radius $R_{\text{min}}$ —its 3 DoF are not fully controllable. To connect the current position and the target by a physically achievable trajectory, the robot cannot travel straight to the target due to its physical constraints. More constraints are introduced by obstacles. To take all these factors into account, it is possible to use a form of a Model Predictive Controller (MPC). ¹ This type of the controller can also be used in many non-robotic applications (see e.g. Hoy et al. ² ). To intercept the paths of the moving obstacles (e.g. humans in the working area), Bayesian representation of the obstacles has been used. ³

One of the great methods of navigation for wheeled mobile robots is a Vector Field Histogram (VFH) combined with the A* searching algorithm (abbreviated as VFH*). The method was originally proposed by Ulrich and Borenstein. ⁴ The concept of the method is described by following steps:

The current position of the robot is expanded by admissible actions into new projected positions.

Each new position is evaluated with respect to (w.r.t.) the target and the previous position. The position that is closer to the target and requires less maneuvering, has a lower cost. If the new position collides with an obstacle, the position is rejected. The total cost of the position is a sum of the previously spent cost and the estimated further cost.

Positions are recursively explored up to a given depth or until the target is reached. The A* searching algorithm is used to achieve optimal behavior. Searching is optimal and complete if the estimated cost is always smaller or equal to the true cost.

The first action along the found trajectory is physically applied to the robot and the whole process repeats. The navigation is local (does not consider the whole world but only limited surroundings) and real-time.

The original paper ⁴ proposed the following cost function for the first expansion

$g({\mathit{r}}_1)={\mu }_1\cdot \Delta \left({\psi }_1,{\psi }_t\right)+{\mu }_2\cdot \Delta \left({\psi }_1,{\psi }_0\right)+{\mu }_3\cdot \Delta \left({\psi }_1,{\psi }_{d,n-1}\right)$

1

and for deeper expansions

$g({\mathit{r}}_i)={\lambda }^{i-1}[{\mu \prime }_1\cdot \text{max}(\Delta \left({\psi }_i,{\psi }_t\right),\Delta \left({\psi }_e,{\psi }_t\right))+{\mu \prime }_2\cdot \Delta \left({\psi }_i,{\psi }_0\right)+{\mu \prime }_3\cdot \Delta \left({\psi }_i,{\psi }_{i-1}\right)]$

2

where i is the depth of the expansion, ${\psi }_i$ is the direction at position ${\mathit{r}}_i$ , ${\psi }_t$ is the direction to the target, ${\psi }_0$ is the current direction, ${\psi }_{d,n-1}$ is the previously decided direction, ${\psi }_e$ is the effective direction

${\psi }_e=\text{atan2}(y_i-y_{i-1},x_i-x_{i-1})$

3

The delta operator over directions is defined by formula

$\Delta \left({\psi }_a,{\psi }_b\right)=\text{min}\left(|{\psi }_a-{\psi }_b|,|{\psi }_a-{\psi }_b-2\pi |,|{\psi }_a-{\psi }_b+2\pi |\right)$

4

A heuristic estimation of the future cost is

$h({\mathit{r}}_i)={\lambda }^i\left[{\mu \prime }_1\cdot \Delta \left({\psi }_e,{\psi }_t\right)+{\mu \prime }_2\cdot \Delta \left({\psi }_t,{\psi }_A\right)+{\mu \prime }_3\cdot \Delta \left({\psi }_t,{\psi }_{i-1}\right)\right]$

5

Coefficient ${\mu }_1$ increases orientation of the robot to the target, coefficients ${\mu }_2$ and ${\mu }_3$ increase the smoothness of the trajectory. Coefficient λ is the discount factor and improves the overall behavior of the searching algorithm. ⁴

The main drawbacks of the original VFH* algorithm are:

The target is defined only by direction (as the target is in infinity) within the method.

The original cost function does not consider the distance travelled by the robot.

Each state used in prediction is expanded only to different directions, but not to different distances (see Figure 1). Therefore, the state space is not covered uniformly.

Figure 1.

Exploration of the nearby robot’s positions.

Other navigational methods are mainly based on grids. Chung and Huang described a navigation algorithm which represents the world as a 2-D grid of occupied or non-occupied cells. The occupancy grid can be obtained simultaneously from the readings of the robots’ sensors. ⁵ Many different sensors can be used to detect the surrounding obstacles and the robot’s own position (e.g. encoders, ultrasonic sensors, electronic compass, etc.). ⁶ Szabo ⁷ described a method for integration of the sensors’ readings by an event representation. The final path obtained by A* searching contains only eight possible directions alongside the grid or diagonally. ⁸ Daniel et al. introduced the Theta* algorithm that also uses a grid, but applies an any-angle search algorithm, which allows connection of more distant grid vertices by a straight line (line of sight). The obtained trajectory is more smooth and efficient than only 8-directional trajectory. ⁹ Yap et al. proposed the Block A* method that utilizes a data base of local distances between blocks. The data base contains more than eight nearest blocks; therefore, it is more efficient than A* or Theta*. ¹⁰

To overcome the non-holonomic nature of the planned grid trajectories (not smooth), the Stanford University team building robot Junior for DARPA Urban Challenge ¹¹ developed algorithm named Hybrid A*. The algorithm uses continuous states within 4-D state space (two horizontal coordinates, orientation, and direction of motion—forward or reverse). The surroundings of the best node were explored in six defined directions (combinations of forward/backward throttle and left/straight/right steering), which results in six child states. Each child state is assigned to one horizontal grid cell. To improve the efficiency of the expansion, two different heuristic functions have been used to evaluate each state: the first considering non-holonomic nature of the robot and the second considering obstacles. It results in fewer nodes being required to be explored during navigation. The resultant path is smoothed by Conjugate Gradient filtering. ¹²

Uniform expansion method

Using the original version of VFH*, the local maneuvering of the robot to a nearby target defined by all three coordinates is not working perfectly. One of the reasons is that the expansion uses constant travel distance (the original histogram used in VFH considered only directions). If the target is closer than the expansion distance, the robot will oscillate. The Hybrid A* algorithm is a good starting point in solving the problem.

To cover the state space $〈x,y,\psi 〉$ more uniformly, we have to discretize the state space into a grid. The grid size corresponds to the tolerance of the whole navigation algorithm. The position of the robot, when snapped to the grid is

$\left[x,y,\psi \right]=\left[n_x{G}_{xy},n_y{G}_{xy},n_{\psi }{G}_{\psi }\right]$

6

where $G_{xy}$ is a translational grid cell size, $G_{\psi }$ is a rotational grid cell size, and $n_x$ , $n_y$ , $n_z$ are integers. Introducing the grid also enables fast searching for already visited positions within the A* searching algorithm, which avoids oscillations and loops during the search.

The exploration of the near positions around the currently best position is done reversely: first, we set the desired position and then we compute the required action to achieve that position. We assume that the robot’s steering control does not need to be continuous (the curvature of the trajectory may change rapidly). The scenario is illustrated in Figure 2, note that direction (angle) ${\psi }_{i-1}$ is represented by a directional arrow.

Figure 2.

Approaching the nearby position in the ith expansion depth.

The length and the direction of the distance vector ${\mathit{P}}_i{\mathit{P}}_i{}_{-1}$ are (in that order)

$d_i=|{\mathit{d}}_i|=\sqrt{{\left(x_i-x_{i-1}\right)}^2+{\left(y_i-y_{i-1}\right)}^2}$

7

${\psi }_{di}=\text{atan2}\left(y_i-y_{i-1},x_i-x_{i-1}\right)$

8

The angle ${\delta }_i$ is equal to

${\delta }_i=\left({\psi }_{di}-{\psi }_{i-1}\right)-2\pi \Vert \frac{{\psi }_{di}-{\psi }_{i-1}}{2\pi }\Vert$

9

where $\Vert x\Vert$ denotes the nearest integer to a given real number x. The distance $d_i$ is:

$d_i=2R_i\text{sin}{\delta }_i$

10

where $R_i$ is the turning radius. Then we obtain

$R_i=\frac{d_i}{2\text{sin}{\delta }_i}$

11

If we assume that the vehicle has two control variables: throttle T and steering S, both are within range $〈-1,1〉$ . Steering is then

$S_i=\frac{R_{\text{min}}}{R_i}=\frac{2R_{\text{min}}\text{sin}{\delta }_i}{d_i}$

12

where $R_{\text{min}}$ is the minimal turning radius of the robot. The length of the trajectory is

$L_i=\{\begin{array}{ll}\hspace{0.17em}\hspace{0.17em}{d}_i\hfill & \text{if }|{\delta }_i|<\epsilon ,\hfill \\ \hspace{0.17em}\hspace{0.17em}{d}_i\frac{|{\delta }_i|}{\text{sin}|{\delta }_i|}\hfill & \text{if }\epsilon \le |{\delta }_i|\le \frac{\pi }{2},\hfill \\ -d_i\frac{\left(\pi -|{\delta }_i|\right)}{\text{sin}\left(\pi -|{\delta }_i|\right)}\hfill & \text{if }\frac{\pi }{2}<|{\delta }_i|\le \left(\pi -\epsilon \right)\hspace{0.17em},\hfill \\ -d_i\hfill & \text{if }|{\delta }_i|>\left(\pi -\epsilon \right)\hspace{0.17em}\hfill \end{array}$

13

where ε is the numerical precision of the data type used in the implementation (e.g. $\epsilon ={10}^{-6}$ ). A negative length $L_i$ means going backwards.

Within one step of the algorithm, it is reasonable to explore up to the maximal achievable distance, which corresponds to full throttle

$L_{\text{max}}=v_{\text{max}}\Delta t$

14

where $v_{\text{max}}$ is the maximal speed of the robot and $\Delta t$ is the time period of one exploration step. The throttle is then

$T_i=\frac{L_i}{L_{\text{max}}}$

15

The final direction of the robot after the turn is

${\psi }_i={\psi }_{i-1}+\frac{L_i}{R_i}={\psi }_{i-1}+\frac{2L_i\text{sin}{\delta }_i}{d_i}$

16

If the computed throttle $T_i$ or steering $S_i$ is outside the $〈-1,1〉$ boundary, the point is unreachable. Figure 3 shows reachable positions within distance $L_{\text{max}}=\frac{\pi }{2}{R}_{\text{min}}$ (quarter turn). The horizontal grid cell size was set to $G_{xy}=L_{\text{max}}/10$ .

Figure 3.

Reachable positions from the current position within one exploration step.

To speed up the calculation, it is possible to precompute a set of possible actions $\left[T,S\right]$ that will later be used for exploration.

Distance between the robot and the obstacle

Evaluation of collisions requires the distance between the robot’s projection into given position ${\mathit{P}}_i$ and the surrounding obstacles. In many cases, the robot and the obstacles have rectangular horizontal footprint. More complex shapes can be computed as a combination of rectangles. The problem of finding the distance to the nearest obstacle is therefore transformed to computing the distance between two arbitrary rotated rectangles. The distance between rectangles is the minimum of distances between all corners of one rectangle and sides of the other rectangle. Figure 4 illustrates the whole situation.

Figure 4.

Minimal distance between the robot and the obstacle.

Position of the robot’s origin is shifted from the rectangle’s centrum ${\mathit{C}}_i$ to the centrum of the non-controlled axle ${\mathit{P}}_i$ by dislocation $e_0$ . Dimensions of the robot are $a_0\times b_0$ , dimensions of the kth obstacle are $a_k\times b_k$ . The position of the robot’s geometrical centrum is

${\mathit{C}}_i={\mathit{P}}_i+e_0\cdot {\left[\text{cos}{\psi }_i\hspace{0.17em}\hspace{0.17em}\text{sin}{\psi }_i\right]}^{\text{T}}$

17

Positions of the obstacle’s corners are

${\mathit{M}}_{k,n}={\mathit{C}}_k+\frac{1}{2}\left[\begin{array}{cc}\text{cos}{\psi }_k& -\text{sin}{\psi }_k\\ \text{sin}{\psi }_k& \text{cos}{\psi }_k\end{array}\right]\cdot \left[\begin{array}{c}\pm a_k\\ \pm b_k\end{array}\right]$

18

where ${\psi }_k$ is the orientation (angle of rotation) of the obstacle and $n=1,2,\cdots 4$ is the index of the corner. The positions of the obstacle’s corners in robot’s local coordinate system $〈x^{\prime },y^{\prime }〉$ are

${\mathit{M}\prime }_{k,n}=\left[\begin{array}{c}{x^{\prime }}_{k,n}\\ {y^{\prime }}_{k,n}\end{array}\right]=\left[\begin{array}{cc}\text{cos}{\psi }_i& \text{sin}{\psi }_i\\ -\text{sin}{\psi }_i& \text{cos}{\psi }_i\end{array}\right]\cdot \left({\mathit{M}}_{k,n}-{\mathit{P}}_i\right)$

19

The distances between the obstacle’s corner and the robot are

$d_{k,n}=\text{min}\left(|{x^{\prime }}_{k,n}-e_0|-a_0,\hspace{0.17em}|{y^{\prime }}_{k,n}|-b_0\right)$

20

Using similar approach, we obtain positions of the robot’s corners ${\mathit{N}}_{i,n}$

${\mathit{N}}_{i,n}={\mathit{C}}_i+\frac{1}{2}\left[\begin{array}{cc}\text{cos}{\psi }_i& -\text{sin}{\psi }_i\\ \text{sin}{\psi }_i& \text{cos}{\psi }_i\end{array}\right]\cdot \left[\begin{array}{c}\pm a_i\\ \pm b_i\end{array}\right]$

21

The positions of the robot’s corners w.r.t. the obstacle are

${{\mathit{N}}^{\prime }}_{i,n}=\left[\begin{array}{c}{x^{\prime }}_{i,n}\\ {y^{\prime }}_{i,n}\end{array}\right]=\left[\begin{array}{cc}\text{cos}{\psi }_k& \text{sin}{\psi }_k\\ -\text{sin}{\psi }_k& \text{cos}{\psi }_k\end{array}\right]\cdot \left({\mathit{N}}_{i,n}-{\mathit{C}}_k\right)$

22

The distances $d_{i,n}$ between the robot’s corners and the obstacle are

$d_{i,n}=\text{min}\left(|{x^{\prime }}_{i,n}|-a_0,\hspace{0.17em}|{y^{\prime }}_{i,n}|-b_0\right)$

23

The distance between the ith robot’s projection and the kth obstacle is the minimum of all eight distances

$c_{i,k}=\text{min}\left(d_{k,1},\hspace{0.17em}{d}_{k,2},\hspace{0.17em}{d}_{k,3},\hspace{0.17em}{d}_{k,4},\hspace{0.17em}{d}_{i,1},\hspace{0.17em}{d}_{i,2},\hspace{0.17em}{d}_{i,3},\hspace{0.17em}{d}_{i,4}\right)$

24

Clearance $c_i$ is the minimum of $c_{i,k}$ across all obstacles k.

If the obstacle or robot is significantly larger than the grid cell, the proposed method provides significant speedup of the clearance calculation compared to widely used computation of clearance using distances between occupied cells. It also allows more precise maneuvering, since each object may occupy its border cells only partially and the robot and the obstacle may occupy the same cell safely without collision. If the true horizontal footprint of the robot is not rectangular, we use minimal area bounding rectangle.

The reliability and safety of the proposed method depends on the way how the position of the obstacle is obtained. The obstacle may be detected by the robots’ local sensors, or, in case of multiple robots, the information may be provided by communication among them. Then, the most critical factor is the safety of the communication, which has to be evaluated separately. ¹³

The cost of single exploration step

To find the optimal trajectory, the A* method expands a non-opened node with the smallest (best) total cost during each iteration. The total cost $f_i$ is a sum of the cost $g_i$ spent by reaching the ith node and the estimated future cost $h_i$ that has to be spent to reach the target. The spent cost $g_i$ increases with distance, changing of the direction, changing of the speed, or going backwards.

The cost of distance

The cost of distance is proportional to the length of the trajectory

$g_{Li}=K_L\cdot |L_i|\hspace{1em}\hspace{1em}[K_L]={\text{m}}^{-1}$

25

where $K_L$ is the (constant) cost per meter.

The cost of changing the direction

If it is required to maintain a smooth trajectory, the cost of changing the heading may be introduced. The cost is proportional to the arc angle

$g_{Si}=K_S\cdot |{\psi }_i-{\psi }_{i-1}|=K_S\cdot |\frac{2L_i\text{sin}{\delta }_i}{d_i}|\hspace{1em}[K_S]={\text{rad}}^{-1}$

26

where $K_S$ is the (constant) cost per radian.

The cost of stopping the robot

To avoid too many reversal points (point where the vehicle has to stop and change the polarity of throttle), we introduce the cost of stopping

$g_{Ti}=K_T\cdot \text{neg}\left(T_{i-1}\cdot T_i\right)\hspace{1em}\hspace{1em}[K_T]=1$

27

where $T_{i-1}$ is the previous throttle, $K_T$ is a fixed cost per one reversal point and $\text{neg}\left(x\right)$ is the following function

$\text{neg}(x)=\{\begin{array}{ll}1\hfill & \text{if }x<0\hfill \\ 0\hfill & \text{if }x\ge 0\hfill \end{array}$

28

The cost of going backwards

For certain mobile robots, it is not convenient to go backwards (e.g. due to worse maneuvering abilities). The cost of going backwards is

$g_{bi}=-K_b\cdot \text{neg}\left(L_i\right)\cdot L_i\hspace{1em}\hspace{1em}[K_b]={\text{m}}^{-1}$

29

where $K_b$ is the (constant) cost per one meter of going backwards.

The cost of danger

The most crucial part of the navigation is avoiding collisions with obstacles. Chen and Zhang ¹⁴ proposed a method for estimation of the distance between moving non-holonomic robots. Since the estimation of the position is not absolutely precise, collision detection should not be binary (the real position of the robot could collide with an obstacle while the estimated position does not collide). Therefore, we have introduced a danger field, which is derived from the potential field navigation method. The danger field defines a dimensionless danger of collision as a function of the robot’s position $\left[x,y,\psi \right]$ . The function is as follows

$D_i={\left[\frac{c_1-c_0}{\text{max}\left(c_i-c_0,\epsilon \right)}\right]}^{\kappa }$

30

where $c_i$ is a clearance (minimal distance) between the robot projected to position ${\mathit{P}}_i$ and the surrounding obstacles, $c_0$ is a safety border in meters around each obstacle, $c_1$ is a normalization constant which corresponds to the distance at which the danger is equal to 1. Coefficient κ is the steepness of the danger function (we use $\kappa =2$ ) and ε is a very small positive number (prevents division by zero). If any obstacle moves, the danger field function also changes due to the changes of clearance.

The cost of danger is then

$g_{Di}=K_D\cdot D_i\hspace{1em}\hspace{1em}[K_D]=1$

31

where $K_D$ is the weight (dimensionless constant) of the danger. If the $K_D$ is higher, the robot is “more afraid” of collision.

The cost of time

The same trajectory can be achieved by a smaller number of large steps (higher speed) or by a larger number of small steps (small speed). It is desirable to select the faster trajectory, so we have introduced the cost of time. Each time step has the cost proportional to parameter $K_t$

$g_{ti}=K_t\Delta t\hspace{1em}\hspace{1em}[K_t]={\text{s}}^{-1}$

32

The overall estimated spent cost is then given by the sum of all partial costs and the cost $g_i{}_{-1}$ spent for reaching the position $P_{i-1}$

$g_i=g_{i-1}+g_{Li}+g_{Si}+g_{Ti}+g_{bi}+g_{Di}+g_{ti}$

33

Estimation of future cost

In this chapter, we will solve the problem of the minimal distance which has to be run by a non-holonomic wheeled robot to move from one position to another. The space of all possible trajectories is too large to search and contains discontinuities; therefore, it is necessary to choose a certain pattern of the trajectory.

Possible trajectories

We have chosen the trajectory pattern shown in Figure 5 which consists of an initial arc $\mathit{P}\mathit{X}$ , then followed by a straight line $\mathit{X}\mathit{Y}$ , and finished by another arc $\mathit{Y}\mathit{B}$ . To keep the trajectory length minimal, the radius of the initial and the final arc turns is $R_{\text{min}}$ . Such trajectory has been proved by Dubins to be the shortest. ¹⁵ These trajectories are commonly referred to as Dubins paths. Due to the motion planning purposes, also reversing should be considered. By modifying the Dubins principle, Reeds and Shepp ¹⁶ introduced a more complex method using at most five segments of arcs and/or straight lines. This article proposes the usage of a three-segment curve but also considers reversing.

Figure 5.

Trajectory pattern, forward trajectory.

First, we define process variables (we omit the index i of the position ${\mathit{P}}_i$ and of other variables):

$\mathit{P}=\left[x_P,y_P\right]$ the evaluated horizontal position,

${\psi }_P$ the evaluated direction,

$\mathit{B}=\left[x_B,y_B\right]$ the target horizontal position,

${\psi }_B$ the target direction,

${\mathit{R}}_1$ the radial vector of the initial turn (perpendicular to the ${\psi }_P$ ),

${\mathit{R}}_3$ the radial vector of the final turn (perpendicular to the target ${\psi }_B$ ),

$\mathit{d}$ the distance vector between the centrums of the turns,

${\psi }_d$ the direction of vector $\mathit{D}$ ,

$L_1$ the length of the initial arc $\mathit{P}\mathit{X}$ ,

$L_2$ the length of the straight line $\mathit{X}\mathit{Y}$ ,

$L_3$ the length of the final arc $\mathit{Y}\mathit{B}$ ,

${\alpha }_1$ the initial arc angle,

${\alpha }_3$ the final arc angle,

$s_1$ the polarity of the initial arc (1 = counterclockwise, −1 = clockwise),

$s_3$ the polarity of the final arc (1 = counterclockwise, −1 = clockwise).

According to Figure 5, the centrum of the initial arc is

${\mathit{C}}_1=\mathit{P}+s_1{\mathit{R}}_1=P+s_1{R}_{\text{min}}\left[-\text{sin}{\psi }_P,\text{cos}{\psi }_P\right]$

34

analogically, the centrum of the final turn is

${\mathit{C}}_3=\mathit{B}+s_3{R}_{\text{min}}\left[-\text{sin}{\psi }_B,\text{cos}{\psi }_B\right]$

35

The distance vector between the centrum of the turns is then

$\mathit{d}={\mathit{C}}_3-{\mathit{C}}_1=\left[d_x,d_y\right]$

36

and its direction is

${\psi }_d=\text{atan2}\left(d_y,d_x\right)$

37

In the following sections, we will compute all possible trajectories matching the given trajectory pattern.

The same polarity of turns

First, we will discuss the case when both turns have the same polarity

$s_1=s_3$

38

The arc angles are then (according to Figure 5)

${\alpha }_{1+}=s_1\left({\psi }_d-{\psi }_P\right)$

39

${\alpha }_{3+}=s_3\left({\psi }_B-{\psi }_d\right)$

40

Positive sign in indices denotes that the straight segment of the trajectory is traveled forwardly. Polarities $s_{-1}$ and $s_3$ were introduced, because we want to avoid negative angles. To keep the arc angles within the interval $\left[0,2\pi \right)$ , we remove the period

${\alpha }_k\leftarrow {\alpha }_k-2\pi \lfloor \frac{{\alpha }_k}{2\pi }\rfloor$

41

where $\lfloor x\rfloor$ denotes the nearest smaller integer to a given number x. A similar relation is valid for all used angles. Positions $\mathit{X}$ and $\mathit{Y}$ can be reached not just by turning by angles ${\alpha }_{1+}$ and ${\alpha }_{3+}$ , respectively, but also by complementary angles $\left(2\pi -{\alpha }_{1+}\right)$ and $\left(2\pi -{\alpha }_{3+}\right)$ in reverse manner (displayed by the dashed line).

There is also a possibility of travelling the straight line reversely (see Figure 6).

Figure 6.

The same polarity of turns, reverse trajectory.

The arc angles in the reverse case are

${\alpha }_{1-}=s_1\left({\psi }_d+\pi -{\psi }_P\right)$

42

${\alpha }_{3-}=s_3\left({\psi }_B-{\psi }_d-\pi \right)$

43

Operation (41) has to be applied to both angles (42) and (43) to keep them within the interval $\left[0,2\pi \right)$ . The length of the straight element is

$d=\sqrt{d_x{}^2+d_y{}^2}$

44

To denote all possible trajectories in a compact way, we will denote each trajectory as a vector $\left[L_1,L_2,L_3\right]$ . The negative sign will mean that the robot should travel backwards. Using this notation, the possible trajectories for the case of the same polarity of turns are

$\begin{array}{c}[R_{\text{min}}{\alpha }_{1+},\hspace{1em}d,\hspace{1em}{R}_{\text{min}}{\alpha }_{3+}]\\ [-R_{\text{min}}\left(2\pi -{\alpha }_{1+}\right),\hspace{1em}d,\hspace{1em}{R}_{\text{min}}{\alpha }_{3+}]\\ [R_{\text{min}}{\alpha }_{1+},\hspace{1em}d,\hspace{1em}-R_{\text{min}}\left(2\pi -{\alpha }_{3+}\right)]\\ [-R_{\text{min}}\left(2\pi -{\alpha }_{1+}\right),\hspace{1em}d,\hspace{1em}-R_{\text{min}}\left(2\pi -{\alpha }_{3+}\right)]\\ [R_{\text{min}}{\alpha }_{1-},\hspace{1em}-d,\hspace{1em}{R}_{\text{min}}{\alpha }_{3-}]\\ [-R_{\text{min}}\left(2\pi -{\alpha }_{1-}\right),\hspace{1em}-d,\hspace{1em}{R}_{\text{min}}{\alpha }_{3-}]\\ [R_{\text{min}}{\alpha }_{1-},\hspace{1em}-d,\hspace{1em}-R_{\text{min}}\left(2\pi -{\alpha }_{3-}\right)]\\ [-R_{\text{min}}\left(2\pi -{\alpha }_{1-}\right),\hspace{1em}-d,\hspace{1em}-R_{\text{min}}\left(2\pi -{\alpha }_{3-}\right)]\end{array}$

45

The opposite polarity of turns

The case when initial and final turns have opposite polarities is shown in Figure 7, the reverse variant is in Figure 8.

Figure 7.

Opposite direction of turns, forward trajectory.

Figure 8.

Opposite direction of turns, reverse trajectory.

The arc angles are

${\beta }_{1+}=s_1\left({\psi }_D-{\psi }_P\right)+\delta$

46

${\beta }_{3+}=s_3\left({\psi }_B-{\psi }_D\right)+\delta$

47

where the angle δ (according to the figure) is

$\delta =\text{asin}\left(\frac{R_{\text{min}}}{d/2}\right)=\text{asin}\left(\frac{2R_{\text{min}}}{d}\right)$

48

The complementary arc angles (dashed arcs) are computed in the same way as in the previous case. From equation (48), it is clear that the trajectory exists only if

$R_{\text{min}}<\frac{d}{2}$

49

The length of the straight section of the trajectory is

$s=\sqrt{d_x{}^2+d_y{}^2-4R_{\text{min}}{}^2}$

50

Like in the case when both circles have the same polarity, there is also the possibility to travel reversely (see Figure 8).

The corresponding arc angles are

${\beta }_{1-}=s_1\left({\psi }_D+\pi -{\psi }_P\right)-\delta$

51

${\beta }_{3-}=s_3\left({\psi }_B-{\psi }_d-\pi \right)-\delta$

52

After removing the period from all arc angles using equation (41), we obtain another set of possible trajectories (valid for the case of opposite arc polarities)

$\begin{array}{c}[R_{\text{min}}{\beta }_{1+},\hspace{1em}s,\hspace{1em}{R}_{\text{min}}{\beta }_{3+}]\\ [-R_{\text{min}}\left(2\pi -{\beta }_{1+}\right),\hspace{1em}s,\hspace{1em}{R}_{\text{min}}{\beta }_{3+}]\\ [R_{\text{min}}{\beta }_{1+},\hspace{1em}s,\hspace{1em}-R_{\text{min}}\left(2\pi -{\beta }_{3+}\right)]\\ [-R_{\text{min}}\left(2\pi -{\beta }_{1+}\right),\hspace{1em}s,\hspace{1em}-R_{\text{min}}\left(2\pi -{\beta }_{3+}\right)]\\ [R_{\text{min}}{\beta }_{1-},\hspace{1em}-s,\hspace{1em}{R}_{\text{min}}{\beta }_{3-}]\\ [-R_{\text{min}}\left(2\pi -{\beta }_{1-}\right),\hspace{1em}-s,\hspace{1em}{R}_{\text{min}}{\beta }_{3-}]\\ [R_{\text{min}}{\beta }_{1-},\hspace{1em}-s,\hspace{1em}-R_{\text{min}}\left(2\pi -{\beta }_{3-}\right)]\\ [-R_{\text{min}}\left(2\pi -{\beta }_{1-}\right),\hspace{1em}-s,\hspace{1em}-R_{\text{min}}\left(2\pi -{\beta }_{3-}\right)]\end{array}$

53

Computing the minimal cost of travelling to the target

There are 4 possible combinations of arc polarities

$[s_1,s_3]\in \left\{\hspace{0.17em}[-1,\hspace{0.17em}-1],\hspace{0.17em}[-1,\hspace{0.17em}1],\hspace{0.17em}[1,\hspace{0.17em}-1],[1,\hspace{0.17em}1]\hspace{0.17em}\right\}$

54

For each combination, we obtain eight possible trajectories (using equation (45) for the same polarities and equation (53) for the opposite polarities). The full set contains 4 × 8 = 32 possible trajectories matching the defined pattern. Each trajectory has its cost, which reflects the requirements of the application. Generally, the cost increases with the distance, changing of the heading, stopping the robot, or going backwards. Note that we do not evaluate danger (since it must be evaluated at each point of the trajectory). The formulas for predicted future costs are similar to the formulas for the spent cost (equations (25) to (33))

$h_{Li}=K_L\left(|L_{1i}|+|L_{2i}|+|L_{3i}|\right)$

55

$h_{Si}=K_S\frac{\left(|L_{1i}|+|L_{3i}|\right)}{R_{\text{min}}}$

56

$h_{Ti}=K_T\left[\text{neg}(T_i\cdot L_{1i})+\text{neg}(L_{1i}\cdot L_{2i})+\text{neg}(L_{2i}\cdot L_{3i})\right]$

57

$h_{bi}=K_b\left[\text{neg}(L_{1i})\cdot L_{1i}+\text{neg}(L_{2i})\cdot L_{2i}+\text{neg}(L_{3i})\cdot L_{3i}\right]$

58

$h_{ti}=K_t\frac{\left(|L_{1i}|+|L_{2i}|+|L_{3i}|\right)}{v_{\text{max}}}$

59

where $T_i$ is the throttle which was used to reach position $P_i$ (corresponds to the forward speed of the robot). The cost of time is computed as if the speed was maximal during the whole trajectory (minimal time). The overall estimated future cost is then given by the sum of all costs

$h_i=h_{di}+h_{Si}+h_{Ti}+h_{bi}+h_{ti}$

60

The estimated future cost is computed for each of the 32 possible trajectories and the best trajectory is selected.

The greed factor

If the original A* searching algorithm is used, the total cost of any predicted position $P_i$ is

$f_i=g_i+h_i$

61

The navigation algorithm finds the optimal trajectory but it examines too many nodes which results in poor real-time performance. To speed up the calculation and searching, we have introduced the greed factor γ. It sacrifices a small portion of the optimal (minimal cost) to decrease the number of nodes required to be expanded during search. The modified total cost function replaces equation (61)

$f_i=\left(1-\gamma \right)g_i+h_i$

62

When the greed factor is equal to 1, the searching does not consider the spent cost $g_i$ that results in greedy search.

Each predicted state of the robot will be represented by the structure Node :

structure Node

 . [x, y, ψ] -- position

 . [T, S] -- action

 . parent -- previous node

 . g -- spent cost

 . h -- estimated future cost

 . f -- total cost

end

The cost spent from the starting state into predicted state is computed using function spent_cost shown in Algorithm 1: The estimated cost from predicted state into next state is computed by the function future_cost described in Algorithm 2:

Algorithm 1.

function spent_cost (node, obstacles)

Algorithm 2.

function future_cost (node, target)

Exploration of the child states is accomplished by the function expand shown in Algorithm 3:

Algorithm 3.

function expand(node, target, obstacles)

Finally, the function navigate implements A* searching with greed factor (see Algorithm 4):

Algorithm 4.

function navigate (from, target, obstacles, T ₀ , S ₀)

Experimental results

The proposed system has many adjustable parameters. To obtain results, that are comparable across experiments, we have used simulation in the simulated world (see Figure 9). The parameters of the robot were as follows: $v_{\text{max}}=100$ pixels s⁻¹, $R_{\text{min}}=50 \text{pixels}$ , the tolerance of reaching the target (also the grid cell size): 10 pixels/5°. The resolution of the visualization was set to 640 × 480 pixels. The simulation was implemented in the OpenCV framework using the C++ programming language. The algorithm was evaluated on a standard PC with double-core Intel Pentium G870 3.10 GHz.

Figure 9.

The simulated world and the optimal trajectory found (300,260 nodes has been explored).

The world contains 12 static obstacles which makes it difficult for the robot to choose the optimal trajectory (many trajectories have the same cost).

We have evaluated the cost of the found trajectory, the computation time, and the count of explored nodes as a function of the greed factor. All the experiments were conducted with the same pseudorandom set of 1000 targets and 20 different settings of the greed factor (20,000 experiments). The cost constants which describe the properties of the optimal trajectory were set to the following values: the prediction step $\Delta t=480\text{ ms}$ , the cost of distance $K_L=1/\text{pixel}$ , the cost of steering $K_S=20{\text{ rad}}^{-1}$ , the cost of stopping $K_T=100$ , the cost of going backwards $K_b=0.3$ , the cost of danger $K_D=200$ , the cost of time $K_t=1{\text{ s}}^{-1}$ . The parameters of the danger function were as follows: the safety border $c_0=5\text{ pixels}$ , the unit distance $c_1=15\text{ pixels}$ , the exponent $\kappa =2$ (Figure 10 shows a map of danger around the obstacles and the robot itself).

Figure 10.

Map of danger.

To limit the simulation time, each experiment was limited to 1 s of computational time. If the algorithm did not find the solution within the given time limit, it was considered as unsuccessful. Such targets were removed from the set of targets. The remaining experiments were averaged for the same setting of the greed factor across all targets.

To demonstrate the importance of the uniform expansion method, we compare its performance with the state expansion in six directions only, which was proposed by the original Hybrid A* method.

As predicted, a higher greed factor allows the method to find the trajectory faster but the obtained trajectory is slightly suboptimal (compare Figures 9 and 11). Figure 12 shows the relation between the average cost per one target and the greed factor. With the greed factor from the range of 0 to 0.8, the cost of the trajectory is increasing only slightly (the greed factor $\gamma =0.8$ caused an average increase of the cost by 35%). Further increase of the greed factor causes rapid increase of the cost (highly suboptimal behavior).

Figure 11.

A suboptimal path computed using a greed factor γ = 0.4 (only 49,240 nodes has been explored).

Figure 12.

Average cost of the trajectory versus greed factor.

Figure 13 shows the average count of nodes which had to be explored to find the solution. With the greed factor from 0 to 0.3, the average count of nodes decreases almost linearly, and for a greed factor above 0.6, it is almost constant.

Figure 13.

Average count of explored nodes versus greed factor.

Computational time is closely related to the count of explored nodes, therefore the relation in Figure 14 is similar to the one in Figure 13. A higher greed factor means lower computational time. For a greed factor above 0.5, the average computational time was below 100 ms.

Figure 14.

Average computation time versus greed factor.

Since we require the algorithm to operate in real-time, the computation time was limited to 1 s. Figure 15 shows the probability of finding the trajectory within the given time limit. Without the greed factor, solutions for only 20% of the targets were found on time. For a greed factor above 0.5, more than 70% of the solutions were found on time. Note that using only six directions for expansion significantly increases the count of explored nodes, thus finding the solution times out in many cases. Figure 14 shows average of successful searches, therefore, it does not reflect poor probability of finding the solution in case of the six-directional expansion.

Figure 15.

Probability of finding the trajectory within timeout (1 s) versus greed factor.

The algorithm has been evaluated in real world using e-puck robot (e-puck is a small mobile robot developed by GCtronic). ^17,18 The testing environment with the e-puck robot inside can be seen in Figure 16. The environment has been modeled also in the control program (see Figure 17). The robot accomplished to pass the proposed trajectory while its position has been estimated using onboard odometers.

Figure 16.

Testing environment with e-puck robot.

Figure 17.

Designed trajectory.

Conclusion

Our proposed method improves the evaluation method of the Hybrid A* algorithm. First, we have introduced a uniform state expansion method which improves searching speed and decreases count of nodes needed to be explored. Then, we have modified and simplified the heuristic method used to estimate the future cost required to reach a given target. The navigation algorithm considers not only the length of the trajectory but also reversing, going backwards, changing the direction, the danger of collision, and the time of travel. Each feature is penalized by a separate parameter which allows simple adjustment of the behavior of the algorithm according to the requirements of any application. The evaluation of danger is not binary as used in many implementations of Hybrid A* (e.g. studies by Kurzer ^1,19 ), but smooth, which reflects the limited precision of the robot’s localization system.

To speed up the real-time computations, we have used the greed factor which allows us to set the right balance between the required computational power and the cost of the projected trajectory. Our experiments show that a greed factor around 0.5 decreases the cost of the obtained trajectory only by 16% but decreases the computational time 5 times, which greatly improves the real-time performance of the algorithm.