Controle PID com TurtleSim

Resumo

Move Turtle é um simples script em python ROS que move a tartaruga no TurtleSim do ROS para coordenadas (pontos) especificadas. Este projeto começou como um projeto de classe para CSCI473 na Colorado School of Mines (CSOM) e evoluiu para um projeto que usei para me ajudar a aprender como sistemas de controle em malha fechada podem ser criados, conhecimento que foi utilizado para me ajudar em meu trabalho no Laboratório de Robótica Centrada no Humano (HCR) da CSOM.

Contexto

Para um pouco de contexto, ROS significa Sistemas Operacionais de Robôs. ROS é um middleware de robótica de código aberto, geralmente emparelhado com o sistema operacional Ubuntu, que é usado para gerenciar melhor os componentes/clusters de software de um sistema robótico. Você pode aprender mais sobre como usar o ROS através do wiki oficial do ROS. O código que você escreve no ROS é em C++ ou Python.

TurtleSim é um simples simulador de robô 2D usado como uma introdução ao ROS e pacotes ROS para novos usuários do ROS. Tudo o que o TurtleSim faz é abrir uma simples exibição 2D com uma tartaruga. Essa tartaruga atua como o “robô” e você pode enviar mensagens para esse “robô” para mover e/ou girar. Bem, ao mover, o TurtleSim gera algum erro aleatório para simular a física do mundo real de uma maneira simples.

Sabendo o que é ROS e TurtleSim, por que eu os estava usando? Bem, no início da primavera de 2020, eu estava fazendo uma aula chamada “Robótica Centrada no Humano” na Colorado School of Mines como parte da minha carga de aulas de ciência da computação de graduação. A aula foi ministrada pelo Dr. Hao Zhang. A aula consistia em três projetos e o primeiro projeto consistia em configurar o ROS, aprender a usar o ROS e implementar um algoritmo de malha aberta ou fechada que faria a tartaruga no TurtleSim desenhar um M. A descrição original do projeto pode ser vista AQUI.

Quando eu estava fazendo a aula, completei este projeto implementando um algoritmo de malha aberta. Essa solução funcionou, mas realmente não era boa e desenhou um A que era “bom o suficiente”, mas não estava nem perto de ser “ótimo”. Depois de enviar o projeto por volta da metade de fevereiro de 2020, eu esqueci dele até um ano depois, em fevereiro de 2021.

Desafio

Em fevereiro de 2021, fui contratado como Assistente de Pesquisa no Laboratório de Robótica Centrada no Humano (HCR) na Colorado School of Mines, onde trabalhei sob o Dr. Hao Zhang. O objetivo do laboratório é fazer pesquisas sobre “autonomia colaborativa ao longo da vida, com o objetivo de permitir que robôs operem e se adaptem ao longo de longos períodos de tempo”. A partir de 1-9-2022 (1 de setembro de 2022), o laboratório se mudou da Colorado School of Mines para a Universidade de Massachusetts Amherst. Mas, quando trabalhei lá, eles ainda estavam localizados no Colorado.

No Laboratório HCR, fui encarregado de trabalhar no projeto Triton. O projeto Triton consistia em múltiplos, dezenas, de robôs chamados Tritons. Os Tritons eram robôs terrestres triangulares omni wheel que pareciam com isso:

Junto com os Tritons, oito Câmeras IR da OptiTrack dispostas em uma “ordem octogonal” a alguns pés acima do chão. Usando essas câmeras, o software Motive da Optitrack, um PC rodando Windows 10 e ROS; um espaço de 2 metros por 2 metros por 2 metros (x, y, z) é criado onde a posição exata no mundo real de objetos com marcadores de captura de movimento pode ser determinada.

Minha primeira tarefa com o projeto Triton foi fazer scripts que fizessem o robô Triton se mover para uma posição específica no mundo real. Na época, não tinha certeza de como alcançar isso, mas me lembrei do primeiro projeto da CSCI473, onde a ideia de um sistema de malha fechada foi introduzida para mim, então comecei a pesquisar realmente sobre sistemas de malha fechada e, por sua vez, sistemas de feedback.

Pesquisa

Durante minha pesquisa, descobri este ótimo vídeo do AerospaceControlsLab:

Neste vídeo, a ideia de um sistema PID foi mostrada e explicada para mim. Um sistema PID é um sistema de controle em malha fechada que produz saídas específicas com base em dados do mundo real. Em termos básicos, é um sistema de feedback que ajusta sua saída com base na diferença entre o valor desejado e o valor medido. Esta é a fórmula para um controlador PID:

$$ u(t) = K_p e(t) + K_i \int e(t) dt + K_d \frac{de(t)}{dt} $$

$u(t)$ é o sinal de controle.
$K_p$, $K_i$, $K_d$ são os ganhos proporcional, integral e derivativo, respectivamente.
$e(t)$ é o sinal de erro (a diferença entre a saída desejada e a saída real).
$\int e(t) dt$ é a integral do erro ao longo do tempo.
$\frac{de(t)}{dt}$ é a derivada do erro.

Para saber mais sobre isso, confira o vídeo do AerospaceControlsLab ou este incrível artigo da wikipedia de onde a fórmula é originada.

Depois de fazer minha pesquisa, bem como obter ajuda de meus amigos e professores inteligentes, determinei que usar um Controlador Proporcional, Controlador P, seria o melhor para ajudar o Triton a alcançar uma coordenada específica.

Como o Triton é um robô terrestre, eu só preciso me preocupar com as coordenadas X e Y. Sabendo disso, então tive que determinar quais valores desejados e valores medidos medir e processar dentro do(s) Controlador(es) P. Em minha pesquisa, encontrei este incrível post no wiki do ROS chamado Ir para o Objetivo onde eles se concentraram nos seguintes erros:

Erro de Distância: A diferença na distância entre a localização desejada (X, Y) e a localização atual (X, Y).
Erro de Theta: A diferença entre a orientação desejada e a orientação atual.

Solução Proposta

Sabendo de tudo isso, a solução para fazer o Triton se mover para uma coordenada específica no mundo real é usar um Controlador Proporcional para o erro de distância do Triton e o erro de theta em relação à coordenada alvo e à coordenada atual do Triton. Mas antes de implementar isso em um sistema físico, eu queria testar a ideia em uma simulação e, na época, pensei que usar o TurtleSim como a simulação era a melhor escolha.

Testando na Simulação

Com a solução teórica delineada, optei por validá-la inicialmente através da simulação, em vez de um cenário do mundo real. O mundo real é frequentemente poluído com ruídos que podem confundir a eficácia da solução e levar a lidar com questões irrelevantes. Reconhecendo isso, revisitei meu projeto da CSCI473 e o adaptei como um campo de teste para essa solução teórica.

Depois de atualizar o código antigo de usar ROS Melodic para usar ROS Noetic e com algum refinamento iterativo, consegui fazer a solução proposta funcionar no TurtleSim. Demonstrações de sua funcionalidade, com alguns caminhos de coordenadas alvo legais, podem ser encontradas no final deste post do blog. O código para tudo isso pode ser visto AQUI.

Conclusão

Em resumo, através deste projeto, a tartaruga do TurtleSim navega efetivamente para coordenadas definidas usando o sistema de malha fechada, superando significativamente meu sistema de malha aberta inicial de 2020. Testado com sucesso, implementei essa solução com os robôs Triton. Mas o ruído do mundo real introduziu problemas inesperados que exigiram semanas de depuração e testes no mundo real. Após essas semanas, consegui fazer o Triton alcançar coordenadas específicas no mundo real, utilizando a solução que testei no TurtleSim. Aqui está uma demonstração do Triton se movendo para coordenadas específicas no mundo real usando o método testado no TurtleSim:

Em retrospectiva, gostaria de ter usado uma simulação que simulasse melhor a física do mundo real. TurtleSim é ótimo para aprender, mas não é ótimo para testar ideias de robótica do mundo real. Na época, pensei que seria bom o suficiente para meu caso de uso simples, mas isso foi posteriormente provado não ser o caso.

O objetivo principal deste post do blog era delinear a criação de um sistema de malha fechada que permite à tartaruga no TurtleSim, e posteriormente aos robôs Triton, navegar com precisão para posições alvo especificadas (X, Y). Acredito que esse objetivo foi alcançado, mas pode ter gerado algum interesse em meu trabalho no Laboratório HCR. Sabendo disso, tenho planos de fazer mais posts no blog sobre minha experiência de trabalho no Laboratório HCR…

Demos do Turtle em Movimento (TurtleSim)

Este vídeo contém as seguintes demos:

Cayley Nodal: Um simples design Cayley Nodal
Círculos: Um design semelhante a uma teia de aranha
Círculos Internos: Um design de “círculos dentro de círculos”
M: Um design de M muito simples