SSL » INTEL

# Planejamento de Trajetória

Nos utilizamos do algoritmo RRT (Rapidly-Exploring Random Tree) para calcular nossa trajetória. Ele busca uma trajetória a partir de um estado inicial até o objetivo expandindo uma árvore de busca. 

Para isso necessitamos de 3 funções básicas:

* Extend(env:enviroment,current:state,target:state,Step:real):state

Essa função dado um estado atual extende dando um passo em direção ao objetivo, ou em uma direção aleatória. Caso uma colisão ocorra ela retorna um estado vazio (EmptyState).

Pseudo código:

```c

r = (Target - Current)/||Targe-Current|| ; //vetor unitário na direção do target

NewState = r*Step + Current ; 

if (Colision(NewState)) NewState = EmptyState; //confere se o novo estado vai colidir com algum obstáculo.

return NewState;

```

* Distance(current:state,target:state):real

Calcula a distância do estado atual ao objetivo, utilizamos a euclidiana nessa implementação.

* RandomState():state

Retorna um novo estado uniformemente distribuído no ambiente.

```c

float random_x;

float random_y;

random_x = fiel_length*(rand()-0.5); //Random gera um número aleatório uniformemente aleatório distribuído entre 0 e 1

random_y = fiel_length*(rand()-0.5); 

```

Basicamente a função estender pode utilizar como target o objetivo ou um ponto aleátório, o que garante que o algoritmo pode encontrar um caminho sem colidir com nenhum obstáculo. Como na figura abaixo:

![](rrt.PNG)

A implementação seguiu o pseudo código abaixo:

![](pseudocódigo.PNG)

***A função Nearest garante que sempre extendemos a árvore do nó mais próximo do objetivo, porém ela também demanda muito processamento, já que tem q varrer a árvore toda a cada iteração. Vamos estudar um jeito para resolver isso.***

Definimos uma árvore de pontos como uma matriz de 4 colunas, onde a primeira é o id do ponto, a segunda a coordenada x, a terceira a coordenada y e a quarta o id do pai. Dessa forma os nós são adicionados com o id sendo incrementado de 1 a cada nó. O id do pai é o id do nó da onde aquale novo ramo foi gerado. Assim quando encontramos um solução podemos fazer o backtracking nessa árvore para reconstruir a trajetória.

O próximo passo foi implentar a extenção do RRT (ERRT), com memória da última trajetória. Assim na hora de escolhermos um target podemos ir em direção ao objetivo, em uma direção aleatória ou em direção a algum ponto da última trajetória, como na imagem e código abaixo.

![](errt.PNG)

![](choose.PNG)

Por último implementamos a suavização da trajetória que segue o algoritmo abaixo descrito. Ela auxilia muito na rapidez com que o robô será capaz de seguir a trajetóra.

![](suavização.png)

## Raio dos Obstáculos

### Robôs Inimigos

Para os robôs inimigos agora é considerada a influência da velocidade relativa. Assim o raio do obstáculo é dado por:

```c

radius_enemy + radius_our + bound(0, |speed_enemy-speed_our|*100, 100)

```

E caso o goal esteja dentro de um obstáculo esse raio é reduzido o suficiente para que um trajetória válida possa ser gerada. O mesmo acontece quando o ponto inicial da trajetória está dentro de um obstáculo, o raio é reduzido afim de ser possível a geração da trajetória.

## Implementações futuras e problemas atuais

* Tornar os nossos robôs obstáculos, porém com um raio menor.

* Tornar as áreas dos goleiros e a parte de trás do gol em obstáculos

* Colocar a bola como obstáculo na situação de stop

* Tornar o RRT otimizado para diferentes personalidades

# Otimização feita (RRT apenas nos obstáculos)

Conversando com outras equipes na RoboCup 2018, nos deram uma dica de como poderíamos otimizar nosso planejamento de trajetória. A ideia consiste em traçar uma linha reta entre o ponto atual e o final (objetivo) do robô, identificar possíveis obstáculos que cruzam essa reta e aplicar o RRT apenas na região em que ocorrem esses conflitos, mantendo a linha reta onde não houver problema. Isso é essencial para diminuir o custo de processamento e reduzir os cálculos para gerar nosso path, já que o algoritmo é aplicado numa região muito menor. 

Pesquisamos o TDP de outras equipes para procurar alguma referência dessa implementação, mas não foi encontrada essa ideia de fazer o RRT apenas nos obstáculos escrita em nenhum deles. Todos os TDPs se baseavam naquele mesmo artigo (autores James Bruce e Manuela Veloso) por onde estudamos o RRT, e não foi encontrado nada diferente daquilo.

A respeito da implementação:

* Criei a VI FindColisionPoints, para encontrar a posição dos obstáculos que cruzam a trajetória reta mais simples entre os pontos inicial e final (Simple Path). Ela retorna um vetor com todas as posições de obstáculos nesse caminho.

* Criei a VI FindStartEndPointsRRT, que serve para encontrar a posição do obstáculo mais distante e do mais próximo do ponto inicial, dentre os que estao no vetor dos pontos de colisão. Em seguida aplica-se um range (por enquanto escolhi o tamanho 200mm pra cada lado) em volta dessas duas extremidades de obstaculos. Essa área interna ao range aplicado é a que será percorrida usando o RRT, e as de fora por uma linha reta. Verifica-se após aplicar o range se o novo ponto cai fora do simple path, o que significa que está muito perto do ponto inicial (ou final) da trajetória, e nesse caso trocamos o ponto gerado que deu problema pelo ponto inicial ou final correspondente. Ou seja, se os pontos encontrados para iniciar ou acabar o RRT forem muito próximos da extremidade (usei um valor de comparação <200mm por enquanto), ou se caírem fora daquele segmento, entao ignora-se a linha reta nesse pequeno caminho e aplica-se o RRT diretamente desde o ponto inicial ou até o ponto destino diretamente.

* Adaptei a VI RRTPlan para essas modificações. Primeiro, modularizei um grande pedaço onde se fazia o algoritmo do RRT de fato, criando uma subVI chamada RRTAlgorithm, para facilitar a visualização da escolha dos pontos inicial e final de onde aplicaríamos o RRT. Em seguida usei as duas VIs anteriores para gerar os pontos de obstáculos e encontrar os pontos iniciais e finais convenientes no algoritmo, gerando o novo path.

Durante a implementação, foi perguntado como ficaria o algoritmo quando existem vários obstáculos em sequência ou um outro obstáculo no end point. 

Resposta: primeiramente eu fiz da forma mais simples, fazendo o RRT em toda a extensão dos obstáculos, desde o primeiro (mais próximo do ponto inicial) até o último (mais perto do ponto final). Optei por essa implementação mais simples a princípio pra verificar se daria certo e observar a trajetória gerada. Depois de debugar e testar dessa maneira, finalmente consegui obter a trajetória desejada pensada dessa forma, e agora posso partir pra implementação mais otimizada ainda: de fazer o rrt em cada obstáculo separadamente (basta fazer um loop no vetor dos obstáculos que eu gerei e tratar os casos de os obstáculos estarem muito próximos e serem aglutinados em apenas 1).

Seguem agora imagens de como ficou a trajetória gerada nessa implementação mais simples. Vejam que com dois ou mais robôs a trajetória ainda não é a ótima, pois ele calcula o rrt em todo o espaço entre os obstáculos também, e isso é ruim no caso deles estarem muito longes um do outro.

![](rrt1.png)

![](rrt2.png)	

Em seguida, foi finalizada a implementação mais otimizada, com o RRT aplicado apenas nos obstáculos e em cada um por vez. Obtive as seguintes trajetórias:

![](novorrt1.png)

![](novorrt3.png)

Veja que, quando dois obstáculos estão muito próximos um do outro, de forma que não é possível executar a trajetória reta entre eles, estes são algutinados em 1 só, e é aplicado o RRT sobre aquela extensão:

![](novorrt2.png)

Iniciou-se a partir daí os testes para medir o tempo que demora para a execução de todo o cálculo e comparar com a implementação anterior para saber se está mais rápido ou não. Foi decidido como seriam os testes: a ideia é que faremos diversos zig zags com um robô e vou medir o tempo máximo de execução da VI RRTPlan pra cada teste desse. Em seguida vou replicar os mesmos zig zags para a outra implementação do RRT e vamos comparar os tempos. As variações entre os zig zags vão ser apenas nas posições e no número dos obstáculos que vão atrapalhar o robô nessa tarefa. Vou deixar cada zig zag rodando por um tempo determinado (Ex: 1 min) e anotar o tempo máximo de execução obtido nesse período.

Fiz os testes do RRT novo e antigo, para comparar os tempos de execução da VI RRT Plan, que contém todo o cálculo da trajetória e também a parte de suavização. Fiz os testes nas mesmas condições (apenas um robô ligado e posicionamento preciso no grsim com o set position), e obtive os seguintes resultados de tempo e fps durante o tempo de observação (1 min para cada zig zag feito):

1º) Zig Zag entre os pontos (0,2000) e (0,-2000) SEM obstáculos.

| RRT antigo |   RRT novo   | 

|---         |      ---     |

|t=0,0225756s|t=0,000511674s| 

|fps=45      |fps=57        | 

2º) Zig Zag entre os pontos (-4000,0) e (4000,0) SEM obstáculos.

| RRT antigo |   RRT novo   | 

|---         |      ---     |

|t=0,0558839s|t=0,000469714s| 

|fps=43      |fps=57        | 

3º) Zig Zag entre os pontos (0,2000) e (0,-2000) 

Obstáculo (robô) no ponto (0,0)

| RRT antigo |   RRT novo   | 

|---         |      ---     |

|t=0,070037s |t=0,0809514s  | 

|fps=50      |fps=57        | 

4º) Zig Zag entre os pontos (-3000,0) e (3000,-0) 

Obstáculos (robôs) nos pontos (-1000,0) e (1000,0)

| RRT antigo |   RRT novo   | 

|---         |      ---     |

|t=0,0533753s|t=0,0781024s  | 

|fps=50      |fps=57        | 

5º) Zig Zag entre os pontos (-3000,0) e (3000,-0) 

Obstáculos (robôs) nos pontos (-2000,0) e (2000,0)

| RRT antigo |   RRT novo   | 

|---         |      ---     |

|t=0,0809107s|t=0,0846672s  | 

|fps=50      |fps=57        | 

Conclusões dos testes:

* Quando não há obstáculos, o RRT novo é na ordem de 100 vezes mais rápido.

* Quando há obstáculos, os dois tempos são de mesma ordem de grandeza, sendo que o RRT antigo é ligeiramente mais rápido: no teste de maior diferença (teste 4), a diferença de tempos foi de 0,0247271s.

* Em todos os testes, o código se manteve com fps mais alto no RRT novo. Nos testes em que não havia obstáculos e a trajetória simples podia ser usada, o fps do RRT antigo se mostrou ainda mais baixo que nos outros casos.