Robot path planning

My goal is to move robot in certain points as shown in the figure. It's initial position is (x0,y0) and move along other coordinates.

I am able to track robot position using a camera which is connected to pc and camera is located at the top of the arena. I've mounted a ir beacon on the robot, camera find this beacon and locates it's coordinate(in cm) in the arena. Using this coordinate how can I move my robot to another position, say new position (x1,y1)

My robot has arduino mega 2560 with two DC motors, communication between pc and robot is done using bluetooth

Update:

Thanks @Chuck for the answer, however I still have few doubts regarding turning angle.

My robot position setup is as shown in the image.

(xc, yc) is the current position and (xt, yt) is the target position.

If I want to align robot in the direction of the target coordinates, I've to calculate atan2 between target and current coordinates. But the angle remains same since it's current position is not changing with respect to the target point. so I assume robot simply makes 360' rotation at current position?

Update:

The path points is as show below in the image, is my initial heading angle assumption is correct?

'1' is the starting point.

Update

Thank you for your patience and time, I'm still struck at turning, my code goes like this

//current points
float xc = -300;
float yc = 300;

//target points

float xt = -300;
float yt = -300;

//turning  angle
float turnAngle;

void setup() {
// pin setup
Serial.begin(9600);
}

void loop() {

turnAngle = atan2((yt-yc), (xt-xc)); //calculate turning angle

turnAngle = turnAngle * 180/3.1415; //convert to degrees

if (turnAngle > 180) {
turnAngle = turnAngle-360;
}

if (turnAngle < -180) {
turnAngle = turnAngle+360;
}

if (turnAngle < -10) {
//turn right
}

if (turnAngle > 10) {
//turn left
}
}

Since angle is always -90' robot only makes right turn in loop at current point, since angle is not changing. I think I'm missing something here.

• Your question isn't clear. Is this not the answer to the question: "I've mounted a camera at the top of the arena, camera tracks beacons position as (x,y) coordinates in cm." ? – Octopus Aug 24 '15 at 17:06
• I need to calculate turning angle, heading direction? – ronroo Aug 24 '15 at 17:42
• This question is so broad it almost isn't a question. Please include more details about your setup. You start out talking about localization, then mention that you have a camera that can track x,y coordinates of the beacons (if you know these coordinates, then what localization info do you NOT have?) then end with your aim of moving the robot on a path. Path planning and localization are two different things, so what exactly is it you're asking? – Matt Brown Aug 24 '15 at 20:32
• So, as I understrand, you want to localize the three beacons using the camera situated on the top of the arena. In this case you will need to use image processing algorithms, but can you be more specific please : is the camera connected to a computer or connected directly to the robot ? Did the robot have enought processing capabilities ? – Amine Horseman Aug 24 '15 at 20:33
• Guys sorry, I've updated the question please let me know if it's still not clear. – ronroo Aug 24 '15 at 21:16

You have to know your initial heading, let's call it $\theta_0$. So you start at some position, $p_{start}$, and you're trying to get to some end position, $p_{end}$.

Assume starting position and ending positions are given by:

$$p_{start} = <x_0 , y_0> \\ p_{end} = <x_1 , y_1> \\$$

Those positions are absolute, but you are trying to get from one to another, so all you really care about are relative distances. Then, let:

$$dX = x_1 - x_0 \\ dY = y_1 - y_0 \\$$

Now you can find the absolute heading $\theta_{0 \rightarrow 1}$ with the arc tangent of $dX$ and $dY$, but the basic $atan{}$ function doesn't handle negative signs correctly because it can't tell if which or both of $dX$ and $dY$ were negative. What you need is atan2.

$atan2{(dY, dX)}$ will give you the absolute heading of $p_{end}$ with respect to $p_{start}$. Now to find how much your vehicle needs to turn, find the difference between your starting heading and the required heading:

$$\mbox{Turn angle}= \theta_{0 \rightarrow 1} - \theta_0 \\ \mbox{Turn angle}= atan2{(dY, dX)} - \theta_0 \\$$

With turn angle, I like my vehicles to turn the direction that requires the least movement, so instead of limiting $\mbox{Turn angle}$ to $0<= \mbox{Turn angle} <= 2\pi$, I would limit the angle to $-\pi<= \mbox{Turn angle} <= \pi$.

In this way you would wind up turning -10 degrees (10 degrees clockwise) instead of +350 (350 degrees CCW). Ultimately you should wind up in the same spot, but -10 looks "right" and +350 looks foolish.

You can limit the turn angle with code like:

while turnAngle > 3.1415
turnAngle = turnAngle - 2*3.1415:
end
while turnAngle < -3.1415
turn Angle = turnAngle + 2*3.1415;
end

This code runs and doesn't care what multiple of +/-2$\pi$ you're in, it spits out the same angle bounded to within +/-$\pi$.

Finally, calculate how far you travel along your new heading with the magnitude of distance between the points:

$$\mbox{distance} = \sqrt{dX^2 + dY^2} \\$$

Remember to set the new absolute heading, $\theta_{0 \rightarrow 1}$, as your current heading then get the next point!

:UPDATE: - Numeric Example

I believe a point of confusion here may be that I didn't state that the absolute heading is relative to the +x-axis. That said, I'll work through OP's problem as shown in the most recent update.

The initial heading $\theta_0$ is in-line with the +x-axis, so $\theta_0 = 0$.

$$p_{start} = <-300,300> \\ p_{end} = <-300,-300> \\$$

$$dX = x_{end} - x_{start} = (-300) - (-300) \\ \boxed{dX = 0} \\$$

$$dY = y_{end} - y_{start} = (-300) - (300) \\ \boxed{dY = -600} \\$$

$$\theta_{0 \rightarrow 1} = atan2(dY,dX) \\ \theta_{0 \rightarrow 1} = atan2(-600,0) \\ \theta_{0 \rightarrow 1} = -1.5708 \mbox{rad} = -90^o$$

$$\mbox{Turn Angle} = \theta_{0 \rightarrow 1} - \theta_0 \\ \mbox{Turn Angle} = -1.5708 - 0 \\ \mbox{Turn Angle} = -1.5708 = -90^o\\$$

The distance you need to travel after turning $\mbox{Turn Angle}$ is given by: $$\mbox{distance} = \sqrt{dX^2 + dY^2} \\ \mbox{distance} = \sqrt{0^2 + (-600)^2} \\ \mbox{distance} = \sqrt{(-600)^2} \\ \mbox{distance} = 600 \\$$

Now the new heading $\theta_1$ is equal to the absolute heading between start and end:

$$\theta_1 = \theta_{0 \rightarrow 1} \\$$

Or, alternatively, the new heading is equal to the initial heading plus the turn angle, but the turn angle was defined by the difference between $\theta_{0 \rightarrow 1}$ and $\theta_0$, so the equation above is just a shortcut.

Now, proceed to the next point:

$$p_{start} = <-300,-300> \\ p_{end} = <0,-300> \\$$

$$dX = x_{end} - x_{start} = (0) - (-300) \\ \boxed{dX = 300} \\$$

$$dY = y_{end} - y_{start} = (-300) - (-300) \\ \boxed{dY = 0} \\$$

$$\theta_{1 \rightarrow 2} = atan2(dY,dX) \\ \theta_{1 \rightarrow 2} = atan2(0,300) \\ \theta_{1 \rightarrow 2} = 0 \mbox{rad} = 0^o$$

$$\mbox{Turn Angle} = \theta_{1 \rightarrow 2} - \theta_1 \\ \mbox{Turn Angle} = 0 - (-1.5708) \\ \mbox{Turn Angle} = +1.5708 = +90^o \\$$

The distance you need to travel after turning $\mbox{Turn Angle}$ is given by: $$\mbox{distance} = \sqrt{dX^2 + dY^2} \\ \mbox{distance} = \sqrt{(300)^2 + (0)^2} \\ \mbox{distance} = \sqrt{(300)^2} \\ \mbox{distance} = 300 \\$$

Now the new heading $\theta_1$ is equal to the absolute heading between start and end:

$$\theta_2 = \theta_{1 \rightarrow 2} \\$$

This process repeats as you see fit.

:Second Update: Code Issues

You need to turn (left or right) by the amount turnAngle. You have not described your physical robot so nobody can tell you exactly how to turn.

You have your code in void loop(), which makes me think you are running the same code over and over, but you never take the following steps:

1. You never declare a starting heading.
2. You fail to define turnAngle as the difference between atan2((yt-yc),(xt-xc)) and the starting heading.
3. After you have turned, you fail to set your new heading.
4. You fail to move forward.
5. You fail to update your current position with the target position

I would suggest modifying your code as follows:

Last update

The wheel base is the distance from the center of the tire to the center of the other tire. Assuming that the motor speeds while turning are the same magnitude but opposite sides (leftMotorSpeed = - rightMotorSpeed), and the wheels are the same diameter, you can find out how quickly you are turning by using the arc length equation s = L*theta, so theta = s/L, where theta is the rotation, s is the arc length, and L is half the wheel base. Then, taking a derivative, $\dot{\theta} = \dot{s}/L$, where $\dot{s}$ is the linear tire speed, given by $\dot{s} = r_\mbox{tire}*v_\mbox{motor}$

So you can take motor speed and get back to rotation rate, and then numerically integrate that and compare to the $\mbox{Turn Angle}$:

thetaDot = (rTire * vMotor) / (wheelBase/2);

then

where dT is the sample/sweep time on the controller. Finally, use a while loop to turn the motors until the desired angle is reached. Putting it all together:

//current points
float xc = -300;
float yc = 300;

//target points

float xt = -300;
float yt = -300;

//turning  angle
float turnAngle;

//***************
//***************

//***************
float turnRight = 1;
float turnLeft = -1;
float wheelBase = <This is a number you get by measuring your vehicle>
float tireRadius = <This is a number you get by measuring your vehicle>
float speedModifier = 0;
float vRightMotor = 0;
float vLeftMotor = 0;
float vMotor = 0;
float theta = 0;
float distance = 0;
//**************

void setup() {
// pin setup
Serial.begin(9600);
}

void loop() {
//*************
destinationHeading = atan2((yt-yc), (xt-xc)); //calculate turning angle
//*************

if (turnAngle > 180) {
turnAngle = turnAngle-360;
}

if (turnAngle < -180) {
turnAngle = turnAngle+360;
}

//***************
if (turnAngle < 0) {
speedModifier = turnRight;
}

if (turnAngle > 0) {
speedModifier = turnLeft
}

theta = 0;
while abs(abs(theta)-abs(turnAngle)) > 0
{
vRightMotor = speedModifier * <100 percent speed - varies by your application>;
vLeftMotor = -speedModifier * <100 percent speed - varies by your application>;
<send the vRightMotor/vLeftMotor speed commands to the motors>
vMotor = vRightMotor;
}
<send zero speed to the motors>