Tính toán moment xoắn động cơ

3. CHƢƠNG III – TÍNH TOÁN THIẾT KẾ CƠ KHÍ

3.2.7 Tính toán moment xoắn động cơ

Mỗi khâu robot có khối lƣợng 350g, đã đƣợc gia trọng để trọng tâm nằm chính giữa khâu. Hệ số masat giữa khâu và môi trƣờng theo phƣơng pháp tuyến n 0,56

Moment xoắn cần thiết cho động cơ

1 1 0,35 0,56* 25, 0 2, 45 . 2 2 i i n i T  m l  x x  kg cm (3.1) 3.2.8 Lựa chọn động cơ

Động cơ RC servo đƣợc chọn sử dụng là loại Towerpro MG946R

Hình 3.7 Ảnh động cơ thực tế dùng trong mô hình Thông số động cơ :

Khối lƣợng : 55g

Đƣờng kính : 40.7*19.7*42.9mm

Vận tốc : 0.2 s/60degree(4.8V) , 0.17 s/60degree(6V) Vol : 4.8 – 7.2V

Hộp số là bánh răng kim loại Dải chết 5us

Với các thông số trên , thì động cơ có thể thắng lực cản và giúp cho robot có thể chuyển động đƣợc.

3.2.9 Thiết kế hệ thống cơ khí : 3.2.9.1Thiết kế mô hình 3D 3.2.9.1Thiết kế mô hình 3D

 Động Cơ

Hình 3.8 : Mô hình 3D của RC servo

Động cơ RC đƣợc vẽ trên những kích thƣớc thật của chi tiết  Giá Động Cơ

Hình 3.9 : Mô hình 3D của giá động cơ  Ống để làm 1 đốt của robot rắn

Hình 3.10 : Mô hình 3D của ống làm 1 đốt của robot  1 đốt của robot rắn

Hình 3.11 : Mô hình 3D 1 đốt của robot  Ống nối các đốt

Hình 3.13 : Robot rắn 6 đốt

4. CHƢƠNG IV - ĐIỀU KHIỂN VÀ THIẾT KẾ MẠCH ĐIỀU KHIỂN 4.1 ĐIỀU KHIỂN 4.1 ĐIỀU KHIỂN

4.1.1 Phƣơng án điều khiển

Trong khuôn khổ luận văn này, chuyển động của robot rắn là sóng ngang (lateral undulation) bám mục tiêu. Điều khiển đƣợc thực hiện ở các module với nhiệm vụ là điều khiển động cơ thực hiện dạng chuyển động .

4.1.1.1Chuyển động hình sin

Cách đơn giản nhất để tạo ra di chuyển dạng sóng trong một chuỗi nối tiếp của n khâu là do các góc khác nhau của từng khâu có 1 tần số chung và 1 hằng số độ lệch pha giữa các khâu với nhau .

Ta đã biết góc tƣơng đối giữa các khâu theo dạng chuyển động dạng sóng ngang nhƣ sau: , sin( ( 1) ) ( 1 )

i d t i i n

        

Với α là biên độ dao động, β là độ lệch pha giữa các khâu kế tiếp, γ quyết định hƣớng của robot ( γ=0 thì robot chạy thẳng), và ω là tần số góc của dao động. Tham số β và ω quyết định tốc độ của dạng sóng truyền dọc theo robot và qua đó xác định tốc độ robot.

 Tối ưu Pareto

Các nghiên cứu theo lý thuyết tối ƣu Pareto về năng lƣợng đã xác định các tính chất: ω là hàm tuyến tính của vận tốc di chuyển tới

α là hàm tỉ lệ với tỉ lệ ct/cn của các hệ số ma sát β là hàm tỉ lệ với số khâu n , ta thƣờng chọn 2

n

  

Kết quả này sẽ đƣợc sử dụng trực tiếp để điều khiển robot chứ không chứng minh lại Động cơ RC Servo đƣợc điều khiển trong khoảng -90 ÷ 900

. Chọn phƣơng trình chuyển động của robot nhƣ sau :

, 2 30sin( ( 1) ) ( 1 6) 6 i d t i  i        

Với γ trong khoảng [-45÷45 ] đƣợc điều khiển định hƣớng theo thuật toán điều khiển Fuzzy.

4.1.1.2Chuyển động hình sin với sự định hƣớng của khâu đầu

Khi rắn chuyển động ,rắn sẽ di chuyển chính xác khi đƣợc định hƣớng bởi khâu đầu. Quá trình này đƣợc thực hiện thông qua việc nhận tín hiệu từ các cảm biến gắn trên đầu mô hình .

Khi robot rắn di chuyển, định hƣớng của khâu đầu cũng thay đổi theo hình sin và do đó tín hiệu cảm biến cũng thay đổi liên tục trong suốt quá trình di chuyển. Điều này làm cho việc điều khiển robot trở nên khó khăn hơn , vì thế ta có thể cần thêm nhiều cảm biến để giúp rắn hoạt động chính xác hơn .

Hình 4.1 : P1 chuyển động theo khâu đầu , P2 là chuyển động hình sin

Để giảm thiểu tối đa những thay đổi định hƣớng của khâu đầu trong quá trình chuyển động trong khi các khâu liên kết khác tiếp tục đi theo chuyển động hình rắn. Ta có phƣơng trình quan hệ của các góc tƣơng đối của các khâu liền kề nhau

sin( ( 1) ) ( 1 ) i t i i n          sin( ) h h t h h      h

 là góc tƣơng đối của khâu đầu . h và h là góc lệnh pha lớn nhất của khâu đầu . Hai tham số này đảm bảo rằng đầu robot vẫn còn khoảng cách song song với mục tiêu .

Từ trên ta thấy , nhiệm vụ của ta là sử dụng liên kết đầu thiết lập các hƣớng chuyển động thông qua tham số h , điều khiển các liên kết còn lại để tạo ra chuyển động sóng ngang và thông qua chuyển động của cơ thể robot rắn đẩy rắn về phía trƣớc .

4.1.2 Điều khiển động cơ RC servo

4.1.2.1Nguyên lý điều khiển động cơ RC Servo

Động cơ RC servo đƣợc điều khiển dựa vào xung PWM đƣa vào dây tín hiệu. Động cơ RC Servo analog có tần số là 50Hz, mỗi chu kỳ xung là 20ms. Khi độ rộng xung khoảng 1.5ms thì động cơ quay về vị trí giữa. Khi độ rộng xung trong khoảng 1.5 ÷ 2.0ms thì động cơ đạt các vị trí ngƣợc chiều kim đồng hồ và khi độ rộng xung trong khoảng 0.7 ÷ 1.0ms thì động cơ đạt các vị trí cùng chiều kim đồng hồ. Ứng với mỗi độ rộng xung, động cơ sẽ đạt các vị trí chính xác khác nhau. Vị trí này có sai số tùy thuộc loại động cơ.

Hình 4.2 : Giản đồ xung điều khiển động cơ RC servo

4.1.2.2Điều khiển động cơ RC servo trong robot rắn

Vi điều khiển đƣợc sử dụng trong mô hình là pic 18F4431 . Tần số PWM trong phần cứng của vi điều khiển là :

PWM period = [ (PR2+1)] x 4 x Tosc x ( TMR2 prescaler value ) Thanh ghi PR2 là thanh ghi 8 bit có giá trị tối đa là 0xFF(255) , ta có :

Chu kỳ PWM = ( 255+1) x 4 x (1/4000000) x 16 = 0.0041 (s) Tần số PWM = 1/chu kỳ PWM = 1/0.0041 = 244.1 (Hz)

Tần số 244.1 Hz là quá cao so với tần số hoạt động của RC servo ( 50Hz) , ta điều khiển động cơ servo bằng PWM tạo bởi phần mềm . Ta sử dụng bộ định thì TIMER0 để tạo ngắt xuất xung .

Thời gian để timer đếm lên 1 đơn vị đựơc tính bằng công thức 4

Focs

prescaler

T 

4.2 ĐIỀU KHIỂN ĐỊNH HƢỚNG 4.2.1 Bộ điều khiển Fuzzy 4.2.1 Bộ điều khiển Fuzzy

Chuyển động của robot rắn theo phƣơng trình

sin( ( 1) ) ( 1 )

i t i i n

        

(4.1)

Có tham số điều khiển hƣớng là γ. Khi γ=0, robot chạy thẳng. Khi γ>0, robot rẽ trái theo đƣờng tròn và ngƣợc lại γ <0 robot sẽ rẽ phải.

Khi tăng độ lớn của γ sẽ làm giảm bán kính của đƣờng tròn, hay nói cách khác thì càng cao, robot sẽ rẽ càng gấp. Nhƣ vậy, việc chọn hợp lý sẽ là sự kết hợp của tốc độ rẽ và độ ổn định của robot.

Ta sử dụng thuật toán Fuzzy để điều khiển hƣớng do bộ điều khiển Fuzzy có thể sử dụng các biến ngôn ngữ để điều khiển hệ thống phức tạp với cấu trúc bộ điều khiển đơn giản

Tín hiệu đƣa vào bộ điều khiển sẽ là độ sai lệch giữa hƣớng di chuyển của robot và hƣớng mong muốn đến mục tiêu

Hình 4.4 : Mô hình xác định hƣớng của robot Hƣớng của robot đƣợc định nghĩa bởi công thứ

1 1 n i i n      (4.2) Hƣớng mong muốn * là hƣớng từ trọng tâm robot đến mục tiêu

Độ lệch hƣớng giữa hƣớng mong và hƣớng của robot là    *

Để thực hiện giải thuật điều khiển điều khiển định hƣớng cho robot rắn, ta coi mục tiêu sẽ đƣợc phát một nguồn sáng về phía robot. Cảm biến định hƣớng đƣợc sử dụng là quang điện trở. Dùng để xác định góc lệch của robot với mục tiêu. Quang điện trở (LDR) là một cảm biến điện trở nhạy sáng làm bằng vật liệu bán dẫn Cadmium Sulfide (CdS). Bình thƣờng, trở kháng của LDR là rất cao khoảng 1.000.000 Ω , khi đƣợc chiếu sáng với ánh sáng thì trở kháng giảm rất nhiều.

Hình 4.5 : Mạch quang điện trở

Khi mức độ ánh sáng thấp điện trở của LDR là cao. Điều này làm các làm các transistors không dẫn. Do đó LED không sáng.

Tuy nhiên, khi có ánh sáng chiếu lên bề mặt LDR nó phân cực cho transistror đầu tiên dẫn, và do đó transistor thứ 2 dẫn, đèn LED sẽ phát sáng.

Một biến trở đặt sẵn có thể điều chỉnh để tăng hoặc giảm trở kháng của mạch (LDR), trong cách này, nó có thể làm cho độ nhạy của mạch thay đổi

Chọn nguồn sáng là một bóng đèn Rạng Đông Compact 40W đặt cách robot 2 m.Độ rọi sáng của bóng đèn vào khoảng 500Lux. Khi robot hoạt động trong điều kiện bình thƣờng, với các bố trí cảm biến nằm ngang, độ rọi sáng vào hai cảm biến do ánh sáng môi trƣờng tạo ra ở vào khoảng 50Lux. Nhƣ vậy, điện trở của LDR sẽ biến thiên trong khoảng 1÷10 kΩ ( 1 lux là độ rọi có đƣợc của một bề mặt có diện tích 1 mét vuông có thông lƣợng chiếu sang 1 lumen )

Một cặp LDR sẽ đóng vai trò cảm biến ánh sáng để định hƣớng cho robot. Đặt 2 LDRs định vị trí chúng 60° theo hƣớng của khâu đầu để xác định góc tƣơng đối của khâu với nguồn sang thông qua tính hiệu của cƣờng độ sáng. Cặp LDR sẽ cung cấp vị trí nguồn sáng về mạch điều khiển và tạo thành một vòng hồi tiếp. Điểm quan trọng ở đây là ta sử dụng giá trị sai lệch giữa LDR trái và LDR phải, và giảm đƣợc nhiễu do ánh sáng bên ngoài.

Hình 4.7 : Bố trí cảm biến định hƣớng

4.2.1.1Mờ Hóa

Giá trị đầu vào của tập mờ là góc lệch của robot rắn với mục tiêu. Giá trị đầu ra là hệ số là hệ số định hƣớng trong phƣơng trình chuyển động của robot rắn. Sử dụng bộ ADC 10 bit của vi điều khiển, với cảm biến LDR 12mm đã đƣợc chọn, điện áp chân Analog đạt đƣợc trong khoảng 1÷5V. Tín hiệu cảm biến đƣợc đƣa về sẽ nằm trong khoảng [100 1000] và tín hiệu sai lệch (LDR trái – LDR phải) nằm trong khoảng [-900 900].

4.2.1.2Hệ luật mờ

Nếu   0 thì  0 (mục tiêu lệch bên trái – robot rẻ trái) Nếu   0 thì  0 (mục tiêu không lệch – robot đi thẳng) Nếu   0 thì  0 (mục tiêu lệch bên phải – robot rẻ phải)

Tín hiệu vào  biến thiên trong khoảng [-900 900 ], ta cho tín hiệu ra biến thiên trong khoảng [- 600 600 ] .

Sử dụng phƣơng pháp suy diễn MAX – MIN để tạo thành quan hệ mờ và phƣơng pháp MAMDANI để kết hợp các luật hợp thành.

Hình 4.8 : Sơ đồ bộ điều khiển Fuzzy

Hình 4.10 : Sơ đồ Membership Function output Trọng số của từng luật đƣợc tính theo suy diễn Min – Max

1 min( ( ) & ( )) w  Ne Ne (4.3) 2 min( ( ) & ( )) w  Ze Ze (4.4) 3 min( ( ) & ( )) w  Po Po  (4.5) Kết quả là đầu ra γ đƣợc tính theo công thức

1 2 3

( ) max(w w w, , )

  

(4.6)

4.2.1.3Giải mờ

Sử dụng phƣơng pháp trọng tâm (centroid) để giải mờ, tích phân số đƣợc tính gần đúng bằng công thức Simpson

* ( ). ( ) B B y ydy y y dy     (4.7) Hình 4.11: Phƣơng pháp giải mờ theo tọa độ trọng tâm

Bộ ADC của vi xử lý sẽ xác định độ sai lệch giữa 2 cảm biến LDR trái và LDR phải.Góc γ đƣợc xác định theo thuật toán Fuzzy và tính góc quay của mỗi module theo phƣơng trình chuyển động. Từ đó chuyển thành xung để kích cho các động cơ servo quay theo mong muốn

4.3 THIẾT KẾ MẠCH ĐIỆN CHO ROBOT RẮN 4.3.1 Mạch Nguồn cho vi điều khiển trên robot 4.3.1 Mạch Nguồn cho vi điều khiển trên robot

Sử dụng loại pin sạc 860mA nguồn 7.4V

Hình 4.13 : Pin sài cho mô hình robot

Tất cả các module của robot điều sử dụng 1 nguồn chung từ pin . Riêng vi điều khiển dùng IC LM7805 để chuyển thành điện áp 5V cho mạch từ pin .

Hình 4.14: Mạch nguồn cho vi điều khiển trên robot

4.3.2 Mạch cảm biến xác định hƣớng

Các cảm biến sẽ đƣợc mắc nối tiếp với một biến trở 5KΩ làm cầu chia áp cho chân Analog AN0 và AN1 của vi điều khiển. Sự biến thiên cƣờng độ sáng làm biến thiên điện áp vào chân Analog. Giá trị điện trở của LDR biến thiên từ 1÷10KΩ nên ta đặt giá trị biến trở khoảng 5KΩ. Điện áp vào chân Analog sẽ biến thiên từ 1÷5V

Hình 4.15 : Sơ đồ mạch định hƣớng LDR

4.3.3 Mạch vi điều khiển

Sử dụng vi điều khiển PIC18F4431 để điều khiển robot rắn.Với các chân RB0-RB4 đƣợc nối với chân tính hiệu của động cơ RC servo. Và hai chân Analog AN0 và AN1 đƣợc nối với cảm biến quang (LDR).

4.3.4 Moduel các động cơ và Led báo tín hiệu của động cơ

Hình 4.17: Module nối tới các động cơ

4.4 LƢU ĐỒ ĐIỀU KHIỂN ROBOT RẮN

Vi điều khiển sẽ đọc tín hiệu Analog từ hai kênh AN0 và AN1 từ đây tính đƣợc góc lệch giữa robot và mục tiêu.

Dùng Fuzzy để tính các góc γ và tính các góc cho từng động cơ.Sau đó chuyển các giá trị góc thành các giá trị xung và xuất ra điều khiển các động cơ điều khiển robot chuyển động tới mục tiêu

5. CHƢƠNG V : MÔ PHỎNG VÀ THỰC NGHIỆM 5.1 CÁC THÔNG SỐ MÔ PHỎNG 5.1 CÁC THÔNG SỐ MÔ PHỎNG

Số khâu n=6 Khối lƣợng của khâu m=0,4 (kg)

Chiều dài khâu l=0,24m Hệ số Kp của servo Kp= 100 Hệ số α=π/6 (rad) Hệ số Ki của servo Ki = 10 Góc lệch pha β=π/3(rad) Hệ số Kd của servo Kd = 10 Vận tốc góc ω=1(rad/s) Moment quán tính I=ml2/3 (kg.m2)

5.2 MÔ PHỎNG VỚI SIM MECHANICS 5.2.1 Xây dựng mô hình mô phỏng 5.2.1 Xây dựng mô hình mô phỏng

Mỗi khâu đƣợc nối với nhau bởi một khớp quay có gắn động cơ servo, robot có ba bậc tự do chuyển động so với đất gồm hai bậc tịnh tiến theo x, y và một bậc quay theo z.

Mỗi khâu sẽ chịu lực tác động của lực ma sát (khối Friction) . Mô hình lực ma sát nhớt nhƣ sau

Hình 5.2 : Mô hình lực ma sát

Hình 5.3 : Mô hình động cơ Servo

5.2.2 Xây dựng mô hình mục tiêu

Ta xây dựng 2 mục tiêu có tọa độ khác nhau để làm mục tiêu cho robot rắn di chuyển.Đầu tiên robot sẽ di chuyển đến mục tiêu thứ nhất và sau đó sẽ tiếp đến mục tiêu thứ hai

Hình 5.4 : Mô hình 2 mục tiêu

5.2.3 Bộ điều khiển của robot

Bộ điều khiển trung tâm sẽ quyết định hình dạng của robot tùy theo mục đích điều khiển (điều khiển hƣớng để bám mục tiêu, hoặc điều khiển tốc độ).Tùy vào mục đích điều khiển bộ điều khiển sẽ cho ra các giá trị để động cơ servo thực thi. Động cơ servo sẽ nhận tín hiệu góc tham chiếu từ bộ điều khiển để cấp moment vào các khớp quay. Do đó hình dạng robot sẽ chạy theo mong muốn của bộ điều khiển.

Ta xây dựng bộ điều khiển Fuzzy để robot di chuyển bám theo mục tiêu

Tín hiệu đƣa vào bộ điều khiển sẽ là độ sai lệch giữa hƣớng di chuyển của robot và hƣớng mong muốn đến mục tiêu .

Tín hiệu vào  biến thiên trong khoảng [-π/2 π/2 ], ta cho tín hiệu γ biến thiên trong khoảng [- 1 1 ] .

Hình 5.6 : Membership Function input

Hình 5.8 : Bộ Điều khiển Fuzzy trung tâm và các tín hiệu điều khiển

5.2.4 Kết quả mô phỏng

Cho robot di chuyển đến mục tiêu thứ nhất có tọa độ [17 17] và mục tiêu thứ 2 có tọa độ [20 25] Kết quả robot đi qua cả 2 mục tiêu

Chuẩn bị qua mục tiêu thứ nhất Qua mục tiêu thứ nhất

Chuẩn bị qua mục tiêu thứ 2 Qua mục tiêu thứ 2

Hình 5.10 : Quá trình di chuyển qua 2 mục tiêu của robot

Việc điều khiển định hƣớng bằng Fuzzy Logic đã thực hiện đƣợc. Tuy nhiên,tọa độ trọng tâm