Robot path planning

Question

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.

Answer 1

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 $$

Now, the change in heading:

$$ \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 $$

Now, the change in heading:

$$ \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

theta = theta + thetaDot*dT;

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 startingHeading = 0;
float currentHeading = startingHeading;
//***************

//***************
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
  destinationHeading = destinationHeading * 180/3.1415; //convert to degrees
  turnAngle = destinationHeading - currentHeading;
  //*************

  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;
    thetaDot = (tireRadius * vMotor) / (wheelBase/2);` 
    theta = theta + thetaDot*dT;
    }
  <send zero speed to the motors>

  currentHeading = destinationHeading;
  distance = sqrt((xt - xc)^2 + (yt - yc)^2);
  if (distance > 0) {
    // go forward
  }
  xc = xt;
  yc = yt;
  //****************
}