Embed Size (px)
Citation preview
Desenvolvimento de um controlador baseado em PC (busPCI) para o robô manipulador PUMA 560
André Miguel Ribeiro de Azevedo
Dissertação para obtenção do Grau de Mestre emEngenharia Mecânica
JúriPresidente: Professor João Rogério Caldas PintoOrientador: Professor Jorge Manuel Mateus MartinsVogal: Professor José Manuel Gutierrez Sá da Costa
Junho - 2012
Resumo
O principal objectivo desta trabalho é integrar a placa de aquisição de dados Quanser Q8 e a pla-taforma de força AMTI BP400600-1000 no harware de controlo do robô manipulador PUMA 560. Osoftware a desenvolver baseia-se em Matlab/Simulink e na toolbox xPC Target. Esta integração per-mitirá a identificação dos parâmetros dinâmicos do manipulador e o desenvolvimento de controladoresbaseados nestes.
No primeiro capítulo, é realçada a importância do conhecimento dos parâmetros dinâmicos de umrobô manipulador, bem como as possíveis técnicas para estimação dos mesmos. A estimação destesparâmetros através de técnicas de identificação é abordada em detalhe, visto ser esta a técnica utili-zada neste trabalho. São apresentados todos os métodos de identificação, bem como as vantagens edesvantagens de cada.
No capítulo 2, são apresentadas todas as bases teóricas necessárias para a execução desta dis-sertação, que vão desde a cinemática até ao controlo, passando pela dinâmica e identificação de robôsmanipuladores.
No capítulo seguinte, é indicado todo o hardware necessário à realização deste trabalho. A inte-gração do hardware com o PC é de seguida explicado, bem como todo o trabalho desenvolvido parapermitir esta mesma integração.
No capítulo 4, é descrito todo o procedimento experimental com vista à identificação dos parâmetrosdinâmicos do PUMA 560. A identificação destes parâmetros permite a implementação de um controla-dor por dinâmica inversa, sendo que os resultados obtidos são de seguida analisados, discutidos e asdevidas conclusões retiradas.
Palavras-Chave: Robô Manipulador, PUMA 560, Identificação, Plataforma de Força, ParâmetrosDinâmicos, Controlador Dinâmica Inversa
iii
iv
Abstract
The aim of this work is to integrate the Quanser Q8 acquisition card and the AMTI BP400600-1000force plate with PUMA 560 manipulator. the software to be developed is based on Matlab/Simulink andxPC Target toolbox. This integration allows the identification of PUMA 560 dynamics parameters andthe development of a controller based on it.
In chapter 1, the importance of the knowledge of this parameters is presented, and also the differenttechniques to estimate them. Then estimation through identification is detailed and the advantages anddisvantages of each identification method are discussed.
In chapter 2, all the theoretic basis required to the execution of this thesis are described. All the mainthemes, from the cinematic to control of robot manipulators are addressed.
In chapter 3, the hardware used is presented and its integration with the PC explained, likewise allthe work developed to allow that integration.
In chapter 4, all the experiments of the identification of the dynamics parameters procedure aredescribed. The identification of the dynamics parameters allows the implementation of an inverse dy-namics control scheme. The results are then checked, discussed and the conclusions drawn. Finally,some future work is suggested.
Keywords: Robot Manipulator, PUMA 560, Identification, Force Plate, Dynamics Parameters, In-verse Dynamics Controller
v
vi
Agradecimentos
Primeiramente, gostaria de agradecer ao meu orientador, Dr. Jorge M. M. Martins, por todo o apoioprestado durante a execução da dissertação e especialmente por me ter introduzido na área da robóticade manipulação.
Um agradecimento muito especial ao Pedro Pires por todo o apoio prestado e pela disponibilidadedemonstrada para ajudar. Um abraço também para todos os que partilharam o laboratório ao longodeste trabalho.
Ao Sr. Raposeiro e ao Eng. Camilo um muito obrigado por todo o apoio técnico prestado.A todos os colegas de curso com os quais privei, sendo que não irei enunciar nenhum, pois todos
foram importantes ao longo deste percurso.À equipa de futsal da AEIST, um enorme obrigado por todos os momentos partilhados desde 2007,
os quais espero que continuem por muitos e bons anos.Aos primos Marco e Bruno, um agradecimento do fundo do coração por todo o apoio prestado nestes
últimos meses.Aos meus pais, o meu mais profundo sentimento de gratidão, não só por todo o apoio, sem o qual
não teria sido possível, mas principalmente por tudo aquilo que sou devê-lo a vocês. Um beijo especialpara a mana Patrícia e para os melhores sobrinhos do mundo Gonçalo e Matilde.
Finalmente, à Mafalda, por todos os momentos ao longo destes 5 anos e por toda a paciência de-monstrada quando o rumo dos acontecimentos não era o desejado. Obrigado por me fazeres acreditar,eternamente grato.
vii
viii
Conteúdo
Conteúdo ix
Lista de Figuras xi
Lista de Tabelas xiii
Notação xv
1 Introdução 11.1 História da Robótica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Estado da Arte . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.3 Objectivos da Dissertação . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.4 Contribuição da Dissertação . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2 Teoria de Robôs Manipuladores 72.1 Cinemática . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1.1 Cinemática directa - notação de Denavit-Hartenberg . . . . . . . . . . . . . . . . . 72.2 Dinâmica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.2.1 Modelo dinâmico externo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.2.2 Modelo dinâmico interno . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.2.3 Modelo dinâmico combinado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.3 Identificação . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162.3.1 Parâmetros do modelo de identificação . . . . . . . . . . . . . . . . . . . . . . . . 162.3.2 Planeamento experimental . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182.3.3 Estimação dos parâmetros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.4 Controlo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202.4.1 Controlador PD . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202.4.2 Controlador por dinâmica inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3 Descrição do Hardware 233.1 Robô Manipulador PUMA 560 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.1.1 Integração do PUMA 560 com PC . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.1.2 Controlador do PUMA 560 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.2 Plataforma de Força e Amplificador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 253.2.1 Plataforma . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 253.2.2 Amplificador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 293.2.3 Integração da Plataforma/Amplificador com PC . . . . . . . . . . . . . . . . . . . . 31
ix
3.3 Aparato Experimental . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4 Resultados Experimentais 354.1 Modelação . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
4.1.1 Cinemática . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354.1.2 Dinâmica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
4.2 Identificação . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394.2.1 Optimização da Trajectória . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394.2.2 Estimação . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
4.3 Controlo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
5 Conclusões e Trabalho Futuro 515.1 Conclusões . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 515.2 Trabalho Futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
A Plataforma e amplificador 57A.1 Relatório de calibração da plataforma . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57A.2 Relatório calibração do amplificador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58A.3 Especificações do amplificador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59A.4 Estrutura dos comandos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
B Código desenvolvido 63B.1 Programação do watchdog da Quanser Q8 . . . . . . . . . . . . . . . . . . . . . . . . . . 63B.2 Configuração do Amplificador DigiAmp DSA-5 . . . . . . . . . . . . . . . . . . . . . . . . 69
x
Lista de Figuras
1.1 Robôs manipuladores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2 Estimação dos parâmetros de um robô manipulador . . . . . . . . . . . . . . . . . . . . . 4
2.1 Representação esquemática de um robô de cadeia aberta . . . . . . . . . . . . . . . . . 82.2 Parâmetros de Denavit-Hartenberg . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.3 Estrutura do algoritmo recursivo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.4 Diagrama de blocos de controlador PD no espaço de juntas . . . . . . . . . . . . . . . . . 212.5 Controlador por dinâmica inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.1 Arquitectura do controlador antiga . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.2 Arquitectura do controlador implementada . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.3 Placa de aquisição Quanser Q8 (à esquerda) e placa de terminais (à direita) . . . . . . . 243.4 Diagrama de blocos do controlador PD implementado . . . . . . . . . . . . . . . . . . . . 253.5 Plataforma AMTI BP4006000-1000 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.6 AMTI DigiAmp DSA-6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.7 Pontes de Wheatstone da plataforma AMTI BP400600-1000 . . . . . . . . . . . . . . . . 273.8 Diagrama genérico da ponte de Wheatstone . . . . . . . . . . . . . . . . . . . . . . . . . 273.9 Convenção de eixos e dimensões da plataforma AMTI BP400600 - 1000 . . . . . . . . . 293.10 Representação esquemática do amplificador DigiAmp DSA-6 . . . . . . . . . . . . . . . . 303.11 Integração da Plataforma/Amplificador com PC . . . . . . . . . . . . . . . . . . . . . . . . 313.12 Ligação Host-Target . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
4.1 Referenciais da configuração base do PUMA 560 . . . . . . . . . . . . . . . . . . . . . . . 364.2 Trajectória para validação dos modelos dinâmicos . . . . . . . . . . . . . . . . . . . . . . 374.3 Validação do modelo interno . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 374.4 Validação do modelo externo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384.5 Validação do modelo combinado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384.6 Trajectória optimizada para identificação dos parâmetros do modelo interno . . . . . . . . 424.7 Trajectória optimizada para identificação dos parâmetros do modelo externo . . . . . . . 434.8 Trajectória optimizada para identificação dos parâmetros do modelo combinado . . . . . 434.9 Binários de junta para a trajectória real do modelo interno . . . . . . . . . . . . . . . . . . 444.10 Forças exercidas na base para a trajectória real do modelo externo . . . . . . . . . . . . . 454.11 Forças exercidas na base e binários de junta para a trajectória real do modelo combinado 454.12 Validação dos parâmetros do modelo interno . . . . . . . . . . . . . . . . . . . . . . . . . 474.13 Validação dos parâmetros do modelo externo . . . . . . . . . . . . . . . . . . . . . . . . . 474.14 Validação dos parâmetros do modelo combinado . . . . . . . . . . . . . . . . . . . . . . . 48
xi
4.15 Controlador por dinâmica inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 494.16 Validação do controlador por dinâmica inversa . . . . . . . . . . . . . . . . . . . . . . . . 494.17 Erro de seguimento e velocidade normalizada . . . . . . . . . . . . . . . . . . . . . . . . 50
xii
Lista de Tabelas
3.1 Especificações da plataforma . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283.2 ID dos comandos para interface Ethernet . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.1 Parâmetros cinemáticos do PUMA 560 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 364.2 Parâmetros da trajectória óptima para os modelos dinâmicos . . . . . . . . . . . . . . . . 414.3 Valor da função de custo ! log
!det
!Y TY
""para a trajectória optimizada . . . . . . . . . 42
4.4 Constantes eléctricas e de binário de motor e relações de transmissão (de: [1]) . . . . . . 444.5 Valor da função de custo ! log
!det
!Y TY
""para a trajectória real . . . . . . . . . . . . . 44
4.6 Parâmetros dinâmicos estimados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 464.7 Erro quadrático médio para os binários de junta . . . . . . . . . . . . . . . . . . . . . . . . 474.8 Erro quadrático médio para as forças de reacção . . . . . . . . . . . . . . . . . . . . . . . 474.9 Erro quadrático médio para as forças de reacção e binários de junta . . . . . . . . . . . . 474.10 Erro médio da posição angular . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
A.1 Sensibilidade por canal para uso geral . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57A.2 Matriz de sensibilidade . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58A.3 Inversa da matriz de sensibilidade . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58A.4 Saídas digitais . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58A.5 Saídas analógicas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59A.6 Especificações DigiAmp DSA-6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
xiii
xiv
Notação
Abreviaturas
BNC Bayonet Neill-Concelman
CAD Computer-aided design
DC Direct Current
LCD Liquid Crystal Display
LS Least-squares
PUMA Programmable Universal Manipulation Arm ou Programmable Universal Machine for Assembly
RCA Radio Corporation of America
SAIL Stanford Artificial Intelligence Laboratory
SVD Singular Value Decomposition
TTL Time To Live
UDP User Datagram Protocol
Lista de Símbolos
!i ângulo entre zi!1 e zi em torno de xi, sendo positivo quando a rotação é feita no sentido anti-horário
qi aceleração da junta i
"i aceleração da junta i
pii aceleração linear do elo i expresso no referencial do corpo i
qi velocidade da junta i
#ii aceleração angular do elo i expresso no referencial do corpo i
"i velocidade da junta i
xv
"r velocidade de junta real
qr aceleração de junta real
#f frequência natural da série de Fourier
#ii velocidade angular do elo i expresso no referencial do corpo i
#ni frequência natural
$g parâmetros dinâmicos de um manipulador
$i vector dos parâmetros inercias do elo i
%fi binário de atrito na junta i
%g vector de forças generalizado
%i binário da junta i
"d posição angular de junta desejada
"i posição angular da junta i
"r posição angular de junta real
&i factor de amortecimento
ai,l amplitude do seno do termo l da junta i
Ai matriz de regressão que descreve o movimento do elo i
ai distância entre Oi e Oi!
bi,l amplitude do coseno do termo l da junta i
xvi
Cbe (q) transformação de coordenadas que descreve a posição e orientação do elemento terminal em
relação ao referencial base
cii centro de massa do elo i expresso no referencial do corpo i
C0n (q) transformação de coordenadas que descreve a posição e orientação do referencial n em relação
ao referencial 0
di coordenada de Oi! segundo zi!1
Fi,i+1 forças exercidas sobre o corpo i devido ao movimento do elo i+ 1
Fi,i forças exercidas sobre o corpo i devido ao próprio movimento
Fji forças exercidas sobre a junta i
fvi parâmetro do atrito viscoso da junta i
g aceleração gravítica
Gi relação de transmissão da junta i
Ipi tensor de inércia do elo i expresso no referencial do do elo i
Iqi tensor de inércia do elo i expresso no referencial do centro de massa
Kai constante eléctrica da junta i
KD ganho derivativo
Kmi constante de binário do motor i
KP ganho proporcional
Mi,i+1 momentos exercidos sobre o corpo i devido ao movimento do elo i+ 1
Mi,i momentos exercidos sobre o corpo i devido ao próprio movimento
xvii
M ii!1 matriz de transformação que define o referencial i! 1 relativamente ao referencial i
mi massa do elo i
M i!1i matriz de transformação que define o referencial i relativamente ao referencial i! 1
Mji momentos exercidos sobre a junta i
q0i posição da junta i em torna da qual ocorre a excitação
qd posições de junta desejadas
qi posição da junta i
qr posições de junta real
T 1b matriz de transmissão de forças
u acção de controlo
W1 vector das forças e momentos exercidos no elo 1
Wb vector das forças e momentos de reacção na base do manipulador
Wi,i+1 vector de forças exercidas no elo i devido ao movimento do elo i+ 1
Wi,i vector de forças exercidas no elo i devido ao movimento do elo i
Wi vector de forças exercidas no elo i
Wji vector das forças e momentos exercidos sobre a junta i
Y matriz de regressão
xviii
Capítulo 1
Introdução
Neste capítulo é feito um breve resumo sobre a história da robótica e sobre o desenvolvimentoverificado na área no decorrer dos anos.
De seguida, a temática acerca da importância do conhecimento das características dinâmicas dosmanipuladores é abordada, bem como as metodologias existentes para avaliação dessas mesmascaracterísticas.
Os principais objectivos deste trabalho são apresentados e são referidas as contribuições do traba-lho desenvolvido.
1.1 História da Robótica
O termo Robô tem origem na palavra checa ”robota” que significa ”trabalho forçado”. O termofoi utilizado pela primeira vez em 1920, numa peça do escritor checo Karel Capek. Nesta peça, osrobôs eram vistos como máquinas incansáveis de aspecto humano e com capacidades avançadas.Todas estas características, juntamente com a evolução tecnológica, despertaram o receio de as tarefasdesenvolvidas até então por humanos, passarem a ser executadas por robôs.
A investigação e desenvolvimento verificado nas décadas de 50 e 60 levou ao desenvolvimento dosprimeiros robôs controlados por computador, sendo que em 1961 começou a ser utilizado, pela GeneralMotors, o primeiro robô manipulador criado. Este manipulador, designado por Unimate (figura 1.1(a)),foi criado por George Devol na década de 50, sendo que este juntamente com Joseph Engelbergerfundaram a primeira empresa a nível mundial dedicada à construção de robôs, a Unimation.
Em 1969, Victor Scheinman, então estudante de Engenharia Mecânica e trabalhando em simultâ-neo no Laboratório de Inteligência Artificial de Stanford (SAIL), desenvolveu o primeiro manipuladorcompletamente eléctrico e controlado por computador, o braço de Stanford (figura 1.1(b)).
A década de 70 é marcada pelo aparecimento de uma série de robôs manipuladores dos quaisse destaca o PUMA (figura 1.1(c)), em 1978, e o qual é utilizado no presente em várias instituiçõesacadémicas para investigação.
No inicio dos anos 80, observa-se um grande crescimento na indústria da robótica com o frequenteaparecimento de novas empresas e robôs. É nesta década que se verifica uma ampla aplicação derobôs na indústria automóvel, bem como um pouco por todas as outras indústrias.
A partir da década de 90, os robôs deixam de ter cariz somente industrial, passando a ser utilizadosem outras aplicações. Esta expansão deve-se principalmente ao avanço tecnológico, bem como àinvestigação desenvolvida no campo da robótica, sendo possível encontrar robôs em cooperação com
1
(a) Unimate (b) Stanford Arm (c) PUMA
Figura 1.1: Robôs manipuladores
seres humanos em residências, locais de trabalho, prestação de serviços, entretenimento, educação,saúde, etc..
Conforme se constata o campo de aplicação dos robôs sofreu um aumento significativo ao longodo tempo, sendo expectável que a utilização se alargue a mais aplicações. A evolução verificada nosúltimos anos, permite prever a introdução de robôs em praticamente todos os campos de aplicaçãopossíveis num futuro próximo.
1.2 Estado da Arte
A evolução tecnológica verificada desde a revolução industrial fez com que a expectativas dos con-sumidores fossem sendo cada vez maiores. Estas expectativas aliadas ao aumento dos padrões dequalidade e à competição internacional fizeram aumentar os níveis de qualidade e de velocidade nosprocessos de produção. Se inicialmente o objectivo das indústrias era produzir o mesmo produto emlarga escala, a baixo custo e com tempo de produção reduzido, esta ideia começou a ser abandonadaao longo dos anos. Passou-se a ter como objectivo a produção de diferentes produtos na mesma linhade produção, mantendo os custos e tempo de produção o mais baixo possível, implicando isto linhasde produção com um certo grau de flexibilidade.
Neste contexto, os robôs manipuladores desempenharam um papel importantíssimo para o aumentoda produtividade e da flexibilidade das linhas de produção. Estes, para além de permitirem executartarefas com velocidade e precisão superior aos humanos têm ainda capacidade para executar tarefasnão possíveis ao Homem. A introdução de robôs manipuladores nas indústrias, para além das van-tagens atrás referidas, permitiu aumentar a segurança nos ambientes industriais, sendo estes usadospara desempenhar tarefas de risco. A primeira indústria a aderir ao uso de robôs manipuladores foi aautomóvel, na qual os manipuladores executavam tarefas desde a paletização, transporte, montagem,corte, soldadura, colagem ou pintura.
A devida execução das tarefas pelo robô manipulador implica o conhecimento do seu comporta-mento cinemático e dinâmico. Enquanto que para o modelo cinemático é necessário conhecer osparâmetros geométricos do manipulador, os quais podem ser obtidos através de técnicas de calibra-ção, o conhecimento dos parâmetros dinâmicos necessários para a construção do modelo dinâmicodo robô manipulador são geralmente de difícil avaliação. De modo ao robô manipulador executar umatarefa com a devida precisão, o conhecimento destes parâmetros é essencial, sendo que por vezesnem os próprios fabricantes têm conhecimento destes valores. Durante anos o problema associado àfalta de conhecimento do valor real dos parâmetros dinâmicos foi evitado através do uso de técnicas decontrolo mais robustas em que não era necessário o conhecimento destes parâmetros. No entanto para
2
aplicações que exigem um elevado grau de precisão este tipo de controlo não é satisfatório. Neste tipode aplicações são necessárias técnicas de controlo mais avançadas nas quais o conhecimentos dosparâmetros dinâmicos do manipulador é um requisito. Durante anos o uso destas técnicas de controlocingiu-se a laboratórios especializados de robótica, sendo que a evolução verificada no campo da infor-mática permitiu o desenvolvimento destas técnicas de controlo em outros ambientes, o que estimuloua necessidade de obter modelos dinâmicos cada vez mais precisos.
Existem diferentes métodos para a estimação dos parâmetros dinâmicos de um robô. O métodomais simples é a utilização de dados de sistemas CAD, sendo que estes dados normalmente nãosão precisos visto que a cablagem, componentes electrónicos e outros componentes do robô não sãoincluídos no modelo CAD. Alternativamente, a desmontagem do robô peça por peça, permite obtertodos os parâmetros inerciais experimentalmente. No entanto este método apresenta a desvantagemde nem todos os parâmetros dinâmicos poderem ser obtidos (ex. atrito), sendo também a desmontagemdo robô um processo moroso e não aconselhável em ambiente industrial.
Por outro lado, a estimação dos parâmetros dinâmicos de um manipulador pode ser alcançadaatravés de técnicas de identificação, sendo este o método usado na generalidade para a estimação dosparâmetros dinâmicos. Os procedimentos experimentais para identificação podem ser encontrados em[2, 3, 4, 5], sendo que todos os outros procedimentos têm por base estes trabalhos.
Várias abordagens têm sido desenvolvidas nas últimas décadas, e podem ser divididas em duascategorias mediante o modelo e o tipo de sensores utilizados. Nas abordagens mais clássicas, [6, 7],os parâmetros são estimados através dos dados recolhidos pelos próprios dispositivos internos domanipulador durante um movimento especifico. Os encoders dos actuadores são usados para recolhados dados relativos ao movimento do robô, enquanto os binários dos actuadores são estimados atravésde leituras da corrente dos actuadores, não sendo necessário sensores adicionais aos presentes nomanipulador. Os modelos dinâmicos que relacionam este tipo de dados são apelidados de modelosinternos.
Uma abordagem alternativa para a estimação dos parâmetros inerciais é o uso de modelos dereacção ou modelos externos. Este modelo relaciona o movimento do robô com as forças de reacçãoexercidas na sua base, e é totalmente independente do atrito nas juntas. Tal como no caso anterior osdados relativos ao movimento do robô são adquiridos através dos encoders dos actuadores, sendo asforças exercidas sobre a base do manipulador medidas através de um sensor externo: uma plataformade força/momentos, sendo que neste tipo de abordagem apenas é possível identificar os parâmetrosinerciais do robô. No trabalho apresentado em [8] é utilizado um sensor de força/momentos na base dorobô para estimar as propriedades de massa estaticamente. A estimação é feita com base nas forçase momentos medidos pelo sensor para diferentes posições de junta e para diferentes orientações dabase. Para além de não ser possível avaliar todos os parâmetros inerciais, esta abordagem requer areorientação da base do robô, o que não é prático. Em [9] é apresentado um método para estimaçãodos parâmetros inerciais através de sensores externos e sem necessidade de reorientação da basedo robô. Para a aquisição dos dados relativos ao movimento do robô são utilizados sensores visuaisde alta precisão, o que é vantajoso visto não ser necessário as leituras dos encoders do actuadores.O trabalho desenvolvido em [10] apresenta um modelo baseado na formulação de Newton-Euler, noqual a estimação dos parâmetros inerciais é feita com base nas leituras obtidas na plataforma de forçase com as posições e velocidades de junta. É utilizada uma técnica com filtro passa-baixo em que éeliminada a necessidade de conhecimento das acelerações de junta. Mais recentemente em [11], eatravés de aplicação experimental é concluído que a identificação dos parâmetros inerciais através domodelo externo leva a resultados mais satisfatórios relativamente ao uso do modelo do interno.
3
Estimação
de parâmetros
CAD IdentificaçãoDesmontagem
peça por peça
Modelo interno Modelo externo
Corrente do motorPlataforma
de força/momentos
Figura 1.2: Estimação dos parâmetros de um robô manipulador
Em [12] é apresentada uma nova abordagem onde é utilizada uma combinação de ambos os mode-los para identificação dos parametros dinâmicos, segundo a qual o uso de ambos os modelos melhoraa estimação dos parâmetros e a predição do binário dos actuadores. No entanto estas conclusões sãobaseadas em resultados de simulação não tendo sido validados experimentalmente.
O processo de identificação pode ser resumido como:
• Formulação do modelo dinâmico do manipulador
• Simplificação do modelo dinâmico do manipulador, onde se verifica quais os parâmetros identifi-cáveis, quais podem ser agrupados e os quais não são identificáveis
• Planeamento da trajectória óptima. Inclui a escolha da parametrização da trajectória e um critérioóptimo
• Identificação, durante a qual são recolhidos os dados relativos ao movimento e às forças neces-sárias
• Estimação dos parâmetros desconhecidos através de algoritmo de estimação
• Validação experimental dos parâmetros estimados.
1.3 Objectivos da Dissertação
Esta dissertação tem como objectivo o desenvolvimento de uma interface do robô manipuladorPUMA 560 com um computador e o desenvolvimento de software para a implementação em temporeal de um controlador por dinâmica inversa. Este tipo de controlo implica o conhecimento dos parâme-tros dinâmicos do robô, os quais podem ser estimados através de técnicas de identificação.
4
Nesta dissertação é utilizada uma técnica de identificação com recurso a um sensor externo (plata-forma de força) sendo que para tal é necessário a integração deste dispositivo com um computador.
Assim, os objectivos destas dissertação passam por:
• Integração do PUMA 560 com um computador
• Integração da plataforma de força com um computador
• Identificação dos parâmetros dinâmicos do PUMA 560
• Implementação de um controlador por dinâmica inversa no PUMA 560
1.4 Contribuição da Dissertação
Pretende-se que o trabalho desenvolvido possibilite a utilização do robô manipulador PUMA 560 emnovas aplicações. Um esquema de controlo por dinâmica inversa permite a utilização do robô numavasta gama de aplicações que o controlador anterior não permitia.
A integração da plataforma de força com o computador possibilita não só o uso desta técnica deidentificação em outros robôs, bem como a utilização deste dispositivo em outras aplicações.
5
6
Capítulo 2
Teoria de Robôs Manipuladores
Neste capitulo a modelação, identificação e controlo de robôs manipuladores é abordada em deta-lhe. Em qualquer aplicação robotizada, a realização de uma tarefa implica a execução de ummovimentoespecifico do robô. A perfeita execução deste movimento depende do sistema de controlo, que por suavez, deve fornecer aos actuadores do robô a informação correcta. O controlo de movimento do robôrequer uma análise cuidada das características estruturais, dos actuadores e dos sensores do mani-pulador, com vista à derivação de modelos matemáticos que descrevam a relação entrada/saída e quecaracterizam os componentes do robô.
A criação de modelos cinemáticos permite relacionar a posição e velocidade das juntas com a posee velocidade do elemento terminal, enquanto que os modelos dinâmicos permitem relacionar o movi-mento do robô com as forças exercidas sobre este. Em ambos os modelos existe a necessidade do co-nhecimento de um conjunto de parâmetros, os quais podem ser obtidos experimentalmente. Enquantoos parâmetros cinemáticos podem ser obtidos com simples processos de calibração e são geralmenteiguais para o mesmo modelo de robô, o mesmo não se verifica nos parâmetros dinâmicos. Para aestimação destes parâmetros usam-se usualmente técnicas de identificação, visto ser um processorelativamente simples, executável em qualquer ambiente e com resultados satisfatórios.
Com o conhecimento destes parâmetros é possível desenvolver e implementar algoritmos de con-trolo que permite ao manipulador executar a tarefa pretendida, sendo que toda a metodologia, desde aderivação do modelo cinemático até ao controlo é descrita nas secções seguintes.
2.1 Cinemática
A modelação de robôs numa maneira sistemática e automática requer um método adequado para adescrição da sua estrutura. Existem diversos métodos e notações, de entre os quais se destaca o deDenavit-Hartenberg [13]. Este método foi deduzido para manipuladores de cadeia aberta e apresentaambiguidades quando aplicado a manipuladores de cadeia fechada. De seguida este método recursivo,que define a posição e a orientação de dois elos consecutivos, é descrito.
2.1.1 Cinemática directa - notação de Denavit-Hartenberg
Um robô manipulador é constituído por n + 1 elos e n juntas. Assume-se que os elos são rígidose que as juntas são de revolução ou prismáticas. Uma junta complexa pode ser representada comouma combinação de juntas de revolução e prismáticas com elos de comprimento e massa nulos. Os
7
elo1
elo2
elon
Figura 2.1: Representação esquemática de um robô de cadeia aberta
elos são numerados de modo a que o elo 0 seja a base do robô e que o elo n seja o elo do elementoterminal. A junta i liga o elo i ao elo i ! 1 e a sua variável é representada por qi. De modo a definir arelação entre a localização dos elos, considera-se um referencial i solidário a cada elo i, tal que:
• o eixo zi é sobre o eixo da junta i + 1
• localizar a origem de Oi na intersecção do eixo zi com a normal comum aos eixos zi!1 e zi.Localizar Oi! na intersecção da normal comum com o eixo zi!1
• escolher o eixo xi sobre a normal comum aos eixos zi!1 e zi com a direcção da Junta i para aJunta i+ 1
• escolher o eixo yi de modo a formar um referencial de mão direita
A matriz de transformação do referencial i! 1 para o referencial i é expressa em função dos seguin-tes parâmetros geométricos:
• !i: ângulo entre zi!1 e zi em torno de xi, sendo positivo quando a rotação é feita no sentidoanti-horário
• di: coordenada de Oi! segundo zi!1
• "i: ângulo entre xi!1 e xi em torno de zi!1, sendo positivo quando a rotação é feita no sentidoanti-horário
• ai: distância entre Oi e Oi!
A variável da junta i, que define a orientação ou a posição entre os elos i e i ! 1, é "i ou di,dependendo se a junta é de revolução ou prismática respectivamente. A matriz de transformação quedefine o referencial i relativamente ao referencial i! 1 é dada por:
M i!1i =
#
$$$$%
c!i !s!ic"i s!is"i aic!is!i c!ic"i !c!is"i ais!i0 s"i c"i di
0 0 0 1
&
''''((2.1)
A matriz de transformação que define o referencial i! 1 relativamente ao referencial i é dada por:
8
elo i! 1
elo i
junta i! 1
junta i
junta i+ 1
ai!1
ai
yi!1
yi
zi!2
zi!1
zi!1
zi
xi!1
xi
!i!1
!i
"i!1
"i
"i+1di
Figura 2.2: Parâmetros de Denavit-Hartenberg
M ii!1 =
#
$$$$%
!ai
Ri!1Ti !dis"i
!dic"i
0 0 0 1
&
''''((2.2)
Assim a transformação de coordenadas que descreve a posição e orientação do referencial n emrelação ao referencial 0 é dado por:
C0n (q) = M0
1 (q1)M12 (q2) . . .M
n!1n (qn) (2.3)
Visto isto, a transformação de coordenadas que descreve a posição e a orientação do referencial doelemento terminal em relação ao referencial base é expresso por:
Cbe (q) = Cb
0C0n (q)Cn
e (2.4)
onde Cb0 e Cn
e são duas transformações homogéneas constantes, que descrevem a posição e ori-entação do referencial 0 em relação ao referencial base, e a posição e orientação do referencial doelemento terminal em relação ao referencial n, respectivamente.
9
2.2 Dinâmica
O conhecimento do modelo dinâmico de ummanipulador permite a simulação de movimento, análiseda estrutura e o desenvolvimento de algoritmos de controlo de manipuladores. Isto possibilita testartécnicas de controlo e planear trajectórias sem a necessidade de possuir o sistema fisicamente.
O modelo dinâmico, consiste em relacionar o movimento do manipulador com as forças aplicadas.Dependendo do tipo de sensores utilizados para a aquisição das forças é possível classificar os mo-delos em duas categorias distintas: interno, onde o valor da corrente no amplificador de potência domanipulador é utilizado para estimar o binário de junta aplicado e ser relacionado com o movimento domanipulador, e o externo, onde as forças são adquiridas através de um sensor externo ao manipuladore relacionados igualmente com o movimento do manipulador. Os dados relativos ao movimento domanipulador são geralmente registados por encoders ou tacómetros. O uso de um sensor externo, ge-ralmente plataformas de força, possibilita a formulação de um modelo dinâmico independente do atritoe da dinâmica do actuador, sendo esta a grande vantagem deste tipo de modelos.
De seguida é apresentada a formulação para a derivação do modelo dinâmico externo, seguida daapresentação da formulação do modelo interno tendo por base o modelo externo. Por último estes doismodelos são combinados num modelo único.
2.2.1 Modelo dinâmico externo
Conforme referido, o modelo dinâmico externo relaciona o movimento do manipulador com as for-ças/momentos exercidos na sua base, e este podem ser relacionados com as forças exercidas no elo 1
através de:
W1 = T 1b Wb (2.5)
onde T 1b é uma matriz de transmissão de forças [14].
Considerando um manipulador com n juntas, em que cada elo i possui um sistema local de coorde-nadas Oi solidário com o elo, as forças devido ao próprio movimento podem ser obtidas tratando o elocomo uma massa. Esta teoria encontra-se explicada em [15]. As equações 2.6 e 2.7, representam asforças e os momentos exercidos sobre um corpo devido ao próprio movimento.
Fi,i = mi
!pii ! g
"+ #i
i "micii + #i
i "!#ii "mic
ii
"(2.6)
Mi,i =!g ! pii
""mic
ii + Iqi #
ii + #i
i "!Iqi #
ii
"+mic
ii "
!#ii " cii
"+mic
ii "
!#ii "
!#ii " cii
""(2.7)
Os termos cii "!#ii " cii
"e cii "
!#ii "
!#ii " cii
""são quadráticos na localização desconhecida do
centro de massa cii, e podem ser eliminados se o tensor de inércia for expresso no referencial do eloi (Ipi ), em vez de ser expresso no referencial do centro de massa (Iqi ). Aplicando isto, a equação 2.7toma a forma de
Mi,i =!g ! pii
""mic
ii + Ipi #
ii + #i
i "!Ipi " #i
i
"(2.8)
10
As equações 2.6 e 2.8 podem ser rescritas e organizadas em forma matricial como
#
$%Fi,i
Mi,i
&
'( =
#
$$%
pii ! g )#ii +
)#ii)#ii 0
0 !g ! pii #ii +
)#ii#
ii
&
''(
#
$%mi
miciiIpi
&
'( (2.9)
mais compactamente
Wi,i = Ai$i (2.10)
onde Wi,i é o , Ai representa a matriz de regressão que descreve o movimento do elo i e $i repre-senta o vector dos parâmetros inerciais do elo i.
As forças exercidas no elo i, são função, não só do movimento do elo i, mas também como de todosos elos posteriores a este. As forças no elo i devido ao movimento do elo i + 1 são determinados,transmitindo as forças aplicadas no elo i+ 1 . Esta transmissão apenas depende da geometria do robôe é expressa pela equação 2.11.
#
$%Fi,i+1
Mi,i+1
&
'( =
#
$%Ri
i+1 0
"rii!1,iRii+1 Ri
i+1
&
'(
#
$%Fi+1,i+1
M i+1,i+1
&
'( (2.11)
mais compactamente
Wi,i+1 = T ii+1Wi+1,i+1 (2.12)
Onde Wi,i+1 representa o vector de forças exercidas no elo i devido ao movimento do elo i + 1 eT ii+1 é a matriz de transmissão de forças.
O vector de forças aplicados no elo i é denotado porWi, e é expresso pela equação 2.13.
Wi =n*
j=i
Wi,j (2.13)
As forças aplicadas a cada elo de um manipulador são expressas pela equação 2.14.
#
$$$$%
W1
W2
...Wn
&
''''(=
#
$$$$%
U11 U12 · · · U1n
0 U22 · · · U2n
0 0. . . ...
0 0 0 Unn
&
''''(
#
$$$$%
$1
$2
...$n
&
''''((2.14)
com,
Uij =
+,
-Ai se i = j
T ii+1T
i+1i+2 . . . T j!1
j Aj se i #= j(2.15)
Relembrando a equação 2.5
11
W1 = T 1b Wb =
.U11 U12 · · · U1n
/
#
$$$$%
$1
$2
...$n
&
''''((2.16)
ou mais compactamente
W1 = T 1b Wb = U1$ (2.17)
reorganizando a equação anterior
Wb =!T 1b
"!1U1$ = Yb$ (2.18)
com
Yb =!T 1b
"!1Ub (2.19)
Onde a equação 2.18 representa a relação entre o movimento do manipulador e as forças exercidasna sua base e pode ser escrita de forma generalizada como:
%g = Yg$g (2.20)
As grandezas presentes na matriz de regressão Ai, são calculadas através de:
#ii =
+,
-Ri!1T
i #i!1i!1 para junta prismática
Ri!1Ti
!#i!1i!1 + "iz0
"para junta de revolução
(2.21)
#ii =
+,
-Ri!1T
i #i!1i!1 para junta prismática
Ri!1Ti
0#i!1i!1 + "iz0 + "i#
i!1i!1 " z0
1para junta de revolução
(2.22)
pii =
+222222,
222222-
Ri!1Ti
0pi!1i!1 + diz0
1+ 2di#i
i "Ri!1Ti z0
+#ii " rii!1,i + #i
i "!#ii " rii!1,i
"para junta prismática
Ri!1Ti pi!1
i!1 + #ii " rii!1,i
+#ii "
!#ii " rii!1,i
"para junta de revolucao
(2.23)
onde todas as grandezas se encontram expressas no referencial do próprio corpo.Um algoritmo recursivo pode ser desenvolvido com o intuito de obter as forças em cada elo do
manipulador. Este algoritmo resume-se a:
• Com condições iniciais conhecidas #00 , p00, #0
0 , usar as equações 2.21, 2.22 e 2.23 para i = 1, . . . , n
para cálculo de #ii, #i
i e pii.
• Com condições conhecidas do elemento terminal,Wn+1, usar as equações 2.9 , 2.11 e 2.13, parai = n, . . . , 1, para cálculo deWi, seguido da equação 2.26 para cálculo de %i.
A estrutura do algoritmo recursivo é ilustrada na figura 2.3.
12
#00 , p
00 ! g00 , #
00
#11 , #
11 , p
11
#n!1n!1 , #
n!1n!1 , p
n!1n!1
#nn , #
nn , p
nn
q1, q1, q1
qn, qn, qn
W1
W2
Wn
Wn+1
...
...
%1
%n
Wb
2.18
2.21
2.21
2.22
2.22
2.23
2.23
2.9
2.92.11
2.112.13
2.13
2.26
2.26
Figura 2.3: Estrutura do algoritmo recursivo
13
2.2.2 Modelo dinâmico interno
Partindo do modelo externo é possível obter uma formulação que permite relacionar os binários dejunta com o movimento do manipulador. Este modelo interno pode simplesmente ser obtido através daprojecção das forças exercidas no elo i, no eixo da junta i [12]. Primeiramente é necessário transmitiras forças que actuam no elo i, e que estão expressas no referencial do corpo i, para um novo referencialOji no qual o eixo zji se encontra alinhado com o eixo da junta i. Isto é obtido através:
#
$%Fji
Mji
&
'( =
#
$$%
Rjii 0
3rjii,jiRjii Rji
i
&
''(
#
$%Fi
Mi
&
'( (2.24)
ou mais compactamente,
Wji = T jii Wi (2.25)
onde Wji representa as forças exercidas sobre o elo i expressos no referencial Oji e T jii a matriz
de transmissão de forças do referencial do elo i para o referencial Oji .Como o eixo zji se encontra alinhado com o eixo da junta i podemos escrever que:
%i = Mzji= T ji
ir6Wi (2.26)
Onde %i, representa o binário da junta i e T jiir6representa a sexta linha da matriz T ji
i .Para um manipulador com n juntas, o modelo dinâmico interno toma a forma de:
#
$$$$%
%1
%2...%n
&
''''(=
#
$$$$%
T j11r6 0 0 0
0 T j22r6 0 0
0 0. . . 0
0 0 0 T jnnr6
&
''''(
#
$$$$%
W1
W2
...Wn
&
''''((2.27)
Lembrando a equação 2.14, a equação anterior pode ser reescrita na seguinte forma
#
$$$$%
%1
%2...%n
&
''''(=
#
$$$$%
T j11r6 0 0 0
0 T j22r6
0 0
0 0. . . 0
0 0 0 T jnnr6
&
''''(
#
$$$$%
U11 U12 · · · U1n
0 U22 · · · U2n
0 0. . . ...
0 0 0 Unn
&
''''(
#
$$$$%
$1
$2
...$n
&
''''((2.28)
simplificando,
#
$$$$%
%1
%2...%n
&
''''(=
#
$$$$%
T j11r6U11 T j1
1r6U12 · · · T j11r6U1n
0 T j22r6U22 · · · T j2
2r6U2n
0 0. . . ...
0 0 0 T jnnr6
Unn
&
''''(
#
$$$$%
$1
$2
...$n
&
''''((2.29)
representando portanto a relação entre o movimento do manipulador e os binários de junta, onde oatrito de junta não é considerado.
14
2.2.3 Modelo dinâmico combinado
Conforme referido e demonstrado em [12], a utilização de um modelo dinâmico que combine osdados relativos ao modelo interno e externo, permite uma melhoria na estimação dos parâmetros iner-ciais, além de permitir também a estimação dos parâmetros de junta, e na predição do binário dosactuadores.
Relembrando a equação 2.16, esta pode ser reescrita como:
Wb = T b1W1 (2.30)
com,
T b1 =
!T 1b
"!1 (2.31)
Este equação pode ser combinada e organizada em forma matricial com a equação 2.27, originandoum modelo dinâmico combinado.
#
$$$$$$$%
Wb
%1
%2...%n
&
'''''''(
=
#
$$$$$$$%
T b1 0 · · · 0
T j11r6 0 · · · 0
0 T j22r6 · · · 0
0 0. . . 0
0 0 T jnnr6
&
'''''''(
#
$$$$%
W1
W2
...Wn
&
''''((2.32)
Relembrando a equação 2.14, podemos escrever#
$$$$$$$%
Wb
%1
%2...%n
&
'''''''(
=
#
$$$$$$$%
T b1 0 · · · 0
T j11r6 0 · · · 0
0 T j22r6 · · · 0
0 0. . . 0
0 0 T jnnr6
&
'''''''(
#
$$$$%
U11 U12 · · · U1n
0 U22 · · · U2n
0 0. . . ...
0 0 0 Unn
&
''''(
#
$$$$%
$1
$2
...$n
&
''''((2.33)
Simplificando a equação anterior, obtemos#
$$$$$$$%
Wb
%1
%2...%n
&
'''''''(
=
#
$$$$$$$%
T b1U11 T b
1U12 · · · T b1U1n
T j11r6U11 T j1
1r6U12 · · · T j11r6U1n
0 T j22r6U22 · · · T j2
2r6U2n
0 0. . . ...
0 0 0 T jnnr6
Unn
&
'''''''(
#
$$$$%
$1
$2
...$n
&
''''((2.34)
Para a dedução do modelo dinâmico interno, o efeito do atrito não foi considerado, no entantodevido à sua influência na dinâmica do manipulador, existe a necessidade de inclui-lo. Isto é obtidoconsiderando o seguinte modelo de atrito
%fi = fvi qi (2.35)
onde fvi representa o parâmetro de atrito viscoso.Relembrando a equação 2.34 e combinando-a com a equação 2.35, o modelo dinâmico combinado
considerando o atrito é descrito pela seguinte equação:
15
#
$$$$$$$%
Wb
%1
%2...%n
&
'''''''(
=
#
$$$$$$$%
T b1U11 T b
1U12 · · · T b1U1n 0 0 0 0
T j11r6U11 T j1
1r6U12 · · · T j11r6U1n q1 0 0 0
0 T j22r6U22 · · · T j2
2r6U2n 0 q2 0 0
0 0. . . ... 0 0
. . . 0
0 0 0 T jnnr6
Unn 0 0 0 qn
&
'''''''(
#
$$$$$$$$$$$$$$$%
$1
$2
...$n
fv1fv2...
fvn
&
'''''''''''''''(
(2.36)
mais compactamente
%g = Yg$g (2.37)
2.3 Identificação
Conforme referido na secção 1.2 a identificação é o método de estimação dos parâmetros quemelhores resultados apresenta. Este método requer que o modelo dinâmico derivado seja linear emrelação aos referidos parâmetros. Caso isto se verifique pode ser aplicado um algoritmo para estimaçãodos parâmetros pretendidos.
2.3.1 Parâmetros do modelo de identificação
Geralmente nem todos os parâmetros do modelo dinâmico podem ser identificados, sendo que estespodem ser divididos em três categorias: completamente identificáveis, identificáveis em combinaçõeslineares e não identificáveis.
O conjunto de parâmetros do modelo dinâmico pode ser simplificado de modo a obter um conjuntosde parâmetros base [16, 17] e é definido como o conjunto mínimo de parâmetros necessários paradescrever o modelo dinâmico. Estes parâmetros são obtidos desprezando os parâmetros que não têmefeito no modelo dinâmico e reagrupando outros em combinações lineares.
O tamanho do conjunto de parâmetros base pode ser obtido através da característica da matriz deidentificação Yg. Os parâmetros sem influência na dinâmica do manipulador e portanto não identificá-veis, são obtidos através da análise da matriz de regressão Yg, sendo estes os parâmetros correspon-dentes às colunas nulas e com valor constante independentemente da configuração do manipulador.O procedimento para obter o conjunto base pode ser baseado na decomposição de valores singulares(SVD) [18, 19, 17] ou na decomposição ortogonal triangular [4]. A simplificação do modelo dinâmicopode ser obtida através da decomposição ortogonal triangular da matriz de regressão Y , assumindoque esta representa a matriz Yg sem as colunas referentes aos parâmetros não identificáveis, conformese apresenta de seguida.
QTY =
4R
0(r!c)"c
5
(2.38)
onde Q é uma matriz ortogonal de dimensão r"r, R é uma matriz triangular superior com dimensãoc" c, r e c são respectivamente o número de linhas e colunas da matriz Y .
16
Teoricamente, os parâmetros dependentes são os correspondentes aos valores da diagonal damatriz R iguais a zero. Definindo ' como sendo o zero numérico
' = r " ("maxi
(|Rii|) (2.39)
onde |Rii| é o valor absoluto de Rii e ( é a precisão do computador.
Assim, se |Rii| < ', então o parâmetro i e a coluna i da matriz de Y são dependentes, sendo que oparâmetro i será agrupado com outro parâmetro. Contrariamente se |Rii| > ', a coluna i da matriz Y
é independente e o parâmetro i são independentes. Agrupando as b colunas independentes na matrizY 1 e os parâmetros correspondentes no vector $1, e as restantes colunas e parâmetros em Y 2 e $2
respectivamente, podemos escrever
Y $ =.Y 1 Y 2
/ 4 $1
$2
5
(2.40)
A matriz Y 2 pode ser escrita em termos de Y 1 da seguinte forma
Y 2 = Y 1) (2.41)
onde ) representa a matriz que permite obter as equações de agrupamento dos parametros $1 com$2.
Consequentemente
Y $ =.Y 1 Y 2
/ 4 $b
0
5
= Y 1$b (2.42)
onde o vector de parâmetros base, $b, é dado por
$b = $1 + )$2 (2.43)
De modo a determinar ), é calculada a decomposição da matriz.Y 1 Y 2
/, a qual é escrita como
.Y 1 Y 2
/=
.Q1 Q2
/ 4 R1 R2
0(r!b)"b 0(r!b)"(c!b)
5
=.Q1R1 Q2R2
/(2.44)
onde R1 é uma matriz r " r triangular superior igual a R1:b,1:b e R2 é uma matriz de dimensãob" (c! b) igual a R1:b,b+1:c. Da equação 2.44 obtém-se
Q1 = Y 1!R1
"!1 (2.45)
Y 2 = Q1R2 = Y 1!R1
"!1R2 (2.46)
e finalmente, com recurso à equação 2.41
) =!R1
"!1R2 (2.47)
17
2.3.2 Planeamento experimental
Um factor determinante no processo de identificação é a escolha da trajectória de excitação. Estadeve excitar suficientemente o sistema de modo a ser possível identificar todos os parâmetros, se talnão acontecer pode não ser possível identificar alguns parâmetros ou estes serem sensíveis ao ruídopresente nos dados. A qualidade da estimação dos parâmetros depende da qualidade dos dadosdisponíveis e está directamente relacionada com a escolha da trajectória de excitação.
A importância da trajectória de excitação na identificação dos parâmetros é reconhecida por muitosinvestigadores, sendo que o primeiro trabalho desenvolvido sobre este assunto pode ser encontradoem [20].
O planeamento de uma trajectória de excitação inclui a escolha da parametrização da trajectória,um critério de optimização e a resolução do problema de optimização.
Critério de optimização
A escolha da trajectória deve garantir a excitação do sistema de tal forma que seja possível identificartodos os parâmetros. Para tal é necessário o uso de técnicas de optimização, sendo que de seguidasão discutidos os critérios de optimização geralmente usados.
Em [20] é sugerido minimizar o número de condição da matriz Y, sendo este definido por
cond (Y ) =*max (Y )
*min (Y )(2.48)
onde *max (Y ) e *min (Y ) representação o maior e menor valor singular da matriz Y , respectiva-mente. Um pequeno número de condição diminui a sensibilidade da solução dos mínimos quadrados aerros no dados recolhidos. A maior parte dos trabalhos relacionados com identificação dos parâmetrosdinâmicos de robôs manipuladores utiliza este critério de optimização.
Em [21] é provado que o uso de um critério de optimização baseado no determinante da matriz deregressão Y , gera trajectórias de excitação que possibilitam a estimação dos parâmetros dinâmicoscom menores intervalos de incerteza. Este tipo de critério é definido por
! log (det (Y )) (2.49)
Parametrização da trajectória de excitação
A escolha da parametrização da trajectória de excitação é uma questão importante, visto que de-termina directamente o número de parâmetros no problema de optimização e o esforço para calcular avelocidade e aceleração através dos dados das posições.
Várias parametrizações têm sido usadas, em [22] são usados polinómios de quinto grau, onde oscoeficientes destes polinómios são optimizados usando algoritmos de optimização não lineares. Em[2] é primeiro resolvido o algoritmo de optimização para encontrar um conjunto de pontos óptimos,sendo estes pontos interpolados de seguida de modo a obter trajectórias de junta suaves, atravésde polinómios de quinto grau ou spilines. Os coeficientes são fixos impondo constrangimentos decontinuidade entre cada troço da trajectória.
Em [21], é introduzido o conceito de excitação periódica. A trajectória de excitação consiste numasoma de funções harmónicas, ou seja, uma série finita de Fourier.
18
qi (t) = q0i +Ni*
l=1
ai,l#f l
sin (#f lt)!bi,l#f l
cos (#f lt)
qi (t) =Ni*
l=1
ai,l cos (#f lt) + bi,l sin (#f lt) (2.50)
qi (t) =Ni*
l=1
!ai,l#f l sin (#f lt) + bi,l#f l cos (#f lt)
onde #f é a frequência natural da série de Fourier, ai,l e bi,l para i = 1 a Ni representam as ampli-tudes dos senos e cosenos e q0i a configuração em torno da qual ocorrerá a excitação. Cada série deFourier possui 2"Ni + 1 parâmetros que representam as variáveis do problema de optimização e quesão representados pelo vector +. De entre várias vantagens deste tipo de trajectórias destaca-se
• a especificação da largura de banda da trajectória de excitação permite evitar ou incluir a flexibili-dade do robô
• o cálculo das velocidades e acelerações de junta de forma analítica
Problema de optimização
A formulação matemática do problema de optimização é dado por:
+ = argmin# ! log (det (Y ))
sujeito aqmin $ q $ qmax
!qmax $ q $ qmax
!qmax $ q $ qmax
(2.51)
onde qmin, qmax, qmax, qmax representam o limite inferior e superior da posição de junta, a veloci-dade e aceleração máxima de junta, respectivamente. Verifica-se que os problemas de optimização detrajectória envolvem geralmente um grande número de variáveis de optimização, sendo que a resolu-ção deste tipo de problemas requer tempo computacional e não sendo garantido que a solução seja ummínimo global.
2.3.3 Estimação dos parâmetros
Depois de o modelo dinâmico ser obtido e da trajectória ser optimizada, a parte experimental podeser iniciada. Durante a parte experimental os dados necessários são adquiridos e com base nestes osparâmetros são estimados.
Existem dois métodos distintos para a estimação, os métodos de estimação offline, onde é usadoum conjunto de dados adquirido e os parâmetros são estimados em apenas um passo, e os métodosde estimação online onde os parâmetros são calculados e actualizados sempre que um novo conjuntode dados é adquirido. Os métodos de estimação offline são frequentemente usados pois a sua imple-mentação é por norma mais simples.
19
O método dos mínimos quadrados é geralmente utilizado para resolver sistemas de equações linearsobredeterminados. Outros métodos mais complexos existem que permitem levar em conta informaçãoacerca do ruído.
Como referido na secção 2.2, o modelo dinâmico pode ser obtido como uma função linear dosparâmetros inerciais.
% = Y (q, q, q)$ (2.52)
A maneira mais simples para estimar $ é usar a solução do método dos mínimos quadrados parasistema sobredeterminados [23]. O estimador dos mínimos quadrados (LS) assume que a matriz deidentificação Y é de característica máxima , isto é, o conjunto de parâmetros é minimo (sub-secção2.3.1) . A solução dos mínimos quadrados é dada por:
$LS =!Y TY
"!1Y T % (2.53)
onde!Y TY
"!1Y T é a pseudo-inversa da matriz Y . O estimador LS assume que os dados recolhi-
dos são corrompidos com ruído branco Gaussiano e que o desvio padrão é igual para todos os canais.Na prática, isto não se verifica, mas devido à simplicidade este método é geralmente utilizado.
2.4 Controlo
O controlo de um robô manipulador resume-se à determinação das forças generalizadas (forças oubinários) a serem desenvolvidas pelos actuadores das juntas de modo a garantir a execução da tarefapretendida. Existem várias técnicas para o controlo de robôs manipuladores, sendo que a técnicautilizada bem como o modo de implementação influenciam a performance e o campo de aplicações dorobô manipulador.
Neste secção são apresentadas duas técnicas de controlo de movimento para robôs manipuladores.Primeiramente é apresentada uma técnica de controlo do tipo proporcional-derivativo (PD), seguida deuma técnica de controlo por dinâmica inversa.
2.4.1 Controlador PD
É provado em [13] que qualquer posição do manipulador é globalmente assimptóticamente estávelsob um controlador com acção linear PD, sendo a lei de controlo dada por
u = KP (qd ! qr) +KD qr (2.54)
onde u é um vector (n" 1) que representa a lei de controlo, qd é um vector (n" 1) das posições dejunta desejadas, qr e qr são vectores (n" 1) das posições e velocidades de junta reais respectivamentee KP e KD são matrizes diagonais (n" n) que representam os ganhos proporcionais e derivativosrespectivamente. A estabilidade é garantida para qualquer valor de KP e KD, desde que as matrizessejam definidas positivas.
O diagrama de blocos resultante é ilustrado na figura 2.4.
20
KP PUMA 560
ddt
KD
qd++
!!
e u qr
qr
Figura 2.4: Diagrama de blocos de controlador PD no espaço de juntas
2.4.2 Controlador por dinâmica inversa
O modelo dinâmico de um robô manipulador pode ser expresso por
% = Y (q, q, q)$ (2.55)
O objectivo é encontrar uma lei de controlo u, função do estado do sistema, capaz de realizarrelações entrada/saída do tipo linear, ou seja, é desejado realizar não uma linearização aproximadamas sim um linearização exacta da dinâmica do sistema obtida com realimentação de estado não-linear.
Considerando a lei de controlo u como função do estado do manipulador na forma
u = Y (q, q, q)$ (2.56)
leva ao sistema descrito por
q = , (2.57)
onde , representa um novo vector de entrada.A lei de controlo não linear na equação 2.56 é denominada de controlo por dinâmica inversa visto
que é baseada na dinâmica inversa do manipulador.Pretende-se que esta lei de controlo seja linear e desacoplado em relação à nova entrada ,. O
problema de controlo do manipulador resume-se a encontrar a lei de controlo , que estabilize o sistema.A escolha
, = qd +KD (qd ! qr) +KP (qd ! qr) (2.58)
conduz a um sistema de 2ª ordem e a escolha de KD e KP como matrizes diagonais do tipo
KP = diag6#2n1, . . . ,#
2nn
7KD = diag {2&1#n1, . . . , 2&n#nn}
permite obter um sistema desacoplado, onde #ni e &i representam a frequência natural e o factorde amortecimento do sistema respectivamente. O diagrama de blocos resultante é representado nafigura 2.5, no qual existem dois anéis de controlo, um interno baseado na dinâmica do manipulador eum externo para correcção do erro de seguimento.
A função do anel interno é obter uma relação entrada/saída linear e desacoplada e o anel externo énecessário para garantir a estabilidade do sistema global.
21
qd +KD (qd ! qr) +KP (qd ! qr) Y (qr, qr, ,)$ PUMA 560
ddt
, % qr
qr
qd
qd
qd
Figura 2.5: Controlador por dinâmica inversa
22
Capítulo 3
Descrição do Hardware
Neste capítulo, o hardware utilizado neste trabalho é apresentado. É feito um breve resumo sobrecada dispositivo, enumerando as suas principais características. Os diferentes protocolos de comunica-ção dos diferentes dispositivos são apresentados, bem como o trabalho desenvolvido para possibilitara interacção entre todos.
3.1 Robô Manipulador PUMA 560
O PUMA 560 é um robô manipulador de classe industrial com seis graus de liberdade. Este manipu-lador possui um espaço operacional considerável e tem capacidade para atingir acelerações elevadas.Originalmente desenvolvido para tarefas de montagem e manipulação, o PUMA 560 é no presenteamplamente utilizado por instituições académicas para fins de pesquisa.
3.1.1 Integração do PUMA 560 com PC
A arquitectura do controlador era baseada na placa de interface TRC004, que por sua vez comuni-cava com um computador externo através da placa TRC006 (bus ISA). Dado que este bus foi substituídopelo bus PCI, torna-se necessário desenvolver novo hardware, utilizando placas de aquisição PCI deforma a obter total conectividade entre o manipulador, controlador e o computador.
PC Externo
TRC006
Controlador
TRC004Analog
ServoAmp.
PUMA 560
Motores
EncodersHardware original
Figura 3.1: Arquitectura do controlador antiga
A nova arquitectura é baseada numa placa de aquisição de dados Quanser Q8 (figura 3.3), a qualé instalada num bus PCI de um computador externo. Os binários dos motores são controlados pelo
23
controlador Mark II, com referência em tensão, enviados a partir do computador externo através daplaca Q8.
A Quanser Q8 é uma placa de controlo com uma extensa gama de entradas e saídas. O facto depossuir entradas e saídas quer analógicas como digitais, bem como entradas para encoders, junta-mente com a possibilidade de fácil integração com Matlab/Simulink, tornam este dispositivo ideal paraa interface com o PUMA 560. As entradas dos encoders da Q8 são ligadas aos encoders das juntasdo PUMA para aquisição da posição, as saídas analógicas são ligadas aos amplificadores de potên-cia para comandar o binário dos motores, as entradas e saídas digitais são conectadas às diferentesfunções de monitorização, a saber, sensores térmicos das juntas e controlo de energia dos motores.Todas estas ligações são efectuadas através da placa de terminais da Q8 que se encontra ilustrada nafigura 3.3. Por questões de segurança, o watchdog da Q8 foi programado de modo a comutar todos oscanais digitais caso não sejam efectuadas escritas na placa em dois instantes de tempo consecutivos.
PC Externo
Quanser
Q8
Controlador
Q8
Terminal
Board
Analog
ServoAmp.
PUMA 560
Motores
EncodersHardware original
Figura 3.2: Arquitectura do controlador implementada
Figura 3.3: Placa de aquisição Quanser Q8 (à esquerda) e placa de terminais (à direita)
3.1.2 Controlador do PUMA 560
Com o objectivo de desenrolar todo o procedimento experimental, foi necessário implementar umcontrolador que possibilite a execução da tarefa pretendida. Foi implementado no PUMA 560 um es-quema de controlo PD com a seguinte lei de controlo:
u = KP ("d ! "r)!KD"r (3.1)
onde u é um vector (n" 1) que representa a acção de controlo, "d é um vector (n" 1) das posiçõesde junta desejadas, "r e "r são vectores (n" 1) das posições e velocidades de junta reais respec-
24
tivamente e KP e KD são matrizes diagonais (n" n) que representam os ganhos proporcionais ederivativos respectivamente.
KP PUMA 560
ddt
KD
"d
++
!!
e u "r
"r
Figura 3.4: Diagrama de blocos do controlador PD implementado
A arquitectura do software implementado é baseado em Matlab e Simulink. O Simulink possibilitaprojectar algoritmos de controlo com extrema facilidade e rapidez e permite o uso de funções específi-cas desenvolvidas em código C e implementadas como S-functions.
O controlador PD foi implementado em Simulink, tal como é ilustrado na figura 3.4. O esquema decontrolo é desenvolvido com blocos de Simulink e a comunicação com a Q8 é garantida através do usoda toolbox xPC Target do Simulink. Esta toolbox permite a integração da Q8 de modo a receber e enviarpara o manipulador, os dados necessários para o seu controlo.
Posteriormente, o xPC Target é utilizado para a execução em tempo real do código C gerado peloReal Time Workshop a partir do diagrama de Simulink.
3.2 Plataforma de Força e Amplificador
O sistema de aquisição de forças de reacção na base do manipulador é constituído por uma plata-forma de forças e um amplificador. A plataforma de força usada é fabricada pela AMTI e trata-se domodelo BP400600-1000. Esta plataforma foi especialmente desenhada para um registo preciso dasforças segundos os eixos XYZ. De entre as várias características desta plataforma destacam-se:
• alta sensibilidade
• baixa interferência entre canais
• alta frequência de aquisição de dados
• estabilidade
O amplificador utilizado é um DigiAmp DSA-6 também fabricado pela AMTI e desenhado para in-terface com as plataformas de força da AMTI. Este amplificador possui saída digital e analógica, baixonível de amplificação e um conjunto de filtros e de algoritmos de condicionamento que proporcionamuma elevada qualidade de sinal e um baixo nível de ruído.
3.2.1 Plataforma
A plataforma mede as forças e os momentos através de seis pontes extensométricas de Wheatstone(ver figura 3.7).
25
Figura 3.5: Plataforma AMTI BP4006000-1000
Figura 3.6: AMTI DigiAmp DSA-6
A ponte de Wheatstone [24] é um circuito usado para medir resistências eléctricas. O circuito écomposto por uma fonte de alimentação, um voltímetro e uma rede de quatro resistências, sendo queos valores de três destas são conhecidos. O diagrama do circuito é representado pela figura 3.8.
Para determinar o valor da resistência desconhecida, as outras três são ajustadas e balanceadasde modo a que a tensão eléctrica no voltímetro caia a zero. Sendo RX a resistência desconhecida aser medida, R1 e R3 as resistências cujos valores são conhecidos e R2 um potenciómetro, se a razãono ramo conhecido, R2/R1, for igual à razão do outro ramo, R3/RX, então a tensão eléctrica entre ospontos C e B será nula. Quando isto se verifica, o circuito diz-se balanceado e o valor da resistênciadesconhecida RX , pode ser obtido através de:
RX =R1R3
R2(3.2)
Neste caso especifico, RX será uma resistência sensível à força e que varia de acordo com a forçaaplicada na plataforma.
Alternativamente se os valores de R1, R2 e R3 são conhecidos mas R2 não é ajustável, o valor datensão ou da corrente eléctrica é utilizado para o cálculo do valor de RX através das leis de Kirchoff.Este tipo de configuração é geralmente utilizada, visto ser mais fácil a leitura da tensão do que o ajustedo potenciómetro.
A figura 3.9 mostra a convenção de eixos usado para a medição das forças e momentos. A origemreal dos eixos XYZ está localizada a uma distância C0 relativa ao centro geométrico da placa superior.A localização da origem relativamente à superfície da placa superior é determinada através de um pro-cedimento de calibração e é fornecida pelo fabricante no relatório de calibração e pode ser encontradono anexo A. A calibração da plataforma envolve a determinação da sensibilidade de cada canal paratodas as componentes da carga aplicada. O relatório de calibração fornece a sensibilidade dos canaisnuma matriz 6" 6. A saída de cada canal é representada pela coluna esquerda e a carga aplicada pelalinha superior (ver tabela A.2). Os termos da diagonal principal representam a sensibilidade de cadacanal enquanto que os restantes termos representam os valores da interferência entre canais. O cálculoda tensão na saída para uma carga aplicada conhecida envolve o uso da matriz de sensibilidade.
26
Fx Fy
Fz Mx
My Mz
+ Excitação
- Excitação
+ Saída- Saída
Ponte Fz = 700 ohm
Pontes Fx, Fy,Mx,My,Mz = 350 ohm
A
B
C D
E
F
G H
J
K
L M
N
R S
a
P
T
U
V W
X
Z
Y
Figura 3.7: Pontes de Wheatstone da plataforma AMTI BP400600-1000
C
A
B
D
V
R1
R2
RX
R3
Figura 3.8: Diagrama genérico da ponte de Wheatstone
27
Capacidade Fz (N) 4450Capacidade Fx, Fy (N) 2225Capacidade Mz (Nm) 700Capacidade Mx (Nm) 1300Capacidade My (Nm) 900
Frequência Natural Fz (Hz) 380Frequência Natural Fx, Fy (Hz) 300
Altura (mm) 82.5Peso (kg) 32
Material da placa superior AlumínioExcitação 10V máximoInterferência menos de 2% em todos os canais
Intervalo de Temperatura -17 a 52#CFx, Fy, Fz histerese ±0.2 % Full Scale Output
Fx, Fy, Fz não linearidades ±0.2 % Full Scale Output
Tabela 3.1: Especificações da plataforma
A tensão eléctrica observada na saída de cada canal, devido a aplicação de uma carga conhecidapode ser calculada através de:
#
$$$$$$$$$%
VFx
VFy
VFz
VMx
VMy
VMz
&
'''''''''(
=
#
$$$$$$$$$%
S11 S12 S13 S14 S15 S16
S21 S22 S23 S24 S25 S26
S31 S32 S33 S34 S35 S36
S41 S42 S43 S44 S45 S46
S51 S52 S53 S54 S55 S56
S61 S62 S63 S64 S65 S66
&
'''''''''(
#
$$$$$$$$$%
Fx
Fy
Fz
Mx
My
Mz
&
'''''''''(
" CF (3.3)
Onde CF , é um factor de conversão, que depende do ganho do amplificador e da tensão de excita-ção.
A inversa da matriz de sensibilidade (ver tabela A.3) pode ser utilizada para o cálculo da cargaaplicada através da tensão eléctrica registada na saída de cada canal através de:
#
$$$$$$$$$%
Fx
Fy
Fz
Mx
My
Mz
&
'''''''''(
=
#
$$$$$$$$$%
B11 B12 B13 B14 B15 B16
B21 B22 B23 B24 B25 B26
B31 B32 B33 B34 B35 B36
B41 B42 B43 B44 B45 B46
B51 B52 B53 B54 B55 B56
B61 B62 B63 B64 B65 B66
&
'''''''''(
#
$$$$$$$$$%
VFx
VFy
VFz
VMx
VMy
VMz
&
'''''''''(
"1
CF(3.4)
Onde,
B = S!1 (3.5)
O factor de conversão, CF , é calculado através de:
CF = Vex "Gamp " 10!6 (3.6)
A tabela 3.1 apresenta o resto das especificações da plataforma AMTI BP400600-1000.
28
Fx
Fy Fz
Cabo
4 oríficios para fixação da base Ø10.3
600
400
122
375
356
13
581
82.5
Figura 3.9: Convenção de eixos e dimensões da plataforma AMTI BP400600 - 1000
3.2.2 Amplificador
O DigiAmp é composto por uma variedade de hardware e software, como se verifica através dafigura 3.10, que proporcionam uma diversidade de características entre as quais se destacam:
• alta estabilidade da tensão de excitação das pontes extensométricas
• ganhos elevados, baixo nível de amplificação de ruído
• conversores A/D de alta resolução
• filtros digitais
• calibração interna dos conversores A/D
Além destas características, o DigiAmp possibilita duas formas distintas de interface, uma física eoutra Ethernet que permite uma completa aquisição dos dados digitais. Ambas as interfaces permitemacesso às configurações e parâmetros que controlam a performance do amplificador. O amplificadorpossuí uma gama de ganhos entre 1000 e 8000, tensão de excitação entre 1 e 10V e filtros passa-baixo com largura de banda entre os 10Hz e os 1.6kHz. O conversor A/D interno permite obter umataxa agregada de 96000 amostras por segundo, referente às 16000 amostras por segundo de cada umdos seis canais.
A configuração destes parâmetros feita por Ethernet, segue o protocolo de comunicação UDP. Pri-meiramente é necessário definir o ID do amplificador, sendo isto feito com recurso à interface físicaoferecida pelo amplificador. Este passo é fundamental, visto que o ID define o endereço IP e a portado DigiAmp, que serão necessários para estabelecer a ligação UDP. O IP do amplificador é definidopor 192.168.0. (150 + ID), e o amplificador encontra-se configurado para enviar dados através da porta
29
Strain GageBridge
(one of six)
High GainAmplifier
AntialiasingFilter
A/D Converter 16 bit
AutozeroD/A Converter
21 bit
ExcitationDrive Amplifier
Ethernet
CommunicationsProcessor
TCP/IP StackEthernet Interface
Bussel orButterworthDigital Filter
Deskewing Filter
Active GainCompensation
D/A Converter13 bit
Analog Output(one of six)
Data conversion control logicInput sample rate 16000 samples
per second per channel.Decimation1 to 32000
Internal calibration and gain compensation code
Control and Communication Code
Drive Voltage Setpoint and Compensation
AlgorithmLead Wire Resistance
Algorithm
Gain,Excitation Voltage
andFilter Setpoint Control
Figura 3.10: Representação esquemática do amplificador DigiAmp DSA-6
1024 + ID. Os dados registados pela plataforma são enviados pelo amplificador para o endereço192.168.0.150 com a porta 1025.
Uma explicação detalhada sobre o funcionamento de sockets encontra-se fora do contexto destadissertação, podendo no entanto ser resumido a:
1. Alocar memória para a socket
2. Criar uma socket do tipo Datagram, com porta e endereço baseados no ID do amplificador
3. Enviar os pacotes com os comandos pretendidos
4. Receber os pacotes com a informação pretendida
5. Fechar a socket
6. Desalocar a memória
A interface Ethernet do DigiAmp reconhece nove comandos distintos. Cada comando é identificadopor um ID único e o qual é codificado como a primeira palavra do pacote enviado. Os diferentes ID’sencontram-se na tabela 3.2, seguidos da descrição da acção que cada comando deverá provocar.
Synchronize: quando o amplificador receber este comando o contador interno é reiniciado, e iniciaimediatamente uma nova contagem dos dados convertidos a uma taxa de 16kHz. A contagem destecontador encontra-se no cabeçalho de todos os pacotes enviados pelo amplificador.
Set State: este comando é utilizado para descarregar os parâmetros de configuração do amplifica-dor, que são: ganho, tensão de excitação, tipo e frequência do filtro e tempo de amostragem. Todos osseis canais do amplificador são simultaneamente configurados por este comando.
Set Cal: comando utilizado pelo fabricante para descarregar para o amplificador constantes decalibração. De modo a evitar uso indevido é necessário introduzir uma palavra-chave neste comando.Geralmente este comando deve apenas ser usado na fábrica para definir as constantes de calibração.
30
COMMAND_SYNCHRONIZE 0x0001COMMAND_SET_STATE 0x0002COMMAND_SET_CAL 0x0004
COMMAND_ID_REQUEST 0x0005COMMAND_START_CONTINUOUS 0x0006
COMMAND_RWIRE 0x0007COMMAND_STOP 0x0008
COMMAND_AUTOZERO 0x0009COMMAND_RESET 0x000B
Tabela 3.2: ID dos comandos para interface Ethernet
PC Externo DigiAmp DSA-6 Plataforma
Placa Ethernet
192.168.0.150
Sinal Analógico
Interface Ethernet
192.168.0.(150+ID)
AMTI
BP400600-1000
Protocolo UDP
Figura 3.11: Integração da Plataforma/Amplificador com PC
ID Request: este comando solicita que o amplificador devolva um pacote com informação relativaao seu ID, revisão, número de série, data de calibração, status e código de erros.
Start Continuous: este comando inicia a transmissão de pacotes por parte do amplificador. Otamanho de cada pacote será: tamanho de (struct StartContinuousAck)+ (12" blockDatasetCount),onde blockDatasetCount é definido na estrutura “struct StartContinuousCommand”.
Lead Wire Resistance: permite que o sistema determine a resistência do fio condutor e calcule umfactor de correcção para aplicar em todos os dados.
Stop: termina a transmissão de dados e coloca o amplificador em stand-byAuto Zero: obriga o amplificador a executar uma rotina de auto calibração de modo a eliminar
offsets.Reset: quando accionado o amplificador executa uma rotina de auto calibração de modo a garantir a
máxima precisão possível. Deve ser executado sempre que o ganho e/ou a tensão de excitação sejamalterados.
A estrutura detalhada de cada um dos comandos anteriores pode ser vista no anexo A.4.
3.2.3 Integração da Plataforma/Amplificador com PC
A integração do conjunto plataforma/amplificador com um computador é baseada no protocolo decomunicação UDP e a sua representação esquemática pode ser vista na figura 3.11. Apesar de estaintegração afigurar-se uma tarefa relativamente simples, alguns problemas surgiram durante a sua im-plementação.
Como se pretende a aquisição dos dados em tempo real, foi desenvolvida uma função em códigoC, para posteriormente ser implementada como S-function, responsável pela configuração de todosos parâmetros do amplificador e pela iniciação da transmissão de dados, para ser usada em ambi-ente xPC Target. O desenvolvimento desta função teve por base as funções xpcudpbytereceive.c e
31
xpcudpbytesend.c presentes na toolbox xPC Target do Simulink.Verificou-se que a função desenvolvida efectuava correctamente a configuração do amplificador,
mas que uma quantidade considerável de pacotes de dados eram perdidos. Após uma exaustiva pes-quisa conclui-se que o protocolo de comunicação UDP não tinha suporte em tempo real para xPCTarget para a versão de Matlab em uso (R2010b). O suporte em tempo real apenas é garantido paraversões do Matlab posteriores à R2010b, através da toolbox xPC Target mais concretamente atravésda biblioteca Real-Time UDP.
O desenvolvimento de uma nova função em código C foi posta de parte devida à complexidade dasua implementação para execução em tempo real. Devido aos factores enunciados, é necessário quea configuração dos parâmetros do amplificador e a iniciação da transmissão de dados seja efectuadaà priori, sendo que para completar esta tarefa é utilizada a linha de comandos do Matlab para o enviodos pacotes com os devidos comandos.
A recepção dos dados do amplificador em tempo real é assim garantida através do uso dos blocosUDP Receive e Network Configuration da biblioteca Real-Time UDP da toolbox xPC Target do Simulink.No primeiro bloco é apenas necessário configurar o endereço e porta do dispositivo que envia os dadosbem como o tamanho de cada pacote de dados. No segundo bloco é necessário configurar o endereçoIP, bus PCI e slot PCI da placa dedicada para a comunicação UDP em tempo real.
3.3 Aparato Experimental
O xPC Target é um ambiente que permite testar e desenvolver sistema em tempo real usandocomputadores usuais. Neste ambiente é utilizado um computador target, separado de computadorhost, que executa as aplicações em tempo real.
Usualmente quando se pretende simular sistemas é utilizado o software Matlab® e Simulink®. Noambiente xPC Target, a aplicação é desenvolvida em Simulink no computador host e posteriormentedescarregada para o computador target, no qual é executado em tempo real.
De modo a integrar todo o hardware necessário à execução da parte experimental desta disserta-ção foi utilizado um computador target que estabelece a comunicação entre os diferentes dispositivos.Este computador corre o xPC Target 5.1 e tem instalado a placa de aquisição de dados Quanser Q8,para controlo do PUMA 560, duas placa de rede, uma para aquisição em tempo real dos dados doamplificador e outra para estabelecer a comunicação com o computador host. Ambos os computadoresapresentam as seguintes especificações:
• Processador: Intel Core 2 CPU 6600 @ 2.4GHz
• Memória: 4GB
Na figura 3.12 é apresentada a representação esquemática do aparato experimental, onde é possívelobservar os diferentes protocolos de comunicação entre os diferentes dispositivos.
Conforme se verifica a interacção do diferente hardware é feita através do computador target. Esteencontra-se ligado aos diferentes dispositivos segundos os diferentes protocolos de comunicação. Acomunicação entre o target e o amplificador é baseada no protocolo UDP e é estabelecida através datoolbox xPC Target que permite a implementação deste protocolo em tempo real. A comunicação entreo target e o manipulador é assegurada através da placa Quanser Q8 que envia, recebe e monitorizatodos os sinais necessários ao controlo do PUMA 560. Por fim, a comunicação host-target é base-ada no protocolo TCP/IP. A aplicação desenvolvida em Simulink é descarregada para o target, onde é
32
DigiAmp DSA-6 xPC Target 5.1 Host
Motores
Encoders
Plataforma
AMTIBP400600-1000
Q8
Terminal
Board
Analog
ServoAmp.
Controlador PUMA 560
Interface FísicaDefinir ID
Interface Ethernet192.168.0.(150+ID)
Placa Rede #1
Placa Rede #1192.168.0.149
Placa Rede #2192.168.0.150
Quanser Q8
Windows XPMatlab R2011b
192.168.0.148
Analógico
Real-Time UDP
TCP/IP
Analógico/Digital
Figura 3.12: Ligação Host-Target
executada em tempo real, em sentido inverso, os dados referentes ao procedimento experimental sãoenviados para o host, para análise e pós-processamento offline.
33
34
Capítulo 4
Resultados Experimentais
No capítulo 2 é apresentado o suporte teórico para a modelação, identificação e controlo de robôsmanipuladores. Neste capítulo é apresentado o modelo cinemático e dinâmico utilizado para o PUMA560, sendo estes validados através da toolbox SimMechanics do Simulink.
Seguidamente é apresentado o processo de identificação dos parâmetros dinâmicos, sendo queeste processo inclui a simplificação do modelo dinâmico, escolha da trajectória de excitação, recolhados dados experimentais, estimação e validação dos parâmetros dinâmicos.
A identificação dos parâmetros dinâmicos do PUMA 560 permite a implementação de um esquemade controlo por dinâmica inversa. Os aspectos relacionados com a implementação do controlador sãoapresentados na secção 4.3.
4.1 Modelação
Para a derivação dos modelos do PUMA 560 apenas se consideraram as três primeiras juntaspor uma questão de simplificação dos modelos e de modo a evitar possíveis problemas inerentes àssingularidades do punho esférico.
A derivação dos modelos para o robô manipulador PUMA 560 foi feita de forma simbólica comrecurso à toolbox Symbolic Math do Matlab. A modelação de sistemas simbólicamente permite evitaralgumas das piores fontes de erro e ineficiências computacionais causadas pela modelação de sistemasnuméricamente. Para além desta vantagem, permite a modelação de sistemas de forma simples erápida em que a estrutura matemática das equações do modelo é mantida, ao contrário da formulaçãode modelos baseados em estrutura numérica. Isto permite ter noção do significado físico de cadaequação e possibilita verificar se as equações foram correctamente deduzidas. Além de todas estasvantagens a função matlabFunctionBlock do Matlab permite a conversão de expressões simbólicaspara blocos de funções que podem ser usados em modelos de Simulink, o que será bastante útil paraa implementação de um controlador por dinâmica inversa com base nos modelos identificados.
4.1.1 Cinemática
Seguindo o exposta na secção 2.1, o primeira passo para a determinação da cinemática directa doPUMA 560 é a colocação dos referenciais sobre o robô. A escolha de referenciais é ilustrada na figura4.1.
35
xb,0
yb,0zb,0
y1 x1
z1
y2
z2
x2
x3
z3y3
Figura 4.1: Referenciais da configuração base do PUMA 560
Elo i ai [m] !i [rad] di [m] "i [rad] referência [rad]1 0 !$/2 0.6718 "1 02 0.4318 0 0 "2 !$/23 !0.0203 $/2 0.1397 "3 $/2
Tabela 4.1: Parâmetros cinemáticos do PUMA 560
Foi seguida a metodologia de Denavit-Hartenberg para a colocação de referenciais sobre o robômanipulador sendo os respectivos parâmetros cinemáticos apresentados na tabela 4.1.
As equações 4.1 e 4.2 representam a orientação e posição do referencial O3 expresso no referencialOb.
R3 =
#
$%cos ("2 + "3) cos ("1) ! sin ("1) sin ("2 + "3) cos ("1)
cos ("2 + "3) sin ("1) cos ("1) sin ("2 + "3) sin ("1)
! sin ("2 + "3) 0 cos ("2 + "3)
&
'( (4.1)
P3 =
#
$%a2 cos ("1) sin ("2)! d3 sin ("1)! a3 cos ("1) cos ("2) cos ("3) + a3 cos ("1) sin ("2) sin ("3)
d3 cos ("1) + a2 sin ("1) sin ("2)! a3 cos ("2) cos ("3) sin ("1) + a3 sin ("1) sin ("2) sin ("3)
d1 + a3 sin ("2 + "3) + a2 cos ("2)
&
'( (4.2)
4.1.2 Dinâmica
A derivação dos modelos dinâmicos foi feita de forma simbólica com recurso à toolbox Symbo-lic Math do Matlab. As equações que permitem derivar o modelo interno, externo e combinado sãoapresentadas na secção 2.2. Os modelos dinâmicos derivados foram reduzidos de acordo com a me-todologia exposta na subsecção 2.3.1. A simplificação dos modelos é obrigatória devida à necessidade
36
0 1 2 3 4 5−4
−2
0
2
4
0 1 2 3 4 5−4
−2
0
2
4
0 1 2 3 4 5−4
−2
0
2
4
0 1 2 3 4 50
0.5
1
1.5
2
2.5
0 1 2 3 4 50
0.5
1
1.5
2
2.5
0 1 2 3 4 50
0.5
1
1.5
2
2.5
0 1 2 3 4 5−1.5
−1
−0.5
0
0.5
1
1.5
0 1 2 3 4 5−1.5
−1
−0.5
0
0.5
1
1.5
0 1 2 3 4 5−1.5
−1
−0.5
0
0.5
1
1.5
" 1(t)[rad]
" 2(t)[rad]
" 3(t)[rad]
" 1(t)8 rad
s!19
" 2(t)8 rad
s!19
" 3(t)8 rad
s!19
" 1(t)8 rad
s!29
" 2(t)8 rad
s!29
" 3(t)8 rad
s!29
Figura 4.2: Trajectória para validação dos modelos dinâmicos
0 1 2 3 4 5−30
−20
−10
0
10
20
30
0 1 2 3 4 5−80
−60
−40
−20
0
20
40
60
80
0 1 2 3 4 5−15
−10
−5
0
5
10
15
SimMec SymCode
% 1(t)[Nm]
% 2(t)[Nm]
% 3(t)[Nm]
Figura 4.3: Validação do modelo interno
de inverter a matriz de regressão Y para estimação dos parâmetros do modelo. Com esta simplificaçãoobtém-se uma matriz de regressão Y com característica máxima, o que permite a inversão da matrizsem perda de informação. Esta simplificação determina os parâmetros identificáveis, os identificáveisem combinações lineares e os não identificáveis. O vector de parâmetros resultante desta simplificaçãopara os diferentes modelos são apresentados na tabela 4.6.
Para validação dos modelos dinâmicos criados, foi modelado um robô através da toolbox SimMecha-nics do Simulink. Os parâmetros dinâmicos deste modelo foram utilizados para validação dos modelosderivados.
O robô modelado em SimMechanics foi actuado em movimento com a trajectória apresentada nafigura 4.2 e os binários de junta e forças de reacção na base foram adquiridos. Esta trajectória foide seguida utilizada com os modelos dinâmicos deduzidos e os binários de junta e forças de reacçãoprevistas pelos modelos foram comparadas com os dados adquiridos do robô modelado. Os resultadossão ilustrados na figura 4.3, 4.4 e 4.5.
37
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5160
180
200
220
240
260
0 1 2 3 4 5−50
0
50
100
150
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5−30
−20
−10
0
10
20
30
SimMec SymCode
Fx(t)[N]
Fy(t)[N]
Fz(t)[N]
Mx(t)[Nm]
My(t)[Nm]
Mz(t)[Nm]
Figura 4.4: Validação do modelo externo
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5160
180
200
220
240
260
0 1 2 3 4 5−50
0
50
100
150
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5−30
−20
−10
0
10
20
30
0 1 2 3 4 5−30
−20
−10
0
10
20
30
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5−15
−10
−5
0
5
10
15
SimMec SymCode
Fx(t)[N]
Fy(t)[N]
Fz(t)[N]
Mx(t)[Nm]
My(t)[Nm]
Mz(t)[Nm]
% 1(t)[Nm]
% 2(t)[Nm]
% 3(t)[Nm]
Figura 4.5: Validação do modelo combinado
38
Verifica-se que existe uma pequena discrepância no binário da junta 1 e no momento de reacçãosegundo z. As expressões para cálculo destas duas grandezas são iguais e as diferenças verificadaspodem ser resultado de dinâmicas não modeladas, sendo os resultados obtidos suficientes para validaros modelos derivados.
4.2 Identificação
A identificação dos parâmetros dinâmicos através de estimação é um processo com várias etapas.A primeira fase, que passa pela derivação e simplificação do modelo dinâmico e da qual resulta ovector de parâmetros base de cada modelo foi exposta na secção anterior. O restante processo deidentificação é exposto nesta secção.
4.2.1 Optimização da Trajectória
O planeamento de uma trajectória de excitação óptima implica optimização não linear com constran-gimentos a nível do movimento (constrangimentos nas posições, velocidades e acelerações de junta dorobô de modo a evitar colisões).
O tipo de parametrização utilizado para a trajectória determina os graus de liberdade do problema deoptimização. A trajectória de excitação óptima escolhida para cada junta é uma série finita de Fourier.A posição angular "i, velocidade "i e aceleração "i de cada junta i é dada pela equação 4.3.
"i (t) = "0i +Ni*
l=1
ai,l#f l
sin (#f lt)!bi,l#f l
cos (#f lt)
"i (t) =Ni*
l=1
ai,l cos (#f lt) + bi,l sin (#f lt) (4.3)
"i (t) =Ni*
l=1
!ai,l#f l sin (#f lt) + bi,l#f l cos (#f lt)
com #f a representar a frequência fundamental da série de Fourier. Considerando #f = 0.2Hz ,o que implica um periodo de Tf = 1/%f = 5 s e considerando uma série com 5 harmónicas (Ni = 5),resulta um vector de váriaveis de optimização com 33 elementos (n " (2"Ni + 1)) . Os parâmetrosai,l, bi,l e "oi são as variáveis no problema de optimização e agrupam-se no vector de decisão +. Oproblema de optimização é formulado como:
39
+ = argmin# ! log!det
!Y TY
""
sujeito a!2.5089 $ "1 $ 2.5089 [rad]!1.6760 $ "2 $ 1.6760 [rad]!2.1049 $ "3 $ 2.1049 [rad]!1.4312 $ "1 $ 1.4312
8rad s-1
9
!0.9425 $ "2 $ 0.94258rad s-1
9
!2.1293 $ "3 $ 2.12938rad s-1
9
!10 $ "1 $ 108rad s-2
9
!5 $ "2 $ 58rad s-2
9
!15 $ "3 $ 158rad s-2
9
(4.4)
O vector de parâmetros de optimização + é organizado da seguinte forma:
+ =
#
$%-1
-2
-3
&
'( (4.5)
com
-i =
#
$$$$$$$$$$$$$$$$$$%
"0iai,1
ai,2...
ai,5
bi,1
bi,2...
bi,5
&
''''''''''''''''''(
(4.6)
Para a solução do problema de optimização foi utilizada a função de optimização fminsearchcon doMatlab. Esta função permite a solução de problemas não lineares com constrangimentos não lineares.
A solução do problema de optimização convergiu para os valores apresentados na tabela 4.2 aofim de 10000 iterações (sensivelmente 12h de computação). O valor da função de custo associado àsolução obtida é apresentado na tabela 4.3.
4.2.2 Estimação
A solução do problema de optimização permite obter as trajectórias desejadas no espaço de juntasque são posteriormente as entradas de referência no controlador PD implementado. A posição angularreal de cada junta é adquirida através de encoders ópticos. Estas leituras sofrem de erros de quantiza-ção e a estimação das velocidades e acelerações de junta através de diferenciação numérica amplificalargamente estes erros.
Uma das vantagens da utilização de trajectórias de excitação periódicas é a possibilidade do cál-culo das velocidades e acelerações de junta de forma analítica. Para este propósito é calculada a
40
Modelo Interno Modelo Externo Modelo Combinado-1 -2 -3 -1 -2 -3 -1 -2 -3
2.2727 !1.0180 0.0942 2.2192 !1.4582 !1.8453 !2.2011 !1.4509 1.1689!0.0102 0.7992 0.0111 0.0357 0.0719 !0.0291 0.0337 0.0304 !0.0011!0.0022 !7.4604" 10!5 0.0072 0.0266 !0.0072 0.0024 !0.0149 0.0694 0.04520.0184 !0.0095 !0.0263 !0.0110 !0.0045 !4.281" 10!4 0.0356 !4.0014" 10!4 !0.04680.0129 0.0618 0.0737 !0.0121 0.2904 0.0090 1.3908 !0.0055 0.00731.3520 0.0513 !0.0139 !0.0542 0.0208 0.4960 !0.0152 0.0841 9.8684" 10!4
!8.9573" 10!6 0.0051 0.0086 0.0032 0.0941 !0.0246 !0.0030 0.0555 0.02530.0172 !0.0189 !6.9180" 10!4 !0.0359 0.0104 !0.0159 !0.0069 0.3061 !3.8403" 10!4
!0.0270 !0.1329 0.0114 0.0444 0.0556 !0.1952 !0.0099 !0.0015 0.00290.0578 0.0129 !2.9547" 10!4 1.2699 0.0096 !5.5625" 10!5 !0.0121 !0.0228 0.05600.0099 !0.0248 0.4532 !0.2104 !0.5365 0.7641 !0.0292 !0.5914 !0.6136
Tabela 4.2: Parâmetros da trajectória óptima para os modelos dinâmicos
41
Modelo ! log!det
!Y TY
""
Interno !176.0787Externo !211.5930
Combinado !202.1715
Tabela 4.3: Valor da função de custo ! log!det
!Y TY
""para a trajectória optimizada
0 1 2 3 4 51.9
2
2.1
2.2
2.3
2.4
2.5
2.6
0 1 2 3 4 5−2
−1.5
−1
−0.5
0
0 1 2 3 4 50
0.05
0.1
0.15
0.2
0 1 2 3 4 5−3
−2
−1
0
1
2
3
0 1 2 3 4 5−1
−0.5
0
0.5
1
0 1 2 3 4 5−1
−0.5
0
0.5
1
0 1 2 3 4 5−15
−10
−5
0
5
10
15
20
0 1 2 3 4 5−2
−1.5
−1
−0.5
0
0.5
1
1.5
0 1 2 3 4 5−4
−2
0
2
4
6
des real
" 1(t)[rad]
" 2(t)[rad]
" 3(t)[rad]
" 1(t)8 rad
s!19
" 2(t)8 rad
s!19
" 3(t)8 rad
s!19
" 1(t)8 rad
s!29
" 2(t)8 rad
s!29
" 3(t)8 rad
s!29
Figura 4.6: Trajectória optimizada para identificação dos parâmetros do modelo interno
transformada de Fourier discreta das leituras dos encoders , sendo apenas consideradas as principaislinhas espectrais para a reconstrução do sinal. A transformada de Fourier não introduz erros devido àperiodicidade da excitação. Esta abordagem é simples, eficiente e exacta.
O procedimento experimental para aquisição dos dados necessários para a estimação dos parâ-metros dinâmicos é iniciado, onde a entrada do controlador PD é a posição de junta optimizada. Osdados relativos à posição angular de cada junta são obtidos através de encoders ópticos e as velocida-des e acelerações são obtidas seguindo a teoria atrás exposta. A trajectória optimizada e a realmenterealizada pelo manipulador são apresentadas na figura 4.6, 4.7 e 4.8 .
Os binários de junta que permitem a execução da trajectória são estimadas através da acção decontrolo u do controlador PD. A relação entre o binário de junta e a acção de controlo u é dada por
%i = uiKaiKmiGi (4.7)
onde ui representa a acção de controlo da junta i (V), Kai constante eléctrica da junta i (AV!1),Kmi é a constante de binário do motor i (NmA!1) e Gi a relação de transmissão do motor i para juntai. O valor destas constantes é apresentado na tabela 4.4.
Os dados relativos às forças de reacção na base do robô foram adquiridos pela plataforma de força,
42
0 1 2 3 4 5−1.4
−1.2
−1
−0.8
−0.6
−0.4
0 1 2 3 4 5−1.8
−1.7
−1.6
−1.5
−1.4
−1.3
0 1 2 3 4 5−2.1
−2
−1.9
−1.8
−1.7
−1.6
0 1 2 3 4 5−2
−1
0
1
2
0 1 2 3 4 5−1.5
−1
−0.5
0
0.5
1
0 1 2 3 4 5−1.5
−1
−0.5
0
0.5
1
1.5
0 1 2 3 4 5−15
−10
−5
0
5
10
15
0 1 2 3 4 5−8
−6
−4
−2
0
2
4
6
0 1 2 3 4 5−10
−5
0
5
10
des real
" 1(t)[rad]
" 2(t)[rad]
" 3(t)[rad]
" 1(t)8 rad
s!19
" 2(t)8 rad
s!19
" 3(t)8 rad
s!19
" 1(t)8 rad
s!29
" 2(t)8 rad
s!29
" 3(t)8 rad
s!29
Figura 4.7: Trajectória optimizada para identificação dos parâmetros do modelo externo
0 1 2 3 4 5−2.6
−2.4
−2.2
−2
−1.8
0 1 2 3 4 5−1.8
−1.7
−1.6
−1.5
−1.4
−1.3
−1.2
−1.1
0 1 2 3 4 51
1.1
1.2
1.3
1.4
0 1 2 3 4 5−2
−1
0
1
2
0 1 2 3 4 5−1.5
−1
−0.5
0
0.5
1
1.5
0 1 2 3 4 5−1
−0.5
0
0.5
1
0 1 2 3 4 5−15
−10
−5
0
5
10
15
0 1 2 3 4 5−8
−6
−4
−2
0
2
4
6
0 1 2 3 4 5−10
−5
0
5
10
des real
" 1(t)[rad]
" 2(t)[rad]
" 3(t)[rad]
" 1(t)8 rad
s!19
" 2(t)8 rad
s!19
" 3(t)8 rad
s!19
" 1(t)8 rad
s!29
" 2(t)8 rad
s!29
" 3(t)8 rad
s!29
Figura 4.8: Trajectória optimizada para identificação dos parâmetros do modelo combinado
43
i Kai Kmi Gi
1 0.8251 0.223 62.612 0.8251 0.226 107.363 0.8251 0.240 53.69
Tabela 4.4: Constantes eléctricas e de binário de motor e relações de transmissão (de: [1])
Modelo ! log!det
!Y TY
""
Interno !184.4263Externo !220.9290
Combinado !257.0924
Tabela 4.5: Valor da função de custo ! log!det
!Y TY
""para a trajectória real
observando-se que as leituras estavam corrompidas com ruído mecânico induzido pelo movimento dorobô. Consequência os dados adquiridos pela plataforma foram filtrados offline através da função filtfiltdo Matlab, com um filtro passa-baixo de segunda ordem com uma frequência de corte de 25Hz.
Os dados apresentados na figura 4.9, 4.10 e 4.11, correspondem aos binários de junta e forças dereacção na base do robô para o movimento do robô ilustrado na figura 4.6, 4.7 e 4.8 respectivamente.O valor da função de custo para a trajectória real é apresentado na tabela 4.5
Após aquisição e pós-processamento offline dos dados necessários, os parâmetros dinâmicos decada modelo podem ser identificados através de um algoritmo de estimação. O método dos mínimosquadrados é geralmente utilizado devido à sua simplicidade e resume-se a:
$ =!Y TY
"!1Y T %g (4.8)
Os dados relativos ao movimento do manipulador são utilizados para a construção da matriz deregressão Y aumentada e os dados relativos aos binários de junta e forças de reacção utilizados paraconstrução do vector de forças generalizado %g aumentado. Os parâmetros dinâmicos estimados atra-vés da solução da equação 4.8 são apresentados na tabela 4.6.
A validação dos parâmetros estimados é feita com base na comparação entre os binários de juntae as forças de reacção adquiridos durante um movimento especifico do manipulador e os binários dejunta e forças de reacção previstos pelos modelos dinâmicos para esse mesmo movimento.
A trajectória utilizada para validação dos parâmetros estimados para o modelo interno é ilustradana figura 4.7. Os binários de junta que permitem este movimento são adquiridos e comparados comos estimados pelo modelo Yi$i. Os resultados são ilustrados na figura 4.12 e o erro quadrático médioassociado a cada grandeza é apresentado na tabela 4.7.
0 1 2 3 4 5−60
−40
−20
0
20
40
60
80
0 1 2 3 4 5−20
−10
0
10
20
30
40
50
60
0 1 2 3 4 5−15
−10
−5
0
5
10
15
20
% 1(t)[Nm]
% 2(t)[Nm]
% 3(t)[Nm]
Figura 4.9: Binários de junta para a trajectória real do modelo interno
44
0 1 2 3 4 5−100
−50
0
50
100
150
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5650
700
750
0 1 2 3 4 5−20
0
20
40
60
80
100
0 1 2 3 4 5−150
−100
−50
0
50
0 1 2 3 4 5−60
−40
−20
0
20
40
60
Fx(t)[N]
Fy(t)[N]
Fz(t)[N]
Mx(t)[Nm]
My(t)[Nm]
Mz(t)[Nm]
Figura 4.10: Forças exercidas na base para a trajectória real do modelo externo
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5−150
−100
−50
0
50
100
150
0 1 2 3 4 5640
660
680
700
720
740
760
0 1 2 3 4 5−150
−100
−50
0
50
100
0 1 2 3 4 5−140
−120
−100
−80
−60
−40
−20
0 1 2 3 4 5−60
−40
−20
0
20
40
60
0 1 2 3 4 5−40
−20
0
20
40
60
0 1 2 3 4 5−20
0
20
40
60
80
0 1 2 3 4 5−15
−10
−5
0
5
10
15
20
Fx(t)[N]
Fy(t)[N]
Fz(t)[N]
Mx(t)[Nm]
My(t)[Nm]
Mz(t)[Nm]
% 1(t)[Nm]
% 2(t)[Nm]
% 3(t)[Nm]
Figura 4.11: Forças exercidas na base e binários de junta para a trajectória real do modelo combinado
45
# Modelo Interno ($i) Modelo Externo ($e) Modelo Combinado ($c)Significado físico Valor Significado físico Valor Significado físico Valor
1 Ixz3 0.1567 m3cx3! a3m3 !0.1364 m3cz3 !0.4084
2 Iyz3 !0.0332 m3cz3 0.5043 a3 (m1 +m2) +m3cx30.9898
3 Ixy3+ a3m3cy3
!0.1483 Iyz3 0.1315 Ixz3 0.41504 m3cz3 0.8120 Ixy3
+ a3m3cy30.2406 Iyz3 !0.1669
5 m3cx3! a3m3 !0.0170 Ixz3 0.0188 Ixy3
+ a3m3cy30.5775
6 Ixy2!0.5626 m2cx2
! a2m1 !27.9198 m2cx2! a2m1 !28.0179
7 Izz3 ! Ixx3! a23m3 0.2339 m2cy2
!0.0532 m2cy22.4531
8 Iyz2 0.0672 Izz3 ! Ixx3! a23m3 0.7745 Izz3 ! Ixx3
+ a23 (m1 +m2) !1.25499 Iyy3
! a23m3 0.5939 Ixy2!0.1293 Iyy3
+ a23 (m1 +m2) 1.0551
10 Ixx3+ Iyy1
+ Iyy2! a22m2 !!
a22 ! d23"m3 + 2d3m3cy3
2.1246 Iyy3! a23m3 0.1728 Ixy2
1.1906
11 m2cy2!0.5532 m1cx1
1.0338 m1cx10.1204
12 Ixz2 !!a23 + d23 + 2a3
"m3 !
a2 (m3cy3+m2cz2)
!0.3193 Iyz2 !0.0573m1cz1 +m2cz2 +m3cy3
!d3 (m1 +m2)
!1.5029
13 a2(m2 +m3) +m2cx22.1131 d3m3 +m1cz1 +m2cz2 +m3cy3
6.7060 Iyz2 !0.469514 fv3 10.1970 Ixz2 + a2m1cz1 2.2312 !Ixx2
+ Iyy2+ a22m1 14.1836
15 fv1 9.3467 m1 +m2 +m3 70.9658 Ixz2 + a2m1cz1 4.266516 fv2 20.5084 Ixx2
! Iyy2! a22m1 !12.3473 Ixy1
!0.8808
17 Ixx2+ Ixx3
+ Iyy1+ d23m3 +
2d3m3cy3
1.9853 Ixy1!1.9417 m1 +m2 +m3 70.9001
18 Izz2 ! a22 (m2 +m3) 3.5732 Iyz1 0.2769 Iyz1 !1.4964
19 ! !Iyy1
+ Iyy2+ Ixx3
+ a22m1 +d23m3 + 2d3m3cy3
14.1570Iyy1
+ Ixx2+ Ixx3
!d23 (m1 +m2) + 2d3m3cy3
!0.0007
20 ! ! Izz2 + a22m1 13.2635 Izz2 + a22m1 13.151721 ! ! ! ! fv1 9.502322 ! ! ! ! fv2 20.243423 ! ! ! ! fv3 9.7488
Tabela 4.6: Parâmetros dinâmicos estimados
46
MSE %18N2m2
9%2
8N2m2
9%3
8N2m2
9
Valor 22.6737 53.1213 11.2793
Tabela 4.7: Erro quadrático médio para os binários de junta
0 1 2 3 4 5−60
−40
−20
0
20
40
0 1 2 3 4 5−40
−20
0
20
40
60
80
0 1 2 3 4 5−20
−15
−10
−5
0
5
10
15
real
% 1(t)[Nm]
% 2(t)[Nm]
% 3(t)[Nm]
Yi$i
Figura 4.12: Validação dos parâmetros do modelo interno
MSE Fx
8N2
9Fy
8N2
9Fz
8N2
9
Valor 106.4345 244.3708 110.4701
MSE Mx
8N2m2
9My
8N2m2
9Mz
8N2m2
9
Valor 207.4052 45.0013 44.8581
Tabela 4.8: Erro quadrático médio para as forças de reacção
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5−150
−100
−50
0
50
100
0 1 2 3 4 5−150
−100
−50
0
50
100
150
0 1 2 3 4 5−140
−120
−100
−80
−60
−40
−20
0
0 1 2 3 4 5640
660
680
700
720
740
760
0 1 2 3 4 5−60
−40
−20
0
20
40
60
real
Fx(t)[N]
Fy(t)[N]
Fz(t)[N]
Mx(t)[Nm]
My(t)[Nm]
Mz(t)[Nm]
Ye$e
Figura 4.13: Validação dos parâmetros do modelo externo
MSE Fx
8N2
9Fy
8N2
9Fz
8N2
9
Valor 323.9356 548.3285 167.1399
MSE Mx
8N2m2
9My
8N2m2
9Mz
8N2m2
9
Valor 719.1653 1355.7726 79.4018
MSE %18N2m2
9%2
8N2m2
9%3
8N2m2
9
Valor 52.2090 314.0778 104.0090
Tabela 4.9: Erro quadrático médio para as forças de reacção e binários de junta
47
0 1 2 3 4 5−150
−100
−50
0
50
100
150
0 1 2 3 4 5−150
−100
−50
0
50
0 1 2 3 4 5−60
−40
−20
0
20
40
60
80
0 1 2 3 4 5−150
−100
−50
0
50
100
0 1 2 3 4 5−100
−50
0
50
100
150
0 1 2 3 4 5−20
0
20
40
60
0 1 2 3 4 5620
640
660
680
700
720
740
0 1 2 3 4 5−100
−50
0
50
100
0 1 2 3 4 5−15
−10
−5
0
5
10
15
20
real
Fx(t)[N]
Fy(t)[N]
Fz(t)[N]
Mx(t)[Nm]
My(t)[Nm]
Mz(t)[Nm]
% 1(t)[Nm]
% 2(t)[Nm]
% 3(t)[Nm]
Yc$c
Figura 4.14: Validação dos parâmetros do modelo combinado
Seguindo o mesmo procedimento, a trajectória utilizada para validação dos parâmetros estimadospara o modelo externo é ilustrada na figura 4.8. As forças de reacção na base do manipulador pro-vocadas pelo seu movimento são adquiridas através da plataforma de força e comparadas com asestimadas pelo modelo Ye$e. Os resultados encontram-se ilustrados na figura 4.13 e o erro quadráticomédio associado a cada uma das seis grandezas apresentado na tabela 4.8.
Por fim, a trajectória ilustrada na figura 4.6 é utilizada para validação dos parâmetros estimadospara o modelo combinado. Os binários de junta que permitem a execução do movimento e as forças dereacção na base do manipulador são adquiridas e comparadas com as estimadas pelo modelo Yc$c.Os resultados encontram-se ilustrados na figura 4.14 e o erro quadrático médio associado apresentadona tabela 4.9.
É notório que os resultados obtidos para os parâmetros do modelo dinâmico combinado não sãosatisfatórios. Uma análise mais profunda sobre os resultados obtidos é efectuada no capítulo 5.
4.3 Controlo
A implementação de um esquema de controlo por dinâmica inversa só é possível com o conheci-mento dos parâmetros dinâmicos do robô. A identificação dos parâmetros dinâmicos do modelo internopermite a implementação de um controlador baseado neste modelo. A implementação deste esquemade controlo é ilustrada na figura 4.15. Considerando uma frequência natural #n de 20 rad s e um factorde amortecimento de 0.7 os valores de KP e KD tomam o valor de :
KP = diag {400, 400, 400} KD = diag {28, 28, 28}
48
"d +KD
0"d ! "r
1+KP ("d ! "r) Yi
0"r, "r, ,
1$i PUMA 560
ddt
, % "r
"r
"d
"d
"d
Figura 4.15: Controlador por dinâmica inversa
Para validação deste novo esquema de controlo é utilizada uma nova trajectória de referência eé analisado o erro entre esta e a trajectória efectivamente realizada pelo robô. Os resultados sãoapresentados na figura 4.16 e tabela 4.10.
0 1 2 3 4 5−2.6
−2.5
−2.4
−2.3
−2.2
−2.1
−2
−1.9
Fx(t)[N]
0 1 2 3 4 5−1.5
−1
−0.5
0
0.5
1
1.5
Mx(t)[N
m]
0 1 2 3 4 5−10
−5
0
5
10
!1(t)[N
m]
0 1 2 3 4 5−1.8
−1.6
−1.4
−1.2
−1
−0.8
Fy(t)[N]
0 1 2 3 4 5−1
−0.5
0
0.5
1
1.5
My(t)[N
m]
0 1 2 3 4 5−3
−2
−1
0
1
2
3
4
!2(t)[N
m]
0 1 2 3 4 5−1
−0.5
0
0.5
1
1.5
0 1 2 3 4 5−3
−2
−1
0
1
2
0 1 2 3 4 5−15
−10
−5
0
5
10
15
!3(t)[N
m]
des real
Figura 4.16: Validação do controlador por dinâmica inversa
No capítulo 5 estes resultados são analisados e discutidos em detalhe.
49
Erro Médio "1 [rad] "2 [rad] "3 [rad]Valor 0.0021 0.0075 0.0292
Tabela 4.10: Erro médio da posição angular
0 1 2 3 4 50
0.2
0.4
0.6
0.8
1
!1(t)[N
m]
0 1 2 3 4 50
0.2
0.4
0.6
0.8
1
!2(t)[N
m]
0 1 2 3 4 50
0.2
0.4
0.6
0.8
1
!3(t)[N
m]
|e|no rm | "|no rm
Figura 4.17: Erro de seguimento e velocidade normalizada
50
Capítulo 5
Conclusões e Trabalho Futuro
Neste capítulo são analisados os resultados relativos à identificação dos parâmetros dinâmicos doPUMA 560, bem como a performance do controlador por dinâmica inversa implementado. São retiradasconclusões acerca dos resultados obtidos e é proposto trabalho futuro com vista à melhoria do trabalhodesenvolvido.
5.1 Conclusões
O PUMA 560 encontrava-se integrado com um computador externo através de uma placa de aqui-sição com bus ISA. Dado que este tipo de bus foi substituído pelo PCI, tornou-se necessário o de-senvolvimento de um novo sistema de integração baseado em placas de aquisição PCI. O sistemade integração desenvolvido é baseado na placa de aquisição Quanser Q8 e permite obter uma totalconectividade entre o manipulador, sensores externos, sistemas de imagem, etc.
A integração da plataforma de força AMTI BP400600-1000 com um computador é baseada no pro-tocolo de comunicação UDP e permitiu o desenvolvimento de uma técnica de identificação com vistaà estimação dos parâmetros dinâmicos do PUMA 560. A trabalho desenvolvido na integração destesensor externo com um computador externo permitirá o uso deste em outras aplicações.
Os parâmetros dinâmicos para o modelo interno e externo apresentam resultados satisfatórios. Atra-vés da validação dos parâmetros dinâmicos é possível verificar que o erro quadrático médio é superiornas grandezas referentes ao modelo externo, devendo-se isto ao facto de as leituras das forças de re-acção estarem corrompidas por ruído mecânico induzido pelo movimento do robô, enquanto os dadosrelativos aos binários de junta se encontram livres de ruído pois são estimados através da acção decontrolo.
A utilização de um modelo combinado para a identificação dos parâmetros dinâmicos supõe que aestimação dos parâmetros dinâmicos seja mais precisa, visto que a quantidade de dados disponíveispara esta estimação dos parâmetros é maior. No entanto verifica-se que os parâmetros dinâmicosestimados para o modelo combinado não apresentam os resultados esperados. As causas para estasdiferenças podem dever-se à presença de força induzidas pelo aperto do robô à base da plataforma epelo cabo de ligação entre o manipulador e a unidade de potência. O aperto do manipulador à baseinduz momentos segunda a direcção z e o cabo de ligação induz momentos segundo a direcção x.Isto significa que os dados adquiridos pela plataforma encontram-se adulterados não só pelo ruídomecânico mas também pelos factores atrás referidos.
51
O conhecimento dos parâmetros dinâmicos do modelo interno permite a implementação de umcontrolador baseado neste modelo. Verifica-se que a performance do controlador implementado pordinâmica inversa é aceitável visto que o erro médio de seguimento é relativamente pequeno. Os mai-ores erros verificam-se a velocidades elevadas o que significa que o modelo de atrito considerado éinsuficiente.
5.2 Trabalho Futuro
Com vista à melhoria do trabalho desenvolvido, alguns aspectos podem ser considerados, entre osquais se destacam:
• Desenvolvimento de software para posicionamento automático do manipulador numa posição dereferência. A integração do PUMA 560 com um computador através da placa de aquisição Quan-ser Q8 permite a utilização dos dados relativos aos potenciometros de junta para desenvolver umsistema que permita o posicionamento automático do robô numa posição de referência.
• Verifica-se que os parâmetros dinâmicos estimados para o modelo combinado não são satisfató-rios, sendo que as possíveis causas estão identificadas. O desenvolvimento de um procedimentoque permita eliminar as causas de erro, possibilitará a identificação dos parâmetros dinâmicosdo modelo combinado e consequentemente permitirá a implementação de um controlador pordinâmica inversa baseado neste modelo.
• O controlador por dinâmica inversa implementado com base no modelo interno apresenta os mai-ores erros a velocidades elevadas. Isto significa que o modelo de atrito considerado é insuficiente.Com vista a melhorar o desempenho do controlador é necessário considerar um modelo de atritomais complexo e proceder-se a uma nova identificação dos parâmetros dinâmicos do robô mani-pulador.
52
Bibliografia
[1] P. I. Corke, “In situ Measurement of Robot Motor Electrical Constants,” 1996.
[2] G. Antonelli, F. Caccavale, and P. Chiacchio, “A systematic procedure for the identification of dyna-mic parameters of robot manipulators,” Robotica, vol. 17, pp. 427–435, 1999.
[3] C. H. An, C. G. Atkeson, and J. M. Hollerbach, “Estimation of inertial parameters of rigid body linksof manipulators,” in Decision and Control, 1985 24th IEEE Conference on, pp. 990–995, 1985.
[4] W. Khalil and É. Dombre, Modeling, identification & control of robots. Butterworth-Heinemann, July2004.
[5] A. Mukerjee and D. Ballard, “Self-calibration strategies for robot manipulators,” in Robotics andAutomation, pp. 1050–1057, Mar. 1985.
[6] J. Swevers, C. Ganseman, J. DeSchutter, and H. VanBrussel, “Experimental robot identificationusing optimised periodic trajectories,” Mechanical Systems and Signal Processing, vol. 10, no. 5,pp. 561–577, 1996.
[7] S. Tafazoli, P. Lawrence, and S. Salcudean, “Identification of inertial and friction parameters forexcavator arms,” Ieee Transactions on Robotics and Automation, vol. 15, no. 5, pp. 966–971, 1999.
[8] H. West, E. Papadopoulos, S. Dubowsky, and H. Cheah, “A method for estimating the mass proper-ties of a manipulator by measuring the reaction moments at its base,” in Robotics and Automation,1989. Proceedings., 1989 IEEE International Conference on, pp. 1510–1516, 1989.
[9] B. Raucent, G. Campion, G. Bastin, J. Samin, and P. Willems, “Identification of the Barycentric Para-meters of Robot Manipulators From External Measurements,” Automatica, vol. 28, no. 5, pp. 1011–1016, 1992.
[10] G. Liu, K. Iagnemma, S. Dubowsky, and G. Morel, “A base force/torque sensor approach to robotmanipulator inertial parameter estimation,” in Robotics and Automation, 1998. Proceedings. 1998IEEE International Conference on, pp. 3316–3321, 1998.
[11] M. Grotjahn and B. Heiman, “Determination of dynamic parameters of robots by base sensor mea-surements,” in 6th IFAC Symposium on Robot Control, Syroco, 2000, pp. 277–282.
[12] X. Chenut, J. C. Samin, J. Swevers, and C. Ganseman, “Combining internal and external robotmodels for improved model parameter estimation,” Mechanical Systems and Signal Processing,vol. 14, pp. 691–704, Sept. 2000.
[13] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: Modelling, Planning and Control (Ad-vanced Textbooks in Control and Signal Processing). Springer, 2nd printing. ed., Feb. 2011.
53
[14] J. J. Craig, Introduction to Robotics: Mechanics and Control (3rd Edition). TBS, 2004.
[15] C. G. Atkeson, C. H. An, and J. M. Hollerbach, “Rigid body load identification for manipulators,” inDecision and Control, 1985 24th IEEE Conference on, pp. 996–1002, 1985.
[16] H. Mayeda, K. Yoshida, and K. Osuka, “Base parameters of manipulator dynamic models,” Roboticsand Automation, IEEE Transactions on, vol. 6, no. 3, pp. 312–321, 1990.
[17] S.-Y. Sheu and M. Walker, “Basis sets for manipulator inertial parameters,” in Robotics and Auto-mation, 1989. Proceedings., 1989 IEEE International Conference on, pp. 1517–1522, 1989.
[18] M. Gautier, “Numerical calculation of the base inertial parameters of robots,” in Robotics and Auto-mation, 1990. Proceedings., 1990 IEEE International Conference on, pp. 1020–1025, 1990.
[19] F. Pfeiffer and J. Holzl, “Parameter identification for industrial robots,” IEEE International Conferenceos Robotics and Automation, pp. 1468–1476, May 1995.
[20] B. Armstrong, “On finding ’exciting’ trajectories for identification experiments involving systems withnon-linear dynamics,” in Robotics and Automation. Proceedings. 1987 IEEE International Confe-rence on, pp. 1131–1139, 1987.
[21] J. Swevers, C. Ganseman, D. Tukel, J. DeSchutter, and H. VanBrussel, “Optimal robot excitationand identification,” Ieee Transactions on Robotics and Automation, vol. 13, no. 5, pp. 730–740,1997.
[22] M. Gautier and P. Poignet, “Extended Kalman filtering and weighted least squares dynamic iden-tification of robot,” in Control Engineering Practice, pp. 1361–1372, CNRS, UMR 6597, Inst RechCommun & Cybernet Nantes, F-44321 Nantes 03, France, 2001.
[23] C. A. C. de Wit, B. Siciliano, and G. Bastin, Theory of robot control. Springer Verlag, 1996.
[24] J. Fraden, Handbook of Modern Sensors: Physics, Designs, and Applications. Springer, 4th ed. ed.,Sept. 2010.
[25] Z. Qin and L. Baron, “A new approach to the dynamic parameter identification of robotic manipula-tors,” Robotica, vol. 28, no. July 2010, pp. 539–547.
[26] J. Schaefers and S. J. Xu, “A Parameter Identification Approach Using Optimal Exciting Trajectoriesfor a Class of Industrial Robots,” Journal of Intelligent and Robotic Systems, vol. 15, pp. 25–32.
[27] P. Corke and B. Armstrong-Helouvry, “A search for consensus among model parameters reportedfor the PUMA 560 robot,” in Robotics and Automation, 1994. Proceedings., 1994 IEEE InternationalConference on, pp. 1608–1613, 1994.
[28] “Additional remarks related to the paper: Optimal robot excitation and identification,” Mar. 2011.
[29] J. Feio, Controlo da Impedância de Rôbos Manipuladores para Aplicações em Cirurgia Ortopédica.PhD thesis, Instituto Superior Técnico, Sept. 2004.
[30] M. Gautier and W. Khalil, “Direct calculation of minimum set of inertial parameters of serial robots,”Robotics and Automation, IEEE Transactions on, vol. 6, no. 3, pp. 368–373, 1990.
54
[31] M. Gautier and W. Khalil, “Exciting trajectories for the identification of base inertial parameters ofrobots,” in Decision and Control, 1991., Proceedings of the 30th IEEE Conference on, pp. 494–499,1991.
[32] P. Dutkiewicz, K. Kozlowski, and W. Wroblewski, “Experimental identification of robot and loaddynamic parameters,” in Control Applications, 1993., Second IEEE Conference on, pp. 767–776,1993.
[33] W. Verdonck, J. Swevers, and J.-C. Samin, “Experimental Robot Identification: Advantages of Com-bining Internal and External Measurements and of Using Periodic Excitation,” Journal of DynamicSystems, Measurement, and Control, vol. 123, no. 4, p. 630, 2001.
[34] P. A. P. L. T. N. A. R. P. L. C. Nuno Moreira, “First steps towards an open control architecture for aPUMA 560,” 1998.
[35] M. Farooq and D.-b. Wang, “Implementation of a new PC based controller for a PUMA robot,”Journal of Zhejiang University SCIENCE A, vol. 8, pp. 1962–1970, Nov. 2007.
[36] Modelling Identification and Control of Flexible Manipulators. PhD thesis, Instituto Superior Técnico,Apr. 2007.
[37] B. Raucent, G. Campion, G. Bastin, J. C. Samin, and P. Willems, “On the identification of thebarycentric parameters of robot manipulators from external measurements,” in Robotics and Auto-mation, 1991. Proceedings., 1991 IEEE International Conference on, pp. 1169–1174, 1991.
[38] M. Gautier, “Optimal motion planning for robot’s inertial parameters identification,” in 31st Confe-rence on Decision and Control, pp. 70–73, Ecole Centrale de Nantes/Université de Nantes, Dec.1992.
[39] P. K. Khosla and T. Kanade, “Parameter identification of robot dynamics,” in Decision and Control,1985 24th IEEE Conference on, pp. 1754–1760, 1985.
[40] B. S. O. Khatib, Springer Handbook of Robotics (text only) by B.Siciliano.O. Khatib. with DVD-ROM,and 84 tables, Springer, 2008.
[41] B. Armstrong, O. Khatib, and J. Burdick, “The explicit dynamic model and inertial parameters of thePUMA 560 arm,” in Robotics and Automation. Proceedings. 1986 IEEE International Conferenceon, pp. 510–518, 1986.
[42] P. I. Corke, “The unimation puma servo system,” 1995.
55
56
Apêndice A
Plataforma e amplificador
A.1 Relatório de calibração da plataformaMulti-axis Force Transducer Calibration Data
Model: BP400600-1000 Serial Number: 5287
Calibration Filed under 5287.1
Calibrated on: 03-28-2007 11:29:51
GENERAL USE (Sufficient for most applications)
Location of the center of the top plate relative to the effective XYZ center
of the dynamometer [mm]
xo = 0.158 yo = 0.894 zo = -39.660
The ’Sensitivities’ (output/input) for each channel are:
Forces MomentsµV/Vex/N µV/Vex/Nm
Fx .6745 Mx 1.4541Fy .6747 My 1.8441Fz .1718 Mz 3.1550
Table A.1: Sensibilidade por canal para uso geral
ADVANCED USE (Sensitivity Matrix Analysis) 5287
SENSITIVITY MATRIX S(i,j)
Output of channel i (µV/Vex) is S(i,j) times the mechanical input j (N,Nm)
57
j Fx Fy Fz Mx My Mz
VFx .6745 -.0033 .0001 .0090 -.0011 -.0138VFy .0007 .6747 -.0013 -.0008 .0076 .0046VFz -.0004 -.0013 .1718 .0005 -.0009 .0000VMx -.0004 -.0012 .0013 1.4541 .0143 .0022VMy -.0015 -.0002 -.0003 -.0097 1.8441 .0125VMz .0001 -.0043 .0017 -.0042 -.0022 3.1550
Table A.2: Matriz de sensibilidade
INVERTED SENSITIVITY MATRIX B(i,j)
Input to channel i (N,Nm) is B(i,j) times the electrical output j µV/Vex
j VFx VFy VFz VMx VMy VMz
Fx 1.4825 .0072 -.0007 -.0092 .0009 .0065Fy -.0016 1.4821 .0108 .0008 -.0061 -.0021Fz .0034 .0110 5.8218 -.0022 .0027 .0000Mx .0004 .0012 -.0052 .6877 -.0054 -.0005My .0012 .0002 .0009 .0036 .5422 -.0021Mz -.0001 .0020 -.0031 .0009 .0004 .3170
Table A.3: Inversa da matriz de sensibilidade
A.2 Relatório calibração do amplificadorAdvanced Mechanical Technology
DigiAmp Calibration Certification
Date: 03/28/07
Serial: 1069
Model: DSA-6
AMTI certifies that this amplifier has been tested and internally adjusted so
that all gain excitation voltage products have an error less than the
specified tolerance reported below. AMTI futher certifies that the below
calibration has been performed using tools, equipment and instruments that
are maintained and have periodic calibrations that are traceble to the
National Institute of Standards and Technology.
Digital Outputs
Gain Measured Deviation Specified Tolerance1000 ± 0.013 ± 0.1 %2000 ± 0.033 ± 0.1 %4000 ± 0.053 ± 0.1 %8000 ± 0.107 ± 0.2 %
Table A.4: Saídas digitais
58
Analog Outputs
Gain Measured Deviation Specified Tolerance1000 ± 0.022 ± 0.5 %2000 ± 0.042 ± 0.5 %4000 ± 0.063 ± 0.5 %8000 ± 0.117 ± 0.5 %
Table A.5: Saídas analógicas
A.3 Especificações do amplificador
A.4 Estrutura dos comandosCada comando é transmitido como um pacote UDP. Os comandos são documentados
como estruturas em C/C++.
struct SynchronizeCommand {
CID cid; // Set cid = COMMAND_SYNCHRONIZE;
};
struct SetStateCommand {
CID cid; // Set cid = COMMAND_SET_STATE;
struct amplifiersettings ampSettings;
short blockDatasetCount;
long hostAddress;
};
struct amplifiersettings {
short gain[6]; // 0, 1, 2 or 3 are valid gains
short filterFreq[6]; // filterFreq[0] sets all channel filters
short exVolt[6]; // 0 to 160, sets excitation in \nicefrac{1}{16}^{th}
volt intervals
short decimationNumerator; // must be set to 1
short decimationDenominator; // Integer value 1 to 65000 sets the
decimation rate
};
struct IDRequest {
CID cid; // Set cid = COMMAND_ID_REQUEST
};
struct IDRequestAck {
CID cid; // Reflects back the COMMAND_SET_STATE value
short ID; // Amplifier ID short DSPStatus; // 0 = Ready, 1 = Busy auto
zero, 2 = Busy self cal
59
Item Especificação Comentário/CondiçãoInterface com UtilizadorBalanço da Ponte Software ou manual Interface teclado/LCDAjustamento do Ganho Software ou manual Interface teclado/LCDAjustamento da Excitação Software ou manual Interface teclado/LCDAjustamento do Filtro Software ou manual Interface teclado/LCDCalibração Interna Software ou manual Interface teclado/LCD
Canais de EntradaCanais 6 13 bitTipo de Entrada Strain gage Ponte de 120 a 1400 ohmProtecção da Entrada Protecção ESD 4000 volt
Saídas AnalógicasCanais 6Intervalo de Tensão ±10 VoltsCarga da Impedância >10 K ohm
Saída Digital Ethernet 16 bitConversor A/DResolução 16 bit Linearidade 1 lsb
Taxa Máxima de Dados 16000 Conjuntos de dados de 6canais por segundo
Ganhos Analógicos 1000, 2000, 4000, 8000 Linearidade 0.02%Voltagem de Excitação 0 - 10 Volts Intervalos de 1/16 VoltFiltros de Anti Aliasing 1600 HzFiltros Digitais
Frequência 10, 20, 30, 50, 100, 200,300, 500, 1000 Hz
Tipo Butterwoth ou BesselSincronizaçãoModo Triggered, genlock, free runTipo de Entrada Jack RCA opto-isolado Compativel com TTL
Fonte de Alimentação Secretária 100-240 VAC; 50-60 HzDimensões Físicas 44 x 432 x 318 mm Montagem em prateleiraPeso 5.9 kgInterface EthernetTipo 10 Mbs EthernetProtocolo Protocolo UDPCabo RG - 58 Ficha BNCComprimento Máximo 152.5 m
Amostragem Superior a 96000conjuntos/segundo Pode ser limitado pelo PC
Tabela A.6: Especificações DigiAmp DSA-6
60
short Err; // Returns an error code
char rev[8+1]; // Returns the amplifier revision number
char sn[8+1]; // Returns the amplifier serial number
char date[8+1]; // Returns the amplifier calibration date
};
struct StartContinuousCommand {
CID cid; // Set cid = COMMAND_START_CONTINUOUS
short blockDatasetCount;
};
struct StartContinuousAck {
CID cid;
short status;
struct blockHeader blockHead; // Contains synchronization and timing
information
};
struct blockHeader {
short amplifierID; // 1 to 6
short blockStartHigh; // High word of the block start time
long blockStartTime; // All times are relative to SynchronizeCommand,
in 1/16000 sec ticks
long syncAssertTime; // Indicates trigger transition time relative to
beginning of packet
long syncFlag; // Low word = initial state, 0x40 = high, 0x00 = low.
High word = event flag; 0xFFFF - signaled, 0 not signaled
long spare; // NA
long spare; // NA
};
struct StopCommand {
CID cid; // Set cid = STOP_COMMAND
short status;
};
struct AutoZeroCommand {
CID cid; // Set cid = COMMAND_AUTOZERO
short spare;
};
struct RWireCommand {
CID cid; // Set cid = COMMAND_WIRE
short spare;
};
61
struct ResetCommand {
CID cid; // Set cid = COMMAND_RESET
};
struct SetCalCommand {
CID cid; // Set cid = COMMAND_SET_CAL
struct ampCalibration factors;
};
struct ampCalibration {
struct correction cfactor[6]; // Six channels of calibration constants
short calFlag; // Flag prevents calibration over write (0x55)
short rwireGain; // Lead wire gain constant
short rwireCalConst; // Lead wire calibration constant
char sn[8+1]; // Serial number string
char date[8+1]; // Calibration date string
};
struct correction {
short frontend[4]; // A value for each of the 4 gain settings, 1000,
2000, 4000, 8000
short analog; // Value used to tweak the analog output stage of the
amplifier
};
62
Apêndice B
Código desenvolvido
B.1 Programação do watchdog da Quanser Q8
É apresentado de seguida o código desenvolvido para programação do watchdog da placa de aquisi-ção Quanser Q8. O código foi desenvolvido em código C para ser posteriormente usado com s-functionno diagrama de Simulink. A função deste código é comutar todas as entradas e saídas digitais casonão sejam efectuadas escritas durante dois instantes de tempo consecutivos. Esta comutação provocao corte de energia ao robô manipulador evitando assim qualquer acidente.
#define S_FUNCTION_LEVEL 2#undef S_FUNCTION_NAME#define S_FUNCTION_NAME watchdog
#include " s ims t ruc . h "
# i f de f MATLAB_MEX_FILE#include "mex . h "#endif
# i fndef MATLAB_MEX_FILE#include <windows . h>#include " xpc ta rge t . h "#endif
#define NUM_ARGS 5#define BOARDTYPE_ARG ssGetSFcnParam(S, 0 )#define SAMP_TIME_ARG ssGetSFcnParam(S, 1 ) / / double#define PCI_BUS_ARG ssGetSFcnParam(S, 2 ) / / i n t#define PCI_SLOT_ARG ssGetSFcnParam(S, 3 ) / / i n t#define PERIOD_ARG ssGetSFcnParam(S, 4 ) / / double
#define NUM_I_WORKS (3 )#define BASE_I_IND (0 )
63
#define COUNTER_I_IND (1 )#define LOAD_I_IND (2 )#define NUM_R_WORKS (0 )
#define VENDOR_ID ( uint16_T ) ( 0 x11E3 )#define DEVICE_ID ( uint16_T ) ( 0 x0010 )#define SUBVENDOR_ID ( uint16_T ) ( 0 x5155 )#define PCI_BYTES (0x0400 )
#define MAX_CHAN (2 )
#define THRESHOLD ( 0 . 5 )
#define BIT ( n ) (1 << n )
#define CLAMP_LO( x , l o ) ( x < l o ? l o : x )#define CLAMP_HI ( x , h i ) ( x > h i ? h i : x )#define CLAMP_UINT32 ( x ) (CLAMP_LO(CLAMP_HI ( x , 0 x f f f f f f f f ) , 0 ) )/ / 32!b i t!r e g i s t e r o f f s e t s
#define INTERRUPT_ENABLE_REGISTER (0x00 / 4) / / R/W#define INTERRUPT_STATUS_REGISTER (0x04 / 4) / / R/W#define CONTROL_REGISTER (0x08 / 4) / / R/W#define STATUS_REGISTER (0x0c / 4) / / R
#define COUNTER_PRELOAD_REGISTER (0x10 / 4) / / R#define COUNTER_PRELOAD_LOW_REGISTER (0x10 / 4) / / W
#define COUNTER_REGISTER (0x14 / 4) / / R#define COUNTER_PRELOAD_HIGH_REGISTER (0x14 / 4) / / W
#define WATCHDOG_PRELOAD_REGISTER (0x18 / 4) / / R#define WATCHDOG_PRELOAD_LOW_REGISTER (0x18 / 4) / / W#define WATCHDOG_REGISTER (0 x1c / 4) / / R#define WATCHDOG_PRELOAD_HIGH_REGISTER (0x1c / 4) / / W#define COUNTER_CONTROL_REGISTER (0x20 / 4) / / R/W
/ / I n t e r r u p t Enable Reg is ter b i t s ! watchdog po r t i on#define WD_ENABLE BIT (21) / / 0 = d isab le i n t e r r u p t
/ / 1 = enable i n t e r r u p t/ / Reading from the I n t e r r u p t Enable Reg is ter/ / r e tu rns the contents o f the r e g i s t e r
/ / I n t e r r u p t Status Reg is ter b i t s ! watchdog po r t i on#define WD_STATUS BIT (21) / / 0 = d id not cause an i n t e r r u p t
64
/ / 1 = d id cause an i n t e r r u p t/ / To c l ea r a b i t i n the I n t e r r u p t Status Regis ter ,/ / w r i t e a ’1 ’ to the b i t
/ / Counter Cont ro l Reg is te r b i t s ! watchdog po r t i on#define WD_LD BIT (25) / / 0 : no load opera t ion
/ / 1 : load watchdog counter from ac t i v e preload/ / and WD_VAL
#define WD_VAL BIT (24) / / value of watchdog counter output/ / ( ignored i f WD_LD = 0)
#define WD_ACT BIT (23) / / 0 : deac t ive watchdog fea tu res of/ / WATCHDOG counter/ / 1 : ac t i v e watchdog fea tu res
#define WD_SEL BIT (22) / / 0 : WATCHDOG output i s watchdog/ / i n t e r r u p t s t a t e ( ac t i v e low )/ / 1 : ou tput i s output o f WATCHDOG counter
#define WD_OUTEN BIT (21) / / 0 : d i sab le WATCHDOG output/ / ( ou tpu t always high )/ / 1 : enable WATCHDOG output ( value determined/ / by WD_SEL)
#define WD_PRSEL BIT (20) / / 0 : use Watchdog Preload Low reg/ / 1 : use Watchdog Preload High reg/ / ( ignored i f WD_MODE = 1)
#define WD_WSET BIT (19) / / 0 : use watchdog r e g i s t e r se t/ / #0 f o r w r i t e s to pre load regs/ / 1 : use r e g i s t e r se t #1
#define WD_RSET BIT (18) / / 0 : use watchdog r e g i s t e r se t/ / #0 f o r ac t i v e set and reads/ / 1 : use r e g i s t e r se t #1
#define WD_MODE BIT (17) / / 0 : square wave mode (WD_PRSEL/ / se lec t s pre load r e g i s t e r )/ / 1 : PWM mode ( both preload low and high/ / r e g i s t e r s used )
#define WD_ENAB BIT (16) / / 0 : d i sab le count ing of/ / watchdog counter/ / 1 : enable watchdog counter
65
s ta t i c char_T msg [ 256 ] ;
s ta t i c void md l I n i t i a l i z eS i z e s ( SimStruct *S){
s i z e_ t i ;
ssSetNumSFcnParams(S, NUM_ARGS) ;i f ( ssGetNumSFcnParams (S) != ssGetSFcnParamsCount(S ) ){
s p r i n t f (msg, "%d inpu t args expected , %d passed " ,NUM_ARGS, ssGetSFcnParamsCount(S ) ) ;
ssSetEr rorSta tus (S, msg ) ;return ;
}
ssSetNumContStates (S, 0 ) ;ssSetNumDiscStates (S, 0 ) ;
ssSetSimStateCompliance (S, HAS_NO_SIM_STATE ) ;
ssSetNumSampleTimes (S, 1 ) ;ssSetNumRWork(S, NUM_R_WORKS) ;ssSetNumIWork (S, NUM_I_WORKS ) ;ssSetNumPWork (S, 0 ) ;ssSetNumModes (S, 0 ) ;ssSetNumNonsampledZCs (S, 0 ) ;
for ( i = 0 ; i < NUM_ARGS; i ++)ssSetSFcnParamTunable(S, i , 0 ) ;
ssSetOptions (S,SS_OPTION_DISALLOW_CONSTANT_SAMPLE_TIME|SS_OPTION_EXCEPTION_FREE_CODE ) ;
}
s ta t i c void mdl In i t ia l i zeSampleT imes ( SimStruct *S){
i f (mxGetPr (SAMP_TIME_ARG) [ 0 ] == !1.0){
ssSetSampleTime(S, 0 , INHERITED_SAMPLE_TIME) ;ssSetOffsetTime (S, 0 , FIXED_IN_MINOR_STEP_OFFSET) ;
} else {ssSetSampleTime(S, 0 , mxGetPr (SAMP_TIME_ARG ) [ 0 ] ) ;ssSetOffsetTime (S, 0 , 0 . 0 ) ;
}
66
}
#define MDL_STARTs ta t i c void mdlStar t ( SimStruct *S){# i fndef MATLAB_MEX_FILE
uint32_T * IWork = ssGetIWork (S ) ;uint32_T countervalue = ( uint32_T ) ( mxGetPr (PERIOD_ARG) [ 0 ] / 30e!9 ) ! 1;in t_T pc iS l o t = ( in t_T )mxGetPr (PCI_SLOT_ARG ) [ 0 ] ;i n t_T pciBus = ( in t_T )mxGetPr (PCI_BUS_ARG ) [ 0 ] ;void * bar ;vo la t i l e uint32_T *base ;in t_T boardtype = ( in t_T )mxGetPr (BOARDTYPE_ARG) [ 0 ] ;char *devname ;uint16_T subdev ;xpcPCIDevice p c i I n f o ;
switch ( boardtype ){
case 1:devname = "Quanser Q8" ;subdev = 0x0200 ;break ;
case 2:devname = "Quanser Q4" ;subdev = 0x02AC ;break ;
}
i f ( xpcGetPCIDeviceInfo ( VENDOR_ID, DEVICE_ID , SUBVENDOR_ID, subdev ,pciBus , pc iS lo t , &p c i I n f o ) )
{s p r i n t f (msg, "No %s at bus %d s l o t %d " , devname , pciBus , pc iS l o t ) ;ssSetEr rorSta tus (S, msg ) ;return ;
}
bar = ( void * ) p c i I n f o . BaseAddress [ 0 ] ;
base = ( uint32_T * ) xpcReserveMemoryRegion ( bar ,
i f ( ! xpc IsMode l In i t ( ) ){
67
/ / Clear a l l i n t e r r u p t sbase [ INTERRUPT_STATUS_REGISTER ] = 4294967295;
/ / Check i f i n t e r r u p t i o n by Watchdog i s enable/ / and ac t i v e i t i f i t was d isab lei f ( ( base [ INTERRUPT_ENABLE_REGISTER ] & WD_ENABLE ) == 0x00000000 ){
base [ INTERRUPT_ENABLE_REGISTER ] = WD_ENABLE;}
}
IWork [ BASE_I_IND ] = ( uint32_T ) base ;IWork [COUNTER_I_IND ] = ( uint32_T ) countervalue ;IWork [ LOAD_I_IND ] = ( uint32_T ) WD_ENAB | WD_ACT | WD_OUTEN | WD_LD | WD_VAL;
#endif}
s ta t i c void mdlOutputs ( SimStruct *S, in t_T t i d ){# i fndef MATLAB_MEX_FILE
uint32_T * IWork = ssGetIWork (S ) ;uint32_T counter = IWork [COUNTER_I_IND ] ;uint32_T load = IWork [ LOAD_I_IND ] ;vo la t i l e uint32_T *base = ( vo la t i l e uint32_T * ) IWork [ BASE_I_IND ] ;
/ / Set the Watchdog preload value ( which w i l l determine the per iod )base [WATCHDOG_PRELOAD_REGISTER] = ( uint32_T ) counter ;
/ / Ac t i va te the Watchdog and Force the counter to load immediate lybase [COUNTER_CONTROL_REGISTER ] = ( uint32_T ) load ;
#endif}
s ta t i c void mdlTerminate ( SimStruct *S){# i fndef MATLAB_MEX_FILE
uint32_T * IWork = ssGetIWork (S ) ;vo la t i l e uint32_T *base = ( vo la t i l e uint32_T * ) IWork [ BASE_I_IND ] ;
i f ( ! xpc IsMode l In i t ( ) ){
base [ INTERRUPT_ENABLE_REGISTER ] = 0;base [COUNTER_CONTROL_REGISTER ] = 0;
}
68
#endif}
# i f de f MATLAB_MEX_FILE#include " s imu l ink . c "#else#include " cg_sfun . h "#endif
B.2 Configuração do Amplificador DigiAmp DSA-5
De seguida é apresentado o código C desenvolvido para configuração do amplificador e aquisiçãode dados relativos às forças de reacção na base do manipulador. Este código não pode no entanto serutilizado pois foi desenvolvido para uma versão do Maltab (2010b) sem suporte para UDP em temporeal. O suporte de UDP em tempo real apenas existe para versões do Matlab a partir da 2011a, e requero uso de uma placa de rede dedicada. Este código quando implementado como s-function, estabelecea comunicação pela placa que assegura a comunicação host-target e não pela placa de rede dedicada.Como o amplificador só aceita comunicações de um IP especifico e visto não ser possível ter as duasplaca com o mesmo endereço o código desenvolvido inicialmente não pode ser utilizado.
#define S_FUNCTION_LEVEL 2#undef S_FUNCTION_NAME#define S_FUNCTION_NAME pen
#include <s tddef . h>#include < s t d l i b . h>#include " s ims t ruc . h "#include <math . h>
# i f de f MATLAB_MEX_FILE#include "mex . h "#endif
# i f de f MATLAB_MEX_FILE#include " i nc lude / udpcom. c "#else#include <windows . h>#include " i nc lude / udp_xpcimport . h "#include " i nc lude / u t i l _ xpc impo r t . h "#endif
#define NUM_OUTPUTS 1/ / Output Por t 0#define OUT_PORT_0_NAME y0
69
#define OUTPUT_0_WIDTH ssGetSFcnParam(S, 1)#define OUTPUT_DIMS_0_COL 1#define OUTPUT_0_DTYPE real_T#define OUTPUT_0_COMPLEX COMPLEX_NO#define OUT_0_FRAME_BASED FRAME_NO#define OUT_0_DIMS 1!D#define OUT_0_ISSIGNED 1#define OUT_0_WORDLENGTH 8#define OUT_0_FIXPOINTSCALING 1#define OUT_0_FRACTIONLENGTH 3#define OUT_0_BIAS 0#define OUT_0_SLOPE 0.125
#define NPARAMS 8
#define ID_ARG ssGetSFcnParam(S, 0)#define NO_BYTES_ARG ssGetSFcnParam(S, 1)#define SAMPLE_TIME_ARG ssGetSFcnParam(S, 2)#define BLOCK_DATASET_ARG ssGetSFcnParam(S, 3)#define GAIN_ARG ssGetSFcnParam(S, 4)#define FILTER_FREQUENCY_ARG ssGetSFcnParam(S, 5)#define EX_VOLTAGE_ARG ssGetSFcnParam(S, 6)#define DEC_DEN_ARG ssGetSFcnParam(S, 7)#define VBL_LEN_ARG (1 )
#define COMMAND_SYNCHRONIZE 0x0001#define COMMAND_SET_STATE 0x0002#define COMMAND_SET_CAL 0x0004#define COMMAND_ID_REQUEST 0x0005#define COMMAND_START_CONTINUOUS 0x0006#define COMMAND_RWIRE 0x0007#define COMMAND_STOP 0x0008#define COMMAND_AUTOZERO 0x0009#define COMMAND_RESET 0x000B
#define SAMPLE_TIME_0 INHERITED_SAMPLE_TIME#define NUM_DISC_STATES 0#define DISC_STATES_IC [ 0 ]#define NUM_CONT_STATES 0#define CONT_STATES_IC [ 0 ]#define SAMP_TIME_IND (0 )
#define SFUNWIZ_GENERATE_TLC 1#define SOURCEFILES "__SFB__ "#define PANELINDEX 6
70
#define USE_SIMSTRUCT 0#define SHOW_COMPILE_STEPS 0#define CREATE_DEBUG_MEXFILE 0#define SAVE_CODE_ONLY 0#define SFUNWIZ_REVISION 3.0
#define MAX_SIZE 60
#define NO_I_WORKS (2 )
#define NO_R_WORKS (0 )
#define NO_P_WORKS (1 )
typedef short CID ;
struct amp l i f i e r s e t t i n g s{
short gain [ 6 ] ;short f i l t e r F r e q [ 6 ] ;short exVol t [ 6 ] ;short decimationNumerator ;short decimationDenominator ;
} ;
struct SetStateCommand{
CID c id ;struct amp l i f i e r s e t t i n g s ampSettings ;short blockDatasetCount ;long hostAddress ;
} ;
struct StartContinuousCommand{
CID c id ; / / Set c id =COMMAND_START_CONTINUOUS ;short blockDatasetCount ;
} ;
struct StopCommand{
CID c id ; / / Set c id =COMMAND_STOP;} ;
struct AutoZeroCommand
71
{CID c id ; / / Set c id =COMMAND_AUTOZERO;short spare ;
} ;
struct ResetCommand{
CID c id ; / / Set c id =COMMAND_RESET;short spare ;
} ;
struct SynchronzeCommand{
CID c id ; / / Set c id =COMMAND_SYNCHRONIZE;} ;
struct SetStateCommand SetState ;struct StartContinuousCommand Star tCont inuous ;struct StopCommand Stop ;struct AutoZeroCommand AutoZero ;struct ResetCommand Reset ;struct SynchronzeCommand Sync ;
s ta t i c char_T msg [ 256 ] ;i n t i ;
s ta t i c void md l I n i t i a l i z eS i z e s ( SimStruct *S){
ssSetNumSFcnParams(S, NPARAMS) ;i f ( ssGetNumSFcnParams (S) != ssGetSFcnParamsCount(S ) ){
return ; / * Parameter mismatch w i l l be repor ted by Simul ink * /}
i f ( mxGetM(NO_BYTES_ARG) !=1 | | mxGetN(NO_BYTES_ARG) !=1 ){
s p r i n t f (msg, " Output po r t width argument must be a sca la r \ n " ) ;ssSetEr rorSta tus (S, msg ) ;return ;
}
ssSetNumContStates (S, NUM_CONT_STATES ) ;ssSetNumDiscStates (S, NUM_DISC_STATES) ;
i f ( ! ssSetNumOutputPorts (S, NUM_OUTPUTS) )
72
return ;
ssSetOutputPortWidth (S, 0 , ( i n t_T ) mxGetPr (NO_BYTES_ARG) [ 0 ] ) ;ssSetOutputPortDataType (S, 0 , SS_UINT8 ) ;
i f ( ! ssSetNumInputPorts (S, 0 ) )return ;
ssSetNumSampleTimes (S, 1 ) ;ssSetNumRWork(S, NO_R_WORKS) ;ssSetNumIWork (S, NO_I_WORKS) ;ssSetNumPWork (S, NO_P_WORKS) ;ssSetNumModes (S, 0 ) ;ssSetNumNonsampledZCs (S, 0 ) ;
for ( i = 0 ; i < NPARAMS; i ++ ){
/ / None of the parameters are tunab lessSetSFcnParamTunable(S, i , 0 ) ;
} ;}
md l In i t ia l i zeSamp leT imes ( SimStruct *S){
ssSetSampleTime (S, 0 , mxGetPr (SAMPLE_TIME_ARG ) [ SAMP_TIME_IND ] ) ;ssSetOffsetTime (S, 0 , 0 . 0 ) ;
}
#define MDL_START s ta t i c void mdlStar t ( SimStruct * S){# i fndef MATLAB_MEX_FILE
uchar_T ipAddress [ 1 6 ] ;in t8_T ID ;in t_T ipPor t , count , ipPortR , broadcast , l oca lPor t , noBytes , i d ;char s t r [ 4 8 ] ;char s t r 1 [ 4 ] ;char s t r 2 [ 4 ] ;char s t r 3 [ 2 ] ;u int8_T * bu f f e r ;ID = ( in t_T ) mxGetPr ( ID_ARG ) [ 0 ] ;
i f ( ID == 1 ){
s p r i n t f ( ipAddress , " 192.168.0.151 " ) ;ipPor tR = ( in t_T ) 1025;
73
} else i f ( ID == 2 ) {s p r i n t f ( ipAddress , " 192.168.0.152 " ) ;ipPor tR = ( in t_T ) 1026;
} else i f ( ID == 3 ) {s p r i n t f ( ipAddress , " 192.168.0.153 " ) ;ipPor tR = ( in t_T ) 1027;
} else i f ( ID == 4 ) {s p r i n t f ( ipAddress , " 192.168.0.154 " ) ;ipPor tR = ( in t_T ) 1028;
} else i f ( ID == 5 ) {s p r i n t f ( ipAddress , " 192.168.0.155 " ) ;ipPor tR = ( in t_T ) 1029;
} else i f ( ID == 6 ) {s p r i n t f ( ipAddress , " 192.168.0.156 " ) ;ipPor tR = ( in t_T ) 1030;
} elses p r i n t f ( ipAddress , " 192.168.0.151 " ) ;
SetState . c id = COMMAND_SET_STATE;
SetState . ampSettings . gain [ 0 ] = ( in t8_T ) mxGetPr (GAIN_ARG) [ 0 ] ;SetState . ampSettings . gain [ 1 ] = ( in t8_T ) mxGetPr (GAIN_ARG) [ 1 ] ;SetState . ampSettings . gain [ 2 ] = ( in t8_T ) mxGetPr (GAIN_ARG) [ 2 ] ;SetState . ampSettings . gain [ 3 ] = ( in t8_T ) mxGetPr (GAIN_ARG) [ 3 ] ;SetState . ampSettings . gain [ 4 ] = ( in t8_T ) mxGetPr (GAIN_ARG) [ 4 ] ;SetState . ampSettings . gain [ 5 ] = ( in t8_T ) mxGetPr (GAIN_ARG) [ 5 ] ;SetState . ampSettings . f i l t e r F r e q [ 0 ] = ( in t8_T ) mxGetPr (FILTER_FREQUENCY_ARG ) [ 0 ] ;SetState . ampSettings . f i l t e r F r e q [ 1 ] = 0;SetState . ampSettings . f i l t e r F r e q [ 2 ] = 0;SetState . ampSettings . f i l t e r F r e q [ 3 ] = 0;SetState . ampSettings . f i l t e r F r e q [ 4 ] = 0;SetState . ampSettings . f i l t e r F r e q [ 5 ] = 0;SetState . ampSettings . exVo l t [ 0 ] = ( in t8_T ) mxGetPr (EX_VOLTAGE_ARG) [ 0 ] ;SetState . ampSettings . exVo l t [ 1 ] = ( in t8_T ) mxGetPr (EX_VOLTAGE_ARG) [ 1 ] ;SetState . ampSettings . exVo l t [ 2 ] = ( in t8_T ) mxGetPr (EX_VOLTAGE_ARG) [ 2 ] ;SetState . ampSettings . exVo l t [ 3 ] = ( in t8_T ) mxGetPr (EX_VOLTAGE_ARG) [ 3 ] ;SetState . ampSettings . exVo l t [ 4 ] = ( in t8_T ) mxGetPr (EX_VOLTAGE_ARG) [ 4 ] ;SetState . ampSettings . exVo l t [ 5 ] = ( in t8_T ) mxGetPr (EX_VOLTAGE_ARG) [ 5 ] ;SetState . ampSettings . decimationNumerator = 1;SetState . ampSettings . decimationDenominator = ( int16_T ) mxGetPr (DEC_DEN_ARG) [ 0 ] ;SetState . blockDatasetCount = ( in t8_T ) mxGetPr (BLOCK_DATASET_ARG ) [ 0 ] ;SetState . hostAddress = 3232235670;
Star tCont inuous . c id = COMMAND_START_CONTINUOUS ;Star tCont inuous . blockDatasetCount = ( in t8_T ) mxGetPr (BLOCK_DATASET_ARG ) [ 0 ] ;
74
AutoZero . c id = COMMAND_AUTOZERO;AutoZero . spare = 0;
Reset . c id = COMMAND_RESET;
Sync . c id = COMMAND_SYNCHRONIZE;
i pPo r t = ( in t_T ) 1025;broadcast = ! strcmp ( ipAddress , " 255.255.255.255 " ) ;l o c a lPo r t = ( in t_T ) 1025;
memcpy( s t r , ( char * ) &SetState , sizeof ( SetState ) ) ;memcpy( s t r1 , ( char * ) &Star tCont inuous , sizeof ( Star tCont inuous ) ) ;memcpy( s t r2 , ( char * ) &AutoZero , sizeof ( AutoZero ) ) ;memcpy( s t r3 , ( char * ) &Sync , sizeof ( Sync ) ) ;
i d = 0;
i f ( xpce IsMode l In i t ( )== fa l s e ){
i d = xpceUDPOpenSend( ipAddress , loca lPor t , ipPor t , broadcast , MAX_SIZE ) ;
i f ( i d < 0){
s p r i n t f (msg, " Er ro r : No udp Channel f r ee or udp e r r o r " ) ;ssSetEr rorSta tus (S, msg ) ;
}
count = xpceUDPSend ( id , s t r , sizeof ( SetState ) ) ;count = xpceUDPSend ( id , s t r3 , sizeof ( Sync ) ) ;count = xpceUDPSend ( id , s t r1 , sizeof ( Star tCont inuous ) ) ;count = xpceUDPSend ( id , s t r1 , sizeof ( Star tCont inuous ) ) ;xpceUDPClose( i d ) ;
}
/ / Receive packets from ampnoBytes = ( in t_T ) mxGetPr (NO_BYTES_ARG) [ 0 ] ;
/ / a l l o c memory and s to re po in t e rbu f f e r = ( u int8_T * ) ca l l o c ( noBytes , sizeof ( u int8_T ) ) ;
i f ( bu f f e r == NULL){
s p r i n t f (msg, " Er ro r i n udpbytereceive : memory a l l o c a t i o n f a i l e d " ) ;
75
ssSetEr rorSta tus (S, msg ) ;return ;
}
ssSetPWorkValue (S, 0 , ( void * ) bu f f e r ) ;
/ / i n i t i a l i z e bu f f e r w i th zerosmemset ( ( void * ) bu f fe r , 0 , sizeof ( u int8_T ) ) ;
i d = 0;
i f ( xpce IsMode l In i t ( )== fa l s e )i d = xpceUDPOpenReceive ( ipPortR , ipAddress , noBytes ) ;
i f ( i d < 0){
s p r i n t f (msg, " Er ro r : No UDP channel f r ee or UDP e r r o r Receive " ) ;ssSetEr rorSta tus (S, msg ) ;
}
ssSetIWorkValue (S, 0 , i d ) ;ssSetIWorkValue (S, 1 , noBytes ) ;
#endif}
s ta t i c void mdlOutputs ( SimStruct *S, in t_T t i d ){# i fndef MATLAB_MEX_FILE
uint8_T * bu f f e r ;i n t_T id , noBytes , count ;uint32_T ipAddressSender ;i n t_T len ;
#define IS_VBL_LEN ( ( i n t ) ( 1 ) )# i fndef MIN #define MIN(a , b ) ( ( a ) < ( b ) ? ( a ) : ( b ) )#endif
bu f f e r = ( u int8_T * ) ssGetPWorkValue (S, 0 ) ;i d = ssGetIWorkValue (S, 0 ) ;noBytes = ssGetIWorkValue (S, 1 ) ;
count = xpceUDPReceive ( id , bu f fe r , noBytes , ( void * )&ipAddressSender , &len ) ;
i f ( count == 0)
76
{i f ( IS_VBL_LEN)
{memcpy ( ( void * ) ssGetOutputPor tSignal (S, 0 ) ,( void * ) ( u int8_T * ) bu f fe r , MIN( len , noBytes ) ) ;
}}
#endif}
s ta t i c void mdlTerminate ( SimStruct *S){# i fndef MATLAB_MEX_FILE
void * bu f f e r = ssGetPWorkValue (S, 0 ) ;i n t i d = ssGetIWorkValue (S, 0 ) ;uchar_T ipAddress [ 1 6 ] ;in t8_T ID ;in t_T ipPor t , count ;i n t_T broadcast ;i n t_T l o c a lPo r t ;char s t r [ 4 ] ;
i f ( xpce IsMode l In i t ( )== fa l s e ){
xpceUDPClose( i d ) ;}
i f ( bu f f e r != NULL){
f r ee ( bu f f e r ) ;}
ID = ( in t_T ) mxGetPr ( ID_ARG ) [ 0 ] ;
i f ( ID == 1 )s p r i n t f ( ipAddress , " 192.168.0.151 " ) ;
else i f ( ID == 2 )s p r i n t f ( ipAddress , " 192.168.0.152 " ) ;
else i f ( ID == 3)s p r i n t f ( ipAddress , " 192.168.0.153 " ) ;
else i f ( ID == 4)s p r i n t f ( ipAddress , " 192.168.0.154 " ) ;
else i f ( ID == 5)s p r i n t f ( ipAddress , " 192.168.0.155 " ) ;
else i f ( ID == 6)
77
s p r i n t f ( ipAddress , " 192.168.0.156 " ) ;else
s p r i n t f ( ipAddress , " 192.168.0.151 " ) ;
Stop . c id = COMMAND_STOP;
memcpy( s t r , ( char * ) &Stop , sizeof ( Stop ) ) ;
i pPo r t = ( in t_T ) 1025;broadcast = ! strcmp ( ipAddress , " 255.255.255.255 " ) ;l o c a lPo r t = ( in t_T ) 1025;i d = 0;
i f ( xpce IsMode l In i t ( )== fa l s e ){
i d = xpceUDPOpenSend( ipAddress , loca lPor t , ipPor t ,broadcast , sizeof ( Stop ) ) ;
}
i f ( i d < 0){
s p r i n t f (msg, " Er ro r : No udp Channel f r ee or udp e r r o r " ) ;ssSetEr rorSta tus (S, msg ) ;
}
i f ( xpce IsMode l In i t ( )== fa l s e ){
count = xpceUDPSend ( id , s t r , sizeof ( Stop ) ) ;xpcBusyWait ( . 1 ) ;count = xpceUDPSend ( id , s t r , sizeof ( Stop ) ) ;
}
i f ( xpce IsMode l In i t ( )== fa l s e ){
xpceUDPClose( i d ) ;}
#endif}
# i f de f MATLAB_MEX_FILE#include " s imu l ink . c "#else#include " cg_sfun . h "#endif
78
![Multi-Layer Perceptron in Matlab NN Toolbox [Part 1]](https://img.document.onl/doc/110x75/62057c23b540662ec754025f/multi-layer-perceptron-in-matlab-nn-toolbox-part-1.jpg)
