ĐỘNG LỰC HỌC ROBOT Phương trình Newton-Euler / phương pháp không đệ quy (non – recursive approach) Nguyễn Quang Hoàng Email: hoang.nguyenquang@hust.edu.vn Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler -2- Phương trình Newton-Euler: phương pháp không đệ quy non recursive approach Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler -3- Xét tay máy dạng chuỗi n khâu động, n dof (với hai loại khớp T và R) • Phương pháp D-H sử dụng để phân tích động học • Xác định gia tốc khối tâm các khâu (jacobi tịnh tiến) • Xác định vận tốc góc, gia tốc góc các khâu (jacobi quay) • Tách riêng các khâu, • Áp dụng phương trình Newton-Euler cho từng khâu • Khử các lực liên kết lý tưởng (tổng công ảo của các lực liên kết lý tưởng bằng không). • PT vi phân chuyển động ở dạng tọa độ suy rộng đủ. Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -4- ma = å F l = å m (F ) C k C C k Chiếu trong hệ cơ sở R0 ma(0) = å f(0) C k I(0)w (0) + w(0)I(0)w(0) = å m(0)(F ) C C C k Chiếu trong hệ gắn liền vật (B) hoặc Ri ma( )i = å f( )i C k I( )i w ( )i + w( )i I( )i w( )i = å m( )i (F ) C C C k Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -5- Tính vận tốc, gia tốc khối tâm, vận tốc góc va gia tốc góc Véc tơ tọa độ suy rộng q = [q ,q , ...,q ]T 1 2 f Vị trí khối tâm, ma trận cosin chỉ hướng của các khâu : r (q), R (q), k = 1, 2, ...,n Ck k Vận tốc khối tâm vật k (0) d v = r (q) = J q Ck dt Ck Tk ma trận Jacobi tịnh tiến: é¶x ¶x ¶x ù ê Ck Ck Ck ú ê ¶q ¶q ¶q ú ê 1 2 f ú ¶ ê¶y ¶y ¶y ú J = r (q) = ê Ck Ck Ck ú Tk ¶q Ck ê ¶q ¶q ¶q ú ê 1 2 f ú ê¶z ¶z ¶z ú ê Ck Ck Ck ú ê ¶q ¶q ¶q ú êë 1 2 f úû Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -6- Đưa vào ký hiệu véc tơ quay một cách hình thức j = j (q) k k Vận tốc góc vật k (0) d w = j (q) = J q k dt k Rk ma trận Jacobi quay é¶w ¶w ¶w ù ê kx kx kx ú ê ¶q ¶q ¶q ú ê 1 2 f ú ¶ ê¶w ¶w ¶w ú J = w(0) = ê ky ky ky ú Rk ¶q k ê ¶q ¶q ¶q ú ê ú ê 1 2 f ú ê¶w ¶w ¶w ú ê kz kz kz ú êë ¶q ¶q ¶q úû 1 2 f Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -7- Cách tính ma trận Jacobi quay d R (q) = R (q) k dt k w(0) = R R T w(0) = S-1(w(0)) k k k k k w(0) = J q, k Rk é¶w ¶w ¶w ù ê kx kx kx ú ê ¶q ¶q ¶q ú ê 1 2 f ú ¶w(0) ê¶w ¶w ¶w ú J = k = ê ky ky ky ú Rk ¶q ê ¶q ¶q ¶q ú ê ú ê 1 2 f ú ê¶w ¶w ¶w ú ê kz kz kz ú êë ¶q ¶q ¶q úû 1 2 f Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -8- Gia tốc khối tâm và gia tốc góc vật k nhận được bằng cách đạo hàm theo thời gian các véc tơ vận tốc và vận tốc góc (0) d v = r (q) = J q d Ck dt Ck Tk w(0) = j (q) = J q, k dt k Rk d2 a(0) = r (q) = J q + J q Ck dt2 Ck Tk Tk a(0) º w (0) = J q + J q k k Rk Rk a(0) = J q + J q Ck Tk Tk a(0) = J q + J q k Rk Rk Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -9- Phương trình Newton-Euler: không đệ quy m a(0) = f = fa + fc, k = 1, 2,...,n k Ck k k k I(0)a(0) + w(0)I(0)w(0) = m = ma + mc Ck k k Ck k k k k m (J q + J q)= fa + fc k Tk Tk k k ( ) C I(0) J q + J q + w(0)I(0)w(0) = ma + mc Ck Rk Rk k Ck k k k f a , f c ,ma ,mc m J q + m J q = fa + fc k k k k k Tk k Tk k k I(0)J q + I(0)J q + w(0)I(0)w(0) = ma + mc Ck Rk Ck Rk k Ck k k k ma trận của ten xơ quán tính của vật k đối với I(0) = R I(k)RT khối tâm C chiếu trong hệ cố định R Ck k Ck k k 0 Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -10- m J q +m J q = fa + fc k Tk k Tk k k I(0)J q + I(0)J q + w(0)I(0)w(0) = ma + mc é m J q ù Ck Rk Ck Rk k Ck k k k ê 1 T 1 ú ê ú Viết cho các khâu, sau đó ghép lại được ê ú ê m J q ú M q q( ) + k q q( , ) = Qa + Qc [ ( ,k q q)] = êê n Tn ú, 6n´1 I(0)J q + w(0)I(0)w(0) ú ê C 1 R1 1 C 1 1 ú ê ú ê ú é ù é ù é ù ê ú m J fa fc êI(0)J q + w(0)I(0)w(0) ú ê 1 T 1 ú ê 1 ú ê 1 ú ë Cn Rn n Cn n û ê ú ê ú ê ú ê ú ê ú ê ú êm J ú ê fa ú ê fc ú [M q( )] = ê n Tn ú, Qa = ê n ú, Qc = ê n ú 6n´f êI(0)J ú êma ú êmc ú ê C 1 R1 ú ê ú ê ú ê ú ê 1 ú ê 1 ú ê ú ê ú ê ú ê ú ê ú ê ú êI(0)J ú êma ú êmc ú ë Cn Rn û ë n û ë n û Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -11- Khử các lực liên kết lý tưởng đưa phương trình về dạng tối giản Với giả thiết các liên kết trong hệ là lý tưởng n n å(f c ⋅ dr + mc ⋅ dj ) = 0 å(fc )T ⋅ dr + (mc )T ⋅ dj = 0 k Ck k k k Ck k k k=1 k=1 ¶r ¶j dr = Ck dq = J dq, dj = k dq = J dq, Ck ¶q Tk k ¶q Rk n n å(fc )T ⋅ dr + (mc )T ⋅ dj = å[(fc )T ⋅ J dq + (mc )T ⋅ J dq] = 0 k Ck k k k Tk k Rk k=1 k=1 n é ù dqT å[(J )T fc + (J )T mc ] = dqT êëJT .. JT JT .. JT ûú Qc = 0 Tk k Rk k T 1 Tn R1 Rn k=1 Do các qi độc lập, nên suy ra: é ù JT = éJT .. JT JT .. JT ù JT .. JT JT .. JT Qc = 0 êë úû êë T 1 Tn R1 Rn úû T 1 Tn R1 Rn f´6n Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -12- Khử các lực liên kết lý tưởng đưa phương trình về dạng tối giản M q q( ) + k q q( , ) = Qa + Qc JT = éJT .. JT JT .. JT ù êë T 1 Tn R1 Rn úû f´6n J M q qT ( ) + J k q qT ( , ) = J QT a + J QT c M q q( ) + k q q( , ) = Q M q( ) = J M qT ( ), k q q( , ) = J k q qT ( , ), Q = J QT a Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -13- Xác định các lực liên kết (cách khác) Qc = -H q( )l M q q( ) + H q( )l = Qa - k q q( , ) (1) JT (q H q) ( ) = 0 Đưa vào ma trận M = diag{m E ,m E ,,m E ,I(0),I(0),,I(0)} 1 3 2 3 n 3 C 1 C 2 Cn M-1 = diag{m-1E ,m-1E ,,m-1E ,[I(0) ]-1,[I(0) ]-1,,[I(0) ]-1} 1 3 2 3 n 3 C 1 C 2 Cn Viết được M q( ) = M J q( ) Nhận thấy rằng HT (q M) -1M q( ) = HT (q M) -1MJ q( ) = HT (q J q) ( ) = 0 Do đó, từ (1) suy ra được HT (q M) -1H q( )l = HT (q M) -1Qa - HT (q M) -1k q q( , ) l = Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -14- Xác định các lực liên kết ta viết véc tơ lực/mô men do các lực liên kết dạng: Qc = -H q( )l l là véc tơ chứa các lực liên kết cần tìm M q q( ) + k q q( , ) = Qa - H q( )l Viết lại phương trình trên dạng éqù é ù éM q( ) H q( )ù ê ú = Qa - k q q( , ) det êM q( ) H q( )ú ¹ 0 ëê û ú êlú ë û êë úû éqù ( ) ê ú é ù-1 ê ú = êM q( ) H q( )ú Qa - k q q( , ) êlú ë û ë û Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -15- Phương trình PVCĐ với các ma trận sau: M q q( ) +C q q q( , ) = Q Q = Bu -g q( ) M q q( ) +C q q q( , ) + g q( ) = Bu M q( ) = S(JT m J + JT R I( )k R JT ) Tk k Tk Rk k Ck k Rk C q q( , ) = S (JT m J + JT R I( )k R JT + JT R I( )k R JT ) k Tk k Tk Rk k Ck k Rk Rk k k Ck k Rk g q( ) = -Sm JT ( )q g(0) k Tk Đối với tay máy phẳng: I = 0 S(JT R I( )k R JT q) = 0üï k C k Rk k k Ck k Rk ïï k ý C q q( , ) = S(JT m J ) J = const J = 0 ïï Tk k Tk Rk Rk ïþ Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -16- Chứng minh được (M - 2C) = -(M - 2C)T M = SJT m J , M = SJT R I(k)RT J , C = SJT m J , t Tk k Tk r Rk k Ck k Rk t Tk k Tk C = S(JT R I(k)RT J + JT (0)R I(k )RT J ), (0) = R R T r Rk k Ck k Rk Rk k k Ck k Rk k k k = S(JT R I(k)RT J + JT R R T R I(k)RT J ). Rk k Ck k Rk Rk k k k Ck k Rk M = S(JT m J + JT m J ) t Tk k Tk Tk k Tk M = S(JT R I(k)RT J + JT R I (k)RT J + JT R I(k)R T J + JT R I(k )RT J ). r Rk k Ck k Rk Rk k Ck k Rk Rk k Ck k Rk Rk k Ck k Rk N = M - 2C = S(JT m J - JT m J ) = -NT t t t Tk k Tk Tk k Tk t N = M - 2C r r r = S(JT R I(k)RT J - JT R I(k)RT J ) + S(JT R I(k)R T J - JT R I (k )RT J ) Rk k Ck k Rk Rk k Ck k Rk Rk k Ck k Rk Rk k Ck k Rk = -NT . r N M 2C (M 2C ) (M 2C ) NT . t t r r Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -17- Các bước áp dụng phương trình Newton-Euler cho tay máy robot chuỗi 1. Động học: các Jacobi và đạo hàm của chúng, vận tốc góc 2. Tách các vật – đặt các lực hoạt động và các lực liên kết 3. Viết PT Newton-Euler cho từng vật 4. Ghép được phương trình M q q( ) + k q q( , ) = Qa + Qc 5. Khử các lực liên kết, thu được PTVP CĐ dạng tối giản M q q( ) +C q q q( , ) + g q( ) = Bu 6. Xác định các lực liên kết nếu cần. Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -18- Ví dụ: Lập phương trình vi phân chuyển động cho tay may quay 1 dof. y Cho biết: O m, a = OC, u a1=OC 1 u é ù y1 êI 0 0 ú ê 1x ú C I(1) = 0 I 0 C ê 1y ú ê 0 0 I ú x êë 1z úû x mg 1 Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -19- Ví dụ: Vị trí khối tâm và ma trận quay của vật đối với hệ cố định éa cosq ù écosq -sinq 0ù ê 1 1 ú ê 1 1 ú r = êa sinq ú, R = 0 R = êsinq cosq 0ú y C ê 1 1 ú 1 1 ê 1 1 ú 1 ê 0 ú ê 0 0 1ú O êë úû êë úû a =OC u 1 é 0 -q 0ù é 0 ù y1 ê 1 ú ê ú w(0) = R R T = êq 0 0ú w(0) = ê 0 ú C 1 1 1 ê 1 ú 1 ê ú ê 0 0 0ú êq ú êë úû êë 1 úû x mg x1 Các ma trận Jacobi tịnh tiến và Jacobi quay é-a sinq ù é ù0 ¶r ê 1 1 ú ¶w(0) ê ú J = C1 = ê a cosq ú, J = 1 = ê ú0 T 1 ¶q ê 1 1 ú R1 ¶q ê ú ê 0 ú ê ú1 êë úû ê úë û Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -20- phương trình Newton-Euler m J q +m J q = fa + fc Y y O k Tk k Tk k k O (0) (0) (0) (0) (0) a c X a1=OC I J q + I J q + w I w = m + m O Ck Rk Ck Rk k Ck k k k u C Các lực và mô men tác dụng lên các khâu được đưa ra x mg ém gù éX ù ê 1 ú ê O ú fa = ê 0 ú, fc = êY ú 1 ê ú 1 ê O ú ê 0 ú ê 0 ú êë úû êë úû é 0 ù é 0 ù ê ú ê ú ma = ê 0 ,ú mc = ê 0 ú 1 ê ú 1 ê ú êu ú êa sinq X -a cosq Y ú êë 1 úû êë 1 1 O 1 1 O úû Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -21- Giả sử ma trận của ten xơ quán tính khối của các vật đối y với hệ gắn vào vật có gốc tại khối tâm như sau: O éI 0 0 ù a1=OC ê 1x ú I(0) = RI(1)RT , u I(1) = ê 0 I 0 ú, C C y C 1 ê 1y ú 1 ê 0 0 I ú I(1) = RT I(0)R C êë 1z úû C C x Từ đây tính được ma trận quán tính khối đối với khối mg x1 tâm và chiếu trong hệ cố định R0 : é* * 0 ù ê ú I(0) = R I(1)RT = ê* * 0 ú C 1 1 C 1 1 ê ú ê0 0 I ú êë 1z úû Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -22- Đạo hàm theo thời gian các ma trận Jacobi y é-a q cosq ù é ù0 O ê 1 1 1 ú ê ú a =OC J = ê-a q sinq ú, J = ê ú0 u 1 T 1 ê 1 1 1 ú R1 ê ú ê 0 ú ê ú0 êë úû ê úë û C Các lực và mô men tác dụng lên các khâu được đưa ra x mg ém gù éX ù ê 1 ú ê O ú fa = ê 0 ú, fc = êY ú 1 ê ú 1 ê O ú ê 0 ú ê 0 ú êë úû êë úû é 0 ù é 0 ù ê ú ê ú ma = ê 0 ,ú mc = ê 0 ú 1 ê ú 1 ê ú êu ú êa sinq X -a cosq Y ú êë 1 úû êë 1 1 O 1 1 O úû Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -23- Phương trình Newton-Euler é-m a sinq ù é-m a q cosq ù ém gù é X ù ê 1 1 1 ú ê 1 1 1 1 ú ê 1 ú ê O ú ê m a cosq ú ê-m a q sinq ú ê 0 ú ê Y ú ê 1 1 1 ú ê 1 1 1 1 ú ê ú ê O ú ê 0 ú ê 0 ú ê 0 ú ê 0 ú ê ú q + ê ú q = ê ú + ê ú ê 0 ú ê 0 ú ê 0 ú ê 0 ú ê ú ê ú ê ú ê ú ê 0 ú ê 0 ú ê 0 ú ê 0 ú ê ú ê ú ê ú ê ú ê I ú ê 0 ú ê u ú êa sinq X -a cosq Y ú êë 1z úû êë úû êë 1 úû êë 1 1 O 1 1 O úû Bỏ các dòng 0 é-m a sinq ù é-m a q cosq ù ém gù é X ù ê 1 1 1 ú ê 1 1 1 1 ú ê 1 ú ê O ú ê m a cosq úq + -ê m a q sinq úq = ê 0 ú + ê Y ú ê 1 1 1 ú 1 ê 1 1 1 1 ú 1 ê ú ê O ú ê I ú ê 0 ú ê u ú êX a sinq -Y a cosq ú êë 1z úû êë úû êë 1 úû êë O 1 1 O 1 1 úû M q q( ) + k q q( , ) = Qa + Qc Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -24- é-m a sinq ù é-m a q cosq ù ém gù é X ù ê 1 1 1 ú ê 1 1 1 1 ú ê 1 ú ê O ú ê m a cosq úq + -ê m a q sinq úq = ê 0 ú + ê Y ú ê 1 1 1 ú 1 ê 1 1 1 1 ú 1 ê ú ê O ú ê I ú ê 0 ú ê u ú êX a sinq -Y a cosq ú êë 1z úû êë úû êë 1 úû êë O 1 1 O 1 1 úû M q q( ) + k q q( , ) = Qa + Qc Ma trận khử các lực liên kết (bỏ các dòng 0 tương ứng) éé ùT ù é ù êê-a sinq ú é ùT ú JT = JT JT JT = ê 1 1 , 1 ú = [-a sinq a cosq 1 ] êë T 1 R1 úû êê a cosq ú ê úë û ú 1 1 1 1 1´3 ëê ûú êë 1 1 úû Phương trình tối giản JT M q q( ) + J k q qT ( , ) = J QT a + J QT c M q q( ) + k q q( , ) = Q (I + m a2 )q + m a g sinq = u 1z 1 1 1 1 1 1 1 Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -25- JT = [-a sinq a cosq 1 ] 1 1 1 1 é-m a sinq ù ê 1 1 1 ú M q( ) = J M qT ( ) = [-a sinq a cosq 1 ]ê m a cosq ú = 1 1 1 1 ê 1 1 1 ú ê I ú êë 1z úû é-m a q cosq ù ê 1 1 1 1 ú k q q( , ) = JT k q q( , ) = [-a sinq a cosq 1 ]ê-m a q sinq úq = 1 1 1 1 ê 1 1 1 1 ú 1 ê 0 ú êë úû ém gù ê 1 ú Q = J QT a = [-a sinq a cosq 1 ]ê 0 ú = 1 1 1 1 ê ú ê u ú êë 1 úû é X ù ê O ú J QT c = [-a sinq a cosq 1 ]ê Y ú = ? 1 1 1 1 ê O ú êX a sinq -Y a cosq ú êë O 1 1 O 1 1 úû Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -26- Xác định cả lực liên kết: XO, YO é-m a sinq ù é-m a q cosq ù ém gù é X ù ê 1 1 1 ú ê 1 1 1 1 ú ê 1 ú ê O ú ê m a cosq úq + -ê m a q sinq úq = ê 0 ú + ê Y ú ê 1 1 1 ú 1 ê 1 1 1 1 ú 1 ê ú ê O ú ê I ú ê 0 ú ê u ú êX a sinq -Y a cosq ú êë 1z úû êë úû êë 1 úû êë O 1 1 O 1 1 úû é-m a sinq ù é-m a q cosq ù ém gù é 1 0 ù ê 1 1 1 ú ê 1 1 1 1 ú ê 1 ú ê ú éX ù ê m a cosq úq + -ê m a q sinq úq = ê 0 ú + ê 0 1 ú ê O ú ê 1 1 1 ú 1 ê 1 1 1 1 ú 1 ê ú ê ú êY ú ê I ú ê 0 ú ê u ú êa sinq -a cosq ú êë O úû êë 1z úû êë úû êë 1 úû êë 1 1 1 1 úû é ùT M q q( ) + k q q( , ) = Qa - H q( )l, l = êX Y ú ë O O û Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -27- Xác định cả lực liên kết: XO, YO é ùT M q q( ) + k q q( , ) = Qa - H q( )l, l = êX Y ú ë O O û éqù éM q( ) H q( )ù ê ú = Qa - k q q( , ) êë ú êû lú êë úû éqù ( ) ê ú é ù-1 ê ú = êM q( ) H q( )ú Qa - k q q( , ) êlú ë û ë û é-m a sinq -1 0 ù é q ù é-m a q cosq ù ém gù ê 1 1 1 ú ê 1 ú ê 1 1 1 1 ú ê 1 ú ê m a cosq 0 -1 ú êX ú + -ê m a q sinq úq = ê 0 ú ê 1 1 1 ú ê O ú ê 1 1 1 1 ú 1 ê ú ê I -a sinq a cosq ú êY ú ê 0 ú ê u ú êë 1z 1 1 1 1 ú êû ë O úû êë úû êë 1 úû Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -28- Ví dụ 2: n = 2, f = 2 B m , I éq ù éx ù y0 g 2 C2 q = ê 1 ú = ê ú x d a2 C2 êq ú êjú A êë 2 úû êë úû u1 C x0 O 1 m1 u2 é ù é1 0ù êq ú ¶r ê ú ê 1 ú J = C = ê0 0 ,ú r = 0 , 1 ê ú ê ú T 1 ¶q ê ú C ê ú 0 0 1 ê ú êë 0 úû ë û é ù é1 -a sinq ù êq + d + a cosq ú ¶r ê 2 2 ú ê 1 2 2 ú J = C = ê0 a cosq ú, r = a sinq 2 ê ú ê ú T 2 ¶q ê 2 2 ú C ê 2 2 ú 0 0 2 ê ú êë 0 úû ë û Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -29- é1 0 0ù ê ú B R = R0 = ê0 1 0ú 1 1 ê ú ê0 0 1ú y g m2, IC2 êë úû 0 x d a2 C écosq -sinq 0ù 2 ê ú A ê 2 2 ú u1 x R = R0 = sinq cosq 0 O C 0 2 2 ê 2 2 ú 1 ê 0 0 1ú m u2 êë úû 1 é0 0 0ù é ù0 é ù ê ú ê ú ê0 0ú ê ú ê ú ¶w(0) w(0) = R RT = ê0 0 0ú w(0) = ê ú0 J = 1 = ê0 0ú 1 1 1 ê ú 1 ê ú R1 ¶q ê ú ê0 0 0ú ê ú0 ê0 0ú ë û ë û êë úû é 0 -q 0ù é 0 ù é ù ê 2 ú ê ú ê0 0ú ê ú ê ú ¶w(0) w(0) = R RT = êq 0 0ú w(0) = ê 0 ú J = 2 = ê0 0ú 2 2 2 ê 2 ú 2 ê ú R2 ¶q ê ú ê 0 0 0ú êq ú ê0 1ú ë û ë 2 û êë úû Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -30- é1 0ù é0 0ù ê ú ê ú J = ê0 0ú J = ê0 0 ,ú B T 1 ê ú T 1 ê ú ê0 0ú ê0 0ú y g m2, IC2 êë úû êë úû 0 x d a2 C é0 0ù é0 0ù 2 ê ú ê ú A J = ê0 0ú J = ê0 0ú u1 C x0 R1 ê ú R1 ê ú O 1 ê0 0ú ê0 0ú m êë úû êë úû 1 u2 é1 -a sinq ù é0 -a q cosq ù ê 2 2 ú ê 2 2 2 ú J = ê0 a cosq ú J = ê0 -a q sinq ú, T 2 ê 2 2 ú T 2 ê 2 2 2 ú ê0 0 ú ê0 0 ú êë úû êë úû é0 0ù é0 0ù ê ú ê ú J = ê0 0ú J = ê0 0ú R2 ê ú R2 ê ú ê0 1ú ê0 0ú êë úû êë úû Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -31- Lực trên các khâu B y g m2, IC2 0 N x d Y a2 C A 2 XA A A u1 C P2 x O 1 0 n XA P 1 u u 1 2 2 YA é u ù é -X ù é 0 ù é 0 ù ê 1 ú ê A ú ê ú ê ú fa = -ê m gú, fc = êN -Y ú, ma = ê 0 ú, mc = ê 0 ú 1 ê 1 ú 1 ê A ú 1 ê ú 1 ê ú ê 0 ú ê 0 ú ê-u ú ên -dY ú êë úû êë úû êë 2 úû êë 1 A úû é 0 ù éX ù é 0 ù é 0 ù ê ú ê A ú ê ú ê ú fa = -ê m gú, fc = êY ú, ma = ê 0 ú, mc = ê 0 ú 2 ê 2 ú 2 ê A ú 2 ê ú 2 ê ú ê 0 ú ê 0 ú êu ú êa sinq X -a cosq Y ú êë úû êë úû êë 2 úû êë 2 2 A 2 2 A úû Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -32- ma trận quán tính khối đối đối với khối tâm chiếu trong hệ gắn liền vật éI 0 0 ù éI 0 0 ù ê 1x ú ê 2x ú I(1) = ê 0 I 0 ú, I(2) = ê 0 I 0 ú C 1 ê 1y ú C 2 ê 2y ú ê 0 0 I ú ê 0 0 I ú êë 1z úû êë 2z úû Từ đây tính được ma trận quán tính khối đối đối với khối tâm chiếu trong hệ cố định R0 éI 0 0 ù é* * 0 ù ê 1x ú ê ú I(0) = R I(1)RT = ê 0 I 0 ú, I(0) = R I(2)RT = ê* * 0 ú C 1 1 C 1 1 ê 1y ú C 2 2 C 2 2 ê ú ê 0 0 I ú ê0 0 I ú êë 1z úû êë 2z úû Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -33- Các phương trình Newton-Euler m J q +m J q = fa + fc k Tk k Tk k k I(0)J q + I(0)J q + w(0)I(0)w(0) = ma + mc Ck Rk Ck Rk k Ck k k k Bỏ đi các dòng 0, phương trình trên trở thành ém 0 ù é 0 ù é u ù é -X ù ê 1 ú ê ú ê 1 ú ê A ú ê 0 0 ú ê 0 ú ê-m gú ê N -Y ú ê ú ê ú ê 1 ú ê A ú ê 0 0 ú éq ù ê 0 ú ê -u ú ê n -dY ú ê ú ê 1 ú + ê ú = ê 2 ú + ê 1 A ú êm -m a sinq ú êq ú ê-m a q2 cosq ú ê 0 ú ê X ú ê 2 2 2 2 ú êë 2 úû ê 2 2 2 2 ú ê ú ê A ú ê 0 m a cosq ú ê-m a q2 sinq ú ê-m gú ê Y ú ê 2 2 2 ú ê ú ê 2 ú ê A ú ê ú ê 2 2 2 2 ú ê ú ê ú êë 0 I úû ê 0 ú êë u úû êëa sinq X -a cosq Y úû 2z ë û 2 2 2 A 2 2 A Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -34- Ma trận khử các lực liên kết tương ứng với phương trình trên JT = éJT JT JT JT ù êë T 1 R1 T 2 R2 úû 2´6 éé ùT é ùT ù êê1 0ú é ùT ê1 -a sinq ú é ùT ú JT = ê , 0 0 , 2 2 , 0 1 ú êê0 0ú êë úû ê0 a cosq ú êë úû ú êëêë úû êë 2 2 úû úû é1 0 0 1 0 0ù = ê ú ê0 0 0 -a sinq a cosq 1ú êë 2 2 2 2 úû ém 0 ù é 0 ù é u ù é -X ù ê 1 ú ê ú ê 1 ú ê A ú ê 0 0 ú ê 0 ú ê-m gú ê N -Y ú ê ú ê ú ê 1 ú ê A ú ê 0 0 ú éq ù ê 0 ú ê -u ú ê n -dY ú ê ú ê 1 ú + ê ú = ê 2 ú + ê 1 A ú êm -m a sinq ú êq ú ê-m a q2 cosq ú ê 0 ú ê X ú ê 2 2 2 2 ú êë 2 úû ê 2 2 2 2 ú ê ú ê A ú ê 0 m a cosq ú ê-m a q2 sinq ú ê-m gú ê Y ú ê 2 2 2 ú ê ú ê 2 ú ê A ú ê ú ê 2 2 2 2 ú ê ú ê ú êë 0 I úû ê 0 ú êë u úû êëa sinq X -a cosq Y úû 2z ë û 2 2 2 A 2 2 A Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -35- Phương trình tối giản B y g m2, IC2 0 x d a2 C2 M q q( ) +C q q q( , ) + g q( ) = Bu A x u1 C 0 O 1 m1 u2 M q( ) = S(JT m J + JT R I( )k R JT ) Tk k Tk Rk k Ck k Rk C q q q( , ) = S(JT m J + JT R I( )k R JT + JT R I( )k R JT )q Tk k Tk Rk k Ck k Rk Rk k k Ck k Rk g q( ) = -Sm JT ( )q g(0) k Tk é m + m -m a sinq ù éq ù é-m a q2 cosq ù é 0 ù éu ù ê 1 2 2 2 2 ú ê 1 ú + ê 2 2 2 2 ú + ê ú = ê 1 ú ê-m a sinq I + m a2 ú êq ú ê 0 ú êm ga cosq ú êu ú êë 2 2 2 2z 2 2 úû êë 2 úû êë úû êë 2 2 2 úû êë 2 úû Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -36- Ma trận H(q) é -X ù é0 0 -1 0 ù ê A ú ê ú ê N -Y ú ê1 0 0 -1 ú é N ù ê A ú ê ú ê ú ê n -dY ú ê0 1 0 -d ú ê n ú Qc = ê 1 A ú = ê ú ê 1 ú = -H q( )l ê X ú ê0 0 1 0 ú êX ú ê A ú ê ú ê A ú ê Y ú ê0 0 0 1 ú êY ú ê A ú ê ú êë A úû êa sinq X -a cosq Y ú ê0 0 a sinq -a cosq ú êë 2 2 A 2 2 A úû êë 2 2 2 2 úû é N ù ê ú ê n ú l = ê 1 ú êX ú ê A ú êY ú êë A úû Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -37- ém 0 ù é 0 ù é u ù é0 0 -1 0 ù ê 1 ú ê ú ê 1 ú ê ú ê 0 0 ú ê 0 ú ê-m gú ê1 0 0 -1 ú é N ù ê ú ê ú ê 1 ú ê ú ê ú ê 0 0 ú éq ù ê 0 ú ê -u ú ê0 1 0 -d ú ê n ú ê ú ê 1 ú + ê ú = ê 2 ú + ê ú ê 1 ú êm -m a sinq ú êq ú ê-m a q2 cosq ú ê 0 ú ê0 0 1 0 ú êX ú ê 2 2 2 2 ú êë 2 úû ê 2 2 2 2 ú ê ú ê ú ê A ú ê 0 m a cosq ú ê-m a q2 sinq ú ê-m gú ê0 0 0 1 ú êY ú ê 2 2 2 ú ê ú ê 2 ú ê ú êë A úû ê ú ê 2 2 2 2 ú ê ú ê ú êë 0 I úû ê 0 ú êë u úû êë0 0 a sinq -a cosqúû 2z ë û 2 2 2 2 M q q( ) + k q q( , ) = Qa - H q( )l éqù éM q( ) H q( )ù ê ú = Qa - k q q( , ) êë ú êû lú êë úû éqù ( ) ê ú é ù-1 ê ú = êM q( ) H q( )ú Qa - k q q( , ) êlú ë û ë û Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -38- x Ví dụ 3: y 2 2 éa cosq ù g ê 1 1 ú O ê ú 2 r = êa sinq ú, l C1 ê 1 1 ú y 2 C 0 y 1 2 êë úû 0 q 2 O x 1 écosq -sinq 0ù l 1 ê 1 1 ú C 1 R = 0 R = êsinq cosq 0ú 1 1 1 1 ê 1 1 ú q 2 ê ú 1 ê 0 0 1ú O ë û 0 x 0 él cosq + a cos(q + q )ù ê 1 1 2 1 2 ú r = êl sinq + a sin(q + q ) ,ú C ê 1 1 2 1 2 ú a = O C , 2 ê 0 ú êë úû 1 0 1 écos(q + q ) -sin(q + q ) 0ù a = O C ê ú 2 1 2 ê 1 2 1 2 ú R = 0 R 1R = sin(q + q ) cos(q + q ) 0 2 1 2 ê 1 2 1 2 ú ê 0 0 1ú êë úû Nguyen Quang Hoang - Department of Applied Mechanics
Phương trình Newton-Euler: không đệ quy -39- x y 2 éa cosq ù 2 ê 1 1 ú g r = êa sinq ú, C ê 1 1 ú O 1 ê ú 2 êë 0 úû y l C y 1 2 2 0 q é ù 2 ê-a sinq 0ú O x ¶r ê 1 1 ú 1 1 J = C = a cosq 0 , l 1 ê ú T 1 ¶q 1 1 C 1 ê 0 0ú 1 1 êë úû q 2 1 O 0 é ù x -a q cosq 0 0 ê 1 1 1 ú J = ê-a q sinq 0 ,ú T 1 ê 1 1 1 ú ê 0 0ú êë úû é0ù é0 0ù é ù ê ú ¶w(0) ê ú ê0 0ú w(0) = R R T = ... w(0) = ê0ú J = 1 = ê0 0ú ê ú 1 1 1 1 ê ú R1 ¶q ê ú J = ê0 0ú ê ú ê ú R1 ê ú êq ú êë1 0úû ê0 0ú ë 1û ë û Nguyen Quang Hoang - Department of Applied Mechanics Phương trình Newton-Euler: không đệ quy -40- é ù x l cosq +a cos(q +q ) y 2 ê 1 1 2 1 2 ú 2 r = êl sinq +a sin(q +q ) ,ú g C ê 1 1 2 1 2 ú O 2 ê 0 ú 2 êë úû l C y y 2 2 1 0 q ¶r 2 J = C2 O x 1 T 2 ¶q 1 l C 1 é-l sinq -a sin(q + q ) -a sin(q + q )ù 1 1 ê 1 1 2 1 2 2 1 2 ú q 2 ê ú 1 = ê l cosq + a cos(q + q ) a cos(q + q ) ú, O 1 1 2 1 2 2 1 2 0 ê 0 0 ú x ê ú 0 ë û é-l q cosq -a cos(q + q )(q + q ) -a cos(q + q )(q + q )ù ê 1 1 1 2 1 2 1 2 2 1 2 1 2 ú J = ê-l q sinq -a sin(q + q )(q + q ) -a sin(q + q )(q + q )ú T 2 ê 1 1 1 2 1 2 1 2 2 1 2 1 2 ú ê 0 0 ú êë úû é 0 ù é0 0ù é0 0ù ê ú ¶w(0) ê ú ê ú (0) T (0) ê ú J = 2 = ê0 0ú J = ê0 0ú w = R R = ... w = ê 0 ú R2 ¶q ê ú R2 ê ú 2 2 2 2 ê ú ê ú ê ú êq +q ú êë1 1úû êë0 0úû ë 1 2 û Nguyen Quang Hoang - Department of Applied Mechanics
Hiển thị 20 trang đầu. Tải tài liệu để xem đầy đủ.
Chưa có bình luận nào. Hãy là người đầu tiên!
Bình luận (0)
Chưa có bình luận nào. Hãy là người đầu tiên!