Improved A* Algorithm for Path Planning of Spherical Robot Considering Energy Consumption

. 2023 Aug 11;23(16):7115. doi: 10.3390/s23167115

Algorithm 1 Improved A* Algorithm for Path Planning of Spherical Robot Considering Energy Consumption

/*Initialization*/

Load data of the 3D grid map and parameters of the spherical robot;
Set the start point $(x_{1}, y_{1})$ and the target point $(x_{z}, y_{z})$ ;
Create an open list and a closed list;
Introduce the start point into the open list and initialize the closed list empty;
The number of points in the open list is a, and the number of points in the closed list is b;

/*Iterative search*/

6.
while a ≠ 0
7.
Find the next point n $(x_{n}, y_{n})$ with the minimum f(n) in the open list, remove it from open list and add it into the closed list;
8.
if ( $x_{n} \neq x_{z}$ ) or ( $y_{n} \neq y_{z}$ )
9.
Add all passible neighbor points of point n into a set called subs.
10.
if subs is empty
11.
Go to Step 6;
12.
end if
13.
Select a new point (n + 1) from subs.
14.
Calculate distance and angle between point n and point (n + 1).
15.
Calculate motor torque $T_{M}$ based on force analysis.
16.
Calculate the energy consumption of temporary path based on ECEM.
17.
if point (n + 1) is in the open list
18.
if there is lower cost to reach point (n + 1)
19.
renew f(n);
20.
end if
21.
end if
22.
if point (n + 1) is in the open list
23.
if there is lower cost to reach point (n + 1)
24.
renew f(n);
25.
end if
26.
end if
27.
Add point (n + 1) into open list;
28.
Calculate $H^{'} (n + 1)$ based on DEM;
29.
Calculate $f (n + 1) = k E (n + 1) + H^{'} (n + 1)$ ;
30.
Go to Step 6;
31.
end if
32.
end while
33.
Add point n into closed list;
34.
Find parent points in closed list one by one;
35.
Output the best path and its cost value;
36.
Post-process results and visualization