Futebol de Robos F180 » Inteligência

# 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)

## Implementações futuras e problemas atuais