Вы находитесь на странице: 1из 46

VISCA: Seguimiento de Caras Servo-Controlado

Rodrigo Munguia, Rafael Munguía, Antoni Grau

Department of Automatic Control, UPC ,


Pau Gargallo #5 08028 Barcelona España
{rodrigo.munguia, antoni.grau}@upc.edu
http://www-esaii.upc.es

Resumen

En este documento se presenta el diseño, implementación y resultados del


software y hardware para un sistema robotizado (cámara-servo-motorizado)
de dos grados de libertad (pan and tilt) para el seguimiento de caras
(tracking), robusto y económico. La detección de caras se realiza mediante
un clasificador Haar. Con el objetivo de brindar robustez al sistema y debido a
los falsos positivos que genera el clasificador, se propone utilizar dos etapas
de filtrado digital: Un prefiltro para la eliminación de “outliers” y un filtro de
kalman para estimar la posición óptima de la cara en la imagen a partir de la
salida de la etapa de prefiltrado y las entradas de control para el movimiento
de la cámara. El control de los dos grados de libertad de la cámara se realiza
mediante un proceso de Visual Servoing.

El seguimiento servo-controlado de objetos en tiempo real en una entrada


de video es un problema interesante y difícil en el área de la visión artificial y
la robótica. En estas áreas se han realizado diferentes avances pero sigue
siendo un tópico abierto a la investigación.

1
2
Contenido

1 Introducción..........................................................................................5
2 Marco teórico .......................................................................................6
2.1 Detección de objetos en imágenes (Clasificador de HAAR) ...........6
2.2 El filtro de Kalman............................................................................7
2.3 Visual Servoing................................................................................9
3 Sistema “VISCA” ................................................................................10
3.1 Planteamiento del problema..........................................................10
3.2 Descripción general de la solución (Diagramas a bloques) ..........11
3.3 Cámara robotizada (servo-controlada)..........................................13
3.4 Software de la PC..........................................................................15
3.4.1 Captura de video...................................................................16
3.4.2 Detección de caras en la imagen .........................................16
3.4.3 Etapa de filtrado....................................................................16
3.5 Tarjeta controladora ......................................................................20
3.5.1 Hardware...............................................................................20
3.5.2 Software de la tarjeta ............................................................22
4 Resultados .........................................................................................24
5 Anexos ...............................................................................................25
5.1 Codigo de la clase principal de VISCA (visual c++) ......................25
5.2 Código de los Microcontroladores ATMEL ....................................41
6 Referencias ........................................................................................46

3
4
1 Introducción
El seguimiento servo-controlado de objetos en tiempo real en una entrada
de video es un problema interesante y difícil en el área de la visión artificial y
la robótica. En estas áreas se han realizado diferentes avances pero sigue
siendo un tópico abierto a la investigación.
Los sistemas para detección y seguimiento automático de objetivos
particulares con una cámara (en nuestro caso caras) tienen un amplio rango
de aplicaciones científicas y comerciales. En robótica actualmente hay una
gran cantidad de investigación en esta área, que va desde aplicaciones
relacionadas con la robótica móvil, en especial en la creación de mapas de
navegación y reconocimiento de objetos y también en la robótica industrial,
donde la incorporación de técnicas de visión han dotado de mayores
capacidades a los robots industriales. Los sistemas de de detección y
seguimiento robotizado de objetos por visión tiene un gran potencial de
aplicaciones comerciales, que van desde los sistemas de video conferencia
donde se desea tener un “camarógrafo robot” , pasando por las aplicaciones
de video vigilancia y seguridad donde la detección de caras es fundamental
para la identificación manual o automática de individuos hasta sistemas
especializados como control de trafico urbano donde en lugar de caras se
pueden seguir automóviles automáticamente y detectar e identificar placas.
Todo el potencial de estos sistemas en sus posibles aplicaciones
científicas y comerciales además de que al investigar el costo comercial de
algunos sistemas más limitados (se limitan a seguir el movimiento sin
detectar objetivos específicos), nos motivo a diseñar e implementar un
sistema de detección y seguimiento robotizado de objetos mediante una
cámara; económico, robusto y versátil.
En la sección 2 se exponen brevemente las bases de algunas de las
principales técnicas usadas en este proyecto, posteriormente en la sección 3
se explica detalladamente el sistema VISCA y en la sección 4 los resultados y
aplicaciones de nuestro sistema.

5
2 Marco teórico
En esta sección de describen brevemente algunas de las técnicas mas importantes
utilizadas en este trabajo.

2.1 Detección de objetos en imágenes (Clasificador de HAAR)

El detector de objetos usado en este trabajo fue inicialmente propuesto en


[4]. Primero, un clasificador (llamado cascade of boosted classifiers working
with haar-like features) es entrenado con unos cientos de vistas de ejemplos
de un objeto en particular (ej. una cara o un automóvil), llamados ejemplos
positivos, que son escalados al mismo tamaño (ej. 20 x 20), y también
entenado con ejemplos negativos (imágenes arbitrarias del mismo tamaño).
Después de que el clasificador es entrenado, puede ser aplicado a regiones
de interés (del mismo tamaño que fue usado durante el entrenamiento) en
una imagen de entrada. La salida del clasificador marca “1” si la región es
congruente con el objeto (ej. cara/automóvil), y “0” en el caso contrario. Para
buscar el objeto en la totalidad de la imagen, se puede mover la ventana de
búsqueda a lo largo de la imagen y revisar cada locación usando el
clasificador. El clasificador es diseñado para que pueda ser fácilmente
“redimensionado” en orden de ser capaz de encontrar objetos de interés a
diferentes tamaños, lo cual es mas eficiente que redimensionar la imagen por
si misma. Por lo tanto, para encontrar un objeto de tamaño desconocido en la
imagen, la búsqueda se realiza varias veces a diferentes escalas.

La palabra “cascade” en el nombre del clasificador significa que el


clasificador resultante consiste en varios clasificadores simples que son
aplicados subsecuentemente a una región de interés hasta que en alguna
etapa el candidato es rechazado o todas las etapas son pasadas. La palabra
“boosted” significa que los clasificadores correspondientes a cada etapa son
a la vez complejos y están construidos de clasificadores simples usando una
de las diferentes técnicas de boosting (peso por voto) como Adaboost, Real
Adaboost etc. Los clasificadores más básicos son árboles de decisión con al
menos 2 ramas. Características Haar son la entrada de los clasificadores
más básicos.

6
Figura 1 Características Haar.

La Figura 1 muestra un ejemplo de características Haar. Las usadas en un


clasificador en particular dependen de su forma (1a, 2b, etc) posición en la
región de interés y la escala (no es la misma escala usada en al etapa de
detección). Por ejemplo, en el caso de la característica (2c) la respuesta es
calculada como la diferencia entre la suma de los píxeles de la imagen bajo
el rectángulo cubriendo la característica completa (incluyendo las franjas
blancas y negras) y la suma de los píxeles de la imagen bajo la franja negra
multiplicada por 3 en orden de compensar la diferencia de tamaños entre
áreas. La suma de los valores de los píxeles sobre las regiones
rectangulares es calculada rápidamente usando imágenes integrales.

2.2 El filtro de Kalman

El filtro de Kalman [5] es un conjunto de ecuaciones matemáticas que


proveen eficiente y recursiva forma de estimar el estado de un proceso, de
manera que minimiza la media del error cuadrático: El filtro es muy poderoso
en diferentes aspectos: soporta estimaciones del pasado, presente e incluso
de estado futuros y puede ser incluso preciso cuando la naturaleza del
sistema modelado no es bien conocida.

El filtro de Kalman se enfoca en el problema general de estimar el estado x


Є Rn de un proceso controlado discreto en el tiempo, que es gobernado por
una ecuación diferencial lineal y estocástica:

7
xk = Axk −1 + Bu k −1 + wk −1 (1)

Con mediciones z Є Rm:

z k = Hx k + v k (2)

Las variables aleatorias wk y vk representan el ruido del proceso y la


medición respectivamente. Se asumen a ser independientes (una de otra),
blancas y con distribución de probabilidad normal (gaussiana).

p ( w) ≈ N (0, Q) (3)
p (v) ≈ N (0, R ) (4)

Q y R son las matrices de covarianza del ruido del proceso y medición


respectivamente. La matriz n x n A¸ en la ecuación diferencial Ec. (1)
relaciona el estado en el instante previo K-1 con el paso actual K. La matriz n
x l B relaciona la entrada opcional de control u Є Rl al estado x. La matriz m
x n H en la ecuación de medición Ec. (2) relaciona el estado a la medición zk.

El filtro de Kalman estima el proceso utilizando una forma de control


retroalimentado: El filtro estama el estado del proceso en algún instante de
tiempo y entonces obtiene retroalimentación en forma de mediciones
(ruidosas). Por lo que las ecuaciones del filtro de Kalman recaen en dos
grupos: Actualización en el tiempo (time update) y ecuaciones de
actualización de la medición (measurement update). (Figura 2) las
ecuaciones de time update son responsables de proyectar hacia delante (en
tiempo) el estado actual y el error de covarianza estimada para obtener la
estimación a priori para el siguiente paso. Las ecuaciones measurement
update son responsables de la retroalimentación, al incorporar una nueva
medición en la estimación a priori para obtener una estimación a posteriori
mejorada. Para mayor información sobre el algoritmo del filtro de Kalman, su
origen, justificación y ecuaciones consultar [5].

Figura 2 El ciclo del filtro de Kalman.

8
2.3 Visual Servoing

Usar retroalimentación visual para controlar un robot es comúnmente


llamado visual servoing (VS). Características visuales como puntos, líneas y
regiones pueden ser usadas por ejemplo para alinear el mecanismo terminal
de un manipulador con un objeto. La visión es la parte del sistema de control
que provee la retroalimentación acerca del estado del entorno. En términos
de la manipulación una de las principales motivaciones para incorporar visión
en el lazo de control fue la demanda por incrementar la flexibilidad de los
sistemas robóticos.

Un sistema de lazo cerrado de control de un sistema robótico donde la


visión es usada como el sensor principal, usualmente consiste en dos
procesos, seguimiento y control (tracking and control), El seguimiento provee
una continua estimación y actualización de las características durante el
movimiento robot-objeto. Basada en esta entrada sensorial una secuencia de
control es generada. El sistema muchas veces también requiere una
inicialización automática que generalmente incluye una segmentación y
reconocimiento de objetos. En la literatura existe una buena introducción al
VS en forma de tutorial en [1,2]

9
3 Sistema “VISCA”
En esta sección se explica el diseño, implementación y funcionamiento de
nuestro sistema, primero se indica la problemática a resolver, posteriormente
se expone el sistema en general y los módulos que lo conforman y por ultimo
se explican los aspectos específicos de cada modulo que conforma el
sistema.

Figura 3. Planteamiento del problema

3.1 Planteamiento del problema

El problema a resolver se puede resumir como: Teniendo una cámara en


una posición fija x,y,z del entorno desconocida a la cual se le puede controlar
el movimiento de dos grados de libertad angular (α,θ) (Pan and tilt) mediante
servos, dadas condiciones dinamicas de movimiento, iluminación o ruido, y
teniendo una o mas personas en el rango de visión total de la cámara,
entendiéndose por rango total de visión de la cámara no solo lo contenido en
la imagen captada por la misma en un instante si no en todo lo que
potencialmente pudiera ser captado moviendo la cámara dentro de los dos
grados de libertad disponibles. Implementar un sistema automático capaz de
identificar seleccionar y seguir el movimiento de las caras de las personas en
el entorno de la cámara mediante el control de sus dos grados de libertad. El
sistema debe ser razonablemente robusto a los cambios dinámicos del

10
entorno y del movimiento de las personas. En la Figura 3 se ilustra el
planteamiento del problema.

3.2 Descripción general de la solución (Diagramas a bloques)

El problema en general se puede definir como el diseño de un sistema de


control de lazo cerrado: El objetivo es que mediante el control de la
orientación de una cámara, seguir automáticamente (enfocar en el centro de
la imagen) las caras de las personas, en movimiento, dentro del campo de
visión de la cámara. Visto mas técnicamente si una cara es detectada en las
imágenes correspondientes al video entrante de una cámara con posición
(xr,yr,zr) del espacio en R3 y orientación (α,θ) y si la cara es detectada en una
posición de la imagen en píxeles (xi,yi) diferente del centro de la imagen
(xo,yo), podemos hablar de un error έ causado por la orientación actual de la
cámara (α,θ).:

ε xy = [x0 , y0 ] − [xi − yi ] (5)

ε x = [xo − xi ] (6)
ε y = [ y 0 − yi ] (7)

Como en este caso el lazo de control es cerrado mediante el empleo de


visión, podemos considerar al sistema como un sistema de Visual Servoing
(VS) (sección 2.3). En la Figura 4. Se muestra un diagrama simplificado del
esquema de VS utilizado en este proyecto.

Entonces el problema consiste en diseñar un sistema automático que


mediante el control de la orientación (α,θ) de la cámara minimice el error έ en
la imagen a lo largo del tiempo (έ->0) dadas las diferentes variaciones en
(xi,yi) causadas por el movimiento de la persona.

Desde luego que al resolver este problema en la practica, surgen


dificultades propias de cualquier implementación, si tomamos en cuenta que
por ejemplo el solo hecho de detectar caras en tiempo real en una cámara
estática es ya de por si un problema interesante, podemos prever que la
implementación de nuestro problema en su totalidad implica un reto mucho
mayor, considerando que para solventarlo es necesario la resolución de
varios problemas individuales mediante la utilización de diferentes técnicas,

11
como por ejemplo la detección de caras en imágenes, así como el diseño de
un enfoque general para resolver el problema en su totalidad acoplando los
diferentes módulos del sistema. Muchas veces es en esta parte donde surgen
las mayores vicisitudes de las implementaciones, teniendo que resolver
problemas que no salían a relucir, teniendo en cuenta los módulos del
sistema por separado. De esta manera, cuestiones como la sintonización de
los parámetros del sistema y sus constantes de tiempo fueron también retos
interesantes de resolver.

Figura 4. Diagrama simplificado del control por Visual Servoing.

Un enfoque de diseño interesante es el de que las capacidades del sistema


dependan únicamente del hardware propio del sistema, sin embargo ciertas
técnicas como la detección de caras y el filtro de kalman, implican una
considerable capacidad de procesamiento que escapan a las posibilidades de
los microcontroladores. Por esta razón el sistema VISCA se distribuye entre
el hardware diseñado por nosotros y la PC que captura el video de la cámara.

El sistema se puede dividir en tres subsistemas o módulos principales que


interactúan entre si, especificados en la Figura 5: La cámara y su plataforma
servo-motorizada. La tarjeta controladora y su respectivo software embebido
y el software de la PC. En las siguientes sub-secciones se explican estos
módulos del sistema.

12
Figura 5. Diagrama a bloques del sistema.

3.3 Cámara robotizada (servo-controlada)

El problema tal y como es planteado en la sección 3.1 y formalizado en la


sección 3.2 considera que contamos con una cámara a la cual podemos
controlar dos grados de libertad (Pan and tilt). Una web-cam comercial que
cuenta con esta función es [3], claro que por obvias razones comerciales no
facilitan el acceso de bajo nivel para controlar el movimiento de la cámara
además de que para ser una web-cam es relativamente costosa , por otro
lado algunas cámaras profesionales cuentan con esta función pero
escapaban de nuestro reducido presupuesto además de viola nuestro
lineamiento de diseño de implementar un sistema lo menos costoso posible,
así que nuestra solución fue construir nuestra propia plataforma servo-
motorizada de dos grados de libertad y montar en ella una web-cam
convencional de tamaño reducido. Dado que nuestro objetivo actual era la
implementación de un prototipo que demostrase las capacidades de nuestro
sistema (software y hardware electrónico) la plataforma servo-motorizada que
construimos resulto suficiente para nuestros objetivos actuales, aunque
desde luego tendrá que ser objeto de crítica de un ingeniero mecánico.

13
Figura 6. Imagen de la cámara servo-motorizada.

En la Figura 6 se muestra nuestra cámara servo-motorizada (todavía no se


considera servo-controlada, ya que no hemos explicado como hacerlo). El
armazón esta construido de madera balsa por su fácil manejo y ligereza,
algunos pivotes plásticos y una base de madera mas rígida. La web-cam que
utilizamos es una creative live ultra for notebook¸ [6] la seleccionamos porque
ofrece una buena calidad de imagen con un censor CCD de 640 x 480
píxeles a un alto frame-rate (30 FPS), y sobre todo porque son de las pocas
del mercado que posee un lente de visión amplia (wide len) que permite un
ángulo de visión de 76º contra los típicos 40º de muchas otras cámaras, esto
nos permite aumentar el área útil del entorno sobre la cual se pueden
detectar caras, dando mayor posibilidad al sistema de no perder la cara. Una
consideración importante en el diseño de la plataforma servo-motorizada fue
el tratar de hacer coincidir los ejes de rotación (α,θ) con el punto focal de la
cámara, de esta manera por ejemplo un giro en α o en θ causan una rotación
pura de la cámara , sin alterar la posición (xr,yr,zr) de la cámara en el espacio,
esto permite eliminar del modelo cinemático de la cámara su posición (xr,yr,zr)
en el espacio, al ser esta constante. Facilitando el proceso de Visual
Servoing. Los servos utilizados son dos Futaba estándar [7]. Estos servos
utilizados a menudo en modalismo tienen la cualidad de ser económicos,
fiables y de contar con un torque suficiente para nuestra aplicación.

14
3.4 Software de la PC

Dado que ya contamos con nuestra cámara servo-motorizada de dos


grados de libertad, ahora explicaremos nuestra propuesta para resolver el
problema general, a partir de la captura de la imagen y la detección de las
caras, hasta el cierre del lazo de control mediante el control enviado a los
servos de la cámara para disminuir el error en la imagen.

El software de la PC tiene como objetivo capturar el video de la cámara por


el puerto USB y a partir de ellas detectar las caras en la imagen, filtrar la
información de la etapa de detección de caras y enviar por el puerto RS232 la
información de error έ a la tarjeta controladora.

Figura 7. Software de la PC.

15
3.4.1 Captura de video

La captura de video se realiza directamente (sin pasar por una etapa de


almacenamiento) a partir del driver de la cámara. Para acelerar el
procesamiento de video se trabajan con punteros a memoria. Cabe
mencionar que para esta y otras funciones relacionadas con el
procesamiento de imágenes y visión, utilizamos las librerías de código abierto
openCV [8], estas librerías proveen una buena cantidad de funciones para
visión bajo licencia GNU y que nuestro programa fue programado en Visual
C++.

3.4.2 Detección de caras en la imagen

La detección de caras en la imagen se realiza mediante un clasificador


Haar (seccion 2.1). A este clasificador se le pueden ajustar algunos
parámetros como el tamaño mínimo que puede tener una cara en la imagen o
la cantidad mínima de zonas detectadas para considerar la detección de una
cara. Dependiendo del ajuste de los paramentos del clasificador se obtiene
un compromiso entre la calidad de la detección y la velocidad de cómputo, a
mayor calidad de detección mayor coste computacional y a menor calidad de
detección menor coste computacional. Ya que nuestra aplicación requiere un
funcionamiento en tiempo real, seleccionamos parámetros de valor medio. Es
importante señalar que aunque la salida del clasificador (coordenadas del
centro de la cara) es buena dista mucho de ser optima, esto significa que en
ciertas condiciones se producen falsos positivos (detecciones erróneas de
zonas de la imagen), esto podría causar consecuencias catastróficas a
nuestra aplicación, causando inestabilidad en el sistema y en el moviendo de
la cámara (esta podría volverse “loca”). Además la salida del clasificador
también presenta cierto ruido (pequeñas variaciones de las coordenadas
detectadas) lo que también podría inducir pequeñas perturbaciones al
sistema. Es por todo lo anterior que una etapa de filtrado se vuelve
imprescindible y viene a ser una importante parte de nuestra aplicación.

3.4.3 Etapa de filtrado

Para la estimación óptima de la posición de la cara en la imagen se


propone la utilización de un filtro de Kalman (seccion 2.2)., su uso tiene varias
ventajas como el hecho de proveer una salida suave y estable, reducción de l

16
tiempo de procesamiento, al limitar la zona de búsqueda de la cara utilizando
la predicción del filtro, la incorporación a las estimaciones de los comandos
de control, la modelización y estimación conjunta de otros parámetros del
sistema (por ejemplo el tamaño de la cara) y su posible utilización a futuro
como modelo de fusión con otros sensores (audio).

Sin embargo el filtro de Kalman es sensible a la presencia de outliers en


las mediciones, esto significa que funciona muy bien cuando el valor real de
la variable evaluada se encuentra contemplado en la medición actual y su
modelo de ruido gaussiano. Dicho de otra manera el filtro de Kalman tiene
dificultades cuando le entran valores que no corresponden a la variable
estimada (en este caso la cara), situación que es habitual a la salida del
clasificador. Por tal motivo diseñamos una sencilla etapa de filtrado entre la
salida del clasificador y el filtro de Kalman, que llamamos “filtro de outliers”.

El filtro de outliers filtra datos correspondientes a caras muy grandes


(establece una distancia mínima entre la persona y la cámara para su
detección), desecha mediciones detectadas a una distancia mayor l que la
estimación obtenida por el filtro de Kalman y establece un parámetro mínimo
de estabilidad en la medición para que una segunda cara detectada en la
imagen se considerada a ser tomada en cuenta pasa su seguimiento.

El filtro de Kalman es aplicado a la salida del filtro de outliers. Como se


menciono anteriormente el filtro de Kalman estima óptimamente sistemas
dinámicos especificados en la forma de estado-espacio. Para aplicarlo
nosotros modelamos el sistema de la siguiente manera:

Una nueva imagen de la secuencia se adquiere y se procesa en cada


instante, ti = t0 + i, donde i es un número natural. Se asume que el intervalo
de muestreo es 1 por simplicidad, y lo suficientemente pequeño para
considerar que el movimiento de características entre dos imágenes
consecutivas de la secuencia es lineal.

Vamos a considerar para el desarrollo un único punto como característica


(posición de la cara en la imagen) pi=[xi,yi] en la imagen adquirida en el
instante tk moviéndose con velocidad vi =[vxi, yyi]. Describimos el movimiento
en el plano de la imagen con el vector de estado:

[
xi = xi , yi , v xi , v yi ] (8)

Se considera un intervalo de muestreo de imágenes lo suficientemente


pequeño (y por tanto con velocidad del punto constante entre dos imágenes
consecutivas de la secuencia), se puede describir el modelo del sistema
como:

17
pi = pi −1 + vi −1 + υ
vi = vi −1 + η (9)

vi −1 = pi −1 − pi −2 (10)

Donde υ y η son ruido blanco Gaussiano con matrices de covarianza Q y R


respectivamente. En términos del vector de estado xi , Ec.8 para su
implementación en el filtro de Kalman se puede escribir como:

xi = Axi −1 + Bui −1 + w (11)

con:
⎡1 0 1 0⎤ ⎡0 0⎤
⎢0 ⎢0 0⎥⎥
1 0 1⎥⎥ ⎡υ ⎤
A=⎢ B=⎢ w=⎢ ⎥
⎢0 0 1 0⎥ ⎢1 0⎥ ⎣η ⎦
⎢ ⎥ ⎢ ⎥
⎣0 0 0 1⎦ ⎣0 1⎦

Donde ui-1 es nuestra entrada de control al sistema, es decir, el efecto en


las coordenadas pi=[xi,yi] que estimamos posterior a la aplicación de nuestro
control en los servos para mover los ángulos de la cámara (α,θ) con el
objetivo de la disminución del error disminución de έxy =[xo,yo]-[xi,yi] Ec.5.
Como medición se utiliza zi y su modelo se establece como:

⎡1 0 0 0 ⎤ ⎡ p i ⎤
z i = Hxi −1 + µ = ⎢ ⎥⎢ ⎥ + µ (12)
⎣ 0 1 0 0 ⎦ ⎣ vi ⎦

Donde H es la matriz de medida y µ es la incertidumbre de la medida


modelada como ruido blanco Gaussiano.

Aplicando el filtro de Kalman para cada frame i obtenemos la estimación


de la posición de la cara pi=[xi,yi] . Con la ecuación Ec.5 obtenemos el error
έxy que tendrá que ser disminuido por el movimiento de los grados de libertad
(α,θ) de la cámara.

La siguiente función del software de la PC es enviar el error έxy en píxeles


a la tarjeta controladora por el puerto rs232. Es importante mencionar que
hay un tiempo de variable T milisegundos, mínimo a esperar antes de enviar
έxy+1. T es determinado en función de la respuesta del sistema: tiempo de
comunicaciones entre PC y la tarjeta controladora, tiempo de procesamiento
del microcontrolador, tiempo de respuesta del servo entre la posición actual
(α,θ) y la nueva (α’,θ’) para corregir έxy y el tiempo mínimo que tarda en
detectar nuevamente la cara el sistema. T es un parámetro de sintonización
muy importante porque si es menor a un valor adecuado entonces el sistema

18
no convergerá entrando en un estado de oscilación creciente y permanente
hasta que pierda la cara. Por otro lado si es muy grande, la respuesta del
sistema será lenta.

El primero se calcularon y se probaron los valores de tiempo


correspondientes a movimientos muy pequeños (|έ|min) = |έ|->0 , (con |έ|
determinado por Ec.14) con lo que establecimos Tmin, después probamos con
desplazamientos de la cámara grandes (|έ|max) = 100 para determinar un Tmax,
con esto determinamos el valor de T como una función lineal de la distancia
en la imagen que tenían que corregir έxy el movimiento en los servos, sin
embargo al probarlo encontramos que la relación entre T y |έ| no era lineal.
Así por ejemplo para un valor |έ| = (|έ|max-|έ|min)/2 el tiempo T requerido no
era (Tmax- Tmin)/2, sino un valor de T tendiente mas a Tmax. Al final ajustamos
un polinomio cuadrático para obtener T.

T (ε ) = k1 ε + k 2 ε + k 3
2
(13)

Con K1 = -.0266 , K2 = 4.66 y K3 = 100 y

ε = ( xo − xi ) 2 + ( yo − yi ) 2 (14)

En la Figura 8se muestran los milisegundos de espera T correspondientes


a un error de έ píxeles. Estimado T se iniciaba un timer en otro thread (para
que el procesamiento principal continuara normalmente) al expirar el timer
después de un tiempo T se activa una bandera que indica al sistema que es
posible enviar una nueva estimación de έxy a la tarjeta controladora.

Figura 8. Grafica de la Eq 13..

19
3.5 Tarjeta controladora

La tarjeta controladora tiene como función recibir la información del error


έxy de la PC por el puerto rs232, y en base a έxy estimar y generar el PWM
(pulse wide modulation) (modulación por ancho de pulso) necesario para
generar el movimiento (α’,θ’) en los servos que mueven la cámara para
disminuir έxy. A continuación explicamos el hardware y software que
implementamos para estas funciones, algunas de las ideas para la
implementación de la comunicación rs232 entre la PC y el microcontrolador,
la comunicación entre microcontroladores y el control del los servos por PWM
las basamos en [9].

3.5.1 Hardware

La tarjeta esta basada en dos microcontroladores ATMEL RISC de 8 bits


[10]. El ATMEGA8535 y el AT902313. El 8535 tiene la función de encargarse
del protocolo de comunicaciones con la PC, recibiendo y mandando
información a través de su puerto UART integrado. Un MAX232 realiza la
conversión de niveles de voltajes de TTL que maneja el microcontrolador a
los niveles utilizados por el rs232. El 8535 es un microcontrolador bastante
versátil con varias funciones integradas, la tarjeta se diseño de manera que si
en lo posterior se integraran otros sensores como por ejemplo alguno basado
en audio, pudieran ser fácilmente anexado mediante un nuevo modulo
electrónico conectado al 8535.

El microcontrolador 2313 tiene la función dedicada de recibir los datos


“digeridos” del error έxy, estimar y generar el PWM para mover los servos lo
necesario para corregir dicho error. La tarjeta se complementa por electrónica
analógica periférica necesaria. Para el diseño del esquemático y la
generación del PCB de la tarjeta utilizamos la versión demo de eagle [11]. En
la Figura 9 se muestra el esquemático de la tarjeta controladora
(Microcontroladores, integrados y electrónica digital y analógica periférica
necesaria). En la Figura 10 se muestra una imagen de la misma.

20
Figura 9. Esquemático de la tarjeta controladora.

21
Figura 10. Imagen de la tarjeta controladora.

3.5.2 Software de la tarjeta

Como se menciono anteriormente tiene la tarjeta tiene que recibir la


información del error έxy de la PC por el puerto rs232, y en base a έxy estimar
y generar el PWM necesario para generar el movimiento (α’,θ’). Para
programar los microcontroladores empleamos el compilador BASCOM [12].

El programa del 8535 implementa un sencillo protocolo para rechazar


errores en la comunicación con la PC, y a sus ves implementa su parte
correspondiente al protocolo de comunicación en paralelo con el
microcontrolador 2313. Como se menciono, el 2313 recibe έxy =[xo,yo]-[xi,yi]
en píxeles, correspondiente a la diferencia entre la localización de la cara a
seguir en la imagen y el centro de la imagen.

Como conocemos el ángulo de visión de la cámara Ωx=76º en horizontal y


el largo en píxeles de la imagen (320), es posible determinar el ángulo α’
correspondiente al error έxy en el eje de las x. Análogamente como
conocemos el ancho en píxeles de la imagen (240) y su proporción con el
largo, también podemos determinar el ángulo de visión en el eje y, Ωy=57º , y
por tanto también obtener el ángulo θ’ correspondiente al error έxy en el eje de
las y. De esta manera los ángulos de corrección (α’,θ’). se estiman:

76 57
α '= εx θ '= εy (15)
320 240

22
Figura 11. Disminución del error έx mediante el giro de α’ la cámara sobre el eje
α, vista x,z y x,y (imagen)

En la Figura 11se ilustra la estimación del movimiento necesario en los


servos (α’,θ’) para corregir el error έxy . Cabe mencionar que debido a la
configuración de los grados de libertad de la cámara, no es necesario estimar
la profundidad a la que se encuentra la cara, basta con saber que respecto a
la cámara, el centro de la cara se encuentra a una distancia desconocida,
pero sobre una línea recta que comienza en el foco de la cámara y pasa por
pi=[xi,yi] en el marco de referencia de la imagen. De esta manera la escala
correspondiente a lo que mide un píxel en el mundo real varia con la
distancia, entre mas retirado se encuentre un objeto de la cámara, un píxel
perteneciente a dicho objeto corresponde a una medida mayor.

Por ultimo cada servo es situado en su nueva posición (αi,θi) mediante la


modulación PWM correspondiente [9] donde:

[α i ,θ i ] = [α i−1 ,θ i−1 ] + [α ' ,θ '] (16)

El lazo de control se cierra cuando (αi,θi) determinan las nuevas


coordenadas pi=[xi,yi] de la cara en la imagen que deberían de representar
έxyi < έxyi-1. Desde luego que los movimientos de la persona producirán que el
lazo de control continué por tiempo indefinido, consiguiendo que la cámara
siga automáticamente el rostro de la persona en movimiento.

23
4 Resultados
Después de la etapa de pruebas y depuraciones, concluimos que los
resultados del sistema VISCA fueron satisfactorios. Logramos implementar
un sistema funcional con poca inversión económica y muchas horas de
trabajo. El seguimiento de las caras fue razonablemente robusto en
estabilidad y velocidad. Desde luego que se puede continuar trabajando en el
sistema para mejorarlo. Por ejemplo se podría integrar la comunicación entre
PC y tarjeta controladora en un solo cable USB para sustituir la comunicación
rs232, El software de la PC pudiera ser depurado he integrado en un driver
estándar de manera que el sistema pudiera ser utilizado conjuntamente con
cualquier aplicación comercial externa de video-conferencia como el
Messenger o video-vigilancia por mencionar algunas. Un aspecto
interesante en el que nos gustaría trabajar posteriormente es el de la fusión
de video y audio para implementar funciones que automáticamente simulen lo
que haría un “camarógrafo” humano al captar personas hablando.

Las posibles aplicaciones del sistema VISCA abarcan cualquier aplicación


de investigación o comercial, en la que resulte útil el seguimiento de caras de
manera servo-controlada. Por la modularidad del proyecto no seria difícil
hacer modificaciones para adaptarlo a algún tipo de aplicación en específico.
Un posibilidad interesante es el de entrenar el clasificador para la detección
de otro tipo de objeto, por ejemplo el cuerpo completo de un peatón o un
automóvil, esta modificación permitiría aumentar el rango de aplicaciones
posibles. En el estado actual del proyecto seria directa su aplicación
comercial en sistemas de videoconferencia o de video vigilancia, en este
ultimo tipo de aplicaciones, seria posible agregar a la detección, incluso un
modulo de reconocimiento de rostros.

24
5 Anexos

5.1 Codigo de la clase principal de VISCA (visual c++)

A continuación se anexa el código de la clase principal en c++ de VISCA. Es


importante hacer notar que el software completo incluye otras clases secundarias así
como dependencias a algunas librerías, la más importante la OPENCV de Intel [8] la
cual provee algunas de las funciones mas importantes del sistemas como es el detector
por clasificador Haar o las funciones de acceso al stream de video de la cámara o
entre otras funciones de visión. Por tal motivo el código se provee con el objetivo de
ilustrar un poco la idea de nuestro algoritmo y la utilización de algunas funciones del
OPENCV.

///////////////////////////////////////////////////////////////////////
/// Proyecto VISCA (2006)
///
/// Rodrigo Munguía, Rafael Munguia
///
///
//////////////////////////////////////////////////////////////////////

#include "StdAfx.h"
#include <afxwin.h>
#include "ftrack.h"

#include "cv.h"
#include "cvcam.h"
#include "highgui.h"
//#include "lrtimer.h"

///////////////////////////////////////////
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <limits.h>
#include <time.h>
#include <ctype.h>

#include <ctime>
//#include "WaitableTimer.h"
//#include "Timer.h"

CSerialCom port;

static CvMemStorage* storage = 0;


static CvHaarClassifierCascade* cascade = 0;

//void detect_face( IplImage* image,IplImage* dest );


void detect_face( IplImage* image);

25
void MandarAngulo(float alfa,float teta);

//const char* cascade_name = "haarcascade_frontalface_alt.xml";


const char* cascade_name =

"C:\\....... \\ViscaCodigo\\haar\\haarcascade_frontalface_alt.xml";

////////////////////////////////////////////////////////////////

float alfaX;
float tetaY;
float AngX;
float AngY;
int Servo1T;
int Servo2T;
float VelX;
float VelY;
float VelXM;
float VelYM;
bool OpP = false;
CvPoint pt1, pt2,ptw,ptp,ptc;
int count;

int ContMasDosVentanas = 0;
int ContDistMasEntreCaras = 0;
int Bandera1 = 0;
int BanderaNuevaMedicion = 0;
double ZizeCuadro = 40;

int BanderaTemp = 0;
int TrackingContinuo = 0;
int SingleTrack = 0;
int BanderaControl = 0;

int ByteXtemp , ByteYtemp;


int ByteX , ByteY;

int NumeroFramesSinDeteccion = 0;

///////////// Kalman //////////////////

CvKalman* kalman ;

CvMat* state;
CvMat* process_noise;
CvMat* measurement;
CvMat* control;

float Cara = 40;

//Timer t1;

CMMTimer m_Timer;
UINT m_MaxResolution;

int t_on;
int t_off;

26
void callback(IplImage* image);

/*
char* TempChar;

UINT LeesThread( LPVOID pParam )


{
//--put your thread code here
while(TRUE) //loop if you want it to run continuously
{
int x;
x = 2;
CString g_Message;
int p_PeriodInSecs = 1;
float temp = 1000;

time_t t1;
t1=clock();
while((float)(clock()-t1)<temp);
//MandarAngulo(alfaX,tetaY);
//if((ByteX != ByteXtemp)||(ByteY != ByteYtemp))
//{
if (BanderaTemp == 1)
{
MandarAngulo(ByteX,ByteY);
}
}
return 0;
}
*/

void FTrack::SeguimientoContinuo(bool estado)


{
if (estado == true)
{
TrackingContinuo = 1;
BanderaTemp = 1;
}
else
{
TrackingContinuo = 0;
BanderaTemp = 0;
}

FTrack::FTrack(void)
{

// SetTimer(1, 200, 0);

// AfxBeginThread(LeesThread, TempChar);
m_Timer.AttachListener(*this);

// Determine the maximum resolution for this timer


TIMECAPS TimeCaps;

27
m_Timer.GetDevCaps(&TimeCaps);
m_MaxResolution = TimeCaps.wPeriodMax;

alfaX = 86;
tetaY = 106;

ptw.x = 320/2;
ptw.y = 240/2;

/////////////////// Haar ////////////////////

cascade = (CvHaarClassifierCascade*)cvLoad( cascade_name, 0, 0,


0 );
if( !cascade )
{
fprintf( stderr, "ERROR: Could not load classifier cascade\n" );
//return -1;
}

storage = cvCreateMemStorage(0);

///////////////////////////////////
kalman = cvCreateKalman( 5, 3, 2 );
state = cvCreateMat( 5, 1, CV_32FC1 );
process_noise = cvCreateMat( 5, 1, CV_32FC1 );
measurement = cvCreateMat( 3, 1, CV_32FC1 );
control = cvCreateMat( 2, 1, CV_32FC1 );

const float A[] = {1,0,1,0,0,


0,1,0,1,0,
0,0,1,0,0,
0,0,0,1,0,
0,0,0,0,1};
const float H[] = {1,0,0,0,0,
0,1,0,0,0,
0,0,0,0,1 };
/*
const float B[] = {1,0,0,0,0,0,
0,1,0,0,0,0};
*/
const float B[] = {0,0,
0,0,
1,0,
0,1,
0,0};

const float PN[] = {.1,0,0,0,0,


0,.1,0,0,0,
0,0,.0001,0,0,
0,0,0,.0001,0,
0,0,0,0,.1};

//CvMat* A = cvCreateMat( 4, 4, CV_32FC1 );

memcpy( kalman->transition_matrix->data.fl, A, sizeof(A));


memcpy( kalman->measurement_matrix->data.fl, H, sizeof(H));
memcpy( kalman->control_matrix->data.fl , B, sizeof(B));

//cvSetIdentity( kalman->process_noise_cov, cvRealScalar(2) );

28
//cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1) );

cvSetIdentity( kalman->process_noise_cov, cvRealScalar(.01) );


//memcpy( kalman->process_noise_cov->data.fl , PN, sizeof(PN));

cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(.001)


);

//cvSetIdentity( kalman->error_cov_post, cvRealScalar(1));

const float IniState[] = {160,120,0,0,40};


memcpy( kalman->state_post->data.fl , IniState, sizeof(IniState));

float h[] = {0,0}; // sin tomar encuenta entradas de control


memcpy( control->data.fl , h, sizeof(h));

const CvMat* prediction = cvKalmanPredict( kalman, control);

int g = 4;

VelX = 0;
VelY = 0;
count = 0;

FTrack::~FTrack(void)
{

cvcamExit();
}

////////////////////////////////////////////////////////////////////////
//////////////////
////////////// COMUNICACIONES
//////////////////////////////////////////////////////////7
bool FTrack::OpenPort(void)
{

if(port.OpenPort("com4"))
{
port.ConfigurePort(9600,8,0,NOPARITY ,ONESTOPBIT );
port.SetCommunicationTimeouts(0,100,0,0,0);
//MandarAngulo(90,110);
alfaX = 90;
tetaY = 110;
OpP = true;
return true;
}
else
{
return false;
}
}
bool FTrack::ClosePort(void)
{
port.ClosePort();
OpP = false;

29
return true;

}
int FTrack::ReadPort(int salida[512])
{

int Temp[512];

BYTE data=0;
port.ReadByte(data);
if(!data==0)
{
salida[0]= data;

int g = 1;
while ( !data == 0)
{

port.ReadByte(data);
if(!data==0)
{
salida[g]= data;
g++;
}
int r;
}
return g;

}
else
{
return 0;
}
}
void FTrack::WriteChar(BYTE caracter)
{

port.WriteByte(caracter);

////////////////// FIN DE METODOS PARA COMUNICACIONES


///////////////////////////////
////////////////////////////////////////////////////////////////////////
//////////////
///////////////// VIDEO ////////////////

void callback(IplImage* image)//Draws blue horizontal lines across the


image
{
//IplImage* image1 = image;

//detect_and_draw(image);
int t_on = clock(); //inicio medicion tiempo

IplImage *frame, *frame_copy = 0;


frame = image;

30
frame_copy = cvCreateImage( cvSize(frame->width,frame->height),
IPL_DEPTH_8U, frame-
>nChannels );
if( frame->origin == IPL_ORIGIN_TL )
cvCopy( frame, frame_copy, 0 );
else
cvFlip( frame, frame_copy, 0 );

//detect_face( frame_copy ,image);


detect_face( frame_copy);

int t_off = clock(); // // fin medicion tiempo


float Temp;

Temp = (static_cast<float>(t_off - t_on))/CLOCKS_PER_SEC;

Temp = Temp;

cvReleaseImage( &frame_copy );

void FTrack::Capture(void)
{
IplImage *frame, *frame_copy = 0;
CvCapture* capture = 0;
capture = cvCaptureFromCAM(-1);

cvNamedWindow( "Video", 1 );

if( capture )
{
for(;;)
{
//detect_and_draw(image);
// int t_on = clock(); //inicio medicion tiempo

if( !cvGrabFrame( capture ))


break;
frame = cvRetrieveFrame( capture );
if( !frame )
break;
if( !frame_copy )
frame_copy = cvCreateImage( cvSize(frame->width,frame-
>height),
IPL_DEPTH_8U, frame-
>nChannels );
if( frame->origin == IPL_ORIGIN_TL )
cvCopy( frame, frame_copy, 0 );
else
cvFlip( frame, frame_copy, 0 );

detect_face( frame_copy );

if( cvWaitKey( 10 ) >= 0 )


break;

31
/*
int t_off = clock(); // // fin medicion tiempo
float Temp;

Temp = (static_cast<float>(t_off -
t_on))/CLOCKS_PER_SEC;

Temp = Temp;
*/
}

cvReleaseImage( &frame_copy );
cvReleaseCapture( &capture );
}

/////////////////////////// RUTINA PRINCIPAL "void detect_face(


IplImage* img )"!!!!!!!!!!!!!!
/////////////////////////// (Se ejecuta cada FRAME)
////////////////////7
////////////////////////////////////////////////////////////////////////
///7

void detect_face( IplImage* img )


{
//int t_on = clock(); //inicio medicion tiempo

int scale = 1;
IplImage* temp = cvCreateImage( cvSize(img->width/scale,img-
>height/scale), 8, 3 );
//CvPoint pt1, pt2,ptw,ptp,ptc;
int i;

CvPoint ptempw;
double DistMediciones;

// ptempw.x = ptw.x;
// ptempw.y = ptw.y;

/////////////////

CvFont font;
cvInitFont( &font, 0,.5, .5, 0, 1, 1);
char Strfe[10];
char Strfe2[10];

double ZizeCuadroTemp;

//cvPyrDown( img, temp, CV_GAUSSIAN_5x5 );


cvClearMemStorage( storage );

if( cascade )
{

///// FUNCION QUE EJECUTA EL DETECTOR DE CARAS HAAR

32
CvSeq* faces = cvHaarDetectObjects( img, cascade, storage,
1.2, 3,
CV_HAAR_DO_CANNY_PRUNING,
cvSize(40, 40) );

Bandera1 = 0;

for( i = 0; i < (faces ? faces->total : 0); i++ )


{
Bandera1 = faces->total;

CvRect* r = (CvRect*)cvGetSeqElem( faces, i );


pt1.x = r->x*scale;
pt2.x = (r->x+r->width)*scale;
pt1.y = r->y*scale;
pt2.y = (r->y+r->height)*scale;

//ZizeCuadroTemp = (pt2.y-pt1.y)/2

////////////////// FILTRO DE OUTLIERS

if (i == Bandera1 - 1 )
{
// ContMasDosVentanas = 0;
ptempw.x = pt1.x + (pt2.x-pt1.x)/2;
ptempw.y = pt1.y + (pt2.y-pt1.y)/2;

DistMediciones = sqrt(((ptw.x - ptempw.x)*(ptw.x - ptempw.x)) + ((ptw.y


- ptempw.y)*(ptw.y - ptempw.y)));
ZizeCuadroTemp = (pt2.y-pt1.y);

if(ZizeCuadroTemp < 95) //filtra cuadros muy grandes


{
if (DistMediciones < Cara*1)// medida para determinar si una nueva cara
detectada en una poscion distante de la

// medicion actual es tomada en cuenta


{

ContDistMasEntreCaras = 0;
ptw.x = ptempw.x;
ptw.y = ptempw.y;

BanderaNuevaMedicion++;
//ZizeCuadro =
ZizeCuadroTemp;
}
else
{

ContDistMasEntreCaras++;

if (ContDistMasEntreCaras > 15)//minimo numero de veces seguidas que


debe detectar una cara antes de toamrla en cuenta
{
ptw.x = ptempw.x;
ptw.y = ptempw.y;

33
BanderaNuevaMedicion++;

}
}
}

int kaka = 0;
}
/*
if (faces->total > 1)
{
ContMasDosVentanas++;
}

if ((faces->total > 1)&(ContMasDosVentanas > 5))// parametro para


controlar el numero de frames cossecutivos

// que debe aparecer una segunda "cara" para toamrla en cuenta


{
//ptw.x = pt1.x + (pt2.x-pt1.x)/2;
//ptw.y = pt1.y + (pt2.y-pt1.y)/2;

}
*/
cvCircle( img, ptw, 4, CV_RGB (0, 0, 255 ),NULL, 5, 0 );
//cvRectangle( dest, pt1, pt2, CV_RGB(255,0,0), 3, 8, 0 );
cvRectangle( img, pt1, pt2, CV_RGB(255,0,0), 3, 5, 0 );

} ///////////////// FIN DEL FILTRO DE OUTLIERS

////////////////// ESTIMACION DEL ESTADO DEL SISTEMA


FILTRO DE KALMAN y ENVIO DE ERROR A LA TARJETA

//(BanderaTemp == 1)

//if ((BanderaNuevaMedicion > 0)&(BanderaTemp == 1))


if (BanderaNuevaMedicion > 0)
{
BanderaNuevaMedicion = 0;
NumeroFramesSinDeteccion = 0;

if (BanderaControl == 1)
{
float h[] = {VelX,VelY}; // tomando en cuenta el control
// memcpy( control->data.fl , h, sizeof(h));
BanderaControl = 0;
}
else
{
float h[] = {0,0}; // sin tomar encuenta entradas de control
memcpy( control->data.fl , h, sizeof(h));
//const CvMat* prediction =
cvKalmanPredict( kalman, control);
}
//float h[] = {0,0};

34
const CvMat* prediction = cvKalmanPredict( kalman, control);

/*
ptp.x = prediction->data.fl[0];
ptp.y = prediction->data.fl[1];
if (ptp.x < 1) ptp.x = 1;
if (ptp.x > 320) ptp.x = 320;
if (ptp.y < 1) ptp.y = 1;
if (ptp.y > 240) ptp.x = 240;

ptc.x = 320/2;
ptc.y = 240/2;
Cara = prediction->data.fl[4];
//VelX = prediction->data.fl[2];
//VelY = prediction->data.fl[3];

///////////7
VelX = ptc.x - ptp.x;
VelY = ptc.y - ptp.y;
// VelX = ptc.x - ptw.x;
// VelY = ptc.y - ptw.y;
/////////////////////
cvCircle( img, ptp, 5, CV_RGB (0, 255, 0 ),NULL, 5, 0 );
cvLine( img, ptc, ptp, CV_RGB (255, 255, 0 ),2, 5, 0 );

cvCircle( img, ptc, 5, CV_RGB (255, 0, 0 ),NULL, 5, 0 );


AngX = 0;
AngY = 0;
*/

// if (BanderaNuevaMedicion > 0)
// {

float M[] = {ptw.x,ptw.y,pt2.x-pt1.x};


memcpy( measurement->data.fl , M, sizeof(M));

//for (int r = 0;r< 5;r++)


//{
const CvMat* correccion = cvKalmanCorrect( kalman, measurement );
//}

ptp.x = correccion->data.fl[0];
ptp.y = correccion->data.fl[1];
if (ptp.x < 1) ptp.x = 1;
if (ptp.x > 320) ptp.x = 320;
if (ptp.y < 1) ptp.y = 1;
if (ptp.y > 240) ptp.x = 240;

ptc.x = 320/2;
ptc.y = 240/2;
Cara = correccion->data.fl[4];

VelX = ptc.x - ptp.x;


VelY = ptc.y - ptp.y;

35
AngX = 0;
AngY = 0;

AnchoVentanaM = (320 / Cara)*.21; //estandariza pasa el ancho de ventana

DisPlanoCaraM = AnchoVentanaM/1.5625; // camara con 76º de vision


//DisPlanoCaraM = AnchoVentanaM/1.06; // camara con 56º de vision
/*

VelXM = (VelX/Cara)*.21;
VelYM = (VelY/Cara)*.21;

AngX = (atan(VelXM/DisPlanoCaraM))*180/3.1416;// angulo de correccion en


X grados
AngY = (atan(VelYM/DisPlanoCaraM))*180/3.1416;// angulo de correccion en
Y grados

AngX = AngX /7.5;


AngY = AngY /7.5;
*/
//int ByteX , ByteY;
ByteX = VelX * .79375 + 127;
ByteY = VelY * .79375 + 127;

//AngX = ByteX * .2992125;


//AngY = ByteY * .2992125;

AngX = VelX * .2375;


AngY = VelY * .2375;

alfaX = alfaX + AngX;


tetaY = tetaY + AngY;

if(OpP)// si puerto esta abierto


{

//if ((abs(AngX) > 1)||(abs(AngY) > 1))

if ((BanderaTemp == 1)&((TrackingContinuo == 1)||(SingleTrack == 1)))


{

MandarAngulo(ByteX,ByteY);
BanderaControl = 1;
BanderaTemp = 0;
double DistRecorrido = sqrt((VelX*VelX)+(VelY*VelY));
int TiempoEspera;
/*
inear model Poly2:
f(x) = p1*x^2 + p2*x + p3
Coefficients:
p1 = -0.02667
p2 = 4.667
p3 = 100
*/

36
//TiempoEspera = -0.02667* (DistRecorrido*DistRecorrido) + 4.667*
DistRecorrido + 100;//100;
TiempoEspera = -0.0866* (DistRecorrido*DistRecorrido) + 12.17*
DistRecorrido + 150;
//TiempoEspera = 150;
//TiempoEspera = 2.5 * DistRecorrido + 50;

//t1.SetTimeout(TiempoEspera); // tiempo de espera para mandar las


siguientes lecturas al microcontrolador
//t1.StartTicking();

m_Timer.Start(TiempoEspera, m_MaxResolution);
// t_on = clock(); //inicio medicion tiempo

float ControlX,ControlY;
ControlX = VelX;// * -12; //12
ControlY = VelY;// * 7; //7
//ControlX = ptc.x;
//ControlY = ptc.y;

float h[] = {ControlX,ControlY}; // tomando en cuenta el control


memcpy( control->data.fl , h, sizeof(h));
const CvMat* Prediction = cvKalmanPredict( kalman, control);
// ptp.x = Prediction->data.fl[0];
// ptp.y = Prediction->data.fl[1];
/*
float M[] = {ptc.x,ptc.y,pt2.x-pt1.x};
memcpy( measurement->data.fl , M, sizeof(M));
const CvMat* correccion = cvKalmanCorrect( kalman, measurement );
// ptp.x = correccion->data.fl[0];
// ptp.y = correccion->data.fl[1];
*/
SingleTrack = 0;
}
//}

}
//_itoa( AngX, Strfe, 10 );
//_itoa( AngY, Strfe2, 10 );

int g =2;
}
else
{
NumeroFramesSinDeteccion++;
if ((NumeroFramesSinDeteccion > 50)&(TrackingContinuo == 1) ) // numero
de frames sin deteccion antes de centrar la camara
{
MandarAngulo(254,127);//centrar la camara
NumeroFramesSinDeteccion = 0;
}
}
/////////////////

37
}
/// DIBUJO DE INFORMACION EN LA IMAGEN
_itoa( VelX, Strfe, 10 );
_itoa( VelY, Strfe2, 10 );

CvPoint ptempP;
ptempP.x = ptp.x;
ptempP.y = ptp.y + 40;
CString sal;
//sal = sal + " .";
sal = Strfe;
sal = sal + ",";
sal = sal + Strfe2;
cvPutText( img, sal,ptempP , &font, CV_RGB (255,255,0));

cvCircle( img, ptp, 4, CV_RGB (0, 255, 0 ),NULL, 5, 0 );


cvLine( img, ptc, ptp, CV_RGB (255, 255, 0 ),2, 5, 0 );
cvCircle( img, ptc, 3, CV_RGB (255, 0, 0 ),NULL, 5, 0 );
/*
cvResizeWindow( "Video y Deteccion", 640, 480 );
//cvConvertScale( img, img, 2, 0 );
IplImage* imgTemp = cvCreateImage(cvSize(640,480), 8, 3 );
cvResize(img, imgTemp, CV_INTER_LINEAR );

cvShowImage( "Video y Deteccion", imgTemp );


cvReleaseImage( &imgTemp );
*/
cvShowImage( "Video y Deteccion", img );
cvReleaseImage( &temp );

/*
int t_off = clock(); // // fin medicion tiempo
float Temp;

Temp = (static_cast<float>(t_off - t_on))/CLOCKS_PER_SEC;

Temp = Temp;
*/
}

void FTrack::Update(CMMTimer &Timer)


{

BanderaTemp = 1;
m_Timer.Stop();
int x;
x = m_Timer.GetTotalMilliseconds() ;
int y;
y = 4;
//m_Timer.Reset();
/*
t_off = clock(); // // fin medicion tiempo
float Temp;

Temp = (static_cast<float>(t_off - t_on))/CLOCKS_PER_SEC;

38
Temp = Temp;
*/
}

void FTrack::Tick()
{

BanderaTemp = 1;
// t1.StopTicking();
}

void FTrack::Prueba()
{
BanderaTemp = 1;
SingleTrack = 1;
}

/*
UINT MyThreadProc( LPVOID pParam )
{
for(int i = 0; i == 1;i = i )
{
float x = ptw.x ;

return 0; // thread completed successfully


}
}
*/

void FTrack::IniciarCaptura(int width,int height,int FTPs)


{

cvNamedWindow( "Video y Deteccion", 1 );

cvNamedWindow("Video", CV_WINDOW_AUTOSIZE);

if( ShowCamVideo((HWND)cvGetWindowHandle("Video"), width,


height,FTPs) )
{

int ShowCamVideo(HWND hwnd, int width, int height,int FTPs)


{

VidFormat vidFmt={ width, height, FTPs};


//VidFormat vidFmt={ 640, 480, 5.0};

int ncams = cvcamGetCamerasCount( );

cvcamSetProperty(0, CVCAM_PROP_ENABLE, CVCAMTRUE);

39
cvcamSetProperty(0, CVCAM_PROP_RENDER, CVCAMTRUE);

cvcamSetProperty(0, CVCAM_PROP_SETFORMAT, &vidFmt);

cvcamSetProperty(0, CVCAM_PROP_WINDOW, &hwnd);

cvcamSetProperty(0, CVCAM_PROP_CALLBACK,callback);

//cvDestroyWindow("Video");

//Set Video Format Property

//cvcamGetProperty(0, CVCAM_VIDEOFORMAT,NULL);

//cvcamInit();

if( !cvcamInit() )
return 0;

cvcamStart();
return 1;
}
void MandarAngulo(float alfa,float teta)
{
int Servo1;
int Servo2;

Servo1 = alfa;
Servo2 = teta;

//Servo1 = 1.144*alfa+16;
//Servo2 = 1.13*teta-6;

//if((abs(Servo1 - Servo1T) > 2) | (abs(Servo2 - Servo2T) > 2) )


//{
port.WriteByte(255);
port.WriteByte(Servo1);
port.WriteByte(Servo2) ;
Servo1T = Servo1;
Servo2T = Servo2;
//}

BYTE data=0;
port.ReadByte(data);

//CWnd* pWnd = GetDlgItem(IDC_EDIT3);


//pWnd->SetWindowText("hola");

int g =2;

40
5.2 Código de los Microcontroladores ATMEL

El siguiente código corresponde respectivamente al de los Microcontroladores


ATMEGA8535 y ATMEGA2323 de ATMEL. Como se menciono anteriormente en
el documento, este código es para el compilador BASCOM [12].

ATMEGA8535
'$regfile = "8535def.dat"

$crystal = 4000000
$baud = 9600

Enable Interrupts

Ddrd = &B11111110
Ddrc = &B11101110

Reset Portc.3
'Config Portc = Input

Config Portb = Output


'Config Portb = Output
'Config Portc = Output

Config Serialin = Buffered , Size = 3

'On Urxc Recibir

Dim Pila As String * 6

Dim V As Byte
Dim Vsal As Byte
Dim Count As Byte
Dim Dato1 As Byte
Dim Dato2 As Byte

Dato1 = 127
Dato2 = 127

Print Chr(255) ; Chr(255) ; Chr(255) ;

Config Portb = Output

Portb = 127
Vsal = 127

Count = 1

Do

41
If Ischarwaiting() = 1 Then 'Detecta si
hay algun dato esperando en el UART

V = Inkey()

' Print V

If Count = 3 Then
'00 servo1

Dato2 = V

Gosub Mandardatos

Count = 1
End If
If Count = 2 Then

Dato1 = V

Count = 3
End If

If V = 255 Then
Count = 2
End If

End If

Loop

Mandardatos:
' MANDAR LOS DATOS AL 2313

Vsal.7 = Dato2.0 '00 servo1


Vsal.6 = Dato2.1
Vsal.5 = Dato2.2
Vsal.4 = Dato2.3
Vsal.3 = Dato2.4
Vsal.2 = Dato2.5
Vsal.1 = Dato2.6
Vsal.0 = Dato2.7
Portb = Vsal

Bitwait Pinc.4 , Set

Set Portc.3

Waitms 1
Reset Portc.3

Vsal.7 = Dato1.0
Vsal.6 = Dato1.1
Vsal.5 = Dato1.2
Vsal.4 = Dato1.3
Vsal.3 = Dato1.4
Vsal.2 = Dato1.5
Vsal.1 = Dato1.6
Vsal.0 = Dato1.7

42
Portb = Vsal

Waitms 1

Return

End 'end program

ATMEGA2323

$crystal = 4000000

Ddrd = &B11111011

Config Int0 = Rising


Enable Interrupts

Enable Int0

Config Portb = Input

Ddrd = &B10011011

On Int0 Recibir
'Nosave

Dim Serv1 As Byte


Dim Serv2 As Byte
Dim Serv3 As Byte
Dim Serv4 As Byte
Dim Serv1tt As Byte
Dim Serv2tt As Byte

Dim I As Byte
Dim Ent As Byte

Dim S0 As Bit
Dim S1 As Bit

Dim Para1 As Bit


Dim Para2 As Bit

Dim Tm1 As Byte


Dim Tm2 As Byte
Dim Tm3 As Byte
Dim Tm4 As Byte

Serv1 = 127
Serv2 = 127
Serv1tt = 118

43
Serv2tt = 118

Serv3 = 50
Serv4 = 200
Tm1 = 255 - 118
Tm2 = 255 - 118
'Tm3 = 255 - Serv3
'Tm4 = 255 - Serv4

Dim Bytecorreccion1 As Single


Dim Bytecorreccion2 As Single

Para1 = 1
Para2 = 1

Dim F1 As Byte
F1 = 0

Dim Bx As Single
Dim By As Single
Dim Bcx As Byte
Dim Bcy As Byte

Do

If Serv1 > 127 Then 'TETA


Bx = Serv1
Bytecorreccion1 = Bx * .3424
Bytecorreccion1 = Bytecorreccion1 - 43.48
Bcx = Bytecorreccion1
Serv1tt = Serv1tt + Bcx
If Serv1tt > 150 Then
Serv1tt = 150
End If
Serv1 = 127
Tm1 = 255 - Serv1tt
End If

If Serv1 < 127 Then


Bx = Serv1

Bytecorreccion1 = Bx * .3424

Bytecorreccion1 = 43.48 - Bytecorreccion1

Bcx = Bytecorreccion1
Serv1tt = Serv1tt - Bcx
If Serv1tt < 38 Then
Serv1tt = 38
End If
Serv1 = 127
Tm1 = 255 - Serv1tt

End If

If Serv2 > 127 Then 'ALFA

44
By = Serv2
Bytecorreccion2 = By * .3424
Bytecorreccion2 = Bytecorreccion2 - 43.48
Bcy = Bytecorreccion2
Serv2tt = Serv2tt + Bcy
If Serv2tt > 225 Then
Serv2tt = 225
End If
Serv2 = 127
Tm2 = 255 - Serv2tt
End If

If Serv2 < 127 Then


By = Serv2

Bytecorreccion2 = By * .3424

Bytecorreccion2 = 43.48 - Bytecorreccion2

Bcy = Bytecorreccion2
Serv2tt = Serv2tt - Bcy
If Serv2tt < 22 Then
Serv2tt = 22
End If
Serv2 = 127
Tm2 = 255 - Serv2tt

End If
'SERVO 1
'If Para1 = 0 Then
Set Portd.0
Waitus 350
For I = 0 To Serv1tt
Waitus 4
Next
Reset Portd.0
'End If

For I = 0 To Tm1
Waitus 4
Next

'servo 2
'If Para2 = 0 Then
Set Portd.1
Waitus 350
For I = 0 To Serv2tt
Waitus 4
Next
Reset Portd.1
'End If

For I = 0 To Tm2
Waitus 4
Next
Waitms 12

Loop

45
6 Referencias
[1] Hutchinson, S.,HAger, G. & Corke “A tutorial on visual servo control” IEEE transactions on
Robotics and Automation 1996.
[2] Ezio Mails “Survey of vision-based robot control” 2001
[3] Logitech, www.logitech.com
[4] Paul Viola and Michael J. Jones. “Rapid Object Detection using a Boosted Cascade of Simple
Features”. IEEE CVPR, 2001.
[5] Greg Welch, Gary Bishop. “An Introduction To the Kalman Filter.” Technical Report TR95-041,
University of North Carolina at Chapel Hill, 1995 Online version is available at
http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
[6] Live! Ultra for Notebooks, www.creative.com
[7] Servo Futaba estándar S3003 www.futaba.com
[8] Open Computer Vision Library “OpenCV” http://sourceforge. net /projects/opencv/
[9] Rodrigo Munguía, Antoni Grau “Implementación de un robot móvil con control en PC y
odometría mediante un ratón óptico PS2” Technical Report ESAII-RT 06-01, Universidad
Politecnica de Cataluña, 2006.
[10] www.atmel.com/avr
[11] EAGLE Layout Editor, http://www.cadsoft.de/
[12] BASCOM http://www.mcselec.com/

46