Soft landing guidance¶
$$ \newcommand{\eg}{{\it e.g.}} \newcommand{\ie}{{\it i.e.}} \newcommand{\argmin}{\operatornamewithlimits{argmin}} \newcommand{\mc}{\mathcal} \newcommand{\mb}{\mathbb} \newcommand{\mf}{\mathbf} \newcommand{\minimize}{{\text{minimize}}} \newcommand{\diag}{{\text{diag}}} \newcommand{\cond}{{\text{cond}}} \newcommand{\rank}{{\text{rank }}} \newcommand{\range}{{\mathcal{R}}} \newcommand{\null}{{\mathcal{N}}} \newcommand{\tr}{{\text{trace}}} \newcommand{\dom}{{\text{dom}}} \newcommand{\dist}{{\text{dist}}} \newcommand{\R}{\mathbf{R}} \newcommand{\SM}{\mathbf{S}} \newcommand{\ball}{\mathcal{B}} \newcommand{\bmat}[1]{\begin{bmatrix}#1\end{bmatrix}} $$ ASE7030: Convex Optimization, Inha University.
Jong-Han Kim (jonghank@inha.ac.kr)
Jiwoo Choi (jiwoochoi@inha.edu)
We consider a soft landing problem for a rocket. The particle mass dynamics for each coordinate in ENU (East-North-Up) frame is governed by
$$ \begin{aligned} \dot{v_e} &= u_e - \gamma v_e \\ \dot{p_e} &= v_e \\ \dot{v_n} &= u_n - \gamma v_n \\ \dot{p_n} &= v_n \\ \dot{v_u} &= u_u - \gamma v_u - g\\ \dot{p_u} &= v_u \end{aligned} $$Denoting the velocity, position, gravity vector in ENU frame by $v=(v_e,v_n,v_u)$, $p=(p_e,p_n,p_u)$, $g=(0,0,-g)$, the damping coefficient by $\gamma$, the trapezoidal integration assuming constant acceleration during the sampling interval gives,
$$ \begin{aligned} v_{t+1} &= v_t + {h}\left( u_t - \gamma v_t -g \right) \\ &= \left(1-\gamma h\right) v_t + h u_t - hg \\ p_{t+1} &= p_t + \frac{h}{2}\left( v_t + v_{t+1} \right) \\ &= p_t + \frac{h}{2}\left( v_t + \left(1-\gamma h\right) v_t + h u_t \right) \\ &= p_t + \left(h-\frac{1}{2}\gamma h^2\right) v_t + \frac{1}{2} h^2 u_t \end{aligned} $$Then with $x = (p_e,p_n,p_u,v_e,v_n,v_u)$ the above dynamics can be coded into the following:
$$ \begin{aligned} x_{t+1} &= Ax_t + Bu_t \end{aligned} $$In other words,
$$ \bmat{p_e \\ p_n \\ p_u \\ v_e \\ v_n \\ v_u }_{t+1} = \underbrace{\bmat{ 1 & 0 & 0 & \left(1-0.5\gamma h\right)h & 0 & 0 \\ 0 & 1 & 0 & 0 & \left(1-0.5\gamma h\right)h & 0 \\ 0 & 0 & 1 & 0 & 0 & \left(1-0.5\gamma h\right)h \\ 0 & 0 & 0 & 1-\gamma h & 0 & 0 \\ 0 & 0 & 0 & 0 & 1-\gamma h & 0 \\ 0 & 0 & 0 & 0 & 0 & 1-\gamma h }}_{A} \bmat{p_e \\ p_n \\ p_u \\ v_e \\ v_n \\ v_u}_t + \underbrace{\bmat{ 0.5h^2 & 0 & 0 \\ 0 & 0.5h^2 & 0 \\ 0 & 0 & 0.5h^2 \\ h & 0 & 0 \\ 0 & h & 0 \\ 0 & 0 & h }}_{B} \bmat{u_e \\ u_n \\ u_u }_t + \underbrace{\bmat{0 \\ 0 \\ -0.5h^2g \\ 0 \\ 0 \\ -hg}}_{b} $$The rocket is initially at $x_0 = (500,200,2000,40,-10,-300)$, and we consider a finite horizon of $T=20$ with $N=200$.
In [1]:
import numpy as np
import matplotlib.pyplot as plt
import scipy.sparse as ssp
import scipy.sparse.linalg as sla
from scipy.linalg import sqrtm
N = 200 # number of timesteps
T = 20 # time will vary from 0 to T with stepsize h
ts = np.linspace(0,T,N+1)
h = T/N
gamma = .05 # damping, 0 is no damping
g = 9.8
n = 6 # state size
m = 3 # control size
I3 = ssp.eye(3)
A = ssp.bmat([[I3, (1-gamma*h/2)*h*I3], \
[None, (1-gamma*h)*I3]])
B = ssp.bmat([[h**2/2*I3], \
[ h*I3]])
b = np.array([0,0,-0.5*g*h**2,0,0,-g*h])
x_0 = np.array([500,200,2000,40,-10,-300])
We formulate the soft landing problem via linear quadratic regulator design problem. With the design parameters $Q_1,\dots,Q_N \ge 0$ and $R_0,\dots,R_{N-1}>0$, the LQR problem can be stated as follows (we can omit the first stage cost term $x_0^TQ_0 x_0$).
$$ \begin{aligned} \underset{u,x}{\minimize} \quad & \sum_{t=0}^{N-1} \left( x_t^TQ_tx_t + u_t^TR_tu_t \right) + x_N^TQ_Nx_N \\ \text{subject to} \quad & x_{t+1} = Ax_t + Bu_t + b, \quad\forall t=0,\dots,N-1, \end{aligned} $$We define time-varying $Q$ such that $$ Q_t = \bmat{ 10e^{-(N-t)/18} \\ & 10e^{-(N-t)/18} \\ & & 0 \\ & & & 100e^{-(N-t)/18} \\ & & & & 100e^{-(N-t)/18} \\ & & & & & 0 } $$ for $t=0,\dots,N-1$ along with $$ Q_N = \bmat{ 10 \\ & 10 \\ & & 100 \\ & & & 100 \\ & & & & 100 \\ & & & & & 1000 } $$
and $R_t=I$ for all $t$.
In [12]:
Q = []
R = []
Q_tilde = []
R_tilde = []
for t in range(N):
Qt = np.exp(-(N-t)/18)*ssp.diags([10,10, 0,100,100, 0])
Rt = ssp.eye(m)
Q.append(Qt)
R.append(Rt)
Q.append(ssp.diags([ 10,10,100,100,100,1000]))
labels = [r'$p_e$',r'$p_n$',r'$p_u$',r'$v_e$',r'$v_n$',r'$v_u$',
r'$u_e$',r'$u_n$',r'$u_u$']
plt.figure(figsize=(14,8), dpi=100)
for i in range(6):
plt.subplot(2,3,i+1)
plt.stairs([q.toarray()[i,i] for q in Q], label=rf'$Q({i+1},{i+1})$: penalty on {labels[i]}')
plt.legend()
plt.grid()
plt.xlabel(r'$t$')
plt.show()
In [3]:
# draw plots
def draw_plots(t1,x1,u1,t2=[],x2=[],u2=[]):
# t1: time, (N+1,)-vector
# x1: state trajectory, 6x(N+1) matrix
# u1: control vector, 3xN matrix
# t2, x2, u2: optional for comparing two trajectories, size same as above
labels = [r'$p_e$',r'$p_n$',r'$p_u$',r'$v_e$',r'$v_n$',r'$v_u$',
r'$u_e$',r'$u_n$',r'$u_u$']
plt.figure(figsize=(14,9), dpi=100)
for i in range(6):
plt.subplot(3,3,i+1)
plt.plot(t1,x1[i],label=labels[i])
if len(t2):
plt.plot(t2,x2[i])
plt.legend()
plt.grid()
for i in range(3):
plt.subplot(3,3,i+7)
plt.plot(t1[:-1],u1[i],label=labels[i+6])
if len(t2):
plt.plot(t2[:-1],u2[i])
plt.xlabel(r'$t$')
plt.legend()
plt.grid()
plt.show()
return
# draw 3D plot
def draw_3d_traj(x1,u1,x2=[],u2=[]):
# x1: state trajectory, 6x(N+1) matrix
# u1: control vector, 3xN matrix
# x2, u2: optional for comparing two trajectories, size same as above
fig = plt.figure(figsize=(10,10), dpi=100)
ax = fig.add_subplot(projection='3d')
ax.view_init(elev=10, azim=-75, roll=0)
_ = 10
N = x1.shape[1]-1
ax.plot(*x1[:3,:])
if len(x2):
ax.plot(*x2[:3,:])
for i in range(0, N, 10):
__ = np.linalg.norm(u1[:,i])/200
ax.quiver(x1[0,i],x1[1,i],x1[2,i],-u1[0,i]*_,-u1[1,i]*_,-u1[2,i]*_, \
color='magenta', linewidths=1, arrow_length_ratio = 0)
ax.quiver(x1[0,i],x1[1,i],x1[2,i],u1[0,i]/__,u1[1,i]/__,u1[2,i]/__, \
color='gray', linewidths=1, arrow_length_ratio = 0)
if len(x2):
__ = np.linalg.norm(u2[:,i])/200
ax.quiver(x2[0,i],x2[1,i],x2[2,i],-u2[0,i]*_,-u2[1,i]*_,-u2[2,i]*_, \
color='red', linewidths=1, arrow_length_ratio = 0)
ax.quiver(x2[0,i],x2[1,i],x2[2,i],u2[0,i]/__,u2[1,i]/__,u2[2,i]/__, \
color='black', linewidths=1, arrow_length_ratio = 0)
th = np.linspace(0,2*np.pi)
ax.plot(100*np.cos(th),100*np.sin(th),0,alpha=0.5,color='green')
ax.plot(200*np.cos(th),200*np.sin(th),0,alpha=0.5,color='green')
ax.set_xlabel(r'$p_e$')
ax.set_ylabel(r'$p_n$')
ax.set_zlabel(r'$p_u$')
ax.axis('equal')
plt.show()
return
In [4]:
# make 3D animation
!pip install celluloid
from celluloid import Camera
from matplotlib import rc
def make_3d_anim(x1,u1,x2=[],u2=[],playback_speed=2):
# x1: state trajectory, 6x(N+1) matrix
# u1: control vector, 3xN matrix
# x2, u2: optional for comparing two trajectories, size same as above
fig = plt.figure(figsize=(10,10), dpi=100)
ax = fig.add_subplot(projection='3d')
ax.view_init(elev=10, azim=-75, roll=0)
_ = 10
N = x1.shape[1]-1
camera = Camera(fig)
for i in range(0,N,playback_speed):
ax.plot(*x1[:3,:], alpha=0.2)
if len(x2):
ax.plot(*x2[:3,:], alpha=0.2)
__ = np.linalg.norm(u1[:,i])/200
ax.plot(x1[0,:i+1],x1[1,:i+1],x1[2,:i+1])
ax.quiver(x1[0,i],x1[1,i],x1[2,i],-u1[0,i]*_,-u1[1,i]*_,-u1[2,i]*_, \
color='magenta', linewidths=2, arrow_length_ratio = 0)
ax.quiver(x1[0,i],x1[1,i],x1[2,i],u1[0,i]/__,u1[1,i]/__,u1[2,i]/__, \
color='gray', linewidths=4, arrow_length_ratio = 0)
if len(x2):
__ = np.linalg.norm(u2[:,i])/200
ax.plot(x2[0,:i+1],x2[1,:i+1],x2[2,:i+1])
ax.quiver(x2[0,i],x2[1,i],x2[2,i],-u2[0,i]*_,-u2[1,i]*_,-u2[2,i]*_, \
color='red', linewidths=2, arrow_length_ratio = 0)
ax.quiver(x2[0,i],x2[1,i],x2[2,i],u2[0,i]/__,u2[1,i]/__,u2[2,i]/__, \
color='black', linewidths=4, arrow_length_ratio = 0)
th = np.linspace(0,2*np.pi)
ax.plot(100*np.cos(th),100*np.sin(th),0,alpha=0.5,color='green')
ax.plot(200*np.cos(th),200*np.sin(th),0,alpha=0.5,color='green')
ax.set_xlabel(r'$p_e$')
ax.set_ylabel(r'$p_n$')
ax.set_zlabel(r'$p_u$')
ax.axis( 'equal')
camera.snap()
plt.close()
anim = camera.animate(blit=False, interval=100)
rc('animation', html='jshtml')
return anim
Requirement already satisfied: celluloid in /usr/local/lib/python3.11/dist-packages (0.2.0) Requirement already satisfied: matplotlib in /usr/local/lib/python3.11/dist-packages (from celluloid) (3.10.0) Requirement already satisfied: contourpy>=1.0.1 in /usr/local/lib/python3.11/dist-packages (from matplotlib->celluloid) (1.3.1) Requirement already satisfied: cycler>=0.10 in /usr/local/lib/python3.11/dist-packages (from matplotlib->celluloid) (0.12.1) Requirement already satisfied: fonttools>=4.22.0 in /usr/local/lib/python3.11/dist-packages (from matplotlib->celluloid) (4.56.0) Requirement already satisfied: kiwisolver>=1.3.1 in /usr/local/lib/python3.11/dist-packages (from matplotlib->celluloid) (1.4.8) Requirement already satisfied: numpy>=1.23 in /usr/local/lib/python3.11/dist-packages (from matplotlib->celluloid) (2.0.2) Requirement already satisfied: packaging>=20.0 in /usr/local/lib/python3.11/dist-packages (from matplotlib->celluloid) (24.2) Requirement already satisfied: pillow>=8 in /usr/local/lib/python3.11/dist-packages (from matplotlib->celluloid) (11.1.0) Requirement already satisfied: pyparsing>=2.3.1 in /usr/local/lib/python3.11/dist-packages (from matplotlib->celluloid) (3.2.1) Requirement already satisfied: python-dateutil>=2.7 in /usr/local/lib/python3.11/dist-packages (from matplotlib->celluloid) (2.8.2) Requirement already satisfied: six>=1.5 in /usr/local/lib/python3.11/dist-packages (from python-dateutil>=2.7->matplotlib->celluloid) (1.17.0)
Soft landing LQR by least squares¶
(Problem 1) Find the optimal maneuver acceration for the above LQR problem via least squares solutions. Report the terminal position and the terminal velocity at touchdown.
In [13]:
##################
# your code here
# formulation
# solution
# simulation
# your code here
##################
terminal position: [ 0.06401034 0.0114091 -0.17587069] terminal velocity: [-0.00075494 -0.00013456 -0.10731695]
In [14]:
# draw plots
draw_plots(ts,x_ls,u_ls)
draw_3d_traj(x_ls,u_ls)
In [15]:
# make 3D animation
make_3d_anim(x_ls,u_ls,playback_speed=2)
Out[15]: