SSC5955 Slides adaptados de Masahiro Ono - MIT

Transcrição

1 SSC5955 Slides adaptados de Masahiro Ono - MIT

2 Sumário Problema de Planejamento de Rotas Kinodynamic path planning Abordagem para Planejamento de Rota Programação Linear (PL) Programação Inteira (PI) Programação Linear Inteira Mista (PLIM) Eemplo Receding Horizon Control MPC (model-predictive control)

3 Problema de Planejamento de Rota min C( r) r s. t. r R r: rota R: Conjunto de rotas possíveis C: função de custo Início Fim 3

4 4

5 Commercial Airplane Path Planning o-fly zones around Japan (Air zones that are reserved for military use) 5

6 Problema de Planejamento de Rota Início Fim 6

7 Problema de Planejamento de Rota Início Grafo de Visibilidade Fim 7

8 Planejamento de Rota Início Grafo de Visibilidade + Algoritmo de Busca (Dijstra, A*, etc) Fim

9 Planejamento de Rota Início Fim 9

10 Path Planning Início Fim 0

11 Kinodynamic Path Planning Veículos que eecutem uma trajetória em alta velocidade podem ter dificuldade para seguir a trajetória estabelecida. A dinânica do veículo precisa ser eplicitamente considerada. Isso caracteriza o chamado Kinodynamic path planning.

12 Kinodynamic Path Planning Calculus of variations Rapidly-Eploring Random Tree (RRT) Brachistochrone curve Constrained Optimization

13 Abordagem para Planejamento de Rota Problema de Planejamento de Rota Codificação Problema de Otimização Restrita Solver Solução 3

14 Abordagem para Planejamento de Rota Problema de Otimização Otimização Convea Programação geométrica Otimização Canônica Programação Linear Programação Quadrática Programação ão linear Otimização ão Convera Programação Inteira Programação Inteira Mista Programação Linear Inteira Mista. 4

15 Programação Linear (PL) min c T s. t. A b 5

16 Programação Linear (PL) min s. t., 0, 0 6

17 Programação Linear (PL) 5/ 3 4/ 3 min, s. t. 0, 0 Solution: (/3, /3) 7

18 Programação Linear Forma mais simples de otimização restrita. Aplicação em diversos problemas utrição Animal Operação de linhas aéreas Planejamento de rotas Soluções obtidas em tempo polinomial Algoritmo de Karmarar (984) Solver comercial disponível ILOG CPLEX 8

19 Programação Inteira (PI) min c T s. t. A b onde é inteiro 9

20 Programação Inteira(PI) 4/ 3 min, s. t. 0, e são inteiros 0 Solution: (, 0) and (0,) 0

21 Programação Linear Inteira Mista (PLIM) min c T s. t. A b onde alguma componente de é inteira

22 Programação Linear Inteira Mista (PLIM) 4/ 3 min, s. t. 0, é inteira 0 Solution: (, 0) and (0,)

23 Programação Linear Inteira Mista (PLIM) Formulação geral: praticamente todo problema pode ser aproimado e formulado como um MILP Tempo eponencial para resolver: Branch and bound Eponencial no número de variáveis inteiras Solver comercial disponível ILOG CPLEX 3

24 Eemplo Veículo autônomo em um cenário bidimensional 4 ma ma, F F F F F F y m y y Dinâmicas Dinâmica discreta no tempo y F F m t m t m t m t y y t t y y,, / 0 0 / / / t+ = A t + Bu t Início Fim Velocidade Inicial (Thrust limits)

25 Eemplo Veículo autônomo em um cenário bidimensional Restrições espaciais: Veículo precisa estar dentro da região y h T g Fim 4 n or h T n H g y g n h T g y Velocidade Inicial Início y h T 3 g 3 5

26 Eemplo Formulação usando Programação Linear (LP) min :, u: s. t. H 0 u ma C( A, u g ( 0,, ) start goal u u Bu ma u ) ( 0,, ) ( 0,, ) Custo Dinâmicas Restrições espaciais Posição inicial Posição final Limites de empuo T y y, u F F T, y, 6

27 Eemplo Qual função de custo utilizar? Eemplo: mínimo esforço no controle C(, u u ) u F, F y, Truques min u min u u u u u 0, u u 0, ou min v v u, v u, 7

28 Receding Horizon Control min :, u: s. t. H 0 u ma C( A, u g ( 0,, ) start goal u u Bu ma u ) ( 0,, ) ão é uma boa ideia fiar (horizonte de tempo) ( 0,, ) 8

29 Receding Horizon Control Fim First steps Início 9

30 Receding Horizon Control Fim Próimos passos Primeiros passos Início 30

31 Receding Horizon Control (RHC) Fim Próimos passos Próimos passos Primeiros passos Início 3

32 RHC - Formulação Matemática min :, u: s. t. H 0 u ma C( A g start goal u ( Bu, u ma u ( 0,, ) u ( ) 0,, f ( ) Custo para chegar ao fim 0,, ) ) Custo Dinâmica Restrições espaciais Posição e velocidade iniciais Posição e velocidade finais Limites de empuo T y y, u F F T, y, 3

33 RHC - Custo para chegar ao fim min :, u: J(, u u ) f ) Função de custo = custo do segmento de rota atual Estimativa do custo do estado final ao Fim ( Custo para chegar ao fim Fim Custo para chegar ao fim guia a rota até o fim. Similar a função heurística do algoritmos A*. Próimos passos Primeiros passos Início 33

34 RHC - Custo para chegar ao fim min :, u : T u cd Distância de até o Fim Esforço de controle ao longo da rota Fim d c: peso relativo entre esforço de controle e distância c=0 : veículo não se move c=+infinity : veículo se direciona ao Fim o mais rápido possível Próimos passos Primeiros passos Início 34

35 RHC - Aproimação do Cálculo da Distância Problema: Truque. Ideia: aproimar o círculo pelo poliedro. 35 ) ( ) ( Goal Goal y y d ão Linear!!! ),, ( ) / sin( ) / cos( min n y y n n d d Goal Goal T ) ( ) ( min Goal Goal y y Aproimação Goal Projeção d Goal Goal y y ) / sin( ) / cos( n n

36 Eemplo RHC t = 0 0 segundos depois. Posição prevista em t = 0 Fim Plano Posição atual (t = 0) Início 36

37 Eemplo RHC As incertezas do ambiente alteram a rota prevista. A rota atual difere da rota planejada t = 0 Posição prevista em t = 0 Posição atual em t = 0 Fim Plano Rota atual Início 37

38 Horizonte de Planejamento > Horizonte de Eecução t = 0 3 segundos mais tarde. Posição prevista em t = 0 Fim Plano Posição atual (t = 0) Início 38

39 Horizonte de Planejamento > Horizonte de Eecução 3 segundos mais tarde. t = 3 Um pouco distante da rota planejada Posição prevista em t = 0 Fim Plano Posição atual(t = 3) Início 39

40 Horizonte de Planejamento > Horizonte de Eecução Abandona o plano depois de t = 4 t = 3 Fim Posição atual (t = 3) Início 40

41 Horizonte de Planejamento > Horizonte de Eecução Abandona o plano depois de t = 4 t = 6 Posição prevista em t = 3 Replaneja para outro horizonte de planejamento Fim Posição atual (t = 3) Início 4

42 Horizonte de Planejamento > Horizonte de Eecução 3 segundos mais tarde t = 6 Posição prevista em t = 3 Fim Posição atual (t = 6) Início 4

43 Horizonte de Planejamento > Horizonte de Eecução 3 segundos mais tarde t = 6 Abandona o plano após t = 7 Fim Posição atual (t = 6) Início 43

44 Horizonte de Planejamento > Horizonte de Eecução 3 segundos mais tarde t = 6 Abandona o plano após t = 7 Posição prevista em t = 6 Fim Replaneja rota Posição atual (t = 6) Início 44

45 Horizonte de Planejamento > Horizonte de Eecução Horizonte de planejamento: 0seg Horizonte de eecução: 3seg (Horizonte de planejamento > horizonte de eecução) para lidar com incertezas. t = 6 Posição prevista em t = 6 Fim Sempre, horizonte de eecução = passo Posição atual (t = 6) Início 45

46 Qual a necessidade de fazer um planejamento que nunca será eecutado?? Resposta: Planejador usa a predição futura tal que o plano na próima janela de tempo seja consistente com o plano em eecução. MPC = Model Predictive Control (Constrained optimization + Receding horizon) 46