📚

線形MPCでの倒立振子入門

2025/05/03に公開

はじめに

以前の記事では、モデル予測制御（MPC）の基本として、線形なばね・マス・ダンパ系を扱いました。目標値に素早く追従させる制御ができましたが、対象が線形だったので素直な問題設定でしたね。

今回は制御対象として Gymnasium (Pendulum-v1) を使います。これはいわゆる倒立振子と呼ばれるモデルを表現したものになっています。倒立振子の難しさは、主に2点です。

不安定性: 目標である「倒立状態」は、放っておけばすぐに倒れてしまう不安定な平衡点です。常に能動的に制御し続ける必要があります。
非線形性: 振子の運動方程式は三角関数を含む非線形なものです。特に、振子が大きく揺れている状態では、線形システムとして近似することが難しくなります。

これらの特性を持つ倒立振子に対して、線形MPCをどのように適用していくのか、その工夫と限界について、今回まとめてみたいと思います。

Gymnasium 環境とモデルパラメータ

今回の実験では、制御対象として Gymnasium ライブラリの Pendulum-v1 環境を利用します。Gymnasium は強化学習の研究開発で広く使われているツールキットで、様々なシミュレーション環境が提供されています。

import gymnasium as gym

# 環境の作成 (render_mode='human' で描画)
env = gym.make('Pendulum-v1', render_mode='human', g=9.81) 
# 初期状態を取得
obs, info = env.reset()
# 行動を適用して次の状態などを取得
# action = np.array([1.0]) # 例: トルク1.0
# next_obs, reward, terminated, truncated, info = env.step(action)

このように簡単に環境を呼び出して、reset() で初期化、step(action) で制御入力を与えてシミュレーションを進めることができます。

さて、MPC を実装する上で重要な点があります。それは、MPCは制御対象の内部モデルを必要とするということです。未来の状態を予測するために、 $A_d$ や $B_d$ といったシステム行列が必要になります。これらの行列は、システムの物理パラメータ（質量 $m$ , 長さ $l$ , 重力 $g$ , 制御周期 $dt$ など）に依存します。

今回の実験では、Gymnasium 環境の内部パラメータを正確に知っている、という前提で進めます。つまり、 $m, l, g, dt$ の値を Gymnasium 環境オブジェクトから直接取得し、それを使って $A_d, B_d$ を計算します。

以下の関数 get_env_params で、環境オブジェクトからこれらのパラメータを読み取っています。

def get_env_params(env):
    """環境から物理パラメータを取得する。"""
    try:
        # Pendulum-v1のパラメータを直接読み取る
        g = env.unwrapped.g
        m = env.unwrapped.m
        l = env.unwrapped.l
        dt = env.unwrapped.dt
        max_torque = env.unwrapped.max_torque
        print(f"Read params from env: g={g}, m={m}, l={l}, dt={dt}, max_torque={max_torque}")
        return g, m, l, dt, max_torque
    except AttributeError:
        # 読み取れない場合のデフォルト値
        print("Warning: Could not read parameters directly from env. Using defaults.")
        g, m, l, dt, max_torque = 10.0, 1.0, 1.0, 0.05, 2.0
        return g, m, l, dt, max_torque

現実のシステムでは、このように正確なパラメータが分かっていることは稀です。その場合は、システム同定などの手法でモデルパラメータを推定する必要がありますが、今回はその話は置いておき、「モデルは既知」としてMPCの実装に集中します。

倒立振子モデル

まず、倒立振子がどのようなシステムか見てみましょう。一番シンプルなものは、図のように固定された回転軸の周りを自由に回転できる一本の棒（質量 $m$ 、長さ $l$ ）です。先端におもりが集中していると考えます。回転軸にはモーターが付いていて、トルク $u$ を加えることができます。

(ここに倒立振子の簡単な図を想像してください。垂直上向きを $\theta=0$ とします)

この振子の運動方程式は、回転角度を $\theta$ （垂直上向きが0）、角速度を $\dot{\theta}$ とすると、以下のように表されます（簡単のため、空気抵抗や摩擦は無視します）。

\ddot{\theta} = \frac{mgl \sin(\theta) + u}{I}

ここで、 $g$ は重力加速度、 $I$ は慣性モーメント（先端質点なら $I=ml^2$ ）です。Gymnasium の Pendulum-v1 では、係数を整理して次のような式が使われています。

\ddot{\theta} = \frac{3g}{2l} \sin(\theta) + \frac{3}{ml^2} u

この式には $\sin(\theta)$ が含まれているため、非線形システムです。線形MPCを適用するには、これを線形近似する必要があります。

線形化と離散化

今回は、目標である倒立状態 ( $\theta=0, \dot{\theta}=0$ ) の周りで線形化します。状態ベクトルを $x=[\theta, \dot{\theta}]^T$ とすると、 $\theta \approx 0$ では $\sin(\theta) \approx \theta$ と近似できるので、運動方程式は次のような線形システム $\dot{x}=Ax+Bu$ で近似できます。

A = \begin{bmatrix} 0 & 1 \\ \frac{3g}{2l} & 0 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ \frac{3}{ml^2} \end{bmatrix}

この連続時間モデルを、制御周期 $dt$ で離散化します。ゼロ次ホールド（入力 $u$ が $dt$ 間一定と仮定）を用いると、離散時間モデル $x_{k+1}=A_d x_k+B_d u_k$ が得られます。 $A_d = e^{Adt}$ 、 $B_d = (\int_0^{dt} e^{A\tau} d\tau) B$ ですね。これらの行列は、Python では scipy.linalg.expm を使って計算できます。

以下の関数 linearize_and_discretize で、この計算を行っています。

from scipy.linalg import expm
import numpy as np

def linearize_and_discretize(g, m, l, dt):
    """
    倒立平衡点周りでシステムを線形化し、ZOHで離散化。
    状態 x = [theta, theta_dot]
    """
    # 連続時間モデル
    A_cont = np.array([
        [0, 1],
        [3 * g / (2 * l), 0] # 不安定平衡点周りなので正
    ])
    B_cont = np.array([
        [0],
        [3 / (m * l**2)]
    ])
    
    # ZOHによる離散化 (Van Loan's method)
    # [[A, B], [0, 0]] * dt の行列指数関数を計算
    n = A_cont.shape[0]
    p = B_cont.shape[1]
    M = np.zeros((n + p, n + p))
    M[:n, :n] = A_cont
    M[:n, n:] = B_cont
    M_exp = expm(M * dt)
    
    Ad = M_exp[:n, :n]
    Bd = M_exp[:n, n:]
    return Ad, Bd

これで、MPC内部で未来の状態を予測するための線形モデル $(A_d, B_d)$ が手に入りました。

ハイブリッドMPC：安定化と振り上げ

線形MPCの問題設定を簡単に振り返っておきましょう。各時刻で、予測ホライズン $N$ にわたる未来の制御入力系列 $U = \{u_k, ..., u_{k+N-1}\}$ を、以下のコスト関数 $J$ を最小化するように決定します。

J(x_k, U) = \sum_{i=0}^{N-1} (x_{k+i|k}^T Q x_{k+i|k} + u_{k+i}^T R u_{k+i}) + x_{k+N|k}^T P x_{k+N|k}

最小化は、離散時間線形モデル $x_{k+1} = A_d x_k + B_d u_k$ と入力制約 $|u_k| \le u_{max}$ の下で行われます。これは二次計画問題(QP)として解けるのでしたね。

cvxpy を用いてこのQP問題を定義する関数が setup_mpc_problem です。

import cvxpy as cp

def setup_mpc_problem(Ad, Bd, N, Q, R, P, max_torque):
    """cvxpy を用いて MPC の QP 問題を定義する。"""
    nx = Ad.shape[1] # 状態次元 (2)
    nu = Bd.shape[1] # 入力次元 (1)
    U = cp.Variable((nu, N), name='U')
    X = cp.Variable((nx, N + 1), name='X')
    x0 = cp.Parameter(nx, name='x0') # 初期状態はパラメータとして定義

    cost = 0
    for k in range(N):
        cost += cp.quad_form(X[:, k], Q) # 状態コスト
        cost += cp.quad_form(U[:, k], R) # 入力コスト
    cost += cp.quad_form(X[:, N], P) # 終端コスト

    constraints = [X[:, 0] == x0] # 初期状態制約
    for k in range(N):
        constraints += [X[:, k+1] == Ad @ X[:, k] + Bd @ U[:, k]] # 状態遷移
        constraints += [cp.abs(U[:, k]) <= max_torque] # 入力制約

    problem = cp.Problem(cp.Minimize(cost), constraints)
    return problem, x0, U # 問題オブジェクト、初期状態パラメータ、入力変数を返す

そして、QPを解いて実際の制御入力（最初のステップの $u_k^*$ ）を計算するのが compute_mpc_action です。

def compute_mpc_action(current_state_mpc, mpc_problem, x0_param, u_var, max_torque):
    """線形MPC (QP) を解いて最初の制御入力を計算する。"""
    x0_param.value = current_state_mpc # 現在の状態をセット
    try:
        mpc_problem.solve(solver=cp.OSQP, warm_start=True) # ソルバー実行
    except cp.SolverError as e:
        # ... エラー処理 ...
        return 0.0
    # ... 解の状態チェック ...
    optimal_u0 = u_var.value[0, 0] # 最初の制御入力を取得
    return np.clip(optimal_u0, -max_torque, max_torque)

重要な点: この線形MPCは、あくまで倒立状態 ( $\theta=0$ ) 付近で線形近似が有効な範囲でのみ、安定化のために使用します。

では、振子が大きく揺れていて線形近似が有効でない場合はどうするか？
ここでハイブリッド制御の考え方を導入します。倒立状態から遠い場合は、MPCを使わず、代わりに振子にエネルギーを与えるようなシンプルな制御を行います。これを振り上げ制御と呼びます。

今回は、以下のシンプルな制御則を採用しました（試行錯誤の結果、以前の記事から符号を反転させています）。

u=-k_{swing}\dot{\theta}\cos(\theta)

目的は、倒立状態近傍まで振子を「押し上げる」ことです。複雑なことはせず、とにかくエネルギーを与える方向にトルクを加える、という役割に徹してもらいます。特に意味のある式ではないので、適当に振動エネルギーをあたえるものであればこの場合はなんでも良いです。

def compute_swing_up_action(obs, k_swing, max_torque):
    """シンプルな振り上げ制御ロジック。"""
    cos_theta, sin_theta, theta_dot = obs
    # エネルギーを与える方向にトルクを計算（符号に注意）
    action = -k_swing * theta_dot * cos_theta 
    return np.clip(action, -max_torque, max_torque)

そして、現在の状態 obs から角度 $\theta$ を計算し、その絶対値がある閾値 theta_threshold より小さいかどうかで、MPC (compute_mpc_action) と振り上げ (compute_swing_up_action) を切り替えるのが select_action_functional 関数です。

def select_action_functional(obs, theta_threshold, k_swing, max_torque, 
                             mpc_problem, x0_param, u_var):
    cos_theta, sin_theta, theta_dot = obs
    theta = np.arctan2(sin_theta, cos_theta)

    if abs(theta) < theta_threshold:
        # 倒立に近い -> MPCで安定化
        current_state_mpc = np.array([theta, theta_dot])
        action_scalar = compute_mpc_action(current_state_mpc, mpc_problem, x0_param, u_var, max_torque)
    else:
        # 倒立から遠い -> 振り上げ制御
        action_scalar = compute_swing_up_action(obs, k_swing, max_torque)

    return np.array([action_scalar], dtype=np.float32)

このように、線形MPCが有効な範囲 ( $\theta$ が小さい) ではMPCを使い、それ以外の範囲ではシンプルな振り上げ制御を使う、という戦略で不安定・非線形な倒立振子に挑みます。

Pythonによる実装コード

それでは、ここまでの制御戦略を Python で実装したコードを見ていきましょう。順に振り返りながら説明します。

主な関数は以下の通りです。

1. 環境パラメータの取得 (get_env_params)

Gymnasium 環境オブジェクトから、重力加速度 g、質量 m、長さ l、時間ステップ dt、最大トルク max_torque などの物理パラメータを取得します。これは本来はシステム同定に相当する処理が必要なのでした。今回はその部分が主題ではないので、制御対象の物理パラメータが何らかの方法で正確にわかっているものとします。

def get_env_params(env):
    """環境から物理パラメータを取得する。"""
    try:
        g = env.unwrapped.g
        m = env.unwrapped.m
        l = env.unwrapped.l
        dt = env.unwrapped.dt
        max_torque = env.unwrapped.max_torque
        print(f"Read params from env: g={g}, m={m}, l={l}, dt={dt}, max_torque={max_torque}")
        return g, m, l, dt, max_torque
    except AttributeError:
        print("Warning: Could not read parameters directly from env. Using defaults.")
        g, m, l, dt, max_torque = 10.0, 1.0, 1.0, 0.05, 2.0
        return g, m, l, dt, max_torque

2. 線形化と離散化 (linearize_and_discretize)

得られた物理パラメータを使って、倒立平衡点周りで連続時間モデル $A, B$ を計算し、ゼロ次ホールド (ZOH) を用いて離散時間モデル $A_d, B_d$ を求めます。線形化しているため、ここで利用されるシステムのパラメータは「平衡点周り」でしか成り立たないものであることに注意して進めます。

def linearize_and_discretize(g, m, l, dt):
    """
    倒立平衡点周りでシステムを線形化し、ZOHで離散化。
    状態 x = [theta, theta_dot]
    """
    A_cont = np.array([
        [0, 1],
        [3 * g / (2 * l), 0] 
    ])
    B_cont = np.array([
        [0],
        [3 / (m * l**2)]
    ])
    # ZOHによる離散化 (Van Loan's method)
    M = np.block([
        [A_cont, B_cont],
        [np.zeros((B_cont.shape[1], A_cont.shape[1])), np.zeros((B_cont.shape[1], B_cont.shape[1]))]
    ])
    M_exp = expm(M * dt)
    Ad = M_exp[:A_cont.shape[0], :A_cont.shape[1]]
    Bd = M_exp[:A_cont.shape[0], A_cont.shape[1]:]
    return Ad, Bd

3. MPC問題の設定 (setup_mpc_problem)

cvxpy を使ってQP問題を定義します。状態系列 X と入力系列 U を最適化変数とし、初期状態 x0 をパラメータとして定義します。コスト関数と制約条件（状態遷移、入力制限）を記述し、cvxpy の Problem オブジェクト、初期状態パラメータ、入力変数を返します。

def setup_mpc_problem(Ad, Bd, N, Q, R, P, max_torque):
    """cvxpy を用いて MPC の QP 問題を定義する。"""
    nx = Ad.shape[1] 
    nu = Bd.shape[1] 
    U = cp.Variable((nu, N), name='U')
    X = cp.Variable((nx, N + 1), name='X')
    x0 = cp.Parameter(nx, name='x0')

    cost = 0
    for k in range(N):
        cost += cp.quad_form(X[:, k], Q) 
        cost += cp.quad_form(U[:, k], R) 
    cost += cp.quad_form(X[:, N], P) 

    constraints = [X[:, 0] == x0] 
    for k in range(N):
        constraints += [X[:, k+1] == Ad @ X[:, k] + Bd @ U[:, k]]
        constraints += [cp.abs(U[:, k]) <= max_torque]

    problem = cp.Problem(cp.Minimize(cost), constraints)
    return problem, x0, U

4. 振り上げ制御アクション計算 (compute_swing_up_action)

シンプルな振り上げロジックです。現在の観測値 obs とゲイン k_swing を受け取り、 $u = k_{swing} \cdot \dot{\theta} \cdot \cos(\theta)$ を計算してクリップした結果を返します。

def compute_swing_up_action(obs, k_swing, max_torque):
    """シンプルな振り上げ制御ロジック。"""
    cos_theta, sin_theta, theta_dot = obs
    # エネルギーを与える方向にトルクを計算（符号に注意）
    action = -k_swing * theta_dot * cos_theta
    return np.clip(action, -max_torque, max_torque)

5. MPC制御アクション計算 (compute_mpc_action)

QP問題を解く関数です。現在の状態 current_state_mpc ( $[\theta, \dot{\theta}]$ ) と、setup_mpc_problem で作成したQP問題関連オブジェクトを受け取ります。cvxpy の初期状態パラメータ x0_param を更新し、problem.solve() を呼び出します。解が得られたら、最初の制御入力 $u_0^*$ を返します。

def compute_mpc_action(current_state_mpc, mpc_problem, x0_param, u_var, max_torque):
    """線形MPC (QP) を解いて最初の制御入力を計算する。"""
    x0_param.value = current_state_mpc
    try:
        mpc_problem.solve(solver=cp.OSQP, warm_start=True, verbose=False)
    except cp.SolverError as e:
        print(f"Solver failed! Error: {e}. Returning zero action.")
        return 0.0

    # ... (エラーチェックと値の取得) ...
    if mpc_problem.status not in [cp.OPTIMAL, cp.OPTIMAL_INACCURATE] or u_var.value is None:
        print(f"Warning: MPC problem status {mpc_problem.status} or value is None. Returning zero action.")
        return 0.0

    optimal_u0 = u_var.value[0, 0]
    return np.clip(optimal_u0, -max_torque, max_torque)

6. 制御選択 (select_action_functional)

メインの制御関数です。現在の観測値 obs と制御パラメータ、QP問題オブジェクトを受け取ります。角度 $\theta$ を計算し、その絶対値が閾値 theta_threshold より小さいかどうかで、MPC制御 (compute_mpc_action) と振り上げ制御 (compute_swing_up_action) を呼び分け、最終的なアクション（要素1の配列）を返します。

def select_action_functional(obs, theta_threshold, k_swing, max_torque, 
                             mpc_problem, x0_param, u_var):
    """
    現在の観測状態に基づいて、振り上げ/MPC制御を選択し、行動を計算して返す。
    """
    cos_theta, sin_theta, theta_dot = obs
    theta = np.arctan2(sin_theta, cos_theta) 

    if abs(theta) < theta_threshold:
        # MPCモード
        current_state_mpc = np.array([theta, theta_dot])
        action_scalar = compute_mpc_action(current_state_mpc, mpc_problem, x0_param, u_var, max_torque)
    else:
        # 振り上げモード
        action_scalar = compute_swing_up_action(obs, k_swing, max_torque)

    return np.array([action_scalar], dtype=np.float32)

7. シミュレーションループ (if __name__ == "__main__":)

最後に、これらの関数を使ってシミュレーションを実行するメインの部分です。まず各種パラメータを設定し、環境を初期化します。次に、get_env_params, linearize_and_discretize, setup_mpc_problem を呼び出して制御に必要な準備をします。その後、シミュレーションループ内で select_action_functional を呼び出してアクションを決定し、環境をステップ実行します。

if __name__ == "__main__":
    # --- 設定 ---
    render = True 
    N = 20                  
    Q_diag = np.array([5.0, 0.1]) 
    R_val = 0.01               
    k_swing = 2.0           
    theta_threshold = 0.25   
    n_episodes = 1
    max_steps_per_episode = 500
    
    # --- 初期化 ---
    env = gym.make('Pendulum-v1', render_mode='human' if render else None, g=9.81)
    g, m, l, dt, max_torque = get_env_params(env)
    Ad, Bd = linearize_and_discretize(g, m, l, dt)
    Q = np.diag(Q_diag)
    R = np.diag([R_val])
    P = Q # 終端コスト
    mpc_problem, x0_param, u_var = setup_mpc_problem(Ad, Bd, N, Q, R, P, max_torque)

    print("\n--- Starting Simulation ---")

    # --- シミュレーションループ ---
    for episode in range(n_episodes):
        obs, info = env.reset()
        total_reward = 0
        print(f"\n--- Episode {episode+1} ---")
        for step in range(max_steps_per_episode):
            # 行動を選択 (関数ベース)
            action = select_action_functional(
                obs, theta_threshold, k_swing, max_torque,
                mpc_problem, x0_param, u_var
            )
            # 環境を1ステップ進める
            obs, reward, terminated, truncated, info = env.step(action)
            total_reward += reward
            if render:
                env.render()
            done = terminated or truncated
            if done:
                break # または continue
        # ... (ループ終了後の処理) ...
    env.close()
    print("\nSimulation finished.")

これらのコードを pendulum_mpc_controller.py として保存し、必要なライブラリをインストールすれば、python pendulum_mpc_controller.py でシミュレーションを実行できるはずです。

シミュレーション結果（イメージ）と考察

実際に上記のコードを実行してみると、(パラメータが適切であれば) 次のような挙動が観察されるはずです。

振り上げフェーズ: 最初、振子は真下あたりで静止しているか、ランダムな状態から始まります。select_action_functional は $\theta$ の絶対値が大きいと判断し、振り上げ制御 (compute_swing_up_action) を選択します。振子は左右に大きく揺れ始め、徐々に振幅を増していきます。この段階では、制御入力は比較的大きく、ダイナミックな動きが見られるでしょう。
切り替え: 振子の振幅が増し、倒立位置近く（例えば $|\theta| < \pi/4$ [rad]）を通過する瞬間が訪れます。このとき、select_action_functional は制御モードを線形MPC (compute_mpc_action) に切り替えます。
安定化フェーズ: MPCは、予測ホライズン $N$ ステップ先までの状態を予測し、倒立状態 $[0, 0]^T$ に近づけるための最適な制御入力（の最初のステップ）を計算します。振子は頂上付近で細かく振動するかもしれませんが、次第にその振れ幅は小さくなり、最終的には倒立状態で安定することが期待されます。制御入力も、安定化に必要な最小限のトルクに収束していくはずです。

もちろん、これは理想的なシナリオです。実際には、いくつかの課題や考慮事項があります。

モデル誤差: 線形化はあくまで近似です。特に、切り替え閾値付近では、実際の非線形ダイナミクスと線形モデルのズレ（モデル誤差）が無視できないかもしれません。これが、振動や不安定性を引き起こす可能性があります。
パラメータ調整: 振り上げゲイン $k_{swing}$ 、MPCのコスト行列 $Q, R, P$ 、予測ホライズン $N$ 、切り替え閾値 $\theta_{threshold}$ など、調整すべきパラメータが多くあります。これらの値は、システムの挙動に大きく影響します。特に $Q$ と $R$ のバランス（状態誤差をどれだけ重視するか vs 入力エネルギーをどれだけ抑えるか）は、応答速度と安定性のトレードオフに関わる重要な調整項目です。これは試行錯誤が必要な部分で、意外と職人芸だったりすることも…
計算時間: 各ステップでQP問題を解く必要があるため、線形MPCと言えども計算コストがかかります。今回の Pendulum 環境（状態2次元、入力1次元）で、予測ホライズン $N=20$ 程度なら、通常のPCでもリアルタイム（ $dt=0.05$ 秒以内）に計算可能だと思いますが、より複雑なシステムや長い予測ホライズンが必要な場合は、計算時間が問題になる可能性があります。
振り上げ制御の性能: 今回採用した振り上げ制御は非常にシンプルなので、必ずしもうまく振り上げられない場合や、時間がかかりすぎる場合もあり得ます。より高度な振り上げ戦略を使うことも考えられます。

このように、実際に動かしてみると、理論通りにいかない部分や、さらなる改善の余地が見えてくるものです。それもまた、制御システム設計の難しさであり、面白さでもあると思います。

実を言うと振り上げ制御には明確な悪さをする要素がある。Gymnasiumの初期状態はランダムなため、運が悪いとMPCが線形近似領域で動作しても倒立を成り立たせられない場面が出てくる。。。なぜだろうか？

まとめ

今回は、不安定かつ非線形なシステムである倒立振子に対して、線形MPCを適用する試みとして、ハイブリッド制御のアプローチを実装・実験してみました。

結果として、

振子が倒立状態から遠い場合は、シンプルな振り上げ制御でエネルギーを与える。
倒立状態に近づいたら、その近傍での線形近似モデルに基づいた線形MPCに切り替えて安定化する。
という戦略により、倒立振子を制御できることが確認できました。

このアプローチのポイントは、「線形MPCが有効なのは、線形近似が妥当な目標状態近傍に限られる」という点を認識し、それ以外の領域は別の（シンプルな）制御則でカバーするという点です。

もちろん、このハイブリッド制御にも課題はあります。制御を切り替える閾値 theta_threshold の設定は経験的ですし、切り替えの瞬間に制御入力が不連続になる可能性もあります。振り上げ制御も非常に単純なものなので、効率は最適ではないかもしれません。

しかし、この実験は、目標とする動作点（平衡点）の周りで適切に線形化できれば、線形MPCが不安定なシステムの安定化に有効なツールとなり得ることを示唆しています。

今回のハイブリッド制御からさらに発展させるなら、以下のようなアプローチが考えられます。

ゲインスケジューリングMPC: 複数の動作点（例：真下、水平、真上）で線形化を行い、それぞれの線形モデルに対応するMPCを用意しておき、現在の状態に応じて最適なMPCを選択・補間して使う方法。（今回の振り上げ適当制御の部分をなるべく減らす感じですね）
時変MPC (Time-Varying MPC): 各制御ステップで現在の状態周りの線形化を毎回行い、 $x_{k+1}=A_k x_k+B_k u_k$ のような時変の線形モデルを扱えるMPCを使う方法。計算コストは上がりますが、より広い範囲で非線形性に対応できます。ただし非線形性が強すぎる場合には、とある点での線形近似が制御を不可能にするほど現実と乖離してしまう可能性もあります。
非線形MPC (NMPC): システムの非線形モデルを直接扱って最適化計算を行う方法。最も正確ですが、計算コストが非常に高くなるため、リアルタイム制御には工夫が必要です。また、もはや複雑な非線形モデルを制御しようとする場合には、制御対象の同定自体が難しく、NMPCをしたところで「全然合っていない複雑なモデル」を扱っている可能性がある点にも注意が必要です。

これらは線形MPCからの自然な発展形と言えるでしょう。