JP7260406B2 - Link mechanism control device and control method - Google Patents
Link mechanism control device and control method Download PDFInfo
- Publication number
- JP7260406B2 JP7260406B2 JP2019109024A JP2019109024A JP7260406B2 JP 7260406 B2 JP7260406 B2 JP 7260406B2 JP 2019109024 A JP2019109024 A JP 2019109024A JP 2019109024 A JP2019109024 A JP 2019109024A JP 7260406 B2 JP7260406 B2 JP 7260406B2
- Authority
- JP
- Japan
- Prior art keywords
- joint
- actuator
- speed
- virtual
- link
- 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
Links
Images
Landscapes
- Manipulator (AREA)
Description
本発明は、アクチュエータによりリンクを駆動するリンク機構の制御装置などに関する。 The present invention relates to a control device for a link mechanism that drives a link by an actuator.
従来、リンク機構の制御装置として特許文献１に記載されたものが知られている。このリンク機構は、２関節リンク機構タイプのものであり、ベースと、第１及び第２リンクと、第１及び第２関節を備えている。このリンク機構では、ベース及び第１リンクが第１関節を介して接続され、第１リンク及び第２リンクが第２関節を介して接続されている。
2. Description of the Related Art Conventionally, the one described in
また、第１関節は、能動関節タイプのものであり、アクチュエータとしての第１モータを備えている。さらに、第２関節も、能動関節タイプのものであり、アクチュエータとしての第２モータを備えている。この制御装置では、第１モータを制御することによって、第１関節の角度が制御され、第２モータを制御することによって、第２関節の角度が制御される。 Also, the first joint is of an active joint type and has a first motor as an actuator. Furthermore, the second joint is also of the active joint type and has a second motor as an actuator. In this control device, the angle of the first joint is controlled by controlling the first motor, and the angle of the second joint is controlled by controlling the second motor.
上記リンク機構を産業ロボットなどに適用した場合、重量物を第２リンクの先端部で取り扱うことがある。その場合、手先速度を落とすことなく、重量物を保持するためには、アクチュエータとして高出力タイプのモータが必要となる。その結果、アクチュエータの消費エネルギが増大し、製造コスト及びランニングコストの増大を招いてしまう。この問題を解消するために、アクチュエータの動力を関節間で伝達するように構成することが考えられる。そのように構成した場合、アクチュエータの消費エネルギを低減することができるものの、動力の伝達状態に応じて、リンクの速度を適切に制御する必要が生じる。 When the link mechanism is applied to an industrial robot or the like, a heavy object may be handled by the tip of the second link. In that case, a high output type motor is required as an actuator in order to hold a heavy object without slowing down the hand speed. As a result, the energy consumption of the actuator increases, leading to increased manufacturing costs and running costs. In order to solve this problem, it is conceivable to configure the actuator so that the power of the actuator is transmitted between the joints. In such a configuration, although the energy consumption of the actuator can be reduced, it becomes necessary to appropriately control the speed of the link according to the power transmission state.
本発明は、上記課題を解決するためになされたもので、関節間での動力の伝達状態に応じて、リンクの速度を適切に制御することができるリンク機構の制御装置などを提供することを目的とする。 SUMMARY OF THE INVENTION It is an object of the present invention to provide a link mechanism control device capable of appropriately controlling the speed of a link in accordance with the state of power transmission between joints. aim.
上記目的を達成するために、請求項１に係る発明は、第１関節１０及び第２関節２０にそれぞれ連結された第１リンク３０及び第２リンク５０が、第１アクチュエータ（第１モータ１１）及び第２アクチュエータ（第２モータ２１）によってそれぞれ駆動され、第１関節１０及び第２関節２０の間が、動力伝達機構４０によって、第１アクチュエータに起因する動力が第１関節１０から第２関節２０に伝達可能になる第１状態、及び第２アクチュエータに起因する動力が第２関節２０から第１関節１０に伝達可能になる第２状態の少なくとも一方の状態が成立するように、第１関節１０及び第２関節２０の間を連結する連結状態と、連結を解除する連結解除状態とに切り換えられ、第１関節１０及び第２関節２０の間が連結状態にある場合、第１関節１０の速度である第１速度と第２関節２０の速度である第２速度が比例するように、第１関節１０及び第２関節２０が作動するリンク機構１の制御装置６０であって、第１関節１０は、回転軸線周りに回転可能に構成され、第２関節２０は、回転軸線周りに回転可能に構成され、第１アクチュエータは、第１リンク３０を第１関節１０の回転軸線周りに回転駆動し、第２アクチュエータは、第２リンク５０を第２関節２０の回転軸線周りに回転駆動し、第１速度は、第１関節１０の回転速度であり、第２速度は、第２関節２０の回転速度であり、第１関節１０及び第２関節２０の間が連結状態と連結解除状態との間で切り換わるように、動力伝達機構４０を制御する機構制御部（コントローラ７０、ＳＴＥＰ１）と、第１関節１０及び第２関節２０の間が連結解除状態にある場合のリンク機構１の動特性を、動力伝達機構４０が存在しないと仮定した条件で定義した仮想モデル［式（１６）］と、動力伝達機構４０が存在する実際の条件で定義した実モデル［式（１０）～（１５）］とに基づき、モデル化誤差（推定誤差ベクトルτｆ＿ｈａｔ）を推定するモデル化誤差推定部（コントローラ７０、外乱オブザーバ８２、ＳＴＥＰ５）と、第１関節１０及び第２関節２０の間が連結解除状態にある場合、モデル化誤差を用いて、第１アクチュエータ及び第２アクチュエータを制御するアクチュエータ制御部（コントローラ７０、ＳＴＥＰ６，８）と、を備えることを特徴とする。
In order to achieve the above object, the invention according to
このリンク機構の制御装置によれば、第１関節及び第２関節の間が連結状態と連結解除状態との間で切り換わるように、動力伝達機構が制御され、第１関節及び第２関節の間が連結解除状態にある場合、モデル化誤差を用いて、第１アクチュエータ及び第２アクチュエータが制御される。このモデル化誤差は、第１関節及び第２関節の間が連結解除状態にある場合のリンク機構の動特性を、動力伝達機構が存在しないと仮定した条件で定義した仮想モデルと、動力伝達機構が存在する実際の条件で定義した実モデルとに基づいて推定される。 According to this link mechanism control device, the power transmission mechanism is controlled such that the first joint and the second joint are switched between the connected state and the disconnected state. The modeling error is used to control the first actuator and the second actuator when they are in a decoupled state. This modeling error is a virtual model that defines the dynamic characteristics of the link mechanism when the first joint and the second joint are in a disconnected state under the condition that the power transmission mechanism does not exist, and the power transmission mechanism is estimated based on a real model defined in real conditions where
したがって、第１関節及び第２関節の間が連結解除状態にある条件下での、動力伝達機構が実際に存在することに起因するモデル化誤差を適切に補償しながら、第１アクチュエータ及び第２アクチュエータを制御することができる。それにより、制御精度を向上させることができる。 Therefore, the first actuator and the second joint are properly compensated for the modeling error due to the actual presence of the power transmission mechanism under the condition that the connection between the first joint and the second joint is disconnected. Actuators can be controlled. Thereby, control accuracy can be improved.
請求項２に係る発明は、請求項１に記載のリンク機構１の制御装置６０において、第１関節１０及び第２関節２０の間が連結状態にある場合、第１関節１０及び第２関節２０を１つの仮想関節と見なしたときの仮想関節の速度である仮想速度（仮想角速度ωｚ）を、第１速度及び第２速度を２つの独立変数とする２変数１次式［式（２３）］によって算出する仮想速度算出部（コントローラ７０、仮想角速度算出部９２、ＳＴＥＰ２７）をさらに備え、アクチュエータ制御部は、第１関節１０及び第２関節２０の間が連結状態にある場合、仮想速度を用いて、第１アクチュエータ及び第２アクチュエータを制御する（ＳＴＥＰ７，８）ことを特徴とする。
According to a second aspect of the invention, in the
このリンク機構の制御装置によれば、第１関節及び第２関節の間が連結状態にある場合、第１関節及び第２関節を１つの仮想関節と見なしたときの仮想関節の速度である仮想速度が、第１速度及び第２速度を２つの独立変数とする２変数１次式によって算出され、仮想速度を用いて、第１アクチュエータ及び第２アクチュエータが制御される。 According to this link mechanism control device, when the first joint and the second joint are in a connected state, the speed of the virtual joint when the first joint and the second joint are regarded as one virtual joint A virtual velocity is calculated by a two-variable linear equation with a first velocity and a second velocity as two independent variables, and the virtual velocity is used to control the first actuator and the second actuator.
この場合、第１関節及び第２関節の間が連結状態にあるときには、両者の速度が比例するように、第１関節及び第２関節が作動するので、第１関節及び第２関節を１つの仮想関節と見なすことが可能となる。したがって、そのような仮想関節の速度である仮想速度を、第１速度及び第２速度を２つの独立変数とする２変数１次式によって算出することが可能になる。さらに、そのような仮想速度を用いて、第１アクチュエータ及び第２アクチュエータを制御することができるので、第１アクチュエータの出力と第２アクチュエータの出力とが互いに干渉し合うのを回避しながら、２つのアクチュエータを制御することができる。 In this case, when the first joint and the second joint are in a connected state, the first joint and the second joint operate so that their velocities are proportional. It can be regarded as a virtual joint. Therefore, the virtual velocity, which is the velocity of such a virtual joint, can be calculated by a two-variable linear expression with the first velocity and the second velocity as two independent variables. Further, such virtual velocities can be used to control the first and second actuators so that the outputs of the first and second actuators are avoided from interfering with each other while avoiding interference with each other. can control one actuator.
前述した目的を達成するために、請求項３に係る発明は、第１関節１０及び第２関節２０にそれぞれ連結された第１リンク３０及び第２リンク５０が、第１アクチュエータ及び第２アクチュエータによってそれぞれ駆動され、第１関節１０及び第２関節２０の間が、動力伝達機構４０によって、第１アクチュエータに起因する動力が第１関節１０から第２関節２０に伝達可能になる第１状態、及び第２アクチュエータに起因する動力が第２関節２０から第１関節１０に伝達可能になる第２状態の少なくとも一方の状態が成立するように、第１関節１０及び第２関節２０の間を連結する連結状態と、連結を解除する連結解除状態とに切り換えられ、第１関節１０及び第２関節２０の間が連結状態にある場合、第１関節１０の速度である第１速度と第２関節２０の速度である第２速度が比例するように、第１関節１０及び第２関節２０が作動するリンク機構１の制御装置６０であって、第１関節１０は、回転軸線周りに回転可能に構成され、第２関節２０は、回転軸線周りに回転可能に構成され、第１アクチュエータは、第１リンク３０を第１関節１０の回転軸線周りに回転駆動し、第２アクチュエータは、第２リンク５０を第２関節２０の回転軸線周りに回転駆動し、第１速度は、第１関節１０の回転速度であり、第２速度は、第２関節２０の回転速度であり、第１関節１０及び第２関節２０の間が連結状態と連結解除状態との間で切り換わるように、動力伝達機構４０を制御する機構制御部（コントローラ７０、ＳＴＥＰ１）と、第１関節１０及び第２関節２０の間が連結状態にある場合、第１関節１０及び第２関節２０を１つの仮想関節と見なしたときの仮想関節の速度である仮想速度（仮想角速度ωｚ）を、第１速度及び第２速度を２つの独立変数とする２変数１次式［式（２３）］によって算出する仮想速度算出部（コントローラ７０、仮想角速度算出部９２、ＳＴＥＰ２７）と、第１関節１０及び第２関節２０の間が連結状態にある場合、仮想速度を用いて、第１アクチュエータ及び第２アクチュエータを制御するアクチュエータ制御部（コントローラ７０、ＳＴＥＰ７，８）と、を備えることを特徴とする。
In order to achieve the object described above, the invention according to
このリンク機構の制御装置によれば、第１関節及び第２関節の間が連結状態と連結解除状態との間で切り換わるように、動力伝達機構が制御される。また、第１関節及び第２関節の間が連結状態にある場合、第１関節及び第２関節を１つの仮想関節と見なしたときの仮想関節の速度である仮想速度が、第１速度及び第２速度を２つの独立変数とする２変数１次式によって算出される。そして、仮想速度を用いて、第１アクチュエータ及び第２アクチュエータが制御されるので、前述したように、第１アクチュエータの出力と第２アクチュエータの出力とが互いに干渉し合うのを回避しながら、２つのアクチュエータを制御することができる。 According to this link mechanism control device, the power transmission mechanism is controlled such that the first joint and the second joint are switched between the connected state and the disconnected state. Further, when the first joint and the second joint are in a connected state, the virtual speed, which is the speed of the virtual joint when the first joint and the second joint are regarded as one virtual joint, is the first speed and It is calculated by a two-variable linear expression with the second speed as two independent variables. Then, since the first actuator and the second actuator are controlled using the virtual velocity, as described above, the output of the first actuator and the output of the second actuator are prevented from interfering with each other. can control one actuator.
請求項４に係る発明は、請求項２又は３に記載のリンク機構１の制御装置６０において、２変数１次式では、第１速度の乗算係数である第１乗算係数（α／ｐ）が、所定値α（０≦α≦１）及び値１－αの一方を含み、第２速度の乗算係数である第２乗算係数（１－α）が、所定値α及び値１－αの他方を含むとともに、第１乗算係数及び第２乗算係数の一方が第１速度及び第２速度の間の比例関係を表す比例パラメータ（比例係数ｐ）を含むように構成されていることを特徴とする。
The invention according to claim 4 is the
このリンク機構の制御装置によれば、第１速度の乗算係数である第１乗算係数が、所定値α（０≦α≦１）及び値１－αの一方を含み、第２速度の乗算係数である第２乗算係数が、所定値α及び値１－αの他方を含んでいるので、この所定値αの値を変更することにより、第１速度及び第２速度の重み付けを変更しながら、仮想速度を算出することができる。これに加えて、第１乗算係数及び第２乗算係数の一方が第１速度及び第２速度の間の比例関係を表す比例パラメータを含むように構成されているので、第１速度及び第２速度の比例関係を適切に反映させながら、仮想速度を算出することができる。
According to this link mechanism control device, the first multiplication factor, which is the multiplication factor for the first speed, includes one of the predetermined value α (0≦α≦1) and the
請求項５に係る発明は、請求項２ないし４のいずれかに記載のリンク機構１の制御装置６０において、仮想速度（仮想角速度ωｚ）の目標となる仮想目標速度（仮想目標角速度ωｚ＿ｒｅｆ）を算出する仮想目標速度算出部（コントローラ７０、仮想目標角速度算出部９３、ＳＴＥＰ２８）をさらに備え、アクチュエータ制御部は、仮想速度が仮想目標速度になるように、所定の制御アルゴリズム［式（２５）］により、仮想目標出力（仮想目標トルクτｚ＿ｒｅｆ）を算出し、第１乗算係数（β／ｐ）が所定値β（０≦β≦１）及び値１－βの一方を含み、第２乗算係数が所定値β及び値１－βの他方を含むとともに、第１乗算係数及び第２乗算係数の一方が第１速度及び第２速度の間の比例関係を表す比例パラメータ（比例係数ｐ）を含むように構成し、第１アクチュエータの目標出力（第１目標トルクτ１＿ｒｅｆ）及び第２アクチュエータの目標出力（第２目標トルクτ２＿ｒｅｆ）をそれぞれ、仮想目標出力に対して第１乗算係数及び第２乗算係数を乗算することにより算出するとともに、第１アクチュエータの出力（第１実トルクτ１）及び第２アクチュエータの出力（第２実トルクτ２）がそれぞれ第１アクチュエータの目標出力及び第２アクチュエータの目標出力になるように、第１アクチュエータ及び第２アクチュエータを制御する（ＳＴＥＰ７，８）ことを特徴とする。
According to a fifth aspect of the invention, in the
このリンク機構の制御装置によれば、仮想速度が仮想目標速度になるように、所定の制御アルゴリズムにより、仮想目標出力が算出されるので、仮想目標出力は、フィードフォワード制御項又はフィードバック制御項を含むように算出されることになる。さらに、そのような仮想目標出力に第１乗算係数及び第２乗算係数をそれぞれ乗算することにより、第１アクチュエータの目標出力及び第２アクチュエータの目標出力が算出されるので、これらの目標出力は、仮想速度を仮想目標速度にするための、フィードフォワード制御項又はフィードバック制御項の成分を含むことになる。そして、第１アクチュエータの出力及び第２アクチュエータの出力がそれぞれ第１アクチュエータの目標出力及び第２アクチュエータの目標出力になるように、第１アクチュエータ及び第２アクチュエータが制御される。したがって、仮想速度を仮想目標速度にするための制御入力成分を分配しながら、第１アクチュエータ及び第２アクチュエータを制御することができる。 According to this link mechanism control device, the virtual target output is calculated by a predetermined control algorithm so that the virtual speed becomes the virtual target speed. will be calculated to include Furthermore, by multiplying such a virtual target output by the first multiplication factor and the second multiplication factor, respectively, the target output of the first actuator and the target output of the second actuator are calculated. It will include a component of a feedforward control term or a feedback control term to bring the virtual velocity to the virtual target velocity. Then, the first actuator and the second actuator are controlled such that the output of the first actuator and the output of the second actuator become the target output of the first actuator and the target output of the second actuator, respectively. Therefore, the first actuator and the second actuator can be controlled while distributing the control input component for setting the virtual velocity to the virtual target velocity.
また、第１乗算係数が、所定値β（０≦β≦１）及び値１－βの一方を含み、第２乗算係数が所定値β及び値１－βの他方を含むとともに、第１乗算係数及び第２乗算係数の一方が第１速度及び第２速度の間の比例関係を表す比例パラメータを含むように構成されているので、この所定値βの値を変更することにより、第１速度及び第２速度の比例関係を適切に反映させながら、２つの目標出力への制御入力成分の分配割合を変更することができる。 Further, the first multiplication coefficient includes one of the predetermined value β (0≤β≤1) and the value 1-β, the second multiplication coefficient includes the other of the predetermined value β and the value 1-β, and the first multiplication Since one of the coefficient and the second multiplication coefficient is configured to include a proportional parameter representing the proportional relationship between the first speed and the second speed, by changing the value of this predetermined value β, the first speed and the second speed, the distribution ratio of the control input component to the two target outputs can be changed.
前述した目的を達成するために、請求項６に係る発明は、第１関節１０及び第２関節２０にそれぞれ連結された第１リンク３０及び第２リンク５０が、第１アクチュエータ及び第２アクチュエータによってそれぞれ駆動され、第１関節１０及び第２関節２０の間が、動力伝達機構４０によって、第１アクチュエータに起因する動力が第１関節１０から第２関節２０に伝達可能になる第１状態、及び第２アクチュエータに起因する動力が第２関節２０から第１関節１０に伝達可能になる第２状態の少なくとも一方の状態が成立するように、第１関節１０及び第２関節２０の間を連結する連結状態と、連結を解除する連結解除状態とに切り換えられ、第１関節１０及び第２関節２０の間が連結状態にある場合、第１関節１０の速度である第１速度と第２関節２０の速度である第２速度が比例するように、第１関節１０及び第２関節２０が作動するリンク機構１の制御方法であって、第１関節１０は、回転軸線周りに回転可能に構成され、第２関節２０は、回転軸線周りに回転可能に構成され、第１アクチュエータは、第１リンク３０を第１関節１０の回転軸線周りに回転駆動し、第２アクチュエータは、第２リンク５０を第２関節２０の回転軸線周りに回転駆動し、第１速度は、第１関節１０の回転速度であり、第２速度は、第２関節２０の回転速度であり、第１関節１０及び第２関節２０の間が連結状態と連結解除状態との間で切り換わるように、動力伝達機構４０を制御し（ＳＴＥＰ１）、第１関節１０及び第２関節２０の間が連結解除状態にある場合のリンク機構１の動特性を、動力伝達機構４０が存在しないと仮定した条件で定義した仮想モデル［式（１６）］と、動力伝達機構４０が存在する実際の条件で定義した実モデル［式（１０）～（１５）］とに基づき、モデル化誤差（推定誤差ベクトルτｆ＿ｈａｔ）を推定し（ＳＴＥＰ５）、第１関節１０及び第２関節２０の間が連結解除状態にある場合、モデル化誤差を用いて、第１アクチュエータ及び第２アクチュエータを制御する（ＳＴＥＰ６，７）ことを特徴とする。
In order to achieve the object described above, the invention according to claim 6 provides a
請求項７に係る発明は、請求項６に記載のリンク機構１の制御方法において、第１関節１０及び第２関節２０の間が連結状態にある場合、第１関節１０及び第２関節２０を１つの仮想関節と見なしたときの仮想関節の速度である仮想速度（仮想角速度ωｚ）を、第１速度及び第２速度を２つの独立変数とする２変数１次式［式（２３）］によって算出し（ＳＴＥＰ２７）、第１関節１０及び第２関節２０の間が連結状態にある場合、仮想速度を用いて、第１アクチュエータ及び第２アクチュエータを制御する（ＳＴＥＰ７，８）ことを特徴とする。
The invention according to claim 7 is the method for controlling the
前述した目的を達成するために、請求項８に係る発明は、第１関節１０及び第２関節２０にそれぞれ連結された第１リンク３０及び第２リンク５０が、第１アクチュエータ（第１モータ１１）及び第２アクチュエータ（第２モータ２１）によってそれぞれ駆動され、第１関節１０及び第２関節２０の間が、動力伝達機構４０によって、第１アクチュエータに起因する動力が第１関節１０から第２関節２０に伝達可能になる第１状態、及び第２アクチュエータに起因する動力が第２関節２０から第１関節１０に伝達可能になる第２状態の少なくとも一方の状態が成立するように、第１関節１０及び第２関節２０の間を連結する連結状態と、連結を解除する連結解除状態とに切り換えられ、第１関節１０及び第２関節２０の間が連結状態にある場合、第１関節１０の速度である第１速度と第２関節２０の速度である第２速度が比例するように、第１関節１０及び第２関節２０が作動するリンク機構１の制御方法であって、第１関節１０は、回転軸線周りに回転可能に構成され、第２関節２０は、回転軸線周りに回転可能に構成され、第１アクチュエータは、第１リンク３０を第１関節１０の回転軸線周りに回転駆動し、第２アクチュエータは、第２リンク５０を第２関節２０の回転軸線周りに回転駆動し、第１速度は、第１関節１０の回転速度であり、第２速度は、第２関節２０の回転速度であり、第１関節１０及び第２関節２０の間が連結状態と連結解除状態との間で切り換わるように、動力伝達機構４０を制御し（ＳＴＥＰ１）、第１関節１０及び第２関節２０の間が連結状態にある場合、第１関節１０及び第２関節２０を１つの仮想関節と見なしたときの仮想関節の速度である仮想速度（仮想角速度ωｚ）を、第１速度及び第２速度を２つの独立変数とする２変数１次式［式（２３）］によって算出し（ＳＴＥＰ２７）、第１関節１０及び第２関節２０の間が連結状態にある場合、仮想速度を用いて、第１アクチュエータ及び第２アクチュエータを制御することを特徴とする。
In order to achieve the object described above, the invention according to claim 8 provides the
請求項９に係る発明は、請求項７又は８に記載のリンク機構１の制御方法において、２変数１次式では、第１速度の乗算係数である第１乗算係数（α／ｐ）が、所定値α（０≦α≦１）及び値１－αの一方を含み、第２速度の乗算係数である第２乗算係数（１－α）が、所定値α及び値１－αの他方を含むとともに、第１乗算係数及び第２乗算係数の一方が第１速度及び第２速度の間の比例関係を表す比例パラメータ（比例係数ｐ）を含むように構成されていることを特徴とする。
The invention according to claim 9 is the control method for the
請求項１０に係る発明は、請求項７ないし９のいずれか記載のリンク機構１の制御方法において、仮想速度（仮想角速度ωｚ）の目標となる仮想目標速度（仮想目標角速度ωｚ＿ｒｅｆ）を算出し（ＳＴＥＰ２８）、仮想速度が仮想目標速度になるように、所定の制御アルゴリズム［式（２５）］により、仮想目標出力（仮想目標トルクτｚ＿ｒｅｆ）を算出し、第１乗算係数（β／ｐ）が所定値β（０≦β≦１）及び値１－βの一方を含み、第２乗算係数が所定値β及び値１－βの他方を含むとともに、第１乗算係数及び第２乗算係数の一方が第１速度及び第２速度の間の比例関係を表す比例パラメータ（比例係数ｐ）を含むように構成し、第１アクチュエータの目標出力（第１目標トルクτ１＿ｒｅｆ）及び第２アクチュエータの目標出力（第２目標トルクτ２＿ｒｅｆ）をそれぞれ、仮想目標出力に対して第１乗算係数及び第２乗算係数を乗算することにより算出し、第１アクチュエータの出力及び第２アクチュエータの出力がそれぞれ第１アクチュエータの目標出力及び第２アクチュエータの目標出力になるように、第１アクチュエータ及び第２アクチュエータを制御する（ＳＴＥＰ７，８）ことを特徴とする。
According to a tenth aspect of the invention, in the control method of the
以下、図面を参照しながら、本発明の一実施形態に係るリンク機構の制御装置について説明する。まず、図１～３を参照しながら、本実施形態のリンク機構１について説明する。このリンク機構１は、基体２の上部の取付部材２ａに連結されており、この基体２は、上下方向に所定長さで延びている。
A link mechanism control device according to an embodiment of the present invention will be described below with reference to the drawings. First, the
このリンク機構１は、シリアルリンクタイプの２関節リンク機構であり、第１関節１０、第２関節２０、第１リンク３０、動力伝達機構４０及び第２リンク５０などを備えている。
The
第１リンク３０は、第１関節１０と第２関節２０の間に所定長さで延びており、基部３１及びカバー部３２を有している（図２参照）。この基部３１の一端部には、第１関節１０の後述する第１減速機１２が設けられており、他端部には、第２関節２０の後述する第２減速機２２が設けられている。
The
また、動力伝達機構４０は、第１リンク３０の基部３１の一端部と他端部の間に設けられている。さらに、カバー部３２は、これらの第１減速機１２、第２減速機２２及び動力伝達機構４０を全体にわたってカバーするように設けられている。
Also, the
さらに、第２リンク５０は、第２関節２０から所定長さで延びており、その先端部には、重り５１が取り付けられている。
Further, the
一方、第１関節１０は、能動関節タイプのものであり、第１モータ１１及び第１減速機１２などを備えている。この第１モータ１１（第１アクチュエータ）は、ブラシレスＤＣモータで構成され、そのケーシング及びステータが基体２の取付部材２ａに固定されている。この第１モータ１１は、図５に示すように、コントローラ７０に電気的に接続されており、このコントローラ７０からの駆動信号が入力されたときに、この駆動信号に従って回転する。
On the other hand, the first joint 10 is of an active joint type, and includes a
この第１モータ１１の回転軸１１ａの先端部には、動力伝達機構４０の第１プーリ４１が設けられている。この第１プーリ４１は、第１モータ１１の回転軸１１ａと同心にかつ一体に回転するように、第１モータ１１の回転軸１１ａに固定されている。なお、本実施形態では、第１モータ１１が第１アクチュエータに相当する。
A
また、第１減速機１２は、波動歯車機構タイプのものであり、ウェーブ・ジェネレータ、フレクスプライン及びサーキュラ・スプライン１２ａで構成されている。この第１減速機１２の場合、ウェーブ・ジェネレータは、第１モータ１１の回転軸１１ａと同心にかつ一体に回転するように、第１モータ１１の回転軸１１ａに固定されている。
The
また、フレクスプラインは、その基体２側の端部が取付部材２ａに固定され、サーキュラ・スプライン１２ａは、第１リンク３０の基部３２の第１減速機１２側の部分に固定されている。
The flexspline has its end on the
以上の構成により、第１リンク３０は、第１モータ１１の回転軸１１ａの回転に伴い、所定減速比（例えば、値１／１００）で減速されながら、第１モータ１１の回転軸周りすなわち第１関節１０の回転軸周りに回転する。すなわち、本実施形態では、回転軸１１ａが第１関節１０の回転軸に相当する。
With the above configuration, the
一方、第２関節２０は、第１関節１０と同様の能動関節タイプのものであり、第２モータ２１及び第２減速機２２などを備えている。この第２モータ２１（第２アクチュエータ）は、第１モータ１１と同様のＤＣモータで構成され、そのケーシング及びステータが第２リンク５０の第２減速機２２側の端部に固定されている。
On the other hand, the second joint 20 is of an active joint type similar to the first joint 10, and includes a
この第２モータ２１は、前述したコントローラ７０に電気的に接続されており、このコントローラ７０からの駆動信号が入力されたときに、この駆動信号に従って回転する。第２モータ２１の回転軸２１ａの先端部には、動力伝達機構４０の第２プーリ４６が設けられている。
The
この第２プーリ４６は、第２モータ２１の回転軸２１ａと同心にかつ一体に回転するように、第２モータ２１の回転軸２１ａに固定されている。また、第２モータ２１の回転軸２１ａは、第２減速機２２を介して第１リンク３０及び第２リンク５０に連結されている。なお、本実施形態では、第２モータ２１が第２アクチュエータに相当する。
The
第２減速機２２は、第１減速機１２と同じ減速比の波動歯車機構タイプのものであり、ウェーブ・ジェネレータ、フレクスプライン及びサーキュラ・スプライン２２ａで構成されている。この第２減速機２２の場合、ウェーブ・ジェネレータが、第２モータ２１の回転軸２１ａと同心にかつ一体に回転するように、第２モータ２１の回転軸２１ａに固定されている。
The
また、フレクスプラインは、その第２リンク５０側の端部が第２リンク５０に固定され、サーキュラ・スプライン２２ａは、第１リンク３０の第２減速機２２側の端部に固定されている。
The flexspline is fixed to the
以上の構成により、第２モータ２１の回転軸２１ａの回転に伴い、第２リンク５０は、前述した所定減速比で減速されながら、第２モータ２１の回転軸周りすなわち第２関節２０の回転軸周りに回転する。すなわち、本実施形態では、回転軸２１ａが第２関節２０の回転軸に相当する。
With the above configuration, as the
また、前述した動力伝達機構４０は、前述した第１プーリ４１、第１ベルト４２、遊星歯車機構４３、電磁ブレーキ４４、第２ベルト４５及び前述した第２プーリ４６を備えている。
Further, the
この遊星歯車機構４３は、シングルプラネタリタイプのものであり、サンギヤ４３ａ、プラネタリギヤ４３ｃ、プラネタリキャリア４３ｄ及びリングギヤ４３ｅを備えている（図４参照）。この遊星歯車機構４３では、キャリア側プーリ４３ｆが、プラネタリキャリア４３ｄと同心にかつ一体に回転するように、プラネタリキャリア４３ｄに固定されている。
This planetary gear mechanism 43 is of a single planetary type and includes a
このキャリア側プーリ４３ｆと前述した第１プーリ４１との間には、前述した第１ベルト４２が巻掛けられている。以上の構成により、第１モータ１１とプラネタリキャリア４３ｄの間では、第１プーリ４１、第１ベルト４２及びキャリア側プーリ４３ｆを介して、トルクが伝達される。
Between the carrier-
さらに、この遊星歯車機構４３では、リング側プーリ４３ｇが、リングギヤ４３ｅと同心にかつ一体に回転するように、リングギヤ４３ｅに固定されている。このリング側プーリ４３ｇと前述した第２プーリ４６との間には、前述した第２ベルト４５が巻掛けられている。以上の構成により、リングギヤ４３ｅと第２モータ２１の間では、第２プーリ４６、第２ベルト４５及びリング側プーリ４３ｇを介して、トルクが伝達される。
Furthermore, in this planetary gear mechanism 43, a ring-
一方、前述した電磁ブレーキ４４は、サンギヤ４３ａを回転停止状態と回転可能状態との間で切り換えるためのものであり、サンギヤ４３ａと一体の回転軸４３ｂの一端部側に設けられている。この電磁ブレーキ４４は、図示しないコントローラ７０に電気的に接続されており、コントローラ７０からの駆動信号が入力されたときにオン動作状態となり、それ以外のときにはオフ動作状態となる。
On the other hand, the above-described
この遊星歯車機構４３では、電磁ブレーキ４４がオン動作状態のときには、サンギヤ４３ａが回転停止状態に保持される一方、電磁ブレーキ４４がオフ動作状態のときには、サンギヤ４３ａは回転可能状態になる。この場合、サンギヤ４３ａを回転可能状態／回転停止状態に切り換える構成として、電磁ブレーキ４４に代えて、電動アクチュエータや油圧駆動アクチュエータを用いてもよい。
In this planetary gear mechanism 43, the
以上の構成により、この動力伝達機構４０では、電磁ブレーキ４４がオン動作状態のときには、プラネタリキャリア４３ｄとリングギヤ４３ｅの間でトルク伝達が可能になるとともに、トルク伝達の際、両者は同一方向に回転する。その結果、第１モータ１１の回転軸１１ａと第２モータ２１の回転軸２１ａの間、すなわち第１関節１０と第２関節２０の間でトルク伝達が可能になる。一方、電磁ブレーキ４４がオフ作状態のときには、プラネタリキャリア４３ｄとリングギヤ４３ｅは、トルクが両者の間で実質的に伝達されない状態となる。
With the above configuration, in the
以上のように、本実施形態の動力伝達機構４０では、電磁ブレーキ４４がオン動作状態のときには、第１関節１０と第２関節２０の間が連結され、両者の間でのトルク伝達が可能になる一方、電磁ブレーキ４４がオフ動作状態のときには、両者の間が実質的に遮断された状態になる。なお、以下の説明では、電磁ブレーキ４４がオン動作状態にあるときを「動力伝達機構４０が連結状態にある」といい、電磁ブレーキ４４がオフ動作状態にあるときを「動力伝達機構４０が連結解除状態にある」という。
As described above, in the
この場合、動力伝達機構４０が連結状態にあるときには、第１モータ１１の回転軸１１ａと第２モータ２１の回転軸２１ａが、４つのプーリ４１，４３ｆ，４３ｇ，４６、２本のベルト４２，４５及び遊星歯車機構４３を介して連結される。
In this case, when the
そのため、１組のプーリ４１，４３ｆの変速比Ｎ１、１組のプーリ４３ｇ，４６の変速比Ｎ２及び遊星歯車機構４３の変速比ｒにより、第１モータ１１の角速度ω１と第２モータ２１の角速度ω２の間で、ｐを比例係数（比例パラメータ）として、ω１＝ｐ・ω２が成立することになる。すなわち、２つの角速度ω１，ω２は互いに拘束し合う関係にある。本実施形態のリンク機構１の場合、ω１＝ｐ・ω２が成立することにより、第１関節１０の回転速度と、第２関節２０の回転速度が比例することになる。なお、以下の説明では、第１モータ１１の角速度ω１を「第１角速度ω１」と呼び、第２モータ２１１の角速度ω２を「第２角速度ω２」と呼ぶ。
Therefore, the angular velocity ω1 of the
この比例係数ｐは、後述するように、上述した３つの変速比Ｎ１，Ｎ２，ｒの設定により、値０以外の実数として様々な値に設定することが可能である（後述する式（２９）参照）。これに加えて、遊星歯車機構４３のサンギヤ４３ａ、プラネタリキャリア４３ｄ及びリングギヤ４３ｅにおいて、プーリ４３ｆ，４３ｇが取り付けられる２つの回転要素と、電磁ブレーキ４４によって停止される遊星歯車機構４３の残りの回転要素とを選択することにより、比例係数ｐを正値又は負値に設定することが可能である。
As will be described later, this proportionality coefficient p can be set to various values as real numbers other than 0 by setting the above-mentioned three speed ratios N1, N2, and r (equation (29) described later) reference). In addition to this, in the
また、動力伝達機構４０が連結状態にある場合、第１関節１０と第２関節２０との間で、第１モータ１１及び第２モータ２１に起因するトルクが伝達されることになる。したがって、第１モータ１１及び第２モータ２１の発生トルクと、動力伝達機構４０を介して伝達されるトルクとによって、リンク機構１を駆動することができる。それにより、動力伝達機構４０が連結状態にある場合、連結解除状態の場合と比べて、第１モータ１１及び第２モータ２１の消費電力を低減することができる。
Also, when the
次に、図５を参照しながら、本実施形態の制御装置６０について説明する。同図に示すように、制御装置６０は、コントローラ７０、第１回転センサ６１、第１電流センサ６２、第２回転センサ６３及び第２電流センサ６４を備えている。
Next, the
第１回転センサ６１は、ロータリエンコーダで構成され、第１モータ１１の回転角（以下「第１回転角」という）θ１及び角速度（以下「第１角速度」という）ω１を検出して、それらを表す検出信号をコントローラ７０に出力する。
The
また、第１電流センサ６２は、第１モータ１１を流れる電流（以下「第１電流」という）Ｉ１を検出して、それを表す検出信号をコントローラ７０に出力する。
Also, the first
さらに、第２回転センサ６３は、第１回転センサ６１と同様にロータリエンコーダで構成され、第２モータ２１の回転角（以下「第２回転角」という）θ２及び角速度（以下「第２角速度」という）ω２を検出して、それらを表す検出信号をコントローラ７０に出力する。
Further, the
また、第２電流センサ６４は、第２モータ２１を流れる電流（以下「第２電流」という）Ｉ２を検出して、それを表す検出信号をコントローラ７０に出力する。
Also, the second
一方、コントローラ７０は、ＣＰＵ、ＲＡＭ、ＲＯＭ及びＩ／Ｏインターフェース（いずれも図示せず）などからなるマイクロコンピュータで構成されており、前述した各種のセンサ６１～６４の検出信号などに応じて、後述するように、リンク機構制御処理を実行する。なお、本実施形態では、コントローラ７０が、機構制御部、モデル化誤差推定部、アクチュエータ制御部、仮想速度算出部及び仮想目標速度算出部に相当する。
On the other hand, the
次に、図６を参照しながら、本実施形態のコントローラ７０の機能的な構成について説明する。同図に示すように、まず、制御対象１００を、目標トルクベクトルτ＿ｒｅｆを制御入力とし、角速度ベクトルωを出力とする系として定義する。
Next, the functional configuration of the
この目標トルクベクトルτ＿ｒｅｆは、下式（１）に示すように、第１目標トルクτ１＿ｒｅｆ及び第２目標トルクτ２＿ｒｅｆを要素とするベクトルであり、コントローラ７０によって所定制御周期ΔＴで算出される。これらの第１目標トルクτ１＿ｒｅｆ及び第２目標トルクτ２＿ｒｅｆはそれぞれ、第１モータ１１及び第２モータ２１の目標トルクである。
This target torque vector τ_ref is a vector whose elements are the first target torque τ1_ref and the second target torque τ2_ref, as shown in the following equation (1), and is calculated by the
上式（１）における記号（ｋ）付きの各離散データは、所定制御周期ΔＴで算出されたデータであることを示しており、記号ｋ（ｋは正の整数）は各離散データの制御タイミング（制御時刻）を表している。例えば、記号ｋは今回の制御タイミングで算出（又はサンプリング）された今回値であることを示しており、この点は、以下の離散データにおいても同様である。また、以下の説明では、各離散データにおける記号（ｋ）を適宜省略する。 Each discrete data with the symbol (k) in the above equation (1) indicates that it is data calculated at a predetermined control period ΔT, and the symbol k (k is a positive integer) is the control timing of each discrete data. (control time). For example, the symbol k indicates the current value calculated (or sampled) at the current control timing, and this point also applies to the following discrete data. Also, in the following description, the symbol (k) in each discrete data will be omitted as appropriate.
なお、本実施形態では、第１目標トルクτ１＿ｒｅｆが第１アクチュエータの目標出力に相当し、第２目標トルクτ２＿ｒｅｆが第２アクチュエータの目標出力に相当する。 In this embodiment, the first target torque τ1_ref corresponds to the target output of the first actuator, and the second target torque τ2_ref corresponds to the target output of the second actuator.
また、角速度ベクトルωは、下式（２）に示すように、第１角速度ω１及び第２角速度ω２を要素とするベクトルである。 Also, the angular velocity vector ω is a vector whose elements are the first angular velocity ω1 and the second angular velocity ω2, as shown in the following equation (2).
図６に示すように、コントローラ７０は、目標角速度算出部７１、２つの切換部７２，７３、連結状態判定部７４、第１トルクコントローラ８０及び第２トルクコントローラ９０を備えている。
As shown in FIG. 6 , the
この目標角速度算出部７１では、以下に述べるように、目標角速度ベクトルω＿ｒｅｆが算出される。この目標角速度ベクトルω＿ｒｅｆは、下式（３）に示すように、第１目標角速度ω１＿ｒｅｆ及び第２目標角速度ω２＿ｒｅｆを要素とするベクトルであり、これらの第１及び第２目標角速度ω１＿ｒｅｆ，ω２＿ｒｅｆはそれぞれ、第１及び第２角速度ω１，ω２の目標値に相当する。
この場合、第１目標角速度ω１＿ｒｅｆは、第１回転角θ１がその目標値θ１ｒｅｆになるように、所定の制御アルゴリズムによって算出される。また、第２目標角速度ω２＿ｒｅｆは、第２回転角θ２がその目標値θ２ｒｅｆになるように、所定の制御アルゴリズムによって算出される。この場合、所定の制御アルゴリズムとしては、例えば、各種のフィードフォワード制御アルゴリズム、各種のフィードバック制御アルゴリズ、及び各種のフィードバック制御アルゴリズムとフィードフォワード制御アルゴリズムの組合せなどが用いられる。目標角速度算出部７１では、目標角速度ベクトルω＿ｒｅｆが、以上のように算出された後、切換部７２に出力される。
In this case, the first target angular velocity ω1_ref is calculated by a predetermined control algorithm so that the first rotation angle θ1 becomes its target value θ1ref. Also, the second target angular velocity ω2_ref is calculated by a predetermined control algorithm so that the second rotation angle θ2 becomes the target value θ2ref. In this case, as the predetermined control algorithm, for example, various feedforward control algorithms, various feedback control algorithms, combinations of various feedback control algorithms and feedforward control algorithms, and the like are used. After the target angular velocity vector ω_ref is calculated in the target angular
また、連結状態判定部７４では、動力伝達機構４０が連結状態にあるか否かが判定され、その判定結果が切換部７２，７３に対して出力される。そして、切換部７２では、目標角速度算出部７１からの目標角速度ベクトルω＿ｒｅｆ、及び連結状態判定部７４からの判定結果が入力されたときに、この判定結果に基づき、目標角速度ベクトルω＿ｒｅｆを、第１トルクコントローラ８０及び第２トルクコントローラ９０の一方に対して出力する。
Further, the connection
具体的には、切換部７２では、目標角速度ベクトルω＿ｒｅｆが、動力伝達機構４０が連結解除状態にあるときには第１トルクコントローラ８０に出力され、動力伝達機構４０が連結状態にあるときには第２トルクコントローラ９０に出力される。
Specifically, in the
第１トルクコントローラ８０では、後述する手法により、目標トルクベクトルτ＿ｒｅｆが算出され、これが切換部７３に出力される。また、第２トルクコントローラ９０では、後述する手法により、目標トルクベクトルτ＿ｒｅｆが算出され、これが切換部７３に出力される。
In the
そして、切換部７３では、連結状態判定部７４の判定結果に基づき、動力伝達機構４０が連結解除状態にあるときには、第１トルクコントローラ８０によって算出された目標トルクベクトルτ＿ｒｅｆが制御対象１００に出力される。一方、動力伝達機構４０が連結解除状態にあるときには、第２トルクコントローラ９０によって算出された目標トルクベクトルτ＿ｒｅｆが制御対象１００に出力される。
Then, based on the determination result of the connection
次に、第１トルクコントローラ８０について説明する。この第１トルクコントローラ８０は、動力伝達機構４０が連結解除状態にあるときに、目標トルクベクトルτ＿ｒｅｆを算出するものであり、図７に示すように、速度コントローラ８１、外乱オブザーバ８２及び加算器８３を備えている。
Next, the
この速度コントローラ８１では、目標角速度ベクトルω＿ｒｅｆ及び角速度ベクトルωに基づき、以下に述べるように、目標トルクベクトルのフィードバック項ベクトルτ＿ｆｂが算出され、これが加算器８３に出力される。
Based on the target angular velocity vector ω_ref and the angular velocity vector ω, the
このフィードバック項ベクトルτ＿ｆｂは、下式（４）に示すように、第１ＦＢ制御項τ１＿ｆｂ及び第２ＦＢ制御項τ２＿ｆｂを要素とするベクトルであり、これらのＦＢ制御項τ１＿ｆｂ，τ２＿ｆｂは、下式（５），（６）に示すＰ制御アルゴリズムにより算出される。 This feedback term vector τ_fb is a vector whose elements are the first FB control term τ1_fb and the second FB control term τ2_fb, as shown in the following equation (4). ), calculated by the P control algorithm shown in (6).
この場合、２つのＦＢ制御項τ１＿ｆｂ，τ２＿ｆｂを、Ｐ制御アルゴリズムに代えて、ＰＩＤ制御アルゴリズム、ＰＤ制御アルゴリズム、ＰＩ制御アルゴリズム及び応答指定型制御アルゴリズムなどの、各種のフィードバック制御アルゴリズムを用いて算出してもよい。 In this case, the two FB control terms τ1_fb and τ2_fb are calculated using various feedback control algorithms such as a PID control algorithm, a PD control algorithm, a PI control algorithm and a response-specified control algorithm instead of the P control algorithm. may
また、外乱オブザーバ８２では、下式（７）により、推定誤差ベクトルτｆ＿ｈａｔが算出される。下式（７）のＫｏｂｓ＿ｏｆｆは、所定ゲインである。なお、本実施形態では、外乱オブザーバ８２がモデル化誤差推定部に相当し、推定誤差ベクトルτｆ＿ｈａｔがモデル化誤差に相当する。
この推定誤差ベクトルτｆ＿ｈａｔは、下式（８）に示すように、２つの推定誤差τｆ１＿ｈａｔ，τｆ２＿ｈａｔを要素とするベクトルである。また、上式（７）のＪｎｏｍは、下式（９）のように定義される慣性モーメントのノミナル値行列である。 This estimated error vector τf_hat is a vector whose elements are two estimated errors τf1_hat and τf2_hat, as shown in the following equation (8). Also, Jnom in the above equation (7) is the nominal value matrix of the moment of inertia defined as in the following equation (9).
以上の推定誤差ベクトルτｆ＿ｈａｔの演算アルゴリズムは、以下に述べるように、導出される。 The calculation algorithm for the above estimated error vector τf_hat is derived as described below.
まず、図８に示すように、本実施形態のリンク機構１を、第１モータ１１及び第１減速機１２などに起因して慣性モーメントＪ１１を生じ、第２モータ２１及び第２減速機２２などに起因して慣性モーメントＪ２２を生じるシステムと見なす。このシステムにおいて、電磁ブレーキ４４がオフ状態にあるとき、すなわち動力伝達機構４０が連結解除状態にあるときの動特性をモデリングすると、下式（１０）～（１５）のモデルが得られる。
上式（１０）のＪａｃｔは、上式（１１）のように定義される、実際の慣性モーメント行列であり、上式（１１）のＪｎｍ（ｎ＝１～２，ｍ＝１～２）は、第１モータ１１、第１減速機１２、第２モータ２１及び第２減速機２２などによって決まる慣性モーメントである。また、式（１０）のＤωは、上式（１２）のように定義される角加速度ベクトルである。
Jact in the above formula (10) is the actual moment of inertia matrix defined as in the above formula (11), and Jnm (n = 1 to 2, m = 1 to 2) in the above formula (11) is , the moment of inertia determined by the
この角加速度ベクトルＤωの要素である第１角加速度Ｄω１及び第２角加速度Ｄω２は、上式（１３），（１４）によって算出される。また、上式（１０）のτは、上式（１５）のように定義される実トルクベクトルである。さらに、上式（１５）のτ１，τ２はそれぞれ、第１実トルク及び第２実トルクであり、第１及び第２モータ１１，２１で実際に発生しているトルクに相当する。
A first angular acceleration Dω1 and a second angular acceleration Dω2, which are elements of the angular acceleration vector Dω, are calculated by the above equations (13) and (14). Also, τ in the above equation (10) is the actual torque vector defined as in the above equation (15). Furthermore, τ1 and τ2 in the above equation (15) are the first actual torque and the second actual torque, respectively, and correspond to torques actually generated by the first and
なお、本実施形態では、第１実トルクτ１が第１アクチュエータの出力に相当し、第２実トルクτ２が第２アクチュエータの出力に相当する。 In this embodiment, the first actual torque τ1 corresponds to the output of the first actuator, and the second actual torque τ2 corresponds to the output of the second actuator.
一方、図８のシステムにおいて、動力伝達機構４０が存在しないと仮定してモデリングすると、下式（１６）の仮想モデルが得られる。
上記（１１）及び式（１６）の関係に基づき、動力伝達機構４０の存在がリンク機構１に与える影響をモデル化誤差（すなわち外乱）として捉え、これを下式（１７）に示す誤差ベクトルτｆとして定義し、この誤差ベクトルτｆを上式（１６）の仮想モデルに付加すると、下式（１８）が得られる。
Based on the relationship of the above equations (11) and (16), the influence of the presence of the
この式（１７）の要素τｆ１，τｆ２はそれぞれ、第１及び第２トルクτ１，τ２に影響を及ぼす第１誤差及び第２誤差である。 Elements τf1 and τf2 of this equation (17) are the first and second errors that affect the first and second torques τ1 and τ2, respectively.
上式（１８）において、トルクベクトルτを目標トルクベクトルτ＿ｒｅｆに、誤差ベクトルτｆを推定誤差ベクトルτｆ＿ｈａｔにそれぞれ置き換えると、下式（１９）が得られる。
この式（１９）に基づき、角加速度ベクトルＤωは、下式（２０）のように定義することができる。
また、推定誤差ベクトルτｆ＿ｈａｔは、下式（２１）のように定義することができる。
この式（２１）に、上式（２０）を代入することにより、前述した式（７）が得られる。 By substituting the above equation (20) into this equation (21), the above equation (7) is obtained.
以上のように、外乱オブザーバ８２では、前述した式（７）により、推定誤差ベクトルτｆ＿ｈａｔが算出され、これが加算器８３に出力される。
As described above, the
そして、加算器８３では、下式（２２）により、目標トルクベクトルτ＿ｒｅｆが算出される。
次に、前述した第２トルクコントローラ９０について説明する。この第２トルクコントローラ９０は、動力伝達機構４０が連結状態にあるときに、目標トルクベクトルτ＿ｒｅｆを算出するものであり、図９に示すように、混合比算出部９１、仮想角速度算出部９２、仮想目標角速度算出部９３、分配比算出部９４及び速度コントローラ９５を備えている。
Next, the aforementioned
混合比算出部９１では、混合比αが算出される。この混合比αは、後述する仮想角速度ωｚ及び仮想目標角速度ωｚ＿ｒｅｆの算出に用いられるものであり、具体的には、後述する手法により、０≦α≦１の範囲内の値として算出される。
The
また、仮想角速度算出部９２では、下式（２３）により、仮想角速度ωｚが算出される。なお、本実施形態では、仮想角速度算出部９２が仮想速度算出部に相当し、仮想角速度ωｚが仮想速度に相当する。
この式（２３）に示すように、仮想角速度ωｚは、２つの値α，１－αを重み係数とする、第１角速度ω１を比例係数ｐで除算した値ω１／ｐと、第２角速度ω２との加重平均演算値として算出される。 As shown in this equation (23), the virtual angular velocity ωz is the value ω1/p obtained by dividing the first angular velocity ω1 by the proportionality coefficient p, with two values α and 1−α as weighting factors, and the second angular velocity ω2 It is calculated as a weighted average calculation value of
この場合、第１関節１０の角速度を第１関節角速度ω１＿ｊとすると、この第１関節角速度ω１＿ｊは、第１角速度ω１を前述した所定減速比で減速した値に相当する。さらに、第２関節２０の角速度を第２関節角速度ｗ２＿ｊとすると、この第２関節角速度ｗ２＿ｊは、第２角速度ω２を前述した所定減速比で減速した値に相当する。 In this case, assuming that the angular velocity of the first joint 10 is a first joint angular velocity ω1_j, this first joint angular velocity ω1_j corresponds to a value obtained by reducing the first angular velocity ω1 with the above-described predetermined reduction ratio. Further, assuming that the angular velocity of the second joint 20 is a second joint angular velocity w2_j, this second joint angular velocity w2_j corresponds to a value obtained by reducing the second angular velocity ω2 with the above-described predetermined reduction ratio.
したがって、上式（２３）は、第１関節角速度ω１＿ｊ及び第２関節角速度ｗ２＿ｊの関係を定義したものに相当する。同じ理由により、ω１＿ｊ＝ｐ・ｗ２＿ｊが成立するので、上式（２３）の第１角速度ω１及び第２角速度ω２をそれぞれ、第１関節角速度ω１＿ｊ及び第２関節角速度ｗ２＿ｊに置き換えた式を用いて、仮想角速度ωｚを算出するように構成してもよい。 Therefore, the above equation (23) corresponds to defining the relationship between the first joint angular velocity ω1_j and the second joint angular velocity w2_j. For the same reason, ω1_j=p·w2_j is established, so using the equation obtained by replacing the first angular velocity ω1 and the second angular velocity ω2 in the above equation (23) with the first joint angular velocity ω1_j and the second joint angular velocity w2_j, respectively, , the virtual angular velocity ωz may be calculated.
さらに、仮想目標角速度算出部９３では、下式（２４）により、仮想目標角速度ωｚ＿ｒｅｆが算出される。なお、本実施形態では、仮想目標角速度算出部９３が仮想目標速度算出部に相当し、仮想目標角速度ωｚ＿ｒｅｆが仮想目標速度に相当する。
この式（２４）に示すように、仮想目標角速度ωｚ＿ｒｅｆは、２つの値α，１－αを重み係数とする、第１目標角速度ω１＿ｒｅｆを比例係数ｐで除算した値ω１＿ｒｅｆ／ｐと、第２目標角速度ω２＿ｒｅｆとの加重平均演算値として算出される。 As shown in this equation (24), the virtual target angular velocity ωz_ref is a value ω1_ref/p obtained by dividing the first target angular velocity ω1_ref by the proportionality coefficient p, with two values α and 1−α as weighting factors, and a second It is calculated as a weighted average calculated value with the target angular velocity ω2_ref.
一方、分配比算出部９４では、分配比βが算出される。この分配比βは、後述する目標トルクベクトルτ＿ｒｅｆの算出に用いられるものであり、後述する手法により、０≦β≦１の範囲内の値として算出される。
On the other hand, the
次いで、速度コントローラ９５では、まず、下式（２５）に示すＰ制御アルゴリズムにより、仮想目標トルクτｚ＿ｒｅｆ（仮想目標出力）が算出される。
この場合、仮想目標トルクτｚ＿ｒｅｆを、Ｐ制御アルゴリズムに代えて、ＰＩＤ制御アルゴリズム、ＰＤ制御アルゴリズム、ＰＩ制御アルゴリズム及び応答指定型制御アルゴリズムなどの、各種のフィードバック制御アルゴリズムを用いて算出してもよい。 In this case, the virtual target torque τz_ref may be calculated using various feedback control algorithms such as a PID control algorithm, a PD control algorithm, a PI control algorithm and a response-specifying control algorithm instead of the P control algorithm.
次いで、下式（２６）～（２８）により、目標トルクベクトルτ＿ｒｅｆが算出される。
第２トルクコントローラ９０では、以上の式（２３）～（２８）の演算アルゴリズムにより、目標トルクベクトルτ＿ｒｅｆが算出される。以上のように、目標トルクベクトルτ＿ｒｅｆの算出において、仮想角速度ωｚ、仮想目標角速度ωｚ＿ｒｅｆ及び仮想目標トルクτｚ＿ｒｅｆを用いたのは以下の理由による。
In the
すなわち、本実施形態のリンク機構１の場合、電磁ブレーキ４４がオンされ、動力伝達機構４０が連結状態にあるときには、ω１＝ｐ・ω２が成立するので、図１０に示すように、２つの回転軸１１ａ，２１ａが仮想のプーリ機構４７によって直結された状態と見なすことができる。したがって、動力伝達機構４０が連結状態にあるときには、１つの仮想軸を回転軸とする仮想関節において、その仮想軸の角速度である仮想角速度ωｚなどを用いて、リンク機構１を制御することが可能になる。
That is, in the case of the
以下、その力学的な原理について説明する。本実施形態の動力伝達機構４０の場合、遊星歯車機構４３を用いている関係上、電磁ブレーキ４４がオンされ、連結状態にあるときには、前述した比例係数ｐと、前述した３つの変速比Ｎ１，Ｎ２，ｒとの間で、下式（２９）が成立する。
また、サンギヤ４３ａに作用するトルクをτｓとすると、下式（３０）が成立する。
Further, when the torque acting on the
この式（３０）を、トルクτｓを含む式と含まない式とに分けて書き換えるとともに、トルクτｓを含まない式に上式（２９）を代入すると、下式（３１）が得られる。 Rewriting this equation (30) by dividing it into an equation including the torque τs and an equation not including the torque τs, and substituting the above equation (29) into the equation not including the torque τs yields the following equation (31).
この式（３１）にω１＝ｐ・ω２を代入すると、下式（３２）が得られる。 By substituting ω1=p·ω2 into the equation (31), the following equation (32) is obtained.
ここで、仮想軸ｚに作用する仮想のトルクを仮想トルクτｚとし、これを下式（３３）に示すように定義する。
また、仮想慣性モーメントＪｚを下式（３４）に示すように定義する。
さらに、第２角速度ω２は、ω１＝ｐ・ω２の関係により、下式（３５）に示すように書き換えることができる。
この式（３５）の第２角速度ω２を仮想角速度ωｚに置き換えると、下式（３６）が得られる。この式（３５）は前述した式（２３）に相当するものである。
さらに、前述した式（３２）に式（３４），（３５）を代入し、第２角加速度Ｄω２を仮想角加速度Ｄωｚに置き換えると、下式（３７）が得られる。
この式（３７）を参照すると明らかなように、仮想角加速度Ｄωｚと仮想トルクベクトルτｚの間で，一般的な運動方程式が成立する状態となる。したがって、動力伝達機構４０が連結状態にある場合、前述した式（２３）～（２８）の演算アルゴリズムにより、目標トルクベクトルτ＿ｒｅｆを算出した場合、これを用いてリンク機構１を適切に制御できることが判る。
As is clear from the expression (37), a general equation of motion is established between the virtual angular acceleration Dωz and the virtual torque vector τz. Therefore, when the
次に、図１１を参照しながら、本実施形態のリンク機構制御処理について説明する。この制御処理は、前述した動力伝達機構４０の連結状態／連結解除状態の切り換えを制御するとともに、第１及び第２モータ１１，２１の運転状態を制御するものであり、コントローラ７０によって、前述した所定制御周期ΔＴで実行される。
Next, the link mechanism control process of this embodiment will be described with reference to FIG. This control process controls switching between the connected state/disconnected state of the
同図に示すように、まず、電磁ブレーキ制御処理を実行する（図１１／ＳＴＥＰ１）。この電磁ブレーキ制御処理では、第１及び第２回転角θ１，θ２と、第１及び第２電流Ｉ１，Ｉ２などのパラメータに応じて、電磁ブレーキ４４がオン／オフされる。
As shown in the figure, first, an electromagnetic brake control process is executed (FIG. 11/STEP1). In this electromagnetic brake control process, the
次いで、目標角速度ベクトルω＿ｒｅｆを算出する（図１１／ＳＴＥＰ２）。具体的には、前述したように、第１回転角θ１がその目標値θ１ｒｅｆになるように、所定のフィードバック制御アルゴリズムによって、第１目標角速度ω１＿ｒｅｆが算出される。また、第２回転角θ２がその目標値θ２ｒｅｆになるように、所定のフィードバック制御アルゴリズムによって、第２目標角速度ω２＿ｒｅｆが算出される。なお、これらの目標値θ１ｒｅｆ，θ２ｒｅｆは図示しない算出処理によって算出される。 Next, a target angular velocity vector ω_ref is calculated (FIG. 11/STEP2). Specifically, as described above, the first target angular velocity ω1_ref is calculated by a predetermined feedback control algorithm so that the first rotation angle θ1 becomes the target value θ1ref. Also, a second target angular velocity ω2_ref is calculated by a predetermined feedback control algorithm so that the second rotation angle θ2 becomes the target value θ2ref. These target values θ1ref and θ2ref are calculated by a calculation process (not shown).
次に、電磁ブレーキ４４がオンされているか否かを判定する（図１１／ＳＴＥＰ３）。この判定が否定で（図１１／ＳＴＥＰ３…ＮＯ）、電磁ブレーキ４４がオフされているとき、すなわち、動力伝達機構４０が連結解除状態にあるときには、前述した式（４）～（６）により、フィードバック項ベクトルτ＿ｆｂを算出する。
Next, it is determined whether or not the
その後、前述した式（７）により、推定誤差ベクトルτｆ＿ｈａｔを算出する（図１１／ＳＴＥＰ５）。次いで、前述した式（２２）により、目標トルクベクトルτ＿ｒｅｆを算出する（図１１／ＳＴＥＰ６）。 After that, the estimated error vector τf_hat is calculated by the above-described formula (7) (FIG. 11/STEP 5). Next, the target torque vector τ_ref is calculated by the above-described formula (22) (FIG. 11/STEP 6).
次に、電流ＦＢ制御処理を実行して（図１１／ＳＴＥＰ８）、本処理を終了する。この電流ＦＢ制御処理では、目標トルクベクトルτ＿ｒｅｆの２つの要素τ１＿ｒｅｆ，τ２＿ｒｅｆをトルク定数で除算することにより、第１及び第２目標電流Ｉ１＿ｒｅｆ，Ｉ２＿ｒｅｆがそれぞれ算出される。そして、第１及び第２電流Ｉ１，Ｉ２がそれぞれ、第１及び第２目標電流Ｉ１＿ｒｅｆ，Ｉ２＿ｒｅｆになるようにフィードバック制御される。 Next, a current FB control process is executed (FIG. 11/STEP8), and this process ends. In this current FB control process, the first and second target currents I1_ref and I2_ref are calculated by dividing the two elements τ1_ref and τ2_ref of the target torque vector τ_ref by the torque constant. Then, the first and second currents I1 and I2 are feedback-controlled so as to become the first and second target currents I1_ref and I2_ref, respectively.
一方、前述した判定が肯定で（図１１／ＳＴＥＰ３…ＹＥＳ）、電磁ブレーキ４４がオンされているとき、すなわち、動力伝達機構４０が連結状態にあるときには、連結時制御処理を実行する（図１１／ＳＴＥＰ７）。この連結時制御処理は、具体的には、図１２に示すように実行される。
On the other hand, when the above-described determination is affirmative (FIG. 11/
同図に示すように、まず、第１停止条件が成立しているか否かを判定する（図１２／ＳＴＥＰ２０）。この場合、第１モータ１１がこれを停止すべき運転状態にあるときに、第１停止条件が成立したと判定され、それ以外のときには、第１停止条件が不成立であると判定される。具体的には、例えば、第１電流Ｉ１が過電流状態になっているときに、第１停止条件が成立したと判定される。
As shown in the figure, first, it is determined whether or not the first stop condition is satisfied (FIG. 12/STEP 20). In this case, it is determined that the first stop condition is met when the
この判定が肯定で（図１２／ＳＴＥＰ２０…ＹＥＳ）、第１停止条件が成立しているときには、前述した混合比α及び分配比βをいずれも値０に設定する（図１２／ＳＴＥＰ２１）。
When this determination is affirmative (FIG. 12/
一方、上記の判定が否定で（図１２／ＳＴＥＰ２０…ＮＯ）、第１停止条件が不成立であるときには、第２停止条件が成立しているか否かを判定する（図１２／ＳＴＥＰ２２）。この場合、第２モータ２１がこれを停止すべき運転状態にあるときに、第２停止条件が成立したと判定され、それ以外のときには、第２停止条件が不成立であると判定される。具体的には、例えば、第２電流Ｉ２が過電流状態になっているときに、第２停止条件が成立したと判定される。
On the other hand, when the above determination is negative (FIG. 12/
この判定が肯定で（図１２／ＳＴＥＰ２２…ＹＥＳ）、第２停止条件が成立しているときには、混合比α及び分配比βをいずれも値１に設定する（図１２／ＳＴＥＰ２３）。
When this determination is affirmative (FIG. 12/
一方、上記の判定が否定で（図１２／ＳＴＥＰ２２…ＮＯ）、第２停止条件が不成立であるときには、最小電流条件が成立しているか否かを判定する（図１２／ＳＴＥＰ２４）。この場合、第１及び第２モータ１１，２１をその消費電流が最小になるように運転可能な状態にあるときには、最小電流条件が成立したと判定され、それ以外のときには、最小電流条件が不成立であると判定される。
On the other hand, when the above determination is negative (FIG. 12/
この判定が肯定で（図１２／ＳＴＥＰ２４…ＹＥＳ）、最小電流条件が成立しているときには、混合比αを所定値α１に設定するとともに、分配比βを値ｐ２／（ｐ２＋１）に設定する（図１２／ＳＴＥＰ２５）。このように分配比βを値ｐ２／（ｐ２＋１）に設定した理由は、β＝ｐ２／（ｐ２＋１）のときに、２つの目標トルクτ１＿ｒｅｆ，τ２＿ｒｅｆの２乗和（τ１＿ｒｅｆ２＋τ２＿ｒｅｆ２）が最小値になることによる。すなわち、第１及び第２モータ１１，２１の消費電流が最小になることによる。
When this determination is affirmative (FIG. 12/STEP 24 . . . YES) and the minimum current condition is satisfied, the mixture ratio α is set to a predetermined value α1 and the distribution ratio β is set to a value p2/(p2+1) ( FIG. 12/STEP 25). The reason why the distribution ratio β is set to the value p2/(p2+1) is that when β=p2/(p2+1), the sum of the squares of the two target torques τ1_ref and τ2_ref (τ1_ref2+τ2_ref2) becomes the minimum value. according to. That is, the current consumption of the first and
一方、上記の判定が否定で（図１２／ＳＴＥＰ２４…ＮＯ）、最小電流条件が不成立であるときには、第１電流Ｉ１及び第２電流Ｉ２に応じて、図示しないマップを検索することにより、混合比α及び分配比βを算出する（図１２／ＳＴＥＰ２６）。 On the other hand, when the above determination is negative (FIG. 12/STEP 24 . . . NO) and the minimum current condition is not satisfied, the mixture ratio α and distribution ratio β are calculated (FIG. 12/STEP 26).
以上のように、混合比α及び分配比βを算出した後、前述した式（２３）により、仮想角速度ωｚを算出する（図１２／ＳＴＥＰ２７）。次いで、前述した式（２４）により、仮想目標角速度ωｚ＿ｒｅｆを算出する（図１２／ＳＴＥＰ２８）。 After calculating the mixture ratio α and the distribution ratio β as described above, the virtual angular velocity ωz is calculated by the above-described equation (23) (FIG. 12/STEP 27). Next, the virtual target angular velocity ωz_ref is calculated by the above-described formula (24) (FIG. 12/STEP 28).
その後、前述した式（２５）により、仮想目標トルクτｚ＿ｒｅｆを算出する（図１２／ＳＴＥＰ２９）。次いで、前述した式（２６）～（２８）により、目標トルクベクトルτ＿ｒｅｆを算出して（図１２／ＳＴＥＰ３０）、本処理を終了する。 After that, the virtual target torque τz_ref is calculated by the above-described equation (25) (FIG. 12/STEP 29). Next, the target torque vector τ_ref is calculated by the above-described equations (26) to (28) (FIG. 12/STEP 30), and this process is terminated.
図１１に戻り、連結時制御処理（図１１／ＳＴＥＰ７）を以上のように実行した後、前述したように、電流ＦＢ制御処理を実行して（図１１／ＳＴＥＰ８）、本処理を終了する。 Returning to FIG. 11, after the connection control process (FIG. 11/STEP7) is executed as described above, the current FB control process is executed (FIG. 11/STEP8) as described above, and this process ends.
次に、図１３を参照しながら、本実施形態のコントローラ７０における推定誤差ベクトルτｆ＿ｈａｔの効果について説明する。同図は、第１モータ１１を停止し、所定の正弦波電流を第２モータ２１に供給した際の誤差の推移を表しており、この誤差は、具体的には、第１回転角θ１と値０と偏差（θ１－０）に相当する。これは、第１モータ１１が停止されている関係上、第１回転角θ１が値０に保持されるべきであるので、第１回転角θ１と値０との偏差を制御誤差と見なせることによる。
Next, the effect of the estimated error vector τf_hat in the
また、同図において、実線で示す曲線は、本実施形態の推定誤差ベクトルτｆ＿ｈａｔを用いて目標トルクベクトルτ＿ｒｅｆを算出したときの制御結果である。さらに、破線で示す曲線は、比較のために、推定誤差ベクトルτｆ＿ｈａｔを用いることなく、目標トルクベクトルτ＿ｒｅｆを算出したとき、すなわち推定誤差ベクトルτｆ＿ｈａｔの２つの要素τｆ１＿ｈａｔ，τｆ２＿ｈａｔをいずれも値０として、目標トルクベクトルτ＿ｒｅｆを算出したときの比較制御結果である。 Further, in the same figure, the curve indicated by the solid line is the control result when the target torque vector τ_ref is calculated using the estimated error vector τf_hat of this embodiment. Furthermore, for comparison, the curve indicated by the dashed line is obtained when the target torque vector τ_ref is calculated without using the estimated error vector τf_hat, that is, when the two elements τf1_hat and τf2_hat of the estimated error vector τf_hat are both 0, It is a comparison control result when the target torque vector τ_ref is calculated.
これら２つの曲線を参照すると明らかなように、本実施形態の制御結果の方が、比較制御結果と比べて、誤差が１／５程度に減少しており、推定誤差ベクトルτｆ＿ｈａｔの効果によって、制御精度が向上することが判る。 As is clear from referring to these two curves, the control result of the present embodiment has an error reduced to about 1/5 compared to the comparative control result, and the effect of the estimated error vector τf_hat allows the control It can be seen that the accuracy is improved.
以上のように、本実施形態の制御装置６０によれば、動力伝達機構４０が連結状態と連結解除状態との間で切り換わるように、電磁ブレーキ４４がオン／オフ制御される。動力伝達機構４０が連結解除状態にある場合、推定誤差ベクトルτｆ＿ｈａｔを用いて、第１モータ１１及び第２モータ２１がそれぞれ制御される。
As described above, according to the
この推定誤差ベクトルτｆ＿ｈａｔの演算式（７）は、第１関節１０及び第２関節２０の間が連結解除状態にある場合のリンク機構１における第１角速度ω１、第２角速度ω２、第１モータ１１の実トルクτ１及び第２モータ２１の実トルクτ２の関係を、動力伝達機構４０が存在しないと仮定した条件で定義した仮想モデル［式（１６）］と、動力伝達機構４０が存在する実際の条件で定義した実モデル［式（１０）］とに基づいて導出される。
Equation (7) for the estimated error vector τf_hat is the first angular velocity ω1, the second angular velocity ω2, and the
したがって、第１関節１０及び第２関節２０の間が連結解除状態にある条件下での、動力伝達機構４０が実際に存在することに起因して生じるモデル化誤差、すなわち推定誤差ベクトルτｆ＿ｈａｔを適切に補償しながら、第１モータ１１及び第２モータ２１をそれぞれ制御することができる。それにより、制御精度を向上させることができる。
Therefore, the modeling error caused by the actual existence of the
また、動力伝達機構４０が連結状態にある場合、第１関節１０及び第２関節２０を１つの仮想関節と見なしたときの仮想関節の回転速度である仮想角速度ωｚが、第１モータ１１及び第２モータ２１を２つの独立変数とする算出式（２３）によって算出され、この仮想角速度ωｚを用いて、第１モータ１１及び第２モータ２１が制御される。
Further, when the
前述したように、動力伝達機構４０が連結状態にあるときには、ω１＝ｐ・ω２が成立するように、第１モータ１１の回転軸１１ａ及び第２モータ２１の回転軸２１ａが回転するので、第１関節１０及び第２関節２０を１つの仮想関節と見なすことが可能となる。したがって、そのような仮想関節の角速度である仮想角速度ωｚを、第１モータ１１及び第２モータ２１を２つの独立変数とする算出式（２３）によって算出することが可能になる。さらに、そのような仮想角速度ωｚを用いて、第１モータ１１及び第２モータ２１を制御することができるので、制御時の演算の容易化を図ることができる。
As described above, when the
さらに、仮想角速度ωｚの算出式（２３）では、第１角速度ω１の乗算係数が混合比αと比例係数ｐの積に設定され、第２角速度ω２の乗算係数が値１－αに設定されているので、混合比αを変更することにより、第１モータ１１及び第２モータ２１の重み付けを変更しながら、仮想角速度ωｚを算出することができる。これに加えて、第１角速度ω１の乗算係数が混合比αと比例係数ｐの積に設定されているので、第１モータ１１及び第２モータ２１の比例関係を適切に反映させながら、仮想角速度ωｚを算出することができる。
Furthermore, in the formula (23) for calculating the virtual angular velocity ωz, the multiplication factor of the first angular velocity ω1 is set to the product of the mixture ratio α and the proportionality factor p, and the multiplication factor of the second angular velocity ω2 is set to the
これに加えて、仮想角速度ωｚが仮想目標角速度ωｚ＿ｒｅｆになるように、所定のフィードバック制御アルゴリズムにより、仮想目標トルクτｚ＿ｒｅｆが算出され、この目標トルクベクトルτ＿ｒｅｆに、値β／ｐ及び値１－βをそれぞれ乗算することにより、第１モータ１１及び第２モータ２１への目標トルクτ１＿ｒｅｆ，τ２＿ｒｅｆが算出される。
In addition to this, a virtual target torque τz_ref is calculated by a predetermined feedback control algorithm so that the virtual angular velocity ωz becomes the virtual target angular velocity ωz_ref. Target torques τ1_ref and τ2_ref to the
したがって、これらの目標トルクτ１＿ｒｅｆ，τ２＿ｒｅｆは、仮想角速度ωｚを仮想目標角速度ωｚ＿ｒｅｆにするための、フィードバック制御成分を含むことになる。そして、第１モータ１１及び第２モータ２１の実際のトルクが目標トルクτ１＿ｒｅｆ，τ２＿ｒｅｆになるように、第１モータ１１及び第２モータ２１が制御される。したがって、仮想角速度ωｚを仮想目標角速度ωｚ＿ｒｅｆにするための制御入力成分を分配しながら、第１モータ１１及び第２モータ２１を制御することができる。
Therefore, these target torques τ1_ref and τ2_ref include feedback control components for making the virtual angular velocity ωz equal to the virtual target angular velocity ωz_ref. Then, the
また、２つの目標トルクτ１＿ｒｅｆ，τ２＿ｒｅｆは、値β／ｐ及び値１－βをそれぞれ仮想目標トルクτｚ＿ｒｅｆに乗算することにより算出されるので、この分配比βの値を変更することにより、第１モータ１１及び第２モータ２１の比例関係を適切に反映させながら、２つの目標トルクτ１＿ｒｅｆ，τ２＿ｒｅｆへの制御入力成分の分配割合を変更することができる。
Also, since the two target torques τ1_ref and τ2_ref are calculated by multiplying the virtual target torque τz_ref by the value β/p and the
なお、前述したように、本実施形態の場合、第１関節角速度ω１＿ｊは、第１角速度ω１を前述した所定減速比で減速した値に相当し、第２関節角速度ｗ２＿ｊは、第２角速度ω２を前述した所定減速比で減速した値に相当する。したがって、前述した式（２）などにおいて、第１モータ１１及び第２モータ２１を第１関節角速度ω１＿ｊ及び第２関節角速度ｗ２＿ｊにそれぞれ置き換えた数式を用いて、図１１～１２の制御処理を実行してもよい。
As described above, in the case of the present embodiment, the first joint angular velocity ω1_j corresponds to a value obtained by reducing the first angular velocity ω1 with the above-described predetermined reduction ratio, and the second joint angular velocity w2_j corresponds to the second angular velocity ω2. It corresponds to a value reduced by the predetermined speed reduction ratio described above. Therefore, the control processing of FIGS. 11 and 12 is executed using the formulas obtained by replacing the
なお、実施形態は、第１関節及び第２関節として、回転軸周りに回転する第１関節１０及び第２関節２０を用いた例であるが、第１関節及び第２関節の少なくとも一方として、直動関節を用いてもよい。 In addition, the embodiment is an example using the first joint 10 and the second joint 20 that rotate around the rotation axis as the first joint and the second joint, but as at least one of the first joint and the second joint, A prismatic joint may also be used.
例えば、第１関節及び第２関節の一方を直動関節とし、第１関節及び第２関節の他方を回転軸周りに回転する関節とした場合、動力伝達機構が連結状態にあって、第１関節の動力及び第２関節の動力が動力伝達機構を介して伝達される際、この動力伝達機構の構成により、第１関節及び第２関節の一方の速度が他方の速度と同一の次元になるように変換されることになる。すなわち、第１関節及び第２関節の間の比例係数ｐがこの動力伝達機構の構成によって定まることになる。以上の構成は、動力伝達機構が連結状態にある場合、第１関節及び第２関節は、第１関節の速度と第２関節の速度が比例するように作動することに相当する。 For example, if one of the first joint and the second joint is a translational joint and the other of the first joint and the second joint is a joint that rotates around the rotation axis, the power transmission mechanism is in the connected state and the first When the power of the joint and the power of the second joint are transmitted through the power transmission mechanism, the configuration of the power transmission mechanism causes the velocity of one of the first and second joints to be of the same dimension as the velocity of the other. will be converted to That is, the proportionality coefficient p between the first joint and the second joint is determined by the configuration of this power transmission mechanism. The above configuration corresponds to operating the first joint and the second joint such that the speed of the first joint and the speed of the second joint are proportional when the power transmission mechanism is in the connected state.
さらに、実施形態は、第１関節及び第２関節の速度として角速度ω１，ω２を用いた例であるが、これらに代えて、単位がｒｐｍ又はｒｐｓの回転速度を用いてもよい。また、上記のように直動関節を用いる場合には、移動速度を用いればよい。 Furthermore, the embodiment is an example using the angular velocities ω1 and ω2 as the velocities of the first joint and the second joint, but instead of these, rotational velocities in units of rpm or rps may be used. Moreover, when using a prismatic joint as described above, the moving speed may be used.
また、仮想角速度ωｚの算出式として、実施形態の式（２３）に代えて、下式（３８）～（４０）のいずれかを用いてもよい。その場合には、目標仮想角速度ωｚ＿ｒｅｆの算出式として、仮想角速度ωｚの算出に用いる式（３８）～（４０）のいずれかにおいて、仮想角速度ωｚを目標仮想角速度ωｚ＿ｒｅｆに置き換えたものを用いればよい。
さらに、目標トルクτ１＿ｒｅｆ，τ２＿ｒｅｆの算出式（２６），（２７）において、値βと値１－βを入れ換えてもよい。さらに、算出式（２６）の値β／ｐを値βに、算出式（２７）の値１－βを値（１－β）／ｐにそれぞれ置き換えてもよい。これに加えて、算出式（２６）の値β／ｐを値１－βに、算出式（２７）の値１－βを値β／ｐにそれぞれ置き換えてもよい。
Furthermore, in the calculation formulas (26) and (27) of the target torques τ1_ref and τ2_ref, the value β and the
一方、実施形態は、動力伝達機構として、４つのプーリ４１，４３ｆ，４３ｇ，４６、２本のベルト４２，４５、遊星歯車機構４３及び電磁ブレーキ４４を組み合わせた動力伝達機構４０を用いた例であるが、本発明の動力伝達機構は、これに限らず、第１関節及び第２関節の間を連結する連結状態と、連結を解除する連結解除状態とに切換可能に構成されたものであればよい。例えば、動力伝達機構として、歯車機構を用いてもよく、スプロケットとチェーンを組み合わせた機構などを用いてもよい。
On the other hand, the embodiment uses a
また、実施形態は、第１アクチュエータとして、ブラシレスＤＣモータである第１モータ１１を用いた例であるが、本発明の第１アクチュエータは、これに限らず、第１リンクを駆動するものであればよい。例えば、第１アクチュエータとして、ＡＣモータを用いたり、リニアモータ又は油圧アクチュエータなどの直動アクチュエータを用いたり、直動アクチュエータとギヤ機構を組み合わせたものを用いたり、ばねなどを用いたりしてもよい。また、第１アクチュエータとして、油圧モータ、空気圧モータ又は水圧モータを用いてもよい。
Also, the embodiment is an example of using the
さらに、実施形態は、第２アクチュエータとして、ブラシレスＤＣモータである第２モータ２１を用いた例であるが、本発明の第２アクチュエータは、これに限らず、第２リンクを駆動するものであればよい。例えば、第２アクチュエータとして、ＡＣモータを用いたり、リニアモータ又は油圧アクチュエータなどの直動アクチュエータを用いたり、直動アクチュエータとギヤ機構を組み合わせたものを用いたり、ばねなどを用いたりしてもよい。また、第２アクチュエータとして、油圧モータ、空気圧モータ又は水圧モータを用いてもよい。
Furthermore, the embodiment is an example of using the
一方、実施形態は、電磁ブレーキ４４を用いた例であるが、電磁ブレーキ４４に代えて、電動アクチュエータや油圧駆動アクチュエータを用いてもよい。
On the other hand, the embodiment is an example using the
また、実施形態は、動力伝達機構４０が連結状態にあるときに、第１モータ１１のトルク及び第２モータ２１のトルクが、第１関節１０と第２関節２０との間で双方向に伝達可能になるように、動力伝達機構４０を構成した例であるが、動力伝達機構４０が連結状態にあるときに、第１モータ１１のトルクのみが、第１関節１０から第２関節２０に伝達可能な第１状態になるように、動力伝達機構４０を構成してもよい。また、動力伝達機構４０が連結状態にあるときに、第２モータ２１のトルクのみが、第２関節２０にから第１関節１０伝達可能な第２状態になるように、動力伝達機構４０を構成してもよい。
Further, in the embodiment, when the
１ リンク機構
１０ 第１関節
１１ 第１モータ（第１アクチュエータ）
２０ 第２関節
２１ 第２モータ（第２アクチュエータ）
３０ 第１リンク
４０ 動力伝達機構
５０ 第２リンク
６０ 制御装置
７０ コントローラ（機構制御部、モデル化誤差推定部、アクチュエータ制御部、仮想 速度算出部、仮想目標速度算出部）
８２ 外乱オブザーバ（モデル化誤差推定部）
９２ 仮想角速度算出部（仮想速度算出部）
９３ 仮想目標角速度算出部（仮想目標速度算出部）
τｆ＿ｈａｔ 推定誤差ベクトル（モデル化誤差）
ωｚ 仮想角速度（仮想速度）
ωｚ＿ｒｅｆ 仮想目標角速度（仮想目標速度）
α 混合比（所定値）
ｐ 比例係数（比例パラメータ）
τｚ＿ｒｅｆ 仮想目標トルク（仮想目標出力）
β 分配比（所定値）
τ１ 第１実トルク（第１アクチュエータの出力）
τ２ 第２実トルク（第２アクチュエータの出力）
τ１＿ｒｅｆ 第１目標トルク（第１アクチュエータの目標出力）
τ２＿ｒｅｆ 第２目標トルク（第２アクチュエータの目標出力）
1
20 second joint 21 second motor (second actuator)
30
82 Disturbance observer (modeling error estimator)
92 virtual angular velocity calculator (virtual velocity calculator)
93 virtual target angular velocity calculator (virtual target angular velocity calculator)
τf_hat estimated error vector (modeling error)
ωz virtual angular velocity (virtual velocity)
ωz_ref Virtual target angular velocity (virtual target velocity)
α Mixing ratio (predetermined value)
p proportionality factor (proportional parameter)
τz_ref Virtual target torque (virtual target output)
β distribution ratio (predetermined value)
τ1 1st actual torque (output of 1st actuator)
τ2 2nd actual torque (output of 2nd actuator)
τ1_ref First target torque (target output of first actuator)
τ2_ref Second target torque (target output of second actuator)
Claims (10)
前記第１関節は、回転軸線周りに回転可能に構成され、
前記第２関節は、回転軸線周りに回転可能に構成され、
前記第１アクチュエータは、前記第１リンクを前記第１関節の前記回転軸線周りに回転駆動し、
前記第２アクチュエータは、前記第２リンクを前記第２関節の前記回転軸線周りに回転駆動し、
前記第１速度は、前記第１関節の回転速度であり、
前記第２速度は、前記第２関節の回転速度であり、
前記第１関節及び前記第２関節の間が前記連結状態と前記連結解除状態との間で切り換わるように、前記動力伝達機構を制御する機構制御部と、
前記第１関節及び前記第２関節の間が前記連結解除状態にある場合の前記リンク機構の動特性を、前記動力伝達機構が存在しないと仮定した条件で定義した仮想モデルと、前記動力伝達機構が存在する実際の条件で定義した実モデルとに基づき、モデル化誤差を推定するモデル化誤差推定部と、
前記第１関節及び前記第２関節の間が前記連結解除状態にある場合、前記モデル化誤差を用いて、前記第１アクチュエータ及び前記第２アクチュエータを制御するアクチュエータ制御部と、
を備えることを特徴とするリンク機構の制御装置。 A first link and a second link respectively connected to the first joint and the second joint are driven by the first actuator and the second actuator, respectively, and a power transmission mechanism is provided between the first joint and the second joint. , a first state in which power caused by the first actuator can be transmitted from the first joint to the second joint, and power caused by the second actuator can be transmitted from the second joint to the first joint so that at least one of the second states of When the joint and the second joint are in the connected state, the first joint and the second joint are arranged so that the first speed, which is the speed of the first joint, and the second speed, which is the speed of the second joint, are proportional to each other. A control device for a link mechanism in which the second joint operates,
The first joint is configured to be rotatable around a rotation axis,
The second joint is configured to be rotatable around a rotation axis,
The first actuator rotates the first link about the rotation axis of the first joint,
the second actuator rotationally drives the second link about the rotation axis of the second joint;
The first speed is the rotational speed of the first joint,
The second speed is the rotation speed of the second joint,
a mechanism control unit that controls the power transmission mechanism such that the first joint and the second joint are switched between the connected state and the disconnected state;
a virtual model defining the dynamic characteristics of the link mechanism when the connection between the first joint and the second joint is in the disconnected state on the assumption that the power transmission mechanism does not exist; and the power transmission mechanism. a modeling error estimator for estimating a modeling error based on a real model defined under actual conditions in which
an actuator control unit that controls the first actuator and the second actuator using the modeling error when the first joint and the second joint are in the disconnected state;
A control device for a link mechanism, comprising:
前記第１関節及び前記第２関節の間が前記連結状態にある場合、前記第１関節及び前記第２関節を１つの仮想関節と見なしたときの当該仮想関節の速度である仮想速度を、前記第１速度及び前記第２速度を２つの独立変数とする２変数１次式によって算出する仮想速度算出部をさらに備え、
前記アクチュエータ制御部は、前記第１関節及び前記第２関節の間が前記連結状態にある場合、前記仮想速度を用いて、前記第１アクチュエータ及び前記第２アクチュエータを制御することを特徴とするリンク機構の制御装置。 In the control device for the link mechanism according to claim 1,
When the first joint and the second joint are in the connected state, the virtual speed, which is the speed of the virtual joint when the first joint and the second joint are regarded as one virtual joint, further comprising a virtual speed calculation unit that calculates by a two-variable linear expression in which the first speed and the second speed are two independent variables,
The actuator control unit controls the first actuator and the second actuator using the virtual velocity when the first joint and the second joint are in the connected state. Mechanism control device.
前記第１関節は、回転軸線周りに回転可能に構成され、
前記第２関節は、回転軸線周りに回転可能に構成され、
前記第１アクチュエータは、前記第１リンクを前記第１関節の前記回転軸線周りに回転駆動し、
前記第２アクチュエータは、前記第２リンクを前記第２関節の前記回転軸線周りに回転駆動し、
前記第１速度は、前記第１関節の回転速度であり、
前記第２速度は、前記第２関節の回転速度であり、
前記第１関節及び前記第２関節の間が前記連結状態と前記連結解除状態との間で切り換わるように、前記動力伝達機構を制御する機構制御部と、
前記第１関節及び前記第２関節の間が前記連結状態にある場合、前記第１関節及び前記第２関節を１つの仮想関節と見なしたときの当該仮想関節の速度である仮想速度を、前記第１速度及び前記第２速度を２つの独立変数とする２変数１次式によって算出する仮想速度算出部と、
前記第１関節及び前記第２関節の間が前記連結状態にある場合、前記仮想速度を用いて、前記第１アクチュエータ及び前記第２アクチュエータを制御するアクチュエータ制御部と、
を備えることを特徴とするリンク機構の制御装置。 A first link and a second link respectively connected to the first joint and the second joint are driven by the first actuator and the second actuator, respectively, and a power transmission mechanism is provided between the first joint and the second joint. , a first state in which power caused by the first actuator can be transmitted from the first joint to the second joint, and power caused by the second actuator can be transmitted from the second joint to the first joint so that at least one of the second states of When the joint and the second joint are in the connected state, the first joint and the second joint are arranged so that the first speed, which is the speed of the first joint, and the second speed, which is the speed of the second joint, are proportional to each other. A control device for a link mechanism in which the second joint operates,
The first joint is configured to be rotatable around a rotation axis,
The second joint is configured to be rotatable around a rotation axis,
The first actuator rotates the first link about the rotation axis of the first joint,
the second actuator rotationally drives the second link about the rotation axis of the second joint;
The first speed is the rotational speed of the first joint,
The second speed is the rotation speed of the second joint,
a mechanism control unit that controls the power transmission mechanism such that the first joint and the second joint are switched between the connected state and the disconnected state;
When the first joint and the second joint are in the connected state, the virtual speed, which is the speed of the virtual joint when the first joint and the second joint are regarded as one virtual joint, a virtual speed calculator that calculates a two-variable linear expression with the first speed and the second speed as two independent variables;
an actuator control unit that controls the first actuator and the second actuator using the virtual velocity when the first joint and the second joint are in the connected state;
A control device for a link mechanism, comprising:
前記２変数１次式では、前記第１速度の乗算係数である第１乗算係数が、所定値α（０≦α≦１）及び値１－αの一方を含み、前記第２速度の乗算係数である第２乗算係数が、所定値α及び値１－αの他方を含むとともに、当該第１乗算係数及び当該第２乗算係数の一方が前記第１速度及び前記第２速度の間の比例関係を表す比例パラメータを含むように構成されていることを特徴とするリンク機構の制御装置。 In the link mechanism control device according to claim 2 or 3,
In the two-variable linear expression, the first multiplication factor, which is the multiplication factor for the first speed, includes one of a predetermined value α (0≦α≦1) and a value 1−α, and the multiplication factor for the second speed includes the other of the predetermined value α and the value 1−α, and one of the first multiplication factor and the second multiplication factor is a proportional relationship between the first velocity and the second velocity A control device for a linkage mechanism, characterized in that it is configured to include a proportional parameter representing:
前記仮想速度の目標となる仮想目標速度を算出する仮想目標速度算出部をさらに備え、
前記アクチュエータ制御部は、前記仮想速度が当該仮想目標速度になるように、所定の制御アルゴリズムにより、仮想目標出力を算出し、第１乗算係数が所定値β（０≦β≦１）及び値１－βの一方を含み、第２乗算係数が所定値β及び値１－βの他方を含むとともに、当該第１乗算係数及び当該第２乗算係数の一方が前記第１速度及び前記第２速度の間の比例関係を表す比例パラメータを含むように構成し、前記第１アクチュエータの目標出力及び前記第２アクチュエータの目標出力をそれぞれ、前記仮想目標出力に対して前記第１乗算係数及び前記第２乗算係数を乗算することにより算出するとともに、前記第１アクチュエータの出力及び前記第２アクチュエータの出力がそれぞれ前記第１アクチュエータの前記目標出力及び前記第２アクチュエータの前記目標出力になるように、前記第１アクチュエータ及び前記第２アクチュエータを制御することを特徴とするリンク機構の制御装置。 The link mechanism control device according to any one of claims 2 to 4,
further comprising a virtual target speed calculation unit that calculates a virtual target speed that is a target of the virtual speed;
The actuator control unit calculates a virtual target output by a predetermined control algorithm so that the virtual speed becomes the virtual target speed, and the first multiplication factor is a predetermined value β (0≦β≦1) and a value 1 -β, the second multiplication factor includes the other of the predetermined value β and the value 1-β, and one of the first multiplication factor and the second multiplication factor is one of the first velocity and the second velocity. wherein the target output of the first actuator and the target output of the second actuator are respectively obtained by multiplying the virtual target output by the first multiplication factor and the second multiplication factor. The first actuator output is calculated by multiplying coefficients, and the output of the first actuator and the output of the second actuator become the target output of the first actuator and the target output of the second actuator, respectively. A control device for a link mechanism, which controls an actuator and the second actuator.
前記第１関節は、回転軸線周りに回転可能に構成され、
前記第２関節は、回転軸線周りに回転可能に構成され、
前記第１アクチュエータは、前記第１リンクを前記第１関節の前記回転軸線周りに回転駆動し、
前記第２アクチュエータは、前記第２リンクを前記第２関節の前記回転軸線周りに回転駆動し、
前記第１速度は、前記第１関節の回転速度であり、
前記第２速度は、前記第２関節の回転速度であり、
前記第１関節及び前記第２関節の間が前記連結状態と前記連結解除状態との間で切り換わるように、前記動力伝達機構を制御し、
前記第１関節及び前記第２関節の間が前記連結解除状態にある場合の前記リンク機構の動特性を、前記動力伝達機構が存在しないと仮定した条件で定義した仮想モデルと、前記動力伝達機構が存在する実際の条件で定義した実モデルとに基づき、モデル化誤差を推定し、
前記第１関節及び前記第２関節の間が前記連結解除状態にある場合、前記モデル化誤差を用いて、前記第１アクチュエータ及び前記第２アクチュエータを制御することを特徴とするリンク機構の制御方法。 A first link and a second link respectively connected to the first joint and the second joint are driven by the first actuator and the second actuator, respectively, and a power transmission mechanism is provided between the first joint and the second joint. , a first state in which power caused by the first actuator can be transmitted from the first joint to the second joint, and power caused by the second actuator can be transmitted from the second joint to the first joint so that at least one of the second states of When the joint and the second joint are in the connected state, the first joint and the second joint are arranged so that the first speed, which is the speed of the first joint, and the second speed, which is the speed of the second joint, are proportional to each other. A control method for a link mechanism that operates the second joint,
The first joint is configured to be rotatable around a rotation axis,
The second joint is configured to be rotatable around a rotation axis,
The first actuator rotates the first link about the rotation axis of the first joint,
the second actuator rotationally drives the second link about the rotation axis of the second joint;
The first speed is the rotational speed of the first joint,
The second speed is the rotation speed of the second joint,
controlling the power transmission mechanism such that the first joint and the second joint are switched between the connected state and the disconnected state;
a virtual model defining the dynamic characteristics of the link mechanism when the connection between the first joint and the second joint is in the disconnected state on the assumption that the power transmission mechanism does not exist; and the power transmission mechanism. Estimate the modeling error based on a real model defined in real conditions where
A method of controlling a link mechanism, comprising: controlling the first actuator and the second actuator using the modeling error when the connection between the first joint and the second joint is in the disconnected state. .
前記第１関節及び前記第２関節の間が前記連結状態にある場合、前記第１関節及び前記第２関節を１つの仮想関節と見なしたときの当該仮想関節の速度である仮想速度を、前記第１速度及び前記第２速度を２つの独立変数とする２変数１次式によって算出し、
前記第１関節及び前記第２関節の間が前記連結状態にある場合、前記仮想速度を用いて、前記第１アクチュエータ及び前記第２アクチュエータを制御することを特徴とするリンク機構の制御方法。 In the control method of the link mechanism according to claim 6,
When the first joint and the second joint are in the connected state, the virtual speed, which is the speed of the virtual joint when the first joint and the second joint are regarded as one virtual joint, Calculated by a two-variable linear expression in which the first speed and the second speed are two independent variables,
A control method of a link mechanism, wherein when the first joint and the second joint are in the connected state, the virtual velocity is used to control the first actuator and the second actuator.
前記第１関節は、回転軸線周りに回転可能に構成され、
前記第２関節は、回転軸線周りに回転可能に構成され、
前記第１アクチュエータは、前記第１リンクを前記第１関節の前記回転軸線周りに回転駆動し、
前記第２アクチュエータは、前記第２リンクを前記第２関節の前記回転軸線周りに回転駆動し、
前記第１速度は、前記第１関節の回転速度であり、
前記第２速度は、前記第２関節の回転速度であり、
前記第１関節及び前記第２関節の間が前記連結状態と前記連結解除状態との間で切り換わるように、前記動力伝達機構を制御し、
前記第１関節及び前記第２関節の間が前記連結状態にある場合、前記第１関節及び前記第２関節を１つの仮想関節と見なしたときの当該仮想関節の速度である仮想速度を、前記第１速度及び前記第２速度を２つの独立変数とする２変数１次式によって算出し、
前記第１関節及び前記第２関節の間が前記連結状態にある場合、前記仮想速度を用いて、前記第１アクチュエータ及び前記第２アクチュエータを制御することを特徴とするリンク機構の制御方法。 A first link and a second link respectively connected to the first joint and the second joint are driven by the first actuator and the second actuator, respectively, and a power transmission mechanism is provided between the first joint and the second joint. , a first state in which power caused by the first actuator can be transmitted from the first joint to the second joint, and power caused by the second actuator can be transmitted from the second joint to the first joint The first When the joint and the second joint are in the connected state, the first joint and the second joint are arranged so that the first speed, which is the speed of the first joint, and the second speed, which is the speed of the second joint, are proportional to each other. A control method for a link mechanism that operates the second joint,
The first joint is configured to be rotatable around a rotation axis,
The second joint is configured to be rotatable around a rotation axis,
The first actuator rotates the first link about the rotation axis of the first joint,
the second actuator rotationally drives the second link about the rotation axis of the second joint;
The first speed is the rotational speed of the first joint,
The second speed is the rotation speed of the second joint,
controlling the power transmission mechanism such that the first joint and the second joint are switched between the connected state and the disconnected state;
When the first joint and the second joint are in the connected state, the virtual velocity, which is the velocity of the virtual joint when the first joint and the second joint are regarded as one virtual joint, Calculated by a two-variable linear expression in which the first speed and the second speed are two independent variables,
A method of controlling a link mechanism, wherein when the first joint and the second joint are in the connected state, the virtual velocity is used to control the first actuator and the second actuator.
前記２変数１次式では、前記第１速度の乗算係数である第１乗算係数が、所定値α（０≦α≦１）及び値１－αの一方を含み、前記第２速度の乗算係数である第２乗算係数が、所定値α及び値１－αの他方を含むとともに、当該第１乗算係数及び当該第２乗算係数の一方が前記第１速度及び前記第２速度の間の比例関係を表す比例パラメータを含むように構成されていることを特徴とするリンク機構の制御方法。 In the control method of the link mechanism according to claim 7 or 8,
In the two-variable linear expression, the first multiplication factor, which is the multiplication factor for the first speed, includes one of a predetermined value α (0≦α≦1) and a value 1−α, and the multiplication factor for the second speed includes the other of the predetermined value α and the value 1−α, and one of the first multiplication factor and the second multiplication factor is a proportional relationship between the first velocity and the second velocity A control method for a link mechanism, comprising: a proportional parameter representing
前記仮想速度の目標となる仮想目標速度を算出し、
前記仮想速度が当該仮想目標速度になるように、所定の制御アルゴリズムにより、仮想目標出力を算出し、
第１乗算係数が所定値β（０≦β≦１）及び値１－βの一方を含み、第２乗算係数が所定値β及び値１－βの他方を含むとともに、当該第１乗算係数及び当該第２乗算係数の一方が前記第１速度及び前記第２速度の間の比例関係を表す比例パラメータを含むように構成し、
前記第１アクチュエータの目標出力及び前記第２アクチュエータの目標出力をそれぞれ、前記仮想目標出力に対して前記第１乗算係数及び前記第２乗算係数を乗算することにより算出し、
前記第１アクチュエータの出力及び前記第２アクチュエータの出力がそれぞれ前記第１アクチュエータの前記目標出力及び前記第２アクチュエータの前記目標出力になるように、前記第１アクチュエータ及び前記第２アクチュエータを制御することを特徴とするリンク機構の制御方法。 In the link mechanism control method according to any one of claims 7 to 9,
calculating a virtual target speed as a target of the virtual speed;
calculating a virtual target output by a predetermined control algorithm so that the virtual speed becomes the virtual target speed;
The first multiplication factor includes one of a predetermined value β (0≦β≦1) and the value 1−β, the second multiplication coefficient includes the other of the predetermined value β and the value 1−β, and the first multiplication coefficient and one of the second multiplication factors comprising a proportional parameter representing a proportional relationship between the first speed and the second speed;
calculating a target output of the first actuator and a target output of the second actuator by multiplying the virtual target output by the first multiplication factor and the second multiplication factor, respectively;
controlling the first actuator and the second actuator such that the output of the first actuator and the output of the second actuator become the target output of the first actuator and the target output of the second actuator, respectively; A control method for a link mechanism, characterized by:
Priority Applications (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2019109024A JP7260406B2 (en) | 2019-06-11 | 2019-06-11 | Link mechanism control device and control method |
| CN202010381472.1A CN111941403B (en) | 2019-05-17 | 2020-05-08 | Link mechanism, control device and control method thereof |
| US16/874,650 US11181175B2 (en) | 2019-05-17 | 2020-05-14 | Link mechanism, control device and control method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2019109024A JP7260406B2 (en) | 2019-06-11 | 2019-06-11 | Link mechanism control device and control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2020199604A JP2020199604A (en) | 2020-12-17 |
| JP7260406B2 true JP7260406B2 (en) | 2023-04-18 |
Family
ID=73743030
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2019109024A Active JP7260406B2 (en) | 2019-05-17 | 2019-06-11 | Link mechanism control device and control method |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7260406B2 (en) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2007072546A1 (en) | 2005-12-20 | 2007-06-28 | Harmonic Drive Systems Inc. | Finger joint mechanism |
| JP2011230256A (en) | 2010-04-28 | 2011-11-17 | Nidec Sankyo Corp | Industrial robot |
| JP2017177233A (en) | 2016-03-28 | 2017-10-05 | 裕 山下 | Joint driving device and multi-axial manipulator |
-
2019
- 2019-06-11 JP JP2019109024A patent/JP7260406B2/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2007072546A1 (en) | 2005-12-20 | 2007-06-28 | Harmonic Drive Systems Inc. | Finger joint mechanism |
| JP2011230256A (en) | 2010-04-28 | 2011-11-17 | Nidec Sankyo Corp | Industrial robot |
| JP2017177233A (en) | 2016-03-28 | 2017-10-05 | 裕 山下 | Joint driving device and multi-axial manipulator |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2020199604A (en) | 2020-12-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11559891B2 (en) | Robot system and method for controlling robot system | |
| JP3981773B2 (en) | Robot controller | |
| EP3627693B1 (en) | Motor control system, control method for motor control system, and robot system | |
| JP2005349555A (en) | Controller for flexible joint robot arm | |
| JP7260406B2 (en) | Link mechanism control device and control method | |
| WO2005063455A1 (en) | Robot controller | |
| JP2016506222A (en) | Method and corresponding apparatus for coupling the shaft of an electric motor to the axle of an electric vehicle or hybrid vehicle. | |
| JP3831264B2 (en) | Electric vehicle control device | |
| JPH09212203A (en) | Robot controller | |
| JP6720714B2 (en) | Electric vehicle control method and electric vehicle control device | |
| JP2019103344A (en) | Control device of vehicle | |
| JP6728633B2 (en) | Electric vehicle control method and control device | |
| WO2019044237A1 (en) | Robot control apparatus | |
| US11181175B2 (en) | Link mechanism, control device and control method thereof | |
| JP2017192248A (en) | Control method for electric vehicle, and control apparatus therefor | |
| JP2016005296A (en) | Control method of motor drive device, motor drive device and robot device | |
| JP5633268B2 (en) | Robot control device | |
| JP7099186B2 (en) | Rotating electric machine control device | |
| JPH08328627A (en) | Servo controller, servo controlling method, robot controller using these and robot controlling method | |
| JP4289275B2 (en) | Control method for articulated robot | |
| JP7249603B2 (en) | Control device, control method and robot system | |
| JP2790634B2 (en) | Exercise machine control device | |
| JP7044532B2 (en) | Vehicle drive and vehicle control | |
| JP2023063015A (en) | Method and device for controlling electric vehicle | |
| WO2023053212A1 (en) | Motor control device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20211126 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20220928 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20221004 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20221128 |
|
| 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: 20230404 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20230406 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 7260406 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |