Introdução à Robótica 2021/2022

Apresentação dos componentes do robô

1. Visão global dos vários componentes e sua interligação:

2. Motor com encoder:

3. Sensor de luz refletida

4. Unidade de processamento e comunicação sem fios e USB

5. Placa de ligações e drive de potência dos motores:

6. Alimentação de energia baseada em baterias:

SENSOR DE POSIÇÃO/VELOCIDADE – ENCODER

INCREMENTAL (ótico ou magnético):
INFRAVERMELHOS: sensibilidade cor e textura da superfície
refletora (reflexão simples):

MOTORES ROTATIVOS DE CORRENTE CONTÍNUA COM

ESCOVAS:
ATUAÇÃO COMUTADA DE UM MOTOR DC:
ATUAÇÃO COMUTADA DE UM MOTOR DC – PONTE EM H:
 CINEMÁTICA – TRAÇÃO DIFERENCIAL

 x( t )  v( t ) cos(( t ))

d    
 y( )   ( ) ( ( )) 
t = v t sin  t
dt
( t )   ( t ) 
   

v1( t ) + v2 ( t ) v1( t ) − v2 ( t )
v( t ) = ( t ) =
2 b
d (i ) + d 2 (i ) d1 (i ) − d 2 (i )
d (i ) = 1  (i ) =
2 b
ESTIMATIVA DA POSE DO ROBÔ BASEADA NA
ODOMETRIA

  (i ) 
x(i + 1) = x(i ) + d (i ) cos (i ) + 
 2 
  (i ) 
y(i + 1) = y(i ) + d (i ) sin  (i ) + 
 2 
 (i +1) =  (i ) +  (i )
Trabalho Laboratorial #1 – Odometria e equações de movimento de um robô

1. Considere o robot móvel disponível para esta unidade curricular. Este

desloca-se num plano horizontal por intermédio de duas rodas motrizes diferenciais e
com uma terceira roda livre para apoio.

a. Programação do robô e preparação para a sua utilização.

i) Descarregue o firmware disponível nos conteúdos da unidade curricular no

Sigarra. Descomprima o mesmo para uma pasta à sua escolha.

ii) Execute o programa “Visual Studio Code”. Utilize a opção File->Open

Folder para indicar a pasta referida no ponto anterior.

iii) Selecione na lista à esquerda o ficheiro “min.cpp” e desloque o cursor até

à linha 315. Deve visualizar algo similar à seguinte figura:

Descomente as linhas 315 e 316 apagando as duas barras inicias “//”.

Substitua XXXX na linha 315 pelo SSID da rede wireless a utilizar, no nosso
caso TP-Link_28CD, e na linha 316 xxxx pela chave de acesso 49871005.

iv) Ligue o cabo USB ao robô, compile e descarregue o firmware usando s

seta “->” presente na barra azul (parte de baixo do programa). No final deve
ver uma menssagem indicando o sucesso da operação.
v) Pressione o PlatformIO: Serial Monitor (símbolo na barra azul). Deve
visualizar as mensagens enviados do robô para o computador:

Assinalado a vermelho podem ver o endereço IP com que o robô ficou depois
de se ligar à rede sem fios. Este endereço será diferente em todos os robôs.

ii) Descarregue o software para o computador disponível nos conteúdos da

unidade curricular no Sigarra. Descomprima o ficheiro e execute o programa
“PCRobot.exe”. Introduza o endereço anterior no campo “Robot IP:” e
pressione depois o botão “Connect”. Deve visualizar algo do similar à
seguinte figura:
ii) Pode também ligar-se ao robô através da porta série (cabo USB). Para
isso deve-se verificar qual é a porta série utilizada pressionando com o botão
do lado direto no símbolo Start do Windows escolhendo a opção “Device
Manager”:

Deve aparecer o quadro seguinte onde no item “Ports (COM & LPT)
poderemos ver a porta série, neste caso a COM4:
 Introduzindo-se esse valor no campo “COM Port” e pressionando o botão
“Open” deveremos ter a ligação ao robô por cabo. Antes de efetuar esta
ligação deve fechar o programa Visual Studio Code” para que este liberte a
porta série.

b. Movimente o robô em linha reta de modo a determinar a relação entre os

impulsos gerados pelos sensores nas rodas e o deslocamento linear de cada
roda.

i) Pressionar o botão “Clear Enc” para colocar a zero as somas das

contagens dos impulsos do enconder “Senc 1” e “Senc 2”.

ii) Escrever um valor para “PMW 1” e “PMW 2” na coluna “Write”, por exemplo
-30 e -30. Pressionar depois “Set PWM”.

iii) Pressionar “Set State” ao lado de “201”, colocando o robô no estado 201
que aplica ops valores de PWM nos motores. Para parar pressionar “Set
State” ao lado de “200”.
 Usando os valores “Senc 1” e “Senc 2” calcule a constante que relaciona os
impulsos com a distância percorrida (ver equações da cinemática).

c. Movimente agora o robô apenas com velocidade angular e determine o valor

da distância entre as rodas. Escrever um valor para “PMW 1” e “PMW 2” na
coluna “Write”, por exemplo -30 e 30. Pressionar depois “Set PWM” e deixar
o robô fazer uma volta completa (2π radianos). Utilize as equações da
cinemática para determinar a distância entre rodas.

d. Verifique que pode efetuar movimentos compostos, ou seja em que

simultaneamente possuem translação e rotação e com uma só experiência
determinar os parâmetros das alíneas anteriores.

2. Alterando o código presente no ficheiro “robot.cpp”, nomeadamente o procedimento

“odometry” (linhas 33 a 54) implemente as equações que estimam a velocidade do robô
e a estimativa da sua posição e orientação em função dos impulsos recebidos dos
encoder. Ver equações nos slides “CINEMÁTICA – TRAÇÃO DIFERENCIAL” e
“ESTIMATIVA DA POSE DO ROBÔ BASEADA NA ODOMETRIA”. No ficheiro “robot.h”
está definida a classe “Robot_t” e podem ver as variáveis associadas ao robô. Devem
utilizar “enc1” e “enc2” para a leitura os impulsos provenientes dos encoders, “ds” e
“dtheta” para os descolamentos linear e angular em cada ciclo de controlo e “x”, “y” e
“theta” para a posição e orientação do robô no referencial cartesiano global. Pode ver
os valores calculados para “x”, “y” e “theta” na tabela de variáveis presente no programa
PCRobot. Para inicializar estas variáveis com um determinado valor, insira o mesmo na
coluna da direita e pressione o botão “Set XYϴ”.

Para atuar no robô de alguma forma deve utilizar o código presente no ficheiro
“control.cpp” onde pode criar estados com as atuações pretendidas e, através do
programa no computador, colocar o robô nesse estado.

Será também útil ver o código presente no procedimento “real_loop”, dentro do ficheiro
main.cpp. Este código é executado periodicamente, a uma frequência muito elevada, e
chama os procedimentos e funções referidas anteriormente.
3. Controlo em malha fechada da velocidade de cada roda.

Encontre a relação média entre o valor do PWM aplicado e a velocidade linear do robô
em metros por segundo.

i) Aplique um determinado valor de PWM nas duas rodas, leia o valor

dos impulsos gerados em cada ciclo de 40 ms (Enc1 e Enc2 em
PCRobot).
ii) Converta o valor da alínea anterior para metros por segundo. Primeiro
converte para metros por ciclo e depois para metros por segundo.

a. O ganho calculado na alínea anterior (kf) será o bloco de Feedforward da

figura seguinte.

O controlador PID por realimentação terá uma equação do tipo:

Out_PID = Kp*Erro + Ki*Integral_do_erro + Kd*Derivada_do_erro

No ficheiro “PID.cpp”, no procedimento “calc(.)”, poderá ver uma

aproximação numérica destas equações. Repare no “Anti windup” e na
limitação do sinal de saída.

No ciclo principal do programa, presente no ficheiro “main.cpp”,

procedimento “real_loop(.)”, poderá observar o seguinte código:

// Auto Control mode selection:

// States for 0 to 199 are for PID control
// States for 200 to 255 are for direct PWM control
if (robot.state >= 200) robot.control_mode = cm_pwm;
else robot.control_mode = cm_pid;

if (robot.control_mode == cm_pid) {
if (robot.v1ref != 0) {
robot.v1_PWM = PID1.calc(robot.v1ref, robot.v1e);
} else {
robot.v1_PWM = 0;
PID1.Se = 0;
}
if (robot.v2ref != 0) {
robot.v2_PWM = PID2.calc(robot.v2ref, robot.v2e);
} else {
robot.v2_PWM = 0;
PID2.Se = 0;
}
}
Como se pode observar para estados 200 a 255 o PWM enviado para o
motor é o determinado no “control(.)” em “comtrol.cpp” e para estados 0 a
199 o PWM é calculado com o controlador PID+FEEDFORWARD. Repare
que quando a referência de velocidade é zero se coloca o PWM e a variável
relacionada com a integração a zero. Isto porque, devido à zona morta do
motor é possível ter o motor parado com algum PWM o que não é desejável
(pode testar alterando o código). Desta forma o motor pára rapidamente
devido ao atrito. Noutras situações, pode não ser o mais adequado (por
exemplo querer manter o robô parado mesmo numa descida ou subida).

i) Comece por introduzir apena o valor de Kf calculado.

i) Introduza um valor de referência para a velocidade linear “v”.

 Repare como no código, a velovidade linear “v” e angular “w” são
convertidas em velocidades nas rodas:

robot.v1ref = robot.v + robot.w * DISTWHEELS/2;

robot.v2ref = robot.v - robot.w * DISTWHEELS/2;

Com este controlo apenas baseado no Feedforward é possível ter

velocidades ligeiramente diferentes em cada roda e é muito fácil
perturbar a velocidade de cada roda.

ii) Calcule para uma velocidade linear de 0,05 m/s qual o número de
impulso por ciclo que deve ter em cada roda. Introduza agora um
valor para o ganho, por exemplo Kp = 300. Observe o valor obtido em
regime permanente para os impulsos por ciclo para diferentes valores
de ganho. Repare que neste robô a diferente entre ter as rodas no ar
e pousadas não é significativa (não é sempre assim em todos os
robôs). Suba o ganho em passos de 300 a 500 e observe os
diferentes valores em regime permanente e as oscilações quando se
passa de uma referência 0 m/s para 0.05 m/s. Quando começar a
oscilar bastante anote o valor. Assuma um valor bastante mais baixo
(seja ¼ deste valor) para se ter uma boa margem de segurança.

iii) Altera agora a constante relacionada com a integração (Ki), iniciando

em 1000, incrementando com passos de 1000 e observando a
resposta a variações na velocidade de referência em 0 m/s e
0,05 m/s. Escolha um valor que não gere sobre elongações nem
oscilações.

iv) Introduza agora novamente o valor de Kf. Observe que agora temos
uma sobre elongação forte. Introduzindo um peque valor em Kd
(cerca de 10) diminui essa sobre elongação mas rapidamente
começa a gerar oscilações. Diminua a sobre elongação diminuindo
simultaneamente Ki e Kf.

4. Num dos estados em “control(.)” implemente um controlador que impõe uma

velocidade linear “v” constante e uma velocidade angular “w” que varia em função da
leitura dos sensores de linha de modo que o robô consiga seguir uma linha. As leituras
dos sensores (IR1 a 5) estão disponíveis num vetor IRLine.IR_values[0 a 4].

a. Comece, por exemplo, por usar apenas os dois sensores de cada extremidade
e um novo estado 100.

b. Aumente a velocidade linear até ao limite máximo que conseguir sem que o robô
perca a linha.

c. Observando os valores das leituras dos sensores repare que existe uma
pequena zona na transição entre preto e branco que o sensor dá uma leitura
intermédia entre os valores de branco e preto. Usando este facto faça agora um
controlador que use o sensor do meio e siga a borda interior da linha,
5. Crie agora um estado 99 para apenas inicializar a posição e orientação do robô,
variáveis robot.x, robot.y e robot.theta no valor zero correspondente ao início da primeira
reta mais longa, ou seja estamos a considerar que o referencial cartesiano tem o eixo X
coincidente com essa reta e a origem nesse ponto. De seguida passa logo para o estado
100.

Acrescente mais regras de transição de estado e os respetivos estados de modo que o

robô passe para o estado 101 um pouco antes de chegar à curva, para o 102 um pouco
depois de sair da curva e assim sucessivamente. Desta forma o robô sabe sempre em
que parte da pista está. Crie também um estado final em que o robô pare sensivelmente
a meio da reta inicial.

6. Aproveitando o facto de saber em que parte da pista está o robô pode, nas retas, ter
alguma certeza sobre o ângulo e uma das coordenadas, usando os impulsos apenas
para estimar a outra coordenada. Veja o procedimento “odometry” e actue em
conformidade.

7. Teste agora, em alguns estados, movimentar o robô sem usar a linha, ou seja, por
exemplo quando entrar num estado correspondente a uma curva, aplicar uma
velocidade linear e angular constantes e com um valor que leve o robô ao início da
próxima reta, ignorando os sensores de linha. Teste a sua eficiência tapando a linha
com uma folha.

8. Implemente um estado em que o robot se movimente para um determinado ponto

com coordenadas (xf, yf) e pare assim que estiver a uma distância inferior a um certo
limite. Atue na velocidade angular de modo a corrigira a diferença entre a orientação
atual do robô e a orientação que deve ter quando aponta para esse ponto de destino.

9. Utilize o procedimento da alínea anterior para navegar em zonas sem linhas ou com
as linhas interrompidas.