LQR制御(Linear Quadratic Regulator)とは、対象の運動を数式モデルで表し、傾きや振動を小さくしながら、操作量も無駄に大きくしない制御を数学的に求める手法で、もう少し硬く言えば、制御対象を線形システムでモデル化し、状態の偏差と制御入力の大きさを表す二次形式の評価関数（コスト）を最小化するように、最適な状態フィードバック制御入力を決める最適制御手法だ。
竿燈で考えると、竿燈の状態を複数の数字でまとめるのだが最小なら、傾き θ と角速度 ω の2つ。ω は「傾きがどれくらい速く変化しているか」を表す。
この x を「状態ベクトル」と呼ぶ。竹の振動を加えるなら、第一モード q₁、第二モード q₂ とその速度も x に追加できる。
ここで状態方程式は、
ここでの記号の意味は、
|
記号 |
意味 |
| x | 現在の状態（傾き、速度、振動など） |
| A | 操作しなかったとき、状態同士がどう影響して変化するか |
| B | 操作 u が状態にどれくらい効くか |
| u | 制御入力（支点に加えるトルクなど） |
例えば2状態なら、A と B は行列になる。行列は「複数の関係をまとめて計算する表」と考えればよいかな。
(リカッチ方程式やリカッチ行列についてはググって欲しい)
LQRの中心は評価関数である。傾きや振動が大きいことにも、操作が強すぎることにも罰点を付ける。
• Qを大きくする：状態のずれを強く嫌う。例えば「傾きを絶対に小さくしたい」
• Rを大きくする：強い操作を嫌う。無理に力を使わず穏やかに制御したい
• LQRは J が最小になるような制御を求める
つまり、「倒れない」だけでなく、「振動を抑える」「無駄に大きな力を使わない」といった複数の目的を、QとRで重み付けして同時に扱える。
で、最終的な制御式は意外に単純で、
見た目はPD制御(Iは無い)に似ているが、kは人間が決めるのではなくA・B・Q・Rから数学的に計算する。
この部分(LQRゲインを求める)はPythonで書くと、
import numpy as np
from scipy.linalg import solve_continuous_are
# x = [theta, omega]
A = np.array([[0.0, 1.0],
[9.0, -0.3]])
B = np.array([[0.0],
[1.0]])
# thetaを特に小さくしたい、という重み
Q = np.diag([40.0, 5.0])
R = np.array([[1.0]])
# Riccati方程式を解く
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.solve(R, B.T @ P)
print(K)
# 例: [[20.0, 6.4149…]]
x = np.array([[0.08], # theta [rad]
[0.20]]) # omega [rad/s]
u = -K @ x
print(float(u[0, 0]))
この場合は、LQRが K ≈ [20.0, 6.41] を計算している。つまり、θ（傾き）と ω（角速度）に、それぞれ数学的に決めた重みを掛けて操作することになる。
次はAdaptive制御・・・の予定
