ROBOT HAI BÁNH TỰ CÂN BẰNG - 2 WHEEL SELF BALANCING ROBOT

I. DẪN NHẬP

Hôm nay mình xin chia sẻ với các bạn cách làm một robot tự cân bằng trên hai bánh xe bằng Arduino từ xe đồ chơi bị hỏng của thằng nhóc ở nhà. Tất nhiên, hai động cơ và hai bánh xe chưa bị hỏng nhé. Để robot tự cân bằng trên hai bánh xe thì chuyển động của nó tương tự như việc giữ thăng bằng một cây gậy trên ngón tay. Điều này chắc các bạn cũng đã từng thử trước đây. Để giữ thăng bằng, chúng ta phải di chuyển ngón tay của mình nhanh hay chậm theo hướng nghiêng và tốc độ nghiêng của cây gậy. Chúng ta bắt đầu tìm hiểu xem làm thế nào mà Arduino có thể tự điều chỉnh được như thế.

II. B.O.M

Các vật tư cần thiết để làm một robot 2 bánh tự cân bằng như sau:

III. CHUẨN BỊ

Trước tiên, chúng ta phải bỏ một ít thời gian để tìm hiểu các thông tin cơ bản sau đây trước khi tiến hành làm một robot tự cân bằng.

3.1. Nguyên tắc con lắc ngược (inverted pendulum)

• Nó giống như việc giữ thăng bằng một cây gậy trên ngón tay. Để giữ thăng bằng, chúng ta phải di chuyển ngón tay của mình theo hướng nghiêng và tốc độ nghiêng của cây gậy. Để điều khiển động cơ bánh xe cho robot tự cân bằng qua mạch cầu L298N, chúng ta cần một số thông tin về trạng thái của robot như: điểm thăng bằng cần cài đặt cho robot, hướng mà robot đang nghiêng, góc nghiêng và tốc độ nghiêng. Tất cả các thông tin này được thu thập từ MPU6050 và đưa vào bộ điều khiển PID để tạo ra một tín hiệu điều khiển động cơ, giữ cho robot ở điểm thăng bằng.
• Về phần lý thuyết và các công thức, các bạn có thể tìm hiểu qua google với các từ khóa: inverted pendulum (con lắc ngược), self-balancing robot hay 2 wheel self-balancing.

3.2. Điều khiển vòng kín P.I.D

• Về P.I.D, các bạn có thể tham khảo trên các website hay qua bài viết của tôi tại: http://arduino.vn/result/5401-pid-sp...
• Ở đây, giá trị cài đặt bộ P.I.D (SP) là điểm cân bằng được hiểu là góc so với phương thẳng đứng, vuông góc với robot. Nếu phần cứng cho robot hoàn hảo, cân bằng và đối xứng thì với thiết kế của mình góc này sẽ là 1800, thực tế điểm SP của mình là 178.700 . Tại sao là 1800 hay 178.700, các bạn hãy xem chương trình bên dưới. Tín hiệu hồi tiếp feedback (PV) là sự kết hợp giữa Gyroscope và Accelerometer được thu thập từ MPU-6050. Output của bộ PID là tín hiệu điều xung tốc độ cho hai động cơ DC sao cho PV tiến tới điểm cân bằng SP.

3.3. MPU-6050

• MPU-6050 là cảm biến của hãng InvenSense tích hợp 6 trục cảm biến bao gồm:
  • Con quay hồi chuyển 3 trục (Gyroscope).
  • Cảm biến gia tốc 3 chiều (Accelerometer).
• Trên cộng đồng cũng đã có một bài viết rất hay về MPU-6050 của bạn Phan Vu Hoang tại: http://arduino.vn/bai-viet/960-doi-d.... Trong đó, bạn Phan Vu Hoang có đề cập đến sensor fusion – sự kết hợp cảm biến. Đó là dùng cả gyro và accel để đo góc và dùng một số loại thuật toán để gộp 2 giá trị với nhau, bù trừ nhau để đưa ra kết quả chính xác nhất. Ở dự án này, mình không dùng bộ lọc do kết quả đạt được từ sensor fusion là tương đối ổn định nhưng mình sẽ thử với bộ lọc Kalman.
• Khi tìm hiểu về MPU-6050, các bạn sẽ gặp phải thuật ngữ QUATERNION, YAW, PITCH, ROLL. Và theo mình, đây là cách giải thích đơn giản và dể hiểu nhất:
• Về Quaternion, các bạn tham khảo bài viết của tác giả Bùi Quang Minh tại địa chỉ này ​https://minhcly.wordpress.com/2014/0...
• Về Yaw, Pitch, Raw, các bạn tham khảo bài viết của tác giả Nguyễn Bình Long tại: https://kienthucbay.wordpress.com/ta...

Xin phép tác giả trích lược lại như sau:

Một máy bay có thể thực hiện bao nhiêu kiểu chuyển động. Các loại chuyển động đó xảy ra xung quanh những trục nào?

Một máy bay có thể thực hiện 3 kiểu chuyển động. Nó có thể gọi pitch, roll và yaw.

• Pitch là kiểu chuyển động khi mũi của máy bay chúc lên trên hoặc chúi xuống dưới. Chuyển động pitch diễn ra xung quanh trục ngang của máy bay.

Roll là kiểu chuyển động khi một trong hai cánh của máy bay liệng xuống còn cánh còn lại thì liệng lên. Ví dụ, nếu máy bay đang roll sang bên trái thì cánh trái sẽ liệng xuống còn cánh phải thì liệng lên. Chuyển động roll diễn ra xung quanh trục dọc thân máy bay.

Yaw là kiểu chuyển động khi mũi của máy bay di chuyển qua phải hoặc qua trái. Chuyển động yaw diễn ra xung quanh trục thẳng đứng, vuông góc với thân máy bay.

• Hiểu được các chuyển động của máy bay ở trên sẽ giúp chúng ta dễ dàng hình dung ra được robot tự cân bằng sẽ hoạt động như thế nào. Và chắc các bạn cũng đoán được nó chuyển động theo kiểu nào rồi!
• Ngoài ra, các bạn còn phải tìm hiểu về cách calibrate MPU-6050 (Việt Nam mình hay gọi là “ca líp” như là “ca líp cân”, thường được dùng để hiệu chỉnh các sensor như loadcell, pressure transmitter, flow transmitter …)
• Các bạn có thể xem các chuyển động yaw, pitch, roll của robot tự cân bằng tại địa chỉ:

IV. SƠ ĐỒ MẠCH VÀ KHUNG ROBOT

4.1. Phần cứng và khung robot

• Khi làm phần cứng các bạn lưu ý làm phần khung cho robot phải cứng cáp, chịu được va đập trong quá trình test và đối xứng thì robot sẽ đẹp và dễ cân bằng hơn.
• Phần khung robot: bằng mica, thiết kế của mình còn thiếu một tầng chứa pin do chưa có đủ tiền để mua nó. 
• Robot bằng đồ chơi dễ tìm được điểm cân bằng do bánh của nó có các gai nhỏ. Do đây là đồ chơi bị hỏng nên nó bị vẹo một tí. Sau đó mình đã mua 2 động cơ DC và bánh xe khác để thử. Kết quả thật tuyệt vời! Đây là hai phiên bản của em nó.

4.2. Sơ đồ hiệu chỉnh P, I, D bằng chương trình

Với sơ đồ này, các bạn phải tìm các thông số P, I, D bằng các phép thử và theo mình sẽ mất nhiều thời gian. Nhưng Arduino sẽ còn nhiều chân Analog để có thể làm việc khác. Một cách khác là điều chỉnh P, I, D qua Serial nhưng nó sẽ không hiệu quả lắm do Arduino thường xuyên bị treo (pending/ freezing). Qua tìm hiểu, mình thấy có rất nhiều người trên các diễn đàn than phiền về vấn đề này nhưng vẫn chưa có cách giải quyết....

4.3. Sơ đồ hiệu chỉnh P.I.D qua biến trở

Với việc có thêm các biến trở để hiệu chỉnh các hệ số P, I, D sẽ làm giảm rất nhiều thời gian mò mẫm các hệ số này sao cho robot hoạt động ổn định, mượt mà. Các hệ số P, I và D được tìm như sau:

• Đặt tất cả các biến trở P, I, D về 0.
• Tăng dần biến trở P cho đến khi robot bắt đầu dao động qua lại xung quanh điểm cân bằng nhưng robot vẫn không bị ngã.
• Tăng dần biến trở D cho đến khi robot không còn dao động. Lúc này, robot hoạt động tương đối ổn định nhưng sẽ bị khựng khựng khi bị tác động bằng tay.
• Tăng dần biến trở I từ từ cho đến khi hệ thống hoạt động ổn định mượt mà ngay cả khi đẩy mạnh robot về một phía. Nếu giá trị biến trở I lớn nó sẽ làm cho robot đáp ứng chậm.

Với robot của mình, các thông số PID tìm được là:

• KP = 10.50.
• KI  = 67.44.
• KD = 0.88.

V. THƯ VIỆN VÀ CHƯƠNG TRÌNH

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

Chương trình chính và việc áp dụng các thư viện được tham khảo từ nhiều nguồn khác nhau, trang tham khảo chính: https://github.com/lukagabric/Franko...

1. #include"PID_v1.h"
2. #include"LMotorController.h"
3. #include"I2Cdev.h"
4.  
5. #include"MPU6050_6Axis_MotionApps20.h"
6.  
7. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
8. #include"Wire.h"
9. #endif
10.  
11. #define LOG_INPUT 0
12. #define MANUAL_TUNING 1
13. #define LOG_PID_CONSTANTS 1//MANUAL_TUNING must be 1
14. #define MOVE_BACK_FORTH 0
15.  
16. #define MIN_ABS_SPEED 5
17.  
18. //MPU
19.  
20. MPU6050 mpu;
21.  
22. // MPU control/status vars
23. bool dmpReady =false;// set true if DMP init was successful
24. uint8_t mpuIntStatus;// holds actual interrupt status byte from MPU
25. uint8_t devStatus;// return status after each device operation (0 = success, !0 = error)
26. uint16_t packetSize;// expected DMP packet size (default is 42 bytes)
27. uint16_t fifoCount;// count of all bytes currently in FIFO
28. uint8_t fifoBuffer[64];// FIFO storage buffer
29.  
30. // orientation/motion vars
31. Quaternion q;// [w, x, y, z] quaternion container
32. VectorFloat gravity;// [x, y, z] gravity vector
33. float ypr[3];// [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
34.  
35. //PID
36.  
37. #if MANUAL_TUNING
38. double kp , ki, kd;
39. double prevKp, prevKi, prevKd;
40. #endif
41. double originalSetpoint =178.70;// 181.13
42. double setpoint = originalSetpoint;
43. double movingAngleOffset =0.15;// 0.3- OK, 0.15 - OK
44. double input, output;
45. int moveState=0;//0 = balance; 1 = back; 2 = forth
46.  
47. #if MANUAL_TUNING
48. PID pid(&input,&output,&setpoint,0,0,0, DIRECT);
49. #else
50. PID pid(&input,&output,&setpoint,10.50,67.44,0.88, DIRECT);// time 5ms & 10ms, sometimes Kp(17.35, 16.86) Ki(302.05, 301.05) Kd(1.21)
51. #endif
52.  
53. //MOTOR CONTROLLER
54.  
55. int ENA =3;
56. int IN1 =4;
57. int IN2 =8;
58. int IN3 =5;
59. int IN4 =7;
60. int ENB =6;
61.  
62. LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4,1,1);
63.  
64. //timers
65.  
66. long time1Hz =0;
67. long time5Hz =0;
68.  
69. volatilebool mpuInterrupt =false;// indicates whether MPU interrupt pin has gone high
70. void dmpDataReady()
71. {
72. mpuInterrupt =true;
73. }
74.  
75. void setup()
76. {
77. // join I2C bus (I2Cdev library doesn't do this automatically)
78. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
79. Wire.begin();
80. TWBR =24;// 400kHz I2C clock (200kHz if CPU is 8MHz)
81. #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
82. Fastwire::setup(400,true);
83. #endif
84.  
85. // initialize serial communication
86. // (115200 chosen because it is required for Teapot Demo output, but it's
87. // really up to you depending on your project)
88. Serial.begin(115200);
89. while(!Serial);// wait for Leonardo enumeration, others continue immediately
90.  
91. // initialize device
92. Serial.println(F("Initializing I2C devices..."));
93. mpu.initialize();
94.  
95. // verify connection
96. Serial.println(F("Testing device connections..."));
97. Serial.println(mpu.testConnection()? F("MPU6050 connection successful"): F("MPU6050 connection failed"));
98.  
99. // load and configure the DMP
100. Serial.println(F("Initializing DMP..."));
101. devStatus = mpu.dmpInitialize();
102.  
103. // supply your own gyro offsets here, scaled for min sensitivity
104. mpu.setXGyroOffset(39);
105. mpu.setYGyroOffset(14);
106. mpu.setZGyroOffset(6);
107. mpu.setZAccelOffset(1788);// 1688 factory default for my test chip
108.  
109. // make sure it worked (returns 0 if so)
110. if(devStatus ==0)
111. {
112. // turn on the DMP, now that it's ready
113. Serial.println(F("Enabling DMP..."));
114. mpu.setDMPEnabled(true);
115.  
116. // enable Arduino interrupt detection
117. Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
118. attachInterrupt(0, dmpDataReady, RISING);
119. mpuIntStatus = mpu.getIntStatus();
120.  
121. // set our DMP Ready flag so the main loop() function knows it's okay to use it
122. Serial.println(F("DMP ready! Waiting for first interrupt..."));
123. dmpReady =true;
124.  
125. // get expected DMP packet size for later comparison
126. packetSize = mpu.dmpGetFIFOPacketSize();
127. //setup PID
128. pid.SetMode(AUTOMATIC);
129. pid.SetSampleTime(5);// 10 - OK, 5 - GOOD, 1- CHANGE PID
130. pid.SetOutputLimits(-255,255);// 80 - OK Strong enough
131. }
132. else
133. {
134. // ERROR!
135. // 1 = initial memory load failed
136. // 2 = DMP configuration updates failed
137. // (if it's going to break, usually the code will be 1)
138. Serial.print(F("DMP Initialization failed (code "));
139. Serial.print(devStatus);
140. Serial.println(F(")"));
141. }
142. }
143.  
144. void loop()
145. {
146. // if programming failed, don't try to do anything
147. if(!dmpReady)return;
148.  
149. // wait for MPU interrupt or extra packet(s) available
150. while(!mpuInterrupt && fifoCount < packetSize)
151. {
152. //no mpu data - performing PID calculations and output to motors
153. pid.Compute();
154. motorController.move(output, MIN_ABS_SPEED);
155. unsignedlong currentMillis = millis();
156.  
157. if(currentMillis - time1Hz >=1000)
158. {
159. loopAt1Hz();
160. time1Hz = currentMillis;
161. }
162. if(currentMillis - time5Hz >=5000)
163. {
164. loopAt5Hz();
165. time5Hz = currentMillis;
166. }
167. }
168.  
169. // reset interrupt flag and get INT_STATUS byte
170. mpuInterrupt =false;
171. mpuIntStatus = mpu.getIntStatus();
172.  
173. // get current FIFO count
174. fifoCount = mpu.getFIFOCount();
175.  
176. // check for overflow (this should never happen unless our code is too inefficient)
177. if((mpuIntStatus &0x10)|| fifoCount ==1024)
178. {
179. // reset so we can continue cleanly
180. mpu.resetFIFO();
181. Serial.println(F("FIFO overflow!"));
182.  
183. // otherwise, check for DMP data ready interrupt (this should happen frequently)
184. }
185. elseif(mpuIntStatus &0x02)
186. {
187. // wait for correct available data length, should be a VERY short wait
188. while(fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
189.  
190. // read a packet from FIFO
191. mpu.getFIFOBytes(fifoBuffer, packetSize);
192. // track FIFO count here in case there is > 1 packet available
193. // (this lets us immediately read more without waiting for an interrupt)
194. fifoCount -= packetSize;
195.  
196. mpu.dmpGetQuaternion(&q, fifoBuffer);
197. mpu.dmpGetGravity(&gravity,&q);
198. mpu.dmpGetYawPitchRoll(ypr,&q,&gravity);
199. #if LOG_INPUT
200. Serial.print("ypr\t");
201. Serial.print(ypr[0]*180/M_PI);
202. Serial.print("\t");
203. Serial.print(ypr[1]*180/M_PI);
204. Serial.print("\t");
205. Serial.println(ypr[2]*180/M_PI);
206. #endif
207. input = ypr[1]*180/M_PI +180;
208. }
209. }
210.  
211. void loopAt1Hz()
212. {
213. #if MANUAL_TUNING
214. setPIDTuningValues();
215. #endif
216. }
217.  
218. void loopAt5Hz()
219. {
220. #if MOVE_BACK_FORTH
221. moveBackForth();
222. #endif
223. }
224.  
225. //move back and forth
226.  
227.  
228. void moveBackForth()
229. {
230. moveState++;
231. if(moveState >2) moveState =0;
232. if(moveState ==0)
233. setpoint = originalSetpoint;
234. elseif(moveState ==1)
235. setpoint = originalSetpoint - movingAngleOffset;
236. else
237. setpoint = originalSetpoint + movingAngleOffset;
238. }
239.  
240. //PID Tuning (3 potentiometers)
241.  
242. #if MANUAL_TUNING
243. void setPIDTuningValues()
244. {
245. readPIDTuningValues();
246. if(kp != prevKp || ki != prevKi || kd != prevKd)
247. {
248. #if LOG_PID_CONSTANTS
249. Serial.print(kp);Serial.print(", ");Serial.print(ki);Serial.print(", ");Serial.println(kd);
250. #endif
251.  
252. pid.SetTunings(kp, ki, kd);
253. prevKp = kp; prevKi = ki; prevKd = kd;
254. }
255. }
256.  
257. void readPIDTuningValues()
258. {
259. int potKp = analogRead(A0);
260. int potKi = analogRead(A1);
261. int potKd = analogRead(A2);
262. kp = map(potKp,0,1023,0,25000)/100.0;//0 - 250
263. ki = map(potKi,0,1023,0,100000)/100.0;//0 - 1000
264. kd = map(potKd,0,1023,0,500)/100.0;//0 - 5
265. }
266. #endif

5.2. Các thư viện cho dự án

Các bạn phải inlcude các thư viện sau vào chương trình chính:

• Thư viện I2Cdev
• Thư viện MPU6050
• Thư viện LMotorController
• Thư viện PID_v1

V. LỜI KẾT

• Việc tinh chỉnh các thông số PID qua các biến trở sẽ giúp chúng ta hiểu hơn về điều khiển vòng kín có hồi tiếp vốn dĩ là rất phức tạp và các mô phỏng vật thể qua MPU-6050 cho ta cảm nhận thực về các chuyển động trong không gian. 
• Robot hai bánh tự cân bằng hoạt động khá tốt ngay cả ở mặt đường gồ ghề, nghiêng, trên thảm, trên nệm và mang thêm các vật nhẹ trên nó. Nó cũng có thể lấy lại cân bằng khi bị xoay trái - phải hay đẩy tới - lui.