Upload
haduong
View
221
Download
0
Embed Size (px)
Citation preview
Universidade Federal do Rio de Janeiro
MODELAGEM CINEMÁTICA E DINÂMICA DA MARCHA DE UM ROBÔ BÍPEDE
Gabriel Santiago Rego
2009
COPPE/UFRJCOPPE/UFRJ
MODELAGEM CINEMÁTICA E DINÂMICA DA MARCHA DE UM ROBÔ BÍPEDE
Gabriel Santiago Rego
Dissertação de Mestrado apresentada ao
Programa de Pós-graduação em Engenharia
Civil, COPPE, da Universidade Federal do Rio
de Janeiro, como parte dos requisitos necessários
à obtenção do título de Mestre em Engenharia
Civil.
Orientador: Max Suell Dutra
Rio de Janeiro
Junho de 2009
MODELAGEM CINEMÁTICA E DINÂMICA DA MARCHA DE UM ROBÔ BÍPEDE
Gabriel Santiago Rego
DISSERTAÇÃO SUBMETIDA AO CORPO DOCENTE DO INSTITUTO ALBERTO
LUIZ COIMBRA DE PÓS-GRADUAÇÃO E PESQUISA DE ENGENHARIA (COPPE)
DA UNIVERSIDADE FEDERAL DO RIO DE JANEIRO COMO PARTE DOS
REQUISITOS NECESSÁRIOS PARA A OBTENÇÃO DO GRAU DE MESTRE EM
CIÊNCIAS EM ENGENHARIA MECÂNICA.
Aprovada por:
________________________________________________
Prof. Max Suell Dutra, Dr. -Ing.
________________________________________________ Prof. Jules Ghislain Slama, D. Sc.
________________________________________________ Prof. Felipe Maia Galvão França, Ph.D.
________________________________________________ Prof. Armando Carlos de Pina Filho, D.Sc.
RIO DE JANEIRO, RJ - BRASIL
JUNHO DE 2009
iii
Rego, Gabriel Santiago
Modelagem Cinemática e Dinâmica da Marcha de um
Robô Bípede / Gabriel Santiago Rego. – Rio de Janeiro:
UFRJ/COPPE, 2009.
VIII, 193 p.: il.; 29,7 cm.
Orientador: Max Suell Dutra
Dissertação (mestrado) – UFRJ/ COPPE/ Programa de
Engenharia Mecânica, 2009.
Referências Bibliográficas: p. 120-124.
1. Biomecânica. 2. Robótica. 3. Locomoção Bípede. I.
Dutra, Max Suell. II. Universidade Federal do Rio de
Janeiro, COPPE, Programa de Engenharia Mecânica. III.
Titulo.
iv
Dedico este trabalho aos meus pais Getúlio e Margarethe que me ensinaram o valor do
amor e da educação que me transforma no homem que sou, e ao meu orientador Max Suell
que teve a paciência e sabedoria de um verdadeiro mestre.
v
Resumo da Dissertação apresentada à COPPE/UFRJ como parte dos requisitos necessários para a obtenção do grau de Mestre em Ciências (M.Sc.)
MODELAGEM CINEMÁTICA E DINÂMICA DA MARCHA DE UM ROBÔ BÍPEDE
Gabriel Santiago Rego
Junho/2009
Orientador: Max Suell Dutra
Programa: Engenharia Mecânica
Analisando os novos trabalhos com robôs bípedes e comparando com a máquina
humana, tão perfeita em sua otimização de energia em sua locomoção, claramente se vê o
quanto se pode desenvolver na movimentação destes tipos de robôs. Os robôs bípedes têm
grandes vantagens em relação a outros meios de locomoção. Diferente de robôs com três ou
mais pernas, eles ocupam um menor espaço no ambiente, e, diferente de robôs com rodas
que precisam de um trajeto contínuo, eles conseguem se adequar a vários tipos de terrenos.
O presente trabalho visa apresentar um estudo cinemático e dinâmico de uma passada, ou
seja, de um ciclo completo da locomoção do bípede, onde os resultados serão utilizados
para modelar um sistema de equilíbrio que utilize um mecanismo giroscópio, considerando
a diferença na topologia da movimentação nas fases suporte simples (um pé em contato
com o solo) e suporte duplo (dois pés em contato com o solo) formando cadeias aberta e
fechada. A metodologia dos sistemas multicorpos será apresentada para a solução do
sistema formado em ambas as cadeias cinemáticas.
vi
Abstract of Dissertation presented to COPPE/UFRJ as a partial fulfillment of the
requirements for the degree of Master of Science (M.Sc.)
KINEMATICS AND DYNAMICS MODELING OF THE BIPED ROBOT MARCH
Gabriel Santiago Rego
June /2009
Advisor: Max Suell Dutra
Department: Mechanical Engineering
Analyzing the new works about biped robots and comparing them with the human
machine, so perfect in the optimization of the use of energy during locomotion, it is clearly
seen how much the movement of these types of robots can developed. Biped robots have
great advantages in relation to another types of locomotion. Different robots with three or
more legs occupy smaller space in the environment and, different of robots with wheels that
need continuos trajectory, they can be adequate to a lot of environments types. The present
work presents the kinematics and dynamic studies of a stride, in the other words, the
complete cycle of the biped locomotion. The results are used to model a balanced system
that use a gyroscopic mechanism, considering the difference of topology in the movement
in the simple support fase (when one foot is in contact with the ground) and duple support
fase (when both feet are in contact with the ground), forming open and close chains. The
multibody systems methodology will be presented for the solution of the systems produced
in both kinematics chains.
vii
ÍNDICE
1.INTRODUÇÃO ..............................................................................................
1.1 Seqüência do Processo de Locomoção ................................................
1.2 Planos Anatômicos ...................................................................................
1.3 Periodicidade da Marcha .......................................................................
1.4 Eventos na Marcha ...................................................................................
1.5 Distâncias Medidas ...................................................................................
1.6 Determinantes no Modo de Andar ........................................................
2.CINEMÁTICA ..............................................................................................
2.1 Modelo Utilizado ...................................................................................
2.2 Restrições ...................................................................................
2.3 Graus de Liberdades e Coordenadas Generalizadas ..................................
2.4 Fase De Suporte Simples .......................................................................
2.4.1 Cinemática Direta ................................................
2.4.2 Velocidades e Acelerações dos pontos nas juntas......................
2.4.3 Cinemática Inversa ...........................................................
2.4.4 Velocidades e Acelerações dos Ângulos das juntas...................
2.5 Fase De Suporte Duplo .......................................................................
2.5.1 Velocidades e Acelerações dos Ângulos das Juntas...................
3.FUNÇÕES DOS ÂNGULOS DAS JUNTAS NO TEMPO ........................
3.1 Análise Harmônica ...................................................................................
3.2 Análise das Funções dos Ângulos no Tempo ..................................
3.2.1 Análise das Coordenadas Generalizadas em Cadeia Fechada ......................................................... 3.2.2 Análise das Coordenadas Dependentes em Cadeia Fechada .........................................................
4.DINÂMICA ..............................................................................................
4.1 Dinâmica de Mecanismos de Sistemas Multicorpos .......................
4.2 Dinâmica com restrições .......................................................................
4.3 Principio Generalizado de d’Alembert para Sistemas de Partículas .......................................................................
1
2
4
5
8
12
13
17
17
20
21
22
23
26
28
35
35
40
42
45
47
47
55
69
69
70
71
viii
4.4 O Método dos Multiplicadores de Lagrange para Sistemas de Partículas ...............................................
4.5 Equação de Lagrange para Corpos Rígidos ...............................................
4.5.1 Obtenção da Energia Cinética ................................................
4.5.2 Obtenção da Energia Potencial ................................................
4.6 Equação de Movimento para Fase de Suporte Simples ........................
4.7 Equação de Movimento para Fase de Suporte Duplo...................................
5.MODELAGEM DO MECANISMO DE EQUILÍBRIO ....................................
5.1 Os Ângulos de Euler ...................................................................................
5.2 O Giroscópio e os Ângulos de Euler ...............................................
5.2.1 Coordenadas Ignoráveis ...........................................................
5.2.2 Precessão em regime constante ...............................................
5.3 Forças Atuantes na Cintura .......................................................................
6.CONCLUSÃO E TRABALHOS FUTUROS ...............................................
REFERÊNCIAS BIBLIOGRÁFICAS ...........................................................
ANEXO A. Programação do Cálculo da Cinemática Desenvolvido no MatLab ....
ANEXO B. Programação do Cálculo da Dinâmica Desenvolvido no MatLab ........
ANEXO C. Programação do Cálculo do Dispositivo Giroscópio Desenvolvido no MatLab .............................................................................................
71
72
74
78
79
85
97
97
99
104
105
106
117
120
125
158
181
1
1. Introdução
O sistema locomotor humano é um exemplo do grau de inteligência existente na
natureza. A capacidade de controlar influências externas se adequando a ambientes bem
variados, utilizando ou dissipando as energias existentes no processo, tornam a
locomoção bastante eficiente e suave. O estudo de sistemas que realizam funções
motoras, sobretudo em membros mecânicos como pernas e braços, tem como objetivo
não somente projetar robôs autônomos, mas também na reabilitação de pessoas que
tenham sofrido algum tipo de acidente que tenha prejudicado seu sistema locomotor. O
estudo da marcha está inserido neste contexto, e tem sido intensivamente estudada
desde a segunda metade do século XX. Uma visão do estudo da técnica até 1990 pode
ser encontrada em trabalhos como [1], [2], [3] e [4].
A utilização de apenas dois membros para locomoção possibilita o deslocamento
em espaços menores, aumentando a quantidade de locais propícios para sua
movimentação. Em resumo, pode-se dizer que esta locomoção é o deslocamento do
centro de massa de um corpo por uma trajetória senoidal ótima do ponto de vista
energético e funcional. Esta definição [5] e [6] esta baseada na teoria do “consumo de
energia mínima”, onde o movimento simultâneo da pélvis e dos corpos constituintes nos
membros inferiores produz um padrão suave de movimento. Assim, o mau
funcionamento de um ou mais componentes podem levar ao aumento de energia
requerida neste sistema. Esta teoria vem sendo amplamente explorada por pesquisadores
com o objetivo do desenvolvimento de um modelo dinâmico para a locomoção humana.
Vários trabalhos têm desenvolvido este estudo voltado para a aplicação em
amputados. Estudos do desenvolvimento de modelos da locomoção humana voltada
para amputados podem ser encontrados em trabalhos como [7] e [8].
Neste trabalho um estudo da cinemática e da dinâmica de robôs bípedes visa dar
uma visão geral de todo o processo de modelagem do sistema de locomoção sendo
aplicado em um sistema de equilíbrio do robô que será modelado por um giroscópio
acoplado no centro da cintura. Um estudo recente da cinemática de robôs bípedes pode
ser encontrado em [9]. Contudo, alguns esclarecimentos devem ser preliminarmente
dados, de modo a um melhor entendimento sobre os estudos descritos neste trabalho.
2
1.1 Sequência do Processo de locomoção
Segundo MACKAY-LYONS [10], existem redes de células nervosas que
produzem movimentos rítmicos específicos para um grande número de vertebrados.
Estas redes nervosas na medula espinhal são capazes de produzir movimentos rítmicos,
tais como nadar, andar e pular. Esses sistemas nervosos especializados são conhecidos
como osciladores nervosos ou geradores centrais de padrões.
Segundo VAUGHAN et al [11], a “programação” destes geradores centrais de
padrões do sistema locomotor humano ocorre no centro supra-espinhal e envolve a
conversão de idéias em atividades nos músculos necessários para se locomover. A saída
da rede neural que resulta desta programação supra-espinhal pode ser imaginada como
um comando do centro locomotor, sendo transmitida através do cerebelo e da corda
espinhal. Um estudo bem descritivo sobre os geradores centrais de padrão formado por
um conjunto de osciladores não-lineares mutuamente acoplados foi dado por PINA
FILHO [12] em 2005.
A execução deste comando envolve duas componentes: A ativação dos centros
neurais inferiores que determinam a seqüência de ativação dos músculos requeridos, e a
execução de um sensoriamento feedback dos músculos e juntas que modificam os
movimentos.
Esta interação entre o sistema nervoso central, sistemas nervosos periféricos e o
sistema músculo-esquelético é ilustrado na Figura 1.1. Os loops do sensoriamento
feedback não são mostrados nesta figura.
3
Figura 1.1: As sete componentes que formam a base funcional que caracteriza como se
caminha. Esta aproximação constitui um modelo de causa e efeito [11].
Os músculos desenvolvem tensão que geram forças e momentos através das juntas
sinoviais. As forças e momentos nas juntas movem as partes esqueléticas rígidas que
por sua vez movem o conjunto rígido como a coxa, pé ou o dedo, exercendo forças nos
ambientes externos, movimentando o corpo pela reação do terreno.
A seqüência e eventos que são necessários para a marcha podem ser descritos
como as seguintes:
1. Registro e ativação do comando da marcha no sistema nervosa central.
2. Transmissão dos sinais através do sistema nervoso periférico.
3. Contração do músculo pelo desenvolvimento de tensão.
4. Geração de forças e momentos através das juntas sinoviais.
4
5. Regulação das forças e momentos nas juntas pelos segmentos esqueléticos rígidos nas
suas antropometrias.
6. Movimentação dos segmentos na maneira reconhecida pela marcha funcional.
7. Geração das forças de reação do solo.
Estes sete eventos em cadeia resultam na forma de movimentação que caracteriza
como a locomoção humana se desenvolve, ou a marcha humana como ilustrado na
Figura 1.2.
Figura 1.2: A seqüência dos sete eventos que levam a caminhada [11].
1.2 Planos Anatômicos
Para uma melhor compreensão da locomoção são adotadas algumas definições
quanto aos planos de movimento e regiões do corpo. O estudo da marcha de um bípede
[13] foi separado em três planos anatômicos: O plano frontal ou coronal, o plano
transverso e o plano sagital.
O plano frontal ou coronal divide o corpo nas regiões anterior e posterior, o plano
transverso o divide em superior e inferior. O plano sagital divide o mesmo nas regiões
direita e esquerda como ilustrado na Figura 1.3.
5
Figura 1.3: Planos anatômicos da marcha [13].
A movimentação se dá nos três planos. Sem dúvida, o principal plano para estudo
é o plano sagital. É nele onde acontece a maioria dos movimentos [13]. Entretanto, os
movimentos nos outros dois planos também têm importância e em um estudo mais
profundo do movimento em três dimensões não devem ser desprezados.
1.3 Periodicidade da Marcha
O ato de andar tem dois requisitos básicos:
1. Movimento periódico de cada pé de uma posição de suporte a seguinte.
2. Suficientes forças de reação do solo aplicadas no pé para suportar o corpo.
Estes dois elementos são necessários para ocorrer qualquer forma de locomoção
de um bípede, seja ela a caminhada ou a corrida. Este movimento periódico da perna é a
essência do ciclo natural da marcha humana.
A Figura 1.4 ilustra o movimento de uma roda da esquerda para a direita. No
primeiro momento a reta de referencia da roda esta totalmente na vertical. A roda não
6
esta estacionaria aqui. Por convenção, o começo do ciclo é considerado como 0%. Com
a movimentação da roda, a reta de referencia desenvolve uma rotação no sentido anti-
horário. Em 20% sua rotação é de 72o. A cada 20% a roda sofre uma rotação de 72o ate
a reta chegar a sua posição original onde o ciclo se completa (100% e 360o).
Figura 1.4: A rotação de uma roda representando o ciclo natural do movimento em
progresso [11].
Esta analogia com a roda pode ser aplicada na marcha humana. Quando se pensa
em alguém caminhando, se pensa em um ciclo característico de movimentos que se
repete passo a passo. A descrição da caminhada é normalmente proposta como um
único ciclo, assumindo que os sucessivos ciclos são todos os mesmos. Embora esta
afirmação não seja totalmente verdadeira, será valida para a maioria dos casos [11]. A
Figura 1.5 ilustra um ciclo de uma criança de 8 anos normal. Por convenção o ciclo
começa quando um pé (neste caso o pé direito) faz contato com o solo.
Figura 1.5: A marcha de um garoto de 8 anos normal [11].
7
Existem duas fases principais na marcha humana: Durante a fase de suporte o pé
está no solo suportando o corpo, enquanto na fase de balanço o mesmo pé está
desencostado do solo se movimentando em um formato pendular ate a proximidade de
tocar o solo quando o ciclo se reinicia.
A fase de suporte pode ser subdividida em três subfases, como pode ser visto na
Figura 1.5. A fase de suporte simples ou balanço é caracterizado pela sustentação do
corpo por apenas uma perna, enquanto a outra se encontra em um movimento pendular.
A fase de suporte duplo ocorre quando as duas pernas estão em contato com o solo, o
suportando. Nesta fase ocorre a troca das funções das pernas conforme Figura 1.6. Os
cálculos cinemáticos e dinâmicos serão elaborados separados para cada fase, uma vez
que suas características, principalmente o número de graus de liberdade é diferente.
Figura 1.6: Fases de suporte duplo e balanço durante o andar [13].
A nomenclatura na Figura 1.5 refere-se à parte direita do corpo. A mesma
terminologia pode ser aplicada na perna esquerda. Assim, o primeiro suporte duplo é
para perna direita, o segundo suporte duplo é para perna esquerda. Em uma marcha
normal há uma simetria natural entre os lados direito e esquerdo que pode ser visto na
Figura 1.7. Observa-se também a simetria nos tempos das fases de suporte (62%) e
balanço (38%). A proporção de cada fase vai variar conforme o espaço percorrido em
cada ciclo devido às condições como o tamanho do passo ou a velocidade do bípede,
Principalmente na parte em suporte duplo caracterizado por ser uma cadeia fechada.
8
Figura 1.7: Tempo de cada fase na marcha para um determinado passo [11].
1.4 Eventos na Marcha
Tradicionalmente, o ciclo da marcha é dividido em oito períodos ou eventos.
Cinco na fase de suporte e três na fase de balanço. O nome desses eventos são
autodescritivos e baseados no movimento do pé, como vistos na Figura 1.12. A fase de
suporte possui os seguintes eventos:
1. O Contato inicial inicia o ciclo da marcha e representa o ponto que o centro de massa
do corpo está em sua posição mais baixa.
Figura 1.8: Contato inicial e aceitação de carga [14].
9
2. Aceitação de Carga acontece quando a superfície plantar do pé toca o solo.
3. Suporte médio ocorre quando o pé em balanço passa o pé em suporte. O centro de
massa do corpo está em sua posição mais alta.
Figura 1.9: Suporte médio [14].
4. Suporte final ocorre assim que o calcanhar perde contato com o solo e é iniciada a
contração da musculatura tibial posterior para a impulsão do membro para o balanço.
5. No Pré-balanço termina a fase de suporte assim que o pé deixa o solo.
10
Figura 1.10: Pré-balanço e suporte final [14].
Os eventos da fase de balanço são:
6. No balanço inicial a aceleração começa assim que o pé deixa o solo e o sujeito ativa
os músculos flexores do quadril para acelerar a perna para frente.
7. Balanço médio ocorre quando o pé passa diretamente através do corpo, coincidente
com o suporte médio para o outro pé.
8. No balanço final a desaceleração descreve a ação dos músculos quando eles freiam a
perna e estabilizam o pé em preparação para o próximo contato inicial.
11
Figura 1.11: Balanço inicial, balanço médio e balanço final [14].
Figura 1.12: A tradicional nomenclatura que descreve os eventos observados na marcha
humana [11].
1. Contato inicial (0%) 2. Aceitação de carga (0-10%) 3. Suporte médio (10-30%)
12
4. Suporte final (30-50%) 5. Pré-balanço (50-60%) 6. Balanço inicial (60-70%) 7. Balanço médio (70-85%) 8. Balanço final (85-100%)
1.5 Distâncias Medidas
Considerando que as Figuras 1.5 a 1.12 enfatizam os aspectos temporais da
marcha humana, as Figuras 1.13 e 1.14 ilustram alguns parâmetros de distância muito
utilizados.
O ciclo da marcha pode ser definido pelo tempo de duração de uma passada,
caracterizado pelo período compreendido entre o primeiro toque do pé contra o solo até
o instante em que o mesmo pé volta a tocar o solo, iniciando uma nova fase de suporte.
A Figura 1.13 mostra a interpretação para uma passada e para um passo, que é definido
pela distância entre os dois pés no instante do contato inicial com o solo. O
comprimento da passada é a distância atravessada por uma pessoa durante dois passos
(um ciclo) e pode ser medido como o comprimento entre o calcanhar de um pé ate o
mesmo um ciclo após.
Figura 1.13: Passo e Passada [12].
Dois comprimentos de passo (esquerdo mais o direito) fazem uma passada. Em
sujeitos normais, os dois passos serão aproximadamente iguais.
13
Outro parâmetro muito utilizado é a largura do passo (figura 1.14), que é a
distancia lateral entre os pés e tem o valor de poucos centímetros para um sujeito
normal [14]. Finalmente, o ângulo do pé relativo à linha de progressão pode também
prover importante informação, mostrando o ângulo de rotação interna ou externa da
extremidade inferior durante a fase de suporte.
Figura 1.14: Parâmetros de distancia de uma marcha [14].
1.6 Determinantes do Modo de Andar
A marcha humana tem como característica ser um sistema que envolve um grande
número de graus de liberdade. Para a realização da modelagem do locomotor bípede,
que é composta pelas pernas e quadril, é preciso fazer uma redução desse número, que
considere apenas os mais importantes para a realização da marcha. Segundo
SAUNDERS et al [15] alguns dos principais movimentos que compõem a marcha são:
compasso, inclinação pélvica, rotação pélvica, flexão do joelho, flexão plantar e
deslocamento lateral da pélvis.
O compasso é caracterizado pelo movimento da pélvis de subida e descida ao
longo do deslocamento. A inclinação pélvica consiste na rotação e translação da pélvis
no plano frontal, aproximando o centro de massa a perna que sustenta o corpo. A
rotação pélvica é o movimento de rotação no plano transversal. A flexão do joelho
ocorre de modo a aumentar o amortecimento das forças geradas pelo solo, assim como a
flexão plantar que também tem uma grande importância no torque para o deslocamento.
Os movimentos descritos são ilustrados nas Figuras 1.15 e 1.16.
14
Figura 1.15: Compasso, Rotação pélvica, inclinação pélvica, flexão do joelho da
perna de apoio [16].
Figura 1.16: Flexão plantar [16].
A marcha ainda possui um movimento lateral da pélvis como ilustrado na Figura
1.17. Devido ao peso ser transferido alternadamente entre um membro a outro e por
existir uma separação dos pés mais curta que o tamanho da pélvis, o corpo balança de
um lado para o outro durante o deslocamento.
15
Figura 1.17: Deslocamento lateral da pélvis [16].
A Figura 1.18 apresenta um modelo tridimensional proposto por DUTRA [17]
com os seis determinantes do modo de andar descritos anteriormente.
Figura 1.18: Modelo tridimensional de um bípede [17].
16
O modelo utilizado neste trabalho foi composto dos principais movimentos da
marcha, que são encontrados no plano sagital. São eles: o compasso, a flexão do joelho
e a flexão plantar. O modelo adotado simplifica o ciclo da marcha reduzindo de um
sistema tridimensional para um sistema bidimensional, diminuindo sua complexidade
resultando na possibilidade de abordar vários tópicos deste problema dando bons
resultados para futuros estudos como será visto em futuros capítulos.
17
2. Cinemática
O estudo cinemático é o principio do conhecimento sobre o problema, sendo o
ponto de partida para a maioria dos projetos de mecanismos, dando base para qualquer
estudo posterior. Na análise cinemática, o estudo se restringe aos aspectos geométricos
da locomoção do corpo, sem se preocupar com as forças produzidas no sistema.
Nas análises usadas na cinemática, os graus de liberdade do sistema são os
primeiros a serem identificados. Relações cinemáticas são desenvolvidas em termo dos
seus graus de liberdade e suas derivadas no tempo. O primeiro passo determina as
localizações e orientações dos corpos no sistema mecânico que é referente à análise de
posição. Neste primeiro passo, todas as variáveis dos movimentos requeridos são
determinadas. O segundo passo da análise cinemática é a análise de velocidades, que é
usado para determinar as respectivas velocidades dos corpos no sistema como função da
razão dos graus de liberdade com o tempo. Isto pode ser obtido diferenciando as
relações cinemáticas obtidas da análise de posição. Uma vez que as posições e
velocidades tenham sido determinadas, o terceiro passo da análise cinemática, que é a
análise das acelerações, pode ser estudado. Nesta análise, as relações de velocidades são
mais uma vez diferenciadas em relação ao tempo para obter as respectivas acelerações
dos corpos do sistema.
Diferentes procedimentos existem para deduzir as expressões de deslocamento do
sistema, as quais futuramente fornecerão as velocidades e acelerações pela derivação
das equações de movimento em função do tempo, tais como as análises geométricas,
próprias para mecanismos simples, e a análise por meio das chamadas matrizes de
rotação e translação, ideal para mecanismos mais complexos.
2.1 Modelo Utilizado
A marcha humana é complexa. Para viabilizar o estudo da marcha de um bípede,
um modelo será definido deixando-o menos complexo. O modelo escolhido para análise
é semelhante ao adotado por DUTRA [17].
18
O modelo adotado se desenvolve em duas dimensões e leva em consideração os
seguintes movimentos da marcha: O compasso, a flexão do joelho da perna de apoio, e a
flexão plantar do pé da perna de apoio, como ilustrado na Figura 2.1.
Figura 2.1: Principais movimentos da marcha considerados no modelo. (1) o compasso,
(4) a flexão do joelho, (5) a flexão plantar do pé [17].
O modelo é formado por quatro pares de elementos: O fêmur, tíbia, pé e dedo. As
pernas são idênticas tanto nas dimensões quanto nas massas. As características adotadas
para o modelo de cada corpo dados por DUTRA [17] são definidas a partir de uma
média de valores obtidos referentes a uma série de pessoas adultas, visto na tabela 2.1.
Neste modelo é considerado que todas as juntas são rotacionais, possuindo um
grau de liberdade em cada junta, e todos os graus de liberdade do mecanismo são
controláveis por meio de atuadores.
19
Tabela 2.1: Características do modelo
Modelo Comprimento (m) Massa (kg) Momento de
inércia (kgm2)
Dedo 0,03 0,2 2,7.10-5
Pé 0,11 0,6 6,0.10-4
Tíbia 0,37 4 0,046
Fêmur 0,37 6 0,07
Cintura 0,1 20 1,07
O comprimento da cintura descrito na Tabela 2.1 se refere ao seu comprimento ao
longo do plano transverso. Os outros comprimentos são descritos no plano sagital.
Para análise cinemática o ciclo da marcha pode ser dividido em duas fases:
simples e duplo suporte (Figura 2.2.).
Figura 2.2: As duas fases encontradas na marcha mostradas no modelo [17].
A colisão é outro fator de importância neste estudo. ZHENG et al [18] descreveu
em seu estudo que cada vez que um pé toca no solo forças adicionais são aplicadas pelo
solo que alteram as velocidades e acelerações dos corpos da perna. Por esta razão,
algumas condições devem ser impostas para evitar impactos no pé que tocará o solo.
Considerando a existência de colisões, o impacto da perna em balanço com o solo é
considerado como sendo inelástico e sem deslizamento. Isto resulta que durante o ciclo
20
da marcha à configuração do robô se mantém inalterada. Assim, os momentos angulares
do bípede sobre o pé de apoio e da perna de apoio sobre o quadril são conservados [19].
Neste modelo, não foram considerados valores relativos ao impacto e atrito nas
análises realizadas. Mesmo sendo o assunto importante, a complexidade deste sistema
torna-o muitas vezes sem solução. Em uma análise mais apurada, tais fenômenos
devem ser levados em consideração.
2.2 Restrições
Sistemas mecânicos podem ser classificados como sistemas mecânicos livres e
sistemas mecânicos fechados. Se os vetores posição e velocidades relativas a um
referencial de um mecanismo podem ser expressos por qualquer valor, o sistema pode
ser chamado de livre. Freqüentemente, sistemas mecânicos contêm restrições, que
restringem o movimento do mecanismo, sendo descritos na literatura como sistemas
mecânicos fechados. As restrições podem ser classificadas como [20]:
• Restrições Geométricas ou restrições cinemáticas.
• Restrições holonômicas ou restrições não-holonômicas.
As restrições geométricas restringem a posição do sistema pela geometria
existente na mesma. As restrições cinemáticas restringem as velocidades do sistema. As
restrições cinemáticas integráveis são equivalentes às restrições geométricas.
Restrições holonômicas são as restrições geométricas e as restrições cinemáticas
integráveis. As restrições não-holonômicas são as restrições cinemáticas não
integráveis.
Sistemas que contem somente restrições holonômicas são chamados de sistema
holonômicos. Se existir pelo menos uma restrição não-holonômica, o sistema será
denominado sistema não-holonômico.
Com as condições impostas no modelo, ou seja, sem impacto e sem
escorregamento, o sistema será considerado holonômico. As características
desconhecidas do terreno têm grande valor na resposta do sistema, mas torna o projeto
21
extremamente complexo e não-holonômico. Assim para esse modelo é considerado um
terreno sempre constante e contínuo.
2.3 Graus de Liberdades e Coordenadas Generalizadas
Sendo um sistema com N corpos incluindo g restrições geométricas e k restrições
cinemáticas. O numero f de graus de liberdade pode ser dado por [20]:
Espacial f = 6N-g-k
Plano f = 3N-g-k
Outro modo de se obter o número de graus de liberdade de um sistema consiste no
seguinte: Considere uma cadeia cinemática espacial arbitraria onde:
• nB é o numero de corpos do sistema.
• nG é o numero de juntas do sistema.
• fGi é o numero de graus de liberdade em cada junta.
O número total de graus de liberdade de uma cadeia cinemática relativo a um
referencial é:
1
6 (6 )Gn
B Gi
i
f n f=
= − −∑
1
6( )Gn
B G Gi
i
f n n f=
= − −∑
Com o numero de loops cinemáticos iguais a:
nL = nG-nB (2.1)
Assim o numero total de graus de liberdade é:
1
6Gn
Gi L
i
f f n=
= −∑ (2.2)
22
Considerando a Equação 2.2, os seguintes casos podem ser analisados:
* f=1 Mobilidade-1 de um mecanismo.
* f=0 Estrutura estaticamente determinada.
* f<0 Estrutura estaticamente indeterminada.
Em alguns casos a Equação 2.2 leva a resultados errados. Por exemplo, em
cadeias cinemáticas planas ou esféricas onde cada corpo tem somente três graus de
liberdade. Nestes casos o numero total de graus de liberdade será dado por:
1
3( )Gn
B G Gi
i
f n n f=
= − −∑
1
3Gn
Gi L
i
f f n=
= −∑ (2.3)
A posição de todos os corpos de um mecanismo com f graus de liberdade é
determinada por f coordenadas independentes, denominadas como as coordenadas
generalizadas. Neste modelo todas as juntas terão um grau de liberdade e todo sistema
pode ser desenvolvido no plano, logo, a equação apropriada para o modelo do robô
bípede estudado será a Equação 2.3.
2.4 Fase de Suporte Simples
Como visto anteriormente, na fase de suporte simples ou balanço, o pé que se
moverá em movimento pendular, perde contato com o solo enquanto o outro se encontra
totalmente fixo ao chão. Esta subfase é chamada de balanço inicial. Na subfase seguinte,
denominada balanço médio, o pé em movimento ultrapassa o que está suportando,
proporcionando o cruzamento da força do solo com a força no centro de gravidade do
corpo. O calcanhar da perna de suporte começa seu movimento de flexão plantar
auxiliando no deslocamento do bípede. Na subfase final, balanço final, o pé em balanço
entra na eminência do contato com o solo.
23
Toda a fase se dá em cadeia aberta, e considerando os movimentos propostos foi
montado o modelo conforme a Figura 2.3.
Como visto na Equação 2.3 serão sete graus de liberdade (GDL), sendo θ1 e θ7 as
articulações do pé, θ2 e θ6 do calcanhar, θ3 e θ5 do joelho e θ4 da cintura. É adotado que
durante todo o movimento, o ângulo θ6 permaneça com aceleração constante, já se
preparando com o futuro toque com o solo, e θ7, com aceleração também constante,
alinhando os dedos com o pé.
Figura 2.3: Modelo da fase de suporte simples.
2.4.1 Cinemática Direta
Para os cálculos da cinemática direta, foi utilizado o método de DENAVIT &
HARTENBERG [21]. Este método consiste na obtenção da localização de corpos
adjacentes através de matrizes de transformação de translação e rotação do respectivo
referencial, através de quatro parâmetros: d, θ, a e α. O parâmetro d representa a
24
translação do eixo x no eixo z, enquanto que o parâmetro a representa a translação do
eixo z no eixo x. O ângulo θ é a rotação do eixo x sobre o eixo z, e α a rotação do eixo z
relativo ao eixo x. Desta forma pode-se escrever a matriz de transformação por
DENAVIT & HARTENBERG [21] da seguinte forma:
),().,().,().,( 11111 αθ xRxaTzRzdTH n
n
n
n
nn
n
n
n
n
n
−−−−− =
Onde T representa as matrizes de translação e R as matrizes de rotação. Assim:
1
cos( ) ( ) cos( ) ( ) ( ) cos( )
( ) cos( )cos( ) cos( ) ( ) ( )
0 ( ) cos( )
0 0 0 1
n n n n
n n n nn
n
sen sen sen a
sen sen asenH
sen d
θ θ α θ α θ
θ θ α θ α θ
α α−
− − ==
(2.4)
Este modelo trabalha em duas dimensões resultando em todos os parâmetros α e d,
iguais à zero, considerando que não há rotação do eixo z, e translação do eixo x em
relação ao eixo z. O parâmetro a, que representa a translação do eixo z sobre o eixo x
receberá o valor do comprimento de cada corpo da perna referente à transformação que
estiver sendo feita, sendo lf igual ao comprimento do fêmur, lt comprimento da tíbia, lp o
comprimento do pé e ld o comprimento do dedo. Os parâmetros θ, que representam a
rotação do eixo x sobre o eixo z, serão as coordenadas generalizadas do sistema. Assim
resolvendo a Equação 2.4 para os parâmetros do bípede, tem-se:
1
cos( ) ( ) 0 cos( ).
( ) cos( ) 0 ( ).
0 0 1 0
0 0 0 1
n n n n
n n n nn
n
sen a
sen sen aH
θ θ θ
θ θ θ−
− =
Todos os pontos das articulações mostrados na Figura 2.4 podem ser encontrados
considerando a Equação 2.5.
=
1
0
0
0
1
0Hz
y
x
n
n
n
n
(2.5)
25
01
12
21
10 ...... HHHHH n
n
n
nn
−−
−= 1, 2,...,7n =
Figura 2.4: Posição das coordenadas das juntas.
Visto isso, as posições das juntas podem ser encontradas, onde:
[ ] [ ]101 0 0 0 1
T T
cal cal cal calP x y z H= =
[ ]20 0 0 0 1
T
joeP H=
[ ]30 0 0 0 1
T
cinP H=
[ ]40 0 0 0 1
T
joBP H=
[ ]50 0 0 0 1
T
cBP H=
[ ]60 0 0 0 1
T
pBP H=
26
[ ]70 0 0 0 1
T
dBP H=
Onde, cal denota calcanhar, joe, joelho, cin cintura, joB joelho em balanço, cB
calcanhar em balanço, pB pé em balanço e dB dedo em balanço.
Considerando que todo movimento se desenvolve em duas dimensões, não há
variação em torno de z, logo, as equações de posição das articulações utilizando a
Equação 2.5 são:
1
1
cos( );
( );
cal p
cal p
x l
y l sen
θ
θ
=
= (2.6)
1 2 1
1 2 1
cos( ) cos( );
( ) ( );
joe t p
joe t p
x l l
y l sen l sen
θ θ θ
θ θ θ
= − +
= − + (2.7)
1 2 3 1 2 1
1 2 3 1 2 1
cos( ) cos( ) cos( );
( ) ( ) ( );cin f t p
cin f t p
x l l l
y l sen l sen l sen
θ θ θ θ θ θ
θ θ θ θ θ θ
= − + + − +
= − + + − + (2.8)
1 2 3 4 1 2 3 1 2 1
1 2 3 4 1 2 3 1 2 1
cos( ) cos( ) cos( ) cos( );
( ) ( ) ( ) ( );joB f f t p
joB f f t p
x l l l l
y l sen l sen l sen l sen
θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ
= − + − + − + + − +
= − + − + − + + − + (2.9)
1 2 3 4 5 1 2 3 4
1 2 3 1 2 1
1 2 3 4 5 1 2 3 4
1 2 3 1 2 1
cos( ) cos( )
cos( ) cos( ) cos( );
( ) ( )
( ) ( ) ( );
cB t f
f t p
cB t f
f t p
x l l
l l l
y l sen l sen
l sen l sen l sen
θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ
= − + − − + − + − +
− + + − +
= − + − − + − + − +
− + + − +
(2.10)
2.4.2 Velocidades e Acelerações dos pontos nas juntas
Uma vez obtida as equações de posição das articulações, as equações de
velocidade e aceleração podem ser encontradas derivando-as em relação ao tempo.
Derivando as Equações 2.8 e 2.10 são encontradas as velocidades da articulação da
cintura e do calcanhar em balanço. Qualquer outra velocidade pode ser obtida seguindo
o mesmo procedimento.
27
1 2 3 1 2 3 1 2 1 2 1 1
1 2 3 1 2 3 1 2 1 2 1 1
( ) ( ) ( ) ( ) ( );
( ) cos( ) ( ) cos( ) cos( );
cin f t p
cin f t p
x l sen l sen l sen
y l l l
θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ θ θ
= − − + − + − − − −
= − + − + + − − +
& & & & & &&
& & & & & && (2.11)
1 2 3 4 5 1 2 3 4 5
1 2 3 4 1 2 3 4
1 2 3 1 2 3 1 2 1 2 1 1
1 2 3 4 5 1 2 3 4 5
( ) ( )
( ) ( )
( ) ( ) ( ) ( ) ( );
( ) ( )
(
cB t
f
f t p
cB t
f
x l sen
l sen
l sen l sen l sen
y l cos
l
θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ
= − − + − − − + − − −
− + − − + − −
− − − − − − − −
= − + − − − + − − +
& & & & &&
& & & &
& & & & & &
& & & & &&
1 2 3 4 1 2 3 4 1 2 3 1 2 3
1 2 1 2 1 1
) cos( ) ( ) cos( )
( ) cos( ) cos( );
f
t p
l
l l
θ θ θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ
− + − − + − + − − − − +
− − +
& & & & & & &
& & &
(2.12)
Derivando-as uma segunda vez em relação ao tempo, podem-se encontrar as
equações de aceleração. Derivando as Equações 2.11 e 2.12 as acelerações da
coordenada da cintura e da coordenada do calcanhar em balanço ao longo do
movimento são encontradas.
Mais uma vez, a aceleração de qualquer articulação pode ser encontrada seguindo
o mesmo procedimento.
2 2 21 2 3 1 2 3 1 2 3 1 2 3
2 2 21 2 1 2 1 2 1 2 1 1 1 1
2 21 2 3 1 2 3 1 2
( ) ( ) ( ) cos( )
( ) ( ) ( ) cos( ) ( ) cos( );
( ) cos( ) (
cin f f
t t p p
cin f
x l sen l
l sen l l sen l
y l
θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ
= − − + − + − − + − + −
− − − − − − −
= − + − + − − +
&& && && & & &&&
&& && & & && &
&& && && & & &&&23 1 2 3
2 2 21 2 1 2 1 2 1 2 1 1 1 1
) ( )
( ) cos( ) ( ) ( ) cos( ) ( );
f
t t p p
l sen
l l sen l l sen
θ θ θ
θ θ θ θ θ θ θ θ θ θ θ θ
− + +
− − − − − + −&& && & & && &
(2.13)
28
1 2 3 4 5 1 2 3 4 5
2 2 2 2 21 2 3 4 5 1 2 3 4 5 1 2 3 4
2 2 2 21 2 3 4 1 2 3 4 1 2 3 4
1 2 3
( ) ( )
( ) cos( ) ( )
( ) ( ) cos( )
( )
cB t
t f
f
x l sen
l l
sen l
θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ
= − − + − − − + − − −
− + − − − + − − − − + −
− + − − − + − − + − −
− −
&& && && && &&&&
& & & & & && && && && �
& & & &
&& && && 2 2 21 2 3 1 2 3 1 2 3
2 2 21 2 1 2 1 2 1 2 1 1 1 1
1 2 3 4 5 1 2 3 4 5
2 2 2 21 2 3 4
( ) ( ) cos( )
( ) ( ) ( ) cos( ) ( ) cos( );
( ) ( )
(
f f
t t p p
cB t
t
l sen l
l sen l l sen l
y l cos
l
θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ
θ θ θ θ
− − − − + − − −
− − − − − − −
= − + − − − + − − −
− + −
& & &
&& && & & && &
&& && && && &&&&
& & & & 25 1 2 3 4 5 1 2 3 4
2 2 2 21 2 3 4 1 2 3 4 1 2 3 4
2 2 21 2 3 1 2 3 1 2 3 1 2 3
1 2 1 2
) ( ) ( )
cos( ) ( ) ( )
( ) cos( ) ( ) ( )
( ) cos( ) (
f
f
f f
t t
sen l
l sen
l l sen
l l
θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ
− − + − − + − + −
− + − − − + − − + − +
− + − + − − + − + +
− − −
& && && && && �
& & & &
&& && && & & &
&& && &2 2 21 2 1 2 1 1 1 1) ( ) cos( ) ( );p psen l l senθ θ θ θ θ θ θ− − + −& && &
(2.14)
2.4.3 Cinemática Inversa
O propósito da análise da cinemática inversa é equacionar as coordenadas
generalizadas em função de algum ponto já conhecido no movimento. Esta análise será
muito útil em futuros estudos como a dinâmica ou controle. A solução deste problema é
de fundamental importância, pois transforma deslocamentos e orientações especificados
atribuídos a algum ponto do sistema, nos correspondentes movimentos das juntas no
espaço que permitem executar o movimento desejado [22].
Nas equações da cinemática direta, a posição final desejada e a matriz de rotação
podem ser calculadas de uma única maneira, uma vez que as variáveis das juntas são
conhecidas. Entretanto, a cinemática inversa é muito mais complexa pelas seguintes
razões [22]:
● As equações para solucionar o problema são em geral não lineares, e assim não
é sempre possível encontrar uma solução em cadeia fechada.
● Múltiplas soluções podem existir.
● Pode não haver soluções admissíveis, de acordo com a estrutura do sistema.
As soluções do sistema em cadeia fechada podem ser encontradas tanto com uma
abordagem algébricas quanto para soluções geométricas. Nos casos onde é complexa a
29
solução do sistema em cadeia fechada, é mais apropriado abordar as técnicas de
soluções numéricas. Essa técnica tem a clara vantagem de ser aplicada a qualquer
estrutura, contudo, em geral, elas não permitem a computação de todas as soluções
admissíveis [22].
Assim que um pé deixa o chão, a posição do mesmo não terá uma influência
relevante na cinemática do processo, pois a posição do bípede no recontato da perna em
balanço com o solo pode ser encontrado pelo ponto do calcanhar. Entretanto, no
momento do recontato com o solo, o ângulo θ6 se posicionará em torno de noventa
graus de modo que o calcanhar sofra o impacto com o re-contato. A articulação do dedo
também não terá influência alguma no pé em balanço, conseqüentemente é preferível
considerar para este ponto do estudo, o ângulo θ6 igual a noventa graus e θ7 igual a zero
em toda a trajetória.
Deste modo, considerando o ângulo θ6 e θ7 constante ao longo de todo trajetória e
adotando que é conhecida a coordenada do calcanhar em balanço. O modelo, antes em
cadeia aberta, pode ser considerado agora como cadeia fechada conforme a Figura 2.5.
Figura 2.5: cadeia cinemática fechada.
30
Em cadeia fechada, o número de GDL pode ser determinado pela Equação 2.3.
Neste modelo, fG é igual a seis, nG igual a seis e nB igual a cinco. Visto isso, têm-se três
GDL. Deste modo considerando que os ângulos θ3, θ4 e θ5 serão as coordenadas
generalizadas, os ângulos indeterminados estarão em função destes três ângulos. Estes
ângulos foram escolhidos considerando que suas trajetórias, velocidades e acelerações
já estarão pré-determinadas.
As equações de posição serão determinadas a partir do método dos pares
característicos de HILLER & WOERNLE [23]. Este método consiste em dividir a
cadeia em duas partes ligando duas articulações e as liberando como visto na Figura 2.6.
Assim as equações de uma divisão serão igualadas a sua outra parte.
Figura 2.6: Método dos pares característicos.
Liberando os ângulos θ2 e θ6 pode-se obter θ1 em função dos ângulos conhecidos.
Através de trigonometria, utilizando as leis de seno e cosseno, segundo a Figura 2.7 foi
encontrado:
31
42
θα = (2.15)
2 2 21 12 cos( )f f fl l d l d α= + −
41 2 cos 2f
d lθ =
(2.16)
Figura 2.7: Decomposição da cadeia fechada.
Descobertos os valores de α e d1, o valor de d3 pode ser encontrado considerando um
mecanismo de quatro barras conforme a Figura 2.8.
Seja:
1 3 5cos( ) cos( ( ))t t
X d l lα θ π α θ= + − − − + (2.17)
32
3 5( ) ( ( ))t t
Y l sen l senα θ π α θ= − − − + (2.18)
Figura 2.8: Novas decomposições da cadeia fechada.
Utilizando as Equações 2.17 e 2.18, tem-se:
2 2 23d X Y= + (2.19)
O comprimento d3 em função do ângulo θ1 pode ser encontrado através da
seguinte equação.
23 6 1 6 1( ).( )d r r r r= − − (2.20)
Onde
33
6
6 6
0
x
r y
=
(2.21)
1
1 1
cos( )
( )
0
p
p
l
r l sen
θ
θ
=
(2.22)
Resolvendo a Equação 2.20, substituindo pelas Equações 2.21 e 2.22.
6 1 6 1cos( ) ( )x y sen Aθ θ ′+ = (2.23)
Para
2 2 2 26 6 3
2p
p
x y l dA
l
+ + −′ = (2.24)
Para y6 diferente de zero, tem-se:
6
AA
y
′= e 6
6
xB
y= (2.25)
Utilizando a Equação 2.24 e substituindo quando cabível pela Equação 2.25,
pode-se encontrar a equação do ângulo θ1.
2
1 2
1arccos
2 1
A B
B Bθ
+ −=
+ − (2.26)
Para y6 igual a zero, o pé se encontrará no chão, e neste caso a fase será de suporte
duplo, sendo visto mais adiante. Conhecendo θ1 pode-se encontrar θ2 utilizando também
os cálculos trigonométricos de acordo com a Figura 2.9.
34
Figura 2.9: Configuração geométrica para achar θ2.
Como visto na Figura 2.9, tem-se:
22 23 6
3
arccos2
p
p
d l r
d lβ
+ − =
(2.27)
2 2 22 1 1 52 ( ( ))
t td l d l d cos π α θ= + − − + (2.28)
2 2 23 2
3
arccos2
t
t
l d d
l dη
+ −=
(2.29)
Assim o ângulo θ2 pode ser equacionado da seguinte forma:
2 180oθ η α= − − (2.30)
35
2.4.4 Velocidades e Acelerações dos Ângulos das Juntas
Considerando:
2
2
1
2 1
A Bu
B B
+ −=
+ − (2.31)
Substituindo a Equação 2.31 na Equação 2.26, tem-se:
( )1 arccos uθ = (2.32)
Deste modo, derivando a Equação 2.32 e a Equação 2.30 duas vezes pode-se
encontrar as velocidades e acelerações dos ângulos θ1 e θ2 respectivamente, onde:
1 2
1
1u
uθ
−=
−
& & (2.33)
( )2
1 3 22
1 1
11uu u
uu
θ− −
= −−−
&& & && (2.34)
2θ η α= − −& & & (2.35)
2θ η α= − −&& && && (2.36)
2.5 FASE DE SUPORTE DUPLO
A fase de suporte se caracteriza pela presença dos dois pés encostados no chão ao
longo de toda a respectiva fase. Quando o calcanhar toca o solo, o pé começa a se
adequar ao solo transferindo as cargas para outras articulações, principalmente a do
joelho. Esta é a subfase de contato inicial e aceitação de carga citada no capítulo 1. O pé
antes responsável pelo suporte começa a produzir o torque empurrando o corpo para
frente se preparando para executar o movimento de balanço [14].
36
Segundo a Equação 2.3 têm-se mais uma vez três GDL. Considerando o
conhecimento prévio mais uma vez dos ângulos θ3, θ4 e θ5, e o ponto do calcanhar que é
igual passo do bípede pode-se usar o mesmo método dos pares característicos de
HILLER & WOERNLE [23] para encontrar os ângulos de interesse θ2, θ6 e θ7. O ciclo
da marcha começa com a fase de suporte duplo, por isso o ângulo θ1 terá o valor de π
durante toda a fase. A configuração da fase de suporte duplo é mostrada na Figura 2.10.
Figura 2.10: Modelo de suporte duplo.
O desenvolvimento das equações dos ângulos das juntas se desenvolverá de forma
semelhante à utilizada na cinemática inversa da cadeia aberta. Utilizando a Figura 2.11,
serão encontradas as seguintes equações:
42
θα π= − (2.37)
41 2 cos 2f
d lθπ = −
(2.38)
37
Figura 2.11: Decomposição do suporte duplo.
Figura 2.12: Nova decomposição do suporte duplo.
38
De acordo com a Figura 2.12, a variável d3 pode ser encontrada, sabendo que:
2 2 23d X Y= + (2.39)
Onde
5 3( ) ( ( ))t t
Y l sen l senα θ π α θ= − − − + (2.40)
1 5 3cos( ) cos( ( ))t t
X d l lα θ π α θ= + − − − + (2.41)
Encontrado d3 e desenvolvendo o mesmo em função do ângulo θ1, pode-se
encontrar a seguinte equação:
2 2 23
2p
p
P l dA
l P
+ −′ = (2.42)
7cos( ) Aθ ′= (2.43)
Assim o ângulo θ7 terá a seguinte equação:
( )7 arccos Aθ π ′= − (2.44)
A variável θ6 será calculada na fase de suporte duplo da mesma forma que da
calculada na cinemática inversa da fase de suporte simples. Assim:
2 2 23
3
arccos2
p
p
d l P
d lβ
+ −=
(2.45)
2 2 22 1 1 32 ( ( ))
t td l d l d cos π α θ= + − − + (2.46)
2 2 23 2
3
arccos2
t
t
l d d
l dη
+ −=
(2.47)
6θ π η β= − − (2.48)
39
Considerando o modelo como um polígono de seis lados, a soma dos ângulos
internos será de 7200. Visto isso, o ângulo θ2 poderá ser equacionado da seguinte forma:
2 3 6 7 4 5( )θ π θ θ θ θ θ= + + + − + (2.49)
A geometria mostrada nas figuras do modelo mostra várias restrições que são
impostas ao sistema quando o mesmo se encontra em cadeia fechada. Uma das mais
importantes é o tamanho do passo.
Primeiramente, o tamanho do passo terá influencia direta no tamanho inicial do
ângulo da cintura θ4, de modo que o bípede se movimente o mais harmonicamente
possível. Seguindo a lei dos cossenos, pode-se dizer que o tamanho do passo, sabendo
que a variável d3 é função das coordenadas generalizadas, têm um tamanho restrito de:
2 2 23 32p pP d l d l> + −
3( )p
P d l> − (2.50)
Onde P é igual ao valor do passo menos o valor do comprimento do pé. Assim:
3s p pP l d l− > −
3sP d> (2.51)
Se o passo for maior que a variável d3 significa que o robô bípede não conseguirá
encostar um pé se não recolher o outro do solo. Quando isto acontece, a caminhada é
descaracterizada e o bípede entra em outro estágio de movimento que pode ser chamado
de corrida. Quando se está correndo, não há fase de suporte duplo e existe um tempo
onde as duas pernas estarão fora de contato com o solo, que é chamado de balanço
duplo.
Outra restrição importante é que o tamanho do segmento de reta d3 deve ser
sempre decrescente no tempo ao longo do ciclo, pois sendo:
2 2 23
7cos( )2 2
p
p p
P l d
l P l Pθ
+= − (2.52)
40
Com o aumento de d3 ocorre uma diminuição do ângulo θ7 resultando em um
movimento retrogrado do ângulo do pé.
2.5.1 Velocidades e Acelerações dos Ângulos das Juntas
Seguindo o mesmo processo utilizado na fase de balanço, podem-se encontrar as
velocidades e acelerações dos ângulos requeridos. Considerando:
u A′=
Substituindo na Equação 2.44, e derivando as Equações 2.44, 2.48 e 2.49 uma
vez, acham-se as equações das velocidades ângulos θ7, θ6 e θ2 respectivamente e
derivando uma segunda vez, as acelerações dos mesmos são encontrados. Assim, tem-
se:
7 2
1
1u
uθ =
−
& & (2.53)
( )2
7 3 22
1 1
11uu u
uu
θ = −−−
&& & && (2.54)
A velocidade e a aceleração do ângulo θ6 tem a seguinte forma:
6θ η β= − −& &&& (2.55)
6θ η β= − −&& &&&& (2.56)
Conhecido as velocidades e acelerações das outras juntas, tem-se para θ2 as
seguintes funções para a velocidade e aceleração:
2 3 6 7 4 5( )θ θ θ θ θ θ= + + − +& & & & & & (2.57)
2 3 6 7 4 5( )θ θ θ θ θ θ= + + − +&& && && && && && (2.58)
As equações obtidas serão muito úteis em estudos futuros como a dinâmica. As
equações de velocidades e posições, por exemplo, serão muito utilizadas para a se obter
41
as equações de energia cinética e potencial, requisitadas na equação de Lagrange a fim
de se conhecer as equações de movimento.
42
3. Funções dos Ângulos das Juntas no Tempo
Em 1984 BRAUNE & FISCHER [24] publicaram um livro sobre a
movimentação humana, cujo conteúdo continha uma detalhada investigação sobre a
marcha humana. Nestes experimentos, membros e outros pontos importantes do corpo
foram marcados e fotografados em curtos intervalos de tempo ao longo de um ciclo da
marcha em diferentes posições. Foram investigados entre outros pontos, o pé, a coxa,
parte inferior da perna, braços, dorso, ombros e a cabeça. Desta pesquisa, o curso dos
ângulos das juntas pôde ser determinado, onde variação dos resultados pode ser
encontrada dependendo do tipo de teste, do tamanho do passo ou da carga.
Figura 3.1: Traje utilizado nos experimentos de BRAUNE & FISCHER [24].
43
As Figuras 3.1 e 3.2 apresentam o experimento de BRAUNE &e FISCHER [24]
em uma movimentação da marcha humana. Os comprimentos da coxa e parte inferior da
perna se encontram entorno de 0,37m. O tamanho da passada durante o teste é de
0,73m.
Figura 3.2: Pontos obtidos no movimento da marcha no teste (foto original de
BRAUNE & FISCHER [24] em 1894).
A Figura 3.3 mostra os ângulos θ1, θ3, θ9, θ12, e θ14, que foram inferidas pela
fotografia de BRAUNE & FISCHER [24] por DUTRA [17]. Os respectivos seis
ângulos são mostrados na Figura 3.4.
44
Figura 3.3: Medida dos ângulos de DUTRA [17] em função do tempo τ=2πt/T.
Figura 3.4: ângulos de DUTRA [17].
45
Em τ =0 os ângulos correspondem ao inicio da fase de suporte duplo. O tempo
τ=2π corresponde ao final do ciclo completo da marcha.
3.1 Análise Harmônica
Conhecido os pontos do movimento da marcha ao longo do tempo pelo
experimento de BRAUNE & FISHER [24], podem-se encontrar as funções dos ângulos
das juntas através da transformada rápida de Fourier. A transformada de Fourier
expressa uma função em termos de funções de base sinusoidais, como soma de funções
sinusoidais multiplicados por coeficientes expressos pelas amplitudes. A transformada
rápida de Fourier, conhecido como FFT (Fast Fourier Transform) é um algoritmo
eficiente para se calcular a transformada de Fourier [25]. Sendo o movimento humano,
um movimento periódico, os ângulos de processo podem ser mais facilmente
representados por uma série de Fourier, sendo f(t) uma função repetitiva de período T.
Assim:
( ) ( )f t f t T= +
( )0
1
( ) cos( ) ( )2 k k
k
af t a kwt b sen kwt
∞
=
= + +∑ (3.1)
Sendo
2w
T
π=
0
1( )a f t dt
π
ππ −= ∫
1( ) cos( )ka f t kwt dt
π
ππ −= ∫ k=0, 1, 2,...
1( ) ( )
kb f t sen kwt dt
π
ππ −= ∫ k=0, 1, 2,...
46
DUTRA [17] submeteu os dados à análise da transformada rápida de Fourier, e
particularmente os ângulos dos joelhos e da cintura, e pode determinar as funções dos
respectivos ângulos muito próximos das funções buscadas. Assim:
Para 0≤wt≤π
3 30 3 cos(2 )wtθ θ θ= +
9 90 9 cos( )wtθ θ θ= +
12 120 12 cos(2 )wtθ θ θ= +
Para π ≤ wt ≤ 2π
3 120 12 cos(2 )wtθ θ θ= − −
9 90 9 cos( )wtθ θ θ= +
12 30 3 cos(2 )wtθ θ θ= − −
Na primeira metade do ciclo o ângulo θ12 é do joelho que fará a função de suporte
para o corpo, enquanto o ângulo θ3 pertence ao joelho em balanço. Na segunda metade
o ângulo θ12 pertencerá à perna em balanço e θ3 a perna em suporte. Por este motivo as
funções se mostram contrarias em cada meio ciclo.
Para os demais ângulos, várias funções harmônicas são necessárias para
composição de sua função. Futuramente estas funções serão encontradas mais não com
a análise de Fourier.
Na Figura 3.5 as médias dos valores de θ3, θ9 e θ12 (θ3m, θ9m, θ12m) e suas
aproximações são representadas por θ3h, θ9h e θ12h encontrados por DUTRA [17].
47
Figura 3.5: Medidas dos ângulos de DUTRA [17] e suas aproximações por função
harmônica.
Assim, as aproximações harmônicas forneceram seguintes coeficientes:
30
120
90
32
13
0
o
o
o
θ
θ
θ
=
= −
=
3
12
09
29
10
54
o
o
θ
θ
θ
= −
=
=
De modo a evitar singularidade de posição (stretched legs) nos joelhos, os
menores ângulos dos mesmos são determinados para θ3 e θ12 com 3° para qualquer valor
de passo. Esse valor foi considerado por ser um ângulo muito encontrado na marcha
humana [17]. O valor da constante θ90 = 0° foi designado para manter a simetria do
movimento.
3.2 Análise das Funções dos Ângulos no Tempo
3.2.1 Análise das Coordenadas Generalizadas em Cadeia Fechada
Como ilustrado na Figura 3.6, alguns ângulos estudados neste trabalho divergem
dos estudados por DUTRA [17], contudo as afirmações descritas acima cabem
perfeitamente para se obter as funções no tempo da cintura e do joelho do bípede.
48
Figura 3.6: Ângulos das juntas
Ao longo do período do ciclo da marcha, três pontos no tempo são importantes
para análise da marcha. Em t0, o bípede se encontra com os dois pés no solo,
imediatamente após o contato do pé da perna que estava em balanço como visto na
Figura 3.7a. Este é o início do ciclo, assim, como a fase de suporte duplo. No tempo t1,
ilustrado na Figura 3.7b, a fase de suporte duplo se finaliza com a perna traseira prestes
a entrar em balanço. Por definição, a fase de suporte duplo dura aproximadamente 20%
do período de um ciclo [11]. Em t2, o centro de massa do bípede ultrapassa a junta do
pé, onde por definição, é onde ocorrem as forças de reação do solo, como visto na
Figura 3.7c. No tempo T/2, o membro antes em balanço, encosta no solo e trocará sua
função, tornando-se o membro que irá exercer a função de suporte, ilustrado na Figura
3.7d.
49
(a) (b)
(c) (d)
Figura 3.7: Pontos importantes da locomoção ao longo do tempo.
Os ângulos da cintura e dos joelhos serão obtidos segundo a análise da
transformada rápida de Fourier, como já descrito por DUTRA [17], obtendo a seguinte
função:
Para 0≤wt≤T/2
3 30 3 cos(2 )wtθ θ θ= + (3.2)
4 40 4 cos( )wtθ θ θ= + (3.3)
5 50 5 cos(2 )wtθ θ θ= + (3.4)
50
Como o sentido do vetor posição dos ângulos está desenvolvido nas equações
cinemáticas de posição, as funções no tempo serão sempre positivas. Na segunda
medade do ciclo, o joelho em balanço fará a função de suporte, e o joelho em suporte
fará a função de balanço, resultando na inversão das funções de posição. Assim:
Para T/2 ≤ wt ≤ T
3 50 5 cos(2 )wtθ θ θ= + (3.5)
5 30 3 cos(2 )wtθ θ θ= + (3.6)
4 40 4 cos( )wtθ θ θ= + (3.7)
Suas velocidades e acelerações podem ser encontradas derivando as suas equações
de posição. Assim tem-se:
Para 0≤wt≤T/2
3 32 (2 )w sen wtθ θ= −& (3.8)
23 34 cos(2 )w wtθ θ= −&&
(3.9)
4 4 ( )w sen wtθ θ= −& (3.10)
24 4 cos( )w wtθ θ= −&&
(3.11)
5 52 (2 )w sen wtθ θ= −& (3.12)
25 54 cos(2 )w wtθ θ= −&&
(3.13)
Para T/2 ≤ wt ≤ T
3 52 (2 )w sen wtθ θ= −& (3.14)
23 54 cos(2 )w wtθ θ= −&&
(3.15)
51
4 4 ( )w sen wtθ θ= −& (3.16)
24 4 cos( )w wtθ θ= −&&
(3.17)
5 32 (2 )w sen wtθ θ= −& (3.18)
25 34 cos(2 )w wtθ θ= −&&
(3.19)
As constantes das funções serão encontradas dependendo das condições
submetidas ao sistema, ou seja, do valor do passo desejado, e das restrições,
principalmente na fase de suporte duplo.
Os ângulos iniciais das juntas dos joelhos serão sempre iguais a três graus. Isto
implica que desenvolvendo as Equações 3.4 e 3.5 tem-se:
30 3 3oθ θ+ =
50 5 3oθ θ+ =
Assim as constantes da função do ângulo da cintura θ4 podem ser determinadas.
A constante θ40 terá o valor de π de modo a manter a simetria do sistema entre o
ângulo da cintura no inicio e ao final de meio ciclo, onde a configuração do bípede é a
mesma, mas com a troca de posição dos membros. Sabendo os ângulos iniciais do
bípede a constante 4θ pode ser determinada em função do passo desejado.
O ciclo começa em cadeia fechada, como visto anteriormente. Três coordenadas
generalizadas devem ser fornecidas para se obter a posição de todas as coordenadas.
Considerando que para cada valor de passo dado pelo bíbede, um valor ótimo do
ângulo θ7 existe para se obter uma velocidade da cintura constante, foi considerando
que em todo começo de ciclo o ângulo θ7 terá o valor de 20o dado pela media obtida
por esses valores ótimos encontrados. Assim o ângulo inicial de θ4 pode ser
determinado.
52
Figura 3.8: Configuração da cadeia para encontrar θ4 inicial.
Seguindo mais uma vez o método dos pares característicos como mostrado na
Figura 3.8, o segmento de reta d3 pode ser determinado em função de θ4, e em função
do passo. Conforme a Figura 3.9, tem-se:
2 cos( )fd l α= (3.20)
4
2
θα π= − (3.21)
53
Figura 3.9: Desenho geométrico do bípede para encontrar θ4 inicial.
Considerando mais uma vez a geometria como um mecanismo de quatro barras,
pode-se dizer que:
5 3( ( ) ( ))tY l sen senα θ α θ= − − + (3.22)
5 3(cos( ) cos( ))tX d l α θ α θ= + − + + (3.23)
Como θ3 inicial é igual a θ5 inicial igual a 3o, resolvendo e substituindo a
Equação 3.20 na Equação 3.23:
52 ( )cos( )tY l sen θ α= − (3.24)
5(2 2 cos( )) cos( )f tX l l θ α= + (3.25)
54
Sendo:
52 ( )tA l sen θ= − e 5(2 2 cos( ))f tB l l θ= +
E substituindo nas Equações 3.24 e 3.25, tem-se:
cos( )
cos( )
Y A
X B
α
α
=
=
Assim:
2 2 2 2 23 cos ( ) cos ( )d A Bα α= +
3
2 2cos( )
d
A Bα =
+ (3.26)
E ao mesmo tempo:
2 2 23 72 cos( )p pd l p l p θ= + −
Resolvendo a Equação 3.26, substituindo a Equação 3.21 dentro da mesma
pode-se encontrar o ângulo θ4 inicial. Assim tem-se:
34 2 2
2 cosd
arcA B
θ
= − +
A constante restante da equação do ângulo da cintura então pode ser
determinada para um passo proposto, tendo o seguinte formato.
( )27
4 2 2
2 cos( )2 cos
p pp p l larc
A B
θθ π
+ − = − − +
(3.27)
As constantes das funções dos ângulos dos joelhos também serão variáveis em
função do passo. Considerando que a velocidade do centro de massa da cintura se
manterá constante o máximo possível para diminuir a demanda de energia. E que a
posição da cintura é função apenas dos ângulos do pé θ1, do calcanhar θ2 e do joelho
55
θ3, as constantes dos ângulos dos joelhos θ3 e θ5 serão determinadas numericamente de
modo que quando o ciclo de cadeia fechada termine, o ângulo θ2 tenha o valor
determinado se o ponto da cintura tivesse velocidade constante ao longo de todo ciclo.
Assim, na fase de cadeia abeta, a função do ângulo θ2 será desenvolvida para a
velocidade da cintura se manter constante.
Para um período de 2s e um passo igual a 0,40m tem-se a função das três
coordenadas generalizadas em cadeia fechada ilustrado na Figura 3.10.
Figura 3.10: Função da posição das três coordenadas generalizadas no tempo.
3.2.2 Análise das Coordenadas Dependentes em Cadeia Fechada
Como dito, o ciclo começa em cadeia fechada. Os três ângulos já determinados
foram escolhidos para serem as coordenadas generalizadas do sistema. Logo os quatro
ângulos restantes na cadeia serão funções dos ângulos independentes na fase de
suporte duplo.
56
O ângulo entre o pé em suporte com o dedo, o ângulo θ1, terá o valor de π
durante toda fase de cadeia fechada e na parte inicial da fase de cadeia aberta. Quando
o centro de massa do bípede ultrapassar o pé em suporte, ou seja, no tempo t2, o
calcanhar vai começar a levantar e θ1 começar a diminuir. O ângulo final será o
complemento do ângulo θ7 inicial, já que na segunda parte do ciclo os objetivos se
inverterão. Como na fase de balanço ele é independente, uma função de segundo grau
será atribuída ao ângulo θ1 de modo a ele se movimentar de forma mais suave,
facilitando o exercício dos atuadores e gastando menos energia possível. Sendo:
02
1 70
( )
( )
p
x
f
l xt
v
θ π θ
− −=
= −
Onde θ1f é o ângulo final de θ1. As funções de posição, velocidade e aceleração
podem ser escritas da seguinte forma:
21 1 2 3a a t a tθ = + + (3.28)
1 2 32a a tθ = +& (3.29)
1 32aθ =&& (3.30)
A função de posição foi desenvolvida para a posição de θ1 ser continua. Para se
construir a função três pontos são requeridos. A velocidade do ângulo do pé com o
dedo quando o pé atinge o solo será zero, se mantendo neste valor até que o centro de
massa volte a ultrapassar o pé em suporte no tempo t2. Assim têm-se os seguintes
pontos.
1 2
1 1
1
( )
( / 2)
( / 2) 0
f
t
T
T
θ π
θ θ
θ
=
=
=&
Desenvolvendo as equações de posição, velocidade e aceleração, as constantes
das Equações 3.28, 3.29 e 3.30, foram encontradas tendo os seguintes valores:
57
22
1 2 2
1 22
2 2
12 2
22 2
13 2
22 2
( )4
4( )
4( )
4
f
f
f
Tt t T
aT
t t T
Ta
Tt t T
aT
t t T
πθ
π θ
π θ
− +=
− +
− −=
− +
−=
− +
Para um período de 2s e um passo igual a 0,40m o ângulo θ1 em função do
tempo é mostrado na Figura 3.11 em um tempo de meio ciclo.
Figura 3.11: Função de posição do ângulo θ1 no tempo.
Todo o ciclo da marcha é proposto de modo que a velocidade da cintura no
eixo x seja o mais constante possível, pois, como visto, a velocidade constante da
cintura proporciona um menor consumo de energia. Assim tem-se:
2s
Px
T=&
58
02
sP
x x tT
= + (3.31)
Conhecido os ângulos iniciais das coordenadas generalizadas e o passo desejado,
todos os pontos iniciais do sistema já podem ser encontrados, assim como todos os
ângulos das juntas. A Figura 3.12 mostra a posição inicial da cintura (x0, y0).
Figura 3.12: Posição inicial do ciclo.
A posição inicial da cintura (x0, y0), função dos ângulos θ1, θ2 e θ3 pode ser
escrita como:
0 1 1 2 1 2 3cos( ) cos( ) cos( )p i t i i f i i ix l l lθ θ θ θ θ θ= + − + − +
Onde θ1i, θ2i e θ3i representam os valores iniciais dos ângulos do pé, calcanhar e
joelho. Conhecido a posição inicial da cintura, a velocidade da cintura pode ser
encontrada pela Equação 3.31.
59
Durante a fase de suporte duplo o ângulo θ2 será função das coordenadas
generalizadas. As equações de posição, velocidade e aceleração são as encontradas no
capitulo 2, nas Equações 2.49, 2.57 e 2.58.
Quando a fase de balanço começa o ângulo θ2 é equacionado de modo que a
velocidade da cintura seja constante. Considerando que a posição da cintura pode ser
dada pela seguinte equação:
1 1 2 1 2 3cos( ) cos( ) cos( )p t fx l l lθ θ θ θ θ θ= + − + − + (3.32)
Sabendo que o ângulo θ1 tem o valor de π durante toda a fase de suporte duplo, e
resolvendo a equação por trigonometria, tem-se:
2 1 1 3 2 1
1 3 1
cos( )( cos( ) cos( )) ( )( ( )
( )) cos( )
t f t
f p
l l sen l sen
l sen x l
θ θ θ θ θ θ
θ θ θ
+ + + +
+ + = − (3.33)
Para:
2 1 1 3cos( ) cos( )t fA l lθ θ θ= + + (3.34)
2 1 1 3( ) ( )t fB l sen l senθ θ θ= + + (3.35)
2 1cos( )pC x l θ= − (3.36)
Substituindo as Equações 3.34, 3.35 e 3.36 em 3.33, tem-se:
2 2 2 2 2cos( ) ( )A B sen Cθ θ+ = (3.37)
Resolvendo a Equação 3.37 pode-se escrever a equação de θ2 em função da
posição da cintura em x e dos ângulos θ1 e θ3 como:
( ) ( )
( ) ( )
2 2 2 2 22 2 2 2 2 2 2 2
2 2 22 2
2 2 2 2 22 2 2 2 2 2 2 2
2 2 22 2
2 2 4( )( )cos( )
2( )
2 2 4( )( )cos( )
2( )
A C A C A B C B
A B
A C A C A B C B
A B
θ
θ
− − + − − + −=
+
− − − − − + −=
+
60
O ângulo θ2 varia entre os valores mínimo e máximo de 30o e 150o. Ele começa
positivo, decrescendo seu valor pela primeira parcela que vai diminuindo ate chegar ao
seu valor negativo transformando o ângulo θ2 em um ângulo agudo. Assim a função de
θ2 tem a seguinte escrita:
2 2 22 2 2 2 2 2
2 2 22 2
arccos( )
A C B A B C
A Bθ
+ + − = +
(3.38)
Sendo:
2 2 22 2 2 2 2 2
2 22 2( )
A C B A B Cu
A B
+ + −=
+ (3.39)
E substituindo a Equação 3.39 em 3.38, a velocidade e aceleração podem ser
encontradas derivando uma e duas vezes respectivamente, tendo:
( )2 1/ 22
1
1u
uθ = −
−
& &
( ) ( )2
2 1/ 2 3/ 22 2
1 1
1 1u uu
u uθ = − −
− −
&& && &
Para um período de 2s e um passo igual a 0,40m o ângulo θ2 em função do
tempo é ilustrado na Figura 3.13.
61
Figura 3.13: Função da posição do ângulo θ2 no tempo.
Os ângulos das coordenadas do calcanhar e do pé θ6 e θ7 serão funções das
coordenadas generalizadas quando o bípede se encontrar em suporte duplo. As
funções de posição, velocidade e aceleração podem ser encontradas no capitulo 2, nas
Equações 2.48, 2.55 e 2.56 para o ângulo do calcanhar θ6 e nas Equações 2.44, 2.53 e
2.54 para o ângulo do pé θ7 respectivamente.
Quando os ângulos θ6 e θ7 do robô bípede se encontrar em suporte simples, eles
serão independentes. Para um deslocamento suave, uma função de segundo grau foi
definida a essas coordenadas.
O ângulo do calcanhar que irá fazer o movimento de balanço começa o
movimento pendular com o ângulo já conhecido, no tempo t1. Para que o pé não se
encoste ao solo, o ângulo do calcanhar aumenta até um determinado ângulo máximo e
começa a diminuir para preparar o pé para o recontado com o solo com suavidade.
Três pontos são dados para se obter a função de segundo grau do ângulo θ6. Em t1, no
final da fase de suporte duplo, o ângulo θ6i é determinado conhecendo as coordenadas
generalizadas θ3, θ4 e θ5. No final de meio ciclo, o ângulo final θ6f terá o valor do θ2
inicial e um ângulo máximo θ6Max será atribuído no tempo de pico tpico. Assim, tem-se
os seguintes três pontos.
62
6 1 6
6 6
6 6
( )
( )
( / 2)
i
pico Max
f
t
t
T
θ θ
θ θ
θ θ
=
=
=
(3.40)
Para os seguintes valores:
1
6
6 20
/ 2
2/ 2
pico
Max
f
T tt
θ π
θ θ
+=
=
=
(3.41)
As funções de posição, velocidade e aceleração do ângulo do calcanhar θ6 na
fase de balanço são:
26 6 6 6A B t C tθ = + + (3.42)
6 6 62B C tθ = +& (3.43)
6 62Cθ =&& (3.44)
Resolvendo a Equação 3.42 com os pontos da Equação 3.41 podem-se achar os
coeficientes, sendo:
( )( )
( )( )
6 6 1
6 6
6 2 212 2
1
( / 2)
( / 2)
/ 4 ( / 2)/ 4
( / 2)
Max f
i f
p
p
p
t T
t TA
t T t Tt T
t T
θ θθ θ
− −− −
−=
− −− −
−
( ) ( )2 26 6 16
6
/ 4
( / 2)
Max f p
p
a t TB
t T
θ θ− − −=
−
26 6 16 26/ 4 / 2fC a T a Tθ= − −
Para um período de 2s e um passo igual a 0,40m a posição do ângulo θ6 em
função do tempo é ilustrado na Figura 3.14.
63
Figura 3.14: Função da posição do ângulo θ6 no tempo.
Após o pé em balanço desencostar do solo, o ângulo do dedo θ7 irá diminuir
suavemente até o ângulo e a velocidade se tornar igual a zero, preparado o pé para o
recontado com o solo.
Uma função de segundo grau também é atribuída ao ângulo θ7, onde os pontos
utilizados serão:
7 1 7
7
7
( )
( / 2) 0
( / 2) 0
it
T
T
θ θ
θ
θ
=
=
=&
(3.45)
As funções da posição, velocidade e aceleração serão:
27 7 7 7A B t C tθ = + + (3.46)
7 7 72B C tθ = +& (3.47)
6 72Cθ =&& (3.48)
64
Resolvendo a Equação 3.36 para os pontos citados na Equação 3.48 encontram-
se os resultados das constantes, onde:
( )7
7 2
1 / 2iA
t T
θ=
−
6 7B A T= −
26 7 / 4C A T=
Para um período de 2s e um passo igual a 0,40m o ângulo do pé θ7 em função do
tempo é ilustrado na Figura 3.15.
Figura 3.15: Função da posição do ângulo θ7 no tempo.
Na segunda parte do ciclo, o ângulo θ1 terá o valor do complemento da função θ7
no primeiro meio período, e ao contrario para o ângulo θ7. A Figura 3.16 ilustra as
funções θ1 e θ7 ao longo de um ciclo completo.
65
Figura 3.16: Função da posição dos ângulos θ1 e θ7 no tempo para um ciclo completo.
No segundo meio ciclo, o ângulo θ6 terá o valor da função θ2 no primeiro meio
ciclo assim como a função θ6 terá o valor da função de θ2. A Figura 3.17 ilustra as
funções θ2 e θ6 durante um ciclo da marcha completo.
Figura 3.17: Função da posição dos ângulos θ2 e θ6 no tempo para um ciclo completo.
66
Figura 3.18: Posição da cintura ao longo de meio ciclo.
A Figura 3.18 apresenta a posição da cintura durante meio ciclo da marcha, para
um período de 2 segundos e um passo de 0,40m. A velocidade da cintura no eixo x se
manteve com um valor bem próximo de uma constante com foi proposto. A Figura
3.19 apresenta o deslocamento do bípede para este meio ciclo da marcha.
67
Figura 3.19: Deslocamento do robô bípede para um passo de 0,4m.
As Figuras 3.20 e 3.21 ilustram o deslocamento do bípede para um passo de
0,5m e 0,3m respectivamente com um período de 2 segundos.
Figura 3.20: Deslocamento do robô bípede para um passo de 0,5m.
68
Figura 3.21: Deslocamento do robô bípede para um passo de 0,3m.
69
4. Dinâmica
Este capítulo mostrará uma análise da dinâmica do robô ao longo da locomoção
do bípede, mostrando os torques exercidos pelos atuadores responsáveis pela marcha e
as reações das forças existentes no sistema gerando informações para o controle do
equilíbrio.
Dependendo do número de coordenadas selecionadas para definir a configuração
do sistema mecânico, diferentes estruturas de equações podem ser obtidas e diferentes
processos de soluções podem ser adotados. Algumas das formulações levam a equações
que são expressas em termos das forças de restrições, enquanto em outras formulações,
as forças de restrição são eliminadas automaticamente. Por instante, as equações de
movimento de um sistema podem ser formuladas usando um mínimo de coordenadas
independentes ou usando coordenadas redundantes que não são totalmente
independentes. É sempre possível obter equações dinâmicas no qual não incluam forças
de restrição quando os graus de liberdade são usados. O princípio do trabalho virtual na
dinâmica representa uma poderosa ferramenta que levam sistematicamente a uma
variedade de equações dinâmicas de sistemas multicorpos com restrição no qual estas
equações não incluem forças de restrição [26].
4.1 Dinâmica de Mecanismos de Sistemas Multicorpos.
No estudo da dinâmica de sistemas mecânicos, existem duas formas de análise
que podem ser encontradas. São elas a dinâmica direta e a dinâmica inversa. Na
dinâmica inversa, as trajetórias de todos os graus de liberdade são especificadas e o
objetivo é determinar as forças que produzem estes movimentos. Este tipo de análise
requer apenas a solução de sistemas de equações algébricas. Neste tipo de análise,
métodos numéricos de integração não são necessários dês de que as posições,
velocidades e acelerações do sistema sejam conhecidas. No caso da dinâmica direta,
entretanto, as forças que produzem o movimento são dadas e o objetivo é determinar as
coordenadas da posição, velocidades e acelerações. Neste tipo de análise, as acelerações
são as primeiras variáveis a serem determinadas usando as leis de movimento. Estas
acelerações devem então ser integradas para determinar as coordenadas e as
velocidades. Em grande parte das aplicações, uma forma fechada de solução é difícil de
70
ser obtida e, assim, o recurso dos métodos numéricos de integração devem ser usados
[26].
Neste capitulo algumas clássicas descrições das formulações de equações de
movimento de sistemas mecânicos de sistemas multicorpos são apresentados. Os
métodos são mostrados de acordo com suas eficiências computacionais quando
aplicados à geração e solução das equações de movimento automaticamente. O esforço
numérico requerido depende do número de graus de liberdades necessários para
descrever o sistema mecânico e formulação das equações de restrição.
4.2 Dinâmica com restrições
Juntas mecânicas com trajetórias especificadas impõem em sistema multicorpos
restrições na movimentação dos sistemas e componentes. Por causa das restrições
cinemáticas das juntas e das trajetórias especificadas, a seleção das coordenadas e a
forma das equações de movimento não são tarefas triviais e tem sido objeto de muitos
trabalhos em dinâmica de multicorpos computacional. A eficiência, geralmente, e o
numero de algoritmos do procedimento de solução produzem muita dependência na
escolha das coordenadas e a forma do resultado da equação de movimento [27].
Juntas e trajetórias especificadas introduzem forças de restrição que podem
explicitamente aparecer na formulação ou podem ser eliminadas caso a equação
dinâmica for expressa em termos de outras coordenadas independentes para os graus de
liberdade. O número de forças de restrição independentes é sempre igual ao número de
equações de restrições independentes, no qual é igual ao número de coordenadas
dependentes. Obviamente, se não há restrições entre as juntas, não existirá forças de
restrição e não haverá coordenadas dependentes. Este simples fato é crucial para o
entendimento da base das diferentes formas das equações dinâmicas de movimento [27].
Quando as relações cinemáticas são expressas em termo dos graus de liberdade, a
aplicação do principio do trabalho virtual em dinâmica leva a um número de equações
diferenciais dinâmicas iguais a o número de graus de liberdade do sistema. Nestas
equações as forças de restrição são automaticamente eliminadas. Forças de restrição,
entretanto, aparecem em equações dinâmicas se estas equações são formuladas em
termos de coordenadas que não são totalmente independentes. O número de forças de
71
restrição independentes que aparecem nestas equações é igual ao número de
coordenadas dependentes usado na formulação da dinâmica.
4.3 Principio Generalizado de d’Alembert para Sistemas de Partículas.
Considere um sistema com N partículas e massa mi, i = 1,...,N, onde a posição é
dada no eixo de referencia (x,y,z) por:
i
i i
i
x
r y
z
=
i
i i
i
x
r y
z
=
&
& &
&
i
i i
i
x
r y
z
=
&&
&& &&
&&
Forças Fi atuam nas massas. Forças de reações Ki, devido a restrições ideais
(forças de reação não executam trabalho virtual em seus deslocamentos virtuais) são
introduzidas, levando ao principio generalizado de d’Alembert seguindo a segunda lei
de NEWTON para partículas de massa mi.
[ ]1
. 0N
í i i i
i
F m a rδ=
− =∑
Para esta equação, várias outras podem ser derivadas.
4.4 O Método dos Multiplicadores de Lagrange para Sistemas de
Partículas
Considere um sistema com N partículas de massa mi com posição ri, alem de g
restrições geométricas.
1( ; ,..., ) 0nf t r rα = 1,..., gα =
E k restrições cinemáticas
72
1 11
( ; ,..., ) ( ; ,..., ) 0N
i n i n
i
l t r r v d t r rβ β βφ=
≡ + =∑ &
Para este sistema com f=3N-g-K graus de liberdade, a equação de movimento
usando multiplicadores de Lagrange é [20]:
1 1
g ka
i i i i
i
fm a F l
rα β β
α β
δλ µ
δ= =
= + +∑ ∑ 1,...,i N=
1( ; ,..., ) 0nf t r rα = 1,..., gα =
1 11
( ; ,..., ) ( ; ,..., ) 0N
i n i n
i
l t r r v d t r rβ β βφ=
≡ + =∑ & 1,..., kβ =
Neste sistema de (3N+g+k) equações, existem (3N+g+k) variáveis
desconhecidas.
As forças de reação do sistema são [20]:
1 1
g ka
i i
i
fR l
rα β β
α β
δλ µ
δ= =
= +∑ ∑ 1,...,i N=
O método dos multiplicadores de Lagrange pode ser derivado para um sistema de
corpos rígidos via segunda lei de Newton e a conservação do momento angular.
As vantagens do método dos multiplicadores de Lagrange são:
• A equação é válida para sistemas holonômicos e não-holonômicos.
• Forças de reação podem ser calculadas mais facilmente.
As desvantagens do método dos multiplicadores e Lagrange são:
• Para um sistema com apenas f=3N-g-k graus de liberdade, 3N+g+k equações têm
que ser resolvidas.
4.5 Equação de Lagrange para Corpos Rígidos.
Considerando um sistema com f coordenadas generalizadas independentes, sendo:
73
1 2, ,..., fq q q
Correspondendo os graus de liberdade, a equação de Lagrange para sistemas
holonômicos pode ser derivada do principio generalizado de d’Alembert [28]. A
equação é:
i
i i
d L LQ
dt q q
∂ ∂− =
∂ ∂ &
c pL E E= −
Onde L é conhecido como o Lagrangeano, Qi são as forças generalizadas, Ec é a
energia cinética total do sistema e Ep a energia potencial total do sistema.
As vantagens da equação de Lagrange são [20]:
Mínimo número f de equações.
• Para a geração da equação, somente a energia cinética e potencial, e as forças
generalizadas são requeridas.
Desvantagens da Equação de Lagrange [20].
• O esforço requerido para expressar as energias do mecanismo e de sistemas
multicorpos complexos é alto.
Equações de movimento para sistemas multicorpos com restrição podem ser
derivadas com a equação de Lagrange para sistemas sem restrição e subseqüentemente
introduzindo os multiplicadores de Lagrange [20], produzindo um sistema com f+g
equações.
Esta aproximação é implementada em muitos programas para simulação de
sistemas multicorpos. Assim a equação de Lagrange com os multiplicadores de
Lagrange seria [20]:
74
1
g
ai
i i i
fd L LQ
dt q q rα
α
δλ
δ=
∂ ∂− = +
∂ ∂ ∑
&
4.5.1 Obtenção da Energia Cinética
Considere um sistema com N corpos rígidos. A energia cinética total é dada pela
soma da contribuição relativa a movimentação de cada corpo e a contribuição relativa à
movimentação de cada atuador nas juntas.
Considerando que a energia cinética relativa aos atuadores seja muito menor do
que a energia cinética de cada corpo neste bípede, a energia do sistema pode ser dada
por:
1
N
c ci
i
E E=
=∑ i=1,...,N (4.1)
A energia cinética de cada corpo é dada por [22]:
1
2i
T
ci i i
V
E p p dVρ= ∫ & & (4.2)
Onde p& denota o vetor velocidade linear e ρ a massa especifica da partícula
elementar de um volume dV, e Vi é o volume do corpo i.
Considere o vetor posição p da partícula e o vetor posição pc do centro de massa
do corpo i, ambos expressos em relação a um eixo de referencia inercial como mostrado
na Figura 4.1. Assim:
T
i ix iy iz i cir r r r p p = = − (4.3)
i ci i ip p w r= + ×& & (4.4)
75
Figura 4.1. Descrição cinemática do corpo i para formulação do Lagrangeano [22].
A energia cinética pode ser escrita substituindo a Equação 4.4 em 4.2.
1( ) ( )
2i
T
ci ci i i ci i i
V
E p w r p w r dVρ= + × + ×∫ & &
1( ) ( ) ( ) ( )
2i
TT T T
ci ci ci ci i i i ci i i i i
V
E p p p w r w r p w r w r dVρ= + × + × + × ×∫ & & & & (4.5)
Desenvolvendo a Equação 4.5 chega-se a Equação 4.6.
1 1 1( ) ( ) ( )
2 2 2i i
T T T T T
ci i ci ci ci i i i ci i i i i i i
V V
E m p p p w r w r p dV w r r I r r dV wρ ρ
= + × + × + −
∫ ∫& & & & (4.6)
A configuração mostrada na Equação 4.6 mostra que a energia cinética é formada
pelas seguintes contribuições:
Componente da Energia cinética devido à translação.
1
2T
ciT i ci ciE m p p= & & (4.7)
76
Contribuição mutua.
Considerando neste trabalho que todos os corpos são simétricos a energia cinética
proveniente da segunda integral será zero considerando que:
( ) ( )T T
i i ci ci iw r p p w r× = − ×& &
Assim:
1( ) ( ) 0
2i
T T
ci i i i ci
V
p w r w r p dVρ× + × =∫ & &
Componente da Energia cinética devido à Rotação.
Para:
( )i
T T
i i i i i
V
I r r I r r dVρ
= − ∫ (4.8)
Sendo:
( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( )
2 2
2 2
2 2
iy
iy
iz ix iy ix iz
m m m
i iy ix ix iz iy iz
m m m
iz ix iz iy ix
m m m
r r dm r r dm r r dm
I r r dm r r dm r r dm
r r dm r r dm r r dm
+ − −
= − + − − − +
∫ ∫ ∫
∫ ∫ ∫
∫ ∫ ∫
A energia cinética proveniente da rotação do corpo pode ser definida como:
1
2ciR i i iE w I w= (4.9)
A matriz Ii é simétrica e representa o tensor de inércia relativo ao centro de massa
do corpo i quando expresso em um eixo de referencia. A posição do corpo i depende da
configuração do robô e então do tensor de inércia quando expressa em uma base de
referencia, é uma configuração dependente. Se a velocidade angular do corpo i é
77
expressa com referencia a uma base fixa ao corpo (como na convenção de DENAVIT-
HARTENBERG [19]), tem-se:
i T
i i iw R w= (4.10)
Onde Ri é a matriz de rotação da base do corpo i para uma base inercial. Quando
se refere a uma base no corpo, o tensor de inércia é constante. Sendo iIi tal tensor, é fácil
verificar a seguinte relação [22].
i T
i i i iI R I R= (4.11)
Se os eixos do corpo i coincidem com os principais eixos de inércia, os produtos
de inércia serão nulos e o tensor de inércia relativo ao centro de massa será uma matriz
diagonal.
Somando as contribuições das energias cinéticas de translação e rotação dadas
pelas Equações 4.7 e 4.9, a Energia cinética do corpo i pode ser dada por:
1
2T i T
ci i ci ci i i i i iE m p p w R I R w= +& & (4.12)
Neste ponto, é necessário expressar a energia cinética como função das
coordenadas generalizadas do sistema, que são as variáveis nas juntas. Para este
propósito, o método geométrico para computação do jacobiano pode ser aplicado para o
corpo antes do corpo efetivo. Produzindo:
1 21 2
...ci ci cici i
i
p p pp q q q
q q q
∂ ∂ ∂= + + +
∂ ∂ ∂& & & &
Assim, para as velocidades de translação e rotação tem-se:
1 1 2 2 ...ci Ti Ti Tii i Tip J q J q J q J q= + + + =& & & & & (4.13)
1 1 2 2 ...i Ri Ri Rii i Riw J q J q J q J q= + + + =& & & & (4.14)
78
Onde as contribuições das colunas do jacobiano relativas a velocidades das juntas
têm sido tomadas de acordo com o corpo i. Assim os jacobianos de translação e rotação
podem ser encontrados por:
[ ]1 ... 0 ... 0Ti Ti TiiJ J J= (4.15)
[ ]1 ... 0 ... 0Ri Ri RiiJ J J= (4.16)
Assim, a energia cinética do corpo i em função das coordenadas generalizadas
pode ser obtida por:
1 1
2 2T T T T i T
ci i Ti Ti Ri i i i RiE m q J J q q J R I R J q= +& & & & (4.17)
A energia cinética total do sistema pode ser determinada pelo somatório de todas
as energias cinéticas de cada corpo do sistema encontrado pela Equação 4.17. Assim:
1
1 1
2 2
nT T T T i T
c i Ti Ti Ri i i i Ri
i
E m q J J q q J R I R J q=
= +∑ & & & & (4.18)
Tomando:
1
( )n
T T i T
i Ti Ti Ri i i i Ri
i
M q m J J J R I R J=
= +∑
A energia cinética do sistema pode ser escrita por:
1( )
2T
cE q M q q= & & (4.19)
Onde a matriz M (n x n) é simétrica e positiva definida.
4.5.2 Obtenção da Energia Potencial.
A energia potencial total é obtida pela soma da contribuição relativa a cada corpo
do sistema.
79
1
n
p pi
i
E E=
=∑ (4.20)
Mais uma vez assumindo os corpos do robô como rígidos, a contribuição de
energia devido a forças de gravidade pode ser expresso por:
i
T T
pi i i ci
V
E g p dV m g pρ= − = −∫ (4.21)
Onde g é o vetor aceleração da gravidade em uma base de referencia. Para este
trabalho a vertical se encontra o eixo y produzindo um vetor g = [0 –g 0]T. Assim, a
energia potencial total do sistema pode ser dada substituindo a Equação 4.21 em 4.20,
obtendo:
1
( )n
T
p i ci
i
E q m g p=
=∑ (4.22)
Considerando que a posição de cada centro de massa pci e função das coordenadas
generalizadas, a energia potencial é função das posições das coordenadas generalizadas,
mas não das velocidades.
4.6 Equação de Movimento para Fase de Suporte Simples
O ciclo da marcha terá características diferentes para as fases de suporte duplo e
suporte simples. A diferença mais importante consiste na quantidade de graus de
liberdade existentes em cada ciclo.
Na fase de suporte simples, pode se considerar que todas as juntas são
independentes perfazendo um total de sete graus de liberdade no sistema, ilustrado na
Figura 4.2. Para simplificação dos cálculos, é considerado que neste robô, os centros de
massa se localizam no centro de cada corpo.
80
Figura 4.2: Modelo do robô bípede com os centros de massa na fase de suporte simples.
A obtenção da equação de movimento para a fase de suporte simples será feita
utilizando o método da equação de Lagrange. Para se encontrar a energia cinética e
potencial, é necessário encontrar as posições e velocidades das coordenadas
generalizadas. Utilizando as equações de DENAVIT & HARTEMBERG [19]
desenvolvidas no capítulo da cinemática pode-se encontrar as posições dos centros de
massa. Assim tem-se:
[ ] [ ]101 0,5 0 0 0 1
T T
pe de de deP x y z H= = ⋅ (4.23)
[ ]2 11 00,5 0 0 0 1
T
feP H H= ⋅ (4.24)
[ ]3 22 00,5 0 0 0 1
T
tiP H H= ⋅ (4.25)
[ ]30 0 0 0 1
T
cinP H= (4.26)
[ ]4 33 00,5 0 0 0 1
T
feBP H H= ⋅ (4.27)
81
[ ]5 44 00,5 0 0 0 1
T
tiBP H H= ⋅ (4.28)
[ ]6 55 00,5 0 0 0 1
T
peBP H H= ⋅ (4.29)
[ ]7 66 00,5 0 0 0 1
T
deBP H H= ⋅ (4.30)
Onde “pe” denota pé, “fe” fêmur, “ti” tíbia, “cin” cintura, “feB” fêmur em
balanço, “tiB” tíbia em balanço, “peB” pé em balanço e “deB” dedo em balanço.
A posição da cintura também será importante uma vez que ela possui massa
proveniente da parte superior do bípede mais seu mecanismo de controle de equilíbrio
considerado situado na cintura neste trabalho. O vetor q das coordenadas generalizadas
na fase de suporte de suporte simples é dado por:
[ ]1 2 3 4 5 6 7
Tq θ θ θ θ θ θ θ= (4.31)
Assim o vetor velocidade das coordenadas generalizadas é:
1 2 3 4 5 6 7
T
q θ θ θ θ θ θ θ = & & & & & & && (4.32)
Pelo método do jacobiano, a energia cinética do sistema é dada pela Equação
4.19:
1( )
2T
cE q M q q= & &
Onde
8
1
( ) ( )T T i T
i Ti Ti Ri i i i Ri
i
M q m J J J R I R J=
= +∑ (4.33)
Sabendo que i numera os corpos sendo um referente ao pé em suporte e oito ao
dedo em balanço, contando com a massa da cintura. O valor das massas e dos
momentos de inércia de cada corpo já é conhecido e foram apresentados na tabela 2.1.
O jacobiano de translação é uma matriz que pode ser encontrada por:
82
1 2 3 4 5 6 7
ci ci ci ci ci ci ciTi
p p p p p p pJ
θ θ θ θ θ θ θ
∂ ∂ ∂ ∂ ∂ ∂ ∂=
∂ ∂ ∂ ∂ ∂ ∂ ∂ (4.34)
O Jacobiano de rotação pode ser escrito [22] da seguinte forma:
[ ]1 2 3 4 7...Ri Ri Ri Ri Ri RiJ J J J J J= (4.35)
Sendo
1Rij jJ z −=
Onde zj-1 é o vetor unidade do eixo z na base j-1, com j=1,2,...,7 [22].
A energia cinética assim pode ser encontrada resolvendo a Equação 4.19.
Conhecido as posições, a energia potencial também pode ser encontrada pela Equação
4.22.
8
1
( ) T
p i ci
i
E q m g p=
=∑ (4.36)
Conhecido as energias pode-se encontrar a equação de movimento pela equação
de Lagrange. Para sistemas de cadeia aberta, a equação de Lagrange fornece o seguinte:
pc cEE Ed
Qdt q q q
∂ ∂ ∂− + =
∂ ∂ ∂ & (4.37)
Resolvendo a Equação 4.37 por partes para as energias já conhecidas, tem-se:
( )cEM q q
q
∂=
∂&
&
( ) ( )cEdM q q M q q
dt q
∂= +
∂ &&& &
& (4.38)
Para o segundo termo da Equação 4.37 tem-se:
83
1 ( )
2Tc
i
E M qq q
q q
∂ ∂=
∂ ∂& & (4.39)
E para a derivada da energia potencial pelas coordenadas generalizadas, tem-se:
8
1
T
i ci
p i
m g pE
q q
=
∂ ∂ =
∂ ∂
∑ (4.40)
Assim, substituindo cada termo na Equação 4.37, a equação de movimento do
bípede na fase de cadeia aberta, ou seja, em suporte simples é mostrado na Equação
4.41.
8
11 ( )( ) ( )
2
T
i ci
iT
m g pM q
M q q M q q q q Qq q
=
∂
∂ + − + =∂ ∂
∑&&& & & & (4.41)
Considerando:
8
1
T
i ci
i
m g p
Gq
=
∂ =
∂
∑ (4.42)
( )( )1( , ) ( )
2
Tq M qC q q M q
q
∂ = − ∂
&&& (4.43)
E substituindo as expressões 4.42 e 4.43 na Equação 4.41, a equação de
movimento fica com a seguinte forma:
( ) ( , )M q q C q q q G Q+ + =&& & & (4.44)
A matriz M simboliza a matriz de massa do sistema que produz os torques
provenientes da inércia produzida pelo sistema. A matriz C simboliza a matriz de
amortecimento provocada pelos torques provenientes das forças de coriolis e centrífugas
existentes no sistema. A matriz de gravidade G mostra os torques provenientes da força
de gravidade atuante no sistema.
84
Os componentes da matriz C e do vetor das forças de gravidade são calculados
através das seguintes equações:
( ) ( )7
1
1
2ij ik
ij k k
k k i
M Mc q q
q q=
∂ ∂ = − ∂ ∂
∑ & & i=1,2,...,7 e j=1,2,...,7 (4.45)
( )p
i
i
Eg
q
∂=
∂ i=1,2,...,7 (4.46)
O vetor dos torques generalizados será proporcionado pelos torques dos atuadores
em cada junta. Nesta fase todas as juntas produzem torque para produção da
movimentação.
O ângulo da cintura também sofrerá um torque considerando que a parte superior
do bípede está inclinada para frente como na Figura 4.3.
Figura 4.3: Torque produzido pelos atuadores e pelo tórax do bípede.
O vetor dos torques generalizados é:
85
[ ]1 2 3 4 5 6 7
TQ T T T T T T T= (4.47)
Neste trabalho, a influência da parte superior do bípede não será abordada dando
ênfase nas reações que a parte inferior do bípede influencia na dinâmica.
4.7 Equação de Movimento para Fase de Suporte Duplo
Na primeira parte do ciclo da marcha o sistema se encontra em cadeia fechada. O
numero de graus de liberdade do sistema é menor e forças de restrição aparecerão na
equação de movimento.
O modelo do sistema na fase de suporte duplo se encontra na Figura 4.4.
Figura 4.4: modelo do bípede na fase de suporte duplo.
Utilizando o método da equação de Lagrange, é conhecido que os dedos não
possuem energia, pois permanecem sem velocidade e no chão. O sistema possui três
graus de liberdade e os ângulos dos joelhos θ3 e θ5 e o ângulo da cintura θ4 são os
escolhidos para serem as coordenadas generalizadas. Assim os ângulos θ1, θ2, θ6 e θ7
86
serão coordenadas dependentes das coordenadas generalizadas. A Figura 4.5 mostra os
torques motores na fase de suporte duplo.
Figura 4.5: Torques motores na fase de suporte duplo.
De modo a obter a equação de movimento da fase de suporte duplo com os
Multiplicadores de Lagrange é necessário saber as equações de restrição.
Duas equações de restrição existirão no sistema. São elas:
1 1 2 3 4 5 6 1 2 3 4 5
1 2 3 4 1 2 3 1 2 1
cos( ) cos( )
cos( ) cos( ) cos( ) cos( ) 0
p t
f f t p
l l
l l l l Ps
φ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ
= − + − − + + − + − − +
− + − + − + + − + − = (4.48)
2 1 2 3 4 5 6 1 2 3 4 5
1 2 3 4 1 2 3 1 2 1
( ) ( )
( ) ( ) ( ) ( ) 0
p t
f f t p
l sen l sen
l sen l sen l sen l sen
φ θ θ θ θ θ θ θ θ θ θ θ
θ θ θ θ θ θ θ θ θ θ
= − + − − + + − + − − +
− + − + − + + − + = (4.49)
Em cadeia fechada, o ângulo θ1 permanecerá sempre igual a π, e encostado ao
solo. Assim o pé não influenciará na dinâmica do bípede. As coordenadas do sistema
serão redundantes e se apresentam como:
87
[ ]2 3 4 5 6
Tq θ θ θ θ θ= (4.50)
Suas velocidades serão:
2 3 4 5 6
T
q θ θ θ θ θ = & & & & &&
A equação de Lagrange adicionada com os multiplicadores de Lagrange terá a
seguinte forma [20]:
( )pc cEE Ed
S q Qdt q q q
λ∂ ∂ ∂
− + = + ∂ ∂ ∂ &
(4.51)
Desenvolvendo por partes mais uma vez a equação de Lagrange tem-se:
( ) ( )cEdM q q M q q
dt q
∂= +
∂ &&& &
& (4.52)
1 ( )
2Tc
i
E M qq q
q q
∂ ∂=
∂ ∂& & (4.53)
8
1
T
i cip i
m g pE
q q
=
∂ ∂ =
∂ ∂
∑ (4.54)
A matriz S poderá ser encontrada pela derivada das Equações 4.48 e 4.49 pelo
vetor das coordenadas, resultando em:
1 2
2 2
1 2
3 3
1 2
6 6
S
φ φθ θ
φ φθ θ
φ φθ θ
∂ ∂ ∂ ∂
∂ ∂
∂ ∂= ∂ ∂
∂ ∂
M M
(4.55)
O vetor dos multiplicadores de Lagrange é:
[ ]1 2λ λ λ= (4.56)
88
Resolvendo e substituindo as Equações 4.52, 4.53, 4.54, 4.55 e 4.56 na Equação
4.51 a Equação de movimento do robô bípede pode ser encontrada.
A Equação de movimento para a fase de suporte duplo é:
8
11 ( )( ) ( )
2
T
i ci
iT
m g pM q
M q q M q q q q S Qq q
λ=
∂
∂ + − + = +∂ ∂
∑&&& & & & (4.57)
Considerando
8
1
T
i ci
i
m g p
Gq
=
∂ =
∂
∑
( )( )1( , ) ( )
2
Tq M qC q q M q
q
∂ = − ∂
&&&
A Equação de movimento do bípede para suporte duplo é:
( ) ( , )M q q C q q q G S Qλ+ + = +&& & & (4.58)
O vetor dos torques generalizados pode ser conseguido pela seguinte equação:
[ ]2 3 4 5 6
TQ T T T T T= (4.59)
Considerando que não existirá torque nas juntas dos dedos com os pés θ1 e θ7 na
fase de suporte duplo serão cinco equações e sete incógnitas. Os torques nas juntas e as
constantes lambdas. Duas outras equações seriam necessárias para solução do problema.
Entretanto neste trabalho é considerado que na fase de suporte duplo apenas os
atuadores da cintura e dos calcanhares produzem torque motor. Assim, como os torques
dos joelhos na fase de suporte duplo terão o valor de zero, haverá cinco incógnitas e
cinco equações no sistema.
Para os cálculos dos componentes das matrizes C e G, as equações 4.45 e 4.46
podem ser utilizadas respectivamente para i=1,2,...,5; e j=1,2,...,5.
89
Para um passo de 0,40m e um período de 2s, as Figuras 4.6 e 4.7 mostram os
torques motores nos atuadores localizados nas juntas em meio período.
Figura 4.6. Torque motor nas juntas dos ângulos θ1, θ2, θ3 e θ4.
Em t = 0,2 segundos a fase de suporte duplo acaba caracterizando o tempo t1. O
atuador T1 localizado entre o pé e o dedo que fará o papel de suporte permanece com o
ângulo fixo e T1 tem o valor de zero. Após t1, o torque T1 apresentado será produzido
pelo solo que restringe o movimento do pé. Em t com o valor aproximado de 0,32s, ou
seja, no tempo t2 a cintura ultrapassa o ponto x do pé em suporte e T1 começa a atuar
diminuído o ângulo θ1.
90
Figura 4.7: Torque motor nas juntas dos ângulos θ5, θ6 e θ7.
Considerando TI, os torques de inércia; TA, os torques de amortecimento e TG os
torques de gravidade, onde:
( )IT M q q= && (4.60)
( , )AT C q q q= & & (4.61)
( )GT G q= (4.62)
Os torques provenientes da inércia do movimento e da força da gravidade terão a
maior contribuição que resulta na função dos torques motores nas juntas. Os torques de
amortecimento, resultantes das forças de coriolis e centrífugas, terão pouca influência
na dinâmica do bípede, principalmente os corpos mais afastados do referencial inercial
que são o pé em balanço e o dedo, como mostrado nas Figuras 4.8 a 4.13.
91
Figura 4.8: Torques provenientes da inércia TI atuando nos ângulos θ1, θ2, θ3 e θ4.
Figura 4.9: Torques provenientes da inércia TI atuando nos ângulos θ5, θ6 e θ7.
92
Figura 4.10: Torques provenientes da força de gravidade TG atuando nos ângulos θ1, θ2,
θ3 e θ4.
Figura 4.11: Torques provenientes da força de gravidade TG atuando nos ângulos θ5, θ6
e θ7.
93
Os torques de gravidade terão seus valores máximos no início e no final do
ciclo. Isto se deve as maiores distancias dos centros de massa em relação ao referencial
inercial.
No tempo t2, neste caso em t =0,3s, o começo da movimentação do pé em suporte
provoca o aumento nos torques de amortecimento dos ângulos dos corpos da perna em
suporte, alcansando seus valores máximos, como ilustrado na Figura 4.12.
Figura 4.12: Torques provenientes da força de coriolis e centrifugas TA atuando nos
ângulos θ1, θ2, θ3 e θ4.
Os torques de amortecimento provenientes dos ângulos do joelho em balanço θ5,
do pé em balanço θ6 e do dedo em balanço θ7 terão um valor muito próximo de zero,
como ilustrado na Figura 4.13.
94
Figura 4.13: Torques provenientes da força de coriolis e centrifugas TA atuando nos
ângulos θ5, θ6 e θ7.
Se, para um mesmo valor de passo, a frequência do ciclo diminuir, aumentando o
período do ciclo, os valores dos torque motores terão uma significativa redução de seus
valores. Isto se deve ao fato de a aceleração dos ângulos dos joelhos θ3 e θ5, e da cintura
θ4 serem funções diretas do quadrado da freqüência. O ângulo do calcanhar θ2 também
terá uma significativa redução de sua aceleração, considerando que, na fase de suporte
duplo é função dos três ângulos vistos acima, e na fase de suporte simples é função do
ângulo do joelho θ3.
Para um passo de 0,40m e um período de 4, as Figuras 4.14 e 4.15 ilustram os
valores dos torques motores em meio período.
95
Figura 4.14: Torque motor nas juntas dos ângulos θ1, θ2, θ3 e θ4.
Figura 4.15: Torque motor nas juntas dos ângulos θ5, θ6 e θ7.
96
As Figuras 4.16 e 4.17 ilustram uma comparação entre os torques motores para
um meio período de 2s e 1s respectivamentes. Como visto, os valores menores de
torques serão resultados da diminuição das acelerações em cada junta provocando
reduções dor torques de inércia. Os torques de amortecimentos que já tinham valores
muito baixos diminuirão mais onde sua influencia só pode ser sentido no ângulo do
calcanhar que fará a função de suporte θ2. Os torques de gravidade não sofrerão
alteração. Entretanto, pelo maior tempo de torque, os atuadores serão obrigados a
manter seus torques por mais tempo.
Figura 4.16: Comparação dos Torques T1, T2, T3 e T4 para meio período de 2 e 1s.
Figura 4.17: Comparação dos Torques T5, T6 e T7 para meio período de 2 e 1s.
97
5. Modelagem do Mecanismo de Equilíbrio do Robô Bípede
A modelagem de sistemas de equilíbrio em robôs bípedes ainda é pouco abordada
pelos estudos nesta área. O efeito giroscópio é um artifício que apresenta muitas
vantagens para esta função. Neste capitulo um estudo de uma modelagem para controlar
a estabilidade do robô será abordado, onde um giroscópio será utilizado para esta
função. Será descrita toda a dinâmica existente no sistema para a obtenção de
resultados.
O dispositivo giroscópio pode ser descrito essencialmente como um ou vários
discos livres que podem girar em qualquer direção e que possuem a propriedade de se
opor a qualquer tentativa de mudança na sua direção original. Um exemplo muito
comum é, ao girar a roda de uma bicicleta no ar e tentar mudar a direção de seu eixo
bruscamente, percebe-se uma enorme reação.
De um modo geral, pode-se dizer que o funcionamento do giroscópio é baseado
no princípio da inércia [28]. Estes dispositivos têm muitas funções utilizadas na ciência.
Um exemplo muito comum foi utilizado quando o giroscópio substituiu a bússola na
navegação marítima. Na aviação, é usado no mecanismo do girocompasso e do piloto
automático, que permite vôos em condições de visibilidade muito baixas. Nos vôos
espaciais é um dispositivo fundamental na orientação das espaçonaves.
5.1 Os Ângulos de Euler
Entre os parâmetros mais utilizados para descrever a orientação de um sistema
estão os três independentes ângulos de Euler. Muitos tipos diferentes de ângulos de
Euler são comumente usados na analise de mecanismos e sistemas. Os mais utilizados
ângulos de Euler serão descritos e definidos e as matrizes de transformação em termo
destes ângulos serão desenvolvidas [26].
Os ângulos de Euler envolvem três sucessivas rotações sobre três eixos que não
são, em geral, perpendiculares. Resolvendo essas três rotações sucessivas em sua
seqüência própria, o sistema de coordenadas pode achar qualquer orientação.
Considerando o sistema de coordenadas XYZ e XiYiZi , sendo eles inicialmente
coincidentes, a seqüência começa, como ilustrado na Figura 5.1, com uma rotação do
98
sistema XiYiZi em um ângulo φi sobre o eixo Z. Considerando φi uma rotação sobre Z, a
matriz de transformação resultante desta transformação é:
1
cos 0
cos 0
0 0 1
i i
i
i i
sen
A sen
φ φ
φ φ
− =
(5.1)
Figura 5.1: Ângulos de Euler [26].
O sistema de coordenada XiYiZi é então rotacionado em um ângulo θi sobre o
atual eixo Xi. A mudança na orientação do sistema de coordenada XiYiZi, resultado da
rotação θi é descrito usando a matriz:
2
1 0 0
0 cos
0 cos
i
i i
i i
A sen
sen
θ θ
θ θ
= −
(5.2)
Finalmente, o sistema de coordenada XiYiZi é rotacionado em um ângulo de ψi
sobre o atual eixo Zi. A mudança na orientação do sistema de coordenadas XiYiZi,
resultado desta rotação é dado pela seguinte matriz:
3
cos 0
cos 0
0 0 1
i i
i
i i
sen
A sen
ψ ψ
ψ ψ
− =
(5.3)
99
Usando as matrizes de transformação iA1, iA2 e iA3 dada pelas Equações 5.1, 5.2 e
5.3 respectivamente, a orientação final do sistema de coordenadas XiYiZi pode ser
definida no sistema XYZ pela matriz de transformação dada por:
1 2 3i i i iA A A A= (5.4)
Esta equação pode ser usada para definir a matriz iA em termo dos ângulos φi, θi e
ψi como:
cos cos cos cos cos cos
cos cos cos cos cos cos cos
cos cos
i i i i i i i i i i i i
i
i i i i i i i i i i i i
i i i i i
sen sen sen sen sen sen
A sen sen sen sen sen
sen sen sen
ψ φ θ φ ψ ψ φ θ φ ψ θ φ
ψ φ θ φ ψ ψ φ θ φ ψ θ φ
θ ψ θ ψ θ
− − − = + − + −
Os três ângulos φi, ψi e θi são chamados de ângulos de Euler. A orientação de
qualquer corpo rígido no espaço pode ser obtida executando estas três sucessivas e
independentes rotações.
Na discussão apresentada nesta seção, foi considerada a seqüência das rotações
sobre os eixos Zi, Xi e Zi. Outras sequências que também são usados [26] para definir os
ângulos de Euler são as rotações sobre Zi, Yi e Xi, ou rotações sobre Zi, Yi e Zi. Os usos
destas sequências levam a matrizes de transformação que estão diferentes da
apresentada neste item, contudo, o processo usado para definir a matriz de
transformação usando estas seqüências é a mesma descrita nesta seção. Existem também
outras escolhas para as coordenadas de rotação que são comumente usadas com
freqüência para descrever a orientação de um corpo rígido no espaço. Entre estas
coordenadas estão os quatro parâmetros de Euler e os três independentes parâmetros de
Rodrigues.
5.2 O Giroscópio e os Ângulos de Euler
O estudo do movimento giroscópio é um dos problemas mais interessantes
estudados na dinâmica espacial [26]. Este problema ocorre quando o eixo de rotação de
um corpo rígido muda. O giroscópio visto na Figura 5.2 consiste em um rotor que gira
em torno do seu eixo de direção Z3 que é montado em um anel denominado anel
interno. Como mostrado na figura, o rotor é livre para rodar em torno do seu eixo de
100
simetria, relativo ao anel interno, e o anel interno é livre para rodar em torno de seu eixo
X2 que é perpendicular ao eixo do rotor. O eixo X2 é montado em um segundo anel,
chamado de anel externo, que é livre para rodar em torno do eixo Z1. O rotor cujo centro
de gravidade se mantém fixo pode realizar qualquer posição arbitraria como visto na
Figura 5.2 por meio de três sucessivas rotações.
Figura 5.2: Giroscópio montado como suspensão cardan [23].
1. Uma rotação φ do anel externo sobre o eixo Z1.
2. Uma rotação θ do anel interno sobre o eixo X2.
3. Uma rotação ψ do rotor sobre seu eixo Z3.
Estes três ângulos de Euler são chamados de precessão, nutação e o spin
respectivamente, e o tipo de montagem usado no giroscópio é chamado de suspensão
cardan.
A velocidade angular do rotor pode ser escrita como:
1 2 3w k i kφ θ ψ= + +& & & (5.5)
Onde k1 é o vetor unidade ao longo do eixo Z1, i2 é o vetor unidade ao longo do
eixo X2, e k3 é o vetor unidade ao longo do eixo Z3. O vetor unidade k1 é:
[ ]1 0 0 1T
k = (5.6)
101
Sendo a rotação φ em torno do eixo Z1, o vetor unitário i2 é definido como
2
cos( ) ( ) 0 1 cos( )
( ) cos( ) 0 0 ( )
0 0 1 0 0
sen
i sen sen
φ φ φ
φ φ φ
− = =
(5.7)
Sendo a rotação θ em torno do eixo X2, o vetor unitário k3 é definido como:
3
cos( ) ( ) 0 1 0 0 0 ( ) ( )
( ) cos( ) 0 0 cos( ) ( ) 0 cos( ) ( )
0 0 1 0 ( ) cos( ) 1 cos( )
sen sen sen
k sen sen sen
sen
φ φ φ θ
φ φ θ θ φ θ
θ θ θ
− = − = −
(5.8)
Substituindo na Equação 5.5 as Equações 5.6, 5.7 e 5.8, a velocidade do rotor em
relação ao referencial inercial pode ser escrita como:
0 cos( ) ( ) ( )
0 ( ) cos( ) ( )
1 0 cos( )
sen sen
w sen sen
φ φ θ
φ θ φ ψ φ θ
θ
= + + −
& & &
Logo:
cos( ) ( ) ( )
( ) cos( ) ( )
cos( )
sen sen
w sen sen
θ φ ψ φ θ
θ φ ψ φ θ
φ ψ θ
+
= − +
& &
& &
& &
(5.9)
Ao qual a Equação 5.9 pode ser escrita em forma de matriz como
0 cos( ) ( ) ( )
0 ( ) cos( ) ( )
1 0 cos( )
sen sen
w sen sen
φ φ θ φ
φ φ θ θ
θ ψ
= −
&
&
&
(5.10)
Esta equação pode ser escrita da seguinte forma
w Gγ= & (5.11)
Onde G é a matriz cujas colunas são os vetores unitários k1, i2 e k3. Esta matriz é
definida em termos dos ângulos de Euler, onde:
102
0 cos( ) ( ) ( )
0 ( ) cos( ) ( )
1 0 cos( )
sen sen
G sen sen
φ φ θ
φ φ θ
θ
= −
(5.12)
e
[ ]T
γ φ θ ψ= (5.13)
Derivando a velocidade angular em relação ao tempo, pode-se encontrar a
aceleração angular absoluta α do mesmo.
G Gα γ γ= + &&& & (5.14)
Onde o primeiro termo pode ser mostrado explicitamente como:
0 ( ) ( ) cos( ) cos( ) ( )
0 cos( ) ( ) ( ) cos( ) cos( )
0 0 ( )
sen sen sen
G sen sen
sen
φ φ φ θ θ φ θ φ
φ φ φ θ φ φ θ θ
θ θ
− +
= − −
& & &
& & & &
&
( ) ( ) cos( ) cos( ) ( )
cos( ) ( ) ( ) cos( )cos( )
( )
sen sen sen
G sen sen
sen
φ θφ φ θ θψ φ θ φψ
γ φ φθ φ θ φψ φ θ θψ
θ θψ
− + +
= + − −
& & & && &
& & & & && & &
& &
(5.15)
A equação de movimento do rotor do giroscópio pode ser facilmente encontrada
utilizando a equação de Lagrange. Para este fim, é definido primeiro o vetor velocidade
angular no sistema de coordenadas localizada no rotor como:
( ) ( ) cos( ) 0 ( ) ( ) cos( )
cos( ) ( ) ( ) 0 cos( ) ( ) ( )
cos( ) 0 1 cos( )
sen sen sen sen
w sen sen sen sen
φ θ φ φ φ ψ θ θ ψ
φ θ φ θ φ ψ θ θ ψ
θ ψ φ θ ψ
+ = − = − +
& & &
& & &
&& &
(5.16)
Por causa da simetria do rotor em torno de X3, seu produto de inércia é igual a
zero, e ixx=iyy. O tensor de inércia do rotor no sistema de coordenadas do rotor é:
103
0 0
0 0
0 0
xx
yy
zz
i
I i
i
θθ
=
(5.17)
Sendo o centro de massa do rotor fixo, a energia cinética pode ser escrita por:
( ) ( )22 2 21 1
( ) cos( )2 2
T
C xx zzE w I w i sen iθθ φ θ θ φ θ ψ = = + + +
& & & & (5.18)
Usando os ângulos de Euler φ, θ, ψ como coordenadas generalizadas do rotor, a
equação de movimento do rotor pode ser descrita por:
C CE EdM
dtφ
φ φ
∂ ∂− =
∂ ∂ & (5.19)
C CE EdM
dtθ
θ θ
∂ ∂ − =
∂ ∂ & (5.20)
C CE EdM
dtψ
ψ ψ
∂ ∂− =
∂ ∂ & (5.21)
Onde, Mφ, Mθ e Mψ são as componentes dos torques generalizados aplicados,
associados com os ângulos φ, θ e ψ respectivamente. Utilizando a equação da energia
cinética descrita na Equação 5.18, a equação de movimento do rotor é:
( )2 ( ) cos( ) cos( )xx zz
di sen i M
dtφφ θ φ θ ψ θ + + =
& & & (5.22)
( )2 ( )cos( ) cos( ) ( )xx xx zzi i sen i sen Mθθ φ θ θ φ θ ψ φ θ− + + =&& & & && (5.23)
( )cos( )zz
di M
dtψφ θ ψ + =
& & (5.24)
A seguir, importantes casos serão discutidos.
104
5.2.1 Coordenadas Ignoráveis
De acordo com as Equações 5.22 e 5.24, pode ser visto que a energia cinética do
rotor não é uma função explicita dos ângulos de precessão e spin φ e ψ, sendo:
0C CE E
φ ψ
∂ ∂= =
∂ ∂
Se os torques Mφ e Mψ são iguais a zero, a equação de Lagrange vistas nas
Equações 5.19 e 5.21 produz:
0d T
dt φ
∂=
∂ & (5.25)
0d T
dt ψ
∂=
∂ & (5.26)
ou
1CE
dφ
∂=
∂ & (5.27)
2CE
dψ
∂=
∂ & (5.28)
Onde d1 e d2 são constantes. Neste caso especial, os ângulos de precessão e spin φ
e ψ são chamados de coordenadas ignoráveis dês de que os momentos generalizados
associados com estas coordenadas são conservados [23]. O uso das Equações 5.27 e
5.28 produzem as seguintes integrais de movimento:
( )21( ) cos( ) cos( )xx zzi sen i dφ θ φ θ ψ θ + + =
& & & (5.29)
( ) 2cos( )zzi dφ θ ψ + = & & (5.30)
105
Onde as constantes d1 e d2 podem ser determinadas usando as condições iniciais
existentes no sistema. Neste caso especial, a equação para o ângulo de nutação θ pode
ser escrito como:
22( ) cos( ) ( )xxi sen d sen Mθθ φ θ θ φ θ − + =
&& & & (5.31)
5.2.2 Precessão em regime constante
Considere o caso especial onde o rotor gira constantemente com uma velocidade
de precessão e com um ângulo constante θ. Com uma velocidade de spin constante,
estas condições podem ser descritas por:
.
.
.
cons
cons
cons
φ
θ
ψ
=
=
=
&
&
0
0
0
φ
θ θ
ψ
=
= =
=
&&
& &&
&&
(5.31)
Nestas condições as Equações 5.22, 5.23 e 5.34 podem ser reduzidas para:
0Mφ = (5.32)
( )2 ( ) cos( ) ( )zz xx zzM i i sen i senθ φ θ θ ψφ θ= − +& && (5.33)
0Mψ = (5.34)
É claro que neste caso, Mφ e Mψ são iguais a zero, e o único torque diferente de
zero atuando no rotor é o torque associado com o ângulo de nutação θ. O eixo deste
torque é perpendicular ao eixo de precessão. Se assumirmos que:
2πθ =
O torque Mθ se apresenta em sua forma mais simples
zzM iθ ψφ= && (5.35)
106
5.3 Forças Atuantes na Cintura
A modelagem do dispositivo de equilíbrio do bípede será caracterizada por um
giroscópio localizado no centro da cintura. Considerando a cintura como um sistema,
existem as forças de reação do fêmur sobre a cintura que o giroscópio atuará
promovendo o equilíbrio.
Conhecida a dinâmica do bípede, as forças atuantes na cintura podem ser
encontradas. A Figura 5.3 mostra os ângulos da juntas do bípede em relação a um
referencial inercial.
Figura 5.3: Ângulos absolutos das juntas.
Os ângulos absolutos podem ser encontrados seguindo as seguintes expressões.
107
1 1
2 1 2
3 1 2 3
4 1 2 3 4
5 1 2 3 4 5
6 1 2 3 4 5 6
7 1 2 3 4 5 6 7
ϕ θ
ϕ θ θ
ϕ θ θ θ
ϕ θ θ θ θ
ϕ θ θ θ θ θ
ϕ θ θ θ θ θ θ
ϕ θ θ θ θ θ θ θ
=
= −
= − +
= − + −
= − + − −
= − + − − +
= − + − − + +
(5.36)
A Figura 5.4 mostra o diagrama de corpo livre existente nos corpos do bípede
desenvolvidos para encontrar as forcas existentes nas juntas do robô.
Figura 5.4: Diagrama de corpo livre dos corpos do bípede.
De acordo com a Figura 5.4 têm-se as seguintes equações para os corpos 2 e 3
respectivamente:
108
12 32 2
12 32 2
12 2 12 2 32 2
32 2 2 3 2
( ) cos( ) ( )2 2 2
cos( )2
x x t x
y y t t y
t t tx y x
ty t
F F m a
F F m g m a
l l lF sen F F sen
lF T T I
ϕ π ϕ ϕ
π ϕ α
− =
− − =
+ − + +
+ − + − =
(5.37)
32 3 3
32 3 3
32 3 32 3 3 3
3 3 3 4 3
( ) cos( ) ( )2 2 2
cos( )2
x cx f x
y cy f f y
f f f
x y cx
f
cy f
F F m a
F F m g m a
l l lF sen F F sen
lF T T I
ϕ π ϕ ϕ
π ϕ α
− =
− − =
+ − + +
+ − + − =
(5.38)
Rearmando as Equações 5.37 e 5.38 na forma matricial, elas se apresentam na
seguinte forma:
23 23A F R× = (5.39)
Sendo:
2 2 2 2
3 3 3 3
1 0 1 0 0 0
0 1 0 1 0 0
( ) cos( ) ( ) cos( ) 0 02 2 2 2
0 0 1 0 1 0
0 0 0 1 0 1
0 0 ( ) cos( ) ( ) cos( )2 2 2 2
t t t t
f f f f
l l l lsen sen
A
l l l lsen sen
ϕ ϕ ϕ ϕ
ϕ ϕ ϕ ϕ
− −
− − =
− −
− −
12
12
32
2332
3
3
x
y
x
y
cx
cy
F
F
FF
F
F
F
=
109
2
2
2 2 3
233
3
3 3 4
t x
t y t
t
f x
f y f
f
m a
m a m g
I T TR
m a
m a m g
I T T
α
α
+ − +
= +
− +
Para encontrar as forças atuantes nas juntas dos corpos 2 e 3, tem-se:
123 23F A R−= × (5.40)
O diagrama de corpo livre dos corpos 4 e 5, mostrados na Figura 5.4 fornecerão as
seguintes equações:
65 45 5
65 45 5
65 5 65 5 45 5
45 5 6 5 5
( ) cos( ) ( )2 2 2
cos( )2
x x t x
y y t t y
t t tx y x
ty t
F F m a
F F m g m a
l l lF sen F F sen
lF T T I
ϕ ϕ ϕ
ϕ α
− =
− − =
+ + +
+ + − =
(5.41)
45 4 4
45 4 4
45 4 45 4 4 4
4 4 5 4 4
( ) cos( ) ( )2 2 2
cos( )2
x cx f x
y cy f f y
f f f
x y cx
f
cy f
F F m a
F F m g m a
l l lF sen F F sen
lF T T I
ϕ ϕ ϕ
ϕ α
− =
− − =
+ + +
+ + − =
(5.42)
Resolvendo o sistema na forma matricial para achar as forças atuantes nas juntas
dos corpos 4 e 5 tem-se:
145 45F B R−= × (5.43)
Onde
110
5 5 5 5
4 4 4 4
1 0 1 0 0 0
0 1 0 1 0 0
( ) cos( ) ( ) cos( ) 0 02 2 2 2
0 0 1 0 1 0
0 0 0 1 0 1
0 0 ( ) cos( ) ( ) cos( )2 2 2 2
t t t t
f f f f
l l l lsen sen
B
l l l lsen sen
ϕ ϕ ϕ ϕ
ϕ ϕ ϕ ϕ
− − =
− −
65
65
45
4545
4
4
x
y
x
y
cx
cy
F
F
FF
F
F
F
=
5
5
5 6 5
454
4
4 5 4
t x
t y t
t
f x
f y f
f
m a
m a m g
I T TR
m a
m a m g
I T T
α
α
+ − +
= +
− +
Os torques apresentados são os torques referentes aos atuadores nas juntas
encontrados no capítulo da dinâmica. As acelerações de cada corpo já foram
encontradas pela cinemática, onde:
nx
n n Tn Tn
ny
aa P J q J q
a
= = = +
&& & & && 1, 2,...,7n = (5.44)
n Rn Rnw J q J qα = = +&& & && 1, 2,...,7n = (5.45)
Assim, as forças que o corpo 3 que se refere ao fêmur em suporte fará na cintura
(força suporte Fs=-F3cy), e as forças do corpo 4 referente ao fêmur que estará em
balanço (força balanço Fb=-F4cy) farão, são ilustradas em função do tempo na Figura 5.5
para um passo de 0,4m e em um meio período de um ciclo de 2 segundo.
111
Figura 5.5: Forças no eixo y atuantes na cintura.
Diminuindo mais uma vez a frequência do ciclo, as forças atuantes na cintura
também têm uma busca redução. A Figura 5.6 apresenta as forças atuantes na cintura
para um passo de 0,4m e um meio período de um ciclo de 4 segundos.
Figura 5.6: Forças no eixo y atuantes na cintura para um meio período de 2s.
112
Devido aos maiores torques na perna de apoio, a força exercida na cintura,
principalmente no final do ciclo onde a distância ate o referencial inercial é maior,
produz as maiores forças nas juntas. Conhecido as forças nas juntas da cintura, a mesma
é considerada como um corpo de modo a controlar a estabilidade do bípede com um
giroscópio localizado no centro da cintura.
Considerando que o bípede se desenvolve em duas dimensões, os torques no
eixo y na cintura, provenientes das forças no eixo x serão anuladas pelas restrições
existentes nas juntas da cintura com o fêmur que possui apenas um grau de liberdade
que é a rotação em z.
O giroscópio modelado como um disco fino será acoplado à cintura no seu centro.
O modelo do corpo da cintura é ilustrado na Figura 5.7, bem como seus eixos de
referência localizados no seu centro de massa.
Figura 5.7: Giroscópio no centro da cintura.
O disco terá uma massa de 10kg com um raio de 0,1m e a cintura uma massa de
10kg resultando em uma massa total na cintura igual à fornecida por [17]. O tamanho
do disco foi escolhido de modo a ser proporcional com o tamanho do sistema. O
113
comprimento da cintura é de 0,1m como dito na Tabela 2.1. O momento de inércia do
disco em z (Izd) pode ser calculado como:
21
2zdI mr=
20,05zdI kgm= (5.46)
O momento de inércia da cintura no eixo z é de 1,02kgm2. Somado com a do
giroscópio, o momento de inércia total da cintura, em relação ao centro de massa
localizado no giroscópio é de 1,07kgm2. Considerando que o giroscópio irá girar com
velocidade de spin constante, e o ângulo de nutação será de 90o, a equação do
giroscópio, resolvendo as equações 5.22, 5.23 e 5.24, será:
xxM iφ φ= && (5.47)
zzM iθ ψφ=∑ && (5.48)
Conhecido o momento de inércia, os momentos atuantes na cintura serão
provocados pelas forças de reação das pernas atuantes na cintura. Assim o somatório
dos momentos pode ser descrito por:
( )2 2 2cin cin cin
s b s b
l l lM F F F Fθ = − = −∑ (5.49)
Uma vez que o momento Mθ seja constante, não haverá aceleração no eixo de
precessão resultando na equação 5.47 igual a zero. No caso deste modelo, o momento
no eixo x Mθ não é uma função constante no tempo, pois as forças atuantes na cintura
não são. Isto significa que haverá um momento no eixo y que é o eixo de precessão.
A Figura 5.8 ilustra o torque no eixo x produzido pelas forças atuantes nas juntas
da cintura para um passo de 0,4m e um período de 2 segundos.
114
Figura 5.8: Torques exercidos no eixo x da cintura ao longo de meio período.
Assim, para cada velocidade de spin dada ao giroscópio, uma velocidade angular
de precessão no eixo y será imposta ao sistema. Sendo:
( )2
cin s b
zz
l F F
iφ
ψ
−=&
& (5.50)
Os vetores da velocidade de precessão (wp=φ& ), da velocidade de spin (ws=ψ& ) e
do torque provocados pelas reações dos fêmures na cintura (Mθ) são ilustrados na
Figura 5.9.
115
Figura 5.9: Os vetores de precessão, spin e do torque Mθ.
Algumas velocidades de precessão são dadas no tempo para determinadas
velocidades de spin, e apresentadas na Figura 5.10.
Figura 5.10: Velocidade de precessão para diferentes velocidades de spin.
116
O vetor velocidade de precessão estará no eixo y. A restrição a rotação neste
eixo provocada pelas juntas rotacionais localizadas nas juntas do fêmur com a cintura
impedirão a rotação da cintura, estabilizando o sistema. Entretanto, para um sistema
real, quanto maior a velocidade de precessão, maior a possibilidade da desestabilização
do sistema. Considerando a velocidade de precessão inversamente proporcional à
velocidade de spin, quanto maior a mesma, menor será a rotação imposta no eixo y.
117
6. Conclusão e Trabalhos Futuros
A movimentação do modelo adotado neste trabalho ocorre no plano sagital. Foram
consideradas as principais determinantes do modo de andar neste plano, sendo eles: o
compasso, a flexão do joelho e a flexão plantar. O modelo adotado simplifica o ciclo da
marcha dando bons resultados para futuros estudos como o controle. Os movimentos
suprimidos no modelo, apesar de terem funções fisiológicas não interferem no ciclo da
marcha, resultando em um modelo que oferece bons resultados, tanto para cinemática,
dinâmica, ou para outros estudos mais aprofundados.
As funções dos ângulos das juntas foram estudadas para um deslocamento
contínuo. A velocidade do bípede pode ser controlada pela freqüência do ciclo, ou pelo
passo desejado, alterando automaticamente todas as funções dos ângulos das juntas. A
velocidade da cintura permaneceu próxima de um valor constante para diferentes
condições do sistema como o passo ou o período do ciclo, resultando em menos
desperdício de energia.
Na dinâmica, a influência das duas variáveis que podem determinar a velocidade
do bípede, ou seja, o passo e a freqüência do ciclo se mostraram importantes nos valores
dos torques nas juntas. Uma vez que o valor do passo é diminuído, os valores referentes
aos torques provocados pela força da gravidade também diminuirão, atenuando também
os torques motores necessários para o movimento. As acelerações dos ângulos dos
joelhos e da cintura são funções diretas do quadrado da frequência do ciclo,
influenciando na intensidade dos torques relativos às forças de inércia. Ambos podem
ser controlados para uma melhor economia de energia em uma determinada velocidade,
diminuído os torques motores.
Os resultados do mecanismo giroscópio para a função de equilíbrio se mostraram
satisfatórios. As restrições localizadas nas juntas rotacionais serão suficientes para
impedir a rotação dada pela velocidade de precessão ao longo deste eixo para baixas
velocidades de precessão em um sistema real.
Sem dúvida, em um modelamento nos três planos, o plano principal seria o plano
sagital. É nele onde acontece à maioria dos movimentos e os mais importantes.
Contudo, os movimentos nos outros dois planos também têm suas importâncias em uma
118
análise nas três dimensões e, em um estudo mais profundo do movimento, os três planos
devem ser utilizados. Um estudo da locomoção de um robô bípede desenvolvido nos
três planos seria bem mais complexo, pois aumentaria significativamente seus graus de
liberdade em comparação com o deste trabalho, entretanto, os resultados teriam muitos
benefícios para certas áreas de estudo. Um exemplo seria na parte da reabilitação da
marcha em homens amputados, onde os movimentos nos planos frontal e transverso não
podem ser desprezados.
Neste trabalho não foram considerados valores relativos ao impacto e atrito nas
análises realizadas. Mesmo sendo o assunto importante, a complexidade deste
fenômeno torna-o muitas vezes sem solução. Em trabalhos futuros, tais fenômenos
devem ser levados em consideração.
Um dos maiores benefícios do robô bípede é que, não necessariamente, o terreno
deve ser continuo. Estudos da locomoção em diferentes solos é uma boa opção de
trabalhos futuros. A colisão é outro fator importante a ser estudado. Cada vez que um pé
toca no solo, forças adicionais são aplicadas que altera toda a dinâmica dos corpos do
sistema. Quanto maior for o passo dado pelo bípede, maiores serão os torques de reação
provocados pelo impacto, causados pelo aumento das distancias até os eixos de
referencia. Maiores frequências também resultarão em maiores forças de impacto,
resultadas das acelerações dos corpos.
A análise das funções de posição dos ângulos das juntas, em uma movimentação
em três dimensões aumenta o numero de graus de liberdade e conseqüentemente da
complexidade existente no sistema. O modo de vincular às equações de modo a manter
o ciclo o mais suave possível é um bom exemplo da complexidade existente no
problema. Outro estudo que pode ser proposto nesta área é na fase transiente de um
ciclo, quando o bípede possui uma velocidade qualquer em sua cintura e deseja acelerar
ou frear, dependendo de sua vontade.
As forças de atritos existentes no tipo de solo também exercem significativas
influencias no estudo deste tipo de locomoção. O atrito é responsável pelo
deslocamento, e pela restrição da rotação de precessão imposta pelo giroscópio. Estudos
nesta área também têm bastante relevância para a locomoção dos bípedes.
119
Neste trabalho, foi considerado que as localizações dos centros de massa de cada
corpo estariam sempre nos centros geométricos dos corpos. Em diferentes lugares
resultariam em diferentes torques motores, onde teriam um lugar ótimo para a maior
redução dos mesmos.
Em relação ao mecanismo de equilíbrio, quanto maior a velocidade de precessão,
maior a possibilidade da desestabilização do sistema. Considerando a velocidade de
precessão inversamente proporcional a velocidade de spin, quanto maior a velocidade
de spin menor será a rotação imposta no eixo de precessão. Em um estudo nos três
planos, as forças de reação existentes na cintura no eixo x também teriam que ser
levadas em consideração, uma vez que resultariam em um torque no eixo y que também
teriam que ser estabilizadas.
120
REFERÊNCIAS BIBLIOGRÁFICAS
[1] PRATT, J. E., Virtual Model Control of Bipedal Walking Robot. M.Sc., dissertation,
Massachusetts Institute of technology, USA, 1995.
[2] PRATT, J. E., Exploiting Inherent Robustness and Natural Dynamics in the Control
of Bipedal Walking Robots. D.Sc., Tese, Massachusetts Institute of technology, USA,
2000.
[3] RAIBERT, M. H., Legged robots, Communications of the ACM, 29(6): 499-514.
1986.
[4] VUKOBRATOVIC, M., BOROVAC, B., SURLA, D., STOVIC, D., Biped
Locomotion, Springer Verlag, Berlin, 1990.
[5] SADEGHI, H., PRINCE, F., ZABJEK, K.F., SADEGHI, S., LABELLE, H., “Knee
flexors / extensors in gait of elderly and young able-bodied men (II)”, The Knee, (9) pp.
55 – 63, 2002.
[6] SADEGHI, H.S., SADEGHI, S., PRINCE, F., ALLARD, P., LABELLE, H.,
VAUGHAN, C.L., “Functional roles of ankle and hip sagital muscle moments in able-
bodied gait”, Journal of Biomechanics, (16) pp. 688 – 695, 2001.
[7] RAPTOPOULOS, L. S. C., Estudo e desenvolvimento de equipamento de baixo
custo para análise da marcha de amputados, Tese - Universidade Federal do Rio de
Janeiro, COPPE. 2003
[8] SILVA, F. L., Estudo para o Desenvolvimento de uma Prótese Trans-femoral,
dissertação de M.Sc., Universidade Federal do Rio de Janeiro, COPPE. 2007.
[9] REGO, G. S., DUTRA, M. S. “Cinemática da Locomoção de Robôs bípedes”.
Congresso Nacional de Engenharia Mecânica, Salvados, 2008.
[10] MACKAY-LYONS, M., 2002, “Central Pattern Generation of Locomotion: A
review of the evidence”, Physical Therapy, vol. 82(1), 69-83.
121
[11] VAUGHAN, C. L., DAVIS, B. L., O’CONNOR, J.C., Dynamics of Human Gait.
2 ed. Cape Town, Christopher L Vaughan, 1992.
[12] PINA FILHO, A. C., Estudos de Osciladores Não-Lineares Mutuamente
Acoplados Aplicados na Locomoção de um Robô bípede. Tese de D.Sc. COPPE/UFRJ,
RJ, Brasil, 2005.
[13] INMAN, V.T., RALSTON, H.J., TODD, F., Human Walking. In:
LIEBERMAN, J.C., (Ed.), Williams & Wilkins, Baltimore, London, 1981.
[14] PERRY, J., Gait Analysis: Normal and Pathological Function, Slack, USA,
1992.
[15] SAUNDERS, J.B., INMAN, V., EBERHART, H., The Major Determinants in
Normal and Pathological Gait, J. Bone Jt. Surgey 35A, 1953.
[16] McMAHON, T.A., Muscles, Reflexes and Locomotion. USA, Princeton University
Press, 1984.
[17] DUTRA, M. S., Bewegungskoordination und Steuerung einer zweibeinigen
Gehmaschine. Aachen, Germany, Shaker Verlag. 1995.
[18] ZHENG, Y. F., HEMAMI, H., Inpact Effects of Biped Contact with the
Environment, IEEE Transactions on systems. Man and Cybernetics, Vol. 14, pp437-
443. 1984.
[19] GOSWAMI, A., THUILOT, B., ESPIAU, B., Compass-like biped robot. Part I:
Stability and bifurcation of passive gaits, Institute National de Recherché en
Informatique et en Automatique – INRIA, n. 2996. 1996.
[20] HILLER, H.M., KECSKEMÉTHY, A., WOERNLE, C., Computer-Aided Kinematics and Dynamics of Multibory systems, Universität GH, Duisburg. 1990.
[21] DENAVIT, J., HARTENBERG, R. S., A Kinematic Notation for Lower-Pair
Mechanisms Based on Matrices, ASME journal of Applied Mechanics, pp. 215-221. 1955.
[22] SICILIANO, B., SCIAVICCO, L., Modeling and Control of a robot
manipulators. 2 ed., Springer, USA, 2007.
122
[23] HILLER, M., WOERNLE, C., The Characteristic Pair of Joints: An Effective
Approach for the Inverse Kinematic Problem of Robots. IEEE, pp.846-851. 1988.
[24] BRAUNE,W.; FISCHER, O., Der Gang des Menschen. K. S. Gesellssc. D.
Wisensch., Leipzing, Originalaugabe. 1894.
[25] OPPENHEIM, A.V., Signals and Systems. 1ed. Prentice Hall, 1996.
[26] SHABANA, A. A., Computational Dynamics. 2 ed., John Wile and Son, New
York, USA, 2001
[27] WITTENBURG, J., Dynamics of multibody systems, 2ed. Ed Springer, Germany,
1977.
[28] GREBOGI, C., POON, L., SAUER, T., YORKE, J. A., AUERBACH, D.,
Handbook of Dynamical Systems, Vol. 2, Cap. 7, Elsevier, Amsterdam. 2002
[29] WESTERVELT, E. R., GRIZZLE, J. W., CHEVALLEREAU, C., CHOI, J. H.,
MORRIS, B., Feedback Control of Dynamic Bipedal Robot Locomotion, Taylon &
Francis Group, New York, USA, 2007.
BIBLIOGRAFIA NÃO REFERENCIADA
[30] CARSON, M.C., HARRINGTON, M.E., THOMPSON, N., O’CONNOR, J.J.,
“Kinematic analysis of a multi-segment foot model for research and clinical
applications: a repeatability analysis”, Journal of biomechanics, (34) pp. 1299 – 1307,
2001.
[31] DUTRA, M. S. PINA FILHO, A. C. de, ROMANO, V.F., Modelling of a bipedal
locomotor using coupled nonlinear oscillators of Van der Pol, Biological Cybernetics,
vol. 88 (4), 2003.
[32] PINA FILHO, A. C. de, DUTRA, M. S., RAPTOPOULOS, L. S. C., Modeling of a
Bipedal Robot Using Mutually Coupled Rayleigh Oscillators, Biological Cybernetics,
92(1), 1-7, 2005.
123
[33] EBERHART, H.D., “Fundamental studies of human locomotion and other
information relating to design of artificial limbs”, Subcontractor’s Report to National
Council, Berkeley, California, 1947.
[34] RAPTOPOULOS, L. S. C, DUTRA, M. S., D`ANGELO, M. D., “Dynamic model
of normal human locomotion”, 17th Congresso Brasileiro de Engenharia Mecânica –
COBEM2003, São Paulo, Novembro, 2003.
[35] RAPTOPOULOS, L. S. C., DUTRA, M. S., D`ANGELO, M. D., “Cinemática do
sistema locomotor humano: modelo mecânico e cinemática inversa”, CONEM2002,
João Pessoa, Paraíba, 2002.
[36] RAPTOPOULOS, L. S. C., DUTRA, M. S., D`ANGELO, M. D., “Kinematics
Analysis of Human Locomotion”, 16th Congresso Brasileiro de Engenharia Mecânica –
COBEM2001, Uberlândia – MG, Novembro, 2001.
[37] ROSE, J., GAMBLE, J.G., Human Walking, Second Edition, Williams &
Wilkins, 1994.
[38] FRENCH, A. P., Vibrations and Waves, W. W. Norton & Company, 1971.
[39] GAO, J., Ein Beitragsszur sto freien Gehbewegung, VDI Verlag, Düsseldorf,
Germany. 1992
[40] MEIROVITCH, L., Elements of Vibration Analysis”, McGraw-Hill, USA, 1975.
[41] WINTER, D., “Biomechanical motor patterns in normal walking”, J. Motor
Behave., vol. 15, no. 4, pp. 302-330, 1983.
[42] CARMO, J., NASCIMENTO, A.O., COSTA, J.C., ROCHA, A.F.,
“Instrumentação para aquisição e avaliação das forças exercidas nos pedais por
ciclistas”, Brasilian Journal of Biomechanics, n. 3, pp. 31 – 37, 2001.
[43] CAPPOZZO, A., “Gait analysis methodology”, Human Movement Science, (3) pp.
27 – 54, 1984.
124
[44] ANDRIACCHI, T.P., ALEXANDER, E.J., “Studies of human locomotion: past,
present and future”, Journal of Biomechanics, (33) pp. 1217 – 1224, 2000.
[45] ANDRIACCHI, T.P., OGLE, J.A., GALANTE, J.O., “Walking speed as a basis for
normal and abnormal gait measurements”, Journal of Biomechanics, (10) pp. 261 – 268,
1977.
125
Anexo A. Programação do Cálculo da Cinemática
Desenvolvido no MatLab
clear all clc %%%%% CINEMATICA DIRETA - FASE BALANÇO %%%%%%% %%%%%%% PLANO - 7 G.D.L %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%% %%%% Constantes %%%% Ccorpo = 0.4; lfe = 0.37; lti = 0.37; lpe = 0.11; lde = 0.03; %% parâmetros DH %% a1 = lpe; a2 = lti; a3 = lfe; a4 = lfe; a5 = lti; a6 = lpe; a7 = lde; %%% d1,...,d7 = 0 %%%%%% %%% alpha1,...,alpha7 = 0 %%%%%% %% Matriz DH %% Ha=sym('[cos(theta),-sin(theta),0,0;sin(theta),cos(theta),0,0;0,0,1,D;0,0,0,1]'); Hb=sym('[1,0,0,A;0,cos(alpha),-sin(alpha),0;0,sin(alpha),cos(alpha),0;0,0,0,1]'); H=Ha*Hb; %% transformação 7-6 %% H76 = subs(H,'D',0); H76 = subs(H76,'alpha',0); H76 = subs(H76,'A',a7); H76 = subs(H76,'theta','th7');
126
H76 = simplify(H76); %% transformação 6-5 %% H65 = subs(H,'D',0); H65 = subs(H65,'alpha',0); H65 = subs(H65,'A',a6); H65 = subs(H65,'theta','th6'); H65 = simplify(H65); %% transformação 5-4 %% H54 = subs(H,'D',0); H54 = subs(H54,'alpha',0); H54 = subs(H54,'A',a5); H54 = subs(H54,'theta','- th5'); H54 = simplify(H54); %% transformação 4-3 %% H43 = subs(H,'D',0); H43 = subs(H43,'alpha',0); H43 = subs(H43,'A',a4); H43 = subs(H43,'theta','- th4'); H43 = simplify(H43); %% transformação 3-2 A %% H32 = subs(H,'D',0); H32 = subs(H32,'alpha',0); H32 = subs(H32,'A',a3); H32 = subs(H32,'theta','th3'); H32 = simplify(H32); %% transformação 2-1 %% H21 = subs(H,'D',0); H21 = subs(H21,'alpha',0); H21 = subs(H21,'A',a2); H21 = subs(H21,'theta','- th2'); H21 = simplify(H21); %% transformação 1-0 %% H10 = subs(H,'D',0);
127
H10 = subs(H10,'alpha',0); H10 = subs(H10,'A',a1); H10 = subs(H10,'theta','th1'); H10 = simplify(H10); %% posição dos pontos principais %% Tcal = H10; Tjoe = H10*H21; Tcin = H10*H21*H32; TjoB = H10*H21*H32*H43; TcaB = H10*H21*H32*H43*H54; TpeB = H10*H21*H32*H43*H54*H65; TdeB = H10*H21*H32*H43*H54*H65*H76; %%% EXEMPLO %%% tht1 = pi; tht2 = pi/2; tht3 = 0; tht4 = pi; tht5 = 0; tht6 = pi/2; tht7 = 0; Tcal = subs(Tcal,'th1',tht1); Tjoe = subs(Tjoe,'th1',tht1); Tjoe = subs(Tjoe,'th2',tht2); Tcin = subs(Tcin,'th1',tht1); Tcin = subs(Tcin,'th2',tht2); Tcin = subs(Tcin,'th3',tht3); TjoB =subs(TjoB,'th1',tht1); TjoB =subs(TjoB,'th2',tht2); TjoB =subs(TjoB,'th3',tht3); TjoB =subs(TjoB,'th4',tht4); TcaB =subs(TcaB,'th1',tht1); TcaB =subs(TcaB,'th2',tht2); TcaB =subs(TcaB,'th3',tht3); TcaB =subs(TcaB,'th4',tht4); TcaB =subs(TcaB,'th5',tht5); TpeB =subs(TpeB,'th1',tht1); TpeB =subs(TpeB,'th2',tht2); TpeB =subs(TpeB,'th3',tht3); TpeB =subs(TpeB,'th4',tht4); TpeB =subs(TpeB,'th5',tht5); TpeB =subs(TpeB,'th6',tht6); TdeB =subs(TdeB,'th1',tht1);
128
TdeB =subs(TdeB,'th2',tht2); TdeB =subs(TdeB,'th3',tht3); TdeB =subs(TdeB,'th4',tht4); TdeB =subs(TdeB,'th5',tht5); TdeB =subs(TdeB,'th6',tht6); TdeB =subs(TdeB,'th7',tht7); axis([-0.7 0.7 0 1.2]) %%% GRÁFICO %%%% line([lde,0],[0,0],[0,0]) line([0,Tcal(1,4)],[0,Tcal(2,4)],[0,Tcal(3,4)]) line([Tcal(1,4),Tjoe(1,4)],[Tcal(2,4),Tjoe(2,4)],[Tcal(3,4),Tjoe(3,4)]) line([Tjoe(1,4),Tcin(1,4)],[Tjoe(2,4),Tcin(2,4)],[Tjoe(3,4),Tcin(3,4)]) line([Tcin(1,4),TjoB(1,4)],[Tcin(2,4),TjoB(2,4)],[Tcin(3,4),TjoB(3,4)]) line([TjoB(1,4),TcaB(1,4)],[TjoB(2,4),TcaB(2,4)],[TjoB(3,4),TcaB(3,4)]) line([TcaB(1,4),TpeB(1,4)],[TcaB(2,4),TpeB(2,4)],[TcaB(3,4),TpeB(3,4)]) line([TpeB(1,4),TdeB(1,4)],[TpeB(2,4),TdeB(2,4)],[TpeB(3,4),TpeB(3,4)]) %%%%%%%% FIM %%%%%%%%%%%
129
clear all clc %%%%% CINEMÁTICA INVERSA - FASE SUPORTE DUPLO %%%%%%% %%%%%%% PLANO - 3 G.D.L %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%% %% ponto do Calcanhar %% %%% P = Passo %%% P = .34; Cin = 0.1; lfe = 0.37; lti = 0.37; lpe = 0.11; lde = 0.03; %%%%%%%%%%% CG %%%%%%%%%%%%%%% th1 = pi; syms th3 th4 th5 %%%%%%%%% memória de calculo %%%%%%%%%% a1 = (th4/2)-th3; a2 = (th4/2)+th5; d = sqrt(2*(lfe^2)*(1+cos(th4))); dQ = 2*(lfe^2)*(1+cos(th4)); X = d-lti*(cos(a2)+cos(a1)); Y = lti*(sin(a1)-sin(a2)); d3 = sqrt(X^2+Y^2); d3Q = X^2+Y^2; %%%%%%%%%%%%%%%%%%%%%%% %% theta 7 %%%%%% %%%%%%%%%%%%%%%%%%%%%%% A =(P^2 + lpe^2 - d3Q)/(2*lpe*(P)); th7 = pi - acos(A); %%%%%%%%%%%%%%%%%%%%%%% Bta2 = acos((d3^2 + lpe^2 - P^2)/(2*d3*lpe)); Bta3 = acos((d3Q - dQ)/(2*lti*d3)+ d/d3*cos(a1)); %%%%%%%%%%%%%%%%%%%%%%%
130
%% theta 6 %%%%%% %%%%%%%%%%%%%%%%%%%%%%% th6 = pi - Bta2 - Bta3; %%%%%%%%%%%%%%%%%%%%%% %% theta 2 %%%%%% %%%%%%%%%%%%%%%%%%%%%%% th2 = pi + th3 + th6 + th7 - (th4 + th5); %%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Exemplo Posição %% th3 = (30*pi)/180; th4 = pi+(30*pi)/180; th5 = (30*pi)/180; tht1 = th1; tht2 = eval(th2); tht6 = eval(th6); tht7 = eval(th7); Pcal = [lpe*cos(tht1);lpe*sin(tht1);0]; Pjoe = [lti*cos(tht1-tht2)+ lpe*cos(tht1);lti*sin(tht1-tht2)+ lpe*sin(tht1);0]; Pc = [lfe*cos(tht1-tht2+th3) + lti*cos(tht1-tht2)+ lpe*cos(tht1);lfe*sin(tht1-tht2+th3)+lti*sin(tht1-tht2)+ lpe*sin(tht1);Cin]; PjoB = [lfe*cos(tht1-tht2+th3-th4)+ lfe*cos(tht1-tht2+th3) + lti*cos(tht1-tht2)+ lpe*cos(tht1);lfe*sin(tht1-tht2+th3-th4) + lfe*sin(tht1-tht2+th3)+lti*sin(tht1-tht2)+ lpe*sin(tht1);Cin]; PcaB = [lti*cos(tht1-tht2+th3-th4-th5) + lfe*cos(tht1-tht2+th3-th4)+ lfe*cos(tht1-tht2+th3) + lti*cos(tht1-tht2)+ lpe*cos(tht1);lti*sin(tht1-tht2+th3-th4-th5)+ lfe*sin(tht1-tht2+th3-th4) + lfe*sin(tht1-tht2+th3)+lti*sin(tht1-tht2)+ lpe*sin(tht1);Cin]; PpeB = [lpe*cos(tht1-tht2+th3-th4-th5+tht6)+lti*cos(tht1-tht2+th3-th4-th5) + lfe*cos(tht1-tht2+th3-th4)+ lfe*cos(tht1-tht2+th3) + lti*cos(tht1-tht2)+ lpe*cos(tht1);lpe*sin(tht1-tht2+th3-th4-th5+tht6)+lti*sin(tht1-tht2+th3-th4-th5)+ lfe*sin(tht1-tht2+th3-th4) + lfe*sin(tht1-tht2+th3)+lti*sin(tht1-tht2)+ lpe*sin(tht1);Cin]; PdeB = [lde*cos(tht1-tht2+th3-th4-th5+tht6+tht7)+lpe*cos(tht1-tht2+th3-th4-th5+tht6)+lti*cos(tht1-tht2+th3-th4-th5) + lfe*cos(tht1-tht2+th3-th4)+ lfe*cos(tht1-tht2+th3) + lti*cos(tht1-tht2)+ lpe*cos(tht1);lde*sin(tht1-tht2+th3-th4-th5+tht6+tht7)+lpe*sin(tht1-tht2+th3-th4-th5+tht6)+lti*sin(tht1-tht2+th3-th4-th5)+ lfe*sin(tht1-tht2+th3-th4) + lfe*sin(tht1-tht2+th3)+lti*sin(tht1-tht2)+ lpe*sin(tht1);Cin]; line([lde,0],[0,0],[0,0]) line([0,Pcal(1,1)],[0,Pcal(2,1)],[0,Pcal(3,1)]) line([Pcal(1,1),Pjoe(1,1)],[Pcal(2,1),Pjoe(2,1)],[Pcal(3,1),Pjoe(3,1)])
131
line([Pjoe(1,1),Pc(1,1)],[Pjoe(2,1),Pc(2,1)],[0,0]) line([Pc(1,1),Pc(1,1)],[Pc(2,1),Pc(2,1)],[0,Pc(3,1)]) line([Pc(1,1),PjoB(1,1)],[Pc(2,1),PjoB(2,1)],[Pc(3,1),PjoB(3,1)]) line([PjoB(1,1),PcaB(1,1)],[PjoB(2,1),PcaB(2,1)],[PjoB(3,1),PcaB(3,1)]) line([PcaB(1,1),PpeB(1,1)],[PcaB(2,1),PpeB(2,1)],[PcaB(3,1),PpeB(3,1)]) line([PpeB(1,1),PdeB(1,1)],[PpeB(2,1),PdeB(2,1)],[PpeB(3,1),PdeB(3,1)]) Am1 = (P^2 + lpe^2 - d3Q); Am2 = (2*lpe*(P)); %%%%%%%% FIM %%%%%%%%%%%
132
clear all clc tic
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%% PROGRAMA PARA OBTENÇÃO DAS FUNÇÕES DE POSIÇÃO, VELODIDADE E %%%%% %%%%%% ACELERAÇÃO DOS ÂNGULOS DAS JUNTAS NO TEMPO %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% 1. CONSTANTE DASFUNÇÕES NO TEMPO %%% lde = .03; lpe = .11; lti = .37; lfe = .37; Cin = .1; %%% posição da cintura no plano transverso %%% P = .29; Ps = P + lpe; %%% Valor do passo do bípede %%% w = pi; %%% Valor da frequência do ciclo %%% T = 2*pi/w; vx = Ps/(T/2); %%% valor da velocidade da cintura desejada %%% C = 60; %%% C = meio ciclo %%% t = linspace(0,T,(2*C)); th3 = zeros(1,(2*C)); th1 = th3; th2 = th3; th4 = th3; th5 = th3; th6 = th3; th7 = th3; Dth1 = th3; D2th1 = th3; Dth2 = th3; D2th2 = th3; Dth3 = th3; D2th3 = th3; Dth4 = th3; D2th4 = th3; Dth5 = th3; D2th5 = th3; Dth6 = th3; D2th6 = th3; Dth7 = th3; D2th7 = th3; X = th3; x = th3; y =th3; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% 2. COORDENADAS GENERALIZADAS %%%% th3i = 11*pi/180; th3B = -8*pi/180; th5i = 24*pi/180; th5B = -21*pi/180; %%%%%%%%%%%%%%%%%%%% %% 2.1 Função th3 %%
133
for i=1:(C) th3(i) = th3i + th3B*cos(2*w*t(i)); Dth3(i) = - 2*w*th3B*sin(2*w*t(i)); D2th3(i) = - 4*(w^2)*th3B*cos(2*w*t(i)); end for i=(C+1):(2*C) th3(i) = th5i + th5B*cos(2*w*t(i)); Dth3(i) = - 2*w*th5B*sin(2*w*t(i)); D2th3(i) = - 4*(w^2)*th5B*cos(2*w*t(i)); end %%%%%%%%%%%%%%%%%%%% %% 2.2 função th4 %% th4i = pi; th7i = 20*pi/180; d3=sqrt(lpe^2 + P^2 - 2*lpe*P*cos(pi-th7i)); Ad = 2*lti*sin(3*pi/180); Bd = 2*lfe + 2*lti*cos(3*pi/180); alpha = acos(d3/((Ad^2+Bd^2)^(1/2))); th4B = (2*pi-2*alpha) - th4i; alpha = 0; for i=1:(2*C) th4(i) = th4i + th4B*cos(w*t(i)+ alpha); Dth4(i) = -w*th4B*sin(w*t(i)+ alpha); D2th4(i) = -(w^2)*th4B*cos(w*t(i)+ alpha); end %%%%%%%%%%%%%%%%%%%% %% 2.3 Função th5 %% for i=1:(C) th5(i) = th5i + th5B*cos(2*w*t(i)); Dth5(i) = -2*w*th5B*sin(2*w*t(i)); D2th5(i) = -4*(w^2)*th5B*cos(2*w*t(i)); end for i=(C+1):(2*C) th5(i) = th3i + th3B*cos(2*w*t(i)); Dth5(i) = -2*w*th3B*sin(2*w*t(i)); D2th5(i) = -4*(w^2)*th3B*cos(2*w*t(i)); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 3. COORDENADAS DEPENDENTES %% %%%%%% valores iniciais %%%%%%% th30 = th3(1,1);
134
th40 = th4(1,1); th50 = th5(1,1); th10 = pi; th70 = pi - acos((P^2 + lpe^2 - (((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2))/(2*lpe*P)); th60 = pi - acos(((sqrt(((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2))^2 + lpe^2 - P^2)/(2*(sqrt(((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2))*lpe)) - acos(((((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2) - (2*(lfe^2)*(1+cos(th40))))/(2*lti*(sqrt(((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2)))+ (sqrt(2*(lfe^2)*(1+cos(th40))))/(sqrt(((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2))*cos((th40/2)-th30)); th20 = pi + th30 + pi - acos(((sqrt(((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2))^2 + lpe^2 - P^2)/(2*(sqrt(((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2))*lpe)) - acos(((((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2) - (2*(lfe^2)*(1+cos(th40))))/(2*lti*(sqrt(((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2)))+ (sqrt(2*(lfe^2)*(1+cos(th40))))/(sqrt(((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2))*cos((th40/2)-th30)) + pi - acos((P^2 + lpe^2 - (((sqrt(2*(lfe^2)*(1+cos(th40))))-lti*(cos((th40/2)+th50)+cos((th40/2)-th30)))^2+(lti*(sin((th40/2)-th30)-sin((th40/2)+th50)))^2))/(2*lpe*P)) - (th40 + th50); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 3.1 Posição desejada da cintura %%% x0 = lpe*cos(th10)+lti*cos(th10-th20)+lfe*cos(th10-th20+th30); for i=1:C X(i) = x0+vx*t(i); end %%%%%%%%%%%%%%%%%%%%%% %% 3.2 Função th1 %% t2 = (-lpe-x0)/vx; th1e = -(pi-th70); th1e = rad2deg(th1e); a0 = (-45*T^2+th1e*(t2^2-t2*T))/(t2^2-t2*T+(T^2)/4); a1 = T*(180+th1e)/(t2^2-t2*T+(T^2)/4);
135
a2 = (-180-th1e)/(t2^2-t2*T+(T^2)/4); for i=1:(C) if t(i)<=t2 th1(i) = -a0 - a1*t2 - a2*t2^2; Dth1(i) = 0; D2th1(i) = 0; else th1(i) = - a0 - a1*t(i) - a2*t(i)^2; Dth1(i) = -a1-2*a2*t(i); D2th1(i) = -2*a2; end th1(i) = deg2rad(th1(i)); Dth1(i) = deg2rad(Dth1(i)); D2th1(i) = deg2rad(D2th1(i)); end %%%%%%%%%%%%%%%%%%%% %% 3.3 Função th2 %% Y = th2; for i=1:(2*C/10) th2(i) = pi + (th3(i)) + pi - acos(((sqrt(((sqrt(2*(lfe^2)*(1+cos((th4(i))))))-lti*(cos(((th4(i))/2)+(th5(i)))+cos(((th4(i))/2)-(th3(i)))))^2+(lti*(sin(((th4(i))/2)-(th3(i)))-sin(((th4(i))/2)+(th5(i)))))^2))^2 + lpe^2 - P^2)/(2*(sqrt(((sqrt(2*(lfe^2)*(1+cos((th4(i))))))-lti*(cos(((th4(i))/2)+(th5(i)))+cos(((th4(i))/2)-(th3(i)))))^2+(lti*(sin(((th4(i))/2)-(th3(i)))-sin(((th4(i))/2)+(th5(i)))))^2))*lpe)) - acos(((((sqrt(2*(lfe^2)*(1+cos((th4(i))))))-lti*(cos(((th4(i))/2)+(th5(i)))+cos(((th4(i))/2)-(th3(i)))))^2+(lti*(sin(((th4(i))/2)-(th3(i)))-sin(((th4(i))/2)+(th5(i)))))^2) - (2*(lfe^2)*(1+cos((th4(i))))))/(2*lti*(sqrt(((sqrt(2*(lfe^2)*(1+cos((th4(i))))))-lti*(cos(((th4(i))/2)+(th5(i)))+cos(((th4(i))/2)-(th3(i)))))^2+(lti*(sin(((th4(i))/2)-(th3(i)))-sin(((th4(i))/2)+(th5(i)))))^2)))+ (sqrt(2*(lfe^2)*(1+cos((th4(i))))))/(sqrt(((sqrt(2*(lfe^2)*(1+cos((th4(i))))))-lti*(cos(((th4(i))/2)+(th5(i)))+cos(((th4(i))/2)-(th3(i)))))^2+(lti*(sin(((th4(i))/2)-(th3(i)))-sin(((th4(i))/2)+(th5(i)))))^2))*cos(((th4(i))/2)-(th3(i)))) + pi - acos((P^2 + lpe^2 - (((sqrt(2*(lfe^2)*(1+cos((th4(i))))))-lti*(cos(((th4(i))/2)+(th5(i)))+cos(((th4(i))/2)-(th3(i)))))^2+(lti*(sin(((th4(i))/2)-(th3(i)))-sin(((th4(i))/2)+(th5(i)))))^2))/(2*lpe*P)) - ((th4(i)) + (th5(i))); Dth2(i) = (Dth3(i))+2*(1/2*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-
136
(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)/lpe-1/4*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2+lpe^2-P^2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)/lpe*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))))/(4-((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2+lpe^2-P^2)^2/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)/lpe^2)^(1/2)+(1/2*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))+2*lfe^2*sin((th4(i)))*(Dth4(i)))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)-1/4*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2-2*lfe^2*(1+cos((th4(i)))))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-
137
1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i)))*lfe^2*sin((th4(i)))*(Dth4(i))-1/2*2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*cos(1/2*(th4(i))-(th3(i)))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i))))/(1-(1/2*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2-2*lfe^2*(1+cos((th4(i)))))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)+2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i))))^2)^(1/2)+(-2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))-2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))/lpe/P/(4-(P^2+lpe^2-(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2-lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^2/lpe^2/P^2)^(1/2)-(Dth4(i))-(Dth5(i)); D2th2(i) = (D2th3(i))+2*(1/2*(2*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/4*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(3/2)*lfe^4*sin((th4(i)))^2*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*cos((th4(i)))*(Dth4(i))*(Dth4(i))-
138
1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(D2th4(i))-lti*(-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))-cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))))+2*lti^2*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))+cos(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))+sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))))/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)/lpe-1/2*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)/lpe*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))+3/8*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2+lpe^2-P^2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(5/2)/lpe*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-
139
lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-1/4*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2+lpe^2-P^2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)/lpe*(2*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/4*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(3/2)*lfe^4*sin((th4(i)))^2*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*cos((th4(i)))*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(D2th4(i))-lti*(- cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))-cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))))+2*lti^2*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))+cos(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))+sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i))))))/(4-((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2+lpe^2-P^2)^2/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)/lpe^2)^(1/2)-(1/2*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-
140
cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)/lpe-1/4*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2+lpe^2-P^2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)/lpe*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))))/(4-((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2+lpe^2-P^2)^2/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)/lpe^2)^(3/2)*(-2*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2+lpe^2-P^2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)/lpe^2*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))+((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2+lpe^2-P^2)^2/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^2/lpe^2*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-
141
sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))))+(1/2*(2*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/4*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(3/2)*lfe^4*sin((th4(i)))^2*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*cos((th4(i)))*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(D2th4(i))-lti*(-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))-cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))))+2*lti^2*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))+cos(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))+sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i))))+2*lfe^2*cos((th4(i)))*(Dth4(i))*(Dth4(i))+2*lfe^2*sin((th4(i)))*(D2th4(i)))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)-1/4*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))+2*lfe^2*sin((th4(i)))*(Dth4(i)))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-1/4*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-
142
1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))+2*lfe^2*sin((th4(i)))*(Dth4(i)))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))+3/8*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2-2*lfe^2*(1+cos((th4(i)))))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(5/2)*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-1/4*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2-2*lfe^2*(1+cos((th4(i)))))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*(2*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-
143
lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/4*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(3/2)*lfe^4*sin((th4(i)))^2*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*cos((th4(i)))*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(D2th4(i))-lti*(-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))-cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))))+2*lti^2*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))+cos(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))+sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))))-1/4*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(3/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i)))*lfe^4*sin((th4(i)))^2*(Dth4(i))*(Dth4(i))+1/4*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*cos(1/2*(th4(i))-(th3(i)))*lfe^2*sin((th4(i)))*(Dth4(i))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))+1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*lfe^2*sin((th4(i)))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i)))*lfe^2*cos((th4(i)))*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i)))*lfe^2*sin((th4(i)))*(D2th4(i))+1/4*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*cos(1/2*(th4(i))-(th3(i)))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-
144
lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))*lfe^2*sin((th4(i)))*(Dth4(i))+3/4*2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(5/2)*cos(1/2*(th4(i))-(th3(i)))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))+1/2*2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-1/2*2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*cos(1/2*(th4(i))-(th3(i)))*(2*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/4*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(3/2)*lfe^4*sin((th4(i)))^2*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*cos((th4(i)))*(Dth4(i))*(Dth4(i))-
145
1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(D2th4(i))-lti*(-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))-cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))))+2*lti^2*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))+cos(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))+sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))))+1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*lfe^2*sin((th4(i)))*(Dth4(i))+1/2*2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*sin(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i))))/(1-(1/2*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2-2*lfe^2*(1+cos((th4(i)))))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)+2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i))))^2)^(1/2)+(1/2*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-
146
1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))+2*lfe^2*sin((th4(i)))*(Dth4(i)))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)-1/4*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2-2*lfe^2*(1+cos((th4(i)))))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i)))*lfe^2*sin((th4(i)))*(Dth4(i))-1/2*2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*cos(1/2*(th4(i))-(th3(i)))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i))))/(1-(1/2*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2-2*lfe^2*(1+cos((th4(i)))))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)+2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-
147
(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i))))^2)^(3/2)*(1/2*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2-2*lfe^2*(1+cos((th4(i)))))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)+2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i))))*(1/2*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))+2*lfe^2*sin((th4(i)))*(Dth4(i)))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)-1/4*((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2-2*lfe^2*(1+cos((th4(i)))))/lti/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*cos(1/2*(th4(i))-(th3(i)))*lfe^2*sin((th4(i)))*(Dth4(i))-1/2*2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(3/2)*cos(1/2*(th4(i))-(th3(i)))*(2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))+2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-
148
cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)/((2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2+lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^(1/2)*sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i))))+(-2*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))-2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/4*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(3/2)*lfe^4*sin((th4(i)))^2*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*cos((th4(i)))*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(D2th4(i))-lti*(-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))-cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))))-2*lti^2*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))-2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))+cos(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))+sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))))/lpe/P/(4-(P^2+lpe^2-(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2-lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^2/lpe^2/P^2)^(1/2)+(-2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))-2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))/lpe^3/P^3/(4-(P^2+lpe^2-(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2-lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^2/lpe^2/P^2)^(3/2)*(P^2+lpe^2-(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2-lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)*(-2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))-2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-
149
(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))-(D2th4(i))-(D2th5(i)); end DY1 = Y; DY3 = Y; DX = Y; DY11 = Y; DY13 = Y; DY1X = Y; DY31 = Y; DY33 = Y; DY3X = Y; DX1 = Y; DX3 = Y; DXX = Y; DY = Y; D2Y = Y; for i=(2*C/10+1):C Y(i) = (-(-2*((X(i)-lpe*cos(th1(i)))/(lfe*sin(th3(i))))*((lti+lfe*cos(th3(i)))/(lfe*sin(th3(i)))))+(-1)^2*sqrt((-2*((X(i)-lpe*cos(th1(i)))/(lfe*sin(th3(i))))*((lti+lfe*cos(th3(i)))/(lfe*sin(th3(i)))))^2-4*(((lti+lfe*cos(th3(i)))/(lfe*sin(th3(i))))^2 + 1)*(((X(i)-lpe*cos(th1(i)))/(lfe*sin(th3(i))))^2-1)))/(2*(((lti+lfe*cos(th3(i)))/(lfe*sin(th3(i))))^2 + 1)); DY1(i) = 1/148/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*cos((th3(i)))*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))/sin((th3(i)))^2+11/74*sin((th1(i)))-1/148/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))/sin((th3(i)))^2; DY3(i) = 1/74*(1369*cos((th3(i)))-1369*cos((th3(i)))^3-121*cos((th1(i)))^2+2200*(X(i))*cos((th1(i)))+1369-1369*cos((th3(i)))^2-10000*(X(i))^2+121*cos((th3(i)))*cos((th1(i)))^2-2200*cos((th3(i)))*(X(i))*cos((th1(i)))+10000*cos((th3(i)))*(X(i))^2)/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)/sin((th3(i)))^3; DX(i) = 1/148/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*cos((th3(i)))*(-2200*cos((th1(i)))+20000*(X(i)))/sin((th3(i)))^2+50/37-1/148/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*(-2200*cos((th1(i)))+20000*(X(i)))/sin((th3(i)))^2; DY11(i) = 1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*cos((th3(i)))*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))^2/sin((th3(i)))^4+1/148/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*cos((th3(i)))*(242*sin((th1(i)))^2-242*cos((th1(i)))^2+2200*(X(i))*cos((th1(i))))/sin((th3(i)))^2+11/74*cos((th1(i)))-1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))^2/sin((th3(i)))^4-1/148/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*(242*sin((th1(i)))^2-242*cos((th1(i)))^2+2200*(X(i))*cos((th1(i))))/sin((th3(i)))^2; DY13(i) = -1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*cos((th3(i)))*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))/sin((th3(i)
150
))^2*(-2738/sin((th3(i)))+2*(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^3*cos((th3(i))))-1/148/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)/sin((th3(i)))*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))-1/74/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*cos((th3(i)))^2*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))/sin((th3(i)))^3+1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))/sin((th3(i)))^2*(-2738/sin((th3(i)))+2*(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^3*cos((th3(i))))+1/74/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))/sin((th3(i)))^3*cos((th3(i))); DY1X(i) = 1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*cos((th3(i)))*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))/sin((th3(i)))^4*(-2200*cos((th1(i)))+20000*(X(i)))+550/37/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*cos((th3(i)))*sin((th1(i)))/sin((th3(i)))^2-1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))/sin((th3(i)))^4*(-2200*cos((th1(i)))+20000*(X(i)))-550/37/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*sin((th1(i)))/sin((th3(i)))^2; DY31(i) = 1/74*(242*cos((th1(i)))*sin((th1(i)))-2200*(X(i))*sin((th1(i)))-242*cos((th1(i)))*cos((th3(i)))*sin((th1(i)))+2200*cos((th3(i)))*(X(i))*sin((th1(i))))/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)/sin((th3(i)))^3+1/148*(1369*cos((th3(i)))-1369*cos((th3(i)))^3-121*cos((th1(i)))^2+2200*(X(i))*cos((th1(i)))+1369-1369*cos((th3(i)))^2-10000*(X(i))^2+121*cos((th1(i)))^2*cos((th3(i)))-2200*cos((th3(i)))*(X(i))*cos((th1(i)))+10000*cos((th3(i)))*(X(i))^2)/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)/sin((th3(i)))^5*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i)))); DY33(i) = 1/74*(-1369*sin((th3(i)))+4107*cos((th3(i)))^2*sin((th3(i)))+2738*cos((th3(i)))*sin((th3(i)))-121*cos((th1(i)))^2*sin((th3(i)))+2200*sin((th3(i)))*(X(i))*cos((th1(i)))-10000*sin((th3(i)))*(X(i))^2)/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)/sin((th3(i)))^3-1/148*(1369*cos((th3(i)))-1369*cos((th3(i)))^3-121*cos((th1(i)))^2+2200*(X(i))*cos((th1(i)))+1369-1369*cos((th3(i)))^2-10000*(X(i))^2+121*cos((th1(i)))^2*cos((th3(i)))-2200*cos((th3(i)))*(X(i))*cos((th1(i)))+10000*cos((th3(i)))*(X(i))^2)/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-
151
2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)/sin((th3(i)))^3*(-2738/sin((th3(i)))+2*(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^3*cos((th3(i))))-3/74*(1369*cos((th3(i)))-1369*cos((th3(i)))^3-121*cos((th1(i)))^2+2200*(X(i))*cos((th1(i)))+1369-1369*cos((th3(i)))^2-10000*(X(i))^2+121*cos((th1(i)))^2*cos((th3(i)))-2200*cos((th3(i)))*(X(i))*cos((th1(i)))+10000*cos((th3(i)))*(X(i))^2)/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)/sin((th3(i)))^4*cos((th3(i))); DY3X(i) = 1/74*(2200*cos((th1(i)))-20000*(X(i))-2200*cos((th3(i)))*cos((th1(i)))+20000*cos((th3(i)))*(X(i)))/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)/sin((th3(i)))^3+1/148*(1369*cos((th3(i)))-1369*cos((th3(i)))^3-121*cos((th1(i)))^2+2200*(X(i))*cos((th1(i)))+1369-1369*cos((th3(i)))^2-10000*(X(i))^2+121*cos((th1(i)))^2*cos((th3(i)))-2200*cos((th3(i)))*(X(i))*cos((th1(i)))+10000*cos((th3(i)))*(X(i))^2)/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)/sin((th3(i)))^5*(-2200*cos((th1(i)))+20000*(X(i))); DX1(i) = 1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*cos((th3(i)))*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))/sin((th3(i)))^4*(-2200*cos((th1(i)))+20000*(X(i)))+550/37/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*cos((th3(i)))*sin((th1(i)))/sin((th3(i)))^2-1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*(-242*cos((th1(i)))*sin((th1(i)))+2200*(X(i))*sin((th1(i))))/sin((th3(i)))^4*(-2200*cos((th1(i)))+20000*(X(i)))-550/37/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*sin((th1(i)))/sin((th3(i)))^2; DX3(i) = -1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*cos((th3(i)))*(-2200*cos((th1(i)))+20000*(X(i)))/sin((th3(i)))^2*(-2738/sin((th3(i)))+2*(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^3*cos((th3(i))))-1/148/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)/sin((th3(i)))*(-2200*cos((th1(i)))+20000*(X(i)))-1/74/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*cos((th3(i)))^2*(-2200*cos((th1(i)))+20000*(X(i)))/sin((th3(i)))^3+1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*(-2200*cos((th1(i)))+20000*(X(i)))/sin((th3(i)))^2*(-2738/sin((th3(i)))+2*(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^3*cos((th3(i))))+1/74/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*(-2200*cos((th1(i)))+20000*(X(i)))/sin((th3(i)))^3*cos((th3(i)));
152
DXX(i) = 1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*cos((th3(i)))*(-2200*cos((th1(i)))+20000*(X(i)))^2/sin((th3(i)))^4+5000/37/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)*cos((th3(i)))/sin((th3(i)))^2-1/296/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(3/2)*(-2200*cos((th1(i)))+20000*(X(i)))^2/sin((th3(i)))^4-5000/37/(-(-2738*cos((th3(i)))+121*cos((th1(i)))^2-2200*(X(i))*cos((th1(i)))-2738+10000*(X(i))^2)/sin((th3(i)))^2)^(1/2)/sin((th3(i)))^2; DY(i) = DY1(i)*Dth1(i)+DY3(i)*Dth3(i)+DX(i)*vx; D2Y(i) = (DY11(i)*Dth1(i) + DY13(i)* Dth3(i) + DY1X(i)*vx)*Dth1(i) + DY1(i)*D2th1(i) + (DY31(i)*Dth1(i) + DY33(i)*Dth3(i) + DY3X(i)*vx)*Dth3(i)+ DY3(i)*D2th3(i)+ (DX1(i)*Dth1(i) +DX3(i)*Dth3(i) + DXX(i)*vx)*vx; th2(i) = th1(i)-acos(Y(i)); Dth2(i) = Dth1(i) - 1/(sqrt(1-Y(i)^2))*DY(i); D2th2(i) = D2th1(i) - 1/(1-Y(i)^2)^(3/2)*Y(i)*DY(i)^2 - 1/(sqrt(1-Y(i)^2))*D2Y(i); end %%%%%%%%%%%%%%%%%%%%%%%%%%% %% 3.4 Função th7 e th6 %% t1 = pi/4; Am1 = zeros(1,C); Am2 = Am1; EV = Am1; for i=1:(2*C/10)
%%%%%%% th7 %%%%%%%% th7(i) = pi - acos((P^2 + lpe^2 - (((sqrt(2*(lfe^2)*(1+cos((th4(i))))))-lti*(cos(((th4(i))/2)+(th5(i)))+cos(((th4(i))/2)-(th3(i)))))^2+(lti*(sin(((th4(i))/2)-(th3(i)))-sin(((th4(i))/2)+(th5(i)))))^2))/(2*lpe*P)); Dth7(i) = (-2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))-2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))/lpe/P/(4-(P^2+lpe^2-(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2-lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^2/lpe^2/P^2)^(1/2); D2th7(i) = (-2*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(
153
i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))-2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/4*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(3/2)*lfe^4*sin((th4(i)))^2*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*cos((th4(i)))*(Dth4(i))*(Dth4(i))-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(D2th4(i))-lti*(-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))-cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))))-2*lti^2*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))-2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))*(1/2*(Dth4(i))-(Dth3(i)))+cos(1/2*(th4(i))-(th3(i)))*(1/2*(D2th4(i))-(D2th3(i)))+sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(D2th4(i))+(D2th5(i)))))/lpe/P/(4-(P^2+lpe^2-(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2-lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^2/lpe^2/P^2)^(1/2)+(-2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))-2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))))/lpe^3/P^3/(4-(P^2+lpe^2-(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2-lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)^2/lpe^2/P^2)^(3/2)*(P^2+lpe^2-(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))^2-lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))^2)*(-2*(2^(1/2)*(lfe^2*(1+cos((th4(i)))))^(1/2)-lti*(cos(1/2*(th4(i))+(th5(i)))+cos(1/2*(th4(i))-(th3(i)))))*(-1/2*2^(1/2)/(lfe^2*(1+cos((th4(i)))))^(1/2)*lfe^2*sin((th4(i)))*(Dth4(i))-lti*(-sin(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i)))-sin(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))))-2*lti^2*(sin(1/2*(th4(i))-(th3(i)))-sin(1/2*(th4(i))+(th5(i))))*(cos(1/2*(th4(i))-(th3(i)))*(1/2*(Dth4(i))-(Dth3(i)))-cos(1/2*(th4(i))+(th5(i)))*(1/2*(Dth4(i))+(Dth5(i))))); %%%%%%% th6 %%%%%%%% th6(i) = -pi - th3(i) + th2(i) - th7(i) + th4(i) + th5(i); Dth6(i) = -Dth3(i) + Dth2(i) - Dth7(i) + Dth4(i) + Dth5(i); D2th6(i) = -D2th3(i) + D2th2(i) - D2th7(i) + D2th4(i) + D2th5(i);
154
d3 = (((sqrt(2*(lfe^2)*(1+cos(th4(i)))))-lti*(cos((th4(i)/2)+th5(i))+cos((th4(i)/2)-th3(i))))^2+(lti*(sin((th4(i)/2)-th3(i))-sin((th4(i)/2)+th5(i))))^2); CF = P^2 + lpe^2; Am1(i) = P^2 + lpe^2 -d3; Am2(i) = (2*lpe*P); EV(i) = (Am1(i))^2-(Am2(i))^2; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 4. SEGUNDA METADE DO CICLO %%% for i=(2*C/10+1):C %%%%%%% th7 %%%%%%%% A7 = (th7(2*C/10)/(t(2*C/10)-T/2)^2); B7 = -2*A7*T/2; C7 = A7*T^2/4; th7(i)=A7*t(i)^2+B7*t(i)+C7; Dth7(i) = 2*A7*t(i)+B7; D2th7(i) = 2*A7; %%%%%%% th6 %%%%%%%% tl = (T + 2*t(2*C/10))/4; tB = t(2*C/10); th6B = th6(2*C/10); th6l = pi/2; A6 = ((th6B-th20)-(th6l-th20)*(tB-T/2)/(tl-T/2))/((tB^2-T^2/4)-(tl^2-T^2/4)*(tB-T/2)/(tl-T/2)); B6 = ((th6l-th20)-A6*(tl^2-T^2/4))/(tl-T/2); C6 = th20-A6*(T^2/4)-B6*(T/2); th6(i) = A6*t(i)^2 + B6*t(i)+C6; Dth6(i) = 2*A6*t(i)+B6; D2th6(i) = 2*A6; end for i=(C+1):(2*C) th1(i) = pi-th7(i-C); th2(i) = th6(i-C); th6(i) = th2(i-C); th7(i) = pi-th1(i-C); Dth1(i) = -Dth7(i-C); Dth2(i) = Dth6(i-C); Dth6(i) = Dth2(i-C); Dth7(i) = -Dth1(i-C); D2th1(i) = -D2th7(i-C); D2th2(i) = D2th6(i-C); D2th6(i) = D2th2(i-C); D2th7(i) = -D2th1(i-C); end Pxpb = x; Pxci =x; Pxfb = x; Pxfe = x; Pypb = x;
155
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 5. VERIFICAÇÃO %%%%%%%% for N=2 %%% Número de passos desejados %%% for i=1:C Pded = [(N*Ps-Ps);0;0]; Pcal = [(N*Ps-Ps) + lpe*cos(th1(i));lpe*sin(th1(i));0]; Pjoe = [(N*Ps-Ps) + lti*cos(th1(i)-th2(i))+ lpe*cos(th1(i));lti*sin(th1(i)-th2(i))+ lpe*sin(th1(i));0]; Pc = [(N*Ps-Ps) + lfe*cos(th1(i)-th2(i)+th3(i)) + lti*cos(th1(i)-th2(i))+ lpe*cos(th1(i));lfe*sin(th1(i)-th2(i)+th3(i))+lti*sin(th1(i)-th2(i))+ lpe*sin(th1(i));Cin]; x(i) = (N*Ps-Ps) + lfe*cos(th1(i)-th2(i)+th3(i)) + lti*cos(th1(i)-th2(i))+ lpe*cos(th1(i)); y(i) = lfe*sin(th1(i)-th2(i)+th3(i))+lti*sin(th1(i)-th2(i))+ lpe*sin(th1(i)); PjoB = [(N*Ps-Ps)+ lfe*cos(th1(i)-th2(i)+th3(i)-th4(i))+ lfe*cos(th1(i)-th2(i)+th3(i)) + lti*cos(th1(i)-th2(i))+ lpe*cos(th1(i));lfe*sin(th1(i)-th2(i)+th3(i)-th4(i)) + lfe*sin(th1(i)-th2(i)+th3(i))+lti*sin(th1(i)-th2(i))+ lpe*sin(th1(i));Cin]; PcaB = [(N*Ps-Ps)+ lti*cos(th1(i)-th2(i)+th3(i)-th4(i)-th5(i)) + lfe*cos(th1(i)-th2(i)+th3(i)-th4(i))+ lfe*cos(th1(i)-th2(i)+th3(i)) + lti*cos(th1(i)-th2(i))+ lpe*cos(th1(i));lti*sin(th1(i)-th2(i)+th3(i)-th4(i)-th5(i))+ lfe*sin(th1(i)-th2(i)+th3(i)-th4(i)) + lfe*sin(th1(i)-th2(i)+th3(i))+lti*sin(th1(i)-th2(i))+ lpe*sin(th1(i));Cin]; PpeB = [(N*Ps-Ps)+ lpe*cos(th1(i)-th2(i)+th3(i)-th4(i)-th5(i)+th6(i))+lti*cos(th1(i)-th2(i)+th3(i)-th4(i)-th5(i)) + lfe*cos(th1(i)-th2(i)+th3(i)-th4(i))+ lfe*cos(th1(i)-th2(i)+th3(i)) + lti*cos(th1(i)-th2(i))+ lpe*cos(th1(i));lpe*sin(th1(i)-th2(i)+th3(i)-th4(i)-th5(i)+th6(i))+lti*sin(th1(i)-th2(i)+th3(i)-th4(i)-th5(i))+ lfe*sin(th1(i)-th2(i)+th3(i)-th4(i)) + lfe*sin(th1(i)-th2(i)+th3(i))+lti*sin(th1(i)-th2(i))+ lpe*sin(th1(i));Cin]; PdeB = [(N*Ps-Ps)+ lde*cos(th1(i)-th2(i)+th3(i)-th4(i)-th5(i)+th6(i)+th7(i))+lpe*cos(th1(i)-th2(i)+th3(i)-th4(i)-th5(i)+th6(i))+lti*cos(th1(i)-th2(i)+th3(i)-th4(i)-th5(i)) + lfe*cos(th1(i)-th2(i)+th3(i)-th4(i))+ lfe*cos(th1(i)-th2(i)+th3(i)) + lti*cos(th1(i)-th2(i))+ lpe*cos(th1(i));lde*sin(th1(i)-th2(i)+th3(i)-th4(i)-th5(i)+th6(i)+th7(i))+lpe*sin(th1(i)-th2(i)+th3(i)-th4(i)-th5(i)+th6(i))+lti*sin(th1(i)-th2(i)+th3(i)-th4(i)-th5(i))+ lfe*sin(th1(i)-th2(i)+th3(i)-th4(i)) + lfe*sin(th1(i)-th2(i)+th3(i))+lti*sin(th1(i)-th2(i))+ lpe*sin(th1(i));Cin]; line([(Pded(1,1)+lde),Pded(1,1)],[0,0],[0,0]) line([Pded(1,1),Pcal(1,1)],[0,Pcal(2,1)],[0,Pcal(3,1)]) line([Pcal(1,1),Pjoe(1,1)],[Pcal(2,1),Pjoe(2,1)],[Pcal(3,1),Pjoe(3,1)]) line([Pjoe(1,1),Pc(1,1)],[Pjoe(2,1),Pc(2,1)],[0,0]) line([Pc(1,1),Pc(1,1)],[Pc(2,1),Pc(2,1)],[0,Pc(3,1)]) line([Pc(1,1),PjoB(1,1)],[Pc(2,1),PjoB(2,1)],[Pc(3,1),PjoB(3,1)]) line([PjoB(1,1),PcaB(1,1)],[PjoB(2,1),PcaB(2,1)],[PjoB(3,1),PcaB(3,1)]) line([PcaB(1,1),PpeB(1,1)],[PcaB(2,1),PpeB(2,1)],[PcaB(3,1),PpeB(3,1)])
156
line([PpeB(1,1),PdeB(1,1)],[PpeB(2,1),PdeB(2,1)],[PpeB(3,1),PdeB(3,1)]) ylabel('Y(m)') xlabel('X(m)') Pxpb(i)=PpeB(1,1); Pxci(i)=Pc(1,1); Pxfb(i)=PjoB(1,1); Pxfe(i)=Pjoe(1,1); Pypb(i)=PpeB(2,1); end end save funTempo th1 th2 th3 th4 th5 th6 th7 save VelAce Dth3 Dth4 Dth5 D2th3 D2th4 D2th5 save vel Dth1 Dth2 Dth3 Dth4 Dth5 Dth6 Dth7 save ace D2th1 D2th2 D2th3 D2th4 D2th5 D2th6 D2th7 for i=1:2*C th1(i)=rad2deg(th1(i)); th2(i)=rad2deg(th2(i)); th3(i)=rad2deg(th3(i)); th4(i)=rad2deg(th4(i)); th5(i)=rad2deg(th5(i)); th6(i)=rad2deg(th6(i)); th7(i)=rad2deg(th7(i)); end plot(t,th3,'b-',t,th4,'r--',t,th5,'k.') title(texlabel('Posição theta3(-) theta4(--) theta5(.)')) ylabel(texlabel('(Grau)')) xlabel('t(s)') figure plot(t,Dth3,'b-',t,Dth4,'r--',t,Dth5,'k.') title(texlabel(' velocidade angular theta3(-) theta4(--) theta5(.)')) ylabel(texlabel('(rad/s)')) xlabel('t(s)') figure plot(t,D2th3,'b-',t,D2th4,'r--',t,D2th5,'k.') title(texlabel(' Aceleração angular theta3(-) theta4(--) theta5(.)')) ylabel(texlabel('(rad/s^2)')) xlabel('t(s)') figure plot(t,th1,'b--',t,th2,'r.-',t,th6,'k-',t,th7,'.') title(texlabel('Posição theta1(--), theta2 (-.-), theta6 (-), theta7 (.)')) ylabel(texlabel('theta (Grau)')) xlabel('t(s)') figure
157
plot(t,Dth1,'b--',t,Dth2,'r.-',t,Dth6,'k-',t,Dth7,'.') title(texlabel('velocidade angular theta1(--), theta2 (-.-), theta6 (-), theta7 (.)')) ylabel(texlabel('(rad/s)')) xlabel('t(s)') figure plot(t,D2th1,'b--',t,D2th2,'r.-',t,D2th6,'k-',t,D2th7,'.') title(texlabel('Aceleração angular theta1(--), theta2 (-.-), theta6 (-), theta7 (.)')) ylabel(texlabel('(rad/s^2)')) xlabel('t(s)') figure plot(t,th1,'b--',t,th7,'r-') title(texlabel('Posição theta1(--), theta7 (-)')) ylabel(texlabel('theta (Grau)')) xlabel('t(s)') figure plot(t,th2,'b--',t,th6,'r-') title(texlabel('Posição theta2(--), theta6 (-)')) ylabel(texlabel('theta (Grau)')) xlabel('t(s)') figure plot(t,th1) title(texlabel('Posição theta1')) ylabel(texlabel('theta (Grau)')) xlabel('t(s)') figure plot(t,th2) title(texlabel('Posição theta2')) ylabel(texlabel('theta (Grau)')) xlabel('t(s)') figure plot(t,th6) title(texlabel('Posição theta6')) ylabel(texlabel('theta (Grau)')) xlabel('t(s)') figure plot(t,th7) title(texlabel('Posição theta7')) ylabel(texlabel('theta (Grau)')) xlabel('t(s)') toc %%%%%%%% FIM %%%%%%%%%%%
158
Anexo B. Programação da Memória de Cálculo da Dinâmica Desenvolvido no MatLab
clear all clc load parametros %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%% DINÂMICA -CADEIA ABERTA %%%%%%% %%%%%% %%%%%%% %%%%%% OBTENÇÃO EQUAÇÃO DE MOVIMENTO %%%%%%% %%%%%% PARA A FASE DE SUPORTE SIMPLES %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%% 1 - CINEMATICA DIRETA - FASE BALANCO %%%%%%% %% 1.1 - Constantes e parâmetros DH %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% tic %%% Começar a Contagem %%% Cin = 0.1; lde = .03; lpe = .11; lti = .37; lfe = .37; CMp = 0.5; CMt = 0.5; CMf = 0.5; CMd = 0.5; %%%% Valores das Massas e dos momentos de inércia dos corpos do sistema %%% Md = 0.2; Mp = 0.6; Mt = 4; Mf = 6; Mc = 20; Id = 2.7*10^(-5); Ip = 6*10^(-4); It = 0.046; If = 0.07; Ic = 1.07; g = [0;9.81;0]; %%%% vetor Gravidade %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% posição das juntas %%
159
syms th1 th2 th3 th4 th5 th6 th7 Pcal = [lpe*cos(th1);lpe*sin(th1);0]; Pjoe = [lti*cos(th1-th2)+ lpe*cos(th1);lti*sin(th1-th2)+ lpe*sin(th1);0]; PjoB = [lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PcaB = [lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PpeB = [lpe*cos(th1-th2+th3-th4-th5+th6)+lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lpe*sin(th1-th2+th3-th4-th5+th6)+lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%% 2 - VELOCIDADE %%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%% Memória de cálculo para velocidade rotacional %% JoR = [0;0;1]; JoSR = [0;0;0]; R1 = [cos(th1),-sin(th1),0;sin(th1),cos(th1),0;0,0,1]; R2 = [cos(th1-th2),-sin(th1-th2),0;sin(th1-th2),cos(th1-th2),0;0,0,1]; R3 = [cos(th1-th2+th3),-sin(th1-th2+th3),0;sin(th1-th2+th3),cos(th1-th2+th3),0;0,0,1]; R4 = [cos(th1-th2+th3-th4),-sin(th1-th2+th3-th4),0;sin(th1-th2+th3-th4),cos(th1-th2+th3-th4),0;0,0,1]; R5 = [cos(th1-th2+th3-th4-th5),-sin(th1-th2+th3-th4-th5),0;sin(th1-th2+th3-th4-th5),cos(th1-th2+th3-th4-th5),0;0,0,1]; R6 = [cos(th1-th2+th3-th4-th5+th6),-sin(th1-th2+th3-th4-th5+th6),0;sin(th1-th2+th3-th4-th5+th6),cos(th1-th2+th3-th4-th5+th6),0;0,0,1]; R7 = [cos(th1-th2+th3-th4-th5+th6+th7),-sin(th1-th2+th3-th4-th5+th6+th7),0;sin(th1-th2+th3-th4-th5+th6+th7),cos(th1-th2+th3-th4-th5+th6+th7),0;0,0,1]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 2.1 - posição dos centros de massa %%% %% 0 < CM < 1 %%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% P0 = [0;0;0]; PpS = [CMp*lpe*cos(th1);CMp*lpe*sin(th1);0]; PtS = [CMt*lti*cos(th1-th2)+ lpe*cos(th1);CMt*lti*sin(th1-th2)+ lpe*sin(th1);0]; PfS = [CMf*lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMf*lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);0];
160
Pc = [lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin/2]; PfB = [CMf*lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMf*lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PtB = [CMt*lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMt*lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PpB = [CMp*lpe*cos(th1-th2+th3-th4-th5+th6)+lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMp*lpe*sin(th1-th2+th3-th4-th5+th6)+lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PdB = [CMd*lde*cos(th1-th2+th3-th4-th5+th6+th7)+lpe*cos(th1-th2+th3-th4-th5+th6)+lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMd*lde*sin(th1-th2+th3-th4-th5+th6+th7)+lpe*sin(th1-th2+th3-th4-th5+th6)+lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; %%%%%%% 2.2 - Jacobiano %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Q = sym('[th1;th2;th3;th4;th5;th6;th7]'); DtQ = sym('[Dth1;Dth2;Dth3;Dth4;Dth5;Dth6;Dth7]'); %% 2.2.1 - Velocidade do pé %% PpSP0=PpS-P0; Jv11 = diff(PpSP0,'th1'); Jv12 = diff(PpSP0,'th2'); Jv13 = diff(PpSP0,'th3'); Jv14 = diff(PpSP0,'th4'); Jv15 = diff(PpSP0,'th5'); Jv16 = diff(PpSP0,'th6'); Jv17 = diff(PpSP0,'th7'); Jo11 = JoR; Jo12 = [0;0;0]; %%% Jo12 = Jo13 = Jo14... = Jo17 %% Jv1 = [Jv11,Jv12,Jv13,Jv14,Jv15,Jv16,Jv17]; Jo1 = [Jo11,Jo12,Jo12,Jo12,Jo12,Jo12,Jo12]; Vpe = Jv1*DtQ; Wpe = Jo1*DtQ; %% 2.2.2 - Velocidade da Tíbia %% PtSP0 = PtS - P0; PtSPpS = PtS - Pcal;
161
Jv21 = diff(PtSP0,'th1'); Jv22 = diff(PtSPpS,'th2'); Jv23 = diff(PtSPpS,'th3'); Jv24 = diff(PtSPpS,'th4'); Jv25 = diff(PtSPpS,'th5'); Jv26 = diff(PtSPpS,'th6'); Jv27 = diff(PtSPpS,'th7'); Jo21 = JoR; Jo22 = JoR; Jo23 = [0;0;0]; %%% Jo23 = Jo24... = Jo27 %% Jv2 = [Jv21,Jv22,Jv23,Jv24,Jv25,Jv26,Jv27]; Jo2 = [Jo21,Jo22,Jo23,Jo23,Jo23,Jo23,Jo23]; Vti = Jv2*DtQ; Wti = Jo2*DtQ; %% 2.2.3 - Velocidade do fêmur %% PfSP0 = PfS - P0; PfSPpS = PfS - Pcal; PfSPtS = PfS - Pjoe; Jv31 = diff(PfSP0,'th1'); Jv32 = diff(PfSPpS,'th2'); Jv33 = diff(PfSPtS,'th3'); Jv34 = diff(PfSPtS,'th4'); Jv35 = diff(PfSPtS,'th5'); Jv36 = diff(PfSPtS,'th6'); Jv37 = diff(PfSPtS,'th7'); Jo31 = JoR; Jo32 = JoR; Jo33 = JoR; Jo34 = [0;0;0]; %%% Jo34... = Jo37 %% Jv3 = [Jv31,Jv32,Jv33,Jv34,Jv35,Jv36,Jv37]; Jo3 = [Jo31,Jo32,Jo33,Jo34,Jo34,Jo34,Jo34]; Vfe = Jv3*DtQ; Wfe = Jo3*DtQ; %% 2.2.4 EX - Velocidade da cintura %% %pcc = sym('[0;0;0.5*lpv]'); %Pc = Tcin(1:3,4) + pcc; PcP0 = Pc - P0; PcPpS = Pc - Pcal; PcPtS = Pc - Pjoe; PcPfS = Pc - Pc;
162
JvC1 = diff(PcP0,'th1'); JvC2 = diff(PcPpS,'th2'); JvC3 = diff(PcPtS,'th3'); JvC4 = diff(PcPfS,'th4'); JvC5 = diff(PcPfS,'th5'); JvC6 = diff(PcPfS,'th6'); JvC7 = diff(PcPfS,'th7'); JoC1 = JoR; JoC2 = JoR; JoC3 = JoR; JoC4 = [0;0;0]; JvC = [JvC1,JvC2,JvC3,JvC4,JvC5,JvC6,JvC7]; JoC = [JoC1,JoC2,JoC3,JoC4,JoC4,JoC4,JoC4]; Vc = JvC*DtQ; Wc = JoC*DtQ; %% 2.2.4 - Velocidade do fêmur em balanço %% PfBP0 = PfB - P0; PfBPpS = PfB - Pcal; PfBPtS = PfB - Pjoe; PfBPcS = PfB - Pc; Jv41 = diff(PfBP0,'th1'); Jv42 = diff(PfBPpS,'th2'); Jv43 = diff(PfBPtS,'th3'); Jv44 = diff(PfBPcS,'th4'); Jv45 = diff(PfBPcS,'th5'); Jv46 = diff(PfBPcS,'th6'); Jv47 = diff(PfBPcS,'th7'); Jo41 = JoR; Jo42 = JoR; Jo43 = JoR; Jo44 = JoR; Jo45 = [0;0;0]; Jv4 = [Jv41,Jv42,Jv43,Jv44,Jv45,Jv46,Jv47]; Jo4 = [Jo41,Jo42,Jo43,Jo44,Jo45,Jo45,Jo45]; Vfb = Jv4*DtQ; Wfb = Jo4*DtQ; %% 2.2.5 - Velocidade da tíbia em balanço %% PtBP0 = PtB - P0; PtBPpS = PtB - Pcal; PtBPtS = PtB - Pjoe; PtBPcS = PtB - Pc; PtBPfB = PtB - PjoB; Jv51 = diff(PtBP0,'th1'); Jv52 = diff(PtBPpS,'th2'); Jv53 = diff(PtBPtS,'th3'); Jv54 = diff(PtBPcS,'th4');
163
Jv55 = diff(PtBPfB,'th5'); Jv56 = diff(PtBPfB,'th6'); Jv57 = diff(PtBPfB,'th7'); Jo51 = JoR; Jo52 = JoR; Jo53 = JoR; Jo54 = JoR; Jo55 = JoR; Jo56 = [0;0;0]; Jv5 = [Jv51,Jv52,Jv53,Jv54,Jv55,Jv56,Jv57]; Jo5 = [Jo51,Jo52,Jo53,Jo54,Jo55,Jo56,Jo56]; Vtb = Jv5*DtQ; Wtb = Jo5*DtQ; %% 2.2.6 - Velocidade do pé em balanço %% PpBP0 = PpB - P0; PpBPpS = PpB - Pcal; PpBPtS = PpB - Pjoe; PpBPcS = PpB - Pc; PpBPfB = PpB - PjoB; PpBPtB = PpB - PcaB; Jv61 = diff(PpBP0,'th1'); Jv62 = diff(PpBPpS,'th2'); Jv63 = diff(PpBPtS,'th3'); Jv64 = diff(PpBPcS,'th4'); Jv65 = diff(PpBPfB,'th5'); Jv66 = diff(PpBPtB,'th6'); Jv67 = diff(PpBPtB,'th7'); Jo61 = JoR; Jo62 = JoR; Jo63 = JoR; Jo64 = JoR; Jo65 = JoR; Jo66 = JoR; Jo67 = [0;0;0]; Jv6 = [Jv61,Jv62,Jv63,Jv64,Jv65,Jv66,Jv67]; Jo6 = [Jo61,Jo62,Jo63,Jo64,Jo65,Jo66,Jo66]; Vpb = Jv6*DtQ; Wpb = Jo6*DtQ; %% 2.2.7 - Velocidade do dedo em balanço %% PdBP0 = PdB - P0; PdBPpS = PdB - Pcal; PdBPtS = PdB - Pjoe; PdBPcS = PdB - Pc; PdBPfB = PdB - PjoB; PdBPtB = PdB - PcaB; PdBPpB = PdB - PpeB; Jv71 = diff(PdBP0,'th1');
164
Jv72 = diff(PdBPpS,'th2'); Jv73 = diff(PdBPtS,'th3'); Jv74 = diff(PdBPcS,'th4'); Jv75 = diff(PdBPfB,'th5'); Jv76 = diff(PdBPtB,'th6'); Jv77 = diff(PdBPpB,'th7'); Jo71 = JoR; Jo72 = JoR; Jo73 = JoR; Jo74 = JoR; Jo75 = JoR; Jo76 = JoR; Jo77 = JoR; Jv7 = [Jv71,Jv72,Jv73,Jv74,Jv75,Jv76,Jv77]; Jo7 = [Jo71,Jo72,Jo73,Jo74,Jo75,Jo76,Jo76]; Vdb = Jv7*DtQ; Wdb = Jo7*DtQ; %% 3.2 - Energia Potencial %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Eppe = Mp*(g.')*PpS; Epti = Mt*(g.')*PtS; Epfe = Mf*(g.')*PfS; Epc = Mc*(g.')*Pc; Epfb = Mf*(g.')*PfB; Eptb = Mt*(g.')*PtB; Eppb = Mp*(g.')*PpB; Epdb = Md*(g.')*PdB; Ep = Eppe + Epti + Epfe + Epc + Epfb + Eptb + Eppb + Epdb; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%% 4 - EQUAÇÃO DE MOVIMENTO %%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ips = R1*Ip*(R1.'); Its = R2*It*(R2.'); Ifs = R3*If*(R3.'); Ic = R3*Ic*(R3.'); Ifb = R4*If*(R4.'); Itb = R5*It*(R5.'); Ipb = R6*Ip*(R6.'); Idb = R7*Id*(R7.'); %% 4.1 - MATRIZ DE MASSA %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
165
MT = Mp*(Jv1.')*Jv1 + Mt*(Jv2.')*Jv2 + Mf*(Jv3.')*Jv3 + Mc*(JvC.')*JvC + Mf*(Jv4.')*Jv4 + Mt*(Jv5.')*Jv5 + Mp*(Jv6.')*Jv6+ Md*(Jv7.')*Jv7; MR = (Jo1.')*Ips*Jo1 + (Jo2.')*Its*Jo2 + (Jo3.')*Ifs*Jo3 + (JoC.')*Ic*JoC + (Jo4.')*Ifb*Jo4 + (Jo5.')*Itb*Jo5 + (Jo6.')*Ipb*Jo6 + (Jo7.')*Idb*Jo7; M = MT + MR; %% 4.2 - MATRIZ DE AMORTECIMENTO %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Hm1 = sym('[0,0,0,0,0,0,0]'); Hm2 = Hm1; G = Hm1.'; DtM = (Hm1.')*Hm1; C = (Hm2.')*Hm2; for i=1:7 for j=1:7 for k=1:7 C1 = M(i,j); DC1 = diff(C1,Q(k,1)); Hm1(1,k) = DC1(1,1); C2 = M(j,k); DC2 = diff(C2,Q(i,1)); Hm2(1,k) = DC2(1,1); end DtM(i,j) = Hm1*DtQ; C(i,j) = 0.5*(Hm2*DtQ); end end %% 4.2 - MATRIZ GRAVIDADE %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:7 G(i,1) = diff(Ep,Q(i,1)); end save EqmovCA M DtM C G toc %%% Terminar a Contagem %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 4.3 - EQUAÇÃO DE MOVIMENTO %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%% %%%%%%% %%%%% %%%%% %% M.D2tQ + (DtM - 2C).DtQ + G = T %% %%%%%%%%%% FIM %%%%%%%%%%%%%%
166
clear all clc load parametros %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%% DINÂMICA -CADEIA FECHADA %%%%%%% %%%%%% EQUAÇÃO COM MULTIPLICADORES DE LAGRANGE %%%%%%% %%%%%% OBTENÇÃO EQUAÇÃO DE MOVIMENTO %%%%%%% %%%%%% PARA A FASE DE SUPORTE DUPLO %%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%% 1 - CINEMATICA DIRETA - FASE SUPORTE DUPLO %%%%%%% %% 1.1 - Constantes e parâmetros DH %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% tic %%% Começar a Contagem %%% Ccorpo = 0.4; Cin = 0.1; lde = .03; lpe = .11; lti = .37; lfe = .37; lpl = 0.1; CMp = 0.5; CMt = 0.5; CMf = 0.5; CMd = 0.5; %%%% Valores das Massas e dos momentos de inércia dos corpos do sistema %%%%%%% Md = 0.2; Mp = 0.6; Mt = 4; Mf = 6; Mc = 20; Id = 2.7*10^(-5); Ip = 6*10^(-4); It = 0.046; If = 0.07; Ic = 1.07; g = [0;9.81;0]; %%%% vetor gravidade %%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% posição da juntas %%
167
th1 = pi; syms th2 th3 th4 th5 th6 th7 Pcal = [lpe*cos(th1);lpe*sin(th1);0]; Pjoe = [lti*cos(th1-th2)+ lpe*cos(th1);lti*sin(th1-th2)+ lpe*sin(th1);0]; PjoB = [lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PcaB = [lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PpeB = [lpe*cos(th1-th2+th3-th4-th5+th6)+lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lpe*sin(th1-th2+th3-th4-th5+th6)+lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%% 2 - VELOCIDADE %%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%% Memória de cálculopara velocidade rotacional %% JoR = [0;0;1]; JoSR = [0;0;0]; R1 = [cos(th1),-sin(th1),0;sin(th1),cos(th1),0;0,0,1]; R2 = [cos(th1-th2),-sin(th1-th2),0;sin(th1-th2),cos(th1-th2),0;0,0,1]; R3 = [cos(th1-th2+th3),-sin(th1-th2+th3),0;sin(th1-th2+th3),cos(th1-th2+th3),0;0,0,1]; R4 = [cos(th1-th2+th3-th4),-sin(th1-th2+th3-th4),0;sin(th1-th2+th3-th4),cos(th1-th2+th3-th4),0;0,0,1]; R5 = [cos(th1-th2+th3-th4-th5),-sin(th1-th2+th3-th4-th5),0;sin(th1-th2+th3-th4-th5),cos(th1-th2+th3-th4-th5),0;0,0,1]; R6 = [cos(th1-th2+th3-th4-th5+th6),-sin(th1-th2+th3-th4-th5+th6),0;sin(th1-th2+th3-th4-th5+th6),cos(th1-th2+th3-th4-th5+th6),0;0,0,1]; R7 = [cos(th1-th2+th3-th4-th5+th6+th7),-sin(th1-th2+th3-th4-th5+th6+th7),0;sin(th1-th2+th3-th4-th5+th6+th7),cos(th1-th2+th3-th4-th5+th6+th7),0;0,0,1]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 2.1 - posiçao do centro de massa %%% %% 0 < CM < 1 %%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% P0 = [0;0;0]; PpS = [CMp*lpe*cos(th1);CMp*lpe*sin(th1);0]; PtS = [CMt*lti*cos(th1-th2)+ lpe*cos(th1);CMt*lti*sin(th1-th2)+ lpe*sin(th1);0];
168
PfS = [CMf*lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMf*lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);0]; Pc = [lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin/2]; PfB = [CMf*lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMf*lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PtB = [CMt*lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMt*lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PpB = [CMp*lpe*cos(th1-th2+th3-th4-th5+th6)+lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMp*lpe*sin(th1-th2+th3-th4-th5+th6)+lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PdB = [CMd*lde*cos(th1-th2+th3-th4-th5+th6+th7)+lpe*cos(th1-th2+th3-th4-th5+th6)+lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMd*lde*sin(th1-th2+th3-th4-th5+th6+th7)+lpe*sin(th1-th2+th3-th4-th5+th6)+lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; %%%%%%% 2.2 - Jacobiano %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Q = sym('[th2;th3;th4;th5;th6]'); DtQ = sym('[Dth2;Dth3;Dth4;Dth5;Dth6]'); %%% 2.2.1 - Velocidade do pé %% %% 2.2.2 - Velocidade da Tíbia %% PtSPpS = PtS - Pcal; Jv22 = diff(PtSPpS,'th2'); Jv23 = diff(PtSPpS,'th3'); Jv24 = diff(PtSPpS,'th4'); Jv25 = diff(PtSPpS,'th5'); Jv26 = diff(PtSPpS,'th6'); Jo22 = JoR; Jo23 = [0;0;0]; Jv2 = [Jv22,Jv23,Jv24,Jv25,Jv26]; Jo2 = [Jo22,Jo23,Jo23,Jo23,Jo23]; Vti = Jv2*DtQ; Wti = Jo2*DtQ;
169
%% 2.2.3 - Velocidade do femur %% PfSPpS = PfS - Pcal; PfSPtS = PfS - Pjoe; Jv32 = diff(PfSPpS,'th2'); Jv33 = diff(PfSPtS,'th3'); Jv34 = diff(PfSPtS,'th4'); Jv35 = diff(PfSPtS,'th5'); Jv36 = diff(PfSPtS,'th6'); Jo32 = JoR; Jo33 = JoR; Jo34 = [0;0;0]; Jv3 = [Jv32,Jv33,Jv34,Jv35,Jv36]; Jo3 = [Jo32,Jo33,Jo34,Jo34,Jo34]; Vfe = Jv3*DtQ; Wfe = Jo3*DtQ; %% 2.2.4 EX - Velocidade da cintura %% PcPpS = Pc - Pcal; PcPtS = Pc - Pjoe; PcPfS = Pc - Pc; JvC2 = diff(PcPpS,'th2'); JvC3 = diff(PcPtS,'th3'); JvC4 = diff(PcPfS,'th4'); JvC5 = diff(PcPfS,'th5'); JvC6 = diff(PcPfS,'th6'); JoC2 = JoR; JoC3 = JoR; JoC4 = [0;0;0]; JvC = [JvC2,JvC3,JvC4,JvC5,JvC6]; JoC = [JoC2,JoC3,JoC4,JoC4,JoC4]; Vc = JvC*DtQ; Wc = JoC*DtQ; %% 2.2.4 - Velocidade do femur em balanço %% PfBPpS = PfB - Pcal; PfBPtS = PfB - Pjoe; PfBPcS = PfB - Pc;
170
Jv42 = diff(PfBPpS,'th2'); Jv43 = diff(PfBPtS,'th3'); Jv44 = diff(PfBPcS,'th4'); Jv45 = diff(PfBPcS,'th5'); Jv46 = diff(PfBPcS,'th6'); Jo42 = JoR; Jo43 = JoR; Jo44 = JoR; Jo45 = [0;0;0]; Jv4 = [Jv42,Jv43,Jv44,Jv45,Jv46]; Jo4 = [Jo42,Jo43,Jo44,Jo45,Jo45]; Vfb = Jv4*DtQ; Wfb = Jo4*DtQ; %% 2.2.5 - Velocidade da tíbia em balanço %% PtBPpS = PtB - Pcal; PtBPtS = PtB - Pjoe; PtBPcS = PtB - Pc; PtBPfB = PtB - PjoB; Jv52 = diff(PtBPpS,'th2'); Jv53 = diff(PtBPtS,'th3'); Jv54 = diff(PtBPcS,'th4'); Jv55 = diff(PtBPfB,'th5'); Jv56 = diff(PtBPfB,'th6'); Jo52 = JoR; Jo53 = JoR; Jo54 = JoR; Jo55 = JoR; Jo56 = [0;0;0]; Jv5 = [Jv52,Jv53,Jv54,Jv55,Jv56]; Jo5 = [Jo52,Jo53,Jo54,Jo55,Jo56]; Vtb = Jv5*DtQ; Wtb = Jo5*DtQ; %% 2.2.6 - Velocidade do pe em balanço %% PpBPpS = PpB - Pcal; PpBPtS = PpB - Pjoe; PpBPcS = PpB - Pc; PpBPfB = PpB - PjoB; PpBPtB = PpB - PcaB; Jv62 = diff(PpBPpS,'th2'); Jv63 = diff(PpBPtS,'th3'); Jv64 = diff(PpBPcS,'th4'); Jv65 = diff(PpBPfB,'th5'); Jv66 = diff(PpBPtB,'th6');
171
Jo62 = JoR; Jo63 = JoR; Jo64 = JoR; Jo65 = JoR; Jo66 = JoR; Jv6 = [Jv62,Jv63,Jv64,Jv65,Jv66]; Jo6 = [Jo62,Jo63,Jo64,Jo65,Jo66]; Vpb = Jv6*DtQ; Wpb = Jo6*DtQ; %% 3.2 - Energia Potencial %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Epti = Mt*(g.')*PtS; Epfe = Mf*(g.')*PfS; Epc = Mc*(g.')*Pc; Epfb = Mf*(g.')*PfB; Eptb = Mt*(g.')*PtB; Eppb = Mp*(g.')*PpB; Ep = Epti + Epfe + Epc + Epfb + Eptb + Eppb; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%% 4 - EQUAÇÃO DE MOVIMENTO %%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Its = R2*It*(R2.'); Ifs = R3*If*(R3.'); Ic = R3*Ic*(R3.'); Ifb = R4*If*(R4.'); Itb = R5*It*(R5.'); Ipb = R6*Ip*(R6.'); %% 4.1 - MATRIZ DE MASSA %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MT = Mt*(Jv2.')*Jv2 + Mf*(Jv3.')*Jv3 + Mc*(JvC.')*JvC + Mf*(Jv4.')*Jv4 + Mt*(Jv5.')*Jv5 + Mp*(Jv6.')*Jv6; MR = (Jo2.')*Its*Jo2 + (Jo3.')*Ifs*Jo3 + (JoC.')*Ic*JoC + (Jo4.')*Ifb*Jo4 + (Jo5.')*Itb*Jo5 + (Jo6.')*Ipb*Jo6; M = MT + MR; %% 4.2 - MATRIZ DE AMORTECIMENTO %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
172
Hm1 = sym('[0,0,0,0,0]'); Hm2 = Hm1; G = Hm1.'; S = [G,G]; DtM = (Hm1.')*Hm1; C = (Hm2.')*Hm2; for i=1:5 for j=1:5 for k=1:5 C1 = M(i,j); DC1 = diff(C1,Q(k,1)); Hm1(1,k) = DC1(1,1); C2 = M(j,k); DC2 = diff(C2,Q(i,1)); Hm2(1,k) = DC2(1,1); end DtM(i,j) = Hm1*DtQ; C(i,j) = 0.5*(Hm2*DtQ); end end %% 4.2 - MATRIZ GRAVIDADE %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:5 G(i,1) = diff(Ep,Q(i,1)); end %% 4.3 - Multiplicadore de Lagrange %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% phi1 = lpe*cos(th1-th2+th3-th4-th5+th6)+lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ P; phi2 = lpe*sin(th1-th2+th3-th4-th5+th6)+lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2); for i=1:5 S1 = diff(phi1,Q(i,1)); S(i,1) = S1; end for i=1:5 S2 = diff(phi2,Q(i,1)); S(i,2) = S2; end syms lambda1 lambda2 L = [lambda1;lambda2]; SL = S*L; save EqmovCFml M DtM C G S L
173
toc %%% Terminar a Contagem %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 4.3 - EQUAÇÃO DE MOVIMENTO %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%% %%%%%%% %%%%% %%%%% %% M.D2tQ + (DtM - 2C).DtQ + G = T + SL %% %%%%%%%%%% FIM %%%%%%%%%%%%%%
174
clear all clc tic load eqmovCA load parametros %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%% %%%%%%%% %%%%%%% RESULTADOS OBTIDOS NA DINÂMICA PARA O PASOO %%%%%%%% %%%%%%% E FREQUÊNCIA DESEJADO NA FASE DE SUPORTE SIMPLES %%%%%%%% %%%%%%% %%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% CONSTANTES %% load funtempo load vel load ace T = 2*pi/w; [Lin,Col] = size(th1); t = linspace(0,T/2,Col); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 1. FUNÇÃO NO TEMPO %% th1r =th1; th2r =th2; th3r =th3; th4r =th4; th5r =th5; th6r =th6; th7r =th7; Dth1r = Dth1; Dth2r = Dth2; Dth3r = Dth3; Dth4r = Dth4; Dth5r = Dth5; Dth6r = Dth6; Dth7r = Dth7; MinCA = zeros(7,Col); MamCCA = zeros(7,Col); MamMCA = zeros(7,Col); MgraCA = zeros(7,Col); Cr = zeros(7); DtMr = zeros(7);
175
for i=(2*Col/10+1):Col th1 = th1r(i); th2 = th2r(i); th3 = th3r(i); th4 = th4r(i); th5 = th5r(i);th6 = th6r(i); th7 = th7r(i); Dth1 = Dth1r(i); Dth2 = Dth2r(i); Dth3 = Dth3r(i); Dth4 = Dth4r(i);Dth5 = Dth5r(i); Dth6 = Dth6r(i); Dth7 = Dth7r(i); D2qI = [D2th1(i);D2th2(i);D2th3(i);D2th4(i);D2th5(i);D2th6(i);D2th7(i)]; DqI = [Dth1r(i);Dth2r(i);Dth3r(i);Dth4r(i);Dth5r(i);Dth6r(i);Dth7r(i)]; Mr = eval(M); MinCA(1:7,i)=Mr*D2qI; %%%%%% Torques de Inércia %%%%%% for j=1:3 for k=1:3 CA = C(k,j); Cr(k,j)=eval(CA); end end MamCCA(1:7,i) = Cr*DqI; %%%%%% Torques de F Centrípetas e Coriolis %%%%%% for j=1:3 for k=1:3 CA = DtM(k,j); DtMr(k,j)=eval(CA); end end MamMCA(1:7,i) = DtMr*DqI; %%%%%% Torques de F Centrípetas e Coriolis %%%%%% Gr = eval(G); MgraCA(1:7,i)=Gr; %%%%%% Torques de Gravidade %%%%%% end save TorquesCA MinCA MamCCA MamMCA MgraCA toc %%%%%%%%%% FIM %%%%%%%%%%%%%%
176
clear all clc tic load eqmovCFml load parametros %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%% %%%%%%%% %%%%%%% RESULTADOS OBTIDOS NA DINÂMICA PARA O PASOO %%%%%%%% %%%%%%% E FREQUÊNCIA DESEJADO NA FASE DE SUPORTE DUPLO %%%%%%%% %%%%%%% %%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% CONSTANTES %% load funtempo load vel load ace T = 2*pi/w; [Lin,Col] = size(th1); t = linspace(0,T/2,Col); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 1. FUNÇÃO NO TEMPO %% th2r =th2; th3r =th3; th4r =th4; th5r =th5; th6r =th6; Dth2r = Dth2; Dth3r = Dth3; Dth4r = Dth4; Dth5r = Dth5; Dth6r = Dth6; MinCF = zeros(5,2*Col/10); MamCCF = zeros(5,2*Col/10); MamMCF = zeros(5,2*Col/10); MgraCF = zeros(5,2*Col/10); Sr = zeros(5,2*(2*Col/10)); Sr = sym(Sr); Cr = zeros(5); DtMr = zeros(5); for i=1:(2*Col/10)
177
th2 = th2r(i); th3 = th3r(i); th4 = th4r(i); th5 = th5r(i);th6 = th6r(i); Dth2 = Dth2r(i); Dth3 = Dth3r(i); Dth4 = Dth4r(i);Dth5 = Dth5r(i); Dth6 = Dth6r(i); D2qI = [D2th2(i);D2th3(i);D2th4(i);D2th5(i);D2th6(i)]; DqI = [Dth2r(i);Dth3r(i);Dth4r(i);Dth5r(i);Dth6r(i)]; Mr = eval(M); MinCF(1:5,i)=Mr*D2qI; %%%%%% Torques de Inércia %%%%%% for j=1:3 for k=1:3 CA = C(k,j); Cr(k,j)=eval(CA); end end MamCCF(1:5,i) = Cr*DqI; %%%%%% Torques de F Centrípetas e Coriolis %%%%%% for j=1:3 for k=1:3 CA = DtM(k,j); DtMr(k,j)=eval(CA); end end MamMCF(1:5,i) = DtMr*DqI; %%%%%% Torques de F Centrípetas e Coriolis %%%%%% Gr = eval(G); MgraCF(1:5,i)=Gr; %%%%%% Torques de Gravidade %%%%%% Sr(1:5,2*i-1:2*i) = eval(S); end save TorquesCFml MinCF MamCCF MamMCF MgraCF Sr toc %%%%%%%%%% FIM %%%%%%%%%%%%%%
178
clear all clc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%% %%%%%%%% %%%%%%% RESULTADOS OBTIDOS NA DINÂMICA PARA O PASOO %%%%%%%% %%%%%%% E FREQUÊNCIA DESEJADO AO LONGO DE MEIO CICLO %%%%%%%% %%%%%%% %%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% load torquesCFml load torquesCA load parametros [lin,Col]=size(MinCA); T = 2*pi/w; t = linspace(0,T/2,Col); Ti = zeros(7,Col); TaM = Ti; TaC = Ti; Tg = Ti; Tr = Ti; %%%% Torque de inercia %%% Ti(1:7,1:Col)=MinCA; Ti(2:6,1:(2*Col/10))=MinCF; %%%% Torque de Amortecimento %%% TaM(1:7,1:Col)=MamMCA; TaM(2:6,1:(2*Col/10))=MamMCF; TaC(1:7,1:Col)=MamCCA; TaC(2:6,1:(2*Col/10))=MamCCF; %%% Torque de gravidade %%% Tg(1:7,1:Col)=MgraCA; Tg(2:6,1:(2*Col/10))=MgraCF; Ta = TaM - TaC; %%% torque de restrição %%% Tm = Ti + TaM - TaC + Tg; for i=1:(2*Col/10) Lambda2 = (Tm(5,i)*Sr(2,2*i-1)/Sr(4,2*i-1)-Tm(3,i))/(Sr(4,2*i)*Sr(2,2*i-1)/Sr(4,2*i-1)-Sr(2,2*i)); Lambda1 = (Tm(3,i)-Sr(2,2*i)*Lambda2)/Sr(2,2*i-1); Tr(2:6,i)=Sr(:,2*i-1:2*i)*[Lambda1;Lambda2]; end %%% Torque Motor %%% Tm = Ti + TaM - TaC + Tg - Tr;
179
save TorqueMotor Tm %% Plotagens %% %%%%%%%%%%%%%%% %%%% Torque de inérjcia %%%%% plot(t,Ti(1,:),'r--',t,Ti(2,:),'b-',t,Ti(3,:),'k-.',t,Ti(4,:),'g.') title(texlabel(' theta1(--) theta2(-) theta3(-.-) theta4(.)')) ylabel(texlabel('Torque de Inércia(Nm)')) xlabel('t(s)') figure plot(t,Ti(5,:),'r--',t,Ti(6,:),'b-',t,Ti(7,:),'k-.') title(texlabel(' theta5(--) theta6(-) theta7(-.-)')) ylabel(texlabel('Torque de Inércia(Nm)')) xlabel('t(s)') figure plot(t,Ti(4,:),'r--',t,Ti(6,:),'b-',t,Ti(7,:),'k-.') title(texlabel('theta4(--) theta6(-) theta7(-.-)')) ylabel(texlabel('Torque de inércia(Nm)')) xlabel('t(s)') figure %%%%% Torque Amortecimento %%%%% plot(t,Ta(1,:),'r--',t,Ta(2,:),'b-',t,Ta(3,:),'k-.',t,Ta(4,:),'g.') title(texlabel(' theta1(--) theta2(-) theta3(-.-) theta4(.)')) ylabel(texlabel('Torque de Amortecimento(Nm)')) xlabel('t(s)') figure plot(t,Ta(5,:),'r--',t,Ta(6,:),'b-',t,Ta(7,:),'k-.') title(texlabel(' theta5(--) theta6(-) theta7(-.-)')) ylabel(texlabel('Torque de Amortecimento(Nm)')) xlabel('t(s)') figure plot(t,Ta(4,:),'r--',t,Ta(6,:),'b-',t,Ta(7,:),'k-.') title(texlabel('theta4(--) theta6(-) theta7(-.-)')) ylabel(texlabel('Torque de Amortecimento(Nm)')) xlabel('t(s)') figure %%% Torque de Gravidade %%%% plot(t,Tg(1,:),'r--',t,Tg(2,:),'b-',t,Tg(3,:),'k-.',t,Tg(4,:),'g.') title(texlabel(' theta1(--) theta2(-) theta3(-.-) theta4(.)')) ylabel(texlabel('Torque de Gravidade(Nm)')) xlabel('t(s)') figure plot(t,Tg(5,:),'r--',t,Tg(6,:),'b-',t,Tg(7,:),'k-.') title(texlabel(' theta5(--) theta6(-) theta7(-.-)')) ylabel(texlabel('Torque de Gravidade(Nm)')) xlabel('t(s)') figure plot(t,Tg(4,:),'r--',t,Tg(6,:),'b-',t,Tg(7,:),'k-.') title(texlabel('theta4(--) theta6(-) theta7(-.-)'))
180
ylabel(texlabel('Torque de Gravidade(Nm)')) xlabel('t(s)') figure %%% Torque Motor %%%%%%% plot(t,Tm(1,:),'r--',t,Tm(2,:),'b-',t,Tm(3,:),'k-.',t,Tm(4,:),'g.') title(texlabel(' T1(--) T2(-) T3(-.-) T4(.)')) ylabel(texlabel('Torque (Nm)')) xlabel('t(s)') figure plot(t,Tm(5,:),'r--',t,Tm(6,:),'b-',t,Tm(7,:),'k-.') title(texlabel(' T5(--) T6(-) T7(-.-)')) ylabel(texlabel('Torque (Nm)')) xlabel('t(s)') figure plot(t,Tm(4,:),'r--',t,Tm(6,:),'b-',t,Tm(7,:),'k-.') title(texlabel('T4(--) T6(-) T7(-.-)')) ylabel(texlabel('Torque (Nm)')) xlabel('t(s)') %%%%%%%%%% FIM %%%%%%%%%%%%%%
181
Anexo C. Programação da Memória de Cálculo do Dispositivo
Giroscópio Desenvolvido no MatLab
clear all clc load parametros %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%% %%%%% %%%%% OBTENÇÃO DAS VELOCIDADES E ACELERAÇÕES DOS CENTROS DE MASSA DO %%%%% %%%%% SISTEMA EM RELAÇÃO A O REFERENCIAL INERCIAL PARA OS VALORES %%%%% %%%%% DO PASSO E FREQUÊNCIA DESEJADOS %%%%% %%%%% %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%% 1 - CINEMATICA DIRETA - FASE BALANCO %%%%%%% %% 1.1 - Constantes e prametros DH %% %%%%%%%%%%%%%%%%%%%%%%%%%%%% tic %%% Começar a Contagem %%% Cin = 0.1; lde = .03; lpe = .11; lti = .37; lfe = .37; CMp = 0.5; CMt = 0.5; CMf = 0.5; CMd = 0.5; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% posição das juntas %% syms th1 th2 th3 th4 th5 th6 th7 Pcal = [lpe*cos(th1);lpe*sin(th1);0]; Pjoe = [lti*cos(th1-th2)+ lpe*cos(th1);lti*sin(th1-th2)+ lpe*sin(th1);0]; PjoB = [lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin];
182
PcaB = [lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PpeB = [lpe*cos(th1-th2+th3-th4-th5+th6)+lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lpe*sin(th1-th2+th3-th4-th5+th6)+lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%% 2 - VELOCIDADE %%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%% Memória de cálculopara velocidade rotacional %% JoR = [0;0;1]; JoSR = [0;0;0]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 2.1 - posição do centro de massa %%% %% 0 < CM < 1 %%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% P0 = [0;0;0]; PpS = [CMp*lpe*cos(th1);CMp*lpe*sin(th1);0]; PtS = [CMt*lti*cos(th1-th2)+ lpe*cos(th1);CMt*lti*sin(th1-th2)+ lpe*sin(th1);0]; PfS = [CMf*lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMf*lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);0]; Pc = [lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin/2]; PfB = [CMf*lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMf*lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PtB = [CMt*lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMt*lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PpB = [CMp*lpe*cos(th1-th2+th3-th4-th5+th6)+lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMp*lpe*sin(th1-th2+th3-th4-th5+th6)+lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin]; PdB = [CMd*lde*cos(th1-th2+th3-th4-th5+th6+th7)+lpe*cos(th1-th2+th3-th4-th5+th6)+lti*cos(th1-th2+th3-th4-th5) + lfe*cos(th1-th2+th3-th4)+ lfe*cos(th1-th2+th3) + lti*cos(th1-th2)+ lpe*cos(th1);CMd*lde*sin(th1-th2+th3-th4-th5+th6+th7)+lpe*sin(th1-th2+th3-th4-th5+th6)+lti*sin(th1-th2+th3-th4-th5)+ lfe*sin(th1-th2+th3-th4) + lfe*sin(th1-th2+th3)+lti*sin(th1-th2)+ lpe*sin(th1);Cin];
183
%%%%%%% 2.2 - Jacobiano %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Q = sym('[th1;th2;th3;th4;th5;th6;th7]'); DtQ = sym('[Dth1;Dth2;Dth3;Dth4;Dth5;Dth6;Dth7]'); D2tQ = sym('[D2th1;D2th2;D2th3;D2th4;D2th5;D2th6;D2th7]'); %%% 2.2.1 - Velocidade e Aceleração do pé %%% PpSP0=PpS-P0; Jv11 = diff(PpSP0,'th1'); Jv12 = diff(PpSP0,'th2'); Jv13 = diff(PpSP0,'th3'); Jv14 = diff(PpSP0,'th4'); Jv15 = diff(PpSP0,'th5'); Jv16 = diff(PpSP0,'th6'); Jv17 = diff(PpSP0,'th7'); Jo11 = JoR; Jo12 = [0;0;0]; %%% Jo12 = Jo13 = Jo14... = Jo17 %% Jv1 = [Jv11,Jv12,Jv13,Jv14,Jv15,Jv16,Jv17]; Jo1 = [Jo11,Jo12,Jo12,Jo12,Jo12,Jo12,Jo12]; Vpe = Jv1*DtQ; Wpe = Jo1*DtQ; %%% Aceleração do pé %%% DJv1 = zeros(3,7); DJv1 = sym(DJv1); Hm1 = sym('[0,0,0,0,0,0,0]'); for i=1:3 for j=1:7 for k=1:7 Hm1(1,k) = diff(Jv1(i,j),Q(k,1)); end DJv1(i,j) = Hm1*DtQ; end end Ape = DJv1*DtQ + Jv1*D2tQ; Alphape = Jo1*D2tQ; %% 2.2.2 - Velocidade da Tibia %% PtSP0 = PtS - P0; PtSPpS = PtS - Pcal; Jv21 = diff(PtSP0,'th1'); Jv22 = diff(PtSPpS,'th2'); Jv23 = diff(PtSPpS,'th3'); Jv24 = diff(PtSPpS,'th4');
184
Jv25 = diff(PtSPpS,'th5'); Jv26 = diff(PtSPpS,'th6'); Jv27 = diff(PtSPpS,'th7'); Jo21 = JoR; Jo22 = JoR; Jo23 = [0;0;0]; %%% Jo23 = Jo24... = Jo27 %% Jv2 = [Jv21,Jv22,Jv23,Jv24,Jv25,Jv26,Jv27]; Jo2 = [Jo21,Jo22,Jo23,Jo23,Jo23,Jo23,Jo23]; Vti = Jv2*DtQ; Wti = Jo2*DtQ; %%% Aceleração da Tíbia %%% DJv2 = zeros(3,7); DJv2 = sym(DJv2); Hm2 = sym('[0,0,0,0,0,0,0]'); for i=1:3 for j=1:7 for k=1:7 Hm2(1,k) = diff(Jv2(i,j),Q(k,1)); end DJv2(i,j) = Hm2*DtQ; end end Ati = DJv2*DtQ + Jv2*D2tQ; Alphati = Jo2*D2tQ; %% 2.2.3 - Velocidade do femur %% PfSP0 = PfS - P0; PfSPpS = PfS - Pcal; PfSPtS = PfS - Pjoe; Jv31 = diff(PfSP0,'th1'); Jv32 = diff(PfSPpS,'th2'); Jv33 = diff(PfSPtS,'th3'); Jv34 = diff(PfSPtS,'th4'); Jv35 = diff(PfSPtS,'th5'); Jv36 = diff(PfSPtS,'th6'); Jv37 = diff(PfSPtS,'th7'); Jo31 = JoR; Jo32 = JoR; Jo33 = JoR; Jo34 = [0;0;0]; %%% Jo34... = Jo37 %% Jv3 = [Jv31,Jv32,Jv33,Jv34,Jv35,Jv36,Jv37]; Jo3 = [Jo31,Jo32,Jo33,Jo34,Jo34,Jo34,Jo34];
185
Vfe = Jv3*DtQ; Wfe = Jo3*DtQ; %%% Aceleração do femur %%% DJv3 = zeros(3,7); DJv3 = sym(DJv3); Hm3 = sym('[0,0,0,0,0,0,0]'); for i=1:3 for j=1:7 for k=1:7 Hm3(1,k) = diff(Jv3(i,j),Q(k,1)); end DJv3(i,j) = Hm3*DtQ; end end Afe = DJv3*DtQ + Jv3*D2tQ; Alphafe = Jo3*D2tQ; %% 2.2.4 EX - Velocidade da cintura %% %pcc = sym('[0;0;0.5*lpv]'); %Pc = Tcin(1:3,4) + pcc; PcP0 = Pc - P0; PcPpS = Pc - Pcal; PcPtS = Pc - Pjoe; PcPfS = Pc - Pc; JvC1 = diff(PcP0,'th1'); JvC2 = diff(PcPpS,'th2'); JvC3 = diff(PcPtS,'th3'); JvC4 = diff(PcPfS,'th4'); JvC5 = diff(PcPfS,'th5'); JvC6 = diff(PcPfS,'th6'); JvC7 = diff(PcPfS,'th7'); JoC1 = JoR; JoC2 = JoR; JoC3 = JoR; JoC4 = [0;0;0]; JvC = [JvC1,JvC2,JvC3,JvC4,JvC5,JvC6,JvC7]; JoC = [JoC1,JoC2,JoC3,JoC4,JoC4,JoC4,JoC4]; Vc = JvC*DtQ; Wc = JoC*DtQ; %%% Aceleração do femur %%% DJvC = zeros(3,7); DJvC = sym(DJvC); HmC = sym('[0,0,0,0,0,0,0]'); for i=1:3 for j=1:7 for k=1:7
186
HmC(1,k) = diff(JvC(i,j),Q(k,1)); end DJvC(i,j) = HmC*DtQ; end end Acin = DJvC*DtQ + JvC*D2tQ; Alphacin = JoC*D2tQ; %% 2.2.4 - Velocidade do femur em balanço %% PfBP0 = PfB - P0; PfBPpS = PfB - Pcal; PfBPtS = PfB - Pjoe; PfBPcS = PfB - Pc; Jv41 = diff(PfBP0,'th1'); Jv42 = diff(PfBPpS,'th2'); Jv43 = diff(PfBPtS,'th3'); Jv44 = diff(PfBPcS,'th4'); Jv45 = diff(PfBPcS,'th5'); Jv46 = diff(PfBPcS,'th6'); Jv47 = diff(PfBPcS,'th7'); Jo41 = JoR; Jo42 = JoR; Jo43 = JoR; Jo44 = JoR; Jo45 = [0;0;0]; Jv4 = [Jv41,Jv42,Jv43,Jv44,Jv45,Jv46,Jv47]; Jo4 = [Jo41,Jo42,Jo43,Jo44,Jo45,Jo45,Jo45]; Vfb = Jv4*DtQ; Wfb = Jo4*DtQ; %%% Aceleração do femur em balanço %%% DJv4 = zeros(3,7); DJv4 = sym(DJv4); Hm4 = sym('[0,0,0,0,0,0,0]'); for i=1:3 for j=1:7 for k=1:7 Hm4(1,k) = diff(Jv4(i,j),Q(k,1)); end DJv4(i,j) = Hm4*DtQ; end end Afb = DJv4*DtQ + Jv4*D2tQ; Alphafb = Jo4*D2tQ; %% 2.2.5 - Velocidade da tibia em balanço %% PtBP0 = PtB - P0; PtBPpS = PtB - Pcal;
187
PtBPtS = PtB - Pjoe; PtBPcS = PtB - Pc; PtBPfB = PtB - PjoB; Jv51 = diff(PtBP0,'th1'); Jv52 = diff(PtBPpS,'th2'); Jv53 = diff(PtBPtS,'th3'); Jv54 = diff(PtBPcS,'th4'); Jv55 = diff(PtBPfB,'th5'); Jv56 = diff(PtBPfB,'th6'); Jv57 = diff(PtBPfB,'th7'); Jo51 = JoR; Jo52 = JoR; Jo53 = JoR; Jo54 = JoR; Jo55 = JoR; Jo56 = [0;0;0]; Jv5 = [Jv51,Jv52,Jv53,Jv54,Jv55,Jv56,Jv57]; Jo5 = [Jo51,Jo52,Jo53,Jo54,Jo55,Jo56,Jo56]; Vtb = Jv5*DtQ; Wtb = Jo5*DtQ; %%% Aceleração da tíbia em balanço %%% DJv5 = zeros(3,7); DJv5 = sym(DJv5); Hm5 = sym('[0,0,0,0,0,0,0]'); for i=1:3 for j=1:7 for k=1:7 Hm5(1,k) = diff(Jv5(i,j),Q(k,1)); end DJv5(i,j) = Hm5*DtQ; end end Atb = DJv5*DtQ + Jv5*D2tQ; Alphatb = Jo5*D2tQ; %% 2.2.6 - Velocidade do pe em balanço %% PpBP0 = PpB - P0; PpBPpS = PpB - Pcal; PpBPtS = PpB - Pjoe; PpBPcS = PpB - Pc; PpBPfB = PpB - PjoB; PpBPtB = PpB - PcaB; Jv61 = diff(PpBP0,'th1'); Jv62 = diff(PpBPpS,'th2'); Jv63 = diff(PpBPtS,'th3'); Jv64 = diff(PpBPcS,'th4'); Jv65 = diff(PpBPfB,'th5'); Jv66 = diff(PpBPtB,'th6');
188
Jv67 = diff(PpBPtB,'th7'); Jo61 = JoR; Jo62 = JoR; Jo63 = JoR; Jo64 = JoR; Jo65 = JoR; Jo66 = JoR; Jo67 = [0;0;0]; Jv6 = [Jv61,Jv62,Jv63,Jv64,Jv65,Jv66,Jv67]; Jo6 = [Jo61,Jo62,Jo63,Jo64,Jo65,Jo66,Jo66]; Vpb = Jv6*DtQ; Wpb = Jo6*DtQ; %%% Aceleração do pé balanço %%% DJv6 = zeros(3,7); DJv6 = sym(DJv6); Hm6 = sym('[0,0,0,0,0,0,0]'); for i=1:3 for j=1:7 for k=1:7 Hm6(1,k) = diff(Jv6(i,j),Q(k,1)); end DJv6(i,j) = Hm6*DtQ; end end Apb = DJv6*DtQ + Jv6*D2tQ; Alphapb = Jo6*D2tQ; %% 2.2.7 - Velocidade do dedo em balanço %% PdBP0 = PdB - P0; PdBPpS = PdB - Pcal; PdBPtS = PdB - Pjoe; PdBPcS = PdB - Pc; PdBPfB = PdB - PjoB; PdBPtB = PdB - PcaB; PdBPpB = PdB - PpeB; Jv71 = diff(PdBP0,'th1'); Jv72 = diff(PdBPpS,'th2'); Jv73 = diff(PdBPtS,'th3'); Jv74 = diff(PdBPcS,'th4'); Jv75 = diff(PdBPfB,'th5'); Jv76 = diff(PdBPtB,'th6'); Jv77 = diff(PdBPpB,'th7'); Jo71 = JoR; Jo72 = JoR; Jo73 = JoR; Jo74 = JoR; Jo75 = JoR; Jo76 = JoR; Jo77 = JoR;
189
Jv7 = [Jv71,Jv72,Jv73,Jv74,Jv75,Jv76,Jv77]; Jo7 = [Jo71,Jo72,Jo73,Jo74,Jo75,Jo76,Jo76]; Vdb = Jv7*DtQ; Wdb = Jo7*DtQ; %%% Aceleração do dedo balanço %%% DJv7 = zeros(3,7); DJv7 = sym(DJv7); Hm7 = sym('[0,0,0,0,0,0,0]'); for i=1:3 for j=1:7 for k=1:7 Hm7(1,k) = diff(Jv7(i,j),Q(k,1)); end DJv7(i,j) = Hm7*DtQ; end end Adb = DJv7*DtQ + Jv7*D2tQ; Alphadb = Jo7*D2tQ; save ACA Ape Ati Afe Acin Afb Atb Apb Adb Alphape Alphati Alphafe Alphacin Alphafb Alphatb Alphapb Alphadb save VCA Vpe Vti Vfe Vc Vfb Vtb Vpb Vdb Wpe Wti Wfe Wc Wfb Wtb Wpb Wdb %%%%%%%%%% FIM %%%%%%%%%%%%%%
190
clear all clc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%% %%%%%%%%%%%%%% %%%%% OBTENÇÃO DAS REAÇÕE EXISTENTES NA CINTURA %%%%%%%%%%%%%% %%%%% %%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 1 - Parâmetros %% load parametros load ACA load ace load funTempo load vel load TorqueMotor Ccorpo = 0.4; Cin = 0.1; lde = .03; lpe = .11; lti = .37; lfe = .37; lpl = 0.1; CMp = 0.5; CMt = 0.5; CMf = 0.5; CMd = 0.5; Md = 0.2; Mp = 0.6; Mt = 4; Mf = 6; Mc = 20; Id = 2.7*10^(-5); Ip = 6*10^(-4); It = 0.046; If = 0.07; Ic = 1.07; g = 9.81; T = 2*pi/w; [Lin,Col] = size(th1); t = linspace(0,T/2,Col); ape = zeros(3,Col); ati = zeros(3,Col); afe = zeros(3,Col); acin = zeros(3,Col); afb = zeros(3,Col); atb = zeros(3,Col); apb = zeros(3,Col); adb = zeros(3,Col);
191
Alpe = zeros(3,Col); Alti = zeros(3,Col); Alfe = zeros(3,Col); Alcin = zeros(3,Col); Alfb = zeros(3,Col); Altb = zeros(3,Col); Alpb = zeros(3,Col); Aldb = zeros(3,Col); FB = zeros(6,Col); FS = zeros(6,Col); Fcin=zeros(2,Col); th1r = th1; th2r = th2; th3r = th3; th4r = th4; th5r = th5; th6r = th6; th7r = th7; Dth1r = Dth1; Dth2r = Dth2; Dth3r = Dth3; Dth4r = Dth4; Dth5r = Dth5; Dth6r = Dth6; Dth7r = Dth7; D2th1r = D2th1; D2th2r = D2th2; D2th3r = D2th3; D2th4r = D2th4; D2th5r = D2th5; D2th6r = D2th6; D2th7r = D2th7; for i=1:Col th1 = th1r(1,i); th2 = th2r(1,i); th3 = th3r(1,i); th4 = th4r(1,i); th5 = th5r(1,i); th6 = th6r(1,i); th7 = th7r(1,i); Dth1 = Dth1r(1,i); Dth2 = Dth2r(1,i); Dth3 = Dth3r(1,i); Dth4 = Dth4r(1,i); Dth5 = Dth5r(1,i); Dth6 = Dth6r(1,i); Dth7 = Dth7r(1,i); D2th1 = D2th1r(1,i); D2th2 = D2th2r(1,i); D2th3 = D2th3r(1,i); D2th4 = D2th4r(1,i); D2th5 = D2th5r(1,i); D2th6 = D2th6r(1,i); D2th7 = D2th7r(1,i); ape(:,i) = eval(Ape); ati(:,i) = eval(Ati); afe(:,i) = eval(Afe); acin(:,i) = eval(Acin); afb(:,i) = eval(Afb); atb(:,i) = eval(Atb); apb(:,i) = eval(Apb); adb(:,i) = eval(Adb); Alpe(:,i) = eval(Alphape); Alti(:,i) = eval(Alphati); Alfe(:,i) = eval(Alphafe); Alcin(:,i) = eval(Alphacin); Alfb(:,i) = eval(Alphafb); Altb(:,i) = eval(Alphatb); Alpb(:,i) = eval(Alphapb); Aldb(:,i) = eval(Alphadb); end for i=1:Col phi1 = th1r(1,i); phi2 = th1r(1,i) - th2r(1,i); phi3 = th1r(1,i) - th2r(1,i) + th3r(1,i); phi4 = th1r(1,i) - th2r(1,i) + th3r(1,i) - th4r(1,i); phi5 = th1r(1,i) - th2r(1,i) + th3r(1,i) - th4r(1,i) - th5r(1,i); phi6 = th1r(1,i) - th2r(1,i) + th3r(1,i) - th4r(1,i) - th5r(1,i) +th6r(1,i);
192
phi7 = th1r(1,i) - th2r(1,i) + th3r(1,i) - th4r(1,i) - th5r(1,i) +th6r(1,i) + th7r(1,i); MFsf = [1,0,-1,0;0,1,0,-1]; MFzr = [0,0;0,0]; MFBm5 = [lti/2*sin(phi5),lti/2*cos(phi5),lti/2*sin(phi5),lti/2*cos(phi5),0,0]; MFBm4 = [0,0,lfe/2*sin(phi4),lfe/2*cos(phi4),lfe/2*sin(phi4),lfe/2*cos(phi4)]; MFSm2 = [lti/2*sin(phi2),lti/2*cos(pi-phi2),lti/2*sin(phi2),lti/2*cos(pi-phi2),0,0]; MFSm3 = [0,0,lfe/2*sin(phi3),lfe/2*cos(pi-phi3),lfe/2*sin(phi3),lfe/2*cos(pi-phi3)]; MFB = [MFsf,MFzr;MFBm5;MFzr,MFsf;MFBm4]; MFS = [MFsf,MFzr;MFSm2;MFzr,MFsf;MFSm3]; VFB = [(Mt*atb(1,i));(Mt*(atb(2,i)+g));(It*Altb(3,i) - Tm(5,i) + Tm(6,i));(Mf*afb(1,i));(Mf*(afb(2,i)+g));(If*Alfb(3,i) -Tm(4,i)+Tm(5,i))]; VFS = [(Mt*ati(1,i));(Mt*(ati(2,i)+g));(It*Alti(3,i) + Tm(3,i) - Tm(2,i));(Mf*afe(1,i));(Mf*(afe(2,i)+g));(If*Alfe(3,i) +Tm(4,i)-Tm(3,i))]; FB(:,i) = MFB\VFB; FS(:,i) = MFS\VFS; end Fcin(1,:)=-FS(6,:); Fcin(2,:)=-FB(6,:); plot(t,Fcin(1,:),'r',t,Fcin(2,:),'b') xlabel('t') ylabel('força suporte-vermelho força balanço- azul') save FG FB FS %%%%%%%%%% FIM %%%%%%%%%%%%%%
193
clear all clc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%% %%%%% %%%%% OBTENÇÃO DAS VELOCIDADES DE PRECESSÃO %%%%% %%%%% PARA VELOCIDADES DE SPIN DESEJADAS %%%%% %%%%% %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% load parametros load FG lcin = 0.1; Mc = 20; %%%% Massa da cintura %%% Ic = 1.07; %%%% Momento de inércia da cintura %%% T = 2*pi/w; [Lin,Col] = size(FB); t = linspace(0,T/2,Col); n=100; %%%% n = numero de rotações de spin por segundo %% ws = zeros(1,5); wp = zeros(5,Col); for j=1:5 ws(1,j) = n*2*pi; n=n-20; for i=1:Col wp(j,i) =(lcin*(FS(1,i)-FB(1,i)))/(2*Ic*ws(1,j)); end end plot(t,wp(1,:),'r-',t,wp(2,:),'k--',t,wp(3,:),'b-.',t,wp(4,:),'r.',t,wp(5,:),'ko') title(texlabel('omega =100rad/s(-), omegas =80rad/s(--),omegas =60rad/s(-.), omegas =40rad/s(.), omegas =20rad/s(o)')) ylabel(texlabel('omegap (rad/s)')) xlabel('t(s)') %%%%%%%%%% FIM %%%%%%%%%%%%%%