移動制御装置及び移動制御方法、並びにコンピューター・プログラム
【課題】マニピュレーターのリーチング動作や移動ロボットの移動など、自動機械の移動を好適に制御する。
【解決手段】自動機械の現在位置及び最終目標位置をパラメトリック曲線の端末とみなし、これら２点を結ぶパラメトリック曲線を内分比ｒ：１−ｒで内分する内分点を、仮想バネ・ダンパ系の平衡点、すなわち仮目標位置として算出する。そして、この仮目標位置を２次のフィルターで鈍らせ、目標位置ｘdの生成を行なう。複雑な経路への追従制御が実現されるとともに、ベースとなるオンライン目標位置整形の特徴である対外乱適応能力を両立した移動制御器を構成できる。
【解決手段】自動機械の現在位置及び最終目標位置をパラメトリック曲線の端末とみなし、これら２点を結ぶパラメトリック曲線を内分比ｒ：１−ｒで内分する内分点を、仮想バネ・ダンパ系の平衡点、すなわち仮目標位置として算出する。そして、この仮目標位置を２次のフィルターで鈍らせ、目標位置ｘdの生成を行なう。複雑な経路への追従制御が実現されるとともに、ベースとなるオンライン目標位置整形の特徴である対外乱適応能力を両立した移動制御器を構成できる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、マニピュレーターのリーチング動作や移動ロボットの移動など、自動機械の移動を制御する移動制御装置及び移動制御方法、並びにコンピューター・プログラムに係り、特に、複雑な経路の移動能力と対外乱適応能力を備えた自動機械の移動制御装置及び移動制御方法、並びにコンピューター・プログラムに関する。
【背景技術】
【０００２】
人間が活動する環境は、狭空間からなり、また、時間の経過とともに変化する動的環境である。このような環境で、マニピュレーターのリーチング動作、あるいは車輪型又はその他の移動ロボットの移動など、各種の自動機械を活動させようとすると、人や物（以下では、まとめて「障害物」とする）との接触が起こり得る。このため、自動機械を動作させ、さまざまなタスクを実現するためには、複雑な経路を移動する能力と、安全性やタスク遂行のために障害物との接触などの外乱への適応能力が要求される。
【０００３】
一般にマニピュレーターのリーチング動作や移動ロボットの移動といった、自動機械の「移動」においては、与えられた目標位置及び目標到達時刻から目標軌道を生成する移動制御手法が多く用いられている。言い換えれば、自動機械の経路は、目標時刻毎の目標位置を求める、時間を引数とする関数で表現される。上述したように、自動機械を人間の生活環境へ適用する場合、さまざまな障害物との接触が想定される。従来の移動制御手法によると、障害物との接触により目標到達時刻に目標位置に到達しなくなると、制御系が破綻してしまう。すなわち、時刻を陽に含む制御系では、外乱への柔軟性に欠け、十分な安全性を確保することは難しい。
【０００４】
制御系に時刻を陽に含むことなく目標位置への到達を実現する手法として、「仮想バネ・ダンパ仮説」が挙げられる（例えば、非特許文献１を参照のこと）。仮想バネ・ダンパ仮説は、自動機械の現在位置と目標位置間を仮想的なバネ・ダンパ系で接続し、バネによって発生する引力で目標位置に引き付けるというモデルで移動制御を行なうものである。例えば、仮想的なバネ・ダンパ系によって発生する引力に基づいてマニピュレーターの手先を制御する。外乱による振動は、ダンパによって抑制される。仮想バネ・ダンパ仮説は、自動機械の移動経路を時間関数で表現するものではなく、したがって、外乱に柔軟に対応でき、障害物との接触が生じても目標位置に到達させることができる。
【０００５】
仮想バネ・ダンパ仮説によると、動作開始時のバネの復元力が最大、すなわち、動作開始時に手先等の制御対象点の加速度が最大となるため、滑らか且つ安全な動作を実現することは難しく、ハードウェアへの負担も大きい。この問題を解消するべく、時刻に依存した関数を用いることで、仮想バネ係数を変化させる時変スティフネスを導入した手先のリーチング手法が提案されている（例えば、非特許文献２を参照のこと）。しかしながら、この手法は、時刻を陽に含まないことで構成されていた自律系の特性が失われてしまうため、さまざまな障害物との接触が想定される動的環境下での適用は困難である。
【０００６】
さらに、仮想バネ・ダンパ仮説を基に、その仮想バネ・ダンパ系に与える目標位置をオンラインで整形する手法が提案されている（例えば、非特許文献３を参照のこと）。このオンライン目標位置整形では、移動の開始位置から最終的な目標位置までの間に平衡点を設け、最終的な目標位置よりも現在位置から近い平衡点を仮想的な目標位置として仮想バネ・ダンパ仮説を適用することで、速度プロファイルの設計が可能となり、自動機械の動き出しを滑らかにすることができる。平衡点は、現在位置から最終的な目標位置までの残りの距離を基に決定される。オンライン目標位置整形による手法は、軌道を陽に設計することなく、且つ、制御系に時刻を陽に含むことなく、滑らかなリーチング動作を生成するものである。オンラインで目標位置整形を行なう場合、例えばマニピュレーターの状態を手先位置としてフィードバックすることで外力が加えられる際にはそれに柔軟に倣い、外力が解消した後には速度の急変動を起こすことなく、目標位置へのリーチング動作を再開可能な特性を持つ。
【０００７】
しかしながら、仮想バネ係数を変化させる時変スティフネスを導入した手法、並びに、仮想バネ・ダンパ系に与える目標位置をオンラインで整形する手法はいずれも、あくまでも始点（現在位置）と終点（目標位置）の２点間の移動を制御する手法に過ぎない。すなわち、より複雑な動きを要するタスク、障害物を避けながら複雑な移動経路を要する移動に適用するのは難しい。
【０００８】
従来の軌道計画に基づく手法においても（例えば、非特許文献４を参照のこと）、マニピュレーターの手先にコンプライアンス特性を持たせることで外乱への倣いが可能になる。しかしながら、この従来手法は、制御系が時刻を陽に含むため、長期に渡り外乱が発生するような場合には、十分な倣い性能を得ることができない。また、外乱に柔軟に対応する特性を重視して、マニピュレーターの手先に持たせるコンプライアンスを大きくすると、目標とする位置への収束性能が著しく低下してしまう。
【先行技術文献】
【非特許文献】
【０００９】
【非特許文献１】Ｓ．Ａｒｉｍｏｔｏ ａｎｄ Ｍ．Ｓｅｋｉｍｏｔｏ“Ｈｕｍａｎ−Ｌｉｋｅ Ｍｏｖｅｍｅｎｔｓ ｏｆ Ｒｏｂｏｔｉｃ Ａｒｍｓ ｗｉｔｈ Ｒｅｄｕｎｄａｎｔ ＤＯＦｓ：Ｖｉｒｔｕａｌ Ｓｐｒｉｎｇ−Ｄｕｍｐｅｒ Ｈｙｐｏｔｈｅｓｉｓ ｔｏ Ｔａｃｋｌｅ ｔｈｅ Ｂｅｒｎｓｔｅｉｎ Ｐｒｏｂｌｅｍ”（Ｐｒｏｃ．Ｏｆ ＩＥＥＥ ｉｎｔ．Ｃｏｎｆ．ｏｎ ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ｐｐ．１８６０−１８６６，２００６）
【非特許文献２】Ｓ． Ａｒｉｍｏｔｏ ｅｔ ａｌ“ Ｎａｔｕｒａｌ Ｒｅｓｏｌｕｔｉｏｎ ｏｆ Ｉｌｌ−ｐｏｓｅｄｎｅｓｓ ｏｆ Ｉｎｖｅｒｓｅ Ｋｉｎｅｍａｔｉｃｓ ｆｏｒ Ｒｅｄｕｎｄａｎｔ Ｒｏｂｏｔｓ：Ａ Ｃｈａｌｌｅｎｇｅ ｔｏ Ｂｅｒｎｓｔｅｉｎ’ｓ Ｄｅｇｒｅｅｓ−ｏｆ−Ｆｒｅｅｄｏｍ Ｐｒｏｂｌｅｍ”（Ａｄｖａｎｃｅｄ Ｒｏｂｏｔｉｃｓ，Ｖｏｌ．１９，Ｎｏ．４，ｐｐ．４０１−４３４，２００５）
【非特許文献３】Ｆｕｍｉ Ｓｅｔｏ ａｎｄ Ｔｏｍｏｍｉｃｈｉ Ｓｕｇｉｈａｒａ,“Ｏｎｌｉｎｅ Ｒｅｆｅｒｅｎｃｅ Ｓｈａｐｉｎｇ ｗｉｔｈ Ｅｎｄ−ｐｏｉｎｔ Ｐｏｓｉｔｉｏｎ Ｆｅｅｄｂａｃｋ ｆｏｒ Ｌａｒｇｅ Ａｃｃｅｌｅｒａｔｉｏｎ Ａｖｏｉｄａｎｃｅ ｏｎ Ｍａｎｉｐｕｌａｔｏｒ Ｃｏｎｔｒｏｌ，２００９ ＩＥＥＥ／ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ，ｐｐ．５７４３−５７４８，Ｓｔ．Ｌｏｕｉｓ，Ｏｃｔ，１４ｔｈ，２００９）
【発明の概要】
【発明が解決しようとする課題】
【００１０】
本発明の目的は、マニピュレーターのリーチング動作や移動ロボットの移動など、自動機械の移動を好適に制御することができる、優れた移動制御装置及び移動制御方法、並びにコンピューター・プログラムを提供することにある。
【００１１】
本発明のさらなる目的は、人間が活動する狭空間からなる環境で自動機械が活動すべく、複雑な経路の移動能力と対外乱適応能力を備えた、優れた移動制御装置及び移動制御方法、並びにコンピューター・プログラムを提供することにある。
【課題を解決するための手段】
【００１２】
本願は、上記課題を参酌してなされたものであり、請求項１に記載の発明は、
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画部と、
前記経路ノード列から前記現在位置に最も近いノードを探索し、前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成し、前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する移動制御部と、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御部と、
を具備する、自動機械の移動制御装置である。
【００１３】
本願の請求項２に記載の発明によれば、請求項１に記載の移動制御装置の経路計画部は、ＲＲＴで発見した軌道を初期条件とし、さらに軌道上の各通過点を質点とした多質点系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避のうち少なくとも１つの要求を満たす経路を計画して、経路ノード列を出力するように構成されている。
【００１４】
本願の請求項３に記載の発明によれば、請求項１に記載の移動制御装置の経路計画部は、ＲＲＴで発見した軌道を初期条件とし、軌道上の各通過点に適当な初期姿勢を割り当てた剛体を配置した多剛体系力学モデルを構築し、多剛体系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避のうち少なくとも１つの要求を満たす経路を計画して、経路ノード列を出力するように構成されている。
【００１５】
本願の請求項４に記載の発明によれば、請求項１に記載の移動制御装置の移動制御部は、前記の生成したコントロール・ポイント列の近傍を通過するＮＵＲＢＳ曲線を生成し、前記ＮＵＲＢＳ曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定するように構成されている。
【００１６】
本願の請求項５に記載の発明によれば、請求項１に記載の移動制御装置の駆動制御部は、自動機械の身体の一部に発生する力、位置、速度、加速度、外界との接触拘束、自動機械を構成する各関節の発生力、関節値、関節速度、関節加速度、可動範囲拘束を同時に満たす、自動機械の関節駆動方法を決定して、各関節を駆動するアクチュエーターへの指令値を出力するように構成されている。
【００１７】
また、本願の請求項６に記載の発明は、
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画ステップと、
前記経路ノード列から前記現在位置に最も近いノードを探索する最近傍点探索ステップと、
前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成するコントロール・ポイント列生成ステップと、
前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点を生成する平衡点生成ステップと、
前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する仮想バネ・ダンパ発生力決定ステップと、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御ステップと、
を有する、自動機械の移動制御方法である。
【００１８】
また、本願の請求項７に記載の発明は、
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画部、
前記経路ノード列から前記現在位置に最も近いノードを探索し、前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成し、前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する移動制御部、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御部、
としてコンピューターを機能させ、自動機械の移動制御を行なうための処理を前記コンピューター上で実行させるようにコンピューター可読形式で記述されたコンピューター・プログラムである。
【００１９】
本願の請求項７に係るコンピューター・プログラムは、コンピューター上で所定の処理を実現するようにコンピューター可読形式で記述されたコンピューター・プログラムを定義したものである。換言すれば、本願の請求項７に係るコンピューター・プログラムをコンピューターにインストールすることによって、コンピューター上では協働的作用が発揮され、本願の請求項１に係る移動制御装置と同様の作用効果を得ることができる。
【発明の効果】
【００２０】
本発明によれば、マニピュレーターのリーチング動作や移動ロボットの移動など、自動機械の移動を好適に制御することができる、優れた移動制御装置及び移動制御方法、並びにコンピューター・プログラムを提供することができる。
【００２１】
本発明によれば、仮想バネ・ダンパ系に与える目標位置をオンラインで整形し、軌道を陽に設計することなく、且つ、制御系に時間を陽に含むことなく移動制御を実現する手法に、パラメトリック曲線であるＮＵＲＢＳ曲線を導入することで、複雑な経路の移動能力と対外乱適応能力を両立した移動制御を実現することができる。
【００２２】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【図面の簡単な説明】
【００２３】
【図１】図１は、仮想バネ・ダンパ系及びオンライン目標得位置整形を含んだ移動制御系の構成図である。
【図２】図２は、自動機械の制御システムの構成例を示した図である。
【図３】図３は、図２に示した制御システム１００で実行される処理手順を示したフローチャートである。
【図４】図４は、図２に示した制御システム１００を適用した２次元移動ロボットの移動制御における経路変更の様子を示した図である。
【図５】図５は、ＮＵＲＢＳ曲線上の自動機械の現在位置と最終目標位置の間を内分比ｒ：１−ｒで内分する仮目標位置を示した図である。
【発明を実施するための形態】
【００２４】
以下、図面を参照しながら本発明の実施形態について詳細に説明する。
【００２５】
仮想バネ・ダンパ仮説を基に、その仮想バネ・ダンパ系に与える目標位置をオンラインで整形する手法は、軌道を陽に設計することなく、且つ、制御系に時刻を陽に含むことなく、滑らかなリーチング動作を生成することができるが、２点間の移動を制御する手法であり、より複雑な動きを要するタスク、障害物を避けながら複雑な移動経路を要する移動に適用するのは難しい（前述）。これに対し、本発明者らは、時刻を陽に含まないオンライン位置整形により滑らかな移動を実現させるが、目標位置整形に制御対象の状態を位置としてフィードバックすることで、目標位置への収束性能を落とすことなく、外乱への適応能力を実現する移動制御方法について、以下に説明する。この移動制御方法によれば、移動速度の急変動を防止することができる。また、目標位置整形にパラメトリック曲線であるＮＵＲＢＳ曲線を用いることで、複雑な経路への追従も実現することができる。
【００２６】
本発明者らが提案する移動制御方法は、仮想ダンパ仮説及びオンライン目標位置整形を好適に取り入れることで実現される。これらの手法についてはそれぞれ非特許文献１、３に記載されているが、本明細書中でも説明しておく。
【００２７】
仮想バネ・ダンパ仮説
マニピュレーターの関節数をｍ、作業空間の自由度をｌ（通常は、２又は３次元の自由度）、仮想バネ・ダンパ系の剛性係数行列及び粘性係数行列をそれぞれＫp、ξ∈Ｒl×lとおくと、仮想バネ・ダンパ仮説によれば、マニピュレーターの現在手先位置ｘ∈Ｒlに対して、下式（１）で表わされる力Ｆd∈Ｒlが働く（現在手先位置ｘの１次時間微分は、マニピュレーターの手先の速度である）。∈
【００２８】
【数１】
【００２９】
上式（１）において、ｘd∈Ｒlは、仮想バネ・ダンパ系の平衡点として与える（仮想の）目標位置である。要するに、Ｆdは、マニピュレーターの手先を仮想目標位置ｘに引き付ける引力である。
【００３０】
上式（１）を関節空間に展開すると、下式（２）のように表わされる。但し、θ∈Ｒmはマニピュレーターの現在関節角度であり（θの１次時間微分は角速度）、Ｃ∈Ｒm×mは関節の粘性係数行列、Ｊ（θ）∈Ｒl×mはマニピュレーターの現在手先位置ｘまでのヤコブ行列を表すものとする。
【００３１】
【数２】
【００３２】
オンライン目標位置整形
従来の仮想バネ・ダンパ仮説では（例えば、非特許文献１を参照のこと）、仮想バネ・ダンパ系の平衡点として与える目標位置ｘdを、最終目標位置ｘg∈Ｒlに固定する。また、その他の従来方法では、仮想バネ・ダンパ系の平衡点として与える目標位置ｘdを時刻ｔの関数として設計することが多い。また、オンライン目標位置整形では、移動の開始位置から最終的な目標位置までの間に平衡点を設け、最終的な目標位置よりも現在位置から近い平衡点を仮想的な目標位置として仮想バネ・ダンパ仮説を適用することで、自動機械の動き出しを滑らかにする。
【００３３】
オンライン目標位置整形では、仮想バネ・ダンパ系の平衡点として与える目標位置ｘdを、最終目標位置ｘgと現在位置ｘに応じて下式（３）により実時間で整形する。但し、ＫI、ＫI1、ＫI2は非負の定数係数である。
【００３４】
【数３】
【００３５】
ここで、を下式（４）のように置き（但し、０≦ｒ≦１）、入力を最終目標位置ｘg及び現在位置ｘ、出力を仮想バネ・ダンパ系の平衡点として与える目標位置ｘdとすると、伝達関数は下式（５）のように表わされる。
【００３６】
【数４】
【００３７】
【数５】
【００３８】
上式（５）は、最終目標位置ｘgを、現在の位置ｘから最終目標位置ｘgまでの経路を内分比ｒ：１−ｒで内分する内分点で置き換え、さらにその内分点の動きを２次のフィルターで鈍らせた値を、仮想バネ・ダンパ系の平衡点ｘdとして与えることを意味する。このように目標位置を実時間で整形することで、制御対象となる自動機械の動作開始時の加速度及び速度が０になり、滑らかな動き出しが実現される。
【００３９】
図１には、仮想バネ・ダンパ系及びオンライン目標位置整形を含んだ移動制御系の構成図を示している。仮想バネ・ダンパ仮説では、制御対象となる自動機械の運動が、障害物との接触などにより発生した外力により阻害されると、接触した障害物に過大な力を発生させることや、外力が解消された際に急加速が発生することが問題となる。これに対し、オンライン目標位置整形により、単純な２次遅れ系ではなく、現在の位置ｘがフィードバックされると、平衡点ｘdは最終目標位置ｘgと現在位置ｘとのそのときの内分点へと収束するようにされる。したがって、障害物との積極などにより発生する外力によって現在位置ｘの動きが阻害された場合であっても、平衡点ｘdは現在位置ｘから大きく離れることがなく、過度の接触力が発生したり急加速したりすることはない。また、図１からも分かるように、オンライン目標位置整形は時間を陽に含まないので、外乱が長期にわたって発生した場合であっても、移動制御がその影響を受けることはない。外乱が除去された後には、目標位置へと緩やかに収束させることができる。
【００４０】
複雑経路の追従性能の付加（拡張目標位置整形）
非特許文献３で開示されるオンライン目標位置整形は、始点（現在位置）と終点（目標位置）の２点間、すなわち直線的な移動を制御する手法であり、より複雑な動きを要するタスク、障害物を避けながら複雑な移動経路を要する移動に適用するのは難しい（前述）。
【００４１】
例えば、人間の住環境には机や椅子などのさまざまな障害物が存在するので、このような環境内で移動ロボットが活動するには、２点間の移動だけでは障害物との接触などを回避するのは難しく、タスク遂行能力に問題が生じる。つまり、複雑なタスクを遂行するには、複雑な経路に追従可能な目標位置整形器が必須となる。
【００４２】
２次元又は３次元の作業空間上で移動ロボットなどの移動経路を計画する経路計画方法として、ポテンシャル法（例えば、太田順外著「知能ロボット入門 −動作計画問題の解法−」（コロナ社、２００１）を参照のこと）や、ＲＲＴ（例えば、Ｓｔｅｖｅｎ Ｍ．ＬａＶａｌｌｅ，“Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ：Ａ Ｎｅｗ Ｔｏｏｌ ｆｏｒ Ｐａｔｈ Ｐｌａｎｎｉｎｇ”（ＴＲ．９８−１１，Ｃｏｍｐｕｔｅｒ Ｓｃｉｅｎｃｅ Ｄｐｔ．，Ｉｏｗａ Ｓｔａｔｅ Ｕｎｉｖｅｒｓｉｔｙ．Ｏｃｔ．１９９８）を参照のこと）などが知られているが、これらの多くは、経路計画の結果を通過点列で表現する。また、本出願人に既に譲渡されている特開２００９−２１１５７１号公報に開示されている軌道計画装置によれば、ＲＲＴで発見した軌道を初期条件とし、さらに軌道上の各通過点を質点とした多質点系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避といった要求を満たす軌道を生成することができる。さらに、本出願人に既に譲渡されている特開２０１０−１５５３２８号公報に開示されている軌道計画装置によれば、ＲＲＴで発見した軌道を初期条件とし、軌道上の各通過点に適当な初期姿勢を割り当てた剛体を配置した多剛体系力学モデルを構築し、多剛体系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避といった要求を満たす軌道を生成することができる。
【００４３】
ここで、上式（５）の右辺の一部である下式（６）について着目する。
【００４４】
【数６】
【００４５】
上式（６）は、１つのパラメーターｒに対して座標点を対応付けることにより表され、一般的なパラメーター曲線と等価である。そこで、本発明者らは、上式（６）に、代表的なパラメトリック曲線の１つであるＮＵＲＢＳ（Ｎｏｎ−ＵｎｉｆｏｒｍＲａｔｉｏｎａｌ Ｂ−Ｓｐｌｉｎｅｓ：非均一有理Ｂスプライン）曲線を適用することにする。すなわち、現在位置及び最終目標位置をパラメトリック曲線の端末とみなし、これら２点を結ぶパラメトリック曲線を内分比ｒ：１−ｒで内分する内分点を、仮想バネ・ダンパ系の平衡点、すなわち仮目標位置として算出する。そして、この仮目標位置を２次のフィルター（前述）で鈍らせて、仮目標位置ｘdの生成を行なう。これにより、複雑な経路への追従制御が実現されるとともに、ベースとなるオンライン目標位置整形の特徴である対外乱適応能力を両立した移動制御器を構成することができる。
【００４６】
例えば、三浦曜、望月一正共著「ＣＡＤ・ＣＧ技術者のための実践ＮＵＲＢＳ」（工業調査会、２００１）には、ＮＵＲＢＳ曲線について詳細に記述されている。以下では、ＮＵＲＢＳ曲線を移動制御に適用するための留意点を中心に説明する。
【００４７】
ＮＵＲＢＳ曲線は、ＮＵＲＢＳ近似とＮＵＲＢＳ補間に大別される（周知）。ＮＵＲＢＳ近似は、曲線がその点列の近傍を通過し、ＮＵＲＢＳ補間は、曲線が点列を通過するものである。曲線の特性及び計算効率の点から、本発明者らは、移動制御にはＮＵＲＢＳ近似を用いることにした。十分な粒度の通過点列であれば、ＮＵＲＢＳ近似により、動作上問題ない近似経路を得ることができることをここで述べておく。すなわち、移動制御系への入力は、軌道計画法によって得られた通過点列であり、この通過点列をＮＵＲＢＳ曲線のコントロール・ポイント列とみなす。そして、コントロール・ポイントの近傍を通過するＮＵＲＢＳ曲線を生成し、そのＮＵＲＢＳ曲線上における始点である現在位置と最終目標位置を内分比ｒ：１−ｒで内分する内分点を仮目標位置として算出することになる。勿論、ＮＵＲＢＳ近似ではなくＮＵＲＢＳ補間を用いた場合も、ほぼ同等の機能を実現できるが、計算量は増加する点に留意されたい。
【００４８】
ＮＵＲＢＳ曲線は、同次座標空間において定義されたＢスプライン曲線を中心投影によって通常座標空間上に射影することで得られる。コントロール・ポイントの混ぜ合わせが同次座標空間で行なわれるため、各コントロール・ポイントｑi（軌道計画法によって得られる通過点列に相当）には重みωiが与えられる。
【００４９】
同次座標空間におけるＢスプライン曲線は、同次座標のコントロール・ポイント列Ｑi（ωiｘi，ωiｙi，ωiｚi，ωi）をＢスプライン基底関数で混ぜ合わせることにより、下式（７）に示すように、Ｐω（ｔ）として得られる。
【００５０】
【数７】
【００５１】
上記の同次座標空間におけるＢスプライン曲線Ｐw（ｔ）を中心投影すると、曲線Ｐ*（ｔ）は、下式（８）に示す通りとなる。
【００５２】
【数８】
【００５３】
上記の曲線Ｐ*（ｔ）は、同次座標空間におけるω＝１の面上に投影された曲線を表す。これを通常座標系で表現すると、下式（９）に示す通りとなり、これがＮＵＲＢＳ曲線の式となる。
【００５４】
【数９】
【００５５】
但し、上式（９）において、ｑi（ｘi，ｙi，ｚi）は通常座標系におけるコントロール・ポイントで、Ｑi＝（ωiｑi，ωi）なる関係がある。また、ノット（ｋｎｏｔ：節点）・ベクトルをＴ＝［ｔ0，…，ｔm+n+1］とし、ｍ、ｎはそれぞれ曲線の階数及び通過点列数（コントロール・ポイント数）を表す。また、Ｎi,m（ｔ）は、Ｂスプライン規定関数を表し、その計算方法は、計算機上での実装に適した方法として知られるｄｅ Ｂｏｏｒ Ｃｏｘの漸化式を用いる。
【００５６】
参考までに、ｄｅ Ｂｏｏｒ Ｃｏｘの漸化式を以下に示しておく。但し、本発明の要旨は、Ｂスプライン規定関数の計算方法が下式（１０）に限定される訳ではなく、数多の計算方法の中から適宜選択すればよいことを十分理解されたい。
【００５７】
【数１０】
【００５８】
一般に、ノット・ベクトルは、単調増加且つｍ＋ｎ個のノットで構成されていることを満足していれば、基本的には自由に設定することができる（但し、ｍは曲線の階数、ｎはコントロール・ポイント数）。本明細書では、本実施形態における移動制御で用いるノット・ベクトルの作成方法の一例について言及しておく。ノット・ベクトルは、曲線の値域を表すパラメーターであり、ノット・ベクトルの生成により、移動速度プロファイルに特徴を出すことも可能である。例えば，曲率の大きい箇所において、ノット・ベクトル間隔を大きくとることで、移動速度の変化を抑える効果を課すことも可能である。
【００５９】
一般に、ｉ番目の通過点に対して曲線パラメーターを設定する場合、パラメーターの間隔が通過点間の区間長の比を反映するようにするのが望ましいとされている。本実施形態においてもこれに倣い、区間長の比を反映した曲線パラメーターの設定を行なうが、いま区間長として曲線長を当てることができないので、通過点間の距離若しくはその平方根の比で決定する。本明細書では、通過点間の距離を用いた曲線パラメーターの設定方法について記載する。
【００６０】
曲線全体のパラメーターの変位器を０≦ｔ≦１とすると、ｉ番目の通過点に対する曲線パラメーターは、下式（１１）のように決定することができる。
【００６１】
【数１１】
【００６２】
次に、（１１）に示した曲線パラメーターを用いて、下式（１２）に示すように、ノット・ベクトル列を生成する。但し、端末ノットは曲線階数と同じだけ多重に定義し、端末コントロール・ポイントを必ず通過するＮＵＲＢＳ曲線を生成する。
【００６３】
【数１２】
【００６４】
上式（９）で表わされるＮＵＲＢＳ曲線Ｐ（ｔ）の引数ｔとして内分比ｒを代入することで、ＮＵＲＢＳ曲線上の自動機械の現在位置と最終目標位置の間を内分比ｒ：１−ｒで内分する仮目標位置を算出することができる（図５を参照のこと）。
【００６５】
なお、当業界で周知のように、ＮＵＲＢＳ曲線以外にもパラメトリック曲線はある。しかしながら、Ｂｅｚｉｅｒ曲線やＦｅｒｇｕｓｏｎ曲線を含む多項式曲線では、複数のセグメントをつなぎ合わせて１つの曲線を生成することから、セグメント内での連続性が保証されないという欠点がある。これに対し、ＮＵＲＢＳ曲線は、曲線全体を通してＣm-2級連続性が保証される。つまり、コントロール・ポイントの配置に関係なく、各ノットに対応する位置の前後でｍ−２階までの微分係数が連続となる。また、ＮＵＲＢＳ曲線は、特定の制御点を移動した場合に、影響を受ける範囲はその制御点の近傍に限定され、離れた部分の曲線には影響を与えないといった、他の曲線にない局所性も有する。これらの性質は、現実の物体を制御する上で重要な要素となる。したがって、本発明者らは、パラメトリック曲線としてＮＵＲＢＳ曲線を採用した。
【００６６】
図２には、マニピュレーターや移動ロボットなどの自動機械の制御システムの構成例を示している。図示の制御システム１００は、経路計画部１１０と、移動制御部１２０と、駆動制御部１３０で構成される。
【００６７】
経路計画部１１０は、移動指令を入力して、当該指令を満たす（例えば、指定した最終目標位置まで移動するための）自動機械の経路を計画し、計画した経路上にある複数のノードの系列すなわち経路ノード列を出力する。移動指令は、例えばさまざまな障害物が存在する人間の生活環境であり、経路計画部１１０は、障害物を回避した、曲線状の経路を計画し、その経路ノード列を出力する。例えば、本出願人に既に譲渡されている特開２００９−２１１５７１号公報あるいは特開２０１０−１５５３２８号公報に開示されている軌道計画装置（いずれも前述）を用いて経路計画部１１０を構成することができ、ＲＲＴで発見した軌道を初期条件とし、さらに軌道上の各通過点を質点とした多質点系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び、自動機械と障害物との浸透回避といった要求を満たす経路上のノード列を生成する。
【００６８】
経路計画は一般に計算負荷が高い。本実施形態では、経路計画部１１０は、短い周期で動作する移動制御部１２０及び駆動制御部１３０とは異なる、比較的遅い周期で動作するものとして扱う。但し、本発明の要旨は、経路計画部１１０が遅い周期で動作することに限定されるものではなく、早い周期で動作してもシステム１００全体に影響はない。
【００６９】
移動制御部１２０は、経路計画部１１０から出力された経路を自動機械が移動するための制御を行なう。移動制御部１２０は、仮想バネ・ダンパを用いて目標位置に引き付ける引力を作用させて、自動機械の移動制御を行なう。このとき、現在位置と最終目標位置間を内分比ｒ：１−ｒで内分する内分点を仮想バネ・ダンパ系に与える仮目標位置として算出して、オンラインで目標位置を整形する。また、経路計画部１１０が出力する経路は、障害物との浸透を回避した曲線状であることから、これをパラメトリック曲線の１つであるＮＵＲＢＳ曲線として扱い、仮想バネ・ダンパ系に与える仮目標位置を算出する。
【００７０】
図２に示す例では、移動制御部１２０は、最近傍通過点探索部１２１と、コントロール・ポイント列生成部１２２と、平衡点生成部１２３と、仮想バネ・ダンパ発生力決定部１２６で構成される。また、平衡点生成部１２３は、ノット・ベクトル生成部１２４と、ＮＵＲＢＳ近似部１２５で構成される。
【００７１】
最近傍点探索部１２１は、経路計画部１１０との動作周期を吸収する機能を提供し、経路計画部１１０で計画された経路ノード列から、自動機械の現在位置に最も近いノードを探索する。但し、経路計画部１１０と移動制御部１２０の動作周期が同じであれば、最近傍点探索部１２１は必要ない。
【００７２】
コントロール・ポイント列生成部１２２は、自動機械の現在位置と、経路ノード列のうち最近傍点探索部１２１にて探索した現在位置に最も近いノードから終端ノードまでのノード群を併せることで、コントロール・ポイント列｛ｑi｝を生成する。すなわち、コントロール・ポイント列生成部１２２は、経路計画部１１０が生成した経路ノード列のうち既に通過したノード群を排除する作用がある。但し、外乱などの影響により、現在位置の最近傍点が既に通過したノードとなる場合もあるため、コントロール・ポイント列生成部１２２は、経路計画部１１０から経路の変更要求が次に送信されるまで、計画した経路ノード列を保持する必要がある。
【００７３】
平衡点生成部１２３では、コントロール・ポイントの近傍を通過するＮＵＲＢＳ曲線を生成し、そのＮＵＲＢＳ曲線上における始点である自動機械の現在位置と最終目標位置（終端ノード）の間を内分比ｒ：１−ｒで内分する内分点を平衡点（仮目標位置）として算出する。具体的には、平衡点生成部１２３内のノット・ベクトル生成部１２４は、コントロール・ポイント列生成部１２２で決定したコントロール・ポイント列｛ｑi｝の距離に基づいて、上述した演算式（１１）、（１２）により、ノット・ベクトル列｛ｔi｝を生成する。次に、ＮＵＲＢＳ近似部１２５は、生成したノット・ベクトル列｛ｔi｝、及び、上述した内分比ｒに基づいて、仮目標位置となる平衡点を決定する。上式（９）で表わされるＮＵＲＢＳ曲線Ｐ（ｔ）の引数ｔとして内分比ｒを代入することで、ＮＵＲＢＳ曲線上の自動機械の現在位置と最終目標位置の間を内分比ｒ：１−ｒで内分する平衡点が算出される（図５を参照のこと）。
【００７４】
仮想バネ・ダンパ発生力決定部１２６は、平衡点生成部１２３で決定した平衡点を仮目標位置ｘdとして用い、現在位置とこの仮目標位置ｘd間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力Ｆdを上述した演算式（１）により決定し、駆動制御部１３０へ送信する。若しくは、仮想バネ・ダンパ発生力決定部１２６は、仮想バネ・ダンパ発生力に適当な定数係数を乗ずることで、速度入力へ変換し、駆動制御部１３０へ送信するようにしてもよい。
【００７５】
駆動制御部１３０は、入力された仮想バネ・ダンパ発生力Ｆdに基づいて、自動機械を構成する各関節の駆動を制御する。具体的には、駆動制御部１３０は、他の運動拘束（例えば、自動機械の身体の一部に発生する力、位置、速度、加速度、外界との接触拘束、自動機械を構成する各関節の発生力、関節値、関節速度、関節加速度、可動範囲拘束など）を同時に満たす、自動機械の関節駆動方法を決定して、各関節を駆動するアクチュエーターへの指令値（例えば、関節入力トルクτ）を出力する。
【００７６】
自動機械の関節を駆動するアクチュエーターが力制御型であれば、自動機械の全身を強調させて各関節を駆動する全身協調方法として、操作空間制御を適用することができる。例えば、本出願人に既に譲渡されている特開２００７−１０８９５５号公報には、操作空間制御を適用した全身協調方法について開示されている。また、関節を駆動するアクチュエーターが位置制御型であれば、全身協調方法として、一般化された逆キネマティクスを用いることができる。例えば、本出願人に既に譲渡されている特許第３９７２８５４号号公報には、一般化された逆キネマティクスを用いた全身協調方法について開示されている。本明細書では、自動機械の全身協調方法の詳細については説明を省略する。
【００７７】
図３には、図２に示した制御システム１００で実行される処理手順をフローチャートの形式で示している。
【００７８】
最近傍点探索処理では、最近傍点探索部１２１が、経路計画部１１０で計画された経路ノード列から、現在位置に最も近いノードを探索する（ステップＳ３１）。
【００７９】
続くコントロール・ポイント列生成処理では、コントロール・ポイント列生成部１２２が、現在位置と、上記ステップＳ３１で探索した現在位置に最も近いノードから終端ノードまでのノード群とを組み合わせることで、コントロール・ポイント列を生成する（ステップＳ３２）。
【００８０】
続くノット・ベクトル生成処理では、ノット・ベクトル生成部１２４が、上記ステップＳ３２で決定したコントロール・ポイント列のコントロール・ポイント間距離に基づいて、上述した演算式（１１）、（１２）に従って、ノット・ベクトルを生成する（ステップＳ３３）。
【００８１】
続くＮＵＲＢＳ近似処理では、ＮＵＲＢＳ近似部１２５が、上記ステップＳ３３で生成したノット・ベクトル列及び上述した内分比ｒに基づいて、ＮＵＲＢＳ近似曲線上の平衡点を決定する（ステップＳ３４）。上式（９）で表わされるＮＵＲＢＳ曲線Ｐ（ｔ）のｔに内分比ｒを代入することで、現在位置と最終目標位置を内分比ｒ：１−ｒで内分する平衡点が算出される。
【００８２】
そして、仮想バネ・ダンパ発生力決定処理では、仮想バネ・ダンパ発生力決定部１２６が、上記ステップＳ３４で算出した、仮想バネ・ダンパ系の平衡点から、仮想バネ・ダンパ発生力を決定する（ステップＳ３５）。
【００８３】
図２に示した制御システム１００によれば、自動機械は、複雑な経路への追従制御（移動制御）が可能になる。
【００８４】
制御システム１００は、例えば非特許文献３で開示されるオンライン目標位置整形をベースとするが、目標位置整形の自律系の性質を継承するため、対外乱適応能力を有する。すなわち、制御システム１００は、外乱への受動性を有することにより、安全性を有する。
【００８５】
そして、制御システム１００は、複雑な経路への追従制御と、対外乱適応能力という性質を両立するので、この制御システム１００で制御される移動ロボットは、人間の住環境のような動的環境への適応が可能となる。
【００８６】
また、制御システム１００は、速度プロファイルを設計できるという目標位置整形の特徴を有していることから、自動機械の滑らかな動き出しを実現することができる。
【００８７】
また、移動制御部１２０が自律系で構成されているので、経路の変更に柔軟且つ円滑に対応することができる。図４には、図２に示した制御システム１００を適用した２次元移動ロボットの移動制御における経路変更の様子を示している。同図中、横軸が移動ロボットの前方方向距離Ｘ［ｍ］、縦軸が移動ロボットの横方向距離Ｙ［ｍ］である。図示のグラフには、初回経路計画結果を表す経路ノード列を白抜き丸（○）で示し、経路変更後の経路ノード列を四角（□）で示し、経路再計画実行開始点を黒丸（●）で示している。また、目標位置整形により得られた目標位置ｘdの軌跡を点線で示し、ロボット移動位置軌跡を実線で示している。経路計画部１１０と移動制御部１２０の動作周期の相違に起因して、経路再計画時で多少経路から外れるが、そのずれはごく微小であり問題はない。すなわち、移動制御部１２０が経路の変更に柔軟且つ円滑に対応していることを確認することができる。
【産業上の利用可能性】
【００８８】
以上、特定の実施形態を参照しながら、本発明について詳細に説明してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【００８９】
本発明の適用範囲は特定の次元に限定されないので、マニピュレーターのリーチングや、移動ロボットの移動制御など、さまざまな自動機械の移動制御ら適用することが可能であり、自動機械のさまざまなタスクの実行を補助することができる。
【００９０】
また、本明細書では、コントロール・ポイント列の近傍を通過するパラメトリック曲線としてＮＵＲＢＳ曲線を生成する実施形態を中心に説明してきたが、Ｂｅｚｉｅｒ曲線やＦｅｒｇｕｓｏｎ曲線を含む多項式曲線など、他のパラメトリック曲線を適用することも可能である。Ｂｅｚｉｅｒ曲線やＦｅｒｇｕｓｏｎ曲線を含む多項式曲線では、複数のセグメントをつなぎ合わせて１つの曲線を生成することから、セグメント内での連続性が保証されないという欠点があるが、非特許文献３で開示されるオンライン目標位置整形をベースとし、目標位置整形の自律系の性質を継承して対外乱適応能力を有するといった、同様の効果を得ることができる。
【００９１】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【符号の説明】
【００９２】
１００…制御システム
１１０…経路計画部
１２０…移動制御部
１２１…最近傍通過点探索部
１２２…コントロール・ポイント列生成部
１２３…平衡点生成部
１２４…ノット・ベクトル生成部
１２５…ＮＵＲＢＳ近似部
１２６…仮想バネ・ダンパ発生力決定部
１３０…駆動制御部
【技術分野】
【０００１】
本発明は、マニピュレーターのリーチング動作や移動ロボットの移動など、自動機械の移動を制御する移動制御装置及び移動制御方法、並びにコンピューター・プログラムに係り、特に、複雑な経路の移動能力と対外乱適応能力を備えた自動機械の移動制御装置及び移動制御方法、並びにコンピューター・プログラムに関する。
【背景技術】
【０００２】
人間が活動する環境は、狭空間からなり、また、時間の経過とともに変化する動的環境である。このような環境で、マニピュレーターのリーチング動作、あるいは車輪型又はその他の移動ロボットの移動など、各種の自動機械を活動させようとすると、人や物（以下では、まとめて「障害物」とする）との接触が起こり得る。このため、自動機械を動作させ、さまざまなタスクを実現するためには、複雑な経路を移動する能力と、安全性やタスク遂行のために障害物との接触などの外乱への適応能力が要求される。
【０００３】
一般にマニピュレーターのリーチング動作や移動ロボットの移動といった、自動機械の「移動」においては、与えられた目標位置及び目標到達時刻から目標軌道を生成する移動制御手法が多く用いられている。言い換えれば、自動機械の経路は、目標時刻毎の目標位置を求める、時間を引数とする関数で表現される。上述したように、自動機械を人間の生活環境へ適用する場合、さまざまな障害物との接触が想定される。従来の移動制御手法によると、障害物との接触により目標到達時刻に目標位置に到達しなくなると、制御系が破綻してしまう。すなわち、時刻を陽に含む制御系では、外乱への柔軟性に欠け、十分な安全性を確保することは難しい。
【０００４】
制御系に時刻を陽に含むことなく目標位置への到達を実現する手法として、「仮想バネ・ダンパ仮説」が挙げられる（例えば、非特許文献１を参照のこと）。仮想バネ・ダンパ仮説は、自動機械の現在位置と目標位置間を仮想的なバネ・ダンパ系で接続し、バネによって発生する引力で目標位置に引き付けるというモデルで移動制御を行なうものである。例えば、仮想的なバネ・ダンパ系によって発生する引力に基づいてマニピュレーターの手先を制御する。外乱による振動は、ダンパによって抑制される。仮想バネ・ダンパ仮説は、自動機械の移動経路を時間関数で表現するものではなく、したがって、外乱に柔軟に対応でき、障害物との接触が生じても目標位置に到達させることができる。
【０００５】
仮想バネ・ダンパ仮説によると、動作開始時のバネの復元力が最大、すなわち、動作開始時に手先等の制御対象点の加速度が最大となるため、滑らか且つ安全な動作を実現することは難しく、ハードウェアへの負担も大きい。この問題を解消するべく、時刻に依存した関数を用いることで、仮想バネ係数を変化させる時変スティフネスを導入した手先のリーチング手法が提案されている（例えば、非特許文献２を参照のこと）。しかしながら、この手法は、時刻を陽に含まないことで構成されていた自律系の特性が失われてしまうため、さまざまな障害物との接触が想定される動的環境下での適用は困難である。
【０００６】
さらに、仮想バネ・ダンパ仮説を基に、その仮想バネ・ダンパ系に与える目標位置をオンラインで整形する手法が提案されている（例えば、非特許文献３を参照のこと）。このオンライン目標位置整形では、移動の開始位置から最終的な目標位置までの間に平衡点を設け、最終的な目標位置よりも現在位置から近い平衡点を仮想的な目標位置として仮想バネ・ダンパ仮説を適用することで、速度プロファイルの設計が可能となり、自動機械の動き出しを滑らかにすることができる。平衡点は、現在位置から最終的な目標位置までの残りの距離を基に決定される。オンライン目標位置整形による手法は、軌道を陽に設計することなく、且つ、制御系に時刻を陽に含むことなく、滑らかなリーチング動作を生成するものである。オンラインで目標位置整形を行なう場合、例えばマニピュレーターの状態を手先位置としてフィードバックすることで外力が加えられる際にはそれに柔軟に倣い、外力が解消した後には速度の急変動を起こすことなく、目標位置へのリーチング動作を再開可能な特性を持つ。
【０００７】
しかしながら、仮想バネ係数を変化させる時変スティフネスを導入した手法、並びに、仮想バネ・ダンパ系に与える目標位置をオンラインで整形する手法はいずれも、あくまでも始点（現在位置）と終点（目標位置）の２点間の移動を制御する手法に過ぎない。すなわち、より複雑な動きを要するタスク、障害物を避けながら複雑な移動経路を要する移動に適用するのは難しい。
【０００８】
従来の軌道計画に基づく手法においても（例えば、非特許文献４を参照のこと）、マニピュレーターの手先にコンプライアンス特性を持たせることで外乱への倣いが可能になる。しかしながら、この従来手法は、制御系が時刻を陽に含むため、長期に渡り外乱が発生するような場合には、十分な倣い性能を得ることができない。また、外乱に柔軟に対応する特性を重視して、マニピュレーターの手先に持たせるコンプライアンスを大きくすると、目標とする位置への収束性能が著しく低下してしまう。
【先行技術文献】
【非特許文献】
【０００９】
【非特許文献１】Ｓ．Ａｒｉｍｏｔｏ ａｎｄ Ｍ．Ｓｅｋｉｍｏｔｏ“Ｈｕｍａｎ−Ｌｉｋｅ Ｍｏｖｅｍｅｎｔｓ ｏｆ Ｒｏｂｏｔｉｃ Ａｒｍｓ ｗｉｔｈ Ｒｅｄｕｎｄａｎｔ ＤＯＦｓ：Ｖｉｒｔｕａｌ Ｓｐｒｉｎｇ−Ｄｕｍｐｅｒ Ｈｙｐｏｔｈｅｓｉｓ ｔｏ Ｔａｃｋｌｅ ｔｈｅ Ｂｅｒｎｓｔｅｉｎ Ｐｒｏｂｌｅｍ”（Ｐｒｏｃ．Ｏｆ ＩＥＥＥ ｉｎｔ．Ｃｏｎｆ．ｏｎ ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ｐｐ．１８６０−１８６６，２００６）
【非特許文献２】Ｓ． Ａｒｉｍｏｔｏ ｅｔ ａｌ“ Ｎａｔｕｒａｌ Ｒｅｓｏｌｕｔｉｏｎ ｏｆ Ｉｌｌ−ｐｏｓｅｄｎｅｓｓ ｏｆ Ｉｎｖｅｒｓｅ Ｋｉｎｅｍａｔｉｃｓ ｆｏｒ Ｒｅｄｕｎｄａｎｔ Ｒｏｂｏｔｓ：Ａ Ｃｈａｌｌｅｎｇｅ ｔｏ Ｂｅｒｎｓｔｅｉｎ’ｓ Ｄｅｇｒｅｅｓ−ｏｆ−Ｆｒｅｅｄｏｍ Ｐｒｏｂｌｅｍ”（Ａｄｖａｎｃｅｄ Ｒｏｂｏｔｉｃｓ，Ｖｏｌ．１９，Ｎｏ．４，ｐｐ．４０１−４３４，２００５）
【非特許文献３】Ｆｕｍｉ Ｓｅｔｏ ａｎｄ Ｔｏｍｏｍｉｃｈｉ Ｓｕｇｉｈａｒａ,“Ｏｎｌｉｎｅ Ｒｅｆｅｒｅｎｃｅ Ｓｈａｐｉｎｇ ｗｉｔｈ Ｅｎｄ−ｐｏｉｎｔ Ｐｏｓｉｔｉｏｎ Ｆｅｅｄｂａｃｋ ｆｏｒ Ｌａｒｇｅ Ａｃｃｅｌｅｒａｔｉｏｎ Ａｖｏｉｄａｎｃｅ ｏｎ Ｍａｎｉｐｕｌａｔｏｒ Ｃｏｎｔｒｏｌ，２００９ ＩＥＥＥ／ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ，ｐｐ．５７４３−５７４８，Ｓｔ．Ｌｏｕｉｓ，Ｏｃｔ，１４ｔｈ，２００９）
【発明の概要】
【発明が解決しようとする課題】
【００１０】
本発明の目的は、マニピュレーターのリーチング動作や移動ロボットの移動など、自動機械の移動を好適に制御することができる、優れた移動制御装置及び移動制御方法、並びにコンピューター・プログラムを提供することにある。
【００１１】
本発明のさらなる目的は、人間が活動する狭空間からなる環境で自動機械が活動すべく、複雑な経路の移動能力と対外乱適応能力を備えた、優れた移動制御装置及び移動制御方法、並びにコンピューター・プログラムを提供することにある。
【課題を解決するための手段】
【００１２】
本願は、上記課題を参酌してなされたものであり、請求項１に記載の発明は、
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画部と、
前記経路ノード列から前記現在位置に最も近いノードを探索し、前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成し、前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する移動制御部と、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御部と、
を具備する、自動機械の移動制御装置である。
【００１３】
本願の請求項２に記載の発明によれば、請求項１に記載の移動制御装置の経路計画部は、ＲＲＴで発見した軌道を初期条件とし、さらに軌道上の各通過点を質点とした多質点系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避のうち少なくとも１つの要求を満たす経路を計画して、経路ノード列を出力するように構成されている。
【００１４】
本願の請求項３に記載の発明によれば、請求項１に記載の移動制御装置の経路計画部は、ＲＲＴで発見した軌道を初期条件とし、軌道上の各通過点に適当な初期姿勢を割り当てた剛体を配置した多剛体系力学モデルを構築し、多剛体系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避のうち少なくとも１つの要求を満たす経路を計画して、経路ノード列を出力するように構成されている。
【００１５】
本願の請求項４に記載の発明によれば、請求項１に記載の移動制御装置の移動制御部は、前記の生成したコントロール・ポイント列の近傍を通過するＮＵＲＢＳ曲線を生成し、前記ＮＵＲＢＳ曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定するように構成されている。
【００１６】
本願の請求項５に記載の発明によれば、請求項１に記載の移動制御装置の駆動制御部は、自動機械の身体の一部に発生する力、位置、速度、加速度、外界との接触拘束、自動機械を構成する各関節の発生力、関節値、関節速度、関節加速度、可動範囲拘束を同時に満たす、自動機械の関節駆動方法を決定して、各関節を駆動するアクチュエーターへの指令値を出力するように構成されている。
【００１７】
また、本願の請求項６に記載の発明は、
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画ステップと、
前記経路ノード列から前記現在位置に最も近いノードを探索する最近傍点探索ステップと、
前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成するコントロール・ポイント列生成ステップと、
前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点を生成する平衡点生成ステップと、
前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する仮想バネ・ダンパ発生力決定ステップと、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御ステップと、
を有する、自動機械の移動制御方法である。
【００１８】
また、本願の請求項７に記載の発明は、
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画部、
前記経路ノード列から前記現在位置に最も近いノードを探索し、前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成し、前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する移動制御部、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御部、
としてコンピューターを機能させ、自動機械の移動制御を行なうための処理を前記コンピューター上で実行させるようにコンピューター可読形式で記述されたコンピューター・プログラムである。
【００１９】
本願の請求項７に係るコンピューター・プログラムは、コンピューター上で所定の処理を実現するようにコンピューター可読形式で記述されたコンピューター・プログラムを定義したものである。換言すれば、本願の請求項７に係るコンピューター・プログラムをコンピューターにインストールすることによって、コンピューター上では協働的作用が発揮され、本願の請求項１に係る移動制御装置と同様の作用効果を得ることができる。
【発明の効果】
【００２０】
本発明によれば、マニピュレーターのリーチング動作や移動ロボットの移動など、自動機械の移動を好適に制御することができる、優れた移動制御装置及び移動制御方法、並びにコンピューター・プログラムを提供することができる。
【００２１】
本発明によれば、仮想バネ・ダンパ系に与える目標位置をオンラインで整形し、軌道を陽に設計することなく、且つ、制御系に時間を陽に含むことなく移動制御を実現する手法に、パラメトリック曲線であるＮＵＲＢＳ曲線を導入することで、複雑な経路の移動能力と対外乱適応能力を両立した移動制御を実現することができる。
【００２２】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【図面の簡単な説明】
【００２３】
【図１】図１は、仮想バネ・ダンパ系及びオンライン目標得位置整形を含んだ移動制御系の構成図である。
【図２】図２は、自動機械の制御システムの構成例を示した図である。
【図３】図３は、図２に示した制御システム１００で実行される処理手順を示したフローチャートである。
【図４】図４は、図２に示した制御システム１００を適用した２次元移動ロボットの移動制御における経路変更の様子を示した図である。
【図５】図５は、ＮＵＲＢＳ曲線上の自動機械の現在位置と最終目標位置の間を内分比ｒ：１−ｒで内分する仮目標位置を示した図である。
【発明を実施するための形態】
【００２４】
以下、図面を参照しながら本発明の実施形態について詳細に説明する。
【００２５】
仮想バネ・ダンパ仮説を基に、その仮想バネ・ダンパ系に与える目標位置をオンラインで整形する手法は、軌道を陽に設計することなく、且つ、制御系に時刻を陽に含むことなく、滑らかなリーチング動作を生成することができるが、２点間の移動を制御する手法であり、より複雑な動きを要するタスク、障害物を避けながら複雑な移動経路を要する移動に適用するのは難しい（前述）。これに対し、本発明者らは、時刻を陽に含まないオンライン位置整形により滑らかな移動を実現させるが、目標位置整形に制御対象の状態を位置としてフィードバックすることで、目標位置への収束性能を落とすことなく、外乱への適応能力を実現する移動制御方法について、以下に説明する。この移動制御方法によれば、移動速度の急変動を防止することができる。また、目標位置整形にパラメトリック曲線であるＮＵＲＢＳ曲線を用いることで、複雑な経路への追従も実現することができる。
【００２６】
本発明者らが提案する移動制御方法は、仮想ダンパ仮説及びオンライン目標位置整形を好適に取り入れることで実現される。これらの手法についてはそれぞれ非特許文献１、３に記載されているが、本明細書中でも説明しておく。
【００２７】
仮想バネ・ダンパ仮説
マニピュレーターの関節数をｍ、作業空間の自由度をｌ（通常は、２又は３次元の自由度）、仮想バネ・ダンパ系の剛性係数行列及び粘性係数行列をそれぞれＫp、ξ∈Ｒl×lとおくと、仮想バネ・ダンパ仮説によれば、マニピュレーターの現在手先位置ｘ∈Ｒlに対して、下式（１）で表わされる力Ｆd∈Ｒlが働く（現在手先位置ｘの１次時間微分は、マニピュレーターの手先の速度である）。∈
【００２８】
【数１】
【００２９】
上式（１）において、ｘd∈Ｒlは、仮想バネ・ダンパ系の平衡点として与える（仮想の）目標位置である。要するに、Ｆdは、マニピュレーターの手先を仮想目標位置ｘに引き付ける引力である。
【００３０】
上式（１）を関節空間に展開すると、下式（２）のように表わされる。但し、θ∈Ｒmはマニピュレーターの現在関節角度であり（θの１次時間微分は角速度）、Ｃ∈Ｒm×mは関節の粘性係数行列、Ｊ（θ）∈Ｒl×mはマニピュレーターの現在手先位置ｘまでのヤコブ行列を表すものとする。
【００３１】
【数２】
【００３２】
オンライン目標位置整形
従来の仮想バネ・ダンパ仮説では（例えば、非特許文献１を参照のこと）、仮想バネ・ダンパ系の平衡点として与える目標位置ｘdを、最終目標位置ｘg∈Ｒlに固定する。また、その他の従来方法では、仮想バネ・ダンパ系の平衡点として与える目標位置ｘdを時刻ｔの関数として設計することが多い。また、オンライン目標位置整形では、移動の開始位置から最終的な目標位置までの間に平衡点を設け、最終的な目標位置よりも現在位置から近い平衡点を仮想的な目標位置として仮想バネ・ダンパ仮説を適用することで、自動機械の動き出しを滑らかにする。
【００３３】
オンライン目標位置整形では、仮想バネ・ダンパ系の平衡点として与える目標位置ｘdを、最終目標位置ｘgと現在位置ｘに応じて下式（３）により実時間で整形する。但し、ＫI、ＫI1、ＫI2は非負の定数係数である。
【００３４】
【数３】
【００３５】
ここで、を下式（４）のように置き（但し、０≦ｒ≦１）、入力を最終目標位置ｘg及び現在位置ｘ、出力を仮想バネ・ダンパ系の平衡点として与える目標位置ｘdとすると、伝達関数は下式（５）のように表わされる。
【００３６】
【数４】
【００３７】
【数５】
【００３８】
上式（５）は、最終目標位置ｘgを、現在の位置ｘから最終目標位置ｘgまでの経路を内分比ｒ：１−ｒで内分する内分点で置き換え、さらにその内分点の動きを２次のフィルターで鈍らせた値を、仮想バネ・ダンパ系の平衡点ｘdとして与えることを意味する。このように目標位置を実時間で整形することで、制御対象となる自動機械の動作開始時の加速度及び速度が０になり、滑らかな動き出しが実現される。
【００３９】
図１には、仮想バネ・ダンパ系及びオンライン目標位置整形を含んだ移動制御系の構成図を示している。仮想バネ・ダンパ仮説では、制御対象となる自動機械の運動が、障害物との接触などにより発生した外力により阻害されると、接触した障害物に過大な力を発生させることや、外力が解消された際に急加速が発生することが問題となる。これに対し、オンライン目標位置整形により、単純な２次遅れ系ではなく、現在の位置ｘがフィードバックされると、平衡点ｘdは最終目標位置ｘgと現在位置ｘとのそのときの内分点へと収束するようにされる。したがって、障害物との積極などにより発生する外力によって現在位置ｘの動きが阻害された場合であっても、平衡点ｘdは現在位置ｘから大きく離れることがなく、過度の接触力が発生したり急加速したりすることはない。また、図１からも分かるように、オンライン目標位置整形は時間を陽に含まないので、外乱が長期にわたって発生した場合であっても、移動制御がその影響を受けることはない。外乱が除去された後には、目標位置へと緩やかに収束させることができる。
【００４０】
複雑経路の追従性能の付加（拡張目標位置整形）
非特許文献３で開示されるオンライン目標位置整形は、始点（現在位置）と終点（目標位置）の２点間、すなわち直線的な移動を制御する手法であり、より複雑な動きを要するタスク、障害物を避けながら複雑な移動経路を要する移動に適用するのは難しい（前述）。
【００４１】
例えば、人間の住環境には机や椅子などのさまざまな障害物が存在するので、このような環境内で移動ロボットが活動するには、２点間の移動だけでは障害物との接触などを回避するのは難しく、タスク遂行能力に問題が生じる。つまり、複雑なタスクを遂行するには、複雑な経路に追従可能な目標位置整形器が必須となる。
【００４２】
２次元又は３次元の作業空間上で移動ロボットなどの移動経路を計画する経路計画方法として、ポテンシャル法（例えば、太田順外著「知能ロボット入門 −動作計画問題の解法−」（コロナ社、２００１）を参照のこと）や、ＲＲＴ（例えば、Ｓｔｅｖｅｎ Ｍ．ＬａＶａｌｌｅ，“Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ：Ａ Ｎｅｗ Ｔｏｏｌ ｆｏｒ Ｐａｔｈ Ｐｌａｎｎｉｎｇ”（ＴＲ．９８−１１，Ｃｏｍｐｕｔｅｒ Ｓｃｉｅｎｃｅ Ｄｐｔ．，Ｉｏｗａ Ｓｔａｔｅ Ｕｎｉｖｅｒｓｉｔｙ．Ｏｃｔ．１９９８）を参照のこと）などが知られているが、これらの多くは、経路計画の結果を通過点列で表現する。また、本出願人に既に譲渡されている特開２００９−２１１５７１号公報に開示されている軌道計画装置によれば、ＲＲＴで発見した軌道を初期条件とし、さらに軌道上の各通過点を質点とした多質点系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避といった要求を満たす軌道を生成することができる。さらに、本出願人に既に譲渡されている特開２０１０−１５５３２８号公報に開示されている軌道計画装置によれば、ＲＲＴで発見した軌道を初期条件とし、軌道上の各通過点に適当な初期姿勢を割り当てた剛体を配置した多剛体系力学モデルを構築し、多剛体系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避といった要求を満たす軌道を生成することができる。
【００４３】
ここで、上式（５）の右辺の一部である下式（６）について着目する。
【００４４】
【数６】
【００４５】
上式（６）は、１つのパラメーターｒに対して座標点を対応付けることにより表され、一般的なパラメーター曲線と等価である。そこで、本発明者らは、上式（６）に、代表的なパラメトリック曲線の１つであるＮＵＲＢＳ（Ｎｏｎ−ＵｎｉｆｏｒｍＲａｔｉｏｎａｌ Ｂ−Ｓｐｌｉｎｅｓ：非均一有理Ｂスプライン）曲線を適用することにする。すなわち、現在位置及び最終目標位置をパラメトリック曲線の端末とみなし、これら２点を結ぶパラメトリック曲線を内分比ｒ：１−ｒで内分する内分点を、仮想バネ・ダンパ系の平衡点、すなわち仮目標位置として算出する。そして、この仮目標位置を２次のフィルター（前述）で鈍らせて、仮目標位置ｘdの生成を行なう。これにより、複雑な経路への追従制御が実現されるとともに、ベースとなるオンライン目標位置整形の特徴である対外乱適応能力を両立した移動制御器を構成することができる。
【００４６】
例えば、三浦曜、望月一正共著「ＣＡＤ・ＣＧ技術者のための実践ＮＵＲＢＳ」（工業調査会、２００１）には、ＮＵＲＢＳ曲線について詳細に記述されている。以下では、ＮＵＲＢＳ曲線を移動制御に適用するための留意点を中心に説明する。
【００４７】
ＮＵＲＢＳ曲線は、ＮＵＲＢＳ近似とＮＵＲＢＳ補間に大別される（周知）。ＮＵＲＢＳ近似は、曲線がその点列の近傍を通過し、ＮＵＲＢＳ補間は、曲線が点列を通過するものである。曲線の特性及び計算効率の点から、本発明者らは、移動制御にはＮＵＲＢＳ近似を用いることにした。十分な粒度の通過点列であれば、ＮＵＲＢＳ近似により、動作上問題ない近似経路を得ることができることをここで述べておく。すなわち、移動制御系への入力は、軌道計画法によって得られた通過点列であり、この通過点列をＮＵＲＢＳ曲線のコントロール・ポイント列とみなす。そして、コントロール・ポイントの近傍を通過するＮＵＲＢＳ曲線を生成し、そのＮＵＲＢＳ曲線上における始点である現在位置と最終目標位置を内分比ｒ：１−ｒで内分する内分点を仮目標位置として算出することになる。勿論、ＮＵＲＢＳ近似ではなくＮＵＲＢＳ補間を用いた場合も、ほぼ同等の機能を実現できるが、計算量は増加する点に留意されたい。
【００４８】
ＮＵＲＢＳ曲線は、同次座標空間において定義されたＢスプライン曲線を中心投影によって通常座標空間上に射影することで得られる。コントロール・ポイントの混ぜ合わせが同次座標空間で行なわれるため、各コントロール・ポイントｑi（軌道計画法によって得られる通過点列に相当）には重みωiが与えられる。
【００４９】
同次座標空間におけるＢスプライン曲線は、同次座標のコントロール・ポイント列Ｑi（ωiｘi，ωiｙi，ωiｚi，ωi）をＢスプライン基底関数で混ぜ合わせることにより、下式（７）に示すように、Ｐω（ｔ）として得られる。
【００５０】
【数７】
【００５１】
上記の同次座標空間におけるＢスプライン曲線Ｐw（ｔ）を中心投影すると、曲線Ｐ*（ｔ）は、下式（８）に示す通りとなる。
【００５２】
【数８】
【００５３】
上記の曲線Ｐ*（ｔ）は、同次座標空間におけるω＝１の面上に投影された曲線を表す。これを通常座標系で表現すると、下式（９）に示す通りとなり、これがＮＵＲＢＳ曲線の式となる。
【００５４】
【数９】
【００５５】
但し、上式（９）において、ｑi（ｘi，ｙi，ｚi）は通常座標系におけるコントロール・ポイントで、Ｑi＝（ωiｑi，ωi）なる関係がある。また、ノット（ｋｎｏｔ：節点）・ベクトルをＴ＝［ｔ0，…，ｔm+n+1］とし、ｍ、ｎはそれぞれ曲線の階数及び通過点列数（コントロール・ポイント数）を表す。また、Ｎi,m（ｔ）は、Ｂスプライン規定関数を表し、その計算方法は、計算機上での実装に適した方法として知られるｄｅ Ｂｏｏｒ Ｃｏｘの漸化式を用いる。
【００５６】
参考までに、ｄｅ Ｂｏｏｒ Ｃｏｘの漸化式を以下に示しておく。但し、本発明の要旨は、Ｂスプライン規定関数の計算方法が下式（１０）に限定される訳ではなく、数多の計算方法の中から適宜選択すればよいことを十分理解されたい。
【００５７】
【数１０】
【００５８】
一般に、ノット・ベクトルは、単調増加且つｍ＋ｎ個のノットで構成されていることを満足していれば、基本的には自由に設定することができる（但し、ｍは曲線の階数、ｎはコントロール・ポイント数）。本明細書では、本実施形態における移動制御で用いるノット・ベクトルの作成方法の一例について言及しておく。ノット・ベクトルは、曲線の値域を表すパラメーターであり、ノット・ベクトルの生成により、移動速度プロファイルに特徴を出すことも可能である。例えば，曲率の大きい箇所において、ノット・ベクトル間隔を大きくとることで、移動速度の変化を抑える効果を課すことも可能である。
【００５９】
一般に、ｉ番目の通過点に対して曲線パラメーターを設定する場合、パラメーターの間隔が通過点間の区間長の比を反映するようにするのが望ましいとされている。本実施形態においてもこれに倣い、区間長の比を反映した曲線パラメーターの設定を行なうが、いま区間長として曲線長を当てることができないので、通過点間の距離若しくはその平方根の比で決定する。本明細書では、通過点間の距離を用いた曲線パラメーターの設定方法について記載する。
【００６０】
曲線全体のパラメーターの変位器を０≦ｔ≦１とすると、ｉ番目の通過点に対する曲線パラメーターは、下式（１１）のように決定することができる。
【００６１】
【数１１】
【００６２】
次に、（１１）に示した曲線パラメーターを用いて、下式（１２）に示すように、ノット・ベクトル列を生成する。但し、端末ノットは曲線階数と同じだけ多重に定義し、端末コントロール・ポイントを必ず通過するＮＵＲＢＳ曲線を生成する。
【００６３】
【数１２】
【００６４】
上式（９）で表わされるＮＵＲＢＳ曲線Ｐ（ｔ）の引数ｔとして内分比ｒを代入することで、ＮＵＲＢＳ曲線上の自動機械の現在位置と最終目標位置の間を内分比ｒ：１−ｒで内分する仮目標位置を算出することができる（図５を参照のこと）。
【００６５】
なお、当業界で周知のように、ＮＵＲＢＳ曲線以外にもパラメトリック曲線はある。しかしながら、Ｂｅｚｉｅｒ曲線やＦｅｒｇｕｓｏｎ曲線を含む多項式曲線では、複数のセグメントをつなぎ合わせて１つの曲線を生成することから、セグメント内での連続性が保証されないという欠点がある。これに対し、ＮＵＲＢＳ曲線は、曲線全体を通してＣm-2級連続性が保証される。つまり、コントロール・ポイントの配置に関係なく、各ノットに対応する位置の前後でｍ−２階までの微分係数が連続となる。また、ＮＵＲＢＳ曲線は、特定の制御点を移動した場合に、影響を受ける範囲はその制御点の近傍に限定され、離れた部分の曲線には影響を与えないといった、他の曲線にない局所性も有する。これらの性質は、現実の物体を制御する上で重要な要素となる。したがって、本発明者らは、パラメトリック曲線としてＮＵＲＢＳ曲線を採用した。
【００６６】
図２には、マニピュレーターや移動ロボットなどの自動機械の制御システムの構成例を示している。図示の制御システム１００は、経路計画部１１０と、移動制御部１２０と、駆動制御部１３０で構成される。
【００６７】
経路計画部１１０は、移動指令を入力して、当該指令を満たす（例えば、指定した最終目標位置まで移動するための）自動機械の経路を計画し、計画した経路上にある複数のノードの系列すなわち経路ノード列を出力する。移動指令は、例えばさまざまな障害物が存在する人間の生活環境であり、経路計画部１１０は、障害物を回避した、曲線状の経路を計画し、その経路ノード列を出力する。例えば、本出願人に既に譲渡されている特開２００９−２１１５７１号公報あるいは特開２０１０−１５５３２８号公報に開示されている軌道計画装置（いずれも前述）を用いて経路計画部１１０を構成することができ、ＲＲＴで発見した軌道を初期条件とし、さらに軌道上の各通過点を質点とした多質点系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び、自動機械と障害物との浸透回避といった要求を満たす経路上のノード列を生成する。
【００６８】
経路計画は一般に計算負荷が高い。本実施形態では、経路計画部１１０は、短い周期で動作する移動制御部１２０及び駆動制御部１３０とは異なる、比較的遅い周期で動作するものとして扱う。但し、本発明の要旨は、経路計画部１１０が遅い周期で動作することに限定されるものではなく、早い周期で動作してもシステム１００全体に影響はない。
【００６９】
移動制御部１２０は、経路計画部１１０から出力された経路を自動機械が移動するための制御を行なう。移動制御部１２０は、仮想バネ・ダンパを用いて目標位置に引き付ける引力を作用させて、自動機械の移動制御を行なう。このとき、現在位置と最終目標位置間を内分比ｒ：１−ｒで内分する内分点を仮想バネ・ダンパ系に与える仮目標位置として算出して、オンラインで目標位置を整形する。また、経路計画部１１０が出力する経路は、障害物との浸透を回避した曲線状であることから、これをパラメトリック曲線の１つであるＮＵＲＢＳ曲線として扱い、仮想バネ・ダンパ系に与える仮目標位置を算出する。
【００７０】
図２に示す例では、移動制御部１２０は、最近傍通過点探索部１２１と、コントロール・ポイント列生成部１２２と、平衡点生成部１２３と、仮想バネ・ダンパ発生力決定部１２６で構成される。また、平衡点生成部１２３は、ノット・ベクトル生成部１２４と、ＮＵＲＢＳ近似部１２５で構成される。
【００７１】
最近傍点探索部１２１は、経路計画部１１０との動作周期を吸収する機能を提供し、経路計画部１１０で計画された経路ノード列から、自動機械の現在位置に最も近いノードを探索する。但し、経路計画部１１０と移動制御部１２０の動作周期が同じであれば、最近傍点探索部１２１は必要ない。
【００７２】
コントロール・ポイント列生成部１２２は、自動機械の現在位置と、経路ノード列のうち最近傍点探索部１２１にて探索した現在位置に最も近いノードから終端ノードまでのノード群を併せることで、コントロール・ポイント列｛ｑi｝を生成する。すなわち、コントロール・ポイント列生成部１２２は、経路計画部１１０が生成した経路ノード列のうち既に通過したノード群を排除する作用がある。但し、外乱などの影響により、現在位置の最近傍点が既に通過したノードとなる場合もあるため、コントロール・ポイント列生成部１２２は、経路計画部１１０から経路の変更要求が次に送信されるまで、計画した経路ノード列を保持する必要がある。
【００７３】
平衡点生成部１２３では、コントロール・ポイントの近傍を通過するＮＵＲＢＳ曲線を生成し、そのＮＵＲＢＳ曲線上における始点である自動機械の現在位置と最終目標位置（終端ノード）の間を内分比ｒ：１−ｒで内分する内分点を平衡点（仮目標位置）として算出する。具体的には、平衡点生成部１２３内のノット・ベクトル生成部１２４は、コントロール・ポイント列生成部１２２で決定したコントロール・ポイント列｛ｑi｝の距離に基づいて、上述した演算式（１１）、（１２）により、ノット・ベクトル列｛ｔi｝を生成する。次に、ＮＵＲＢＳ近似部１２５は、生成したノット・ベクトル列｛ｔi｝、及び、上述した内分比ｒに基づいて、仮目標位置となる平衡点を決定する。上式（９）で表わされるＮＵＲＢＳ曲線Ｐ（ｔ）の引数ｔとして内分比ｒを代入することで、ＮＵＲＢＳ曲線上の自動機械の現在位置と最終目標位置の間を内分比ｒ：１−ｒで内分する平衡点が算出される（図５を参照のこと）。
【００７４】
仮想バネ・ダンパ発生力決定部１２６は、平衡点生成部１２３で決定した平衡点を仮目標位置ｘdとして用い、現在位置とこの仮目標位置ｘd間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力Ｆdを上述した演算式（１）により決定し、駆動制御部１３０へ送信する。若しくは、仮想バネ・ダンパ発生力決定部１２６は、仮想バネ・ダンパ発生力に適当な定数係数を乗ずることで、速度入力へ変換し、駆動制御部１３０へ送信するようにしてもよい。
【００７５】
駆動制御部１３０は、入力された仮想バネ・ダンパ発生力Ｆdに基づいて、自動機械を構成する各関節の駆動を制御する。具体的には、駆動制御部１３０は、他の運動拘束（例えば、自動機械の身体の一部に発生する力、位置、速度、加速度、外界との接触拘束、自動機械を構成する各関節の発生力、関節値、関節速度、関節加速度、可動範囲拘束など）を同時に満たす、自動機械の関節駆動方法を決定して、各関節を駆動するアクチュエーターへの指令値（例えば、関節入力トルクτ）を出力する。
【００７６】
自動機械の関節を駆動するアクチュエーターが力制御型であれば、自動機械の全身を強調させて各関節を駆動する全身協調方法として、操作空間制御を適用することができる。例えば、本出願人に既に譲渡されている特開２００７−１０８９５５号公報には、操作空間制御を適用した全身協調方法について開示されている。また、関節を駆動するアクチュエーターが位置制御型であれば、全身協調方法として、一般化された逆キネマティクスを用いることができる。例えば、本出願人に既に譲渡されている特許第３９７２８５４号号公報には、一般化された逆キネマティクスを用いた全身協調方法について開示されている。本明細書では、自動機械の全身協調方法の詳細については説明を省略する。
【００７７】
図３には、図２に示した制御システム１００で実行される処理手順をフローチャートの形式で示している。
【００７８】
最近傍点探索処理では、最近傍点探索部１２１が、経路計画部１１０で計画された経路ノード列から、現在位置に最も近いノードを探索する（ステップＳ３１）。
【００７９】
続くコントロール・ポイント列生成処理では、コントロール・ポイント列生成部１２２が、現在位置と、上記ステップＳ３１で探索した現在位置に最も近いノードから終端ノードまでのノード群とを組み合わせることで、コントロール・ポイント列を生成する（ステップＳ３２）。
【００８０】
続くノット・ベクトル生成処理では、ノット・ベクトル生成部１２４が、上記ステップＳ３２で決定したコントロール・ポイント列のコントロール・ポイント間距離に基づいて、上述した演算式（１１）、（１２）に従って、ノット・ベクトルを生成する（ステップＳ３３）。
【００８１】
続くＮＵＲＢＳ近似処理では、ＮＵＲＢＳ近似部１２５が、上記ステップＳ３３で生成したノット・ベクトル列及び上述した内分比ｒに基づいて、ＮＵＲＢＳ近似曲線上の平衡点を決定する（ステップＳ３４）。上式（９）で表わされるＮＵＲＢＳ曲線Ｐ（ｔ）のｔに内分比ｒを代入することで、現在位置と最終目標位置を内分比ｒ：１−ｒで内分する平衡点が算出される。
【００８２】
そして、仮想バネ・ダンパ発生力決定処理では、仮想バネ・ダンパ発生力決定部１２６が、上記ステップＳ３４で算出した、仮想バネ・ダンパ系の平衡点から、仮想バネ・ダンパ発生力を決定する（ステップＳ３５）。
【００８３】
図２に示した制御システム１００によれば、自動機械は、複雑な経路への追従制御（移動制御）が可能になる。
【００８４】
制御システム１００は、例えば非特許文献３で開示されるオンライン目標位置整形をベースとするが、目標位置整形の自律系の性質を継承するため、対外乱適応能力を有する。すなわち、制御システム１００は、外乱への受動性を有することにより、安全性を有する。
【００８５】
そして、制御システム１００は、複雑な経路への追従制御と、対外乱適応能力という性質を両立するので、この制御システム１００で制御される移動ロボットは、人間の住環境のような動的環境への適応が可能となる。
【００８６】
また、制御システム１００は、速度プロファイルを設計できるという目標位置整形の特徴を有していることから、自動機械の滑らかな動き出しを実現することができる。
【００８７】
また、移動制御部１２０が自律系で構成されているので、経路の変更に柔軟且つ円滑に対応することができる。図４には、図２に示した制御システム１００を適用した２次元移動ロボットの移動制御における経路変更の様子を示している。同図中、横軸が移動ロボットの前方方向距離Ｘ［ｍ］、縦軸が移動ロボットの横方向距離Ｙ［ｍ］である。図示のグラフには、初回経路計画結果を表す経路ノード列を白抜き丸（○）で示し、経路変更後の経路ノード列を四角（□）で示し、経路再計画実行開始点を黒丸（●）で示している。また、目標位置整形により得られた目標位置ｘdの軌跡を点線で示し、ロボット移動位置軌跡を実線で示している。経路計画部１１０と移動制御部１２０の動作周期の相違に起因して、経路再計画時で多少経路から外れるが、そのずれはごく微小であり問題はない。すなわち、移動制御部１２０が経路の変更に柔軟且つ円滑に対応していることを確認することができる。
【産業上の利用可能性】
【００８８】
以上、特定の実施形態を参照しながら、本発明について詳細に説明してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【００８９】
本発明の適用範囲は特定の次元に限定されないので、マニピュレーターのリーチングや、移動ロボットの移動制御など、さまざまな自動機械の移動制御ら適用することが可能であり、自動機械のさまざまなタスクの実行を補助することができる。
【００９０】
また、本明細書では、コントロール・ポイント列の近傍を通過するパラメトリック曲線としてＮＵＲＢＳ曲線を生成する実施形態を中心に説明してきたが、Ｂｅｚｉｅｒ曲線やＦｅｒｇｕｓｏｎ曲線を含む多項式曲線など、他のパラメトリック曲線を適用することも可能である。Ｂｅｚｉｅｒ曲線やＦｅｒｇｕｓｏｎ曲線を含む多項式曲線では、複数のセグメントをつなぎ合わせて１つの曲線を生成することから、セグメント内での連続性が保証されないという欠点があるが、非特許文献３で開示されるオンライン目標位置整形をベースとし、目標位置整形の自律系の性質を継承して対外乱適応能力を有するといった、同様の効果を得ることができる。
【００９１】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【符号の説明】
【００９２】
１００…制御システム
１１０…経路計画部
１２０…移動制御部
１２１…最近傍通過点探索部
１２２…コントロール・ポイント列生成部
１２３…平衡点生成部
１２４…ノット・ベクトル生成部
１２５…ＮＵＲＢＳ近似部
１２６…仮想バネ・ダンパ発生力決定部
１３０…駆動制御部
【特許請求の範囲】
【請求項１】
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画部と、
前記経路ノード列から前記現在位置に最も近いノードを探索し、前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成し、前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する移動制御部と、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御部と、
を具備する、自動機械の移動制御装置。
【請求項２】
前記経路計画部は、ＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）で発見した軌道を初期条件とし、さらに軌道上の各通過点を質点とした多質点系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避のうち少なくとも１つの要求を満たす経路を計画して、経路ノード列を出力する、
請求項１に記載の移動制御装置。
【請求項３】
前記経路計画部は、ＲＲＴで発見した軌道を初期条件とし、軌道上の各通過点に適当な初期姿勢を割り当てた剛体を配置した多剛体系力学モデルを構築し、多剛体系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避のうち少なくとも１つの要求を満たす経路を計画して、経路ノード列を出力する、
請求項１に記載の移動制御装置。
【請求項４】
前記移動制御部は、前記の生成したコントロール・ポイント列の近傍を通過するＮＵＲＢＳ曲線を生成し、前記ＮＵＲＢＳ曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する、
請求項１に記載の移動制御装置。
【請求項５】
前記駆動制御部は、自動機械の身体の一部に発生する力、位置、速度、加速度、外界との接触拘束、自動機械を構成する各関節の発生力、関節値、関節速度、関節加速度、可動範囲拘束を同時に満たす、自動機械の関節駆動方法を決定して、各関節を駆動するアクチュエーターへの指令値を出力する、
請求項１に記載の移動制御装置。
【請求項６】
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画ステップと、
前記経路ノード列から前記現在位置に最も近いノードを探索する最近傍点探索ステップと、
前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成するコントロール・ポイント列生成ステップと、
前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点を生成する平衡点生成ステップと、
前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する仮想バネ・ダンパ発生力決定ステップと、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御ステップと、
を有する、自動機械の移動制御方法。
【請求項７】
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画部、
前記経路ノード列から前記現在位置に最も近いノードを探索し、前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成し、前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する移動制御部、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御部、
としてコンピューターを機能させ、自動機械の移動制御を行なうための処理を前記コンピューター上で実行させるようにコンピューター可読形式で記述されたコンピューター・プログラム。
【請求項１】
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画部と、
前記経路ノード列から前記現在位置に最も近いノードを探索し、前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成し、前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する移動制御部と、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御部と、
を具備する、自動機械の移動制御装置。
【請求項２】
前記経路計画部は、ＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）で発見した軌道を初期条件とし、さらに軌道上の各通過点を質点とした多質点系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避のうち少なくとも１つの要求を満たす経路を計画して、経路ノード列を出力する、
請求項１に記載の移動制御装置。
【請求項３】
前記経路計画部は、ＲＲＴで発見した軌道を初期条件とし、軌道上の各通過点に適当な初期姿勢を割り当てた剛体を配置した多剛体系力学モデルを構築し、多剛体系動力学シミュレーションを実行して、軌道の円滑化、最適化、及び障害物との浸透回避のうち少なくとも１つの要求を満たす経路を計画して、経路ノード列を出力する、
請求項１に記載の移動制御装置。
【請求項４】
前記移動制御部は、前記の生成したコントロール・ポイント列の近傍を通過するＮＵＲＢＳ曲線を生成し、前記ＮＵＲＢＳ曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する、
請求項１に記載の移動制御装置。
【請求項５】
前記駆動制御部は、自動機械の身体の一部に発生する力、位置、速度、加速度、外界との接触拘束、自動機械を構成する各関節の発生力、関節値、関節速度、関節加速度、可動範囲拘束を同時に満たす、自動機械の関節駆動方法を決定して、各関節を駆動するアクチュエーターへの指令値を出力する、
請求項１に記載の移動制御装置。
【請求項６】
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画ステップと、
前記経路ノード列から前記現在位置に最も近いノードを探索する最近傍点探索ステップと、
前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成するコントロール・ポイント列生成ステップと、
前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点を生成する平衡点生成ステップと、
前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する仮想バネ・ダンパ発生力決定ステップと、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御ステップと、
を有する、自動機械の移動制御方法。
【請求項７】
障害物が存在する環境で、自動機械の現在位置から最終目標位置まで自動機械が移動するための、前記自動機械と前記障害物との浸透を回避した経路を計画し、計画した経路上の複数のノードからなる経路ノード列を出力する経路計画部、
前記経路ノード列から前記現在位置に最も近いノードを探索し、前記自動機械の現在位置と、前記経路ノード列のうち前記現在位置に最も近いノードから終端ノードまでのノード群を併せたコントロール・ポイント列を生成し、前記コントロール・ポイントの近傍を通過するパラメトリック曲線を生成し、前記パラメトリック曲線上の前記現在位置と終端ノードの間を内分比ｒ：１−ｒで内分する平衡点として算出し、前記現在位置と前記平衡点の間を接続する仮想バネ・ダンパ系に発生する仮想バネ・ダンパ発生力を決定する移動制御部、
前記仮想バネ・ダンパ発生力に基づいて、前記自動機械を構成する各関節の駆動を制御する駆動制御部、
としてコンピューターを機能させ、自動機械の移動制御を行なうための処理を前記コンピューター上で実行させるようにコンピューター可読形式で記述されたコンピューター・プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１２−１５７９５５（Ｐ２０１２−１５７９５５Ａ）
【公開日】平成２４年８月２３日（２０１２．８．２３）
【国際特許分類】
【出願番号】特願２０１１−２０５１２（Ｐ２０１１−２０５１２）
【出願日】平成２３年２月２日（２０１１．２．２）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
【公開日】平成２４年８月２３日（２０１２．８．２３）
【国際特許分類】
【出願日】平成２３年２月２日（２０１１．２．２）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
[ Back to top ]