Mobile robot algorithm implementation error

Question

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

But I'm getting this instead

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

Answer 1

So I'll point out what might be some problems, at-a-glance, but wow this is one of the longest questions I've seen here. I'm pretty swamped with real-life stuff at the moment, so I'll just point these few issues out, you try them, update the question with the results (please don't respond with new information in comments on this answer), and then if it's still broke I'll take another pass.

For starters, some of your code uses variables that I didn't see mentioned anywhere, such as (confusingly) tk and kt. Furthermore, there are some magic numbers that I can't understand, such as the assignments for tk and kt - they're both set to 2 despite the comment beside tk=2;, which says, % Locks in the chosen sign. If you manually set tk=2;, then it's not locking in anything chosen programmatically; it's just a hard-coded value that becomes difficult to change.

That said, I would guess that your real problem is probably in your use of atan. In fact, this function has bitten me so many times that I will never use it again. Use atan2 everywhere you ever would consider using atan. The problem with atan is that if you have (-y)/x or y/(-x), both of those expressions evaluate to -(y/x), and you lose quadrant information, giving you incorrect angles.

atan2 fixes this by accepting two arguments, y and x, as opposed to y/x, and it treats the signs correctly. In your ApplyRule10 function, under the line, % AUXILIARY ANGLES, I AM NOT SURE IF THIS IS THE BEST WAY TO CALCULATE THEM:, you have:

aux_angle_1 = atan(vr_t(2)/vr_t(1)) - atan(aux_1(2)/aux_1(1)) ; 
aux_angle_2 = atan(vr_t(2)/vr_t(1)) - atan(aux_2(2)/aux_2(1)) ; 

My suggestion would be to change these atan functions to atan2 and see if that fixes anything. Again, update your question with the results and we'll go from there!