(57)【要約】      （修正有）【課題】トルク飽和した場合であっても、代数ループを用いて仮想物体の位置をフィードバックすることで、追従性を向上させつつ安定性を向上させる力制御装置を提供する。【解決手段】制御対象の位置検出器と、制御対象の力センサ１１と、制御対象の移動機構と、トルク指令値を入力とするモータを有する駆動装置と、トルク指令値を決定する演算装置とを備え、演算装置が、所定の動特性を有する仮想物体１２及び位置制御器１３を保持しており、制御対象に加える目標力及び力センサ１１の測定値を仮想物体１２に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、位置制御器１３に次の時刻における目標位置として与えたときに算出されるトルクを求め、当該トルクが予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及び前記トルクに基づいて代数ループを用いて、仮想物体１２の次の時刻における位置を定める。【選択図】図２
2019020826.tif 000002
【特許請求の範囲】
【請求項１】
制御対象の位置を検出する位置検出器と、
前記制御対象が物体と接触する力を検出する力センサと、
前記制御対象を移動可能に保持する移動機構と、
トルク指令値を入力として前記移動機構を動作させるモータを有する駆動装置と、
前記トルク指令値を決定する演算装置とを備え、
前記演算装置が、
所定の動特性を有する仮想物体、及び位置制御則を保持しており、前記制御対象に加える目標力、及び前記力センサの測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御則に次の時刻における目標位置として与えたときに算出されるトルクを求め、
前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、
前記トルクが予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及び前記トルクに基づいて、前記仮想物体の次の時刻における位置が定められることを特徴とする力制御装置。
【請求項２】
請求項１に記載の力制御装置において、
前記演算手段が、
前記トルクが予め設定された所定の範囲外にある場合に、当該所定の範囲の境界上にあって前記トルクに最も近いトルクをトルク指令値として定め、前記位置制御器が算出するトルク指令値が、定められた前記トルク指令値となる次の時刻における目標位置を算出し、当該目標位置を前記仮想物体の次の時刻の位置として定める力制御装置。
【請求項３】
請求項２に記載の力制御装置において、
前記演算手段が、連続時間において、前記トルクをτ、前記所定のトルクの範囲をＦとし、前記仮想物体の位置をｑｘとし、前記制御対象の実際の位置をｑｓ、前記目標力をｆｄ、前記力センサの測定値をｆとし、前記仮想物体の前記所定動特性がある関数Γｃによって
2019020826.tif 000038
と表されるとした場合に、
2019020826.tif 000039
を満たすようにｑｘが更新されてτが決定される力制御装置。（ただし、関数Φｃはｑｓが目標値ｑｘに追従するように定義された位置制御器とする。）
【請求項４】
請求項２に記載の力制御装置において、
前記演算手段が、離散時間中の時刻ｋにおいて、前記トルクをτ、前記所定のトルクの範囲をＦとし、前記仮想物体の位置をｑｘとし、前記制御対象の実際の位置をｑｓ、前記目標力をｆｄ、前記力センサの測定値をｆとし、前記仮想物体の前記所定動特性がある関数Γｃによって
2019020826.tif 000040
と表されるとした場合に、
2019020826.tif 000041
を満たすようにｑｘ（ｋ）及びτ（ｋ）が決定される力制御装置。（ただし、関数Φはｑｓが目標値ｑｘに追従するように定義された位置制御器とし、Ｓ（ｋ−１）は積分項、前ステップの測定位置ｐｓ（ｋ−１）、前ステップの目標位置ｐｄ（ｋ−１）などの制御器内に蓄えられた状態変数からなる集合であり、この関数の出力τ（ｋ）を前記トルク指令値とする。）
【請求項５】
請求項４に記載の力制御装置において、
前記関数Γが、
2019020826.tif 000042
の入出力関係を持つ関数で与えられる力制御装置（ここで、Ｔ、Ｍｖ及びＢｖは正の定数である）。
【請求項６】
請求項４に記載の力制御装置において、
前記関数Φが、
2019020826.tif 000043
のアルゴリズムと同一の入出力関係を持つ関数である力制御装置（ここで、Ａ＾、Ｂ、Ｌ、Ｍ及びＴは正の定数である）。
【請求項７】
請求項４に記載の力制御装置において、
ｑｘ（ｋ）及びτ（ｋ）を決定する計算が、
2019020826.tif 000044
のアルゴリズムと同一の入出力関係を持つ力制御装置。
【請求項８】
コンピュータが、
制御対象の位置を検出する位置検出ステップと、
前記制御対象が物体と接触する力を検出する力検出ステップと、
トルク指令値を入力として前記移動機構を動作させるモータを有する駆動装置の前記トルク指令値を演算する演算ステップと、
前記演算ステップの演算結果に基づいて前記制御対象を移動する移動ステップとを実行し、
前記演算ステップが、
前記コンピュータの内部に保持された所定の動特性を有する仮想物体、及び位置制御則を用い、前記制御対象に加える目標力、及び前記力検出ステップの測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御則に次の時刻における目標位置として与えたときに算出されるトルクを求め、
前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、
前記トルクが予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及び前記トルクに基づいて、前記仮想物体の次の時刻における位置を定めることを特徴とする力制御方法。
【請求項９】
制御対象の位置を検出する位置検出手段、
前記制御対象が物体と接触する力を検出する力検出手段、
トルク指令値を入力として前記移動機構を動作させるモータを有する駆動装置の前記トルク指令値を演算する演算手段、
前記演算手段の演算結果に基づいて前記制御対象の移動を制御する移動制御手段としてコンピュータを機能させ、
前記演算手段が、
前記コンピュータの内部に保持された所定の動特性を有する仮想物体、及び位置制御則を用い、前記制御対象に加える目標力、及び前記力検出ステップの測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御則に次の時刻における目標位置として与えたときに算出されるトルクを求め、
前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、
前記トルクが予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及び前記トルクに基づいて、前記仮想物体の次の時刻における位置を定めることを特徴とする力制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、力センサを用いてアドミッタンス制御を行う力制御装置等に関する。
【背景技術】
【０００２】
ロボット・マニピュレーターが外部的環境に接して動作している場合、ロボットのエンドエフェクタと環境の間の接触力を制御するための適切な制御器が必要である。この種の制御器は、非特許文献１及び２の中で提案されている。力センサが利用可能であり、ロボットのジョイントに高い摩擦がある状況では、アドミッタンス制御（非特許文献３）と呼ばれる制御手法がよく用いられている（非特許文献３〜５）。この種の制御器では、単純な動特性をもつ仮想物体を制御器の中で想定し、力センサで測定された力の情報によって仮想物体の運動をシミュレートする。また、ロボットは仮想物体の運動を追跡するように位置制御される。位置制御が十分に正確である限り、ロボットの外力に対する反応は仮想物体のそれに近い。この制御法は、力覚提示（非特許文献４，５）、遠隔操作（非特許文献６）、リハビリテーション（非特許文献７）、人間機械共同作業（非特許文献８）及びある種の製造工程（非特許文献９）に適用されている。ほとんどの応用例においては、ロボットのアクチュエータ力の大きさはハードウェア制限か安全上の理由により制限されている。
【０００３】
アドミッタンス制御でのアクチュエータ力飽和の１つの問題は、アクチュエータ力が飽和する場合に、ロボットの位置が仮想物体の位置から遠ざかってしまうということである。アクチュエータ力は飽和する可能性があり、仮想物体の速度に急激な変化が発生する可能性がある。大きな外力がロボットに働く可能性がある場合に、仮想物体の慣性と粘性を大きく設定することは、飽和を防ぐことに寄与する。しかし、それは接触力に対する鈍重な反応性に帰着し、多くの場合不適切である。
【０００４】
なお外力のフィードバックループ及び内部位置ループを使用する制御方法は、１９７０年代以来文献で見つけることができる。それらの制御器は明示的力制御器（非特許文献１１〜１３）及びインピーダンス制御器（非特許文献１４〜１７）に分類することができる。前者はエンドエフェクタ上の力を制御し、また、後者はエンドエフェクタの見かけの機械インピーダンスを制御する。この２つのタイプは、同様のフレームワーク（違いは目標力コマンドが提供されるかどうかである）の中で実現することができる。それらの制御器は、「仮想内部モデル追従制御」（非特許文献９，１８）、「位置制御ベース・インピーダンス制御」（非特許文献１９，２０）及び「アドミッタンス制御」（非特許文献４，５，７，８）のようなさまざまな名前で呼ばれている。
【０００５】
安定性はアドミッタンス制御器の主要な課題である。不安定性の主な原因は、制御器の中の時間遅れ（非特許文献１２，２３）、内部位置制御システムの帯域制限（非特許文献２４〜２６）である。仮想質量と仮想粘性は、不安定性を抑えるためには、高く設定すべきであるということが知られている。しかしながら、それが制御器の反応性の質を下げる場合があるので、大きな仮想質量か仮想粘性は必ずしも望ましくない。実現可能な最小の仮想質量はアドミッタンス制御される装置のひとつの評価指標でもある（非特許文献５）。仮想質量及び仮想粘性を増加させずに安定性を向上するために、仮想物体位置から実際の位置への伝達関数は、１に近い必要がある。これを実現するために、従来の技術のうちのいくつかは、ロボットの動特性（非特許文献２２，２７，２８の影響を償うためにフィードフォワード項（位相進み補償器とみなすことができる）を含む位置制御器を使用している。
【０００６】
アクチュエータ飽和に関して、どのようにそれを回避しなければならないかという問題についての唯一の解析的な研究はGonzalez及びWidmannの研究（非特許文献２９）である。この研究では、飽和がシステムを開ループ系と一時的に等価にするので、飽和が制御対象の安定性を改善することが指摘されている。しかしながら、この技術では、正の飽和と負の飽和が交互に発生する状況（これは通常望ましくない）を考慮していない。
【０００７】
このような問題を解決するために用いることができる制御技術が、非特許文献１０及び特許文献１によって提案された、Proxy-based  Sliding  Mode制御（ＰＳＭＣ）である。この技術は、単純なスライディングモード制御器のある種の離散時間近似でもあり、飽和のあるＰＩＤ制御器の拡張でもある。
【０００８】
ＰＳＭＣは、トルク不飽和時にはＰＩＤと等価であるため目標位置に対する現在位置追従特性が不十分であることもある。またＰＳＭＣの場合、トルク飽和に関しては、飽和から不飽和への遷移時にτが不連続になって、ロボットを触る作業者に、違和感を感じさせる原因となっている。このような問題に対して、位置指令が頻繁及び急激に変わるときでも位置の追従性を向上させることができ、しかもトルク飽和時でも不連続な動きを抑えることができる位置制御器を備えたアドミッタンス制御を用いた力制御装置に関する技術が開示されている（特許文献２）。
【先行技術文献】
【非特許文献】
【０００９】
【非特許文献１】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.
【非特許文献２】G. Zeng and A. Hemami, “An overview of robot force control,” Robotica, vol. 15, pp. 473-482, 1997.
【非特許文献３】C. Ott, R. Mukherjee, and Y. Nakamura, “Unified impedance and admittance control,” in Proc. IEEE Int. Conf. Robotics and Automation, 2010, pp. 554-561.
【非特許文献４】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.
【非特許文献５】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.
【非特許文献６】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.
【非特許文献７】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.
【非特許文献８】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.
【非特許文献９】H. Arai, “Force-controlled metal spinning machine using linear motors,” in Proc. IEEE Int. Conf. Robotics and Automation, 2006, pp. 4031-4036.
【非特許文献１０】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.
【非特許文献１１】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.
【非特許文献１２】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.
【非特許文献１３】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.
【非特許文献１４】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.
【非特許文献１５】G. Hirzinger, “Direct digital robot control using a force-torque sensor,” in Proc. IFAC Symp. Real Time Digital Control Applications, 1983, pp. 243-255.
【非特許文献１６】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.
【非特許文献１７】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.
【非特許文献１８】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.
【非特許文献１９】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.
【非特許文献２０】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.
【非特許文献２１】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.
【非特許文献２２】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.
【非特許文献２３】D. A. Lawrence, “Impedance control stability properties in common implementation,” in Proc. IEEE Int. Conf. Robotics and Automation, 1988, pp. 1185-1190.
【非特許文献２４】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.
【非特許文献２５】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.
【非特許文献２６】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.
【非特許文献２７】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.
【非特許文献２８】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.
【非特許文献２９】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.
【特許文献】
【００１０】
【特許文献１】特開２００７−１０２７４８号公報
【特許文献２】特許第６０３２８１１号
【発明の概要】
【発明が解決しようとする課題】
【００１１】
特許文献２に示す技術は、位置の追従性を向上させつつ、トルク飽和時でも不連続な動きを抑えることができるものの、仮想物体とロボットとの位置が離れることで、どうしても追従処理に違和感が生じてしまう可能性がある。
【００１２】
本発明は、トルク飽和した場合であっても、仮想物体の位置をフィードバックすることで、仮想物体と制御対象との位置ずれを低減し、追従性を向上させつつ安定性を向上させる力制御装置等を提供する。
【課題を解決するための手段】
【００１３】
本発明に係る力制御装置は、制御対象の位置を検出する位置検出器と、前記制御対象が物体と接触する力を検出する力センサと、前記制御対象を移動可能に保持する移動機構と、トルク指令値を入力として前記移動機構を動作させるモータを有する駆動装置と、前記トルク指令値を決定する演算装置とを備え、前記演算装置が、所定の動特性を有する仮想物体、及び位置制御則を保持しており、前記制御対象に加える目標力、及び前記力センサの測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御則に次の時刻における目標位置として与えたときに算出されるトルクを求め、前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、前記トルクが予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及び前記トルクに基づいて、前記仮想物体の次の時刻における位置が定められるものである。
【００１４】
このように、本発明に係る力制御装置においては、制御対象の位置を検出する位置検出器と、前記制御対象が物体と接触する力を検出する力センサと、前記制御対象を移動可能に保持する移動機構と、トルク指令値を入力として前記移動機構を動作させるモータを有する駆動装置と、前記トルク指令値を決定する演算装置とを備え、算出されたトルクが予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及び前記トルクに基づいて、前記仮想物体の次の時刻における位置が定められるため、制御対象と仮想物体との位置のずれが低減され、追従性を向上させつつ安定性を向上させることができるという効果を奏する。
【図面の簡単な説明】
【００１５】
【図１】従来の力制御装置の処理概念の一例を示す機能ブロック図である。
【図２】第１の実施形態に係る力制御装置の処理概念の一例を示す機能ブロック図である。
【図３】第１の実施形態に係る力制御装置のハードウェア構成図である。
【図４】第１の実施形態に係る力制御装置の機能ブロック図である。
【図５】第１の実施形態に係る力制御装置の動作を示すフローチャートである。
【図６】実施例において使用したロボットの写真である。
【図７】従来の手法を用いた場合のアドミッタンス制御の結果を示す図である。
【図８】本発明に係る力制御装置を用いた場合のアドミッタンス制御の結果を示す図である。
【発明を実施するための形態】
【００１６】
以下、本発明の実施の形態を説明する。また、本実施形態の全体を通して同じ要素には同じ符号を付けている。
【００１７】
（本発明の第１の実施形態）
本実施形態に係る力制御装置について、図１ないし図５を用いて説明する。図１は、従来の力制御装置の処理概念の一例を示す機能ブロック図であり、図２は、本実施形態に係る力制御装置の処理概念の一例を示す機能ブロック図である。図１に示すように、従来技術におけるアドミッタンス制御は、力センサ１１の測定値ｆと目標力（制御対象であるロボット１４に対して加える力）ｆｄとから所定の動特性を有する仮想物体１２の位置Ｐｄを目標位置として求める。目標位置Ｐｄを満たすように位置制御器１３がロボット１４のモータのトルク指令値τを算出する。外部からロボット１４に加えられる力ｈ、力センサ１１の測定値ｆ及びトルク指令値τに基づいて、ロボット１４が位置Ｐに移動する。
【００１８】
図１の従来技術の場合は、トルク指令値τがトルク範囲（例えば、安全性、ロボット１４の駆動特性、環境等に応じて予め設定されるトルクの上限値及び下限値）外となる場合は、仮想物体１２の位置Ｐｄとロボット１４の位置Ｐとが乖離してしまう。従来技術においては、この飽和状態から不飽和状態に遷移する必要があるため、どうしても追従処理に違和感が生じてしまう。そこで、本実施形態においては、例えば、図２に示すような処理を行う。
【００１９】
図２において、位置制御器１３で算出されたトルク指令値τについて、代数ループでフィードバックすることで、仮想物体１２の位置ｑｘとロボット１４の位置ｑｓとを一致させることができ、従来のような追従処理における違和感を排除することが可能となる。
【００２０】
以下、本実施形態に係る力制御装置の構成及びアルゴリズムについて、具体的に説明する。図３は、本実施形態に係る力制御装置のハードウェア構成図である。力制御装置１は、制御対象であるロボット１４と、当該ロボット１４に付設され当該ロボット１４の周囲環境にある物体からの反力を測定する力センサ１１と、アドミッタンス制御を行うためのプログラム等が格納されたＲＯＭ２３と、必要に応じて当該プログラムをＲＡＭ２２に読み込み処理を実行するＣＰＵ２１と、タッチパネル、キーボード、マウス等の入力機器からの入力を受け付けたり、プリンタやディスプレイ等にデータを出力するためのインタフェースである入出力Ｉ／Ｆ２４とを備える。各処理部はバスを介して接続され、情報のやり取りを行う。なお、上記ハードウェアの構成はあくまで一例であり、必要に応じて変更可能である。
【００２１】
図４は、本実施形態に係る力制御装置の機能ブロック図である。力制御装置１は、環境に対して所定の力を作用させるロボット１４と、当該ロボット１４の移動を制御する演算装置４０とを備える。ロボット１４は、周囲環境にある物体からの反力を測定する力センサ１１と、ロボット１４又は当該ロボット１４の一部（例えば、アーム部等）を移動する移動機構４１と、当該移動機構４１を駆動するためのモータからなる駆動装置４２とを備える。演算装置４０は、力センサ１１からの測定結果と移動機構４１の位置情報とに基づいて、駆動装置４２のモータのトルク指令値を演算し、駆動装置４２に出力する。演算装置４０によるトルク指令値の演算処理について、以下に詳細に説明する。
【００２２】
（１）数学的準備
Ｆを実数上の閉区間とする。それに対する飽和関数と法錐（Normal  Cone）は下記のように定義される。
【００２３】
【数１】
2019020826.tif 000003
【００２４】
飽和関数と法錐との関係は下記のように表される（参考文献：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)）。
【００２５】
【数２】
2019020826.tif 000004
【００２６】
符号関数を下記のように定義する。
【００２７】
【数３】
2019020826.tif 000005
【００２８】
正の定数Ｆで定義される閉区間［−Ｆ，Ｆ］について、下記が成立する。
【００２９】
【数４】
2019020826.tif 000006
【００３０】
符号関数と法錐は下記のように、互いに逆写像の関係にある。
【００３１】
【数５】
2019020826.tif 000007
【００３２】
また、符号関数と飽和関数の関係は下記のように表される。
【００３３】
【数６】
2019020826.tif 000008
【００３４】
（２）次に、提案する制御則の一般表現について説明する。
（２．１）連続時間表現
制御対象（ロボット１４）の実際の位置をｑｓ、仮想物体１２の位置をｑｘ、駆動装置４２における発生力をτ、力センサ１１の測定値をｆ、アドミッタンス制御器に与える目標力をｆｄとする（ｆｄ＝０で固定してもよい）。また、目標位置をｑｘとしたときのロボット１４の位置制御則が適切な関数Φｃによって
【００３５】
【数７】
2019020826.tif 000009
【００３６】
と与えられているものとする。さらに、仮想物体が実現するべき動特性は、適切な関数Гｃによって
【００３７】
【数８】
2019020826.tif 000010
【００３８】
と与えられているものとする。これらの関数の具体例は（３）節で後述する。駆動装置４の発生力τの許容範囲を集合Ｆと定めたとき、提案する制御器の連続時間表現は下記のように表される。
【００３９】
【数９】
2019020826.tif 000011
【００４０】
これはτ及びｑｘ（二階微分）を未知数とする連立微分方程式と見なすことができる。また、図２のような代数ループを含んだブロック線図として表すこともできる。式（１１ａ）の法錐Ｎｆ（・）は引数であるが、τがＦ外に存在することを許容しないため、上記を満たすためには、τがＦ内になければならない。そうなるようにｑｘが更新される（ｑｘ（二階微分）が決定される）というように解釈できる。式（１０）が満たされるようにｑｘ（二階微分）を定め、それを式（１１ｂ）に代入した際に算出される発生力τがＦ内にあれば、そのτの値はそのまま採用される。一方で、そのτがＦ外にあれば、τはＦの境界上のτに最も近い値で飽和し、そのときは式（１１ｂ）が満たされるようにｑｘ（二階微分）が定められることになる。
【００４１】
（２．２）離散時間表現（代数ループを含んだ表現）
制御器（１１）の離散時間表現を導く。
【００４２】
【数１０】
2019020826.tif 000012
【００４３】
のように離散化すると、式（１１ａ）は適切な関数Γを用いて、
【００４４】
【数１１】
2019020826.tif 000013
【００４５】
という形で書き直すことができる。また、式（１１ｂ）は適切な関数Φを用いて、
【００４６】
【数１２】
2019020826.tif 000014
【００４７】
という形で書き直すことができる。ここで、Ｓ（ｋ−１）は、例えば、積分項や前ステップの測定位置ｑｓ（ｋ−１）や前ステップの目標位置ｐｄ（ｋ−１）などからなる集合である。この関数の出力値τ（ｋ）をモータへのトルク指令値とする。さらに別の関数Φｓを用いて、
【００４８】
【数１３】
2019020826.tif 000015
【００４９】
のように次ステップのＳ（ｋ）を決定するものとする。このＳ（ｋ）は、次ステップの入力値の一つＳ（ｋ−１）として用いる。式（１２ａ）及び式（１２ｂ）は未知数τ（ｋ）とｑｘ（ｋ）についての連立方程式と見なすことができ、これも図２のようなブロック線図のように解釈することができる。これらを用いると、提案するアドミッタンス制御のアルゴリズムは下記のように表される。
【００５０】
【数１４】
2019020826.tif 000016
【００５１】
これらの表現は、代数包含式（１４ｂ）を解く工程を含んでいるため、このまま実装することは困難である。
【００５２】
（２．３）離散時間表現（アルゴリズムとしての表現）
代数包含式（１４ｂ）を解く方法を考える。ここで、以下のような仮定をおく。
仮定１  関数Φ（ｑｘ，ｑｓ，Ｓ）は引数ｑｘについて厳密に単調増加である。
多くの典型的な制御器はこの仮定を満たす。この仮定のもとで、以下の式が成り立つことは明らかである。
【００５３】
【数１５】
2019020826.tif 000017
【００５４】
また、この仮定のもとで、下記を満たす一価関数Φｉｎｖが存在することは明らかである。
【００５５】
【数１６】
2019020826.tif 000018
【００５６】
この関数Φｉｎｖは、いわば逆制御器とも呼ぶことができる。通常の制御器は目標位置に対してトルク指令値を与える。それに対し逆制御器は、指定されたトルク指令値を出力するのに必要な目標位置を算出する。式（１５）を用いると、式（１４ｂ）の代数包含式は下記のように書き直すことができる。
【数１７】
2019020826.tif 000019
【００５７】
ここでは、法錐ＮＦ（τ（ｋ））の返り値が（−∞，０]，０，[０，∞）の３通りしかあり得ないことを考慮している。式（８）より、上式は下記のようになる。
【００５８】
【数１８】
2019020826.tif 000020
【００５９】
ここで、式（２０）は、式（１６）で定義される逆制御器Φｉｎｖを用いると、
【００６０】
【数１９】
2019020826.tif 000021
【００６１】
と書き直すことができる。したがって、式（１４）のアルゴリズムは下記のように書き直すことができる。
【００６２】
【数２０】
2019020826.tif 000022
【００６３】
上式が、提案するアドミッタンス制御のアルゴリズムの一つの形態である。仮定１に追加して、以下の仮定２が満たされる場合には、アルゴリズムをさらに簡略化することができる。
仮定２  Φ（ｑｘ，ｑｓ，Ｓ）＝Ｋ＾（ｑｘ−Φｐ（ｑｓ，Ｓ））がすべてのｑｘ，ｑｓ，Ｓに対して満たされる正の定数Ｋ＾と関数Φｐ（ｑｓ，Ｓ）が存在する。
【００６４】
これは、制御器Φ（ｑｘ，ｑｓ，Ｓ）が目標値ｑｘに対して線形であることを意味し、多くの典型的な制御器Φ（ｑｘ，ｑｓ，Ｓ）はこの仮定を満たす。このとき、逆制御器関数は、
【００６５】
【数２１】
2019020826.tif 000023
【００６６】
となり、アルゴリズム（２２）は下記のようになる。
【００６７】
【数２２】
2019020826.tif 000024
【００６８】
アルゴリズム（２２）における式（２２ｄ）の演算が、アルゴリズム（２４）においては式（２４ｅ）のように単純化されている。このため、関数Φ（）の構造によっては、アルゴリズム（２４）のタイプの実装によって計算量が削減できる。
【００６９】
（３）次に、関数Φｃ及びГｃの実例について説明する。
（３．１）関数Φｃの例：目標加速度フィードフォワード付きＰＩＤ制御の場合
目標加速度フィードフォワード付きＰＩＤ制御の場合は、連続時間領域における制御器関数Φｃは下記のように与えられる。
【００７０】
【数２３】
2019020826.tif 000025
【００７１】
ここで、Ｍｑｘ（ｑｘは二階微分）が目標加速度のフィードフォワード項であり、Ｋ、Ｂ及びＬはそれぞれ比例、微分、積分ゲインである。このとき、下式
【００７２】
【数２４】
2019020826.tif 000026
【００７３】
を離散化すると、下記のような形になる。
【００７４】
【数２５】
2019020826.tif 000027
【００７５】
ここで、ａはａ＝∫（ｑｘ−ｑｓ）ｄｔに相当し、
【００７６】
【数２６】
2019020826.tif 000028
【００７７】
である。また、Ａ＾＝Ｋ＋Ｂ／Ｔ＋ＬＴであり、
【００７８】
【数２７】
2019020826.tif 000029
【００７９】
である。これらを考慮すると、上記の関数Φｃに対応する関数Φ，ΦＳ，Φｉｎｖは下記のようになる。
【００８０】
【数２８】
2019020826.tif 000030
【００８１】
また、上記の関数Φ（）は明らかに仮定２を満たすため、関数Φｐを下記のように定めることができる。
【００８２】
【数２９】
2019020826.tif 000031
【００８３】
したがって、この関数はアルゴリズム（２４）の形式で実装できる。実装した結果は以下のようになる。
【００８４】
【数３０】
2019020826.tif 000032
【００８５】
（３．２）関数Гｃの例：質量・ダンパ系の場合
仮想物体の動特性が単純な慣性・粘性系で与えられている場合、動特性は、
【００８６】
【数３１】
2019020826.tif 000033
【００８７】
となるので、関数Гｃは、
【００８８】
【数３２】
2019020826.tif 000034
【００８９】
となる。ここで式（３５）を離散化すると
【００９０】
【数３３】
2019020826.tif 000035
【００９１】
となる。これをｑｘ（ｋ）について解くと、
【００９２】
【数３４】
2019020826.tif 000036
【００９３】
となる。すなわち、関数Гは下記のようになる。
【００９４】
【数３５】
2019020826.tif 000037
【００９５】
次に、本実施形態に係る力制御装置の動作について説明する。図５は、本実施形態に係る力制御装置の動作を示すフローチャートである。まず、ロボット１４に対して外部から加わった力、及び力センサ１１の測定値に基づいて、演算装置４０がアドミッタンス制御の演算処理を行う（Ｓ１）。この演算処理は、上記で説明した通りである。演算の結果、ロボット１４を駆動する際のモータのトルク指令値が出力され、ロボット１４に入力される（Ｓ２）。トルク指令値にしたがってロボット１４が駆動する（Ｓ３）。ロボット１４の駆動を終了する場合は、処理を終了する。駆動（停止状態も含む）を継続する場合は、駆動した結果、ロボット１４の位置や力センサ１１の測定値を出力し、ステップＳ１のアドミッタンス制御の演算処理に戻る。以降、同様の処理を繰り返して行う。
【００９６】
このように、本実施形態に係る力制御装置においては、算出されたトルク指令値が予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及びトルク指令値に基づいて代数ループを用いて、仮想物体の次の時刻における位置が定められるため、制御対象と仮想物体との位置のずれが低減され、追従性を向上させつつ安定性を向上させることができる。
【実施例】
【００９７】
本発明に係る力制御装置について、以下の実験を行った。図６は、実験で使用したロボット１４の写真である。ロボット１４の第３関節に上記アドミッタンス制御則を実装し、その他の関節についてはロックした。そして、力センサ１１に接触して動作させた場合と、リンク部（力センサ１１以外の部位）に接触して動作させた場合の挙動を観察した。
【００９８】
図７は、従来の手法を用いた場合のアドミッタンス制御の結果を示す図であり、図８は、本発明に係る力制御装置を用いた場合のアドミッタンス制御の結果を示す図である。図７及び図８において、
（１）〜時刻Ａ：力センサ１１を手で持った状態で動作
（２）時刻Ｂ〜時刻Ｃ：力センサ１１外の場所に力を加えている状態
（３）時刻Ｃ〜：ロボット１４が動かないようにリンク部を手で固定したまま、力センサ１に力を加えている状態
のそれぞれにおける測定結果が示されている。グラフの一番上は、仮想物体１２の位置とロボット１４の位置とを示しており、真ん中は、力センサ１１の測定値を示しており、一番下は、モータの駆動トルクを示している。
【００９９】
図７及び図８からわかるように、（１）においては、いずれの手法においてもロボット１４の位置が仮想物体１２の位置によく追従している。（２）においては、図７の従来手法では、ロボット１４の位置と仮想物体１２の位置とが乖離しており、このとき、加えていた力を開放すると、バネのように元の位置に戻ろうとして危険である。一方、図８の本願の手法では、仮想物体１２の位置がロボット１４の位置に追従しているため、加えていた力を開放しても上記のような危険性は全くない。（３）においては、図７の従来手法では、仮想物体１２の位置だけが動き、手を離すとロボット１４が遠く離れた仮想物体１２の位置追従しようとして危険である。一方、図８の本願の手法では、仮想物体１２の位置は動かず、ロボット１４の位置も動かないため、上記のような危険性は全くない。
【０１００】
以上のように、トルク飽和が発生するような状況においては、従来手法に比べて、本願の手法の方が安全性が格段に向上していることが明らかとなった。
【符号の説明】
【０１０１】
１  力制御装置
１１  力センサ
１２  仮想物体
１３  位置制御器
１４  ロボット
２１  ＣＰＵ
２２  ＲＡＭ
２３  ＲＯＭ
２４  入出力Ｉ／Ｆ
４０  演算装置
４１  移動機構
４２  駆動装置
【図１】
2019020826.tif 000045
【図２】
2019020826.tif 000046
【図３】
2019020826.tif 000047
【図４】
2019020826.tif 000048
【図５】
2019020826.tif 000049
【図６】
2019020826.tif 000050
【図７】
2019020826.tif 000051
【図８】
2019020826.tif 000052