Các phương pháp xử lý giá trị trả về từ cảm biến thành vị trí của xe so với đường line: - Phương pháp so sánh: sử dụng mạch lấy ngưỡng 0, 1 hoặc giải thuật lọc ngưỡng bằng lập trình để c
ĐỀ BÀI THIẾT KẾ
Đề bài 3: Robot dò line phát hiện vật cản: Thiết kế robot dò line phát hiện tuyến đường có vật cản và lựa chọn tuyến đường phù hợp Yêu cầu vận hành của hệ thống:
- Robot đi từ vị trí “bắt đầu” di chuyển đến một trong hai vị trí “kết thúc”
- Vật cản được đặc ngẫu nhiên ở một trong hai “khu vực xuất hiện vật cản” Robot được yêu cầu phát hiện ra tuyến đường có vật cản và di chuyển trên tuyến đường còn lại
- Vật cản là khối hình trụ có đường kính tiết diện 50 mm
Khu vực xuất hiện vật cản
Hình 1.1 Sơ đồ sa bàn cho robot dò line phát hiện vật cản
Các đặc tính của sa bàn được mô tả:
- Nền sa bàn màu trắng
- Di chuyển trong mặt phẳng
- Tốc độ di chuyển thấp nhất 0.1 𝑚/𝑠
- Sai số bám đường dẫn ± 3 𝑚𝑚
- Sai số vị trí dừng cuối đường dẫn ± 5 𝑚𝑚
TỔNG QUAN
Cơ khí
Hình 2.2 Sơ đồ nguyên lý cơ của Fire Bird V Robot Đặc điểm: Kết cấu xe có 3 bánh, hai động cơ DC dẫn động hai bánh sau, bánh tự lựa phía trước hỗ trợ dẫn hướng Xe điều hướng nhờ sự chênh lệch tốc độ giữa hai động cơ, sinh ra vận tốc góc 𝜔 giúp xe cua theo hướng động cơ có vận tốc nhỏ hơn Liên hệ giữa 𝜔 và vận tốc bánh trái 𝑣 𝐿𝑒 và vận tốc bánh phải 𝑣 𝑅𝑖 :
Hình 2.3 Phân tích động học của kết cấu xe 2 bánh dẫn động, 1 bánh dẫn hướng
Cơ cấu xe 3 bánh có thể bỏ qua xem xét các yếu tố như đồng phẳng giữa 4 bánh, khả năng bám đường của xe Trong khi đó, các yếu tố này cần được xem xét kỹ lưỡng trong cơ cấu xe 4 bánh, 6 bánh…
Bảng 2.1 Thông số kỹ thuật Fire Bird V Robot
Thông số Giá trị Đơn vị
Cảm biến dò line IR (white line) -
Cảm biến vật cản Sharp IR Range
Hình 2.5 Sơ đồ nguyên lý cơ của Micro Maqueen
Bảng 2.2 Thông số kỹ thuật Micro Maqueen
Thông số Giá trị Đơn vị
Cảm biến dò line Light Dependent Resistor -
Cảm biến vật cản Cảm biến siêu âm HC-SR04 -
Hình 2.7 Sơ đồ nguyên lý cơ của PiCar-S Đặc điểm: Cơ cấu xe 4 bánh, 2 động cơ dẫn động 2 bánh sau, 2 bánh trước dẫn hướng bằng một động cơ DC servo qua cơ cấu 4 bản lề
Bảng 2.3 Thông số kỹ thuật PiCar-S
Thông số Giá trị Đơn vị
Cảm biến dò line IR -
Cảm biến vật cản Ultrasonic -
6 d Phân tích cơ cấu xe 4 bánh
Phân tích động học của cơ cấu xe 4 bánh với hai dạng: góc quay 2 bánh trước giống nhau và góc quay 2 bánh trước khác nhau:
Cơ cấu 4 bánh góc quay 2 bánh trước giống nhau:
Hình 2.8 Cơ cấu xe 4 bánh góc quay 2 bánh trước giống nhau Ở hình 2.8, ngoài cơ cấu bốn bản lề ra, còn có cơ cấu lái kiểu bàn xoay Trong cơ cấu này, ᴠiệc đánh lái được thực hiện bằng cách quaу một trục cứng ᴠà thường là cầu trước bằng một động cơ dẫn hướng Khi đi theo đường quanh một khúc cua, các bánh xe trong cơ cấu này sẽ bị trượt sang một bên Khi góc đánh lái càng lớn, хu hướng хe bị lật nghiêng càng tăng do góc lệch giữa vector vận tốc bánh trước và bánh sau tăng lên, làm mất ổn định cho xe Chính ᴠì thế, động học lái bàn хoaу có tính mất ổn định cao nên chỉ phù hợp cho các хe cơ giới di chuуển ᴠới tốc độ chậm
Hình 2.9 Cơ cấu lái kiểu bàn xoay
Cơ cấu 4 bánh góc quay 2 bánh trước khác nhau:
Hình 2.10 Cơ cấu xe 4 bánh góc quay 2 bánh trước khác nhau Ở hình 2.9, ngoài cơ cấu 2 bánh dẫn hướng độc lập bởi 2 động cơ dẫn hướng, cơ cấu lái Ackermann được thiết kế để các bánh xe đạt được gần đúng một tâm quay tức thời chung, để tránh việc lốp xe bị trượt sang một bên khi đi theo đường quanh một khúc cua gặp phải ở dạng cơ cấu lái dạng bàn xoay
Hình 2.12 Khả năng cua của 2 cơ cấu 1
1 Nguồn hình: Wikipedia, link: https://en.wikipedia.org/wiki/Ackermann_steering_geometry
Cảm biến
Camera đóng vai trò như một cảm biến hiệu quả giúp robot nhận diện, phát hiện vật xung quanh nó và nhiều các tụ khác Trong ứng dụng dò line, camera sẽ ghi lại hình ảnh của đường line, sau đó xử lý ảnh và trả về dạng ma trận, thông qua các phép tính cuối cùng đưa ra được thông tin sai lệch vị trí tương đối giữa line so với xe
Việc sử dụng cảm biến camera đòi hỏi người sử dụng cần có kiến thức về xử lý ảnh để tối ưu tốc độ xử lý ảnh, đưa được giá trị sai số giữa line với robot nhanh nhất Ngoài ra, khối lượng bộ nhớ cần sẽ lớn hơn rất nhiều so với các loại cảm biến dò line khác vì các loại khác trả về các giá trị số thay vì ma trận ảnh như camera
Cấu tạo của quang trở gồm phần trên và phần dưới là các màng kim loại được đấu nối với nhau thông qua các đầu cực Linh kiện này được thiết kế theo cách cung cấp diện tích tiếp xúc tối đa nhất với 2 màng kim loại và được đặt trong một hộp nhựa có thể giúp tiếp xúc được với ánh sáng và có thể cảm nhận được sự thay đổi của cường độ ánh sáng Trong bóng tối, giá trị điện trở cao, và giảm mạnh khi được chiếu sáng
Nguyên lý hoạt động của quang trở dựa trên nguyên lý hiệu ứng quang điện trong một khối vật chất Khi các photon có năng lượng đủ lớn đập vào, sẽ khiến cho các electron bật ra khỏi các phân tử và trở thành các electron tự do trong khối chất và từ chất bán dẫn chuyển thành dẫn điện Mức độ dẫn điện của quang trở tùy thuộc vào phần lớn các photon được hấp thụ Khi ánh sáng lọt vào quang trở, các electron sẽ được giải phóng và độ dẫn điện sẽ được tăng lên
1 Nguồn hình: Auto HTM, link: https://auto-htm.com/chuyen-de/tim-hieu-ve-cac-nguyen-ly-goc-dat-banh- xe-o-to-con
Hình 2.13 Sơ đồ mạch điện cảm biến phototransistor 1
Phototransistor (transistor quang) là linh kiện bán dẫn 3 lớp có cực B nhạy cảm với ánh sáng Cực B cảm nhận ánh sáng và chuyển nó thành dòng điện chạy giữa cực thu C và cực phát E Phototransistor trả về giá trị điện áp dạng analog
Khi không có ánh sáng chiếu trên bề mặt của Transistor, có ít hạt mang điện tích, lúc này dòng điện bão hòa ngược nhỏ hay dòng IB, nghĩa là giá trị điện áp trả về lớn Khi có ánh sáng chiếu vào, làm tăng dòng IB, giá trị điện áp analog trả về nhỏ Biểu đồ dưới đây cho thấy cường độ dòng điện tăng cùng với cường độ ánh sáng
Hình 2.14 Cường độ dòng điện cực B thay đổi theo mức độ chiếu sáng 2
Ngoài ra, để cảm biến có thể hoạt động tốt, trả về giá trị đúng hoặc sai số nhỏ nhất so với thực tế, các yếu tố như khoảng cách giữa các cảm biến cần phù hợp với
1 Nguồn hình: CytronTech, Maker Reflect, link: https://www.cytrontech.vn/p-maker-reflect-simplifying-ir- reflectance-sensor-for-beginner
2 Nguồn hình: Circuit Globe, Phototransistor, link: https://circuitglobe.com/phototransistor.html
10 đường line, khoảng cách từ cảm biến đến bề mặt dò line cần đảm bảo ở những khoảng cho phép Trong một dãy cảm biến, các cảm biến cần trả về giá trị analog giống nhau ở những điều kiện giống nhau, do đó cần phải có bước hiệu chuẩn cảm biến Một phương pháp để hiệu chuẩn cảm biến được thực hiện theo công thức sau:
- 𝑦 𝑚𝑎𝑥 ; 𝑦 𝑚𝑖𝑛 : giá trị lớn nhất, nhỏ nhất mong muốn của tất cả cảm biến sau khi hiệu chuẩn
- 𝑥 𝑚𝑎𝑥,𝑖 ; 𝑥 𝑚𝑖𝑛,𝑖 : giá trị lớn nhất, nhỏ nhất của cảm biến thứ i trước khi hiệu chuẩn
- 𝑥 𝑗,𝑖 ; 𝑦 𝑗,𝑖 : giá trị trước và sau khi hiệu chuẩn của cảm biến thứ i
Các phương pháp xử lý giá trị trả về từ cảm biến thành vị trí của xe so với đường line:
- Phương pháp so sánh: sử dụng mạch lấy ngưỡng 0, 1 hoặc giải thuật lọc ngưỡng bằng lập trình để chuyển tín hiệu điện áp đọc từ cảm biến về thành mức cao hoặc mức thấp, từ đó suy ra vị trí của xe so với đường line
Hình 2.15 Mức so sánh của cảm biến ứng với các vị trí line khác nhau khi robot di chuyển
- Phương pháp xấp xỉ: sử dụng cho khác cảm biến trả về tín hiệu analog, các giá trị này đọc về qua các phép tính xấp xỉ để tìm ra vị trí của xe so với đường line Phương pháp xấp xỉ cho ra kết quả có sai số giữa vị trí thực tế với vị trí tính toán nhỏ Đường line
Vị trí tâm đường line giao với dãy cảm biến Giá trị cảm biến đo được
Hình 2.16 Phương pháp xấp xỉ trọng số
Theo phương pháp xấp xỉ trọng số này, gọi 𝑒 là tọa độ của robot (tính từ tâm dãy cảm biến đến vị trí tâm đường line), 𝑑 là khoảng cách giữa 2 cảm biến liền kề, lúc này được xác định như sau:
Cảm biến hồng ngoại (IR sensor):
Cảm biến hồng ngoại là thiết bị điện tử có khả năng đo và phát hiện bức xạ hồng ngoại trong môi trường xung quanh Cảm biến hoạt động bằng cách phát ra và phát hiện bức xạ hồng ngoại, thường cấu tạo có hai phần: diode phát sáng (LED) và đầu thu Khi một vật thể đến gần cảm biến, ánh sáng hồng ngoại từ đèn LED sẽ phản xạ khỏi vật thể và được đầu thu phát hiện Cảm biến hồng ngoại hoạt động đóng vai trò là cảm biến tiệm cận và chúng thường được sử dụng trong các hệ thống phát hiện chướng ngại vật (như trong robot)
Có vật cản LED phát và đầu thu vật cản
Hình 2.17 Nguyên lý phát hiện vật cản của cảm biến hồng ngoại
Hình 2.18 Sơ đồ mạch điện cảm biến hồng ngoại
IR LED luôn phát ánh sáng hồng ngoại, led thu bình thường có nội trở lớn, khi led thu nhận tia hồng ngoại chiếu vào đủ lớn thì nội trở giảm xuống Trường hợp khi có vật cản phía trước thì những chùm tia hồng ngoại đập vào vật cản và phản xạ lại led thu làm cho nó thay đổi giá trị nội trở và dẫn đến thay đổi mức điện áp ở phía đầu vào của op-amp Khi khoảng cách càng gần thì sự thay đổi sẽ càng lớn Và khi đó điện áp đầu vào không đảo được so sánh với giá trị điện áp không đổi ghim trên biến trở R3 Nếu
13 như giá trị điện áp đầu vào không đảo lớn hơn đầu vào đảo, op-amp xuất ra mức, nếu như áp đầu vào không đảo nhỏ hơn đầu vào đảo, op amp sẽ xuất ra mức 0 Các điện trở R1, R2, R4 được sử dụng để đảm bảo dòng điện tối thiểu mA đi qua các thiết bị LED
IR như Photodiode, đèn Led thông thường Biến trở R3 dùng để điều chỉnh độ nhạy của mạch
Nhận xét về cảm biến hồng ngoại: vì cảm biến nhạy với ánh sáng hồng ngoại nên dễ bị nhiễu do các yếu tố về nhiệt độ, ánh sáng mặt trời, đèn vonfram… làm kết quả nhận được không đúng với thực tế
Giải thuật điều khiển bám line
a Điều khiển ON-OFF Điều khiển này dựa vào phương pháp so sánh để xử lý tín hiệu cảm biến Sau đó chia trường hợp ở các vị trí khác nhau từ ngưỡng trả về của cảm biến mà quyết định tốc độ của động cơ dẫn động Điều khiển ON-OFF là điều khiển vòng hở, do đó việc bám line sẽ xảy ra hiện tượng giật, rung lắc b Bộ điều khiển bám theo tiêu chuẩn Lyapunov 1
1 Huu Danh Lam et al., Smooth Tracking Controller for AGV through Junction using CMU Camera,
Tuyển tập HN Cơ điện tử lần 7, VCM- 2014, pp.597-601, Vietnam, 2014
Hình 2.22 Mô hình động học của robot dò line với 3 thông số vị trí 𝑒 1 , 𝑒 2 , 𝑒 3
- 𝑒 1 : sai số khoảng cách từ cảm biến dò line đến điểm tham chiếu R trên line theo phương vuông góc với trục 2 bánh dẫn động Thường 𝑒 1 = 0 do điểm tham chiếu
R được xác định trùng với điểm đặt cảm biến
- 𝑒 2 : sai số khoảng cách từ cảm biến dò line đến điểm tham chiếu R trên line theo phương song song với trục 2 bánh dẫn động
- 𝑒 3 : sai số góc lệch giữa trục đối xứng của robot và tiếp tuyến của line tại điểm tham chiếu R
Tốc độ 𝑣 và 𝜔 của robot được xác định theo công thức:
Trong đó: 𝑣 𝑅 và 𝜔 𝑅 lần lượt là vận tốc dài và vận tốc góc của điểm tham chiếu R trên line, 𝑘 1 , 𝑘 2 , 𝑘 3 là các hệ số dương của bộ điều khiển theo tiêu chuẩn Lyapunov
Hạn chế của bộ điều khiển này là cần thiết kế cảm biến sao cho có thể xác định được góc lệch 𝑒 3 , mà các cảm biến như quang điện trở hoặc phototransistor rất khó hoặc không thể xác định đúng được 𝑒 3 Do đó, bộ điều khiển này chỉ phù hợp khi sử dụng cảm biến là camera
Hình 2.23 Minh hoạt các sai số vị trí trên cảm biến camera c Bộ điều khiển PID bám line Để sử dụng được bộ điều khiển PID, trước tiên cần tuyến tính hóa hệ thống nếu hệ thống là hệ phi tuyến Ứng dụng trong dò line, bộ điều khiển PID chỉ điều khiển thông số 𝑒 2 từ cảm biến ( thể hiện trong hình 2.17), vì vậy cần đưa mô hình gồm 𝑒 1 , 𝑒 2 ,
𝑒 3 về mô hình chỉ chứa một thông số 𝑒 2
Bộ điều khiển PID sẽ xử lý sai số 𝑒 2 , sau đó xuất ra giá trị vận tốc góc 𝜔 để điều khiển tốc độ của 2 động cơ, trong đó vận tốc dài 𝑣 của 2 động cơ được cố định ở giá trị
𝑣 𝑅 Tốc độ động cơ được điều khiển vòng kín
Bộ điều khiển PID gồm 3 khâu:
- Khâu tỉ lệ (𝐾 𝑃 ): khâu bắt buộc phải có trong bộ điều khiển PID, tín hiệu điều chỉnh tỉ lệ với sai lệch đầu vào, việc tăng 𝐾 𝑃 giúp bộ điều khiển phản ứng mạnh hơn với sai số, nhưng vượt quá mức cho phép sẽ gây tình trạng mất ổn định
- Khâu tích phân (𝐾 𝐼 ): 𝐾 𝐼 càng lớn thì sai số xác lập càng nhỏ, sai lệch so với đường line theo thời gian giảm dần về 0 nhanh hơn nhưng đồng thời cũng sẽ làm tăng độ vọt lố
- Khâu vi phân (𝐾 𝐷 ): 𝐾 𝐷 càng lớn thì độ vọt lố càng giảm, hạn chế hiện tượng rung lắc, tăng độ ổn định cho xe.
Cấu trúc điều khiển
a Điều khiển tập trung Ưu điểm: mạch chỉ sử dụng một vi điều khiển đảm nhận tất cả các chức năng nhờ vào đó mà mạch điện phần cứng đơn giản Chỉ cần tính toán chọn lựa, thiết kế phần cứng phù hợp (điện áp hoạt động, cổng I/O phù hợp,…) Giảm tải được phần code giao tiếp, truyền gửi dữ liệu giữa master và slave
Nhược điểm: vì chỉ có một vi điều khiển để xử lý tất cả các chức năng, do đó việc lập trình tốn rất nhiều thời gian để tính toán, số lượng công việc cần hoàn thành là rất lớn trong khoảng thời gian nhỏ nên cần vi điều khiển có cấu hình mạnh, tốc độ xử lý nhanh để đáp ứng kịp thời gian vừa lấy dữ liệu về từ cảm biến, vừa phải tính toán điều khiển tốc độ động cơ… Các chức năng không thể thực hiện đồng thời, phải đợi chức năng này thực hiện xong thì chức năng khác mới được thực hiện
Vi điều khiển Cảm biến vật cản
Hình 2.24 Cấu trúc điều khiển tập trung b Điều khiển phân cấp Ưu điểm: mạch gồm nhiều vi điều khiển đảm nhiệm các vai trò khác nhau xử lý các công việc khác nhau như nhận và xử lý dữ liệu trả về từ cảm biến, điều khiển động cơ… giúp giảm khối lượng tính toán, công việc cho các vi điều khiển thay vì tập trung hết vào một vi điều khiển như ở điều khiển tập trung Các chức năng khác nhau có thể
19 thực hiện đồng thời với nhau bằng các vi điều khiển khác nhau, giúp tổng thời gian xử lý tác vụ giảm
Nhược điểm: mạch điện phức tạp, cần nhiều linh kiện hơn, cần thêm khoảng thời gian giao tiếp giữa 2 vi điều khiển với nhau Cần phân chia công việc phù hợp và quản lý được thời gian xử lý giữa các vi điều khiển để đồng bộ với nhau Đảm bảo tín hiệu giao tiếp giữa các vi điều khiển phải chính xác, không bị nhiễu
Dưới đây là một số cấu trúc điều khiển phân cấp gồm 2 hay nhiều vi điều khiển:
Slave Cảm biến vật cản
Master Cảm biến dò line
Slave 1 Cảm biến vật cản
Master Cảm biến vật cản
Master Cảm biến vật cản
Hình 2.25 (a), (b) cấu trúc phân tán một Master nhiều Slave; (c), (d) cấu trúc phân tán một Master một
Đặc đầu bài bài toán thiết kế robot dò line
Vì sa bàn là một mặt phẳng, do đó các yếu tố về lực có thể bỏ qua do sự thay đổi của ngoại lực là không đáng kể Vì vậy chỉ cần quan tâm đến thông số vận tốc của xe khi di chuyển Đề bài đặt ra yêu cầu thiết kế robot dò line, vì vậy thông số cần quan tâm thứ 2 là sai số bám line của robot
Vật đầu bài bài toán thiết kế robot dò line này ta cần quan tâm đến 2 thông số chính là vận tốc 𝑣 và sai số 𝑒
- Sai số 𝑒 phụ thuộc vào các phương pháp tính toán vị trí từ các giá trị trả về (theo khảo sát và thực nghiệm, sai số đo theo phương pháp xấp xỉ vị trí cảm biến so với tâm đường line từ các giá trị analog trả về của cảm biến, sai số chênh lệch có thể đạt đến ~5𝑚𝑚), tốc độ di chuyển của robot và thời gian lấy mẫu của bộ điều khiển toàn hệ thống Mục tiêu với mong muốn sai số không vượt một nửa bề dày line (< 13𝑚𝑚), ta xác định sai số tối đa mà robot có thể đạt là 10𝑚𝑚, khi đó tốc độ robot có thể đạt nếu thời gian lấy mẫu toàn hệ thống nằm trong khoảng từ 0.1𝑠 ÷ 0.2𝑠 được căn cứ như sau:
Hình 2.26 Phân tích sai số vị trí vào đường cong sa bàn của robot
21 Trong đó 𝑂 là tâm đường cong, 𝐶 là điểm bám line của robot, ∆𝑠 là đoạn đường robot đi được trong thời gian ∆𝑡
Theo hình vẽ, ta có:
Với sai số 𝑒 𝑚𝑎𝑥 = 10𝑚𝑚, loại trừ sai số do phương pháp xấp xỉ có thể gây ra là 5𝑚𝑚, vậy ứng 𝐶𝐵 = 5𝑚𝑚, thời gian lấy mẫu ∆𝑡 = 0.1𝑠 ÷ 0.2𝑠, ta xác định được tốc độ 𝑣 của robot:
Vậy để đáp ứng được sai số 𝑒 = 10𝑚𝑚, ta chọn trước vận tốc dự kiến robot đạt được là 𝑣 = 400 𝑚𝑚/𝑠 Đầu bài bài toán thiết kế robot dò line:
- Sai số bám line tính từ vị trí tâm cảm biến đến tâm đường line: 𝑒 = ±10𝑚𝑚
- Vận tốc dự kiến robot dò line: 𝑣 = 0.4 𝑚/𝑠
PHƯƠNG ÁN THIẾT KẾ
Phương án thiết kế
Một số cơ cấu cơ khí được thực hiện trong robot dò line:
Hình 3.1 Các cơ cấu cơ khí ứng dụng trong robot dò line
Qua tổng quan tìm hiểu và đánh giá giữa các cơ cấu cơ khí, rút ra được một số nhận định sau:
- Các cơ cấu (b), (c) trong Hình 3.1, là loại điều khiển tốc độ bằng động cơ dẫn động phía sau và dẫn hướng phía trước, góc quay của bánh dẫn hướng là bằng nhau Nhược điểm của loại này là xảy ra hiện tượng trượt bên của bánh dẫn hướng, làm cho khả năng điều khiển bám line không chính xác, không đáp ứng được sai số đã đề ra, ngoài ra khi bánh dẫn hướng xoay 1 góc lớn là, vector vận tốc của bánh sau và bánh trước có góc lệch lớn có thể khiến xe bị lật nghiêng
- Loại cơ cấu (d) bù được nhược điểm trượt biên của (b) và (c), do 2 bánh dẫn hướng trước có góc quay khác nhau ( được điều hướng bằng 2 động cơ), nhưng loại này thường không được sử dụng do khả năng đồng bộ 2 động cơ dẫn hướng để tạo ra góc xoay trùng tâm quay tức thời là rất khó khăn, do đó một loại cơ cấu khác được thiết kế là cơ cấu Ackermann Tuy nhiên, cơ cấu Ackermann thiết kế cơ khí và mô hình hóa robot phức tạp
- Loại cơ cấu (a) là loại dẫn động và dẫn hướng bằng 2 bánh sau, bánh trước tự lựa Loại này dẫn hướng nhờ chênh lệch tốc độ giữa 2 bánh, không xảy ra hiện tượng trượt bên khi vào cua, do đó có thể đáp ứng được sai số bám line nếu ta điều khiển tốc độ giữa 2 bánh một cách phù hợp Ngoài ra vì có 3 bánh nên về mặt thiết kế có thể bỏ qua phần đồng phẳng giữa các bánh vì chỉ có 3 điểm tiếp xúc
Vì vậy, với sa bàn đồng phẳng và yêu cầu đầu bài đặc ra, ta lựa chọn cơ cấu (a) để thiết kế b Cảm biến
Yếu tố lựa chọn cảm biến dựa vào phương sẽ áp dụng cho robot để xác định vị trí của robot so với đường line
Với phương pháp so sánh, cảm biến được lựa chọn sẽ trả về ngưỡng 0 1, từ đó ứng với các trạng thái của cảm biến, sẽ đưa ra 1 bảng trạng thái về vị trí của cảm biến, sai số phụ thuộc rất nhiều vào phần cứng như khoảng cách giữa 2 cảm biến, khả năng nhận biết line của mỗi cảm biến
Với phương pháp xấp xỉ, cảm biến trả về giá trị analog, qua các tính toán xấp xỉ trọng số có thể suy ra được vị trí trả về với độ chính xác rất cao chênh lệch so với thực tế chỉ khoảng 5𝑚𝑚
Do đó, ta chọn cảm biến trả về giá trị analog với phương pháp xấp xỉ trọng số để xác định được vị trí của robot so với tâm line
Cảm biến vật cản: từ tìm hiểu tổng quan về cảm biến, đánh giá và nhận xét vật cản của đề bài, cảm biến siêu âm (cụ thể là HC-SR04) có thể đáp ứng được những yêu cầu cần thiết để tiến hành nhận dạng được vật cản trên sa bàn, giá trị trả về tuyến tính theo khoảng cách tốt hơn so với các loại cảm biến vật cản khác
Hình 3.2 Đồ thị giá trị khoảng cách trả về và giá trị thực tế của 3 loại cảm biến
Vì vậy, để nhận biết đường line có vật cản, ta chọn cảm biến siêu âm HC-SR04 c Bộ điều khiển điều khiển
Với đầu bài đặt ra gồm 2 thông số là sai số bám line 𝑒 và vận tốc 𝑣, vì vậy bộ điều khiển cần đáp ứng được khả năng điều khiển vận tốc và sai số bám line của robot Nhìn chung với robot dò line, bộ điều khiển cần điều khiển được vận tốc dài 𝑣 và vận tốc góc 𝜔 phụ thuộc vào sai số vị trí của robot với tâm đường line, một số bộ điều khiển đã được sử dụng:
- Điều khiển ON-OFF: loại này dựa vào bảng trạng thái của vị trí đã được định sẵn do cảm biến trả về và thực hiện điều chỉnh tốc độ xe theo một bảng trạng thái tương ứng với bảng trạng thái vị trí của robot Là điều khiển vòng hở, gây tình trạng rung lắc mạnh khi cảm biến đảo trạng thái
Khoảng cách thực tế (mm)
K ho ảng cá ch đo được (m m )
HC-SR04 ,Sharp GP2Y0A02YK0F và VL53L0X
- Bộ điều khiển bám line theo tiêu chuẩn Lyapunov: gồm 3 thông số về sai số vị trí, và 2 thông số đầu ra của bộ điều khiển là vận tốc 𝑣 và 𝜔 giúp bám line tốt với sai số rất nhỏ với điều kiện sử dụng cảm biến là camera Các loại cảm biến dò line khác không thể xác định được sai số vị trí về góc, nên nếu sử dụng bộ điều khiển này, khả năng bám line của robot sẽ không đúng theo thiết kế mà bộ điều khiển đề ra
- Bộ điều khiển PID bám line: chỉ điều khiển một thông số là vận tốc góc 𝜔 với một thống số về sai số vị trí được lấy từ dãy cảm biến analog, do đó nó có thể đáp ứng được sai số bám line theo mong muốn, phù hợp với giới hạn chỉ đo được một thông số vị trí của dãy cảm biến analog
Vì vậy, với cảm biến analog được chọn, ta sử dụng bộ điều khiển PID để bám line d Cấu trúc điều khiển
Qua tìm hiểu và đánh giá ưu nhược điểm của cấu trúc điều khiển trong phần tổng quan, để các chức năng có thể cùng thực hiện giúp giảm thiểu thời gian xử lý tác vụ cũng như hạn chế chậm trễ có thể gây ra do cấu trúc điều khiển tập trung đảm nhiệm quá nhiều thứ, đồng thời để giảm sự phức tạp về mạch điện và độ lớn dữ liệu truyền nhận qua giao tiếp, cấu trúc điều khiển được thiết kế phân cấp với 1 Master và 1 Slave.
Sơ đồ nguyên lý thiết kế
Hình 3.3 Sơ đồ nguyên lý phần cơ khí
IR sensor 1 IR sensor 2 IR sensor 3 IR sensor 4 IR sensor 5
Cảm biến phát hiện vật cản
Battery Mạch chuyển nguồn Encoder 2 Motor 2 Motor 1 Encoder 1
Hình 3.4 Sơ đồ nguyên lý phần điện c Bộ điều khiển
Bộ điều khiển PID bám line
Hình 3.5 Giải thuật bộ điều khiển PID bám line
Slave Cảm biến vật cản
Hình 3.6 Cấu trúc điều khiển phân cấp
THIẾT KẾ CƠ KHÍ
Sơ đồ nguyên lý cơ khí
Hình 4.1 Sơ đồ nguyên lý robot dò line
Tính toán kích thước xe
Do phương án lựa chọn là dạng Robot xe dò line 3 bánh, trong đó 2 bánh sau dẫn động, bánh trước dẫn hướng nên mô hình động lực học của xe được mô tả như sau:
Xét xe ở trạng thái tĩnh:
Hình 4.2 Sơ đồ phân tích lực
- 𝐺 : tọa độ trọng tâm của robot
- 𝐹 𝑚𝑠1 : lực ma sát giữa 1 bánh chủ động và mặt đường
- 𝑁 1 : phản lực của 1 bánh chủ động
- 𝐹 𝑚𝑠2 : lực ma sát giữa 1 bánh bị động và mặt đường
- 𝑁 2 : phản lực của 1 bánh bị động
Ta có hệ phương trình động lực:
[ 4.2] Điều kiện để các bánh xe luôn bám mặt đường:
[ 4.3] Điều kiện để xe không bị lật khi vào cua:
Hình 4.3 Sơ đồ phân tích lực điều kiện chống lật khi robot vào cua Để xe không lật thì moment do lực li tâm gây ra không lớn hơn momen do trọng lực gây ra:
𝐻 ≥ 0.1 Điều kiện để xe không trượt khi vào cua:
Hình 4.4 Sơ đồ phân tích lực điều kiện chống trượt khi robot vào cua Để xe không trượt khi vào cua thì tổng lực ma sát tác dụng lên xe lớn hơn lực ly tâm:
Ta có các điều kiện ràng buộc như sau:
Chọn sơ bộ các kích thước:
Thay vào công thức trên ta được:
Tính toán công suất động cơ
Từ thông số kích thước của xe, tiến hành chọn bánh xe với tiêu chí kích thước nhỏ gọn Bởi xe làm việc trong bề mặt nhẵn, không gồ ghề, … nên không cần thiết lựa chọn bánh xe với đường kính lớn Mặt khác, bánh xe đường kính lớn thì vận tốc góc của động cơ sẽ nhỏ
Trên thị trường hiện có các loại bánh xe với các kích thước lần lượt như sau:
Hình 4.5 Phân tích lực trên bánh chủ động
0.0425= 23.5 (𝑟𝑎𝑑 𝑠⁄ ) 2 Phương trình cân bằng momen tại tâm bánh xe:
Bánh xe được dẫn động thông qua khớp khối lục giác, do đó ta nhân thêm hiệu suất truyền 𝜂 𝑘𝑛 = 0.98 Ta có momen xoắn động cơ:
Số vòng quay động cơ:
Công suất cần thiết của động cơ, do động cơ thường chỉ đạt hiệu suất đỉnh là 75% khi có tải định mức nên ta nhân thêm hệ số an toàn 1.3:
Như vậy, động cơ sử dụng cho robot phải thỏa mãn:
Do đó chọn loại động cơ GM25-370 với thông số kỹ thuật như sau:
Bảng 4.1 Thông số kỹ thuật động cơ GM25-370
Thông số Giá trị Đơn vị
Tốc độ tối đa khi có tải 140 𝑣𝑔/𝑝ℎ
Momen xoắn tối đa 0.52 𝑁𝑚 Điện áp hoạt động 12 𝑉𝐷𝐶
Dòng tối đa khi tải 750 𝑚𝐴
Tính toán dung sai
a Dung sai độ đồng trục Đối với mặt trụ thì việc xác định cấp chính xác hình dạng có thể dựa vào quan hệ giữa cấp chính xác kích thước
34 Theo tài liệu về dung sai của Ninh Đức Tốn trang 23 tài liệu [12], dung sai cho kích thước IT7 và IT8 thường sử dụng trong lĩnh vực cơ khí thông dụng Do đó, ta chọn IT8
Từ dung sai kích thước trên, ta dựa vào bảng 5.2 trang 76 tài liệu [12], chọn độ chính xác hình học tương đối ở mức thường, ta tra được cấp chính xác hình dạng là 8 Tiếp tục tra theo bảng 11, phụ lục 2, đối với độ đồng trục thì kích thước danh nghĩa giữa các bề mặt tạo phần tử đối xứng được lấy làm kích thước danh nghĩa, ở đây kích thước đó chính là khoảng cách giữa hai tấm gá trục với kích thước thiết kế là 125 𝑚𝑚
Từ đó, với cấp chính xác lựa chọn ở trên, ta tra được dung sai độ đồng trục là 80 𝜇𝑚 Sai số này ta coi là sai số chung cho cả hai phương 𝑥 và 𝑦
Hình 4.6 Chuỗi kích thước theo phương X
Phương trình chuỗi kích thước:
- 𝐴 1 là khâu thành phần tăng, cũng là khoảng cách giữa hai chốt định vị
- 𝐴 2 là khâu thành phần giảm, cũng là khoảng cách từ chốt định vị đến tâm trục Kích thước danh nghĩa khâu khép kín:
Hệ số cấp chính xác:
Trong đó: 𝑇 Σ là dung sai khâu khép kín, ở đây bằng với giá trị 80 𝜇𝑚 chọn ở trên
So với bảng 4.1 tài liệu [12], chọn cấp chính xác có hệ số gần với 𝑎 𝑚 nhất, ta chọn được hệ số chính xác thực tế là 40, ứng với cấp chính xác là 9 Sau khi xác định cấp chính xác chung của các khâu thành phần, ta tra sai lệch và dung sai cho các khâu thành phần theo bảng tiêu chuẩn (TCVN 2245-99), với khâu tăng coi như kích thước lỗ cơ bản, khâu giảm coi như kích thước trục cơ bản Từ đó ta có:
Suy ra dung sai khâu 𝐴 2 :
Vì 𝐴 2 là khâu giảm nên:
2 − 0 − 0 = 0.026 𝑚𝑚 [ 4.18] Sai lệch giới hạn khâu 𝐴 2 :
Do khả năng công nghệ, ta làm tròn các giá trị dung sai thành:
Hình 4.7 Chuỗi kích thước theo phương Y
Phương trình chuỗi kích thước:
- 𝐴 1 là khâu thành phần tăng
- 𝐴 2 là khâu thành phần giảm
Kích thước danh nghĩa khâu khép kín:
Hệ số cấp chính xác:
Trong đó: 𝑇 Σ là dung sai khâu khép kín, ở đây bằng với giá trị 80 𝜇𝑚 chọn ở trên
So với bảng 4.1 tài liệu [12], chọn cấp chính xác có hệ số gần với 𝑎 𝑚 nhất, ta chọn được hệ số chính xác thực tế là 40, ứng với cấp chính xác là 9 Sau khi xác định cấp chính xác chung của các khâu thành phần, ta tra sai lệch và dung sai cho các khâu thành phần theo bảng tiêu chuẩn (TCVN 2245-99), với khâu tăng coi như kích thước lỗ cơ bản, khâu giảm coi như kích thước trục cơ bản Từ đó ta có:
Suy ra dung sai khâu 𝐴 2 :
Vì 𝐴 2 là khâu giảm nên:
2 − 0 − 0 = 0.026 𝑚𝑚 [ 4.26] Sai lệch giới hạn khâu 𝐴 2 :
Do khả năng công nghệ, ta làm tròn các giá trị dung sai thành:
Mô hình 3D robot dò line
Hình 4.8 Tổng thể mô hình 3D của robot dò line
THIẾT KẾ ĐIỆN
Sơ đồ nguyên lý hệ thống điện
IR sensor 1 IR sensor 2 IR sensor 3 IR sensor 4 IR sensor 5
Cảm biến phát hiện vật cản
Battery Mạch chuyển nguồn Encoder 2 Motor 2 Motor 1 Encoder 1
Hình 5.1 Sơ đồ nguyên lý hệ thống phần điện
Cảm biến dò line
Với sa bàn trắng và đường line đen Ta sử dụng phototransistor kết hợp với LED hồng ngoại để nhận biết bên cạnh đó ta đảm bảo việc che chắn tốt đến tránh ảnh hưởng của nhiễu
Hình 5.2 Sơ đồ nguyên lý của cặp LED trong mạch cảm biến b Tính toán giá trị điện trở
Với sơ đồ nguyên lý trên, ta kết hợp với các giá trị được cho trong datasheet của TCRT5000:
Dựa vào đồ thị Hình 5.3, ta chọn 𝐼 𝐶 = 4 𝑚𝐴 Điện trở 𝑹 𝟐 :
Hình 5.3 Đồ thị thể hiện mối quan hệ giữa 𝐼 𝐹 − 𝐼 𝐶 và 𝑉 𝐶𝐸 − 𝐼 𝐶 c Bố trí cảm biến
Có 2 cách đăṭ cảm biến trên dãy cảm biến: đặt doc (position 2) và đăṭ ngang (position 1)
Khi đi từ nền trắng vào nền đen, cảm biến phải di chuyển một khoảng X d thì giá trị analog của nó mới thật sự xác định Ta chọn cách bố trí cảm biến sao cho giá trị X d nhỏ để đảm bảo cảm biến ổn định và sẽ cho kết quả chính xác hơn
Dựa vào đồ thị Hình 5.4, ta chọn cảm biến bố trí nằm ngang (position 1)
Hình 5.4 Ảnh hưởng của hướng đặt cảm biến đến switching distance X d d Khoảng cách từ cảm biến đến mặt đường
42 Khoảng cách cảm biến so với mặt đường cần đảm bảo thu được tín hiệu tại các vị trí nền trắng là như nhau Xuất hiện vùng giao thoa giữa 2 cực phát và cực thu
Hình 5.5 Khoảng cách từ cảm biến đến nền đường
Dựa vào thông số kỹ thuật cảm biến ta có:
- Góc chiếu của led phát 𝛼 = 16°
- Góc chiếu của led thu 𝛽 = 30°
- Khoảng cách giữa hai bóng: 𝑏 = 5 𝑚𝑚
Xác định độ cao tối thiểu ℎ, với 𝐿 = ℎ + 0.7:
Thí nghiệm đo khoảng cách giữa cảm biến và nền đường:
Theo tính toán sơ bộ ở trên, ta đo với độ cao từ 4𝑚𝑚 ÷ 14𝑚𝑚, ta đo giá trị analog của các cảm biến trên nền trắng và nền đen và độ chênh lệch giữa giá trị analog hai nền trắng đen để chọn vị trí đặt cảm biến tốt nhất
Hình 5.6 Giá trị analog của cảm biến ứng với các độ cao khác nhau trên nền đen
Hình 5.7 Giá trị analog của cảm biến ứng với các độ cao khác nhau trên nền trắng
Dựa vào các đồ thị trên ta thấy khoảng cách từ 9𝑚𝑚 ÷ 10𝑚𝑚, cảm biến cho giá trị analog ổn định và độ lệch giữa nền trắng và đen là lớn nhất, ta chọn khoảng cách giữa cảm biến và nền sa bàn là 10 mm e Khoảng cách giữa 2 cảm biến
Giá trị analog Độ cao (mm)
Giá trị analog theo độ cao trên nền đen sensor0 sensor1 sensor2 sensor3 sensor4
Giá trị analog Độ cao (mm)
Giá trị analog theo độ cao trên nền trắng sensor0 sensor1 sensor2 sensor3 sensor4
44 Để cảm biến hoạt động tốt thì các led phải tách biệt nhau, các led không được giao thoa với nhau, vì sẽ gây nên sai số khi hoạt động Phạm vi quét của 2 led liền kề được mô tả lại như sau:
Hình 5.8 Phạm vi quét led của 2 cảm biến liền kề
Khoảng cách giữa 2 led phát và thu liền kề phải đảm bảo:
Khoảng cách giữa 2 led trong 1 cảm biến là 𝑏 = 5𝑚𝑚 Do đó khoảng cách tối thiểu giữa 2 cảm biến sẽ là: 𝑑 = 𝑙 + 5 ≥ 13.64𝑚𝑚
Ngoài ra khi hoạt động sẽ có trường hợp cảm biến nằm trong vùng bất định thì giá trị analog của cảm biến đưa về sẽ như nhau Do đó sẽ không xác định được chính xác vị trí cảm biến so với tâm đường line Vùng bất định của cảm biến được mô tả lại như sau:
Hình 5.9 Vùng bất định của dãy cảm biến
45 Trên Hình 5.9 Ta thấy khi cảm biến dịch chuyển sang phải một đoạn 26 − 𝑑 thì luôn có 2 led nằm trong đường line, và do đó tín hiệu analog đo được sẽ như nhau, tương tự khi cảm biến di chuyển sang trái đoạn 2𝑑 − 26 thì chỉ có 1 led nằm trong đường line Ta phải chọn giá trị của d sao cho các khoảng cách này là nhỏ nhất
Vậy ta chọn 𝑑 = 17 𝑚𝑚 f Số lượng cảm biến Để xe bám được line ta cần ít nhất 3 cảm biến và để nhận biết được ngã rẽ ta cần thêm hai cảm biến ngoài cùng Vì thế giả sử khi tâm đường line trùng với tâm nội suy từ cảm biến thì cần 3 cặp cảm biến, đồng thời khi xe lệch về bên trái/phải ta cần thêm 1 cặp cảm biến ở mỗi bên để nhận dạng được phía lệch của xe Từ lập luận trên ta nhận thấy Hình với việc sử dụng 5 cảm biến ta hoàn toàn có thể xác định được phía lệch của xe
Hình 5.10 Sơ đồ bố trí sử dụng 5 cảm biến g Thiết kế mạch dò line
Sơ đồ nguyên lý mạch dò line:
Hình 5.11 Sơ đồ nguyên lý mạch dò line
Hình 5.12 Mạch PCB đi dây mặt dưới
AOUT1AOUT2AOUT3AOUT4AOUT5
Hình 5.13 Mạch PCB mặt trên h Hiệu chuẩn cảm biến
Tín hiêụ analog cho mỗi cảm biến là khác nhau, khi làm viêc̣ trong cùng điều kiêṇ như nhau sẽ cho ra giá tri ̣khác nhau Do đó ta phải tiến hành calip cảm biến Công thức calib:
- 𝑦 𝑚𝑎𝑥 ; 𝑦 𝑚𝑖𝑛 : giá trị lớn nhất, nhỏ nhất mong muốn của tất cả cảm biến sau khi hiệu chuẩn
- 𝑥 𝑚𝑎𝑥,𝑖 ; 𝑥 𝑚𝑖𝑛,𝑖 : giá trị lớn nhất, nhỏ nhất của cảm biến thứ i trước khi hiệu chuẩn
- 𝑥 𝑗,𝑖 ; 𝑦 𝑗,𝑖 : giá trị trước và sau khi hiệu chuẩn của cảm biến thứ i
Từ giá trị thực nghiệm ta thu được công thức calip cho 5 cảm biến:
Bảng 5.1 Công thức hiệu chỉnh cảm biến
48 Sensor 3 990 3810 𝑦 𝑗,3 = 990 + 1.021276596(𝑥 𝑗,0 − 990) Sensor 4 1206 3827 𝑦 𝑗,4 = 990 + 1.098817245(𝑥 𝑗,0 − 1206) Chọn 𝑦 𝑚𝑎𝑥 = 3870 và 𝑦 𝑚𝑖𝑛 = 990 i Phương pháp xấp xỉ theo trọng số tìm vị trí tâm line
Ta không thể xác định chính xác vị trí của xe so với đường line, do đó ta cần một phép toán để có thể tính được giá trị đó thông qua các giá trị analog của cảm biến, phép toán đó chính là phép nội suy Đường line
Vị trí tâm đường line giao với dãy cảm biến Giá trị cảm biến đo được
Hình 5.14 Phương pháp xấp xỉ trọng số
Theo phương pháp xấp xỉ trọng số này, gọi 𝑒 là tọa độ của robot (tính từ tâm dãy cảm biến đến vị trí tâm đường line), 𝑑 là khoảng cách giữa 2 cảm biến liền kề, lúc này được xác định như sau:
Thí nghiệm đo vị trí từ phương pháp xấp xỉ theo trọng số: Ở độ cao ℎ = 10 𝑚𝑚, đo các giá trị trong khoảng từ −30 𝑚𝑚 ÷ 30 𝑚𝑚 theo trái sang phải với vị trí cân bằng nằm ở tâm đường line, mỗi lần dịch chuyển 1 mm
Hình 5.15 Thiết bị thí nghiệm đo vị trí cảm biến so với đường line
Hình 5.16 Biểu đồ giá trị thực và giá trị tính toán
Ta thấy khi tính vị trí của cảm biến theo hàm xấp xỉ tuyến tính trên, sai số nhỏ nhất là 𝑒 𝑚𝑖𝑛 = 0.0327 𝑚𝑚 và sai số lớn nhất là 𝑒 𝑚𝑎𝑥 = 4.622
Quan hệ giữa tọa độ thực và tọa độ tính toán
Tọa độ thực Tọa độ tính toán
Hình 5.17 Biểu đồ phân phối dữ liệu sai số vị trí
Qua phân tích dữ liệu thu được từ excel, ta lấy được giá trị độ lệch chuẩn sai số của cảm biến là 𝜎 = 2.09406, số này được dùng để đưa sai số từ phương pháp xấp xỉ vào mô phỏng, giúp mô phỏng đạt được kết quả thực tế nhất.
Cảm biến siêu âm
Cảm biến siêu âm HC-SR04 có thông số kỹ thuật:
Bảng 5.2 Thông số kỹ thuật HC-SR04 Điện áp hoạt động 5V
Dòng điện hoạt động < 2mA
Khoảng cách đo 2 – 500 cm Độ phân giải 0.3 cm
Phân phối chuẩn sai lệch từ phương pháp xấp xỉ
Tính toán nguồn
Thiết bị Số lượng Điện áp đầu vào (V) Công suất tối đa(w)
Chọn nguồn có điện áp tối thiểu là 12V
Với xe hoạt động liên tục trong 30 phút:
Ta chọn 3 pin lion 18650 có thông số như sau:
Kiểu pin 18650 Li-ion rechargeable battery Điện áp trung bình 3.7VDC, sạc đầy 4.2V
Dòng xả tối đa liên tục Max 6.3A (3C x 2100mAh) Điện trở trong 20 mΩ
Mạch chuyển nguồn và các kết nối ngoại vi
Dựa vào sơ đồ nguyên lý của hệ thống điện và các thông số cần thiết sau đây được tổng hợp từ các phần trên:
- Nguồn điện cấp cho driver và mạch tổng: 12V
- Điện áp cấp cho 2 vi điều khiển, siêu âm, servo: 5V
- Điện áp cấp cho encoder, cảm biến dò line: 3V3 (do bộ ADC của STM32F103C8T6 được cung cấp điện áp từ 2.4V – 3.6V nên điện áp input của thiết bị có ADC từ 2.4V – 3.6V)
- Giao tiếp SPI giữa 2 vi điều khiển
- Master đọc giá trị của cảm biến dò line (5 chân analog) và phát xung để tính khoảng cách đến vật cản (2 chân xung Trig, Echo)
- Slave điều khiển driver dùng 7 chân (2 chân xung PWMA, PWMB; 4 chân digital output để điều khiển chiều hai động cơ, 1 chân digital output chế độ standby) và đọc giá trị encoder trả về dùng 4 chân (2 chân xung của hai kênh A, B của mỗi động cơ)
Hình 5.18 Sơ đồ nguyên lý hạ áp 12V – 5V dùng IC LM7805
Hình 5.19 Sơ đồ nguyên lý hạ áp 5V – 3.3V dùng IC AMS1117
Hình 5.20 Các khối header ngoại vi
GND U1_PA0 U1_PA1 U1_PA2 U1_PA3 U1_PA4 5V_input
Hình 5.21 (a) Sơ đồ đi dây và (b) mô hình 3D của mạch điện
THIẾT KẾ BỘ ĐIỀU KHIỂN
Mô hình hóa robot dò line
Hình 6.2 Mô hình động học của robot dò line Điểm M : trung điểm của hai bánh chủ động
56 Điểm C : điểm dò line của robot (trung điểm dãy cảm biến dò line) Điểm R : điểm tham chiếu cần bám a Phương trình động học của robot tại M – C – R
𝑣 : vận tốc dài của robot
𝜔 : vận tốc góc của robot
Trong đó: 𝑑 là khoảng cách từ điểm M đến điểm dò line C
Với 𝑣 𝑅 và 𝜔 𝑅 là vận tốc mong muốn của robot tại điểm tham chiếu R b Sai lệch giữa vị trí dò line C và vị trí bám line mong muốn R
[ 6.4] Phương trình tracking error được viết lại:
57 Phương trình động học của sai số dò line:
Tuyến tính hóa bằng phương pháp xấp xỉ Taylor
Từ [ 6.8], [ 6.9] và [ 6.10], ta thu được phương trình:
[ 6.11] Điểm cân bằng của hệ phi tuyến tại 𝑥 0 = [𝑒 1 𝑒 2 𝑒 3 ] 𝑇 = [0 0 0] 𝑇 khi đầu vào là
𝑢 0 = [𝑣 𝑅 𝜔 𝑅 ] 𝑇 Tuyến tính hóa hệ thống xung quanh điểm cân bằng 𝑥 0 với đầu vào là
Cảm biến dò line chỉ có thể xác định được sai số 𝑒 2 và có thể bỏ qua sai số 𝑒 1 vì vị trí R được xác định bằng chính điểm giữa cảm biến, do đó 𝑒 1 = 0, ta có thể loại 𝑒̇ 1 khỏi phương trình [ 6.12] Do vậy, cần biến đổi phương trình [ 6.12] về dạng phương trình trạng thái chỉ chứa biến liên quan và luật điều khiển theo thiết kế 𝑢:
Thiết kế bộ điều khiển bám line
Từ phương trình [ 6.13], ta có:
𝑒̈ 2 = 𝑒̇ 3 𝑣 𝑅 − 𝑑𝜔̇ = −𝑣 𝑅 𝜔 − 𝑑𝜔̇ [ 6.14] Phương trình trên có dạng:
59 Phương trình trạng thái của hệ:
Phương trình được viết lại:
Phương trình [ 6.20] là phương trình không gian trạng thái dạng 𝑥̇ = 𝐴 𝑥 + 𝐵𝑢 Bài toán thiết bộ điều khiển được thiết kế theo dạng bài toán thiết kế luật điều khiển hồi tiếp 𝑢 = −𝑘 × 𝑥 a Kiểm tra tính điều khiển được và tính quan sát được Để thiết kế bộ điều khiển hồi tiếp thì mô hình phải đạt được hai điều kiện sau: tính điều khiển được và tính quan sát được
Tính điều khiển được của mô hình:
Mô hình có tính điều khiển được khi ma trận 𝑃 𝐶 toàn hạng, ta có:
Ma trận 𝑃 𝐶 toàn hạng Vậy hệ điều khiển được
Tính quan sát được của mô hình:
Mô hình có tính quan sát được khi ma trận 𝑃 𝑂 toàn hạng, ta có:
Ma trận 𝑃 𝑂 toàn hạng Vậy hệ quan sát được b Bộ điều khiển bám line
Hình 6.3 Bộ điều khiển hồi tiếp
Ma trận độ lợi của bộ điều khiển hồi tiếp:
Phương trình đặc trưng của hệ kín:
⟺ 𝑠 2 + (−𝑘 2 𝑣 𝑅 − 𝑘 1 𝑑)𝑠 − 𝑣 𝑅 𝑘 1 = 0 [ 6.27] Xét tính ổn định theo tiêu chuẩn ổn định Routh:
Phương trình [ 6.27] là hệ bậc 2 Do đó có thể phân tích theo dạng:
𝑠 2 + 2𝜉𝜔 𝑛 𝑠 + 𝜔 𝑛 2 = 0 [ 6.29] Đồng nhất thức 2 phương trình, ta được:
Với hệ số giảm chấn 𝜉 = 1, 𝑣 𝑅 = 400 𝑚𝑚/𝑠, 𝑑 = 180 𝑚𝑚:
𝑣 𝑅 < 0 Đúng với mọi giá trị
Từ luật điều khiển 𝑢 = −𝐾𝑥 = −𝑘 1 𝑥 1 − 𝑘 2 𝑥 2 , thế 𝑥 2 = 𝑥̇ 1 + 𝑑𝜔 được:
Vậy các hệ số của bộ điều khiển:
[ 6.34] Đầu ra của bộ điều khiển PID bám line là 𝜔 (𝑟𝑎𝑑/𝑠), robot di chuyển với tốc độ
𝑣 𝑅 = 400 𝑚𝑚/𝑠 Quan hệ giữa tốc độ góc𝜔và tốc độ động cơ 𝑣 𝐿𝑒 , 𝑣 𝑅𝑖 như sau:
Hình 6.4 Quan hệ giữa tốc độ góc 𝜔 từ PID bám line với tốc độ động cơ 𝑣 𝐿𝑒 , 𝑣 𝑅𝑖
Thiết kế bộ điều khiển động cơ
a Xác định khoảng tuyến tính của động cơ và tần số xung PWM
Chọn tín hiệu ra là tốc độ quay của động cơ (vòng/phút) và tín hiệu vào là xung PWM Cho các giá trị PWM từ 0% - 100% và tìm đáp ứng của tốc độ động cơ khi đạt trạng thái xác lập, dựa trên các mẫu thu thập được ta tiến hành xử lý dữ liệu và vẽ đồ thị quan hệ giữa %PWM và tốc độ động cơ (RPM):
Hình 6.5 Đồ thị biểu diễn đường tuyến tính động cơ 1 -50
%PWM Khảo sát tuyến tính driver + động cơ 1 đường đặc tuyếnLinear (đường đặc tuyến)
Hình 6.6 Đồ thị biểu diễn đường tuyến tính động cơ 2
Từ các đồ thị trên, ta thấy tốc độ quay của 2 động cơ đều là một hàm tuyến tính theo giá trị PWM trong khoảng từ 0-100 %PWM Do đó, hàm truyền của cả 2 động cơ tại từng giá trị PWM trong khoảng này gần như nhau b Xây dựng hàm truyền động cơ
Hình 6.7 Mô hình hệ thống điều khiển tốc độ động cơ DC
- 𝐽 : Momen quán tính của rotor (Motor of Inertia of the Rotor) – là thước đo sức cản của tải tới tốc độ của động cơ
Khảo sát tuyến tính driver + động cơ 2 đường đặc tínhLinear (đường đặc tính)
- 𝑏 : Hệ số ma sát nhớt (Viscous Friction) – là thông số để xác định ma sát động lực học của động cơ
- 𝐿 : Điện cảm phần ứng (Armature Inductance) – là thông số xác định tốc độ dòng điện truyền tới động cơ để nó có thể khởi động được
- 𝑅 : Điện trở đầu cuối (Terminal Resistance) – là thông số để xác định mức độ năng lượng mà động cơ tiêu thụ khi chạy với một giá trị cường độ dòng điện cho trước
- 𝐾 𝑒 : Hằng số lực điện động ngược (Back – Emf Constant) – là thông số chi phối trong việc xác định tốc độ của động cơ ở trạng thái ổn định tại một giá trị điện áp nhất định
- 𝐾 𝑡 : Hằng số momen xoắn (Torque Constant) – là thông số xác định giá trị của dòng điện để động cơ đạt được momen xoắn cho trước
- 𝐼 : Cường độ dòng điện chạy trong cuộn dây của motor
- 𝑉 : điện áp trên hai đầu cuộn dây motor – ngõ vào
- :vị trí của trục – ngõ ra
Phương trình vi phân mô tả hệ thống:
Hàm truyền của động cơ được viết lại với giá trị đầu ra là tốc độ quay của động cơ và giá trị đầu vào là điện áp vào của động cơ
66 Trong đó, các thông số của động cơ được xác định qua datasheet của động cơ trên mặt lý thuyết để tra Để điều khiển tự động, hệ thống động cơ bao gồm cả động cơ và driver điều khiển, do đó hàm truyền cần phải có cả driver Ứng với mỗi động cơ, mỗi driver khác nhau sẽ có hàm truyền khác nhau và không tuân theo phương trình, con số cụ thể có sẵn Do đó khi thực hiện mô hình thực nghiệm, cần tiến hành tìm hàm truyền cụm động cơ bằng thực nghiệm
Chọn thời gian lấy mẫu động cơ là 𝑡 𝑠 = 10𝑚𝑠 Tiến hành chạy thực nghiệm với các %PWM khác nhau tại mỗi thời điểm, từ đó ta thu được số liệu về đáp ứng tốc độ của động cơ (𝑣ò𝑛𝑔/𝑝ℎú𝑡 ) theo thời gian để tính toán hàm truyền có độ tương thích cao nhất với hầu hết các giá trị %PWM:
Hình 6.8 Đồ thị đường đáp ứng tốc độ của động cơ 1 theo thời gian tương ứng với mỗi giá trị %PWM
Hình 6.9 Đồ thị đường đáp ứng tốc độ của động cơ 2 theo thời gian tương ứng với mỗi giá trị %PWM
Ta nhận thấy, ứng với mỗi giá trị %PWM, đường đáp ứng của động cơ có dạng hàm truyền bậc 1
Hàm truyền của hệ bậc 1 có dạng:
- 𝑇 :Thời điểm hệ đáp ứng đạt 63,2% giá trị xác lập Cực của hệ quán tính bậc 1 càng xa trục ảo thì 𝑇 càng nhỏ, hệ đáp ứng càng nhanh
- 𝐾 :Hệ số khuếch đại của hệ thống, là tỉ lệ giữa giá trị xác lập so với tín hiệu đầu vào Để thực hiện thủ công, ta thực hiện xác định đường đáp ứng của động cơ bởi 1 giá trị %PWM thuộc khoảng tuyến tính của động cơ Khi đó, đường đáp ứng sẽ có dạng hàm truyền bậc 1, ta tiến hành xác định khoảng thời gian đường đáp ứng đạt 63.2% giá trị xác lập và hệ số khuếch đại 𝐾 bằng cách lấy giá trị xác lập chia cho giá
68 trị %PWM ở trên Nhưng vì để đạt được độ chính xác nhất, hàm truyền sau khi tìm có độ tương thích cao nhất với mọi giá trị %PWM, ta sử dụng công cụ System Identification của Matlab, tìm được hàm truyền lần lượt của 2 động cơ:
Hàm truyền động cơ 1 (động cơ phải):
Hàm truyền động cơ 2 (động cơ trái):
Hình 6.10 So sánh kết quả hàm truyền tìm được với dữ liệu thu được từ thực tế của động cơ 1 (fit 94.63%)
Hình 6.11 So sánh kết quả hàm truyền tìm được với dữ liệu thu được từ thực tế của động cơ 2 (fit 94.19%) c Bộ điều khiển PID động cơ
Yêu cầu bộ điều khiển:
- Hệ số giảm chấn 𝜁 = 1, nhằm hạn chế động cơ thay đổi tốc độ
- Thời gian xác lập: 𝑇 𝑠 = 0,1 𝑠 (bằng 10 lần chu kỳ lấy mẫu)
Mô tả bộ điều khiển:
Hình 6.12 Bộ điều khiển PID
Thiết kế bộ điều khiển động cơ 1 (động cơ phải):
Hàm truyền hệ vòng hở:
Hàm truyền tổng của cả hệ thống vòng kín:
(73.2𝐾 𝐷 + 1)𝑠 2 + (73.2𝐾 𝑃 + 32.05)𝑠 + 73.2𝐾 𝐼 [ 6.46] Phương trình đặc trưng của hệ thống cần thiết kế:
Từ yêu cầu đã đặt ra, ta xác định tần số dao động riêng 𝜔 𝑛 tương ứng:
Phương trình đặc trưng của hệ thống mong muốn có thành phần bậc hai là:
𝑠 2 + 2𝜁𝜔 𝑛 𝑠 + 𝜔 𝑛 2 = 𝑠 2 + 80𝑠 + 1600 [ 6.49] Đồng nhất hệ số ta có:
Thiết kế bộ điều khiển động cơ 2 (động cơ trái):
Hàm truyền hệ vòng hở:
71 Hàm truyền tổng của cả hệ thống vòng kín:
(76.66𝐾 𝐷 + 1)𝑠 2 + (76.66𝐾 𝑃 + 32.65)𝑠 + 76.66𝐾 𝐼 [ 6.52] Phương trình đặc trưng của hệ thống cần thiết kế:
Từ yêu cầu đã đặt ra, ta tìm được tần số dao động riêng 𝜔 𝑛 tương ứng:
1 × 0,1 = 40(𝑟𝑎𝑑 𝑠⁄ ) Phương trình đặc trưng của hệ thống mong muốn có thành phần bậc hai là:
Kết quả bộ điều khiển PID động cơ, thực nghiệm với 2 giá trị tốc độ lần lượt là
Hình 6.13 Kết quả tốc độ lý thuyết và thực tế khi áp dụng bộ điều khiển PID
72 Nhận xét: đường đáp ứng tốc độ 2 động cơ giữa lý thuyết và thực tế khi áp dụng bộ điều khiển PID là tương tự nhau, đạt giá trị mong muốn tại 0.1s, phù hợp với mục tiêu thiết kế bộ điều khiển PID tốc độ động cơ Do đó không cần phải hiệu chỉnh lại các hệ số PID.
Kết quả mô phỏng và hiệu chỉnh các thông số của robot dò line
a Các thông số đầu vào mô phỏng bám line
Bảng 6.2 Thông số đầu vào mô phỏng bám line
Thông số Giá trị Đơn vị
Khoảng cách từ trục động cơ đến cảm biến: 𝑑 170 𝑚𝑚
Thời gian lấy mẫu PID bám line 0.1 𝑠
Thời gian lấy mẫu PID động cơ 0.01 𝑠 b Kết quả mô phỏng
Hình 6.14 Vết bám line của robot dò line
Hình 6.15 Sai số bám line 𝑒 2 tương ứng với thay đổi tốc độ góc 𝜔
Hình 6.16 Đồ thị vận tốc dài 𝑣 của 2 động cơ
Nhận xét: trong Hình 6.15, ta nhận thấy sai số lớn nhất đạt 14mm khi vào cua, cũng trong đoạn đường này sai số không thể giảm và giữ ở giá trị cố định Do đó ta cần thêm khâu I vào bộ điều khiển PID bám line, mục đích là để giảm sai số theo thời gian dần về 0 nhanh hơn Kết quả sau khi thêm khâu I vào, với 𝐾 𝐼 = 0.05:
Hình 6.17 Kết quả sai số bám line với 𝐾 𝐼 = 0.05
Ta nhận thấy trong đaọn vào đường cong (giây thứ 6 trở đi), khi thêm 𝐾 𝐼 thì sai số không còn đứng cố định nữa mà giảm dần về 0 nhanh hơn và sai số bám line nhỏ hơn với giá trị lớn nhất ở đoạn cong là −7.49𝑚𝑚
Với 𝐾 𝐼 = 0.1, kết quả mô phỏng đạt được:
Hình 6.18 Kết quả sai số bám line với 𝐾 𝐼 = 0.1
Khi tăng 𝐾 𝐼 lên 0.1, ta nhận thấy sai số ở đoạn cong là −6.037𝑚𝑚 và sai số lớn nhất cả quá trình là 7.084𝑚𝑚, nhỏ hơn so với 𝐾 𝐼 = 0.05 Nhưng đồng thời khi tăng
𝐾 𝐼 , độ vọt lố cũng sẽ xảy ra, do đó với kết quả mô phỏng với hệ số 𝐾 𝐼 = 0.1 thỏa mãn sai số bám line < 10𝑚𝑚 đã đặt ra, ta chấp nhận 𝐾 𝐼 = 0.1 được thêm vào bộ điều khiển PID bám line Áp dụng sai số do từ thiết bị cảm biến, sai lệch tính toán phương pháp xấp xỉ trọng số vào mô phỏng robot bám line, với độ lệch chuẩn của sai số là 𝜎 = 2.09406 Trong mô phỏng Matlab, ta thêm sai số này bằng cách sử dụng hàm “randn”:
Kết quả mô phỏng như sau:
Hình 6.19 Vết bám line có sai số từ kết quả đo của cảm biến
Hình 6.20 Sai số bám line 𝑒 2 có sai số từ kết quả đo của cảm biến
Hình 6.21 Vận tốc 𝑣 2 động cơ
Nhận xét: khi vừa bắt đầu vào đường cong, sai số đạt lớn nhất −10.75𝑚𝑚, còn các đoạn đường còn lại đều < 10𝑚𝑚 do đó robot vẫn có khả năng bám line tốt và đạt yêu cầu sai số cho phép khi
Mô phỏng tương tự với đường line nằm bên trái với 𝐾 𝑖 = 0.1:
Hình 6.22 Vết bám line của robot dò line với 𝐾 𝐼 = 0.1
Hình 6.23 Kết quả sai số với 𝐾 𝐼 = 0.1
Hình 6.24 Vận tốc 𝑣 của 2 động cơ với 𝐾 𝐼 = 0.1
Hình 6.25 Vết bám line có sai số từ kết quả đo của cảm biến
Hình 6.26 Sai số bám line 𝑒 2 có sai số từ kết quả đo của cảm biến
Hình 6.27 Vận tốc 𝑣 của 2 động cơ có sai số từ kết quả đo của cảm biến
CẤU TRÚC ĐIỀU KHIỂN VÀ LẬP TRÌNH
Lựa chọn vi điều khiển
Để đảm thực hiện đầy đủ các nhiệm vụ mà robot cần thực hiện, vi điều khiển cần đạt những yêu cầu sau:
- Cần có ít nhất 5 kênh chuyển đổi ADC tương ứng với 5 cặp led hồng ngoại thu, phát
- Cần có ít nhất 5 bộ timer/counter để thực việc tính toán vị trí xe, đọc encoder trả về từ 2 động cơ, băm xung và đọc cảm biến siêu âm
- Cần có ít nhất 2 khối tạo xung PWM
- Cần có thời gian đáp ứng thỏa mãn yêu cầu của đã bài toán đặt ra (0.01s)
Sau khi xem xét các yêu cầu đã đề ra, chọn 2 kit STM32F103C8T6 Blue Pill
Hình 7.1 Kit STM32F103C8T6 Blue Pill
Các đặc điểm của kit:
- Clock, reset và quản lý nguồn:
+ Điện áp hoạt động từ 2.0 → 3.6𝑉
+ Sử dụng thạch anh ngoài từ 4𝑀ℎ𝑧 → 20𝑀ℎ𝑧
+ Thạch anh nội dùng dao động RC ở mode 8𝑀ℎ𝑧 hoặc 40𝐾ℎ𝑧
- Có 4 bộ timer/counter 16 bit thường dùng (có 4 khối tạo xung PWM, mỗi timer đều có ngắt, bộ đếm counter encoder motor)
- Hai bộ ADC 12 bit với 9 kênh cho mỗi bộ:
+ Khoảng giá trị chuyển đổi từ 0 − 3.6 𝑉
+ Có chế độ lấy mẫu 1 kênh hoặc nhiều kênh
- Hỗ trợ chuẩn các giao tiếp: UART, SPI, I2C
Giao tiếp giữa 2 vi điều khiển
Bảng 7.1 So sánh các giao thức truyền thông UART, SPI, I2C Đặc điểm UART SPI I2C
4 dây (số lượng dây tăng khi số thiết bị 2 dây
Full duplex (không phân biệt master - slave)
Full duplex (1 master - nhiều slave)
Half duplex (Nhiều master - nhiều slave)
Truyền không đồng bộ (Tốc độ khoảng 2.25Mb/s - 4.5Mb/s)
Truyền đồng bộ (Tốc độ khoảng 18Mb/s)
Truyền đồng bộ (Tốc độ hỗ trợ 100Kb/s, 400Kb/s)
Khoảng cách 12m trên lý thuyết Trên bo mạch Trên bo mạch
Giao tiếp bằng chân chọn chip
Lên đến 127 thiết bị (7-bit address) Giao tiếp bằng địa chỉ
Từ bảng so sánh, so với yêu cầu đề bài, ta xác định cần có 1 master và 1 slave truyền nhận đồng bộ và chúng giao tiếp ngay trên bo mạch Đồng thời để đảm bảo tốc độ truyền nhanh chóng, ta chọn giao tiếp SPI.
Thời gian đáp ứng của vi điều khiển
Từ các đặc điểm kit STM32F103C8T6 ta có các thông số thời gian lấy mẫu sau:
Thời gian chuyển đổi ADC:
- Chu kì lệnh vi điều khiển:
- Thời gian chuyển đổi ADC của TCRT5000: 𝑡𝑐 = 1𝜇𝑠 3000), giá trị digital bằng 1
- Dưới ngưỡng 3000, giá trị digital bằng 0
Vị trí cảm biến từ trái qua phải như sau:
PIN0 PIN1 PIN2 PIN3 PIN4
Hình 7.4 Vị trí các cảm biến
Nhận biết ngã 3 của sa bàn:
- Ta thấy rằng trong tất cả các trạng thái, chỉ duy nhất có ngã 3 là có bề rộng đường line lớn hơn hẳn các trạng thái khác → dựa trên cơ sở này để phát hiện ngã 3
87 bằng cách: khi có 2 cảm biến (không tính PIN2) nằm ở hai phía so với cảm biến PIN2 vượt ngưỡng (giá trị digital bằng 1) Thì robot sẽ xác đị tại vị trí đó là ngã 3 của sa bàn
Hình 7.5 Cách nhận biết vị trí ngã 3 từ ngưỡng trả về của cảm biến
Hình 7.6 Vị trí đặt vật cản
- Góc quét: tại khu vực xuất hiện vật cản, nếu ta xét từ điểm A, góc tạo bởi AB và
AC khoảng 36° Để tiện trong việc làm tròn số, sai số do cảm biến và chênh lệch đoạn dừng khi phát hiện ngã 3, ta chọn góc quét là từ 0° đến 45°
- Khoảng cách phát hiện vật: xét tại điểm A, khoảng cách từ điểm A đến vùng cận trên và cận dưới của vùng phát hiện vật cản trong khoảng từ 65cm đến 101cm Để tiện trong việc làm tròn số, sai số do cảm biến và chênh lệch đoạn dừng khi phát hiện ngã 3, ta chọn khoảng cách phát hiện vật trong khoảng từ 60cm đến 110cm
- Thời điểm sử dụng cảm dò vật cản: Theo datasheet của cảm biến siêu âm HC- SR04, giữa 2 lần lấy mẫu phải đảm bảo khoảng thời gian lớn hơn 0.05s, trong khi ngắt là 0.01s Ta có thể khắc phục trường hợp này bằng cách sử dụng biến count để đếm→ thời gian lấy mẫu của cảm biến siêu âm bằng 0.06s Tuy nhiên khi sử dụng cảm biến, cứ mỗi lần lấy mẫu, ta phải mất 10us để kích chân Trig Để hạn chế thời gian delay trên, ta chỉ sử dụng cảm biến siêu âm chỉ trong thời gian quét vật cản
- Tính toán tốc độ quét: qua quá trình thực nghiệm, ta thấy rằng khi quét một góc 45° trong 0.6s thì xe vẫn phát hiện vật cản, vì thế, ta xét:
𝑏 = 195 𝑚𝑚 - khoảng cách giữa 2 bánh xe
𝐷 = 85 𝑚𝑚 - đường kính của bánh xe
Số lượng lấy mẫu cảm biến siêu âm:
0.06 = 10 [ 7.5] c Tác vụ điều khiển Đối với mỗi trạng thái cần có các tác vụ xử lý khác nhau, ở đây nhóm đã trình bày các tác vụ để điều khiển xe như sau:
- Trạng thái áp dụng: áp dụng tất cả các đoạn line trên sa bàn, trừ vị trí ngã 3
- Nhận biết: đọc giá trị cảm biến trên đường line không vi phạm các trường hợp đặc biệt
- Nhiệm vụ: giúp xe bám theo đường line
- Giải thuật: do đã biết trước quỹ đạo đường line nên ta sử dụng bộ điều khiển PID bám line
- Tín hiệu gửi về Slave: tốc độ 2 động cơ và 1 giá trị bằng 0 ứng với việc bám line bình thường
- Áp dụng cho vị trí phát hiện ngã 3
- Dấu hiệu nhận biết: khi có 2 cảm biến (ngoại trừ cảm biến ở giữa), nằm ở 2 phía so với cảm biến ở giữa vượt ngưỡng, và xe chưa xoay tại chỗ (rotate=0)
{digital [PIN0] == 1 || digital [PIN1] == 1 digital [PIN3] == 1 || digital [PIN4] == 1
- Nhiệm vụ: trước hết phải xét cờ ở trạng thái bận để tránh các tác vụ khác ảnh hưởng, sau đó thực hiện gồm 2 nhiệm vụ
+ Quét vật cản 1 góc 45 0 theo hướng đường bên trái
90 + Giữ vận tốc bánh trái và bánh phải cố định bằng nhau, bánh trái(𝜔 2 ) ngược chiều, bánh phải (𝜔 1 ) cùng chiều cho đến khi hoàn thành tác vụ quét vật cản
+ Thời gian thực hiện: 𝑡 1 = 0.8𝑠, bao gồm 0.1s nghỉ, 0.6s tiếp xoay tại chỗ, 0.1s còn lại nghỉ Kết thúc nhiệm vụ 1, lưu lại giá trị ứng với việc có phát hiện vật cản ở đoạn đường bên trái hay không
+ Quét vật cản bằng cảm biến siêu âm, trong khoảng từ 60cm đến 110cm nếu có giá trị thỏa mãn thì xác nhận phát hiện vật cản
+ Tín hiệu gửi về Slave: 0.1s đầu gửi giá trị bằng 3 ứng với việc xe dừng tại chỗ, 0.6s kế tiếp gửi giá trị bằng 1 ứng với việc xe xoay tại chỗ ngược chiều kim đồng hồ, 0.1s còn lại gửi giá trị bằng 3 ứng với việc xe dừng tại chỗ
+ Giữ vận tốc bánh trái và bánh phải cố định bằng nhau, bánh trái cùng chiều, bánh phải ngược chiều cho đến khi hoàn thành tác vụ trở lại đường line (khi cảm biến ngoài cùng phát hiện được line, qua bộ điều khiển PID bám line, robot tự động bám line sau đó)
+ Thấy vật ở bên trái đường: trở về đường line khi cảm biến ngoài cùng bên trái và cảm biến ngoài cùng bên phải hoặc kế nó vượt ngưỡng
{digital [PIN3] == 1 || digital [PIN4] == 1 digital [PIN0] == 1
+ Không thấy vật ở bên trái đường: trở về đường line khi cảm biến PIN1 bên trái vượt ngưỡng cảm biến (1) digital [PIN1] == 1
91 + Tín hiệu gửi về Slave: gửi giá trị bằng 2 ứng với việc xe xoay tại chỗ cùng chiều kim đồng hồ
- Áp dụng cho việc chuyển động sau khi nhận viết được đường có vật cản
- Dấu hiệu nhận biết: khi có 2 cảm biến (ngoại trừ cảm biến ở giữa), nằm ở 2 phía so với cảm biến ở giữa vượt ngưỡng và xe đã xoay tại chỗ quét vật cản (𝑟𝑜𝑡𝑎𝑡𝑒 = 1)
- Nhiệm vụ: giúp xe chọn hướng rẽ đúng
- Giải thuật: vẫn áp dụng bộ điều khiển PID bám line, tuy nhiên xử lý giá trị cảm biến như sau:
+ Xác định vật cản ở bên trái đường: kéo giá trị 2 cảm biến ngoài cùng bên trái về mức y min (ứng với nền trắng)
{calib [PIN0] = y min calib [PIN1] = y min
+ Xác định vật không nằm ở bên trái đường: kéo giá trị 2 cảm biến ngoài cùng bên phải về mức y min (ứng với nền trắng)
{calib [PIN3] = y min calib [PIN4] = y min
- Tín hiệu gửi về Slave: tốc độ 2 động cơ và 1 giá trị bằng 0 ứng với việc bám line bình thường
- Áp dụng khi bị rời khỏi line hoặc đến vị trí kết thúc đường line
+ Dấu hiệu nhận biết: dữ liệu trước đó của cảm biến ngay trước khi rời line có 1 trong 2 cảm biến ở vị trí ngoài cùng vượt ngưỡng
92 [pre_digital [PIN0] == 1 pre_digital [PIN4] == 1
+ Nhiệm vụ: rẽ cho xe quay trở lại đường line
+ Giải thuật: đặt sai số bằng sai số tối đa (ta đặt bằng 34 ứng với khoảng cách từ mắt giữa đến mắt ngoài cùng) và sử dụng bộ điều khiển PID bám line để cho xe trở về đường line pre_digital[PIN0] == 1 → e 2 = 34 pre_digital[PIN4] == 1 → e 2 = −34
+ Tín hiệu gửi về Slave: tốc độ 2 động cơ và 1 giá trị bằng 0 ứng với việc bám line
- Tại vị trí kết thúc line:
+ Dấu hiệu nhận biết: dữ liệu trước đó của cảm biến, cả 2 cảm biến ở vị trí ngoài cùng không vượt ngưỡng
KẾT QUẢ CHẠY THỰC NGHIỆM
Đường rẽ bên trái
Hình 8.1 So sánh sai số bám line thực tế và mô phỏng
Nhật xét: kết quả sai số từ dữ liệu thực tế với 𝐾 𝐼 = 0.05 có đường xu hướng tương đối giống với kết quả mô phỏng Với sai số di chuyển trên đường thẳng < 5𝑚𝑚,
Thời gian (s) Đồ thị sai số bám line thực tế
96 sai số khi bắt đầu vào đường cong bán kính 500𝑚𝑚, sai số lên −10𝑚𝑚 và giảm dần Sau khi tới ngã 3 sa bàn, sai số đạt giá trị lớn nhất tại đó với thực nghiệm là 24𝑚𝑚
Hình 8.2 So sánh vận tốc thực tế và mô phỏng của động cơ 1 (phải) 0
Thời gian (s) Đồ thị vận tốc động cơ 1
Hình 8.3 So sánh vận tốc thực tế và mô phỏng của động cơ 2 (trái) Đường đáp ứng vận tốc của cả 2 động cơ phù hợp với kết quả mô phỏng, đạt giá trị vận tốc đã đề ra là 400 𝑚𝑚/𝑠 Thời gian thực hiện trên thực tế là 17.11𝑠
Thời gian Đồ thị vận tốc động cơ 2
Hình 8.4 So sánh sai số bám line thực tế và mô phỏng
Nhận xét: kết quả sai số từ dữ liệu thực tế với 𝐾 𝐼 = 0.1 có đường xu hướng tương đối giống với kết quả mô phỏng, sai số nhỏ hơn so với kết quả sử dụng 𝐾 𝐼 = 0.05 Với sai số di chuyển trên đường thẳng < 5𝑚𝑚, sai số khi bắt đầu vào đường cong bán kính 500𝑚𝑚, sai số lên −10𝑚𝑚, đồ thị sai số giảm nhanh hơn Sau khi tới ngã 3 sa bàn, sai số đạt giá trị lớn nhất tại đó với thực nghiệm là 14𝑚𝑚
Thời gian (s) Đồ thị sai số bám line thực tế
Hình 8.5 So sánh vận tốc thực tế và mô phỏng của động cơ 1 (phải) 0
Thời gian (s) Đồ thị vận tốc động cơ 1
Hình 8.6 So sánh vận tốc thực tế và mô phỏng của động cơ 2 (trái) Đường đáp ứng vận tốc của cả 2 động cơ trong mô phỏng phù hợp với kết quả thực tế, đạt được vận tốc yêu cầu, thời gian thực hiện giữa thực nghiệm và mô phỏng gần như nhau, với thời gian hoàn thành thực tế là 17.27𝑠
Thời gian (s) Đồ thị vận tốc động cơ 2