hệ thống điều khiển robot scara

Ch­¬ng 1: tæng quan c¸c hÖ thèng ®iÒu khiÓn robot. I. Giíi thiÖu chung: Tõ gi÷a thËp kû 18, J.de Vancauson x©y dùng c¸c bupbe c¬ khÝ cã thÓ ch¬i nh¹c, ®©y ®­îc coi lµ s¶n phÈm ®Çu tiªn cña Tù ®éng ho¸ - Robot. C¸c thiÕt bÞ ®iÒu khiÓn cã thÓ ghi nhí tÝn hiÖu ®iÖn tõ vµ dïng l¹i víi sù trî gióp c¸c ho¹t ®éng c¬ khÝ cña m¸y mãc ®­îc cÊp chøng nhËn vµo nh÷ng n¨m 1950 t¹i Anh, Mü, §øc. N¨m 1968 Robot Shakey ra ®êi, ph¸t triÓn h¬n c¸c lo¹i Robot kh¸c lµ nã ®­îc trang bÞ c¸c c¶m biÕn va ch¹m, c¸c camera vµ nã cã thÓ di chuyÓn trªn sµn nhµ.

Chơng 1:

tổng quan các hệ thống điều khiển robot

I Giới thiệu chung:

Từ giữa thập kỷ 18, J.de Vancauson xây dựng cácbupbe cơ khí có thể chơi nhạc, đây đợc coi là sản phẩm

đầu tiên của Tự động hoá - Robot

Các thiết bị điều khiển có thể ghi nhớ tín hiệu điện

từ và dùng lại với sự trợ giúp các hoạt động cơ khí của máymóc đợc cấp chứng nhận vào những năm 1950 tại Anh, Mỹ,

Đức

Năm 1968 Robot Shakey ra đời, phát triển hơn các loạiRobot khác là nó đợc trang bị các cảm biến va chạm, cáccamera và nó có thể di chuyển trên sàn nhà

Robot dùng ngôn ngữ lập trình và máy tính ra đời năm

1973 Ngôn ngữ WAVE kết hợp với ngôn ngữ AL thành ngônngữ VAL, đợc phát triển mạnh vào thời gian sau đó bởiVictor Sheinman và Bruce Simano

Các Robot phục vụ công nghiệp cơ khí đợc phát triển

từ năm 1974, sau khi hãng Kawasaki nhận đợc giấy phép củaUnimation dùng Robot trong việc hàn hồ quang cho ô tô

Ngày nay, Robot đợc sử dụng rộng rãi và phát triểntrong các ngành kỹ thuật Dới sự điều khiển của máy tínhcho phép Robot có thể có nhiều ứng dụng hơn nữa

Robot công nghiệp đợc định nghĩa là một cơ cấu máy

có thể lập trình đợc, có khả năng làm việc một cách tự

động không cần sự trợ giúp của con ngời và giữa các taymáy có thể hợp tác đợc với nhau

Các Robot công nghiệp ngày nay thờng đợc đặt trên

đế và gắn chặt với sàn Cơ thể đợc gắn với để, tổ hợpcánh tay đợc nối với cơ thể Cuối cánh tay là cổ tay

Cổ tay gồm nhiều phần tử cho phép Robot định vị

đa dạng các vị trí Quan hệ chuyển động giữa các phần tửkhác nhau của cơ thể nh: cổ tay, cánh tay đợc thực hiệnqua một chuỗi các khớp nối Các chuyển động bao gồmchuyển động quay và chuyển động tịnh tiến

Về mặt cơ khí, Robot đợc cấu tạo từ những thanh nối

và khớp Các thanh nối đợc ghép với nhau bởi các khớp, chophép Robot có các chuyển động đa dạng

2 Các chuyển động của Robot:

Robot đợc thiết kế để thực hiện các nhiệm vụ khácnhau trong sản xuất Các công việc đợc thực hiện bởi khảnăng chuyển động của cơ thể, cánh tay, cổ tay của Robotqua một chuỗi các chuyển động và vị trí Cổ tay đợc sửdụng cho Robot thực hiện chính xác công việc Các chuyển

động của Robot đợc chia làm hai chuyển động cơ bản làchuyển động của cổ tay và chuyển động của toàn bộ cơthể Các chuyển động riêng lẻ đợc ghép nối và gắn chặt vớihai dạng chuyển động này và chúng đợc giới hạn bởi số bậc

tự do (deggrees of freedom) Các Robot thông thờng có 4

đầu ra có liên hệ với chuyển động đầu vào

Các khớp nối đợc sử dụng trong thiết kế Robot côngnghiệp điển hình là khớp tịnh tiến và khớp quay

+ Khớp tịnh tiến gồm cả chuyển động trợt và di

+ Có 3 loại khớp quay trong tay máy Robot:

Truyền động khí nén thờng sử dụng trong các Robot

có số bậc tự do nhỏ, yêu cầu độ tác động nhanh cao

4 Hệ thống nhận dạng:

Cảm biến đợc sử dụng nh thiết bị ngoại vi của Robot,gồm 2 loại đơn giản nh: công tắc hành trình và hệ thốngcamera Cảm biến cũng đợc dùng nh các phần tử tích hợpcủa hệ thống phản hồi vị trí

Các loại cảm biến thông dụng:

+ Cảm biến va chạm: là cảm biến phản ứng lại lực khi

+ Camera đợc sử dụng thực hiện việc kiểm tra, quansát.

Cảm biến là phần tử quan trọng trong hệ thống điềukhiển và giám sát an toàn

II Phân loại Robot công nghiệp:

Hình 1 1: Cấu hình Robot dạng Decac

1.2 Dạng hình trụ (hình 1 2):

Trục cơ bản là một trụ dọc, Robot chuyển động lên và

trục, tịnh tiến dọc trục phạm vi làm việc của Robot đợc mởrộng theo một hình trụ bao quanh trục cơ bản.

Hình 1 2: Cấu hình Robot dạng hình trụ

1.3 Dạng hình cầu (hình 1 3):

Sử dụng các khớp lồng vào nhau, giúp cho Robot có khảnăng chuyển động lên hoặc xuống theo chiều ngang củatrục quay Dạng đa khớp nối cho phép Robot mở rộngkhoảng không gian làm việc theo hình cầu

Hình 1 3: Cấu hình Robot dạng hình cầu

1.4 Dạng khớp nối (hình 1 4):

Tơng tự nh cánh tay con ngời, nó gồm hai phần tửthẳng tơng ứng với cánh tay và cẳng tay Các phần tử này

đợc ghép nối với nhau bởi hai khớp tơng ứng với khớp bả vai vàkhớp khuỷu tay Cổ tay đợc nối với cẳng tay.

Hình 1 4: Cấu hình Robot dạng khớp nối

2 Phân loại theo phơng pháp điều khiển:

2.1 Robot giới hạn chuyển động liên tục:

Robot giới hạn chuyển động liên tục không sử dụng hệ

điều khiển bám để di chuyển tới các vị trí lân cận trongcác khớp nối Chúng đợc giới hạn điều khiển bằng các côngtắc hành trình để dừng và thiết kế điểm cuối cho mỗichuyển động cho mỗi khớp nối

2.2 Robot lặp lại với điều khiển từ điểm tới điểm:

Robot lặp lại sử dụng hệ điều khiển với một chuỗi các

vị trí hoặc chuyển động đợc dạy cho Robot, ghi lại trong bộnhớ và đợc lặp lại dới sự điều khiển của chính nó Chia làmhai loại:

+ Lặp lại từ điểm tới điểm

+ Lặp lại liên tục

2.3 Robot điều khiển theo quỹ đạo liên tục:

Robot chuyển động với quỹ đạo đã đợc xác định trớc.Quỹ đạo chuyển động là một chuỗi các điểm mô tả trên đ-ờng chuyển động Tín hiệu đặt là vị trí mong muốn của

cổ tay, Robot chuyển động theo luật điều khiển xác địnhtrớc để đến đích

3 Phân loại theo số bậc tự do:

Mỗi bậc tự do tơng ứng với một chuyển động độc lậpcủa Robot Số bậc tự do là số vị trí cần thiết để xác địnhhoàn toàn cấu hình của Robot Thông thờng số bậc tự docủa Robot là số khớp nối từ thân đến cổ tay

III Mô hình toán học hệ thống truyền động:

Các khớp trong Robot đợc truyền động bởi hệ thốngtruyền động điện, thuỷ lực hoặc khí nén Mỗi hệ thốngtruyền động có u, nhợc điểm khác nhau.

Hệ thống truyền động điện sử dụng các động cơ

điện một chiều hoặc động cơ đồng bộ Sơ đồ cấu trúc

động học hệ thống điều khiển cho khớp nh ở hình 1 5

+ Động cơ điện một chiều nam châm vĩnh cửu

Mt

M

c = i Trong đó: Mt: là mô men khớp

i: tỷ số truyền

: hiệu suất bộ truyền

Mô hình toán học động cơ và hệ truyền động khớp:

Hình 1 6: Mô hình toán học động cơ

Trong đó: E(p) là sức điện động bộ biến đổi

B là hệ số nhớt

IV Tổng quan về các phơng pháp điều khiển:

Theo nhiệm vụ làm việc của Robot có thể chia làm 2bài toán điều khiển: bài toán điều khiển thô và bài toán

điều khiển tinh

Giai đoạn vận chuyển chi tiết trong quá trình lắp ráp,Robot chuyển động trong không gian theo một quỹ đạo nào

đó gọi là chuyển động tự do, không tiếp xúc với đối tợng.Trong giai đoạn này chỉ điều khiển quỹ đạo, gọi là điềukhiển thô, tay Robot chuyển động theo một quỹ đạo đặttrớc

Khi tay Robot tiếp xúc với đối tợng, Robot phải sinh ramột lực phù hợp để đa các chi tiết cần lắp ráp vào đúng vịtrí, gọi là điều khiển tinh, liên quan đến điều khiển lực,

đồng thời cũng vừa điều khiển quỹ đạo

Điều khiển quĩ đạo có thể thông qua điều khiển quĩ

đạo khớp hoặc điều khiển trực tiếp quĩ đạo tay

Điều khiển Robot

Điều khiển

thô

Điều khiển tinh

Hình 1 7: Phân loại các phơng pháp điều khiển Robot

1 Điều khiển quĩ đạo khớp:

Thiết kế quĩ đạo điều khiển cho từng khớp Chuyển

động của tay phụ thuộc vào chuyển động của từng khớpriệng biệt

Ưu điểm:

+ Điều khiển trực tiếp động cơ truyền động khớp vềtốc độ và mô men Có thể đảm bảo một số điều kiện giớihạn của hệ truyền động điện

+ Giải bài toán động học ngợc đơn giản

Nhợc điểm: do không trực tiếp điều khiển vị trí củatay nên khó đảm bảo đợc đờng di chuyển bám theo quỹ

đạo đặt trớc

2 Điều khiển quĩ đạo tay:

Thiết kế quĩ đạo điều khiển trực tiếp cho tay Robot,chuyển động của các khớp phụ thuộc vào chuyển động củatay

Ưu điểm:

+ Đảm bảo tay di chuyển đợc chính xác theo quỹ đạomong muốn của Robot

Nhợc điểm:

+ Bài toán động học ngợc có khối lợng tính toán lớn

+ Khó đảm bảo một số điều kiện nh: gia tốc, mô men

3 Điều khiển lực:

ở giai đoạn lắp ráp, Robot phải sinh ra lực phù hợp để

đa chi tiết vào vị trí êm, đồng thời vừa phải thực hiện

điều khiển quĩ đạo

Trở kháng là khái niệm đánh giá độ cứng của cơ cấu,chống lại lực tác dụng lên nó.

+ Điều khiển trở kháng thụ động: cơ cấu đàn hồi cótâm điều khiển từ xa Khi chi tiết đa đến vị trí lắp ráp,tay máy không cần sinh lực để đa vào Trục chi tiết và trụccủa lỗ không trùng nhau, do đó các lò xo trên tay máy bị uốn,

đẩy chi tiết sao cho trục của chi tiết và trục của lỗ trùngnhau, đồng thời sinh ra mô men quay quanh trục của lỗ,

đẩy chi tiết vào đúng vị trí cần lắp đặt

+ Điều khiển trở kháng tích cực: dùng các phản hồi lực

và vị trí để điều khiển tay máy lắp ráp chi tiết Mô phỏngRobot dới dạng vật tiếp xúc với đối tợng, lực sinh ra phụ thuộckhe hở và độ cứng của môi trờng Sau khi đo đợc lực, thiết

Tín hiệu đặt là góc quay hoặc mô men, tín hiệu ra là

vị trí của tay máy

a Luật điều khiển phản hồi:

Coi các bộ biến đổi và động cơ là khâu quán tính.Trong đó: đ là tín hiệu đặt góc quay, thờng làkhối đặt quỹ đạo

= đ - là sai số giữa tín hiệu vị trí đặt

Ke

đ

Hình 1 9: Sơ đồ điều khiển phản hồi

Trong trờng hợp đơn giản, ta dùng bộ biến đổi PD

Mđk = KP + KD Khớp thứ i:

b Luật điều khiển mô men tính toán:

Từ phơng trình động lực học, thiết kế luật điềukhiển để khử phi tuyến giữa các khớp

đk = Khi đó:

H( ) + h( , ) + g( ) = H( ) + h( , ) + g( ) (1.7)

Với Robot cụ thể ta có: H( )  0, nên

đk

Hình 1 11: Sơ đồ điều khiển mô men tính toán

Chọn luật điều khiển đảm bảo một số chỉ tiêu: quátrình quá độ và sai số tĩnh Thờng chọn bộ điều chỉnh U

là PID

= = đ + KP + KD + KI dt (1.9)

Với = đ - ta có:

+ đ + KP + KD + KI dt = 0 (1.11)

Khớp thứ i ta có:

i + đi + KPi i + KDi i + KIi i dt = 0 (1 11)Bằng phơng pháp mô hình hoá hoặc phơng pháp đặtnghiệm cực, ta xác định đợc các hệ số KP, KD, KI sao cho hệthống hội tụ, ổn định tức là các sai số i  0

Ưu điểm: khử đợc tính phi tuyến của hệ thống, biến

+ Chỉ thực hiện đợc khi biết đầy đủ các tham số củaRobot.

+ Luật điều khiển không thích nghi khi tham số biến

đổi

V ứng dụng của Robot:

1 Ưu điểm của Robot:

Robot có thể làm việc liên tục trong thời gian dài,chúng chỉ ngừng hoạt động khi cần duy tu, bảo dỡng, thaythế

Robot có khả năng làm việc trong môi trờng độc hại,khu vực nguy hiểm, hoặc những nơi con ngời không thể

đến đợc

Với chơng trình đợc đặt trớc, Robot có khả năng làmviệc với hiệu suất cao hơn con ngời, tiết kiệm nguyên vậtliệu , độ chính xác làm việc cao

Giá thành và chi phí lắp đặt, chế tạo Robot ngày càngthấp do sự tiến bộ của khoa học kỹ thuật

Khi thay đổi công việc, lập trình lại cho Robot nhanhhơn và chi phí thấp hơn so với việc đào tạo một côngnhân

2 ứng dụng của Robot:

+ Trong ngành công nghiệp cơ khí, Robot thực hiệnnhững công việc nặng nhọc nh nâng hạ, lắp ráp các chitiết lớn, cồng kềnh yêu cầu độ chính xác cao Sử dụng Robot

+ Trong sinh hoạt, Robot trợ giúp ngời già, phục vụ trongnhà bếp làm giảm sức lao động của con ngời.

Nh vậy cùng với sự phát triển của khoa học kỹ thuật,công nghệ tự động hoá, Robot ngày càng đợc hoàn thiện và

có nhiều ứng dụng trong công nghiệp cũng nh trong cuộcsống hàng ngày của con ngời

Chơng 2:

tính toán động học - động lực học robot serpent

I Khái niệm robot scara -serpent:

Robot SCARA (Selectively Compliant Articulated RobotArm) có nghĩa là có thể lựa chọn dễ dàng khớp nối của cánhtay Robot Chúng có một lợng nhỏ đàn hồi trong quá trìnhlàm việc

Do chuyển động của Robot SCARA đơn giản, dễ dàngnên nó đợc sử dụng khá phổ biến trong công nghiệp RobotSerpent là loại cơ bản trong nhóm Robot công nghiệp này.

Truyền động cho 2 khớp của tay máy và cổ tay bằng

động cơ servo một chiều có phản hồi vị trí tạo thành mộtvòng điều khiển kín Chuyển động thẳng đứng đợc thựchiện bằng pitton khí nén

Chiều cao của Robot có thể thay đổi dễ dàng bằngcách thay đổi vị trí gá thân Robot trên trục cơ bản, giúptay máy thuận lợi trong việc thay đổi công việc

Robot Serpent có thể đợc lập chơng trình từ máy tínhbằng cách đặt dữ liệu cho mỗi trục Hoặc điều khiển bằngtay sử dụng thiết bị lái điện (steering) cho tay máy dùng cáccuôn dây điện từ trong pendant

II khái niệm về động học vị trí robot:

Một tay máy có cấu tạo gồm một chuỗi các thanh nốicứng đợc liên kết với nhau bởi các khớp Mặt khác các thanhnối và các khớp đặt ở các vị trí khác nhau trong không gian

Động học vị trí sẽ giải quyết mối quan hệ vị trí của khớp vàtay

Ta đặt lên mỗi thanh nối một hệ trục toạ độ, sử dụngphép biến đổi đồng nhất để miêu tả mối quan hệ về vịtrí và hớng giữa các hệ toạ độ

Phép biến đổi đồng nhất mô tả quan hệ tịnh tiến vàquay giữa giữa hệ thống toạ độ một thanh nối và một thanhnối kế tiếp bằng một ma trận vuông A (4x4)

1 Các phép biến đổi đồng nhất:

Một vectơ = a + b + c đợc mô tả bằng một ma trậncột:

= Trong đó: a = x/w

b = y/w

c = z/w

w gọi là hệ số tỷ lệ

Một vectơ điểm nh trên đợc sử dụng để định nghĩa

vị trí cuối cùng của cánh tay trong tay máy Robot

a Phép biến đổi tịnh tiến:

Tịnh tiến một điểm lần lợt theo 3 trục x, y, z tơng ứngvới khoảng cách a, b, c:

H = Trans(a, b, c) =

ý nghĩa: H biểu diễn một khung toạ độ đặc biệt cócác vectơ đơn vị , , trùng hớng với , , trong hệ toạ độgốc và gốc toạ độ có vị trí biểu diễn bằng vectơ: T.

b Phép biến đổi quay:

Quay một vectơ theo 3 trục với 1 góc  đợc thực hiệnbởi phép quay:

+ Quay quanh trục x một góc :

ý nghĩa của phép biến đổi:

+ Phép biến đổi H biểu diễn bằng một ma trận 4 x 4,biểu diễn một hệ toạ độ mới so với hệ toạ độ gốc, trong đó

ba cột đầu tiên của ma trận H biểu diễn hớng của hệ toạ độmới so với hệ toạ độ gốc và cột thứ t biểu diễn vị trí gốc toạ

độ

c Phép quay xung quanh một trục bất kỳ:

Phép quay xung quanh vectơ K một góc :

H = Rot (K, )Vectơ K có điểm gốc trùng với gốc tọa độ và có cácthành phần trên 3 trục là kx, ky, kz:

= kx + ky + kz

Rot (K, ) =

Trong đó: C biểu dễn cho Cos

S biểu diễn cho Sin

V = biểu diễn cho Ver = 1 - Cos

2 Biểu diễn tay Robot:

Hệ toạ độ tay Robot đợc biểu diễn bằng một ma trận 4

T =

Có một số phép biểu diễn hớng của tay Robot nh:

+ Phép biểu diễn Euler

+ Phép biến đổi Roll - Pitch - Yaw

2 1 Phép biểu diễn Euler:

Phép biểu diễn Euler là tổ hợp của ba phép quay:

+ Quay một góc  quanh trục z

+ Quay một góc  quanh trục y

+ Quay một góc  quanh trục z (mới)

Euler (, ,  = Rot (z, ) Rot (y, ) Rot (z, )

=

=

2 2 Phép biến đổi Roll, Pitch, Yaw:

Phép biến đổi Roll, Pitch, Yaw là tổ hợp của 3 phépquay:

+ Quay quanh trục x một góc 

+ Quay quanh trục y một góc 

+ Quay quanh trục z một góc 

RPY = Rot (z, ) Rot (y, ) Rot (x, )

=

3 Động học vị trí:

Động học vị trí Robot gồm 2 bài toán:

Động học thuận: cho phép tính đợc vị trí và hớng của

tay Robot từ vị trí và hớng của các khớp Robot Có hai phơngpháp giải bài toán động học thuận là:

+ Dùng các phép biến đổi

+ Gán hệ toạ độ cho các thanh nối

Động học ngợc: giải bài toán khi biết vị trí tay của

Robot, tính toán vị trí các khớp để điều khiển các khớpgiúp tay chuyển động theo quỹ đạo mong muốn

4 Động học thuận Robot:

Để xây dựng động học Robot, một cách tổng quát gáncho mỗi thanh nối một hệ trục toạ độ và tìm quan hệ giữa

2 hệ toạ độ 2 thanh nối liền nhau Từ đó có thể xác định

đợc quan hệ vị trí của tay Robot với các khớp

Xét 2 khớp liền nhau và 2 thanh nối liền nhau (n và n +1)nh hình 2 2

+ Thanh nối n liên kết giữa khớp n và n + 1

+ Hai khớp có trục chéo nhau trong không gian

an là độ dài pháp tuyến chung của 2 trục khớp n và n + 1

dn là khoảng cách của 2 chân pháp tuyến chung trên trục n

n là góc giữa 2 pháp tuyến chung n và n - 1

n là góc giữa 2 trục n và n + 1

Qui tắc đặt hệ toạ độ cho thanh n:

+ Gốc toạ độ đặt trên trục n + 1 và trùng với chânpháp tuyến chung giữa trục n và n + 1.

+ Trục zn trùng với trục n + 1

+ Trục xn trùng với hớng của an, đi ra từ trục n + 1

Hình 2 2: Đặt hệ toạ độ cho thanh nối Robot

Hệ toạ độ n + 1 nhận đợc từ hệ toạ độ n bằng 4 phépbiến đổi:

+ Quay quanh zn-1 góc n sao cho trục xn-1 trùng hớng vớitrục xn

+ Tịnh tiến gốc On-1 theo trục zn-1 đoạn dn

+ Tịnh tiến theo trục x đoạn an

+ Quay quanh trục x góc n sao cho trục zn-1 trùng hớngvới trục n - 1

Gọi phép biến đổi giữa n và n - 1 là An:

An = Rot (z, n) Trans (0, 0, dn) Trans (an, 0, 0) Rot(x, n)

III xây dựng động học vị trí của robot serpent:

Robot Serpent gồm 3 khớp chuyển động quay và mộtkhớp chuyển động tịnh tiến

+ Khớp 1 quay quanh trục Z0 góc 1

+ Khớp 2 quay quanh trục Z1 góc 2

+ Khớp 3 chuyển động tịnh tiến theo trục Z2 đoạn d3.+ Khớp 4 quay quanh trục Z3 góc 4

Hình 2 3: Cấu hình Robot SerpentTheo cấu hình Robot ta có bảng tham số:

Bảng 2 1: Tham số động học

Robot Serpent

1234

00

a1

a2

00

00

d3

0Theo Denavit Hartenberg: toạ độ tay của Robot đợc xác

IV xây dựng phơng trình động học ngợc robot serpent: Động học ngợc giải quyết bài toán xác định các biến khớp khi biết vị trí tay Robot.

= a1 Cos1 + a2 Cos1 Cos2 - a2 Sin1 Sin2

= (a1 + a2 Cos2) Cos1 - a2 Sin1 Sin2

Ph¬ng tr×nh (2 11):

py = a1 Sin1 + a2 Sin(1 + 2) =

= a1 Sin1 + a2 Sin1 Cos2 + a2 Cos1 Sin2

= (a1 + a2 Cos2) Sin1 + a2 Sin2 Cos1

ThÕ ph¬ng tr×nh (2 14) vµo ta cã:

py = (a1 + a2 Cos2) Sin1 + a2 Sin2

(a1 + a2 Cos2).py = (a1 + a2 Cos2)2 Sin1 + a2 Sin2(px +

a2 Sin1 Sin2)

(a1 + a2 Cos2).py - a2 Sin2 px = [(a1 + a2 Cos2)2 + a22.Sin22)] Sin1

hệ giữa mô men của khớp này với gia tốc của khớp khác.

V.1 Hàm Lagrange và các vấn đề động lực học:

Lagrange định nghĩa sự khác biệt giữa động năng vàthế năng của hệ thống

L = K - PTrong đó:

+ K: là tổng động năng của hệ thống

+ P là tổng thế năng của hệ thống

Do đó phơng trình động lực học đợc xác định bằngbiểu thức:

Fi =

Trong đó: qi là biến khớp

qi = i đối với khớp quay

qi = ri đối với khớp tịnh tiến

F biểu diễn lực hoặc mô men

Cấu hình Robot Serpent:

Hình 2 4: Cấu hình động lực học Robot Serpent

+ J2.( + )2

P2 = 0

Khíp 3:

v3 =

lg2 = l2

lg3 = l3K= [(m1+m2+m3+m4).l12+(m2+m3+m4).l22+J1+J2+J4] + + (m2+m3+m4)l1.l2.Cos2 +

1 Khíp 1:

M1 =

a = [(m1+m2+m3+m4)l12+(m2+m3+m4)l22+J1+J2+J4] +

+2(m2+m3+m4)l1.l2.Cos2 ++[(m2+m3+m4)l22+J2+J4] ++(m2+m3+m4)l1.l2.Cos2 +J4

b = [(m1+m2+m3+m4)l12+(m2+m3+m4)l22+J1+J2+J4]

+

+2(m2+m3+m4)l1.l2.Cos2 2(m2+m3+m4)l1.l2.Sin2 +

+[(m2+m3+m4)l22+J2+J4] +

+(m2+m3+m4)l1.l2.Cos2 (m2+m3+m4)l1.l2.Sin2 +

b = [(m2+m3+m4)l22+J2+J4] +

(m2+m3+m4)l1.l2.Cos2

(m2+m3+m4)l1.l2.Sin2 ++ [(m2+m3+m4)l22+J2+J4] + J4

c = (m2+m3+m4)l1.l2(-Sin2) + (m2+m3+m4)l1.l2Sin2)

M2 = [(m2+m3+m4)l22+J2+J4] + (m2+m3+m4)l1.l2.Cos2 + [(m2+m3+m4)l22+J2+J4] + J4 +