Khai báo biến: Định nghĩa kiểu dữ liệu và đặt tên cho các biến sử dụng trong quá trình lập trình. Lưu ý khi khai báo cần xác định rõ ràng kiểu dữ liệu của biến, loại biến và dấu của biến, điều này sẽ giúp tối ưu hóa trong quá trình biên dịch mã, tránh tình trạng khai báo thừa ảnh hưởng đến tốc độ xử lý.
Đọc tín hiệu từ cảm biến góc nghiêng MPU sẽ trả về giá trị độ (degree) do đó ta cần có phương pháp chuyển giá trị này sang đơn vị Radian:
(kalAngleY - offset)*pi/180
Trong đó:
kalAngleY là giá trị góc đọc về từ cảm biến và offset là hệ số khi đặt cảm biến, gía trị sẽ có sai số khiến cho xe không thể cân bằng, do đó ta lấy góc đọc được trừ đi sai số có được giá trị điều khiển cho góc nghiêng.
Đọc encoder động cơ JGA25 trả về 1 vòng được 11 xung:
Góc tới = (𝑋𝑢𝑛𝑔𝑅+𝑋𝑢𝑛𝑔𝐿)∗360 2∗11
Chuyển qua Radian: góc này sử dụng để điều khiển robot đi tới hoặc đi lui Theta = góc tới*(pi/180)
Góc xoay = 360𝑅
11𝑊 (𝑋𝑢𝑛𝑔𝑅 − 𝑋𝑢𝑛𝑔𝐿)
Với Xung_L và Xung_R lần lượt là số xung encoder của hai đông cơ trái và phải.
Chuyển qua radian:
Góc Phi sử dụng điều khiển robot xoay trái hoặc xoay phải: Phi = gócxoay * (pi/180).
Các đại lượng này được đưa vào bộ điều khiển LQR. Khi xe nghiêng thì động cơ sẽ đảo chiều và điều khiển để robot không bị đổ . Tín hiệu xung PWM của arduino sẽ nhận từ 0-255 vì thanh ghi chỉ có 8 bit.
4.2.Kết quả mô hình thực tế
4.3.Hoạt động của mô hình
Hình 4.4: Giá trị góc nghiêng psi trong quá trình chạy mô hình
Hình 4.5: Giá trị góc tới theta trong quá trình chạy mô hình
Nhận xét:
Từ đồ thị thể hiện giá trị các góc điều khiển ta thấy mô hình có đạt được giá trị đáp ứng được tại vị trí cân bằng, tuy nhiên còn chưa thực sự tối ưu, thời gian làm việc của hệ tại điểm cân bằng còn chưa được tốt nhất.
KẾT LUẬN
Qua những thời gian nghiên cứu và thực hiện đề tài, nhóm đã có cái nhìn tổng quát về lĩnh vực điều khiển hệ thống phi tuyến. Đặc biệt đồ án đã nghiên cứu và giới thiệu một số ứng dụng hữu ích và mới nhất sử dụng công nghệ này trên thế giới, để chứng minh cho độc giả thấy được đây là một công nghê hay, cần được tiếp tục nghiên cứu ứng dụng nhiều hơn nữa. Và hứa hẹn sẽ làm thay đổi và mở kỷ nguyên mới cho công nghệ 4.0
Từ yêu cầu đề tài, nhóm đã thực hiện và nghiên cứu được những vấn đề sau: Thiết kế khung Robot phù hợp việc cân bằng, khả năng di chuyển của Robot Thiết lập được mô hình toán và hàm trạng thái cho mô hình
Xây dựng chương trình mô phỏng hoạt động bằng MatLAB-Simulink Giao tiếp được giữa Arduino và MPU 6050
Đọc giá trị góc tốt nhất, hạn chế nhiễu khi sử dụng bộ lọc Kalman Robot có thể tự cân bằng
Ngoài những vấn đề đạt được, nhóm cũng có những vấn đề cần cải thiện và các hướng phát triển cho đề tài trong tương lai:
Phát triển thêm hệ thống xử lý ảnh để giám sát theo dõi theo đường đi của Robot.
Phát triển thêm hệ thống điều khiển Robot di chuyển thông qua module IOT. Điều khiển thích nghi để hệ thống xe có thể thực nghiệm cho người đứng lên với các cân nặng khác nhau và thay đổi trong quá trình hoạt động của xe.
Nhận dạng thông số hệ thống bằng các phương pháp nhận dạng để có thể áp dụng giải thuật LQR.
Ứng dụng các giải thuật điều khiển thông minh dùng mạng neuron,… trong điều khiển hệ thống.
Nâng cấp giải thuật điều khiển để áp dụng cho hệ xe một bánh tự cân bằng… Hướng phát triển sắp tới mà học viên dự kiến là sẽ áp dụng các phương pháp thích nghi, tự dò tìm thông số điều khiển phù hợp thông qua quá trình điều khiển online.
Ngoài ra, một hướng phát triển khác là dùng các phương pháp nhận dạng để xác định thông số mô hình động cơ. Từ đó, tính toán giá trị LQR phù hợp nhất để có thể điều khiển xe chạy tới, lui, quẹo trái, quẹo phải trong thực tế.
Mặc dù đã rất cố gắng trong việc nghiên cứu và thực hiện đồ án, nhưng do thời gian và hiểu biết của nhóm còn hạn chế nên đồ án chỉ dừng lại ở mức ứng dụng đơn giản, mà chưa kịp thực hiện triển khai ứng dụng thực tế bởi việc này sẽ cần thêm rất nhiều kiến thức cũng như sự hiểu biết ở lĩnh vực công nghệ khác. Đồng thời, đồ án chắc chắn cũng không tránh khỏi những thiếu sót, nên nhóm chúng em rất mong nhận được ý kiến đóng góp từ thầy.
TÀI LIỆU THAM KHẢO
Tiếng Việt:
[1] Nguyễn Thị Phương Hà, Huỳnh Thái Hoàng (2005). Lý thuyết điều khiển nâng cao trang 236 – 242.
[2] Huỳnh Thái Hoàng (2014). Bài giảng Lý thuyết điều khiển nâng cao, chương 3 trang 90 - 115 Điều khiển tối ưu toàn phương tuyến tính LQR.
[3] Trần Anh Tứ (2014). Luận văn thạc sỹ “Nghiên cứu, thiết kế, thử nghiệm xe hai bánh tự cân bằng”.
[4] Nguyễn Văn Đông Hải, Hướng dẫn điều khiển LQR cho hệ xe hai bánh tự cân bằng
“https://www.youtube.com/watch?v=eeaqPCHMAXg&t=2426s”.
[5] Đào Văn Hiệp (2004). Giáo trình Kỹ thuật Robot trang 47-52.
[6]Nguyễn Thị Hồng Phương (2011). Bài toán điều khiển được và phương trình Riccati
PHỤ LỤC
#include <Wire.h> // Cung cấp thư viện I2C #include "Kalman.h" // Thư viện bộ lọc Kalman
#define ToRad PI/180 // định nghĩa hằng chuyển đổi giá trị độ sang Radian
#define ToDeg 180/PI // định nghĩa hằng chuyển đổi giá trị Radian sang Độ
Kalman kalman ; // Khởi tạo một hằng Kalman dùng để gọi các lệnh trong thư viện Kalman
uint32_t timerloop, timerold; // Tạo ra thời gian lặp để lấy mẫu // Chân điều khiển cho Motor //
#define LED 13
#define leftpwm 10 // Điều khiển PWM cho động cơ 1 #define leftdir_t 6 // Điều khiển chiều
#define leftdir_n 12
#define rightpwm 11 // Điều khiển PWM cho động cơ 2 #define rightdir_t 8 // Điều khiển chiều
#define rightdir_n 9
volatile long leftencoder;
// Đọc và lưu giá trị của encoder trái ( volatile là kiểu khai báo dành cho giá trị của biến thay đổi một cách không xác định.)
volatile long rightencoder; // Đọc và lưu giá trị của encoder phải #define leftencoder_a 3 // đọc trạng thái kênh A, LOW or HIGH #define leftencoder_b 4 // đọc trạng thái kênh B, LOW or HIGH #define rightencoder_a 19
#define rightencoder_b 5 // dữ liệu MPU6050 //
float mpudata; // Biến xác định góc hiện thời
float AcX, AcZ; // giá trị Gyroscope theo trục X và Z float Gyro; // giá trị vận tốc góc quay tính theo trục Y
uint32_t timer; // cập nhật và tính toán thời gian lấy mẫu, đọc thời gian lấy mẫu và dữ liệu từ MPU, thời gian cho bộ lọc Kalmal tính góc psi
uint8_t i2cData[14]; // Lưu giá trị đọc từ cảm biến MPU // Dữ liệu LQR //
long PWML, PWMR; // Giá trị xung PWM cho cầu H
float k1, k2, k3, k4, k5, k6; // các thông số ma trận K
bool falldown; // Chạy = true; Dừng = false, định nghĩa để điều khiển ĐC
float theta, psi, phi; // Góc
float thetadot, psidot, phidot; // Giá trị vận tốc góc tương ứng float thetaold, psiold, phiold; // Giá trị trước đó của các góc float leftvolt; // Điện áp ra của Motor trái trong LQR
float rightvolt; // Điện áp ra của Motor phải trong LQR
float addtheta; // lưu lại giá trị đặt để động cơ di chuyển và bám theo nó
float addphi; // lưu lại giá trị đặt để động cơ di chuyển và bám theo nó
int ForwardBack; // 1 -> Tới; -1 -> Lùi; 0 -> Ngừng và cân bằng int LeftRight;
void setup()
{ // Set up các ngỏ vào ra cho hệ xe // pinMode(leftpwm, OUTPUT); pinMode(leftdir_t, OUTPUT); pinMode(leftdir_n, OUTPUT); pinMode(rightpwm, OUTPUT); pinMode(rightdir_t, OUTPUT); pinMode(rightdir_n, OUTPUT); // Ngỏ vào của encoder
// mặc định nhận giá trị HIGH
pinMode(leftencoder_a, INPUT_PULLUP); pinMode(rightencoder_a, INPUT_PULLUP); pinMode(leftencoder_b, INPUT);
pinMode(rightencoder_b, INPUT);
// Khởi tạo chuẩn giao tiếp bandte giữa Arduino và máy tính
Serial.begin(115200);
// Dữ liệu MPU6050 //
Wire.begin(); // Khởi tạo thư viện cho MPU hoạt động // Write to all four registers at the same time while (i2cWrite(0x19, i2cData, 4, false)); while (i2cWrite(0x6B, 0x01, true));
while (i2cRead(0x75, i2cData, 1)); if (i2cData[0] != 0x68)
{
Serial.print(F("Error reading sensor")); while(true);
}
delay(100); // Chờ cảm biến ổn định while (i2cRead(0x3B, i2cData, 6));
AcX = (int16_t)((i2cData[0] << 8) | i2cData[1]); AcZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); double pitch = (-AcX, AcZ)*RAD_TO_DEG;
kalman.setAngle(pitch); timer = micros(); // Các giá trị K cần lựa chọn // k1 = -0.00086 ; // k1*theta 0.1 k2 = 0.032; // k2*thetadot 0.38 k3 = -598; // k3*psi 129 705 178 214 k4 = 11; // k4*psidot 10 k5 = 0.038; // k5*phi 0.038 k6 = -0.32; // k6*phidot 0.32
// Cài điểm bắt đầu // addphi = 0;
addtheta = 0; //Ngắt encoder //
attachInterrupt(digitalPinToInterrupt(leftencoder_a), left_isr, FALLING); // Pin 3 ~ motor trái, ngắt xuống.
attachInterrupt(digitalPinToInterrupt(rightencoder_a), right_isr, FALLING);// Pin 19 ~ motor phải.
void loop() {
if((micros() - timerloop) > 6000) // thời gian lấy mẫu 6s {
readMPU6050(); // đọc giá trị từ cảm biến // tính góc theta
theta = gettheta(leftencoder,rightencoder)*ToRad;
// tính góc nghiêng phi, 0.54 là sai số góc nghiêng tùy thuộc vào mỗi con cảm biến
psi = (mpudata + 0.3)*ToRad; // tính góc xoay phi
phi = getphi(leftencoder,rightencoder)*ToRad; // Cập nhật và so sánh thời gian lấy mẫu//
float dt = (float)(micros() - timerloop)*1000000.0; // Gán lại thời gian đầu cho vòng lặp
timerloop = micros();
// Cập nhật lại giá trị vận tốc góc vào// thetadot = (theta - thetaold)/dt;
psidot = (psi - psiold)/dt; phidot = (phi - phiold)/dt;
// Cập nhật lại giá trị góc vào// thetaold = theta;
psiold = psi; phiold = phi;
// Tính toán đưa ra xung PWM cấp vào hai động cơ getlqr(theta + addtheta, thetadot, psi, psidot, phi + addphi, phidot);
motorcontrol(PWML, PWMR, (mpudata+0.3), falldown);// điều khiển motor
// Gửi dữ liệu góc nghiêng lên//
//Serial.println(psi*ToDeg); //Serial.println("\t\t"); Serial.println(theta*ToDeg); Serial.println("\t\t"); //Serial.println(phi*ToDeg); //Serial.println("\t\t"); } }
//hàm con đọc encoder motor trái // void left_isr() { if(digitalRead(leftencoder_a)) leftencoder--; else leftencoder++; }
//hàm con đọc encoder motor phải // void right_isr() { if(digitalRead(rightencoder_a)) rightencoder--; else rightencoder++;
// Đọc giá trị góc Psi // void readMPU6050()
{
while (i2cRead(0x3B, i2cData, 14));
AcX = (int16_t)((i2cData[0] << 8) | i2cData[1]); // gia tốc theo trục X
AcZ = (int16_t)((i2cData[4] << 8) | i2cData[5]); // gia tốc theo trục Z
Gyro = (int16_t)((i2cData[10] << 8) | i2cData[11]); // Vận tốc góc quay theo trục Y
double dt = (double)(micros() - timer)/1000000.0; // thời gian
timer = micros(); // trả về thời gian đầu lấy mẫu
double pitch = atan2(-AcX, AcZ)*RAD_TO_DEG; // tính giá trị góc theo trục X và Z đổi ra độ
double Gyrorate = Gyro/131.0; // chia cho độ nhạy LBS của cảm biến
mpudata = kalman.getAngle(pitch, Gyrorate, dt); // Sữ dụng bộ lọc Kalman kết hợp giữa gia tốc và vận tốc góc tính ra góc hiện thời của cảm biến
if(abs(mpudata) > 45 ) // giới hạn góc hoạt động cho hệ xe falldown = true; // Ngừng Motor
else
falldown = false; // Chạy }
// Đọc giá trị Theta//
float gettheta(long lencoder, long rencoder) {
float angle = 16.36*(lencoder + rencoder);
//(1/2(thetal+thetar) mà theta bằng (xung/11)*360 xung ở đây là lencoder và rencoder)
return angle; }
// đọc giá trị góc phi//
float getphi(long lencoder, long rencoder) {
// 0.21 được tính toán theo công thức R/W float angle = 0.21*(rencoder - lencoder); return angle;
}
// Hàm LQR//
void getlqr(float theta_, float thetadot_, float psi_, float psidot_, float phi_, float phidot_)
{
leftvolt = k1*theta_ + k2*thetadot_ + k3*psi_ + k4*psidot_ - k5*phi_ - k6*phidot_;
rightvolt = k1*theta_ + k2*thetadot_ + k3*psi_ + k4*psidot_ + k5*phi_ + k6*phidot_;
PWML = map(leftvolt, -(k3*PI)/28, (k3*PI)/28, -250, 250); // Giới hạn góc
PWMR = map(rightvolt, -(k3*PI)/28, (k3*PI)/28, -250, 250); PWML = constrain(PWML, -240, 240); //giới hạn xung cho cầu H PWMR = constrain(PWMR, -240, 240 );
// hàm điều khiển động cơ//
void motorcontrol(long PWML, long PWMR, float angle, bool stopstate)
{
if(stopstate == 1) stopandreset(); else
{
// góc psi trong khoảng -0.3 đến 0.3 thì stop động cơ if( abs(angle) < 0.3 ) stopandreset();
else { if(leftvolt > 0) { leftmotor(abs(PWML), 1); // Forward } else if(leftvolt < 0) { leftmotor(abs(PWML), 0); // Back } else { stopandreset(); } if(rightvolt > 0) { rightmotor(abs(PWMR), 1); // Forward } else if(rightvolt < 0) { rightmotor(abs(PWMR), 0); // Back } else { stopandreset(); } } } }
//Control left motor
void leftmotor(uint8_t lpwm, int direct) { if(direct == 1) { // angle > 0 digitalWrite(leftdir_t, HIGH); digitalWrite(leftdir_n, LOW); analogWrite(leftpwm, lpwm); } else { digitalWrite(leftdir_t, LOW); digitalWrite(leftdir_n, HIGH); analogWrite(leftpwm, lpwm); } }
//Control right motor
void rightmotor(uint8_t rpwm, int direct) { if(direct == 1) { // angle > 0 digitalWrite(rightdir_t, HIGH); digitalWrite(rightdir_n, LOW); analogWrite(rightpwm, rpwm); } else { digitalWrite(rightdir_t, LOW); digitalWrite(rightdir_n, HIGH); analogWrite(rightpwm, rpwm); } } // Dừng và reset dữ liệu//
// dữ liệu góc và encoder sẽ reset về không// void stopandreset()
{
analogWrite(leftpwm, 0); analogWrite(rightpwm, 0); // Reset default place // leftencoder = 0;
rightencoder = 0;
// Reset zero set point // addtheta = 0;
addphi = 0; }