Controle de Trajetórias

Transcrição

1 Controle de Trajetórias Construção de Mapas Localização posição do robô features Extração de Informação ação Planejamento da Missão trajetória Percepção Controle de Trajetória velocidades dados brutos Sensoriamento Cognição Planejamento de Trajetórias Controle de Locomoção Atuação Ambiente do Robô software embarcado aplicação robótica 1 Controle de Trajetórias Problema: Dada uma trajetória com o robô posicionado no seu ponto inicial, determinar uma função de controle u(t) tal que, quando aplicada ao robô, o mesmo percorre a trajetória planejada até o seu ponto alvo. A função de controle pode ser realizada em: Malha aberta: o controle não leva em conta o erro entre as trajetórias pretendida e real. Malha fechada: o erro entre as trajetórias pretendida e real é levado em conta no controle. Em ambos os casos a pose do robô deve ser a mais precisa possível (via localização, por exemplo). 2

2 Controle em Malha Aberta No controle em malha aberta a trajetória é dividida em segmentos simples tais como retas e círculos. Dada uma velocidade linear fixa, a ação de controle consiste em estabelecer: 1) A atuação no robô, comumente velocidade em cada roda, para realizar o segmento de trajetória atual. 2) O tempo que esta ação de controle deve perdurar. No caso de segmento de reta o tempo de duração da ação é dado por onde D é a distância do segmento a ser percorrida e V é a velocidade linear no trecho. 3 Controle em Malha Aberta No caso de segmento de círculos devemos estabelecer: 1) O raio de curvatura do círculo. 2) O segmento de círculo que o robô deve percorrer. Φ R 4

3 Controle em Malha Aberta D2 Φ1 R2 R1 Φ2 ICR1 D1 ICR2 D3 5 Controle em Malha Aberta Deslocamento passo-a-passo A trajetória pode ser percorrida com controles de orientação (/motion/heading) e de deslocamento (/motion/dislocate). θ D A velocidade é reduzida (até zero) quando o robô se aproxima da distância estipulada. 6

4 Controle em Malha Aberta Ao atingir uma certa proximidade do ponto de destino o robô já se locomove para o próximo ponto (sem esperar a velocidade reduzir a zero). 7 Controle em Malha Fechada r(t) sinal de referência + Σ - e(t) sinal de erro Controlador u(t) sinal de controle Sistema (Robô) y(t) saída Sinal de referência: trajetória. Sinal de erro: desvio da trajetória. Sinal de controle: velocidades das rodas ou velocidades linear e rotacional. Sinal de saída: trajetória real percorrida. 8

5 Controladores PID P r(t) + Σ e(t) I Σ u(t) Sistema (Robô) y(t) - D 9 Controladores PID Para um erro em degrau: O parâmetro proporcional Kp determina a "rapidez" com que o controlador tenta atingir a nova referência a fim de zerar o erro. O parâmetro integral Ki é necessário para zerar o erro de regime. O parâmetro derivativo Kd limita a taxa de variação da saída do controlador melhorando a estabilidade do sistema e diminuindo o overshoot. Os parâmetros Kp, Ki e Kd devem ser ajustados para se obter uma boa resposta (tempo de subida, overshoot, erro de regime) e sem oscilações em regime. Existem várias técnicas de sintonia destes parâmetros, por exemplo, Ziegler-Nichols e Twiddle. 10

6 Controladores PID Wikipedia 11 Controladores PID A versão discreta de um controlador PID é dada por: Incorporando Δt nas constantes Ki e Kd temos: 12

7 Controle de Trajetória No controle de trajetória comumente consideramos dois erros: Erro de tracking: quão distante o robô se encontra da trajetória pretendida. Erro de orientação: quão distante o robô se encontra da orientação dada pela trajetória no ponto atual. tracking orientação Estes erros devem ser minimizados simultaneamente pelo controlador. 13 Controle de Trajetória Comumente o variável de controle é a orientação do robô. A orientação é corrigida visando diminuir o erro de tracking, ou seja, forçando o robô atingir um ponto da trajetória a uma certa distância a frente. Esta distância é denominada "lookahead". erro de orientação Ponto mais próximo do robô lookahead 14

8 Controle de Trajetória O robô percorre a trajetória com velocidade de percurso constante exceto em um trecho final quando se aproxima do alvo. Este trecho pode ser estipulado por um ponto da trajetória, por exemplo, a partir do penúltimo ponto da trajetória. A desaceleração pode ser dada por um controle proporcional. Assim, a velocidade linear de percurso V é dada por:, se o robô se encontra no trecho final, caso contrário (velocidade de percurso) onde Px e Py é a posição atual do robô e Gx e Gy são as coordenadas do ponto alvo. 15 Controle de Trajetória A orientação do robô é dada pelo controlador PID. O erro é calculado por: Y Próximo ponto (alvo corrente) Ty et Δy = Ty - Py Pθ Py Δx = Tx - Px X Px Tx A ação de controle é dada por: 16

9 Controle de Trajetória Como determinar se o alvo corrente foi atingido? A forma mais direta é considerar que o ponto foi atingido quando o robô se encontra a uma distância ξ do ponto (bola de raio ξ). Entretanto isto pode causar um degrau abrupto no erro: et 0 et+1 -π/2 Neste caso o controlador PID produzirá uma oscilação na orientação do robô. 17 Controle de Trajetória Possíveis soluções: Suavização da trajetória (antes de percorre-la). Diminuir a velocidade enquanto o robô estiver próximo do ponto atingido (dentro do círculo). introduzir um filtro passa-baixa no erro: Tk Tk+1 18

10 Controle de Trajetória Algoritmo: 1. Faça PontoAtual = PontoAlvo = 1 (primeiro ponto da trajetória). 2. Imprima a velocidade estipulada de percurso (Vmax). 3. Enquanto o PontoAtual!= N (número de pontos da trajetória) faça: - Leia a pose do robô; - Determine o ponto K da trajetória mais próximo do robô a partir do PontoAtual; - Determine a distância do robô até este ponto (erro de tracking) - Atualize o erro quadrático médio (EQM); - Determine o alvo corrente PontoAlvo = K + Lookahead; - Enquanto o PontoAlvo não for atingido, faça: * Leia a pose do robô; * Determine o erro de orientação; * Compute a atuação a partir deste erro (controlador PID); * Atue na orientação do robô; * Se ultimo segmento atue na velocidade do robô; * Sleep(Δt); - Faça PontoAtual = PontoAlvo. 4. Páre o robô. 19 Controladores nebulosos (fuzzy) utilizam regras de controle derivadas de conjuntos nebulosos. Conjunto nebulosos utilizam variáveis linguísticas (por exemplo: alto, médio, baixo) e funções de pertinência que atribuem graus de pertinência para estas variáveis do conjunto fuzzy. Por exemplo, Seja o conjunto nebuloso "Velocidade do Robô": Grau de Pertinência 1 Média Baixa Alta mm/s

11 Variáveis de Entrada (Valores Linguísticos) Variáveis de Saída (Valores Linguísticos) Inferência Nebulosa Defuzzificação Fuzzificação Variáveis Brutas (Valores Numéricos) Variáveis de Controle (Valores Numéricos) Processo 21 Fuzzificação (Nebulização) Grau de Pertinência 1 Média Baixa Alta x= P(media) = 0.7 P(alta) =

12 Inferência Nebulosa SE (Velocidade == média) E (Distância == longe) ENTÃO (Segurança = alta) P(media) = 0.4 P(longe) = 0.7 P(alta) = 0.4 (menor) SE (Velocidade == alta) OU (Distância == longe) ENTÃO (Segurança = media) P(alta) = 0.6 P(próxima) = 0.7 P(media) = 0.7 (maior) 23 Combinação de regras (Mamdani) A combinação de regras se dá com a operação de união dos conjuntos nebulosos "cortados" com os valores obtidos para os consequentes das regras. 1 média baixa alta Segurança 24

13 Deffuzificação. Método do Centróide A saída do processo de deffuzificação y é dada pelo centróide da área formada pela combinação das funções de pertinência da variável x. centróide 0.25 y = 0.58 (saída) Segurança 25 Controlador Nebuloso para controle de trajetória desenvolvido por Leonardo R. Olivi. Velocidade da Roda Direita (Vr) Erro de Orientação (α) Controlador Nebuloso Velocidade Angular (ω) Velocidade da Roda Direita (Ve) 26

14 Regiões de Atuação 27 Regiões de Atuação (Conjunto Nebuloso) RR: Rear Right RT: Right CT: Center LF: Left RL: Rear Left 28

15 Velocidade Angular (Conjunto Nebuloso) HR: High Right MR: Medium Right NT: Neutral ML: Medium Left HL: High Left 29 Velocidade das Rodas (Conjunto Nebuloso) HN: High Negative MN: Medium Negative NT: Neutral MP: Medium Positive HP: High Positive 30

16 Inferência Fuzzy (25 Regras) ω HR MR NT ML HL VL: MP VL: MP VL: MP VL: MP VL: MP RR VR: VR: VR: VR: MN MN MN MN VR: MN VL: MP VL: HP VL: MP VL: MP VL: MP RT VR: VR: NT VR: NT MN VR: NT VR: NT α V : HP VL: HP VL: HP VL: MP VL: NT CT L VR: NT VR: MP VR: HP VR: HP VR: HP VL: NT VL: NT VL: MN VL: NT VL: NT LF VR: MP VR: MP VR: MP VR: HP VR: HP V : MN VL: MN VL: MN VL: MN VL: MN RL L VR: MP VR: MP VR: MP VR: MP VR: MP 31 A "defuzificação" por meio de centróide produz o seguinte mapeamento (α, ω) (VR,VL) 32

17 Left (LF) VL (Neutral) VR (High Positive) Left (LF) High Left (HL) VL (Neutral) High Right (HR) VR (Medium Positive) 33 34

18 Replanejamento de Rotas Durante o percurso de uma trajetória o robô deve monitorar a presença de obstáculos ao longo do percurso: obstáculos transientes (pessoas, outros robôs, etc.); obstáculos permanentes ausentes no mapa (mobílias, passagens fechadas, etc.); outros impedimentos (degraus, localização imprecisa, etc.). O que fazer quando um obstáculo for detectado pelos sensores estereoceptivos? parar o robô; localizar o robô (se possível); atualizar do mapa; replanejar a rota. 35 Replanejamento de Rotas A atualização do mapa consiste em inserir (ou remover) os objetos detectados no percurso, ou seja, tornar células que eram livres em ocupadas e vice-versa. Esta ação de mapeamento é local, ou seja, em torno da posição atual do robô. 36

19 Replanejamento de Rotas Assim que o mapa ao redor do robô for atualizado, a rota é replanejada a partir a posição atual do robô até o ponto objetivo (alvo) original. Idealmente, o algoritmo D* (Dynamic A*) deveria ser utilizado: usa os dados processados pelo A* (conjuntos aberto e fechado) para recalcular uma rota. D* é mais difícil de implementar que o A*. Na prática executa-se novamente o A* ao invés de utilizar o D*. Pode ocorrer que seja necessário várias execução do algoritmo A* para contornar o obstáculo: 37 Replanejamento de Rotas Ao invés de replanejar a rota pode-se tentar executar um algoritmo de desvio de obtáculos, por exemplo, o algoritmo do Bug. Funciona bem se o obstáculo interferir pontualmente na rota, por exemplo: 38

20 Replanejamento de Rotas Uma outra alternativa para contornar obstáculos é, localmente, usar o algoritmo de Campos Potenciais. Obstáculo Rota definida por Campos Potenciais Alvo para o algoritmo de Campos Potenciais 39