[23]. The FTE algorithm calculates the heuristic using the
cost to go between the node in the currently expanding
search tree and the root (start or goal) of the opposite search
tree (bidirectional heuristic path algorithm (BHPA)), while
the FTF algorithm calculates the heuristic using the cost
to go between the node in the currently expanding search
tree and some nodes in the open list of the opposite search
tree (bidirectional heuristic FTF algorithm (BHFFA)). In
other words, the BHFFA finally selects the node with the
minimum heuristic among the nodes in the open list of
the opposite search tree. In general, as shown in [24], the
BHFFA is more efficient than the BHPA, although the
BHFFA may find a longer path and require more heuristic
calculations than the BHPA.
As mentioned above, a bidirectional heuristic search
is more efficient than a unidirectional heuristic search.
Further, a unidirectional heuristic search may find a path
with an error of a certain resolution at a goal, while a
bidirectional heuristic search can solve a problem with a
certain resolution at a goal. Nonetheless, a bidirectional
heuristic search still exhibits a discontinuity at the position
connecting both trees [7]. However, we propose an
algorithm to solve the discontinuity of connected points.
2.4 Overview of Our Approach
To accommodate the kinematics of car-like vehicles and
find paths even for narrow passages, we propose our spline-
and bidirectional-search-based hybrid A
∗
algorithm. We
can easily apply the kinematics to hybrid A
∗
using the
cubic Bezier curve, as in our previous study (spline- and
unidirectional-search-based RRT
∗
)[26], and the bidirec-
tional searching framework [7] to our path planning algo-
rithm. As mentioned in Section 2.2, because hybrid A
∗
searches a tree with a certain resolution, the conventional
path planner using bidirectional searching on a grid may
find a path where both trees expanded from the start and
a goal are connected with a certain resolution as shown
in Fig. 1b. However, we propose a bidirectional search-
ing algorithm that solves the discontinuity of connected
point using two concatenated cubic Bezier curves. Further,
to find paths in environments with narrow passages effi-
ciently rather than optimally, we use a method combining
our proposed heuristic based on the concept of a vector field
histogram (VFH) [25] and a conventional heuristic based on
the Dubins curve when only using a forward movement or
Reeds–Shepp curve [27] when including a rearward move-
ment. To compute a base heuristic using the Dubins or
Reeds–Shepp curves, the FTF algorithm is utilized. Finally,
we apply our collision-verification algorithm mentioned in
Section 2.1 to the abovementioned bidirectional heuristic
search framework.
3 Proposed Algorithms
In this section, we explain the procedure for generating
motion primitives and tree connections to satisfy the G
2
continuity and introduce a heuristic to expand the node
toward narrow passages with efficiency rather than opti-
mization.
3.1 G
2
Continuous Bidirectional Path Planning
Algorithm
The path planner should conform to the kinematics of a
car-like vehicle such that the vehicle can easily follow the
path generated by it. Hence, we propose a spline-based node
expansion method; that is, using a cubic Bezier curve, we
generate motion primitives that satisfy the constraints of
the minimum turning radius and G
2
continuity [28]. Our
expansion method allows node expansions in the forward
and rearward directions. However, although the motion
primitives of conventional hybrid A
∗
consist of straight
movements and right/left turns with circular arcs (right and
left turns with multiple turning radii), our motion primitives
consist of turns with multiple turning radii depending on
cubic Bezier curves as well as straight movements for
G
2
continuity. Also, we propose a bidirectional searching
algorithm that solves the discontinuity of point connected at
forward and backward trees using two concatenated cubic
Bezier curves.
The detailed procedures for generating motion primitives
using a cubic Bezier curve are as follows. First, given the
length of a motion primitive (L
m
) and turning radius, we
compute the state (x,y,θ) of the expanding nodes (the end
point of a circular arc), as illustrated in Fig. 2a. Sequentially,
given the states of two end points (X
parent
and X
new
), as
shown in Fig. 2b, we can obtain x
int
and γ from their
geometrical relation. From γ and the given or additionally
calculated turning radius, we can additionally determine β
and d showninFig.2b(see[26] for more details) for
each turning radius. Finally, we can compute the control
points for two concatenated cubic Bezier curves (P
0−3,1−2
in Fig. 2b) from each circular arc. X
parent
and X
new
in Fig.
2b denote the states of the current and newly expanding
nodes in Fig. 2a, respectively. In our algorithm, given the
length of the motion primitive, the minimum turning radius,
and the number of circular arcs, we compute the turning
radius (r
i
) of each circular arc by dividing the angle (θ
m
)
equivalently between the straight line and the circular arc
with the minimum turning radius (r
min
).
As bidirectional searching is applied to hybrid A
∗
, both
trees of bidirectional hybrid A
∗
may be discontinuously
connected since tree searching is addressed with a certain
resolution, as mentioned in Section 2.2. Our premise, i.e.,
Page 3 of 14 39J Intell Robot Syst (2021) 103: 39