ロボット、制御システムおよび制御プログラム
【課題】制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮を図ることができるシステム等を提供する。
【解決手段】本発明の制御システム１によれば、空間補間用の基底関数を結合係数とする、モデル空間に配置された点列の線形結合により表現される線分を生成する補間処理が実行される。その結果、各関節機構の屈曲状態（各軸回りの回転角度により表現される。）を表わすクォータニオンの目標軌道またはその候補が生成される。
【解決手段】本発明の制御システム１によれば、空間補間用の基底関数を結合係数とする、モデル空間に配置された点列の線形結合により表現される線分を生成する補間処理が実行される。その結果、各関節機構の屈曲状態（各軸回りの回転角度により表現される。）を表わすクォータニオンの目標軌道またはその候補が生成される。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、制御システムおよびその制御対象の１つであるロボットなどに関する。
【背景技術】
【０００２】
ロボットアームなど、回転自由度を有する関節機構を備えている制御対象の動作を定める軌道を探索する手法が提案されている（特許文献１参照）。たとえば、関節機構の角度（関節角度）またはローカル座標系における制御対象の指定部分の位置または姿勢を表わすオイラー角の軌道を探索することが考えられる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１０−００５７６１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、制御対象の関節角度により軌道が表現された場合、当該制御対象が不必要に大きく動くような非効率的な軌道が生成される可能性がある。また、ローカル座標系における制御対象の指定部分の位置または姿勢を表わすオイラー角により軌道が表現された場合、当該制御対象の関節角度限界が厳密に考慮されないために探索時間が長くなる傾向がある。これは、軌道を記述する空間に、特異点、すなわち、ヤコビ行列が非正則となり、微分が定義されえない点（内挿できない）点が存在するためである。
【０００５】
そこで、本発明は、制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮を図ることができるシステム等を提供することを解決課題とする。
【課題を解決するための手段】
【０００６】
前記課題を解決するための本発明の制御システムは、２または３つの直交軸のそれぞれ回りの回転角度によって屈曲状態が表現される関節機構を備えている制御対象の動作を制御するシステムであって、前記関節機構における各軸回りの回転角度が、クォータニオンによって表現されるモデル空間において、相互に離れて配置されている点列に基づき、空間補間用の基底関数を結合係数とする、前記点列の線形結合として表現される線分を生成する補間処理を実行するとともに、前記補間処理により生成された一の線分の基礎である前記点列に点を加えて新たな点列を定義した上で、前記新たな点列を基礎として前記補間処理を実行することにより、当該一の線分が延長線分によって延長された新たな線分を生成する延長処理を実行するように構成されている第１演算処理要素と、前記第１演算処理要素により延長処理が実行されることにより最近に延長された前記一の線分の基礎である前記新たな点列と、前記補間処理により生成された他の線分の基礎である他の点列とを基礎として前記補間処理を実行することにより、前記一の線分および前記他の線分が接続線分によって接続された形の線分を、前記関節機構における各軸回りの回転角度を表わすクォータニオンの目標軌道またはその候補として生成する接続処理を実行するように構成されている第２演算処理要素とを備え、前記第１演算処理要素および前記第２演算処理要素のそれぞれが、前記関節機構の屈曲状態が２つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０である第１種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行する一方、前記関節機構の屈曲状態が３つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０に拘束されない第２種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行するように構成されていることを特徴とする。
【０００７】
本発明の制御システムによれば、空間補間用の基底関数を結合係数とする、モデル空間に配置された点列の線形結合により表現される線分を生成する補間処理が実行される。「線分」は直線および曲線のいずれであってもよい。その結果、関節機構における各軸回りの回転角度を表わすクォータニオンの目標軌道またはその候補が生成される。クォータニオンにより表現されるモデル空間には特異点が無いため軌道の探索時間の短縮が図られる。また、クォータニオンにより記述されるモデル空間の補間は球面線形補間となるため、冗長性が排除された効率的な軌道が計算されうる。よって、制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮が図られる。
【０００８】
前記第１演算処理要素が、前記モデル空間において、前記延長線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第１判定処理を実行するように構成され、前記第１判定処理の結果が否定的であることを要件として、前記一の線分を棄却した上で前記延長処理を実行するように構成されていてもよい。
【０００９】
前記第２演算処理要素が、前記モデル空間において、前記接続線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第２判定処理を実行するように構成され、前記第２判定処理の結果が否定的であることを要件として、前記第１演算処理要素が、前記新たな点列を再定義した上で前記延長処理を実行するように構成されていてもよい。
【００１０】
当該構成の制御システムによれば、障害物との接触を回避しながらも、制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮が図られる。
【００１１】
前記課題を解決するための本発明の制御プログラムは、制御対象に搭載されているコンピュータを前記制御システムとして機能させることを特徴とする。
【００１２】
前記課題を解決するための本発明のロボットは、基体と、前記基体から延設されている肢体とを備え、前記肢体を動かすことによりタスクを実行するように構成されている前記制御対象としてのロボットであって、前記制御システムを備えていることを特徴とする。
【００１３】
本発明のロボットによれば、その効率的な動作を表現する目標軌道にしたがって動作が制御されうる。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施形態としてのロボットの構成説明図。
【図２】ロボットの制御システムの構成説明図。
【図３】制御システムの機能説明図。
【図４】モデル空間における目標軌道の生成方法に関する説明図。
【図５】モデル空間における目標軌道の実施例に関する説明図。
【図６】目標軌道にしたがったロボットの動作態様に関する説明図。
【図７】目標軌道にしたがったロボットの動作態様に関する説明図。
【図８】ロボットの動作に関する説明図。
【発明を実施するための形態】
【００１５】
本発明の制御システムおよびその制御対象の実施形態について図面を用いて説明する。
【００１６】
（ロボットの構成）
まず、本発明の制御対象の一実施形態としてのロボットの構成について説明する。
【００１７】
図１に示されているロボットＲは脚式移動ロボットであり、人間と同様に、基体Ｂ０と、基体Ｂ０の上方に配置された頭部Ｂ１と、基体Ｂ０の上部に上部両側から延設された左右の腕体Ｂ２と、左右の腕体Ｂ２のそれぞれの先端に設けられているハンドＨと、基体Ｂ０の下部から下方に延設された左右の脚体Ｂ４とを備えている。
【００１８】
基体Ｂ０はヨー軸回りに相対的に回動しうるように上下に連結された上部および下部により構成されている。頭部Ｂ１は基体Ｂ０に対してヨー軸回りに回動する等、動くことができる。
【００１９】
腕体Ｂ２は第１腕体リンクＢ２２と、第２腕体リンクＢ２４とを備えている。基体Ｂ０と第１腕体リンクＢ２１とは肩関節機構（第１腕関節機構）Ｂ２１を介して連結され、第１腕体リンクＢ２２と第２腕体リンクＢ２４とは肘関節機構（第２腕関節機構）Ｂ２３を介して連結され、第２腕体リンクＢ２４とハンドＨとは手首関節機構（第３腕関節機構）Ｂ２５を介して連結されている。肩関節機構Ｂ２１はロール、ピッチおよびヨー軸回りの回動自由度を有し、肘関節機構Ｂ２３はロールおよびピッチ軸回りの回動自由度を有し、手首関節機構Ｂ２５はロール、ピッチ、ヨー軸回りの回動自由度を有している。
【００２０】
脚体Ｂ４は第１脚体リンクＢ４２と、第２脚体リンクＢ４４と、足部Ｂ５とを備えている。基体Ｂ０と第１脚体リンクＢ４２とは股関節機構（第１脚関節機構）Ｂ４１を介して連結され、第１脚体リンクＢ４２と第２脚体リンクＢ４４とは膝関節機構（第２脚関節機構）Ｂ４３を介して連結され、第２脚体リンクＢ４４と足部Ｂ５とは足関節機構（第３脚関節機構）Ｂ４５を介して連結されている。
【００２１】
股関節機構Ｂ４１はロール、ピッチおよびロール軸回りの回動自由度を有し、膝関節機構Ｂ４３はピッチ軸回りの回動自由度を有し、足関節機構Ｂ４５はロールおよびピッチ軸回りの回動自由度を有している。股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５は「脚関節機構群」を構成する。なお、脚関節機構群に含まれる各関節機構の並進および回転自由度は適宜変更されてもよい。また、股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５のうち任意の１つの関節機構が省略された上で、残りの２つの関節機構の組み合わせにより脚関節機構群が構成されていてもよい。さらに、脚体Ｂ４が膝関節とは別の第２脚関節機構を有する場合、当該第２脚関節機構が含まれるように脚関節機構群が構成されてもよい。足部Ｂ５の底には着床時の衝撃緩和のため、特開２００１−１２９７７４号公報に開示されているような弾性素材Ｂ５２が設けられている。
【００２２】
ロボットＲには、ロボットＲの世界座標系における位置および姿勢などの内部状態を測定するための複数の内部状態センサＳ1が搭載されている。ロボットＲの各関節機構の屈曲角度（関節角度）に応じた信号を出力するエンコーダ（図示略）、基体Ｂ０の姿勢（方位角および仰角により特定される。）に応じた信号を出力する傾斜センサ、および、足部Ｂ５および着床および離床の別を判定するための圧力センサなどが内部状態センサＳ1に該当する。ロボットＲの周囲の様子を撮像し、当該撮像座標に基づいて世界座標系に固定されている標識の位置を認識することにより、世界座標系におけるロボットＲの位置を認識するための撮像装置が内部状態センサＳ1に該当する。
【００２３】
たとえば、頭部Ｂ１に搭載され、ロボットＲの前方を撮像範囲とするＣＣＤカメラ、赤外線カメラ等、種々の周波数帯域における光を感知しうる左右一対の頭カメラＣ１が撮像装置として採用されうる。また、基体Ｂ０の前側下部に搭載され、ロボットＲの前方下方に向けて発せられた近赤外レーザー光の物体による反射光を検知することによりこの物体の位置や方位等を測定するための腰カメラ（アクティブセンサ）Ｃ２が撮像装置として採用されうる。
【００２４】
ロボットＲには、その周辺における物体の位置などの外部状態を測定するための外部状態センサＳ2が搭載されている。前記のような撮像装置が外部状態センサＳ2に該当する。
【００２５】
ロボットＲは制御システム１の一部を構成するコンピュータと、前記の複数の関節機構のそれぞれを動かすための複数のアクチュエータ２とを備えている。ロボットＲの内部状態および外部状態に応じて制御システム１から出力される制御指令にしたがってアクチュエータ２のそれぞれの動作が制御されることにより、ロボットＲがさまざまな態様で適応的に行動することができる。
【００２６】
なお、ロボットＲ（図１参照）のほか、車両など、部分的または全体的に目標軌道にしたがって変位するように動作が制御されるあらゆる装置が制御対象とされてもよい。
（制御システムの構成）
図２に示されている制御システム１は、ロボットＲに搭載されているコンピュータにより構成されている。制御システム１は、後述する演算処理を実行するように構成されている第１演算処理要素１１および第２演算処理要素１２を備えている。
【００２７】
第１演算処理要素１１および第２演算処理要素１２のそれぞれは、ハードウェア資源としてのプロセッサおよびメモリにより構成されている。第１演算処理要素１１および第２演算処理要素１２は、共通のハードウェア資源により構成されていてもよく、少なくとも部分的に異なるハードウェア資源により構成されていてもよい。
【００２８】
演算処理要素が演算処理を実行するように構成されているとは、当該演算処理要素を構成するプロセッサがメモリから必要なソフトウェアを読み取り、当該ソフトウェアを実行することにより演算処理を実行するようにプログラムされていることを意味する。
【００２９】
（ロボットの機能）
前記構成のロボットＲおよび制御システム１の機能について説明する。ここで、たとえば、図８に示されているようにロボットＲが腕体Ｂ２を動かすことにより、ハンドＨおよびコップが、テーブルに置かれている他のコップと接触しないようにハンドＨで把持しているコップを動かし、このテーブルに置くというタスクを実行する際の動作が制御される場合について説明する。
【００３０】
第１演算処理要素１１により、クォータニオンにより定義されるモデル空間における始点位置および終点位置が認識される（図３／ＳＴＥＰ０２）。
【００３１】
始点位置は、たとえば、ロボットＲが片手Ｈで把持しているコップを動かし始める時点における肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの屈曲状態を表わすクォータニオンの位置を表わしている。終点位置は、たとえば、ロボットＲが片手Ｈで把持しているコップをテーブルに置いた時点における肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの屈曲状態を表わすクォータニオンの位置を表わしている。関節機構ごとに始点位置および終点位置は原則的に異なり、別個のクォータニオンの軌道が生成される（図５（ａ）（ｂ）参照）。
【００３２】
肘関節機構Ｂ２３は前記のようにピッチおよびロールの２つの回転自由度を有しており、その屈曲状態は２つの直交軸回りの回転角度により表わされる。このため、肘関節機構Ｂ２３の屈曲状態は、実数成分が０である「第１種のクォータニオン」により表わされる。当該２つの直交軸回りの回転によって、肘関節機構Ｂ２３に固定された座標系から見たハンドＨの方向が、ノルムが１になるように正規化された３次元ベクトルｖ＝（ｖx，ｖy，ｖz）として表現される場合、第１種のクォータニオン実数成分ｖwおよび各虚数成分ｖμ（μ＝ｘ，ｙ，ｚ）は式（５０）により表現される。
【００３３】
vw＝0, vμ＝v ..(50)。
【００３４】
第１種のクォータニオンｑ11およびｑ12の球面線型補間の結果ｑは、補間比率ｔを用いて式（１００）により表現される。
【００３５】
q(t)＝R-1q11R（ここでR=exp(ωt/2), ω≡ln(q11-1q12)である。） ..(100)。
【００３６】
肩関節機構Ｂ２１は前記のようにピッチおよびロールの３つの回転自由度を有しており、その屈曲状態は３つの直交軸回りの回転角度により表わされる。このため、肩関節機構Ｂ２１の屈曲状態は、実数成分が０に拘束されない「第２種のクォータニオン」により表わされる。当該３つの直交軸回りの回転が、１つの軸r＝（rx, ry, rz）回りの角度θの回転として表現される場合、第２種のクォータニオンの実数成分Uwおよび各虚数成分Uμ（μ＝ｘ，ｙ，ｚ）は式（２００）により表わされる。
【００３７】
Uw＝cos(θ/2), Uμ＝rμsin(θ/2) ..(200)。
【００３８】
始点位置および終点位置のそれぞれは、内部状態センサＳ1および外部状態センサＳ2のそれぞれの出力信号に基づき、制御システム１によって計算されてもよく、外部端末装置から無線通信によって制御システム１に対して入力されてもよい。
【００３９】
また、始点側ツリーおよび終点側ツリーのうち一方が選択される（図３／ＳＴＥＰ０４）。この際、後述する軌道の延長軌道生成の失敗に伴う選択ツリーの変更回数を示す指数ｉが「０」に初期設定される。
【００４０】
その上で、ＲＲＴアルゴリズムにしたがって任意の位置に点（ノード）ｑが生成される（図３／ＳＴＥＰ０６）。始点側ツリーは初期段階では始点そのものであり、その後、ＲＲＴアルゴリズムにしたがって始点を基準として拡がるように逐次生成される点ｑが逐次連結されることによって徐々に成長する。同様に、終点側ツリーは初期段階では終点そのものであり、その後、ＲＲＴアルゴリズムにしたがって終点を基準として拡がるように逐次生成される点ｑが逐次連結されることによって徐々に成長する。選択ツリーを対象として「延長処理」が実行される。
【００４１】
「延長処理」に際して、まず、選択ツリーの中から点ｑに最も近い点ｑnearが選択される（図３／ＳＴＥＰ０８）。
【００４２】
点ｑが最近点ｑnearから閾値εを超えて離れている場合、線型補間により当該点ｑが最近点ｑnearから閾値εの距離まで近づけられ、この近づけられた点ｑが今回制御点ｑnewとして定義される（図３／ＳＴＥＰ１０）。
【００４３】
そして、選択ツリーの根元から最近点ｑnearに至る点列であって、Ｂ−スプライン曲線の基礎となっている点列に対して、今回制御点ｑnewが末尾に追加された新たな点列を基礎としてＢ−スプライン曲線による補間処理（球面線形補間処理）が実行される。選択ツリーの根元から伸びる当該Ｂ−スプライン曲線のうち、今回制御点ｑnewが旧点列に追加されたことによって延長された部分が「延長軌道」として計算される（図３／ＳＴＥＰ１２）。
【００４４】
（第１種のクォータニオンの補間処理）
回転自由度「２」を有する肘関節機構Ｂ２３の屈曲状態を表わす第１種のクォータニオン（実数成分が０の単位クォータニオン）のＢ−スプライン曲線q(t)は与えられた制御点群｛qi｝に基づき、式（１２）にしたがって計算される。
【００４５】
q(t)＝R-1q0R,
R＝Πi=1~nqi(t),
qi(t)≡exq{ωiB~i,k(t)/2} (i=1,2,..,n), ωi≡log((qi-1)-1qi) ..(12)。
【００４６】
ここで「B~i,k(t)」は累積Ｂ−スプライン基底関数であり、正規化されたＢ−スプライン基底関数Bj,k(t)に基づき、式（０１）にしたがって表現される。
【００４７】
B~i,k(t)≡Σj=1~nBj,k(t) ..(01)。
【００４８】
正規化されたＢ−スプライン関数Ｂj,k(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) ..(02)。
【００５０】
第１種のクォータニオンのＢ−スプライン曲線の微分は式（１４）にしたがって表現される。
【００５１】
dq(t)/dt＝(1/2)Σi=1~n(Piq0R+Rq0Qi)
Qi≡q1(t)q2(t)…qi(t)ωi{dB~i,k (t)/dt}qi+1(t)…qn-1(t)qn(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-1q0-1RQi+q0R+R-1Qi) ..(16)。
【００５４】
（第２種のクォータニオンの補間処理）
回転自由度「３」を有する肩関節機構Ｂ２１の屈曲状態を表わす第２種のクォータニオン（実数成分が０に拘束されない単位クォータニオン）のＢ−スプライン曲線q(t)は与えられた制御点群｛qi｝に基づき、式（２２）にしたがって計算される。
【００５５】
q(t)＝Πi=0~nqi(t),
q0(t)≡exq{ω0B~0,k(t)}, ω0≡log(q0),
qi(t)≡exq{ωiB~i,k(t)}, ωi≡log((qi-1)-1qi) (i=1,2,..,n) ..(22)。
【００５６】
第２種のクォータニオンのＢ−スプライン曲線の微分は式（２４）にしたがって表現される。
【００５７】
dq(t)/dt＝Σi=1~nq0(t)q1(t)…qi-1(t)qi(t){dB~i,k (t)/dt}qi+1(t)qi+2(t)…qn(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~nqi(t)}-1q0B~0,k(t)log(q0){dB~0,k(t)/dt}{Πi=1~nqi(t)}
＋Σi=1~nqn-1(t)qn-1-1(t)…qi+1-1(t){dB~i,k (t)/dt}qi+1(t)…qn-1(t)qn(t) ..(26)。
【００６０】
前記補間処理により、たとえば、図４（ａ）に示されているように、選択ツリー（始点側ツリー）に応じて定められる前回軌道（細線参照）に続く延長軌道（太線参照）が定義される。
【００６１】
さらに、延長軌道が、テーブルおよびテーブルに載せられている他のコップなどの障害物を表わすクォータニオン空間における障害物領域から外れており、かつ、ロボットＲにその関節角度の許容範囲を超えた行動を強いる軌道ではないことという指定条件を満たすか否かがが判定される（図３／ＳＴＥＰ１４）。当該障害物領域は、外部状態センサＳ2の出力信号に基づき、その位置、姿勢および形状が認識されうる。
【００６２】
延長軌道が指定条件を満たしている判定された場合（図３／ＳＴＥＰ１４‥ＹＥＳ）、すなわち、延長処理が成功した場合、今回制御点ｑnewまたは当該制御点ｑnewまでの延長軌道が選択ツリーに追加される（図３／ＳＴＥＰ１５）。さらに、前回軌道および延長軌道が今回軌道として定義され、当該今回軌道は次回の延長処理に際して、前回軌道として取り扱われる。延長処理に続いて「接続処理」が実行される。
【００６３】
「接続処理」に際して、まず、選択ツリーが変更される（図３／ＳＴＥＰ１６）。これにより、始点側ツリーがそれまでの選択ツリーであった場合、終点側ツリーが新たな選択ツリーとして以降の処理が実行される。同様に、終点側ツリーがそれまでの選択ツリーであった場合、始点側ツリーが新たな選択ツリーとして、選択ツリーの中から、先に生成された今回制御点ｑnew（図３／ＳＴＥＰ１０参照）に最も近い点ｑnearが選択される（図３／ＳＴＥＰ１８）。
【００６４】
そして、始点側ツリーと終点側ツリーのそれぞれの根元から最近点ｑnearに至る点列であって、これまでの始点側位置軌道および終点側位置軌道の基礎となっている点列に対して、今回制御点ｑnewが途中に追加された新たな点列を基礎としてＢ−スプライン曲線による補間処理が実行される。始点および終点を接続する当該Ｂ−スプライン曲線のうち、今回制御点ｑnewの旧点列への追加によって生成された部分が「接続軌道」として生成される（図３／ＳＴＥＰ２０）。
【００６５】
接続軌道は、今回制御点ｑnewのノード番号を「ｉ」と定めた場合、曲線上のパラメータｔの区間が［ｔi+1，ｔi+N+1］と設定されることにより求められる。これにより、図４（ｂ）に示されているように、始点から伸びる始点側軌道（一点鎖線参照）および終点から伸びる終点側軌道（二点鎖線参照）を接続する接続軌道（太線参照）が生成される。
【００６６】
さらに、接続軌道が指定条件を満たすか否かが判定される（図３／ＳＴＥＰ２２）。
【００６７】
接続軌道が指定条件を満たしていると判定された場合（図３／ＳＴＥＰ２２‥ＹＥＳ）、すなわち、接続処理が成功した場合、接続軌道ならびに当該接続軌道により接続された始点側軌道および終点側軌道が、始点から終点まで至るクォータニオンの目標軌道として生成される（図３／ＳＴＥＰ２４）。
【００６８】
これにより、図５（ａ）に示されているように肘関節機構Ｂ２３の屈曲状態を表わす第１種のクォータニオンの目標軌道が生成される。同様に、図５（ｂ）に示されているように肩関節機構Ｂ２１の屈曲状態を表わす第２種のクォータニオンの目標軌道が生成される。
【００６９】
そして、図８に示されているように、目標軌道にしたがってハンドＨが矢印付き曲線で表わされているように実空間において変位するようにロボットＲの動作が制御される。これにより、動作中に他の物体との接触を回避し、かつ、各関節角度が許容範囲から外れることを防止しながら、ハンドＨで把持しているコップをテーブルに置くというタスクをロボットＲに円滑に実行させることができる。
【００７０】
その一方、接続軌道が指定条件を満たしていないと判定された場合（図３／ＳＴＥＰ２２‥ＮＯ）、すなわち、接続処理が失敗した場合、延長処理が再び実行される（図３／ＳＴＥＰ０８〜ＳＴＥＰ０１４参照）。
【００７１】
また、延長軌道が指定条件を満たしていないと判定された場合（図３／ＳＴＥＰ１４‥ＮＯ）、すなわち、延長処理が失敗した場合、今回制御点ｑnewまたは当該制御点ｑnewまでの延長軌道は選択ツリーに追加されずに棄却される（図３／ＳＴＥＰ２５）。その上で、指数ｉが上限値Ｋ以下であるか否かが判定される（図３／ＳＴＥＰ２６）。
【００７２】
指数ｉが上限値Ｋ以下であると判定された場合（図３／ＳＴＥＰ２６‥ＹＥＳ）、選択ツリーが変更される（図３／ＳＴＥＰ２８）。この際、指数ｉが１だけ増加する。その上で、任意の点ｑの生成（図３／ＳＴＥＰ０６参照）以降の処理が当該変更後の選択ツリーを対象として実行される。
【００７３】
指数ｉが上限値Ｋを超えていると判定された場合（図３／ＳＴＥＰ２６‥ＮＯ）、軌道探索が失敗したと認識される（図３／ＳＴＥＰ３０）。これに応じて、制御システム１は、ロボットＲの当該軌道にしたがった動作を停止させて物体の移動（外部状態センサＳ2を通じて認識される）などの状況変化を待つ、あるいは、脚体Ｂ４を動かしてロボットＲをその場から移動させたり、その場で基体Ｂ０をひねったりすることによりタスク実行開始条件をロボットＲの動作によって変化させる。その上で、制御システム１は前記のような軌道探索を再実行する。
【００７４】
（本発明の制御システムの作用効果）
前記機能を発揮する制御システム１によれば、空間補間用の基底関数を結合係数とする、モデル空間に配置された点列の線形結合により表現される線分を生成する補間処理が実行される（式（１２）（２２）、図４（ａ）（ｂ）参照）。その結果、各関節機構の屈曲状態（各軸回りの回転角度により表現される。）を表わすクォータニオンの目標軌道またはその候補が生成される（図５（ａ）（ｂ）参照）。クォータニオンにより表現されるモデル空間には特異点が無いため軌道の探索時間の短縮が図られる。また、クォータニオンにより記述されるモデル空間の補間は球面線形補間となるため、冗長性が排除された効率的な軌道が計算されうる。よって、制御対象であるロボットＲの効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮が図られる。
【００７５】
図６（ａ）には、ロボットＲの各関節機構の屈曲状態の変化態様が、クォータニオン、オイラー角および関節角度のそれぞれの軌道によって表現され、当該軌道にしたがって肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの動作が制御された際のハンドＨの３次元位置軌道が実線、一点鎖線および二点鎖線のそれぞれにより示されている。図６（ｂ）には、図６（ａ）に示されているハンドＨの３次元位置軌道が、３つの２次元平面に投影された結果としてのハンドＨの２次元位置軌道が示されている。
【００７６】
図７（ａ）には、ロボットＲの各関節機構の屈曲状態の変化態様が、クォータニオンの軌道によって表現され、当該軌道にしたがって肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの動作が制御された際のハンドＨの腕体Ｂ２の動作態様および３次元位置軌道（実線）が示されている。図７（ｂ）には、ロボットＲの各関節機構の屈曲状態の変化態様が、関節角度の軌道によって表現され、当該軌道にしたがって肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの動作が制御された際のハンドＨの腕体Ｂ２の動作態様および３次元位置軌道（一点鎖線）が示されている。
【００７７】
図６および図７から明らかなように、関節機構の屈曲常態の変化態様がクォータニオンの軌道によって表現された場合のハンドＨの位置軌道（図６（ａ）（ｂ）実線、図７（ａ）実線参照）は、関節機構の屈曲常態の変化態様がオイラー角または関節角度の軌道によって表現された場合のハンドＨの位置軌道（図６（ａ）（ｂ）一点鎖線または二点鎖線、図７（ｂ）一点鎖線参照）と比較して著しく短い。すなわち、本発明の手法によれば、制御対象に関節機構を効率的に動かしてタスクを実行させることができることがわかる。
【００７８】
（本発明の他の実施形態）
第１判定処理および第２判定処理が省略され、接続処理が終了した段階で生成された軌道が指定条件を満たすか否かが判定され、当該判定結果が肯定的であったことを要件としてこの軌道がクォータニオンの目標軌道として設定されてもよい。
【００７９】
屈曲状態が第１種のクォータニオンによって表現される肘関節機構Ｂ２３等の関節機構は、ロールおよびピッチ軸のそれぞれ回りの回動軸を有するように構成されていたが、２つの直交軸回りの回転角度により屈曲状態が表現される構成であれば、さまざまな構造が採用されてもよい。同様に、屈曲状態が第２種のクォータニオンによって表現される肩関節機構Ｂ２１等の関節機構は、ロール、ピッチおよびヨー軸のそれぞれ回りの回動軸を有するように構成されていたが、３つの直交軸回りの回転角度により屈曲状態が表現される構成であれば、さまざまな構造が採用されてもよい。
【００８０】
Ｂ−スプライン曲線（式（１２）参照）のほか、式（３２）により定義されるHermite曲線または式（４２）により定義されるBezier曲線が空間補間曲線として用いられることによって、延長処理および接続処理のそれぞれが実行されてもよい。
【００８１】
P(t)＝(2t3−3t2＋1)Pstart＋(t3−2t2＋t)G1start＋(−2t3＋3t2)Pend＋(t3−t2)G1end..(32)
【００８２】
ここで、ＰstartおよびＰendは曲線の端点の位置ベクトルを表わしている。また、Ｇ１startおよびＧ１endは当該端点における曲線の接線ベクトル（１階微分）を表わしている。延長処理および接続処理に際して、制御点列Ｐi（i＝1,2, ..m）の各区間をHermite曲線で補間することによってHermite曲線がRRT-Connectに適用される。この際、Ｇ１start、Ｇ１endは隣り合う区間で１階微分の連続性が保たれるように選択されなければならない。
【００８３】
x(t)＝Σi=0~mJn,i(t)Pi ..(42)。
【００８４】
ここで、Bezier基底関数（またはBernstein基底関数）Jn,i(t)は式（４４）により定義される。
【００８５】
Jn,i(t)＝n!/{i!(n−i)!}ti(1−t)n-i ..(44)。
【００８６】
延長処理および接続処理において用いられる空間補間は、原則的に、空間補間用の基底関数を結合係合とする、モデル空間に配置された点列の線形結合により表現される線分を形成する空間補間であること（第１条件）、および、既存の制御点列に新しい制御点が付加されたとき、既存の制御点によって作られる部分曲線が変化しないこと（第２条件）という２つの条件を満たしている必要がある。
【００８７】
ただし、Bezier曲線は第２条件を満たさないものの、Hermite曲線と同様、経路点列が複数の区間に分割されることにより、複数のBezier曲線が接続されることで、空間補間曲線として採用されうる。
【符号の説明】
【００８８】
１‥制御システム、２‥アクチュエータ、１１‥第１演算処理要素、１２‥第２演算処理要素、Ｒ‥ロボット（制御対象）。
【技術分野】
【０００１】
本発明は、制御システムおよびその制御対象の１つであるロボットなどに関する。
【背景技術】
【０００２】
ロボットアームなど、回転自由度を有する関節機構を備えている制御対象の動作を定める軌道を探索する手法が提案されている（特許文献１参照）。たとえば、関節機構の角度（関節角度）またはローカル座標系における制御対象の指定部分の位置または姿勢を表わすオイラー角の軌道を探索することが考えられる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１０−００５７６１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、制御対象の関節角度により軌道が表現された場合、当該制御対象が不必要に大きく動くような非効率的な軌道が生成される可能性がある。また、ローカル座標系における制御対象の指定部分の位置または姿勢を表わすオイラー角により軌道が表現された場合、当該制御対象の関節角度限界が厳密に考慮されないために探索時間が長くなる傾向がある。これは、軌道を記述する空間に、特異点、すなわち、ヤコビ行列が非正則となり、微分が定義されえない点（内挿できない）点が存在するためである。
【０００５】
そこで、本発明は、制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮を図ることができるシステム等を提供することを解決課題とする。
【課題を解決するための手段】
【０００６】
前記課題を解決するための本発明の制御システムは、２または３つの直交軸のそれぞれ回りの回転角度によって屈曲状態が表現される関節機構を備えている制御対象の動作を制御するシステムであって、前記関節機構における各軸回りの回転角度が、クォータニオンによって表現されるモデル空間において、相互に離れて配置されている点列に基づき、空間補間用の基底関数を結合係数とする、前記点列の線形結合として表現される線分を生成する補間処理を実行するとともに、前記補間処理により生成された一の線分の基礎である前記点列に点を加えて新たな点列を定義した上で、前記新たな点列を基礎として前記補間処理を実行することにより、当該一の線分が延長線分によって延長された新たな線分を生成する延長処理を実行するように構成されている第１演算処理要素と、前記第１演算処理要素により延長処理が実行されることにより最近に延長された前記一の線分の基礎である前記新たな点列と、前記補間処理により生成された他の線分の基礎である他の点列とを基礎として前記補間処理を実行することにより、前記一の線分および前記他の線分が接続線分によって接続された形の線分を、前記関節機構における各軸回りの回転角度を表わすクォータニオンの目標軌道またはその候補として生成する接続処理を実行するように構成されている第２演算処理要素とを備え、前記第１演算処理要素および前記第２演算処理要素のそれぞれが、前記関節機構の屈曲状態が２つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０である第１種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行する一方、前記関節機構の屈曲状態が３つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０に拘束されない第２種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行するように構成されていることを特徴とする。
【０００７】
本発明の制御システムによれば、空間補間用の基底関数を結合係数とする、モデル空間に配置された点列の線形結合により表現される線分を生成する補間処理が実行される。「線分」は直線および曲線のいずれであってもよい。その結果、関節機構における各軸回りの回転角度を表わすクォータニオンの目標軌道またはその候補が生成される。クォータニオンにより表現されるモデル空間には特異点が無いため軌道の探索時間の短縮が図られる。また、クォータニオンにより記述されるモデル空間の補間は球面線形補間となるため、冗長性が排除された効率的な軌道が計算されうる。よって、制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮が図られる。
【０００８】
前記第１演算処理要素が、前記モデル空間において、前記延長線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第１判定処理を実行するように構成され、前記第１判定処理の結果が否定的であることを要件として、前記一の線分を棄却した上で前記延長処理を実行するように構成されていてもよい。
【０００９】
前記第２演算処理要素が、前記モデル空間において、前記接続線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第２判定処理を実行するように構成され、前記第２判定処理の結果が否定的であることを要件として、前記第１演算処理要素が、前記新たな点列を再定義した上で前記延長処理を実行するように構成されていてもよい。
【００１０】
当該構成の制御システムによれば、障害物との接触を回避しながらも、制御対象の効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮が図られる。
【００１１】
前記課題を解決するための本発明の制御プログラムは、制御対象に搭載されているコンピュータを前記制御システムとして機能させることを特徴とする。
【００１２】
前記課題を解決するための本発明のロボットは、基体と、前記基体から延設されている肢体とを備え、前記肢体を動かすことによりタスクを実行するように構成されている前記制御対象としてのロボットであって、前記制御システムを備えていることを特徴とする。
【００１３】
本発明のロボットによれば、その効率的な動作を表現する目標軌道にしたがって動作が制御されうる。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施形態としてのロボットの構成説明図。
【図２】ロボットの制御システムの構成説明図。
【図３】制御システムの機能説明図。
【図４】モデル空間における目標軌道の生成方法に関する説明図。
【図５】モデル空間における目標軌道の実施例に関する説明図。
【図６】目標軌道にしたがったロボットの動作態様に関する説明図。
【図７】目標軌道にしたがったロボットの動作態様に関する説明図。
【図８】ロボットの動作に関する説明図。
【発明を実施するための形態】
【００１５】
本発明の制御システムおよびその制御対象の実施形態について図面を用いて説明する。
【００１６】
（ロボットの構成）
まず、本発明の制御対象の一実施形態としてのロボットの構成について説明する。
【００１７】
図１に示されているロボットＲは脚式移動ロボットであり、人間と同様に、基体Ｂ０と、基体Ｂ０の上方に配置された頭部Ｂ１と、基体Ｂ０の上部に上部両側から延設された左右の腕体Ｂ２と、左右の腕体Ｂ２のそれぞれの先端に設けられているハンドＨと、基体Ｂ０の下部から下方に延設された左右の脚体Ｂ４とを備えている。
【００１８】
基体Ｂ０はヨー軸回りに相対的に回動しうるように上下に連結された上部および下部により構成されている。頭部Ｂ１は基体Ｂ０に対してヨー軸回りに回動する等、動くことができる。
【００１９】
腕体Ｂ２は第１腕体リンクＢ２２と、第２腕体リンクＢ２４とを備えている。基体Ｂ０と第１腕体リンクＢ２１とは肩関節機構（第１腕関節機構）Ｂ２１を介して連結され、第１腕体リンクＢ２２と第２腕体リンクＢ２４とは肘関節機構（第２腕関節機構）Ｂ２３を介して連結され、第２腕体リンクＢ２４とハンドＨとは手首関節機構（第３腕関節機構）Ｂ２５を介して連結されている。肩関節機構Ｂ２１はロール、ピッチおよびヨー軸回りの回動自由度を有し、肘関節機構Ｂ２３はロールおよびピッチ軸回りの回動自由度を有し、手首関節機構Ｂ２５はロール、ピッチ、ヨー軸回りの回動自由度を有している。
【００２０】
脚体Ｂ４は第１脚体リンクＢ４２と、第２脚体リンクＢ４４と、足部Ｂ５とを備えている。基体Ｂ０と第１脚体リンクＢ４２とは股関節機構（第１脚関節機構）Ｂ４１を介して連結され、第１脚体リンクＢ４２と第２脚体リンクＢ４４とは膝関節機構（第２脚関節機構）Ｂ４３を介して連結され、第２脚体リンクＢ４４と足部Ｂ５とは足関節機構（第３脚関節機構）Ｂ４５を介して連結されている。
【００２１】
股関節機構Ｂ４１はロール、ピッチおよびロール軸回りの回動自由度を有し、膝関節機構Ｂ４３はピッチ軸回りの回動自由度を有し、足関節機構Ｂ４５はロールおよびピッチ軸回りの回動自由度を有している。股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５は「脚関節機構群」を構成する。なお、脚関節機構群に含まれる各関節機構の並進および回転自由度は適宜変更されてもよい。また、股関節機構Ｂ４１、膝関節機構Ｂ４３および足関節機構Ｂ４５のうち任意の１つの関節機構が省略された上で、残りの２つの関節機構の組み合わせにより脚関節機構群が構成されていてもよい。さらに、脚体Ｂ４が膝関節とは別の第２脚関節機構を有する場合、当該第２脚関節機構が含まれるように脚関節機構群が構成されてもよい。足部Ｂ５の底には着床時の衝撃緩和のため、特開２００１−１２９７７４号公報に開示されているような弾性素材Ｂ５２が設けられている。
【００２２】
ロボットＲには、ロボットＲの世界座標系における位置および姿勢などの内部状態を測定するための複数の内部状態センサＳ1が搭載されている。ロボットＲの各関節機構の屈曲角度（関節角度）に応じた信号を出力するエンコーダ（図示略）、基体Ｂ０の姿勢（方位角および仰角により特定される。）に応じた信号を出力する傾斜センサ、および、足部Ｂ５および着床および離床の別を判定するための圧力センサなどが内部状態センサＳ1に該当する。ロボットＲの周囲の様子を撮像し、当該撮像座標に基づいて世界座標系に固定されている標識の位置を認識することにより、世界座標系におけるロボットＲの位置を認識するための撮像装置が内部状態センサＳ1に該当する。
【００２３】
たとえば、頭部Ｂ１に搭載され、ロボットＲの前方を撮像範囲とするＣＣＤカメラ、赤外線カメラ等、種々の周波数帯域における光を感知しうる左右一対の頭カメラＣ１が撮像装置として採用されうる。また、基体Ｂ０の前側下部に搭載され、ロボットＲの前方下方に向けて発せられた近赤外レーザー光の物体による反射光を検知することによりこの物体の位置や方位等を測定するための腰カメラ（アクティブセンサ）Ｃ２が撮像装置として採用されうる。
【００２４】
ロボットＲには、その周辺における物体の位置などの外部状態を測定するための外部状態センサＳ2が搭載されている。前記のような撮像装置が外部状態センサＳ2に該当する。
【００２５】
ロボットＲは制御システム１の一部を構成するコンピュータと、前記の複数の関節機構のそれぞれを動かすための複数のアクチュエータ２とを備えている。ロボットＲの内部状態および外部状態に応じて制御システム１から出力される制御指令にしたがってアクチュエータ２のそれぞれの動作が制御されることにより、ロボットＲがさまざまな態様で適応的に行動することができる。
【００２６】
なお、ロボットＲ（図１参照）のほか、車両など、部分的または全体的に目標軌道にしたがって変位するように動作が制御されるあらゆる装置が制御対象とされてもよい。
（制御システムの構成）
図２に示されている制御システム１は、ロボットＲに搭載されているコンピュータにより構成されている。制御システム１は、後述する演算処理を実行するように構成されている第１演算処理要素１１および第２演算処理要素１２を備えている。
【００２７】
第１演算処理要素１１および第２演算処理要素１２のそれぞれは、ハードウェア資源としてのプロセッサおよびメモリにより構成されている。第１演算処理要素１１および第２演算処理要素１２は、共通のハードウェア資源により構成されていてもよく、少なくとも部分的に異なるハードウェア資源により構成されていてもよい。
【００２８】
演算処理要素が演算処理を実行するように構成されているとは、当該演算処理要素を構成するプロセッサがメモリから必要なソフトウェアを読み取り、当該ソフトウェアを実行することにより演算処理を実行するようにプログラムされていることを意味する。
【００２９】
（ロボットの機能）
前記構成のロボットＲおよび制御システム１の機能について説明する。ここで、たとえば、図８に示されているようにロボットＲが腕体Ｂ２を動かすことにより、ハンドＨおよびコップが、テーブルに置かれている他のコップと接触しないようにハンドＨで把持しているコップを動かし、このテーブルに置くというタスクを実行する際の動作が制御される場合について説明する。
【００３０】
第１演算処理要素１１により、クォータニオンにより定義されるモデル空間における始点位置および終点位置が認識される（図３／ＳＴＥＰ０２）。
【００３１】
始点位置は、たとえば、ロボットＲが片手Ｈで把持しているコップを動かし始める時点における肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの屈曲状態を表わすクォータニオンの位置を表わしている。終点位置は、たとえば、ロボットＲが片手Ｈで把持しているコップをテーブルに置いた時点における肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの屈曲状態を表わすクォータニオンの位置を表わしている。関節機構ごとに始点位置および終点位置は原則的に異なり、別個のクォータニオンの軌道が生成される（図５（ａ）（ｂ）参照）。
【００３２】
肘関節機構Ｂ２３は前記のようにピッチおよびロールの２つの回転自由度を有しており、その屈曲状態は２つの直交軸回りの回転角度により表わされる。このため、肘関節機構Ｂ２３の屈曲状態は、実数成分が０である「第１種のクォータニオン」により表わされる。当該２つの直交軸回りの回転によって、肘関節機構Ｂ２３に固定された座標系から見たハンドＨの方向が、ノルムが１になるように正規化された３次元ベクトルｖ＝（ｖx，ｖy，ｖz）として表現される場合、第１種のクォータニオン実数成分ｖwおよび各虚数成分ｖμ（μ＝ｘ，ｙ，ｚ）は式（５０）により表現される。
【００３３】
vw＝0, vμ＝v ..(50)。
【００３４】
第１種のクォータニオンｑ11およびｑ12の球面線型補間の結果ｑは、補間比率ｔを用いて式（１００）により表現される。
【００３５】
q(t)＝R-1q11R（ここでR=exp(ωt/2), ω≡ln(q11-1q12)である。） ..(100)。
【００３６】
肩関節機構Ｂ２１は前記のようにピッチおよびロールの３つの回転自由度を有しており、その屈曲状態は３つの直交軸回りの回転角度により表わされる。このため、肩関節機構Ｂ２１の屈曲状態は、実数成分が０に拘束されない「第２種のクォータニオン」により表わされる。当該３つの直交軸回りの回転が、１つの軸r＝（rx, ry, rz）回りの角度θの回転として表現される場合、第２種のクォータニオンの実数成分Uwおよび各虚数成分Uμ（μ＝ｘ，ｙ，ｚ）は式（２００）により表わされる。
【００３７】
Uw＝cos(θ/2), Uμ＝rμsin(θ/2) ..(200)。
【００３８】
始点位置および終点位置のそれぞれは、内部状態センサＳ1および外部状態センサＳ2のそれぞれの出力信号に基づき、制御システム１によって計算されてもよく、外部端末装置から無線通信によって制御システム１に対して入力されてもよい。
【００３９】
また、始点側ツリーおよび終点側ツリーのうち一方が選択される（図３／ＳＴＥＰ０４）。この際、後述する軌道の延長軌道生成の失敗に伴う選択ツリーの変更回数を示す指数ｉが「０」に初期設定される。
【００４０】
その上で、ＲＲＴアルゴリズムにしたがって任意の位置に点（ノード）ｑが生成される（図３／ＳＴＥＰ０６）。始点側ツリーは初期段階では始点そのものであり、その後、ＲＲＴアルゴリズムにしたがって始点を基準として拡がるように逐次生成される点ｑが逐次連結されることによって徐々に成長する。同様に、終点側ツリーは初期段階では終点そのものであり、その後、ＲＲＴアルゴリズムにしたがって終点を基準として拡がるように逐次生成される点ｑが逐次連結されることによって徐々に成長する。選択ツリーを対象として「延長処理」が実行される。
【００４１】
「延長処理」に際して、まず、選択ツリーの中から点ｑに最も近い点ｑnearが選択される（図３／ＳＴＥＰ０８）。
【００４２】
点ｑが最近点ｑnearから閾値εを超えて離れている場合、線型補間により当該点ｑが最近点ｑnearから閾値εの距離まで近づけられ、この近づけられた点ｑが今回制御点ｑnewとして定義される（図３／ＳＴＥＰ１０）。
【００４３】
そして、選択ツリーの根元から最近点ｑnearに至る点列であって、Ｂ−スプライン曲線の基礎となっている点列に対して、今回制御点ｑnewが末尾に追加された新たな点列を基礎としてＢ−スプライン曲線による補間処理（球面線形補間処理）が実行される。選択ツリーの根元から伸びる当該Ｂ−スプライン曲線のうち、今回制御点ｑnewが旧点列に追加されたことによって延長された部分が「延長軌道」として計算される（図３／ＳＴＥＰ１２）。
【００４４】
（第１種のクォータニオンの補間処理）
回転自由度「２」を有する肘関節機構Ｂ２３の屈曲状態を表わす第１種のクォータニオン（実数成分が０の単位クォータニオン）のＢ−スプライン曲線q(t)は与えられた制御点群｛qi｝に基づき、式（１２）にしたがって計算される。
【００４５】
q(t)＝R-1q0R,
R＝Πi=1~nqi(t),
qi(t)≡exq{ωiB~i,k(t)/2} (i=1,2,..,n), ωi≡log((qi-1)-1qi) ..(12)。
【００４６】
ここで「B~i,k(t)」は累積Ｂ−スプライン基底関数であり、正規化されたＢ−スプライン基底関数Bj,k(t)に基づき、式（０１）にしたがって表現される。
【００４７】
B~i,k(t)≡Σj=1~nBj,k(t) ..(01)。
【００４８】
正規化されたＢ−スプライン関数Ｂj,k(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) ..(02)。
【００５０】
第１種のクォータニオンのＢ−スプライン曲線の微分は式（１４）にしたがって表現される。
【００５１】
dq(t)/dt＝(1/2)Σi=1~n(Piq0R+Rq0Qi)
Qi≡q1(t)q2(t)…qi(t)ωi{dB~i,k (t)/dt}qi+1(t)…qn-1(t)qn(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-1q0-1RQi+q0R+R-1Qi) ..(16)。
【００５４】
（第２種のクォータニオンの補間処理）
回転自由度「３」を有する肩関節機構Ｂ２１の屈曲状態を表わす第２種のクォータニオン（実数成分が０に拘束されない単位クォータニオン）のＢ−スプライン曲線q(t)は与えられた制御点群｛qi｝に基づき、式（２２）にしたがって計算される。
【００５５】
q(t)＝Πi=0~nqi(t),
q0(t)≡exq{ω0B~0,k(t)}, ω0≡log(q0),
qi(t)≡exq{ωiB~i,k(t)}, ωi≡log((qi-1)-1qi) (i=1,2,..,n) ..(22)。
【００５６】
第２種のクォータニオンのＢ−スプライン曲線の微分は式（２４）にしたがって表現される。
【００５７】
dq(t)/dt＝Σi=1~nq0(t)q1(t)…qi-1(t)qi(t){dB~i,k (t)/dt}qi+1(t)qi+2(t)…qn(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~nqi(t)}-1q0B~0,k(t)log(q0){dB~0,k(t)/dt}{Πi=1~nqi(t)}
＋Σi=1~nqn-1(t)qn-1-1(t)…qi+1-1(t){dB~i,k (t)/dt}qi+1(t)…qn-1(t)qn(t) ..(26)。
【００６０】
前記補間処理により、たとえば、図４（ａ）に示されているように、選択ツリー（始点側ツリー）に応じて定められる前回軌道（細線参照）に続く延長軌道（太線参照）が定義される。
【００６１】
さらに、延長軌道が、テーブルおよびテーブルに載せられている他のコップなどの障害物を表わすクォータニオン空間における障害物領域から外れており、かつ、ロボットＲにその関節角度の許容範囲を超えた行動を強いる軌道ではないことという指定条件を満たすか否かがが判定される（図３／ＳＴＥＰ１４）。当該障害物領域は、外部状態センサＳ2の出力信号に基づき、その位置、姿勢および形状が認識されうる。
【００６２】
延長軌道が指定条件を満たしている判定された場合（図３／ＳＴＥＰ１４‥ＹＥＳ）、すなわち、延長処理が成功した場合、今回制御点ｑnewまたは当該制御点ｑnewまでの延長軌道が選択ツリーに追加される（図３／ＳＴＥＰ１５）。さらに、前回軌道および延長軌道が今回軌道として定義され、当該今回軌道は次回の延長処理に際して、前回軌道として取り扱われる。延長処理に続いて「接続処理」が実行される。
【００６３】
「接続処理」に際して、まず、選択ツリーが変更される（図３／ＳＴＥＰ１６）。これにより、始点側ツリーがそれまでの選択ツリーであった場合、終点側ツリーが新たな選択ツリーとして以降の処理が実行される。同様に、終点側ツリーがそれまでの選択ツリーであった場合、始点側ツリーが新たな選択ツリーとして、選択ツリーの中から、先に生成された今回制御点ｑnew（図３／ＳＴＥＰ１０参照）に最も近い点ｑnearが選択される（図３／ＳＴＥＰ１８）。
【００６４】
そして、始点側ツリーと終点側ツリーのそれぞれの根元から最近点ｑnearに至る点列であって、これまでの始点側位置軌道および終点側位置軌道の基礎となっている点列に対して、今回制御点ｑnewが途中に追加された新たな点列を基礎としてＢ−スプライン曲線による補間処理が実行される。始点および終点を接続する当該Ｂ−スプライン曲線のうち、今回制御点ｑnewの旧点列への追加によって生成された部分が「接続軌道」として生成される（図３／ＳＴＥＰ２０）。
【００６５】
接続軌道は、今回制御点ｑnewのノード番号を「ｉ」と定めた場合、曲線上のパラメータｔの区間が［ｔi+1，ｔi+N+1］と設定されることにより求められる。これにより、図４（ｂ）に示されているように、始点から伸びる始点側軌道（一点鎖線参照）および終点から伸びる終点側軌道（二点鎖線参照）を接続する接続軌道（太線参照）が生成される。
【００６６】
さらに、接続軌道が指定条件を満たすか否かが判定される（図３／ＳＴＥＰ２２）。
【００６７】
接続軌道が指定条件を満たしていると判定された場合（図３／ＳＴＥＰ２２‥ＹＥＳ）、すなわち、接続処理が成功した場合、接続軌道ならびに当該接続軌道により接続された始点側軌道および終点側軌道が、始点から終点まで至るクォータニオンの目標軌道として生成される（図３／ＳＴＥＰ２４）。
【００６８】
これにより、図５（ａ）に示されているように肘関節機構Ｂ２３の屈曲状態を表わす第１種のクォータニオンの目標軌道が生成される。同様に、図５（ｂ）に示されているように肩関節機構Ｂ２１の屈曲状態を表わす第２種のクォータニオンの目標軌道が生成される。
【００６９】
そして、図８に示されているように、目標軌道にしたがってハンドＨが矢印付き曲線で表わされているように実空間において変位するようにロボットＲの動作が制御される。これにより、動作中に他の物体との接触を回避し、かつ、各関節角度が許容範囲から外れることを防止しながら、ハンドＨで把持しているコップをテーブルに置くというタスクをロボットＲに円滑に実行させることができる。
【００７０】
その一方、接続軌道が指定条件を満たしていないと判定された場合（図３／ＳＴＥＰ２２‥ＮＯ）、すなわち、接続処理が失敗した場合、延長処理が再び実行される（図３／ＳＴＥＰ０８〜ＳＴＥＰ０１４参照）。
【００７１】
また、延長軌道が指定条件を満たしていないと判定された場合（図３／ＳＴＥＰ１４‥ＮＯ）、すなわち、延長処理が失敗した場合、今回制御点ｑnewまたは当該制御点ｑnewまでの延長軌道は選択ツリーに追加されずに棄却される（図３／ＳＴＥＰ２５）。その上で、指数ｉが上限値Ｋ以下であるか否かが判定される（図３／ＳＴＥＰ２６）。
【００７２】
指数ｉが上限値Ｋ以下であると判定された場合（図３／ＳＴＥＰ２６‥ＹＥＳ）、選択ツリーが変更される（図３／ＳＴＥＰ２８）。この際、指数ｉが１だけ増加する。その上で、任意の点ｑの生成（図３／ＳＴＥＰ０６参照）以降の処理が当該変更後の選択ツリーを対象として実行される。
【００７３】
指数ｉが上限値Ｋを超えていると判定された場合（図３／ＳＴＥＰ２６‥ＮＯ）、軌道探索が失敗したと認識される（図３／ＳＴＥＰ３０）。これに応じて、制御システム１は、ロボットＲの当該軌道にしたがった動作を停止させて物体の移動（外部状態センサＳ2を通じて認識される）などの状況変化を待つ、あるいは、脚体Ｂ４を動かしてロボットＲをその場から移動させたり、その場で基体Ｂ０をひねったりすることによりタスク実行開始条件をロボットＲの動作によって変化させる。その上で、制御システム１は前記のような軌道探索を再実行する。
【００７４】
（本発明の制御システムの作用効果）
前記機能を発揮する制御システム１によれば、空間補間用の基底関数を結合係数とする、モデル空間に配置された点列の線形結合により表現される線分を生成する補間処理が実行される（式（１２）（２２）、図４（ａ）（ｂ）参照）。その結果、各関節機構の屈曲状態（各軸回りの回転角度により表現される。）を表わすクォータニオンの目標軌道またはその候補が生成される（図５（ａ）（ｂ）参照）。クォータニオンにより表現されるモデル空間には特異点が無いため軌道の探索時間の短縮が図られる。また、クォータニオンにより記述されるモデル空間の補間は球面線形補間となるため、冗長性が排除された効率的な軌道が計算されうる。よって、制御対象であるロボットＲの効率的な動作を表現する軌道の確実な探索と、当該軌道の探索時間の短縮が図られる。
【００７５】
図６（ａ）には、ロボットＲの各関節機構の屈曲状態の変化態様が、クォータニオン、オイラー角および関節角度のそれぞれの軌道によって表現され、当該軌道にしたがって肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの動作が制御された際のハンドＨの３次元位置軌道が実線、一点鎖線および二点鎖線のそれぞれにより示されている。図６（ｂ）には、図６（ａ）に示されているハンドＨの３次元位置軌道が、３つの２次元平面に投影された結果としてのハンドＨの２次元位置軌道が示されている。
【００７６】
図７（ａ）には、ロボットＲの各関節機構の屈曲状態の変化態様が、クォータニオンの軌道によって表現され、当該軌道にしたがって肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの動作が制御された際のハンドＨの腕体Ｂ２の動作態様および３次元位置軌道（実線）が示されている。図７（ｂ）には、ロボットＲの各関節機構の屈曲状態の変化態様が、関節角度の軌道によって表現され、当該軌道にしたがって肩関節機構Ｂ２１および肘関節機構Ｂ２３のそれぞれの動作が制御された際のハンドＨの腕体Ｂ２の動作態様および３次元位置軌道（一点鎖線）が示されている。
【００７７】
図６および図７から明らかなように、関節機構の屈曲常態の変化態様がクォータニオンの軌道によって表現された場合のハンドＨの位置軌道（図６（ａ）（ｂ）実線、図７（ａ）実線参照）は、関節機構の屈曲常態の変化態様がオイラー角または関節角度の軌道によって表現された場合のハンドＨの位置軌道（図６（ａ）（ｂ）一点鎖線または二点鎖線、図７（ｂ）一点鎖線参照）と比較して著しく短い。すなわち、本発明の手法によれば、制御対象に関節機構を効率的に動かしてタスクを実行させることができることがわかる。
【００７８】
（本発明の他の実施形態）
第１判定処理および第２判定処理が省略され、接続処理が終了した段階で生成された軌道が指定条件を満たすか否かが判定され、当該判定結果が肯定的であったことを要件としてこの軌道がクォータニオンの目標軌道として設定されてもよい。
【００７９】
屈曲状態が第１種のクォータニオンによって表現される肘関節機構Ｂ２３等の関節機構は、ロールおよびピッチ軸のそれぞれ回りの回動軸を有するように構成されていたが、２つの直交軸回りの回転角度により屈曲状態が表現される構成であれば、さまざまな構造が採用されてもよい。同様に、屈曲状態が第２種のクォータニオンによって表現される肩関節機構Ｂ２１等の関節機構は、ロール、ピッチおよびヨー軸のそれぞれ回りの回動軸を有するように構成されていたが、３つの直交軸回りの回転角度により屈曲状態が表現される構成であれば、さまざまな構造が採用されてもよい。
【００８０】
Ｂ−スプライン曲線（式（１２）参照）のほか、式（３２）により定義されるHermite曲線または式（４２）により定義されるBezier曲線が空間補間曲線として用いられることによって、延長処理および接続処理のそれぞれが実行されてもよい。
【００８１】
P(t)＝(2t3−3t2＋1)Pstart＋(t3−2t2＋t)G1start＋(−2t3＋3t2)Pend＋(t3−t2)G1end..(32)
【００８２】
ここで、ＰstartおよびＰendは曲線の端点の位置ベクトルを表わしている。また、Ｇ１startおよびＧ１endは当該端点における曲線の接線ベクトル（１階微分）を表わしている。延長処理および接続処理に際して、制御点列Ｐi（i＝1,2, ..m）の各区間をHermite曲線で補間することによってHermite曲線がRRT-Connectに適用される。この際、Ｇ１start、Ｇ１endは隣り合う区間で１階微分の連続性が保たれるように選択されなければならない。
【００８３】
x(t)＝Σi=0~mJn,i(t)Pi ..(42)。
【００８４】
ここで、Bezier基底関数（またはBernstein基底関数）Jn,i(t)は式（４４）により定義される。
【００８５】
Jn,i(t)＝n!/{i!(n−i)!}ti(1−t)n-i ..(44)。
【００８６】
延長処理および接続処理において用いられる空間補間は、原則的に、空間補間用の基底関数を結合係合とする、モデル空間に配置された点列の線形結合により表現される線分を形成する空間補間であること（第１条件）、および、既存の制御点列に新しい制御点が付加されたとき、既存の制御点によって作られる部分曲線が変化しないこと（第２条件）という２つの条件を満たしている必要がある。
【００８７】
ただし、Bezier曲線は第２条件を満たさないものの、Hermite曲線と同様、経路点列が複数の区間に分割されることにより、複数のBezier曲線が接続されることで、空間補間曲線として採用されうる。
【符号の説明】
【００８８】
１‥制御システム、２‥アクチュエータ、１１‥第１演算処理要素、１２‥第２演算処理要素、Ｒ‥ロボット（制御対象）。
【特許請求の範囲】
【請求項１】
２または３つの直交軸のそれぞれ回りの回転角度によって屈曲状態が表現される関節機構を備えている制御対象の動作を制御するシステムであって、
前記関節機構における各軸回りの回転角度が、クォータニオンによって表現されるモデル空間において、相互に離れて配置されている点列に基づき、空間補間用の基底関数を結合係数とする、前記点列の線形結合として表現される線分を生成する補間処理を実行するとともに、前記補間処理により生成された一の線分の基礎である前記点列に点を加えて新たな点列を定義した上で、前記新たな点列を基礎として前記補間処理を実行することにより、当該一の線分が延長線分によって延長された新たな線分を生成する延長処理を実行するように構成されている第１演算処理要素と、
前記第１演算処理要素により延長処理が実行されることにより最近に延長された前記一の線分の基礎である前記新たな点列と、前記補間処理により生成された他の線分の基礎である他の点列とを基礎として前記補間処理を実行することにより、前記一の線分および前記他の線分が接続線分によって接続された形の線分を、前記関節機構における各軸回りの回転角度を表わすクォータニオンの目標軌道またはその候補として生成する接続処理を実行するように構成されている第２演算処理要素とを備え、
前記第１演算処理要素および前記第２演算処理要素のそれぞれが、前記関節機構の屈曲状態が２つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０である第１種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行する一方、前記関節機構の屈曲状態が３つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０に拘束されない第２種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行するように構成されていることを特徴とする制御システム。
【請求項２】
請求項１記載の制御システムにおいて、
前記第１演算処理要素が、前記モデル空間において、前記延長線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第１判定処理を実行するように構成され、前記第１判定処理の結果が否定的であることを要件として、前記一の線分を棄却した上で前記延長処理を実行するように構成されていることを特徴とする制御システム。
【請求項３】
請求項１または２記載の制御システムにおいて、
前記第２演算処理要素が、前記モデル空間において、前記接続線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第２判定処理を実行するように構成され、前記第２判定処理の結果が否定的であることを要件として、前記第１演算処理要素が、前記新たな点列を再定義した上で前記延長処理を実行するように構成されていることを特徴とする制御システム。
【請求項４】
制御対象に搭載されているコンピュータを請求項１〜３のうちいずれか１つに記載の制御システムとして機能させることを特徴とする制御プログラム。
【請求項５】
基体と、前記基体から延設されている肢体とを備え、前記肢体を動かすことによりタスクを実行するように構成されている前記制御対象としてのロボットであって、請求項１〜３のうちいずれか１つに記載の制御システムを備えていることを特徴とするロボット。
【請求項１】
２または３つの直交軸のそれぞれ回りの回転角度によって屈曲状態が表現される関節機構を備えている制御対象の動作を制御するシステムであって、
前記関節機構における各軸回りの回転角度が、クォータニオンによって表現されるモデル空間において、相互に離れて配置されている点列に基づき、空間補間用の基底関数を結合係数とする、前記点列の線形結合として表現される線分を生成する補間処理を実行するとともに、前記補間処理により生成された一の線分の基礎である前記点列に点を加えて新たな点列を定義した上で、前記新たな点列を基礎として前記補間処理を実行することにより、当該一の線分が延長線分によって延長された新たな線分を生成する延長処理を実行するように構成されている第１演算処理要素と、
前記第１演算処理要素により延長処理が実行されることにより最近に延長された前記一の線分の基礎である前記新たな点列と、前記補間処理により生成された他の線分の基礎である他の点列とを基礎として前記補間処理を実行することにより、前記一の線分および前記他の線分が接続線分によって接続された形の線分を、前記関節機構における各軸回りの回転角度を表わすクォータニオンの目標軌道またはその候補として生成する接続処理を実行するように構成されている第２演算処理要素とを備え、
前記第１演算処理要素および前記第２演算処理要素のそれぞれが、前記関節機構の屈曲状態が２つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０である第１種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行する一方、前記関節機構の屈曲状態が３つの直交軸のそれぞれ回りの回転角度によって表現される場合、実数成分が０に拘束されない第２種のクォータニオンを表わす点列に基づいて前記前記補間処理を実行するように構成されていることを特徴とする制御システム。
【請求項２】
請求項１記載の制御システムにおいて、
前記第１演算処理要素が、前記モデル空間において、前記延長線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第１判定処理を実行するように構成され、前記第１判定処理の結果が否定的であることを要件として、前記一の線分を棄却した上で前記延長処理を実行するように構成されていることを特徴とする制御システム。
【請求項３】
請求項１または２記載の制御システムにおいて、
前記第２演算処理要素が、前記モデル空間において、前記接続線分が実空間における物体を表わす障害物領域から外れていることを含む指定条件を満たしているか否かを判定する第２判定処理を実行するように構成され、前記第２判定処理の結果が否定的であることを要件として、前記第１演算処理要素が、前記新たな点列を再定義した上で前記延長処理を実行するように構成されていることを特徴とする制御システム。
【請求項４】
制御対象に搭載されているコンピュータを請求項１〜３のうちいずれか１つに記載の制御システムとして機能させることを特徴とする制御プログラム。
【請求項５】
基体と、前記基体から延設されている肢体とを備え、前記肢体を動かすことによりタスクを実行するように構成されている前記制御対象としてのロボットであって、請求項１〜３のうちいずれか１つに記載の制御システムを備えていることを特徴とするロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１１−２３０２１３（Ｐ２０１１−２３０２１３Ａ）
【公開日】平成２３年１１月１７日（２０１１．１１．１７）
【国際特許分類】
【出願番号】特願２０１０−１０１４９２（Ｐ２０１０−１０１４９２）
【出願日】平成２２年４月２６日（２０１０．４．２６）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
【公開日】平成２３年１１月１７日（２０１１．１１．１７）
【国際特許分類】
【出願日】平成２２年４月２６日（２０１０．４．２６）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
[ Back to top ]