Luận văn cơ điện tử Robot hai bánh tự cân bằng

1.3 Phương pháp nghiên cứu Đề tài được tiếp cận dựa trên các phương pháp sau:  Phương pháp khảo sát tài liệu, tìm hiểu các tài liệu liên quan đến đến đề tài như: cấu trúc robot hai bá

- -

BÁO CÁO NGHIÊN CỨU KHOA HỌC

ĐỀ TÀI:

ROBOT HAI BÁNH TỰ CÂN BẰNG

TRÀ TI NA NGUYỄN TẤN HẬU

BIÊN HÒA, THÁNG 12/2013

TRƯỜNG ĐẠI HỌC LẠC HỒNG KHOA CƠ ĐIỆN – ĐIỆN TỬ

- -

BÁO CÁO NGHIÊN CỨU KHOA HỌC

ĐỀ TÀI:

ROBOT HAI BÁNH TỰ CÂN BẰNG

Sinh viên thực hiện: TRÀ TI NA NGUYỄN TẤN HẬU Giáo viên hướng dẫn: ThS LÊ HOÀNG ANH

BIÊN HÒA, THÁNG 12/2013

em gặp khó khăn trong việc lập trình, thiết kế bo mạch

Em cũng xin chân thành cảm ơn các thầy cô trong khoa Cơ điện – Điện tử, cũng như các thầy cô trong trường đã giảng dạy, giúp đỡ chúng em trong suốt bốn năm học qua Chính các thầy cô đã xây dựng cho chúng em những kiến thức nền tảng và những kiến thức chuyên môn để em có thể hoàn thành đề tài này

Ngoài ra, em xin gởi lời cảm ơn đến gia đình, người thân, bạn bè và các anh, chị sinh viên khóa trên đã luôn ủng hộ và giúp đỡ trong suốt quá trình chúng em thực hiện đề tài

MỤC LỤC

LỜI CẢM ƠN i

MỤC LỤC… ii

DANH MỤC CÁC CHỮ VIẾT TẮT v

DANH MỤC HÌNH ẢNH vi

DANH MỤC BẢNG BIỂU viii

Chương 1: GIỚI THIỆU 1

1.1 Giới thiệu 1

1.2 Mục tiêu của đề tài 1

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

1.4 Giới hạn của đề tài 1

1.5 Tình hình nghiên cứu trong nước và trên thế giới 2

1.5.1 Trong nước 2

1.5.2 Nước ngoài 2

1.5.2.1 nBot 2

1.5.2.2 EquipoiseBot 3

1.5.2.3 WobblyBot 4

1.5.2.4 tiltOne 4

1.5.2.5 Robot kiểu rolling của hãng TOYOTA 5

1.6 Nội dung đề tài 5

Chương 2:THIẾT KẾ MÔ HÌNH HỆ THỐNG 7

2.1 Sơ đồ khối của hệ thống 7

2.2 Tính toán và thiết kế cho mô hình hệ thống 7

2.3 Mô hình robot hai bánh thực tế 10

2.4 Hệ thống điện trên mô hình robot 11

2.4.1 Mạch điều khiển Arduino Due 11

2.4.2 Mạch điều khiển động cơ 12

2.4.3 Cảm biến IMU GY-86 13

2.4.4 Động cơ DC 13

2.4.5 Nguồn điện 14

2.5 Các phương thức giao tiếp 15

2.5.1 Điều xung độ rộng xung PWM 15

2.5.2 Giao thức I2C 16

2.5.3 Giao tiếp UART 18

2.6 Thu thập dữ liệu 20

Chương 3: XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN CÂN BẰNG 21

3.1 Nguyên lý điều khiển cân bằng 21

3.2 Bộ lọc Kalman 21

3.2.1 Giới thiệu 21

3.2.2 Bộ lọc Kalman rời rạc 22

3.2.2.1 Quá trình ước lượng 23

3.2.2.2 Giải thuật lọc Kalman rời rạc 20

3.3 Xây dựng bộ lọc Kalman áp dụng cho cảm biến IMU 25

3.4 Giải thuật điều khiển PID 31

3.5 Xây dựng giải thuật PID điều khiển robot 33

3.6 Giải thuật điều khiển robot cân bằng bám theo vị trí ban đầu 35

Chương 4: KẾT QUẢ THỰC NGHIỆM 37

4.1 Độ chính xác của cảm biến 37

4.2 Giải thuật cân bằng PID trên mô hình robot 39

4.3 Kết quả thực hiện giải thuật trên mô hình robot 40

Chương 5: KẾT LUẬN 41

5.1 Kết luận 41

5.2 Hướng phát triển của đề tài 41

TÀI LIỆU THAM KHẢO 43

PHỤ LỤC 1: GIỚI THIỆU VỀ ARDUINO

PHỤ LỤC 2: SƠ ĐỒ NGUYÊN LÝ MẠCH ĐIỀU KHIỂN ĐỘNG CƠ PHỤ LỤC 3: SƠ ĐỒ NGUYÊN LÝ MẠCH CẢM BIẾN IMU GY-86 PHỤ LỤC 4: SƠ ĐỒ NGUYÊN LÝ MẠCH ARDUINO DUE

PHỤ LỤC 5: CODE CHƯƠNG TRÌNH

DANH MỤC CÁC TỪ VIẾT TẮT

CPU - Central Processing Unit

DAC - Digital to Analog Converter

DC - Direct Current

EEPROM - Electrically Erasable Programmable Read Only Memory FET - Field-Effect Transistor

IC - International Circuit

I/O - Input / Output

I2C - Inter Intergrated Circuit

IMU - Inertial Measurement Unit

JTAG - Joint Test Action Group

LED - Light Emitting Diode

LSB - Least Significant Bit

MSB - Most Significant Bit

OTG - On The Go

PC - Personal Computer

PID - Proportional Integral Derivative

PWM - Pulse Width Modulation

SCL - Simply Connect Last

SDA - Serial Data Line

SPI - Serial Peripheral Interface

SRAM - Static Random Access Memory

TWI - Two-Wire Inter

UART - Universal Asynchronous Receiver – Transmitter

USB - Universal Serial Bus

VAV - Unmanned Aerial Vehicle

DANH MỤC HÌNH ẢNH

Hình 1.1 Mô hình robot hai bánh cân bằng 2

Hình 1.2 Robot hai bánh cân bằng nBot 3

Hình 1.3 Robot hai bánh cân bằng EquipoiseBot 3

Hình 1.4 Robot hai bánh cân bằng WobblyBot 4

Hình 1.5 Robot hai bánh cân bằng tiltOne 4

Hình 1.6 Robot hai bánh của hãng TOYOTA 5

Hình 2.1 Sơ đồ khối hệ thống 7

Hình 2.2 Hình 2.2 Quãng đường robot cần di chuyển để thực hiện cân bằng khi nghiêng một góc 200 so với góc tại vị trí cân bằng 7

Hình 2.3 Môtả thời gian robot ngã từ trạng thái cân bằng đến một góc nghiêng 200 8 Hình 2.4 Mô hình robot hai bánh tự cân bằng 10

Hình 2.5 Bo Arduino Due AT91SAM3X8E 11

Hình 2.6 Mạch điều khiển động cơ cầu H 12

Hình 2.7 Cảm biến IMU GY-86 13

Hình 2.8 Động cơ DC Planet 14

Hình 2.9 Pin LI-PO sử dụng cho robot 15

Hình 2.10 Sơ đồ dạng xung điều chế PWM 16

Hình 2.11 Sơ đồ kết nối theo giao thức I2C 18

Hình 2.12 Mô tả hoạt động của UART 19

Hình 2.13 Mô tả hoạt động thu thập dữ liệu 20

Hình 3.1 Mô tả quá trình cân bằng của robot hai bánh 21

Hình 3.2 Quy trình của bộ lọc Kalman 24

Hình 3.3 Quy trình hoàn chỉnh của bộ lọc Kalman 25

Hình 3.4 Hệ thống điều khiển vòng kín 32

Hình 3.5 Đổ thị biễu diễn hệ số PID theo thời gian 33

Hình 3.6 Robot cân bằng bám theo vị trí ban đầu 35

Hình 3.7 Sơ đồ thuật giải robot bam theo vị trí ban đầu 36 Hình 4.1 Biểu đồ so sánh góc nghiêng theo trục y của bộ lọc Complementary, bộ lọc Kalman và góc nghiêng theo trục y thu trực tiếp từ cảm biến IMU 37Hình 4.2 Biểu đồ so sánh thời gian đáp ứng giữa bộ lọc Complementary và bộ lọc Kalman… 38Hình 4.3 Biểu đồ mô so sánh độ ổn định ngõ ra của bộ lọc Kalman với bộ lọc

Complementary 38

Hình 4.4 Góc nghiêng theo trục y của robot khi thực hiện giải thuật cân bằng 39

Hình 5.1 Robot đang thực hiện việc cân bằng 41

DANH MỤC BẢNG BIỂU

Bảng 2.1 Bảng thu thập thời gian robot ngã do lực hút của Trái Đất 8

Bảng 2.2 Chọn động cơ với các thông số 10

Bảng 2.3 Thông số kỹ thuật bo Arduino Due AT91SAM3X8E 11

Bảng 2.4 Thông số kỹ thuật của động cơ DC Planet 14

Bảng 3.1 Ảnh hưởng của các thành phần KP, KI, KD đối với hệ kín 33

Bảng 3.2 Phương pháp Ziegler–Nichols 35

linh hoạt nhưng lại không chiếm nhiều không gian [1], [3]

1.2 Mục tiêu của đề tài

Mục tiêu của đề tài là xây dựng mô hình robot hai bánh thực tế có khả năng tự cân bằng có cấu trúc đơn giản phục vụ cho việc nghiên cứu, giảng dạy tại phòng thí nghiệm, mô hình sản phẩm trưng bày hay những trò chơi với mục đích giải trí

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

Đề tài được tiếp cận dựa trên các phương pháp sau:

 Phương pháp khảo sát tài liệu, tìm hiểu các tài liệu liên quan đến đến đề tài như: cấu trúc robot hai bánh tự cân bằng, cảm biến IMU, mạch điều khiển động cơ

 Phương pháp khảo sát các thuật toán lọc nhiễu cho cảm biến IMU như: bộ lọc Kalman, bộ lọc Complementary và thuật toán điều khiển PID

 Phương pháp thực nghiệm tiến hành xây dựng các thuật toán trên mô hình robot hai bánh thực tế

1.4 Giới hạn của đề tài

Đề tài tập trung vào việc xây dựng mô hình phần cứng robot như: kết cấu cơ khí, mạch điều khiển động cơ, các thuật toán trên vi điều khiển như bộ lọc Kalman

giải thuật cân bằng PID Robot chỉ có thể cân bằng tại một vị trí chưa có khả năng vừa di chuyển vừa cân bằng

1.5 Tình hình nghiên cứu trong nước và trên thế giới

1.5.2.1 nBot

nBot do ông Anderson sáng chế nBot lấy ý tưởng cân bằng là các bánh xe sẽ phải chạy theo hướng mà phần trên robot sắp ngã Nếu bánh xe có thể di chuyển để trọng tâm robot luôn rơi vào chính giữa thì robot sẽ giữ được cân bằng

Hình 1.2 Robot hai bánh cân bằng nBot

1.5.2.2 EquipoiseBot

EquipoiseBot sử dụng hai cảm biến là cảm biến gia tốc và cảm biến con quay hồi chuyển Góc nghiêng của robot được tính toán từ hai giá trị khác nhau Một từ cảm biến gia tốc bằng cách sử dụng thuật toán Tangens-Funktion và một từ cảm biến con quay hồi chuyển sử dụng phương pháp tích phân Hai giá trị này được kết hợp trong một bộ lọc để cho ra một giá trị mới và được sử dụng cho đầu vào bộ PID

để điều khiển các động cơ, giữ cho robot cân bằng

Hình 1.3 Robot hai bánh cân bằng EquipoiseBot

1.5.2.3 WobblyBot

WobblyBot thực chất là mô hình sử dụng con lắc ngược, được gắn dưới phần thân ở giữa hai bánh xe WobblyBot được thiết kế với phần dưới của robot nặng hơn nhiều so với phần trên giúp robot có khả giữ cân bằng tốt nhất

Hình 1.4 Robot hai bánh cân bằng WobblyBot

1.5.2.5 Robot kiểu rolling của hãng TOYOTA

Đây là robot có công dụng phục vụ con người do hãng TOYOTA thiết kế Mẫu robot này có khả năng di chuyển nhanh và ít chiếm không gian Đồng thời đôi tay của robot có thể làm nhiều công việc khác nhau, chủ yếu được sử dụng với mục đích giải trí

Hình 1.6 Robot hai bánh của hãng TOYOTA

1.6 Nội dung đề tài

Nội dung phần còn lại của đề tài gồm các chương như sau:

 Chương 2: THIẾT KẾ MÔ HÌNH HỆ THỐNG

Nội dung chương 2 trình bày sơ đồ khối hệ thống, xây dựng mô hình robot hai bánh thực tế, thiết kế hệ thống điện và các thành phần tham gia điều khiển robot như: bo Arduino Due, mạch điều khiển động cơ, cảm biến IMU GY-86, động cơ Planet và Pin LI-PO Phần cuối chương giới thiệu các phương thức giao tiếp sử dụng trên mô hình như: điều chế độ rộng xung PWM, giao thức I2C, giao tiếp UART và thu thập dữ liệu để vẽ dạng sóng ngõ ra trên phần mềm Labview

 Chương 3: XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN CÂN BẰNG

Phần đầu chương 3 giới thiệu về nguyên lý điều khiển cân bằng của robot, giới thiệu tổng quan về bộ lọc Kalman, cơ sở, bản chất, giải thuật của bộ lọc Kalman rời rạc Giải thuật điều khiển PID được sử dụng để đáp ứng ngõ ra nhanh, chính xác

trong việc cân bằng robot và cuối cùng là giải thuật điều khiển robot cân bằng bám theo vị trí ban đầu

 Chương 4: KẾT QUẢ THỰC NGHIỆM

Chương 4 trình bày kết quả thực nghiệm về độ chính xác khi sử dụng bộ lọc Complementary, bộ lọc Kalman so với dữ liệu thô từ cảm biến, xây dựng giải thuật điều khiển PID trên mô hình thực tế

 Chương 5: KẾT LUẬN

Nội dung chương 5 trình bày tóm tắt các kết quả mà đề tài đã đạt được và hướng phát triển để khắc phục những giới hạn nhằm hoàn thiện đề tài tốt hơn

Mạch điều khiển động cơ cầu H

Nguồn cung cấp

Hình 2.1 Sơ đồ khối hệ thống

2.2 Tính toán và thiết kế cho mô hình hệ thống

Giả sử góc nghiêng mà robot có thể giữ cân bằng được giới hạn trong khoảng

±200 so với góc tại vị trí cân bằng lý tưởng 900 theo trục y

50 cm

a) Robot ở trạng thái cân bằng

0

Hình 2.2 Quãng đường robot cần di chuyển để thực hiện cân bằng khi nghiêng một

góc 20 0 so với góc tại vị trí cân bằng

Như mô tả trong Hình 2.2 để robot có thể giữ được cân bằng khi ngã thì robot phải di chuyển một đoạn đường là trong khoảng thời gian cho phép để

có thể hứng được trọng tâm của robot, các giá trị này được tính toán như sau:

(2.1)

50 cm

a) Robot ở trạng thái cân bằng

Bảng 2.1 Bảng thu thập thời gian robot ngã do lực hút của Trái Đất

Thời gian (ms)

mô hình) đến góc nghiêng giới hạn 88 ± 200

trong khoảng thời gian trung bình là

950 ms, đây cũng là giá trị thời gian giới hạn cần thiết để robot di chuyển để giữ được cân bằng

Giả sử chọn bánh xe có bán kính là 5cm và moment xoắn của động cơ là 63 Ncm thì lực đẩy sẽ được tính theo công thức sau:

Dựa vào các giá trịnh tính toán, thông số của động cơ được chọn như trong Bảng 2.2 để đáp ứng các yêu cầu trên

Bảng 2.2 Chọn động cơ với các thông số

Tốc độ chưa qua bộ giảm tốc 9000 vòng/phút

Tốc độ khi đã qua bộ giảm tốc 468 vòng/phút

2.3 Mô hình robot hai bánh thực tế

Đề tài sử dụng bo mạch Arduino Due AT91SAM3X8E đóng vai trò như “bộ não” của robot, điều khiển cho robot giữ cân bằng Khung robot được chế tạo từ inox với hai động cơ DC được đặt đồng trục, cho phép robot có thể di chuyển được theo hai hướng trước và sau Hai động cơ được điểu khiển bởi hai mạch cầu H sử dụng IC IR2184 để điều khiển kích FET, cảm biến IMU GY-86 được sử dụng để xác định góc nghiêng của robot Hai động cơ DC 24V, 60W tốc độ 468 vòng/phút, encoder 500 xung/vòng Hai bánh xe được bọc cao su có nhiều rãnh để tăng độ bám cao giúp robot cân bằng tốt hơn

Cảm biến IMU GY-86

Mạch điều khiển động cơ cầu H

Hình 2.4 Mô hình robot hai bánh tự cân bằng

2.4 Hệ thống điện trên mô hình robot

2.4.1 Mạch điều khiển Arduino Due

Arduino Due là bo mạch sử dụng vi điều khiển của hãng Atmel SAM3X8E ARM Cortex-M3 CPU Đây là bo Arduino đầu tiên dựa trên bộ vi điều khiển lõi ARM 32-bit Bo Arduino Due có 54 ngõ vào/ngõ ra là tín hiệu số (trong đó có 12 chân có thể xuất tín hiệu PWM), 12 chân ngõ vào analog và 2 chân ngõ ra DAC, xung nhịp 84 MHz, một cổng kết nối USB OTG, jack nguồn, nút nhấn reset CPU và các cổng giao tiếp đa dạng như: 4 UART, SPI, TWI (I2C) và JTAG

Bo Arduino Due có thể được cấp nguồn và nạp chương trình từ máy tính thông qua cáp USB hoặc sử dụng nguồn cấp từ adapter với điện áp từ 7 đến 12VDC

Hình 2.5 Bo Arduino Due AT91SAM3X8E

 Thông số kỹ thuật bo Arduino Due AT91SAM3X8E

Bảng 2.3 Thông số kỹ thuật bo Arduino Due AT91SAM3X8E

Operating Voltage 3.3V

Input Voltage (recommended) 7-12V

Input Voltage (limits) 6-16V

Digital I/O Pins 54 (of which 12 provide PWM output)

Analog Input Pins 12

Analog Outputs Pins 2 (DAC)

Total DC Output Current on all 130 mA

DC Current for 3.3V Pin 800 mA

DC Current for 5V Pin 800 mA

Flash Memory 512 KB all available for the user applications SRAM 96 KB (two banks: 64KB and 32KB)

2.4.2 Mạch điều khiển động cơ

Mạch cầu H dùng IC kích FET chuyên dụng IR2184 cho phép điều khiển động

cơ với dòng liên tục 10A và dòng tức thời lên đến 30A Mạch này được thiết kế

dành cho các ứng dụng điều khiển tốc độ và vị trí cho động cơ DC

Hình 2.6 Mạch điều khiển động cơ cầu H

 Đặc tính kỹ thuật của mạch:

 Dòng liên tục 10A

 Dòng đỉnh 30A (200 ms)

 Điện áp cấp từ 24V

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

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

 Bảo vệ ngắn mạch

 Dùng IC kích FET chuyên dụng IR2184

2.4.3 Cảm biến IMU GY-86

Cảm biến IMU GY-86 bao gồm các cảm biến đo lường quán tính như: cảm biến vận tốc góc (gyroscope) 3 trục, cảm biến gia tốc (accelerometer) 3 trục, cảm biến từ trường (magnetometer) 3 trục và cảm biến áp suất không khí (barometer)

Mô đun này được sử dụng nhiều trong việc điều khiển các thiết bị vận hành tự động cần định hướng như: robot tự hành, UAVs (thiết bị bay không người lái), robot và

xe hai bánh tự cân bằng hoặc các hệ thống cân bằng khác…

Các cảm biến trên mô đun hỗ trợ giao tiếp I2C với tốc độ lên tới 400kb/s và hoạt động ở mức áp 3.3V Mô đun được thiết kế tích hợp sẵn IC chuyển áp tín hiệu

và IC ổn áp LDO 3.3V, qua đó hoàn toàn tương thích với hệ thống ngoài với điện

áp là 3.3V hoặc 5V

Hình 2.7 Cảm biến IMU GY-86

 Các thông số kỹ thuật của cảm biến IMU GY-86

Hình 2.8 Động cơ DC Planet

 Các thông số kỹ thuật của động cơ DC

Bảng 2.4 Thông số kỹ thuật của động cơ DC Planet

Tốc độ chưa qua bộ giảm tốc 9000 vòng/phút

Tốc độ khi đã qua bộ giảm tốc 468 vòng/phút

2.4.5 Nguồn điện

Pin LI-PO được sử dụng làm nguồn cung cấp năng lượng cho robot hoạt động

Hình 2.9 Pin LI-PO sử dụng cho robot

 Thông số kỹ thuật của Pin LI-PO

 Công suất tối thiểu: 2650mAh

ra

Các tín hiệu PWM khi biến đổi thì có cùng một tần số và khác nhau về độ rộng của sườn dương hay sườn âm

Hình 2.10 Sơ đồ dạng xung điều chế PWM

Hình 2.10 là đồ thị dạng xung khi điều khiển bằng PWM với độ rộng xung đầu

ra tương ứng và được tính bằng %

 Ứng dụng của PWM trong điều khiển

PWM được ứng dụng nhiều trong điều khiển, điển hình nhất là điều khiển động cơ và các bộ xung áp, điều áp Sử dụng PWM điều khiển độ nhanh chậm của động cơ hay cao hơn nữa, PWM còn được dùng để điều khiển sự ổn định tốc độ động cơ

Ngoài lĩnh vực điều khiển hay ổn định tải thì PWM còn được sử dụng để điều chế các mạch nguồn như : boot, buck, nghịch lưu 1 pha và 3 pha [2]

Đường bus thực hiện truyền thông nối tiếp I2C gồm hai đường là đường truyền dữ liệu nối tiếp SDA và đường truyền nhịp xung đồng hồ nối tiếp SCL Vì cơ chế hoạt động là đồng bộ nên cần có một nhịp xung tín hiệu đồng bộ Các thiết bị hỗ trợ I2

C đều có một địa chỉ định nghĩa trước, trong đó một số bit địa chỉ thấp có thể cấu hình Thiết bị khởi tạo quá trình truyền thông là thiết bị chủ và cũng là thiết bị tạo xung nhịp đồng bộ, điều khiển cho phép kết thúc quá trình truyền Nếu thiết bị chủ muốn truyền thông với thiết bị khác sẽ gửi kèm thông tin địa chỉ của thiết bị muốn truyền trong dữ liệu truyền Thiết bị tớ đều được gán và định địa chỉ thông qua đó thiết bị chủ có thể thiết lập truyền thông và trao đổi dữ liệu Bus dữ liệu được thiết

kế để cho phép thực hiện nhiều thiết bị chủ và tớ ở trên cùng bus

 Quá trình truyền thông I2

C được mô tả như sau:

Tín hiệu START được tạo ra bởi thiết bị chủ, sau đó sẽ truyền đi dữ liệu 7 bit chứa địa chỉ của thiết bị tớ mà thiết bị chủ muốn truyền thông, theo thứ tự là các bit

có trọng số lớn nhất MSB sẽ được truyền trước Bit thứ tám tiếp theo sẽ chứa thông tin để xác định thiết bị tớ sẽ thực hiện vai trò nhận (0) hay gửi (1) dữ liệu Tiếp theo

sẽ là một bit ACK xác nhận bởi thiết bị nhận đã nhận được 1 byte trước đó hay không Thiết bị truyền (gửi) sẽ truyền đi 1 byte dữ liệu bắt đầu bởi MSB Tại điểm cuối của byte truyền, thiết bị nhận sẽ tạo ra một bit xác nhận ACK mới Khuôn mẫu

9 bit này (gồm 8 bit dữ liệu và 1 bit xác nhận) sẽ được lặp lại nếu cần truyền tiếp byte nữa Khi thiết bị chủ đã trao đổi xong dữ liệu cần và sẽ quan sát bit xác nhận ACK cuối cùng rồi sau đó sẽ tạo ra một tín hiệu dừng STOP để kết thúc quá trình truyền thông I2C là một giao diện truyền thông đặc biệt thích hợp cho các ứng dụng truyền thông giữa các thiết bị trên cùng một bo mạch với khoảng cách ngắn và tốc

độ thấp Ví dụ như truyền thông giữa CPU với các khối chức năng trên cùng một bo mạch như: EEPROM, cảm biến, đồng hồ tạo thời gian thực Hầu hết các thiết bị hỗ trợ I2C hoạt động ở tốc độ 400Kbps, một số cho phép hoạt động ở tốc độ cao vài Mbps I2C khá đơn giản để thực thi kết nối nhiều thiết bị vì I2C hỗ trợ cơ chế xác định địa chỉ

Hình 2.11 Sơ đồ kết nối theo giao thức I 2 C

Mỗi dây SDA hay SCL đều được nối với điện áp dương của nguồn cấp thông qua một điện trở kéo lên (pull‐up resistor) Sự cần thiết của các điện trở kéo này là

vì chân giao tiếp I2C của các thiết bị ngoại vi thường là dạng cực máng hở (open‐drain or open‐collector) Giá trị của các điện trở này khác nhau tùy vào từng thiết bị

và chuẩn giao tiếp, thường dao động trong khoảng 1KΩ đến 4.7KΩ

2.5.3 Giao tiếp UART

UART là viết tắt của (Universal Asynchronous Receiver Transmitter) Thường

là một mạch tích hợp được sử dụng trong việc truyền dẫn dữ liệu nối tiếp giữa máy tính và các thiết bị ngoại vi Rất nhiều vi điều khiển hiện nay đã được tích hợp UART, vì vấn đề tốc độ và độ điện dung của UART không thể so sánh với các giao tiếp mới hiện nay nên các dòng PC và Laptop đời mới không còn tích hợp cổng UART

Cũng như giao tiếp SPI và I2C có 1 dây truyền dữ liệu và 1 dây được sử dụng

để truyền xung clock (SCL) để đồng bộ trong giao tiếp Với UART thì không có dây SCL, vấn đề được giải quyết khi mà việc truyền UART được dùng giữa hai vi

xử lý với nhau, đồng nghĩa với việc mỗi vi xử lý có thể tự tạo ra xung clock

Để bắt đầu cho việc truyền dữ liệu bằng UART, một START bit được gửi đi sau đó là các bit dữ liệu và kết thúc quá trình truyền là STOP bit

Hình 2.12 Mô tả hoạt động của UART

Khi ở trạng thái chờ mức điện áp ở mức 1 (high) Khi bắt đầu truyền START bit sẽ chuyển từ 1 xuống 0 để báo hiệu cho bộ nhận là quá trình truyền dữ liệu sắp xảy ra Sau START bit là đến các bit dữ liệu D0-D7 (theo Hình 2.10 các bit này có thể ở mức 1 hoặc 0 tùy theo dữ liệu) Sau khi truyền hết dữ liệu thì đến bit Parity để

bộ nhận kiểm tra tính đúng đắn của dữ liệu truyền Cuối cùng là STOP bit báo cho thiết bị rằng các bit đã được gửi xong Thiết bị nhận sẽ tiến hành kiểm tra khung truyền nhằm đảm báo tính đúng đắn của dữ liệu

 Các thông số cơ bản trong truyền nhận UART:

 Baud rate (tốc độ baud): khoảng thời gian dành cho 1 bit được truyền Phải được cài đặt giống nhau ở gửi và nhận

 Frame (khung truyền): khung truyền quy định về số bit trong mỗi lần truyền

 Start bit: là bit đầu tiên được truyền trong một khung truyền Báo hiệu cho thiết bị nhận có một gói dữ liệu sắp được truyền đến

 Data: dữ liệu cần truyền Bit có trọng số nhỏ nhất LSB được truyền trước sau đó đến bit MSB

 Parity bit: kiểm tra dữ liệu truyền có đúng không

 Stop bit: là 1 bit hoặc nhiều bit báo cho thiết bị rằng các bit đã được gửi xong

 Thiết bị nhận sẽ tiến hành kiểm tra khung truyền nhằm đảm bảo tính đúng đắn của dữ liệu

2.6 Thu thập dữ liệu

Hình 2.13 Mô tả hoạt động thu thập dữ liệu

Để thu thập được dữ liệu từ cảm biến thì một trong những UART của bo Arduino Due được sử dụng UART đã được chuyển đổi với các thiết bị USB tích hợp và gửi dữ liệu đến máy tính thông qua cổng USB Dữ liệu sau đó được hiển thị trong "Serial monitor" được hỗ trợ bởi phần mềm Arduino và dữ liệu này được sử

dụng để vẽ dạng sóng ngõ ra của cảm biến trên phần mềm Labview

Chương 3

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

KHIỂN CÂN BẰNG 3.1 Nguyên lý điều khiển cân bằng

(a) Nghiêng trước (b) Cân bằng (c) Nghiêng sau

Hình 3.1 Mô tả quá trình cân bằng của robot hai bánh

Khi robot đứng thẳng tại vị trí cân bằng robot sẽ bị lực tác động làm cho robot ngã về phía trước hoặc phía sau như Hình 3.1 Nếu robot bị đổ về phía trước như Hình 3.1(a) thông qua cảm biến IMU robot sẽ xác định được góc nghiêng và điều khiển hai bánh xe di chuyển về phía trước theo hướng mà robot bị nghiêng Hay khi robot bị đổ về phía sau như Hình 3.1(c) thì robot sẽ điều khiển hai bánh xe di chuyển về phía sau theo hướng robot bị nghiêng cho đến khi góc nghiêng của robot bằng với góc mà tại đó robot đứng thẳng thì robot sẽ dừng lại [4]

3.2 Bộ lọc Kalman

3.2.1 Giới thiệu

Bộ lọc Kalman về cơ bản là một thuật toán xử lý dữ liệu truy hồi được sử dụng

để ước lượng trạng thái của hệ thống Khi đề cập đến trạng thái của hệ thống nghĩa

là đề cập đến vectơ bao gồm biến miêu tả một số đặc tính riêng biệt của hệ thống Ví dụ như góc nghiêng của robot được xác định dựa vào giá trị của cảm biến

con quay hồi chuyển và góc nghiêng của robot được xác định dựa vào giá trị đọc về

là sự thống kê tối ưu nhất đối với phép đo này

Bộ lọc Kalman được ứng dụng trong nhiều lĩnh vực, đặc biệt là trong điều khiển và dự đoán Bộ lọc Kalman được sử dụng trong điều khiển để ước lượng trạng thái Khi điều khiển một hệ thống điều quan trọng là phải biết được hệ thống

sẽ tiếp tục hoạt động như thế nào Trong một hệ thống phức tạp không phải lúc nào cũng có thể đo đạc được các biến, bộ lọc Kalman cung cấp thông tin không phải bằng cách đo trực tiếp mà bằng phương pháp ước lượng giá trị của các biến này và ước lượng nhiễu phép đo Bộ lọc Kalman có thể được sử dụng để điều khiển quy trình sản xuất, máy bay, tàu thủy, tàu vũ trụ và robot Bộ lọc Kalman được sử dụng

để dự đoán trạng thái tương lai của hệ thống, trạng thái mà con người rất khó hoặc không thể thực hiện như: dự đoán dòng chảy của các dòng sông trong cơn lũ, giá cả giao dịch của thực phẩm … [5]

3.2.2 Bộ lọc Kalman rời rạc

3.2.2.1 Quá trình ước lượng

Bộ lọc Kalman đề cập đến bài toán tổng quát nhằm ước lượng trạng thái của một quá trình được mô hình hóa một cách rời rạc theo thời gian bằng một phương trình ngẫu nhiên tuyến tính như sau:

(3.1)

Với giá trị đo đạc là :

Trong đó wk và vk là hai vector biến ngẫu nhiên đại diện cho nhiễu hệ thống

và nhiễu đo đạc Hai biến ngẫu nhiên này độc lập với nhau và được giả sử là tuân theo phân bố Gaussian với giá trị trung bình bằng 0 và ma trận hiệp biến (covariance matrix) lần lượt là Q và R

p(w) ~ N(0, Q) (3.3) p(v) ~ N(0, R) (3.4)

Trong thực tế, ma trận Q và R có thể thay đổi theo thời gian hoặc theo phép

đo, tuy nhiên các ma trận này được giả định là hằng số

Ma trận có kích thước là ma trận liên kết của trạng thái trước, bước thứ – và trạng thái hiện tại bước thứ Ma trận có kích thước là ma trận liên kết giữa ngõ vào tùy biến với trạng thái Ma trận có kích thước , trong phương trình phép đo (3.2) liên kết trạng thái với kết quả đo đạc Chú ý rằng các ma trận Q, R, A, H có thể thay đổi theo thời gian (từng bước ), nhưng ở đây chúng được giả sử là không đổi [5]

3.2.2.2 Giải thuật lọc Kalman rời rạc

Bộ lọc Kalman ước lượng tiến trình bằng cách sử dụng hình thức kiểm soát phản hồi: bộ lọc ước lượng trạng thái của tiến trình tại vài thời điểm và sau đó thu nhận sự phản hồi bằng các giá trị đo thực tế (có nhiễu) Như vậy, phương trình của

bộ lọc Kalman chia thành hai nhóm: phương trình cập nhật thời gian và phương trình cập nhật giá trị đo Phương trình cập nhật thời gian chịu trách nhiệm dự đoán trước (theo thời gian) sử dụng giá trị hiện tại và hiệp biến số ước lượng để dự đoán ước lượng tiền nghiệm cho thời điểm kế tiếp Phương trình cập nhật giá trị đo chịu trách nhiệm đối với sự phản hồi nghĩa là kết hợp giá trị mới với ước lượng tiền nghiệm nhằm hiệu chỉnh ước lượng hậu nghiệm

Phương trình cập nhật thời gian có thể được xem như là phương trình dự đoán, trong khi phương trình cập nhật phép đo có thể xem như phương trình hiệu chỉnh Thật vậy, thuật toán ước lượng cuối cùng rất giống với thuật toán dự đoán, hiệu chỉnh nhằm giải quyết các vấn đề số như trong Hình 3.2 [5]

Hình 3.2 Quy trình của bộ lọc Kalman

Các công thức tương ứng với quá trình cập nhật thời gian được cho trong công thức (3.5) và (3.6) Công thức tương ứng với quá trình cập nhật phép đo được cho trong công thức (3.7), (3.8) và (3.9)

Sau mỗi bước cập nhật thời gian và cập nhật phép đo, quá trình được lặp lại với ước lượng hậu nghiệm trước đó để dự đoán một ước lượng tiền nghiệm mới Quá trình lặp lại tự nhiên này là một đặc điểm khá thu hút của bộ lọc Kalman làm cho việc xây dựng bộ lọc Kalman dễ thực hiện hơn so với bộ lọc Wiener vốn được thiết kế để hoạt động trực tiếp trên tất cả các dữ liệu ước lượng Thay vì vậy, bộ lọc Kalman tham chiếu các giá trị ước lượng hiện tại với tất cả các phép đo trước đó Hình 3.3 cho thấy toàn bộ hoạt động của bộ lọc [6]

Hình 3.3 Quy trình hoàn chỉnh của bộ lọc Kalman

3.3 Xây dựng bộ lọc Kalman áp dụng cho cảm biến IMU

Cảm biến IMU gồm hai cảm biến tích hợp là cảm biến gia tốc và cảm biến con quay hồi chuyển Bộ lọc Kalman được sử dụng để kết hợp các giá trị từ cảm biến gia tốc và cảm biến con quay hồi chuyển để cho ra các góc điều khiển chính là: x, y,

Và cuối cùng đây được gọi là trạng thái hậu :

̂ (3.12)

̂ ước tính của trạng thái tại thời điểm k và bao gồm cả lúc k ở hiện tại

Vấn đề trạng thái hệ thống chính bị ẩn nên chỉ có thể được quan sát thông qua quan sát Điều này cũng được gọi là một mô hình Hidden Markov Điều này có nghĩa rằng trạng thái sẽ được dựa trên trạng thái tại thời điểm k và tất cả các trạng thái trước đó

Chiếc mũ trên các hệ số dùng để ước tính các trạng thái Vì vậy trạng thái được ký hiệu:

Trạng thái của hệ thống tại thời điểm k được cho bởi:

Tiếp theo là ma trận , đó là mô hình chuyển trạng thái được áp dụng cho tình trạng trước đó

Trong trường hợp này được định nghĩa là:

A = [

Tiếp theo là kiểm soát đầu vào , trong trường hợp này là đo con quay hồi chuyển trong độ mỗi giây (° / s ) tại thời điểm k , điều này cũng được gọi là tốc độ Viết lại phương trình trạng thái như sau:

̇ (3.16)

Tiếp theo là ma trận B Được gọi là mô hình kiểm soát đầu vào, được định nghĩa là:

là nhiễu quá trình đó là phân phối Gaussian với hệ số trung bình bằng không với hiệp phương sai đến thời điểm k

Có thể thấy ma trận hiệp biến phụ thuộc vào thời gian hiện tại k, do sự thay đổi của gia tốc

Giá trị đo lường :

Sự đo lường của trạng thái thực sự Quan sát được cho bởi:

 Phương trình bộ lọc Kalman:

 Dự đoán:

Hai phương trình đầu tiên để dự đoán trạng thái hiện tại và ma trận hiệp phương sai lỗi tại thời gian k Đầu tiên các bộ lọc sẽ cố ước tính trạng thái hiện tại dựa trên tất cả các trạng thái trước và dùng đo con quay hồi chuyển :

Đó cũng là lý do tại sao được gọi là kiểm soát đầu vào, vì là một đầu vào thêm

để ước tính trạng thái tại thời điểm hiện tại k đựa trên trạng thái trước đó Tiếp theo là phương trình ước tính tiên hiệp phương sai lỗi ma trận dựa vào

ma trận hiệp phương sai lỗi trước đó:

Ma trận H được sử dụng để lập bản đồ trạng thái vào không gian quan sát là

đo lường từ các gia tốc

trong trường hợp này không phải là một ma trận và chỉ được viết như sau: