JP7375206B2

JP7375206B2 - 局所点群地図の構築方法及び視覚ロボット

Info

Publication number: JP7375206B2
Application number: JP2022540796A
Authority: JP
Inventors: ヤン，ヨンシアン; ライ，チンウェイ; シャオ，ガンジュン
Original assignee: Zhuhai Amicro Semiconductor Co Ltd
Current assignee: Zhuhai Amicro Semiconductor Co Ltd
Priority date: 2020-10-30
Filing date: 2021-06-03
Publication date: 2023-11-07
Anticipated expiration: 2041-06-03
Also published as: WO2022088680A1; EP4083921A4; US20230249353A1; EP4083921A1; JP2023509153A; CN112348893B; KR20220113977A; CN112348893A

Description

本発明は局所点群地図の技術分野に関し、特に点群データに基づいた局所点群地図の構築方法及び視覚ロボットに関する。

３ｄ－ｔｏｆ技術は、三次元点群地図を有効範囲内に復元することができるが、現在のところ、３ｄ－ｔｏｆカメラは、３ｄ－ｔｏｆが比較的正確な奥行き値を完全に取得できないため、誤差が一般的に大き過ぎるという問題があり、具体的には、障害物の距離が近過ぎたり遠過ぎたりすると、奥行き値に明らかな誤差があり、単一フレームに基づく奥行きデータ分析が超近距離での障害物を効果的に識別することができず、従って障害物の測位を完了するためにより正確な点群地図が必要である。

中国特許ＣＮ１１００３１８２５Ａに開示されているレーザ測位初期化方法は、連続的に取得されたマルチフレームのレーザ点群を接合することにより点群をサンプリングして三次元座標系を構築し、リアルタイムなヒストグラムの形でサンプリングされた点群情報を記憶し、しかしながら、このレーザ測位初期化方法は複数の象限内にリアルタイムな三次元ヒストグラムを作成するように、大量の特徴行列演算を呼び出し、複数の座標系を構築する必要があり、点群データに対応する特徴マッチング演算は複雑すぎて、簡略化された局所点群地図の構築に寄与せず、そして、上記近過ぎる障害物の測位と識別の問題を依然として解決できない。

より簡略化された局所点群地図を構築し、３ｄ－ｔｏｆカメラにより収集された点群データにノイズ誤差があるという問題を克服し、局所点群地図を使用して現在位置の近過ぎる障害物をリアルタイムで正確に測位して識別するために、本発明は局所点群地図の構築方法を開示し、具体的な技術的解決手段は以下のとおりである。

局所点群地図の構築方法であって、該局所点群地図の構築方法は、移動ロボットの移動中、移動ロボットの３ｄ－ｔｏｆカメラを制御して奥行き画像を収集し続け、３ｄ－ｔｏｆカメラによりリアルタイムに収集された現在フレームの奥行き画像を、所定の顕著な位置姿勢変化条件に基づいてキーフレームシーケンス内に挿入し、次にステップ２に進むステップ１と、キーフレームシーケンス内の奥行き画像の所定の有効特徴点が位置姿勢変換を行い、カメラ座標系における点群になるように制御し、次にステップ３に進むステップ２と、移動ロボットの３ｄ－ｔｏｆカメラのリアルタイムな視野角範囲内に、水平地盤に位置する１つの所定のターゲット平面領域を設定し、この所定のターゲット平面領域にそれにマッチングする検出ブロックを設定し、点群を所定のターゲット平面領域の真上に三次元ヒストグラムの形でラベルングし、次にステップ４に進むステップ３と、各検出ブロックにおける点群の高さ位置の分布特徴に基づいて、先ず対応する検出ブロック内の離散分布状態の点群を削除し、次に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除し、次にステップ５に進むステップ４と、各検出ブロック内に分布する残りの点群の位置を局所点群地図の三次元位置としてラベルングし、局所点群地図の構築を実現するステップ５と、を含む。

従来技術に比べて、該技術的解決手段は、ロボット周辺の広範囲の領域の位置を反映する点群を、所定の顕著な位置姿勢変化条件に基づいてサンプリングし、移動ロボットにこれらのキーフレームを利用して移動ロボットの前方の障害物の分布状況を完全にカバーできる局所点群地図を作成させ、次に三次元ヒストグラムの形で異なる高さ位置の点群を説明し、異なる検出ブロックにおける点群の三次元領域分布特徴を利用して漂遊点及び現在位置で収集視線が遮断される点群を除去するのに便利であり、近過ぎる又は遠過ぎる障害物シーンでの局所点群地図の適応性が向上され、超近距離での障害物の位置の識別測位の難しさ及び誤判定の確率が顕著に低減される。

さらに、３ｄ－ｔｏｆカメラによりリアルタイムに収集された現在フレームの奥行き画像を、所定の顕著な位置姿勢変化条件に基づいてキーフレームシーケンス内に挿入する前記方法は具体的に、移動ロボットの３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像中の所定の有効特徴点と、前記キーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点との相対位置姿勢をそれぞれ計算するステップであって、キーフレームシーケンスは移動ロボットが予め記憶した、所定の有効特徴点を含む連続的に配列された奥行き画像のシーケンスである、ステップと、計算されたすべての相対位置姿勢が所定の顕著な位置姿勢変化条件を満たす場合、３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像を新たなキーフレームとして構成し、次にこの新たなキーフレームを前記キーフレームシーケンスに挿入し、次回上記相対位置姿勢計算に参与するキーフレームが移動ロボットの最新の相対位置関係を反映するようにするステップとを含み、所定の有効特徴点は前記３ｄ－ｔｏｆカメラの視野角範囲内での移動ロボットの前方の障害物の分布位置特徴を特徴づけるためのものであり、障害物の分布位置特徴は障害物の存在及び／又は障害物の不在を含む。

従来技術に比べて、移動ロボットにこれらのキーフレームを利用して移動ロボットの前方の障害物の分布状況を広範囲にカバーできる局所点群地図を作成させ、これにより、現在フレームの奥行き画像と更新をサポートするこれらのキーフレームとの相対位置姿勢を融合処理することによって、ロボットがリアルタイムに収集された単一フレームの奥行き画像に基づき機体のエッジ位置が近接している障害物又は機体に近過ぎる障害物を識別し測位しにくいという問題を解決する。

さらに、前記キーフレームシーケンス内の各キーフレーム中の所定の有効特徴点と、それに対応するキーフレームでの３ｄ－ｔｏｆカメラとの距離及び偏向角はすべて平行移動及び回転の方式で前記現在フレームの奥行き画像に対応して属するカメラ座標系に変換され、前記位置姿勢変換が実現され、前記局所点群地図の地図座標系は現在フレームの奥行き画像に対応するカメラ座標系である。より正確な局所点群地図の構築に有利であり、移動ロボットのリアルタイムの測位とナビゲーションが容易になる。

さらに、前記した計算されたすべての相対位置姿勢が所定の顕著な位置姿勢変化条件を満たすことについて、具体的に、計算された相対位置姿勢は、移動ロボットの３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像中の所定の有効特徴点のそれぞれと、前記位置姿勢変換が発生した後のキーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点との間の、奥行き距離の変化量及びその対応する偏向角の変化量を含み、あらゆる奥行き距離の変化量がすべて所定の距離変化閾値よりも大きく、且つあらゆる偏向角の変化量がすべて所定の角度変化閾値よりも大きい場合、前記所定の顕著な位置姿勢変化条件を満たすと決定する。

従来技術に比べて、該技術的解決手段は、２フレームの奥行き画像の間の所定の有効特徴点に対応する実際の位置点（固定されていると見なされる）と３ｄ－ｔｏｆカメラとの距離の変化量が十分に大きいであるかどうかを判断し、２フレームの奥行き画像の間の所定の有効特徴点に対応する実際の位置点（固定されていると見なされる）の、３ｄ－ｔｏｆカメラに対する偏向角の変化量が十分に大きいであるかどうかを判断することによって、機体の変位が十分に大きいであるかどうか又は機体の回転角度が十分に大きいであるかどうかを特徴づけ、これにより、新たなキーフレームとして構成される奥行き画像と現在フレームの奥行き画像とのギャップが十分に大きくなり、明確で広域のロボット周辺情報を提供することができ、属するキーフレームシーケンス内の奥行き画像のカバーされる可視範囲が十分に大きくなり（例えば、右から見る又は左から見る角度、又は、近距離から見る又は遠距離から見る変位であり、これらの角度間、位置間のロボットの位置姿勢変換が十分に大きい必要がある）、キーフレームシーケンス内の奥行き画像には、障害物を含むキーフレームと障害物を含まないキーフレームの両方が存在する。

さらに、前記キーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点と、それに対応するキーフレームでの３ｄ－ｔｏｆカメラとの距離は、前記３ｄ－ｔｏｆカメラにより予め収集され前記キーフレームシーケンスに挿入された奥行き画像の所定の有効特徴点に対応する実際の位置点から３ｄ－ｔｏｆカメラの結像平面までの奥行き距離であり、この奥行き距離の変化量は前記移動ロボットの変位の大きさ程度を表すことに用いられ、前記キーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点の、その対応するキーフレームでの３ｄ－ｔｏｆカメラに対する偏向角は、前記３ｄ－ｔｏｆカメラが奥行き画像を収集し、且つ前記キーフレームシーケンスに挿入する時のレンズ向き角度であり、このレンズ向き角度の変化量は前記移動ロボットの回転角度を表すことに用いられる。

従来技術に比べて、該技術的解決手段によれば、移動ロボットの相対位置姿勢が比較的大きいことを表すマルチフレームのキーフレームを容易に選別することができ、特に移動ロボットの機体が大きな角度で回転し又は大きく変位した場合、３ｄ－ｔｏｆカメラが近過ぎる障害物を識別できないと、予め記憶されたキーフレームシーケンス中の関連する有効特徴点（すなわち、予め保存された障害物を含むキーフレーム）を利用してこの近過ぎる障害物の正確な位置を測位する。

さらに、前記ステップ１は、移動ロボットの３ｄ－ｔｏｆカメラが現在フレームの奥行き画像を現在収集した時、移動ロボットの内部に設置された慣性センサを呼び出して移動ロボットの現在の位置姿勢を測定するステップと、次に予め保存されたキーフレームシーケンス内の各キーフレームに対応する移動ロボットのリアルタイムな位置姿勢を呼び出し、移動ロボットの現在の位置姿勢と、各キーフレームに対応する移動ロボットのリアルタイムな位置姿勢との間の相対位置姿勢をそれぞれ計算するステップとを含む。従来技術に比べて、本技術的解決手段は、機体内部の慣性センサを利用して、現在フレームの奥行き画像の有効収集位置と各キーフレームの有効収集位置との間の相対位置姿勢を計算し、これにより、前記移動ロボットにより収集された所定の有効特徴点の間の位置変化量を直接求め、ロボットの測位計算の簡略化に有利であり、移動ロボットのリアルタイムの測位とナビゲーションが容易になる。

さらに、前記した計算されたすべての相対位置姿勢が所定の顕著な位置姿勢変化条件を満たすことについて、具体的に、計算された相対位置姿勢は、移動ロボットの３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像中の所定の有効特徴点のそれぞれと、予め収集された前記キーフレーム中の同じ又は同じタイプの所定の有効特徴点との間の発生する移動ロボットの走行距離の変化量及び同じ収集過程で発生する偏向角の変化量を含み、あらゆる走行距離の変化量がすべて所定の距離変化閾値よりも大きく、且つあらゆる偏向角の変化量がすべて所定の角度変化閾値よりも大きい場合、前記所定の顕著な位置姿勢変化条件を満たすと決定し、前記所定の有効特徴点は前記３ｄ－ｔｏｆカメラの視野角範囲内での移動ロボットの前方の障害物の分布位置特徴を特徴づけるためのものであり、障害物の分布位置特徴は障害物の存在及び／又は障害物の不在を含む。

従来技術に比べて、該技術的解決手段は、２フレームの奥行き画像の対応する実際の収集位置点の変化量が十分に大きいであるかどうかを判断し、２フレームの奥行き画像の対応する実際の機体偏向角の変化量が十分に大きいであるかどうかを判断することによって、機体の変位が十分に大きいであるかどうか又は機体の回転角度が十分に大きいであるかどうかを特徴づけ、これにより、新たなキーフレームとして構成される奥行き画像と現在フレームの奥行き画像とのギャップが十分に大きくなり、明確で広域のロボット周辺情報を提供することができ、属するキーフレームシーケンス内の奥行き画像のカバーされる可視範囲が十分に大きくなり（例えば、右から見る又は左から見る角度、又は、近距離から見る又は遠距離から見る変位であり、これらの角度間、位置間のロボットの位置姿勢変換が十分に大きい必要がある）、キーフレームシーケンス内の奥行き画像には、障害物を含むキーフレームと障害物を含まないキーフレームの両方が存在する。

さらに、前記慣性センサは移動ロボットの機体中心に内蔵されるエンコーダディスク及び移動ロボットの機体中心に内蔵されるジャイロスコープを含み、エンコーダディスクは移動ロボットの走行距離の変化量を測定することに用いられ、ジャイロスコープは同じ収集過程での移動ロボットの偏向角の変化量を測定することに用いられ、エンコーダディスクにより測定された移動ロボットの位置座標は前記移動ロボットの機体中心と３ｄ－ｔｏｆカメラとの剛性接続関係によって前記現在フレームの奥行き画像が対応して属するカメラ座標系に変換され、局所点群地図が構築される。

さらに、前記キーフレームシーケンス内に３フレーム又は３フレーム以上の奥行き画像が存在する。前記局所点群地図を構築することに用いられる。該技術的解決手段は、局所点群地図を構築し有効特徴点によって障害物を測位することにより十分な点群サンプルを提供することができ、局所点群地図はより安定し有効的になる。

さらに、前記移動ロボットは先入れ先出しのメモリー記憶の形で前記キーフレームシーケンスをキャッシュし、それにより前記移動ロボットは、３ｄ－ｔｏｆカメラにより現在リアルタイムに収集された１フレームの奥行き画像に時間的に最も近い前記キーフレームを移動中に取得する。記憶空間の最適化を実現するために、該技術的解決手段は先入れ先出しの方式でキューを記憶し最も新しく選別して挿入されたキーフレームを記憶し、最も有効的なフレームシーケンスしか記憶しないことを確保し、これは現在フレームに最も近い３フレームの奥行き画像であってもよい。

さらに、前記所定のターゲット平面領域は長方形領域であり、長方形領域の一辺の中点は３ｄ－ｔｏｆカメラの位置であり、この辺の長さが前記移動ロボットの機体直径に等しく、該辺に垂直な一辺は３ｄ－ｔｏｆカメラの視野角の前方に設置され、前記３ｄ－ｔｏｆカメラにより収集された奥行き画像中の検出可能な奥行き距離を特徴づけることに用いられる。それにより、前記所定のターゲット平面領域は、前記移動ロボットがスムーズに通過できるターゲット検出領域を収容できるように構成され、上記ステップ２におけるキーフレームの位置姿勢変換されたカメラ座標系での点群と組み合わせて、この領域の上方の障害物点群の最大の高さ位置及び分布特徴を測位し、近距離条件で障害物を回避するルートを効果的に計画する。

さらに、前記ステップ４において、１つの前記点群の三次元座標位置がそのうち１つの前記検出ブロック内にあることが検出された場合、この前記点群がこの前記検出ブロック内に分布すると決定し、この点群の高さを記録する。それにより点群の位置情報の記憶と適用の方式を簡略化する。

さらに、各点群を三次元ヒストグラムの形で記憶する前記方法は、前記三次元ヒストグラムが属する三次元座標系の座標原点が３ｄ－ｔｏｆカメラの位置であるように設定し、前記所定のターゲット平面領域がこの三次元座標系の横軸の正方向の前記機体直径の半分の距離をカバーするように設定し、前記所定のターゲット平面領域がこの三次元座標系の横軸の負方向の前記機体直径の半分の距離をカバーするように設定し、前記所定のターゲット平面領域がこの三次元座標系の縦軸の正方向の前記検出可能な奥行き距離をカバーするように設定し、前記検出ブロックがこの三次元座標系の垂直軸に沿って分布するように設定し、この三次元座標系の垂直軸で前記検出ブロックが占有する垂直高さを構成して前記検出ブロックに分布する点群の高さを表し、前記点群を分割して前記所定数の三次元ヒストグラムを取得し、三次元ヒストグラムの形で前記点群を記憶することを実現するステップを含み、前記三次元ヒストグラムが属する三次元座標系は前記局所地図の座標系と同じである。それにより、ヒストグラム統計の方式で局所地図の点群は局所地図の座標系における特定の検出領域上の三次元の点状地図に変換され、三次元点状地図に分布する長方形のブロックの高さを利用して障害物の点群の高さを特徴づけることが実現され、漂遊点群を顕著で迅速に選別し除去することに便利である。

さらに、前記所定のターゲット平面領域の面積と前記検出ブロックの水平投影面積との比率は所定の整数であり、それにより所定のターゲット平面領域は所定の整数の検出ブロックによって完全に均等に分割されて占有される。前記検出ブロックによる前記所定のターゲット平面領域の点群カバー率が向上される。

さらに、前記ステップ４において、対応する検出ブロック内の離散分布状態の点群を削除する前記方法は、具体的に、前記検出ブロック内に分布する点群の数が、前記離散分布状態を説明するための測位点数閾値未満であるかどうかを判断し、ＹＥＳの場合にこの検出ブロックに分布する点群を削除し、次に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除し、ＮＯの場合に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を直接削除するステップ、及び／又は、１つの検出ブロックの近傍領域に点群が分布していないかどうかを判断し、ＹＥＳの場合にこの検出ブロック内に分布する点群を削除し、次に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除し、ＮＯの場合に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を直接削除するステップを含む。従来技術に比べて、あまりに離散的に分布する点群による障害物の識別測位へのノイズ干渉、特に遠過ぎる障害物による測位誤差の問題を減少し、さらに近過ぎる障害物の識別の精度を向上させる。

さらに、前記ステップ４において、収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除する前記方法は、３ｄ－ｔｏｆカメラから離れる方向に沿って各列の検出ブロックに分布する点群の高さを検出し、各列の検出ブロック内に低から高へ変化する検出ブロックシーケンスが存在するかどうかをそれぞれ判断し、ＹＥＳの場合にステップ４２に進み、ＮＯの場合にステップ４３に進むステップ４１であって、低から高への変化は、判断に参与する各検出ブロック中に分布する最大の高さに対応する点群の高さ位置の変化であり、各検出ブロックに分布する点群の最大の高さは各検出ブロックの高さを表す、ステップ４１と、この検出ブロックシーケンス中の高さが最も高い点群が属する検出ブロックを記録し、その後この記録された検出ブロックの後方の高さが比較的低い検出ブロックに分布する点群を削除し、次にステップ４１に戻るステップ４２と、この検出ブロックシーケンス中の高さが最も高い点群が属する検出ブロックを記録し、その後この記録された検出ブロックの後方の検出ブロックに分布する点群を削除するステップ４３とを含む。

従来技術に比べて、検出ブロックに分布する点群の削除は高さが最も高い点群に基づいて行われ、具体的に、実際の物理的環境における山形に応じて、山頂の高さ位置に持ち上げられる前の点群を保持し、山の裏側の収集視線が遮断される低い点群を削除するように選択し、それにより、移動ロボットの現在位置に顕著な視線遮断効果を有する障害物を保持し、所定のターゲット平面領域の対応する検出ブロックから、実際の高さ位置の遮断により実際のシーンの障害物測位に適用できない点群を削除し、奥行き画像における予め収集された低い点群位置による誤判定の問題を克服し、近過ぎる障害物の位置の誤判定現象を顕著に減少する。

さらに、前記キーフレームシーケンス内のキーフレーム上の所定の有効特徴点は、非露出過度点、非露出不足点、非遠過ぎ点、非近過ぎ点及び最大垂直高さが２０mmよりも大きい画素点を含み、非遠過ぎ点の奥行き値と非近過ぎ点の奥行き値の両方は２００mm～５００mmである。従来技術に比べて、信頼できない特徴点の影響は除外される。

さらに、前記所定の有効特徴点は、前記障害物を特徴づけるための特徴点、又は前記障害物以外の標準的な道路標識を特徴づけるための特徴点であり、前記３ｄ－ｔｏｆカメラは移動ロボットの移動中に、前記所定の有効特徴点の異なるフレームの奥行き画像を異なる方向から収集する。それにより、３ｄ－ｔｏｆカメラの視野角範囲内の移動ロボットの前方の障害物の分布位置特徴を特徴づける。

さらに、１つの検出ブロックの８つの近傍領域に点群が分布していないかどうかを判断し、ＹＥＳの場合にこの検出ブロック内に分布する点群を削除する。それにより離散点を削除し、近距離での障害物の効果的な識別を実現する。

視覚ロボットであって、３ｄ－ｔｏｆカメラと、前記局所点群地図の構築方法を実行するための処理ユニットとを含み、３ｄ－ｔｏｆカメラは、その視野角が前記視覚ロボットの前方をカバーできるように視覚ロボットの機体に組み立てられる。それにより、該３ｄ－ｔｏｆカメラは移動ロボットの移動中に異なる視野角でターゲット障害物の点群をサンプリングし、次に該視覚ロボットが特定の位置に移動する時、３ｄ－ｔｏｆカメラの現在の視野角範囲内の所定のターゲット平面領域の点群の高さ位置の分布特徴を利用して、漂遊点を除去し、正確な測位のための最大の高さ及び合理的に分布する点群しか局所地図に保持せず、向きを変える過程で、該視覚ロボットは、それが向きを変える前に視野角範囲内で識別した機体のエッジ位置に近い障害物又は機体に近過ぎる障害物を再識別することができ、近過ぎる又は遠過ぎる障害物シーンでの測位適応性が向上され、超近距離での障害物位置の誤判定の確率が顕著に低減される。

本発明の一実施例に開示される局所点群地図の構築方法のフローチャートである。

以下、本発明の実施例における図面を参照しながら、本発明の実施例における技術的解決手段を詳細に説明する。

本発明の実施例は局所点群地図の構築方法を開示し、図１に示すように、該局所点群地図の構築方法はステップ１～ステップ５を含む。

ステップ１、移動ロボットの移動中、移動ロボットの３ｄ－ｔｏｆカメラを制御して奥行き画像を収集し続け、３ｄ－ｔｏｆカメラによりリアルタイムに収集された現在フレームの奥行き画像を、所定の顕著な位置姿勢変化条件に基づいてキーフレームシーケンス内に挿入し、次にステップ２に進む。

なお、移動ロボットの走行規則は主に、エッジに沿った形及び弓字形の２つがあり、ここで、弓字形に応じる走行は、空き領域の清掃を実現することに用いられ、前記移動ロボットは弓字形に応じて走行する過程で様々な障害物に遭い、弓字形ルートに沿って９０°向きを変える前に距離が非常に近い障害物を検出されると、９０°向きを変える過程で同じ障害物が検出されなくなり、計画された弓字形ルートは障害物の方位に基づき比較的大きい角度を選択して回転し、掃除ロボットは障害物から離れる方向へ回転し、障害物を回避できるからである。

前記移動ロボットは、自動掃除機、スマート掃除機、ロボット掃除機等と呼ばれる掃除ロボットであってもよく、スマート家電の一種であり、特定の人工知能を利用して、部屋内に弓字形の計画清掃作業を自動的に完了することができる。

一実施例として、ステップ１において、３ｄ－ｔｏｆカメラによりリアルタイムに収集された現在フレームの奥行き画像を、所定の顕著な位置姿勢変化条件に基づいてキーフレームシーケンス内に挿入する前記方法は具体的に以下を含む。

移動ロボットの３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像中の所定の有効特徴点と、前記キーフレームシーケンス内の各キーフレーム（位置姿勢変換後のキーフレーム）中の同じ所定の有効特徴点との相対位置姿勢をそれぞれ計算し、キーフレームシーケンスは移動ロボットが予め記憶した、所定の有効特徴点を含む連続的に配列された奥行き画像のシーケンスであり、連続的な奥行き画像フレーム中の各フレームの機体の相対位置情報及びカメラの向き情報を記憶することに用いられ、トラバーサルによって識別された機体に近過ぎる又は機体に密着される障害物の奥行き画像情報を含み、これにより、その後このタイプの障害物を識別できない時、これらの関連する奥行き画像の参照座標情報を利用して測位を支援する。

ここで、所定の有効特徴点は前記３ｄ－ｔｏｆカメラの視野角範囲内での移動ロボットの前方の障害物の分布位置特徴を特徴づけるためのものであり、障害物の分布位置特徴は障害物の存在及び／又は障害物の不在を含む。

計算されたすべての相対位置姿勢が所定の顕著な位置姿勢変化条件を満たす場合、３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像を新たなキーフレームとして構成し、次にこの新たなキーフレームを前記キーフレームシーケンスに挿入し、次回上記相対位置姿勢計算に参与するキーフレームが移動ロボットの最新の相対位置関係を反映するようにし、該ステップ１は、移動ロボットの３ｄ－ｔｏｆカメラにより現在収集された奥行き画像と、キーフレームシーケンス内の各フレームの奥行き画像との間の相対位置姿勢の大きさ程度を判断し、その後機体の変位程度が非常に顕著な奥行き画像又は機体の回転程度が非常に顕著な奥行き画像を選別することに用いられる。

ステップ１で計算された相対位置姿勢に含まれる偏向角と相対距離の両方が比較的大きいと決定し、移動ロボットが大きく変位し又は回転角が大きく変化することを特徴づける時、３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像を新たなキーフレームとして構成し、次にこの新たなキーフレームを前記キーフレームシーケンスに挿入し、次回上記相対位置姿勢計算に参与するキーフレームが移動ロボットの最新の相対位置関係を反映するようにする。

前記した計算されたすべての相対位置姿勢が所定の顕著な位置姿勢変化条件を満たすことについて、具体的に、計算された相対位置姿勢は、移動ロボットの３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像中の所定の有効特徴点のそれぞれと、前記位置姿勢変換が発生した後のキーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点との間の、奥行き距離の変化量及びその対応する偏向角の変化量を含み、固定されている前記所定の有効特徴点に対する移動ロボットの変位変化量（前記奥行き距離の変化量に対応する）、及びこの前記所定の有効特徴点の周りの角度偏差量（前記偏向角の変化量に対応する）を表し、あらゆる奥行き距離の変化量がすべて所定の距離変化閾値よりも大きく、且つあらゆる偏向角の変化量がすべて所定の角度変化閾値よりも大きい場合、前記所定の顕著な位置姿勢変化条件を満たすと決定し、移動ロボットが大きく変位し又は回転角が大きく変化する（例えば、弓字形で計画される走行中に９０°向きを変える）ことを特徴づける。

従来技術に比べて、本実施例は、２フレームの奥行き画像の間の所定の有効特徴点に対応する実際の位置点（固定されていると見なされる）と３ｄ－ｔｏｆカメラとの距離の変化量が十分に大きいであるかどうかを判断し、２フレームの奥行き画像の間の所定の有効特徴点に対応する実際の位置点（固定されていると見なされる）の、３ｄ－ｔｏｆカメラに対する偏向角の変化量が十分に大きいであるかどうかを判断することによって、機体の変位が十分に大きいであるかどうか又は機体の回転角度が十分に大きいであるかどうかを特徴づけ、これにより、新たなキーフレームとして構成される奥行き画像と現在フレームの奥行き画像とのギャップが十分に大きくなり、明確で広域のロボット周辺情報を提供することができ、属するキーフレームシーケンス内の奥行き画像のカバーされる可視範囲が十分に大きくなり（例えば、右から見る又は左から見る角度、又は、近距離から見る又は遠距離から見る変位であり、これらの角度間、位置間のロボットの位置姿勢変換が十分に大きい必要がある）、キーフレームシーケンス内の奥行き画像には、障害物を含むキーフレームと障害物を含まないキーフレームの両方が存在し、構築された局所点群地図の情報を後続処理し障害物に対する正確な測位機能を実行することを支援する。

好ましくは、前記キーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点と、それに対応するキーフレームでの３ｄ－ｔｏｆカメラとの距離は、前記３ｄ－ｔｏｆカメラにより予め収集され前記キーフレームシーケンスに挿入された奥行き画像の所定の有効特徴点に対応する実際の位置点から３ｄ－ｔｏｆカメラの結像平面までの奥行き距離であり、この奥行き距離の変化量は前記移動ロボットの変位の大きさ程度を表すことに用いられ、前記キーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点の、その対応するキーフレームでの３ｄ－ｔｏｆカメラに対する偏向角は、前記３ｄ－ｔｏｆカメラが奥行き画像を収集し、且つ前記キーフレームシーケンスへの挿入を実現する時のレンズ向き角度であり、このレンズ向き角度の変化量は前記移動ロボットの回転角度を表すことに用いられる。

従来技術に比べて、本実施例は、所定の有効特徴点の位置姿勢情報を抽出し、且つフレーム同士のオフセット量の比較演算に参与し、移動ロボットの相対位置姿勢が比較的大きいことを表すマルチフレームのキーフレームを容易に選別することができ、特に移動ロボットの機体が大きな角度で回転し又は大きく変位した場合、３ｄ－ｔｏｆカメラが近過ぎる障害物を識別できない時、予め記憶されたキーフレームシーケンス中の関連する有効特徴点（すなわち、予め保存された障害物を含むキーフレーム）を利用して後続の点群削除操作と組み合わせることによりこの近過ぎる障害物の具体的な位置を測位する。

別の一実施例として、ステップ１において、３ｄ－ｔｏｆカメラによりリアルタイムに収集された現在フレームの奥行き画像を、所定の顕著な位置姿勢変化条件に基づいてキーフレームシーケンス内に挿入する前記方法のステップは、具体的にステップＳ１０１～ステップＳ１０６を含む。

ステップＳ１０１、移動ロボットは現在の作業領域内に予め計画された弓字形ルートに沿って移動し、移動中に、移動ロボットは３ｄ－ｔｏｆカメラを使用して現在の作業領域内の奥行き画像を収集し、障害物の奥行き画像を含み、次にステップＳ１０２に進む。

ステップＳ１０２、移動ロボットの３ｄ－ｔｏｆカメラが現在フレームの奥行き画像を現在収集した時、移動ロボットの内部に設置された慣性センサを呼び出して移動ロボットの現在の位置姿勢を測定して保存し、なお、３ｄ－ｔｏｆカメラにより収集された奥行き画像は機体の前方の有効特徴点を含み、非露出過度点、非露出不足点、非遠過ぎ点、非近過ぎ点及び最大垂直高さが２０mmよりも大きい画素点を含むと、該ステップを実行でき、次にステップＳ１０３に進む。

ステップＳ１０３、予め保存されたキーフレームシーケンス内の各キーフレームに対応する移動ロボットのリアルタイムな位置姿勢（収集位置）を呼び出し、移動ロボットの現在の位置姿勢と、各キーフレームに対応する移動ロボットのリアルタイムな位置姿勢（予め記憶される収集位置）との間の相対位置姿勢をそれぞれ計算する。

次に、ステップＳ１０４に進む。好ましくは、前記キーフレームシーケンスは移動ロボットが予め記憶した、所定の有効特徴点を含む連続的に配列された奥行き画像のシーケンスであり、連続的な奥行き画像フレーム中の各フレームの機体の相対位置情報及びカメラの向き情報を記憶することに用いられ、トラバーサルによって識別された機体に近過ぎる又は機体に密着される障害物の奥行き画像情報を含み、これにより、その後このタイプの障害物を識別できない時、これらの奥行き画像と同期収集された慣性情報を利用して測位を支援する。

所定の有効特徴点は前記３ｄ－ｔｏｆカメラの視野角範囲内での移動ロボットの前方の障害物の分布位置特徴を特徴づけるためのものであり、障害物の分布位置特徴は障害物の存在及び／又は障害物の不在を含む。

前記した計算されたすべての相対位置姿勢が所定の顕著な位置姿勢変化条件を満たすことについて、具体的に、計算された相対位置姿勢は、移動ロボットの３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像中の所定の有効特徴点のそれぞれと、予め収集された前記キーフレーム中の同じ又は同じタイプの所定の有効特徴点との間の、移動ロボットの走行距離の変化量及び同じ収集過程で発生する偏向角の変化量を含む。

あらゆる走行距離の変化量がすべて所定の距離変化閾値よりも大きく、且つあらゆる偏向角の変化量がすべて所定の角度変化閾値よりも大きい場合、前記所定の顕著な位置姿勢変化条件を満たすと決定する。

なお、前記慣性センサは移動ロボットの機体中心に内蔵されるエンコーダディスク及び移動ロボットの機体中心に内蔵されるジャイロスコープを含み、エンコーダディスクは移動ロボットの走行距離の変化量を測定することに用いられ、ジャイロスコープは同じ収集過程での移動ロボットの偏向角の変化量を測定することに用いられ、エンコーダディスクにより測定された移動ロボットの位置座標は前記移動ロボットの機体中心と３ｄ－ｔｏｆカメラとの剛性接続関係によって前記現在フレームの奥行き画像が対応して属するカメラ座標系に変換され、局所点群地図が構築され、それにより、移動ロボットが前記所定の有効特徴点を収集する時の移動軌跡を局所点群地図にラベルングし、本実施例は、慣性データを利用して移動ロボットの位置姿勢変化情報を直接取得するため、上記実施例の特徴点計算に対して、相対位置姿勢の計算量が簡略化される。

好ましくは、前記キーフレームシーケンス内の各キーフレーム中の同じターゲット障害物に関する特徴点は位置姿勢変換によって同じ局所点群地図に変換され、同じターゲット障害物をラベルングし異なるキーフレームでの移動ロボットの位置姿勢を取得することに用いられる。

ステップＳ１０４、ステップＳ１０３で計算されたすべての相対位置姿勢が所定の顕著な位置姿勢変化条件を満たすかどうかを判断し、ＹＥＳの場合にステップＳ１０５に進み、ＮＯの場合にステップＳ１０６に進む。該ステップは、現在フレームの奥行き画像の対応するロボット位置姿勢とキーフレームシーケンス内の各フレームの奥行き画像の対応するロボット位置姿勢との間の相対位置姿勢の大きさ程度を判断し、その後機体の変位程度が非常に顕著な奥行き画像又は機体の回転程度が非常に顕著な奥行き画像を選別することに用いられる。

ステップＳ１０５、ステップＳ１０３で計算された相対位置姿勢に含まれる偏向角と相対距離の両方が比較的大きいと決定し、移動ロボットが大きく変位し又は回転角が大きく変化することを特徴づける時、３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像を新たなキーフレームとして構成し、次にこの新たなキーフレームを前記キーフレームシーケンスに挿入し、次回上記相対位置姿勢計算に参与するキーフレームが移動ロボットの最新の相対位置関係を反映するようにし、次にステップＳ２０１に戻り、移動し続けて現在の作業領域内の奥行き画像を収集する。

ステップＳ１０６、ステップＳ１０３で計算された相対位置姿勢に含まれる偏向角と相対距離の両方が小さいと決定し、移動ロボットが少しだけ変位し又は回転角が大きく変化しないことを特徴づける時、３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像を新たなキーフレームとして構成せず、ステップＳ１０１に戻り、移動し続けて現在の作業領域内の奥行き画像を収集する。

上記ステップは、３ｄ－ｔｏｆカメラが現在フレームの奥行き画像を収集する時に同期に取得された慣性データと、キーフレームシーケンス内のすべての奥行き画像を収集した時に同期に取得された慣性データとを利用して融合演算し、移動ロボットの位置変化状況を特徴づける相対位置姿勢を求め、次に明確で広域のロボット周辺情報を反映するキーフレームを所定の顕著な位置姿勢変化条件に基づいて選択し、従来技術に比べて、移動ロボットにこれらのキーフレームを利用して移動ロボットの前方の障害物の分布状況を広範囲にカバーできる局所点群地図を作成させ、これにより、現在フレームの奥行き画像と更新をサポートするこれらのキーフレームとの相対位置姿勢を融合処理することによって、ロボットがリアルタイムに収集された単一フレームの奥行き画像に基づき機体のエッジ位置に近い障害物又は機体に近過ぎる障害物を識別し測位しにくいという問題を解決する。

なお、移動ロボットは弓字形で清掃する時、９０°向きを変えた後、障害物が本体に非常に近い場合に検出できない問題を解決するために、本実施例では、予め記憶された画像シーケンス中の奥行き画像が使用され、例えば、現在の収集時点がｔであり、ｔ－１時点、ｔ－２時点、ｔ－３時点で収集された奥行き画像フレーム、又はタイミング性を有する他のより昔のキーフレーム画像を使用することができ、必ずしも奥行き画像のエッジに対応する障害物ではなく、キーフレームにおいて本体の前方に出現される有効点である限り、現在フレームと融合して相対位置姿勢を計算することができる。そして、機体中心の慣性センサによりリアルタイムに収集された位置情報を取得する必要がある。

一実施例として、あらゆる奥行き距離の変化量がすべて所定の距離変化閾値よりも大きく、且つあらゆる偏向角の変化量がすべて所定の角度変化閾値よりも大きい場合、前記所定の顕著な位置姿勢変化条件を満たすと決定し、移動ロボットが大きく変位し又は回転角が大きく変化する（例えば、弓字形で計画される走行中に９０°向きを変える）ことを特徴づける。

従来技術に比べて、該技術的解決手段は、２フレームの奥行き画像の対応する実際の収集位置点の変化量が十分に大きいであるかどうかを判断し、２フレームの奥行き画像の対応する実際の機体偏向角の変化量が十分に大きいであるかどうかを判断することによって、機体の変位が十分に大きいであるかどうか又は機体の回転角度が十分に大きいであるかどうかを特徴づけ、これにより、新たなキーフレームとして構成される奥行き画像と現在フレームの奥行き画像とのギャップが十分に大きくなり、明確で広域のロボット周辺情報を提供することができ、属するキーフレームシーケンス内の奥行き画像のカバーされる可視範囲が十分に大きくなり（例えば、右から見る又は左から見る角度、又は、近距離から見る又は遠距離から見る変位であり、これらの角度間、位置間のロボットの位置姿勢変換が十分に大きい必要がある）、キーフレームシーケンス内の奥行き画像には、障害物を含むキーフレームと障害物を含まないキーフレームの両方が存在し、障害物の測位計算ステップが簡略化される。

ステップ２、キーフレームシーケンス内の奥行き画像の所定の有効特徴点が位置姿勢変換を行い、カメラ座標系における点群になるように制御し、次にステップ３に進み、前記キーフレームシーケンス内の各キーフレーム中の所定の有効特徴点と、それに対応するキーフレームでの３ｄ－ｔｏｆカメラとの距離及び偏向角はすべて平行移動及び回転の方式で前記現在フレームの奥行き画像に対応して属するカメラ座標系に変換され、前記位置姿勢変換が実現され、又は、エンコーダディスクにより測定された移動ロボットの位置座標を、前記移動ロボットの機体中心と３ｄ－ｔｏｆカメラとの剛性接続関係によって前記現在フレームの奥行き画像が対応して属するカメラ座標系に変換され、前記位置姿勢変換が同様に実現され、前記局所点群地図の地図座標系は現在フレームの奥行き画像に対応するカメラ座標系である。

それにより、各方位で収集された所定の有効特徴点をすべて同じ局所点群座標系（相対距離情報及び偏向角情報）に変換することで、移動ロボットの相対位置姿勢を決定し、移動ロボットの前方に障害物が存在するかどうかを容易に判断でき、同時に、移動ロボットが前記所定の有効特徴点を収集する時の移動軌跡をラベルングすることができ、ここで、同じ局所点群座標系は現在フレームの奥行き画像の対応するカメラ座標系である。

本実施例は、より正確な局所点群地図の復元に有利であり、移動ロボットのリアルタイムの測位とナビゲーションが容易になる。好ましくは、前記キーフレームシーケンス内の各キーフレーム中の同じターゲット障害物に関する特徴点は位置姿勢変換によって同じ局所点群地図座標系に変換され、同じターゲット障害物をラベルングし異なるキーフレームでの移動ロボットの位置姿勢を取得することに用いられる。

ステップ３、移動ロボットの３ｄ－ｔｏｆカメラのリアルタイムな視野角範囲内に、水平地盤に位置する１つの所定のターゲット平面領域を設定し、この所定のターゲット平面領域にそれにマッチングする検出ブロックを設定し、点群を所定のターゲット平面領域の真上に三次元ヒストグラムの形でラベルングし、次にステップ４に進む。

具体的に、前記所定のターゲット平面領域は長方形領域であり、長方形領域の一辺の中点は３ｄ－ｔｏｆカメラの位置であり、この辺の長さが前記移動ロボットの機体直径に等しく、該辺に垂直な一辺は３ｄ－ｔｏｆカメラの視野角の前方に設置され、前記３ｄ－ｔｏｆカメラにより収集された奥行き画像中の検出可能な奥行き距離を特徴づけることに用いられる。

それにより、前記所定のターゲット平面領域は、前記移動ロボットがスムーズに通過できるターゲット検出領域を収容できるように構成され、上記ステップ２におけるキーフレームの位置姿勢変換されたカメラ座標系での点群と組み合わせて、この領域の上方の障害物点群の最大の高さ位置及び分布特徴を測位し、近距離条件で障害物を回避するルートを効果的に計画する。

各点群を三次元ヒストグラムの形でラベルングする前記方法は、前記三次元ヒストグラムが属する三次元座標系の座標原点が３ｄ－ｔｏｆカメラの位置であるように設定し、前記所定のターゲット平面領域がこの三次元座標系の横軸の正方向の前記機体直径の半分の距離をカバーするように設定し、前記所定のターゲット平面領域がこの三次元座標系の横軸の負方向の前記機体直径の半分の距離をカバーするように設定し、前記所定のターゲット平面領域がこの三次元座標系の縦軸の正方向の前記検出可能な奥行き距離をカバーするように設定し、前記検出ブロックがこの三次元座標系の垂直軸に沿って分布するように設定し、この三次元座標系の垂直軸で前記検出ブロックが占有する垂直高さを構成して前記検出ブロックに分布する点群の高さを表し、前記点群を分割して前記所定数の三次元ヒストグラムを取得し、三次元ヒストグラムの形で前記点群を記憶することを実現するステップを含み、前記三次元ヒストグラムが属する三次元座標系は前記局所地図の座標系と同じである。

それにより、ヒストグラム統計の方式で局所地図の点群を局所地図の座標系における特定の検出領域上の三次元点状地図に変換され、三次元点状地図に分布する長方形のブロックの高さを利用して障害物の点群の高さを特徴づけることが実現され、漂遊点群を顕著で迅速に除去することに便利である。

本実施例では、前記所定のターゲット平面領域の対応する所定の座標値の範囲は、前記３ｄ－ｔｏｆカメラの中心を座標原点として、最左側－１５０mm～最右側＋１５０mmである横軸方向における座標のカバーされる範囲、最も近い０mm～最も遠い３００mmである縦軸方向における座標範囲（奥行き距離として具現化される）を含み、本実施例は、水平投影面積が３０mm＊３０mmである各検出ブロックにおいて、前記局所地図の点群位置を三次元ヒストグラムの形で統計し、所定のターゲット平面領域の座標値の範囲にないものに対してブロック化し属する点群の最大の高さを記録することを実行せず、有効のターゲット空間領域内に点群の最大の高さがラベルングされる三次元点状地図を生成する。

それにより、前記所定のターゲット平面領域は、前記移動ロボットがスムーズに通過できるターゲット検出領域を収容できるように構成され、この領域の上方の障害物点群の最大の高さ位置及び分布特徴を識別して測位することにより、近距離条件で障害物を回避するルートをリアルタイムで効果的に計画する。

好ましくは、前記所定のターゲット平面領域の面積と前記検出ブロックの水平投影面積との比率は所定の整数であり、それにより所定のターゲット平面領域は所定の整数の検出ブロックによって完全に均等に分割されて占有される。各行にＮ個の検出ブロックが配置され、各列にＭ個の検出ブロックが配置され、行方向は機体の前進方向に垂直な側方向であり、列方向は機体の前進方向であり、本実施例では３ｄ－ｔｏｆカメラの光軸方向に相当し、所定の整数はＮとＭの積であり、前記検出ブロックによる前記所定のターゲット平面領域の点群カバー率が向上される。

ステップ４、各検出ブロックにおける点群の高さ位置の分布特徴に基づいて、先ず対応する検出ブロック内の離散分布状態の点群を削除し、次に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除し、次にステップ５に進む。前記ステップ４において、１つの前記点群の三次元座標位置がそのうち１つの前記検出ブロック内にあることが検出された場合、この前記点群がこの前記検出ブロック内に分布すると決定し、この点群の高さを記録し、それにより点群の位置情報の記憶と適用の方式を簡略化する。

さらに、点群が各検出ブロックに入る状況に基づき、各検出ブロック内に分布する点群の高さをそれぞれ記録し、各検出ブロック内に分布する点群の最大の高さを含み、各検出ブロック内に分布する最大の高さの点群をこの検出ブロックの最大の高さ位置として識別し、前記点群を所定のターゲット平面領域の真上に三次元ヒストグラムの形でラベルングする。

ここで、収集視線が遮断される高さ特徴を有する検出ブロック内の点群は、高さが比較的高い特定の検出ブロックに収集視線が遮断される比較的低い検出ブロック内に分布する点群であり、この比較的低い点群は前記３ｄ－ｔｏｆカメラの前方に位置し、且つこの３ｄ－ｔｏｆカメラから離れる方向に沿って高さが比較的高い検出ブロックの後方に配置される。

前記ステップ４において、対応する検出ブロック内の離散分布状態の点群を削除する前記方法は、具体的に、前記検出ブロック内に分布する点群の数が、前記離散分布状態を説明するための測位点数閾値未満であるかどうかを判断し、ＹＥＳの場合にこの検出ブロックに分布する点群を削除し、次に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除し、ＮＯの場合に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を直接削除するステップ、及び／又は、１つの検出ブロックの近傍領域に点群が分布していないかどうかを判断し、ＹＥＳの場合にこの検出ブロック内に分布する点群を削除し、次に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除し、ＮＯの場合に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を直接削除するステップを含む。

具体的に、外部干渉、例えば視線の遮断、障害物等の要素の影響により、点群データにおいて一般的に被測位識別の障害物の点群から比較的遠い離散点、すなわち外れ値が存在し、点群地図の測位とナビゲーションの精度に悪影響を与える。本実施例では、前記ステップ４は具体的に以下を含む。

前記検出ブロック内に分布する点群の数が測位点数閾値未満であると判断した場合、この検出ブロック内に分布する点群を削除し、具体的に、この検出ブロック内に分布する点群をクリアーし、その点群を所定のターゲット平面領域の真上から削除する。

又は、前記検出ブロック内に分布する点群の数が測位点数閾値未満であると判断した場合、この検出ブロックの近傍領域に点群が分布していないかどうかを判断し続け、ＹＥＳの場合にこの検出ブロック内に分布する点群を削除する。

又は、１つの検出ブロックの近傍領域に点群が分布していないと判断した場合、この検出ブロック内に分布する点群は相対的に独立して所定のターゲット平面領域の真上に存在することを表し、この検出ブロック内に分布する点群を削除し、具体的に、この検出ブロック内に分布する点群をクリアーし、その点群を所定のターゲット平面領域の真上から削除する。

又は、１つの検出ブロックの近傍領域に点群が分布していないと判断した場合、この検出ブロック内に分布する点群の数が測位点数閾値未満であるかどうかを判断し続け、ＹＥＳの場合にこの検出ブロック内に分布する点群を削除する。

上記ステップでは、あまりに離散的に分布する点群による障害物の識別測位へのノイズ干渉、特に遠過ぎる障害物による測位誤差の問題を減少し、さらに近過ぎる障害物の識別の精度を向上させる。

前記ステップ４において、収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除する前記方法はステップ４１～ステップ４３を含む。

ステップ４１、３ｄ－ｔｏｆカメラから離れる方向に沿って各列の前記検出ブロックに分布する点群の高さを検出し、各列の検出ブロック内に低から高へ変化する検出ブロックシーケンスが存在するかどうかをそれぞれ判断し、ＹＥＳの場合にステップ４２に進み、ＮＯの場合にステップ４３に進み、低から高への変化は、判断に参与する各検出ブロック中に分布する最大の高さに対応する点群の高さ位置の変化であり、各検出ブロックに分布する点群の最大の高さは各検出ブロックの実際の高さを表し、前記三次元ヒストグラムの各長方形のブロックの安定した高さとして反映される。

ステップ４２、この検出ブロックシーケンス中の高さが最も高い点群が属する検出ブロックを記録し、その後この記録された検出ブロックの後方の高さが比較的低い検出ブロックに分布する点群を削除し、次にステップ４１に戻り、元の検出トラバーサル方向に沿って、対応する一列の残りの検出ブロック（点群が削除される検出ブロックを含んでもよい）中に低から高へ変化する検出ブロックシーケンスがまだ存在するかどうかを判断し続け、まだ存在する場合に、前回判断された検出ブロックシーケンスに比べてこの検出ブロックシーケンスの高さが最も高い点群はよりも高いと決定し、上記削除ステップを繰り返し実行し続け、それにより、対応する一列の検出ブロック中の高さが最も高い点群が属する検出ブロックの後方の検出ブロックに分布する点群を完全に削除する。

ステップ４３、この検出ブロックシーケンス中の高さが最も高い点群が属する検出ブロックを記録し、その後この記録された検出ブロックの後方の検出ブロックに分布する点群を削除する。

本実施例では、座標系が変換された後の点群に現在の測位に役立たない誤った点を最初にフィルタリングする必要があり、本実施例のステップ４では、三次元ヒストグラムの形で記憶される点群に多くの不合理な点が存在し、例えば、壁の場合に、点群中の厚さが１０cmにもなり、従って、上記ステップに基づき、機体の現在位置の前方に収集された、壁の点群の高さを特徴づける検出ブロックが視線を完全に遮断するかどうかを判断し、ＹＥＳの場合に、この検出ブロックの後ろの検出ブロック内の点群が有効（すなわち３ｄ－ｔｏｆカメラにより予め収集されて、且つ前記局所地図にラベルングされる）であっても、現在位置での３ｄ－ｔｏｆカメラにより収集されるべきではなく、従って、この検出ブロックの後ろの検出ブロック内の点群をすべて削除する必要があり、現在位置での３ｄ－ｔｏｆカメラの視野角範囲内の点群の実際の高さ位置を考慮せず、予め収集され、且つ前記局所地図にラベルングされる対応する点群の位置を直接使用し測位すると移動ロボットの現在位置での障害物回避操作の実行に役立たないという影響を回避し、また、点群自体の誤差はリアルタイムな測位に干渉要素をもたらすと理解でき、現在位置で高過ぎる障害物の後方を見ることができないが、３ｄ－ｔｏｆカメラにより関連する奥行き画像が収集されており、この場合、本実施例の移動ロボットは常に高さが最も高い検出ブロックの後ろの検出ブロック内の点群を利用して障害物を測位し、測位誤差が発生しやすい。

従って、前方の実際に遭った障害物をリアルタイムに測位し識別するために、この検出ブロックの後ろの低い検出ブロック内の点群をすべて削除し、高さが最も高い検出ブロックの前の点群を保持する必要があり、それにより点群位置測定の誤差による悪影響を解消する。

従って、従来技術に比べて、上記ステップ４１～ステップ４３によれば、検出ブロックに分布する点群の削除は高さが最も高い点群に基づいて行われ、具体的に、実際の物理的環境における山形に応じて、山頂の高さ位置に持ち上げられる前の点群を保持し、山の裏側の対応する低い点群を削除するように選択し（収集視線が遮断される高さ特徴を有する検出ブロック内の点群）、それにより、移動ロボットの現在位置に顕著な視線遮断効果を有する障害物を保持し、所定のターゲット平面領域の対応する検出ブロックから、実際の高さ位置の遮断により現在位置の障害物測位に適用できない点群を削除し、局所地図の点群データのフィルタリング及び最適化が実現され、そして、点群自体の測定誤差（高過ぎる障害物の後ろの検出ブロックが実際のシーンにおいて見えないが、奥行き画像では収集され、且つ局所地図に変換される）による測位干渉要素が克服され、近過ぎる障害物の点群位置の誤判定現象が顕著に減少され、前記局所地図のリアルタイムな測位の精度が向上されと理解できる。

ステップ５、各検出ブロック内に分布する残りの点群の位置を局所点群地図の三次元位置としてラベルングし、局所点群地図の構築を実現する。本実施例では、前記キーフレームシーケンス内に３フレーム又は３フレーム以上の奥行き画像が存在し、局所点群地図を構築し有効特徴点によって障害物を測位することにより十分な点群サンプルを提供することができ、ステップ５まで実行すると各検出ブロック内の誤差を引き起こやすい点群の一部が削除されるが、前記キーフレームシーケンス内に存在する奥行き画像の関連する測位サンプルの特徴点が十分であるため、局所点群地図がより安定し有効的になることを確保できる。

好ましくは、前記移動ロボットは先入れ先出し（ＦＩＦＯキュー）のメモリー記憶の形で前記キーフレームシーケンスをキャッシュし、それにより前記移動ロボットは移動中に、前記キーフレームシーケンス内に最も新しく挿入される前記キーフレームをリフレッシュし、３ｄ－ｔｏｆカメラにより現在リアルタイムに収集された１フレームの奥行き画像に時間的に最も新しい前記キーフレームを取得する。記憶空間の最適化を実現するために、本実施例は先入れ先出しの方式でキューを記憶し最も新しく選別して挿入されたキーフレームを記憶し、最も有効的なフレームシーケンスしか記憶しないことを確保し、これは現在フレームに最も近い３フレームの奥行き画像であってもよい。先入れ先出し（ＦＩＦＯキュー）の形でキャッシュされる奥行き画像データは冗長ではないため、さらに有効的な局所点群地図の測位情報が正確に維持される。

以上より、上記ステップでは、移動ロボットはこれらのキーフレームを利用して移動ロボットの前方の障害物の分布状況を広範囲にカバーできる局所点群地図を作成し、これにより、現在フレームの奥行き画像と更新をサポートするこれらのキーフレームとの相対位置姿勢を融合処理し、ロボット周辺の広範囲の領域の位置を反映する点群を、所定の顕著な位置姿勢変化条件に基づいてサンプリングすることによって、移動ロボットにこれらのキーフレームを利用して移動ロボットの前方の障害物の分布状況を完全にカバーできる局所点群地図を作成させ、次に三次元ヒストグラムの形で異なる高さ位置の点群を説明し、異なる検出ブロックにおける点群の三次元領域分布特徴を利用して漂遊点を除去するのに便利であり、ロボットがリアルタイムに収集された単一フレームの奥行き画像に基づき機体のエッジ位置に近い障害物又は機体に近過ぎる障害物を識別し測位しにくいという問題を解決し、近過ぎる又は遠過ぎる障害物シーンでの局所点群地図の適応性が向上され、超近距離での障害物の位置の識別測位の難しさ及び誤判定の確率が顕著に低減される。

上記実施例では、前記所定の有効特徴点は非露出過度点、非露出不足点、非遠過ぎ点、非近過ぎ点及び最大垂直高さが２０mmよりも大きい画素点を含み、非遠過ぎ点の奥行き値と非近過ぎ点の奥行き値の両方は２００mm～５００mmであり、３ｄ－ｔｏｆカメラの視野角範囲内の移動ロボットの前方の障害物の有効位置特徴点の集合を特徴づけることに用いられる。

具体的に、非露出過度点、非露出不足点は３ｄ－ｔｏｆカメラにより直接取得されるものであり、各フレームの奥行き画像中の画素点のそれぞれに対して、露出過度、露出不足、遠過ぎ、近過ぎという４つの状態があり、露出過度点と露出不足点は信頼できない点であり、近過ぎ点と遠過ぎ点の両方はキュー（ＦＩＦＯ）内に記憶されない。

それにより、光の明るさにより大きく影響される露出過度点及び露出不足点、距離誤差により大きく影響される遠過ぎ点及び近過ぎ点を含む奥行き画像中の信頼できない画素点を排除する。奥行き画像の測位の安定性が向上される。

前記所定の有効特徴点は、移動ロボットの前進方向上の有効なマーカーの特徴点であり、前記３ｄ－ｔｏｆカメラは移動ロボットの移動中に、前記所定の有効特徴点の異なるフレームの奥行き画像を異なる方向から収集し、向きを変える過程で、前記所定の有効特徴点を、前後に左側から収集し、接近して収集し、右側から収集してから離れて収集し、それにより、同じマーカーの視線が遮断されない奥行き画像及び同じマーカーの視線が遮断された奥行き画像を前後収集し、移動ロボットが前記局所点群地図の構築方法を完了する時、先ず三次元ヒストグラムの形で高さが最も高い点群を記憶することはこのマーカーの収集視線が遮断される環境位置特徴を特徴づける可能性がある。

視覚ロボットであって、３ｄ－ｔｏｆカメラと、前記局所点群地図の構築方法を実行するための処理ユニットとを含み、３ｄ－ｔｏｆカメラは、その視野角が前記視覚ロボットの前方をカバーできるように視覚ロボットの機体に組み立てられる。

それにより、該３ｄ－ｔｏｆカメラは移動ロボットの移動中に異なる視野角でターゲット障害物の点群をサンプリングし、次に該視覚ロボットが特定の位置に移動する時、３ｄ－ｔｏｆカメラの現在の視野角範囲内の所定のターゲット平面領域の点群の高さ位置の分布特徴を利用して、漂遊点を除去し、正確な測位のための最大の高さ及び合理的に分布する点群しか局所地図に保持せず、向きを変える過程で、該視覚ロボットは、それが向きを変える前に視野角範囲内で識別した機体のエッジ位置に近い障害物又は機体に近過ぎる障害物を再識別することができ、近過ぎる又は遠過ぎる障害物シーンでの測位適応性が向上され、超近距離での障害物位置の誤判定の確率が顕著に低減される。

明らかに、上記実施例は明確な説明のための例示的なものに過ぎず、実施形態を限定するものではない。当業者にとって、上記説明に基づき他の様々な変化や変更を行うことができる。ここでは、あらゆる実施形態を網羅する必要がなく、且つ網羅できない。これにより由来する明らかな変化や変更は本発明創造の保護範囲に属する。

Claims

局所点群地図の構築方法であって、
移動ロボットの移動中、移動ロボットの３ｄ－ｔｏｆカメラを制御して奥行き画像を収集し続け、３ｄ－ｔｏｆカメラによりリアルタイムに収集された現在フレームの奥行き画像を、所定の顕著な位置姿勢変化条件に基づいてキーフレームシーケンス内に挿入し、次にステップ２に進むステップ１と、
キーフレームシーケンス内の奥行き画像の所定の有効特徴点が位置姿勢変換を行い、カメラ座標系における点群になるように制御し、次にステップ３に進むステップ２と、
移動ロボットの３ｄ－ｔｏｆカメラのリアルタイムな視野角範囲内に、水平地盤に位置する１つの所定のターゲット平面領域を設定し、この所定のターゲット平面領域にそれにマッチングする検出ブロックを設定し、点群を所定のターゲット平面領域の真上に三次元ヒストグラムの形でラベルングし、次にステップ４に進むステップ３と、
各検出ブロックにおける点群の高さ位置の分布特徴に基づいて、先ず対応する検出ブロック内の離散分布状態の点群を削除し、次に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除し、次にステップ５に進むステップ４と、
各検出ブロック内に分布する残りの点群の位置を局所点群地図の三次元位置としてラベルングし、局所点群地図の構築を実現するステップ５と、を含む、ことを特徴とする局所点群地図の構築方法。
３ｄ－ｔｏｆカメラによりリアルタイムに収集された現在フレームの奥行き画像を、所定の顕著な位置姿勢変化条件に基づいてキーフレームシーケンス内に挿入する前記方法は具体的に、
移動ロボットの３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像中の所定の有効特徴点と、前記キーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点との相対位置姿勢をそれぞれ計算するステップであって、キーフレームシーケンスは移動ロボットが予め記憶した、所定の有効特徴点を含む連続的に配列された奥行き画像のシーケンスである、ステップと、
計算されたすべての相対位置姿勢が所定の顕著な位置姿勢変化条件を満たす場合、３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像を新たなキーフレームとして構成し、次にこの新たなキーフレームを前記キーフレームシーケンスに挿入し、次回前記相対位置姿勢計算に参与するキーフレームが移動ロボットの最新の相対位置関係を反映するようにするステップと、を含み、
所定の有効特徴点は前記３ｄ－ｔｏｆカメラの視野角範囲内での移動ロボットの前方の障害物の分布位置特徴を特徴づけるためのものであり、障害物の分布位置特徴は障害物の存在及び／又は障害物の不在を含む、ことを特徴とする請求項１に記載の局所点群地図の構築方法。
前記キーフレームシーケンス内の各キーフレーム中の所定の有効特徴点と、それに対応するキーフレームでの３ｄ－ｔｏｆカメラとの距離及び偏向角はすべて平行移動及び回転の方式で前記現在フレームの奥行き画像に対応して属するカメラ座標系に変換され、前記位置姿勢変換を実現し、
前記局所点群地図の地図座標系は現在フレームの奥行き画像に対応するカメラ座標系である、ことを特徴とする請求項２に記載の局所点群地図の構築方法。
前記した計算されたすべての相対位置姿勢が所定の顕著な位置姿勢変化条件を満たすことについて、具体的に、
計算された相対位置姿勢は、移動ロボットの３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像中の所定の有効特徴点のそれぞれと、前記位置姿勢変換が発生した後のキーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点との間の、奥行き距離の変化量及びその対応する偏向角の変化量を含み、
あらゆる奥行き距離の変化量がすべて所定の距離変化閾値よりも大きく、且つあらゆる偏向角の変化量がすべて所定の角度変化閾値よりも大きい場合、前記所定の顕著な位置姿勢変化条件を満たすと決定する、ことを特徴とする請求項３に記載の局所点群地図の構築方法。
前記キーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点と、それに対応するキーフレームでの３ｄ－ｔｏｆカメラとの距離は、前記３ｄ－ｔｏｆカメラにより予め収集され前記キーフレームシーケンスに挿入された奥行き画像の所定の有効特徴点に対応する実際の位置点から３ｄ－ｔｏｆカメラの結像平面までの奥行き距離であり、この奥行き距離の変化量は前記移動ロボットの変位の大きさ程度を表すことに用いられ、
前記キーフレームシーケンス内の各キーフレーム中の同じ所定の有効特徴点の、その対応するキーフレームでの３ｄ－ｔｏｆカメラに対する偏向角は、前記３ｄ－ｔｏｆカメラが奥行き画像を収集し、且つ前記キーフレームシーケンスに挿入する時のレンズ向き角度であり、このレンズ向き角度の変化量は前記移動ロボットの回転角度を表すことに用いられる、ことを特徴とする請求項４に記載の局所点群地図の構築方法。
前記ステップ１は、
移動ロボットの３ｄ－ｔｏｆカメラが現在フレームの奥行き画像を現在収集した時、移動ロボットの内部に設置された慣性センサを呼び出して移動ロボットの現在の位置姿勢を測定するステップと、
次に予め保存されたキーフレームシーケンス内の各キーフレームに対応する移動ロボットのリアルタイムな位置姿勢を呼び出し、移動ロボットの現在の位置姿勢と、各キーフレームに対応する移動ロボットのリアルタイムな位置姿勢との間の相対位置姿勢をそれぞれ計算するステップと、を含む、ことを特徴とする請求項１に記載の局所点群地図の構築方法。
前記した計算されたすべての相対位置姿勢が所定の顕著な位置姿勢変化条件を満たすことについて、具体的に、
計算された相対位置姿勢は、移動ロボットの３ｄ－ｔｏｆカメラにより現在収集された現在フレームの奥行き画像中の所定の有効特徴点のそれぞれと、予め収集された前記キーフレーム中の同じ又は同じタイプの所定の有効特徴点との間の発生する移動ロボットの走行距離の変化量及び同じ収集過程で発生する偏向角の変化量を含み、
あらゆる走行距離の変化量がすべて所定の距離変化閾値よりも大きく、且つあらゆる偏向角の変化量がすべて所定の角度変化閾値よりも大きい場合、前記所定の顕著な位置姿勢変化条件を満たすと決定し、
前記所定の有効特徴点は前記３ｄ－ｔｏｆカメラの視野角範囲内での移動ロボットの前方の障害物の分布位置特徴を特徴づけるためのものであり、障害物の分布位置特徴は障害物の存在及び／又は障害物の不在を含む、ことを特徴とする請求項６に記載の局所点群地図の構築方法。
前記慣性センサは移動ロボットの機体中心に内蔵されるエンコーダディスク及び移動ロボットの機体中心に内蔵されるジャイロスコープを含み、エンコーダディスクは移動ロボットの走行距離の変化量を測定することに用いられ、ジャイロスコープは同じ収集過程での移動ロボットの偏向角の変化量を測定することに用いられ、
エンコーダディスクにより測定された移動ロボットの位置座標は前記移動ロボットの機体中心と３ｄ－ｔｏｆカメラとの剛性接続関係によって前記現在フレームの奥行き画像が対応して属するカメラ座標系に変換され、局所点群地図が構築される、ことを特徴とする請求項７に記載の局所点群地図の構築方法。
前記キーフレームシーケンス内に３フレーム又は３フレーム以上の奥行き画像が存在する、ことを特徴とする請求項８に記載の局所点群地図の構築方法。
前記移動ロボットは先入れ先出しのメモリー記憶の形で前記キーフレームシーケンスをキャッシュし、それにより前記移動ロボットは、３ｄ－ｔｏｆカメラにより現在リアルタイムに収集された１フレームの奥行き画像に時間的に最も近い前記キーフレームを移動中に取得する、ことを特徴とする請求項９に記載の局所点群地図の構築方法。
前記所定のターゲット平面領域は長方形領域であり、長方形領域の一辺の中点は３ｄ－ｔｏｆカメラの位置であり、この辺の長さが前記移動ロボットの機体直径に等しく、該辺に垂直な一辺は３ｄ－ｔｏｆカメラの視野角の前方に設置され、前記３ｄ－ｔｏｆカメラにより収集された奥行き画像中の検出可能な奥行き距離を特徴づけることに用いられる、ことを特徴とする請求項１に記載の局所点群地図の構築方法。
前記ステップ４において、１つの前記点群の三次元座標位置がそのうち１つの前記検出ブロック内にあることが検出された場合、この前記点群がこの前記検出ブロック内に分布すると決定し、この点群の高さを記録する、ことを特徴とする請求項１１に記載の局所点群地図の構築方法。
各点群を三次元ヒストグラムの形でラベルングする前記方法は、前記三次元ヒストグラムが属する三次元座標系の座標原点が３ｄ－ｔｏｆカメラの位置であるように設定し、前記所定のターゲット平面領域がこの三次元座標系の横軸の正方向の前記機体直径の半分の距離をカバーするように設定し、前記所定のターゲット平面領域がこの三次元座標系の横軸の負方向の前記機体直径の半分の距離をカバーするように設定し、前記所定のターゲット平面領域がこの三次元座標系の縦軸の正方向の前記検出可能な奥行き距離をカバーするように設定し、前記検出ブロックがこの三次元座標系の垂直軸に沿って分布するように設定し、この三次元座標系の垂直軸で前記検出ブロックが占有する垂直高さを構成して前記検出ブロックに分布する点群の高さを表し、前記点群を分割して所定数の三次元ヒストグラムを取得し、三次元ヒストグラムの形で前記点群を記憶することを実現するステップを含み、
前記三次元ヒストグラムが属する三次元座標系は前記局所点群地図の座標系と同じである、ことを特徴とする請求項１１に記載の局所点群地図の構築方法。
前記所定のターゲット平面領域の面積と前記検出ブロックの水平投影面積との比率は所定の整数であり、それにより所定のターゲット平面領域は所定の整数の検出ブロックによって完全に均等に分割されて占有される、ことを特徴とする請求項１３に記載の局所点群地図の構築方法。
前記ステップ４において、対応する検出ブロック内の離散分布状態の点群を削除する前記方法は、具体的に、
前記検出ブロック内に分布する点群の数が、前記離散分布状態を説明するための測位点数閾値未満であるかどうかを判断し、ＹＥＳの場合にこの検出ブロックに分布する点群を削除し、次に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除し、ＮＯの場合に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を直接削除するステップ、
及び／又は、
１つの検出ブロックの近傍領域に点群が分布していないかどうかを判断し、ＹＥＳの場合にこの検出ブロック内に分布する点群を削除し、次に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除し、ＮＯの場合に収集視線が遮断される高さ特徴を有する検出ブロック内の点群を直接削除するステップを含む、ことを特徴とする請求項１３に記載の局所点群地図の構築方法。
前記ステップ４において、収集視線が遮断される高さ特徴を有する検出ブロック内の点群を削除する前記方法は、
３ｄ－ｔｏｆカメラから離れる方向に沿って各列の検出ブロックに分布する点群の高さを検出し、各列の検出ブロック内に低から高へ変化する検出ブロックシーケンスが存在するかどうかをそれぞれ判断し、ＹＥＳの場合にステップ４２に進み、ＮＯの場合にステップ４３に進むステップ４１であって、低から高への変化は、判断に参与する各検出ブロック中に分布する最大の高さに対応する点群の高さ位置の変化であり、各検出ブロックに分布する点群の最大の高さは各検出ブロックの高さを表す、ステップ４１と、
この検出ブロックシーケンス中の高さが最も高い点群が属する検出ブロックを記録し、その後この記録された検出ブロックの後方の高さが比較的低い検出ブロックに分布する点群を削除し、次にステップ４１に戻るステップ４２と、
この検出ブロックシーケンス中の高さが最も高い点群が属する検出ブロックを記録し、その後この記録された検出ブロックの後方の検出ブロックに分布する点群を削除するステップ４３と、を含む、ことを特徴とする請求項１５に記載の局所点群地図の構築方法。
前記キーフレームシーケンス内のキーフレーム上の所定の有効特徴点は、非露出過度点、非露出不足点、非遠過ぎ点、非近過ぎ点及び最大垂直高さが２０mmよりも大きい画素点を含み、非遠過ぎ点の奥行き値と非近過ぎ点の奥行き値の両方は２００mm～５００mmである、ことを特徴とする請求項１６に記載の局所点群地図の構築方法。
前記所定の有効特徴点は、障害物を特徴づけるための特徴点、又は障害物以外の標準的な道路標識を特徴づけるための特徴点であり、
前記３ｄ－ｔｏｆカメラは移動ロボットの移動中に、前記所定の有効特徴点の異なるフレームの奥行き画像を異なる方向から収集する、ことを特徴とする請求項１６に記載の局所点群地図の構築方法。
１つの検出ブロックの８つの近傍領域に点群が分布していないかどうかを判断し、ＹＥＳの場合にこの検出ブロック内に分布する点群を削除する、ことを特徴とする請求項１５に記載の局所点群地図の構築方法。
視覚ロボットであって、３ｄ－ｔｏｆカメラと、請求項１～１９のいずれか１項に記載の局所点群地図の構築方法を実行するための処理ユニットとを含み、
３ｄ－ｔｏｆカメラは、その視野角が前記視覚ロボットの前方をカバーできるように視覚ロボットの機体に組み立てられる、ことを特徴とする視覚ロボット。