JP2023545279A - 融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体 - Google Patents

融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体 Download PDF

Info

Publication number
JP2023545279A
JP2023545279A JP2023521741A JP2023521741A JP2023545279A JP 2023545279 A JP2023545279 A JP 2023545279A JP 2023521741 A JP2023521741 A JP 2023521741A JP 2023521741 A JP2023521741 A JP 2023521741A JP 2023545279 A JP2023545279 A JP 2023545279A
Authority
JP
Japan
Prior art keywords
road sign
orientation
robot
point cloud
distortion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
JP2023521741A
Other languages
English (en)
Inventor
周陽
張濤
陳美文
劉運航
何科君
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Pudu Technology Co Ltd
Original Assignee
Shenzhen Pudu Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Pudu Technology Co Ltd filed Critical Shenzhen Pudu Technology Co Ltd
Publication of JP2023545279A publication Critical patent/JP2023545279A/ja
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808—Evaluating distance, position or velocity data
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0234—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons
    • G05D1/0236—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Abstract

融合測位方法であって、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のレーダ点群関連データを取得すること（Ｓ１０１）と、環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐｉに最も近い道路標識Ｌjの位置Ｐjを取得すること（Ｓ１０２）と、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得すること（Ｓ１０３）と、道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、誤差Ｅ１および誤差Ｅ２が最小となるまで、ロボットの位置姿勢を反復最適化すること（Ｓ１０４）と、を含む。【選択図】図１

Description

［関連出願の参照］
本出願は、２０２０年１２月２９日に中国特許庁に提出された、出願番号が２０２０１１５８６１３３３であり、出願名称が「融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体」である中国特許出願の優先権を主張し、その全内容は参照により本出願に組み込まれる。
本発明は、ロボット分野に関し、特に、融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体に関する。
移動ロボットは、様々な場面に応用されており、正確な測位はロボットがスムーズに任務を完了するための鍵である。一般に、ロボットの測位は、３次元または２次元のレーザレーダにより実現され、そのうち、２次元レーザレーダの測位方法では、主に、レーザレーダの点群データと予め構築された２次元グリッド地図とをマッチングさせてロボットの位置姿勢の推定を実現する。しかしながら、このような測位方法は、実際の応用場面において、長い廊下や、広大な場所など、ある自由度が２次元レーザレーダの観測範囲を超えてしまう場合があるため、いくつかの問題があった。このような場合に、２次元レーザレーダでは、ロボットの２次元空間における位置姿勢の３つの自由度を完全に拘束することができず、ロボットの位置姿勢推定に誤りが生じる。
ロボットが長い廊下や広大な環境にあってロボットを測位する際に遭遇する上記の問題に対して、１つの方策として、高光反射率の道路標識（Ｌａｎｄｍａｒｋ）を人手で作成して補助的な測位を行い、たとえば、光反射板または光反射柱を用いて補助的な測位を行う。しかしながら、光反射柱または光反射板などの高光反射率の道路標識を用いて補助的な測位を行うだけでは、環境改修に対する要求の高い環境に適合することはできない。
本発明の様々な実施例は、融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体を提供する。
融合測位方法であって、
ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記道路標識Ｌｉの道路標識地図上の位置Ｐｉと前記環境物の歪み除去点群データとを含む、前記環境物のレーダ点群関連データを、取得することと、
前記道路標識Ｌｉの道路標識地図上の位置Ｐｉと道路標識地図とを近傍マッチングし、前記道路標識地図上の前記位置Ｐｉに最も近い道路標識Ｌjの位置Ｐjを取得することと、
前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得することと、
前記道路標識Ｌｉの道路標識地図上の位置Ｐｉ、前記道路標識Ｌjの前記道路標識地図上の位置Ｐj、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、前記道路標識Ｌｉと前記道路標識Ｌjの前記道路標識地図上の位置差値である誤差Ｅ１、および、前記ロボットの推定位置姿勢と前記グローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値である誤差Ｅ２が、最小となるまで、前記ロボットの位置姿勢を反復最適化することと、を含む。
融合測位装置であって、
ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと前記環境物の歪み除去点群データとを含む、前記環境物のレーダ点群関連データを、取得するための第１の取得モジュールと、
前記道路標識Ｌｉの道路標識地図上の位置Ｐｉと道路標識地図とを近傍マッチングし、前記道路標識地図上の前記位置Ｐｉに最も近い道路標識Ｌjの位置Ｐjを取得するための第２の取得モジュールと、
前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得するための第３の取得モジュールと、
前記道路標識Ｌｉの道路標識地図上の位置Ｐｉ、前記道路標識Ｌjの前記道路標識地図上の位置Ｐj、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、前記道路標識Ｌｉと前記道路標識Ｌjの前記道路標識地図上の位置差値である誤差Ｅ１、および、前記ロボットの推定位置姿勢と前記グローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値である誤差Ｅ２が、最小となるまで、前記ロボットの位置姿勢を反復最適化するための反復最適化モジュールと、を備える。
デバイスであって、メモリと、プロセッサと、前記メモリに記憶され、前記プロセッサ上で実行可能なコンピュータープログラムと、を備え、前記プロセッサが前記コンピュータープログラムを実行することにより、上記の融合測位方法の技術的解決手段のステップが実現される。
コンピュータープログラムが記憶されるコンピューター可読記憶媒体であって、前記コンピュータープログラムがプロセッサによって実行されることにより、上記の融合測位方法の技術的解決手段のステップが実現される。
本発明の１つまたは複数の実施例の詳細は、以下の図面および説明に記載されている。本発明のその他の特徴および利点は、明細書、図面、および特許請求の範囲から明らかになる。
以下、本発明の実施例または従来技術の技術的解決手段をより明確に説明するために、実施例または従来技術の説明に必要な図面を簡単に説明するが、明らかに、以下の説明での図面は、本発明のいくつかの実施例に過ぎず、当業者であれば、発明的な努力を費やすことなく、これらの図面から他の実施例の図面を得ることができる。
本発明の実施例によって提供される融合測位方法のフローチャートである。 本発明の実施例によって提供される融合測位装置の構成模式図である。 本発明の実施例によって提供されるデバイスの構成模式図である。
以下、本発明の理解を容易にするために、関連図面を参照して本発明をより完全に説明する。図面は、本発明の好ましい実施例を示す。しかしながら、本発明は、多くの異なる形態で実現することができ、本明細書に記載の実施例に限定されない。逆に、これら実施例は、本発明の開示内容の理解をより徹底的かつ完全にすることを目的として提供される。
別途定義されない限り、本明細書で使用されるすべての技術用語および科学用語は、発明の技術分野に属する当業者によって一般的に理解されるものと同じ意味を有する。本発明の明細書で使用される用語は、具体的な実施例を説明することのみを目的とし、本発明を限定することを意図しない。本明細書で使用される「および／または」という用語は、１つまたは複数の関連する列挙された項目の任意のおよびすべての組み合わせを含む。
本発明は、ロボットに応用することができる融合測位方法を提案し、当該ロボットは、伝菜ロボットなどの、レストランで作業するロボットであってもよいし、病院などの医療現場で作業する薬送りロボットであってもよいし、倉庫などの現場で作業する搬送ロボットであってもよい。図１に示すように、融合測位方法は、主に、ステップＳ１０１からＳ１０４を含み、詳しくは以下のとおりである。
ステップＳ１０１において、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと環境物の歪み除去点群データとを含む、環境物のレーダ点群関連データを取得する。
本発明の実施例において、環境とは、ロボットが作業する環境を意味し、環境物とは、ロボットが作業する環境中のあらゆる物体を意味する。環境物は、環境中に設置された道路標識、および、ある荷物、木、壁、または机などのその他の物体を含む。道路標識は、ロボットが作業する環境において、２次元レーザレーダの可視範囲内に設置された光反射柱、光反射バーなどの棒状物であってもよく、ここで、光反射柱の直径が５ｃｍ程度であってもよく、光反射バーの幅も５ｃｍ程度であってもよい。より正確な補助的な測位を行うためには、これらの光反射柱、光反射バーなどの道路標識を少なくとも３つ以上設置する必要があり、また、できる限り同じ直線上に設置しなく、間隔もできる限り１米より大きいように保持する必要がある。
本発明の一実施例として、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のレーダ点群関連データを取得することは、ステップＳ１０１１からステップＳ１０１４により実現でき、以下説明する。
ステップＳ１０１１において、２次元レーザレーダにより環境物をサンプリングして、環境物の原初レーダ点群データを得る。
本発明の実施例において、ロボットに搭載された２次元レーザレーダのサンプリング過程では、従来の技術と同様に、周囲の環境にレーザビームを発射して、リアルタイムで現在環境を走査し、飛行時間測距法を用いてロボットと環境中の道路標識との間の距離を算出し、レーザビームが環境物に衝突した度に、当該レーザビームのロボットに対する角度、レーザ光源と衝突した環境物との間の距離などの情報を取得し、複数のレーザビームが環境物に衝突したときの上記の情報によって環境物の原初レーダ点群データが構成される。２次元レーザレーダがサンプリングした道路標識Ｌｉは、環境中のいずれかの道路標識である。
ステップＳ１０１２において、原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、環境物の原初レーダ点群データに対して歪み除去を行って、環境物の歪み除去点群データを得る。
２次元レーザレーダが環境をサンプリングする際にロボットが移動しているため、サンプリングされる点毎にレーザレーダ座標系における座標の原点は異なる。たとえば、２次元レーザレーダが２番目の点をサンプリングする際に、ロボットに変位が発生したので、２次元レーザレーダが位置するレーザレーダ座標系の原点にも変位が発生した。同様に、２番目の点から１走査周期の終了時の最後の点まで、サンプリングされる点毎に、レーザレーダ座標系の原点がいずれも異なるため、レーダ点群データに歪みが発生し、このような歪みを除去する必要がある。具体的には、原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、環境物の原初レーダ点群データに対して歪み除去を行って、環境物の歪み除去点群データを得ることは、１走査周期後の、２次元レーザレーダが位置するレーザレーダ座標系の新たな原点を確定し、相対的位置姿勢△Ｔと、原初レーダ点群データのうちの各点の旧原点のレーザレーダ座標系での座標Ｐとを乗算し、乗算後の結果△Ｔ*Ｐを原初レーダ点群データのうちの各点の座標とすることであってもよい。そのうち、相対的位置姿勢△Ｔとは、原初レーダ点群データのうちの各点の、新たな原点に対して得られたロボット位置姿勢データである。また、上記の、１走査周期後の、２次元レーザレーダが位置するレーザレーダ座標系の新たな原点を確定することとは、２次元レーザレーダの１走査周期が終了し、２次元レーザレーダが１フレームのレーザレーダ点群データの最後の点をサンプリングした際に、このときの２次元レーザレーダの中心点を２次元レーザレーダが位置するレーザレーダ座標系の新たな原点として確定することである。
ステップＳ１０１３において、環境物のうちの道路標識Ｌｉの歪み除去点群データをクラスタリングして、道路標識Ｌｉの点群クラスタを得る。
前述したように、環境物には、環境中に設置されている光反射柱、光反射バーなどの道路標識が含まれるため、ステップＳ１０１２で得られた環境物の歪み除去点群データにも、環境物のうちの道路標識Ｌｉの歪み除去点群データが含まれる。環境物のうちの道路標識Ｌｉの歪み除去点群データを抽出する方法として、環境物の原初レーダ点群データに対して歪み除去を行って、環境物の歪み除去点群データを得た後、環境物の歪み除去点群データのうちの各点に対応するレーザ光反射強度と予め設定された光強度閾値とを比較することにより、レーザ光反射強度が予め設定された光強度閾値よりも小さい点を除去し、環境物の歪み除去点群データのうちのレーザ光反射強度が予め設定された光強度閾値以上である（すなわち、予め設定された光強度閾値よりも大きいまたはそれと等しい）点群データを保持し、これらのレーザ光反射強度が予め設定された光強度閾値以上である点群データを、環境物のうちの道路標識Ｌｉの歪み除去点群データとする。たとえば、２次元レーザレーダで検出可能なレーザ光反射強度を０～１と仮定すると、光反射柱、光反射バーなどの道路標識のレーザ光反射強度が通常０.８５以上であるため、予め設定された光強度閾値を０.８５に設定し、そして保持環境物の歪み除去点群データのうちの、レーザ光反射強度が０.８５以上である点群データを保持し、これらのレーザ光反射強度が０.８５以上である点群データは、環境物のうちの道路標識Ｌｉの歪み除去点群データとされる。
ノイズのある密度ベースのクラスタリング（Ｄｅｎｓｉｔｙ-Ｂａｓｅｄ Ｓｐａｔｉａｌ Ｃｌｕｓｔｅｒｉｎｇ ｏｆ Ａｐｐｌｉｃａｔｉｏｎｓ wｉｔｈ Ｎｏｉｓｅ，ＤＢＳＣＡＮ）アルゴリズムには、任意の形状の稠密データセットをクラスタリングでき、クラスタリングと同時に異常点を発見でき、クラスタリングの結果には偏りが生じることが少ないなどの利点があることを考慮して、本発明の実施例において、ＤＢＳＣＡＮアルゴリズムを用いて環境物のうちの道路標識Ｌｉの歪み除去点群データをクラスタリングして、道路標識Ｌｉの点群クラスタを得ることができる。
ステップＳ１０１４において、道路標識Ｌｉの点群クラスタのロボット本体座標系での幾何学的中心座標を求め、道路標識Ｌｉの点群クラスタのロボット本体座標系での幾何学的中心座標を道路標識Ｌｉの道路標識地図上の位置Ｐｉとする。
クラスタリングにより道路標識Ｌｉの点群クラスタを得た後に、道路標識Ｌｉの点群クラスタを１つの質点とみなすことができ、幾何学的または物理的の方法により道路標識Ｌｉの点群クラスタのロボット本体座標系での幾何学的中心座標を求めて、道路標識Ｌｉの点群クラスタのロボット本体座標系での幾何学的中心座標を道路標識Ｌｉの道路標識地図上の位置Ｐｉとすることができる。本発明の実施例において、光反射柱の直径と光反射バーの幅が小さいため、道路標識Ｌｉの点群クラスタの幾何学的中心を簡単に求めることで、光反射柱、光反射バーなどの道路標識の幾何学的中心をほぼ近似的に表すことができる。
ステップＳ１０２において、環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐｉに最も近い道路標識Ｌjの位置Ｐjを取得する。
本発明の実施例において、道路標識地図とは、予め作製された、道路標識（Ｌａｎｄｍａｒｋ）の世界座標系での位置を反映した地図であり、道路標識地図での点は１つの道路標識とみなされる。前述したように、道路標識Ｌｉの道路標識地図上の位置Ｐｉは即ち、道路標識Ｌｉの点群クラスタのロボット本体座標系での幾何学的中心座標である。具体的には、ｋＮＮ（ｋ-Ｎｅａｒｅｓｔ Ｎｅｉｇｈｂｏｒ，ｋ-最近傍）アルゴリズムなどの最近傍アルゴリズムを用いて、道路標識Ｌｉの道路標識地図上の位置Ｐｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐｉに最も近い道路標識Ｌjの位置Ｐjを取得することができる。
ステップＳ１０３において、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得する。
本発明の実施例において、２次元グリッド地図は、２次元グリッド確率地図または２次元占有グリッド地図（Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐ）とも呼ばれ、このような地図は、平面を個々のグリッドに分割し、各グリッドに１つの占有率（Ｏｃｃｕｐａｎｃｙ）を与えたものである。占有率とは、２次元占有グリッド地図上で１つのグリッドが障碍物に占有されている状態（ｏｃｃｕｐｉｅｄ状態）、障碍物に占有されていない状態（Ｆｒｅｅ状態）、またはこの２つの状態の間に介在する確率である。そのうち、障碍物に占有されている状態を１、障碍物に占有されていない状態を０、２つの状態の間に介在する状態を０～１の間の値で表す。明らかに、占有率は、あるグリッドが障碍物に占有されている可能性の大きさを示しており、１つのグリッドの占有率が大きいほど当該グリッドが障碍物に占有されている可能性が高いことを示しており、そうでない場合は逆である。
本発明の一実施例として、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得することは、ステップＳ１０３１からステップＳ１０３３により実現でき、以下説明する。
ステップＳ１０３１において、ロボットの現在時刻の前の時刻の位置姿勢から、位置姿勢検索空間における複数の位置姿勢候補を確定する。
ロボットの位置姿勢とは、ロボットの世界座標系での状態を意味し、当該状態を位置と姿勢で表し、具体的には、ロボットの世界座標系での横座標ｘ、縦座標ｙ、および横軸に対する角度θ、すなわち（ｘ，ｙ，θ）で、ロボットのある時刻における位置姿勢を表している。推定により得られたロボットの現在時刻の前の時刻の位置姿勢を（ｘ，ｙ，θ）とすると、当該位置姿勢において各分量に増分△ｘ、増分△ｙ、および増分△θを与えると、位置姿勢検索空間（△ｘ，△ｙ，△θ）には複数の位置姿勢候補（ｘ１，ｙ１，θ１）、（ｘ２，ｙ２，θ２）、…、（ｘｉ，ｙｉ，θｉ）、…、（ｘｎ，ｙｎ，θｎ）が存在することになる。
ステップＳ１０３２において、複数の位置姿勢候補のうちの各々の位置姿勢候補を利用して、環境物の歪み除去点群データを２次元グリッド地図に投影し、各々の位置姿勢候補の２次元グリッド地図上のマッチングスコアを算出する。
ここで、環境物の歪み除去点群データを２次元グリッド地図に投影するとは、ある変換関係に従って、環境物の歪み除去点群データを２次元グリッド地図上のある占有位置またはグリッドに変換することであり、位置姿勢候補が異なると、環境物の歪み除去点群データが２次元グリッド地図上の異なるグリッドに変換されることになる。ある位置姿勢候補（ｘｉ，ｙｉ，θｉ）を想定し、環境物の歪み除去点群データを（ｘj，ｙj，θj）とし、位置姿勢候補（ｘｉ，ｙｉ，θｉ）に対応して当該（ｘj，ｙj，θj）を２次元グリッド地図上に投影したグリッド位置を
で表すと、
となる。
そのうち、zｋは、２次元グリッド地図上で位置が
であるグリッドの番号である。
前述したように、異なるグリッドには異なる占有率が与えられている。ある位置姿勢候補（ｘｉ，ｙｉ，θｉ）に対応する２次元グリッド地図上のマッチングスコアは、環境物の歪み除去点群データのうちの各歪み除去点群データを２次元グリッド地図上の異なるグリッドに投影した場合の、これら異なるグリッドの占有率の和であり、すなわち、環境物の歪み除去点群データのうちのいずれかの歪み除去点群データ（ｘj，ｙj，θj）を２次元グリッド地図上の位置が
であるグリッドに投影し、対応する占有率をＯjとすると、位置姿勢候補（ｘｉ，ｙｉ，θｉ）に対応して、当該環境物の歪み除去点群データを２次元グリッド地図に投影した場合の、２次元グリッド地図上のマッチングスコアは
であり、そのうち、ｍは、当該環境物の歪み除去点群データの歪み除去点群データの数である。
ステップＳ１０３３において、複数の位置姿勢候補のうち、２次元グリッド地図上のマッチングスコアが最も高い位置姿勢候補を、ロボットの２次元グリッド地図でのグローバル位置姿勢として確定する。
ステップＳ１０３２の分析から分かるように、上記のｎ個の位置姿勢候補について、ｎ個の異なるマッチングスコア
が得られ、ｎ個の位置姿勢候補のうち２次元グリッド地図上のマッチングスコアが最も高い位置姿勢候補を、ロボットの２次元グリッド地図でのグローバル位置姿勢として確定し、すなわち、
を選択し、
に対応する位置姿勢候補（ｘｋ，ｙｋ，θｋ）をロボットの２次元グリッド地図でのグローバル位置姿勢として確定し、当該グローバル位置姿勢をロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとする。
ステップＳ１０４において、道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、道路標識Ｌｉと道路標識Ｌjとの間の道路標識地図上の位置差値である誤差Ｅ１、および、ロボットの推定位置姿勢とグローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値である誤差Ｅ２が、最小となるまで、ロボットの位置姿勢を反復最適化する。
本発明の一実施例として、道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、誤差Ｅ１および誤差Ｅ２が最小となるまで、ロボットの位置姿勢を反復最適化することは、ロボットの位置姿勢を状態として非線形最小二乘法問題を構築し、誤差Ｅ１を非線形最小二乘法問題の誤差制約条件として、誤差Ｅ１および誤差Ｅ２が最小となるまで、ロボットの位置姿勢を反復最適化することであってもよく、ここで、誤差Ｅ１は、道路標識Ｌｉと道路標識Ｌjとの間の道路標識地図上の位置差値であり、誤差Ｅ２は、ロボットの推定位置姿勢とグローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値である。
本発明の別の一実施例として、道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、誤差Ｅ１および誤差Ｅ２が最小となるまで、ロボットの位置姿勢を反復最適化することは、さらに、道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、粒子フィルタモデルまたは拡張カルマンフィルタモデルを構築し、粒子フィルタモデルまたは拡張カルマンフィルタモデルを用いて、誤差Ｅ１および誤差Ｅ２が最小となるまで、ロボットの推定位置姿勢を補正し続けることであってもよく、ここでの誤差Ｅ１と誤差Ｅ２の意味は、前述した実施例で言及された誤差Ｅ１と誤差Ｅ２の意味と同じである。本発明の実施例において、拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ，ＥＫＦ）は、非線形の場合の標準のカルマンフィルタの拡張形態であり、効率の高い再帰型フィルタすなわち自己回帰フィルタであり、その基本的な考え方は、テイラー級数展開を用いて非線形システムを線形化し、次にカルマンフィルタフレームワークを用いてデータをフィルタリングすることである。粒子フィルタは、状態空間で伝搬する１セットのランダムサンプルを探すことにより確率密度関数を近似的に表し、積分運算の代わりにサンプル平均値を用いて、システム状態の最小分散推定を得る過程であり、これらのサンプルは、「粒子」と具象的に呼ばれる。
上記の道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、粒子フィルタモデルまたは拡張カルマンフィルタモデルを構築し、粒子フィルタモデルを用いて、誤差Ｅ１および誤差Ｅ２が最小となるまで、ロボットの推定位置姿勢を補正し続けることは、ロボットの推定位置姿勢と予め設定された時間帯における位置姿勢変化量に基づいて、現在粒子群を取得することであって、粒子群のうちの各々の粒子はロボットの１つの位置姿勢を表し、粒子群はロボットの位置姿勢の確率分布を表すものであることと、現在粒子群のうちの各々の粒子に重みを割り当て、割り当てられた重みに基づいてロボットの第１の推定位置姿勢を得ることであって、重みは、各々の粒子が現在実際の位置姿勢である確率を表すことと、第１の推定位置姿勢、道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、予め設定された走査マッチングモデルを用いて走査マッチングを行って、第２の推定位置姿勢を得ることと、誤差Ｅ１と誤差Ｅ２が最小となるまで、第２の推定位置姿勢を補正し続け、この時に得られた推定位置姿勢をロボットの最終位置姿勢とすることと、を含んでもよい。
上記の図１に例示した融合測位方法から分かるように、本発明の技術的解決手段において、一方では、環境物をサンプリングし、環境物のうちの道路標識Ｌｉの位置Ｐｉに最も近い道路標識Ｌjをマッチングし、道路標識Ｌｉと道路標識Ｌjとの間の道路標識地図上の位置差値Ｅ１を取得し、もう一方では、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの推定位置姿勢とロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値Ｅ２を取得し、最後に、誤差Ｅ１および誤差Ｅ２が最小となるまで、ロボットの位置姿勢を反復最適化する。道路標識のみで測位を補助する従来技術と比較すると、本発明の技術的解決手段は、道路標識測位と２次元グリッド地図測位を融合することで、ロボットの測位をより正確に行うことができる。
図２は、本発明の実施例によって提供される融合測位装置であり、当該融合測位装置は、第１の取得モジュール２０１と、第２の取得モジュール２０２と、第３の取得モジュール２０３と、反復最適化モジュール２０４と、を備えてもよく、詳しくは以下のとおりである。
第１の取得モジュール２０１は、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと環境物の歪み除去点群データとを含む、環境物のレーダ点群関連データを、取得する。
第２の取得モジュール２０２は、環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐｉに最も近い道路標識Ｌjの位置Ｐjを取得する。
第３の取得モジュール２０３は、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得する。
反復最適化モジュール２０４は、道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、道路標識Ｌｉと道路標識Ｌjとの間の道路標識地図上の位置差値である誤差Ｅ１、および、ロボットの推定位置姿勢とグローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値である誤差Ｅ２が、最小となるまで、ロボットの位置姿勢を反復最適化する。
代替的には、図２に例示された第１の取得モジュール２０１は、サンプリングユニットと、補正ユニットと、クラスタリングユニットと、中心求めユニットと、を備えてもよい。
サンプリングユニットは、２次元レーザレーダにより環境物をサンプリングして、環境物の原初レーダ点群データを得る。
補正ユニットは、原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、環境物の原初レーダ点群データに対して歪み除去を行って、環境物の歪み除去点群データを得る。
クラスタリングユニットは、環境物のうちの道路標識Ｌｉの歪み除去点群データをクラスタリングして、道路標識Ｌｉの点群クラスタを得る。
中心求めユニットは、道路標識Ｌｉの点群クラスタのロボット本体座標系での幾何学的中心座標を求め、道路標識Ｌｉの点群クラスタのロボット本体座標系での幾何学的中心座標を道路標識Ｌｉの道路標識地図上の位置Ｐｉとする。
代替的には、上記の補正ユニットは、新たな原点確定ユニットと、乗算ユニットと、を備えてもよい。
新たな原点確定ユニットは、１走査周期後の、２次元レーザレーダが位置するレーザレーダ座標系の新たな原点を確定する。
乗算ユニットは、相対的位置姿勢△Ｔと、原初レーダ点群データのうちの各点の旧原点のレーザレーダ座標系での座標Ｐとを乗算し、乗算後の結果△Ｔ*Ｐを原初レーダ点群データのうちの各点の座標とする。そのうち、相対的位置姿勢△Ｔは、原初レーダ点群データのうちの各点の、新たな原点に対して得られたロボット位置姿勢データである。
代替的には、図２に例示された装置は、比較モジュールと、選別モジュールと、をさらに備えてもよい。
比較モジュールは、環境物の歪み除去点群データのうちの各点に対応するレーザ光反射強度と予め設定された光強度閾値とを比較する。
選別モジュールは、環境物の歪み除去点群データのうちの、レーザ光反射強度が予め設定された光強度閾値以上である点群データを、環境物のうちの道路標識Ｌｉの歪み除去点群データとして保持する。
代替的には、図２に例示された第３の取得モジュール２０３は、検索ユニットと、マッチングユニットと、グローバル位置姿勢確定ユニット、を備えてもよい。
検索ユニットは、ロボットの現在時刻の前の時刻の位置姿勢から、位置姿勢検索空間における複数の位置姿勢候補を確定する。
マッチングユニットは、複数の位置姿勢候補のうちの各々の位置姿勢候補を利用して、環境物の歪み除去点群データを２次元グリッド地図に投影し、各々の位置姿勢候補の２次元グリッド地図上のマッチングスコアを算出する。
グローバル位置姿勢確定ユニットは、複数の位置姿勢候補のうち、２次元グリッド地図上のマッチングスコアが最も高い位置姿勢候補を、ロボットの２次元グリッド地図でのグローバル位置姿勢として確定する。
代替的には、図２に例示された反復最適化モジュール２０４は、問題構築ユニットと、第１の補正ユニットと、を備えてもよい。
構築ユニットは、ロボットの位置姿勢を状態として非線形最小二乘法問題を構築する。
第１の補正ユニットは、誤差Ｅ１を非線形最小二乘法問題の誤差制約条件として、誤差Ｅ１および誤差Ｅ２が最小となるまで、ロボットの位置姿勢を反復最適化する。
代替的には、図２に例示された反復最適化モジュール２０４は、モデル構築ユニットと、第２の補正ユニットと、を備えてもよい。
モデル構築ユニットは、道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、粒子フィルタモデルまたは拡張カルマンフィルタモデルを構築する。
第２の補正ユニットは、粒子フィルタモデルまたは拡張カルマンフィルタモデルを用いて、誤差Ｅ１および誤差Ｅ２が最小となるまで、ロボットの推定位置姿勢を補正し続ける。
以上の技術的解決手段の説明から分かるように、本発明の技術的解決手段において、一方では、環境物をサンプリングし、環境物のうちの道路標識Ｌｉの位置Ｐｉに最も近い道路標識Ｌjをマッチングし、道路標識Ｌｉと道路標識Ｌjとの間の道路標識地図上の位置差値Ｅ１を取得し、もう一方では、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの推定位置姿勢とロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値Ｅ２を取得し、最後に、誤差Ｅ１および誤差Ｅ２が最小となるまで、ロボットの位置姿勢を反復最適化する。道路標識のみで測位を補助する従来技術と比較すると、本発明の技術的解決手段は、道路標識測位と２次元グリッド地図測位を融合することで、ロボットの測位をより正確に行うことができる。
図３は、本発明の一実施例によって提供されるデバイスの構成模式図である。図３に示すように、当該実施例のデバイス３は、主に、プロセッサ３０と、メモリ３１と、メモリ３１に記憶され、プロセッサ３０上で実行可能の、融合測位方法のプログラムなどのコンピュータープログラム３２と、を備える。プロセッサ３０がコンピュータープログラム３２を実行する際に、上記の融合測位方法の実施例のステップが実現され、たとえば、図１に示すステップＳ１０１からＳ１０４が実現される。または、プロセッサ３０がコンピュータープログラム３２を実行する際に、上記の各装置の実施例の各モジュール/ユニットの機能が実現され、たとえば、図２に示す第１の取得モジュール２０１、第２の取得モジュール２０２、第３の取得モジュール２０３、および反復最適化モジュール２０４の機能が実現される。
例示的に、融合測位方法のコンピュータープログラム３２は、主に、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと環境物の歪み除去点群データとを含む、環境物のレーダ点群関連データを、取得することと、環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐｉに最も近い道路標識Ｌjの位置Ｐjを取得することと、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得することと、道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、道路標識Ｌｉと道路標識Ｌjとの間の道路標識地図上の位置差値である誤差Ｅ１、および、ロボットの推定位置姿勢とグローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値である誤差Ｅ２が、最小となるまで、ロボットの位置姿勢を反復最適化することと、を含む。コンピュータープログラム３２は、１つまたは複数のモジュール/ユニットに分割されてもよく、１つまたは複数のモジュール/ユニットは、メモリ３１に記憶され、プロセッサ３０により実行されることにより本発明を実施する。１つまたは複数のモジュール/ユニットは、特定機能を実行することができる一連のコンピュータープログラム命令セグメントであってもよく、当該命令セグメントは、コンピュータープログラム３２のデバイス３上の実行過程を記述したものであってもよい。たとえば、コンピュータープログラム３２は、第１の取得モジュール２０１と、第２の取得モジュール２０２と、第３の取得モジュール２０３と、反復最適化モジュール２０４（仮想装置中のモジュール）との機能に分割されてもよく、各モジュールの具体的な機能は以下のようであってもよい。第１の取得モジュール２０１は、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと環境物の歪み除去点群データとを含む、環境物のレーダ点群関連データを、取得する。第２の取得モジュール２０２は、環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐｉに最も近い道路標識Ｌjの位置Ｐjを取得する。第３の取得モジュール２０３は、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得する。反復最適化モジュール２０４は、道路標識Ｌｉの道路標識地図上の位置Ｐｉ、道路標識Ｌjの道路標識地図上の位置Ｐj、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、道路標識Ｌｉと道路標識Ｌjとの間の道路標識地図上の位置差値である誤差Ｅ１、および、ロボットの推定位置姿勢とグローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値である誤差Ｅ２が、最小となるまで、ロボットの位置姿勢を反復最適化する。
デバイス３は、プロセッサ３０とメモリ３１とを備えるが、これらに限定されない。当業者が理解するように、図３は単にデバイス３の例であり、デバイス３を限定するものではなく、図示されているよりも多いかまたは少ない構成要素、いくつかの構成要素の組み合わせ、または、異なる構成要素を備えてもよく、たとえば、コンピューティングデバイスは、入力／出力デバイス、ネットワークアクセスデバイス、バスなどを備えることができる。
プロセッサ３０とは、中央処理ユニット（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ，ＣＰＵ）であってもよく、他の汎用プロセッサ、デジタル信号プロセッサ （Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ，ＤＳＰ）、専用集積回路（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ，ＡＳＩＣ）、フィールドプログラマブルゲートアレイ（Ｆｉｅｌｄ-Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ，ＦＰＧＡ）または他のプログラマブルロジックデバイス、分離ゲートまたはトランジスタロジックデバイス、分離ハードウェアコンポーネントなどであってもよい。汎用プロセッサは、マイクロプロセッサであってもよく、或いは、当該プロセッサは従来のいずれかのプロセッサなどであってもよい。
メモリ３１とは、デバイス３のハードディスクまたは内部メモリなどの、デバイス３の内部記憶ユニットであってもよい。メモリ３１は、デバイス３に搭載されたプラグインハードディスク、スマートメモリカード（Ｓｍａｒｔ Ｍｅｄｉａ Ｃａｒｄ，ＳＭＣ）、セキュアデジタル（Ｓｅｃｕｒｅ Ｄｉｇｉｔａｌ，ＳＤ）カード、フラッシュメモリカード（Ｆｌａｓｈ Ｃａｒｄ）などの、デバイス３の外部記憶デバイスであってもよい。さらに、メモリ３１は、デバイス３の内部記憶ユニットと外部記憶デバイスの両方を備えてもよい。メモリ３１は、コンピュータープログラム、および、デバイスに必要な他のプログラムとデータを記憶するためのものである。メモリ３１は、出力されたデータまたは出力しようとするデータを一時的に記憶するためのものであってもよい。
以上説明した実施例の各技術特徴は任意に組み合わせることができ、説明を簡潔にするために、上記の実施例中の各技術特徴のすべての可能な組み合わせを記載していないが、これらの技術特徴の組み合わせは矛盾しない限り、いずれも本明細書に記載された範囲としてみなすべきである。
以上説明した実施例は、本発明の幾つかの実施形態を示したにすぎず、その記載は具体的かつ詳細なものであるが、特許請求の範囲を限定するものではない。当業者であれば、本願の思想から逸脱することなく、複数の変形や改良を加えることが可能であり、これらはいずれも本発明の保護範囲に属することを留意すべきである。したがって、本出願の特許請求の保護範囲は、添付の特許請求の範囲に準ずるものとする。

Claims (20)

  1. 融合測位方法であって、
    ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと前記環境物の歪み除去点群データとを含む、前記環境物のレーダ点群関連データを、取得することと、
    前記道路標識Ｌｉの道路標識地図上の位置Ｐｉと道路標識地図とを近傍マッチングし、前記道路標識地図上の前記位置Ｐｉに最も近い道路標識Ｌjの位置Ｐjを取得することと、
    前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得することと、
    前記道路標識Ｌｉの道路標識地図上の位置Ｐｉ、前記道路標識Ｌjの前記道路標識地図上の位置Ｐj、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、前記道路標識Ｌｉと前記道路標識Ｌjの前記道路標識地図上の位置差値である誤差Ｅ１、および、前記ロボットの推定位置姿勢と前記グローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値である誤差Ｅ２が、最小となるまで、前記ロボットの位置姿勢を反復最適化することと、を含む
    ことを特徴とする前記融合測位方法。
  2. 前記ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記環境物のレーダ点群関連データを取得することは、
    前記２次元レーザレーダにより前記環境物をサンプリングし、前記環境物の原初レーダ点群データを得ることと、
    前記原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、前記環境物の原初レーダ点群データに対して歪み除去を行って、前記環境物の歪み除去点群データを得ることと、
    前記環境物のうちの道路標識Ｌｉの歪み除去点群データをクラスタリングして、前記道路標識Ｌｉの点群クラスタを得ることと、
    前記点群クラスタの前記ロボット本体座標系での幾何学的中心座標を求め、前記点群クラスタの前記ロボット本体座標系での幾何学的中心座標を前記道路標識Ｌｉの道路標識地図上の位置Ｐｉとすることと、を含む
    ことを特徴とする請求項１に記載の融合測位方法。
  3. 前記原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、前記環境物の原初レーダ点群データに対して歪み除去を行って、前記環境物の歪み除去点群データを得ることは、
    １走査周期後の、前記２次元レーザレーダが位置するレーザレーダ座標系の新たな原点を確定することと、
    相対的位置姿勢△Ｔと、前記原初レーダ点群データのうちの各点の旧原点のレーザレーダ座標系での座標Ｐとを乗算し、乗算後の結果△Ｔ*Ｐを前記原初レーダ点群データのうちの各点の座標とすることであって、前記相対的位置姿勢△Ｔは、前記原初レーダ点群データのうちの各点の、前記新たな原点に対して得られたロボット位置姿勢データであることと、を含む
    ことを特徴とする請求項２に記載の融合測位方法。
  4. 前記環境物の原初レーダ点群データに対して歪み除去を行って、前記環境物の歪み除去点群データを得ることの後に、
    前記融合測位方法は、
    前記環境物の歪み除去点群データをのうちの各点に対応するレーザ光反射強度と予め設定された光強度閾値とを比較することと、
    前記環境物の歪み除去点群データのうちのレーザ光反射強度が前記予め設定された光強度閾値以上である点群データを、前記環境物のうちの道路標識Ｌｉの歪み除去点群データとして保持することと、をさらに含む
    ことを特徴とする請求項２に記載の融合測位方法。
  5. 前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得することは、
    前記ロボットの現在時刻の前の時刻の位置姿勢から、位置姿勢検索空間における複数の位置姿勢候補を確定することと、
    前記複数の位置姿勢候補のうちの各々の位置姿勢候補を利用して、前記環境物の歪み除去点群データを前記２次元グリッド地図に投影し、前記各々の位置姿勢候補の前記２次元グリッド地図上のマッチングスコアを算出することと、
    前記複数の位置姿勢候補のうち、前記２次元グリッド地図上のマッチングスコアが最も高い位置姿勢候補を前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢として確定することと、を含む
    ことを特徴とする請求項１に記載の融合測位方法。
  6. 前記道路標識Ｌｉの道路標識地図上の位置Ｐｉ、前記道路標識Ｌjの前記道路標識地図上の位置Ｐj、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、誤差Ｅ１および誤差Ｅ２が最小となるまで、前記ロボットの位置姿勢を反復最適化することは、
    前記ロボットの位置姿勢を状態として非線形最小二乘法問題を構築することと、
    前記誤差Ｅ１を前記非線形最小二乘法問題の誤差制約条件として、前記誤差Ｅ１および前記誤差Ｅ２が最小となるまで、前記ロボットの位置姿勢を反復最適化することと、を含む
    ことを特徴とする請求項１に記載の融合測位方法。
  7. 前記道路標識Ｌｉの道路標識地図上の位置Ｐｉ、前記道路標識Ｌjの前記道路標識地図上の位置Ｐj、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、誤差Ｅ１および誤差Ｅ２が最小となるまで、前記ロボットの位置姿勢を反復最適化することは、
    前記道路標識Ｌｉの道路標識地図上の位置Ｐｉ、前記道路標識Ｌjの前記道路標識地図上の位置Ｐj、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、粒子フィルタモデルまたは拡張カルマンフィルタモデルを構築することと、
    前記粒子フィルタモデルまたは拡張カルマンフィルタモデルを用いて、誤差Ｅ１および誤差Ｅ２が最小となるまで、前記ロボットの推定位置姿勢を補正し続けることと、を含む
    ことを特徴とする請求項１に記載の融合測位方法。
  8. 融合測位装置であって、
    ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記環境物のうちの道路標識Ｌｉの道路標識地図上の位置Ｐｉと前記環境物の歪み除去点群データとを含む、前記環境物のレーダ点群関連データを、取得するための第１の取得モジュールと、
    前記道路標識Ｌｉの道路標識地図上の位置Ｐｉと道路標識地図とを近傍マッチングし、前記道路標識地図上の前記位置Ｐｉに最も近い道路標識Ｌjの位置Ｐjを取得するための第２の取得モジュールと、
    前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得するための第３の取得モジュールと、
    前記道路標識Ｌｉの道路標識地図上の位置Ｐｉ、前記道路標識Ｌjの前記道路標識地図上の位置Ｐj、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、前記道路標識Ｌｉと前記道路標識Ｌjの前記道路標識地図上の位置差値である誤差Ｅ１、および、前記ロボットの推定位置姿勢と前記グローバル位置姿勢観測値Ｇｐｏｓｅとの間の差値である誤差Ｅ２が、最小となるまで、前記ロボットの位置姿勢を反復最適化するための反復最適化モジュールと、を備える
    ことを特徴とする前記融合測位装置。
  9. 前記第１の取得モジュールは、
    前記２次元レーザレーダにより前記環境物をサンプリングし、前記環境物の原初レーダ点群データを得るためのサンプリングユニットと、
    前記原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、前記環境物の原初レーダ点群データに対して歪み除去を行って、前記環境物の歪み除去点群データを得るための補正ユニットと、
    前記環境物のうちの道路標識Ｌｉの歪み除去点群データをクラスタリングして、前記道路標識Ｌｉの点群クラスタを得るためのクラスタリングユニットと、
    前記点群クラスタの前記ロボット本体座標系での幾何学的中心座標を求め、前記点群クラスタの前記ロボット本体座標系での幾何学的中心座標を前記道路標識Ｌｉの道路標識地図上の位置Ｐｉとするための中心求めユニットと、を備える
    ことを特徴とする請求項８に記載の融合測位装置。
  10. 前記補正ユニットは、
    １走査周期後の、前記２次元レーザレーダが位置するレーザレーダ座標系の新たな原点を確定するための新たな原点確定ユニットと、
    相対的位置姿勢△Ｔと前記原初レーダ点群データのうちの各点の旧原点のレーザレーダ座標系での座標Ｐとを乗算し、乗算後の結果△Ｔ*Ｐを前記原初レーダ点群データのうちの各点の座標とするための乗算ユニットであって、前記相対的位置姿勢△Ｔは、前記原初レーダ点群データのうちの各点の、前記新たな原点に対して得られたロボット位置姿勢データである乗算ユニットと、を備える
    ことを特徴とする請求項９に記載の融合測位装置。
  11. 前記環境物の歪み除去点群データのうちの各点に対応するレーザ光反射強度と予め設定された光強度閾値とを比較するための比較モジュールと、
    前記環境物の歪み除去点群データのうちのレーザ光反射強度が前記予め設定された光強度閾値以上である点群データを、前記環境物のうちの道路標識Ｌｉの歪み除去点群データとして保持するための選別モジュールと、をさらに備える
    ことを特徴とする請求項９に記載の融合測位装置。
  12. 前記第３の取得モジュールは、
    前記ロボットの現在時刻の前の時刻の位置姿勢から、位置姿勢検索空間における複数の位置姿勢候補を確定するための検索ユニットと、
    前記複数の位置姿勢候補のうちの各々の位置姿勢候補を利用して、前記環境物の歪み除去点群データを前記２次元グリッド地図に投影し、前記各々の位置姿勢候補の前記２次元グリッド地図上のマッチングスコアを算出するためのマッチングユニットと、
    前記複数の位置姿勢候補のうち、前記２次元グリッド地図上のマッチングスコアが最も高い位置姿勢候補を前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢として確定するためのグローバル位置姿勢確定ユニットと、を備える
    ことを特徴とする請求項８に記載の融合測位装置。
  13. 前記反復最適化モジュールは、
    前記ロボットの位置姿勢を状態として非線形最小二乘法問題を構築するための構築ユニットと、
    前記誤差Ｅ１を前記非線形最小二乘法問題の誤差制約条件として、前記誤差Ｅ１および前記誤差Ｅ２が最小となるまで、前記ロボットの位置姿勢を反復最適化するための第１の補正ユニットと、を備える
    ことを特徴とする請求項８に記載の融合測位装置。
  14. 前記反復最適化モジュールは、
    前記道路標識Ｌｉの道路標識地図上の位置Ｐｉ、前記道路標識Ｌjの前記道路標識地図上の位置Ｐj、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇｐｏｓｅに基づいて、粒子フィルタモデルまたは拡張カルマンフィルタモデルを構築するためのモデル構築ユニットと、
    前記粒子フィルタモデルまたは拡張カルマンフィルタモデルを用いて、誤差Ｅ１および誤差Ｅ２が最小となるまで、前記ロボットの推定位置姿勢を補正し続けるための第２の補正ユニットと、を備える
    ことを特徴とする請求項８に記載の融合測位装置。
  15. デバイスであって、
    メモリと、プロセッサと、前記メモリに記憶され、前記プロセッサ上で実行可能なコンピュータープログラムと、を備え、
    前記プロセッサが前記コンピュータープログラムを実行することにより、請求項１に記載の方法のステップが実現される
    ことを特徴とする前記デバイス。
  16. 前記ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記環境物のレーダ点群関連データを取得することは、
    前記２次元レーザレーダにより前記環境物をサンプリングし、前記環境物の原初レーダ点群データを得ることと、
    前記原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、前記環境物の原初レーダ点群データに対して歪み除去を行って、前記環境物の歪み除去点群データを得ることと、
    前記環境物のうちの道路標識Ｌｉの歪み除去点群データをクラスタリングして、前記道路標識Ｌｉの点群クラスタを得ることと、
    前記点群クラスタの前記ロボット本体座標系での幾何学的中心座標を求め、前記点群クラスタの前記ロボット本体座標系での幾何学的中心座標を前記道路標識Ｌｉの道路標識地図上の位置Ｐｉとすることと、を含む
    ことを特徴とする請求項１５に記載のデバイス。
  17. 前記原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、前記環境物の原初レーダ点群データに対して歪み除去を行って、前記環境物の歪み除去点群データを得ることは、
    １走査周期後の、前記２次元レーザレーダが位置するレーザレーダ座標系の新たな原点を確定することと、
    相対的位置姿勢△Ｔと、前記原初レーダ点群データのうちの各点の旧原点のレーザレーダ座標系での座標Ｐとを乗算し、乗算後の結果△Ｔ*Ｐを前記原初レーダ点群データのうちの各点の座標とすることであって、前記相対的位置姿勢△Ｔは、前記原初レーダ点群データのうちの各点の、前記新たな原点に対して得られたロボット位置姿勢データであることと、を含む
    ことを特徴とする請求項１６に記載のデバイス。
  18. 前記環境物の原初レーダ点群データに対して歪み除去を行って、前記環境物の歪み除去点群データを得ることの後に、
    前記プロセッサは、
    前記環境物の歪み除去点群データをのうちの各点に対応するレーザ光反射強度と予め設定された光強度閾値とを比較することと、
    前記環境物の歪み除去点群データのうちのレーザ光反射強度が前記予め設定された光強度閾値以上である点群データを、前記環境物のうちの道路標識Ｌｉの歪み除去点群データとして保持することと、をさらに実行する
    ことを特徴とする請求項１６に記載のデバイス。
  19. 前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇｐｏｓｅとして取得することは、
    前記ロボットの現在時刻の前の時刻の位置姿勢から、位置姿勢検索空間における複数の位置姿勢候補を確定することと、
    前記複数の位置姿勢候補のうちの各々の位置姿勢候補を利用して、前記環境物の歪み除去点群データを前記２次元グリッド地図に投影し、前記各々の位置姿勢候補の前記２次元グリッド地図上のマッチングスコアを算出することと、
    前記複数の位置姿勢候補のうち、前記２次元グリッド地図上のマッチングスコアが最も高い位置姿勢候補を前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢として確定することと、を含む
    ことを特徴とする請求項１５に記載のデバイス。
  20. コンピュータープログラムが記憶されるコンピューター可読記憶媒体であって、
    前記コンピュータープログラムがプロセッサによって実行されることにより、請求項１に記載の方法のステップが実現される
    ことを特徴とするコンピューター可読記憶媒体。
JP2023521741A 2020-12-29 2021-12-02 融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体 Pending JP2023545279A (ja)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
CN202011586133.3A CN112764053B (zh) 2020-12-29 2020-12-29 一种融合定位方法、装置、设备和计算机可读存储介质
CN202011586133.3 2020-12-29
PCT/CN2021/135015 WO2022142992A1 (zh) 2020-12-29 2021-12-02 融合定位方法、装置、设备和计算机可读存储介质

Publications (1)

Publication Number Publication Date
JP2023545279A true JP2023545279A (ja) 2023-10-27

Family

ID=75696343

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2023521741A Pending JP2023545279A (ja) 2020-12-29 2021-12-02 融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体

Country Status (5)

Country Link
US (1) US20230400585A1 (ja)
EP (1) EP4206736A1 (ja)
JP (1) JP2023545279A (ja)
CN (1) CN112764053B (ja)
WO (1) WO2022142992A1 (ja)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112764053B (zh) * 2020-12-29 2022-07-15 深圳市普渡科技有限公司 一种融合定位方法、装置、设备和计算机可读存储介质
CN114643579B (zh) * 2022-03-29 2024-01-16 深圳优地科技有限公司 一种机器人定位方法、装置、机器人及存储介质
CN115127559A (zh) * 2022-06-28 2022-09-30 广东利元亨智能装备股份有限公司 一种定位方法、装置、设备和存储介质
CN117367419A (zh) * 2022-06-29 2024-01-09 深圳市海柔创新科技有限公司 机器人定位方法、装置和计算可读存储介质
CN115183778A (zh) * 2022-07-01 2022-10-14 北京斯年智驾科技有限公司 一种基于码头石墩的建图方法、装置、设备以及介质
CN115290098B (zh) * 2022-09-30 2022-12-23 成都朴为科技有限公司 一种基于变步长的机器人定位方法和系统
CN115307646B (zh) * 2022-10-08 2023-03-24 浙江光珀智能科技有限公司 一种多传感器融合的机器人定位方法、系统及装置
CN115686011A (zh) * 2022-10-31 2023-02-03 广州高新兴机器人有限公司 机器人沿栅栏沿边行走的算法、系统及电子设备
CN115421125B (zh) * 2022-11-07 2023-01-10 山东富锐光学科技有限公司 一种基于数据融合的雷达点云数据惯性校正方法
CN116164747B (zh) * 2022-12-15 2023-09-05 广东智能无人系统研究院(南沙) 一种水下机器人的定位与导航方法及系统
CN117570998B (zh) * 2024-01-17 2024-04-02 山东大学 基于反光柱信息的机器人定位方法及系统

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106908775B (zh) * 2017-03-08 2019-10-18 同济大学 一种基于激光反射强度的无人车实时定位方法
CN108732603B (zh) * 2017-04-17 2020-07-10 百度在线网络技术（北京）有限公司 用于定位车辆的方法和装置
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
CN109270544A (zh) * 2018-09-20 2019-01-25 同济大学 基于杆状物识别的移动机器人自定位系统
US20200301015A1 (en) * 2019-03-21 2020-09-24 Foresight Ai Inc. Systems and methods for localization
CN110221603B (zh) * 2019-05-13 2020-08-14 浙江大学 一种基于激光雷达多帧点云融合的远距离障碍物检测方法
CN110487286B (zh) * 2019-08-09 2022-12-20 上海电器科学研究所（集团）有限公司 基于点特征投影与激光点云融合的机器人位姿判断方法
CN110927740B (zh) * 2019-12-06 2023-09-08 合肥科大智能机器人技术有限公司 一种移动机器人定位方法
CN111045017B (zh) * 2019-12-20 2023-03-31 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
CN111060092B (zh) * 2019-12-31 2021-07-30 芜湖哈特机器人产业技术研究院有限公司 一种反光柱的快速匹配方法
CN111781609B (zh) * 2020-04-10 2022-11-29 昆山同日智能技术有限公司 一种agv激光导航多边定位方法
CN111812669B (zh) * 2020-07-16 2023-08-04 南京航空航天大学 绕机检查装置及其定位方法、存储介质
CN112764053B (zh) * 2020-12-29 2022-07-15 深圳市普渡科技有限公司 一种融合定位方法、装置、设备和计算机可读存储介质

Also Published As

Publication number Publication date
CN112764053A (zh) 2021-05-07
CN112764053B (zh) 2022-07-15
US20230400585A1 (en) 2023-12-14
EP4206736A1 (en) 2023-07-05
WO2022142992A1 (zh) 2022-07-07

Similar Documents

Publication Publication Date Title
JP2023545279A (ja) 融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体
CN111536964B (zh) 机器人定位方法及装置、存储介质
CN109521403B (zh) 多线激光雷达的参数标定方法及装置、设备及可读介质
CN108871353B (zh) 路网地图生成方法、系统、设备及存储介质
CN108732582B (zh) 车辆定位方法和装置
CN111508021B (zh) 一种位姿确定方法、装置、存储介质及电子设备
CN111060888B (zh) 一种融合icp和似然域模型的移动机器人重定位方法
JP5430456B2 (ja) 幾何特徴抽出装置、幾何特徴抽出方法、及びプログラム、三次元計測装置、物体認識装置
CN112847343B (zh) 动态目标跟踪定位方法、装置、设备和存储介质
US10627520B2 (en) Method and apparatus for constructing reflectance map
CN108267141B (zh) 道路点云数据处理系统
WO2022000260A1 (zh) 地图的更新方法、装置、可移动平台及存储介质
CN108734780B (zh) 用于生成地图的方法、装置和设备
Serafin et al. Using extended measurements and scene merging for efficient and robust point cloud registration
CN114200430A (zh) 激光雷达与相机的标定方法、系统、设备及存储介质
JP6230442B2 (ja) 算出装置、方法及びプログラム
CN112462769A (zh) 机器人定位方法、装置、计算机可读存储介质及机器人
WO2004023394A1 (en) Environmental reasoning using geometric data structure
CN113012063A (zh) 一种动态点云修复方法、装置及计算机设备
CN113459088B (zh) 地图调整方法、电子设备及存储介质
CN115265519A (zh) 一种在线点云地图构建方法、装置
CN111815684B (zh) 基于统一残差模型的空间多元特征配准优化方法及装置
CN112381873A (zh) 一种数据标注方法及装置
CN114266876B (zh) 定位方法、视觉地图的生成方法及其装置
JP2020086751A (ja) マップ生成装置、および、マップ生成方法

Legal Events

Date Code Title Description
A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20230410

A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20230410