特許 6934173 力制御装置、力制御方法及び力制御プログラム

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

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

(11)【特許番号】6934173

(24)【登録日】2021年8月25日

(45)【発行日】2021年9月15日

(54)【発明の名称】力制御装置、力制御方法及び力制御プログラム

(51)【国際特許分類】

   G05B 17/02 20060101AFI20210906BHJP

   B25J 9/16 20060101ALI20210906BHJP

【ＦＩ】

   G05B17/02

   B25J9/16

【請求項の数】8

【全頁数】20

(21)【出願番号】特願2017-136283(P2017-136283)

(22)【出願日】2017年7月12日

(65)【公開番号】特開2019-20826(P2019-20826A)

(43)【公開日】2019年2月7日

【審査請求日】2020年4月3日

(73)【特許権者】

【識別番号】504145342

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

(74)【代理人】

【識別番号】100099634

【弁理士】

【氏名又は名称】平井 安雄

(72)【発明者】

【氏名】菊植 亮

【審査官】 松本 泰典

(56)【参考文献】

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

【文献】 特開２００９−０６６６９６（ＪＰ，Ａ）

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

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

【文献】 菊植亮、水谷孝樹，アドミッタンス制御の内部位置制御におけるプロクシべースト・スライディングモード制御の有効性，第２９回日本ロボット学会学術講演会予稿集ＤＶＤ−ＲＯＭ ２０１１年 Ｔｈｅ ２９ｔｈ Ａｎｎｕａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｆ ｔｈｅ Ｒｏｂｏｔｉｃｓ Ｓｏｃｉｅｔｙ ｏｆ Ｊａｐａｎ，日本，一般社団法人日本ロボット学会，２０１１年 ９月 ７日，pp.1446-1449

【文献】 Nansai Shunsuke et.al.，Dynamic Hybrid Position Force Control using Virtual Internal Model to Realize a Cutting Task by a Snake-like Robot，2016 6th IEEE International Conference on Biomedical Robotics and Biomechatronics (BioRob)，SG，IEEE，２０１６年 ６月２６日，p.151-156

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

Ｇ０５Ｂ １７／０２

Ｂ２５Ｊ ９／１６

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

【請求項1】

制御対象の位置を検出する位置検出器と、
前記制御対象が物体と接触する力を検出する力センサと、
前記制御対象を移動可能に保持する移動機構と、
トルク指令値を入力として前記移動機構を動作させるモータを有する駆動装置と、
前記トルク指令値を決定する演算装置とを備え、
前記演算装置が、
所定の動特性を有する仮想物体、及び位置制御器を保持しており、前記制御対象に加える目標力、及び前記力センサの測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御器に次の時刻における目標位置として与えたときに算出されるトルクを求め、
前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、
前記トルクが予め設定された所定の範囲外にある場合は、前記所定の範囲の境界上にあって前記トルクに最も近いトルクをトルク指令値として定め、前記位置制御器が算出するトルク指令値が、定められた前記トルク指令値となる次の時刻における目標位置を算出し、当該目標位置を前記仮想物体の次の時刻の位置として定めるように、前記トルク指令値について代数ループでフィードバックすることを特徴とする力制御装置。

【請求項2】

請求項１に記載の力制御装置において、
前記演算装置が、連続時間において、前記トルクをτ、前記所定のトルクの範囲をＦとし、前記仮想物体の位置をｑ_ｘとし、前記制御対象の実際の位置をｑ_ｓ、前記目標力をｆ_ｄ、前記力センサの測定値をｆとし、前記仮想物体の前記所定動特性がある関数Γ_ｃによって

【数1】

と表されるとした場合に、

【数2】

を満たすようにｑ_ｘが更新されてτが決定される力制御装置。（ただし、関数Φ_ｃはｑ_ｓが目標値ｑ_ｘに追従するように定義された位置制御器とする。）

【請求項3】

請求項１に記載の力制御装置において、
前記演算装置が、離散時間中の時刻ｋにおいて、前記トルクをτ、前記所定のトルクの範囲をＦとし、前記仮想物体の位置をｑ_ｘとし、前記制御対象の実際の位置をｑ_ｓ、前記目標力をｆ_ｄ、前記力センサの測定値をｆとし、前記仮想物体の前記所定動特性がある関数Γによって

【数3】

と表されるとした場合に、

【数4】

を満たすようにｑ_ｘ（ｋ）及びτ（ｋ）が決定される力制御装置。（ただし、関数Φはｑ_ｓが目標値ｑ_ｘに追従するように定義された位置制御器とし、Ｓ（ｋ−１）は積分項、前ステップの測定位置ｐ_ｓ（ｋ−１）、前ステップの目標位置ｐ_ｄ（ｋ−１）などの制御器内に蓄えられた状態変数からなる集合であり、この関数の出力τ（ｋ）を前記トルク指令値とする。）

【請求項4】

請求項３に記載の力制御装置において、
前記関数Γが、

【数5】

の入出力関係を持つ関数で与えられる力制御装置（ここで、Ｔ、Ｍ_ｖ及びＢ_ｖは正の定数である）。

【請求項5】

請求項３に記載の力制御装置において、
前記関数Φが、

【数6】

のアルゴリズムと同一の入出力関係を持つ関数である力制御装置（ここで、Ａ＾、Ｂ、Ｌ、Ｍ及びＴは正の定数である）。

【請求項6】

請求項３に記載の力制御装置において、
ｑ_ｘ（ｋ）及びτ（ｋ）を決定する計算が、

【数7】

のアルゴリズムと同一の入出力関係を持つ力制御装置。

【請求項7】

コンピュータが、
制御対象の位置を検出する位置検出ステップと、
前記制御対象が物体と接触する力を検出する力検出ステップと、
トルク指令値を入力として前記制御対象を移動可能に保持する移動機構を動作させるモータを有する駆動装置の前記トルク指令値を演算する演算ステップと、
前記演算ステップの演算結果に基づいて前記制御対象を移動する移動ステップとを実行し、
前記演算ステップが、
前記コンピュータの内部に保持された所定の動特性を有する仮想物体、及び位置制御器を用い、前記制御対象に加える目標力、及び前記力検出ステップの測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御器に次の時刻における目標位置として与えたときに算出されるトルクを求め、
前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、
前記トルクが予め設定された所定の範囲外にある場合は、前記所定の範囲の境界上にあって前記トルクに最も近いトルクをトルク指令値として定め、前記位置制御器が算出するトルク指令値が、定められた前記トルク指令値となる次の時刻における目標位置を算出し、当該目標位置を前記仮想物体の次の時刻の位置として定めるように、前記トルク指令値について代数ループでフィードバックすることを特徴とする力制御方法。

【請求項8】

制御対象の位置を検出する位置検出手段、
前記制御対象が物体と接触する力を検出する力検出手段、
トルク指令値を入力として前記制御対象を移動可能に保持する移動機構を動作させるモータを有する駆動装置の前記トルク指令値を演算する演算手段、
前記演算手段の演算結果に基づいて前記制御対象の移動を制御する移動制御手段としてコンピュータを機能させ、
前記演算手段が、
前記コンピュータの内部に保持された所定の動特性を有する仮想物体、及び位置制御器を用い、前記制御対象に加える目標力、及び前記力検出手段の測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御器に次の時刻における目標位置として与えたときに算出されるトルクを求め、
前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、
前記トルクが予め設定された所定の範囲外にある場合は、前記所定の範囲の境界上にあって前記トルクに最も近いトルクをトルク指令値として定め、前記位置制御器が算出するトルク指令値が、定められた前記トルク指令値となる次の時刻における目標位置を算出し、当該目標位置を前記仮想物体の次の時刻の位置として定めるように、前記トルク指令値について代数ループでフィードバックすることを特徴とする力制御プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、力センサを用いてアドミッタンス制御を行う力制御装置等に関する。

【背景技術】

【0002】

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

【0003】

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

【0004】

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

【0005】

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

【0006】

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

【0007】

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

【0008】

ＰＳＭＣは、トルク不飽和時にはＰＩＤと等価であるため目標位置に対する現在位置追従特性が不十分であることもある。またＰＳＭＣの場合、トルク飽和に関しては、飽和から不飽和への遷移時にτが不連続になって、ロボットを触る作業者に、違和感を感じさせる原因となっている。このような問題に対して、位置指令が頻繁及び急激に変わるときでも位置の追従性を向上させることができ、しかもトルク飽和時でも不連続な動きを抑えることができる位置制御器を備えたアドミッタンス制御を用いた力制御装置に関する技術が開示されている（特許文献２）。

【先行技術文献】

【非特許文献】

【0009】

【非特許文献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.

【特許文献】

【0010】

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

【特許文献2】特許第６０３２８１１号

【発明の概要】

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

【0011】

特許文献２に示す技術は、位置の追従性を向上させつつ、トルク飽和時でも不連続な動きを抑えることができるものの、仮想物体とロボットとの位置が離れることで、どうしても追従処理に違和感が生じてしまう可能性がある。

【0012】

本発明は、トルク飽和した場合であっても、仮想物体の位置をフィードバックすることで、仮想物体と制御対象との位置ずれを低減し、追従性を向上させつつ安定性を向上させる力制御装置等を提供する。

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

【0013】

本発明に係る力制御装置は、制御対象の位置を検出する位置検出器と、前記制御対象が物体と接触する力を検出する力センサと、前記制御対象を移動可能に保持する移動機構と、トルク指令値を入力として前記移動機構を動作させるモータを有する駆動装置と、前記トルク指令値を決定する演算装置とを備え、前記演算装置が、所定の動特性を有する仮想物体、及び位置制御則を保持しており、前記制御対象に加える目標力、及び前記力センサの測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御則に次の時刻における目標位置として与えたときに算出されるトルクを求め、前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、前記トルクが予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及び前記トルクに基づいて、前記仮想物体の次の時刻における位置が定められるものである。

【0014】

このように、本発明に係る力制御装置においては、制御対象の位置を検出する位置検出器と、前記制御対象が物体と接触する力を検出する力センサと、前記制御対象を移動可能に保持する移動機構と、トルク指令値を入力として前記移動機構を動作させるモータを有する駆動装置と、前記トルク指令値を決定する演算装置とを備え、算出されたトルクが予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及び前記トルクに基づいて、前記仮想物体の次の時刻における位置が定められるため、制御対象と仮想物体との位置のずれが低減され、追従性を向上させつつ安定性を向上させることができるという効果を奏する。

【図面の簡単な説明】

【0015】

【図1】従来の力制御装置の処理概念の一例を示す機能ブロック図である。

【図2】第１の実施形態に係る力制御装置の処理概念の一例を示す機能ブロック図である。

【図3】第１の実施形態に係る力制御装置のハードウェア構成図である。

【図4】第１の実施形態に係る力制御装置の機能ブロック図である。

【図5】第１の実施形態に係る力制御装置の動作を示すフローチャートである。

【図6】実施例において使用したロボットの写真である。

【図7】従来の手法を用いた場合のアドミッタンス制御の結果を示す図である。

【図8】本発明に係る力制御装置を用いた場合のアドミッタンス制御の結果を示す図である。

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

【0016】

以下、本発明の実施の形態を説明する。また、本実施形態の全体を通して同じ要素には同じ符号を付けている。

【0017】

（本発明の第１の実施形態）
本実施形態に係る力制御装置について、図１ないし図５を用いて説明する。図１は、従来の力制御装置の処理概念の一例を示す機能ブロック図であり、図２は、本実施形態に係る力制御装置の処理概念の一例を示す機能ブロック図である。図１に示すように、従来技術におけるアドミッタンス制御は、力センサ１１の測定値ｆと目標力（制御対象であるロボット１４に対して加える力）ｆ_ｄとから所定の動特性を有する仮想物体１２の位置Ｐ_ｄを目標位置として求める。目標位置Ｐ_ｄを満たすように位置制御器１３がロボット１４のモータのトルク指令値τを算出する。外部からロボット１４に加えられる力ｈ、力センサ１１の測定値ｆ及びトルク指令値τに基づいて、ロボット１４が位置Ｐに移動する。

【0018】

図１の従来技術の場合は、トルク指令値τがトルク範囲（例えば、安全性、ロボット１４の駆動特性、環境等に応じて予め設定されるトルクの上限値及び下限値）外となる場合は、仮想物体１２の位置Ｐ_ｄとロボット１４の位置Ｐとが乖離してしまう。従来技術においては、この飽和状態から不飽和状態に遷移する必要があるため、どうしても追従処理に違和感が生じてしまう。そこで、本実施形態においては、例えば、図２に示すような処理を行う。

【0019】

図２において、位置制御器１３で算出されたトルク指令値τについて、代数ループでフィードバックすることで、仮想物体１２の位置ｑ_ｘとロボット１４の位置ｑ_ｓとを一致させることができ、従来のような追従処理における違和感を排除することが可能となる。

【0020】

以下、本実施形態に係る力制御装置の構成及びアルゴリズムについて、具体的に説明する。図３は、本実施形態に係る力制御装置のハードウェア構成図である。力制御装置１は、制御対象であるロボット１４と、当該ロボット１４に付設され当該ロボット１４の周囲環境にある物体からの反力を測定する力センサ１１と、アドミッタンス制御を行うためのプログラム等が格納されたＲＯＭ２３と、必要に応じて当該プログラムをＲＡＭ２２に読み込み処理を実行するＣＰＵ２１と、タッチパネル、キーボード、マウス等の入力機器からの入力を受け付けたり、プリンタやディスプレイ等にデータを出力するためのインタフェースである入出力Ｉ／Ｆ２４とを備える。各処理部はバスを介して接続され、情報のやり取りを行う。なお、上記ハードウェアの構成はあくまで一例であり、必要に応じて変更可能である。

【0021】

図４は、本実施形態に係る力制御装置の機能ブロック図である。力制御装置１は、環境に対して所定の力を作用させるロボット１４と、当該ロボット１４の移動を制御する演算装置４０とを備える。ロボット１４は、周囲環境にある物体からの反力を測定する力センサ１１と、ロボット１４又は当該ロボット１４の一部（例えば、アーム部等）を移動する移動機構４１と、当該移動機構４１を駆動するためのモータからなる駆動装置４２とを備える。演算装置４０は、力センサ１１からの測定結果と移動機構４１の位置情報とに基づいて、駆動装置４２のモータのトルク指令値を演算し、駆動装置４２に出力する。演算装置４０によるトルク指令値の演算処理について、以下に詳細に説明する。

【0022】

（１）数学的準備
Ｆを実数上の閉区間とする。それに対する飽和関数と法錐（Normal Cone）は下記のように定義される。

【0023】

【数1】

【0024】

飽和関数と法錐との関係は下記のように表される（参考文献：Acary, V., Brogliato, B.: “Numerical Methods for Nonsmooth Dynamical Systems”, Applications in Mechanics and Electronics, Lecture Notes in Applied and Computational Mechanics, vol.35. Springer (2008)）。

【0025】

【数2】

【0026】

符号関数を下記のように定義する。

【0027】

【数3】

【0028】

正の定数Ｆで定義される閉区間［−Ｆ，Ｆ］について、下記が成立する。

【0029】

【数4】

【0030】

符号関数と法錐は下記のように、互いに逆写像の関係にある。

【0031】

【数5】

【0032】

また、符号関数と飽和関数の関係は下記のように表される。

【0033】

【数6】

【0034】

（２）次に、提案する制御則の一般表現について説明する。
（２．１）連続時間表現
制御対象（ロボット１４）の実際の位置をｑ_ｓ、仮想物体１２の位置をｑ_ｘ、駆動装置４２における発生力をτ、力センサ１１の測定値をｆ、アドミッタンス制御器に与える目標力をｆ_ｄとする（ｆ_ｄ＝０で固定してもよい）。また、目標位置をｑ_ｘとしたときのロボット１４の位置制御則が適切な関数Φ_ｃによって

【0035】

【数7】

【0036】

と与えられているものとする。さらに、仮想物体が実現するべき動特性は、適切な関数Г_ｃによって

【0037】

【数8】

【0038】

と与えられているものとする。これらの関数の具体例は（３）節で後述する。駆動装置４の発生力τの許容範囲を集合Ｆと定めたとき、提案する制御器の連続時間表現は下記のように表される。

【0039】

【数9】

【0040】

これはτ及びｑ_ｘ（二階微分）を未知数とする連立微分方程式と見なすことができる。また、図２のような代数ループを含んだブロック線図として表すこともできる。式（１１ａ）の法錐Ｎ_ｆ（・）は引数であるが、τがＦ外に存在することを許容しないため、上記を満たすためには、τがＦ内になければならない。そうなるようにｑ_ｘが更新される（ｑ_ｘ（二階微分）が決定される）というように解釈できる。式（１０）が満たされるようにｑ_ｘ（二階微分）を定め、それを式（１１ｂ）に代入した際に算出される発生力τがＦ内にあれば、そのτの値はそのまま採用される。一方で、そのτがＦ外にあれば、τはＦの境界上のτに最も近い値で飽和し、そのときは式（１１ｂ）が満たされるようにｑ_ｘ（二階微分）が定められることになる。

【0041】

（２．２）離散時間表現（代数ループを含んだ表現）
制御器（１１）の離散時間表現を導く。

【0042】

【数10】

【0043】

のように離散化すると、式（１１ａ）は適切な関数Γを用いて、

【0044】

【数11】

【0045】

という形で書き直すことができる。また、式（１１ｂ）は適切な関数Φを用いて、

【0046】

【数12】

【0047】

という形で書き直すことができる。ここで、Ｓ（ｋ−１）は、例えば、積分項や前ステップの測定位置ｑ_ｓ（ｋ−１）や前ステップの目標位置ｐ_ｄ（ｋ−１）などからなる集合である。この関数の出力値τ（ｋ）をモータへのトルク指令値とする。さらに別の関数Φ_ｓを用いて、

【0048】

【数13】

【0049】

のように次ステップのＳ（ｋ）を決定するものとする。このＳ（ｋ）は、次ステップの入力値の一つＳ（ｋ−１）として用いる。式（１２ａ）及び式（１２ｂ）は未知数τ（ｋ）とｑ_ｘ（ｋ）についての連立方程式と見なすことができ、これも図２のようなブロック線図のように解釈することができる。これらを用いると、提案するアドミッタンス制御のアルゴリズムは下記のように表される。

【0050】

【数14】

【0051】

これらの表現は、代数包含式（１４ｂ）を解く工程を含んでいるため、このまま実装することは困難である。

【0052】

（２．３）離散時間表現（アルゴリズムとしての表現）
代数包含式（１４ｂ）を解く方法を考える。ここで、以下のような仮定をおく。
仮定１ 関数Φ（ｑ_ｘ，ｑ_ｓ，Ｓ）は引数ｑ_ｘについて厳密に単調増加である。
多くの典型的な制御器はこの仮定を満たす。この仮定のもとで、以下の式が成り立つことは明らかである。

【0053】

【数15】

【0054】

また、この仮定のもとで、下記を満たす一価関数Φ_ｉｎｖが存在することは明らかである。

【0055】

【数16】

【0056】

この関数Φ_ｉｎｖは、いわば逆制御器とも呼ぶことができる。通常の制御器は目標位置に対してトルク指令値を与える。それに対し逆制御器は、指定されたトルク指令値を出力するのに必要な目標位置を算出する。式（１５）を用いると、式（１４ｂ）の代数包含式は下記のように書き直すことができる。

【数17】

【0057】

ここでは、法錐Ｎ_Ｆ（τ（ｋ））の返り値が（−∞，０]，０，[０，∞）の３通りしかあり得ないことを考慮している。式（８）より、上式は下記のようになる。

【0058】

【数18】

【0059】

ここで、式（２０）は、式（１６）で定義される逆制御器Φ_ｉｎｖを用いると、

【0060】

【数19】

【0061】

と書き直すことができる。したがって、式（１４）のアルゴリズムは下記のように書き直すことができる。

【0062】

【数20】

【0063】

上式が、提案するアドミッタンス制御のアルゴリズムの一つの形態である。仮定１に追加して、以下の仮定２が満たされる場合には、アルゴリズムをさらに簡略化することができる。
仮定２ Φ（ｑ_ｘ，ｑ_ｓ，Ｓ）＝Ｋ＾（ｑ_ｘ−Φ_ｐ（ｑ_ｓ，Ｓ））がすべてのｑ_ｘ，ｑ_ｓ，Ｓに対して満たされる正の定数Ｋ＾と関数Φ_ｐ（ｑ_ｓ，Ｓ）が存在する。

【0064】

これは、制御器Φ（ｑ_ｘ，ｑ_ｓ，Ｓ）が目標値ｑ_ｘに対して線形であることを意味し、多くの典型的な制御器Φ（ｑ_ｘ，ｑ_ｓ，Ｓ）はこの仮定を満たす。このとき、逆制御器関数は、

【0065】

【数21】

【0066】

となり、アルゴリズム（２２）は下記のようになる。

【0067】

【数22】

【0068】

アルゴリズム（２２）における式（２２ｄ）の演算が、アルゴリズム（２４）においては式（２４ｅ）のように単純化されている。このため、関数Φ（）の構造によっては、アルゴリズム（２４）のタイプの実装によって計算量が削減できる。

【0069】

（３）次に、関数Φ_ｃ及びГ_ｃの実例について説明する。
（３．１）関数Φ_ｃの例：目標加速度フィードフォワード付きＰＩＤ制御の場合
目標加速度フィードフォワード付きＰＩＤ制御の場合は、連続時間領域における制御器関数Φ_ｃは下記のように与えられる。

【0070】

【数23】

【0071】

ここで、Ｍｑ_ｘ（ｑ_ｘは二階微分）が目標加速度のフィードフォワード項であり、Ｋ、Ｂ及びＬはそれぞれ比例、微分、積分ゲインである。このとき、下式

【0072】

【数24】

【0073】

を離散化すると、下記のような形になる。

【0074】

【数25】

【0075】

ここで、ａはａ＝∫（ｑ_ｘ−ｑ_ｓ）ｄｔに相当し、

【0076】

【数26】

【0077】

である。また、Ａ＾＝Ｋ＋Ｂ／Ｔ＋ＬＴであり、

【0078】

【数27】

【0079】

である。これらを考慮すると、上記の関数Φｃに対応する関数Φ，Φ_Ｓ，Φ_ｉｎｖは下記のようになる。

【0080】

【数28】

【0081】

また、上記の関数Φ（）は明らかに仮定２を満たすため、関数Φ_ｐを下記のように定めることができる。

【0082】

【数29】

【0083】

したがって、この関数はアルゴリズム（２４）の形式で実装できる。実装した結果は以下のようになる。

【0084】

【数30】

【0085】

（３．２）関数Г_ｃの例：質量・ダンパ系の場合
仮想物体の動特性が単純な慣性・粘性系で与えられている場合、動特性は、

【0086】

【数31】

【0087】

となるので、関数Г_ｃは、

【0088】

【数32】

【0089】

となる。ここで式（３５）を離散化すると

【0090】

【数33】

【0091】

となる。これをｑ_ｘ（ｋ）について解くと、

【0092】

【数34】

【0093】

となる。すなわち、関数Гは下記のようになる。

【0094】

【数35】

【0095】

次に、本実施形態に係る力制御装置の動作について説明する。図５は、本実施形態に係る力制御装置の動作を示すフローチャートである。まず、ロボット１４に対して外部から加わった力、及び力センサ１１の測定値に基づいて、演算装置４０がアドミッタンス制御の演算処理を行う（Ｓ１）。この演算処理は、上記で説明した通りである。演算の結果、ロボット１４を駆動する際のモータのトルク指令値が出力され、ロボット１４に入力される（Ｓ２）。トルク指令値にしたがってロボット１４が駆動する（Ｓ３）。ロボット１４の駆動を終了する場合は、処理を終了する。駆動（停止状態も含む）を継続する場合は、駆動した結果、ロボット１４の位置や力センサ１１の測定値を出力し、ステップＳ１のアドミッタンス制御の演算処理に戻る。以降、同様の処理を繰り返して行う。

【0096】

このように、本実施形態に係る力制御装置においては、算出されたトルク指令値が予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及びトルク指令値に基づいて代数ループを用いて、仮想物体の次の時刻における位置が定められるため、制御対象と仮想物体との位置のずれが低減され、追従性を向上させつつ安定性を向上させることができる。

【実施例】

【0097】

本発明に係る力制御装置について、以下の実験を行った。図６は、実験で使用したロボット１４の写真である。ロボット１４の第３関節に上記アドミッタンス制御則を実装し、その他の関節についてはロックした。そして、力センサ１１に接触して動作させた場合と、リンク部（力センサ１１以外の部位）に接触して動作させた場合の挙動を観察した。

【0098】

図７は、従来の手法を用いた場合のアドミッタンス制御の結果を示す図であり、図８は、本発明に係る力制御装置を用いた場合のアドミッタンス制御の結果を示す図である。図７及び図８において、
（１）〜時刻Ａ：力センサ１１を手で持った状態で動作
（２）時刻Ｂ〜時刻Ｃ：力センサ１１外の場所に力を加えている状態
（３）時刻Ｃ〜：ロボット１４が動かないようにリンク部を手で固定したまま、力センサ１に力を加えている状態
のそれぞれにおける測定結果が示されている。グラフの一番上は、仮想物体１２の位置とロボット１４の位置とを示しており、真ん中は、力センサ１１の測定値を示しており、一番下は、モータの駆動トルクを示している。

【0099】

図７及び図８からわかるように、（１）においては、いずれの手法においてもロボット１４の位置が仮想物体１２の位置によく追従している。（２）においては、図７の従来手法では、ロボット１４の位置と仮想物体１２の位置とが乖離しており、このとき、加えていた力を開放すると、バネのように元の位置に戻ろうとして危険である。一方、図８の本願の手法では、仮想物体１２の位置がロボット１４の位置に追従しているため、加えていた力を開放しても上記のような危険性は全くない。（３）においては、図７の従来手法では、仮想物体１２の位置だけが動き、手を離すとロボット１４が遠く離れた仮想物体１２の位置追従しようとして危険である。一方、図８の本願の手法では、仮想物体１２の位置は動かず、ロボット１４の位置も動かないため、上記のような危険性は全くない。

【0100】

以上のように、トルク飽和が発生するような状況においては、従来手法に比べて、本願の手法の方が安全性が格段に向上していることが明らかとなった。

【符号の説明】

【0101】

１ 力制御装置
１１ 力センサ
１２ 仮想物体
１３ 位置制御器
１４ ロボット
２１ ＣＰＵ
２２ ＲＡＭ
２３ ＲＯＭ
２４ 入出力Ｉ／Ｆ
４０ 演算装置
４１ 移動機構
４２ 駆動装置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】