Path planning based on potential fields from rough mereology

This paper focuses on path planning for a remote robotic agent using rough mereology potential field method. We test the proposed path-creation and path-finding algorithms and propose working alternative versions. Furthermore we apply path smoothing with custom collision detection to further optimize the route from the robot initial position to the goal.

The algorithm

The Square Fill algorithm was proposed by Osmialowski and Polkowski in Spatial Reasoning Based on Rough Mereology: A Notion of a Robot Formation and Path Planning Problem for Formations of Mobile Autonomous Robots, In: Transactions on Rough Sets XII. We used it as a base for our testing.

  1. Define initial values for current_distance and direction,
  2. Create an empty queue Q,
  3. Add the first potential field to Q,
  4. Enumerate through Q:
    • Check if the potential field is out of bounds or is colliding or already there,
    • Increase the current_distance to the goal,
    • Define neighbours depending on the direction (clockwise, anticlockwise),
    • Add neighbours to Q,
    • Change the direction to opposite,
    • Remove current potential field from Q.
  5. If Q is empty, finish.

The tests

The default Square Fill Algorithm alternates between clockwise and anticlockwise methods while creating its potential field neighbours. We experimented with possible outcomes:

Depending on the complexity of the map and placement of the robot, goal and obstacles different results were produced. For a simple map containing two obstacles the following algorithm time and path length (in pixels) was returned:

Path smoothing

After the path from the robot initial position to the goal is created, we are applying a smoothing algorithm, to make the route optimal. We modified the standard smoothing algorithm, by adding custom conditions, to keep a safe distance from the robot to possible collision fields. The final algorithm can be represented as:

def smooth_path(self, figure, path, alpha=0.1, beta=0.1, iterations=100):
        result = copy.deepcopy(path)  # Copy not to alter the initial path

        for k in range(iterations):
            other = copy.deepcopy(result)
            for i in range(1, len(path) - 2):
                temp_x = result[i].x + alpha * (other[i-1].x + other[i+1].x - 2 * result[i].x)
                temp_x += beta * (other[i].x - result[i].x)
                temp_y = result[i].y + alpha * (other[i-1].y + other[i+1].y - 2 * result[i].y)
                temp_y += beta * (other[i].y - result[i].y)

                test_field = Field(x=temp_x, y=temp_y)
                should_be_added = True
                for coll in self.collision_coords:
                    if test_field.distance_to(coll) < math.sqrt(math.pow(coll.r * 2, 2) + math.pow(coll.r * 2, 2)):
                        should_be_added = False
                        continue
                if should_be_added:
                    result[i].x = temp_x
                    result[i].y = temp_y
        self.draw_path(figure, result)
        self.path_points = result
write_path_to_file(result)

Video presentation

Reference