ホーム > 特許ランキング > 国立大学法人信州大学
知財求人 - 知財ポータルサイト「IP Force」
▶ 国立大学法人信州大学の特許一覧 ▶ トヨタ自動車株式会社の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024142918
(43)【公開日】2024-10-11
(54)【発明の名称】軌道生成システム、軌道生成方法、及び、プログラム
(51)【国際特許分類】
B25J 9/10 20060101AFI20241003BHJP
【ＦＩ】
B25J9/10 A
【審査請求】未請求
【請求項の数】9
【出願形態】ＯＬ
(21)【出願番号】P 2023055327
(22)【出願日】2023-03-30
(71)【出願人】
【識別番号】504180239
【氏名又は名称】国立大学法人信州大学
(71)【出願人】
【識別番号】000003207
【氏名又は名称】トヨタ自動車株式会社
(74)【代理人】
【識別番号】100103894
【弁理士】
【氏名又は名称】家入 健
(72)【発明者】
【氏名】山崎 公俊
(72)【発明者】
【氏名】岩崎 拓也
(72)【発明者】
【氏名】竹下 佳佑
【テーマコード（参考）】
3C707
【Ｆターム（参考）】
3C707AS34
3C707BS10
3C707BS27
3C707CS08
3C707CV06
3C707CW06
3C707KS12
3C707KT03
3C707MS07
3C707MS08
3C707MS09
3C707WA03
3C707WA16
3C707WK04
(57)【要約】
【課題】サービス環境との接触を回避しつつタスクを完遂できるモバイルマニピュレータの軌道を短時間で生成する。
【解決手段】接触判定部２２は、モバイルマニピュレータ１がタスクを完了するまでの間にリアルタイムに取得した三次元データに基づくサービス環境とモバイルマニピュレータ１が接触するか否か判定する。回避動作データ抽出部２３は、接触すると接触判定部２２が判定した場合、複数の候補動作データから、リアルタイムに取得した三次元データに基づくサービス環境と接触しないＳＶを有する動作データを回避動作データとして抽出する。遷移軌道生成部２４は、初期動作データの軌道Ｑから回避動作データの軌道Ｑへ遷移する遷移軌道を生成する。回避動作データ抽出部２３は、ニューラルネットワークにより構築してもよい。
【選択図】図１０
【解決手段】接触判定部２２は、モバイルマニピュレータ１がタスクを完了するまでの間にリアルタイムに取得した三次元データに基づくサービス環境とモバイルマニピュレータ１が接触するか否か判定する。回避動作データ抽出部２３は、接触すると接触判定部２２が判定した場合、複数の候補動作データから、リアルタイムに取得した三次元データに基づくサービス環境と接触しないＳＶを有する動作データを回避動作データとして抽出する。遷移軌道生成部２４は、初期動作データの軌道Ｑから回避動作データの軌道Ｑへ遷移する遷移軌道を生成する。回避動作データ抽出部２３は、ニューラルネットワークにより構築してもよい。
【選択図】図１０
【特許請求の範囲】
【請求項1】
目標手先位置姿勢と、モバイルマニピュレータの手先位置姿勢を当該目標手先位置姿勢に移動させるための前記モバイルマニピュレータの軌道と、当該軌道に従って前記モバイルマニピュレータを動作させるときに前記モバイルマニピュレータが描く三次元形状である掃引空間と、を含む動作データを複数記憶する記憶部と、
第１のタイミングで環境の三次元データを取得し、タスクに対応する前記目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データであって、前記第１のタイミングで取得した前記三次元データに基づく前記環境と接触しない掃引空間を有する動作データを前記記憶部から複数抽出する第１動作データ抽出部と、
前記第１動作データ抽出部が抽出した前記複数の動作データの何れか１つである第１動作データに基づいて、前記タスクを開始するタスク開始部と、
前記環境の三次元データを前記第１のタイミングよりも後にリアルタイムに取得することにより、前記モバイルマニピュレータが前記タスクを完了するまでの間にリアルタイムに取得した前記三次元データに基づく環境と前記モバイルマニピュレータが接触するか否か判定する接触判定部と、
接触すると前記接触判定部が判定した場合、前記第１動作データ抽出部が抽出した前記複数の動作データから、リアルタイムに取得した前記三次元データに基づく環境と接触しない掃引空間を有する動作データを第２動作データとして抽出する第２動作データ抽出部と、
前記第１動作データの軌道から前記第２動作データの軌道へ遷移する遷移軌道を生成する遷移軌道生成部と、
を含む、
軌道生成システム。
第１のタイミングで環境の三次元データを取得し、タスクに対応する前記目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データであって、前記第１のタイミングで取得した前記三次元データに基づく前記環境と接触しない掃引空間を有する動作データを前記記憶部から複数抽出する第１動作データ抽出部と、
前記第１動作データ抽出部が抽出した前記複数の動作データの何れか１つである第１動作データに基づいて、前記タスクを開始するタスク開始部と、
前記環境の三次元データを前記第１のタイミングよりも後にリアルタイムに取得することにより、前記モバイルマニピュレータが前記タスクを完了するまでの間にリアルタイムに取得した前記三次元データに基づく環境と前記モバイルマニピュレータが接触するか否か判定する接触判定部と、
接触すると前記接触判定部が判定した場合、前記第１動作データ抽出部が抽出した前記複数の動作データから、リアルタイムに取得した前記三次元データに基づく環境と接触しない掃引空間を有する動作データを第２動作データとして抽出する第２動作データ抽出部と、
前記第１動作データの軌道から前記第２動作データの軌道へ遷移する遷移軌道を生成する遷移軌道生成部と、
を含む、
軌道生成システム。
【請求項2】
前記遷移軌道の軌道始点と軌道終点の間の差分は所定値未満である、
請求項１に記載の軌道生成システム。
請求項１に記載の軌道生成システム。
【請求項3】
前記遷移軌道の軌道始点から見た前記遷移軌道の軌道終点の方向と、前記第２動作データの軌道の遷移完了直後の方向と、が成す角度が所定値未満である、
請求項１に記載の軌道生成システム。
請求項１に記載の軌道生成システム。
【請求項4】
前記遷移軌道の軌道終点は、
前記遷移軌道の軌道始点と前記軌道終点の間の差分が所定値未満となる第１の条件と、
前記軌道始点から見た前記軌道終点の方向と、前記第２動作データの軌道の遷移完了直後の方向と、が成す角度が所定値未満となる第２の条件と、
を同時に満たす複数の軌道点のうち最も下流側の軌道点である、
請求項１に記載の軌道生成システム。
前記遷移軌道の軌道始点と前記軌道終点の間の差分が所定値未満となる第１の条件と、
前記軌道始点から見た前記軌道終点の方向と、前記第２動作データの軌道の遷移完了直後の方向と、が成す角度が所定値未満となる第２の条件と、
を同時に満たす複数の軌道点のうち最も下流側の軌道点である、
請求項１に記載の軌道生成システム。
【請求項5】
前記遷移軌道生成部は、前記モバイルマニピュレータの連続する２つの軌道点の差分を小さくするための第１コスト関数と、リアルタイムに取得した前記三次元データに基づく環境を回避するための第２コスト関数と、に基づく二次計画法により前記遷移軌道を生成する、
請求項１に記載の軌道生成システム。
請求項１に記載の軌道生成システム。
【請求項6】
前記遷移軌道生成部は、前記モバイルマニピュレータの連続する２つの軌道点の差分を、前記モバイルマニピュレータがリアルタイムに取得した前記三次元データに基づく環境を回避するための回避量に基づいて重み付けし、重み付け後の差分の総和を求めることにより前記第１コスト関数を演算する、
請求項５に記載の軌道生成システム。
請求項５に記載の軌道生成システム。
【請求項7】
前記遷移軌道生成部は、前記モバイルマニピュレータとリアルタイムに取得した前記三次元データに基づく環境との重なり量の対数に基づいて、前記モバイルマニピュレータがリアルタイムに取得した前記三次元データに基づく環境を回避するための回避量を演算する、
請求項５に記載の軌道生成システム。
請求項５に記載の軌道生成システム。
【請求項8】
コンピュータが、目標手先位置姿勢と、モバイルマニピュレータの手先位置姿勢を当該目標手先位置姿勢に移動させるための前記モバイルマニピュレータの軌道と、当該軌道に従って前記モバイルマニピュレータを動作させるときに前記モバイルマニピュレータが描く三次元形状である掃引空間と、を含む動作データを複数記憶する記憶部を参照して軌道を生成する軌道生成方法であって、
第１のタイミングで環境の三次元データを取得し、タスクに対応する前記目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データであって、前記第１のタイミングで取得した前記三次元データに基づく前記環境と接触しない掃引空間を有する動作データを前記記憶部から複数抽出し、
前記複数の動作データの何れか１つである第１動作データに基づいて、前記タスクを開始し、
前記環境の三次元データを前記第１のタイミングよりも後にリアルタイムに取得することにより、前記モバイルマニピュレータが前記タスクを完了するまでの間にリアルタイムに取得した前記三次元データに基づく環境と前記モバイルマニピュレータが接触するか否か判定し、
接触すると判定した場合、前記複数の動作データから、リアルタイムに取得した前記三次元データに基づく環境と接触しない掃引空間を有する動作データを第２動作データとして抽出し、
前記第１動作データの軌道から前記第２動作データの軌道へ遷移する遷移軌道を生成する、
軌道生成方法。
第１のタイミングで環境の三次元データを取得し、タスクに対応する前記目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データであって、前記第１のタイミングで取得した前記三次元データに基づく前記環境と接触しない掃引空間を有する動作データを前記記憶部から複数抽出し、
前記複数の動作データの何れか１つである第１動作データに基づいて、前記タスクを開始し、
前記環境の三次元データを前記第１のタイミングよりも後にリアルタイムに取得することにより、前記モバイルマニピュレータが前記タスクを完了するまでの間にリアルタイムに取得した前記三次元データに基づく環境と前記モバイルマニピュレータが接触するか否か判定し、
接触すると判定した場合、前記複数の動作データから、リアルタイムに取得した前記三次元データに基づく環境と接触しない掃引空間を有する動作データを第２動作データとして抽出し、
前記第１動作データの軌道から前記第２動作データの軌道へ遷移する遷移軌道を生成する、
軌道生成方法。
【請求項9】
コンピュータに、請求項８に記載の軌道生成方法を実行させるプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、軌道生成システム、軌道生成方法、及び、プログラムに関する。
【背景技術】
【0002】
特許文献１は、モバイルマニピュレータの軌道生成装置を開示している。具体的には、複数の動作データを格納するデータベースを利用する。
【0003】
各動作データは、目標手先位置姿勢と、モバイルマニピュレータの手先位置姿勢を当該目標手先位置姿勢に移動させるためのモバイルマニピュレータの軌道と、当該軌道に従ってモバイルマニピュレータを動作させるときのモバイルマニピュレータが横切る空間である掃引空間（Swept Volume：ＳＶ)と、の組み合わせから構成される。
【0004】
モバイルマニピュレータのタスク開始時においては、サービス環境の三次元点群データを取得し、タスクに対応する目標手先位置姿勢をキーとしてデータベース内で検索を実行し、タスクに対応する目標手先位置姿勢に近い目標手先位置姿勢を有する動作データであって、サービス環境と干渉しない掃引空間を有する動作データを抽出する。
【0005】
そして、モバイルマニピュレータは、抽出した動作データの軌道に従って動作する。
【先行技術文献】
【特許文献】
【0006】
【特許文献1】特開２０２１－０２４０１９号公報
【発明の概要】
【発明が解決しようとする課題】
【0007】
しかし、上記特許文献１の構成では、モバイルマニピュレータがタスクを開始した後のモバイルマニピュレータの移動中に、モバイルマニピュレータと接触する虞のある未知の障害物が存在しないことを前提としている。
【0008】
従って、モバイルマニピュレータがタスクを実行した後のモバイルマニピュレータの移動中に、モバイルマニピュレータが時々刻々と変化するサービス環境に接触すると判定した場合、データベースから適切な動作データを抽出する抽出処理を改めて実行しなければならないだろう。即ち、当該抽出処理が完了するまで、モバイルマニピュレータは長時間の待機を余儀なくされる。
【0009】
本発明の目的は、モバイルマニピュレータがサービス環境に接触すると判定したとき、当該サービス環境との接触を回避しつつタスクを完遂できるモバイルマニピュレータの軌道を短時間で生成する技術を提供することにある。
【課題を解決するための手段】
【0010】
本開示の第１の観点によれば、
目標手先位置姿勢と、モバイルマニピュレータの手先位置姿勢を当該目標手先位置姿勢に移動させるための前記モバイルマニピュレータの軌道と、当該軌道に従って前記モバイルマニピュレータを動作させるときに前記モバイルマニピュレータが横切る空間である掃引空間と、を含む動作データを複数記憶する記憶部と、
第１のタイミングで環境の三次元データを取得し、タスクに対応する前記目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データであって、前記第１のタイミングで取得した前記三次元データに基づく前記環境と接触しない掃引空間を有する動作データを前記記憶部から複数抽出する第１動作データ抽出部と、
前記第１動作データ抽出部が抽出した前記複数の動作データの何れか１つである第１動作データに基づいて、前記タスクを開始するタスク開始部と、
前記環境の三次元データを前記第１のタイミングよりも後にリアルタイムに取得することにより、前記モバイルマニピュレータが前記タスクを完了するまでの間にリアルタイムに取得した前記三次元データに基づく環境と前記モバイルマニピュレータが接触するか否か判定する接触判定部と、
接触すると前記接触判定部が判定した場合、前記第１動作データ抽出部が抽出した前記複数の動作データから、リアルタイムに取得した前記三次元データに基づく環境と接触しない掃引空間を有する動作データを第２動作データとして抽出する第２動作データ抽出部と、
前記第１動作データの軌道から前記第２動作データの軌道へ遷移する遷移軌道を生成する遷移軌道生成部と、
を含む、
軌道生成システムが提供される。
以上の構成によれば、モバイルマニピュレータがタスクを開始した後のモバイルマニピュレータの移動中に、モバイルマニピュレータがサービス環境に接触すると判定したとき、当該サービス環境との接触を回避しつつタスクを完遂できるモバイルマニピュレータの軌道を短時間で生成することができる。
目標手先位置姿勢と、モバイルマニピュレータの手先位置姿勢を当該目標手先位置姿勢に移動させるための前記モバイルマニピュレータの軌道と、当該軌道に従って前記モバイルマニピュレータを動作させるときに前記モバイルマニピュレータが横切る空間である掃引空間と、を含む動作データを複数記憶する記憶部と、
第１のタイミングで環境の三次元データを取得し、タスクに対応する前記目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データであって、前記第１のタイミングで取得した前記三次元データに基づく前記環境と接触しない掃引空間を有する動作データを前記記憶部から複数抽出する第１動作データ抽出部と、
前記第１動作データ抽出部が抽出した前記複数の動作データの何れか１つである第１動作データに基づいて、前記タスクを開始するタスク開始部と、
前記環境の三次元データを前記第１のタイミングよりも後にリアルタイムに取得することにより、前記モバイルマニピュレータが前記タスクを完了するまでの間にリアルタイムに取得した前記三次元データに基づく環境と前記モバイルマニピュレータが接触するか否か判定する接触判定部と、
接触すると前記接触判定部が判定した場合、前記第１動作データ抽出部が抽出した前記複数の動作データから、リアルタイムに取得した前記三次元データに基づく環境と接触しない掃引空間を有する動作データを第２動作データとして抽出する第２動作データ抽出部と、
前記第１動作データの軌道から前記第２動作データの軌道へ遷移する遷移軌道を生成する遷移軌道生成部と、
を含む、
軌道生成システムが提供される。
以上の構成によれば、モバイルマニピュレータがタスクを開始した後のモバイルマニピュレータの移動中に、モバイルマニピュレータがサービス環境に接触すると判定したとき、当該サービス環境との接触を回避しつつタスクを完遂できるモバイルマニピュレータの軌道を短時間で生成することができる。
【0011】
前記遷移軌道の軌道始点と軌道終点の間の差分は所定値未満であってもよい。以上の構成によれば、前記遷移軌道を生成する生成処理が安定化する。
【0012】
前記遷移軌道の軌道始点から見た前記遷移軌道の軌道終点の方向と、前記第２動作データの軌道の遷移完了直後の方向と、が成す角度が所定値未満であってもよい。以上の構成によれば、前記遷移軌道から前記第２動作データの軌道に乗り換えるときの前記モバイルマニピュレータの滑らかな動作を実現できる。
【0013】
前記遷移軌道の軌道終点は、
前記遷移軌道の軌道始点と前記軌道終点の間の差分が所定値未満となる第１の条件と、
前記軌道始点から見た前記軌道終点の方向と、前記第２動作データの軌道の遷移完了直後の方向と、が成す角度が所定値未満となる第２の条件と、
を同時に満たす複数の軌道点のうち最も下流側の軌道点であってもよい。
以上の構成によれば、前記遷移軌道を生成する生成処理が安定化し、前記遷移軌道から前記第２動作データの軌道に乗り換えるときの前記モバイルマニピュレータの滑らかな動作を実現できると共に、前記モバイルマニピュレータがタスクを実行するための所要時間を短縮することができる。
前記遷移軌道の軌道始点と前記軌道終点の間の差分が所定値未満となる第１の条件と、
前記軌道始点から見た前記軌道終点の方向と、前記第２動作データの軌道の遷移完了直後の方向と、が成す角度が所定値未満となる第２の条件と、
を同時に満たす複数の軌道点のうち最も下流側の軌道点であってもよい。
以上の構成によれば、前記遷移軌道を生成する生成処理が安定化し、前記遷移軌道から前記第２動作データの軌道に乗り換えるときの前記モバイルマニピュレータの滑らかな動作を実現できると共に、前記モバイルマニピュレータがタスクを実行するための所要時間を短縮することができる。
【0014】
前記遷移軌道生成部は、前記モバイルマニピュレータの連続する２つの軌道点の差分を小さくするための第１コスト関数と、リアルタイムに取得した前記三次元データに基づく環境を回避するための第２コスト関数と、に基づく二次計画法により前記遷移軌道を生成してもよい。
以上の構成によれば、前記遷移軌道を合理的に生成することができる。
以上の構成によれば、前記遷移軌道を合理的に生成することができる。
【0015】
前記遷移軌道生成部は、前記モバイルマニピュレータの連続する２つの軌道点の差分を、前記モバイルマニピュレータがリアルタイムに取得した前記三次元データに基づく環境を回避するための回避量に基づいて重み付けし、重み付け後の差分の総和を求めることにより前記第１コスト関数を演算してもよい。
以上の構成によれば、前記モバイルマニピュレータとリアルタイムに取得した三次元点群データに基づくサービス環境との接触を回避するように遷移軌道を生成するに際し、連続する２つの軌道点が過度に離れてしまうことを抑制できる。
以上の構成によれば、前記モバイルマニピュレータとリアルタイムに取得した三次元点群データに基づくサービス環境との接触を回避するように遷移軌道を生成するに際し、連続する２つの軌道点が過度に離れてしまうことを抑制できる。
【0016】
前記遷移軌道生成部は、前記モバイルマニピュレータとリアルタイムに取得した前記三次元データに基づく環境との重なり量の対数に基づいて、前記モバイルマニピュレータがリアルタイムに取得した前記三次元データに基づく環境を回避するための回避量を演算してもよい。
以上の構成によれば、前記二次計画法による前記遷移軌道の生成処理が安定化する。
以上の構成によれば、前記二次計画法による前記遷移軌道の生成処理が安定化する。
【0017】
本開示の第２の観点によれば、
コンピュータが、目標手先位置姿勢と、モバイルマニピュレータの手先位置姿勢を当該目標手先位置姿勢に移動させるための前記モバイルマニピュレータの軌道と、当該軌道に従って前記モバイルマニピュレータを動作させるときに前記モバイルマニピュレータが横切る空間である掃引空間と、を含む動作データを複数記憶する記憶部を参照して軌道を生成する軌道生成方法であって、
第１のタイミングで環境の三次元データを取得し、タスクに対応する前記目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データであって、前記第１のタイミングで取得した前記三次元データに基づく前記環境と接触しない掃引空間を有する動作データを前記記憶部から複数抽出し、
前記複数の動作データの何れか１つである第１動作データに基づいて、前記タスクを開始し、
前記環境の三次元データを前記第１のタイミングよりも後にリアルタイムに取得することにより、前記モバイルマニピュレータが前記タスクを完了するまでの間にリアルタイムに取得した前記三次元データに基づく環境と前記モバイルマニピュレータが接触するか否か判定し、
接触すると判定した場合、前記複数の動作データから、リアルタイムに取得した前記三次元データに基づく環境と接触しない掃引空間を有する動作データを第２動作データとして抽出し、
前記第１動作データの軌道から前記第２動作データの軌道へ遷移する遷移軌道を生成する、
軌道生成方法が提供される。
以上の方法によれば、モバイルマニピュレータがタスクを開始した後のモバイルマニピュレータの移動中に、モバイルマニピュレータがサービス環境に接触すると判定したとき、当該サービス環境との接触を回避しつつタスクを完遂できるモバイルマニピュレータの軌道を短時間で生成することができる。
コンピュータが、目標手先位置姿勢と、モバイルマニピュレータの手先位置姿勢を当該目標手先位置姿勢に移動させるための前記モバイルマニピュレータの軌道と、当該軌道に従って前記モバイルマニピュレータを動作させるときに前記モバイルマニピュレータが横切る空間である掃引空間と、を含む動作データを複数記憶する記憶部を参照して軌道を生成する軌道生成方法であって、
第１のタイミングで環境の三次元データを取得し、タスクに対応する前記目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データであって、前記第１のタイミングで取得した前記三次元データに基づく前記環境と接触しない掃引空間を有する動作データを前記記憶部から複数抽出し、
前記複数の動作データの何れか１つである第１動作データに基づいて、前記タスクを開始し、
前記環境の三次元データを前記第１のタイミングよりも後にリアルタイムに取得することにより、前記モバイルマニピュレータが前記タスクを完了するまでの間にリアルタイムに取得した前記三次元データに基づく環境と前記モバイルマニピュレータが接触するか否か判定し、
接触すると判定した場合、前記複数の動作データから、リアルタイムに取得した前記三次元データに基づく環境と接触しない掃引空間を有する動作データを第２動作データとして抽出し、
前記第１動作データの軌道から前記第２動作データの軌道へ遷移する遷移軌道を生成する、
軌道生成方法が提供される。
以上の方法によれば、モバイルマニピュレータがタスクを開始した後のモバイルマニピュレータの移動中に、モバイルマニピュレータがサービス環境に接触すると判定したとき、当該サービス環境との接触を回避しつつタスクを完遂できるモバイルマニピュレータの軌道を短時間で生成することができる。
【0018】
コンピュータに、上記の軌道生成方法を実行させるプログラムが提供される。
【発明の効果】
【0019】
本開示によれば、モバイルマニピュレータがタスクを開始した後のモバイルマニピュレータの移動中に、モバイルマニピュレータがサービス環境に接触すると判定したとき、当該サービス環境との接触を回避しつつタスクを完遂できるモバイルマニピュレータの軌道を短時間で生成することができる。
【図面の簡単な説明】
【0020】
【図1】モバイルマニピュレータの斜視図である。
【図2】モバイルマニピュレータの機能ブロック図である。
【図3】管理サーバの機能ブロック図である。
【図4】基本データベースのデータ構造図である。
【図5】候補データベースのデータ構造図である。
【図6】初期ＳＶ、回避ＳＶ、遷移ＳＶを対比させるための図である。
【図7】回避方向の説明図である。
【図8】探索範囲の説明図である。
【図9】軌道点と障害物との位置関係を示す図である。
【図10】軌道生成システムの制御フローである。
【発明を実施するための形態】
【0021】
以下、本開示を適用した具体的な実施形態について、図面を参照しながら詳細に説明する。ただし、本開示が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載および図面は、適宜、簡略化されている。
【0022】
まず、図１を参照して、モバイルマニピュレータ１を説明する。モバイルマニピュレータ１は、いわゆるＨＳＲ（Human Support Robot）である。モバイルマニピュレータ１は、食器の後片付けをするタスクや、収納棚から物品を取り出すタスク、物品を収容棚に収容するタスク、その他のタスクを自律的に実行可能に構成されている。
【0023】
モバイルマニピュレータ１は、上記のタスクを実行するために、移動台車２と、移動台車２の上に搭載されるロボット３と、を含む。
【0024】
移動台車２は、典型的には、複数の車輪と駆動モータを備える。駆動モータにより複数の車輪の少なくとも１つを回転させることにより、モバイルマニピュレータ１のサービス環境内での移動が実現される。
【0025】
サービス環境は、環境の一具体例である。環境は、モバイルマニピュレータ１から見た外部環境と言及することもできる。
【0026】
ロボット３は、ロボット本体４と、ロボット本体４によって支持される多関節型のマニピュレータ５と、マニピュレータ５の先端に設けられるエンドエフェクタ６と、環境センサ７と、制御部８と、を備える。
【0027】
マニピュレータ５は、上腕リンク９と前腕リンク１０を有する。上腕リンク９は、ロボット本体４に肩関節１１を介して連結している。上腕リンク９と前腕リンク１０は肘関節１２を介して連結している。上腕リンク９の先端には、上記のエンドエフェクタ６が手首関節１３を介して連結している。
【0028】
環境センサ７は、モバイルマニピュレータ１のサービス環境を走査することにより、サービス環境の三次元点群データを制御部８に出力する。環境センサ７は、典型的には、ＲＧＢ－Ｄカメラ、ステレオカメラ、レーザレンジファインダ、超音波センサである。三次元点群データは、三次元データの一具体例である。環境センサ７は、モバイルマニピュレータ１に搭載されることに代えて、サービス環境に設置されてもよい。モバイルマニピュレータ１が環境センサ７を備えることに加えて、サービス環境に別の環境センサが設置されてもよい。
【0029】
本実施形態において、軌道生成システム１００は、モバイルマニピュレータ１と管理サーバ８０を含む。軌道生成システム１００は、モバイルマニピュレータ１と管理サーバ８０による分散処理により成立している。しかし、管理サーバ８０を省略してもよい。この場合、軌道生成システム１００は、モバイルマニピュレータ１単独により実現されることになる。
【0030】
図２は、モバイルマニピュレータ１の機能ブロック図を示している。図２に示すように、制御部８は、CPU８ａ（Central Processing Unit）と、RAM（Random Access Memory）やROM（Read Only Memory）、SSD（Solid State Drive）を含むメモリ８ｂ、通信インターフェース８ｃを備えている。そして、CPU８ａがメモリ８ｂに記憶されている制御プログラムを読み出して実行することで、制御プログラムは、CPU８ａなどのハードウェアを、タスク準備部２０、タスク実行部２１、接触判定部２２、回避動作データ抽出部２３、遷移軌道生成部２４、軌道更新部２５、として機能させる。
【0031】
図３は、管理サーバ８０の機能ブロック図を示している。図３に示すように、管理サーバ８０は、CPU８０ａ（Central Processing Unit）と、RAM（Random Access Memory）やROM（Read Only Memory）、SSD（Solid State Drive）を含むメモリ８０ｂ、通信インターフェース８０ｃを備えている。そして、CPU８０ａがメモリ８０ｂに記憶されている制御プログラムを読み出して実行することで、制御プログラムは、CPU８０ａなどのハードウェアを、データベース生成部８１、基本データベース８２、データ受信部８３、候補動作データ抽出部８５、候補データベース８６、として機能させる。
【0032】
図２に示すモバイルマニピュレータ１の制御部８は、通信インターフェース８ｃを介して管理サーバ８０と双方向通信可能である。図３に示す管理サーバ８０は、通信インターフェース８０ｃを介してモバイルマニピュレータ１と双方向通信可能である。
【0033】
図３に示すデータベース生成部８１は、基本データベース８２を生成する。データベース生成部８１は、モバイルマニピュレータ１の運用を開始する準備段階で基本データベース８２を生成する。即ち、モバイルマニピュレータ１がタスクを開始する時点では基本データベース８２が既に生成されているものとする。基本データベース８２は、記憶部の一具体例である。
【0034】
図４には、基本データベース８２のデータ構造を示している。図４に示すように、基本データベース８２は、目標手先位置姿勢ｙと、モバイルマニピュレータ１の手先位置姿勢を当該目標手先位置姿勢ｙに移動させるためのモバイルマニピュレータ１の軌道Ｑと、当該軌道Ｑに従ってモバイルマニピュレータ１を動作させるときのモバイルマニピュレータ１が横切る空間である掃引空間（swept volume）（以下、単にＳＶと称する。）と、を含む動作データを複数記憶するデータベースである。本実施形態において、基本データベース８２は、Ｎ個の動作データを記憶している。
【0035】
手先位置姿勢とは、サービス環境に固定されたサービス座標系におけるエンドエフェクタ６の位置及び姿勢を意味する。
【0036】
軌道Ｑは、複数の軌道点ｑから構成されている。複数の軌道点ｑは、モバイルマニピュレータ１の動作を所定のタイムステップで離散化したものである。各軌道点ｑは、以下の式で示すように定義される。ここで、下記式におけるθは、モバイルマニピュレータ１のマニピュレータ５の複数の関節の関節角度、モバイルマニピュレータ１の移動台車２のサービス座標系における位置及び向きを表す変数である。従って、各軌道点ｑは、サービス環境内におけるモバイルマニピュレータ１の状態を一義的に表現している。下記式においてＮは、モバイルマニピュレータ１のモバイルマニピュレータ１の自由度と移動台車２の自由度の合計である。
【0037】
【0038】
ＳＶ(Swept Volume)は、軌道Ｑに従ってモバイルマニピュレータ１を動作させるときにモバイルマニピュレータ１が描く三次元形状である。換言すれば、ＳＶは、軌道Ｑに従ってモバイルマニピュレータ１を動作させるときにモバイルマニピュレータ１が横切る空間である。本実施形態においてＳＶは、ボクセル法により表現されている。ボクセル法とは、空間や物の形状を同一の小さな立方体の組み合わせにより表現する手法である。
【0039】
データベース生成部８１は、典型的には、以下のような手順により基本データベース８２を生成する。即ち、まず、データベース生成部８１は、例えばＧａｚｅｂｏシミュレータなどのシミュレータでサービス環境を構築する。このとき、データベース生成部８１は、サービス環境内に机や棚、物品等の障害物をランダムに配置する。次に、データベース生成部８１は、目標手先位置姿勢ｙをランダムに設定し、当該目標手先位置姿勢ｙに至るまでの軌道Ｑを生成する。データベース生成部８１は、目標手先位置姿勢ｙを所定のルールに基づいて設定してもよい。データベース生成部８１は、例えば、モバイルマニピュレータ１の前方エリア内で目標手先位置姿勢ｙをランダムに設定してもよい。また、データベース生成部８１は、目標手先位置姿勢ｙをユーザー入力に基づいて設定してもよい。データベース生成部８１は、典型的には、ＣＢｉＲＲＴ（Constrained Bi-directional Rapidly-Exploring Random Tree）に基づいて軌道Ｑを生成する。具体的には、データベース生成部８１は、シミュレータ上でモバイルマニピュレータ１の環境センサ７から出力される三次元点群データを回避するように軌道Ｑを設定する。そして、データベース生成部８１は、目標手先位置姿勢ｙと軌道Ｑ、ＳＶを互いに関連付けた動作データを生成し、生成した動作データを基本データベース８２に格納する。一例として、データベース生成部８１は、数万通りの動作データを生成し、基本データベース８２に格納する。
【0040】
図２に戻り、モバイルマニピュレータ１のタスク準備部２０は、ユーザー端末からタスクデータを取得することをトリガーに、環境センサ７を制御することで、モバイルマニピュレータ１がタスクを開始する時点でのサービス環境の三次元点群データを環境センサ７から取得する。そして、タスク準備部２０は、受信した三次元点群データを管理サーバ８０に送信すると共に、タスクに対応する目標手先位置姿勢を目標データとして管理サーバ８０に送信する。
【0041】
図３に示す管理サーバ８０のデータ受信部８３は、モバイルマニピュレータ１から三次元点群データ及び目標データを受信する。
【0042】
候補動作データ抽出部８５は、第１動作データ抽出部の一具体例である。候補動作データ抽出部８５は、基本データベース８２から、目標データが示す目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データを抽出する。ここで、タスク毎に目標手先位置姿勢は千差万別となることから、目標データが示す目標手先位置姿勢と完全に一致する目標手先位置姿勢を有する動作データは現実として存在しない。従って、候補動作データ抽出部８５は、基本データベース８２から目標データが示す目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データを抽出するに際し、目標データが示す目標手先位置姿勢に近い目標手先位置姿勢を有する動作データを基本データベース８２から抽出する。具体的には、候補動作データ抽出部８５は、目標データが示す目標手先位置姿勢と、動作データの目標手先位置姿勢と、の差分が所定量未満であるとき、当該動作データを基本データベース８２から抽出する。所定量とは、典型的には、目標手先位置について言えば３０ｃｍであり、目標手先姿勢について９０度である。候補動作データ抽出部８５は、目標データが示す目標手先位置姿勢を入力すると、基本データベース８２から目標データが示す目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データを抽出するように学習された学習済みのニューラルネットワークにより構築してもよい。
【0043】
候補動作データ抽出部８５は、基本データベース８２から抽出した複数の動作データを更に絞る。具体的には、候補動作データ抽出部８５は、基本データベース８２から抽出した複数の動作データから、タスクの開始時におけるサービス環境と接触しないＳＶを有する動作データを複数抽出する。このとき、候補動作データ抽出部８５は、抽出に先立って、タスクの開始時におけるサービス環境を示す三次元点群データをボクセルデータに変換する。これにより、候補動作データ抽出部８５は、サービス環境を示すボクセルデータと、各動作データが有するＳＶのボクセルデータと、の物理的な干渉を極めて高速に判定することができる。候補動作データ抽出部８５は、こうして抽出した複数の動作データを候補データベース８６に候補動作データとして格納する。図５には、候補データベース８６のデータ構造の一例を示している。図５に示すように、候補動作データ抽出部８５は、一例として、数十通りの候補動作データを基本データベース８２から抽出し、候補データベース８６に格納する。このように候補データベース８６に格納されている複数の候補動作データのうちどの候補動作データを使用しても、モバイルマニピュレータ１は、タスクの開始時におけるサービス環境と接触することなくタスクを完遂することができる。
【0044】
ただし、候補動作データ抽出部８５による抽出は、あくまでモバイルマニピュレータ１がタスクを開始する時点で取得したサービス環境の三次元点群データに基づいている。この三次元点群データは、モバイルマニピュレータ１がタスクを開始する時点でモバイルマニピュレータ１から観測できたサービス環境の一部を表しているに過ぎない。従って、モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中にサービス環境に接触してしまう虞がある。本実施形態では、後述するように、モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中にサービス環境に接触してしまう虞がある場合、モバイルマニピュレータ１は、候補データベース８６を再度参照してモバイルマニピュレータ１の軌道を修正する。
【0045】
モバイルマニピュレータ１がタスクを開始する時点とは、第１のタイミングの一具体例である。第１のタイミングは、モバイルマニピュレータ１がタスクを開始する時点であることに代えて、モバイルマニピュレータ１がタスクを開始する時点よりも少し前のタイミングであってもよく、タスク開始時よりも少し後のタイミングであってもよい。
【0046】
なお、候補動作データ抽出部８５は、環境センサ７がサービス環境を測定することで環境センサ７が生成した三次元点群データを環境センサ７から取得する。従って、候補動作データ抽出部８５が環境センサ７を含むように解釈してはならない。
【0047】
図２に戻り、タスク実行部２１は、タスク開始部の一具体例である。タスク実行部２１は、管理サーバ８０の候補動作データ抽出部８５が抽出した複数の候補動作データの何れか１つである初期動作データに基づいてタスクを開始する。具体的には、タスク実行部２１は、管理サーバ８０の候補データベース８６を参照し、候補データベース８６に格納されている複数の候補動作データから１つの候補動作データを初期動作データとして選択する。このとき、タスク実行部２１は、典型的には、複数の候補動作データのうち、当該候補動作データの目標手先位置姿勢がタスクに対応する目標手先位置姿勢に最も近いものを選択する。タスク実行部２１は、初期動作データの軌道Ｑに従ってモバイルマニピュレータ１の移動台車２及びマニピュレータ５、エンドエフェクタ６を制御する。初期動作データは、第１動作データの一具体例である。
【0048】
なお、初期動作データの目標手先位置姿勢と、タスクに対応する目標手先位置姿勢は前述するように完全には一致していない。従って、タスク実行部２１は、タスクを開始するに際し、初期動作データの目標手先位置姿勢と、タスクに対応する目標手先位置姿勢と、の差分を補完する差分補完軌道を生成してもよい。
【0049】
次に、接触判定部２２は、モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中にサービス環境の三次元点群データをリアルタイムに取得することにより、モバイルマニピュレータ１がタスクを完了するまでの間にモバイルマニピュレータ１が、リアルタイムに取得した三次元点群データに基づくサービス環境と接触するか否か判定する。換言すれば、接触判定部２２は、モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中にサービス環境の三次元点群データをリアルタイムに取得することにより、モバイルマニピュレータ１がタスクを完了するまでの間にモバイルマニピュレータ１が、「最新の」三次元点群データに基づくサービス環境と接触するか否か判定する。例えば、モバイルマニピュレータ１がタスクを開始した時点でモバイルマニピュレータ１の環境センサ７から見て死角だった領域に物品があり、当該物品が初期動作データの軌道Ｑ上にあるとする。この場合、モバイルマニピュレータ１が初期動作データの軌道Ｑに従って動作している限り、モバイルマニピュレータ１は、やがて当該物品と接触することになるだろう。
【0050】
モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中にサービス環境の三次元点群データをリアルタイムに取得することは、サービス環境の三次元データをモバイルマニピュレータ１がタスクを開始する時点よりも後にリアルタイムに取得することの一具体例である。即ち、接触判定部２２は、モバイルマニピュレータ１がタスクを開始した後であってモバイルマニピュレータ１の動作停止中にサービス環境の三次元点群データをリアルタイムに取得してもよい。
【0051】
接触判定部２２は、具体的には以下のように動作する。即ち、接触判定部２２は、環境センサ７を制御することにより、モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中に、所定のインターバルで、サービス環境の三次元点群データを取得する。そして、接触判定部２２は、高速判定のために、取得した三次元点群データをボクセルデータに変換する。接触判定部２２は、変換後のボクセルデータと、初期動作データのＳＶのボクセルデータと、を比較することにより、モバイルマニピュレータ１がタスクを完了するまでの間にモバイルマニピュレータ１が、リアルタイムに取得した三次元点群データに基づくサービス環境と接触するか否か判定する。
【0052】
なお、接触判定部２２は、環境センサ７がサービス環境を測定することで環境センサ７が生成した三次元点群データを環境センサ７から取得する。従って、接触判定部２２が環境センサ７を含むように解釈してはならない。
【0053】
接触すると接触判定部２２が判定した場合、回避動作データ抽出部２３は、管理サーバ８０の候補データベース８６を参照し、候補データベース８６に格納されている複数の候補動作データから、リアルタイムに取得した三次元点群データに基づくサービス環境と接触しないＳＶを有する候補動作データを回避動作データとして抽出する。換言すれば、回避動作データ抽出部２３は、管理サーバ８０の候補データベース８６を参照し、候補データベース８６に格納されている複数の候補動作データから、「最新の」三次元点群データに基づくサービス環境と接触しないＳＶを有する候補動作データを回避動作データとして抽出する。回避動作データ抽出部２３は、第２動作データ抽出部の一具体例である。回避動作データ抽出部２３は、具体的には、以下のように動作する。
【0054】
即ち、回避動作データ抽出部２３は、接触判定部２２が生成した最新のボクセルデータと、各候補動作データのＳＶのボクセルデータと、を比較することにより、複数の候補動作データから、リアルタイムに取得した三次元点群データに基づくサービス環境と接触しないＳＶを有する候補動作データを選択し、選択した候補動作データを回避動作データとして抽出する。なお、リアルタイムに取得した三次元点群データに基づくサービス環境と接触しないＳＶを有する候補動作データが複数ある場合、回避動作データ抽出部２３は、何らかのルールに従って、複数の候補動作データの何れか１つを選択することになるだろう。例えば、回避動作データ抽出部２３は、初期動作データのＳＶと最も近いＳＶを有する候補動作データを選択してもよい。回避動作データ抽出部２３は、リアルタイムに取得した最新の三次元点群データを入力すると、リアルタイムに取得した三次元点群データに基づくサービス環境と接触しないＳＶを有する候補動作データを回避動作データとして抽出するように学習された学習済みのニューラルネットワークにより構築してもよい。
【0055】
遷移軌道生成部２４は、初期動作データの軌道Ｑから回避動作データの軌道Ｑへ遷移する遷移軌道を二次計画法により生成する。遷移軌道の軌道始点は、初期動作データの軌道Ｑに含まれる複数の軌道点の何れかである。遷移軌道の軌道終点は、回避動作データの軌道Ｑに含まれる複数の軌道点の何れかである。図６には、初期動作データの軌道ＱのＳＶを初期ＳＶとして示し、回避動作データの軌道ＱのＳＶを回避ＳＶとして示し、遷移軌道のＳＶを遷移ＳＶとして示している。また、図６には、遷移軌道の軌道始点ｑｓｔａｒｔと軌道終点ｑｅｎｄを併せて示している。そして、図６には、モバイルマニピュレータ１がタスクを開始する時点ではサービス環境に存在せず、モバイルマニピュレータ１がタスクを開始した後に初めてサービス環境に出現した障害物を併せて図示している。遷移軌道生成部２４が遷移軌道を生成する詳細な演算処理は後述する。
【0056】
軌道更新部２５は、モバイルマニピュレータ１がタスクを開始してからタスクを完了するまでの軌道を、回避動作データの軌道Ｑと遷移軌道に基づいて更新する。具体的には、モバイルマニピュレータ１がタスクを開始してからタスクを完了するまでの軌道は、更新前では初期動作データの軌道Ｑであった。軌道更新部２５は、モバイルマニピュレータ１がタスクを開始してからタスクを完了するまでの軌道を、初期動作データの軌道Ｑと、遷移軌道と、回避動作データの軌道Ｑと、をこの記載順に連結した軌道とする。詳しくは、モバイルマニピュレータ１が遷移軌道の軌道始点ｑｓｔａｒｔに至るまではモバイルマニピュレータ１が初期動作データの軌道Ｑに従って動作するものとし、モバイルマニピュレータ１が遷移軌道の軌道始点ｑｓｔａｒｔに至ったらモバイルマニピュレータ１が遷移軌道に従って動作するものとし、モバイルマニピュレータ１が遷移軌道の軌道終点ｑｅｎｄに至ったらモバイルマニピュレータ１が回避動作データの軌道Ｑに従って動作するものとする。
【0057】
次に、遷移軌道生成部２４が遷移軌道を生成する生成処理を詳細に説明する。まず、遷移軌道生成部２４は、まず、遷移軌道の軌道始点ｑｓｔａｒｔ及び軌道終点ｑｅｎｄを決定し、次に、遷移軌道の軌道始点ｑｓｔａｒｔ及び軌道終点ｑｅｎｄを連結する遷移軌道を生成する。
【0058】
＜遷移軌道の軌道始点ｑｓｔａｒｔ＞
遷移軌道の軌道始点ｑｓｔａｒｔは、下記（１）及び（２）を同時に満たす軌道点とする。
（１）接触判定部２２により接触があると判定した時点から所定時間経過したときに、初期動作データの軌道Ｑに従って動作しているモバイルマニピュレータ１が到達する軌道点よりも下流の、初期動作データの軌道Ｑの軌道点であること。
（２）初期動作データの軌道Ｑに従って動作しているモバイルマニピュレータ１がリアルタイムに取得した三次元点群データに基づくサービス環境に接触するときの軌道点よりも上流の、初期動作データの軌道Ｑの軌道点であること。
遷移軌道の軌道始点ｑｓｔａｒｔは、下記（１）及び（２）を同時に満たす軌道点とする。
（１）接触判定部２２により接触があると判定した時点から所定時間経過したときに、初期動作データの軌道Ｑに従って動作しているモバイルマニピュレータ１が到達する軌道点よりも下流の、初期動作データの軌道Ｑの軌道点であること。
（２）初期動作データの軌道Ｑに従って動作しているモバイルマニピュレータ１がリアルタイムに取得した三次元点群データに基づくサービス環境に接触するときの軌道点よりも上流の、初期動作データの軌道Ｑの軌道点であること。
【0059】
即ち、遷移軌道の軌道始点ｑｓｔａｒｔを障害物の近くに設定すれば、遷移軌道を生成する生成処理のための時間を稼げるので、モバイルマニピュレータ１の移動を停止させることなくモバイルマニピュレータ１が遷移軌道に乗り移ることができる。これに対し、遷移軌道の軌道始点ｑｓｔａｒｔを障害物の遠くに設定すれば、空間的に滑らかな遷移軌道が実現される。
【0060】
＜遷移軌道の軌道終点ｑｅｎｄ＞
遷移軌道の軌道終点ｑｅｎｄをどのように決定するかは重要な課題である。即ち、モバイルマニピュレータ１が遷移軌道に従って動作するときに目標手先位置姿勢から遠ざかるように後退するようでは、モバイルマニピュレータ１がタスクを短時間で完遂できなくなる。従って、軌道更新部２５は、回避動作データの軌道Ｑに含まれる複数の軌道点から下記式に基づいて軌道終点ｑｅｎｄの候補を絞り込む。
遷移軌道の軌道終点ｑｅｎｄをどのように決定するかは重要な課題である。即ち、モバイルマニピュレータ１が遷移軌道に従って動作するときに目標手先位置姿勢から遠ざかるように後退するようでは、モバイルマニピュレータ１がタスクを短時間で完遂できなくなる。従って、軌道更新部２５は、回避動作データの軌道Ｑに含まれる複数の軌道点から下記式に基づいて軌道終点ｑｅｎｄの候補を絞り込む。
【0061】
【0062】
ここで、Q’は、軌道終点ｑｅｎｄの候補となり得る複数の軌道点の集合である。ｑｉは、回避動作データの軌道Ｑのｉ番目の軌道点である。Ｑは、回避動作データの軌道Ｑである。ｑｓｔａｒｔは、遷移軌道の軌道始点ｑｓｔａｒｔである。
【0063】
軌道終点ｑｅｎｄの候補を絞り込む第１の条件は、距離による制限である。即ち、軌道始点ｑｓｔａｒｔと軌道点ｑｉの間の差分が所定値ｄ以上である場合、当該軌道点ｑｉは候補から除外する。端的に言えば、軌道始点ｑｓｔａｒｔにおけるモバイルマニピュレータ１の位置と軌道終点ｑｅｎｄにおけるモバイルマニピュレータ１の位置が離れすぎている場合、二次計画法による安定した遷移軌道の生成が困難になるからである。裏を返せば、上記第１の条件を満たすように軌道終点ｑｅｎｄの候補を絞り込めば、二次計画法による安定した遷移軌道の生成が実現される。
【0064】
軌道終点ｑｅｎｄの候補を絞り込む第２の条件は、遷移軌道から回避動作データの軌道Ｑに乗り移るときのモバイルマニピュレータ１の滑らかな動作を実現するための条件である。即ち、遷移軌道の軌道始点ｑｓｔａｒｔから見た軌道点ｑｉの方向と、回避動作データの軌道Ｑの遷移完了直後の方向と、が成す角度が９０度を超える場合、当該軌道点ｑｉは候補から除外する。
【0065】
そして、遷移軌道生成部２４は、絞り込まれた集合Q’の中で最も下流側の軌道点ｑｉを軌道終点ｑｅｎｄに決定する。これにより、モバイルマニピュレータ１がタスクを実行するための所要時間を短縮することができる。仮に、上記式を満たす軌道点ｑｉが存在しない場合、遷移軌道生成部２４は、上記式を満たす軌道点ｑｉが出現するまで所定値ｄを徐々に大きくすればよい。
【0066】
＜遷移軌道の生成＞
次に、遷移軌道生成部２４は、遷移軌道の軌道始点ｑｓｔａｒｔ及び軌道終点ｑｅｎｄの間の遷移軌道を二次計画法により生成する。
次に、遷移軌道生成部２４は、遷移軌道の軌道始点ｑｓｔａｒｔ及び軌道終点ｑｅｎｄの間の遷移軌道を二次計画法により生成する。
【0067】
即ち、遷移軌道生成部２４は、下記式（１）で示す目的関数を最小化することで、遷移軌道の軌道始点ｑｓｔａｒｔ及び軌道終点ｑｅｎｄを連結する遷移軌道を生成する。
【0068】
【0069】
ここで、f(x)は、モバイルマニピュレータ１の動作を小さくするためのポテンシャル関数である。即ち、このポテンシャル関数は、モバイルマニピュレータの連続する２つの軌道点の差分を小さくするためのポテンシャル関数である。このポテンシャル関数は、第１コスト関数の一具体例である。また、g(x)は、障害物回避のためのペナルティ関数である。即ち、ペナルティ関数は、モバイルマニピュレータ１とリアルタイムに取得した三次元点群データに基づくサービス環境との干渉深さを小さくするためのペナルティ関数である。このペナルティ関数は、第２コスト関数の一具体例である。μは、任意の定数であって、２つの目的関数のバランスを調整する。
【0070】
上記式（１）の変数ｘは、遷移軌道ｘを意味する。遷移軌道ｘは、下記式のように、複数の軌道点ｑｉの集合である。
【0071】
【0072】
ここで、ｑｉは、遷移軌道ｘにおけるｉ番目の軌道点である。Ｔは、遷移軌道ｘのタイムステップ数である。遷移軌道生成部２４は、遷移軌道ｘのＳＶがリアルタイムに取得した三次元点群データに基づくサービス環境に接触しなくなるまで遷移軌道ｘを更新することにより、遷移軌道ｘを生成する。
【0073】
（ペナルティ関数）
図７は、ボクセル空間において、モバイルマニピュレータ１のリンクが障害物と干渉している様子と、この干渉時におけるモバイルマニピュレータ１のリンクの望ましい回避方向と、を示す図である。ここで、本実施形態の環境センサ７は、障害物の表面のみを検出するものであり、障害物の表面よりも内側の部分は検出できないことに留意されたい。以降、障害物の表面が存在するボクセルを障害物ボクセルと称し、モバイルマニピュレータ１のリンクが存在するボクセルをリンクボクセルと称する。障害物ボクセルとリンクボクセルの重なっている部分は図７に示す通りである。
図７は、ボクセル空間において、モバイルマニピュレータ１のリンクが障害物と干渉している様子と、この干渉時におけるモバイルマニピュレータ１のリンクの望ましい回避方向と、を示す図である。ここで、本実施形態の環境センサ７は、障害物の表面のみを検出するものであり、障害物の表面よりも内側の部分は検出できないことに留意されたい。以降、障害物の表面が存在するボクセルを障害物ボクセルと称し、モバイルマニピュレータ１のリンクが存在するボクセルをリンクボクセルと称する。障害物ボクセルとリンクボクセルの重なっている部分は図７に示す通りである。
【0074】
遷移軌道生成部２４は、障害物回避のためのペナルティ関数を決定するために、図７の回避方向vavoid(l,t)及びその方向への回避量sdltを求める。ここで、添え字のスモールＬはリンク番号であり、ｔはタイムステップの番号である。
【0075】
まず、遷移軌道生成部２４は、以下のような手順で上記の回避方向vavoid(l,t)を決定する。即ち、遷移軌道生成部２４は、回避方向vavoid(l,t)をボクセルの探索により決定する。ボクセルの探索は各リンクボクセルを中心とした所定の範囲にて行う。探索範囲はボクセルの一辺を１としたとき、中心のボクセルからL1ノルムがｄ以下となる範囲とする。ここでｄは任意の定数である。図８には、二次元平面における探索範囲を示している。遷移軌道生成部２４は、この探索を全てのリンクボクセルに対して行う。そして、遷移軌道生成部２４は、回避方向vavoid(l,t)を下記式（３）に従って決定する。
【0076】
【0077】
ここで、Mltは、tステップ目のスモールＬ個目のリンクについて探索を行ったとき、探索したボクセルが障害物ボクセルと重複した重なり数であり、viは探索範囲の中心のリンクボクセルから障害物ボクセルへの方向の単位ベクトルである。上記式（３）において、viと逆方向のベクトルを加算していき、その総和を自身のノルムで除算することで単位ベクトルに変換している。これにより、障害物が存在する方向から離れるための回避方向vavoid(l,t)が得られる。
【0078】
次に、遷移軌道生成部２４は、以下のような手順で、上記回避方向への回避量sdltを求める。即ち、障害物を避けるための回避量sdltは，障害物とリンクとの干渉の深度（干渉深さ）により求める方法が考えられるが、実際は障害物の表面のみしかボクセル化できない。この場合、単純な干渉計算では干渉の深度を算出することは難しい。そこで、リンクボクセルと重なっている障害物ボクセルの数Cltを用いて回避量sdltを決定する。Cltは図７を例にすると“重なり”で示すボクセルの総数に相当する。そして、遷移軌道生成部２４は、回避量sdltを下記式（４）に従って決定する。
【0079】
【0080】
ここで、ａとｂは任意の定数である。右辺で対数を使用している理由は、モバイルマニピュレータ１のリンクが障害物と大きく干渉する場合にCltの値が急激に大きくなることにより、回避量sdltが同様に過大となって最小化演算（最適化演算）が不安定になるのを防ぐためである。
【0081】
上記の回避方向vavoid(l,t)及び回避量sdltを用いて、ペナルティ関数g(x)は、下記式（５）で定義される。
【0082】
【0083】
ここで、Δrltは最適化後のリンクの位置変化量であり、下記式（６）で近似される。
【0084】
【0085】
ここで、qtダッシュは最適化前のリンクの関節角度であり、Jlt(qtダッシュ)はリンクの位置についてのヤコビ行列である。上記ペナルティ関数は、最適化によるリンクの位置変化量の回避方向vavoid(l,t)成分が回避量sdltに足りない量の総和となっている。つまり、リンクの位置変化量が設定した回避量sdltに違反した量ともいえる。これをペナルティ関数として二次計画法における目的関数とすることで、障害物と接触しない遷移軌道を生成することができる。
【0086】
（ポテンシャル関数）
一方で、動作のポテンシャル関数は、二つの役割を持っている。一つ目の役割は、モバイルマニピュレータ１の動作の最小化である。そして、二つ目の役割は、軌道点間の広がりを抑制することである。このポテンシャル関数は、軌道点間を自然長がゼロであるバネにより連結した際のバネに蓄えられるポテンシャルの関数として下記式（７）により定義される。
一方で、動作のポテンシャル関数は、二つの役割を持っている。一つ目の役割は、モバイルマニピュレータ１の動作の最小化である。そして、二つ目の役割は、軌道点間の広がりを抑制することである。このポテンシャル関数は、軌道点間を自然長がゼロであるバネにより連結した際のバネに蓄えられるポテンシャルの関数として下記式（７）により定義される。
【0087】
【0088】
上記式におけるωkは、ｋステップ目の軌道点とｋ＋１ステップ目の軌道点を連結するバネのバネ定数に相当する。このωkを設定する理由は、障害物回避のためのペナルティ関数g(x)の影響により図９に示すように障害物の近傍で軌道点間が広がってしまうことを抑制するためである。障害物回避のためのペナルティ関数では、各軌道点が障害物と接触しないように設定されるため、軌道点間が広がってしまった場合、軌道点間の動作を障害物と接触しないように生成することが困難となる場合がある。図９の例では、モバイルマニピュレータ１は、軌道点qu+1から軌道点qu+2に直線的に移動する際、障害物と接触してしまうだろう。そのため、広がりやすい軌道点間のばね定数ωkを大きくすることで障害物近傍での軌道点間の広がりを抑制する。互いから離れやすい２つの軌道点では、設定した回避量sdltが必然的に大きくなるため、障害物回避のためのペナルティ関数の影響を強く受けると考えられる。例えば、ｋステップ目の軌道点とｋ＋１ステップ目の軌道点の間が広がっている場合、回避量sdlkと回避量sdlk+1の値が大きいことが考えられる。よって、ωkを下記式（８）及び（９）に基づいて設定する。
【0089】
【0090】
【0091】
【0092】
ここで、ｃ及びｄは任意の定数である。また、上記式（９）のｍｉｎ関数は集合の中の最小の要素を抽出するものであり、σは標準偏差を意味する。ωkチルダは前後のステップの設定された回避量sdltを足し合わせたものである。これにより、障害物によるペナルティ関数の影響を受けた軌道点間のばね定数ωkを大きくすることができる。このばね定数ωkは、他のばね定数と比べ相対的に大きければよいため、最終的なばね定数ωkは、ωkチルダを正規化した値とすればよい。正規化することでばね定数ωkは過大にならない。また、上記式（９）は標準偏差による除算を含むため、全体的な回避量sdltの大きさの変化に影響され難い。そのため上記式（４）における定数ａ及びｂの影響を緩和し、上記の定数ｃ及びｄを含む各種のパラメータの調整が容易になる。
【0093】
（二次計画法への適用）
以下、障害物回避のためのペナルティ関数と動作のポテンシャル関数を二次計画法へ適応するための方法を説明する。まず、二次計画法の基本形は以下の通りである。
以下、障害物回避のためのペナルティ関数と動作のポテンシャル関数を二次計画法へ適応するための方法を説明する。まず、二次計画法の基本形は以下の通りである。
【0094】
【0095】
【0096】
まず、障害物回避のためのペナルティ関数g(x)について説明する。ペナルティ関数g(x)は、スラック変数sltを用いて目的関数に追加する。このとき、最適化の変数ｘは次のように拡張される。
【0097】
【0098】
そして、上記式（１２）の不等式制約に次のような制約を追加する。
【0099】
【0100】
【0101】
上の二つの式により、スラック変数sltの取り得る最小値は次のようになる。
【0102】
【0103】
これは、上記式（５）の各項と同じ値である。そして、障害物によるコスト関数g(xダッシュ)を以下のようにすることで、目的関数に追加可能な形に変形することができる。
【0104】
【0105】
ここで、0TNは，長さTxNの零ベクトルである。上記式は式（１１)のbTxに代入することで目的関数に追加することが出来る。
【0106】
次に動作のポテンシャル関数f(x)ついてである。前述したポテンシャル関数f(x)は、次のように表現できる。
【0107】
【0108】
【0109】
ここで、INxNはNxNの単位行列である。上記式は、式（１１）のAをKTKとすることで二次計画法の目的関数とすることができる。また、スラック変数による変数の拡張に対しては、変数の次元数と合わせるように行列Kに0を追加して拡張することで対応できる。
【0110】
最後に、上記式（１２）の制約にモバイルマニピュレータ１のマニピュレータ５の関節角度の上限と下限を設定することで、実行可能領域をモバイルマニピュレータ１の可動範囲に制約する。
【0111】
次に、図１０を参照して、軌道生成システム１００の動作フローを説明する。
【0112】
まず、管理サーバ８０のデータベース生成部８１は、基本データベース８２を生成する（Ｓ１００）。
【0113】
ユーザー端末９０でタスク入力が行われると（Ｓ１１０）、ユーザー端末９０は、タスクデータをモバイルマニピュレータ１に送信する（Ｓ１２０）。モバイルマニピュレータ１のタスク準備部２０は、ユーザー端末９０からタスクデータを受信したことをトリガーとして、サービス環境の三次元点群データを取得し（Ｓ１３０）、当該三次元点群データと、タスクに対応する目標手先位置姿勢を示す目標データと、を管理サーバ８０に送信する（Ｓ１４０）。
【0114】
管理サーバ８０の候補動作データ抽出部８５は、基本データベース８２から候補動作データを抽出し（Ｓ１５０）、抽出した候補動作データを候補データベース８６に格納する。
【0115】
次に、モバイルマニピュレータ１のタスク実行部２１は、候補データベース８６を参照し（Ｓ１６０）、複数の候補動作データの何れか１つである初期動作データに基づいてタスクを開始する（Ｓ１７０）。
【0116】
次に、モバイルマニピュレータ１の接触判定部２２は、モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中にサービス環境の三次元点群データを取得する（Ｓ１８０）。そして、接触判定部２２は、モバイルマニピュレータ１がタスクを完了するまでの間にモバイルマニピュレータ１がサービス環境と接触するか判定する（Ｓ１９０）。モバイルマニピュレータ１がタスクを完了するまでの間にモバイルマニピュレータ１がサービス環境と接触しないと接触判定部２２が判定した場合、接触判定部２２は、処理をＳ２４０に進める。一方、モバイルマニピュレータ１がタスクを完了するまでの間にモバイルマニピュレータ１がサービス環境と接触すると接触判定部２２が判定した場合、接触判定部２２は、処理をＳ２００に進める。
【0117】
ステップＳ２００において、回避動作データ抽出部２３は、管理サーバ８０の候補データベース８６を参照し（Ｓ２００）、候補データベース８６に格納されている複数の候補動作データから、サービス環境と接触しないＳＶを有する候補動作データを回避動作データとして抽出する（Ｓ２１０）。
【0118】
次に、遷移軌道生成部２４は、初期動作データの軌道Ｑから回避動作データの軌道Ｑへ遷移する遷移軌道を二次計画法により生成する（Ｓ２２０）。
【0119】
次に、軌道更新部２５は、モバイルマニピュレータ１がタスクを完了するまでの軌道を、回避動作データの軌道Ｑと遷移軌道に基づいて更新する（Ｓ２３０）。このとき、軌道更新部２５は、モバイルマニピュレータ１がタスクを完了するまでの軌道を更新するに際し、前述した差分補完軌道を改めて生成することになるだろう。
【0120】
そして、制御部８は、モバイルマニピュレータ１がタスクを完了したか判定する（Ｓ２４０）。ここで、タスクには、前述した差分補完軌道も含まれることに留意されたい。モバイルマニピュレータ１がタスクを完了したと制御部８が判定した場合、制御部８は、処理を終了する。一方、モバイルマニピュレータ１がタスクを完了していないと制御部８が判定した場合、制御部８は、処理をＳ１８０に戻す。以降、モバイルマニピュレータ１がサービス環境と接触することを接触判定部２２が判定する度に（Ｓ１９０）、ステップＳ２００からステップＳ２４０が繰り返されることになる。
【0121】
以上に、本開示の好適な実施形態を説明したが、上記実施形態は以下の特徴を有する。
軌道生成システム１００は、基本データベース８２（データベース）と、候補動作データ抽出部８５（第１動作データ抽出部）と、タスク実行部２１（タスク開始部）と、接触判定部２２と、回避動作データ抽出部２３（第２動作データ抽出部）と、遷移軌道生成部２４と、を含む。
基本データベース８２は、目標手先位置姿勢と、モバイルマニピュレータ１の手先位置姿勢を当該目標手先位置姿勢に移動させるためのモバイルマニピュレータ１の軌道Ｑと、当該軌道Ｑに従ってモバイルマニピュレータ１を動作させるときにモバイルマニピュレータ１が横切る空間としてのＳＶ(Swept Volume)と、を含む動作データを複数記憶する。
候補動作データ抽出部８５は、モバイルマニピュレータ１のタスクの開始時において、サービス環境の三次元データを取得し、タスクに対応する目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データであって、モバイルマニピュレータ１のタスクの開始時に取得した三次元データに基づくサービス環境と接触しないＳＶを有する動作データを候補動作データとして基本データベース８２から複数抽出する。
タスク実行部２１は、複数の候補動作データの何れか１つである初期動作データ（第１動作データ）に基づいて、タスクを開始する。
接触判定部２２は、モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中にサービス環境の三次元データをリアルタイムに取得することにより、モバイルマニピュレータ１がタスクを完了するまでの間にリアルタイムに取得した三次元データに基づくサービス環境とモバイルマニピュレータ１が接触するか否か判定する。
回避動作データ抽出部２３は、接触すると接触判定部２２が判定した場合、複数の候補動作データから、リアルタイムに取得した三次元データに基づくサービス環境と接触しないＳＶを有する動作データを回避動作データ（第２動作データ）として抽出する。
遷移軌道生成部２４は、初期動作データの軌道Ｑから回避動作データの軌道Ｑへ遷移する遷移軌道を生成する。
以上の構成によれば、モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中に、モバイルマニピュレータ１がサービス環境に接触すると判定したとき、当該サービス環境との接触を回避しつつタスクを完遂できるモバイルマニピュレータ１の軌道を短時間で生成することができる。なお、モバイルマニピュレータ１の軌道を短時間で生成できるのは、回避動作データ抽出部２３が回避動作データを抽出するに際し、基本データベース８２に格納されている複数の動作データから回避動作データを抽出するのではなく、候補動作データ抽出部８５が抽出した複数の候補動作データから回避動作データを抽出するからである。
軌道生成システム１００は、基本データベース８２（データベース）と、候補動作データ抽出部８５（第１動作データ抽出部）と、タスク実行部２１（タスク開始部）と、接触判定部２２と、回避動作データ抽出部２３（第２動作データ抽出部）と、遷移軌道生成部２４と、を含む。
基本データベース８２は、目標手先位置姿勢と、モバイルマニピュレータ１の手先位置姿勢を当該目標手先位置姿勢に移動させるためのモバイルマニピュレータ１の軌道Ｑと、当該軌道Ｑに従ってモバイルマニピュレータ１を動作させるときにモバイルマニピュレータ１が横切る空間としてのＳＶ(Swept Volume)と、を含む動作データを複数記憶する。
候補動作データ抽出部８５は、モバイルマニピュレータ１のタスクの開始時において、サービス環境の三次元データを取得し、タスクに対応する目標手先位置姿勢に対応する目標手先位置姿勢を有する動作データであって、モバイルマニピュレータ１のタスクの開始時に取得した三次元データに基づくサービス環境と接触しないＳＶを有する動作データを候補動作データとして基本データベース８２から複数抽出する。
タスク実行部２１は、複数の候補動作データの何れか１つである初期動作データ（第１動作データ）に基づいて、タスクを開始する。
接触判定部２２は、モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中にサービス環境の三次元データをリアルタイムに取得することにより、モバイルマニピュレータ１がタスクを完了するまでの間にリアルタイムに取得した三次元データに基づくサービス環境とモバイルマニピュレータ１が接触するか否か判定する。
回避動作データ抽出部２３は、接触すると接触判定部２２が判定した場合、複数の候補動作データから、リアルタイムに取得した三次元データに基づくサービス環境と接触しないＳＶを有する動作データを回避動作データ（第２動作データ）として抽出する。
遷移軌道生成部２４は、初期動作データの軌道Ｑから回避動作データの軌道Ｑへ遷移する遷移軌道を生成する。
以上の構成によれば、モバイルマニピュレータ１がタスクを開始した後のモバイルマニピュレータ１の移動中に、モバイルマニピュレータ１がサービス環境に接触すると判定したとき、当該サービス環境との接触を回避しつつタスクを完遂できるモバイルマニピュレータ１の軌道を短時間で生成することができる。なお、モバイルマニピュレータ１の軌道を短時間で生成できるのは、回避動作データ抽出部２３が回避動作データを抽出するに際し、基本データベース８２に格納されている複数の動作データから回避動作データを抽出するのではなく、候補動作データ抽出部８５が抽出した複数の候補動作データから回避動作データを抽出するからである。
【0122】
また、遷移軌道の軌道始点ｑｓｔａｒｔと軌道終点ｑｅｎｄの間の差分は所定値未満であってもよい。以上の構成によれば、遷移軌道を生成する生成処理が安定化する。
【0123】
また、遷移軌道の軌道始点ｑｓｔａｒｔから見た遷移軌道の軌道終点ｑｅｎｄの方向と、第２動作データの軌道の遷移完了直後の方向と、が成す角度が所定値未満であってもよい。以上の構成によれば、遷移軌道から回避動作データの軌道に乗り換えるときのモバイルマニピュレータ１の滑らかな動作を実現できる。
【0124】
また、遷移軌道の軌道終点ｑｅｎｄは、
遷移軌道の軌道始点ｑｓｔａｒｔと軌道終点ｑｅｎｄの間の差分が所定値未満となる第１の条件と、
軌道始点ｑｓｔａｒｔから見た軌道終点ｑｅｎｄの方向と、回避動作データの軌道Ｑの遷移完了直後の方向と、が成す角度が所定値未満となる第２の条件と、
を同時に満たす複数の軌道点ｑｉのうち最も下流側の軌道点ｑｉであってもよい。
以上の構成によれば、遷移軌道を生成する生成処理が安定化し、遷移軌道から回避動作データの軌道に乗り換えるときのモバイルマニピュレータ１の滑らかな動作を実現できると共に、モバイルマニピュレータ１がタスクを実行するための所要時間を短縮することができる。
遷移軌道の軌道始点ｑｓｔａｒｔと軌道終点ｑｅｎｄの間の差分が所定値未満となる第１の条件と、
軌道始点ｑｓｔａｒｔから見た軌道終点ｑｅｎｄの方向と、回避動作データの軌道Ｑの遷移完了直後の方向と、が成す角度が所定値未満となる第２の条件と、
を同時に満たす複数の軌道点ｑｉのうち最も下流側の軌道点ｑｉであってもよい。
以上の構成によれば、遷移軌道を生成する生成処理が安定化し、遷移軌道から回避動作データの軌道に乗り換えるときのモバイルマニピュレータ１の滑らかな動作を実現できると共に、モバイルマニピュレータ１がタスクを実行するための所要時間を短縮することができる。
【0125】
また、式（１）に示すように、遷移軌道生成部２４は、モバイルマニピュレータ１の連続する２つの軌道点の差分を小さくするためのポテンシャル関数（第１コスト関数）と、リアルタイムに取得した三次元データに基づくサービス環境を回避するためのペナルティ関数（第２コスト関数）と、に基づく二次計画法により遷移軌道を生成する。
以上の構成によれば、遷移軌道を効率よく生成することができる。
以上の構成によれば、遷移軌道を効率よく生成することができる。
【0126】
また、式（７）に示すように、遷移軌道生成部２４は、モバイルマニピュレータ１の連続する２つの軌道点の差分を、モバイルマニピュレータ１がリアルタイムに取得した三次元データに基づくサービス環境を回避するための回避量sdltに基づいて重み付けし、重み付け後の差分の総和を求めることによりポテンシャル関数を演算する。
以上の構成によれば、モバイルマニピュレータ１とリアルタイムに取得した三次元点群データに基づくサービス環境との接触を回避するように遷移軌道を生成するに際し、連続する２つの軌道点が過度に離れてしまうことを抑制できる。
以上の構成によれば、モバイルマニピュレータ１とリアルタイムに取得した三次元点群データに基づくサービス環境との接触を回避するように遷移軌道を生成するに際し、連続する２つの軌道点が過度に離れてしまうことを抑制できる。
【0127】
また、式（４）に示すように、遷移軌道生成部２４は、モバイルマニピュレータ１とリアルタイムに取得した三次元点群データに基づくサービス環境との重なり量Cltの対数に基づいて、前記モバイルマニピュレータがリアルタイムに取得した前記三次元データに基づくサービス環境を回避するための回避量sdltを演算する。
以上の構成によれば、二次計画法による遷移軌道の生成処理が安定化する。
以上の構成によれば、二次計画法による遷移軌道の生成処理が安定化する。
【0128】
上記の実施形態は以下のように変更できる。
【0129】
図１０に示すように、ユーザー端末９０は、タスクデータをモバイルマニピュレータ１に送信することとしたが（Ｓ１２０）、これに代えて、ユーザー端末９０は、タスクデータを管理サーバ８０に送信するようにしてもよい。この場合、管理サーバ８０からモバイルマニピュレータ１にタスクデータが転送されることになるだろう。
【0130】
上述の例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）を含む。非一時的なコンピュータ可読媒体の例は、更に、ＣＤ－ＲＯＭ（Read Only Memory）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭを含む。非一時的なコンピュータ可読媒体の例は、更に、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【0131】
１ モバイルマニピュレータ
２ 移動台車
３ ロボット
４ ロボット本体
５ マニピュレータ
６ エンドエフェクタ
７ 環境センサ
８ 制御部
８ｂ メモリ
８ｃ 通信インターフェース
９ 上腕リンク
１０ 前腕リンク
１１ 肩関節
１２ 肘関節
１３ 手首関節
２０ タスク準備部
２１ タスク実行部
２２ 接触判定部
２３ 回避動作データ抽出部
２４ 遷移軌道生成部
２５ 軌道更新部
８０ 管理サーバ
８０ｂ メモリ
８０ｃ 通信インターフェース
８１ データベース生成部
８２ 基本データベース
８３ データ受信部
８５ 候補動作データ抽出部
８６ 候補データベース
９０ ユーザー端末
１００ 軌道生成システム
２ 移動台車
３ ロボット
４ ロボット本体
５ マニピュレータ
６ エンドエフェクタ
７ 環境センサ
８ 制御部
８ｂ メモリ
８ｃ 通信インターフェース
９ 上腕リンク
１０ 前腕リンク
１１ 肩関節
１２ 肘関節
１３ 手首関節
２０ タスク準備部
２１ タスク実行部
２２ 接触判定部
２３ 回避動作データ抽出部
２４ 遷移軌道生成部
２５ 軌道更新部
８０ 管理サーバ
８０ｂ メモリ
８０ｃ 通信インターフェース
８１ データベース生成部
８２ 基本データベース
８３ データ受信部
８５ 候補動作データ抽出部
８６ 候補データベース
９０ ユーザー端末
１００ 軌道生成システム
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】