Theory about Path Planning

Problem definition

We will start with the following hypothesis: the locomotion environment (the map) is known; we know the initial (start) position and the final (desired, target) position of the Robot (R). In these conditions the path planning problem means to find the shortest path from the starting point to the target point which avoids the (known) obstacles (see figure 1). It is important to mention that we have used the term shortest to introduce a cost function minimum. For most situations the cost function is in fact the length from the two mentioned points, but we can imagine also other functions, like travel lime etc.

Figure 1. The environment map; the initial position (red circle); the target position (green circle)

The path planning problem is solved in the following steps:

Discretize the environment, i.e. transform the continuous environment into a discrete one. Because the environment is a two dimension map, the results of this step is the matrix $M_{m\times n}$ . An intuitive image of this operation is the map overlapped with a grid (see figure 2).

Each mesh of the mentioned grid can be classified in two sets: the first is composed by meshes which cover obstacle free spaces; the second is composed by meshes which cover obstacles or parts from the obstacles (see figure 3).

The meshes from the first set are empty mesh, where the locomotion is possible, the set consist of filled mesh corresponds to spaces where the locomotion is not possible (see figure 4).

For each mesh an element of the $M_{m\times n}$ matrix is defined: zero for the empty meshes and one (or NaN) for the filled meshes (see figure 5).

Define the map matrix M mxn — Figure 5. Define the map matrix M

The second step computes the shortest path. Many solutions are possible, in the next section we will focus on the A* algorithm. An illustration of the result of this step is presented in figure 6. It is a subset of the matrix $M_{m\times n}$ , with neighboring elements which start from the element used for the initial position and end with the element used for marking the target position.

Figure 6. The shortest path, a set of the map matrix elements

The third step uses the mentioned elements and transforms them in positions on the map. Each element (figure 7) is transformed into a coordinate (x,y)

The set of position is used to obtain a smooth trajectory, which is in fact the (R) path from the start position to the goal position (see fig.8). In next section we will present the smoothing algorithm. We will observe that the smooth trajectory is also defined as a collection of points.

Figure 8. The smooth trajectory: The robot’s path

Map representation

The map is a 2D or 3D representation of the environment. The map boundaries have been modeled with a rectangle and the obstacles have been modeled by lines represented by interconnected of points. In fact, the map consists on the following elements (properties):

Boundary dimensions: $D=[x_{min} \: \: x_{max} \: \: y_{min} \: \: y_{max}]$
Obstacles:
- Set of coordinates of points in matrix form $P=[P_{1} \: \: P_{2} \: \: ... \: \: P_{n} ]$ $P_{i}=\left[\begin{array}{cc} x_{i}\\ y_{i} \end{array}\right]$
- The incident matrix, which define the lines by interconnected points:

$M_{ic}(i,j)=\begin{cases} 1 & \exists L_{i,j} \\ 0, & \neg \exists L_{i,j} \end{cases}$ where $L_{i,j}$ is the line from point P_i to P_j;

The task matrix which contain the initial and the final position of (R) $T=[P_{s} \: \: P_{t} ];$

The map representation is made by drawing the boundary rectangle, the initial and the final positions of the Robot ( R ) and, using the incident matrix, the lines. Figure 9 gives an example of environment representation.

Figure 9. Graphical representation of the environment

Map discretization

Additionally to the parameters defined in the previous section, the map discretization needs the sampling ratio $\Delta l$ .

Our solution consists on the following steps:

Move (translate) the origin of the reference system in the lower left corner of the map, so the obstacles’ coordinate matrix and the task matrix will change:

(1) $\begin{equation*} P_{new}=P-\left[\begin{array}{cc} x_{min}\\ y_{min} \end{array}\right];\: \: T_{new}=T-\left[\begin{array}{cc} x_{min}\\ y_{min} \end{array}\right]; \end{equation*}$

Compute the environment matrix $M_{n\_ x \: n\_ y}$ dimensions

(2) $\begin{equation*} n\_ x, \: y=\left[\frac{ l_{x,y}}{\Delta l}\right] \end{equation*}$

where l_x,yis the environment length in x, y direction

Discretize the points positions:

(3) $\begin{equation*} P_{disc}=\left[\frac{ P_{new}}{\Delta l}\right]; \: \: T_{disc}=\left[\frac{T_{new}}{\Delta l}\right] \end{equation*}$

Compute the environment matrix elements. For this reason we have used the lines defined with the discrete points. We start with a zero matrix and rewrite the elements.
- Check if the line is on Y direction $L_{ij}\in Y$
- Check if the line is on X direction $L_{ij}\in X$

(4) $\begin{equation*} \begin{cases} L_{ij}\in Y & \Rightarrow M\left( P_{disc,i}(1),P_{disc,i}(2) \right,...,,M \left( P_{disc,i}(1),P_{disc,j}(2) \right) =NaN \\ L_{ij}\in X & \Rightarrow M\left( P_{disc,i}(1),P_{disc,i}(2)\right),...,M \left( P_{disc,j}(1),P_{disc,i}(2) \right) =NaN \\ L_{ij} & \Rightarrow M\left( P_{disc,i}(1),P_{disc,i}(2)\right),...,M\left( P_{disc,i}(1)+s\cdot k_x,P_{disc,i}(2)+s\cdot k_y \right),...,M\left( P_{disc,j}(1),P_{disc,j}(2) \right) =NaN\end{cases} \end{equation*}$

where $s=\frac{\left( P_{disc,j}(2),P_{disc,i}(2) \right)}{\left( P_{disc,j}(1),P_{disc,i}(1) \right) }$

$k_x=\overline{1,\left( P_{disc,j}(1),P_{disc,i}(1) \right) }, k_y=\overline{1,\left( P_{disc,j}(2),P_{disc,i}(2) \right) }$

Figure 10 illustrate this process.

Figure 11.Compute the environment matrix

Thicken the (lines) obstacles with k elements in both directions. This is useful in order to compute a safety path (avoiding the collisions with the obstacles). We start with a zero matrix M_B and rewrite its elements.

(5) $\begin{equation*} \begin{cases} M(i,j)=NaN & \Rightarrow M_B(i-k : i+k, j-k : j+k) \\ M(i,j) \neq NaN \end{cases} \end{equation*}$

Flip the M_B matrix in Y direction:

(6) $\begin{equation*} M(i,j)=M_B(i,n\_ y -j +1) \end{equation*}$

Using the previous algorithm, the environment presented in figure 9 has been transformed in the matrix presented in figure 13 which is the sparsity pattern of matrix M

Figure 13. The final result of map discretization

**A star (A^*) algorithm**

The well knows A star algorithm is an optimal graph search algorithm. A star belong to best first search category algorithms and use a cost function which combine the traveling cost from the initial nod to the current node with a heuristic, which estimate the cost of traveling from the current node to the final (goal) node from the class. This combination gives the path-based evaluation function.

(7) $\begin{equation*} f(node)=g(node)+h(node) \end{equation*}$

where:

$g(node)$ is the traveling cost from the initial nod to the current (node);

$h(node)$ is the heuristic function.

In our case each element of the matrix M becomes a node, the element which matches the start point is the initial node and the element which matches the goal point is the goal node.

I order to compute the cost function a stepping procedure must be defined. This procedure defines the links from the current node to the next nodes: the locomotion possibilities.

Several rules can be imagined. An attractive (simple) rule is inspired from the chess rook locomotion model. The next node can be reached in the same line from the current column to the neighboring column; or in the same column from the current line to the neighboring lines. Moving from one node to the next node will increase the $g(node)$ function with 1. The $h(node)$ function computes the distance by adding the number of columns and rows which separate the current nod and the goal node. The heuristics is eluding the possible obstacles. Figure 14 gives an illustration of the mentioned concept.

Figure 14. The g(node) and h(node) definition illustration (Rook model)

Other rules can be imagined. If we continue to use the chess play allegory the new model can be the King locomotion rules. Figure 15 illustrate the new rules.

Figure 15. The g(node) and h(node) definition illustration (King model)

The A star algorithm compute the shortest path from the start node to the goal node. Before we start to present the algorithm it is important to mention that two results are to be expected: finding the path or denying its existence. The algorithm can be finished with the following statements:

STOP_failure this means there are no solutions;
RESOLVED, this means that the path has been computed;
STOP_succes an intermediary result which means that the search is ended (the target node have been funded) and the path can be computed.

A star includes the following steps:

Create two empty lists of nodes: the Open list and the Close list,
1. Check if the start node is (also) the goal nod, if yes stop the algorithm with success:
  (8) $\begin{equation*} M(i_S,j_S) \equiv M(i_T,j_T) \rightarrow STOP_{succes}\end{equation*}$
2. If not, compute the costs functions and add the start node to the close list;
  (9) $\begin{equation*} \begin{cases}O = \{ \}\\ \left[\begin{array}{c} M(i,j)_S\\ h(i,j) \\ g(i,j)=0 \\ f(i,j) \end{array}\right] \end{cases} \end{equation*}$
Perform one step (using one of the previous stepping procedures) and generate new nodes (the set of new nodes):
(10) $\begin{equation*} Step \left( M(i,j) \right) = \{ M(i+1,j),M(i-1,j),(i,j+1),M(i,j-1) \}\end{equation*}$
1. Check if the new nodes are empty nodes. If the nodes are outside the boundary or is a filled node, eliminate them from the set of new nodes;
  (11) $\begin{equation*} \forall M(k,l) \in Step \left( M(i,j) \right),M(k,l) = NaN \rightarrow Step \left( M(i,j) \right) = Step \left( M(i,j) \right) -M(k,l)\end{equation*}$
2. Check if the new nodes are in the close list. If yes, eliminate them from the set of new nodes
  (12) $\begin{equation*} \forall M(k,l) \in Step \left( M(i,j) \right),M(k,l) \in C \rightarrow Step \left( M(i,j) \right) = Step \left( M(i,j) \right) -M(k,l)\end{equation*}$
Compute the cost functions for the new nodes
(13) $\begin{equation*} \forall M(k,l) \in Step \left( M(i,j) \right) \rightarrow \left[\begin{array}{c} M(k,l) \\ h(k,l) \\ g(k,l) \\ f(k,l) \end{array}\right] = \left[\begin{array}{c} M(k,l) \\ |k_T-k|+|l_T-l| \\ g(i,j)+1 \\ h(k,l)+g(k,l) \end{array}\right] \end{equation*}$
1. Check if the nodes (from the set of new nodes) are already in the open list. If yes, there are duplicate nodes. If the cost function of the duplicate node, which belongs in the new nodes set, is bigger than the cost function of the same node, from the open list, eliminate it from the set; at contrary, eliminate the node from the open list.
  (14) $\begin{equation*} \begin{array}{c} \forall M(k,l) \in Step \left( M(i,j) \right), \exists M(k,l) \in O \wedge f(k,l)_{step} \geq f(k,l)_O \rightarrow Step \left( M(i,j) \right)=Step \left( M(i,j) \right) - M(k,l) \\ \forall M(k,l) \in Step \left( M(i,j) \right), \exists M(k,l) \in O \wedge f(k,l)_{step} < f(k,l)_O \rightarrow O=O - M(k,l) \end{array} \end{equation*}$
Add the new nodes to the open list
(15) $\begin{equation*} O=O \cup Step \left( M(i,j) \right) \end{equation*}$
1. Check if the open list is empty, if yes stop the algorithm with failure
  (16) $\begin{equation*} O=\emptyset \rightarrow STOP_{failure} \end{equation*}$
From the open list choose the node with the minimum of the cost function
(17) $\begin{equation*} (p,r)= \min_{arg} \left( f(i,j) | M(i,j) \in O \right) \end{equation*}$
1. Add the selected node to the close list and remove it from the open list
  (18) $\begin{equation*} \begin{array}{c} C=C \cup M(p,r) \\ O=O-M(p,r) \end{array} \end{equation*}$
2. Check if the selected nod is the target node, if yes stop search with success
  (19) $\begin{equation*} M(p,r) \equiv M(k_T,l_T) \rightarrow STOP_{succes} \end{equation*}$
3. if not, go to 2 and step the selected node
If the search algorithm has been closed with success, the path can be constructed from the close list. Create an empty Path list:
1. Add the target node to the Path list
  (20) $\begin{equation*} P = \{M(i,j)_T \} \end{equation*}$
2. Step the last added nod from the Path list and find the duplicate node from the close list which has the minimum cost function
  (21) $\begin{equation*} \forall M(k,l) \in Step \left( M(i,j) \right),M(k,l) \in C \rightarrow P = P \cup M(k,l) \end{equation*}$
3. Check if the last added nod is the start nod, if yes, flip the path list and end the algorithm with result
  (22) $\begin{equation*} M(i_S,j_S) \equiv M(k,l) \rightarrow RESULT \end{equation*}$
4. If not, go to 6.2

Compute the desired trajectory

Using the Path list (computed in the previous section) two additional steps are needed.

Firstly, the nodes $M(i,j)$ must be transformed in corresponding point $P(x,y)$ on map (reverse of the discretization)

(23) $\begin{equation*} \begin{array}{c} x=i \cdot \Delta l + x_{min} \\ y=l_y - j \cdot \Delta l + y_{min} \end{array} \end{equation*}$

Secondly, the trajectory defined by points must be smoothed. This is an optimization problem to move the points in a new location in which they minimize two hypothetical distances: between neighboring points in the new position, and the original position of the points and the current position of points. Figure 15 illustrate this concept.

path_planning_fig_16_Smoothing the trajectory — Figure 16. Smoothing the trajectory

A hypothetical attraction between the neighboring points is use to move the points closer. This movement of thiese points is compensated by reducing in parallel the distance between the initial position of points and the current position of points. The second interaction tends to preserve the original positions of the points.

Using this idea, an iterative (gradient descendent) loop can be constructed for each point contained in the Path list, less the start and target point. The loop is stopped after a maximum number of iteration (initially imposed) or, if the points displacement will decrees under an (initialy imposed) limit:

(24) $\begin{equation*} \begin{array}{c} P_i(k+1)=P_i(k)+\alpha(P_i(0)-P_i(k)) \\ P_i(k+1)=P_i(k) +\beta (( P_{i-1}(k)-P_i(k))+ ( P_{i+1}(k)-P_i(k))\end{array} \end{equation*}$

where: $P_i(k)$ is the point i at k iteration, $\alpha$ and $\beta$ are the gradient descend coefficients.

The final result of path planning is a trajectory defined like a list (set) of points. The trajectory links the start point with the target point, it is the shortest path between these point, it is avoiding the obstacles and it is a smooth curve.

Map representation

Map discretization

A star (A*) algorithm

Compute the desired trajectory

**A star (A^*) algorithm**