HW2(3) 基于MPC的火箭软着陆问题

HW2(3) 基于MPC的火箭软着陆问题

题目需求

在本问题中，我们将比较TILQR（时不变LQR）与QP-based MPC控制器在火箭发射器的软着陆问题上的性能比较。（着陆问题是一个stablization问题，一般用时不变LQR）

流程：

• 系统动力学与离散化
• 参考轨迹设计
• LQR控制器设计
• MPC控制器设计
  • 无约束
  • 有约束
  • 改变Horizon长度

系统动力学与离散化

为了简化处理，我们仅考虑一个平面版本的火箭着陆问题，通过调整推力的大小和方向来控制火箭的位移和旋转，其系统方程为
$$\begin{array}{r}x=\left[\begin{array}{c}{p}_x\\ p_z\\ \theta \\ v_x\\ v_z\\ \omega \\ T\\ \varphi \end{array}\right],u=\left[\begin{array}{c}\dot{T}\\ \dot{\varphi }\end{array}\right],\dot{x}=\left[\begin{array}{c}{v}_z\\ v_z\\ \omega \\ \frac{T}{m}\cos (\theta +\varphi )\\ \frac{T}{m}\sin (\theta +\varphi )\\ \frac{T}{2J}L\sin (\varphi )\\ \dot{T}\\ \dot{\varphi }\end{array}\right]\end{array}\text{\hspace{1em}(1)}$$
其中$p_x{p}_z\theta$分别是水平位移、竖直位移和倾角，$v_x{v}_{y}w$是前者的速度，$T$是总推力，$\varphi$是推力角度，输入变量是两者的变化量。为了保证控制系统稳定、安全，应该将总推力限制为75%–150%的自身重力，推力角限制在$\pm$10度，火箭倾角限制在$\pm$5度（这样限制的原因也是为了防止系统离线性化点太远，则根据线性化模型算出来的最优控制率就会变得不准确）。
$$\begin{array}{rl}|\theta |& \le 5^{\circ }\\ |\varphi |& \le 10^{\circ }\\ z& \ge 0\\ 0.75& \le \frac{1}{mg}T\le 1.5\end{array}\text{\hspace{1em}(2)}$$
将系统在平衡点线性化后，设计对应的MPC或LQR稳定控制率。

model = PlanarRocket(max_roll=5.0) # 动力学模型来自RobotZoo库
n,m = state_dim(model), control_dim(model)
xeq = [zeros(6); model.m*model.g; 0] # 设置线性化点
ueq = zeros(2) 
norm(dynamics(model, xeq, ueq)) ≈ 0 # make sure it's an equilibrium point
dt = 0.1  # time step (s)
tf = 25   # time horizon (s)
N = Int(tf / dt) + 1

# Evaluate the contimous and discrete Jacobians
zeq = KnotPoint(xeq,ueq,dt)   # create a `KnotPoint` type that stores everything together
∇f = RobotDynamics.DynamicsJacobian(model) #获得雅可比矩阵函数J(z)
jacobian!(∇f, model, zeq) # 给定特定点求雅可比矩阵的值
discrete_jacobian!(RK4, ∇f, model, zeq) # 离散化
# Extract pieces of the Jacobian
A = ∇f.A 
B = ∇f.B;

在本次作业中，LQR和MPC的权重矩阵设计使用了Bryson’s Rule，它避免了LQR问题参数整定的频繁trail and error method，可以作为一个较好的初始值使用Q R，它指出，Q R矩阵可以指定成最大容许状态误差的和最大容许输入的平方倒数。

在本例中，
$$\begin{array}{rl}Q& =diag[\frac{1}{5^2},\frac{1}{5^2},\frac{1}{(5\pi /180{)}^2},\frac{1}{5^2},\frac{1}{5^2},\frac{1}{(5\pi /180{)}^2},\frac{1}{(0.1mg{)}^2},\frac{1}{(10\pi /180{)}^2}]\\ R& =diag[\frac{1}{(20mg{)}^2},\frac{1}{(10\pi /180{)}^2}]\end{array}\text{\hspace{1em}(3)}$$

参考轨迹设计

我们使用简单的线性插值来设计一个参考轨迹，

function nominal_trajectory(x0,N,dt)
    Xref = [zero(x0) for k = 1:N] #初始化变量
    pos = range(x0[1:3], zeros(3), length=N) #对x y theta线性插值
    # TODO: Design a trajectory that linearly interpolates from x0 to the origin
    for i=1:(N-1)
        Xref[i][1:3] = pos[i]
        Xref[i][4:6] = (pos[i+1]-pos[i]) / dt
        Xref[i][7:8] = x0[7:8] # 推力的角度即保持之前值，其实线性插值是一样的，因为起点终点都不变，都是在平衡点处
    end
    Xref[end] = Xref[N-1]
    # Convert to static arrays for plotting
    return SVector{8}.(Xref)
end

x0_ref = [-200, 500, 0, 0, 0, 0, 0, 0.] + xeq # 起点在位置为-200，500，注意，其他量不是全0，推力作为一种状态是有值的

# Generate reference trajectory
Xref = nominal_trajectory(x0_ref,N,dt) 
Uref = [copy(ueq) for k = 1:N]	# Uref是用来算Cost的，参考值是悬停态（平衡态）的输入值
tref = range(0,tf, length=N)

设计LQR控制器

有了系统平衡点的线性模型、优化问题的Q R和参考轨迹之后，设计一个LQR控制器来将系统稳定到平衡点就是个简单的问题了（只要别离平衡点太远）。从HW2(1) 中可以看出，在stablization问题中，无限时域的LQR往往比有限时域的LQR的表现更好，因此我们在本问题中也使用无限时域的LQR控制器。

运行结果如下，从结果中可以看出，LQR控制器在着陆时，甚至陷进去地面去了，因此肯定是实际不可行的。

function get_control(ctrl::LQRController, x, t)
    u = zero(ctrl.Uref[1])
    k = get_k(ctrl, t)
    u = ctrl.Uref[k] - ctrl.K * (x - ctrl.Xref[k])
    # 这里形式与前面作业中HW2(1)或Lecture 9还不同，前面我们取Uref[k] = U_balance Xref[k] = X_balance
    # 而这里我们给每一个中间时刻都设计一个参考轨迹点，防止初始时候误差太大，导致系统的输出太离谱
    return u
end
function lqr(A,B,Q,R; P=Matrix(Q), tol=1e-8, max_iters=400, verbose=false)
    n,m = size(B) # 初始化
    K = zeros(m,n)
    
    K = dlqr(A,B,Q,R)  # 偷懒，调库求解，具体实现方式参考HW2(1)(2)
    P = dare(A,B,Q,R)

    return K,P
end

MPC控制器

MPC控制器与Lecture 9的形式基本相同，最大的区别在于，这里每个轨迹点的参考点是不同的，并不都是在平衡点，而是一个线性插值的参考轨迹（MPC运行过程中，时间窗口一直往前，因此参考轨迹也沿着轨迹曲线一直往前取，以步长N索引，若窗口终点超过了参考轨迹的终点，则后面的参考都取在参考轨迹的终点，表示要稳定在终点），所有的参考输入都是悬停输入。

将MPC问题转换成一个QP问题，
$$\begin{array}{rlrl}& {\text{minimize}}_z& & \frac{1}{2}{z}^{T}Pz+q^{T}z\\ & \text{subject to}& & Dz=d\\ & & & Cz\le d\end{array}\text{\hspace{1em}(4)}$$
其中，$\bar{x}$是参考轨迹，其中当$x=\bar{x}$时取Cost取最小值。因为整个系统都是在悬停态平衡点点线性化的，所以都要减去平衡点的值。
$$\begin{array}{r}\left[\begin{array}{ccccccc}B& -I& & & & & \\ & A& B& -I& & & \\ & & & & \ddots & & \\ & & & & & A& B-I\end{array}\right]\left[\begin{array}{c}{u}_1\\ x_2\\ u_2\\ ⋮\\ x_{N-1}\\ u_{N-1}\\ x_N\end{array}\right]=\left[\begin{array}{c}-A(x_1-x_{eq})\\ 0\\ ⋮\\ 0\end{array}\right]\end{array}\text{\hspace{1em}(5)}$$

$$\begin{array}{r}P=\left[\begin{array}{ccccc}R& & & & \\ & Q& & & \\ & & \ddots & & \\ & & & R& \\ & & & & Q_f\end{array}\right],q=\left[\begin{array}{c}-R({\bar{u}}_1-u_{eq})\\ -Q({\bar{x}}_2-x_{eq})\\ ⋮\\ -R({\bar{u}}_{N-1}-u_{eq})\\ -Q_f({\bar{x}}_N-x_{eq})\end{array}\right]\end{array}\text{\hspace{1em}(6)}$$

本问题中$\bar{u}=u_{eq}$，所以$q$中不包含有$R$项

代码主要包含两个函数，初始化QP，和更新QP，初始化QP主要是写$PDC$矩阵的值，在整个MPC更新过程都不改变，而随着时间的往前，参考轨迹不断变化，因此要修改$d$来设定起始点与参考点。

function buildQP!(ctrl::MPCController, A,B,Q,R,Qf; kwargs...)
    for j = 1:(N-2)	# N是MPC的前瞻步长，Horizon大小 n是状态维度 m是输入维度
        ctrl.P[((j-1)*(n+m)).+(1:m),((j-1)*(n+m)).+(1:m)] .= R     # (j-1)*(n+m)代表跳过前j-1个（u x）变量 （u1与x2为一对）
        ctrl.P[(m+(j-1)*(n+m)).+(1:n),(m+(j-1)*(n+m)).+(1:n)] .= Q # (j-1)*(n+m)+m代表跳过跳过前j-1个（x u）变量+本次的u
    end	
    ctrl.P[((N-2)*(n+m)).+(1:m),((N-2)*(n+m)).+(1:m)] .= R		# R为m*m，Q为n*n
    ctrl.P[(m+(N-2)*(n+m)).+(1:n),(m+(N-2)*(n+m)).+(1:n)] .= Qf	# Qf一般与Q不一样，因此要单独设置
    
    ctrl.A[1:n, 1:m] = B			# 这个A矩阵指的是QP中的约束矩阵C和D
    ctrl.A[1:n, m.+(1:n)] = -I(n)	# C矩阵的第一行与其他行不一样，要单独设置
    
    for j = 1:(N-2)	# C矩阵的列是固定的一定是(m+n)*(N-1)列
        # n+代表跳过第一个特殊的x1. A是与第j个x相乘，其索引为m+(j-1)*(n+m) m+代表跳过u1
        ctrl.A[(n+(j-1)*n).+(1:n),(m+(j-1)*(n+m)).+(1:n)] .= A 	
        # n+代表跳过第一个特殊的x1. B是与第j个u相乘，其索引为m+(j-1)*(n+m)+n)，代表跳过u1和本次的x
        ctrl.A[(n+(j-1)*n).+(1:n),(m+(j-1)*(n+m)+n).+(1:m)] .= B 
        ctrl.A[(n+(j-1)*n).+(1:n),(m+(j-1)*(n+m)+n+m).+(1:n)] .= -I(n)
    end
    
    if size(ctrl.A,1) > (N-1)*n   # A矩阵是约束矩阵，正常来讲只有系统方程约束，则只有(N-1)*n行，若有其他约束，则会大于这个值
        for i = 1:N-1 # 下面这一段填的是所有状态的约束 m代表跳过u1，(i-1)*(n+m)跳过前i对
            ctrl.A[(N-1)*n+(i-1)*4+1, m+3+(i-1)*(n+m)] = 1  #x(3)
            ctrl.lb[(N-1)*n+(i-1)*4+1] = -5/180.0*pi
            ctrl.ub[(N-1)*n+(i-1)*4+1] =  5/180.0*pi
            
            ctrl.A[(N-1)*n+(i-1)*4+2, m+8+(i-1)*(n+m)] = 1  #x(8)
            ctrl.lb[(N-1)*n+(i-1)*4+2] = -10/180.0*pi
            ctrl.ub[(N-1)*n+(i-1)*4+2] =  10/180.0*pi
            
            ctrl.A[(N-1)*n+(i-1)*4+3, m+2+(i-1)*(n+m)] = 1  #x(2)
            ctrl.lb[(N-1)*n+(i-1)*4+3] = 0.0
            ctrl.ub[(N-1)*n+(i-1)*4+3] = Inf
            
            ctrl.A[(N-1)*n+(i-1)*4+4, m+7+(i-1)*(n+m)] = 1  #x(7)
            ctrl.lb[(N-1)*n+(i-1)*4+4] = -0.25*model.m*model.g 
            ctrl.ub[(N-1)*n+(i-1)*4+4] = 0.5*model.m*model.g   
        end
    end
    initialize_solver!(ctrl; kwargs...) # 初始化OSQP solver
    return nothing
end

function update_QP!(ctrl::MPCController, x, time)
    ctrl.lb[1:n] .= -A*(x - Xref[end]) #更新初始状态约束，一直线性化的点都是平衡点
    ctrl.ub[1:n] .= -A*(x - Xref[end]) #初始状态处没有参考轨迹，是确定的，也不是优化变量
    Q = ctrl.P[m+1:m+n,m+1:m+n]
    Qf = ctrl.P[(N-2)*(n+m)+m+1:(N-1)*(n+m),(N-2)*(n+m)+m+1:(N-1)*(n+m)]
    k = searchsortedlast(ctrl.times, time)
    Nref = size(ctrl.Xref,1)    
    for j = 1:(N-2)    #每个参考轨迹都是等步长的，Knot Point也是等步长的，从当前位置往后移
        idx = k+j
        if idx >= Nref	# 若到终点了，则参考点选取为终点
            idx = Nref
        end
        ctrl.q[(m+(j-1)*(n+m)).+(1:n)] .= -Q*(ctrl.Xref[idx]-ctrl.Xref[end])
    end
    idx = k+N-1
    if idx >= Nref 		# 若到终点了，则参考点选取为终点
        idx = Nref
    end
    ctrl.q[(m+(N-2)*(n+m)).+(1:n)] .= -Qf*(ctrl.Xref[idx]-ctrl.Xref[end])
    return nothing
end

function get_control(ctrl::MPCController{OSQP.Model}, x, time)
    update_QP!(ctrl, x, time)
    OSQP.update!(ctrl.solver, q=ctrl.q, l=ctrl.lb, u=ctrl.ub)
    # Solve QP
    results = OSQP.solve!(ctrl.solver)
    Δu = results.x[1:2]	#只取u1来控制
    
    k = get_k(ctrl, time) #得到当前时刻的参考点，零阶保持器形式
    return ctrl.Uref[k] + Δu 
end

仿真结果

从仿真结果（Horizon=50）可以看出，无论是无约束的MPC还是有约束（状态倾角约束）的MPC都比LQR运行得好，都能完成火箭软着陆任务，但是带约束的MPC在降落到地的时候会平缓一些。

从曲线结果可以看出，

• 不带约束的MPC在和LQR有明显的超调，但是MPC的超调小一些，原因是因为，MPC控制器相比LQR式的reactive controller显然有一种预测的作用，在到达地面前会提前采取一些措施，因而不会降到地面之下
• 带约束的MPC最终结果还是有一点点的超调，与优化器对硬约束的软化有关
• 当时间窗口Horizon取为1时，MPC的控制结果与LQR基本一样
• 当时间窗口Horizon取较小时，如21，降落曲线还是会有一些地面上的超调（高度高于地面）。

MPC无约束

MPC带约束

MPC/LQR对比（约束与Horizon）

MPC horizon对比