Bài 11: lập trình bay tự động a → b

Thương hiệu: (Đang cập nhật ...) Loại: (Đang cập nhật ...)

Liên hệ

Mô tả :

Lập trình điều khiển hướng, khoảng cách và quỹ đạo Drone ESP32 Sau khi hoàn thành Bài 9 – Nạp chương trình ESP32 và Bài 10 – Hiệu chỉnh Drone, học viên đã biết cách lập trình ESP32, đọc MPU6050, xử lý Roll/Pitch/Yaw, PID và Motor Mixing. Bài 11 chuyển sang một cấp độ cao hơn: Lập trình để Drone ...

Chi tiết sản phẩm
Thẻ tags

Lập trình điều khiển hướng, khoảng cách và quỹ đạo Drone ESP32

Sau khi hoàn thành Bài 9 – Nạp chương trình ESP32 và Bài 10 – Hiệu chỉnh Drone, học viên đã biết cách lập trình ESP32, đọc MPU6050, xử lý Roll/Pitch/Yaw, PID và Motor Mixing.

Bài 11 chuyển sang một cấp độ cao hơn:

Lập trình để Drone tự động di chuyển từ điểm A đến điểm B dựa trên dữ liệu vị trí và hướng.

1. MỤC TIÊU BÀI HỌC

Sau bài học, học viên có thể:

Hiểu nguyên lý bay tự động A → B.
Hiểu điểm A, điểm B và Waypoint.
Tính khoảng cách giữa Drone và mục tiêu.
Tính hướng đến điểm B.
Tính sai số vị trí.
Tính sai số hướng.
Xây dựng thuật toán Navigation cơ bản.
Kết hợp Navigation với PID.
Thiết lập điều kiện Drone đến điểm B.
Hiểu những giới hạn của hệ thống định vị.

2. TỪ ĐIỀU KHIỂN THỦ CÔNG ĐẾN TỰ ĐỘNG

Ở các bài trước:

Joystick
   ↓
ESP32
   ↓
ESP-NOW
   ↓
Drone
   ↓
PID
   ↓
Motor Mixing
   ↓
Motor

Người điều khiển trực tiếp quyết định Drone đi đâu.

Ở bài này:

Điểm A → Điểm B

Người điều khiển chỉ đưa ra mục tiêu.

Drone tự thực hiện:

Đo vị trí
   ↓
Tính hướng
   ↓
Tính khoảng cách
   ↓
Điều khiển
   ↓
Đo lại
   ↓
Hiệu chỉnh
   ↓
Đến B

Đây là nguyên lý của điều khiển vòng kín.

3. BÀI TOÁN A → B

Giả sử Drone đang ở:

A = (xA, yA)

Điểm cần đến:

B = (xB, yB)

Mục tiêu:

Drone phải xác định:

Đang ở đâu?
Điểm B ở đâu?
B cách bao xa?
B nằm theo hướng nào?
Cần điều khiển Drone như thế nào?

4. XÁC ĐỊNH VỊ TRÍ

Đây là phần quan trọng nhất của hệ thống.

Có thể sử dụng:

Ngoài trời

GPS/GNSS.
IMU.
Compass.
Barometer.

Trong nhà

Optical Flow.
UWB.
Camera.
ToF.
Beacon.

MPU6050 chủ yếu đo gia tốc và tốc độ góc, không phải cảm biến định vị tuyệt đối.

Nếu chỉ sử dụng MPU6050 để tích phân gia tốc trong thời gian dài, sai số vị trí sẽ tích lũy.

5. HỆ TỌA ĐỘ

Trong bài thực hành đơn giản, sử dụng hệ tọa độ X-Y:

             Y+
             ↑
             │
             │
             │
             └────────→ X+

Ví dụ:

Drone  = (2,3)
Target = (8,7)

6. TÍNH KHOẢNG CÁCH

Tính:

dx = xTarget - xDrone
dy = yTarget - yDrone

Ví dụ:

dx = 8 - 2 = 6
dy = 7 - 3 = 4

Khoảng cách:

D = √(dx² + dy²)

Kết quả:

D = √(6² + 4²)
  ≈ 7,21

Nếu đơn vị tọa độ là mét, Drone cách mục tiêu khoảng 7,21 m.

7. TÍNH HƯỚNG ĐẾN B

Có thể sử dụng:

targetHeading = atan2(dy, dx);

Kết quả cho biết hướng từ vị trí hiện tại đến điểm B.

Sau đó so sánh:

Target Heading
      ↓
Current Heading
      ↓
Heading Error

Ví dụ:

Target = 90°
Current = 70°

Error = +20°

Drone cần điều chỉnh Yaw để hướng về mục tiêu.

8. XỬ LÝ SAI SỐ GÓC

Phải lưu ý trường hợp:

Target = 5°
Current = 355°

Không nên tính đơn giản:

5 - 355 = -350°

Vì thực tế Drone chỉ cần xoay khoảng:

+10°

Do đó cần chuẩn hóa sai số góc về khoảng:

-180° → +180°

Ví dụ:

error = targetHeading - currentHeading;

while (error > 180)
    error -= 360;

while (error < -180)
    error += 360;

9. SAI SỐ VỊ TRÍ

Ví dụ:

Target X = 10 m
Current X = 7 m

Ta có:

Error X = 10 - 7
        = 3 m

Tương tự:

Error Y = Target Y - Current Y

Từ sai số này, Navigation Controller tạo ra lệnh chuyển động.

10. NAVIGATION KẾT HỢP PID

Đây là phần quan trọng nhất của bài.

Navigation xác định:

Drone cần đi hướng nào?

PID xác định:

Motor cần thay đổi như thế nào để Drone đạt trạng thái đó?

Mô hình:

Target Position
      ↓
Position Error
      ↓
Navigation
      ↓
Desired Roll/Pitch/Yaw
      ↓
PID
      ↓
Motor Mixing
      ↓
Motor
      ↓
Drone
      ↓
Sensor
      ↓
Feedback

11. ĐIỀU KHIỂN ROLL VÀ PITCH

Ví dụ Drone cần di chuyển theo trục X.

Target X
   ↓
Current X
   ↓
Error X
   ↓
Position Controller
   ↓
Desired Pitch
   ↓
Pitch PID
   ↓
Motor Mixing

Tương tự với trục Y:

Error Y
   ↓
Position Controller
   ↓
Desired Roll
   ↓
Roll PID
   ↓
Motor Mixing

Như vậy Drone có thể biến sai số vị trí thành góc nghiêng cần thiết.

12. ĐIỀU KHIỂN ĐỘ CAO

Drone còn phải kiểm soát trục Z.

Ví dụ:

Target Altitude = 2 m
Current Altitude = 1,6 m

Sai số:

Error Z = 0,4 m

Bộ điều khiển độ cao tạo lệnh Throttle.

Có thể hình dung:

X → Pitch
Y → Roll
Z → Throttle
Yaw → Heading

13. THUẬT TOÁN BAY A → B

Quy trình cơ bản:

START
  ↓
Đọc vị trí Drone
  ↓
Đọc vị trí B
  ↓
Tính dx, dy
  ↓
Tính khoảng cách
  ↓
Đã đến B?
 ┌──────┴──────┐
Có            Không
 ↓               ↓
STOP        Tính hướng
                ↓
          Tính sai số hướng
                ↓
          Điều khiển Yaw
                ↓
        Điều khiển Roll/Pitch
                ↓
                PID
                ↓
          Motor Mixing
                ↓
          Cập nhật vị trí
                ↓
              Lặp lại

14. PSEUDOCODE

Có thể xây dựng thuật toán cơ bản:

readPosition();

dx = targetX - currentX;
dy = targetY - currentY;

distance = sqrt(dx * dx + dy * dy);

if (distance < tolerance) {

    stopDrone();

} else {

    targetHeading = atan2(dy, dx);

    headingError =
        normalizeAngle(targetHeading - currentHeading);

    yawCommand = yawController(headingError);

    movementCommand =
        positionController(dx, dy);

    motorMixing(
        movementCommand,
        yawCommand
    );
}

Đây là mô hình phục vụ giảng dạy. Khi triển khai thực tế cần bổ sung giới hạn tốc độ, failsafe và cơ chế chuyển sang điều khiển thủ công.

15. TẠI SAO PHẢI GIẢM TỐC GẦN ĐIỂM B?

Giả sử Drone còn cách B:

20 m

có thể di chuyển nhanh hơn.

Nhưng khi chỉ còn:

2 m

nếu vẫn giữ tốc độ cao, Drone có thể bay vượt qua mục tiêu.

Do đó:

Khoảng cách lớn
→ tốc độ cao hơn

Khoảng cách giảm
→ giảm tốc

Gần B
→ tiếp cận chậm

Đạt B
→ Hover / Stop

Đây là nguyên tắc quan trọng để giảm hiện tượng overshoot.

16. TẠO VÙNG ĐÍCH

Trong thực tế không nên yêu cầu:

Distance = 0

vì hệ thống định vị luôn có sai số.

Thay vào đó đặt:

Tolerance = 0,5 m

Ví dụ:

Distance < 0,5 m

thì coi như Drone đã đến B.

NAVIGATE
   ↓
Distance < Tolerance
   ↓
ARRIVED
   ↓
HOVER

Giá trị tolerance cần được lựa chọn theo độ chính xác của hệ thống định vị.

17. STATE MACHINE

Để chương trình dễ quản lý, chia thành các trạng thái:

IDLE
 ↓
TAKEOFF
 ↓
NAVIGATE
 ↓
APPROACH
 ↓
ARRIVED
 ↓
HOVER / LAND

IDLE

Chờ nhiệm vụ.

TAKEOFF

Cất cánh đến độ cao đặt trước.

NAVIGATE

Bay về điểm B.

APPROACH

Giảm tốc khi gần B.

ARRIVED

Đã vào vùng mục tiêu.

HOVER

Giữ vị trí.

Hoặc:

LAND

Thực hiện hạ cánh.

18. BAY QUA NHIỀU WAYPOINT

Sau khi A → B hoạt động, có thể mở rộng:

A → B → C → D

Ví dụ:

A ●──────● B
          │
          │
          ● C
          │
          ● D

Chương trình kiểm tra:

Đã đến B?
   ↓ Có
Chuyển C

Đã đến C?
   ↓ Có
Chuyển D

Mỗi điểm được gọi là một Waypoint.

Đây là nền tảng của việc lập trình Mission Flight.

19. MÔ PHỎNG TRƯỚC KHI BAY

Không nên cho Drone bay ngay khi vừa viết thuật toán.

Trước tiên mô phỏng:

A = (0,0)
B = (10,0)

Sau đó giả lập:

(0,0)
(1,0)
(2,0)
(3,0)
...
(10,0)

Khi:

Distance < Tolerance

chuyển:

ARRIVED

Cách này giúp phát hiện lỗi thuật toán trước khi đưa vào Drone thật.

20. BÀI THỰC HÀNH

Thực hành 1

Cho:

A = (0,0)
B = (10,5)

Học viên tính:

dx
dy
Khoảng cách.
Hướng đến B.

Thực hành 2

Cho:

Current = (3,4)
Target = (8,10)

Viết chương trình ESP32 tính:

dx
dy
distance
targetHeading

Hiển thị kết quả bằng Serial Monitor.

Thực hành 3

Tạo 4 waypoint:

A = (0,0)
B = (5,0)
C = (5,5)
D = (0,5)

Lập trình:

A → B → C → D

Khi Drone nằm trong vùng tolerance của waypoint hiện tại, chuyển sang waypoint tiếp theo.

21. CÁC LỖI THƯỜNG GẶP

Drone đi sai hướng

Kiểm tra:

Heading.
Compass.
Hướng cảm biến.
Coordinate system.
Dấu góc.

Drone đi ngược

Kiểm tra:

Roll/Pitch sign.
Motor Mapping.
Motor Mixing.

Drone vượt điểm B

Kiểm tra:

Tốc độ.
Giảm tốc gần B.
PID.
Tolerance.

Drone dao động quanh B

Kiểm tra:

PID.
Độ trễ cảm biến.
Nhiễu vị trí.
Tốc độ điều khiển.

Drone không giữ được vị trí

Kiểm tra:

Cảm biến vị trí.
Sai số định vị.
PID.
Trọng tâm.
Điều kiện môi trường.

22. AUTO VÀ MANUAL

Drone nên có hai chế độ:

MANUAL ↔ AUTO

MANUAL

Người điều khiển trực tiếp.

AUTO

Thuật toán thực hiện nhiệm vụ.

Trong quá trình nghiên cứu, cần có phương án chuyển về MANUAL khi hệ thống tự động hoạt động không đúng.

Đây là một yêu cầu quan trọng khi thiết kế hệ thống điều khiển Drone.

23. CHECKLIST TRƯỚC KHI THỬ BAY

Phần cứng

☐ Motor đúng vị trí
☐ Cánh đúng vị trí
☐ Khung chắc chắn
☐ Pin phù hợp
☐ Dây kết nối chắc chắn

Cảm biến

☐ MPU6050 hoạt động
☐ Calibration hoàn tất
☐ Vị trí được xác định đúng
☐ Heading hợp lý

Phần mềm

☐ Motor Mapping đúng
☐ Sensor Mapping đúng
☐ PID hoạt động
☐ Coordinate system đúng
☐ Waypoint đúng
☐ Tolerance đúng
☐ Có giới hạn tốc độ

An toàn

☐ Khu vực thử nghiệm thông thoáng
☐ Không có người trong vùng nguy hiểm
☐ Có Manual Override
☐ Có cơ chế dừng phù hợp

24. CÂU HỎI KIỂM TRA

Bay tự động A → B khác gì điều khiển bằng joystick?
Waypoint là gì?
Vì sao Drone phải biết vị trí?
Công thức tính khoảng cách giữa hai điểm?
atan2() dùng để làm gì?
Vì sao phải chuẩn hóa sai số góc?
Navigation Controller có nhiệm vụ gì?
PID có nhiệm vụ gì?
Vì sao cần giảm tốc khi gần B?
Tolerance có tác dụng gì?
Vì sao MPU6050 không thể dùng đơn độc để xác định vị trí tuyệt đối lâu dài?
Vì sao cần kiểm tra thuật toán bằng mô phỏng trước khi bay?

25. KẾT LUẬN

Sau Bài 11, học viên đã chuyển từ:

Điều khiển Drone bằng người

sang:

Drone thực hiện nhiệm vụ theo thuật toán.

Chuỗi xử lý quan trọng cần ghi nhớ:

Vị trí
 ↓
Điểm mục tiêu
 ↓
Tính khoảng cách
 ↓
Tính hướng
 ↓
Tính sai số
 ↓
Navigation
 ↓
Roll / Pitch / Yaw / Throttle
 ↓
PID
 ↓
Motor Mixing
 ↓
Motor
 ↓
Drone
 ↓
Cảm biến
 ↓
Feedback

Nguyên lý cốt lõi:

Drone tự động không đơn giản là cho motor chạy trong một khoảng thời gian. Drone phải liên tục đo trạng thái, so sánh với mục tiêu và điều chỉnh lệnh điều khiển dựa trên sai số.