# Autonomous and Safe Navigation of Mobile Robots in Vineyard with Smooth Collision Avoidance
## 2 Pillar Detection from Lidar Sensor Data
Here, we explain the smoothing of the sharp turn across the point B. First, we check if the distance between the point B and the crop (\(d_{c}\)) is greater than a threshold safe distance (\(\delta_{s}\)). If not, then the point B is shifted until a safety distance is reached. \[\text{action}= \begin{cases}\text{smooth the turn},&\text{if }d_{c}\geq\delta_{s}\\ \text{shift the point},&\text{otherwise}.\end{cases} \]

In order to smooth the turn at point B in Figure 6a, two points P\({}_{1}\) and P\({}_{2}\) are found such that the line joining the two points \(\overline{\text{P}_{1}\text{P}_{2}}\) is at a safe threshold distance (\(\delta_{\text{thresh}}\)) from the obstacle. We find point P\({}_{1}\) on the line \(\overline{\text{BA}}\) at a fixed small distance from the point B. Similarly, point P\({}_{2}\) is marked on the line \(\overline{\text{BC}}\) such that, \(\overline{\text{BP}_{1}}=\overline{\text{BP}_{2}}\). The line joining the two points \(\overline{\text{P}_{1}\text{P}_{2}}\) is at a safe threshold distance (\(\delta_{\text{thresh}}\)) from the obstacle. The slope of lines \(\overline{\text{AB}}\) and \(\overline{\text{BC}}\) are (Figure 6a),

\[m_{AB}=\frac{y_{2}-y_{1}}{x_{2}-x_{1}},\ \ \ m_{BC}=\frac{y_{2}-y_{3}}{x_{2}-x_{3}}. \tag{2}\]The lines \(\overline{\mathrm{P_{1}O}}\) and \(\overline{\mathrm{P_{2}O}}\) are perpendicular to the lines \(\overline{\mathrm{AB}}\) and \(\overline{\mathrm{BC}}\), respectively. Therefore, the slopes of lines \(\overline{\mathrm{P_{1}O}}\) and \(\overline{\mathrm{P_{2}O}}\) are,

\[m_{P_{1}O}=\frac{-1}{m_{AB}},\ \ \ m_{P_{2}O}=\frac{-1}{m_{BC}}. \tag{3}\]

The center of the circle whose arc generates the smooth path is the point of intersection of lines \(\overline{\mathrm{P_{1}O}}\) and \(\overline{\mathrm{P_{2}O}}\) (point \(\mathrm{O(c_{x},c_{y})}\) in Figure 6a). The general equation of a line of slope \(m\) passing through a point \((x_{1},y_{1})\) is,

\[ \begin{split}& y-y_{1}=m(x-x_{1})\\ \implies& y=mx-mx_{1}+y_{1}.\end{split}  \tag{4}\]

Lines \(\overline{\mathrm{P_{1}O}}\) and \(\overline{\mathrm{P_{2}O}}\) intersect at \(\mathrm{O(c_{x},c_{y})}\), therefore,

\[ \begin{split} c_{y}&=m_{P_{1}O}(c_{x}-p_{x1})+p_{ y1}=m_{P_{2}O}(c_{x}-p_{x2})+p_{y2},\\ &\implies m_{P_{1}O}\cdot c_{x}-m_{P_{1}O}\cdot p_{x1}+p_{y1}=m_{P_{ 2}O}\cdot c_{x}-m_{P_{2}O}\cdot p_{x2}+p_{y2}\\ &\implies m_{P_{1}O}\cdot c_{x}-m_{P_{2}O}\cdot c_{x}=m_{P_{1}O} \cdot p_{x1}-m_{P_{2}O}\cdot p_{x2}+p_{y2}-p_{y1}\\ &\implies c_{x}\cdot(m_{P_{1}O}-m_{P_{2}O})=m_{P_{1}O}\cdot p_{x1}-m_{ P_{2}O}\cdot p_{x2}+p_{y2}-p_{y1}.\end{split} \]

Thus, the \(x\)-coordinate of the center of the circle is

\[c_{x}=\frac{m_{P_{1}O}\cdot p_{x1}-m_{P_{2}O}\cdot p_{x2}+p_{y2}-p_{y1}}{(m_{ P_{1}O}-m_{P_{2}O})}. \tag{5}\]

Similarly, the \(y\)-coordinate of the center of the circle is,

\[c_{y}=m_{P_{1}O}\cdot c_{x}-m_{P_{1}O}\cdot p_{x1}+p_{y1}. \tag{6}\]

The radius of the circle \(r\) is,

\[r=\sqrt{(c_{x}-p_{x1})^{2}+(c_{y}-p_{y1})^{2}},\ \ \ \mathrm{or}\ \ r=\sqrt{(c_{x}-p_{x2})^{2}+(c_{y}-p_{y2})^{2}}. \tag{7}\]

The arc \(\widehat{\mathrm{P_{1}P_{2}}}\) between points \(\mathrm{P_{1}}\) and \(\mathrm{P_{2}}\) shown in Figure 6a is the robot's smooth path. The curve \(\widehat{\mathrm{P_{1}P_{2}}}\) is tangential to the robot's original path, which guarantees \(\mathrm{G^{1}}\) geometric continuity. Moreover, the curve is smoother for the robot to navigate. For obstacle avoidance, the robot must first shift in one direction and cross the obstacle, and then resume its position in the center of the lane. Since smooth path generation requires a set of three points, obstacle avoidance requires a set of six points (or two sets each of three points). These points can be generated in real-time for both static and dynamic obstacles. One of the points can be the starting position of the robot itself, i.e., the current position. The other two points need to be generated considering the width of the vineyard lane (W) and the safety threshold (\(\delta_{s}\)).