satmotion-planning

SAT based motion planning


              SAT BASED MOTION PLANNING ALGORITHM

A simple motion planning problem can be remodelled as a SAT solving problem. Can anyone explain how is this possible?

In this problem, we have to find a collision free path from start to end location.


Solution

  • The simplest example could look like this.

    Let's introduce 2D grid of N rows and M columns, a moving agent A starts at a node (x,y). His target T has coordinates (x_i, y_j):

    a grid

    To reach a target the agent should perform several steps - move left, right, up or down consequently. We don't know how many steps it needs, so we have to limit this number ourselves. Let's say, we are searching for a plan that consists of K steps. In this case, we should add N*M*K boolean variables: N and M represent coordinates, K - time. If a variable is True then the agent currently at a node (x,y) at time k.

    Next, we add various constraints:

    1. The agent must change his position at each step (this is optional, actually)
    2. If robot at step k is at a position (x,y), then at step k+1 it must be at one of four adjacent nodes
    3. SAT formula is satisfied if and only if the agent at step k is at the target node

    I'll not discuss a detailed implementation of the constraints here, it's not that difficult. The similar approach could be used for multiagent planning.

    This example is just an illustration. People use satplan and STRIPS in real life.

    EDIT1 In the case of a collision-free path you should add additional constraints:

    1. If a node contains an obstacle, an agent can't visit it. E.g. corresponding boolean variables can't be True at any timestep e.g. it's always False
    2. If we are talking about a multiagent system, then two boolean variables, corresponding to two agents being at same timestep at the same node, can't be True simultaneously:

    AND (agent1_x_y_t, agent2_x_y_t) <=> False

    EDIT2

    How to build a formula that would be satisfied. Iterate over all nodes and all timestamps, e.g. over each Boolean variable. For each Boolean variable add constraints (I'll use Python-like pseudocode):

     formula = []
     for x in range(N):
         for y in range(M):
             for t in range (K):
                 current_var = all_vars[x][y][t]
                 # obstacle
                 if obstacle:
                     formula = AND (formula, NOT (current_var))
    
                 # an agent should change his location each step
                 prev_step = get_prev_step (x,y,t)
                 change = NOT (AND (current_var, prev_step))
                 formula = AND (formula, change)
    
                 adjacent_nodes = get_adj (x,y, k+1)
    
                 constr = AND (current_var, only_one_is_true (adjacent_nodes))
                 formula = AND (formula, constr)
     satisfy (formula)