# Mobile robot algorithm implementation error

I am working in reproducing a robotics paper, first simulating it in MATLAB in order to implement it to a real robot afterwards. The robot's model is:

$$\dot{x}=V(t)cos\theta$$ $$\dot{y}=V(t)sin\theta$$ $$\dot{\theta}=u$$

The idea is to apply an algorithm to avoid obstacles and reach a determines target. This algorithm uses a cone vision to measure the obstacle's properties. The information required to apply this system is:

1) The minimum distance $d(t)$ between the robot and the obstacle (this obstacle is modelled as a circle of know radius $R$).

2) The obstacle's speed $v_{obs}(t)$

3)The angles $\alpha_{1}(t)$ and $\alpha_{2}(t)$ that form the robot's cone vision, and

4) the heading $H(t)$ from the robot to the target

First a safe distance $d_{safe}$ between the robot and the obstacle is defined. The robot has to reach the target without being closer than $d_{safe}$ to the obstacle.

An extended angle $\alpha_{0} \ge arccos\left(\frac{R}{R+d_{safe}} \right)$ is defined, where $0 \le \alpha_{0} \le \pi$

Then the following auxiliary angles are calculated:

$\beta_{1}(t)=\alpha_{1}(t)-\alpha_{0}(t)$

$\beta_{2}=\alpha_{2}(t)+\alpha_{0}(t)$

Then the following vectors are defined:

$l_{1}=(V_{max}-V)[cos(\beta_{1}(t)),sin(\beta_{1}(t))]$

$l_{2}=(V_{max}-V)[cos(\beta_{2}(t)),sin(\beta_{1}(2))]$

here $V_{max}$ is the maximum robot's speed and $V$ a constant that fulfills $\|v_{obs}(t)\| \le V \le V_{max}$

This vectors represent the boundaries of the cone vision of the vehicle

Given the vectors $l_{1}$ and $l_{2}$ , the angle $\alpha(l_1,l_2)$ is the angle between $l_{1}$ and $l_{2}$ measured in counterclockwise direction, with $\alpha \in (-\pi,\pi)$ . Then the function $f$ is

The evasion maneuver starts at time $t_0$. For that the robot find the index h:

$h = min|\alpha(v_{obs}(t_0)+l_j(t_0),v_R(t_0))|$

where $j={1,2}$ and $v_R(t)$ is the robot's velocity vector

Then, from the two vectors $v_{obs}(t_0)+l_j(t_0)$ we choose that one that forms the smallest angle with the robot's velocity vector. Once h is determinded, the control law is applied:

$u(t)=-U_{max}f(v_{obs}(t)+l_h(t),v_R(t))$

$V(t)=\|v_{obs}(t)+l_h(t)\| \quad \quad (1)$

This is a sliding mode type control law, that steers the robot's velocity $v_R(t)$ towards a switching surface equal to the vector $v_{obs}(t)+l_h(t)$. Ideally the robot avoids the obstacle by surrounding it a

While the robot is not avoiding an obstacle it follows a control law:

$u(t)=0$

$V(t)=V_{max} \quad \quad (2)$

Hence the rules to switch between the two laws are:

R10 Switching from (2) to (1) occurs whenthe distance to the obstacle is equal to a constant C, which means when $d(t_0)=C$ and this distance is becoming smaller in time i.e. $\dot{d(t)}<0$

R11 Switching from (1) to (2) occurs when $d(t_*)<1.1a_*$ and the vehicle is pointing towards the obstacle, i.e. $\theta(t_*)=H(T_*)$

where $a_*=\frac{R}{cos\alpha_0}-R$

Ideally the result should be similar to this

While I understand the theory there's obviously a flaw in my implementation that I haven't been able to solve. In my opinion the robot manages to avoid the obstacle but at certain point (in the red circle), the robot turns to the wrong side, making impossible the condition $H(t) = \theta(t)$ to be achieved.

I feel that I am not measuring properly the angle alpha between the $v_{obs}(t)+l_h(t)$ and $v_{R}(t)$ , because while debugging I can see that at certain point it stops switching between negative and positive values and become only positive, leading the robot's to the wrong side. It also seems to be related with my problem here: Angle to a circle tangent line