F1 Local Navigation + VFF

This is the blog of the P3-RM, where I programmed the behavior of a F1 with VFF.

Objetive

The objective of the practice was to program the behavior of an F1 car using VFF (Vector Field Following) to follow targets placed on the map while avoiding obstacles on the road, in this case, other cars.

Coding

Libraries

• This library provides access to mathematical functions and constants.

import math

Implementation

Handling of the targets

We obtain the target, but when the car is close and since the distance to the target is unlikely to be exactly 0, it changes the target when it is relatively close.

dx = target_global_pose.x - robot_pose.x
dy = target_global_pose.y - robot_pose.y
distance = math.sqrt(dx**2 + dy**2)

if distance < TARGET_THRESHOLD:
      target.setReached(True)
      target = GUI.getNextTarget()
      target_global_pose = target.getPose()

Attractive force

The attractive force is calculated using the current position of the car and the position of the next target, so that the greater the distance, the more force is applied.

distance = math.sqrt(x_rel**2 + y_rel**2)
magnitude = max(MIN_FORCE, min(distance, MAX_FORCE))

a_force_x = magnitude * (x_rel / distance)
a_force_y = magnitude * (y_rel / distance)

Repulsive force

To create the repulsive force, we will use a laser. Depending on the position and distance of the detected object, there is a priority order:

• Objects VERY close to the car
• Objects in front of the car
• Objects on the sides

laser_data = HAL.getLaserData()
values = laser_data.values

for i in range(70, 110):
      # Here we store the min_distance

if min_distance >= 8.0:
      for i in range(len(values)):
      # Here we store the min_distance

if any(value < 0.7 for value in values):
      # Here we store the angle corresponding to that distance.

angle = math.radians(min_index - 90)

raw_repulsion_magnitude = -RF_BASE / max(min_distance, 0.1)
repulsion_magnitude = max(-MAX_FORCE, min(raw_repulsion_magnitude, 0))

r_force_x = repulsion_magnitude * math.cos(angle)
r_force_y = repulsion_magnitude * math.sin(angle)

Combined force

The total force that we will later use to derive the velocities is a weighted sum of the attractive and repulsive forces.

Ka = 2
Kr = 3

combined_force = [Ka * attractive_force[0] + Kr * repulsion_force[0], 
                  Ka * attractive_force[1] + Kr * repulsion_force[1]]

Transform the forces into velocities.

Once we have calculated the total force, we can derive the linear and angular velocities such that the linear velocity is the magnitude of the force and the angular velocity is calculated as atan(F_y / F_x).

linear_vel = math.sqrt(combined_force[0]**2 + combined_force[1]**2)
linear_vel = max(MIN_VEL, linear_vel)

angular_vel = math.atan2(combined_force[1], combined_force[0])

HAL.setV(linear_vel)
HAL.setW(angular_vel)

Difficulties

• At one point, the repulsive force acted like an attractive force, and I had to remove several things and step back a bit to fix that issue.
• Modify the constants Ka and Kr to optimize the movement.

Logbook

• Day 1 (20/10/24): Implementation of tracking and target switching.
• Day 2 (21/10/24): Implementation of repulsive force.
• Day 3 (23/10/24): Fixing issues with the repulsive force.
• Day 4 (25/10/24): Since I couldn't fix it, I’ll remove the velocity to focus on fixing the forces.
• Day 5 (27/10/24): Implementation of velocities.

Functionality