Skip to the content.

Lab 5 – Trajectory Tracking Controller

Objective

The goal of this lab is to implement a controller that enables the robot to follow a trajectory. The same controller can be used as a “go-to-position” controller.

screenshot_Webots

Figure 1. Webots screenshot - at the console (bottom) you see reference values of linear speed u_ref and angular speed w_ref generated by the trajectory tracking controller.

Pre-requisites

Tasks

Your main task is to write code to implement the functions below to add the controller to your line-following behavior from Lab 3. In the main loop of your program, those functions should be called in a sequence:

# Trajectory tracking controller
[u_ref, w_ref] = traj_tracking_controller(dxd, dyd, xd, yd, x, y, phi, a)

# Convert reference speeds to wheel speed commands
[leftSpeed, rightSpeed] = wheel_speed_commands(u_ref, w_ref, D, R)

The variable a contains the distance [m] between the point whose position is being controlled and the center point between the wheels. The value of a can be arbitrary. The smaller the value of a, the closer to the center the controlled point is. But, a cannot be equal to zero. Very small values can cause accuracy problems due to rounding errors. Recommended values are ‘a = 0.05’ or ‘a = 0.1’. The other variables used in the functions above were defined in Lab 3.

Explanation about the trajectory tracking controller can be found in section 4 of [1]. The controller equation to be implemented here is equation (20). This controller was first presented in [2], where more details about its stability are given. See references at the bottom of this page.

The tasks are listed below:

  1. Write the function traj_tracking_controller(dxd, dyd, xd, yd, x, y, phi, a) to calculate reference values of linear and angular velocities. Those values should later be used to calculate the reference speeds of each wheel. Test your code before moving to the next step.
  2. Write the function wheel_speed_commands(u_ref, w_ref, D, R) to calculate the reference speeds for the left and right wheels. Those are the values that need to be sent to the wheel controllers for the robot to follow the desired trajectory.
  3. Test the trajectory tracking controller using the desired values below. You might need to adjust the controller gains.
    xd = 0.0	# desired position x [m]
    yd = 0.0	# desired position y [m]
    dxd = 0.0	# desired speed in x [m/s]
    dyd = 0.0	# desired speed in y [m/s]
    
  4. Test different values of desired positions xd, yd and check if the robot behaves as expected. If you keep the desired speeds as zero, changing the values of xd and yd should make the controller behave as a go-to-position controller.

I recommend you try to modify your code from Lab 3 to implement the trajectory-tracking controller. Try doing it yourself, first. If your code is not working, or you need inspiration, you can use the provided template.

Other details

Position (x=0, y=0) is in the center of the track. To facilitate the comparison with the speeds calculated by Webots, print the values calculated by your functions, as shown in Figure 2.

Robot pose in Webots

Figure 2. Webots screenshot showing robot pose calculated by the simulator (left) and by the Python code (bottom).

Think about the following questions

Solution

Try to implement the code yourself before checking the solution! After a successfull implementation, or if you need more inspiration than the template, an example code is available here.

Challenge: Generate a Trajectory

Create the functions to generate a trajectory of your choice inside the field. The functions must be properly defined to generate desired positions (xd, yd) and speeds with their corresponding time derivates (dxd, dyd).

You are free to choose the generated trajectory, as long as:

Adjust the controller gains to make your robot follow the trajectory with as little error as possible.

In your demonstration, you have to show:

Conclusion

After following this lab you should know how to implement moving controllers for mobile robots, especially to go to position and follow a trajectory.

References

[1] Martins, Felipe Nascimento, and Alexandre Santos Brandão. “Motion control and velocity-based dynamic compensation for mobile robots.” Applications of mobile robots (2018).

[2] Martins, F. N., Celeste, W. C., Carelli, R., Sarcinelli-Filho, M., & Bastos-Filho, T. F. (2008). An adaptive dynamic controller for autonomous mobile robot trajectory tracking. Control Engineering Practice, 16(11), 1354-1363.

Next Lab

Go to Lab 6 - Combine Behaviors to Execute a Complex Mission

Back to main page.