MyEnigma

とある自律移動システムエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

打ち上げロケットの非線形モデル予測制御問題をJuliaで解いてみた

目次

はじめに

この記事は制御工学 Advent Calendar 2017 23日目の記事です。

qiita.com

 

以前の記事で、SpaceXの自動着陸ロケットは、

線形MPCを使っているようだという記事を書きましたが、

myenigma.hatenablog.com

 

今回は、下記の資料を元に、

非常にシンプルなロケットの打ち上げ制御シミュレーションを

Juliaを使って書いてみようと思います。

github.com

 

シンプルなロケット打ち上げモデル

今回のシミュレーションでは、

垂直に飛ぶロケットの最終高度を最大化するように

制御してみたいと思います。

 

今回使用するモデルとしては、

ロケットのスラストを

入力として制御できるものとし、

その他にも、ロケット重量や、

燃料消費率、重力、空気抵抗などを考慮しないといけないとします。

  

今回のシミュレーションでは、離散時間のモデルを使用し、

タイムステップはnとします。

時間刻みの量は⊿tとすると、

シミュレーション時間は tf = n × ⊿tになります。

今回のシミュレーションでは⊿tも最適化変数とします。

また今回のシミュレーションの積分には台形積分を利用することにしました。

 

状態ベクトルとしては、

速度v, 高度h, そして残り燃料mの3つとします。

制御入力としては、スラストTとします。

ロケットのシミュレーションで面白い所は、

スラストTに応じて、燃料が減り、

それによりロケットのダイナミクスが変化する所です。

前述の通り、今回のシミュレーションの目的は、

最終時刻t_fにおける高度h_t_fを最大化することです。

また今回のシミュレーションでは、

状態ベルトルと入力値すべてを正規化された値で表現するとします。

 

ロケットの運動モデルとしては、

下記の3式を使います。

f:id:meison_amsl:20171204030209p:plain

一つ目は高度と速度の方程式です。

二つ目は、加速度の方程式です。

スラストTと空気抵抗Dの差分をmで割った値が加速度になり、

その値と重力の差分が最終的な高さ方向の加速度になります。

最後の式はロケットの質量の変化の式です。

スラストTに応じて燃料が減り、ロケットの質量も変化します。

空気抵抗Dは、下記の式のように高度と速度に応じて変化するものとします。

f:id:meison_amsl:20171204030644p:plain

重力も各高度に応じて、下記の式で変化するものとします。

f:id:meison_amsl:20171204030743p:plain

詳細なシミュレーションモデルは下記のリンク先の資料を参照ください。

 

上記のロケットのモデルはかなり簡素化されたものですが、

モデルも非線形なので、上記の非線形モデル予測制御問題は、

非線形最適化を使って解く必要があります。

 

Juliaによるサンプルコード

下記がJuliaで上記のシミュレーションを実施するコードです。

最適化ライブラリとしてJulia製のJuMPを、

myenigma.hatenablog.com

ソルバーとしては、Ipoptを使いました。

 

#
# Optimization based rocket control simulation
#
# author: Atsushi Sakai
#

using PyCall, JuMP, Ipopt

@pyimport matplotlib.pyplot as plt

function main()
    println(PROGRAM_FILE," start!!")
    mod = Model(solver=IpoptSolver(print_level=0))

    # Constants
    # Note that all parameters in the model have been normalized
    # to be dimensionless. See the COPS3 paper for more info.
    h_0 = 1    # Initial height
    v_0 = 0    # Initial velocity
    m_0 = 1    # Initial mass
    g_0 = 1    # Gravity at the surface

    # Parameters
    T_c = 3.5  # Used for thrust
    v_c = 620  # Used for drag
    h_c = 300.0 # parameter of drag
    m_c = 0.6  # Fraction of initial mass left at end

    # Derived parameters
    c     = 0.5*sqrt(g_0*h_0)  # Thrust-to-fuel mass
    m_f   = m_c*m_0            # Final mass
    D_c   = 0.5*v_c*m_0/g_0    # Drag scaling
    T_max = T_c*g_0*m_0        # Maximum thrust

    n = 100   # Time steps

    # State variables
    @variable(mod, v[0:n] ≥ 0)            # Velocity
    @variable(mod, h[0:n] ≥ h_0)          # Height
    @variable(mod, m_f ≤ m[0:n] ≤ m_0)    # Mass
    @variable(mod, Δt ≥ 0, start = 1/n)   # Time step

    # Control: thrust
    @variable(mod, 0 ≤ T[0:n] ≤ T_max)
 
    # Objective: maximize altitude at end of time of flight
    @objective(mod, Max, h[n])

    @expression(mod, t_f, Δt*n)          # Time of flight

    # Initial conditions
    @constraint(mod, v[0] == v_0)
    @constraint(mod, h[0] == h_0)
    @constraint(mod, m[0] == m_0)
    @constraint(mod, m[n] == m_f)

    # Forces
    # Drag(h,v) = Dc v^2 exp( -hc * (h - h0) / h0 )
    @NLexpression(mod, drag[j=0:n], D_c*(v[j]^2)*exp(-h_c*(h[j]-h_0)/h_0))
    # Grav(h)   = go * (h0 / h)^2
    @NLexpression(mod, grav[j=0:n], g_0*(h_0/h[j])^2)

    # Dynamics
    for j in 1:n
        # h' = v Trapezoidal integration
        @NLconstraint(mod, h[j] == h[j-1] + 0.5*Δt*(v[j]+v[j-1]))

        # v' = (T-D(h,v))/m - g(h) Trapezoidal integration
        @NLconstraint(mod, v[j] == v[j-1] + 0.5*Δt*(
                (T[j  ] - drag[j  ] - m[j  ]*grav[j  ])/m[j  ] +
                (T[j-1] - drag[j-1] - m[j-1]*grav[j-1])/m[j-1] ))

        # m' = -T/c Trapezoidal integration
        @NLconstraint(mod, m[j] == m[j-1] - 0.5*Δt*(T[j] + T[j-1])/c)
    end

    # Provide starting solution
    for k in 0:n
        setvalue(h[k], 1)
        setvalue(v[k], (k/n)*(1 - (k/n)))
        setvalue(m[k], (m_f - m_0)*(k/n) + m_0)
        setvalue(T[k], T_max/2)
    end

    # Solve for the control and state
    status = solve(mod)

    # Display results
    plt.subplots(1)
    x=[getvalue(Δt)*i for i in 0:n]
    y=[getvalue(h[i]) for i in 0:n]
    plt.plot(x, y)
    plt.grid(true)
    plt.xlabel("Time[s]")
    plt.ylabel("Altitude")
    
    plt.subplots(1)
    y=[getvalue(m[i]) for i in 0:n]
    plt.plot(x, y)
    plt.xlabel("Time[s]")
    plt.ylabel("Mass")
    plt.grid(true)

    plt.subplots(1)
    y=[getvalue(v[i]) for i in 0:n]
    plt.plot(x, y)
    plt.xlabel("Time[s]")
    plt.ylabel("Velocity")
    plt.grid(true)

    plt.subplots(1)
    y=[getvalue(T[i]) for i in 0:n]
    plt.plot(x, y)
    plt.xlabel("Time[s]")
    plt.ylabel("Thrust")
    plt.grid(true)

    plt.show()

    println(PROGRAM_FILE," Done!!")
end


if contains(@__FILE__, PROGRAM_FILE)
    @time main()
end

github.com

 

下記がシミュレーション結果のグラフです。

スラストを利用するたびに、ロケットの重量も変化し、

それによりモデルの振る舞いも変化するのが面白いですね。

f:id:meison_amsl:20171204101933p:plain

f:id:meison_amsl:20171204101942p:plain

f:id:meison_amsl:20171204101953p:plain

f:id:meison_amsl:20171204102003p:plain

  

参考資料

qiita.com

qiita.com

qiita.com

qiita.com

qiita.com

qiita.com

 

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com