DigitalServo

Blog

About

Library

    線形制御系設計

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

    モーションコントロール

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

    モータドライブ

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

    システム同定

    ホワイトボックスモデル

作業空間加速度制御

一般にモータは機械システムに組み込まれて使用することを想定しており，機械システムの状態量を制御することが要求される。ここで，個々のモータを制御する座標系を関節空間，機械システムのタスクが定義される座標系を作業空間と呼ぶ。ここでは機械システムの運動制御系設計の基礎として，作業空間における加速度制御系の構築方法について述べる。

数学的準備

逆問題

多自由度系の制御では関節空間と作業空間での座標変換が必要となり，その相互変換について理解する必要がある。関節空間の状態量 x∈Rn\bm{x}\in\mathbb{R}^{n} を作業空間の状態量 y∈Rm\bm{y}\in \mathbb{R}^{m} に変換する座標変換行列 A∈Rm×n\bm{A}\in \mathbb{R}^{m\times n} が与えられた場合について考える。
Ax=y\begin{align} \bm{A}\bm{x}=\bm{y} \end{align}
ここで，作業空間の状態量 y\bm{y} が与えられた際に関節空間の状態量 x\bm{x} を推定する問題を考える。この問題は連立数が mm の nn 元連立方程式の求解と等価な問題であり，その求解法は mm に依存する。

m=nm = n の場合

連立数と未知変数が等しく，A\bm{A} のランクが nn となる場合に本問題の解は一意に定まる。
x^=A−1y\begin{align} \hat{\bm{x}}=\bm{A}^{-1}\bm{y} \end{align}

m>nm > n の場合

連立数が未知変数より多い，すなわちパラメータに対して制約が過剰であるため一般には解が存在しない (優決定問題)。この問題に対して，与えられた像 y\bm{y} と想定される像 Ax\bm{A}\bm{x} の誤差の L2L_2 ノルムを最小とする x\bm{x} を解として採用する。
x^=arg min⁡x 12∥Ax−y∥22\begin{align} \hat{\bm{x}}=\argmin_{\bm{x}} \ \frac{1}{2}\| \bm{A}\bm{x}-\bm{y} \|^2_2 \end{align}
この解は最小二乗法によって求めることができる。
∂∂x(12∥Ax−y∥22)=0⇔AT(Ax−y)=0⇔ATAx−ATy=0\begin{align} \frac{\partial}{\partial \bm{x}} \left(\frac{1}{2}\| \bm{A}\bm{x}-\bm{y} \|^2_2\right) &= \bm{0} \\ \Leftrightarrow \bm{A}^{\mathrm T}(\bm{A}\bm{x}-\bm{y}) &= \bm{0} \\ \Leftrightarrow \bm{A}^{\mathrm T}\bm{A}\bm{x}-\bm{A}^{\mathrm T}\bm{y} &= \bm{0} \end{align}
∴x^=(ATA)−1ATy\begin{align} \therefore \hat{\bm{x}} = \left(\bm{A}^{\mathrm T}\bm{A}\right)^{-1}\bm{A}^{\mathrm T}\bm{y}\end{align}

m<nm < n の場合

連立数が未知変数より少ない，すなわちパラメータに対して制約が不足するため解が一意に定まらない (劣決定問題)。これは変換行列 A\bm{A} に零空間 Ker(A)\rm Ker({\it \bm{A}}) が存在するためである。
Ker(A):={x ∣ Ax=0}\begin{align} {\rm Ker}(\bm{A}) := \{ \bm{x}\ |\ \bm{A}\bm{x} = \bm{0} \} \end{align}
この問題の一般解について，解候補のうち L2L_2 ノルムを最小とする特殊解 xm\bm{x}_{\rm m} と零空間の一般解 xnull\bm{x}_{\rm null} の和として表現する。
x^=xm+xnullxm≡arg min⁡x 12∥x∥22  s. t. Ax=yxnull=(I−AT(AAT)−1A)k\begin{align} \hat{\bm{x}}&=\bm{x}_{\rm m}+\bm{x}_{\rm null} \\ \bm{x}_{\rm m} &\equiv \argmin_{\bm{x}} \ \frac{1}{2}\|\bm{x}\|^2_2\ \ {\rm s.\ t.\ }\bm{A}\bm{x}=\bm{y} \\ \bm{x}_{\rm null} &= \left(\bm{I}-\bm{A}^{\mathrm T}\left(\bm{A}\bm{A}^{\mathrm T}\right)^{-1}\bm{A}\right)\bm{k} \end{align}
この特殊解 xm\bm{x}_{\rm m} はラグランジュの未定乗数法によって求めることができる。
L(x,λ)=12xTx+λ(Ax−y){∂∂xL(xm,λ)=xm−ATλ=0∂∂λL(xm,λ)=Axm−y=0⇔{xm=ATλAATλ=y⇔{xm=AT(AAT)−1yλ=(AAT)−1y\begin{align} &\mathcal{L}(\bm{x}, \lambda)=\frac{1}{2}\bm{x}^{\mathrm T}\bm{x}+\lambda(\bm{A}\bm{x}-\bm{y})\\ &\left\{\begin{matrix}\frac{\partial}{\partial \bm{x}}\mathcal{L}(\bm{x}_{\rm m}, \lambda)=\bm{x}_{\rm m}-\bm{A}^{\mathrm T}\lambda=\bm{0}\\ \frac{\partial}{\partial \lambda}\mathcal{L}(\bm{x}_{\rm m}, \lambda)=\bm{A}\bm{x}_{\rm m}-\bm{y}=\bm{0}\end{matrix}\right.\\ \Leftrightarrow&\left\{\begin{matrix}\bm{x}_{\rm m}=\bm{A}^{\mathrm T}\lambda\\\bm{A}\bm{A}^{\mathrm T}\lambda=\bm{y}\\\end{matrix}\right.\\ \Leftrightarrow&\left\{\begin{matrix}\bm{x}_{\rm m}=\bm{A}^{\mathrm T}\left(\bm{A}\bm{A}^{\mathrm T}\right)^{-1}\bm{y}\\ \lambda=\left(\bm{A}\bm{A}^{\mathrm T}\right)^{-1}\bm{y}\end{matrix}\right. \end{align}
以上より，本問題の一般解は次のようになる。
x^=AT(AAT)−1y+(I−AT(AAT)−1A)k\begin{align} \hat{\bm{x}} &=\bm{A}^{\mathrm T}\left(\bm{A}\bm{A}^{\mathrm T}\right)^{-1}\bm{y}+\left(\bm{I}-\bm{A}^{\mathrm T}\left(\bm{A}\bm{A}^{\mathrm T}\right)^{-1}\bm{A}\right)\bm{k} \end{align}

一般化逆行列による求解

変換行列 A\bm{A} に対して，一般化逆行列を次のように定める。
A+≡{A−1(m=n)(ATA)−1AT(m>n)AT(AAT)−1(m<n)\begin{align} \bm{A}^{+} &\equiv \left\{\begin{matrix} \bm{A}^{-1} & (m=n) \\ \left(\bm{A}^{\mathrm T}\bm{A}\right)^{-1}\bm{A}^{\mathrm T} & (m>n) \\ \bm{A}^{\mathrm T}\left(\bm{A}\bm{A}^{\mathrm T}\right)^{-1} & (m<n) \end{matrix}\right. \end{align}
このとき，本問題の解は次のように表される。
x^={A+y(m=n)A+y(m>n)A+y+(I−A+A)k(m<n)\begin{align} \hat{\bm{x}}&=\left\{\begin{matrix} \bm{A}^{+}\bm{y} & (m=n) \\ \bm{A}^{+}\bm{y} & (m>n) \\ \bm{A}^{+}\bm{y} + (\bm{I} - \bm{A}^{+}\bm{A})\bm{k} & (m<n) \end{matrix}\right. \end{align}
また，A+\bm{A}^{+} は m≥nm \ge n のとき A\bm{A} の左逆元となり，m≤nm \le n のとき A\bm{A} の右逆元となる。
AA+=A+A=I(m=n)A+A=I(m>n)AA+=I(m<n)\begin{align} \begin{matrix} \bm{A}\bm{A}^{+}=\bm{A}^{+}\bm{A}=\bm{I} & (m=n) \\ \bm{A}^{+}\bm{A}=\bm{I} & (m>n) \\ \bm{A}\bm{A}^{+}=\bm{I} & (m<n) \end{matrix} \end{align}

作業空間加速度制御系の設計方法

運動学

関節空間および作業空間における一般化位置を q,x\bm{q}, \bm{x} とし，次の座標変換 hqh_{q} が与えられた場合について考える。
hq:q∈Rn→x∈Rm\begin{align} h_{q}: \bm{q}\in\mathbb{R}^{n} \rightarrow \bm{x}\in\mathbb{R}^{m} \end{align}
ただし，mm, nn は各座標系上の状態量の次数を表し，冗長度を p=n−mp=n-m と定める。ここでは劣駆動系の制御については検討せず，p≥0p\ge 0 の場合について考える。関節空間および作業空間が可微分多様体であり，写像 hqh_{q} が可微分であるとき，速度と加速度について次のように順変換を記述することができる。
x˙=Jaco(q)q˙x¨=Jaco(q)q¨+J˙aco(q)q˙\begin{align} \dot{\bm{x}} &= \bm{J}_{\rm aco}(\bm{q})\dot{\bm{q}} \\ \ddot{\bm{x}} &= \bm{J}_{\rm aco}(\bm{q})\ddot{\bm{q}} + \dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}} \end{align}
ただし， Jaco(q)\bm{J}_{\rm aco}(\bm{q}) はヤコビ行列を表し，以下のように定義される。
Jaco(q)≡∂hq(q)∂q∈Rm×n\begin{align} \bm{J}_{\rm aco}(\bm{q}) \equiv \frac{\partial h_{q}(\bm{q})}{\partial \bm{q}} \in \mathbb{R}^{m \times n} \end{align}

逆運動学

作業空間加速度 x¨\ddot{\bm{x}} が与えられたとき，関節空間加速度 q¨\ddot{\bm{q}} の一般解は次のように記述される。
q¨=Jaco+(q)(x¨−J˙aco(q)q˙)+q¨nullq¨null≡Jaconull(q)ξ\begin{align} \ddot{\bm{q}}&=\bm{J}_{\rm aco}^{+}(\bm{q})\left(\ddot{\bm{x}}-\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right)+ \ddot{\bm{q}}_{\rm null} \\ \ddot{\bm{q}}_{\rm null} &\equiv \bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi} \end{align}
ここで，Jaco+(q),Jaconull(q)\bm{J}_{\rm aco}^{+}(\bm{q}), \bm{J}_{\rm aco}^{\rm null}(\bm{q}) はヤコビ行列の一般化逆行列および零空間 Ker(Jaco(q))\rm Ker({\it \bm{J}_{\rm aco}\left(\bm{q})\right)} への直行射影を表し，次のように記述される。
Jaco+(q)≡JacoT(q)(Jaco(q)JacoT(q))−1Jaconull(q)≡I−Jaco+(q)Jaco(q)\begin{align} \bm{J}_{\rm aco}^{+}(\bm{q})&\equiv \bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\left(\bm{J}_{\rm aco}(\bm{q})\bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\right)^{-1} \\ \bm{J}_{\rm aco}^{\rm null}(\bm{q}) &\equiv \bm{I}-\bm{J}_{\rm aco}^{+}(\bm{q})\bm{J}_{\rm aco}(\bm{q}) \end{align}
非冗長系 (m=n)(m=n) においては Ker(Jaco(q))={0}\rm Ker({\it \bm{J}_{\rm aco}\left(\bm{q})\right)}=\{\bm{0}\} となり，解は一意に定まる。

作業空間等価慣性の導入

作業空間における力と加速度の関係を記述するために，作業空間におけるプラントの等価慣性を定義する。プラントのエネルギは座標系に依らず不変であるため，関節空間上のプラントの慣性を J(q)\bm{J}(\bm{q})，作業空間上のプラントの等価慣性を M(q)\bm{M}(\bm{q}) として次式が成立する。
12q˙TJ(q)q˙=12x˙TM(q)x˙\begin{align} \frac{1}{2}\dot{\bm{q}}^{\mathrm T}\bm{J}(\bm{q})\dot{\bm{q}} &= \frac{1}{2}\dot{\bm{x}}^{\mathrm T}\bm{M}(\bm{q})\dot{\bm{x}} \end{align}
したがって，各座標系上の慣性に関して以下の等式が成立する。
J(q)=JacoT(q)M(q)Jaco(q)M−1(q)=Jaco(q)J−1(q)JacoT(q)\begin{align} \bm{J}(\bm{q}) &= \bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\bm{M}(\bm{q})\bm{J}_{\rm aco}(\bm{q})\\ \bm{M}^{-1}(\bm{q}) &= \bm{J}_{\rm aco}(\bm{q})\bm{J}^{-1}(\bm{q})\bm{J}^{\mathrm T}_{\rm aco}(\bm{q}) \end{align}

逆動力学

作業空間における目標動作が与えられたとき，それを実現する関節空間の駆動力を求める問題を逆動力学問題という。すなわち，作業空間における力参照値が与えられた際に関節空間の力参照値を導出する問題と見ることができる。以下では一般化力に関する座標変換の逆変換について考える。

関節空間と作業空間の一般化力をそれぞれ τ,f\bm{\tau}, \bm{f} と表現した場合，各座標系における運動方程式は次のように記述される。
τ=J(q)q¨f=M(q)x¨\begin{align} \bm{\tau}&=\bm{J}(\bm{q})\ddot{\bm{q}}\\ \bm{f}&=\bm{M}(\bm{q})\ddot{\bm{x}} \end{align}
作業空間加速度 x¨\ddot{\bm{x}} が与えられたとき，上式と作業空間等価慣性の定義から以下の等式を得る。
τ=J(q){Jaco+(q)(x¨−J˙aco(q)q˙)+q¨null}=J(q)Jaco+(q)M−1(q)(f−M(q)J˙aco(q)q˙)+J(q)q¨null=JacoT(q)(f−M(q)J˙aco(q)q˙)+J(q)q¨null\begin{align} \bm{\tau} &= \bm{J}(\bm{q})\left\{\bm{J}_{\rm aco}^{+}(\bm{q})\left(\ddot{\bm{x}}-\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right) + \ddot{\bm{q}}_{\rm null} \right\} \\ &= \bm{J}(\bm{q})\bm{J}_{\rm aco}^{+}(\bm{q})\bm{M}^{-1}(\bm{q})\left(\bm{f}-\bm{M}(\bm{q})\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right) + \bm{J}(\bm{q})\ddot{\bm{q}}_{\rm null} \\ &= \bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\left(\bm{f}-\bm{M}(\bm{q})\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right) + \bm{J}(\bm{q})\ddot{\bm{q}}_{\rm null} \end{align}
したがって，一般化力の逆変換 hfh_{f} は次のように記述される。
hf:f∈Rm→τ∈Rnhf(f)≡JacoT(q)(f−M(q)J˙aco(q)q˙)+J(q)Jaconull(q)ξ\begin{align} h_{f}&: \bm{f}\in\mathbb{R}^{m} \rightarrow \bm{\tau}\in\mathbb{R}^{n} \\ h_{f}(\bm{f}) &\equiv \bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\left(\bm{f}-\bm{M}(\bm{q})\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right) + \bm{J}(\bm{q})\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi} \end{align}

計算トルク法による実現

作業空間における目標動作が与えられたとき，関節空間の運動を制御することで間接的にその運動を生成することとなる。この関節空間の運動を実現するための制御入力を求めるために計算トルク法を使用する。作業空間上の運動制御のための力参照値 fref\bm{f}^{\rm ref} が与えられたとき，逆動力学より関節空間の力参照値 τref\bm{\tau}^{\rm ref} および加速度参照値 q¨ref\ddot{\bm{q}}^{\rm ref} は次のように算出される。
τref=JacoT(q)(fref−M(q)J˙aco(q)q˙)+J(q)Jaconull(q)ξq¨ref=J−1τref=J−1(q)JacoT(q)(fref−M(q)J˙aco(q)q˙)+Jaconull(q)ξ\begin{align} \bm{\tau}^{\rm ref} &= \bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\left(\bm{f}^{\rm ref}-\bm{M}(\bm{q})\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right) +\bm{J}(\bm{q})\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi} \\ \ddot{\bm{q}}^{\rm ref} &= \bm{J}^{-1}\bm{\tau}^{\rm ref}\\ &=\bm{J}^{-1}(\bm{q})\bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\left(\bm{f}^{\rm ref}-\bm{M}(\bm{q})\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right) + \bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi} \end{align}
ここで，作業空間の等価慣性 M(q)\bm{M}(\bm{q}) を仮想的に I\bm{I} に固定し，作業空間上で加速度制御を実現することを考える。これは関節空間の仮想慣性を Jv(q)\bm{J}_{\rm v}(\bm{q}) として，以下のように制御することで達成される。
Jv(q)=JacoT(q)Jaco(q)\begin{align} \bm{J}_{\rm v}(\bm{q}) = \bm{J}_{\rm aco}^{\mathrm T}(\bm{q})\bm{J}_{\rm aco}(\bm{q}) \end{align}
この関節空間の仮想慣性制御が達成されるとき，次の等式が成立する。
M(q)=IJv−1(q)JacoT(q)=Jaco+(q)\begin{align} \bm{M}(\bm{q}) &= \bm{I} \\ \bm{J}^{-1}_{\rm v}(\bm{q})\bm{J}^{\mathrm T}_{\rm aco}(\bm{q}) &= \bm{J}_{\rm aco}^{+}(\bm{q}) \end{align}
上式を使用して，関節空間における加速度参照値を以下のように決定する。
q¨ref=Jaco+(q)(fref−J˙aco(q)q˙)+Jaconull(q)ξ\begin{align} \ddot{\bm{q}}^{\rm ref} &= \bm{J}_{\rm aco}^{+}(\bm{q})\left(\bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right)+\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi} \end{align}

動力学の確認

作業空間において力参照値 fref\bm{f}^{\rm ref} が与えられ，計算トルク法によって関節空間の加速度参照値が決定される場合について考える。関節空間にて加速度制御が達成される場合，次式が成立する。
q¨=q¨ref=Jaco+(q)(fref−J˙aco(q)q˙)+Jaconull(q)ξ\begin{align} \ddot{\bm{q}} &= \ddot{\bm{q}}^{\rm ref} \\ &=\bm{J}_{\rm aco}^{+}(\bm{q})\left(\bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right)+\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi} \end{align}
このとき，作業空間の加速度 x¨\ddot{\bm{x}} は次の値を取る。
x¨=Jaco(q)q¨+J˙aco(q)q˙=Jaco(q)Jaco+(q)(fref−J˙aco(q)q˙)+Jaco(q)Jaconull(q)ξ+J˙aco(q)q˙=fref\begin{align} \ddot{\bm{x}} &= \bm{J}_{\rm aco}(\bm{q})\ddot{\bm{q}} + \dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}} \\ &=\bm{J}_{\rm aco}(\bm{q})\bm{J}_{\rm aco}^{+}(\bm{q})\left(\bm{f}^{\rm ref}-\dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}}\right)+\bm{J}_{\rm aco}(\bm{q})\bm{J}_{\rm aco}^{\rm null}(\bm{q})\bm{\xi} + \dot{\bm{J}}_{\rm aco}(\bm{q})\dot{\bm{q}} \\ &= \bm{f}^{\rm ref} \end{align}
以上より，作業空間上で加速度制御が実現されることが確認できた。

CONTENTS

© DigitalServo