En esta entrada veremos como podemos aplicar un control proporcional, proporcional derivativo y proporcional,derivativo e integral(PID), mediante un robot con modelo de conducción tipo vehicular es decir, bycicle model.
Te recomiendo checar este video del Laboratorio aeroespacial del MIT sobre una muy breve intro al control PID:
https://www.youtube.com/watch?v=4Y7zG48uHRo
El robot recorrerá linealmente el eje x en un mapa de entorno, a su lado una pared que es la que vamos a tratar de evitar y la que “sensaremos virtualmente” para producir nuestro control basado en el pasado, presente y futuro.
Bien recordemos la fórmula del control PID(si no la conoces te sugiero que veas algunos videos o leas algo de ello, no te desesperes es algo complicado pero el código es sencillo):
Lo cual no es complicada hasta cuando le aplicas trasformadas de Laplace para funciones de transferencia, pero no te preocupes porque no será necesario aquí.
Esta fórmula es la que ocuparemos, es la inversa(como decía, no desesperes):
Cabe aclarar que no estoy introduciendo el sensing y los métodos de localización o estimación de la posición, eso es contenido de otra entrada.
La idea del control PID se representa en el siguiente cuadro:
Todo está basado en python 3, por lo que si no lo tienes te recomiendo instalar un paquete científico llamado anaconda 3 que te brinda herramientas compatibles en versiones entre sí para programar a gusto sin los problemas de incompatibilidad, instala también la librería Visual Python, en anaconda 3 lo descargas a través del conda como conda install -c vpython vpython.
Una guía para su el uso de visual python, la librería 3D para modelado físico, formas y entornos físicos 3D:
http://www.glowscript.org/docs/VPythonDocs/index.html
Como editor de python puedes usar Brackets o sublime text, también puedes usar el Spyder IDE para propósitos científicos incluido en el paquete de conda 3, que por supuesto es gratis descargar.
Bien pues comencemos:
#3D PID
“”“
Created on Sun Jul 16 10:57:18 2017
PID control
@author: Eduardo Rasgado
”""
#importamos las librerías necesarias
#the desire trajectory for the robot is the x axis
import vpython
import time
import random
import numpy as np
#La clase robot nos va a permitir mover el robot, sensar y darle forma de objeto con las #funciones dentro de esta, esta clase emula el drive system deseado.
class robot(object):
def init(self,length=20.0):
#xcreates robot and initializes location/orientaton to 0,0,0.
self.x = 0.0
self.y = 0.0
self.orientation= 0.0
self.length = length
#Definimos los atributos de ruidos para simular la imprecisión de los sensores en
#entornos reales, esto lo usaremos con la formula de la campana de gauss para
#generar un rango de posibles posiciones del vehículo y el ángulo de sus llantas
self.steering_noise = 0.0
self.distance_noise = 0.0
self.steering_drift = 0.0
def setter(self,x,y,orientation):
#esta función nos permite definir los parámetros en x y y la orientación tetha iniciales
#o en algún punto del mapa, ya que al definir un robot se setean aleatoriamente las
#posiciones
#sets a robot coordinate
self.x = x
self.y = y
#ojo: el modulo lo ocupamos para que nuestra orientación no se salga mas alla de 2pi
self.orientation = orientation % (2.0 * np.pi)
def set_noise(self, steering_noise,distance_noise):
#seteamos los ruidos de sensores
self.steering_noise = steering_noise
self.distance_noise = distance_noise
def set_steeringdrift(self,drift):
#setea el ángulo que hará poner a prueba nuestro nuestro control
#simula un obstáculo en el camino que hará cambiar la dirección del robot por motivos
#físicos
self.steering_drift = drift
def move(self, steering, distance,tolerance=0.001,max_steering_angle=np.pi/4.0):
#steering=el ángulo de las llantas delanteras de un vehículo normal, el ángulo máximo
#lo definimos como pi/4 que es la restricción mecánica de un sistema de llantas vehicular
#distance= el avance a realizar, que depende del tiempo y la dt en nuestra función #principal de control
if steering > max_steering_angle:
steering= max_steering_angle
if steering < -max_steering_angle:
steering=-max_steering_angle
if distance < 0.0:
distance = 0.0
#make a new copy
res = robot()
res.length = self.length
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
res.steering_drift = self.steering_drift
#appying noise
steering2 = random.gauss(steering,self.steering_noise)
distance2 = random.gauss(distance,self.distance_noise)
#apply steering_drift
steering2 += self.steering_drift
#Ahora si, la cinemática de un vehículo de 4 ruedas bycicle drive system
#Execute motion
#beta = tan(alpha)x d/L
turn = np.tan(steering2) * distance2 / self.length
if abs(turn) <tolerance: #linea recta
res.x = self.x + distance2 * np.cos(self.orientation)
res.y = self.y + distance2 * np.sin(self.orientation)
res.orientation = (self.orientation + turn) % (2.0 * np.pi)
else:
#turn > 0
#approximate bycicle model for motion
#globales:
#R = d/beta
radius = distance2 / turn
#cx = x-Rsin(tetha)
cx = self.x - (radius*np.sin(self.orientation))
#cy = y+Rcos(tetha)
cy = self.y + (radius*np.cos(self.orientation))
#tetha = (tetha+beta)mod 2pi
res.orientation = (self.orientation + turn) % (2.0*np.pi)
#x = cx+Rsin(tetha+beta)
res.x = cx + (np.sin(res.orientation)*radius)
#y = cy - Rcos(tetha+beta)
res.y = cy - (np.cos(res.orientation)*radius)
#La llanta tiene su propio stering__angle o ángulo de giro y también
#existe un ángulo tetha ángulo del vehículo en el mundo, la fórmula
#res.orientation = (self.orientation + turn) % (2.0*np.pi) nos da la orientación global
#como en la imagen nuestras llantas ruedan pero hacia x con un yaw motion, tomando
#en cuenta los ángulos de euler pitch,yaw,roll.
return res,steering2
def __repr__(self):
return '[x=%s, y=%s, orient=%s]'%(self.x,self.y,self.orientation)
def run_proportional(param):
#control proporcional!
myrobot = robot()
#comienza situado a eje x=0 eje y=5 unidades alejado de la linea target
myrobot.setter(0.0,5.0,0.0)
speed = 1.0 #motion distance equalt to speed (we assume time = 1)
N=500
tau =-param
xs = []
ys = []
orient= []
for i in range(N):
#comienza el movimiento
xs.append(myrobot.x)
ys.append(myrobot.y)
#la distancia sensada desde la posición del vehículo hasta la linea target es
#el cross-track error que requerimos=myrobot.y
#tau es la constante proporcional llamada ganancia proporcional en teoría
a = myrobot.ytau
steer = a
#el robot se mueve de acuerdo a los datos del presente
myrobot,errr = myrobot.move(steer,speed)
orient.append(myrobot.orientation)
if i==150:
myrobot.set_steeringdrift(40.0/180.0np.pi) #volantazo
if i ==400:
myrobot.set_steeringdrift(-20.0/180.0*np.pi) #volantazo
#print(myrobot,steer)
return xs,ys,orient
def run_proportional_derivative(param1,param2):
myrobot2 = robot()
myrobot2.setter(0.0,5.0,0.0)
#myrobot2.set_steeringdrift(10.0/180.0np.pi) #10 degrees
speed = 1.0 #motion distance equalt to speed (we assume time = 1)
N=500
dt =1.0
tau_p =param1
tau_d =param2
xs2 = []
ys2 = []
orient = []
last_error = myrobot2.y
for i in range(N):
xs2.append(myrobot2.x)
ys2.append(myrobot2.y)
diff_cte = myrobot2.y - last_error
steering_angle = -tau_p * myrobot2.y - tau_d * (diff_cte/dt)
last_error = myrobot2.y
#el auto se mueve de acuerdo a los datos del presente y futuro(parte derivativa)
#pero para esto no hacemos una predicción directa mas bien obtenemos la diferencia
#entre el crosstrack error actual y el anterior y lo dividimos entre la derivada del tiempo #dt y ello multiplicado por la ganancia derivativa
myrobot2,errr = myrobot2.move(steering_angle,speed)
orient.append(myrobot2.orientation)
if i==150:
myrobot2.set_steeringdrift(40.0/180.0np.pi) #volantazo
if i ==400:
myrobot2.set_steeringdrift(-20.0/180.0*np.pi) #volantazo
#print(myrobot2,steering_angle)
return xs2,ys2,orient
def run_proportional_integrative_derivative(param1,param2,param3):
myrobot = robot()
myrobot.setter(0.0,5.0,0.0)
#myrobot.set_steeringdrift(10.0/180.0np.pi) #10 degrees
speed = 1.0 #motion distance equalt to speed (we assume time = 1)
N=500
dt =1.0
tau_p =param1
tau_d =param2
tau_i = param3
xs = []
ys = []
orient = []
last_error = myrobot.y
integrated_error =0.0
for i in range(N):
xs.append(myrobot.x)
ys.append(myrobot.y)
#un control robusto basado en el agregado de una ganancia integral multiplicado por
#la acumulación de todos los errores pasados mas el actual, con ello
#agregamos el pasado para nuestro control completo
diff_cte = myrobot.y - last_error
integrated_error +=myrobot.y
last_error = myrobot.y
steering_angle = -tau_p * myrobot.y - tau_d * (diff_cte/dt) -tau_i * integrated_error
if i==150:
myrobot.set_steeringdrift(40.0/180.0np.pi) #volantazo
if i ==400:
myrobot.set_steeringdrift(-20.0/180.0*np.pi) #volantazo
#robot se mueve
myrobot,errr = myrobot.move(steering_angle,speed)
orient.append(myrobot.orientation)
#print(myrobot,steering_angle)
return xs,ys,orient
def make_robot():
#reset the robot back
#usada por la función twiddle para testar un robot una y otra vez desde el comienzo del road
robot0 = robot()
robot0.setter(0,5.0,0)
robot0.set_steeringdrift(40.0/180.0*np.pi)
return robot0
def run_test(roboto, params,n=100,speed=1.0):
#esta función es para usarla en el ajuste de los parámetros optimizados para nuestro
#control PID en el algoritmo twiddle
x_trajectory = []
y_trajectory = []
err = 0
prev_cte = roboto.y
int_cte = 0.0
for i in range(2*n):
cte = roboto.y
diff_cte = (cte-prev_cte)/speed
int_cte+= cte
prev_cte = cte
steer = -params[0]*cte -params[1]*diff_cte - params[2]*int_cte
roboto,errr = roboto.move(steer,speed)
x_trajectory.append(roboto.x)
y_trajectory.append(roboto.y)
if i >=n:
#suma de los errores
err += cte**2
return x_trajectory, y_trajectory, err/n #promedio de errores
def twiddle(tol=0.2):
#algoritmo twiddle para optimizar los ganancias P, D e I
#p contiene ganancias “buenas” que serán optimizadas, hasta reducir lo máximo
#el error en el recorrido y la compensación de la posición
#devuelve p,d,i
p = [2.0,6.0,0.004]
dp = [1,1,1]
robot11 = make_robot()
x_trajectory,y_trajectory,best_err = run_test(robot11,p)
it = 0
while sum(dp)>tol:
print(“Iteration {}, best error = {}”.format(it,best_err))
for i in range(len§):
p[i] +=dp[i]
robot11 = make_robot()
x_trajectory,y_trajectory, err = run_test(robot11,p)
if err<best_err:
best_err = err
dp[i]*=1.1
else:
p[i] -= 2.0 * dp[i]
robot11 = make_robot()
x_trajectory,y_trajectory,err = run_test(robot11,p)
if err<best_err:
best_err = err
dp[i] += 1.1
else:
p[i]+=dp[i]
dp[i]*=0.9
it+=1
return p
def representer(xs,ys,name,clock,orienter):
#acá hice algo raro con los ejes puesto que el eje en el que se desplaza linealmente el
#robot es x, pero el eje del roll lo cambie a pitch, o eje y, por motivos de representación
#que no afecta al modelo, es decir el plano de movimientos del vehículo será y.
#función que representará en 3D con la librería vpython, el simulador.
escena = vpython.canvas(width=1200,height=600)
escena.title = name
escena.background = vpython.color.cyan
pos = vpython.vector(xs[0],2,-ys[0])
escena.range = 100
escena.center = vpython.vector(0,0,0)
escena.forward=vpython.vector(1,-2,1)
suelo = vpython.box(pos = vpython.vector(250,-1,0),size=vpython.vector(500,0.01,200),color=vpython.color.white,texture=vpython.textures.wood)
vehicle = vpython.box(pos=pos,color=vpython.color.red,size=vpython.vector(8,4,6))
trayectoria = vpython.curve(color=vpython.color.yellow)
road = vpython.curve(color=vpython.color.black)
muro = vpython.box(pos=vpython.vector(0,20,10),color=vpython.color.blue,size=vpython.vector(20,40.99,2),texture=vpython.textures.stucco)
piedra1 = vpython.sphere(pos=vpython.vector(150,-1,-ys[150]),color=vpython.color.black,radius=1)
piedra2 = vpython.sphere(pos=vpython.vector(xs[400],-1,-ys[400]),color=vpython.color.black,radius=1)
time.sleep(clock)
escena.range = 45
for i in range(len(xs)):
if i>0:
vehicle.rotate(axis=vpython.vector(0,1,0),angle=-orienter[i-1])
pos = vpython.vector(xs[i],2,-ys[i])
#suelo.pos=vpython.vector(i/2.0,-1,0)
#suelo.size = vpython.vector(i,0.01,200)
muro.pos =vpython.vector(i/2,20,10)
muro.size= vpython.vector(i,40.99,2)
vehicle.pos = pos
escena.center = vpython.vector(xs[i],5,-ys[i])
trayectoria.append(pos)
vehicle.rotate(axis=vpython.vector(0,-1,0),angle=-orienter[i])
road.append(vpython.vector(i,2,0))
vpython.rate(40)
suelo = vpython.box(pos=vpython.vector(250,-1,0),size=vpython.vector(500,0.01,200),color=vpython.color.white)
#así funciona el mundo de la funcion representer():
#esto no afecta(si entiedes sabras que solo cambié los ejes base), solo se me paso, pero ustedes pueden cambiar esos, ejes, como dije no #afecta
#el main del programa, presionar enter dentro de la terminal cuando acabe cada simulación
#esto se sabe porque en pantalla de terminal aparece "siguiente"
xs,ys,orient1 = run_proportional(0.1) #try with 0.3
name = “Proportional control”
#clock:Podemos ajustarlo para dar o quitar tiempo de espera hasta la simulación
#la uso para darme tiempo de poner la pantalla del host(las simulaciones se muestran usando
#el buscador y abriendo un host que automáticamente se hace al clicar el archivo py )
clock1=4
representer(xs,ys,name,clock1,orient1)
input(“siguiente”)
#Control proporcional basado en el crosstrack error actual, presenta un
#movimiento tipo bang bang, no tiene la robustez necesaria para reacionar a los cambios
#imprevistos
#Menor error estacionario pero mayores oscilaciones
xs2,ys2,orient2= run_proportional_derivative(0.2,3.0) #try with 0.3
name2 ="Control proporcional derivativo"
clock2=8
representer(xs2,ys2,name2,clock2,orient2)
input(“siguiente”)
#Control PD, se evitan las prolongadas variaciones pero no corrige el offset y nunca llega a
#la zona deseada
#Reducimos oscilaciones y pendientes
xs3,ys3,orient3= run_proportional_integrative_derivative(0.2,3.0,0.004) #try with 0.3
name3 = "Control PID"
representer(xs3,ys3,name3,clock2,orient3)
input(“siguiente”)
#Control PID:
#Se minimiza el error, offset
#Se minimiza el tamaño de las oscilaciones al tiempo que
#se maximiza el tiempo de respuesta
#Kp=minimiza el error
#Ki=compensa el cambio brusco por la piedra
#Kd=evita que se pase de mas el ajuste del control(overshoots),cuando
#la variable del proceso cambia muy rápido actúa como un freno para prevenir el #overshooting.
#ajustando parametros de kp, kd, ki
params = twiddle()
kp,kd,ki = params
#robot1 = make_robot() #usar como opcion sin obstaculos
#Lo que veras en consola cuando se estén optimizando los valores para el PID
print(kp,kd,ki)
#testing de las constantes obtenidas
#x_trajectory,y_trajectory,err = run_test(robot1,params) #usar como opcion sin obstaculos
xs4,ys4,orient4= run_proportional_integrative_derivative(kp,kd,ki)
name4 = "Control PID con constantes optimas"
representer(xs4,ys4,name4,clock2,orient4)
input(“Fin”)
#El resultado final de la optimización es un robot que responde de forma eficiente a
#los cambios bruscos de su posición.
“”“
Las pruebas se pueden hacer jugando con el twiddle, con los puntos de inicio de cada robot y metiendo + o - piedras
recordar al variar los valores de twiddle cambiar los de make_robot
”""
Mira detrás del vehículo como varían las trayectorias, la linea negra es la trayectoria deseada,
la fija en eje x, y la linea amarilla es la trayectoria del vehículo.
No olvides identar bien algunos renglones que podrían desidentarse por el formato de blogger.
Credits: Curso Artificial Intelligence For Robotics, Sebastian Thrun
Papers de Sebastian Thrun, Universidad de Stanford
Control de movimiento de robots manipuladores, Rafael Kelly.
Mucho trabajo en casa pegado al pc
Si te interesa subiré la simulación de una inteligencia artificial con el uso de grafos que resuelve laberintos en 3D usando el algoritmo A*.
Chao!
