/// Incluye los funciones comunes de Ros y de STD (math.h,Services.h,..) #include "ros/ros.h" /// Tipo de mensaje Servicio para la aplicacion de fuerzas (Gazebo) #include "gazebo_msgs/ApplyBodyWrench.h" /// Tipo de mensaje de Odometria (common_msg) #include "nav_msgs/Odometry.h" /// Libreria de bullet necesaria para transformar de Quaternion a RPY #include "LinearMath/btQuaternion.h" #include "LinearMath/btMatrix3x3.h" /// Tipo de mensaje de Posicion + RPY (modelquad) #include "modelquad/Position_RPY.h" /// Tipo de mensaje Servicio recepcion posiciones deseadas #include "modelquad/EnvioPosDeseada.h" /// Tipo de mensaje de 4 datos para el giro de los motores (modelquad) #include "modelquad/Giros_Motores.h" /// Coeficientes del PID discretizado por la aproximacion de Tustin #include "modelquad/Controlador_Tustin.h" /// Propiedades dinamicas del quadrotor #include "modelquad/Propiedades_Quad.h" // #define _USE_MATH_DEFINES //(PI) NO HACE FALTA YA ESTA EN ros.h // #include "math.h" /// Objetos de los tipo de mensaje de posicion actual y deseadas /// Se declaran como variables globales para que sean accesibles desde /// todas las funciones modelquad::Position_RPY pos_actual; modelquad::Position_RPY pos_deseada; /// Flags para la espera inicial de una posicion deseada (y actual) int actual_ok=0; int deseada_ok=0; /// Flag para limites dinamicos. A 1 si llego una nueva pos_deseada int cambio_deseada=0; /// Limites dinamicos, se inicializan a 0.10 por ejemplo float limiteX=0.10; float limiteY=0.10; /// Funcion lecturas topic posicion actual (sensor odometria) /// Odom es un objeto del tipo Odometry en el que se guarda el mensaje /// recibido por el topic /Odometria void Calculo_posicion_actual(const nav_msgs::Odometry odom) { //~ ROS_INFO("Entra actual"); /// Variable para saber si esta girando en yaw a la izquierda(yaw>0) /// o a la derecha(yaw<0). Se declara static para que conserve su /// valor de una llamada a otra, sin volver a leerse esta linea static int a=0; /// POSICION (Respecto al sistema de referencia del plano (fijo)) float pos_X=odom.pose.pose.position.x; float pos_Y=odom.pose.pose.position.y; float pos_z=odom.pose.pose.position.z; /// ORIENTACION (Respecto al sistema de referencia del QUAD (MOVIL)) /// quaternion float qx=odom.pose.pose.orientation.x; float qy=odom.pose.pose.orientation.y; float qz=odom.pose.pose.orientation.z; float qw=odom.pose.pose.orientation.w; /// Conversion quaternion a angulos de Euler (RPY). btQuaternion q(qx,qy,qz,qw); double roll, pitch, yaw; /// No vale float para getRPY() btMatrix3x3(q).getRPY(roll, pitch, yaw); /// Escalado del yaw tal que sea positivo de [0 a 2pi] y negativo /// de ]0 a -2pi]. El yaw leido por el sensor del quadrotor es /// positivo de [0 a pi] y negativo de ]pi a 2pi], lo cual provoca /// el cambio de signo en pi, dificultando el control if((yaw>M_PI/2)&&(yaw-M_PI)&&(a==0)) ///girando a la derecha { a=2; } if ((yaw<0)&&(a==1)) ///girando a la izq +pi (convierto angulo en +) { yaw=yaw+2*M_PI; } if ((yaw>0)&&(a==2)) ///girando a la der -pi (convierto angulo en -) { yaw=yaw-2*M_PI; } if((yaw-M_PI/4))///Reset (ojo: del yaw modificado) { a=0; } /// Relleno del mensaje que se enviara por /Pos_rpy_actual /// Ademas al ser variable global son las que se utilizan como /// posiciones actuales en main() pos_actual.posicion.x=pos_X; pos_actual.posicion.y=pos_Y; pos_actual.posicion.z=pos_z; pos_actual.orientacion.x=roll; pos_actual.orientacion.y=pitch; pos_actual.orientacion.z=yaw; ROS_INFO("\nPosicion (x,y,z):\t (%f,%f,%f)\nOrientation (r,p,y):\t (%f,%f,%f)", pos_X,pos_Y,pos_z,roll, pitch, yaw); //~ ROS_INFO("a: %d", a); /// Flag a 1 actual_ok=1; } /// Función servicio recepcion de posiciones deseadas bool Calculo_posicion_deseada(modelquad::EnvioPosDeseada::Request &req, modelquad::EnvioPosDeseada::Response &res) { //~ ROS_INFO("Entra servicio pos_deseada"); /// Se almacena en la variable global pos_deseada para ser accesible /// desde main() pos_deseada.posicion.x=req.pos_des.posicion.x; pos_deseada.posicion.y=req.pos_des.posicion.y; pos_deseada.posicion.z=req.pos_des.posicion.z; pos_deseada.orientacion.x=req.pos_des.orientacion.x; pos_deseada.orientacion.y=req.pos_des.orientacion.y; pos_deseada.orientacion.z=req.pos_des.orientacion.z; ROS_INFO("Posicion deseada (x,y,z)= (%f,%f,%f)", pos_deseada.posicion.x, pos_deseada.posicion.y, pos_deseada.posicion.z); /// Flag a 1 deseada_ok=1; /// Flag a 1 cambio_deseada=1; /// Rspuesta servicio res.confirmacion=true; return true; } int main(int argc, char **argv) { ROS_INFO("Valor de pi utilizado: %f",M_PI); /// VARIABLES LOCALES: /// posicion actual y anteriores float Xk_T=0; float Xk_T_d=0; float Xk_Q_d=0; float Yk_T=0; float Yk_T_d=0; float Yk_Q_d=0; float Xk_T_g=0; float Yk_T_g=0; float Xk_1=0; float Xk_2=0; float Yk_1=0; float Yk_2=0; float zk=0; float zk_1=0; float zk_2=0; float rk=0; float rk_1=0; float rk_2=0; float pk=0; float pk_1=0; float pk_2=0; float yk=0; float yk_1=0; float yk_2=0; /// Terminos integrales y derivativos PIDs float derivadaX=0; float derivadaY=0; float integralX=0; float integralY=0; float integralz=0; float integralr=0; float integralp=0; float integraly=0; /// Accion de control en aceleracion float uXk, uYk, uzk, urk, upk, uyk; /// Error actual float eXk, eYk, ezk, erk, epk, eyk; /// Errores en z,r,p,y anteriores float eXk_1=0; float eXk_2=0; float eYk_1=0; float eYk_2=0; float ezk_1=0; float ezk_2=0; float erk_1=0; float erk_2=0; float epk_1=0; float epk_2=0; float eyk_1=0; float eyk_2=0; /// Accion de control paso anterior float uXk_1=0; float uYk_1=0; float uzk_1=0; float urk_1=0; float upk_1=0; float uyk_1=0; /// Fuerzas y momentos incrementales (los que se envian a Gazebo) float U1, U2, U3, U4; /// Fuerzas y momentos anteriores (el que se esta aplicando) float U1ant=0; float U2ant=0; float U3ant=0; float U4ant=0; /// Fuerzas y momentos actuales (Total a aplicar) float U1act=0; float U2act=0; float U3act=0; float U4act=0; /// Coeficientes parametros aerodinamicos float A = 1.0/4/b; float B = 1.0/2/b/l; float C = 1.0/4/d; /// CREACION NODO CON SU NOMBRE Y ETIQUETA: ros::init(argc, argv, "control"); ros::NodeHandle n; /// Espera a que empieze a emitir /clock ros::Time::waitForValid(); /// SERVICIO APLICACION DE FUERZAS: /// Objetos de la clase de gazebo::ApplyBodyWrench para rellenar y /// enviar por servicio de aplicacion de fuerzas (Gazebo) gazebo_msgs::ApplyBodyWrench srv1; /// Rellenamos las opciones que no variaran (Todas a excepcion de la Z /// y el instante de aplicacion de aplicacion) srv1.request.body_name = "quad::Base"; srv1.request.reference_frame= "quad::Base"; srv1.request.reference_point.x=0; srv1.request.reference_point.y=0; srv1.request.reference_point.z=0; srv1.request.wrench.force.x=0; srv1.request.wrench.force.y=0; srv1.request.wrench.force.z=0; srv1.request.wrench.torque.x=0; srv1.request.wrench.torque.y=0; srv1.request.wrench.torque.z=0; srv1.request.duration = ros::Duration(1200); ///Nota: La idea es que la fuerza se apliquen durante 20 minutos, ///siendo todas a excepcion de la primera incrementos. Realmente para ///simular la bateria habria que ir disminuyendo esa duracion confor- ///me pasa el tiempo de simulacion, que de momento no se esta haciendo /// Objeto a rellenar con el error modelquad::Position_RPY error; ///Objeto a rellenar con los giro de los motores modelquad::Giros_Motores giros; /// TOPICS: /// Topic por el que se reciben los mensajes del sensor de odometria ros::Subscriber Sub_pos_actual = n.subscribe("/Odometria", 1, Calculo_posicion_actual); /// Topic por el que se enviaran la posicion y RPY actuales ros::Publisher Pub_pos_actual = n.advertise("Pos_RPY_actual", 100); /// Topic por el que se enviaran la posicion y RPY actuales ros::Publisher Pub_giros_motores = n.advertise("Giros_Motores", 100); /// SERVICIOS: /// Creacion servicio de recepcion de la posicion deseada ros::ServiceServer service = n.advertiseService ("envio_pos_deseada", Calculo_posicion_deseada); /// Cliente al servicio de aplicacion de fuerzas (Gazebo) ros::ServiceClient client = n.serviceClient("/gazebo/apply_body_wrench"); /// Se duerme durante 5 segundos para que todo este en orden en Gazebo ros::Duration(5,0).sleep(); /// Espera activa hasta primera lectura de posiciones actual y deseada /// para no empezar el control y permanecer en la posicion inicial /// mientras que el controlador estaria integrando errores incorrectos while((actual_ok==0)||(deseada_ok==0)) { /// Lecturas posicion actual ros::spinOnce(); /// Se duerme 10 microseg para no sobrecargar la CPU ros::Duration(0,10000).sleep(); } /// Condiciones iniciales -- soluciona los problemas en el desègue zk_1=pos_actual.posicion.z; zk_2=pos_actual.posicion.z; /// Frecuencia del ciclo (while) 10 Hz, necesario incluir loop.sleep() ros::Rate loop(10); /// BUCLE PRINCIPAL while(ros::ok()) { ///Llamada a todos los mensajes que esten en cola ros::spinOnce(); /// Publicacion en topic de la posicion actual Pub_pos_actual.publish(pos_actual); /// Posicion actual del quadrotor respecto al /// SR de tierra Xk_T = pos_actual.posicion.x; Yk_T = pos_actual.posicion.y; zk = pos_actual.posicion.z; /// SR del Quad rk = pos_actual.orientacion.x; pk = pos_actual.orientacion.y; yk = pos_actual.orientacion.z; /// Posicion deseada del quadrotor respecto al SR de Tierra Xk_T_d = pos_deseada.posicion.x; Yk_T_d = pos_deseada.posicion.y; /// Transformacion de las posiciones en x e y deseadas al SR Quad /// (Cambio SR de tierra al del quadrotor) Xk_Q_d = Xk_T_d * cos(yk) + Yk_T_d * sin(yk) - Yk_T * sin(yk) - Xk_T * cos(yk); Yk_Q_d = -Xk_T_d * sin(yk) + Yk_T_d * cos(yk) - Yk_T * cos(yk) + Xk_T * sin(yk); /// Posicion actual del quadrotor respecto al SR de tierra pero /// girado tal que su x e y son paralelos al del quadrotor /// (necesario para hacer la derivada de la referencia en el PID) Xk_T_g = Xk_T * cos(yk) + Yk_T * sin(yk); Yk_T_g = -Xk_T * sin(yk) + Yk_T * cos(yk); //ROS_INFO("X_Q_d: %f, Y_Q_d: %f, X_T_g: %f, Y_T_g: %f", Xk_Q_d, Yk_Q_d, Xk_T_g, Yk_T_g);//COMPROBADO !! /// Limites dinamicos: if(cambio_deseada==1) { ///Limites en Roll y pitch deseados proporicionales al error. ///Siendo esta constante de proporcionalidad el limite cuando ///el error es de un metro debido a que se hizo asi la ///sintonizacion limiteX=0.1*std::abs(Xk_Q_d); limiteY=0.1*std::abs(Yk_Q_d); ///Limites minimos: Casos en que apenas varie x o y ///Nunca deben ser cero pues no se contrarrestarian perturbac. if((limiteX<0.001)&&(limiteX>0)){limiteX=0.001;} if((limiteY<0.001)&&(limiteY>0)){limiteY=0.001;} ///Limites maximos: Casos en que se desee un incremento en X e Y ///nayor de 2 m. Un incremento deseado de 2m nos lleva a un ///limite de 0.2 rad(unos 11.5 grados). Graficamente se mide una ///velocidad de 1.108 m/s if(limiteX>0.2){limiteX=0.2;} if(limiteY>0.2){limiteY=0.2;} ROS_INFO("limiteX: %f limiteY: %f", limiteX, limiteY); cambio_deseada=0; } /// CONTROL EN X e Y <> //~ // Posiciones en SR del quad son directamnte el error cometido //~ error.posicion.x= Xk_Q_d; //~ error.posicion.y= Yk_Q_d; //~ eXk=error.posicion.x; //~ eYk=error.posicion.y; //~ //~ integralX=(eXk+eXk_1); //~ integralY=(eYk+eYk_1); //~ //~ derivadaX=-(Xk_T_g - 2*Xk_1 + Xk_2); //~ derivadaY=-(Yk_T_g - 2*Yk_1 + Yk_2); //~ //~ ///PIDs X e Y --> Roll y Pitch deseados //~ uXk = uXk_1 + kp_X*(eXk-eXk_1) + (T/(2*Ti_X))*integralX + (Td_X/T)*derivadaX; //~ uYk = uYk_1 + kp_Y*(eYk-eYk_1) + (T/(2*Ti_Y))*integralY + (Td_Y/T)*derivadaY; //~ //~ /// Limites en roll y pitch //~ if(uXk>limiteX){uXk=limiteX;} //~ if(uXk<-limiteX){uXk=-limiteX;} //~ if(uYk>limiteY){uYk=limiteY;} //~ if(uYk<-limiteY){uYk=-limiteY;} //~ //~ /// Ref del roll (Y --> -Roll) //~ pos_deseada.orientacion.x = - uYk; //~ /// Ref del pith (X --> Pitch) //~ pos_deseada.orientacion.y = uXk; //~ //~ ROS_INFO("X: Proporcional: %f, Integral= %f, Derivada= %f", kp_X*(eXk-eXk_1), (T/(2*Ti_X))*integralX, (Td_X/T)*derivadaX); //~ ROS_INFO("Y: Proporcional: %f, Integral= %f, Derivada= %f", kp_Y*(eYk-eYk_1), (T/(2*Ti_Y))*integralY, (Td_Y/T)*derivadaY); //~ //~ ROS_INFO("Roll deseado: %f, Pitch deseado: %f", pos_deseada.orientacion.x, pos_deseada.orientacion.y); /// FIN CONTROL X Y //~ ROS_INFO("Posicion deseada (x,y,z)= (%f,%f,%f) Orientacion deseada (r,p,y)= (%f,%f,%f)", pos_deseada.posicion.x, pos_deseada.posicion.y, pos_deseada.posicion.z, pos_deseada.orientacion.x, pos_deseada.orientacion.y, pos_deseada.orientacion.z ); /// Calculo del error error.posicion.z= pos_deseada.posicion.z - pos_actual.posicion.z; error.orientacion.x = pos_deseada.orientacion.x - pos_actual.orientacion.x; error.orientacion.y = pos_deseada.orientacion.y - pos_actual.orientacion.y; error.orientacion.z = pos_deseada.orientacion.z - pos_actual.orientacion.z; ///Nota: Se rellena primero como mensaje Pos_RPY por si es ///necesario mas tarde publicarlo //~ ROS_INFO("\n Error (x,y,z,r,p,y):\t (%f,%f,%f,%f,%f,%f)", error.posicion.x, error.posicion.y, error.posicion.z,error.orientacion.x,error.orientacion.y,error.orientacion.z); /// Solo para facilitar escritura ezk=error.posicion.z; //z erk=error.orientacion.x; //r epk=error.orientacion.y; //p eyk=error.orientacion.z; //y //~ ROS_INFO("X: u-1:%f, e: %f, e-1: %f, e-2: %f", uXk_1, eXk, eXk_1, eXk_2); //~ ROS_INFO("Y: u-1:%f, e: %f, e-1: %f, e-2: %f", uYk_1, eYk, eYk_1, eYk_2); //~ ROS_INFO("Z: u-1:%f, e: %f, e-1: %f, e-2: %f", uzk_1, ezk, ezk_1, ezk_2); //~ ROS_INFO("R: u-1:%f, e: %f, e-1: %f, e-2: %f", urk_1, erk, erk_1, erk_2); //~ ROS_INFO("P: u-1:%f, e: %f, e-1: %f, e-2: %f", upk_1, epk, epk_1, epk_2); //~ ROS_INFO("Y: u-1:%f, e: %f, e-1: %f, e-2: %f", uyk_1, eyk, eyk_1, eyk_2); integralz=(ezk+ezk_1); integralr=(erk+erk_1); integralp=(epk+epk_1); integraly=(eyk+eyk_1); /// PIDs Z, roll, pitch y yaw uzk = uzk_1 + kp_z*(ezk-ezk_1) + (T/(2*Ti_z))*integralz - (Td_z/T)*(zk - 2*zk_1 + zk_2); urk = urk_1 + kp_r*(erk-erk_1) + (T/(2*Ti_r))*integralr - (Td_r/T)*(rk - 2*rk_1 + rk_2); upk = upk_1 + kp_p*(epk-epk_1) + (T/(2*Ti_p))*integralp - (Td_p/T)*(pk - 2*pk_1 + pk_2); uyk = uyk_1 + kp_y*(eyk-eyk_1) + (T/(2*Ti_y))*integraly - (Td_y/T)*(yk - 2*yk_1 + yk_2); //~ ROS_INFO("Z: Proporcional: %f, Integral= %f, Derivada= %f", kp_z*(ezk-ezk_1), (T/(2*Ti_z))*integralz, (Td_z/T)*(zk - 2*zk_1 + zk_2)); //~ ROS_INFO("Aceleraciones: (z,r,p,y): (%3.10f , %f , %f , %f)",uzk,urk,upk,uyk); /// Modelo Inverso (aceleraciones-->Fuerzas y momentos) /// Fuerza[N] U1act = m*(uzk + g)/(cos(rk)*cos(pk)); /// Momentos[N m] U2act = urk*Ixx; U3act = upk*Iyy; U4act = uyk*Izz; ///Nota: Serian las fuerzas actuales a aplicar, pero a Gazebo se le ///envian incrementales segun la forma escogida ROS_INFO("Uact: %f,%f,%f,%f",U1act,U2act,U3act,U4act); /// Fuerzas Incrementales respecto a la anterior U1=U1act-U1ant; U2=U2act-U2ant; U3=U3act-U3ant; U4=U4act-U4ant; //~ ROS_INFO("Uant: %f,%f,%f,%f",U1ant,U2ant,U3ant,U4ant); //~ ROS_INFO("U: %f,%f,%f,%f",U1,U2,U3,U4); /// Relleno servicio: Fuerzas y momentos a enviar srv1.request.wrench.force.z=U1; srv1.request.wrench.torque.x=U2; srv1.request.wrench.torque.y=U3; srv1.request.wrench.torque.z=U4; /// Instante de aplicacion de las fuerzas ros::Time ahora = ros::Time::now(); srv1.request.start_time = ahora; ///Nota: se deja asi por si se quiere insertar un retraso, pues ///podria resultar inestable la aplicacion en un instante anterior ///al actual de simulacion /// LLAMADA AL SERVICIO APLICACION DE FUERZA (Gazebo) client.call(srv1); /// CALCULO GIRO MOTORES giros.G1 = sqrt( A*U1act - B*U3act - C*U4act); giros.G2 = sqrt( A*U1act - B*U2act + C*U4act); giros.G3 = sqrt( A*U1act + B*U3act - C*U4act); giros.G4 = sqrt( A*U1act + B*U2act + C*U4act); /// Publicacion por el topic Pub_giros_motores.publish(giros); /// ACTUALIZACIONES DE VARIABLES Xk_2=Xk_1; Xk_1=Xk_T_g; Yk_2=Yk_1; Yk_1=Yk_T_g; zk_2=zk_1; zk_1=zk; rk_2=rk_1; rk_1=rk; pk_2=pk_1; pk_1=pk; yk_2=yk_1; yk_1=yk; uXk_1=uXk; uYk_1=uYk; uzk_1=uzk; urk_1=urk; upk_1=upk; uyk_1=uyk; eXk_2=eXk_1; eYk_2=eYk_1; ezk_2=ezk_1; erk_2=erk_1; epk_2=epk_1; eyk_2=eyk_1; eXk_1=eXk; eYk_1=eYk; ezk_1=ezk; erk_1=erk; epk_1=epk; eyk_1=eyk; U1ant=U1act; U2ant=U2act; U3ant=U3act; U4ant=U4act; /// Espera fin de ciclo (10 Hz) //~ ROS_INFO("Comienza sleep"); loop.sleep(); //~ ROS_INFO("Fin sleep"); } return 0; } e-REdING. Biblioteca de la Escuela Superior de Ingenieros de Sevilla.


MODELADO, CONTROL Y SIMULACIÓN DE UN QUADROTOR EQUIPADO CON UN BRAZO MANIPULADOR ROBÓTICO

: RAGEL DE LA TORRE, RICARDO
: Ingeniería Industrial
Contenido del proyecto:
Directorio raíz  >  ros_workspace  >  modelquad  >  src  >  control  >  control_RP.cpp