目次
はじめに
この記事は制御工学 Advent Calendar 2017 23日目の記事です。
以前の記事で、SpaceXの自動着陸ロケットは、
線形MPCを使っているようだという記事を書きましたが、
今回は、下記の資料を元に、
非常にシンプルなロケットの打ち上げ制御シミュレーションを
Juliaを使って書いてみようと思います。
シンプルなロケット打ち上げモデル
今回のシミュレーションでは、
垂直に飛ぶロケットの最終高度を最大化するように
制御してみたいと思います。
今回使用するモデルとしては、
ロケットのスラストを
入力として制御できるものとし、
その他にも、ロケット重量や、
燃料消費率、重力、空気抵抗などを考慮しないといけないとします。
今回のシミュレーションでは、離散時間のモデルを使用し、
タイムステップはnとします。
時間刻みの量は⊿tとすると、
シミュレーション時間は tf = n × ⊿tになります。
今回のシミュレーションでは⊿tも最適化変数とします。
また今回のシミュレーションの積分には台形積分を利用することにしました。
状態ベクトルとしては、
速度v, 高度h, そして残り燃料mの3つとします。
制御入力としては、スラストTとします。
ロケットのシミュレーションで面白い所は、
スラストTに応じて、燃料が減り、
それによりロケットのダイナミクスが変化する所です。
前述の通り、今回のシミュレーションの目的は、
最終時刻t_fにおける高度h_t_fを最大化することです。
また今回のシミュレーションでは、
状態ベルトルと入力値すべてを正規化された値で表現するとします。
ロケットの運動モデルとしては、
下記の3式を使います。
一つ目は高度と速度の方程式です。
二つ目は、加速度の方程式です。
スラストTと空気抵抗Dの差分をmで割った値が加速度になり、
その値と重力の差分が最終的な高さ方向の加速度になります。
最後の式はロケットの質量の変化の式です。
スラストTに応じて燃料が減り、ロケットの質量も変化します。
空気抵抗Dは、下記の式のように高度と速度に応じて変化するものとします。
重力も各高度に応じて、下記の式で変化するものとします。
詳細なシミュレーションモデルは下記のリンク先の資料を参照ください。
上記のロケットのモデルはかなり簡素化されたものですが、
モデルも非線形なので、上記の非線形モデル予測制御問題は、
非線形最適化を使って解く必要があります。
Juliaによるサンプルコード
下記がJuliaで上記のシミュレーションを実施するコードです。
最適化ライブラリとしてJulia製のJuMPを、
ソルバーとしては、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
下記がシミュレーション結果のグラフです。
スラストを利用するたびに、ロケットの重量も変化し、
それによりモデルの振る舞いも変化するのが面白いですね。
参考資料
MyEnigma Supporters
もしこの記事が参考になり、
ブログをサポートしたいと思われた方は、
こちらからよろしくお願いします。