特許 6032811 アドミッタンス制御を用いた力制御装置及び位置制御装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6032811

(24)【登録日】2016年11月4日

(45)【発行日】2016年11月30日

(54)【発明の名称】アドミッタンス制御を用いた力制御装置及び位置制御装置

(51)【国際特許分類】

   G05B 13/02 20060101AFI20161121BHJP

【ＦＩ】

   G05B13/02 A

【請求項の数】6

【全頁数】30

(21)【出願番号】特願2013-28989(P2013-28989)

(22)【出願日】2013年2月18日

(65)【公開番号】特開2014-157548(P2014-157548A)

(43)【公開日】2014年8月28日

【審査請求日】2016年2月3日

(73)【特許権者】

【識別番号】504145342

【氏名又は名称】国立大学法人九州大学

(74)【代理人】

【識別番号】100091443

【弁理士】

【氏名又は名称】西浦 ▲嗣▼晴

(72)【発明者】

【氏名】菊植 亮

【審査官】 牧 初

(56)【参考文献】

【文献】 特開平９−２０４２１７（ＪＰ，Ａ）

【文献】 特開平９−１０１８１５（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０５Ｂ １３／００−１３／０４

Ｇ０５Ｄ ３／００− ３／２０

Ｂ２５Ｊ ９／００− ９／２２

Ｂ２５Ｊ １３／００−１３／０８

(57)【特許請求の範囲】

【請求項1】

物体の位置を検出する位置検出器と、
前記物体が他の物体と接触する力を検出する力センサと、
前記物体を移動可能に保持する移動機構と、トルク指令値を入力として前記移動機構を動作させるモータを備えた駆動装置と、
単純な動特性を持つ仮想物体を演算装置内に想定し、前記物体に加える目標力と前記力センサが検出した接触力とを入力として前記仮想物体の運動をシミュレートするシミュレート部及び、前記物体の運動が前記仮想物体の運動に追従するように前記トルク指令値を出力する位置制御器を備えたアドミッタンス制御器とからなるアドミッタンス制御を用いた力制御装置であって、
前記位置制御器は、離散時間系における時刻をｋとしたときに、時刻ｋにおける前記トルク指令値τ（ｋ）を
τ（ｋ）＝Ｆsat［τ^＊（ｋ）／Ｆ］
の式を満たすように算出して出力し、但し、Ｆは前記モータが出すことを許容されている最大の力であり、τ^＊（ｋ）は

【数1】

を満たす値であり、τ_ｆ（ｋ）は予め同定された前記移動機構と前記駆動装置の動特性に基づいて算出される加速度 ▽² ｐ_ｄ（ｋ）／Ｔ²を実現するために必要であると推定されるトルク値であり、σ（ｋ）は

【数2】

を満たす値であり、Ｃ_ｓ、Ｃ_１、Ｃ_２、Ｃ_３は

【数3】

を満たす値であり，Ｔはサンプリング間隔であり、ｐ_ｄ（ｋ）は目標位置であり、ｐ（ｋ）は現在位置であり、定数Ｂは微分ゲインであり、定数Ｋは比例ゲインであり、定数Ｌは積分ゲインであり、Ｈは正の定数であり、ＪはＨ²／４以下の定数であり，状態変数ａ（ｋ）は

【数4】

を満たすように決定されることを特徴とする力制御装置。

【請求項2】

物体の位置を検出する位置検出器と、
前記物体が他の物体と接触する力を検出する力センサと、
前記物体を移動可能に保持する移動機構と、トルク指令値を入力として前記移動機構を動作させるモータを備えた駆動装置と、
単純な動特性を持つ仮想物体を演算装置内に想定し、前記物体に加える目標力と前記力センサが検出した接触力とを入力として前記仮想物体の運動をシミュレートするシミュレート部及び、前記物体の運動が前記仮想物体の運動に追従するように前記トルク指令値を出力する位置制御器を備えたアドミッタンス制御器とからなるアドミッタンス制御を用いた力制御装置であって、
前記位置制御器は、離散時間系における時刻をｋとしたときに、時刻ｋにおける前記トルク指令値τ（ｋ）を
τ（ｋ）＝Ｆsat［τ^＊（ｋ）／Ｆ］
の式を満たすように算出し、但し、Ｆは前記モータが出すことを許容されている最大の力であり、
上記式において、τ^＊（ｋ）は

【数5】

を満たす値であり、τ_ｆ（ｋ）及びσ（ｋ）は、

【数6】

を満たす値であり、Ｃ_ｓ、Ｃ_１、Ｃ_２、Ｃ_３は

【数7】

を満たす値であり、Ｍは定数であり、Ｔはサンプリング間隔であり、ｐ_ｄ（ｋ）は目標位置であり、ｐ（ｋ）は現在位置であり、定数Ｂは微分ゲインであり、定数Ｋは比例ゲインであり、定数Ｌは積分ゲインであり、Ｈは正の定数であり、ＪはＨ²／４以下の定数であり，状態変数ａ（ｋ）は

【数8】

を満たすように決定されることを特徴とする力制御装置。

【請求項3】

前記Ｈは０．０５〜２秒の定数である請求項１または２に記載の力制御装置。

【請求項4】

物体の位置を検出する位置検出器と、前記物体を移動可能に保持する移動機構と、トルク指令値を入力として前記移動機構を動作させるモータを備えた駆動装置と、前記物体の目標位置と前記位置検出器が検出した現在位置とに基づいて前記駆動装置に前記トルク指令値を出力する位置制御器とを備えてなる位置制御装置であって、
前記位置制御器は、離散時間系における時刻をｋとしたときに、時刻ｋにおける前記トルク指令値τ（ｋ）を
τ（ｋ）＝Ｆsat［τ^＊（ｋ）／Ｆ］
の式を満たすように算出し、但し、Ｆは前記モータが出すことを許容されている最大の力であり、
上記式において、τ^＊（ｋ）は、

【数9】

を満たす値であり、τ_ｆ（ｋ）は予め同定された前記移動機構と前記駆動装置の動特性に基づいて算出される加速度 ▽² ｐ_ｄ（ｋ）／Ｔ²を実現するために必要であると推定されるトルク値であり、σ（ｋ）は、

【数10】

を満たす値であり、Ｃ_ｓ、Ｃ_１、Ｃ_２、Ｃ_３は

【数11】

を満たす値であり、上記式において、Ｔはサンプリング間隔であり、ｐ_ｄ（ｋ）は目標位置であり、ｐ（ｋ）は現在位置であり、定数Ｂは微分ゲインであり、定数Ｋは比例ゲインであり、定数Ｌは積分ゲインであり、Ｈは正の定数であり、ＪはＨ²／４以下の定数であり、状態変数ａ（ｋ）は

【数12】

を満たすように決定されることを特徴とする位置制御装置。

【請求項5】

物体の位置を検出する位置検出器と、前記物体を移動可能に保持する移動機構と、トルク指令値を入力として前記移動機構を動作させるモータを備えた駆動装置と、前記物体の目標位置と前記位置検出器が検出した現在位置とに基づいて前記駆動装置に前記トルク指令値を出力する位置制御器とを備えてなる位置制御装置であって、
前記位置制御器は、離散時間系における時刻をｋとしたときに、時刻ｋにおける前記トルク指令値τ（ｋ）を
τ（ｋ）＝Ｆsat［τ^＊（ｋ）／Ｆ］
の式を満たすように算出し、但し、Ｆは前記モータが出すことを許容されている最大の力であり、
上記式において、τ^＊（ｋ）は

【数13】

を満たす値であり、τ_ｆ（ｋ）及びσ（ｋ）は、

【数14】

を満たす値であり、Ｃ_ｓ、Ｃ_１、Ｃ_２、Ｃ_３は

【数15】

を満たす値であり、Ｍは定数であり、Ｔはサンプリング間隔であり、ｐ_ｄ（ｋ）は目標位置であり、ｐ（ｋ）は現在位置であり、定数Ｂは微分ゲインであり、定数Ｋは比例ゲインであり、定数Ｌは積分ゲインであり、Ｈは正の定数であり、ＪはＨ²／４以下の定数であり，状態変数ａ（ｋ）は

【数16】

を満たすように決定されることを特徴とする位置制御装置。

【請求項6】

前記Ｈは０．０５〜２秒の定数である請求項４または５に記載の位置制御装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、アドミッタンス制御を用いた力制御装置及び該力制御装置にも適用できる位置制御装置に関するものである。

【背景技術】

【0002】

ロボット・マニピュレーターが外部的環境に接して動作している場合、ロボットのエンドエフェクタと環境の間の接触力を制御するための適切な制御器が必要である。この種の制御器は、非特許文献１及び２の中で提案されている。力センサが利用可能であり、ロボットのジョイントに高い摩擦がある状況では、アドミッタンス制御（非特許文献３）と呼ばれる制御手法がよく用いられている（非特許文献３〜５）。アドミッタンス制御の典型的な実装例を図１に示す。この種の制御器では、単純な動特性をもつ仮想物体を制御器の中で想定し、力センサで測定された力の情報によって仮想物体の運動をシミュレートする。また、ロボットは仮想物体の運動を追跡するように位置制御される。位置制御が十分に正確である限り、ロボットの外力に対する反応は仮想物体のそれに近い。この制御法は、力覚提示（非特許文献４，５）、遠隔操作（非特許文献６）、リハビリテーション（非特許文献７）、人間機械共同作業（非特許文献８）及びある種の製造工程（非特許文献９）に適用されている。ほとんどの応用例においては、ロボットのアクチュエータ力の大きさはハードウェア制限か安全上の理由により制限されている。

【0003】

アドミッタンス制御でのアクチュエータ力飽和の１つの問題は、アクチュエータ力が飽和する場合に、ロボットの位置が仮想物体の位置から遠ざかってしまうということである。アクチュエータ力は飽和する可能性があり、仮想物体の速度に急激な変化が発生する可能性がある。大きな外力がロボットに働く可能性がある場合に、仮想物体の慣性と粘性を大きく設定することは、飽和を防ぐことに寄与する。しかし、それは接触力に対する鈍重な反応性に帰着し、多くの場合、不適切である。

【0004】

このような問題を解決するために用いることができる制御技術が、非特許文献１０及び特許文献１によって提案された、Proxy-based Sliding Mode制御（ＰＳＭＣ）である。この技術は、単純なスライディングモード制御器のある種の離散時間近似でもあり、飽和のあるＰＩＤ制御器の拡張でもある。

【0005】

なお外力のフィードバックループ及び内部位置ループを使用する制御方法は、１９７０年代以来文献で見つけることができる。それらの制御器は明示的力制御器（非特許文献１１〜１３）及びインピーダンス制御器（非特許文献１４〜１７）に分類することができる。前者はエンドエフェクタ上の力を制御し、また、後者はエンドエフェクタの見かけの機械インピーダンスを制御する。この２つのタイプは、同様のフレームワーク（ただ一つの違いはその間の、目標力コマンドが提供されるかどうかである）の中で実現することができる。それらの制御器は、「仮想内部モデル追従制御」（非特許文献９，１８）、「位置制御ベース・インピーダンス制御」（非特許文献１９，２０）及び「アドミッタンス制御」（非特許文献４，５，７，８）のようなさまざまな名前で呼ばれている。

【0006】

安定性はアドミッタンス制御器の主要な課題である。不安定性の主な原因は、制御器の中の時間遅れ（非特許文献１２，２３）、内部位置制御システムの帯域制限（非特許文献２４〜２６）である。仮想質量と仮想粘性は、不安定性を抑えるためには、高く設定すべきであるということが知られている。しかしながら、それが制御器の反応性の質を下げる場合があるので、大きな仮想質量か仮想粘性はいくつかの適用には望ましくない。実現可能な最小の仮想質量はアドミッタンス制御される装置のひとつの評価指標でもある（非特許文献５）。仮想質量及び仮想粘性を増加させずに安定性を向上するために、仮想物体位置から実際の位置への伝達関数は、１に近い必要がある。これを実現するために、従来の技術のうちのいくつかは、ロボットの動特性（非特許文献２２，２７，２８の影響を償うためにフィードフォワード項（これは位相進み補償器とみなすことができる）を含む位置制御器を使用している。

【0007】

アクチュエータ飽和は、どのようにそれを回避しなければならないかという問題についての唯一の解析的な研究はGonzalez及びWidmannの研究（非特許文献２９）である。この研究では、飽和がシステムを開ループ系と一時的に等価にするので、飽和が制御対象の安定性を改善することが指摘されている。しかしながら、この技術では、正の飽和と負の飽和が交互に発生する状況（これは通常望ましくない）を考慮していない。

【先行技術文献】

【非特許文献】

【0008】

【非特許文献1】D. E. Whitney, “Historical perspective and state of the art in robot force control,” Int. J. Robotics Research, vol. 6, no. 1, pp. 3-14, 1987.

【非特許文献2】G. Zeng and A. Hemami, “An overview of robot force control,” Robotica, vol. 15, pp. 473-482, 1997.

【非特許文献3】C. Ott, R. Mukherjee, and Y. Nakamura, “Unified impedance and admittance control,” in Proc. IEEE Int. Conf. Robotics and Automation, 2010, pp. 554-561.

【非特許文献4】M. Ueberle and M. Buss, “Control of kinesthetic haptic interfaces,” in Proc. Workshop on Touch and Haptics, IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 2004, pp. 1-14.

【非特許文献5】R. Q. Van Der Linde and P. Lammertse, “HapticMaster - a generic force controlled robot for human interaction,” Industrial Robot, vol. 30, no. 6, pp. 515-524, 2003.

【非特許文献6】A. Peer and M. Buss, “A new admittance-type haptic interface for bimanual manipulations,” IEEE/ASME Trans. Mechatronics, vol. 13, no. 4, pp. 416-428, 2008.

【非特許文献7】P. R. Culmer, A. E. Jackson, S. Makower, R. Richardson, J. A. Cozens, M. C. Levesley, and B. B. Bhakta, “A control strategy for upper limb robotic rehabilitation with a dual robot system,” IEEE/ASME Trans. Mechatronics, vol. 15, no. 4, pp. 575-585, 2010.

【非特許文献8】N. Takesue, H. Murayama, K. Fujiwara, K. Matsumoto, H. Konosu, and H. Fujimoto, “Kinesthetic assistance for improving task performance − the case of window installation assist −,” Int. J. Automation Technology, vol. 3, no. 6, pp. 663-670, 2009.

【非特許文献9】H. Arai, “Force-controlled metal spinning machine using linear motors,” in Proc. IEEE Int. Conf. Robotics and Automation, 2006, pp. 4031-4036.

【非特許文献10】R. Kikuuwe, S. Yasukouchi, H. Fujimoto, and M. Yamamoto, “Proxybased sliding mode control: A safer extension of PID position control,” IEEE Trans. Robotics, vol. 26, no. 4, pp. 860-873, 2010.

【非特許文献11】J. De Schutter, “A study of active compliant motion control methods for rigid manipulators based on a generic scheme,” in Proc. IEEE Int. Conf. Robotics and Automation, vol. 4, 1987, pp. 1060-1065.

【非特許文献12】H. Ishikawa, C. Sawada, K. Kawase, and M. Takata, “Stable compliance control and its implementation for a 6 DOF manipulator,” in Proc. IEEE Int. Conf. Robotics and Automation, vol. 1, 1989, pp. 98-103.

【非特許文献13】H. Seraji, “Adaptive admittance control: an approach to explicit force control in compliant motion,” in Proc. IEEE Int. Conf. Robotics and Automation, 1994, pp. 2705-2712.

【非特許文献14】D. E. Whitney, “Force feedback control of manipulator fine motions,” Trans. ASME: J. Dynamic Systems, Measurement, and Control, vol. 99, no. 2, pp. 91-97, 1977.

【非特許文献15】G. Hirzinger, “Direct digital robot control using a force-torque sensor,” in Proc. IFAC Symp. Real Time Digital Control Applications, 1983, pp. 243-255.

【非特許文献16】J. De Schutter and H. Van Brussel, “A methodology for specifying and controlling compliant robot motion,” in Proc. 25th Conf. Decision and Control, 1986, pp. 1871-1876.

【非特許文献17】J. A. Maples and J. J. Becker, “Experiments in force control of robotic manipulators,” in Proc. IEEE Int. Conf. Robotics and Automation, 1986, pp. 695-702.

【非特許文献18】K. Kosuge, K. Furuta, and T. Yokoyama, “Virtual internal model following control of robot arms,” in Proc. 1987 IEEE Int. Conf. Robotics and Automation, 1987, pp. 1549-1554.

【非特許文献19】D. A. Lawrence and R. M. Stoughton, “Position-based impedance control - achieving stability in practice,” in Proc. AIAA Guidance, Navigation and Control Conference, 1987, pp. 221-226.

【非特許文献20】D. Surdilovic and J. Kirchhof, “A new position based force/impedance control for industrial robots,” in Proc. IEEE Int. Conf. Robotics and Automation, 1996, pp. 629-634.

【非特許文献21】R. Volpe and P. K. Khosla, “A theoretical and experimental investigation of explicit force control strategies for manipulators,” IEEE Trans. Automatic Control, vol. 38, no. 11, pp. 1634-1650, 1993.

【非特許文献22】J. J. Gonzalez and G. R. Widmann, “A force commanded impedance control scheme for robots with hard nonlinearities,” IEEE Trans. Control Systems Technology, vol. 3, no. 4, pp. 398-408, 1995.

【非特許文献23】D. A. Lawrence, “Impedance control stability properties in common implementation,” in Proc. IEEE Int. Conf. Robotics and Automation, 1988, pp. 1185-1190.

【非特許文献24】S. D. Eppinger and W. P. Seering, “Understanding bandwidth limitations in robot force control,” Proc. IEEE Int. Conf. Robotics and Automation, vol. 4, pp. 904-909, 1987.

【非特許文献25】T. Valency and M. Zacksenhouse, “Accuracy/robustness dilemma in impedance control,” Trans. ASME: J. Dynamic Systems, Measurement, and Control, vol. 125, no. 3, pp. 310-319, 2003.

【非特許文献26】S. H. Kang, M. Jin, and P. H. Chang, “A solution to the accuracy/ robustness dilemma in impedance control,” IEEE/ASME Trans. Mechatronics, vol. 14, no. 3, pp. 282-294, 2009.

【非特許文献27】S. Tafazoli, S. E. Salcudean, K. Hashtrudi-Zaad, and P. D. Lawrence, “Impedance control of a teleoperated excavator,” IEEE Trans. Control Systems Technology, vol. 10, no. 3, pp. 355-367, 2002.

【非特許文献28】G. Ferretti, G. Magnani, and P. Rocco, “Impedance control for elastic joints industrial manipulators,” IEEE Trans. Robotics and Automation, vol. 20, no. 3, pp. 488-498, 2004.

【非特許文献29】J. J. Gonzalez and G. R. Widmann, “Investigation of nonlinearities in the force control of real robots,” IEEE Trans. Systems, Man, and Cybernetics, vol. 22, no. 5, pp. 1183-1193, 1992.

【特許文献】

【0009】

【特許文献1】特開２００７−１０２７４８号公報

【発明の概要】

【発明が解決しようとする課題】

【0010】

ＰＳＭＣは、トルク不飽和時にはＰＩＤと等価であるため目標位置に対する現在位置追従特性が不十分であることもある。またＰＳＭＣの場合、トルク飽和に関しては、飽和から不飽和への遷移時にτが不連続になって、ロボットを触る作業者に、違和感を感じさせる原因となっている。

【0011】

本発明の目的は、位置指令が頻繁及び急激に変わるときでも位置の追従性を向上させることができ、しかもトルク飽和時でも不連続な動きを押さえることができる位置制御器を備えたアドミッタンス制御を用いた力制御装置を提供することにある。

【0012】

本発明の他の目的は、トルク飽和時でも不連続な動きを押さえることができる位置制御装置を提供することにある。

【課題を解決するための手段】

【0013】

本発明のアドミッタンス制御を用いた力制御装置は、物体の位置を検出する位置検出器と、物体が他の物体と接触する力を検出する力センサと、物体を移動可能に保持する移動機構と、トルク指令値を入力として移動機構を動作させるモータを備えた駆動装置と、アドミッタンス制御器とを備えている。そしてアドミッタンス制御器が、単純な動特性を持つ仮想物体を演算装置内に想定し、前記物体に加える目標力と前記力センサが検出した接触力とを入力として前記仮想物体の運動をシミュレートするシミュレート部及び、物体の運動が仮想物体の運動に追従するようにトルク指令値を出力する位置制御器を備えて構成されている。

【0014】

力制御装置で用いる位置制御器は、離散時間系における時刻をｋとしたときに、時刻ｋにおける前記トルク指令値τ（ｋ）をτ（ｋ）＝Ｆsat［τ^＊（ｋ）／Ｆ］の式を満たすように算出する。但し、Ｆはモータが出すことを許容されている最大の力であり、上記式において、τ^＊（ｋ）は、

【数1】

【0015】

を満たす値であり、上記式において、τ_ｆ（ｋ）は予め同定された前記移動機構と前記駆動装置の動特性に基づいて算出される加速度 ▽² ｐ_ｄ（ｋ）／Ｔ²を実現するために必要であると推定されるトルク値である。

【0016】

またσ（ｋ）は

【数2】

【0017】

を満たす値であり、Ｃ_ｓ、Ｃ_１、Ｃ_２、Ｃ_３は

【数3】

【0018】

を満たす値であり，Ｔはサンプリング間隔であり、ｐ_ｄ（ｋ）は目標位置であり、ｐ（ｋ）は現在位置であり、定数Ｂは微分ゲインであり、定数Ｋは比例ゲインであり、定数Ｌは積分ゲインであり、Ｈは正の定数であり、ＪはＨ²／４以下の定数である。そして状態変数ａ（ｋ）は

【数4】

【0019】

を満たすように決定される。なお実用上、Ｈは０．０５〜２秒の定数であるのが好ましい。またτ_ｆ（ｋ）は下記のように表すこともできる。

【数5】

【0020】

上記の構成を有する位置制御器を用いると、トルク制御器のトルクが飽和している期間と不飽和の期間の間の滑らかな遷移と測定した力に対する即時的な応答が可能になる。すなわちトルクが不飽和の場合には、位置指令が頻繁及び急激に変わるときでも位置の追従性を向上させることができ、またトルクが飽和している場合には、負荷に加えられた力の変化に速く応答し、より滑らかでより安定なシステムの実現に寄与する。

【0021】

本発明の位置制御装置を備えたアドミッタンス制御器に用いた力制御装置は、通常状態においては、加速度フィードフォワードを持った比例積分微分（ＰＩＤ）制御器として作用し、アクチュエータ力が飽和する場合はある種のスライディングモード制御器として作用する。本発明の位置制御制御装置を使用するアドミッタンス制御器は、アクチュエータ力の飽和期間及び不飽和期間の間の滑らかな遷移を実現する。さらにアクチュエータ力が飽和する場合でさえ、加えられた力の変化に速く応答する。これにより、より滑らかでより安定な性質の実現に寄与している。

【図面の簡単な説明】

【0022】

【図1】本発明の位置制御装置を含むアドミッタンス制御器を備えた力制御装置に適用した実施の形態の概略を示す図である。

【図2】力制御装置の具体的な構成を示すブロック図である。

【図3】位置制御器で使用する式を導出するために用いるブロック図である。

【図4】式（２８ａ）及び式（２８ｂ）の物理的な解釈を説明するために用いる図である。

【図5】（Ａ）及び（Ｂ）は、実験状況を示す図である。

【図6】（Ａ）は実験装置の根本関節まわりの摩擦力の大きさを示す図であり、（Ｂ）はエンドエフェクタの位置とゴムシートからの接触力の関係を示す図である。

【図7】６台の位置制御器で得られた結果を示す図である。

【図8】図７中のPSMC2＋AFF及びPSMC＋AFFの結果を別の色と時間スケールで表示したものである。

【図9】衝突の後の押し付け動作の実験結果を示す図である。

【図10】（Ａ）乃至（Ｃ）は、３つの内部位置制御器で得られた位置、測定された力及びアクチュエータ力のデータを示す図である。

【発明を実施するための形態】

【0023】

以下図面を参照して本発明の実施の形態の一例を詳細に説明する。図１は、本発明の位置制御装置を含むアドミッタンス制御器を備えた力制御装置に適用した実施の形態の概略を示す図であり、図２は具体的な構成を示すブロックである。図１に示す力制御装置は、自動車工場でウインドウを搭載する際に使用されるウインドウ搭載支援ロボットや、リハビリテーション施設で患者の移動及び搬送に使用されるリハビリテーション支援ロボット等に使用されるロボットが移動機構１として用いられている。ウインドウ搭載支援ロボットは、重いウインドウを把持したロボットの腕を作業者が引っ張ってウインドウを自動車の取り付け位置まで運ぶ目的で使用される。この例のロボットでは、ウインドウをスムーズに運べるようにすることは勿論のこと、作業者がウインドウを必要以上に速く移動させることがないように、またモータの位置制御においてトルクがある値を上下限として飽和状態になるようになっている。そのためトルクがある値を不飽和状態と飽和状態との間をスムーズに遷移することと、飽和状態において作業者に操作の違和感を感じさせないようにする必要がある。

【0024】

＜実施の形態の構成＞
図２の力制御装置は、ロボットの移動機構１を、駆動装置３に位置制御器５からトルク指令を与えて駆動する。位置制御装置５は、力センサ７と、位置検出器９とシミュレート部１１を備えたアドミッタンス制御器４内に配置されている。位置検出器９は、移動機構１のアームに把持された図示しない物体の位置を検出する。力センサ７は、物体が他の物体と接触する力を検出する。他の物体とは、例えば作業者の手であったり、ロボットの周囲環境にある部材である。位置制御器５は、物体の目標位置と位置検出器９が検出した現在位置とに基づいて駆動装置３にトルク指令値を出力する。シミュレート部１１は、単純な動特性を持つ仮想物体を演算装置内に想定し、物体に加える図示しない上位コントローラから与えられる接触力の目標力と力センサ７が検出した接触力とを入力として仮想物体の運動をシミュレートする。そして位置制御器５が、シミュレート部がシミュレートした運動（目標位置）に追従するように、トルク指令を出力する。

【0025】

力制御装置で用いる位置制御器１１は、離散時間系における時刻をｋとしたときに、時刻ｋにおけるトルク指令値τ（ｋ）をτ（ｋ）＝Ｆsat［τ^＊（ｋ）／Ｆ］の式を満たすように算出する。但し、Ｆはモータが出すことを許容されている最大の力であり、上記式において、τ^＊（ｋ）は、

【数6】

【0026】

を満たす値である。

【0027】

上記式において、τ_ｆ（ｋ）は予め同定された前記移動機構と前記駆動装置の動特性に基づいて算出される加速度 ▽² ｐ_ｄ（ｋ）／Ｔ²を実現するために必要であると推定されるトルク値である。またσ（ｋ）は、

【数7】

【0028】

を満たす値である。そしてＣ_ｓ、Ｃ_１、Ｃ_２、Ｃ_３は

【数8】

【0029】

を満たす値である。また上記式において、Ｔはサンプリング間隔であり、ｐ_ｄ（ｋ）は仮想物体の位置即ち目標位置であり、ｐ（ｋ）は現在位置であり、定数Ｂは微分ゲインであり、定数Ｋは比例ゲインであり、定数Ｌは積分ゲインであり、Ｈは正の定数であり、ＪはＨ²／４以下の定数である。制御対象によって異なるが、Ｈは０．０５〜２秒の定数であることが好ましく、特にＨが０．１〜０．２秒の値であれば、多くの制御対象に実用上適用可能であることを実験により確認した。

【0030】

またτ_ｆ（ｋ）は、

【数9】

【0031】

を満たす値としても表現できる。なおＭは定数である。

【0032】

そしてＨ及びＪは、トルク不飽和時に満たされる下記の式と、

【数10】

【0033】

トルク飽和時に満たされる下記の式

【数11】

【0034】

を満たす値である。

【0035】

上記の構成を有する位置制御器５を用いると、トルク制御器のトルクが飽和している期間と不飽和の期間の間の滑らかな遷移と測定した力に対する即時的な応答が可能になる。すなわちトルクが不飽和の場合には、位置指令が頻繁及び急激に変わるときでも位置の追従性を向上させることができ、またトルクが飽和している場合には、負荷に加えられた力の変化に速く応答し、より滑らかでより安定なシステムの実現に寄与する。 上記構成を備えた位置制御装置５を備えたアドミッタンス制御器４に用いた力制御装置は、通常状態においては、加速度フィードフォワードを持った比例積分微分（ＰＩＤ）制御器として作用し、アクチュエータ力が飽和する場合は、ある種のスライディングモード制御器として作用する。そしてこのアドミッタンス制御器４は、アクチュエータ力の飽和期間及び不飽和期間の間の滑らかな遷移を実現する。さらにアクチュエータ力が飽和する場合でさえ、加えられた力の変化に速く応答する。これにより、より滑らかでより安定な性質の実現に寄与する。

【0036】

＜理論的な説明＞
以下に位置制御器５が上記式を満たすことにより、本発明の効果を得られることを理論的に説明する。

【0037】

［予備的な解析］
以下の説明に用いるために、次の２つの関数を定義する。

【数12】

【0038】

これらは次の定理によって関連づけられる。

【0039】

定理１：２つの実数u及びyについて、下式が満たされる。

【数13】

【0040】

上記定理は、以下のように証明することができる。

【数14】

【0041】

定理１の直接の結果として、任意の正のスカラーＸ，Ｙ，Ｚ及びスカラーx，y，zについて、下記の関係が満たされることが判る。

【数15】

【0042】

本発明の位置制御装置は、不連続なｓｇｎ関数を用いた連続時間表現で示される。しかし、定理１あるいは上記の関係（４）の使用によって、それは連続なｓａｔ関数を用いた離散時間型アルゴリズムに変換される。

【0043】

位置制御器５で使用する式を導出するために、図３に示すブロック図に示すように、質点からなり、アドミッタンス制御され、環境に接している一次元のロボットを考える。以下の説明では、位置検出器により検出された現在位置をｐとし、ロボットの質量をＭｒとし、ｐ_ｄを目標位置とする。さらに、ｆはロボットから環境に加えられる力、τはロボットに作用するアクチュエータ力（トルク指令）及びｈは他のすべての出所からのロボットに作用する力である。その後、ロボットの運動方程式は以下のように書くことができる。なお以下の式において、文字（例えばｐ）の上の一つのドットマークは１回微分していることを意味し、二つのドッドマーク「・・」は二回微分していることを意味する。

【数16】

【0044】

目標力ｆ_ｄを持ったアドミッタンス制御器は、仮想物体の運動のシミュレータ及び仮想物体の運動に追従することをロボットに強いるための位置制御器のコンビネーションとして構築することができる。この位置制御器の１つの例は以下のように記述することができる。

【数17】

【0045】

ここで、ｍ，ｂ，Ｋ，Ｂ，Ｌ及びＭは、正定数である。その入力は、目標力ｆ_ｄ∈Ｒ、力センサ７によって測定された力ｆ∈Ｒ、及び位置センサによって測定された位置ｐ∈Ｒである。その出力はアクチュエータ力（トルク）τである。式（６）は、外力ｆ及び入力である目標力ｆ_ｄを受ける仮想物体（その質量はｍ＞0、粘性はｂ＞0、また、位置はｐ_ｄ∈Ｒである）の動特性を記述する。方程式（６）は、測定された力ｆに目標値（目標力）ｆ_ｄを追跡させるための位置制御器の式と解釈することもできる。

【0046】

式（７）はアドミッタンス制御器中の位置制御器が、不飽和状態で動作するときのアクチュエータ力（トルク）τである。それは目標位置ｐ_ｄを入力とし、Ｋ，Ｂ及びＬはそれぞれ、Ｐ（比例）ゲイン、Ｄ（微分）ゲイン、Ｉ（積分）ゲインである。定数Ｍは装置の慣性Ｍｒを補償するためのものであり、

【数18】

【0047】

は位相進み補償器と見なすことができる。それは目標位置ｐ_ｄから現在位置ｐへの位相遅れを少なくする。この項は、ｐからｆへのフィードバックループが存在する状況（たとえばロボットが環境に接している場合）においてシステムの安定性を改善する。

【0048】

環境に接するアドミッタンス制御対象の安定性は、環境の応答特性が線形の場合、伝達関数に基づいて解析することができる。しかしながら、アクチュエータ力τの大きさには常に上限が存在するため、特にＫが大きい場合、τが容易に飽和することになる。このような場合、制御器は非線形になり、線形制御理論によって解析するのが難しくなる。

【0049】

［ＰＤ（比例微分）型スライディングモード制御器］
アクチュエータ飽和（位置制御器の飽和）に関する主な問題の１つは、負の飽和と正の飽和の間の反復的な遷移である。このような状況が発生する可能性を考察するために、飽和時においては、式（７）の代わりに、アクチュエータ飽和を持った非常に高いゲインのＰＤ（比例微分）制御器は下記の式を満たすものと近似的に表現できる。

【数19】

【0050】

ただし、

【数20】

【0051】

この制御器が満たす条件式（８）は下記と等価である。

【数21】

【0052】

これは、式（８）が非常に高いゲインを持った飽和つきＰＤ制御器と見なすことができることを意味する。式（８）はＰＤ型スライディングモード制御器が満たす条件式と呼ぶことができる。

【0053】

（８）式の条件を満たす位置制御器の動きを分析するために、式（８）を式（５）に代入してみる。その後、sの１回微分を考えれば、次の運動方程式を得ることができる。

【数22】

【0054】

ここで留意すべきは、

【数23】

【0055】

であり、また、

【数24】

【0056】

が式（１１）の右辺及び左辺の両方に含まれているということである。式（４）の使用によって、次のように、右辺中の

【数25】

【0057】

を消去することができ、下記式（１２）を得ることができる。

【数26】

【0058】

更に、式（５）に式（１２）を代入することによって、τは下記（１６）式を満たすことが判る。

【数27】

【0059】

これは、下記の条件が満たされないときに、スライディングモード（s＝０）からリーチングモード（到達モード）（s≠０）への遷移が生じることを意味する。

【数28】

【0060】

これは下式と等価である。

【数29】

【0061】

逆に、状態空間内の直線s＝０に状態

【数30】

【0062】

が到達すると、リーチングモード（到達モード）からスライディングモードへの遷移が起こる。状態がスライディングモードに残るために、条件（１５）は満たされている必要がある。また、そうでなければ反復的な遷移が、負の飽和及び正の飽和の間に生じる。方程式（１５）は、

【数31】

【0063】

が大きいときに満たされないことが判る。但し、Ｈを大きくすると、スライディングモードに居続けられる

【数32】

【0064】

の範囲は拡大する。

【0065】

リーチングモード（到達モード）からスライディングモードまで遷移する際に、アクチュエータ力τが不連続に変化することが式（１３）から判るもう一つのことである。不連続に変化するのは、式（１０）の中のκが無限の場合に限られるが、κが大きい時は、τが急激に変化する。上記不連続の変化が生じる場合、あたかもｆまたはｆ_ｄが不連続的に変わるかのように、アドミッタンスにコントロールされるロボットは振る舞う。特にロボットが物理的に人間のユーザと相互作用することになっている場合、これは明らかに望ましくない。

【0066】

［ＰＤＤ²型スライディングモード制御器］
反復する飽和及びアクチュエータ力の不連続性の問題を克服するために、次の条件を満たす位置制御器を考察する。

【数33】

【0067】

そこで

【数34】

【0068】

式（１６）が

【数35】

【0069】

と等価であるので、この制御器は、非常に高いゲインを持ったＰＤＤ²（比例，微分，２回微分）制御器の極端な場合として見ることができる。式（１６）を満たたす位置制御器はＰＤＤ²型スライディングモード制御器と呼ぶことができよう。

【0070】

この制御則の下では、位置制御部分システムの運動方程式は以下のように記述することができる。

【数36】

【0071】

式（４）の使用によって、式（１９）は以下のように書き直すことができる。

【数37】

【0072】

さらに、式（２０）を式（５）に代入することによって、下記が満たされることが分かる。

【数38】

【0073】

式（２１）は、

【数39】

【0074】

またはそれと等価な

【数40】

【0075】

が満たされている間において、状態がスライディングモード（σ＝０）であることを意味する。これは、速度

【数41】

【0076】

が大きい場合においてもスライディングモードが達成可能であることを意味する（前述のＰＤ型スライディングモード制御（８）の場合はそうではなかった）。方程式（２１）は、さらに

【数42】

【0077】

ｆあるいはｈに不連続性がなければ、アクチュエータ力τが常に連続的なことを示す。アドミッタンス制御の下で、

【数43】

【0078】

の不連続性は外力ｆあるいは目標力ｆ_ｄの不連続性からのみから発生する。逆の視点から見ると、

【数44】

【0079】

ｆあるいはｈの中の不連続性が力τに適切に反映されることが分かる。

【0080】

式（２０）は、スライディングモードにおいて、ｆとｈの影響を受けずに、ＨとＪによって特徴付けられた滑らかな軌道に沿って目標位置ｐ_ｄに現在位置ｐが収束することを示している。Ｊ≦＜H²／４のとき、収束軌道は過減衰になり、オーバーシュートは発生しない。

【0081】

［微分代数型の実装］
前述の２つの不連続な位置制御器すなわち式（８）及び式（１６）を満たす位置制御器は、制御器の中の不可避な無駄時間のために実現できない。考えられる解決方法としては、式（８）及び式（１６）の不連続性を以下のようにそれぞれ滑らかにすることである。

【数45】

【0082】

ここでＫは有限値の正定数である。このアプローチは「境界層」（J.-J. E. Slotine, “The robust control of robot manipulators,” Int. J. Robotics Research, vol. 4, no. 2, pp. 49-63, 1985.及びV. I. Utkin, J. Guldner, and J. Shi, Sliding Mode Control in Electro-Mechanical Systems, 2nd ed. CRC Press, 2009.）アプローチとして知られている。ここで、ｓｇｎの不連続性が境界層（｜s｜＜Ｆ／Ｋ、あるいは｜σ｜＜Ｆ／Ｋ）の中で滑らかにされている。だが、このアプローチは必ずしも使用可能な制御則を提供するとは限らない。式（２４）を満たす位置制御器に関しては、それが、ゲインＫＨの微分フィードバックを使用するが、それは非常に高くなりえるので、速度計測値のノイズは過度に拡大されることになる。

【0083】

式（２５）を満たす位置制御器は高ゲイン加速度フィードバックを必要とするが、それはさらに実際上困難である。

【0084】

非特許文献１０は、ＰＤ型スライディングモード制御器［式（８）を満たす位置制御器］の一つの近似であるProxy−basedスライディングモード制御（ＰＳＭＣ）を提案した。ＰＳＭＣの連続時間表現は次の微分代数方程式として表現できる。

【数46】

【0085】

ここで、a∈Ｒは新しく導入された状態変数、Ｋ，Ｂ及びＬは適切な正定数である。式（２６）はτとaに関する微分代数方程式（１組の連立微分方程式）として見ることができ、Ｋを大きくすると式（２６）はＰＤ型スライディングモード制御器（８）の式に近くなる。式（２６）は不連続関数ｓｇｎを含んでいるが、式（２６）は実際は連続な制御器の条件式である。これは関係（４）の使用によって、以下のように式（２６）を等価的に書き直すことができるという事実を通して見ることができる。

【数47】

【0086】

これは連続関数だけを含んでいる。

【0087】

制御器（２６）（すなわち（２７））の可能な物理的な１つの解釈は図２のように説明できる。ここで、

【数48】

【0088】

は、プロクシの位置として解釈することができる。それは質量がない仮想物体である。”プロクシ”という用語は、力覚提示（例えばD. C. Ruspini, K. Kolarov, and O. Khatib, “The haptic display of complex graphical environments,” in Proc. ACM SIGGRAPH 97, 1997, pp. 345-352.）の分野で使用されているものである。なお本願明細書中では、これはアドミッタンス制御器４の中の仮想物体とは全く関係がない。プロクシはＰＩＤ制御器［式（２６ａ）を満たす位置制御器］を介して実際の制御対象に接続され、スライディングモード制御器［式（２６ｂ）を満たす位置制御器］によって目標位置ｐ_ｄ（それはアドミッタンス制御の中で使用される仮想物体の位置である）に接続される。プロクシには質量がないので、２つの制御器によって生成された力は互いと釣り合う。そのため、式（２６ａ）と式（２６ｂ）の両方の中でτが用いられている。方程式（２６ｂ）は、

【数49】

【0089】

が不飽和の時期に満たされることを示唆している。また、ロボット位置ｐは、ＰＩＤ制御器（２６ａ）の効果として、プロクシ位置ｐ_sに追従するように制御される。Ｋを増加するとｐ_sがｐに近くなることを理解できる。これは、式（２６）を満たす位置制御器が高ゲイン速度フィードバックを含んでいないＰＤ型スライディングモード制御器［式（８）を満たす位置制御器］の近似であることを意味する。

【0090】

式（８）を満たす位置制御器もまた、アドミッタンス制御器の内部の位置制御器として欠点がある。１つは、Ｋが高い場合、式（８）は式（１３）に近くなるため、リーチングモード（到達モード）からスライディングモードへの遷移で不連続のアクチュエータ力が発生するということである。さらにスライディングモードにおいては、式（８）は加速度フィードフォワード項［式（７）］中の

【数50】

【0091】

のないＰＩＤ制御器と等価になる。そのためスライディングモードにおいて、ロボットが堅い環境に接している場合には不安定になる傾向がある。

【0092】

［本発明の位置制御器］
前述式（１６）を満たすＰＤＤ²型スライディングモード制御器はいくつかの理論的な長所を持つが、従来の境界層アプローチによっては実装できない。そこで発明者は、微分代数近似を備えたＰＤＤ²型スライディングモード制御を実現することを考えた。この制御を行う位置制御器は下記の微分代数方程式を満たす。

【数51】

【0093】

ここで、ａは位置制御器に記憶された内部状態変数、Ｋ，Ｂ，Ｌ，Ｈ，Ｊ，Ｍ及びＦは負でない定数である。式（２８）を満たす位置制御器は、現在位置ｐ及び目標位置ｐ_ｄを入力とし、状態を更新し、アクチュエータ力（トルク）τを出力とする。

【0094】

式（２８）を満たす位置制御器にＭ＝０及びＪ＝０を代入すると式（２６）になる。式（２６）を満たした位置制御器は、発明者が先に提案した非特許文献１０の中のＰＳＭＣである。式（２８）中の項

【数52】

【0095】

は、ロボット慣性Ｍｒの影響を償うことにより位置制御対象の帯域幅を拡張するためのものである。この項はスライディングモードにおける接触安定性を改善する。（２８ｂ）中の項

【数53】

【0096】

は、先に説明したＰＤＤ²型スライディングモードをほぼ実現するためのものである。

【0097】

式（２８ａ）及び（２８ｂ）の物理的な解釈を図４を用いて説明する。ＰSMCの場合と同様に、プロクシ位置は

【数54】

【0098】

に相当する。図４中のＰＩＤ制御器は付加的な加速度フィード・フォワード項を持つ式（２８ｂ）に置換されている。また、スライディングモード制御器はＰＤＤ²型の式（２８ａ）で置換されている。Ｋが増加すると、ｐ_sがｐに近づき、式（２８ａ）及び（２８ｂ）を満たす位置制御器が式（１６）を満たすＰＤＤ²型スライディングモード制御器に近くなる。

【0099】

［本発明の位置制御器の特性］
以下の説明では、便宜上、

【数55】

【0100】

と定義する。それにより、式（２８ａ）及び（２９ｂ）は以下のように書きかえられる。

【数56】

【0101】

Ｋを無限大に近づけると、上記式（２９ａ）及び（２９ｂ）は次の２つのサブシステムへ分解される。

【数57】

【0102】

これは、式（２９ａ）及び（２９ｂ）を満たす位置制御器（すなわちそれと等価な式（２９）を満たす位置制御器）は式（１６）を満たすＰＤＤ²型スライディングモード制御器の近似であることを意味する。

【0103】

式（２８ａ）及び（２９ｂ）を注意深く観察すると、

【数58】

【0104】

が｜τ｜＜Fを満たすτを実現する限りにおいて、ｐ_sは、

【数59】

【0105】

を満たすように時間変化することが判る。この期間に、（２８ａ）及び（２９ｂ）は加速度フィードフォワードを持ったＰＩＤ制御器として作用する。τが｜τ｜＜Ｆを満たさない場合、ｐ_sは、

【数60】

【0106】

を満たすように時間変化する。これは、実際の加速度

【数61】

【0107】

が位置制御器の中で使用されず、速度フィードバックが、ゲインＢ（それは過減衰であるほど高い必要がない）でのみ行われることを意味する。この意味で、微分代数近似（２８）は、式（２５）で示される単純な境界層近似とは本質的に異なる。

【0108】

［離散時間システムへの実装］
ここで、式（２８ａ）及び（２８ｂ）を満たす位置制御器の離散時間型アルゴリズムを導出する。後退オイラー法の使用によって、式（２８ａ）及び（２８ｂ）は以下のように近似することができる。

【数62】

【0109】

そこで

【数63】

【0110】

であり、また、

【数64】

【0111】

である。ここで、ｋは離散時間インデックスである整数であり、Ｔ＞０は時間刻み幅である。また、▽は後退差分演算子を表し、▽z(k)＝z(k)−z（k−１）として定義され、▽²z(k)＝z(k)−２z（k−１）＋z（k−２）を満たす。

【0112】

方程式（３２ａ）及び（３２ｂ）はτ(k)及びσ(k)を未知数とする１セットの連立方程式である、定理１の使用によって、式（３２ａ）は以下のようにも表すことができる。

【数65】

【0113】

τ*(k)が未知数τ(k)とσ(k)に依存していないことに気づくと、式（４０）が式（３２）のτ(k)についての解析解であることが分かる。もう一方の未知数a(k)は、式（３２ｂ）により得ることができる。

【0114】

結論としては、微分代数方程式（２８ａ）及び（２８ｂ）の解は、次の計算手順で数値的に得ることができる。

【数66】

【0115】

ここでＣs，Ｃ1，Ｃ2及びＣ3は（３６）から（３９）に定義されている。この制御アルゴリズム（４１ａ）〜（４１ｅ）は、現在位置ｐ(k)及び仮想物体の位置ｐ_ｄ(k)を入力とし、アクチュエータ力τ(k)を出力とする位置制御器が満たす条件式である。

【0116】

（４１ａ）〜（４１ｅ）にM＝０及びJ＝０を代入する、それはＰＳＭＣのアルゴリズム（非特許文献１０中の式（３２））と等価になる。連続時間領域のＰＳＭＣの安定性は非特許文献１０で議論されている、同様のアプローチを新しい位置制御器へ適用することは難しい。これは、連続時間形式（２８ａ）及び（２８ｂ）の中のｓｇｎの引数の中に

【数67】

【0117】

があるためである。そこでここでは、実験的に検証する。

【0118】

［実験］
・実験装置
上記の提案された制御アルゴリズム（４１ａ）〜（４１ｅ）を満たす位置制御器は、図５（ａ）及び（ｂ）に示される６自由度産業用ロボットMOTOMAN−HＰ3J（安川電機株式会社）の使用により実験的にテストされた。ロボットはARTリナックス（登録商標）・オペレーティング・システムを実行するＰＣでコントロールされ、波動歯車装置及びオプティカルエンコーダに統合された６つのＡＣサーボモータを持つ。６軸力センサ（ニッタ株式会社）はロボットの先端に付けられ、また、図５（ａ）のように、力センサに円柱状形状のヘッド付きのボルトがインストールされた。

【0119】

１枚のスポンジゴムが、エンドエフェクタ（つまりボルトヘッド）が接触するべき「環境」として使用された。ゴムシートは、図５（ｂ）に示されるようなロボットの台座に固定されたアルミニウム・フレームに付けられた。実験では、根元の関節だけが使用され、また、他の関節は根元関節が回転したとき、エンドエフェクタが一定の角度を維持して環境と接触するように制御された。次の記述では、物理量はすべて、エンドエフェクタの円弧パス（０．４４ｍの半径を備えた）に沿った並進システムで測定された。それは根元関節まわりの回転システムに対応する。

【0120】

根元関節のギヤ比は１００であった。予備試験を通じて、図６（ａ）に示されるように、根元関節の中の摩擦力の大きさが約１０Ｎだったことが分かった。図６（ｂ）に示されるように、エンドエフェクタの位置とゴムシートからの接触力の関係は得られた。それは、浅い押し付けにおいてはゴムシートの弾性係数が約６００Ｎ／ｍだったことを示す。

【0121】

実験では、根元関節は、時間刻み幅Ｔ＝０．００１sを備えたアドミッタンス制御器によってコントロールされた。仮想物体を示す式（６）の動特性パラメータは、m＝１kg及びb＝１ Ｎ・s／mに選ばれ、また、それは次の離散時間型制御器として作用した。

【数68】

【0122】

次の６台の内部位置制御器が使用された。

【0123】

・PSMC2＋AFF：本発明の位置制御器［制御アルゴリズム（４１ａ）〜（４１ｅ）を満たすもの］
・PSMC2：［制御アルゴリズム（４１ａ）〜（４１ｅ）において、M＝０としたもの］
・PSMC＋AFF：［制御アルゴリズム（４１ａ）〜（４１ｅ）においてJ＝０としたもの］
・PSMC2：［制御アルゴリズム（４１ａ）〜（４１ｅ）においてM＝0及びJ＝0としたもの。非特許文献１０の式（３２）と等価］
・SPID＋AFF：飽和つきＰＩＤ制御器＋加速度フィードフォワード、すなわち、式（７）で｜τ｜をFで制限したもの
・SPID：飽和つきＰＩＤ制御器、すなわち、式（７）でＭ＝0とし、｜τ｜をＦで制限したもの
別段の定めがない限り、位置制御器用のパラメータはＫ＝６０００Ｎ／ｍ、Ｂ＝１０００ Ｎ・s／m、Ｌ＝９００００Ｎ／ｍ／ｓ、Ｆ＝２０Ｎ、Ｈ＝０．４s、Ｊ＝０．０４s²、Ｍ＝１０kg・m²として選ばれ、ゲインＫ，Ｂ及びＬは、システムの不安定化しない範囲でできるだけ大きく設定された。また、Ｆは、関節中の摩擦力の大きさより十分に大きく設定された。ＨとＪの値は０．２sの時定数の臨界減衰に帰着するために選ばれた。

【0124】

Ｍの値は、制御器PSMC2＋AFFが安定した接触を実現するように選ばれた。小さいＭの値は不安定性あるいは発振に帰着した。また、より大きなＭ値は、力センサによって測定されたfの中のノイズを拡大することになり、τの中の高周波振動に帰着した。理論的な視点から見て、Ｍは、制御対象（前述のＭｒ）の慣性を考えて選ばれるべきである。しかし、本実験装置では、関節の弾性の存在のためにそれを同定することが難しかった。式（４１ａ）〜（４１ｅ）の中のτ_fを、非特許文献（G. Ferretti, G. Magnani, and P. Rocco, “Impedance control for elastic joints industrial manipulators,” IEEE Trans. Robotics and Automation, vol. 20, no. 3, pp. 488-498, 2004.）の中のもののような、より精密なフィードフォワード項に置換することにより、安定性を向上することは可能であると考える。

【0125】

［実験Ｉ］変動する力での押し付け動作
実験のこのセットでは、ロボットは指定された目標力によってゴムシートと接触を維持するように制御された。ロボットは、ゴムシートと軽く接触するように最初にセットされ、アドミッタンス制御が開始された。目標力ｆ_ｄは、−８Ｎ及び−３Ｎの間で０．５s毎に切り替えられた。

【0126】

図７は、６台の位置制御器で得られた結果を示している。ここでｆ≧０は、ロボットがそのときゴム・シートに接していないことを意味する。後者３つの位置制御器は、ゴムシート上でエンドエフェクタがバウンドし、性能が劣っていることが見て取れる。PSMC2＋AFF、PSMC2、PSMC＋AFF及びPSMCの比較を通じて、Ｊ＞０及びＭ＞０は小さい跳ね返りと小さい振動、そして、よりよい接触力のトラッキングに帰着することが読み取れる。

【0127】

図８は、図７中のPSMC2＋AFF及びPSMC＋AFFの結果を別の色と時間スケールで表示したものである。図８の中の円によって強調されるように、リーチングモード（｜τ｜＝F）からスライディングモード（｜τ｜＜Ｆ）までの推移では、アクチュエータの力τの変化はPSMC2＋AFF（J＝0）よりPSMC2＋AFF（J＞0）がむしろ滑らかである。

【0128】

［実験ＩＩ］衝突の後の押し付け動作
実験の第２セットでは、ロボットをまずゴム・シートから約０．１６ｍ遠ざけてセットし、目標力は、ｆ_ｄ＝５Ｎに定めた。衝突により、ロボットは、実験Ｉよりも強く跳ね返る傾向を示した。Ｊ及びＭの異なる値を備えた制御器PSMC2＋AFF（特殊ケースとしてPSMC＋AFF及びPSMC2を含む）は、それらのパラメータの影響を示すために使用された。Ｈ²／４より大きなＪ値は試していない。これは、その場合、リーチング（飽和）モードからスライディング（不飽和）モードへの遷移の後に、オーバーシュートを発生することが理論上明らかであるからである。

【0129】

図９は結果を示す。Ｍの増加が振動を大きくすることがわかる。振動を引き起こすのと同じく、Ｊの増加によって弾み幅が大きくなることもわかる。これらの結果により、新しい位置の制御器におけるＭ＞０及びＪ＞０の必要性がわかる。

【0130】

［実験ＩＩＩ］
この実験中では、実験者がエンドエフェクタを軽く手でつまみ、およそ１Hzの周波数で揺り動かした。この実験では、PSMC2＋AFF、PSMC2及びPSMC＋AFFのみを使用した。これは、他の位置制御器の性能が低いということがすでに実験Ｉ中において示されているからである。目標力はｆ_ｄ＝０とした。

【0131】

図１０に、３つの内部位置制御器で得られた位置ｐ、測定された力f及びアクチュエータ力τのデータを示す。Ｊ＞０を含むことにより、接触力の変動が小さく滑らかになっていることが示されている。それは、実験者がロボットを滑らかに容易に移動させることができたことを意味する。図１０（ｃ）の中のｐ−fプロットは、実験者が彼の運動の方向を変更した時Ｊ＝０を持った制御器が実験者の運動に対する抵抗力を発生し、これによって、大きな力が実験者の手とエンドエフェクタの間で発生されたことを示す。注目すべき点は、アクチュエータ力τにすべての制御器がほとんど常に飽和となっているということである。図１０下部のグラフについての注意深い観察によって、正の飽和から負の飽和までの遷移がｆの増加が引き金となって起きていることが分かる。ｆが増加しているとき、PSMC2＋AFF及びPSMC2は、PSMC＋AFFよりレスポンスが速く、fの一層の増加を防いでいる。すなわち、Ｊ＞０はアクチュエータ力が飽和しているときでも迅速なレスポンスを実現することに寄与することを理解することができる。それは、ロボットの滑らかな運動及び接触力の滑らかな変化に帰着する。

【0132】

上記実施の形態は、本発明の位置制御器をアドミッタンス制御器の内部位置制御器として用いたものである。しかしながら、本発明の位置制御器は、アドミッタンス制御から本質的に独立したものである。したがって、アドミッタンス制御器の内部位置制御器としても当然にして適用することは可能である。

【産業上の利用可能性】

【0133】

本発明の位置制御器は、アドミッタンス制御器の内部位置制御器として使用に適している。また本発明の位置制御器はPSMC（非特許文献１０）の拡張で、特にアクチュエータ力が飽和する場合においても動作を継続することがロボットに要求される場面への適用に有利である。

【0134】

本発明の位置制御器は、加速度フィードバックを含むスライディングモード位置制御器についての理論的考察から導出されたが、本発明の位置制御器の中では直接の加速度フィードバックは用いられていない。本発明の位置制御器は、飽和期間と不飽和期間の間の滑らかな遷移を実現する。また、飽和期間にも、加えられた力の変化に速く応答する。

【符号の説明】

【0135】

１ 移動機構
３ 駆動装置
５ 位置制御器
７ 力センサ
９ 位置検出器
１１ シミュレート部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】