はじめに
本記事では電圧入力モータとエンコーダでサーボ系を作成した際に、単純なLQR制御だと収束しなかったため静止摩擦を考慮したものです。参考になれば幸いです。
LQR制御とは
LQR制御については以下の記事で詳細が説明されています。本記事では、実際に作成した状態方程式や静止摩擦モデルについて説明していきます。
電圧入力サーボ系の状態方程式
入力が電圧$u$で、出力が角度$\theta$のモータを角度制御する際の状態方程式を考えます。
1. モデルの前提
DCモータの運動方程式と電気回路の式を用いて、状態方程式を導出します。
電気回路の式（電圧 - 電流関係）
モータの電気回路は、インダクタンス$L$と抵抗$R$を持つ回路として記述できます。
V = L \frac{di}{dt} + Ri + K_e \omega
ここで、
- $V$は入力電圧（制御入力）
- $i$はモータ電流
- $L$はモータのインダクタンス
- $R$はモータの抵抗
- $K_e$は逆起電力定数
- $\omega = \dot{\theta}$は角速度
運動方程式（トルク - 角加速度関係）
モータの回転運動は次の式で記述されます。
J \frac{d\omega}{dt} = K_t i - B \omega
ここで、
- $J$は慣性モーメント
- $K_t$はトルク定数
- $B$は粘性摩擦係数
2. 状態空間表現
状態変数を
x_1 = \theta, \quad x_2 = \omega, \quad x_3 = i
と定義すると、状態方程式は以下のように表されます。
\begin{align}
\frac{d}{dt}
\begin{bmatrix}
x_1 \\ x_2 \\ x_3
\end{bmatrix}
&=
A
\begin{bmatrix}
x_1 \\ x_2 \\ x_3
\end{bmatrix}
+
B
V
\\
&=
\begin{bmatrix}
0 & 1 & 0 \\
0 & -\frac{B}{J} & \frac{K_t}{J} \\
0 & -\frac{K_e}{L} & -\frac{R}{L}
\end{bmatrix}
\begin{bmatrix}
x_1 \\ x_2 \\ x_3
\end{bmatrix}
+
\begin{bmatrix}
0 \\ 0 \\ \frac{1}{L}
\end{bmatrix}
V
\end{align}
出力方程式（角度）は
\begin{align}
y &=C\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}\\
&= \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}
\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}
\end{align}
これが、入力$V$に対して角度$\theta$を出力するDCモータの線形な状態方程式です。
通常のLQR制御での解法
LQR制御では、以下の二次評価関数を最小化する制御入力$u$を求めます。
J = \int_0^\infty \left( x^T Q x + u^T R u \right) dt
ここで、
- $Q$は状態の重み行列（大きいほどその状態を減らすように制御）
- $R$は入力の重み（大きいほど入力を小さく抑えようとする）
LQR制御による最適制御入力は
u = -K x
となり、ゲイン行列 ( K ) は リカッチ方程式を解くことで求まります。
(解き方は上述の記事を参考にしてください。)
A^T P + P A - P B R^{-1} B^T P + Q = 0
この方程式を解いて行列$P$を求め、フィードバックゲイン$K$は
K = R^{-1} B^T P
によって得られます。
静止摩擦の導入
静止摩擦力は厳密にはシステムの角速度が0の場合に発生しますが、ここでは以下のような連続関数でモデル化できることを仮定します。
T_s(\omega) = -T_{max} \exp(\frac{\omega}{\omega_s})^2 \tanh(\frac{\theta}{\epsilon})
ここで、
- $T_s(\omega)$は静止摩擦トルク
- $T_{max}$は最大静止摩擦トルク
- $\omega_s$は静止摩擦力が支配的な最大速度
- $\epsilon$は十分小さい角度
ハイパボリックタンジェント($\tanh$)で角度の残差$\theta$の符号に合わせる点がポイントです。
ここを角速度$\omega$にすると摩擦で止まったときに摩擦力が0になります。
ハイパボリックタンジェントの他に、符号関数($\rm{sgn}$)を使うこともできますが、この場合も同様に$\theta$のほうが都合が良いです。
この摩擦トルクをシンプルに状態方程式に適用すると以下のようになる。
\frac{d}{dt}
\begin{bmatrix}
x_1 \\ x_2 \\ x_3
\end{bmatrix}
=
\begin{bmatrix}
0 & 1 & 0 \\
0 & -\frac{B}{J} & \frac{K_t}{J} \\
0 & -\frac{K_e}{L} & -\frac{R}{L}
\end{bmatrix}
\begin{bmatrix}
x_1 \\ x_2 \\ x_3
\end{bmatrix}
+
\begin{bmatrix}
0 \\ 0 \\ \frac{1}{L}
\end{bmatrix}
V
+
\begin{bmatrix}
0 \\ 1 \\ 0
\end{bmatrix}
T_s(\omega)
しかし、T_s(\omega)の挙動が非線形なので、この状態方程式は簡単に解くことができません。
そこで、一度T_s(\omega)を状態方程式から分離して、T_s(\omega)が入力Vに与える影響を考えることにしました。
静止摩擦トルクT_s(\omega)を出力するには、先のトルク - 角加速度関係の運動方程式から
T_s(\omega) = K_t i
となる電流$i$が流れる必要があります。
ここで、以下のDCモータの電流と電圧の関係式から
i = \frac{V - K_e \omega}{R}
T_s(\omega) = K_t \frac{V - K_e \omega}{R}
したがって
V = \frac{R}{K_t}T_s(\omega) + K_e \omega
これをLQR制御の最適制御入力に加算します。
V
=
-K
\begin{bmatrix}
\theta \\ \omega \\ i
\end{bmatrix}
+\frac{R}{K_t}T_s(\omega) + K_e \omega
以上より、摩擦が考慮された制御入力を作成することができました。
具体的なプログラム
以下に私が作成した摩擦を考慮したLQR制御のPythonスクリプトを示します。
class LQR:
def __init__(self, motor_inductance, motor_redistance, inertia_moment, torque_constant, viscous_friction_coefficient, static_friction_torque, stribeck_vel):
self.motor_inductance = motor_inductance #モータインダクタンス
self.motor_redistance = motor_redistance # モータ抵抗
self.inertia_moment = inertia_moment # 慣性モーメント
self.torque_constant = torque_constant #トルク定数
self.back_emf_constant = torque_constant #逆起電力係数
self.viscous_friction_coefficient = viscous_friction_coefficient #粘性抵抗係数
# 摩擦パラメータ
self.static_friction_torque = static_friction_torque # 静止摩擦トルク 発振しない程度に上げる
self.stribeck_vel = stribeck_vel # Stribeck速度 摩擦の影響を大きく受ける範囲を表す
# システム行列
self.A = np.array([[0, 1, 0],
[0, -self.viscous_friction_coefficient/self.inertia_moment, self.torque_constant / self.inertia_moment],
[0, -self.back_emf_constant / self.motor_inductance, -self.motor_redistance / self.motor_inductance]])
self.B = np.array([[0],
[0],
[1/self.motor_inductance]])
self.C = np.array([[1, 0, 0]]) # 観測行列
# LQR の重み行列
Q = np.diag([10, 1, 1]) # 角度の誤差を大きく penalize
R = np.array([[1]]) # 入力の重み
# リカッチ方程式を解いて LQR のゲインを求める
P = scipy.linalg.solve_continuous_are(self.A, self.B, Q, R)
self.lqr_gain = np.linalg.inv(R) @ self.B.T @ P
self.input = np.array([[0]])
self.x = np.array([[0.0],
[0.0],
[0.0]])
self.old_x = 0.0
def state_update(self, x, dt):
self.x[0][0] = x
self.x[1][0] = (x - self.old_x) / dt
self.x[2][0] = (self.input[0][0] - self.back_emf_constant * self.x[1][0]) / self.motor_redistance
self.old_x = x
def update(self, x, dt):
self.state_update(x, dt)
self.input = -self.lqr_gain @ self.x
friction_torque = (self.static_friction_torque * np.exp(- (self.x[1][0] / self.stribeck_vel) ** 2)) * math.tanh(self.x[0][0] / 0.000001)
self.input[0][0] += (friction_torque + self.torque_constant * self.back_emf_constant * self.x[1][0] / self.motor_redistance) / (self.torque_constant / self.motor_redistance)
return self.input[0][0]
まとめ
本記事では、非線形で扱いにくい摩擦に関する部分を分離して入力との関係を求めることで、摩擦を考慮した制御入力を生成する方法について作成しました。
制御についてはまだまだ勉強中なので拙い点や間違っている点もあるかと思いますが、参考になれば幸いです。