Quy trình thực hiện của bộ lọc Kalman

Phương trình cập nhật thời gian cho bộ lọc Kalman rời rạc:

x^k= A x^k−1 +B uk−1

P−k¿= AP k−1 AT +Q ¿

Phương trình cập nhật giá trị đo lường cho bộ lọc Kalman rời rạc:

K k =P−k¿ H T ¿ ¿¿ x^k=x^k K ( zk−H x^k ) Pk=(1−Kk H ) P− k¿¿ [2.46] [2.47] [2.48] [2.49] [2.50] trạng thái

Nhiệm vụ đầu tiên trong suốt quá trình cập nhật giá trị đo lường là tính tốn độ lợi Kalman

ước lượng sau posteriori bằng cách kết hợp giá trị đo được theo công thức ̂̂

trên. Bước cuối cùng là tính giá trị sai số ước lượng tương quan posteriori vào .ở

Sau mỗi chu trình tính tốn của bộ lọc Kalman, các giá trị được cập nhật theo cặp, tiến trình được lặp lại với ước lượng posteri ori của trạng thái trước dùng để dự đoán ước lượng priori mới. Trạng thái đệ quy tự nhiên là một trong những điểm đặc trưng của bộ lọc Kalman, nó thay thế điều kiện đệ quy ước lượng hiện tại cho giá trị đã qua.

Trong điều kiện thực hiện thực tế của bộ lọc, giá trị nhiễễ̃u tương quan R thường được dùng làm giá trị ưu tiên để tính tốn cho bộ lọc. Trên thực tế, việc đo các giá trị ma trận R là rất phổ biến bởi vì chúng ta có thể đo quy trình theo nhiều cách vì vậy mà thường lấy mẫu giá trị để đưa ra khuynh hướng thay đổi của giá trị nhiễễ̃u.

Sự xác định rõ tương quan nhiễễ̃u q trình Q thường rất khó bởi vì điều điển hình là chúng ta khơng có khả năng quan sát trực tiếp tiến trình mà chúng ta đang ước lượng. Đơi khi sự liên hệ tới những quy trình mẫu đơn giản có thể đưa ra những giá trị chấp nhận được nếu một mẫu xen vào khơng chắc chắn đủ với tiến trình thơng qua sự lựa chọn Q . Chắc chắn trong trường hợp này,mẫu đó sẽ hi vọng rằng giá trị tiến trình là đáng tin cậy.

Trong những trường hợp khác, dù muốn hay khơng chúng ta đều có cái chuẩn hợp lí cho việc lựa các thơngsố, thường thì chất lượng bộ lọc sẽ tốt hơn nhiều lần khi có chứa sự hiệu chỉnh các tham số Q và R.Sự hiệu chỉnh thường được thực hiện gián tiếp, thường thì với sự giúp đỡ của một bộ lọc Kalman khác trong quy trình chung, liên hệ như một hệ thống đồng nhất.

Với điều kiện Q và R là các hằng số thực,cả hai cho phép ước lượng sai số tương quan và độ lợi Kalman sẽ ổn định nhanh chóng và sau đó trở thành hằng số.

Trong điều kiện luận văn, thông số Q và R được hiệu chỉnh dựa vào quá trình thử sai để dự đoán khuynh hướng hiệu chỉnh của hệ thống và tìm ra bộ thơng số phù hợp nhất.

Hình 7: Tổng quan chu trình thực hiện bộ lọc Kalman hồn chỉnh.

3. Giải thuật điều khiển3. 1 Bộ điều khiển PID. 3. 1 Bộ điều khiển PID.

Bộ điều khiển PID với thông số K p, K I ,K Dcố định như trên, hệ thống chỉ làm việc tốt trong điều kiện hệ số K p, K I ,K Dđã được chỉnh định tối ưu và trong quá trình làm việc, các thơng số trong mơ hình khơng đổi.

Hình 8: Sơ đồ hệ thống điều khiển sử dụng PID.

Giải thuật tính tốn bộ điều khiển PID được cho bởi cơng thức:

u (t )=K p e (t )+ KI∫ e (t )+ K D

Trong đó các hệ số K p, K I ,K Dlần lượt là các thông số đặc trưng của các khâu tỷ lệ, tích phân, vi phân của bộ điều khiển PID. Giá trị K ptác động lên sai số hiện tại, giá trị K I tác động lên sai số trong quá khứ, trong khi đó giá trị K D ảnh hưởng lên tốc độ thay đổi của sai số. Ảnh hưởng của ba thông số K p, K I ,K Dđến chất lượng của hệ thống được tổng hợp trong Hình 9: Ảnh hưởng của các thơng số bộ điều khiển PID đến hệ thống Thông số Thời gian lên ts Thời gian quá độ tqd Độ vọt lố Sai số xác lập K P

Giảm Thay đổi nhẹ Tăng Giảm K I Giảm Tăng Tăng Khử K D Thay đổi nhẹ Giảm Giảm Không thay đổi.

Mặc dù bộ điều khiển PID có thể áp dụng rộng rãi cho nhiều vấn đề điều khiển ngay cả khi chúng ta khơng nắm được mơ hình tốn mơ tả hệ thống. Tuy nhiên, khó khăn cơ bản của bộ điều khiển PID là nó là một hệ thống điều khiển phản hồi với các thơng số khơng đổi do đó nó sẽ nhạy cảm với nhiễễ̃u và thường khơng đáp ứng được bài tốn tối ưu. Để hệ thống hoạt động ổn định ở mức chấp nhận được, việc xác định các hệ số K p, K I ,K Dcủa bộ điều khiển PID là cần thiết và không phải là một bài tốn dễễ̃ dàng.

Thơng số

K p

K I K D

Hình 9: Ảnh hưởng của các thơng số bộ điều khiển PID đến hệ thống.

3.2 Cấu trúc bộ điều khiển PID cho robot hai bánh tự cân bằng

Ba bộ PID được sử dụng để điều khiển robot hai bánh tự cân bằng, bao gồm: - Bộ PID điều khiển góc nghiêng (ψ ¿

- Bộ PID điều khiển vị trí (θ) - Bộ PID điều khiển góc xoay (ϕ )

Hình 10: Cấu trúc bộ điều khiển PID cho hệ robot hai bánh tự cân bằng.

Hàm truyền đạt bộ điều khiển PID liên tục:

G PID (S )=UE ((

SS)) =K P + K

sI + KD (1+sτS )

Rời rạc hóa đạo hàm theo thời gian:

f (k )−f (k−1) y (k )≈ T s

[2.51]

[2.52]

Rời rạc hóa tích phân theo thời gian:

TIEU LUAN MOI download : skknchat123@gmail.com moi nhat

∫ y (t ) dt= y ( k )≈ y (k −1)+ 0

Phép biến đổi rời rạc (z-Tranform)

∞ X [ z ]=Z {x [k ]}=∑ x [k ] z−k k=0 z= Ae jθ= A (cos θ+ J sin θ) Ta có Z {x [ k−n ]}=z−n X [z ] và Z {x [k ]}=X [z ] Do đó: ( ) = y k Và y (k )= y (k −1) U [ z] =K E [ z] p ⇔ U [z ] = E [ z ] ⇔ U [z ] = E [ z ] ⇔ U [z ] = E [ z ] ⇔ U [z ]=z−1 U [z ]+ aE [ z ]+b z−1 E [z ]+c z−2 E [z ]

⇔ u [k ]=u [k−1 ]+ae [ k ]+ be [k−1 ]+ce [k −2]

Trong đó: a=K p + Ki

[2.53] [2.54] [2.55] [2.56] [2.57] [2.58] [2.59] [2.60] [2.61] [2.62]

4. Các thành phần chính của mơ hình

4.1. Mạch Điều Khiển Động Cơ Bước A4988

THƠNG SỐ KỸ THUẬT

Cơng suất ngõ ra lên tới 35V, dịng đỉnh 2A.

Có 5 chế : Full bc, ẵ bc, ẳ bc, 1/8 bc, 1/16 bước

Điểu chỉnh dòng ra bằng triết áp, nằm bên trênCurrent Limit = VREF × 2.5

Tự động ngắt điện khi q nhiệt.

Trình điều khiển cung cấp năm độ phân giải bước khác nhau: bước đầy đủ, bước cao hơn, bước một phần tư, bước tám và bước thứ mười sáu. Ngồi ra, nó có một chiết áp để điều chỉnh đầu ra hiện tại, ngắt nhiệt q nhiệt và bảo vệ dịng điện chéo.

Hình 12: Bảng cấu hình chân tạo độ phân giải bước ra module A4988.

Điện áp logic của nó là từ 3 đến 5,5 V và dòng điện tối đa trên mỗi pha là 2A nếu được cung cấp làm mát bổ sung tốt hoặc dịng điện liên tục 1A trên mỗi pha mà khơng cần tản nhiệt hoặc làm mát.

CÁCH SỬ DỤNG

Lựa chọn chế full hay ẵ hay ẳ s c thụng qua 3 pin MS1 MS2 MS3. Mình thường nối thẳng 3 pin này với công tắc bit 3p để dễễ̃ thiết lập từ trên phần cứng. Lưu ý là nếu thả nổi 3 pin này tức là mode full step.

Bật tắt động cơ thì thơng qua pin ENABLE, mức LOW là bật module, mức HIGH là tắt module. Điều khiển chiều quay của động cơ thông qua pin DIR. Điều khiển bước của động cơ thông qua pin STEP, mỗi xung là tương ứng với 1 bước (hoặc vi bước)

Hai chân Sleep với Reset luôn nối với nhau.

Hình 13: Sơ đồ chân của trình điều khiển A4988.

Bây giờ chúng ta hãy quan sát sơ đồ chân của trình điều khiển và kết nối nó với động cơ bước và bộ điều khiển. Vì vậy, chúng tơi sẽ bắt đầu với 2 chân ở phía bên phải của nút để cấp nguồn cho trình điều khiển, chân VDD và Ground mà chúng tôi cần kết nối chúng với nguồn điện từ 3 đến 5,5 V và trong trường hợp của chúng tơi, đó sẽ là bộ điều khiển của chúng tơi, Arduino Bo mạch cung cấp 5 V. 4 chân sau dùng để

kết nối động cơ. Các chân 1A và 1B sẽ được kết nối với một cuộn dây của động cơ và chân 2A và 2B với cuộn dây khác của động cơ. Để cấp nguồn cho động cơ, chúng ta sử dụng 2 chân tiếp theo, Ground và VMOT, chúng ta cần kết nối chúng với Nguồn điện từ 8 đến 35 V và chúng ta cũng cần sử dụng tụ tách có ít nhất 47 µF để bảo vệ bo mạch điều khiển khỏi tăng điện áp.

Hai chân tiếp theo, Step và Direction là những chân mà chúng ta thực sự sử dụng để điều khiển chuyển động của động cơ. Chân Direction điều khiển hướng quay của động cơ và chúng tơi cần kết nối nó với một trong các chân kỹ thuật số trên bộ vi điều khiển của chúng tôi, hoặc trong trường hợp của chúng tơi, tơi sẽ kết nối nó với chân số 4 của Bảng Arduino của tôi.

Với chân Step, chúng tôi điều khiển các bước của động cơ và với mỗi xung được gửi đến chân này, động cơ sẽ di chuyển một bước. Vì vậy, điều đó có nghĩa là chúng ta khơng cần bất kỳ lập trình phức tạp nào, bảng thứ tự pha, đường điều khiển tần số, v.v., bởi vì trình biên dịch tích hợp của Trình điều khiển A4988 sẽ đảm nhiệm mọi thứ. Ở đây chúng ta cũng cần đề cập rằng 2 chân này không được kéo đến bất kỳ điện áp nào bên trong, vì vậy chúng ta khơng nên để chúng trơi nổi trong chương trình của chúng ta.

Tiếp theo là Pin SLEEP và mức logic thấp đặt bo mạch ở chế độ nghỉ để giảm thiểu tiêu thụ điện năng khi động cơ không được sử dụng.

Tiếp theo, chân ĐẶT LẠI đặt bộ dịch sang trạng thái Trang chủ được xác định trước. Trạng thái nhà hoặc Vị trí microstep nhà này có thể được nhìn thấy từ các Hình này từ Biểu dữ liệu A4988. Vì vậy, đây là những vị trí ban đầu từ nơi động cơ khởi động và chúng khác nhau tùy thuộc vào độ phân giải microstep. Nếu trạng thái đầu vào cho chân này là mức logic thấp thì tất cả các đầu vào STEP sẽ bị bỏ qua. Chân Reset là một chân nổi nên nếu chúng ta khơng có ý định điều khiển nó trong chương trình của mình, chúng ta cần kết nối nó với chân SLEEP để đưa nó lên cao và kích hoạt bo mạch.

4.2. Vi điều khiển STM32F103C8T6.

Hình 14: Kit phát triển STM32F103C8T6 Blue Pill ARM Cortex-M3.

- STM32F103C8T6 là vi điều khiển 32bit, thuộc họ F1 của dòng chip STM32 hãng ST.

- Lõi ARM COTEX M3.

- Tốc độ tối đa 72Mhz. - Bộ nhớ :

64 kbytes bộ nhớ Flash 20 kbytes SRAM

-Clock, reset và quản lý nguồn Điện áp hoạt động từ 2.0 → 3.6V.

Sử dụng thạch anh ngoài từ 4Mhz → 20Mhz.

Thạch anh nội dùng dao động RC ở mode 8Mhz hoặc 40Khz. - Chế độ điện áp thấp:

Có các mode: ngủ, ngừng hoạt động hoặc hoạt động ở chế độ chờ.

Cấp nguồn ở chân Vbat bằng pin ngoài để dùng bộ RTC và sử dụng dữ liệu được lưu trữ khi mất nguồn cấp chính.

- 2 bộ ADC 12 bit với 9 kênh cho mỗi bộ Khoảng giá trị chuyển đổi từ 0 – 3.6 V Có chế độ lấy mẫu 1 kênh hoặc nhiều kênh. - DMA:

7 kênh DMA

Có hỗ trợ DMA cho ADC, UART, I2C, SPI. - 7 bộ Timer:

3 Timer 16 bit hỗ trợ các mode Input Capture/ Output Compare/ PWM.

1 Timer 16 bit hỗ trợ để điều khiển động cơ với các mode bảo vệ ngắt Input, dead- time.

2 Watchdog Timer để bảo vệ và kiểm tra lỗi.

1 Systick Timer 24 bit đếm xuống cho hàm Delay,…. - Có hỗ trợ 9 kênh giao tiếp:

2 bộ I2C. 3 bộ USART 2 SPI

1 CAN

USB 2.0 full-speed interface

- Kiểm tra lỗi CRC và 96-bit ID.

Các thông số kĩ thuật:

Điện áp cấp 5VDC qua cổng Micro USB sẽ được chuyển đổi thành 3.3VDC qua IC nguồn và cấp cho Vi điều khiển chính.

Tích hợp sẵn thạch anh 8Mhz.

Tích hợp sẵn thạnh anh 32Khz cho các ứng dụng RTC.

Ra chân đầy đủ tất cả các GPIO và giao tiếp: CAN, I2C, SPI, UART, USB,... Tích hợp Led trạng thái nguồn, Led PC13, Nút Reset.

Kích thước: 53.34 x 15.24mm.

4.3. Kit ESP32 DOIT DevKit V1

Hình 15: Board kit phats triển ESP32 DOIT DevKit V1.

Thông số kỹ thuật:

ESP32 là lõi kép, điều này có nghĩa là nó có 2 bộ vi xử lý. Nó được tích hợp Wi-Fi và bluetooth.

Nó chạy các chương trình 32 bit.

Xung nhịp (clock frequency) có thể lên đến 240MHz và nó có RAM 512 kB. Loại board này có 30 hoặc 36 chân, mỗi hàng có 15 chân.

Nó cũng có sẵn nhiều loại ngoại vi (peripheral), như: cảm ứng điện dung (capacitive touch), ADC, DAC, UART, SPI, I2C và nhiều hơn nữa.

Nó được tích hợp với cảm biến hiệu ứng Hall (Hall effect sensor) và cảm biến nhiệt độ.

Mơi trường lập trình

ESP32 có thể được lập trình trên nhiều mơi trường lập trình khác nhau. Bạn có thể sử dụng:

Arduino IDE

Espressif IDF (IoT Development Framework) MicroPython

JavaScript LUA v.v.

Trong những dự án này, chúng ta sẽ lập trình ESP32 chủ yếu bằng Arduino IDE hoặc MicroPython. 4.4. Bánh xe Thông số kỹ thuật: - Chất liệu: Nhựa cứng, lớp đệm mút, cao su tốt. - Đường kính: 65mm. - Độ rộng bánh: 27mm. 28

Hình 16: Bánh xe 65mm khớp lục giác.4.5. Mạch Giảm Áp DC XL4015 (5A) 4.5. Mạch Giảm Áp DC XL4015 (5A) Hình 17: Mạch giảm áp DC XL4015 (5A). Thơng số mạch giảm áp DC XL4015 (5A): - IC chính: XL4015.

- Tích hợp led báo điện áp đầu ra và nhôm tản nhiệt cho IC chính.

- Điện áp đầu vào: 8~36VDC

4.6. Mạch Giảm Áp DC LM2596 3A.

- Điện áp đầu ra: 1.25 ~ 32VDC. - Dòng đầu ra tối đa: 5A

- Hiệu suất : 96% - Tần số xung: 180KHz - Tích hợp Mosfet đóng ngắt tần số cao.

- Maximum Duty Cycle: 100% - Minimum Drop Out: 0.3VDC - Nhiệt độ làm việc : -40 ~ 125 độ C

TIEU LUAN MOI download : skknchat123@gmail.com moi nhat

Hình 18: Mạch giảm áp DCLM2596 3A. LM2596 3A.

4.7. Pin cell 18650 2000mAh.

Thông số kỹ thuật:

- Điện áp đầu vào: Từ 3V đến 30V.

- Điện áp đầu ra: Điều chỉnh được trong khoảng 1.5V đến 30V.

- Dòng đáp ứng tối đa là 3A.

- Hiệu suất: 92% - Cơng suất: 15W - Kích thước: 45 (dài) * 20 (rộng) * 14 (cao) mm Thông số kỹ thuật: - Điện áp: 3.7v

- Dung lượng: 2000mah

- Điện áp sạc đầy: 4.2v

Hình 19: Pin cell 18650 2000mAh.4.8. Trụ đồng đực cái 20mm. 4.8. Trụ đồng đực cái 20mm.

Hình 20: Trụ đồng đực cái 20mm.

4.9. Hộp đế pin 18650 4 cell.

Thông số kỹ thuật: