A method to control a lift vehicle, comprising: determining, using a processor of the lift vehicle, a first positional state of the lift vehicle with respect to a payload controlled by a plurality of lift vehicles (102, 104,...) including the lift vehicle; determining, using the processor, a second positional state of the lift vehicle with respect to a goal location; detecting, using a sensor of the lift vehicle, distances to other ones of the plurality of lift vehicles (102, 104,...); determining, using the processor, a third positional state of the lift vehicle based on the detected distances to the other ones of the plurality of lift vehicles (102, 104,...); and calculating a control command to control the lift vehicle based on the first positional state, the second positional state, and the third positional state, wherein calculating the control command comprises: determining a first function representative of a desired change in movement of the lift vehicle, the first function being based on the first and second positional states; determining a second function representative of a separation cost between the lift vehicle and at least one of the other ones of the lift vehicles (102, 104,...), the second function being based on the third positional state, said separation cost being a function of the distance of the lift vehicle and at least one of the other ones of the lift vehicles (102, 104,...); combining the first and second functions to calculate a combined control function; and determining a solution to the combined control function. determining, using a processor of the lift vehicle, a first positional state of the lift vehicle with respect to a payload controlled by a plurality of lift vehicles (102, 104,...) including the lift vehicle; determining, using the processor, a second positional state of the lift vehicle with respect to a goal location; detecting, using a sensor of the lift vehicle, distances to other ones of the plurality of lift vehicles (102, 104,...); determining, using the processor, a third positional state of the lift vehicle based on the detected distances to the other ones of the plurality of lift vehicles (102, 104,...); and calculating a control command to control the lift vehicle based on the first positional state, the second positional state, and the third positional state, wherein calculating the control command comprises: determining a first function representative of a desired change in movement of the lift vehicle, the first function being based on the first and second positional states; determining a second function representative of a separation cost between the lift vehicle and at least one of the other ones of the lift vehicles (102, 104,...), the second function being based on the third positional state, said separation cost being a function of the distance of the lift vehicle and at least one of the other ones of the lift vehicles (102, 104,...); combining the first and second functions to calculate a combined control function; and determining a solution to the combined control function. determining a first function representative of a desired change in movement of the lift vehicle, the first function being based on the first and second positional states; determining a second function representative of a separation cost between the lift vehicle and at least one of the other ones of the lift vehicles (102, 104,...), the second function being based on the third positional state, said separation cost being a function of the distance of the lift vehicle and at least one of the other ones of the lift vehicles (102, 104,...); combining the first and second functions to calculate a combined control function; and determining a solution to the combined control function.