*3.2. Map Building*

A point cloud can be useful for environment visualization, object detection or feature determination, and semantic segmentation [17,18], but it is not suitable for path planning. Therefore, there is a need to process it into a map. In our approach, a grid-based traversability [19] map was chosen. This is a very popular representation of the environment, and it is also very convenient for path planning. It can be used for algorithms such as A\* [20], the diffusion method [21], and reinforcement learning path planning [22].

Two types of lattices were used: square and hexagonal. The former was used as a reference, and the latter is a new approach. There are reasons to believe that the latter is better. As shown in [1], straight lines, circles, and polynomials are statistically better represented on a hexagonal grid than on a square grid. This is important because straight lines are very common on maps, as buildings are composed of such lines. A robot's path, due to the need to avoid obstacles, can resemble a polynomial graph.

In the study described in [1], the quality of the representation of different curves (lines, circles, and polynomials) for square and hexagonal grids was compared. For lines, this was performed as follows. First, the parameters of the straight line were randomly generated. Then, it was drawn on both types of grids (with the same resolution). After this, the average distance between each cell belonging to the representation and the original straight line was calculated. This experiment was repeated 10,000 times, and the average error for all these lines was calculated. The quality of circles and polynomials was checked analogously. Summary results are presented in Table 1. As one can see, for each type of curve, the hexagonal grid represented it better.

**Table 1.** Mean error of the representation of different curves on the square and hexagonal lattice.


To create a grid-based map from a point cloud, we need discretization. Therefore, when creating a map on a square lattice, each point was assigned to a cell as follows:

$$\begin{aligned} \mathbf{x}\_{\\$} &= \lfloor \mathbf{x}/w \rfloor \\ y\_{\text{s}} &= \lfloor y/w \rfloor \end{aligned} \tag{3}$$

where:

> *x*, *y*—original coordinates in the point cloud;

*xs*, *ys*—discrete coordinates in the square grid map;

*w*—width of one cell.

Obviously, several points may be assigned to one cell. The height of this cell is calculated based on the average, according to the formula:

$$z\_s = \frac{1}{n} \Sigma\_{i=1}^n z\_i \tag{4}$$

where:

> *zs*—height of the cell in the square grid map;

*n*—number of points assigned to the cell;

*zi*—original z coordinates for all points, which were assigned to a given cell.

 The results can be seen in Figure 8. The scale near the map determines the elevation.

The first step necessary to build a map on a hexagonal grid is to convert the Cartesian coordinates to the coordinates used for hexagons. When creating a map, the so-called cube coordinate system [23,24] (this can be seen in Figure 9) is much more useful than the ASA. Therefore, the former was used when generating the map, and the latter was used when

planning the path (a simple way to switch between these coordinate systems is given at the end of this subsection). The transformation between the Cartesian and cube systems is given by the following formulas:

$$\begin{array}{l} \mathbf{x}\_{\varepsilon} = \mathbf{x} \\ y\_{\varepsilon} = \frac{\sqrt{3}}{2}\mathbf{y} - \frac{1}{2}\mathbf{x} \\ \mathbf{y}\_{\varepsilon} = -\mathbf{y}\_{\varepsilon} - \mathbf{x}\_{\varepsilon} \end{array} \tag{5}$$

where:

> *x*, *y*—original Cartesian coordinates in the point cloud;

*xc*, *yc*, *ζc*—continuous cube-hexagonal coordinates.

The next step is to discretize the variables. The following algorithm was used for this purpose [23]. First, auxiliary variables are calculated analogously to the square grid:

*x*

*y*

ˆ

ˆ

ˆ

$$\begin{array}{l} \mathfrak{X}\_{\hbar} = \lfloor \mathfrak{x}\_{\mathfrak{c}} / w \rfloor \\ \hat{y}\_{\hbar} = \lfloor y\_{\mathfrak{c}} / w \rfloor \\ \hat{\xi}\_{\hbar} = \lfloor \tilde{\zeta}\_{\mathfrak{c}} / w \rfloor \end{array} \tag{6}$$

where *<sup>x</sup>*<sup>ˆ</sup>*h*, *y*<sup>ˆ</sup>*h*, ˆ *ζh*—discretized hexagonal coordinates. If the variables satisfy the following condition *x*ˆ*h* + *y*ˆ*h* + ˆ *ζh* = 0, they become the final discrete coordinates on the hexagonal grid. Otherwise, it is necessary to correct the coordinates based on the following Algorithm 1 ({•} denotes the fractional part).

#### **Algorithm 1** Discretization of hexagonal coordinates.

```
if xˆh + yˆh +
              ˆ
             ζh = 0 then
  xh ← xˆh
  yh ← yˆh
  ζh ←
         ˆ
         ζh
else
  if {xc/w}≥{yc/w} and {xc/w}≥{ζc/w} then
     xh ← −yˆh − zˆh
    yh ← yˆh
     ζh ←
           ˆ
           ζh
  else if {yc/w}≥{xc/w} and {yc/w}≥{ζc/w} then
     xh ← xˆh
    yh ← −xˆh −
                   ˆ
                   ζh
     ζh ←
           ˆ
           ζh
  else
     xh ← xˆh
    yh ← yˆh
     ζh ← −xˆc − yˆh
  end if
end if
```
The height is calculated analogously as before. The results can be seen in Figure 10. In Figure 11, a comparison between the square and hexagonal map is shown. The image shows a close-up of the same southern section of the map, so that the differences between the lattices can be seen more clearly.

**Figure 8.** The 2.5D map on a square grid. The color indicates at what height an obstacle has been detected within the cell. The scale is placed on the right side of the figure. The black rectangle marks the section shown in the enlarged version in Figure 11.

**Figure 9.** Cube coordinate system with an example point.

**Figure 10.** The 2.5D map on a hexagonal grid. The color indicates at what height an obstacle has been detected within the cell. The scale is placed on the right side of the figure. The black rectangle marks the section shown in the enlarged version in Figure 11.

**Figure 11.** *Cont*.

**Figure 11.** Close-up comparison of the grids.

The last step is a conversion between the cube coordinates and ASA according to the formula:

$$\begin{cases} a = \mathbf{x}\_{\text{h}} \mod 2\\ r = \lfloor \mathbf{x}\_{\text{h}}/2 \rfloor \\ c = -\zeta\_{\text{h}} - \lfloor (\mathbf{x}\_{\text{h}} + 1)/2 \rfloor \end{cases} \tag{7}$$

## **4. Path Planning**

In our conference paper [1], we showed that the hexagonal map is useful during path planning. Figure 12 presents collision-free paths computed based on hexagonal and square grids.

**Figure 12.** Path planning using square grids (**a**) and hexagonal grids (**b**). Black lines represent fragments of obstacles; yellow cells represent the planned path [1].

In this article, we sugges<sup>t</sup> using the diffusion method for path planning [21]. The method is not effective for large areas with few obstacles. For small areas with a large number of obstacles, the path generation time is shorter than for other methods (potential field, RRT). An additional advantage of the approach is that we can easily take into account the cost of driving over different surfaces. Therefore, we decided to use the diffusion method. In this approach, collision-free path planning is performed on a hexagonal grid. Obstacle-free cells represent the possible robot positions (states). Two states are distinguished: the robot position (*cR*) and the goal position (*cG*).

In the first step, a diffusion map is initialized. A big value is assigned to the cell, which represents the goal position (*cG*), and the values 0.0 are attached to other cells.

In classical path planning systems, we divide cells into two classes: free from obstacles and occupied. For 2.5D maps, class membership is determined by thresholding. A cell is occupied if the observed height exceeds a certain threshold, and free of obstacles otherwise. In classical systems, we look for the shortest path, but we look for the path with the shortest travel time in many cases. The travel time (robot speed) can be related to the type of ground, distance to obstacles, etc. To solve this problem, we introduced an additional parameter cf. In the current version of the system, the value of this parameter is zero for free cells and infinity (very large integer) for occupied cells. In future works, we want to build a surface recognition system and then the parameter will be fully used. In the case of a perfectly smooth surface (asphalt), the value of this parameter equals 0.0. In the case of uneven terrain, the value is increased proportionally to the increase in the cost (time) of movement.

For each unoccupied cell (*caij*), the value (*vaij*) is calculated according to the formula:

$$\upsilon\_{\rm aij} = \max\_{c\_{\rm ekl} \in N\_{\rm aij}} (\upsilon\_{\rm ekl} - cf\_{\rm ekl} - dist(c\_{\rm ekl}, c\_{\rm aij})) \tag{8}$$

where:

> *c fekl*—the value of the cost function assigned to the cell *cekl*, 0 ≤ *cekl* ≤ ∞;

*Naij*—neighborhood of the cell *caij*.

This process continues until stability is established.

During the next step, the list of cells is generated. The first cell represents the robot position. The next one is indicated by the neighbor of (*cR*) with a maximum value of *vakl*. The process continues until the cell with the maximum function value v is reached.

The method has several advantages:

