「詳説ロボット運動学」第６．２節ハイブリッド制御の補足説明　　　２００７．１．１０

１．　位置・力のハイブリッド制御の前提

２． 　変数の定義と力の釣り合い式

３．　ハイブリッド制御

３．１　ハイブリッド制御１　−輪郭既知で位置と力を独立にＦＢ制御する場合−

３．２　ハイブリッド制御２　−輪郭既知で位置ＦＢ制御と力ＦＢ＋ＦＦ制御する場合−

３．３ 　ハイブリッド制御３　−輪郭既知で作業座標系で位置と力を分離してＦＢ制御する場合−

３．４　ハイブリッド制御４　−輪郭既知で位置・力制御を各関節に対応させる場合−

３．５　ハイブリッド制御５　−輪郭既知で力制御のみ行う場合−

３．６　ハイブリッド制御６　−輪郭未知で力制御のみ行う場合−

３．７　ハイブリッド制御７　−速度制御・位置制御型ドライバー回路による位置・力制御−

［補１］　関節変位から輪郭接触点・撓み・接触力を決める計算

［補２］　指定の接触力で撓んで指定の輪郭上位置に接触する関節変位の計算

［補３］　関節速度からばねの撓み速度を決める計算

［補４］　力（撓み）制御系の安定性の検討

・ハイブリッド制御にもいろいろな手法がある．本書では簡単な説明しかしていないが，その具体的な手法を述べる．

１．位置・力のハイブリッド制御の前提

・制御対象：物体表面をなぞりながら，面に垂直に指定した力をロボットハンドから与える制御を考える．

・ハンドのばね性のよる力制御：ハンドと物体間にはばね性があり（手首の柔らかさ・ばね付き接触子など），力はそのばねの撓みによって与える．

ばね性はアーム長手方向・横方向にあるとし，回転の剛性は非常の大きい．

・力センサ：その撓みは手首の力センサによって計測できる．

・摩擦・外力の作用：接触子と物体輪郭の間には摩擦がない．重力や他の外力の作用はない．

・関節トルクによる力制御：関節トルクによって力を与える方式もあるが，ここでは取り扱わない．

　　理論的に関節トルクを換算して外部に力を与えることはできるが，実際にはＤＤロボットでない限り減速機の摩擦や他の機械部分の摩擦の影響で正しい力を与えることができない．

・２次元問題を例として説明する．

・ドライバー回路：３．１〜３．６ではトルク制御型ロボットアーム，３．７では速度制御型・位置制御型ロボットアームについて考える．

・力制御の特性：関節トルクによって力制御する場合，設定した力に対応する必要トルク（ＦＦトルク）と力の誤差に比例するトルク（ＦＢトルク）が必要である．

　　定常状態では

　　　　

　　であり，ＦＦトルクがあってとなる．

　　すなわち，ＦＦトルクがないと定常偏差が生じる点で位置制御と異なる．

ＦＦトルクなしでもこの定常偏差をなくす便法がある．１つは積分要素を使うこと，もう１つはＦＢゲインを大きくして実用上力誤差を小さくすることである．

・物体の輪郭：なぞる物体の輪郭は既知とする．未知の場合は３．６で説明する．

２． 変数の定義と力の釣り合い式

・接触子の位置を物体輪郭上に置き，法線方向にの力を，接線方向には０の力を手首のばね性によって与える方式を考える．

・,：物体輪郭上の接触子のあるべき位置

：面に垂直な接触力のΣB座標系表示

：面に垂直な接触力のΣH座標系表示

：手首の柔らかさによる撓み（接触子の変位）のΣH座標系表示

　　　　　　　縮む向き，下向きを正とする．

：指定の力に対応する撓みのΣH座標系表示．指令撓み．

：手首（または接触子）のばね定数のΣH座標系表示

　　　　（回転方向には硬く，力が掛かっても傾きは生じないとする）

θn：アーム長手方向から面法線へなす角（時計回りに正）

　　　　⇒　　　　 　　　（２−１）

φ：第２アームの傾きで，円筒座標ロボットでは回転関節角φそのものであり，SCARAロボットでは２関節角の和である．

・：指令撓み変位を関節変位に変換した値　　　　　　　　　　　（２−２）

：実際の撓みを関節変位に変換した値　　　　　　　　　　　　　　（２−３）

　　SCARAロボット：

　　円筒座標ロボット： → 以降と表現する．

：微小関節変位から（ΣH表示）に変換するヤコビアン．関節変位があって接触子先端が動かないとしたときばねが撓むその比率．

円筒座標ロボット：，　　　　　　　　　　　　（２−４）

SCARAロボット：，　　　　　　　（２−５）

・：ばねの撓み速度．　　　　　　　　　　　　　　　　　　　　　　　（２−６）

力制御（撓み制御）に利用する．は位置制御・力制御の関節速度の和である．

・法線方向に，接線方向には０の力を与えるばねの撓みは

　　　　⇒　，（２−７）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　 （２−８）

・力→撓み→関節微小変位と換算して，関節微小変位が上記の値となるように制御する．

・：輪郭上の指令位置

　：その位置における指令した力による撓み　　　　　　　　　　　　　　　　　（２−９）

　：接触子先端を指令位置とする関節変位

　：接触子先端の指令撓みを生じさせる関節変位．撓みが小さければ成り立つ．（２−１０）

　τ：モータトルク

　：接触力を換算したモータトルク　　　　　　　　　（２−１１）

・ロボットアームの動力学式

　　　　　　　　　　　　　　　　　　　　　　　　　（２−１２）

　重力は考えない．

・モータトルクとして位置フィードバックトルク・力フィードバックトルク・フィードフォワードトルクなどのいろいろな形で与えるいくつかの方式がある．

・フィードフォワードトルクは考えない．以下の項では適宜説明するにとどめる．

３．ハイブリッド制御

３．１　 ハイブリッド制御１

−輪郭既知で位置と力（撓み）を独立にフィードバック制御する場合−

・図２のように位置・力の各PID制御系は独立に構成する．

・制御対象は４つ（位置・力各２成分），制御変数は２つであるから正しい制御はできないが，近似的に制御できる条件を後で考える．

・それぞれのゲインはモータ・ドライバー・減速機の特性を含み，制御系の出力は関節トルクであるとする．

・：輪郭追従の指令値

：輪郭曲線を変換した関節変位（大変位）

：関節変位の実際値（大変位）

：各位置における指令値としての撓み．位置によって変わる．

：指令撓みを変換した指令関節変位（微小変位）

：測定された撓みを変換した関節変位（微小変位）　　　　　　　　　　　　　（３−１−１）

　，：位置誤差，撓み誤差の関節変位表現　　（３−１−２）

・モータ駆動トルクとロボットの動力学方程式は

　　　　　　（３―１−３）

　　　　　　　　　　　　　　　　　　　　　　　　　（３−１−４）

・定常状態（釣り合い状態）では

　　　　　　　　　　　　　　　　　　　　（３−１−５）

　　　　　　　　　　　　　　　　　　　　（３−１−６）

・でなくても釣り合い，位置・力が指令値通りにならない．

・撓みを生じさせる関節変位が十分な精度で指令関節変位に近い値となるためには

　　　　　　　　　　　　　　　　　　　　　　　　（３−１−７）

　すなわち撓み制御系のゲインと位置制御系のゲインによる先端位置換算のサーボ剛性がばね定数より十分大きい必要がある．

　例えば，接触力を１％以下の誤差とするにはをばね定数の100倍大きくしなければならない．

・減速比1：uの減速機を使う場合，制御系のサーボ剛性は関節変位に換算して倍になる（厳密に言えば関節１，２の先端換算サーボ剛性は，，これらが直列に連結しているので，合成の先端サーボ剛性はである）．

・センサや手首のばね性を含めてロボットメカニズムの剛性は通常サーボ剛性の換算値より小さい．

・すなわち上記の条件は実用上可能であり，多くの場合この方式で行っている．

３．２　ハイブリッド制御２

−輪郭既知で位置・力（撓み）フィードバック＋フィードフォワード制御する場合−

・力制御の偏差をなくすために必要な力に相当する関節トルクを加算する方式である．

・この場合も制御対象は４つ，制御変数は２つであるから，正しい制御はできない．

・モータ駆動トルクとロボットの動力学方程式は

　　（３−２−１）

　　　　，　　　　　　　　　　　　　　　　　　（３−２−２）

　　　　　　　　　　　　　　　　　　　　　　　（３−２−３）

・定常状態（釣り合い状態）では

　　　　　　　　　　　（３−２−４）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３−２−５）

・が十分な精度でに近い値となるためには

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（３−２−６）

　すなわち力（撓み）制御系のゲインが位置制御系のゲインより十分大きい必要がある．

・これは３．１より条件が緩く，実用上可能である．ただしＦＦトルクが必要な場合は少ない．

３．３　ハイブリッド制御３

−輪郭既知で作業座標系で位置と力を分離してフィ−ドバック制御する場合−

・作業座標系では輪郭に沿う方向に動いて力は０，法線方向には運動せずに指定の力（撓み）とする．

・作業座標系を(X,Y)空間でなく，ΣB座標系とする（図１参照）．

・輪郭上の移動量の指令値を，実際値をとする．この移動量から接触位置の指令値・実際値が計算できる（本ホームページ「詳説ロボットの運動学」第６．１節ロボットの線形制御への補足説明参照）．

・t方向に指令値となるようにフィードバック制御する．その誤差修正のための力はとして

　　　　　　　　　　　　　　　　　　　　（３−３−１）

・n方向の力はばね撓みから分かる．

　　　　　　　　　　　　　　　　　　　　　　　　　（３−３−２）

・力の誤差を法線方向の撓み誤差に直し，その誤差修正のためのn方向の力を求める．

・撓み誤差　　　　　　　　　　　　　　　　（３−３−３）

・誤差修正力は

　　　　　　　　　　　　　　　　　　（３−３−４）

・作業空間上の修正フィードバック力を関節トルクに変換してモータに与える．

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　(３−３−５)

・は関節速度からΣB座標系の速度への変換行列（ヤコビアン）で

　円筒座標ロボット：　　　　　　　　（３−３−６）

　SCARAロボット：　（３−３−７）

・モータ駆動トルクとロボットの動力学方程式は

　　　　

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　(３−３−８)

　　　　　　　　　　　　　　　　　　　　　　　（３−３−９）

・，はスカラー量である．

・FFトルクも与えたとして，定常状態では接触子は輪郭線上にあり，法線力も指令の力となっている．輪郭線上では撓みがないとすれば接線方向の位置も指令位置と同じあるが，撓みがあればその分だけずれる．位置制御は関節変位を観測して行うので撓みによるずれは修正されない．また撓みの制御は法線方向のｍであるから当然接線方向の撓みによるずれは，力制御（撓み制御）では修正されない．

・接線方向移動量に法線力による接線方向の撓みの理論値を加算して指令値とすれば誤差は小さくなる．

・このハイブリッド制御で，接線方向に力を与えると手首のばねが撓んでずれが生じ，その撓みに相当する反力がある．しかしロボットの姿勢は変わらない．

３．４　ハイブリッド制御４

−輪郭既知で位置・力（撓み）制御を各関節に対応させる場合−

・例えば第１関節変位を輪郭線上の位置sの制御に，第２関節を法線力制御に利用する方法である．

・フィードバックトルクにフィードフォワードトルクを加算して

　　　　　　　　　　　　（３−４−１）

　　　　　（３−４−２）

　とする．

・，はスカラー量である．

・の指令値は，位置に対応する関節変位であり，それとの差を誤差としてフィードバックする．．

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　(３−４−３)

・の指令値は，位置の対応する変位と力に対応する関節変位との和とし，実際の撓みに相当する関節変位との差を誤差としてフィードバックする（接線力は制御しない）．

指令値の撓み：　　　　　　　（３−４−４）

　　　　指令値の撓みに相当する関節変位：　　　　　　　　（３−４−５）

　　　　実際の撓み：（そのときの法線力：）（３−４−６）

　　　　撓みの差：　　　　　　　　　　　　　　　　　　（３−４−７）

撓み誤差に対応する関節変位：　　　　　　　（３−４−８）

・定常状態では

　　　　　　　　　　　　　　　　　　　（３−４−９）

　　　　　　　　　　　　　　　　　　　　　　　　（３−４−10）

・ならば近似的に指令の位置で指令の法線力となる．

・また関節力をＦＦする必要はない．

・このハイブリッド４制御法は，このままでは適用できない場合がある．

・それはだけでは軌道を進めることができない状態，またはだけでは法線力を制御できない状態にある場合である．

・例えば円筒座標ロボットのアームが円輪郭の接線上にあるときはアームを伸縮させても法線方向の力制御ができない．

・この特異状態を避けるために，「φ1を位置制御，φ2を力制御」と固定せず，特異状態に近くなったとき「φ1を力制御，φ2を位置制御」と入れ替えるとよい．

・もっと幅を広くして，例えば円筒座標ロボットでは，アームが円輪郭接線となす角abs(θn)＜45゜ならばφ2（アーム伸縮）を，abs(θn)＞45゜ならばφ1（アーム回転）を力制御に利用すればよい．

３．５　ハイブリッド制御５　−輪郭既知で力（撓み）制御のみ行う場合−

・法線方向に，接線方向に０の力を与える力制御をする．

・これはハイブリッド制御でなく力制御である．

・ただし，法線・接線方向を既知とする．すなわち輪郭軌道は既知とする．

・関節駆動トルクは

　　　　　　　　　　　　（３−５−１）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　 (３−５−２)

　　　　　　　　　　　　　　　　　　 （３−５−３）

・定常状態ではであり，正確に力制御されている．

・ならば，ＦＦトルクは必要ない．

・輪郭をなぞりながら力制御するには少し移動→力制御という操作が必要である．すなわち，初めにを輪郭軌道の始点付近に置き，その状態から力制御（撓み制御）する．その状態からを少し動かしてその位置で力制御する．この操作を次々と続ける．

３．６　ハイブリッド制御６　−輪郭未知で力制御のみ行う場合−

・フィードバック制御システムは３．５輪郭既知の場合と同じである．

・物体輪郭は未知なので，初めに物体の探索をしなければならない．すなわち手探りで物体に接触するまでアームを動かす．接触の有無は手首の力センサ（ばねの撓み）で分かる．

・接触後の操作は

（１）接触力の法線方向・接線方向をセンサによって計測・計算する．

（２）法線力を指令の力となるように制御する．

（３）接線方向にわずか動かして（１）に戻る．

３．７ ハイブリッド制御７　−速度制御型・位置制御型ドライバー回路による位置・力制御−

・速度制御型・位置制御型ドライバーを持つロボットに共通の特性は下記の通りである.

１．制御式は動力学式でなく，運動学式で表現される．

２．また高速運動で速度の急激な変化（過大な加速度）には追従できない．

３．力制御ができないので，ハンドまたはエンドエフェクタ（接触子）には柔らかさを持たせ，そのばねの撓みによって力を発生させ制御する方法が必要である．

４．基本的には位置と力に対応する撓み量だけの位置ずれの和を制御によって実現すればよい．ただし撓み量は，たとえ接触力が一定でも，接触角が変化し，また物体の設置誤差があることにより，状況に応じて変化する．

５．接触力の対応するＦＦトルクは必要がない．

６．３．１〜３．６のハイブリッド制御方式は（ＦＦトルクを除いて），全て速度制御型ドライバーを持つシステムに適用できる．

（１）速度制御型ドライバーを持つロボット

・速度制御型ドライバー回路によるモータ制御ではモータの負荷トルクに関係なく，入力信号に比例する回転速度を出せるようになっている．

・速度入力に対する応答速度の遅れは無視する．

・これらの特性を保証するために，ドライバーのカタログには負荷トルクや慣性モーメントなどの限界が記載されている．

・制御系の方程式は，各関節が独立なので

　　　　　（i=1,2）　　　　　 （３−７−１）

・定常状態では偏差のない位置制御ができている．

（２）位置制御型ドライバーを持つロボット

・位置制御型ドライバーは位置指令に対して応答は近似的に１次遅れで表される．

・位置に関する制御はドライバーでＦＢが構成されているので，ハイブリッド系ではＦＢブロックを構成する必要がない．

・力に関する制御は，力に対応する必要撓みを指令位置に加算し，ドライバーに任せればよい．

・制御系の方程式は，各関節が独立なので

　　　　　　　（i=1,2）　　　　　　　　　 （３−７−１）

・定常状態では偏差のない位置制御ができている．

［補１］関節変位から輪郭接触点と撓み・接触力を決める計算（関数caldeflect( )）

・物体がないとき，関節変位を与えると接触子先端位置が順運動学により決定できる．

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　（補１−１）

・が物体内部にあれば手首のばねが撓んで接触し，外部なら接触しない．

・が物体内部にある条件は，とすると

　　　　　　　　　　　　　　　　　　　　　（補１−２）

　ただし，とも輪郭曲線に近いとする．

・近似的に図補１のPが輪郭線上にあるとして，θnを求めると

　　　，　　　　　　　　　　　　　　（補１−３）

はＰ付近の輪郭法線の傾きである．

・接触すれば接触力は輪郭の法線方向に作用し，P点が輪郭に乗るまでだけ法線方向に縮まり，接線方向にはだけ移動するが，その量はばね定数に依存する．

・は図補２を参照して

　　　　，　または　

・法線方向撓みから接触力（法線力）が以下のように決まる．

・接線方向の力を０とすると各ばねの撓みは

　　　　，　　　　　　　　　　　　　　　　　（補１−９）

・法線方向の力が与えられれば各撓みは

　　　　　　　　　　　　　（補１−10）

　　　　　　　　　　 　（補１−11）

・が分かっているからは（補１−10）式より

　　∴　　　　　　　　　　　　　　　　　 　（補１−12）

・接線方向撓みは

　　

（補１−13）

・各ばねの撓みは

　　　　　　　　　　　　　　　　　　　　（補１−14）

　　　　　　　　　　　　　　　　　　　　（補１−15）

・すなわち関節変位が与えられれば，位置関係からが決まり，ばねの強さからが決まる．

［補２］指定の接触力で撓み指定の輪郭上位置に接触する関節変位の計算(関数calcpnt( ))

・接触子先端が輪郭上の指定位置Rにありかつ指定の接触力Fnとなるような関節変位を求める．

・図を参照して

　　　　　　　　　　　　　　　　　　　　　　　　　　（補２−１）

・ただし

　　　　，　　　　　　　　　　 （補２−２）

・撓みの成分は（補１−10），（補１−11）のように，Fnが与えられると

　　　　　　　　　　　　　　　　　　　（補２−３）

　　　　　　　　　　 　　　　　　　　（補２−４）

・となる関節変位を逆運動学式によって求めればよい．

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　（補２―５）

・別の関節変位の求め方として，指定位置を与える関節変位をに撓みに相当する関節変位を加算したものをと考える．

・縦横ばねの撓みは（補１−９）のように

　　　　，　，　　　　　　（補２−６）

・ばね撓みの関節変位換算値は

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　（補２−７）

・は微小関節変位とばね撓みの換算のヤコビアンで（２−４），（２−５）式より

円筒座標ロボット：，　　　　　　　　　　（補２−８）

SCARAロボット：，　　　　　（補２−９）

・指定位置で接触力Fnとする関節変位の近似値は

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（補２−10）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（補２―11）

［補３］関節速度からばねの撓み速度を決める計算

・関節速度を与えると輪郭に接触していないときの接触子先端の速度が決まる．

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　（補３−１）

・微小時間の間に動く先端の微小変位量を改めてとする．

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　（補３−２）

・図補３のように，接触子先端の動く前の位置をS，輪郭がないときの移動後の位置をP，輪郭上を動いて後の位置をRとする．PRが移動によるばねの新たな撓みである．

・図において

　　　　，　　　　　　　　　　　　　　　　　　　（補３−３）

，　　　　　　　　　　 　（補３−４）

　　　　，　　　　　　　　　　（補３−５）

　　　　　　　　　　　　　　　　　　　（補３−６）

　　　　　　　　　　　　　　　　　　　（補３−７）

・従って，関節速度と各ばねの撓み速度の関係は以下の順に求められる．

　　　　，　　　　　　　　　　　　（補３−８）

　　　　　　　　　　　　　　　　　　　（補３−９）

　　　　　　　　　　　　　　　　　　　（補３−10）

　は（２−４），（２−５）式にある．

・との関係を表すと

　　　　，　　　　　　　　　　　　　　 （補３−11）

　　　　　　　　　　　　　　　　　　　　（補３−12）

　　　　　　　　　　　　　　　　　　　　（補３−13）

・速度の関係は撓みの速度フィードバックに，または微小関節変位の撓み換算に利用する．

［補４］力（撓み）制御系の安定性の検討（トルク制御型ドライバーの場合）

・関節１，２は干渉するので単独の制御系だけでは安定かどうかは決められない．

・図補４のブロック線図において各ブロックの伝達関数（変換関数）は以下のようになる．

・関節変位は撓みδ＝０で接触子先端が輪郭に接触している状態を０とする．

・：ばね撓みδに相当する先端変位を与える微小関節変位の関係

・目標関節変位はである．

・PID制御：　　　　　　　　　　　　　　　　　　　　　（補４−１）

　　　　　　　　　　　　　（補４−２）

　　　　　　　　　　　　　（補４−３）

　は線形化したモータ＋アームの慣性モーメントで，２つの系の特性は同じものとする．

・ロボット（１）：，　　　　　　　（補４−４）

　ロボットの動特性を線形化して単に慣性項のみを考える．

・ロボット（２）：，　　　　　　（補４−５）

　　　　　　　　はδ＝０の接触点からのずれである．

・ばね：，　　　　　　　　　　　　（補４−６）

　　　　　　　　　　　　　　　　　　　　　　（補４−７）

　　　　　　　　　　　　　　　　　（補４−８）

　　　　　　　　　　　　　　 　　　（補４−９）

　∴　　，　　　　（補４−10）

・入出力関係は

　　　　，　　　　　　　（補４−11）

　　　　　　　　　　　　　　　　　　　　　　　（補４−12）

　　　　　（補４−13）

　　　　　　　　　　　　　　　　　　　　　（補４−14）

・安定条件は

　　　　　　　　　　　　　　　　　　　　　　　　　　（補４−15）

　の特性根が実部が負であることである．

　　　　

　　　　　（補４−16）

　　　　　　　（補４−17）

と置くと上式は

　　　　　　　　　　　　　　　　　　　　　　　（補４−18）

・Hurwitzの安定判別によれば

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（補４−19）

　　　　　　　　　　　　　　　　　　　　　　　　　　　　（補４−20）

　が安定条件である．

・第１式は常に満足するが，第２式ではAが小さいと不安定となる恐れがある．

　AはJ，θが含まれ，ロボットの形態によって変わる．すなわち運動中のサーボ剛性や減衰比が大きく変わる可能性がある．