特表 2024-525327 ＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法及びシステム

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

(12)【公報種別】公表特許公報(A)

(11)【公表番号】

(43)【公表日】2024-07-12

(54)【発明の名称】ＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法及びシステム

(51)【国際特許分類】

   G01C 21/28 20060101AFI20240705BHJP

   G06T 7/579 20170101ALI20240705BHJP

【ＦＩ】

G01C21/28

G06T7/579

【審査請求】有

【予備審査請求】未請求

(21)【出願番号】P 2023576197

(86)(22)【出願日】2022-05-30

(85)【翻訳文提出日】2024-01-31

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

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

(87)【国際公開日】2022-12-15

(31)【優先権主張番号】202110644536.7

(32)【優先日】2021-06-09

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

(81)【指定国・地域】

(71)【出願人】

【識別番号】521017642

【氏名又は名称】山東大学

【氏名又は名称原語表記】ＳＨＡＮＤＯＮＧ ＵＮＩＶＥＲＳＩＴＹ

【住所又は居所原語表記】Ｎｏ．１７９２３， Ｊｉｎｇｓｈｉ Ｒｏａｄ， Ｌｉｘｉａ Ｄｉｓｔｒｉｃｔ Ｊｉｎａｎ， Ｓｈａｎｄｏｎｇ ２５００６１， Ｃｈｉｎａ

(71)【出願人】

【識別番号】523463971

【氏名又は名称】山東亜歴山大智能科技有限公司

【氏名又は名称原語表記】ＳＨＡＮＤＯＮＧ ＡＬＥＳＭＡＲＴ ＩＮＴＥＬＬＩＧＥＮＴ ＴＥＣＨＮＯＬＯＧＹ ＣＯ．，ＬＴＤ．

【住所又は居所原語表記】４ｔｈ ｆｌｏｏｒ，Ｉｎｎｏｖａｔｉｏｎ Ｂｕｉｌｄｉｎｇ，Ｎｏ．１７９２３，Ｊｉｎｇｓｈｉ Ｒｏａｄ，Ｌｉｘｉａ Ｄｉｓｔｒｉｃｔ，Ｊｉｎａｎ，Ｓｈａｎｄｏｎｇ ２５００６１，Ｃｈｉｎａ

(74)【代理人】

【識別番号】110000291

【氏名又は名称】弁理士法人コスモス国際特許商標事務所

(72)【発明者】

【氏名】周 軍

(72)【発明者】

【氏名】趙 一凡

(72)【発明者】

【氏名】欧 金順

(72)【発明者】

【氏名】皇 攀凌

(72)【発明者】

【氏名】孟 広輝

(72)【発明者】

【氏名】高 新彪

(72)【発明者】

【氏名】李 留昭

(72)【発明者】

【氏名】林 楽彬

【テーマコード（参考）】

2F129

5L096

【Ｆターム（参考）】

2F129AA03

2F129BB03

2F129BB19

2F129BB33

2F129BB47

2F129EE02

2F129FF39

2F129GG17

2F129GG18

5L096AA06

5L096AA09

5L096BA05

5L096CA02

(57)【要約】

本開示は、ＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法及びシステムを提供する。前記方法は、同期校正された、モバイルロボットによって測定されたレーザ点群データと視覚点群データを取得するステップと、取得されたレーザ点群データと視覚点群データを融合させるステップと、融合後の点群データに対して、点群運動歪除去処理及び点群フィルタリング処理を行うステップと、処理後の点群データに基づき、グラフ最適化に基づく多重制約ファクタグラフアルゴリズムを用いて、ＩＭＵ事前積分データ、点群キーフレームデータ及びＧＮＳＳデータを制約ファクタグラフに加え、ループクロージャ検出を行うことにより、再構成された３次元地図を得るステップと、を含む。本開示は、鉱山シーンでの３次元再構成を有効に実現することができ、最終的に、色付き点群地図を得ることができ、鉱山のシーン再構成の精度を向上させる。
【選択図】図１

【特許請求の範囲】

【請求項1】

同期校正された、モバイルロボットによって測定されたレーザ点群データと視覚点群データを取得するステップと、
取得されたレーザ点群データと視覚点群データを融合させるステップと、
融合後の点群データに対して、点群運動歪除去処理及び点群フィルタリング処理を行うステップと、
処理後の点群データに基づき、グラフ最適化に基づく多重制約ファクタグラフアルゴリズムを用いて、ＩＭＵ事前積分データ、点群キーフレームデータ及びＧＮＳＳデータを制約ファクタグラフに加え、ループクロージャ検出を行うことにより、再構成された３次元地図を得るステップと、
を含むことを特徴とする、ＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法。

【請求項2】

現在レーザフレームの特徴情報と地図の特徴情報を用いて繰返し最適化を行い、ＩＭＵ事前積分データ、点群キーフレームデータ及びＧＮＳＳデータをグラフ最適化アルゴリズムのファクタとしてファクタグラフに加え、ファクタグラフ最適化を実行することにより、全てのキーフレームポーズを更新し、最適化後のポーズ及び再構成後の３次元地図を最終的に得る
ことを特徴とする、請求項１に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法。

【請求項3】

過去キーフレームからキーフレームを見つけ、現在フレームとキーフレーム付近の複数フレームの点群のマッチングを行うことにより、変換ポーズを取得し、閉ループファクタグラフデータを構築し、ファクタグラフに加えて最適化を行う
ことを特徴とする、請求項２に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法。

【請求項4】

過去キーフレームのうち、距離が所定値よりも小さく、且つ時間間隔が所定値よりも大きいフレームをキーフレームとする
ことを特徴とする、請求項３に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法。

【請求項5】

取得されたレーザ点群データと視覚点群データを融合させるステップは、
タイムスタンプ内挿アルゴリズムを用いて、マッチングされたレーザ点群データと視覚点群データを取得し、キーホールイメージング法により、レーザ点群をカメラ平面に投影させ、ＸＹＺ座標に従って、カメラ平面に投影したレーザ点群と視覚点群をマッチングさせ、融合後の点群ＰｏｉｎｔＣｌｏｕｄ：ＸＹＺＲＩＲＧＢを得るステップを含み、
Ｘはレーザ点群のｘ軸座標であり、Ｙはレーザ点群のｙ軸座標であり、Ｚはレーザ点群のｚ軸座標であり、Ｉはレーザ点群の光度であり、Ｒはレーザビームそれぞれのビーム番号であり、ＲＧＢは視覚点群の色情報である
ことを特徴とする、請求項１に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法。

【請求項6】

点群運動歪除去処理は、
１フレームのレーザの走査開始時刻を時刻Ｔとし、該フレームのレーザの走査終了時刻を時刻Ｔ＋ΔＴとすると、ＩＭＵ３軸の角速度データ及び加速度データに対して事前積分を行い、期間ΔＴ内のロボットの相対運動を得た後、期間ΔＴ内の全てのレーザ点を１つ目のレーザ点に変換することにより、レーザ点群運動歪除去を実現するステップを含む
ことを特徴とする、請求項１に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法。

【請求項7】

点群フィルタリング処理は、
フレームごとのレーザ点群を深度画像として投影させるステップであって、マルチビームレーザレーダの走査ビームをｒビームとし、フレームごとの各ビームによる走査でレーザ点がｎ個得られるとすると、深度画像がｒ×ｎのマトリックスとなり、各マトリックスに点群の座標情報及び光度情報が含まれるステップと、
隣り合う点群間の位置関係を深度画像から取得し、ｋｄ－ｔｒｅｅ探索アルゴリズムを用いて、点群の隣接点を探索し、点群間の幾何学関係により、地面点及びノイズに対してフィルタリング処理を行うステップと、を含む
ことを特徴とする、請求項１に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法。

【請求項8】

同期校正された、モバイルロボットによって測定されたレーザ点群データと視覚点群データを取得するように配置されたデータ取得モジュールと、
取得されたレーザ点群データと視覚点群データを融合させるように配置された点群融合モジュールと、
融合後の点群データに対して、点群運動歪除去処理及び点群フィルタリング処理を行うように配置された点群データ処理モジュールと、
処理後の点群データに基づき、グラフ最適化に基づく多重制約ファクタグラフアルゴリズムを用いて、ＩＭＵ事前積分データ、点群キーフレームデータ及びＧＮＳＳデータを制約ファクタグラフに加え、ループクロージャ検出を行うことにより、再構成された３次元地図を得るように配置された３次元地図再構成モジュールと、
を備えることを特徴とする、ＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成システム。

【請求項9】

プログラムが記憶されたコンピュータ可読記憶媒体であって、該プログラムがプロセッサによって実行されると、請求項１から請求項７のいずれか１項に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法におけるステップを実現することを特徴とする、コンピュータ可読記憶媒体。

【請求項10】

メモリと、プロセッサと、メモリに記憶され、プロセッサで実行可能なプログラムとを含む電子デバイスであって、前記プログラムが前記プロセッサによって実行されると、請求項１から請求項７のいずれか１項に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法におけるステップを実現することを特徴とする、電子デバイス。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、シーン再構成の技術分野に関し、特に、ＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法及びシステムに関する。

【背景技術】

【0002】

この部分の説明は、単に本開示に関連する背景技術を提供するためのものであり、必ず従来技術を構成するものであるとは限らない。

【0003】

５Ｇ世代と無人運転時代の到来に伴い、自己位置推定と環境地図作成（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ，ＳＬＡＭ）技術は、無人運転の測位、高精度な地図収集、ＡＲ（Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ，拡張現実）、測地地図作成作業等の分野において幅広く応用されている。

【0004】

発明者の発見によると、鉱山の環境において、空気中に埃や浮遊粒子状物質が多く存在するため、レーザレーダ、視覚センサによるデータ収集は、精度が低く、ＳＬＡＭの鉱山シーンへの応用が大きく制限されてしまう。また、レーザレーダは、物体の距離情報及び光度情報しか取得することができず、物体の色情報が取得不能なため、生成される３次元再構成地図は、情報量が不十分であり、正確に表示できず、仮想シーンでの建設機械に対する遠隔操作の要求を満たすことができない。一方、視覚カメラは、充実な色付き点群を得ることができるが、測定データの正確度が低いため、単一のセンサだけでは、鉱山環境でのＳＬＡＭ地図作成の要求を満たすことができない。

【発明の概要】

【0005】

従来技術の不足を解決するために、本開示は、鉱山シーンでの３次元再構成を有効に実現することができ、最終的に、色付き点群地図を得ることができ、鉱山のシーン再構成の精度を向上させる、ＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法及びシステムを提供する。

【0006】

上記目的を実現するために、本開示は、次の技術手段を採用する。

【0007】

本開示の第１側面は、
同期校正された、モバイルロボットによって測定されたレーザ点群データと視覚点群データを取得するステップと、
取得されたレーザ点群データと視覚点群データを融合させるステップと、
融合後の点群データに対して、点群運動歪除去処理及び点群フィルタリング処理を行うステップと、
処理後の点群データに基づき、グラフ最適化に基づく多重制約ファクタグラフアルゴリズムを用いて、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ，慣性計測ユニット）事前積分データ、点群キーフレームデータ及びＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ，全地球的衛星航法システム）データを制約ファクタグラフに加え、ループクロージャ検出を行うことにより、再構成された３次元地図を得るステップと、
を含む、ＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法を提供する。

【0008】

さらに、現在レーザフレームの特徴情報と地図の特徴情報を用いて繰返し最適化を行い、ＩＭＵ事前積分データ、点群キーフレームデータ及びＧＮＳＳデータをグラフ最適化アルゴリズムのファクタとしてファクタグラフに加え、ファクタグラフ最適化を実行することにより、全てのキーフレームポーズを更新し、最適化後のポーズ及び再構成後の３次元地図を最終的に得る。

【0009】

よりさらに、過去キーフレームからキーフレームを見つけ、現在フレームとキーフレーム付近の複数フレームの点群のマッチングを行うことにより、変換ポーズを取得し、閉ループファクタグラフデータを構築し、ファクタグラフに加えて最適化を行う。

【0010】

よりさらに、過去キーフレームのうち、距離が所定値よりも小さく、且つ時間間隔が所定値よりも大きいフレームをキーフレームとする。

【0011】

さらに、取得されたレーザ点群データと視覚点群データを融合させるステップは、
タイムスタンプ内挿アルゴリズムを用いて、マッチングされたレーザ点群データと視覚点群データを取得し、キーホールイメージング法により、レーザ点群をカメラ平面に投影させ、ＸＹＺ座標に従って、カメラ平面に投影したレーザ点群と視覚点群をマッチングさせ、融合後の点群ＰｏｉｎｔＣｌｏｕｄ：ＸＹＺＲＩＲＧＢを得るステップを含み、
Ｘはレーザ点群のｘ軸座標であり、Ｙはレーザ点群のｙ軸座標であり、Ｚはレーザ点群のｚ軸座標であり、Ｉはレーザ点群の光度であり、Ｒはレーザビームそれぞれのビーム番号であり、ＲＧＢは視覚点群の色情報である。

【0012】

さらに、点群運動歪除去処理は、
１フレームのレーザの走査開始時刻を時刻Ｔとし、該フレームのレーザの走査終了時刻を時刻Ｔ＋ΔＴとすると、ＩＭＵ３軸の角速度データ及び加速度データに対して事前積分を行い、期間ΔＴ内のロボットの相対運動を得た後、期間ΔＴ内の全てのレーザ点を１つ目のレーザ点に変換することにより、レーザ点群運動歪除去を実現するステップを含む。

【0013】

さらに、点群フィルタリング処理は、
フレームごとのレーザ点群を深度画像として投影させるステップであって、マルチビームレーザレーダの走査ビームをｒビームとし、フレームごとの各ビームによる走査でレーザ点がｎ個得られるとすると、深度画像がｒ×ｎのマトリックスとなり、各マトリックスに点群の座標情報及び光度情報が含まれるステップと、
隣り合う点群間の位置関係を深度画像から取得し、ｋｄ－ｔｒｅｅ探索アルゴリズムを用いて、点群の隣接点を探索し、点群間の幾何学関係により、地面点及びノイズに対してフィルタリング処理を行うステップと、を含む。

【0014】

本開示の第２側面は、
同期校正された、モバイルロボットによって測定されたレーザ点群データと視覚点群データを取得するように配置されたデータ取得モジュールと、
取得されたレーザ点群データと視覚点群データを融合させるように配置された点群融合モジュールと、
融合後の点群データに対して、点群運動歪除去処理及び点群フィルタリング処理を行うように配置された点群データ処理モジュールと、
処理後の点群データに基づき、グラフ最適化に基づく多重制約ファクタグラフアルゴリズムを用いて、ＩＭＵ事前積分データ、点群キーフレームデータ及びＧＮＳＳデータを制約ファクタグラフに加え、ループクロージャ検出を行うことにより、再構成された３次元地図を得るように配置された３次元地図再構成モジュールと、
を備える、ＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成システムを提供する。

【0015】

本開示の第３側面は、プログラムが記憶されたコンピュータ可読記憶媒体であって、該プログラムがプロセッサによって実行されると、本開示の第１側面に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法におけるステップを実現する、コンピュータ可読記憶媒体を提供する。

【0016】

本開示の第４側面は、メモリと、プロセッサと、メモリに記憶され、プロセッサで実行可能なプログラムとを含む電子デバイスであって、前記プログラムが前記プロセッサによって実行されると、本開示の第１側面に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法におけるステップを実現する、電子デバイスを提供する。

【0017】

従来技術と比べると、本開示は、次の有益な効果がある。

【0018】

１、本開示に記載されている方法、システム、媒体又は電子デバイスによれば、鉱山シーンでの３次元再構成を有効に実現することができ、最終的に、色付き点群地図を得ることができ、鉱山のシーン再構成の精度を向上させる。

【0019】

２、本開示に記載されている方法、システム、媒体又は電子デバイスによれば、レーザレーダ、ＩＭＵ、北斗測位システム、視覚カメラによって、複数種類のセンサの統合を実現し、鉱山等の複雑なシーンにおける、単一のセンサによる地図作成の正確度が低い欠点を補い、安定した信頼性の高いＳＬＡＭ地図作成を実現する。

【0020】

３、本開示に記載されている方法、システム、媒体又は電子デバイスによれば、レーザレーダと視覚カメラの点群を融合させることで、色情報付きの３次元再構成地図を構築することができ、遠隔の没入型施工の要求を満たす。

【0021】

４、本開示に記載されている方法、システム、媒体又は電子デバイスによれば、鉱山環境について、３Ｄレーザ点群を深度画像として投影させ、地面点を抽出する。鉱山環境において埃が多い状況について、クラスタアルゴリズムにより、ノイズに対する正確なフィルタリング処理を実現する。

【0022】

５、本開示に記載されている方法、システム、媒体又は電子デバイスによれば、ＧＴＳＡＭオープンソースライブラリを用いて、ＩＭＵ事前積分ファクタ、レーザ点群、北斗測位情報、視覚点群を制約ファクタに加え、ポーズのリアルタイム更新と正確な地図作成を実現する。

【0023】

本開示の付加側面の利点は、一部が以下の説明に提供され、他の一部が以下の説明により明らかになり、或いは、本開示の実践により理解される。

【0024】

本開示の一部を構成する添付図面は、本開示に対するさらなる理解を提供するためのものであり、本開示の模式的な実施例及びその説明は、本開示を解釈するためのものであり、本開示を不適切に限定するものではない。

【図面の簡単な説明】

【0025】

【図1】本開示の実施例１により提供されるＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法の模式的フローチャートである。

【図2】本開示の実施例１により提供されるレーザ点群と視覚点群の融合方法の模式図である。

【図3】本開示の実施例１により提供される地面点除去のための幾何学関係の模式図である。

【図4】本開示の実施例１により提供されるクラスタアルゴリズムによるノイズ除去方法の模式図である。

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

【0026】

以下、添付図面と実施例を参照しながら本開示をさらに説明する。

【0027】

なお、以下の詳細な説明は、いずれも例示的なものであり、本開示に対するさらなる説明の提供を意図する。特に断らない限り、本明細書に用いられる全ての技術及び科学用語は、本開示の属する技術分野の技術者が通常理解する意味と同じ意味を持つ。

【0028】

注意すべきこととして、ここで使用される用語は、単に具体的な実施形態を説明するためのものであり、本開示による示例的な実施形態に対する制限を意図しない。ここで使用される場合、文脈において特に断らない限り、単数形の表現に複数形も含まれることを意図する。また、理解すべきこととして、本明細書において、用語「含有」及び／又は「含む」が使用される場合、特徴、工程、操作、機構、部材及び／又はそれらの組合せが存在することを明示する。

【0029】

矛盾しない限り、本開示における実施例、及び実施例における特徴は、互いに組み合わせることができる。

【実施例1】

【0030】

図１に示すように、本開示の実施例１は、次のステップを含むＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法を提供する。

【0031】

Ｓ１において、タイムスタンプによって、レーザ点群と視覚点群の同期化及び校正を行い、新たなデータ構造を定義し、点群データの融合を実現する。

【0032】

Ｓ２において、マルチビームレーザレーダとＩＭＵ融合により、レーザ点群に対して運動歪除去を行い、鉱山シーンについて、点群フィルタリング処理を行う。

【0033】

Ｓ３において、グラフ最適化に基づく多重制約ファクタグラフアルゴリズムを用いて、ＩＭＵ、レーザレーダ、ＧＮＳＳ等の制約情報を制約ファクタグラフに加え、バックエンドループクロージャ検出及び地図作成を実現する。

【0034】

Ｓ１は、主に次の内容を含む。

【0035】

図２に示すように、タイムスタンプによって、レーザ点群と視覚点群の同期化及び校正を行い、新たなデータ構造を定義し、点群データの融合を実現する。

【0036】

ＧＮＳＳタイミングシステムにより、レーザレーダとカメラに対して、時間ハード同期化を行い、校正用ボードを固定することにより、レーザレーダとカメラに対して空間校正を行い、外部パラメータ行列を取得した後に、カメラをレーザレーダ座標系に投影させる。ＰＣＬ点群ライブラリを用いて、新たな点群データ構造を次のように定義する。
ＰｏｉｎｔＣｌｏｕｄ：ＸＹＺＲＩＲＧＢ（レーザ点群のデータ構造はＰｏｉｎｔＣｌｏｕｄ：ＸＹＺＲＩである）。

【0037】

Ｘはレーザ点群のｘ軸座標であり、Ｙはレーザ点群のｙ軸座標であり、Ｚはレーザ点群のｚ軸座標であり、Ｉはレーザ点群の光度であり、Ｒはレーザビームそれぞれのビーム番号であり、ＲＧＢは視覚点群の色情報である。

【0038】

レーザレーダ及びカメラの時間・空間校正が完成すると、フレームごとのレーザデータ、フレームごとの視覚カメラデータを容器に保持し、タイムスタンプ内挿アルゴリズムを用いて、マッチングされたレーザ点群と視覚点群を取得する。キーホールイメージング法により、レーザ点群をカメラ平面に投影させる。ＸＹＺ座標に従って、カメラ平面に投影したレーザ点群と視覚点群をマッチングさせ、融合後の点群を取得する。この融合後の点群において、レーザ点群データがＸＹＺＲＩとされ、視覚点群がＲＧＢ情報とされ、ＲＧＢ情報は、後続の地図作成のみに用いられるため、レーザ点群として扱うことができる。カメラの作動メカニズムにより、色付き点群は、レーザ点群全体においてほんの一部であるため、以降、依然としてレーザ点群と総称する。

【0039】

Ｓ２は、主に次の内容を含む。

【0040】

１フレームのレーザの走査開始時刻を時刻Ｔとし、該フレームのレーザの走査終了時刻を時刻Ｔ＋ΔＴとし、ＩＭＵ３軸の角速度、加速度データに対して事前積分を行い、期間ΔＴ内のロボットの相対運動（直線運動と非直線運動を含む）を求めた後、期間ΔＴ内の全てのレーザ点を１つ目のレーザ点に変換することにより、レーザ点群運動歪除去を実現する。この方法によって、レーザ点群の運動歪を有効に除去することができる。

【0041】

ＰＣＬライブラリにより、フレームごとのレーザ点群を深度画像として投影させる。マルチビームレーザレーダの走査ビームをｒビームとし、フレームごとの各ビームによる走査でレーザ点がｎ個得られるとする。この場合、深度画像はｒ×ｎのマトリックスとなり、各マトリックスに点群の座標情報、光度情報が含まれる。深度画像によって、隣り合う点群間の位置関係を得ることができ、また、ｋｄ－ｔｒｅｅ探索アルゴリズムを用いて、点群の隣接点を探索する。点群間の幾何学関係により、地面点及びノイズに対してフィルタリング処理を行う。

【0042】

地面点除去のための幾何学関係について、図３に示すように、座標系の中心がレーザレーダの幾何学中心にある。

【0043】

【数1】

但し、ＯＡ、ＯＢは、隣り合う２つのレーザビームの同一時刻におけるレーザ点が地面に到達する距離であり、α、βは、２つのレーザビームと水平面との夾角である。鉱山の地面が不平坦であるため、αが２．５°よりも小さく、且つ、隣り合う２つの点の高さの差、即ちΔ_Ｚが５ｃｍよりも小さい場合、地面点と見なす。各角度α及びΔ_Ｚに対し計算を行い、点群が地面点として算出された場合、地面点をフィルタリングにより除去する。

【0044】

関連ノイズ除去のための幾何学関係は、図４に示される。

【0045】

【数2】

但し、ＯＡ、ＯＢは、２つのレーザビームの深さであり、鉱山シーンでは埃が多いことを考え、αの閾値をＡとし、α＞Ａである場合、はずれ値と見なす。α＜Ａである場合、同一物体と見なし、深度画像の行、列でそれぞれ計算する。同一物体と見なされたレーザ点の数が３０を超えた場合、同一物体と見なす。

【0046】

Ｓ３は、主に次の内容を含む。

【0047】

グラフ最適化に基づく多重制約ファクタグラフアルゴリズムを用いて、ＩＭＵ、レーザ点群、ＧＮＳＳ等の制約情報を制約ファクタグラフに加え、バックエンドループクロージャ検出及び地図作成を実現する。

【0048】

ｓｃａｎ－ｔｏ－ｍａｐにより、現在レーザフレームの特徴情報と地図の特徴情報を用いて繰返し最適化を行い、全てのキーフレームポーズを更新する。

【0049】

ｇｔｓａｍオープンソースライブラリを用いて、ＩＭＵ事前積分データ、キーフレームデータ、ＧＮＳＳデータをグラフ最適化アルゴリズムのファクタとしてファクタグラフに加え、ファクタグラフ最適化を実行することにより、全てのキーフレームポーズを更新する。

【0050】

過去の約２０ｓの過去キーフレームのうち、距離が近く、時間間隔が長いフレームをキーフレームとして選択し、現在フレームとキーフレーム付近の複数フレームの点群のマッチングを行うことにより、変換ポーズを取得し、閉ループファクタグラフデータを構築し、ファクタグラフに加えて最適化を行う。

【0051】

ｇｔｓａｍオープンソースライブラリを用いて、最終の最適化したポーズを得るとともに、３次元点群の構築を実現する。

【0052】

融合後の点群は、レーザ点群の測定データが正確であるという特徴を有しながら、視覚点群の色・テクスチャが充実であるという特徴を有し、最終的に、色情報を有する３次元再構成地図を得ることができる。

【実施例2】

【0053】

本開示の実施例２は、
同期校正された、モバイルロボットによって測定されたレーザ点群データと視覚点群データを取得するように配置されたデータ取得モジュールと、
取得されたレーザ点群データと視覚点群データを融合させるように配置された点群融合モジュールと、
融合後の点群データに対して、点群運動歪除去処理及び点群フィルタリング処理を行うように配置された点群データ処理モジュールと、
処理後の点群データに基づき、グラフ最適化に基づく多重制約ファクタグラフアルゴリズムを用いて、ＩＭＵ事前積分データ、点群キーフレームデータ及びＧＮＳＳデータを制約ファクタグラフに加え、ループクロージャ検出を行うことにより、再構成された３次元地図を得るように配置された３次元地図再構成モジュールと、
を備える、ＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成システムを提供する。

【0054】

前記システムの動作方法は、実施例１により提供されるＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法と同様であるため、ここで繰り返して述べない。

【実施例3】

【0055】

本開示の実施例３は、プログラムが記憶されたコンピュータ可読記憶媒体であって、該プログラムがプロセッサによって実行されると、本開示の実施例１に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法におけるステップを実現する、コンピュータ可読記憶媒体を提供する。

【実施例4】

【0056】

本開示の実施例４は、メモリと、プロセッサと、メモリに記憶され、プロセッサで実行可能なプログラムとを含む電子デバイスであって、前記プログラムが前記プロセッサによって実行されると、本開示の実施例１に記載のＳＬＡＭに基づくモバイルロボットによる鉱山のシーン再構成方法におけるステップを実現する、電子デバイスを提供する。

【0057】

当業者であれば理解できるが、本開示の実施例は、方法、システム又はコンピュータプログラム製品として提供することができる。したがって、本開示は、ハードウェア実施例、ソフトウェア実施例、又は、ソフトウェアとハードウェアを組み合わせた実施例としてもよい。それに、本開示は、コンピュータ利用可能なプログラムコードを含む１又は複数のコンピュータ利用可能な記憶媒体（磁気ディスクメモリ及び光学メモリ等を含むが、これらに限られない）で実施されるコンピュータプログラム製品としてもよい。

【0058】

本開示は、本開示の実施例による方法、デバイス（システム）、及び、コンピュータプログラム製品のフローチャート及び／又はブロック図を参照しながら説明された。なお、フローチャート及び／又はブロック図における各工程及び／又はブロック、並びに、フローチャート及び／又はブロック図における工程及び／又はブロックの組合せは、コンピュータプログラムコマンドにより実現してもよい。これらのコンピュータプログラムコマンドを、汎用コンピュータや専用コンピュータ、埋込式プロセッサ、他のプログラマブルデータ処理デバイスのプロセッサに提供し、１つの機器を生成することにより、コンピュータや他のプログラマブルデータ処理デバイスのプロセッサで実行されるコマンドによって、フローチャートの１又は複数の工程、及び／又は、ブロック図の１又は複数のブロックで指定される機能を実現するための装置を生成することができる。

【0059】

これらのコンピュータプログラムコマンドは、特定な動作をするようにコンピュータや他のプログラマブルデータ処理デバイスをガイドできるコンピュータ可読メモリに記憶されてもよい。これにより、該コンピュータ可読メモリに記憶されたコマンドによって、フローチャートの１又は複数の工程、及び／又は、ブロック図の１又は複数のブロックで指定される機能を実現するためのコマンド装置を含む製品を生成する。

【0060】

これらのコンピュータプログラムコマンドは、コンピュータや他のプログラマブルデバイスで一連の操作ステップを実行して、コンピュータによる処理を生成するように、コンピュータや他のプログラマブルデータ処理デバイスにインストールされてもよい。これにより、コンピュータや他のプログラマブルデバイスで実行されるコマンドは、フローチャートの１又は複数の工程、及び／又は、ブロック図の１又は複数のブロックで指定される機能を実現するためのステップを提供する。

【0061】

当業者であれば理解できるが、上記実施例の方法における全て又は一部の工程は、コンピュータプログラムによって関連するハードウェアに指示することで完成されてもよい。前記のプログラムは、コンピュータ可読取記憶媒体に記憶されてもよい。該プログラムは、実行時に、上記各方法の実施例の工程を含んでもよい。前記の記憶媒体は、磁気ディスクや光ディスク、リードオンリーメモリ（Ｒｅａｄ－Ｏｎｌｙ Ｍｅｍｏｒｙ，ＲＯＭ）、ランダムアクセスメモリ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ，ＲＡＭ）等であってもよい。

【0062】

以上、本開示の好適な実施例を説明したが、これらは本開示を制限するためのものではない。当業者にとって、本開示は、種々の変更と変形が可能である。本開示の精神及び原則から逸脱せずになされるあらゆる変更、等価置換、改良等は、いずれも本開示の保護範囲内に含まれるべきである。

【図1】

【図2】

【図3】

【図4】

【国際調査報告】