A Chance Constraint Predictive Control and Estimation Framework for Spacecraft Descent with Field Of View Constraints

The 6dof spacecraft dynamics model is the following

$$\dot{\xi}(t)=f(\xi(t),u(t))$$

(1)

with

$$\displaystyle\begin{bmatrix}\dot{\mathbf{r}}_{a}\\ \dot{\mathbf{v}}_{a}\\ \dot{\bm{\Theta}}\\ \dot{\bm{\omega}}_{b}\\ \end{bmatrix}=\begin{bmatrix}\mathbf{v}_{a}\\ \mathbf{u}+\mathbf{F}-2\mathbf{\Omega}\times\mathbf{v}_{a}-\mathbf{\Omega}\times(\mathbf{\Omega}\times\mathbf{r}_{a})\ \\ \mathbf{B}^{-1}\bm{\omega}_{b}\\ \mathbf{J}^{-1}(\mathbf{M}-\bm{\omega}_{b}\times\mathbf{J}\bm{\omega}_{b})\end{bmatrix}$$

(2)

where

$$\xi=[\mathbf{r}_{a}^{T}\ {\mathbf{v}_{a}}^{T}\ \bm{\Theta}^{T}\ \bm{\omega}_{b}^{T}]^{T}$$

(3)

$$u=[\mathbf{u}^{T}\ \mathbf{M}^{T}]^{T}$$

(4)

The translational component of the model is taken from [8], and rotational component based on an example in [9]. $\xi$ is the state vector. The spacecraft is modeled as a point mass with the only external force $\mathbf{F}$. The control $u$ is such that each degree of freedom has a control channel. The translational and attitude dynamics in (2) are decoupled. $\mathbf{B}=[R_{21}R_{1i}\mathbf{e}_{i}\ R_{1i}\mathbf{e}_{j}\ \ \mathbf{e}_{k}]$ where

	$$\displaystyle[\mathbf{e}_{i}\ \mathbf{e}_{j}\ \mathbf{e}_{k}]=\begin{bmatrix}1&0&0\\ 0&1&0\\ 0&0&1\\ \end{bmatrix}$$
	$$\displaystyle R_{ia}=\begin{bmatrix}\cos(n_{i}t)&-\sin(n_{i}t)&0\\ \sin(n_{i}t)&\cos(n_{i}t)&0\\ 0&0&1\\ \end{bmatrix}R_{1i}=\begin{bmatrix}\cos(\psi)&-\sin(\psi)&0\\ \sin(\psi)&\cos(\psi)&0\\ 0&0&1\\ \end{bmatrix}$$
	$$\displaystyle R_{21}=\begin{bmatrix}\cos(\theta)&0&\sin(\theta)\\ 0&1&0\\ -\sin(\theta)&0&\cos(\theta)\\ \end{bmatrix}R_{b2}=\begin{bmatrix}1&0&0\\ 0\ &\cos(\phi)&-\sin(\phi)\\ 0\ &\sin(\phi)&\cos(\phi)\\ \end{bmatrix}$$

The different types of measurements employed can be summarized in three main classes: 1: The straight line distance from the spacecraft to asteroid surface marker, known hereafter as a feature point, taken from lidar. 2: The location of feature points in the spacecraft body z-axis aligned camera field of view. 3: Direct angular velocity measurement $\bm{\omega}_{b}$, taken from IMU. The measurement model is designed to become inaccurate as the spacecraft body z-axis becomes misaligned with the feature points.

$$n(t)=[{\textbf{\emph{N}}_{1}(t)}^{T}\ ...\ {\textbf{\emph{N}}_{4}}(t)^{T}\ \emph{N}_{c}(t)\ {\textbf{\emph{N}}_{b}}(t)^{T}]^{T}$$

(5)

is the nosie vector with $\textbf{\emph{N}}_{1},...,\textbf{\emph{N}}_{4},\textbf{\emph{N}}_{b}\in\mathbb{R}^{3}$. $n(t)$ is a Gaussian random vector with zero mean and constant covariance $P$. Define $\mathbf{d}_{a}^{k}=\mathbf{r}_{a}-\mathbf{p}_{a}^{k}$ where $\mathbf{p}_{a}^{k}$ is the $k$th feature point location in the asteroid fixed frame and is known. Then $\mathbf{d}_{b}^{k}=R_{ba}\mathbf{d}_{a}^{k}$. The estimated quantity $\hat{\mathbf{d}}_{b}^{k}$ is

$\displaystyle\hat{\mathbf{d}}_{b}^{k}=\mathbf{d}_{b}^{k}+\bigg{(}1-\frac{-\mathbf{d}_{b}^{k}}{||\mathbf{d}_{b}^{k}||}\cdot\mathbf{e}_{k}\bigg{)}\textbf{\emph{N}}_{k}$

The quantity in parenthesis represents the following notion: the further off of the center of the field of view a feature point would register on the camera, the less ability there is to accurately assess and gimbal a range sensor to the feature point. From lidar, $d_{b}^{k}={||\hat{\mathbf{d}}_{b}^{k}||}_{2}$. Additionally, using the pinhole camera model provides a relation between the pixel location of the feature point on the spacecraft body z aligned camera, $\mathbf{c}=[c_{1}\ c_{2}]^{T}$, to the spatial location $\mathbf{d}_{b}$, with $f_{len}$ equal to the focal length of the camera. The measured quantity $\mathbf{c}^{k}$ is

$\displaystyle\mathbf{c}^{k}=\bigg{(}\frac{f_{len}}{[0\ 0\ 1]\hat{\mathbf{d}}_{b}^{k}}+\emph{N}_{c}\bigg{)}\begin{bmatrix}1\ 0\ 0\\ 0\ 1\ 0\\ \end{bmatrix}\hat{\mathbf{d}}_{b}^{k}$

The last class of measurement is $\hat{\bm{\omega}_{b}}=\bm{\omega}_{b}+\textbf{\emph{N}}_{b}$. $f_{len}$ is assumed to hold constant for a period of time. In summary,

	$$\displaystyle y(t)=h(t,\xi(t),n(t))$$		(6)
	$$\displaystyle y=\big{[}d_{b}^{1}\ ...\ d_{b}^{4}\ {\mathbf{c}^{1}}^{T}\ ...\ {\mathbf{c}^{4}}^{T}\ {\hat{\bm{\omega}_{b}}}^{T}\big{]}^{T}$$		(7)

A discrete Extended Kalman Filter is implemented with non-additive noise under the standard formulation to provide the estimation $\hat{\xi}$. The linearized state information used is given from:

$$A_{t}=\frac{\delta\hat{f}(t)}{\delta\xi(t)}\bigg{|}_{t,\hat{\xi}(t),u(t)}$$ (8) $$H_{t}=\frac{\delta h(t)}{\delta\xi(t)}\bigg{|}_{\begin{subarray}{c}t,\hat{\xi}(t)\\ n(t)=0\end{subarray}}$$ (9) $$V_{t}=\frac{\delta h(t)}{\delta N(t)}\bigg{|}_{\begin{subarray}{c}t,\hat{\xi}(t),\\ n(t)=0\end{subarray}}$$ (10)

The state error covariance after the filter is run at time $t$ is $\Sigma_{t}$, with $\hat{f}$ describing the spacecraft dynamics model with $\hat{\mathbf{F}}$ in place of $\mathbf{F}$.

The constraints to keep the feature points in field of view, and box constraints on the state, respectively, are

	$$\displaystyle||H_{t,fov}(\xi_{k|t}-\xi_{0|t})+h_{fov}(\xi_{0|t})||_{\infty}\leq s^{\prime}_{fov}$$		(13)
	$$\displaystyle||\xi_{k|t}||_{\infty}\leq s^{\prime}_{\xi}$$		(14)

which can be rewritten as $S[H_{t}^{T}\ I]^{T}\xi_{k|t}\leq s$, and $d$ is the number of output constraints, $d_{u}$ the number of control constraints, and $s=[s_{fov}^{T}\ s_{\xi}^{T}]^{T}$ with subscripts $fov$ denoting the rows of $H$ corresponding to outputs $\mathbf{c}^{1}...\mathbf{c}^{4}$, and subscript $\xi$ those applicable to box state constraints.

An accurate convexification strategy is needed to perform LQMPC due to the high nonlinearity of the measurement model seen in situations where the spacecraft descends close to the feature points. Successive convexification approaches such as those in [11] can admit solutions to opimality of a non-convex optimal control (OCP) problem. Such a non-convex OCP can be constructed with $f$ propogated dynamics as opposed to (8-9), however these types of algorithms take many iterations and Jacobian evaluations per time instant. Instead of the approach of solving the non-convex OCP, the constraints introduced aim to confine the system (11-12) such that the LQMPC law is acting on a suitably convex region of the system.

A stochastic MPC law with horizon $N$ has tightened constraints compared to the disturbance free case as a means of probabalistically constraining the stochastic system. For a set-valued constraint $Y$, this can be written with the P-difference $Y\ominus\mathcal{Y}^{\beta}$, with $\mathcal{Y}^{\beta}$ a confidence ellipsoid with some confidence parameter $\beta$. Control strategies applied to stochastic systems to treat constraints like reference governors [7] often use a prestabilized plant via a state feedback law to propogate estimate error uncertainty that approaches a steady state value with increase in $N$. In the reference governor approach of [7] there is no consideration of control constraints, which can render a state feedback law infeasible, whereas in the case of stochastic MPC an auxiliary decision variable $v$ can be used to achieve a control objective [10]. The control $u$ is $u=Ke+v$, where $e$ is the error between (11) and its disturbance free case used in the MPC problem, $\xi_{k+1|t}=A_{t}\xi_{k|t}+B_{t}u_{k|t}$, with $\hat{\xi}(t)=\xi_{0|t}$. At time $t$ and step $k$ in the MPC horizon, $e_{k|t}=\hat{\xi}(k+t)-\xi_{k|t}$. The closed-loop component $Ke$ is used with the confidence ellipsoid to formulate the tightened constraints.

$\Phi_{t}$ is the stabilized error state matrix $\Phi_{t}=A_{t}+B_{t}K_{t}$ and is strictly Schur. The system (11) can then be represented with a primal dynamics part (15) and an error dynamics part (16)

	$\displaystyle\zeta_{k+1|t}=A_{t}\zeta_{k|t}+B_{t}v_{k|t}$		(15)
	$\displaystyle e_{k+1|t}=\Phi_{t}e_{k|t}+G_{t}n(k+t)$		(16)

with $\xi_{k|t}=\zeta_{k|t}+e_{k|t}$. Consider the initial estimation error covariance $\Xi_{0|t}$ at each time taken as $\Xi_{0|t}=\Sigma_{t}$ and corresponding output error covariance $\Upsilon_{0|t}$. Over the MPC horizon they are propogated according to

	$$\displaystyle\Xi_{k+1|t}=\Phi_{t}\Xi_{k|t}\Phi_{t}^{T}+G_{t}PG_{t}^{T}$$		(17)
	$$\displaystyle\Upsilon_{k|t}=H_{t}\Xi_{k|t}H_{t}^{T}+V_{t}PV_{t}^{T}$$		(18)

with $e_{k|t}\sim\mathcal{N}(0,\Xi_{k|t})$ and similarly $\tilde{y}_{k|t}\sim\mathcal{N}(0,\Upsilon_{k|t})$, where $\tilde{y}_{k|t}=\hat{y}(k+t)-y_{k|t}$, which is the same estimation error uncertainty propogation as [7]. The strictly Schur property ensures $\Xi_{k|t}$ does not tend to infinity with increasing $N$, and allows for a choice of $\beta$ that does not reflect less confidence as $N\rightarrow\infty$ to accomodate the increasing uncertainty.

The output constraint at time $t$ is given as $\zeta_{k|t}\in\tilde{Y}_{N}^{\beta}=\cap_{k=0}^{N}Y_{k}^{\beta}$, where $Y_{k}^{\beta}$ is described in [7] for a prescribed $\beta$ under the assumption of a polytopic constraint set, and is given below, alongside the analogous input constraint at time $t$, $v_{k|t}\in\tilde{U}_{N}^{\beta}=\cap_{k=0}^{N}U_{k}^{\beta}$,

$$Y\ominus\mathcal{Y}_{k}^{\beta}=Y_{k}^{\beta}=\\ \{\xi\ |\ S^{T}_{i}[H_{t}^{T}\ I]^{T}\xi\leq=s_{i}-h_{\mathcal{Y}_{k}^{\beta}}(S_{i}),\ i=1,...,d\}\\ h_{\mathcal{Y}_{k}^{\beta}}(S_{i})=\begin{cases}\sqrt{F^{-1}(\beta,p)}\sqrt{S_{i}^{T}\ \Upsilon_{k|t}S_{i}}&\mbox{if }s_{i}\in s_{fov}\\ \sqrt{F^{-1}(\beta,p)}\sqrt{S_{i}^{T}\ \Xi_{k|t}S_{i}}&\mbox{if }s_{i}\in s_{\xi}\end{cases}$$

(19)

$$U\ominus\mathcal{U}_{k}^{\beta}=U_{k}^{\beta}=\\ \{u\ |\ M^{T}_{i}u\leq m_{i}-h_{\mathcal{U}_{k}^{\beta}}(M_{i}),\ i=1,...,d_{u}\}\\ h_{\mathcal{U}_{k}^{\beta}}(M_{i})=\sqrt{F^{-1}(\beta,p)}\sqrt{M_{i}^{T}\ K_{t}\Xi_{k|t}K_{t}^{T}M_{i}}$$

(20)

$Y=\{\xi\ |\ S_{i}^{T}[H_{t}^{T}\ I]^{T}\xi\leq s_{i},\ i=1,...,d\}$ is the feasible set of $\xi$ given the $d$ constraints (13-14), and $S^{T}_{i}$ the $i$th row of $S$. With a slight abuse of notation $s_{i}\in s_{fov}$ denotes $s_{i}$ is found in the $s_{fov}$ partition. $\mathcal{Y}_{k}^{\beta}=\{v\in\textrm{Im}\ \Upsilon_{k|t}\ |\ v^{T}\Upsilon_{k|t}^{+}v\leq\chi^{-1}(\beta,p)\}$ and is the confidence ellipsoid with confidence $\beta$ [7]. $h_{\mathcal{Y}_{k}^{\beta}}(S_{i})$ is the support function of $\mathcal{Y}_{k}^{\beta}$ at $S_{i}$. $U=\{u\ |\ M_{i}^{T}u\leq m_{i},\ i=1,...,d_{u}\}$.

A specific terminal constraint and terminal constraint tightening to address stability of the system used in the MPC formulation is not considered in this work, although the MPC formulation that follows tends the system towards equilibrium at each time $t$.

The MPC problem is reducable to a quadratic program allowing for fast simulation results. Redundant constraints resulting from (19,20) are also removed. MPC solves for the open loop control component $v$. The formulation is given as

	$$\displaystyle\min_{\delta v_{t},\epsilon}J(\delta\zeta_{0|t})=\sum_{k=0}^{N-1}(||\delta\zeta_{k|t}||_{Q}^{2}+||\delta v_{k|t}||_{R}^{2})+||\delta\zeta_{N|t}||_{Q}^{2}+{||\epsilon||}_{W}^{2}$$		(21a)
	$$\displaystyle s.t.\ \delta\zeta_{k+1|t}=A_{t}\delta\zeta_{k|t}+B_{t}\delta v_{k|t}$$		(21b)
	$$\displaystyle S[H_{t}^{T}\ I]^{T}\zeta_{k|t}\in Y^{MPC}+\epsilon$$		(21c)
	$$\displaystyle Mv_{k|t}\in U^{MPC}$$		(21d)

with $\epsilon$ slack variable to maintain feasibility such as in the case of empty $Y^{MPC}$, $U$ box constraints on the control channels with $m=[m_{trans}^{T}\ m_{rot}^{T}]^{T}$, $Q,R,W$ weights, and

$$\displaystyle Y^{MPC}=\begin{bmatrix}s_{1}-h_{\mathcal{Y}_{N}^{\beta}}(S_{1})\\ \vdots\\ s_{d}-h_{\mathcal{Y}_{N}^{\beta}}(S_{d})\end{bmatrix},\ U^{MPC}=\begin{bmatrix}m_{1}-h_{\mathcal{U}_{N}^{\beta}}(M_{1})\\ \vdots\\ m_{d_{u}}-h_{\mathcal{U}_{N}^{\beta}}(M_{d_{u}})\end{bmatrix}$$

where (21c, 21d) encode $y_{k|t}\in\tilde{Y}_{N}^{\beta}$ and $u_{k|t}\in\tilde{U}_{N}^{\beta}$ respectively.

$\delta\zeta_{k|t}=\zeta_{k|t}-\xi^{ref}_{t}$, $\delta v_{k|t}=v_{k|t}-u^{ref}_{t}$, with $(\xi^{ref}_{t},u^{ref}_{t})$ the equilibrium pair for the system (21b), with $\xi_{t}^{ref}$ specified before the computation of (21). $u^{ref}_{t}$ is computed as $u^{ref}_{t}=(B_{t}^{T}B_{t})^{-1}B_{t}(I-A_{t})\xi^{ref}_{t}$.