Universidade Federal do Rio de Janeiro MODELAGEM ...w2.files.scire.net.br/atrio/ufrj-pem_upl/THESIS/118/pemufrj2009msc... · 4.5.1 Obtenção da Energia Cinética ... estudo da marcha

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



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



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.


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.)



Junho/2009


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


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


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.


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.


68


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ÀNGELO, 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ÀNGELO, 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ÀNGELO, 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 %%%%%%%%%%%%%%

Documents

Universidade Federal do Rio de Janeiro MODELAGEM ...w2.files.scire.net.br/atrio/ufrj-pem_upl/THESIS/118/pemufrj2009msc... · 4.5.1 Obtenção da Energia Cinética ... estudo da marcha