Trong đó:
+ l là khoảng cách giữa hai bánh xe (m).
+ I là tâm ảo của đường tròn khi robot được chạy theo quỹ đạo cong. + R là khoảng cách từ tâm ảo đến trọng tâm của robot.
Khi đó vận tốc của từng động cơ được tính theo cơng thức sau:
= + ×
{
= − ×
Vi điều khiển nhận và xử lý tốc độ nhận được rồi xuất xung PWM để điều khiển hai động cơ thông qua cầu H, kết hợp với thuật toán PID chạy theo vận tốc đã đặt.
3.1.2.4.2. IMU
Node Imu thực hiện nhận giá trị từ module IMU MPU6050 thông qua Jetson Nano. Những giá trị này sẽ được tách và xử lí thành những giá trị Roll Pitch Yaw tương ứng. Sau đó sẽ được truyền sang Node Odometry để ước lượng hướng quay của robot.
3.1.2.4.3. Odometry
Node Odometry thực hiện ước lượng vị trí của robot (x, y, θ) từ thơng tin của encoder và IMU. Vị trí của robot được biểu diễn qua bộ ba thông số k = {x, y, θ}, trong đó x, y là toạ độ trong hệ toạ độ Decade và θ là hướng của robot so với trục x. Quỹ đạo của robot sau thời gian t là k0:t = x0, x1, x2,… xt. Giả sử robot có vị trí đầu (x0, y0, θ0). Vị trí mới của robot sau một khoảng thời gian t di chuyển được một đoạn d và xoay góc θ như trên hình 3.9 được tính theo cơng thức:
′
′
Trong đó, x, y và θ là khoảng cách di chuyển và góc xoay của robot trong khoảng Δt, được tính theo cơng thức:
= ⋅ cos( ) { = − ⋅ sin( )
= ⋅Δ
Tốc độ góc có thể được tính từ encoder rồi quy ra góc quay của robot. Tuy nhiên trong q trình thử nghiệm, việc tính tốc độ góc từ encoder có sai số lớn, gây ảnh hưởng bài tốn xác định vị trí. Vì vậy, IMU được dùng để đọc trực tiếp giá trị góc quay của robot. Khoảng cách di chuyển d của robot được tính từ khoảng cách di chuyển độc lập của hai bánh xe trái và phải thông qua giá trị đọc về của encoder theo công thức:
=+ ℎ
right
{
Trong đó:
Δencoder_left, Δencoder_right là số xung encoder đọc được trong khoảng thời gian Δt (xung). dleft, dright là khoảng cách hai bánh xe trái và phải đi được (m). tick_meter là số xung encoder khi bánh xe đi được một mét (ppm)