I'm implementing a reinforcement learning task, to solve a parking task for autonomous robots. So basically, the idea of the task is to start at a certain Point in front of the parking spot and drive to a pose without colliding with obstacles. The Agent has reached the goal if the a given Position and a Heading angle of the robot matches the goal pose.

I Have actually a lot of Problems shaping a Reward Function to solve this task. So I ask you guys, to help me with this. What I need a Reward Function depending on the following:

• (Distance reward) The closer the robot is to the target, the higher the reward
• (Orientation reward) The smaller the tolerance of the heading angle to the angle of the target position, the higher the reward
• (Speed reward) The slower the speed when approaching the target position, the higher the reward

My current Reward function looks like this:

    current_distance = self.get_euclidean_distance(current_position, desire_position)
    self.distance_reward = (-1)* current_distance/self.max_dist_to_targ

    # Heading Reward. 0: indicates vertical parking, -1: means reverse parking state
    heading_angle = abs(current_heading_angle - target_angle)
    if heading_angle > 0 and heading_angle < np.pi:
        self.heading_angle_reward = -1 * abs(heading_angle - np.pi/2) / np.pi
    else:
        self.heading_angle_reward = -2 * abs(heading_angle - (3*np.pi)/2) / (2*np.pi - 1)
        
    # Goal Reward
    self.goal_reward = 0
    if current_distance <= cp.pose_tolerance and heading_angle <= np.radians(cp.heading_angle_tolerance):
        self.goal_reward = 150

    # Collistion Penalty
    if not self.is_near_by_an_object2(beams_coords, current_trans_vel):
        self.collision_penalty = 0
    else:
        self.collision_penalty = -10

    reward = 10*((1 - w) * self.distance_reward + w * self.heading_angle_reward) +\
            10*self.goal_reward + 10* self.collision_penalty 

I would be very happy if someone can suggest me what is wrong with the function and how to implement the velocity reward! Thank you guys.