特表2023-549298自己位置推定・地図作成方法、ロボット及びコンピュータ記憶媒体
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公表特許公報(A)
(11)【公表番号】
(43)【公表日】2023-11-22
(54)【発明の名称】自己位置推定・地図作成方法、ロボット及びコンピュータ記憶媒体
(51)【国際特許分類】
G06T 7/70 20170101AFI20231115BHJP
【ＦＩ】
G06T7/70 Z
【審査請求】有
【予備審査請求】未請求
(21)【出願番号】P 2023552378
(86)(22)【出願日】2021-06-16
(85)【翻訳文提出日】2023-05-17
(86)【国際出願番号】 CN2021100300
(87)【国際公開番号】W WO2022110767
(87)【国際公開日】2022-06-02
(31)【優先権主張番号】202011367335.9
(32)【優先日】2020-11-27
(33)【優先権主張国・地域又は機関】CN
(81)【指定国・地域】
(71)【出願人】
【識別番号】522271823
【氏名又は名称】北京邁格威科技有限公司
【氏名又は名称原語表記】ＭＥＧＶＩＩ （ＢＥＩＪＩＮＧ） ＴＥＣＨＮＯＬＯＧＹ ＣＯ．， ＬＴＤ．
(74)【代理人】
【識別番号】100146374
【弁理士】
【氏名又は名称】有馬 百子
(72)【発明者】
【氏名】賀 一家
(72)【発明者】
【氏名】頼 文▲セン▼
(72)【発明者】
【氏名】劉 驍
(72)【発明者】
【氏名】沈 毅
【テーマコード（参考）】
5L096
【Ｆターム（参考）】
5L096AA09
5L096BA05
5L096CA02
5L096DA02
5L096FA03
5L096FA67
5L096FA69
5L096GA32
5L096JA11
(57)【要約】
自己位置推定・地図作成方法、装置、ロボット（１００）及びコンピュータ記憶媒体を提供する。方法は、ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得るステップ（Ｓ１１０）と、ｔ－１時刻に対するｔ時刻におけるロボット（１００）の相対姿勢を推定するステップ（Ｓ１２０）と、相対姿勢に応じて、ｔ時刻に対応する複数の線分及びｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得るステップ（Ｓ１３０）と、各時刻におけるロボット（１００）の姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、各時刻におけるロボット（１００）の姿勢及びすべての直線の直線パラメータを得るステップ（Ｓ１４０）と、を含む。直線を表す際に、二直線パラメトリック表示形式を使用するため、計算の計算量をできるだけ低減させることができ、それによって計算効率を向上させ、計算時間を短縮させることができる。【選択図】図２
【特許請求の範囲】
【請求項1】
ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得るステップと、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定するステップと、
前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得るステップと、
各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得るステップと、を含むことを特徴とする自己位置推定・地図作成方法。
【請求項2】
ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定する前記ステップは、
【請求項3】
前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行う前記ステップは、前記ｔ－１時刻において前記ロボットにより形成されるロボット座標系を基準座標系として決定するステップと、
前記相対姿勢に応じて、前記ｔ時刻に対応する線分の２つの端点の座標を前記基準座標系に変換し、前記ｔ時刻に対応する線分の前記基準座標系での表現式を得るステップと、
前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行うステップと、を含むことを特徴とする請求項１又は２に記載の方法。
【請求項4】
前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う前記ステップの前に、前記ｔ時刻に対応する各線分に対して、予め決定された属性特徴及び予め保存された属性特徴閾値に応じて、前記ｔ－１時刻に対応するすべての線分から差異線分をフィルタリングし、前記ｔ－１時刻に対応する残りの線分を該線分に対応する候補線分として決定するステップをさらに含み、
これに対応して、前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う前記ステップは、
前記ｔ時刻に対応する各線分に対して、該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行うステップを含むことを特徴とする請求項３に記載の方法。
【請求項5】
該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行う前記ステップの前に、該線分に対応する候補線分の数がゼロではないことを決定するステップをさらに含み、
さもないと、前記方法は、
該線分が新しい直線に属することを決定するステップをさらに含むことを特徴とする請求項４に記載の方法。
【請求項6】
前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う前記ステップは、
【請求項7】
各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得る前記ステップは、前記各時刻における前記ロボットの姿勢の表現式を設定するステップと、
前記線分マッチングを経て得られたすべての直線の表現式を設定するステップと、
前記姿勢の表現式及び前記すべての直線の表現式によって残差ベクトルを構築するステップと、
前記残差ベクトルの平方和を最小化することによって、前記各時刻における姿勢と前記すべての直線との間の最適推定式を得るステップと、
前記最適推定式を反復解法で解いて、前記各時刻における姿勢及びすべての直線の直線パラメータを得るステップと、を含むことを特徴とする請求項１～６のいずれか一項に記載の方法。
【請求項8】
ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得るように構成される抽出モジュールと、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定するように構成される推定モジュールと、
前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得るように構成されるマッチングモジュールと、
各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得るように構成される最適化モジュールと、を含むことを特徴とする自己位置推定および地図作成装置。
【請求項9】
前記推定モジュールは、
【請求項10】
前記マッチングモジュールは、前記ｔ－１時刻において前記ロボットにより形成されるロボット座標系を基準座標系として決定し、
前記相対姿勢に応じて、前記ｔ時刻に対応する線分の２つの端点の座標を前記基準座標系に変換し、前記ｔ時刻に対応する線分の前記基準座標系での表現式を得て、
前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行うように構成されることを特徴とする請求項８又は９に記載の装置。
【請求項11】
前記ｔ時刻に対応する各線分に対して、予め決定された属性特徴及び予め保存された属性特徴閾値に応じて、前記ｔ－１時刻に対応するすべての線分から差異線分をフィルタリングし、前記ｔ－１時刻に対応する残りの線分を該線分に対応する候補線分として決定するように構成されるフィルタリングモジュールをさらに含み、これに対応して、前記マッチングモジュールは、前記ｔ時刻に対応する各線分に対して、該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行うように構成されることを特徴とする請求項１０に記載の装置。
【請求項12】
該線分に対応する候補線分の数がゼロではないことを決定するように構成される決定モジュールをさらに含み、ゼロであると決定モジュールにより決定される場合、前記装置は、該線分が新しい直線に属することを決定するように構成される追加モジュールをさらに含むことを特徴とする請求項１１に記載の装置。
【請求項13】
前記マッチングモジュールは、前記ｔ時刻に対応する各線分に対して、該線分と前記ｔ－１時刻に対応する各単一線分を線分ペアとして形成し、予め構築された損失関数
前記損失値が最も小さい時に対応する線分ペアを同一直線に属するものとして決定するように構成され、
【請求項14】
前記最適化モジュールは、前記各時刻における前記ロボットの姿勢の表現式を設定し、
前記線分マッチングを経て得られたすべての直線の表現式を設定し、
前記姿勢の表現式及び前記すべての直線の表現式によって残差ベクトルを構築し、
前記残差ベクトルの平方和を最小化することによって、前記各時刻における姿勢と前記すべての直線との間の最適推定式を得て、
前記最適推定式を反復解法で解いて、前記各時刻における姿勢及びすべての直線の直線パラメータを得るように構成されることを特徴とする請求項８～１３のいずれか一項に記載の装置。
【請求項15】
メモリと、プロセッサとを含み、前記メモリは前記プロセッサに接続され、前記メモリは、プログラムを記憶することに用いられ、
前記プロセッサは、前記メモリに記憶されているプログラムを呼び出して請求項１－７のいずれか１項に記載の方法を実行することを特徴とするロボット。
【請求項16】
コンピュータにより実行されると、請求項１－７のいずれか１項に記載の方法を実行するコンピュータプログラムが記憶されていることを特徴とするコンピュータ記憶媒体。
【発明の詳細な説明】
【技術分野】
【0001】
＜関連出願の相互参照＞
本願は、２０２０年１１月２７日に中国特許局に提出された、出願番号が２０２０１１３６７３３５．９、名称が「自己位置推定・地図作成方法、装置、ロボット及びコンピュータ記憶媒体」の中国特許出願の優先権を主張し、その全内容が引用により本願に組み込まれている。
本願は、２０２０年１１月２７日に中国特許局に提出された、出願番号が２０２０１１３６７３３５．９、名称が「自己位置推定・地図作成方法、装置、ロボット及びコンピュータ記憶媒体」の中国特許出願の優先権を主張し、その全内容が引用により本願に組み込まれている。
【0002】
本願は、自律航法の分野に属し、具体的には、自己位置推定・地図作成方法、装置、ロボット及びコンピュータ記憶媒体に関する。
【背景技術】
【0003】
移動ロボットの運転中に、自己位置推定をリアルタイムに行うためにデータを継続的に取得する必要があり、また、取得されたデータに応じて周囲環境に対して地図作成を行う必要もある。
【0004】
一般的に、移動ロボットはＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ、自己位置推定と地図作成の同時実行）アルゴリズムによって、自己位置推定と地図作成の同時実行を行うためのデータを取得及び決定し、従って、ＳＬＡＭアルゴリズムの優劣は移動ロボットの移動中の自己位置推定精度及び安定性を直接決定しており、従って、安定した効率的で高精度なＳＬＡＭアルゴリズムを提案する必要がある。
【0005】
関連技術で使用されるＳＬＡＭアルゴリズムは、計算量が大きく、計算効率が低いなどの問題があり、さらに自己位置推定と地図作成の同時実行の効率を損なってしまう。
【発明の概要】
【発明が解決しようとする課題】
【0006】
上記事情に鑑みて、本願は、上記問題の少なくとも１つを改善できる自己位置推定・地図作成方法、装置、ロボット及びコンピュータ記憶媒体を提供することを目的とする。
【課題を解決するための手段】
【0007】
本願の実施例は以下のように実現される。
【0008】
本願の実施例は、ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得るステップと、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定するステップと、前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得るステップと、各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得るステップと、を含む自己位置推定・地図作成方法を提供する。
【0009】
選択的に、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定する前記ステップは、前記ｔ時刻における前記ロボットの姿勢を
に設定し、前記ｔ－１時刻における前記ロボットの姿勢を
に設定するステップと、前記ロボットの前記ｔ時刻におけるロボット座標系の、世界座標系に対する回転行列及び並進ベクトルを
にそれぞれ設定するステップと、予め保存されたロボットホイールエンコーダ姿勢推定アルゴリズムに応じて、前記ｔ－１時刻に対する前記ｔ時刻における相対姿勢
を算出するステップと、を含み、
は式
を満たし、
【0010】
選択的に、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行う前記ステップは、前記ｔ－１時刻において前記ロボットにより形成されるロボット座標系を基準座標系として決定するステップと、前記相対姿勢に応じて、前記ｔ時刻に対応する線分の２つの端点の座標を前記基準座標系に変換し、前記ｔ時刻に対応する線分の前記基準座標系での表現式を得るステップと、前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行うステップと、を含む。
【0011】
選択的に、前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う前記ステップの前に、前記方法は、前記ｔ時刻に対応する各線分に対して、予め決定された属性特徴及び予め保存された属性特徴閾値に応じて、前記ｔ－１時刻に対応するすべての線分から差異線分をフィルタリングし、前記ｔ－１時刻に対応する残りの線分を該線分に対応する候補線分として決定するステップをさらに含み、
これに対応して、前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う前記ステップは、前記ｔ時刻に対応する各線分に対して、該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行うステップを含む。
これに対応して、前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う前記ステップは、前記ｔ時刻に対応する各線分に対して、該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行うステップを含む。
【0012】
選択的に、該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行う前記ステップの前に、前記方法は、該線分に対応する候補線分の数がゼロではないことを決定するステップをさらに含み、さもないと、前記方法は、該線分が新しい直線に属することを決定するステップをさらに含む。
【0013】
選択的に、前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う前記ステップは、前記ｔ時刻に対応する各線分に対して、該線分と前記ｔ－１時刻に対応する各単一線分を線分ペアとして形成し、予め構築された損失関数
に応じて、該線分に対応する各線分ペアの損失値を計算するステップと、前記損失値が最も小さい時に対応する線分ペアを同一直線に属するものとして決定するステップと、を含み、
【0014】
選択的に、各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得る前記ステップは、前記各時刻における前記ロボットの姿勢の表現式を設定するステップと、前記線分マッチングを経て得られたすべての直線の表現式を設定するステップと、前記姿勢の表現式及び前記すべての直線の表現式によって残差ベクトルを構築するステップと、前記残差ベクトルの平方和を最小化することによって、前記各時刻における姿勢と前記すべての直線との間の最適推定式を得るステップと、前記最適推定式を反復解法で解いて、前記各時刻における姿勢及びすべての直線の直線パラメータを得るステップと、を含む。
本願の実施例は、抽出モジュール、推定モジュール、マッチングモジュール及び最適化モジュールを含む自己位置推定および地図作成装置を提供する。抽出モジュールは、ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得るように構成され、推定モジュールは、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定するように構成され、マッチングモジュールは、前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得るように構成され、最適化モジュールは、各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得るように構成される。
本願の実施例は、抽出モジュール、推定モジュール、マッチングモジュール及び最適化モジュールを含む自己位置推定および地図作成装置を提供する。抽出モジュールは、ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得るように構成され、推定モジュールは、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定するように構成され、マッチングモジュールは、前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得るように構成され、最適化モジュールは、各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得るように構成される。
【0015】
選択的に、前記推定モジュールは、前記ｔ時刻における前記ロボットの姿勢を
に設定し、前記ｔ－１時刻における前記ロボットの姿勢を
に設定し、前記ロボットの前記ｔ時刻におけるロボット座標系の、世界座標系に対する回転行列及び並進ベクトルを
にそれぞれ設定し、予め保存されたロボットホイールエンコーダ姿勢推定アルゴリズムに応じて、前記ｔ－１時刻に対する前記ｔ時刻における相対姿勢
を算出するように構成され、
を満たし、
に設定し、前記ロボットの前記ｔ時刻におけるロボット座標系の、世界座標系に対する回転行列及び並進ベクトルを
【0016】
選択的に、前記マッチングモジュールは、前記ｔ－１時刻において前記ロボットにより形成されるロボット座標系を基準座標系として決定し、前記相対姿勢に応じて、前記ｔ時刻に対応する線分の２つの端点の座標を前記基準座標系に変換し、前記ｔ時刻に対応する線分の前記基準座標系での表現式を得て、前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行うように構成される。
【0017】
選択的に、前記装置は、前記ｔ時刻に対応する各線分に対して、予め決定された属性特徴及び予め保存された属性特徴閾値に応じて、前記ｔ－１時刻に対応するすべての線分から差異線分をフィルタリングし、前記ｔ－１時刻に対応する残りの線分を該線分に対応する候補線分として決定するように構成されるフィルタリングモジュールをさらに含み、
これに対応して、前記マッチングモジュールは、前記ｔ時刻に対応する各線分に対して、該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行うように構成される。
これに対応して、前記マッチングモジュールは、前記ｔ時刻に対応する各線分に対して、該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行うように構成される。
【0018】
選択的に、前記装置は、該線分に対応する候補線分の数がゼロではないことを決定するように構成される決定モジュールをさらに含み、ゼロであると決定モジュールにより決定される場合、前記装置は、該線分が新しい直線に属することを決定するように構成される追加モジュールをさらに含む。
【0019】
選択的に、前記マッチングモジュールは、前記ｔ時刻に対応する各線分に対して、該線分と前記ｔ－１時刻に対応する各単一線分を線分ペアとして形成し、
【0020】
選択的に、前記最適化モジュールは、前記各時刻における前記ロボットの姿勢の表現式を設定し、前記線分マッチングを経て得られたすべての直線の表現式を設定し、前記姿勢の表現式及び前記すべての直線の表現式によって残差ベクトルを構築するように構成され、前記残差ベクトルの平方和を最小化することによって、前記各時刻における姿勢と前記すべての直線との間の最適推定式を得て、前記最適推定式を反復解法で解いて、前記各時刻における姿勢及びすべての直線の直線パラメータを得る。
【0021】
本願の実施例は、メモリと、プロセッサとを含み、前記メモリは前記プロセッサに接続され、前記メモリはプログラムを記憶するように構成され、前記プロセッサは前記メモリに記憶されているプログラムを呼び出して上記いずれかの可能な自己位置推定・地図作成方法を実行するロボットをさらに提供する。
【0022】
本願の実施例は、コンピュータにより実行されると、上記いずれかの可能な自己位置推定・地図作成方法を実行するコンピュータプログラムに記憶されている不揮発性コンピュータ可読取記憶媒体（以下、コンピュータ記憶媒体と略称される）をさらに提供する。
【0023】
本願のほかの特徴及び利点は後述される明細書において説明されており、そして、明細書から部分的に明らかになるか、又は本願の実施例を実施することによって把握される。本願の目的及びほかの利点は記載される明細書及び図面において特に示される構造によって実現及び獲得される。
【図面の簡単な説明】
【0024】
本願の実施例又は従来技術の技術的解決手段をより明確に説明するために、以下、実施例に使用される必要がある図面を簡単に説明する。明らかなように、以下の説明における図面は単に本願のいくつかの実施例であり、当業者であれば、創造的な労働をせずに、これらの図面に基づいてほかの図面を得ることができる。図示により、本願の上記及びほかの目的、特徴及び利点はより明らかになる。すべての図面において同じ符号は同じ部分を示す。実際のサイズに比例して図面を拡大・縮小して作成することを意図しておらず、本願の趣旨を重点的に示す。
【図1】本願の実施例に係るロボットの構造模式図を示す。
【図2】本願の実施例に係る自己位置推定・地図作成方法のフローチャートを示す。
【図3】本願の実施例に係る点群に応じて直線特徴抽出を行う模式図を示す。
【図4】本願の実施例に係る線分マッチングの模式図を示す。
【図5】本願の実施例に係る自己位置推定および地図作成装置の構造ブロック図を示す。
【発明を実施するための形態】
【0025】
以下、本願の実施例の図面を参照しながら本願の実施例の技術的解決手段を説明する。
【0026】
なお、類似する符号や文字は以下の図面において類似する項目を示し、従って、一旦ある項目が１つの図面において定義されると、その後の図面においてさらに定義及び解釈される必要がない。また、本願の説明では、例えば「第１」、「第２」などのような関係用語は単に１つの実体又は操作ともう１つの実体又は操作とを区別するためのものであり、必ずしもこれらの実体又は操作間にいかなる実際の関係又は順序が存在することを指示又は暗示しない。そして、「含む」などの用語又はいかなるほかの変形は非排他的な包含をカバーすることを意図し、それにより一連の要素を含むプロセス、方法、物品又は機器はこれらの要素に加えて、明確にリストされていないほかの要素をさらに含むか、又はこのプロセス、方法、物品又は機器に固有の要素をさらに含む。より多くの制限がない場合、「１つの…を含む」という文で限定される要素は、前記要素を含むプロセス、方法、物品又は機器に別の同じ要素がさらに存在することを排除しない。
【0027】
また、本願では、「及び／又は」という用語は、単に関連対象の関連関係を説明するためのものに過ぎず、３種の関係が存在できることを表し、例えば、Ａ及び／又はＢは、Ａのみが存在すること、ＡとＢの両方が存在すること、Ｂのみが存在することという３種の場合を表してもよい。
【0028】
関連技術では、グラフ最適化技術を使用して直線特徴とロボット姿勢をリアルタイムで最適化するＳＬＡＭアルゴリズムが提案されているが、出願人は、研究を行ったところ、該アルゴリズムでは、直線特徴に対応する６つのパラメータを使用して直線を表し、直線についての記述が最小パラメトリック表示ではなく、従って、後続の計算量が大きく、計算効率が低いという問題を招き、さらに自己位置推定と地図作成の同時実行の効率を損なってしまうことを見出した。
【0029】
関連技術における自己位置推定と地図作成の同時実行手段に存在する欠陥（計算量が大きく、計算効率が低い）は出願人により実践及び鋭意研究された結果であり、従って、上記欠陥を見出すプロセス、及び以下で上記欠陥に対して本願の実施例に提案される解決手段はすべて本願への出願人の貢献としてみなすべきである。
【0030】
上記問題の少なくとも１つを改善するために、本願の実施例は、計算プロセスにおける計算量を低減させ、それによって計算効率を向上させることができる自己位置推定・地図作成方法、装置、ロボット及びコンピュータ記憶媒体を提供する。
【0031】
該技術は、対応するソフトウェア、ハードウェア、及びソフトウェアとハードウェアとの組合せによって実現されてもよい。以下、本願の実施例を詳細に説明する。
【0032】
まず、図１を参照して本願の実施例における自己位置推定・地図作成方法、装置を実現するためのロボット１００を説明する。
【0033】
ロボット１００は、プロセッサ１１０、メモリ１２０、及びレーザー照射コンポーネント１３０を含んでもよい。
【0034】
なお、図１に示すロボット１００のコンポーネント及び構造は制限的ではなく例示的なものであり、必要に応じて、ロボット１００はほかのコンポーネント及び構造をさらに有してもよい。
【0035】
プロセッサ１１０、メモリ１２０、レーザー照射コンポーネント１３０、及びロボット１００に出現する可能性があるほかのコンポーネントは互いに直接又は間接的に電気的接続されてデータの伝送又は対話を実現する。例えば、プロセッサ１１０、メモリ１２０、レーザー照射コンポーネント１３０、及び出現する可能性があるほかのコンポーネントは互いに１つ又は複数の通信バス又は信号線により電気的接続を実現する。
【0036】
レーザー照射コンポーネント１３０は、通電されると、一定の周波数で外部へレーザーを照射し、外部物体でレーザーを反射した反射レーザーを受光し、それにより各レーザー照射時刻に対応する点群データを生成する。
【0037】
選択的に、レーザー照射コンポーネント１３０は、マルチスレッドレーザーを発するためのマルチスレッドのレーザーコンポーネントであってもよいか、又は、レーザー照射コンポーネント１３０は、シングルスレッドレーザーを発するためのシングルスレッドのレーザーコンポーネントであってもよい。
【0038】
選択的に、シングルスレッドレーザーに基づいて得られる点群データのデータ複雑度はマルチスレッドレーザーに基づいて得られる点群データのデータ複雑度よりも低く、従って、計算量の低減及びデータ複雑度の低減の目的から、シングルスレッドレーザーをレーザー照射コンポーネント１３０として直接使用することができ、それにより後続で得られる点群データは二次元離散点からなる。
【0039】
メモリ１２０は、プログラムを記憶することに用いられ、例えば、後述される自己位置推定・地図作成方法に対応するプログラム又は後述される自己位置推定および地図作成装置を記憶する。選択的に、メモリ１２０内に自己位置推定および地図作成装置が記憶されている場合、自己位置推定および地図作成装置はソフトウェア又はファームウェア（ｆｉｒｍｗａｒｅ）の形式でメモリ１２０に記憶可能な少なくとも１つのソフトウェア機能モジュールを含む。
【0040】
選択的に、自己位置推定および地図作成装置に含まれるソフトウェア機能モジュールはロボット１００のオペレーティングシステム（ｏｐｅｒａｔｉｎｇ ｓｙｓｔｅｍ、ＯＳ）に固定化されてもよい。
【0041】
プロセッサ１１０は、メモリ１２０に記憶されている実行可能モジュール、例えば、自己位置推定および地図作成装置に含まれるソフトウェア機能モジュール又はコンピュータプログラムを実行することに用いられる。プロセッサ１１０は実行命令を受信すると、コンピュータプログラムを実行することができ、例えば、ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得るステップと、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定するステップと、前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得るステップと、各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得るステップと、を実行する。
【0042】
勿論、本願のいずれかの実施例に開示されている方法は、プロセッサ１１０に適用されるか、又はプロセッサ１１０により実現されてもよい。
【0043】
以下、本願に係る自己位置推定・地図作成方法を説明する。
【0044】
図２に参照されるように、本願の実施例は上記ロボット１００に適用される自己位置推定・地図作成方法を提供する。以下、図２に示すステップＳ１１０～ステップＳ１４０と併せて、点群データが二次元離散点からなることを例として、本願の実施例を説明する。
【0045】
ステップＳ１１０では、ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得る。
【0046】
ロボット１００は通電して起動されると、レーザー照射コンポーネント１３０によって外部へレーザーを発し、それによって現時刻における環境状況を反映するための点群データを取得することができる。
【0047】
後続の各時刻において、ロボット１００はレーザー照射コンポーネント１３０によって現時刻における点群データを取得し続ける。
【0048】
点群データには複数の二次元離散点が含まれる。
【0049】
シングルスレッドレーザーにより生成される複数の平面における二次元離散点は式
で表されてもよい。ｘは現時刻のロボット座標系ｏにおける二次元離散点の横座標を表し、ｙは現時刻のロボット座標系ｏにおける二次元離散点の縦座標を表す。R2は二次元離散点の表現式Ｐにｘとｙの２つの実数変数が含まれることを表す。
【0050】
一般的には、ロボット１００には第１識別点及び第２識別点が予め設定されている。
【0051】
任意の時刻において、ロボット１００は、現時刻における第１識別点の位置及び現時刻における第２識別点の位置を取得することによって、現時刻に対応するロボット座標系を決定することができる。
【0052】
例えば、いずれかの時刻において、ロボット１００は、第１識別点の位置を座標原点とし、第２識別点の位置と第１識別点の位置とを結ぶ線が位置する直線をｙ軸とし、ｙ軸に垂直な直線をｘ軸とすることによって、現時刻に対応するロボット座標系を決定する。
【0053】
ただし、ロボット１００が移動し続けており、従って、各時刻に対応するロボット座標系はいずれも異なる可能性がある。また、一般的に、起動の初期時刻においてロボット１００により決定されるロボット座標系をグローバルな世界座標系ｗとする。
【0054】
第１識別点は、ロボット１００の幾何学的中心点の位置であってもよく、ロボット１００のほかの位置に対応する点であってもよいが、本願の実施例では特に限定しない。
【0055】
【0056】
ｎは正の整数である。
【0057】
ステップＳ１２０では、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定する。ｔ－１時刻は（ｔ－１）時刻として表されてもよい。
【0058】
ｔ時刻におけるロボット１００の位置はｔ時刻におけるロボット１００の姿勢と呼ばれる。
【0059】
【0060】
【0061】
【0062】
【0063】
異なる時刻同士におけるロボットの相対姿勢の推定アルゴリズムは、ロボットホイールエンコーダの姿勢推定アルゴリズム、点群反復最接近点（ＩＣＰ、Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）のマッチングアルゴリズムなどを含んでもよいが、これらに限定されない。本願の実施例では、
の推定アルゴリズムの具体的なタイプを制限しない。
【0064】
ロボットホイールエンコーダの姿勢推定アルゴリズムを例として、該アルゴリズムに応じて、ロボットは上記ｔ－１時刻に対するｔ時刻における相対姿勢
を算出することができる。
【0065】
ここで、
を満たし、ｖは内蔵されたホイールエンコーダによりロボット１００が測定した自体の進行時の線速度であり、wは内蔵されたホイールエンコーダによりロボット１００が測定した自体の進行時の角速度であり、△ｔはｔ時刻とｔ－１時刻との時間間隔である。上記エンコーダは角変位センサであり、ロボットホイールが所定時間内に回転するラジアンを検出することによってロボット姿勢の変化を決定し、主に光電式、接触式、電磁式の３種のタイプに分けられ、具体的には、実際の状況に応じて、選択されるエンコーダのタイプを設定することができる。
【0066】
ただし、上記ステップＳ１１０とステップＳ１２０の実行順序には厳密な優先順位は存在しない。理解できるように、いくつかの実施形態では、ステップＳ１２０はステップＳ１１０の前に実行されてもよく、別のいくつかの実施形態では、ステップＳ１２０はステップＳ１１０の後に実行されてもよく、別のいくつかの実施形態では、ステップＳ１２０とステップＳ１１０は同時に実行されてもよい。
【0067】
ステップＳ１３０では、前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得る。
【0068】
一般的には、レーザー照射コンポーネント１３０は環境において１０－５０フレーム／秒の速度で点群データを収集し、従って、同一の空間シーンに対して、隣接する数フレームで収集された点群データ間の差異性が小さい。この前提の下では、地図を作成する必要がある場合、計算量低減の目的から、隣接する時刻において抽出された線分間に対して相互マッチングを行う必要があり、それによって同一直線に属する線分を関連付け、同一直線として決定する。また、後続で地図を作成する際に、１つの完全なグラフを形成し、作成の精度及び完全性を向上させることに寄与する。
【0069】
選択的に、線分マッチングのプロセスは以下の方式を参照することができる。
【0070】
ステップＳ１３１では、前記相対姿勢に応じて、前記ｔ時刻に対応する線分の２つの端点の座標を前記基準座標系に変換し、前記ｔ時刻に対応する線分の前記基準座標系での表現式を得る。
【0071】
現時刻をｔ＝ｃ、前時刻をｔ－１＝ｒ、前時刻に対する現時刻における相対姿勢を
として仮定し、前時刻（ｔ－１時刻）においてロボット１００により形成されたロボット座標系を基準座標系とする。
【0072】
【0073】
【0074】
ステップＳ１３２では、前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う。
【0075】
選択的に、座標系変換後の現時刻に対応する線分と前時刻に対応する線分に対して直接マッチングを行うことができる。
【0076】
選択的に、線分マッチングを行う前に、まず、前時刻において得られた線分をフィルタリングして前時刻に対応する線分中の差異線分を除去し、残りの線分を候補線分として決定することができる。
【0077】
各線分について、それに対応する差異線分は該線分との間に大きな差異があり、両者が同一直線に属する確率が小さいことを表す。
【0078】
線分マッチングを行う前に、現時刻に対応する線分について、前時刻に応する線分に対して差異線分除去を行った場合、座標系変換後の現時刻に対応する線分と前時刻に対応する線分に対してマッチングを行う際に、座標系変換後の現時刻に対応する線分及び前時刻に対応する候補線分に対して直接マッチングを行うことができ、それによってマッチング操作を実行する時のデータ量を低減させることができる。
【0079】
以下の方式を参照して候補線分をフィルタリングすることができる。
【0080】
現時刻に対応する各線分に対して、予め決定された属性特徴及び予め保存された属性特徴閾値に応じて、属性特徴と属性特徴閾値を比較することによって、前時刻に対応するすべての線分から差異線分をフィルタリングし、前時刻における残りの線分を該線分に対応する候補線分として決定することができる。
【0081】
【0082】
【0083】
【0084】
【0085】
ｄ１、ｄ２、ｄ０の直感的な意味は以下の図４に示される。
【0086】
以下、線分マッチングのプロセスについて説明を行う。
【0087】
ｔ時刻に対応する各線分に対して、該線分とｔ－１時刻に対応する各単一線分とを線分ペアとして形成し、予め構築された損失関数に応じて該線分に対応する各線分ペアの損失値を計算し、その後、損失値が最も小さい時に対応する線分ペアを同一直線に属するものとして決定する。
【0088】
【0089】
【0090】
上記損失値計算プロセスを繰り返して現時刻ｃにおいて観測されたある線分Li
cと前時刻ｒにおいて観測された各線分（線分フィルタリングを行った場合、ここでは前時刻ｒにおいて観測された各候補線分である）との損失値を得ることができる。後続では、各損失値の大きさを比較することによって、損失値が最も小さい時に対応する前時刻ｒにおいて観測された線分とLi
cを同一直線に属するものとして決定する。このとき、２つの線分は、両者が同一直線に属することを示すように、同じ直線識別子（例えば直線ＩＤ）を設定することができ、同一の表現式で表すことができる。
【0091】
また、現時刻において観測されたある線分Li
cの場合、前時刻ｒにおいて観測された各線分に対して線分フィルタリングを行うと、最終的に得られる候補線分は０であると、このとき、該線分Li
cは新しく出現する直線の一部であることを示し、このとき、該線分Li
cに１つの新しい直線識別子を設定することができる。後続の時刻において該線分Li
cと同一直線に属する線分が発生すると、該線分Li
cと同一直線に属する後続の線分に該新しい直線識別子を継承させることができる。
【0092】
各時刻において上記線分マッチングによって現時刻において観測された線分と前時刻において観測された線分に対して反復マッチングを行った後に、第１時刻と現時刻において同一直線に属する線分を関連付けることができる。
【0093】
また、二次元直線は平面内で２つの自由度しかなく、即ち、ｙ＝ｋｘ＋ｂであり、直線の向きが既知で且つ直線と原点との距離が既知であるという前提の下では、一意に決定されたｘ、ｙは１つの直線を一意に決定でき、従って、２つのパラメータによって１つの直線を一意に決定することができる。
【0094】
【0095】
ステップＳ１４０では、各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得る。
【0096】
ロボット１００が現時刻において推定する必要があるロボット軌跡がｎ個の時刻における姿勢
を含むことを仮定するとともに、現時刻において線分マッチングを経て得られた直線がｍ本あり、ｍ本の直線を
として表すことを仮定する。
【0097】
【0098】
【0099】
【0100】
残差ベクトルの平方和を最小化することによって、各時刻における姿勢とすべての直線との間の最適推定式
を得ることができる。
【0101】
上記最適推定式を反復解法で解くすることによって、各時刻における姿勢及びすべての直線の直線パラメータを得ることができ、それによってロボット１００の軌跡推定及び地図作成を完了する。
【0102】
非線形最小二乗問題の解法は、例えば、ガウスニュートンアルゴリズム（Ｇａｕｓｓ－Ｎｅｗｔｏｎ）、レベンベルク－マーカルトアルゴリズム（Ｌｅｖｅｎｂｅｒｇ－Ｍａｒｑｕａｒｄｔ）、最急降下法などの従来の計算方式を使用して反復解法を行うことができ、本願の実施例では解法を特に限定しない。
【0103】
以上のように、本願の実施例に係る自己位置推定・地図作成方法は少なくとも以下の利点を有する。
【0104】
（１）直線を表す際に、二直線パラメトリック表示形式を使用するため、後続で自己位置推定および地図作成を実行するためのデータを決定する計算量をできるだけ低減させることができ、それによって地図における自己位置推定および地図作成効率を向上させ、後続の自己位置推定と地図作成の同時実行の効率を向上させる。
【0105】
（２）線分マッチング時にマッチングする必要がある線分の数を低減させることによって、計算量を低減させ、さらに計算効率を向上させる。
【0106】
図５に示すように、本願の実施例は自己位置推定および地図作成装置４００をさらに提供し、自己位置推定および地図作成装置４００はロボットに設置されてもよく、自己位置推定および地図作成装置４００は、抽出モジュール４１０、推定モジュール４２０、マッチングモジュール４３０及び最適化モジュール４４０を含んでもよい。
【0107】
抽出モジュール４１０は、ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得るように構成され、
推定モジュール４２０は、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定するように構成され、
マッチングモジュール４３０は、前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得るように構成され、
最適化モジュール４４０は、各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得るように構成される。
推定モジュール４２０は、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定するように構成され、
マッチングモジュール４３０は、前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得るように構成され、
最適化モジュール４４０は、各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得るように構成される。
【0108】
選択的に、前記推定モジュール４２０は、前記ｔ時刻における前記ロボットの姿勢
に設定し、前記ｔ－１時刻における前記ロボットの姿勢を
に設定し、前記ロボットの前記ｔ時刻におけるロボット座標系の、世界座標系に対する回転行列及び並進ベクトルを
にそれぞれ設定し、予め保存されたロボットホイールエンコーダ姿勢推定アルゴリズムに応じて、前記ｔ－１時刻に対する前記ｔ時刻における相対姿勢
【0109】
選択的に、前記マッチングモジュール４３０は、前記ｔ－１時刻において前記ロボットにより形成されるロボット座標系を基準座標系として決定し、前記相対姿勢に応じて、前記ｔ時刻に対応する線分の２つの端点の座標を前記基準座標系に変換し、前記ｔ時刻に対応する線分の前記基準座標系での表現式を得て、前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行うように構成される。
【0110】
選択的に、前記装置は、前記ｔ時刻に対応する各線分に対して、予め決定された属性特徴及び予め保存された属性特徴閾値に応じて、前記ｔ－１時刻に対応するすべての線分から差異線分をフィルタリングし、前記ｔ－１時刻に対応する残りの線分を該線分に対応する候補線分として決定するように構成されるフィルタリングモジュールをさらに含み、
これに対応して、前記マッチングモジュール４３０は、前記ｔ時刻に対応する各線分に対して、該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行うように構成される。
これに対応して、前記マッチングモジュール４３０は、前記ｔ時刻に対応する各線分に対して、該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行うように構成される。
【0111】
選択的に、前記装置は、該線分に対応する候補線分の数がゼロではないことを決定するように構成される決定モジュールをさらに含み、ゼロであると決定モジュールにより決定される場合、前記装置は、該線分が新しい直線に属することを決定するように構成される追加モジュールをさらに含む。
【0112】
選択的に、前記マッチングモジュール４３０は、前記ｔ時刻に対応する各線分に対して、該線分と前記ｔ－１時刻に対応する各単一線分を線分ペアとして形成し、予め構築された損失関数
に応じて、該線分に対応する各線分ペアの損失値を計算し、前記損失値が最も小さい時に対応する線分ペアを同一直線に属するものとして決定するように構成され、
【0113】
選択的に、前記最適化モジュール４４０は、前記各時刻における前記ロボットの姿勢の表現式を設定し、前記線分マッチングを経て得られたすべての直線の表現式を設定し、前記姿勢の表現式及び前記すべての直線の表現式によって残差ベクトルを構築し、前記残差ベクトルの平方和を最小化することによって、前記各時刻における姿勢と前記すべての直線との間の最適推定式を得て、前記最適推定式を反復解法で解いて、前記各時刻における姿勢及びすべての直線の直線パラメータを得るように構成される。
【0114】
本願の実施例に係る自己位置推定および地図作成装置４００について、その実現原理及び発生する技術的効果は上記方法実施例と同様であり、説明を簡素化するために、装置実施例の言及されていない部分について、上記方法実施例の対応する内容を参照すればよい。
【0115】
また、本願の実施例は、コンピュータ記憶媒体をさらに提供し、該コンピュータ記憶媒体にコンピュータプログラムが記憶されており、該コンピュータプログラムがコンピュータにより実行されると、上記自己位置推定・地図作成方法に含まれるステップを実行する。
【0116】
また、本発明実施例はロボットをさらに提供し、プロセッサと、前記プロセッサに接続されるメモリとを含み、前記メモリ内にコンピュータプログラムが記憶されており、前記コンピュータプログラムが前記プロセッサにより実行されると、前記ロボットは上記自己位置推定・地図作成方法に含まれるステップを実行する。ロボットの構造模式図は図１を参照すればよい。
【0117】
以上のように、本願の実施例に係る自己位置推定・地図作成方法、装置、ロボット及びコンピュータ記憶媒体は、ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得て、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定し、前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得て、各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得る。直線を表す際に、二直線パラメトリック表示形式を使用するため、データ計算時の計算量をできるだけ低減させることができ、それによってデータ取得効率を向上させ、計算時間を短縮させることができる。
【0118】
本願に係る実施例では、開示されている装置及び方法はほかの形態で実現できると理解すべきである。以上説明された装置実施例は単に例示的なものであり、例えば、図面におけるフローチャート及びブロック図は本願の実施例に係る装置、方法及びコンピュータプログラム製品の実現可能な体系アーキテクチャ、機能及び操作を示す。この点では、フローチャート又はブロック図における各ブロックは１つのモジュール、プログラムセグメント又はコードの一部を表すことができ、前記モジュール、プログラムセグメント又はコードの一部は、所定の論理機能を実現するための１つ又は複数の実行可能命令を含む。なお、いくつかの代替可能な実現形態では、ブロックにおいてマークされた機能は図面においてマークされた順序とは異なる順序で発生してもよい。例えば、２つの連続するブロックは実際にはほぼ並行して実行されてもよく、場合によって逆の順序で実行されてもよく、これは係る機能次第である。なお、ブロック図及び／又はフローチャートにおける各ブロック、及びブロック図及び／又はフローチャートにおけるブロックの組合せは、所定の機能又は動作を実行するための、ハードウェアに基づく専用システムによって実現されてもよいか、又は専用ハードウェアとコンピュータ命令との組合せによって実現されてもよい。
【0119】
また、本願の実施例における各機能モジュールは統合して１つの独立した部分を形成してもよく、各モジュールは単独で存在してもよく、２つ以上のモジュールは統合して１つの独立した部分を形成してもよい。
【0120】
前記機能はソフトウェア機能モジュールの形態で実現され独立した製品として販売又は使用される時、１つのコンピュータ記憶媒体に記憶されてもよい。このような理解に基づいて、本願の技術的解決手段は、本質的に、又は従来技術に貢献する部分又は該技術的解決手段の一部がソフトウェア製品の形態で具現化されてもよく、該コンピュータソフトウェア製品は１つの記憶媒体に記憶され、１台のコンピュータ機器（パーソナルコンピュータ、ノートパソコン，サーバ、又はネットワーク機器などであってもよい）に本願の各実施例に記載の方法のすべて又は一部のステップを実行させるための複数の命令を含む。上記記憶媒体は、Ｕディスク、ポータブルハードディスクドライブ、読み出し専用メモリ（ＲＯＭ、Ｒｅａｄ－Ｏｎｌｙ Ｍｅｍｏｒｙ）、ランダムアクセスメモリ（ＲＡＭ、Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、磁気ディスク又は光ディスクなど、プログラムコードを記憶できる様々な媒体を含む。
【0121】
以上、本願の具体的な実施形態を説明したが、本願の保護範囲はこれに限定されるものではなく、当業者が本願に開示されている技術的範囲を逸脱せずに容易に想到し得る変化や置換はすべて本願の保護範囲に属する。
産業上の利用可能性
産業上の利用可能性
【0122】
本願に係る技術的解決手段では、直線を表す際に、二直線パラメトリック表示形式を使用するため、後続で自己位置推定および地図作成を実行するためのデータを決定する計算量をできるだけ低減させることができ、それによって地図における自己位置推定および地図作成効率を向上させ、後続の自己位置推定と地図作成の同時実行の効率を向上させる。
【符号の説明】
【0123】
１００－ロボット；１１０－プロセッサ；１２０－メモリ；１３０－レーザー照射コンポーネント；４００－自己位置推定および地図作成装置；４１０－抽出モジュール；４２０－推定モジュール；４３０－マッチングモジュール；４４０－最適化モジュール。
【図1】
【図2】
【図3】
【図4】
【図5】
【手続補正書】
【提出日】2023-05-17
【手続補正1】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項1】
ｔ時刻において、取得された点群データに対して直線特徴抽出を行い、対応する複数の線分を得るステップと、ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定するステップと、
前記相対姿勢に応じて、前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行い、各線分が属する直線を得るステップと、
各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得るステップと、を含むことを特徴とする自己位置推定・地図作成方法。
【請求項2】
ｔ－１時刻に対する前記ｔ時刻におけるロボットの相対姿勢を推定する前記ステップは、
【請求項3】
前記ｔ時刻に対応する複数の線分及び前記ｔ－１時刻において直線特徴抽出を行って得られた複数の線分に対して線分マッチングを行う前記ステップは、前記ｔ－１時刻において前記ロボットにより形成されるロボット座標系を基準座標系として決定するステップと、
前記相対姿勢に応じて、前記ｔ時刻に対応する線分の２つの端点の座標を前記基準座標系に変換し、前記ｔ時刻に対応する線分の前記基準座標系での表現式を得るステップと、
前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行うステップと、を含むことを特徴とする請求項１又は２に記載の方法。
【請求項4】
前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う前記ステップの前に、前記ｔ時刻に対応する各線分に対して、予め決定された属性特徴及び予め保存された属性特徴閾値に応じて、前記ｔ－１時刻に対応するすべての線分から差異線分をフィルタリングし、前記ｔ－１時刻に対応する残りの線分を該線分に対応する候補線分として決定するステップをさらに含み、
これに対応して、前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う前記ステップは、
前記ｔ時刻に対応する各線分に対して、該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行うステップを含むことを特徴とする請求項３に記載の方法。
【請求項5】
該線分の前記基準座標系での表現式及び該線分に対応する候補線分の表現式に対してマッチングを行う前記ステップの前に、該線分に対応する候補線分の数がゼロではないことを決定するステップをさらに含み、
さもないと、前記方法は、
該線分が新しい直線に属することを決定するステップをさらに含むことを特徴とする請求項４に記載の方法。
【請求項6】
前記ｔ時刻に対応する線分の前記基準座標系での表現式及び前記ｔ－１時刻に対応する複数の線分の表現式に対してマッチングを行う前記ステップは、
【請求項7】
各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得る前記ステップは、前記各時刻における前記ロボットの姿勢の表現式を設定するステップと、
前記線分マッチングを経て得られたすべての直線の表現式を設定するステップと、
前記姿勢の表現式及び前記すべての直線の表現式によって残差ベクトルを構築するステップと、
前記残差ベクトルの平方和を最小化することによって、前記各時刻における姿勢と前記すべての直線との間の最適推定式を得るステップと、
前記最適推定式を反復解法で解いて、前記各時刻における姿勢及びすべての直線の直線パラメータを得るステップと、を含むことを特徴とする請求項１～６のいずれか一項に記載の方法。
【請求項8】
メモリと、プロセッサとを含み、前記メモリは前記プロセッサに接続され、前記メモリは、プログラムを記憶することに用いられ、
前記プロセッサは、前記メモリに記憶されているプログラムを呼び出して請求項１－７のいずれか１項に記載の方法を実行することを特徴とするロボット。
【請求項9】
コンピュータにより実行されると、請求項１－７のいずれか１項に記載の方法を実行するコンピュータプログラムが記憶されていることを特徴とするコンピュータ記憶媒体。【手続補正3】
【補正対象書類名】明細書
【補正対象項目名】０００２
【補正方法】変更
【補正の内容】
【0002】
本願は、自律航法の分野に属し、具体的には、自己位置推定・地図作成方法、ロボット及びコンピュータ記憶媒体に関する。
【手続補正4】
【補正対象書類名】明細書
【補正対象項目名】０００６
【補正方法】変更
【補正の内容】
【0006】
上記事情に鑑みて、本願は、上記問題の少なくとも１つを改善できる自己位置推定・地図作成方法、ロボット及びコンピュータ記憶媒体を提供することを目的とする。
【手続補正5】
【補正対象書類名】明細書
【補正対象項目名】００１４
【補正方法】変更
【補正の内容】
【0014】
選択的に、各時刻における前記ロボットの姿勢及びすべての直線の二直線パラメトリック表示形式に対して同時最適化処を行い、前記各時刻における前記ロボットの姿勢及びすべての直線の直線パラメータを得る前記ステップは、前記各時刻における前記ロボットの姿勢の表現式を設定するステップと、前記線分マッチングを経て得られたすべての直線の表現式を設定するステップと、前記姿勢の表現式及び前記すべての直線の表現式によって残差ベクトルを構築するステップと、前記残差ベクトルの平方和を最小化することによって、前記各時刻における姿勢と前記すべての直線との間の最適推定式を得るステップと、前記最適推定式を反復解法で解いて、前記各時刻における姿勢及びすべての直線の直線パラメータを得るステップと、を含む。
【手続補正6】
【補正対象書類名】明細書
【補正対象項目名】００１５
【補正方法】削除
【補正の内容】
【手続補正7】
【補正対象書類名】明細書
【補正対象項目名】００１６
【補正方法】削除
【補正の内容】
【手続補正8】
【補正対象書類名】明細書
【補正対象項目名】００１７
【補正方法】削除
【補正の内容】
【手続補正9】
【補正対象書類名】明細書
【補正対象項目名】００１８
【補正方法】削除
【補正の内容】
【手続補正10】
【補正対象書類名】明細書
【補正対象項目名】００１９
【補正方法】削除
【補正の内容】
【手続補正11】
【補正対象書類名】明細書
【補正対象項目名】００２０
【補正方法】削除
【補正の内容】
【国際調査報告】