MODELO CINEMÁTICO DE UM ROBÔ MÓVEL

Transcrição

1 MODELO CINEMÁTICO DE UM ROBÔ MÓVEL y r v ω r E v E y ω E v D b ω D r D θ x x (x,y) = Posição do referencial fixo no robô em relação ao referencial fixo no espaço de trabalho. θ = Ângulo de orientação do robô em relação ao referencial fixo no espaço de trabalho. b = Comprimento do eixo. r = Raio de giro do robô. r D (r E ) = Raio da roda direita (esquerda) ω = Velocidade angular do robô. ω D (ω E ) = Velocidade angular da roda direita (esquerda). v = Velocidade linear do robô v = ω.r v D (v E ) = Velocidade linear da borda da roda direita (esquerda). v D = ω D.r D v E = ω E.r E 1

2 Para movimentos infinitesimais: r b ω.dt v E.dt v.dt v D.dt v D.dt = ω(r+b/2)dt v D +v E = ω D.r D +ω E.r E = 2.ω.r = 2.v v E.dt = ω(r-b/2)dt v D -v E = ω D.r D -ω E.r E = ω.b v = (r D /2) (r E /2). ω D V = V T W.W ω (r D /b) -(r E /b) ω E onde: V = [v ω] T W = [ω D ω E ] T V T W = (r D /2) (r E /2) (r D /b) -(r E /b) W = ( V T W ) -1.V = W T V.V onde, W T V = ( V T W ) -1 2

3 Relação entre velocidades das rodas (ω E /ω D ) para mover-se com raio de giro r: (2.ω.r)/(ω.b) = (2.r/b) = (ω D.r D +ω E.r E )/( ω D.r D -ω E.r E ) (ω E /ω D ) = [(r-b/2).r D ]/[(r+b/2).r E ] Restrições não holonômicas: Restrições não holonômicas atuam nas velocidades do robô. Devido ao atrito das rodas, o robô não pode se deslocar lateralmente (na direção do eixo). A velocidade linear sempre aponta na direção definida pela orientação θ do robô. y v θ x v.dt θ dx dy Para deslocamentos infinitesimais: dx = v.dt.cosθ dx/dt = x' = v.cosθ dy = v.dt.senθ dy/dt = y' = v.senθ dθ = ω.dt dθ/dt = θ' = ω (dy/dx) = tan(θ) = senθ/cosθ dy.cosθ - dx.senθ = 0 v.dt = dx.cosθ + dy.senθ cosθ senθ x' = v - senθ cosθ y' 0 3

4 Modelo cinemático: Definindo o Vetor de Variáveis de Configuração, ou, simplesmente, configuração q = [x y θ] T, lembrando que: x' = v.cosθ; y' = v.senθ; θ' = ω; x' cosθ 0 x' cosθ 0 y' = senθ 0. y' onde, senθ 0 = q T V θ' 0 1 θ' 0 1 q' = q T V.V q' = q T V. V T W.W Propriedades da matriz q T V : i) ( q T V ) T.( q T V ) = cosθ senθ 0. cosθ 0 = senθ ( q T V ) T.q' = ( q T V ) T. ( q T V ).V = V V = ( q T V ) T.q' ii) ( q T V ).( q T V ) T = cosθ 0. cosθ senθ 0 senθ ( q T V ).( q T V ) T = cos 2 θ cosθ.senθ 0 cosθ.senθ sen 2 θ onde, a matriz ( q T V ).( q T V ) T é singular. Apesar disto, q' = ( q T V ).( q T V ) T.q' visto que, de (i): ( q T V ).( q T V ) T.q' = ( q T V ).V = q' 4

5 iii) ( q T V ) T = ( q T V ) + Ou seja, ( q T V ) T é a matriz pseudo-inversa de q T V, o que decorre diretamente de (i) e (ii). iv) ( q T V ) T.( q T V )' = cosθ senθ 0. -senθ 0.θ' = cosθ q'' = (q')' = (( q T V ).V)' = q T V.V' + ( q T V )'.V ( q T V ) T. q'' = ( q T V ) T. q T V.V'' + ( q T V ) T.( q T V )'.V ( q T V ) T. q'' = V'' v) ( q T V ).(( q T V ) T )' = cosθ 0. -senθ cosθ 0.θ' senθ ( q T V ).(( q T V ) T )' = - cosθ.senθ cos 2 θ 0.θ' - sen 2 θ cosθ.senθ onde, a matriz ( q T V ).(( q T V ) T )' é singular. V = (( q T V ) T.q')' = ( q T V ) T.q'' + (( q T V ) T )'.q' ( q T V ).V' = ( q T V ).(( q T V ) T.q')' = ( q T V ).( q T V ) T.q'' + ( q T V ).(( q T V ) T )'.q' Mas, (( q T V ) T )'.q' = θ'. -senθ cosθ 0. x' = y' 0 θ' ( q T V ).V' = ( q T V ).( q T V ) T.q'' multiplicando por ( q T V ) T V' = ( q T V ) T.q'' 5

6 ESFORÇOS ESTÁTICOS y τ θ f L f E τ E f D b τ D θ x f L = Força resultante no robô. f D (f E ) = força na borda da roda direita (esquerda). τ θ = Torque resultante no robô. τ D (τ E ) = Torque na roda direita (esquerda). τ D = f D.r D τ E = f E.r E f D = τ D /r D f E = τ E /r E 6

7 Esforços resultantes no robô, F V = [f L τ θ ] T : f L = f D + f E = (τ D /r D ) + (τ E /r E ) τ θ = (f D.b/2) - (f E.b/2) = (τ D.b/2r D ) - (τ E.b/2r E ) f L = (1/r D ) (1/r E ). τ D F V = (( V T W ) T ) -1.τ τ θ (b/2r D ) (-b/2r E ) τ E onde, F V = [f L τ θ ] T τ = [τ D τ E ] T (1/r D ) (1/r E ) = ( W T V ) T = (( V T W ) T ) -1 (b/2r D ) (-b/2r E ) τ = ( V T W ) T. F V Esforços em espaço de configuração, F = [f x f y τ θ ] T : f x = f L.cosθ f x cosθ 0. f L f y = f L.senθ f y = senθ 0 τ θ τ θ = τ θ τ θ 0 1 F = q T V.F V ( q T V ) T.F = ( q T V ) T. q T V.F V F V = ( q T V ) T.F F = q T V.(( V T W ) T ) -1.τ τ = ( V T W ) T.( q T V ) T.F Conservação de Potência: Sejam: potência em espaço de configuração = P q = F T.q' potência em espaço de atuadores = P W = τ T.W P q = F T.q' = [ q T V.(( V T W ) T ) -1.τ] T.[ q T V. V T W.W] = τ T.( V T W ) -1 ( q T V ) T. q T V. V T W.W = τ T.( V T W ) -1. V T W.W = τ T.W = P W P q = P W 7

8 MODELO DINÂMICO DE UM ROBÔ MÓVEL Robô Móvel: duas rodas, com acionamento diferencial através de motores CC. Dinâmica de Atuadores (Motores CC): 1/ρ D,E i D,E K D,E. ω D,E K D,E. i D,E e D,E ± ± J D,E 1/β D,E τ D,E J D,E. ω' D,E β D,E. ω D,E e D,E = Tensão de armadura do motor (Direito, Esquerdo). i D,E = Corrente de armadura do motor (Direito, Esquerdo). 1/ρ D,E = Resistência de armadura (Direita, Esquerda). K D,E = Constante de força contra-eletromotriz / torque do motor (Direito, Esquerdo). J D,E = Momento de inércia do rotor (Direito, Esquerdo). β D,E = Coeficiente de atrito do motor (Direito, Esquerdo). τ D,E = Carga mecânica no motor (Direito, Esquerdo). Equação Elétrica do Motor: E = ρ -1.i + K W.W i = -ρ.k W.W + ρ.e onde: E = [e D e E ] T i = [i D i E ] T W = [ω D ω E ] T ρ = ρ D 0 K W = K D 0 0 ρ E 0 K E 8

9 Equação Mecânica do Motor: K W.i = J W.W' + β W.W + τ onde: τ = [τ D τ E ] T J W = J D 0 β W = β D 0 0 J E 0 β E Substituindo a equação elétrica na equação mecânica do motor: τ = - J W.W' -(ρ.k W 2 + β W ).W + ρ.k W.E lembrando que: W = W T V.V e W' = W T V.V', τ = - J W. W T V.V' -(ρ.k W 2 + β W ). W T V.V + ρ.k W.E Dinâmica do Robô: Equação Mecânica do Robô: τ θ f L J θ 1/β θ m 1/β L J θ. ω' β θ. ω m. v' β L. v onde, m = Massa do robô. J θ = Momento de inércia do robô. β L = Coeficiente de atrito das rodas em movimento linear. β θ = Coeficiente de atrito das rodas em movimento rotacional. 9

10 Lei de Newton: f L = m.v' + β L.v Lei de Euler: τ θ = J θ.ω' + β θ.ω F V = J V.V' + β V.V onde: F V = [f L τ θ ] T J V = m 0 β V = β L 0 0 J θ 0 β θ Em termos dos torques nas rodas (F V = ( W T V ) T.τ): ( W T V ) T.τ = J V.V' + β V.V Multiplicando a dinâmica de atuadores por ( W T V ) T : ( W T V ) T.τ = -[( W T V ) T.J W. W T V ].V' [( W T V ) T.(ρ.K W 2 + β W ). W T V ].V + ( W T V ) T.ρ.K W.E Igualando as duas últimas equações e isolando o termo em E: ( W T V ) T.ρ.K W.E = [J V + ( W T V ) T.J W. W T V ].V' + + [β V + ( W T V ) T.(ρ.K W 2 + β W ). W T V ].V Fazendo: M V = [J V + ( W T V ) T.J W. W T V ] = matriz de inércia, simétrica e definida positiva. B V = [β V + ( W T V ) T.(ρ.K W 2 + β W ). W T V ] = matriz de coeficientes de atritos viscosos, simétrica e definida positiva. Fazendo: τ V = ( W T V ) T.ρ.K W.E E = [( W T V ) T.ρ.K W ] -1.τ V Modelo Dinâmico em V: τ V = M V.V' + B V.V 10

11 Substituindo V = ( q T V ) T.q' e V' = ( q T V ) T.q'': τ V = M V. ( q T V ) T.q'' + B V. ( q T V ) T.q' Multiplicando os dois lados por q T V : q T V.τ V = [ q T V.M V.( q T V ) T ].q'' + [ q T V.B V.( q T V ) T ].q' Chamando: M q = [ q T V.M V.( q T V ) T ] = Matriz de inércia em espaço de configuração, simétrica e definida positiva. B q = [ q T V.B V.( q T V ) T ] = Matriz de coeficientes de atritos viscosos em espaço de configuração, simétrica e definida positiva. τ q = q T V.τ V τ V = ( q T V ) T.τ q Modelo Dinâmico em q: τ q = M q.q'' + B q.q' Propriedade da Matriz de Inércia: M q ' = q T V.M V.(( q T V ) T )' + ( q T V )'.M V.( q T V ) T Dado um vetor e, tomando a forma quadrática [e T.M q '.e]/2: [e T.M q '.e]/2 = e T.[ q T V.M V.(( q T V ) T )' + ( q T V )'.M V.( q T V ) T ].e/2 = = e T.[ q T V.M V.(( q T V ) T )'].e/2 + e T.[( q T V )'.M V.( q T V ) T ].e/2 = = e T.[ q T V.M V.(( q T V ) T )'].e 11

12 Forma Linear em Parâmetros: Modelo Dinâmico em V Linear em Parâmetros: τ V = M V.V' + B V.V = M V11 M V12. v' + B V11 B V12. v M V12 M V22 ω' B V12 B V22 ω = M V11.v'+M V12.ω' + B V11.v+B V12.ω M V12.v'+M V22.ω' B V12.v+B V22.ω = v' ω' 0. M V11 + v ω 0. B V11 0 v' ω' M V12 0 v ω B V12 M V22 B V22 = Φ Vm.m + Φ Vb.b = Φ V.P onde, Φ Vm = v' ω' 0 Φ Vb = v ω 0 0 v' ω' 0 v ω m = M V11 b = B V11 M V12 M V22 Φ V = [Φ Vm Φ Vb ] B V12 B V22 P = m b τ V = Φ V.P 12

13 Modelo Dinâmico em q Linear em Parâmetros: Multiplicando o modelo em V pela matriz q T V : q T V.τ V = q T V.Φ V.P τ q = Φ q.p onde: τ q = q T V.τ V Φ q = q T V.Φ V Φ q = [ q T V.Φ Vm q T V.Φ Vb ] = [Φ qm Φ qb ] onde: Φ qm = q T V.Φ Vm Φ qb = q T V.Φ Vb As matrizes Φ Vm e Φ Vb podem ser expressas como: Φ Vm = [(I 11.V') (I 12.V') Φ Vb = [(I 11.V) (I 12.V) (I 22.V')] (I 22.V)] onde: I 11 = 1 0 I 12 = 0 1 I 22 = Assim, lembrando que V = ( q T V ) T.q' e V' = ( q T V ) T.q'': Φ qm = [[ q T V I 11 ( q T V ) T q''] [ q T V I 12 ( q T V ) T q''] [ q T V I 22 ( q T V ) T q'']] Φ qb = [[ q T V I 11 ( q T V ) T q'] [ q T V I 12 ( q T V ) T q'] [ q T V I 22 ( q T V ) T q']] 13

14 Modelo Dinâmico Completo: Lembrando que, do modelo dinâmico em V, temos: τ V = M V.V' + B V.V e τ V = ( W T V ) T.ρ.K W.E V' = (M V ) -1.[-B V.V + ( W T V ) T.ρ.K W.E] Lembrando que o modelo cinemático é dado por: q' = [ q T V ].V Agrupando as duas equações acima obtemos o modelo dinâmico completo, incluindo dinâmica de atuadores, para o robô móvel: Equação de Estado: V' = [-M V -1.B V ] 0. V + [M V -1. W T V T.ρ.K W ]. E q' [ q T V ] 0 q 0 Equação de Saída: q = 0 I. V q Características do Modelo: Modelo de quinta ordem. Não linear (cos(θ) e sen(θ) na matriz q T V ). Sistema MIMO (Vetor de entradas E / Vetor de saídas, q) Sistema subatuado (duas entradas, E para três saídas, q). 14