JP2012240144A - Robot control device and robot system - Google Patents
Robot control device and robot system Download PDFInfo
- Publication number
- JP2012240144A JP2012240144A JP2011111071A JP2011111071A JP2012240144A JP 2012240144 A JP2012240144 A JP 2012240144A JP 2011111071 A JP2011111071 A JP 2011111071A JP 2011111071 A JP2011111071 A JP 2011111071A JP 2012240144 A JP2012240144 A JP 2012240144A
- Authority
- JP
- Japan
- Prior art keywords
- manipulator
- prohibited area
- unit
- angle
- robot control
- 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.)
- Pending
Links
Images
Landscapes
- Manipulator (AREA)
Abstract
Description
本発明は、ロボット制御装置およびロボットシステムに関する。 The present invention relates to a robot control device and a robot system.
近年、産業用ロボットの一つとして、要求される位置姿勢の自由度よりも多くの自由度（冗長自由度）を有する多関節ロボットが開発されている。 In recent years, articulated robots having more degrees of freedom (redundancy degrees of freedom) than the required degrees of freedom of position and orientation have been developed as one of industrial robots.
この種の多関節ロボットは、冗長自由度を有することで、対象物に対して求められた位置姿勢を取った場合であっても、マニピュレータの姿勢を種々変更することができる。これにより、たとえば、比較的狭隘な場所であっても障害物を回避しながら作業を行うことが可能となる。なお、特許文献１には、冗長自由度を有するマニピュレータを制御するための一手法が開示されている。
This type of multi-joint robot has redundant degrees of freedom, so that the posture of the manipulator can be variously changed even when the position and posture obtained with respect to the object are taken. Thereby, for example, it is possible to perform work while avoiding an obstacle even in a relatively narrow place.
ところで、多くのマニピュレータは、予め教示された動作経路に沿って動作するが、適用用途によっては、作業対象物が動いている等、位置が不確かな作業対象物に対して作業を行う場合もある。このような場合には、作業対象物の位置を検知するセンサ等の情報に基づいてマニピュレータの目標位置姿勢を修正することとなる。 By the way, many manipulators operate along a motion path taught in advance. However, depending on the application, there are cases in which work is performed on a work object whose position is uncertain, such as the work object moving. . In such a case, the target position / posture of the manipulator is corrected based on information such as a sensor for detecting the position of the work object.
しかしながら、マニピュレータが冗長自由度を有する場合、特許文献１のように予め冗長軸を含む各駆動軸全ての動作量を教示により決定しておく手法では、たとえば作業対象物の位置が移動した場合に冗長軸の位置が必ずしも適切な位置とならず、マニピュレータが作業対象物や周囲の障害物と接触する可能性がある。
However, when the manipulator has redundant degrees of freedom, as in
開示の技術は、上記に鑑みてなされたものであって、冗長自由度を有するマニピュレータを適切に制御することができるロボット制御装置およびロボットシステムを提供することを目的とする。 The disclosed technique has been made in view of the above, and an object thereof is to provide a robot control device and a robot system that can appropriately control a manipulator having redundant degrees of freedom.
本願の開示するロボット制御装置は、複数の関節軸を有するマニピュレータを制御するロボット制御装置であって、目標位置に基づいて前記関節軸の角度を計算する角度計算部と、前記角度計算部によって計算された前記関節軸の角度から特定される前記マニピュレータの各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、前記禁止領域に含まれる前記部位の位置を前記禁止領域外の位置へ修正する位置修正部と、前記位置修正部によって修正された前記部位の位置に基づいて前記関節軸の角度を再計算する再計算部とを備える。 A robot control device disclosed in the present application is a robot control device that controls a manipulator having a plurality of joint axes, and calculates an angle of the joint axis based on a target position, and is calculated by the angle calculation unit. When any one of the positions of each part of the manipulator specified from the angle of the joint axis is included in a predetermined prohibited area, the position of the part included in the prohibited area is a position outside the prohibited area. And a recalculator that recalculates the angle of the joint axis based on the position of the part corrected by the position corrector.
また、本願の開示するロボットシステムは、複数の関節軸を有するマニピュレータおよび当該マニピュレータを制御するロボット制御装置を備えるロボットシステムであって、前記ロボット制御装置は、目標位置に基づいて前記関節軸の角度を計算する角度計算部と、前記角度計算部によって計算された前記関節軸の角度から特定される前記マニピュレータの各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、前記禁止領域に含まれる前記部位の位置を前記禁止領域外の位置へ修正する位置修正部と、前記位置修正部によって修正された前記部位の位置に基づいて前記関節軸の角度を再計算する再計算部とを備える。 In addition, a robot system disclosed in the present application is a robot system including a manipulator having a plurality of joint axes and a robot control device that controls the manipulator, and the robot control device determines the angle of the joint axis based on a target position. The forbidden region when any one of the positions of each part of the manipulator specified from the angle of the joint axis calculated by the angle calculating unit is included in a predetermined forbidden region A position correcting unit that corrects the position of the part included in the position to a position outside the prohibited region; and a recalculating part that recalculates the angle of the joint axis based on the position of the part corrected by the position correcting unit; Is provided.
本願の開示するロボット制御装置およびロボットシステムの一つの態様によれば、冗長自由度を有するマニピュレータを適切に制御することができる。 According to one aspect of the robot control device and the robot system disclosed in the present application, it is possible to appropriately control a manipulator having redundant degrees of freedom.
以下に添付図面を参照して、本願の開示するロボット制御装置およびロボットシステムのいくつかの実施例を詳細に説明する。ただし、これらの実施例における例示で本発明が限定されるものではない。 Exemplary embodiments of a robot control device and a robot system disclosed in the present application will be described below in detail with reference to the accompanying drawings. However, the present invention is not limited to the examples in these examples.
まず、本実施例に係るロボットシステムについて図１を用いて説明する。図１は、本実施例に係るロボットシステムの構成を示す図である。 First, the robot system according to the present embodiment will be described with reference to FIG. FIG. 1 is a diagram illustrating a configuration of a robot system according to the present embodiment.
なお、以下では、本願の開示するロボットシステムを、乳牛に対して搾乳作業を行う自動搾乳システムへ適用した場合の例について説明するが、本願の開示するロボットシステムは、これに限ったものではなく、たとえば家畜に対して検査や消毒等を自動的に行う場合にも適用することができる。また、本願の開示するロボットシステムは、家畜以外の対象物について所定の作業を行う場合にも適用することができる。 In the following, an example in which the robot system disclosed in the present application is applied to an automatic milking system that performs milking operations on dairy cows will be described. However, the robot system disclosed in the present application is not limited to this. For example, the present invention can also be applied to a case where a livestock is automatically inspected or disinfected. The robot system disclosed in the present application can also be applied to a case where a predetermined work is performed on an object other than livestock.
図１に示すように、本実施例に係るロボットシステムは、マニピュレータ１と、ロボット制御装置２と、入力装置３とを含んで構成される。
As shown in FIG. 1, the robot system according to this embodiment includes a
マニピュレータ１は、複数の関節（後述するアクチュエータ）を備える多関節ロボットである。マニピュレータ１は、エンドエフェクタとして搾乳部１００を備える。そして、マニピュレータ１は、ロボット制御装置２からの指示に従って各関節を個別に任意の角度だけ回転させることによって、搾乳部１００をターゲット部Ｔ（乳牛Ｃの乳頭）へ向けて移動させて搾乳を行う。
The
なお、マニピュレータ１の先端部（たとえば、搾乳部１００の近傍）には、ターゲット部Ｔを検知するための物体検知センサ１０１が設けられている。
Note that an
ロボット制御装置２は、マニピュレータ１の動作制御を行うコントローラである。具体的には、ロボット制御装置２は、マニピュレータ１と通信ネットワークを介して相互に接続されており、かかる通信ネットワークを介し、マニピュレータ１に対して各関節の駆動を指示することによってマニピュレータ１を目標位置へ移動させる。
The
ここで、マニピュレータ１による搾乳作業は、図示しないゲージに乳牛Ｃを収容させた状態で行われる。このように乳牛Ｃをゲージに収容することで乳牛Ｃの動きが規制されるため、ターゲット部Ｔの位置を大まかに固定することができる。ただし、乳牛Ｃがゲージで動くことがあるため、ターゲット部Ｔの位置は細かく変化することとなる。
Here, the milking operation by the
そこで、ロボット制御装置２は、物体検知センサ１０１の検知結果に基づいてターゲット部Ｔの位置を認識し、認識したターゲット部Ｔの位置を目標位置として搾乳部１００を移動させることとしている。これにより、図示しないゲージ内において不規則に動くターゲット部Ｔに追従してマニピュレータ１を移動させることが可能となる。
Therefore, the
なお、ロボット制御装置２としては、たとえば、記憶装置や電子演算器あるいは表示装置等を備えるコンピュータを用いることができる。
As the
入力装置３は、作業者等からの操作を受け付ける装置である。かかる入力装置３は、たとえば有線ＬＡＮ（Local Area Network）や無線ＬＡＮといった通信ネットワークで相互に接続されている。入力装置３は、作業者等からの操作に応じて、マニピュレータ１による搾乳作業の開始や終了あるいは各種の設定変更等をロボット制御装置２に対して指示する。
The
次に、ロボット制御装置２の構成について図２を用いて説明する。図２は、ロボット制御装置２の構成を示すブロック図である。なお、図２では、ロボット制御装置２の特徴を説明するために必要な構成要素のみを示しており、一般的な構成要素についての記載を省略している。
Next, the configuration of the
図２に示すように、ロボット制御装置２は、制御部２１と、記憶部２２とを備える。また、制御部２１は、角度計算部２１ａと、位置修正部２１ｂと、再計算部２１ｃと、動作指示部２１ｄと、設定処理部２１ｅとを備える。また、記憶部２２は、禁止領域データ２２ａと、バネ・ダンパモデルデータ２２ｂとを記憶する。
As shown in FIG. 2, the
角度計算部２１ａは、物体検知センサ１０１の検知結果に基づいてマニピュレータ１の各関節の角度を計算する処理部である。具体的には、角度計算部２１ａは、物体検知センサ１０１の検知結果を用いて搾乳部１００の移動先となる目標位置を決定し、搾乳部１００の位置を目標位置と一致させるように、マニピュレータ１の各関節部に設けられたアクチュエータの回転角度を計算する。
The
ここで、マニピュレータ１の構成について図３を用いて説明する。図３は、マニピュレータ１の構成を示す図である。図３に示すように、マニピュレータ１は、基台１０と、第１リンク１１〜第６リンク１６と、フランジ１７と、搾乳部１００と、物体検知センサ１０１とを備える。
Here, the configuration of the
基台１０は、壁面や床面等の設置面Ｗに基端部が固定され、先端部において第１アクチュエータ１１ａ（第１関節軸）を介して第１リンク１１を回転可能に支持する構造材である。
The
第１リンク１１は、第１アクチュエータ１１ａが駆動することによって基台１０の先端部を中心として回転する構造材である。かかる第１リンク１１は、先端部において第２アクチュエータ１２ａ（第２関節軸）を介して第２リンク１２を回転可能に支持する。第２リンク１２は、第２アクチュエータ１２ａが駆動することによって第１リンク１１の先端部を中心として回転する構造材である。かかる第２リンク１２は、先端部において第３アクチュエータ１３ａ（第３関節軸）を介して第３リンク１３を回転可能に支持する。
The
第３リンク１３は、第３アクチュエータ１３ａが駆動することによって第２リンク１２の先端部を中心として回転する構造材である。かかる第３リンク１３は、先端部において第４アクチュエータ１４ａ（第４関節軸）を介して第４リンク１４を回転可能に支持する。第４リンク１４は、第４アクチュエータ１４ａが駆動することによって第３リンク１３の先端部を中心として回転する構造材である。かかる第４リンク１４は、先端部において第５アクチュエータ１５ａ（第５関節軸）を介して第５リンク１５を回転可能に支持する。
The
第５リンク１５は、第５アクチュエータ１５ａが駆動することによって第４リンク１４の先端部を中心として回転する構造材である。かかる第５リンク１５は、先端部において第６アクチュエータ１６ａ（第６関節軸）を介して第６リンク１６を回転可能に支持する。第６リンク１６は、第６アクチュエータ１６ａが駆動することによって第５リンク１５の先端部を中心として回転する構造材である。かかる第６リンク１６は、先端部において第７アクチュエータ１７ａ（第７関節軸）を介してフランジ１７と、フランジ１７に取り付けられた搾乳部１００および物体検知センサ１０１とを回転可能に支持する。
The
なお、本実施例では、第３アクチュエータ１３ａが冗長軸としてあらかじめ設定されているものとするが、冗長軸として設定されるアクチュエータは、第３アクチュエータ１３ａ以外であってもよい。このように、マニピュレータ１が冗長軸を備えることで、ターゲット部Ｔに対して求められた位置姿勢を取った場合であってもマニピュレータ１の姿勢を種々変更することができる。
In the present embodiment, the
また、第１アクチュエータ１１ａ〜第７アクチュエータ１７ａは、たとえば、減速機一体型のサーボモータによって構成される。また、第１アクチュエータ１１ａ〜第７アクチュエータ１７ａには、各アクチュエータ１１ａ〜１７ａの回転位置を検出するエンコーダがそれぞれ内蔵される。かかるエンコーダによって検出された回転位置は、通信ネットワークを介してロボット制御装置２へ入力されることとなる。
Further, the
物体検知センサ１０１は、ターゲット部Ｔを検知するためのセンサであり、ターゲット部Ｔの動作範囲を十分にカバーする検知領域を備える。ここでは、物体検知センサ１０１がカメラであるものとするが、これに限ったものではなく、３次元計測が可能なレーザセンサ等を物体検知センサ１０１として用いてもよい。
The
物体検知センサ１０１によって撮像された画像の画像情報は、通信ネットワークを介してロボット制御装置２の角度計算部２１ａへリアルタイムに入力される。
Image information of an image captured by the
図２へ戻り、角度計算部２１ａについての説明を続ける。角度計算部２１ａは、物体検知センサ１０１から入力される画像情報に基づいてターゲット部Ｔの位置を検出する。これにより、ロボット制御装置２は、ターゲット部Ｔの位置をリアルタイムに認識することができる。なお、画像情報からターゲット部Ｔの位置を検出する手法としては、いずれの公知技術を用いてもかまわない。
Returning to FIG. 2, the description of the
つづいて、角度計算部２１ａは、検出したターゲット部Ｔの位置を目標位置として決定し、かかる目標位置と搾乳部１００の位置とを一致させるように第１アクチュエータ１１ａ〜第７アクチュエータ１７ａの角度をそれぞれ計算する。これにより、マニピュレータ１の次の位置姿勢が仮決定される。
Subsequently, the
なお、本実施例では、第４アクチュエータ１４ａ〜第７アクチュエータ１７ａの駆動により直線移動させることが可能な点を制御点としている。制御点とは、マニピュレータ１の動作位置を制御する位置である。具体的には、第６アクチュエータ１６ａの軸心付近に制御点が位置する。角度計算部２１ａは、目標位置に基づいて制御点の位置を決定した後、決定した制御点の位置に基づいて第１アクチュエータ１１ａ〜第７アクチュエータ１７ａの角度を計算する。なお、第１アクチュエータ１１ａ〜第７アクチュエータ１７ａの角度の計算には、逆キネマティクス演算等の一般的な手法を用いることができる。
In this embodiment, the control point is a point that can be linearly moved by driving the
角度計算部２１ａは、算出した各アクチュエータ１１ａ〜１７ａの角度データを位置修正部２１ｂへ渡す。
The
位置修正部２１ｂは、角度計算部２１ａによって計算された各アクチュエータ１１ａ〜１７ａの角度から特定されるマニピュレータ１の各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、禁止領域に含まれる部位の位置を禁止領域外の位置へ修正する処理部である。
The
具体的には、位置修正部２１ｂは、角度計算部２１ａから各アクチュエータ１１ａ〜１７ａの角度データを受け取ると、まず、受け取った角度データから各部位の移動後の位置座標を特定する。ここでは、第１リンク１１〜第６リンク１６、フランジ１７および第１アクチュエータ１１ａ〜第７アクチュエータ１７ａ等の位置座標を特定することとなる。
Specifically, when the
つづいて、位置修正部２１ｂは、特定した各部位の位置座標および記憶部２２に記憶された禁止領域データ２２ａを用い、移動によりマニピュレータ１が禁止領域に侵入することとなるか否かを判定する。禁止領域とは、障害物等との接触を避けるためにマニピュレータ１の侵入を禁止した領域である。禁止領域データ２２ａは、かかる禁止領域の位置座標を含んだデータである。
Subsequently, the
ここで、本実施例において設定される禁止領域について図４を用いて説明する。図４は、禁止領域の一例を示す図である。 Here, the prohibited areas set in the present embodiment will be described with reference to FIG. FIG. 4 is a diagram illustrating an example of the prohibited area.
図４に示すように、本実施例では、図示しないゲージに収容された乳牛Ｃの左後脚ＣＬおよび右後脚ＣＲが位置する領域として統計的に割り出された領域を覆うように、禁止領域Ａ１および禁止領域Ａ２がそれぞれ設定される。 As shown in FIG. 4, in this embodiment, it is prohibited to cover the area statistically determined as the area where the left hind leg CL and the right hind leg CR of the cow C housed in a gauge (not shown) are located. Area A1 and prohibited area A2 are set.
なお、本実施例では、マニピュレータ１が、乳牛Ｃの背後から乳牛Ｃの両後脚ＣＬ，ＣＲの間を通ってターゲット部Ｔへ向かうこととしている。このため、上記のように左後脚ＣＬおよび右後脚ＣＲの周囲を覆うような禁止領域Ａ１，Ａ２を設定することとしたが、禁止領域は、マニピュレータがどの位置からターゲット部へ向かうかによって適宜変更可能である。たとえば、マニピュレータが乳牛の左側面からターゲット部へ向かう仕様である場合には、左後脚の周囲を覆う領域のみを禁止領域として設定してもよい。
In the present embodiment, the
また、ここでは、禁止領域Ａ１，Ａ２を搾乳対象となる乳牛Ｃに依らず一様に適用する場合の例について説明するが、搾乳対象となる乳牛Ｃの体格に応じて禁止領域Ａ１，Ａ２を動的に変化させてもよい。 Moreover, although the example in the case of applying uniformly the prohibition area | region A1, A2 irrespective of the dairy cow C used as milking object is demonstrated here, according to the physique of the cow C used as milking object, prohibited area A1, A2 is described. It may be changed dynamically.
たとえば、乳牛Ｃに設けられたＩＤタグから乳牛Ｃの体格に関する情報（サイズや重量など）を取得し、取得した情報に基づいて禁止領域の大きさを変化させてもよい。このようにすれば、搾乳対象となる乳牛Ｃの大きさに合わせて禁止領域Ａ１，Ａ２を適切に設定することができる。 For example, information (size, weight, etc.) regarding the physique of the dairy cow C may be acquired from an ID tag provided on the dairy cow C, and the size of the prohibited area may be changed based on the acquired information. In this way, the prohibited areas A1 and A2 can be appropriately set according to the size of the cow C to be milked.
また、禁止領域Ａ１，Ａ２は、作業者が入力装置３を用いて手動で変更することもできる。禁止領域Ａ１，Ａ２の設定変更処理は、後述する設定処理部２１ｅによって行われる。
The prohibited areas A1 and A2 can be manually changed by the operator using the
図２へ戻り、位置修正部２１ｂの説明を続ける。位置修正部２１ｂは、移動後におけるマニピュレータ１の何れかの位置座標が、禁止領域Ａ１（または禁止領域Ａ２）に含まれる場合に、移動後のマニピュレータ１が禁止領域Ａ１（または禁止領域Ａ２）に侵入すると判定する。
Returning to FIG. 2, the description of the
移動後のマニピュレータ１が禁止領域Ａ１（または禁止領域Ａ２）に侵入すると判定すると、位置修正部２１ｂは、位置修正処理を行う。位置修正処理とは、移動によって禁止領域Ａ１（または禁止領域Ａ２）へ侵入することとなるマニピュレータ１の部位の位置を禁止領域Ａ１および禁止領域Ａ２外の位置へ修正する処理である。
If it is determined that the moved
ここで、位置修正部２１ｂにより実行される位置修正処理について図５−１および図５−２を用いて説明する。図５−１は、目標位置へ移動する前のマニピュレータ１を示す図であり、図５−２は、移動後のマニピュレータ１の位置が位置修正処理により修正される様子を示す図である。なお、図５−２では、目標位置への移動によってマニピュレータ１の第４リンク１４が禁止領域Ａ１へ侵入することとなる場合の例について示している。
Here, the position correction process executed by the
図５−１に示すように、ロボット制御装置２では、角度計算部２１ａが、物体検知センサ１０１から入力される画像情報に基づいて目標位置Ｓを決定するとともに、決定した目標位置Ｓに基づいて制御点の位置を決定する。
As illustrated in FIG. 5A, in the
そして、角度計算部２１ａは、現在の制御点の位置が決定した制御点の位置と一致するように各アクチュエータ１１ａ〜１７ａの角度を計算する。これにより、目標位置Ｓへ移動後のマニピュレータ１の位置姿勢（各部位の位置座標）が特定される。目標位置Ｓへ移動後のマニピュレータ１を図５−２において破線で示す。
Then, the
ここで、図５−２に示すように、目標位置Ｓへの移動によってマニピュレータ１の第４リンク１４が禁止領域Ａ１に侵入するとする。かかる場合、位置修正部２１ｂは、第４リンク１４の位置を禁止領域Ａ１外の位置へ修正する。
Here, as shown in FIG. 5B, it is assumed that the
具体的には、位置修正部２１ｂは、禁止領域Ａ１の境界面と第４リンク１４との間に仮想的なバネ・ダンパモデルを設定する。そして、位置修正部２１ｂは、第４リンク１４の移動によってバネ・ダンパモデルへ加わる力と、仮想バネの弾性力および仮想ダンパの粘性力とがつりあう位置（平衡点）を算出し、この平衡点の位置を第４リンク１４の移動後の位置として修正する。
Specifically, the
以下、位置修正処理の内容について図６を用いてさらに詳細に説明する。図６は、位置修正処理の動作例を示す図である。図６では、移動前のマニピュレータ１を実線で、移動後のマニピュレータ１を破線でそれぞれ示している。なお、位置修正部２１ｂは、各アクチュエータ１１ａ〜１７ａから出力されるエンコーダ値（各アクチュエータ１１ａ〜１７ａの回転位置）からマニピュレータ１の移動前の位置姿勢を特定することが可能である。
Hereinafter, the contents of the position correction process will be described in more detail with reference to FIG. FIG. 6 is a diagram illustrating an operation example of the position correction process. In FIG. 6, the
図６に示すように、位置修正部２１ｂは、まず、移動後の第４リンク１４と禁止領域Ａ１との交点Ｐ１の座標を算出する。つづいて、位置修正部２１ｂは、交点Ｐ１と、交点Ｐ１に位置することとなる第４リンク１４の移動前の位置Ｐ１’との間に、バネ・ダンパモデルＭを仮想的に設定する。かかるバネ・ダンパモデルＭは、記憶部２２に記憶されたバネ・ダンパモデルデータ２２ｂを用いて設定される。
As shown in FIG. 6, the
バネ・ダンパモデルデータ２２ｂは、仮想バネのバネ定数および自然長、仮想ダンパのダンパ定数等の数値を含んだデータである。なお、かかるバネ・ダンパモデルデータ２２ｂは、作業者等が入力装置３を操作することによって任意に設定変更することができる。
The spring /
つづいて、位置修正部２１ｂは、第４リンク１４の移動によってバネ・ダンパモデルＭに加わる力を算出する。そして、位置修正部２１ｂは、算出した力とバネ・ダンパモデルデータ２２ｂとして記憶されたバネ定数等の数値を用いて平衡点の位置座標を算出する。
Subsequently, the
具体的には、第４リンク１４の移動によってバネ・ダンパモデルＭに加わる力をＦ、仮想ダンパのダンパ定数をＤ、バネ定数をｋ、仮想バネの自然長をｘ０とすると、位置修正部２１ｂは、
位置修正部２１ｂは、かかる未知数ｘを算出することで、平衡点の位置座標を得ることができる。そして、位置修正部２１ｂは、かかる平衡点の位置座標を位置Ｐ１’の移動後の位置座標として再計算部２１ｃへ渡す。
The
図２へ戻り、再計算部２１ｃについて説明する。再計算部２１ｃは、位置修正部２１ｂから受け取った第４リンク１４の修正後の位置座標に基づいて、各アクチュエータ１１ａ〜１７ａの角度を再計算する処理部である。
Returning to FIG. 2, the
具体的には、再計算部２１ｃは、搾乳部１００が目標位置Ｓに位置した状態を維持しつつ、第４リンク１４の位置Ｐ１’が平衡点に位置するように、各アクチュエータ１１ａ〜１７ａの角度を再計算する。
Specifically, the
なお、マニピュレータ１は、冗長軸を備えるため、搾乳部１００が目標位置Ｓに位置した状態を維持しつつ、その他の部位の位置姿勢を変更することができる。再計算部２１ｃは、再計算後の各アクチュエータ１１ａ〜１７ａの角度を動作指示部２１ｄへ渡す。
In addition, since the
動作指示部２１ｄは、再計算部２１ｃから受け取った各アクチュエータ１１ａ〜１７ａの角度に基づいて各アクチュエータ１１ａ〜１７ａを回転駆動させる処理部である。これにより、マニピュレータ１は、図５−２において実線で示したように禁止領域Ａ１へ侵入することなく目標位置Ｓへ移動することとなる。
The
設定処理部２１ｅは、入力装置３からの指示に従って禁止領域データ２２ａまたはバネ・ダンパモデルデータ２２ｂの内容を変更する処理部である。具体的には、設定処理部２１ｅは、入力装置３からの指示に従って、禁止領域Ａ１，Ａ２の大きさや位置を変更したり、仮想バネのバネ定数や自然長、仮想ダンパのダンパ定数等を変更したりする。
The setting
なお、設定処理部２１ｅは、上述したように、外部から乳牛Ｃの体格に関する情報を取得し、取得した情報に基づいて禁止領域データ２２ａの内容を変更するようにしてもよい。
As described above, the setting
記憶部２２は、不揮発性メモリやハードディスクドライブといった記憶デバイスで構成され、禁止領域データ２２ａと、バネ・ダンパモデルデータ２２ｂとを記憶する。
The
なお、本実施例では、ロボット制御装置２と入力装置３とを別体として説明したが、これに限ったものではなく、ロボット制御装置２と入力装置３とを１つの装置として構成してもよい。また、本実施例では、物体検知センサ１０１からの画像情報に基づいて目標位置を決定する処理をロボット制御装置２で行うこととしたが、かかる処理を他の装置で行うようにしてもよい。
In the present embodiment, the
次に、ロボット制御装置２の具体的動作について図７を用いて説明する。図７は、ロボット制御装置２が実行する処理手順を示すフローチャートである。
Next, a specific operation of the
図７に示すように、ロボット制御装置２では、角度計算部２１ａが、物体検知センサ１０１からの画像情報を取得し（ステップＳ１０１）、取得した画像情報に基づいて目標位置を決定する（ステップＳ１０２）。また、角度計算部２１ａは、決定した目標位置に基づいて各アクチュエータ１１ａ〜１７ａの角度を計算する（ステップＳ１０３）。
As shown in FIG. 7, in the
つづいて、ロボット制御装置２では、位置修正部２１ｂが、マニピュレータ１の各部位の移動後の位置を特定し（ステップＳ１０４）、特定した各部位の位置が禁止領域Ａ１（または禁止領域Ａ２）に含まれるか否かを判定する（ステップＳ１０５）。
Subsequently, in the
かかる処理において、マニピュレータ１の各部位の移動後の位置が禁止領域Ａ１（または禁止領域Ａ２）に含まれない場合（ステップＳ１０５，Ｎｏ）、動作指示部２１ｄは、ステップＳ１０３において算出された各アクチュエータ１１ａ〜１７ａの角度に基づいてマニピュレータ１に対して動作指示を行って（ステップＳ１０８）、処理を終了する。
In such a process, when the moved position of each part of the
一方、マニピュレータ１の各部位の移動後の位置が禁止領域Ａ１（または禁止領域Ａ２）に含まれると判定した場合（ステップＳ１０５，Ｙｅｓ）、位置修正部２１ｂは、バネ・ダンパモデルデータ２２ｂを用いて位置修正処理を行う（ステップＳ１０６）。
On the other hand, when it is determined that the moved position of each part of the
そして、ロボット制御装置２では、再計算部２１ｃが、修正後の位置に基づいて各アクチュエータ１１ａ〜１７ａの角度を再計算し（ステップＳ１０７）、動作指示部２１ｄが、各アクチュエータ１１ａ〜１７ａの再計算後の角度に基づいてマニピュレータ１に対して動作指示を行って（ステップＳ１０８）、処理を終了する。
In the
なお、ここでは、ステップＳ１０３において第１アクチュエータ１１ａ〜第７アクチュエータ１７ａの角度を全て計算することとしたが、これに限ったものではない。たとえば、ステップＳ１０３では基本軸（たとえば、第１アクチュエータ１１ａ〜第４アクチュエータ１４ａ）の角度のみを計算し、ステップＳ１０７において基本軸の角度の再計算を行った後、残りの軸（たとえば、第５アクチュエータ１５ａ〜第７アクチュエータ１７ａ）の角度を計算することとしてもよい。
Here, all the angles of the
上述してきたように、本実施例では、角度計算部が、目標位置に基づいて各アクチュエータの角度を計算し、位置修正部が、角度計算部によって計算された各アクチュエータの角度から特定されるマニピュレータの各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、禁止領域に含まれる部位の位置を禁止領域外の位置へ修正し、再計算部が、位置修正部によって修正された部位の位置に基づいて各アクチュエータの角度を再計算することとした。したがって、冗長自由度を有するマニピュレータを適切に制御することができる。 As described above, in this embodiment, the angle calculation unit calculates the angle of each actuator based on the target position, and the position correction unit is identified from the angle of each actuator calculated by the angle calculation unit. When any of the positions of each of the parts is included in the predetermined prohibited area, the position of the part included in the prohibited area is corrected to a position outside the prohibited area, and the recalculation unit is corrected by the position correcting unit The angle of each actuator was recalculated based on the position of the part. Accordingly, it is possible to appropriately control a manipulator having a redundancy degree of freedom.
また、本実施例では、位置修正部が、禁止領域に含まれることとなる部位と禁止領域の境界面との間に仮想的なバネ・ダンパモデルを設定し、当該部位がバネ・ダンパモデルへ作用させる力とバネ・ダンパモデルが当該部位へ作用させる力とがつりあう位置を当該部位の修正後の位置とすることとした。 Further, in this embodiment, the position correction unit sets a virtual spring / damper model between the part that is included in the prohibited area and the boundary surface of the prohibited area, and the part is changed to the spring / damper model. The position where the force to be applied and the force applied by the spring / damper model to the part is determined as the corrected position of the part.
これにより、マニピュレータの修正後の位置姿勢が一意に決定されるため、たとえば、マニピュレータの修正後の位置姿勢をシミュレーション等により探索的に決定する場合と比較して演算量を削減することができる。 As a result, the corrected position and orientation of the manipulator are uniquely determined. Therefore, for example, it is possible to reduce the amount of calculation compared to a case where the corrected position and orientation of the manipulator is determined exploratoryly by simulation or the like.
したがって、本実施例のように作業対象物の位置が変化する場合であっても、障害物等との接触を適切に回避しつつ、マニピュレータを作業対象物に対してリアルタイムに追従させることが可能となる。 Therefore, even when the position of the work object changes as in the present embodiment, it is possible to cause the manipulator to follow the work object in real time while appropriately avoiding contact with an obstacle or the like. It becomes.
また、本実施例では、位置修正部が、禁止領域に含まれることとなる部位と禁止領域との交点を算出し、算出した交点に基づいてバネ・ダンパモデルの設定位置を決定することとした。したがって、バネ・ダンパモデルの設定位置を少ない演算量で決定することができる。 Further, in the present embodiment, the position correcting unit calculates the intersection between the part that is included in the prohibited region and the prohibited region, and determines the setting position of the spring / damper model based on the calculated intersection. . Therefore, the setting position of the spring / damper model can be determined with a small amount of calculation.
ところで、上述してきた実施例では、移動により禁止領域へ侵入することとなるリンクと禁止領域との交点をバネ・ダンパモデルの設定位置として決定することとしたが、バネ・ダンパモデルの設定位置は、これに限ったものではない。 By the way, in the embodiment described above, the intersection of the link and the prohibited area that will enter the prohibited area by movement is determined as the setting position of the spring / damper model, but the setting position of the spring / damper model is This is not the only one.
以下では、バネ・ダンパモデルの設定位置の他の例について図８を用いて説明する。図８は、バネ・ダンパモデルの設定位置の他の例を示す図である。なお、図８では、移動の途中で禁止領域Ａ１に接した状態となったマニピュレータ１を一点鎖線で、移動後のマニピュレータ１を破線でそれぞれ示している。
Hereinafter, another example of the setting position of the spring / damper model will be described with reference to FIG. FIG. 8 is a diagram showing another example of the setting position of the spring / damper model. In FIG. 8, the
図８に示すように、禁止領域Ａ１に侵入することとなる第４リンク１４と禁止領域Ａ１との交点をそれぞれＰ１，Ｐ２とすると、バネ・ダンパモデルＭの第４リンク１４側への設定位置は、交点Ｐ１，Ｐ２に限らず、第４リンク１４の線分Ｐ１，Ｐ２上の何れの位置であってもよい。
As shown in FIG. 8, when the intersections of the
たとえば、交点Ｐ１および交点Ｐ２の中点Ｐ３を、バネ・ダンパモデルＭの第４リンク１４側への設定位置としてもよいし、第４リンク１４が禁止領域Ａ１と最初に接する点Ｐ４をバネ・ダンパモデルＭの第４リンク１４側への設定位置としてもよい。
For example, the midpoint P3 of the intersection point P1 and the intersection point P2 may be set as the setting position on the
また、バネ・ダンパモデルＭの禁止領域Ａ１側への設定位置についても、禁止領域Ａ１の線分Ｐ１，Ｐ２上の何れの位置であってもよい。 Further, the set position of the spring / damper model M toward the prohibited area A1 may be any position on the line segments P1 and P2 of the prohibited area A1.
また、上述してきた実施例では、搾乳対象となる乳牛に依らず禁止領域を固定とする（もしくは、乳牛の体格に応じて動的に変更する）こととしたが、これに限ったものではない。 In the embodiment described above, the prohibited area is fixed (or dynamically changed according to the physique of the dairy cow) regardless of the dairy cow to be milked, but is not limited thereto. .
たとえば、作業対象物である乳牛の乳頭と障害物である両後脚とは連繋しているため、乳頭の位置がずれれば両後脚の位置もずれる可能性がある。そこで、物体検知センサからの画像情報に基づいて決定されるターゲット部の位置に応じて、禁止領域の位置または大きさを動的に変更するようにしてもよい。 For example, since the nipple of the dairy cow that is the work object and the two hind legs that are the obstacles are connected, the position of both hind legs may be shifted if the position of the nipple is shifted. Therefore, the position or size of the prohibited area may be dynamically changed according to the position of the target portion determined based on the image information from the object detection sensor.
このようにすれば、マニピュレータ１と乳牛の後脚との接触をより確実に回避することができる。
In this way, contact between the
また、上述してきた実施例では、マニピュレータの一例として、７個のアクチュエータを有する７自由度のマニピュレータを用いて説明したが、マニピュレータの自由度は、これに限ったものではなく、８自由度以上の自由度を有するマニピュレータであってもよい。 In the above-described embodiments, a 7-degree-of-freedom manipulator having seven actuators has been described as an example of a manipulator. However, the degree of freedom of the manipulator is not limited to this, and the degree of freedom is 8 degrees or more. It may be a manipulator having a degree of freedom.
また、６自由度以下のマニピュレータであっても、作業対象物へのアプローチに１以上の自由度が許容される場合には、相対的にマニピュレータが冗長自由度を有することになるが、そのような場合であっても本発明を適用することができる。 Further, even if the manipulator has 6 degrees of freedom or less, if one or more degrees of freedom are allowed in the approach to the work object, the manipulator has a relative degree of freedom. Even in such a case, the present invention can be applied.
さらなる効果や変形例は、当業者によって容易に導き出すことができる。このため、本発明のより広範な態様は、以上のように表しかつ記述した特定の詳細および代表的な実施の形態に限定されるものではない。したがって、添付の特許請求の範囲およびその均等物によって定義される総括的な発明の概念の精神または範囲から逸脱することなく、様々な変更が可能である。 Further effects and modifications can be easily derived by those skilled in the art. Thus, the broader aspects of the present invention are not limited to the specific details and representative embodiments shown and described above. Accordingly, various modifications can be made without departing from the spirit or scope of the general inventive concept as defined by the appended claims and their equivalents.
Ｃ 乳牛
Ｔ ターゲット部
Ｗ 設置面
ＣＬ 左後脚
ＣＲ 右後脚
Ａ１ 禁止領域（左後脚側）
Ａ２ 禁止領域（右後脚側）
Ｍ バネ・ダンパモデル
１ マニピュレータ
１１ 第１リンク
１２ 第２リンク
１３ 第３リンク
１４ 第４リンク
１５ 第５リンク
１６ 第６リンク
１７ フランジ
１１ａ 第１アクチュエータ
１２ａ 第２アクチュエータ
１３ａ 第３アクチュエータ
１４ａ 第４アクチュエータ
１５ａ 第５アクチュエータ
１６ａ 第６アクチュエータ
１７ａ 第７アクチュエータ
１００ 搾乳部
１０１ 物体検知センサ
２ ロボット制御装置
２１ 制御部
２１ａ 角度計算部
２１ｂ 位置修正部
２１ｃ 再計算部
２１ｄ 動作指示部
２１ｅ 設定処理部
２２ 記憶部
２２ａ 禁止領域データ
２２ｂ バネ・ダンパモデルデータ
３ 入力装置
C Dairy cow T Target part W Installation surface CL Left hind leg CR Right hind leg A1 Prohibited area (left hind leg side)
A2 prohibited area (right rear leg side)
M spring /
Claims (7)
目標位置に基づいて前記関節軸の角度を計算する角度計算部と、
前記角度計算部によって計算された前記関節軸の角度から特定される前記マニピュレータの各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、前記禁止領域に含まれる前記部位の位置を前記禁止領域外の位置へ修正する位置修正部と、
前記位置修正部によって修正された前記部位の位置に基づいて前記関節軸の角度を再計算する再計算部と
を備えることを特徴とするロボット制御装置。 A robot control device for controlling a manipulator having a plurality of joint axes,
An angle calculator that calculates an angle of the joint axis based on a target position;
When any of the positions of each part of the manipulator specified from the angle of the joint axis calculated by the angle calculation unit is included in a predetermined prohibited area, the position of the part included in the prohibited area is determined. A position correction unit for correcting to a position outside the prohibited area;
A robot control device comprising: a recalculation unit that recalculates an angle of the joint axis based on the position of the part corrected by the position correction unit.
前記禁止領域に含まれることとなる前記部位と前記禁止領域の境界面との間に仮想的なバネ・ダンパモデルを設定し、前記部位が前記バネ・ダンパモデルへ作用させる力と前記バネ・ダンパモデルが前記部位へ作用させる力とがつりあう位置を前記部位の修正後の位置とすることを特徴とする請求項１に記載のロボット制御装置。 The position correcting unit is
A virtual spring / damper model is set between the part to be included in the forbidden area and a boundary surface of the forbidden area, and the force that the part acts on the spring / damper model and the spring / damper The robot control apparatus according to claim 1, wherein a position where the force applied to the part by the model balances is a corrected position of the part.
前記禁止領域に含まれることとなる前記部位と前記禁止領域との交点を算出し、算出した交点に基づいて前記バネ・ダンパモデルの設定位置を決定することを特徴とする請求項２に記載のロボット制御装置。 The position correcting unit is
The intersection of the said part and the prohibition area | region which will be contained in the said prohibition area is calculated, The setting position of the said spring damper model is determined based on the calculated intersection. Robot control device.
作業対象物に関する情報を取得するとともに、取得した前記作業対象物に関する情報に基づいて前記禁止領域を変更することを特徴とする請求項４に記載のロボット制御装置。 The setting processing unit
The robot control apparatus according to claim 4, wherein the robot control apparatus acquires information related to a work target and changes the prohibited area based on the acquired information related to the work target.
前記ロボット制御装置は、
目標位置に基づいて前記関節軸の角度を計算する角度計算部と、
前記角度計算部によって計算された前記関節軸の角度から特定される前記マニピュレータの各部位の位置のうち何れかが所定の禁止領域に含まれる場合に、前記禁止領域に含まれる前記部位の位置を前記禁止領域外の位置へ修正する位置修正部と、
前記位置修正部によって修正された前記部位の位置に基づいて前記関節軸の角度を再計算する再計算部と
を備えることを特徴とするロボットシステム。 A robot system comprising a manipulator having a plurality of joint axes and a robot controller for controlling the manipulator,
The robot controller is
An angle calculator that calculates an angle of the joint axis based on a target position;
When any of the positions of each part of the manipulator specified from the angle of the joint axis calculated by the angle calculation unit is included in a predetermined prohibited area, the position of the part included in the prohibited area is determined. A position correction unit for correcting to a position outside the prohibited area;
A robot system comprising: a recalculation unit that recalculates an angle of the joint axis based on the position of the part corrected by the position correction unit.
をさらに備え、
前記ロボット制御装置は、
前記入力装置からの指示に従って前記禁止領域または前記バネ・ダンパモデルの設定変更を行うことを特徴とする請求項６に記載のロボットシステム。 An input device that accepts input operations;
The robot controller is
The robot system according to claim 6, wherein the setting of the prohibited area or the spring / damper model is changed in accordance with an instruction from the input device.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011111071A JP2012240144A (en) | 2011-05-18 | 2011-05-18 | Robot control device and robot system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011111071A JP2012240144A (en) | 2011-05-18 | 2011-05-18 | Robot control device and robot system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2012240144A true JP2012240144A (en) | 2012-12-10 |
Family
ID=47462375
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2011111071A Pending JP2012240144A (en) | 2011-05-18 | 2011-05-18 | Robot control device and robot system |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2012240144A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014018912A (en) * | 2012-07-18 | 2014-02-03 | Seiko Epson Corp | Robot control device, robot control method, robot control program and robot system |
| JP2016107384A (en) * | 2014-12-09 | 2016-06-20 | トヨタ自動車株式会社 | Interference avoidance method, control device and program |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05345291A (en) * | 1992-06-15 | 1993-12-27 | Fanuc Ltd | Working area limitation for robot |
| JP2007203380A (en) * | 2006-01-30 | 2007-08-16 | Kawasaki Heavy Ind Ltd | Instruction support device for robot |
| JP2010052093A (en) * | 2008-08-28 | 2010-03-11 | Kobe Steel Ltd | Method for avoiding interference of robot and robot system |
| JP2010152684A (en) * | 2008-12-25 | 2010-07-08 | Toshiba Corp | Method and apparatus for generating trajectory of mobile body |
| JP2011011263A (en) * | 2009-06-30 | 2011-01-20 | Fanuc Ltd | Off-line program creation device of robot with additional axis |
-
2011
- 2011-05-18 JP JP2011111071A patent/JP2012240144A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05345291A (en) * | 1992-06-15 | 1993-12-27 | Fanuc Ltd | Working area limitation for robot |
| JP2007203380A (en) * | 2006-01-30 | 2007-08-16 | Kawasaki Heavy Ind Ltd | Instruction support device for robot |
| JP2010052093A (en) * | 2008-08-28 | 2010-03-11 | Kobe Steel Ltd | Method for avoiding interference of robot and robot system |
| JP2010152684A (en) * | 2008-12-25 | 2010-07-08 | Toshiba Corp | Method and apparatus for generating trajectory of mobile body |
| JP2011011263A (en) * | 2009-06-30 | 2011-01-20 | Fanuc Ltd | Off-line program creation device of robot with additional axis |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2014018912A (en) * | 2012-07-18 | 2014-02-03 | Seiko Epson Corp | Robot control device, robot control method, robot control program and robot system |
| JP2016107384A (en) * | 2014-12-09 | 2016-06-20 | トヨタ自動車株式会社 | Interference avoidance method, control device and program |
| CN105690410A (en) * | 2014-12-09 | 2016-06-22 | 丰田自动车株式会社 | Collision avoidance method, control device, and program |
| US9827675B2 (en) | 2014-12-09 | 2017-11-28 | Toyota Jidosha Kabushiki Kaisha | Collision avoidance method, control device, and program |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107921637B (en) | Robot system | |
| JP6420229B2 (en) | A robot system including a video display device that superimposes and displays an image of a virtual object on a video of a robot | |
| JP6963748B2 (en) | Robot system and robot system control method | |
| JP4844453B2 (en) | Robot teaching apparatus and teaching method | |
| US9221176B2 (en) | Robot system and method for controlling the same | |
| US20220388160A1 (en) | Control device, control system, robot system, and control method | |
| JP5458769B2 (en) | Robot control device | |
| JPWO2018092243A1 (en) | Work position correction method and work robot | |
| JP7007791B2 (en) | Robot driving methods, computer programs, and robot systems | |
| CN114845841B (en) | Control method, control device, robot system, program, and recording medium | |
| JP2012240144A (en) | Robot control device and robot system | |
| JP2011140084A (en) | Controller, robot, robot system, and following method of robot | |
| US20170028548A1 (en) | Robot control device and robot control method | |
| WO2020027106A1 (en) | Robot system | |
| JP7447568B2 (en) | Simulation equipment and programs | |
| JP2006318301A (en) | Adaptive visual feedback control method | |
| JP7518610B2 (en) | Control device, control system, robot system, and control method | |
| WO2023053374A1 (en) | Control device and robot system | |
| CN116330303B (en) | SCARA robot motion control method, SCARA robot motion control device, SCARA robot motion control terminal equipment and SCARA robot motion control medium | |
| US20210299869A1 (en) | Teaching Method | |
| JP2024000289A (en) | Control method and control device of robot system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20130201 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20131121 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20140107 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20140430 |