2

I cannot upload pics so I will try to explain my problem the best. 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. I want to find the angles alpha_1 and alpha_2 from 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 substracting 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 causingmy trouble) but as both the car and the cicle get closer, phi_c becomes larger than aux_t and one of the alphas suddenly change its sign. For example I am getting this:

aux_t//////phi_c//////alpha_1//////alpha_2
-0.81//////+0.52//////-1.33//////-0.29

-0.74//////+0.61//////-1.35//////-0.12

-0.69//////+0.67//////-1.37//////-0.02

-0.64//////+0.74//////-1.38//////+0.1

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? Here is the section of my code:

Wobbler28
  • 23
  • 3
  • this maybe because `atan2` returns a value of `aux_t` between -pi and pi, so it changes sign as soon as it crosses that half-line. –  Mar 13 '16 at 13:45

2 Answers2

4

As far as your math goes, the only change I would make would be to subtract (pi/2 - theta) from the target's angle rather than add it. This will give you angles in the more typical orientation (counter-clockwise being positive).

I'm not completely sure why you think that alpha_2 is wrong in the data that you posted in your answer. What is happening is that your robot is getting very close to the target and the alpha_2 tangent line actually moves to the other side or the line pointing in the direction your robot is looking. I have created a similar situation here where the labels on the tangent lines are the angle relative to the robot (yellow line) and all angles are forced to be between 0 and 2*pi.

enter image description here

To address you question about forcing an angle to be within a particular range. To do this you will want to use the modulus (mod in MATLAB). In these examples, I have used mod(theta, 2*pi) because technically if your robot is facing away from the target the angles can be greater than pi.

As a test I have performed a simple simulation that moves the robot around and shows the angles of the tangent lines relative to the robot's direction (again, between 0 and 2pi)

enter image description here

If you really want your angles to be between 0 and pi, you could instead use mod(theta, pi) instead.

alpha_1 = mod(alpha_1, pi);
alpha_2 = mod(alpha_2, pi);
Suever
  • 64,497
  • 14
  • 82
  • 101
  • Thanks a lot for our valuable help. My doubts came from the fact that I was not (and still am not) getting what I want as the final robot's behavior, so when I started to troubleshoot the programs the numbers I mentioned seemed unnatural to me and thought that they could negatively affect the control law. However after looking at your answer it makes a lot of sense, it didn't occured to me that the second tangent line was going to the other side with respect to the robot's direction. I would use the mod of 2*pi for my alpha angles and check where else I can be getting wrong. Regards – Wobbler28 Mar 13 '16 at 17:16
0

The point is at P, the circle at O, and the radius r. You want the tangent to the circle that goes through P, cal it T

So we know the distance OP, we know the distance OT, which is r, and we know that angle PTO is a right angle. So we use Pythagoras to get the distance TP. Then cos theta (the angle OPT is TP/OP. We could save a step and calculate sin theta, but cos theta is easier to deal with because it gives us the dot product of the direction vectors PT and PO. Often you won't actually need to call acos().

Malcolm McLean
  • 6,258
  • 1
  • 17
  • 18