Robot equilibrista

[gera]

Muy bueno chelito!! Yo creo q jugando un poco con los valores del pid vas a lograr excelentes resultados
una pregunta, como sensas el angulo de inclinacion??
saludos!!

[willynovi]

Gera, lo del uso del modulo bluetooth es algo fundamental o se puede prescindir de él?
Digo, podrias usar algún otra comunicacón o modulo como los TWS-433 y RWS-433?

Y el giroscopio sabes si puede reemplazarse por algunotro elemento mas economico, aunque el resultado sea menos preciso?

[gera]

El modulo bluetooth no es para nada esencial. Pero facilita mucho el trabajo.
Yo lo uso para visualizar datos como la lectura de los sensores, y envio comandos para tunear el PID y cosas asi. Entonces no tengo q programar el pic cada vez q quiero cambiar un valor.
Ahora que estoy sin el modulo, tengo un cordon umbilical para comunicarme

La inclinacion puede medirse de muchas formas. Una es con un acelerometro y un gyro combinados, pero tambien he visto bots que miden la distancia al piso. Te dejo un video muy piola que encontre:

Saludos!!

[willynovi]

lo de la distancia al piso ya lo habia visto, el tema es que el sistema de medición deberia ser independiente del medio en el que transite, ya que si vamos por un terreno inclinado o con obstaculos esta situación no seria muy fiable, la de usar ese tipo de sensores.

Todavia no me he puesto con esto, pero la idea es ir aprendiendo algunos conceptos para ver que puedo llegar a hacer 

[gera]

Claro, para terrenos inclinados no sirve. No te queda otra que usar un acelerometro y un gyro.
Por la experiencia q llevo hasta ahora con mi robot, te puedo decir q la medicion del angulo de inclinacion (precision y tiempo) es critica.
Por eso mismo me voy a dedicar un tiempo a optimizar dicha medicion.

Animate!! Se lo ve complicado, pero de a poco se va avanzando y como le dije a nano es muy entretenido!
saludos!!

[chelitog555]

el calculo del angulo lo estamos manejando de la siguiente manera, primero se toman las medidas de los sensores

Código: [Seleccionar]

Read_ADC();
medida_0 = (float)(medida_giroscopo *3.3)/4096;
medida_0 = (float)(medida_0 - 1.343042)/0.00067;
//medida_1 = (medida_x - VOFF_X)/S;
medida_2 = (float)(medida_y - VOFF_Y)/S;
medida_3 = (float)(medida_z - VOFF_Z)/S;

// Angulo Calculado con Acelerometro
angle_m = atan2(-medida_2, medida_3); //angulo en radianes entre ejeZ y ejeY
angle_m = angle_m - (3.14159/2.0); //correccion para establecer el centro absoluto
angle_m = (angle_m *180)/3.14159; //en grados

gyro = medida_0; //grados por segundo, lectura del gyroscopio

y luego empleamos las siguientes funciones del filtro de Kalman para filtrar y fusionar las medidas, a continuacion presento alguns referencias con la descripcion de las funciones utilizadas para el filtro.

la mejor explicacion del filtro que he encontrado

Pequena traduccion y explicacion de las funciones de Kalman

esta pagina ayuda mucho a entender las ecuaciones

el siguiente codigo es para Mplab C30

Código: [Seleccionar]

void state_update() //Prediccion
{
float Pdot[4];

/*
 * The predict function. Updates 2 variables:
 * our model-state x and the 2x2 matrix P
 *     
 * x = [ angle, bias ]'
 *
 *   = F x + B u
 *
 *   = [ 1 -dt, 0 1 ] [ angle, bias ] + [ dt, 0 ] [ dotAngle 0 ]
 *
 *   => angle = angle + dt (dotAngle - bias)
 *      bias  = bias
 *
 *
 * P = F P transpose(F) + Q
 *
 *   = [ 1 -dt, 0 1 ] * P * [ 1 0, -dt 1 ] + Q
 *
 *  P(0,0) = P(0,0) - dt * ( P(1,0) + P(0,1) ) + dt² * P(1,1) + Q(0,0)
 *  P(0,1) = P(0,1) - dt * P(1,1) + Q(0,1)
 *  P(1,0) = P(1,0) - dt * P(1,1) + Q(1,0)
 *  P(1,1) = P(1,1) + Q(1,1)
 */

Pdot[0] =  - P[0][1] - P[1][0];
Pdot[1] = -P[1][1];
//Pdot[2] = -P[1][1];
//Pdot[3] = Q_gyro;


gyro -= q_bias;
gyrograd = gyro;
angulo += gyro * dt ;

P[0][0] += (Pdot[0] * dt) + (P[1][1]*dt*dt) + Q_angle;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[1] * dt;
P[1][1] += Q_gyro;
}

void kalman_update( float angle_m )
{
float K_0=0;
float K_1=0;

/*
 *  The update function updates our model using
 *  the information from a 2nd measurement.
 *  Input angle_m is the angle measured by the accelerometer.
 *
 *  y = z - H x
 *
 *  S = H P transpose(H) + R
 *    = [ 1 0 ] P [ 1, 0 ] + R
 *    = P(0,0) + R
 *
 *  K = P transpose(H) S^-1
 *    = [ P(0,0), P(1,0) ] / S
 *
 *  x = x + K y
 *
 *  P = (I - K H) P
 *
 *    = ( [ 1 0,    [ K(0),
 *          0 1 ] -   K(1) ] * [ 1 0 ] ) P
 *
 *    = [ P(0,0)-P(0,0)*K(0)  P(0,1)-P(0,1)*K(0),
 *        P(1,0)-P(0,0)*K(1)  P(1,1)-P(0,1)*K(1) ]
 */

K_0 = P[0][0] / (R_angle + P[0][0]);
K_1 = P[1][0] / (R_angle + P[0][0]);

P[0][0] -= K_0 * P[0][0];
P[0][1] -= K_0 * P[0][1];
P[1][0] -= K_1 * P[0][0];
P[1][1] -= K_1 * P[0][1];

angulo += K_0 * (angle_m - angulo);
q_bias += K_1 * (angle_m - angulo);
}

[gera]

Hmm, el famoso filtro de kalman. Si logras una buena medicion del angulo con el, no deberias tener problemas equilibrando tu robot. Es cuestion de jugar con las constantes del PID.

Por cierto, estoy haciendo unas pruebas con el filtro de kalman y las mediciones son bastante malas. Me podrias contar como hiciste para determinar los valores de las constantes? (R_angle, Q_angle y Q_gyro)
Gracias!!

[chelitog555]

Pues los valores iniciales los tome de un codigo ya implementado en la pagina de MAV-BLOG Ejemplo con Codigo Fuente y los modifique hasta obtener buenos resultados, tambien realice una simulacion del filtro en Matlab y la compare con los valores calculados por el dsPic y fueron muuuuyyy similares, tambien con dicha simulacion aprendi que el filtro empieza a dar muy buenas mediciones cuando la matriz P de 2x2 empieza a tener convergencia.

y por una sugerencia de Safareig aumente el valor de R_angle de 0.3 a 0.6 y realmente obtuve mucha mas estabilidad en la medida.

a mi me parece que la parte del filtro ya debe estar bien, sin embargo no creo que sea tan sencillo como solamente sintonizar el PID debido a que los Drivers y motores pueden tener la misma referencia pero sus respuestas siguen siendo diferentes y no lineales. yo creo que para obtener el resultado final voy a tener que implementar un PID de velocidad para cada motor para igualar sus respuestas y luego un PID de posicion o equilibrio que controle los anteriores PIDs. en total 3 PID, pero todavia no he implementado todo el codigo.

Código: [Seleccionar]

//float P[2][2] = { { 1, 0 }, { 0, 1 }, };   // se puede iniciar con matriz identica y luego tendra convergencia
//float P[2][2] = { { -0.0005, 0.0015 }, { -1.7829, -1.8521 }, } //estos valores se obtuvieron luego de tener convergencia;
float P[2][2] = { { 0.017326, -0.029120 }, { -0.031015, 1.784845 }, };
float R_angle = 0.6; //valores iniciales filtro de kalman
float Q_angle = 0.001; //R_a=0.3;
float Q_gyro = 0.003; //Q_a=0.001;
float q_bias=0.0; //Q_g=0.003

[willynovi]

yo creo que para obtener el resultado final voy a tener que implementar un PID de velocidad para cada motor para igualar sus respuestas y luego un PID de posicion o equilibrio que controle los anteriores PIDs

Yo creo que si, los motores debes controlarlos de esa forma, no se bien como los controlas actualmente, pero creo es unos de los puntos mas importantes.

Veo en tu último video que has cambiado las ruedas, o se trata de otro equilibrista? No alcanzo a distinguir bien la diferencias 

Recuerda que para poder implementar el control de velocidad de los motores debes disponer de unos que te entreguen sobrados la velocidad maxima que necesitas para equilibrarlo, lo mismo con la aceleración.
Por decirlo así, si con una aceleración de inclinación X (la máxima) necesitas Y aceleración en los motores, esta debes tomarla como la necesaria en los motores, si no puedes alcanzar esa aceleracion en los motores, entonces nunca podras compensar esa perturbación del equilibrio en tu robot.
De forma inversa puedes determinar que aceleración y velocidad tienen los motores y determinar la maxima perturbacion del equilibrio que puedes absorver.

[gera]

Muchas gracias Chelito!! Ahi pude obtener una medicion mas estable del filtro de kalman. Pero todavia existe un pequeño error en la medicion que hace q el robot no se pueda estabilizar.. es como si la medicion tuviera "juego", no se si me explico.
Voy a seguir haciendo pruebas, cambiando la frecuencia de muestreo y parametros del filtro. En estos dias subo un video mostrando como va hasta ahora la cosa.
saludos!!!

[gera]

Miren lo que conseguiii!! jejeje


Despues de implementar el filtro de kalman y sintonizar bien el pid, mejoro mucho el comportamiento del robot, pero seguia sin poder mantener el equilibrio. Resulta que con una pequeña inclinacion, los motores ya estaban al 100% y no se podia recuperar.
Me pedi estos motores de 24V 500rpm para estar sobrado. Obviamente la proxima version de mi robot no va a estar montado en la misma PCB, sino que voy a armar una estructura mas robusta, alta y resistente.
Cuando tenga novedades les aviso.
Saludos!!!

[AKENAFAB]

Se ven muy padres los motores!

De cuantas rpm son los mas pequeños???

[willynovi]

y de curiosidad, donde te pediste esos motores?
No conocias estos http://www.ignis.com.ar/PDF/mr6.pdf

Claro eso es lo que comentaba un poco mas arriba, que al tener unos motores lentos, la capacidad de equilibrarse es menor, si la perturvación es excesiva, la reaccion de los motores no es suficiente.

El peso de toda la estructura no creo que valla a ser mucho, entonces los motores trabajaran casi en vacio, ademas de que casi no soportarian carga al tener la mayor parte el peso en la parte superior del robot.
En ese caso, con que puente H vas a manejar esos motores, veo que tienen una Inom de 3Amp?

Saludos y espero que sigas adelante con este proyecto, yo todavia no me animo a comprar el giroscopo, estuve viendo algunos pero no los he estudiado como para saber cual es mejor.

[gera]

Se ven muy padres los motores!

De cuantas rpm son los mas pequeños???

El motor chiquito es este. Es de unas 120rpm

y de curiosidad, donde te pediste esos motores?
No conocias estos http://www.ignis.com.ar/PDF/mr6.pdf

Claro eso es lo que comentaba un poco mas arriba, que al tener unos motores lentos, la capacidad de equilibrarse es menor, si la perturvación es excesiva, la reaccion de los motores no es suficiente.

El peso de toda la estructura no creo que valla a ser mucho, entonces los motores trabajaran casi en vacio, ademas de que casi no soportarian carga al tener la mayor parte el peso en la parte superior del robot.
En ese caso, con que puente H vas a manejar esos motores, veo que tienen una Inom de 3Amp?

Saludos y espero que sigas adelante con este proyecto, yo todavia no me animo a comprar el giroscopo, estuve viendo algunos pero no los he estudiado como para saber cual es mejor.

Los motores los compre a los de AP&S en buenos aires. Consulte a los de ignis, pero me respondieron mejor los otros. Muy atentos, les conte sobre mi proyecto y me asesoraron muy bien. Para manejarlos pienso usar el L298.

Con el tema del gyro, te conviene usar uno de mucha sensibilidad. El mio es de 300º/s, pero el robot jamas cae a esa velocidad. Yo creo que con uno de 60º/s estas sobrado. El tema es conseguir buen precio, porq estan carisimos y son dificiles de encontrar :S
Suerte!!

[willynovi]

Gera, gracias por el dato de los motores, no los tenia presente a ese proveedor, me intereso mucho el micromotor SPL4 como para hacer un minisumo. Luego le pregunto precios, haran envio al interior?

Por lo del giroscopo es buen dato eso de los grados, y si que varia el precio en funcion de eso, vi algunos que hasta tienen comunicación integrada y a buen precio, luego en casa que tengo los link lo reviso, el tema seguro es poder traerlos a argentina.

Todavia no me lei todos los mensajes del hilo, seguro por ahi consigo mas datos sobre los giroscopos y como hacer para conseguirlos aca en argentina. Mi idea es usarlo en un bipedo para darle un poco de autonomia.

Claro el L298 lo podes usar tranquilo, yo creo que no vas a tener problemas, ademas que si ya haces el robot mas grande puedes ponerle algun disipador. Yo tengo hecho un puente h para un solo motor con un solo L298 y en teoria podría llegar a 4Amp, todavia no lo he probado con carga.

Saludos, Willy