Mindstorm NXT dựa trên PID:
Nhƣ đã trình bày trong phần 2.2, tìm đƣờng tĩnh toàn cục là tìm ra một đƣờng đi chắc chắn, một danh sách tập các điểm tƣơng ứng với đƣờng đi để xe tự hành có thể di chuyển. Trong phần này sẽ trình bày một phƣơng pháp đặc trƣng của đƣờng đi điểm tới điểm (point-to-point) mà xe tự hành có thể sử dụng để di chuyển chính xác theo đƣờng đi mong muốn.
Hình 5.6 – Đường đi mong muốn và tập các đường đi của robot [14]
là một đƣờng đi đƣợc xác định bởi một chuỗi N lộ trình đƣờng đi mong muốn và tiểu đƣờng đi Pi kết nối tập các điểm từ wi đến wi+1
đƣợc mô tả trong Hình 5.6 trên. Cho ni là đơn vị vector để theo dõi lỗi. Khi đó, vị trí hiện tại đƣợc cho xt, hàm theo dõi lỗi là:
et = xttan(µt)ni (1)
Bộ điều khiển đƣợc hiện thực dựa trên vòng lặp đóng sử dụng điều khiển P trong bộ điều khiển PID:
ut = Kpet (2)
Vị trí hiện tại xt của xe tự hành có thể đƣợc xác định bởi một camera trong môi trƣờng phòng thí nghiệm hoặc bằng tín hiệu GPS trong một môi trƣờng ngoài trời. Và Kp đƣợc xác định bằng phƣơng pháp điều chỉnh Ziegler- Nichols (xem Bảng 5.2), một phƣơng pháp hƣớng dẫn tìm kiếm tự điều chỉnh của bộ điều khiển PID [5].
Bảng 5.2 - PID theo phương pháp điều chỉnh Ziegler–Nichols [14]
Control type Kp Ki Kd
P Ku/2 - -
PI Ku/2.2 1.2Kp/Tu -
Classic PID 0.60Ku 2Kp/Tu KpTu/8
Thiết lập giá Ki và giá trị Kd bằng zero và tạo hành động điều khiển giống nhƣ một bộ điều khiển P đơn giản và điều chỉnh Kp cho đến khi nó đạt đến một lợi ích ổn định Ku mà tại đầu ra của các dao động vòng lặp điều khiển với biên độ không đổi. Và thời gian dao động Tu đƣợc sử dụng để thiết lập P, I, và D đạt đƣợc ổn định, tùy thuộc vào kiểu của bộ điều khiển đƣợc sử dụng.
wi w(i+1)
αi α(i+1)
µt
xt
Cuối tại một đoạn trong tập các đƣờng đi, quá trình di chuyển từ một đoạn trong tập các đƣờng đi i đến i + 1 hoàn thành và vận tốc hiện tại giảm xuống khoảng bằng zero. Nghĩa là, các xe tự hành phải dừng tại tập các điểm để quay chính xác đến một góc và để có thể tiếp tục di chuyển trên một đoạn đƣờng trong tập các đƣờng đi tiếp theo. Nhƣ trong Hình 5.6, các xe tự hành sẽ quay một góc độ αi về bên trái tại tập các điểm wi và một góc độ αi+1 về bên phải tại waypoint wi+1. Đoạn chƣơng trình sau hiện thực thao tác quay của xe.
turn = algorithmPID(times); if (stopFlag)
break; else
{
leftMotorSpeed = normalMotorSpeed - turn; rightMotorSpeed = normalMotorSpeed + turn; }
_drivePort.SetDrivePower(leftMotorSpeed, rightMotorSpeed);