Futebol de Robos F180 » Inteligência

Eu e o Antonio vamos trabalhar nesta tarefa. Vamos antes terminar a tarefa de refazer a visão orientada a objeto para depois atacar aqui.

Previsão de começar essa na metade da semana que vem.

Falando agora da matriz de covariância do erro de estado P.

Diferentemente das matrizes R e Q, que são fixas durante todas as iterações do código, para a matriz P nós fazemos apenas a estimativa para a primeira iteração, P_0|0. As próprias equações do Kalman fazem estimativas para a matriz P (a priori na predição e a posteriori na atualização). Ou seja, precisamos estimar a matriz de covariância do erro do estado inicial apenas.

Para começar, percebemos que o estado do robô na primeira iteração, x_0|0, está com tudo 0 no código. Ou seja, para qualquer robô, estamos dizendo que ele começa no ponto (0,0) com orientação 0 e com velocidades nulas.

Vamos mudar isso e colocar x_0|0 com a posição vinda da câmera e com velocidades nulas. Esse estado inicial tem erros na posição apenas, porque o robô realmente está parado assim que a gente começa a rodar o código. A matriz P_0|0 que vamos estimar é a matriz de covariância do erro desse estado inicial e, como foi dito, tem erro apenas de medição. Então o que pensamos foi pegar o vetor 2D de erro de medições calculado para matriz R e adicionar em cada elemento do vetor valores 0 para os erros das 3 velocidades (v_tang, v_norm e v_ang), já que não há erro de velocidade no estado inicial e usar esse vetor 2D resultante para calcular a matriz de covariâcia P_0|0.

Por fim, a matriz Q é a matriz de covariância do ruído do processo. Então teoricamente temos que, a cada instante e com o robô andando, fazer o erro do estado gerado pelo Kalman (já com as matrizes R e P devidamente estimadas) com o estado real. Mas, mais uma vez não temos o estado real, pois não temos como saber a posição real do robô nem suas velocidades.

A ideia que o major Renault deu foi começar o Kalman com uma matriz Q carteada (ou nula talvez), fazer a predição e anotar o estado gerado; fazer a atualização e calcular o erro entre os estados gerados pelas duas fases. Calcular a matriz de covariância proveniente desse erro. Fazer isso por várias iterações até que a matriz Q comece a convergir (não sabemos se ela vai convergir, é uma esperança).

Um detalhe é que nosso Kalman não tem a componente opicional B_k.u_K das velocidades de controle. Então na predição, não mudamos velocidades, apenas posição. Acredito que isso deva interferir na estimativa da matriz Q.

Tiramos essa componente B_k.u_K da equação porque achamos que mais estava atrapalhando do que ajudando, já que não é, atualmente, nem um pouco confiável que o robô no campo execute as velocidades que estamos mandando.

Uma ideia que tivemos semestre passado e até fez o Kalman ficar razoavelmente bom no grsim foi pegar as posições vindas da câmera e derivá-las com o bloco de derivada do labview, que utiliza o método de tustin, e usar as velocidades geradas no vetor u_k.

Mas, por enquanto, vamos trabalhar sem essa componente.

Outro ponto importante que atrapalha o bom funcionamento do Kalman é estrutural.

Um desses problemas foi o gerado pela mudança e criação do while Read UDP (problema já descrito e em implementação na tarefa http://redmine.roboime.com.br/issues/1223).

Outro problema é que o major Renault disse que seria melhor que o Kalman trabalhasse a taxa constante (o dt ou intervalo que entra na matriz F). Ainda precisamos pensar sobre isso.

Outro problema, também relacionado ao dt ou intervalo, é que o timestamp dos frames está como instante em que o pacote chega ao nosso código (na verdade é o instante em que começamos a decodificar o pacote). Isso foi feito durante a larc 2018 pois, pelo o que entendemos, os computadores da visão estavam com horários um pouco defasados, então tinha informação que chegava depois com timestamp anterior e isso causava problemas no controle. Lembrando que o pacote vem com timestamp. E o ideal seria que o dt utilizado no Kalman fosse o intervalo entre as medições. Também precisamos pensar sobre isso.

O foco do nosso trabalho será começar com a estimação das matriz R e P, pois nelas temos ideias que, aparentemente, parecem que vão funcionar bem. E para isso, pensamos em começar fazendo uma VI separada do código principal que pegasse certo número de medições e fizesse as estimativas de R e P. Pra testar, vamos copiar e colar as matrizes no código.

Depois que fizermos elas, vamos partir para a estimação da matriz Q.

Lembrando que o grsim possui uma opção para ativar ruído gaussiano, além de opção de desaparecimento de robôs e bola. Isso vai ser importante para ir testando o resultado do nosso trabalho. Mas, claro que vamos sempre focar em testar no PIRF depois de terminada cada etapa.

Tudo que descrevi até agora foi falado para robôs. Mas para a bola vai ser a mesma coisa.

Estou montando uma VI que fica no while da visão para estimar as matrizes de covariância R e P da maneira que especifiquei nos comentários acima.

A VI que eu criei é um método de Vision e é uma máquina de estados. Tem que selecionar o id da câmera para a qual vai estimar as matrizes (vai ser uma par de matrizes R e P pra cada câmera), e clicar num botão de ok. Há um enum dizendo onde deve-se posicionar o robô. Ao terminar naquele ponto, o enum vai dizer onde posicionar novamente, depois que posicionar o robô, aperta ok de novo e assim vai até terminar os 5 pontos daquela câmera.

Eu fiz uma versão para o grsim (apenas fazendo a média como sendo o ponto real) e estou fazendo uma versão para o campo, tendo 5 pontos em cada câmera como já dito na tarefa. Essa versão do campo que estou fazendo contempla 4 câmeras como no PIRF e na LARC; por enquanto, fiz o código para uma dessas câmeras, falta copiar para as outras 3 e trocar alguns detalhes para cada câmera. Depois vou fazer uma versão para a RoboCup, que tem apenas uma câmera cobrindo o campo todo.

Assim que terminar a implementação da VI, vamos testar no PIRF e anotar os resultados.

Bom trabalho, concertou o erro? oq deu?

O erro que eu comentei aí na verdade é um erro de imprecisão de posicionamento. Colocando o robô não exatamente em cima da posição analisada, a estimação não fica boa.

Falta testar no PIRF e analisar como fica a estimação.

Antes disso, vou modificar um pouco o código para permitir fazer apenas uma média como sendo a posição real em cada um dos 5 pontos da câmera ao invés de utilizar o ponto real como sendo a coordenada exata de centro do gol, meio do campo e etc, por causa do erro de posicionamento que podemos cometer como citado acima.

Gabriel Borges da Conceição escreveu:

O erro que eu comentei aí na verdade é um erro de imprecisão de posicionamento. Colocando o robô não exatamente em cima da posição analisada, a estimação não fica boa.

Entendi

Antes disso, vou modificar um pouco o código para permitir fazer apenas uma média como sendo a posição real em cada um dos 5 pontos da câmera ao invés de utilizar o ponto real como sendo a coordenada exata de centro do gol, meio do campo e etc, por causa do erro de posicionamento que podemos cometer como citado acima.

Acho boa essa ideia.