thiết kế và chế tạo xe hai bánh tự cân bằng
LỜI CAM ĐOAN Kính gửi: Hội đồng bảo vệ đồ án tốt nghiệp Khoa Điện Tử - Viễn Thông, Trường Đại Học Dân Lập Duy Tân. Em tên là: Nguyễn Phạm Công Đức. Lớp: K15EVT, Khoa Điện Tử - Viễn Thông, Trường Đại Học Dân Lập Duy Tân. Em xin cam đoan nội dung của đồ án này không phải là bản sao chép của bất cứ đồ án hoặc công trình đã có từ trước. Đây là đồ án do em thực hiện dưới sự hướng dẫn trực tiếp của TS. Hà Đắc Bình. Mọi tham khảo dùng trong đồ án đều được trích dẫn rõ ràng tên tác giả, thời gian, địa điểm công bố. Mọi sao chép không hợp lệ, vi phạm quy chế đào tạo, hay gian trá, em xin chịu hoàn toàn trách nhiệm. Đà Nẵng, ngày 9 tháng 12 năm 2013 Sinh viên thực hiện LỜI CẢM ƠN Qua đồ án này em xin chân thành cảm ơn thầy giáo hướng dẫn TS. Hà Đắc Bình đã tận tình giúp đỡ em thực hiện đồ án, đồng thời em cũng xin cảm ơn Thầy TS. Đặng Việt Hùng và các thầy cô trong khoa Điện tử - Viễn thông đã luôn tạo điều kiện thuận lợi cho em học tập để thực hiện đồ án này một cách tốt nhất. MỤC LỤC 3.5 Quá trình thực nghiệm..............................................................................................................52 3.6.1. Dạng sóng đo được của cảm biến gia tốc.............................................................................54 Cần thiết kế thêm trong mạch con cảm biến vận tốc góc Gyro MPU6050. Từ đó sẽ cho ta biết được góc ngã chính xác của thân xe so với vị trí cân bằng hơn.......................................................59 Ưu điểm của Gyro:.........................................................................................................................59 DANH MỤC CÁC HÌNH VẼ Hình 1.1 Các module được thêm vào xe tự cân bằng........................................................................7 Hình 1.2 Các module được thêm vào xe tự cân bằng........................................................................8 Hình 1.3 Mô tả nguyên lý giữ thăng bằng..........................................................................................9 Hình 1.4 Mô tả cách bắt đầu di chuyển.............................................................................................9 Hình 1.5 Phân tích lực......................................................................................................................12 Hình 1.7 Đồ thị hàm truyền đáp ứng khi chưa có Kp, Ki, Kd............................................................14 Hình 1.8 Đồ thị hàm truyền theo thời gian ứng với giá trị Kp..........................................................15 Hình 1.9 Đồ thị hàm truyền theo thời gian ứng với giá trị Kp, Kd....................................................16 Hình 1.10 Đồ thị hàm truyền theo thời gian ứng với giá trị Kp, Ki...................................................17 Hình 1.11 Đồ thị hàm truyền theo thời gian ứng với giá trị Kp, Ki, Kd.............................................18 Hình 1.12 Mô tả vị trí của xe cân bằng............................................................................................18 Hình 1.13 Bộ lọc trung bình.............................................................................................................22 Hình 2.1 Mô hình khung đế đơn giản..............................................................................................24 Hình 2.2 Sơ đồ khối nguồn điện cung cấp toàn mạch.....................................................................25 Hình 2.3 Mạch cầu H L6203.............................................................................................................26 Hình 2.4 Sơ đồ chân IC L6203..........................................................................................................26 Hình 2.5 Sơ đồ khối mạch Driver.....................................................................................................27 Hình 2.6 Điều chế xung PWM..........................................................................................................28 Hình 2.8 Sơ đồ khối của cảm biến gia tốc góc ADXL335..................................................................29 Hình 2.9 Phân tích lực .....................................................................................................................30 Hình 2.10 Hệ trục tộ độ của cảm biến.............................................................................................30 Hình 2.11 Giá trị góc nghiêng tính theo hàm arcsin của gia tốc.......................................................31 Hình 2.12 Giao diện Kit Stellaris LM3S1968.....................................................................................32 Hình 2.13 Sơ đồ nguyên lý mạch điều khiển trung tâm...................................................................35 Hình 2.14 Khối điều khiển trung tâm sử dụng Kit Stellaris LM3S1968.............................................37 Hình 2.15 Mạch Driver sử dụng IC L6203........................................................................................37 Hình 2.16 Pin Li-ion 4.2V.................................................................................................................38 Hình 2.17 Động cơ DC......................................................................................................................39 Hình 2.18 Bánh xe............................................................................................................................40 Hình 2.19 Cảm biến gia tốc ADXL335...............................................................................................40 Hình 2.20 Khung xe..........................................................................................................................41 Hình 2.21 Mô hình xe tự cân bằng hoàn chỉnh................................................................................42 Hình 3.1 Sơ đồ khối chính................................................................................................................43 Hình 3.2 Sơ đồ khối điều khiển bằng thuật toán PID.......................................................................45 Hình 3.3 Sơ đồ khối đọc dữ liệu từ ADXL33....................................................................................46 Hình 3.4 Sơ đồ khối Cấu hình hoạt động ADC0...............................................................................46 Hình 3.5 Sơ đồ khối điều khiển động cơ..........................................................................................49 Hình 3.6 Xung trên chân X_OUT của cảm biến gia tốc khi robot ở vị trí cân bằng...........................55 Hình 3.7 Xung trên chân X_OUT của cảm biến gia tốc khi robot nghiêng về trước.........................55 Hình 3.8 Xung trên chân X_OUT của cảm biến gia tốc khi robot nghiêng về sau.............................56 Hình 3.9 Độ rộng xung PWM khi robot ở vị trí cân bằng.................................................................56 Hình 3.10 Độ rộng xung PWM khi robot ngã về phía sau................................................................57 Hình 3.11 Độ rộng xung PWM khi robot ngã về phía trước.............................................................57 Hình 2.6.Cảm biến Gyro MPU6050..................................................................................................60 DANH MỤC CÁC BẢNG BIỂU Bảng 1.1. Đặc tính của luật điều khiển P, I và D..............................................................................13 Bảng 2.1 Kiểm soát logic cho IC L6203.............................................................................................26 Bảng 3.1 Đặc tính của luật điều khiển P, I và D khi thử nghiệm......................................................54 DANH MỤC CÁC TỪ VIẾT TẮT P Propotional Tỉ lệ I Itergral Tích phân D Derivative đạo hàm PWM Pulse Width Modulation Điều chế độ rộng xung RAM Ramdom Access Memory Bộ nhớ truy xuất ngẫu nhiên ROM Read-Only Memory Bộ nhớ trong IC Ingrated Circuit Mạch điện tích hợp ADC Analog Digital Converter Bộ chuyển đổi tín hiệu tương tự sang tín hiệu số GPIO General Purpose Input Output điều khiển Ngõ vào ra để giao tiếp của vi 1 LỜI MỞ ĐẦU Với sự phát triển của khoa học công nghệ hiện nay, các mô hình Robot đang dần thay thế con người trong cuộc sống hằng ngày hay trong các quá trình sản xuất.Việc lên ý tưởng và thiết kế thành công một mô hình xe tự cân bằng trên 2 bánh đồng trục cũng đã được nhiều tác giả nghiên cứu và chế tạo thành công. Đề tài xe hai bánh tự cân bằng có thể xem là một cầu nối kinh nghiệm từ mô hình thăng bằng con lắc ngược đến việc nghiên cứu và chế tạo các loại robot di chuyển bằng 2 bánh trong tương lai. Mục tiêu của đề tài là thiết kế và chế tạo một xe hai bánh tự cân bằng. Đề tài này được quan tâm từ việc tính toán các thông số đầu vào và ra, dựa trên đó để xây dựng các mô phỏng, đến việc thiết kế mô hình, thực hiện phần điện tử và điều khiển, viết các chương trình điều khiển với mục đích cuối cùng là tạo ra một mô hình xe di chuyển cân bằng trên hai bánh xe đồng trục được lắp trên hai động cơ dựa theo các định luật cơ học Newton và lực quán tính. Mô hình là một chiếc xe có hai bánh được đặt dọc trục với nhau (khác với xe đạp là trục của hai bánh xe song song). Trên mô hình sử dụng các cảm biến để đo gia tốc góc của thân xe. Nhờ cảm biến này, xe sẽ có thể tự giữ thăng bằng và di chuyển. Trong hệ thống cảm biến, để loại trừ các tín hiệu nhiễu từ hệ thống và nhiễu từ tín hiệu đo, sai số của ngõ ra, đồng thời có thể ước lượng chính xác giá trị đo trong tương lai của cảm biến cũng như kết hợp các tín hiệu, bộ lọc tính trung bình được sử dụng nhằm cho một kết quả tốt hơn về gia tốc góc của xe. Từ các tín hiệu đo, thông qua một số đại lượng đặc trưng của mô hình (khối lượng, chiều dài, chiều cao vật, đường kính bánh…) ta sẽ tính được momen quán tính nghiêng (lật của mô hình), từ đó đưa ra các giá trị điều khiển phù hợp cho các bánh xe để giữ cho mô hình luôn đứng vững hoặc di chuyển với một vận tốc ổn định. Toàn bộ mô hình được điều khiển bằng một vi điều khiển LM3S1968. Đây là thế hệ tương đối cao cấp của họ Cortex-M3 based Microcontroller có thể xử lý và thực thi chương trình ở tốc độ cao (đạt đến 50MHz) trong việc tính toán các giá trị cảm biến và đưa ra bộ truyền động (động cơ điện). Bộ vi điều khiển đóng vai trò thứ nhất trong đề tài như một bộ lọc trung bình với tín hiệu vào từ gyro. Với các dữ liệu về góc đã xử lý và tín hiệu tạo xung cho động cơ DC, vai trò thứ hai của vi điều 2 khiển trong đề tài sẽ tính toán dựa vào thuật toán PID và đưa ra tín hiệu điều khiển bộ truyền động, đến bánh xe để giữ thăng bằng/di chuyển. 3 ĐẶT VẤN ĐỀ +Mục tiêu và phạm vi nghiên cứu Mục tiêu của đề tài là xây dựng xe hai bánh cân bằng di chuyển trên địa hình phẳng. Khả năng di chuyển cân bằng trên hai bánh làm phương tiện di chuyển hiệu quả và linh động hơn, dễ dàng xoay trở trong điều kiện không gian chật hẹp. Trong khuôn khổ 8 tuần thực hiện luận văn tốt nghiệp đại học, những mục tiêu của đề tài được đề ra như sau: − Tìm hiểu về các loại xe tự cân bằng, nguyên lý cơ bản về cân bằng. − Tính toán các tham số khi phân tích lực của mô hình. − Thiết kế bản vẽ, chế tạo mô hình theo kích thước thực. − Thiết kế mạch điều khiển trung tâm, làm nhiệm vụ xử lý tín hiệu đo và đưa ra các quyết định điều khiển. − Thiết kế mạch điện tử kết hợp các cảm biến thực hiện chức năng đo gia tốc góc. − Thiết kế mạch điều khiển công suất cho hai động cơ DC. − Kết hợp giải thuật PID cho vi điều khiển kết hợp và bù trừ cảm biến để có được giá trị gia tốc góc chính xác. − Xây dựng thuật toán điều khiển cho động cơ, giữ thăng bằng và ngăn ngừa quá tải của các bánh xe. − Lập trình điều khiển. +Phương pháp nghiên cứu − Tham khảo mô hình lý thuyết gồm có: • Tiếp cận từ mô hình con lắc ngược đến mô hình thật của đề tài. • Mô phỏng mô hình bằng MatLAB từ mô hình con lắc ngược. − Tiếp cận mô hình thực, gồm có: • Thiết kế khung sườn cơ khí của mô hình. • Mạch điều khiển bánh xe. • Mạch cảm biến gia tốc góc. • Bộ điều khiển trung tâm. • Lập trình vi điều khiển. Mô hình bao gồm một thân mang hai động cơ DC, bo mạch sử dụng bộ điều khiển trung tâm của Kit LM3S1968 của hãng TI để điều khiển mạch driver cho những động cơ, điều khiển những cảm biến cần thiết để đo các giá trị góc. Các tín hiệu đo gia tốc góc từ cảm biến gyro được thông qua một bộ lọc tính trung bình được lập trình trên vi điều khiển LM3S1968 để có các thông số đo góc chính xác. Bảng OLED trên Kit hiển thị các thông số hoạt động của xe. Pin được lắp 2 bên thân xe bằng nhựa để cung cấp toàn bộ năng lượng cho xe hoạt động. +Ý nghĩa khoa học và thực tiễn 4 -Ý nghĩa khoa học Xây dựng được một phương tiện vận chuyển mới trong khu vực chật hẹp. Làm phương tiện vận chuyển hàng hoá đến những nơi đã được lập trình sẵn ở trong các tòa nhà, phòng làm việc, những không gian chật hẹp, khó xoay trở. Thậm chí kết hợp trên các humanoid robot, nếu được kết hợp với các robot camera, robot dò đường, robot lái mặt đường thì hiệu quả các công dụng cụ thể cực kỳ linh hoạt. Tuy vậy, cần phải tiến hành giải quyết thêm về phần lên, xuống dốc cầu thang (không thể leo lên các bậc thang cao). -Tình hình nghiên cứu trong và ngoài nước Hiện nay chưa có thông tin cụ thể nào về việc chế tạo xe hai bánh tự cân bằng dùng trên robot cũng như xe hai bánh tự cân bằng ở Việt Nam. Nhưng trên thế giới, ở một vài nước, các kỹ thuật viên và một số sinh viên đã nghiên cứu và cho ra đời các dạng xe hai bánh như thế. Dưới đây là một số thông tin về chúng. + nBot[16] nBot do David P. Anderson sáng chế. nBot được lấy ý tưởng để cân bằng như sau: các bánh xe sẽ phải chạy xe theo hướng mà phần trên robot sắp ngã. Nếu bánh xe có thể được lái theo cách đứng vững theo trọng tâm robot, robot sẽ vẫn được giữ cân bằng. Trong thực tế, điều này đòi hỏi hai cảm biến thông tin phản hồi: cảm biến góc nghiêng để đo góc nghiêng của robot với trọng lực, và encoder trên bánh xe để đo vị trí cơ bản của robot. Bốn thông số ngõ vào để xác định hoạt động và vị trí của xe con lắc ngược cân bằng là: 1) góc nghiêng. 2) đạo hàm của góc nghiêng, vận tốc góc. 3) vị trí bánh xe. 4) đạo hàm vị trí bánh, vận tốc bánh xe. Bốn giá trị đo lường được cộng lại và phản hồi tới điện áp động cơ, tương ứng với momen quay, cân bằng, và bộ phận lái robot. + Balance bot I Balance-bot I (do Sanghyuk, Hàn Quốc thực hiện) là một robot hai bánh tự cân bằng bằng cách kiểm soát thông tin phản hồi. Hệ thống cao 50cm. Khung chính được làm bằng nhôm. Nó có hai trục bánh xe nối với hộp giảm tốc và động cơ DC cho sự phát động. Tổng cộng có ba bộ vi xử lý 5 Atmel được sử dụng. Vi điều khiển chính (master) thi hành những nguyên lý kiểm soát và thuật toán ước lượng. Một vi điều khiển khác kiểm soát tất cả cảm biến analog. Vi điều khiển thứ ba điều khiển động cơ DC. Linear quadratic regulator (LQR) được thiết kế và thực thi mạch điều khiển. Nó có bốn giá trị khác nhau – góc nghiêng, vận tốc góc nghiêng, góc quay bánh xe, và vận tốc góc quay, sau đó nó tạo lệnh cho động cơ DC để điều chỉnh tốc độ bánh xe. +BaliBot Balibot, một robot hai bánh cân bằng, là một trong các mẫu đầu tiên về robot hai bánh có trọng tâm phía trên các bánh xe. Không có hệ thống điều khiển hoạt động, robot sẽ bị ngã. Khi robot có nhận biết hướng mà nó sặp ngã, các bánh xe sẽ di chuyển về phí ngã và thẳng góc với chính nó. Cảm biến góc nghiêng để đo góc nghiêng của robot, gia tốc kế Motorola MMA2260 được sử dụng, thiết bị có cấu trúc MEMS. PIC16F7876 của hãng Microchip được chọn làm trung tâm điều khiển cho robot.PIC tích hợp một bộ biến đổi A/D nhiều kênh để đo cảm biến góc nghiêng và 6 các ngõ I/O để kiểm soát hai servo được mô tả cho sự quay vòng tiếp theo. Điện được cung cấp bằng 4 cục pin AA và được ổn áp dropout. Nguồn điện 6V không qua ổn áp được phân phối đến động cơ servo qua tụ điện 3300 µ F qua bù năng lượng cho vi mạch điều khiển khi công suất ngõ ra từ các servo được hoạt động. Mạch điện tử được xây dựng trên bảng project board Radio Shack RS 276-150 và lắp ráp phí trên các motor servo, trên khung bằng nhôm. Nguồn điện được đặt gần đỉnh và hoat động như trọng lượng của con lắc ngược. Một phiên bản khác của BaliBot sử dụng các cảm biến hồng ngoại để đo khoảng cách thay vì dùng các cảm biến đo góc. + Equibot Equibot là robot cân bằng do Dan Piponi thực hiện. Cơ bản nó dựa vào vi điều khiển ATMega32 RISC. Cả hai servo Hitec HS-311 chuẩn được sửa đổi cho xoay vòng 360o và nguồn điện vào được nối trực tiếp với các động cơ để PWM kiểm soát chúng. Một trong hai servo được gắn với bộ điều khiển tứ cực LQR, đó là phần phức tạp nhất trong cấu trúc robot, bánh còn lại bắt chước tốc độ của bánh thứ nhất. Equibot chỉ có một loại cảm biến hồng ngoại Sharp thay cho cảm biến về góc. Nó được đặt thấp để đo khoảng cách với sàn. Ngõ ra từ thiết bị được dùng để xác định hướng robot di chuyển. -Tình hình thực tế: Hiện tại, trong điều kiện đường xá giao thông ngày càng chật hẹp, không khí ngày càng ô nhiễm, việc nghiên cứu và chế tạo một mô hình xe điện gọn nhẹ, dễ xoay xở, không tốn nhiên liệu đốt là một nhu cầu thực sự cần thiết. Về khía cạnh khoa học và công nghệ, mô hình xe 2 bánh tự cân bằng thực sự là một bước đệm quan trọng để có kinh nghiệm trong việc tính toán mô hình và chế tạo các robot hai chân.Ngoài ra mô hình còn là sự bổ sung cần thiết về các giải pháp công nghệ di chuyển của các robot di động 3 bánh, 4 bánh…làm phong phú những lựa chọn giải pháp để chuyển dộng trong không gian cho các robot. 7 Về yếu tố tâm lý con người, mô hình xe hai bánh tự cân bằng thực sự là một dấu chẩm hỏi lớn cho những người chưa từng thấy hay dùng nó: tại sao xe có thể di chuyển và thăng bằng được? Điều này cuốn hút nhu cầu được sử dụng một chiếc xe hai bánh tự cân bằng. Với nhứng lý do khách quan như đã nêu, đề tài có lẽ có một nhu cầu nhất định trong tình hình hiện nay của Việt Nam cũng như toàn thế giới. CHƯƠNG 1 LÝ THUYẾT TIẾP CẬN 1.1 Tổng quan về Robot cân bằng. Động cơ bánh xe Khối cảm biến Bộ điều khiển Nguồn điều khiển Phần công suất Động cơ bánh xe Nguồn công suất Hình 1.1 Các module được thêm vào xe tự cân bằng Khối cảm biến Bộ phận không thể thiếu của xe cân bằng đó là khối cảm biến. Khối này có chức năng nhận biết (phân biệt) đâu là góc ngã của thân robot khi di chuyển. Thông thường là những loại cảm biến gia tốc, cảm biến vận tốc góc và cảm biến góc. Bộ điều khiển Bộ điều khiển là trung tâm xử lý của hệ thống xe. Bộ điều khiển có nhiệm vụ nhận tín hiệu từ khối cảm biến và phát tín hiệu điều khiển 2 động cơ bánh xe để di chuyển xe đúng theo ý đồ thiết kế. Khối công suất Nói đúng hơn đây là khối driver. Driver là một mạch điện có khả năng nhận xung điều khiển (thường là xung PWM) từ bộ điều khiển và xuất ra điện áp để điều khiển tốc độ 2 động cơ bánh xe. Driver là khối rất quan trọng của hệ thống, bởi nó 8 là một trong những yếu tố quan trọng ảnh hưởng tới độ chính xác của việc điều khiển. Khối nguồn Đúng nguyên tắc điều khiển, hệ thống sẽ có 2 nguồn riêng biệt gọi là “nguồn điều khiển” và “nguồn công suất”. Nguồn điều khiển là nguồn cấp điện cho bộ điều khiển, khối cảm biến thông thường có mức điện áp khoảng 5V. Trong khi đó nguồn công suất là nguồn cấp cho Driver, các động cơ, điện áp của nguồn này phụ thuộc vào điện áp của động cơ hoạt động. Thiết kế một bộ nguồn tốt cũng là một thành công trong việc làm xe tự cân bằng. Thiết bị ngoại vi Tùy thuộc vào đặc thù công việc thực hiện mà trên xe được trang bị nhiều thiết bị ngoại vi khác nhau như các module giao tiếp, camera, cơ cấu nâng (dùng trong việc sắp xếp hàng hóa ở nhà máy, cảng biển, nhà hàng)… Các module thông thường được dùng trong xe tự cân bằng có thể xem ở hình sau: RFID Wireless Khối cảm biến Các cảm biến va chạm… Khối lái (RC servo) Khối cảm biến Bộ điều khiển Cơ cấu tác động khác Động cơ bánh xe Phần công suất Động cơ bánh xe Encoder Encoder Nguồnđược côngthêm vào xe tự cân bằng. Nguồn điều Hình 1.2 Các module Hồi tiếp về bộ suất khiển 1.2 Thế nào là xe 2 bánh tự cân bằng điều khiển Battery 9 Cân bằng Bị nghiêng Hình 1.3 Mô tả nguyên lý giữ thăng bằng. Đối với các xe ba hay bốn bánh, việc thăng bằng và ổn định của chúng là nhờ trọng tâm của chúng nằm trong bề mặt chân đế do các bánh xe tạo ra. Đối với các xe 2 bánh có cấu trúc như xe đạp, việc thăng bằng khi không di chuyển là hoàn toàn không thể, vì việc thăng bằng của xe dựa trên tính chất con quay hồi chuyển ở hai bánh xe khi đang quay. Còn đối với xe hai bánh tự cân bằng, là loại xe chỉ có hai bánh với trục của hai bánh xe trùng nhau, để cho xe cân bằng, trọng tâm của xe (bao gồm các vật đặt trên xe) cần được giữ nằm ngay giữa các bánh xe. Điều này giống như ta giữ một cây gậy dựng thẳng đứng cân bằng trong lòng bàn tay. Thực ra, trọng tâm của toàn bộ xe không được biết nằm ở vị trí nào, cũng không có cách nào tìm ra nó, và có thể không có khả năng di chuyển bánh xe đủ nhanh để giữ nó luôn ở dưới toàn bộ trọng tâm. Về mặt kỹ thuật, góc giữa sàn xe và chiều trọng lực có thể biết được. Do vậy, thay vì tìm cách xác định trọng tâm nằm giữa các bánh xe, xe cần được giữ thẳng đứng, vuông góc với sàn xe (góc cân bằng khi ấy là zero). Hình 1.4 Mô tả cách bắt đầu di chuyển. Khi robot đứng cân bằng thì góc nghiêng θ của thân robot và trục thẳng đứng (trục gia tốc trọng trường) bằng 0. Khi robot nghiêng về phía trước θ > 0, nếu không có điều khiển thì theo quán tính, hai bánh xe sẽ chạy về phía sau (phía ngược lại của thân robot đang nghiêng), dẫn đến robot sẽ bị ngã. Nên trong trường hợp này, chúng ta sẽ điều khiển cho 2 10 bánh xe chạy về phía trước (phía mà robot đang nghiêng ) nhằm cho góc lệch θ = 0, robot sẽ thăng bằng trở lại. Ngược lại khi robot nghiêng về phía sau θ < 0, thì chúng ta điều khiển cho 2 bánh xe chạy về phía trước. 11 1.3 Phân tích lực Hệ thống xe hai bánh tự cân bằng được đặc trưng bởi các biến gia tốc trọng trường . Góc lệch của xe so với mặt phẳng nằm ngang là góc . Gia tốc do bánh xe gây ra có chiều ngược với bánh xe. Gia tốc g 1 tổng hợp từ gia tốc gia tốc trọng trường và gia tốc a của bánh xe được điều khiển. x ax a ay gx gy g1y g1 y g1x g 12 Hình 1.5 Phân tích lực Khi chưa có tác động của gia tốc a, xe chỉ chịu sự tác động của gia tốc trọng trường g = 9.8 m/s2: • gx = g* cos ( ) ; • gy = g* sin ( ) ; Tác động thêm gia tốc a: • ax = a* sin ( ) ; • ax = a* cos ( ) ; • g1x = gx + ax ; • g1 y = gy + ay; • g1 = g12x + g12y 1.4 Thuật toán điều khiển PID. PID là cách viết tắc của các từ Propotional (tỉ lệ), Integral (tích phân) và Derivative (đạo hàm). Tuy xuất hiện rất lâu nhưng đến nay PID vẫn là giải thuật điều khiển được dùng nhiều nhất trong các ứng dụng điều khiển tự động. P + ∑ Setpoint Error - I Kp*e(t) Ki*∫edt ∑ U D Kd* Hình 1.6 Sơ đồ khối thuật toán PID Hàm truyền của phương pháp điều khiển PID có dạng như sau: U = Kp*e + Ki*∫edt + Kd* • Kp = Hệ số tỉ lệ • Ki = Hệ số tích phân • Kd = Hệ số vi phân de dt Output Process 13 Trước tiên, ta tìm hiểu xem bộ điều khiển PID làm việc như thế nào, ta lấy sơ đồ hệ thống ở trên. Biến e thể hiện cho sai số giữa đầu vào mong muốn Setpoint và đầu ra Output. Tín hiệu lệch này sẽ được gửi đến bộ điều khiển PID và bộ điều khiển PID sẽ tính được tích phân và vi phân của sai số. Tín hiệu điều khiển U có giá trị bằng Kp lần biên độ của sai số cộng với Ki lần tích phân của sai số cộng với Kd lần vi phân của sai số. U = Kp*e + Ki*∫edt + Kd* de dt Tín hiệu U sẽ được gửi tới cơ câu chấp hành, và cho ra đầu ra Output. Đầu ra Output sẽ được gửi đến sensor đo và ta lại có giá trị sai số e mới. Bộ điều khiển sẽ lại lấy giá trị sai số mới, tính toán tích phân, vi phân và lại đưa ra tín hiệu điều khiển. Quá trình này sẽ được lặp đi lặp lại. Đặc tính của luật điều khiển P, I và D Luật điều khiển P có tác dụng làm giảm thời gian quá độ nhưng không thể khử được steady-state error. Luật điều khiển I có tác dụng khử được steady-state error, nhưng nó làm giảm khả năng đáp ứng tức thời của hệ thống. Luật điều khiển D có tác dụng tăng độ ổn định của hệ thống giảm overshoot, và cải thiện khả năng đáp ứng tức thời. Hiệu quả của từng luật điều khiển P, I, và D lên hệ thống vòng kín được tổng kết qua bảng sau: Thông số Kp Ki Kd Thời gian quá Độ vọt Thời gian đáp Steady-state độ Giảm Giảm Ít thay đổi lố Tăng Tăng Giảm ứng Ít thay đổi Tăng Giảm error Giảm Khử được Ít thay đổi Bảng 1.1. Đặc tính của luật điều khiển P, I và D. Sau đây là một minh họa bằng Matlab cho việc chọn Kp, Ki và Kd sao cho hệ thống đáp ứng được 3 tiêu chí sau: • Quá độ nhanh. • Overshoot nhỏ nhất. • Không có steady-state error. 14 Trước hết ta khảo sát đáp ứng bậc thang đơn vị của hệ thống hở. Tạo một m-file có các lệnh như sau: num=1; den=[1 10 20]; plant=tf(num, den); step(plant) Chạy m-file này ta sẽ có đồ thị như sau: Hình 1.7 Đồ thị hàm truyền đáp ứng khi chưa có Kp, Ki, Kd. Giá trị DC gain của hàm truyền là 1/20, do vậy 0.05 là giá trị cuối của output đáp ứng đầu vào bậc thang đơn vị. Như vậy steady-state error là 0.95, thực sự là khá lớn. Hơn nữa, thời gian quá độ khoảng 1 giây, thời gian để ổn định khoảng 1, 5 giây. Bây giờ nhiệm vụ của ta là thiết kế bộ điều khiển sao cho giảm thời gian quá độ, thời gian đến ổn định ngắn, và khử được steady-state error. Luật điều khiển P Chúng ta đã biết luật điều khiển P làm giảm thời gian quá độ, tăng overshoot, và giảm steady-state error. Chọn Kp = 300 và thêm vào m-file đoạn mã lệnh sau: Kp=300; contr=Kp; sys_cl=feedback(contr*plant, 1); t=0:0.01:2; 15 step(sys_cl, t) Chạy m-file trong MATLAB ta được kết quả như sau. Hình 1.8 Đồ thị hàm truyền theo thời gian ứng với giá trị Kp Chú ý: Hàm feedback dùng để tạo một hàm truyền vòng kín từ một hàm truyền vòng hở cho trước (thay cho việc thiết lập lại bằng tay). Từ đồ thị trên ta thấy rằng luật điều khiển P làm giảm thời gian quá độvà steady-state error, nhưng tăng độ quá điều chỉnh, và làm kéo dài thời gian đến ổn định đi 1 chút. Điều khiển tỉ lệ-vi phân Bây giờ chúng ta xét đến luật điều khiển PD. Như bảng đã nêu ở phần trước, ta thấy rằng luật điều khiển D làm giảm được độ quá điều chỉnh và làm hệ thống nhanh đến ổn định. Như trước ta vẫn lấy Kp = 300, lấy Kd = 10 và ta thêm vào m-file đoạn mã lệnh sau và chạy cho ra đồ thị. Kp=300; Kd=10; contr=tf([Kd Kp], 1); sys_cl=feedback(contr*plant, 1); t=0:0.01:2; step(sys_cl, t) 16 Hình 1.9 Đồ thị hàm truyền theo thời gian ứng với giá trị Kp, Kd. Đồ thị này cho ta thấy ngay luật điều khiển D đã giảm được độ quá điều chỉnh và làm hệ thống nhanh đến ổn định so với trước, và tác động không đáng kể tới thời gian quá độ cũng như steady-state error. Điều khiển tỉ lệ-tích phân Trước khi xây dựng bộ điều khiển PID, chúng ta xem xét tới luật điều khiển PI . Như bảng tổng kết, ta thấy rằng luật điều khiển I làm giảm thời gian quá độ, nhưng làm tăng độ quá điều chỉnh và kéo dài thời gian đến ổn định của hệ thống, nhưng lại khử được steady-state error. Bây giờ ta chọn lại Kp = 30, Ki = 70. Tạo m-file với các dòng lệnh sau. Kp=30; Ki=70; contr=tf([Kp Ki], [1 0]); sys_cl=feedback(contr*plant, 1); t=0:0.01:2; step(sys_cl, t) Chạy chương trình trong MATLAB ta được đồ thị sau: 17 Hình 1.10 Đồ thị hàm truyền theo thời gian ứng với giá trị Kp, Ki. Lý do chúng ta giảm Kp đi là vì luật điều khiển I cũng có tác dụng làm giảm thời gian quá độ và tăng độ quá điều chỉnh như luật điều khiển P. Đồ thị trên cho ta thấy luật điều khiển I đã khử được sai số xác lập. Điều khiển tỉ lệ-tích phân-vi phân(PID) Bây giờ ta xét đến bộ điều khiển PID. Sau một số lần chạy thử với các tham số khác nhau, ta chọn được Kp=350, Ki=300, và Kd=50 hệ thống sẽ đạt được đặc tính như mong muốn. Kp=350; Ki=300; Kd=50; contr=tf([Kd Kp Ki], [1 0]); sys_cl=feedback(contr*plant, 1); t=0:0.01:2; step(sys_cl, t) 18 Hình 1.11 Đồ thị hàm truyền theo thời gian ứng với giá trị Kp, Ki, Kd. Bây giờ ta đã có một hệ thống điều khiển vòng kín không có độ quá điều chỉnh, quá độ nhanh, và không có sai số xác lập. Áp dụng vào mô hình xa tự cân bằng Để có cái nhìn rõ hơn bản chất của giải thuật PID trong việc giúp xe tự cân bằng, Luận văn sẽ đề cập một ví dụ điều khiển tốc độ của một xe trên đường thẳng. Giả sử có một xe 2 bánh đồng trục có gắn 2 động cơ DC. Động cơ sinh ra một lực để đẩy xe chạy tới hoặc lui trên một đường thẳng để giúp xe đến được vị trí cân bằng. Bị nghiêng Cân bằng Bị nghiêng Hình 1.12 Mô tả vị trí của xe cân bằng 19 Gọi F là lực do động cơ tạo ra điều khiển xe. Ban đầu xe ở vị trí bị nghiêng nhiệm vụ đặt ra là điều khiển lực F (một cách tự động) để đẩy xe đến đúng vị trí cân bằng với các yêu cầu: chính xác (accurate), nhanh (fast response), ổn định (small overshot). Một điều rất tự nhiên, nếu vị trí hiện tại của xe bị nghiêng có góc nghiêng lớn so với vị trí cân bằng, hay nói cách khác sai số(error) lớn, chúng ta cần tác động lực F lớn để nhanh chóng đưa xe về vị trí cân bằng. Một cách đơn giản để công thức hóa ý tưởng này là dùng quan hệ tuyến tính: F=Kp*e Trong đó: + Kp là một hằng số dương nào đó mà chúng ta gọi là hệ số P +e là sai số cần điều khiển tức là độ chênh lệch giữa góc nghiêng và vị trí cân bằng của xe. Mục tiêu điều khiển là đưa e tiến về 0 càng nhanh càng tốt. Rõ ràng nếu Kp lớn thì F cũng sẽ lớn và xe rất nhanh chóng tiến về vị trí cân bằng. Tuy nhiên, lực F quá lớn sẽ làm cho gia tốc của xe rất nhanh (định luật II của Newton: F=ma). Khi xe đã đến vị trí cân bằng (tức e=0), thì tuy lực F=0 (vì F=Kp*e=F=Kp*0) nhưng do quán tính xe vẫn tiếp tục tiến về bên phải và lệch vị trí cân bằng về bên phải, sai số e lại trở nên khác 0, giá trị sai số lúc này được gọi là overshot (độ vọt lố). Lúc này, sai số e là số âm, lực F lại xuất hiện nhưng với chiều ngược lại để kéo xe về lại vị trí cân bằng. Nhưng một lần nữa, do Kp lớn nên giá trị lực F cũng lớn và có thể kéo xe lệch về bên trái vị trí cân bằng. Quá trình cứ tiếp diễn, xe cứ mãi dao động quanh vị trí cân bằng. Có trường hơp xe dao động càng ngày càng xa vị trí cân bằng. Bộ điều khiển lúc này được coi là không ổn định. Một đề xuất nhằm giảm độ vọt của xe là sử dụng một thành phần “thắng” trong bộ điều khiển. Sẽ rất lý tưởng nếu khi xe đang ở xa vị trí cân bằng, bộ điều khiển sinh ra lực F lớn nhưng khi xe đã tiến gần đến vị trí cân bằng thì thành phần “thắng” sẽ giảm tốc độ xe lại. Chúng ta đều biết khi một vật dao động quanh 1 điểm thì vật đó có vận tốc cao nhất ở tâm dao động (quanh vị trí cân bằng). Nói một cách khác, ở gần vị trí cân bằng sai số e của xe thay đổi nhanh nhất (cần phân biệt: e thay đổi nhanh nhất không phải e lớn nhất). Mặt khác, tốc độ thay đổi của e có thể tính bằng đạo hàm của biến này theo thời 20 gian. Như vậy, khi xe từ vị trí bị nghiêng tiến về gần vị trí cân bằng, đạo hàm của sai số e tăng giá trị nhưng ngược chiều của lực F (vì e đang giảm nhanh dần). Nếu sử dụng đạo hàm làm thành phần “thắng” thì có thể giảm được độ vọt lố của xe. Thành phần “thắng” này chính là thành phần D (Derivative) trong bộ điều khiển PID mà chúng ta đang khảo sát. Thêm thành phần D này vào bộ điều khiển P hiện tại, chúng ta thu được bộ điều khiển PD như sau: F = Kp*e+Kd* de dt Trong đó: + (de/dt) là vận tốc thay đổi của sai số e. +Kd là một hằng số không âm gọi là hệ số D (Derivative gain). Sự hiện diện của thành phần D làm giảm độ vọt của xe, khi xe tiến gần về vị trí cân bằng, lực F gồm 2 thành phần Kp*e > =0 (P) và Kd*(de/dt) < = 0 (D). Trong một số trường hợp thành phần D có giá trị lớn hơn thành phần P và lực F đổi chiều, “thắng” xe lại, vận tốc của xe vì thế giảm mạnh ở gần vị trí cân bằng. Một vấn đề nảy sinh là nếu thành phần D quá lớn so với thành phần P hoặc bản thân thành phần P nhỏ thì khi xe tiến gần vị trí cân bằng (chưa thật sự đến vị trí cân bằng), xe có thể dừng hẳn, thành phần D bằng 0 (vì sai số e không thay đổi nữa), lực F = Kp*e. Trong khi Kp và e lúc này đều nhỏ nên lực F cũng nhỏ và có thể không thắng được lực ma sát tĩnh. Hãy tưởng tượng tình huống bạn dùng sức của mình để đẩy một xe tải nặng vài chục tấn, tuy lực đẩy tồn tại nhưng xe không thể di chuyển. Như thế, xe sẽ đứng yên mãi dù sai số e vẫn chưa bằng 0. Sai số e trong tình huống này gọi là steady state error (sai số trạng thái tĩnh). Để tránh steady state error, người ta thêm vào bộ điều khiển một thành phần có chức năng “cộng dồn” sai số. Khi steady state error xảy ra, 2 thành phần P và D mất tác dụng, thành phần điều khiển mới sẽ “cộng dồn” sai số theo thời gian và làm tăng lực F theo thời gian. Đến một lúc nào đó, lực F đủ lớn để thắng ma sát tĩnh và đẩy xe tiến tiếp về vị trí cân bằng. Thành phần “cộng dồn” này chính là thành phần I (Integral - tích phân) trong bộ điều khiển PID. Vì chúng ta điều biết, tích phân một đại lượng theo thời gian chính là tổng của đại lượng đó theo thời gian. Bộ điều khiển đến thời điểm này đã đầy đủ là PID: 21 F= Kp*e + Ki*∫edt + Kd* de dt (chú ý: ∫edt là tích phân của biến e theo t) Trong đó: + ∫edt là tích phân của biến e theo t. +Ki là một hằng số không âm gọi là hệ số I (Integral gain). Như vậy, chức năng của từng thành phần trong bộ điều khiển PID giờ đã rõ. Tùy vào mục đích và đối tượng điều khiển mà bộ điều khiển PID có thể được lượt bớt để trở thành bộ điều khiển P, PI hoặc PD. Công việc chính của tác giả thiết kế bộ điều khiển PID áp dụng cho robot cân bằng là chọn các hệ số Kp, Kd và Ki sao cho bộ điều khiển hoạt động tốt và ổn định (quá trình này gọi là PID gain tuning). Đây không phải là việc dễ dàng vì nó phụ thuộc vào nhiều yếu tố. Vậy ta chọn các hệ số cho PID như sau: - Chọn Kp trước: thử bộ điều khiển P với mô hình xe cân bằng thật (hoặc mô phỏng), điều chỉnh Kp sao cho thời gian đáp ứng đủ nhanh, chấp nhận độ vọt nhỏ. - Thêm thành phần D để loại bỏ độ vọt lố, tăng Kd từ từ, thử nghiệm và chọn giá trị thích hợp. Steady state error có thể sẽ xuất hiện. - Thêm thành phần I để giảm steady-state error. Nên tăng Ki từ bé đến lớn để giảm steady state error đồng thời không để cho overshot xuất hiện trở lại. 1.5. Lọc nhiễu tín hiệu từ cảm biến Để xử lý nhiễu từ cảm biến, luận văn này sử dụng bộ lọc trung bình. Đây là kỹ thuật lọc tuyến tính, hoạt động như 1 bộ lọc thông thấp. Bộ lọc này được sử dụng để lọc các tín hiệu từ cảm biến gia tốc góc đọc về. Ở đây luận văn sử dụng một cửa sổ lọc(ma trận 1 x n) quét qua lần lượt các giá trị của cảm biến nhận được. Vậy giá trị đầu ra của cảm biến sau khi qua bộ lọc là giá trị trung bình của n giá trị liên tiếp mà vi điều khiển đọc về. Việc tính toán này khá đơn giản với hai bước gồm tính tổng các thành phần trong cửa sổ lọc và sau đó chia tổng này cho n phần tử của cửa sổ lọc. Mục đích của việc sử dụng bộ lọc trung bình này giúp cho vi điều khiển nhận được những giá trị trả về của cảm biến được mịn hơn và hạn chế được nhiễu tín hiệu. 22 Sẽ dễ hình dung hơn bằng mô tả các giá trị cảm biến đọc về như hình dưới đây: Hình 1.13 Bộ lọc trung bình 23 CHƯƠNG 2 THIẾT KẾ PHẦN CỨNG 2.1 Thiết kế cơ khí. Cấu trúc cơ khí của đề tài được chia làm hai phần: khung và hai động cơ. - Khung được làm bằng Nhựa (20 cm x 30 cm x 4 cm). - Hai động cơ đặt trên hai bánh xe bằng hợp kim nhôm có đường kính 10cm. Thiết kế phần đế robot Mặt đế Robot đóng vai trò vô cùng quan trọng trong việc làm tăng khả năng hoạt động ổn định của Robot, đặc biệt là đối với các loại Robot cân bằng. Yêu cầu cơ bản nhất cho phần đế Robot là độ cứ vững và độ đồng phẳng “ tương đối “ trên địa hình di chuyển, tất nhiên không phải địa hình di chuyển nào cũng giống nhau về nhiều yếu tố như độ phẳng, độ ma sát trên bánh xe ( phụ thuộc vào khả năng tiếp xúc của bánh xe chủ động so với mặt sàn ). + Độ cứng vững Phần này liên quan tới khả năng thiết kế của từng người, nếu có kinh nghiệm trong thiết kế cơ khí phần này tương đối đơn giản, quan trọng là làm sao cho những phần kết nối giữa trục động cơ, khung đế với phần thân của xe được chắc chắn. + Độ đồng phẳng Đây gần như là một trong những vấn đề quan trọng nhất khi tiến hành lắp ráp 1 khung đế Robot, như bạn đã biết, nếu làm một Robot với 3 bánh xe thì chẳng cần phải quan tâm đến điều này nhiều, vì 3 bánh sẽ luôn nằm trên một mặt phẳng, nhưng nếu muốn chế tạo 1 khung đế chỉ có 2 bánh thì vấn đề đặt ra là làm sao cho 2 bánh phải nằm trên 1 mặt phẳng. Đó không chỉ đơn giản là việc canh đều trên 1 tấm phằng cho đều mà cái chính là làm sao “giữ” được cho nó luôn phẳng, vì như bạn biết địa hình khi di chuyển là rất khác nhau vì thế không phải khi chế tạo thật phẳng thì di chuyển trên sân cũng phẳng. 24 Hình 2.1 Mô hình khung đế đơn giản 2.2 Mạch điện tử 2.2.1 Nguồn điện Hệ thống điện tử được lấy từ nguồn chính 12V(4 Pin li-on 4.2V nối tiếp nhau) được đưa qua IC ổn áp LM7805 để chuyển thành điện áp 5V cung cấp cho vi điểu khiển, các bảng mạch nối với nhau bằng cáp điện và kết nối với những nguồn điện khác nhau để cách ly xung nhiễu, trong đó 2 bảng mạch đóng vai trò chính là bộ lái công suất của mỗi bánh xe với công suất 2A, điện áp 12V, và một bảng mạch kết hợp giữa cảm biến gia tốc góc với kit điều khiển trung tâm LM3S1968. Sơ đồ khối làm việc của toàn bộ các thành phần điện – điện tử của mô hình có thể được mô tả theo hình dưới đây: 25 Gyro-61 ADXL 335 Nguồn điện Vi Điều Khiển toàn mạch(5V-12V) Kit LM3S1968 Động MạchCơ dirver DC1 OLED hiển thị Mạch dirver Hình 2.2 Sơ đồ khối nguồn điện cung cấp toàn mạch Cơ DC2 Tín hiệu từ cảm biến gyro được thu nhận vào Động vi điều khiển LM3S1968, được xử lý cùng với thuật toán PID để có được các lệnh điều khiển tốc độ bánh xe phù hợp. Các lệnh này được mã hoá thông qua mạch driver, đưa tới bộ MOSFET công suất để cung cấp năng lượng phù hợp (PWM duty cycle) cho hai động cơ hoạt động. 2.2.2 Mạch driver điều khiển động cơ 2.2.2.1 Điều khiển động cơ với IC L6203 IC L6203 là mạch điều khiển cầu đầy đủ cho các ứng dụng trong việc điều khiển động cơ DC thể hiện trong kỹ thuật MultiPower-BCD. Bằng cách sử dụng kỹ thuật tích hợp để tối ưu hóa các mạch logic và giai đoạn tổng hợp để đạt được hiệu suất tốt nhất có thể.Các stransistor DMOS đầu ra có thể hoạt động ở mức điện áp lên đến 42V đạt hiệu quả tốc độ chuyển đổi cao. Tất cả các thông số đầu vào là TL, CMOS với các cổng logic tương thích. Mỗi kênh(nữa cầu) của IC được điều khiển bới 1 cổng logic riêng biệt. Mạch tích hợp L6203 là mạch điều khiển cầu đầy đủ cho động cơ DC, nó có thể điều khiển động cơ ở mức điện áp từ 12V đến 42V và có tần số hoạt động hiệu quả cao lên đến 100KHz. 26 Hình 2.3 Mạch cầu H L6203 2.2.2.2 Kết nối chân cho IC L6203 Hình 2.4 Sơ đồ chân IC L6203 PWM H L L H X I/O Enable Function L H Động cơ quay nghịch H H Động cơ quay thuận L H Động cơ dừng H H Động cơ dừng X L Động cơ dừng L=low, H=hight, X=không xác định Bảng 2.1 Kiểm soát logic cho IC L6203 27 2.2.2.3 Mạch nguyên lý điều khiển động cơ Hình 2.5 Sơ đồ khối mạch Driver 2.2.2.4 Điều khiển tốc độ. Trong đề tài này sử dụng 2 động cơ DC để giúp robot chuyển động tới, lui. Tốc độ của động cơ được điều khiển bởi độ rộng xung(PWM). Điều chế độ rộng xung là một kỹ thuật để tăng giảm lượng điện năng cung cấp cho động cơ. Bởi vì không phải lúc nào động cơ cũng phải vận hành ở mức công suất tối đa hoặc tối thiểu mà nó phải được tuyến tính theo mỗi trạng thái nhất định. Để điểu khiển 1 động cơ, cần dùng 3 tín hiệu từ vi điều khiển LM3S1968. Trên Pin 11 của IC L6203 luôn đặt ở mức điện áp cao(5V) để cho phép mạch luôn hoạt động. Trên Pin 5(PWM) của L6203 đặt các tín hiệu PWM từ vi điều khiển vào.Điện áp của tín hiệu sẽ quyết định động cơ quay nhanh hay chậm. Trên Pin 7(I/O) sẽ do vi điều khiển quy ước là 0 hoặc 1(quay tới hoặc lui). Hình dưới đây sẽ minh họa cho khái niệm này, Ton và Toff lần lượt là khoảng thời gian kích(trạng thái HIGHT) và ngắt tín hiệu(trạng thái LOW) trong một chu kỳ. 28 Hình 2.6 Điều chế xung PWM Chu kỳ của một tín hiệu ra PWM là khoảng thời gian mà sau đó mẫu tín hiệu được lặp lại. • Tần số PWM = 1/chu kỳ. • Hệ số duty cyle là tỉ lệ bề rộng của mức “T ON”(mức 1) so với bề rộng chu kỳ. 2.2.3 Cảm biến gia tốc góc ADXL335 Cảm biến gia tốc góc ADXL335 là một module nhỏ, mỏng, điện năng thấp, đưa ra thông số trên 3 trục tọa độ X, Y, Z, giá trị đọc về của cảm biến sẽ là “hình chiếu của gia tốc trọng trường” trên từng trục tọa độ tương ứng . Nó có thể đo gia tốc tĩnh, tức là gia tốc trọng trường trong các ứng dụng cảm biến độ nghiêng, cũng như trong các chuyển động, sốc, hoặc rung động… Ta biết rằng, gia tốc trọng trường tại một điểm là không đổi (g = 9, 8 m/s 2). Khi cảm biến quay 1 góc nào đó, hệ trục tọa độ gắn với cảm biến cũng quay theo, do đó hình chiếu của gia tốc trọng trường lên các trục tọa độ đó sẽ thay đổi. Từ các giá trị đó, ta xác định được góc nghiêng hiện tại của cảm biến, cũng như góc mà cảm biến đã quay đi so với vị trí trước. Giá trị đọc về của các cảm biến gia tốc thường được tính theo đơn vị "g", g tức là gia tốc trọng trường(m/s2). Do đó kết quả tính toán góc nghiêng, sau khi chia cho nhau sẽ mất đi thành phần "g". Suy ra, kết quả đo góc không phụ thuộc gia tốc trọng trường, tức không phụ thuộc vào vị trí địa lý. 29 Về cảm biến gia tốc ADXL335: +Giá trị đọc về từ cảm biến là hình chiếu của g trên 3 trục toạ độ X, Y, Z. +Kết quả ngõ ra ở dạng tín hiệu tương tự, có thể lựa chọn kết quả dạng 10-bit hoặc 8-bit. +Nhiệt độ hoạt động: -40 ° ~ 85 °. +Độ nhạy +/-3g. +Giao tiếp theo chuẩn ADC. +Băng thông: 50Hz. +Điện áp hoạt động: 3 ~ 5V. +Tiêu thụ điện năng: 400uA. Hình 2.7 Cảm biến gia tốc ADXL335 Hình 2.8 Sơ đồ khối của cảm biến gia tốc góc ADXL335 Cảm biến gia tốc ba chiều kiểu tụ cho phép xác định một cách độc lập các gia tốc theo các phương trục toạ độ X, Y và Z. Theo sơ đồ khối ta thấy cảm biến này có ba đầu trên các kênh Xout, Yout và Zout (độ rộng xung tỉ lệ với gia tốc). Cảm biến có thể được sử dụng để đo cả gia tốc tĩnh và gia tốc động. Xác định góc nghiêng bằng giá trị gia tốc trên 1 trục: Góc nghiêng được xác định dựa vào giá trị gia tốc tĩnh. Luận văn này chỉ sử dụng trục X để tính toán góc nghiêng. Gọi θ chính là góc nghiêng của trục x so với mặt phẳng ngang. Ta gọi trục nằm ngang là (Δ), với chiều dương của góc nghiêng θ là chiều kim đồng hồ (chiều hướng về gia tốc trọng trường g). 30 Hình 2.9 Phân tích lực Ta có: gx=g*sin θx Giá trị gia tốc tĩnh đọc được từ các ngõ ra của cảm biến gia tốc chính là hình chiếu của g lên mỗi trục tương ứng. Như vậy ta sẽ xác định được: θx = arcsin(gx/g) Với θ nằm trong khoảng -90º → +90º. Hệ trục tọa độ: Hình 2.10 Hệ trục tộ độ của cảm biến Ngõ ra của các loại accelerometer là tuyến tính ở dạng analog, với ADXL335 độ nhạy danh định ở mode ±3g là 49 count/g, ở mod 10-bit dữ liệu. Giá trị góc θ tính theo phương pháp này càng chính xác khi càng gần 0º, và sai số rất lớn khi càng gần ± 90º. 31 Hình 2.11 Giá trị góc nghiêng tính theo hàm arcsin của gia tốc Theo đồ thị trên, ta thấy, khi giá trị góc lân cận 0º độ dốc đáp ứng nhỏ hơn nhiều so với khi góc theta tiến tới ±90º. Chính vì độ dốc lớn ở vùng gần ±90º làm giá trị góc nghiêng đọc ở vùng này thiếu chính xác. Với độ nhạy là 49 count/g. Với giá trị gia tốc đọc vào được là 0x01 và 0x02, tương ứng là 1 và 2 trong hệ thập phân, ta sẽ tính được góc nghiêng chênh lệch bao nhiêu. Ta có: θ(acc=1) = arcsin(1/49) = 1, 1o θ(acc=2) = arcsin(2/49) = 2, 2o Trong khi đó, với các giá trị gia tốc rất gần với 3g (tức là giá trị output gần với 49) thì độ chênh lệch lại rất lớn. θ(acc=48) = arcsin(48/49) = 87o. θ(acc=49) = arcsin(49/49) = 90o. 2.2.4 Bộ xử lý trung tâm – Kit LM3S1968 2.2.4.1 Tính năng của vi điều khiển LM3S1968 Mạch điều khiển động cơ là KIT LM3S1968 giao tiếp từ Gyro tới IC L2603. Bằng cách gởi các bit chỉnh hướng và tín hiệu PWM đến IC L2603, Kit này kiểm soát động tốc độ và hướng quay động cơ. 32 Hình 2.12 Giao diện Kit Stellaris LM3S1968 Các tính năng cơ bản của Kit Stellaris LM3S1968: • Vi điều khiển Stellaris LM3S1968 có nhiều Port xuất nhập đa dụng: Port A, Port B, Port C, Port D, Port E, Port F, Port G, Port H. • Thiết lập đơn giản, cáp USB giao tiếp nối tiếp, gỡ lỗi và cung cấp nguồn điện(5V) cho kit hoạt động. • Đồ họa OLED hiển thị với độ phân giải 128 x 96 điểm ảnh. • Bộ pin nội 3 V và hỗ trợ cho trên mô-đun khi chip hoạt động ở chế độ ngủ đông. • • • • • • • • Dãy điện áp cung cấp: 4, 37 - 5, 25 V từ kết nối USB. 42 kênh ngắt với 8 cấp độ ưu tiên khác nhau. 256 KB cho bộ nhớ flash. 64-KB cho bộ nhớ SRAM. Chân UARTs. 8 kênh ADC chuyển đổi tín hiệu tương tự sang tín hiệu số, độ phân giải 10-bit. 2 mô-đun I2C. Port để điều chế độ rộng xung PWM. - 1 Port 16-bit truy cập. - 2 Port so sánh. - Tạo ra hai tín hiệu PWM độc lập. • 5-52 GPIO là các chân tín hiệu vào ra. GPIO 33 Các mô-đun GPIO gồm 8 khối GPIO vật lý, mỗi tương ứng với một cổng GPIO riêng biệt (Port A, Port B, Port C, Port D, Port E, Port F, Port G, Port H). GPIO mô-đun hỗ trợ 5-52 chân lập trình đầu vào / đầu ra, tùy thuộc vào thiết bị ngoại vi được sử dụng. Các mô-đun GPIO có các tính năng sau đây : • 5-52 GPIO, tùy thuộc vào cấu hình. • Chịu được mức điện áp 5V trong cấu hình đầu vào. • Lập trình điều khiển ngắt cho GPIO: − Tạo mặt nạ ngắt. − Edge-kích hoạt lên, xuống hoặc cả hai. − Level-nhạy với giá trị cao hoặc thấp. • Mặt nạ che bit trong cả đọc và ghi các hoạt động thông qua các dòng địa chỉ. • Có thể được sử dụng để bắt đầu một chuỗi mẫu ADC. • Ổ đệm 2 mA, 4 mA và 8 mA cho truyền thông kỹ thuật số, lên đến bốn miếng đệm có thể điều khiển 18 mA cho các ứng dụng cao hiện tại. • Điều khiển tốc độ quay cho ổ đệm 8 mA. ADC Bộ chuyển đổi tương tự sang số (ADC) là một thiết bị ngoại vi có thể chuyển đổi liên tục một điện áp tương tự với một số tín hiệu số riêng biệt. Các mô-đun ADC hỗ trợ 8 kênh đầu vào cộng với một bộ cảm biến nhiệt độ bên trong. Bốn bộ đệm mẫu tuần tự cho phép lấy mẫu nhanh chóng lên đến 8 nguồn đầu vào tương tự mà không cần can thiệp điều khiển. Mỗi mẫu tuần tự cung cấp chương trình linh hoạt với nguồn đầy đủ cấu hình đầu vào, kích hoạt sự kiện và tạo ngắt. Mỗi module ADC có một chức năng so sánh kỹ thuật số cho phép các giá trị được chuyển đổi chuyển sang một đơn vị so sánh cung cấp tám bộ so sánh mẫu kỹ thuật số. Các mô-đun Stellaris ADC cung cấp các tính năng sau : • • • • • Tám kênh đầu vào tương tự. Cảm biến nhiệt độ bên trong chip. Tốc độ lấy mẫu là một triệu mẫu / giây. Linh hoạt, cấu hình chuyển đổi tương tự sang số. Điều khiển linh động các kích hoạt: - Bộ điều khiển ( phần mềm ). - Timers. - Analog Comparators. 34 - PWM. - GPIO. • Phần cứng trung bình lên đến 64 mẫu để cải thiện độ chính xác. • Chuyển đổi sử dụng một tài liệu tham khảo 3 -V nội bộ. • Nguồn và đất cho các mạch tương tự là riêng biệt với nguồn và đất của các tín hiệu số. I2C Bus giao tiếp giữ các IC với nhau (I 2C) cung cấp truyền dữ liệu hai chiều thông qua hai dây thiết kế (một dòng dữ liệu nối tiếp SDA và một dòng xung nối tiếp SCL). Các giao diện bus I2C đến I2C thiết bị ngoại vi như là bộ nhớ (RAMs và ROM), thiết bị mạng, màn hình LCD, máy phát nhạc. Bus I2C có thể được sử dụng để kiểm tra hệ thống và chuẩn đoán trong phát triển sản phẩm và sản xuất. Mỗi thiết bị trên bus I2C có thể được cấu hình Master hoặc Slave. Mô-đun I2C hỗ trợ việc gửi và nhận dữ liệu, có thể là một Master hoặc Slave hay có thể hoạt động đồng thời như Master và Slave. Cả hai chế độ Master và Slave có thể tạo ra ngắt. Vi điều khiển LM3S1968 bao gồm bốn mô-đun I2C với các tính năng sau: Thiết bị trên bus I2C có thể được cấu hình là Master hoặc Slave: − Hỗ trợ cả truyền và nhận dữ liệu như là một Master hoặc là một Slave. − Hỗ trợ hoạt động đồng thời Master và Slave. Bốn cấu hình I2C: − Master chỉ truyền. − Master chỉ nhận. − Slave truyền. − Slave nhận. Hai tốc độ truyền dẫn. − Tốc độ tiêu chuẩn (100 Kbps). − Chế độ nhanh (400 Kbps). Dao động mức thấp trong thời gian chờ ngắt. Master và Slave tạo ra ngắt thời gian chờ: 35 − Master tạo ra ngắt khi truyền hoặc nhận hoạt động hoàn thành (hoặc bị hủy bỏ do một lỗi). − Slave tạo ra ngắt khi dữ liệu đã được chuyển giao hay theo yêu cầu của một Master hay khi điều kiện START hoặc STOP phát hiện. 2.2.4.2 Mạch điều khiển trung tâm Hình 2.13 Sơ đồ nguyên lý mạch điều khiển trung tâm Vi điều khiển LM3S1968 đóng vai trò rất quan trọng trong mạch. Từ chân ADC0 của vi điều khiển được nối với chân X_OUT của cảm biến để nhận tín hiệu trả về. Từ đó nhờ bộ chuyển đổi ADC được tích hợp sẵn trong chip để chuyển đổi tín hiệu tương tự sang tín hiệu số. Các chân PWM4, PWM1 được nối với chân IN1 trong mạch Driver để xuất tín hiệu PWM điều khiển động cơ. Các chân PWM5, U1TX được nối với chân IN2 trong mạch Driver để xuất tín hiệu 0, 1 giúp mạch driver phân biệt được phải dùng tín hiệu PWM ở mức cao hoặc thấp. Nếu ở mức cao thì động cơ sẽ chạy tới ngược lại động cơ sẽ chạy lui. Các chân OUT1, OUT2 được nối 2 động cơ để cung cấp nguồn cho 2 động cơ hoaạt động. Các chân ENABLE được nối với nguồn 5V. Cài đặt PWM trong vi điều khiển: 36 Đối với điều khiển PWM bằng phần cứng từ vi điều khiển LM3S1968 có tích hợp bộ PWM sẵn trong kit.để bắt đầu PWM thì phần mềm của chip phải thực hiện các nhiệm vụ sau: • Đặt chu kỳ của counter điều chế xung vuông(lấy xung từ thạch anh). • Khởi tạo bộ PWM hoạt động. • Quy đinh chân GPIO_PIN_2 của PORT F và GPIO_PIN_2 của PORT D 2 chân là chân xuất tín hiệu PWM. • Quy đinh chân GPIO_PIN_3 của PORT F và GPIO_PIN_3 của PORT D 2 chân điều khiển quay tới hoặc lui của 2 động cơ. • Cấu hình bộ PWM. • Nạp duty cycle vào bộ điều khiển PWM. • Đặt trạng thái điều xung ở mức cao hay thấp.(Nếu cao thì sẽ điều khiển động - • • • • • • • • cơ quay tới, ngược lại quay lui). Cho phép bộ điều khiển PWM hoạt động. Đọc tín hiệu từ cảm biến gia tốc trả về và xuất ra OLED Khai báo độ sâu của kích thước mảng để lưu giá trị trả về từ cảm biến. Khởi tạo xung clock 16Mhz cho hệ thống. Kích hoạt module ADC0. Thiết lập tốc độ lấy mẫu 250k trong mỗi giây. Trước khi thiết lập ADC sequencer.Ta cần phải disable ADC sequencer 1. Cấu hình ADC sequencer. Cấu hình tất cả 4 steps trong sequencer ADC.Trong luận văn này sẽ đặt trung bình tất cả 4 steps sequencer làm nhiệm vụ lưu giá trị của cảm biến trả về. -Lưu ý: ở steps cuối cùng đòi hỏi phải cấu hình cờ ngắt để module ADC hiểu rằng đây là việc chuyển đồi cuối cùng trên sequencer 1. • Cho phép module ADC hoạt động. • Sau đó, kích hoạt bộ chuyển đổi ADC bằng phần mềm có tích hợp sẵn trong vi điều khiển. • Sau khi đã chuyển đổi hoàn thành, bây giờ có thể đọc giá trị của ADC từ ADC sample Sequencer 1.Hàm này có chức năng sao lưu mẫu dữ liệu chuyển đổi ADC chứa trong ADC Sample Sequencer 1 vào một bộ đệm trong bộ nhớ. 2.2.5 Hình chụp các mạch điện tử Một số hình ảnh về phần cứng của xe cân bằng: 37 Hình 2.14 Khối điều khiển trung tâm sử dụng Kit Stellaris LM3S1968 Hình 2.15 Mạch Driver sử dụng IC L6203 38 Hình 2.16 Pin Li-ion 4.2V Pin của xe có trọng lượng khá nặng cần phải được phân bố đều ở hai bên thành xe, không nên để lệch vì như thế rất có khả năng làm hai động cơ hoặc động ở tốc độ khác nhau. 39 Hình 2.17 Động cơ DC Các bộ phận phải được gắn chặt vào khung đế, sức nặng chia đều ở hai bánh xe chủ động có nghĩa là trọng tâm của phần khung đế nằm trên đường thẳng đối xứng dọc theo khung đế, các bánh xe phải tiếp xúc đều với mặt sàn. Giá đặt động cơ, pulley cần chú ý các mặt phẳng gá đảm bảo độ song song và vuông góc, đảm bảo cho mặt phẳng truyền động không bị nghiêng chéo (sẽ ảnh hưởng đến động lực học robot). 40 Hình 2.18 Bánh xe. Bánh xe khi gá vào động cơ phải đảm bảo sự đồng tâm khi quay không bị đảo, lắc ( lời khuyên nên dùng máy tiện để gia công cơ khí khi gá động cơ vào trục). Hình 2.19 Cảm biến gia tốc ADXL335 Khi đặt cảm biến gia tôc lên thân xe. Nên lưu ý đặt ở vị trí giữa thân xe vì lúc di chuyển cảm biến sẽ ít bị dao động nhất. Từ đó sẽ cho ta tín hiệu tốt và ít bị nhiễu hơn. 41 Hình 2.20 Khung xe. Khung xe là phần khá quan trọng, phải chọn cho phù hợp các yếu tố như độ cứng vững, kích thước, trọng lượng và quan trọng đây là những vật liệu không có sẵn bạn phải tự gia công cho phù hợp với thiết kế của mình. Phần khung cần lưu ý về kích thước (mặt chân đế), hình dáng sao cho gá đặt các bộ phận như động cơ, bánh xe, pin, các bảng mạch điện và cảm biến được dễ dàng. Để đơn giản, luận văn đã chọn khung xe là một tấm meca dày 10mm hay nhựa tấm dày 10mm cho rẻ tiền và dễ tìm mua. Do khung xe được làm bằng nhựa nên rất giòn dễ vỡ khi va đập mạnh, vì vậy cần phải có một sợi dây được nối vào đầu xe ngay ở trung tâm để giữ lại khi xe bị ngã. 42 Hình 2.21 Mô hình xe tự cân bằng hoàn chỉnh 43 CHƯƠNG 3 LẬP TRÌNH VÀ THỬ NGHIỆM 3.1 Sơ đồ khối điều khiển chính. Begin Khởi tạo ADC, PWM, OLED I_value, D_value, I_array[15], Error, Vantoc Kp, Ki, Kd Read ADC, hiển thị ADC lên màn hình OLED Tính toán Error Tính toán I_Value Tính toán D_Value Vantoc=Kp*Error + Ki* I_value + Kd* D_value Khối điều khiển động cơ Hình 3.1 chính Sơ đồ khối 44 3.2.Sơ đồ khối điều khiển bằng thuật toán PID Begin Khởi tạo ADC, PWM, LCD ulPeriod=10000, Center=553//Góc nghiêng =0 I_value, D_value, I_array[15], Error, vantoc, Read ADC, hiển thị ADC lên OLED ADC < Center ADC > Center Center = Center -5 Center = Center + 5 Error=ADC - Center Hiển thị OLED Dịch I_array[15] sang 1 vị trí I_array[0]=Error I_value = 1 45 1 D_value= I_array[0] - I_array[1] Vantoc=Kp*Error + Ki* I_value + Kd* D_value Vantoc > ulperiod - 11 Vantoc< -ulperiod+11 Vantoc= ulPeriod-11 Vantoc = -ulPeriod+11 Hình 3.2 Sơ đồ khối điều khiển bằng thuật toán PID Update Vantoc Hiển thị OLED 46 3.3 Sơ đồ khối Read ADC. 3.3.1 Sơ đô khối đọc dữ liệu từ ADXL33 Dữ liệu tương tự từ ADXL335 ADC chanel 0 Biến đổi điện áp đọc được thành giá trị tương ứng. Hiện thị lên LCD và xử lý Hình 3.3 Sơ đồ khối đọc dữ liệu từ ADXL33. 3.3.2 Cấu hình hoạt động ADC0 Khởi tạo xung clock 16MHz cho hệ thống Cấu hình ADC0 Kích hoạt ADC0 Nhận dữ liệu từ cảm biến Xóa cờ ngắt ADC0 Kích hoạt quá trình chuyển đổi Đợi hoàn thành quá trình chuyển đổi Xử lý giá trị được chuyển đổi Hình 3.4 Sơ đồ khối Cấu hình hoạt động ADC0 Chương trình đọc sensor ADXL335 #include "inc/hw_memmap.h" #include "inc/hw_types.h" 47 #include "driverlib/adc.h" #include "driverlib/gpio.h" #include "driverlib/sysctl.h" #include "drivers/rit128x96x4.h" #include "stdio.h" ////Bien ADC/// long data1; unsigned long gt[4]; volatile unsigned long Data_ADC; volatile unsigned long Data_ADC_1; static unsigned long sensor_array[10]; /************************************/ void ADC_INT() { //ENABLE ADC SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC0); SysCtlADCSpeedSet(SYSCTL_ADCSPEED_125KSPS); //---------------------------------------------------------------------------//TRIGGER TO CONVERT ADCSequenceConfigure(ADC0_BASE, 1, ADC_TRIGGER_PROCESSOR, 0); //---------------------------------------------------------------------------ADCSequenceStepConfigure(ADC0_BASE, 1, 0, ADC_CTL_CH0); ADCSequenceStepConfigure(ADC0_BASE, 1, 1, ADC_CTL_CH0); ADCSequenceStepConfigure(ADC0_BASE, 1, 2, ADC_CTL_CH0); ADCSequenceStepConfigure(ADC0_BASE, 1, 3, ADC_CTL_CH0 | ADC_CTL_IE | ADC_CTL_END); //---------------------------------------------------------------------------//ENABLE SEQUENCE 3 ADCSequenceEnable(ADC0_BASE, 1); //---------------------------------------------------------------------------} /****Mang dung cho bo loc****/ static unsigned long sensor_array[10]; void Read_ADC(void) { ADCIntClear(ADC0_BASE, 1); ADCProcessorTrigger(ADC0_BASE, 1); while(!ADCIntStatus(ADC0_BASE, 1, false)) { } ADCSequenceDataGet(ADC0_BASE, 1, gt); Data_ADC = (gt[0] + gt[1] + gt[2] + gt[3] )/4; Data_ADC_1 = Data_ADC; sensor_array[2]=sensor_array[1]; sensor_array[1]=sensor_array[0]; sensor_array[0]=Data_ADC; Data_ADC_1 = sensor_array[0] + sensor_array[1] + sensor_array[2]; 48 Data_ADC_1 = Data_ADC_1/3; } int main(void) { SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN); ADC_INT(); RIT128x96x4Init(1000000); RIT128x96x4StringDraw("ADCX", 0, 0, 15); while(1){ Read_ADC(); RIT128x96x4UDecOut3(Data_ADC_1, 30, 0, 15); } } 49 3.4 Sơ đồ khối điều khiển động cơ Begin Vi điều khiển nhận tín hiệu điều khiển động cơ Cấu hình chân PF3 và chân PD2 là chân điều khiển quay tới, lui cho động cơ thông qua biến flag(0, 1) Flag=0 Flag=1 động cơ quay tới động cơ quay lui Hình 3.5 Sơ đồ khối điều khiển động cơ. Xuất tín hiệu điều khiển động cơ quay nhanh hay chậm -Biến flag suy ra từ Vantoc chương trình điềuqua khiển thuật toán đếnbiến chân PF2 vàở chân PD1 thông biếnbằng Period PID. • Nếu biến Vantoc < 0 thì flag=1 thì động cơ sẽ quay tới. • Ngược lai, biến Vantoc Cho > 0 thì flagbộ=0PWM thì động sẽ quay lui. phép hoạtcơ động -Biến period được gọiđưa là độ Period 10.000) tínrộng hiệu xung(-10.000 độ rộng xung =0;tt--) { I_array[tt] = I_array[tt-1]; I_value = I_value + I_array[tt-1]; } D_value = I_array[0]-I_array[1]; Sự hiện diện của thành phần D làm giảm độ vọt của xe, khi xe tiến gần về vị trí cân bằng nhưng lại nãy sinh ra vấn đề sai số trong trạng thái tĩnh (steady state error) vì sẽ có những lúc khi xe gần đến vị trí cân bằng nhưng do thành phần D_value làm giảm tốc độ của xe, có lúc làm cho xe chưa đến vị trí cân bằng mà Vantoc đã gần bằng 0. 54 Để tránh vấn đề sai số trong trạng thái tĩnh lúc này tôi thêm vào bộ điều khiển thành phần I_Value có chức năng “cộng dồn” sai số theo thời gian và sẽ làm tăng giá trị Vantoc theo thời gian. Đến một lúc nào đó, giá trị Vantoc đủ lớn để thắng ma sát tĩnh và đẩy xe tiến tiếp về vị trí cân bằng Bộ điều khiển đến thời điểm này đã đầy đủ là PID: Vantoc = Vantoc = Kp*Error + Ki*I_Value + Kd*D_value Sau quá trình thực nghiệm tôi chọn Kp = 280, Ki = 20, Kd= -180 Và rút ra được đặc tính của các thông số như sau: Thông số Thời gian quá Độ vọt Thời gian đáp Steady-state độ Kp=280 Ki= 20 Giảm Giảm Kd= -180 Ít thay đổi lố Tăng Tăng Giảm ứng error Ít thay đổi Tăng Tăng Khử được Giảm Ít thay đổi Bảng 3.1 Đặc tính của luật điều khiển P, I và D khi thử nghiệm. 3.6. Kết quả thử nghiệm 3.6.1. Dạng sóng đo được của cảm biến gia tốc Sau khi hoàn thành việc lắp ráp hệ thống ta tiến hành kiểm tra hoạt động của robot và đo dạng xung thực tế của hệ thống để đối chiếu với lý thuyết. Khi robot ở vị trí cân bằng mức điện áp do cảm biến gia tốc trả về dao động trong khoảng 1.5436 V. 55 Hình 3.6 Xung trên chân X_OUT của cảm biến gia tốc khi robot ở vị trí cân bằng. Khi robot nghiêng về phía trước mức điện áp do cảm biến gia tốc trả về dao động trong khoảng 1.2276 V. Hình 3.7 Xung trên chân X_OUT của cảm biến gia tốc khi robot nghiêng về trước 56 Khi robot nghiêng về phía sau mức điện áp do cảm biến gia tốc trả về dao động trong khoảng 1.8684 V. Hình 3.8 Xung trên chân X_OUT của cảm biến gia tốc khi robot nghiêng về sau 3.6.2. Dạng sóng đo được của xung PWM Khi robtot ở vị trí cân bằng, lúc này giá trị của cảm biến gia tốc trả về là ADXL = 558, vì robot đang dao động ở vị trí cân bằng nên độ rông xung PWM khi này rất nhỏ. Hình 3.9 Độ rộng xung PWM khi robot ở vị trí cân bằng 57 Khi robot ngã về phía sau, lúc đó cảm biến gia tốc trả về có giá trị ADXL = 571, vì robot đang có xu hướng ngã về phía sau nên độ rộng xung lúc này được vi điều khiển lấy ở mức âm để điều khiển động cơ quay theo chiều ngược giúp robot tiến về sau. Hình 3.10 Độ rộng xung PWM khi robot ngã về phía sau Khi robot ngã về phía trước, lúc đó cảm biến gia tốc trả về có giá trị ADXL = 546, vì robot đang có xu hướng ngã về phía trước nên độ rộng xung lúc này được vi điều khiển lấy ở mức dương để điều khiển động cơ quay theo chiều thuận giúp robot tiến về trước. Hình 3.11 Độ rộng xung PWM khi robot ngã về phía trước 58 3.6.3 Kết luận Sau nhiều lần thử nghiệm, mô hình robot đã đạt được những kết quả sau: -Theo các xung đo được trên cảm biến gia tốc ADXL335 tôi thấy cảm biến hoạt động tương đối đúng với lý thuyết. -Xác định được độ lệch của thân xe so với vị trí cân bằng. -Xác định được các thông số Kp,Ki,Kd. -Thời gian đáp ứng của động cơ đủ nhanh để đưa thân xe về vị trí cân bằng lúc xe bị nghiêng (những góc nhỏ) nhưng vẫn còn bị tình trạng vọt lố. Tùy thuộc vào vị trí ban đầu của xe khi tiếp xúc với mặt phẳng mà độ vọt lố nhiều hay ít. -Robot vẫn chưa thật sự ổn định, vẫn còn dao động mạnh khi ở vị trí cân bằng do giá trị cảm biến trả về ở vị trí này có sự sai lệch quá lớn. 59 KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 1.Những kết quả đạt được Thiết kế và hoàn thiện mô hình cơ khí xe tự cân bằng. • Thiết kế mạch driver có thể điều khiển động cơ với công suất 2A, điện áp 12V. • Thiết kế mạch điện phối hợp giữa cảm biến gia tốc và vi điều khiển. • Thiêt kế và chế tạo khung xe hoàn chỉnh giúp xe có thể chịu được những va đập mạnh trong khi thử nghiệm. Tiếp cận và áp dụng tương đối thành công thuật toán PID vào mô hình. Trong quá trình thực hiện luận văn đã giúp tôi tiếp cận với chip ARM Cortex-M4 cụ thể là vi xử lý LM3S1968 Stellaris của TI, qua đó giúp tôi hiểu được một số tính năng của nó như: GPIO, PWM, ADC… Mặc dù chưa khai thác hết những ứng dụng của vi xử lý nhưng qua đó cũng giúp ích cho tôi rất nhiều về khả năng tìm kiếm tài liệu, bổ sung rất nhiều kiến thức về vi điều khiển mà tôi còn thiếu và giúp tôi định hướng phát triển công việc sau này. 2.Những kết quả chưa đạt được Xe tự cân bằng vẫn chưa giữ được sự ổn định cần thiết, vẫn còn rung và lắc khi ở vị trí cân bằng. Động cơ của xe không đủ mạnh (2 vòng/s) để tạo độ vọt khi xe ngã ở những góc lớn. 3.Những vấn đề chưa giải quyết Giá trị sensor trả về là gia tốc góc chứ không phải là góc nghiêng của xe. Đặc biệt khi ở vị trí cân bằng giá trị trả về thiếu chính xác. Ban đầu mới thử nghiệm thì giá trị trả về của cảm biến ở vị trí cân bằng là 555 nhưng sau 1 thời gian thì giá trị thay đổi là 557, 558. Nguồn của xe cũng không thật sự ổn định.Khi Pin mới được sạc thì động cơ vọt mạnh nhưng sau đó 1 thời gian thì động cơ có vẻ yếu lại.Vì vậy rất khó khăn trong việc xác định các thông số Kp, Ki, Kd trong thuật toán PID. 4.Hướng phát triển. • Cần thiết kế thêm trong mạch con cảm biến vận tốc góc Gyro MPU6050. Từ đó sẽ cho ta biết được góc ngã chính xác của thân xe so với vị trí cân bằng hơn. Ưu điểm của Gyro: +Quay với góc chính xác +Ít phụ thuộc yếu tố bên ngoài 60 +Giá rẻ (tầm khoảng 200.000 ngàn). +Chuẩn giao tiếp I2C Hình 2.6.Cảm biến Gyro • Sử dụng động cơ tốt hơn, mạnh hơn để ngoài việc xe di chuyển được còn MPU6050 có khả mang chở đồ vật nặng. • Sử dụng loại pin ổn định hơn có thời gian hoạt động lâu hơn. • Phối hợp hai thuật toán PID và Q Learning. 61 TÀI LIỆU THAM KHẢO Tài liệu ngoài nước: [1] Stellaris® Peripheral Driver Library USER’S GUIDE, Luminary Micro, September 29, 2008. [2] Data Sheet ADXL335, Small, Low Power, 3-Axis ±3 g Accelerometer. [3] Data Sheet L6201, L6202, L6203. Tài liệu wedsite: [1] picvietnam.com/download/thuyetminh.pdf [2] www.ti.com/litv/pdf/spmu052c [3] www.ti.com/product/lm3s1968 . PHỤ LỤC Code chương trình: #include "inc/hw_memmap.h" #include "inc/hw_types.h" #include "driverlib/adc.h" #include "driverlib/gpio.h" #include "driverlib/sysctl.h" #include "stdio.h" #include "inc/hw_ints.h" #include "inc/hw_memmap.h" #include "inc/hw_types.h" #include "driverlib/debug.h" #include "driverlib/gpio.h" #include "driverlib/pwm.h" #include "driverlib/sysctl.h" #include "driverlib/interrupt.h" #include "drivers/rit128x96x4.h" #include "driverlib/timer.h" #include "math.h" #include "stdlib.h" #include //dinh nghia dc 1 #define MOTOR_PORT GPIO_PORTF_BASE #define MOTOR_PORT_INT INT_GPIOF #define MOTOR_PORT3 GPIO_PORTH_BASE //#define MOTOR_PORT_INT3 INT_GPIOH #define MOTOR_PORT_ENABLE SYSCTL_PERIPH_GPIOF //#define MOTOR_PORT_ENABLE3 SYSCTL_PERIPH_GPIOH //#define PORTG SYSCTL_PERIPH_GPIOG #define PIN_PWM GPIO_PIN_2 #define TURN_LR GPIO_PIN_3 //#define ENABLE GPIO_PIN_0 #define LEFT 0 #define RIGHT 8 #define LED GPIO_PIN_2 //dinh nghia dc 2 #define MOTOR_PORT1 GPIO_PORTD_BASE #define MOTOR_PORT2 GPIO_PORTG_BASE #define MOTOR_PORT_INT1 INT_GPIOD #define MOTOR_PORT_INT2 INT_GPIOG #define MOTOR_PORT_ENABLE1 SYSCTL_PERIPH_GPIOD #define MOTOR_PORT_ENABLE2 SYSCTL_PERIPH_GPIOG #define CHANNEL_A1 GPIO_PIN_0 #define CHANNEL_B1 GPIO_PIN_2 #define CHANNEL_C1 GPIO_PIN_1 #define PIN_PWM1 GPIO_PIN_1 #define TURN_LR1 GPIO_PIN_3 //#define ENABLE1 GPIO_PIN_1 /*****************************/ static int dc1_flag; static int dc2_flag; static signed long van_toc; // -ulPeriod [...]... bánh có cấu trúc như xe đạp, việc thăng bằng khi không di chuyển là hoàn toàn không thể, vì việc thăng bằng của xe dựa trên tính chất con quay hồi chuyển ở hai bánh xe khi đang quay Còn đối với xe hai bánh tự cân bằng, là loại xe chỉ có hai bánh với trục của hai bánh xe trùng nhau, để cho xe cân bằng, trọng tâm của xe (bao gồm các vật đặt trên xe) cần được giữ nằm ngay giữa các bánh xe Điều này giống... bánh xe Encoder Encoder Nguồnđược côngthêm vào xe tự cân bằng Nguồn điều Hình 1.2 Các module Hồi tiếp về bộ suất khiển 1.2 Thế nào là xe 2 bánh tự cân bằng điều khiển Battery 9 Cân bằng Bị nghiêng Hình 1.3 Mô tả nguyên lý giữ thăng bằng Đối với các xe ba hay bốn bánh, việc thăng bằng và ổn định của chúng là nhờ trọng tâm của chúng nằm trong bề mặt chân đế do các bánh xe tạo ra Đối với các xe 2 bánh. .. trên robot cũng như xe hai bánh tự cân bằng ở Việt Nam Nhưng trên thế giới, ở một vài nước, các kỹ thuật viên và một số sinh viên đã nghiên cứu và cho ra đời các dạng xe hai bánh như thế Dưới đây là một số thông tin về chúng + nBot[16] nBot do David P Anderson sáng chế nBot được lấy ý tưởng để cân bằng như sau: các bánh xe sẽ phải chạy xe theo hướng mà phần trên robot sắp ngã Nếu bánh xe có thể được lái... việc giúp xe tự cân bằng, Luận văn sẽ đề cập một ví dụ điều khiển tốc độ của một xe trên đường thẳng Giả sử có một xe 2 bánh đồng trục có gắn 2 động cơ DC Động cơ sinh ra một lực để đẩy xe chạy tới hoặc lui trên một đường thẳng để giúp xe đến được vị trí cân bằng Bị nghiêng Cân bằng Bị nghiêng Hình 1.12 Mô tả vị trí của xe cân bằng 19 Gọi F là lực do động cơ tạo ra điều khiển xe Ban đầu xe ở vị trí... cứu và chế tạo một mô hình xe điện gọn nhẹ, dễ xoay xở, không tốn nhiên liệu đốt là một nhu cầu thực sự cần thiết Về khía cạnh khoa học và công nghệ, mô hình xe 2 bánh tự cân bằng thực sự là một bước đệm quan trọng để có kinh nghiệm trong việc tính toán mô hình và chế tạo các robot hai chân.Ngoài ra mô hình còn là sự bổ sung cần thiết về các giải pháp công nghệ di chuyển của các robot di động 3 bánh, ... các robot di động 3 bánh, 4 bánh làm phong phú những lựa chọn giải pháp để chuyển dộng trong không gian cho các robot 7 Về yếu tố tâm lý con người, mô hình xe hai bánh tự cân bằng thực sự là một dấu chẩm hỏi lớn cho những người chưa từng thấy hay dùng nó: tại sao xe có thể di chuyển và thăng bằng được? Điều này cuốn hút nhu cầu được sử dụng một chiếc xe hai bánh tự cân bằng Với nhứng lý do khách quan... được giữ cân bằng Trong thực tế, điều này đòi hỏi hai cảm biến thông tin phản hồi: cảm biến góc nghiêng để đo góc nghiêng của robot với trọng lực, và encoder trên bánh xe để đo vị trí cơ bản của robot Bốn thông số ngõ vào để xác định hoạt động và vị trí của xe con lắc ngược cân bằng là: 1) góc nghiêng 2) đạo hàm của góc nghiêng, vận tốc góc 3) vị trí bánh xe 4) đạo hàm vị trí bánh, vận tốc bánh xe Bốn... Robot cân bằng Động cơ bánh xe Khối cảm biến Bộ điều khiển Nguồn điều khiển Phần công suất Động cơ bánh xe Nguồn công suất Hình 1.1 Các module được thêm vào xe tự cân bằng Khối cảm biến Bộ phận không thể thiếu của xe cân bằng đó là khối cảm biến Khối này có chức năng nhận biết (phân biệt) đâu là góc ngã của thân robot khi di chuyển Thông thường là những loại cảm biến gia tốc, cảm biến vận tốc góc và cảm... bánh xe Bốn giá trị đo lường được cộng lại và phản hồi tới điện áp động cơ, tương ứng với momen quay, cân bằng, và bộ phận lái robot + Balance bot I Balance-bot I (do Sanghyuk, Hàn Quốc thực hiện) là một robot hai bánh tự cân bằng bằng cách kiểm soát thông tin phản hồi Hệ thống cao 50cm Khung chính được làm bằng nhôm Nó có hai trục bánh xe nối với hộp giảm tốc và động cơ DC cho sự phát động Tổng cộng... PHẦN CỨNG 2.1 Thiết kế cơ khí Cấu trúc cơ khí của đề tài được chia làm hai phần: khung và hai động cơ - Khung được làm bằng Nhựa (20 cm x 30 cm x 4 cm) - Hai động cơ đặt trên hai bánh xe bằng hợp kim nhôm có đường kính 10cm Thiết kế phần đế robot Mặt đế Robot đóng vai trò vô cùng quan trọng trong việc làm tăng khả năng hoạt động ổn định của Robot, đặc biệt là đối với các loại Robot cân bằng Yêu cầu ... chân đế bánh xe tạo Đối với xe bánh có cấu trúc xe đạp, việc thăng không di chuyển hoàn toàn không thể, việc thăng xe dựa tính chất quay hồi chuyển hai bánh xe quay Còn xe hai bánh tự cân bằng, ... bằng, loại xe có hai bánh với trục hai bánh xe trùng nhau, xe cân bằng, trọng tâm xe (bao gồm vật đặt xe) cần giữ nằm bánh xe Điều giống ta giữ gậy dựng thẳng đứng cân lòng bàn tay Thực ra, trọng... thông tin cụ thể việc chế tạo xe hai bánh tự cân dùng robot xe hai bánh tự cân Việt Nam Nhưng giới, vài nước, kỹ thuật viên số sinh viên nghiên cứu cho đời dạng xe hai bánh Dưới số thông tin