FPGA-Implementation of Inverse Kinematics and Servo Controller for Robot Manipulator Fundamentos da...

FPGA-FPGA-Implementation of Implementation of Inverse Kinematics Inverse Kinematics

and Servo and Servo Controller for Controller for

Robot ManipulatorRobot ManipulatorFundamentos da RobóticaFundamentos da Robótica

Ênio Prates Vasconcelos Filho Ênio Prates Vasconcelos Filho 07/6016107/60161

AutoresAutores

Ying-Shieh Kung, IEEE Member; Ying-Shieh Kung, IEEE Member; Kuan-Hsuan Tseng Kuan-Hsuan Tseng Chia-Sheng ChenChia-Sheng Chen Hau-ZenHau-Zen An-Peng WangAn-Peng Wang

PublicadoPublicado

Artigo publicado em “Proceedings of Artigo publicado em “Proceedings of the 2006 IEEE”. International the 2006 IEEE”. International Conference on Robotics and Conference on Robotics and Biomimetics. Dezembro 17-20, 2006, Biomimetics. Dezembro 17-20, 2006, Kumming, China.Kumming, China.

ResumoResumo

Implementação, em FPGA, da Implementação, em FPGA, da cinemática inversa e do servo-cinemática inversa e do servo-controlador para um manipulador controlador para um manipulador Robótico;Robótico;

Redução do uso de unidades lógicas Redução do uso de unidades lógicas da FPGA pela implementação de da FPGA pela implementação de uma FSM para cálculo da cinemática uma FSM para cálculo da cinemática inversa;inversa;

Resultados experimentais;Resultados experimentais;

IntroduçãoIntrodução

Controle de robôs é, atualmente, um Controle de robôs é, atualmente, um grande foco de pesquisas;grande foco de pesquisas;

Existem várias implementações, Existem várias implementações, visando otimizar o processo:visando otimizar o processo: Controle distribuído de movimento em Controle distribuído de movimento em

micro controladores;micro controladores; Controle baseado em lógica Fuzzy em Controle baseado em lógica Fuzzy em

FPGA;FPGA; Associações de DSP e FPGA;Associações de DSP e FPGA;

InovaçãoInovação

Implementação de cálculos Implementação de cálculos complexos na FPGA, como a complexos na FPGA, como a cinemática inversa;cinemática inversa;

Redução de utilização da FPGA, sem Redução de utilização da FPGA, sem perda de performance do sistema;perda de performance do sistema;

FPGAFPGA

A FPGA Stratix II é o centro do sistema A FPGA Stratix II é o centro do sistema robótico proposto. robótico proposto. Fabricante: Altera;Fabricante: Altera; 60440 ULs;60440 ULs; 492 pinos de I/O;492 pinos de I/O; 36 blocos de DSP;36 blocos de DSP; 2544192 bits de RAM2544192 bits de RAM Microprocessador NIOS II de 32 Bits;Microprocessador NIOS II de 32 Bits; 16 Mb de Memória Flash e de SDRAM;16 Mb de Memória Flash e de SDRAM; 1 Mb de SRAM;1 Mb de SRAM;

Modelo matemático do Modelo matemático do sistemasistema

A equação dinâmica A equação dinâmica de um robô de n de um robô de n juntas é dada por:juntas é dada por:

Já a dinâmica de Já a dinâmica de motores DC é dada motores DC é dada por: por:

Onde:Onde: M(q) é a matriz M(q) é a matriz

inercial;inercial; Vm(q,q) é o vetor de Vm(q,q) é o vetor de

Coriolis;Coriolis; G(q) é o vetor da G(q) é o vetor da

gravidade;gravidade; F(q) é o vetor de F(q) é o vetor de

atrito;atrito;

)()q(q)q,(qM(q).....

qGFqVm

vKRFB mmmmm ...

qqJ

)(),(

}/{},{

}{~}/*{

}{},{

ii

ammi

mambm

mmmm

vecvvcev

RKdiagKrdiagR

BdiagRKKBdiagB

JdiagJqvecq

ii

iiiii

ii

Modelo matemático do Modelo matemático do sistema(2)sistema(2)

Através de manipulações, temos:Através de manipulações, temos:

Onde TL é uma força externa ou Onde TL é uma força externa ou influência de outros movimentos. influência de outros movimentos. Fazendo a transmissão ter uma valor Fazendo a transmissão ter uma valor pequeno, o sistema de controle P para pequeno, o sistema de controle P para posição e PI para velocidade pode ser posição e PI para velocidade pode ser representado por:representado por:

ia

mLm

a

bmmmm v

R

KTq

R

KKBqJ

i

i

ii

i

ii

iii

...

)(

Cinemática inversa para um Cinemática inversa para um manipulador Robóticomanipulador Robótico

Sistema de Sistema de coordenadas do coordenadas do robô, utilizando robô, utilizando DH;DH;

Parâmetros Parâmetros da da CinemáticaCinemática

Cinemática InversaCinemática Inversa

A cinemática inversa nem sempre é A cinemática inversa nem sempre é facilmente obtenível;facilmente obtenível;

Coordenadas de Juntas:

θθ i,di

CoordenadasCartesianas:

x,y,z,roll,pitch,yaw

Cinemática Direta

Cinemática Inversa

Cinemática Inversa (2)Cinemática Inversa (2)

Passo 1: Passo 1: θθ1= 1= θθ5=atan2[y,x];5=atan2[y,x]; Passo 2: b=Passo 2: b=±sqrt(x²+y²);±sqrt(x²+y²); Passo 3: Passo 3: θθ3=acos((b²+(d1-d5-z)²-(a2)²-3=acos((b²+(d1-d5-z)²-(a2)²-

(a3)²)/(2.a2.a3));(a3)²)/(2.a2.a3)); Passo 4: s2=(a2+a3.c3)(d1-d5-z)-a3.b.s3;Passo 4: s2=(a2+a3.c3)(d1-d5-z)-a3.b.s3; Passo 5: c2=(a2+a3.c3)b+a3.s3(d1-d5-z);Passo 5: c2=(a2+a3.c3)b+a3.s3(d1-d5-z); Passo 6: Passo 6: θθ2=atan2(s2,c2);2=atan2(s2,c2); Passo 7: Passo 7: θθ4=-4=-θθ2-2-θθ3;3;

Projetando os CircuitosProjetando os Circuitos

Arquitetura implementada na FPGAArquitetura implementada na FPGA

Projetando os Projetando os Circuitos(2)Circuitos(2)

Na implementação Na implementação do calculo da do calculo da cinemática inversa, cinemática inversa, foi utilizada uma foi utilizada uma FSM de 42 FSM de 42 estados, ao invés estados, ao invés de computação de computação paralela;paralela;

Recurso Tempo

FPGA 840ns

Software em C dentro do NIOS

5.6ms

Projetando os Projetando os Circuitos(3)Circuitos(3)

Também foi utilizada uma FSM para Também foi utilizada uma FSM para o servo controlador;o servo controlador;

Projetando os Projetando os Circuitos(4)Circuitos(4)

Projetando os Projetando os Circuitos(5)Circuitos(5)

PWM – 18kHzPWM – 18kHz

Uso da FPGAUso da FPGA

Experimentos e Experimentos e ResultadosResultados

Foi Foi implementado o implementado o sistema sobre o sistema sobre o robô micro-robô micro-articulado articulado Mitsubishi Mitsubishi Movemaster RV-Movemaster RV-M1.M1.

Experimentos e Experimentos e Resultados (2)Resultados (2)

Experimentos e Experimentos e Resultados (3)Resultados (3)

SimulaçõesSimulações

Foram realizadas duas simulações a Foram realizadas duas simulações a partir dos sistemas propostos no partir dos sistemas propostos no artigo.artigo.

As simulações foram feitas através da As simulações foram feitas através da ferramenta Simulink, do Matlab;ferramenta Simulink, do Matlab;

A primeira refere-se ao sistema A primeira refere-se ao sistema completo;completo;

A segunda refere-se ao calculo da A segunda refere-se ao calculo da cinemática inversa;cinemática inversa;

SimulaçõesSimulações

SimulaçõesSimulações

ConclusõesConclusões

O artigo busca mostrar a viabilidade e O artigo busca mostrar a viabilidade e eficiência de se implementar o calculo eficiência de se implementar o calculo da cinemática inversa e o sistema de da cinemática inversa e o sistema de controle do manipulador robótico na controle do manipulador robótico na FPGA;FPGA;

Tal intuito não pode ser verificado em Tal intuito não pode ser verificado em função da não implementação na FPGA; função da não implementação na FPGA;

Contudo, pelas simulações realizadas Contudo, pelas simulações realizadas em Simulink, fica demonstrada a em Simulink, fica demonstrada a veracidade das equações propostas;veracidade das equações propostas;

ConclusõesConclusões

A idéia é bastante interessante, uma A idéia é bastante interessante, uma vez que um dos grandes gargalos vez que um dos grandes gargalos para a exatidão do movimento de um para a exatidão do movimento de um manipulador robótico encontra-se no manipulador robótico encontra-se no tempo de computação de sua nova tempo de computação de sua nova posição, bem como para alcançar posição, bem como para alcançar sua velocidade determinada;sua velocidade determinada;

Fim!Fim!