JP5480799B2 - 移動装置およびロボットならびにこれらの制御システム - Google Patents

移動装置およびロボットならびにこれらの制御システム Download PDF

Info

Publication number
JP5480799B2
JP5480799B2 JP2010278470A JP2010278470A JP5480799B2 JP 5480799 B2 JP5480799 B2 JP 5480799B2 JP 2010278470 A JP2010278470 A JP 2010278470A JP 2010278470 A JP2010278470 A JP 2010278470A JP 5480799 B2 JP5480799 B2 JP 5480799B2
Authority
JP
Japan
Prior art keywords
reference point
trajectory
line segment
point
control system
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.)
Active
Application number
JP2010278470A
Other languages
English (en)
Other versions
JP2012128585A (ja
Inventor
信之 大野
眞 関谷
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.)
Honda Motor Co Ltd
Original Assignee
Honda Motor Co Ltd
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 Honda Motor Co Ltd filed Critical Honda Motor Co Ltd
Priority to JP2010278470A priority Critical patent/JP5480799B2/ja
Priority to US13/313,318 priority patent/US8942847B2/en
Publication of JP2012128585A publication Critical patent/JP2012128585A/ja
Application granted granted Critical
Publication of JP5480799B2 publication Critical patent/JP5480799B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/35—Nc in input of data, input till input file format
    • G05B2219/35148—Geometric modeling for swept volume of moving solids
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40428—Using rapidly exploring random trees algorithm RRT-algorithm
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40475—In presence of moving obstacles, dynamic environment

Description

本発明は、移動機能を有する装置等およびその制御システムに関する。
脚式移動ロボット等の移動装置の周辺に存在する通行人等の物体による当該移動装置の移動の妨げを効率的に回避する観点から、当該移動装置を適当な形態で移動または行動させる技術的手法が提案されている（特許文献１参照）。
特開２０１０−０７９８５２号公報
しかし、従来の技術は、実空間座標系における移動軌道に従って移動装置を制御するにとどまり、時間座標系までをも考慮していない技術が多い。このため、移動装置および物体のそれぞれの実空間における時系列的な変位態様が、時空間座標系における軌道により表現される場合、移動装置の軌道が、例えば、移動装置の状況に鑑みてそのアクチュエータの動作性能を超えた軌道になる等の当該移動装置に無理な挙動を強いる等、不適当なものになる可能性があった。
そこで、本発明は、アクチュエータの動作性能等の制御対象の状況に鑑みて、時空間座標系における当該制御対象の目標軌道を適当な形態で探索することができるシステム等を提供することを解決課題とする。
前記課題を解決するための本発明の制御システムは、アクチュエータの動作を制御することにより、実空間における制御対象の時系列的な位置を、実空間座標系および時間座標系により定義される時空間座標系において定義される目標軌道にしたがって制御するように構成されているシステムであって、前記時空間座標系において、第１基準点および第２基準点の初期位置関係が、前記アクチュエータの動作性能に応じて定まる第１条件を満たすように当該初期位置関係を設定した上で、前記時空間座標系において配列される点列に基づき、前記第１基準点から第１線分を延長するとともに前記第２基準点から第２線分を延長する延長処理を実行した上で、前記第１線分および第２線分を連結する連結処理を実行することにより得られる線分を軌道候補として探索するように構成されている第１演算処理要素と、前記軌道候補が、前記制御対象とは別個の物体の時系列的な位置軌道を前記時空間座標系において表わす物体軌道と接触するか否かを判定するように構成されている第２演算処理要素とを備え、前記第２演算処理要素により前回軌道候補が前記物体軌道と接触すると判定されたことを要件として、前記第１演算処理要素が、前記第１基準点および前記第２基準点の今回位置関係が、前記時空間座標系における前記第１基準点および前記第２基準点の今回空間間隔が前回空間間隔よりも短くなる、前記第１基準点および前記第２基準点の今回時間間隔が前回時間間隔よりも長くなる、あるいは、前記第１基準点および前記第２基準点の今回空間間隔が前回空間間隔よりも短くなり、かつ、今回時間間隔が前回時間間隔よりも長くなるという第２条件を満たすように当該今回位置関係を設定した上で、今回軌道候補を探索する一方、前記第２演算処理要素により前回軌道候補が前記物体軌道と接触しないと判定されたことを要件として、当該前回軌道候補を前記目標軌道として設定するように構成されていることを特徴とする。前記第１演算処理要素が、前記軌道候補の探索に際して用いられる前記第１基準点に続く前記点列から、先の点に対して時系列的に過去に位置する点を排除するとともに、前記軌道候補の探索に際して用いられる前記第２基準点に続く前記点列から、先の点に対して時系列的に未来に位置する点を排除するように構成されていてもよい。
前記課題を解決するための本発明の前記制御対象としての移動装置は、アクチュエータと、前記制御システムとを備え、前記制御システムによって前記アクチュエータの動作が制御されることにより、実空間において目標軌道にしたがって移動するように構成されていることを特徴とする。
前記課題を解決するための本発明のロボットは、基体と、前記基体から延設された肢体と、アクチュエータと、前記制御システムとを備え、前記制御システムによって前記アクチュエータの動作が制御されることにより、実空間において目標軌道にしたがって前記制御対象としての前記肢体を変位させるように構成されていることを特徴とする。
本発明の制御システム、移動装置またはロボット（以下、適宜「制御システム等」という。）によれば、時空間座標系において物体軌道と接触しないような軌道候補が目標軌道として生成される。この目標軌道にしたがってアクチュエータの動作が制御されることにより、制御対象に物体と接触する事態を回避させながら実空間を変位させることができる。
時空間座標系における軌道候補の第１基準点および第２基準点の相対的な位置関係は、制御対象が第１空間位置（第１基準点の実空間成分）から第２空間位置（第２基準点の実空間成分）まで移動する際の速度の高低を定める。たとえば、時空間座標系における第１基準点（時系列的に先の点）および第２基準点（時系列的に後の点）の時間間隔、すなわち、制御対象が実空間において第１空間位置から第２空間位置まで変位するのに要する時間が長いほどその速度が低くなる。時空間座標系における第１基準点および第２基準点の空間間隔、すなわち、制御対象が第１空間位置から第２空間位置まで移動する実空間距離が短いほど速度が低くなる。
そこで、時空間座標系における第１基準点および第２基準点の初期位置関係が「第１条件」を満足するように設定される。「第１条件」はアクチュエータの動作性能に応じて定められている。
これにより、制御対象を実空間において第１空間位置から第２空間位置まで変位させる際の目標速度が、アクチュエータの動作性能に応じた制御対象の変位速度の許容範囲から外れるような不適当な値になる事態が回避される。
さらに、前回軌道候補が物体軌道に接触すると判定された場合、時空間座標系における第１基準点および第２基準点の今回位置関係が「第２条件」を満足するように設定される。「第２条件」は第１基準点および第２基準点の今回空間間隔が前回空間間隔よりも短くなる、第１基準点および第２基準点の今回時間間隔が前回時間間隔よりも長くなる、あるいは、第１基準点および第２基準点の今回空間間隔が前回空間間隔よりも短くなり、かつ、今回時間間隔が前回時間間隔よりも長くなるという条件である。
これにより、軌道候補の再探索が繰り返されるたび、制御対象を実空間において第１空間位置から第２空間位置まで変位させる際の速度を徐々に低下させるような軌道候補が探索される。このため、制御対象の目標速度が、アクチュエータの動作性能に応じた制御対象の変位速度の許容範囲の上限値を超えるような不適当な値になる事態が回避される。
よって、アクチュエータの動作性能等の制御対象の状況に鑑みて、時空間座標系における当該制御対象の目標軌道が適当な形態で探索されうる。
なお、本発明の構成要素が情報を「認識する」とは、当該構成要素が情報をデータベースから検索すること、メモリ等の記憶装置から情報を読み取ること、センサ等の出力信号に基づき情報を測定、算定、推定、判定すること、測定等された情報をメモリに格納すること等、当該情報をさらなる情報処理のために準備または用意するために必要なあらゆる情報処理を実行することを意味する。
当該構成の制御システム等によれば、時空間座標系において、時系列的に過去にさかのぼるような実現不可能な軌道候補が探索される事態が回避されうる。
前記第１演算処理要素が、前記軌道候補の探索に際して用いられる前記点列から、先の点に対して、前記アクチュエータの動作性能に応じて定まる許容範囲から外れている点を排除するように構成されていてもよい。
当該構成の制御システム等によれば、制御対象が、アクチュエータの動作性能に応じた許容範囲から外れるような速度で変位することを強いられる事態が回避されうる。
前記第１演算処理要素が、前記時空間座標系において１つの第１基準点に対して複数の第２基準点を定義し、あるいは、１つの第２基準点に対して複数の第１基準点を定義した上で、前記延長処理および前記連結処理を実行することにより最も早く得られた、前記１つの第１基準点と前記複数の第２基準点のうち１つの第２基準点とを結ぶ線分、または、前記複数の第１基準点のうち１つの第１基準点と前記１つの第２基準点とを結ぶ線分を、前記軌道候補として探索するように構成されていてもよい。
当該構成の制御システム等によれば、アクチュエータに対して目標軌道の探索結果に応じた動作指令信号が可及的速やかに送信されうる。このため、目標軌道の探索と並行して制御対象の変位態様がリアルタイムで制御される際、この制御対象の動作が一時的に停止または休止状態となる事態が回避されうる。
前記第１演算処理要素が、前記時空間座標系において１つの第１基準点に対して複数の第２基準点を定義し、あるいは、１つの第２基準点に対して複数の第１基準点を定義した上で、前記延長処理および前記連結処理を実行することにより、前記１つの第１基準点と前記複数の第２基準点のそれぞれとを結ぶ線分、または、前記複数の第１基準点のそれぞれと前記１つの第２基準点とを結ぶ線分から、第１基準点および第２基準点の時間間隔に応じて１つの線分を選択し、当該選択した線分を前記軌道候補として探索するように構成されていてもよい。
当該構成の制御システム等によれば、制御対象を目標軌道にしたがって実空間における第１空間位置から第２空間位置まで変位させるための所要時間の長短が調節されうる。たとえば、所要時間が最も短く済むような軌道候補が目標軌道として設定されることにより、可能な限り速やかに制御対象を変位させることができる。
前記第１演算処理要素が、前記制御対象と前記物体との接触確率の高低に応じて、前記時空間座標系における前記第１基準点および前記第２基準点の初期時間間隔および初期空間間隔のうち少なくとも一方の長短を調節することにより前記第１基準点および前記第２基準点の初期位置関係を設定するように構成されていてもよい。
前記第１演算処理要素が、前記第２演算処理要素により前記軌道候補が前記物体軌道と接触すると判定されたことを要件として、前記制御対象と前記物体との接触確率の高低に応じて、前記時空間座標系における前記第１基準点および前記第２基準点の今回時間間隔および今回空間間隔のうち少なくとも一方の長短を、前回時間間隔および前回空間間隔のうち少なくとも一方を調節することにより前記第１基準点および前記第２基準点の今回位置関係を設定するように構成されていてもよい。
当該構成の制御システム等によれば、制御対象と物体との接触確率を低下させうるような軌道候補が早期に探索されるように、時空間座標系における第１基準点および第２基準点の位置関係が適当に設定または補正されうる。
前記第１演算処理要素が、前記時空間座標系において、相互に離れて配置されている点列に基づき、空間補間用の基底関数を結合係数とする、前記点列の線形結合として表現される線分を生成する補間処理を実行するとともに、前記補間処理により生成された一の線分の基礎である前記点列に点を加えて新たな点列を定義した上で、前記新たな点列を基礎として前記補間処理を実行することにより、当該一の線分が延長線分によって延長された新たな線分を生成する前記延長処理を実行するとともに、前記延長処理が実行されることにより最近に延長された前記一の線分の基礎である前記新たな点列と、前記補間処理により生成された他の線分の基礎である他の点列とを基礎として前記補間処理を実行することにより、前記一の線分および前記他の線分が連結線分によって連結された形の線分を前記軌道候補として生成する前記連結処理を実行するように構成されていてもよい。
当該構成の制御システム等によれば、空間補間用の基底関数を結合係数とする、時空間座標系に配置された点列の線形結合により表現される線分を生成する補間処理が実行されることにより、目標軌道またはその候補が生成される。このため、時空間座標系に配置された複数の点のそれぞれを通る折れ線ではなく、当該複数の点（制御点）の線形結合により表わされる、第１基準点および第２基準点を連続的にまたは滑らかに連結する線分が制御対象の目標軌道またはその候補として生成される。これにより、目標軌道にしたがった制御対象の変位形態の連続性または円滑性の維持が図られうる。
前記第２演算処理要素が、前記時空間座標系において、前記延長線分が前記物体軌道から外れていることを含む指定条件を満たしているか否かを判定する第１判定処理を実行するように構成され、前記第１演算処理要素が、前記第１判定処理の結果が否定的であることを要件として、前記一の線分を棄却した上で前記延長処理を実行するように構成されていてもよい。
前記第２演算処理要素が、前記時空間座標系において、前記連結線分が前記物体軌道から外れていることを含む指定条件を満たしているか否かを判定する第２判定処理を実行するように構成され、前記第１演算処理要素が、前記第２判定処理の結果が否定的であることを要件として、前記新たな点列を再定義した上で前記延長処理を実行するように構成されていてもよい。
当該構成の制御システム等によれば、物体との接触を回避しながら、前記のように目標軌道にしたがった制御対象の変位の連続性または円滑性の維持が図られる。
本発明の一実施形態としてのロボットの構成説明図。 本発明の一実施形態としての制御システムの構成説明図。 制御システムにより実行される演算処理に関する説明図。 時空間座標系における第１基準点および第２基準点の位置関係設定に関する説明図。 第１実施形態における軌道候補の探索結果に関する説明図。 第２実施形態における軌道候補探索処理に関する説明図。 第２実施形態における延長処理および連結処理に関する説明図。 第２実施形態における軌道候補の探索結果に関する説明図。 ロボットの挙動に関する説明図。 ロボットの他の挙動に関する説明図。
（移動装置（ロボット）の構成）
図１に示されているロボットＲは脚式移動ロボットであり、人間と同様に、基体Ｂ０と、基体Ｂ０の上方に配置された頭部Ｂ１と、基体Ｂ０の上部に上部両側から延設された左右の腕体Ｂ２と、左右の腕体Ｂ２のそれぞれの先端に設けられているハンドＨと、基体Ｂ０の下部から下方に延設された左右の脚体Ｂ４とを備えている。
基体Ｂ０はヨー軸回りに相対的に回動しうるように上下に連結された上部および下部により構成されている。頭部Ｂ１は基体Ｂ０に対してヨー軸回りに回動する等、動くことができる。
腕体Ｂ２は第１腕体リンクＢ２２と、第２腕体リンクＢ２４とを備えている。基体Ｂ０と第１腕体リンクＢ２２とは肩関節機構（第１腕関節機構）Ｂ２１を介して連結され、第１腕体リンクＢ２２と第２腕体リンクＢ２４とは肘関節機構（第２腕関節機構）Ｂ２３を介して連結され、第２腕体リンクＢ２４とハンドＨとは手首関節機構（第３腕関節機構）Ｂ２５を介して連結されている。肩関節機構Ｂ２１はロール、ピッチおよびヨー軸回りの回動自由度を有し、肘関節機構Ｂ２３はピッチ軸回りの回動自由度を有し、手首関節機構Ｂ２５はロール、ピッチ、ヨー軸回りの回動自由度を有している。
脚体Ｂ４は第１脚体リンクＢ４２と、第２脚体リンクＢ４４と、足部Ｂ５とを備えている。基体Ｂ０と第１脚体リンクＢ４２とは股関節機構（第１脚関節機構）Ｂ４１を介して連結され、第１脚体リンクＢ４２と第２脚体リンクＢ４４とは膝関節機構（第２脚関節機構）Ｂ４３を介して連結され、第２脚体リンクＢ４４と足部Ｂ５とは足関節機構（第３脚関節機構）Ｂ４５を介して連結されている。
股関節機構Ｂ４１はロール、ピッチおよびロール軸回りの回動自由度を有し、膝関節機構Ｂ４３はピッチ軸回りの回動自由度を有し、足関節機構Ｂ４５はロールおよびピッチ軸回りの回動自由度を有している。股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５は「脚関節機構群」を構成する。なお、脚関節機構群に含まれる各関節機構の並進および回転自由度は適宜変更されてもよい。また、股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５のうち任意の１つの関節機構が省略された上で、残りの２つの関節機構の組み合わせにより脚関節機構群が構成されていてもよい。さらに、脚体Ｂ４が膝関節とは別の第２脚関節機構を有する場合、当該第２脚関節機構が含まれるように脚関節機構群が構成されてもよい。足部Ｂ５の底には着床時の衝撃緩和のため、特開２００１−１２９７７４号公報に開示されているような弾性素材Ｂ５２が設けられている。
ロボットＲには、ロボットＲの世界座標系における位置および姿勢などの内部状態を測定するための複数の内部状態センサＳ1が搭載されている。ロボットＲの各関節機構の屈曲角度（関節角度）に応じた信号を出力するエンコーダ（図示略）、基体Ｂ０の姿勢（方位角および仰角により特定される。）に応じた信号を出力する傾斜センサ、および、足部Ｂ５および着床および離床の別を判定するための圧力センサなどが内部状態センサＳ1に該当する。ロボットＲの周囲の様子を撮像し、当該撮像座標に基づいて世界座標系に固定されている標識の位置を認識することにより、世界座標系におけるロボットＲの位置を認識するための撮像装置が内部状態センサＳ1に該当する。
たとえば、頭部Ｂ１に搭載され、ロボットＲの前方を撮像範囲とするＣＣＤカメラ、赤外線カメラ等、種々の周波数帯域における光を感知しうる左右一対の頭カメラＣ１が撮像装置として採用されうる。また、基体Ｂ０の前側下部に搭載され、ロボットＲの前方下方に向けて発せられた近赤外レーザー光の物体による反射光を検知することによりこの物体の位置や方位等を測定するための腰カメラ（アクティブセンサ）Ｃ２が撮像装置として採用されうる。
ロボットＲには、その周辺における物体の位置などの外部状態を測定するための外部状態センサＳ2が搭載されている。前記のような撮像装置が外部状態センサＳ2に該当する。
ロボットＲは制御システム１の一部を構成するコンピュータと、前記の複数の関節機構のそれぞれを動かすための複数のアクチュエータ２とを備えている。ロボットＲの内部状態および外部状態に応じて制御システム１から出力される制御指令にしたがってアクチュエータ２のそれぞれの動作が制御されることにより、ロボットＲがさまざまな態様で適応的に行動することができる。
なお、ロボットＲ（図１参照）のほか、車両など、部分的または全体的に目標軌道にしたがって変位するように動作が制御されるあらゆる装置が制御対象とされてもよい。
（制御システムの構成）
図２に示されている制御システム１は、ロボットＲに搭載されているコンピュータにより構成されている。制御システム１は、後述する演算処理を実行するように構成されている第１演算処理要素１１および第２演算処理要素１２を備えている。
第１演算処理要素１１および第２演算処理要素１２のそれぞれは、ハードウェア資源としてのプロセッサおよびメモリにより構成されている。第１演算処理要素１１および第２演算処理要素１２は、共通のハードウェア資源により構成されていてもよく、少なくとも部分的に異なるハードウェア資源により構成されていてもよい。
演算処理要素が演算処理を実行するように「構成されている」とは、当該演算処理要素を構成するプロセッサがメモリから必要なソフトウェアを読み取り、当該ソフトウェアを実行することにより演算処理を実行するようにプログラムされていることを意味する。
（ロボットの機能）
ここで、たとえば、図９に示されているようにロボットＲが左右の脚体Ｂ４を動かして、動いている人間（物体）の間をすり抜けながら目的地まで移動するというタスクを実行するようにアクチュエータ２の動作が制御される場合について説明する。
まず、制御システム１が停電状態から給電状態に切り替えられたこと等、トリガとなる事象に応じて、軌道候補の探索処理の繰り返し回数を表わす指数ｋが「１」にリセットされる（図３／ＳＴＥＰ００１）。
その上で、第１演算処理要素１１により、時空間座標系において第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の位置関係が設定される（図３／ＳＴＥＰ００２）。時空間座標系は、たとえば図４に示されているように２次元の実空間座標ｐ＝（ｘ，ｙ）および時間座標ｔ（１次元）により定義される３次元の空間座標系として定義される。実空間座標系は３次元であってもよい。
ここで、第２基準点ｑ2（ｋ）＝（ｐ2（ｋ），ｔ2（ｋ））の成分である第２時刻ｔ2（ｋ）のみが調節されることにより、第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の位置関係が調節される実施形態について考察する。すなわち、本実施形態では、第１基準点ｑ1（ｋ）＝（ｐ1（ｋ），ｔ1（ｋ））の成分である第１空間位置ｐ1（ｋ）および第１時刻ｔ1（ｋ）、ならびに、第２基準点ｑ2（ｋ）の成分である第２空間位置ｐ2（ｋ）は一定に維持される。一定に維持される成分は、リモコン等により外部から制御システム１に入力されてもよく、内部状態センサＳ1の出力信号等に基づいて計測されてもよい。
実空間におけるロボットＲの現在位置、出発予定位置または通過予定位置等が第１空間位置ｐ1（ｋ）として認識される。ロボットＲの現在位置は、ロボットＲに搭載されている受信機により人工衛星から受信されるＧＰＳ信号のほか、必要に応じて内部状態センサＳ1を構成する加速度センサ等の出力信号に基づいて計測されうる。現在時刻、出発予定時刻または通過予定時刻等が第１時刻ｔ1（ｋ）として認識される。現在時刻は、ロボットＲの制御システム１を構成する時計により計測されうる。
第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の初期位置関係（ｋ＝１）は、「第１条件」を満足するように設定される。「第１条件」は、ロボットＲの速度が、アクチュエータ２の動作性能のほか、ロボットＲの姿勢安定性の確保などの観点から定まる許容範囲［Ｖmin，Ｖmax］に収まるという条件である。第１条件は、第１空間位置ｐ1（ｋ）および第２空間位置ｐ2（ｋ）の空間間隔（直線距離）ｄｅｓ（ｐ2（ｋ）−ｐ1（ｋ））と、第１時刻ｔ1（ｋ）および第２時刻ｔ2（ｋ）の時間間隔ｔ2（ｋ）−ｔ1（ｋ）とに基づき、関係式（００１）により表現される。
Vmin≦des(p2(k)−p1(k))/(t2(k)−t1(k))≦Vmax ..(001)。
一定に維持される成分については指数ｋの付記を省略することにすると、本実施形態では関係式（００１）は関係式（００２）により表わされる。
Vmin≦des(p2−p1)/(t2−t1)≦Vmax ..(002)。
これにより、図４に示されているように、第１時刻ｔ1に対して、時間間隔Δｔ（ｋ）＝ｄｅｓ（ｐ2（ｋ）−ｐ1（ｋ））／Ｖmaxだけ時系列的に後の時刻が第２時刻ｔ2（ｋ＝１）として設定される。
時空間座標系における位置関係が設定された第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）を結ぶ軌道候補ｔｒ（ｋ）が探索される（図３／ＳＴＥＰ００３）。ｋ＝１の段階では、図４に破線で示されているように、時空間座標系において第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）を結ぶ直線線分が前回軌道候補ｔｒ（ｋ）（ｋ＝１）として探索される。
さらに、第２演算処理要素１２により、前回軌道候補ｔｒ（ｋ）と、人間等の物体の時系列的な位置を時空間座標系において表わす物体軌道ｔｒｏとの接触有無が判定される（図３／ＳＴＥＰ００４）。
当該判定に際して、制御対象であるロボットＲと、物体とのそれぞれの空間的広がりが考慮される。たとえば、ロボットＲが実空間において一定の広がりを有するロボット領域により表わされ、これと同様に物体も実空間において一定の広がりを有する物体領域により表わされる。
演算処理負荷の軽減のため、ロボットＲの空間的広がりが、ロボット領域よりも空間的に縮小された点領域として取り扱われる一方、物体の空間的広がりが、ロボット領域が縮小された分だけ物体領域が拡張された結果としてのミンコフスキー和として取り扱われてもよい（特開２００８−１４９４３６号公報等参照）。これにより、図４に示されているように、ロボットＲの軌道候補ｔｒ（ｋ）は異なる２つの点を結ぶ線分として認識される。その一方、物体軌道ｔｒｏは実空間における物体（正確には物体およびロボット）の広がりを表わす断面を有し、時間方向に延びる柱状体として認識される。
ロボットＲの空間的広がりは、制御システム１を構成するメモリにあらかじめ格納されている。そのほか、基体Ｂ０を基準とした腕体Ｂ２の広がり等、ロボットＲの姿勢が考慮されることにより、ロボットＲの空間的広がりが逐次算出されてもよい。ロボットＲの姿勢は、内部状態センサＳ1を構成するセンサの出力信号により表わされる関節角度等のキネマティクスパラメータに基づき、キネマティクスモデルにしたがって算出されうる。
物体の空間的広がりは、外部状態センサＳ2を構成する頭カメラＣ１等により撮像された画像の解析処理により認識されうる。物体の時系列的な位置（物体の空間的広がりの中心位置）は、物体の現在速度に基づいて予測されうる。物体の速度は、異なる時刻における撮像画像のそれぞれにおける物体の移動量が画像座標系から実空間座標系に座標変換されることにより求められる。物体が自律移動機能のほかに通信機能を有する他のロボットまたは移動装置である場合、当該物体との通信に基づき、その時系列的な予定位置が制御システム１により認識されてもよい。
第２演算処理要素１２により、前回軌道候補ｔｒ（ｋ）と物体軌道ｔｒｏとが接触すると判定された場合（図３／ＳＴＥＰ００４‥ＮＯ）、指数ｋが１だけ増やされる（図３／ＳＴＥＰ００６）。その上で、指数ｋが上限値Ｎを超えているか否かが判定される（図３／ＳＴＥＰ００７）。
指数ｋが上限値Ｎを超えていると判定された場合（図３／ＳＴＥＰ００７‥ＹＥＳ）、目標軌道は設定されない。この場合、ロボットＲが停止状態を維持する、または、ロボットＲが停止する等の制御処理が実行される。
その一方、指数ｋが上限値Ｎ以下であると判定された場合（図３／ＳＴＥＰ００７‥ＮＯ）、第１演算処理要素１１により、時空間座標系において第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の今回位置関係が設定される（図３／ＳＴＥＰ００２）。
第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の今回位置関係（ｋ＞１）は、「第２条件」を満足するように設定される。「第２条件」は、今回時間間隔Δｔ（ｋ）＝ｔ2（ｋ）−ｔ1が、前回時間間隔Δｔ（ｋ−１）＝ｔ2（ｋ−１）−ｔ1よりも長くなるという条件である。これにより、図４に矢印で示されているように、今回の第２基準点ｑ2（ｋ）が、前回の第２基準点ｑ2（ｋ−１）よりも時系列的に未来側にシフトされる。
その上で、時空間座標系における今回位置関係が設定された第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）を結ぶ軌道候補ｔｒ（ｋ）（ｋ＞１）が探索される（図３／ＳＴＥＰ００３）。軌道候補ｔｒ（ｋ）（ｋ＞１）の探索方法については後で詳細に説明する。
その上で、前記と同様に今回軌道候補ｔｒ（ｋ）と物体軌道ｔｒｏとの接触有無の判定以降の処理が繰り返される（図３／ＳＴＥＰ００４等参照）。今回軌道候補ｔｒ（ｋ）が、物体軌道ｔｒｏと接触しないと判定された場合（図３／ＳＴＥＰ００４‥ＹＥＳ）、当該今回軌道候補が目標軌道として設定される（図３／ＳＴＥＰ００５）。そして、当該目標軌道にしたがってロボットＲが移動するように、アクチュエータ２の動作が制御される。
（軌道候補の探索方法（第１実施形態））
第１演算処理要素１１により実行される軌道候補の探索処理の第１実施形態としてRRT-Connectが採用される（Knuffer et al. “An Efficient Approach to Single-Query Path Planning”, In Proc. 2000 IEEE Int’l Conf. on Robotics and Automation (ICRA 2000)参照）。
この場合、時空間座標系において配列される点列（ノード）に基づき、第１基準点ｑ1（ｋ）から第１線分を延長するとともに第２基準点ｑ2（ｋ）から第２線分を延長する延長処理が実行される。その上で、第１線分および第２線分を連結する連結処理が実行されることにより得られる線分が軌道候補ｔｒ（ｋ）として探索される。この結果、図５に示されているように、時空間座標系において第１基準点ｑ1（ｋ）と第２基準点ｑ2（ｋ）との中間に介在する複数のノード（点列）が連結された形の軌道候補ｔｒ（ｋ）が探索される。
軌道候補ｔｒ（ｋ）はスムージング処理により、折れ曲がり箇所の角が丸められることにより得られる曲線線分が最終的な軌道候補ｔｒ（ｋ）として定義されてもよい。
（軌道候補の探索方法（第２実施形態））
まず、前記のように時空間座標系における第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）のそれぞれの位置が認識される（図６／ＳＴＥＰ２０２）。
また、第１基準点側ツリーおよび第２基準点側ツリーのうち一方が選択される（図６／ＳＴＥＰ２０４）。この際、後述する軌道の延長軌道生成の失敗に伴う選択ツリーの変更回数を示す指数ｉが「１」に初期設定される。
その上で、ＲＲＴアルゴリズムにしたがって任意の位置に点（ノード）ｑが生成される（図６／ＳＴＥＰ２０６）。第１基準点側ツリーは初期段階では第１基準点ｑ1（ｋ）そのものであり、その後、ＲＲＴアルゴリズムにしたがって第１基準点ｑ1（ｋ）を基準として拡がるように逐次生成される点ｑが逐次連結されることによって徐々に成長する（図７（ｂ）破線参照）。同様に、第２基準点側ツリーは初期段階では第２基準点ｑ2（ｋ）そのものであり、その後、ＲＲＴアルゴリズムにしたがって第２基準点ｑ2（ｋ）を基準として拡がるように逐次生成される点ｑが逐次連結されることによって徐々に成長する（図７（ｂ）参照）。当該選択ツリーを対象として「延長処理」が実行される。
「延長処理」に際して、まず、選択ツリーの中から点ｑに最も近い点ｑnearが選択される（図６／ＳＴＥＰ２０８）。
点ｑが最近点ｑnearから閾値εを超えて離れている場合、線型補間により当該点ｑが最近点ｑnearから閾値εの距離まで近づけられ、この近づけられた点ｑが今回制御点ｑnewとして定義される（図６／ＳＴＥＰ２１０）。
そして、選択ツリーの根元から最近点ｑnearに至る点列であって、これまでのＢ−スプライン曲線の基礎となっている点列に対して、今回制御点ｑnewが末尾に追加された新たな点列を基礎としてＢ−スプライン曲線による補間処理が実行される。選択ツリーの根元から伸びる当該Ｂ−スプライン曲線のうち、今回制御点ｑnewが旧点列に追加されたことによって延長された部分が「延長軌道」として生成される（図６／ＳＴＥＰ２１２）。
Ｂ−スプライン曲線ｘ（ｔ）は、式（０１１）のように正規化されたＢ−スプライン基底関数Ｂi,N（ｔ）を結合係数とする、制御点列Ｐi（点の位置を表わす。ｉ＝１，２，‥ｍ）の線形結合により表現される。延長軌道は、曲線上のパラメータｔの区間が［ｔn-N-2，ｔn-N-1］と設定されることにより求められる。Ｎはオーダーを意味する。ｎはノット数を意味する。ｍは制御点の数を意味する。
x(t)＝Σi=1~mBi,N(t)Pi ..(011)。
正規化されたＢ−スプライン基底関数Bi,N(t)は次のCox-deBoorの帰納式（０１２）により定義される。
Bi,1(t)＝1（ti＜t＜ti+1の場合）,0（その他の場合）,
Bi,k(t)＝{(t−ti)/(ti+k-1−ti)}Bi,k-1(t)＋{(ti+k−t)/(ti+k−ti+1)}Bi+1,k-1(t) ..(012)。
これにより、たとえば、図７（ａ）に示されているように、選択ツリー（第１基準点側ツリー）に応じて定められる前回軌道（細線参照）に続く延長軌道（太線参照）が定義される。
さらに、第１判定処理が実行される（図６／ＳＴＥＰ２１４）。具体的には、延長軌道が、実空間における人間などの物体の時系列的な位置を表わす物体軌道から外れており、かつ、ロボットＲにその関節角度の許容範囲を超えた行動を強いる軌道ではないことという指定条件を満たすか否かがが判定される。当該物体軌道は、前記のように外部状態センサＳ2の出力信号に基づき、時空間座標系における存在態様が認識されうる。
延長軌道が指定条件を満たしていると判定された場合（図６／ＳＴＥＰ２１４‥ＹＥＳ）、すなわち、延長処理が成功した場合、今回制御点ｑnewまたは当該制御点ｑnewまでの延長軌道が選択ツリーに追加される（図６／ＳＴＥＰ２１５）。さらに、前回軌道および延長軌道が今回軌道として定義され、当該今回軌道は次回の延長処理に際して、前回軌道として取り扱われる。延長処理に続いて「連結処理」が実行される。
「連結処理」に際して、まず、選択ツリーが変更される（図６／ＳＴＥＰ２１６）。これにより、第１基準点側ツリーがそれまでの選択ツリーであった場合、第２基準点側ツリーが新たな選択ツリーとして以降の処理が実行される。同様に、第２基準点側ツリーがそれまでの選択ツリーであった場合、第１基準点側ツリーが新たな選択ツリーとして以降の処理が実行される。
選択ツリーの中から、先に生成された今回制御点ｑnew（図６／ＳＴＥＰ２１０参照）に最も近い点ｑnearが選択される（図６／ＳＴＥＰ２１８）。
そして、第１基準点側ツリーと第２基準点側ツリーのそれぞれの根元から最近点ｑnearに至る点列であって、これまでの第１基準点側軌道および第２基準点側軌道の基礎となっている点列に対して、今回制御点ｑnewが途中に追加された新たな点列を基礎としてＢ−スプライン曲線による補間処理が実行される。第１基準点および第２基準点を連結する当該Ｂ−スプライン曲線のうち今回制御点ｑnewの旧点列への追加によって生成された部分が「連結軌道」として計算される（図６／ＳＴＥＰ２２０）。
連結軌道は、今回制御点ｑnewのノード番号を「ｉ」と定めた場合、曲線上のパラメータｔの区間が［ｔi+1，ｔi+N+1］と設定されることにより求められる。これにより、図７（ｂ）に示されているように、第１基準点から伸びる第１基準点側軌道（一点鎖線参照）および第２基準点から伸びる第２基準点側軌道（二点鎖線参照）を連結する連結軌道（太線参照）が生成される。
さらに、連結軌道が指定条件を満たすか否かが判定される（図６／ＳＴＥＰ２２２）。
連結軌道が指定条件を満たしていると判定された場合（図６／ＳＴＥＰ２２２‥ＹＥＳ）、すなわち、連結処理が成功した場合、連結軌道ならびに当該連結軌道により連結された第１基準点側軌道および第２基準点側軌道が、第１基準点ｑ1（ｋ）から第２基準点ｑ2（ｋ）まで至るロボットＲの軌道候補ｔｒ（ｋ）として生成または探索される（図６／ＳＴＥＰ２２４）。図８には、本発明の手法によって障害物を回避するように生成された、第１基準点ｑ1（ｋ）から第２基準点ｑ2（ｋ）に至る軌道候補ｔｒ（ｋ）が例示されている。
そして、図９に示されているように、ロボットＲが左右の脚体Ｂ４を動かして、動いている人間（物体）の間をすり抜けながら目的地まで移動するというタスクを実行するようにアクチュエータ２の動作が制御される。
その一方、連結軌道が指定条件を満たしていないと判定された場合（図６／ＳＴＥＰ２２２‥ＮＯ）、すなわち、連結処理が失敗した場合、延長処理が再び実行される（図６／ＳＴＥＰ２０８〜ＳＴＥＰ２１４参照）。
さらに、延長軌道が指定条件を満たしていないと判定された場合（図６／ＳＴＥＰ２１４‥ＮＯ）、すなわち、延長処理が失敗した場合、今回制御点ｑnewまたは当該制御点ｑnewまでの延長軌道は選択ツリーに追加されずに棄却される（図６／ＳＴＥＰ２２５）。その上で、指数ｉが上限値Ｋ以下であるか否かが判定される（図６／ＳＴＥＰ２２６）。
指数ｉが上限値Ｋ以下であると判定された場合（図６／ＳＴＥＰ２２６‥ＹＥＳ）、選択ツリーが変更される（図６／ＳＴＥＰ２２８）。この際、指数ｉが１だけ増加する。その上で、任意の点ｑの生成（図６／ＳＴＥＰ２０６参照）以降の処理が当該変更後の選択ツリーを対象として実行される。
指数ｉが上限値Ｋを超えていると判定された場合（図６／ＳＴＥＰ２２６‥ＮＯ）、軌道候補探索が失敗したと認識される（図６／ＳＴＥＰ２３０）。これに応じて、制御システム１は、ロボットＲの当該軌道にしたがった動作を停止させて物体の移動（外部状態センサＳ2を通じて認識される）などの状況変化を待つ、あるいは、脚体Ｂ４を動かしてロボットＲをその場から移動させたり、その場で基体Ｂ０をひねったりすることによりタスク実行開始条件をロボットＲの動作によって変化させる。その上で、制御システム１は前記のような軌道候補探索を再実行する。
（点排除処理）
第１演算処理要素１１により、軌道探索処理（図３／ＳＴＥＰ００３参照）に際して用いられる点列から、軌道候補の生成基礎として不適当な点の排除処理が実行される。
具体的には、軌道候補ｔｒ（ｋ）の探索処理に際して用いられる第１基準点ｑ1（ｋ）に続く点列から、先の点に対して時系列的に過去に位置する点が排除される。また、軌道候補ｔｒ（ｋ）の探索に際して用いられる第２基準点ｑ2（ｋ）に続く点列から、先の点に対して時系列的に未来に位置する点が排除される。これにより、時空間座標系において、時系列的に過去にさかのぼるような実現不可能な軌道候補ｔｒ（ｋ）が探索される事態が回避されうる。
また、軌道候補ｔｒ（ｋ）の探索に際して用いられる点列から、先の点に対して、アクチュエータ２の動作性能に応じて定まる許容範囲から外れている点が排除される。これにより、ロボットＲが、アクチュエータ２の動作性能に応じた許容範囲から外れるような速度で変位することを強いられる事態が回避されうる。
（本発明の制御システムの作用効果）
前記機能を発揮する制御システム１およびロボットＲによれば、時空間座標系において物体軌道ｔｒｏと接触しないような軌道候補ｔｒ（ｋ）が目標軌道として生成される。この目標軌道にしたがってアクチュエータ２の動作が制御されることにより、制御対象であるロボットＲに物体と接触する事態を回避させながら実空間を変位させることができる。
時空間座標系における軌道候補の第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の相対的な位置関係は、制御対象が第１空間位置ｐ1（ｋ）から第２空間位置ｐ2（ｋ）まで移動する際の速度の高低を定める。たとえば、時空間座標系における時系列的に先の第１基準点ｑ1（ｋ）および時系列的に後の第２基準点ｑ2（ｋ）の時間間隔、すなわち、制御対象が第１空間位置ｐ1（ｋ）から第２空間位置ｐ2（ｋ）まで変位するのに要する時間が長いほどその速度が低くなる。時空間座標系における第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の空間間隔、すなわち、制御対象が第１空間位置ｐ1（ｋ）から第２空間位置ｐ2（ｋ）まで移動する距離が短いほど速度が低くなる。
そこで、時空間座標系における第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の初期位置関係（ｋ＝１）が「第１条件」を満足するように設定される。「第１条件」はアクチュエータ２の動作性能に応じて定められている。
これにより、ロボットＲ（制御対象）を第１空間位置ｐ1（ｋ）から第２空間位置ｐ2（ｋ）まで変位させる際の目標速度が、アクチュエータ２の動作性能に応じた制御対象の変位速度の許容範囲から外れるような不適当な値になる事態が回避される。
さらに、前回軌道候補ｔｒ（ｋ−１）が物体軌道ｔｒｏに接触すると判定された場合、時空間座標系における第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の今回位置関係（ｋ＞１）が「第２条件」を満足するように設定される。「第２条件」は第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の今回時間間隔が前回時間間隔よりも長くなるという条件である。
これにより、軌道候補ｔｒ（ｋ）の再探索が繰り返されるたび、制御対象を実空間において第１空間位置ｐ1（ｋ）から第２空間位置ｐ2（ｋ）まで変位させる際の速度を徐々に低下させるような軌道候補ｔｒ（ｋ）が探索される。このため、制御対象の目標速度が、アクチュエータ２の動作性能に応じた制御対象の変位速度の許容範囲の上限値を超えるような不適当な値になる事態が回避される。
よって、アクチュエータ２の動作性能等の制御対象の状況に鑑みて、時空間座標系における当該制御対象の目標軌道が適当な形態で探索されうる（図９参照）。
特に、第２実施形態の軌道候補探索手法によれば、空間補間用の基底関数を結合係数とする、時空間座標系に配置された点列の線形結合により表現される線分を生成する補間処理が実行されることにより、目標軌道またはその候補が生成される（式（１１）、図７（ａ）（ｂ）参照）。このため、時空間座標系に配置された複数の点のそれぞれを通る折れ線（図５参照）ではなく、当該複数の点（制御点）の線形結合により表わされる、第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）を連続的にまたは滑らかに連結する線分が制御対象の目標軌道またはその候補として生成される（図７（ｂ）、図８参照）。これにより、目標軌道にしたがった制御対象の動作の連続性または円滑性の維持が図られうる。
（本発明の他の実施形態）
ロボットＲ（制御対象）が大局経路にしたがって動いている途中で、人間等の物体との接触を回避するための迂回経路を生成する際に、前記のような目標軌道の探索手法が用いられてもよい。
図１０に示されているようにロボットＲが腕体Ｂ２を動かすことにより、ハンドＨおよびコップが、テーブルに置かれている他のコップと接触しないようにハンドＨで把持しているコップを動かし、このテーブルに置くというタスクを実行する際に、前記のような目標軌道の探索手法が用いられてもよい。
この場合、第１基準点ｑ1（ｋ）は、実空間においてロボットＲがコップを把持している状態の一方のハンドＨの位置に対応している。第２基準点ｑ2（ｋ）は、たとえば、実空間においてロボットＲがコップをテーブルに置く際のハンドＨの位置に対応している。
第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）のそれぞれは、内部状態センサＳ1および外部状態センサＳ2のそれぞれの出力信号に基づき、制御システム１によって計算されてもよく、外部端末装置から無線通信によって制御システム１に対して入力されてもよい。
ハンドＨが矢印付き曲線で表わされているように実空間において変位するようにロボットＲの動作が制御される。これにより、動作中に他の物体との接触を回避し、かつ、各関節角度が許容範囲から外れることを防止しながら、ハンドＨで把持しているコップをテーブルに置くというタスクをロボットＲに円滑に実行させることができる。
前記実施形態によれば、第１空間位置ｐ1（ｋ）、第１時刻ｔ1（ｋ）および第２空間位置ｐ2（ｋ）が固定される一方、第２時刻ｔ2（ｋ）が変更されることにより、時空間座標系における第１基準点ｑ1（ｋ）＝（ｐ1（ｋ），ｔ1（ｋ））および第２基準点ｑ2（ｋ）＝（ｐ2（ｋ），ｔ2（ｋ））の位置関係が調節される。この実施例（実施例１）も含めて、表１に示されているさまざまな実施例により当該位置関係が調節されてもよい。
Figure 0005480799
 ○：変更可能、−：固定
@0001
たとえば、第１空間位置ｐ1（ｋ）がロボットＲ等の制御対象の現在位置またはタスク実行等のために必須の位置等の変更不可の位置であり、かつ、第１時刻ｔ1（ｋ）が現在時刻等の変更不可の時刻である場合、表１における実施例１、２または５の変更形態にしたがって第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の位置関係が調節されうる。
これとは逆に第２空間位置ｐ2（ｋ）および第２時刻ｔ2（ｋ）が変更不可である場合、表１における実施例３、４または１０の変更形態にしたがって第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の位置関係が調節されうる。
第１演算処理要素１１が、次のように軌道候補ｔｒ（ｋ）の探索処理を実行するように構成されていてもよい。
すなわち、時空間座標系において１つの第１基準点ｑ1（ｋ）に対して複数の第２基準点ｑ2（ｋ；ｍ2）（ｍ2＝１〜Ｍ2）が定義された上で、延長処理および連結処理の実行により最も早く得られた、当該１つの第１基準点ｑ1（ｋ）と当該複数の第２基準点ｑ2（ｋ；ｍ2）のうち１つの第２基準点ｑ2（ｋ；ｊ2）とを結ぶ線分が軌道候補ｔｒ（ｋ）として探索されてもよい。
これに代えて、時空間座標系において１つの第２基準点ｑ2（ｋ）に対して複数の第１基準点ｑ1（ｋ；ｍ1）（ｍ1＝１〜Ｍ1）が定義された上で、延長処理および連結処理の実行により最も早く得られた、当該複数の第１基準点ｑ1（ｋ；ｍ1）のうち１つの第１基準点ｑ1（ｋ；ｊ1）と当該１つの第２基準点ｑ2（ｋ）とを結ぶ線分が軌道候補ｔｒ（ｋ）として探索されてもよい。
当該構成の制御システム１によれば、アクチュエータ２に対して目標軌道の探索結果に応じた動作指令信号が可及的速やかに送信されうる。このため、目標軌道の探索と並行して制御対象の変位態様がリアルタイムで制御される際、この制御対象の動作が一時的に停止または休止状態となる事態が回避されうる。
さらに、時空間座標系において１つの第１基準点ｑ1（ｋ）に対して複数の第２基準点ｑ2（ｋ；ｍ2）（ｍ2＝１〜Ｍ2）が定義された上で、延長処理および連結処理の実行により、当該１つの第１基準点ｑ1（ｋ）と当該複数の第２基準点ｑ2（ｋ；ｍ）のそれぞれとを結ぶ線分が生成され、当該複数の線分から第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ；ｍ2）の時間間隔に応じて１つの線分が選択され、当該選択線分が軌道候補ｔｒ（ｋ）として探索されてもよい。
これに代えて、時空間座標系において１つの第２基準点ｑ2（ｋ）に対して複数の第１基準点ｑ1（ｋ；ｍ1）（ｍ1＝１〜Ｍ1）が定義された上で、延長処理および連結処理の実行により、当該複数の第１基準点ｑ1（ｋ；ｍ1）のそれぞれと当該１つの第２基準点ｑ2（ｋ）とを結ぶ線分が生成され、当該複数の線分から第１基準点ｑ1（ｋ；ｍ1）および第２基準点ｑ2（ｋ）の時間間隔に応じて１つの線分が選択され、当該選択線分が軌道候補ｔｒ（ｋ）として探索されてもよい。
当該構成の制御システム１によれば、制御対象を目標軌道にしたがって第１空間位置ｐ1（ｋ）から第２空間位置ｐ2（ｋ）まで変位させるための所要時間の長短が調節されうる。たとえば、所要時間が最も短く済むような軌道候補ｔｒ（ｋ）が目標軌道として設定されることにより、可能な限り速やかに制御対象を変位させることができる。
第１演算処理要素１１が、時空間座標系における第１基準点ｑ1（ｋ）＝（ｐ1（ｋ），ｔ1（ｋ））および第２基準点ｑ2（ｋ）＝（ｐ2（ｋ），ｔ2（ｋ））の初期時間間隔ｔ2（ｋ）−ｔ1（ｋ）および初期空間間隔ｄｅｓ（ｐ2（ｋ）−ｐ1（ｋ））のうち少なくとも一方の長短を調節することにより第１基準点ｐ1（ｋ）および第２基準点ｐ2（ｋ）の初期位置関係（ｋ＝１）を設定するように構成されていてもよい。
第１時刻ｔ1（ｋ）および第２時刻ｔ2（ｋ）のうち少なくとも一方が変更されることにより、時間間隔ｔ2（ｋ）−ｔ1（ｋ）の長短が調節される（表１、実施例１，３，５〜８，１０〜１５参照）。第１空間位置および第２空間位置のうち少なくとも一方が変更されることにより、初期空間間隔ｄｅｓ（ｐ2（ｋ）−ｐ1（ｋ））の長短が調節される（表１、実施例２，４，５，７〜１５参照）。
制御対象と物体との接触確率の高低は、制御システム１により認識された制御対象の周辺に存在する物体の多少、制御対象と物体との空間間隔の長短、または、制御対象の速度ベクトルの延長線分と、物体の速度ベクトルの延長線分との交差の有無などに応じて評価されうる。
第１演算処理要素１１が、第２演算処理要素１２により軌道候補ｔｒ（ｋ−１）が物体軌道ｔｒｏと接触すると判定されたことを要件として（図３／ＳＴＥＰ００４‥ＮＯ参照）、制御対象と物体との接触確率の高低に応じて、第１基準点ｑ1（ｋ）＝（ｐ1（ｋ），ｔ1（ｋ））および第２基準点ｑ2（ｋ）＝（ｐ2（ｋ），ｔ2（ｋ））の今回時間間隔ｔ2（ｋ）−ｔ1（ｋ）および今回空間間隔ｄｅｓ（ｐ2（ｋ）−ｐ1（ｋ））のうち少なくとも一方の長短を、前回時間間隔および前回空間間隔のうち少なくとも一方に基づいて調節することにより第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の今回位置関係（ｋ＞１）を設定するように構成されていてもよい。
たとえば、前回時間間隔ｔ2（ｋ−１）−ｔ1（ｋ−１）に対する今回時間間隔ｔ2（ｋ）−ｔ1（ｋ）の増加量が、接触確率が高いほど大きくなるように第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の今回位置関係が設定されてもよい。また、前回空間間隔ｄｅｓ（ｐ2（ｋ−１）−ｐ1（ｋ−１））に対する今回時間間隔ｄｅｓ（ｐ2（ｋ）−ｐ1（ｋ））の増加量または減少量が、接触確率が高いほど大きくなるように第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の今回位置関係が設定されてもよい。
当該構成の制御システム１によれば、制御対象と物体との接触確率を低下させうるような軌道候補ｔｒ（ｋ）が早期に探索されるように、時空間座標系における第１基準点ｑ1（ｋ）および第２基準点ｑ2（ｋ）の位置関係が適当に設定または補正されうる。
第２実施形態の軌道候補探索処理に際して、第１判定処理および第２判定処理が省略され、連結処理が終了した段階において生成された位置軌道が指定条件を満たすか否かが判定され、当該判定結果が肯定的であったことを要件としてこの位置軌道が目標軌道として設定されてもよい。
Ｂ−スプライン曲線（式（０１１）参照）のほか、式（０２１）により定義されるHermite曲線または式（０３１）により定義されるBezier曲線が空間補間曲線として用いられることによって、延長処理および連結処理のそれぞれが実行されてもよい。
P(t)＝(2t3−3t2＋1)Pstart＋(t3−2t2＋t)G1start＋(−2t3＋3t2)Pend＋(t3−t2)G1end..(021)。
ここで、ＰstartおよびＰendは曲線の端点の位置ベクトルを表わしている。また、Ｇ１startおよびＧ１endは当該端点における曲線の接線ベクトル（１階微分）を表わしている。延長処理および連結処理に際して、制御点列Ｐi（i＝1,2, ..m）の各区間をHermite曲線で補間することによってHermite曲線がRRT-Connectに適用される。この際、Ｇ１start、Ｇ１endは隣り合う区間で１階微分の連続性が保たれるように選択されなければならない。
x(t)＝Σi=0~mJn,i(t)Pi ..(031)。
ここで、Bezier基底関数（またはBernstein基底関数）Jn,i(t)は式（０３２）により定義される。
Jn,i(t)＝n!/{i!(n−i)!}ti(1−t)n-i ..(032)。
延長処理および連結処理において用いられる空間補間は、原則的に、空間補間用の基底関数を結合係合とする、時空間座標系に配置された点列の線形結合により表現される線分を形成する空間補間であること（第１補足条件）、および、既存の制御点列に新しい制御点が付加されたとき、既存の制御点によって作られる部分曲線が変化しないこと（第２補足条件）という２つの条件を満たしている必要がある。
ただし、Bezier曲線は第２条件を満たさないものの、Hermite曲線と同様、経路点列が複数の区間に分割されることにより、複数のBezier曲線が連結されることで、空間補間曲線として採用されうる。
１‥制御システム、２‥アクチュエータ、１１‥第１演算処理要素、１２‥第２演算処理要素、Ｒ‥ロボット（制御対象）。

Claims (11)

  1. アクチュエータの動作を制御することにより、実空間における制御対象の時系列的な位置を、実空間座標系および時間座標系により定義される時空間座標系において定義される目標軌道にしたがって制御するように構成されているシステムであって、
    前記時空間座標系において、第１基準点および第２基準点の初期位置関係が、前記アクチュエータの動作性能に応じて定まる第１条件を満たすように当該初期位置関係を設定した上で、前記時空間座標系において配列される点列に基づき、前記第１基準点から第１線分を延長するとともに前記第２基準点から第２線分を延長する延長処理を実行した上で、前記第１線分および第２線分を連結する連結処理を実行することにより得られる線分を軌道候補として探索するように構成されている第１演算処理要素と、
    前記軌道候補が、前記制御対象とは別個の物体の時系列的な位置軌道を前記時空間座標系において表わす物体軌道と接触するか否かを判定するように構成されている第２演算処理要素とを備え、
    前記第２演算処理要素により前回軌道候補が前記物体軌道と接触すると判定されたことを要件として、前記第１演算処理要素が、前記第１基準点および前記第２基準点の今回位置関係が、前記時空間座標系における前記第１基準点および前記第２基準点の今回空間間隔が前回空間間隔よりも短くなる、前記第１基準点および前記第２基準点の今回時間間隔が前回時間間隔よりも長くなる、あるいは、前記第１基準点および前記第２基準点の今回空間間隔が前回空間間隔よりも短くなり、かつ、今回時間間隔が前回時間間隔よりも長くなるという第２条件を満たすように当該今回位置関係を設定した上で、今回軌道候補を探索する一方、前記第２演算処理要素により前回軌道候補が前記物体軌道と接触しないと判定されたことを要件として、当該前回軌道候補を前記目標軌道として設定するように構成され、
    前記第１演算処理要素が、前記軌道候補の探索に際して用いられる前記第１基準点に続く前記点列から、先の点に対して時系列的に過去に位置する点を排除するとともに、前記軌道候補の探索に際して用いられる前記第２基準点に続く前記点列から、先の点に対して時系列的に未来に位置する点を排除するように構成されていることを特徴とする制御システム。
  2. 請求項１記載の制御システムにおいて、
    前記第１演算処理要素が、前記軌道候補の探索に際して用いられる前記点列から、先の点に対して、前記アクチュエータの動作性能に応じて定まる許容範囲から外れている点を排除するように構成されていることを特徴とする制御システム。
  3. 請求項１又は２記載の制御システムにおいて、
    前記第１演算処理要素が、前記時空間座標系において１つの第１基準点に対して複数の第２基準点を定義し、あるいは、１つの第２基準点に対して複数の第１基準点を定義した上で、前記延長処理および前記連結処理を実行することにより最も早く得られた、前記１つの第１基準点と前記複数の第２基準点のうち１つの第２基準点とを結ぶ線分、または、前記複数の第１基準点のうち１つの第１基準点と前記１つの第２基準点とを結ぶ線分を、前記軌道候補として探索するように構成されていることを特徴とする制御システム。
  4. 請求項１又は２記載の制御システムにおいて、
    前記第１演算処理要素が、前記時空間座標系において１つの第１基準点に対して複数の第２基準点を定義し、あるいは、１つの第２基準点に対して複数の第１基準点を定義した上で、前記延長処理および前記連結処理を実行することにより、前記１つの第１基準点と前記複数の第２基準点のそれぞれとを結ぶ線分、または、前記複数の第１基準点のそれぞれと前記１つの第２基準点とを結ぶ線分から、第１基準点および第２基準点の時間間隔に応じて１つの線分を選択し、当該選択した線分を前記軌道候補として探索するように構成されていることを特徴とする制御システム。
  5. 請求項１〜４のうちいずれか１つに記載の制御システムにおいて、
    前記第１演算処理要素が、前記制御対象と前記物体との接触確率の高低に応じて、前記時空間座標系における前記第１基準点および前記第２基準点の初期時間間隔および初期空間間隔のうち少なくとも一方の長短を調節することにより前記第１基準点および前記第２基準点の初期位置関係を設定するように構成されていることを特徴とする制御システム。
  6. 請求項１〜５のうちいずれか１つに記載の制御システムにおいて、
    前記第１演算処理要素が、前記第２演算処理要素により前記軌道候補が前記物体軌道と接触すると判定されたことを要件として、前記制御対象と前記物体との接触確率の高低に応じて、前記時空間座標系における前記第１基準点および前記第２基準点の今回時間間隔および今回空間間隔のうち少なくとも一方の長短を、前回時間間隔および前回空間間隔のうち少なくとも一方を調節することにより前記第１基準点および前記第２基準点の今回位置関係を設定するように構成されていることを特徴とする制御システム。
  7. 請求項１〜６のうちいずれか１つに記載の制御システムにおいて、
    前記第１演算処理要素が、前記時空間座標系において、相互に離れて配置されている点列に基づき、空間補間用の基底関数を結合係数とする、前記点列の線形結合として表現される線分を生成する補間処理を実行するとともに、前記補間処理により生成された一の線分の基礎である前記点列に点を加えて新たな点列を定義した上で、前記新たな点列を基礎として前記補間処理を実行することにより、当該一の線分が延長線分によって延長された新たな線分を生成する前記延長処理を実行するとともに、
    前記延長処理が実行されることにより最近に延長された前記一の線分の基礎である前記新たな点列と、前記補間処理により生成された他の線分の基礎である他の点列とを基礎として前記補間処理を実行することにより、前記一の線分および前記他の線分が連結線分によって連結された形の線分を前記軌道候補として生成する前記連結処理を実行するように構成されていることを特徴とする制御システム。
  8. 請求項７記載の制御システムにおいて、
    前記第２演算処理要素が、前記時空間座標系において、前記延長線分が前記物体軌道から外れていることを含む指定条件を満たしているか否かを判定する第１判定処理を実行するように構成され、
    前記第１演算処理要素が、前記第１判定処理の結果が否定的であることを要件として、前記一の線分を棄却した上で前記延長処理を実行するように構成されていることを特徴とする制御システム。
  9. 請求項７または８記載の制御システムにおいて、
    前記第２演算処理要素が、前記時空間座標系において、前記連結線分が前記物体軌道から外れていることを含む指定条件を満たしているか否かを判定する第２判定処理を実行するように構成され、
    前記第１演算処理要素が、前記第２判定処理の結果が否定的であることを要件として、前記新たな点列を再定義した上で前記延長処理を実行するように構成されていることを特徴とする制御システム。
  10. アクチュエータと、請求項１〜９のうちいずれか１つに記載の制御システムとを備え、前記制御システムによって前記アクチュエータの動作が制御されることにより、実空間において目標軌道にしたがって移動するように構成されていることを特徴とする前記制御対象としての移動装置。
  11. 基体と、前記基体から延設された肢体と、アクチュエータと、請求項１〜９のうちいずれか１つに記載の制御システムとを備え、前記制御システムによって前記アクチュエータの動作が制御されることにより、実空間において目標軌道にしたがって前記制御対象としての前記肢体を変位させるように構成されていることを特徴とするロボット。
JP2010278470A 2010-12-14 2010-12-14 移動装置およびロボットならびにこれらの制御システム Active JP5480799B2 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
JP2010278470A JP5480799B2 (ja) 2010-12-14 2010-12-14 移動装置およびロボットならびにこれらの制御システム
US13/313,318 US8942847B2 (en) 2010-12-14 2011-12-07 Mobile apparatus and robot, and control system thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2010278470A JP5480799B2 (ja) 2010-12-14 2010-12-14 移動装置およびロボットならびにこれらの制御システム

Publications (2)

Publication Number Publication Date
JP2012128585A JP2012128585A (ja) 2012-07-05
JP5480799B2 true JP5480799B2 (ja) 2014-04-23

Family

ID=46200160

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2010278470A Active JP5480799B2 (ja) 2010-12-14 2010-12-14 移動装置およびロボットならびにこれらの制御システム

Country Status (2)

Country Link
US (1) US8942847B2 (ja)
JP (1) JP5480799B2 (ja)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101339480B1 (ko) 2012-12-14 2013-12-10 고려대학교 산학협력단 Ｒｒｔ 기반의 듀얼 트리 구조를 이용한 이동 로봇의 궤적 계획 방법
TW201529565A (zh) * 2013-11-28 2015-08-01 Univ Kyushu Nat Univ Corp 發光材料、有機發光元件及化合物
CN105446350B (zh) * 2014-09-26 2018-05-29 科沃斯机器人股份有限公司 自移动机器人移动界限划定方法
CN105807906B (zh) * 2014-12-31 2019-03-12 吴江 运动识别方法
JP6417269B2 (ja) 2015-04-27 2018-11-07 本田技研工業株式会社 移動体の移動速度推定装置及び移動体の制御装置
CN109976347B (zh) * 2019-04-11 2023-10-13 中南大学 一种基于快速扩展随机树和势场法的视觉伺服路径规划方法
US20230302636A1 (en) * 2022-03-23 2023-09-28 Mitsubishi Electric Research Laboratories, Inc. Systems and Methods for Flexible Robotic Manipulation by Fast Online Load Estimation

Family Cites Families (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2675970B2 (ja) * 1993-12-27 1997-11-12 アイシン・エィ・ダブリュ株式会社 ナビゲーション装置
JP3555461B2 (ja) * 1998-09-01 2004-08-18 トヨタ自動車株式会社 経路決定方法及び移動体交通制御システム
JP4279425B2 (ja) * 1999-11-05 2009-06-17 本田技研工業株式会社 脚式歩行ロボットの足部構造
US7447593B2 (en) * 2004-03-26 2008-11-04 Raytheon Company System and method for adaptive path planning
WO2006121091A1 (ja) * 2005-05-13 2006-11-16 Toyota Jidosha Kabushiki Kaisha 経路計画装置
JP4143103B2 (ja) * 2006-12-20 2008-09-03 本田技研工業株式会社 移動装置、ならびにその制御システム、制御プログラムおよび監督システム
EP1972415B1 (en) * 2007-03-23 2019-01-02 Honda Research Institute Europe GmbH Robots with collision avoidance functionality
EP1972416B1 (en) * 2007-03-23 2018-04-25 Honda Research Institute Europe GmbH Robots with occlusion avoidance functionality
JP4328813B2 (ja) * 2007-04-06 2009-09-09 本田技研工業株式会社 移動装置、ならびにその制御方法および制御プログラム
JP4328819B2 (ja) * 2007-11-30 2009-09-09 本田技研工業株式会社 移動装置およびその制御プログラム
JP4978494B2 (ja) * 2008-02-07 2012-07-18 トヨタ自動車株式会社 自律移動体、及びその制御方法
JP4717105B2 (ja) * 2008-08-29 2011-07-06 株式会社日立製作所 自律移動ロボット装置及びかかる装置における飛び出し衝突回避方法
JP5063549B2 (ja) * 2008-09-29 2012-10-31 本田技研工業株式会社 移動装置
KR101554515B1 (ko) * 2009-01-07 2015-09-21 삼성전자 주식회사 로봇의 경로계획장치 및 그 방법
KR101667030B1 (ko) * 2009-08-10 2016-10-17 삼성전자 주식회사 로봇의 경로 계획 장치 및 그 방법

Also Published As

Publication number Publication date
US20120150350A1 (en) 2012-06-14
JP2012128585A (ja) 2012-07-05
US8942847B2 (en) 2015-01-27

Similar Documents

Publication Publication Date Title
JP5480799B2 (ja) 移動装置およびロボットならびにこれらの制御システム
JP4645601B2 (ja) 環境地図の生成方法及び移動ロボット
US7769491B2 (en) Obstacle avoiding apparatus, obstacle avoiding method, obstacle avoiding program, and mobile robot apparatus
WO2011016280A1 (ja) ロボットおよび制御システム
JP5261495B2 (ja) 重み行列を用いたリアルタイム自己衝突および障害物回避
US8855814B2 (en) Robot and control method thereof
US8768512B2 (en) Robot and control method thereof
JP5465137B2 (ja) ロボットおよび制御システム
JP5489965B2 (ja) 行動制御システムおよびロボット
JP5404519B2 (ja) ロボット、制御システムおよび制御プログラム
US11292132B2 (en) Robot path planning method with static and dynamic collision avoidance in an uncertain environment
JP5465142B2 (ja) ロボットおよびその行動制御システム
US20120078419A1 (en) Robot and control method thereof
EP4157589A1 (en) A robot path planning method with static and dynamic collision avoidance in an uncertain environment
JP6450273B2 (ja) 移動ロボットの動作環境情報生成装置
JP6026393B2 (ja) 移動装置
JP5306934B2 (ja) ロボットおよび制御システム
JP5456557B2 (ja) ロボット、制御システムおよび制御プログラム
JP5539001B2 (ja) 制御装置
JP6647143B2 (ja) 機能装置ならびにその制御装置および制御方法
JP5450218B2 (ja) ロボットおよび制御システム
CN113146637A (zh) 一种机器人笛卡尔空间的运动规划方法
JP2011031349A (ja) ロボットおよび制御システム
JP4488811B2 (ja) ロボット制御方法およびロボットシステム
JP5539000B2 (ja) 制御装置

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20121127

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20130422

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20130507

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20130703

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20140121

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20140214

R150 Certificate of patent or registration of utility model

Ref document number: 5480799

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150