特許 7502409 中間ウェイポイント生成器

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-06-10

(45)【発行日】2024-06-18

(54)【発明の名称】中間ウェイポイント生成器

(51)【国際特許分類】

   G05D 1/43 20240101AFI20240611BHJP

   G01C 21/34 20060101ALI20240611BHJP

【ＦＩ】

G05D1/43

G01C21/34

【請求項の数】 26

(21)【出願番号】P 2022502242

(86)(22)【出願日】2019-09-13

(65)【公表番号】

(43)【公表日】2022-10-07

(86)【国際出願番号】 US2019051092

(87)【国際公開番号】W WO2021025707

(87)【国際公開日】2021-02-11

【審査請求日】2022-03-11

(31)【優先権主張番号】16/569,885

(32)【優先日】2019-09-13

(33)【優先権主張国・地域又は機関】US

(31)【優先権主張番号】62/883,438

(32)【優先日】2019-08-06

(33)【優先権主張国・地域又は機関】US

(73)【特許権者】

【識別番号】518096722

【氏名又は名称】ボストン ダイナミクス，インコーポレイテッド

(74)【代理人】

【識別番号】100079108

【弁理士】

【氏名又は名称】稲葉 良幸

(74)【代理人】

【識別番号】100109346

【弁理士】

【氏名又は名称】大貫 敏史

(74)【代理人】

【識別番号】100117189

【弁理士】

【氏名又は名称】江口 昭彦

(74)【代理人】

【識別番号】100134120

【弁理士】

【氏名又は名称】内藤 和彦

(72)【発明者】

【氏名】フェイ，ジーナ，クリスティーン

(72)【発明者】

【氏名】リッツィ，アルフレッド

【審査官】大古 健一

(56)【参考文献】

【文献】特開２０１７－１８２５０２（ＪＰ，Ａ）

【文献】特表２０１９－５００６９１（ＪＰ，Ａ）

【文献】特開２０１６－０８１４０４（ＪＰ，Ａ）

【文献】特開２００６－２３９８４４（ＪＰ，Ａ）

【文献】特開２０１９－０２１１９７（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／００ － １／８７

Ｇ０１Ｃ ２１／３４

(57)【特許請求の範囲】

【請求項1】

ロボット（１０）のデータ処理ハードウェア（３６）において、ナビゲーションルート（１１２）を受信することであって、前記ナビゲーションルート（１１２）は、前記ロボット（１０）の環境（８）内の出発地（１１３）から目的地（１１４）まで走行するための第一のウェイポイント（２１０）及び第二のウェイポイント（２１０）を含む、前記ナビゲーションルート（１１２）を受信することと、
前記データ処理ハードウェア（３６）において、前記環境（８）内の１つ又は複数の障害物を示す胴部障害物マップ（４１２）を受信することと、
前記データ処理ハードウェア（３６）により、前記胴部障害物マップ（４１２）及び前記ナビゲーションルート（１１２）に基づいて第一のグラフ（４１６Ｓ）を生成することであって、前記第一のグラフ（４１６Ｓ）は、第一のノード（８１２）、第二のノード（８１２）、及び、前記第一のノード（８１２）と前記第二のノード（８１２）を接続する一つ又は複数のエッジ（８１０）を含む、前記第一のグラフ（４１６Ｓ）を生成することと、
前記データ処理ハードウェア（３６）により、前記第一のノード（８１２）から前記第二のノード（８１２）までの経路（１３１０）を計画することと、
前記データ処理ハードウェア（３６）により、前記ロボット（１０）のイメージセンサ（３１）の視線が前記１つ又は複数の障害物のうちの障害物と交差する前記経路（１３１０）の点（１５３０）を特定することと、
前記データ処理ハードウェア（３６）により、前記点（１５３０）を特定することに基づいて、前記点（１５３０）において中間ウェイポイント（３１０）を生成することと、
前記データ処理ハードウェア（３６）により、前記第一のウェイポイント（２１０）と前記第二のウェイポイント（２１０）との間の前記ナビゲーションルート（１１２）に前記中間ウェイポイント（３１０）を追加することと、
前記データ処理ハードウェア（３６）により、前記第一のウェイポイント（２１０）、前記中間ウェイポイント（３１０）、及び前記第二のウェイポイント（２１０）を介した前記出発地（１１３）から前記目的地（１１４）までの前記ロボット（１０）の走行を命令することと、を含む方法（１６００）。

【請求項2】

前記第一のウェイポイント（２１０）、前記第二のウェイポイント（２１０）、及び前記中間ウェイポイント（３１０）の各々は、平面内の位置を示す１つ又は複数のそれぞれの座標、ヨー値、及び時刻値に関連付けられ、前記時刻値は、前記ロボット（１０）がそれぞれの前記ウェイポイント（２１０、３１０）へと走行するための推定時間量を示す、請求項１に記載の方法（１６００）。

【請求項3】

前記データ処理ハードウェア（３６）により、前記ナビゲーションルート（１１２）内に前記第一のウェイポイント（２１０）及び前記第二のウェイポイント（２１０）を保持することをさらに含む、請求項１又は２に記載の方法（１６００）。

【請求項4】

前記第一のグラフ（４１６Ｓ）を生成することは、
前記胴部障害物マップ（４１２）に基づいてマップ（４１４）を生成することであって、前記マップ（４１４）は、複数のエレメント（５１０）からなる第一の二次元グリッドを含み、前記複数のエレメント（５１０）からなる前記第一の二次元グリッドの各エレメント（５１０）は、前記ロボット（１０）のそれぞれのヨーコンフィギュレーションセット（５１２）に関連付けられる、前記マップ（４１４）を生成することと、
前記マップ（４１４）から圧縮マップ（５００）を生成することであって、前記圧縮マップ（５００）は、複数のエレメント（５１０）からなる第二の二次元グリッドを含む、前記圧縮マップ（５００）を生成することと、
前記圧縮マップ（５００）から前記第一のグラフ（４１６Ｓ）を生成することと、
を含む、請求項１～３の何れか１項に記載の方法（１６００）。

【請求項5】

前記第一のノード（８１２）から前記第二のノード（８１２）までの前記経路（１３１０）を計画することは、
前記圧縮マップ（５００）から第二のグラフ（４１６Ｄ）を生成することであって、前記第二のグラフ（４１６Ｄ）は、ヨー自由ゾーン（５１０ＣＺ）に分類される前記複数のエレメント（５１０）からなる前記第二の二次元グリッドのうちの一つ又は複数のエレメント（５１０）を含む、前記第二のグラフ（４１６Ｄ）を生成することと、
前記第一のグラフ（４１６Ｓ）からの一つ又は複数のエッジ（８１０）を前記一つ又は複数のエレメント（５１０）に関係付けることと、
を含む、請求項４に記載の方法（１６００）。

【請求項6】

前記一つ又は複数のエッジ（８１０）を前記一つ又は複数のエレメント（５１０）に関係付けることは、
前記第一のグラフ（４１６Ｓ）と前記第二のグラフ（４１６Ｄ）を複合させて第三のグラフ（４１６Ｆ）を生成することと、
前記第三のグラフ（４１６Ｆ）について探索アルゴリズムを実行することと、
を含む、請求項５に記載の方法（１６００）。

【請求項7】

前記第一のグラフ（４１６Ｓ）を生成することは、複数のセル（６１０）を前記圧縮マップ（５００）の上に重ねることをさらに含む、請求項４に記載の方法（１６００）。

【請求項8】

前記第一のグラフ（４１６Ｓ）を生成することは、前記複数のセル（６１０）のうちの各セル（６１０）をエッジ（８１０）又はノード（８１２）の何れかに分類することをさらに含む、請求項７に記載の方法（１６００）。

【請求項9】

各セル（６１０）を分類することは、フラッドフィルアルゴリズムを実行することを含む、請求項８に記載の方法（１６００）。

【請求項10】

前記第一のノード（８１２）から前記第二のノード（８１２）までの前記経路（１３１０）を計画することは、一つ又は複数のエッジ（８１０）を枝切りすることを含む、請求項４に記載の方法（１６００）。

【請求項11】

前記点（１５３０）を特定することは、
前記経路（１３１０）に関連付けられた最小ヨー及び最大ヨー（１５５０）を特定することと、
前記最小ヨーと前記最大ヨー（１５５０）に基づいて包絡線（１５６０）を特定することと、
前記点（１５３０）におけるヨーが前記包絡線（１５６０）の外にあることを特定することと、
を含む、請求項１に記載の方法（１６００）。

【請求項12】

前記データ処理ハードウェア（３６）により、前記ナビゲーションルート（１１２）を下位経路生成器（１３０）に送信することをさらに含む、請求項１～１１の何れか１項に記載の方法（１６００）。

【請求項13】

前記データ処理ハードウェア（３６）により、前記中間ウェイポイント（３１０）を前記ナビゲーションルート（１１２）に追加すべきか否かを特定することと、
前記中間ウェイポイント（３１０）を前記ナビゲーションルート（１１２）に追加すると特定したことに応答して、前記データ処理ハードウェア（３６）により、変更されたナビゲーションルート（１１２）を前記下位経路生成器（１３０）へと伝えることと、
をさらに含む、請求項１２に記載の方法（１６００）。

【請求項14】

ロボット（１０）において、
胴部（１１）と、
前記胴部（１１）に連結され、前記ロボット（１０）を環境（８）内で操縦するように構成された脚（１２）と、
前記脚（１２）と通信するデータ処理ハードウェア（３６）と、
前記データ処理ハードウェア（３６）と通信するメモリハードウェア（３８）であって、前記データ処理ハードウェア（３６）上で実行されると、前記データ処理ハードウェア（３６）に、
ナビゲーションルート（１１２）を受信することであって、前記ナビゲーションルート（１１２）は、前記ロボット（１０）の環境（８）内の出発地（１１３）から目的地（１１４）まで走行するための第一のウェイポイント（２１０）及び第二のウェイポイント（２１０）を含む、前記ナビゲーションルート（１１２）を受信することと、
前記環境（８）内の１つ又は複数の障害物を示す胴部障害物マップ（４１２）を受信することと、
前記胴部障害物マップ（４１２）及び前記ナビゲーションルート（１１２）に基づいて第一のグラフ（４１６Ｓ）を生成することであって、前記第一のグラフ（４１６Ｓ）は、第一のノード（８１２）、第二のノード（８１２）、及び、前記第一のノード（８１２）と前記第二のノード（８１２）を接続する一つ又は複数のエッジ（８１０）を含む、前記第一のグラフ（４１６Ｓ）を生成することと、
前記第一のノード（８１２）から前記第二のノード（８１２）までの経路（１３１０）を計画することと、
前記ロボット（１０）のイメージセンサ（３１）の視線が前記１つ又は複数の障害物のうちの障害物と交差する前記経路（１３１０）の点（１５３０）を特定することと、
前記点（１５３０）を特定することに基づいて、前記点（１５３０）において中間ウェイポイント（３１０）を生成することと、
前記中間ウェイポイント（３１０）を、前記第一のウェイポイント（２１０）と前記第二のウェイポイント（２１０）との間の前記ナビゲーションルート（１１２）に追加することと、
前記第一のウェイポイント（２１０）、前記中間ウェイポイント（３１０）、及び前記第二のウェイポイント（２１０）を介した前記出発地（１１３）から前記目的地（１１４）までの前記ロボット（１０）の走行を命令することと、
を含む動作を実行させる命令を記憶するメモリハードウェア（３８）と、
を含むロボット（１０）。

【請求項15】

前記第一のウェイポイント（２１０）、前記第二のウェイポイント（２１０）、及び前記中間ウェイポイント（３１０）の各々は、平面内の位置を示す１つ又は複数のそれぞれの座標、ヨー値、及び時刻値に関連付けられ、前記時刻値は、前記ロボット（１０）がそれぞれの前記ウェイポイント（２１０、３１０）へと走行するための推定時間量を示す、請求項１４に記載のロボット（１０）。

【請求項16】

前記ナビゲーションルート（１１２）内に前記第一のウェイポイント（２１０）及び前記第二のウェイポイント（２１０）を保持することをさらに含む、請求項１４又は１５に記載のロボット（１０）。

【請求項17】

前記第一のグラフ（４１６Ｓ）を生成することは、
前記胴部障害物マップ（４１２）に基づいてマップ（４１４）を生成することであって、前記マップ（４１４）は複数のエレメント（５１０）からなる第一の二次元グリッドを含み、前記複数のエレメント（５１０）からなる前記第一の二次元グリッドの各エレメント（５１０）は前記ロボット（１０）のそれぞれのヨーコンフィギュレーションセット（５１２）に関連付けられる、前記マップ（４１４）を生成することと、
前記マップ（４１４）から圧縮マップ（５００）を生成することであって、前記圧縮マップ（５００）は、複数のエレメント（５１０）からなる第二の二次元グリッドを含む、前記圧縮マップ（５００）を生成することと、
前記圧縮マップ（５００）から前記第一のグラフ（４１６Ｓ）を生成することと、
を含む、請求項１４～１６の何れか１項に記載のロボット（１０）。

【請求項18】

前記第一のノード（８１２）から前記第二のノード（８１２）までの前記経路（１３１０）を計画することは、
前記圧縮マップ（５００）から第二のグラフ（４１６Ｄ）を生成することであって、前記第二のグラフ（４１６Ｄ）は、ヨー自由ゾーン（５１０ＣＺ）に分類される前記複数のエレメント（５１０）からなる前記第二の二次元グリッドのうちの一つ又は複数のエレメント（５１０）を含む、前記第二のグラフ（４１６Ｄ）を生成することと、
前記第一のグラフ（４１６Ｓ）からの一つ又は複数のエッジ（８１０）を前記一つ又は複数のエレメント（５１０）に関係付けることと、
を含む、請求項１７に記載のロボット（１０）。

【請求項19】

前記一つ又は複数のエッジ（８１０）を前記一つ又は複数のエレメント（５１０）に関係付けることは、
前記第一のグラフ（４１６Ｓ）と前記第二のグラフ（４１６Ｄ）を複合させて第三のグラフ（４１６Ｆ）を生成することと、
前記第三のグラフ（４１６Ｆ）について探索アルゴリズムを実行することと、
を含む、請求項１８に記載のロボット（１０）。

【請求項20】

前記第一のグラフ（４１６Ｓ）を生成することは、複数のセル（６１０）を前記圧縮マップ（５００）の上に重ねることをさらに含む、請求項１７に記載のロボット（１０）。

【請求項21】

前記第一のグラフ（４１６Ｓ）を生成することは、前記複数のセル（６１０）のうちの各セル（６１０）をエッジ（８１０）又はノード（８１２）の何れかに分類することをさらに含む、請求項２０に記載のロボット（１０）。

【請求項22】

各セル（６１０）を分類することは、フラッドフィルアルゴリズムを実行することを含む、請求項２１に記載のロボット（１０）。

【請求項23】

前記第一のノード（８１２）から前記第二のノード（８１２）までの前記経路（１３１０）を計画することは、一つ又は複数のエッジ（８１０）を枝切りすることを含む、請求項１７に記載のロボット（１０）。

【請求項24】

前記点（１５３０）を特定することは、
前記経路（１３１０）に関連付けられた最小ヨー及び最大ヨー（１５５０）を特定することと、
前記最小ヨーと前記最大ヨー（１５５０）に基づいて包絡線（１５６０）を特定することと、
前記点（１５３０）におけるヨーが前記包絡線（１５６０）の外にあることを特定することと、
を含む、請求項１４に記載のロボット（１０）。

【請求項25】

前記ナビゲーションルート（１１２）を下位経路生成器（１３０）に送信することをさらに含む、請求項１４～２４の何れか１項に記載のロボット（１０）。

【請求項26】

前記中間ウェイポイント（３１０）を前記ナビゲーションルート（１１２）に追加すべきか否かを特定することと、
前記中間ウェイポイント（３１０）を前記ナビゲーションルート（１１２）に追加すると特定したことに応答して、変更されたナビゲーションルート（１１２）を前記下位経路生成器（１３０）へと伝えることと、
をさらに含む、請求項２５に記載のロボット（１０）。

【発明の詳細な説明】

【技術分野】

【0001】

技術分野
[0001] 本開示は、制約、特に地形により加えられる制約の存在時に中間ウェイポイントを生成することに関する。

【背景技術】

【0002】

背景
[0002] ロボット機器は、制約された環境内を走行して様々な作業や機能を果たすためにますます利用されている。これらのロボット機器は多くの場合、このような制約された環境内で障害物と接触しないように、又は閊えたり、引っ掛かったりしないように走行する必要がある。こうしたロボット機器がさらに普及する中で、目的地までうまく走行しながら、障害物との接触を回避するリアルタイムでのナビゲーションとルート計画が求められている。

【発明の概要】

【課題を解決するための手段】

【0003】

概要
[0003] 本開示の１つの態様は、ロボットのナビゲーションシステムのための中間ウェイポイントを生成する方法を提供する。方法は、ロボットのデータ処理ハードウェアにおいて、ナビゲーションルートを受信することを含む。ナビゲーションルートは、出発地から始まって目的地で終わり、上位ナビゲーションデータに基づく一連の上位ウェイポイントを含む。上位ナビゲーションデータは、ロボットが走行する領域内の静止障害物の場所を表す。方法はまた、データ処理ハードウェアにおいて、イメージセンサからのロボットの周囲の環境の画像データを受信することも含む。方法はまた、データ処理ハードウェアにより、少なくとも１つの中間ウェイポイントを画像データに基づいて生成することも含む。方法はまた、データ処理ハードウェアにより、ナビゲーションルートの一連の上位ウェイポイントに少なくとも１つの中間ウェイポイントを追加することと、データ処理ハードウェアにより、ロボットを出発地から一連の上位ウェイポイント及び少なくとも１つの中間ウェイポイントに沿って目的地に向かってナビゲートすることも含む。

【0004】

[0004] 本開示の実施例は、以下の任意選択的特徴の１つ又は複数を含み得る。幾つかの実施例において、各上位ウェイポイントと各中間ウェイポイントは、平面内の位置を示す２つの座標、ヨー値、及び時刻値を含む。時刻値は、ロボットがそれぞれのウェイポイントへと走行する推定時間量を示す。幾つかの例において、方法は、データ処理ハードウェアにより、一連の上位ウェイポイントの各々をナビゲーションルート上に保持することをさらに含む。方法はまた、データ処理ハードウェアにおいて、胴部障害物マップを受信することも含み得る。胴部障害物マップは、ロボットの胴部が通過できない障害物のマップを含む。それに加えて、少なくとも１つの中間ウェイポイントを生成することは胴部障害物マップに基づく疎グラフを生成することも含み得る。疎グラフは、ノード及びエッジのリストを含む。ノード及びエッジは、ロボットが環境中で移動し得る経路を表す。方法はまた、ノードのリストのうちの第一のノードからノードのリストのうちの第二のノードまでの概略経路を計画することも含み得る。第一のノードと第二のノードは各々、環境内のある空間を表す。方法はまた、概略経路に沿った、イメージセンサの視線が失われる点を特定することと、少なくとも１つの中間ウェイポイントの１つを視線が失われる点において生じさせることと、も含み得る。

【0005】

[0005] 幾つかの実施例において、疎グラフを生成することは、胴部障害物マップに基づいてコンフィギュレーション空間全体マップを生成することを含む。コンフィギュレーション空間全体マップはエレメントの二次元グリッドを含み、グリッドの各エレメントは環境のある空間を表し、コンフィギュレーション空間全体マップの各エレメントはそれぞれのヨーコンフィギュレーションセットを含み得る。各ヨーコンフィギュレーションは有効か無効かに分類され得、有効ヨーコンフィギュレーションはロボットの、それぞれのエレメントに関連付けられた空間における障害物との接触の点で安全なヨーコンフィギュレーションを表し、無効ヨーコンフィギュレーションはロボットの、それぞれのエレメントに関連付けられた空間における障害物との接触の点で安全でないヨーコンフィギュレーションを表す。方法はまた、コンフィギュレーション空間全体マップからコンフィギュレーション空間圧縮マップを生成することも含み得る。コンフィギュレーション空間圧縮マップは、エレメントの第二の二次元グリッドを含み、第二のグリッドの各エレメントは環境のある空間を表す。第二のグリッドの各エレメントはまた、（ｉ）ヨー衝突ゾーン、（ｉｉ）ヨー自由ゾーン、又は（ｉｉｉ）ヨー制約ゾーンのうちの１つに分類される。方法は、幾つかの例において、コンフィギュレーション空間圧縮マップから疎グラフを生成することを含む。

【0006】

[0006] 任意選択により、第一のノードから第二のノードまでの概略経路を計画することは、コンフィギュレーション空間圧縮マップから密グラフを生成することを含む。密グラフは、ヨー自由ゾーンに分類されるエレメントを含む。方法はまた、疎グラフからのエッジを密グラフからのエレメントに関係付けることも含み得る。エッジを密グラフのエレメントに関係付けることは、幾つかの実施例において、疎グラフと密グラフを複合させて最終グラフを生成することを含む。幾つかの例において、方法はまた、最終グラフについてＡ^＊探索アルゴリズムを実行することも含む。

【0007】

[0007] 幾つかの例において、疎グラフを生成することは、複数のボロノイセルをコンフィギュレーション空間圧縮マップの上に重ねることをさらに含み、各ボロノイセルは、疎グラフ上でヨー制約ゾーンに分類される。各ボロノイセルは、ヨー衝突ゾーンに分類される少なくとも２つのエレメントから等距離にある。任意選択により、疎グラフを生成することは、各ボロノイセルをエッジ又はノードの何れかに分類することをさらに含む。各ボロノイセルを分類することは、フラッドフィルアルゴリズムを実行することを含み得る。

【0008】

[0008] 幾つかの実施例において、第一のノードから第二のノードへの概略経路を計画することは、エッジを枝切りすることを含み、枝切りされた各エッジは、閾値長さ未満であり、ヨー衝突ゾーン又はヨー制約ゾーンの何れかに分類されるエレメントを含む。概略経路に沿った、イメージセンサによる視線が失われる点を特定することは、計画された概略経路に沿った各エレメントにおける最小許容ヨー及び最大許容ヨーを特定することと、最小許容ヨーと最大許容ヨーに基づいて最小包絡線を特定することと、をさらに含み得る。方法はまた、概略経路上のある点における必要なヨーを最小包絡線の外にあることを特定することも含み得る。

【0009】

[0009] 幾つかの例において、方法は、データ処理ハードウェアにより、上位ウェイポイントと少なくとも１つの中間ウェイポイントを有するナビゲーションルートを下位経路生成器に送信することをさらに含む。方法はまた、データ処理ハードウェアにより、少なくとも１つの中間ウェイポイントをナビゲーションルートに追加すべきか否かを特定することと、少なくとも１つの中間ウェイポイントをナビゲーションルートに追加しないと特定したことに応答して、データ処理ハードウェアにより、ナビゲーションルートを変更せずに下位経路生成器へと伝えることも含み得る。

【0010】

[0010] 本開示の他の態様は、胴部と、胴部に連結され、環境内でロボットを操縦するように構成された脚と、を含むロボットを提供する。ロボットはまた、脚と通信するデータ処理ハードウェアと、データ処理ハードウェアと通信するメモリハードウェアも含む。メモリハードウェアは、データ処理ハードウェアにより実行されると、データ処理ハードウェアに動作を実行させる命令を記憶する。動作は、ナビゲーションルートを受信することを含む。ナビゲーションルートは、出発点から始まって目的地で終わる一連の上位ウェイポイントを含み、上位ナビゲーションデータに基づく。上位ナビゲーションデータは、ロボットが走行することになる領域内の静止障害物の場所を表す。動作はまた、イメージセンサからのロボットの周囲の環境の画像データを受信することも含む。動作はまた、画像データに基づいて少なくとも１つの中間ウェイポイントを生成することも含む。動作はまた、少なくとも１つの中間ウェイポイントをナビゲーションルートの一連の上位ウェイポイントに追加することと、ロボットを出発地から一連の上位ウェイポイントと少なくとも１つの中間ウェイポイントに沿って目的地に向かってナビゲートすること、も含む。

【0011】

[0011] この態様は、以下の任意選択的特徴の１つ又は複数を含み得る。幾つかの実施形態において、各上位ウェイポイントと各中間ウェイポイントは、平面上の位置を示す２つの座標、ヨー値、及び時刻値を含む。時刻値は、ロボットがそれぞれのウェイポイントまで走行する推定時間量を示す。幾つかの例において、動作は、一連の上位ウェイポイントの各々をナビゲーションルート上に保持することをさらに含む。動作はまた、胴部障害物マップを受信することも含み得る。胴部障害物マップは、ロボットの胴部が通過できない障害物のマップを含む。追加的に、少なくとも１つの中間ウェイポイントを生成することは、胴部障害物マップに基づいて疎グラフを生成することを含み得る。疎グラフは、ノード及びエッジのリストを含む。ノード及びエッジは、ロボットが環境中で移動し得る経路を表す。動作はまた、ノードのリストのうちの第一のノードからノードのリストのうちの第二のノードまでの概略経路を計画することも含み得る。第一のノードと第二のノードは各々、環境中のある空間を表す。動作はまた、概略経路に沿った、イメージセンサの視線が失われる点を特定することと、少なくとも１つの中間ウェイポイントのうちの１つを視線が失われるこの点において生成することも含み得る。

【0012】

[0012] 幾つかの実施例において、疎グラフを生成することは、胴部障害物マップに基づいてコンフィギュレーション空間全体マップを生成することを含む。コンフィギュレーション空間全体マップは、エレメントの二次元グリッドを含み得て、グリッドの各エレメントは環境のある空間を表し、コンフィギュレーション空間全体マップの各エレメントはそれぞれのヨーコンフィギュレーションセットを含む。各ヨーコンフィギュレーションは有効か無効かに分類され、有効ヨーコンフィギュレーションは、それぞれのエレメントに関連付けられる空間において障害物と接触しないという点で安全なロボットのヨーコンフィギュレーションを表し、無効ヨーコンフィギュレーションは、それぞれのエレメントに関連付けられる空間において障害物と接触するという点で安全でないロボットのヨーコンフィギュレーションを表す。動作はまた、コンフィギュレーション空間全体マップからコンフィギュレーション空間圧縮マップを生成することも含み得る。コンフィギュレーション空間圧縮マップは、エレメントの第二の二次元グリッドを含み、第二のグリッドの各エレメントは環境のある空間を表す。第二のグリッドの各エレメントはまた、（ｉ）ヨー衝突ゾーン、（ｉｉ）ヨー自由ゾーン、又は（ｉｉｉ）ヨー制約ゾーンのうちの１つに分類される。動作は、幾つかの例において、コンフィギュレーション空間圧縮マップから疎グラフを生成することを含む。

【0013】

[0013] 任意選択的に、第一のノードから第二のノードまでの概略経路を計画することは、コンフィギュレーション空間圧縮マップから密グラフを生成することを含む。密グラフは、ヨー自由ゾーンに分類されるエレメントを含む。動作はまた、疎グラフからのエッジを密グラフからのエレメントに関係付けることも含み得る。エッジを密グラフからのエレメントに関係付けることは、幾つかの実施例において、疎グラフと密グラフを組み合わせて最終グラフを生成すること含む。幾つかの例において、動作はまた、最終グラフについてＡ^＊探索アルゴリズムを実行することも含む。

【0014】

[0014] 幾つかの例において、疎グラフを生成することは、複数のボロノイセルをコンフィギュレーション空間圧縮マップの上に重ねることをさらに含み、各ボロノイセルは、疎グラフ上のヨー制約ゾーンに分類される。各ボロノイセルは、ヨー衝突ゾーンに分類される少なくとも２つのエレメントから等距離にある。任意選択的に、疎グラフを生成することは、各ボロノイセルをエッジ又はノードの何れかに分類することを含む。各ボロノイセルを分類することは、フラッドフィルアルゴリズムを実行することを含み得る。

【0015】

[0015] 幾つかの実施形態において、第一のノードから第二のノードまでの概略経路を計画することは、エッジを枝切りすることを含み、枝切りされた各エッジは閾値長さ未満であり、ヨー衝突ゾーン又はヨー制約ゾーンの何れに分類されるエレメントを含む。概略経路に沿った、イメージセンサの視線が失われる点を特定することは、計画された概略経路に沿った各エレメントにおける最小許容ヨーと最大許容ヨーを特定することと、最小許容ヨーと最大許容ヨーに基づいて最小包絡線を特定することを含み得る。動作はまた、概略経路上のある点において必要なヨーが最小包絡線の外にあることを検出することも含み得る。

【0016】

[0016] 幾つかの例において、動作は、上位ウェイポイントと少なくとも１つの中間ウェイポイントを有するナビゲーションルートを下位経路生成器に送信することをさらに含む。動作はまた、少なくとも１つの中間ウェイポイントをナビゲーションルートに追加すべきか否かを特定することと、少なくとも１つの中間ウェイポイントをナビゲーションルートに追加しないことを特定したことに応答して、ナビゲーションルートを変更せずに下位経路生成器に伝えることも含み得る。

【0017】

[0017] 本開示の１つ又は複数の実施例の詳細は、添付の図面及び以下の説明の中に示されている。その他の態様、特徴、及び利点は図面の説明から、及び特許請求の範囲から明らかとなるであろう。

【図面の簡単な説明】

【0018】

図面の説明

【図1】[0018]ロボットのためのナビゲーションシステムの、中間ウェイポイントを生成するシステムの例示的システムの概略図である。

【図2A】[0019]上位ウェイポイントを使用するナビゲーションルートを走行するロボットの概略図である。

【図2B】[0019]上位ウェイポイントを使用するナビゲーションルートを走行するロボットの概略図である。

【図3】[0020]上位ウェイポイント及び中間ウェイポイントを使用する図２Ａ及び２Ｂのナビゲーションルートを走行するロボットの概略図である。

【図4】[0021]図１のナビゲーションシステムの例示的な中間ウェイポイント生成器の概略図である。

【図5】[0022]コンフィギュレーション空間圧縮マップの概略図である。

【図6】[0023]ボロノイセルを重ねた図５のコンフィギュレーション空間マップの概略図である。

【図7】[0024]エレメントとボロノイセルの最も近い障害物との間の距離を示す、図６のボロノイセルの概略図である。

【図8A】[0025]図６のボロノイセルを塗りつぶすフラッドフィルアルゴリズムの概略図である。

【図8B】[0025]図６のボロノイセルを塗りつぶすフラッドフィルアルゴリズムの概略図である。

【図8C】[0025]図６のボロノイセルを塗りつぶすフラッドフィルアルゴリズムの概略図である。

【図8D】[0025]図６のボロノイセルを塗りつぶすフラッドフィルアルゴリズムの概略図である。

【図8E】[0025]図６のボロノイセルを塗りつぶすフラッドフィルアルゴリズムの概略図である。

【図9】[0026]図６のボロノイセルから得られたエッジとノードの概略図である。

【図10】[0027]図６のボロノイセルから枝切りされた得られたエッジとノードの概略図である。

【図11A】[0028]二次元疎グラフの概略図である。

【図11B】[0028]ＳＥ２疎グラフの概略図である。

【図12A】[0029]ゴールの目的地のための追加ノードの概略図である。

【図12B】[0029]ゴールの目的地のための追加ノードの概略図である。

【図13】[0030]ヨー自由ゾーン及びヨー制約ゾーンを通る経路の概略図である。

【図14】[0031]ヨー制約ゾーンを通る経路の概略図である。

【図15A】[0032]視線が失われる場所に設置された中間ウェイポイントを含むロボットのためのナビゲーションルートの概略図である。

【図15B】[0032]視線が失われる場所に設置された中間ウェイポイントを含むロボットのためのナビゲーションルートの概略図である。

【図16】[0033]ロボットのナビゲーションシステムのための中間ウェイポイントを生成する例示的な方法のフローチャートである。

【図17】[0034]本明細書に記載のシステム及び方法を実施するために使用され得る例示的なコンピューティングデバイスの概略図である。

【発明を実施するための形態】

【0019】

[0035] 各種の図面中の同様の参照記号は同様の要素を示す。

【0020】

[0036] 脚式ロボット装置（「ロボット」とも呼ぶ）が主流となりつつあるのに伴い、ロボットを様々な制約のある環境中で自発的にナビゲートする必要性が増している。ロボットは大型及び／又は静止物体（例えば、壁、ドア等）に関する情報を記憶する上位マップデータに依拠することが多い。目的地のゴールが与えられると、ロボットは多くの場合、まず、この上位マップから経路又はコースをプロットして静止障害物を避けて走行し、その後、下位ナビゲーションデータを収集する認識システムに依拠して、移動中に遭遇する小型の動的物体を避けて走行する。この種の環境中を走行する場合、ロボットは、上位マップでは考慮されないが、下位システムでそれを避けるには大きすぎる障害物に頻繁に遭遇する。このような状況では、ロボットが閊えたり、引っ掛かったりすることは珍しくない。本願の実施例は、上位ルートに対する中間ウェイポイントをリアルタイムで生成し、それゆえ脚式ロボット機器がより大型の動的物体間を効率的に走行できるようにする中間ウェイポイント生成器のためのシステムと方法に関する。

【0021】

[0037] 図１を参照すると、ロボット又はロボット機器１０は胴部１１と２つ以上の脚１２を含み、ロボット１０が制約のある環境８中を走行できるようにするナビゲーションシステム１００を実行する。各脚１２は胴部１１に連結され、脚ジョイント１８により分離される上部分１４と下部分１６を有し得る。各脚１２の下部分１６は足１９で終わる。各脚の足１９は任意選択的であり、脚１２のうちの１つ又は複数の下部分の終端は車輪に連結され得、又は各脚１２の先端は地表面９に直接接触し得る。ロボット１０は、重力方向に沿った垂直重力軸Ｖｇと、質量中心ＣＭを有し、これはロボット１０の分散した質量の加重相対位置の合計がゼロになる点である。ロボット１０は、ロボット１０がとる特定の姿勢又はスタンスを定義するための、垂直重力軸Ｖｇ（すなわち、重力に関する固定参照フレーム）に関するＣＭに基づくポーズＰをさらに有する。ロボット１０の姿勢は、空間内のロボット１０の姿勢角度又は角度位置により定義できる。脚１２の胴部１１に関する移動により、ロボット１０のポーズＰ（すなわち、ロボットのＣＭの位置とロボット１０の姿勢又は姿勢角度との組合せ）が変わる。

【0022】

[0038] 幾つかの実装形態において、ロボット１０は１つ又は複数の付属物をさらに含み、これは例えば胴部１１上に配置され、胴部１１に関して移動するように構成された関節腕２０である。関節腕２０は、５又はそれ以上の自由度を有し得る。さらに、関節腕２０は、マニピュレーションアーム又は単に付属物と互換的に呼ばれ得る。図の例において、関節腕２０は、相互に関して、及び胴部１１にも関して回転可能な２つの部分２２、２４を含むが、関節腕２０はそれより多い、又は少ない部分を含んでもよく、これらも本開示の範囲から逸脱しない。第一の部分２２は、第二の部分２４から関節腕ジョイント２６により分離され得る。エンドエフェクタ２８は、マニピュレータヘッド２８と互換的に呼ばれ得、関節腕２０の第二の部分２４の先端に連結され得、物体を握り／把持するための１つ又は複数のアクチュエータ２９を含み得る。

【0023】

[0039] ロボット１０はまた、少なくとも１つのイメージングセンサ又はカメラ３１を備えるビジョンシステム３０も含み、各センサ又はカメラ３１はロボット１０を取り巻く環境８の画像データ又はセンサデータ１７を視野角３２及び視野３４で捕捉する。ビジョンシステム３０は、視野角３２を調整することによって、又は視野３４を何れかの方向に移動させるために（ロボット１０とは独立して、又はロボット１０を介して）カメラ３１をパンニング及び／又はチルティングすることによって、視野３４を移動させるように構成され得る。代替的に、ビジョンシステム３０は複数のセンサ又はカメラ３１を含み得て、ビジョンシステム３０はロボット１０の周囲の概して３６０度の視野を捕捉する。

【0024】

[0040] ビジョンシステム３０のカメラ３１は、幾つかの実装形態において、１つ又は複数のステレオカメラ（例えば、１つ又は複数のＲＧＢＤステレオカメラ）を含む。他の例では、ビジョンシステム３０は、走査型光検知測距（ＬＩＤＡＲ）センサ若しくは走査型レーザ検出測距（ＬＡＤＡＲ）センサ等の１つ又は複数のレーダセンサ、光スキャナ、飛行時間センサ、又は他の３次元（３Ｄ）体積イメージセンサ（又はセンサのあらゆるこのような組合せ）を含む。幾つかの実施例において、ナビゲーションシステム１００は認識システム１０２を含み、これはセンサデータ１７を受信して処理し、処理されたセンサデータ１７を中間ウェイポイント生成器１２０及び下位経路生成器１３０へと伝える。

【0025】

[0041] ビジョンシステム３０は、カメラ又はセンサ３１により捕捉された画像データから得られた画像データ又はセンサデータ１７をロボット１０のデータ処理ハードウェア３６に提供する。データ処理ハードウェア３６は、メモリハードウェア３８とデジタル通信し、幾つかの実装形態において、リモートシステムであり得る。リモートシステムは、単一のコンピュータ、複数のコンピュータ、又はスケーラブル／エラスティックコンピューテングリソース及び／又は記憶リソースを有する分散システム（例えば、クラウド環境）であり得る。ロボット１０のナビゲーションシステム１００は、データ処理ハードウェア３６上で実装される。

【0026】

[0042] 図の例において、ナビゲーションシステム１００は上位ウェイポイント生成器１１０を含み、これはマップデータ５０（すなわち、ロボット１０が走行することになる領域内の静止障害物の場所を表す上位ナビゲーションデータ）を受信し、ナビゲーションルート１１２のための、ロボット１０の現在場所からゴールの目的地までの大型及び／又は静止障害物を避ける概略経路をプロットする１つ又は複数の上位ウェイポイント２１０（図２Ａ及び２Ｂ）を生成する。例えば、上位ウェイポイント生成器１１０は、１０メートルを超える範囲（例えば、ロボット１０から１０メートルを超える距離）にわたる概略経路を生成し得る。ナビゲーションシステム１００はまた、中間ウェイポイント生成器１２０も含み、これはナビゲーションルート１１２と、ビジョンシステム３０からの画像又はセンサデータ１７を受信し、中間ウェイポイント生成器１２０を含む。センサデータ１７に基づいて、中間ウェイポイント生成器１２０は、ロボット１０が移動するための概略経路又は軌道を形成するナビゲーションルート１１２の上位ウェイポイント２１０（図２Ａ、２Ｂ、及び３）に、１つ又は複数の中間ウェイポイント３１０（図３）を追加し得る。ナビゲーションルート１１２の上位ウェイポイント２１０及び追加された中間ウェイポイント３１０は下位経路生成器１３０へと伝えられ、これは、センサデータ１７と組み合わせて、ロボット１０の現在の場所から次のウェイポイント２１０、３１０まで走行するためのロボット１０の個々のステップの各々をプロットするステップ計画１４２を生成する。ステップ計画１４２を使って、ロボット１０は、ステップ計画１４２にしたがうことによって足１９又は脚１２の先端をステップ計画１４２により示される場所における地表面９に置いて、環境８中を進む。幾つかの実施例において、下位経路生成器１３０は、より下位の障害物回避を、ほぼ４メートル規模の障害物を回避するポテンシャル場方式を使用することにより提供する。ポテンシャル場方式は、ゴールに向かう速度と障害物回避速度の和を積分することにより、ロボット１０を障害物から離れるように「押しやる」ことができる。

【0027】

[0043] 幾つかの実施例において、ナビゲーションシステム１００の少なくとも一部は、ロボット１０と通信するリモートデバイス上で実装される。例えば、上位ウェイポイント生成器１１０はリモートデバイス上で実装されて、上位ウェイポイント２１０の１つ又は複数を生成し得て、ロボット１０上で実装される中間ウェイポイント生成器１２０は上位ウェイポイント２１０をリモートデバイスから受信し得る。任意選択的に、ナビゲーションシステム１００の全体がリモートデバイス上で実装され得て、リモートデバイスはロボット１０を制御し、及び／又はそれに命令して、ステップ計画１４２に基づいて環境８内を進行するようにし得る。

【0028】

[0044] 次に図２Ａ及び２Ｂを参照すると、下位経路生成器１３０は（例えば、ポテンシャル場方式を使用）、特定の形状及び／又は凸面の障害物間を走行する際に問題に遭遇し得る。例えば、下位経路生成器１３０は、対応するナビゲーションルート１１２の上位ウェイポイント２１０まで走行しようとしているときに、障害物又は表面２２０上を滑り落ちて、極小２３０に引っ掛かるかもしれない（図２Ａ）。他の例では、下位経路生成器１３０は、ロボット１０を方向転換させることに関する、必要なコンテキスト又は意思決定能力を持たないかもしれない。すなわち、下位経路生成器１３０は、いつ「方向転換してから走行」すべきで、いつ「走行してから方向転換」すべきかを区別するのに苦慮し得る（図２Ｂ）。

【0029】

[0045] 次に図３を参照すると、追加のウェイポイント（すなわち、中間ウェイポイント３１０）をナビゲーションルート１１２に追加することが、下位経路生成器１３０が適正なトポロジ上の判断を下すのを助けるが、これは、追加のウェイポイント３１０によって、下位経路生成器１３０にとって軌道が凸状に見え、下位経路生成器が障害物２２０を回避してうまくナビゲートできるようにし得るからである。中間ウェイポイント生成器１２０は、ナビゲーションルート１１２に追加される何れの追加的中間ウェイポイント３１０もロボット１０から、マップジッタの影響を最小化するのに十分な距離にあることを確実にし得る。さらに、中間ウェイポイント生成器１２０は何れの上位ウェイポイント２１０も除去又は削除しないようにし得、これは、上位ウェイポイント生成器１１０が中間ウェイポイント生成器１２０には利用できない追加のマップコンテキストを知っている可能性があり、上位ウェイポイント２１０を除去することで予想外の結果を招くかもしれないからである。

【0030】

[0046] 次に図４を参照すると、幾つか実施例において、中間ウェイポイント生成器１２０は、疎グラフ生成器４１０、概略経路プランナ４２０、及びウェイポイントプレーサ４３０を含む。疎グラフ生成器４１０は、胴部障害物マップ４１２を例えば認識システム１０２から受信する。胴部障害物マップ４１２は、ロボット１０に近接する（例えば、４メートル以内）障害物（例えば、ロボット１０の胴部１１が通過できない障害物）のマップである。胴部障害物マップ４１２から、疎グラフ生成器４１０は疎グラフ４１６Ｓを生成する。後でより詳しく述べるように、疎グラフ４１６Ｓは、ノード８１２（図８Ａ－８Ｅ）とエッジ８１０（図８Ａ－８Ｅ）との間の有効な相互接続リストを提供する。ノード８１２とエッジ８１０は、ロボット１０の付近のヨー制約ゾーン（すなわち、操縦が困難か、又は厳しい領域）を通る有効なポテンシャル経路を表し、それゆえ、疎グラフ４１６Ｓによりロボット１０がヨー制約ゾーンを通るためのポテンシャル経路を特定するのに必要な情報が提供される。

【0031】

[0047] 疎グラフ生成器４１０は、疎グラフ４１６Ｓを概略経路プランナ４２０に伝える。概略経路プランナ４２０は、出発地からゴールの目的地まで、疎グラフ４１６Ｓを用いて概略経路１３１０（図１３）をプロットする。幾つかの例において、概略経路プランナ４２０は、探索アルゴリズム（例えばＡ^＊）を使用して、疎グラフ４１６Ｓにより示される有効ポテンシャル経路の最適なルートを特定することによって、概略経路１３１０（すなわち、ノード８１２とエッジ８１０との間の相互接続のリスト）をプロットする。概略経路プランナ４２０は、この経路１３１０をウェイポイントプレーサ４３０に伝える。ウェイポイントプレーサ４３０は、ロボットの現在の場所から、経路１３１０を辿ったときにどこで視線が失われるかを特定し、中間ウェイポイント３１０をその場所に設置する。すなわち、ウェイポイントプレーサ４３０は、ロボット１０が現在の場所から概略経路１３１０に沿って見た場合に。ロボット１０が経路の視線を失う（例えば、障害物による）場所を特定し、中間ウェイポイント３１０を経路１３１０上の、視線が失われた場所に設置する。中間ウェイポイント３１０は、ロボット１０を、下位経路生成器１３０が成功裏に障害物を避けることのできる場所まで案内する。中間ウェイポイント３１０は、ナビゲーションルート１１２に含められ、下位経路生成器１３０に伝えられる。

【0032】

[0048] 幾つかの実施例において、疎グラフ生成器４１０は当初、受信した胴部障害物マップ４１２に基づいてコンフィギュレーション空間全体マップ４１４を生成する。幾つかの例において、コンフィギュレーション空間全体マップ４１４はエレメント５１０、５１０ａ－ｎの二次元（２Ｄ）グリッド又はアレイを含み、各エレメント５１０は、障害物がロボット１０が特定のエレメント５１０に進入するのを阻止するか否かを示す環境８の領域（例えば、３センチメートル×３センチメートルの正方形）を表す。任意選択により、コンフィギュレーション空間全体マップ４１４は、１２８のエレメント×１２８のエレメントのアレイである。幾つかの例において、各エレメント５１０は複数のそれぞれのヨーコンフィギュレーション５１２、５１２ａ－ｎ（ヨーコンフィギュレーションのそれぞれのセット５１２、５１２ａ－ｎ）に関連付けられてよく、コンフィギュレーション空間全体マップ４１４は各エレメント５１０のそれぞれのヨーコンフィギュレーション５１２の各々に関する状態を示し得る。例えば、各エレメント５１０は、６４のそれぞれのヨーコンフィギュレーションに関連付けられ得る。この例では、各ヨーコンフィギュレーションは、その前のコンフィギュレーションから５．６２５度回転させられることになり、これは、３６０度（すなわち、全円）を６４（すなわち、ヨーコンフィギュレーションの数）で割ると５．６２５度となるからである。コンフィギュレーション空間全体マップ４１４はそれぞれのエレメント５１０に関連付けられるそれぞれのヨーコンフィギュレーション５１２の各々について、それぞれのヨーコンフィギュレーション５１２の結果、ロボット１０の選択点がエレメント５１０により表される空間を占めたときに胴部障害物と衝突するか否かを示し得る。すなわち、コンフィギュレーション空間全体マップ４１４は、各ヨーコンフィギュレーション５１２が有効か（すなわち、ロボット１０がそのヨーコンフィギュレーション５１２で障害物と衝突しない）、無効か（すなわち、ロボット１０がそのヨーコンフィギュレーションで障害物と衝突する）を示し得る。選択点は例えば、ロボット１０の胴部１１の中心点（例えば、各脚１２の最上部から等距離）を含み得る。

【0033】

[0049] 幾つかの実施例において、疎グラフ生成器４１０は、コンフィギュレーション空間全体マップ４１４からコンフィギュレーション空間圧縮マップ５００を生成する。コンフィギュレーション空間圧縮マップ５００は、各エレメントを複数のヨーコンフィギュレーション５１２に関連付けるのではなく、コンフィギュレーション空間マップ５００の各エレメント５１０を分類し得る。例えば、各エレメント５１０は、胴部障害物エレメント５１０Ａ（図５）、ヨー衝突エレメント５１０Ｂ（図５）、ヨー自由エレメント５１０Ｃ（図５）、又はヨー制約エレメント５１０Ｄ（図５）に分類され得る。すなわち、コンフィギュレーション空間全体マップ４１４からのヨーコンフィギュレーション５１２はコンフィギュレーション空間圧縮マップ５００の各エレメント５１０を分類するために使用され得る。

【0034】

[0050] 次に図５を参照すると、例示的なコンフィギュレーション空間圧縮マップ５００が示されている。この例において、コンフィギュレーション空間圧縮マップ５００は１２８×１２８のエレメント５１０のグリッドを含み、各々が胴部障害物エレメント５１０、ヨー衝突エレメント５１０Ｂ、ヨー自由エレメント５１０Ｃ、又はヨー制約エレメント５１０Ｄに分類される。胴部障害物エレメント５１０Ａは、ロボット１０が通過できない障害物（すなわち、胴部障害物マップ４１２から受け取った障害物）が占めるエレメントを表す。例えば、ロボット１０が避けなければならない椅子、箱、又は壁は胴部障害物エレメント５１０Ａに分類され得る。ヨー衝突エレメント５１０Ｂは、ロボット１０に関する全てのヨーコンフィギュレーション５１２（例えば、６４のヨーコンフィギュレーションの全て）が、ロボット１０の選択点（例えば、胴部１１の中心）がエレメント５１０Ｂにより表される空間を占めたときに胴部障害物（すなわち、胴部障害物エレメント５１０Ａ）と衝突するという結果になるエレメントである。例えば、壁のすぐ隣の空間は、実際には壁が含まれていなくても、ロボット１０がその壁に衝突せずに立つには壁に近すぎる可能性がある。

【0035】

[0051] ヨー自由エレメント５１０Ｃは、何れのヨーコンフィギュレーション５１２も胴部障害物と衝突することにならないという点で、ヨー衝突エレメント５１０Ｂの反対である。換言すれば、ヨー自由エレメント５１０Ｃは、ロボット１０が胴部障害物に衝突せずに全てのヨーコンフィギュレーション５１２を通じてスピンし得る（すなわち、選択点における軸を中心に３６０度スピンする）環境８中の領域を表す。ヨー制約エレメント５１０Ｄは、幾つかのヨーコンフィギュレーション５１２が安全、すなわち有効であり（すなわち、ロボット１０が障害物と衝突しない）、また幾つかのヨーコンフィギュレーション５１２が安全でない（すなわち、ロボット１０がこれらのヨーコンフィギュレーション５１２において障害物と衝突する無効のヨーコンフィギュレーション５１２）エレメントである。例えば、ロボット１０は壁の横をそれに平行に移動でき得るが、９０度方向転換すれば壁と衝突し得る。この例において、幾つかのヨーコンフィギュレーション５１２（すなわち、ロボット１０が壁に平行であるヨーコンフィギュレーション５１２）は安全であり、また幾つか（すなわち、ロボット１０が壁に垂直となるヨーコンフィギュレーション５１２）は安全ではない。

【0036】

[0052] 疎グラフ生成器４１０は、幾つかの例において、コンフィギュレーション空間圧縮マップ５００のヨー制約ゾーン５１０ＤＺ（すなわち、ヨー制約エレメント５１０Ｄのゾーン）を通る有効な経路を特定する。ここで図６を参照とすると、幾つかの実施例において、疎グラフ生成器４１０は、複数のボロノイセル６１０をコンフィギュレーション空間圧縮マップ５００に重ねている。ボロノイセル６１０は、ヨー制約ゾーン５１０ＤＺ及び／又はヨー自由ゾーン５１０ＣＺ内の、ヨー衝突エレメント５１０Ｂに分類される少なくとも２つのエレメント５１０から等距離にある全てのエレメント５１０を含む。換言すれば、ボロノイセル６１０は、ヨー衝突ゾーン５１０ＢＺから最も遠い（及びしたがって、胴部障害物から最も遠い）エレメント５１０である。代替的に、ボロノイセル６１０は、ヨー制約ゾーン５１０ＤＺ及び／又はヨー自由ゾーン５１０ＣＺ内の、少なくとも２つの胴部障害物エレメント５１０Ａから等距離にある全てのエレメント５１０を含む。図６の例では、セル６１０の中心はヨー衝突ゾーン５１０ＢＺ間のヨー自由ゾーン５１０ＣＺとヨー制約ゾーン５１０ＤＺ内にある。ボロノイセル６１０は、ロボット１０がすでにアクセスし得るデータ（例えば、符号付距離の場）から容易に計算できる。

【0037】

[0053] ボロノイセル６１０としてマークすべきエレメント５１０を特定するために、疎グラフ生成器４１０は、コンフィギュレーション空間マップ５００上で障害物（すなわち、胴部障害物エレメント５１０Ａ）から等距離である（コンフィギュレーション空間マップ５００の）エレメント５１０を特定する。すなわち、各エレメント５１０は本質的に、最も近い障害物境界までの距離と方向を有する。図７は、ヨー制約ゾーン５１０ＤＺにより分離される２つのヨー衝突ゾーン５１０ＢＺを含むコンフィギュレーション空間マップ５００の一部を示す。数個のボロノイセル６１０がヨー制約ゾーン５１０ＤＺを通って、ヨー衝突ゾーン５１０ＢＺ間に延びている。幾つかの例において、疎グラフ生成器は、コンフィギュレーション空間マップ５００の各エレメント５１０を分析して、そのエレメント５１０をボロノイセル６１０としてマークすべきか否かを特定する。各エレメント５１０を分析して、そのエレメントがボロノイセル６１０であるか否かを特定する際、幾つかの実施例において、疎グラフ生成器４１０は、あるエレメント５１０（すなわち、分析中のエレメント５１０）及びそれに隣接するエレメント５１０が、最も近い障害物境界として実質的に異なる場所を指しているか否かを特定する。すなわち、コンフィギュレーション空間マップ５００中の各エレメント５１０について、疎グラフ生成器４１０は、それぞれのエレメントに最も近い障害物境界、それぞれのエレメントの下側の隣接エレメントの最も近い障害物境界、及びそれぞれの要素の右側の隣接エレメントの最も近い障害物境界の位置を特定し得る。３つの最も近い障害物境界のうちの何れか２つ間の距離が閾値距離を満たす場合は常に、そのエレメント５１０はボロノイセル６１０としてマークされ得る。閾値距離は、何れの数のエレメント（例えば、１エレメント、２エレメント、５エレメント等）都としても表現され得る。

【0038】

[0054] 図の例において、疎グラフ生成器４１０が、「Ａ」と表示されたエレメント５１０を分析する場合、疎グラフ生成器４１０は、エレメント「Ａ」に最も近いヨー衝突エレメント５１０Ｂ（又は胴部障害物エレメント５１０Ａ）がエレメント５１０ａであると特定する。疎グラフ生成器４１０はすると、その要素の右側の隣接要素（「Ｂ」と表示）に最も近いヨー衝突エレメント５１０Ｂを特定し得て、これはこの場合、５１０ｂである。疎グラフ生成器４１０はすると、その要素の下側の隣接要素（「Ｃ」と表示）及び最も近いヨー衝突エレメント５１０Ｂ（この場合、再びエレメント５１０ａ）からの距離を特定し得る。疎グラフ生成器４１０は、幾つかの例において、３つの最も近い障害物境界のうちの２つが閾値距離内にあるか否かを特定し得る（すなわち、「Ａ」と「Ｂ」、「Ａ」と「Ｃ」、及び「Ｂ」と「Ｃ」間の距離の３つのうちの２つ）。エレメントが同じ距離だけ離れた２つの障害物境界を有する場合、疎グラフ生成器４１０は両エレメント間の距離を特定し得る。

【0039】

[0055] この例において、「Ａ」と「Ｂ」の最も近い障害物境界間の距離は１０エレメントであり、「Ａ」と「Ｃ」の最も近い障害物境界間の距離はゼロエレメントであり（すなわち、これらは同じ最も近い障害物境界を共有する）、「Ｂ」と「Ｃ」の最も近い障害物境界間の距離は再び１０エレメントである。閾値距離が１０エレメント未満である場合、エレメント「Ａ」がボロノイセル６１０としてマークされることになる。

【0040】

[0056] 前述のように、疎グラフ生成器４１０は、コンフィギュレーション空間マップ５００の各エレメントについて同様の分析を行い得る。引き続き図７を参照すると、疎グラフ生成器４１０は、エレメント「Ｄ」をボロノイセル６１０とはマークしないかもしれず、これは、その右側の隣接エレメント（「Ｅ」）及び下側の隣接エレメント（「Ｆ」）との３つの最も近い障害物境界が閾値（例えば、３エレメント）を満たさないからである。これは、最も近い障害物境界（すなわち、エレメント５１０ｃ、５１０ｄ）かの距離が１エレメントしかなく、閾値を満たさないかもしれないことによる。

【0041】

[0057] 次に図８Ａ－８Ｅを参照すると、疎グラフ生成器４１０は、幾つかの実施例において、ボロノイセル６１０から得られるエッジ８１０とノード８１２を特定する。図８Ａは、ヨー制約ゾーン５１０ＤＺ内の例示的なボロノイセル６１０を示す。疎グラフ生成器は、幾つかの実施例において、ボロノイセル６１０とマークされる任意の（すなわち、ランダム又は準ランダムな）エレメント５１０ｆｆを選択する。任意のエレメント５１０ｆｆから始めて、疎グラフ生成器はフラッドフィルを実行し、ボロノイセル６１０としてマークされた各エレメント５１０から任意に選択されたエレメント５１０ｆｆまでの距離を記録する。図の例では、各エレメントは任意のエレメント５１０ｆｆからの距離（単位はエレメント）で表示されている。フラッドフィルは、あるアレイの中のあるノードに接続される領域を特定するアルゴリズムである。この場合、フラッドフィルは、ボロノイセル６１０としてマークされた全エレメント５１０を通じて続いて流れ、最終的に全部にある距離が割り当てられる。任意に選択されたエレメント５１０ｆｆは完全にランダムに選択され得るが、選択をロボット１０の近隣内のエレメント５１０に限定して、何れのエラーの影響も最小化することが有利であり得る。

【0042】

[0058] ボロノイセル６１０としてマークされた各エレメントにある距離が割り当てられた後、疎グラフ生成器４１０は、幾つかの例において、ボロノイセル６１０の各分枝の端又はエンドノード８１２を特定する。各分枝のエンドノード８１２は局所的に、任意に選択されたエレメント５１０ｆｆから最大の距離を有する。ある分枝のエンドノード８１２から、疎グラフ生成器４１０は、幾つかの例において、再び選択されたエレメント５１０ｆｆに向かって戻る間に各エレメント５１０をマークする。疎グラフ生成器４１０がそれ以前にすでにマークされたエレメント５１０に遭遇するたびに、生成器はそのエレメントとの組合せに固有のラベルを付け得る。例えば、図８Ｂにおいて、疎グラフ生成器４１０は、エンドノード８１２ａを出発し、再び選択されたエレメント５１０ｆｆに向かって移動し、途中の各エレメントにマークして、エッジ８１０ａを形成する。すなわち、エンドノード８１２ａから選択されたエレメント５１０ｆｆまでのボロノイセル６１０は、エッジ８１０ａを形成し得る。疎グラフ生成器４１０は、全てのエレメントがマークされるまで底のプロセスを隷属し得る。例えば、図８Ｃにおいて、疎グラフ生成器４１０は感度ノード８１２ｂからエレメント５１０ｆｆに向かって移動し、各エレメントをマークして他のエッジ８１０ｂを形成し、最終的に８１２ｃにおいてそれ以前にマークされたエレメントに遭遇する（図８Ｂ）。疎グラフ生成器４１０は、固有のラベルを８１２ｃの接合点エレメント（すなわち、それ以前にマークされた最初の遭遇エレメント）に割り当て得る。疎グラフ生成器４１０は、引き続き復路を移動し、最終的に選択されたエレメント５１０ｆｆに到達し得て、それ以前にマークされた各エレメントは別のエッジ８１０ｃと認識する。

【0043】

[0059] 次に図８Ｄを参照すると、疎グラフ生成器４１０は、エンドノード８１２ｄから始めて、エレメント５１０ｆｆへと戻り、再び、途中の各エレメントをマークして、別のエッジ８１０ｄを形成し得る。全エレメント５１０がマークされ（そして、あるエッジ８１０に割り当てられ）た後、疎グラフ生成器４１０は、幾つかの実施例において、選択されたエレメント５１０ｆｆが２つのエッジ８１０にしか接触しない（すなわち、３つ又はそれ以上のエッジ間の接合点ではない）か否かを特定する。この場合、疎グラフ生成器４１０は２つのエッジを１つのエッジに合成し得る。例えば、図８Ｄに示されるように、選択されたエレメント５１０ｆｆは２つの他のエッジ（すなわち、エッジ８１０ｃ及びエッジ８１０ｄ）としか接触せず、図８Ｅで、エッジ８１０ｃとエッジ８１０ｄが１つのエッジ８１０へと合成される。フラッドフィルとエレメントマーキングの最終的な結果として、疎グラフ生成器４１０にはエッジ８１０に沿って順番が付けられたエレメント５１０のセット及びノードエレメント８１２のリストが提供される。疎グラフ４１６Ｓからのノード及びエッジのリストは、近くの環境の制約空間（すなわち、コンフィギュレーション空間圧縮マップ５００のヨー制約ゾーン５１０ＤＺ）を通って移動し得る。

【0044】

[0060] 図９は、複数のエッジ８１０とノード８１２に分類されるボロノイセル６１０が重ねられたコンフィギュレーション空間マップ５００を示しており、各エッジ８１０は他のエッジ８１０に（ノード８１２を通じて）、又はヨー自由ゾーン５１０ＣＺに（同じくノード８１２を通じて）接続される。疎グラフ生成器４１０は、疎グラフ４１６Ｓを生成する際、幾つかの実施例において、ヨー制約ゾーン５１０ＤＺの中にあるボロノイセル６１０とマークされたエレメント５１０だけを考慮するが、これは、ヨー自由ゾーン５１０ＣＺを通る経路は後の時点で特定され得るからである。疎グラフ生成器４１０は、図８Ａ－８Ｅに関して説明したように、ボロノイセル６１０とマークされたエレメントを個々のエッジ８１０及びノード８１２に分け得る。幾つかの例において、疎グラフ生成器４１０は、エッジ８１０がヨー自由ゾーン５１０ＣＺに接続されるノード８１２にラベルを付け、又はこれらを識別する。すなわち、疎グラフ生成器４１０は、ヨー自由ゾーン５１０ＣＺとヨー制約ゾーン５１０ＤＺを接続するボロノイセル６１０に沿ったノード８１２（すなわち、端若しくはエッジ８１０又はエッジ８１０間の接合点）を識別する。疎グラフ生成器４１０はさらに、複数のエッジ８１０のための接合点であるノード８１２を識別し得る。幾つかの例において、疎グラフ生成器４１０は、同じヨー自由ゾーン５１０ＣＺに接続されるノード８１２は全部が相互に接続されていると仮定する。すなわち、疎グラフ生成器は、ヨー自由ゾーン５１０ＣＺを通る動きを、ノード８１２間に経路が存在するかぎり（すなわち、同じヨー自由ゾーン５１０ＣＺに接続されているかぎり）、（少なくとも一時的に）無視し得る。エッジ８１０及びノード８１２を備えた疎グラフ生成器４１０はまず、ボロノイセル６１０が重ねられたコンフィギュレーション空間圧縮マップ５００内でそれ以前に特定されたエッジ８１０及びノード８１２に基づいて、ナビゲーション上の重要性を持たないエッジ８１０を枝切りすることによって最終的な２Ｄコンフィギュレーションマップ１０００（図１０）を生成し得る。例えば、図１０は、複数のボロノイセル６１０が重ねられ、最終的な２Ｄコンフィギュレーションマップ１０００を生成するために複数の重要でないエッジ１０１０が枝切りされたコンフィギュレーション空間圧縮マップ５００を示す。すなわち、疎グラフ生成器４１０は、幾つかの実施例において、閾値長さより短く、ヨー制約ゾーン５１０ＤＺ内で終了又は終結する、重要でないエッジ１０１０を、これらはロボット１０が通過する上で重要でない可能性が極めて高いため、枝切りする。任意選択により、閾値長さはほぼロボット１０の半径であり得る。幾つかの例において、疎グラフ生成器４１０は、枝切りの後、接合点ノード８１２が２つ以下のエッジ８１０に式接続され立てない場合、エッジ８１０を１つにまとめる。

【0045】

[0061] 次に図１１Ａ及び１１Ｂを参照すると、疎グラフ生成器４１０は、幾つかの実施例において、最終的な２Ｄコンフィギュレーションマップ１０００（図１１Ａ）をマップのＳＥ２表現１１００（図１１Ｂ）に変換する。ＳＥ２は、標準的なデカルト座標と共にヨーを含む（すなわち、ｘ座標、ｙ座標、及びヨー）座標系である。すなわち、ＳＥ２マップ１１００は、最終的な２Ｄコンフィギュレーションマップ１０００をとり、ヨーを含む。図１１Ｂは、ヨーが高さであるＳＥ２マップ１１００を示す。ロボット１０は何れの数のヨーコンフィギュレーション５１２でも持ち得る（例えば、前述のように６４）が、ＳＥ２マップ１１００は２つのヨーコンフィギュレーション、すなわち前方と後方のみを考慮することによって簡素化され得る。すなわち、一般に、ロボット１０はその移動経路と整合するヨーコンフィギュレーション５１２内で移動し、それゆえ、ＳＥ２マップ１１００はこれら２つのヨーコンフィギュレーションのみを考慮することによって大幅に簡素化され得る。２Ｄマップ１０００の各エッジ８１０は、反対の接線（すなわち、「前方」及び「後方」のヨーコンフィギュレーション１１１０ａ、１１１０ｂの２つのＳＥ２エッジにより表され得る。各２Ｄノード８１２（図１１Ａ）は、２Ｄマップ１０００の表現の中でノード８１２に接続されたエッジ１１１０（図１１Ｂ）の数の２倍で表される。例えば、図１１Ａは、１つのノード８１２に接続された３つのエッジ８１０を示しており、それがＳＥ２マップ１１００では６つのノード１１１２へと並進する。ロボット１０がノード１１１２においてその場で回転することが示唆され、疎グラフ生成器４１０は、ノード１１１２間の相互接続を、コンフィギュレーション空間全体マップ４１４からの有効なヨーコンフィギュレーション５１２に基づいて特定し得る。

【0046】

[0062] ＳＥ２マップ１１００を完成させるために、幾つかの実施例において、疎グラフ生成器４１０は、ロボット１０のためのマップ１１００の入口と出口を特定する。次に図１２Ａ及び１２Ｂを参照すると、疎グラフ生成器４１０は、オフマップゴール１２１０（すなわち、標的目的地を直線に沿ってマップ１１００の境界に投影し得る。ゴール１２１０がヨー自由ゾーン５１０ＣＺ内に設定されると、疎グラフ生成器４１０はゴール１２１０に関連付けられるそれぞれのエレメントを使用し得る。しかしながら、ヨー制約ゾーン５１０ＤＺ内では、疎グラフ生成器４１０はボロノイセル６１０としてマークされる最も近いエレメント５１０に沿って（２Ｄマップ１０００から）、又はヨー自由ゾーン５１０ＣＺの境界に沿って探索し得る。最も近いエレメントがヨー自由ゾーン５１０ＣＺの境界上にある場合、疎グラフ生成器４１０はそのエレメントをゴールに設定し得る。エッジ８１０上の元も近いエレメントがボロノイセル６１０としてマークされた場合、疎グラフ生成器４１０はそれぞれの最も近いエレメントに新たなノード１２２０を設置することにより、エッジ８１０を２つに分割し得る。図１２Ａに示される例において、ゴール１２１０はヨー制約ゾーン５１０ＤＺ内に設定される。疎グラフ生成器は、最も近い所望のエレメントがボロノイセル６１０であると特定する。疎グラフ生成器４１０は、ゴール１２１０に最も近いエレメントに新たなノード１２２０を作り、その前のエッジ８１０を２つの別々のエッジ８１０に分離する。幾つかの実施例において、疎グラフ生成器４１０は、２つの新たなノード１２２０を作り、１つのノード１２２０は前方ヨーコンフィギュレーション５１２を表し、１つのノード１２２０は後方ヨーコンフィギュレーション５１２を表す。

【0047】

[0063] 新しいノード１２２０が設置されると、疎グラフ生成器４１０は、ロボット１０の出発位置（例えば、ロボットの現在の場所）が前方ヨーコンフィギュレーションノード１２２０、後方ヨーコンフィギュレーションノード１２２０、又はその両方の何れに接続されているかを特定する。すなわち、疎グラフ生成器４１０は、ロボット１０がそれぞれのヨーコンフィギュレーションのノード１２２０に接続できるかを特定する。疎グラフ生成器４１０は、幾つかの例において、当初の、真のゴール１２１０と新しいノード１２２０に沿ったエッジ８１０の２つのヨーコンフィギュレーション（すなわち、前方及び後方）との間の角度１２２２を特定する。図１２Ｂに示される例において、角度がある閾値を満たすと（例えば、角度がゾーン１２３０内にあると）、前方対向ノード１２２０がロボットの場所に接続される。角度が第二の閾値を満たすと（例えば、角度がゾーン１２４０内にあると）、後方対向ノード１２２０がロボットの場所に接続される。角度が第三の閾値を満たすと（例えば、角度がゾーン１２５０内にあると）、前方対向ノード１２２０及び後方対向ノード１２２０の両方がロボットの出発地に接続される。すなわち、ロボット１０は、前方又は後方の何れか向かうノード１２２０の場所まで移動できる。

【0048】

[0064] 幾つかの例において、疎グラフ生成器４１０は、ロボット１０が障害物と衝突することを示すノード８１２を接続する。例えば、疎グラフ生成器４１０は常に、ロボットが中心点から回転することによって方向転換し、これによって一部の方向転換が不可能となり得ると仮定し得る。しかしながら、幾つかの状況では、下位経路生成器１３０は依然として、ロボット１０の中心ではないピボット点で回転させることによって経路をナビゲートすることが可能であり得、これらの状況では、疎グラフ生成器４１０は生成器１２０がそうでなければロボットが移動不可能であると判断し得るエッジ８１０をノード８１２間に作り得る。

【0049】

[0065] 再び図４を参照すると、中間ウェイポイント生成器１２０はまた、概略経路プランナ４２０も含み、これは、疎グラフ生成器４１０から疎グラフ４１６Ｓを受信して、ロボット１０が移動してゴール１２１０、すなわち、標的の目的地に対応するノード１２２０（図１２Ａ）まで移動するためのトポロジ上適正な経路（すなわち概略経路１３１０）を特定するように構成される。幾つかの実施形態において、概略経路プランナ４２０はまた、コンフィギュレーション空間全体マップ４１４及び／又はコンフィギュレーション空間圧縮マップ５００も受信する。次に図４及び１３を参照すると、マップ１１００はヨー自由ゾーン５１０ＣＺ内の開始点１３２０からヨー制約ゾーン５１０ＤＺを通って別のヨー自由ゾーン５１０ＣＺまでナビゲートする概略経路１３１０を含む。

【0050】

[0066] 経路１３１０を生成するために、概略経路プランナ４２０は探索アルゴリズム（例えば、Ａ^＊探索アルゴリズム）を実行し得る。概略経路プランナ４２０は、探索アルゴリズムを使って２Ｄヨー自由ゾーン５１０ＣＺの密グリッド又はグラフ４１６Ｄを探索して経路１３１０をプロットし得る。疎グラフ生成器４１０はまた、密グラフ４１６Ｄを生成して、それを疎グラフ４１６Ｓと組み合わせ、組み合わせたグラフを概略経路プランナ４２０に送信し得る。代替的に、概略経路プランナはこの密グラフ４１６Ｄを生成し得る。密グラフはコンフィギュレーション空間圧縮マップ５００から特定されたヨー自由ゾーン５１０ＣＺの相互接続されたノードのリストである。すなわち、疎グラフ４１６Ｓがヨー制約ゾーン５１０ＤＺ内のエレメント５１０の相互接続されたノード８１２のリストを含んでいるのと同様に、密グラフ４１６Ｄはヨー自由ゾーン５１０ＣＺ内のエレメント５１０の相互接続されたノードのリストを含む。ロボット１０はヨー自由ゾーン５１０ＣＺでは自由にスピンし得るため、ヨー自由空間は、方位（すなわち、ヨー）情報を必要としないことから、概略経路プランナ４２０は密グラフ４１６Ｄ（疎グラフ４１６Ｓに関して密）を探索し得る。概略経路プランナ４２０は、疎グラフ４１６Ｓと密グラフ４１６Ｄを組み合わせて、最終グラフ４１６Ｆを形成し得る。最終グラフ４１６Ｆは、ヨー自由ゾーン５１０ＣＺとヨー制約ゾーン５１０ＤＺの両方の中の相互接続されたノードの全てを含む。概略経路プランナ４２０はまた、探索アルゴリズムを使って、方位情報が含められたヨー制約ゾーン５１０ＤＺ内及び、ヨー自由ゾーン５１０ＣＺ内で、ボロノイセル６１０に関する最終グラフ４１６Ｆを探索し得る。概略経路プランナ４２０は、ヨー自由ゾーン５１０ＣＺをヨー制約ゾーン５１０ＤＺに関係付け、ヨー制約ゾーン５１０ＤＺに概略経路１３１０を通し得る。

【0051】

[0067] 密グラフ４１６Ｄは方位情報を含まない（それゆえ、最終グラフ４１６Ｆはヨー自由ゾーン５１０ＣＺ内の方位情報を含まない）ため、概略経路プランナ４２０は、幾つかの実施例において、何れかのエッジ８１０に関する何れかの方位（すなわち、前方又は後方）を逆転すべきか否かを特定する。任意選択により、概略経路プランナ４２０は、エッジ８１０に沿って前方を向きながら歩行するようにバイアスされる。概略経路プランナ４２０は、ヨーの方位を逆転させることできる（すなわち、前方及び後方の両方の方位が有効である）経路１３１０のセグメントを識別し得る。概略経路プランナ４２０はすると、経路１３１０を逆転するためのコスト（すなわち、必要な回転量）を計算し得る。

【0052】

[0068] 例えば、次に図１４のマップ１１００を参照すると、概略経路プランナ４２０は、出発地１４１０（方位はそれぞれの矢印と一致）からゴール場所１４２０（方位はそれぞれの矢印と一致）までの経路１４３０を計画した。前方へのバイアスにより、探索アルゴリズムは、ヨー制約ゾーン５１０ＤＺを通じて前向きの方位（経路Ａ－Ｃ）を選択し得る。概略経路プランナ４２０は、任意選択により、両方の経路Ａ－Ｃ（前方を向く）と経路Ｂ－Ｄ（後方を向く）の両方のコストを計算する。ある例において、ある開始及びゴールの向きは後方を向いているため、後方経路Ｂ－Ｄのほうが前方向きの経路Ａ－Ｃより安価であり（すなわち、必要な回転量が少ない）、概略経路プランナ４２０はそれに応じて経路１４３０を更新し得る。

【0053】

[0069] 再び図４を参照すると、中間ウェイポイント生成器１２０は、幾つかの実施例において、ウェイポイントプレーサ４３０も含む。ウェイポイントプレーサ４３０は、コンフィギュレーション空間全体マップ４１４と、経路１３１０を含む疎グラフ４１６Ｓを概略経路プランナ４２０から受信する。ウェイポイントプレーサ４３０は、中間ウェイポイント３１０を設置するための適当な場所のための経路１３１０を処理する。ウェイポイントプレーサ４３０は、幾つかの例において、視線が最初に失われる（例えば、障害物による）まで経路１３１０に沿って探索して、ウェイポイント３１０をその場所に設置する。任意選択により、ウェイポイントプレーサ４３０は、処理時間を短縮するために、探索の前に経路１３１０をダウンサンプルする。

【0054】

[0070] 次に図１５Ａを参照すると、ロボット１０は経路１３１０に沿って、２つの胴部障害物ゾーン５１０ＡＺ間にあるヨー制約ゾーン５１０ＤＺを走行する。ウェイポイントプレーサ４３０は、ロボット１０の現在の場所から経路１３１０に沿った様々な地点まで光線１５１０を引く。光線１５１０の１つが胴部障害物ゾーン５１０ＡＺの一部（又は、代替的に、ロボット１０の幅を考慮するために、胴部障害物ゾーン５１０ＡＺの閾値距離以内）と交差すると、ウェイポイントプレーサ４３０は中間ウェイポイント３１０を経路１３１０上の、視線が失われたとき、又はその直前に光線１５１０が経路１３１０と交差する点１５３０に設置する。それゆえ、視線が失われる場所の特定は、デカルト座標（すなわち、ｘ及びｙ座標）内で比較的容易である。しかしながら、ＳＥ２空間（すなわち、ヨーを含む）内の視線の特定はより複雑となる。

【0055】

[0071] 次に図１５Ｂを参照すると、ヨーを含めることに対応するために、ウェイポイントプレーサ４３０は、ダウンサンプルされた経路１３１０に沿った各点において、最小許容ヨー及び最大許容ヨーを特定して、包絡線１５５０、１５５０ａ－ｎを生成する。ウェイポイントプレーサ４３０は、幾つかの例において、ダウンサンプルされた各点における最小（すなわち、最も内側の）包絡線１５６０、１５６０ａ－ｎを追跡し、包絡線の幅が包絡線閾値より小さくなるか、又は経路１３１０上の点１５３０における必要なヨーコンフィギュレーションが包絡線１５５０の外に出たときの何れかの時点で、中間ウェイポイント３１０を設置する。図の例において、包絡線１５５０ｃのヨーは内側の包絡線１５６０ｃの外にあり、中間ウェイポイント３１０のための候補である。

【0056】

[0072] 幾つかの例において、ウェイポイントプレーサ４３０はまた、ヨー自由ゾーン５１０ＣＺからヨー制約ゾーン５１０ＤＺに移動する際、方位が閾値量（例えば、６０度）より大きく変化した場合に、中間ウェイポイント３１０を追加する。任意選択により、ウェイポイントプレーサ４３０は、ゴールでの方位（すなわち、ゴールポーズ）が現在のマップの外にあり（すなわち、ロボットの現在の場所から遠すぎる）、ゴールまでの視線が失われた場合、経路１３１０の終端に中間ウェイポイント３１０を追加し得る。それに加えて、ウェイポイントプレーサ４３０は、ウェイポイントを狭い空間内で接合点からシフトさせ得る。例えば、中間ウェイポイント３１０は、接合点の周囲での閾値半径の外に出るまで、経路１３１０に投影され得る。

【0057】

[0073] 上位ウェイポイント２１０は、平面上の位置を示す２つの座標（すなわち、ｘ及びｙ座標）、ヨー値、及び時刻値を含み得る。時刻値は、ロボットがそれぞれのウェイポイント３１０まで走行するための予想時間量を示し得る。ｘ、ｙ、及びヨー値と共に、ウェイポイントプレーサ４３０は、同様に、中間ウェイポイント３１０までの予想時刻値も追加し得る。予想時刻値は、受信した何れかの速度限界を使って、出発地からｘ、ｙ、及びヨーにおけるウェイポイントまでの最大時間量を考慮して特定され得る。ウェイポイントプレーサ４３０は、中間ウェイポイント３１０後の計画軌道上の各上位ウェイポイント２１０の時間をオフセットし得る。

【0058】

[0074] 再び図４を参照すると、ウェイポイントプレーサ４３０は、何れかの中間ウェイポイントをナビゲーションルート１１２の上位ウェイポイント２１０に追加する。中間ウェイポイント生成器は、ナビゲーションルート１１２を下位経路生成器１３０に伝えて、各ウェイポイント２１０、３１０までナビゲートするためのステップ計画１４２を生成する。中間ウェイポイント生成器１２０が中間ウェイポイント３１０は不要であると判断した（又は、適当な場所を見つけられなかった）場合、中間ウェイポイント生成器１２０は受け取ったナビゲーションルート１１２を変更せずに下位経路生成器１３０に伝え得る。

【0059】

[0075] 図１６は、ロボット１０のための中間ウェイポイントを生成する方法１６００の動作の例示的な配置のフローチャートである。動作１６０２において、方法１６００は、ロボット１０のデータ処理ハードウェア３６において、ナビゲーションルート１１２を受信することを含む。ナビゲーションルート１１２は、出発地１１３から始まって目的地１１４で終わる一連の上位ウェイポイント２１０を含む。ナビゲーションルート１１２は、ロボット１０が走行することになる領域内の静止障害物の場所を表す上位ナビゲーションデータ５０に基づく。

【0060】

[0076] 方法１６００は、動作１６０４において、データ処理ハードウェア３６においてイメージセンサ３１からロボット１０の周囲の環境８の画像データ１７を受信することを含む。動作１６０６で、方法１６００は、データ処理ハードウェア３６により、少なくとも１つの中間ウェイポイント３１０を画像データに基づいて生成することと、動作１６０８において、データ処理ハードウェア３６により、少なくとも１つの中間ウェイポイント３１０をナビゲーションルート１１２の一連の上位ウェイポイント２１０に追加することを含む。動作１６１０において、方法１６００は、データ処理ハードウェア３６により、ロボット１０を出発地１１３から一連の上位ウェイポイント２１０及び少なくとも１つの中間ウェイポイント３１０に沿って目的地１１４に向かってナビゲートすることを含む。

【0061】

[0077] 図１７は、本明細書に記載されたシステムと方法を実施するために使用され得る例示的なコンピューティングデバイス１７００（例えば、データ処理ハードウェア３６及びメモリハードウェア２０）の概略図である。ここに示されるコンポーネント、それらの接続及び関係、並びにそれらの機能は、例に過ぎないものとし、本明細書に記載され、及び／又は特許請求される本発明の実施を限定するものではない。

【0062】

[0078] コンピューティングデバイス１７００は、プロセッサ１７１０（例えば、データ処理ハードウェア３６）、メモリ１７２０（例えば、メモリハードウェア３８）、記憶装置１７３０、メモリ１７２０と高速拡張ポート１７５０に接続される高速インタフェース／コントローラ１７４０、及び低速バス１７７０と記憶装置１７３０に接続される低速インタフェース／コントローラ１７６０を含む。コンポーネント１７１０、１７２０、１７３０、１７４０、１７５０、及び１７６０の各々は、各種のバスを使って相互接続され、共通のマザボード上に、又はその他の適当な方法で実装され得る。プロセッサ１７１０は、コンピューティングデバイス１７００内で実行されるための命令を処理することができ、これにはメモリ１７２０内又は記憶装置１７３０上に記憶される、高速インタフェース１７４０に連結されたディスプレイ１７８０等の外部入力／出力デバイス上にグラフィカルユーザインタフェース（ＧＵＩ）のためのグラフィック情報を表示させる命令が含まれる。他の実装形態において、複数のプロセッサ及び／又は複数のバスが、複数のメモリ及びメモリタイプと共に適切に使用され得る。また、複数のコンピューティングデバイス１７００が接続され得て、各デバイスが必要な動作の一部を（例えば、サーババンク、ブレードサーバ群、又はマルチプロセッサシステムとして）提供する。プロセッサ１７１０は、図１及び４の中間ウェイポイント生成器を実装し得る。

【0063】

[0079] メモリ１７２０は、コンピューティングデバイス１７００内に非一時的に情報を保存する。メモリ１７２０は、コンピュータ可読媒体、揮発性メモリユニット、又は不揮発性メモリユニットであり得る。不揮発性メモリ１７２０は、プログラム（例えば、命令のシーケンス）又はデータ（例えば、プログラム状態情報）をコンピューティングデバイス１７００により使用されるために一時的又は永久的に記憶するために使用される物理デバイスであり得る。不揮発性メモリの例には、フラッシュメモリ及びリードオンリメモリ（ＲＯＭ）／プログラマブルリードオリメモリ（ＰＲＯＭ）／イレーサブルプログラマブルリードオンリメモリ（ＥＰＲＯＭ）／エレクトロニカルイレーサブルプログラマブルリードオンリメモリ（ＥＥＰＲＯＭ）（例えば、典型的に、ブートプログラム等、ファームウェアのために使用される）が含まれるが、これらに限定されない。揮発性メモリの例には、ランダムアクセスメモリ（ＲＡＭ）、ダイナミックランダムアクセスメモリ（ＤＲＡＭ）、スタティックランダムアクセスメモリ（ＳＲＡＭ）、相変化メモリ（ＰＣＭ）、及びディス又はテープが含まれるが、これらに限定されない。

【0064】

[0080] 記憶装置１７３０は、コンピューティングデバイス１７００のための大量記憶を提供できる。幾つかの実装形態において、記憶装置１７３０は、コンピュータ可読媒体である。各種の異なる実装形態において、記憶装置１７３０はフロッピディスクデバイス、ハードディスクデバイス、光ディスクデバイス、又はテープデバイス、フラッシュメモリ若しくはその他同様のソリッドステートメモリデバイス又は、記憶エリアネットワーク若しくはその他の構成のデバイスを含むデバイスのアレイであり得る。追加的な実装形態において、コンピュータプログラム製品は、情報キャリア内で有形の状態に具現化される。コンピュータプログラム製品は、実行されると、１つ又は複数の方法、例えば前述の方法を実行する命令を含む。情報キャリアは、コンピュータ又は機械可読媒体、例えばメモリ１７２０、記憶装置１７３０、又はプロセッサ１７１０上のメモリである。

【0065】

[0081] 高速コントローラ１７４０は、コンピューティングデバイス１７００のためのバンド幅集約動作を管理し、他方で、低速コントローラ１７６０は、よりバンド幅集約性の低い動作を管理する。このような担当作業の割り当ては例示にすぎない。幾つかの実装形態において、高速コントローラ１７４０は、メモリ１７２０及び、各種の拡張カード（図示せず）を受け入れ得る高速拡張ポート１７５０に連結される。幾つかの実装形態において、低速コントローラ１７６０は、記憶装置１７３０及び低速拡張ポート１７９０に連結される。低速拡張ポート１７９０は、各種の通信ポート（例えば、ＵＳＢ、Bluetooth、イーサネット、無線イーサネット）を含み得て、１つ又は複数の入力／出力デバイス、例えばキーボード、ポインティングデバイス、スキャナ、又はスイッチや又はルータ等のネットワークデバイスに、例えばネットワークアダプタを通じて連結され得る。

【0066】

[0082] 本明細書の記載のシステム及び技術の各種の実装形態は、デジタル電子機器及び／又は光回路構成、集積回路、特別に設計されたＡＳＩＣ（特定用途集積回路）、コンピュータハードウェア、ファームウェア、ソフトウェア、及び／又はこれらの組合せで実現できる。これらの各種の実装形態は、特定目的用でも汎用でもよい、記憶システム、少なくとも１つの入力装置、及び少なくとも１つの出力装置からデータ及び命令を受信し、そこにデータ及び命令を送信するために連結された少なくとも１つのプログラマブルプロセッサを含むプログラム可能システム上で実行可能及び／又は解釈可能な１つ又は複数のコンピュータプログラムでの実装を含むことができる。

【0067】

[0083] これらのコンピュータプログラム（プログラム、ソフトウェア、ソフトウェアアプリケーション、又はコードとしても知られる）は、プログラム可能プロセッサのための機械命令を含み、高レベル手続き型及び／又はオブジェクト指向プログラミング言語及び／又はアセンブリ／機械言語で実装できる。本明細書で使用されるかぎり、「機械可読媒体」及び「コンピュータ可読媒体」という用語は、機械命令及び／又はデータをプログラム可能プロセッサに提供するために使用されるあらゆるコンピュータプログラム製品、非一時的コンピュータ可読媒体、装置、及び／又はデバイス（例えば、磁気ディスク、光ディスク、メモリ、プログラマブルロジックデバイス（ＰＬＤｓ））を指し、機械可読信号として機械命令を受け取る機械可読媒体も含まれる。「機械可読信号」という用語は、機械命令及び／又はデータをプログラム可能プロセッサに提供するために使用されるあらゆる信号を指す。

【0068】

[0084] 本明細書に記載されるプロセッサ及びロジックフローは、入力データを処理し、出力を生成することによって機能を果たすために１つ又は複数のコンピュータプログラムを実行する、データ処理ハードウェアとも呼ばれる１つ又は複数のプログラム可能プロセッサにより実行可能である。プロセッサ及びロジックフローはまた、特殊用途の論理回路構成、例えばＦＰＧＡ（フィールドプログラマブルゲートアレイ）又はＡＳＩＣ（特定用途集積回路）等によっても実行可能である。コンピュータプログラムの実行に適したプロセッサには、例えば汎用及び特殊用途のマイクロプロセッサ、あらゆる種類のデジタルコンピュータの１つ又は複数のプロセッサが含まれる。一般に、プロセッサはリードオンリメモリ若しくはランダムアクセスメモリ又はその両方から命令及びデータを受信する。コンピュータの基本要素は、命令を実行するためのプロセッサと、命令及びデータを記憶するための１つ又は複数のメモリデバイスである。一般に、コンピュータはまた、データを記憶するための１つ又は複数の大量記憶装置、例えば磁気、磁気光ディスク、又は光ディスクを含むか、これらからデータを受信し、又はそれらにデータを送信するために動作的に連結される。しかしながら、コンピュータは、このようなデバイスを含んでいなくてもよい。コンピュータプログラム命令及びデータを記憶するのに適したコンピュータ可読媒体には、あらゆる形態の不揮発性メモリ、媒体、及びメモリデバイスが含まれ、これらには例えば、半導体メモリデバイス、例えばＥＰＲＯＭ、ＥＥＰＲＯＭ、及びフラッシュメモリデバイス、磁気ディスク、例えば内蔵ハードディスク若しくはリムーバブルディスク、磁気光ディスク、並びにＣＤ ＲＯＭ及びＤＶＤ－ＲＯＭディスクが含まれる。プロセッサとメモリは、特殊用途の論理回路構成により補足され、又はその中に組み込まれることも可能である。

【0069】

[0085] 様々な実施形態を説明した。しかしながら、本開示の主旨と範囲から逸脱することなく、各種の改良が加えられてよいと理解されたい。したがって、その他の実装形態も以下の特許請求の範囲に含まれる。

【図1】

【図2A】

【図2B】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8A】

【図8B】

【図8C】

【図8D】

【図8E】

【図9】

【図10】

【図11A】

【図11B】

【図12A】

【図12B】

【図13】

【図14】

【図15A】

【図15B】

【図16】

【図17】