## 3 System Design and Implementation
### Trajectory Planning (Clockwise)
This is shown in the picture in blue and is calculated as follows:  
\[\overrightarrow{M_{d}}=\overrightarrow{M_{2}}-\overrightarrow{M_{1}}= \overrightarrow{Z}-\overrightarrow{S}+r\cdot(\text{sin}\phi_{Z}-\text{sin} \phi_{S}\quad\text{cos}\phi_{S}-\text{cos}\phi_{Z}) \tag{7}\]  
\[\overrightarrow{M_{d}}=(Z_{x}-S_{x}+r\cdot(\text{sin}\phi_{Z}-\text{sin}\phi_ {S})\quad Z_{y}-S_{y}+r\cdot(\text{cos}\phi_{S}-\text{cos}\phi_{Z})) \tag{8}\]  
The normal vectors \(N_{1}\) and \(N_{2}\) are both orthogonal to the vector connecting the two circle centers, and since they are unit vectors, they each have a length of 1. Therefore, they are both the same. They can be determined over a 90\({}^{\circ}\) rotation in the mathematically positive sense of the connection vector \(M_{d}\) and subsequent normalization. \[\stackrel{{\rightarrow}}{{N_{1}}}=\stackrel{{ \rightarrow}}{{N_{2}}}=\frac{\stackrel{{\rightarrow}}{{M_{d}}}}{{| \stackrel{{\rightarrow}}{{M_{d}}}|}}\cdot R_{(90^{\prime})} \tag{9}\]  
\[\stackrel{{\rightarrow}}{{N_{1}}}=\stackrel{{ \rightarrow}}{{N_{2}}}=\frac{\left(Z_{x}-S_{x}+r\cdot(\sin\!\phi_{Z}-\sin\! \phi_{S})\quad Z_{y}-S_{y}+r\cdot(\cos\!\phi_{S}-\cos\!\phi_{Z})\right)}{ \sqrt{\left(Z_{x}-S_{x}+r\cdot(\sin\!\phi_{Z}-\sin\!\phi_{S})\right)^{2}+\left( Z_{y}-S_{y}+r\cdot(\cos\!\phi_{S}-\cos\!\phi_{Z})\right)^{2}}}\cdot \begin{pmatrix}0&-1\\ 1&0\end{pmatrix}  \tag{10}\]  
\[\stackrel{{\rightarrow}}{{N_{1}}}=\stackrel{{ \rightarrow}}{{N_{2}}}=\frac{\left(S_{y}-Z_{y}+r\cdot(\cos\!\phi_{Z}-\cos\! \phi_{S})\quad Z_{x}-S_{x}+r\cdot(\sin\!\phi_{Z}-\sin\!\phi_{S})\right)}{ \sqrt{\left(Z_{x}-S_{x}+r\cdot(\sin\!\phi_{Z}-\sin\!\phi_{S})\right)^{2}+\left( Z_{y}-S_{y}+r\cdot(\cos\!\phi_{S}-\cos\!\phi_{Z})\right)^{2}}} \tag{11}\]  
With the connecting vector \(M_{d}\) around the circle radius \(r\) in the above-determined normal direction \(N\), we can obtain the exact part of the desired trajectory on which the vehicle travels straight ahead. This line segment is nestled at the beginning and end of the two circles at the start and goal circle. The straight line represents one of the two common tangents of two circles. However, only the tangent can be calculated here in a forward direction to enter and leave, because both run by a right curve. The other tangent would have to be traversed in the reverse direction, which is undesirable. It is worth mentioning again that both parties in the same direction must be traversed by the method shown here, so that the vectors spanning \(M_{d}\), \(N_{1}\), and \(N_{2}\) form a rectangle. Next, a quality criterion for the observed line can be determined. For this purpose, the length of the trajectory is considered. Since the trajectory is composed of three sections, these sections are first considered individually. This begins with the circular area on the home circle. The required length of the arc length \(l_{1}\) corresponds to the starting circuit from the starting point \(S\) of the vehicle to the exit point \(B_{1}\). It should be noted that the circle is traversed by a right-hand bend in the mathematically negative sense. Therefore, the angle is swept by the arc \(\phi_{S}-\varphi_{M_{d}}\). The angle \(\varphi_{M_{d}}\) is the pitch angle of the vector \(M_{d}\) and is calculated as follows:  
\[\phi_{M_{d}}=atan\Big{(}M_{dy},M_{dx}\Big{)} \tag{12}\]  
It must finally be ensured that the swept angle \(\varphi_{S}-\varphi_{M_{d}}\) is in the range between 0 and 360 degrees. To determine the length \(l_{1}\),  
\[l_{1}=r\cdot\left[(\phi_{S}-\phi_{M_{d}})mod(2\pi)\right] \tag{13}\]  
The length of \(l_{2}\) is the straight line connecting the two circles to the length of the corresponding vector \(M_{d}\). \[l_{2}=|\stackrel{{\rightarrow}}{{M_{d}}}| \tag{14}\]  
Now, even the length \(l_{3}\) is missing on the target circle. This is calculated analogously to the length \(l_{1}\) on the home circle, where the swept angle for a right turn is \(\varphi_{M_{d}}-\varphi_{Z}\).