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

Cụ thể về đề tài này, nhóm sẽ tìm hiểu về việc sử dụng PID hay còn biết đến là ProportionalIntegral 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 r

ĐẠI HỌC QUỐC GIA HÀ NỘI TRƯỜ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ệp

Mã lớp học phần: EMA3083_20

Nhóm sinh viên: Phan Trung Kiên

Đặng Minh Lân

Đỗ Thị Loan

21021330 21021332 21021334

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

Chương 1: Giới thiệu hệ thống 1.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 tay robot 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ụng cá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ệc

sử dụng PID hay còn biết đến là ProportionalIntegral 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 đối tượ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ương phá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ết bà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ánh tay 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ây dựng một hệ thống điều khiển linh hoạt và chính xác để cánh tay robot có thể di chuyể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 đưa cá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ều khiể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 con robot 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ắt laser 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ên

có 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ều khiể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ên chí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 xoay

1 Ở khớp thứ 3 cần có 1 thanh truyền động từ động cơ đến trục xoay 2 Động cơ thứ 4 truyề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 cho

hệ 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ột module 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ủa mộ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ột chip 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óc xoay theo 3 trục MPU6050 có độ chính xác khá cao, giúp xác định vị trí và hướng chuyể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ác châ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ển chí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ảm biế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ộ 2 giá 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ất biế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 3 gốc tọa độ bao gồm : gốc tọa độ tham chiếu O0X0Y0Z0, hệ tọa độ ứng với khớp 2

O1X1Y1Z1 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ận

thuần nhất của các phép biến đổi cơ bản theo đúng thứ tự:

A i=R z(θ i) T z(d i).T x(a i) R x(α i)

Sau khi thực hiện phép nhân ma trận, ta thu được kết quả sau:

Ti =

Trong đó Sθθ i ,C θ i , Sθ α i , C α i lần lượt là kí hiệu viết tắt của sin ⁡¿ ¿) , cos ⁡¿ ¿) , sin ⁡(α i) ,

cos ⁡(α i)

Từ lý thuyết trên nhóm đã sử dụng matlab tính được Ma trận D-H cho hệ 0 → 1 cho hệ thống :

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:

P x=L1cos ⁡(θ¿¿1)+L2cos ⁡(θ1+θ2)¿

P y=L1sin(θ¿ ¿1)+L2sin ⁡(θ1+θ2)¿

p x2

+p y2

=L12

−L22 +2 L1L2cos θ2

cos ⁡(θ¿¿2)=p x

2 +p y2

−L12

−L22

2 L1L2 ¿

θ2=cos−1 p x

2

+p y2

−L12

−L22

2 L1L2

θ1=atan2 p x

p y+atan2

L2sin θ2

L1 +¿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 +02−852−1352 2∗85∗135 θ1=atan2 0

12 0+atan2

135 sinθ2 85+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ằm ngang đ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ó thanh nà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à

l2=135mm.

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ôn ngữ 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àm atan2() 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 tham

số 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ống 2.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ển trụ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ều khiể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 được giá 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 động

cơ đế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ọc ngượ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ết quả 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ần mề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ài toá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 để đưa tay gắp đến vị trí mong muốn, phần 2 điều khiển động cơ khớp 1 xoay đến vị trí mong muố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 quay củ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 được xâ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 động

cơ 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ận thấy động cơ khớp 2 theo phương nằm ngang đ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 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óc setpoint 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ến góc quay MPU6050 đã được sử dụng để làm đầu vào cho thuật toán Các hệ số K p, K i,

K d đượ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 ra thờ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ủa

1 vòng PID Ở đây nhóm đã để thời gian phản hồi là 100 milis Các hệ số này vô cùng quan trọng, nó quyết định tính chất hoạt động của hệ thống Sau đó đầu ra của thuật toán PID sẽ được đi qua module băm xung L298N để cấp nguồn cho động cơ quay đến

vị trí mong muốn Nguồn này chính là điện áp có mức dao động từ 5V đến 12V (tính chấ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 áp nguồ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, Đầu tiê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 tay robot đến vị trí mong muốn Tại đây cần delay chương trình khoảng 3s để 2 động cơ 1

và động cơ 2 thực hiện xong nhiệm vụ Sau đó động cơ ở khớp số 1 sẽ quay và điều chỉ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ều khiển Microchip ATmega328 được phát triển bởi Arduino.cc Bảng mạch được trang

bị 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ạch

mở 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àng

sử 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ủa Microchip 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à RAM 2KB 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 gian thự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 gian phả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 code cổ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úp thu 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ể theo dõ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ực hiệ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ết nố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ấp nguồn cũng như giao tiếp với cảm biến MPU6050 qua giao tiếp I2C và module điều khiển động cơ qua giao tiếp SPI

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