articles:rmrc [RoboLab.Suehiro]

この文書は読取専用です。文書のソースを閲覧することは可能ですが、変更はできません。もし変更したい場合は管理者に連絡してください。
====== 分解運動（速度）制御 ======

[[articles:jacobian_matrix|ヤコビ行列]]でも述べたが，
望ましい手先座標系（位置・姿勢）の微小変位（速度）を実現する
関節パラメタの微小変位（速度）を
ヤコビ行列（の逆行列）を用いることにより求めることができる．

[[articles:ik_numerical|逆運動学の数値解法]]では，
これを用いて目標座標系に連続的に直線的に遷移させ
それを実現する関節パラメタへと収束させることで
逆運動学問題を陽に解くことなく逆運動学解を得た．

逆運動学を数値的に解く過程で実際のロボットアームを動作させれば
ロボットアームの手先座標系が直線的に目標座標系へと
移動することは容易に理解できるだろう．
その過程では，軌道の分割点での明示的な逆運動学解の計算が
不要であることはいうまでもない．

このような動作制御手法を分解運動速度制御（resolved motion rate control）と呼ぶ．

なぜ分解運動「速度」制御かといえば,ヤコビ行列が表す関係は微小量,
すなわち単位時間で考えれば速度となること，
また多くのロボットアーム（モータ）のサーボアンプが速度制御モードを備えていて
直接速度を指示することができること，
などの事情による
((動力学計算によりコリオリ力，重力などの非線形項を補償し，
ヤコビ行列を加速度に適用した制御手法（計算トルク法）などもある))．

分解運動速度制御を用いると単純な直線的な動作だけでなく任意の軌道をたどることができる．

またサーボ系として利用すれば，手先座標系（位置・姿勢）に対する三次元空間での制御則を実現できる．
これと力センサフィードバックと組み合わせれば
位置・姿勢の６軸ごとに制御則を変えたハイブリッド制御や
ダンピング制御やインピーダンス制御を行うこともできる．

このヤコビ行列を用いた制御系の構成は手先座標系に対してだけでなく，
関節角に対するヤコビ行列が求められれば任意のパラメタに対して
適用できる．
そのパラメタがセンサ情報で得られるものであれば
そのセンサ値直接的にサーボループに組み込むこともできる．
<!--
これは基本的には[[articles:newton_raphson|ニュートン・ラフソン法]]や
[[articles:gauss_newton|ガウス・ニュートン法]]に基づいているので
いろいろと応用ができるのは当然である．
-->

ここでは分解運動速度制御を用いた種々のフィードバック制御法について述べる．
これを用いたアームの軌道生成については[[articles:trajectory|こちら]]で別途述べる．

===== 手先座標系の制御 =====

手先座標系を目標座標系に一致させる制御法を考える．

これは基本的には逆運動学の数値解法と同じであるが，
違いはステップ幅の決め方である．

ステップ幅といっても制御の場合は連続系で扱い
それをサンプリング周期を考慮してステップ幅に落とし込むというのが
考えやすい．

==== 比例制御 ====

まずは単純な比例制御を扱う．
下位のサーボ系が速度制御モードである場合は
安定で分かりやすい制御となる．

目標座標系を$T_{ref}$，現在の手先座標系を$T$とすると，
その差分は[[articles:ik_numerical#２．目標との差の計算|逆運動学の数値解法]]での
方法と同様に求めることができる．それを$\boldsymbol{e}$とする．

ここで注意してほしい点は，逆運動学の数値解法の計算では，現在値から目標値への差分を求めている
という点である．

一般に制御系の誤差ベクトルを考えるときは目標値から現在値への差分とすることが多い．
しかし，現時点でのヤコビ行列を使うことを考えれば，その逆の方が分かりやすい．

ここでもその考え方を踏襲する．その場合，比例ゲインの符号が一般には負であるのが
正に反転することに注意して欲しい．

比例制御則は以下のように書ける．

$$
\boldsymbol{v}=k\boldsymbol{e}
$$

$\boldsymbol{v}$は手先座標系の並進速度および回転速度である．

$k$は比例ゲインであり正の定数である．これを対角行列として要素ごとに
ゲインを指定することもできるが，それぞれの収束の時定数が異なることになり
この単純な制御においてはあまり望ましくないだろう．

これをヤコビ行列をを用いて，
$$
\boldsymbol{\dot{\theta}}=J^+\boldsymbol{v}
$$
とすることで，分解運動速度制御による手先座標系の比例制御が実現できる．

下位のサーボ系が速度制御雨の場合はこの値を指令値として入力する．

関節角の刻み幅はこれにサンプル周期を乗じたものになる．
下位のサーボ系が位置制御の場合はそれを次の目標値として入力することになる．

下位のサーボ系の速度制御が理想的であればこの伝達関数は，
$$
G(s)=\frac{1}{1+s/k}
$$
という単純な一時遅れ系になり$k$は時定数の逆数となっている．

=== 注意点など ===

この手法の注意点，問題点は以下のようなものが挙げられる．

まず，刻み幅が大きくなりすぎるとヤコビ行列の（擬似）逆行列を用いた線形近似が
成り立たなくなるという点である．

これに関しては，サンプリング周期が1msecであれば，1mオーダのアームで10m/secの速度でも刻み幅は1cmであり
線形計算による誤差はほとんど問題にならない．

これは[[articles:fk_jacob_ik|運動学に関するプログラム]]で示したように，
汎用のノートpcであればpythonプログラムで簡単に実現できるレベルである．
組込などの非力なcpuであってもC言語などに書き下せば十分対応可能であろう．

一方で，これはゲインを大きくしすぎて目標値周りでも大きな速度指令値を出せば
それにサンプリング周期を乗じた値で振動的に振る舞うことを示していて
十分な注意が必要となる．

もう一つの大きな問題点は物理的，機械的実現性の問題である．

たとえば目標値が急に変化した場合，この制御則の下ではいきなり
ある速度を指令することになる．
それを実現するためには大きな（原理的には無限大の）加速度を生じさせることになる．
これは物理的には不可能であるし，近似的に可能であったとしても衝撃を発生させるため
機械的にも望ましいものではない．

この問題を避けるには，座標系の誤差を修正する指令値を仮想的な力や加速度として，
それに基づいて速度を決定する仕組みを導入する必要がある．
これについては力覚フィードバックの節でもう少し議論する．

さらに，そしておそらく一番大きな問題点は特異姿勢である．
分解運動速度制御がうまく働くためにはヤコビ行列ができるだけ均質な方が良い．
均質というのは特異値分解をしたときの特異値の値に大きな開きがないということである．
極端に小さな特異値が存在すると，逆行列で解を求めたときに
そこに対応した関節軸（の線型結合）を大きく動かさなくてはならなくなる．
明らかにゼロとなっている場合は，擬似逆行列として解くと動かさないという解が得られるが
((得られた解は分解運動速度制御としては正しい解になっていない))
数値計算の誤差で値が残った場合は，異常で危険な動作が生成されることになる．

==== 力覚フィードバック ====
力覚センサを用い，その値をフィードバックすることで外力に対応したいろいろな動作応答を実現できる．

力覚フィードバックを用いた制御には，コンプライアンス制御，ダンピング制御，インピーダンス制御，力制御など種々の制御法がある．
手先の位置・姿勢に対して座標軸ごとに異なる制御を行うハイブリッド制御（通常，位置と力のハイブリッド）などもある．

=== ダンピング制御 ===

ダンピング制御は速度制御されたロボットアームにとっては比較的分かりやすい．
外力に押された形でそれに比例した速度（偏差）で動く．

手先での速度を$\boldsymbol{v}$（姿勢の回転速度も含めても良い），
中立（目標）速度を$\boldsymbol{v}_0$，
ダンピング係数を$D$とすると，外力を$\boldsymbol{f}$とすると，
$$
\boldsymbol{f}=D(\boldsymbol{v} - \boldsymbol{v}_0)
$$
すなわち，外力に対する手先の速度は，
$$
\boldsymbol{v} = \frac{1}{D} \boldsymbol{f} + \boldsymbol{v}_0
$$
これにヤコビ行列の逆行列をかけることで，これを実現する関節速度を得ることができる．

通常のダンピングは$\boldsymbol{v}_0=0$とすることが多い．

逆に，目標速度を，
$$
\boldsymbol{v}_0 = -\frac{1}{D} \boldsymbol{f}
$$
とすることで，接触力を制御した習い動作などが実現できる．

=== コンプライアンス制御 ===
コンプライアンス制御は，外力に比例して位置偏位を生じさせる．
$$
\boldsymbol{f} = K (\boldsymbol{x} - \boldsymbol{x}_0)
$$
すなわち，
$$
\boldsymbol{x}=\frac{1}{K} \boldsymbol{f} + \boldsymbol{x}_0
$$
しかし，外力が変化したときに瞬時にその位置に移動するというのは無理がある
((実は，ダンピング制御も見えにくいが同様な問題がある．))．

これを避ける一つの方法は上記の$\boldsymbol{x}$を目標値$\boldsymbol{x}_{ref}$として比例制御を行うことである．
すると，
$$
\boldsymbol{v}=k \boldsymbol{e} = k(\boldsymbol{x}_{ref} - \boldsymbol{x} ) 
= \frac{k}{K} \boldsymbol{f} + k (\boldsymbol{x}_0 - \boldsymbol{x})
$$
として，分解運動速度制御を行う．
この式は，中立速度が$k (\boldsymbol{x}_0 - \boldsymbol{x})$の比例制御であたえられた，
ダンピング係数が$\frac{K}{k}$のダンピング制御となっている．

=== 力制御, ハイブリッド制御 ===

=== インピーダンス制御 ===

インピーダンス制御はホーガンが提唱したもので，
力覚センサフィードバックにより，手先の見かけのインピーダンスが変化することに着目し，
逆に，手先でのインピーダンス特性を明示的に指示し，
それに従うようにフィードバック制御を行うことを提案した
((N. Hogan: Impedance Control; An Approach to Manipulation,
Part I, II, III. ASME J. of Dynamic Systems, Measurement
and Control, Vol.101, pp.1-24, 1985.))．
インピーダンス特性としては，バネ，ダンパ，マスの２次系を想定していて
コンプライアンス制御やダンピング制御を含んだものになっている．

ホーガンの論文は，トルク制御された関節を持つアームを対象として
手先の力に対する手先の加速度を逆運動学計算により
関節トルクを求める計算トルク方式により実現している．
これはいわゆる分解運動加速度制御になっている．

速度制御された関節の場合は，理想的な速度制御が実現されていれば
アームの動力学が隠蔽されるため逆運動学計算なしに
インピーダンス制御を実現することができる．

このような制御を実現する上で大事なことは内部にインピーダンスモデルを持つということである．

どういうことかと言うと，たとえばバネ特性の場合，外力がゼロであっても振動的な
振る舞いを行わなくてはいけない
((振動的な振る舞いは実際のロボットアームにとっては好ましくはないが))．