En esta práctica se realizará el recorrido de 5 balizas que serán nuestros objetivos. Para ello se emplearán controladores PID para lograr conseguir la navegación local del cuadricóptero. En esta práctica tendremos que emplear la plataforma JdeRobot, la librería OpenCV y el lenguaje de programación Python.
Para la ejecución de la práctica necesitaremos dos terminales. En un terminal lazaremos el mundo de las balizas en Gazebo, y en el otro lanzaremos nuestro código.
1: gazebo ardrone-beacons.world
1: gazebo ardrone-beacons.world
2: ./position_control.py --Ice.Config=position_control_conf.cfg
Para poder realizar la práctica debemos conocer la posición del drone y las posiciones de las balizas, para poder obtener el error que tenemos respecto a la posición y poder corregirlo.
Para realizar la práctica he seguido los siguientes pasos:
1. El primer paso es crear una clase llamada pid, donde introduciré el código del controlador PID. Lo primero que hago es inicializar dicha clase y después me defino el método calculateU para poder calcular el controlador proporcional, el controlador derivativo y el controlador integral, y después sumarlos, proporcionando la corrección que habrá que aplicar al dar la velocidad al drone. Para la realización de dicho método debemos conocer como se calcula cada controlador. El controlador proporcional sigue la siguiente fórmula: u = -kp * error. El controlador derivativo se consigue siguiendo la fórmula: u = -kd * (error actual - error anterior). El controlador integral lo obtenemos empleando la siguiente fórmula: u = -ki * (error actual + todos los errores anteriores). La clase es la siguiente:
return u
2. Lo siguiente es instanciar dos objetos (uno por el eje y otro por el eje y) en el constructor de la clase MyAlgorithm:
self.pidX = self.pid(0.655,0.000112,0.00029)
self.pidY = self.pid(0.655,0.000112,0.00029)
3. Después en el método execute tenemos que obtener la siguiente baliza si ya hemos alcanzado la anterior. La siguiente baliza se obtiene de la siguiente forma:
actualBeacon = self.getNextBeacon()
4. Tenemos que tener en cuenta que si existe la siguiente baliza tendremos que llevar a cabo un cierto algoritmo para alcanzar dicho objetivo. Si no existe la baliza es debido a que hemos recorrido ya todas las balizas por lo que habrá que parar el drone. Para parar el drone empleamos:
self.cmdvel.sendCMDVel(0,0,0,0,0,0)
Si existe la baliza (if actualBeacon != None) tendremos que realizar los siguientes pasos:
4.1. Obtener la posición de la baliza:
posX_target = actualBeacon.getPose().x
posY_target = actualBeacon.getPose().y
4.2. Calculamos el error en el eje x y en el eje y del drone respecto a la baliza a alcanzar. Para ello tenemos que saber que la posición del drone se puede obtener con self.pose.getPose3D().x y self.pose.getPose3D().y. Calculamos el error de la siguiente forma:
errorx = posX_target - self.pose.getPose3D().x
4.3. Calculamos la corrección que proporcionaremos a cada eje y que le daremos a la velocidad de cada eje:
controladorX = self.pidX.calculateU(errorx)
controladorY = self.pidY.calculateU(errory)
4.4. Si el valor absoluto de las correcciones calculadas en el paso anterior es mayor que el error mínimo que se nos proporciona en la práctica (self.minError) definimos para los ejes x e y la velocidad (que será el controlador en cada eje con signo negativo). Se puede observar en las siguientes líneas:
if abs(controladorX) > self.minError:
self.cmdvel.setVX(-controladorX)
if abs(controladorY) > self.minError:
self.cmdvel.setVY(-controladorY)
4.5. Si los dos controladores, tanto en el eje x como en el eje y, son menores que el error mínimo, entonces tenemos que poner la baliza como marcada y el error acumulado de cada pid a cero. Si no es así, enviamos la velocidad definida para cada eje en el paso anterior a los motores. Lo vemos en las siguientes líneas de código:
if (abs(controladorX) <= self.minError) and (abs(controladorY) <= self.minError):
actualBeacon.setReached(True)
self.pidX.acumulate_error = 0
self.pidY.acumulate_error = 0
else:
self.cmdvel.sendVelocities()
El vídeo de la práctica es el siguiente:
Para realizar la práctica he seguido los siguientes pasos:
1. El primer paso es crear una clase llamada pid, donde introduciré el código del controlador PID. Lo primero que hago es inicializar dicha clase y después me defino el método calculateU para poder calcular el controlador proporcional, el controlador derivativo y el controlador integral, y después sumarlos, proporcionando la corrección que habrá que aplicar al dar la velocidad al drone. Para la realización de dicho método debemos conocer como se calcula cada controlador. El controlador proporcional sigue la siguiente fórmula: u = -kp * error. El controlador derivativo se consigue siguiendo la fórmula: u = -kd * (error actual - error anterior). El controlador integral lo obtenemos empleando la siguiente fórmula: u = -ki * (error actual + todos los errores anteriores). La clase es la siguiente:
| class pid(object): |
| def __init__(self, kp, kd, ki): |
| # Constant of PID control |
| self.kp = kp |
| self.kd = kd |
| self.ki = ki |
| self.error = 0 |
| self.acumulate_error = 0 |
| def calculateU(self, e): |
| proportional = self.kp * e |
| derivate = self.kd * (e - self.error) |
| self.acumulate_error = self.acumulate_error + e |
| integral = self.ki*(self.acumulate_error) |
| u = -(proportional) -(derivate) -(integral) |
| self.error = e |
2. Lo siguiente es instanciar dos objetos (uno por el eje y otro por el eje y) en el constructor de la clase MyAlgorithm:
self.pidX = self.pid(0.655,0.000112,0.00029)
self.pidY = self.pid(0.655,0.000112,0.00029)
3. Después en el método execute tenemos que obtener la siguiente baliza si ya hemos alcanzado la anterior. La siguiente baliza se obtiene de la siguiente forma:
actualBeacon = self.getNextBeacon()
4. Tenemos que tener en cuenta que si existe la siguiente baliza tendremos que llevar a cabo un cierto algoritmo para alcanzar dicho objetivo. Si no existe la baliza es debido a que hemos recorrido ya todas las balizas por lo que habrá que parar el drone. Para parar el drone empleamos:
self.cmdvel.sendCMDVel(0,0,0,0,0,0)
Si existe la baliza (if actualBeacon != None) tendremos que realizar los siguientes pasos:
4.1. Obtener la posición de la baliza:
posX_target = actualBeacon.getPose().x
posY_target = actualBeacon.getPose().y
4.2. Calculamos el error en el eje x y en el eje y del drone respecto a la baliza a alcanzar. Para ello tenemos que saber que la posición del drone se puede obtener con self.pose.getPose3D().x y self.pose.getPose3D().y. Calculamos el error de la siguiente forma:
errorx = posX_target - self.pose.getPose3D().x
| errory = posY_target - self.pose.getPose3D().y |
4.3. Calculamos la corrección que proporcionaremos a cada eje y que le daremos a la velocidad de cada eje:
controladorX = self.pidX.calculateU(errorx)
controladorY = self.pidY.calculateU(errory)
4.4. Si el valor absoluto de las correcciones calculadas en el paso anterior es mayor que el error mínimo que se nos proporciona en la práctica (self.minError) definimos para los ejes x e y la velocidad (que será el controlador en cada eje con signo negativo). Se puede observar en las siguientes líneas:
if abs(controladorX) > self.minError:
self.cmdvel.setVX(-controladorX)
if abs(controladorY) > self.minError:
self.cmdvel.setVY(-controladorY)
4.5. Si los dos controladores, tanto en el eje x como en el eje y, son menores que el error mínimo, entonces tenemos que poner la baliza como marcada y el error acumulado de cada pid a cero. Si no es así, enviamos la velocidad definida para cada eje en el paso anterior a los motores. Lo vemos en las siguientes líneas de código:
if (abs(controladorX) <= self.minError) and (abs(controladorY) <= self.minError):
actualBeacon.setReached(True)
self.pidX.acumulate_error = 0
self.pidY.acumulate_error = 0
else:
self.cmdvel.sendVelocities()
El vídeo de la práctica es el siguiente:
El código de la práctica se encuentra en el siguiente enlace:
No hay comentarios:
Publicar un comentario