ECE 602 FINAL PROJECT - Convex Optimization Strategies for Coordinating Large-Scale Robot Formations

Pranav Ganti (20663913), Leonid Koppel (20433722)

Contents

Introduction

Multi-robot teams are becoming increasingly viable as a solution in a variety of different fields, due to advances in wireless communications and embedded computing. Potential applications include surveillance, search and rescue, or military operations. Dr. Jason Derenick and Dr. John Spletzer, in their paper "Convex Optimization Strategies for Coordinating Large-Scale Robot Formations" [1], investigate the use of convex optimization strategies to coordinate these multi-agent teams. This problem can be formulated as a a Second-Order Cone Program (SOCP), and either the maximum distance or total distance travelled by these robots between formations can be minimized. In this project, we look to use CVX to solve these SOCPs, and implement the extensions proposed by the authors, such as additional workspace constraints and path-planning in a polygonal environment.

Problem Formulation

Consider $m$ robots in a plane with initial pose $P \in \mathbf R^{m\times 2} $ and a target formation $S \in \mathbf R^{m\times 2}$. We want the robots to move to a final pose $Q$ which is similar to $S$ - that is, having the same shape up to translation, rotation, and scale. This similarity is denoted by $Q \sim S$. We constrain scale to be positive, disallowing reflection.

The two related problems are as follows: given an initial pose $P$, choose $Q\sim S$ to either

minimize $$ \max_{i=1,\ldots,m} ||q_i - p_i||_2 $$, the maximum distance travelled; or

minimize $$ \sum_{i=1}^m ||q_i - p_i||_2 $$, the total distance travelled

where $p_i = (p_i^x, p_i^y)$ and $q_i = (q_i^x, q_i^y),\quad i=1,\ldots,m$ represent the coordinates of each point in the initial and final pose. The desired shape is represented by $s_i = (s_i^x, s_i^y)$.

Note that we do not consider the assignment problem, the problem of choosing which robot corresponds to which $s_i$ in the target shape. We only find the optimal translation, orientation, and scale of the shape given a fixed correspondence. This problem is convex, while the assignment problem is not.

Additionally, we reproduce two extensions demonstrated in the original paper: adding polygonal workplace constraints, and motion planning through an environment defined as a series of polygons. These problems are formulated in later sections.

Proposed solution

Derenick and Spletzer start with the constraints on the final pose, given the desired shape. The constraints are, for $i=2,\ldots,m$:

$$ \begin{array}{cl} q_i^x - q_1^x &= \alpha(s_i^x \cos\theta - s_i^y \sin\theta) \\ q_i^y - q_1^y &= \alpha(s_i^x \sin\theta + s_i^y \cos\theta) \end{array} $$

These constraints simply express an affine transformation of the target shape. We can thus express them as affine functions.

Here there appears to be an omission in Derenick and Spletzer's paper. They specify $s_1 := \mathbf 0$, and state, "without loss of generality, we can define the formation orientation and scale, repectively, as"

$$ \theta = \arctan \frac{q_2^y - q_1^y}{q_2^x - q_1^x},\quad \alpha = \frac{||q_2 - q_1||}{||s_2||_2} $$

However, the above expression for $\theta$ is only valid if we also define $s_2^y = 0$. That is, the target shape must be expressed such that the first two points lie on the horizontal axis. Otherwise, it is impossible for $\theta$, which obviously expresses a relationship between $S$ and $Q$, to be independent of $S$. We obtained incorrect results until we added this requirement, which is demonstrated in the Analysis section.

With the requirement $s_2^y = 0$ in place, the affine form of the constraints is found to be

$$ \begin{array}{cl} ||s_2||(q_i^x - q_1^x) &= (s_i^x, -s_i^y)(q_2 - q_1) \\ ||s_2||(q_i^y - q_1^y) &= (s_i^y, s_i^x)(q_2 - q_1) \end{array} $$

for $i=3,\ldots,m$. Note that there are $2m-4$ constraints, leaving four free variables: the 2D translation, rotation, and scale of the target shape.

The constraints can be expressed in the matrix form $Aq = 0$. Then the problems are

$$ \begin{array}{llll} \min_q &\max_{i=1,\ldots,m} ||q_i - p_i||_2 \quad \textnormal{or} \quad &\min_q &\sum_{i=1}^m ||q_i - p_i||_2 \\ \textnormal{s.t. } &Aq = 0 & \textnormal{s.t. } &Aq = 0 \end{array} $$

Using the epigraph form, the problems can be restated as second-order cone programs:

$$ \begin{array}{llll} \min_q &t \qquad\qquad \textnormal{or} \quad &\min_q &\mathbf 1^T s \\ \textnormal{s.t. } &y_i \leq t & \textnormal{s.t. } &y_i \leq s \\ & Aq = 0 && Aq = 0 \end{array} $$

where $y_i = ||q_i - p_i||_2$, $t \in \mathbf R$, $s \in \mathbf R^m$.

In this report, we demonstrate only the second problem, minimizing total distance. This matches the demonstrations in the original paper.

Data synthesis

For our reproduction, we used a shape repesenting the letters "UW". We generate points for any $m$ by interpolating along a series of curves representing the letters, as implemented in UW_points.m.

m = 100;
S = UW_points(m);

We produce the initial pose by arbitrarily transforming the shape, and adding random noise to each point. We do this because, as noted above, the assignment problem is not addressed; this way, we start with a somewhat sensible assignment where each robot's initial point $p_i$ is correlated to its target point. This appears to be what Derenick and Spletzer did in their paper, as choosing completely random $p_i$ does not produce such pleasing results.

p_theta = -pi/2 + 0.2;
p_scale = 1;
R = [cos(p_theta) sin(p_theta); -sin(p_theta) cos(p_theta)];
p_trans = [5 10];
P = p_scale * S*R + p_trans; % Transform
P = P + 6*rand(m,2); % Add uniform random noise

An example is presented below:

figure(1); clf;
plot_shape;

Solution

To implement the constraints in matrix form $Aq = 0$, we let $q \in \mathbf R^{2m}$ be the "flattened" vector form of $Q$, where odd elements represent $x$-values and even elements represent $y$-values. Then $A \in \mathbf R^{2(m-2)\times 2m}$. The matrix $A$ is generated in constraint_matrix.m

A = constraint_matrix(S);

Here we formulate the problem to minimize total distance:

cvx_begin quiet
variable q(2*m) % vector form of q: odd elements x, even elements y
Q = reshape(q, 2, m)'; % matrix form, each row has (x, y) point
y = norms(Q-P, 2, 2);  % y_i = || q_i - p_i ||_2
variable t(m)   % auxiliary variable for epigraph form

minimize ones(1,m) * t
subject to
    y <= t
    A*q == 0
cvx_end

Results

The figure below shows the initial pose (filled circles) and the final pose (empty black circles) which forms the target shape while minimizing total distance travelled, represented by line segments.

figure(2); clf;
plot_robots(P, Q);

Integrating Workspace Constraints

An additional constraint that can be implemented is that of a "workspace constraint". A convex shape, such as a polyhedron, can be integrated as a constraint for the final configuration to lie within. The workspace C can be expressed as:

$C = \{x \in \mathbf{R}^2: Cx \leq d\}$

For the purpose of demonstration, let us specify an arbitrary 5-sided polyhedron which lies within the original operating environment.

The 5 equations which specify the polyhedra are as follows:

$-x_2 \leq -6$

$x_2 \leq 18$

$x_1 \leq 20$

$-x_1 \leq -8$

$2/3x_1 -x_2 \leq 10/3$

This would be placed into the following matrix inequality:

$A_{c}x \leq b_c$, with:

A_c = [0 -1;
       0  1;
       1  0;
      -1 0;
       2/3 -1];

b_c = [-6, 18, 20, -8, 10/3]';

Once again, we formulate the problem to minimize total distance:

cvx_begin quiet
variable q(2*m) % vector form of q: odd elements x, even elements y
Q = reshape(q, 2, m)'; % matrix form, each row has (x, y) point
y = norms(Q-P, 2, 2);  % y_i = || q_i - p_i ||_2
variable t(m)   % auxiliary variable for epigraph form

minimize ones(1,m) * t
subject to
    y <= t
    A*q == 0

    % The following inequalities constrain the workspace to the polyhedron
    % defined above.
    for i = 1:m
        A_c*Q(i, :)' <= b_c;
    end
cvx_end

The figure below shows the final pose minimizing total distance travelled, constrained to the arbitrary workspace (black lines). The initial pose is the same as before.

figure(3); clf;
plot_robots(P, Q);
draw_polyhedron;

We can see here now that the optimal robot locations are constrained within the polyhedron.

Notes on Complexity

One of Derenick and Spletzer's contributions to this paper was the formulation of this problem in a manner that can be solved efficiently, even as the number of robots increases past 1000.

To do this, however, they first look to reformulate the problem as a inverse log-barrier problem. We can recall from Boyd that the log-barrier method is an interior-point method, which works to solve equality constraints using Newton's method. A barrier method can be formulated as:

$$ \begin{array}{lll} \min &f_{o}(x) + \sum_{i=1}^m I_{\_}(f_{i}(x)) \\ \textnormal{s.t. } &Ax = b \end{array} $$

where $I_{\_}$ represents the indicator function for the inequality constraint. For the log-barrier method, however, the indicator function is replaced with the log function, which acts as a sufficient approximation. As per Boyd, this implicitly defines the inequality constraints, with only the equality constraints explicitly stated.

As seen above, the minimum total distance problem had been reformulated in epigraph form. This has been illustrated by Derenick and Spletzer as follows:

$$ \begin{array}{lll} \min_{q,t_{1}} &\tau_{k}t_{1} - \sum_{i = 1}^m \log(t_{1}^2 - (q_{i} - p_{i})^T(q_{i} - p_{i}))\\ \textnormal{s.t. } &Aq = 0 \end{array} $$

where $A$ and $q$ have been previously defined. Derenick and Spletzer define the variable $\tau_{k}$ as the "inverse log-barrier scaler for the kth iteration". This has been defined as:

$$ \tau_{k + 1} = \mu^k\tau_{k},\quad \mu > 1 $$

We believe that this is a typo in the paper; $\tau$ should be defined as either $\tau_{k+1} = \mu^k\tau_{1}$, or $\tau_{k+1} = \mu\tau_{k}$.

This formulation alone, however, is not sufficient for scalability. In order to maintain O(m) complexity with an increasing number of robots, Derenick and Spletzer reformulate the Karush-Kuhn-Tucker system as a monobanded system; the full derivation for this is discussed within the paper. The final set of constraints are then defined as:

$$ \begin{array}{lll} \min_{q,t} &\frac{\tau_{k}}{m}\sum_{i=1}^mt_{i} - \sum_{i = 1}^m \log(t_{1}^2 - (q_{i} - p_{i})^T(q_{i} - p_{i}))\\ \textnormal{s.t. } &||s_{2}||_{2}(q_{l}^x - d_{j}^x) = [s_{l}^x, -s_{l}^y](d_{j + 1} - d_{j})\\ \quad &||s_{2}||_{2}(q_{l}^y - d_{j}^y) = [s_{l}^y, s_{l}^x](d_{j+1} - d_{j})\\ \quad &t_{i + 1} = t_{i}, \quad i = 1, \ldots, m-1\\ \quad &d_{2i + 1} = d_{2i-1}, \quad i = 1, \ldots, m-3\\ \quad &d_{2(i+1)} = d_{2i}, \quad i = 1, \ldots, m-3\\ \quad &d_{i} = q_{i},\: i \in {1, 2} \end{array} $$

for $l = 3, \ldots, m$ and $j = 2(i -3) + 1$.

This formulation of the problem is not solvable using CVX. The objective function actually violates the Disciplined Convex Programming (DCP) ruleset, as the log function consists of two convex terms being summed together; CVX deems this an illegal operation. It would be possible to write a custom solver, but since this section merely describes an optimized method of solving the same problem, we feel that it is outside the scope of the ECE 602 Final project.

Motion planning in polygonal environments

Derenick and Spletzer extend the above problem by considering a robot formation moving through a polygonal environment. We assume we already have a map of the environment decomposed into triangles, such that any two consecutive triangles form a convex region, and that a path through $k$ triangles has been found by some high-level planner. We seek to find a path for each robot which minimizes the total distance travelled while maintaining formation and not colliding with the environment.

The constraints for each triangle are expressed as three halfspace inequalities of the form $c^T x \leq d$.

We generate a randomized environment with 16 triangles and the corresponding constraints in the functions below.

k = 16; % number of triangles
triangles = generate_env(k);
[C,d] = triangle_constraints(triangles);

There is a slight hitch: in this problem, the optimal path tends toward $\alpha = 0$, thus a minimum scale constraint is desired. Unfortunately, while maximum scale constraints are convex, a minimum scale constraint of the form

$$ \alpha \geq \alpha_{min} $$

is not in convex (recall $\alpha = ||q_2 - q_1||/||s_2||$, thus its sublevel sets but not superlevel sets are convex).

Spletzer's earlier work [2] includes an approximation scheme to allow a minimum scale constraint, but also notes that a scale constraint is convex if orientation is fixed.

For this demonstration we use the latter approach, and set a fixed orientation, which appears to be what Derenick and Spletzer used for their demonstration. Following their example, we use a circular formation of robots, generating the points in circle_points.m. We place the initial pose $P$ inside the first triangle.

m = 12; % number of robots
S = circle_points(m);
P = 3*S - [5.5 1];

We formulate the problem as

$$ \begin{array}{lll} \min_q &\sum_{i=1}^k \mathbf 1^T t_i \\ \textnormal{s.t. } &|| q_{ij} - q_{i-1,j}||_2 \leq t_{ij}, \quad & i = 1,\ldots,k,\quad j = 1,\ldots,m \\ &q_{ij} - q_{i,1} = \alpha s_{i}, \quad & i = 1,\ldots,k,\quad j = 2,\ldots,m \\ &C_{i} q_{ij} \leq d_i, \quad & i = 1,\ldots,k,\quad j = 1,\ldots,m \\ &\alpha \geq \alpha_{min} \end{array} $$

where $t_i \in \mathbf R^m, i = 1,\ldots,k$. This is expressed in CVX below.

min_scale = 2;
PrevQ = P;

cvx_begin quiet
variable q(2*m, k-1) % vector form of q, for each triangle 1..k
variable scale nonnegative
Q = reshape(q, 2, m, k-1); % matrix form, with 3rd dimension for each triangle
Q = permute(Q, [2 1 3]);
variable t(m, k-1)   % auxiliary variable for epigraph form

minimize sum(t(:))
subject to
    for i = 1:k-1
        % Minimize total distance
        norms(Q(:,:,i) - PrevQ, 2, 2) <= t(:, i);
        % Constrain shape with fixed orientation
        for j = 2:m
            Q(j,:,i) - Q(1,:,i) == scale * S(j,:)
        end
        % Constraint to inside triangle
        for j = 1:m
             C(:,:,i+1) * Q(j,:,i)' <= d(:,:,i+1)
        end
        % Constraint minimum scale
        scale >= min_scale

        PrevQ = Q(:,:,i);
    end
cvx_end

Results

The below figure presents the polygonal environment (black dashed lines) and the optimal sequential path which minimizes total distance travelled while avoiding collisions. Orientation and minimum scale of the robot formation are constrained.

figure(4); clf;
plot_triangles(triangles);
plot_robot_path(P, Q);

Analysis

We were able to recreate the results obtained by the authors. However, we identified an unstated assumption in the paper. This section will discuss that finding in detail, and then discuss the applications and limitations of the work in general.

Limitation on $\theta$

The main objective of this paper was to formulate swarm robotic motion as a convex optimization problem, where the swarm of robots maintained a "shape", the "geometric information that remains when location, scale, and rotational effects are removed". Derenick and Spletzer's definition for the formation orientation, $\theta$, is incorrect without the additional choice $s^y_2 = 0$.

Shown below is the simplest example illustrating this shortcoming. Let us construct a triangle, with vertices at (0,0), (0, 1), and (1,0). The initial location of the robots are directly at the vertices of the shape. In theory, the robots should already be at their optimal location, and no motion should be required.

% Define triangle shape
sx = [0, 0, 1];
sy = [0, 1, 0];
S = [sx; sy]';

% Define number of points
m = 3;

% Define initial position to be exactly the location of the shape
px = sx;
py = sy;
P = [px; py]';

% Define constraint matrix
A = constraint_matrix(S);

Here we formulate the problem to minimize total distance:

cvx_begin quiet
variable q(2*m) % vector form of q: odd elements x, even elements y
Q = reshape(q, 2, m)'; % matrix form, each row has (x, y) point
y = norms(Q-P, 2, 2);  % y_i = || q_i - p_i ||_2
variable t(m)   % auxiliary variable for epigraph form

minimize ones(1,m) * t
subject to
    y <= t
    A*q == 0
cvx_end

figure(5);
clf; hold on;
plot_triangle_example(Q, P, S);

Clearly, this result is incorrect. The expected result is that the final pose (blue stars) and initial pose (red circles) are identical. For comparison, if the vertices in the shape are defined as:

sx = [0, 1, 0];
sy = [0, 0, 1];
S = [sx; sy]';

That is, the 2nd point is (1,0) instead of (0,1), we see that the solution is correctly determined.

figure(6);
openfig('expected_triangle_example.fig');

This demonstrates that the requirement $s^y_2 = 0$ is important. Note this choice can still be made without loss of generality; it just requires that the representation of the target shape ,$s$, be correctly rotated when the problem is formulated.

Limitations in our reproduction

We only demonstrated the solution for the "total distance" problem, though the paper also formulates the "minimax" distance problem. However, only the former problem was demonstrated in the figures in the paper. Since the solutions are very similar, our implementation could easily be changed to solve the minimax problem.

We did not reproduce the optimization discussed in the "Notes on Complexity" section. However, we found that our solution for minimum total distance, as implemented in CVX, took approximately 0.15 seconds of CPU time for $m=1000$. This matches the authors' result for their optimized algorithm. This suggests that a decade's worth of CPU advances can make up for a less efficient implementation. (Of course, an opmimized algorithm would still be faster, especially for greater $m$).

Applications and limitations

The optimization methods demonstrated can be useful for controlling a swarm of robots, but have several shortcomings. These shortcomings mostly stem from considering only convex optimization problems.

First, Derenick and Spletzer's method does not address the assignment problem. For example, if the initial pose is a uniformly random distribution of robots, we obtain a result similar to one below:

figure(7);
openfig('assignment_problem.fig');

That is, when the assignmentof the robots is completely random, the distance-mininimizing pose tends toward a point in the centre of the initial pose. In a real-world situation, this result is unlikely to be useful, and a better assignment of robots is desired. A suboptimal, approximate assignment may suffice; alternatively, the methods in this paper may be useful as part of a solution for the (nonconvex) assignment problem.

Another limitation, which is noted by the authors, is that the robots' kinematic constraints are not considered. Nonholonomic kinematics, in particular, present a non-convex problem. However, the authors claim, "although the results will no longer be optimal for such robots, they should still be quite good in practice."

The path planning solution also does not consider collisions between the robots. Thus, in practice it can only be used as an input to some lower-level motion planner.

In the path-planning case, the convexity requirement is a considerable drawback: the minimum-scale constraint is convex only if orientation is fixed. In a real-world situation, it is desirable to have a minimum-scale constraint (since robots physically must maintain some distance from each other) and often not desirable to have a fixed orientation. In this case, the approximations proposed in the authors' other work [2] may be useful.

One higher-level limitation of this work is that it assumes a central planner which is aware of all of the robots' positions. In reality, control of the robot swarm may be decentralized.

Some of these limitations are addressed in the authors' later works. For example, [3] addresses collision avoidance between the robots.

Conclusion

We successfully reproduced the results obtained by Derenick and Spletzer for the minimimum total distance problem, and identified an unstated assumption in their derivation. We also reproduced the two extensions demonstrated in the paper: the addition of workspace constraints, and path planning in a polygonal environment. We found the solution to be useful for high-level planning, but too limited to stand alone without additional algorithms for assignment and collision avoidance.

References

Source code

main.m is the source file used to generate this report.

The following functions are used for data synthesis:

The following functions are used to formulate the optimization problems:

The following functions are used for plotting results:


Published with MATLAB® R2016b