An Improved Hybrid Path Planning Algorithm in Indoor Environment

Heuristic factor plays a key role in optimal path planning, which can guide the search direction of A-star algorithm. When h(n) = 0, A-star algorithm is equivalent to Dijkstra algorithm; When the estimated generation value of the heuristic factor h(n) is less than the real cost value, the search range of A-star algorithm becomes larger and the number of search nodes becomes more, which can ensure the generation of the optimal path; When the estimated generation value of the heuristic factor h(n) is greater than the actual cost value, the search range of A-star algorithm becomes smaller, the number of search nodes becomes less, which cannot ensure the generation of the optimal path. Seeking more realistic path planning results, the optimized heuristic factor h(n) was introduced as follows: (3) hn=2ys−yend+xs−xend−ys−yend,ys−yend≥xs−xend2xs−xend+ys−yend−xs−xend,ys−yend<xs−xend h\left( n \right) = \left\{ {\matrix{ {\sqrt 2 \left| {{y_s} - {y_{end}}} \right| + \left| {{x_s} - {x_{end}}} \right| - \left| {{y_s} - {y_{end}}} \right|,\left| {{y_s} - {y_{end}}} \right| \ge \left| {{x_s} - {x_{end}}} \right|} \hfill \cr {\sqrt 2 \left| {{x_s} - {x_{end}}} \right| + \left| {{y_s} - {y_{end}}} \right| - \left| {{x_s} - {x_{end}}} \right|,\left| {{y_s} - {y_{end}}} \right| < \left| {{x_s} - {x_{end}}} \right|} \hfill \cr } } \right.

Figure 2.

Since the traditional A-Star algorithm has an unsmooth path problem in the path planning process, the optimization method of tangent point of angle bisector was introduced to optimize the A-star algorithm, which helps generate smoother paths. The optimization model is shown in Fig. 2.

Assuming that the initial node position of the mobile robot is A(x₀, y₀), the turning point is smoothed in turn, and the turning points B and D are smoothed in turn until reach the end point A(x_i, y_i). The following are the steps for path optimization:

Step 1: determine if the three points are collinear, that is, if the node is a turning point. Make a vertical line crossing the angle bisector at the previous node of the turning point. In the figure above, the slope of edge AB is k₁, the slope of edge BC is k₂, and the slope of the angle bisector of the turning angle is denoted as k′.

Where: (4) k1=y2−y1x2−x1,k2=y3−y2x3−x2,kn=yn+1−ynxn+1−xn {k_1} = {{{y_2} - {y_1}} \over {{x_2} - {x_1}}},{k_2} = {{{y_3} - {y_2}} \over {{x_3} - {x_2}}},{k_n} = {{{y_{n + 1}} - {y_n}} \over {{x_{n + 1}} - {x_n}}}

According to the slope relation formula k′−k11+k1⋅k′=k2−k′1+k2⋅k′ {{k^\prime - {k_1}} \over {1 + {k_1} \cdot k^\prime }} = {{{k_2} - k^\prime } \over {1 + {k_2} \cdot k^\prime }} , the expression of the slope k1′ k_1^\prime of BO₁ is as follows: (5) k′2+21−k1k2k1+k2⋅k′−1=0 {k^{'2}} + {{2\left( {1 - {k_1}{k_2}} \right)} \over {{k_1} + {k_2}}} \cdot {k^\prime } - 1 = 0

Then, the coordinates of the intersection point O₁ is: (6) x0=x1+k1y1+k1k′x2−y21+k′k1y0=k′x−x0+y \left\{ {\matrix{ {{x_0} = {{{x_1} + {k_1}{y_1} + {k_1}\left( {{k^\prime }{x_2} - {y_2}} \right)} \over {1 + {k^\prime }{k_1}}}} \hfill \cr {{y_0} = {k^\prime }\left( {x - {x_0}} \right) + y} \hfill \cr } } \right.

Step 2: make a circle with the vertical length as the radius through the intersection O₁, and judge whether there is an intersection with the tangent circle. The tangent circle equation is: (7) x−x02+y−y02=r02 {\left( {x - {x_0}} \right)^2} + {\left( {y - {y_0}} \right)^2} = r_0^2

The radius r₀ of the tangent circle is represented as: (8) r0=x02+x12+y12+y02−2x0x1−2y0y1 {r_0} = \sqrt {x_0^2 + x_1^2 + y_1^2 + y_0^2 - 2{x_0}{x_1} - 2{y_0}{y_1}}

Step 3: determine whether there is a next turning point. If it exists, return to step 1; Otherwise, replace the distance between nodes with an arc.

Step 4: determine whether the optimized path contains every node from the path planned by A-star algorithm. If so, optimization process completes; Otherwise, return to step 1.

The following is a simulation experiment of the improved A-star algorithm on the grid model in Fig. 1 in MATLAB, where each grid in the abscissa and ordinate represents a distance of 1m. The verification results are as follows:

Figure 3.

As evident in Fig. 3(b), compared to Fig. 3(a), the red trajectory was obviously smoothed and optimized in the trajectories of (12,12), (26,30) two meshes. By comparing various indicators in Table 1, it is evident that the improved A-Star algorithm effectively eliminated turning points, shortened the path length, improved path smoothness, reduced the time required to search for paths, and greatly reduced the number of nodes that need to be expanded during the path planning process. This shows that the A-Star algorithm modified with the heuristic factor surpasses the traditional A-Star in smoothness of path planning, reliability, and in judging actual trajectories.

Although the traditional artificial potential field algorithm is relatively easy to implement, when there are obstacles around target point and target point is within the range of obstacles influence, there may be a phenomenon where the target point is unreachable. To make the magnitude of the repulsive force change as the gravitational force changes with the distance, the repulsive force function was modified by referring to the gravitational potential field function. By introducing the relative position (m − m_end)^p, the repulsive force decreases continuously when it approaches the target point. The modified repulsion field function can be expressed as: (13) E→repm=12krep1m−mpobs−1r2m−mendp,m−mpobs≤r0,m−mpobs>r {\vec E_{rep}}\left( m \right) = \left\{ {\matrix{ {{1 \over 2}{k_{rep}}{{\left( {{1 \over {m - {m_{pobs}}}} - {1 \over r}} \right)}^2}{{\left( {m - {m_{end}}} \right)}^p}} \hfill & {,m - {m_{pobs}} \le r} \hfill \cr 0 \hfill & {,m - {m_{pobs}} > r} \hfill \cr } } \right.

In the above formula, k_rep is the repulsion field gain coefficient, m − m_pobs is the Euclidean distance between robot position and the obstacle, r is the repulsion radius, and p is an adjustable parameter. Based on the traditional gravitational potential field function, the relative position (m − m_end)^p was introduced and the repulsive field parameters are adjusted. This made the repulsive force of the mobile robot decreased as it approached the target point.

With the negative gradient of the repulsive force field function signifying its intensity, the repulsive force can be expressed as: (14) F→repm=−∇E→repm=F→rep1+F→rep2,m−mpobs≤r0,m−mpobs>r {\vec F_{rep}}\left( m \right) = - \nabla {\vec E_{rep}}\left( m \right) = \left\{ {\matrix{ {{{\vec F}_{rep1}} + {{\vec F}_{rep2}}} \hfill & {,m - {m_{pobs}} \le r} \hfill \cr 0 \hfill & {,m - {m_{pobs}} > r} \hfill \cr } } \right. Where: (15) F→rep1=krep1m−mpobs−1rm−mendpm−mpobs2 {\vec F_{rep1}} = {k_{rep}}\left( {{1 \over {m - {m_{pobs}}}} - {1 \over r}} \right){{{{\left( {m - {m_{end}}} \right)}^p}} \over {m - {m_{pobs}}^2}} (16) F→rep2=−p2krep1m−mpobs−1r2m−mendp−1 {\vec F_{rep2}} = - {p \over 2}{k_{rep}}{\left( {{1 \over {m - {m_{pobs}}}} - {1 \over r}} \right)^2}{\left( {m - {m_{end}}} \right)^{p - 1}}

The repulsive force F→rep1 {\vec F_{rep1}} points to the direction of the robot from the obstacle. The repulsive force F→rep2 {\vec F_{rep2}} points to the target point from the robot.

The force analysis after adjusting the repulsion field parameters is shown in Fig. 5, where the x-axis and y-axis represent the distance traveled, measured in meters.

Figure 5.

The artificial potential field being a blend of gravitational and repulsive fields, therefore, it is logical to divide the dynamic potential field into two sections: a gravity field based on relative velocity and a repulsion field based on relative velocity.

Gravity field based on relative velocity: in an indoor unstructured environment, a mobile robot faces the challenge of path planning in the presence of dynamic obstacles. By adding a relative velocity term to the traditional gravitational potential field function, a dynamic gravitational potential field function is formed. The function for the relative velocity gravitational field can be formulated as: (17) E→attm,v=12kattm−mendp+12kattv−vendp {\vec E_{att}}\left( {m,v} \right) = {1 \over 2}{k_{att}}{\left( {m - {m_{end}}} \right)^p} + {1 \over 2}{k_{att}}{\left( {v - {v_{end}}} \right)^p}

In the above formula, (v − v_end) is the relative speed of the mobile robot to the target point in space m. k_att is the gravitational gain coefficient, p is the adjustable parameter, m_end is the endpoint coordinates (x_end, y_end), m is the robot current point coordinates (x, y), and (m − m_end) represents the Euclidean distance between point m and the end point.

Then the gravity function is expressed as: (18) F→attm,v=−∇E→attm=p2kattm−mendp−1+p2kattv−vendp−1 {\vec F_{att}}\left( {m,v} \right) = - \nabla {\vec E_{att}}\left( m \right) = {p \over 2}{k_{att}}{\left( {m - {m_{end}}} \right)^{p - 1}} + {p \over 2}{k_{att}}{\left( {v - {v_{end}}} \right)^{p - 1}}

The gravity F→attm {\vec F_{att}}\left( m \right) points to the target point, and the size of the velocity gravity function F→attv {\vec F_{att}}\left( v \right) is related to the relative velocity between mobile robot and target point.

Repulsion field based on relative velocity: similar to the dynamic gravitational force field, the relative velocity term is also added in the construction of the dynamic repulsive field. Additionally, considering that there may be dynamic obstacles in the environment, the relative velocity based on obstacles is added. Thus, the repulsion field function based on relative velocity is as follows: (19) E→repm,v=E→repm+E→repv,m−mpobs≤r and 0≤vE→repm,m−mpobs≤r and 0≤v0,m−mpobs>r {\vec E_{rep}}\left( {m,v} \right) = \left\{ {\matrix{ {{{\vec E}_{rep}}\left( m \right) + {{\vec E}_{rep}}\left( v \right)} \hfill & {,m - {m_{pobs}} \le r\;and\;0 \le v} \hfill \cr {{{\vec E}_{rep}}\left( m \right)} \hfill & {,m - {m_{pobs}} \le r\;and\;0 \le v} \hfill \cr 0 \hfill & {,m - {m_{pobs}} > r} \hfill \cr } } \right.

In which, (20) E→repm=12krep1m−mpobs−1r2m−mendp {\vec E_{rep}}\left( m \right) = {1 \over 2}{k_{rep}}{\left( {{1 \over {m - {m_{pobs}}}} - {1 \over r}} \right)^2}{\left( {m - {m_{end}}} \right)^p} (21) E→repv=kattv−vpobse {\vec E_{rep}}\left( v \right) = {k_{att}}{\left( {v - {v_{pobs}}} \right)^e}

E→repm {\vec E_{rep}}\left( m \right) is the repulsion field function of the mobile robot in space m, E→repv {\vec E_{rep}}\left( v \right) is the velocity field function. v−vpobs \left( {v - {v_{pobs}}} \right) represents the moving speed of the mobile robot relative to the obstacle. e represents a unit vector. Then the repulsion function is: (22) F→repm,v=−∇E→repm,v=F→repm+F→repv,m−mpobs≤r and 0≤vF→repm,m−mpobs≤r and 0≤v0,m−mpobs>r {\vec F_{rep}}\left( {m,v} \right) = - \nabla {\vec E_{rep}}\left( {m,v} \right) = \left\{ {\matrix{ {{{\vec F}_{rep}}\left( m \right) + {{\vec F}_{rep}}\left( v \right)} \hfill & {,m - {m_{pobs}} \le r\;and\;0 \le v} \hfill \cr {{{\vec F}_{rep}}\left( m \right)} \hfill & {,m - {m_{pobs}} \le r\;and\;0 \le v} \hfill \cr 0 \hfill & {,m - {m_{pobs}} > r} \hfill \cr } } \right.

In which, F→repm=F→rep1+F→rep2 {\vec F_{rep}}\left( m \right) = {\vec F_{rep1}} + {\vec F_{rep2}} : (23) F→rep1=krep1m−mpobs−1rm−mendpm−mpobs2 {\vec F_{rep1}} = {k_{rep}}\left( {{1 \over {m - {m_{pobs}}}} - {1 \over r}} \right){{{{\left( {m - {m_{end}}} \right)}^p}} \over {m - {m_{pobs}}^2}} (24) F→rep2=−p2krep1m−mpobs−1r2m−mendp−1 {\vec F_{rep2}} = - {p \over 2}{k_{rep}}{\left( {{1 \over {m - {m_{pobs}}}} - {1 \over r}} \right)^2}{\left( {m - {m_{end}}} \right)^{p - 1}} (25) F→repv=−krepe {\vec F_{rep}}\left( v \right) = - {k_{rep}}^e

For the purpose of confirming the validity of the improved algorithm, a 30m×30m grid was constructed in the MATLAB environment, and two sets of experimental comparisons were carried out to analyze the improved artificial potential field algorithm, comparing it with the traditional version. The first set of experiments selected the grid map environment in Fig. 1 for simulation comparison. The unit of horizontal and vertical axes is m. As depicted in Fig. 6, these are the experimental results.

Figure 6.

The second set of experiments was simulated, using red circles to represent obstacles set up, to test the path planning effectiveness of the two algorithms when encountering local obstacles.

Fig. 6 reveals that in a static setting, the improved artificial potential field algorithm achieves smoother path planning and more precise obstacle detection, outperforming the traditional approach. However, in the dynamic environment portrayed in Fig. 7, the traditional algorithm fails to navigate to the target due to challenges in obstacle recognition. The lengthy path and extended runtime depicted in Fig. 7(a) are symptomatic of the complex obstacle environment, which poses a challenge for the traditional algorithm. Fortunately, the enhanced algorithm overcomes this limitation.

Figure 7.

As Table 2 indicates, by modifying the repulsion field parameters, the challenge of inaccessible target points was conquered, the oscillation phenomenon was effectively eliminated, compared with the traditional artificial potential field algorithm. Its running time was reduced by 13.92%, the path length was reduced by 4%. By way of comparative experiments, the efficacy of the improved artificial potential field algorithm was thoroughly validated.

The experiment used MATLAB to conduct simulation tests on the hybrid path algorithm. The experimental environment was also set as the grid map model depicted in Fig. 1. The experiment was carried out in both dynamic and static environments, grouped according to whether the obstacles are movable or not. The figure below demonstrates the experimental results.

The above three sets of experiments were conducted in a static environment to compare the path planning information generated by the improved A-star algorithm, the improved artificial potential field algorithm, DWA algorithm, and the hybrid algorithm. The comparison was based on their respective planning path lengths, search path durations and ability to handle dynamic obstacles.

As evident from Table 3 and Fig. 9, in the context of identical obstacles, the A-Star algorithm demonstrates an ability to plan a short path with rapid search speed during the path planning process, but it does not have the ability to dynamically avoid obstacles. Despite its longer planned path and increased search duration, the artificial potential field algorithm boasts the crucial capability of dynamically steering clear of obstacles, making it a practical choice for complex indoor navigation scenarios. By fusing the benefits of the two algorithms, the hybrid approach achieves not only a shorter search path and time but also the flexibility to handle dynamic obstacles effectively.

Figure 9.

The path planning experiment of the hybrid algorithm was conducted in a 30m×30m environment. In the dynamic environment, the parameters for obstacle avoidance were set to include a step increment of 0.1, a gravitational and velocity gain of 3, an obstacle detection radius of 3 units, and a repulsive gain of 5. The predefined obstacle was programmed to traverse a path from the point (12, 17) to (16, 17). The figure below demonstrates the experimental results.

The purple circles in Fig. 10(a) represent static obstacles that already exist, while the blue circles represent dynamic obstacles that appear randomly. In Fig. 10(b), green circles represent persistent dynamic obstacles, which move left and right between positions with ordinates of 3 and 17. These images compared the path under the hybrid algorithm with the static simulated global path to show the impact of the introduction of dynamic and moving obstacles on the return of the global path. In Fig. 10(a), the hybrid algorithm can effectively avoid dynamically appearing obstacles in the simulated indoor environment and return to the global path, ensuring the real-time nature of the path planning task. Mobile obstacles were introduced in Fig. 10(b). Mobile robot effectively avoided dynamic obstacles and returned to the global path, which effectively verified the effectiveness of the improved hybrid algorithm. Additionally, this enhancement significantly improved the real-time performance of path planning.

Figure 10.