DigitalServo

Blog

About

Library

    線形制御系設計

    線形システムの表現
    線形システムの特性解析
    制御器設計
    観測器設計

    モーションコントロール

    加速度制御
    剛体の姿勢制御

    モータドライブ

    駆動制御法
    磁極位置推定法

    システム同定

    ホワイトボックスモデル

姿勢制御器の設計

関節空間の加速度制御系の構築

外乱オブザーバの設計

機体慣性を J\bm{J}、機体に作用するトルクを τ\bm{\tau} とすると、機体角速度 ω\bm{\omega} は次のように記述される。
ω˙=J−1(S(Jω)ω+τ)\begin{align} \dot{\bm{\omega}}&=\bm{J}^{-1}\left(S(\bm{J}\bm{\omega})\bm{\omega}+\bm{\tau}\right) \end{align}
ただし、S(⋅)S(\cdot) は S(x)y=x×y (x, y∈R3)S(\bm{x})\bm{y}=\bm{x}\times \bm{y}\ (\bm{x},\ \bm{y} \in \mathbb{R}^3) を満たす歪対称行列を表す。ここで、作用トルク τ\bm{\tau} を制御入力 τu\bm{\tau}_{\rm u} と 外乱 τd\bm{\tau}_{\rm d} に分けて記述する。
ω˙=J−1(S(Jω)ω+τu+τd)\begin{align} \dot{\bm{\omega}}&=\bm{J}^{-1}\left(S(\bm{J}\bm{\omega})\bm{\omega}+\bm{\tau}_{\rm u}+\bm{\tau}_{\rm d}\right) \end{align}
外乱トルク τd\bm{\tau}_{\rm d} を推定し、除去することを目的として外乱オブザーバを設置する。外乱オブザーバを導出するため、系の支配方程式から以下の拡張状態空間表現を得る。ただし、外乱トルクの生成モデルとして τ˙d=0\dot{\bm{\tau}}_{\rm d}=\bm{0} を採用した。
ddt[τdω]=[00J−10][τdω]+[0J−1]u+vy=[0I][τdω],\begin{align} \frac{d}{dt} \begin{bmatrix} \bm{\tau}_{\rm d} \\ \bm{\omega} \end{bmatrix} &= \begin{bmatrix} \bm{0} & \bm{0} \\ \bm{J}^{-1} & \bm{0} \end{bmatrix} \begin{bmatrix} \bm{\tau}_{\rm d} \\ \bm{\omega} \end{bmatrix} + \begin{bmatrix} \bm{0} \\ \bm{J}^{-1} \end{bmatrix} \bm{u}+\bm{v}\\ \bm{y} &= \begin{bmatrix} \bm{0} & \bm{I} \end{bmatrix} \begin{bmatrix} \bm{\tau}_{\rm d} \\ \bm{\omega} \end{bmatrix}, \end{align}
ここで、v\bm{v} は駆動雑音、系の摂動および τd˙\dot{\bm{\tau}_{\rm d}} を含むモデル化誤差等のプロセス雑音、u\bm{u} は拡張状態空間表現における入力量とし、以下のように定義した。
u≡S(Jω)ω+τu\begin{align} \bm{u}&\equiv S(\bm{J}\bm{\omega})\bm{\omega}+\bm{\tau}_{\rm u} \end{align}
機体慣性 J\bm{J} が既知かつ ω\bm{\omega} が観測可能である場合に S(Jω)ωS(\bm{J}\bm{\omega})\bm{\omega} は予測可能量となり、系への既知入力と見做すことができる。この拡張状態空間表現に対して最小次元オブザーバを構築することで、外乱オブザーバを得る。
τ^d=−Q(u^−sJ^ω)\begin{align} \hat{\bm{\tau}}_{\rm d}&=-\bm{Q}(\hat{\bm{u}} -s\hat{\bm{J}}\bm{\omega}) \end{align}
ただし、上付き文字 X^\hat{\bm{X}} は X\bm{X} の推定値を表すものとし、Q\bm{Q} は推定式をプロパとするフィルタとする。

機体角加速度制御系の設計

推定外乱 τ^d\hat{\bm{\tau}}_{\rm d} を用いた外乱抑圧機構を設置し，機体角加速度制御を実現する。制御入力 τu\bm{\tau}_{\rm u} を参照入力 τref\bm{\tau}^{\rm ref} と推定外乱 τ^d\hat{\bm{\tau}}_{\rm d} によって次のように構成する。
τu=τref−τ^d.\begin{align} \bm{\tau}_{\rm u}&=\bm{\tau}^{\rm ref}-\hat{\bm{\tau}}_{\rm d}. \end{align}
参照入力 τref\bm{\tau}^{\rm ref} はノミナルプラントに対する追従特性を確立することを目的として，以下のように設計する。
τref=J^αref−S(J^ω)ω\begin{align} \bm{\tau}^{\rm ref} = \hat{\bm{J}}\bm{\alpha}^{\rm ref}-S(\hat{\bm{J}}\bm{\omega})\bm{\omega} \end{align}
ただし，αref\bm{\alpha}^{\rm ref} は機体角加速度参照値を表す。また，この制御則に従う場合に次の代数方程式が成立する。
sω=J−1(S(Jω)ω+τu+τd)τu=τref+Q(S(J^ω)ω+τu−sJ^ω),\begin{align} s\bm{\omega}&=\bm{J}^{-1}\left(S(\bm{J}\bm{\omega})\bm{\omega}+\bm{\tau}_{\rm u}+\bm{\tau}_{\rm d} \right) \\ \bm{\tau}_{\rm u}&=\bm{\tau}^{\rm ref}+\bm{Q}(S(\hat{\bm{J}}\bm{\omega})\bm{\omega}+\bm{\tau}_{\rm u} -s\hat{\bm{J}}\bm{\omega}), \end{align}
ここで，制御入力に関する式を整理して以下の等式を得る。
τu=τref+Q(τu+S(J^ω)ω−J^J−1(S(Jω)ω+τu+τd))⇔(R+T)τu=τref+QS(J^ω)ω−RS(Jω)ω−RτdR≡QJ^J−1T≡I−Q\begin{align} \bm{\tau}_{\rm u}&=\bm{\tau}^{\rm ref}+\bm{Q}(\bm{\tau}_{\rm u} + S(\hat{\bm{J}}\bm{\omega})\bm{\omega} - \hat{\bm{J}}\bm{J}^{-1}\left(S(\bm{J}\bm{\omega})\bm{\omega}+\bm{\tau}_{\rm u}+\bm{\tau}_{\rm d} \right)) \\ \Leftrightarrow \left( \bm{R} + \bm{T} \right) \bm{\tau}_{\rm u} &= \bm{\tau}^{\rm ref} + \bm{Q}S(\hat{\bm{J}}\bm{\omega})\bm{\omega} - \bm{R}S(\bm{J}\bm{\omega})\bm{\omega} - \bm{R}\bm{\tau}_{\rm d} \\ \bm{R} &\equiv \bm{Q}\hat{\bm{J}}\bm{J}^{-1} \\ \bm{T} &\equiv \bm{I} - \bm{Q}\end{align}
これを用いてダイナミクスに関する式を整理する。
s(R+T)Jω=(R+T)τu+(R+T){S(Jω)ω+τd}=τref+(QS(J^ω)+TS(Jω))ω+Tτd=J^αref+T(S(Jω)−S(J^ω))ω+Tτd\begin{align} s\left(\bm{R} + \bm{T} \right)\bm{J}\bm{\omega}&= \left( \bm{R} + \bm{T} \right) \bm{\tau}_{\rm u} + \left( \bm{R} + \bm{T} \right)\left\{ S(\bm{J}\bm{\omega})\bm{\omega} + \bm{\tau}_{\rm d} \right\} \\ &=\bm{\tau}^{\rm ref} + \left(\bm{Q}S(\hat{\bm{J}}\bm{\omega}) + \bm{T}S(\bm{J}\bm{\omega})\right)\bm{\omega} +\bm{T}\bm{\tau}_{\rm d}\\ &=\hat{\bm{J}}\bm{\alpha}^{\rm ref} + \bm{T}\left(S(\bm{J}\bm{\omega}) - S(\hat{\bm{J}}\bm{\omega})\right)\bm{\omega} +\bm{T}\bm{\tau}_{\rm d}\\ \end{align}
以上より，機体角速度に関するダイナミクスは次のように記述される。
sω=Tααref+J−1τeTα≡J−1(QJ^J−1+T)−1J^τe≡(QJ^J−1+T)−1T[(S(Jω)−S(J^ω))ω+τd]\begin{align} s\bm{\omega}&=\bm{T}_{\alpha}\bm{\alpha}^{\rm ref} + \bm{J}^{-1}\bm{\tau}_{\rm e} \\ \bm{T}_{\alpha} &\equiv \bm{J}^{-1}\left(\bm{Q}\hat{\bm{J}}\bm{J}^{-1} + \bm{T} \right)^{-1} \hat{\bm{J}}\\ \bm{\tau}_{\rm e} &\equiv \left(\bm{Q}\hat{\bm{J}}\bm{J}^{-1}+\bm{T}\right)^{-1} \bm{T} \left[ \left(S(\bm{J}\bm{\omega})-S(\hat{\bm{J}}\bm{\omega}) \right) \bm{\omega} + \bm{\tau}_{\rm d} \right] \end{align}
外乱オブザーバの推定帯域内では Q≈I\bm{Q}\approx\bm{I} かつ T≈0\bm{T}\approx\bm{0} となり，Tα≈I\bm{T}_{\alpha}\approx\bm{I} および τe≈0\bm{\tau}_{\rm e}\approx\bm{0} となる。したがって，外乱推定帯域内において機体角加速度制御が実現する。

多様体上の加速度制御系の構築

機体の角加速度制御が実現していることを前提として、SO(3)\rm SO(3) において加速度制御系を構築することを考える。ここでは機体座標系の一般化速度を機体角速度 ω\bm{\omega}、SO(3)\rm SO(3) の一般化位置をクオタニオン q\bm{q} とする。ここで、クオタニオン q\bm{q} は以下の制約を満たすものとする。
q≡{[r0r]∈R4∣r02+rTr=1} \begin{align} \bm{q} &\equiv \left\{ \left. \begin{bmatrix} r_0 \\ \bm{r} \end{bmatrix} \in \mathbb{R}^4 \right| r_0^2 + \bm{r}^{\mathrm T}\bm{r} = 1 \right\} \\ \end{align}
また、機体角速度 ω\bm{\omega} とクオタニオン q\bm{q} の関係は次のように記述される。
q˙=12[−r1−r2−r3r0−r3r2r3r0−r1−r2r1r0]ω≡Jq(q)ω, \begin{align} \dot{\bm{q}} &= \frac{1}{2} \begin{bmatrix} -r_1 & -r_2 & -r_3 \\ r_0 & -r_3 & r_2\\ r_3 & r_0 & -r_1\\ -r_2 & r_1 & r_0 \end{bmatrix} \bm{\omega} \\ &\equiv\bm{J}_{q}(\bm{q})\bm{\omega}, \end{align}
ただし、Jq(q)\bm{J}_{q}(\bm{q}) はヤコビ行列を表す。上式より、 SO(3)\rm SO(3) における状態変数の次元は 44、角速度の次元は 33 であり、一見するとシステムは劣駆動系に見える。しかしながら、クオタニオンのノルムに関する制約により状態変数の選択自由度が低下し、実質的には 33 個の状態変数によって SO(3)\rm SO(3) における姿勢を表現できる。そこで、一般化位置 r∈R3\bm{r} \in \mathbb{R}^{3} に対して制御を行う。本制御では、各座標系の一般化速度の関係から導出されるヤコビ行列を使用する。
r˙=12[r0−r3r2r3r0−r1−r2r1r0]ω≡Jr(q)ω, \begin{align} \dot{\bm{r}} &= \frac{1}{2} \begin{bmatrix} r_0 & -r_3 & r_2\\ r_3 & r_0 & -r_1\\ -r_2 & r_1 & r_0 \end{bmatrix} \bm{\omega} \\ &\equiv\bm{J}_{r}(\bm{q})\bm{\omega}, \end{align}
ここで、作業空間加速度制御の設計法より、機体角加速度参照値 αref\bm{\alpha}^{\rm ref} を次のように設計することで r\bm{r} の一般化加速度制御が達成される。ただし、fref\bm{f}^{\rm ref} を SO(3)\rm SO(3) の一般化力とする。
αref=Jr−1(q)(fref−J˙r(q)r˙)≈Jr−1(q)fref\begin{align} \bm{\alpha}^{\rm ref} &= \bm{J}_{r}^{-1}(\bm{q}) \left( \bm{f}^{\rm ref} - \dot{\bm{J}}_{r}(\bm{q})\dot{\bm{r}} \right) \\ &\approx \bm{J}_{r}^{-1}(\bm{q}) \bm{f}^{\rm ref} \end{align}

姿勢制御系の構築

以上の設計により SO(3)\rm SO(3) において加速度制御が実現するため、加速度制御に基づく運動制御より単純な制御器によって姿勢制御系を構築することができる。以下では SO(3)\rm SO(3) の一般化力参照値 fref\bm{f}^{\rm ref} を次のように設計することを考える。　
fref=Cr(rcmd−r)\begin{align} \bm{f}^{\rm ref} &= \bm{C}_{r}(\bm{r}^{\rm cmd} - \bm{r}) \end{align}
ただし、Cr,rcmd\bm{C}_{r}, \bm{r}^{\rm cmd} はそれぞれクオタニオン制御器、一般化位置指令値を表す。加速度制御が実現する場合、一般化位置に関して以下の代数方程式が成立する。
s2r=Cr(rcmd−r)⇔r=(s2I+Cr)−1Crrcmd\begin{align} s^2\bm{r} &= \bm{C}_{r}(\bm{r}^{\rm cmd} - \bm{r}) \\ \Leftrightarrow \bm{r} &= (s^2\bm{I} + \bm{C}_{r})^{-1}\bm{C}_{r}\bm{r}^{\rm cmd} \end{align}
クオタニオン制御器の設計により、系の減衰率および周波数応答を調整することができる。

計算トルク法による制御入力の決定

姿勢制御系に含まれるカスケード構造について整理し，機体に対するトルク入力 τu\bm{\tau}_{\rm u} の計算方法を確認する。クオタニオン指令値 rcmd\bm{r}^{\rm cmd} が与えられたとき，SO(3)\rm SO(3) の一般化力参照値 fref\bm{f}^{\rm ref} は次のように計算された。
fref=Cr(rcmd−r)\begin{align} \bm{f}^{\rm ref} &= \bm{C}_{r}(\bm{r}^{\rm cmd} - \bm{r}) \end{align}
続いて，SO(3)\rm SO(3) において加速度制御を実現するため，機体角加速度参照値は次のように計算される。
αref=Jr−1(q)fref=Jr−1(q)Cr(rcmd−r)\begin{align} \bm{\alpha}^{\rm ref} &= \bm{J}_{r}^{-1}(\bm{q}) \bm{f}^{\rm ref} \\ &= \bm{J}_{r}^{-1}(\bm{q})\bm{C}_{r}(\bm{r}^{\rm cmd} - \bm{r}) \end{align}
最後に，機体角加速度制御を実現するため，トルク入力は次のように算出される。
τu=J^αref−S(J^ω)ω−τ^d=J^Jr−1(q)Cr(rcmd−r)−S(J^ω)ω−τ^d\begin{align} \bm{\tau}_{\rm u}&=\hat{\bm{J}}\bm{\alpha}^{\rm ref} - S(\hat{\bm{J}}\bm{\omega})\bm{\omega} -\hat{\bm{\tau}}_{\rm d} \\ &=\hat{\bm{J}}\bm{J}_{r}^{-1}(\bm{q})\bm{C}_{r}(\bm{r}^{\rm cmd} - \bm{r}) - S(\hat{\bm{J}}\bm{\omega})\bm{\omega} -\hat{\bm{\tau}}_{\rm d} \end{align}
本制御系において設計自由度が与えられるシステムはクオタニオン制御器と外乱オブザーバのみであり，簡潔な設計によって性能指定を行うことができる。

CONTENTS

© DigitalServo