## 3 System Design and Implementation
### Trajectory Planning (Clockwise)
\[l_{3}=r\cdot\left[(\phi_{M_{d}}-\phi_{Z})mod(2\pi)\right] \tag{15}\]  
The trajectory length \(l\) is now the sum of the last three computed lengths. \[l=l_{1}+l_{2}+l_{3}=r\cdot\left[(\phi_{S}-\phi_{M_{d}})mod(2\pi)+(\phi_{M_{d}} -\phi_{Z})mod(2\pi)\right]+|\stackrel{{\rightarrow}}{{M_{d}}}| \tag{16}\]  
The trajectory has been derived for a right turn in the above derivation. The angle in the mathematically negative sense of rotation is painted over, so the circle centers are opposite to the normal vectors. In contrast, a left-hander will go through the angle in the mathematically positive sense, so the circle's center point here is in the direction of the normal vectors. Moreover, the sign of the angular change is important in calculating the quality criterion. The derivation of a left-hander should not be performed, because it can be carried out similarly to the derivation of the right-hander. Instead, a sequence of steps  
middle by the red connecting line. The hypotenuses of the two triangles thus have the length \(M_{d}/2\). The angle \(\mu\) can be calculated using trigonometry on a right-angled triangle. \[\mu=\arccos(\frac{r}{\frac{|M_{d}|}{2}})=\arccos(\frac{2\cdot r}{|\stackrel{{ \rightarrow}}{{M_{d}}}|}) \tag{29}\]  
It should be noted that for a valid solution, the arccosine argument must be between \(-1\) and \(1\). Otherwise, there is no real solution for the angle \(\mu\), and the given geometric problem is unsolvable. This can happen if, for example, the two circles overlap. An auxiliary angle \(\xi_{1}\) is now introduced, which is not shown in Figure 3. In this case, it is the sum of the angle \(\varphi M_{d}\) of the connection vector \(M_{d}\) and the angle \(\mu\). \[\xi_{1}=\phi_{M_{d}}+\mu=\phi_{M_{d}}+\cos(\frac{2\cdot r}{|\stackrel{{ \rightarrow}}{{M_{d}}}|}) \tag{30}\]  
The direction of the normal vector \(N_{1}\) can now be determined with this auxiliary angle, since this angle rotates it exactly \(\xi_{1}\) from the x-axis of the absolute coordinate system. The normal vector \(N_{2}\) is the inverse vector of the vector \(N_{1}\). \[\stackrel{{\rightarrow}}{{N_{1}}}=(\cos\xi_{1}\quad\sin\xi_{1}) =-\stackrel{{\rightarrow}}{{N_{2}}} \tag{31}\]  
The points \(B_{1}\) and \(B_{2}\) are away from the respective circle center by the distance r in the corresponding normal direction. Thus, for these points,  
\[\phi_{M_{d}}=atan\Big{(}M_{dy},M_{dx}\Big{)} \tag{12}\]0  
These points represent the circle's entry or exit point. The orientation that the robot should have in these points is given by the angles \(\varphi B_{1}\) and \(\varphi B_{2}\). These two angles are equal because they are both the tangent angle of the red connecting line. The two angles are rotated by \(\pi/2\) in the mathematically negative sense against the normal vector \(N_{1}\), which has the direction \(\xi_{1}\). This results in \(\varphi_{Bi}\) for the tangent angle:  
\[\phi_{M_{d}}=atan\Big{(}M_{dy},M_{dx}\Big{)} \tag{12}\]1  
Alternatively, the tangent angles \(\varphi_{Bi}\) can also be determined using the target circle. The angles \(\mu\) and \(\phi_{M_{d}}\) remain the same. If we look at the angles in the target circle in Figure 3, we can set up the following summation equation:  
\[\phi_{M_{d}}=atan\Big{(}M_{dy},M_{dx}\Big{)} \tag{12}\]2 Circle trajectory with alternating flow direction (right- and left-handed). \[\phi_{M_{d}}=atan\Big{(}M_{dy},M_{dx}\Big{)} \tag{12}\]3  
where \(\pi\) represents a semicircle in radians. If we solve the equation for \(\xi_{2}\), we can get  
\[\phi_{M_{d}}=atan\Big{(}M_{dy},M_{dx}\Big{)} \tag{12}\]4  
The tangent angles \(\varphi_{Bi}\) are rotated by \(\pi/2\) in the mathematically positive sense by the angle \(\xi_{2}\). Thus,  
\[\phi_{M_{d}}=atan\Big{(}M_{dy},M_{dx}\Big{)} \tag{12}\]5  
which corresponds exactly to Equation (33). This means that we obtain the same result with both methods of calculation. Now, all that is missing is the determination of the quality criterion, i.e., to evaluate the trajectory length.