Trang 1
Chương 4. Động học: vận tốc và Jacobi -1- Chương 4. Động học: vận tốc và Jacobi -2- CHƯƠNG 4. ĐỘNG HỌC VẬN TỐC và JACOBI Khái niệm vận tốc góc của vật rắn, công thức cộng vận tốc góc Chương 4. Công thức tính vận tốc góc từ ma trận côsin chỉ phương. Động học vận tốc và ma trận Jacobi Vận tốc và gia tốc của điểm thuộc khâu i Kinematics: velocity and jacobian Ma trận Jacobi: jacobi tịnh tiến và jacobi quay Nguyễn Quang Hoàng Email: hoang.nguyenquang@hust.edu.vn Phương pháp xác định theo đạo hàm riêng Phương pháp xác định từ công thức hình học Cấu hình kỳ dị của robot Động học ngược ở mức vận tốc và gia tốc Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -3- Chương 4. Động học: vận tốc và Jacobi -4- Véc tơ vận tốc góc của vật rắn Chuyển động quay vô cùng bé quanh trục quay tức thời Xét vật rắn quay quanh trục tức thời qua điểm O, với góc quay vô cùng bé d u u Véc tơ vận tốc góc của N N N d d d vật rắn z0 r dr dr P P P 1 r P dr x O y0 O 0 dr = d uq ´r = dq ´r, dq = udq Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 2
Chương 4. Động học: vận tốc và Jacobi -5- Chương 4. Động học: vận tốc và Jacobi -6- Véc tơ vận tốc góc của vật rắn Véc tơ vận tốc góc của vật rắn Chuyển động quay vô cùng bé Xét vật rắn B chuyển động đối với hệ quy chiếu R . Vận tốc góc của w dr dq 0 = w ´r, w = u = qu vật rắn đặc trưng cho tốc độ thay c dt dt u e e đổi hướng của vật rắn trong hệ quy 3 N 2 Suy được ra d chiếu đó. r dc z dr O O e = w ´c, "c Î B 0 r P O’ 1 dt P1 Định nghĩa: vận tốc góc của vật Thật vậy x O y rắn là đại lượng vectơ thỏa mãn hệ 0 0 thức sau dr dr 1 = w ´r & 2 = w ´r , dt 1 dt 2 R d 0 d (r - r) = w ´(r - r) d c = w ´c c = w ´c, "c Î B dt 2 1 2 1 dt dt Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -7- Chương 4. Động học: vận tốc và Jacobi -8- Véc tơ vận tốc góc của vật rắn Véc tơ vận tốc góc của vật rắn Áp dụng: Vật rắn quay quanh trục z cố định w c R ( ) ( ) ( ) e w º 0 wB = e d e ⋅e +e d e ⋅e +e d e ⋅e 3 e 1 dt 2 3 2 dt 3 1 3 dt 1 2 2 r e 3 O O e 1 de = +1.dje d e = je j O’ 1 2 dt 1 2 w de = -1.dje d e = -je 2 1 dt 2 1 e Công thức liên hệ giữa véc tơ vận tốc góc của vật rắn với d e = 0 de 1 dt 3 2 các véc tơ đơn vị của hệ động như sau e e 2 2 d ( ) ( ) ( ) w º R wB = e d e ⋅e +e d e ⋅e +e d e ⋅e d de 0 1 1 dt 2 3 2 dt 3 1 3 dt 1 2 e 1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 3
Chương 4. Động học: vận tốc và Jacobi -9- Chương 4. Động học: vận tốc và Jacobi -10- Véc tơ vận tốc góc của vật rắn Véc tơ vận tốc góc của vật rắn Áp dụng: Vật rắn quay quanh trục z cố định Hiển nhiên đối với trường hợp vật rắn quay quanh trục cố định. Xét hai điểm A w ( ) ( ) ( ) và B thuộc vật. Vận tốc của hai điểm w = e d e ⋅e +e d e ⋅e +e d e ⋅e này được tính 1 dt 2 3 2 dt 3 1 3 dt 1 2 e = e (-je ⋅e )+e (0 ⋅e )+e (je ⋅e ) 3 R0d u 1 1 3 2 1 3 2 2 j v = r = w ´r , w A dt A A = je r r A B 3 R d 0 v = r = w ´r , e B dt B B 1 R d e 0 2 (r - r ) = w ´r - w ´r = w ´(r - r ) dt B A B A B A u = r - r R0d B A u = w ´u, "u Î B dt Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -11- Chương 4. Động học: vận tốc và Jacobi -12- Véc tơ vận tốc góc của vật rắn Véc tơ gia tốc góc của vật rắn Cách định nghĩa khác về véc tơ vận tốc góc Gia tốc góc đặc trưng cho tốc độ thay đổi vận tốc góc của Theo định lý Euler vật rắn theo thời gian - Xét chuyển động quay trong thời gian dt, (góc quay d , quanh trục u quay u) d - Trục quay tức thời là trục chứa véc tơ vận tốc góc, w a = e = w = w - Véc tơ vận tốc góc là véc tơ nằm trên trục quay tức thời dt dq Trường hợp vật rắn quay quanh trục w = u z cố định dt dq dq dq w w = u = e = qe dq w Trường hợp vật rắn quay quanh trục z cố định dt dt z z dq dq w = u = e = qe u d u dt dt z z O a = e = w = w = qe O dt z Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 4
Chương 4. Động học: vận tốc và Jacobi -13- Chương 4. Động học: vận tốc và Jacobi -14- Công thức cộng vận tốc góc của vật rắn Công thức cộng vận tốc góc của vật rắn Đạo hàm véc tơ trong hệ quy chiếu động Đạo hàm véc tơ trong hệ quy chiếu động a Đứng trong R0 Xét véc tơ a và hai hqc R0 và R1 w a R d e 0 w Trong hệ quy chiếu R1: e a = a(1)e(1) + a(1)e(1) + a(1)e(1) 3 dt 1 1 2 2 3 3 2 e a = a(1)e(1) + a(1)e(1) + a(1)e(1) ( ) 3 e 1 1 2 2 3 3 R +R0 wR1 ´ a(1)e(1) + a(1)e(1) + a(1)e(1) 2 0 O R e 1 1 2 2 3 3 Đứng trong R1 1 1 Đứng trong R1 O’ R O e R 0 R 1d (1) (1) (1) (1) (1) (1) O’ 1 1 a = a e + a e + a e R d dt 1 1 2 2 3 3 1 R R R a = a(1)e(1) + a(1)e(1) + a(1)e(1) 0d e(1) = 0 w 1 ´e(1), Đứng trong R0 dt j j dt 1 1 2 2 3 3 R0d j = 1, 2, 3 a = a(1)e(1) + a(1)e(1) + a(1)e(1) Liên hệ R R dt 1 1 2 2 3 3 0d 1d R R a = a + 0 w 1 ´a R R R dt dt +a(1) 0d e(1) + a(1) 0d e(1) + a(1) 0d e(1) 1 dt 1 2 dt 2 3 dt 3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -15- Chương 4. Động học: vận tốc và Jacobi -16- Công thức cộng vận tốc góc của vật rắn Công thức cộng vận tốc góc của vật rắn Công thức cộng vận tốc góc Liên hệ a Î B, R º B B B Đứng trong R0 2 a R d R d a 0 1 R R R a = a + 0 w 1 ´a R 2 dt dt 2 R d 0 a = R wB ´a = R wR ´a 0 0 2 dt R R R R R R R 0 wB ´a = 0 w 1 ´a + 1 w 2 ´a 1 R R Đứng trong R1 0 0 1 (R R R R ) = 0 w 1 + 1 w 2 ´a R d 1 a = R wB ´a = R wR ´a 1 1 2 dt R wB = R wR + R wR 0 0 1 1 2 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 5
Chương 4. Động học: vận tốc và Jacobi -17- Chương 4. Động học: vận tốc và Jacobi -18- Công thức cộng vận tốc góc của vật rắn Công thức cộng vận tốc góc của vật rắn Công thức cộng gia tốc góc Công thức cộng vận tốc góc B a Î B, R º B B a n a a Î B, R º B R R n n n R 0 R R R 1 1 R 0 R k k R d ( ) R d ( ) 0 0 R wB = R wR + R wR + ... + R wR 0 0 1 1 2 n-1 n dt dt R B R R R R R R ån R R R d n 0 w = 0 w 1 + 1 w 2 + ... + n-1 w n = k-1 w k = 0 å R wR k-1 k k=1 dt k=1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -19- Chương 4. Động học: vận tốc và Jacobi -20- Công thức cộng vận tốc góc của vật rắn Công thức cộng gia tốc góc B a R0d R c = w ´c, "c Î B a Î B, R º B n, = 2 2 dt 2 R d R d 0 1 a = a + R wR ´a R d ( ) R d ( ) 0 1 0 R wB = 0 R wR + R wR R R dt dt 0 0 1 1 2 1 dt dt 0 R d ( ) R d ( ) R B R R R R R R ån R R 0 1 w = w + w + ... + w = w = R wR + R wR + R wR ´ R wR 0 0 1 1 2 n-1 n k-1 k 0 1 1 2 0 1 1 2 dt dt k=1 Hay R R R R w = w + w , w = 0 w 1 , w = 1 w 2 a e r e r a = a + a + w ´w a e r e r Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 6
Chương 4. Động học: vận tốc và Jacobi -21- Chương 4. Động học: vận tốc và Jacobi -22- Liên hệ vận tốc, gia tốc các điểm thuộc vật Liên hệ vị trí hai điểm r = r + u w P v P O Liên hệ vận tốc hai điểm e u e Liên hệ vận tốc, gia tốc d d r 3 2 v = r = (r +u) P các điểm thuộc vật P dt P dt O r O R e O’ O 1 1 v = v + w´u, P O R Liên hệ gia tốc hai điểm 0 d d a = v = (v + w ´u) P dt P dt O a = a + a´u + w ´(w ´u) P O Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -23- Chương 4. Động học: vận tốc và Jacobi -24- Liên hệ vận tốc, gia tốc các điểm thuộc vật Liên hệ vận tốc, gia tốc các điểm thuộc vật Định lý hình chiếu vận tốc hai điểm thuộc vật Liên hệ vị trí hai điểm R {e(0),e(0),e(0)}, e(0) = [e(0),e(0),e(0) ]T 0 1 2 3 1 2 3 r = r + u r = r + u R {e ,e ,e }, e(1) = [e ,e ,e ]T B A w B v P O 1 1 2 3 1 2 3 v = v + w´u e w P v 3 u e e(1) = [e(0),e(0),e(0) ]R = e(0)T R B A r 2 A e 1 2 3 B 3 u e v ⋅u = v ⋅u + (w´u)⋅u A 2 r R e r Chiếu phương trình véc tơ trong B A O’ B 1 1 O r e các hệ quy chiếu R và R 0 R O’ O R 1 0 1 v ⋅u = v ⋅u 0 1 R : r(0) = r(0) + u(0), B A R 0 P O 0 R : r(1) = r(1) + u(1), 1 P O hc (v ) = hc (v ), "A B, Î body R : r(0) = r(0) + R u0 (1) u(0) = R u0 (1) AB B AB A 0 P O 1 1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 7
Chương 4. Động học: vận tốc và Jacobi -25- Chương 4. Động học: vận tốc và Jacobi -26- Liên hệ vận tốc, gia tốc các điểm thuộc vật Liên hệ vận tốc, gia tốc các điểm thuộc vật Liên hệ vận tốc hai điểm Liên hệ gia tốc hai điểm v = v + w´u a = a + a´u + w ´(w ´u) w v P O P O P w v e u e Chiếu phương trình véc tơ trong các hệ P Chiếu phương trình véc tơ trong hệ 3 2 quy chiếu R0 và R1 quy chiếu R0 r e u e 3 r O e 2 R : v(0) = v(0) + w(0)u(0), r a(0) = a(0) + (0)u(0) + (0)(0)u(0) O’ O 1 0 P O O P O R r e 1 1 ( ) R (1) (1) (1) (1) O’ O = a(0) + (0) + (0)(0) u(0) 0 R : v = v + w u , R O 1 P O 1 R 0 v(0) = Rv(1), u(0) = Ru(1) a(0) = a(0) + (S((0)) + S((0)) (S (0)))u(0) P P P O Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -27- Chương 4. Động học: vận tốc và Jacobi -28- Liên hệ vận tốc, gia tốc các điểm thuộc vật Liên hệ vận tốc, gia tốc các điểm thuộc vật Liên hệ gia tốc hai điểm Lưu ý các biểu thức w v P a(0) = Ra(1) a(1) = R-1a(0) = R aT (0) a = a + a´u + w ´(w ´u) e P P P P P P O 3 u e é ù 2 ê 0 -w w ú r ê z y ú Chiếu phương trình véc tơ trong O º S() = ê w 0 -w ú r e ê z x ú hệ quy chiếu R1 O’ O 1 ê-w w 0 ú R ë y x û 1 (1) (1) (1) (1) R w ´u u º S( )u 0 a(1) = a(1) + (1)u(1) + (1)(1)u(1) P O (0)u(0) º S((0))u(0) = a(1) + ((1) + (1)(1) )u(1) O w ´(w ´u) (1)(1)u(1) º S((1)) (S (1))u(1) a(1) = a(1) + (S((1)) + S((1)) (S (1)))u(1) (0)(0)u(0) º S((0)) (S (0))u(0) P O Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 8
Chương 4. Động học: vận tốc và Jacobi -29- Chương 4. Động học: vận tốc và Jacobi -30- Liên hệ véc tơ vận tốc góc và ma trận cosin chỉ hướng r = r + u u(0) = Ru(1) P O r(0) = r(0) + Ru(1) v = v + w ´u P O P O r(0) = r(0) + Ru (1) v(0) = v(0) + (0)u(0) Liên hệ véc tơ vận tốc P O P O (2) v(0) = v(0) + Ru (1) (1) v(0) = v(0) + (0)Ru(1) góc và ma trận cosin chỉ P O P O So sánh (1) và (2) suy ra hướng R = (0)R RR -1 = (0)RR-1 (0) = RR T (0) = RR -1 = RR T Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -31- Chương 4. Động học: vận tốc và Jacobi -32- Liên hệ véc tơ vận tốc góc và ma trận cosin chỉ hướng Liên hệ véc tơ vận tốc góc và ma trận cosin chỉ hướng Cách khác r(0) = r(0) + Ru(1) (0) = R(1) w v P O R = [e ,e ,e ] = [ ,i j k, ] P x y z in R in R 0 0 v(0) = v(0) + Ru (1) e u e P O v(0) = Rv(1) e = w ´e , e = w ´e , e = w ´e ] 3 x x y y z z 2 ( ) P P r v(1) = RT v(0) = RT v(0) + Ru (1) P P O u(0) = Ru(1) [e ,e ,e ] = w ´[e ,e ,e ] r O e x y z x y z O 1 O’ v(1) = v(1) + R RuT (1) (1) P O Tính trong R0 R = (0)[e(0),e(0),e(0) ]=(0)R x x x Tính trong R1 v = v + w ´u So sánh (1) và (2) suy ra P O RR T = (0)RRT R RT = (1)[e(1),e(1),e(1) ]=(1)E x x x v(1) = v(1) + (1)u(1) P O (0) = RR T R RT = (1) v(1) = v(1) + (1)u(1) (2) (1) = R RT P O (1) = R RT Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 9
Chương 4. Động học: vận tốc và Jacobi -33- Chương 4. Động học: vận tốc và Jacobi -34- Tính vận tốc góc các khâu robot, Jacobi quay • Đánh số các khâu: 0 -> n, khớp 2 1->n a2 q 3 • Dựng các hệ trục tọa độ gắn liền 3 q a E Tính vận tốc góc các các khâu. d1 z0 2 3 • Xây dựng các ma trận biến đổi 1 khâu robot, Jacobi quay tọa độ thuần nhất tương đối q • Tính các ma trận biến đổi tọa độ 1 x0 thuần nhất tuyệt đối. • Suy ra các ma trận cosin chỉ hướng tuyệt đối • Áp dụng công thức tính ma trận sóng ứng với vận tốc góc (lưu ý véc tơ vận tốc góc chiếu trong hệ nào: cố định hay hệ gắn liền vật) Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -35- Chương 4. Động học: vận tốc và Jacobi -36- Vận tốc góc của khâu i trong chuỗi động Vận tốc góc của khâu i trong chuỗi động Dựa trên ma trận biến đổi tọa độ thuần nhất : chứa vị trí gốc Vận tốc góc khâu i chiếu trong hệ cơ sở R : tọa độ O trong hệ cố định R và ma trận cosine chỉ hướng 0 i 0 của khâu i trong hệ cố định R0: 2 w(0) = R R0 0T R0 = w(0)R0 Ri1(q ) r(i1)(q ) a2 q i i i i i i 3 Ai1(q ) i i O i 3 i E i i 0 1 q a3 2 d z0 1 i1 1 é ù é ù 0 -w(0) w(0) w(0) T0( )q Aj (q ), ê i z, i y, ú ê i x, ú i j 1 j q1 w(0) = ê w(0) 0 -w(0)ú (0) ê (0)ú j 0 ê ú w = êw ú x0 i ê i z, i x, ú i ê i y, ú ê-w(0) w(0) 0 ú êw(0)ú R0( )q r0 ( )q ë i y, i x, û ë i z, û i1 T0( )q i O R0( )q Rj (q ) i 0 i1 i j 1 j j 0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 10
Chương 4. Động học: vận tốc và Jacobi -37- Chương 4. Động học: vận tốc và Jacobi -38- Vận tốc góc của khâu i trong chuỗi động Vận tốc góc của khâu i trong chuỗi động Tính đạo hàm ma trận cosin chỉ hướng A(q) Tính đạo hàm ma trận cosin chỉ hướng A(q) q = [q ,q ,...,q ]T ¶r ( )q ¶r ( )q ¶r ( )q 1 2 n r ( )q = 11 q + 11 q ++ 11 q ér ( )q r ( )q r ( )q ù 11 ¶q 1 ¶q 2 ¶q n ê 11 12 13 ú 1 2 n R0 = R0( )q = êr ( )q r ( )q r ( ) ,q ú n ¶r ( )q i i êê 21 22 23 úú = å 11 q ¶q k êër ( )q r ( )q r ( )q úû k=1 k 31 32 33 ¶r ( )q ¶r ( )q ¶r ( )q é ù r ( )q = ij q + ij q +...+ ij q êr ( )q r ( )q r ( )q ú ij ¶q 1 ¶q 2 ¶q n ê 11 12 13 ú 1 2 n R 0 = êr ( )q r ( )q r ( )q ú n ¶r ( )q i ê 21 22 23 ú = å ij q , i j, = 1,2, 3 êr ( )q r ( )q r ( )q ú ¶q k ë 31 32 33 û k=1 k Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -39- Chương 4. Động học: vận tốc và Jacobi -40- Vận tốc góc của khâu i trong chuỗi động Vận tốc góc của khâu i trong chuỗi động Vận tốc góc khâu i chiếu trong hệ cơ sở R : Vận tốc góc khâu phụ thuộc tuyến tính vào véc tơ vận tốc 0 é ù khớp T ê 0 -w(0) w(0) ú q = [q ,q ,...,q ] ê i z, i y, ú 1 2 n w(0) = ê w(0) 0 -w(0)ú = R R 0 0T i ê i z, i x, ú i i Do đó được viết dạng tích ma trận với véc tơ như sau: ê-w(0) w(0) 0 ú ë i y, i x, û w(0) = J ( ) ,q q J ( ),q i = 1,2,...,n R i Ri Ri é ù i w(0) P ê i x, ú O i Ma trận Jacobi quay của w(0) = êêw(0)ú i w(0) = J ( ) ,q q ú n Rn khâu i trong hệ cơ sở. i ê i y, ú (0) R0 êëw úû i z, Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 11
Chương 4. Động học: vận tốc và Jacobi -41- Chương 4. Động học: vận tốc và Jacobi -42- Vận tốc góc của khâu i trong chuỗi động Vận tốc góc của khâu i trong chuỗi động Ma trận Jacobi quay của khâu i trong hệ cơ sở R0. Vận tốc góc khâu i chiếu trong hệ gắn liền khâu i, R: i é¶w(0) ¶w(0) ¶w(0) ù ¶w(0) ê i x, i x, i x, ú 0 ( )i ( )i ê ¶q ¶q ¶q ú J ( )q = i i z, i y, ê 1 2 n ú Ri ¶q êê¶w(0) ¶w(0) ¶w(0) úú w( )i ( )i 0 ( )i R0T R0 J ( )q = ê i y, i y, i y, ú i i z, i x, i i Ri ê ¶q ¶q ¶q ú ( )i ( )i 0 Ri ê 1 2 n ú i y, i x, ê¶w(0) ¶w(0) ¶w(0) ú é ù Pi ê i z, i z, i z, ú ¶w(0) ¶w(0) ¶w(0) O ê n x, n x, n x, ú i êê ¶q ¶q ¶q úú ê ú é ù ë 1 2 n û ê ¶q ¶q ¶q ú wi) ê¶w(0)1 ¶w(0)2 ¶w(0)n ú ê i x, ú R0 ê n y, n y, n y, ú ê ú J ( )q = ê ú w( )i = êw( )i ú Rn ê ¶q ¶q ¶q ú i ê i y, ú ê 1 2 n ú w( )i ê¶w(0) ¶w(0) ¶w(0) ú ê ú êê n z, n z, n z, úú ë i z, û w( )i = J( )i ( )q q ê ¶q ¶q ¶q ú ë 1 2 n û i R i, Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -43- Chương 4. Động học: vận tốc và Jacobi -44- Vận tốc góc của khâu i trong chuỗi động Vận tốc góc của khâu i trong chuỗi động Ma trận Jacobi quay của khâu i trong hệ gắn liền khâu Ri. é ù w(0) = J ( )q q wi) i R i, ê i x, ú w( )i = êêw( )i ú w( )i = J( )i ( )q q w(0) = R q( )w( )i i i y, ú i R i, i i i ê ú w( )i = J( )i ( )q q êw( )i ú ë i z, û i R i, é¶w( )i ¶w( )i ¶w( )i ù Ri Ri ê i x, i x, i x, ú ê ú Pi Pi ê ¶q ¶q ¶q ú O O ê 1 2 n ú i i ê¶w( )i ¶w( )i ¶w( )i ú Ji ( )q = ê i y, i y, i y, ú J ( )q = R q J( ) ( )i ( )q R i, ê ¶q ¶q ¶q ú ê 1 2 n ú R i, i R i, ê¶w( )i ¶w( )i ¶w( )i ú R0 R0 ê i z, i z, i z, ú êê ¶q ¶q ¶q úú ë 1 2 n û Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 12
Chương 4. Động học: vận tốc và Jacobi -45- Chương 4. Động học: vận tốc và Jacobi -46- Vận tốc điểm thuộc khâu i trong chuỗi động Xét điểm P có yọa độ điểm P trong hệ tọa độ khâu i, Ri. R i u( )i = [x( )i ,y( )i ,z( )i ]T = const P Tính vận tốc các điểm, P P P P Oi Jacobi tịnh tiến Tọa độ của điểm P trong hệ cố định R0: R 0 r(0)( )q = r(0)( )q +R0( )q u( )i P O i P i Vận tốc và gia tốc, Jacobi của điểm N thuộc khâu i Vận tốc của điểm P trong hệ cố định R0: v(0)( )q = r(0)( )q = r(0)( )q +R 0( )q u( )i v(0) = r(0) P P O i P O O i i i = r(0)( )q + w(0)R u0 ( )i O i i P i Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -47- Chương 4. Động học: vận tốc và Jacobi -48- Vận tốc điểm thuộc khâu i trong chuỗi động Vận tốc điểm thuộc khâu i trong chuỗi động Tính theo tọa độ thuần nhất Vận tốc điểm P phụ thuộc tuyến tính R vào véc tơ vận tốc khớp R rˆ(0)( )q =T0( )q uˆ( )i i i P i P q = [q ,q ,...,q ]T é ù é ù Pi Pi R0( )q r0 ( )q u( )i O 1 2 n O = ê i O ú ê P ú i i ê 0 i1 ú ê 1 ú êë ú êû ë úû R Do đó được viết thành R 0 0 éR 0 r(0)ù éw(0)R0 v(0)ù v(0) = J ( )q q T 0 = ê i O ú = ê i i O ú ê 0 0i ú ê 0 0i ú P T i, i ê ú ê ú ë û ë û év(0)ù éw(0)R0 v(0)ù éu( )i ù Jacobi tịnh tiến vˆ(0) = ê P ú =T u 0 ˆ( )i = ê i i O ú ê P ú P ê 1 ú i P ê 0 0i ú ê 1 ú êë úû êë úû êë úû MAPLE: J = jacobian(v, qdot) T Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 13
Chương 4. Động học: vận tốc và Jacobi -49- Chương 4. Động học: vận tốc và Jacobi -50- Vận tốc điểm thuộc khâu i trong chuỗi động Vận tốc điểm thuộc khâu i trong chuỗi động Tính Jacobi tịnh tiến Tính Jacobi tịnh tiến éu( )i ù v(0) = J ( ) ,q q rˆ(0)( )q =T0( )q ê P ú, r(0)( )q = r0 ( )q +R0( )q u( )i P T i, P n ê 1 ú P O i P êë úû i é¶x(0) ¶x(0) ¶x(0) ù ê P P P ú r(0)( )q = [x(0)( ),q y(0)( ),q z(0)( )]q T ê ú P P P P ê ¶q ¶q ¶q ú ê 1 2 n ú é ù ¶y(0) ¶y(0) ¶y(0) n J ( )q = ê P P P ú êêå(¶x(0) / ¶q )q úú T êê ¶q ¶q ¶q úú é ù P j j 1 2 n êx(0)( )q ú ê j=1 ú ê¶z(0) ¶z(0) ¶z(0) ú ê P ú ê n ú ê P P P ú v(0) = êy(0)( )q ú = êå(¶y(0) / ¶q )q ú = J ( ) ,q q ê ¶q ¶q ¶q ú P P ê P j j ú T i, êë úû ê ú 1 2 n êz(0)( )q ú ê j=1 ú ë P û êån ú ê (¶z(0) / ¶q )q ú ê P j j ú MAPLE: JT = jacobian(rP, q) ë j=1 û Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -51- Chương 4. Động học: vận tốc và Jacobi -52- Vận tốc điểm tác động cuối E Vận tốc điểm tác động cuối E Tương tự đối với điểm tác động cuối E (thuộc khâu cuối EE, n) r(0)( )q = [x(0)( ),q y(0)( ),q z(0)( )]q T E E E E éR0( )q r0 ( )q ù éu(n)ù é ù rˆ(0)( )q =T0( )q uˆ(n) = ê n O ú ê E ú êx(0)( )q ú E n E ê 0 n1 ú ê 1 ú ê E ú êë ú êû ë úû v(0) = êy(0)( )q ú = J ( ) ,q q r(0)( )q = r0 ( )q +R0( )q u(n) E ê E ú T E O n E êz(0)( )q ú é¶x(0) ¶x(0) ¶x(0) ù n ë E û ê ú ê E E E ú r(0)( )q = [x(0)( ),q y(0)( ),q z(0)( )]q T R ê ¶q ¶q ¶q ú E E E E i v(0) = J ( ) ,q q ê¶y(0)1 ¶y(0)2 ¶y(0)n ú E T J ( )q = ê E E E ú Oi T ê ¶q ¶q ¶q ú ê 1 2 n ú O Ma trận Jacobi tịnh tiến ê¶z(0) ¶z(0) ¶z(0) ú R0 n ê ú ê E E E ú MAPLE: J = jacobian(r , q) êë ¶q ¶q ¶q úû T E 1 2 n Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 14
Chương 4. Động học: vận tốc và Jacobi -53- Chương 4. Động học: vận tốc và Jacobi -54- Ma trận Jacobi Ma trận Jacobi Véc tơ chứa vận tốc điểm E và vận tốc góc khâu cuối Ví dụ. Xét tay máy phẳng 3 dof như trên hình vẽ, hãy xác định ma trận Jacobi của khâu cuối. = [v(0)T ,(0)T T] = [v ,v ,v ,w(0),w(0),w(0)]T Biết ma trận biến đổi tọa độ thuần nhất như sau: E n E x, E y, E z, nx ny nz cq sq 0 l sq q cq l cq Kết hợp Jacobi tịnh tiến và Jacobi quay của khâu cuối EE: 13 13 1 1 2 1 3 13 sq cq 0 l cq q sq l sq T0( )q 13 13 1 1 2 1 3 13 x v(0) = J ( ) ,q q üï 3 0 0 1 0 y 3 E T ïï = J q q( ) 3 ýï 0 0 0 1 E w(0) = J ( ) ,q q n R ïïþ R q3 i y q2 0 O é ù O q q q z 2 J ( )q i 13 1 3 2 J q( ) = ê T ú êJ ( )q ú R On l q ê ú 0 1 1 ë R û O 0 x 0 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -55- Chương 4. Động học: vận tốc và Jacobi -56- Ma trận Jacobi Ma trận Jacobi Tọa độ điểm cuối E q q q 13 1 3 Jacobi tịnh tiến ??? x r(0) r(0)( )q q = [q ,q ,q ]T r(0) r(0)( )q y 3 E E 3 E E 1 2 3 E q3 x l sq q cq l cq y q2 éê¶x(0) ¶x(0) ¶x(0) ùú e 1 1 2 1 3 13 0 O E E E z 2 ê ú y l cq q sq l sq 2 ê ¶q ¶q ¶q ú ê 1 2 n ú e 1 1 2 1 3 13 l ¶r(0)( )q ¶y(0) ¶y(0) ¶y(0) z 0 1 q1 J ( )q = E = ê E E E ú O0 T ¶q ê ¶q ¶q ¶q ú e x0 ê ú ê 1 2 n ú ê¶z(0) ¶z(0) ¶z(0) ú Jacobi tịnh tiến ??? ê E E E ú q = [q ,q ,q ]T ê ¶q ¶q ¶q ú 1 2 3 ë 1 2 n û Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 15
Chương 4. Động học: vận tốc và Jacobi -57- Chương 4. Động học: vận tốc và Jacobi -58- Ma trận Jacobi Ma trận Jacobi Ma trận cosin chỉ hướng khâu 3 q q q Jacobi tịnh tiến ??? q q q 13 1 3 13 1 3 cq sq 0 cq sq 0 13 13 13 13 r(0) R0( )q sq cq 0 , R0,T ( )q sq cq 0 J ( )q E 3 13 13 3 13 13 T q 0 0 1 0 0 1 l cq q sq l sq cq l sq 1 1 2 1 3 13 1 3 13 l sq q cq l cq sq l cq sq cq 0 1 1 2 1 3 13 1 3 13 13 13 0 0 0 R 0( )q q cq sq 0 , q q q 3 13 13 13 13 1 3 0 0 0 (0) R R 0 0T ?? 3 3 3 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -59- Chương 4. Động học: vận tốc và Jacobi -60- Ma trận Jacobi Ma trận Jacobi Véc tơ vận tốc góc khâu 3 Kết hợp lại ta được ma trận Jacobi tổng thể l cq q sq l sq cq l sq 1 1 2 1 3 13 1 3 13 0 q 0 0 l sq q cq l cq sq l cq 1 1 2 1 3 13 1 3 13 13 J ( )q 0 0 0 (0) R R 0 0T q 0 0 (0) 0 J q T 3 3 3 13 J q( ) 3 R J ( )q 0 0 0 0 0 0 q q R 1 3 0 0 0 Jacobi quay é¶w(0) ¶w(0) ¶w(0) ù 1 0 1 ê i x, i x, i x, ú ê ¶q ¶q ¶q ú i = 3 ê 1 2 n ú [v(0), v(0), v(0), (0), (0), (0)]T ê¶w(0) ¶w(0) ¶w(0) ú 0 0 0 Ex Ey Ez 3x 3y 3z ê i y, i y, i y, ú J ( )q = ê ú Ri ê ¶q ¶q ¶q ú J 0 0 0 ê 1 2 n ú R,3 = J q q( ) ê¶w(0) ¶w(0) ¶w(0) ú 1 0 1 ê i z, i z, i z, ú êê ¶q ¶q ¶q úú ë 1 2 n û Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 16
Chương 4. Động học: vận tốc và Jacobi -61- Chương 4. Động học: vận tốc và Jacobi -62- Ma trận Jacobi Trong trường hợp robot phẳng này, nếu chọn véc tơ • Cần tính vận tốc góc của tất cả các khâu • Các điểm quan tâm sau này là: khối tâm Ci của các khâu, [v(0), v(0), (0)]T = J q q( ) điểm tác động cuối, các điểm chịu tác dụng của lực,.. Ex Ey 3z r(0)( )q J ( )q ma trận Jacobi được viết lại thành Ci T i, r(0)( )q J ( )q l cq q sq l sq cq l sq E T E, J ( )q 1 1 2 1 3 13 1 3 13 R0( )q w(0) J ( )q J q( ) T l sq q cq l cq sq l cq i i R,i J ( )q 1 1 2 1 3 13 1 3 13 R 1 0 1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -63- Chương 4. Động học: vận tốc và Jacobi -64- Phương pháp Denavit-Hartenberg Biểu diễn các tham số Denavit-Hartenberg z z’i-1 i-1 zi i Phương pháp hình học zi-2 i-1 khớp i khâu i+1 khâu i i xác định ma trận Jacobi khâu i-1 khớp i+1 khớp i-1 ai xi O’ Oi i i di x’i O i x Hệ trục tọa độ (Oxyz) i-1 i-1 i gắn liền khâu i. Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 17
Chương 4. Động học: vận tốc và Jacobi -65- Chương 4. Động học: vận tốc và Jacobi -66- Phương pháp ma trận Denavit-Hartenberg Phương pháp ma trận Denavit-Hartenberg Khớp i là khớp trượt (tịnh tiến) : d = d(t) z z’i-1 i i i-1 zi Khớp i là khớp quay: = (t) i i i zi-2 i-1 Biến khớp hay tọa độ suy rộng q khớp khâu i i khâu i+1 i khâu i-1 i q (1 )d , khớp i-1 khớp i+1 i i i i i ai 1 xi véc tơ đơn vị Khớp i là khớp quay O’ Oi i i trục z i 0 Khớp i là khớp tịnh tiến d i-1 i x’ i z º e(i-1) O i i-1 x i-1 z Các véc tơ đơn vị của hệ qc Ri gắn liền vào khâu thứ i i-1 Vận tốc góc tương đối của khâu (i) đối với khâu (i-1) {e( )i ,e( )i ,e( )i } º {x ,y ,z } x y z i i i i-1w = qz = rq z i i i-1 i i i-1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -67- Chương 4. Động học: vận tốc và Jacobi -68- Thiết lập ma trận Jacobi theo phương pháp hình học Thiết lập ma trận Jacobi theo phương pháp hình học Vận tốc góc của khâu thứ i (i = 1,2,...,n) Ma trận Jacobi quay 0 1 i-1 åi j-1 w = w + w +...+ w = w Lưu ý rằng, hình chiếu của véc tơ đơn vị z e( )i trong hệ i 1 2 i j i z j=1 z(0) R0 i quy chiếu R i chính là cột thứ ba của ma trận quay å 0 i w = r q z + r q z +...+ rq z = r q z i 1 1 0 2 2 1 i i i-1 j j j-1 j=1 Hình chiếu của véc tơ đơn vị z e( )i trong hqc Ri i 3 Chiếu véc tơ vận tốc góc khâu i trong hệ cơ sở R0: z( )i = k = [0, 0,1]T z(0) = R z0 ( )i = R k0 i i i i i (0) = r q z(0) + r q z(0) +...+ rq z(0) i 1 1 0 2 2 1 i i i-1 Chiếu véc tơ vận tốc góc khâu i trong hệ khâu Ri: ( )i = r q z( )i + r q z(i) +...+ rq z(i) i 1 1 0 2 2 1 i i i-1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 18
Chương 4. Động học: vận tốc và Jacobi -69- Chương 4. Động học: vận tốc và Jacobi -70- Thiết lập ma trận Jacobi theo phương pháp hình học Thiết lập ma trận Jacobi theo phương pháp hình học åi (0) = rq k + r q R k0 +..+ rq R0 k + w = r q z + r q z +...+ rq z = r q z i 1 1 0 2 2 1 i i i-1 j j j-1 n 1 1 2 2 1 i i i-1 j=1 ...+ r q R0 k = J ( )q q Chiếu véc tơ vận tốc góc khâu i trong hệ quy chiếu cơ sở R : n n n-1 R 0 (0) = r q z(0) + r q z(0) +...+ rq z(0) Từ đây viết được ma trận Jacobi quay như sau i 1 1 0 2 2 1 i i i-1 J ( )q = [r k,r R k0 ,..,r R0 k,...,r R0 k] Chiếu véc tơ vận tốc góc khâu n trong hệ quy chiếu cơ sở R 1 2 1 i i-1 n n-1 R0: J ( )q = [J ,J ,..,J ,...,J ] (0) = rq z(0) + r q R z0 (1) +...+ r q R0 z(n-1) R R1 R2 Ri Rn n 1 1 0 2 2 1 1 n n n-1 n-1 Cột i của mt Jacobi quay (0) 0 0 ìïz(0) = R0 k Khớp i là khớp quay = rq k + r q R k +..+ rq R k + J = íïï i-1 i-1 n 1 1 2 2 1 i i i-1 ...+ r q R0 k = J ( )q q R i, ïï 0 Khớp i là khớp tịnh tiến n n n-1 R ïî Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -71- Chương 4. Động học: vận tốc và Jacobi -72- Thiết lập ma trận Jacobi theo phương pháp hình học Thiết lập ma trận Jacobi theo phương pháp hình học Ma trận Jacobi tịnh tiến q z thành phần vận tốc i do biến i-1 ån d v(0) = J ( )q q = J q , J ( )q = [J ,..,J ,...,J ] khớp i tham gia vào vận tốc vE. i E T T i, i T T1 Ti Tn 2. khớp tịnh tiến: i=1 z r thành phần vận tốc q do biến i-1 s i i Oi-1 E khớp i tham gia vào vận tốc v . v ( )i = q z = d z E E i i-1 i i-1 r Ở đây cần xem xét hai loại khớp: E khớp quay và khớp tịnh tiến: r s O E Cột i của mt Jacobi tịnh tiến khớp tịnh tiến i-1 1. khớp quay r E v ( )i = q z ´s ¶v(0) ìïz(0)(r(0) -r(0) ) Khớp i là khớp quay E i i-1 J = E = íïï i-1 E O = q z ´(r -r ) khớp quay i-1 i i-1 E O T i, ¶q ïï z(0) i-1 i ïî i-1 Khớp i là khớp tịnh tiến q z = qz i i-1 i i-1 Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 19
Chương 4. Động học: vận tốc và Jacobi -73- Chương 4. Động học: vận tốc và Jacobi -74- Thiết lập ma trận Jacobi theo phương pháp hình học Thiết lập Jacobi quay (đối với hệ tọa độ khâu) Kết hợp Jacobi tịnh tiến và Jacobi quay Chiếu véc tơ vận tốc góc khâu i trong hệ cơ sở R0: J q( ) = [J ,J ,..,J ,...,J ] 1 2 i n (0) = r q z(0) + r q z(0) +...+ rq z(0) J ( )q là cột i của ma trận Jacobi J q( ) i 1 1 0 2 2 1 i i i-1 i ìïïz(0) (0) = J ( )q q J = íï i-1 Khớp i là khớp tịnh tiến, = 0 i Ri ï i i 0 ïïî Chiếu véc tơ vận tốc góc khâu i trong hệ khâu Ri: ìïz(0) (r(0) -r(0) ) J = íïï i-1 E O ( )i = r q z( )i + r q z(i) +...+ rq z(i) ï i-1 Khớp i là khớp quay, = 1 i 1 1 0 2 2 1 i i i-1 i ï z(0) = R0 k i ïî i-1 i-1 viết ở dạng gộp lại ( )i = Ji ( )q q ér z(0)(r(0) -r(0) )+(1- r )z(0) ù i Ri J = ê i i-1 E O i i-1ú ? Ji ( )q i ê riz-1(0) ú Ri êë i i-1 úû Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -75- Chương 4. Động học: vận tốc và Jacobi -76- Thiết lập Jacobi quay (đối với hệ tọa độ khâu) Thiết lập ma trận Jacobi theo phương pháp hình học (0) = J ( )q q Ví dụ : tay máy phẳng hai bậc tự do với hai khớp quay i Ri (0) = R0( )q ( )i ( )i = [R0( )]q T (0) O i i i i i i 2 ( )i = Ji ( )q q r l q i Ri 2 2 y0 O2 ? Ji ( )q ( )i = [R0( )]q T (0) l O 1 1 Ri i i i = [R0( )]q T J ( )q q O q1 0 i Ri x 0 Ji ( )q = [R0( )]q T J ( )q Ri i Ri Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Trang 20
Chương 4. Động học: vận tốc và Jacobi -77- Chương 4. Động học: vận tốc và Jacobi -78- Thiết lập ma trận Jacobi theo phương pháp hình học Thiết lập ma trận Jacobi theo phương pháp hình học z(0) = z(0) = [0, 0, 1]T éS z( (0))(r(0) -r(0)) S z( (0))(r(0) -r(0))ù 0 1 J = ê 0 O O 1 O O ú O ê z(0)2 0 z(0)2 1 ú 2 êë 0 1 úû é ù0 él cq ù ê ú ê 1 1ú r l q2 Thực hiện các tính toán ta nhận được ma trận Jacobi r(0) = ê ú0 , r(0) = êl sq ú, y O 2 ê ú ê ú 0 2 O ê ú O ê 1 1 ú é ù 0 1 -l sq -l s q( +q ) -l s q( +q ) ê úë û0 êë 0 úû l1 O ê 1 1 2 1 2 2 1 2 ú 1 ê ú O q ê l cq +l c q( +q ) l c q( +q ) ú 0 1 ê 1 1 2 1 2 2 1 2 ú él cq +l c q( +q )ù x ê 0 0 ú ê 1 1 2 1 2 ú 0 J q( ) = ê ú r(0) = êl sq +l s q( +q ) ,ú ê 0 0 ú O ê 1 1 2 1 2 ú ê ú 2 ê 0 ú ê 0 0 ú êë úû ê ú êë 1 1 úû Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics Chương 4. Động học: vận tốc và Jacobi -79- Chương 4. Động học: vận tốc và Jacobi -80- Thiết lập ma trận Jacobi theo phương pháp hình học Thiết lập ma trận Jacobi theo phương pháp hình học Jacobi của điểm P bất kỳ thuộc khâu thứ k x = J q q( ) Dùng để tính vận tốc điểm P và vận tốc góc của khâu k chứa điểm P v k P q ,q ,...,q P q = [q , q ] ,T x = [v(0) , v(0) , w(0)]T k+1 k+2 n 1 2 E x, E y, 2 không có mặt trong vận tốc O góc khâu k, vận tốc điểm P. k é-l sq -l s q( +q ) -l s q( +q )ù ê 1 1 2 1 2 2 1 2 ú Do đó các cột từ k+1 đến cột n R ê ú 0 J q( ) = ê l cq +l c q( +q ) l c q( +q ) ú của ma trận Jacobi này bằng 0 ê 1 1 2 1 2 2 1 2 ú êë 1 1 úû év(0) ù ê P ú = J ( ) ,q q J ( )q = [J ,J ,..,J ,...,J ] êw(0)ú P P 1 2 i n êë k úû Nguyen Quang Hoang Nguyen Quang Hoang Department of Applied Mechanics Department of Applied Mechanics
Hiển thị 20 trang đầu. Tải tài liệu để xem đầy đủ.
Bình luận (0)
Chưa có bình luận nào. Hãy là người đầu tiên!