A real-time multi-constraints obstacle avoidance method using LiDAR

LiDAR is mounted on the top of the robot and used to detect environment information in a 2D scanning plane, with a maximum scanning range. Note that the 2D LiDAR can only scan one plane. In practice, if the height of the obstacle is less than the height of the LiDAR, it cannot be detected. Therefore, the installation height of LiDAR needs to weigh the actual application environment and should not be fixed. At each scanning period, LiDAR gives 360-degree surrounding scanning and generates $N_{p}$ data points, also called lase-point cloud. Scanning data can be expressed as $\left(d_{i}^{t},\theta_{i}^{t}\right)$, where the $d_{i}^{t}$ and $\theta_{i}^{t}$ respectively denotes the relative distance and orientation centered on the robot, with the LiDAR scanned objects for $i=1,\ldots,N_{p}$ at the time $t$. As shown in figure 1, the original data of the LiDAR in polar coordinates is shown, where the green dots indicate the obstacles, $R$ represents the LiDAR scanning radius, and the white triangle denotes the robot.

Within the scanning area, every object can be simplified as multiple points. If there are obstacles, a series of point sets are generated to represent obstacles; otherwise, there are no data points. It is not convenient to use the original LiDAR data directly to represent surroundings because of the data points are discrete. A single point does not indicate an obstacle, but clusters the data points. There have been several studies focusing on clustering methods for point cloud data. In this paper, the scanning area is equally divided into $N_{m}$ sectors, and each sector is given a unique number $id\left(id=1,\ldots,N_{m}\right)$. Generally, that is meaningful to equally divide the scanning area, the smaller the sector division, the more flexible the path planning, while the amount of calculation is increased. All data points will be distributed in the sector corresponding to the $\left(d_{i}^{t},\theta_{i}^{t}\right)$ within the scanning area. In the way, only need to pay attention to which sectors are distributed with data points; the remaining sectors belong to collision-free space. A local grid map is constructed within the LiDAR scanning area, which is used to analyze the current environment.

The scanning area is further divided into small grid cells that can accurately locate the obstacle in the specific grid units. Because roughly creating a grid map in a circular scanning area of radius $R$ may produce some incomplete grid units, making it impossible to assign a corresponding coordinate position to some grid units. In order to solve the problem that the incomplete grid cells cannot be assigned coordinates, in this paper, the largest square space in a circular scanning area is used to replace the LiDAR scanning area, and then the square is divided into several grid cells. So each clustered data points can be allocated in a specific grid.

In this paper, the robot moves on the horizontal ground and obtains real-time pose information by reading the odometry. Its position and orientation can be simplified as $P_{robot}^{t}\left(x_{r},y_{r},\psi_{r}\right)$ in the 2D space at time $t$, and $\psi_{r}$ indicates the yaw angle. In fact, given the error of the odometry, the application environment is limited to small indoor scenes. It can be easily calculated that grid units position $P_{grid}^{t,i}\left(x_{i},y_{i}\right)$ based on the $P_{robot}^{t}\left(x_{r},y_{r},\psi_{r}\right)$ at time $t$.

In the grid map, an expansion coefficient is set for each obstacle, and if a grid unit is occupied by an obstacle, its adjacent eight grid units are not included in the collision-free space. However, how to logically re-plan an optimal sub-path among these collision-free grids to achieve the avoidance obstacles at a lower cost is the problem to be solved in this paper.

The robot can re-plan an optimal path online toward sub-goal and successfully avoid unexpected obstacles or threats at a lower cost. As a sub-goal selected at a specific moment, it employs the key criterion for convergence to the goal position. On the other hand, the sub-goal is locally optimal and globally convergent, because the cost function based on multi-constraints is capable of analyzing thoroughly the detection information of the LiDAR. Concerning known local environment information, each grid cell in collision-free space is estimated by the cost function, the result of the cost function denotes the cost of the corresponding grid cell as a sub-goal, the smaller the value, the smaller the cost.

The significance of the customized cost function consisting of $\!ds\left(P_{grid}^{t,j},P_{goal}\right)\!,\!\theta_{1}\left(P_{{robot}}^{t},P_{grid}^{t,j}\right)\!$ and $\!\theta_{2}\left(P_{grid}^{t,j},P_{{goal}}\right)\!$ is to comprehensively evaluate the real-time information, including the current pose information of the robot, the LiDAR scanning area, and the goal position, to generate the sub-goal. $P_{goal}$ is the goal position information, $P_{robot}^{t}$ denotes the robot pose information at the time $t,$ and $P_{{grid}}^{t,j}$ indicates the position information of the $j$-$th$ grid unit in collision-free space at the time $t$.

$\!ds(P_{grid}^{t,j},P_{goal})\!$ is used to calculate the euclidean distance between $P_{{grid}}^{t,j}$ and $P_{{goal}}$ at time $t$, an essential contribution $\!ds(P_{grid}^{t,j},P_{goal})\!$ is to ensure the global convergence of sub-goal. In the specific implementation, the scanning information is maximized to achieve the local optimal obstacle avoidance result. Only the most marginal grid units in the collision-free space are selected, which can promote the processing efficiency. The higher the distance to the obstacle, the more flexible the path to avoid the obstacle, which means a more substantial area around the robots are unobstructed.

$\!\theta_{1}(P_{robot}^{t},P_{grid}^{t,j})\!$ returns the azimuth angle from $P_{robot}^{t}$ to $P_{{grid}}^{t,j}$ at time $t$, which is angle with the yaw$(\psi)$ of robots. As one of the constraint factors, $\!\theta_{1}(P_{robot}^{t},P_{{grid}}^{t,j})\!$ embodies the angle deviation relationship between the robot and the grid cells in collision-free space.

$\!\theta_{2}(P_{grid}^{t,j},P_{{goal}})\!$ generates the azimuth angle from $P_{{grid}}^{t,j}$ to $P_{{goal}}$ at time $t$, which is used to indicate the magnitude of deviation of the selected grid unit from the goal position. The additional contribution of constraints factors $\!\theta_{1}(P_{robot}^{t},P_{{grid}}^{t,j})\!$ and $\!\theta_{2}(P_{grid}^{t,j},P_{{goal}})\!$ are to ensure the smoothness of the path.

The proposed cost function is defined as Eq.(2):

$\alpha,\beta$ and $\omega$ is the weight coefficient respectively.

The above three constraints are not directly added after calculation, but are normalized first, and then added. Since the cost function is composed of multiple constraints, and different constraints represent information in different dimensions, such as distance and different angle. To prevent a certain constraint from being too dominant in the cost function,and the normalized method is used to smooth the constraint in this paper.

Since the latest local known information is different each time, the number of available grids is different each time.

Finally, with the appropriate weight parameters, the normalized constraints are introduced into the cost function to estimate the cost for each available grid cell and choose the minimum cost.

Based on the latest local known information each time, the grid cell corresponding to the minimum cost value is selected as the sub-goal $P_{sg}(x,y)$, which satisfies the optimization criterion such as distance, time, and cost in a limited scanning area, so the robot is capable of immediately responding to any moving obstacles within the scanning area of LiDAR. In the grid map of the scanning area, the path of the robot is connected by multiple grid cells. Although the robot is capable of moving in any direction in a 2D plane, the motion model of the robot is further defined in this paper, assuming that the robot is only allowed to move along the front, rear, left, right, and diagonal lines between adjacent grids. The resolution of the grid cell is small enough not to affect the smoothness of the path. Therefore, how to quickly re-plan a sub-path from the current robot position to the sub-goal position is urgently needed. Generally, a straight motion is considered as the primary solution because straight-line motion usually indicates that the path is the shortest, while the shortest path is not necessarily the optimal path in many practical problems.

The improved ACO, in this paper, is used to plan optimal sub-path from the latest robot position to the latest defined sub-goal, The robot moves one step along the sub-path, then sets the current position as the start point, extracts the local sub-goal position based on the latest local known information, and calls ACO to re-plan the sub-path to avoid moving obstacles until it reaches the goal position. The flowchart of the proposed method is shown in figure $2$.

Note that the next sub-goal position is calculated if and only if the robot reaches the sub-goal point, which will cause a delay in updating the environmental information, and the robot will not be able to respond to moving obstacles before reaching the sub-goal position.

The ACO algorithm has the characteristics of global optimization, parallelism, and strong robustness, and has been widely used in offline path planning problems. In this paper, the ACO algorithm is used to online re-plan the optimal sub-path of the robot from the current position to the sub-goal position in the grid map of the scanning area. The fundamental definition of the ACO algorithm is typically given by Eq.(5):

where the ${P}_{ij}^{k}(t)$ is the transition probability of ant $k$ moving from $i(x_{i},y_{i})$ to $j(x_{j},y_{j})$ at the time $t;\tau_{ij}(t)$ denotes the pheromone from $i(x_{i},y_{i})$ to $j(x_{j},y_{j})$ at the time $t$; $\phi$ is the relative importance of pheromone, $\gamma$ is the relative importance of heuristic factor, $J_{k}(i)$ is a collection of positions that allows the ant to walk next; and $\eta_{ij}(t)$ represent the heuristic factor from $i(x_{i},y_{i})$ to $j(x_{j},y_{j})$ at the time $t$,

$d_{ij}$ is the distance from $i(x_{i},y_{i})$ to $j(x_{j},y_{j})$.

After all ants complete once iteration, the pheromone update method is as follows:

where the $\rho\in(0,1)$ is the rate of pheromone evaporation, and $1-\rho$ is the residual pheromone factor; $\Delta\tau_{ij}(t)$ is the amount of deposited pheromone from $i\left(x_{i},y_{i}\right)$ to $j\left(x_{j},y_{j}\right)$ at time $t$, and $\Delta\tau_{ij}^{k}(t)$ is the amount of deposited pheromone from $i\left(x_{i},y_{i}\right)$ to $j\left(x_{j},y_{j}\right)$ by ant $k$ at time $t$; $Q$ is positive constant; $\mathbf{m}$ and $N$ are the set of ants colony and the number of iterations, respectively; $L_{k}$ indicates the path length by ant $k$ in this iteration.

Based on the traditional ACO algorithm, to enhance the stability of robot movement, corner heuristic information is introduced into the path transition probability function to reduce the number of large corners and the number of corners in the path. The improved path transition probability function is as follows:

$v_{ij}(t)$ is the corner heuristic function from $i\left(x_{i},y_{i}\right)$ to $j\left(x_{j},y_{j}\right)$ at time t. $v_{ij}(t)$ and $\theta_{ij}$ are inversely proportional.

In the pheromone update phase, on the one hand, the traditional ACO algorithm updates the pheromone of all ants, including the worst ants, which will be misleading for the next generation. On the other hand, only the length of the path is considered, as shown in Eq. (7), (8) and (9). To coordinate the smoothness of the path and the convergence speed of our method. each ant gets a score based on its path length and number of corners, and ranks the score in descending order. The improved pheromone update rule is that the pheromone increment is determined by weighting the path length and the number of path corners, which can ensure the smoothness of the sub-path. At the same time, only some good ants with higher scores are updated to avoid the impact of the worst ants on the offspring ants, thereby improving the convergence speed of the algorithm.

$w_{k}$ is the weighted score of the path length and number of corners of ant $k$, $\mathrm{S}\left(w_{k}\right)$ is the sort function to sort $w_{k}$ from large to small, $\delta$ and $\zeta$ are the positive weight coefficients, $T_{k}$ indicates the number of corners of the path generated by ant $k$. $\mathrm{rank}$ is the ant order after reordering

The implementation steps of the improved ACO method are as follows:

Step 1: Algorithm initialization, $P_{robot}^{t}$ and $P_{sg}(x,y)$ are initialized to the ant colony start position and sub-goal position respectively, and the search space is the known grid map of the scanning area.

Step 2: Calculate the path transition probability of each ant according to the $\operatorname{Eq}(10)$ and choose the next feasible position $j$ according to the roulette rule. If $j$ is the sub-goal point, the step 3 is performed; otherwise, still, execute the step 2.

Step 3: After all ants have completed an iteration, $H_{n}^{k}$ records the path trajectory generated by ant $k$ in the $n$-$th$ iteration. Find the ant in $H_{n}^{k}$ that does not reach the sub-goal point as the set $\mathbf{U}$. Randomly select an ant $s$ to re-modify its path trajectory in this iteration.

$f\left(H_{n-1}^{k}\right)$ is the optimal path before the $n$-$th$ iteration. When set $U$ is valid, randomly select one ant $s$ in $U$ and assign $f\left(H_{n-1}^{k}\right)$ to it ; if all ants reach the target point, randomly select anyone to assign.

Step 4: According to the $\operatorname{Eq}(14)$ to calculate the score of each ant, and then update the pheromone of the selected ant according to the $\operatorname{Eq}(13)$.

Step 5: Determine whether the number of iterations reaches the maximum number of iterations $N$ . If it does, the step 6 is performed. Otherwise, steps 2 to 5 are executed cyclically.

Step 6: The optimal sub-path described by $\operatorname{Eq}(17)$, $l_{1}$ represents the position of the grid cell corresponding to $P_{robot}^{t}$, $l_{\mu}$ is the position of the grid cell corresponding to sub-goal $P_{sg}(x,y)$.

adding $P_{path}\left(l_{1},l_{2},\ldots,l_{\mu}\right)$ to the desired path, the robot moves one step along the sub-path to the next position $l_{2}$. Repeat the steps 1 to 6 until the robot reaches the goal position.

The convergence speed of improved ACO algorithm is shown in figure $3$. The black solid line shows the convergence speed of the improved ACO algorithm. The concept of corners is involved in both the transition probability phase and the pheromone update phase to build up the smoothness of the path. Update only some good ants to avoid the impact of the worst ants on the offspring ants. It can be seen that the improved ACO has faster convergence speed. At the same time, it should be pointed out that it is not advisable to assign multiple ants in once iteration, which will increase the probability that the algorithm falls into the local minima.

Before the real experiment of the robot, a simulation of the algorithm is implemented in MATLAB to compare the effect of different parameters on the results. In order to simulate the actual environment more realistically, an obstacle map is loaded by a grid map in the simulation program, which corresponds to a 40m square real area. Black areas indicate the collision-free area, and the white grids denote the obstacles, which also called occupied units. In the simulation of this paper, each grid cell in the grid map corresponds to a 1.5m square area. Eight grid cells adjacent to the occupied unit are excluded from collision-free space as a safe distance.

Refer to the above simulation results, the robot experiment platform of this paper is shown in figure 7(a). The proposed algorithm needs real-time information and process data online, including robot state, real-time environment information. So a $360^{\circ}$ LiDAR and an onboard computer installed on the EAI robot. EAI is a differential drive mobile robot with a maximum speed of 0.8 m/s and the drive wheel diameter of 125 mm, and this differential drive allows the implementation and measurements of path-planning algorithms. The architecture of the robot platform is illustrated in detail in figure 7(b). According to the actual experiment environment, the installation height of the LiDAR in two experiments is 0.6m. Also, the LiDAR scans the environmental information around the robot in real-time at 5Hz and publishes it. The onboard computer runs complex algorithms at high frequency, including LiDAR data collection and processing, obstacle analysis, and online path planning.

In order to highlight the accuracy and practicality of the proposed algorithm to solve unexpected threats or uncertain obstacles, the first experiment scene was set in a real narrow corridor of 2.1m wide with static obstacles consisting of multiple cubes of $0.6\times 0.6\times 0.9\mathrm{m}$. EAI robot radius is 0.203m and a safe distance of 0.2m from the obstacle. Further, the detection distance of the LiDAR is set to 1.2m to reduce the response time of the robot in scene 1. Scenario 2 is selected to verify the availability of the proposed algorithm in an unstructured, dynamic environment. Two real experimental scenarios with different effects are shown in figure 8.

The onboard computer records the trajectory data of the robot, the real experiment environment is displayed in a certain proportion in MATLAB, and the saved robot trajectory is drawn at the same time. The complex U-shaped obstacles and L-shaped obstacles are used to verify the robustness and practicality of the proposed algorithm in figure 8(a), the closest distance between obstacles is about 0.9m, and the maximum width is 1.2m. Due to the extremely narrow corridor, the robot speed is 0.2m/s. In fact, the robot analyzes the LiDAR data in real-time to make the best decision in line with the current state. Similarly, the implementation of the algorithm is presented in the form of multiple consecutive instants in figure 9.

In the real experiment, a camera is mounted on the top of the robot and records the motion of the robot in video form. The robot successfully avoids the obstacle and reaches the goal position with minimal cost and smooth route, as shown in figure 10. However, the other methods fail to work well with LiDAR data, and the path length is long and not smooth enough.

In order to analyze the experiment results quantitatively, table 3 lists the results of each algorithm running five times at a speed of 0.2m/s. The recorded video can be downloaded from the link in Appendix A.

It can be seen from table 3 that the method proposed in this paper takes less time, which means that the convergence speed is faster. Also, the smooth characteristics of the path have been compared in Table 2. Thus, considering time, path length, smoothness, and convergence properties, that the path generated by the proposed algorithm is not only globally convergent but also locally optimal.

As shown in figure 8(b), experiment 2 aims to verify the feasibility and robustness of the proposed algorithm under an unstructured, dynamic scenario with sudden external disturbances. In real experiments, the robot moves at a speed of 0.4 m/s and 0.6 m/s without external interference. Then, when the robot moves again at a speed of 0.6 m/s, a plurality of unexpected moving obstacles appears around the robot to hinder its movement. The trajectory of the robot is converted to convergence to illustrate the robustness of the algorithm, as shown in figure 11. At the same time, the corresponding video can be downloaded from the link in Appendix B.

In Figure 11, in the absence of external disturbance, the convergence curve is relatively smooth, corresponding to the trajectory generated by the robot moving at a speed of 0.4 m/s and 0.6 m/s, respectively. For external disturbances, although there is jitter in the convergence curve due to sudden external disturbances, the jitter is minimal, which further demonstrate that our method can effectively deal with dynamic obstacles under an unstructured, changing scenario.