## 2 Three-Dimensional Dubins Path
### Two-Dimensional Dubins Path
#### 2.1.2 RSL and LSR  
For the RSL case, a trajectory consists of first turning right on a right circular arc at waypoint \(\mathbf{p}_{i}\), going straight, and then turning left on a left arc until the next waypoint \(\mathbf{p}_{i+1}\) is reached. Figure 6 shows the pull-out point \(\mathbf{q}_{0,i}\), the wheel-over point \(\mathbf{q}_{1,i}\), and a straight line connecting them, respectively.  
Figure 5: LSL path.  
Figure 6: RSL path.  
The pull-out and wheel-over points are computed as follows:  
\[\begin{split}\mathbf{q}_{0,i}&=\mathbf{q}_{0,i}^{\prime} +(\mathbf{q}_{0,i}^{\prime}-\mathbf{c}_{i})\cos\psi+\mathbf{e}_{rot0,i}\times( \mathbf{q}_{0,i}^{\prime}-\mathbf{c}_{i})\sin\psi\\ \mathbf{q}_{1,i}&=\mathbf{q}_{1,i}^{\prime}+(\mathbf{ q}_{1,i}^{\prime}-\mathbf{c}_{i+1})\cos\psi-\mathbf{e}_{rot1,i}\times(\mathbf{q}_{1,i}^{ \prime}-\mathbf{c}_{i+1})\sin\psi\end{split} \tag{9}\]  
where  
\[\begin{split}\mathbf{q}_{0,i}^{\prime}&=-R\mathbf{e }_{rot0,i}\times\mathbf{e}_{c,i}+\mathbf{c}_{i}\\ \mathbf{q}_{1,i}^{\prime}&=-R\mathbf{e}_{rot1,i} \times\mathbf{e}_{c,i}+\mathbf{c}_{i+1}\\ \psi&=\sin^{-1}\left(\frac{2R}{\|\mathbf{c}_{i+1}- \mathbf{c}_{i}\|_{2}}\right)\end{split} \tag{10}\]  
and \(\mathbf{e}_{rot0,i}=-\mathbf{e}_{n,i}\), \(\mathbf{e}_{rot1,i}=\mathbf{e}_{n,i}\) are rotation axes, and \(\mathbf{c}_{i}=\mathbf{c}_{r,i}\), \(\mathbf{c}_{i+1}=\mathbf{c}_{l,i+1}\) are centers of circular arcs.  
The rotation angle \(\theta_{0,i}\) about the rotation axis to traverse from point \(\mathbf{p}_{i}\) to \(\mathbf{q}_{0,i}\) can be determined by  
\[\phi_{0} =\cos^{-1}\left(\frac{(\mathbf{p}_{i}-\mathbf{c}_{i})\cdot( \mathbf{q}_{0,i}^{\prime}-\mathbf{c}_{i})}{R^{2}}\right) \tag{11}\] \[\theta_{0,i} =\begin{cases}\phi_{0}+\psi,&\text{if }\mathbf{e}_{i}\cdot \mathbf{e}_{d,0}\geq 0\\ 2\pi-\phi_{0}+\psi,&\text{else}\end{cases}\]  
where  
\[\mathbf{e}_{d,0}=\frac{\mathbf{q}_{0,i}^{\prime}-\mathbf{p}_{i}}{\|\mathbf{q}_ {0,i}^{\prime}-\mathbf{p}_{i}\|_{2}}.\]  
Similarly, the rotation angle \(\theta_{1,i}\) to traverse from point \(\mathbf{q}_{1,i}\) to \(\mathbf{p}_{i+1}\) is given as follows:  
\[\phi_{1} =\cos^{-1}\left(\frac{(\mathbf{p}_{i+1}-\mathbf{c}_{i+1})\cdot( \mathbf{q}_{1,i}^{\prime}-\mathbf{c}_{i+1})}{R^{2}}\right) \tag{12}\] \[\theta_{1,i} =\begin{cases}\phi_{1}+\psi,&\text{if }\mathbf{e}_{i+1}\cdot \mathbf{e}_{d,1}\geq 0\\ 2\pi-\phi_{1}+\psi,&\text{else}\end{cases}\]  
where  
\[\mathbf{e}_{d,1}=\frac{\mathbf{p}_{i+1}-\mathbf{q}_{1,i}^{\prime}}{\|\mathbf{ p}_{i+1}-\mathbf{q}_{1,i}^{\prime}\|_{2}}\]  
For the LSR case, a trajectory consists of first turning left on a left circular arc, followed by straight, and turning right on a right arc until the next waypoint is reached. Figure 7 shows the pull-out and wheel-over points and a straight line connecting them, respectively.  
Figure 7: LSR path.  
The pull-out, wheel-over points, and rotation angle are computed by the same Equations (9)-(12) with the following replacements:  
\[\mathbf{e}_{rot0,i} =\mathbf{e}_{n,i} \tag{13}\] \[\mathbf{e}_{rot1,i} =-\mathbf{e}_{n,i}\] \[\mathbf{c}_{i} =\mathbf{c}_{l,i}\] \[\mathbf{c}_{i+1} =\mathbf{c}_{r,i+1}\]  
The distance \(s_{i}\) between two waypoints along the Dubins path is computed as follows:  
\[s_{i}=R\ \theta_{0,i}+\|\mathbf{q}_{1,i}-\mathbf{q}_{0,i}\|_{2}+R\ \theta_{1,i} \tag{14}\]