We are going to implement a PID controller to control the orientation of the robot. The goal is to move the robot to a desired position.
To control the robot movement, we can loop through 2 steps:
The first step can be implemented with Odometry-based localization (see odometry-based_localization.ipynb). The second step will be implemented with a simple PID controller with one input (orientation error) and one output (desired angular speed). By defining the desired linear speed (for example, as a constant value), the desired wheel speeds can be calculate (see the first function of the Notebook robot_behaviors.ipynb).
import numpy as np
First, the orientation error needs to be calculated. For convenience, the function below also calculates the distance error from the desired position (xd, yd) to the actual robot position (x, y) obtained via odometry.
Note that the desired orientation is the orientation of the straight line that connects the current and desired positions of the robot.
def get_pose_error(xd, yd, x, y, phi):
""" Returns the position and orientation errors.
Orientation error is bounded between -pi and +pi radians.
"""
# Position error:
x_err = xd - x
y_err = yd - y
dist_err = np.sqrt(x_err**2 + y_err**2)
# Orientation error
phi_d = np.arctan2(y_err,x_err)
phi_err = phi_d - phi
# Limits the error to (-pi, pi):
phi_err_correct = np.arctan2(np.sin(phi_err),np.cos(phi_err))
return dist_err, phi_err_correct
Test the function for different values of current pose and desired position.
# Actual robot pose:
x, y, phi = 0, 0, np.pi/4
# Desired robot position:
xd, yd = -1, 1
position_err, orientation_err = get_pose_error(xd, yd, x, y, phi)
print(f'Distance error = {position_err} m.')
print(f'Orientation error = {orientation_err} rad.')
Distance error = 1.4142135623730951 m. Orientation error = 1.5707963267948966 rad.
A PID controller calculates an output that is propotional to the error (P), to its integral (I), and to its derivative (D). A simple PID controller can be implemented as follows:
def pid_controller(e, e_prev, e_acc, delta_t, kp=1.0, kd=0, ki=0):
""" PID algortithm: must be executed every delta_t seconds
The error e must be calculated as: e = desired_value - actual_value
e_prev contains the error calculated in the previous step.
e_acc contains the integration (accumulation) term.
"""
P = kp*e # Proportional term; kp is the proportional gain
I = e_acc + ki*e*delta_t # Intergral term; ki is the integral gain
D = kd*(e - e_prev)/delta_t # Derivative term; kd is the derivative gain
output = P + I + D # controller output
# store values for the next iteration
e_prev = e # error value in the previous interation (to calculate the derivative term)
e_acc = I # accumulated error value (to calculate the integral term)
return output, e_prev, e_acc
# The values below are initialized to test the function.
# When implementing this, you must update e_prev and e_acc properly at every step.
e = orientation_err
e_prev = orientation_err*0.9
e_acc = 0
delta_t = 0.01
# Controller gains:
kp = 0.5
kd = 0.01
ki = 0.1
# Obtain the desired angular speed:
w_d, e_prev, e_acc = pid_controller(e, e_prev, e_acc, delta_t, kp, kd, ki)
print(f'Desired angular speed w_d = {w_d} rad/s.')
print(f'Previous error = {e_prev} rad.')
print(f'Accumulated error = {e_acc}.')
Desired angular speed w_d = 0.9440485924037328 rad/s. Previous error = 1.5707963267948966 rad. Accumulated error = 0.0015707963267948967.
After completing this notebook, you should understand how to apply a PID controller to implement a go-to-goal behavior for a mobile robot.