Atividade #611
FechadaImplementação de controle por trajetória
Descrição
Seguindo as sugestões do Cap Renault, implementarei da seguinte forma:
Primeiro continuaremos usando o GoTo porém ao nos aproximarmos a uma distância d do nó já nos dirigiremos para o próximo, tentando tornar assim a movimentação mais suave. Mas dessa forma ainda reduziremos muito nossa velocidade quando tivermos muitos waypoints.
Sendo assim em um segundo momento passaremos a controlar a nossa velocidade tendo feedback de posição e velocidade.
Para que essas duas implementações possam funcionar precisaremos parar de recalcular a tragetória a toda instante. Para isso criaremos uma função para verificar a validade da trajetória e se é necessário um recálculo (começarei por aqui). Ela utilizará dois critérios, sendo eles:
- A trajetória já calculada passa agora por algum obstáculo?
- Os obstáculos que podem influenciar na trajetória se moveram mais que uma distância r (parâmetro)?
Dessa maneira teremos as velocidades que queremos que o robô assuma durante a tragetória, dessa forma podemos fazer um controle de malha fechada com realimentação de velocidade. Mas também queremos que ele se mantenha sobre a tragetória, então uma das componentes da saída será um compensação na velocidade para que ele volte a seguir a tragetória, assim existirá um controle com realimentação de posição.
Para que isso possa funcionar precisamos saber para qual waypoint o robô deve se direcionar quando ele sai da tragetória. Não encontrei uma solução para isso ainda.
Arquivos
Atualizado por Luciano Barreira há quase 7 anos
- Título alterado de Implementação de controle por tragetória para Implementação de controle por trajetória
Atualizado por Nicolas Oliveira há mais de 6 anos
- Arquivo malhaextendida.PNG malhaextendida.PNG adicionado
- Arquivo malhasimples.PNG malhasimples.PNG adicionado
Baseado na discussão da última reunião projetei a malha de controle de trajetória. Escrevi duas na verdade, pois a segunda depende da estimação da velocidade do robô e n sei se esse é um dado confiável atualmente. Além disso eu teria que calcular a aceleração e integrar para achar a velocidade a ser enviada, mas acho q isso n seria problema.
Capitão gostaria do feedback do senhor para começar a implementação.
Seguem as duas malhas:
Outra dúvida: Como seria o processo de definir as constantes PID da malha? Já que atualmente não tenho um modelo físico dele. Seria viável talvez pegar a resposta ao degrau de velocidade no robô ou algo do tipo para obter um modelo?
Atualizado por Luiz Renault Leite Rodrigues há mais de 6 anos
Onias, quais foram os métodos que vimos para determinação das constantes PID?
Nicolas, vamos começar com o controle proporcional. Estime esta constante em função da correção que deseja para uma determinada distância do segmento da trajetória.
Atualizado por Nicolas Oliveira há mais de 6 anos
- Arquivo pathfallower.mp4 pathfallower.mp4 adicionado
- Arquivo pathfollower1.mp4 pathfollower1.mp4 adicionado
- Arquivo pathfollwer2.mp4 pathfollwer2.mp4 adicionado
Fiz a primeira implementação do controle por trajetória. Quando o robô se move em direção a bola a sua frente o resultado parece satisfatório, visto que a transição entre waypoints é muito fluida agora, o robô praticamente não reduz sua velocidade e chega praticamente sem parar na hora de chutar a bola. Porém quando a bola está atrás dele a situação muda um pouco já que ele está com muito overshoot. Por ele errar a bola a trajetória é recalculada e ele n consegue acompanha-lá. Isso, provavelmente, se deve ao controle da velocidade paralela que não está realimentado. O controle lateral está bom, ele n desvia quase nada da trajetória lateralmente.
No último vídeo dá pra perceber claramente que ele passa direto do último waypoint pois tenta frear bruscamente.
Uma solução seria desacelerá-lo um pouco ao chegar perto do waypoint final, ou usar como realimentação a velocidade estimada.
Seguem vídeos como a velocidade limitada em 1m/s para testes:
Atualizado por Luiz Renault Leite Rodrigues há mais de 6 anos
Que tipo de controle está usando?
Aparentemente apenas proporcional, sendo overshoot característica deste.
Uma outra possibilidade é ajustar a trajetória pra chegar no ponto com uma determinada direção do vetor velocidade, assim, teria que fazer uma curva mais aberta em função da velocidade final de chegada.
Ou seja, deve haver uma rotina para cálculo da trajetória não passando apenas o ponto de destino, mas passando também um vetor de velocidade. Assim, faz-se um cálculo reverso dos últimos waypoints de forma a permitir obter a velocidade final, dadas as características do robô, como aceleração, momento de inércia, velocidade máxima, etc.
Atualizado por Nicolas Oliveira há mais de 6 anos
Estou usando controle proporcional para a velocidade transversal a trajetória, ou seja, o controle lateral. E para a velocidade paralela eu apenas defino a velocidade com base na orientação dos 2 waypoints no qual o robô está entre, sempre sendo a velocidade máxima. Dessa forma o problema acontece quando o vetor de velocidade muda repentinamente, nessa minha malha (utilizando a simples descrita acima) ele apenas troca a velocidade de direção do nada, então o robô não consegue frear e se ajustar a trajetória.
Estou pensando em como posso implementar essa sugestão que o senhor deu.
Atualizado por Luciano Barreira há mais de 6 anos
Não sei se ajuda, mas achei no código da Tigers alguns pedaços de código de onde eles geram o perfil de trajetória:
```Java
private void generateTrajectory(final IVector2 s0, final IVector2 s1, final IVector2 v0, final double vmax,
final double acc)
{
double inc = Math.PI / 8.0;
double alpha = Math.PI / 4.0;
// binary search, some iterations (fixed)
while (inc > 1e-7)
{
double cA = Math.cos(alpha);
double sA = Math.sin(alpha);x = new BangBangTrajectory1D(s0.x(), s1.x(), v0.x(), vmax * cA, acc * cA);
y = new BangBangTrajectory1D(s0.y(), s1.y(), v0.y(), vmax * sA, acc * sA);double diff = Math.abs(x.getTotalTime() - y.getTotalTime());
if (diff < 0.0001)
{
break;
}
if (x.getTotalTime() > y.getTotalTime())
{
alpha = alpha - inc;
} else
{
alpha = alpha + inc;
}inc *= 0.5;
}
}
``````Java
public BangBangTrajectory1D(final double initialPos, final double finalPos,
final double initialVel, final double maxVel, final double maxAcc)
{
this.initialPos = initialPos;
this.finalPos = finalPos;
this.initialVel = initialVel;
this.maxAcc = maxAcc;
this.maxVel = maxVel;
generateTrajectory(initialPos, initialVel, finalPos, maxVel, maxAcc);
}
// metodo pra tratar os casos de perfil triangular ou trapezoidal
void generateTrajectory(final double x0, final double xd0, final double xTrg, final double xdMax,
final double xddMax)
{
parts.clear();
{
final double t2;
final double v1;
final double t1;
final double acc;
final double s1;
{
double a1;
double a3;
double t1;
double t2;
double t3;
double v2;
double s1;
double s2;if (v0 > v1)
{
a1 = -aMax;
} else
{
a1 = aMax;
}if (v1 > 0)
{
a3 = -aMax;
} else
{
a3 = aMax;
}t1 = (v1 - v0) / a1;
v2 = v1;
t3 = -v2 / a3;s1 = s0 + (0.5 * (v0 + v1) * t1);
s2 = s3 - (0.5 * v2 * t3);
t2 = (s2 - s1) / v1;parts.add(new BBTrajectoryPart(t1, a1, v0, s0));
parts.add(new BBTrajectoryPart(t1 + t2, 0, v1, s1));
parts.add(new BBTrajectoryPart(t1 + t2 + t3, a3, v2, s2));
}```
Esse parts é esta classe aqui:
Cada parte tem uma velocidade, posição, aceleração e tempo.
```Java
private final List<BBTrajectoryPart> parts = new ArrayList<>();
/**
* Part of trajectory
*/
@Persistent
static class BBTrajectoryPart
{
/** /
double tEnd;
/* /
double acc;
/* /
double v0;
/* */
double s0;@SuppressWarnings("unused")
private BBTrajectoryPart()
{
// required for Berkeley
}/**
* @param tEnd
* @param acc
* @param v0
* @param s0
*/
private BBTrajectoryPart(final double tEnd, final double acc, final double v0, final double s0)
{
this.tEnd = tEnd;
this.acc = acc;
this.v0 = v0;
this.s0 = s0;
}
}
```Atualizado por Nicolas Oliveira há mais de 6 anos
- Arquivo accel1.png accel1.png adicionado
- Arquivo accel2.png accel2.png adicionado
- Arquivo accel3.png accel3.png adicionado
- Arquivo vel1.png vel1.png adicionado
- Arquivo vel2.png vel2.png adicionado
- Arquivo vel3.png vel3.png adicionado
Apliquei um degrau de velocidade no robô para que eu pudesse obter a resposta da aceleração ao degrau, fiz no grSim. Mas esse mesmo processo pode ser replicado facilmente no robô real. Salvei a posição no tempo em .csv e tratei no matlab. A aceleração n fica muito bem definida pq o tempo entre as amostras n é constante, sendo assim optei pelo tempo médio para facilitar.
Seguem os resultados:
Podemos perceber que em média aceleração fica entre 15m/s^2 e 20m/s^2. Já a desaceleração pode chegar a 80m/s^2 no simulador.
Vou trabalhar com esses valores.
Atualizado por Nicolas Oliveira há mais de 6 anos
- Arquivo malha.PNG malha.PNG adicionado
- Arquivo GamevsStop.mp4 GamevsStop.mp4 adicionado
- Arquivo jogo.mp4 jogo.mp4 adicionado
- Arquivo zigzagVel1.mp4 zigzagVel1.mp4 adicionado
- Arquivo zigzagVel0.mp4 zigzagVel0.mp4 adicionado
- Situação alterado de Em andamento para Feedback
Luiz Renault Leite Rodrigues escreveu:
Uma outra possibilidade é ajustar a trajetória pra chegar no ponto com uma determinada direção do vetor velocidade, assim, teria que fazer uma curva mais aberta em função da velocidade final de chegada.
Ou seja, deve haver uma rotina para cálculo da trajetória não passando apenas o ponto de destino, mas passando também um vetor de velocidade. Assim, faz-se um cálculo reverso dos últimos waypoints de forma a permitir obter a velocidade final, dadas as características do robô, como aceleração, momento de inércia, velocidade máxima, etc.
Utilizando controle PD em todas as direções agora. Agora nessa primeira implementação sou capaz de definir a velocidade que o robô deve ter ao final da trajetória. Assim para situações como o Stop posso definir que quero que ele chegue parado, porém ele ainda vai andando entre os waypoints com máxima velocidade (que é o nosso maior objetivo). Assim sempre que digo que quero chegar com velocidade zero ao final me preocupo em chegar exatamente no ponto final. Utilizando assim no último trecho da trajetória um controle PD de posição também no eixo paralelo. Já quando não quero chegar parado ao final me preocupo apenas em setar a velocidade sem dar tanta atenção a posição,já que naturalmente não conseguiria parar nela.
Segue a malha utilizada atualmente:
Velocidade limitada em 1,5m/s para os testes.
No zigzag de teste podemos ver como o controle se comporta quando queremos que o robô chegue parado:
E quando ele deve chegar com velocidade igual a 1m/s:
Nesse vídeo defini que o robô deveria chegar para chutar a bola com velocidade de 0,5m/s, podemos ver que ele n fica totalmente controlado mas essa nova "habilidade" pode ser muito interessante quando a bola for sair ou quando ele for um goleiro por exemplo.
Aqui podemos ver que ele se move com velocidade máxima em toda a trajetória e reduz para 0,5m/s na hora do chute e quando é dado stop, chega parado no destino:
Próximo passo é poder definir com qual módulo e direção de velocidade o robô deve chegar no destino e testar com robôs reais. E também levar em consideração as características físicas do robô nas curvas e mudanças de direção. Aceito sugestões do que está faltando para otimizar esse controle. Acho que atualmente já superou o controle antigo.
Atualizado por Luiz Renault Leite Rodrigues há mais de 6 anos
Excelente trabalho! Quero ver o time da LARC contra o atual.
Atualizado por Nicolas Oliveira há mais de 6 anos
- Situação alterado de Feedback para Em andamento
Hoje foram realizados testes no campo da imbel e antes de apresentar os resultados deve-se ressaltar que a câmera estava a 30fps o que prejudica bastante nosso controle. A comunicação estava sensível a distância também, talvez seja interessante levarmos uma daquelas antenas para deixar na placa de comunicação.
O robô consegue desviar bem dos obstáculos quando sua velocidade paralela limite é de 1,5m/s quando aumentamos para 2m/s ele já tem muita dificuldade nas curvas.
Irei implementar uma redução de velocidade com base na angulação das curvas, pelo simulador posso perceber que com 1m/s ele realiza qq curva, então o robô real deve ter uma velocidade desse tipo também.
Nos dois vídeos abaixo podemos vê-lo desviando do robô inimigo de lado e de frente para a trajetória.
Podemos vê-lo seguindo uma circunferência no modo de teste (finalmente rs) no vídeo abaixo:
O controle se mostrou bem satisfatório no ajuste lateral e quando ele deve chegar parado no objetivo ou com velocidade. O próximo passo é incluir na IA todas as possibilidades que esse controle trás a movimentação dos robôs.
Atualizado por Nicolas Oliveira há mais de 6 anos
- Arquivo controle1.mp4 controle1.mp4 adicionado
- Arquivo controle2.mp4 controle2.mp4 adicionado
- Arquivo controle3.mp4 controle3.mp4 adicionado
Esqueci de subir os videos
Nicolas Oliveira escreveu:
Hoje foram realizados testes no campo da imbel e antes de apresentar os resultados deve-se ressaltar que a câmera estava a 30fps o que prejudica bastante nosso controle. A comunicação estava sensível a distância também, talvez seja interessante levarmos uma daquelas antenas para deixar na placa de comunicação.
O robô consegue desviar bem dos obstáculos quando sua velocidade paralela limite é de 1,5m/s quando aumentamos para 2m/s ele já tem muita dificuldade nas curvas.
Irei implementar uma redução de velocidade com base na angulação das curvas, pelo simulador posso perceber que com 1m/s ele realiza qq curva, então o robô real deve ter uma velocidade desse tipo também.Nos dois vídeos abaixo podemos vê-lo desviando do robô inimigo de lado e de frente para a trajetória.
Error executing the video macro (undefined method `find_by_filename' for [#<Attachment id: 1367, container_id: 611, container_type: "Issue", filename: "controle1.mp4", disk_filename: "180407023622_controle1.mp4", filesize: 2154913, content_type: "video/mp4", digest: "2eac781fbc93eae36a3339b62e144c33", downloads: 0, author_id: 20, created_on: "2018-04-07 02:36:22.000000000 +0000", description: "", disk_directory: "2018/04">, #<Attachment id: 1366, container_id: 611, container_type: "Issue", filename: "controle2.mp4", disk_filename: "180407023512_controle2.mp4", filesize: 1193173, content_type: "video/mp4", digest: "697a1fb101171d1ce0d641f041b5c505", downloads: 0, author_id: 20, created_on: "2018-04-07 02:35:12.000000000 +0000", description: "", disk_directory: "2018/04">, #<Attachment id: 1368, container_id: 611, container_type: "Issue", filename: "controle3.mp4", disk_filename: "180407023631_controle3.mp4", filesize: 1603535, content_type: "video/mp4", digest: "f3e6c637984287e5182f72bf852dfde6", downloads: 0, author_id: 20, created_on: "2018-04-07 02:36:31.000000000 +0000", description: "", disk_directory: "2018/04">]:Array Did you mean? find_index)Error executing the video macro (undefined method `find_by_filename' for [#<Attachment id: 1367, container_id: 611, container_type: "Issue", filename: "controle1.mp4", disk_filename: "180407023622_controle1.mp4", filesize: 2154913, content_type: "video/mp4", digest: "2eac781fbc93eae36a3339b62e144c33", downloads: 0, author_id: 20, created_on: "2018-04-07 02:36:22.000000000 +0000", description: "", disk_directory: "2018/04">, #<Attachment id: 1366, container_id: 611, container_type: "Issue", filename: "controle2.mp4", disk_filename: "180407023512_controle2.mp4", filesize: 1193173, content_type: "video/mp4", digest: "697a1fb101171d1ce0d641f041b5c505", downloads: 0, author_id: 20, created_on: "2018-04-07 02:35:12.000000000 +0000", description: "", disk_directory: "2018/04">, #<Attachment id: 1368, container_id: 611, container_type: "Issue", filename: "controle3.mp4", disk_filename: "180407023631_controle3.mp4", filesize: 1603535, content_type: "video/mp4", digest: "f3e6c637984287e5182f72bf852dfde6", downloads: 0, author_id: 20, created_on: "2018-04-07 02:36:31.000000000 +0000", description: "", disk_directory: "2018/04">]:Array Did you mean? find_index)Podemos vê-lo seguindo uma circunferência no modo de teste (finalmente rs) no vídeo abaixo:
Error executing the video macro (undefined method `find_by_filename' for [#<Attachment id: 1367, container_id: 611, container_type: "Issue", filename: "controle1.mp4", disk_filename: "180407023622_controle1.mp4", filesize: 2154913, content_type: "video/mp4", digest: "2eac781fbc93eae36a3339b62e144c33", downloads: 0, author_id: 20, created_on: "2018-04-07 02:36:22.000000000 +0000", description: "", disk_directory: "2018/04">, #<Attachment id: 1366, container_id: 611, container_type: "Issue", filename: "controle2.mp4", disk_filename: "180407023512_controle2.mp4", filesize: 1193173, content_type: "video/mp4", digest: "697a1fb101171d1ce0d641f041b5c505", downloads: 0, author_id: 20, created_on: "2018-04-07 02:35:12.000000000 +0000", description: "", disk_directory: "2018/04">, #<Attachment id: 1368, container_id: 611, container_type: "Issue", filename: "controle3.mp4", disk_filename: "180407023631_controle3.mp4", filesize: 1603535, content_type: "video/mp4", digest: "f3e6c637984287e5182f72bf852dfde6", downloads: 0, author_id: 20, created_on: "2018-04-07 02:36:31.000000000 +0000", description: "", disk_directory: "2018/04">]:Array Did you mean? find_index)O controle se mostrou bem satisfatório no ajuste lateral e quando ele deve chegar parado no objetivo ou com velocidade. O próximo passo é incluir na IA todas as possibilidades que esse controle trás a movimentação dos robôs.
Atualizado por Nicolas Oliveira há mais de 6 anos
Um dos problemas desse controle (e do antigo tbm) é que usando PID somos capazes de limitar as velocidades em cada eixo, mas não limitamos a velocidade total do robô. Então quando eu coloco que o máximo em cada eixo é de 2,5 m/s a velocidade máxima total fica em 3,5355 m/s. O que se eu não me engano fica acima da velocidade máxima da categoria, e descontrolava nosso robô. Coloquei um limitador na velocidade total em 3 m/s e o overshoot melhorou consideravelmente. Agora devemos testar e refinar as contantes em campo, e otimiza-las para os nossos robôs.
Atualizado por Nicolas Oliveira há mais de 6 anos
Capitão, uma dúvida que eu tenho é que usando controle PID n garantimos que o robô desacelera com o máximo que ele pode, certo? O que eu faço atualmente para que ele acelere com o máximo que pode e mandar um degrau de velocidade. Mas na desaceleração como eu faria isso? Se eu mandasse um degrau para ele parar eu n teria como garantir que ele iria parar no lugar certo.
Atualizado por Nicolas Oliveira há mais de 6 anos
Nicolas Oliveira escreveu:
Um dos problemas desse controle (e do antigo tbm) é que usando PID somos capazes de limitar as velocidades em cada eixo, mas não limitamos a velocidade total do robô. Então quando eu coloco que o máximo em cada eixo é de 2,5 m/s a velocidade máxima total fica em 3,5355 m/s. O que se eu não me engano fica acima da velocidade máxima da categoria, e descontrolava nosso robô. Coloquei um limitador na velocidade total em 3 m/s e o overshoot melhorou consideravelmente. Agora devemos testar e refinar as contantes em campo, e otimiza-las para os nossos robôs.
Atualizado por Luciano Barreira há mais de 6 anos
- Versão ajustado para RoboCup 2018
Atualizado por Nicolas Oliveira há mais de 6 anos
- Situação alterado de Em andamento para Resolvida
Atualizado por Nicolas Oliveira há mais de 6 anos
- Situação alterado de Resolvida para Fechada
Irei focar no controle por velocidade.