Risk Bounded Nonlinear Robot Motion Planning With Integrated Perception & Control

In an autonomous robot, the environmental state $\mathcal{Z}_{t}$ must be estimated with a perception system represented by a noisy on-board sensor measurements. We assume that a high-level perception system, such as Semantic SLAM described in [18] is in place and it processes high dimensional raw data $\Theta_{t}\in\mathbb{R}^{N_{\Theta}},N_{\Theta}\in\mathbb{N}$ to recognize the robot and the obstacles to produce noisy joint measurements of their respective states. In particular, the perception system recognizes the robot and obstacles through their distinctive features through mappings $\Upsilon_{x}:\mathbb{R}^{N_{\Theta}}\rightarrow\mathbb{R}^{n},\Upsilon_{\mathcal{O}}:\mathbb{R}^{N_{\Theta}}\rightarrow\mathbb{R}^{l}$, labels the entities accordingly and returns a noisy measurement about their position and orientation. We abstract this whole process and assume that the perception system produces noisy measurements of the robot and obstacle states using an assumed nonlinear output model

where $y_{t}\in\mathbb{R}^{p}$ is the output measurement and $\mathcal{S}:\mathbb{R}^{n_{z}}\rightarrow\mathbb{R}^{p}$ is a nonlinear function. The additive measurement noise $v_{t}\in\mathbb{R}^{p}$ is a zero-mean random vector independent and identically distributed across time according to some prescribed distribution $\mathbb{P}_{v}$ with covariance $\Sigma_{v}$.

There can be interactions between the uncertainties of the perception system and the process disturbance, due to coupling between robot and environmental states in the perception and the use of an output feedback control policy. Often, this interaction is ignored for the sake of simplicity and as a result can impose undue risk. To model the interaction, we assume that the process noise $w_{t}$ and the measurement noise $v_{t}$ are cross correlated with each other. That is, the joint noise $\begin{bmatrix}w_{t}\\ v_{t}\end{bmatrix}$ has zero mean with covariance

where $M=\mathbb{E}[w_{t}v^{\top}_{t}]\in\mathbb{R}^{n_{z}\times p}$ denotes the rank-one cross-correlation matrix across all time steps $t$ and the matrix $M\neq\mathbf{0}_{n_{z}\times p}$ is such that the joint noise covariance $\Sigma_{wv}$ given by (14) is positive definite.

The robot is nominally subject to constraints on the state and input of the form, $\forall t=0,\dots,T-1$,

where the $\mathcal{U}\subset\mathbb{R}^{m}$ are assumed to be convex polytope. We can rewrite the set description of the required sets using the robot inputs and environmental state as

where $b_{u}\in\mathbb{R}^{n_{u}},b_{0}\in\mathbb{R}^{n_{E}},b_{it}\in\mathbb{R}^{n_{i}},$ and $A_{u},A_{0},$ and $A_{i}$ are matrices of appropriate dimension. Suppose that $n_{ob_{i}}$ and $n_{env}$ be the number of constraints for obstacle $i\in\mathcal{B}$ and the environment $\mathcal{X}$ respectively. Then, the total number of constraints is denoted by

In this subsection, we elaborate about the Unscented Kalman Filter algorithm which serves as an estimator module for a perception system present in the autonomy stack shown in Figure 1. Later in subsection 4.2, we will describe how the estimates from UKF are then used to realize an output feedback based controller.

Noise Decorrelation. Before describing the UKF, we present a decorrelation scheme [21] to handle cross-correlation between process noise and sensor induced by the coupling between perception and control layers from (14). The scheme uses a pseudo state process equation to reconstruct a corresponding pseudo process noise, which is no longer correlated with the measurement noise. It is evident from (13) that

Thus the environmental dynamics given in (10) can be rearranged as follows

where $f^{*}(\mathcal{Z}_{t},u_{t}),w^{*}_{t}$ are the corresponding pseudo process dynamics and pseudo process noise respectively. The term $\mathcal{H}y_{t}$ is considered as a known input and is approximated with $\mathcal{H}\hat{y}_{t}$. The pseudo gain term $\mathcal{H}$ is a design parameter chosen such that the cross-correlation between the pseudo process noise and the sensor noise is made zero. That is,

The mean and the covariance of the pseudo process noise are then given by

Then, applying Schur complement on (14), it is easy to observe that $\Sigma^{*}_{w}$ is positive definite as well:

With the new pseudo process equation $f^{*}(\cdot)$ given by (25) and the measurement update given by (13), the standard Unscented Kalman Filter can be implemented as the new process noise $w^{*}_{t}$ is no longer cross-correlated with the sensor noise $v_{t}$.

The Unscented Kalman Filter. The UKF is a popular tool for nonlinear state estimation. It uses the so-called Unscented Transformation (UT) in both the propagation and update step, yielding derivative-free Kalman filtering for nonlinear systems. For Gaussian inputs, the moment estimates from UT are accurate up to the third order approximation and for the case of non-Gaussian, the approximations are accurate to at least the second-order as described in [22]. An ensemble of $2n_{z}+1$ samples called the sigma points following the Van Der Merwe algorithm is generated deterministically as follows:

where $\hat{Z}_{t-1},\hat{\Sigma}_{\mathcal{Z}_{t-1}}$ denote the posterior UKF estimate of mean and covariance of the environmental state at $t-1$ and $\left[\sqrt{(n_{z}+\lambda)\hat{\Sigma}_{\mathcal{Z}_{t-1}}}\right]_{i}$ is the $i^{th}$ row or column of the matrix square root $\sqrt{(n_{z}+\lambda)\hat{\Sigma}_{\mathcal{Z}_{t-1}}}$. The weights of the sigma points in the calculation of propagated mean and covariance are

The scaling parameter $\lambda=\alpha^{2}_{u}(n+\kappa)-n_{z}$, where $\alpha_{u},\beta_{u},\kappa$ are used to tune the unscented transformation. Then, every sigma point is propagated through non-linear state update equation to yield an ensemble of sigma points capturing the a priori statistics of $\mathcal{Z}_{t}$ namely $\mathcal{Z}^{-}_{t},\Sigma^{-}_{\mathcal{Z}_{t}}$. In the update step, we use the transformed set of sigma points described above and propagate them using the given nonlinear measurement model. Finally, the aposteriori state $\hat{Z}_{t}$ and covariance $\hat{\Sigma}_{\mathcal{Z}_{t}}$ are computed using the output residual and the obtained Unscented Kalman filter gain, $\mathcal{L}_{t}$ as follows

In this subsection, we elaborate about the feedback controller that represents an important module in the autonomy stack shown in Figure 1. Since the robot dynamics is both nonlinear and stochastic, we will be describing an output feedback based nonlinear model predictive control algorithm to steer the robots in the considered setting. Specifically, the steering law for steering the robots is realized through a multiple shooting-based nonlinear model predictive controller [19], [20] that uses the state estimate obtained through an Unscented Kalman filter at each time step. An illustration of the output feedback is shown in Figure 2.

The error at a time step $t$ is

where $x_{s}$ represents a sample in the free space to be steered to. At every time instance $t$, the nonlinear model predictive controller repeatedly solves the following optimization problem in a receding horizon fashion with prediction horizon $N_{t}$ to find a control input sequence as follows

where only the first control input in the optimal sequence $u^{\dagger}_{t}$ is applied at time $t$ and the horizon is shifted to $t+1$ for the problem to be solved again. Here, $Q,R$ denote the state penalty and control penalty matrices respectively.

In order to efficiently explore the reachable set of the dynamics and increase the likelihood of generating collision-free trajectories, authors in [23] described the limitations of the standard Euclidean distance metric and rather advocated the use of an optimal “cost-to-go” metric, that takes into account a dynamics and control-related quantities to steer the robot. When the dynamics are linear and obstacles are ignored, the optimal “cost-to-go” between any two nodes in the tree can be computed using dynamic programming. However, with nonlinear dynamics and potentially non-holonomic constraints, cost-to-go can be tailored to the specific robot dynamics. Inspired by [24], we present an approximate cost-to-go for some special robot representations and car-like dynamics. The initial pose of a robot, $p_{0}$ can be defined using a tuple $(x,y,\psi)$, where $(x,y)$ are the positions in global coordinate frame and $\psi$ being its orientation with respect to the positive $x$ axis. We use an egocentric polar coordinate system to describes the relative location of the target pose $p_{T}$ observed from the robot with radial distance $r$ along the line of sight vector from the robot to the target, orientation of the target $\phi$, and orientation of the robot $\delta$, where angles are measured from the line of sight vector as shown in the Figure 3.

Here, $k_{\phi}>0$ is a constant that represent the weight of $\phi$ with respect to the radial distance $r$ and $k_{\delta}>0$ is another constant that emphasizes the weight on $|\delta|$ as $\delta$ can take both positive and negative values. The non-holonomic distance in (45) is asymmetric meaning that $\mathfrak{D}(p_{0},p_{T})$ is not equal to $\mathfrak{D}(p_{T},p_{0})$.

In the first step, using the Sample$(\cdot)$ function, a random pose $x_{rand}$ is sampled from the free space $\mathcal{X}^{\texttt{free}}_{t}$. Then the tree node, $N_{nearest}$ that is nearest to the sampled pose is selected using the NearestNode$(\cdot)$ function (line 3 of Algorithm 1) which uses the “cost-to-go” distance metric defined in (45). Attempts are then made to steer the robot from the nearest tree node to the random sample using the Steer$(\cdot)$ function that employs the steering law explained in Section 4 (line 4). The control policy obtained is then used to propagate the state mean and covariance, and the entire trajectory $(\bar{\sigma},\bar{\Pi})$ is returned by the Steer$(\cdot)$ function. Using the DR-Feasible$(\cdot)$ function, each state distribution in the trajectory is checked for distributionally robust probabilistic constraint satisfaction discussed in the next section. Further, the line connecting subsequent state distributions in the trajectory are also checked for collision with the obstacle sets $\mathcal{O}_{it},i\in\mathcal{B}$. An outline of the DR-Feasible subroutine is shown in Algorithm 2. If the entire trajectory $(\bar{\sigma},\bar{\Pi})$ is probabilistically feasible, a new node $N_{min}$ with that distribution sequence $(\bar{\sigma},\bar{\Pi})$ is created (line 7) but not yet added to $\mathcal{T}$. Instead, nearby nodes are identified for possible connections via the NearNodes$(\cdot)$ function (line 8), which returns a subset of nodes $\mathcal{N}_{near}\subseteq\mathcal{T}$, if they are within a search radius ensuring probabilistic asymptotic optimality guarantees specified in [25]. The search radius is given by

where $\mathcal{N}_{t}$ refers to the number of nodes in the tree at time $t$, the positive scalar $\mu_{max}$ is the maximum radius specified by the user, and $\gamma$ refers to the planning constant based on the $d$ dimensional environment and is selected using [25, Theorem 1]. Then we seek to identify the lowest-cost, probabilistically feasible connection from the $\mathcal{N}_{near}$ nodes to $x_{rand}$ (lines 10-14). For each possible connection, a distribution sequence is simulated via the steering law (line 11). If the resulting sequence is probabilistically feasible, and the cost of that node represented as $c_{rand}=J[N_{near}]+\Delta J(\bar{\sigma},\bar{\Pi}$, is lower than the cost of $N_{min}$ denote by $J[N_{min}]$, then a new node with this sequence replaces $N_{min}$ (line 14). The lowest-cost node is ultimately added to $\mathcal{T}$ (line 15). Finally, edges are rewired based on attempted connections from the new node $N_{min}$ to nearby nodes $\mathcal{N}_{near}$ (lines 17-22), ancestors excluded (line 17) which are found using Ancestors$(\cdot)$ function. A distribution sequence is simulated via the steering law from $N_{min}$ to the terminal state of each nearby node $N_{near}\in\mathcal{N}_{near}$ (line 18). If the resulting sequence is probabilistically feasible, and the cost of that node $c_{min}$ is lower than the cost of $N_{near}$ given by $J[N_{near}]$ (line 19), then a new node with this distribution sequence replaces $N_{near}$ (lines 21-22). The tree expansion procedure is then repeated until a node from the goal set is added to the tree. At that point, a distributionally robust feasible trajectory is obtained from the tree root to $\mathcal{X}_{goal}$.

Unlike most stochastic motion planning algorithms that often assume a functional form (often Gaussian) for probability distributions to model uncertainties, we will focus here on uncertainty modeling using moment-based ambiguity sets. Since the initial environmental state $\mathcal{Z}_{0}$ is not assumed to be Gaussian, then neither are the environmental state distributions $\mathbb{P}_{\mathcal{Z}_{t}}$ at any point of future time $t$. The ambiguity set defining the true environmental state is

To study the stage risk constraints (21) for the safety of the planned reference trajectory, we follow the steps similar to the one described in [16]. Let $\mathfrak{P}_{Safe}$ denote the event that plan $\mathfrak{P}$ succeeds and $\mathfrak{P}_{Fail}$ as the complementary event (i.e. failure). Given a confidence $\beta\in(0,0.5]$, we consider the specification that a plan succeeds with high probability or equivalently that it fails with low probability. That is,

Failure of the total plan requires at least one stage risk constraints to be violated. Then, $\forall t\in[0:T]$,

Applying Boole’s law, probability of the success event can be bounded as