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:

Detection of a moving object

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:

Diagram #2

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?

  • $\begingroup$ Why you are making it so complicated?? First Find the angle between the robot and the center of the circle (i.e. angle = atan2(P2-RobotY/P1-RobotX) - RobotHeadingAngle), then based on this angle, find the rest. I will probably come back to this later with more details but I hope you got it. $\endgroup$ – CroCo Apr 13 '16 at 20:54
  • $\begingroup$ Hi, I'm not sure if I'm understanding you correctly, but I think what you wrote is what I am doing. I would appreciate your clarification, thanks for your answer. $\endgroup$ – Wobbler28 Apr 14 '16 at 10:41
  • $\begingroup$ Those numbers look exactly right to me. -values are to the left. The + value is to the right. The circle is now so close that one edge of it is to the right. There is no issue here that I see. $\endgroup$ – Octopus Apr 14 '16 at 19:03
  • $\begingroup$ There might be a problem with egocentric and global coordinates--you might want to make sure that your angles are defined consistently and that they stay within the defined range (e.g. $(-\pi,\pi]$). $\endgroup$ – NBCKLY Jul 29 '16 at 14:47

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,

enter image description here

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

  • $\begingroup$ Assuming that $\theta\in(-\pi,\pi]$, what should the domains of the other angles be? $\endgroup$ – NBCKLY Jul 29 '16 at 14:52
  • $\begingroup$ @NBCKLY, which angles? and why you're assuming that $\theta \in (-\pi,\pi]$. This is the true range of the angle. $\endgroup$ – CroCo Nov 10 '16 at 14:06
function [boundedAngle] = BoundAngle (unboundedAngle)
while unboundedAngle < -pi
    unboundedAngle = unboundedAngle + 2*pi;
while unboundedAngle > pi
    unboundedAngle = unboundedAngle - 2*pi;
boundedAngle = unboundedAngle;

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.

  • $\begingroup$ Thanks for your answer. I am not completely sure, but as far as I understand the "vision" of the robot is a 0-180 degrees laser sensor and the robot cannot actaully see what is at its back, but I'm going to find out more about this. Regarding the sign, it seemed unintuitive to me but as you say it seems to be correct. Sadly the final result doesn't match the expected so I have to continue looking for the problem. Regards $\endgroup$ – Wobbler28 Mar 13 '16 at 23:16
  • 1
    $\begingroup$ @Chuck, wrapping the angle is a technical issue for this post. It seems to me there is a problem with the formula that determines the angles. $\endgroup$ – CroCo Apr 13 '16 at 20:56

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.