implementação de controlador ilqr em braço robótico com ...€¦ · convergência. 2019....
TRANSCRIPT
UNIVERSIDADE DE SÃO PAULO – USP
ESCOLA DE ENGENHARIA DE SÃO CARLOS – EESC
DEPARTAMENTO DE ENGENHARIA MECÂNICA – SEM
CAPA
Choi Wang Dzak
Implementação de controlador iLQR em braço robótico
com quatro graus de liberdade: Estudo da função custo,
linearização de trajetória e convergência
São Carlos
2019
1
2
Choi Wang Dzak
Implementação de controlador iLQR em braço robótico com quatro graus de
liberdade: Estudo da função custo, linearização de trajetória e convergência
Monografia apresentada ao Curso de
Engenharia Mecânica, da Escola de
Engenharia de São Carlos da Universidade
de São Paulo, como parte dos requisitos
para obtenção do título de Engenheiro
Mecânico.
Orientador: Prof. Dr. Glauco Augusto de
Paula Caurin
São Carlos
2019
3
4
FICHA C ATALOGR ÁFIC A
5
6
FOLH A D E AVALIAÇ ÃO OU APROVAÇ ÃO
7
8
DEDICATÓRIA
Dedico este trabalho à minha querida família,
cujo apoio foi indispensável para que eu
chegasse até aqui.
9
10
AGRADECIMENTOS
Agradeço à minha família pelo apoio e suporte durante toda a graduação.
Agradeço aos meus amigos Sarah, Elisa e João Gabriel pela paciência, apoio,
noites estudando, exercícios trocados e milk-shakes tomados, que levarei para o resto
da vida.
Agradeço ao Prof. Dr. Glauco Caurin pela oportunidade e pelo apoio, e ao
doutorando Henrique Garcia pelos conselhos e pela orientação que foram
fundamentais para o ajuste do algoritmo do Trabalho de Conclusão de Curso.
11
12
EPÍGRAFE
𝑆 = 𝑘 ∙ ln(𝑊)
Ludwig Eduard Boltzmann
13
14
RESUMO
Dzak. C. W. Implementação de controlador iLQR em braço robótico com quatro graus de liberdade: Estudo da função custo, linearização de trajetória e convergência. 2019. Monografia (Trabalho de Conclusão de Curso) – Escola de Engenharia de São Carlos, Universidade de São Paulo, São Carlos, 2019.
O regulador quadrático linear iterativo (iLQR) é implementado em um braço
robótico para se investigar as vantagens e desvantagens do seu uso em uma
aplicação genérica. Afinal, braços robóticos são amplamente utilizado e a sua
dinâmica, apesar da complexidade matemática, é simples se comparado com outros
modelos da literatura, como robôs humanoides. É também um tipo de robô
frequentemente utilizado devido a sua versatilidade e, consequentemente, bastante
estudado sob a ótica da cinemática inversa. O presente trabalho investiga, então, uma
diferente perspectiva, preterindo modelos mecânicos e associando ao modelo uma
função custo. Assim, um modelo é desenvolvido no Framework de Modelagem e
Simulação de Sistemas Dinâmicos MuJoCo e, ao mesmo tempo, o algoritmo iLQR e
as ferramentas matemáticas necessárias também são implementados. Em seguida,
estuda-se o algoritmo em si, analisando o significado físico da função custo e as
condições de convergência do método. Assim, foi possível verificar a dependência da
distância do objetivo, avaliar uma possível paralelização e a possibilidade da
implementação do algoritmo iLQR em situações reais, levando em conta desempenho
e o cumprimento do objetivo.
Palavras chave: iLQR, controle ótimo, braço robótico
15
16
ABSTRACT
Dzak. C. W. Implementação de controlador iLQR em braço robótico com quatro
graus de liberdade: Estudo da função custo, linearização de trajetória e
convergência. 2019. Monografia (Trabalho de Conclusão de Curso) – Escola de
Engenharia de São Carlos, Universidade de São Paulo, São Carlos, 2019.
We implemented the iterative Linear Quadratic Regulator (iLQR) for a robotic
arm to study its advantages and disadvantages in a generic application. Robotic arms
are widely used and its dynamics, despite its mathematical complexity, they are simpler
than other models found in the literature, such as humanoid robots. It is also a kind of
robot frequently employed due to its versatility and, consequently, often studied under
inverse kinematics perspective. Therefore, the present study investigates a different
viewpoint, avoiding mechanical modeling and associating the model to a cost function.
Hence, to that end, we developed a model in the Modeling and Dynamical Systems
Simulation Framework MuJoCo and, at the same time, the iLQR algorithm and the
required mathematical methods are also implemented. Afterwards, we study the
algorithm itself, interpreting the physical meaning of the cost function and its
convergence conditions. Thus, we could verify the dependency of the distance to the
objective; evaluate a potential parallelization and the possibility of a real situation
implementation of the iLQR algorithm, considering performance and goal achievement.
Keywords: iLQR, optimal control, robotic arm.
17
LISTA DE ILUSTRAÇÕES
Figura 1 - Esquematização do modelo fork-join utilizado pelo OpenMP ................... 29
Figura 2 - Fluxograma do algoritmo iLQR ................................................................. 40
Figura 3 - Braço robótico numa posição arbitrária, indicando os graus de liberdade,
isto é, um grau de liberade em vertical e 3 horizontais. ............................................ 42
Figura 4 - Posição inicial do braço robótico. Ele é inicialmente declarado na posição
vertical e em seguida perturbado para sair da posição de singularidade. ................. 50
Figura 5 - Braço robótico perturbado, cujo estado é o estado inicial do algoritmo para
qualquer posição desejada da ponta do braço. ......................................................... 50
Figura 6 – Situação A, posição x = 2,5; y = 2,5; z = 2,5 ............................................ 51
Figura 7 – Situação B, posição x = 0, y = 3, z = 4 ..................................................... 51
Figura 8 – Situação C, posição x = 1; y = -3; z = 4 .................................................... 51
Figura 9 – Situação D, posição x = -2, y = -3, z = 4 .................................................. 51
Figura 10 - Evolução da simulação da situação A ..................................................... 52
Figura 11 - Evolução da simulação da situação B ..................................................... 52
Figura 12 - Evolução da simulação da situação C .................................................... 52
Figura 13 - Evolução da simulação da situação D .................................................... 52
Figura 14 - Custos [sem unidade] das trajetórias ao longo do tempo [s]. .................. 53
18
19
LISTA DE TABELAS
Tabela 1 - Derivadas matriciais e suas notações ...................................................... 32
Tabela 2 - Definições de derivadas matriciais ........................................................... 32
Tabela 3 - Braço robótico atingindo algumas posições arbitrariamente escolhidas,
aumentando a distância e mudando o quadrante. Toma-se a origem como o centro
da base do robô; z o eixo vertical da renderização; x a vertical da página; y a
horizontal da página. ................................................................................................. 51
Tabela 4 - Evolução do alcance do objetivo ao longo do tempo em segundos. ........ 52
Tabela 5 - Comparação do desempenho para números de threads diferentes ........ 54
20
21
LISTA DE SIGLAS E ABREVIATURAS
BLAS Basic Linear Algebra Subprograms
DDP Differential Dynamic Programming
IDE Integrated Development Environment
iLQG Iterative Linear Quadratic Gaussian
iLQR Iterative Linear Quadratic Regulator
LAPACK Linear Algebra Package
LQG Linear Quadratic Gaussian
LQR Linear Quadratic Regulator
MATLAB Matrix Laboratory
MKL Math Kernel Library
MuJoCo Multi-Joint dynamics with Contact
OpenMP Open Multi-Processing
22
23
Sumário
CAPA ........................................................................................................................................................ 0
FICHA CATALOGRÁFICA ........................................................................................................................... 4
FOLHA DE AVALIAÇÃO OU APROVAÇÃO ................................................................................................. 6
DEDICATÓRIA .......................................................................................................................................... 8
AGRADECIMENTOS................................................................................................................................ 10
EPÍGRAFE ............................................................................................................................................... 12
RESUMO ................................................................................................................................................ 14
ABSTRACT .............................................................................................................................................. 16
LISTA DE ILUSTRAÇÕES .......................................................................................................................... 17
LISTA DE TABELAS .................................................................................................................................. 19
LISTA DE SIGLAS E ABREVIATURAS ........................................................................................................ 21
Sumário ................................................................................................................................................. 23
1. Introdução ..................................................................................................................................... 25
1.1. Motivação .............................................................................................................................. 25
2. Objetivos ....................................................................................................................................... 26
3. Revisão bibliográfica ..................................................................................................................... 27
3.1. Ferramentas computacionais utilizadas ................................................................................ 27
3.1.1. MATLAB ......................................................................................................................... 27
3.1.2. Microsoft Visual Studio ................................................................................................. 28
3.1.3. OpenMP......................................................................................................................... 28
3.1.4. Math Kernel Library (MKL) ............................................................................................ 29
3.1.5. MuJoCo .......................................................................................................................... 30
3.2. Fundamentos matemáticos .................................................................................................. 31
3.2.1. Cálculo diferencial de matrizes ..................................................................................... 31
3.2.2. Diferenciação por diferenças finitas ............................................................................. 32
3.2.3. Heurística de Levenberg–Marquardt ............................................................................ 33
3.2.4. Inversão de uma matriz simétrica por decomposição em autovalores e autovetores. 34
3.3. Fundamentos de sistemas de controle ................................................................................. 35
3.3.1. Sistemas não lineares .................................................................................................... 35
3.3.2. Linearização de uma trajetória...................................................................................... 35
3.4. O algoritmo iLQR ................................................................................................................... 37
4. Metodologia .................................................................................................................................. 41
4.1. Programação ......................................................................................................................... 42
4.1.1. MATLAB ......................................................................................................................... 42
24
4.1.2. Microsoft Visual Studio ................................................................................................. 43
4.2. Parâmetros e características da simulação ........................................................................... 44
4.3. Testes .................................................................................................................................... 48
5. Resultados ..................................................................................................................................... 48
6. Discussão ....................................................................................................................................... 55
7. Conclusão ...................................................................................................................................... 56
8. Referências .................................................................................................................................... 57
25
1. Introdução
1.1. Motivação
Entende-se por controle ótimo a área da Teoria do Controle que visa
otimização de uma função custo (ou função objetivo) de um sistema dinâmico durante
um dado período de tempo. Nesse contexto, encontra-se inicialmente o problema de
controle LQG (Linear-Quadratic-Gaussian), que se refere a sistemas dinâmicos
lineares influenciados por ruído branco aditivo e gaussiano. O problema LQG, por sua
vez, pode ser divido em duas partes: o filtro de Kalman (ou LQE, isto é, linear-quadratic
state estimator) e o problema de controle LQR (Linear-Quadratic Regulator). O
primeiro visa estimar, através de ferramentas estatísticas, o estado real do sistema
dinâmico por um observador afetado por ruído branco aditivo gaussiano, enquanto
que o segundo trata da operação e controle de um sistema dinâmico linear associado
a uma função quadrática de custo. Em outras palavras, a solução do problema LQR é
a ação de controle que minimiza a função custo associada a um sistema dinâmico.
Por outro lado, a maioria dos problemas reais não pode ser descrita por
simples sistemas lineares, o que limita a aplicação de controles como os descritos
acima. Dito isso, paralelamente ao iLQG (iterative Linear Quadratic Gaussian), o iLQR
(iterative Linear-Quadratic Regulator) pode ser considerado uma expansão do
problema LQR, isto é, ele visa a operação e controle de sistemas dinâmicos não
lineares associados a uma função quadrática de custo. O iLQR expande, então, as
aplicações de controladores LQR para dinâmicas muito mais ricas e complexas, como,
por exemplo, um braço com dois graus de liberdade no plano, porém atuado por
músculos, cuja dinâmica é altamente não linear (LI; TODOROV, 2011).
Dessa forma, procura-se investigar as capacidades e limitações do algoritmo
iLQR, em especial, em um cenário que pode ser aplicado numa situação real. Assim,
dentre as aplicações da robótica na indústria, foi tomado, então, um braço robótico
como exemplo, com o objetivo de estabelecer metas para o algoritmo e verificar os
resultados. Isto é, pode-se criar exigências reais para determinar a eficácia do método.
Além disso, braços robóticos são amplamente utilizados, o que significa que é possível
atingir uma grande fatia da robótica na indústria.
26
2. Objetivos
Sabendo do potencial da ferramenta em questão, procura-se investigar o
custo computacional do uso desse método, as dificuldades na sua implementação,
suas vantagens e desvantagens em relação a outros métodos, especificamente no
controle de cadeias cinemáticas e a eficácia do método em atingir um determinado
objetivo.
Para isso, será necessário:
Avaliar parâmetros da linearização da trajetória;
Características do simulador (ou motor de física) utilizado, ou seja, o
ambiente de modelagem;
Analisar as diferenças entre o objetivo desejado e o objetivo alcançado
pelo método.
27
3. Revisão bibliográfica
Neste capítulo, serão expostos as principais ferramentas computacionais
utilizadas e fundamentos teóricos, considerando o que é abordado durante o curso de
graduação. Devido a isso, serão abordados temas com os quais o leitor pode já estar
familiarizado.
3.1. Ferramentas computacionais utilizadas
Nessa seção serão expostos os softwares utilizados, dando ênfase àqueles
relacionados diretamente aos cálculos matemáticos.
3.1.1. MATLAB
Matlab é um software de computação numérica desenvolvido pela
MathWorks, para cálculos envolvendo matrizes. É um software de alta performance,
cujos principais atrativos são:
Operações matriciais implementadas de forma simples, intuitiva e em
linguagem de alto nível;
Funções simples e poderosas para plotagem de gráficos;
Por ser um dos principais softwares de computação numérica no
mercado, conta com conectividade simplificado para o usuário com
diversas ferramentas.
O MATLAB oferece um ambiente extremamente propício para projetos de
engenharia devido à facilidade de utilização e à variedade de ferramentas
matemáticas disponíveis. Entretanto, isso tem um custo que se reflete na
performance. O MATLAB utiliza uma linguagem interpretada, isto é, dispensa a
necessidade de compilação (conversão do código inteiro para código de máquina),
porém um interpretador é usado para converter cada linha de código num outro
código, geralmente código de máquina. Isso torna a execução de um código em
MATLAB mais lenta em relação a linguagens compiladas como o C e o C++. Ainda
assim, juntamente com suas ferramentas matemáticas, o MATLAB é um software
muito maleável nas mãos do usuário, de forma que é possível obter um algoritmo
funcionando de forma rápida e precisa para provar um determinado conceito.
28
O MATLAB foi utilizado juntamente com o MuJoCo HAPTIX (ver seção 3.1.5.
MuJoCo), como um primeiro passo na modelagem, para verificar o funcionamento do
algoritmo iLQR.
3.1.2. Microsoft Visual Studio
O Microsoft Visual Studio é uma IDE (Integrated Development Environment),
isto é, um ambiente de desenvolvimento integrado que suporta uma variada gama de
linguagens de programação, dentre elas, C++, que foi a linguagem utilizada. Possui
ferramentas de suporte para o usuário, como o IntelliSense, que traz sugestões de
auto completar durante a programação e ferramentas de debug, capazes de acessar
dados de ponteiros.
O Microsoft Visual Studio foi utilizado como IDE principal, onde foram
utilizadas as bibliotecas expostas nas seções seguintes.
3.1.3. OpenMP
OpenMP (Open Multi-Processing) é um conjunto aberto de sub-rotinas
destinadas à paralelização do código de um programa, nas linguagens C, C++ e
Fortran. Além disso, está implementado para várias arquiteturas e sistemas
operacionais, incluindo Linux, Windows e macOS.
OpenMP utiliza o modelo fork-join, em que uma thread, chamada thread
mestre, executa as tarefas de forma serial, até encontrar uma diretiva de paralelização
(fork). Nesse momento, as tarefas são distribuídas entre os membros de um time
threads, executadas paralelamente, até encontrarem uma diretiva de barreira. Nesse
ponto, as threads são sincronizadas, se juntam à thread mestre e o time de threads é
disperso.
29
Figura 1 - Esquematização do modelo fork-join utilizado pelo OpenMP
Esse modelo é interessante pois os times de threads podem ser definidos
durante a execução e, como mostrado no imagem, uma thread escravo também pode
criar threads escravos, tornando-se mestre desse time de threads.
Em especial, é importante salientar que o algoritmo iLQR é altamente serial,
isto é, a execução de uma etapa qualquer depende do resultado da etapa anterior.
Isso, a priori, desencorajaria a paralelização do algoritmo. Entretanto, uma das etapas,
o cálculo de diferenças finitas, é paralelizável e realizado diversas vezes no algoritmo.
Por ser uma das tarefas mais dispendiosas, torna-se, então, vantajoso o uso de multi-
processamento.
3.1.4. Math Kernel Library (MKL)
MKL é uma biblioteca de operações matemáticas otimizadas, que inclui sub-
rotinas destinadas a resolução de problemas computacionais de grande porte. Entre
suas sub-rotinas, estão, por exemplo, BLAS (Basic Linear Algebra Subprograms),
LAPACK (Linear Algebra Package) para álgebra linear, funções para Redes neurais
profundas, Transformadas de Fourier, geradores de números aleatórios e matemática
vetorizada.
Essas bibliotecas são especialmente necessárias, primeiramente, porque o
trabalho foi desenvolvido em C++, que não possui nativamente rotinas matemáticas
sequer para multiplicação de matrizes. Em segundo lugar, as bibliotecas tipo BLAS e
LAPACK (variam um pouco, pois são desenvolvidas por várias entidades, como o
OpenBLAS e o MKL, mas são intercambiáveis) são otimizadas em nível de código de
máquina para álgebra linear. Isso significa que muito dificilmente seria possível
implementar uma operação, já presente nessas bibliotecas, de forma mais eficiente.
30
Assim, o MKL foi utilizado para a álgebra linear existente no algoritmo iLQR,
isto é: multiplicações de matrizes, multiplicação de matrizes e vetores e autovalores e
autovetores de matrizes simétricas (usando redução para uma matriz tridiagonal e
algoritmo QR)
3.1.5. MuJoCo
MuJoco (Multi-Joint Dynamics with Contact) é um motor de física, isto é, um
programa que simula aproximadamente sistemas físicos, englobando, em geral,
corpos rígidos, detecção de colisão, corpos macios e mecânica dos fluidos. O MuJoCo
em especial é utilizado para simular com precisão e sistemas dinâmicos complexos
com alto desempenho. É constituído de bibliotecas dinâmicas com interface para C,
de forma a maximizar o desempenho, compatíveis com Windows, Linux e macOS
(TODOROV; EREZ; TASSA, 2012). Dentre os principais atrativos do MuJoCo, para o
presente projeto, destacam-se:
Coordenadas generalizadas (ou espaço de juntas) com dinâmicas de
contato;
O MuJoCo permite gerar modelos dinâmicos espaciais complexos, com
elevado número de graus de liberdade, mantendo a precisão, pois utiliza coordenadas
generalizadas, em contraposição a relações cinemáticas impostas numericamente.
Os principais motores de física do mercado ou utilizam coordenadas generalizadas,
mas modelos de contato demasiadamente simplificados; ou utilizam modelos
modernos de contato, mas coordenadas cartesianas. Nesse aspecto, o MuJoCo é
capaz de utilizar, para a simulação, coordenadas generalizadas e modelos modernos
de contato com múltiplos pontos de contato na presença de atrito.
Modelo compilado;
Para inserir um modelo no MuJoCo, é necessário escrevê-lo em formato .xml
e importa-lo durante a execução. Ao fazer isso, o MuJoCo gera um struct (estrutura
de dados) em C, com as principais informações do modelo. Isso confere à simulação
bastante velocidade, uma vez que as informações estão disponíveis em níveis muito
próximos ao código de máquina, se comparado a uma aplicação em MATLAB, por
exemplo.
31
Multi-processamento e separação de modelo e dinâmica;
O MuJoCo separa uma simulação em duas partes: mjModel e mjData. O
primeiro contém as principais informações que descrevem um modelo (como número
de graus de liberdade, número de estados, tipos de objetos dentro da simulação...),
enquanto que o segundo contém informações da dinâmica do sistema (como
posições, velocidades, acelerações, tempo da simulação...). Isso pode ser utilizado
para multi-threading, pois um único mjModel pode ter vários mjData, o que protege a
simulação principal contra o acesso por mais de uma thread.
Além disso, como citado anteriormente, há também o MuJoCo HAPTIX, uma
variante do software MuJoCo. Trata-se de uma ferramenta mais simples, mas
bastante conveniente, que se conecta ao MATLAB e ao C de forma simples e preserva
as principais funcionalidades do MuJoCo em C. A utilização do MuJoCo HAPTIX com
o MATLAB permite ao usuário definir estados e sinais de controle para a simulação,
assim como obter essas informações de uma simulação em curso. Não é possível,
entretanto, trabalhar com mais de uma simulação ao mesmo tempo. Assim, apesar
disso, do desempenho limitado também pelo atraso da comunicação e pela velocidade
de processamento reduzida no MATLAB, foi possível usar essa ferramenta para os
testes iniciais do algoritmo iLQR.
3.2. Fundamentos matemáticos
Nessa seção serão comentados rapidamente algumas ferramentas
matemáticas empregadas, cuja importância sobressai para o entendimento do
algoritmo iLQR.
3.2.1. Cálculo diferencial de matrizes
A diferenciação de matrizes, provavelmente intuitiva para o leitor, foi, logo no
início do projeto, um desafio para o autor. Nessa seção, não serão abordadas as
demonstrações, mas apenas as equações e conceitos principais.
Primeiramente, podemos dividir as diferenciações quanto existência de
definição ou não, como na Tabela 1, onde são mostradas também as suas notações:
32
Tabela 1 - Derivadas matriciais e suas notações
Tipo Escalar Vetor Matriz
Escalar 𝑑𝑦
𝑑𝑥
𝑑𝒚
𝑑𝑥
𝑑𝒀
𝑑𝑥
Vetor 𝑑𝑦
𝑑𝒙
𝑑𝒚
𝑑𝒙 -
Matriz 𝑑𝑦
𝑑𝑿 - -
Assim, temos também:
Tabela 2 - Definições de derivadas matriciais
𝑑𝑦
𝑑𝑥
𝑑𝒚
𝑑𝑥=
[ 𝑑𝑦1𝑑𝑥⋮𝑑𝑦𝑛𝑑𝑥 ]
𝑑𝑦
𝑑𝒙=
[ 𝑑𝑦
𝑑𝑥1⋮𝑑𝑦
𝑑𝑥𝑛]
𝑑𝒚
𝑑𝒙=
[ 𝑑𝑦1𝑑𝑥1
⋯𝑑𝑦𝑚𝑑𝑥1
⋮ ⋱ ⋮𝑑𝑦1𝑑𝑥𝑛
…𝑑𝑦𝑚𝑑𝑥𝑛 ]
𝑑𝑦
𝑑𝑿=
[ 𝑑𝑦
𝑑𝑥1,1⋯
𝑑𝑦
𝑑𝑥1,𝑛⋮ ⋱ ⋮𝑑𝑦
𝑑𝑥𝑚,1…
𝑑𝑦
𝑑𝑥𝑚,𝑛]
𝑑𝒀
𝑑𝑥=
[ 𝑑𝑦1,1𝑑𝑥
⋯𝑑𝑦1,𝑛𝑑𝑥
⋮ ⋱ ⋮𝑑𝑦𝑚,1𝑑𝑥
…𝑑𝑦𝑚,𝑛𝑑𝑥 ]
3.2.2. Diferenciação por diferenças finitas
Diferenças finitas é um método numérico para a obtenção da aproximação
das derivadas de uma função quando esta não pode ser escrita analiticamente.
De modo geral, em relação às derivadas de primeira ordem, para uma função
de apenas uma variável, ou funções multivariáveis, temos as equações:
33
Que são conhecidas como diferenças finitas centradas.
Para derivadas de segunda ordem funções multivariadas, podemos empregar
a fórmula:
Para casos que tratam de derivadas cruzadas de segunda ordem.
Em especial, para uma derivada dupla, a equação se reduz para:
3.2.3. Heurística de Levenberg–Marquardt
O algoritmo de Levenberg–Marquardt é um método para obtenção de mínimos
locais que pode ser entendido com uma junção de dois métodos: o método dos
gradientes, Equação (5); e o método de Gauss-Newton, Equação (6).
Em que 𝛾 é uma constante positiva, H representa a matriz Hessiana, ∇
representa gradiente e u representa a variável iterada.
O método dos gradientes apresenta convergência de primeira ordem,
enquanto que o método de Gauss-Newton apresenta convergência de segunda
ordem. O algoritmo de Levenberg–Marquardt propõe utilizar uma mistura dos dois
métodos, com base no erro da iteração. Isto é, se o erro for pequeno, tende-se mais
para o método de Gauss-Newton e se o erro for grande, tende-se para o método dos
gradientes. Assim, o algoritmo torna-se, então:
𝑑𝑓
𝑑𝑥=𝑓(𝑥 + ℎ) − 𝑓(𝑥 − ℎ)
2ℎ
(1)
𝜕𝑓(𝑥, 𝑦)
𝜕𝑥=𝑓(𝑥 + ℎ, 𝑦) − 𝑓(𝑥 − ℎ, 𝑦)
2ℎ
(2)
𝜕𝑓(𝑥, 𝑦)
𝜕𝑥𝜕𝑦
=𝑓(𝑥 + ℎ, 𝑦 + 𝑘) − 𝑓(𝑥 − ℎ, 𝑦 + 𝑘) − 𝑓(𝑥 + ℎ, 𝑦 − 𝑘) + 𝑓(𝑥 − ℎ, 𝑦 − 𝑘)
4ℎ𝑘
(3)
𝜕𝑓(𝑥, 𝑦)
𝜕𝑥2=𝑓(𝑥 + ℎ, 𝑦 + 𝑘) − 2𝑓(𝑥, 𝑦) + 𝑓(𝑥 − ℎ, 𝑦 − 𝑘)
ℎ2
(4)
𝑢𝑘+1 = 𝑢𝑘 − 𝛾∇𝑓(𝑢𝑘) (5)
𝑢𝑘+1 = 𝑢𝑘 − [Η𝑓(𝑢𝑘)]−1∇𝑓(𝑢𝑘)
(6)
34
Em que λ é um parâmetro de regularização e D é uma matriz com apenas os
elementos diagonal da matriz Hessiana. Nesse momento, fazemos também uma
observação quanto a esse tópico: no entendimento do autor, o algoritmo propriamente
dito de Levenberg–Marquardt é utilizado no problema de mínimos quadrados não
lineares; já a heurística se refere ao uso de um parâmetro de regularização que ajusta
a ordem de convergência de um determinado método numérico.
3.2.4. Inversão de uma matriz simétrica por decomposição em
autovalores e autovetores.
Entende-se por autovetores de uma transformação A, as direções que não
são alteradas ao se aplicar essa transformação A. Além disso, ao aplicar essa
transformação, a direção de um autovetor v é multiplicada pelo autovalor λ associado
a esse autovetor.
A decomposição em autovalores e autovetores de uma matriz quadrada A n x
n, com n autovetores linearmente independentes é da forma:
Em que Λ é uma matriz diagonal com os autovalores de A e Q é uma matriz
tal que suas colunas são os autovetores de A.
Assim, a inversa de A é simplesmente:
Como Λ-1 é uma matriz diagonal, temos simplesmente que:
Em especial, para uma matriz simétrica A, temos:
𝑢𝑘+1 = 𝑢𝑘 − [Η𝑓(𝑢𝑘) + 𝜆𝐷)]−1∇𝑓(𝑢𝑘)
(7)
𝐴 = 𝑄Λ𝑄−1 (8)
𝐴−1 = 𝑄Λ−1𝑄−1 (9)
Λ−1𝑖𝑖 =1
𝜆𝑖 (10)
𝐴−1 = 𝑄Λ−1𝑄𝑇 (11)
35
3.3. Fundamentos de sistemas de controle
Nessa seção serão discutidos alguns conceitos importantes para o algoritmo
iLQR.
3.3.1. Sistemas não lineares
São considerados não lineares os sistemas que não admitem superposição
dos efeitos, ou seja, não respeitam 2 condições: aditividade e homogeneidade
(FRANKLIN; POWELL; EMAMI-NAEINI, 1994). Ambos princípios podem ser
transcritos nas igualdades (12) e (13):
Em outras palavras, sistemas não lineares não podem ser escritos na forma
da Equação (14), devendo ser descritas na forma da Equação (15)
Tais sistemas podem ser linearizados em torno de um ponto de equilíbrio,
utilizando a expansão da Série de Taylor até a primeira ordem. O sistema linearizado
passa, então, a ser função de perturbações em torno do ponto de equilíbrio. Assim,
tem-se:
Em que TOS seriam os termos de ordem superior.
3.3.2. Linearização de uma trajetória
Dado um sistema dinâmico não linear que percorre uma trajetória nominal
x*(t), a partir de um estado inicial x0 e um sinal de controle nominal u*(t), temos que
(FARRELL, 2006):
𝑓(𝑥 + 𝑦) = 𝑓(𝑥) + 𝑓(𝑦) (12)
𝑓(𝛼𝑥) = 𝛼𝑓(𝑥) (13)
�̇� = 𝐴𝑥 + 𝐵𝑢 (14)
�̇� = 𝑓(𝑥, 𝑢) (15)
𝑥0̇ + 𝛿�̇� = 𝑓(𝑥0, 𝑢0) + [𝜕𝑓
𝜕𝑥]𝑥0𝛿𝑥 + [
𝜕𝑓
𝜕𝑥]𝑢0𝛿𝑢 + 𝑇𝑂𝑆 (16)
𝛿�̇� = [𝜕𝑓
𝜕𝑥]𝑥0
𝛿𝑥 + [𝜕𝑓
𝜕𝑥]𝑢0
𝛿𝑢 (17)
𝑥 ∗̇(𝑡) = 𝑓(𝑥∗(𝑡), 𝑢∗(𝑡)) (18)
36
Escrevendo a função dinâmica em função de pequenos desvios em relação à
trajetória nominal, temos;
Fazendo a expansão em Série de Taylor de 𝑓(𝑥∗ + 𝛿𝑥, 𝑢∗ + 𝛿𝑢), ao redor de
x*(t), u*(t) e truncando os termos de ordem superior,
Considerando agora f em função dos desvios em relação à trajetória nominal,
podemos reescrever, fazendo z = δx:
No algoritmo iLQR, em especial, utiliza-se a trajetória linearizada na forma de
equação de recorrência (ou equação à diferenças), isto é:
Assim, usando o método de Euler na Equação (22) para aproximar a
sequência da Equação (24),
Em que dt denota o passo da integração no tempo, ou o intervalo de
amostragem e as matrizes A e B são as derivadas a cada instante de tempo.
𝑥∗(0) = 𝑥0∗ (19)
𝛿�̇� = �̇� − 𝑥 ∗̇ = 𝑓(𝑥∗ + 𝛿𝑥, 𝑢∗ + 𝛿𝑢) − 𝑓(𝑥∗, 𝑢∗) (20)
𝑓(𝑥∗ + 𝛿𝑥, 𝑢∗ + 𝛿𝑢)
= 𝑓(𝑥∗, 𝑢∗) +𝜕𝑓
𝜕𝑥(𝑥∗, 𝑢∗)𝛿𝑥 +
𝜕𝑓
𝜕𝑢(𝑥∗, 𝑢∗)𝛿𝑢
(21)
�̇� = 𝐴(𝑡)𝑧 + 𝐵(𝑡)𝑢 (22)
𝐴 =𝜕𝑓
𝜕𝑥(𝑥∗, 𝑢∗), 𝐵 =
𝜕𝑓
𝜕𝑢(𝑥∗, 𝑢∗) (23)
𝑥𝑘+1 = 𝑓(𝑥𝑘, 𝑢𝑘) (24)
𝑥𝑘+1 = 𝑥𝑘 + (𝑑𝑡 ∗ 𝐴(𝑡))𝑥𝑘, +𝑑𝑡 ∗ 𝐵(𝑡)𝑢𝑘 (25)
𝑥𝑘+1 = (𝐼 + 𝑑𝑡 ∗ 𝐴(𝑡))𝑥𝑘, +𝑑𝑡 ∗ 𝐵(𝑡)𝑢𝑘 (26)
37
3.4. O algoritmo iLQR
Analogamente ao LQR simples (KIRK, 1971), cujo objetivo é, dado uma
sistema dinâmico que pode ser descrito por:
E associado à uma função de custo da forma:
Dada uma ação de controle u, tal que minimiza o custo J:
O iLQR, por sua vez, expande esse conceito para sistemas não lineares, isto,
é, dado um sistema dinâmico da Equação (30), cuja trajetória é descrita pela equação
de recorrências, equação (31):
Deseja-se encontrar a sequência de controles:
Que minimize o custo total J,
Em que lf denota o custo final e l(xi,ui) denota o custo calculado ao longo da
trajetória.
O iLQR é um algoritmo baseado em DDP (Differential Dynamic Programming),
ou seja, visa, a partir de uma trajetória inicial, encontrar a trajetória que minimize a
função custo. Assim, tem-se que:
�̇� = 𝐴𝑥 + 𝐵𝑢 (27)
𝐽 = ∫ (𝑥𝑇𝑄𝑥 + 𝑢𝑇𝑅𝑢)𝑑𝑡∞
0
(28)
𝑢 = −𝐾𝑥 (29)
�̇�(𝑡) = 𝑓(𝑥(𝑡), 𝑢(𝑡)) (30)
𝑥𝑘+1 = 𝑓(𝑥𝑘, 𝑢𝑘) (31)
𝑈 = 𝑢𝑘 = {𝑢0, 𝑢1, 𝑢2…𝑢𝑛−1} (32)
𝐽(𝑥, 𝑈) = 𝑙𝑓(𝑥𝑛) + ∑ 𝑙(𝑥𝑖,𝑢𝑖)
𝑁−1
𝑖=0
(33)
𝑉(𝑥, 𝑖) = min𝑢[𝑙(𝑥, 𝑢) + 𝑉(𝑓(𝑥, 𝑢), 𝑖 + 1)] (34)
38
A Equação (34) denota o cost-to-go ótimo, no instante i (TASSA; MANSARD;
TODOROV, 2014). Ela também é conhecida como Equação de Bellman. Com isso,
podemos também definir Q, tal que Q representa a mudança na função custo V’, isto
é V no instante i+1, em função de uma perturbação (δx, δu), cuja expansão de
segunda ordem é dada, pela Equação (36):
𝑄(𝛿𝑥, 𝛿𝑢) = 𝑙(𝑥 + 𝛿𝑥, 𝑢 + 𝛿𝑢) − 𝑙(𝑥, 𝑢) + 𝑉′(𝑓(𝑥 + 𝛿𝑥, 𝑢 + 𝛿𝑢))
− 𝑉′(𝑓(𝑥, 𝑢))
(35)
≈1
2[1𝛿𝑥𝛿𝑢] [1 𝑄𝑥
𝑇 𝑄𝑢𝑇
𝑄𝑥 𝑄𝑥𝑥 𝑄𝑥𝑢𝑄𝑢 𝑄𝑢𝑥 𝑄𝑢𝑢
] [1𝛿𝑥𝛿𝑢] (36)
𝑄𝑥 = 𝑙𝑥 + 𝑓𝑥𝑇𝑉′𝑥 (37)
𝑄𝑢 = 𝑙𝑢 + 𝑓𝑢𝑇𝑉′𝑥 (38)
𝑄𝑥𝑥 = 𝑙𝑥𝑥 + 𝑓𝑥𝑇𝑉′𝑥𝑥𝑓𝑥 + 𝑉′𝑥 ∙ 𝑓𝑥𝑥 (39)
𝑄𝑢𝑥 = 𝑙𝑢𝑥 + 𝑓𝑢𝑇𝑉′𝑥𝑥𝑓𝑥 + 𝑉′𝑥 ∙ 𝑓𝑢𝑥 (40)
𝑄𝑢𝑢 = 𝑙𝑢𝑢 + 𝑓𝑢𝑇𝑉′𝑥𝑥𝑓𝑢 + 𝑉′𝑥 ∙ 𝑓𝑢𝑢 (41)
𝑄(𝛿𝑥, 𝛿𝑢) = 𝑙(𝑥 + 𝛿𝑥, 𝑢 + 𝛿𝑢) − 𝑙(𝑥 + 𝛿𝑥, 𝑢 + 𝛿𝑢)
+ 𝑉′(𝑓(𝑥 + 𝛿𝑥, 𝑢 + 𝛿𝑢)) − 𝑉′(𝑓(𝑥, 𝑢))
(42)
Em que os termos à direita, das Equações (39), (40) e (41) denotam uma
contração tensorial. Como no algoritmo iLQR, é feita uma aproximação de primeira
ordem da trajetória, esses termos são desconsiderados. Além disso, fx e fu denotam
as derivadas parciais de f em relação à variável x e à variável u, respectivamente.
Minimizando a aproximação quadrática em (36), temos que a perturbação δu*
ótima é dada por:
Finalmente, resta apenas uma atualização do valor de V para cálculo do passo
anterior, i – 1, ou seja,
𝛿𝑢∗ =𝑎𝑟𝑔𝑚𝑖𝑛𝛿𝑢
𝑄(𝛿𝑥, 𝛿𝑢) = 𝑘 + 𝐾𝛿𝑥 (43)
𝑘 ≜ −𝑄𝑢𝑢−1𝑄𝑢 (44)
𝐾 ≜ −𝑄𝑢𝑢−1𝑄𝑢𝑥 (45)
39
Entretanto, como podemos observar, são utilizadas informações das
derivadas de segunda ordem iterativamente e recursivamente, o que aproxima o DDP
ao método de Gauss-Newton. De fato, o DDP possui também convergência de
segunda ordem, mas, como o iLQR faz uma linearização da trajetória em primeira
ordem (TASSA; EREZ; TODOROV, 2012), essa propriedade não é mais garantida.
Ainda assim, é necessário regularizar a matriz Quu, para melhorar as chance de
convergência do algoritmo. Isso se deve ao fato que se a matriz Quu não for definida
positiva, isto é, se todos os seus autovalores não forem positivos, o método pode
divergir (TASSA; EREZ; TODOROV, 2012). Em outras palavras, o método precisa
“apontar” para uma direção que diminua o erro. Existem na literatura alguns métodos
para essa regularização, mas no presente trabalho foi utilizada a heurística de
Levenberg–Marquardt.
Assim, o algoritmo, basicamente, em:
1. Forward rollout: Linearizar a trajetória no tempo, calculando também os
custos l, e suas derivadas lx e lxx;
2. Backward pass: Encontrar as matrizes de ganho ótimas k e K, através
das Equações (37) a (47).
3. Forward pass: calcular a mudança δu para a sequência de controle.
4. Avaliar a nova trajetória e atualizar parâmetro da regularização. Se o
erro atingir o critério de parada ou divergir, sair. Caso contrário, repetir
o algoritmo com a nova sequência de controle.
𝑉𝑥 = 𝑄𝑥 − 𝐾𝑇𝑄𝑢𝑢𝑘 (46)
𝑉𝑥𝑥 = 𝑄𝑥𝑥 − 𝐾𝑇𝑄𝑢𝑢𝐾 (47)
40
Figura 2 - Fluxograma do algoritmo iLQR
Em especial, é importante ressaltar, que primeiro passo do backward pass, as
matrizes Vx e Vxx são inicializadas, respectivamente, com lx e lxx.
É interessante também comentar a diferença entre as funções l, V e Q. Todas
elas são chamadas arbitrariamente de função custo, porém isso contém algumas
imprecisões.
Função custo l:
A função l é a função custo, propriamente dita. Ela recebe como parâmetros
o estado e ação de controle e retorna um valor associado a eles. Tomemos como
exemplo, o custo de uma viagem de carro, em que x é a nossa posição final em relação
ao objetivo; e u, nossa ação de controle, seria o consumo instantâneo de combustível.
Nossa função custo poderia ser, simplesmente:
Função cost-to-go V:
A função V é a função do custo “acumulado” associado ao tempo (ou iteração)
e ao estado, entre o estado i até o objetivo, isto é, uma função que avalia a qualidade
{𝑙 = 𝛼𝑢2
𝑙𝑓 = 𝛽𝑥2
(48)
41
da nossa trajetória {xi, xi+1, xi+2, ..., xf} e da ações de controle ainda que serão tomadas.
No nosso, exemplo, seria a previsão do consumo total de combustível, a partir da
posição atual, mais a distância, em que pararemos ao final da corrida, em relação ao
objetivo. Nesse momento, pode parecer estranha a soma de duas grandezas com
unidades distintas. Entretanto, torna-se evidente também o uso das constantes ou
pesos α e β na equação (48): podemos usá-las para converter o custo “instantâneo”
para uma grandeza comum, uma moeda, por exemplo.
Função Q
A função Q é, às vezes, chamada de pseudo-Hamiltoniano (TASSA;
MANSARD; TODOROV, 2014), em referência ao Hamiltoniano de sistemas contínuos.
Este por sua vez, é uma analogia ao Hamiltoniano da mecânica clássica, que pode
ser visto com a energia total do sistema. A função Q descreve uma mudança no valor
da função V, em função de pequenas alterações (δx, δu). Isto é, minimizar Q
significaria, minimizar o aumento do custo, que, por consequência, está na direção da
minimização do custo total da trajetória. Como dito anteriormente, é necessário que
Quu seja definida positiva. Isso significa, na verdade, que Q é uma função convexa em
relação a u e com um mínimo (e não um máximo).
4. Metodologia
Foi feito um modelo de um braço robótico com 4 graus de liberdade. Devido à
ausência de unidades nativas no MuJoCo, a maioria das suas propriedades
dinâmicas, como amortecimento e ganho, foram atribuídas de tal forma que o modelo
apresentasse, por exemplo, rigidez à perturbação e tempo de acomodação aceitáveis.
Isto é, foram atribuídos alguns valores iniciais, utilizado um sinal de controle qualquer
e a simulação executada, onde o modelo foi perturbado com as ferramentas
interativas do MuJoCo. Nesse processo, rigidez, overshoot, vibração e outros
parâmetros foram observados e ajustados através das propriedades dinâmicas do
modelo. As dimensões foram tomadas de forma que o modelo ficasse proporcional a
um robô real.
42
Figura 3 - Braço robótico numa posição arbitrária, indicando os graus de liberdade, isto é, um grau de liberade em vertical e 3 horizontais.
4.1. Programação
Como dito anteriormente, a simulação foi feita tanto no MATLAB com o
MuJoCo HAPTIX como no MuJoCo em C. Nessa seção, então, serão abordados
detalhes da implementação, levando em conta o acoplamento entre o algoritmo iLQR
e o software escolhido.
4.1.1. MATLAB
No MATLAB, a principal dificuldade seria obter as informações do estado, uma
vez que o HAPTIX foi desenvolvido como um simulador genérico. Assim, para isso,
foram utilizadas as funções:
mj_get_state, que retorna uma estrutura de dados (struct) com os
valores de posição e velocidade das juntas;
mj_set_state, que pode ser utilizada para atribuir os valores citados
anteriormente na simulação;
mj_get_control, que retorna uma estrutura de dados (struct) com o sinal
de controle;
E mj_set_control, que é utilizada para atribuir o sinal de controle;
mj_step, que faz uma integração no tempo, para obter o próximo
estado.
43
Essas funções foram utilizadas tanto para operar a simulação como para se
obter as derivadas da linearização da trajetória. Em outras palavras, basicamente,
utiliza-se o mj_get_state para se obter o estado da simulação num instante t, aplica-
se uma perturbação finita com mj_set_state, faz-se a integração no tempo, com
mj_step e obtém-se o estado com a perturbação com mj_get_state, novamente.
Finalmente, calcula-se a derivada em relação ao grau de liberdade perturbado, com a
Equação (1) e restaura-se o estado inicial do sistema. Em seguida, pode-se
implementar o algoritmo iLQR no MATLAB.
Como podemos ver, é bastante simples fazer a implementação no MATLAB.
Entretanto, além do desempenho reduzido, existem outras desvantagens ao se optar
por essa abordagem. Entre elas, tem-se:
Não é possível alterar o passo temporal da integração (mj_step)
durante a simulação. Isso significa que não podemos regular um
importante parâmetro da precisão da linearização durante a execução.
A função mj_step necessita que a simulação no HAPTIX esteja pausda,
caso contrário, retorna imediatamente sem alterar nada. Além disso,
com a simulação corretamente pausada, após ser chamada, calcular e
renderizar, ela espera 1 ms. Isso implica que: não pode-se perturbar
uma simulação em curso; e o desempenho é ainda mais reduzido com
os cálculos da renderização e a espera de 1 ms. No caso do presente
projeto, por exemplo, são 4 graus de liberdade, com 4 atuadores, o que
significam também 8 estados. Utilizando as diferenças finitas
centradas, isso significa 2 x (8 + 4) = 24 ms, no mínimo, de tempo morto
para a linearização de um único instante. Como se trata da linearização
de uma trajetória, iterativamente, isso se torna praticamente inviável,
no contexto computacional.
4.1.2. Microsoft Visual Studio
A implementação no Microsoft Visual Studio em C/C++ é capaz de oferecer
muito mais desempenho que em MATLAB, devido à própria natureza da linguagem e
à possibilidade de paralelização. Entretanto, existe também uma enorme dificuldade
para se superar, pois o C/C++ é uma linguagem complexa. Por outro lado, o MuJoCo
também disponibiliza nessa plataforma muito mais liberdade e ferramentas que no
44
HAPTIX, que se traduzem funções mais diversas e capazes de acessar muito mais
informações e até mesmo cálculos intermediários. As mais notórias são:
mj_forward(const mjModel *m, mjData *d):
Essa função calcula todas as variáveis necessárias para a integração de um
passo no tempo, a partir dos estados e dos controles, porém não faz a integração
propriamente dita. Em outras palavras, ela corresponde apenas à função dinâmica:
E retorna a derivada no tempo �̇� do vetor de estados.
mj_step(const mjModel *m, mjData *d):
Essa função realiza todas os cálculos realizados pela função mj_forward, mas
também integra no tempo, efetivamente, avançando a simulação (no tempo).
Normalmente, a integração é feita utilizando método de Euler, mas o método de
Runge-Kutta também pode ser escolhido.
Essas funções foram particularmente importantes, pois com mj_forward foi
possível calcular as derivadas lx e lxx da função custo de forma mais econômica (sem
a integração, como no MATLAB) e de forma mais precisa, afinal, as derivadas são em
relação somente aos estados. Fazer uma integração no tempo significaria variar um
parâmetro, o tempo, considerado constante nessa diferenciação. Além disso, como
citado anteriormente, o MuJoCo em C permite utilizar mais de uma simulação por
modelo. Isso é viabilizado ao criar vários mjData a partir do mesmo modelo, o que
permite aplicar perturbações diferentes e fazer a computação em paralelo.
4.2. Parâmetros e características da simulação
Dentre os aspectos mais importantes da simulação, podemos destacar:
A função custo;
Ainda que o objetivo da algoritmo iLQR seja reduzir o esforço do projeto do
controlador, através de uma função custo fisicamente mais fácil de se interpretar, isso
nem sempre é tão simples assim. A escolha da função custo impacta diretamente no
desempenho do algoritmo e pode até mesmo levá-lo a divergência. No nosso caso,
foi escolhida uma função simples, da forma:
�̇� = 𝑓(𝑥, 𝑢) (49)
45
Em que p e r representam os pesos dessas parcelas, sponta e sdesejado
representam, respectivamente a posição da ponta no instante final e a posição
desejada e o somatório é somatório da velocidade ao quadrado de todas juntas no
instante final.
Cálculo e paralelização das derivadas;
O OpenMP possui 2 principais diretivas de distribuição de tarefas de uma
região paralelizada do código: single, sections e for. Ou seja, utilizando single, o
programador especifica que uma parte do código será executada apenas pela thread
mestre; usando sections, o programador especifica explicitamente as seções
paralelas do código. No exemplo abaixo, o programador usa a diretiva “#pragma omp
parallel”, que cria a região paralela, e na região paralela, ele especifica quais e como
as tarefas serão consideradas um “bloco”, através da diretiva “#pragma omp sections”:
#pragma omp parallel { #pragma omp sections { #pragma omp section { // Primeira seção do código } #pragma omp section { // Segunda seção do código } #pragma omp section { // Terceira seção do código } ... }
}
Já a diretiva for, “#pragma omp for”, paraleliza cada iteração do laço for, de
acordo com uma segunda diretiva de agendamento, que podem ser:
{
𝑙 = 𝑝𝑢2
𝑙𝑥 = 2𝑝 ∙ 𝑎𝑏𝑠(𝑢)𝑙𝑥𝑥 = 2𝑝
𝑙𝑓 = 𝑟((𝑠𝑝𝑜𝑛𝑡𝑎 − 𝑠𝑑𝑒𝑠𝑒𝑗𝑎𝑑𝑜)2 + ∑ 𝑣𝑖2)𝐺𝐷𝐿𝑖=0
(50)
46
static;
As tarefas divididas entre as threads possuem tamanho fixo, isto, é, cada
thread irá executar um número fixo predeterminado de iterações.
dynamic;
As tarefas são divididas em um tamanho fixo, porém são atribuídas durante a
execução, ou seja, as tarefas são atribuídas às threads, essas tarefas são executadas,
e, ao terminá-las, a thread busca novas tarefas do tamanho predefinido. Ao contrário
do static, as tarefas não são atribuídas totalmente às threads logo no início. Dessa
forma, dynamic é melhor usado quando as iterações podem diferir sensivelmente
entre si.
guided;
É igual ao dynamic, porém com uma diferença: o tamanho das tarefas é
proporcional ao número de iterações faltantes e o usuário pode estabelecer o tamanho
mínimo. Como o dynamic, é adequado quando as cargas das iterações podem ser
diferentes entre si, mas tenta diminuir o overhead (cálculo não relacionado ao objetivo
do código, porém necessário para que ele funcione), pois atribui blocos maiores de
tarefas no início.
runtime;
Escolhe um dos tipos acima durante a execução do programa.
#pragma omp parallel { #pragma omp for // Diretiva de agendamento for (...) { // código } }
Dentre eles, o que possui menor overhead é o static, o que o torna, portanto,
nossa escolha primária. Entretanto, como dito no final da seção 3.1.3, o algoritmo
iLQR é bastante serial. As etapas backward pass e forward pass são multiplicações
de matrizes recursivamente, onde, portanto, não é possível paralelizar, com as
diretivas do OpenMP. Assim, resta apenas o forward rollout e a linearização da
trajetória. Grosso modo, fazemos uma perturbação na variável sendo diferenciada,
47
avançamos o estado e calculamos as diferenças finitas. Nesse momento, fazemos
uma importante observação: Normalmente, para fazer a integração, seria suficiente
fazer a integração de Euler utilizando a derivada no tempo do estados, ou seja:
Em que xk é o estado atual, cujas informações são diretamente acessadas em
mjData; e ẋk é a derivada no tempo do estados, calculada por mj_forward. Entretanto,
essa função não faz uma integração, logo, não inclui o amortecimento implícito
calculado na integração (TODOROV, 2014). Isso se deve ao fato de que existem duas
formas de implementar amortecimento no MuJoCo: como um controle proporcional à
velocidade da junta ou como uma propriedade da junta. O primeiro poderia ser
instalado em um call-back e calculado na pipeline da função mj_forward. Contudo,
segundo a documentação do MuJoCo, o segundo é preferível, pois aumenta a
estabilidade do método de Euler. Além disso, afirma-se que o amortecimento é
avaliado somente instante temporal seguinte. Dessa forma, para se considerar os
efeitos do amortecimento é necessário usar a função mj_step, que é um pouco mais
lenta. Assim, temos um trade-off entre estabilidade e performance.
Finalmente, é necessário introduzir também um conceito utilizado na
implementação do algoritmo iLQR:
Finite horizon: Dado um objetivo, tenta alcança-lo utilizando um
instante fixo no tempo. Assim, a simulação avança no tempo e a janela
de tempo para as iterações subsequentes do iLQR vão diminuindo
(TODOROV, 2006);
Entretanto, dada a natureza do problema do presente trabalho, optou-se por
fazer um horizonte proporcional à distância em relação ao objetivo, isto é:
Em que T é a duração do horizonte, n é o número de divisões do intervalo e
cada um com seus respectivos valores máximos e mínimos.
𝑥𝑘+1 = 𝑥𝑘 + 𝑥�̇�𝑑𝑡 (51)
{
𝑇 = 𝑇𝑚𝑎𝑥𝑠
𝑠𝑖, 𝑇𝑚𝑖𝑛 < 𝑇 < 𝑇𝑚𝑎𝑥
𝑛 = 𝑛𝑚𝑎𝑥𝑠
𝑠𝑖, 𝑛𝑚𝑖𝑛 < 𝑛 < 𝑛𝑚𝑎𝑥
(52)
48
4.3. Testes
Foram utilizados 3 tipos de atuadores presentes no MuJoCo:
Motor: O sinal de controle é uma saída de torque na junta;
Position: O sinal de controle determina uma posição. Possui apenas
um parâmetro, kp, que é o ganho do erro da posição.
Velocity: O sinal de controle determina a velocidade da junta. Possui
apenas um parâmetro, kv, é o ganho do erro da velocidade.
Com motor e position, foram feitas simulações para o ajuste manual dos
pesos, visando o melhor cumprimento da função objetivo. Isso não pode ser realizado
com o tipo velocity, pois, apesar de implementado, não foi obtida uma resposta
satisfatória ou algum indicativo de que valia a pena. Em especial, é importante
observar que a função custo varia de acordo com o atuador, isto é:
Para velocity, foi utilizada a função custo como descrito na Equação (50), mas
para position e o motor, foi utilizado:
De forma que o objetivo tornava-se, então, minimizar os deslocamentos.
Para o motor, em especial, era possível utilizar também a função custo (50),
sendo que a convergência é alcançada mais rápido. Porém, o objetivo, isto é a posição
desejada ficava bastante prejudicada.
Além disso, foram feitos testes de performance com e sem a paralelização,
para se verificar o ganho de desempenho.
5. Resultados
Como mencionado, o modelo em MATLAB foi utilizado, em geral, para
verificar o modelo do MuJoCo em C. Utilizou-se um modelo com atuador tipo position,
que converge em situações “fáceis”, isto é, objetivos próximos com linearizações não
muito grosseiras (intervalos de amostragem médios)
{
𝑙 = 𝑝(𝑢𝑘 − 𝑢𝑘−1)
2
𝑙𝑥 = 2𝑝 ∙ 𝑎𝑏𝑠(𝑢𝑘 − 𝑢𝑘−1)𝑙𝑥𝑥 = 2𝑝
𝑙𝑓 = 𝑟(𝑠𝑝𝑜𝑛𝑡𝑎 − 𝑠𝑑𝑒𝑠𝑒𝑗𝑎𝑑𝑜)2 + 𝑞∑ 𝑣𝑖2𝐺𝐷𝐿𝑖=0
(53)
49
Dentre os modelos utilizados, motor e position apresentaram os melhores
resultados. Motor em especial não só convergia com linearizações mais grosseiras,
como também atingiu melhor o resultado final. Em média, um atuador position
necessitava passos temporais 3 a 4 vezes menores para rodar, isto é, utilizou-se um
passo temporal de 6,7 ms com motor e 2,5 ms com position. Além disso, ainda que
função custo especificada na equação (53) tinha como objetivo minimizar os
deslocamentos, a simulação apresentava um comportamento imprevisto: O braço
robótico ficava inicialmente parado na maior parte do tempo e pouco depois da metade
do tempo decorrido, movia-se atingindo a posição final, porém retornava logo em
seguida para posição inicial ou próxima dela. Acredita-se que isso se deve a presença
de um controle intermediário, inerente do MuJoCo, entre o iLQR e a função dinâmica,
porém mais testes nesse tópico precisariam ser feitos.
50
Figura 4 - Posição inicial do braço robótico. Ele é inicialmente declarado na posição vertical e em seguida perturbado para sair da posição de singularidade.
Figura 5 - Braço robótico perturbado, cujo estado é o estado inicial do algoritmo para qualquer posição desejada da ponta do braço.
Como dito anteriormente, o objetivo da simulação era simplesmente atingir
uma dada posição, num dado intervalo de tempo. Na implementação, além do passo
temporal variante, indicado pelas equações (52), foi implementada uma condição de
“tolerância” para a posição. Isto é, se a distância da ponta do braço fosse maior que
um dado valor, o algoritmo seria executado.
51
Tabela 3 - Braço robótico atingindo algumas posições arbitrariamente escolhidas, aumentando a distância e mudando o quadrante. Toma-se a origem como o centro da base do robô; z o eixo vertical da renderização; x a vertical da página; y a horizontal da página.
Figura 6 – Situação A, posição x = 2,5; y = 2,5; z = 2,5
Figura 7 – Situação B, posição x = 0, y = 3, z = 4
Figura 8 – Situação C, posição x = 1; y = -3; z = 4
Figura 9 – Situação D, posição x = -2, y = -3, z = 4
52
Tabela 4 - Evolução do alcance do objetivo ao longo do tempo em segundos.
Figura 10 - Evolução da simulação da situação A
Figura 11 - Evolução da simulação da situação B
Figura 12 - Evolução da simulação da situação C
Figura 13 - Evolução da simulação da situação D
0
1
2
3
4
5
6
7
8
0 1 2 3 4 5
Situação A
Posição x Posição y
Posição z Distância
-2
-1
0
1
2
3
4
5
6
7
8
0 1 2 3 4 5
Situação B
Posição x Posição y
Posição z Distância
-4
-2
0
2
4
6
8
0 1 2 3 4 5
Situação C
Posição x Posição y
Posição z Distância
-4
-2
0
2
4
6
8
0 1 2 3 4 5
Situação D
Pos x: Pos y: Pos z: Distance:
53
Figura 14 - Custos [sem unidade] das trajetórias ao longo do tempo [s].
Nas imagens da Tabela 4, podemos ver as coordenadas do objetivo e a
convergência de cada uma delas. Em especial, verifica-se que as situações C e D
possuem mais oscilações e, inclusive em D, verifica-se que houve algo parecido com
um sobressinal mais pronunciado que nas outras situações. Na Figura 14, podemos
ver a evolução dos custos das trajetórias ao longo do tempo. Podemos ver que para
objetivos relativamente próximos, isto é, as curvas em azul e vermelho,
correspondentes às posições [2,5 2,5 2,5] e [0 3 4], respectivamente, o algoritmo
comporta-se bem. Verifica-se um pico, provavelmente, devido a algum problema na
convergência ou à própria programação dele. Como a simulação foi feita utilizando os
seguintes passos:
1. Verificar distância;
2. Ajustar número de amostragens da linearização e janela de tempo, de
acordo com a distância do objetivo;
3. Copiar sinal de controle ainda não executado para o início da
sequência de controle;
4. Executar algoritmo iLQR;
5. Repetir;
0
3.000.000
6.000.000
9.000.000
12.000.000
15.000.000
18.000.000
21.000.000
0 1 2 3 4 5
Custos ao longo do tempo
Custo A [2,5 2,5 2,5] Custo B [0 3 4] Custo C [1 -3 4] Custo D [-2 -3 4]
54
É possível que alguns parâmetros não atualizados com a distância, como os
pesos, causem interferências na convergência e na qualidade do resultado. Isto é, a
sequência é recalculada toda vez que o algoritmo é chamado. Isso emula um feedback
do erro, porém exigiria também um ajuste dos pesos, que não foi levado em
consideração nessa implementação. Isso significa que, na abordagem atual, o mesmo
peso é usado para calcular a sequência de controle mesmo que a simulação esteja
perto do objetivo. Verifica-se também, em contrapartida, que objetivos longes da
posição inicial, como as situações C e D, causam bastante oscilação na função custo.
Isso se deve a qualidade da linearização da trajetória, que fica prejudicada ao
aumentar a distância em relação ao objetivo. Assim, podemos interpretar a distância
física no modelo, também como uma distância matemática da solução ótima.
Foram também tomadas medidas de tempo antes e depois da chamada da
função que calcula as derivadas numéricas, variando-se o número de threads
empregadas. Foram feitas aproximadamente 100 amostragens por situação,
utilizando um computador com processador i7-7500U (4 núcleos, 2,7 GHz até 3,5
GHz) e 16 GB de memória RAM. Podemos ver, que para esse modelo, 1 thread
oferece o melhor desempenho. Provavelmente, isso se deve ao fato de que o modelo
utilizado é bem simples, com apenas 4 graus de liberdade. Isso significa que cada
thread seria responsável pela diferenciação de apenas 2 estados e 1 atuador. Nesse
caso, podemos concluir que o overhead não compensa a paralelização.
Adicionalmente, como comentado na seção 4.1.1. MATLAB, podemos ver que o
tempo morto de 24 ms é inviável em termos computacionais, o que mostra que a
escolha do uso da linguagem C/C++ foi bastante vantajosa.
Tabela 5 - Comparação do desempenho para números de threads diferentes
Número de threads Duração média [s] Tempo/min(tempo) [%]
1 0,016171303 100
2 0,017334976 107,195917
3 0,019987634 123,5994056
4 0,022286569 137,815541
55
6. Discussão
Como esperado, o algoritmo iLQR atinge o objetivo determinado, ignorando a
multiplicidade de soluções da cadeia cinemática. Como o braço robótico tinha 4 graus
de liberdade, mas, como apenas 3 coordenadas eram determinadas no objetivo, um
sistema com múltiplas soluções é formado. Entretanto, utilizando o iLQR, mais uma
condição de contorno é adicionada ao sistema, sendo ela a função custo. Em outras
palavras, existe apenas uma trajetória ótima que minimiza localmente o custo total. O
iLQR, entretanto, não é infalível. Por ser um método numérico iterativo, existem
condições para sua convergência. Adicionalmente, o iLQR encontra o mínimo local,
isto é, o mínimo mais próximo da trajetória inicial. Isso significa que o iLQR é
dependente de boas escolhas (de pesos, funções custo, critérios de convergência...),
boas condições iniciais e de funções “bem comportadas”. Nesse contexto, a passo
temporal é uma variável extremamente importante. Como o iLQR difere do DDP,
principalmente pela aproximação da trajetória, utilizando uma aproximação de
primeira e não de segunda ordem, para se manter a qualidade da aproximação, é
necessário diminuir o passo temporal. Um passo temporal muito largo irá causar
rapidamente a divergência do método, pois, como os cálculos são recursivos no
backward pass, o erro aumenta exponencialmente. Dito isso, torna-se, então, evidente
a importância da regularização com a heurística de Levenberg–Marquardt.
Finalmente, o uso do iLQR para o controle de um braço robótico em real-time
é uma opção que está fortemente atrelada ao hardware disponível. Como
demonstrado, as etapas do iLQR não são triviais. De modo geral, uma única iteração
do iLQR leva em média de 10 a 30 iterações. Tomando, então, 20 iterações com 100
linearizações e 1 thread (0,016 s), são aproximadamente 30 s para se gerar uma
sequência de controle. Naturalmente, esse valor diminui ou aumenta, no caso do
presente trabalho, de acordo com a distância, mas esse valor já impõe algumas
restrições. Por outro lado, pode ser efetivamente utilizado se a trajetória e o sinal de
controle puderem ser calculados previamente e executados independentemente,
numa situação de automação industrial, por exemplo. Além disso, a precisão do
posicionamento também precisa ser trabalhada. Suponha a situação: O braço robótico
está longe do objetivo e utilizamos um peso pf na posição final, e um peso pu no sinal
de controle. Utilizamos um pf alto e um pu baixo, pois queremos principalmente que o
braço chegue ao objetivo. Entretanto, com o braço robótico perto do objetivo, a
56
situação se inverte: queremos que o braço se mantenha na posição, com estabilidade
e sem vibrações, ou seja, minimizar a atuação, e como já estamos no objetivo, pf não
precisa ser tão alto. Além disso, as variações de ambos sinais sofrem uma escala.
Isto é, a amplitude do sinal de controle para movimentação pode ser ordens de
grandeza maior que para estabilização; e a distância inicial também pode bem maior
que a tolerância de posicionamento. Dessa forma, para um braço robótico, seria
interessante pelo menos que a função custo possa enxergar essa diferença e ajustar
adequadamente os pesos.
7. Conclusão
No presente projeto, foi demonstrado que o iLQR é capaz de superar não só
não linearidades, como também multiplicidade de soluções, encontrando, assim, uma
trajetória localmente ótima. Isso ocorre devido à associação da dinâmica do sistema
à uma função custo, que passa a ser a condição de contorno do projeto. Além disso,
o iLQR reduz o esforço do projetista, pois, uma vez implementado o algoritmo, ele
pode ser utilizado para modelos diferentes. Isso se deve ao fato de que o algoritmo
não depende da física do modelo, isto é, baseia-se na otimização do ponto de vista
matemático. Assim, é possível reutilizar o algoritmo em outros modelos apenas
ajustando a função custo e o horizonte da simulação. Especificamente quanto ao
braço robótico, vê-se que, com o atual parâmetro de regularização, o iLQR necessita
de alto poder de processamento, o que dificulta aplicações em real-time. Ao mesmo
tempo, aplicações industriais são muitas vezes repetitivas, o que significa que uma
trajetória pode ser previamente calculada e aplicada nessa situação. Dessa forma,
mostra-se que o iLQR é uma ferramenta bastante versátil para o controle de sistemas
dinâmicos não lineares, mas com aplicação ainda limitada pelo hardware atual
57
8. Referências
FARRELL, J. A. ADAPTIVE APPROXI MATlON Unifying Neural , Fuzzy and
Traditional Adaptive Approximation Approaches. [s.l: s.n.]
FRANKLIN, G.; POWELL, J. D.; EMAMI-NAEINI, A. Feedback control of dynamic
systems, 3e. In: American Society of Mechanical Engineers, Dynamic Systems and
Control Division (Publication) DSC, Anais...1994.
KIRK, D. E. Optimal control theory—an introduction, Donald E. Kirk, Prentice Hall,
Inc., New York(1971), 452 poges.$13.50. AIChE Journal, v. 17, n. 4, p. 1018–1018,
jul. 1971.
LI, W.; TODOROV, E. Iterative Linear Quadratic Regulator Design for Nonlinear
Biological Movement Systems. p. 222–229, 2011.
TASSA, Y.; EREZ, T.; TODOROV, E. Synthesis and stabilization of complex
behaviors through online trajectory optimization. IEEE International Conference on
Intelligent Robots and Systems, p. 4906–4913, 2012.
TASSA, Y.; MANSARD, N.; TODOROV, E. Control-Limited Differential Dynamic
Programming. 2014.
TODOROV, E. Optimal Control Theory. In: Bayesian Brain. [s.l.] The MIT Press,
2006. p. 268–298.
TODOROV, E. Convex and analytically-invertible dynamics with contacts and
constraints: Theory and implementation in MuJoCo. Proceedings - IEEE
International Conference on Robotics and Automation, p. 6054–6061, 2014.
TODOROV, E.; EREZ, T.; TASSA, Y. MuJoCo: A physics engine for model-based
control. In: 2012 IEEE/RSJ International Conference on Intelligent Robots and
Systems, Anais...IEEE, out. 2012. Disponível em:
<http://ieeexplore.ieee.org/document/6386109/>. Acesso em: 9 jul. 2018.