Nome: Plácido Capp Campos RA:157024

Estudo de Dois Controladores para Rastreamento de

Trajetória de Veículos Terrestres

Para esse projeto vamos comparar dois diferentes controles para o rastreamento de
trajetória para veículos terrestres, para esse estudo dois diferentes modelos foram
utilizados, o primeiro deles foi utilizado por [1] para simular a trajetória de um robô, para
esse caso consideramos que o veículo (ou robô) se locomove com uma velocidade 𝑣 que
é uma entrada de controle e sua direção de movimento é determinada pelo ângulo 𝜃.
Existe ainda um atuador que decide a velocidade de rotação 𝜃 que será chamado de w.
Dessa forma podemos encontrar o modelo abaixo:
𝑥̇ = 𝑣 cos(𝜃)
𝑦̇ = 𝑣 sin(𝜃)

𝜃̇ = 𝑤
Podemos ver abaixo uma imagem de vista superior do robô, com essa modelagem
podemos imaginar o robô como um ponto seguindo uma trajetória, ele possui apenas
direção de movimento e velocidade como parâmetros de controle. Na figura o ângulo 𝜃 a
ser rastreado é representado pelo ângulo da tangente à trajetória com o eixo x no sentido
anti-horário.

Figura 1: Visão superior de robô [1]

Para esse modelo o autor desenvolve um modelo linear a partir de um controle não linear
utilizando a função de lyapunov e demonstra sua convergência para os parâmetros
corretos.

Figura 2: Controle não linear obtido da função de Lyapunov

Ele então lineariza o controle e obtém:

Figura 3: Modelo linear do controlador

Ainda o autor oferece alguma intuição sobre como escolher as constantes de controle
𝐾𝑥 , 𝐾𝑦 𝑒 𝐾𝜃 . Para isso ele percebe que se nos movermos apenas em x com velocidade 𝑉𝑥
teremos apenas uma relação com o 𝐾𝑥 e podemos escolhê-lo. De maneira geral quanto
maior for o 𝐾𝑥 mais rápido o sistema converge mas ele deve ser de uma ordem de
grandeza menor que o inverso do tempo de amostragem alerta o autor. Para os outros
coeficientes podemos encontra-los baseado no amortecimento desejado:

Figura 4: Relação de coeficientes de controle com o amortecimento

Para implementar o sistema fiz a malha de controle abaixo:
 Figura 5: Malha de controle para método de Lyuapunov
Obtive sucesso em seguir uma trajetória retilínea com alguma dificuldade, a escolha
errada de coeficientes para o controle facilmente desestabiliza o sistema, e ele não é muito
robusto a ruído. Encontrei os coeficientes por tentativa e erro após alguns problemas em
realizar o método sugerido pelo autor e obtive:

Figura 6: Método de Lyapunov para trajetória retilínea

Para esse resultado utilizei uma trajetória com referências:
 Figura 7: Referências a serem seguidas
O método apresentado exige todas as referências possíveis, a posição x e y a ser seguida,
a velocidade, a direção e velocidade do veículo a cada instante. Todos esses valores são
funções de apenas 3 valores: x, y e o tempo em que ele deve terminar, ou de outra forma
de x, y e velocidade linear. Na figura acima temos yr por xr no primeiro quadro,
velocidade linear de referência, direção do veículo e velocidade de rotação.
Entretanto quando adicionei ruídos no sistema mesmo que em ordens de variância de
10^-3 obtive:

Figura 8: Lyapunov linear com erros estocásticos

Isso sugere que o método seja pouco robusto a variações no sistema e considerei que não
seria adequado para a aplicação desejada, parti então para o segundo modelo.
O segundo controle que estudei foi utilizado parcialmente para a modelagem do robô
Stanley que foi vencedor do DARPA Grand Challenge em 2006. Esse modelo é eficiente
velocidades baixas e moderadas mas pode ser facilmente remodelado para aceitar
velocidades altas se for o caso [3]. O modelo considera a velocidade linear das rodas da
frente 𝑣𝑓 , a direção das rodas de trás 𝜃 e a rotação das rotas da frente em relação ás de
trás 𝛿. O único parâmetro do carro é o comprimento entre as rodas da frente e de trás L
que foi considerado 1 por simplicidade.

Figura 9: Modelo considerado em [2], foto retirada de [3]

Com isso podemos obter o modelo abaixo:
𝑥̇ = 𝑣𝑓 cos(𝜃 + 𝛿)

𝑦̇ = 𝑣𝑓 sin(𝜃 + 𝛿)
𝑣𝑓
𝜃̇ = sin(𝜃 + 𝛿)
L
Para o controle desse sistema utilizando como referência as rodas da frente foi encontrada
por [2] uma solução muito simples: Dada uma velocidade escolhida previamente o único
parâmetro de controle será 𝛿. Então encontraremos a cada instante o ponto da trajetória
desejada mais próxima do centro das rodas da frente do veículo, na figura abaixo essa
distância é chamada de x(t), então encontraremos a derivada da trajetória nesse ponto e
finalmente 𝛼 será a diferença entre a direção da trajetória atual e a da trajetória. Por último
teremos ainda uma correção para o ângulo que ainda falta corrigir:
𝑥(𝑡)
𝛿 = 𝛼 + arctan(𝐾 )
𝑣𝑓 (𝑡)
 Figura 10: Modelo utilizado em [2] e [3]
Onde K vai ser uma constante de controle. O autor substitui esse controle em um sistema
mais complexo chamado te Bicycle model para veículos, mas o conceito vale para nosso
caso trocando seno por cosseno:

Dessa forma ele pode aproximar que 𝑥(𝑡) ≈ 𝑥(0)𝑒 −𝑘𝑡 , então para pequenos erros o
sistema converge com taxa esponencial para a resposta.
Fiz um modelo no simulink para esse sistema, mas ele apenas serve para uma base pois a
implementação real do sistema foi totalmente feita em código para que fosse possível
rever os pontos anteriores da trajetória.

Figura 11: Modelo aproximado no simulink

Para a implementação desse controle eu gerei a trajetória de referência xr e yr e estipulei
uma velocidade de entrada. Note que a única diferença entre o controle implementado no
robô Stanley e o aplicado aqui será o controle de velocidade, no meu caso considerei a
velocidade constante, já no robô Stanley foi feito um estudo do local onde ele circulava
para decidir a velocidade adequada. Uma ideia interessante seria um controlador PI
(proporcional e integral) para o controle de velocidade, caso o robô ficasse para trás, mas
não testei essa possibilidade, teríamos apenas que limitar a velocidade para não
ultrapassar os limites onde o modelo funciona bem ou utilizar a modificação sugerida em
[2] para velocidades mais altas.
Fiz a suposição de que o modelo nunca ultrapassaria a referência, isso é evidente se a
velocidade de referência for constante pois dado que o modelo usa a própria velocidade
de referência como entrada e ainda terá erros ele não irá ultrapassar a referência. Mesmo
que a velocidade possa ser controlada de outra forma que o carro ultrapasse a estimativa
podemos supor que a trajetória futura está disponível para o veículo e o algoritmo não
sofrerá nenhuma perda. Aqui considerei acesso apenas a trajetória de referência passada
e a presente.
Para testes utilizei uma entrada linear e uma senoidal, as entradas foram:

Figura 12: Entradas para modelo senoidal

 Figura 13: Entradas para modelo senoidal

Para o caso determinístico obtive:

Figura 14: Saídas para modelo linear determinístico

 Figura 15: Saídas para modelo senoidal determinístico

Se acrescentarmos erros estocásticos podemos ver que o sistema permanece robusto

diferente de antes:

Figura 16: Saídas para modelo linear com ruído de sistema e de feedback
 Figura 17: Saídas para modelo senoidal com ruído de sistema e de feedback

Implementei então um filtro de kalman na forma abaixo:

%% Filtro de Kalman
Mn = P(:,:,k)*H'*pinv(H*P(:,:,k)*H'+H*R*H'); %Ganho
x = x + Mn*(z-H*x); % x[n|n]
P(:,:,k) = (eye(3)-Mn*H)*P(:,:,k);

% Time update
F = [1 0 -Ts*vf(k)*sin(theta(k)+alpha(k));
0 1 Ts*vf(k)*cos(theta(k)+alpha(k));
0 0 1];
P(:,:,k) = F*P(:,:,k)*F' + Q;
%x vai entrar no sistema não linear
xc(k) = x(1);
yc(k) = x(2);

Acrescentando então o filtro de Kalman obtive:

Figura 18: Saídas para modelo senoidal com ruído de sistema e de feedback filtrado
 Figura 19: Saídas para modelo senoidal com ruído de sistema e de feedback filtrado

Referências:
[1] https://arxiv.org/pdf/1604.07446.pdf
[2] A stable tracking control for Method for an Autonomous Mobile
Robot.; Kaynama.Y; Kimura.Y; Miazaki,F.Noguchi,T
[3]
http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20St
anley.pdf