JP4613539B2 - ロボットの動作計画方法 - Google Patents

Description

本発明は、与えられた初期姿勢及び最終姿勢に基づいて、初期姿勢から最終姿勢までのロボットの動作を計画するロボットの動作計画方法に関する。

二足歩行型のロボットの歩行制御において、ＺＭＰ規範が良く知られている（［特許文献１］，［非特許文献１］）。ＺＭＰ（Ｚｅｒｏ Ｍｏｒｍｅｎｔ Ｐｏｉｎｔ）とは、ロボットが位置する床面上において、このロボットの各部材の重力によるモーメントと慣性力によるモーメントと（もし外力が作用していれば、さらに外力によるモーメントと）の総和がゼロとなる点のことである。ＺＭＰ規範とは、このＺＭＰがロボットの接地面が形成する支持多角形（安定領域と呼ばれる）の辺上又は内部にあればロボットは転倒せずに安定に移動することが可能であるとする規範である（［非特許文献２］）。

なお、ＺＭＰ規範は、二足歩行ロボットに対してのみ適用されるものではなく、車輪などによって移動するロボットなどに対しても有効である。また、ロボット自体やロボットの腕などを操作する際には、その経路を設定する必要が生じる。下記［非特許文献３］には、確率機構を用いた経路計画について記載されている。［非特許文献３］の方法では、スタート位置（初期姿勢）とゴール位置（最終姿勢）とが設定され、この間に一つ以上の中間点を経由する経路が設定される。

特開２００３−２３６７８０号公報 ヴィークル搭載型マニュピレータの安定化制御−安定規範とマニュピレータによる補償運動− 黄強，菅野重樹，加藤一郎 計測自動制御学会論文集Ｖｏｌ．３１，Ｎｏ．７，８６１〜８７０ページ １９９５年７月 上体の運動によりモーメントを補償する２足歩行ロボット 高西淳夫 日本ロボット学会誌Ｖｏｌ．１１，Ｎｏ．３，３４８〜３５３ページ １９９３年４月 Ｒａｎｄｏｍｉｚｅｄ Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎｎｉｎｇ ： Ｓ．Ｍ．ＬａＶａｌｌｅ ａｎｄ Ｊ．Ｊ．Ｋｕｆｆｎｅｒ，Ｊｒ． ： Ｐｒｏｃ ｏｆ ＩＥＥＥ Ｉｎｔ． Ｃｏｎｆ． ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ， １９９９

上述した文献などでの経路生成では、何れもロボットの安定した動作を補償することが十分に検討されていない。このため、ロボットが決定された経路でその一部（ロボットアーム）や全体を移動させたときに転倒してしまうなどのおそれがあった。ロボットがその一部や全体を移動させる際には、重力に対するバランスだけでなく、慣性力によるバランスなども考慮する必要があり、これらを十分に考慮しないと転倒してしまう。従って、本発明の目的は、ロボットの初期姿勢から最終姿勢までの安定した動作を計画することのできるロボットの動作計画方法を提供することにある。

請求項１に記載のロボットの動作計画方法は、ロボットの初期姿勢及び最終姿勢に基づいて、初期姿勢と最終姿勢とをつなぐ動作を確率を用いて計画するもので、重力によってロボット各部に作用する複数のモーメントの釣り合い条件に基づいて、動作中のある時点での停止状態でロボットが転倒するか否かを静的に判定する静的転倒判定工程と、静的転倒判定工程において、ロボットが動作の間に静的に転倒しないと判定された場合に、重力及び動作による慣性力によって該ロボットに作用する複数のモーメントのつり合い条件を考慮して動作中にロボットが転倒しないと動的に判定されるように、該ロボット各部の速度及び／又は加速度を設定する速度／加速度変更工程とを備え、静的転倒判定工程は、初期姿勢と最終姿勢との間に設定された複数の中間姿勢の動作を滑らかにする平滑化工程によって平滑化された動作計画を用いてロボットが転倒するか否かを静的に判定し、速度／加速度変更工程は、移動駆動部上に複数の関節を連結した胴体部を有するロボットの複数の関節ごとに、初期姿勢と最終姿勢との間に設定された複数の中間姿勢における最大角速度及び／又は最大角加速度を超えないように、複数の中間姿勢間の動作にかかる動作時間を設定し、平滑化工程によって平滑化された後に最大角加速度を超えないように動作時間を修正することを特徴としている。

請求項２に記載の発明は、請求項１に記載のロボットの動作計画方法において、速度／加速度変更工程における動的転倒判定時に、ＺＭＰ規範に基づいてロボットが動的に転倒しないようにロボットの複数の関節ごとの速度及び／又は加速度を設定することを特徴としている。

請求項３に記載の発明は、請求項１又は２に記載のロボットの動作計画方法において、ロボットの複数の関節ごとについて許容し得る上限値となる最大速度及び／又は最大加速度が予め決定されており、速度／加速度変更工程においては、最大速度及び／又は最大加速度を減少させることを特徴としている。

請求項１に記載のロボットの動作計画方法における速度／加速度変更工程において行われる「重力及び動作による慣性力によってロボットに作用する複数のモーメントのつり合い条件を考慮して動作中にロボットが転倒しないと動的に判定（動的転倒判定）」する工程を、以下、動的転倒判定工程と言うこととする。即ち、動的転倒判定工程は、速度／加速度変更工程に含まれている。請求項１に記載のロボットの動作計画方法によれば、静的には転倒しないと判断されるときには、動的にも転倒しないようにロボット各部の速度（加速度）を設定する。このようにすることで、動的にも転倒しない経路を決定でき、ロボットを安定して動作させることができる。

なお、速度／加速度変更工程においては、速度だけで動的に転倒しないようにしても良いし、加速度だけで動的に転倒しないようにしても良いし、速度と加速度の双方を制御して動的に転倒しないようにしても良い。このようにすれば、静的には転倒しないが動的には転倒するような場合を確実に回避することができる。また、動的転倒判定工程においては、ロボットに作用する重力及び慣性力を考慮するが、ロボットに作用する重力・慣性力以外の力（例えば外力）を考慮する事を除外しているわけではないことは言うまでもない。

請求項２に記載のロボットの動作計画方法によれば、ＺＭＰ規範に基づいて動的転倒判定を行うため、転倒判定を迅速かつ的確に判定することができる。この結果、経路決定を迅速かつ確実に行うことができる。

請求項３に記載のロボットの動作計画方法によれば、ロボット各部（例えばロボットアームの関節部など）毎に予め設定される最大速度や最大加速度を引き下げることで、慣性力を低減し、ロボットの動作を安定化させる。このようにすることで、経路はそのままにしつつもロボットの動作を安定化させることが可能となり、動作計画を迅速かつ確実に行うことができる。

本発明の動作計画方法の一実施形態について以下に説明する。動作計画によってロボットの移動経路が計画される。まず、本実施形態の動作計画方法を実行（演算）する装置（安定動作計画手段）の構成を図１に示す。安定動作計画手段１は、具体的にはコンピュータによって構成されている。図１に示されるように、安定動作計画手段１は外部環境情報を入力する手段である外部環境情報入力手段２と、生成過程にある経路や最終的に生成した（設定した）経路を出力する経路出力手段３とを有している。

外部環境情報入力手段２は、移動経路を生成する自動機械（ロボット自体やロボットアームなど）周辺の情報、特に障害物情報を外部環境として取得する手段であり、カメラや各種センサなどの情報取得デバイスが例として挙げられる。あるいは、外部環境情報入力手段２は、キーボードや光ディスクドライブなどであっても良く、オペレータが手動で外部環境情報を入力しても良いし、すでにデータ化されて光ディスクに格納されている外部環境情報を入力しても良い。経路出力手段３は、具体的には、モニタやプリンタ、記憶可能な媒体を扱うドライブなどである。

外部環境情報入力手段２から入力された外部環境情報は、環境情報記憶手段４に蓄積される。環境情報記憶手段４は、具体的にはハードディスクやＲＡＭなどである。環境情報記憶手段４には、知識としての環境情報（すでに知識として保持している情報）と、上述したような、外部環境情報入力手段２によって新たに入力された環境情報とを保持する。外部環境情報入力手段２によって新たに入力された環境情報は、その後は知識としての環境情報として蓄積される。安定動作計画手段１は、ロボットの初期姿勢と最終姿勢とを入力する初期・最終姿勢入力手段５や、初期姿勢と最終姿勢との間に中間姿勢を設定する中間姿勢生成手段６や、速度設定手段７、加速度設定手段８、衝突判定手段９、転倒判定手段１０なども有している。

速度設定手段７は、ある中間姿勢から次の中間姿勢までの動作にかかる時間を設定することで、ロボット各部の速度を設定する。即ち、ここでは、ある中間姿勢から次の中間姿勢までの単なる位置の変化だけでなく、動作にかかる時間をも考慮している。同様に、加速度設定手段８は、上述した速度の時間変化を考慮することで、ロボット各部の加速度を設定する。なお、速度や加速度は、関節部であれば角速度や角加速度が用いられる場合もありえる。

衝突判定手段９は、環境情報記憶手段４に記憶された障害物に関する情報に基づいて、生成中の動作経路が障害物と干渉するかどうかを判定するものである。動作経路が障害物と干渉するようであれば、中間姿勢生成手段６によって障害物と干渉する姿勢が修正される。転倒判定手段１０は、障害物と干渉しないものとして生成された動作経路に沿ってロボットを動作させたときに、ロボットが転倒するか否かを静的及び動的に判定する。静的に転倒すると判定されるようであれば、中間姿勢生成手段６によって中間姿勢が修正される（動作経路が修正される）。

静的には転倒しない場合は、動的に転倒しないように動作（経路だけでなく速度や加速度を含む）が設定される。このとき、まず、経路のみ（速度や加速度を含まない）を決定し、この経路が静的に転倒しないと判定された場合に、動的に転倒しないように速度や加速度を設定する事が考えられる。なお、静的に転倒すると判断される場合は、中間姿勢生成手段６によって経路（中間姿勢）自体が修正あるいは再生成される。

あるいは、動作（経路だけでなく速度や加速度を含む）を生成し、この動作が静的に転倒しないと判定できる場合に、動的転倒判定を行うようにしても良い。この場合に、動的判定で転倒すると判定される場合には、中間姿勢生成手段６によって経路（中間姿勢）を変更しても良いし、速度や加速度だけを修正しても良い。この場合も、静的に転倒すると判断される場合は、経路（中間姿勢）自体が修正あるいは再生成される。

速度や加速度を修正する場合は、速度設定手段７や加速度設定手段８によって速度や加速度を低減させて（動作経路自体は変わらない）転倒しないようにする。速度や加速度を制御しても転倒を防止できないような場合は、中間姿勢生成手段６によって中間姿勢が修正される（動作経路が修正される）。これらの中間姿勢生成手段６〜転倒判定手段１０などは、ハードディスク内やＲＯＭ内に格納されたプログラムとこれを実行するＣＰＵなどによって実現されている。

次に、動作経路計画の手順について順を追って説明する。図２は、台車１０１上に人型の上体を取り付けたロボット１００を模式的に示した図である。台車１０１には、四つの車輪１０２が取り付けられている。ロボット１００は、これら車輪１０２を内部のモータで駆動して移動することができる。上体は、胴体１０３と左右の腕１０４Ｌ，１０４Ｒと頭部１０５とからなっている。頭部１０５には、上述した外部環境取得手段としてのカメラが人間の目を模して内蔵されている。

胴体１０３には、１Ｂ〜４Ｂの関節ジョイントが配設されている。関節ジョイント１Ｂ，２Ｂ，４Ｂは、紙面に平行でかつ図中左右に伸びる回転軸を持つ関節である。関節ジョイント３Ｂは、紙面に平行でかつ図中上下に伸びる回転軸を持つ関節である。関節ジョイント１Ｂ，２Ｂ，４Ｂによって前のめりや後のめりの体勢を作り、関節ジョイント３Ｂによって腰をまわすのと同様に上体をまわした体勢を作る。

左右の腕１０４Ｌ，１０４Ｒは、左右対称なので右腕１０４Ｒを例に説明する。また、便宜上ここに言う腕には肩も含むものとする。右腕１０４Ｒには、５Ｒ〜１１Ｒの関節ジョイントが配設されている。また、図示されていないが、手先部には、対象物を把持するために、指関節に相当する小さな関節ジョイントが配設されている。関節ジョイント５Ｒは、紙面に平行でかつ図中左右に伸びる回転軸を持つ関節であり、腕を前方に上げる際に用いられる。関節ジョイント６Ｒは、紙面に垂直な回転軸を持つ関節であり、腕を側方に上げる際に用いられる。関節ジョイント７Ｒ，９Ｒは、紙面に平行でかつ図中上下に伸びる回転軸を持つ関節であり、腕をねじる際に用いられる。関節ジョイント８Ｒ，１１Ｒは、紙面に平行でかつ図中左右に伸びる回転軸を持つ関節であり、手先を肩につけるように曲げる際に用いられる。関節ジョイント１０Ｒは、紙面に垂直な回転軸を持つ関節であり、手先部を動かす際に用いられる。

即ち、このロボット１００の上体は、胴体が４自由度（４ＤＯＦ：Ｄｅｇｒｅｅ Ｏｆ Ｆｒｅｅｄｏｍ）、腕部が７自由度（７ＤＯＦ）、手先部が１３自由度（１３ＤＯＦ）の自由度を有している。また、台車１０１を底面側から見た図を図３に示す。図３に示されるように、台車１０１は４つの車輪１０２を備えており、車輪１０２の回転方向や回転速度を制御することで、移動やその場での方向転換が可能である。また、図３中には、車輪１０２と床面との接点を結んだ多角形（支持多角形）によって囲まれたハッチングを施した領域（安定領域α：辺上を含む）が示されている。この安定領域については、後述する転倒判定時にさらに説明する。

次に、上述したロボット１００の動作計画について説明する。動作計画では、ロボット１００の全体あるいは一部（腕１０４Ｒ，１０４Ｌなど）の移動経路とそのときの移動速度及び移動加速度を決定する。ロボット１００が存在する実際の作業空間に対してｘ，ｙ，ｚ座標軸を設定し、同時に、ロボット１００の姿勢を変えるパラメータとなり得る情報量（ここでは、関節１Ｂ〜４Ｂ，５Ｒ〜１１Ｒ，５Ｌ〜１１Ｌの各角度θ_１Ｂ〜θ_４Ｂ，θ_５Ｒ〜θ_１１Ｒ，θ_５Ｌ〜θ_１１Ｌ）を各座標軸の座標として設定可能な多次元座標軸（ここでは１８次元となる）を設定する。即ち、ロボット１００（ここではロボットアームのみで説明しているが）のある姿勢は、多次元座標軸上の一点で示すことができる。このようにすると、実際の作業空間での手首位置Ｘの座標（ｘ，ｙ，ｚ）は、多次元座標軸上に示されるベクトル（θ_１Ｂ，…，θ_４Ｂ，θ_５Ｒ，…，θ_１１Ｒ，θ_５Ｌ，…，θ_１１Ｌ）の値［ベクトル先端の座標位置］を座標変換することで容易に得ることができる。なお、ここでは、説明の便宜上、人間の指に相当する手先部の動きと、台車１０１の移動は考えないことする。

ロボット１００の動作計画に際しては、まず、外部環境情報入力手段２によって外部環境情報が入力される。すでに入力され、環境情報記憶手段４に蓄積された情報のみで不足がない場合は、新たな外部環境情報の入力は必ずしも必要ない。次に、ロボット１００の初期姿勢と最終姿勢とをオペレータがセットする。これらの姿勢があらかじめプリセットされているのであれば、オペレータによる入力は必要ない。入力は、キーボードから座標位置を入力するなどして設定することができる。あるいは、安定動作計画手段１にロボット１００を接続し、ロボット１００を実際に初期姿勢及び最終姿勢をとらせて安定動作計画手段１に読み込ませるようにしても良い。また、装置に組み込まれたカメラや各種センサによって外部環境情報が自動又は半自動で取得されるようにしても良い。初期姿勢及び最終姿勢は、最終的には上述した多次元座標空間上の座標に変換される。

初期・最終姿勢入力手段５は、環境情報記憶手段４に蓄積された情報と照らし合わせて、入力された初期姿勢及び最終姿勢が障害物と衝突していないか（干渉していないか）を判断し、衝突していない場合はこれらを初期姿勢及び最終姿勢として決定する。衝突判定時には、多次元座標軸上の一点の座標として表された姿勢を実際の作業空間に反映させ、そこで外部環境情報として蓄積されている障害物に関する情報と照らし合わせて衝突しているか否かが判定される。このような衝突判定は既存の手法を用いることができる。

なお、もし、入力された初期姿勢及び／又は最終姿勢が障害物と衝突している場合はオペレータに対して再入力を促し、再入力された初期姿勢及び／又は最終姿勢について同様の判定を行い、初期姿勢及び最終姿勢が決定されるまで反復される。次に、公知の手法によって、初期姿勢と最終姿勢との間に中間姿勢が仮設定される（中間姿勢設定工程）。この中間姿勢の仮設定は、中間姿勢生成手段６によって行われる。この中間姿勢の設定方法としては、例えば、上述した多次元座標軸上に中間姿勢に対応する座標をランダムに発生させればよい。

初期姿勢と最終姿勢との間には、順番を含めて複数の中間姿勢が設定される。このとき、中間姿勢が仮設定された状態では、姿勢（空間的位置）のみが仮設定されただけであり、動作速度のような時間的な要素は設定されていない。そこで、次に、時間的要素を決定するため、各関節毎に設定されている最大速度（最大角速度）を超えないように、各中間姿勢の間に必要な時間が決定される（時間設定工程）。この時間設定は、速度設定手段７によって行われる。具体的には、各関節毎に時間−関節角のグラフが生成され、このグラフ上に各中間姿勢に相当する関節角が座標としてプロットされたときに、各座標を順に結ぶ線分の傾きとして表される角速度が最大角速度を超えないように設定される。

可能である場合は最大角速度で設定されるが、このような時間設定はすべての関節において同時に行われるため、最も条件の厳しい関節に合わせてすべての関節についての中間姿勢間の時間、即ち、関節角速度が設定される。なお、速度（角速度）だけでなく、加速度（角加速度）についても同様のことが行われ、各関節における角加速度が予め設定された最大角加速度を超えないように設定される。なお、時間−角加速度のグラフは、時間−角速度のグラフを一階微分すれば容易に得られる。この角加速度に関する時間設定は、加速度設定手段８によって行われる。

上述した時間に関する要素の決定について、ある関節ジョイント部分を例にして説明する。ある関節ジョイントの角度について、初期姿勢での角度をθ_ｓ、最終姿勢の角度をθ_ｇとする。そして、上述した中間姿勢生成時に、この関節ジョイント部分に関して仮設定された中間姿勢での関節ジョイントの角度をθ_１，θ_２，…，θ_ｎと表すものとする。例えば、ここで、時間設定が行われる以前には、θ_ｓとθ_１，θ_ｉとθ_ｉ＋１，θ_ｎとθ_ｇの間の移動にかかる時間を一定時間ｔであると仮定して、関節ジョイントの角度の時間変化をグラフにして示すと図４のようになる。

図４から分かるように、図４の角度変化を示す折れ線の傾きが角速度を表すことになる。ここでは、角速度は時間に対して一定の割合で増減するため、角加速度はゼロである。しかし、ロボット１００を実際に駆動する場合には、関節ジョイント部分のアクチュエータ（モータなど）の機械的性能などから、角速度や角加速度には上述した最大角速度や最大角加速度といった制限を設ける必要が生じる。これらの条件は予め安定動作計画手段１にインプットされており、これらの制約を考慮して時間設定が行われる。即ち、図４のように、単純に中間姿勢間の移動にかかる時間を決めてしまうと、アクチュエータなどの機械的性能などのために実現し得ない経路設計がなされてしまう場合や、アクチュエータの駆動に不都合がある場合などがある。

そこで、各関節ジョイントに関する上述した制約を考慮した最大角速度ω_ｍａｘが予め設定される。そして、θ_ｉからθ_ｉ＋１までの時間間隔を（ｔ_ｉ＋１−ｔ_ｉ）として、｜（θ_ｉ＋１−θ_ｉ）／ω_ｍａｘ｜で定義される時間を設定すれば、角速度が許容範囲内となる時間設定を行える。二つの関節ジョイント部分における角度・角速度の時間変化の例を図５に示す。なお、各中間姿勢の間の時間（例えば、図５中のΔｔ_１の長さ）は全ての関節ジョイント部で一致させる必要がある。図５では、関節Ａと関節Ｂとにおける各中間姿勢での制御タイミングがすでに一致されている状態が示されている。

そこで、次に、このΔｔ_１の長さの求め方、即ち、複数の関節ジョイント部分での中間姿勢の制御タイミングを合わせる方法について説明する。関節Ａに関しては、Δｔ_１の前後で角度がθ_１Ａからθ_２Ａに変化したとし、関節Ｂに関しては、Δｔ_１の前後で角度がθ_１Ｂからθ_２Ｂに変化したとする。このとき、上述したように、関節Ａに関しては、最大角速度ω_Ａｍａｘが設定されていたとすると、関節Ａでの角速度を許容範囲内とするには、Δｔ_１は｜θ_２Ａ−θ_１Ａ｜／ω_Ａｍａｘ以上とする必要がある。同様に、関節Ｂに関しては、最大角速度ω_Ｂｍａｘが設定されていたとすると、関節Ｂでの角速度を許容範囲内とするには、Δｔ_１は｜θ_２Ｂ−θ_１Ｂ｜／ω_Ｂｍａｘ以上とする必要がある。そこで、Δｔ_１＝ｍａｘ｛｜θ_２Ａ−θ_１Ａ｜／ω_Ａｍａｘ，｜θ_２Ｂ−θ_１Ｂ｜／ω_Ｂｍａｘ｝とし、各関節で必要な時間の中で最長の時間を選択してΔｔ_１として決定する。ここでは、二つの関節を例にして説明しているが、関節が複数でも同様に行えば良い。また、Δｔ_１以降の各時間間隔についても同様にタイミングを一致させる。これで、時間設定が終了する（後述するが、さらに修正される場合もある）。

この後、設定された中間姿勢のうち、省略できる姿勢を省略して動作（経路）を単純化する平坦化が行われ（平坦化工程：これについては詳述しない）、その後、動作（経路）を滑らかにする平滑化が行われる（平滑化工程）。平滑化について簡単に説明する。平坦化が終了した時点での経路は、各中間姿勢において急激に動作方向が変わるものとなっている。このような急激に方向の変わる動作を、滑らかな軌跡を描くように修正してロボット１００の動きを滑らかにするのが平滑化である。ここでは、平滑化後の経路を無限回微分可能な関数によって近似する。このため、経路自体の滑らかさに加えて、その経路上の移動速度及び加速度の滑らかさも保証される。さらに、平滑化後の経路についても障害物と衝突するか検証し、衝突する場合は回避されるまで平滑化が再履行される。

平滑化後のある関節についての平滑化後の時間−角度曲線の一例を図６（ａ）に示す。また、図６（ａ）に対応する時間−角速度曲線を図６（ｂ）に、時間−角加速度曲線を図６（ｃ）に示す。平滑化は、直線で構成されている平滑化前の時間−角度曲線における節（ノード）部分を滑らかにする。このため、平滑化前に角速度が最大加速度以下となっていれば、平滑化後も角速度は最大角速度以下となる。しかし、角加速度に関しては、平滑化後に、許容し得る最大角加速度を超えないことは保証されていない。そこで、角速度の最大許容角速度ω_ｍａｘと同様に、角加速度についても最大角加速度ａ_ｍａｘが予め設定され、角加速度がこの最大角加速度ａ_ｍａｘを超えないように平滑化後の経路が修正される（時間修正工程）。この経路修正工程では、時間スケールが局所的に修正され、これによって角加速度が許容範囲内に納められる。

なお、以下、ある関節を例にして説明するが、最終的には全ての関節について同様のことが行われる。図６中、点線が平坦化後・平滑化前の経路についての角度・角速度・角加速度を示しており、実線が平滑化後の経路についての角度・角速度・角加速度を示している（角加速度に関しては、平滑化前の経路ではつねに０であるので図示を省略する）。図６からも分かるように、上述したノードを提示する時刻で角加速度は極大値・極小値をとる。すべてのノードでの角加速度のなかから、上述した最大角加速度ａ_ｍａｘを超えるものを抽出する。そして、最大角加速度ａ_ｍａｘを超えているノードに関しては、その部分時間軸を局所的に引き伸ばして角加速度を許容範囲内に修正する。

なお、ある関節について、角加速度条件から時間軸の引き延ばしが行われた場合は、同時刻における他の関節における角加速度が許容範囲内であっても、全ての関節についての時間軸も引き延ばされ、全体としての制御タイミングが一致するようになされる。なお、障害物との衝突判定を、動作がある程度確定する加速度設定後に行うようにしても良い。このようにして、一旦、動作が決定されたら、この動作に関して転倒判定を行う。まず、静的転倒判定を行う（静的転倒判定工程）。静的転倒判定は、重力の影響のみを考慮してロボット１００が転倒するかどうかを判定する。これは、各部の重量とロボット１００の重心位置などをもとに、公知の方法で算出することで判定できる。例えば、上体を前傾させ過ぎれば、ロボット１００は前方に倒れる。このようなことが起きないかを、ここまでで決定している動作の全行程で判定する。

静的に転倒すると判断される場合は、中間姿勢の生成からやり直しとなる。なお、全経路についてやり直してもよし、転倒すると判断された姿勢の前後のみをやり直してもよい。一方、静的に転倒しないと判断された場合は、今度は動的に転倒しないかどうかの判定が行われる（動的転倒判定工程）。この判定は、上述したＺＭＰ規範に則って行われる。ＺＭＰを計算によって求めるには、まず、各関節の角度・角速度・角加速度を既知値として、各関節間のリンクの角速度・角加速度、及び、ロボット１００の重心の並進加速度を計算する。

次に、各リンクの慣性力と慣性モーメントとを計算する。さらに、計算した慣性力・慣性モーメント及び重心位置を慣性座標系に変換し、最後に、ＺＭＰが算出される。ＺＭＰの算出方法は公知であるため、ここでのさらなる詳しい説明は省略する。そして、このＺＭＰが図３中の安定領域α内（辺上を含む）にあれば、転倒しないで安定して動作を行えると判断できる。なお、上述した計算には、ロボットの各リンクに関するデータ、即ち、図２中のリンク長Ｌ_１〜Ｌ_１２などの形状データや各リンクの重量などが必要となるが、これらは予め環境情報記憶手段に記憶されている。また、算出に必要な角度・角速度・角加速度は上述したものが用いられることとなる。

動的な転倒判定で転倒しないと判断されれば動作計画は終了する。しかし、動的転倒判定によって転倒する場合は、転倒しないように速度（角速度）及び／又は加速度（角加速度）を調整して転倒しないように動作を修正する（速度／加速度変更工程）。修正方法は、いくつか考えられるが、例えば、（１）上述した最大角加速度ａ_ｍａｘは固定したまま、最大角速度ω_ｍａｘを一定の割合で徐減させ、更新された最大角速度ω_ｍａｘを超える角速度部分を修正し、転倒判定（静的及び動的）を行うことを繰り返す、ことが考えられる。ただし、角速度には最小角速度ω_ｍｉｎも設定されており、最大角速度ω_ｍａｘを徐減させていったところ、最小角速度ω_ｍｉｎに達してもまだ動的転倒判定で転倒すると判定された場合は、動作計画を最初からやり直すものとする。

あるいは、（２）上述した最大角速度ω_ｍａｘは固定したまま、最大角加速度ａ_ｍａｘを一定の割合で徐減させ、更新された最大角加速度ａ_ｍａｘを超える角加速度部分を修正し、転倒判定（静的及び動的）を行うことを繰り返す、ことが考えられる。ただし、角加速度には最小角加速度ａ_ｍｉｎも設定されており、最大角加速度ａ_ｍａｘを徐減させていったところ、最小角加速度ａ_ｍｉｎに達してもまだ動的転倒判定で転倒すると判定された場合は、動作計画を最初からやり直すものとする。

あるいは、（３）上述した最大角速度ω_ｍａｘと最大角加速度ａ_ｍａｘとを同時に一定の割合で徐減させ、更新された最大角速度ω_ｍａｘ及び最大角加速度ａ_ｍａｘを超える部分を修正し、転倒判定（静的及び動的）を行うことを繰り返す、ことが考えられる。ただし、この場合も、角速度には最小角速度ω_ｍｉｎが、角加速度には最小角加速度ａ_ｍｉｎが設定されており、最大角速度ω_ｍａｘ及び最大角加速度ａ_ｍａｘを徐減させていったところ、何れかが最小角速度ω_ｍｉｎ又は最小角加速度ａ_ｍｉｎに達してもまだ動的転倒判定で転倒すると判定された場合は、動作計画を最初からやり直すものとする。

なお、本発明は上述した実施形態に限定されるものではない。例えば、上述した実施形態では、屈曲部の関節を例に説明したので、角度・角速度・角加速度で動作計画が説明された。しかし、ロボット１００の姿勢を決めるものとしては、例えば、伸張可能な部分の伸張量なども挙げられる。このような場合は、伸張量・伸張速度・伸張加速度を用いて動作計画が行われる。また、上述した実施形態では、台車１０１を移動させない場合を例に説明したが、移動させた場合にも本発明を適用することが可能である。また、上述した実施形態は台車移動ロボット１００を例にして説明したが、脚式移動ロボットなどに対しても本発明を適用することが可能である。また、一つの大きな動作をいくつかに分割し、それぞれについて動作を計画し、両者を一連の動作として扱うことも可能である。即ち、分割したはじめの計画の最終姿勢が、分割した次の計画の初期姿勢と一致することとなる。

本発明のロボットの動作計画方法の一実施形態を実施する装置の構成図である。 本発明のロボットの動作計画方法の一実施形態によって制御される台車式ロボットを模式的に示す図である。 図２のロボットを底面から見た図である。 各中間姿勢間の時間を一定とした場合の時間−関節角度のグラフである。 時間設定後の時間−関節角度のグラフであり、（ａ）は関節Ａについての、（ｂ）は他の関節Ｂについてのグラフである。 （ａ）時間−角度、（ｂ）時間−角速度、（ｃ）時間−角加速度の関係を示すグラフである。

符号の説明

１…安定動作計画手段、２…外部環境情報入力手段、３…経路出力手段、４…環境情報記憶手段、５…初期・最終姿勢入力手段、６…中間姿勢生成手段、７…速度設定手段、８…加速度設定手段、９…衝突判定手段、１０…転倒判定手段、１００…ロボット、１０１…台車、１０２…車輪、１０３…胴体、１０４Ｒ……右腕、１０４Ｌ……左腕、１０５…頭部。

Claims (3)

1. 移動駆動部上に複数の関節を連結した胴体部を有するロボットの初期姿勢及び最終姿勢に基づいて、初期姿勢と最終姿勢とをつなぐ動作を確率を用いて計画するロボットの動作計画方法において、
   重力によって前記ロボット各部に作用する複数のモーメントのつり合い条件に基づいて、前記動作中のある時点での停止状態で前記ロボットが転倒するか否かを静的に判定する静的転倒判定工程と、
   前記静的転倒判定工程において、前記ロボットが前記動作の間に静的に転倒しないと判定された場合に、重力及び動作による慣性力によって該ロボットに作用する複数のモーメントのつり合い条件を考慮して前記動作中に前記ロボットが転倒しないと動的に判定されるように、該ロボットの一部の速度及び／又は加速度を設定する速度／加速度変更工程とを備え、
   前記静的転倒判定工程は、初期姿勢と最終姿勢との間に設定された複数の中間姿勢の動作を滑らかにする平滑化工程によって平滑化された動作計画を用いて前記ロボットが転倒するか否かを静的に判定し、
   前記速度／加速度変更工程は、
   移動駆動部上に複数の関節を連結した胴体部を有する前記ロボットの前記複数の関節ごとに、初期姿勢と最終姿勢との間に設定された複数の中間姿勢における最大角速度及び／又は最大角加速度を超えないように、前記複数の中間姿勢間の動作にかかる動作時間を設定し、
   前記平滑化工程によって平滑化された後に前記最大角加速度を超えないように前記動作時間を修正すること、
   を特徴とするロボットの動作計画方法。
2. 前記速度／加速度変更工程における動的転倒判定時に、ＺＭＰ規範に基づいて前記ロボットが動的に転倒しないように前記ロボットの複数の関節ごとの速度及び／又は加速度を設定することを特徴とする請求項１に記載のロボットの動作計画方法。
3. 前記ロボットの複数の関節ごとについて許容し得る上限値となる最大速度及び／又は最大加速度が予め決定されており、前記速度／加速度変更工程においては、前記最大速度及び／又は前記最大加速度を減少させることを特徴とする請求項１又は２に記載のロボットの動作計画方法。
   
   

Images