JP2012040633A - Method for determining redundant degree of freedom of redundant manipulator - Google Patents

Method for determining redundant degree of freedom of redundant manipulator Download PDF

Info

Publication number
JP2012040633A
JP2012040633A JP2010182804A JP2010182804A JP2012040633A JP 2012040633 A JP2012040633 A JP 2012040633A JP 2010182804 A JP2010182804 A JP 2010182804A JP 2010182804 A JP2010182804 A JP 2010182804A JP 2012040633 A JP2012040633 A JP 2012040633A
Authority
JP
Japan
Prior art keywords
freedom
redundant
degree
determining
redundancy
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
Application number
JP2010182804A
Other languages
Japanese (ja)
Inventor
Koji Terada
耕志 寺田
Kunimatsu Hashimoto
国松 橋本
Kodai Suzuki
航大 鈴木
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Motor Corp
Original Assignee
Toyota Motor Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Toyota Motor Corp filed Critical Toyota Motor Corp
Priority to JP2010182804A priority Critical patent/JP2012040633A/en
Publication of JP2012040633A publication Critical patent/JP2012040633A/en
Pending legal-status Critical Current

Links

Images

Abstract

PROBLEM TO BE SOLVED: To provide a technology that secures the continuity of an arm angle ψ when changing a hand tip 8 of a redundant manipulator 1 by CP control.SOLUTION: A control device 100 includes: a state acquisition device 20 for acquiring a position and a posture state of the hand tip 8 in each step; an area calculation device 21 for calculating an area in which the arm angle ψ is possible in each step based on the position and the posture state of the hand tip 8 in each step; a combination creation device 22 for creating a combination of areas from a step s0 to a step sn so that areas of adjacent steps are mutually overlapped at least at a part thereof between the adjacent steps; and a redundant degree of a freedom determiner 23 for determining the arm angle ψ in each step of the CP control based on the combination of areas.

Description

本発明は、CP(Continuous Point)制御される冗長マニピュレータの各ステップにおける冗長自由度の決定方法、決定装置、及び、決定プログラムに関する。   The present invention relates to a redundancy degree determination method, a determination device, and a determination program for each step of a redundant manipulator controlled by CP (Continuous Point).

作業に必要な自由度より多くの自由度を有する冗長マニピュレータは、近年、ロボット分野において広く用いられるようになってきている。これは、冗長マニピュレータが有する余分な自由度を有効活用することで、例えば、ロボットの可操作性が向上したり、関節トルクの最適化が図れたり、障害物を回避できたり、所謂特異点を回避できたり、関節限界を回避できたりするからである。   In recent years, redundant manipulators having more degrees of freedom than those required for work have become widely used in the robot field. This is because the redundant manipulator can effectively utilize the extra degrees of freedom, for example, to improve the operability of the robot, optimize joint torque, avoid obstacles, and so-called singularities. This is because it can be avoided and joint limits can be avoided.

非特許文献1は、このような冗長マニピュレータの一例として、7自由度冗長マニピュレータについて言及している。一般に、空間内におけるマニピュレータの手先の位置及び姿勢は、6つのパラメータで表現することができる。従って、7自由度マニピュレータには余分な自由度が1つ存在していることになる。   Non-Patent Document 1 refers to a 7-DOF redundant manipulator as an example of such a redundant manipulator. In general, the position and posture of the hand of the manipulator in the space can be expressed by six parameters. Therefore, one extra degree of freedom exists in the seven degree of freedom manipulator.

先ずは、この7自由度冗長マニピュレータについて図22を参照しつつ説明する。図22に示すように、7自由度冗長マニピュレータの関節は、すべて回転関節で構成されている。肩部の三関節(第1〜第3関節、θ1〜θ3)の関節軸は一点で交差している。同様に、手首部の三関節(第5〜第7関節、θ5〜θ7)の関節軸も一点で交差している。また、隣り合う一対の関節の軸同士は、互いに直交する。このように肩部の三関節と手首部の三関節は、単純な構造をしているため、夫々仮想的な球面機構を構成しているとみなすことができる。そこで、この種のマニピュレータは、一般に、3R−1R−3R型或いはS−R−S型のマニピュレータとして分類されている。   First, the seven-degree-of-freedom redundant manipulator will be described with reference to FIG. As shown in FIG. 22, the joints of the seven-degree-of-freedom redundant manipulator are all composed of rotary joints. The joint axes of the three shoulder joints (first to third joints, θ1 to θ3) intersect at one point. Similarly, the joint axes of the three joints (fifth to seventh joints, θ5 to θ7) of the wrist also intersect at one point. Moreover, the axes of a pair of adjacent joints are orthogonal to each other. Thus, the three joints of the shoulder and the wrist have a simple structure, and can be regarded as constituting a virtual spherical mechanism. Therefore, this type of manipulator is generally classified as a 3R-1R-3R type or SRS type manipulator.

更に、上記の非特許文献1は、冗長自由度を表現するためのパラメータとして、非特許文献2に開示のアームアングルψを採用している。このアームアングルψは、図23に示すようにアーム平面と参照面との成す角度として定義されるものである。アーム平面は、肩部三関節の交点Ps、肘関節位置Pe及び手首部三関節の交点Pwの三点を通る平面である。もし関節の可動範囲に制限がなければ、手先の位置及び姿勢を固定したまま、アームアングルψのみを自由自在に変えることができよう。しかしながら、実在のマニピュレータでは、機構的な制約のため、関節の可動範囲には制限がある。この結果、アームアングルψの実現可能な領域は自ずと限定されることになる。これに対し、非特許文献1は、アームアングルψの実現可能な領域を代数学的に解く手法を提案している。この手法の詳細は、非特許文献1を参照されたい。要するに、非特許文献1によれば、アームアングルψの実現可能な領域は、1つ又は複数の閉じた領域から構成され得るとしている。   Further, the non-patent document 1 employs the arm angle ψ disclosed in the non-patent document 2 as a parameter for expressing the redundancy degree of freedom. The arm angle ψ is defined as an angle formed by the arm plane and the reference plane as shown in FIG. The arm plane is a plane that passes through the three points of the intersection point Ps of the shoulder three joints, the elbow joint position Pe, and the intersection point Pw of the wrist three joints. If the movable range of the joint is not limited, only the arm angle ψ can be freely changed while the position and posture of the hand are fixed. However, in an actual manipulator, the movable range of the joint is limited due to mechanical limitations. As a result, the realizable region of the arm angle ψ is naturally limited. On the other hand, Non-Patent Document 1 proposes a method for algebraically solving a realizable region of the arm angle ψ. See Non-Patent Document 1 for details of this method. In short, according to Non-Patent Document 1, the realizable region of the arm angle ψ can be composed of one or a plurality of closed regions.

清水昌幸、他4名、「関節の可動範囲を考慮に入れた7自由度冗長マニヒ゜ュレータの解析的逆運動学解法」、日本ロホ゛ット学会誌、日本ロホ゛ット学会、2007年、第25巻、第4号、p.122-133Masayuki Shimizu and 4 others, “Analytical Inverse Kinematic Solution of 7 DOF Redundant Manipulator Taking Account of Joint Movement Range”, Journal of the Robotics Society of Japan, The Robotics Society of Japan, 2007, Vol. 25, No. 4 , P.122-133 K.Kreutz-Delgado, M.Long and H. Seraji: "Kinematic Analysis of 7-DOF Manipulators," Int.J.Robotics Research, 1992, vol.11, no.5, pp.469-481.K. Kreutz-Delgado, M. Long and H. Seraji: "Kinematic Analysis of 7-DOF Manipulators," Int. J. Robotics Research, 1992, vol.11, no.5, pp.469-481.

前述の通り、アームアングルψの実現可能な領域は、1つ又は複数の閉じた領域から構成される。しかし、このようなアームアングルψの実現可能な領域と、CP(Continuous Point)制御と、の間には未解決な問題があった。ここで、CP制御とは、図24に示すように、(1)第1の位置及び姿勢状態("s0"を参照)から第2の位置及び姿勢状態("s6"を参照)に至るまでの、マニピュレータの手先の位置及び姿勢状態を例えば所定の時間間隔毎に細かく設定し、(2)各手先位置毎に、逆運動学を用いて各関節の角度を求め、(3)(2)で求めた各関節の角度をロボットによって再生させる、制御のことである。しかし、冗長マニピュレータは冗長な構成であるがため、上記(2)において各関節の角度が一意に定まらず、冗長分の自由度を何らかの基準に基づいて予め決定する必要がある。以下、図24及び図25を参照しつつ、この問題を詳細に説明する。なお、説明の便宜上、冗長自由度を表すパラメータとしてアームアングルψを継続して使用するが、冗長自由度を表すパラメータとしてアームアングルψを使用しなければならない特段の事情はなく、他のパラメータを代わりに使用してもよいことに留意されたい。   As described above, the realizable region of the arm angle ψ is configured by one or a plurality of closed regions. However, there is an unsolved problem between such an area where the arm angle ψ can be realized and CP (Continuous Point) control. Here, as shown in FIG. 24, the CP control is from (1) the first position and posture state (see “s0”) to the second position and posture state (see “s6”). The position and posture state of the hand of the manipulator are finely set, for example, at predetermined time intervals, and (2) the angle of each joint is obtained using inverse kinematics for each hand position, (3) (2) This is the control that causes the robot to reproduce the angles of the joints obtained in step 1. However, since the redundant manipulator has a redundant configuration, the angle of each joint is not uniquely determined in the above (2), and the degree of freedom for the redundancy needs to be determined in advance based on some criterion. Hereinafter, this problem will be described in detail with reference to FIGS. For convenience of explanation, the arm angle ψ is continuously used as a parameter representing the degree of redundancy, but there is no particular circumstance in which the arm angle ψ must be used as a parameter representing the degree of redundancy, and other parameters are used. Note that it may be used instead.

図25には、図24で示した各ステップにおけるアームアングルψの実現可能な領域を示している。図25の横軸に並べたs0〜s6は、図24に示すステップs0〜s6に夫々対応している。図25の縦軸は、アームアングルψである。図25において、上付添字と下付添字を有する「ψ」は、アームアングルψの実現可能な領域(範囲)を意味している。「ψ」の上付添字は、その領域が属するステップを識別するための番号を意味する。一方、「ψ」の下付添字は、その領域が属するステップにおける各領域の識別番号である。確認のため繰り返すと、「ψ」はアームアングルを意味するが、上付添字と下付添字を伴った「ψ」はアームアングルψの実現可能な領域を意味している。文章中では、このアームアングルψの実現可能な領域を、便宜上、ψ(上付添字,下付添字)のようにして表記する。   FIG. 25 shows a feasible region of the arm angle ψ in each step shown in FIG. S0 to s6 arranged on the horizontal axis in FIG. 25 correspond to steps s0 to s6 shown in FIG. The vertical axis in FIG. 25 is the arm angle ψ. In FIG. 25, “ψ” having a superscript and a subscript means a realizable region (range) of the arm angle ψ. The superscript “ψ” means a number for identifying the step to which the area belongs. On the other hand, the subscript “ψ” is an identification number of each area in the step to which the area belongs. To repeat for confirmation, “ψ” means an arm angle, but “ψ” with a superscript and a subscript means a realizable region of the arm angle ψ. In the text, the realizable area of the arm angle ψ is expressed as ψ (superscript, subscript) for convenience.

さて、従来では、冗長分の自由度を予め決定するための基準に関する知見は一切なかった。従って、ステップ0におけるアームアングルψを予め決定するための基準に関する知見も同様になかった。それでは、本願発明者らはどうやってステップ0におけるアームアングルψを決定していたか。本願発明者らに対するインタビューによれば、以下の通りである。即ち、本願発明者らは、先ず、ステップ0におけるψ(0,1)とψ(0,2)の大小を比較し、図25の場合ではψ(0,1)>ψ(0,2)であることから、この単純な大小関係に基づいてψ(0,1)を選択していたのである。そして、本願発明者らは、このψ(0,1)の中間値をステップ0におけるアームアングルψとして採用していた。また、次のステップ1におけるアームアングルψについて言えば、本願発明者らは、アームアングルψの一応の連続性を考慮してψ(0,1)とψ(1,1)との重複関係を重視することで、ψ(1,1)とψ(1,2)のうちψ(1,1)を選択していた。そして、本願発明者らは、このψ(1,1)の中間値をステップ1におけるアームアングルψとして採用していた。   Conventionally, there has been no knowledge about the criteria for predetermining the degree of freedom for redundancy. Therefore, there was no knowledge about the criteria for predetermining the arm angle ψ in step 0 as well. So how did the inventors determine the arm angle ψ at step 0? According to an interview with the inventors of the present application, it is as follows. That is, the inventors first compare the magnitudes of ψ (0,1) and ψ (0,2) in step 0, and in the case of FIG. 25, ψ (0,1)> ψ (0,2). Therefore, ψ (0, 1) was selected based on this simple magnitude relationship. The inventors of the present application have adopted the intermediate value of ψ (0,1) as the arm angle ψ in step 0. Further, regarding the arm angle ψ in the next step 1, the inventors of the present invention consider the overlapping relationship between ψ (0,1) and ψ (1,1) in consideration of the temporary continuity of the arm angle ψ. Emphasis was placed on ψ (1,1) among ψ (1,1) and ψ (1,2). The inventors of the present application have adopted the intermediate value of ψ (1,1) as the arm angle ψ in step 1.

このような場当たり的な基準は、それでも一見すると合理的であるかのようにも見える。しかしながら、この基準によると、ステップ4からステップ5へと移行するときに大きな問題が生じる。というのは、確かに、ψ(0,1)にとっては次のステップ1において自己と重複するアームアングルψの実現可能な領域が存在していた。同様に、ψ(1,1)にとっては次のステップ2において自己と重複するアームアングルψの実現可能な領域が存在していた。しかしながら、ψ(4,1)にとっては次のステップ5において自己と重複するアームアングルψの実現可能な領域が存在していない。従って、仕方なく、アームアングルψの連続性を犠牲にして、唯一存在するψ(5,1)の中から、ステップ5におけるアームアングルψを選択し採用せざるを得なかった。そして、このようにアームアングルψの値が看過できない程に大きく変化する結果、各関節の角速度が有限であることとの兼ね合いもあり、手先が意図した軌道から大きく外れたり、手先が他の物体と干渉したり、といった種々の問題が発生していた。   Such ad hoc standards still seem reasonable at first glance. However, according to this standard, a big problem arises when moving from step 4 to step 5. This is because, for ψ (0,1), there is a realizable region of arm angle ψ that overlaps with itself in the next step 1. Similarly, for ψ (1,1), there is a realizable region of the arm angle ψ that overlaps with itself in the next step 2. However, for ψ (4, 1), there is no realizable region of arm angle ψ that overlaps with itself in the next step 5. Therefore, at the expense of the continuity of the arm angle ψ, the arm angle ψ in step 5 has to be selected and adopted from among the only existing ψ (5,1). As a result, the value of the arm angle ψ changes so much that it cannot be overlooked. As a result, the angular velocity of each joint is also finite, and the hand is greatly deviated from the intended trajectory, or the hand is moved to another object. There were various problems such as interference.

本願発明の目的は、冗長マニピュレータの手先の位置及び姿勢状態を、CP制御にて、第1の位置及び姿勢状態から第2の位置及び姿勢状態へと変化させるに際し、冗長マニピュレータの冗長自由度の連続性を担保する技術を提供することにある。ここで、冗長自由度の連続性とは、「冗長自由度の値の変化がステップ間で緩やかである」ことを意味する。   The object of the present invention is to change the position and posture state of the hand of the redundant manipulator from the first position and posture state to the second position and posture state by CP control. The purpose is to provide technology that guarantees continuity. Here, the continuity of the redundancy degrees of freedom means that “the change in the value of the redundancy degrees of freedom is gentle between steps”.

本願発明の第1の観点によれば、冗長マニピュレータの手先の位置及び姿勢状態を、CP(Continuous Point)制御にて、第1の位置及び姿勢状態から第2の位置及び姿勢状態へと変化させるに際し、CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する方法は、以下の通りである。各ステップにおける前記手先の位置及び姿勢状態を取得する。各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する。隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第1の位置及び姿勢状態から前記第2の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する。前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上の方法によれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記CP制御にて、前記第1の位置及び姿勢状態から前記第2の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。   According to the first aspect of the present invention, the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by CP (Continuous Point) control. At this time, a method of determining the redundancy degree of freedom of the redundant manipulator in each step of CP control is as follows. The position and posture state of the hand at each step are acquired. Based on the position and posture state of the hand at each step, an area where the redundancy degree of freedom of the redundant manipulator at each step can be realized is calculated. A combination of the regions from the first position and posture state to the second position and posture state is created so that the regions at least partially overlap between adjacent steps. Based on the combination of the regions, the redundancy degree of freedom of the redundant manipulator in each step of the CP control is determined. According to the above method, when the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by the CP control, The continuity of the redundancy freedom of the manipulator can be ensured.

好ましくは、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する。以上の方法によれば、最終のステップにおいて前記領域が最大化される。従って、前記第2の位置及び姿勢状態において、前記手先の位置及び姿勢状態をそのままにしつつ、前記冗長自由度を大きく変化させることが可能となる。このことは、例えば前記第2の位置及び姿勢状態から前記手先の位置及び姿勢状態を更に変化させようとする場合、その変化の最初のステップにおける前記冗長自由度の選択の柔軟性に優れることから、少なくとも以下の3つのメリットが生じる。第1に、前記領域の組み合わせの作成が可能となる確率が高くなる。第2に、前記領域の組み合わせの作成可能な組数が複数となる確率が高くなる。第3に、前記領域の組み合わせの作成可能な組数がより多くなる可能性がある。   Preferably, when a plurality of combinations of the areas can be created, the combination having the largest area in the final step is adopted. According to the above method, the area is maximized in the final step. Therefore, in the second position and posture state, the redundancy degree of freedom can be greatly changed while keeping the position and posture state of the hand. This is because, for example, when the hand position and posture state are further changed from the second position and posture state, the flexibility of selection of the redundancy degree of freedom in the first step of the change is excellent. At least the following three advantages arise. First, the probability that a combination of the areas can be created increases. Secondly, there is a high probability that the number of combinations that can be created for the combination of regions is plural. Thirdly, there is a possibility that the number of combinations that can be created for the combination of the regions is increased.

好ましくは、前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上の方法によれば、前記冗長マニピュレータの動きが滑らかになる。   Preferably, when determining the redundancy degree of freedom of the redundant manipulator in each step of the CP control based on the combination of the regions, the square sum of the difference in the degree of redundancy between the steps is minimized. A redundancy degree of freedom of the redundant manipulator in each step of CP control is determined. According to the above method, the redundant manipulator moves smoothly.

好ましくは、前記冗長マニピュレータは、7自由度である。   Preferably, the redundant manipulator has 7 degrees of freedom.

好ましくは、前記冗長マニピュレータは、S−R−S型である。   Preferably, the redundant manipulator is of the SRS type.

本願発明の第2の観点によれば、冗長マニピュレータの手先の位置及び姿勢状態を、CP(Continuous Point)制御にて、第1の位置及び姿勢状態から第2の位置及び姿勢状態へと変化させるに際し、CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する装置は、以下の通りである。決定装置は、各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第1の位置及び姿勢状態から前記第2の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、を備える。以上の構成によれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記CP制御にて、前記第1の位置及び姿勢状態から前記第2の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。   According to the second aspect of the present invention, the position and posture of the hand of the redundant manipulator are changed from the first position and posture to the second position and posture by CP (Continuous Point) control. At this time, an apparatus for determining the redundancy degree of freedom of the redundant manipulator in each step of CP control is as follows. The determination device can realize the redundancy degree of freedom of the redundant manipulator in each step based on the state acquisition means for acquiring the position and posture state of the hand in each step and the position and posture state of the hand in each step. An area calculation means for calculating an area and the area between the first position and posture state to the second position and posture state so that the areas overlap at least partially between adjacent steps. A combination creating means for creating a combination; and a redundancy degree of freedom determining means for determining the degree of freedom of the redundancy manipulator in each step of the CP control based on the combination of the areas. According to the above configuration, when the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by the CP control, The continuity of the redundancy freedom of the manipulator can be ensured.

好ましくは、前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する。以上の構成によれば、最終のステップにおいて前記領域が最大化される。従って、前記第2の位置及び姿勢状態において、前記手先の位置及び姿勢状態をそのままにしつつ、前記冗長自由度を大きく変化させることが可能となる。このことは、例えば前記第2の位置及び姿勢状態から前記手先の位置及び姿勢状態を更に変化させようとする場合、その変化の最初のステップにおける前記冗長自由度の選択の柔軟性に優れることから、少なくとも以下の3つのメリットが生じる。第1に、前記領域の組み合わせの作成が可能となる確率が高くなる。第2に、前記領域の組み合わせの作成可能な組数が複数となる確率が高くなる。第3に、前記領域の組み合わせの作成可能な組数がより多くなる可能性がある。   Preferably, when a plurality of combinations of the areas can be created, the combination creating unit adopts a combination having the largest area in the final step. According to the above configuration, the area is maximized in the final step. Therefore, in the second position and posture state, the redundancy degree of freedom can be greatly changed while keeping the position and posture state of the hand. This is because, for example, when the hand position and posture state are further changed from the second position and posture state, the flexibility of selection of the redundancy degree of freedom in the first step of the change is excellent. At least the following three advantages arise. First, the probability that a combination of the areas can be created increases. Secondly, there is a high probability that the number of combinations that can be created for the combination of regions is plural. Thirdly, there is a possibility that the number of combinations that can be created for the combination of the regions is increased.

好ましくは、前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上の構成によれば、前記冗長マニピュレータの動きが滑らかになる。   Preferably, the redundancy degree-of-freedom determination means determines the redundancy degree of freedom of the redundancy manipulator in each step of the CP control based on the combination of the regions, and the sum of squares of the difference of the redundancy degrees of freedom between the steps. Is determined such that the redundancy degree of freedom of the redundant manipulator in each step of the CP control is minimized. According to the above configuration, the redundant manipulator moves smoothly.

好ましくは、前記冗長マニピュレータは、7自由度である。   Preferably, the redundant manipulator has 7 degrees of freedom.

好ましくは、前記冗長マニピュレータは、S−R−S型である。   Preferably, the redundant manipulator is of the SRS type.

本願発明の第3の観点によれば、冗長マニピュレータの手先の位置及び姿勢状態を、CP(Continuous Point)制御にて、第1の位置及び姿勢状態から第2の位置及び姿勢状態へと変化させるに際し、CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するためのプログラムは、コンピュータを、各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第1の位置及び姿勢状態から前記第2の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、として機能させる。以上のプログラムによれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記CP制御にて、前記第1の位置及び姿勢状態から前記第2の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。   According to the third aspect of the present invention, the position and posture of the hand of the redundant manipulator are changed from the first position and posture to the second position and posture by CP (Continuous Point) control. At this time, the program for determining the redundancy degree of freedom of the redundant manipulator in each step of the CP control includes a computer, a state acquisition means for acquiring the position and posture state of the hand in each step, and the position of the hand in each step. Based on the position and orientation state, the area calculation means for calculating an area where the redundancy degree of freedom of the redundant manipulator in each step can be realized, and the areas are at least partially overlapped between adjacent steps. The set of regions between the first position and posture state and the second position and posture state A combination generating means for generating a combined, based on the combination of the region, the a redundant DOF determination means for determining a redundancy degree of freedom of the redundant manipulator at each step of the CP control, to function as a. According to the above program, when the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by the CP control, The continuity of the redundancy freedom of the manipulator can be ensured.

好ましくは、前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する。以上のプログラムによれば、最終のステップにおいて前記領域が最大化される。従って、前記第2の位置及び姿勢状態において、前記手先の位置及び姿勢状態をそのままにしつつ、前記冗長自由度を大きく変化させることが可能となる。このことは、例えば前記第2の位置及び姿勢状態から前記手先の位置及び姿勢状態を更に変化させようとする場合、その変化の最初のステップにおける前記冗長自由度の選択の柔軟性に優れることから、少なくとも以下の3つのメリットが生じる。第1に、前記領域の組み合わせの作成が可能となる確率が高くなる。第2に、前記領域の組み合わせの作成可能な組数が複数となる確率が高くなる。第3に、前記領域の組み合わせの作成可能な組数がより多くなる可能性がある。   Preferably, when a plurality of combinations of the areas can be created, the combination creating unit adopts a combination having the largest area in the final step. According to the above program, the area is maximized in the final step. Therefore, in the second position and posture state, the redundancy degree of freedom can be greatly changed while keeping the position and posture state of the hand. This is because, for example, when the hand position and posture state are further changed from the second position and posture state, the flexibility of selection of the redundancy degree of freedom in the first step of the change is excellent. At least the following three advantages arise. First, the probability that a combination of the areas can be created increases. Secondly, there is a high probability that the number of combinations that can be created for the combination of regions is plural. Thirdly, there is a possibility that the number of combinations that can be created for the combination of the regions is increased.

好ましくは、前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上のプログラムによれば、前記冗長マニピュレータの動きが滑らかになる。   Preferably, the redundancy degree-of-freedom determination means determines the redundancy degree of freedom of the redundancy manipulator in each step of the CP control based on the combination of the regions, and the sum of squares of the difference of the redundancy degrees of freedom between the steps. Is determined such that the redundancy degree of freedom of the redundant manipulator in each step of the CP control is minimized. According to the above program, the redundant manipulator moves smoothly.

好ましくは、前記冗長マニピュレータは、7自由度である。   Preferably, the redundant manipulator has 7 degrees of freedom.

好ましくは、前記冗長マニピュレータは、S−R−S型である。   Preferably, the redundant manipulator is of the SRS type.

本願発明によれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記CP制御にて、前記第1の位置及び姿勢状態から前記第2の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。   According to the present invention, when the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by the CP control, the redundant manipulator The continuity of redundant degrees of freedom can be ensured.

図1は、冗長マニピュレータの全体模式図である(第1実施形態)。FIG. 1 is an overall schematic diagram of a redundant manipulator (first embodiment). 図2は、冗長マニピュレータの機能ブロック図である(第1実施形態)。FIG. 2 is a functional block diagram of the redundant manipulator (first embodiment). 図3は、冗長マニピュレータの制御フローである(第1実施形態)。FIG. 3 is a control flow of the redundant manipulator (first embodiment). 図4は、冗長マニピュレータの制御フローである(第1実施形態)。FIG. 4 is a control flow of the redundant manipulator (first embodiment). 図5は、冗長自由度の実現可能な領域ψの一覧である(第1実施形態)。FIG. 5 is a list of regions ψ in which the redundancy degree can be realized (first embodiment). 図6は、冗長自由度の実現可能な領域ψの連続性を示すグラフである(第1実施形態)。FIG. 6 is a graph showing the continuity of the region ψ where the redundancy degree of freedom can be realized (first embodiment). 図7は、冗長自由度の実現可能な領域ψの組み合わせを示す図である(第1実施形態)。FIG. 7 is a diagram illustrating combinations of regions ψ in which redundancy degrees of freedom can be realized (first embodiment). 図8は、冗長マニピュレータの全体模式図である(第2実施形態)。FIG. 8 is an overall schematic diagram of the redundant manipulator (second embodiment). 図9は、冗長自由度の実現可能な領域ψを示す図である(第2実施形態)。FIG. 9 is a diagram showing a region ψ in which the degree of redundancy can be realized (second embodiment). 図10は、冗長自由度の実現可能な領域ψの一覧である(第2実施形態)。FIG. 10 is a list of regions ψ in which redundancy degrees of freedom can be realized (second embodiment). 図11は、冗長自由度の実現可能な領域ψの連続性を示すグラフである(第2実施形態)。FIG. 11 is a graph showing the continuity of the region ψ where the redundancy degree of freedom can be realized (second embodiment). 図12は、冗長自由度の実現可能な領域ψの組み合わせを示す図である(第2実施形態)。FIG. 12 is a diagram illustrating combinations of regions ψ in which redundancy degrees of freedom can be realized (second embodiment). 図13は、各ステップにおいて決定された冗長自由度を示す図である(第2実施形態)。FIG. 13 is a diagram showing the degree of redundancy determined in each step (second embodiment). 図14は、各ステップにおいて決定された冗長自由度を示す図である(第3実施形態)。FIG. 14 is a diagram illustrating the redundancy degrees determined in each step (third embodiment). 図15は、冗長自由度の実現可能な領域ψを示す図である(第4実施形態)。FIG. 15 is a diagram showing a region ψ in which the degree of redundancy can be realized (fourth embodiment). 図16は、冗長自由度の実現可能な領域ψの一覧である(第4実施形態)。FIG. 16 is a list of regions ψ in which redundancy degrees can be realized (fourth embodiment). 図17は、冗長マニピュレータの制御フローである(第4実施形態)。FIG. 17 is a control flow of the redundant manipulator (fourth embodiment). 図18は、冗長自由度の実現可能な領域ψの連続性を示すグラフである(第4実施形態)。FIG. 18 is a graph showing the continuity of the region ψ where the redundancy degree of freedom can be realized (fourth embodiment). 図19は、冗長自由度の実現可能な領域ψの組み合わせを示す図である(第4実施形態)。FIG. 19 is a diagram illustrating combinations of regions ψ in which redundancy degrees of freedom can be realized (fourth embodiment). 図20は、図19に示す複数の組み合わせから選択された組み合わせを示す図である(第4実施形態)。FIG. 20 is a diagram illustrating combinations selected from the plurality of combinations illustrated in FIG. 19 (fourth embodiment). 図21は、各ステップにおいて決定された冗長自由度を示す図である(第4実施形態)。FIG. 21 is a diagram showing the degree of redundancy determined in each step (fourth embodiment). 図22は、冗長マニピュレータの全体模式図である(従来技術)。FIG. 22 is an overall schematic diagram of a redundant manipulator (prior art). 図23は、冗長自由度の一例としてのアームアングルの定義説明図である(従来技術)。FIG. 23 is an explanatory diagram of the definition of an arm angle as an example of the redundancy degree (prior art). 図24は、冗長マニピュレータの全体模式図である(従来技術)。FIG. 24 is an overall schematic diagram of a redundant manipulator (prior art). 図25は、各ステップにおいて決定された冗長自由度を示す図である(従来技術)。FIG. 25 is a diagram showing the redundancy degree of freedom determined in each step (prior art).

(第1実施形態)
以下、図1〜7を参照しつつ、本願発明の第1実施形態を説明する。以下、明細書中で「ステップ」とあるのは、CP制御の制御フローの多種多様な処理のステップを示す場合と、CP制御によるマニピュレータの動きのステップを示す場合があるので、両者を混同しないように留意されたい。
(First embodiment)
Hereinafter, the first embodiment of the present invention will be described with reference to FIGS. Hereinafter, the term “step” in the specification may indicate a step of various processes in the control flow of CP control, and may indicate a step of manipulator movement by CP control. Please note that.

図1に示すように、本実施形態において冗長マニピュレータ1は、体幹部2と、肩関節部3と、上腕部4と、肘関節部5と、前腕部6と、手首関節部7と、手先8と、制御装置100(決定装置)と、を主たる構成として備えている。体幹部2は、例えば地面などに固定されている。地面と肩関節部3は体幹部2によって連結されている。肩関節部3と肘関節部5は上腕部4によって連結されている。肘関節部5と手首関節部7は前腕部6によって連結されている。手先8は手首関節部7に支持されている。   As shown in FIG. 1, in the present embodiment, the redundant manipulator 1 includes a trunk portion 2, a shoulder joint portion 3, an upper arm portion 4, an elbow joint portion 5, a forearm portion 6, a wrist joint portion 7, and a hand tip. 8 and a control device 100 (decision device) as main components. The trunk 2 is fixed to the ground, for example. The ground and the shoulder joint 3 are connected by the trunk 2. The shoulder joint 3 and the elbow joint 5 are connected by the upper arm 4. The elbow joint 5 and the wrist joint 7 are connected by a forearm 6. The hand 8 is supported by the wrist joint 7.

肩関節部3は、第1〜第3の関節から構成される球面機構となっている。肘関節部5は、第4の関節から構成されている。手首関節部7は、第5〜第7の関節から構成される球面機構となっている。従って、冗長マニピュレータ1は、所謂S−R−S型マニピュレータに分類される。また、冗長マニピュレータ1は、余分な自由度を1つ有している。以降、冗長自由度を表現するためのパラメータとしては、非特許文献2に開示のアームアングルψを使用するものとする。繰り返すが、冗長自由度を表現するためのパラメータとしては、アームアングルψに限らず、例えば、任意の1自由度の関節角や別の表現のアームアングルでもよい。   The shoulder joint 3 is a spherical mechanism composed of first to third joints. The elbow joint 5 is composed of a fourth joint. The wrist joint portion 7 is a spherical mechanism composed of fifth to seventh joints. Therefore, the redundant manipulator 1 is classified as a so-called SRS type manipulator. Further, the redundant manipulator 1 has one extra degree of freedom. Hereinafter, it is assumed that the arm angle ψ disclosed in Non-Patent Document 2 is used as a parameter for expressing the redundancy degree of freedom. Again, the parameter for expressing the redundancy degree of freedom is not limited to the arm angle ψ, but may be, for example, an arbitrary joint angle of one degree of freedom or an arm angle of another expression.

図2に示すように、冗長マニピュレータ1は、更に、第1関節モータ(肩部)30と、第2関節モータ(肩部)31と、第3関節モータ(肩部)32と、第4関節モータ(肘部)33と、第5関節モータ(手首部)34と、第6関節モータ(手首部)35と、第7関節モータ(手首部)36と、を備えている。第1関節モータ(肩部)30は、肩関節部3を構成する第1関節に搭載され、第1関節を駆動する。同様に、第2関節モータ(肩部)31は、肩関節部3を構成する第2関節に搭載され、第2関節を駆動する。第3関節モータ(肩部)32は、肩関節部3を構成する第3関節に搭載され、第3関節を駆動する。第4関節モータ(肘部)33は、肘関節部5を構成する第4関節に搭載され、第4関節を駆動する。第5関節モータ(手首部)34は、手首関節部7を構成する第5関節に搭載され、第5関節を駆動する。第6関節モータ(手首部)35は、手首関節部7を構成する第6関節に搭載され、第6関節を駆動する。第7関節モータ(手首部)36は、手首関節部7を構成する第7関節に搭載され、第7関節を駆動する。   As shown in FIG. 2, the redundant manipulator 1 further includes a first joint motor (shoulder) 30, a second joint motor (shoulder) 31, a third joint motor (shoulder) 32, and a fourth joint. A motor (elbow part) 33, a fifth joint motor (wrist part) 34, a sixth joint motor (wrist part) 35, and a seventh joint motor (wrist part) 36 are provided. The first joint motor (shoulder portion) 30 is mounted on the first joint constituting the shoulder joint portion 3 and drives the first joint. Similarly, the second joint motor (shoulder portion) 31 is mounted on the second joint constituting the shoulder joint portion 3 and drives the second joint. The third joint motor (shoulder portion) 32 is mounted on the third joint constituting the shoulder joint portion 3 and drives the third joint. The fourth joint motor (elbow part) 33 is mounted on the fourth joint constituting the elbow joint part 5 and drives the fourth joint. The fifth joint motor (wrist part) 34 is mounted on the fifth joint constituting the wrist joint part 7 and drives the fifth joint. The sixth joint motor (wrist part) 35 is mounted on the sixth joint constituting the wrist joint part 7 and drives the sixth joint. The seventh joint motor (wrist part) 36 is mounted on the seventh joint constituting the wrist joint part 7 and drives the seventh joint.

制御装置100は、冗長マニピュレータ1の手先8の位置及び姿勢状態を、CP制御にて、第1の位置及び姿勢状態(ステップs0、図1参照)から第2の位置及び姿勢状態(ステップsn、図1参照)へと変化させるに際し、CP制御の各ステップにおける冗長マニピュレータ1のアームアングルψの決定する。制御装置100は、CPU(Central Processing Unit)10と、RAM(Random Access Memory)11と、ROM(Read Only Memory)12と、を備える。ROM12には、制御プログラム(決定プログラム)が記憶されている。この制御プログラムはCPU10によって読み出され、CPU10上で実行される。これにより、制御プログラムは、CPU10などのハードウェアを、状態取得手段20、領域算出手段21、組み合わせ作成手段22、冗長自由度決定手段23、関節角算出手段24、CP制御手段25、として機能させる。   The control device 100 changes the position and posture state of the hand 8 of the redundant manipulator 1 from the first position and posture state (step s0, see FIG. 1) to the second position and posture state (step sn, When changing to (see FIG. 1), the arm angle ψ of the redundant manipulator 1 in each step of CP control is determined. The control device 100 includes a CPU (Central Processing Unit) 10, a RAM (Random Access Memory) 11, and a ROM (Read Only Memory) 12. The ROM 12 stores a control program (decision program). This control program is read by the CPU 10 and executed on the CPU 10. As a result, the control program causes hardware such as the CPU 10 to function as the state acquisition unit 20, the region calculation unit 21, the combination creation unit 22, the redundancy degree determination unit 23, the joint angle calculation unit 24, and the CP control unit 25. .

状態取得手段20は、CP制御に必要となる、各ステップにおける手先8の位置及び姿勢状態に関する情報としての状態情報を取得する。状態取得手段20による各状態情報の取得の態様としては、有線又は無線を介した他の制御機器からの受信や、例えばキーボードなどの入力手段を介した操作人員からの入力、ROM12からの読み込みなどが考えられる。各状態情報は、手先8の空間に対する位置を表すX値、Y値、Z値と、手先8の空間に対する姿勢を表すθ1値、θ2値、θ3値と、を有し、これら6つのパラメータから構成される。状態取得手段20は、取得した各ステップ毎の状態情報をRAM11に記憶させる。   The state acquisition means 20 acquires state information as information relating to the position and posture state of the hand 8 at each step, which is necessary for CP control. The status acquisition means 20 can acquire each status information from the other control device via wire or wireless, input from an operator via an input means such as a keyboard, reading from the ROM 12, etc. Can be considered. Each state information has an X value, a Y value, and a Z value that represent the position of the hand 8 with respect to the space, and a θ1 value, a θ2 value, and a θ3 value that represent the posture of the hand 8 with respect to the space. Composed. The state acquisition unit 20 stores the acquired state information for each step in the RAM 11.

領域算出手段21は、RAM11から各ステップ毎の状態情報を読み込み、各状態情報に基づいて、各ステップ毎に、冗長マニピュレータ1のアームアングルψの実現可能な領域に関する情報としての領域情報を算出する。領域情報は、すべてのステップにおいて1つのみ存在する場合と、すべてのステップにおいて複数存在する場合と、各ステップにおいて1つ又は複数存在する場合と、がある。領域算出手段21による各領域情報の算出の方法については、非特許文献1を参照されたい。領域算出手段21は、算出した各ステップ毎の領域情報をRAM11に記憶させる。   The area calculation means 21 reads state information for each step from the RAM 11 and calculates area information as information regarding a realizable area of the arm angle ψ of the redundant manipulator 1 for each step based on the state information. . There are a case where only one region information exists in all steps, a case where a plurality of region information exists in all steps, and a case where one or a plurality of region information exists in each step. Refer to Non-Patent Document 1 for the method of calculating each area information by the area calculating means 21. The area calculation means 21 stores the calculated area information for each step in the RAM 11.

組み合わせ作成手段22は、RAM11から各ステップ毎の領域情報を読み込み、隣り合うステップ間で領域同士が少なくとも一部重複するように、第1の位置及び姿勢状態から第2の位置及び姿勢状態に至る間の領域情報の組み合わせに関する情報としての組み合わせ情報を1つ、作成する。「隣り合うステップ間で領域同士が少なくとも一部重複する」とは、「隣り合うステップ間で領域同士が共通する冗長自由度を包含している」と言い換えることができる。なお、最初のステップ(ステップs0)における領域と、最後のステップ(ステップsn)における領域と、は必ずしも重複している必要はない。組み合わせ作成手段22は、作成した組み合わせ情報をRAM11に記憶させる。   The combination creating unit 22 reads the region information for each step from the RAM 11 and reaches the second position and posture state from the first position and posture state so that the regions at least partially overlap between adjacent steps. One piece of combination information is created as information related to the combination of the region information. The phrase “regions at least partially overlap between adjacent steps” can be rephrased as “includes a redundancy degree of freedom common to regions between adjacent steps”. Note that the area in the first step (step s0) and the area in the last step (step sn) do not necessarily have to overlap. The combination creating unit 22 stores the created combination information in the RAM 11.

冗長自由度決定手段23は、RAM11から組み合わせ情報を読み込み、組み合わせ情報に基づいて、CP制御の各ステップにおける冗長マニピュレータ1のアームアングルψを決定する。冗長自由度決定手段23は、決定した各ステップ毎のアームアングルψをRAM11に記憶させる。   The redundant degree-of-freedom determining means 23 reads combination information from the RAM 11 and determines the arm angle ψ of the redundant manipulator 1 in each step of CP control based on the combination information. The redundant degree-of-freedom determination means 23 stores the determined arm angle ψ for each step in the RAM 11.

関節角算出手段24は、RAM11から各ステップ毎のアームアングルψを読み込み、各ステップ毎に、そのステップにおけるアームアングルψと、そのステップにおける状態情報と、に基づいて逆運動学により、そのステップにおける各関節(第1〜第7関節)の関節角を算出する。関節角算出手段24は、算出した各ステップ毎の各関節の関節角をRAM11に記憶させる。   The joint angle calculation means 24 reads the arm angle ψ for each step from the RAM 11 and, for each step, performs inverse kinematics on the basis of the arm angle ψ at that step and the state information at that step. The joint angle of each joint (first to seventh joints) is calculated. The joint angle calculation means 24 stores the calculated joint angle of each joint for each step in the RAM 11.

CP制御手段25は、RAM11から各ステップ毎の各関節の関節角を読み込み、これらの関節角によって規定される冗長マニピュレータ1の動きを再生する。詳しくは、CP制御手段25は、各ステップ毎に各関節(第1〜第7関節)の関節角を達成すべく、第1関節モータ(肩部)30、第2関節モータ(肩部)31、第3関節モータ(肩部)32、第4関節モータ(肘部)33、第5関節モータ(手首部)34、第6関節モータ(手首部)35、第7関節モータ(手首部)36を制御する。   The CP control means 25 reads the joint angle of each joint for each step from the RAM 11 and reproduces the motion of the redundant manipulator 1 defined by these joint angles. Specifically, the CP control means 25 has a first joint motor (shoulder) 30 and a second joint motor (shoulder) 31 in order to achieve the joint angle of each joint (first to seventh joints) at each step. , Third joint motor (shoulder) 32, fourth joint motor (elbow) 33, fifth joint motor (wrist part) 34, sixth joint motor (wrist part) 35, seventh joint motor (wrist part) 36 To control.

次に、図3〜図7を参照しつつ、冗長マニピュレータ1の作動を説明する。   Next, the operation of the redundant manipulator 1 will be described with reference to FIGS.

先ず、有線又は無線を介して他の制御機器から制御装置100へCP制御開始信号が送信され、又は、例えばキーボードなどの入力手段を介して操作人員が制御装置100にCP制御を開始するよう命令する(S300)。   First, a CP control start signal is transmitted from another control device to the control device 100 via wired or wireless, or an operation personnel instructs the control device 100 to start CP control via an input means such as a keyboard, for example. (S300).

すると、状態取得手段20は、CP制御に必要となる、各ステップ毎の状態情報を取得する(S302)。図5〜7において、各ステップ毎の状態情報は、同次変換行列であるTnで示されている。添字のnは、離散化されたCP制御の軌道のステップの総数を表している。状態取得手段20は、取得した各ステップ毎の状態情報TnをRAM11に記憶させる。   Then, the state acquisition unit 20 acquires state information for each step, which is necessary for CP control (S302). 5 to 7, the state information for each step is indicated by Tn which is a homogeneous transformation matrix. The subscript n represents the total number of steps of the discretized CP control trajectory. The state acquisition unit 20 stores the acquired state information Tn for each step in the RAM 11.

次に、領域算出手段21は、RAM11から各ステップ毎の状態情報Tnを読み込み、各状態情報Tnに基づいて、各ステップ毎に、アームアングルψの実現可能な領域に関する情報としての領域情報を算出する(S304)。図5〜7において、各ステップ毎の領域情報は、ψ(i,j)で示している。ここで、「i」は上付添字を意味し、「j」は下付添字を意味している。「i」は、その領域が属するステップを識別するための番号を意味する。一方、「j」は、その領域が属するステップにおいて算出された1つ又は複数の閉じた領域を識別するための番号である。また、下付添字で「mn」とあるのは、そのステップにおいて算出された領域情報の個数に相当している。領域算出手段21は、算出した各ステップ毎の領域情報ψ(i,j)をRAM11に記憶させる。   Next, the area calculation means 21 reads the state information Tn for each step from the RAM 11, and calculates area information as information on the area where the arm angle ψ can be realized for each step based on each state information Tn. (S304). 5 to 7, the area information for each step is indicated by ψ (i, j). Here, “i” means a superscript, and “j” means a subscript. “I” means a number for identifying a step to which the area belongs. On the other hand, “j” is a number for identifying one or more closed areas calculated in the step to which the area belongs. The subscript “mn” corresponds to the number of area information calculated in that step. The area calculation means 21 stores the calculated area information ψ (i, j) for each step in the RAM 11.

次に、制御装置100は、すべてのステップにおいて、上記の領域情報ψ(i,j)の個数が1以上であった場合は、処理をS308に進める。一方で、何れかのステップにおいて、上記の領域情報ψ(i,j)の個数が1以上でなかった場合は、解なしであるから、処理を終了する(S307)。   Next, in all the steps, when the number of the area information ψ (i, j) is 1 or more, the control device 100 advances the process to S308. On the other hand, if the number of the region information ψ (i, j) is not 1 or more in any step, there is no solution, and the process ends (S307).

すべてのステップにおいて、上記の領域情報ψ(i,j)の個数が1以上であった場合は、組み合わせ作成手段22は、RAM11から各ステップ毎の領域情報ψ(i,j)を読み込み、隣り合うステップ間で領域同士が少なくとも一部重複するように、第1の位置及び姿勢状態から第2の位置及び姿勢状態に至る間の領域情報ψ(i,j)の組み合わせに関する情報としての組み合わせ情報を1つ、作成する。   In all steps, when the number of the area information ψ (i, j) is 1 or more, the combination creating means 22 reads the area information ψ (i, j) for each step from the RAM 11 and Combination information as information regarding the combination of region information ψ (i, j) from the first position and posture state to the second position and posture state so that the regions at least partially overlap between the matching steps. Create one.

具体的には、組み合わせ作成手段22は、先ず、図6に示すような連続性グラフを作成する(S400)。詳細には、図5において、隣り合うステップ間で領域情報ψ(i,j)と領域情報ψ(i−1,k)が下記式(1)を満たすならば少なくとも一部重複しているので、領域情報ψ(i,j)と領域情報ψ(i−1,k)は連続しており、領域情報ψ(i,j)と領域情報ψ(i−1,k)は、グラフ理論で言うところのエッジで結合される。例えば、図5において、隣り合うステップs0とステップs1間で領域情報ψ(1,1)と領域情報ψ(0,1)が下記式(1)を満たしているので、領域情報ψ(1,1)と領域情報ψ(0,1)は、図6で太線で示すようにエッジで結合される。図5において、ステップs0には領域情報ψ(i−1,k)がm0個属しており、ステップs1には領域情報ψ(i,j)がm1個属している。従って、ステップs0に属するすべての領域情報ψ(i−1,k)と、ステップs1に属するすべての領域情報ψ(i,j)と、の重複関係を漏れなく判定するには、原則として、m0×m1回、下記式(1)を考慮する必要があることになる。   Specifically, the combination creating unit 22 first creates a continuity graph as shown in FIG. 6 (S400). Specifically, in FIG. 5, if the region information ψ (i, j) and the region information ψ (i−1, k) satisfy the following formula (1) between adjacent steps, at least partly overlaps. The region information ψ (i, j) and the region information ψ (i−1, k) are continuous, and the region information ψ (i, j) and the region information ψ (i−1, k) are expressed by graph theory. They are joined at what they say. For example, in FIG. 5, since the region information ψ (1, 1) and the region information ψ (0, 1) satisfy the following expression (1) between the adjacent steps s0 and s1, the region information ψ (1, 1) and the region information ψ (0, 1) are coupled by an edge as shown by a thick line in FIG. In FIG. 5, m0 pieces of area information ψ (i−1, k) belong to step s0, and m1 pieces of area information ψ (i, j) belong to step s1. Therefore, as a general rule, in order to determine the overlapping relationship between all the area information ψ (i−1, k) belonging to step s0 and all the area information ψ (i, j) belonging to step s1 without omission, It is necessary to consider the following formula (1) m0 × m1 times.

次に、組み合わせ作成手段22は、ステップs0とステップsnの間における連続的な領域情報ψ(i,j)の組み合わせを図6に示す連続性グラフ内で探索する(S402)。なお、この探索アルゴリズムはグラフ理論の分野で種々の提案がなされ公知となっているので、その説明は割愛する。仮に、この探索の結果、組み合わせ作成手段22が、ステップs0とステップsnの間における連続的な領域情報ψ(i,j)の組み合わせとして、図7に示すように、「ψ(0,1)、ψ(1,1)、ψ(2,1)、ψ(3,1)、・・・、ψ(n−1,1)、ψ(n,1)」から成る組み合わせを特定したとする。このようにステップs0とステップsnの間における連続的な領域情報ψ(i,j)の組み合わせが見つかった場合(S404:YES)は、組み合わせ作成手段22は、「ψ(0,1)、ψ(1,1)、ψ(2,1)、ψ(3,1)、・・・、ψ(n−1,1)、ψ(n,1)」から成る組み合わせ情報を作成する。そして、組み合わせ作成手段22は、作成した組み合わせ情報をRAM11に記憶させ、処理を図3の制御フローへと戻す(S406)。一方、ステップ0とステップnの間における連続的な領域情報ψ(i,j)の組み合わせが見つからなかった場合(S404:NO)は、解なしとして処理を終了する(S408)。   Next, the combination creating means 22 searches the continuity graph shown in FIG. 6 for a combination of continuous area information ψ (i, j) between step s0 and sn (S402). Since this search algorithm has been publicly known with various proposals in the field of graph theory, its description will be omitted. As a result of this search, as shown in FIG. 7, the combination creating means 22 uses the combination of continuous region information ψ (i, j) between step s0 and sn as shown in FIG. , Ψ (1,1), ψ (2,1), ψ (3,1),..., Ψ (n−1,1), ψ (n, 1) ”. . When a combination of continuous region information ψ (i, j) between step s0 and step sn is found in this way (S404: YES), the combination creating means 22 selects “ψ (0,1), ψ (1,1), ψ (2,1), ψ (3,1),..., Ψ (n−1,1), ψ (n, 1) ”. Then, the combination creating unit 22 stores the created combination information in the RAM 11 and returns the process to the control flow of FIG. 3 (S406). On the other hand, when the combination of the continuous area information ψ (i, j) between step 0 and step n is not found (S404: NO), the process is terminated as no solution (S408).

次に、冗長自由度決定手段23は、RAM11から組み合わせ情報を読み込み、組み合わせ情報に基づいて、CP制御の各ステップにおける冗長マニピュレータ1のアームアングルψを決定する(S310)。例えば、冗長自由度決定手段23は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ(i,j)の上限値、下限値、好ましくは中間値を採用する。冗長自由度決定手段23は、決定した各ステップ毎のアームアングルψをRAM11に記憶させる。   Next, the redundant degree-of-freedom determination means 23 reads combination information from the RAM 11, and determines the arm angle ψ of the redundant manipulator 1 in each step of CP control based on the combination information (S310). For example, the redundancy degree determining means 23 employs the upper limit value, the lower limit value, preferably the intermediate value of the region information ψ (i, j) belonging to the combination information as the arm angle ψ for each step. The redundant degree-of-freedom determination means 23 stores the determined arm angle ψ for each step in the RAM 11.

次に、関節角算出手段24は、RAM11から各ステップ毎のアームアングルψを読み込み、各ステップ毎に、そのステップにおけるアームアングルψと、そのステップにおける状態情報と、に基づいて逆運動学により、そのステップにおける各関節(第1〜第7関節)の関節角を算出する(S312)。関節角算出手段24は、算出した各ステップ毎の各関節の関節角をRAM11に記憶させる。   Next, the joint angle calculation means 24 reads the arm angle ψ for each step from the RAM 11, and for each step, based on the arm angle ψ at that step and the state information at that step, by inverse kinematics, The joint angle of each joint (first to seventh joints) in that step is calculated (S312). The joint angle calculation means 24 stores the calculated joint angle of each joint for each step in the RAM 11.

そして、CP制御手段25は、RAM11から各ステップ毎の各関節の関節角を読み込み、これらの関節角によって規定される冗長マニピュレータ1の動きを再生する(S314)。   Then, the CP control means 25 reads the joint angle of each joint for each step from the RAM 11, and reproduces the motion of the redundant manipulator 1 defined by these joint angles (S314).

以上に本願発明の好適な第1実施形態を説明した。第1実施形態は、要するに、以下の特徴を有している。   The preferred first embodiment of the present invention has been described above. In short, the first embodiment has the following features.

冗長マニピュレータ1の手先8の位置及び姿勢状態を、CP制御にて、ステップs0からステップsnへと変化させるに際し、CP制御の各ステップにおける冗長マニピュレータ1のアームアングルψを決定する制御装置100は、以下の通りである。制御装置100は、各ステップにおける手先8の位置及び姿勢状態を取得する状態取得手段20と、各ステップにおける手先8の位置及び姿勢状態に基づいて、各ステップにおけるアームアングルψの実現可能な領域を算出する領域算出手段21と、隣り合うステップ間で領域同士が少なくとも一部重複するように、ステップs0からステップsnに至る間の領域の組み合わせを作成する組み合わせ作成手段22と、領域の組み合わせに基づいて、CP制御の各ステップにおけるアームアングルψを決定する冗長自由度決定手段23と、を備える。以上の構成によれば、冗長マニピュレータ1の手先8の位置及び姿勢状態を、CP制御にて、ステップs0からステップsnへと変化させるに際し、冗長マニピュレータ1の冗長自由度の連続性を担保することができる。   When changing the position and posture state of the hand 8 of the redundant manipulator 1 from step s0 to step sn in the CP control, the control device 100 that determines the arm angle ψ of the redundant manipulator 1 in each step of the CP control, It is as follows. Based on the state acquisition means 20 for acquiring the position and posture state of the hand 8 at each step, and the position and posture state of the hand 8 at each step, the control device 100 determines a realizable region of the arm angle ψ at each step. Based on the combination of the area calculation means 21, the combination creation means 22 that creates a combination of areas from step s0 to step sn so that the areas at least partially overlap between adjacent steps, and the area combination. Redundant degree-of-freedom determining means 23 for determining the arm angle ψ in each step of CP control. According to the above configuration, when the position and posture state of the hand 8 of the redundant manipulator 1 is changed from step s0 to step sn by CP control, the continuity of the redundant degree of freedom of the redundant manipulator 1 is ensured. Can do.

また、副次的な効果として、これにより関節角レベルで起きる非連続性が克服され、機構の性能を最大限活用した手先動作が可能となる。   As a secondary effect, discontinuity that occurs at the joint angle level is overcome by this, and hand movements that make maximum use of the performance of the mechanism are possible.

(第2実施形態)
次に、図8〜13を参照しつつ、本願発明の第2実施形態を説明する。ここでは、本実施形態が上記第1実施形態と異なる点を中心に説明し、重複する説明は適宜省略する。また、上記第1実施形態の各構成要素に対応する構成要素には原則として同一の符号を付すこととする。本実施形態では、上記第1実施形態を更に具現化したものである。
(Second Embodiment)
Next, a second embodiment of the present invention will be described with reference to FIGS. Here, the present embodiment will be described with a focus on differences from the first embodiment, and overlapping descriptions will be omitted as appropriate. In addition, in principle, the same reference numerals are assigned to components corresponding to the respective components of the first embodiment. In the present embodiment, the first embodiment is further embodied.

図8に示すように、本実施形態において、冗長マニピュレータ1のCP制御は、ステップs0からステップs5に至るまでの6つのステップを基礎としている。図9には、状態取得手段20が取得した各状態情報Tnと、領域算出手段21が算出した各領域情報ψ(i,j)と、の関係を示している。図9の横軸に各状態情報Tnを並べている。図9の縦軸はアームアングルψである。なお、アームアングルψの原点は特に明示していないが、アームアングルψの取り得る値は−πからπとなっている。各状態情報Tnと、各領域情報ψ(i,j)と、の関係を図5のように一覧表示したのが図10である。図9に示すように、領域情報ψ(0,1)と領域情報ψ(1,1)との間には共通するアームアングルψが存在する。従って、領域情報ψ(0,1)と領域情報ψ(1,1)は連続的であり、図11に示すようにエッジで結合される。同様に、図9に示すように、領域情報ψ(0,1)と領域情報ψ(1,2)との間には共通するアームアングルψが存在する。従って、領域情報ψ(0,1)と領域情報ψ(1,2)は連続的であり、図11に示すようにエッジで結合される。組み合わせ作成手段22は、この要領で処理を進め、やがて、図11に示すような連続性グラフを完成させる(S400)。   As shown in FIG. 8, in this embodiment, the CP control of the redundant manipulator 1 is based on six steps from step s0 to step s5. FIG. 9 shows the relationship between the state information Tn acquired by the state acquisition unit 20 and the region information ψ (i, j) calculated by the region calculation unit 21. Each state information Tn is arranged on the horizontal axis of FIG. The vertical axis in FIG. 9 is the arm angle ψ. Although the origin of the arm angle ψ is not particularly specified, the possible value of the arm angle ψ is from −π to π. FIG. 10 shows a list of the relationship between each state information Tn and each region information ψ (i, j) as shown in FIG. As shown in FIG. 9, a common arm angle ψ exists between the region information ψ (0, 1) and the region information ψ (1, 1). Therefore, the region information ψ (0, 1) and the region information ψ (1, 1) are continuous and are joined by an edge as shown in FIG. Similarly, as shown in FIG. 9, a common arm angle ψ exists between the region information ψ (0,1) and the region information ψ (1,2). Therefore, the region information ψ (0,1) and the region information ψ (1,2) are continuous and are joined at the edge as shown in FIG. The combination creating means 22 advances the processing in this manner, and eventually completes a continuity graph as shown in FIG. 11 (S400).

次に、組み合わせ作成手段22は、ステップs0とステップs5の間における連続的な領域情報ψ(i,j)の組み合わせを図11に示す連続性グラフ内で探索する(S402)。この探索の結果、組み合わせ作成手段22は、ステップs0とステップs5の間における連続的な領域情報ψ(i,j)の組み合わせとして、図12に示すように、「ψ(0,1)、ψ(1,1)、ψ(2,1)、ψ(3,1)、ψ(4,1)、ψ(5,1)」から成る組み合わせを特定する。組み合わせ作成手段22は、「ψ(0,1)、ψ(1,1)、ψ(2,1)、ψ(3,1)、ψ(4,1)、ψ(5,1)」から構成される組み合わせ情報を作成する。そして、組み合わせ作成手段22は、作成した組み合わせ情報をRAM11に記憶させ、処理を図3の制御フローへと戻す(S406)。   Next, the combination creating means 22 searches the continuity graph shown in FIG. 11 for a combination of continuous area information ψ (i, j) between step s0 and step s5 (S402). As a result of this search, the combination creating means 22 obtains “ψ (0,1), ψ as a combination of continuous area information ψ (i, j) between steps s0 and s5 as shown in FIG. (1,1), ψ (2,1), ψ (3,1), ψ (4,1), and ψ (5,1) ”are specified. The combination creating means 22 starts from “ψ (0,1), ψ (1,1), ψ (2,1), ψ (3,1), ψ (4,1), ψ (5,1)”. Create combination information to be configured. Then, the combination creating unit 22 stores the created combination information in the RAM 11 and returns the process to the control flow of FIG. 3 (S406).

次に、冗長自由度決定手段23は、RAM11から組み合わせ情報を読み込み、組み合わせ情報に基づいて、図13に示すように、CP制御の各ステップにおけるアームアングルψを決定する(S310)。各ステップにおける冗長マニピュレータ1の決定されたアームアングルψは、図13において丸印で示している。本実施形態では、図13に示すように、冗長自由度決定手段23は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ(i,j)の中間値を採用している。そして、冗長自由度決定手段23は、決定した各ステップ毎のアームアングルψをRAM11に記憶させる。   Next, the redundant degree-of-freedom determination means 23 reads combination information from the RAM 11, and determines the arm angle ψ in each step of CP control based on the combination information as shown in FIG. 13 (S310). The determined arm angle ψ of the redundant manipulator 1 at each step is indicated by a circle in FIG. In the present embodiment, as shown in FIG. 13, the redundancy degree determining means 23 employs an intermediate value of the region information ψ (i, j) belonging to the combination information as the arm angle ψ for each step. . Then, the redundancy degree of freedom determining means 23 stores the determined arm angle ψ for each step in the RAM 11.

なお、図13において、△ψは「△ψp→p+1」を意味し、「△ψp→p+1」は、ステップpのアームアングルψとステップp+1のアームアングルψとの差分を意味する。 In FIG. 13, Δψ means “Δψ p → p + 1 ”, and “Δψ p → p + 1 ” means the difference between the arm angle ψ of step p and the arm angle ψ of step p + 1.

(第3実施形態)
次に、図14を参照しつつ、本願発明の第3実施形態を説明する。ここでは、本実施形態が上記第2実施形態と異なる点を中心に説明し、重複する説明は適宜省略する。また、上記第2実施形態の各構成要素に対応する構成要素には原則として同一の符号を付すこととする。
(Third embodiment)
Next, a third embodiment of the present invention will be described with reference to FIG. Here, the present embodiment will be described with a focus on differences from the second embodiment, and a duplicate description will be omitted as appropriate. In addition, in principle, the same reference numerals are assigned to components corresponding to the components of the second embodiment.

上記第2実施形態において、冗長自由度決定手段23は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ(i,j)の中間値を採用するとした。これに対し、本実施形態において、冗長自由度決定手段23は、図14に示すように、ステップ間におけるアームアングルψの差分の二乗和が最も小さくなるように、各ステップにおけるアームアングルψを決定する。具体的には、冗長自由度決定手段23は、下記式(2)で定義された変数Qの値が最小となるように、各ステップにおけるアームアングルψを決定する。   In the second embodiment, the redundancy degree determining means 23 adopts the intermediate value of the region information ψ (i, j) belonging to the combination information as the arm angle ψ for each step. On the other hand, in this embodiment, as shown in FIG. 14, the redundancy degree determining means 23 determines the arm angle ψ at each step so that the sum of squares of the difference of the arm angle ψ between steps becomes the smallest. To do. Specifically, the redundancy degree determining means 23 determines the arm angle ψ in each step so that the value of the variable Q defined by the following equation (2) is minimized.

以上に本願発明の第2実施形態を説明した。第2実施形態は、要するに、以下の特徴を有している。   The second embodiment of the present invention has been described above. In short, the second embodiment has the following features.

冗長自由度決定手段23は、組み合わせ情報に基づいて、CP制御の各ステップにおける冗長マニピュレータ1のアームアングルψを決定するに際し、ステップ間におけるアームアングルψの差分の二乗和が最も小さくなるように、CP制御の各ステップにおけるアームアングルψを決定する。以上の構成によれば、冗長マニピュレータ1の動きが滑らかになる。また、省エネや手先8の動き自体が機敏になる、といったメリットもある。   When determining the arm angle ψ of the redundant manipulator 1 at each step of the CP control based on the combination information, the redundancy degree of freedom determining means 23 is configured so that the sum of squares of the differences of the arm angles ψ between the steps is minimized. The arm angle ψ in each step of CP control is determined. According to the above configuration, the movement of the redundant manipulator 1 becomes smooth. In addition, there is a merit that energy saving and movement of the hand 8 itself becomes agile.

(第4実施形態)
次に、図15〜21を参照しつつ、本願発明の第4実施形態を説明する。ここでは、本実施形態が上記第2実施形態と異なる点を中心に説明し、重複する説明は適宜省略する。また、上記第2実施形態の各構成要素に対応する構成要素には原則として同一の符号を付すこととする。
(Fourth embodiment)
Next, a fourth embodiment of the present invention will be described with reference to FIGS. Here, the present embodiment will be described with a focus on differences from the second embodiment, and a duplicate description will be omitted as appropriate. In addition, in principle, the same reference numerals are assigned to components corresponding to the components of the second embodiment.

図15には、状態取得手段20が取得した各状態情報Tnと、領域算出手段21が算出した各領域情報ψ(i,j)と、の関係を示している。各状態情報Tnと、各領域情報ψ(i,j)と、の関係を図5のように一覧表示したのが図16である。そして、本実施形態において制御装置100は、図4の制御フローに代えて、図17の制御フローを実行する。以下、図17に示す制御フローに沿って、本実施形態における冗長マニピュレータ1の作動を説明する。   FIG. 15 shows the relationship between the state information Tn acquired by the state acquisition unit 20 and the region information ψ (i, j) calculated by the region calculation unit 21. FIG. 16 shows a list of the relationship between each state information Tn and each region information ψ (i, j) as shown in FIG. In this embodiment, the control device 100 executes the control flow of FIG. 17 instead of the control flow of FIG. Hereinafter, the operation of the redundant manipulator 1 in the present embodiment will be described along the control flow shown in FIG.

図15に示すように、領域情報ψ(0,1)と領域情報ψ(1,1)との間には共通するアームアングルψが存在する。従って、領域情報ψ(0,1)と領域情報ψ(1,1)は連続的であり、図18に示すようにエッジで結合される。同様に、図15に示すように、領域情報ψ(0,1)と領域情報ψ(1,2)との間には共通するアームアングルψが存在する。従って、領域情報ψ(0,1)と領域情報ψ(1,2)は連続的であり、図18に示すようにエッジで結合される。組み合わせ作成手段22は、この要領で処理を進め、やがて、図18に示すような連続性グラフを完成させる(S500)。   As shown in FIG. 15, a common arm angle ψ exists between the region information ψ (0, 1) and the region information ψ (1, 1). Therefore, the region information ψ (0, 1) and the region information ψ (1, 1) are continuous and are joined at the edge as shown in FIG. Similarly, as shown in FIG. 15, a common arm angle ψ exists between the region information ψ (0, 1) and the region information ψ (1, 2). Therefore, the region information ψ (0, 1) and the region information ψ (1, 2) are continuous and are joined at the edge as shown in FIG. The combination creating means 22 advances the processing in this manner, and eventually completes a continuity graph as shown in FIG. 18 (S500).

次に、組み合わせ作成手段22は、ステップs0とステップs5の間における連続的な領域情報ψ(i,j)の組み合わせを図18に示す連続性グラフ内で探索する(S502)。この探索の結果、組み合わせ作成手段22は、ステップs0とステップs5の間における連続的な領域情報ψ(i,j)の組み合わせとして、図19に示すように、「ψ(0,1)、ψ(1,1)、ψ(2,1)、ψ(3,1)、ψ(4,1)、ψ(5,1)」から成る組み合わせと、「ψ(0,1)、ψ(1,2)、ψ(2,2)、ψ(3,3)、ψ(4,3)、ψ(5,3)」から成る組み合わせと、を特定する。このようにステップs0とステップs5の間における連続的な領域情報ψ(i,j)の組み合わせが見つかった場合は、制御装置100は、処理をS506へと進める。一方、ステップs0とステップs5の間における連続的な領域情報ψ(i,j)の組み合わせが見つからなかった場合は、制御装置100は、解なしとして処理を終了する(S508)。   Next, the combination creating means 22 searches the continuity graph shown in FIG. 18 for a combination of continuous area information ψ (i, j) between step s0 and step s5 (S502). As a result of this search, the combination creating means 22 obtains “ψ (0,1), ψ as a combination of continuous area information ψ (i, j) between steps s0 and s5 as shown in FIG. (1,1), ψ (2,1), ψ (3,1), ψ (4,1), ψ (5,1) ”,“ ψ (0,1), ψ (1 , 2), ψ (2, 2), ψ (3, 3), ψ (4, 3), ψ (5, 3) ”. Thus, when the combination of continuous area | region information (psi) (i, j) between step s0 and step s5 is found, the control apparatus 100 advances a process to S506. On the other hand, when the combination of the continuous area information ψ (i, j) between step s0 and step s5 is not found, the control device 100 ends the process with no solution (S508).

そして、図19に示すように、ステップs0とステップs5の間における連続的な領域情報ψ(i,j)の組み合わせが複数見つかった場合(S506:YES)は、制御装置100は、処理をS510へと進める。一方、ステップs0とステップs5の間における連続的な領域情報ψ(i,j)の組み合わせが1つだけ見つかった場合(S506:NO)は、制御装置100は、処理を図3の制御フローへと戻す(S512)。   Then, as shown in FIG. 19, when a plurality of combinations of continuous area information ψ (i, j) between steps s0 and s5 are found (S506: YES), the control device 100 performs the process in S510. Proceed to. On the other hand, when only one combination of continuous area information ψ (i, j) between step s0 and step s5 is found (S506: NO), the control device 100 moves the process to the control flow of FIG. (S512).

上述のようにステップs0とステップs5の間における連続的な領域情報ψ(i,j)の組み合わせが複数見つかった場合(S506:YES)は、組み合わせ作成手段22は、最終のステップ(ステップs5)における領域情報ψ(5,j)が最も大きな組み合わせを選択して採用する(S510)。本実施形態において、「ψ(0,1)、ψ(1,1)、ψ(2,1)、ψ(3,1)、ψ(4,1)、ψ(5,1)」から成る組み合わせの最終のステップ(ステップs5)における領域情報ψ(5,j)は、領域情報ψ(5,1)である。また、「ψ(0,1)、ψ(1,2)、ψ(2,2)、ψ(3,3)、ψ(4,3)、ψ(5,3)」から成る組み合わせの最終のステップ(ステップs5)における領域情報ψ(5,j)は、領域情報ψ(5,3)である。従って、組み合わせ作成手段22は、各連続的な領域情報ψ(i,j)の組み合わせの最終のステップにおける領域情報ψ(5,j)としての領域情報ψ(5,1)と領域情報ψ(5,3)を比較する。本実施形態では、図15に示すように、領域情報ψ(5,3)が領域情報ψ(5,1)よりもアームアングルψに関して広範な範囲を包含しており、もって、領域情報ψ(5,3)は領域情報ψ(5,1)よりも大きいと言うことができる。従って、組み合わせ作成手段22は、図20に示すように、「ψ(0,1)、ψ(1,2)、ψ(2,2)、ψ(3,3)、ψ(4,3)、ψ(5,3)」から成る組み合わせを選択して採用し、「ψ(0,1)、ψ(1,2)、ψ(2,2)、ψ(3,3)、ψ(4,3)、ψ(5,3)」から成る組み合わせ情報を作成する。そして、組み合わせ作成手段22は、作成し、選択した組み合わせ情報をRAM11に記憶させ、処理を図3の制御フローへと戻す(S514)。   As described above, when a plurality of combinations of continuous area information ψ (i, j) between steps s0 and s5 are found (S506: YES), the combination creating means 22 makes the final step (step s5). The combination having the largest region information ψ (5, j) is selected and adopted (S510). In this embodiment, it consists of “ψ (0,1), ψ (1,1), ψ (2,1), ψ (3,1), ψ (4,1), ψ (5,1)”. The region information ψ (5, j) in the final step (step s5) of the combination is the region information ψ (5,1). In addition, the final combination of “ψ (0,1), ψ (1,2), ψ (2,2), ψ (3,3), ψ (4,3), ψ (5,3)” The region information ψ (5, j) in step (step s5) is the region information ψ (5,3). Therefore, the combination creating unit 22 uses the region information ψ (5,1) and the region information ψ () as the region information ψ (5, j) in the final step of the combination of each continuous region information ψ (i, j). 5,3) is compared. In the present embodiment, as shown in FIG. 15, the region information ψ (5,3) includes a wider range with respect to the arm angle ψ than the region information ψ (5,1). 5,3) can be said to be larger than the region information ψ (5,1). Accordingly, as shown in FIG. 20, the combination creating unit 22 “ψ (0,1), ψ (1,2), ψ (2,2), ψ (3,3), ψ (4,3) , Ψ (5,3) ”is selected and adopted, and“ ψ (0,1), ψ (1,2), ψ (2,2), ψ (3,3), ψ (4 , 3), ψ (5, 3) ”. Then, the combination creating unit 22 stores the created and selected combination information in the RAM 11 and returns the process to the control flow of FIG. 3 (S514).

次に、冗長自由度決定手段23は、図21に示すように、RAM11から組み合わせ情報を読み込み、組み合わせ情報に基づいて、CP制御の各ステップにおける冗長マニピュレータ1のアームアングルψを決定する(S310)。各ステップにおける冗長マニピュレータ1の決定されたアームアングルψは、図21において、丸印で示している。本実施形態では、図21に示すように、冗長自由度決定手段23は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ(i,j)の中間値を採用している。そして、冗長自由度決定手段23は、決定した各ステップ毎のアームアングルψをRAM11に記憶させる。   Next, as shown in FIG. 21, the redundant degree-of-freedom determination means 23 reads combination information from the RAM 11, and determines the arm angle ψ of the redundant manipulator 1 in each step of CP control based on the combination information (S310). . The determined arm angle ψ of the redundant manipulator 1 at each step is indicated by a circle in FIG. In the present embodiment, as shown in FIG. 21, the redundancy degree determining means 23 employs an intermediate value of the region information ψ (i, j) belonging to the combination information as the arm angle ψ for each step. . Then, the redundancy degree of freedom determining means 23 stores the determined arm angle ψ for each step in the RAM 11.

以上に本願発明の好適な第4実施形態を説明した。第4実施形態は、要するに、以下の特徴を有している。   The preferred fourth embodiment of the present invention has been described above. In short, the fourth embodiment has the following features.

組み合わせ作成手段22は、領域情報ψ(i,j)の組み合わせが複数作成可能であったときは、最終のステップにおける領域情報ψ(i,j)が最も大きな組み合わせを採用する。以上の構成によれば、最終のステップにおいて領域情報ψ(i,j)が最大化される。従って、ステップs5において、手先8の位置及び姿勢状態をそのままにしつつ、アームアングルψを大きく変化させることが可能となる。このことは、例えばステップs5から更に手先8の位置及び姿勢状態を変化させようとする場合、その変化の最初のステップにおけるアームアングルψの選択の柔軟性に優れることから、少なくとも以下の3つのメリットが生じる。第1に、領域情報ψ(i,j)の組み合わせの作成が可能となる確率が高くなる。第2に、領域情報ψ(i,j)の組み合わせの作成可能な組数が複数となる確率が高くなる。第3に、領域情報ψ(i,j)の組み合わせの作成可能な組数がより多くなる可能性がある。   When a plurality of combinations of area information ψ (i, j) can be created, the combination creating unit 22 employs the combination having the largest area information ψ (i, j) in the final step. According to the above configuration, the region information ψ (i, j) is maximized in the final step. Therefore, in step s5, it is possible to greatly change the arm angle ψ while keeping the position and posture state of the hand 8 as they are. This is because, for example, when the position and posture state of the hand 8 is further changed from step s5, the flexibility of selection of the arm angle ψ in the first step of the change is excellent, so at least the following three merits: Occurs. First, the probability that a combination of region information ψ (i, j) can be created increases. Secondly, there is a high probability that the number of combinations that can be created for combinations of region information ψ (i, j) is plural. Thirdly, there is a possibility that the number of combinations that can be created for combinations of the region information ψ (i, j) may be increased.

以上に本願発明の好適な第1〜第4実施形態を説明したが、これらの実施形態は適宜に組み合わせることができる。   The preferred first to fourth embodiments of the present invention have been described above, but these embodiments can be combined as appropriate.

1 冗長マニピュレータ
20 状態取得手段
21 領域算出手段
22 組み合わせ作成手段
23 冗長自由度決定手段
24 関節角算出手段
25 CP制御手段
100 制御装置(決定装置)
ψ アームアングル(冗長自由度)
ψ(i,j) 領域情報
Tn 状態情報
DESCRIPTION OF SYMBOLS 1 Redundant manipulator 20 State acquisition means 21 Area | region calculation means 22 Combination preparation means 23 Redundancy freedom determination means 24 Joint angle calculation means 25 CP control means 100 Control apparatus (determination apparatus)
ψ Arm angle (redundancy freedom)
ψ (i, j) region information
Tn status information

Claims (15)

冗長マニピュレータの手先の位置及び姿勢状態を、CP(Continuous Point)制御にて、第1の位置及び姿勢状態から第2の位置及び姿勢状態へと変化させるに際し、CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する方法であって、
各ステップにおける前記手先の位置及び姿勢状態を取得し、
各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出し、
隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第1の位置及び姿勢状態から前記第2の位置及び姿勢状態に至る間の前記領域の組み合わせを作成し、
前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定方法。
When the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by CP (Continuous Point) control, the redundant manipulator in each step of CP control A method for determining the redundancy degree of freedom of
Obtain the position and posture state of the hand in each step,
Based on the position and posture state of the hand in each step, calculate a feasible region of redundancy freedom of the redundant manipulator in each step,
Create a combination of the regions from the first position and posture state to the second position and posture state so that the regions at least partially overlap between adjacent steps,
Determining a redundancy degree of freedom of the redundant manipulator in each step of the CP control based on the combination of the regions;
A method for determining the redundancy degree of freedom of a redundant manipulator.
請求項1に記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する、
冗長マニピュレータの冗長自由度の決定方法。
A method for determining a redundancy degree of freedom of a redundant manipulator according to claim 1,
When a plurality of combinations of the areas can be created, the area in the final step adopts the largest combination,
A method for determining the redundancy degree of freedom of a redundant manipulator.
請求項1又は2に記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定方法。
A method for determining a redundancy degree of freedom of a redundant manipulator according to claim 1 or 2,
When determining the redundancy degree of freedom of the redundant manipulator in each step of the CP control based on the combination of the regions, the CP control is performed so that the sum of squares of the difference of the redundancy degree of freedom between the steps is minimized. Determining the redundancy degree of freedom of the redundant manipulator at each step;
A method for determining the redundancy degree of freedom of a redundant manipulator.
請求項1〜3の何れかに記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記冗長マニピュレータは、7自由度である、
冗長マニピュレータの冗長自由度の決定方法。
A method for determining the redundancy degree of freedom of the redundant manipulator according to any one of claims 1 to 3,
The redundant manipulator has 7 degrees of freedom.
A method for determining the redundancy degree of freedom of a redundant manipulator.
請求項4に記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記冗長マニピュレータは、S−R−S型である、
冗長マニピュレータの冗長自由度の決定方法。
A method for determining a redundancy degree of freedom of a redundant manipulator according to claim 4,
The redundant manipulator is of the SRS type,
A method for determining the redundancy degree of freedom of a redundant manipulator.
冗長マニピュレータの手先の位置及び姿勢状態を、CP(Continuous Point)制御にて、第1の位置及び姿勢状態から第2の位置及び姿勢状態へと変化させるに際し、CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する装置であって、
各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、
各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、
隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第1の位置及び姿勢状態から前記第2の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、
前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、
を備える、
冗長マニピュレータの冗長自由度の決定装置。
When the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by CP (Continuous Point) control, the redundant manipulator in each step of CP control A device for determining the degree of redundancy of
State acquisition means for acquiring the position and posture state of the hand in each step;
Based on the position and posture state of the hand in each step, a region calculating means for calculating a feasible region of the redundancy degree of freedom of the redundant manipulator in each step;
A combination creating means for creating a combination of the regions from the first position and posture state to the second position and posture state so that the regions at least partially overlap between adjacent steps;
Redundant degree-of-freedom determining means for determining a redundant degree of freedom of the redundant manipulator in each step of the CP control based on a combination of the regions;
Comprising
A device for determining the degree of redundancy of a redundant manipulator.
請求項6に記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する、
冗長マニピュレータの冗長自由度の決定装置。
A redundant manipulator redundancy degree determining apparatus according to claim 6,
The combination creating means adopts the combination having the largest area in the final step when a plurality of combinations of the areas can be created.
A device for determining the degree of redundancy of a redundant manipulator.
請求項6又は7に記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定装置。
A redundant manipulator determining apparatus for determining the degree of freedom of redundancy according to claim 6 or 7,
The redundant degree-of-freedom determining means determines the redundant sum of squares of the redundant degrees of freedom between steps when determining the redundant degree of freedom of the redundant manipulator in each step of the CP control based on the combination of the regions. And determining the redundancy degree of freedom of the redundant manipulator in each step of the CP control.
A device for determining the degree of redundancy of a redundant manipulator.
請求項6〜8の何れかに記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記冗長マニピュレータは、7自由度である、
冗長マニピュレータの冗長自由度の決定装置。
A device for determining the redundancy degree of freedom of the redundant manipulator according to any one of claims 6 to 8,
The redundant manipulator has 7 degrees of freedom.
A device for determining the degree of redundancy of a redundant manipulator.
請求項9に記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記冗長マニピュレータは、S−R−S型である、
冗長マニピュレータの冗長自由度の決定装置。
A redundant manipulator redundancy degree determining apparatus according to claim 9,
The redundant manipulator is of the SRS type,
A device for determining the degree of redundancy of a redundant manipulator.
冗長マニピュレータの手先の位置及び姿勢状態を、CP(Continuous Point)制御にて、第1の位置及び姿勢状態から第2の位置及び姿勢状態へと変化させるに際し、CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するためのプログラムであって、
コンピュータを、
各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、
各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、
隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第1の位置及び姿勢状態から前記第2の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、
前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、
として機能させる、
冗長マニピュレータの冗長自由度の決定プログラム。
When the position and posture state of the hand of the redundant manipulator are changed from the first position and posture state to the second position and posture state by CP (Continuous Point) control, the redundant manipulator in each step of CP control A program for determining the redundancy degree of
Computer
State acquisition means for acquiring the position and posture state of the hand in each step;
Based on the position and posture state of the hand in each step, a region calculating means for calculating a feasible region of the redundancy degree of freedom of the redundant manipulator in each step;
A combination creating means for creating a combination of the regions from the first position and posture state to the second position and posture state so that the regions at least partially overlap between adjacent steps;
Redundant degree-of-freedom determining means for determining a redundant degree of freedom of the redundant manipulator in each step of the CP control based on a combination of the regions;
Function as
A program for determining the degree of redundancy of redundant manipulators.
請求項11に記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する、
冗長マニピュレータの冗長自由度の決定プログラム。
A redundancy manipulator determination program for redundant manipulators according to claim 11,
The combination creating means adopts the combination having the largest area in the final step when a plurality of combinations of the areas can be created.
A program for determining the degree of redundancy of redundant manipulators.
請求項11又は12に記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記CP制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定プログラム。
A program for determining a redundancy degree of freedom of a redundant manipulator according to claim 11 or 12,
The redundant degree-of-freedom determining means determines the redundant sum of squares of the redundant degrees of freedom between steps when determining the redundant degree of freedom of the redundant manipulator in each step of the CP control based on the combination of the regions. And determining the redundancy degree of freedom of the redundant manipulator in each step of the CP control.
A program for determining the degree of redundancy of redundant manipulators.
請求項11〜13の何れかに記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記冗長マニピュレータは、7自由度である、
冗長マニピュレータの冗長自由度の決定プログラム。
A program for determining the redundancy degree of freedom of the redundant manipulator according to any one of claims 11 to 13,
The redundant manipulator has 7 degrees of freedom.
A program for determining the degree of redundancy of redundant manipulators.
請求項14に記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記冗長マニピュレータは、S−R−S型である、
冗長マニピュレータの冗長自由度の決定プログラム。
A redundancy manipulator determination program for redundant manipulators according to claim 14,
The redundant manipulator is of the SRS type,
A program for determining the degree of redundancy of redundant manipulators.
JP2010182804A 2010-08-18 2010-08-18 Method for determining redundant degree of freedom of redundant manipulator Pending JP2012040633A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2010182804A JP2012040633A (en) 2010-08-18 2010-08-18 Method for determining redundant degree of freedom of redundant manipulator

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2010182804A JP2012040633A (en) 2010-08-18 2010-08-18 Method for determining redundant degree of freedom of redundant manipulator

Publications (1)

Publication Number Publication Date
JP2012040633A true JP2012040633A (en) 2012-03-01

Family

ID=45897502

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2010182804A Pending JP2012040633A (en) 2010-08-18 2010-08-18 Method for determining redundant degree of freedom of redundant manipulator

Country Status (1)

Country Link
JP (1) JP2012040633A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014161930A (en) * 2013-02-22 2014-09-08 Seiko Epson Corp Robot control system, robot, teaching device, program and robot control method
WO2018088447A1 (en) * 2016-11-11 2018-05-17 Ntn株式会社 Work device and double-arm-type work device

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014161930A (en) * 2013-02-22 2014-09-08 Seiko Epson Corp Robot control system, robot, teaching device, program and robot control method
WO2018088447A1 (en) * 2016-11-11 2018-05-17 Ntn株式会社 Work device and double-arm-type work device

Similar Documents

Publication Publication Date Title
Carretero et al. Kinematic analysis and optimization of a new three degree-of-freedom spatial parallel manipulator
Gan et al. Unified kinematics and singularity analysis of a metamorphic parallel mechanism with bifurcated motion
Tsai et al. Kinematic analysis of 3-DOF position mechanisms for use in hybrid kinematic machines
Tao et al. Interference analysis and workspace optimization of 3-RRR spherical parallel mechanism
JP5580850B2 (en) Fast grip contact calculation for serial robots
Gómez-Bravo et al. Collision free trajectory planning for hybrid manipulators
JP4693643B2 (en) Robot teaching support apparatus and program therefor
Ruggiu et al. Mobility and kinematic analysis of a parallel mechanism with both PPR and planar operation modes
Brandstötter et al. An analytical solution of the inverse kinematics problem of industrial serial manipulators with an ortho-parallel basis and a spherical wrist
Bihari et al. A geometric approach for the workspace analysis of two symmetric planar parallel manipulators
Wu et al. A family of rotational parallel manipulators with equal-diameter spherical pure rotation
Zhang et al. Inverse kinematics of modular Cable-driven Snake-like Robots with flexible backbones
Gharahsofloo et al. An efficient algorithm for workspace generation of delta robot
Landuré et al. Kinematic analysis of a novel kinematically redundant spherical parallel manipulator
Wang et al. Optimal design of a 2-UPR-RPU parallel manipulator
JP2012040633A (en) Method for determining redundant degree of freedom of redundant manipulator
Li et al. Stiffness-maximum trajectory planning of a hybrid kinematic-redundant robot machine
Wang et al. Optimum configuration design and sensitivity analysis of the 3-R RR PPMs with a general kinematic model
Hou et al. Workspace analysis and optimization of 3-puu parallel mechanism in medicine base on genetic algorithm
Ananthanarayanan et al. Real-time inverse kinematics of redundant manipulator using a hybrid (analytical and numerical) method
Huang et al. Design and kinematic analysis of a multiple-mode 5R2P closed-loop linkage
Chung Synthesis of spatial mechanism UR-2SS for path generation
Saafi et al. Development of a spherical parallel manipulator as a haptic device for a tele-operation system: Application to robotic surgery
KR101640135B1 (en) Muti-degree of freedom(dof) system having parallel structure and method for deriving optimum solution thereof
Meng et al. Extension of Motion/Force Transmission Index to Parallel Manipulators With Double Platforms: Case Study on the Par4 Manipulator