LQRで経路追従

動作動画

経路追従

LQR

状態方程式の導出

目標経路への追従が目的であるため、偏差系の状態方程式を構築する。今回の状態方程式に関するロボットモデルと目標経路をFig 1に示す。また変数をTable 1に示す。

Table 1. LQRの変数

パラメータ名	機能
$e_d$	ロボット位置と目標経路の距離差
$\dot{e_d}$	$e_d$の時間微分した値（速度）
$\theta$	ロボット角度
$\theta_r$	目標経路の角度
$e_{\theta}$	ロボット角度と目標経路の角度差
$\dot{e_{\theta}}$	$\theta$の時間微分した値（角速度）
$v_t$	ロボットの並進速度
$\omega_t$	ロボットの角速度
$d_t$	制御周期

Fig 1. モデル図

Fig 2. $e_d$の導出

状態変数は

    X_t=\left[
    \begin{matrix} e_d \\ \dot{e_d} \\ e_{\theta} \\ \dot{e_{\theta}} \end{matrix}
    \right]

であり、制御量$u_t$はロボットへの角速度$\omega_t$とする。

    u_t = \omega_t

状態変数$e_{\theta}$は目標角度$ \theta_r$と$\theta$の差であり、

    e_{\theta} = \theta_r - \theta

$e_{\theta}$を時間微分した$\dot{e_{\theta}}$は角速度であるため、制御量$u_t$と$\dot{e_{\theta}}$は等しい。

    \dot{e_{\theta}} = u_t = \omega_t

状態変数$e_d$（Fig 2）はロボット位置と目標経路の距離差であるため、$e_d$を時間微分した$\dot{e_d}$は

    \dot{e_d} = v_t\cos{e_{\theta}}\sin{e_{\theta}}

$e_{\theta}$が小さいという前提を利用して線形化すると

    \cos{e_{\theta}} = 1

    \sin{e_{\theta}} = e_{\theta}

    \dot{e_d} = v_t e_{\theta}

ここまでをまとめると、状態方程式は以下のとおりになる。

\begin{align}
    X_{t+1} \nonumber\\
    &= AX_t + Bu_t \nonumber\\
    &= \left[
        \begin{matrix} 1 & dt & 0 & 0\\ 0 & 0 & v_t & 0\\ 0 & 0 & 1 & dt\\ 0 & 0 & 0 & 0 \end{matrix}
        \right]
        \left[
        \begin{matrix} e_d \\ \dot{e_d} \\ e_{\theta} \\ \dot{e_{\theta}} \end{matrix}
        \right]
        +
        \left[
        \begin{matrix} 0 \\ 0 \\ 0 \\ 1 \end{matrix}
        \right]\omega_t
\end{align}

フィードバックゲインの導出

フィードバックゲイン$K$は式(1)の$Z$が変化しなくなるまで更新し続けた$Z$を、式(2) に利用することで求まる。

$$
Z = A^TZA-A^TZB(R+B^TZB)^{-1}B^TZA+Q \tag{1}
$$

$Q$は状態に関係する重み行列。$Q$の各行は$e_d$, $\dot{e_d}$,$ e_{\theta} $, $\dot{e_{\theta}}$に対応しているから、特に素早く収束させたい状態のパラメータを大きくする。$R$は制御量に関係する重み行列。今回は制御量が$\omega_t$のみであるため、$R$は行列ではなくスカラとなる。

    Q=\left[
    \begin{matrix} q_1 & 0 & 0 & 0\\ 0 & q_2 & 0 & 0\\ 0 & 0 & q_3 & 0\\ 0 & 0 & 0 & q_4 \end{matrix}
    \right]

    K=(B^TZB+R)^{-1}(B^TZA)\tag{2}

    u_t = \omega_t = -K X_t \tag{3}

式(3)で求めた制御量$\omega_t$はロボットに入力できないため、モーター入力に変換する必要がある。Fig 3は今回使用するロボットであるRoombaの角速度とモータ入力の関係図。近似直線から求めた、傾き$0.0179$と切片$-0.2826$からモータ入力に変換する。

    MotorInput = \frac{1}{0.0179} \omega_t - 0.2826

Fig 3. ロボット角速度[rad/s] - モータ入力

実験

実験では、目標経路のスプライン曲線とオドメトリから測定した経路の誤差を測定する。目標経路は2種類のスプライン曲線を生成して、LQRによって追従させる。ロボットはRoombaを使用した。
動作動画：https://www.youtube.com/watch?v=XXuZxzfc1EI

結果

経路1

1種類目の目標経路と追従結果をFig 4とFig 5に示す。追従誤差は最大で約0.025[m]となった。

Fig 4. 目標経路1

Fig 5. 目標経路1の追従誤差

経路2

2種類目の目標経路と追従結果をFig 6とFig 7に示す。追従誤差は最大で約0.025[m]となった。

Fig 6. 目標経路2

Fig 7. 目標経路2の追従誤差

参考

• https://myenigma.hatenablog.com/entry/2017/08/08/153651
• https://myenigma.hatenablog.com/entry/2016/11/05/033219
• https://qiita.com/taka_horibe/items/47f86e02e2db83b0c570#%E8%BB%8A%E4%B8%A1%E3%81%AE%E8%BB%8C%E9%81%93%E8%BF%BD%E5%BE%93%E5%95%8F%E9%A1%8C%E9%9D%9E%E7%B7%9A%E5%BD%A2%E3%81%AB%E9%81%A9%E7%94%A8%E3%81%99%E3%82%8B
• Tao Yang, Ziwen Bai, Zhiqiang Li, Nenglian Feng and Liqing Chen, "Intelligent Vehicle Lateral Control Method Based on
  Feedforward + Predictive LQR Algorithm"