ĐỘ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
Showing the first 20 pages. Download the document to see it all.
No comments yet. Be the first!
Comments (0)
No comments yet. Be the first!