BÁO CÁO BÀI TẬP LỚN MÔN ROBOT TRONG CÔNG NGHIỆP Điều khiển hệ thống cánh tay robot thông minh dựa trên thuật toán PID

ĐẠI HỌC QUỐC GIA HÀ NỘITRƯỜNG ĐẠI HỌC CÔNG NGHỆ

BÁO CÁO BÀI TẬP LỚN

MÔN: ROBOT TRONG CÔNG NGHIỆP

Điều khiển hệ thống cánh tay robot thông minh dựa trên thuật toán PID

Giảng viên hướng dẫn: TS Đinh Trần HiệpMã lớp học phần: EMA3083_20

Nhóm sinh viên: Phan Trung KiênĐặng Minh Lân Đỗ Thị Loan

21021330 2102133221021334

Chương 1: Giới thiệu hệ thống Chương 2: Nguyên lí hoạt độngChương 3: Kết quả

Chương 1: Giới thiệu hệ thống1.1.Giới thiệu đề tài:

Trong môn học này, nhóm đã lựa chọn đề tài “Điều khiển hệ thống cánh tayrobot thông minh dựa trên thuật toán PID” Đây là một ví dụ điển hình về ứng dụngcác nguyên tắc cơ điện tử trong thực tế Cụ thể về đề tài này, nhóm sẽ tìm hiểu về việcsử dụng PID hay còn biết đến là Proportional-Integral- Derivative (Tỉ lệ - Tích phân -Đạo hàm) để tính toán và điều khiển góc quay của cánh tay robot dựa trên dữ liệu từcảm biến MPU6050 MPU6050 cung cấp dữ liệu về sự chuyển động và hướng của đốitượng, rất phù hợp cho việc sử dụng cánh tay robot; thuật toán PID là một phươngpháp điều khiển phản hồi phổ biến và có hiệu quả cao trong các hệ thống tự động.Ngoài ra, việc điều khiển cánh tay robot cũng được điều khiển dựa trên cơ sở lý thuyếtbài toán động học ngược.

Mục tiêu của đề tài: Tìm hiểu và áp dụng PID để điều khiển góc quay của cánhtay robot dựa trên dữ liệu từ cảm biến kết hợp với bài toán động học ngược để xâydựng một hệ thống điều khiển linh hoạt và chính xác để cánh tay robot có thể dichuyển một cách tốt nhất Ngoài ra tìm hiểu về các bậc tự do của cánh tay nhằm đưacánh tay đến 1 vị trí mong muốn Thông qua việc kết hợp sử dụng phương pháp điềukhiển PID và bài toán động học ngược, đề tài này hứa hẹn mang lại những hiểu biết vềviệc tính toán và điều khiển góc quay của cánh tay robot hay thậm chí là cả một conrobot trong thực tế.

1.2 Mô tả hệ thống:1.2.1 Khâu cơ khí:

Ở trong báo cáo này sử dụng cánh tay robot được làm từ mica qua quá trình cắtlaser và được lắp ghép bởi ốc vít 3M Ta thu được 1 robot với kích thước 14x9x18 cm.Độ dài trục 1 và trục 2 lần lượt là 85mm và 135mm Robot gồm 4 động cơ, tuy nhiêncó 1 động cơ đề điều chỉnh độ rộng khớp gắp nên cánh tay robot có 3 bậc tự do Mỗiđộng cơ sẽ tương ứng một vị trí khớp để điều khiển các trục xoay Mỗi khớp điềukhiển một trục xoay cho phép cánh tay robot xoay quanh trục chính Trên thực tế cónhiều cơ cấu truyền động khác nhau từ động cơ đến trục xoay Ở trong báo cáo này,nhóm đã sử dụng cánh tay robot với các cách truyền động khác nhau Ở khớp đầu tiênchính là trục chính, động cơ sẽ được gắn trực tiếp vào đế của robot Ở khớp thứ 2 sửdụng bánh răng truyền chuyển động từ động cơ đến trục xoay để điều khiển trục xoay1 Ở khớp thứ 3 cần có 1 thanh truyền động từ động cơ đến trục xoay 2 Động cơ thứ 4truyền chuyển động cho độ rộng của khớp gắp qua bánh răng.

Hình 2.1 Hình ảnh cánh tay robot

1.2.2 Khâu cảm biến

Trong hệ thống sử dụng cảm biến MPU6050 để thực hiện phản hồi góc đo chohệ thống, cập nhật đầu vào cho thuật toán PID Cảm biến gia tốc MPU6050 là mộtmodule cảm biến tích hợp, kết hợp cảm biến gia tốc kế và con quay hồi chuyển(gyroscope) Nó được sử dụng để đo và theo dõi chuyển động, gia tốc và góc quay củamột đối tượng.

Hình 2.2 : Hình ảnh module cảm biến MPU6050

Cảm biến góc quay MPU6050: là một module cảm biến đa chức năng thườngđược sử dụng trong các ứng dụng điều khiển và định vị trong robot và các thiết bị diđộng khác Nó còn là một cảm biến linh hoạt và mạnh mẽ, cung cấp thông tin cần thiếtđể điều khiển và theo dõi chuyển động của các thiết bị điện tử một cách chính xác vàhiệu quả MPU6050 kết hợp một bộ gia tốc kế và một bộ góc quay trong cùng mộtchip Bộ gia tốc kế đo gia tốc tuyến tính của thiết bị theo các trục XYZ Bộ góc quayđo tốc độ góc của thiết bị xung quanh các trục XYZ Từ đó nó có thể tính được gócxoay theo 3 trục MPU6050 có độ chính xác khá cao, giúp xác định vị trí và hướngchuyển động của thiết bị một cách chính xác Cảm biến này thường có độ nhạy và độphân giải cấp cao, giúp nhận biết cảnh báo nhỏ nhất trong chuyển động MPU6050

thường được giao tiếp thông qua giao diện I2C (Inter-Integrated Circuit), cho phép nókết nối với các vi điều khiển và các thiết bị khác dễ dàng Module cũng cung cấp cácchân nối điện thông dụng cho dễ dàng kết nối với các mạch khác Trong robot, nó cóthể được sử dụng để đo góc quay của các cánh tay hoặc chân robot, giúp điều khiểnchính xác hành động của chúng

Ở trong báo cáo tập trung sử dụng khả năng đo góc xoay quanh trục Z của cảmbiến nhằm xác định góc xoay của cánh tay robot Từ những giá trị của cảm biến để làmđầu vào cho thuật toán PID Sử dụng PID để điều khiển động cơ quay đến một vị trísetpoint cho trước.

1.2.3 Bài toán động học ngược:

Ban đầu, cho trước vị trí và hướng của khâu thao tác cuối, thường là một bộ giátrị của 6 thông số gồm 3 thông số định vị và 3 thông số định hướng Nếu các hệ tọa độdùng để định vị và định hướng khâu cuối được lập theo phương pháp DH, thì từ bộ 2giá trị trên, ta sẽ xác định được giá trị của tất cả các phần tử trong ma trận thuần nhấtbiến đổi từ hệ tọa độ gốc sang hệ tọa độ cuối.

Hình 2.3 Mô phỏng cánh tay robot

Đối với hệ tọa độ khâu cố định O0X0Y0Z0, trục X0 có thể được chọn bất kỳnhưng thường là phù hợp với vị trí và hình dáng của khâu cố định (khâu 0) Đối với hệtọa độ khâu thao tác cuối OnXnYnZn , trục Zn thường được chọn tùy thuộc vào thiết bịcụ thể sẽ lắp lên đầu công tác của Robot, gốc hệ tọa độ khâu thao tác cuối On thườngđược đặt tại điểm tương tác với môi trường và được gọi là điểm tác động cuối

Cụ thể ở trong hệ thống này nhóm đã chọn được góc tọa độ như hình trên với 3gốc tọa độ bao gồm : gốc tọa độ tham chiếu O0X0Y0Z0, hệ tọa độ ứng với khớp 2O1X1Y1Z1 và hệ trục tọa độ tương ứng với khớp 3 là O2X2Y2Z2 Ngoài ra có l1,l2 tươngứng với chiều dài trục 1,và trục 2, l1 = 85mm, l2 = 135 mm.

Sau khi đã thiết lập được hệ trục tọa độ, nhiệm vụ tiếp theo là xác định thông sốcủa phép biến đổi giữa các hệ trục tọa độ trung gian, hay còn gọi là bộ thông số

Denavit-Hatenberg (bộ thông số D-H).Trong đó các hệ số :

θii là góc giữa trục Xi−1 và trục Xi ;

di là khoảng cách giữa trục Xi−1 và trục Xi ;ai là khoảng cách giữa trục Zi−1 và trục Zi ;αii là góc giữa trục Zi−1 và trục Zi

Từ các thông số đề bài ta có bảng D-H cho hệ thống sau:

Từ các phép biến đổi cơ bản nêu trên, ta tính được ma trận thuần nhất biến đổi từhệ tọa độ Oi-1Xi-1Yi-1Zi-1 sang hệ OiXiYiZi tọa độ Ma trận này bằng tích các ma trậnthuần nhất của các phép biến đổi cơ bản theo đúng thứ tự:

Ma trận D-H cho hệ 1 → 2

Ta có ma trận thuần nhất khi chuyển hệ trục tọa độ từ 0 →2 = *

Các hệ số của ma trận thuần nhất trên lần lượt là:

Trong đó với hệ thống này nhóm cần xác định tọa độ của tay gắp trong mặt phẳng hệ tọa độ OXY nên cần quan tâm đến 2 giá trị Px và Py Trong đó Px, Py lần lượt là tọa độ của tay gắp trong mặp phẳng OXY Đồng nhất hệ số ta được:

Px=L1cos ⁡(θ¿¿1)+L2cos ⁡(θ1+θ2)¿

Py=L1sin(θ¿ ¿1)+L2sin ⁡(θ1+θ2)¿ px2

+2 L1L2cos θ2

cos ⁡(θ¿¿2)=px2

2 L1L2 ¿θ2=cos−1 px

2 L1L2

L2sin θ2L1 +¿L2cos θ2¿

Ở trong hệ thống này nhóm đã thực nghiệm với một số tọa độ của tay gắp khác nhau, cụ thể nhóm đã tiến hành đặt vị trí tay gắp ở tọa độ XY (120,0) Từ đó áp dụng công thức ta thu được

2∗85∗135 θ1=atan2 0

12 0+atan2

135 sinθ285+135∗cos θ2

Từ đó nhóm đã tính được θ1 = 80,45 và θ2=¿118,7

Trong quá trình lắp ráp và thử nhóm nhận thấy động cơ khớp 2 theo phương nằmngang đang là vị trí góc 135º, động cơ khớp 3 theo phương ngang đang là góc 215º Từhai góc tính được của thuật toán tính toán được sẽ chuyển đổi sang vị trí góc của haiđộng cơ Cụ thể vị trí góc của động cơ 1 sẽ là 135-θ1 Tuy nhiên do trục 2 không đượcđiều khiển trực tiếp bới khớp 2 mà được điều khiển qua 1 thanh truyền động Có thanhnày luôn song song với trục 2 nên từ đó ta tính được vị trí góc của động cơ khớp 3 là215-(θ2-θ1) Ngoài ra cũng biết rằng chiều dài trục 1 và trục 2 lần lượt là l1= 85mm và

Hình 3.1 : Mô phỏng cánh tay robot

Hàm atan2() là một hàm lượng giác ngược, ban đầu được xây dựng trong ngônngữ lập trình Fortran, sau đó được đưa vào nhiều ngôn ngữ lập trình khác và dần trởnên phổ biến trong khoa học và kỹ thuật Khác với hàm arctan() (hay còn được viết làatan()) chỉ nhận 1 tham số và có miền nghiệm chỉ là nửa vòng tròn lượng giác, hàmatan2() nhận 2 tham số và có miền nghiệm là toàn bộ vòng tròn lượng giác

Ý nghĩa hình học của hàm atan2() được thể hiện trong Hình 3.23 Giả sử 2 thamsố của hàm lần lượt là y và x, hàm atan2(y,x) sẽ trả về giá trị θ là góc tạo bởi đường

nối gốc tọa độ O(0,0) với điểm có tọa độ (x,y) trên vòng tròn lượng giác (gốc tính từtrục Ox, chiều dương ngược chiều kim đồng hồ).

Chương 2: Thiết kế hệ thống2.1 Sơ đồ khối và nguyên lí hoạt động

Hình 3.1 Sơ đồ khối của hệ thống

Sơ đồ khối hệ thống gồm có 3 khối chính là khối điều khiển, bộ xử lí trung tâm(MCU), thiết bị ngoại vi là máy tính Ở khối điều khiển ở đây gồm khối điều khiểntrục chính sử dụng thuật toán PID và khối điều khiển tay gắp đến vị trí mong muốn sửdụng bài toán động học ngược Các khối này được điều khiển với bộ xử lí trung tâm làArduino Uno R3 để trả về các kết quả mong muốn từ người dùng Đầu tiên ở khối điềukhiển trục chính sử dụng thuật toán PID có 2 khâu là khâu cảm biến góc để đọc đượcgiá trị vị trí góc mà trục chính đang đứng Sau khi có kết quả trả về, khâu trục chínhđược sử dụng động cơ DC kết hợp với thuật toán điều khiển PID để điều khiển độngcơ đến vị trí góc mong muốn Khối điều khiển thứ hai sử dụng bài toán động họcngược để điều khiển các khâu trục khớp và trục gắp của cánh tay ta sử dụng bài toánđộng học ngược để tính ra các vị trí góc mà cánh tay muốn hướng đến đối tượng Kếtquả các vị trí các góc đo được sẽ được hiển thị trên cửa sổ Serial Monitor trên phầnmềm Arduino

2.2 Đầu vào, đầu ra và mối liên hệ giữa đầu vào và đầu ra của hệ thống:

Trong đề tài này nhóm đã đặt ra một yêu cầu cho hệ thống thực hiện gắp vật ởmột vị trí biết trước và di chuyển nó đến một nơi xác định trong tầm kiểm soát Bàitoán được đặt ra bao gồm 2 phần Phần 1 điều khiển động cơ khớp 2 và khớp 3 để đưatay gắp đến vị trí mong muốn, phần 2 điều khiển động cơ khớp 1 xoay đến vị trí mongmuốn

Đầu vào là tọa độ X, Y của đầu gắp trên cách tay robot Và đầu ra là góc quaycủa 2 động cơ so với vị trí tham chiếu Để làm được điều này một thuật toán tính toán

chuyển đổi giữa vị trí và góc xoay của động cơ được thiết lập Thuật toán này đượcxây dựng dựa trên bài toán động học ngược Tiếp đến là thuật toán để điều khiển độngcơ quay đến những góc theo kết quả từ thuật toán tính toán ở trên Bởi hai động cơservo SG90 quy định sẵn vị trí góc của nó Trong quá trình lắp ráp và thử nhóm nhậnthấy động cơ khớp 2 theo phương nằm ngang đang là vị trí góc 135º, động cơ khớp 3theo phương ngang đang là góc 215º Từ hai góc tính được của thuật toán tính toánđược sẽ chuyển đổi sang vị trí góc của hai động cơ Cụ thể vị trí góc của động cơ 1 sẽlà 135-θ1 và vị trí góc động cơ 2 là 215-(θ2-θ1).

Hình 3.2 Mô phỏng tính toán

Ở phần tiếp theo, động cơ xoay đến 1 góc với số đo biết trước nên đầu vào sẽ làgóc setpoint, cụ thể trong báo cáo này nhóm đã thực hiện các thực nghiệm với gócsetpoint là 90º Đầu ra sẽ là một giá trị output để điều khiển băm xung cho động cơ Ởđây nhóm đã xây dựng thuật toán dựa trên thuật toán điều khiển PID Một cảm biếngóc quay MPU6050 đã được sử dụng để làm đầu vào cho thuật toán Các hệ số Kp, Ki,

Kd được lựa chọn để thuật toán hoạt động ổn định và chính xác nhất có thể Ngoài rathời gian phản hồi của cảm biến cũng cần được quan tâm, nó chính là thời gian lặp của1 vòng PID Ở đây nhóm đã để thời gian phản hồi là 100 milis Các hệ số này vô cùngquan trọng, nó quyết định tính chất hoạt động của hệ thống Sau đó đầu ra của thuậttoán PID sẽ được đi qua module băm xung L298N để cấp nguồn cho động cơ quay đếnvị trí mong muốn Nguồn này chính là điện áp có mức dao động từ 5V đến 12V (tínhchất của động cơ - cơ cấu chấp hành) Tốc độ quay của động cơ phụ thuộc và điện ápnguồn đầu vào.

Nói tóm lại, hệ thống được điều khiển với các chức năng lần lượt như sau, Đầutiên cảm biến được kích hoạt, và đợi 3s để cảm biến góc xoay có thể ổn định, sau đó 2động cơ ở khớp 2 và khớp 3 sẽ hoạt động để điều khiển vị trí của đầu gắp cánh tayrobot đến vị trí mong muốn Tại đây cần delay chương trình khoảng 3s để 2 động cơ 1và động cơ 2 thực hiện xong nhiệm vụ Sau đó động cơ ở khớp số 1 sẽ quay và điềuchỉnh dần dần đến vị trí mong muốn qua thuật toán PID Nhóm cũng hướng tới sau đóđộng cơ ở khớp 2 và khớp 3 hoạt động để đưa đầu gắp của cánh tay đến 1 tọa độ mới

2.3 Bộ xử lý trung tâm

Arduino Uno R3 là một bảng mạch vi điều khiển nguồn mở dựa trên vi điềukhiển Microchip ATmega328 được phát triển bởi Arduino.cc Bảng mạch được trangbị các bộ chân đầu vào/ đầu ra Digital và Analog có thể giao tiếp với các bảng mạchmở rộng khác nhau

Hình 3.3 Vi điều khiển trung tâm của hệ thống.

Arduino Uno R3 là bo mạch vi mạch mở và linh hoạt, được thiết kế để dễ dàngsử dụng cho người mới bắt đầu trong lĩnh vực điện tử và lập trình Arduino Uno R3 cóhình dạng nhỏ gọn, chứa một vi mạch chính với bộ điều khiển ATmega328P củaMicrochip Cụ thể trong hệ thống này, nhóm sử dụng vi điều khiển này để thực hiệnđiều khiển toàn bộ chương trình và tính toán cũng như lưu trữ dữ liệu của hệ thống.

Ngoài ra vi mạch này có tốc độ xử lý 16MHz, bộ nhớ flash 32KB và RAM2KB Với tần số dao động này nhằm đồng bộ thời gian trên vi điều khiển và thời gianthực tế Ở trong hệ thống này có sử dụng đến thời gian thực với 100milis Đây chính làthời gian phản hồi của cảm biến hay nó cũng chính là 1 vòng lặp cho thuật toán PID.Tùy vào từng hệ thống và cấu tạo phần cơ khí mà hệ thống cần có những thời gianphản hồi phù hợp.

Ngoài ra nó có thể được lập trình trực tiếp từ máy tính thông qua dây nạp codecổng USB Không chỉ nạp code nó còn là giao tiếp giữa máy tính và vi điều khiển giúpthu thập dữ liệu từ vi điều khiển và hiện lên màn hình Nó giúp người dùng có thể theodõi được các giá trị cảm biến góc MPU6050 trả về cũng như theo dõi các bước thựchiện của hệ thống xem hệ thống có hoạt động ổn định không.

Arduino Uno R3 có một loạt cổng kết nối và chân chức năng, cổng nguồn DC,cổng vào/ra digital và cổng analog Các chân vào/ra digital có thể được sử dụng để kếtnối với các cảm biến và thiết bị điều khiển khác nhau, trong khi các cổng analog có thểsử dụng cho các cảm biến tín hiệu analog Ở đây nhóm đã sử dụng chân này để cấpnguồn cũng như giao tiếp với cảm biến MPU6050 qua giao tiếp I2C và module điềukhiển động cơ qua giao tiếp SPI.

2.4 Cơ cấu chấp hành

Động cơ Servo SG90 được sử dụng để điều khiển 2 bậc liên quan đến bán kínhvà góc quay trong bài toán động học ngược Động cơ Servo SG90 có kích thước nhỏ,là loại được sử dụng nhiều nhất để làm các mô hình nhỏ hoặc các cơ cấu kéo khôngcần đến lực nặng Động cơ servo SG90 180º có tốc độ phản ứng nhanh, các bánh răngđược làm bằng nhựa nên cần lưu ý khi nâng tải nặng vì có thể làm hư bánh răng, độngcơ RC Servo MG90 có tích hợp sẵn Driver điều khiển động cơ bên trong nên có thể dễdàng điều khiển góc quay bằng phương pháp điều độ rộng xung PWM Động cơ servođược thiết kế những hệ thống hồi tiếp vòng kín Tín hiệu ra của động cơ được nối vớimột mạch điều khiển Khi động cơ quay, vận tốc và vị trí sẽ được hồi tiếp về mạchđiều khiển này Nếu có bất kỳ lý do nào ngăn cản chuyển động quay của động cơ, cơcấu hồi tiếp sẽ nhận thấy tín hiệu ra chưa đạt được vị trí mong muốn Mạch điều khiểntiếp tục chỉnh sai lệch cho động cơ đạt được điểm chính xác Các động cơ servo điềukhiển bằng liên lạc vô tuyến được gọi là động cơ servo RC (radio-controlled) Trongthực tế, bản thân động cơ servo không phải được điều khiển bằng vô tuyến, nó chỉ nốivới máy thu vô tuyến trên máy bay hay xe hơi Động cơ servo nhận tín hiệu từ máy thunày.

Hình 3.4 Hình ảnh các động cơ được sử dụng trong hệ thống.

Trục chính sử dụng động cơ DC Giảm Tốc JGB37-520 DC Geared Motor Độngcơ này được điều khiển bằng thuật toán PID và mạch cầu H L298N Động cơ đượcthiết kế với đường kính trục là 6mm, nó phù hợp để gắn vào đế động cơ Lực kéomoment tối đa là 3.1 Kg.cm hoàn toàn đủ lực để xoay cánh tay robot Tông chiều dàicủa động cơ là 5cm Với chiều cao này nhóm đã thiết kế sử dụng cọc đồng 5cm để làmphần giá đỡ cho robot.

2.5 Thuật toán điều khiển của hệ thống:

Ở khối điều khiển trục chính, hệ thống thực hiện thuật toán PID để đưa cánh tayrobot xoay đến vị trí mong muốn:

- Bước 1: Xác định góc cần đạt được (Setpoint), Thiết lập các biến lưu trữ giá trịgóc hiện tại và sai số, thiết lập các hệ số PID: Kp, Ki, Kd.