JP7299097B2

JP7299097B2 - 軌道生成装置

Info

Publication number: JP7299097B2
Application number: JP2019142793A
Authority: JP
Inventors: 公俊山崎; 虎太郎長濱; 瑞朱; 佳佑竹下
Original assignee: Shinshu University NUC; Toyota Motor Corp
Current assignee: Shinshu University NUC; Toyota Motor Corp
Priority date: 2019-08-02
Filing date: 2019-08-02
Publication date: 2023-06-27
Anticipated expiration: 2039-08-02
Also published as: JP2021024019A

Description

特許法第３０条第２項適用２０１９年７月６日ＩＥＥＥＩＣＭＡ２０１９ｗｅｂサイト（ｈｔｔｐ：／／２０１９．ｉｅｅｅ－ｉｃｍａ．ｏｒｇ／）にて頒布の予稿集「Ｐｒｏｃｅｅｄｉｎｇｓｏｆ２０１９ＩＥＥＥＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｆｅｒｅｎｃｅｏｎＭｅｃｈａｔｒｏｎｉｃｓａｎｄＡｕｔｏｍａｔｉｏｎＡｕｇｕｓｔ４－７，Ｔｉａｎｊｉｎ，Ｃｈｉｎａ（ＩＣＭＡ２０１９－３２３）」に公開

本発明は、軌道生成装置に関する。

特許文献１には、モバイルマニピュレータとも称される、ロボットアームの先端のエンドエフェクタの軌道生成装置が開示されている。モバイルマニピュレータを任意の目標地点へ移動させる過程の経路を含む空間を３次元的にブロック化し、各ブロックにモバイルマニピュレータを移動させるための関節角度を計算したデータを記憶したデータベースを作成しておく。そして、実際にモバイルマニピュレータを移動させる際は、まず、ブロックの基準点に移動し、このデータベースを参酌して目標地点までの移動を実行する。

特開２００２－３２６１７４号公報

しかしながら、経路の途中に障害物が存在する場合には、こうした障害物を回避してモバイルマニピュレータを適切に移動させることはできない。

本発明は、このような事情に鑑みなされたものであって、障害物が存在する場合であっても、衝突を回避して移動させる軌道を高速で生成する軌道生成装置を提供することを目的とする。

本発明の一態様にかかる軌道生成装置は、モバイルマニピュレータを任意の目標地点へ移動させる過程の経路を含む空間を３次元的にボクセル化してデータベースに記憶し、記憶されたボクセル情報に基づき、目標地点にモバイルマニピュレータを移動させるためのモバイルマニピュレータの軌道生成装置であって、
目標地点に基づき、前記データベースから、モバイルマニピュレータが動作する１つ又は複数の候補軌道と、各候補軌道に沿ってモバイルマニピュレータが通る空間のボクセル情報と、を取得し、
環境センサにより、モバイルマニピュレータが動作する環境の３次元点群を検出し、３次元点群をボクセル化した３次元環境情報を取得し、
前記取得された１つ又は複数の候補軌道に沿ってモバイルマニピュレータが通る空間のボクセル情報と、前記ボクセル化された３次元環境情報との比較に基づき、障害物と衝突しない軌道を生成する。

本発明により、経路の途中に障害物が存在する場合であっても、衝突を回避して移動させる軌道を高速で生成する軌道生成装置を実現できる。

自律移動ロボットの構成を示す外観図である。自律移動ロボットの概略的なシステム構成を示すブロック図である。実施の形態１にかかるロボットの事前学習を説明するフローチャートである。シミュレータで構築された環境の一例を示す図である。ロボットが通る空間をボクセル化した一例を示す図である。実施の形態１にかかるロボットの動作計画を説明するフローチャートである。ロボットが３次元環境情報を取得する例を説明する図である。干渉チェックを説明する図である。実施の形態２にかかるロボットの事前学習を説明するフローチャートである。実施の形態２にかかるロボットの動作計画を説明するフローチャートである。

以下、本発明を適用した具体的な実施形態について、図面を参照しながら詳細に説明する。ただし、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載および図面は、適宜、簡略化されている。

＜実施の形態１＞
図１及び図２を参照して、本発明の実施形態に係る自律移動ロボットの一例であるロボットの構成について説明する。図１は、本発明の実施の形態１に係る自律移動ロボットの構成を示す外観図である。本実施の形態１に係る自律移動ロボット１は、ＨＳＲ（Human Support Robot）であり、例えば、片付けを行う対象物（以下、対象物）を予め設定された片付け場所に移動させ、対象物を片付け場所に片付ける作業を自律的に行う。

図２は、本発明の実施形態１に係る自律移動ロボットの概略的なシステム構成を示すブロック図である。本実施形態に係る自律移動ロボット１は、ロボット本体２と、ロボット本体２を移動させる移動装置３と、物体を把持し移動させる把持操作装置４と、移動装置３及び把持操作装置４を制御する制御装置５と、環境センサ６と、を備えている。なお、移動装置３と把持操作装置４を合わせて、モバイルマニピュレータ又はモバイルマニピュレータロボットと呼ばれる。

移動装置３は、制御装置５からの制御信号に応じて、例えば、モータなどを駆動させることで複数の車輪を回転させ、ロボット本体２を所望の位置に移動させる。

把持操作装置４は、例えば、複数の手先４４で対象物を把持する把持部４１と、手首関節、肘関節、肩関節などの関節部４２を介して連結される複数のリンク４３と、各関節部４２を駆動するモータなどの複数のアクチュエータと、を含む多関節型アームとして構成されている。

制御装置５は、例えば、ユーザが操作端末７を介して入力した操作信号に基づいて移動装置３及び把持操作装置４を制御することで、自律移動ロボット１に作業を実行させる。操作端末７は、例えば、タブレット端末、ＰＣ（Personal Computer）、スマートフォンなどの携帯端末である。操作端末７と制御装置５は、無線あるいは有線で通信接続されており、相互にデータの送受信を行う。

制御装置５は、例えば、制御処理、演算処理等を行うＣＰＵ（Central Processing Unit）５ａ、ＣＰＵ５ａによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ５ｂ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）５ｃ、などからなるマイクロコンピュータを中心にして構成されている。ＣＰＵ５ａ、メモリ５ｂ、及びインターフェイス部５ｃは、データバスなどを介して相互に接続されている。

環境センサ６は、ロボット１の周囲の対象物などの環境情報（距離情報、画像情報など）を検出する。環境センサ６は、例えば、ロボット１の頭部などに搭載されている（この場合、頭部センサとも呼ばれる）が、これに限られない。環境センサ６は、ロボット１が動作する動作環境内に設置されていてもよい。さらに、環境センサ６は、ロボット１及び動作環境内に複数設けられていてもよい。環境センサ６は、例えば、カメラ（ＲＧＢ－Ｄカメラ、ステレオカメラ）、レーザレンジファインダ、超音波センサなどの距離センサであってもよい。

環境センサ６は、自律移動ロボット１が移動経路を移動する際の移動経路上の対象物などの障害物（例えば、机、棚など）を検出する。環境センサ６は、検出した障害物などの環境情報を制御装置５に出力する。

データベース１０は、後述する事前学習等で得られたロボットの手先ゴール、軌道、および通過する空間のボクセル情報等を記憶する。

制御装置５は、自律移動ロボット１が移動する移動経路を計画し、その移動経路に沿って自律移動ロボット１が移動するよう、移動装置３を制御する。もちろん、自律移動ロボット１は車輪型の移動ロボットに限らず、歩行型やその他の移動ロボットでもよい。制御装置５は、例えば、環境センサ６により検出された環境情報に基づいて、自律移動ロボット１の自己位置推定を行い、計画した移動経路に従って自律移動ロボット１を移動させる。

ロボットが人間に価値を提供するタスクにおいて、タスクを素早く実行することは重要であり、そのためにも環境と衝突しない安全な動作を、高速に計画することが要求されている。こうした要求を実現するために、事前の様々な軌道を計画しておき、それを利用する手法を提案する。事前計算では、ロボット軌道群とそれを実現した際にロボットが通過する空間をボクセル化した情報を計算し、データベースに蓄積する。動作計画時には、蓄積した情報から軌道と通過する空間情報を取り出すとともに、現在の環境情報をボクセルで表現し、ボクセル同士のビット演算にて衝突判定を高速に行う。これらによって、環境と衝突しない動作の計画を、高速に実現する。

図３を参照して、本実施の形態にかかるロボットの事前学習について説明する。
まず、シミュレータで適当な環境を構築する（ステップＳ１０１）。この場合、例えば、図４に示すように、机や棚、物品等の障害物を一定範囲でランダムに配置する。この際、Ｇａｚｅｂｏシミュレータを利用することができる。ロボットモデルは、ＨＳＲとすることができる。

次に、任意の手先ゴール（目標地点）を指定し、軌道を計画する（ステップＳ１０２）。手先ゴールは、生成するルールを定義し、実装することで、自動生成され得る。ルールの例としては、ロボット前方一定エリア内からランダムにサンプリングしてもよいし、あるいは、ロボット前方一定エリアを等間隔にグリッド化して、その中心点としてもよい。また、手先ゴールは、ユーザが操作端末７を介して入力してもよい。この際、例えば、１０ｍｍ刻みでロボットを移動してシミュレーションを行う。ここでは、ＣＢｉＲＲＴ（Constrained Bi-directional Rapidly-Exploring Random Tree）により軌道計画を実施する。軌道は、シミュレータ上の、環境センサ６（頭部３Ｄセンサ）が出力する３次元点群を回避するように設定される。設定された軌道を保存する。

次に、計画された軌道に基づいて、ロボットが通る空間のボクセル表現を計算する（ステップＳ１０３）。ボクセル（例えば、一辺１００ｍｍ）で埋め尽くした空間（ロボットが通る空間をボクセル化した一例）を用意する（図５参照）。ここでは、ボクセルの一辺は１００ｍｍとしたが、より細かくなるように設定してもよいが、その分、干渉チェックにかかる時間が増大することになる。例えば、ボクセルの一辺が１００ｍｍの場合、１軌道にかかる干渉チェック時間は、０．５ｍｓｅｃとなる。こうして用意したボクセルで埋め尽くした空間において、ロボットを、軌道上で様々な姿勢を取らせ、ロボットとボクセルとの干渉をチェックする。

以上のように事前学習した、手先ゴールと、軌道と、ロボットが通る空間のボクセル表現と、をデータベースに蓄積する（ステップＳ１０４）。ここでは、手先ゴールをキーとしたＫｄｔｒｅｅを利用することができ、高速検索化を実現できる。Ｋｄｔｒｅｅ、すなわち、ｋ次元木とは、ｋ次元の空間内のいくつかの点の組織化のため計算機科学で使用されるデータ構造である。

次に図６を参照して、本実施の形態にかかるロボットの動作計画について説明する。
３次元点群（３次元環境情報）を取得する（ステップＳ２０１）。図７に示すように、ロボット１は、環境センサ６（図１）を、ロボットが動く方向、つまり、目標手先位置の方向に向けて、ロボットが動作する予定の空間の障害物を表す３次元環境情報を検出して取得する。次に、３次元点群をボクセル化する（ステップＳ２０２）。ボクセルグリッドフィルタを通して点群を減らし、残った点のｘｙｚから有効なボクセルを選択する。

次に、手先目標をキーとして、データベース１０を検索して、１つ又は複数の候補軌道と、軌道を通る空間のボクセル表現を取得する（ステップＳ２０３）。前述したように、データベース１０では、Ｋｄｔｒｅｅが構築されているので、候補軌道と、軌道を通る空間のボクセルをすぐに検索することができる。

最後に、ボクセル化された３次元点群と、候補軌道ごとにロボットが通る空間のボクセルのビット演算により、ロボットが通る空間と環境の障害物との干渉をチェックする（ステップＳ２０４）。ボクセル化された３次元点は、動作計画のステップＳ２０１、Ｓ２０２で取得したものである。ロボットが通る空間のボクセルは、事前学習のステップＳ１０３で取得したものである。図８は、干渉チェックを説明する図である。図８において、ボクセル化のＳｗｅｐｔＶｏｌｕｍｅ（Ｖ_{ｉ・ｊ・ｋ}）は、ロボットが通る空間のボクセルを示し、実世界のＶｏｘｅｌＭａｐ（Ｍ_{ｉ・ｊ・ｋ}）は、ボクセル化された３次元点群を示す。衝突判定は以下の通りに行われる。すなわち、
Ｖ・Ｍ＝１となる_ｘｙｚが存在する場合には、衝突が発生する。
すべての_ｉｊｋでＶ・Ｍ＝０の場合には、（衝突が発生しないので）動作が許可できる。
これにより、障害物と衝突しない軌道を、軌道候補から選択することができる。

以上説明した動作計画（Ｓ２０１～Ｓ２０４）によれば、処理時間の平均値は、０．０７４秒となり、Motion Planning Networksの軌道生成（約０．１１秒）に比べて、処理時間を３割程度短縮し、高速化することができる。

以上説明した実施の形態１にかかる軌道生成装置は、様々な手先目標指令を生成する機能と、様々な環境形状を生成する機能と、手先日標指令と環境形状から軌道計画を行う機能（低速でもよい）と、ロボットの形状情報及び軌道とロボット形状から、ロボットが通る空間のボクセル表現を算出する機能と、を有する。また、本実施の形態にかかるデータベースは、手先日標指令、軌道、ロボットが通る空間を蓄積し、手先日標指令をキーとして、軌道と、ロボットが通る空間と、を出力する。さらに、本実施の形態にかかる軌道生成装置は、環境センサ等を介して、３次元環境情報を取得する機能と、３次元環境情報のボクセル表現を算出する機能と、３次元環境情報のボクセル表現とロボットが通る空間の重なりがあるかをビット演算により判定（比較）する機能（干渉チェック機能）を有する。

このように、データベースからの軌道候補の探索は、既存の軌道計画よりも高速である。また、ロボットの通る空間と３次元環境の重なりをビット演算で判定するため、軌道候補が環境と衝突しないかについても高速に判定できる。これらを合わせると、環境と衝突しない軌道が高速に生成される

＜実施の形態２＞
図９及び図１０を参照して実施の形態２にかかる事前学習及び学習計画を説明する。図９及び図１０では、実施の形態１（図３及び図６）と同一の処理は、適宜説明を省略する。

図９を参照して、本実施の形態にかかるロボットの事前学習について説明する。
まず、シミュレータで適当な環境を構築する（ステップＳ３０１）。次に、任意の手先ゴール（目標地点）を指定し、軌道を計画する（ステップＳ３０２）。次に、計画された軌道に基づいて、ロボットが通る空間のボクセル表現を計算する（ステップＳ３０３）。以上のステップＳ３０１～Ｓ３０３は、実施の形態１の事前計画のステップＳ１０１～Ｓ１０３と同様に行われる。

シミュレータ上の３次元点群をボクセル化する（ステップＳ３０４）。ここでのボクセル化は、実施の形態１の動作計画のステップＳ２０２と同じように行う。

手先ゴールと環境形状を入力し、軌道とロボットが通る空間を出力するニューラルネットワークの教師付き学習を実施する（ステップＳ３０５）。ここでは、Motion Planning Networksの軌道生成手順に基づき、適宜改良して行ってもよい。様々な軌道を出力できるように、軌道長やロボットが通る空間のボクセル数をニューラルネットワークに入力してもよい。

図１０を参照して、本実施の形態にかかるロボットの動作計画について説明する。
３次元点群を取得する（ステップＳ４０１）。次に、３次元点群をボクセル化する（ステップＳ４０２）。以上のステップＳ４０１、Ｓ４０２は、実施の形態１の動作計画のステップＳ２０１、Ｓ２０２と同様に行われる。

次に、手先ゴールとボクセル化された３次元点群をニューラルネットワークに入力して、軌道と、ロボットが通る空間のボクセルを取得する（ステップＳ４０３）。前述したように、目標軌道長や、ロボットが通る空間のボクセル数を複数準備し、候補軌道と、ロボットが通る空間のボクセル表現の複数のペアを出力することもできる。

最後に、ボクセル化された３次元点群と、候補軌道に沿ってロボットが通る空間のボクセルのビット演算により、ロボットが通る空間と環境のボクセル情報との干渉をチェックする（ステップＳ４０４）。

以上説明した実施の形態１にかかる軌道生成装置は、様々な手先日標指令を生成する機能と、様々な環境形状を生成する機能と、手先日標指令と環境形状から軌道計画を行う機能（低速でもよい）と、ロボットの形状情報及び軌道とロボット形状から、ロボットが通る空間のボクセル表現を算出する機能と、を有する。また、本実施の形態にかかる軌道生成装置は、手先日標指令と環境形状を入力、軌道とロボットが通る空間を出力とするニューラルネットワークの教師付き学習と、手先日標指令と環境形状から、軌道とロボットが通る空間を出力する学習済みモデルと、３次元環境情報を取得する機能と、３次元環境情報のボクセル表現を算出する機能と、３次元環境情報のボクセル表現とロボットが通る空間の重なりがあるかをビット演算により判定（比較）する機能と、を有する。

Motion Planning Networksでは、オートエンコーダで行っている３次元環境の特徴抽出を、ボクセル表現で行った方が性能が良くなる可能性があり、また、軌道が推定された後の衝突判定も出来るので便利となる可能性がある。

さらに、本実施の形態に軌道生成装置は、実施の形態１に対して、機械学習を利用することで、より幅広い環境と手先目標指令に対応できるようになる場合がある。また、軌道候補が環境と衝突しないかの判定を機械学習による推定とは別に行っているため、環境と衝突しないことが保証できる。機械学習による軌道とロボットが通る空間の推定に要する時間は、データベースからの探索と同程度、もしくはわずかに劣る程度だと考えられ十分に高速に、環境と衝突しない軌道が生成され得る。

上述の例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ－ＲＯＭ（Read Only Memory）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、ＤＶＤ（Digital Versatile Disc）、ＢＤ（Blu-ray（登録商標） Disc）、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

また、以上で説明した複数の例は、適宜組み合わせて実施されることもできる。

以上、本発明を上記実施の形態に即して説明したが、本発明は上記実施の形態の構成にのみ限定されるものではなく、本願特許請求の範囲の請求項の発明の範囲内で当業者であればなし得る各種変形、修正、組み合わせを含むことは勿論である。

１自律移動ロボット
２ロボット本体
３移動装置
４把持操作装置
５制御装置
５ａＣＰＵ
５ｂメモリ
５ｃインターフェイス部
６環境センサ
７操作端末
１０データベース
４１把持部
４２関節部
４３リンク
４４手先

Claims

モバイルマニピュレータを任意の目標地点へ移動させる過程の経路を含む空間を３次元的にボクセル化してデータベースに記憶し、記憶されたボクセル情報に基づき、目標地点にモバイルマニピュレータを移動させるためのモバイルマニピュレータの軌道生成装置であって、
事前計画において、モバイルマニピュレータの複数の軌道群の計画を実現した際にモバイルマニピュレータが通過する複数の軌道及び複数の空間のボクセル情報を記憶するデータベースを備え、
目標地点に基づき、前記データベースに記憶された前記複数の軌道及び複数の空間のボクセル情報から、モバイルマニピュレータが動作する１つ又は複数の候補軌道と、各候補軌道に沿ってモバイルマニピュレータが通る空間のボクセル情報と、を取得し、
環境センサにより、モバイルマニピュレータが動作する環境の３次元点群を検出し、前記３次元点群をボクセル化した３次元環境情報を取得し、
前記データベースから取得された１つ又は複数の候補軌道に沿ってモバイルマニピュレータが通る前記空間のボクセル情報と、前記環境センサにより取得された前記ボクセル化された３次元環境情報との比較に基づき、障害物と衝突しない軌道を生成する、軌道生成装置。