Giới thiệu cảm biến Gyro MPU6050 ( CB vận tốc góc) trong Robocon

**thanhpho44** · 07-06-2013, 05:56 PM

Dưới đây là những gì mình đã sử dụng trong mùa Robocon 2013 và khá ổn. Mong mọi người góp ý thêm vì là lần đầu sử dụng nên có khá nhiều thiếu sót.

-Ý tưởng và lý do sử dụng: Sân Robot năm 2013 như các bạn thấy có rất ít line và hầu hết là line cong nên việc dò đường theo line sẽ làm tốc độ robot giảm đáng kể. (cái này là do phương án chạy, đội mình sử dụng thả từng ring một nên việc chạy theo đường thẳng kẻ theo các ring là nhanh nhất). Nên yêu cầu được đặt ra là Robot chạy thẳng được theo 1 đường giả định. quay chính xác góc mà ít ảnh hưởng đến yếu tố bên ngoài.

-Tại sao là Gyro mà ko phải là la bàn số: theo như các bạn biết thì la bàn số ảnh hưởng nhiều vào kim loại có từ tính. mà sân năm 2013 dùng viền hoàn toàn bằng sắt nên khả năng ảnh hưởng bới sân thi đấu rất cao.

-Ưu điểm của Gyro:
+Quay với góc chính xác +Ít phụ thuộc yếu tố bên ngoài +Khá rẻ. Module GY521 tại một số nơi bán có 200k thôi. +Thuật toán lập trình khá đơn giản. +Chuẩn giao tiếp I2C
-Nhược điểm:
+Dễ trôi điểm gốc. Nhưng chống trôi ổn thì chạy hết trận vẫn có thể có độ chính xác cao. +Mình hay gặp trường hợp bị treo chip do việc đọc Gyro phải thực hiện trong ngắt timer. Mình lại dùng một số ngắt nữa nên hay xảy ra trường hợp xung đột ngắt. @@. Sau này mình hạn chế xử lý số liệu trong ngắt timer nên cũng khắc phục được lỗi nhưng vẫn không dám chắc là hết lỗi.
-Hình ảnh thực tế:

[IMG]http://*************/attachments/6763/[/IMG]

-Hàm Init MPU6050: tài liệu hướng dẫn của PNlab đơn giản hơn. cái này mình tìm hiểu khi chưa có tài liệu tiếng việt mà dựa chủ yếu vào các code và tài liệu của các trang máy bay mô hình của nước ngoài nên lằng nhằng. @@.

Mã:

 
void init_sensor_Gyro_MPU6050(void)
 
{
 
    delay_ms(10);
 
    i2c_writereg(MPU6050_ADD,0x6b,0x80);    //reset device, reset cam bien
 
    delay_ms(5);
 
    i2c_writereg(MPU6050_ADD,0x6b,0x00);    //cam bien hoat dong
 
    i2c_writereg(MPU6050_ADD,0x19,0x00);  //Sample Rate Divider = 0; cai nay minh khong hieu lam. @@
 
    i2c_writereg(MPU6050_ADD,0x1a,0x00);  //Configuration;
 
    i2c_writereg(MPU6050_ADD,0x1b,0x10);  //Gyroscope Configuration; 0-8-10-18 ; do phan giai Gyro la 1000do/s
 
    i2c_writereg(MPU6050_ADD,0x6a,0x00);  //User Control; Con MPU6050 co kha nang lam master
 
                                            //cua 1 so con có chuan truyen I2C khac, o day khong dung nen khong config
 
    i2c_writereg(MPU6050_ADD,0x37,0x02);
 
}

-Hàm lấy giá trị trong timer: do con này trả về giá trị là vận tốc góc nên ta tiến hành thực hiện phép tích phân để có giá trị góc do đó phải đưa việc tính toán vào ngắt timer.

Mã:

// Timer2 overflow interrupt service routine
interrupt [TIM2_OVF] void timer2_ovf_isr(void)
{
// Reinitialize Timer2 value
          TCNT2=0x60;
          gyroADC[2]=-read_gyro_Z();
          goc=goc + ((float)gyroADC[2]+12.34)*0.0595/3.28;    //GY-521
          //                              [a]      [b]    [c]
          // 00    0    250    131
          // 08    1    500    65.5
          // 10    2    1000    32.8
          // 18    3    2000    16.4
 
}

Một số chú ý trong chương trình:
+ [a]: là hệ số chống trôi. mỗi cảm biến sẽ có một giá trị khác nhau. Để tìm hệ số này, các bạn cho hiển thị giá trị ADC đọc được của trục Z lên LCD khi cảm biến ĐỨNG YÊN hoàn toàn. [a]=-(giá trị đọc được). ADC của trục Z là số nguyên 16 bit có dấu. để có độ chính xác cao thì ta ép kiểu qua số thực để tính toán. Ví dụ như trên, khi hiển thị ADC Z thì giá trị sẽ dao động xung quanh giá trị -12. có thể là 11-14 nhưng giá trị -12 sẽ hiển thị nhiều nhất. Các bạn thay [a] vào và cho hiển thị giá trị góc khi CB ĐỨNG YÊN. các bạn sẽ thấy tuy không di chuyển cảm biến nhưng giá trị góc vẫn tăng hoặc giảm. giờ bước tinh chỉnh sẽ thuộc về các bạn để có giá trị 12,34 như của mình. mỗi cảm biến ở mỗi địa điểm khác nhau sẽ có 1 giá trị khác nhau nhé. Các bạn có thể thực hiện đọc Gyro lấy mẫu 100 lần khi CB đứng yên rồi chia ra cũng có giá trị này. Các trang nước ngoài thường làm thế. Song mình thấy mỗi lúc mỗi khác nên vẫn áp dụng phương pháp thủ công. @@

+[ b ]: thời gian lấy mẫu. Tuỳ vào giá trị timer của các bạn. Sau khi chống trôi xong thì việc tiếp theo của các bạn là đảm bảo quay đúng góc. Đưa cảm biến lên bề mặt phẳng hoàn toàn, quay 1 góc mà các bạn có thể xác định đc chính xác góc quay, đối chiếu với giá trị góc hiển thị trên LCD. Các bạn tiến hành tinh chỉnh b để có giá trị góc chính xác. Tăng hoặc giảm một lượng rất nhỏ.

+[c]: độ phân giải của cảm biến khi bạn config. [c] chính xác là số bít trên một đơn vị vận tốc. như trên config thì mình chọn khả năng làm việc của cảm biến là 1000 độ/s nên c=32.8. tuy nhiên để đạt độ chính xác cao hơn thì mình chọn c=3.28. là số bít trên 0,1 đơn vị vận tốc. Do đó thay vì ta quay 1 góc 90 độ thì giá trị góc trả về là 900.

[IMG]http://*************/attachments/6765/[/IMG]

Quan trọng nhất ở đây vẫn là việc chống trôi. các bạn chống trôi tốt có thể 3 phút thi đấu chỉ trôi 3 độ là cùng. sai số cho phép được.

Ở file đính kèm của mình có 2 file nén. file GY521mpu_6050.rar là mình lấy từ trang PNLab. file còn lại là mình dùng để làm thư viện chuẩn của mình. viết bằng Codevision

Cách kết nối và gá đặt cảm biến trên Robot:
-Như hình cảm biến thì các bạn chỉ cần quan tâm đến 4 chân đầu tiên của cảm biến là:GND, VCC, SDA, SCL.
+GND và VCC là 2 chân nguồn của cảm biến. áp là 5V. MPU6050 chỉ hoạt động điện áp 3v3. tuy nhiên trên module GY521 đã chuyển đổi điện áp nên mình không phải lo vấn đề này.+SDA và SCL thì các bạn kết nối theo chuẩn I2C như các thiết bị I2C khác.-Gá đặt trên Robot. ở đây mình chỉ dùng trục Z để xác định góc quay do sân năm này ko có mặt phẳng nghiêng. với các sân có góc nghiêng thì các bạn phải tính thêm cả trục X và Y để có độ chính xác. Khi các bạn dùng trục Z thì để cảm biến nằm phẳng sao cho hướng trục Z trên cảm biến hướng lên trên. cố gắng càng hướng lên trên càng tốt để tránh sai số. nên gá ở khung đế robot để hạn chế xe dịch nhất. tránh xa các động cơ, cuộn dây vì các thiết bị này có từ trường và nhiễu lớn lên bus I2C của mình. CB thì không bị nhiễu nhưng Bus truyền rất dễ ảnh hưởng. có thể gá luôn lên trên mạch. Nhưng lúc này thì phải gá mạch cố định hoàn toàn.

Góp ý thêm cho các bạn. Nếu các bạn dùng cho máy bay mô hình thì có thể dùng module GY86. giá bây giờ khá rẻ. năm ngoái mình mua là 1200k. hiện giờ còn 700k ở PNLab và 500k ở thegioiic. Module này tích hợp MPU6050(Gyro, Acc, Nhiệt độ), HMC5883L(Mag hay la bàn số) và MS5611 (CB Áp suất). Khi dùng module này các bạn nên chú ý con la bàn. vì để giao tiếp với nó, các bạn phải giao tiếp với con MPU6050 như mình giải thích ở trên. con MPU6050 sẽ làm master và con HMC5883L làm slaver của con này. Cách giao tiếp các bạn có thể tham khảo code mình dưới đây.

View more random threads:

**zinzin8x** · 07-06-2013, 06:02 PM

Bài viết rất hay... cám ơn bạn đã chia sẻ... nhưng bạn có thể cho thông tin thêm về phần cứng được không ví dụ sensor kết nối với hệ thống điều khiển như thế nào...

**grantevil** · 07-06-2013, 06:29 PM

Cái này nó dùng giao thức I2C thôi anh ạ.

Cái cần tìm hiểu là các cấu trúc mã lệnh gửi xuống nó và cấu trúc câu trả lời lên MCU, và ta có thể lọc lấy thông tin cần thiết!

**suckhoenhakhoa247** · 07-06-2013, 06:40 PM

Topic nghiên cứu khoa học: Dùng Kalman Filter để lọc giá trị đo được của cảm biến để xác định giá trị quay thực tế của xe.

Về Kalman Filter thì có thể hiểu đơn giản thế này. (Problem statement)
1. Nó là 1 thuật toán, dùng để ước lượng giá trị của một đại lượng nào đó (ở đây là góc), mà cảm biến có sai số, cũng như mô hình không đủ chính xác (ở đây là hiểu biết về khả năng quay của cái xe). Cụ thể hơn như sau:
2. Nếu chỉ đơn thuần lấy giá trị góc từ cảm biến thì do nhiễu mà cảm biến không phản ánh đúng góc thực tế cái xe quay được. (bài viết của chủ topic mới giải quyết chỗ này)
3. Nếu chỉ đơn thuần là có thông số về khả năng quay của cái xe, giả thiết như phát lệnh cho hai động cơ trái và phải để xe quay, nhân với thời gian thì có thể biết robot quay được 1 góc nào đấy.

Nếu kết hợp được thông tin từ 2 và 3 thì có thể ước lượng được chính xác hơn về giá trị, phản ánh chính xác về góc quay của robot.

Thân.

**collagenadiva** · 07-06-2013, 06:42 PM

Cái này truyền theo giao thức I2C bạn à. mình đã update cách kết nối và gá đặt. Mình sẽ update cách lấy giá trị sau.

**Dangtinfree** · 07-06-2013, 07:11 PM

À tại vì mình nhìn thấy các ký hiệu trục X và Y trên bo mạch ... nên thắc mắc không biết có sử dụng gì trong lắp ráp không...

**thanhtrang** · 07-06-2013, 07:27 PM

Topic nghiên cứu khoa học: Dùng Kalman Filter để lọc giá trị đo được của cảm biến để xác định giá trị quay thực tế của xe.

Về Kalman Filter thì có thể hiểu đơn giản thế này. (Problem statement)
1. Nó là 1 thuật toán, dùng để ước lượng giá trị của một đại lượng nào đó (ở đây là góc), mà cảm biến có sai số, cũng như mô hình không đủ chính xác (ở đây là hiểu biết về khả năng quay của cái xe). Cụ thể hơn như sau:
2. Nếu chỉ đơn thuần lấy giá trị góc từ cảm biến thì do nhiễu mà cảm biến không phản ánh đúng góc thực tế cái xe quay được. (bài viết của chủ topic mới giải quyết chỗ này)
3. Nếu chỉ đơn thuần là có thông số về khả năng quay của cái xe, giả thiết như phát lệnh cho hai động cơ trái và phải để xe quay, nhân với thời gian thì có thể biết robot quay được 1 góc nào đấy.

Nếu kết hợp được thông tin từ 2 và 3 thì có thể ước lượng được chính xác hơn về giá trị, phản ánh chính xác về góc quay của robot.

Thân.

Mình cũng từng xem qua bộ lọc Kalman. nhưng áp dụng vào Robot mình thấy khó nên không nghiên cứu nữa:
-Khó xác định đúng tốc độ robot. Robot chạy trên sân ảnh hưởng rất lớn đến bề mặt sân. mình không thể tính toán hết sự trượt bánh xe với mặt sân, mỗi lúc mỗi khác chứ không lần nào giống lần nào.
-Mình áp dụng vào Robot mà không cần bọ lọc vẫn chạy ổn định nên mình không có ý định dùng thêm bộ lọc. mỗi trận đấu là 3 phút tuy nhiên con tự động chỉ hoạt động 11-12s nên cũng không ảnh hưởng lắm.

À tại vì mình nhìn thấy các ký hiệu trục X và Y trên bo mạch ... nên thắc mắc không biết có sử dụng gì trong lắp ráp không...

Mình chỉ dùng trục Z nên trục X, Y quay hướng náo cũng được nhưng trục Z buộc phải vuông góc với mặt sân. khi khởi động mình cho giá trị góc=0 thì mình xem như đó là gốc toạ độ luôn.

**thienquoc** · 07-06-2013, 07:47 PM

Gửi bởi tba150990

Mình cũng từng xem qua bộ lọc Kalman. nhưng áp dụng vào Robot mình thấy khó nên không nghiên cứu nữa:
-Khó xác định đúng tốc độ robot. Robot chạy trên sân ảnh hưởng rất lớn đến bề mặt sân. mình không thể tính toán hết sự trượt bánh xe với mặt sân, mỗi lúc mỗi khác chứ không lần nào giống lần nào.
-Mình áp dụng vào Robot mà không cần bọ lọc vẫn chạy ổn định nên mình không có ý định dùng thêm bộ lọc. mỗi trận đấu là 3 phút tuy nhiên con tự động chỉ hoạt động 11-12s nên cũng không ảnh hưởng lắm.

Cái kalman gain (K) là 1 trọng số, để ta nên "tin" vào giá trị cảm biến đo về, hay giá trị mà ta tự suy ra được từ mô hình hóa cái xe.

Do thực tế ở đây là sai số do con cảm biến bé (hay nói đúng ra là cảm biến đủ chính xác), nên ta "tin" vào giá trị của cảm biến là đúng và hợp lý, chứ ai lại "tin" vào cái mô hình cái xe không chính xác, "đèo bòng" thêm có khi lại mang họa (nghĩa là mất chính xác đi nhiều).

Về mặt lý thuyết, nếu kết hợp được cả hai thông tin trên thì có thể giá trị về góc đo được phản ánh chính các giá trị góc của robot.
Thế nên mình mới đề xuất là NCKH (để học)

**blogthanhlap** · 07-06-2013, 07:49 PM

cái này kết hợp PID ổn ổn thí chắc chơi được. hihi.

**thanhpho83** · 07-06-2013, 08:04 PM

Gửi bởi tba150990

cái này kết hợp PID ổn ổn thí chắc chơi được. hihi.

Hì hì, bài toán mình đề xuất với cái trên là khác nhau rồi

Mình mới đề cập đến việc làm sao đo lường chính xác thôi
trong khi PID lại giải quyết bài toán khác, tức là điều khiển chính xác theo yêu cầu.

nghĩa là chỗ mình nói mới chỉ tác động đến phần feedback sensor thôi à.