Chương trình điều khiển

Chương trình điều khiển xe cân bằng sử dụng con quay hồi chuyền bao gồm các vòng lặp chính và vòng lặp phụ.

Có hai vòng lặp chính:

 Thu thập tín hiệu góc nghiêng của xe và vị trí góc xoay của bánh đà.

 Điều khiển góc xoay và vân tốc góc xoay của bánh đà bằng thuật toán PID.

3.4.3.1. Chương trình thu tín hiệu từ cảm biến MPU 6050

Hình 3. 35 Sơ đồ kết nối chân giữa cảm biến MPU 6050 và Arduino Uno R3

Bảng 3.7Sơ đồ kết nối chân giữa cảm biến MPU 6050 và Arduino Uno R3

MPU 6050 Arduino Uno R3

Vcc 5V

GND GND

SDA A4

SCL A5

Từ những nghiên cứu và khảo sát thực nghiệm, cảm biến MPU 6050 là cảm biến gia tốc 6 bậc tự do. Có thể tính toán và đo 3 góc với tốc độ xử lý cao thông qua giao thức I2C.

 Truyền tín hiệu giữa MPU 6050 và Arduino Uno R3:

Tín hiệu truyền từ cảm biến MPU 6050 tới Arduino Uno R3 thông qua giao thức I2C với tốc độ xử lý cao và đồng thời liên tục. Như đã biết, I2C là một giao thức giao tiếp cục bộ giũa các thiết bị chính và thiết bị phụ với nhau thông qua các kênh địa chỉ. Đối với MPU 6050 địa chỉ giao tiếp giữa chip xử lý và cảm biến là 0x68, nhưng khi kết nối chân A0 trên cảm biến về mass thì địa chỉ giao tiếp giữa nó và máy tính là 0x69.

Tuy nhiên, tín hiệu của cảm biến rất dễ bị nhiễu. Do đó, chúng ta cần phải có một bộ lọc để có thể thu được tín hiệu chính xác.

Khi cảm biến đặt ở vị trí cân bằng với giá trị là 2,45 độ và động cơ dẫn động bánh đà được khởi động, có thể thấy khi không có bộ lọc, cảm biến bị nhiễu rất nhiều. Biên độ dao động của cảm biến vào khoảng (-0,65 – 6) độ. Việc này sẽ rất khó khăn trong việc điều khiển xe cân bằng. Bên cạnh đó, khi có bộ lọc dường như tín hiệu rất ổn định, có rất ít dao động và dao động có biên độ không cao và khoảng thời gian giao động nhỏ.

Hình 3. 36 Tín hiệu khi cảm biến ở vị trí cần bằng và động cơ dẫn động bánh đà được khởi động

Hình 3. 37 Tín hiệu khi thay đổi góc nghiêng của xe

Khi thay đổi góc nghiêng của xe, có thể thấy khi có bộ lọc, dường như tín hiệu thể hiện rất rõ khả năng tính toán góc của cảm biến, không có sai sót nào. Mặt khác khi không có bộ lọc, tín hiệu sai số rất nhiều, có lúc độ nghiêng của xe vọt lên rất cao so với thực tế.

So sánh hai kết quả, có thể chắc chắn rằng sự chênh lệch rõ ràng của tín hiệu cảm biến khi có bộ lọc và khi không có lọc. Đây là một trong những tín hiệu quan trọng trong quá trình điều khiển xe cân bằng.

3.4.3.2 Chương trình thu tín hiệu từ cảm biến Encoder

Bảng 3.8 Sơ đồ nối chân giữa cảm biến Encoder và Arduino Uno R3

Encoder Arduino Uno R3

GND GND

A 2

VCC 5V

 Nguyên lý thu tín hiệu:

Nguyên lý cơ bản của encoder, đó là một đĩa tròn xoay, quay quanh trục. Trên đĩa có các lỗ (rãnh). Người ta dùng một đèn led để chiếu lên mặt đĩa. Khi đĩa quay, chỗ không có lỗ (rãnh), đèn led không chiếu xuyên qua được, chỗ có lỗ (rãnh), đèn led sẽ chiếu xuyên qua. Khi đó, phía mặt bên kia của đĩa, người ta đặt một con mắt thu. Với các tín hiệu có, hoặc không có ánh sáng chiếu qua, người ta ghi nhận được đèn led có chiếu qua lỗ hay không. Số xung đếm được và tăng lên nó tính bằng số lần ánh sáng bị cắt! Như vậy là encoder sẽ tạo ra các tín hiệu xung vuông và các tín hiệu xung vuông này được cắt từ ánh sáng xuyên qua lỗ. Nên tần số của xung đầu ra sẽ phụ thuộc vào tốc độ quay của tấm tròn đó. Đối với encoder mình đang dùng thì nó có 2 tín hiệu ra lệch pha nhau 90. Hai tín hiệu này có thể xác định được chiều quay của động cơ.

Có rất nhiều phương pháp đọc tín hiệu encoder, trong đó phương pháp ngắt ngoài được xem là dễ và chính xác nhất. Nối kênh A của encoder với 1 ngắt ngoài (INT2 chẳng hạn) và kênh B với một chân nào đó bất kỳ (không phải chân ngắt). Cứ mỗi lần ngắt ngoài xảy ra, tức có 1 xung xuất hiện trên ở kênh A thì trình phục vụ ngắt ngoài tự động được gọi. Trong trình phục vụ ngắt này chúng ta kiểm tra mức của kênh B, tùy theo mức của kênh B chúng ta sẽ tăng biến đếm xung lên 1 hoặc giảm đi 1. Tuy nhiên, bạn cần phải tính toán rất cẩn thận khi sử dụng phương pháp này. Ví dụ trường hợp encoder có độ phân giải 2000 xung/vòng, motor bạn quay với vận tốc 100 vòng/s thì tần số xung trên kênh A của encode là 2000x100=200KHz, nghĩa là cứ mỗi 5 us ngắt ngoài xảy ra một lần. Tần số ngắt như thế là quá cao cho AVR, điều này có nghĩa là AVR chỉ tập trung cho mỗi việc “đếm xung”, không có đủ thời gian để thực thi các việc khác. Trong bài này, chúng ta chọn độ phân giải của encoder là 112 (112 xung trên mỗi vòng quay). Vận tốc tối đa của động cơ được chọn vào khoảng 30 vòng/s nên tần số xung lớn nhất từ encoder là 112x30=3,36KHz.

3.4.3.3. Điều khiển góc xoay và vận tốc góc xoay bằng thuật toán PID

Một bộ điều khiển vi tích phân tỉ lệ (bộ điều khiển PID- Proportional Integral Derivative) là một cơ chế phản hồi vòng điều khiển (bộ điều khiển) tổng quát được sử dụng rộng rãi trong các hệ thống điều khiển công nghiệp – bộ điều khiển PID là bộ điều khiển được sử dụng nhiều nhất trong các bộ điều khiển phản hồi. Bộ điều khiển PID sẽ tính toán giá trị "sai số" là hiệu số giữa giá trị đo thông số biến đổi và giá trị đặt mong muốn. Bộ điều khiển sẽ thực hiện giảm tối đa sai số bằng cách điều chỉnh giá trị điều khiển đầu vào. Trong trường hợp không có kiến thức cơ bản (mô hình toán học) về hệ thống điều khiển thì bộ điều khiển PID là sẽ bộ điều khiển tốt nhất. Tuy nhiên, để đạt được kết quả tốt nhất, các thông số PID sử dụng trong tính toán phải điều chỉnh theo tính chất của hệ thống-trong khi kiểu điều khiển là giống nhau, các thông số phải phụ thuộc vào đặc thù của hệ thống.

Giải thuật tính toán bộ điều khiển PID bao gồm 3 thông số riêng biệt, do đó đôi khi nó còn được gọi là điều khiển ba khâu: các giá trị tỉ lệ, tích phân và đạo hàm, viết tắt là P,I, và D. Giá trị tỉ lệ xác định tác động của sai số hiện tại, giá trị tích phân xác định tác động của tổng các sai số quá khứ, và giá trị vi phân xác định tác động của tốc độ biến đổi sai số. Tổng chập của ba tác động này dùng để điều chỉnh quá trình thông qua một phần tử điều khiển như vị trí của van điều khiển hay bộ nguồn của phần tử gia nhiệt. Nhờ vậy, những giá trị này có thể làm sáng tỏ về quan hệ thời gian: P phụ thuộc vào sai số hiện tại, I phụ thuộc vào tích lũy các sai số quá khứ, và D dự đoán các sai số tương lai, dựa vào tốc độ thay đổi hiện tạiBằng cách điều chỉnh 3 hằng số trong giải thuật của bộ điều khiển PID, bộ điều khiển có thể dùng trong những thiết kế có yêu cầu đặc biệt. Đáp ứng của bộ điều khiển có thể được mô tả dưới dạng độ nhạy sai số của bộ điều khiển, giá trị mà bộ điều khiển vọt lố điểm đặt và giá trị dao động của hệ thống. Lưu ý là công dụng của giải thuật PID trong điều khiển không đảm bảo tính tối ưu hoặc ổn định cho hệ thống. Vài ứng dụng có thể yêu cầu chỉ sử dụng một hoặc hai khâu tùy theo hệ thống. Điều này đạt được bằng cách thiết đặt đội lợi của các đầu ra không mong muốn về 0. Một bộ điều khiển PID sẽ được gọi là bộ điều khiển PI, PD, P hoặc I nếu vắng mặt các tác động bị khuyết. Bộ điều khiển PI khá phổ biến, do đáp ứng vi phân khá nhạy đối với các nhiễu đo lường, trái lại nếu thiếu giá trị tích phân có thể khiến hệ thống không đạt được giá trị mong muốn.

Sơ đồ điều khiển PID được đặt tên theo ba khâu hiệu chỉnh của nó, tổng của ba khâu này tạo thành bởi các biến điều khiển (MV).

Ta có:

Trong đó:

Hình 3. 40 Sự thay đổi trong khâu tích phân

Hình 3. 39 Sự thay đổi trong khâu tỉ lệ

 Khâu tỉ lệ

Khâu tỉ lệ (đôi khi còn được gọi là độ lợi) làm thay đổi giá trị đầu ra, tỉ lệ với giá trị sai số hiện tại. Đáp ứng tỉ lệ có thể được điều chỉnh bằng cách nhân sai số đó với một

hằng số Kp, được gọi là độ lợi tỉ lệ.

Khâu tỉ lệ được cho bởi:

Trong đó:

Pout: thừa số tỉ lệ của đầu ra.

Kp: Độ lợi tỉ lệ, thông số điều chỉnh

e: sai số.

t: thời gian hay thời gian tức thời (hiện tại).

Độ lợi của khâu tỉ lệ lớn là do thay đổi lớn ở đầu ra mà sai số thay đổi nhỏ. Nếu độ lợi của khâu tỉ lệ quá cao, hệ thống sẽ không ổn định. Ngược lại, độ lợi nhỏ là do đáp ứng đầu ra nhỏ trong khi sai số đầu vào lớn, và làm cho bộ điều khiển kém nhạy, hoặc đáp ứng chậm. Nếu độ lợi của khâu tỉ lệ quá thấp, tác động điều khiển có thể sẽ quá bé khi đáp ứng với các nhiễu của hệ thống.

 Khâu tích phân

Phân phối của khâu tích phân (đôi khi còn gọi là reset) tỉ lệ thuận với cả biên độ sai số lẫn quảng thời gian xảy ra sai số. Tổng sai số tức thời theo thời gian (tích phân sai số) cho ta tích lũy bù đã được hiệu chỉnh trước đó. Tích lũy sai số sau đó được nhân với độ lợi tích phân và cộng với tín hiệu đầu ra của bộ điều khiển. Biên độ phân phối của khâu tích phân trên tất cả tác động điều chỉnh được xác định bởi độ lợi tích phân .

Thừa số tích phân được cho bởi:

Trong đó:

Iout: thừa số tích phân của đầu ra.

Ki: độ lợi tích phân, 1 thông số điều chỉnh.

e: sai số .

t: thời gian hoặc thời gian tức thời (hiện tại) : một biến tích phân trung gian.

Khâu tích phân (khi cộng thêm khâu tỉ lệ) sẽ tăng tốc chuyển động của quá trình tới điểm đặt và khử số dư sai số ổn định với một tỉ lệ chỉ phụ thuộc vào bộ điều khiển.

Tuy nhiên, vì khâu tích phân là đáp ứng của sai số tích lũy trong quá khứ, nó có thể khiến giá trị hiện tại vọt lố qua giá trị đặt (ngang qua điểm đặt và tạo ra một độ lệch với các hướng khác). Để tìm hiểu thêm các đặc điểm của việc điều chỉnh độ lợi tích phân và độ ổn của bộ điều khiển.

Tốc độ thay đổi của sai số quá trình được tính toán bằng cách xác định độ dốc của sai số theo thời gian (tức là đạo hàm bậc một theo thời gian) và nhân tốc độ này với độ

lợi tỉ lệ . Biên độ của phân phối khâu vi phân (đôi khi được gọi là tốc độ) trên tất cả các

hành vi điều khiển được giới hạn bởi độ lợi vi phân.

 Khâu vi phân

Hình 3. 41 Sự thay đổi khâu vi phân

Trong đó:

Dout: thừa số vi phân của đầu ra.

Kd: Độ lợi vi phân, một thông số điều chỉnh.

e: Sai số .

t: thời gian hoặc thời gian tức thời (hiện tại) Khâu vi phân làm chậm tốc độ thay đổi của

đầu ra bộ điều khiển và đặc tính này là đang chú ý nhất để đạt tới điểm đặt của bộ điều khiển. Từ đó, điều khiển vi phân được sử dụng để làm giảm biên độ vọt lố được tạo ra bởi thành phần tích phân và tăng cường độ ổn định của bộ điều khiển hỗn hợp. Tuy nhiên, phép vi phân của một tín hiệu sẽ khuếch đại nhiễu và do đó khâu này sẽ nhạy hơn đối với nhiễu trong sai số, và có thể khiến quá trình trở nên không ổn

định nếu nhiễu và độ lợi vi phân đủ lớn. Do đó một xấp xỉ của bộ vi sai với băng thông giới hạn thường được sử dụng hơn. Chẳng hạn như mạch bù sớm pha.

Khâu tỉ lệ, tích phân, vi phân được cộng lại với nhau để tính toán đầu ra của bộ điều khiển PID. Định nghĩa rằng u(t) là đầu ra của bộ điều khiển, biểu thức cuối cùng của giải thuật PID là:

Trong đó các thông số điều chỉnh là:

 Độ lợi tỉ lệ, Kp

Giá trị càng lớn thì đáp ứng càng nhanh do đó sai số càng lớn, bù khâu tỉ lệ càng lớn. Một giá trị độ lợi tỉ lệ quá lớn sẽ dẫn đến quá trình mất ổn định và dao động.

 Độ lợi tích phân KI

Giá trị càng lớn kéo theo sai số ổn định bị khử càng nhanh. Đổi lại là độ vọt lố càng lớn: bất kỳ sai số âm nào được tích phân trong suốt đáp ứng quá độ phải được triệt tiêu tích phân bằng sai số dương trước khi tiến tới trạng thái ổn định.

 Độ lợi vi phân, KD

Giá trị càng lớn càng giảm độ vọt lố, nhưng lại làm chậm đáp ứng quá độ và có thể dẫn đến mất ổn định do khuếch đại nhiễu tín hiệu trong phép vi phân sai số.

Ứng dụng thuật toán PID vào trình điều khiển góc xoay và tốc độ góc xoay bánh đà: Đây là một trong những vòng lặp quan trọng trong hệ thống điều khiển xe cân bằng. Dựa trên nguyên lý của động lực học con quay hồi chuyển, tức là khi góc nghiêng của xe thay đổi, ngay lập tức bánh đà cũng thay đổi góc xoay tương ứng để tạo ra mô men lực chống lại sự đổ nghiêng của xe. Bên cạnh đó tốc độ góc xoay cũng là một trong những thông số ảnh hưởng rất lớn tới giá trị của mô men lực. Vận tốc càng lớn thì mô men càng lớn. Do đó, khi ứng dụng thuật toán PID vào trình điều khiển phải thỏa mãn hai thông số trên thì chương trình mới thực sự ổn định.

Đầu tiên, thiết lập điểm đặt cho chương trình nghĩa là góc ở vị trí cân bằng của xe, đơn vị là độ.

Hình 3. 42 Lưu đồ thiết lập góc đặt mong muốn cho bộ điều khiển PID

Sau khi nhận được giá trị điểm đặt, thuật toán bắt đầu tìm kiếm giá trị đầu vào. Thuật toán bắt đầu nhận tín hiệu đầu vào liên tục từ cảm biến góc nghiêng của xe. Khi xe bắt đầu nghiêng về một phía hợp với phương thẳng đứng một giá trị góc, nhận thấy có sự sai lệch, ngay lập tức hệ thống bắt đầu tính toán sự sai lệch này

e= Setpoint- Input

Trong đó:

e : là sai số.

Setpoint: Góc đặt mong muốn của xe. Input: Giá trị góc nghiêng của xe trả về.

Sau khi có được độ sai lệch e, thuật toán bắt đầu tính toán bằng công thức:

Ứng với các độ lợi về tỉ lệ, tích phân và vi phân mà hệ thống được thiết lập trước đó. Giá trị đầu ra của thuật toán cũng chính là giá trị tốc độ góc dùng để băm xung cho động cơ điều khiển góc xoay, việc này được thực hiện bởi bộ xử lý trung tâm kết hợp với cầu H điều khiển động cơ DC.

Hình 3. 43 Lưu đồ tính toán tốc độ góc xoay bánh đà

3.4.3.4. Chương trình chính

Chương trình điều khiển xe cân bằng là một chương trình rất phức tạp bao gồm nhiều vòng lặp lớn nhỏ cho phép kết hợp tính toán, so sánh các thông số của xe để đảm bảo xe được ổn định trong quá trình hoạt động.

Trong đó có vòng lặp thu thập tín hiệu cảm biến góc nghiêng và xung vị trí góc xoay bánh đà thuật toán điều khiển tốc độ góc xoay PID.

Khi bộ thuật toán PID tính ra tôc độ góc xoay bánh đà, ngay lập tức tín hiệu này sẽ