ホーム > 特許ランキング > 国立大学法人九州工業大学
▶ 国立大学法人九州工業大学の特許一覧 ▶ トヨタ自動車株式会社の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024039143
(43)【公開日】2024-03-22
(54)【発明の名称】軌道設定装置、軌道設定方法、及びプログラム
(51)【国際特許分類】
G05D 1/43 20240101AFI20240314BHJP
G05D 1/683 20240101ALI20240314BHJP
【ＦＩ】
G05D1/02 H
G05D1/12 Z
【審査請求】未請求
【請求項の数】7
【出願形態】ＯＬ
(21)【出願番号】P 2022143477
(22)【出願日】2022-09-09
(71)【出願人】
【識別番号】504174135
【氏名又は名称】国立大学法人九州工業大学
(71)【出願人】
【識別番号】000003207
【氏名又は名称】トヨタ自動車株式会社
(74)【代理人】
【識別番号】100103894
【弁理士】
【氏名又は名称】家入 健
(72)【発明者】
【氏名】田向 権
(72)【発明者】
【氏名】石田 裕太郎
(72)【発明者】
【氏名】柴田 智広
(72)【発明者】
【氏名】山下 勝司
(72)【発明者】
【氏名】藪下 英典
【テーマコード（参考）】
5H301
【Ｆターム（参考）】
5H301AA02
5H301BB14
5H301CC03
5H301CC06
5H301CC08
5H301CC10
5H301GG08
5H301LL02
5H301LL06
5H301LL11
(57)【要約】
【課題】環境地図を作成する必要がなく処理量を低減できること。
【解決手段】人に追従して移動するロボットの軌道を設定する軌道設定装置は、物体を検出する検出手段と、人とロボットとの距離に基づいてロボットの速度を算出し、速度とロボットのヨー角候補とに基づいて、時刻ｔのロボットの位置を算出する位置算出手段と、算出されたロボットの位置に対する人の位置のポテンシャルと、算出されたロボットの位置から最も近い障害物のポテンシャルと、ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と算出されたロボットの位置及び人の位置を通る線の角度との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数のヨー角候補及びヨーレート候補の中から１組のヨー角候補及びヨーレート候補を選択し、ロボットの軌道を設定する軌道設定手段と、を備える。
【選択図】図２
【解決手段】人に追従して移動するロボットの軌道を設定する軌道設定装置は、物体を検出する検出手段と、人とロボットとの距離に基づいてロボットの速度を算出し、速度とロボットのヨー角候補とに基づいて、時刻ｔのロボットの位置を算出する位置算出手段と、算出されたロボットの位置に対する人の位置のポテンシャルと、算出されたロボットの位置から最も近い障害物のポテンシャルと、ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と算出されたロボットの位置及び人の位置を通る線の角度との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数のヨー角候補及びヨーレート候補の中から１組のヨー角候補及びヨーレート候補を選択し、ロボットの軌道を設定する軌道設定手段と、を備える。
【選択図】図２
【特許請求の範囲】
【請求項1】
人に追従して移動するロボットの軌道を設定する軌道設定装置であって、
物体を検出する検出手段と、
前記検出手段により検出された人とロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出する位置算出手段と、
少なくとも、前記位置算出手段により算出されたロボットの位置に対する前記検出手段により検出された人の位置のポテンシャルと、前記位置算出手段により算出されたロボットの位置から最も近い、前記検出手段により検出された障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記位置算出手段により算出されたロボットの位置及び前記検出手段により検出された人の位置を通る線の角度との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定する軌道設定手段と、
を備える、軌道設定装置。
物体を検出する検出手段と、
前記検出手段により検出された人とロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出する位置算出手段と、
少なくとも、前記位置算出手段により算出されたロボットの位置に対する前記検出手段により検出された人の位置のポテンシャルと、前記位置算出手段により算出されたロボットの位置から最も近い、前記検出手段により検出された障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記位置算出手段により算出されたロボットの位置及び前記検出手段により検出された人の位置を通る線の角度との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定する軌道設定手段と、
を備える、軌道設定装置。
【請求項2】
請求項１記載の軌道設定装置であって、
前記評価関数は、前記ロボットのヨー角候補と前回選択したヨー角候補との差分を前記評価パラメータとして更に含む、軌道設定装置。
前記評価関数は、前記ロボットのヨー角候補と前回選択したヨー角候補との差分を前記評価パラメータとして更に含む、軌道設定装置。
【請求項3】
請求項１又は２記載の軌道設定装置であって、
前記評価関数は、前記ロボットのヨーレート候補と前回選択したヨーレート候補との差分を前記評価パラメータとして更に含む、軌道設定装置。
前記評価関数は、前記ロボットのヨーレート候補と前回選択したヨーレート候補との差分を前記評価パラメータとして更に含む、軌道設定装置。
【請求項4】
請求項１又は２記載の軌道設定装置であって、
前記軌道設定手段は、前記各評価パラメータに予め設定された重み付け係数を夫々乗算し、足し合わせた演算結果が最小となる前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定する、軌道設定装置。
前記軌道設定手段は、前記各評価パラメータに予め設定された重み付け係数を夫々乗算し、足し合わせた演算結果が最小となる前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定する、軌道設定装置。
【請求項5】
請求項１又は２記載の軌道設定装置であって、
前記位置算出手段は、前記検出手段により検出された人とロボットとの距離に比例した前記ロボットの速度を算出する、軌道設定装置。
前記位置算出手段は、前記検出手段により検出された人とロボットとの距離に比例した前記ロボットの速度を算出する、軌道設定装置。
【請求項6】
人に追従して移動するロボットの軌道を設定する軌道設定方法であって、
前記人とロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出するステップと、
少なくとも、前記算出されたロボットの位置に対する前記人の位置のポテンシャルと、前記算出されたロボットの位置から最も近い障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記算出されたロボットの位置及び前記人の位置を通る線との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定するステップと、
を含む軌道設定方法。
前記人とロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出するステップと、
少なくとも、前記算出されたロボットの位置に対する前記人の位置のポテンシャルと、前記算出されたロボットの位置から最も近い障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記算出されたロボットの位置及び前記人の位置を通る線との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定するステップと、
を含む軌道設定方法。
【請求項7】
人と該人に追従して移動するロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出する処理と、
少なくとも、前記算出されたロボットの位置に対する前記人の位置のポテンシャルと、前記算出されたロボットの位置から最も近い障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記算出されたロボットの位置及び前記人の位置を結ぶ線との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定する処理と、
をコンピュータに実行させるプログラム。
少なくとも、前記算出されたロボットの位置に対する前記人の位置のポテンシャルと、前記算出されたロボットの位置から最も近い障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記算出されたロボットの位置及び前記人の位置を結ぶ線との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定する処理と、
をコンピュータに実行させるプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動ロボットの軌道を設定する軌道設定装置、軌道設定方法、及びプログラムに関する。
【背景技術】
【0002】
自己位置推定及び環境地図を用いてロボットの軌道を設定する軌道設定装置が知られている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開２０１９－０２８９８８号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
上記軌道設定装置において、環境地図を作成する必要があるため、処理量が増加する虞がある。
【0005】
本発明は、このような問題点を解決するためになされたものであり、環境地図を作成する必要がなく処理量を低減できる軌道設定装置、軌道設定方法、及びプログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【0006】
上記目的を達成するための本発明の一態様は、
人に追従して移動するロボットの軌道を設定する軌道設定装置であって、
物体を検出する検出手段と、
前記検出手段により検出された人とロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出する位置算出手段と、
少なくとも、前記位置算出手段により算出されたロボットの位置に対する前記検出手段により検出された人の位置のポテンシャルと、前記位置算出手段により算出されたロボットの位置から最も近い、前記検出手段により検出された障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記位置算出手段により算出されたロボットの位置及び前記検出手段により検出された人の位置を通る線の角度との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定する軌道設定手段と、
を備える、軌道設定装置
である。
この一態様において、前記評価関数は、前記ロボットのヨー角候補と前回選択したヨー角候補との差分を前記評価パラメータとして更に含んでいてもよい。
この一態様において、前記評価関数は、前記ロボットのヨーレート候補と前回選択したヨーレート候補との差分を前記評価パラメータとして更に含んでいてもよい。
この一態様において、前記軌道設定手段は、前記各評価パラメータに予め設定された重み付け係数を夫々乗算し、足し合わせた演算結果が最小となる前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定してもよい。
この一態様において、前記位置算出手段は、前記検出手段により検出された人とロボットとの距離に比例した前記ロボットの速度を算出してもよい。
上記目的を達成するための本発明の一態様は、
人に追従して移動するロボットの軌道を設定する軌道設定方法であって、
前記人とロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出するステップと、
少なくとも、前記算出されたロボットの位置に対する前記人の位置のポテンシャルと、前記算出されたロボットの位置から最も近い障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記算出されたロボットの位置及び前記人の位置を通る線との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定するステップと、
を含む軌道設定方法
であってもよい。
上記目的を達成するための本発明の一態様は、
人と該人に追従して移動するロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出する処理と、
少なくとも、前記算出されたロボットの位置に対する前記人の位置のポテンシャルと、前記算出されたロボットの位置から最も近い障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記算出されたロボットの位置及び前記人の位置を結ぶ線との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定する処理と、
をコンピュータに実行させるプログラム
であってもよい。
人に追従して移動するロボットの軌道を設定する軌道設定装置であって、
物体を検出する検出手段と、
前記検出手段により検出された人とロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出する位置算出手段と、
少なくとも、前記位置算出手段により算出されたロボットの位置に対する前記検出手段により検出された人の位置のポテンシャルと、前記位置算出手段により算出されたロボットの位置から最も近い、前記検出手段により検出された障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記位置算出手段により算出されたロボットの位置及び前記検出手段により検出された人の位置を通る線の角度との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定する軌道設定手段と、
を備える、軌道設定装置
である。
この一態様において、前記評価関数は、前記ロボットのヨー角候補と前回選択したヨー角候補との差分を前記評価パラメータとして更に含んでいてもよい。
この一態様において、前記評価関数は、前記ロボットのヨーレート候補と前回選択したヨーレート候補との差分を前記評価パラメータとして更に含んでいてもよい。
この一態様において、前記軌道設定手段は、前記各評価パラメータに予め設定された重み付け係数を夫々乗算し、足し合わせた演算結果が最小となる前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定してもよい。
この一態様において、前記位置算出手段は、前記検出手段により検出された人とロボットとの距離に比例した前記ロボットの速度を算出してもよい。
上記目的を達成するための本発明の一態様は、
人に追従して移動するロボットの軌道を設定する軌道設定方法であって、
前記人とロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出するステップと、
少なくとも、前記算出されたロボットの位置に対する前記人の位置のポテンシャルと、前記算出されたロボットの位置から最も近い障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記算出されたロボットの位置及び前記人の位置を通る線との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定するステップと、
を含む軌道設定方法
であってもよい。
上記目的を達成するための本発明の一態様は、
人と該人に追従して移動するロボットとの距離に基づいて該ロボットの速度を算出し、該算出した速度とロボットのヨー角候補とに基づいて、時刻ｔの前記ロボットの位置を算出する処理と、
少なくとも、前記算出されたロボットの位置に対する前記人の位置のポテンシャルと、前記算出されたロボットの位置から最も近い障害物のポテンシャルと、前記ロボットのヨーレート候補に基づいて算出した時刻ｔのロボットのヨー角と前記算出されたロボットの位置及び前記人の位置を結ぶ線との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数の前記ヨー角候補及びヨーレート候補の中から１組の前記ヨー角候補及びヨーレート候補を選択し、前記ロボットの軌道を設定する処理と、
をコンピュータに実行させるプログラム
であってもよい。
【発明の効果】
【0007】
本発明によれば、環境地図を作成する必要がなく処理量を低減できる軌道設定装置、軌道設定方法、及びプログラムを提供することができる。
【図面の簡単な説明】
【0008】
【図1】フォローミー制御の制御系を説明するための図である。
【図2】本実施形態に係る軌道設定装置の概略的なシステム構成を示すブロック図である。
【図3】移動ロボットの速度ｖと、人及び移動ロボットの間の距離ｄと、の関係の一例を示す図である。
【図4】移動ロボットの移動方向を示す図である。
【図5】本実施形態に係る軌道設定方法のフローを示すフローチャートである。
【図6】障害物点群を占有格子マップの各グリッドに近似した図である。
【図7】a個のポテンシャル配列を占有格子マップ上に投影する方法を示す図である。
【図8】ＬＵＴの生成方法を示す図である。
【図9】各処理をパイプラインで接続して実行する方法を示す図である。
【発明を実施するための形態】
【0009】
実施形態１
以下、図面を参照して本発明の実施形態について説明する。高齢化社会において、人の生活をサポートするロボットの開発が望まれている。例えば、カフェテリアなどで配膳をサポートする機能や、空港、ショッピングモールなどの大型施設で案内やおもてなしを行う機能、などは、高齢者の自立的な生活に重要となる。
以下、図面を参照して本発明の実施形態について説明する。高齢化社会において、人の生活をサポートするロボットの開発が望まれている。例えば、カフェテリアなどで配膳をサポートする機能や、空港、ショッピングモールなどの大型施設で案内やおもてなしを行う機能、などは、高齢者の自立的な生活に重要となる。
【0010】
そのような重要な機能を実現するために、人を認識し、その人に追従して移動する、所謂フォローミー制御（人追従認識制御）を行う移動ロボットの開発が行われている。本実施形態に係る軌道設定装置は、上記フォローミー制御を行う移動ロボットに搭載され、移動ロボットの軌道を設定する。
【0011】
ここで、上述した移動ロボットのフォローミー制御について、詳細に説明する。図１は、フォローミー制御の制御系を説明するための図である。
【0012】
移動ロボットは、例えば、ＬＲＦ（Laser rangefinder）により、追従する人の脚部を検出する（ステップＳ１０１）。移動ロボットは、人の脚部のポイントクラウドをクラスタリングし（ステップＳ１０２）、人の脚らしさ判定を行う（ステップＳ１０３）。移動ロボットは、カメラにより撮影された画像に対して画像処理を行い、追従する人及びその脚部の形状を認識する（ステップＳ１０４）。
【0013】
移動ロボットは、脚らしさ判定の結果及び画像処理の認識結果に基づいて、パーティクルフィルタを適用し（ステップＳ１０５）、追従する人の座標位置（Ｗａｙｐｏｉｎｔ）を算出する（ステップＳ１０６）。移動ロボットは、人の移動に応じて、その人の座標位置を生成する。移動ロボットは、生成した人の座標位置を追従する移動制御を行う。
【0014】
移動ロボットは、上述した処理と同時に、例えば、ＬＲＦ及びＲＧＢ－Ｄなどのカメラにより、障害物点群を生成する（ステップＳ１０７）。移動ロボットは、生成した障害物点群に基づいて、障害物のマッピングを行い（ステップＳ１０８）、障害物のポテンシャルを算出する（ステップＳ１０９）。
【0015】
移動ロボットは、上述した人の座標位置を追従しつつ、障害物のポテンシャルに基づいて障害物を回避するような、各時刻で未来の応答を予測しながら最適化を行うモデル予測制御を行う（ステップＳ１１０）。
【0016】
ところで、従来の軌道設定装置は、自己位置推定及び環境地図を用いて移動ロボットの軌道を設定している。このため、軌道設定装置は、環境地図を作成する必要があるため、処理量が増加する虞がある。
【0017】
これに対し、本実施形態に係る軌道設定装置は、上述の如く、先行する人と移動ロボットとの相対的な位置関係を用いて移動ロボットの軌道を設定するため、環境地図を作成する必要がなく処理量を低減できる。
【0018】
図２は、本実施形態に係る軌道設定装置の概略的なシステム構成を示すブロック図である。本実施形態に係る軌道設定装置１は、物体を検出するセンサ２と、移動ロボットの位置を算出する位置算出部３と、移動ロボットの軌道を設定する軌道設定部４と、を備えている。
【0019】
なお、軌道設定装置１は、例えば、ＣＰＵ（Central Processing Unit）やＧＰＵ（Graphics Processing Unit）などのプロセッサと、ＲＡＭ（Random Access Memory）やＲＯＭ（Read Only Memory）などの内部メモリと、ＨＤＤ（Hard Disk Drive）やＳＤＤ（Solid State Drive）などのストレージデバイスと、ディスプレイなどの周辺機器を接続するための入出力Ｉ／Ｆと、装置外部の機器と通信を行う通信Ｉ／Ｆと、を備えた通常のコンピュータのハードウェア構成を有する。
【0020】
軌道設定装置１は、例えば、プロセッサが内部メモリを利用しながら、ストレージデバイスや内部メモリなどに格納されたプログラムを実行することで、後述の各機能的な構成要素を実現することができる。
【0021】
センサ２は、検出手段の一具体例である。センサ２は、移動ロボットに搭載されたＬＲＦやカメラなどで構成されている。
【0022】
位置算出部３は、位置算出手段の一具体例である。位置算出部３は、ｅ時刻の移動ロボットの位置を算出する。まず、位置算出部３は、センサ２により検出された追従する人（以下、単に人）と移動ロボットとの距離に基づいて、移動ロボットの速度を算出する。
【0023】
位置算出部３は、センサ２により検出された人と移動ロボットとの距離に比例した移動ロボットの速度を算出してもよい。これにより、移動ロボットが人に接近するに従って減速することで安全性を向上させ、移動ロボットが人から離間するに従って増速することで、追従性を向上させている。
【0024】
【0025】
上記式において、ｖｍｉｎは移動ロボットの最低速度、ｖｍａｘは移動ロボットの最高速度、ｄは移動ロボットと人との距離、ｄｍｉｎは移動ロボットと人との最低距離、ｄｍａｘは移動ロボットと人との最高距離、αは速度係数である。αは（ｖｍａｘ－ｖｍｉｎ）／（ｄｍａｘ－ｄｍｉｎ）により算出される。ｖｍｉｎ、ｖｍａｘ、ｄｍｉｎ、及びｄｍａｘは、予め位置算出部３に設定されていてもよい。例えば、図３に示すように、ｖｍｉｎ＝０．０、ｖｍａｘ＝０．６、ｄｍｉｎ＝１．０、ｄｍａｘ＝２．０に設定されている。
【0026】
図４は、移動ロボットの移動方向を示す図である。図４に示す如く、移動ロボットは、人を追従するため、移動範囲が広く略全方位に移動可能である。図４及び後述の式において、ｘｙｄｉｒは移動ロボットのヨー角候補[ｒａｄ]、Ｎは移動ロボットのヨー角候補の数、ｘｙｄｉｒ_ｓｔｅｐはヨー角候補間の差分[ｒａｄ]、ｙａｗは移動ロボットのヨーレート候補[ｒａｄ／ｓ]、Ｍは移動ロボットのヨーレート候補の数、ｙａｗｓｔｅｐはヨーレート候補間の差分[ｒａｄ／ｓ]、Ｌは１軌道上の座標の数、ｅｖａｌｓｔｅｐは予測の間隔[ｓ]、φｔ－１は１時刻前の移動ロボットのヨー角[ｒａｄ]である。
【0027】
例えば、０からｘｙｄｉｒ_ｓｔｅｐ差分で増加させて、Ｎ個のヨー角候補ｘｙｄｉｒが生成される。位置算出部３は、上記算出した速度ｖと、移動ロボットのヨー角候補ｘｙｄｉｒと、に基づき、下記式を用いて移動ロボットの移動を直線近似し、ｅ時刻の移動ロボットの２次元座標位置（ｘｔｅ、ｙｔｅ）を算出してもよい。
【0028】
例えば、０から差分ｙａｗｓｔｅｐで増加させて、Ｍ個のヨーレート候補ｙａｗが生成される。位置算出部３は、移動ロボットのヨーレート候補ｙａｗに基づき、下記式を用いて、ｅ時刻の移動ロボットのヨー角φｔｅを算出してもよい。
【数2】
【0029】
位置算出部３は、図４に示す如く、移動ロボットのヨーレート候補ｙａｗに基づき、最後時刻ｅ＝Ｌの移動ロボットのヨー角φｔＬを算出する。
【0030】
位置算出部３は、複数のヨー角候補ｘｙｄｉｒに対して、夫々、ｅ時刻の移動ロボットの位置（ｘｔｅ、ｙｔｅ）を算出する。また、位置算出部３は、複数のヨーレート候補ｙａｗに対して、夫々、ｅ時刻の移動ロボットのヨー角φｔｅを算出する。
【0031】
軌道設定部４は、軌道設定手段の一具体例である。軌道設定部４は、評価関数を用いて、複数のヨー角候補及びヨーレート候補の中から１組のヨー角候補及びヨーレート候補を選択することで、移動ロボットの軌道を設定する。
【0032】
評価関数は、少なくとも、位置算出部３により算出された移動ロボットの位置に対するセンサ２により検出された人の位置のＷａｙｐｏｉｎｔポテンシャルと、位置算出部３により算出された移動ロボットの位置から最も近い、センサ２により検出された障害物のポテンシャルと、移動ロボットのヨーレート候補に基づいて算出したｅ時刻の移動ロボットのヨー角と位置算出部３により算出された移動ロボットの位置及びセンサ２により検出された人の位置を通る線の角度との角度差と、を評価パラメータとして含む。
【0033】
軌道設定部４は、位置算出部３により算出された移動ロボットの位置（ｘｔｅ、ｙｔｅ）と、センサ２により検出された人の位置（ｘｗ、ｙｗ）と、に基づいて、下記式を用いて、Ｗａｙｐｏｉｎｔポテンシャルｐｗを算出する。
【0034】
上記人の位置（ｘｗ、ｙｗ）は、例えば、センサ２により検出された人の各部のうち、移動ロボットから最近傍の部分の位置である。下記式において、σｗｘ、σｗｙは、ポテンシャルの分散である。Ｗａｙｐｏｉｎｔポテンシャルｐｗが低いほど、移動ロボットの人に対する追従性がよくなる。
【数3】
【0035】
軌道設定部４は、位置算出部３により算出された移動ロボットの位置（ｘｔｅ、ｙｔｅ）と、センサ２により検出された最近傍の障害物の位置（ｘｏ、ｙｏ）と、に基づいて、下記式を用いて、障害物ポテンシャルｐｏを算出する。下記式において、σｏｘ、σｏｙは、ポテンシャルの分散である。この障害物ポテンシャルが低いほど、移動ロボットが障害物を回避し易くなる。
【数4】
【0036】
軌道設定部４は、位置算出部３により算出されたｅ時刻の移動ロボットのヨー角φｔｅと、位置算出部３により算出された移動ロボットの位置（ｘｔｅ、ｙｔｅ）と、センサ２により検出された人の位置（ｘｏｐ、ｙｏｐ）と、に基づいて、下記式を用いて、角度差ａｎｇｌｅを算出する。
【0037】
ａｔａｎ２（ｙ、ｘ）は、原点とある点（ｘ、ｙ）が成す角度を返す関数である。したがって、ａｔａｎ２（ｙｏｐ－ｙｔｅ、ｘｏｐ－ｘｔｅ）は、位置算出部３により算出された移動ロボットの位置（ｘｔｅ、ｙｔｅ）及びセンサ２により検出された人の位置（ｘｏｐ、ｙｏｐ）を通る線の角度を示す。
【数5】
【0038】
なお、評価関数の評価パラメータに、上記移動ロボットと人との角度差ａｎｇｌｅを含めているのは、移動ロボットのカメラの画角内に人が入るようにするためだけでなく、移動ロボットと、人との方向を合わせることで、移動ロボットと人との間のインターラクション性を向上させるためである。この角度差が小さいほど、移動ロボットと人との間のインターラクション性が向上する。
【0039】
軌道設定部４は、下記式を用いて、上記算出した各評価パラメータｐｗ、ｐｏ、ａｎｇｌｅに予め設定された重み付け係数ｗｗ、ｗｐ、ｗａｎｇｌｅを夫々乗算し、足し合わせた評価関数の評価値Ｗを算出する。
Ｗ＝ｗｗｐｗ＋ｗｐｐｏ＋ｗａｎｇｌｅａｎｇｌｅ
Ｗ＝ｗｗｐｗ＋ｗｐｐｏ＋ｗａｎｇｌｅａｎｇｌｅ
【0040】
なお、重み付け係数ｗｗ、ｗｐ、ｗａｎｇｌｅは、予め軌道設定部４に設定されているが、ユーザは入力装置などを介して、これら設定値を任意の値に設定変更できる。例えば、移動ロボットの人に対する追従性を重視したい場合、Ｗａｙｐｏｉｎｔポテンシャルの重み付け係数ｗｗを他の重み付け係数に対して相対的に大きく設定してもよい。障害物の回避を重視したい場合、障害物ポテンシャルの重み付け係数ｗｐを他の重み付け係数に対して相対的に大きく設定してもよい。人とのインターラクション性を重視したい場合、角度差の重み付け係数ｗａｎｇｌｅを相対的に大きく設定してもよい。
【0041】
軌道設定部４は、各ヨー角候補ｘｙｄｉｒに対する評価パラメータｐｗ、ｐｏを算出し、各ヨーレート候補ｙａｗに対する評価パラメータａｎｇｌｅを算出する。そして、軌道設定部４は、各ヨー角候補ｘｙｄｉｒ及びヨーレート候補ｙａｗに対する評価関数の評価値Ｗを夫々算出する。
【0042】
軌道設定部４は、上記算出した評価値Ｗが最小となるヨー角候補ｘｙｄｉｒ及びヨーレート候補ｙａｗを選択し、移動ロボットの軌道を設定する。
【0043】
続いて、本実施形態に係る軌道設定方法について説明する。図５は、本実施形態に係る軌道設定方法のフローを示すフローチャートである。位置算出部３は、センサ２により検出された人と移動ロボットとの距離に基づいて、移動ロボットの速度を算出する（ステップＳ５０１）。
【0044】
軌道設定部４は、位置算出部３により算出された移動ロボットの位置（ｘｔｅ、ｙｔｅ）と、センサ２により検出された人の位置（ｘｗ、ｙｗ）と、に基づいて、Ｗａｙｐｏｉｎｔポテンシャルｐｗを算出する（ステップＳ５０２）。
【0045】
軌道設定部４は、位置算出部３により算出された移動ロボットの位置（ｘｔｅ、ｙｔｅ）と、センサ２により検出された最近傍の障害物の位置（ｘｏ、ｙｏ）と、に基づいて、障害物ポテンシャルｐｏを算出する（ステップＳ５０３）。
【0046】
軌道設定部４は、位置算出部３により算出された最後時刻Ｌの移動ロボットのヨー角φｔＬと、位置算出部３により算出された移動ロボットの位置（ｘｔｅ、ｙｔｅ）と、センサ２により検出された人の位置（ｘｏｐ、ｙｏｐ）と、に基づいて、角度差ａｎｇｌｅを算出する（ステップＳ５０４）。
【0047】
軌道設定部４は、算出した各評価パラメータｐｗ、ｐｏ、ａｎｇｌｅに予め設定された重み付け係数ｗｗ、ｗｐ、ｗａｎｇｌｅを夫々乗算し、足し合わせた評価関数の評価値Ｗを算出する（ステップＳ５０５）。
【0048】
軌道設定部４は、上記算出した評価値Ｗが最小となるヨー角候補ｘｙｄｉｒ及びヨーレート候補ｙａｗを選択し、移動ロボットの軌道を設定する（ステップＳ５０６）。
【0049】
以上、本実施形態に係る軌道設定装置１において、位置算出部３は、センサ２により検出された人と移動ロボットとの距離に基づいて移動ロボットの速度を算出し、算出した速度と移動ロボットのヨー角候補とに基づいて、時刻ｔの移動ロボットの位置を算出する。軌道設定部４は、少なくとも、位置算出部３により算出されたロボットの位置に対するセンサ２により検出された人の位置のポテンシャルと、位置算出部３により算出された移動ロボットの位置から最も近い、センサ２により検出された障害物のポテンシャルと、移動ロボットのヨーレート候補に基づいて算出した時刻ｔの移動ロボットのヨー角と位置算出部３により算出された移動ロボットの位置及びセンサ２により検出された人の位置を通る線の角度との角度差と、を評価パラメータとして含む評価関数を用いて評価を行うことで、複数のヨー角候補及びヨーレート候補の中から１組のヨー角候補及びヨーレート候補を選択し、移動ロボットの軌道を設定する。
【0050】
これにより、先行する人と移動ロボットとの相対的な位置関係を用いて移動ロボットの軌道を設定するため、環境地図を作成する必要がなく処理量を低減できる。また、評価関数の評価パラメータとして、Ｗａｙｐｏｉｎｔポテンシャル及び障害物のポテンシャルを含めることで、人に対する追従性及び障害物の回避性を向上させるだけでなく、移動ロボットと人との角度差をさらに含めることで、移動ロボットと、人との方向を合わせることで、移動ロボットと人との間のインターラクション性を向上させることができる。
【0051】
実施形態２
本実施形態２において、評価関数は、上記実施形態１に係る評価パラメータに加えて、今回選択した移動ロボットのヨー角候補と前回選択したヨー角候補との差分を評価パラメータとして更に含む。
本実施形態２において、評価関数は、上記実施形態１に係る評価パラメータに加えて、今回選択した移動ロボットのヨー角候補と前回選択したヨー角候補との差分を評価パラメータとして更に含む。
【0052】
今回選択した移動ロボットのヨー角候補と前回選択したヨー角候補との差分、すなわち、移動方向の変化が小さいほど、移動ロボットはスムーズに動作できる。このように、移動ロボットの動作性に関する評価パラメータを更に含む評価関数を用いて評価を行うことで、移動ロボットがスムーズに動作できる、より最適な移動ロボットの軌道を設定できる。
【0053】
軌道設定部４は、下記式を用いて、今回選択した移動ロボットのヨー角候補ｘｙｄｉｒと前回選択したヨー角候補ｘｙｄｉｒ_ｐｒｅｖとの差分ｄｉｆｆｘｙｄｉｒを算出する。
ｄｉｆｆｘｙｄｉｒ＝｜ｘｙｄｉｒ－ｘｙｄｉｒ_ｐｒｅｖ｜
ｄｉｆｆｘｙｄｉｒ＝｜ｘｙｄｉｒ－ｘｙｄｉｒ_ｐｒｅｖ｜
【0054】
軌道設定部４は、下記式を用いて、算出した各評価パラメータｐｗ、ｐｏ、ｄｉｆｆｘｙｄｉｒ、ａｎｇｌｅに予め設定された重み付け係数ｗｗ、ｗｐ、ｗｘｙｄｉｆｆ、ｗａｎｇｌｅを夫々乗算し、足し合わせた評価関数の評価値Ｗを算出する。
Ｗ＝ｗｗｐｗ＋ｗｐｐｏ＋ｗｘｙｄｉｆｆｄｉｆｆｘｙｄｉｒ＋ｗａｎｇｌｅａｎｇｌｅ
Ｗ＝ｗｗｐｗ＋ｗｐｐｏ＋ｗｘｙｄｉｆｆｄｉｆｆｘｙｄｉｒ＋ｗａｎｇｌｅａｎｇｌｅ
【0055】
ヨー角候補の重み付け係数ｗｘｙｄｉｆｆは、予め軌道設定部４に設定されている。なお、移動ロボットをスムーズに動作させたいなどの動作性を重視する場合、ヨー角候補の重み付け係数ｗｘｙｄｉｆｆを他の重み付け係数に対して相対的に大きく設定してもよい。
【0056】
軌道設定部４は、上記算出した評価値Ｗが最小となるヨー角候補ｘｙｄｉｒ及びヨーレート候補ｙａｗを選択し、移動ロボットの軌道を設定する。
【0057】
実施形態３
本実施形態３において、評価関数は、上記実施形態２に係る評価パラメータに加えて、今回選択した移動ロボットのヨーレート候補ｙａｗと前回選択したヨーレート候補ｙａｗとの差分を評価パラメータとして更に含む。
本実施形態３において、評価関数は、上記実施形態２に係る評価パラメータに加えて、今回選択した移動ロボットのヨーレート候補ｙａｗと前回選択したヨーレート候補ｙａｗとの差分を評価パラメータとして更に含む。
【0058】
今回選択した移動ロボットのヨーレート候補と前回選択したヨーレート候補との差分、すなわち、移動方向の速度変化が小さいほど、移動ロボットはスムーズに動作できる。このように、移動ロボットの動作性に関する評価パラメータを更に含む評価関数を用いて評価を行うことで、移動ロボットがスムーズに動作できる、より最適な移動ロボットの軌道を設定できる。
【0059】
軌道設定部４は、下記式を用いて、今回選択した移動ロボットのヨーレート候補ｙａｗと前回選択したヨーレート候補ｙａｗｐｒｅｖとの差分ｄｉｆｆｙａｗを算出する。
ｄｉｆｆｙａｗ＝｜ｙａｗ－ｙａｗｐｒｅｖ｜
ｄｉｆｆｙａｗ＝｜ｙａｗ－ｙａｗｐｒｅｖ｜
【0060】
軌道設定部４は、下記式を用いて、算出した各評価パラメータｐｗ、ｐｏ、ｄｉｆｆｘｙｄｉｒ、ｄｉｆｆｙａｗ、ａｎｇｌｅに予め設定された重み付け係数ｗｗ、ｗｐ、ｗｘｙｄｉｆｆ、ｗｙａｗ、ｗａｎｇｌｅを夫々乗算し、足し合わせた評価関数の評価値Ｗを算出する。
Ｗ＝ｗｗｐｗ＋ｗｐｐｏ＋ｗｘｙｄｉｆｆｄｉｆｆｘｙｄｉｒ＋ｗｙａｗｄｉｆｆｙａｗ＋ｗａｎｇｌｅａｎｇｌｅ
Ｗ＝ｗｗｐｗ＋ｗｐｐｏ＋ｗｘｙｄｉｆｆｄｉｆｆｘｙｄｉｒ＋ｗｙａｗｄｉｆｆｙａｗ＋ｗａｎｇｌｅａｎｇｌｅ
【0061】
ヨーレート候補の重み付け係数ｗｙａｗは、予め軌道設定部４に設定されている。なお、移動ロボットをスムーズに動作させたいなどの動作性を重視する場合、ヨーレート候補の重み付け係数ｗｙａｗを他の重み付け係数に対して相対的に大きく設定してもよい。
【0062】
軌道設定部４は、上記算出した評価値Ｗが最小となるヨー角候補ｘｙｄｉｒ及びヨーレート候補ｙａｗを選択し、移動ロボットの軌道を設定する。
【0063】
なお、本実施形態３において、評価関数は、上記実施形態１に係る評価パラメータに加えて、今回選択した移動ロボットのヨーレート候補ｙａｗと前回選択したヨーレート候補ｙａｗｐｒｅｖとの差分ｄｉｆｆｙａｗを評価パラメータとして更に含む構成であってもよい。
【0064】
実施形態４
本実施形態４において、軌道設定装置１は、後述の如く、移動ロボットの予測軌道と占有格子マップとの関係を、ＬＵＴ（ルックアップテーブル）として予め計算し、そのＬＵＴを用いて、障害物ポテンシャルを算出する。
本実施形態４において、軌道設定装置１は、後述の如く、移動ロボットの予測軌道と占有格子マップとの関係を、ＬＵＴ（ルックアップテーブル）として予め計算し、そのＬＵＴを用いて、障害物ポテンシャルを算出する。
【0065】
移動ロボットは、例えば、ＬＲＦ及びＲＧＢ－Ｄなどのカメラにより、障害物点群を生成する。なお、障害物点群は、後述の如く、占有格子マップの各グリッドに近似されるため、疎でよい。
【0066】
軌道設定装置１は、図６に示す如く、生成された障害物点群を占有格子マップの各グリッドに近似して、マッピングを行う。例えば、ｏｂｊｓｐａｒｓｅ個の障害物点群の場合、その計算回数は、ｏｂｊｓｐａｒｓｅ回となる。
【0067】
軌道設定装置１は、図７（ａ）に示す如く、障害物ポテンシャルを表すａ個のポテンシャル配列を算出する。障害物を限りなく薄く考えた場合、障害物ポテンシャルは、障害物を中心とし衝突の危険性を示したもので、障害物に近づくほど衝突の危険性が高くなりポテンシャルが高くなる。
【0068】
障害物ポテンシャルは、上述の障害物ポテンシャルｐｏの算出方法で算出される。ここで、障害物ポテンシャルの範囲は、移動ロボットの半径ｒとポテンシャルの分散σｏにより決定される。
【0069】
ａ個は、下記式により算出される。ｄは、ポテンシャルが十分小さくなる障害物からの距離を示す。ｒｅｓｏｌｕｔｉｏｎは、占有格子マップのグリッド間隔である。
ａ＝（２×（ｄ／ｒｅｓｏｌｕｔｉｏｎ））２
ａ＝（２×（ｄ／ｒｅｓｏｌｕｔｉｏｎ））２
【0070】
軌道設定装置１は、図７（ａ）に示す如く、a個のポテンシャル配列の中心を、占有格子マップの各グリッドに近似した障害物点群の各点にそれぞれ合わせ、a個のポテンシャル配列を占有格子マップ上に投影することで、図７（ｂ）に示す如く、障害物ポテンシャルマップを生成する。なお、ポテンシャル配列を占有格子マップ上に投影した際に、ポテンシャルが重なる点は、高い方の値を採用する。
【0071】
障害物ポテンシャルマップのデータ型は、占有格子マップ型であり、各グリッドの座標および、そのグリッドの値（障害物ポテンシャルの大きさ、スカラ値）を有する。各グリッド値は、移動ロボットを中心（ｘｒ＝０、ｙｒ＝０）として、上記障害物ポテンシャルｐｏの式を用いて算出される。
【0072】
軌道設定装置１は、図８に示す如く、移動ロボットの予測軌道を、占有格子マップの各グリッドに近似した予測軌道点を求める。軌道設定装置１は、予測軌道点と、障害物ポテンシャルマップと、の関係を算出し、予測軌道点と、障害物ポテンシャルとを対応付けたＬＵＴを生成する。
【0073】
例えば、ＬＵＴにおいて、予測軌道点１の座標（５、－２）、予測軌道点２の座標（４、－１）、予測軌道点３の座標（３、－１）、・・・に夫々、障害物ポテンシャルが対応付けられる。これにより、予測軌道点から、ＬＵＴを参照するだけで障害物ポテンシャルを即座に得ることができる。
【0074】
本実施形態３に係る軌道設定装置１は、図９に示す如く、各処理をパイプラインで接続して実行する。例えば、従来、Ｐｓａｍｐｌｅ個の予測軌道点と、ｏｂｊｒｅａｌ個の障害物点群との間で、最近傍を探索した場合、その探索回数は、Ｐｓａｍｐｌｅ×ｏｂｊｒｅａｌとなる。したがって、全体の計算量は、点群生成計算と、探索計算Ｐｓａｍｐｌｅ×ｏｂｊｒｅａｌと、ポテンシャル計算Ｐｓａｍｐｌｅと、を加算したものになる。
【0075】
なお、実際の値として、例えば、Ｐｓａｍｐｌｅは、１６２５であり、ｏｂｊｒｅａｌは、～３０８２８０であり、ｏｂｊｓｐａｒｓｅは、～９６００であり、ａは～１４４となっている。
【0076】
一方で、本実施形態３における計算量は、図９に示すように、マッピング計算ｏｂｊｓｐａｒｓｅ回と、ポテンシャル生成計算ｏｂｊｓｐａｒｓｅ×ａと、ポテンシャル計算Ｐｓａｍｐｌｅと、を加算したものとなる。したがって、Ｐｓａｍｐｌｅ×ｏｂｊｒｅａｌ＞ｏｂｊｓｐａｒｓｅ×ａであることから、本実施形態３における計算量は、従来と比較して、大幅に少なく抑えることができ、処理の高速化が可能となる。
【0077】
本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他のさまざまな形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【0078】
本発明は、例えば、図５に示す処理を、プロセッサにコンピュータプログラムを実行させることにより実現することも可能である。
【0079】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ－ＲＯＭ（Read Only Memory）、ＣＤ－Ｒ、ＣＤ－Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【0080】
プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【0081】
なお、上述した各実施形態に係る軌道設定装置１を構成する各部は、プログラムにより実現するだけでなく、その一部または全部を、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ（Field-Programmable Gate Array）などの専用のハードウェアにより実現することもできる。
【符号の説明】
【0082】
１ 軌道設定装置、２ センサ、３ 位置算出部、４ 軌道設定部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】