JP2011230213A - ロボット、制御システムおよび制御プログラム - Google Patents

Abstract

【課題】制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮を図ることができるシステム等を提供する。
【解決手段】本発明の制御システム１によれば、空間補間用の基底関数を結合係数とする、モデル空間に配置された点列の線形結合により表現される線分を生成する補間処理が実行される。その結果、各関節機構の屈曲状態（各軸回りの回転角度により表現される。）を表わすクォータニオンの目標軌道またはその候補が生成される。
【選択図】 図５

Description

本発明は、制御システムおよびその制御対象の１つであるロボットなどに関する。

ロボットアームなど、回転自由度を有する関節機構を備えている制御対象の動作を定める軌道を探索する手法が提案されている（特許文献１参照）。たとえば、関節機構の角度（関節角度）またはローカル座標系における制御対象の指定部分の位置または姿勢を表わすオイラー角の軌道を探索することが考えられる。

特開２０１０−００５７６１号公報

しかし、制御対象の関節角度により軌道が表現された場合、当該制御対象が不必要に大きく動くような非効率的な軌道が生成される可能性がある。また、ローカル座標系における制御対象の指定部分の位置または姿勢を表わすオイラー角により軌道が表現された場合、当該制御対象の関節角度限界が厳密に考慮されないために探索時間が長くなる傾向がある。これは、軌道を記述する空間に、特異点、すなわち、ヤコビ行列が非正則となり、微分が定義されえない点（内挿できない）点が存在するためである。

そこで、本発明は、制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮を図ることができるシステム等を提供することを解決課題とする。

前記課題を解決するための本発明の制御システムは、２または３つの直交軸のそれぞれ回りの回転角度によって屈曲状態が表現される関節機構を備えている制御対象の動作を制御するシステムであって、前記関節機構における各軸回りの回転角度が、クォータニオンによって表現されるモデル空間において、相互に離れて配置されている点列に基づき、空間補間用の基底関数を結合係数とする、前記点列の線形結合として表現される線分を生成する補間処理を実行するとともに、前記補間処理により生成された一の線分の基礎である前記点列に点を加えて新たな点列を定義した上で、前記新たな点列を基礎として前記補間処理を実行することにより、当該一の線分が延長線分によって延長された新たな線分を生成する延長処理を実行するように構成されている第１演算処理要素と、前記第１演算処理要素により延長処理が実行されることにより最近に延長された前記一の線分の基礎である前記新たな点列と、前記補間処理により生成された他の線分の基礎である他の点列とを基礎として前記補間処理を実行することにより、前記一の線分および前記他の線分が接続線分によって接続された形の線分を、前記関節機構における各軸回りの回転角度を表わすクォータニオンの目標軌道またはその候補として生成する接続処理を実行するように構成されている第２演算処理要素とを備え、前記第１演算処理要素および前記第２演算処理要素のそれぞれが、前記関節機構の屈曲状態が２つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０である第１種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行する一方、前記関節機構の屈曲状態が３つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０に拘束されない第２種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行するように構成されていることを特徴とする。

本発明の制御システムによれば、空間補間用の基底関数を結合係数とする、モデル空間に配置された点列の線形結合により表現される線分を生成する補間処理が実行される。「線分」は直線および曲線のいずれであってもよい。その結果、関節機構における各軸回りの回転角度を表わすクォータニオンの目標軌道またはその候補が生成される。クォータニオンにより表現されるモデル空間には特異点が無いため軌道の探索時間の短縮が図られる。また、クォータニオンにより記述されるモデル空間の補間は球面線形補間となるため、冗長性が排除された効率的な軌道が計算されうる。よって、制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮が図られる。

前記第１演算処理要素が、前記モデル空間において、前記延長線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第１判定処理を実行するように構成され、前記第１判定処理の結果が否定的であることを要件として、前記一の線分を棄却した上で前記延長処理を実行するように構成されていてもよい。

前記第２演算処理要素が、前記モデル空間において、前記接続線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第２判定処理を実行するように構成され、前記第２判定処理の結果が否定的であることを要件として、前記第１演算処理要素が、前記新たな点列を再定義した上で前記延長処理を実行するように構成されていてもよい。

当該構成の制御システムによれば、障害物との接触を回避しながらも、制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮が図られる。

前記課題を解決するための本発明の制御プログラムは、制御対象に搭載されているコンピュータを前記制御システムとして機能させることを特徴とする。

前記課題を解決するための本発明のロボットは、基体と、前記基体から延設されている肢体とを備え、前記肢体を動かすことによりタスクを実行するように構成されている前記制御対象としてのロボットであって、前記制御システムを備えていることを特徴とする。

本発明のロボットによれば、その効率的な動作を表現する目標軌道にしたがって動作が制御されうる。

本発明の一実施形態としてのロボットの構成説明図。 ロボットの制御システムの構成説明図。 制御システムの機能説明図。 モデル空間における目標軌道の生成方法に関する説明図。 モデル空間における目標軌道の実施例に関する説明図。 目標軌道にしたがったロボットの動作態様に関する説明図。 目標軌道にしたがったロボットの動作態様に関する説明図。 ロボットの動作に関する説明図。

本発明の制御システムおよびその制御対象の実施形態について図面を用いて説明する。

（ロボットの構成）
まず、本発明の制御対象の一実施形態としてのロボットの構成について説明する。

図１に示されているロボットＲは脚式移動ロボットであり、人間と同様に、基体Ｂ０と、基体Ｂ０の上方に配置された頭部Ｂ１と、基体Ｂ０の上部に上部両側から延設された左右の腕体Ｂ２と、左右の腕体Ｂ２のそれぞれの先端に設けられているハンドＨと、基体Ｂ０の下部から下方に延設された左右の脚体Ｂ４とを備えている。

基体Ｂ０はヨー軸回りに相対的に回動しうるように上下に連結された上部および下部により構成されている。頭部Ｂ１は基体Ｂ０に対してヨー軸回りに回動する等、動くことができる。

腕体Ｂ２は第１腕体リンクＢ２２と、第２腕体リンクＢ２４とを備えている。基体Ｂ０と第１腕体リンクＢ２１とは肩関節機構（第１腕関節機構）Ｂ２１を介して連結され、第１腕体リンクＢ２２と第２腕体リンクＢ２４とは肘関節機構（第２腕関節機構）Ｂ２３を介して連結され、第２腕体リンクＢ２４とハンドＨとは手首関節機構（第３腕関節機構）Ｂ２５を介して連結されている。肩関節機構Ｂ２１はロール、ピッチおよびヨー軸回りの回動自由度を有し、肘関節機構Ｂ２３はロールおよびピッチ軸回りの回動自由度を有し、手首関節機構Ｂ２５はロール、ピッチ、ヨー軸回りの回動自由度を有している。

脚体Ｂ４は第１脚体リンクＢ４２と、第２脚体リンクＢ４４と、足部Ｂ５とを備えている。基体Ｂ０と第１脚体リンクＢ４２とは股関節機構（第１脚関節機構）Ｂ４１を介して連結され、第１脚体リンクＢ４２と第２脚体リンクＢ４４とは膝関節機構（第２脚関節機構）Ｂ４３を介して連結され、第２脚体リンクＢ４４と足部Ｂ５とは足関節機構（第３脚関節機構）Ｂ４５を介して連結されている。

股関節機構Ｂ４１はロール、ピッチおよびロール軸回りの回動自由度を有し、膝関節機構Ｂ４３はピッチ軸回りの回動自由度を有し、足関節機構Ｂ４５はロールおよびピッチ軸回りの回動自由度を有している。股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５は「脚関節機構群」を構成する。なお、脚関節機構群に含まれる各関節機構の並進および回転自由度は適宜変更されてもよい。また、股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５のうち任意の１つの関節機構が省略された上で、残りの２つの関節機構の組み合わせにより脚関節機構群が構成されていてもよい。さらに、脚体Ｂ４が膝関節とは別の第２脚関節機構を有する場合、当該第２脚関節機構が含まれるように脚関節機構群が構成されてもよい。足部Ｂ５の底には着床時の衝撃緩和のため、特開２００１−１２９７７４号公報に開示されているような弾性素材Ｂ５２が設けられている。

ロボットＲには、ロボットＲの世界座標系における位置および姿勢などの内部状態を測定するための複数の内部状態センサＳ₁が搭載されている。ロボットＲの各関節機構の屈曲角度（関節角度）に応じた信号を出力するエンコーダ（図示略）、基体Ｂ０の姿勢（方位角および仰角により特定される。）に応じた信号を出力する傾斜センサ、および、足部Ｂ５および着床および離床の別を判定するための圧力センサなどが内部状態センサＳ₁に該当する。ロボットＲの周囲の様子を撮像し、当該撮像座標に基づいて世界座標系に固定されている標識の位置を認識することにより、世界座標系におけるロボットＲの位置を認識するための撮像装置が内部状態センサＳ₁に該当する。

たとえば、頭部Ｂ１に搭載され、ロボットＲの前方を撮像範囲とするＣＣＤカメラ、赤外線カメラ等、種々の周波数帯域における光を感知しうる左右一対の頭カメラＣ１が撮像装置として採用されうる。また、基体Ｂ０の前側下部に搭載され、ロボットＲの前方下方に向けて発せられた近赤外レーザー光の物体による反射光を検知することによりこの物体の位置や方位等を測定するための腰カメラ（アクティブセンサ）Ｃ２が撮像装置として採用されうる。

ロボットＲには、その周辺における物体の位置などの外部状態を測定するための外部状態センサＳ₂が搭載されている。前記のような撮像装置が外部状態センサＳ₂に該当する。

ロボットＲは制御システム１の一部を構成するコンピュータと、前記の複数の関節機構のそれぞれを動かすための複数のアクチュエータ２とを備えている。ロボットＲの内部状態および外部状態に応じて制御システム１から出力される制御指令にしたがってアクチュエータ２のそれぞれの動作が制御されることにより、ロボットＲがさまざまな態様で適応的に行動することができる。

なお、ロボットＲ（図１参照）のほか、車両など、部分的または全体的に目標軌道にしたがって変位するように動作が制御されるあらゆる装置が制御対象とされてもよい。
（制御システムの構成）
図２に示されている制御システム１は、ロボットＲに搭載されているコンピュータにより構成されている。制御システム１は、後述する演算処理を実行するように構成されている第１演算処理要素１１および第２演算処理要素１２を備えている。

第１演算処理要素１１および第２演算処理要素１２のそれぞれは、ハードウェア資源としてのプロセッサおよびメモリにより構成されている。第１演算処理要素１１および第２演算処理要素１２は、共通のハードウェア資源により構成されていてもよく、少なくとも部分的に異なるハードウェア資源により構成されていてもよい。

演算処理要素が演算処理を実行するように構成されているとは、当該演算処理要素を構成するプロセッサがメモリから必要なソフトウェアを読み取り、当該ソフトウェアを実行することにより演算処理を実行するようにプログラムされていることを意味する。

（ロボットの機能）
前記構成のロボットＲおよび制御システム１の機能について説明する。ここで、たとえば、図８に示されているようにロボットＲが腕体Ｂ２を動かすことにより、ハンドＨおよびコップが、テーブルに置かれている他のコップと接触しないようにハンドＨで把持しているコップを動かし、このテーブルに置くというタスクを実行する際の動作が制御される場合について説明する。

第１演算処理要素１１により、クォータニオンにより定義されるモデル空間における始点位置および終点位置が認識される（図３／ＳＴＥＰ０２）。

始点位置は、たとえば、ロボットＲが片手Ｈで把持しているコップを動かし始める時点における肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの屈曲状態を表わすクォータニオンの位置を表わしている。終点位置は、たとえば、ロボットＲが片手Ｈで把持しているコップをテーブルに置いた時点における肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの屈曲状態を表わすクォータニオンの位置を表わしている。関節機構ごとに始点位置および終点位置は原則的に異なり、別個のクォータニオンの軌道が生成される（図５（ａ）（ｂ）参照）。

肘関節機構Ｂ２３は前記のようにピッチおよびロールの２つの回転自由度を有しており、その屈曲状態は２つの直交軸回りの回転角度により表わされる。このため、肘関節機構Ｂ２３の屈曲状態は、実数成分が０である「第１種のクォータニオン」により表わされる。当該２つの直交軸回りの回転によって、肘関節機構Ｂ２３に固定された座標系から見たハンドＨの方向が、ノルムが１になるように正規化された３次元ベクトルｖ＝（ｖ_x，ｖ_y，ｖ_z）として表現される場合、第１種のクォータニオン実数成分ｖ_wおよび各虚数成分ｖ_μ（μ＝ｘ，ｙ，ｚ）は式（５０）により表現される。

v_w＝0, v_μ＝v ..(50)。

第１種のクォータニオンｑ₁₁およびｑ₁₂の球面線型補間の結果ｑは、補間比率ｔを用いて式（１００）により表現される。

q(t)＝R^-1q₁₁R（ここでR=exp(ωt/2), ω≡ln(q₁₁ ^-1q₁₂)である。） ..(100)。

肩関節機構Ｂ２１は前記のようにピッチおよびロールの３つの回転自由度を有しており、その屈曲状態は３つの直交軸回りの回転角度により表わされる。このため、肩関節機構Ｂ２１の屈曲状態は、実数成分が０に拘束されない「第２種のクォータニオン」により表わされる。当該３つの直交軸回りの回転が、１つの軸r＝（r_x, r_y, r_z）回りの角度θの回転として表現される場合、第２種のクォータニオンの実数成分U_wおよび各虚数成分U_μ（μ＝ｘ，ｙ，ｚ）は式（２００）により表わされる。

U_w＝cos(θ/2), U_μ＝r_μsin(θ/2) ..(200)。

始点位置および終点位置のそれぞれは、内部状態センサＳ₁および外部状態センサＳ₂のそれぞれの出力信号に基づき、制御システム１によって計算されてもよく、外部端末装置から無線通信によって制御システム１に対して入力されてもよい。

また、始点側ツリーおよび終点側ツリーのうち一方が選択される（図３／ＳＴＥＰ０４）。この際、後述する軌道の延長軌道生成の失敗に伴う選択ツリーの変更回数を示す指数ｉが「０」に初期設定される。

その上で、ＲＲＴアルゴリズムにしたがって任意の位置に点（ノード）ｑが生成される（図３／ＳＴＥＰ０６）。始点側ツリーは初期段階では始点そのものであり、その後、ＲＲＴアルゴリズムにしたがって始点を基準として拡がるように逐次生成される点ｑが逐次連結されることによって徐々に成長する。同様に、終点側ツリーは初期段階では終点そのものであり、その後、ＲＲＴアルゴリズムにしたがって終点を基準として拡がるように逐次生成される点ｑが逐次連結されることによって徐々に成長する。選択ツリーを対象として「延長処理」が実行される。

「延長処理」に際して、まず、選択ツリーの中から点ｑに最も近い点ｑ_nearが選択される（図３／ＳＴＥＰ０８）。

点ｑが最近点ｑ_nearから閾値εを超えて離れている場合、線型補間により当該点ｑが最近点ｑ_nearから閾値εの距離まで近づけられ、この近づけられた点ｑが今回制御点ｑ_newとして定義される（図３／ＳＴＥＰ１０）。

そして、選択ツリーの根元から最近点ｑ_nearに至る点列であって、Ｂ−スプライン曲線の基礎となっている点列に対して、今回制御点ｑ_newが末尾に追加された新たな点列を基礎としてＢ−スプライン曲線による補間処理（球面線形補間処理）が実行される。選択ツリーの根元から伸びる当該Ｂ−スプライン曲線のうち、今回制御点ｑ_newが旧点列に追加されたことによって延長された部分が「延長軌道」として計算される（図３／ＳＴＥＰ１２）。

（第１種のクォータニオンの補間処理）
回転自由度「２」を有する肘関節機構Ｂ２３の屈曲状態を表わす第１種のクォータニオン（実数成分が０の単位クォータニオン）のＢ−スプライン曲線q(t)は与えられた制御点群｛q_i｝に基づき、式（１２）にしたがって計算される。

q(t)＝R^-1q₀R,
R＝Π_i=1~nq_i(t),
q_i(t)≡exq{ω_iB_~i,k(t)/2} (i=1,2,..,n), ω_i≡log((q_i-1)^-1q_i) ..(12)。

ここで「B_~i,k(t)」は累積Ｂ−スプライン基底関数であり、正規化されたＢ−スプライン基底関数B_j,k(t)に基づき、式（０１）にしたがって表現される。

B_~i,k(t)≡Σ_j=1~nB_j,k(t) ..(01)。

正規化されたＢ−スプライン関数Ｂ_j,k(t)は、次のCox-deBoorの帰納式（０２）を満たすように定義されている。

B_i,1(t)＝1（t_i＜t＜t_i+1の場合）,0（その他の場合）,
B_i,k(t)＝{(t−t_i)/(t_i+k-1−t_i)}B_i,k-1(t)＋{(t_i+k−t)/(t_i+k−t_i+1)}B_i+1,k-1(t) ..(02)。

第１種のクォータニオンのＢ−スプライン曲線の微分は式（１４）にしたがって表現される。

dq(t)/dt＝(1/2)Σ_i=1~n(P_iq₀R+Rq₀Q_i)
Q_i≡q₁(t)q₂(t)…q_i(t)ω_i{dB_~i,k (t)/dt}q_i+1(t)…q_n-1(t)q_n(t) ..(14)。

第１種のクォータニオンにより、肘関節機構Ｂ２３のひねり（twist）または屈曲状態の時系列的な変化態様は式（１６）により表現される。

q(t+Δt)＝q(t)exq(ω(t)Δt)
ω(t)≡lim_Δt→0log(q^-1(t)q(t+Δt))/Δt
＝lim_Δt→0log(q^-1(t){q(t)+(dq(t)/dt)Δt})/Δt
＝lim_Δt→0log(1+q^-1(t)(dq(t)/dt)Δt})/Δt
＝q^-1(t)(dq(t)/dt)
＝(1/2)Σ_i=1~n(R^-1q₀ ^-1RQ_i ⁺q₀R+R^-1Q_i) ..(16)。

（第２種のクォータニオンの補間処理）
回転自由度「３」を有する肩関節機構Ｂ２１の屈曲状態を表わす第２種のクォータニオン（実数成分が０に拘束されない単位クォータニオン）のＢ−スプライン曲線q(t)は与えられた制御点群｛q_i｝に基づき、式（２２）にしたがって計算される。

q(t)＝Π_i=0~nq_i(t),
q₀(t)≡exq{ω₀B_~0,k(t)}, ω₀≡log(q₀),
q_i(t)≡exq{ω_iB_~i,k(t)}, ω_i≡log((q_i-1)^-1q_i) (i=1,2,..,n) ..(22)。

第２種のクォータニオンのＢ−スプライン曲線の微分は式（２４）にしたがって表現される。

dq(t)/dt＝Σ_i=1~nq₀(t)q₁(t)…q_i-1(t)q_i(t){dB_~i,k (t)/dt}q_i+1(t)q_i+2(t)…q_n(t) ..(24)。

第１種のクォータニオンにより、肘関節機構Ｂ２３のひねり（twist）または屈曲状態の時系列的な変化態様は式（２６）により表現される。

q(t+Δt)＝q(t)exq(ω(t)Δt)
ω(t)≡lim_Δt→0log(q^-1(t)q(t+Δt))/Δt
＝lim_Δt→0log(q^-1(t){q(t)+{dq(t)/dt}Δt})/Δt
＝lim_Δt→0log(1+q^-1(t){dq(t)/dt}Δt})/Δt
＝q^-1(t){dq(t)/dt}
＝{Π_i=1~nq_i(t)}^-1q₀ ^B _~ ^0,k(t)log(q₀){dB_~0,k(t)/dt}{Π_i=1~nq_i(t)}
＋Σ_i=1~nq_n ^-1(t)q_n-1 ^-1(t)…q_i+1 ^-1(t){dB_~i,k (t)/dt}q_i+1(t)…q_n-1(t)q_n(t) ..(26)。

前記補間処理により、たとえば、図４（ａ）に示されているように、選択ツリー（始点側ツリー）に応じて定められる前回軌道（細線参照）に続く延長軌道（太線参照）が定義される。

さらに、延長軌道が、テーブルおよびテーブルに載せられている他のコップなどの障害物を表わすクォータニオン空間における障害物領域から外れており、かつ、ロボットＲにその関節角度の許容範囲を超えた行動を強いる軌道ではないことという指定条件を満たすか否かがが判定される（図３／ＳＴＥＰ１４）。当該障害物領域は、外部状態センサＳ₂の出力信号に基づき、その位置、姿勢および形状が認識されうる。

延長軌道が指定条件を満たしている判定された場合（図３／ＳＴＥＰ１４‥ＹＥＳ）、すなわち、延長処理が成功した場合、今回制御点ｑ_newまたは当該制御点ｑ_newまでの延長軌道が選択ツリーに追加される（図３／ＳＴＥＰ１５）。さらに、前回軌道および延長軌道が今回軌道として定義され、当該今回軌道は次回の延長処理に際して、前回軌道として取り扱われる。延長処理に続いて「接続処理」が実行される。

「接続処理」に際して、まず、選択ツリーが変更される（図３／ＳＴＥＰ１６）。これにより、始点側ツリーがそれまでの選択ツリーであった場合、終点側ツリーが新たな選択ツリーとして以降の処理が実行される。同様に、終点側ツリーがそれまでの選択ツリーであった場合、始点側ツリーが新たな選択ツリーとして、選択ツリーの中から、先に生成された今回制御点ｑ_new（図３／ＳＴＥＰ１０参照）に最も近い点ｑ_nearが選択される（図３／ＳＴＥＰ１８）。

そして、始点側ツリーと終点側ツリーのそれぞれの根元から最近点ｑ_nearに至る点列であって、これまでの始点側位置軌道および終点側位置軌道の基礎となっている点列に対して、今回制御点ｑ_newが途中に追加された新たな点列を基礎としてＢ−スプライン曲線による補間処理が実行される。始点および終点を接続する当該Ｂ−スプライン曲線のうち、今回制御点ｑ_newの旧点列への追加によって生成された部分が「接続軌道」として生成される（図３／ＳＴＥＰ２０）。

接続軌道は、今回制御点ｑ_newのノード番号を「ｉ」と定めた場合、曲線上のパラメータｔの区間が［ｔ_i+1，ｔ_i+N+1］と設定されることにより求められる。これにより、図４（ｂ）に示されているように、始点から伸びる始点側軌道（一点鎖線参照）および終点から伸びる終点側軌道（二点鎖線参照）を接続する接続軌道（太線参照）が生成される。

さらに、接続軌道が指定条件を満たすか否かが判定される（図３／ＳＴＥＰ２２）。

接続軌道が指定条件を満たしていると判定された場合（図３／ＳＴＥＰ２２‥ＹＥＳ）、すなわち、接続処理が成功した場合、接続軌道ならびに当該接続軌道により接続された始点側軌道および終点側軌道が、始点から終点まで至るクォータニオンの目標軌道として生成される（図３／ＳＴＥＰ２４）。

これにより、図５（ａ）に示されているように肘関節機構Ｂ２３の屈曲状態を表わす第１種のクォータニオンの目標軌道が生成される。同様に、図５（ｂ）に示されているように肩関節機構Ｂ２１の屈曲状態を表わす第２種のクォータニオンの目標軌道が生成される。

そして、図８に示されているように、目標軌道にしたがってハンドＨが矢印付き曲線で表わされているように実空間において変位するようにロボットＲの動作が制御される。これにより、動作中に他の物体との接触を回避し、かつ、各関節角度が許容範囲から外れることを防止しながら、ハンドＨで把持しているコップをテーブルに置くというタスクをロボットＲに円滑に実行させることができる。

その一方、接続軌道が指定条件を満たしていないと判定された場合（図３／ＳＴＥＰ２２‥ＮＯ）、すなわち、接続処理が失敗した場合、延長処理が再び実行される（図３／ＳＴＥＰ０８〜ＳＴＥＰ０１４参照）。

また、延長軌道が指定条件を満たしていないと判定された場合（図３／ＳＴＥＰ１４‥ＮＯ）、すなわち、延長処理が失敗した場合、今回制御点ｑ_newまたは当該制御点ｑ_newまでの延長軌道は選択ツリーに追加されずに棄却される（図３／ＳＴＥＰ２５）。その上で、指数ｉが上限値Ｋ以下であるか否かが判定される（図３／ＳＴＥＰ２６）。

指数ｉが上限値Ｋ以下であると判定された場合（図３／ＳＴＥＰ２６‥ＹＥＳ）、選択ツリーが変更される（図３／ＳＴＥＰ２８）。この際、指数ｉが１だけ増加する。その上で、任意の点ｑの生成（図３／ＳＴＥＰ０６参照）以降の処理が当該変更後の選択ツリーを対象として実行される。

指数ｉが上限値Ｋを超えていると判定された場合（図３／ＳＴＥＰ２６‥ＮＯ）、軌道探索が失敗したと認識される（図３／ＳＴＥＰ３０）。これに応じて、制御システム１は、ロボットＲの当該軌道にしたがった動作を停止させて物体の移動（外部状態センサＳ₂を通じて認識される）などの状況変化を待つ、あるいは、脚体Ｂ４を動かしてロボットＲをその場から移動させたり、その場で基体Ｂ０をひねったりすることによりタスク実行開始条件をロボットＲの動作によって変化させる。その上で、制御システム１は前記のような軌道探索を再実行する。

（本発明の制御システムの作用効果）
前記機能を発揮する制御システム１によれば、空間補間用の基底関数を結合係数とする、モデル空間に配置された点列の線形結合により表現される線分を生成する補間処理が実行される（式（１２）（２２）、図４（ａ）（ｂ）参照）。その結果、各関節機構の屈曲状態（各軸回りの回転角度により表現される。）を表わすクォータニオンの目標軌道またはその候補が生成される（図５（ａ）（ｂ）参照）。クォータニオンにより表現されるモデル空間には特異点が無いため軌道の探索時間の短縮が図られる。また、クォータニオンにより記述されるモデル空間の補間は球面線形補間となるため、冗長性が排除された効率的な軌道が計算されうる。よって、制御対象であるロボットＲの効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮が図られる。

図６（ａ）には、ロボットＲの各関節機構の屈曲状態の変化態様が、クォータニオン、オイラー角および関節角度のそれぞれの軌道によって表現され、当該軌道にしたがって肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの動作が制御された際のハンドＨの３次元位置軌道が実線、一点鎖線および二点鎖線のそれぞれにより示されている。図６（ｂ）には、図６（ａ）に示されているハンドＨの３次元位置軌道が、３つの２次元平面に投影された結果としてのハンドＨの２次元位置軌道が示されている。

図７（ａ）には、ロボットＲの各関節機構の屈曲状態の変化態様が、クォータニオンの軌道によって表現され、当該軌道にしたがって肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの動作が制御された際のハンドＨの腕体Ｂ２の動作態様および３次元位置軌道（実線）が示されている。図７（ｂ）には、ロボットＲの各関節機構の屈曲状態の変化態様が、関節角度の軌道によって表現され、当該軌道にしたがって肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの動作が制御された際のハンドＨの腕体Ｂ２の動作態様および３次元位置軌道（一点鎖線）が示されている。

図６および図７から明らかなように、関節機構の屈曲常態の変化態様がクォータニオンの軌道によって表現された場合のハンドＨの位置軌道（図６（ａ）（ｂ）実線、図７（ａ）実線参照）は、関節機構の屈曲常態の変化態様がオイラー角または関節角度の軌道によって表現された場合のハンドＨの位置軌道（図６（ａ）（ｂ）一点鎖線または二点鎖線、図７（ｂ）一点鎖線参照）と比較して著しく短い。すなわち、本発明の手法によれば、制御対象に関節機構を効率的に動かしてタスクを実行させることができることがわかる。

（本発明の他の実施形態）
第１判定処理および第２判定処理が省略され、接続処理が終了した段階で生成された軌道が指定条件を満たすか否かが判定され、当該判定結果が肯定的であったことを要件としてこの軌道がクォータニオンの目標軌道として設定されてもよい。

屈曲状態が第１種のクォータニオンによって表現される肘関節機構Ｂ２３等の関節機構は、ロールおよびピッチ軸のそれぞれ回りの回動軸を有するように構成されていたが、２つの直交軸回りの回転角度により屈曲状態が表現される構成であれば、さまざまな構造が採用されてもよい。同様に、屈曲状態が第２種のクォータニオンによって表現される肩関節機構Ｂ２１等の関節機構は、ロール、ピッチおよびヨー軸のそれぞれ回りの回動軸を有するように構成されていたが、３つの直交軸回りの回転角度により屈曲状態が表現される構成であれば、さまざまな構造が採用されてもよい。

Ｂ−スプライン曲線（式（１２）参照）のほか、式（３２）により定義されるHermite曲線または式（４２）により定義されるBezier曲線が空間補間曲線として用いられることによって、延長処理および接続処理のそれぞれが実行されてもよい。

P(t)＝(2t³−3t²＋1)P_start＋(t³−2t²＋t)G1_start＋(−2t³＋3t²)P_end＋(t³−t²)G1_end..(32)

ここで、Ｐ_startおよびＰ_endは曲線の端点の位置ベクトルを表わしている。また、Ｇ１_startおよびＧ１_endは当該端点における曲線の接線ベクトル（１階微分）を表わしている。延長処理および接続処理に際して、制御点列Ｐ_i（i＝1,2, ..m）の各区間をHermite曲線で補間することによってHermite曲線がRRT-Connectに適用される。この際、Ｇ１_start、Ｇ１_endは隣り合う区間で１階微分の連続性が保たれるように選択されなければならない。

x(t)＝Σ_i=0~mJ_n,i(t)P_i ..(42)。

ここで、Bezier基底関数（またはBernstein基底関数）J_n,i(t)は式（４４）により定義される。

J_n,i(t)＝n!/{i!(n−i)!}tⁱ(1−t)^n-i ..(44)。

延長処理および接続処理において用いられる空間補間は、原則的に、空間補間用の基底関数を結合係合とする、モデル空間に配置された点列の線形結合により表現される線分を形成する空間補間であること（第１条件）、および、既存の制御点列に新しい制御点が付加されたとき、既存の制御点によって作られる部分曲線が変化しないこと（第２条件）という２つの条件を満たしている必要がある。

ただし、Bezier曲線は第２条件を満たさないものの、Hermite曲線と同様、経路点列が複数の区間に分割されることにより、複数のBezier曲線が接続されることで、空間補間曲線として採用されうる。

１‥制御システム、２‥アクチュエータ、１１‥第１演算処理要素、１２‥第２演算処理要素、Ｒ‥ロボット（制御対象）。

Claims (5)

1. ２または３つの直交軸のそれぞれ回りの回転角度によって屈曲状態が表現される関節機構を備えている制御対象の動作を制御するシステムであって、
   前記関節機構における各軸回りの回転角度が、クォータニオンによって表現されるモデル空間において、相互に離れて配置されている点列に基づき、空間補間用の基底関数を結合係数とする、前記点列の線形結合として表現される線分を生成する補間処理を実行するとともに、前記補間処理により生成された一の線分の基礎である前記点列に点を加えて新たな点列を定義した上で、前記新たな点列を基礎として前記補間処理を実行することにより、当該一の線分が延長線分によって延長された新たな線分を生成する延長処理を実行するように構成されている第１演算処理要素と、
   前記第１演算処理要素により延長処理が実行されることにより最近に延長された前記一の線分の基礎である前記新たな点列と、前記補間処理により生成された他の線分の基礎である他の点列とを基礎として前記補間処理を実行することにより、前記一の線分および前記他の線分が接続線分によって接続された形の線分を、前記関節機構における各軸回りの回転角度を表わすクォータニオンの目標軌道またはその候補として生成する接続処理を実行するように構成されている第２演算処理要素とを備え、
   前記第１演算処理要素および前記第２演算処理要素のそれぞれが、前記関節機構の屈曲状態が２つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０である第１種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行する一方、前記関節機構の屈曲状態が３つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０に拘束されない第２種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行するように構成されていることを特徴とする制御システム。
2. 請求項１記載の制御システムにおいて、
   前記第１演算処理要素が、前記モデル空間において、前記延長線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第１判定処理を実行するように構成され、前記第１判定処理の結果が否定的であることを要件として、前記一の線分を棄却した上で前記延長処理を実行するように構成されていることを特徴とする制御システム。
3. 請求項１または２記載の制御システムにおいて、
   前記第２演算処理要素が、前記モデル空間において、前記接続線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第２判定処理を実行するように構成され、前記第２判定処理の結果が否定的であることを要件として、前記第１演算処理要素が、前記新たな点列を再定義した上で前記延長処理を実行するように構成されていることを特徴とする制御システム。
4. 制御対象に搭載されているコンピュータを請求項１〜３のうちいずれか１つに記載の制御システムとして機能させることを特徴とする制御プログラム。
5. 基体と、前記基体から延設されている肢体とを備え、前記肢体を動かすことによりタスクを実行するように構成されている前記制御対象としてのロボットであって、請求項１〜３のうちいずれか１つに記載の制御システムを備えていることを特徴とするロボット。

Images