Simple Pure Pursuit navigation algorithm

[Ted Meyers]

I've been working on a simple pure pursuit algorithm for navigation.  The idea is to always try to turn the rover to point at an imaginary pursuit point which travels ahead of the rover along the course from waypoint to waypoint.  I've found a few special cases that help a lot: first, stop the pursuit point and wait if the rover gets too far behind.  Second, stop the pursuit point at each waypoint and wait for the rover to get within range of the waypoint.  Third, jump the pursuit point out ahead a bit when starting from a waypoint (assuming that the rover made it to the waypoint, it is already on track).

Also, never compare angles, there are too many special cases where things wrap around, etc.  Always convert to rectangular coordinates and use cross-products, dot-products, and vector math.

I'll attach the Processing code, for anyone interested (sorry it is a bit primitive), and the youtube video of the simulator running: RoverSim  BTW, I love Processing for this sort of thing!

[Ted Meyers]

Okay, for some reason I couldn't attach the processing code, so here it is:

double STEP_TIME = 0.1;

double SCALE_P = 1.0;

double SCALE_V = 8.0;

double WP_DIST = 30.0;

double FOLLOW_DIST = 50.0;

int X_OFF = 0;

int Y_OFF = 0;

int CRUMB_SZ = 500;

int NUM_WP=5;

int X = 0;

int Y = 1;

int DIM = 2;

boolean isStart = false;

int sim_counter = 0;

double sim_t = 0.0;

double sim_v = 0.0;

double sim_d = 0.0;

double sim_xp = 0.0;

double sim_yp = 0.0;

double sim_xv = 0.0;

double sim_yv = 0.0;

double steer_fx = 0.0;

double steer_fy = 0.0;

double[][] way_pts = {{300.0,500.0},{100.0, 500.0},{100.0,100.0}, {500.0,100.0},{500.0, 500.0}};

int cur_way_pt = 0;

double[][] crumbs = new double[CRUMB_SZ][DIM];

int crumb_idx = 0;

void setPosition(double x, double y) {

  sim_xp = x;

  sim_yp = y;

}

void setVelocity(double val) {

  sim_v = val;

}

void setSteering(double val) {

  sim_d = val;

}

void changeSteeringBy(double delta) {

  sim_d += delta;

}

void setFollow(double x, double y) {

  steer_fx = x;

  steer_fy = y;

}

int getCurrentWaypoint() {

  return cur_way_pt;

}

int getPreviousWaypoint() {

  int pwp = (cur_way_pt==0)?(NUM_WP-1):cur_way_pt-1;

  return pwp;

}

void setup() {

  size(600, 600);

  initSim();

}

void draw() { 

  background(240);

 

  if (isStart) {

    updateSim();

    updateSteering(sim_xp, sim_yp, sim_d);

    updateWaypoint(sim_xp, sim_yp);

    updatePursuit(sim_xp, sim_yp);

    updateCrumbs(sim_xp, sim_yp);

    updateRover();

  }

    

  drawCrumbs();

  drawWayPoints();

  drawRover();

  drawPursuit();

  

  delay(1);

}

void initSim() {

  cur_way_pt=1;

  setFollow(280.0, 500);

  setPosition(300.0, 400.0);

  setVelocity(1.5);

  setSteering(4);

  updateRover();

}

void updateSim() {

  sim_counter++;

  sim_t += STEP_TIME;

  if ((sim_counter % 20) == 0) {

    sim_d += random(-0.2, 0.2);

    sim_xp += random(-1.5, 1.5);

    sim_yp += random(-1.5, 1.5);

  }

}  

void updateSteering(double x, double y, double dir) {

  double fdx = (x-steer_fx);

  double fdy = (y-steer_fy);  

  double fdist = calcDist(fdx, fdy);

  

  // Compute the cross product -- tells which way to turn

  double cdir = cos((float)dir);

  double sdir = sin((float)dir);

  double xprod = (sdir*fdx - cdir*fdy)/fdist;

  

  double val = 0.0;

  if (xprod < -0.1) val = 0.06;

  else if (xprod > 0.1) val = -0.06;

  changeSteeringBy(-val);

}

void updateWaypoint(double x, double y) {

  double cwpx = way_pts[cur_way_pt][X];

  double cwpy = way_pts[cur_way_pt][Y];

  double cdist = calcDist(x-cwpx, y-cwpy);

  

  if (cdist < WP_DIST) cur_way_pt++;

  if (cur_way_pt >= NUM_WP) cur_way_pt = 0;

}

void updatePursuit(double x, double y) {

  double fdist = calcDist(x-steer_fx, y-steer_fy);  

  if (fdist < FOLLOW_DIST) {

    int cwp = getCurrentWaypoint();

    int pwp = getPreviousWaypoint();

    double wpx = way_pts[cwp][X] - way_pts[pwp][X];

    double wpy = way_pts[cwp][Y] - way_pts[pwp][Y];

    double wpdist = calcDist(wpx, wpy);

    double cdist = calcDist(x-way_pts[cwp][X], y-way_pts[cwp][Y]);

    

    double wp_ratio = (wpdist - cdist + FOLLOW_DIST)/wpdist;

    if (wp_ratio > 1.0) wp_ratio = 1.0;

    

    double nx = way_pts[pwp][X] + wp_ratio*wpx;

    double ny = way_pts[pwp][Y] + wp_ratio*wpy;

    setFollow(nx, ny);

  }

}

void updateCrumbs(double x, double y) {

  if ((sim_counter % 3) == 0) {

    crumbs[crumb_idx][X] = sim_xp;

    crumbs[crumb_idx][Y] = sim_yp;

    crumb_idx++;

    if (crumb_idx >= CRUMB_SZ) crumb_idx = 0;

  }

}

void updateRover() {

  sim_xv = sim_v*cos((float)sim_d);

  sim_yv = sim_v*sin((float)sim_d);

  sim_xp += sim_xv;

  sim_yp += sim_yv;

}

void drawRover() {

  float x = calcDrawX(sim_xp);

  float y = calcDrawY(sim_yp);

  float xd = x + calcDrawV(sim_xv);

  float yd = y + calcDrawV(sim_yv);

    

  stroke(0);

  fill(250, 0, 0);

  ellipse(x, y, 10, 10);

  stroke(0, 0, 200);

  line(x, y, xd, yd);

}

void drawPursuit() {  

  stroke(180);

  float nx = calcDrawX(steer_fx);

  float ny = calcDrawY(steer_fy);

  float d = calcDraw(2*FOLLOW_DIST);

  fill(0, 250, 0);

  ellipse(nx, ny, 5, 5);

  noFill();

  ellipse(nx, ny, d, d);

}

void drawCrumbs() {  

  stroke(100);

  for (int i=0; i<CRUMB_SZ; i++) {

    point((float)crumbs[i][X], (float)crumbs[i][Y]);

  }

}

void drawWayPoints() {

  fill(0, 0, 250);

  for (int i=0; i<NUM_WP; i++) {

    stroke(0);

    float x = calcDrawX(way_pts[i][0]);

    float y = calcDrawY(way_pts[i][1]);

    if (i==cur_way_pt) {

      float d = calcDraw(2*WP_DIST);

      noFill();

      ellipse(x, y, d, d);

      fill(0, 0, 250);

    }

    ellipse(x,y, 5, 5);

  }

}

float calcDrawV(double d) {

  float v = (float)(d*SCALE_V);

  return v;

}

float calcDraw(double d) {

  float v = (float)(d*SCALE_P);

  return v;

}

float calcDrawX(double x) {

  float dx = X_OFF + calcDraw(x);

  return dx;

}

float calcDrawY(double y) {

  float dy = Y_OFF + calcDraw(y);

  return dy;

}

double calcDist(double x, double y) {

  double d = sqrt((float)(x*x + y*y));

  return d;

}

void keyPressed() {

  switch (key) {

    case ' ':

      initSim();

      break;

    case 'p':

      isStart = !isStart;

      break;

  }

}

[Michael Shimniok]

I've been working on a simple pure pursuit algorithm for navigation.  The idea is to always try to turn the rover to point at an imaginary pursuit point which travels ahead of the rover along the course from waypoint to waypoint.  I've found a few special cases that help a lot: first, stop the pursuit point and wait if the rover gets too far behind.  Second, stop the pursuit point at each waypoint and wait for the rover to get within range of the waypoint.  Third, jump the pursuit point out ahead a bit when starting from a waypoint (assuming that the rover made it to the waypoint, it is already on track).

Sounds like you've nicely and pragmatically dodged some of the crazy issues I ran into when trying to implement pure pursuit -- I was trying to make it as mathematically correct as I could (that's assuming I was understanding the definition correctly) but it was definitely tricky dealing with waypoint changes and calculating a point along the path that is precisely a set distance away led to nightmares when the robot got too far off the path. Realistically it's unnecessary to be that "correct" to the definition, I think. 

The main issue is to make sure  the robot has a way to deal with not only heading but also cross-track error. Secondarily, that the robot can negotiate turns smoothly and within handling limits. The true pure pursuit algorithm itself isn't all that great at making smooth turns with rectangular paths and four waypoints. Your way is seems better.

Also, never compare angles, there are too many special cases where things wrap around, etc.  Always convert to rectangular coordinates and use cross-products, dot-products, and vector math.

The angle stuff has given me headaches for awhile now (I mean everywhere in my code). I usually normalize to -180 to +180 when adding/subtracting and normalize 0-360 otherwise which seems to work reliably but I am still chasing a bug or two. The MatrixPilot folks had the right idea by representing angles as, e.g., 0x00 - 0xff and letting the hardware do the wrapping around.

I'll attach the Processing code, for anyone interested (sorry it is a bit primitive), and the youtube video of the simulator running: RoverSim  BTW, I love Processing for this sort of thing!

Looks really good. Processing is definitely awesome for this sort of quick visualization/prototyping kind of deal. 

Michael

[Ted Meyers]

The angle stuff has given me headaches for awhile now (I mean everywhere in my code). I usually normalize to -180 to +180 when adding/subtracting and normalize 0-360 otherwise which seems to work reliably but I am still chasing a bug or two. The MatrixPilot folks had the right idea by representing angles as, e.g., 0x00 - 0xff and letting the hardware do the wrapping around.

Yeah, using binary coded angles (Binary Angular Measurement (BAM) helps a lot and is a really cool trick to boot, but will still give you headaches if you try to do something like average angles (like in a filter).  The problem is when you try to average 359 degrees and 1 degree, you get 180 degrees.  Then you switch to -180 to +180, which fixes that, but eventually you try to average -179 and 179 and get 0.  Oops, add a special case for this, but then you run into another problem caused by that special case -- I'll let you figure it out.

No, the solution is to convert the angles to vectors and average the vectors.  No special cases, no BAM tricks required; it just works -- Highly recommended!

Ted

[Julien Hoachuck]

Hey nice piece of code! I just had some questions... 

1. What is the purpose of updateCrumbs and all associated with it?

2. What is the purpose of wpratio?

- Julien

[Thomas Roell]

Wondering whether one could use splines as path along the waypoints, with one endpoint being the current rover position. 

[Ted Meyers]

Ok, haven't looked at that code in a while -- sorry about the complete lack of comments (it was just for testing out some ideas).  Anyway, the "crumbs" stuff is for displaying the simulated rover's path on the screen.  "wp_ratio" is the ratio of the distance traveled to the waypoint -- it is used to update the pursuit point.

[Julien Hoachuck]

Don't worry about the lack of comments :D At least I got a response... What I don't see is the actual calculation for the arc length:

      arc length = 2x/(l^2)

Is this somehow simplified in the "wpratio"?

[Ted Meyers]

Kind of; it's not actually calculating an arc length, hence the "Simple" Pure Pursuit.  The idea is to go back to the origin of the pursuit curve -- a pursuer (predator) and a pursued (prey), where the predator is constantly turning in order to directly follow its prey.  The point of wp_ratio is to calculate/update the position of the prey.  (Although, I am simulating a minimum turn radius for the "predator").  Other than that, it's really very simple, the only complication is what to do when the prey reaches the waypoint and its transition to the next waypoint.

--
You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/8b730681-1f1f-4e56-854c-f068ac2ed2a5%40googlegroups.com.

For more options, visit https://groups.google.com/d/optout.

[Luis Pedro Cobos Yela-vives]

Hello, I have been working on something similar but on a matlab script running in labview; and I can't manage to get my steering angle correctly,you guys seem topoicwise on this, could you guys help me out?

[Jon Watte]

It's kind of hard to help when we don't know what you've already tried, and how you know it's failing.

In general, what I do is have a local "forward" and "right" vector for the rover.

Then, I project the vector from rover to target to the "right" vector baseline (subtract (target dot forward) from target)

Then I dot the outcome of that with the "right vector and multiply by some sensitivity factor, and that's my steering angle (0 in the center)

Sincerely,

jw

Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

--

You received this message because you are subscribed to the Google Groups "diyrovers" group.

To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/4cd54d08-28cb-4e58-bc6a-fd6b4b018865%40googlegroups.com.

[Wayne Holder]

I came up with a solution to this problem some time ago and it's worked quite well for me.  I've attached a Java .jar file that draws a demo animation (you should be able to run it just by double clicking it, but you'll have to rename the extension from .raj to to .jar, as Google won't let me upload a .jar file), as well as the Java source code which contains the code that makes it work.  The method intersect() computes the two intersection points, 0 and 1, between a circle of a defined radius around point c and a line drawn though two points, a and b, and/or the tangent point closest to the line from point c from which you can pick the point closest to the goal as the steer toward.  In this case, point a represents the waypoints you've just passed and point b is the waypoint your heading for.  Point c is the car/rover.  Note: it's probably easier to understand if you just run the demo...

You can click and drag in the window to move point c around and watch the code compute the point to steer toward based on the intersection of the circle and the line between the two waypoints.  Note: If the line is outside the circle, it steers toward the tangent point until it's close enough to head in the direction of the goal.  There is also code that computes the distance to the goal, which you can use to decide when you've reached it (or gone past is the distance is negative, which helps avoid the problem of the car/rover not detecting it reached the waypoint because it drove too wide to the left or right to get close enough.)

By varying the radius of the intersect circle and how close you have to be to the destination waypoint before you switch to the next waypoint you can make the car/rover smoothly cutting the corners rather than driving past it and then wildly oversteering to get back on track.  Note: this varies with speed and the car/rover's max steering angle.

Wayne

On Sun, May 7, 2017 at 12:11 PM, Luis Pedro Cobos Yela-vives <lpc...@gmail.com> wrote:

--

You received this message because you are subscribed to the Google Groups "diyrovers" group.

[Luis Pedro Cobos Yela-vives]

I kind of needed to be accepted into the group before I can further participate, now that I am in, I'll show you what I have

Some notes  as background first: The robot already works and recives data. It is able to mantain a constant speed and move the steering wheel. The problem happens with the pure pursuit algorithm I'm trying to implement. It works in matlab script through labview, sending the angle in BUS-CAN format. The 15.26108 factor is the relationship between the angle of the wheels and the steering wheel. Orientation goes from -180 to 180 degrees (value is also sent in radians) being 0 at X axis increasing values.

Ok, I used as a base to guide myself the realesed code from Thomas Hellström and Ola Ringdahl (paper: http://www8.cs.umu.se/~thomash/reports/07_Hellstrom.pdf). I am having problems with the results my code gives. First I can't use it to go from not the origin it gives me an error (starting somewhere that it is not 0,0 and ending in 0,0). Second the steering angles resulting do not make that much sense (they are in the ranges between 0.69817824377 and - 0.69817824377; but normally get stucked in either the maximum or the minimum).

I attach the matlab codes, that are used and some pictures of the part of the labview file that uses it

Maybe one of you has an idea to help me out =D

On Sunday, May 7, 2017 at 11:46:26 PM UTC+2, Jon Watte wrote:

It's kind of hard to help when we don't know what you've already tried, and how you know it's failing.

Sincerely,

jw

Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

On Sun, May 7, 2017 at 12:11 PM, Luis Pedro Cobos Yela-vives <lpc...@gmail.com> wrote:

Hello, I have been working on something similar but on a matlab script running in labview; and I can't manage to get my steering angle correctly,you guys seem topoicwise on this, could you guys help me out?

On Monday, November 19, 2012 at 6:56:51 AM UTC+1, Ted Meyers wrote:

I've been working on a simple pure pursuit algorithm for navigation.  The idea is to always try to turn the rover to point at an imaginary pursuit point which travels ahead of the rover along the course from waypoint to waypoint.  I've found a few special cases that help a lot: first, stop the pursuit point and wait if the rover gets too far behind.  Second, stop the pursuit point at each waypoint and wait for the rover to get within range of the waypoint.  Third, jump the pursuit point out ahead a bit when starting from a waypoint (assuming that the rover made it to the waypoint, it is already on track).

Also, never compare angles, there are too many special cases where things wrap around, etc.  Always convert to rectangular coordinates and use cross-products, dot-products, and vector math.

I'll attach the Processing code, for anyone interested (sorry it is a bit primitive), and the youtube video of the simulator running: RoverSim  BTW, I love Processing for this sort of thing!

--

You received this message because you are subscribed to the Google Groups "diyrovers" group.

To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

[Jon Watte]

PNG is not a great format for reading text :-)

My guess is that perhaps you are dividing by steering-wheel factor instead of multiplying?

0.6 radians would mean about 40 degrees steering angle. (Then again, I don't particularly know the code you're using -- let's talk about first principles, and write the code you actually need)

Anyway, what is your coordinate system?

I recommend doing this:

1) Do path planning/mapping/whatever in some assumed "global" coordinate system. (If you drive very far, then do origin shifting -- different subject.)

2) If you represent the pose of the rover with matrix P, then you can transform the target vector t into rover-relative coordinates using simply tr = inv(P) * t

3) Once you have tr, there are multiple ways of turning that into steering wheel input.

The most basic one is to calculate the angle to tr by using atan2(), and then simply set the wheels at that angle. The draw-back of this method is that the rover will basically take an asymptotic trajectory towards the goal, without ever driving 100% straight.

A more advanced one is to calculate the maximum steering angle allowed for the current rover speed (for stability) and steer by maximum left/right until the target is within some narrow band of "acceptably straight ahead," and then switch over to a proportional steering controller based on how much left/right the target is.

Even more advanced models will take the vehicle dynamics into account, plan a preferred trajectory, and issue controls to follow that trajectory. But then we're past the point where "simple target chasing" is the right input :-)

So, some code:

/*

 * Assuming -Y == right, X == forward, clockwise steering direction (like a compass, left-handed)

 * Calculate a simple steering angle to head towards the given target.

 */

float desiredSteeringAngleSimple(Matrix pose, Vector target) {

    Vector relativeTarget = invert(pose) * target;

    float steerRadians = atan2f(-relativeTarget.y, relativeTarget.x);

    float controlSteer = Gain * steerRadians;

    return controlSteer;

}

/*

 * Assuming -Y == right, X == forward, clockwise steering direction (like a compass, left-handed)

 * Calculate a steering angle to head towards the given target based on vehicle capabilities.

 */

float desiredSteeringAngleBetter(Matrix pose, Vector target, Vector velocity) {

    Vector relativeTarget = invert(pose) * target;

    float steerRadians = atan2f(-relativeTarget.y, relativeTarget.x);

    float controlSteer = HighGain * steerRadians;

    float maxControlSteer = calcMaxControlSteerBasedOnVelocity(velocity);

    if (controlSteer < -maxControlSteer) {

        controlSteer = -maxControlSteer;

    } else if (controlSteer > maxControlSteer) {

        controlSteer = maxControlSteer;

    }

    return controlSteer;

}

"calcMaxControlSteerBasedOnVelocity" of course depends on your vehicle.

Sincerely,

jw

Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/41f83fd6-24d5-4a37-a5a8-79d94252f8fe%40googlegroups.com.

[Luis Pedro Cobos Yela-vives]

Thanks for answering. The code in the PNG is the matlab pure pursuit pasted in labview.

 My coordinate system is set by a previous program, it takes to points in UTM, the first one is the new origin (0,0) and the second one degines the angle of the axis. After this all points are given in (x,y) in refernce to the new axis. The vehicle orientation is given by the heading, this ranges from pi to -pi radians

The speed is constant. It is a project to try an ADB radar.

The vehicle has a relation from -40 to 40 deg answering to α ≈ 15.26108* δ, wher δ(steering angle of the outer wheel) and α(stering wheel angle). Therefore my propulsion gain is 1

 Your asumptions of x and y forward and left are correct.

My points have a minimum distance between them  of S, S=Kla*VehicleLength/12, (kla(is look ahead point constant))

I like your second equation, but I don't get all the variables.

If you have labview I can show you the pure pursuit part.

regards

[Ted Meyers]

You might want to consider that the math may be getting on your way.  The origins of the Pure Pursuit algorithm are base on aircraft combat, where on aircraft attempts to catch another by flying directly towards its current position (the pursuit point).  (As opposed to flying where it will be, or where it was).  Most discussions of the algorithm attempt to be very clinical and pristine in calculating exact  radius curves to follow, etc.  I disagree with this approach when used in real-world conditions; things get messy out there, you don't need a lot of precision, and you need to be able to deal with noise.  So, if what you are working on is a real-world application and not just a mathematical/computer simulation, don't worry about steering angle -- just make the steering corrections large enough as to be significant.  Tuning the following distance and putting in some dead zone and a PID control on the steering should get you close enough.

Or, even if your project is about getting the math perfect, you might try using discrete turning angle (like -40, 0, 40) and seeing what happens.  That might give you some insight into what you are doing wrong in the math.

On Tue, May 9, 2017 at 5:51 AM, Luis Pedro Cobos Yela-vives <lpc...@gmail.com> wrote:

Thanks for answering. The code in the PNG is the matlab pure pursuit pasted in labview.

 My coordinate system is set by a previous program, it takes to points in UTM, the first one is the new origin (0,0) and the second one degines the angle of the axis. After this all points are given in (x,y) in refernce to the new axis. The vehicle orientation is given by the heading, this ranges from pi to -pi radians

The speed is constant. It is a project to try an ADB radar.

The vehicle has a relation from -40 to 40 deg answering to α ≈ 15.26108* δ, wher δ(steering angle of the outer wheel) and α(stering wheel angle). Therefore my propulsion gain is 1

 Your asumptions of x and y forward and left are correct.

My points have a minimum distance between them  of S, S=Kla*VehicleLength/12, (kla(is look ahead point constant))

I like your second equation, but I don't get all the variables.

If you have labview I can show you the pure pursuit part.

regards

[Luis Pedro Cobos Yela-vives]

Did you managed to view my code?

Do you have labview with mathscript?
I kind of hit a brick wall and don't know how to solve my problem...

[Jon Watte]

I kind of hit a brick wall and don't know how to solve my problem...

I still don't understand what your problem is. You have not reduced the problem to a simple enough test case that other people on a public mailing list can reasonably look at it and help.

Also,

I like your second equation, but I don't get all the variables.

Okay, which specific variables do you not get?

Sincerely,

jw

Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

--
You received this message because you are subscribed to the Google Groups "diyrovers" group.

To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/7bef164c-f5aa-47e8-ad07-241967efb846%40googlegroups.com.

[Luis Pedro Cobos Yela-vives]

All right; I am trying to implement a pure pursuit algorithm. My problem it is that in theory the code should work(if I test values it does) but when doing a field test it fucks-up. The problem is that the steering angle it calculates are unreasonable.

To know:

*The GPS sends the heading(orientation of the front of the vehicle) in radians from pi to -pi
*The GPS sends coordinates X and Y in an already defined plane of the corresponding UTM, sending the current position in meters away from (0,0).
*The GPS also sends the current speed, yet this is constant and is just for checking that the can message sent is retaining the current speed.
*The vehicle is an SUV, front steering, 4WD almost 4meters long and 2 wide.
* On (0,0) facing forward -Y == right, X == forward,and the heading ==0
* Recorded paths by the path recording program have a minimum distance between them  of S, S=Kla*VehicleLength/12, (kla(is look ahead point constant))

*The vehicle has a relation from -40 to 40 deg answering to α ≈ 15.26108* δ, where δ(steering angle of the outer wheel) and α (steering wheel angle); resulting in a propulsion gain of 1.
*Real Position and heading is received in real time from the bus can 500kbd

*The Angle is send to the robot through the CAN BUS (This works the robot receives the wrong angle my program calculates)

The code goes as follows(I attached it previously):
thePathx=thePathx.' %transposes path from origin
thePathy=thePathy.'
 if(Kla==0)
          Kla=Speed/3.6
      end
nextWayPoint = 0; stop=0;vehiclex=(round(vehiclex*10))/10; vehicley=(round(vehicley*10))/10;
kp =1; % propulsion gain
believedPos= [vehiclex vehicley];  % real position
PTH = [thePathx thePathy];
lookAhead = Kla*vehiclelength; % look ahead distance
nrOfPoints = length(PTH);
[row, pathPoint, pathDist] = getPathPoint(believedPos, PTH); % find nearest point on the path
row=round(row);
segmentVector = PTH(row,:) - PTH(row+1,:);
[pathAngle, r] = cart2pol(segmentVector(1), segmentVector(2));
lookAheadPoint = pathPoint - [cos(pathAngle)*lookAhead, sin(pathAngle)*lookAhead];
[rw,pp,d] = getPathPoint(lookAheadPoint, PTH);
[d,s,p] = pointLineDist(lookAheadPoint,PTH(row,:), PTH(row+1,:));
if(rw == nrOfPoints-1 & s == 0)
    lookAheadPoint = PTH(nrOfPoints,:); % set the last look ahead point to goal point
else
    j=row;   
    while(j<nrOfPoints -1)
        SP = (PTH(j,:)); % startpoint
        EP = (PTH(j+1,:)); % endpoint
        [rest, onSegment, point] = pointLineDist(lookAheadPoint, SP, EP );%Makes it polar   
        if(onSegment == 0 & rest > 0)
            segmentVector = PTH(j+1,:) - PTH(j+2,:);
            [pathAngle, r] = cart2pol(segmentVector(1), segmentVector(2));
            lookAheadPoint = PTH(j+1,:) - [cos(pathAngle)*rest, sin(pathAngle)*rest];
        else
            break
        end
        j=j+1;
    end
end
lookAheadVector = lookAheadPoint - believedPos;
[lookAheadAngle, D] = cart2pol(lookAheadVector(1), lookAheadVector(2));%Makes it polar
alpha = pi/2-vehicleor + lookAheadAngle;
dx = D*cos(alpha);
radius = -D^2/(2*dx);


errorAngle =((calcAngle(radius, vehiclelength)*2)*.8+(lookAheadAngle - vehicleor)*.2);
errorAngle=round((errorAngle*10))/10;
if(errorAngle > pi)
    errorAngle =-(2*pi-errorAngle) * kp;
elseif(errorAngle < -pi)
    errorAngle = 2*pi+errorAngle *kp;
end
steeringAngle = max(min(errorAngle, deg2rad(40)),  deg2rad(-40));
steeringWheelAngle=rad2deg(steeringAngle);
steeringWheelAngledeg=(steeringWheelAngle)*15.26108;

[Luis Pedro Cobos Yela-vives]

The part I didn't get is from here:

float desiredSteeringAngleBetter(Matrix pose, Vector target, Vector velocity) {

    Vector relativeTarget = invert(pose) * target;

    float steerRadians = atan2f(-relativeTarget.y, relativeTarget.x);

    float controlSteer = HighGain * steerRadians;

    float maxControlSteer = calcMaxControlSteerBasedOnVelocity(velocity);

    if (controlSteer < -maxControlSteer) {

        controlSteer = -maxControlSteer;

    } else if (controlSteer > maxControlSteer) {

        controlSteer = maxControlSteer;

    }

    return controlSteer;

}

Where does the vehicle orientation comes in? and the variables on the right of this 2

[Jon Watte]

The problem is that the steering angle it calculates are unreasonable.

What does your data logging say about the inputs and outputs when this happens?

What are some examples of "reasonable" inputs and outputs?

What are some examples of "unreasonable" inputs and outputs?

Sincerely,

jw

Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

--
You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/9b1ee95e-bb59-4880-bc26-47c9c3ceb359%40googlegroups.com.

[Jon Watte]

Where does the vehicle orientation comes in?

That is the "pose" of the vehicle.

Typically, a "pose" is represented as a matrix. In 2D (flat plane) you can use a 3x3 matrix. In 3D, you can use a 4x4 matrix.

So, assuming column-vectors-on-right conventions, the "pose" matrix will look like:

co -sn  X

sn  co  Y

0   0   1

(This assumes counter-clockwise rotation convention, like a compass; if you want clockwise like in trigonometry, flip the signs of the "sn" values.)

(For row-vectors-on-the-left conventions, you transpose this matrix.)

"sn" is the sine of the heading angle, and "co" is the cosine of the heading angle.

Note that this "pose" matrix represents "how do I take a point local to the rover, and transform it into global coordinates."

But the problem you have is that the position of the virtual target is in global coordinates. Thus, you must invert this pose matrix to transform global position to rover-relative (local) position.

Hence:

invert(pose) * target

The atan2f is the "arc tangent" function, except broken out in a quotient so that you resolve the ambiguity of positive versus negative rotations. This function is standard in most language math libraries. I would assume that Matlab has it, too (my code assumes the C/C++ standard library.)

Also, beause the code assumes "compass" headings, but atan2f works in "trigonometry" rotation conventions, the conventions are being translated by ordering the sign/order of the arguments to the function to get the correct output convention.

Sincerely,

jw

Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

--

You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/2b8900ef-39df-46b6-a701-15e8a50c1902%40googlegroups.com.

[Luis Pedro Cobos Yela-vives]

The labview file has a GUI to try it.

For example if my heading is PI/4 and i want to go from -1,-1 to 1,1 and then to 2,2 and to 3,3 and so on the angle is 0
If my heading is PI/2 I could move on in Y and the angle remains 0
and the contrary for -Pi/2
if the heading is 0 it should move on X

To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

[Jon Watte]

The labview file has a GUI to try it.

Well, I suppose that's good for people who use LabView?

[row, pathPoint, pathDist] = getPathPoint(believedPos, PTH); % find nearest point on the path

In Pure Pursuit, you do not "pick the closest point." Instead, you "Pick the first point," and then you drive until you are "close enough" to the first point, and then you "pick the next point."

I e, the selection of point is based on a variable that stores the "index to the next point" and when you're close enough, you increment this index.

For example if my heading is PI/4 and i want to go from -1,-1 to 1,1 and then to 2,2 and to 3,3 and so on the angle is 0
If my heading is PI/2 I could move on in Y and the angle remains 0
and the contrary for -Pi/2
if the heading is 0 it should move on X

I have a hard time making sense of this verbal description of the measurements.

In the steering algorithm, then inputs are:

RoverX, RoverY, RoverHeading, TargetX, TargetY

The output is:

RoverSteering

To create proper validation for this algorithm, you need to come up with variants of the five input variables that exercise both "common" cases, and "extrema" cases of the algorithm.

Then, you create a table of all the combinations of inputs that are interesting.

Then, you run this table through the code, and validate that the output from each of these test cases is correct.

Then, when you test this for real, you should log the five inputs in your data logging solution (whatever that is,) as well as the output.

When you see that the rover makes the wrong decisions, you should make note of the time at which this happens.

This means that you can then go back to your data log, and re-create the table of inputs for the time at hand.

That, in turn, means that you can add this as a test case for the algorithm.

What I'm describing is the very basics of fundamental engineering process, so I'm hoping you are familiar with this.

Good luck on your project!

jw

Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/b3d54e45-1bbb-4f6b-b037-3b12f6950a8f%40googlegroups.com.

[Ted Meyers]

Okay, I'll try to help...

Starting at the bottom of the code and working up: I see that you are clipping the steering angle to be between -40 and 40 degrees.  That is good, but the multiply by 15.26 worries me.  I know that you said something about it being a conversion from the steering wheel to wheel angles, just know that this multiply will get you angles in the range of -600 to +600 (degrees?)

Previous to that, the error angle calculation looks odd, what is calcAngle function doing?  Radius and vehicle length aren't even angles.  Maybe I am misunderstanding something here.  The lookahead and vehicle orientation difference makes sense, but the multiply by 0.2, why?  So, it is normal to take the difference in the heading to the pursuit point and the current vehicle heading, just be careful with the angles if they start to wrap around, might be better to use vector differences and then compute the heading from the vector.  Also, you are unrolling the error angle in radians (by adding or subtracting 2 pi)  that is normal, the multiply by kp is not, but it looks like kp = 1, so it is in effect a no-op.  And the round operation is not really necessary.  Anyway, once you get the heading difference, all you should have to do is scale it to your steering angle correction -- -180 deg to 180 deg of difference scaled to -40 to +40 of steering (or -600 to +600 -- see above).

Oh yeah, and you might want to do some sanity testing on the GPS heading (which in some cases I've seen come out VERY WRONG).  And some sanity checks on the GPS location while you are at it.

[Luis Pedro Cobos Yela-vives]

Calc angle does the following:

beta = acos(vehiclelength/radius);
angle = pi/2 - beta;

My round operation is to have angles with just one decimal. For hardware purposes

I'll do the sanity checks and configure the can reciever to do them.

[Luis Pedro Cobos Yela-vives]

Fixed it,it works

thePathx=thePathx.'

%errorAngle =((calcAngle(radius, vehiclelength)*2)*.8+(lookAheadAngle - vehicleor)*.2);
errorAngle =((atan(vehiclelength/radius))*.8+(lookAheadAngle - vehicleor)*.2);
%errorAngle =(((atan(vehiclelength/radius))));
errorAngle=round((errorAngle*1000))/1000;

if(errorAngle > pi)
    errorAngle =-(2*pi-errorAngle) * kp;
elseif(errorAngle < -pi)
    errorAngle = 2*pi+errorAngle *kp;
end
steeringAngle = max(min(errorAngle, deg2rad(40)),  deg2rad(-40));
steeringWheelAngle=rad2deg(steeringAngle);

steeringWheelAngledeg=(((steeringWheelAngle)*15.26108));

[Ted Meyers]

Ah, so it was the error angle.  You might try the atan2 function, if there is one in labview (at least in C, atan2 is more robust than atan -- it gets the angle correct for all 4 quadrants.)

--

You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/6f7d8765-d118-45fc-a2e3-dcddc883ddd0%40googlegroups.com.

[Luis Pedro Cobos Yela-vives]

Matlab also has an atan2 function(suposedly does the same thing as in C), if I change it it won't mess up?

thanx for the support guys!

To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.