JP6934173B2 - 力制御装置、力制御方法及び力制御プログラム - Google Patents

力制御装置、力制御方法及び力制御プログラム Download PDF

Info

Publication number
JP6934173B2
JP6934173B2 JP2017136283A JP2017136283A JP6934173B2 JP 6934173 B2 JP6934173 B2 JP 6934173B2 JP 2017136283 A JP2017136283 A JP 2017136283A JP 2017136283 A JP2017136283 A JP 2017136283A JP 6934173 B2 JP6934173 B2 JP 6934173B2
Authority
JP
Japan
Prior art keywords
torque
force
command value
torque command
target
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2017136283A
Other languages
English (en)
Other versions
JP2019020826A (ja
Inventor
亮 菊植
亮 菊植
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Kyushu University NUC
Original Assignee
Kyushu University NUC
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Kyushu University NUC filed Critical Kyushu University NUC
Priority to JP2017136283A priority Critical patent/JP6934173B2/ja
Priority to US16/629,758 priority patent/US20200180150A1/en
Priority to PCT/JP2018/025338 priority patent/WO2019013067A1/ja
Priority to EP18832753.0A priority patent/EP3654119A4/en
Publication of JP2019020826A publication Critical patent/JP2019020826A/ja
Application granted granted Critical
Publication of JP6934173B2 publication Critical patent/JP6934173B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/085—Force or torque sensors
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/041—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a variable is automatically adjusted to optimise the performance
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39323—Force and motion control
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39332—Adaptive force control
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/42—Servomotor, servo controller kind till VSS
    • G05B2219/42326—Protection servo for saturation of amplifier
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • General Physics & Mathematics (AREA)
  • Evolutionary Computation (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Hardware Design (AREA)
  • General Engineering & Computer Science (AREA)
  • Geometry (AREA)
  • Human Computer Interaction (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

Description

本発明は、力センサを用いてアドミッタンス制御を行う力制御装置等に関する。
ロボット・マニピュレーターが外部的環境に接して動作している場合、ロボットのエンドエフェクタと環境の間の接触力を制御するための適切な制御器が必要である。この種の制御器は、非特許文献１及び２の中で提案されている。力センサが利用可能であり、ロボットのジョイントに高い摩擦がある状況では、アドミッタンス制御（非特許文献３）と呼ばれる制御手法がよく用いられている（非特許文献３〜５）。この種の制御器では、単純な動特性をもつ仮想物体を制御器の中で想定し、力センサで測定された力の情報によって仮想物体の運動をシミュレートする。また、ロボットは仮想物体の運動を追跡するように位置制御される。位置制御が十分に正確である限り、ロボットの外力に対する反応は仮想物体のそれに近い。この制御法は、力覚提示（非特許文献４，５）、遠隔操作（非特許文献６）、リハビリテーション（非特許文献７）、人間機械共同作業（非特許文献８）及びある種の製造工程（非特許文献９）に適用されている。ほとんどの応用例においては、ロボットのアクチュエータ力の大きさはハードウェア制限か安全上の理由により制限されている。
アドミッタンス制御でのアクチュエータ力飽和の１つの問題は、アクチュエータ力が飽和する場合に、ロボットの位置が仮想物体の位置から遠ざかってしまうということである。アクチュエータ力は飽和する可能性があり、仮想物体の速度に急激な変化が発生する可能性がある。大きな外力がロボットに働く可能性がある場合に、仮想物体の慣性と粘性を大きく設定することは、飽和を防ぐことに寄与する。しかし、それは接触力に対する鈍重な反応性に帰着し、多くの場合不適切である。
なお外力のフィードバックループ及び内部位置ループを使用する制御方法は、１９７０年代以来文献で見つけることができる。それらの制御器は明示的力制御器（非特許文献１１〜１３）及びインピーダンス制御器（非特許文献１４〜１７）に分類することができる。前者はエンドエフェクタ上の力を制御し、また、後者はエンドエフェクタの見かけの機械インピーダンスを制御する。この２つのタイプは、同様のフレームワーク（違いは目標力コマンドが提供されるかどうかである）の中で実現することができる。それらの制御器は、「仮想内部モデル追従制御」（非特許文献９，１８）、「位置制御ベース・インピーダンス制御」（非特許文献１９，２０）及び「アドミッタンス制御」（非特許文献４，５，７，８）のようなさまざまな名前で呼ばれている。
安定性はアドミッタンス制御器の主要な課題である。不安定性の主な原因は、制御器の中の時間遅れ（非特許文献１２，２３）、内部位置制御システムの帯域制限（非特許文献２４〜２６）である。仮想質量と仮想粘性は、不安定性を抑えるためには、高く設定すべきであるということが知られている。しかしながら、それが制御器の反応性の質を下げる場合があるので、大きな仮想質量か仮想粘性は必ずしも望ましくない。実現可能な最小の仮想質量はアドミッタンス制御される装置のひとつの評価指標でもある（非特許文献５）。仮想質量及び仮想粘性を増加させずに安定性を向上するために、仮想物体位置から実際の位置への伝達関数は、１に近い必要がある。これを実現するために、従来の技術のうちのいくつかは、ロボットの動特性（非特許文献２２，２７，２８の影響を償うためにフィードフォワード項（位相進み補償器とみなすことができる）を含む位置制御器を使用している。
アクチュエータ飽和に関して、どのようにそれを回避しなければならないかという問題についての唯一の解析的な研究は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）は下記のように定義される。
Figure 0006934173
飽和関数と法錐との関係は下記のように表される（参考文献：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)）。
Figure 0006934173
符号関数を下記のように定義する。
Figure 0006934173
正の定数Ｆで定義される閉区間［−Ｆ，Ｆ］について、下記が成立する。
Figure 0006934173
符号関数と法錐は下記のように、互いに逆写像の関係にある。
Figure 0006934173
また、符号関数と飽和関数の関係は下記のように表される。
Figure 0006934173
（２）次に、提案する制御則の一般表現について説明する。
（２．１）連続時間表現
制御対象（ロボット１４）の実際の位置をｑｓ、仮想物体１２の位置をｑｘ、駆動装置４２における発生力をτ、力センサ１１の測定値をｆ、アドミッタンス制御器に与える目標力をｆｄとする（ｆｄ＝０で固定してもよい）。また、目標位置をｑｘとしたときのロボット１４の位置制御則が適切な関数Φｃによって
Figure 0006934173
と与えられているものとする。さらに、仮想物体が実現するべき動特性は、適切な関数Гｃによって
Figure 0006934173
と与えられているものとする。これらの関数の具体例は（３）節で後述する。駆動装置４の発生力τの許容範囲を集合Ｆと定めたとき、提案する制御器の連続時間表現は下記のように表される。
Figure 0006934173
これはτ及びｑｘ（二階微分）を未知数とする連立微分方程式と見なすことができる。また、図２のような代数ループを含んだブロック線図として表すこともできる。式（１１ａ）の法錐Ｎｆ（・）は引数であるが、τがＦ外に存在することを許容しないため、上記を満たすためには、τがＦ内になければならない。そうなるようにｑｘが更新される（ｑｘ（二階微分）が決定される）というように解釈できる。式（１０）が満たされるようにｑｘ（二階微分）を定め、それを式（１１ｂ）に代入した際に算出される発生力τがＦ内にあれば、そのτの値はそのまま採用される。一方で、そのτがＦ外にあれば、τはＦの境界上のτに最も近い値で飽和し、そのときは式（１１ｂ）が満たされるようにｑｘ（二階微分）が定められることになる。
（２．２）離散時間表現（代数ループを含んだ表現）
制御器（１１）の離散時間表現を導く。
Figure 0006934173
のように離散化すると、式（１１ａ）は適切な関数Γを用いて、
Figure 0006934173
という形で書き直すことができる。また、式（１１ｂ）は適切な関数Φを用いて、
Figure 0006934173
という形で書き直すことができる。ここで、Ｓ（ｋ−１）は、例えば、積分項や前ステップの測定位置ｑｓ（ｋ−１）や前ステップの目標位置ｐｄ（ｋ−１）などからなる集合である。この関数の出力値τ（ｋ）をモータへのトルク指令値とする。さらに別の関数Φｓを用いて、
Figure 0006934173
のように次ステップのＳ（ｋ）を決定するものとする。このＳ（ｋ）は、次ステップの入力値の一つＳ（ｋ−１）として用いる。式（１２ａ）及び式（１２ｂ）は未知数τ（ｋ）とｑｘ（ｋ）についての連立方程式と見なすことができ、これも図２のようなブロック線図のように解釈することができる。これらを用いると、提案するアドミッタンス制御のアルゴリズムは下記のように表される。
Figure 0006934173
これらの表現は、代数包含式（１４ｂ）を解く工程を含んでいるため、このまま実装することは困難である。
（２．３）離散時間表現（アルゴリズムとしての表現）
代数包含式（１４ｂ）を解く方法を考える。ここで、以下のような仮定をおく。
仮定１ 関数Φ（ｑｘ，ｑｓ，Ｓ）は引数ｑｘについて厳密に単調増加である。
多くの典型的な制御器はこの仮定を満たす。この仮定のもとで、以下の式が成り立つことは明らかである。
Figure 0006934173
また、この仮定のもとで、下記を満たす一価関数Φｉｎｖが存在することは明らかである。
Figure 0006934173
この関数Φｉｎｖは、いわば逆制御器とも呼ぶことができる。通常の制御器は目標位置に対してトルク指令値を与える。それに対し逆制御器は、指定されたトルク指令値を出力するのに必要な目標位置を算出する。式（１５）を用いると、式（１４ｂ）の代数包含式は下記のように書き直すことができる。
Figure 0006934173
ここでは、法錐ＮＦ（τ（ｋ））の返り値が（−∞，０]，０，[０，∞）の３通りしかあり得ないことを考慮している。式（８）より、上式は下記のようになる。
Figure 0006934173
ここで、式（２０）は、式（１６）で定義される逆制御器Φｉｎｖを用いると、
Figure 0006934173
と書き直すことができる。したがって、式（１４）のアルゴリズムは下記のように書き直すことができる。
Figure 0006934173
上式が、提案するアドミッタンス制御のアルゴリズムの一つの形態である。仮定１に追加して、以下の仮定２が満たされる場合には、アルゴリズムをさらに簡略化することができる。
仮定２ Φ（ｑｘ，ｑｓ，Ｓ）＝Ｋ＾（ｑｘ−Φｐ（ｑｓ，Ｓ））がすべてのｑｘ，ｑｓ，Ｓに対して満たされる正の定数Ｋ＾と関数Φｐ（ｑｓ，Ｓ）が存在する。
これは、制御器Φ（ｑｘ，ｑｓ，Ｓ）が目標値ｑｘに対して線形であることを意味し、多くの典型的な制御器Φ（ｑｘ，ｑｓ，Ｓ）はこの仮定を満たす。このとき、逆制御器関数は、
Figure 0006934173
となり、アルゴリズム（２２）は下記のようになる。
Figure 0006934173
アルゴリズム（２２）における式（２２ｄ）の演算が、アルゴリズム（２４）においては式（２４ｅ）のように単純化されている。このため、関数Φ（）の構造によっては、アルゴリズム（２４）のタイプの実装によって計算量が削減できる。
（３）次に、関数Φｃ及びГｃの実例について説明する。
（３．１）関数Φｃの例：目標加速度フィードフォワード付きＰＩＤ制御の場合
目標加速度フィードフォワード付きＰＩＤ制御の場合は、連続時間領域における制御器関数Φｃは下記のように与えられる。
Figure 0006934173
ここで、Ｍｑｘ（ｑｘは二階微分）が目標加速度のフィードフォワード項であり、Ｋ、Ｂ及びＬはそれぞれ比例、微分、積分ゲインである。このとき、下式
Figure 0006934173
を離散化すると、下記のような形になる。
Figure 0006934173
ここで、ａはａ＝∫（ｑｘ−ｑｓ）ｄｔに相当し、
Figure 0006934173
である。また、Ａ＾＝Ｋ＋Ｂ／Ｔ＋ＬＴであり、
Figure 0006934173
である。これらを考慮すると、上記の関数Φｃに対応する関数Φ，ΦＳ，Φｉｎｖは下記のようになる。
Figure 0006934173
また、上記の関数Φ（）は明らかに仮定２を満たすため、関数Φｐを下記のように定めることができる。
Figure 0006934173
したがって、この関数はアルゴリズム（２４）の形式で実装できる。実装した結果は以下のようになる。
Figure 0006934173
（３．２）関数Гｃの例：質量・ダンパ系の場合
仮想物体の動特性が単純な慣性・粘性系で与えられている場合、動特性は、
Figure 0006934173
となるので、関数Гｃは、
Figure 0006934173
となる。ここで式（３５）を離散化すると
Figure 0006934173
となる。これをｑｘ（ｋ）について解くと、
Figure 0006934173
となる。すなわち、関数Гは下記のようになる。
Figure 0006934173
次に、本実施形態に係る力制御装置の動作について説明する。図５は、本実施形態に係る力制御装置の動作を示すフローチャートである。まず、ロボット１４に対して外部から加わった力、及び力センサ１１の測定値に基づいて、演算装置４０がアドミッタンス制御の演算処理を行う（Ｓ１）。この演算処理は、上記で説明した通りである。演算の結果、ロボット１４を駆動する際のモータのトルク指令値が出力され、ロボット１４に入力される（Ｓ２）。トルク指令値にしたがってロボット１４が駆動する（Ｓ３）。ロボット１４の駆動を終了する場合は、処理を終了する。駆動（停止状態も含む）を継続する場合は、駆動した結果、ロボット１４の位置や力センサ１１の測定値を出力し、ステップＳ１のアドミッタンス制御の演算処理に戻る。以降、同様の処理を繰り返して行う。
このように、本実施形態に係る力制御装置においては、算出されたトルク指令値が予め設定された所定の範囲外にある場合は、当該所定の範囲の境界値及びトルク指令値に基づいて代数ループを用いて、仮想物体の次の時刻における位置が定められるため、制御対象と仮想物体との位置のずれが低減され、追従性を向上させつつ安定性を向上させることができる。
本発明に係る力制御装置について、以下の実験を行った。図６は、実験で使用したロボット１４の写真である。ロボット１４の第３関節に上記アドミッタンス制御則を実装し、その他の関節についてはロックした。そして、力センサ１１に接触して動作させた場合と、リンク部（力センサ１１以外の部位）に接触して動作させた場合の挙動を観察した。
図７は、従来の手法を用いた場合のアドミッタンス制御の結果を示す図であり、図８は、本発明に係る力制御装置を用いた場合のアドミッタンス制御の結果を示す図である。図７及び図８において、
（１）〜時刻Ａ：力センサ１１を手で持った状態で動作
（２）時刻Ｂ〜時刻Ｃ：力センサ１１外の場所に力を加えている状態
（３）時刻Ｃ〜：ロボット１４が動かないようにリンク部を手で固定したまま、力センサ１に力を加えている状態
のそれぞれにおける測定結果が示されている。グラフの一番上は、仮想物体１２の位置とロボット１４の位置とを示しており、真ん中は、力センサ１１の測定値を示しており、一番下は、モータの駆動トルクを示している。
図７及び図８からわかるように、（１）においては、いずれの手法においてもロボット１４の位置が仮想物体１２の位置によく追従している。（２）においては、図７の従来手法では、ロボット１４の位置と仮想物体１２の位置とが乖離しており、このとき、加えていた力を開放すると、バネのように元の位置に戻ろうとして危険である。一方、図８の本願の手法では、仮想物体１２の位置がロボット１４の位置に追従しているため、加えていた力を開放しても上記のような危険性は全くない。（３）においては、図７の従来手法では、仮想物体１２の位置だけが動き、手を離すとロボット１４が遠く離れた仮想物体１２の位置追従しようとして危険である。一方、図８の本願の手法では、仮想物体１２の位置は動かず、ロボット１４の位置も動かないため、上記のような危険性は全くない。
以上のように、トルク飽和が発生するような状況においては、従来手法に比べて、本願の手法の方が安全性が格段に向上していることが明らかとなった。
１ 力制御装置
１１ 力センサ
１２ 仮想物体
１３ 位置制御器
１４ ロボット
２１ ＣＰＵ
２２ ＲＡＭ
２３ ＲＯＭ
２４ 入出力Ｉ／Ｆ
４０ 演算装置
４１ 移動機構
４２ 駆動装置

Claims (8)

  1. 制御対象の位置を検出する位置検出器と、
    前記制御対象が物体と接触する力を検出する力センサと、
    前記制御対象を移動可能に保持する移動機構と、
    トルク指令値を入力として前記移動機構を動作させるモータを有する駆動装置と、
    前記トルク指令値を決定する演算装置とを備え、
    前記演算装置が、
    所定の動特性を有する仮想物体、及び位置制御器を保持しており、前記制御対象に加える目標力、及び前記力センサの測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御器に次の時刻における目標位置として与えたときに算出されるトルクを求め、
    前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、
    前記トルクが予め設定された所定の範囲外にある場合は、前記所定の範囲の境界上にあって前記トルクに最も近いトルクをトルク指令値として定め、前記位置制御器が算出するトルク指令値が、定められた前記トルク指令値となる次の時刻における目標位置を算出し、当該目標位置を前記仮想物体の次の時刻の位置として定めるように、前記トルク指令値について代数ループでフィードバックすることを特徴とする力制御装置。
  2. 請求項１に記載の力制御装置において、
    前記演算装置が、連続時間において、前記トルクをτ、前記所定のトルクの範囲をＦとし、前記仮想物体の位置をｑ ｘ とし、前記制御対象の実際の位置をｑ ｓ 、前記目標力をｆ ｄ 、前記力センサの測定値をｆとし、前記仮想物体の前記所定動特性がある関数Γ ｃ によって
    Figure 0006934173
     と表されるとした場合に、
    Figure 0006934173
     を満たすようにｑ ｘ が更新されてτが決定される力制御装置。（ただし、関数Φ ｃ はｑ ｓ が目標値ｑ ｘ に追従するように定義された位置制御器とする。）
  3. 請求項１に記載の力制御装置において、
    前記演算装置が、離散時間中の時刻ｋにおいて、前記トルクをτ、前記所定のトルクの範囲をＦとし、前記仮想物体の位置をｑ ｘ とし、前記制御対象の実際の位置をｑ ｓ 、前記目標力をｆ ｄ 、前記力センサの測定値をｆとし、前記仮想物体の前記所定動特性がある関数Γによって
    Figure 0006934173
     と表されるとした場合に、
    Figure 0006934173
     を満たすようにｑ ｘ （ｋ）及びτ（ｋ）が決定される力制御装置。（ただし、関数Φはｑ ｓ が目標値ｑ ｘ に追従するように定義された位置制御器とし、Ｓ（ｋ−１）は積分項、前ステップの測定位置ｐ ｓ （ｋ−１）、前ステップの目標位置ｐ ｄ （ｋ−１）などの制御器内に蓄えられた状態変数からなる集合であり、この関数の出力τ（ｋ）を前記トルク指令値とする。）
  4. 請求項３に記載の力制御装置において、
    前記関数Γが、
    Figure 0006934173
     の入出力関係を持つ関数で与えられる力制御装置（ここで、Ｔ、Ｍ ｖ 及びＢ ｖ は正の定数である）。
  5. 請求項３に記載の力制御装置において、
    前記関数Φが、
    Figure 0006934173
     のアルゴリズムと同一の入出力関係を持つ関数である力制御装置（ここで、Ａ＾、Ｂ、Ｌ、Ｍ及びＴは正の定数である）。
  6. 請求項３に記載の力制御装置において、
    ｑ ｘ （ｋ）及びτ（ｋ）を決定する計算が、
    Figure 0006934173
     のアルゴリズムと同一の入出力関係を持つ力制御装置。
  7. コンピュータが、
    制御対象の位置を検出する位置検出ステップと、
    前記制御対象が物体と接触する力を検出する力検出ステップと、
    トルク指令値を入力として前記制御対象を移動可能に保持する移動機構を動作させるモータを有する駆動装置の前記トルク指令値を演算する演算ステップと、
    前記演算ステップの演算結果に基づいて前記制御対象を移動する移動ステップとを実行し、
    前記演算ステップが、
    前記コンピュータの内部に保持された所定の動特性を有する仮想物体、及び位置制御器を用い、前記制御対象に加える目標力、及び前記力検出ステップの測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御器に次の時刻における目標位置として与えたときに算出されるトルクを求め、
    前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、
    前記トルクが予め設定された所定の範囲外にある場合は、前記所定の範囲の境界上にあって前記トルクに最も近いトルクをトルク指令値として定め、前記位置制御器が算出するトルク指令値が、定められた前記トルク指令値となる次の時刻における目標位置を算出し、当該目標位置を前記仮想物体の次の時刻の位置として定めるように、前記トルク指令値について代数ループでフィードバックすることを特徴とする力制御方法。
  8. 制御対象の位置を検出する位置検出手段、
    前記制御対象が物体と接触する力を検出する力検出手段、
    トルク指令値を入力として前記制御対象を移動可能に保持する移動機構を動作させるモータを有する駆動装置の前記トルク指令値を演算する演算手段、
    前記演算手段の演算結果に基づいて前記制御対象の移動を制御する移動制御手段としてコンピュータを機能させ、
    前記演算手段が、
    前記コンピュータの内部に保持された所定の動特性を有する仮想物体、及び位置制御器を用い、前記制御対象に加える目標力、及び前記力検出手段の測定値を前記仮想物体に与えた場合の運動をシミュレートして算出される次の時刻の位置を算出し、当該位置を前記位置制御器に次の時刻における目標位置として与えたときに算出されるトルクを求め、
    前記トルクが予め設定された所定の範囲内にある場合は、前記トルクをトルク指令値として前記位置を前記仮想物体の次の時刻における位置と定め、
    前記トルクが予め設定された所定の範囲外にある場合は、前記所定の範囲の境界上にあって前記トルクに最も近いトルクをトルク指令値として定め、前記位置制御器が算出するトルク指令値が、定められた前記トルク指令値となる次の時刻における目標位置を算出し、当該目標位置を前記仮想物体の次の時刻の位置として定めるように、前記トルク指令値について代数ループでフィードバックすることを特徴とする力制御プログラム。

JP2017136283A 2017-07-12 2017-07-12 力制御装置、力制御方法及び力制御プログラム Active JP6934173B2 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
JP2017136283A JP6934173B2 (ja) 2017-07-12 2017-07-12 力制御装置、力制御方法及び力制御プログラム
US16/629,758 US20200180150A1 (en) 2017-07-12 2018-07-04 Force control device, force control method, and force control program
PCT/JP2018/025338 WO2019013067A1 (ja) 2017-07-12 2018-07-04 力制御装置、力制御方法及び力制御プログラム
EP18832753.0A EP3654119A4 (en) 2017-07-12 2018-07-04 FORCE CONTROL DEVICE, FORCE CONTROL PROCEDURE AND FORCE CONTROL PROGRAM

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2017136283A JP6934173B2 (ja) 2017-07-12 2017-07-12 力制御装置、力制御方法及び力制御プログラム

Publications (2)

Publication Number Publication Date
JP2019020826A JP2019020826A (ja) 2019-02-07
JP6934173B2 true JP6934173B2 (ja) 2021-09-15

Family

ID=65002553

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2017136283A Active JP6934173B2 (ja) 2017-07-12 2017-07-12 力制御装置、力制御方法及び力制御プログラム

Country Status (4)

Country Link
US (1) US20200180150A1 (ja)
EP (1) EP3654119A4 (ja)
JP (1) JP6934173B2 (ja)
WO (1) WO2019013067A1 (ja)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110045604B (zh) * 2019-02-27 2022-03-01 沈阳工业大学 音圈电机驱动洛伦兹力型fts重复滑模复合控制方法
JP7248307B2 (ja) * 2020-01-29 2023-03-29 株式会社人機一体 アドミタンス制御を適用した駆動ユニット
CN111897289B (zh) * 2020-08-05 2022-02-18 上海柏楚电子科技股份有限公司 电机驱动机构的力矩信息处理方法、装置、设备与介质
CN112891127B (zh) * 2021-01-14 2022-07-26 东南大学 一种基于自适应阻抗控制的镜像康复训练方法
CN112809687B (zh) * 2021-02-08 2022-04-12 上海电气集团股份有限公司 一种机器人控制器的仿真方法、装置及设备
CN113110051B (zh) * 2021-04-14 2022-03-04 南开大学 考虑误差约束的打磨机器人力/位混合控制方法及系统
CN114789444B (zh) * 2022-05-05 2022-12-16 山东省人工智能研究院 一种基于深度强化学习和阻抗控制的柔顺人机接触方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6032811B2 (ja) 1978-06-20 1985-07-30 東芝テック株式会社 重量測定装置
JP2770982B2 (ja) * 1989-05-25 1998-07-02 株式会社豊田中央研究所 マニピユレータの位置と力の協調制御装置
JP4055090B2 (ja) * 1997-07-08 2008-03-05 株式会社安川電機 ロボットの制御装置
JP2007102748A (ja) 2005-09-06 2007-04-19 Nagoya Institute Of Technology 位置制御装置、位置制御方法及び位置制御プログラム
JP2008073830A (ja) * 2006-09-25 2008-04-03 Fanuc Ltd ロボット制御装置
JP4319232B2 (ja) * 2007-09-12 2009-08-26 トヨタ自動車株式会社 パワーアシスト装置およびその制御方法
JP4997145B2 (ja) * 2008-02-29 2012-08-08 トヨタ自動車株式会社 パワーアシスト装置およびその制御方法
JP6032811B2 (ja) * 2013-02-18 2016-11-30 国立大学法人九州大学 アドミッタンス制御を用いた力制御装置及び位置制御装置
US9186795B1 (en) * 2013-06-24 2015-11-17 Redwood Robotics, Inc. Programming and execution of force-based tasks with torque-controlled robot arms

Also Published As

Publication number Publication date
EP3654119A1 (en) 2020-05-20
US20200180150A1 (en) 2020-06-11
JP2019020826A (ja) 2019-02-07
WO2019013067A1 (ja) 2019-01-17
EP3654119A4 (en) 2021-04-07

Similar Documents

Publication Publication Date Title
JP6934173B2 (ja) 力制御装置、力制御方法及び力制御プログラム
Nemec et al. Human robot cooperation with compliance adaptation along the motion trajectory
US10751874B2 (en) Method of teaching robot and robotic arm control device
Sharifi et al. Cooperative modalities in robotic tele-rehabilitation using nonlinear bilateral impedance control
JP6032811B2 (ja) アドミッタンス制御を用いた力制御装置及び位置制御装置
KR102003216B1 (ko) 로봇을 위한 모터 제어 및/또는 조정
Kikuuwe A sliding-mode-like position controller for admittance control with bounded actuator force
JP6044511B2 (ja) ロボットの制御方法及びロボットシステム
JP7216405B2 (ja) 制御装置
JP7459372B2 (ja) 接触相互作用軌跡を計画するための装置および方法
Li et al. A nonlinear momentum observer for sensorless robot collision detection under model uncertainties
Saków et al. Nonlinear inverse modeling with signal prediction in bilateral teleoperation with force-feedback
Žlajpah et al. Unified virtual guides framework for path tracking tasks
Budolak et al. Series elastic actuation for improved transparency in time delayed haptic teleoperation
Zhang et al. Time delay compensation of a robotic arm based on multiple sensors for indirect teaching
Mokogwu et al. A hybrid position–rate teleoperation system
Saków et al. Sensorless control scheme for teleoperation with force-feedback, based on a hydraulic servo-mechanism, theory and experiment
JP2008217260A (ja) 力覚提示装置
Doulgeri et al. A position/force control for a robot finger with soft tip and uncertain kinematics
JP4587052B2 (ja) 位置制御装置、位置制御方法及び位置制御プログラム
Ducaju et al. Joint stiction avoidance with null-space motion in real-time model predictive control for redundant collaborative robots
Smisek et al. Naturally-transitioning rate-to-force controller robust to time delay by model-mediated teleoperation
JP2004344998A (ja) ロボット制御装置
Gu et al. Adaptive robust tracking control for hybrid models of three-dimensional bipedal robotic walking under uncertainties
Tung et al. Application of MRAC theory for adaptive control of a constrained robot manipulator

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20200403

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20210302

A601 Written request for extension of time

Free format text: JAPANESE INTERMEDIATE CODE: A601

Effective date: 20210426

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20210618

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20210720

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20210806

R150 Certificate of patent or registration of utility model

Ref document number: 6934173

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150