D.2 Testes funcionais nas funcionalidades do sistema embarcado do veículo
6.2 Controle de trajetórias do VILMA01 baseado em imagens
Nesta seção será apresentado a implementação do controle de trajetórias para seguir as linhas da estrada implementado como um nó de ROS realizado com a finalidade de testar os conceitos de condicionamento de controle e planejamento cooperativo. Na figura 6.19 é apre- sentado um diagrama de blocos do controle de trajetórias desenvolvido e implementado.
A seguir será descrita a implementação de cada uma dos blocos dividido nas camadas: Percepção, controle, planejamento e controle cooperativo. Finalmente serão apresentados al- guns resultados experimentais.
6.2.1 Percepção
Para a detecção da linha da estrada, primeiro foi feito um algoritmo de detecção de linha na camada de percepção, utilizando a técnica de detecção de linhas segmentadas (GROMPONE VON GIOI et al., 2012) o qual gera umas retas que são classificadas e fusiona- das para obter uma linha só, através de RANSAC (RANdom SAmple Consensus)(SIEGWART
et al., 2011a)(FISCHLER EBOLLES, 1981) usando a biblioteca PCL (RUSU ECOUSINS, 2011).
A linha resultante é descrita pela equação (6.8), dado que 𝑚𝑖𝑚𝑎𝑔𝑒 é a pendente utilizada para
encontrar o erro de orientação e 𝑏𝑖𝑚𝑎𝑔𝑒utilizado para encontrar o erro lateral em um valor fixo de 𝑦𝑟𝑒𝑓 (no sistemas de coordenadas da imagem) determinado heuristicamente para ter um com- promisso entre a estabilidade e a suavidade da resposta (SNIDER, 2009).
𝑥𝑖𝑚𝑎𝑔𝑒= 𝑚𝑖𝑚𝑎𝑔𝑒𝑘 𝑦𝑟𝑒𝑓 + 𝑏𝑖𝑚𝑎𝑔𝑒𝑘 (6.8)
Os parâmetros da reta são filtrados com um filtro passa baixa de primeira ordem descrito pela equação (6.9). ˙ 𝑚𝑖𝑚𝑎𝑔𝑒 = 1 𝑇𝑙𝑝𝑚 (︀−𝑚𝑖𝑚𝑎𝑔𝑒+ 𝑚𝑅𝐴𝑁 𝑆𝐴𝐶)︀ ˙𝑏𝑖𝑚𝑎𝑔𝑒 = 1 𝑇𝑙𝑝𝑏 (︀−𝑏𝑖𝑚𝑎𝑔𝑒+ 𝑏𝑅𝐴𝑁 𝑆𝐴𝐶)︀ (6.9)
O filtro discretizado por Euler é implementado pela equação (6.10)
𝑚𝑖𝑚𝑎𝑔𝑒𝑘 = 𝑚𝑖𝑚𝑎𝑔𝑒𝑘−1 (1 − 𝛼𝑚) + 𝛼𝑚𝑚𝑅𝐴𝑁 𝑆𝐴𝐶
𝑏𝑖𝑚𝑎𝑔𝑒𝑘 = 𝑏𝑖𝑚𝑎𝑔𝑒𝑘−1 (1 − 𝛼𝑏) + 𝛼𝑏𝑏𝑅𝐴𝑁 𝑆𝐴𝐶 (6.10)
6.2.2 Planejamento
O planejamento neste experimento foi simulado através de um gamepad. O planejamento da velocidade foi feito através de velocidades constantes que são modificadas por um sinal ana- lógico gerado pelo gamepad, limitados com aceleração/desaceleração constante. A referência do controle de trajetórias é descrita por uma linha criada diretamente na imagem através da equação (6.11).
𝑥𝑟𝑒𝑓 = 𝑚𝑟𝑒𝑓𝑘 𝑦𝑟𝑒𝑓 + 𝑏𝑟𝑒𝑓𝑘 (6.11)
Com a finalidade de testar o planejamento cooperativo foram usados dois botões do game- padpara simular o risco de variar referência lateral à direita ou à esquerda. O motorista poderá variar o sinal de referência através do sinal do volante do veículo somente se o planejamento (os botões) estiver ativo, variação esta que será governada pela equação (6.12).
𝑏𝑟𝑒𝑓𝑘 = 𝑏𝑟𝑒𝑓𝑘−1+ 𝛽1𝜏𝑢𝑠𝑒𝑟+ 𝛽2 (6.12)
dado que 𝛽1e 𝛽2são parâmetros heurísticos em 𝑝𝑖𝑥𝑒𝑖𝑠/𝑁 𝑚 e 𝑝𝑖𝑥𝑒𝑖𝑠 respectivamente encontra-
dos heuristicamente e que 𝑘 varia com o período de amostragem do sensor do torque. Note-se que quando o condutor libera a direção, a referência volta a ser a predeterminada inicialmente (𝑏𝑟𝑒𝑓𝑘 = 𝑏𝑟𝑒𝑓0 ) através de uma rampa para suavizar o esforço de controle, ou seja para suavizar a curvatura feita pelo veículo com a finalidade de aumentar a segurança intrínseca do mesmo.
6.2.3 Controle de movimento
A figura 6.20 apresenta as linhas na imagem que são necessárias para o controle, a amarela é a linha detetada pelo processamento da imagem (seção 6.2.1) , a roxa é a linha detetada filtrada no eixo temporal pelo filtro da equação 6.10 e a branca é a linha de referência do controle da equação 6.11. O objetivo final do controle é que as linhas brancas e roxas sejam coincidentes em orientação e o ponto de referencia 𝑦𝑟𝑒𝑓 do erro lateral seja zero (𝑥𝑟𝑒𝑓 = 𝑦𝑟𝑒𝑓).
Para atingir esta estratégia de controle, na camada de controle foi implementado um con- trolador baseado em imagem(CHERUBINIet al., 2011). Ele se chama desta forma porque traba-
Figura 6.20: Controle Baseado em imagem
lha com erros no sistema de coordenadas da imagem e gera a velocidade angular (𝛾) necessária para um veículo não holonômico através de um modelo de câmera generalizada. A velocidade angular calculada é transformada em um ângulo de direção através do modelo cinemático do veículo (seção G.1). Para que os erros da trajetória e da velocidade converjam a zero, três con- troladores PID foram implementados, um para o erro de orientação (𝑒𝜃), outro para o erro lateral
(𝑒𝑙) e um para a velocidade do veículo (𝑒𝑣𝑥). Os dois primeiros sinais de controle são somados
a partir das equações apresentadas em (CHERUBINI et al., 2011) para comandar o ângulo da
direção e o terceiro controlará o freio e o acelerador do veículo.
Os PID implementados são descritos pela equação (6.13) em contínuo e pela equação (6.14)em forma discreta através de Euler.
𝑢{𝑒} = 𝑘𝑝𝑒 + 𝑘𝑖 ˆ 𝑒.𝑑𝑡 + 𝑘𝑑˙𝑒 (6.13) 𝑢{𝑒}𝑘 = 𝑘𝑝𝑒𝑘+ 𝑘𝑑 𝑇𝑖 (𝑒𝑘− 𝑒𝑘−1) + 𝑘𝑝𝑇𝑖 𝑘 ∑︁ 𝑖=0 𝑒𝑘−𝑖 (6.14)
nas quais 𝑒 é erro respetivo 𝑒𝜃 = 𝑎𝑡𝑎𝑛(𝑚𝑟𝑒𝑓) − 𝑎𝑡𝑎𝑛(𝑚𝑖𝑚𝑎𝑔𝑒𝑘 ), 𝑒𝑙 = 𝑥𝑟𝑒𝑓𝑘 − 𝑥 𝑖𝑚𝑎𝑔𝑒
Figura 6.21: Zona morta no controle de velocidade do veículo
𝑣𝑟𝑒𝑓 − 𝑣
𝑥 e 𝑢{𝑒}é o valor do sinal de controle. Os controladores apresentam não linearidades,
uma é que em todos os controladores o integrador é saturado a um determinado valor, outra é que o controlador de velocidade tem uma zona morta na saída (ver figura 6.21) para que a frenagem seja suave e só seja utilizado quando realmente for necessário.
6.2.4 Controle Cooperativo
A primeiro teste de controle cooperativo foi desenvolvido como medida de segurança e consiste que em quando o condutor pressione o pedal do freio o controle de velocidade seja desativado, mas o controle de trajetória continua funcionando normalmente a menos que o con- dutor pressione o botão de emergência, momento no qual todos os controles serão desativados. Para a cooperação no controle de trajetórias, definimos controle cooperativo em que o condutor ajude a convergir o erro lateral a zero, ou em outras palavras ajude a seguir a linha da estrada.
Durante as análises e os testes em simulação feitos em uma arquitetura de controle de trajetórias de duas camadas (GARCIAet al., 2014), com o PID utilizado no firmware do motor, foi possível perceber que qualquer torque gerado pelo condutor vai ser tratado como uma pertu- bação para o PID da direção, não interessando se ele está na direção que diminui o erro lateral. O mesmo comportamento foi encontrado nos testes experimentais do controlador baseado em imagem. Portanto a estratégia adotada foi variar o sinal de referência 𝑏𝑟𝑒𝑓𝑘 com uma velocidade
dependente do torque do usuário se e somente se o torque aplicado pelo condutor tiver a mesma direção do erro e os dois estiverem fora de uma zona morta. O sinal de referência será governado pela equação (6.12).
Portanto o controle cooperativo realmente troca de forma temporal a rota planejada su- perpondo os comandos do planejamento nos casos em que vai ajudar a diminuir o erro lateral. O algoritmo 4 descreve a implementação da proposta.
if (left button or 𝑠𝑔𝑛(𝜏𝑢𝑠𝑒𝑟) = 𝑠𝑔𝑛(𝑒𝑙)) and |𝜏𝑢𝑠𝑒𝑟| > 𝜏𝑚𝑖𝑛then
𝑏𝑟𝑒𝑓𝑘 ← 𝑏𝑟𝑒𝑓𝑘−1+ 𝛽1𝑠𝑔𝑛(𝜏𝑢𝑠𝑒𝑟) + 𝛽2
else if (right button or 𝑠𝑔𝑛(𝜏𝑢𝑠𝑒𝑟) = 𝑠𝑔𝑛(𝑒𝑙)) and |𝜏𝑢𝑠𝑒𝑟| > 𝜏𝑚𝑖𝑛 then
𝑏𝑟𝑒𝑓𝑘 ← 𝑏𝑟𝑒𝑓𝑘−1− 𝛽1𝑠𝑔𝑛(𝜏𝑢𝑠𝑒𝑟) + 𝛽2 else if 𝑎𝑏𝑠(𝑏𝑟𝑒𝑓𝑘 − 𝑏𝑟𝑒𝑓0 ) > ∆𝑏 then 𝑏𝑟𝑒𝑓𝑘 ← 𝑏𝑟𝑒𝑓𝑘−1− 𝑠𝑖𝑔𝑛(𝜏𝑢𝑠𝑒𝑟)∆ 𝑏 else 𝑏𝑟𝑒𝑓𝑘 ← 𝑏𝑟𝑒𝑓0
Algoritmo 4: Algoritmo de planejamento cooperativo no controle baseado em imagens