JP2011031309A - 移動体姿勢生成方法および移動体姿勢生成装置 - Google Patents

Abstract

【課題】移動体の一部分がある動作軌道に従って動作する際、従来は移動体全体の姿勢を逆運動学によって生成していたが、計算が複雑であり、また解が多数存在する場合に解を自動的に選択することができない、という問題があった。
【解決手段】作業空間に、空間中の障害物に基づいて設定される斥力ベクトル場を定義し、移動体全体を３次元形状モデル化して表面に代表点を設定し、代表点に斥力ベクトルを作用させることで移動体全体の姿勢を自動生成する。
【選択図】図１

Description

本発明は、障害物の存在する環境で移動する物体の最適な軌道を生成する移動体軌道生成方法および移動体軌道生成装置に係る。

従来のロボットの動作設計手法には作業現場にて実際にロボットを動作させて教示するオンラインティーチング若しくはロボットを動作させる前にコンピュータ等により教示データを作成するオフラインティーチングがある。オンラインティーチングは組立作業や溶接作業などあらかじめ決められた動作を行う産業用ロボットに広く用いられており、実際の作業環境に合わせた動作の設計が可能であるという利点がある。その一方で、ティーチングに多くの時間がかかると共に、動作の設計に熟練を要するといった欠点がある。オフラインティーチングは干渉を回避するための軌道の生成を、シミュレーション等を利用して確認しながら試行錯誤で行う必要があり、設計と検証に多大な労力が必要となる。

これに対して、ロボットの手先経路の動作設計を自動化する試みとしてはポテンシャル場を用いた経路探索手法（例えば、特許文献１参照。）がある。この手法では作業空間内に目標位置が最小、現在位置が最大となるような仮想的なポテンシャルを設定する。さらに、障害物にも大きなポテンシャルを持たせることで、ポテンシャルが低くなる方向へ軌道を生成することで、障害物を回避しながら目標位置までの経路を生成できる。

さらに、生成した手先経路に沿うようにロボットを動作させるためには、ロボットのリンクと関節から構成される全体姿勢を経路上の手先位置・姿勢情報から決定する必要がある。一般的には、手先の位置・姿勢を入力条件とし、それを実現するための各関節角度を逆算的に算出する逆運動学と呼ばれる計算によって全体姿勢を決定している。

特開２００３−２８０７１０号公報

上述したポテンシャル場を用いた経路探索手法では、ポテンシャル場にローカルミニマムが生じて、経路探索時にローカルミニマムに陥ると軌道を生成できないという問題があった。ローカルミニマムとは、ポテンシャルが局所的に極小となる領域を指す。経路探索時にローカルミニマムの領域に入ると、周囲のポテンシャルがローカルミニマムの領域より高く、経路探索を続行できずに軌道生成不能となるのである。

複雑形状の障害物がある環境では、ローカルミニマムの存在しないポテンシャル場を設定することは非常に困難である。また、ローカルミニマムを回避するために迂回した経路を生成するとなると、軌道が長大になり、不要な変曲点を含む軌道となるなど、最適な軌道とならず、軌道の最適化を図る必要等があった。

また、生成した経路を実現するロボットの全体姿勢を逆運動学により求める手法では、経路上の手先の位置・姿勢を満足する全体姿勢が障害物との干渉等により物理的に存在しない場合に解を求めることができない。さらに、関節数が非常に多い構造のロボットでは、逆運動学から求まる関節角度の組合せが大量に存在するため、解を一意に決めることが困難である。

上述した課題に鑑み、本発明の目的は、ロボットの手先の目標軌道を人が試行錯誤して設計することなく自動生成でき、かつ、目標軌道に沿って移動する場合のロボット全体の姿勢も同様に自動生成できる移動体姿勢生成方法および移動体姿勢生成装置を提供することにある。

上記目的を達成するため、本発明による移動体姿勢生成方法は、移動体の一部分の動作軌道に基づいて移動体全体の姿勢を生成する移動体姿勢生成方法であって、移動体が移動する空間に存在する障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定ステップと、移動体の３次元形状モデルに斥力および引力が作用する代表点を設定する移動体形状モデル設定ステップと、代表点に作用する斥力から移動体の各関節軸に働く力とモーメントを算出する力・モーメント算出ステップと、前記動作軌道上に参照点を設定し、参照点に向けた引力を前記移動体に作用させる引力ベクトル設定ステップと、移動体の３次元形状モデルに作用する力とモーメントから移動体の各関節軸に作用するトルクを算出し、トルクに基づいて各関節軸の角度を変化させる姿勢生成ステップと、を備えることを特徴とする。

また、本発明による移動体姿勢生成方法は、移動体の一部分の動作軌道に基づいて移動体全体の姿勢を生成する移動体姿勢生成方法であって、移動体が移動する空間を設定する空間設定ステップと、前記空間中に前記移動体の一部分が動作を開始する初期位置を設定する初期位置設定ステップと、前記空間中に前記移動体の一部分の目標位置を設定する目標位置設定ステップと、前記空間に存在する障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定ステップと、前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に前記斥力ベクトルが作用する作用点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定ステップと、前記斥力ベクトル場の前記斥力ベクトルを前記作用点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成ステップと、移動体の３次元形状モデルに斥力および引力が作用する代表点を設定する移動体形状モデル設定ステップと、代表点に作用する斥力から移動体の各関節軸に働く力とモーメントを算出する力・モーメント算出ステップと、生成した動作軌道上に参照点を設定し、参照点に向けた引力を移動体に作用させる引力ベクトル設定ステップと、移動体の３次元形状モデルに作用する力とモーメントから移動体の各関節軸に作用するトルクを算出し、トルクに基づいて各関節軸の角度を変化させる姿勢生成ステップと、を備えることを特徴とする。

また、本発明による移動体姿勢生成装置は移動体の一部分の動作軌道に基づいて移動体全体の姿勢を生成する移動体姿勢生成装置であって、移動体が移動する空間に存在する障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定手段と、移動体の３次元形状モデルに斥力および引力が作用する代表点を設定する移動体形状モデル設定手段と、代表点に作用する斥力から移動体の各関節軸に働く力とモーメントを算出する力・モーメント算出手段と、前記動作軌道上に参照点を設定し、参照点に向けた引力を前記移動体に作用させる引力ベクトル設定手段と、移動体の３次元形状モデルに作用する力とモーメントから移動体の各関節軸に作用するトルクを算出し、トルクに基づいて各関節軸の角度を変化させる姿勢生成手段と、を備えることを特徴とする。

また、本発明による移動体姿勢生成装置は、移動体の一部分の動作軌道に基づいて移動体全体の姿勢を生成する移動体姿勢生成装置であって、移動体が移動する空間を設定する空間設定手段と、前記空間中に前記移動体の一部分が動作を開始する初期位置を設定する初期位置設定手段と、前記空間中に前記移動体の一部分の目標位置を設定する目標位置設定手段と、前記空間に存在する障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定手段と、前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に前記斥力ベクトルが作用する作用点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定手段と前記斥力ベクトル場の前記斥力ベクトルを前記作用点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成手段と、移動体の３次元形状モデルに斥力および引力が作用する点を設定する移動体形状モデル設定手段と、移動体に作用する斥力から移動体の各関節軸に働く力とモーメントを算出する力・モーメント算出手段と、生成した動作軌道上に参照点を設定し、参照点に向けた引力を移動体に作用させる引力ベクトル設定手段と、移動体の３次元形状モデルに作用する力とモーメントから移動体の各関節軸に作用するトルクを算出し、トルクに基づいて各関節軸の角度を変化させる姿勢生成手段と、を備えることを特徴とする。

本発明の移動体姿勢生成方法および移動体姿勢生成装置によれば、従来の逆運動学による姿勢生成では物理的に姿勢が求まらないような場合においても、近傍での解を自動的に求めることができ、姿勢を生成することが可能である。また、移動体の自由度が多く移動体の姿勢が大量に存在する場合においても、姿勢を一意に生成することができる。

第１の実施例による移動体姿勢生成のフローチャート。 障害物に基づいて設定される斥力ベクトル場の概要を示す概念図。 動作軌道モデルを構成する連成バネマス構造の概念図。 （ａ）動作軌道生成処理前の動作軌道モデルの状態を示す概念図。（ｂ）動作軌道生成処理後の動作軌道モデルの状態を示す概念図。 ロボットマニピュレータを３次元形状モデル化し、斥力ベクトルが作用する代表点を設定した状態を示す概念図。 ３次元形状モデル化されたロボットマニピュレータの手先に、動作軌道上に設定された参照点からの引力ベクトルを作用させる概念を示す図。 動作軌道に対して鉛直で参照点を含む平面を設定し、移動体の一部分の動作軌道を参照点近傍に収束させる処理の概念を示す図。 第２の実施例による移動体姿勢生成のフローチャート。 第２の実施例において障害物の移動前の状態を示す概念図。 第２の実施例において障害物が移動した状態を示す概念図。 第２の実施例において障害物の移動に伴い動作軌道を再生成した状態を示す概念図。

以下本発明の実施例について図面を参照しながら説明する。

本発明の第１の実施例について、図面を用いて以下説明する。図１は、本実施例による移動体姿勢生成方法の主要なステップを記載したフローチャートである。本実施例の移動体姿勢生成方法において行われる処理について、図１を用いて説明する。なお、各実施例においては、移動体はロボットマニピュレータ、移動体の一部分はロボットマニピュレータの手先であるものとして以下説明する。

一般的にロボットマニピュレータは関節とリンク機構で構成される。関節には回転関節や直動関節の他、球体関節などさまざまな種類がある。関節の動作はモータ等で能動的に動作する場合と、動力源を持たずに受動的に動作する場合がある。各関節間はリンク機構により結びつけられ、関節とリンク機構が交互に直列されたシリアルリンク型と関節とリンクの組合せが並列となったパラレルリンク型などがある。

以降では、回転関節を有するシリアルリンク型のロボットマニピュレータを例に説明する。

まず、ロボットマニピュレータが作業を行う作業空間を定義する処理を行う（ステップＳ１）。作業空間とは、ロボットマニピュレータの初期位置や作業位置、および初期位置と作業位置の中間を含む実際の空間の情報に基づいて構築される空間である。この作業空間は、作業を行う施設等の設計情報、例えば３次元ＣＡＤのデータを用いて設定する、またはカメラの映像や各種センサによる計測等で収集したデータを用いて設定する、あるいは手動で設定する、といったことが考えられる。

具体的には、例えばロボットマニュピレータの操作装置が関連付けられ、各種３次元形状モデルを作成、記憶、修正可能な３次元ＣＡＤシステムを構成する演算装置及び入力装置によって行われる。すなわち、上述の入力装置に施設等の設計情報や収集したデータを入力する、あるいは任意に施設情報等を入力して、上述した作業空間に対応する３次元形状モデルを設定することによって行われる。

次に、作業空間上に斥力ベクトル場を生成する処理を行う（ステップＳ２）。斥力ベクトル場の生成は、作業空間上に３次元の格子を設定し、各格子点に斥力ベクトルを定義することで行う。この斥力ベクトルはステップＳ１で定義された作業空間内の障害物を発生源として設定する。各格子点に設定される斥力ベクトルの方向は障害物表面の法線方向、すなわち障害物表面に垂直な方向とし、斥力ベクトルの大きさは障害物に近いほど大きくなるように設定する。作業空間上に複数の障害物が存在する場合、例えば各格子点に最も近い障害物から発生する斥力ベクトルを、最終的にその格子点の斥力ベクトルとして設定する。

ステップＳ１で定義された作業空間と、ステップＳ２で生成された斥力ベクトル場の例を図２に示す。図２は作業空間に設定された障害物と斥力ベクトル場の概要を示す概念図である。なお、斥力ベクトル場については、斥力ベクトルのおおよその方向のみ示しており、作業空間上に設定される３次元格子は省略している。作業空間中に円筒形状の障害物１０が設定されており、この障害物１０から遠ざかる方向の斥力ベクトル１１が設定されている。

斥力ベクトル場を設定する際、障害物１０が複雑な形状で、突起や極端な狭隘部を有する等のロボットマニピュレータの手先が通過しにくい形状や配置を取る場合は、該当箇所付近の斥力ベクトル１１を大きくする等の重み付けを行ってもよい。

次に、ロボットマニピュレータの手先の初期位置と目標位置を結ぶように、動作軌道モデルを作業空間上に生成・配置する処理を行う（ステップＳ３）。この動作軌道モデルの構造の一例を、図３を用いて説明する。動作軌道モデルは、引っ張りバネ２０とヒンジバネ２１を組み合わせた連結バネ２２を複数結合し、連結バネ２２同士の結合部には質点２３が設定された連成バネマス構造を有する。また、動作軌道モデルの両端に設定される質点２３ａ、質点２３ｂは、動作軌道モデルの配置の際、質点２３ａは手先の初期位置に、質点２３ｂは手先の目標位置に固定される。なお、図３では連結バネ２２を２つ結合したものとして図示しているが、連結バネ２２の数は任意に設定することができる。

なお、図３においては、手先の初期位置と目標位置とを直線的に結ぶように動作軌道モデルを配置しているが、直線的に結ぶ必要はなく、空間中で初期位置と目標位置とを任意に結ぶように配置されていればよい。

次に、ステップＳ３で生成した動作軌道モデルにステップＳ２で設定した斥力ベクトル１１を作用させ、動作軌道を生成する処理を行う（ステップＳ４）。動作軌道モデルに設定された各質点２３について、各質点２３周辺に設定された複数の斥力ベクトル１１の大きさ及び方向を空間補間した斥力ベクトルを作用させる。すると、動作軌道モデルを構成する引っ張りバネ２０、ヒンジバネ２１が変形する。この際、ヒンジバネ２１は隣接する質点２３間を直線形状に保つ方向に力を作用させ、ロボットマニピュレータの動作軌道３１が必要以上に湾曲することを防ぐ。

最終的には、各質点２３に作用する斥力ベクトル１１と引っ張りバネ２０、ヒンジバネ２１の張力がつりあう形状に動作軌道モデルが変形する。この変形した動作軌道モデルの描く軌道がロボットマニピュレータの動作軌道となる。

ステップＳ４における動作軌道の生成の例を、図４を用いて説明する。図４（ａ）は動作軌道モデルに斥力ベクトル１１を作用させていない状態、図４（ｂ）は動作軌道モデルに斥力ベクトル１１を作用させて動作軌道を生成した状態を示している。なお、図４においては簡略的に２次元図で表し、各質点２３を結ぶ連結バネ２２も簡略化して図示している。

ステップＳ３において動作軌道モデルを配置するが、初期状態は図４（ａ）で示すように障害物１０を貫通してもよい。なお、上述したように、質点２３ａは手先の初期位置に、質点２３ｂは手先の目標位置に固定されている。この動作軌道モデルに斥力ベクトル１１を作用させると、図４（ｂ）で示す形状に変形し、障害物１０を紙面右側から迂回する動作軌道が生成される。質点２３ａ、質点２３ｂは位置が固定されているため移動しない。

次に、ステップＳ４で生成した動作軌道について、動作軌道モデルの各質点２３間を曲線補間し、動作軌道を滑らかにする処理を行う（ステップＳ５）。

ステップＳ６における移動体形状モデル設定の例を、図５を用いて説明する。図５ではロボットマニピュレータの３次元形状モデル２５を上述した作業空間に設定し、設定した３次元形状モデル２５上に斥力ベクトル場から受ける斥力を作用させる代表点２６を複数点定義している。この代表点は、構造物との干渉を避けるために、ロボットマニピュレータの外形や内部に設定する。代表点２６の設定箇所は任意だが、例えば図５のような形状のロボットマニピュレータの場合、モデルの構成要素である直方体や円柱部分の外形表面上に均等に設定する（煩雑になるため、図５では代表点２６を一部省略して図示している。）。また、移動体の一部分である手先が作業ツールを持っている場合等、障害物との干渉が懸念される程度の大きさがある場合には手先にも代表点２６を設定する。

次に、ステップＳ６にて定義した代表点２６にステップＳ２で設定した斥力ベクトル１１の大きさと方向の斥力を作用させ、マニピュレータの各リンクごとに斥力の合計と各リンクに付随した関節軸に作用するモーメントの合計を算出する（ステップＳ７）。

次に、図６で示すように、ステップＳ４にて生成した初期位置２７ａから目標位置２７ｂまでの動作軌道上に複数の参照点２７を設定し、ある一つの参照点２７に対してロボットマニピュレータの手先の現在位置と参照点２７の位置偏差に基づく参照点２７方向の引力ベクトル３１をロボットマニピュレータの手先に作用させる（ステップＳ８）。このとき、引力ベクトル３１の大きさは、例えば位置偏差量に比例する成分、位置偏差を積分した成分、位置偏差の時間変動を微分した成分の何れか、またはこれらの組合せから算出して設定する。

次に、ステップＳ７とステップＳ８にて算出したロボットマニピュレータに作用する斥力とモーメントおよび引力を、各リンクに設定された代表点２６の数で除算して、関節軸を基準位置としてそれぞれの平均を各リンクごとに算出する。

さらに現在のロボットマニピュレータの関節角速度をデカルト座標での手先の速度へと変換するヤコビ行列 を用いることで、手先と各リンクごとに算出した斥力（または斥力と引力の和）の平均（Ｆｘ，Ｆｙ，Ｆｚ）とモーメントの平均（Ｍｘ，Ｍｙ，Ｍｚ）から各関節軸に働くトルク を式（１）により求める。

ｎは関節軸のベース側から数えた番号
例えば、ある関節軸に働く から求まるトルクτはその関節軸よりもロボットマニピュレータのベース側（手先から遠い側）にある関節軸のトルクτであり、手先に働くFの場合はすべての関節軸に働くトルクτとなる。最終的に算出したトルクτを各関節軸ごとに合算する。

最後に、算出したトルクの大きさに基づいて関節軸角度を変化させることでロボットマニピュレータの全体に作用する斥力と手先の引力がつりあうまでロボットマニピュレータの全体姿勢を変化させる（ステップＳ９）。

ヤコビ行列を用いて各関節軸に作用するトルクを算出する利点は、順運動学から算出することができるため計算が容易であることにある。一般的にロボットマニピュレータの手先の位置・姿勢からロボットマニピュレータの姿勢つまり各関節角度を算出するには逆運動学を解く必要があるが、逆運動学による解法は順運動学に比べて計算負荷が大きい。

以上のステップＳ６からステップＳ９までの一連の処理により、ステップＳ５までで得られた動作軌道と現在のロボットマニピュレータの姿勢から、動作軌道上に定義した参照点２７へロボットマニピュレータの手先を移動させた時のロボットマニピュレータの全体姿勢が自動で生成される。

次に、動作軌道上の参照点を別の参照点へと更新し（ステップＳ１０）、ステップＳ７からステップＳ９を繰り返すことで初期位置２７ａから目標位置２７ｂまでの動作軌道上に設定したすべての参照点２７におけるロボットマニピュレータの全体姿勢が生成される（ステップＳ１１）。さらに、参照点間の姿勢を補間などにより算出することで初期位置２７ａから目標位置２７ｂまでの滑らかな動作姿勢を生成することができる（ステップＳ１２）。

以上の一連の処理により、構造物との干渉を回避したロボットマニピュレータの手先の動作軌道およびロボットマニピュレータの動作姿勢が生成される。このような動作生成によれば、ポテンシャル法におけるローカルミニマムの問題が生じることなく、移動体の最適な動作軌道および動作姿勢を生成することが可能である。

また、ステップＳ１について説明したとおり移動する障害物を設定する場合は、リアルタイムに斥力ベクトル場と動作軌道モデルの形状を更新する処理を行うことも可能である。また、障害物の移動範囲が決まっている場合は、障害物の移動範囲全体を一つの障害物として設定することも可能である。

また、構造物の設計情報や、実際の作業環境の情報を収集して設定することができるため、設計・計画段階の施設、建設済みの施設の何れについても適用することが可能である。

また、動作軌道モデルを構成する引っ張りバネ２０、ヒンジバネ２１のバネ定数は任意に設定することができる。斥力ベクトル場が同一の場合、バネ定数が大きいほど動作軌道モデルは変形し難く、逆にバネ定数が小さいほど動作軌道モデルは変形し易くなる。したがって、例えば、手先形状が大きいマニピュレータの場合はバネ定数を小さくすることにより、バネ定数が固定の場合の動作軌道よりも障害物１０から離れた動作軌道が生成される。逆に、手先が小さい場合はバネ定数を大きくすることでバネ定数が固定の場合の動作軌道よりも障害物１０に近い動作軌道が生成される。換言すれば、バネ定数を任意に設定することにより、障害物１０に対する移動体の一部分の回避軌道の大きさを任意に変更することが可能である。

つまり、上述の方法により手先の大きさに合わせてロボットマニピュレータの手先と障害物１０の干渉回避距離を確保した動作軌道を生成することができる。また、動作軌道モデルを構成する各バネのバネ定数は同一でなくともよい。例えば、障害物１０が少ない箇所におけるバネ定数を大きくし、障害物１０が多く狭隘な箇所におけるバネ定数を小さくするなど、作業空間内１の障害物１０の形状に合わせて部分的にバネ定数を変えることで、バネ定数が大きい箇所では障害物１０の形状に依存することなく、より直線的な軌道を得ることができ、バネ定数が小さい箇所では障害物１０の形状に沿った干渉回避軌道を得ることができる。

また、動作軌道を生成する際に、質点２３の各々についてロボットマニピュレータの可動範囲を拘束条件としてもよい。すなわち、質点２３の各々は斥力ベクトル１１を作用させることで作業空間中を移動するが、この際にロボットマニピュレータの可動範囲外には移動できないよう設定することで、確実にロボットマニピュレータの可動範囲内に動作軌道を生成することが可能である。

また、本実施例においては動作軌道モデルに関して、間隔を空けて質点２３を離散的に配置したものとして説明したが、連続的に質点２３を配置したものとして、つまり動作軌道モデルが全体として質量を有するものとすることも可能である。

また、図７で示すように、動作軌道上の参照点２７ｃを含みかつ動作軌道に鉛直な平面２９を定義し、ロボットマニピュレータの手先の現在位置２８と鉛直平面２９との位置偏差（例えば、現在位置２８と鉛直平面２９の最短距離を用いる）に応じた引力ベクトル３１を設定し、手先にこの引力を作用させることで、最終的なロボットマニピュレータの手先位置を、参照点２７ｃを含む鉛直平面内に収束させることもできる。この手法を用いることによりロボットマニピュレータの手先の位置を参照点２７ｃに収束させることができるロボットマニピュレータの全体姿勢が物理的に存在しない場合に、参照点２７ｃの近傍におけるロボットマニピュレータの姿勢を生成することができる。

また、各関節軸に働くトルクに基づいて関節軸角度の変化量を算出する際に、関節ごとに重み係数を設定することで、各関節ごとの動き易さを設定することができる。

さらに、この重み係数が可変であることにより、ロボットマニピュレータの姿勢が特異姿勢に近づいた場合は、特異姿勢に関係する関節軸の角度の変化のし易さを変動させることで特異姿勢を回避することができる。

また、ステップＳ８の引力ベクトル設定ステップにおいて、ロボットマニピュレータの設置点に参照点２７や参照点２７を含む鉛直平面内に向けた引力を作用させることでロボットマニピュレータ全体を移動させることもできる。

これはロボットマニピュレータに限らず適用でき、歩行型ロボットなどの複数の関節を持つ移動体について、干渉を回避する姿勢を自動で生成しつつ、目標の位置まで移動させることができる。

本発明の第２の実施例について、図面を用いて以下説明する。なお、第１の実施例と同じ構成には同一の符号を付し、重複する説明は省略する。

図８は、第１の実施例のステップＳ１からステップＳ１２までの移動体姿勢生成のステップを記載したフローチャートである。本実施例による移動体姿勢生成方法は障害物が移動する作業環境を想定したもので、移動体姿勢を生成した後の処理が第１の実施例と異なっている。

まず、作業空間の定義（ステップＳ１）から動作軌道モデルの生成・配置（ステップＳ１２）まで第１の実施例と同様の処理を行う。

次に、作業空間に配置された障害物１０を移動する処理を行う（ステップＳ２１）。

次に、障害物１０の移動に伴う斥力ベクトル場の更新を行う。また、斥力ベクトル場の更新に合わせて動作軌道モデルに作用する斥力ベクトルを更新し、動作軌道モデルを更新する処理を行う。また、更新された斥力ベクトル場および動作軌道モデルを用い、ロボットマニピュレータの姿勢を更新する処理を行う（ステップＳ２２）。

次に、ステップＳ２２で更新された動作軌道モデルについて、動作軌道モデルを構成する各バネの張力が所定の閾値を超えているかいないかを判定する処理を行う（ステップＳ２３）。

ステップＳ２３の判定で、バネ張力が閾値より低かった場合は、移動体の移動を行う（ステップＳ２４）。そして、移動体の一部分（手先）が目標位置に到達したかどうか判定を行い（ステップＳ２５）、到達していた場合は処理を終了し、到達していない場合はステップＳ２１に戻り、障害物１０の移動以降のステップを繰り返し行う。

ステップＳ２３の判定で、バネ張力が閾値よりも高かった場合は、動作軌道モデルを再配置する処理を行う（ステップＳ２６）。この動作軌道モデルの再配置は、更新後の斥力ベクトル場の他、移動体の一部分の現在位置を考慮する点でステップＳ４と異なる。ステップＳ４では手先の初期位置と目標位置のみを動作軌道モデルの拘束点として配置しているが、ステップＳ２２では手先の初期位置、目標位置に加えて手先の現在位置を拘束点として動作軌道モデルを再配置する。この動作軌道モデルの再配置後、再度ステップＳ４以降のステップを繰り返す。

このような処理とすることにより、障害物１０の移動にともなって動作軌道およびロボットマニピュレータの姿勢を逐次更新することが可能である。また、動作軌道モデルを構成するバネの張力を判定することにより、障害物１０の移動によって更新された動作軌道モデルが極端な軌道を描く場合等に、動作軌道モデルの再設定を行うことによって、移動体の現在位置を考慮した最適な動作軌道およびロボットマニピュレータの姿勢への変更を自動で行うことが可能である。

本実施例による動作軌道の変更処理の例を、図面を用いて説明する。なお、ロボットマニピュレータの姿勢については図示を省略する。図９、図１０、図１１はそれぞれ、本実施例によるステップＳ１７以降の処理を説明するための概念図であり、図９は障害物１０が移動する前の状態、図１０は障害物１０が移動した後の状態、図１１はステップＳ２６を経て動作軌道を変更した例を示している。

図９はステップＳ５で最初に動作軌道を生成した状態を示している。障害物１０が複雑な形状をしており、動作軌道モデルがこの障害物１０の凹部を通過するように迂回する軌道をとっている。矢印４０はこの障害物１０の移動方向を示し、破線は移動体の一部分（手先）の初期位置と目標位置とを直線的に結んだもので、変形前の動作軌道モデルの位置に相当する。

図１０はステップＳ２１によって障害物１０が移動し、ステップＳ２２で斥力ベクトル場と動作軌道とが更新された状態を示している。障害物１０が矢印４０方向へ移動したことにより、動作軌道モデルが障害物１０をより大きく迂回するような軌道をとっていることが分かる。

図１１はステップＳ２３で動作軌道モデルのバネ張力が閾値より高いと判定され、ステップＳ２６の動作軌道モデルの再配置を経てステップＳ５で動作軌道が変更された状態を示している。なお、図１１ではステップＳ２６の時点で手先が質点２３ｃの位置まで移動していたものとして図示している。よって、手先の初期位置、目標位置のほか、質点２３ｃの位置も動作軌道モデルの拘束点となっており、動作軌道が変更された結果、図１９や図１０とは逆側から障害物１０を迂回するような軌道をとっている。なお、図１１においては、破線はステップＳ２３で判定された際の動作軌道モデルの形状を示している。

このように、障害物１０の移動に応じて動作軌道およびロボットマニピュレータの姿勢を再計算して最適な軌道・姿勢を更新することが可能である。

なお、本実施例によってはステップＳ２３において動作軌道モデルを構成するバネの張力に基づいて動作軌道モデルを再配置する処理の実行を判定するものとしたが、例えば動作軌道モデルがロボットマニピュレータの可動範囲から逸脱した場合に動作軌道モデルを再配置するような判定とすることも可能である。

また、本実施例においては、全ての参照点で姿勢を生成してから（ステップＳ１１）障害物の移動による更新処理（ステップＳ２１等）を行なうこととしているが、例えばステップＳ１１で移動体の現在位置から１つ先の参照点までの姿勢生成まででステップＳ１２に進み、更新処理（ステップＳ２１等）を行い、ステップＳ２４からステップＳ２５に進んだ場合に次の参照点での姿勢生成（ステップＳ７）以降の処理を行なう構成としても良い。このような処理とすると、ステップＳ２４からステップＳ２６に進んだ場合に、動作を再度生成する量が減るため、計算処理をより合理的にすることができる。

以上、本発明による移動体姿勢生成について複数の実施例を用いて説明してきたが、上述した各実施例においては、ロボットマニピュレータの手先の動作軌道の生成とロボットマニピュレータ全体姿勢の生成を一連の処理として説明したが、手先の動作軌道の生成方法が上述したものと異なる方法であっても、斥力ベクトル場の生成と、移動体形状モデル設定以降の処理を行うことにより、ロボットマニピュレータ全体姿勢を生成することが可能である。

また、各実施例においては移動体がロボットマニピュレータであるものとして説明したが、本発明はこれに限定されない。例えば、水中遊泳ロボット、歩行型ロボット、車輪型ロボット等の、障害物が存在する３次元空間における各種の移動体の軌道生成に関して、本発明を適用することが可能である。

また、各実施例において説明した移動体姿勢生成方法は、各処理を行う機能が実装された入力装置、演算装置、出力装置からなるシステムで実行することが可能であり、例えば各処理を行うためのプログラムを組み込んだ一般的なパーソナルコンピュータを用いることができる。

１０ 障害物
１１、１１ａ、１１ｂ 斥力ベクトル
２０ 引っ張りバネ
２１ ヒンジバネ
２２ 連結バネ
２３、２３ａ、２３ｂ 質点
２５ ３次元形状モデル
２６ 代表点
２７、２７ｃ 参照点
２７ａ 初期位置
２７ｂ 目標位置
２８ 現在位置
２９ 鉛直平面
３１ 引力ベクトル

Claims (11)

1. 移動体の一部分の動作軌道に基づいて移動体全体の姿勢を生成する移動体姿勢生成方法であって、
   移動体が移動する空間に存在する障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定ステップと、
   移動体の３次元形状モデルに斥力および引力が作用する代表点を設定する移動体形状モデル設定ステップと、
   代表点に作用する斥力から移動体の各関節軸に働く力とモーメントを算出する力・モーメント算出ステップと、
   前記動作軌道上に参照点を設定し、参照点に向けた引力を前記移動体に作用させる引力ベクトル設定ステップと、
   移動体の３次元形状モデルに作用する力とモーメントから移動体の各関節軸に作用するトルクを算出し、トルクに基づいて各関節軸の角度を変化させる姿勢生成ステップと、を備えることを特徴とする移動体姿勢生成方法。
2. 移動体の一部分の動作軌道に基づいて移動体全体の姿勢を生成する移動体姿勢生成方法であって、
   移動体が移動する空間を設定する空間設定ステップと、
   前記空間中に前記移動体の一部分が動作を開始する初期位置を設定する初期位置設定ステップと、
   前記空間中に前記移動体の一部分の目標位置を設定する目標位置設定ステップと、
   前記空間に存在する障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定ステップと、
   前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に前記斥力ベクトルが作用する作用点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定ステップと、
   前記斥力ベクトル場の前記斥力ベクトルを前記作用点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成ステップと、
   移動体の３次元形状モデルに斥力および引力が作用する代表点を設定する移動体形状モデル設定ステップと、
   代表点に作用する斥力から移動体の各関節軸に働く力とモーメントを算出する力・モーメント算出ステップと、
   生成した動作軌道上に参照点を設定し、参照点に向けた引力を移動体に作用させる引力ベクトル設定ステップと、
   移動体の３次元形状モデルに作用する力とモーメントから移動体の各関節軸に作用するトルクを算出し、トルクに基づいて各関節軸の角度を変化させる姿勢生成ステップと、を備えることを特徴とする移動体姿勢生成方法。
3. 前記姿勢生成ステップの後に、前記参照点を更新する参照点更新ステップと、前記参照点が前記動作軌道上の目標位置に到達するまで前記力・モーメント算出ステップ、前記引力ベクトル設定ステップ、前記姿勢生成ステップ、前記参照点更新ステップを繰り返す動作生成ステップを備えることを特徴とする請求項１または請求項２記載の移動体姿勢生成方法。
4. 前記引力ベクトル設定ステップで、引力を作用させる移動体の当該箇所の現在位置と生成した動作軌道上の参照点との位置偏差に応じた引力を設定することを特徴とする請求項１または請求項２記載の移動体姿勢生成方法。
5. 前記引力ベクトル設定ステップで、動作軌道上の参照点を含み、かつ動作軌道に鉛直な平面を定義し、引力を作用させる移動体の当該箇所の現在位置と前記鉛直平面との位置偏差に応じた引力を設定することを特徴とする請求項１または請求項２記載の移動体姿勢生成方法。
6. 前記引力ベクトル設定ステップで、位置偏差量に比例する成分、位置偏差を積分した成分、位置偏差の時間変動を微分した成分の組合せからなる引力を設定することを特徴とする請求項４または請求項５記載の移動体姿勢生成方法。
7. 前記姿勢生成ステップで、トルクに基づく各関節軸角度の変化量を算出する過程において関節軸ごとに関節の動き易さを表す係数を設定することを特徴とする請求項１または請求項２記載の移動体姿勢生成方法。
8. 前記姿勢生成ステップで、移動体が特異姿勢に近づいた場合に、関節の動き易さを表す係数を変化させることを特徴とする請求項７記載の移動体姿勢生成方法。
9. 前記姿勢生成ステップで、移動体の現在の姿勢から求まるヤコビ行列を用いることで、３次元形状モデルに作用する力とモーメントから各関節軸に作用するトルクを算出することを特徴とする請求項１または請求項２記載の移動体姿勢生成方法。
10. 移動体の一部分の動作軌道に基づいて移動体全体の姿勢を生成する移動体姿勢生成装置であって、
    移動体が移動する空間に存在する障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定手段と、
    移動体の３次元形状モデルに斥力および引力が作用する代表点を設定する移動体形状モデル設定手段と、
    代表点に作用する斥力から移動体の各関節軸に働く力とモーメントを算出する力・モーメント算出手段と、
    前記動作軌道上に参照点を設定し、参照点に向けた引力を前記移動体に作用させる引力ベクトル設定手段と、
    移動体の３次元形状モデルに作用する力とモーメントから移動体の各関節軸に作用するトルクを算出し、トルクに基づいて各関節軸の角度を変化させる姿勢生成手段と、を備えることを特徴とする移動体姿勢生成装置。
11. 移動体の一部分の動作軌道に基づいて移動体全体の姿勢を生成する移動体姿勢生成装置であって、
    移動体が移動する空間を設定する空間設定手段と、
    前記空間中に前記移動体の一部分が動作を開始する初期位置を設定する初期位置設定手段と、
    前記空間中に前記移動体の一部分の目標位置を設定する目標位置設定手段と、
    前記空間に存在する障害物を発生源とする斥力ベクトルからなる斥力ベクトル場を設定する斥力ベクトル場設定手段と、
    前記初期位置と前記目標位置とを結び、前記初期位置と前記目標位置の間に前記斥力ベクトルが作用する作用点を有し、前記初期位置と前記目標位置に両端を固定された動作軌道モデルを設定する動作軌道モデル設定手段と前記斥力ベクトル場の前記斥力ベクトルを前記作用点に作用させ、変形した前記動作軌道モデルを前記移動体の動作軌道とする動作軌道生成手段と、
    移動体の３次元形状モデルに斥力および引力が作用する点を設定する移動体形状モデル設定手段と、移動体に作用する斥力から移動体の各関節軸に働く力とモーメントを算出する力・モーメント算出手段と、生成した動作軌道上に参照点を設定し、参照点に向けた引力を移動体に作用させる引力ベクトル設定手段と、
    移動体の３次元形状モデルに作用する力とモーメントから移動体の各関節軸に作用するトルクを算出し、トルクに基づいて各関節軸の角度を変化させる姿勢生成手段と、を備えることを特徴とする移動体姿勢生成装置。

Images