## 4 The \(\mathbb{G}^{3}\)-Continuous Extend Procedure
### Computing Reeds-Shepp-Like Paths Using a \(\mathbb{G}^{3}\)-Continuous Path Primitive  
In our extend procedure, we assume that parameters \(\mu_{1}\), \(\mu_{2}\), and \(\kappa_{c}\) are selected or randomly sampled by the global planning algorithm. For simplicity of considerations, we also assume \(\mu_{1}=\mu_{2}\), but the proposed procedure can be trivially adapted for the case of \(\mu_{1}\
eq\mu_{2}\). Finding parameters of two \(\mathbb{G}^{3}\)-continuous primitives \(T(\mathbf{w}_{1},\mathbf{w}_{2},\mu_{1})C(\mathbf{w}_{2},\mathbf{w}_{3})T(\mathbf{w}_{4},\mathbf{w}_{ 3},\mu_{2})S(\mathbf{w}_{4},\mathbf{w}_{5})\) and \(T(\mathbf{w}_{5},\mathbf{w}_{6},\mu_{3})C(\mathbf{w}_{6},\mathbf{w}_{7})T(\mathbf{w}_{8},\mathbf{w}_{ 7},\mu_{4})S(\mathbf{w}_{8},\mathbf{w}_{9})\) connecting prescribed vehicle-body configurations \(\mathbf{w}_{1}\) and \(\mathbf{w}_{5}\) is a nontrivial task. However, one can leverage extensions of the procedure devised by [PERSON] and [PERSON], which were presented in Reference [7] and later Reference [8]. This path-construction procedure requires knowledge of auxiliary circles, on which endpoints \(\mathbf{w}_{4}\) and \(\mathbf{w}_{8}\) of the transition segments must lie. One must also know the difference between an orientation tangent to this circle and orientation tangent to the transition segment at endpoints \(\mathbf{w}_{4}\) and \(\mathbf{w}_{8}\). This difference is denoted by \(\
u\) in Figure 2.  
The reference path is computed during an extend procedure as follows:  
1. The four possible transition segments \(T_{1}\), \(T_{2}\), \(T_{3}\), \(T_{4}\) starting at the prescribed initial vehicle body configuration are computed (Curve (7)). They correspond to forward motion with curvature \(\kappa_{c}\), forward motion with curvature \(-\kappa_{c}\), backward motion with curvature \(\kappa_{c}\), and backward motion with curvature \(-\kappa_{c}\). See Figure 5 for visual interpretation.
2. Step 1 is repeated for the four possible transition segments, \(T_{5}\), \(T_{6}\), \(T_{7}\), \(T_{8}\), ending at a prescribed final vehicle-body configuration.
3. For every transition segment \(T(\mathbf{w}_{1},\mathbf{w}_{2},\mu)\) computed up to this step, find center \(\tilde{\mathbf{q}}_{c}=[x_{c}\,y_{c}]^{\top}\) of the auxiliary circle, on which the next transition segment must lie according to a simple geometric formula: \[\tilde{\mathbf{q}}_{c}=[w_{2x}\,w_{2y}]^{\top}+1/\kappa_{c}[-\sin w_{2\theta}\, \cos w_{2\theta}]^{\top}.\]
4. For every transition segment \(T(\mathbf{w}_{1},\mathbf{w}_{2},\mu)\) computed up to this step, find auxiliary circle radius \(R\) as follows: \[R=\left\|\,\tilde{\mathbf{q}}_{c}-[w_{1x}\,w_{1y}]^{\top}\right\|.\]
5. For every transition segment \(T(\mathbf{w}_{1},\mathbf{w}_{2},\mu)\) without a fixed value of \(\mathbf{w}_{1}\), find \(\
u\), which is straightforward given the knowledge of transition segment Curve (7) and the auxiliary circle.
6. For every circle segment \(C(\mathbf{w}_{1},\mathbf{w}_{2})\), compute its remaining unknown endpoint using the algorithm from Reference [7].
7. If motion cost \(J\) is defined, compute the cost for all the paths and choose the optimal path. Otherwise, return a random path, or all found paths (depending on the utilized global planning algorithm).  
Note that the choice of \(\mu\) should depend on the assumed motion costs. For example, if smooth paths are desired, then our computational studies summarized in Figure 4 show that choosing \(\mu=0.82\) leads to the paths with minimal \(|dx_{d}(s)/ds|\). On the other hand, for the shortest paths, choose \(\mu\) close to 0.5. The proposed extend procedure solves Problem 1; however, to solve the other problems one must account for obstacles. This is solved in the next sections.