Tải bản đầy đủ - 0 (trang)
CHƯƠNG II: HỆ PHƯƠNG TRÌNH ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CỦA ROBOT CÔNG NGHIỆP

CHƯƠNG II: HỆ PHƯƠNG TRÌNH ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CỦA ROBOT CÔNG NGHIỆP

Tải bản đầy đủ - 0trang

GVHD:TS. NGUYỄN CAO CƯỜNG

5082.1.



Hệ phương trình động học Robot :



5092.1.1.



Đặt vấn đề :



510Cơ



cấu chấp hành của Robot thường là một cơ cấu hở gồm một chuỗi các khâu

511(link) nối với nhau bằng các khớp (joints). Các khớp động này là khớp quay (R)

512hoặc khớp tịnh tiến (T). Để Robot có thể thao tác linh hoạt cơ cấu chấp hành của

513nó phải có cấu tạo sao cho điểm mút của khâu cuối cùng đảm bảo dễ dàng di

514chuyển theo một quỹ đạo nào đó, đồng thời khâu này có một hướng nhất định theo

515yêu cầu. Khâu cuối cùng này thường là bàn kẹp (griper), điểm mút của nó chính là

516“điểm tác động cuối” E (end-effector).

517Để



xét vị trí và hướng của E trong khơng gian ta gắn vào nó một hệ toạ độ động

518thứ n và gắn với mỗi khâu động một hệ toạ độ khác, còn gắn liền với giá đỡ một hệ

519toạ độ cố định. Đánh số ký hiệu các hệ này từ 0 đến n bắt đầu từ giá cố định. Khi

520khảo sát chuyển động của Robot cần biết “định vị và định hướng” tại điểm tác

521động cuối trong mọi thời điểm. Các lời giải của bài toán này được xác định từ

522những phương trình Động học của Robot. Các phương trình này là mơ hình Động

523học của Robot. Chúng được xây dựng trên cơ sở thiết lập các mối quan hệ giữa các

524hệ toạ độ động nói trên so với hệ toạ độ cố định.

5252.1.2.



Xác định trạng thái của Robot tai điểm tác động cuối :



526Trạng



thái của Robot tại “điểm tác động cuối” hoàn toàn xác định bằng sự định vị

527và định hướng tại điểm tác động cuối đó.



528



nx sx ax





n n a

TE  �y y y



nz sz az



�0 0 0



px �

py �



pz �



1�



(2.1)



529Trong



đó các phần tử của ma trận 3x1 là toạ độ p x , py, pz của “điểm tác động cuối”

530E. Mỗi cột của ma trận quay 3x3 là một vectơ đơn vị chỉ phương một trục của hệ

531toạ độ động NSA (chính là UVW) biểu diễn trong toạ độ cố định XYZ.



SVTH:Bùi Anh Tiến-TĐH9a2



25



GVHD:TS. NGUYỄN CAO CƯỜNG

532Hệ



toạ độ gắn liền với bàn kẹp của Robot có các vectơ đơn vị chỉ phương các trục

533như sau :



534



a - vector có hướng tiếp cận (approach) với đối tác .



535



s - vector có hướng đường trượt (sliding) đóng mở bàn kẹp .



536



n - vector pháp tuyến (normal).



5372.1.3.



Mơ hình động học :



5382.1.3.1.



Ma trận quan hệ :



539Chọn



hệ toạ độ cố định gắn liền với giá đỡ và các hệ toạ độ gắn với từng khâu

540động. Ký hiệu các hệ toạ độ này từ 0 đến n, kể từ giá cố định trở đi.

541Một



điểm bất kì nào đó trong khơng gian được xác định trong hệ toạ độ thứ i bằng

542bán kính ri và trong hệ toạ độ cố định x 0, y0, z0 được xác định bằng bán kính vector

543r0 :

544



r0 = A1A2…Airi



(2.2)

(2.3)



545



hoặc



r0 = Tiri



546



với



Ti = A1A2…Ai , i= 1, 2, …n (2.4)



Trong đó ma trận A1 mơ tả vị trí hướng của khâu đầu tiên; ma trận A 2 mơ tả

548vị trí và hướng của khâu thứ 2 so với khâu đầu; ma trận A i mô tả vị trí và hướng

549của khâu thứ i so với khâu thứ i-1.

547



550Như



vậy, tích của các ma trận Ai là ma trận Ti mơ tả vị trí và hướng của khâu thứ i

551so với giá trị cố định. Thường kí hiệu ma trận T với 2 chỉ số: trên và dưới. Chỉ số

552dưới chỉ khâu đang xét còn chỉ số trên để chỉ toạ độ được dùng để đối chiếu. Ví dụ,

553biểu thức (2.4) có thể viết lại là :

554

555



với



Ti  0Ti  A1 1Ti



(2.5)



Ti  A2A3...Ai



(2.6)



1



SVTH:Bùi Anh Tiến-TĐH9a2



26



GVHD:TS. NGUYỄN CAO CƯỜNG

556là



ma trận mơ tả vị trí và hướng của khâu thứ i so với khâu thứ nhất. Trong kí hiệu

557thường bỏ qua chỉ số trên nếu chỉ số đó bằng 0.

558Denavit



& Hartenberg đã đề xuất dùng ma trận thuần nhất 4x4 mô tả quan hệ giữa

5592 khâu liên tiếp trong cơ cấu không gian .

5602.1.3.2



Thiết lập hệ toạ độ :

561Gốc của hệ toạ độ gắn liền với khâu thứ i (gọi là hệ toạ độ thứ i) đặt tại giao điểm

562giữa đường vng góc chung (ai) và trục khớp động i+1.

563Trường



hợp 2 trục giao nhau thì gốc hệ toạ độ lấy trùng với giao điểm đó. Nếu 2

564trục song song với nhau thì chọn gốc toạ độ là điểm bất kì trên trục khớp động i+1.

565Trục



zi của hệ toạ độ thứ i nằm dọc theo trục khớp động i+1.



566Trục



xi của hệ toạ độ thứ i nằm dọc theo đường vuông góc chung hướng từ khớp

567động i đến khớp động i+1. Trường hợp 2 trục giao nhau, hướng trục x i trùng với

568hướng vector tích zi x zi-1, tức là vng góc với mặt phẳng chứa zi, zi-1.

569

570

5712.2



Phương trình động học thuân robot

572Robot 2 thanh nối là robot với khâu phẳng, vì vậy cơ chế và phạm vi hoạt động của

573robot nằm trong mặt phẳng bao gồm 2 khớp quay và khớp nối như sau:

574

575

576

577



578



SVTH:Bùi Anh Tiến-TĐH9a2



27



GVHD:TS. NGUYỄN CAO CƯỜNG



579

580

581Các

582

583

584

585

586

587

588



Thiết kế hệ tọa độ cho các thanh nối

bước thực hiện bài toán động học thuận cho tay máy :

Bước 1: Xác định số khớp và số thanh nối.

Bước 2: Gắn lên các thanh nối từ 0 đến n các hệ trục tọa độ.

qi: là góc quay của thanh nối thứ i.

di: là độ lệch khâu.

ai: là độ dài đường vng góc chung giữa zi-1 và zi.

αi: là góc vặn của thanh nối.



Cách xác định trục zi: là trục mà xung quanh nó khớp thứ i+1 quay hoặc dọc theo nó

590khớp ( i = 1 ÷ n-1 ) tịnh tiến.

589



591



Z0: trục mà xung quanh nó khớp 1 quay.

O0: tâm hệ trục tọa độ quy chiếu, chọn một điểm cố định trên đế robot.

Z1: trục mà xung quanh nó khớp 2 quay hoặc khớp tịnh tiến.

Zn-1: trục mà xung quanh nó khớp n quay.

Zn: trùng phương với Zn-1.

Cách xác định trục Xi: Trục X thường được đặt dọc theo pháp tuyến chung và

598hướng từ khớp i đến i+1. Trong trường hợp các trục khớp cắt nhau thì trục X chọn

599theo tích vecto Zi-1 × Zi.

592

593

594

595

596

597



SVTH:Bùi Anh Tiến-TĐH9a2



28



GVHD:TS. NGUYỄN CAO CƯỜNG

Cách xác định trục Yi:



600



601Xác



định theo quy tắc bàn tay phải.



602

603

604

605

606

607

608



Bước 3: Xác định các biến khớp.

Khớp quay tương ứng với biến khớp quay q.

Khớp tịnh tiến tương ứng với biến khớp tịnh tiến d.

Bước 4: Xác định quan hệ giữa hai khung tọa độ i và i-1.

Hệ trục tọa độ i và hệ trục tọa độ i-1 giữa hai khâu nối tiếp nhau có quan hệ

với nhau bằng phép biến đổi đồng nhất, theo trình tự sau:

- Quay xung quanh trục Zi-1 một góc Өi sao cho trục Xi-1 trùng với phương của

trục Xi.

- Tịnh tiến dọc theo trục Zi-1 một đoạn di để gốc khung tọa độ mới trùng chân

pháp tuyến chung trục i-1 và I, ( Xi-1≡ Xi ).

- Tịnh tiến dọc theo trục Xi-1 một đoạn ai ( Oi-1≡ Oi ).

- Quay xung quanh trục Xi-1 một góc αi sao cho trục Zi-1 trùng với trục Zi.



609

610

611

612

613

614

615

616

617

618



Các phép biến đổi trên được thực hiện so với khung tọa độ hiện tại, do đó

phép biến đổi tổng hợp được xác định như sau:

Ai = Rotz (q) Transz (d) Transx (a) Rotx (α).

i-1

Ai = Rotz (Ө) Transz (d) Transx (a) Rotx (α).

=...



619

620

621

622

623



=



624

625

626



Bước 5: Xác định phương trình động học thuận cho tay máy.

Ma trận đồng nhất mơ tả hướng và vị trí của robot trong hệ tọa độ {O}.



627



0



Tn = 0A1.1A2.2A3…n-1An =



6292.2.1.



Tham số của các thanh nối và khớp



628



630



631Xác



định bộ thông số Denavit-Hartenberg(Bảng thơng số DH)



632



633 Từ



hình vẽ ta xác định được các thông số sau :



634



635

636



637



638



+ Độ dài pháp tuyến chung của khớp 1 và khớp 2 là l1

+ Góc chéo giữa hai trục khớp 1 và khớp 2 là α1.

SVTH:Bùi Anh Tiến-TĐH9a2



29



GVHD:TS. NGUYỄN CAO CƯỜNG

+



639



Đây là khớp quay cho nên khoảng cách đo dọc trục khớp động



640



1 từ đường vuông góc chung giữa trục khớp động 2 và khớp động 1 tới



641



đường vng góc chung giữa khớp động 1 và gốc robot là d 1=0.Tương tự ta



642



cũng có d2=0



643



+ Góc giữa 2 đường vng góc chung là θ1



644

645



Tương tự ta xét khớp 2 của robot ta có :



646

647



648

649



+ Ta có độ dài pháp tuyến chung là l2



650

651



+ Độ dài pháp tuyến chung của khớp 1 và khớp 2 là l1

+ Góc chéo giữa hai trục khớp 1 và khớp 2 là α2.



653



+ Góc giữa 2 đường vng góc chung là θ2



655



Vậy biến khớp là θ1 và θ2



652

654



656



6572.2.2. Thiết lập hệ tọa độ

658Hệ tọa độ cố định o0x0y0z0 có

659đến o1

660Hệ tọa độ cố định o1x1y1z1 có

661



gốc o0 đặt tại tâm trục khớp 1, chiều x0 hướng từ o0

gốc o1 đặt tại tâm trục khớp 2



662Hệ

663



tọa độ cố định o2x2y2z2 có gốc o2 đặt tại tâm trục khớp động 2



664Hệ

665



tọa độ cố định o2x2y2z2 có gốc o2 đặt tại tâm trục khớp động cuối khâu 2



666Ba



trục z0, z1,z2 vng góc với mặt phẳng của tờ giấy



6672.2.3. Xây dựng phương trình động học thuận cho robot

668Căn cứ vào các thông số và hệ tọa độ đã được thiết lập ta có

669robot như sau :



bảng thông số DH của



670



Khâu

Өi

li

αi

di

1

Ө1

l1

0

0

2

Ө2

l2

0

0

*

671 Quan hệ giữa hai khung tọa độ o1x1y1z1 và o0x0y0z0 được xác định như sau:

672+ Quay xung quanh trục z0 một góc θ1 sao cho trục x0 trùng với phương của trục x1

673

- Tịnh tiến dọc theo trục x1(phương pháp tuyến chung) một đọan bằng l1

674Ta có phép biến đổi tổng hợp như sau :

6750A1= Rot(z0,θ1 ).Trans(l1,0,0)

SVTH:Bùi Anh Tiến-TĐH9a2



30



GVHD:TS. NGUYỄN CAO CƯỜNG

676Trong

677



đó Rot(z0,θ1 ) là phép quay xung quanh trục z0 một góc θ1



678Rot(z0,θ1 )

679



=



680Trans(l1,0,0)

681



là phép tịnh tiến khung tọa độ o0x0y0z0 theo trục x0 một đoạn bằng l1



682Trans(l1,0,0)=

683

684Ta

685



có 0A1=



686=

687* Quan hệ giữa hai khung tọa độ o2x2y2z2 và o1x1y1z1 được xác định như sau:

688+ Quay xung quanh trục z1 một góc θ2 sao cho trục x1 trùng với phương của trục

689x12

690

- Tịnh tiến dọc theo trục x1(phương pháp tuyến chung) một đọan bằng l2

691Ta có phép biến đổi tổng hợp như sau :

6921A2= Rot(z1,θ2 ).Trans(l2,0,0)

693

694Trong đó Rot(z1,θ2 )

695Rot(z1,θ2 ) =

696

697Trans(l2,0,0)

698



là phép quay xung quanh trục z1 một góc θ2



là phép tịnh tiến khung tọa độ o1x1y1z1 theo trục x1 một đoạn bằng l2



699Trans(l2,0,0)=

700

701Ta

702



có 1A2=



703=

704

705Phương trình

7060A2=0A1.1A2

707

709

710Ta

711



động lực học thuận được xác định như sau:



nhân hàng với hàng cột với cột của hai ma trận từ đó xác định được ma trận sau:



7120A2=

713

714Mà



0



A2=



SVTH:Bùi Anh Tiến-TĐH9a2



31



GVHD:TS. NGUYỄN CAO CƯỜNG

7152.2.4

716nx=

717=

718=0

719

720

721

722

723=

724

725

726

727



Hệ phương trình động học thuận là :



0



7282.3. Xây dựng phương trình động học ngược cho robot

729 Phương trình động học ngược nhằm xác định biến khớp từ

730các tay robot mong muốn.

731Trường

732Có



các vị trí và hướng của



hợp tổng qt ta xét hệ phương trình động học của Robot có n bậc tự do.



thể viết lại như sau:



T



733



 T i. T n

i



n



734



Nhân 2 vế của (2.13) với T



735



Ai-1...A2-1A1-1 Tn =



(2.1)

1

i



ta có:

(2.2)



Kết hợp (2.12) ta có:



736



i



T



737



n



=Ai-1...A2-1A1-1



(2.3)



với i=1,...,n-1



738

739Ứng



với mỗi giá trị của i, khi so sánh các phần tử tương ứng của 2 ma trận ở biểu

740thức (2.3) ta có 6 phương trình tồn tại độc lập để xác định biến khớp qi.Đặc điểm:

- Khơng có phương pháp tổng qt.

- Nghiệm cũng khơng duy nhất.



741

742

743

744







Có 3 phương pháp cơ bản:



SVTH:Bùi Anh Tiến-TĐH9a2



32



GVHD:TS. NGUYỄN CAO CƯỜNG

-Phép đảo hướng.

-Phép đảo vị trí.

-Phép đảo kết hợp.



745

746

747

748







749

750

751



Tn = = A1.A2.A3…An.



752

753



-



754

755

756



Các bước thực hiện bài toán động học ngược:

- Từ phương trình động học thuận cho Robot.



Nhân trước A1-1 với phương trình, xác định được:

( A1 )-1. Tn = A2.A3…An.



757

758



˂=> . = 1Tn



759

760



˂=>



761

762

763

764



=>f(q1) = 1Tn = A2 … An

A2 … An = h(q2, q3, …, qn)

=>f(q1) = h(q2, q3, …, qn).



765

766

767

768



= 1Tn.



- Tìm ở h(q2, q3, …, qn) phần tử 0 hoặc là hằng số rồi cân bằng với phần tử

tương ứng ở f(q1) từ đó tính ra q1.

- Quay lại bước 1 nhân trước với A2-1 sẽ tính được q2.

- Tiếp tục sẽ tính được q3, …, qn.



769Tóm

770



lại:



771

772

773

774

775



A1-1 . Tn = 1Tn

A2-1 . A1-1 . Tn = 2Tn

A3-1 . A2-1 . A1-1 . Tn = 3Tn

An-1-1 … A3-1 . A2-1 . A1-1 . Tn = n-1Tn



776



 Xây dựng phương trình động học ngược cho bài tốn:



777



SVTH:Bùi Anh Tiến-TĐH9a2



33



GVHD:TS. NGUYỄN CAO CƯỜNG



778

779Ta thấy ma trận 0A2

780Mà 0A2=0A1.1A2=

781



đã biết ta cần xác định được



782 Nhân hai vế của phương trình

7831 ta có (0A1)-1.0A2=1A2

7840A1=

785Ta có A11 của (0A1)-1 có dạng:

786



trên với ma trận nghịch đảo của ma trận 0A1 là (0A1)-



787A11 =

788Nhân giá trị ta được A11=

789Tương tự ta sẽ tính các giá trị còn

790Từ đó ta có ma trận: (0A1)-1 =

791Nên (0A1)-1.0A2=

792Nhân hai ma trận với nhau ta có:

7931A2=

794

795Cân

796

797



lại trong (0A1)-1



bằng hai vế của hai ma trận tại cột thứ 4 ta có hệ phương trình sau:



798Với các

799sau:

800

(1)

801

802Bình



thơng số và và a1, a2 giải hệ phương trình ta có các biến khớp và như



phương hai vế của hệ và cộng vế với vế ta có:



SVTH:Bùi Anh Tiến-TĐH9a2



34



GVHD:TS. NGUYỄN CAO CƯỜNG

803

804+

805=

806



=(+



807Bình phương

808( + 809



vế (1) cộng vế với vế ta có:



(2)



810

811

812Chia

813



cả 2 vế của ta có:



814=

815

816Đặt

817

818Ta

819



= cos(y) và =sin(y) vì cos2y+sin2y=1



có phương trình đã cho có dạng:



820Cos(y).+sin(y).=

821

822Cos(y823

824=arcsin-arsin

825Vậy ta có:= và

826

827

828

829



=arcsin-arsin



 Nhận xét: Phương trình động học thuận giúp ta xác định được vị trí của

khâu cuối cùng và phương trình động học ngược giúp ta xác định được khớp

quay của các khớp.



830

8312.4 Xây dựng quan hệ tốc độ của các khớp và tốc độ của tay robot

832Để xây dựng quan hệ tốc độ các khớp và tốc độ của tay robot ta cần phải thiết lập

833phương trình động lực học cho cánh tay robot

834Gọi v là tốc độ chuyển động tịnh tiến của tâm vật thể, w là tốc độ góc, m là khối

835lượng vật thể,Jt là moment quán tính của vật thể

836Sử dụng phương pháp Lagrange để thiết lập phương trình động lực học cho robot

837Giả thiết khớp 1 sinh ra momen M1 tác dụng giữa bệ và thanh nối 1; khớp 2 sinh ra

838momen M2 tác dụng giữa thanh nối 1 và 2; trọng lực có hướng theo trục y

839Ký hiệu động năng và thế năng của thanh nối i là Ki và Pi (i=1,2)



SVTH:Bùi Anh Tiến-TĐH9a2



35



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

CHƯƠNG II: HỆ PHƯƠNG TRÌNH ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CỦA ROBOT CÔNG NGHIỆP

Tải bản đầy đủ ngay(0 tr)

×