Question

I am trying to make a robot in a simulation move an exact distance by sending messages with a linear velocity. Right now my implementation doesn’t make the robot move an exact distance. This is some sample code:

void Robot::travel(double x, double y)
{
    // px and py are the current positions (constantly gets updated as the robot moves in the simulation)
    // x and y is the target position that I want to go to
    double startx = px;
    double starty = py;
    double distanceTravelled = 0;

    align(x, y);
    // This gets the distance from the robot's current position to the target position
    double distance = calculateDistance(px, py, x, y);

    // Message with velocity
    geometry_msgs::Twist msg;
    msg.linear.x = 1;
    msg.angular.z = 0;

    ros::Rate loop_rate(10);
    while (distanceTravelled < distance)
    {
        distanceTravelled = calculateDistance(startx, starty, px, py);
        // Publishes message telling the robot to move with the linear.x velocity
        RobotVelocity_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
}

I asked around and someone suggested that using a PID controller for a feedback loop would solve this problem, but after reading the Wikipedia page I don’t really get how I’d use it in this context. The Wikipedia page has pseudo code for the PID algorithm, but I have no idea what corresponds to what.

previous_error = setpoint - process_feedback
integral = 0
start:
  wait(dt)
  error = setpoint - process_feedback
  integral = integral + (error*dt)
  derivative = (error - previous_error)/dt
  output = (Kp*error) + (Ki*integral) + (Kd*derivative)
  previous_error = error
  goto start

How would I implement this in the context of velocities and distances? Is distance error? Or velocity? Can anyone help please? What is the integral? Derivative? Kp? Ki? Kd? e.t.c.

Thanks.

Answer 1

For your problem, the setpoint will be (x,y). process_feedback will be (px,py). and output will be the velocity at which you need to travel. Kp, Ki, and Kd are parameters that you can tune to get the kind of behavior you want. For example, if Kd is too low you could shoot past the target by not slowing down enough as you approach it.

A PID controller takes three things into consideration:

error: where you want to be vs. where you are

This is certainly a big factor. If you are at point A and your target is at point B, then the vector from A to B tells you a lot about how you need to steer, but it isn’t the only factor.

derivative: how fast you are approaching

If you are approaching the target quickly and you are close to it, you actually need to slow down. The derivative helps take that into consideration.

integral: alignment error

Your robot may not actually do exactly what you tell it to do. The integral helps determine how much you need to compensate for that.