JP3224839B2 - マニピュレータ制御装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、第１関節乃至第７関節
の７個の関節を有するマニピュレータの制御装置に係
り、特に、マニピュレータの先端部の第５関節乃至第７
関節の３個の関節が互いにロール・ピッチ・ロールの関
係にあるとともに第５関節乃至第７関節の関節軸が一つ
の交点を有し、前記制御装置は前記交点の位置を目標位
置に従うようにかつ第７関節に装着された手先の姿勢を
目標姿勢に従うように各々制御するマニピュレータ制御
装置に関する。

【０００２】

【従来の技術】工場内で使用される産業用ロボットを中
心として、６個の関節を有するマニピュレータが多く用
いられている。これらの多くのマニピュレータは、先端
部の３個の関節の関節軸が一点で交わりかつこれらの関
節がロール・ピッチ・ロールの関係にある。

【０００３】このような６個の関節を有するマニピュレ
ータの先端に装着された手先は、マニピュレータが６自
由度を有するために、マニピュレータの動作範囲内にあ
る限り、３次元空間の任意の位置および姿勢を実現する
ことができる。

【０００４】通常、マニピュレータ制御装置は、指定さ
れた目標位置および目標姿勢を表す座標値を各関節の関
節角度の値に座標変換して目標関節角度を求め、次にこ
れらの目標関節角度を実現するように各関節軸を制御す
る。

【０００５】先端部の３個の関節がロール・ピッチ・ロ
ールの関係にあるこれらのマニピュレータが目標とする
位置および姿勢を実現しようとする場合において、次の
２つのやり方が通常よく考えられる。

【０００６】すなわち、一つはマニピュレータの先端の
先端位置と先端姿勢とを制御目標とするもの、他は先端
部の３個の関節の関節軸の交点の位置とマニピュレータ
の先端姿勢とを制御目標とするものである。

【０００７】後者の場合、すなわち先端部の３個の関節
の関節軸の交点の位置を目標位置にしかつマニピュレー
タの先端の姿勢を目標姿勢にする場合には、マニピュレ
ータの基端部の３個の関節によって目標位置を規定し、
先端部の３個の関節によって目標姿勢を規定することに
なる。このため、基端部の３個の関節と先端部の３個の
関節とが分離して扱うことができるので、目標位置およ
び目標姿勢から目標関節角度を求めるための座標変換を
容易に行うことができる。

【０００８】一方、近年、研究開発用を始めとして７個
の関節を有するマニピュレータが開発され始めている。

【０００９】マニピュレータを制御目標とする目標位置
および目標姿勢へ制御できるためには、マニピュレータ
は６個の関節を有すれば十分である。従って、７個の関
節を有するマニピュレータにおいては、目標位置および
目標姿勢を実現するための関節角度は一意には決まらな
い。この残りの一自由度を利用することによって、障害
物の回避、無理のない姿勢の実現あるいは特異姿勢の回
避などを可能にすることができる。

【００１０】障害物の回避とは、目標位置および目標姿
勢を実現しなが肘の姿勢を変化させることによりマニピ
ュレータのリンクが障害物と衝突することを防ぐことで
ある。

【００１１】また、無理のない姿勢の実現とは、ある関
節の関節角度が可動範囲限界付近にまでできるだけ達し
ないように、目標位置および目標姿勢を実現できる種々
の関節角度から適当な関節角度を選択することである。
一般に、６個の関節を有すれば可動範囲内の任意の位置
および姿勢を実現できるが、目標とする位置や姿勢の値
によってはマニピュレータがかなり無理の姿勢になるこ
とがおうおうにしてある。人間の腕は通常、あまり無理
な姿勢をとらない。この理由は人間の腕が７個の関節を
有することに大きく負っている。同様にマニピュレータ
においても、７個の関節を利用することによって無理の
ない姿勢を実現できる。

【００１２】また、無理のない姿勢は、できるだけ多数
の関節を寄与させることによって実現される姿勢であ
る。制御目標を実現するための動作を特定の関節のみに
偏って負担させることは、各関節にはそれぞれ可動範囲
があるのでその関節の可動範囲限界によって制限され、
マニピュレータの可動範囲が狭くなるからである。

【００１３】また、特異姿勢の回避とは、例えば複数の
関節軸が一致あるいは平行になったりすることを回避す
ることである。このような特異姿勢においては、複数の
関節で１個の自由度しか実現できなくなるからである。

【００１４】上述のように、７個の関節を有するマニピ
ュレータは余分の１個の関節を種々の目的を実現するた
めに利用することが可能である。

【００１５】

【発明が解決しようとする課題】しかしながら、これま
で研究開発されてきた７個の関節を有するマニピュレー
タは障害物の回避に利用するものがほとんどであった。
あるいは、そのときどきのマニピュレータの肘の角度を
外部から指定して目的を実現するのみのものであった。

【００１６】従って、特別な障害物がない場合には、マ
ニピュレータが余分の１個の関節を有することを十分に
は利用してはいなかった。また、外部から目標値を与え
なくとも、できるだけ無理のない姿勢を実現するマニピ
ュレータは見受けられなかった。すなわち、目標位置お
よび目標姿勢を実現する関節角度の中で最も無理のない
目標関節角度を算出し、この目標関節角度に追従するよ
うにマニピュレータを制御するマニピュレータ制御装置
は見受けられず、冗長自由度を十分には利用していなか
った。

【００１７】そこで本発明の目的は、上記従来技術の有
する問題を解消し、目標位置および目標姿勢を実現する
関節角度の中で、マニピュレータの姿勢に無理のない各
関節の目標関節角度を演算するマニピュレータ制御装置
を提供することである。

【００１８】

【課題を解決するための手段】上記目的を達成するため
に、本発明は、第１関節乃至第７関節の７個の関節を有
するマニピュレータの制御装置であって、前記マニピュ
レータの先端部の第５関節乃至第７関節の３個の関節が
互いにロール・ピッチ・ロールの関係にあるとともに第
５関節乃至第７関節の関節軸が一つの交点を有し、前記
制御装置は前記交点の位置を目標位置に従うようにかつ
第７関節に装着された手先の姿勢を目標姿勢に従うよう
に各々制御するものであり、前記制御装置は前記手先の
目標姿勢の値より第５関節乃至第７関節の目標関節角度
を演算する姿勢演算装置と、前記交点の目標位置の値よ
り目標位置を実現するための第１関節乃至第４関節の関
節角度の成分と第５関節の関節軸方向と第７関節の関節
軸方向とのずれの大きさを小さくするように作用する第
１関節乃至第４関節の関節角度の成分とを演算し、これ
らの成分を加算して第１関節乃至第４関節の目標関節角
度を演算する位置演算装置と、前記姿勢演算装置および
前記位置演算装置の演算結果により第１関節乃至第７関
節の関節角度を制御する関節角度制御装置とを備え、前
記位置演算装置は、第５関節の関節軸方向と第７関節の
関節軸方向とのずれの大きさを小さくするように作用す
る第１関節乃至第４関節の関節角度の成分の大きさを、
第５関節の関節軸方向と第７関節の関節軸方向とのずれ
の大きさに応じて変動させることによって、第１関節乃
至第７関節の関節角度の所望に応じて基準として設定さ
れる基準関節角度からの各々の偏差の大きさが所定範囲
にあるように第１関節乃至第４関節の目標関節角度を演
算することを特徴とする。

【００１９】

【作用】マニピュレータの先端部の第５関節乃至第７関
節の３個の関節が互いにロール・ピッチ・ロールの関係
にあるとともに第５関節乃至第７関節の関節軸が一つの
交点を有するので、先端部の第５関節乃至第７関節の３
個の関節によって目標姿勢を規定し、基端部の第１関節
乃至第４関節の４個の関節によって目標姿勢を規定し、
先端部の３個の関節と基端部の４個の関節とを分離して
扱うことができるので、目標位置および目標姿勢から目
標関節角度を求めるための座標変換を容易に行うことが
できる。

【００２０】位置演算装置は目標位置を実現するための
第１関節乃至第４関節の関節角度の成分のみならず、第
５関節の関節軸方向と第７関節の関節軸方向とのずれの
大きさを小さくするように作用する第１関節乃至第４関
節の関節角度の成分を演算し、これらの成分を加算して
第１関節乃至第４関節の目標関節角度を演算する。

【００２１】さらに、位置演算装置は、目標姿勢を規定
する第１関節乃至第４関節の４個の関節の残余の一自由
度を利用して、第５関節の関節軸方向と第７関節の関節
軸方向とのずれの大きさを小さくするように作用する第
１関節乃至第４関節の関節角度の成分の大きさを、第５
関節の関節軸方向と第７関節の関節軸方向とのずれの大
きさに応じて変動させることによって、第１関節乃至第
７関節の関節角度の基準関節角度からの各々の偏差の大
きさが所定範囲にあるように第１関節乃至第４関節の目
標関節角度を演算する。

【００２２】

【実施例】以下に本発明によるマニピュレータ制御装置
の一実施例を図１および図２を参照して説明する。

【００２３】図２に示すように、７個の関節を有するマ
ニピュレータ１０が基台２０に取り付けられている。マ
ニピュレータ１０の７個の関節は、基端部の第１関節１
１乃至第４関節１４の４個の関節と、先端部の第５関節
１５乃至第７関節１７の３個の関節とからなる。第７関
節１７の先端には手先２１が装着されている。

【００２４】また、図２（Ｂ）には、第１関節１１乃至
第７関節１７の構成関係が概略的に示されている。

【００２５】マニピュレータ１０の先端部の第５関節１
５、第６関節１６および第７関節１７の３個の関節は互
いにロール・ピッチ・ロールの関係にある。すなわち、
第７関節１７の関節軸方向１７ａは手先２１の先端方向
に向き、かつ第６関節１６の関節軸方向１６ａと垂直に
ある。また、第５関節１５の関節軸方向１５ａは第６関
節１６の関節軸方向１６ａと垂直にあり、かつ第７関節
１７の関節軸方向１７ａと同一平面上にある。図２にお
いて、第５関節１５の関節軸方向１５ａと第７関節１７
の関節軸方向１７ａとは一直線をなす状態を示している
が、一般には第５関節１５の関節軸方向１５ａと第７関
節１７の関節軸方向１７ａとはずれている。

【００２６】また、第５関節１５、第６関節１６および
第７関節１７の３個の関節の関節軸は一つの交点２２を
有する。

【００２７】マニピュレータ１０の位置の制御対象とし
て交点２２を選択し、マニピュレータの姿勢の制御対象
として手先２１の姿勢を選択する。そして、マニピュレ
ータ１０は交点２２の位置を目標位置に従うようにかつ
手先２１の姿勢を目標姿勢に従うように、マニピュレー
タ制御装置１によって各関節の目標関節角度を演算し制
御する。

【００２８】次に図１にマニピュレータ制御装置１の概
略を示す。

【００２９】図１において、マニピュレータ制御装置１
は交点２２の目標位置の値より第１関節１１乃至第４関
節１４の目標関節角度を演算することに関する位置演算
装置２と、手先２１の目標姿勢の値より第５関節１５乃
至第７関節１７の目標関節角度を演算する姿勢演算装置
３と、位置演算装置２および姿勢演算装置３の演算結果
により第１関節乃至第７関節の関節角度を制御する関節
角度制御装置４とを備えている。

【００３０】次に、図１を参照してマニピュレータ制御
装置１の作用について説明する。

【００３１】位置演算装置２には交点２２の位置の指定
された目標位置に関する情報が外部から入力される。ま
た、姿勢演算装置３には手先２１の姿勢の指定された目
標姿勢に関する情報が外部から入力される。

【００３２】位置演算装置２は交点２２の目標位置の値
より、目標位置を実現するための第１関節乃至第４関節
の関節角度の成分である第１成分５のみならず、第５関
節の関節軸方向と第７関節の関節軸方向とのずれの大き
さを小さくするように作用する第１関節乃至第４関節の
関節角度の成分である第２成分６とを演算する。位置演
算装置２は第１成分と第２成分とを加算して第１関節乃
至第４関節の目標関節角度を演算する。

【００３３】また、位置演算装置２は第２成分６の大き
さが第５関節の関節軸方向と第７関節の関節軸方向との
ずれの大きさに応じて変動させることによって、第１関
節乃至第７関節の関節角度の基準関節角度からの各々の
偏差の大きさが所定範囲にあるように第１関節乃至第４
関節の目標関節角度を演算する。

【００３４】位置演算装置２および姿勢演算装置３の演
算結果は関節角度制御装置４に入力される。関節角度制
御装置４は入力された情報に基づきマニピュレータ１０
の第１関節乃至第７関節の関節角度を制御する。

【００３５】以下に数式を用いて上述の内容を説明す
る。

【００３６】交点２２の位置を表す位置ベクトルをｘ、
第１関節１１乃至第４関節１４の関節角度を表す関節角
度ベクトルをθとするとき、位置ベクトルｘと関節角度
ベクトルθの関係は式（１）のように関数ｆで表すこと
ができる。

【００３７】

【数１】 ここで、Ｔは転置を表し、以下においても同様である。
また、ｆ１、ｆ２およびｆ３は各々ｆに関する空間成分
を表す。

【００３８】また、位置ベクトルｘの微小変化量Δｘと
関節角度ベクトルθの微小変化量Δθとの間には、式
（２）で表される関係が成立する。

【００３９】

【数２】 ここで、Ｊ（θ）はヤコビアン行列である。また、θｊ
は第ｊ関節の関節角度である。

【００４０】次に式（２）より、Δｘが与えられたとき
のΔθを求める関係を求める。この関係は式（３）のよ
うに表すことができる。

【００４１】

【数３】 次に、以下に目標関節角度の求め方について説明する。

【００４２】まず、式（１）を用いて第１関節乃至第４
関節の現在の関節角度から交点２２の現在の位置を算出
する。

【００４３】次に交点２２の位置の目標位置と現在位置
との差Δｘを求めた後、式（３）に従ってΔθを求め
る。

【００４４】式（３）から算出したΔθを現在の関節角
度に加えることにより、第１関節乃至第４関節の目標関
節角度が得られる。

【００４５】なお、Δｘの値が大きくなると式（３）は
厳密には成立しない。しかし、マニピュレータ制御装置
１は数ｍｓｅｃから数１０ｍｓｅｃという短い周期で繰
り返し計算を行うので、この短い周期時間においてはΔ
ｘの値が十分に小さいので問題はない。

【００４６】式（３）の右辺において、第１項は交点２
２の目標位置の値より目標位置を実現するための第１関
節１１乃至第４関節１４の関節角度の成分である第１成
分（図１における符号５）を表す。また、第２項は第５
関節１５の関節軸方向１５ａと第７関節１７の関節軸方
向１７ａとのずれの大きさを小さくするように作用する
第１関節１１乃至第４関節１４の関節角度の成分である
第２成分（図１における符号６）を表す。

【００４７】第２成分は交点２２の位置の変化になんら
影響を与えない成分である。すなわち、ベクトルｋの値
をどうのように設定しても交点２２の位置は変化しな
い。

【００４８】この第２成分の有する性質を利用して、以
下のようにしてベクトルｋの値を算出することにより、
第５関節１５の関節軸方向１５ａと第７関節１７の関節
軸方向１７ａとのずれをできるだけ小さくするように、
第１関節１１乃至第４関節１４の目標関節角度を算出す
る。

【００４９】（１） 手先２１の目標姿勢の値から、第
７関節１７の関節軸方向１７ａの単位ベクトルｐ＝（ｐ
１、ｐ２、ｐ３）を算出する。ここで、ｐ１、ｐ２、ｐ
３はベクトルｐの空間成分を表す。

【００５０】（２） 第５関節１５の関節軸方向１５ａ
の単位ベクトルｑ＝（ｑ１、ｑ２、ｑ３）を第１関節１
１乃至第４関節１４の関節角度θｉ（ｉ＝１〜４）で表
したものをｑ＝（θ１、θ２、θ３、θ４）とすると
き、式（５）を満たすようにベクトルｋを求める。

【００５１】

【数４】 ここで、Ｌは単位ベクトルｐ＝（ｐ１、ｐ２、ｐ３）と
単位ベクトルｑ＝（ｑ１、ｑ２、ｑ３）との距離であ
り、式（５）によって表される。

【００５２】

【数５】 式（４）の計算は、計測自動制御学会誌「計測と制
御」、Ｖｏｌ．２５，Ｎｏ．１，吉川恒夫著「ロボット
アームの制御方式」の第２サブタスクの評価関数ｐをＬ
と置換し、このＬに対して、「ロボットアームの制御方
式」の記載中の（４．６）式の計算を行ったことに相当
する。

【００５３】ただし、本実施例においては、「ロボット
アームの制御方式」の記述と異なり評価関数となるＬの
値をなるべく小さくするようにベクトルｋの値を決めた
いので、式（４）の右辺に負符号を付けてある。

【００５４】式（４）により算出したベクトルｋを用い
て、式（３）における第２成分を求める。そして、式
（３）において、第１成分と第２成分との和を演算して
Δθを求める。上述したように、式（３）から算出した
Δθを現在の関節角度に加えることにより、第１関節乃
至第４関節の目標関節角度を求める。これにより、第５
関節１５の関節軸方向１５ａと第７関節１７の関節軸方
向１７ａとのずれをできるだけ小さくするように、第１
関節１１乃至第４関節１４の目標関節角度を算出するこ
とができる。

【００５５】以下にベクトルｋの大きさの設定の仕方を
説明する。なお、ベクトルｋの大きさは式（４）におけ
る各成分の自乗和の平方根として与えられる。

【００５６】ベクトルｋの大きさは次のことを満たすよ
うに決められる。

【００５７】（１） マニピュレータ１の先端部の３個
の関節である第５関節１５乃至第７関節１７の関節角度
が基準となる関節角度である基準関節角度から大きく離
れるにつれて、式（３）における第２成分を増大させる
ようにベクトルｋの大きさを設定する。これにより、第
５関節１５の関節軸方向１５ａと第７関節１７の関節軸
方向１７ａとのずれが極度に大きくなることを防止する
ことができる。

【００５８】（２） マニピュレータ１の基端部の４個
の関節である第１関節１１乃至第４関節１４の関節角度
が基準関節角度から大きく離れるにつれて、式（３）に
おける第２成分を減少させるようにベクトルｋの大きさ
を設定する。これにより、第１関節１１乃至第４関節１
４の関節角度が基準関節角度から極度に離れることを防
止することができる。

【００５９】次に、上述の（１）および（２）を満たす
ようなベクトルｋの大きさの設定について、式（６）乃
至式（１０）を用いて具体的に説明する。

【００６０】式（６）に、先端部の３個の関節である第
５関節１５乃至第７関節１７の関節角度の基準角度から
の偏差ベクトルΔθｔを示す。

【００６１】

【数６】 また、基端部の４個の関節である第１関節１１乃至第４
関節１４の関節角度の基準角度からの偏差ベクトルΔθ
ｒを示す。

【００６２】

【数７】 次に、先端部の３個の関節の関節角度の基準関節角度か
らの距離に相当する評価関数Ｌｔを式（８）によって定
義する。

【００６３】

【数８】 また、基端部の４個の関節の関節角度の基準関節角度か
らの距離に相当する評価関数Ｌｒを式（９）によって定
義する。

【００６４】

【数９】 式（８）および式（９）において、係数ａｉ（１＝１〜
７）は第ｉ関節の関節角度の基準関節角度からの偏差を
どれだけ重視するかについての重み係数であり、正の定
数である。すべての関節について各関節角度の各基準関
節角度からの偏差を小さくするためには、例えば係数ａ
ｉ（１＝１〜７）＝１のように、すべての成分を１に設
定すればよい。

【００６５】次に、式（１０）に示すように、ＬｔとＬ
ｒとの比λを求める。

【００６６】

【数１０】 最終的に、ベクトルｋの大きさを比λに比例するように
設定する。

【００６７】このようにベクトルｋの大きさを設定する
ことにより、第５関節１５の関節軸方向１５ａと第７関
節１７の関節軸方向１７ａとのずれが極度に大きくなる
ことを防止することができるとともに、基端部の４個の
関節の関節角度が基準関節角度から極度に離れることを
防止することができる。

【００６８】なお、第５関節１５の関節軸の回りのロー
ル回転による関節角度の基準関節角度からの偏差と、第
７関節１７の関節軸の回りのロール回転による関節角度
の基準関節角度からの偏差とは、互いに逆符号になるよ
うにすることによって、無理な姿勢を容易に回避するこ
とができる。また、手先２１の装着された第７関節１７
の関節軸の回りのロール回転のみが要求されるときは、
第５関節１５の関節軸の回りのロール回転を止めたま
ま、第７関節１７の関節軸の回りのロール回転の許容範
囲内で任意の姿勢をとることは何等支障がない。

【００６９】本実施例の構成によれば、ベクトルｋの大
きさを比λに比例するように設定したので、第５関節１
５の関節軸方向１５ａと第７関節１７の関節軸方向１７
ａとのずれが極度に大きくなることを防止することがで
きるとともに、基端部の４個の関節の関節角度が基準関
節角度から極度に離れることを防止することができる。

【００７０】この結果、第１関節乃至第７関節の関節角
度の基準関節角度からの各々の偏差の大きさが無理のな
い範囲にあるように選択することができる。そして、あ
る特定の関節の関節角度のみをいたずらに大きくするこ
とを防止することができる。

【００７１】また、これによって、ある特定の関節の関
節角度の可動範囲限界によって制限されることなく、マ
ニピュレータ１０全体の可動範囲を大きくすることがで
きる。

【００７２】また、式（８）および式（９）における係
数ａｉ（１＝１〜７）の値を適当に設定することができ
るので、各関節の関節角度の基準関節角度からの偏差を
各々の偏差が有する許容範囲に応じて重みずけをするこ
とができる。

【００７３】この結果、ある目標位置および目標姿勢を
実現する上で、例えば負担を少なくしたい関節にたいし
てはその負担を少なくすることができる。

【００７４】なお、ベクトルｋの大きさを比λに比例す
るとしたが比λそのものに比例しなくとも、例えば比λ
の自乗に比例する等、比λに関する量に依存するように
設定すればよい。

【００７５】

【発明の効果】以上説明したように、本発明の構成によ
れば、位置演算装置は目標姿勢を規定する第１関節乃至
第４関節の４個の関節の残余の一自由度を利用して、第
５関節の関節軸方向と第７関節の関節軸方向とのずれの
大きさを小さくするように作用する第１関節乃至第４関
節の関節角度の成分の大きさが所定範囲で変動するよう
に第１関節乃至第４関節の目標関節角度を演算する。こ
の結果、第１関節乃至第７関節の関節角度の基準関節角
度からの各々の偏差の大きさが所定範囲にあるようにす
ることができ、マニピュレータの目標位置および目標姿
勢を実現する関節角度の中で、マニピュレータの各関節
は無理のない関節角度をとることができる。

【図面の簡単な説明】

【図１】本発明のマニピュレータ制御装置の一実施例の
ブロック図。

【図２】本発明のマニピュレータ制御装置の制御対象と
なるマニピュレータを示す斜視図（Ａ）と、マニピュレ
ータの関節の構成関係を概略的に示す図（Ｂ）。

【符号の説明】

１ マニピュレータ制御装置 ２ 位置演算装置 ３ 姿勢演算装置 ４ 関節角度制御装置 ５ 第１成分 ６ 第２成分 １０ マニピュレータ １１ 第１関節 １１ａ 第１関節の関節軸方向 １２ 第２関節 １２ａ 第２関節の関節軸方向 １３ 第３関節 １３ａ 第３関節の関節軸方向 １４ 第４関節 １４ａ 第４関節の関節軸方向 １５ 第５関節 １５ａ 第５関節の関節軸方向 １６ 第６関節 １６ａ 第６関節の関節軸方向 １７ 第７関節 ２０ 基台 ２１ 手先 ２２ 交点

───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.⁷，ＤＢ名) B25J 3/00 - 3/10 B25J 9/10 - 9/22 B25J 13/00 - 13/08 B25J 19/02 - 19/06 G05B 19/18 - 19/46

Claims (2)

(57)【特許請求の範囲】
1. 【請求項１】第１関節乃至第７関節の７個の関節を有す
   るマニピュレータの制御装置であって、 前記マニピュレータの先端部の第５関節乃至第７関節の
   ３個の関節が互いにロール・ピッチ・ロールの関係にあ
   るとともに第５関節乃至第７関節の関節軸が一つの交点
   を有し、前記制御装置は前記交点の位置を目標位置に従
   うようにかつ第７関節に装着された手先の姿勢を目標姿
   勢に従うように各々制御するものであり、 前記制御装置は前記手先の目標姿勢の値より第５関節乃
   至第７関節の目標関節角度を演算する姿勢演算装置と、 前記交点の目標位置の値より目標位置を実現するための
   第１関節乃至第４関節の関節角度の成分と第５関節の関
   節軸方向と第７関節の関節軸方向とのずれの大きさを小
   さくするように作用する第１関節乃至第４関節の関節角
   度の成分とを演算し、これらの成分を加算して第１関節
   乃至第４関節の目標関節角度を演算する位置演算装置
   と、 前記姿勢演算装置および前記位置演算装置の演算結果に
   より第１関節乃至第７関節の関節角度を制御する関節角
   度制御装置とを備え、 前記位置演算装置は、第５関節の関節軸方向と第７関節
   の関節軸方向とのずれの大きさを小さくするように作用
   する第１関節乃至第４関節の関節角度の成分の大きさ
   を、第５関節の関節軸方向と第７関節の関節軸方向との
   ずれの大きさに応じて変動させることによって、第１関
   節乃至第７関節の関節角度の所望に応じて基準として設
   定される基準関節角度からの各々の偏差の大きさが所定
   範囲にあるように第１関節乃至第４関節の目標関節角度
   を演算することを特徴とするマニピュレータ制御装置。
2. 【請求項２】第５関節の関節軸方向と第７関節の関節軸
   方向とのずれの大きさを小さくするように作用する第１
   関節乃至第４関節の関節角度の成分の大きさは、第５関
   節乃至第７関節の関節角度の基準関節角度からの距離と
   第１関節乃至第４関節の関節角度の基準関節角度からの
   距離との比またはこの比に関する量に比例することを特
   徴とする請求項１に記載のマニピュレータ制御装置。