HW3 基于iLQR/DDP四旋翼控制

HW3 基于iLQR/DDP四旋翼控制

题目需求

在本题中，需要实现迭代LQR算法（iterative LQR）即微分动态规划DDP的高斯牛顿近似版本。我们将利用iLQR来优化（生成）平面双旋翼无人机的轨迹，并且实现一个高难度的翻滚动作（Flip）。最后，我们在实验中验证根据iLQR/DDP算法所免费得到的TVLQR控制率，并在存在外界扰动的情况下（如风）实现轨迹的鲁棒跟踪。

流程：

• 系统模型与离散化
• 实现iLQR
  • Cost Function
  • 反向迭代过程（Backward Pass）
  • 正向带线搜索rollout过程（Forward Pass）
  • 综合
• 定点轨迹跟踪问题
• 翻滚轨迹跟踪问题
  • 生成翻滚轨迹
  • 优化翻滚轨迹
• 与TVLQR的对比（验证收敛时得到的是TVLQR最优控制率）
• 最优状态反馈跟踪

系统模型与离散化

所用的平面双旋翼无人机模型如下，
x = [ p x p z θ v x v z ω ] , x ˙ = [ v x v z ω 1 m ( u 1 + u 2 ) sin ⁡ θ 1 m ( u 1 + u 2 ) cos ⁡ θ − g l J ( u 2 − u 1 ) ] (1) \begin{align} x = \begin{bmatrix} p_x \\ p_z \\ \theta \\ v_x \\ v_z \\ \omega \end{bmatrix}, \quad \dot{x} = \begin{bmatrix} v_x \\ v_z \\ \omega \\ \frac{1}{m} (u_1 + u_2) \sin{\theta} \\ \frac{1}{m} (u_1 + u_2) \cos{\theta} - g \\ \frac{l}{J} (u_2 - u_1) \end{bmatrix} \end{align}\tag{1} x= ​px​pz​θvx​vz​ω​ ​,x˙= ​vx​vz​ωm1​(u1​+u2​)sinθm1​(u1​+u2​)cosθ−gJl​(u2​−u1​)​ ​​(1)
其中，$u_1{u}_2$分别为两旋翼产生的推力，$mJl$分别为无人机质量、转动惯量和轴距。

在Lecture 9中，我们在最终的平衡点处将模型线性化，将整个系统看成是全局的线性系统，因而可以当作一个时不变LQR问题/Convex MPC问题来求解。而在本作业中，因为要做复杂的翻滚动作，而在翻滚时，显然系统离线性化的点非常远（在悬停态线性化，因而roll=pitch=0），此时若仍然用全局的线性模型则系统控制一定会失败。因此，采用DDP/iLQR类方法，在轨迹中每个点处都对系统线性化，不停地做局部线性化。

定义系统的连续时间状态方程，利用RK4显式数值积分离散化，再通过调用自动微分工具线性化。

function dynamics(x,u)	# 连续时间状态方程
    # planar quadrotor dynamics
    mass = 1.0 
    g = 9.81
    ℓ = 0.3 	# 轴距
    J = 0.2*mass*ℓ^2 #转动惯量
     # unpack state
    px,pz,θ,vx,vz,ω = x    
    return [vx,vz,ω,(1/mass)*(u[1] + u[2])*sin(θ), (1/mass)*(u[1] + u[2])*cos(θ) - g, (ℓ/(2*J))*(u[2]-u[1])]
end

function rk4(x,u,dt)	# 在Forward Rollout中，用的是这个准确的模型而不是下面那个离散化模型
    # rk4 for integration 本文中，dt=0.025 N=61
    k1 = dt*dynamics(x,u)
    k2 = dt*dynamics(x + k1/2,u)
    k3 = dt*dynamics(x + k2/2,u)
    k4 = dt*dynamics(x + k3,u)
    return x + (1/6)*(k1 + 2*k2 + 2*k3 + k4)
end

function dynamics_jacobians(x,u,dt)	# 这个函数在Backward Pass的每一个点都调用。
    # returns the discrete time dynamics jacobians
    A = FD.jacobian(_x -> rk4(_x,u,dt),x)	# 自动微分工具
    B = FD.jacobian(_u -> rk4(x,_u,dt),u)
    return A,B
end

实现iLQR

Cost Function

对于非常常见的轨迹跟踪问题来说，
$$\begin{array}{rl}\ell (x_{1:N},u_{1:N-1})& =J_N(x_N)+\sum _{i=1}^{N-1}J(x_k,u_k)\\ J(x,u)& =\frac{1}{2}(x-x_{ref}{)}^{T}Q(x-x_{ref})+\frac{1}{2}(u-u_{ref}{)}^{T}R(u-u_{ref})\\ J_N(x)& =\frac{1}{2}(x-x_{ref}{)}^T{Q}_f(x-x_{ref})\end{array}\text{\hspace{1em}(2)}$$
在本问题中，$R=diag([0.01,0.01])Q=diag([1,1,1,0.1,0.1,0.1])Q_f=diag([100,100,100,100,100,100])$.

function stage_cost(x,u,xref,uref)	# 过程Cost，与x u有关
    # LQR cost at each knot point (depends on both x and u)
    J = 0.5*(x - xref)'*Q*(x - xref) + 0.5*(u - uref)'*R*(u - uref)
    return J
end

function term_cost(x,xref)	# 终点Cost，只与x相关
    # LQR terminal cost (depends on just x)
    J = 0.5*(x - xref)'*Qf*(x - xref)
    return J
end

function trajectory_cost(X,U1,Xref,Uref) # 整条轨迹的Cost
    # calculate the cost of a given trajectory 
    J=0.0
    for i = 1:N-1
        J += stage_cost(X[i],U1[i],Xref[i],Uref[i])
    end
    J += term_cost(X[end],Xref[end])
    return J
end
        
function stage_cost_expansion(x,u,xref,uref) # Cost对x u的梯度与Hessian
    # if the stage cost function is J, return the following derivatives:
    # ∇²ₓJ,  ∇ₓJ, ∇²ᵤJ, ∇ᵤJ
    Jxx = Q
    Jx = Q*(x-xref)	# 梯度是列向量，Hessian是对梯度求雅可比矩阵
    Juu = R
    Ju = R*(u-uref)	# Jux = Jxu’ = 0
    return Jxx, Jx, Juu, Ju
end

function term_cost_expansion(x,xref)
    # if the terminal cost function is J, return the following derivatives:
    # ∇²ₓJ,  ∇ₓJ
    Jxx = Qf
    Jx = Qf*(x-xref)
    return Jxx, Jx
end

反向迭代（Backward Pass）

反向迭代的主要流程与Lecture 10中类似，主要流程是在每个点求系统方程的线性化，而后求$S(x,u)$的梯度与Hessian，再进行$V(x)$的梯度与Hessian的迭代。

function backward_pass(X,U,Xref,Uref)

    # allocate all our data
    P = [zeros(nx,nx) for i = 1:N]     # cost to go quadratic term V(x)的Hessian
    p = [zeros(nx) for i = 1:N]        # cost to go linear term V(x)的Gradient
    d = [zeros(nu)*NaN for i = 1:N-1]  # feedforward control
    K = [zeros(nu,nx) for i = 1:N-1]   # feedback gain
    ΔJ = 0                           # expected cost decrease 利用梯度值估计
    P[end],p[end] = term_cost_expansion(X[end],Xref[end])
    for i = reverse(1:N-1)
        Jxx, Jx, Juu, Ju = stage_cost_expansion(X[i],U[i],Xref[i],Uref[i]) # 获得Cost函数在当前点的梯度与Hessian
        A , B = dynamics_jacobians(X[i],U[i],dt) # 在当前点线性化系统

        gx = Jx + A'*p[i+1]		# S(x,u)
        gu = Ju + B'*p[i+1]
        Gxx = Jxx + A'*P[i+1]*A
        Guu = Juu + B'*P[i+1]*B
        Gxu = A'*P[i+1]*B
        Gux = B'*P[i+1]*A

        d[i] = Guu\gu			#求解最优反馈+前馈控制率
        K[i] = Guu\Gux

        P[i] = Gxx + K[i]'*Guu*K[i] - Gxu*K[i] - K[i]'*Gux	# V(x)
        p[i] = gx - K[i]'*gu + K[i]'*Guu*d[i] - Gxu*d[i]

        ΔJ += gu'*d[i]
    end
    return d, K, P, ΔJ
end

前向Rollout

前向Rollout需要利用Backward Pass中得到的最优反馈+前馈控制率来利用系统方程前向仿真系统，其中$d_k$为前馈控制率，$K_k$为反馈控制率。最终控制率
$$\begin{array}{r}u(k)=u_0(k)-\alpha d_k-K(x(k)-x_0(k))\end{array}$$
注意，$u_0(k)x_0(k)$为线性化点，而在DDP中，我们是在当前轨迹（轨迹上的点）线性化，因此，实际上$u_0(k)x_0(k)$就是上一次Backward+Forward迭代中所得到的输入和状态轨迹。$x(k)$是当前迭代利用本次Backward Pass所得到的最优控制率来前向仿真过程的状态$x(k)$。而$x_{ref}{u}_{ref}$不是线性化点，只是用来算Cost的。

$\alpha$ 是线搜索步长，初始值为1，代表默认走满一个Newton Step，但是由于迭代是基于局部二次化的，所以往往一个Newton Step的步长会太大了，若实际Cost的下降没有小于预期下降的b倍（b=0.01在本文中，预期下降在Backward Pass中通过Cost to go函数的一阶泰勒展开近似），则为了加速迭代过程与保证迭代的正确性，我们通过缩减$\alpha$的方式来backtrack。详见Lecture 5.

function forward_pass(X,U,Xref,Uref,K,d,ΔJ; max_linesearch_iters = 10)
    Xn = deepcopy(X) # Xref Uref是要跟踪的轨迹，用来算Cost的，X U是当前线性化的点，用来迭代，都是以这个为基础。
    Un = deepcopy(U)
    Jn = NaN
    α = 1.0	#
    J = trajectory_cost(X,U,Xref,Uref)  #线性化点的Cost，也是上一次迭代的Cost
    Jn = 0
    for i = 1:N-1
        Un[i] = U[i] - α*d[i] - K[i]*(Xn[i] - X[i])
        Xn[i+1] = rk4(Xn[i],Un[i],dt)
        Jn += stage_cost(Xn[i],Un[i],Xref[i],Uref[i])
    end
    Jn += term_cost(Xn[N],Xref[N])
    iter = 0				# 保证本次迭代的实际Cost下降要小于上一次迭代加一个阈值
    while iter < max_linesearch_iters && (isnan(Jn) || Jn > (J - 1e-2*α*ΔJ) ) 
        α = 0.5*α	# Backtrack
        Jn = 0		
        iter += 1
        for i = 1:N-1
            Un[i] = U[i] - α*d[i] - K[i]*(Xn[i] - X[i])	# 利用最优控制率来获得u
            Xn[i+1] = rk4(Xn[i],Un[i],dt)	# 利用准确的离散模型rollout，因为我们需要的是真实的Cost，而不是线性化后的
            Jn += stage_cost(Xn[i],Un[i],Xref[i],Uref[i])	
        end
        Jn += term_cost(Xn[N],Xref[N])  
    end
    return Xn, Un, Jn, α
end

iLQR综合

将反向迭代与正向迭代反复进行，并根据前馈控制率$d_k$的无穷范数来判读是否收敛（若控制率收敛了，则前馈控制率接近于0）

function iLQR(x0,U,Xref,Uref;atol=1e-5,max_iters = 100,verbose = true)
    X = [x0 for i = 1:N]
    K = [zeros(nu,nx)*NaN for i = 1:N-1]
    P = [zeros(nx,nx)*NaN for i = 1:N]
    iter = -1

    # forward rollout 整个iLQR问题需要提供一个初始值和一个初始控制率作为初始化
    for i = 1:N-1	# 一般来说，初始控制率选可以保持平衡的。如本节中，我们取u1=u2=0.5*mg
        X[i+1] = rk4(X[i],U[i],dt)
    end
    d = ones(5,5) # 随便初始化的一个值，防止报未定义的错
    U1 = deepcopy(U)
    while iter < max_iters && norm(d,Inf) > atol
        d, K, P, ΔJ = backward_pass(X,U,Xref,Uref)	# 得到最优控制率与期望的Cost下降（一阶近似）
        Xn, Un, Jn, α = forward_pass(X,U,Xref,Uref,K,d,ΔJ)	# Rollout+线搜索
        iter += 1	
        X .= Xn	# 注意，在Julia中，若执行X=Xn，则会新建一个变量，而执行X.=Xn，则是本地的inplace逐元素操作
        U .= Un	# 这一句话非常坑，坑了我一个下午没找到错误。因为在后文调用iLQR函数时，传进来的Uref和初始U是同一个
    end			# 值，假如使用.=修改，则Xref也会变，整个问题就会变得很奇怪无法收敛。使用=则会新建临时变量，没有这个问题
				# 所以要么用=号，要么在传参时做一次deepcopy。
    return X,U,K,P,iter
end

定点轨迹跟踪

在定点轨迹跟踪中，所有的$x_{ref}$都设成目标点，并且$u_{ref}$设成悬停时（期望控制率不要偏离悬停态太多）。

const x0    = [-3, 1.0, 0, 0, 0, 0]                    # initial state
const xgoal = [+3, 1.0, 0, 0, 0, 0]                    # goal state
Xrefline = [copy(xgoal) for i = 1:N]	
Urefline = [copy(uhover) for i = 1:N-1]
# call iLQR
U0 = deepcopy(Urefline)
Xline,Uline,Kline,Pline, iterline = iLQR(x0,U0,Xrefline,Urefline,max_iters=100);

在这个问题中，优化花了42个迭代完全收敛，但是，从下图中可以看出，只需要经过6次迭代，则期望的Cost下降$\Delta J$和前馈控制率$d$就已经降到比较小了，后面的迭代使控制率的Cost变化并不多。因此，在对精度要求并不是那么高的场景的实时控制场景中，只需要几次迭代就可以得到一个满意的控制率。

从结果中还可以看出另外一个现象，在这个并不复杂的非线性系统中（非线性没那么离谱），基本上线搜索步长都没有Backtrack，采用一个full newton step也可以使Cost一直下降。

翻滚轨迹跟踪

翻滚轨迹生成

通过人工编程生成一条翻滚轨迹

function flip_reference()
    # TODO: Design the reference trajectory according to the specs above
    x1ref = zeros(61)*NaN	# 初始化轨迹 
    x2ref = zeros(61)*NaN
    θref = zeros(61)*NaN
    v1ref = zeros(61)*NaN
    v2ref = zeros(61)*NaN
    ωref = zeros(61)*NaN
    x1ref[1:20] = LinRange(-3,0,20)	#dt=0.025，因此前20步对应0-0.5s
    x2ref[1:20] = LinRange(1,1,20)
    θref[1:20] = LinRange(0,0,20)

    x1ref[21:30] = LinRange(0,0,10)
    x2ref[21:30] = LinRange(1,3,10)
    θref[21:40] = LinRange(0,-2*pi,20)

    x1ref[31:40] = LinRange(0,0,10)
    x2ref[31:40] = LinRange(3,1,10)

    x1ref[41:61] = LinRange(0,3,21)
    x2ref[41:61] = LinRange(1,1,21)
    θref[41:61] = LinRange(-2*pi,-2*pi,21)

    for i = 1:60
        v1ref[i] = (x1ref[i+1] - x1ref[i])/dt
        v2ref[i] = (x2ref[i+1] - x2ref[i])/dt
        ωref[i] = (θref[i+1] - θref[i])/dt
    end
    v1ref[61] = v1ref[60]
    v2ref[61] = v2ref[60]
    ωref[61] = ωref[60]

    xref = [x1ref'; x2ref'; θref'; v1ref'; v2ref'; ωref']
    return [x for x in eachcol(xref)]
end

翻滚轨迹优化

与定点飞行的参考$x_{ref}$不同，在翻滚中我们给每个$k$ 都设计一个参考的状态点，希望飞机能够一直按照这个轨迹执行。

Uref = [copy(uhover) for i = 1:N-1] # U的参考值仍然是悬停态
Uflip = deepcopy(Uref)
Xref_ = flip_reference()	# 获得生成的翻滚轨迹
Xflipref =  [copy(Xref_[i]) for i = 1:N] # 每个点都有一个不同的参考Xref
Xflip,Uflip,Kflip,Pflip,iterflip = iLQR(x0,Uref,Xflipref,Uflip);

优化结果如下，由于每个点处都有一个参考点，相当于我们给了整个优化一个比较好的引导，本问题只花了30个迭代就完全收敛。从结果看出，也只需要10步左右的迭代，系统的Cost就基本不变化了，非常有利于实时控制系统。

并且，在本翻滚问题中，由于非线性的模型比定点飞行时复杂（定点飞行时整体姿态变化不大，因此线性化点都比较近，整个问题的非线性没那么强），因此在迭代开始时，经历了几次线搜索的Backtrack，若不加线搜索，则系统难以收敛。

与TVLQR比较

在Lecture 11中我们提到，利用iLQR/DDP方法可以让免费顺带获得一个TVLQR控制率，我们在本实验中验证。

Klqr = deepcopy(Kflip) .* NaN
Plqr = deepcopy(Pflip) .* NaN
A = [zeros(nx,nx) for k = 1:N-1]
B = [zeros(nx,nu) for k = 1:N-1]
Plqr[N] = Qf		# 给定Cost to go末端值
for k=(N-1):-1:1	# 时变LQR控制率生成，与时不变的在Riccti equation迭代公式并没有不同
    A[k],B[k] = dynamics_jacobians(Xflip[k],Uflip[k],dt)	# 在翻滚轨迹点处线性化
    Klqr[k] .= (R + B[k]' * Plqr[k+1] * B[k]) \ (B[k]' * Plqr[k+1] * A[k])
    Plqr[k] .=  Q + A[k]' * Plqr[k+1] * A[k] - A[k]' * Plqr[k+1] * B[k] * Klqr[k]
end
@testset "TVLQR vs iLQR" begin                      
    @test maximum(norm.(Kflip - Klqr,Inf)) < 1e-3  # success Kflip为收敛后（上一步输出的）的反馈矩阵
    @test maximum(norm.(Pflip - Plqr,Inf)) < 1e-3  # success Pflip是收敛后的V(x)的Hessian矩阵
end;

闭环最优状态反馈跟踪

既然iLQR在收敛时有了一个最优的TVLQR控制率，我们利用它实现闭环控制，使其能抵抗系统方程中的扰动（如外部扰动）。

将带扰动的系统方程写为
$$\begin{array}{r}{x}_{k+1}=rk4(x_k,u_k,dt)+[0,0,0,0.1\cdot \mathrm{randn}(3)]\end{array}$$
对比开环执行与闭环执行的效果，验证TVLQR控制率的可靠性。

可以看出TVLQR的轨迹与理想轨迹基本相同，而开环时，炸机咯。

TVLQR

开环控制

function simulate_with_noise(x0,Xflip,Uflip,Kflip,open_loop)
    X = [zeros(nx) for i = 1:N]
    U = [zeros(nu) for i = 1:N-1]
    X[1] = copy(x0)   
    Random.seed!(1)    
    # TODO: simulate with added noise from wind 
    for i = 1:N-1
        if open_loop
            U[i] = Uflip[i]
        else
            U[i] = Uflip[i] -  Kflip[i]*(X[i] - Xflip[i])	# DDP在收敛后，就没有前馈项d了，与TVLQR形式相同
        end
        X[i+1] = rk4(X[i],U[i],dt) + [0,0,0,0.1*randn(),0.1*randn(),0.1*randn()]	# 加入噪声
    end
    return X,U
end