dist:=distance(pos); ou seja, o somatório dos erros mínimos, que advém do cálculo da distância mínima, uma vez, que o erro é função da distância.

Transcrição

1 Implementação das funções do algoritmo De seguida são demonstradas as derivadas de primeira e de segunda ordem, paras as variáveis necessárias, notar que apenas são demonstradas, não é explicado aqui para que são usadas, isso será descrito em outra secção. Primeiro calcula: pos.x:=xy.x + cos(phi)*vis.pointlist[vis_iterator].x-sin(phi)*vis.pointlist[vis_iterator].y; pos.y:=xy.y + sin(phi)*vis.pointlist[vis_iterator].x+cos(phi)*vis.pointlist[vis_iterator].y; onde: xy define o ponto p, ou seja, a posição do robot nas coordenadas globais; vis.pointlist[vis_iterator] define o ponto si, ou seja, o ponto em estudo, vis.pointlist é um vector que contém todos os pontos observados no pré-processamento da imagem; Depois calcula a distância, a função d(.) em: Ou seja: dist:=distance(pos); Isto é, vai ler a um array ( d ), onde estão as distâncias previamente calculadas, qual o valor da distância mínima à linha mais próxima, mais á frente é explicado como este array é preenchido (abaixo) De seguida calcula o somatório do erro dado por: ou seja, o somatório dos erros mínimos, que advém do cálculo da distância mínima, uma vez, que o erro é função da distância. err:=err + (1-c*c)/(c*c+dist*dist); Como se pode ver pelo código, o erro é aproximado por uma função: Devido ao ruído e à imperfeição que advém do pré-processamento da imagem, somos confrontados com uma quantidade substancial de pontos da linha mal detectados, o que influência e distorce a estimativa. Por esse motivo é usado uma aproximação da função de erro que limita a influência dos pontos da linha que têm demasiado erro: em que o parâmetro c é aproximadamente 250. De seguida calcula-se as primeiras e segundas derivadas:

2 Usando a função de aproximação do erro: Para calcular a segundas derivadas usa-se a seguinte aproximação, sendo ignoradas as observações periféricas. A primeira derivada do erro em ordem à distância fica: derr:=dist/(c*c); A segunda derivada do erro em ordem à distância fica: dderr:=1/(c*c); A primeira derivada da distância em ordem à posição do ponto, si, fica: ddist:=gradient(pos); Em que o gradiente (derivada da distância em ordem à posição) é calculado previamente, tal como a distância mínima, e guardado em um array ( dgradient ) que em cada posição deste contém o gradiente/derivada para a respectiva posição, mais à frente é explicado como os valores deste array são calculados (abaixo). A segunda derivada da distância em ordem à posição é nula, como já explicado anteriormente: Do ponto de vista prático o cálculo da derivada de segunda ordem simplifica a função d que é construída a partir das linhas do campo. A maioria destas linhas são paralelas ao eixo de coordenadas, só os cantos e o centro circular é que não são linhas rectas. Assim a função d é seccionamento linear na maior parte do campo e a sua derivada de segunda ordem é zero nessas zonas. A primeira derivada da posição em ordem ao ângulo, phi, ou seja a derivada desta equação em ordem a phi( : Fica: dposdphi.x:=-sin(phi)*vis.pointlist[vis_iterator].x-cos(phi)*vis.pointlist[vis_iterator].y; dposdphi.y:= cos(phi)*vis.pointlist[vis_iterator].x-sin(phi)*vis.pointlist[vis_iterator].y;

3 Ou seja: A segunda derivada A primeira derivada da posição em ordem ao ângulo, phi, ou seja a derivada desta equação em ordem a phi( : Fica: ddposdphi2.x:=-cos(phi)*vis.pointlist[vis_iterator].x+sin(phi)*vis.pointlist[vis_iterator].y; ddposdphi2.y:=-sin(phi)*vis.pointlist[vis_iterator].x-cos(phi)*vis.pointlist[vis_iterator].y; Ou seja: A primeira derivada do erro em ordem à posição é dada por: A segunda derivada do erro em ordem à posição fica: Onde: Logo: hxy.x:=hxy.x + dderr*ddist.x*ddist.x; hxy.y:=hxy.y + dderr*ddist.y*ddist.y;

4 A primeira derivada do erro em ordem ao ângulo (phi) é dada por: A segunda derivada do erro em ordem ao ângulo (phi) é dada por: onde: Então fica: hphi:= hphi + dderr*(ddist.x*dposdphi.x+ddist.y*dposdphi.y)*(ddist.x*dposdphi.x+ddist.y*dposdphi.y) + derr*(ddist.x*ddposdphi2.x+ddist.y*ddposdphi2.y);

5 Em termos de código geral fica: //tem como variáveis de saida a segunda derivada do erro, hxy e hphi, para além do erro function SecondDerivate(out hxy: Vec;out hphi:double; xy:vec; h:double; vis:linepointlist; max_points: longword): double; var phi:double; err, derr, dderr, dist:double; n_points: double; ddist:vec; pos, dposdphi, ddposdphi2: Vec; vis_iterator: longword; //inicialização de variáveis phi:=h; err:=0; hxy.x:=0; hxy.y:=0; hphi:=0; //verifica se a lista de pontos, calculados no pré-processamento, é maior que o máximo de pontos permitido if (round(max_points)> Length(vis.PointList)) then n_points:=length(vis.pointlist); end else n_points:=max_points; //cria um iterador a apontar para o inicio da lista de pontos vis_iterator:=low(vis.pointlist); //para todos os pontos possiveis de tartar, efectua os seguintes cálculos while (vis_iterator<n_points) do //posição, distância e erro pos.x:=xy.x + cos(phi)*vis.pointlist[vis_iterator].x-sin(phi)*vis.pointlist[vis_iterator].y; pos.y:=xy.y + sin(phi)*vis.pointlist[vis_iterator].x+cos(phi)*vis.pointlist[vis_iterator].y; dist:=distance(pos); err:=err + (1-c*c)/(c*c+dist*dist); //se o ponto tiver uma distância demasiado grande (2*c) descarta-se if (dist<2*c) then derr:=dist/(c*c); dderr:=1/(c*c); ddist:=gradient(pos); dposdphi.x:=-sin(phi)*vis.pointlist[vis_iterator].x-cos(phi)*vis.pointlist[vis_iterator].y; dposdphi.y:= cos(phi)*vis.pointlist[vis_iterator].x-sin(phi)*vis.pointlist[vis_iterator].y; ddposdphi2.x:=-cos(phi)*vis.pointlist[vis_iterator].x+sin(phi)*vis.pointlist[vis_iterator].y; ddposdphi2.y:=-sin(phi)*vis.pointlist[vis_iterator].x-cos(phi)*vis.pointlist[vis_iterator].y; hxy.x:=hxy.x + dderr*ddist.x*ddist.x; hxy.y:=hxy.y + dderr*ddist.y*ddist.y; hphi:= hphi + dderr * (ddist.x*dposdphi.x+ddist.y*dposdphi.y) * (ddist.x*dposdphi.x+ddist.y*dposdphi.y) + derr*(ddist.x*ddposdphi2.x+ddist.y*ddposdphi2.y); vis_iterator:= vis_iterator +1; result:=err;

6 Como é preenchido o array de distâncias d O array d é um array com as dimensões do campo de jogos (incluindo a zona de segurança, ou seja, a zona que fica fora das linhas de campo onde o robô pode circular) em células, ou seja, divide-se o campo em pequenas células, 40mm por exemplo, de forma a ter-se pontos de referência, precisamente calculados. Fazendo a analogia a uma imagem, as células, são os pixéis de uma imagem, em que o tamanho de cada célula define a resolução do campo. Inicialmente cada célula do array d é preenchida com valores elevados (distâncias elevadas): max_val:=1e100; //inicialização do vector de distâncias "d" com um valor elevado for yi := 0 to dw-1 do for xi := 0 to dh-1 do d[yi,xi]:=max_val; Depois é executado uma série de passos com o intuito de criar o mapa do campo, de acordo com as medidas definidas para o campo, medidas essas definidas em metros e de acordo com as regras segundo o qual o campo foi construído. Nesses passos são desenhadas virtualmente linhas, arcos e pontos, em uma imagem (do campo) com as dimensões iguais ás do array d. Para cada linha, arco ou ponto criado são calculadas as distâncias destes a todas as células que compõem o campo. Distâncias essas que são guardadas na respectiva posição do array d. Pretende-se que o valor da distância em cada posição seja mínimo, logo, para cada distância em relação a uma posição calculada, é verificado se o novo valor calculado é menor que o existente no array, se o novo valor for menor o existente é substituído por este, caso contrário mantêm-se o existente. //actualização do array com as distâncias mínimas //esta é a única função que altera o valor do array de distâncias //a alteração só é efectuada se o novo valor for menor que o existente procedure update_d(xi, yi:longword; v: double); //se o ponto se encontra dento do campo, actualiza valor do vector if ((xi<=dh-1) and (yi<=dw-1)) then if(v<d[yi,xi]) then d[yi,xi]:=v;

7 Como é preenchido o array de distâncias dgradient O array dgradient tem dimensões idênticas ao array d, uma vez que é necessário saber a informação para todas as células do campo. Este array armazena a derivada do erro em ordem à posição, visto que como a distância ( d ) é diferenciável em praticamente todo o campo, é possível calcular o seu gradiente também em praticamente todo o lado e interpolar os pontos não diferenciáveis. Assim para calcular o gradiente é aplicado o filtro de Sobel em todos os pontos tal como explicado de seguida. O filtro Sobel é aplicado sobretudo em algoritmos de detecção de contornos. Em termos técnicos, consiste num operador que calcula diferenças finitas, dando uma aproximação do gradiente da intensidade das distâncias no campo. Em cada ponto do campo, o resultado da aplicação do filtro Sobel devolve o gradiente ou a norma deste vector. Como funciona: O filtro Sobel calcula o gradiente da intensidade da matriz em cada ponto, dando a direcção da maior variação de distâncias altas para as baixas e a quantidade de variação nessa direcção. Assim, obtêm-se uma noção de como varia a distância em cada ponto, de forma mais suave ou abrupta. Com isto consegue-se estimar a presença de uma transição brusca de distâncias e de qual a orientação desta. Matematicamente falando: Este operador utiliza duas matrizes 3 3 que são convoluídas com a matriz original para calcular aproximações das derivadas, uma para as variações horizontais e uma para as verticais. dgradient.x e dgradient.y serão dois arrays que em cada ponto contêm uma aproximação às derivadas horizontal e vertical de matriz de distâncias. Assim dgradient.x: é igual á média pesada seguindo de acordo com a matriz dgradient.y: é igual á média pesada seguindo de acordo com a matriz Então considerando a seguinte matriz: Nota: como não é possível diferenciar nas extremidades do campo, devido ás limitações do filtro de Sobel, esses pontos são interpolados dos adjacentes.

8 Temos o seguinte código //Percorre apenas a parte interior do campo, pois devido à forma como é aplicado o filtro este não pode efectuar os cálculos nas extremidades for yi:= 1 to dw-2 do for xi := 1 to dh-2 do //cálculos preliminares dos pontos da matriz 3x3 que envolve o ponto lo:=d[yi-1,xi+1]; lm:=d[yi-1,xi]; lu:=d[yi-1,xi-1]; ro:=d[yi+1,xi+1]; rm:=d[yi+1,xi]; ru:=d[yi+1,xi-1]; mo:=d[yi,xi+1]; mu:=d[yi,xi-1]; //cálculo do gradiente de x e de y dentro do campo usando o filtro de sobel dgradient[yi,xi].x:=0.125*(-lo+ro-2*lm+2*rm-lu+ru)/cell_size; dgradient[yi,xi].y:=0.125*(lo-lu+2*mo-2*mu+ro-ru)/cell_size; //como nas linhas laterais não é possível aplicar o filtro de sobel (por não existir uma matriz 3x3 em torno do ponto) expande-se (copia-se) o valor adjacente para o ponto //cálculo do gradiente para as linhas laterais for yi := 1 to dw-2 do dgradient[yi,0].x:=dgradient[yi,1].x; dgradient[yi,0].y:=dgradient[yi,1].y; dgradient[yi,dw-1].x:=dgradient[yi,dw-2].x; dgradient[yi,dw-1].y:=dgradient[yi,dw-2].y; //cálculo do gradiente para as linhas de fundo for xi := 1 to dh-2 do dgradient[0,xi].x:=dgradient[1,xi].x; dgradient[0,xi].y:=dgradient[1,xi].y; dgradient[dh-1,xi].x:=dgradient[dh-2,xi].x; dgradient[dh-1,xi].y:=dgradient[dh-2,xi].y; //para os cantos efectua-se uma média pesada dos valores adjacentes ao ponto //cálculo do gradiente para os cantos dgradient[0,0].x:=0.5*(dgradient[0,1].x+dgradient[1,0].x); dgradient[0,0].y:=0.5*(dgradient[0,1].y+dgradient[1,0].y); dgradient[dh-1,0].x:=0.5*(dgradient[dh-1,1].x+dgradient[dh-2,0].x); dgradient[dh-1,0].y:=0.5*(dgradient[dh-1,1].y+dgradient[dh-2,0].y); dgradient[0,dw-1].x:=0.5*(dgradient[0,dw-2].x+dgradient[1,dw-2].x); dgradient[0,dw-1].y:=0.5*(dgradient[0,dw-2].y+dgradient[1,dw-1].y); dgradient[dh-1,dw-1].x:=0.5*(dgradient[dh-1,dw-2].x+dgradient[dh-2,dw-1].x); dgradient[dh-1,dw-1].y:=0.5*(dgradient[dh-1,dw-2].y+dgradient[dh-2,dw-1].y);