Công thức tính toán Odometry

A. Pose (Tư thế) - các đại lượng trong odometry

Pose, là vị trí của một vật thể (ví dụ như robot), thường trong không gian hai chiều (2D), vì chuyển động robot thường bị giới hạn trên một mặt phẳng. 

B. Pose updating - Cập nhật vị trí robot

C. Công thức tính toán sự thay đổi vị trí

Để xác định vị trí hiện tại của robot và cập nhật pose, sự thay đổi cần được tính bằng dữ liệu đọc từ cảm biến. Với robot, sẽ có ba cảm biến có thể dùng: hai song song với thân robot theo hướng x và một vuông góc với hướng y (vuông góc với bánh truyền động). 

Góc và dịch chuyển

D. Cập nhật thay đổi vị trí theo robot trên các giá trị phía trên

Tạo delta tư thế (pose deltas) theo tương đối so với robot, rồi biến đổi sang hệ toạ độ toàn cục (field-relative) bằng ma trận xoay (rotation matrix) sử dụng heading ban đầu. Công thức:

Lưu ý: Phương pháp này gọi là Euler integration, nhưng ta dùng để tính sự thay đổi đại lượng thay vì tích phân vận tốc; đây chỉ là xấp xỉ đơn giản của lý thuyết gốc.

Cảnh báo: Đây là dành cho lập trình viên nâng cao; tự triển khai từ đầu là bài học hay, nhưng không chắc là tối ưu để có autonomous chính xác nhất. Có nhiều resources đã được phát triển tốt, kiểm thử kỹ, dễ dùng odometry.

E. Odometry Pseudocode  (code ví dụ)

while robot_is_active():
   delta_left_encoder_pos = left_encoder_pos - prev_left_encoder_pos
   delta_right_encoder_pos = right_encoder_pos - prev_right_encoder_pos
   delta_center_encoder_pos = center_encoder_pos - prev_center_encoder_pos //quãng đường di chuyển của bánh xe

   phi = (delta_left_encoder_pos - delta_right_encoder_pos) / trackwidth //góc quay robot
   delta_middle_pos = (delta_left_encoder_pos + delta_right_encoder_pos) / 2
   delta_perp_pos = delta_center_encoder_pos - forward_offset * phi

   delta_x = delta_middle_pos * cos(heading) - delta_perp_pos * sin(heading)
   delta_y = delta_middle_pos * sin(heading) + delta_perp_pos * cos(heading) //sự thay đổi theo 2 trục x y

   x_pos += delta_x
   y_pos += delta_y 
   heading += phi //cập nhật các đại lương

   prev_left_encoder_pos = left_encoder_pos
   prev_right_encoder_pos

while robot_is_active():
   delta_left_encoder_pos = left_encoder_pos - prev_left_encoder_pos
   delta_right_encoder_pos = right_encoder_pos - prev_right_encoder_pos
   delta_center_encoder_pos = center_encoder_pos - prev_center_encoder_pos //quãng đường di chuyển của bánh xe

   phi = (delta_left_encoder_pos - delta_right_encoder_pos) / trackwidth //góc quay robot
   delta_middle_pos = (delta_left_encoder_pos + delta_right_encoder_pos) / 2
   delta_perp_pos = delta_center_encoder_pos - forward_offset * phi

   delta_x = delta_middle_pos * cos(heading) - delta_perp_pos * sin(heading)
   delta_y = delta_middle_pos * sin(heading) + delta_perp_pos * cos(heading) //sự thay đổi theo 2 trục x y

   x_pos += delta_x
   y_pos += delta_y 
   heading += phi //cập nhật các đại lương

   prev_left_encoder_pos = left_encoder_pos
   prev_right_encoder_pos