Luận văn thạc sĩ Kỹ thuật cơ điện tử: Thiết kế robot hỗ trợ bệnh nhân trong vệ sinh cá nhân

Chương | — Tong quan về robot hỗ trợ bệnh nhânCHUONG 1 TONG QUAN VE ROBOT HO TRO BENH NHAN Trong chương này tac giả sẽ giới thiệu sơ lược về robot trong lĩnhuc y tế, tình hình nghiên cứu

Giới thiệu chung về robot hỗ trợ bệnh nhân - ¿2-2-2 2 +E£E££+E£E+EzEzEzEeree: 2

Với những khả năng ưu việt và tiềm năng sử dụng to lớn, robot đã và đang dần được sử dụng trong nhiều hoạt động của con người thay thé sức lao động tay chân, thay con người làm những việc nặng nhọc và mạo hiểm Thậm chí trong lĩnh vực y té, tam ảnh hưởng của robot ngày càng quan trọng Đặc biệt trong những năm gan đây, có nhiều nghiên cứu robot trong lĩnh vực y tế.

Với sự phát triển không ngừng của xã hội thì đi kèm với nó là ngày càng nhiều căn bệnh phát sinh, cùng với môi trường đang bị hủy hoại thì sức khỏe con người đang hàng ngày phải đối mặt với vô vàn những tác nhân gây hại cho cơ thé Nhiều căn bệnh để lại di chứng làm bệnh nhân mat khả năng vận động như bại liệt, tai biến mạch máu não.Tại việt nam, có đến hơn 30% bệnh nhân bị tai bién mach máu não và sau đó bị di chứng là mat kha năng vận động Ngoài ra, theo số liệu từ [8] thì trong năm 2013, ở nước ta có khoảng 29.385 vụ tai nạn giao thông làm hơn 39.000 người chết, đồng thời làm mat kha năng vận động của hơn 25.000 người Với số lượng y bác sĩ có han, và không thé đáp ứng đủ nhu cau của số lượng bệnh nhân ngày càng gia tăng Đồng thời, người già yêu và các bệnh khác khuyết tật ở chân, cũng chiếm một số lượng không nhỏ Hỗ trợ và cải thiện hoạt động của người tàn tật nhận được sự quan tâm trên toàn thế giới Đặc biệt, tại các nước phát triển, van đề này được lưu ý hang dau Có nhiều loại thiết bị chuyên dụng và robot hỗ trợ người tàn tật ra đời như: thiết bị hỗ trợ tập đi bộ: thiết bị trợ lực và hỗ trợ đi bộ (Hình 1.1);

Robot hỗ trợ di chuyển và leo cầu thang (Hình 1.2)

Hình 1.2 Xe lăn hỗ trợ leo cầu thang Observer Maximus [5]

Một van dé được quan tam rat nhiều đó là trợ giúp bệnh nhân trong vệ sinh cá nhân, đặc biệt là việc đi toilet Có hai phương pháp rất thông dụng là sử dụng xe lăn và sử dụng toilet thiết kế đặc biệt (Hình 1.4) Thông thường, để hỗ trợ người tàn tật di chuyên lên xuống xe lăn thì cần phải có người hỗ trợ như hình 1.3 Nhược điểm của giải pháp này là người hỗ trợ phải có sức khỏe tốt để ỉ1Úỳp người tàn tat di chuyờn Đặc biệt, trường hợp khi người tàn tật đi vệ sinh cỏ nhân sẽ gây ra cảm giác không thoải mái cho cả người hỗ trợ và người được hỗ trợ về mặt tâm lý lẫn sức khỏe Vì vậy, các nhà sản xuất đã đưa ra nhiều sản phẩm hỗ trợ người tàn tật di chuyển lên xuống xe lăn Có thé tìm thấy khá nhiều sản phẩm từ rất đơn giản (hình 1.7) đến các hệ thống được hỗ trợ bởi máy móc như hình 1.8

Chương | — Tong quan về robot hỗ trợ bệnh nhân

Xe lăn tay với ghế vệ sinh :

Hình 1.5 Xe lăn có bô Helen keller V2 [5]

Ghé hỗ trợ đơn giản có người hỗ trợ :

Hình 1.6 Ghế hỗ trợ trong nhà vệ sinh [5]

Xe day với sự trợ giúp của người di chuyển lên xuống giường hoặc di vệ sinh

Hình 1.7 Xe đây của công ty Yanko [5]

Hình 1.8 Thiết bi nâng bệnh nhân số hiệu 1405C16 của công ty CASPI [5]

Tình hình nghiên cứu ứng dụng robot hỗ trợ y tế trên thế giới

Vào năm 2011, Toyota đã ra mắt "Independent Walk Assist" - một bộ khung xương ngoài nặng 3,5kg, được gắn vào chân để giúp người già hoặc người khuyết tật đi lại dễ dàng hơn Thiết bị này được trang bị các linh kiện điện tử chuyên dụng, có khả năng dự đoán hướng di chuyển và hỗ trợ lực cho bệnh nhân Nó thực hiện bằng cách nâng và vung đôi chân của người đi về phía trước, đồng thời được tích hợp cảm biến để nhận biết và điều khiển chuyển động của bàn chân và đùi Nhờ bộ robot này, bệnh nhân có thể thực hiện hầu hết các hoạt động thường ngày như đi bộ, leo cầu thang, đứng lên, ngồi xuống hay ngồi xổm.

TOYQTA £ đôi, xuông dôc, sử dung toilet, xe lăn

Hình 1.9 Robot Independent Walk Assist [1]

Phiên ban nâng cap “Walk Training Assist” còn cho phép phục hỏi lại chức năng di chuyên tự nhiên cho những người bị tai biến mach máu não , đột quy hoặc chân thương và sử dụng cơ cau giữ thăng bằng, tương tự như cơ cấu lắp trong xe tự hành Segway Ngoài ra, robot còn giúp giảm tai trọng tác dụng lên dau gôi và căng chân bệnh nhân.

Robot Balance Training Assistance là thiết bị hỗ trợ người bệnh tập thăng bằng thông qua tương tác với trò chơi điện tử Hệ thống bao gồm một chân đế có 2 bánh xe để người bệnh đứng, một tay cầm để họ nắm và một màn hình lớn hiển thị các trò chơi như đá bóng, tennis và bóng rổ Người bệnh cần phải di chuyển cơ thể theo các nhân vật trong trò chơi để luyện tập giữ thăng bằng.

Patient Transfer Assist, con robot nay kha to, cao 0,9 mét va nang dén 140 kg Nó được trang bị 2 cánh tay robot với tính chính xác cao, có chức nang nâng đỡ người bệnh ra khỏi giường va đặt họ vào xe lăn hoặc các thiết bị tương tự Đây là thiết bị bán tự động, vẫn cần sự hỗ trợ của y ta 4

Năm 2006, viện công nghệ NIST nghiện cứu va phát triển xe lăn HLPR giúp bệnh nhân di chuyển tự do và nâng hạ bệnh nhân lên giường, đến toilet hay lên ghế bất kỳ Với cơ câu ghế ngôi gắn vào khung chữ L giúp bệnh nhân có thể xoay 180 quanh trục của khung chữ L mà xe không cân di chuyển Ngoài ra hệ thống nâng hạ có thé đưa bệnh nhân lên đến cao độ 125cm tiện lợi cho nhu cầu với tới nơi cần thiết của bệnh nhân (như với lây sách trên kệ sách cao).

Tình hình nghiên cứu ứng dụng robot hỗ trợ y tế trên trong nước

Với xu hướng phát triển robot hỗ trợ cho y tế, thì Việt Nam cũng không năm ngoài luồng xu hướng đó Có nhiều nghiên cứu và sản phẩm dé phục vu cho nhu cau di chuyển của bệnh nhân khuyết tật, nhưng vì còn hạn chế về kinh phí và kỹ thuật nên robot có thé vẫn chưa được đẹp và khả năng sản xuất phục vụ xã hội vẫn còn bỏ ngõ Năm 2013, PGS.TS Huỳnh Thái Hoàng giới thiệu mẫu xe lăn điện điều khién băng mắt, mục đích giúp đỡ người già, người bệnh di chuyển dễ dang Xe được trang bi cảm biến encoder, la bàn điện tử và cảm biến siêu âm dé định vị và phát hiện vật cản Thông tin từ cảm biến sẽ được xử lý kết hợp với hướng nhìn của mắt để giúp người điều khiển xe lăn băng mat dé dàng Kỹ thuật sử lý ảnh được dùng để tách ra vùng chứa ảnh mat người Xe được truyền động băng hai động cơ DC gắn vào hai bánh sau, vận tốc động cơ được điều khiển bang giải thuật PID.

Hình 1.13 Xe lăn điều khiển băng mat

Cũng trong năm 2013, Nhóm nghiên cứu của GS.TS Võ Văn Tới, bộ môn kỹ thuật y sinh thuộc Trường đại học quốc tế (Đại học quốc gia TP.HCM) đã nghiên cứu chế tạo thành công xe lăn điện thông minh Chiếc xe này rất hữu dụng cho người tàn tật bị liệt (hoặc không có) cả tay và chân Xe có thê di chuyển tự động hoàn toàn nhờ có một camera nhận diện các vật cản, hình ảnh được truyền về hệ thống máy tinh dé xử lý và điều khiển xe lăn tự động di chuyên tránh các

Chương | — Tong quan về robot hỗ trợ bệnh nhân vật cản, nhưng vẫn đi đúng hướng đã định trước Đối với người tàn tật có thể cử động được đầu, nhóm nghiên cứu thiết kế thêm một chiếc nón thông minh, có trang bị hệ thống cảm biến dựa trên gia tốc Người sử dụng đội nón này và các cử động của đâu (nghiêng phải, trái, trước, sau) sẽ được dùng để điều khiến xe lăn chạy theo hướng tương ứng Hệ thống máy tinh sẽ nhận được các tín hiệu điều khiến từ đầu dé xử lý và ra lệnh cho cơ cầu chấp hành điều khiển các bánh xe di chuyên.

Hình 1.14 Xe lăn điều khiển bằng cử động của dau [5]

Sự ra đời của robot hỗ trợ bệnh nhân đã mở ra một hướng mới trong lĩnh vực y tẾ, giảm bớt gánh nặng cho các y bác sĩ Đặc biệt là robot hỗ trợ di chuyển cho bệnh nhân bị bại liệt, luận văn này sẽ hướng tới việc tự động di chuyển bệnh nhõn đến toilet và đưa bệnh nhõn trở lại ỉlường bệnh theo yêu cau của bệnh nhân.

Đối tượng luận văn hướng tới eee ccc cece cscs + 25s 2E2E£EE2E2EEEE 21211 21211 xe 9

Ta phân người tàn tật thành 3 nhóm như sau:

- _ Nhóm 1: người có thể tự đi được một ít mà không cần sự trợ giúp của bat cứ thứ gi - - Nhóm 2: người không thể tự đi được nếu không có sự trợ giúp của người hoặc thiết bi vin - Nhom 3: người bệnh năm liệt giường không thé đi được, cả tay và chân đều không thé cử động.

Robot sẽ hỗ trợ cho bệnh nhân thuộc nhóm 2, tức là người tàn tật nhưng có thể đứng được như hình 1.14 Robot sé di chuyén bệnh nhân đến toilet và đặt bệnh nhân lên bồn vệ sinh theo sự điều khiển của bệnh nhân không cần sự trợ giúp từ người ngoài.

Hình 1.14 Đối tượng sử dụng trong luận văn

Luận văn này tác giả tập trung giải quyết bài toán dùng robot di chuyển bệnh nhân từ giường bệnh đến toilet và từ toilet trở về giường bệnh Câu hình môi trường hoạt động của robot được cho như hình 1.15 với các dữ liệu đầu vào môi trường hoạt động của robot là môi trường indoor với diện tích phòng 4m x 4m ,yêu cầu về phòng vệ sinh để cửa hở và rộng ít nhất 1m để robot đi vào dễ dàng và không có bậc ngăn phòng (nghĩa là mặt sàn tại phòng vệ sinh và phòng ngoai là như nhau) Đồng thời mặt sàn đảm bảo độ bám nhất định để chống trượt.

Mô tả hoạt động của robot : như hình 1.15 robot sẽ được đặt tai vi trí sạc ở góc phòng, khi có tín hiệu từ người sử dụng robot sẽ bám theo đường màu đỏ tới đón bệnh nhân tại giường rồi tiếp tục đi đến toilet Khi có tín hiệu trở về, robot sẽ đi ngược lại quỹ đạo ban nay (hay đường màu xanh) để trả người bệnh lại tường và robot trở về vị trí sạc để tiếp tục chờ sự điều khiển của người bệnh Đây là một bài toán của robot di động bám theo quỹ đạo định san Dé robot di động có thé di chuyển được theo mong muốn thì robot phải thực hiện các tác vụ như sau :

- Mapping : nghĩa là lập bản đồ môi trường làm việc của robot, nêu được cung cấp dữ liệu trước thì robot không cần khả năng lập bản đô.

- Positioning : là khả năng định vi, robot phải biết được vị trí và hướng của nó trong hệ tọa độ quy định.

- Path planning : là hoạch định đường đi khi robot đã biết vị trí và môi trường làm việc

- Motion control: điều khiển các cơ câu dé robot đi theo quỹ đạo đã hoạch định.

Hình 1.15 Môi trường hoạt động cua robot

Mục tiêu, nhiệm vụ và giới hạn luận văn - 55 S222 *++sssserssssess2 12 Chương 2 Mô hình hóa động học & động lực hỌC G G55 5555555555 99556 558895 13

Nghiên cứu, thiết kế, mô phỏng và thực hiện bộ điều khiển cho robot trong việc di chuyên bệnh nhân đên toilet thỏa mãn yêu câu bài toán.

Chương | — Tong quan về robot hỗ trợ bệnh nhân e Nhiém vụ luận văn

Luận văn này tập trung vào mô phỏng phương án điều khiến, lựa chọn giải thuật thích hợp, và nghiên cứu phương pháp định vị và sửa chữa sai số của hệ thống Sau đó, lập trình cho robot hoạt động theo giải thuật đã chọn.

- Luận văn tập trung vào phần điều khiến robot tự động đưa bệnh nhân đến toilet - Tải trọng tối đa của bệnh nhân là 90kg

- Vận tốc tối đa của robot là lm/s.

- Sai lệch vi trí cho phép là +5cm.

Kêt thúc chương I, ta đã có cải nhìn cơ ban về robot hô trợ bệnh nhân, và sẽ đi sâu hơn về robot ở các chương sau

Chương 2 — Mô hình hóa động học và động lực học

MO HÌNH HÓA ĐỘNG HỌC & DONG LỰC HỌC Ở chương này tác giả sẽ mô hình hóa động học robot và mô hình hóa động lực học dé tiễn hành điều khiển ở chương kê tiếp

Bản chất của robot hỗ trỡ bệnh nhân chính là một mobile robot Và ta sẽ tập trung vào hệ mobile robot với sơ đồ nguyên lý như hình sau :

Hình 2.1 Sơ đồ nguyên lý của hệ mobile robot (đơn vị m)

Ta sử dụng hai hệ trục toa độ để xác định vị trí của robot dùng dé mô hình hóa và điều khiến: e Hệ tọa độ gan cô định vào robot {Xp Yr}. e Hệ tọa độ quán tinh {Xị,Y1} có định và robot sẽ di chuyển trong hệ tọa độ này.

Hình 2.2 Hệ trục tọa độ của robot

VỊ trí của robot trong hệ tọa độ robot và hệ tọa độ quán tính lần lượt là: dr — (Xr Yr Op)’, qr = (X11 6]?

Ta có mối quan hệ giữa qạ và q¡ như sau:

2.2 Mô hình động học Các ký hiệu:

A - giao điểm của đường nỗi tâm hai bánh xe chủ động và trục tọa độ OX

C — tọa độ trọng tâm của robot a — là khoảng AC.

L— khoảng cách từ bánh chủ động đến A

Ra — bán kính bánh chu động

Góc quay bánh phải và trái (Or, Or) thể hiện sự chênh lệch góc quay giữa bánh phải và bánh trái Vận tốc robot trong hệ tọa độ quán tính (v) là vận tốc thực tế của robot trong không gian Vận tốc góc của robot trong hệ tọa độ quán tính và hệ tọa độ robot (œ) cho biết sự thay đổi hướng của robot theo thời gian.

Hình 2.3 mô hình robot và các ký hiệu

Vận tốc tịnh tiễn của robot trong hệ tọa độ quán tính :

Vận tốc góc của robot:

Tu (2.1) ta suy ra: ọĂ = R~*(0) qr (2.5)

Phuong trình động học thuận cua robot:

D2 + Ra at cosd x cosỉ —sind 0[p, PR 0 PL on to q,=|¥|=|sin@ cosé@ 0 5 lon -@,| = Rạ——— sind (2.6)

2.3 Mô hình động lực học[10]

Khi bánh xe lăn không trượt, điểm tiếp xúc giữa bánh xe và mặt đất tức thời đứng yên Do đó, vận tốc của điểm tiếp xúc phải bằng không Yêu cầu này dẫn đến hai điều kiện ràng buộc:- Vận tốc của điểm tâm O phải cùng hướng với trục đối xứng của xe.- Tốc độ góc của bánh xe phải bằng tốc độ tuyến tính của điểm tiếp xúc với mặt đất.

—sin@ cos@ -=a 0 0 A(q)=|cos@ sin L —R, 0 (2.11) cos@ sin@ —-L 0 -R,

Ta có: re = = xa + acos8 os JẤc = ta — aệsin0 (243)

Ye =4 + asin8 ~ ở, = ÿa + aÔcos0 Động năng của robot:

Với : Ia=lc + ma” mx2 myé , ; , Ic6?

Phương trình Lagrange (với thé năng băng không) đối với hệ non holonomic như sau:

Với : qi — toa độ Lagrange F, — lực suy rộng

— nhân tử Lagrange Tính toán: d /0T

—( - = mx + 1na8sin8 + ma*cosé (2.17) dt \ử*,

=()= ỷ 6cos6 + maé*sin@ (2.18) dt \ay,) ~ my, — rna8cos8 + ma@*sin

PP (=) = 1-6 + 2ma?0 + max,sin@ + mna#c0cos8 — mayccos@ + may sind

Thế các phương trình trên vào (2.16): mic + magsind + mmaÔ2cos60 = F„ + Œ¡ (2.19) myc — rnaệcos9 + ma6?sin@ = F, + Cy (2.20) (Ie + 2ma?)ệ + maxcsind — mayccos@ = r + Ca (2.21)

F,, Fy — lần lượt là lực tác động theo phương x và y

+— là momen tác động lên robot

C¡, Co, C3 — lần lượt là các ràng buột theo phương x, y, 0 Chuyển sang dang ma trận: m 0 masin@ ma62?cos0 F Cy 0 m —macos@ | + |mmaô2sin6 | = |F,| + | C2 (2.22) masin@ —macos@ I, + 2ma? 0 T C3 vs Fy =(Fp + F, )sin8

Tu cac phuong trinh trén : rcos(Trp + TL)

E Ra 0 0x sinO(tp +T 4 |C05Ư COST) tr

FE.| = e+ ti) _ + sind sn | * (2.23) y R R Tụ T a a1 | —L

—n(,sin8 + Keo = feo cos8 | [—m(y, sind +X ccos6)0]Ì = ma(y-sin@ + #„cos8)Ô

= AT(q)A (2.24) Viết lại phương trình động lực học: m 0 masin@ ] ma62?cos0

0 m —?nacos8 | q+ mad? sin masin@ —macos@ I, + 2ma’ 0

1 [cos@ cos6] _, m(y,sind + #¿cos8)Ôsin8

=m sin9 sin8 Bì + |—rn(„sin8 + #„cos8)8cos8 (2.25)

“LoL TL ma(y-sin@ + #„cos6)Ô

M(q) = | 0 m “acoso masin@d —macos@ lẹ + 2ma? rand =[0 4 0 maôsinổ

1 cos@ cos8P sin@ sindTL L —L

—a Â = —-m(,sin8 + #„cos8)8 Để đơn giản hóa phương trình động lực học ta sẽ tim cách triệt tiêu nhân tử lagrange Ta có: cos@ —asin8 v

STM(q)A"(q) = 0 (2.28) Thế (2.27) vào (2.26) rồi nhân với ma trận SỶ(q):

Vina @ = STM(q)M(q)S(q) + ST(q)W„(q, YS(q) = Log |

Kết thúc chương 2, ta đã có phương trình động hoc va động lực hoc của robot, ta sẽ thiết kế bộ điều khiển bám quỹ đạo cho robot dựa vào hai phương trình này.

Chương 3 Thiết kế bộ điều khiến

THIET KE BO DIEU KHIEN

Từ phương trình động học va động lực học tìm được ở chương trước, trong chương nay ta sẽ thiết kế bộ điều khiến động học và động lực học cho robot bám theo quỹ đạo mong muốn Sau đó tiến hành mô phỏng với hai bộ điều khiển đề xuât.

Chương 3 Thiết kế bộ điều khiến Để điều khiển robot đi theo quỹ đạo như mong muốn đã có nhiều nghiên cứu dựa trên mô hình động học bỏ qua yêu tổ thuộc về động lực học như khối lượng, momen quán tính Để giải quyết van dé này ta sẽ phối hợp mô hình động học và động lực học.

Trong thiết kế quỹ đạo, tính liên tục là điều kiện tiên quyết để tránh gây ra rung động và va chạm đột ngột Để đạt được điều này, quỹ đạo phải đảm bảo tính liên tục của vị trí, vận tốc và trong một số trường hợp, cả gia tốc Bằng cách này, các chuyển động sẽ diễn ra mượt mà và nhẹ nhàng, mang lại trải nghiệm tốt hơn cho người dùng.

Thiết kế quỹ đạo với đường cong bậc ba Quỹ đạo là hàm bậc ba theo thời gian q(t) với q(to)=qs và q(ti)=qr Với tị — to là thời gian hoàn thành quỹ đạo Quỹ đạo bậc 3 có dạng: q(t) = day + ayt + a;t? + astŸ (3.1) Vận tốc mong muốn: g(t) = a, + 2a;t + 3a3t? (3.2) Từ (3.1) và (3.2) ta tinh được 4 thông số của q(t):

‘lt tổ - tổ |ra qo

Vi trí bắt đầu va kết thúc của robot là qo va gr Dé tiến hành mô phỏng ta giả định vo =vr= 0.

Dạng đường đi có thé là bat kỳ ham nào: đường thang, đường tròn va sẽ được định nghĩa bang hàm liên hệ giữa tọa độ x và y của robot Khối tạo quỹ đạo được mô phỏng trong Matlab

Hình 3.1 Khối tao quỹ đạo mô phỏng Matlab Simulink 3.2 Thiết kế bộ điều khiến

3.2.1 Bộ điều khiến động học|S]

VỊ trí và hướng mong muốn robot đạt được là Gr= [Xr Vi 0,]' , vị trí va hướng hiện tai cua robot là q=[x y 0] Ta định nghĩa sai số bám ep: cos@ sind O][*r—* iy = “sine cone O} |¥r — ¥

1 6; — 8 Luật điều khiển hỏi tiếp được đề xuất [8]:

Yo = wn 7 lo, + Kyeyv, + Kgv,sin(ég)

Với Kx, Ky, Ko là các hang số dương.

Luật điều khiển được chứng minh ồn định bằng phương pháp lyapunov.

Chon hàm lyapunov như sau:

Ta thay V là hàm không âm và V= 0 khi e,=0.

= |(o; + v, (Kyey + Kg sin(eg))) ey — K,e,| Cx + |- (a, + v, (Kye, + Ke sin(e9))) ey + 1y sin(¿)| ey

+ |—#(Kyey + Kg sin(eg) )|sin(eg) _ Ke 2- v,Kgsin* (eg)

Dao ham cua hàm V là một ham xác định âm do đó hệ thông sé ôn định nêu vận tôc robot thỏa mãn luật điêu khiên trên, sai s6 e, sẽ hội tụ vê 0 khi t tiên vê vô cùng.

3.2.2 Bộ điêu khiên động lực học

Từ bộ điều khiển động học ta đã có vận tốc v.(t) cần thiết của robot dé lái hệ thống theo quỹ đạo mong muốn O phan này ta cần chuyên tín hiệu v.(t) sang dang momen r để đưa vào hệ thống thực Thông số khối lượng và momen quán tính của xe robot rất khó xác định chính xác và thay đổi theo tải trọng người ma xe nâng đỡ Có nhiều phương pháp điều khiển, trong đó phương pháp computed torque được sử dụng rộng rãi đặc biệt trong lĩnh vực mobile robot Đã có nhiều nghiên cứu về phương pháp này như [10], [11] các kết quả đạt được rất tốt Trong phạm vi luận văn ta chọn phương pháp điều khiển thích nghi — một phương pháp điều khiến rất hay dùng với các thông số hệ thống bat định , dé so sánh với phương pháp computed torque và chọn ra phương pháp phù hợp với luận văn hơn Sơ đồ điều khiển như hình dưới.

Chương 3 Thiết kế bộ điều khiến q BỘ ĐIỀU v(t) \\y - BO DIEU t Mobil Déng hoc

' 5Ì KHIỂN >Ó-—>| KHIỂN DONG vebot Mobile

~ DONG HOC + LUC HOC robot

Hình 3.2 Sơ đồ khối cho bộ điều khién xe robot 3.2.2.1 Điều khiến thích nghỉ

Mô hình động lực học .- - - E2 211131331231 1111111211111 119 2111 111801 11 ng hà 16 Chương 3 Thiết kế bộ điều khiỂn . 2-5 se se xe seesseseseesee 21

Gia sử bánh xe lăn không trượt Như vay sẽ có các ràng buột sau: e Vận tốc của điểm tâm robot sẽ hướng theo trục đối xứng của xe: y,cos@ — #„sin8 = Ôa (2.7) e Bánh xe không bị trượt [7]: y-sind + #¿cos8 + OL = Ra@p (2.8)3„sin8 + 3¿cos8 — OL = Rao, (2.9)Ta có thé viết thành:

—sin@ cos@ -=a 0 0 A(q)=|cos@ sin L —R, 0 (2.11) cos@ sin@ —-L 0 -R,

Ta có: re = = xa + acos8 os JẤc = ta — aệsin0 (243)

Ye =4 + asin8 ~ ở, = ÿa + aÔcos0 Động năng của robot:

Với : Ia=lc + ma” mx2 myé , ; , Ic6?

Phương trình Lagrange (với thé năng băng không) đối với hệ non holonomic như sau:

Với : qi — toa độ Lagrange F, — lực suy rộng

— nhân tử Lagrange Tính toán: d /0T

—( - = mx + 1na8sin8 + ma*cosé (2.17) dt \ử*,

=()= ỷ 6cos6 + maé*sin@ (2.18) dt \ay,) ~ my, — rna8cos8 + ma@*sin

PP (=) = 1-6 + 2ma?0 + max,sin@ + mna#c0cos8 — mayccos@ + may sind

Thế các phương trình trên vào (2.16): mic + magsind + mmaÔ2cos60 = F„ + Œ¡ (2.19) myc — rnaệcos9 + ma6?sin@ = F, + Cy (2.20) (Ie + 2ma?)ệ + maxcsind — mayccos@ = r + Ca (2.21)

F,, Fy — lần lượt là lực tác động theo phương x và y

+— là momen tác động lên robot

C¡, Co, C3 — lần lượt là các ràng buột theo phương x, y, 0 Chuyển sang dang ma trận: m 0 masin@ ma62?cos0 F Cy 0 m —macos@ | + |mmaô2sin6 | = |F,| + | C2 (2.22) masin@ —macos@ I, + 2ma? 0 T C3 vs Fy =(Fp + F, )sin8

Tu cac phuong trinh trén : rcos(Trp + TL)

E Ra 0 0x sinO(tp +T 4 |C05Ư COST) tr

FE.| = e+ ti) _ + sind sn | * (2.23) y R R Tụ T a a1 | —L

—n(,sin8 + Keo = feo cos8 | [—m(y, sind +X ccos6)0]Ì = ma(y-sin@ + #„cos8)Ô

= AT(q)A (2.24) Viết lại phương trình động lực học: m 0 masin@ ] ma62?cos0

0 m —?nacos8 | q+ mad? sin masin@ —macos@ I, + 2ma’ 0

1 [cos@ cos6] _, m(y,sind + #¿cos8)Ôsin8

=m sin9 sin8 Bì + |—rn(„sin8 + #„cos8)8cos8 (2.25)

“LoL TL ma(y-sin@ + #„cos6)Ô

M(q) = | 0 m “acoso masin@d —macos@ lẹ + 2ma? rand =[0 4 0 maôsinổ

1 cos@ cos8P sin@ sindTL L —L

—a Â = —-m(,sin8 + #„cos8)8 Để đơn giản hóa phương trình động lực học ta sẽ tim cách triệt tiêu nhân tử lagrange Ta có: cos@ —asin8 v

STM(q)A"(q) = 0 (2.28) Thế (2.27) vào (2.26) rồi nhân với ma trận SỶ(q):

Vina @ = STM(q)M(q)S(q) + ST(q)W„(q, YS(q) = Log |

Kết thúc chương 2, ta đã có phương trình động hoc va động lực hoc của robot, ta sẽ thiết kế bộ điều khiển bám quỹ đạo cho robot dựa vào hai phương trình này.

THIET KE BO DIEU KHIEN

Từ phương trình động học va động lực học tìm được ở chương trước, trong chương nay ta sẽ thiết kế bộ điều khiến động học và động lực học cho robot bám theo quỹ đạo mong muốn Sau đó tiến hành mô phỏng với hai bộ điều khiển đề xuât.

Chương 3 Thiết kế bộ điều khiến Để điều khiển robot đi theo quỹ đạo như mong muốn đã có nhiều nghiên cứu dựa trên mô hình động học bỏ qua yêu tổ thuộc về động lực học như khối lượng, momen quán tính Để giải quyết van dé này ta sẽ phối hợp mô hình động học và động lực học.

Thiết kế quỹ đạO - ¿1 S221 1 19212121 12121111 112111011 010111211010100 110101211 1 kg 22

Quỹ đạo cân thiết kế phải tạo nên những chuyển động êm, nhẹ nhàng dé tránh tạo nên những rung động, va đập Do đó, thông thường quỹ đạo cần phải đảm bảo tính liên tục về vị trí , vận toc, và đôi khi cả vê gia toc.

Thiết kế quỹ đạo với đường cong bậc ba Quỹ đạo là hàm bậc ba theo thời gian q(t) với q(to)=qs và q(ti)=qr Với tị — to là thời gian hoàn thành quỹ đạo Quỹ đạo bậc 3 có dạng: q(t) = day + ayt + a;t? + astŸ (3.1) Vận tốc mong muốn: g(t) = a, + 2a;t + 3a3t? (3.2) Từ (3.1) và (3.2) ta tinh được 4 thông số của q(t):

‘lt tổ - tổ |ra qo

Vi trí bắt đầu va kết thúc của robot là qo va gr Dé tiến hành mô phỏng ta giả định vo =vr= 0.

Dạng đường đi có thé là bat kỳ ham nào: đường thang, đường tròn va sẽ được định nghĩa bang hàm liên hệ giữa tọa độ x và y của robot Khối tạo quỹ đạo được mô phỏng trong Matlab

Thiết kế bộ điều khiến . 5: tt St 2 2212 122 122 reo 23

Bộ điều khiển động lực học . ¿2 25 2 SE2E*ESEEEE2EEE E1 252152121 21221 xe 24

q BỘ ĐIỀU v(t) \\y - BO DIEU t Mobil Déng hoc

' 5Ì KHIỂN >Ó-—>| KHIỂN DONG vebot Mobile

~ DONG HOC + LUC HOC robot

Hình 3.2 Sơ đồ khối cho bộ điều khién xe robot 3.2.2.1 Điều khiến thích nghỉ

Trong các hệ điều khiển ta thường không biết giá trị chính xác của các tham số của hệ thống, các tham số của hệ thống bi thay đổi sau một thời gian sử dung, hay không có đủ thông tin về các tham số đó Khi đó các phương pháp điều khiển cô điển không áp dung được hoặc cho các kết quả điều khiển không chính xác Đề giải quyết van dé này, người ta đã xây dựng nên lý thuyết điều khiển thích nghi Hệ thong điều khién thích nghi là hệ thống điều khiển có thé tự xác định quy luật của tín hiệu điều khiển dựa trên thông tin của hệ thống trong quá trình làm việc.

= “ s Nà lì pe} MÔ Hive Mắt |

Hình 3.3 Mô hình điều khiển thích nghi theo mô hình mau

Từ phương trình động lực học, ta đặt :

Phuong trình động lực học trở thành:

Gia sử a=0 thì phương trình động lực học trở thành: lý 7]so =

Luật điều khiển được chon dé bám theo v.( và được chứng minh ồn định theo tiêu chuẩn

Tạ = lúa + K;¿(@q — w) Với K, , K> là hai tham số điều khiển dương Sai lệch vận tốc tuyến tính được định nghĩa :

Ve = Uạ — U Đạo hàm công thức trên ta được mô hình động lực học vòng kín cho vận tốc tuyến tính:

Tương tự cho vận tốc góc:

Theo luật điều khién như trên thi thông số m và I là xác định và cô định Nhưng thực tế thi giá trị của hai tham số đó là thay đổi Vì thé luật điều khiển trên được viết lại như sau:

Sai lệch giữa f(n) và I(n) sẽ ảnh hưởng đến tốc độ hội tụ và tạo ra các dao động không mong muốn trong hệ thống điều khiển thích ứng theo mô hình tham chiếu.

Thi Kì Ủ ——Ủa +—(U0ạ — Vv) mm mm Đặt : fì ` Ky

—=a, va — — ứữ; mm mm Thế vào phương trình trên ta được : Ủ = điỦUạ + đ¿(0ạ — 0) Mô hình mẫu cho sai số vận tốc tịnh tiễn là [10]:

Với T là hằng số dương Trong một hệ thống rời rạc thì T phải lớn hơn ít nhất 20 lần thời gian lây mẫu Viết lại công thức trên với Ve = Va — Vim:

Với Vm— vận tốc của mô hình mẫu

Sai sô thích nghi được định nghĩa là hiệu giữa van toc robot va vận tôc của mô hình mẫu : ey = — vạ Lay đạo hàm và khử vân tốc của mô hình mẫu đi, ta được : de, dv dv

= = 109409 (Vg — 9) — Ủa — Tuạ + Tim dt dt dt

Chon hàm Lyapunov như sau [10]: a,—1)* (a,-T) (a, ) „ ( ) Vio >0 va—+ 2

Ta thay V luôn không âm và sé bang không khi các biéu thức bình phương băng 0 Lay đạo hàm

V = —Te¿ + (a, — 1) (cob to) + (a2 — T) lev (va — 0) T at

Chương 3 Thiết kế bộ điều khiến Để Ủ < 0 thi: da,

Tính toán tương tự cho vận tốc góc ta cũng được luật cập nhật : das dt = ~V3€wWa da,

Phương pháp mô-men tính toán, hay còn gọi là phương pháp tuyến tính hóa phản hồi, là một kỹ thuật phổ biến trong điều khiển rô-bốt hiện đại Nó giúp loại bỏ các thành phần phi tuyến tính và bất định trong mô hình Lagrange, mở ra khả năng điều khiển rô-bốt linh hoạt và hiệu quả hơn Nhờ tính ưu việt này, phương pháp mô-men tính toán được ứng dụng rộng rãi trong các rô-bốt di động, giúp chúng hoạt động ổn định và chính xác ngay cả trong môi trường phức tạp.

Dé tuyến tính hóa hệ thống phi tuyến ta chọn luật điều khién như sau, với u là đầu vào phụ r = B@)~1[M(q)u + V„(q,4)9()]

Thế luật điều khiến vào phương trình động lực học ta được : v(t) = tu (3.4) Từ (2.27) và (3.4) ta suy ra phương trình chuyển động cho cả hệ thống: v(t)=u Đầu vào điều khiển duoc đề xuất như sau: u=v.+k,Q — 0) (3.6)

Với K+ là ma trận xác định dương 2x2

Để chứng minh tính ổn định của hệ thống có bộ điều khiển, phương pháp Lyapunov đã được áp dụng Sai số giữa tốc độ đầu ra của bộ điều khiển động học và tốc độ của xe robot được biểu diễn như sau:

_ (ea _ M 1+ COS(@g) + Kye, đe Les] ~ La Ww, + Kyeyv, + Kv, sin(ea)

Có thé thay răng V >0 V chi bang 0 nếu e,=e.=0 Lay dao hàm V

2€,€4 2Kyes5és K, Ku,KyạU, V = 2K,eyẻy + 2K, eyey + x Ê8 sin @€g + y

Ky Ko), Ky v, sin’ es

7 vy, sin? eg — (e, + Kyey)? — x(s + Kot, a)

Dé dang thay đạo ham ham lyapunov luôn 4m , nghĩa là hệ thống ổn định với luật điều khiển dé xuất.

Kết quả mô phỏng, -¿- - 5£ SE 921939 2121511 1212111 212111111 110111111111 0101 Xe 29 Chương 4 Định vị robot và kết quả thực nghiệm - 5 5° ôse se e<escsssees 38

+ erpre mhnyœs [PU — Cul= a sườt act thhydes x > eo Đ

4 2 B_gach_ nghịch daoSV0t2C1 Gow2 mnyoes 2

Hình 3.3 Mô hình simulink bộ điều khiển — luật thích nghi

Tiên hành mô phỏng xe robot với các thông sô của xe như sau

Thông số danh nghĩa Thông số thực có tải m = 101 (kg) m = 171 (kg)

Tiến hành mô phỏng robot bám theo quỹ đạo với phương pháp điều khién thích nghi và phương pháp computed torque.

Quỹ đao là đường thắng x=y e Điều kiện dau của robot : x=1 , y=2 , 0=pi/6 Thong số của bộ điều khiển computed torque K,, K;, K¿u, Kạ=Š

Hình 3.5 và 3.6 thé hiện quỹ đạo và sai số quỹ đạo của robot khi dùng phương pháp computed torque.

TT nan nh can nn gid Ô Ô.Ô.

2L- -~ - 4 a aes Aan Je - L~~~~~~ Jo - L~~~~~~ J - La - 4

Hình 3.5 robot bám theo quỹ đạo x=y với bộ điều khiển computed torque

Hình 3.6 sai số quỹ đạo của xe robot với bộ điều khiển computed torque

25 ms ; ; ey ios ; "ơ etheta 2 nt Rn i SO — mm 7

? Yo ' i 4 i You h m : - Ấrnireeeerrrreeerree ‡e==s=====eerrreerhireeeree Rưưq ơ

(a ieee anne in Se en tre ơ i ne ;

Thông số của bộ điều khiển thích nghi K¡=Ka, Kx=1, KyU, Ko, y¡P, yaP, y3%, Y4=5

Hình 3.7 và 3.8 thé hiện quỹ đạo và sai số quỹ đạo của robot khi dùng phương pháp điều khiến thích nghĩ.

Hình 3.7 robot bám theo quỹ đạo x=y với luật điều khiến thích nghi

2L ~“~“I-~~ ni i eel a | re aa 3 “~~~~~~ T~~~~”” ey — etheta

-0.5[ˆ -; nu sướng TH nnnnnsunn sườn prenererifrrerer 4

Hình 3.8 sai số quỹ đạo của xe robot với luật điều khién thích nghi

Nhân xét: cả hai bộ điều khiển đều bám tốt với quỹ đạo đặt ra, với luật thích nghi thi thời gian dé robot bat đầu bám tốt quỹ đạo chỉ mat 6s còn với computed torque robot mat đến 8.5s.

Nhưng khả năng bám của robot với phương pháp computed torque tốt hơn, sai lệch xấp xỉ 0.Ta tiếp tục mô phỏng với các vị trí ban đầu khác nhau e Điều kiện dau của robot : x=3 , y=1 , 0=3pi/2 Thông số của bộ điều khién : Ky, K;=1, KyU, Ko.

Hình 3.9 đến 3.10 thể hiện quỹ đạo và sai số quỹ đạo của robot khi dùng phương pháp computed torque.

Chương 3 Thiét kê bộ điêu khiên

` 2 y với bộ điêu khi Hình 3.9 robot bám theo quỹ đạo x a a ễỐễ ễ ễẨỂằễễẨẨẨễệỂ- _-_ ễ - - -ễ -.ễỎềẢ„ ey

L ' ' “nn TO RR RR RR RR RR ET

[ _—_——— Am —_—— bs beet eieieieieietetel ——— li — —— = ' ' ' ' '

` 2 ủa xe robot với bộ điêu khiên computed torque z

Hình 3.10 sai sô quỹ đạo c

P, 2 K;, Ky=1, KyU, Ko, y¡ h nghi Kị=

` ủa bộ điêu khiên th ông sô c

Hình 3.11 và 3.12 thé hiện quỹ đạo và sai số quỹ đạo của robot khi dùng phương pháp điều khiến thích nghĩ.

Hình 3.12 sai số quỹ đạo của xe robot với luật điều khién thích nghiNhân xét: ở một vị trí ban đầu khác, xa hơn thì hai bộ điều khiển vẫn bám theo quỹ đạo rat tốt, thời gian đáp ứng của phương pháp điều khiển thích nghi van tốt hon phương pháp computed

Kết quả mô phỏng cho thấy phương pháp điều khiển Computed Torque có khả năng bám quỹ đạo tốt hơn phương pháp Thiết kế bộ điều khiển mô men xoắn Tuy nhiên, luật điều khiển thích nghi yêu cầu hiệu chỉnh nhiều thông số hơn, gây khó khăn trong việc tìm bộ thông số tối ưu Do đó, phương pháp Computed Torque có phần vượt trội với luật điều khiển đơn giản, khả năng đáp ứng tốt với sự thay đổi khối lượng và momen quán tính Vì vậy, tác giả lựa chọn phương pháp Computed Torque để thực hiện luận văn.

Với phương pháp đã chọn ta sẽ mô phỏng với quỹ đạo mong muốn trong không gian phòng bệnh như đã đề cập ở chương 1, theo quỹ đạo đưa bệnh nhân đến toilet, còn quỹ đạo đưa bệnh nhân trở lại giường bệnh chỉ là đi ngược lại quỹ đạo ban đầu nên ta không cần mô phỏng.

Quỹ dao đưa bệnh nhân đến toilet e Điều kiện đầu của robot : x=0.5, y=0.5 , 0=pi/2 Thông số của bộ điều khiển computed torque: Ky, K;, K¿u, Kạ=l5

Quỹ đạo và sai sô của robot :

hhẴn*ẽ*sô—- —- — — —- —- —- —- — — — —— -$_ — y, * 2La “Ẻ Xụ, Vị b) Robot di chuyển cùng chiều kim đồng hỗ

Xo >L+La EndỀ 7 | ods ; Nominalsquare path y¿ * —L \ ig =| |

Chương 4 Dinh vi robot và kết quả thực nghiệm

4.8.2 Sai số loại B a) Robot di chuyển cùng ngược chiều kim đồng hỗ SG X2, Y2

\ \ ùL—3L

Tiêu đề	Thiết kế robot hỗ trợ bệnh nhân trong vệ sinh cá nhân
Tác giả	Nguyen Hoang Vu
Người hướng dẫn	TS. Le Ngoc Bich
Trường học	Trường Đại học Bách Khoa, Đại học Quốc gia TP. HCM
Chuyên ngành	Kỹ thuật cơ điện tử
Thể loại	Luận văn thạc sĩ
Năm xuất bản	2014
Thành phố	TP. HCM

Định dạng
Số trang	72
Dung lượng	38,27 MB