SICNav: Safe and Interactive Crowd Navigation using Model Predictive Control and Bilevel Optimization

The system state is continuous and concatenates the states of all agents, one robot and $N$ humans. At time step $t$, the state is, ${\mathbf{x}}_{t}=({{\mathbf{x}}_{t}^{(r)}},{{\mathbf{x}}_{t}^{(1)}},\dots,{{\mathbf{x}}_{t}^{(N)}})\in\mathbb{R}^{6+6N},$ where the state of the robot, ${{\mathbf{x}}_{t}^{(r)}}\in\mathbb{R}^{4}$, consists of its 2D position, heading, linear velocity at $t-1$, and the state of each human agent, ${{\mathbf{x}}_{t}^{(j)}}\in\mathbb{R}^{6}$, consists of its 2D position, and 2D velocity. We also assume to know the 2D goal position of each human. The goal of other agents is privileged information that is unavailable in the real-world, in Sec. VII-A we discuss how this assumption can be relaxed when deploying the algorithm.

We separate the dynamics of the system into separate functions for the robot and the humans. The robot dynamics, ${{\mathbf{x}}_{{t+1}}^{(r)}}=\mathbf{f}({{\mathbf{x}}_{t}^{(r)}},{\mathbf{u}}_{t}),$ are modeled as a kinematic unicycle model, where the control input for the system, ${\mathbf{u}}_{t}\in\mathbb{R}^{2}$, is a vector of the linear and angular velocity of the robot for time step $t$. The dynamics of each human $\forall j\in\mathbb{I}_{1:N}$, ${{\mathbf{x}}_{{t+1}}^{(j)}}=\mathbf{h}({{\mathbf{x}}_{t}^{(j)}},{{\tilde{\mathbf{u}}}_{t}^{(j)}}),$ are modeled as kinematic integrators, where ${{\tilde{\mathbf{u}}}_{t}^{(j)}}$ denote the human actions. Note that these actions are not an input to the system, but rather predictions of the human actions given the state of the system, ${\mathbf{x}}_{t}$.

We obtain these predictions of human actions at each step, ${{\tilde{\mathbf{u}}}_{t}^{(j)}}$, by solving the ORCA optimization problem [18]. Before defining the complete optimization problem, we introduce a basic version. For one agent $j$ at one time step $t$, we have the following set of optimal (i.e. minimum-cost feasible) velocities,

where the optimization variable $\mathbf{v}\in\mathbb{R}^{2}$ is the velocity of agent $j$ and the preferred velocity ${\mathbf{v}_{\text{pref}}}:\mathbb{R}^{6+6N}\to\mathbb{R}^{2}$ is a vector toward the agent’s goal position. The maximum velocity of the agent is constrained by ${{v}_{\max}}$, and the vectors ${\mathbf{n}^{(j|l)}}:\mathbb{R}^{6+6N}\to\mathbb{R}^{2}$, which are piece-wise linear functions of ${\mathbf{x}}_{t}$, and scalar ${\rho^{(j|l)}}:\mathbb{R}^{6+6N}\to\mathbb{R}^{2}$ which are nonlinear functions of ${\mathbf{x}}_{t}$, define linear collision avoidance constraints between agents $j$ and agent $l,\forall l\in\bigl{\{}r,1,\dots,N\bigr{\}}\setminus\bigl{\{}j\bigr{\}}$. The cost and constraint functions in (1) depend on the positions and velocities of all agents in the system at each time step, therefore, the state of the system, ${\mathbf{x}}_{t}$, parameterizes this optimization problem. In the remainder of this section, we will reference Fig. 2, which illustrates an exemplary environment state (Fig. 2(a)) and a graph of the associated ORCA problem solved by each human (Fig. 2(b)).

Agent-agent collision avoidance: The normal vectors, ${\mathbf{n}^{(j|l)}}({{\mathbf{x}}_{t}})$, and constants, ${\rho^{(j|l)}}({{\mathbf{x}}_{t}})$, are formulated using a Reciprocal Velocity Obstacle (RVO) approach [18]. Assume agent $j$ is solving (1). For each other agent, $l\neq j$ (e.g. the robot and human 2 in Fig. 2), agent $j$ uses the current relative position and velocity between itself and $l$ to compute a region of relative velocity space, called a velocity obstacle [34], which contains all relative velocities that would cause a collision between the agents within some designated time horizon $\tau$. Assuming that both agents follow the same velocity-obstacle protocol, the algorithm finds uses this region to find a half-plane in the velocity space of $j$ that forbid the agent from choosing a velocity for the next time step that would cause a collision with the other agent within $\tau$. In the case of human $j=1$ in Fig 2(b), the green half plane with the feasible side shaded is induced by the position and velocity of the robot$=l$. As illustrated in Fig. 2(a), the robot is below and to the left of human 1 and moving upwards ($y\uparrow$), causing the velocity space of human 1 to be forbidden from velocities that move it to the left ($x\downarrow$) or downwards ($y\downarrow$) as that may cause a collision with the robot within $\tau$. Likewise, the position and velocity of human $l=2$ induces the orange constraint for human 1.

Static obstacle collision avoidance: The algorithm uses a simplified RVO derivation to obtain half-plane constraints that avoid static obstacles defined as line segments, e.g. the static obstacles on the right and left in Fig. 2(a). These constraints are similarly represented with a normal vector, ${\mathbf{n}^{(j|\tilde{l})}}({{\mathbf{x}}_{t}})$, and a scalar, ${\rho^{(j|\tilde{l})}}({{\mathbf{x}}_{t}})$, for all static obstacle indices $\forall\tilde{l}\in\bigl{\{}N+1,\dots,N+M\bigr{\}}$. In the velocity space of human 1 in Fig. 2(b), the static obstacle on the right in Fig. 2(a) induces the dashed purple constraint to forbid human 1 from selecting a velocity towards the right ($x\uparrow$) that would cause a collision with the obstacle.

Feasible velocities: For human 1 in Fig. 2, the feasible set of velocities are formed by the aforementioned non-collision constraints for the static obstacle and the robot, together with human 1’s maximum permissible velocity (blue circle). In the graph for human 1 in Fig. 2(b), we can see that the feasible set of velocities as the intersection of the shaded sides of these three constraints (green, blue and purple). In the case of human 1, the preferred velocity lies in this feasible set and is the optimum.

Empty feasible set and ORCA relaxation: As discussed in [18], in dense scenarios i.e. where agents are in close vicinity to each other, the problem (1) may become infeasible, i.e. the feasible set does not contain any points. In this case, the ORCA algorithm [18] switches to a secondary optimization problem that finds the safest-possible feasible velocity by minimizing the maximum distance of the velocity variable $\mathbf{v}$ to the half-plane ${\mathbf{n}^{(j|l)}}({\mathbf{x}}_{t})^{\top}\mathbf{v}\geq{\rho^{(j|l)}}({\mathbf{x}}_{t})$ (See Sec. 5.2 of [18]). The secondary problem is a relaxation equivalent to moving every pairwise ORCA half-plane outward (i.e. opposite direction to the feasible side) equally, until at least one velocity becomes feasible, then choosing the lowest magnitude velocity. We formulate a similar relaxation by adding a slack variable to the linear collision avoidance constraints in (1). We only add slack variables for the agent-agent collision constraints as only those constraints may result in infeasibility. Putting the relaxation and static obstacle constraints together, we obtain the following optimization problem, which we will use as our prediction model for humans in SICNav.

The relaxed problem is a convex three-dimensional Quadratically Constrained Quadratic Program (QCQP). We use an inexact penalty function to relax the agent-agent collision constraints on the cost function. The upper bound of the error tends to $0$ as $M\to\infty$. Unlike the original paper [18] which picks the smallest velocity after relaxing the constraints, our approach picks the one closest to the preferred velocity.

The slack variable can be visualized as a third dimension protruding out of the page in Fig. 2(b). In the case of human 2 there is no feasible velocity for $\zeta=0$ (not visualized), thus the agent-agent collision constraints are relaxed to $\zeta^{*}=0.04$ (visualized), and we observe the single feasible point (black star) that becomes feasible is selected as the optimum. Since the value of $M$ is large, increasing the value of $\zeta$ is severely penalized, meaning that the constraints are relaxed minimally (i.e. least unsafe) in order to obtain at least one feasible velocity. To discern the consequences of the relaxation, recall that ORCA without the relaxation (1) is guaranteed to be collision-free if the agents follow their optimal velocities for some designated time horizon, $\tau$. An optimal velocity with $\zeta>0$ means that a collision may occur within time $\tau$. However, this time horizon is set to be much larger than one typical time step, e.g. $\tau=2.0s$ in Fig. 2, therefore it is very unlikely that a collision ever occurs in practice.

To analyze the optimal solutions of the ORCA problem (2), we use Lagrange duality theory. First, we define the Lagrangian of the ORCA problem (2). Recall that each agent $j$ has its own instance of the problem, however in the remainder of this section we drop the index $j$ for legibility.

For there to exist some $\mathbold{\lambda}^{*}$ such that the dual optimization is maximized, the problem (2) needs to satisfy the following constraint qualification [36].

Problem (2) satisfies SCQ by construction. Starting at any point, ${\mathbold{\nu}}$, the value of the slack variable dimension, ${\zeta}$, can be increased such that the pairwise collision constraints are relaxed sufficiently for the feasible set to have an interior.

Since the problem is convex and satisfies SCQ, the KKT conditions are necessary and sufficient for global optimality of the problem (2)[36]. Furthermore, note that the cost function in problem (2) is strongly convex with respect to ${\mathbold{\nu}}$ due to the quadratic form of its cost function. Thus, the set ${\mathcal{O}_{rlx}}({\mathbf{x}}_{t})$ is always singleton, i.e. the solution to the optimization problem is always unique[36]. In the next section, we will make use of the KKT conditions and the uniqueness of solutions reformulate a bilevel optimization, with problem (2) as the lower-level, to a single level problem.

In order to solve (3), we replace each instance of the lower level problem with its KKT conditions (6), to obtain the following Mathematical Program with Complementarity Constraints (MPCC),

where the constraints in (7) defined for each human, $\forall j,\forall l,\forall t$ analogously to (3).Each instance of constraint (3g) has been replaced by its KKT conditions to get (7g)-(7j). Note that the Lagrange multipliers of each instance of (3g) have become optimization variables in (7).

We introduce a warm start strategy in order to initialize the optimization of the MPC problem (7) from a feasible trajectory and to mitigate problems caused by scenarios where the lower-level problems do not meet the LICQ. To obtain a feasible trajectory for all agents, we initialize the optimization with trajectories generated from rollouts of the ORCA policy for the robot in addition to the humans. Using ORCA for the robot will allow us to satisfy its safety constraints. In order to also satisfy the robot’s kino-dynamic constraints, we alter the ORCA optimization used for the robot. We illustrate the altered ORCA policy in Fig. 4. Unlike the humans which are modelled as holonomic integrators, the robot has non-holonomic kino-dynamic constraints (7c)-(7e) (unicycle model and limits change in velocity) described in section III-A. To account for these constraints, we add four extra pseudo-collision constraints, illustrated in red in Fig. 4, the first two (crossing lines) bound the change in heading of the robot, and the second two (parallel lines) bound the change in linear velocity of the robot. Since ORCA constraints are linear in the velocity state, we set the constraints to be an underestimate the robot’s actual constraints, hence ensuring that the rollout is feasible, albeit with more conservative constraints.

We use the results of a sequential rollout of this optimization problem as the initial guess for the robot’s actions for the first time we solve the MPC problem. For subsequent MPC optimizations, we bring forward the MPC solution from the previous time step by shifting the trajectory by one time step, and use the altered ORCA policy to update the remaining (final) time step of the MPC horizon. In the case that for some attempted MPC solution we are unable to converge to a trajectory that is feasible and lower-cost than the initial guess, e.g. when we encounter a configuration where one of the lower-level problems does not satisfy LICQ, we use the warm start solution instead.

Given a history of states (positions and velocities) with length $T_{h}=3.2s$, ${{\mathbf{x}}_{-T_{h}:0}^{(j)}}$, for a pedestrian, we want to predict a sequence of future positions with length $T_{p}=4.8s$, ${{\mathbf{p}}_{0:T_{p}}^{(j)}}$. In order to use roll-outs of the ORCA policy, we first need to predict a goal position for each agent. To this end, we use the trajectories in the training dataset to generate goal distributions to sample. The training set contains full trajectories for each agent, ${{\mathbf{x}}_{-T_{h}:T_{p}}^{(j)}}$. First, we create a 2D histogram of the trajectories in the training set based on average historic velocity and acceleration. In each bin, we obtain the goal location as the final position of the agent at the end of each trajectory in the bin, ${{\mathbf{p}}_{T_{p}}^{(j)}}$. For all the goal positions in each bin, we fit a 2D Kernel Density Estimate (KDE) to estimate the density of goal positions.

At test time, we have a scene with $N$ pedestrians. For each agent in the scene, we use its average historic velocity to select the appropriate goal KDE, and obtain $2000$ samples of goal positions for each agent. This way, we are drawing independent goal samples for each agent. However, in order to perform a rollout of the scene using ORCA, we need to perform a rollout for the entire scene, i.e. we need samples from the joint-distribution of goals for every agent. If we naively combine the goals of each agent to get $2000$ joint samples, we ignore the fact that a highly likely goal sample for one agent may get matched with a low likely goal for another. To address this issue, we calculate importance weights for the combined goal samples. We sum the log-likelihoods of each individual goal sample over the number of agents in the scene and normalize with respect to the $2000$ samples. Now, for each of these $2000$ joint samples, we perform an ORCA rollout.

We follow the leave-one-out strategy commonly used with the ETH/UCY datasets [1, 2], where one dataset is selected as the test set, and the others are used for training (generating the goal KDE in our formulation). Following the metrics used in existing literature [1, 2], we evaluate prediction using,

1. 1.

   Average Displacement Error (ADE): the mean Euclidean distance between the ground truth and the maximum likelihood prediction.

2. 2.

   Final Displacement Error (FDE): the Euclidean distance between the maximum likelihood predicted position at the final prediction step, $T_{p}$, and the ground truth.

3. 3.

   Best-of-20: the minimum ADE and minimum FDE from 20 randomly sampled prediction trajectories.

4. 4.

   KDE-based Negative Log Likelihood (NLL) [40]: the mean NLL of the ground truth trajectory under the distribution of a KDE fit to the predicted trajectory samples.

To find the maximum likelihood prediction from our model, we follow a similar approach as calculating the KDE-based NLL metric, however, instead of evaluating the likelihood of the ground truth under the KDE, we evaluate the likelihoods of the predicted samples themselves. We compare our method against two state-of-the-art prediction models Social-GAN [39], Trajectron++ [1] and Y-net [2]. For the maximum likelihood metrics, we also compare our method to a Constant Velocity Motion Model (CVMM) that simply projects the current velocity of the agent forward in time. We were unable to replicate the results of Y-net, and thus cannot report the KDE-NLL metric.

The results of our evaluation are summarized in Table I. We can observe that our ORCA-based approach does not achieve state-of-the-art performance but is nonetheless competitive with top-performing methods. Our method beats S-GAN and CVMM on almost all metrics for all data splits. Our method has the added benefit of being able to be integrated into downstream planning, which is challenging for the other methods.

Testing environment: We evaluate the performance of our planner in a commonly used crowd navigation simulation environment [14], which we extend to simulate static obstacles in addition to the original dynamic agents, and to enable the simulated agents to be controlled by Social Force Model (SFM) [41] in addition to the original ORCA. We conduct two sets of experiments, one where the simulated humans follow ORCA and another where they follow SFM. For each set, we generate 500 random initial and final positions for the humans environments with $N=3$ and $N=5$ simulated humans to get a total of 2000 runs. The simulated testing area is a $2m$-wide corridor with initial positions and goals on either end. In order to ensure a high density of agents in the corridor, we constrain the space using static obstacles, formulated as line-segments, and add a bottleneck that simulates a $1m$ wide doorway. Fig. 6 highlights an interaction at this doorway. In every scenario, the robot is required to move through the simulated doorway to a goal $3m$ directly in front its fixed initial position. Furthermore, every agent must cross the doorway in order to arrive at its goal, ensuring that interactions occur in these scenarios.

Performance metrics: We adopt several metrics from the literature [14, 28, 10]. As the robot proceeds to the goal, data from the trajectories of the robot and agents is collected to score the behavior in the following ways,

• •

  Success Rate: The rate of scenarios in which the robot is able to successfully arrive at its goal within $90s$, which measures how the overall success of the robot to arrive at its goal.

• •

  Average Navigation Time: The average time to completion, which indicates the robot’s time efficiency.

• •

  Collision Frequency: The ratio of time steps that the robot is in a collision state with another agent compared to the total number of time-steps in the 500 scenarios, which measures the safety of the robot.

• •

  Freezing Frequency: The frequency of time steps where the robot needed to reduce its velocity to zero, which measures how often the robot can potentially get stuck in a deadlock with the human agents (aka the encountering the FRP).

Algorithms: We compare two variants of our algorithm with four baseline methods. The first version of our algorithm, SICNav-p, has access to the ground truth goal of position and ORCA parameters of the human agents. The second version, SICNav-np, does not have access to any privileged information about the agents. In order to estimate the intended goal of the agent, SICNav-np projects the current velocity of the agent forward in time, and uses the resulting position as the goal of the human. We compare these two versions of our algorithm with the following baselines: MPC-CVMM has the same setup as SICNav, however with a CVMM instead of the lower-level ORCA model for other agents, i.e. each human is assumed to continue at their current velocity for the complete horizon. We also compare with state of the art RL methods, Socially Attentive Reinforcement Learning (SARL) [14], and Relational Graph Learning (RGL) [15], the robot itself also running ORCA [18], and the classical Dynamic Window Approach (DWA) [4] as a reactive baseline. Note that since ORCA does not have a method to incorporate dynamics constraints by default, the ORCA robot follows a holonomic model without any kino-dynamic (unicycle) constraints, which means that it solves a simpler problem than the other methods.

Implementation¹¹1Code: github.com/sepsamavi/safe-interactive-crowdnav.git.: We implement SICNav in Python using CasADi [42]. To avoid infeasibility we introduce slack variables on the constraints along with quadratic penalty functions. We use a simulation time step of $\delta t=0.25s$ and an MPC horizon of $T=1s$. The RL methods are implemented using Stable Baselines [43]. The cost functions for both RL methods have been altered to include penalties analogous to all components of SICNav’s cost and constraints described in Sec. IV, and the one-step CVMM propagation used to calculate the respective value functions has been replaced with ORCA in order to allow fairer comparisons to our SICNav method, which uses the ORCA model in the lower-level problem. We train the RL algorithms with the same arrangement of static obstacles as the test environment, but with a training set of human initial and goal positions that are not included in the test set. We train separate models for $N=3$ and $N=5$. We intend to make the code for the simulator and methods publicly available upon acceptance.

Quantitative Results and Discussion: Tables II and III summarize the performance of our method and the baselines in the simulator with ORCA agents and SFM agents, respectively. In the following section we analyze the average performance of the crowd navigation methods. A statistical analysis of the significance of these results is provided in Appendix -A.

In simulation scenarios involving three humans simulated by ORCA, the two variants our method, SICNav-p and SICNav-np, achieve a success rate of 1.00, beating all other methods, except ORCA which achieves the same success rate. Accompanying SICNav-p’s success rate are the shortest average navigation time (4.24s) and the lowest rates of collision (0.00) and robot freezing (0.01). Removing privileged information results in negligibly slower navigation time for SICNav-np (4.37), without affecting collision and freezing rates. Using the ORCA for robot plans, however, demonstrates a much longer navigation time in excess of 10s, and a collision rate of 0.01. This poorer performance compared to SICNav-p is despite the fact that the ORCA method is solving a simpler problem without kino-dynamic constraints. RGL demonstrates an average navigation time of 8.20s and success rate of 0.95. The MPC-CVMM method achieves a slightly higher success rate at 0.98, but requires a longer average navigation time (7.06s). SARL, experiences a lowered success rate (0.73), a high average navigation time (27.21s), and increased rates of collision (0.01) and robot freezing (0.26). DWA records the lowest success rate (0.57), the longest average navigation time (52.09s), and a high rate of robot freezing (0.55).

In the scenarios with five humans simulated by ORCA, all SICNav-p, SICNav-np, MPC-CVMM, and ORCA maintain a success rate of 1.00. Similar to the three human tests, SICNav-p and SICNav-np perform better than all the baselines across all four metrics, indicating our method’s consistent performance under increasing crowd densities. Unlike the three human tests, SICNav-np posts the shortest average navigation time at 6.27s compared to SICNav-p’s 6.35s, with both versions of our method maintaining the lowest rates for collision (0.01) and robot freezing (0.02, and 0.03, respectively). MPC-CVMM and ORCA record longer average navigation times (7.47s and 14.98s) and show substantially higher rates of collisions (0.05 and 0.02) and robot freezing (0.05 and 0.09). In comparison, SARL reached an average navigation time of 6.45s, but with a success rate of 0.97 and also with a substantial rate of collisions (0.05). RGL and DWA, under the same conditions, demonstrate lower success rates (0.50 and 0.68 respectively), extended navigation times (46.86s and 48.45s) and increased rates of collisions and robot freezing.

In simulation scenarios involving three humans simulated by SFM, the two variants of our method, SICNav-p and SICNav-np, achieve a perfect success rate of 1.00, maintaining consistent performance with the different human simulation model. SICNav-np, slightly outperforms SICNav-p in terms of average navigation time, recording 4.81s compared to 4.84s with similar collision frequencies of 0.01. The baseline methods exhibit varied performance. For instance, ORCA shows improved navigation time (6.10s) for this human simulation model but with an equivalent collision frequency of 0.01. The MPC-CVMM method achieves a success rate slightly lower at 0.99 and a longer average navigation time (6.25s). SARL significantly struggles with the lowest success rate (0.64) and a much higher average navigation time (34.82s), along with increased collision and freezing rates, suggesting a severe degradation of performance when faced with novel behavior from the simulated humans. With this human simulation model, DWA outperforms our methods in terms of collision frequency (0.00) however at the expense of a lower success rate (0.96), a lengthier navigation times (16.66s), and increased freezing frequency (0.10).

In scenarios with five humans simulated by SFM, both of our models (SICNav-p and SICNav-np) continue to exhibit perfect success rates. SICNav-np presents shorter navigation times (5.14s), slightly better than SICNav-p’s 5.42s. The collision rates for both our models remain low but slightly increased to 0.02 and 0.03, respectively, indicating our model being strained under higher densities of SFM humans. MPC-CVMM and ORCA demonstrate longer navigation times (7.30s and 7.83s) and higher rates of both collisions and freezing. SARL’s success rate depletes to 0.92 with an inflated navigation time of 10.27s. RGL’s performance notably deteriorates, culminating in a drastically reduced success rate of 0.17 and an extended average navigation time. The degradation in performance of the RL methods in the SFM simulator demonstrates the challenge these methods have when faced with human behavior that deviates from the training data. DWA, however, maintains a high success rate of 0.95 and a low collision frequency of 0.01, but with a longer navigation time (20.78s) and increased freezing frequency (0.14).

The results in with the SFM simulated humans are consistent with the results from the ORCA simulated humans. Our proposed methods, SICNav-p and SICNav-np, do not show much degradation in any of the metrics, demonstrating the robustness of our approach to different human simulation models.

Qualitative Analysis: To qualitatively demonstrate the impact of SICNav-p generating robot plans jointly with predictions, in Fig. 5, we illustrate a scenario in which the robot cannot access its goal unless it interacts with other agents blocking its way. Since our SICNav-p method can proactively expect the reaction of the human agents, it slowly approaches the agents in its way, who move to allow the robot to pass in response. Note that the robot is not colliding with the other agents, the agents move out of its way and allow it to pass. On the same scenario we compare the baseline MPC-CVMM, whose cost and constraints are identical to SICNav-p except for the predictions of the other agents which use a CVMM model and remain constant in the planning optimization. Starting from an almost identical position, MPC-CVMM gets stuck as it is unable to account for the cooperation of the other agents.

Fig. 6 illustrates a similar but more complex scenario where the robot cannot access its goal unless it interacts with other agents blocking a doorway. In this scenario, the SICNav-p robot is able to move through the doorway by predicting that the agents will move out of its way. However, the baseline MPC-CVMM robot oscillates back and forth around without proceeding through the doorway.

Testing Environment: We also evaluate the performance of our planner, SICNav-np, on a real robot in a controlled environment. We use a Clearpath Jackal differential drive robot pictured in Fig. 1, which weighs $20kg$ and measures $46cm$ W $\times 60cm$ L $\times\times 50cm$ H. Our operating environment is an indoor space measuring $5m\times 9m$. We conduct a total of 80 runs from five scenarios with a varying number of humans $N\in\bigl{\{}1,2,3\bigr{\}}$. The scenarios are illustrated in Fig. 7. For each run of a particular scenario, all agents were instructed to move from their starting positions (where the agents are illustrated in Fig. 7) to the opposite corner of the room (illustrated by the arrows in Fig. 7). In the scenarios with multiple humans, we tested two variants: one where all the humans start from the opposite side of the room than the robot (variant 1) and another where one human moved in the same direction as the robot (variant 2). These scenarios were designed to cause a conflict zone in the middle of the room where all agents need to interact. We repeated 20 runs for each scenario with $N\in\bigl{\{}1,2\bigr{\}}$ humans and 10 runs for each scenario with $N=3$. We use a VICON motion capture system to measure the positions of the robot and human agents in the environment and use an Extended Kalman Filter (EKF) and Kalman Filters to track the positions and velocities of the robot and humans, respectively.

Algorithms: We compare the performance our SICNav-np method with MPC-CVMM as a baseline. As noted in Sec. VII-A, SICNav-np, does not have access to any privileged information about the agents. In order to estimate the intended goal of the agent, SICNav-np projects the current velocity of the agent forward in time, and uses the resulting position as the human’s goal. MPC-CVMM has the same setup as SICNav-np, however with a CVMM instead of the lower-level ORCA model for other agents, i.e. each human is assumed to continue at their current velocity for each step of the MPC horizon. By comparing our method with MPC-CVMM, we aim to isolate our analysis specifically to the primary contribution of this paper: the effect of using ORCA to model the humans and jointly produce robot plans with predictions.

Implementation: The robot used the Robot Operating System (ROS) 1 middleware. The robot accepts linear and angular velocity commands, which are then executed by its low level controller. We run the algorithms under analysis on an 8-core computer using Intel^(R) Core^(TM) i7-9700K CPU @ 3.6 GHz. We use the same Python implementation of our algorithm as the simulation experiments (Sec. VII-A), which uses CasADi as the modelling language. However, we replace the solver with Acados [44], which generates C++ code from the CasADi models and uses Just-in-time Compilation to allow us to run our algorithm in real-time at 10 Hz. We keep the same discretization step as the simulation of $\delta t=0.25s$ and use an MPC horizon of $T=2s$.

Quantitative Results and Discussion: We evaluate the robot’s path efficiency by measuring the average distance of the robot from the shortest path to its goal in each run. In Fig. 8(a), we observe that in scenarios with $N\in\bigl{\{}2,3\bigr{\}}$, SICNav-np has a lower average deviation from the shortest path, with $0.13m$ and $0.17m$, compared to MPC-CVMM, with $0.30m$ and $0.27m$. Furthermore, we observe that the distribution of values for MPC-CVMM is larger than for SICNav-np. These results indicate that SICNav-np is more efficient in terms of the path it follows, and more consistently chooses the shorter path.

We evaluate the robot’s safety by measuring the total duration of time the robot is too close to any human (body of robot is $<0.35m$ to centroid of human) in each run. In Fig. 8(b) we observe that in the scenarios with $N\in\bigl{\{}1,2\bigr{\}}$ there is not much difference in the average amount of time the robot is too close to a human between the two algorithms, with an average total time of $0.12s$ and $0.38s$ for SICNav-np compared to $0.00s$ and $0.26s$ for MPC-CVMM. However, in the case with $N=3$ humans, we can see that SICNav-np has a lower average duration too close to a human, with $0.36s$, compared to $0.77s$ for MPC-CVMM. While SICNav-np and MPC-CVMM perform similarly with respect to scenarios with two or fewer humans, in scenarios with a higher number of humans, SICNav-np maintains roughly the same amount of duration too close to any human, while the time for MPC-CVMM almost doubles, indicating that SICNav-np is more successful in avoiding close interactions with humans. We can also observe that while the size of the interquartile range is similar between the two methods for cases with $N\in\bigl{\{}1,2\bigr{\}}$, in the cases with $N=3$, the interquartile range is smaller for SICNav-np, indicating that SICNav-np is more consistent in avoiding close interactions with humans. Qualitative Analysis: Reviewing the videos of the runs, we observe that the robot using SICNav-np would often slow down and wait for the human to cross its path before proceeding. We illustrate this observation in Fig. 9 (Video: tiny.cc/sicnav_fig9). The SICNav-np robot predicts that the humans will alter their paths to pass each other and with the robot. As such the robot slows down. In contrast, the robot using MPC-CVMM, predicts that the humans would continue at their current speed and heading. Thus, the MPC-CVMM robot turns to the right to avoid the human’s path leading to a larger deviation from the shortest path than the SICNav-np robot. The observations from this scenario are consistent with the results in Fig. 8(a), where we observe that SICNav-np has a lower average deviation from the shortest path than MPC-CVMM.

In Fig. 10, we illustrate a scenario where the robot is confronted by humans acting adversarially towards the robot (Video: tiny.cc/sicnav_fig10). Fig. 10(a) illustrates the beginning of this scenario: as the robot proceeds towards its goal (green star), the humans with the blue and red hats walk towards the robot without attempting to avoid it. Snapshots of the top-view are illustrated in the first three plots in Fig. 10(c), starting with the humans walking toward the robot at time 00:02. At times 00:02 and 00:03, we observe that the predictions produced by SICNav-np (green lines) expect the humans to go around the robot. In the associated robot MPC plans (blue lines) we can see the robot planning to reverse away from the humans in the first few time steps of the MPC horizon, then proceed forward in the latter part of the horizon. However, in this scenario, the humans do not follow the robot’s predictions that they will deviate their path to avoid the robot, instead they proceed toward the robot. This human behavior deviates from our assumption that the humans follow ORCA. However, since the robot follows SICNav-np in receding horizon fashion, re-planning at 10Hz, new plans take into account the fact that the humans are getting closer to the robot than the SICNav-np prediction expected. As such, subsequent robot plans for the robot to reverse at the beginning of the MPC horizon in order to continue avoiding the humans. Thus, we observe the robot being pushed back, as illustrated in Fig. 10(a). Once the humans stop behaving adversarially, at time 00:11 in this scenario, the robot predictions become more accurate (e.g. human with the blue hat will remain roughly stationary at 00:14), and the robot proceeds to its goal as illustrated in Fig. 10(b). In this scenario, we observe the robustness of our method to adversarial behavior from the humans. In such scenarios, the robot only needs to avoid collisions for one time step of the MPC horizon. If its predictions are incorrect, as long as the robot can avoid the humans for one time step, it can re-plan and continue avoiding collisions.

Our evaluation of SICNav in simulation and with a real robot operating in a lab setting show promising results. Our algorithm runs in real time and enables the robot to interact with other agents while avoiding collisions in tight scenarios with multiple agents. Nonetheless, our paper has several limitations that we intend to address in future work.

Knowledge of Human Intents: SICNav requires information about the human agents’ intended goals in order to predict their future trajectory. In SICNav-np, which does not require knowledge of human goals, we project each agent’s current velocity forward in time to estimate their goal. While this method shows promising performance in our real-robot experiments (Sec. VII-B), in future work, we plan to use a learning-based short-term goal prediction (e.g. [2]) to generate goal estimates that we can then use in the ORCA models integrated into SICNav. To train, we intend to combine a goal predictor with open-loop ORCA rollouts and use a standard prediction loss (e.g. [1, 2]), leveraging the implicit function theorem to differentiate the ORCA $\operatorname*{arg\,min}\{\cdot\}$ as a function. We can even extend this approach to infer the other ORCA parameters from data as well. Our evaluation of ORCA rollouts as an open-loop prediction model (Sec. VI) has shown promising results, even with a rudimentary empirical goal sampling method and ORCA parameters fixed. We also intend to investigate incorporating uncertainty of the predicted ORCA parameters into the SICNav bilevel optimization, e.g. by following a robust bilevel optimization approach [45].

Evaluation Scope: In this work, we evaluate SICNav in simulation and on a real robot operating indoors. In our robot experiments, we use a VICON motion tracking system to obtain position measurements (mm error) of the humans head position in the scene, and use KFs to estimate their positions and velocities. The first limitation of this setup is that we would not have access to such motion tracking infrastructure in a deployment in the wild. Nonetheless, recent human motion estimation work (e.g.[46]) has shown promising results in estimating and predicting human motion from only image data to centimeter accuracy. Furthermore, our estimates of the human centroid did not meet the mm accuracy possible VICON: a second limitation of our experimental setup is that we only tracked the location of each agent’s head. Thus, our estimates of the person’s location were noisy when the person swayed or leaned, e.g. to initiate or stop walking. Previous work using images and lidar [47], has shown position tracking Root Mean Squared Error (RMSE) of less than 15cm for nearby agents. Toward the key next step of deploying SICNav on a real robot in the wild, we intend to evaluate our algorithm’s performance when using the onboard lidar and camera sensors to track the positions of the agents in the scene.

Number of Interactive Humans: In this work, we demonstrate the real-time (10Hz) performance of our algorithm in environments with up to $N=3$ humans modeled using ORCA for a horizon of $T=8$ time steps. The dimension of variables for the MPC optimization problem (7) associated with the KKT-reformulation scale quadratically in the number of humans times the control horizon, $O(N^{2}T)$. As a result, solving for control may become slower than real time for scenarios with many agents ($>>3$). In future work, we intend to limit the number of agents modeled interactively using ORCA to a neighborhood of the $3-4$ most ‘important’ humans each time we solve (7) and use the solution in receding horizon fashion, similar to [11, 6].