LQR（linear quadratic regulator）は、状態が目標から外れることと、操作を大きく使うことの両方に価格を付け、その合計を小さくする状態フィードバックを求める制御法である。PIDが誤差の現在・履歴・変化へ直接反応するのに対し、LQRは位置、速度、角度、角速度、電流などをまとめた「状態」を使い、各状態が将来へどう伝わるかを線形モデルで扱う。倒立振子、姿勢安定化、航空機、ロボットアームの局所安定化で分かりやすい。
LQRは魔法の最適化ボタンではない。状態を測れないならオブザーバが必要であり、モデルの有効範囲を外れればゲインも保証を失う。入力・状態制約を厳密に扱いたければMPCを検討する。本稿では結論、状態空間、リカッチ方程式、可観測性、数値例、PID/MPCとの使い分け、ROS 2実装と故障時安全を順に扱う。ROS 2のノード・ハードウェア境界はROS 2入門、センサ統合の前提はセンサフュージョン入門を参照してほしい。
結論：LQRは「何を嫌うか」をゲインへ変換する
- 線形状態方程式 \dot{x}=Ax+Bu と重み Q,R から、無限時間LQRは u=-Kx を導く。Q は状態偏差、R は操作量をどれほど嫌うかを表す。
- K はリカッチ方程式の解 P から K=R^{-1}B^TP と得る。ゲインを手で一つずつ探すのではなく、モデル内の結合を同時に扱える。
- 安定化できるには少なくとも (A,B) が可安定、推定器を使うには (A,C) が可検出である必要がある。センサがあることと、必要な状態が観測できることは違う。
- LQR自体は入力飽和や障害物を守らない。上限、レート制限、非常停止、状態推定異常時の減速・停止は独立して設計する。
状態空間：位置だけでなく「次にどう動くか」を持つ
状態 x は、その時刻から先の対象の振る舞いを、入力と外乱が分かれば決められる最小限の変数群である。質点なら位置だけでなく速度、回転体なら角度と角速度、モータなら電流・角速度・位置を含み得る。連続時間の線形時不変系を
と書く。u は操作、w はモデル化していない外乱、y は測定値、v はノイズである。A は状態自身の時間発展、B は入力が状態へ入る道筋、C はどの状態をセンサが見せるかを表す。
図: Duskcoil作成の概念図。実測ではなく、状態・測定・推定の信号関係を単純化したもの。
平衡点 (x_e,u_e) まわりで扱うときは、偏差 \tilde{x}=x-x_e、\tilde{u}=u-u_e を作り、\dot{\tilde{x}}=A\tilde{x}+B\tilde{u} に対してLQRを設計する。たとえば倒立振子を「角度ゼロ、速度ゼロ、台車位置ゼロ」に保つ問題は、この局所線形化で理解できる。大きく倒れた状態や接地・飽和を伴う状態まで、局所線形ゲインが回復させるとは限らない。
リカッチ方程式と評価関数
無限時間の連続LQRは、
を最小化する。Q\succeq0、R\succ0 とし、Q を大きくすれば状態偏差を強く嫌い、R を大きくすれば操作を節約する。単位の異なる位置[m]、角度[rad]、速度[m/s]をそのまま足してはいけない。許容値 x_{max}、u_{max} を基準に、例えば対角要素を 1/x_{max}^2、1/u_{max}^2 から始めると、無次元に近い比較ができる。
連続代数リカッチ方程式（CARE）は
であり、安定化解 P を得ると
となる。有限時間・離散時間では差分リカッチ方程式を後ろ向きに解く。サンプル系なら x_{k+1}=A_dx_k+B_du_k として、離散代数リカッチ方程式の解から K を得る。連続系の A,B をそのまま周期制御器へ入れる誤り、角度単位の混在、サンプル遅れの無視は、理論上のゲインを実機で不安定にする典型である。
可制御性・可観測性：ゲインの前に確認する条件
可制御性行列は
であり、\operatorname{rank}\mathcal{C}=n なら任意の状態を入力で動かせる。LQRでは不安定なモードが少なくとも可制御である可安定性が必要となる。可観測性行列は
で、rankが n なら状態を出力履歴から復元できる。位置センサしかない質点でも時間変化から速度を推定できる可能性はあるが、ノイズ・遅れが大きければ実用的な推定精度は別問題である。カルマンフィルタを含む推定器の詳説はカルマンフィルタ入門を参照してほしい。
数値例：二重積分器を止める
質量を正規化し、状態を x=[p\ v]^T、入力を加速度 u とする二重積分器は、
である。位置だけを強く戻したいと Q=\operatorname{diag}(q_p,q_v) の q_p を大きくしがちだが、速度罰則 q_v が小さ過ぎれば、到達を急ぐ鋭い加減速になりやすい。R を小さくし過ぎれば、入力が大きいことを気にしない解になる。得られる K=[k_p\ k_v] により u=-k_pp-k_vv となり、形としてはPDに似る。しかし k_p,k_v は二変数の独立調整でなく、モデルと Q,R から同時に決まる。
実機では |u|\le u_{max}、|\Delta u|\le r_{max} を加える。単純なクリップは必要な保護だが、クリップ後の閉ループはもはや無制約LQRの最適解ではない。頻繁に飽和するなら、参照を緩める、ゲインを再設計する、制約を明示できるMPCにする、という設計判断が必要である。
PID・LQR・MPCの使い分け
| 手法 | 得意な場面 | 主な入力 | 制約の扱い | 注意点 |
|---|---|---|---|---|
| PID | 単一量の高速ループ、モデルが不確か | 誤差と履歴 | 外側の制限器 | 積分ワインドアップ、ノイズ |
| LQR | 局所線形、多状態・多入力の安定化 | 推定状態 | 原則として明示しない | モデル範囲、飽和、推定器 |
| MPC | 予見・多変数・入力/状態制約 | 状態、参照、モデル | 最適化問題に明示 | 計算締切、実行不能、モデル誤差 |
PIDとLQRを競争相手にしない方がよい。モータの電流・速度にPID、姿勢や位置の局所安定化にLQR、経路と制約の調整にMPCを置く階層は自然である。LQRは定数参照に最も素直だが、軌道追従なら時変参照、フィードフォワード、ゲインスケジューリング、または線形化を更新するLTV-LQRを使う。
ロボット実装と故障時安全
ロボットではエンコーダ、IMU、カメラ、LiDARが別々の周期と遅れを持つ。ROS 2のトピック到着順をそのまま状態として使わず、時刻同期した推定状態、共分散、妥当性フラグをコントローラへ渡す。ROS 2入門のライフサイクル、ハードウェアインターフェース、更新周期を、状態推定と制御の境界として設計する。IMUはサイト内の権利確認済み画像で示せるが、写真は推定精度や特定機器の推奨を意味しない。
IMU/INSの代表例画像: Xsens MTi-G（Kallap85, CC BY-SA 4.0）、Wikimedia Commons。状態推定用センサの外観例であり、本文のLQR構成・性能を示す実測画像ではない。
故障時には、推定共分散の急増、センサタイムアウト、エンコーダとIMUの整合不良、入力飽和の持続、制御周期逸脱を監視する。状態が信用できないときに -K\hat{x} を出し続けない。速度ゼロへランプダウン、トルク無効、機械ブレーキ、非常停止のどれへ移るかは、機械の落下・慣性・人との距離に依存する。LQRの安定性証明は、正しいモデル・無飽和・正しい状態という仮定の下にある。独立保護系をその仮定の外側に置く。
実装チェックリスト
- 平衡点、状態・入力の単位、符号、座標系、離散周期を固定したか。
- 線形化点の近傍でモデル残差を測り、適用範囲を決めたか。
- 可制御性・可観測性（少なくとも不安定モード）を数値的に確認したか。
- Q,R を許容状態・許容入力で正規化し、変更理由を記録したか。
- 飽和、レート制限、状態推定の鮮度、非常停止をLQR外側で監視するか。
- シミュレーションだけでなく、低速・低出力の実機で遅れ・摩擦・負荷変化を試験したか。