Phương pháp lập trình Robot Python: Hành vi đi đến mục tiêu

Mục đích tối cao trong sự tồn tại của robot nhỏ của chúng ta trong hướng dẫn lập trình này là đạt được mục tiêu. Vì vậy, làm thế nào để làm cho các bánh xe quay để đến đó? Hãy bắt đầu bằng cách đơn giản hóa thế giới quan của chúng ta một chút và cho rằng không có chướng ngại vật nào cản đường.

Điều này sau đó trở thành một nhiệm vụ đơn giản và có thể dễ dàng lập trình bằng Python. Nếu chúng ta tiến về phía trước trong khi đối mặt với mục tiêu, chúng ta sẽ đạt được điều đó. Nhờ đo mùi của chúng tôi, chúng tôi biết tọa độ và hướng hiện tại của chúng tôi là gì. Chúng tôi cũng biết tọa độ của mục tiêu là gì vì chúng đã được lập trình trước. Do đó, sử dụng một chút đại số tuyến tính, chúng ta có thể xác định vectơ từ vị trí của chúng ta đến mục tiêu, như trong go_to_goal_controller.py:

# return a go-to-goal heading vector in the robot's reference frame
def calculate_gtg_heading_vector( self ):
  # get the inverse of the robot's pose
  robot_inv_pos, robot_inv_theta = self.supervisor.estimated_pose().inverse().vector_unpack()
  
  # calculate the goal vector in the robot's reference frame
  goal = self.supervisor.goal()
  goal = linalg.rotate_and_translate_vector( goal, robot_inv_theta, robot_inv_pos )
  
  return goal

Lưu ý rằng chúng ta đang nhận vectơ đến mục tiêu trong hệ quy chiếu của rô bốt , chứ KHÔNG phải trong tọa độ thế giới. Nếu mục tiêu nằm trên trục X trong hệ quy chiếu của rô bốt, điều đó có nghĩa là mục tiêu nằm ngay trước rô bốt. Do đó, góc của vectơ này so với trục X là sự khác biệt giữa tiêu đề của chúng ta và tiêu đề mà chúng ta muốn ở trên. Nói cách khác, đó là sai số giữa trạng thái hiện tại của chúng ta và những gì chúng ta muốn trạng thái hiện tại của mình. Do đó, chúng tôi muốn điều chỉnh tốc độ quay ω để góc giữa tiêu đề và mục tiêu sẽ thay đổi thành 0. Chúng tôi muốn giảm thiểu sai số:

# calculate the error terms
theta_d = atan2( self.gtg_heading_vector[1], self.gtg_heading_vector[0] )

# calculate angular velocity
omega = self.kP * theta_d

self.kPtrong đoạn mã trên của việc triển khai Python bộ điều khiển là một mức tăng kiểm soát. Nó là một hệ số xác định tốc độ chúng ta quay tương ứng với mức độ xa mục tiêu mà chúng ta đang đối mặt. Nếu lỗi trong tiêu đề của chúng tôi là 0, thì tỷ lệ quay vòng cũng là 0. Trong hàm Python thực bên trong tệp go_to_goal_controller.py, bạn sẽ thấy nhiều lợi ích tương tự hơn, vì chúng tôi đã sử dụng bộ điều khiển PID thay vì hệ số tỷ lệ đơn giản.

Bây giờ chúng ta có vận tốc góc ω , làm thế nào để xác định vận tốc chuyển động v ? Một nguyên tắc chung mà bạn có thể biết theo bản năng: Nếu chúng ta không rẽ, chúng ta có thể đi tiếp với tốc độ tối đa, và khi rẽ càng nhanh, chúng ta càng nên giảm tốc độ. Điều này thường giúp chúng tôi giữ cho hệ thống của mình ổn định và hoạt động trong giới hạn của mô hình của chúng tôi. Do đó, v là một hàm của ω . Trong go_to_goal_controller.pyphương trình là:

# calculate translational velocity
# velocity is v_max when omega is 0,
# drops rapidly to zero as |omega| rises
v = self.supervisor.v_max() / ( abs( omega ) + 1 )**0.5

Một gợi ý để xây dựng công thức này là chúng ta thường giảm tốc độ khi ở gần mục tiêu để đạt được nó với tốc độ bằng không. Công thức này sẽ thay đổi như thế nào? Nó phải bao gồm bằng cách nào đó thay thế v_max()bằng một thứ gì đó tỷ lệ với khoảng cách. OK, chúng ta gần như đã hoàn thành một vòng điều khiển duy nhất. Việc duy nhất cần làm là chuyển hai thông số của mô hình xe đạp một bánh này thành tốc độ bánh xe vi sai và gửi tín hiệu đến các bánh xe. Dưới đây là một ví dụ về quỹ đạo của rô-bốt dưới bộ điều khiển hướng tới mục tiêu, không có chướng ngại vật:

Đây là một ví dụ về quỹ đạo của robot được lập trình.

Như chúng ta có thể thấy, vectơ đến mục tiêu là một tham chiếu hiệu quả để chúng tôi dựa trên các tính toán kiểm soát của mình. Nó là một đại diện nội bộ của "nơi chúng tôi muốn đến." Như chúng ta sẽ thấy, sự khác biệt chính duy nhất giữa việc đi đến mục tiêu và các hành vi khác là đôi khi việc hướng tới mục tiêu là một ý tưởng tồi, vì vậy chúng ta phải tính toán một vectơ tham chiếu khác.