Guided Vector Fields

PreviousAdvanced Pure Pursuit NextAutonomous Movement Libraries

Last updated 1 year ago

Guided Vector Fields

Guided vector fields are an advanced topic and can be pretty tricky to implement. Therefore if you are looking to integrate GVFs into your codebase, we highly recommend that you make use of the library since they make use of GVFs. Because their library is so well documented and tested it will likely save you a lot of time and effort to use their high-quality code.

Guided Vector Fields (GVFs) are a concept within the field of robotics that involves using vector fields to assist in motion planning, control, and navigation of robotic systems. This approach leverages mathematical representations to guide the movement of robots in complex environments, enabling them to efficiently navigate, avoid obstacles, and achieve desired tasks.

At its core, a vector field is a mathematical construct that assigns a vector to each point in space. In the context of robotics, a vector field defines the direction and magnitude of movement that a robot should follow at any given location. Guided Vector Fields take this concept further by incorporating additional information to guide the robot's behavior. This information can come from various sources, such as sensor data, predefined maps, or desired trajectories.

If the above explanation was too complicated, here is a more intuitive explanation:

Imagine your robot wants to follow some arbitrary path. Guided Vector Fields are like invisible pathways that are present at every point on the field that push the robot toward the desired path.

Think of these pathways as arrows spread out across the space where the robot is moving. Each arrow points in a certain direction, showing the robot which way to move. But these pathways are smart – they can change based on what's happening around the robot.

For example, if the robot gets too close to a wall or an obstacle, these pathway arrows push the robot away so it doesn't hit anything. If the robot has a specific place to reach, these arrows help it find the best and smoothest way to get there, avoiding any obstacles in the way.

GVF Calculation

Take a look at the above image. This is an example of a GVF that was generated for the following path. The arrows are the generated vectors that "push" the robot along the path. If you try following the arrows at any starting point you will see that it takes you through the path. It can be helpful to imagine the graph as a river and the vectors as currents that take the robot through the river.

How do we actually calculate the vectors for every possible point the robot could be on?

Firstly we need to imagine the field as a coordinate plane, with each (x,y) coordinate representing a possible location for the robot in inches. Since we already know the total dimensions of the field, we know that our algorithm only needs to generate 20736 vectors since the FTC field dimensions are 12 x 12 feet or 144 x 144 inches.

Then we have to establish some rules for the path our robot must follow (for reasons which should become clear later):

It must be represented by some equation
The equation must be differentiable (we should be able to find the rate of change at every point)

For creating a path that meets these requirements, using Cubic Bezier Splines is a great choice. These splines are crafted by setting 4 control points. Then, a unique equation comes into play, crafting a seamless path that stays within those control points.

The following code shows a sample C++ implementation from Geeks for Geeks. It should be straightforward to implement this in Java.

/* Function that take input as Control Point x_coordinates and
Control Point y_coordinates and draw bezier curve */

// Link https://www.geeksforgeeks.org/cubic-bezier-curve-implementation-in-c/ 
vector<pair<int,int>> points; 
void bezierCurve(int x[] , int y[])
{
    double xu = 0.0 , yu = 0.0 , u = 0.0 ;
    int i = 0 ;
    for(u = 0.0 ; u <= 1.0 ; u += 0.0001)
    {
        xu = pow(1-u,3)*x[0]+3*u*pow(1-u,2)*x[1]+3*pow(u,2)*(1-u)*x[2]
             +pow(u,3)*x[3];
        yu = pow(1-u,3)*y[0]+3*u*pow(1-u,2)*y[1]+3*pow(u,2)*(1-u)*y[2]
            +pow(u,3)*y[3];
        // (xu, yu) is a coordinate point
        points.push_back({xu,yu}); 
    }
}

Now that we have our path, the next step is to create the vectors that the robot can follow. However, for efficiency purposes, we will not be generating the vector values for every possible coordinate. Instead, we only be calculating the vector for the coordinate point the robot is currently on.

To actually calculate the vector for some coordinate point we need to find the point on the curve that is closest to the robot's current position. We can do this by scanning through the list of coordinates that the curve contains.

Once we do that we need to find how to push the robot in the direction of the point.

To do this we use the derivative (rate of change) of the selected point and translate it by the robot's current position to create a movement vector, a list of values that tell the robot at what power it must move in the x and y directions. To convert the vector into motor powers we feed the x and y component of the vector into a field-centric driving function.

Because bezier splines are calculated using a standard formula we can calculate its derivative by hand using some calculus.

For a more in-depth explanation of the math behind GVFs please refer to the following paper:

Using the explanation above your code should look something like this:

Note that this code likely will not work on a physical robot and is just for demonstration purposes.

The following GitHub repository provides a far more sophisticated implementation of this concept:

import java.awt.geom.Point2D;
import java.util.ArrayList;
import java.util.List;

public class BezierGuidedVectorFieldRobot {
    private Point2D currentPosition;
    private double robotSpeed;
    private List<Point2D> controlPoints;

    public BezierGuidedVectorFieldRobot(Point2D initialPosition, double speed, List<Point2D> controlPoints) {
        this.currentPosition = initialPosition;
        this.robotSpeed = speed;
        this.controlPoints = controlPoints;
    }

    public void followBezierVectorField(int numSteps) {
        // rather than steps, use the loop method found in the pure pursuit sections
        // ie keep following the curve until the final point is reached or timeout is reached
        for (int step = 0; step < numSteps; step++) {
            Point2D closestPoint = findClosestPoint();
            Point2D vector = calculateBezierDerivative(controlPoints, closestPoint);
            moveRobot(vector);
            System.out.println("Step " + (step + 1) + ": Current Position: " + currentPosition);
        }
    }

    private Point2D findClosestPoint() {
        double minDistance = Double.MAX_VALUE;
        Point2D closestPoint = null;

        for (double t = 0; t <= 1.0; t += 0.01) {
            Point2D bezierPoint = calculateBezierPoint(controlPoints, t);
            double distance = currentPosition.distance(bezierPoint);

            if (distance < minDistance) {
                minDistance = distance;
                closestPoint = bezierPoint;
            }
        }

        return closestPoint;
    }

    private Point2D calculateBezierPoint(List<Point2D> points, double t) {
        int n = points.size() - 1;
        double x = 0.0;
        double y = 0.0;

        for (int i = 0; i <= n; i++) {
            double coefficient = binomialCoefficient(n, i) * Math.pow(1 - t, n - i) * Math.pow(t, i);
            x += coefficient * points.get(i).getX();
            y += coefficient * points.get(i).getY();
        }

        return new Point2D.Double(x, y);
    }

    private double binomialCoefficient(int n, int k) {
        if (k < 0 || k > n) {
            return 0;
        }

        double coeff = 1.0;
        for (int i = 0; i < k; i++) {
            coeff *= (n - i);
            coeff /= (i + 1);
        }

        return coeff;
    }

    private Point2D calculateBezierDerivative(List<Point2D> points, Point2D point) {
        double t = 0.0;
        double minDistance = Double.MAX_VALUE;
        double closestT = 0.0;

        for (double i = 0.0; i <= 1.0; i += 0.01) {
            Point2D bezierPoint = calculateBezierPoint(points, i);
            double distance = point.distance(bezierPoint);

            if (distance < minDistance) {
                minDistance = distance;
                closestT = i;
            }
        }

        t = closestT;
        double x = 0.0;
        double y = 0.0;
        int n = points.size() - 1;

        for (int i = 0; i <= n; i++) {
            double coefficient = binomialCoefficient(n, i) * Math.pow(1 - t, n - i) * Math.pow(t, i);
            if (i < n) {
                x += coefficient * (points.get(i + 1).getX() - points.get(i).getX()) * n;
                y += coefficient * (points.get(i + 1).getY() - points.get(i).getY()) * n;
            }
        }

        return new Point2D.Double(x, y);
    }

    private void moveRobot(Point2D vector) {
        double magnitude = Math.sqrt(vector.getX() * vector.getX() + vector.getY() * vector.getY());
        double normalizedX = vector.getX() / magnitude;
        double normalizedY = vector.getY() / magnitude;
        
        steerRobot(normalizedX, normalizedY); 
    }

    public static void main(String[] args) {
        Point2D initialPosition = new Point2D.Double(0, 0);
        double robotSpeed = 1.0;

        List<Point2D> controlPoints = new ArrayList<>();
        controlPoints.add(new Point2D.Double(10, 20));
        controlPoints.add(new Point2D.Double(30, 40));
        controlPoints.add(new Point2D.Double(50, 20));
        controlPoints.add(new Point2D.Double(70, 0));

        BezierGuidedVectorFieldRobot robot = new BezierGuidedVectorFieldRobot(initialPosition, robotSpeed, controlPoints);
        
        // rather than steps, use the loop method found in the pure pursuit sections
        // ie keep following the curve until the final point is reached or timeout is reached
        int numSteps = 10;
        robot.followBezierVectorField(numSteps);
    }
}

Implementation

The following repositories contain implementations of path following via GVFs. However, each code base implements the algorithm differently:

This repository makes use of cubic bezier splines (recommended)

This code visualizes the vector field rather than plugging the values into motors. To make this code useable in FTC you will need to plug the movement vector values into a field-centric driving function, which can be found in our mecanum modules.

This repository creates paths by manually entering the coefficients of the spline equation meaning it is not intuitive to use, however, you can easily adapt this code such that it automatically generates the coefficients of a cubic bezier spline.

This repository provides a GVF implementation in the form of a library, although it is written in Kotlin experienced programmers should be able to convert the code into Java with a little effort.

This repository is written in Python however it is a great tool to visualize GVFs.

Written in ROS:

Better than Pure Pursuit?

Using GVFs as opposed to pure pursuit has its advantages and disadvantages. Here's a quick pros and cons list:

Pros

GVF follows the path exactly: Whereas pure pursuit approximates the path with lookahead points, GVF attempts to follow the path in a more exact manner using derivatives. This means that your GVF path following will be more accurate and predictable when following a curve, however, they will both reach their goal points accurately.

Cons

You cannot use nondifferentiable paths: The fact that all paths must follow a curve that has a derivative for all points along it is a big reason why GVF is so accurate. However, it can take away your freedom when you want the robot to take more complex paths (ie a path that loops in on itself). Pure pursuit accepts control points rather than an equation so it can follow any path you desire. This may help teams have more efficient autonomous programs.

The tradeoffs can only be determined through testing. I would personally recommend starting off with pure pursuit and if the algorithm lacks the accuracy your robot requires in path following then switch to GVF-based path following. In an ideal world, your codebase should incorporate both methods.

PreviousAdvanced Pure Pursuit NextAutonomous Movement Libraries

Last updated 1 year ago