Luận văn thạc sĩ Kỹ thuật điều khiển và tự động hóa: Điều khiển cân bằng robot một bánh

NHIỆM VỤ VÀ NỘI DUNG:  Thành lập phương trình toán mô tả hệ robot một bánh  Xây dựng thuật toán điều khiển LQR và điều khiển trượt để giữ cân bằng robot một bánh  Mô phỏng hệ robot m

Tính cấp thiết của luận văn

Con người đã mơ ước từ lâu về các trợ lý người máy cá nhân, tạo ra để thực hiện mọi nhiệm vụ của mình Một trong những yêu cầu như vậy là tạo ra các robot có kích thước và hình dạng tương tự như một con người Nhiều robot hiện có liên quan đến việc sử dụng ba hoặc bốn bánh xe, thiết lập bánh xe cơ sở đủ lớn để robot có thể đứng thẳng đứng Tuy nhiên, dựa vào kích thước của bánh xe cơ sở sẽ giới hạn việc thực hiện các robot, như là bánh xe cơ sở phải nhỏ hơn đáng kể so với chiều cao hình dạng giống như người Vì vậy, chỉ cần một thay đổi nhỏ ở vị trí trọng tâm là đủ để làm cho robot trở nên không ổn định Điều này có thể được sửa chữa bằng cách hạ thấp trọng tâm của robot để giảm sự bất ổn định này Tuy nhiên, điều này thường đi kèm với chi phí đáng kể thêm vào robot Ngoài ra, do sự bất ổn định của robot, nên tốc độ mà các robot có thể di chuyển thường bị hạn chế vì quán tính của robot làm cho nó lật

Robot một bánh là một robot trong đó sử dụng các khái niệm về sự ổn định động lực học đứng thẳng và cân bằng trên quả bóng Điều này liên quan đến việc sử dụng lý thuyết điều khiển để di chuyển bóng và cân bằng, hơn là dựa vào trọng lực Hy vọng rằng với sự phát triển của robot một bánh, và công nghệ liên quan, các robot có thể hành động như con người, hỗ trợ được con người.

Một số công trình nghiên cứu trên thế giới

Khái niệm về robot một bánh thì khá đơn giản Nó bao gồm một quả bóng và một robot đứng giữ cân bằng trên quả bóng Robot có thể cân bằng và lái bóng làm cho nó di chuyển Hơn nữa, robot có thể di chuyển dọc theo mặt nghiêng và lái bóng mà nó không bị đổ

Bắt đầu là robot sử dụng một bánh xe duy nhất hình cầu đã được nghiên cứu

Robot này được xây dựng và thử nghiệm tại đại học Carnegie Mellon University (CMU) trong năm 2005 và được đưa ra tên là Ballbot

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 10

Hình 1 Ballbot đầu tiên của Carnegie Mellon University, USA năm 2005

Robot thứ hai phát triển tại đại học Tohoku Gakuin University (TGU) trong năm 2008, tương tự nhưng sử dụng hệ thống cơ khí phức tạp, nhưng có giải pháp hình dáng thanh lịch

Hình 2 Ballbot của Tohoku Gakuin University, Japan năm 2008 Robot thứ ba phát triển tại đại học ETH Zurich trong năm 2010, tương tự nhưng có những sự cải tiến đáng kể đặc biệt là kiểu dáng bặt mắt

Hình 3 Ballbot của ETH Zurich, Switzerland năm 2010

Ngoài ra, một robot được xây dựng bằng cách sử dụng NXT Lego Mindstorms đã được sản xuất bởi Yamamoto (2009) Robot này hoạt động ổn định và có khả năng di chuyển bằng thiết bị điều khiển từ xa

Hình 4 Ballbot của LEGO NXT

Giới thiệu đề tài

Sự phát triển của robot đứng trên bóng yêu cầu xuất phát từ động lực học, cấu trúc của từng robot, thiết kế và thực hiện bộ điều khiển để ổn định robot

Nguồn gốc của phương trình chuyển động hệ thống robot thực hiện bằng cách sử dụng phương trình Lagrangian với việc tiếp cận về một mô hình robot đơn giản, từ đây ta thực hiện tuyến tính hóa phương trình chuyển động của hệ thống robot LEGO NXT Ballbot đã xây dựng được hoàn thành với thuật toán điều khiển LQR (Linear Quadratic Regulator) Robot này sử dụng cơ cấu lái bóng – chuột máy tính đơn giản, trong đó bóng được vận hành bởi bánh xe hướng về phía trực giao của quả bóng Thiết

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 12 kế robot một bánh hoàn chỉnh đang diễn ra, và được phát triển dựa trên LEGO NXT

Một bộ điều khiển cho hệ thống robot một bánh đã được phát triển bằng cách sử dụng bộ điều khiển hồi tiếp trạng thái đầy đủ, dựa trên một bộ điều khiển cho robot hai bánh tự cân bằng Bộ điều khiển này đã được thực hiện trên LEGO NXT Ballbot, nhưng vẫn chưa được thành công trong việc cân bằng robot Hơn nữa, việc kiểm tra và sửa đổi bộ điều khiển sẽ được thực hiện để đạt được một robot ổn định Khi đạt được mục đích, bộ điều khiển sẽ được phát triển thêm để cho phép điều khiển bằng cách sử dụng một bộ điều khiển cầm tay

Khái niệm về robot một bánh đơn giản đó là một robot tự cân bằng trên một quả bóng Do đó, bóng sẽ hoạt động như các bánh xe hình cầu, cho phép các robot di chuyển theo một vài hướng Trái ngược với robot truyền thống, robot một bánh dựa vào trọng tâm của một khối và dựa trên bánh xe để cân bằng

Luận văn này nhằm thiết kế và xây dựng robot một bánh, sự phát triển của robot một bánh là một hệ thống tổng thể, bao gồm cả nguồn gốc của phương trình chuyển động của robot, thiết kế, thực hiện bộ điều khiển, phần mềm liên quan đến sự cân bằng robot và thử nghiệm hệ thống.

Phương pháp nghiên cứu

Phương pháp nghiên cứu được áp dụng để thực hiện luận văn này là phân tích lý thuyết, mô phỏng trên máy tính và áp dụng kết quả vào thực nghiệm

 Phân tích lý thuyết: là nghiên cứu các lý thuyết cơ sở liên quan đến việc thiết kế thuật toán điều khiển LQR, thuật toán điều khiển trượt, dựa trên cơ sở các lý thuyết này sẽ phân tích cho đối tượng cụ thể được sử dụng trong luận văn

 Mô phỏng trên máy tính: sau khi đã xây dựng xong các thuật toán điều khiển dưới dạng lý thuyết, sẽ tiến hành mô phỏng trên Matlab để kiểm chứng sự hoạt động của thuật toán điều khiển, trên kết quả mô phỏng sẽ rút ra các nhận xét về thuật toán và kinh nghiệm khi áp dụng thuật toán điều khiển vào đối tượng thực

 Áp dụng kết quả vào thực nghiệm: để kiểm chứng tính đúng đắn của các thuật toán điều khiển thiết kế bằng lý thuyết, sẽ nhúng thuật toán điều khiển xuống vi

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 13 xử lý và các khâu chấp hành để điều khiển đối tượng thực, kết quả thực tế giúp ta nhìn nhận chính xác cho chất lượng của thuật toán điều khiển đã được thiết kế

Mục tiêu đề tài

Hệ thống robot hoạt động ổn định ngoài phần cơ cấu hợp lý và phương trình toán hệ thống sát với thực tế, robot còn phải được thiết kế thuật toán điều khiển thích hợp Đối với hệ robot một bánh tự cân bằng thì quá trình điều khiển có thể được chia thành ba phần: điều khiển góc nghiêng của hệ robot so với phương thẳng đứng, điều khiển vị trí di chuyển của robot so với phương nằm ngang và cuối cùng là điều khiển góc xoay của robot Bản thân hệ robot là hệ không ổn định, nên quá trình điểu khiển sẽ gặp rất nhiều khó khăn khi phải thực hiện điều khiển ba thành phần: góc nghiêng, vị trí và góc xoay cùng một thời điểm Tuy nhiên, trong phạm vi đề tài chỉ thực hiện việc điều khiển góc nghiêng và vị trí robot

Kết hợp những ưu điểm của các phương pháp điều khiển tuyến tính và khả năng tính toán mạnh mẽ của vi điều khiển ARM-STM32F4-DISCOVERY, luận văn này trình bày cách giải quyết bài toán trên cơ sở lý thuyết điều khiển tuyến tính là: điều khiển LQR và điều khiển phi tuyến là: điều khiển trượt Sau đó, nhúng các thuật toán này trên ARM-STM32F4-DISCOVERY để điều khiển đối tượng thực, so sánh kết quả điều khiển khi sử dụng các thuật toán này Các vấn đề cơ bản trong luận văn gồm có:

 Nghiên cứu lý thuyết điều khiển LQR

 Nghiên cứu lý thuyết điều khiển trượt

 Áp dụng phương pháp điều khiển LQR cho đối tượng robot một bánh

 Áp dụng phương pháp điều khiển trượt cho đối tượng robot một bánh

 Mô phỏng kết quả điều khiển đối tượng trên Matlab

 Xây dựng bộ lọc Kalman ứng dụng lọc nhiễu trong cảm biến

 Kiểm chứng các kết quả mô phỏng bằng thực nghiệm điều khiển đối tượng thực bằng các thuật toán điều khiển

MÔ TẢ TOÁN HỌC HỆ ROBOT MỘT BÁNH

Giới thiệu

Một robot một bánh vật lý là một thiết bị tương đối đơn giản, đòi hỏi một vài thành phần Các chức năng cơ bản của robot một bánh đó là để cân bằng thẳng đứng trên một quả bóng, đòi hỏi phải có một quả bóng, động cơ để lăn bóng và cơ cấu lái bóng

Nội dung nghiên cứu được trình bày một cách chi tiết công việc trong ba lĩnh vực phát triển của robot Đầu tiên là các lời giải của các mô hình toán đó là sự cần thiết để kiểm soát robot, thứ hai là xây dựng các phần cứng vật lý Phần cuối cùng là lý thuyết điều khiển được sử dụng để duy trì cân bằng robot

Robot một bánh tự cân bằng là một lĩnh vực khó, nó liên quan đến rất nhiều yếu tố khác nhau để điều khiển và kiểm soát được robot và chỉ có một số tài liệu hạn chế tồn tại về chủ đề này Tuy nhiên, sẽ được thảo luận trong các phần sau đây, vấn đề robot có thể được mô hình hóa như hai hệ thống con lắc ngược hoạt động độc lập

Như vậy, ta sẽ xem xét lại nguồn gốc động lực học và điều khiển đã được mở rộng giải pháp cho các vấn đề con lắc ngược.

Mô hình cơ khí robot một bánh

Hệ thống cơ khí robot một bánh được thiết kế bằng phần mềm Solid Work Từ cơ sở kích thước banh bowling ta xây dựng được hệ thống cơ khí cho robot một bánh với kích thước hoàn chỉnh Hệ thống cơ khí robot một bánh được thực hiện trong đề tài gồm hai phần chính:

 Cơ cấu hệ thống lái banh bowling

 Hệ thống truyền động từ động cơ tới con lăn

Robot một bánh sử dụng hai động cơ servo DC truyền động tới con lăn thông qua bộ phận bánh đai và dây đai, con lăn sẽ truyền động ma sát với quả banh để điều khiển giữ thăng bằng cho robot Hai encoder dùng xác định vị trí và vận tốc di chuyển của robot theo hai trục x, y trong mặt phẳng hoạt động

Hình 5 Mô hình cơ khí robot một bánh

2.3 Phần cứng cho hệ robot một bánh

Mạch điều khiển trung tâm sử dụng vi điều khiển STM32F4-DISCOVERY làm nhiệm vụ nhúng thuật toán điều khiển LQR và điều khiển trượt, thu thập tín hiệu từ cảm biến gia tốc Accelerometer, cảm biến vận tốc góc Gyroscope, Encoder Từ đó xuất xung PWM điều khiển động cơ DC Servo

Hình 6 Vi điều khiển ARM-STM32F4-DISCOVERY

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 16 Động cơ sử dụng là động cơ DC Servo 68W, 24V, tốc độ 2500 vòng/phút

Encoder kèm theo 400 xung/vòng, nguồn nuôi encoder 5V

Hình 7 Động cơ DC Servo

Hai mạch cầu H được sử dụng để điều khiển hai động cơ DC Servo Đây là mạch cầu H dùng IC kích FET chuyên dụng IR2184, FET sử dụng IRF3205 nên cho dòng cao

 Có Led báo nguồn cho mạch

 Có Led báo chiều động cơ

 Board được thiết kế nhỏ gọn

IMU tích hợp các cảm biến gia tốc Accelerometer và cảm biến vận tốc góc Gyroscope dùng để đo góc nghiêng và vận tốc góc nghiêng robot

Hình 9 IMU - 9 Degrees of Freedom - MPU-9150 Breakout

 Dãi điện áp cung cấp 2.4V–3.46V

 Tiêu thụ năng lượng thấp

 Cảm biến vận tốc góc Gyroscope 3 trục với độ nhạy lên đến 131

LSBs/dps và có nhiều tầm đo ±250, ±500, ±1000, and ±2000dps

 Cảm biến gia tốc Accelerometer 3 trục có nhiều tầm đo ±2g, ±4g, ±8g and ±16g

Mạch nguồn cho robot sử dụng pin ngoài cung cấp các nguồn 5V cho vi điều khiển STM32F4-DISCOVERY và 24V cho hai động cơ DC Servo

Hình 10 Mạch cấp nguồn cho robot

Toàn bộ quá trình điều khiển có thể mô tả trong hình vẽ:

Hình 12 Cấu trúc phần cứng robot một bánh tự cân bằng

Cơ cấu bộ lái bao gồm các tương tác có ma sát giữa các động cơ lái và quả bóng rất quan trọng cho sự thành công của robot một bánh Có các phương pháp khác nhau để thiết kế thành phần này

Hình 13 Cơ cấu bộ lái của CMU

Hình 14 Cơ cấu bộ lái của TGU

Hinh 15 Cơ cấu bộ lái của ETH

Robot CMU có lẽ là phương pháp cho cơ cấu lái đơn giản nhất, hiệu quả và cũng được dùng trong luận văn này Nó sử dụng cơ cấu lái “ổ chuột-bóng” Hai con lăn lái vuông góc được sử dụng, mỗi con lăn được lái bởi một động cơ servo DC thông qua một vành đai Đối diện mỗi con lăn lái là hai con lăn trượt chạy không tải, con lăn được sử dụng để xác định vị trí các quả bóng Lợi thế của việc sử dụng hệ thống này là hai con lăn lái trực giao với nhau nên kiểm soát có thể được đơn giản hóa trong hai mặt phẳng độc lập Tuy nhiên, mức độ trượt phải diễn ra giữa bóng và các con lăn khi được điều khiển bởi những con lăn trực giao, trong khi đó tại một thời điểm, mức độ ma sát là cần thiết giữa các con lăn lái và bóng Như vậy, một ma sát cao và ma sát thấp tương tác quả bóng lăn là cần thiết cùng một lúc, có thể là vấn đề khó khăn để hoàn thành

Hình chiếu đứng Hình chiếu bằng

Hình 16 Cơ cấu bộ lái chi tiết

Tính chất của quả bóng phụ thuộc vào cơ cấu lái Các robot CMU ban đầu được sử dụng một lớp vỏ thép có đường kính 200 mm được phủ một urethane 3,2 mm bên lớp ngoài Nhưng bóng bị mòn, bóng này sau đó đã được thay thế bằng nhôm nhẹ hơn vỏ đường kính 190.5mm và một lớp 12.7mm urethane Điều này sẽ làm tăng tính giảm xóc trong hệ thống, mặc dù không có hiện tượng gì ảnh hưởng đến hiệu suất của robot

Các robot TGU sử dụng một quả bóng bowling được phủ cao su để tăng độ bám

Bóng rổ bước đầu đã được thử nghiệm trong Robot TGU và mặc dù nó có thể duy trì cân bằng nhưng nó không ổn định Những quả bóng bowling bọc cao su đã được sử dụng, nó cứng hơn và cung cấp hiệu suất tốt hơn

Các tính chất của bóng sử dụng trong robot một bánh phụ thuộc rất lớn vào cơ cấu lái và vì thế hai cấu trúc robot khác nhau sẽ có hai quả bóng rất khác nhau

2.6 Phương pháp Lagrange Động lực học của mô hình robot một bánh có thể rút ra bằng cách sử dụng phương pháp Lagrange Phương pháp này xác định đại lượng L Lagrange ( , , ) q q t , trong đó q là tọa độ tổng quát của hệ thống và t là thời gian Đại lượng L chỉ đơn giản là tổng các động năng của hệ thống T trừ đi thế năng V, được thể hiện trong phương trình:

Việc lựa chọn tọa độ q là tùy ý, với ràng buộc rằng chúng hoàn toàn phải xác định hệ thống Tuy nhiên, sẽ là thuận lợi khi sử dụng tọa độ trực tiếp liên quan đến đại lượng đo lường như các góc nghiêng thân hình và góc của quả banh Hình sau cho thấy những ví dụ về tọa độ lựa chọn khi phân tích động học xét trong một mặt phẳng của robot hoặc hệ thống tương tự

Hình 17 Các hệ tọa độ cho hệ thống cân bằng robot một bánh Phương trình cơ sở chuyển động, các phương trình Euler Lagrange : i i i d L L dt q q F

Trong đó F i là lực tổng quát Điều này dẫn đến phương trình Lagrange cho mỗi tọa độ q i theo sau

Trong đó: M(q) ma trận khối lượng, G(q) ma trận trọng lực, F vecto lực tổng quát Phương trình này là phương trình chuyển động của hệ thống mà có thể là phi tuyến

2.7 Cơ sở để thành lập phương trình toán

Kết quả hiện tại phù hợp nhất là phương trình cơ sở chuyển động robot một bánh được xây dựng tại CMU Phương trình cơ sở này sử dụng mô hình đơn giản và phương pháp Lagrange

Kết quả đạo hàm trong các phương trình chuyển động bao gồm thêm các ảnh hưởng của độ nhớt ma sát D(q)

Kết quả này cung cấp các phương trình chuyển động của robot một bánh Một hạn chế lớn của phương trình này là không xem xét các phương trình toán động cơ Động lực học robot một bánh trong một mặt phẳng sẽ phải bao gồm các ảnh hưởng của động cơ bằng cách thêm động năng động cơ dựa trên các phương trình động cơ DC t b

Trong đó:  là momen động cơ, Kt là hằng số moment động cơ, i là dòng điện động cơ, v là điện áp động cơ,  là vận tốc góc động cơ, L là độ tự cảm, Kb là hằng số emf, R là điện trở động cơ.

Trong ứng dụng này, các độ từ cảm động cơ L là không đáng kể và giả định là bằng 0

2.8 Phương trình toán mô tả hệ robot một bánh

Cơ cấu bộ lái

Cơ cấu bộ lái bao gồm các tương tác có ma sát giữa các động cơ lái và quả bóng rất quan trọng cho sự thành công của robot một bánh Có các phương pháp khác nhau để thiết kế thành phần này

Hình 13 Cơ cấu bộ lái của CMU

Hình 14 Cơ cấu bộ lái của TGU

Hinh 15 Cơ cấu bộ lái của ETH

Robot CMU có lẽ là phương pháp cho cơ cấu lái đơn giản nhất, hiệu quả và cũng được dùng trong luận văn này Nó sử dụng cơ cấu lái “ổ chuột-bóng” Hai con lăn lái vuông góc được sử dụng, mỗi con lăn được lái bởi một động cơ servo DC thông qua một vành đai Đối diện mỗi con lăn lái là hai con lăn trượt chạy không tải, con lăn được sử dụng để xác định vị trí các quả bóng Lợi thế của việc sử dụng hệ thống này là hai con lăn lái trực giao với nhau nên kiểm soát có thể được đơn giản hóa trong hai mặt phẳng độc lập Tuy nhiên, mức độ trượt phải diễn ra giữa bóng và các con lăn khi được điều khiển bởi những con lăn trực giao, trong khi đó tại một thời điểm, mức độ ma sát là cần thiết giữa các con lăn lái và bóng Như vậy, một ma sát cao và ma sát thấp tương tác quả bóng lăn là cần thiết cùng một lúc, có thể là vấn đề khó khăn để hoàn thành

Hình chiếu đứng Hình chiếu bằng

Hình 16 Cơ cấu bộ lái chi tiết

Bóng

Tính chất của quả bóng phụ thuộc vào cơ cấu lái Các robot CMU ban đầu được sử dụng một lớp vỏ thép có đường kính 200 mm được phủ một urethane 3,2 mm bên lớp ngoài Nhưng bóng bị mòn, bóng này sau đó đã được thay thế bằng nhôm nhẹ hơn vỏ đường kính 190.5mm và một lớp 12.7mm urethane Điều này sẽ làm tăng tính giảm xóc trong hệ thống, mặc dù không có hiện tượng gì ảnh hưởng đến hiệu suất của robot

Các robot TGU sử dụng một quả bóng bowling được phủ cao su để tăng độ bám

Bóng rổ bước đầu đã được thử nghiệm trong Robot TGU và mặc dù nó có thể duy trì cân bằng nhưng nó không ổn định Những quả bóng bowling bọc cao su đã được sử dụng, nó cứng hơn và cung cấp hiệu suất tốt hơn

Các tính chất của bóng sử dụng trong robot một bánh phụ thuộc rất lớn vào cơ cấu lái và vì thế hai cấu trúc robot khác nhau sẽ có hai quả bóng rất khác nhau.

Phương pháp Lagrange

Động lực học của mô hình robot một bánh có thể rút ra bằng cách sử dụng phương pháp Lagrange Phương pháp này xác định đại lượng L Lagrange ( , , ) q q t , trong đó q là tọa độ tổng quát của hệ thống và t là thời gian Đại lượng L chỉ đơn giản là tổng các động năng của hệ thống T trừ đi thế năng V, được thể hiện trong phương trình:

Việc lựa chọn tọa độ q là tùy ý, với ràng buộc rằng chúng hoàn toàn phải xác định hệ thống Tuy nhiên, sẽ là thuận lợi khi sử dụng tọa độ trực tiếp liên quan đến đại lượng đo lường như các góc nghiêng thân hình và góc của quả banh Hình sau cho thấy những ví dụ về tọa độ lựa chọn khi phân tích động học xét trong một mặt phẳng của robot hoặc hệ thống tương tự

Hình 17 Các hệ tọa độ cho hệ thống cân bằng robot một bánh Phương trình cơ sở chuyển động, các phương trình Euler Lagrange : i i i d L L dt q q F

Trong đó F i là lực tổng quát Điều này dẫn đến phương trình Lagrange cho mỗi tọa độ q i theo sau

Trong đó: M(q) ma trận khối lượng, G(q) ma trận trọng lực, F vecto lực tổng quát Phương trình này là phương trình chuyển động của hệ thống mà có thể là phi tuyến

Cơ sở để thành lập phương trình toán

Kết quả hiện tại phù hợp nhất là phương trình cơ sở chuyển động robot một bánh được xây dựng tại CMU Phương trình cơ sở này sử dụng mô hình đơn giản và phương pháp Lagrange

Kết quả đạo hàm trong các phương trình chuyển động bao gồm thêm các ảnh hưởng của độ nhớt ma sát D(q)

Kết quả này cung cấp các phương trình chuyển động của robot một bánh Một hạn chế lớn của phương trình này là không xem xét các phương trình toán động cơ Động lực học robot một bánh trong một mặt phẳng sẽ phải bao gồm các ảnh hưởng của động cơ bằng cách thêm động năng động cơ dựa trên các phương trình động cơ DC t b

Trong đó:  là momen động cơ, Kt là hằng số moment động cơ, i là dòng điện động cơ, v là điện áp động cơ,  là vận tốc góc động cơ, L là độ tự cảm, Kb là hằng số emf, R là điện trở động cơ.

Trong ứng dụng này, các độ từ cảm động cơ L là không đáng kể và giả định là bằng 0

Phương trình toán mô tả hệ robot một bánh

Phương trình toán là cần thiết khi sử dụng kỹ thuật điều khiển để tính được độ lợi hồi tiếp và cho phép mô phỏng Phương pháp Lagrange được sử dụng để lấy được các phương trình chuyển động Những phương trình này được tuyến tính và được sử dụng để thiết kế bộ điều khiển

2.8.1 Ký hiệu và định nghĩa

Nguồn gốc của phương trình toán dựa vào một số tham số vật lý của robot

Tham số vật lý này được quy định như sau :

Hình 18 Hệ tọa độ trong mặt phẳng xz

 R b : Bán kính của quả bóng

 L: Khoảng cách từ trung tâm khối lượng của thân robot đến tâm của quả bóng

 M B : Khối lượng của thân robot

 M b : Khối lượng của quả bóng

 I b : Moment quán tính của quả bóng

 I Bx : Moment quán tính của thân robot theo trục x

 I By : Moment quán tính của thân robot theo trục y

 I M : Moment quán tính của động cơ theo trục quay của chúng

  x : Góc của thân robot so với trục thẳng đứng trong mặt phẳng xz

  x : Góc của động cơ so với thân hình robot trong mặt phẳng xz

  y : Góc của thân robot so với trục thẳng đứng trong mặt phẳng yz

  y : Góc của động cơ so với thân hình robot trong mặt phẳng yz

 n: Tỉ số truyền của động cơ đến quả bóng bao gồm cả hướng

  Bb : Hệ số ma sát giữa thân và bóng

  Bg : Hệ số ma sát giữa thân và mặt đất

 K b : Hệ số sức điện động của động cơ

 K t : Hằng số mô men xoắn của động cơ

 R m : Điện trở phần ứng của động cơ

Các phương trình chuyển động của robot trong một mặt phẳng sử dụng phương pháp Lagrange Chỉ có các phương trình chuyển động của mặt phẳng xz được thành lập, còn mặt phẳng yz có phương trình toán giống nhau

Biểu diễn phương pháp Lagrange bắt đầu bằng cách xác định động năng và thế năng của thân robot, bóng và động cơ, như sau:

Vị trí z b 0 Vận tốc z b 0 Động năng tịnh tiến của quả bóng:

 Động năng xoay của quả bóng:

Thế năng của quả bóng: V b = 0

Vị trí x B x b Lsin( ) x Vận tốcx B x b  L cos(  x ) x

Vị trí z B Lcos( ) x Vận tốcz B  L sin(  x ) x Động năng tịnh tiến của thân robot:

 Động năng xoay của thân robot:

Thế năng của thân robot:

Góc   x  x Vận tốc góc   x  x Động năng xoay của động cơ:

Lagrange thì được tính: linb linB rotb rotB rotm b B

Phương trình Euler-Lagrange: i i i d L L dt q q F

Kết quả phương trình toán được xác định:

M ( - (K )/R + (K K )/R - LM R n sin( )) + M (M R sin( ) - + 9.81LM sin( ))

M ( - (K )/R + (K K )/R - M R n sin( )) + M (LM R sin( ) - + 9.81LM sin( ))

Phương trình phi tuyến được tuyến tính hóa xung quanh điểm làm việc tĩnh

4,1 B M b B b b b B b x x gLM I I n M nR M nR LM nR

2.8.4 Phương trình trạng thái đầy đủ

Tương tự, thực hiện theo các bước trên cho mặt phẳng yz ta nhận được kết quả phương trình trạng thái đầy đủ như sau: x  Ax  Bu

  x y x y uu u    v v Tín hiệu điều khiển u bao gồm hai điện áp cấp cho hai động cơ

XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN

Lý thuyết thuật toán điều khiển LQR

Xét hệ thống với biểu diễn trạng thái x  Ax  Bu (3.1)

Vấn đề: Xác định luật điều khiển hồi tiếp trạng thái u Kx (3.2) để đưa vectơ trạng thái x về 0 sao cho hàm mục tiêu J đạt cực tiểu

Q là ma trận đối xứng xác định không âm R là ma trận đối xứng xác định dương Do Q đối xứng và xác định không âm ta luôn có thể tìm được ma trận C, sao cho

QC C T (3.4) Giả thiết ( , )A B ổn định hóa được và ( , )A C phân tách được

Luật điều khiển tối ưu được xác định bởi (3.2) với

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 32 P là ma trận (nxn) đối xứng, xác định dương, là nghiệm của phương trình đại số

A P PA Q PBR B P     (3.6) Nghiệm của phương trình Riccati: Định nghĩa

       (3.7) Ma trận H (2nx2n) có các tính chất sau:

H có n trị riêng với phần thực dương và n trị riêng với phần thực âm

  là trị riêng và vectơ riêng bên phải của H, thì và p 2 T p 1 T  là trị riêng và vectơ riêng bên trái của H

        (3.8) Gọi là ma trận đường chéo (nxn) chứa các trị riêng với phần thực âm của H và X là ma trận (2nxn) của vectơ riêng tương ứng

  (3.9) Với X 1 vàX 2 là các ma trận (nxn) Ta có:

Nghiệm của phương trình Riccati (3.6) được cho bởi:

Chứng minh: từ (3.10) ta có

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 33 từ (3.10) ta có

Ta có các tính chất sau:

P là ma trận đối xứng, xác định dương

Với luật điều khiển (3.2) , (3.5), hệ thống hồi tiếp ổn định tiệm cận

Thật vậy từ (3.6) , ta có

 P là nghiệm của phương trình Lyapunov

 P đối xứng và xác định dương Ma trận trạng thái của hệ thống vòng kín được cho bởi

(3.14)là phân hoạch trị riêng của ma trận trạng thái của hệ thống vòng kín: A BK có các trị riêng là các phần tử của  hệ thống vòng kín ổn định

Bài toán ổn định hóa cũng có thể được đặt ra như sau: xét hệ thống với biểu diễn trạng thái x Ax Bu y Cx

 (3.15) Định nghĩa hàm mục tiêu J

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 34 Với R là ma trận đối xứng, xác định dương Giả thiết ( , )A B ổn định hóa được và ( , )A C phân tách được Luật điều khiển hồi tiếp trạng thái cực tiểu hóa J được xác định bởi (3.2) , (3.5), (3.6) với QC C T

Thiết kế thuật toán điều khiển LQR

Trong chương trước đã tuyến tính hóa hệ phi tuyến robot một bánh quanh điểm cân bằng cho ra phương trình trạng thái sau : x  Ax  Bu

Ma trận độ lợi K được xác định bằng Matlab như sau :

Thông số hệ robot một bánh :

Mô phỏng thuật toán điều khiển LQR

Sơ đồ mô phỏng thuật toán điều khiển LQR thực hiện trong Matlab/Simulink như sau :

Hình 19 Sơ đồ mô phỏng thuật toán điều khiển LQR

 Trường hợp 1 : Khi góc nghiêng ban đầu x 18

Hình 20 Đáp ứng ngõ ra

Dap ung ngo ra theta x th e ta _ _ x (r a d ) t (s)

Dap ung ngo ra theta y th e ta _ _ y (r a d ) t (s)

Hình 21 Tín hiệu điều khiển

Kết luận

Kết quả mô phỏng cho thấy thuật toán điều khiển LQR có thể điều khiển tốt hệ thống, đáp ứng của hệ thống tương đối nhanh, ít vọt lố và khá ổn định

XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN TRƢỢT CHO HỆ ROBOT MỘT BÁNH 4.1 Giới thiệu

Lý thuyết thuật toán điều khiển trượt

Đối tượng điều khiển: Xét hệ thống phi tuyến biểu diễn bởi phương trình vi phân

(y, y, y, y ) (y, y, y, y ) n n n y  f  g  u (4.1) Đặt x 1  y , x 2  y x , 3  y , , x n  y ( n  1) (4.2) Ta được biểu diễn trạng thái

Vấn đề: Xác định tín hiệu điều khiển u sao cho tín hiệu ra y bám theo tín hiệu đặt r Mặt trượt: Định nghĩa tín hiệu sai lệch e   y r (4.4) Và hàm S S  e ( n  1)  a n  2 e ( n  2)   a e a e 1  0 (4.5) Trong đó a a 0 , , , 1 a n  3 ,a n  2 là các hệ số được chọn trước sao cho đa thức đặc trưng của phương trình vi phân sau Hurwitz (có tất cả các nghiệm với phần thực âm)

2 1 0 0 n n e   a n  e    a e a e   (4.6) Khi đó nếu S = 0 thì sai lệch e  0khi t 

Thay (4.4) và (4.2) vào (4.5), ta được

S  x  a  x    a x  a x  r   a  r    a r  a r (4.7) Phương trình S = 0 xác định một mặt cong trong không gian n chiều gọi là mặt trượt (sliding surface)

Vấn đề: xác định luật điều khiển u để đưa các quỹ đạo pha của hệ thống về mặt trượt và duy trì trên mặt trượt một cách bền vững đối với các biến động của f(x) và g(x) Lấy đạo hàm (4.7) và áp dụng (4.2), ta có

S  f x  g x u  a  x  r    a x   r a x  r (4.8) Có thể chọn u sao cho

S  sign (4.9) Trong đó  là một hằng số dương chọn trước Luật điều khiển được xác định bởi

Thiết kế thuật toán điều khiển trượt

Khi tuyến tính hóa hệ thống tại điểm làm việc thì thành phần phi tuyến được bỏ qua hoặc bị giảm bậc, chính các thành phần phi tuyến cộng với các tác nhân nhiễu bên ngoài có thể làm cho hệ thống mất ổn định hoặc chất lượng đáp ứng của hệ thống bị suy giảm Chính vì vậy, trong phần này thuật toán điều khiển trượt được khảo sát để đảm bảo tính ổn định của hệ thống

Phương trình vi phân của hệ thống:

M ( - (K )/R + (K )/R - LM R n sin( )) + M (M R sin( ) - + 9.81LM sin( ))

M ( - ( )/R + (K K )/R - LM R sin( )) + M (LM R sin( ) - + 9.81LM sin( ))

Phương trình trạng thái của hệ thống: Đặt x 1 , x 2 

M ( - (K )/R + (K )/R - LM R nx sin( )) + M (LR sin( ) - + 9.81LM sin( ))

M ( - (K )/R + (K K )/R - LM nx sin( )) + M (LR sin( ) - + 9.81LM sin( ))

Chọn v x sao cho S  sign(S)   0 Luật điều khiển được xác định bởi:

M ( +(K K )/R LM nx sin( ))+M (LR sin( ) -

Thông số hệ robot một bánh:

Mô phỏng thuật toán điều khiển trượt

Sơ đồ mô phỏng thuật toán điều khiển trượt thực hiện trong Matlab/Simulink như sau :

Hình 26 Sơ đồ mô phỏng thuật toán điều khiển trượt

Dap ung ngo ra theta x so voi tin hieu dat ref x th e ta _ _ x (r a d ) t (s) ref x theta x

Dap ung ngo ra theta y so voi tin hieu dat ref y th e ta _ _ y (r a d ) t (s) ref y theta y

Kết luận

Kết quả mô phỏng điều khiển cân bằng hệ robot một bánh bằng thuật toán điều khiển trượt cho kết quả tốt, tín hiệu ra bám sát theo tín hiệu đặt mong muốn, hệ thống đáp ứng nhanh, không vọt lố và ổn định

XÂY DỰNG BỘ ƯỚC LƯỢNG GÓC NGHIÊNG CHO HỆ ROBOT MỘT BÁNH 5.1 Giới thiệu

Thiết bị đo quán tính (IMU: Inertial Measurement Unit)

Thiết bị đo quán tính (IMU: Inertial Measurement Unit) là thiết bị dùng để đo gia tốc tịnh tiến và góc quay của một vật chuyển động trong không gian 3 chiều

Hình 33 Một vật chuyển động trong không gian bao gồm hai chuyển động: chuyển động tịnh tiến và chuyển động xoay

Ngày nay, IMU là một thiết bị không thể thiếu trong nhiều lĩnh vực như máy bay, tên lửa, tàu thủy, ô tô, robot công nghiệp, mô phỏng chuyển động của con người…IMU đóng vai trò vô cùng quan trọng trong một hệ thống vì độ chính xác của nó quyết định đến chất lượng của cả hệ thống

Mặc dù độ chính xác và kích thước của các IMU là khác nhau nhưng nguyên lý và cấu trúc cơ bản để hình thành nên một IMU là giống nhau Bên trong IMU có 9 cảm biến bao gồm 3 cảm biến vận tốc góc (gyroscope), 3 cảm biến gia tốc (accelerometer), và 3 cảm biến từ trường (magnetometer) kết hợp lại với nhau trên 1 trục tọa độ 3 chiều trực giao Chất lượng của một IMU phụ thuộc chính vào 3 yếu tố

Yếu tố thứ nhất là độ chính xác của 9 cảm biến Nếu cảm biến có chất lượng tốt, đồng nghĩa với sai số đo đạc nhỏ, thì việc xác định góc nghiêng sẽ chính xác hơn Yếu tố thứ hai là thuật toán ước lượng góc quay dựa vào 9 giá trị cảm biến này Thuật toán ước lượng thể hiện trình độ công nghệ và thương hiệu của nhà sản xuất Một thuật toán ước lượng tốt có khả năng phát hiện được các nhiễu nền (bias) của vận tốc, gia tốc và từ trường thông qua mô hình chúng bằng các phương trình biến trạng thái Các nhiễu nền này cũng được xem như là các biến trạng thái trong quá trình ước lượng

Yếu tố thứ ba là quá trình cân chỉnh IMU sao cho thuật toán ước lượng góc quay không bị ảnh hưởng khi nhiệt độ môi trường thay đổi, khi có gia tốc, từ trường ngoài tác động và không bị trôi theo thời gian

Hiện nay có 3 phương pháp để biểu diễn góc quay và chuyển động tịnh tiến của một IMU trong không gian là phương pháp góc Euler, phương pháp ma trận xoay Direction Cosine Matrix (DCM), phương pháp toán tử xoay Quaternion Phương pháp được sử dụng rộng rãi và tương đối đơn giản là sử dụng góc Euler theo 3 trục x, y, z, là cơ sở để thiết kế các bộ ước lượng nói chung, đặc biệt là bộ lọc Kalman

Hình 34 Phương pháp góc Euler

IMU được sử dụng trong đề tài là 9 Degrees of Freedom - MPU-9150 Breakout của hãng Sparkfun gồm 3 cảm biến vận tốc góc (gyroscope), 3 cảm biến gia tốc (accelerometer), và 3 cảm biến từ trường (magnetometer) được tích hợp trong một chíp IC

Hình 35 IMU - 9 Degrees of Freedom - MPU-9150 Breakout

Lý thuyết bộ lọc Kalman

Một cách tổng quát, lọc là loại bỏ những phần không quan tâm hoặc không có nghĩa hoặc những phần sai sót để thu được một đầu ra tốt hơn theo nghĩa hợp hơn với mong muốn, quan tâm của chúng ta Trong thực tế các nhiễu rất đa dạng và phức tạp nên rất khó đôi khi là không thể loại bỏ hoàn toàn nhiễu Bộ lọc chỉ làm sạch tín hiệu đến một mức độ nào đó và trong đa số trường hợp ta cũng chỉ cần có thế Khoảng 50 năm trôi qua, bộ lọc Kalman đã trở nên phổ biến Nó xuất hiện trong rất nhiều ứng dụng và bởi vì vấn đề lọc mà nó giải quyết một loạt vấn đề cơ bản trong rất nhiều lĩnh vực, nên nó có thể vẫn luôn còn được dùng nhiều ứng dụng mới nữa ở tương lai trừ khi có một bộ lọc hay một giải pháp nào đó tốt hơn ra đời

Bộ lọc Kalman là một tập hợp các phương trình toán học giúp tối ưu ước lượng trạng thái của một hệ theo nghĩa giá trị ước đoán có sai số bình phương trung bình nhỏ nhất – MMSE dựa trên mô hình hệ thống, giá trị đo và các hiểu biết về nhiễu (của hệ thống lẫn phép đo) Biểu đồ sau đây là một mô tả hình thức của một bộ lọc nói chung và bộ lọc Kalman nói riêng

Hình 36 Sơ đồ khối bộ lọc tổng quát

Hình 37 Sơ đồ khối bộ lọc Kalman Cảm biến (Bộ đo)

Bộ lọc Tín hiệu cần đo

Bộ lọc Kalman Giá trị tính toán

Giá trị ước lượng tối ưu

Biểu thức của trạng thái và giá trị đo

 Trong đó: w: nhiễu hệ thống v: nhiễu đo đạc Theo như mô hình của ước lượng dùng bộ lọc Kalman đã đề cập ở trên, trước tiên mô hình hệ thống sẽ cho ta một giá trị gọi là ước lượng tiên nghiệm (ước lượng chưa qua bộ lọc Kalman) Nó được tính bằng phương trình: ˆ k ( ) ˆ k 1( ) w k x  x    Trong đó : ˆ ( ) k x  : giá trị tiên nghiệm tại bước k ˆ ( ) k 1 x   : giá trị hậu nghiệm tại bước k-1 (giá trị đã tối ưu của bước trước) w k : nhiễu hệ thống tại bước k

Một chú ý quan trọng: Tất cả các nhiễu trong bộ lọc Kalman đều được giả định là tuân theo phân phối chuẩn (Gauss)

Sau đó giá trị tiên nghiệm xˆ ( ) k  sẽ đi qua bộ lọc Kalman để kết hợp với giá trị đoz k tạo ra một ước lượng tối ưu

Có thể tóm tắt quá trình làm việc của lọc Kalman như sau:

Tính tiên nghiệm (từ hậu nghiệm bước trước)  Lọc  Hậu nghiệm (ước lượng tối ưu)

Ta có phương trình quan hệ của ước lượng tiên nghiệm và hậu nghiệm: ˆ k ( ) k l ˆ k ( ) k k x   K x   K z

Trong đó: ˆ ( ) k x  : giá trị tiên nghiệm ˆ ( ) k x  : giá trị hậu nghiệm

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 53 l

K k , K k : các ma trận chưa biết, K k là độ lợi bộ lọc Kalman

Theo quy tắc trực giao, để sai số của ước lượng bình phương trung bình là nhỏ nhất (MMSE) ta phải có: ˆ ( ) T 0 k k i

E   x  x  z    Thay x k x k  1 w k và biểu thức của xˆ ( ) k  vào phương trình thứ nhất ta có:

E    x    K x   K z z    , i  1, 2, , k  1 Tiếp tục thay z k Hx k v k vào ta thu được:

Sử dụng các điều kiện sau:

(Do v k và z là 2 biến ngẫu nhiên độc lập và v k có phân phối chuẩn với kỳ vọng bằng 0 nên ta có E v z ( )  E v E z ( ) ( )  0 ( ) E z  0) Ta biến đổi phương trình trên:

Sai số x k ( ) và x k ( ) là các sai số của ước lượng ứng với tiên nghiệm và hậu nghiệm

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 54 Sai số z k là sai số của đo đạt

Trừ hai phương trình sau(hai phương trình này có là vì ta muốn thỏa quy tắc trực giao):

Thay x k ,xˆ k và z k vào phương trình trên ta có:

E   x    K x   K z   Hx   z  Lưu ý là vì các biến ngẫu nhiên w, v có kỳ vọng bằng 0 và chúng độc lập với x, z nên ta có: ˆ ( ) 0

Và thay K k l , z k vàx k ( ) vào ta được:

Nhắc lại: ta có theo định nghĩa

Cov XY  E   X  Y     E   Với x , y lần lượt là kỳ vọng của hai biến ngẫu nhiên X và Y

Vậy nếu X và Y có kỳ vọng bằng 0 thì

Từ tính chất trên, ta có ma trận hiệp phương sai của sai số ước lượng tiên nghiệm là (thực chất P chính là sai số bình phương trung bình-MSE)

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 55 Bởi vì x k ( ) là biến ngẫu nhiên có kỳ vọng bằng 0

Khai triển phương trình phía trên và thế P k vào ta thu được :

Trong đóR k là ma trận hiệp phương sai của sai số nhiễu đo đạt(cũng thu được do kỳ vọng của nhiễu đo đạt v bằng 0)

Như vậy ta thu được độ lợi của bộ lọc Kalman :

Tương tự, đối với hậu nghiệm của ước lượng ta cũng có ma trận hiệp phương sai của sai số ước lượng hậu nghiệm :

Thay biểu thức của K k l tìm được ở trên l k k

K   I K H vào phương trình quan hệ của tiên nghiệm và hậu nghiệm : ˆ k ( ) k l ˆ k ( ) k k x   K x   K z ta thu được ˆ k ( ) ( k ) ˆ k ( ) k k x    I K H x   K z

Trừ hai vế cho x k và thay z k Hx k v k vào phương trình trên ta được : ˆ k ( ) k ˆ k ( ) k k k k k ˆ k ( ) k x   x  x   K Hx  K v  K Hx   x

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 56 Thế biểu thức vừa thu được vào P k ( ) và sử dụng :

Phương trình ta vừa thu được gọi là dạng Joseph (Joseph form) của phương trình cập nhật ma trận hiệp phương sai

Tiếp tục khai triển phương trình này ta được :

 ( I  K H P k ) k ( )  Phương trình cuối cùng thường được dùng để cập nhậtP k ( ) nhất

Ma trận hiệp phương sai của sai số : Ta có :

P  E x  x   ˆ k ( ) ˆ k 1( ) x  x   Trừ hai vế của phương trình sau cho x k và thế x k x k  1 w k vào ta thu được : ˆ k ( ) k ˆ k 1( ) k x   x x   x

 x k  1 ( ) w  k Thay biểu thức của sai số ước lượng tiên nghiệm x k ( ) vừa thu được vàoP k ( ) và sử dụng :

   Đây chính là biểu thức của ma trận hiệp phương sai của sai số ước lượng tiên nghiệm.

Thiết kế bộ lọc Kalman

Trong phần trên chúng ta đã xây dựng xong các phương trình của bộ lọc Kalman Phần này sẽ tóm tắt lại các phương trình cho việc sử dụng

1 w k k k x x   , w k N(0,Q k ) Mô hình đo : k k k z Hx v , v k N(0,R k ) Ước lượng trạng thái : ˆ k ( ) ˆ k 1( ) x  x   Ma trận hiệp phương sai của sai số :

Cập nhật trạng thái : ˆ ( ) ˆ ( ) [ ˆ ( ) ] k k k k k k y x   x   K z  Hx 

Cập nhật ma trận hiệp phương sai của sai số :

Ma trận độ lợi Kalman :

Và quá trình làm việc của bộ lọc Kalman có thể tóm tắt trong hình sau : Đề tài này sử dụng đến hai cảm biến là cảm biến gia tốc (Accelerometer) và cảm biến vận tốc góc (Gyroscope) để đo góc nghiêng và vận tốc góc nghiêng Công việc ở đây là phải đưa ra được tập hợp dữ liệu từ các cảm biến để có được ước lượng chính xác tốt hơn là chỉ sử dụng riêng lẻ các cảm biến đơn thuần

Vấn đề với cảm biến gia tốc (Accelerometer) là bị nhiễu tác động lớn khi nó được dùng để đo gia tốc trọng trường trong lúc robot di chuyển tới lui Còn vấn đề với cảm biến vận tốc góc (Gyroscope) là nó bị trôi (Drift) theo thời gian, giống như một bánh xe quay, Gyroscope sẽ bắt đầu giảm xuống khi nó mất dần tốc độ

Bộ lọc Kalman là một thuật toán sử dụng một loạt các phép đo quan sát theo thời gian, trong ứng dụng này là cảm biến gia tốc (Accelerometer) và cảm biến vận tốc góc (Gyroscope) Các phép đo chứa nhiễu mà sẽ gây ra sự không chính xác làm ảnh hưởng đáng kể tới chất lượng và ổn định trong toàn bộ hệ thống Bộ lọc Kalman

Hình 38 Thuật toán bộ lọc Kalman

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 59 sẽ ược lượng trạng thái hệ thống, dựa trên trạng thái hiện tại và quá khứ, để ước lượng một cách tối ưu nhất, chính xác hơn nhiều so với các phép đo đơn độc

Hình 39 Bộ lọc Kalman lọc nhiễu trong cảm biến Accelerometer và

 x , y lần lượt là góc nghiêng theo phương x, y

 x , y lần lượt là vận tốc góc nghiêng theo phương x, y ˆ x

 , ˆ y lần lượt là góc nghiêng ước lượng theo phương x, y sau khi qua bộ lọc Kalman ˆ

 x , ˆ y lần lượt là vận tốc góc nghiêng ước lượng theo phương x, y sau khi qua bộ lọc Kalman

Như chúng ta có thể thấy đầu ra của bộ lọc không những có góc nghiêng  mà còn có các nhiễu nền (Bias) của vận tốc góc  b ( [ b x ] , [  b y ] ) thông qua mô hình chúng bằng các phương trình biến trạng thái Các nhiễu nền này cũng được xem như là các biến trạng thái trong qua trình ước lượng Nhiễu nền là lượng Gyroscope bị trôi dạt Điều này có nghĩa rằng chúng ta có thể có được vận tốc góc đúng bằng cách trừ đi nhiễu nền  b từ phép đo Gyroscope

     , [ b k ]  [ b k ]  1 Hiển nhiên ta cũng có : k k

  Biến đổi từ các phương trình trên ta được :

 Từ đây, phương trình trạng thái của hệ thống được cho bởi :

Q k là ma trận hiệp phương sai của nhiễu hệ thống, và trong trường hợp này chính là ma trận hiệp phương sai ước lượng trạng thái của Accelerometer và Bias Các ước lượng Accelerometer và Bias là độc lập, vì vậy nó chính bằng phương sai của ước lượng Accelerometer và Bias :

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 61 var( )

Chúng ta giả sử rằng nhiễu đo lường là như nhau và không phụ thuộc vào thời điểm k : var( ) var( ) k k

Kết luận

Chương này trình bày lý thuyết và xây dựng bộ lọc Kalman áp dụng vào xử lý cảm biến gia tốc Accelerometer và vận tốc góc Gyroscope tích hợp trong IMU để ước lượng tối ưu góc nghiêng và vận tốc góc nghiêng robot Các chương sau sẽ trình bày việc kiểm nghiệm thực tế, cũng như chi tiết cách thức lập trình bộ lọc Kalman được lập trình bằng sơ đồ khối trong môi trường Matlab/Simulink sau đó phát ra mã C/C++ để có thể đưa được vào vi điều khiển ARM-STM32F4-DISCOVERY, đồng thời có kiểm chứng tính chính xác và ưu việt của nó trong việc xử lý chống nhiễu

KẾT QUẢ KIỂM NGHIỆM THỰC TẾ 6.1 Giới thiệu

Bộ lọc Kalman nhúng

Cảm biến gia tốc Accelerometer ba trục tọa độ được dùng để xác định góc định hướng của vật thể trong không gian ba chiều Giá trị cảm biến gia tốc Accelerometer ba trục tọa độ chính là giá trị của gia tốc trọng trường a x , a y ,a z trên ba trục tọa độ tương ứng Định nghĩa  x , y , z lần lượt là góc nghiêng hợp bởi trục x, y, z với mặt phẳng nằm ngang, các góc nghiêng này được tính từ giá trị gia tốc trên ba trục như sau:

Cảm biến vận tốc góc Gyroscope ba trục tọa độ được dùng để xác định vận tốc góc nghiêng của vật thể trong không gian ba chiều Giá trị cảm biến vận tốc góc Gyroscope ba trục tọa độ chính là giá trị của vận tốc góc g x , g y ,g z trên ba trục tọa độ

HỌC VIÊN THỰC HIỆN: NGUYỄN TRỌNG QUANG Page 64 tương ứng, các giá trị này cũng chính là đạo hàm của các góc nghiêng  x , y , z Nên ta có: x g x

Thuật toán Kalman được lập trình bằng sơ đồ khối trong Matlab/Simulink như sau:

Hình 40 Thuật toán Kalman nhúng

  , R k  R 0.03 Các giá trị khởi tạo ban đầu:

Lập trình trong Matlab/Simulink như sau:

Kết quả kiểm nghiệm thực tế bộ lọc Kalman

Góc nghiêng và vận tốc góc nghiêng trước và sau khi qua bộ lọc Kalman theo phương x:

Hình 41 Đồ thị góc nghiêng và vận tốc góc nghiêng theo phương x

Góc nghiêng và vận tốc góc nghiêng trước và sau khi qua bộ lọc Kalman theo phương y:

Hình 42 Đồ thị góc nghiêng và vận tôc góc nghiêng theo phương y

Thuật toán điều khiển LQR nhúng

Sơ đồ thuật toán điều khiển LQR nhúng được thực hiện trong Matlab/Simulink như sau:

Hình 43 Sơ đồ khối thuật toán điều khiển LQR nhúng

Kết quả kiểm nghiệm thực tế thuật toán điều khiển LQR

Góc nghiêng của robot theo phương x khi điều khiển giữ cân bằng robot:

Hình 44 Đồ thị góc nghiêng robot theo phương x

Góc nghiêng của robot theo phương y khi điều khiển giữ cân bằng robot:

Hình 45 Đồ thị góc nghiêng robot theo phương y Tín hiệu điều khiển động cơ theo phương x khi điều khiển giữ cân bằng robot:

Hình 46 Đồ thị tín hiệu điều khiển động cơ theo phương x

Tín hiệu điều khiển động cơ theo phương y khi điều khiển giữ cân bằng robot:

Hình 47 Đồ thị tín hiệu điều khiển động cơ theo phương y

6.6 Thuật toán điều khiển trƣợt nhúng

Sơ đồ thuật toán điều khiển trượt nhúng được thực hiện trong Matlab/Simulink như sau:

Hình 48 Sơ đồ khối thuật toán điều khiển trượt nhúng

6.7 Kết quả kiểm nghiệm thực tế thuật toán điều khiển trƣợt

Hình 49 Đồ thị góc nghiêng robot theo phương x Góc nghiêng của robot theo phương y khi điều khiển giữ cân bằng robot:

Hình 50 Đồ thị góc nghiêng robot theo phương y

Tín hiệu điều khiển động cơ theo phương x khi điều khiển giữ cân bằng robot:

Hình 51 Đồ thị tín hiệu điều khiển động cơ theo phương x Tín hiệu điều khiển động cơ theo phương y khi điều khiển giữ cân bằng robot:

 Từ kết quả thực nghiệm ta thấy giá trị ước lượng góc nghiêng và vận tốc góc nghiêng đạt được tốt hơn khi qua bộ lọc Kalman Sau khi qua bộ lọc giá trị đo đã loại bỏ được nhiễu từ cảm biến gia tốc Accelerometer và loại bỏ được độ trôi phân cực từ cảm biến vận tốc góc Gyroscope

 Khi điều khiển giữ cân bằng cho robot một bánh bằng thuật toán điều khiển LQR thì góc nghiêng của robot dao động quanh giá trị 1 0 theo phương x, 1 0 theo phương y Trong khi đó, việc tác động ngoại lực vào robot thì robot rất dễ mất cân bằng, dao động mạnh và không ổn định

 Khi điều khiển giữ cân bằng cho robot một bánh bằng thuật toán điều khiển trượt thì góc nghiêng của robot dao động quanh giá trị 0.5 0 theo phương x, 0.5 0 theo phương y Trong khi đó, việc tác động ngoại lực vào robot thì robot vẫn giữ được cân bằng và ổn định.

CHƯƠNG 7: KẾT QUẢ ĐẠT ĐƯỢC VÀ ĐỊNH

Trong phạm vi luận văn, tác giả đã hoàn thành việc thiết kế và thi công mô hình robot một bánh Robot một bánh có khả năng tự giữ cân bằng tốt Các vấn đề đã nghiên cứu và thực hiện được trong luận văn gồm :

 Nghiên cứu mô hình toán mô tả hệ robot một bánh, từ đó thành lập được phương trình phi tuyến và tuyến tính hóa của hệ robot một bánh, làm cơ sở nền tảng để xây dựng các thuật toán điêu khiển cho robot một bánh

 Xây dựng được các thuật toán điều khiển LQR và điều khiển trượt trong việc điều khiển giữ cân bằng robot một bánh

 Mô phỏng được các thuật toán điều khiển trên Matlab/Simulink Thuật toán điều khiển LQR được xây dựng trên cơ sở tuyến tính hóa quanh điểm cân bằng, mà bản thân hệ robot một bánh là một hệ thống có tính phi tuyến cao, chính vì vậy khi sử dụng thuật toán điều khiển trượt thì chất lượng điều khiển được cải thiện về độ vọt lố cũng như thời gian xác lập

 Xây dựng được bộ lọc Kalman trong việc lọc nhiễu cảm biến gia tốc Accelerometer và vận tốc góc Gyroscope, từ đó ược lượng được tối ưu góc nghiêng và vận tốc góc nghiêng cho robot một bánh

 Áp dụng được các thuật toán điều khiển đã thiết kế vào điều khiển đối tượng thực để kiểm chứng giữa lý thuyết và thực nghiệm So sánh các thuật toán điều khiển cân bằng robot thấy rằng, thuật toán điều khiển trượt cho kết quả tốt, chịu được ngoại lực tác động lớn, nhưng vẫn cân bằng và ổn định Thuật toán điều khiển LQR tuy giữ cân bằng được, nhưng khi có ngoại lực tác động rất dễ bị mất ổn định và ngã

 Về phần cơ khí : cơ cấu lái (ổ chuột-bóng) truyền động dựa trên nguyên tắc chính là ma sát Do đó, con lăn sẽ bị mài mòn và truyền động giữa con lăn và bóng sẽ có hiện tượng trượt Bên cạnh đó, robot chỉ di chuyển được trên những bề mặt phẳng và có ma sát

 Về thuật toán điều khiển : việc lựa chọn các thông số cho các thuật toán điều khiển hầu hết vẫn được tiến hành theo phương pháp thử sai do đó các thông số được chọn vẫn chưa phải là tối ưu nhất

 Xây dựng robot một bánh hoàn chỉnh bao gồm điều khiển robot di chuyển theo vị trí mong muốn

 Xây dựng các thuật toán điều khiển thích nghi kết hợp với bền vững, đồng thời phát triển các giải thuật tìm kiếm thông số tối ưu cho thuật toán điều khiển như sử dụng giải thuật di truyền để đạt được các thuật toán điều khiển thích nghi bền vững chất lượng cao

 Đưa mô hình vào ứng dụng thực tế.

Kết quả kiểm nghiệm thực tế thuật toán điều khiển trượt

Hình 49 Đồ thị góc nghiêng robot theo phương x Góc nghiêng của robot theo phương y khi điều khiển giữ cân bằng robot:

Hình 50 Đồ thị góc nghiêng robot theo phương y

Tín hiệu điều khiển động cơ theo phương x khi điều khiển giữ cân bằng robot:

Hình 51 Đồ thị tín hiệu điều khiển động cơ theo phương x Tín hiệu điều khiển động cơ theo phương y khi điều khiển giữ cân bằng robot:

Kết luận

 Từ kết quả thực nghiệm ta thấy giá trị ước lượng góc nghiêng và vận tốc góc nghiêng đạt được tốt hơn khi qua bộ lọc Kalman Sau khi qua bộ lọc giá trị đo đã loại bỏ được nhiễu từ cảm biến gia tốc Accelerometer và loại bỏ được độ trôi phân cực từ cảm biến vận tốc góc Gyroscope

 Khi điều khiển giữ cân bằng cho robot một bánh bằng thuật toán điều khiển LQR thì góc nghiêng của robot dao động quanh giá trị 1 0 theo phương x, 1 0 theo phương y Trong khi đó, việc tác động ngoại lực vào robot thì robot rất dễ mất cân bằng, dao động mạnh và không ổn định

 Khi điều khiển giữ cân bằng cho robot một bánh bằng thuật toán điều khiển trượt thì góc nghiêng của robot dao động quanh giá trị 0.5 0 theo phương x, 0.5 0 theo phương y Trong khi đó, việc tác động ngoại lực vào robot thì robot vẫn giữ được cân bằng và ổn định.

CHƯƠNG 7: KẾT QUẢ ĐẠT ĐƯỢC VÀ ĐỊNH

Kết quả đạt được

Trong phạm vi luận văn, tác giả đã hoàn thành việc thiết kế và thi công mô hình robot một bánh Robot một bánh có khả năng tự giữ cân bằng tốt Các vấn đề đã nghiên cứu và thực hiện được trong luận văn gồm :

 Nghiên cứu mô hình toán mô tả hệ robot một bánh, từ đó thành lập được phương trình phi tuyến và tuyến tính hóa của hệ robot một bánh, làm cơ sở nền tảng để xây dựng các thuật toán điêu khiển cho robot một bánh

 Xây dựng được các thuật toán điều khiển LQR và điều khiển trượt trong việc điều khiển giữ cân bằng robot một bánh

 Mô phỏng được các thuật toán điều khiển trên Matlab/Simulink Thuật toán điều khiển LQR được xây dựng trên cơ sở tuyến tính hóa quanh điểm cân bằng, mà bản thân hệ robot một bánh là một hệ thống có tính phi tuyến cao, chính vì vậy khi sử dụng thuật toán điều khiển trượt thì chất lượng điều khiển được cải thiện về độ vọt lố cũng như thời gian xác lập

 Xây dựng được bộ lọc Kalman trong việc lọc nhiễu cảm biến gia tốc Accelerometer và vận tốc góc Gyroscope, từ đó ược lượng được tối ưu góc nghiêng và vận tốc góc nghiêng cho robot một bánh

 Áp dụng được các thuật toán điều khiển đã thiết kế vào điều khiển đối tượng thực để kiểm chứng giữa lý thuyết và thực nghiệm So sánh các thuật toán điều khiển cân bằng robot thấy rằng, thuật toán điều khiển trượt cho kết quả tốt, chịu được ngoại lực tác động lớn, nhưng vẫn cân bằng và ổn định Thuật toán điều khiển LQR tuy giữ cân bằng được, nhưng khi có ngoại lực tác động rất dễ bị mất ổn định và ngã

Một số hạn chế

 Về phần cơ khí : cơ cấu lái (ổ chuột-bóng) truyền động dựa trên nguyên tắc chính là ma sát Do đó, con lăn sẽ bị mài mòn và truyền động giữa con lăn và bóng sẽ có hiện tượng trượt Bên cạnh đó, robot chỉ di chuyển được trên những bề mặt phẳng và có ma sát

 Về thuật toán điều khiển : việc lựa chọn các thông số cho các thuật toán điều khiển hầu hết vẫn được tiến hành theo phương pháp thử sai do đó các thông số được chọn vẫn chưa phải là tối ưu nhất

Hướng phát triển

 Xây dựng robot một bánh hoàn chỉnh bao gồm điều khiển robot di chuyển theo vị trí mong muốn

 Xây dựng các thuật toán điều khiển thích nghi kết hợp với bền vững, đồng thời phát triển các giải thuật tìm kiếm thông số tối ưu cho thuật toán điều khiển như sử dụng giải thuật di truyền để đạt được các thuật toán điều khiển thích nghi bền vững chất lượng cao

 Đưa mô hình vào ứng dụng thực tế

Tiêu đề	Điều khiển cân bằng robot một bánh
Tác giả	Nguyễn Trọng Quang
Người hướng dẫn	TS. Nguyễn Vĩnh Hảo
Trường học	Trường Đại học Bách Khoa - ĐHQG - HCM
Chuyên ngành	Kỹ thuật điều khiển và tự động hóa
Thể loại	Luận văn thạc sĩ
Năm xuất bản	2013
Thành phố	Tp. Hồ Chí Minh

Định dạng
Số trang	78
Dung lượng	1,85 MB