Angle to a circle tangent line

Question

I want to simulate the detection of a moving object by a unicycle type robot. The robot is modelled with position (x,y) and direction theta as the three states. The obstacle is represented as a circle of radius r1 (r_1 in my code). I want to find the angles alpha_1 and alpha_2from the robot's local coordinate frame to the circle, as shown here:

So what I am doing is trying to find the angle from the robot to the line joining the robot and the circle's centre (this angle is called aux_t in my code), then find the angle between the tangent and the same line (called phi_c). Finally I would find the angles I want by adding and subtracting phi_c from aux_t. The diagram I am thinking of is shown:

The problem is that I am getting trouble with my code when I try to find the alpha angles: It starts calculating the angles correctly (though in negative values, not sure if this is causing my trouble) but as both the car and the circle get closer, phi_c becomes larger than aux_t and one of the alphas suddenly change its sign. For example I am getting this:

$$\begin{array}{c c c c} \text{aux_t} & \text{phi_c} & \text{alpha_1} & \text{alpha_2} \\ \hline \text{-0.81} & \text{+0.52} & \text{-1.33} & \text{-0.29} \\ \text{-0.74} & \text{+0.61} & \text{-1.35} & \text{-0.12} \\ \text{-0.69} & \text{+0.67} & \text{-1.37} & \text{-0.02} \\ \text{-0.64} & \text{+0.74} & \text{-1.38} & \text{+0.1} \\ \end{array}$$

So basically, the alpha_2 gets wrong form here. I know I am doing something wrong but I'm not sure what, I don't know how to limit the angles from 0 to pi. Is there a better way to find the alpha angles?

Answer 1

First, determine the angle $\phi$ between the robot $<\!a_{x},a_{y},\theta\!>$ and the target $<\!p_{x},p_{y}\!>$ as follows

$$ \phi = \tan^{-1} \left( \frac{ p_{y} - a_{y} }{ p_{x} - a_{x} } \right) - \theta $$

See the below picture,

Based on $\phi$, you can determine the rest.

Answer 2

function [boundedAngle] = BoundAngle (unboundedAngle)
while unboundedAngle < -pi
    unboundedAngle = unboundedAngle + 2*pi;
end
while unboundedAngle > pi
    unboundedAngle = unboundedAngle - 2*pi;
end
boundedAngle = unboundedAngle;
return:

The above code writes a (Matlab) function to limit angles between +/-pi. You say the angle should be between 0 and pi, but that only means 0-180 degrees. What if the object is behind you? If you want to ignore it, that's your choice, but you can't shift in pi (180 degree) increments and expect to get a valid answer.

Regarding the sign change, that appears to be expected, based on your drawing. I'm not sure what the issue is that you're having with the results.