JP7141403B2 - 実時間オンライン自己運動推定を備えたレーザスキャナ - Google Patents

実時間オンライン自己運動推定を備えたレーザスキャナ Download PDF

Info

Publication number
JP7141403B2
JP7141403B2 JP2019540661A JP2019540661A JP7141403B2 JP 7141403 B2 JP7141403 B2 JP 7141403B2 JP 2019540661 A JP2019540661 A JP 2019540661A JP 2019540661 A JP2019540661 A JP 2019540661A JP 7141403 B2 JP7141403 B2 JP 7141403B2
Authority
JP
Japan
Prior art keywords
data
imu
frequency
scan
mapping system
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.)
Active
Application number
JP2019540661A
Other languages
English (en)
Other versions
JP2020507072A (ja
Inventor
ジー ジャン
ケヴィン ジョセフ ダウリング
サンジヴ シング
Original Assignee
カールタ インコーポレイテッド
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
Priority claimed from PCT/US2017/021120 external-priority patent/WO2017155970A1/en
Priority claimed from PCT/US2017/055938 external-priority patent/WO2018071416A1/en
Application filed by カールタ インコーポレイテッド filed Critical カールタ インコーポレイテッド
Publication of JP2020507072A publication Critical patent/JP2020507072A/ja
Application granted granted Critical
Publication of JP7141403B2 publication Critical patent/JP7141403B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/02—Picture taking arrangements specially adapted for photogrammetry or photographic surveying, e.g. controlling overlapping of pictures
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • 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/481—Constructional features, e.g. arrangements of optical elements
    • G01S7/4811—Constructional features, e.g. arrangements of optical elements common to transmitter and receiver
    • G01S7/4813—Housing arrangements
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/20—Control system inputs
    • G05D1/24—Arrangements for determining position or orientation
    • G05D1/242—Means based on the reflection of waves generated by the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/20—Control system inputs
    • G05D1/24—Arrangements for determining position or orientation
    • G05D1/245—Arrangements for determining position or orientation using dead reckoning
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/20—Control system inputs
    • G05D1/24—Arrangements for determining position or orientation
    • G05D1/246—Arrangements for determining position or orientation using environment maps, e.g. simultaneous localisation and mapping [SLAM]
    • G05D1/2465—Arrangements for determining position or orientation using environment maps, e.g. simultaneous localisation and mapping [SLAM] using a 3D model of the environment
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2105/00—Specific applications of the controlled vehicles
    • G05D2105/80—Specific applications of the controlled vehicles for information gathering, e.g. for academic research
    • G05D2105/87—Specific applications of the controlled vehicles for information gathering, e.g. for academic research for exploration, e.g. mapping of an area
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2107/00—Specific environments of the controlled vehicles
    • G05D2107/60—Open buildings, e.g. offices, hospitals, shopping areas or universities
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2109/00—Types of controlled vehicles
    • G05D2109/20—Aircraft, e.g. drones
    • G05D2109/25—Rotorcrafts
    • G05D2109/254—Flying platforms, e.g. multicopters
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/10—Optical signals
    • G05D2111/17—Coherent light, e.g. laser signals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/50—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors
    • G05D2111/52—Internal signals, i.e. from sensors located in the vehicle, e.g. from compasses or angular sensors generated by inertial navigation means, e.g. gyroscopes or accelerometers
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D2111/00—Details of signals used for control of position, course, altitude or attitude of land, water, air or space vehicles
    • G05D2111/60—Combination of two or more signals
    • G05D2111/63—Combination of two or more signals of the same type, e.g. stereovision or optical flow
    • G05D2111/65—Combination of two or more signals of the same type, e.g. stereovision or optical flow taken successively, e.g. visual odometry or optical flow

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Multimedia (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Image Analysis (AREA)

Description

本発明は、モバイルマッピングシステムに関する。
（優先権の主張）
この出願は、２０１７年１０月１０日出願の「実時間オンライン自己運動推定を備えたレーザスキャナ（ＬＡＳＥＲ ＳＣＡＮＮＥＲ ＷＩＴＨ ＲＥＡＬ－ＴＩＭＥ，ＯＮＬＩＮＥ ＥＧＯ－ＭＯＴＩＯＮ ＥＳＴＩＭＡＴＩＯＮ）」という名称のＰＣＴ出願番号ＰＣＴ／ＵＳ２０１７／０５５９３８（代理人整理番号ＫＲＴＡ－０００８－ＷＯ）に対する優先権を主張し、かつその一部継続出願である。
ＰＣＴ出願番号ＰＣＴ／ＵＳ２０１７／０５５９３８は、２０１７年３月７日出願の「実時間オンライン自己運動推定を備えたレーザスキャナ（ＬＡＳＥＲ ＳＣＡＮＮＥＲ ＷＩＴＨ ＲＥＡＬ－ＴＩＭＥ，ＯＮＬＩＮＥ ＥＧＯ－ＭＯＴＩＯＮ ＥＳＴＩＭＡＴＩＯＮ）」という名称のＰＣＴ出願番号ＰＣＴ／ＵＳ２０１７／０２１１２０（代理人整理番号ＫＲＴＡ－０００５－ＷＯ）に対する優先権を主張し、かつその一部継続出願である。この出願は、同様に、ＰＣＴ出願番号ＰＣＴ／ＵＳ２０１７／０２１１２０に対する優先権を主張するものである。ＰＣＴ出願番号ＰＣＴ／ＵＳ２０１７／０５５９３８は、２０１６年１０月１１日出願の「実時間オンライン自己運動推定を備えたレーザスキャナ（ＬＡＳＥＲ ＳＣＡＮＮＥＲ ＷＩＴＨ ＲＥＡＬ－ＴＩＭＥ，ＯＮＬＩＮＥ ＥＧＯ－ＭＯＴＩＯＮ ＥＳＴＩＭＡＴＩＯＮ）」という名称の米国特許仮出願第６２／４０６，９１０号（代理人整理番号ＫＲＴＡ－０００２－Ｐ０２）に対する優先権を更に主張するものである。
ＰＣＴ出願番号ＰＣＴ／ＵＳ２０１７／０２１１２０は、２０１６年３月１１日出願の「実時間オンライン自己運動推定を備えたレーザスキャナ（ＬＡＳＥＲ ＳＣＡＮＮＥＲ ＷＩＴＨ ＲＥＡＬ－ＴＩＭＥ，ＯＮＬＩＮＥ ＥＧＯ－ＭＯＴＩＯＮ ＥＳＴＩＭＡＴＩＯＮ）」という名称の米国特許仮出願第６２／３０７，０６１号（代理人整理番号ＫＲＴＡ－０００１－Ｐ０１）の利益を主張するものである。
この出願は、２０１７年１月２７日出願の「ライダー及び視覚ベースの自己運動推定及びマッピング（ＬＩＤＡＲ ＡＮＤ ＶＩＳＩＯＮ－ＢＡＳＥＤ ＥＧＯ－ＭＯＴＩＯＮ ＥＳＴＩＭＡＴＩＯＮ ＡＮＤ ＭＡＰＰＩＮＧ）」という名称の米国特許仮出願第６２／４５１，２９４号（代理人整理番号ＫＲＴＡ－０００４－Ｐ０１）に対する優先権を更に主張するものである。
（関連出願の相互参照）
ＰＣＴ／ＵＳ２０１７／０５５９３８、ＰＣＴ／ＵＳ２０１７／０２１１２０、及び米国特許仮出願第６２／４０６，９１０号明細書、第６２／３０７，０６１号明細書、及び第６２／４５１，２９４号明細書の開示は、それらの全体が全ての目的に対して引用によって本明細書に組み込まれている。
自律移動デバイスは、それがそこで作動する地形に関する情報を必要とする場合がある。そのようなデバイスは、地形を提示する予め定められたマップ及びそこに見出される場合があるあらゆる障害物のいずれかに依存する場合がある。これに代えて、デバイスは、実時間データを提供する１又は２以上のセンサを有するコンピュータベースのマッピングシステムを含む静止又は運動中である間にその地形をマッピングする機能を有する場合がある。モバイルコンピュータベースマッピングシステムは、その経時的な位置の変化を推定する（オドメータ）及び／又は３次元空間の点クラウドのような３次元マップ表現を発生させることができる。
例示的マッピングシステムは、マップをそこから構築することができるデータを提供する様々なセンサを含むことができる。一部のマッピングシステムは、１つのそのようなセンサとして立体カメラシステムを用いる場合がある。これらのシステムは、運動推定のスケールを決定するための基準としての２つのカメラ間のベースラインから利益を得ている。双眼システムは、単眼システムよりも好ましく、その理由は、単眼システムは、追加のセンサからデータを受信する又はデバイスの運動に関する仮定を行うことなしには画像のスケールを決定することができない場合があるからである。近年では、研究分野においてＲＧＢ－Ｄカメラが一般的になってきている。そのようなカメラは、個々のピクセルに関連付けられた深度情報を提供することができ、従って、スケールを決定するのを助けることができる。しかし、ＲＧＢ－Ｄカメラを含む一部の方法は、深度情報のカバレージを有する画像区域のみを用いることができ、その結果、特に深度を疎に取得することのみができる開放空間では、大きい画像区域が無駄になる場合がある。
マッピングシステムの他の例では、ＩＭＵは、スケール制約をＩＭＵ加速から与えることができるように１又は２以上のカメラに結合される場合がある。一部の例では、単眼カメラは、カルマンフィルタを用いてＩＭＵに緊密に又は緩く結合することができる。他のマッピングシステムは、モバイルシステムの運動を解くために最適化方法を用いる場合がある。
マッピングシステムの代替例は、運動推定のためのレーザスキャナの使用を含むことができる。しかし、レーザの走査速度からそのようなデータの使用の難しさが生じる場合がある。システムが移動している間に、固定位置レーザスキャナとは異なり、レーザ点は、スキャナの相対移動による影響を受ける。従って、この移動の影響は、到達するレーザ点が異なる時間にシステムに到達する要因である場合がある。その結果、走査速度がマッピングシステムの運動に対して低速である時に、レーザの外部運動に起因して走査歪みが存在する場合がある。この運動の効果は、レーザ自体によって補償することができるが、この補償は、必要とされる補正を与えるために独立運動モデルを必要とする場合がある。一例として、運動は、一定速度として又はガウス過程としてモデル化することができる。一部の例では、ＩＭＵは、運動モデルを与えることができる。そのような方法は、センサ運動を推定するためにレーザ点クラウドによって形成された空間－時間パッチを整合させ、オフラインバッチ最適化でＩＭＵバイアスを補正する。
運動歪みの同様の問題は、ローリングシャッターカメラの使用時に見出される場合がある。特に、画像ピクセルは、経時的に連続的に受信される場合があり、カメラの外来的な運動によって引き起こされる画像歪みをもたらす。一部の例では、視覚オドメトリ方法は、ピクセル読み出し時間を所与として、ＩＭＵを用いてローリングシャッター効果を補償することができる。
一部の例では、モバイルマッピングデバイスの位置を決定するためにＧＰＳ／ＩＮＳ技術を用いることができる。しかし、高精度ＧＰＳ／ＩＮＳソリューションは、用途がＧＰＳ拒絶、軽量、又はコスト重視である時に実際的でない場合がある。正確なＧＰＳマッピングは、ＧＰＳ受信器と少なくとも４つのＧＰＳ衛星（５つが好ましいと考えられるが）間に見通し通信を必要とすることが認識されている。一部の環境では、例えば、高架道路及び他の障害物を含む場合ある都市環境では、４つの衛星から歪みのない信号を受信することが困難である場合がある。
すなわち、特にマッピングデバイスが運動中である間に自律マッピングデバイスを取り囲む地形のロバストマップを発生させるために光学デバイスからのデータを他の運動測定デバイスと融合することに関連付けられたいくつかの技術的課題が存在することが認められるであろう。以下で開示するのは、光学マッピング情報を取得し、かつ歪みが低減したロバストマップを生成することができるマッピングデバイスの方法及びシステムである。
例示的かつ非限定的実施形態により、方法は、ＩＭＵデバイスからデータを第１の計算モジュールで第１の周波数で受信して受信ＩＭＵデータに少なくとも部分的に基づいてモバイルマッピングシステムの第１の推定位置を計算する段階と、第１の推定位置及び視覚－慣性データを第２の計算モジュールで第２の周波数で受信して第１の推定位置及び視覚－慣性データに少なくとも部分的に基づいてモバイルマッピングシステムの第２の推定位置を計算する段階と、第２の推定位置及びレーザ走査データを第３の計算モジュールで第３の周波数で受信して第２の推定位置及びレーザ走査データに少なくとも部分的に基づいてモバイルマッピングシステムの第３の推定位置を計算する段階とを含む。
別の例示的かつ非限定的実施形態により、モバイルマッピングシステムは、ＩＭＵデバイスから第１の周波数でデータを受信して受信ＩＭＵデータに少なくとも部分的に基づいてモバイルマッピングシステムの第１の推定位置を計算するようになった第１の計算モジュールと、第１の推定位置及び視覚－慣性データを第２の周波数で受信して第１の推定位置及び視覚－慣性データに少なくとも部分的に基づいてモバイルマッピングシステムの第２の推定位置を計算するようになった第２の計算モジュールと、第２の推定位置及びレーザ走査データを第３の周波数で受信して第２の推定位置及びレーザ走査データに少なくとも部分的に基づいてモバイルマッピングシステムの第３の推定位置を計算するようになった第３の計算モジュールとを含む。
マッピングシステムの実施形態のブロック図である。 図１のマッピングシステムの３つの計算モジュール及びそのそれぞれのフィードバック特徴のブロック図の実施形態を示す図である。 位置情報をマップに精緻化するためのカルマンフィルタモデルの実施形態を示す図である。 位置情報をマップに精緻化するための因子グラフ最適化モデルの実施形態を示す図である。 視覚－慣性オドメトリサブシステムの実施形態を示す図である。 走査整合サブシステムの実施形態を示す図である。 粗な詳細解像度を有する大域マップの実施形態を示す図である。 繊細詳細解像度を有する小域マップの実施形態を示す図である。 マルチスレッド走査整合の実施形態を示す図である。 シングルスレッド走査整合の実施形態を示す図である。 視覚－慣性オドメトリユニットからのフィードバックデータがデータ劣化に起因して抑制される３つの計算モジュールのブロック図の実施形態を示す図である。 走査整合ユニットからのフィードバックデータがデータ劣化に起因して抑制される３つの計算モジュールのブロック図の実施形態を示す図である。 視覚－慣性オドメトリユニット及び走査整合ユニットからのフィードバックデータがデータ劣化に起因して部分的に抑制される３つの計算モジュールのブロック図の実施形態を示す図である。 モバイルマッピングデバイスの推定軌道の実施形態を示す図である。 例示的かつ非限定的実施形態による双方向情報フローを示す図である。 例示的かつ非限定的実施形態による動的再構成可能システムを示す図である。 例示的かつ非限定的実施形態による動的再構成可能システムを示す図である。 例示的かつ非限定的実施形態によるＩＭＵバイアス補正のための優先度フィードバックを示す図である。 例示的かつ非限定的実施形態によるマップの二層ボクセル表現を示す図である。 例示的かつ非限定的実施形態によるマップの二層ボクセル表現を示す図である。 例示的かつ非限定的実施形態による走査整合のマルチスレッド処理を示す図である。 例示的かつ非限定的実施形態による走査整合のマルチスレッド処理を示す図である。 ＳＬＡＭシステムの例示的かつ非限定的実施形態を示す図である。 ＳＬＡＭシステムの例示的かつ非限定的実施形態を示す図である。 ＳＬＡＭエンクロージャの例示的かつ非限定的実施形態を示す図である。 信頼性レベルを示す点クラウドの例示的かつ非限定的実施形態を示す図である。 信頼性レベルを示す点クラウドの例示的かつ非限定的実施形態を示す図である。 信頼性レベルを示す点クラウドの例示的かつ非限定的実施形態を示す図である。 異なる信頼性レベルメトリックの例示的かつ非限定的実施形態を示す図である。 ＳＬＡＭシステムの例示的かつ非限定的実施形態を示す図である。 ＳＬＡＭシステムに対するタイミング信号の例示的かつ非限定的実施形態を示す図である。 ＳＬＡＭシステムに対するタイミング信号の例示的かつ非限定的実施形態を示す図である。 ＳＬＡＭシステム信号同期の例示的かつ非限定的実施形態を示す図である。 空中－地上協調マッピングの例示的かつ非限定的実施形態を示す図である。 センサパックの例示的かつ非限定的実施形態を示す図である。 レーザ－視覚－慣性オドメトリ及びマッピングソフトウェアシステムのブロック図の例示的かつ非限定的実施形態を示す図である。 オドメトリ推定及び定位に関与する走査の比較の例示的かつ非限定的実施形態を示す図である。 定位における走査整合精度の比較の例示的かつ非限定的実施形態を示す図である。 水平方位センサ試験の例示的かつ非限定的実施形態を示す図である。 垂直方位センサ試験の例示的かつ非限定的実施形態を示す図である。 水平方位センサ試験と下方傾斜センサ試験間の精度比較の例示的かつ非限定的実施形態を示す図である。 センサパックを有する航空機の例示的かつ非限定的実施形態を示す図である。 センサ軌道の例示的かつ非限定的実施形態を示す図である。 自律飛行結果の例示的かつ非限定的実施形態を示す図である。 長期稼働にわたる自律飛行結果の例示的かつ非限定的実施形態を示す図である。
一全般的態様では、本発明は、経時的な位置変化を推定する（オドメータ）及び／又は点クラウドのような３次元空間の３次元マップ表現を発生させるモバイルコンピュータベースのマッピングシステムに関する。マッピングシステムは、以下に限定されるものではないが、慣性測定ユニット（ＩＭＵ）、カメラ、及び／又は３Ｄレーザスキャナを含む複数のセンサを含むことができる。マッピングシステムはまた、経時的なシステムの位置変化を推定するために及び／又は周囲環境のマップ表現を発生させるために複数のセンサと通信状態にあり、これらのセンサからの出力を処理するように構成された少なくとも１つのプロセッサを有するコンピュータシステムを含むことができる。マッピングシステムは、高周波数低待ち時間オンライン実時間自己運動推定を高密度で正確な３Ｄマップ位置合わせと共に可能にすることができる。本発明の開示の実施形態は、同時定位及びマッピング（ＳＬＡＭ）システムを含むことができる。ＳＬＡＭシステムは、ＧＰＳに依存せず、実時間で同時の定位及びマッピングを可能にする多次元（例えば３Ｄ）レーザ走査及び距離測定システムを含むことができる。ＳＬＡＭシステムは、環境内にある物体からのレーザ走査の反射から生じる非常に正確な点クラウドに関するデータを発生させて管理することができる。点クラウド内の点のうちのいずれの移動も、ＳＬＡＭシステムが環境を通って進む時にシステムの場所及び方位の精確な理解を維持することができるように、点クラウド内の点を場所に対する基準点として用いて経時的に正確に追跡される。
一実施形態では、モバイルマッピングシステムの位置及び運動の解像度は、一連の粗から細への更新において順番に精緻化することができる。非限定例では、モバイルマッピングシステムの位置及び運動を高速更新速度を有する粗な解像度からより低速の更新速度を有する微細解像度へと更新するために個別の計算モジュールを用いることができる。例えば、高い更新速度でマッピングシステムの運動又は位置を予測するためのデータをＩＭＵデバイスが第１の計算モジュールに提供することができる。低い更新速度でマッピングシステムの運動又は位置の解像度を改善するためのデータを視覚－慣性オドメトリシステムが第２の計算モジュールに提供することができる。これに加えて、更に低い更新速度で運動推定値を更に精緻化し、マップを位置合わせするためのデータをレーザスキャナが第３の計算走査整合モジュールに提供することができる。一非限定例では、位置及び／又は運動の微細解像度データを処理するように構成された計算モジュールからのデータは、位置及び／又は運動の粗な解像度データを処理するように構成された計算モジュールにフィードバックすることができる。別の非限定例では、計算モジュールは、センサソーシング不良、不正データ、不完全データ、又は非存在データに関連付けられた計算モジュールを自動的に迂回することによってセンサ劣化問題に対処する耐故障性を組み込むことができる。従って、マッピングシステムは、非常に動的な運動の存在下で更に暗く凹凸及び構造のない環境内で作動することができる。
大部分がオフラインバッチシステムである既存のマップ発生技術とは対照的に、本明細書に開示するマッピングシステムは実時間で作動し、運動中である間にマップを発生させることができる。この機能は２つの実用的な利点をもたらす。第１に、ユーザは、三脚又は他の非定常マウント上に固定されるスキャナに限定されない。代わりに、本明細書に開示するマッピングシステムは、モバイルデバイスに関連付け、それによって実時間でマッピングすることができる環境範囲を拡大することができる。第２に、実時間特徴は、データが収集される間に現在のマップ区域に関するフィードバックをユーザに与えることができる。オンライン発生マップは、自律的なナビゲーション及び障害物回避のためのロボット又は他のデバイスを助けることもできる。一部の非限定的実施形態では、そのようなナビゲーション機能は、マッピングシステム自体の中に組み込むことができる。代替非限定的実施形態では、マップデータは、外部ソースマップを必要とする可能性があるナビゲーション機能を有する追加ロボットに提供することができる。
センサに対して、３Ｄモデル化、シーンマッピング、及び環境推測のようないくつかの潜在的な用途が存在する。マッピングシステムは、点クラウドを更に別の処理のための入力として取得する他のアルゴリズムに対して点クラウドマップを提供することができる。更に、マッピングシステムは、屋内と屋外の両方で機能することができる。そのような実施形態は、外部照明を必要とせず、暗所で作動することができる。カメラを有する実施形態は、外部照明が必要とされる可能性があるが、急激な運動に対処することができ、カメラからの画像でレーザ点クラウドを着色することができる。ＳＬＡＭシステムは、ユーザが環境を通って移動する、例えば、歩く、サイクリングする、運転する、飛行する、更にこれらの組み合わせを行う時に点クラウドを実時間で構築して維持することができる。マップは、マッピング器が環境を通って進行する時に実時間で構築される。ＳＬＡＭシステムは、数千個の特徴部を点として追跡することができる。マッピング器が移動する時に、運動の推定を可能にするためにこれらの点が追跡される。従って、ＳＬＡＭシステムは、実時間でＧＰＳのような外部定位技術に依存することなく作動する。実施形態では、物体のような複数の（ほとんどの場合、非常に多数の）環境特徴部が、三角測位のための点として用いられ、システムは、それが環境を通って移動する時に位置及び方位の正確な現在推定を維持するために数多くの場所及び方位計算を実施して更新する。実施形態では、固定特徴部を位置及び方位計算に用いることができるように、環境の内部にある特徴部の相対運動を用いて固定特徴部（壁、ドア、窓、家具、及び備品など）を移動特徴部（人々、車両、及び他の移動品目）から区別することができる。水中ＳＬＡＭシステムは、減衰を低減するために青緑色レーザを用いることができる。
マッピングシステム設計は、自己運動推定におけるドリフトがモジュール自体の周波数よりも低い周波数を有するという考察に従う。従って、３つの計算モジュールが周波数の高い順に配列される。高周波数モジュールは、過激な運動に対処することに特化され、低周波数モジュールは、先行するモジュールに関するドリフトを相殺する。更にこの逐次処理は、前方にあるモジュールがそれ程計算を受け持たず、高周波数で実行し、完全な処理のための後方にあるモジュールに十分な時間を与えるという計算に有利に働く。従って、マッピングシステムは、実時間オンラインで稼働しながら高レベルの精度を得ることができる。
更に、システムは、センサの劣化に対処するように構成することができる。カメラが機能しない場合（例えば、暗さ、劇的な照明変化、又は凹凸のない環境に起因して）、又はレーザが機能しない場合に（例えば構造のない環境に起因して）、対応するモジュールを迂回することができ、システムの残りの部分を確実に機能するように交番することができる。特に、一部の例示的実施形態では、本提案のパイプラインは、問題状態空間内で劣化した部分空間を自動的に決定し、十分に条件付けされた部分空間内で問題を部分的に解く。その結果、各モジュールからの「健全」な部分の組み合わせによって最終的な解が形成される。その結果、出力を発生させるために用いられるモジュールの結果的な結合は、単純にモジュール出力の線形結合でも非線形結合でもない。一部の例示的実施形態では、出力は、１又は２以上のモジュール全体の迂回を残りの機能モジュールの線形結合又は非線形結合との組み合わせで反映する。多数の実験を通してシステムを試験した結果は、システムが、数キロメートルのナビゲーションにわたる高い精度と、環境の劣化及び過激な運動に関するロバスト性とを生じることができることを示した。
下記で開示するモジュール化されたマッピングシステムは、運動推定及びマッピングのための距離センサ、視覚センサ、及び慣性センサからのデータを多層最適化構造を用いることによって処理するように構成される。モジュール化されたマッピングシステムは、
・計算モジュールを動的に再構成する機能、
・計算モジュール内の失敗モードを完全又は部分的に迂回し、残りのモジュールからのデータをセンサ及び／又はセンサデータの劣化に対処する方式で組み合わせ、それによって環境によって誘起されるデータ劣化とモバイルマッピングシステムの過激な運動とに対処する機能、及び
・実時間性能を与えるために計算モジュールを協働的に統合する機能、
を含むことができる特徴を組み込むことによって高い精度、ロバスト性、及び低いドリフトを得ることができる。
本明細書では、３Ｄレーザ、カメラ、及びＩＭＵを用いたオンライン自己運動推定のためのマッピングシステムを開示する。更に、推定運動は、移動環境のマップを構築するためにレーザ点を位置合わせする。多くの現実世界の用途では、自己運動及びマッピングを実時間で行わなければならない。自律ナビゲーションシステムでは、マップは、運動計画及び障害物回避にとって極めて重要である可能性があり、その一方で運動推定は、車両の制御及び操縦にとって重要である。
図１は、本発明の一実施形態によるマッピングシステム１００の簡単なブロック図を描いている。特定の構成要素については下記で開示するが、そのような構成要素は単なる例として提示するものであり、他の均等又は同様な構成要素に関して限定的ではない。例示しているシステムは、Ｘｓｅｎｓ（登録商標）ＭＴｉ－３０ ＩＭＵのようなＩＭＵシステム１０２と、ＩＤＳ（登録商標）ＵＩ－１２２０ＳＥ単色カメラのようなカメラシステム１０４と、Ｖｅｌｏｄｙｎｅ ＰＵＣＫ（商標）ＶＬＰ－１６レーザスキャナのようなレーザスキャナ１０６とを含む。ＩＭＵ１０２は、ｘ－ｙ－ｚ加速度計、ロール－ピッチ－ヨージャイロスコープ、及び磁力計のうちの１又は２以上から導出された慣性運動データと、慣性データとを第１の周波数で提供することができる。一部の非限定例では、第１の周波数は約２００Ｈｚとすることができる。カメラシステム１０４は、７５２×４８０ピクセルの解像度と、７６°の水平視野（ＦＯＶ）と、第２の周波数でのフレーム取り込み速度とを有することができる。一部の非限定例では、フレーム取り込み速度は、約５０Ｈｚの第２の周波数で作動することができる。レーザスキャナ１０６は、３６０°の水平ＦＯＶと、３０°の垂直ＦＯＶとを有し、レーザ回転速度を表す第３の周波数で毎秒０．３百万点を受光することができる。一部の非限定例では、第３の周波数は、約５Ｈｚとすることができる。図１に描いているように、レーザスキャナ１０６は、モータ回転角を測定するために符号器１０９を組み込むモータ１０８に接続することができる。一非限定例では、レーザモータ符号器１０９は、約０．２５0の解像度で作動することができる。
ＩＭＵ１０２、カメラ１０４、レーザスキャナ１０６、及びレーザスキャナモータ符号器１０９は、コンピュータシステム１１０とデータ通信状態にあるものとすることができ、コンピュータシステム１１０は、１又は２以上のプロセッサ１３４と、関連のメモリ１０３Ａとを有し、所望のオドメトリ及び／又はマッピングを実施するのに十分な処理パワーとメモリとを有するいずれかのコンピュータデバイスとすることができる。例えば、２．６ＧＨｚのｉ７クワッドコアプロセッサ（各コア上に２つのスレッドで全体として８つのスレッド）と組み込みＧＰＵメモリとを有するラップトップコンピュータを用いることができる。それに加えて、コンピュータシステムは、ＲＡＭのような１又は２以上のタイプの１次メモリ又は動的メモリ１２０と、ハードディスク又はフラッシュＲＯＭのような１又は２以上のタイプの２次メモリ又は保存メモリ１６０とを有することができる。上記では特定の計算モジュール（ＩＭＵモジュール１２２、視覚－慣性オドメトリモジュール１２６、及びレーザ走査モジュール１３２）を開示したが、そのようなモジュールは、上記で説明した機能を有する例示的モジュールに過ぎず、限定的ではないことを認識しなければならない。同様に、上記で開示したコンピュータデバイス１１０のタイプは、本明細書に開示する目的で上記のセンサと併用することができるコンピュータデバイスのタイプの例に過ぎず、決して限定的ではない。
図１に例示するように、マッピングシステム１００は、粗から細の方式で運動を順番に復元する個別計算モジュールを含む計算モデルを組み込む（図２も参照されたい）。ＩＭＵ１０２からの運動予測（ＩＭＵ予測モジュール１２２）から始めて、視覚－慣性緊密結合方法（視覚－慣性オドメトリモジュール１２６）が運動を推定し、レーザ点を局所的に位置合わせする。続いて走査整合方法（走査整合精緻化モジュール１３２）が、推定運動を更に精緻化する。更に、走査整合精緻化モジュール１３２は、マップ（ボクセルマップ１３４）を構築するために点クラウドデータ１６５を位置合わせする。マップは、マッピングシステムが任意的なナビゲーションシステム１３６の一部として用いることもできる。ナビゲーションシステム１３６は、搭載コンピュータシステム内の計算モジュール、１次メモリとして含めることができ、又は完全に別個のシステムを含むことができることを認識することができる。
各計算モジュールは、センサシステムの各々のうちの１つからのデータを処理することができることを認識することができる。従って、ＩＭＵ予測モジュール１２２は、ＩＭＵシステム１０２から導出されたデータから粗なマップを生成し、視覚－慣性オドメトリモジュール１２６は、カメラシステム１０４からの更に精緻化されたデータを処理し、走査整合精緻化モジュール１３２は、レーザスキャナ１０６及びモータ符号器１０９からの最もきめの細かい解像度データを処理する。それに加えて、よりきめの細かい解像度モジュールの各々は、よりきめの粗いモジュールから与えられたデータを更に処理する。視覚－慣性オドメトリモジュール１２６は、ＩＭＵ予測モジュール１２２が計算し、そこから受信したマッピングデータを精緻化する。同様に、走査整合精緻化モジュール１３２は、視覚－慣性オドメトリモジュール１２６によって与えられたデータを更に処理する。上記で開示したように、センサシステムの各々は、異なる速度でデータを取得する。一非限定例では、ＩＭＵ１０２は、約２００Ｈｚの速度でデータ取得を更新することができ、カメラ１０４は、約５０Ｈｚの速度でデータ取得を更新することができ、レーザスキャナ１０６は、約５Ｈｚの速度でデータ取得を更新することができる。これらの速度は非限定的であり、例えば、それぞれのセンサのデータ取得速度を反映することができる。よりきめの粗いデータは、よりきめの細かいデータよりも速い速度で取得することができ、更に、よりきめの細かいデータよりも速い速度で処理することもできることを認識することができる。上記で様々な計算モジュールによるデータ取得及び処理に対して特定の周波数値を開示したが、絶対周波数及びそれらの相対周波数のどちらもが限定的ではない。
マッピングデータ及び／又はナビゲーションデータは、粗レベルデータと微細レベルデータとを含むものと考えることもできる。従って、１次メモリ（動的メモリ１２０）内には、計算モジュール１２２、１２６、１３２のうちのいずれかがアクセス可能とすることができるボクセルマップ１３４内の粗な位置データを格納することができる。走査整合精緻化モジュール１３２が発生させることができる点クラウドデータ１６５としての詳細マップデータのファイルは、プロセッサ１５０を通じてハードディスク、フラッシュドライブ、又は他のより永続的なメモリのような２次メモリ１６０内に格納することができる。
よりきめの細かい計算のための計算モジュールがよりきめの粗いデータを用いるだけではなく、視覚－慣性オドメトリモジュール１２６と走査整合精緻化モジュール１３２との両方（きめの細かい位置情報及びマッピング）は、そのより精緻化されたマッピングデータをＩＭＵ位置予測を更新するための土台としてそれぞれのフィードバック経路１２８及び１３８を通じてＩＭＵ予測モジュール１２２にフィードバックすることができる。この方式で、粗な位置データ及びマッピングデータを解像度において順番に精緻化することができ、精緻化された解像度のデータは、粗な解像度計算のためのフィードバックレファランスとしての役割を果たす。
図２は、３つの計算モジュール並びにそれぞれのデータ経路のブロック図である。ＩＭＵ予測モジュール１２２は、ＩＭＵ（図１の１０２）からＩＭＵ位置データ２２３を受信することができる。視覚－慣性オドメトリモジュール１２６は、モデルデータをＩＭＵ予測モジュール１２２から受信し、それに加えて１又は２以上の個別追跡視覚特徴部２２７ａ、２２７ｂからの視覚データをカメラ（図１の１０４）から受信することができる。レーザスキャナ（図１の１０６）は、視覚－慣性オドメトリモジュール１２６によって供給される位置データに加えて走査整合精緻化モジュール１３２に提供することができるレーザ決定ランドマーク２３３ａ、２３３ｂに関係するデータを発生させることができる。ＩＭＵ予測モジュール１２２によって計算される位置モデルを精緻化するために、視覚－慣性オドメトリモジュール１２６からの位置推定モデルをフィードバックすることができる１２８。同様に、ＩＭＵ予測モジュール１２２によって計算される位置モデルに追加の補正を施すために、走査整合精緻化モジュール１３２からの精緻化されたマップデータをフィードバックすることができる１３８。
図２に描いて上記で開示したように、モジュール化されたマッピングシステムは、運動関連データを粗から細の方式で順番に復元して精緻化することができる。上記に加えて、各モジュールのデータ処理は、データをモジュールに出力するデバイスの各々のデータ取得及び処理の速度によって決定される可能性がある。ＩＭＵからの運動予測から始めて、視覚－慣性緊密結合方法は、レーザ点を推定して局所的に位置合わせすることができる。続いて走査整合方法は、推定運動を更に精緻化する。走査整合精緻化モジュールは、マップを構築するために点クラウドを位置合わせすることもできる。その結果、マッピングシステムは、データが利用可能になるにつれて各精緻化過程を処理するように時間最適化される。
図３は、図１に描いているものと同じタイプのセンサから導出されたデータに基づく標準カルマンフィルタモデルを例示している。図３に例示するように、カルマンフィルタモデルは、データの分解性能に関係なくセンサのうちのいずれかのものからのいずれかのデータの受信時に位置データ及び／又はマッピングデータを更新する。この場合、例えば、ＩＭＵデータに基づく位置情報推定の状態に関係なくそのようなデータが利用可能になる度に視覚－慣性オドメトリデータを用いて位置情報を更新することができる。従って、カルマンフィルタモデルは、各タイプの測定の相対解像度を利用しない。図３は、位置データを最適化するための標準カルマンフィルタベース方法のブロック図を描いている。カルマンフィルタは、データが与えられるにつれて位置モデル３２２ａ～３２２ｎを順番に更新する。従って、初期位置予測モデル３２２ａから始めて、カルマンフィルタは、後続の位置モデル３２２ｂを予測することができ３２４ａ、このモデルを受信ＩＭＵ機械化データ３２３に基づいて精緻化することができる。位置予測モデルは、予測段階３２４ａにおいてＩＭＵ機械化データ３２３に応じて更新することができ３２２ｂ、この段階に個別視覚特徴部又はレーザランドマークがシードされる更新段階が続く。
図４は、因子グラフ方法に基づく位置最適化を描いている。この方法では、第１の時間４１０におけるモバイルマッピングシステムの姿勢は、データの受信後に第２の時間４２０における姿勢へと更新することができる。因子グラフ最適化モデルは、各精緻化計算中に全てのセンサからの制約を組み合わせる。この場合、ＩＭＵデータ３２３、特徴部データ３２７ａ、３２７ｂ、及びカメラからの同様のもの、並びにレーザランドマークデータ３３３ａ、３３３ｂなどが全て各更新段階のために用いられる。そのような方法は、必要とされる大きいデータ量に起因して各位置精緻化段階に関する計算の複雑度を高めることが認められるであろう。更に、センサは桁で異なる可能性がある独立した速度でデータを提供する可能性があることから、全体の精緻化段階が、最も遅いセンサに対するデータ取得時間に時間的に拘束される。その結果、そのようなモデルは、高速な実時間マッピングには適さない可能性がある。図１及び図２に描いているモジュール化されたシステムは、粗から細の方式で運動を順番に復元する。この方式で、運動精緻化の度合いが、各タイプのデータの可用性によって決定される。
仮定、座標、及び問題
仮定及び座標系
上記で図１に描いたように、モバイルマッピングシステムのセンサシステムは、レーザ１０６とカメラ１０４とＩＭＵ１０２とを含むことができる。カメラは、内部パラメータが理解されているピンホールカメラモデルとしてモデル化することができる。３つのセンサの全ての間の外部パラメータを較正することができる。カメラとレーザとの間の相対姿勢及びレーザとＩＭＵとの間の相対姿勢は、当業技術で公知の方法に従って決定することができる。カメラとレーザとに対して単一の座標系を用いることができる。一非限定例では、カメラ座標系を用いることができ、全てのレーザ点を予備処理においてカメラ座標系内に投影することができる。一非限定例では、ＩＭＵ座標系をカメラ座標系と平行にすることができ、この場合ＩＭＵ測定値を取得時に回転補正することができる。座標系は、以下のように定義することができる。
・カメラ座標系｛Ｃ｝はカメラの光学中心を原点とすることができ、この座標系ではｘ軸は左に向き、ｙ軸は上に向き、ｚ軸は、前を向いてカメラの主軸と一致し、
・ＩＭＵ座標系｛Ｉ｝はＩＭＵ測定中心を原点とすることができ、この座標系ではｘ軸、ｙ軸、及びｚ軸は｛Ｃ｝に対して平行であり、同じ方向に向き、かつ
・開始姿勢において世界座標系｛Ｗ｝は｛Ｃ｝と一致する座標系とすることができる。
一部の例示的実施形態により、ランドマーク位置は必ずしも最適化されるとは限らない。その結果、状態空間において６つの未知数が残り、従って、計算強度が低く保たれる。本発明の開示の方法は、特徴部に精確な深度情報を与えて運動推定精度を保証し、その一方で特徴部の深度をまとめて更に最適化するためにレーザ距離測定を含む。ある一定の状況では更に別の最適化が戻り値の減少を生じる可能性があることからこれらの測定値の何らかの部分しか最適化する必要はない。
例示的かつ非限定的実施形態により、本説明のシステムの較正は、少なくとも部分的にシステムの機械的な幾何学形状に基づくものとすることができる。特に、ライダーとモータシャフトとの間の幾何学的関係に関してシステムのＣＡＤモデルからの機械的測定値を用いてライダーをモータシャフトと相対的に較正することができる。ＣＡＤモデルに関連して取得されるそのような較正は、高い精度と、追加の較正を実施する必要のないドリフトとを与えることが示されている。
ＭＡＰ推定問題
状態推定問題を最大事後確率（ＭＡＰ）推定問題として定式化することができる。システム状態セットとしてχ＝｛ｘi｝（ｉ∈｛１；２；…，ｍ｝）を定義し、制御入力セットとしてＵ＝｛ｕi｝（ｉ∈｛１；２；…，ｍ｝）を定義し、ランドマーク測定値セットとしてＺ＝｛ｚk｝（ｋ∈｛１；２；…，ｎ｝）を定義することができる。本提案のシステムが与えられる時に、Ｚは、視覚特徴部とレーザランドマークとの両方で構成することができる。システムの同時確率が次式のように定義される。
式（１）
Figure 0007141403000001
前式中のＰ（ｘ0）は第１のシステム状態の先行状態であり、Ｐ（ｘi｜ｘi-1），ｕi）は運動モデルを表し、Ｐ（ｚk｜ｘik）はランドマーク測定モデルを表す。式（１）として定式化された各問題に対して、当該問題の対応するベイズネットワーク表現が存在する。ＭＡＰ推定は、式１を最大化することである。ゼロの平均ガウスノイズという仮定の下では、この問題は最小二乗問題と等価である。
式（２）
Figure 0007141403000002
前式中のｒxi及びｒzkは、数学モデル及びランドマーク測定モデルそれぞれに関連付けられた残余誤差である。
式２を解く標準的な手法は、全てのセンサデータ、例えば、視覚特徴部、レーザランドマーク、及びＩＭＵ測定値を大きい因子グラフ最適化問題へと結合することである。それとは逆に、本提案のデータ処理パイプラインは、複数の小さい最適化問題を定式化し、これらの問題を粗から細の方式で解く。最適化問題は、以下のように言い換えることができる：
問題：レーザ、カメラ、及びＩＭＵからのデータを所与として、｛Ｗ｝に対する｛Ｃ｝の姿勢を決定するために問題を式（２）として定式化して解き、続いて推定姿勢を用いてレーザ点を位置合わせし、移動環境のマップを｛Ｗ｝で構築する。
ＩＭＵ予測サブシステム
ＩＭＵ機械化
この小節は、ＩＭＵ予測サブシステムを説明する。システムは｛Ｃ｝を基本センサ座標系と見なすことから、ＩＭＵを｛Ｃ｝に対して特徴付けることができる。上記で「仮定及び座標系」という名称の小節で開示したように、｛Ｉ｝と｛Ｃ｝は平行な座標系である。ω（ｔ）及びａ（ｔ）は、時間ｔにおける｛Ｃ｝の角速度及び加速度それぞれを示す２つの３×１ベクトルである。対応するバイアスをｂω（ｔ）及びｂa（ｔ）と表記し、ｎω（ｔ）及びｎa（ｔ）を対応するノイズとすることができる。ベクトル、バイアス、及びノイズの項を｛Ｃ｝で定義する。それに加えて、ｇは、｛Ｗ｝における一定の重力ベクトルと表記することができる。ＩＭＵ測定項は以下に続く式となる。
式（３）
Figure 0007141403000003

式（４）
Figure 0007141403000004

式中の
Figure 0007141403000005

は、｛Ｗ｝から｛Ｃ｝への回転行列であり、
Figure 0007141403000006

は、｛Ｃ｝と｛Ｉ｝との間の平行移動ベクトルである。
項：
Figure 0007141403000007

は、回転中心（｛Ｃ｝の原点）が｛Ｉ｝の原点と異なることに起因する遠心力を表すことに注意されたい。視覚－慣性ナビゲーション方法の一部の例は、この遠心力項を排除するために運動を｛Ｉ｝においてモデル化する。深度情報が既知の視覚特徴部と未知の視覚特徴部との両方を用いる本明細書に開示する計算方法では、深度が未知の特徴部を｛Ｃ｝から｛Ｉ｝へと変換する段階は容易ではない（下記を参照されたい）。その結果、本明細書に開示するシステムは、代わりに運動の全てを｛Ｃ｝においてモデル化する。実際には、この項の効果を最大限に低減するためにカメラとＩＭＵとが互いの近くに装着される。
ＩＭＵバイアスは、緩慢に変化する変量とすることができる。その結果、運動積分のための直近に更新されたバイアスが用いられる。最初に式３が時間積分される。続いて、結果として得られた方位を式４と共に時間積分のために２回用いて加速度データから平行移動が得られる。
バイアス補正
ＩＭＵバイアス補正は、カメラ又はレーザのいずれかからのフィードバック（図１及び図２の１２８、１３８をそれぞれ参照されたい）によって加えることができる。各フィードバック項は、短い時間量にわたる推定区分的運動を含む。これらのバイアスは、区分的運動中に一定であるようにモデル化することができる。式３から始めて、推定方位をＩＭＵ積分と比較することによってｂω（ｔ）を計算することができる。更新されたｂω（ｔ）を更に１回の積分において用いて平行移動を再計算し、それを推定平行移動と比較してｂa（ｔ）が計算される。
高周波ノイズの効果を低減するために、既知のバイアス個数を保ちながらスライドウィンドウが用いられる。スライドウィンドウ内で用いられるバイアスの個数の非限定例は、２００から１０００を含むことができ、２００ＨｚのＩＭＵ速度に基づく推奨バイアス個数は４００である。１００ＨｚのＩＭＵ速度の場合のスライドウィンドウ内のバイアスの個数の非限定例は、１００から５００であり、典型的なバイアスの個数値は２００である。スライドウィンドウからの平均個数のバイアスが用いられる。この実施では、スライドウィンドウの長さが、バイアスの更新速度を決定するためのパラメータとして機能する。当業技術ではバイアスをモデル化する代替方法が公知であるが、ＩＭＵ処理モジュールを別個の明確に異なるモジュールとして保つために本発明の開示の実施が用いられる。スライドウィンドウ方法は、システムの動的な再構成を可能にすることもできる。この方式で、ＩＭＵは、必要に応じてカメラ又はレーザのいずれか又はカメラとレーザの両方と結合することができる。例えば、カメラが機能しない時に、代わりにレーザのみによってＩＭＵバイアスを補正することができる。
高周波ノイズの効果を低減するために、ある一定個数のバイアスを保ちながらスライドウィンドウを用いることができる。そのような事例では、スライドウィンドウからの平均個数のバイアスを用いることができる。そのような実施では、スライドウィンドウの長さが、バイアスの更新速度を決定するパラメータとして機能する。一部の事例では、バイアスは、ランダムウォークとしてモデル化し、最適化処理によってバイアスを更新することができる。しかし、ＩＭＵ処理を別個のモジュール内に保つためには非標準的な実施が好ましい。この実施は、システムの動的な再構成に有利に働き、すなわち、ＩＭＵをカメラ又はレーザのどちらと結合することもできる。カメラが機能しない時に、代わりにレーザによってＩＭＵバイアスを補正することができる。説明したように、ＩＭＵバイアスを固定するために、順次的モジュール化システム内のモジュール間通信が利用される。この通信は、ＩＭＵバイアスを補正することを可能にする。
例示的かつ非限定的実施形態により、ＩＭＵバイアス補正は、カメラ又はレーザのいずれかからのフィードバックを利用することによって行うことができる。カメラ及びレーザの各々は、短い時間量にわたる推定区分的運動を含む。バイアスを計算する時に、本明細書で説明する方法及びシステムは、区分的運動中に構築すべきバイアスをモデル化する。更に式３から始めて、推定方位をＩＭＵ積分と比較することによって本明細書で説明する方法及びシステムはｂω（ｔ）を計算することができる。更新されたｂω（ｔ）を更に１回の積分において用いて平行移動を再計算し、それを推定平行移動と比較してｂa（ｔ）が計算される。
一部の実施形態では、ＩＭＵ出力は、経時的に比較的定常的な誤差を有する角速度を含む。結果として生じるＩＭＵバイアスは、ＩＭＵがグラウンド真実からの何らかの差を常に有することになるという事実に関連している。このバイアスは経時的に変化する可能性がある。このバイアスは比較的定常的であり、高周波のものではない。上記で説明したスライドウィンドウは、ＩＭＵデータを評価中の指定期間である。
図５を参照すると、視覚－慣性オドメトリサブシステムのシステム図が提示されている。本方法は、視覚をＩＭＵと結合する。視覚とＩＭＵの両方が、区分的運動を推定する最適化問題に制約を与える。それと同時に、本方法は、深度情報を視覚特徴部に関連付ける。特徴部が、レーザ距離測定値が取得可能な区域内に位置する時に、深度をレーザ点から取得することができる。そうでなければ、深度は、それまでの推定運動シーケンスを用いた三角測位から計算することができる。最後のオプションとして、本方法は、制約を異なる手法で定式化することによっていずれの深度も既知ではない特徴部を用いることもできる。このことは、必ずしもレーザ距離カバレージを有するとは限らない可能性がある特徴部、又は必ずしも十分に長く追跡されるとは限らない又はカメラ運動方向に位置していないことに起因して三角測位することができない特徴部に当て嵌まる。これら３つの代替形態は、位置合わせされた点クラウドを構築するために単独又は組み合わせで用いることができる。一部の実施形態では、情報は破棄される。点クラウド内の劣化を識別及び指定するために、固有値及び固有ベクトルを計算して用いることができる。状態空間内で特定の方向に劣化が存在する時に、この状態空間内のこの方向の解を破棄することができる。
視覚－慣性オドメトリサブシステム
視覚－慣性オドメトリサブシステムのブロックシステム図を図５に描いている。最適化モジュール５１０は、ＩＭＵ予測モジュール５２０からの姿勢制約５１２を運動推定５５０のための深度情報を有する又は欠く光学特徴部データに基づくカメラ制約５１５と共に用いる。深度マップ位置合わせモジュール５４５は、追跡されるカメラ特徴部５３０と、レーザ点５４０から取得された深度情報との位置合わせ及び深度関連付けを含むことができる。深度マップ位置合わせモジュール５４５は、前回の計算から取得された運動推定５５０を組み込むこともできる。本方法は、視覚をＩＭＵと緊密に結合する。これらの各々は、区分的運動５５０を推定する最適化モジュール５１０に制約５１２、５１５をそれぞれ与える。それと同時に、本方法は、深度マップ位置合わせモジュール５４５の一部として深度情報を視覚特徴部に関連付ける。特徴部が、レーザ距離測定値が取得可能である区域内に位置している時に、深度はレーザ点から取得される。そうでなければ、深度は、それまでの推定運動シーケンスを用いた三角測位から計算される。最後のオプションとして、本方法は、制約を異なる手法で定式化することによっていずれの深度も既知ではない特徴部を用いることもできる。このことは、レーザ距離カバレージを持たない特徴部、又は十分に長く追跡されない又はカメラ運動方向に位置していないことに起因して三角測位することができない特徴部に当て嵌まる。
カメラ制約
視覚－慣性オドメトリはキー－フレームベースの方法である。ある一定の個数の特徴部を見失った場合又は画像重畳がある一定の比を下回った場合に、新しいキー－フレームが決定される５３５。この場合、右上付き文字ｌ（ｌ∈Ｚ+）が最後のキー－フレームを示すことができ、ｃ（ｃ∈Ｚ+）及びｃ＞ｋが現在フレームを示すことができる。上記で開示したように、本方法は、深度が既知の特徴部と未知の特徴部とを組み合わせる。キー－フレームｌにおける深度が関連付けられた特徴部は、｛Ｃl｝においてＸl＝［ｘl，ｙl，ｚl］Tと表記することができる。相応に、深度が未知の特徴部は、代わりに正規化座標を用いて
Figure 0007141403000008

と表記する。
Figure 0007141403000009

は、システム状態を表す式１内の
Figure 0007141403000010

及びｘとは異なることに注意されたい。２つの理由、すなわち、１）深度関連付けがある程度の処理量を要し、深度関連付けをキー－フレームのみにおいて計算することによって計算強度を低減することができること、及び２）深度マップはフレームｃにおいて取得可能ではない可能性があり、位置合わせは既存の深度マップに依存することからレーザ点を位置合わせすることができない可能性があることからキー－フレームにおける特徴部を深度に関連付けることができる。｛Ｃc｝における正規化特徴部は、
Figure 0007141403000011
と表記する。
Figure 0007141403000012

及び
Figure 0007141403000013

は、３×３の回転行列及び３×１の平行移動ベクトルとし、この場合
Figure 0007141403000014

∈ＳＯ（３）及び
Figure 0007141403000015

∈
Figure 0007141403000016

が成り立ち、
Figure 0007141403000017

及び
Figure 0007141403000018

はＳＥ（３）変換を形成する。フレームｌとｃの間の運動関数を次式のように書くことができる。
式（５）
Figure 0007141403000019
Ｘcは未知深度を有する。ｄcを深度とすると、
Figure 0007141403000020
が成り立つ。
Figure 0007141403000021
を代入し、式５内の１番目及び２番目の行を３番目の行と組み合わせてｄcを排除すると以下に続く式を生じる。
式（６）
Figure 0007141403000022

式（７）
Figure 0007141403000023
Ｒ（ｈ）及びｔ（ｈ）（ｈ∈｛１，２，３｝）は、
Figure 0007141403000024

及び
Figure 0007141403000025

のｈ番目の行である。特徴部に対して深度が取得不能である時に、ｄlをキー－フレームｌにおける未知深度とする。Ｘl及びＸcにそれぞれｄk
Figure 0007141403000026

及びｄc
Figure 0007141403000027

を代入し、式５内の３つ全ての行を組み合わせてｄk及びｄcを排除し、別の制約を生じる。
式（８）
Figure 0007141403000028
運動推定
運動推定処理５１０には、１）式６～７内にあるもののような既知の深度を有する特徴部からのもの、２）式８内にあるもののような未知深度を有する特徴部からのもの、及び３）ＩＭＵ予測５２０からのものという３つの制約セットを組み合わせる最適化問題を解くことが必要とされる。
Figure 0007141403000029

をフレームａとｂとの間の次式の４×４の変換行列として定義することができる。
式（９）
Figure 0007141403000030

Figure 0007141403000031

及び
Figure 0007141403000032

は、対応する回転行列及び平行移動ベクトルである。更に
Figure 0007141403000033

は、指数マッピングを通じて
Figure 0007141403000034

に対応する３×１のベクトルとし、この場合、
Figure 0007141403000035

∈ｓｏ（３）が成り立つ。正規化項θ／｜｜θ｜｜は、回転の方向を表し、｜｜θ｜｜は回転角である。各
Figure 0007141403000036

は、カメラの６ＤＯＦ運動を含む
Figure 0007141403000037

と
Figure 0007141403000038

とのセットに対応する。
求められたフレーム１とｃ－１との間の運動変換、すなわち、
Figure 0007141403000039

を用いてＩＭＵ姿勢制約を定式化することができる。
Figure 0007141403000040

で表記する最後の２つのフレームｃ－１とｃの間の予測変換は、ＩＭＵ機械化から取得することができる。フレームｃにおける予測変換は次式のように計算される。
式（１０）
Figure 0007141403000041

Figure 0007141403000042

及び
Figure 0007141403000043

を
Figure 0007141403000044

に対応する６ＤＯＦ運動とする。ＩＭＵ予測平行移動
Figure 0007141403000045

は、方位に依存することが認められるであろう。例として、方位は、式（４）内の回転行列
Figure 0007141403000046

を通じての重力ベクトルの投影、従って、積分される加速度を決定することができる。
Figure 0007141403000047

は、
Figure 0007141403000048

の関数として定式化することができ、
Figure 0007141403000049

としてＩＭＵ予測モジュール１２２（図１及び図２）によって供給される２００Ｈｚの姿勢、並びに視覚－慣性オドメトリモジュール１２６（図１及び図２）によって供給される５０Ｈｚの姿勢は両方共に姿勢関数であることが認められるであろう。
Figure 0007141403000050

を計算する段階は、フレームｃにおいて開始することができ、加速度は、時間に関して逆に積分することができる。
Figure 0007141403000051

は、式（５）内にある
Figure 0007141403000052

に対応する回転ベクトルとし、
Figure 0007141403000053

及び
Figure 0007141403000054

が求めるべき運動である。制約は、次式のように表すことができる。
式（１１）
Figure 0007141403000055

式中の
Figure 0007141403000056

は、適切にカメラ制約に対して姿勢制約をスケール調整する相対共分散行列である。
視覚－慣性オドメトリサブシステムの実施形態では、姿勢制約は運動モデルを満たし、カメラ制約は式（２）におけるランドマーク測定モデルを満たす。最適化問題は、外れ特徴部の除去のためのロバストな当て嵌めフレームワークに適応されたニュートン勾配降下法を用いることによって解くことができる。この問題では、状態空間は、
Figure 0007141403000057

及び
Figure 0007141403000058

を含む。従って、フルスケールＭＡＰ推定は実施されず、周囲問題を解くためにのみ用いられる。ランドマーク位置は最適化されず、従って、状態空間内では６つの未知数しか用いられず、それによって計算強度が低く保たれる。従って、本方法は、特徴部に精確な深度情報を与えて運動推定精度を保証するためのレーザ距離測定を含む。その結果、一括調節によって特徴部の深度の更に別の最適化を不要とすることができる。
深度関連付け
深度マップ位置合わせモジュール５４５は、前回推定された運動を用いてレーザ点を深度マップに対して位置合わせする。カメラ視野の内部にあるレーザ点５４０は、ある一定量の時間にわたって保たれる。深度マップは、一定の密度を保つようにダウンサンプリングされて高速指標付けのための２ＤのＫＤ木内に格納される。ＫＤ木内では、全てのレーザ点は、カメラ中心の周囲の単位球面上に投影される。点は、２つの角座標によって表される。深度を特徴部に関連付ける時に特徴部をこの球面上に投影することができる。各特徴部に関して３つのレーザ点がこの球面上で求められる。この場合、これらの点の妥当性は、直交座標空間内でのこれら３つの点の間の距離を計算することによるものとすることができる。距離が閾値よりも大きい時に、点が異なる物体、例えば壁及びその手前にある物体からのものである可能性が高く、妥当性検査は失敗する。最後に、直交座標空間内における局所平面パッチを仮定して深度が３つの点から内挿される。
レーザ距離カバレージを持たない特徴部は、それがある一定の距離にわたって追跡されてカメラ運動方向に位置していなかった時に、これらの特徴部が追跡される画像シーケンスを用いて三角測位することができる。そのような手順では、各フレームにおいてベイズ確率モードに基づいて深度を更新することができる。
走査整合サブシステム
このサブシステムは、レーザ走査整合によって先行モジュールからの運動推定を更に精緻化する。図６は、走査整合サブシステムのブロック図を描いている。サブシステムは、局所点クラウド内にあるレーザ点５４０を受信し、供給されたオドメトリ推定５５０を用いてこれらの点を位置合わせする。続いて点クラウドから幾何学的特徴部が検出され６４０、マップに対して整合される。走査整合は、当業技術で公知の多くの方法と同様に特徴部からマップまでの距離を最小化する。しかしオドメトリ推定５５０は、最適化６１０において更に姿勢制約６１２を与える。最適化は、求められた特徴部対応部６１５に対して姿勢制約を処理する段階を含み、この特徴部対応部６１５はレーザ制約６１７を用いて更に処理されてデバイス姿勢６５０が生成される。この姿勢６５０はマップ位置合わせ処理６５５によって処理され、それによって特徴部対応部６１５を求めるのが容易になる。この実施は、マップのボクセル表現を用いる。更にこの実施は、１個から複数個のＣＰＵスレッド上で並列に実行されるように動的に構成することができる。
レーザ制約
レーザ走査を受信すると、本方法は、最初に走査６２０からの点を共通座標系内に位置合わせする。走査番号を示すためにｍ（ｍ∈Ｚ+）を用いることができる。カメラとレーザの両方に対してカメラ座標系を用いることができることを理解されたい。走査ｍは、｛Ｃm｝と表記する走査の始端のカメラ座標系に関連付けることができる。レーザ点５４０を局所的に位置合わせする６２０ために、視覚－慣性オドメトリからのオドメトリ推定５５０を主要点として採用することができ、ＩＭＵ測定値を用いて主要点の間を内挿することができる。
Ｐmを走査ｍからの局所位置合わせ点クラウドとする。Ｐmから２つの幾何学的特徴部セット、すなわち、鋭角縁部上にある一方のもの、言い換えればεmと表記する縁点と、局所平面上にあるもう一方のもの、言い換えればＨmと表記する平面点とを抽出することができる。この抽出は、局所走査における曲率の計算によるものである。遮蔽領域の境界上にある点、及びレーザビームに対してほぼ平行な局所面を有する点のような既に選択済みの隣接点を有する点は採用されない。これらの点は、大きいノイズを含む又はセンサが移動する時に経時的に位置を変化させる可能性が高い。
続いて幾何学的特徴部が構築済みの現在マップに対して整合される。Ｑm-1は、最後の走査を処理した後のマップ点クラウドとし、Ｑm-1を｛Ｗ｝で定義する。Ｑm-1内の点は、それぞれ縁点及び平面点を含む２つのセットへと分離される。センサの周囲のある一定の距離のところで切り取られたマップを格納するためにボクセルを用いることができる。各ボクセルに対して、一方が縁点に対し、もう一方が平面点に対する２つの３Ｄ ＫＤ木を構築することができる。個々のボクセルに対してＫＤ木を用いることにより、照会点を所与として、単一のボクセルに関連付けられた特定のＫＤ木しか検索する必要がない（下記を参照されたい）ことから点検索が高速化される。
走査を整合させる時に、最初にεm及びＨmが、｛Ｗ｝内に取得可能な最良の運動推測を用いて投影され、続いてεm及びＨm内の各点に対して、最近接点の集合がマップ上の対応するセットから求められる。点集合の幾何学的分布を検証するために、関連の固有値及び固有ベクトルを吟味することができる。特に、１つの大きい固有値と２つの小さい固有値とが縁線セグメントを示し、２つの大きい固有値と１つの小さい固有値とが局所平面パッチを示す。整合が妥当である時に、点から対応する点集合までの距離に関する式が定式化される。
式（１２）
Figure 0007141403000059
式中のXmは、εm又はＨm内の点であり、θm（θm∈ｓｏ（３））及びｔm（ｔm∈
Figure 0007141403000060

）は、｛Ｗ｝における｛Ｃm｝の６ＤＯＦ姿勢を示す。
運動推定
走査整合は、式（１２）によって記述される全体の距離を最小化する最適化問題６１０へと定式化される。最適化は、前の運動からの姿勢制約６１２を更に含む。Ｔm-1は、｛Ｗ｝における｛Ｃｍ－１｝の姿勢に関する４×４の変換行列とし、Ｔm-1は最後の走査を処理することによって生成される。
Figure 0007141403000061

は、オドメトリ推定によって与えられる｛Ｃm-1｝から｛Ｃm｝への姿勢変換とする。式（１０）と同様に、｛Ｗ｝における｛Ｃm｝の予測姿勢変換は次式で与えられる。
式（１３）
Figure 0007141403000062
Figure 0007141403000063

及び
Figure 0007141403000064

は、
Figure 0007141403000065

に対応する６ＤＯＦ姿勢とし、
Figure 0007141403000066

は、相対共分散行列とする。制約は次式で与えられる。
式（１４）
Figure 0007141403000067
式（１４）は、カメラが機能状態にあると仮定して前の運動が視覚－慣性オドメトリからのものである場合を意味する。そうでなければ、制約はＩＭＵ予測からのものである。同じ項をＩＭＵ機械化によって表記するために
Figure 0007141403000068

及び
Figure 0007141403000069

を用いることができる。加速度の積分は、方位（式（１１）における
Figure 0007141403000070

と同じ）に依存することから
Figure 0007141403000071

は、θmの関数である。ＩＭＵ姿勢制約は次式で与えられる。
式（１５）
Figure 0007141403000072

式中の
Figure 0007141403000073

は、対応する相対共分散行列である。最適化問題では、式（１４）及び式（１５）は、１つの制約セットへと線形結合される。この線形結合は、視覚－慣性オドメトリの作動モードによって決定される。ロバストな当て嵌めフレームワークに適応されたニュートン勾配降下法によって解かれる最適化問題は、θm及びｔmを精緻化する。
ボクセル内のマップ
マップ上の点はボクセル内に保たれる。２レベルボクセル実施を図７Ａ及び図７Ｂに例示している。Μm-1は、最後の走査を処理した後の第１のレベルのマップ７００上にあるボクセル７０２、７０４のセットを表す。センサ７０６を取り囲むボクセル７０４は、部分集合Μm-1を形成し、Ｓm-1と表記する。６ＤＯＦセンサ姿勢
Figure 0007141403000074

及び
Figure 0007141403000075

を所与として、マップ上でセンサと一緒に移動する対応するＳm-1が存在する。センサがマップの境界に近づくと、境界の反対側にあるボクセル７２５がマップ境界７３０を拡張するように移動される。移動されたボクセル内の点は消去され、マップの切り取りが生じる。
図７Ｂに例示するように、第２のレベルのマップ７５０の各ボクセルｊ（ｊε∈Ｓm-1）は、第１のレベルのマップ７００のものよりもマグニチュードが小さい
Figure 0007141403000076

と表記するボクセルセットによって形成される。走査を整合させる前に、最良の運動推測を用いてεm及びＨm内の点がマップ上に投影され、
Figure 0007141403000077

（

Figure 0007141403000078

）内に埋められる。εm及びＨmからの点で占有されたボクセル７０８は抽出されてＱm-1が形成され、走査整合のための３Ｄ ＫＤ木内に格納される。ボクセル７１０は、εm及びＨmからの点で占有されない。走査整合の完了時に、走査は、マップをなすボクセル７０８内に融合される。その後、マップ点は一定の密度を維持するようにダウンサイズされる。第１のレベルのマップ７００の各ボクセルが第２のレベルのマップ７５０の部分ボクセルよりも大きい空間容積に対応することは認識することができる。従って、第１のレベルのマップ７００の各ボクセルは、第２のレベルのマップ７５０内の複数の部分ボクセルを含み、第２のレベルのマップ７５０内の複数の部分ボクセル上にマッピングすることができる。
図７Ａ及び図７Ｂに関して上述したように、マップ情報を格納するために２つのボクセルレベル（第１のレベルのマップ７００及び第２のレベルのマップ７５０）が用いられる。Ｍm-1に対応するボクセルは、第１のレベルのマップ７００を維持するために用いられ、第２のレベルのマップ７５０内の
Figure 0007141403000079

（
Figure 0007141403000080

）に対応するボクセルは、走査整合のためのセンサの周囲のマップを取り出すために用いられる。マップは、センサがマップ境界に近づく時にのみ切り取られる。従って、センサがマップの内部を進む時に切り取りは必要ない。別の注目点は、一方が縁点に対し、もう一方が平面点に対する２つのＫＤ木がＳm-1内の各個別ボクセルに対して用いられる点である。上述したように、そのようなデータ構造は点検索を高速化することができる。この方式で、
Figure 0007141403000081

（
Figure 0007141403000082

）内の各個別ボクセルに対して２つのＫＤ木を用いるのとは対照的に複数のＫＤ木の間の検索が回避される。
Figure 0007141403000083

（
Figure 0007141403000084

）内の各個別ボクセルに対して２つのＫＤ木を用いる場合は、ＫＤ木の構築及び保守のためにより多くのリソースを必要とする。
表１は、異なるボクセル及びＫＤ木の構成を用いたＣＰＵ処理時間を比較している。時間は、閉鎖及び開放された構造化区域及び植物繁茂区域を網羅する様々なタイプの環境から収集した複数のデータセットから平均したものである。１つのボクセルレベルΜm-1のみを用いる時に、ＫＤ木の構築及び照会に約２倍の処理時間が生じることがわかる。このは、第２のボクセルレベル
Figure 0007141403000085

（
Figure 0007141403000086

）がマップを綿密に検索するのに役立つことに起因する。これらのボクセルなしでは、Ｑm-1内により多くの点が含まれてＫＤ木内に組み込まれる。また、各ボクセルに対してＫＤ木を用いることにより、処理時間は、Μm-1内の全てのボクセルに対してＫＤ木を用いる場合と比較して若干短縮される。
（表１）
表１．ＫＤ木演算における平均ＣＰＵ処理時間の比較
Figure 0007141403000087
並行処理
走査整合は、ＫＤ木を構築する段階と、特徴部一致を反復的に求める段階とを含む。この処理は時間を要し、システム内の主な計算を占有する。１つのＣＰＵスレッドは所望の更新周波数を保証することができないが、マルチスレッド実施は、複雑な処理の問題に対応することができる。図８Ａは、２つの整合器プログラム８１２、８１５が並列で実行される場合を例示している。走査の受信時に、マネージャプログラム８１０は、走査を最新の利用可能マップと整合させるように配列する。複数の構造及び複数の視覚特徴部を有する集合的な環境で構成される一例では、整合は低速であり、次の走査の到着前に完了することができない可能性がある。２つの整合器８１２及び８１５は交互に呼び出される。一方の整合器８１２では、Ｐm８１３ａ、Ｐm-2８１３ｂ、及び更に別のＰm-k（ｋ＝偶数に関する）８１３ｎが、それぞれＱm-2８１３ａ、Ｑm-4８１３ａ、及び更に別のＱm-k（ｋ＝偶数に関する）８１３ｎと整合される。同様に、第２の整合器８１５では、Ｐm+1８１６ａ、Ｐm-1８１６ｂ、及び更に別のＰm-k（ｋ＝奇数に関する）８１６ｎが、Ｑm-1８１６ａ、Ｑm-3８１６ａ、及び更に別のＱm-k（ｋ＝奇数に関する）８１６ｎと整合される。この交互処理の使用は、処理に対して２倍の時間量を与える可能性がある。構造及び視覚特徴部をほとんど持たないきれいな環境で構成される代替例では、計算は軽い。そのような例（図８Ｂ）では、単一の整合器８２０しか呼び出さなくてもよい。交互処理が必要とされないことから、Ｐm、Ｐm-1、が、それぞれＱm-1，Ｑm-2，・・・（８２７ａ、８２７ｂ、８２７ｎを参照されたい）と順番に整合される。一般的には２つのスレッドしか必要とされない可能性があるが、この実施は、最大で４つのスレッドを用いるように構成することができる。
変換統合
最終的な運動推定は、図２に描いている３つのモジュールからの出力の統合である。５Ｈｚの走査整合出力は最も正確なマップを生成し、その一方で５０Ｈｚの視覚－慣性オドメトリ出力及び２００ＨｚのＩＭＵ予測は高周波数運動推定のための統合される。
ロバスト性について
システムのロバスト性は、センサ劣化に対処する機能によって決定される。ＩＭＵは、システム内のバックボーンとして確実に機能しているものと常に仮定する。カメラは、劇的な照明変化に影響され、暗い／凹凸のない環境内で又は著しい運動ぼけが存在する時に（それによって視覚特徴部追跡の喪失が引き起こされる）失敗する可能性もある。レーザは、構造のない環境、例えば、単一の平面が大半を占めるシーンに対処することができない。これに代えて、過激な運動に起因するデータの希薄性によってレーザデータの劣化が引き起こされる恐れがある。そのような過激な運動は非常に動的な運動を含む。本明細書で用いる時に、「非常に動的な運動」は、システムの実質的に突然の回転変位又は直線変位又は実質的に大きいマグニチュードを有する連続的な回転運動又は平行移動運動を意味する。本発明の開示の自己運動決定システムは、非常に動的な運動の存在下並びに暗く凹凸及び構造のない環境内で作動することができる。一部の例示的実施形態では、本発明のシステムは、３６０度毎秒程度の高い回転角速度を受けながら作動することができる。他の実施形態では、システムは、１００ｋｐｈを含むそれ以下の線速度で作動することができる。それに加えて、これらの運動は、角運動と直線運動が結合されたものとすることができる。
視覚－慣性オドメトリモジュールと走査整合モジュールの両方は、式（２）に従って最適化問題を定式化してそれを解く。失敗が発生する時に、それは、劣化した最適化問題に対応し、すなわち、問題の一部の方向の制約が不良状態にあり、解を決定する上でノイズが優勢である。１つの非限定的方法では、問題に関連付けられたλ１，λ２，・・・，λ６と表記する固有値と、ｖ1，ｖ2，・・・，ｖ6と表記する固有ベクトルとを計算することができる。センサの状態空間は６ＤＯＦ（６つの自由度）を含むことから、６つの固有値／固有ベクトルが存在する。一般性を失うことなく、ｖ1，ｖ2，・・・，ｖ6を大きい順に並べ換えることができる。各固有値は、解がその対応する固有ベクトルの方向にどの程度良好な状態にあるかを表す。固有値を閾値と比較することにより、状態空間内で良好な状態にある方向を劣化方向から分離することができる。ｈ（ｈ＝０；１，…，６）を十分に条件付けされた方向の個数とする。２つの行列を次式のように定義することができる。
式（１６）
Figure 0007141403000088
最適化問題を解く時に、初期推測を用いて非線形反復を開始することができる。図２に描いている連続パイプラインを用いて、ＩＭＵ予測は、視覚－慣性オドメトリのための初期推測を与え、視覚－慣性オドメトリの出力は、走査整合のための初期推測として採用される。追加の２つのモジュール（視覚－慣性オドメトリ及び走査整合モジュール）に対して、ｘを解とし、更に△ｘを非線形反復におけるｘの更新とし、この場合、△ｘは、線形化したシステム方程式を解くことによって計算される。最適化処理中に、ｘを全方向に更新する代わりにｘを十分に条件付けされた方向のみに更新し、劣化方向に初期推測を保つことができる。
式（１７）
Figure 0007141403000089
式（１７）において、システムは、ＩＭＵ予測から始めて粗から細の順序で運動に関して求解し、追加の２つのモジュールは、運動を可能な限り求める／精緻化する。問題が良好な状態にある時に、精緻化は６ＤＯＦの全てを含むことができる。それとは別に問題が部分的にしか良好な状態にない時に、精緻化は０ＤＯＦから５ＤＯＦまでを含むことができる。問題が完全に劣化した時に、
Figure 0007141403000090

は、ゼロ行列になり、先行モジュールの出力が保たれる。
式（１４）及び式（１５）に記述されている姿勢制約に戻ると、これら２つの式が走査整合問題において線形結合されることが認められるであろう。式（１６）に定義されているように、
Figure 0007141403000091

及び
Figure 0007141403000092

は、視覚－慣性オドメトリモジュールからの固有ベクトルを含む行列を表し、
Figure 0007141403000093

は、サブシステム内の十分に条件付けされた方向を表し、
Figure 0007141403000094

は、劣化方向を表す。組み合わせ制約は次式で与えられる。
式（１８）
Figure 0007141403000095


Figure 0007141403000096
カメラが機能している正常な時に、
Figure 0007141403000097

が成り立ち、式（１８）は、式（１４）にあるもののような視覚－慣性オドメトリからの姿勢制約で構成される。しかし、カメラデータが完全に劣化した時に、
Figure 0007141403000098

はゼロ行列であり、式（１８）は、式（１５）によるＩＭＵ予測からの姿勢制約で構成される。
カメラ劣化の事例研究
図９Ａに描いているように、視覚特徴部が視覚－慣性オドメトリに対して不十分にしか取得可能ではない時に、ＩＭＵ予測１２２は、視覚－慣性オドメトリ問題における十分に条件付けされた方向の個数に依存して完全に又は部分的に点線で表記している視覚－慣性オドメトリモジュール１２６を迂回する９２４。続いて走査整合モジュール１３２は、走査整合のためのレーザ点を局所的に位置合わせすることができる。迂回するＩＭＵ予測はドリフトを受ける。レーザフィードバック１３８は、カメラフィードバック１２８を補償し、それが取得不能な方向にのみＩＭＵの速度ドリフト及びバイアスを補正する。この場合、カメラデータが劣化していない時には、より高い周波数がカメラフィードバックをより適切なものにすることから、カメラフィードバックは高い優先度を有する。十分な視覚特徴部が見つかる時には、レーザフィードバックは用いられない。
レーザ劣化の事例研究
図９Ｂに示しているように、走査整合１３２が運動推定を精緻化するのに環境構造が不十分である時に、視覚－慣性オドメトリモジュール１２６出力は、レーザ点をマップ上に位置合わせするのに点線で注記しているように走査整合モジュールを完全又は部分的に迂回する９３０。走査整合問題において十分に条件付けされた方向が存在する時に、レーザフィードバックは、これらの方向に精緻化された運動推定を含む。そうでなければレーザフィードバックは空になる１３８。
カメラ及びレーザ劣化の事例研究
より複雑な例では、カメラとレーザの両方が少なくともある程度劣化する。図１０はそのような例を描いている。６つの行を有する垂直バーは、各行が式（１６）における固有ベクトルに対応するＤＯＦ（自由度）である６ＤＯＦ姿勢を表す。この例では、視覚－慣性オドメトリ及び走査整合は、各々が運動の３ＤＯＦを更新し、他の３ＤＯＦにおいて運動を変化しないままに留める。ＩＭＵ予測１０２２ａ～１０２２ｆは、初期ＩＭＵ予測値１００２を含むことができる。視覚－慣性オドメトリは、一部の３ＤＯＦ（１０２６ｃ，１０２６ｅ，１０２６ｆ）を更新し１００４、精緻化された予測１０２６ａ～１０２６ｆを生じる。走査整合は、一部の３ＤＯＦ（１０３２ｂ，１０３２ｄ、１０３２ｆ）を更新し１００６、更に精緻化された予測１０３２ａ～１０３２ｆを生じる。それぞれ、カメラフィードバック１２８はカメラ更新１０２８ａ～１０２８ｆを含み、レーザフィードバック１３８はレーザ更新１０３８ａ～１０３８ｆを含む。図１０を参照すると、網掛けを持たないセル（１０２８ａ、１０２８ｂ、１０２８ｄ、１０３８ａ、１０３８ｃ、１０３８ｅ）は、それぞれのモジュールからのいずれの更新情報も含まない。ＩＭＵ予測モジュールへの全更新１０８０ａ～１０８０ｆは、カメラフィードバック１２８からの更新１０２８ａ～１０２８ｆとレーザフィードバック１３８からの更新１０３８ａ～１０３８ｆとの組み合わせである。フィードバックがカメラ（例えば１０２８ｆ）とレーザ（例えば１０３８ｆ）の両方から取得可能である自由度のうちの１又は２以上において、カメラ更新（例えば１０２８ｆ）は、レーザ更新（例えば１０３８ｆ）よりも高い優先度を有することができる。
しかし、実際には、視覚－慣性オドメトリモジュールと走査整合モジュールとは異なる周波数で実行する可能性があり、各々は独自の劣化方向を有する可能性がある。走査整合出力からの姿勢の間を内挿するためにＩＭＵメッセージを用いることができる。この方式で、視覚－慣性オドメトリ出力と時間整合された区分的運動を作り出すことができる。
Figure 0007141403000099

及び
Figure 0007141403000100

をフレームｃ－１とｃの間に視覚－慣性オドメトリによって推定された６ＤＯＦ運動とし、この場合、
Figure 0007141403000101

及び
Figure 0007141403000102

が成り立つ。
Figure 0007141403000103

及び
Figure 0007141403000104

は、時間内挿後に走査整合によって推定された対応する項とする。
Figure 0007141403000105

及び
Figure 0007141403000106

は、視覚－慣性オドメトリモジュールからの固有ベクトルを含む式（１６）に定義された行列とすることができ、この場合、
Figure 0007141403000107

は十分に条件付けされた方向を表し、
Figure 0007141403000108

は劣化方向を表す。
Figure 0007141403000109

及び
Figure 0007141403000110

を走査整合モジュールからの同じ行列とする。次式は、組み合わせフィードバックｆCを計算する。
式（１９）
Figure 0007141403000111

式中のｆV及びｆsは、カメラフィードバック及びレーザフィードバックを表し、以下に続く式で与えられる。
式（２０）
Figure 0007141403000112

式（２１）
Figure 0007141403000113
ｆCは、状態空間の部分空間内で求められた運動しか含まないことに注意されたい。ＩＭＵ予測からの運動、すなわち、
Figure 0007141403000114

及び
Figure 0007141403000115

は、次式のようにｆCのヌル空間に投影することができる。
式（２２）
Figure 0007141403000116
式（３）と式（４）の統合によってｂω（ｔ）及びｂa（ｔ）の関数として定式化されたＩＭＵ予測運動を表すために
Figure 0007141403000117

及び
Figure 0007141403000118

を用いることができる。方位
Figure 0007141403000119

は、ｂω（ｔ）にしか関連しないが、平行移動
Figure 0007141403000120

は、ｂω（ｔ）とｂa（ｔ）の両方に依存する。バイアスは、次式を解くことによって計算することができる。
式（２３）
Figure 0007141403000121
システムが正常に機能している時には、ｆCは状態空間を張り、式（２２）における
Figure 0007141403000122

及び
Figure 0007141403000123

はゼロ行列である。相応に、ｂω（ｔ）及びｂa（ｔ）は、ｆCから計算される。劣化した時に、ＩＭＵ予測運動
Figure 0007141403000124

及び
Figure 0007141403000125

は、運動を求めることができない（例えば、図１０の組み合わせフィードバックの白色の行１０８０ａ）方向に用いられる。結果は、先に計算されたバイアスがこれらの方向に保たれることである。
実験
スキャナを用いた試験
２つのセンサ組に対してオドメトリ及びマッピング側壁システムを検証した。第１のセンサ組では、Ｖｅｌｏｄｙｎｅライダー（商標）ＨＤＬ－３２ＥレーザスキャナをＵＩ－１２２０ＳＥ単色カメラ及びＸｓｅｎｓ（登録商標）ＭＴｉ－３０ ＩＭＵに取り付けた。このレーザスキャナは、３６０°の水平ＦＯＶ、４０°の垂直ＦＯＶを有し、５Ｈｚの回転速度で０．７百万点／秒を感受する。カメラは、７５２ピクセル×４８０ピクセルの解像度、７６°の水平ＦＯＶ及び５０Ｈｚのフレーム速度に構成した。ＩＭＵ周波数を２００Ｈｚに設定した。第２のセンサ組では、Ｖｅｌｏｄｙｎｅライダー（商標）ＶＬＰ－１６レーザスキャナを同じカメラ及びＩＭＵに取り付けた。このレーザスキャナは３６０°の水平ＦＯＶ、３０°の垂直ＦＯＶを有し、５Ｈｚの回転速度で０．３百万点／秒を感受する。各センサ組は、街路及び路外地それぞれの上で走らせたデータ収集車両に取り付けた。
両方のセンサ組に関して、最大で３００個のＨａｒｒｉｓコーナを追跡した。視覚特徴部を均等に分散させるために、画像は、各々が最大で１０個の特徴部を設ける５×６個の同一の部分領域へと分離した。各部分領域内の特徴部数を維持するために、追跡が特徴部を見失った時に新しい特徴部を生成した。
このソフトウェアは、２．６ＧＨｚのクワッドコアプロセッサ（各コア上に２つのスレッドで全体として８つのスレッド）及び内蔵ＧＰＵを有するラップトップコンピュータ上でロボットオペレーティングシステム（ＲＯＳ）を実行するＬｉｎｕｘ（登録商標）システムで稼働する。視覚特徴部追跡がＧＰＵ及びＣＰＵそれぞれの上で稼働する２つのバージョンのソフトウェアを実施した。処理時間を表２に示している。視覚－慣性オドメトリ（図２の１２６）によって費やされる時間は、環境又はセンサ構成に関連してそれ程大きく変動することはない。ＧＰＵバージョンでは、視覚－慣性オドメトリは、５０Ｈｚで実行されるＣＰＵスレッドの約２５％を消費する。ＣＰＵバージョンでは、視覚－慣性オドメトリはスレッドの約７５％を占有する。センサの第１の組は、第２のセンサ組よりも若干長めの処理時間を生じる。このことは、スキャナがより多くの点を感受し、プログラムが深度マップを維持して深度を視覚特徴部に関連付けるのにより多くの時間を必要とすることに起因する。
走査整合（図２の１３２）は、より長く、更に環境及びセンサ構成に関連して変動する処理時間を消費する。第１のセンサ組では、走査整合は、構造化環境内で作動させた場合に５Ｈｚで実行されるスレッドの約７５％を占有する。しかし、植物繁茂環境内では、マップ上により多くの点が位置合わせされ、プログラムは、一般的にスレッドの約１３５％を消費する。第２のセンサ組では、スキャナはより少ない点しか感受しない。走査整合モジュール１３２は、環境に依存してスレッドの約５０～９５％を用いる。ＩＭＵ予測（図２の１３２）によって費やされる時間は、他の２つのモジュールと比較して無視することができる。
精度試験
本提案のシステムの精度を評価するための試験を実施した。これらの試験では、第１のセンサ組を用いた。センサは、大学キャンパスを巡って走る路外車両上に装着した。１６分以内の２．７ｋｍの走行の後に、キャンパスマップを構築した。試験にわたる平均速度は２．８ｍ／ｓであった。
（表２）
表２．第１及び第２のセンサ組を用いた平均ＣＰＵ処理時間
Figure 0007141403000126
試験にわたる運動推定ドリフトを評価するために、推定軌道及び位置合わせしたレーザ点を衛星画像上に位置合わせされた。この場合、地上のレーザ点を手作業で除去した。軌道を衛星画像上の街路と整合させることにより、水平誤差の上限が＜１：０ｍであることが判明した。また、建物を同じ階で比較することによって垂直誤差が＜２：０ｍであることも判明した。それにより、終了時点で進行距離の＜０：０９％である総相対位置ドリフトが与えられる。測定値に対して精度を保証することができず、従って、位置ドリフトの上限のみを計算したことが認められるであろう。
更に、同じセンサを乗用車の上に装着してより包括的な試験を実施した。乗用車を構造化道路上で９．３ｋｍの行程にわたって走らせた。経路は、植物繁茂環境、橋、丘陵地、及び交通量の多い街路を通り、最終的に開始位置に戻るものであった。標高は経路に沿って７０ｍにわたって変化した。信号機を待つことを除き、試験中の車両速度は９～１８ｍ／ｓの間であった。経路の始点と終点の両方において見られる建物が２箇所に登録されることが判明した。２箇所の登録は、経路長にわたる運動推定ドリフトに起因して発生する。従って、第１の登録は試験の開始時における車両に対応し、第２の登録は試験の終了時における車両に対応する。隔たりの測定結果は＜２０ｍであり、終了時に進行距離の＜０：２２％の相対位置誤差が生じた。
システム内の各モジュールは全体の精度に寄与する。図１１は、精度試験における推定軌道を描いている。視覚－慣性オドメトリシステムによって生成されたモバイルセンサの軌道の第１の軌道プロット１１０２は、ＩＭＵモジュール１２２及び視覚－慣性オドメトリモジュール１２６（図２を参照されたい）を用いたものである。第１の軌道プロット１１０２において用いた構成は、図９Ｂに描いているものと同様である。第２の軌道プロット１１０４は、ＩＭＵモジュール１２２からのＩＭＵ予測を視覚－慣性オドメトリを迂回して走査整合モジュール１３２に直接転送することに基づくものである。この構成は、図９Ａに描いているものと同様である。完全なパイプラインの第３の軌道プロット１１０８は、ＩＭＵモジュール１２２と視覚－慣性オドメトリモジュール１２６と走査整合モジュール１３２との組み合わせ（図２を参照されたい）に基づくものであり、最小量のドリフトしか持たない。最初の２つの構成、軌道プロット１１０２及び１１０４の位置誤差は、それぞれ約４倍及び２倍大きい。
第１の軌道プロット１１０２及び第２の軌道プロット１１０４は、個々のセンサの劣化に遭遇した時に予測されるシステム性能と見なすことができる。走査整合が劣化した場合（図９Ｂを参照されたい）には、システムは、第１の軌道プロット１１０２によって示されるモードに劣化する。視覚が劣化した場合（図９Ａを参照されたい）には、システムは、第２の軌道プロット１１０４によって示されるモードに劣化する。センサのうちのいずれも劣化していない場合（図２を参照されたい）には、システムは、最適化機能の全てを組み込み、軌道プロット１１０８を生じる。別の例では、システムは、ＩＭＵ予測を初期推測として採用するが、レーザ周波数（５Ｈｚ）で稼働することができる。システムは、第４の軌道プロット１１０６を発生させる。結果として得られる精度は、視覚－慣性オドメトリを通り越してレーザと直接結合されたＩＭＵを用いた第２の軌道プロット１１０４と比較して僅かにしか良好ではない。この結果は、全ての制約を重ね合わせて問題を解く場合にカメラの機能性が十分に活用されないことを示している。
システムの別の精度試験は、元の１×速度及び加速した２×速度で進むモバイルセンサを含むものであった。２×速度で進む時に、３つ全てのセンサに関してデータフレームを１枚置きに省き、試験を通してより過激な運動が生じた。その結果を表３に記載している。各速度で３つの構成を評価した。２×速度では、１×速度での精度と比較して視覚－慣性オドメトリ構成及びＩＭＵ＋走査整合構成の精度は、進行距離の０．５４％及び０．３８％という大幅な比率で低減する。しかし、完全なパイプラインは、僅か０．０４％という非常に僅かな比率でしか精度を低減させない。この結果は、カメラとレーザとが互いに補償し合って全体的な精度を保つことを示している。このことは、特に運動が過激な時に確かである。
表３．進行距離の百分率としての相対位置誤差
（表３）
（１×速度での誤差は、図１１の軌道に対応する）
Figure 0007141403000127
図１２を参照すると、双方向情報フローの例示的かつ非限定的実施形態が示されている。例示するように、ＩＭＵ予測モジュールと視覚－慣性オドメトリモジュールと走査整合精緻化モジュールとを含む３つのモジュールが、粗から細に段階的に問題を解く。データ処理フローは左から右へのものであり、３つのモジュールをそれぞれ通過し、それに対してフィードバックフローは右から左へのものであり、ＩＭＵのバイアスを補正する。
図１３ａ及び図１３ｂを参照すると、動的再構成可能システムの例示的かつ非限定的実施形態が示されている。図１３ａに例示するように、視覚－慣性オドメトリにとって視覚特徴部が不十分である時に、ＩＭＵ予測は視覚－慣性オドメトリモジュールを（部分的に）迂回してレーザ点を局所的に位置合わせする。それに対して、図１３ｂに例示するように、走査整合にとって環境構造が不十分である時に、視覚－慣性オドメトリ出力が走査整合精緻化モジュールを（部分的に）迂回してレーザ点をマップ上に位置合わせする。
図１４を参照すると、ＩＭＵバイアス補正のための優先度フィードバックの例示的かつ非限定的実施形態が示されている。例示するように、垂直バーは６ＤＯＦ姿勢を表し、各行は１つのＤＯＦである。劣化した時に、表記された６つ全ての行が「ＩＭＵ」である左のＩＭＵ予測から始めて、視覚－慣性オドメトリが３ＤＯＦにおいて行が「カメラ」と表記されることになる更新を行い、続いて走査整合が別の３ＤＯＦにおいて行が「レーザ」と表記されることになる更新を行う。カメラフィードバックとレーザフィードバックとは左にある垂直バーのように組み合わせられる。カメラフィードバックは高めの優先度を有し、レーザフィードバックからの「レーザ」行は、カメラフィードバックからの「カメラ」行が存在しない場合にのみ埋められる。
図１５ａ及び図１５ｂを参照すると、マップの二層ボクセル表現の例示的かつ非限定的実施形態が示されている。マップ上のボクセルＭm-1（図１５ａ内の全てのボクセル）及びセンサを取り囲むボクセルＳm-1（点塗りつぶし表示ボクセル）が例示されている。Ｓm-1は、Ｍm-1の部分集合である。センサがマップの境界に近づいた時に、境界の反対側（最下行）にあるボクセルがマップ境界を拡張するように移動される。移動されたボクセル内の点はクリアされ、マップは切り取られる。図１５ｂに例示するように、各ボクセルｊ（ｊ∈Ｓm-1）（図１５ａの点塗りつぶし表示ボクセル）は、マグニチュードが小さいボクセルのセット
Figure 0007141403000128

（（図１５ｂ）の全てのボクセル
Figure 0007141403000129

）によって形成される。走査整合の前に、レーザ走査は、最良の運動推測を用いてマップ上に投影することができる。走査からの点によって占有された
Figure 0007141403000130

（
Figure 0007141403000131

）内のボクセルをクロスハッチングで標記している。続いて走査整合のためのクロスハッチング表示ボクセル内のマップ点が抽出されて３Ｄ ＫＤ木内に格納される。
図１６を参照すると、走査整合のマルチスレッド処理の例示的かつ非限定的実施形態が示されている。例示するように、マネージャプログラムが、別個のＣＰＵスレッド上で実行される複数の整合器プログラムを呼び出し、走査を利用可能な最新のマップに整合させる。図１６ａは２スレッドの場合を示している。走査Ｐm，Ｐm-1，・・・は、各整合器上でＱm，Ｑm-1，・・・と整合され、処理に対して２倍の時間量が与えられる。それと比較して、図１６ｂは、１スレッドの場合を示しており、この場合、Ｐm，Ｐm-1，・・・は、Ｑm，Ｑm-1，・・・と整合される。この実施は、最大で４つのスレッドを用いて動的に構成可能である。
実施形態では、実時間ＳＬＡＭシステムを実時間ナビゲーションシステムとの組み合わせで用いることができる。実施形態では、ＳＬＡＭシステムは、ライダー又はレーダベースの障害物検出システム、視覚ベースの障害物検出システム、熱ベースのシステム、又は同様のもののような障害物検出システムとの組み合わせで用いることができる。この障害物検出は、人々、ペット、又は同様のもののような活動障害物を運動検出、熱検出、電場又は磁場検出するか、又は他の機構等によって検出する段階を含むことができる。
実施形態では、ＳＬＡＭシステムに近隣反射を与える物体のような近距離特徴部、並びに近距離特徴部の間にある空間又は近距離特徴部内の開口を通して走査することができる品目のような遠距離特徴部のマッピングを含むことができる空間のマッピングを示すために、環境の特徴部を走査することによって確立された点クラウドは、ＳＬＡＭの一部を形成する画面のような上に表示することができる。例えば、室内の様々な点において異なる室外要素を窓又はドアを通して走査することができることから、マッピング器が室内を動き回る時に隣接する廊下にある品目をそのような空間又は開口を通して走査することができる。この場合、結果として得られる点クラウドは、直近の環境及びこの環境の外側にある遠距離要素の部分マッピングの包括的なマッピングデータを含むことができる。このように、ＳＬＡＭシステムは、「杭柵」を通しての空間のマッピングを近距離内にある空間又は開口（すなわち柵内の間隙）を通して遠距離物の識別によって行う段階を含むことができる。遠距離データは、例えばマッピング器が網羅的にマッピングされた空間（点クラウドの密度に起因して十分に理解された方位及び位置を有する）から疎にのみマッピングされた空間（新しい部屋等）へと移動する時に一貫した場所推定を維持するようなマッピング器が空間から空間へと移動する時にシステムがＳＬＡＭの方位を定めることを補助するために用いることができる。ユーザが近距離の場所から遠距離の場所に移動する時に、ＳＬＡＭシステムは、点クラウドの相対的な密度又は疎度を用いて、例えば、ＳＬＡＭの一部を形成するユーザインターフェースを通じてマッピング器を誘導することができ、例えば、別の空間から開口を通して見ることができない遠距離部分へとマッピング器を導くことができる。
実施形態では、ＳＬＡＭシステムからの点クラウドマップは、カメラ、及びセンサなどのような他の入力からのマッピングと組み合わせることができる。例えば、飛行又は宇宙船の例では、飛行機、ドローン、又は他の飛行モバイルプラットフォームは、ＳＬＡＭシステムに対する基準データとして用いることができ（例えば、走査から生じる点クラウドをＧＰＳ基準の場所に結び付ける）、又は例えば他のシステムからの出力上へのオーバーレイとして更に別の走査データを表示するための基準データを走査から取得することができる他の距離測定機器及び地理的定位機器が既に装備されている可能性がある。例えば、従来のカメラ出力は、オーバーレイとして点クラウドデータと一緒に示すことができ、又はその逆も同様である。
実施形態では、ＳＬＡＭシステムは、各特徴部からの帰還信号の反射強度を示すデータを含む点クラウドを提供することができる。この反射強度は、システムに対する信号の有効性を決定すること、特徴部が互いにどの程度関係するかを決定すること、面ＩＲ反射率を決定すること、及び同様のことを補助するために用いることができる。実施形態では、この反射強度は、マップ内への点クラウド表示を操作するための基準として用いることができる。例えば、ＳＬＡＭシステムは、所与の特徴部、材料、構造、又は同様のものに関する信号の反射率を強調表示するためにある程度の色対比を導入することができる（自動的に又はユーザ制御下で）。それに加えて、色及び反射係数の情報を拡張するためにＳＬＡＭシステムを他のシステムと結合することができる。実施形態では、点クラウド内の点のうちの１又は２以上は、強度パラメータ、密度パラメータ、時間パラメータ、及び地理空間的場所パラメータのような取得データのパラメータに対応する色を用いて表示することができる。点クラウドの着色は、ＳＬＡＭシステムが内部で作動している環境の要素又は特徴部、及び／又は点クラウド自体の取得処理の特徴部の要素をユーザが理解して解析するのを補助することができる。例えば、地理空間的区域内で取得される点の個数を示す密度パラメータを用いて、多くのデータ点が取得される区域を表す色と、データが疎らであって、「実」データではなく偽像の存在を示唆する別の色とを決定することができる。色は、例えば、走査が行われる時に一連の色を通して推移する時間を示すこともでき、その結果、ＳＬＡＭ走査を実施する経路の明確な表示が生じる。着色は、様々な特徴部の間で区別するため（例えば、空間内の家具を壁と比較する）、美術的効果を与えるため、注目区域を強調表示するため（例えば、走査観察者の注目を求めて関連機器を強調表示する）のような表示目的で行うこともできる。
実施形態では、ＳＬＡＭシステムは、「影」（点クラウドが、走査からの比較的少ないデータ点しか持たない区域）を識別することができ、追加の走査を必要とする区域を強調表示することができる（例えばユーザインターフェースを通じて）。例えば、そのような区域は、点クラウドを表示するＳＬＡＭシステムの視覚インターフェース内で影区域がレーザ走査によって十分に「塗色」又は網羅されるまで明滅する又は特定の色で描画することができる。そのようなインターフェースは、まだ走査されていない視野内の区域を強調表示するユーザに対するいずれかのインジケータ（視覚的、文字ベース、音声ベース、又は同様のもの）を含むことができ、いずれかのそのようなインジケータを用いて直接的に又は外部デバイス（ユーザのモバイル電話等）を通じてユーザの注目を得ることができる。他の例示的かつ非限定的実施形態により、本発明のシステムは、未走査領域を識別するために現在走査との比較のためのそれまでに構築された点クラウド及びマップなどのようなＳＬＡＭ上に格納されたデータの外部データを参照することができる。
実施形態では、本明細書に開示する方法及びシステムは、正確な位置及び方位情報を決定するために、又はレーザ走査からの反射信号に基づいて環境特徴部を示す点クラウドデータで構成されるマップを発生させるために外部システムによる処理又は計算を必要とすることなく稼働点において実時間測位出力を提供するＳＬＡＭシステムを含む。実施形態では、本明細書に開示する方法及びシステムは、レーザ走査から収集されたデータの後処理を必要とすることなく実時間測位情報を提供するＳＬＡＭシステムを含むこともできる。
実施形態では、ＳＬＡＭシステムは、車両ナビゲーションシステム（無人飛行体、ドローン、モバイルロボット、無人水中車両、自動運転車両、半自動車両、及び多くの他のもの等のためのもの）のような様々な外部システムと統合することができる。実施形態では、ＳＬＡＭシステムは、車両がそれ自体の環境の範囲内でＧＰＳのような外部システムに頼ることなく進むことを可能にするために用いることができる。
実施形態では、ＳＬＡＭシステムは、位置、方位、又は同様のものの現在推定に関して信頼性レベルを決定することができる。信頼性レベルは、走査において取得可能な点の密度、走査において取得可能な点の直交度、環境の幾何学形状、又は他の因子、又はこれらの組み合わせに基づくものとすることができる。信頼性レベルは、走査のセグメントを低信頼性セグメント、高信頼性セグメント、又は同様のものと呼ぶことができるように走査のルートに沿う各点における位置及び方位の推定に帰するものとすることができる。低信頼性セグメントは、追加の走査、他の技術の使用（外部データに基づいて調節を加えること等）、又は同様のことのために強調表示することができる。例えば、走査が閉ループ（走査の終了点が、既知の最初の場所での開始点と同じである）で行われる場合に、計算された終了場所と開始場所の間のいずれかの差異は、開始場所と終了場所の一致を回復するようにある一定の走査セグメントに対する場所推定を優先的に調節することによって解決することができる。低信頼性セグメント内の場所及び位置の情報は、高信頼性セグメントと比較して優先的に調節することができる。このように、ＳＬＡＭシステムは、閉ループ走査に対して信頼性ベースの誤差補正を用いることができる。
この信頼性ベースの調節の例示的かつ非限定的実施形態では、最初にＧｅｏｒｇｉａ ＴｅｃｈのＭｉｃｈａｅｌ Ｋａｅｓｓ及びＦｒａｎｋ Ｄｅｌｌａｅｒｔによって開発された区分的平滑化及びマッピング（ｉＳＡＭ）アルゴリズムの導出（Ｍ．Ｋａｅｓｓ、Ａ．Ｒａｎｇａｎａｔｈａｎ、及びＦ．Ｄｅｌｌａｅｒｔ著「ｉＳＡＭ：区分的平滑化及びマッピング（ｉＳＡＭ：Ｉｎｃｒｅｍｅｎｔａｌ Ｓｍｏｏｔｈｉｎｇ ａｎｄ Ｍａｐｐｉｎｇ）」、ＩＥＥＥ Ｔｒａｎｓ．ｏｎ Ｒｏｂｏｔｉｃｓ（ＴＲＯ）、第２４巻、第６号、２００８年１２月、１３６５～１３７８ページ、ＰＤＦ）を用いることができる。このアルゴリズムは、マップデータを「セグメント」で処理し、セグメント間の整合の残余誤差を最適化するようにこれらのセグメントの相対位置を反復的に精緻化する。それにより、全てのデータを閉ループ内で調節することによってループを閉じることが可能になる。多くのセグメント化の点は、アルゴリズムがデータを大幅に移動させることを可能にし、それに対して少ないセグメント化の点は高い剛性を生じる。
セグメント化のための点が整合信頼性メトリックに基づいて選択される時に、これを利用してループ閉鎖処理が正確なマップセクションの方々に局所誤差を分散させないように低い信頼性を有する領域内でマップを柔軟にし、高い信頼性を有する区域内で剛性にすることができる。このことは、各点に「柔軟性」を割り当てるようにセグメント化の点に重み付けし、この因子に基づいて誤差を分散させることによって更に改良することができる。
実施形態では、点クラウドの区域又はセグメントに関する信頼性尺度を用いて、例えば改善されたＳＬＡＭ走査を実現するために追加の走査を行うようにユーザを導くことができる。実施形態では、信頼性尺度は、点の密度、及び点の直交度などの組み合わせに基づくものとすることができ、より的確な走査を可能にするようにユーザを導くことができる。点の密度及び点の直交度のような走査属性は、走査が進行するにつれて実時間で決定することができることに注意されたい。同様に、システムは、低信頼性尺度を生じる可能性が高い走査環境の幾何学形状を感知することができる。例えば、平滑な壁を有する長い廊下は、１つの走査セグメントを次のセグメントから区別するためのいずれの不規則性も示さない可能性がある。そのような事例では、システムは、そのような環境内で取得された走査データに対して低めの信頼性尺度を割り当てることができる。システムは、低減する信頼性を決定し、走査を通して指示（「減速」、「左に方向転換」、又は同様のもの等）によってユーザを誘導するためにライダー、カメラ、及び場合によっては他のセンサのような様々な入力を用いることができる。他の実施形態では、システムは、所望のものよりも低い信頼性の区域をユーザインターフェース等を通じてユーザに表示し、それと同時にユーザが低信頼性の区域、容積、又は領域を更に走査することを可能にするように補助を与えることができる。
実施形態では、ＳＬＡＭ出力は、カメラからの出力、及び他のマッピング技術からの出力などのような他のコンテンツと融合することができる。実施形態では、ＳＬＡＭ走査は、走査に対応する時間符号化されたオーディオ音を取り込む随伴用途（任意的にモバイル用途）等によるオーディオトラックの取り込みと併せて実施することができる。実施形態では、ＳＬＡＭシステムは、マッピングシステムが、マッピング器がオーディオ及び／又は音をいつどこで取得したかを含む走査がいつどこで発生したかを正確に示すことができるように、走査中にデータ収集の時間符号化を施す。実施形態では、時間符号化は、オーディオが利用可能なマップ上にあるインジケータの上をクリックすること等によってユーザがアクセスすることができるデータをマップ又は走査の中に挿入すること等により、音が有意なマップ区域内で当該音を定位するために用いることができる。実施形態では、写真、ＨＤビデオ、又は同様のもののような他のメディアフォーマットを取り込んで走査と同期させることができる。これらのメディアフォーマットは、時間情報に基づいて別個にアクセスすることができ、又は走査出力と他のメディアに関する時間情報との時間同期に基づいてマップ自体の中の適切な箇所に挿入することができる。実施形態では、ユーザは、時間データを用いて時間を遡り、異なる時間符号化データを有する複数の走査等に基づいて経時的に何が変化したかを確認することができる。走査は、日付スタンプ又は時間スタンプ付きの稼働記録データのような他の情報を用いて更に改良することができる。上記のことから、走査は、シーン又は空間の多次元データベースの一部とすることができ、この場合、点クラウドデータは、時間ベースのデータ又はメディアを含むこのシーンに関係する他のデータ又はメディアに関連付けられる。実施形態では、計算は、例えば、原点で始まる新しい走査を再開しなければならないのではなく走査中の所与の点に戻ってその点で再開するように、走査をバックアップすることを可能にする方式で段階又はセグメントのシーケンスを通して維持される。それにより、例えば、最初は良好な出力を生成していた走査において後の時点で問題が発生した時に、走査を続けるための開始点としての部分走査情報の使用が可能になる。この場合、ユーザは、走査をある点に戻るまで「破棄し」又は「巻き戻し」、続いてこの点から走査を再開することができる。システムは、点クラウド特徴部に基づいて正確な位置情報及び場所情報を維持することができ、更に他の時間ベースのデータとの時間的配列を可能にするために時間情報を維持することができる。時間ベースのデータは、例えば、ある時間間隔にわたって走査が完了し、異なる時間間隔にわたって取り込まれた他のメディアと同期させる必要がある場合に、走査又は他のメディアを同期させるようなこれらの編集を可能にすることもできる。点クラウド内のデータは、時間スタンプを用いてタグ付けすることができ、それによって走査が指定点から再開することができるように巻き戻し先の時点の後の時間スタンプ付きデータを消去することができる。実施形態では、巻き戻しは、ある時点まで及び／又はある地理空間座標まで巻き戻す等、ある物理的な場所まで行うことができる。
実施形態では、点クラウドを着色し、それをオーバーレイとして用いることによる融合を含むＳＬＡＭベースのマップからの出力とＨＤビデオのような他のコンテンツとの融合を行うことができる。この融合は、ＳＬＡＭシステムと他のメディア取り込みシステムとの間の時間同期を含むことができる。コンテンツは、ビデオ、空間の静止画像、空間のＣＡＤモデル、走査中に取り込まれたオーディオコンテンツ、場所に関連付けられたメタデータ、又は他のデータ又はメディアと融合することができる。
実施形態では、ＳＬＡＭシステムは、点クラウドを操作するために用いることができるツール（例えばＣＡＤ）のような他の技術及びプラットフォームと統合することができる。この統合は、走査をＣＡＤモデル化ツール、急速プロトタイピングシステム、３Ｄ印刷システム、及び点クラウド又は中実モデルデータを用いることができる他のシステム内でモデル化された特徴部と組み合わせる段階を含むことができる。走査は、着色ツールのような後処理ツールに入力として提供することができる。走査は、例えば、注目点、メタデータ、メディアコンテンツ、注釈、ナビゲーション情報、特定の形状を区別するため及び／又は物体を識別するための意味解析を追加するためにマッピングツールに提供することができる。
出力は、地中レーダ、Ｘ線撮像、磁気共鳴撮像、コンピュータ断層映像、赤外線撮像、写真、ビデオ、ＳＯＮＡＲ、レーダ、及びライダーなどのような他の走査システム及び画像取り込みシステムからの出力と組み合わせることができる。この組み合わせは、走査の出力を車載ナビゲーションシステム、手持ち式マッピングシステム、モバイル電話ナビゲーションシステム、及び他のもののようなナビゲーションシステム及びマッピングシステムと統合する段階を含むことができる。走査からのデータは、Ｘ、Ｙ、及びＺの位置情報、並びにピッチ、ロール、及びヨーの情報を含む位置及び方位のデータを他のシステムに提供するために用いることができる。
実時間ＳＬＡＭシステムから取得されるデータは、とりわけ、３Ｄ運動取り込みシステム、音響工学用途、バイオマス測定、航空機建造、考古学、建築及び土木、拡張現実（ＡＲ）、自動走行車、自律モバイルロボット用途、浄化及び処理、ＣＡＤ／ＣＡＭ用途、建設現場管理（例えば進捗確認）、娯楽、探査（宇宙、鉱山、及び水中など）、林業（材木切り出し及びカエデ糖のような他の林業製品の管理）、フランチャイズの管理及び規則遵守（例えば店舗及びレストラン）、確認及び規則遵守のための撮像用途、屋内定位、室内設計、在庫検査、景観設計、保守のための産業空間マッピング、トラック輸送ルートマッピング、軍事／諜報用途、モバイルマッピング、石油パイプライン監視及び掘削、土地建物の評価及び他の不動産用途、小売店屋内定位（例えば、実時間マップを在庫マップに結合すること等）、保安用途、備蓄品監視（鉱石、木材、物資等）、査定（より良い見積もりを行うために事前精査をする段階を含む）、ＵＡＶ／ドローン、モバイルロボット、地下マッピング、３Ｄモデル作成用途、仮想現実（空間着色を含む）、倉庫管理、地区制／計画立案用途、自律遂行任務、検査用途、ドッキング用途（宇宙船及び船舶を含む）、洞窟探検、及びビデオゲーム／拡張現実用途を含む多くの異なる目的のための用いることができる。全ての使用シナリオにおいて、ＳＬＡＭシステムは、言及した使用分野で本明細書で説明するように作動することができる。
例示的かつ非限定的実施形態により、ユニットは、ＩＭＵとカメラ（視覚）とＬｉＤＡＲセンサとのハードウェア同期を含む。ユニットは、暗所又は無構造環境内である期間にわたって作動させることができる。処理パイプラインはモジュールで構成することができる。暗所では、視覚モジュールを迂回することができる。無構造環境では、ＬｉＤＡＲモジュールを迂回又は部分的に迂回することができる。例示的かつ非限定的実施形態では、ＩＭＵ、ライダー、及びカメラのデータ全てに時間スタンプが付けられ、これらのデータを時間的に整合及び同期させることができる。その結果、システムは、画像データと点クラウドデータとを同期させるように自動方式で機能することができる。一部の事例では、同期されたカメラ画像からの色データを用いて塊データピクセルをユーザに表示するために着色することができる。
ユニットは、走査整合のための４つのＣＰＵスレッドを含むことができ、例えば、Ｖｅｌｏｄｙｎｅデータを用いて５Ｈｚで稼働することができる。作動時のユニットの運動は、比較的高速のものとすることができる。例えば、ユニットは、約３６０度／秒の角速度と約３０ｍ／ｓの線速度とで作動することができる。
ユニットは、以前の発生マップに対して定位することができる。例えば、マップをゼロから構築する代わりに、ユニットのソフトウェアは、以前に構築されたマップを参照してこの古いマップのフレームワーク（例えば地理空間又は他の座標）の範囲内でセンサ姿勢及び新しいマップを発生させることができる。ユニットは、定位を用いてマップを更に拡張することができる。古いマップフレーム内で新しいマップを展開させることにより、新しいマップは、更に拡張して古いマップから飛び出すことができる。それにより、最初に初期「バックボーン」走査を生成し、場合によってドリフト及び／又は他の誤差を低減するように後処理し、その後、このマップから再開して建物内の脇部屋のような局所詳細を追加する又は注目領域内の点密度を高める分岐及び連鎖を含む様々な使用モードが可能になる。この方式を採用することにより、広域ドリフトを制限するように特別の注意を払ってバックボーンモデルを発生させることができ、それに続く走査は、局所詳細を取り込むことに集中して発生させることができる。また、より高速な大領域取り込みのための複数のデバイスが同じ基本マップから始めて詳細走査を実施することもできる。
更に、異なるタイプのデバイスによって生成されたモデルから始めて又はＣＡＤ図面から生成されたモデルであっても、そこから再開することもできる。例えば、高い広域精度の固定デバイスが基本マップを構築し、モバイルスキャナが、このマップから再開して詳細を埋めることができる。代替実施形態では、長距離デバイスが建物の外側区域及び大きい内側区域を走査することができ、短距離デバイスが、この走査から再開してより小さい廊下及び部屋並びにより細かい詳細を加えて入れることができる。ＣＡＤ図面から再開することは、ＣＡＤと施工完了時のものとの間の較差を迅速に検出することに関して多大な利点を有することができる。
再開は、場所位置合わせ済みの時間データを提供することもできる。例えば、進捗を視覚的に確認するために複数の走査を構築現場から経時的に取得することができる。他の実施形態では、工場の複数の走査が資産管理のための追跡を補助することができる。
再開は、これに代えて、単純に以前のマップの範囲内の定位データを提供するために用いることができる。これは、ロボット車両を誘導する上で、又は画像、熱マップ、音響のような新しいセンサデータを既存のマップの範囲内で定位する上で有用である可能性がある。
一部の実施形態では、ユニットは、マッピングモードで比較的高いＣＰＵ使用率を用い、かつ長期の定位／ナビゲーションに適する定位モードでは比較的低いＣＰＵ使用率を用いる。一部の実施形態では、ユニットは、時々に内部リセットを実行することによって長期作動をサポートする。この内部リセットは、内部処理中に生成される値のうちのいくつかが経時的に上昇することから有益である。長い作動期間（例えば数日）にわたって、これらの値は、コンピュータ内の値に対するストレージの論理的又は物理的な限界のような限界に達してリセットなしには場合によっては処理を失敗させる可能性がある。一部の事例では、システムは、性能を改善するためにＲＡＭメモリを自動的にフラッシュすることができる。他の実施形態では、システムは、新しく取得したデータと古い及び／又は保管されたデータとの実時間比較を実施する時に必要である可能性があることから、古い走査データをダウンサンプリングすることができる。
他の例示的実施形態では、ユニットは、飛行用途及び空中マップ－地上マップの融合をサポートすることができる。他の実施形態では、ユニットは、ＩＭＵ周波数、例えば１００Ｈｚで姿勢出力を計算することができる。そのような事例では、ソフトウェアは、マップ並びにセンサ姿勢を発生させることができる。センサ姿勢は、展開中のマップに対するセンサの位置及び方位を伝える。高周波数の正確な姿勢出力は、車両運動制御がそのようなデータを必要とすることから自律移動に役立つ。ユニットは、共分散及び推定信頼性を更に用いてセンサが静止している時に姿勢を固定することができる。
図１７（ａ）～図１７（ｂ）を参照すると、ＳＬＡＭの例示的かつ非限定的実施形態が示されている。実質的に半球の走査を発生させるためにライダーが回転される。この回転は、平歯車アセンブリ１７０４によってライダーマウントに平歯車減速を駆動するＤＣモータを有する機構によって実施される。平歯車減速アセンブリ１７０４は、ライダーをモータ１７０８からオフセットすることを可能にする。動力を供給し、回転するライダーからデータを受信するためにライダーの回転に沿ってスリップリングが存在する。走査中のこの機構の方位を記録するための符号器１７０６が同様にライダーの回転に一致している。薄いセクション接触ベアリングがライダーの回転シャフトを支持する。ライダーの回転板上にある釣り合い重りが、回転軸の周囲の重量のバランスをとり、回転を平滑で一定のものにする。下記のライダーの駆動機構及び取り付けの図に描いているように、この機構は、走査点の場所の内挿のための定速の維持を可能にするために最小の揺れ及びがたつきしか持たないように設計される。モータシャフト１７１０が、ライダーコネクタ１７１２と物理的連通状態にあることに注意されたい。
図１８を参照すると、ＳＬＡＭエンクロージャ１８０２の例示的かつ非限定的実施形態が示されている。ＳＬＡＭエンクロージャ１８０２は、様々な図及び斜視図で描かれている。寸法は、実施形態を表しており、ライダー、オドメトリカメラ、着色カメラ、及びユーザインターフェース画面などのような主要構成要素の一般的な特徴及び方位を維持しながらサイズは同様又は異なるものとすることができることから非限定的である。
一部の実施形態では、ユニットは、例えば、個人が歩き回りながらユニットを保持するのを補助するために首装具、肩装具、キャリーバッグ、又は他の着用可能な要素又はデバイス（図示していない）を用いることができる。ユニット又は支持要素又は支持デバイスは、走査中に揺れ又は振動を低減するための１又は２以上の安定化要素を含むことができる。他の実施形態では、ユニットは、手持ち式ユニットの重量を低減するためにショルダーバッグ又は同様のものの中で携帯されるリモートバッテリを用いることができ、従って、走査デバイスは外部電源を有する。
他の実施形態では、カメラ及びライダーは、視野を最大化するように配列される。カメラ－レーザ配列は妥協を引き起こす。一方では、カメラがレーザのＦＯＶを遮蔽し、もう一方ではレーザがカメラを遮蔽する。そのような配列では、これらの両方が若干の遮蔽を受けるが、この遮蔽は、マッピング品質を著しく犠牲にすることはない。一部の実施形態では、視覚処理がレーザデータによって支援されることから、カメラは、レーザと同じ方向を向く。レーザ距離測定は、処理において画像特徴部に深度情報を与える。
一部の実施形態では、空間データの信頼性を表す信頼性メトリックを用いることができる。そのような信頼性メトリック測定は、点の個数、点の分布、点の直交度、及び環境幾何学形状などを含むことができるがこれらに限定されない。レーザデータ処理（例えば走査整合）及び画像処理のための１又は２以上の信頼性メトリックを計算することができる。図１９（ａ）～図１９（ｃ）を参照すると、レーザ整合推定信頼性によって区別される点クラウドを示す例示的かつ非限定的な例が示されている。実際には、そのような画像は色分けすることができるが、例示するように、軌道と点の両方は、記録時の最後の信頼性値に基づいて群内で実線又は点線として描かれている。これらの例では、濃い灰色は不良であり、明るい灰色は良好である。値＞１０を有する全てのものが実線であるように値を閾値処理している。実験を通じて、Ｖｅｌｏｄｙｎｅ＜１を有するものは信頼することができず、＜１０は信頼性が低く、＞１０は非常に良好であることが判明した。
これらのメトリックを用いることにより、例えば、本明細書の他の箇所で解説するループ閉鎖ツールを利用する時に、モデル問題及びオフラインモデル補正を解決するための自動試験が可能になる。更に、これらのメトリックの使用は、整合が不良である時にユーザに報知し、場合によっては自動一時停止し、低信頼性データを破棄し、更に走査時にユーザに報知することを可能にする。図１９（ａ）は、比較的遅いペースで実施された建物階の走査を例示している。図１９（ｂ）は、比較的速いペースで実施された同じ建物階の走査を例示している。遅めの走査ペースから取得された走査と比較した時に、走査が実施される速度に部分的に起因して軽度のほころびの広がりが生じていることに注意されたい。図１９（ａ）は、比較的低い信頼性の潜在的な不具合スポットにズームインした表示を例示している。
図２０を参照すると、走査対走査の整合信頼性メトリック処理の例示的かつ非限定的実施形態が例示されており、フルレーザ走査と以前のフルレーザ走査から構築されたマップとの間で追従する視覚特徴部の平均個数を計算して視覚的に提示することができる。このメトリックは、有用であるが異なる信頼性尺度を提示することができる。図２０では、左の枠内にレーザ走査信頼性メトリック図が提示されており、右の枠内には同じデータに関する視覚特徴部メトリックの平均個数が提示されている。前と同様に、濃い灰色の線は、低い信頼性及び／又は視覚特徴部の少ない平均個数を示している。
一部の実施形態では、ループ閉鎖を用いることができる。例えば、ユニットは、部屋、個室を歩き回り、オフィスを出入りし、開始点に戻る時に作動させることができる。理想的には、開始点及び終了点からのデータメッシュは、正確にメッシュ形成されなければならない。現実には、ある程度のドリフトが存在する可能性がある。本明細書で説明するアルゴリズムは、そのようなドリフトを大幅に矮小化する。典型的な低減は、従来方法に対して１０×程度である（０．２％対２％）。この比は、開始点と終了点の間の距離における誤差をループ中に移動した総距離で除したものを反映する。一部の実施形態では、ソフトウェアは、開始点に戻ったことを認識し、原点に再固定することができる。完了すると、変動を取得してそれを収集データの全てにわたって分散させることができる。他の実施形態では、データ信頼性が不良であったことを信頼性メトリックが示すある一定の点クラウドデータを確定し、低い信頼性を有する区域に調節を加えることができる。
一般的に、本発明のシステムは、明示的及び暗示的なループ閉鎖を用いることができる。一部の事例では、ユーザは、ＳＬＡＭの一部を形成するユーザインターフェース等によってループを閉じるべきことを示すことができる。この明示的ループ閉鎖の結果として、ＳＬＡＭは、始端で取得されたデータと終端で取得されたデータとを互いに一致させてループを閉じるために直近の走査データをループの始端で取得されたデータに整合させることができる。他の実施形態では、本発明のシステムは、暗示的ループ閉鎖を実施することができる。そのような事例では、本発明のシステムは、それ自体が、走査ループに対する原点又は原領域を含む場所を能動的に再走査することになることを認識している自動方式で作動することができる。
一部の実施形態では、信頼性ベースのループ閉鎖を用いることができる。最初に、複数のセグメントを含む区域のループ走査の開始点と終了点を決定することができる。次に、複数のセグメントの信頼性を決定することができる。続いて、ループの始端と終端とを一致させるために高品質セグメントではなく低品質セグメントに調節を加えることができる。
他の例示的実施形態では、多ループの信頼性ベースのループ閉鎖を実施することができる。更に他の実施形態では、意味的に調節を加える信頼性ベースのループ閉鎖を用いることができる。例えば、走査対象要素の属性、すなわち、床が平坦であること、廊下が真直ぐであること等から構造情報を導出することができる。
一部の事例では、ライダー点クラウドデータの着色を用いることができる。一部の実施形態では、何が取り込まれたかを識別するのを補助するために、収集点への大まかな着色を実時間で用いることができる。他の実施形態では、オフラインの写実的着色を用いることができる。他の実施形態では、カメラ内の各ピクセルを固有ライダーピクセルにマッピングすることができる。例えば、点クラウド内のライダーデータに対応する着色カメラ内のピクセルから色データを取得してそれをライダーデータに付加することができる。
例示的かつ非限定的実施形態により、ユニットは、運動に関して粗から細へと求解する順次多層処理パイプラインを用いることができる。最適化の各層において、前の粗な結果が最適化問題への初期推測として用いられる。パイプライン内の段階は以下の通りである。
１．高周波数の更新（２００Ｈｚ程度）を施すが高レベルのドリフトを受ける運動予測のためのＩＭＵ機構から始める。
２．次に、カメラのフレーム速度（３０～４０Ｈｚ）における視覚－慣性オドメトリ最適化によってこの推定を精緻化し、この最適化問題は、ＩＭＵ運動推定を姿勢変化の初期推測として用い、現在のカメラフレームから追跡される一部の特徴部からキーフレームまでの間の運動における残余二乗誤差を最小化しようとする試みにおいてこの姿勢変化を調節する。
３．続いて、この推定は、「走査フレーム」速度によって決定される低めの速度でのレーザオドメトリ最適化によって更に精緻化される。走査データは連続的に到着し、ソフトウェアは、これらのデータを画像フレームと同様の複数のフレームへと定速で分割し、この時点ではこの速度は、ライダー回転機構の１回の回転が各走査フレームを完全なデータ半球にするのに要する時間である。これらのデータは、同じ走査フレーム内の点が収集されることから位置変化の視覚－慣性推定を用いて互いに貼り合わせられる。ライダーオドメトリ姿勢最適化段階において、視覚オドメトリ推定を初期推測として取得し、最適化は、前の走査フレームに整合される現在走査フレーム内で追跡される特徴部における残余誤差を低減しようと試みる。
４．最終段階において、現在走査フレームは、これまでのマップ全体に対して整合される。レーザオドメトリ推定を初期推測として取得し、最適化は、現在走査フレーム内の特徴部とこれまでのマップ内にある特徴部との間の残余二乗誤差を最小化する。
結果として得られるシステムは、高周波数低待ち時間自己運動推定を高密度の正確な３Ｄマップ位置合わせと併せて可能にする。更に、システムは、各段階が前の段階における誤差を補正することができることから、失敗モジュールを迂回する自動再構成によってセンサ劣化に対処することができる。従って、システムは、非常に動的な運動の存在下で、並びに暗い凹凸及び構造のない環境内で作動することができる。実験中に、システムは、９．３ｋｍのナビゲーションにわたって０．２２％の相対位置ドリフトを実証し、ランニング、ジャンプに関して、更には高速道路上での高速運転（最大で３３ｍ／ｓ）に関してさえもロバスト性を実証した。
そのようなシステムの他の主要特徴は、以下のものを含むことができる。
深度を用いる及び用いない視覚特徴部の最適化：ソフトウェアは、追跡される視覚特徴部の深度を最初にこれらの特徴部をレーザデータに関連付けようと試み、次にカメラフレーム間で深度を三角測位しようと試みることによって決定しようと試みることができる。続いて特徴部最適化ソフトウェアは、１つが深度が既知の特徴部に対する及び１つが深度が未知の特徴部に対する２つの異なる誤差計算において全ての特徴部を利用することができる。
レーザ特徴部決定：ソフトウェアは、全体の走査フレームではなく走査ラインデータが到着するにつれてレーザ走査特徴部を抽出することができる。この抽出は、はるかに簡単であり、各点とそのいずれかの側にあるＫ個の最も近い点との間の相対距離によって定義される当該点における平滑度を調べ、続いて最も平滑な点を平面特徴部と分類し、最も鋭角な点を縁部特徴部と分類することによって行われる。この抽出は、不良特徴部とすることができる一部の点の削除も可能にする。
マップ整合及びボクセル化：レーザ整合が実時間でどの程度機能するかということの一部は、マップ及び特徴部データがどのように格納されるかである。正確な整合に必要とされるものを保持しながら格納されるデータを最小限に抑えるために、この格納データのプロセッサ負荷を追跡することは、長期走査及び３次元基本ユニットへの選択的ボクセル化又はダウンサンプリングにとって重要である。プロセッサ負荷に基づいてこのダウンサンプリングのボクセルサイズ又は基本ユニットの実行中の調節は、大きいマップにおいて実時間性能を維持する機能を改善することができる。
並列処理：ソフトウェアは、プロセッサがデータを処理することができる速度よりも速くデータが到着する場合に、実時間性能を維持するために並列処理を利用することができるように構成することができる。この並列処理は、ｖｅｌｏｄｙｎｅのような高速な点／秒のライダーの時により有意である。
ロバスト性：本発明のシステムが前段階の推定を次の推定の一部として含めることなく（初期推測であることは別として）別個の最適化段階を用いる手法は、ある程度の内因的なロバスト性を生み出す。
信頼性メトリック：この処理における各最適化段階は、当該段階自体の結果における信頼性に関する情報を提供することができる。特に、各段階において、結果における信頼性尺度を与えるために最適化後に残っている残余二乗誤差及びフレーム間で追跡する特徴部の個数などを評価することができる。
ユーザには、デバイスによって取得されているデータが与えられたマルチスペクトルモデルの縮小（例えば部分サンプリング）バージョンを提示することができる。一例では、各々が３ｃｍ×３ｃｍ×３ｃｍの大きさの立方体のモデルデータは、ユーザインターフェース上に提示される縮小バージョン内に単一のピクセルとして表すことができる。表示のための選択されるピクセルは、立方体の中心に最も近いピクセルとすることができる。ＳＬＡＭの作動中に生成される代表的な縮小表示を下記で示す。説明するように、容積内に単一のピクセルを表示するという決定は、点クラウド内で予め定められた寸法の空間立方体を占有する１又は２以上の点の存在又はあらゆるそのような点の不在のいずれかを示す２値結果を表す。他の例示的実施形態では、選択ピクセルには、例えば、それによって表される予め定められた立方体の内部にあるピクセルの個数を示す値を属性として付与することができる。この属性は、部分サンプリングされた点クラウドを表示する時に、例えば、この属性の値を反映する色及び／又は強度を利用して各選択ピクセルを表示することによって利用することができる。
視覚フレームは、着色カメラからの単一の２Ｄ色画像を含む。ライダーセグメントは、ライダースキャナの３６０度フル回転を含む。視覚フレームとライダーセグメントは、これらを組み合わせてＩＭＵ及びオドメトリ（例えば高速白／黒）カメラのような関連のセンサから取り込まれた単位位置データに基づいて既存のモデルデータと整合させることができるように同期される。
低い信頼性メトリックを引き起こす問題は、光沢壁、ガラス、透明ガラス、及び狭い廊下からの反射を含むがこれらに限定されない。一部の実施形態では、ユニットのユーザは、例えば、一時停止ボタンを押すこと、及び／又は事前決定された点まで又は過去の要求秒数の巻き戻しを要求すること等によって走査を一時停止し、かつ再開することができる。
例示的かつ非限定的実施形態により、走査中の巻き戻しは以下の通りに進行することができる。最初に、システムのユーザが巻き戻しを望むことを示す。この表意は、ＳＬＡＭの一部を形成するユーザインターフェースの操作によって果たすことができる。巻き戻しを望むことを示した結果として、システムは、走査済みデータ点のうちである時間幅に対応する部分を削除又は別途除去する。全ての走査済みデータ点には時間スタンプが付けられることから、システムは、事前決定時間の後のデータ点を効果的に除去し、従って、走査における過去の点に「巻き戻す」ことができる。本明細書で解説するように、各走査中にカメラからの画像が収集され、時間スタンプが付けられる。その結果、事前決定時点の後のデータ点を除去した後に、システムは、事前決定時点まで巻き戻された走査済み点クラウドを表示しながら事前決定点において記録された画像の表示をユーザに提示することができる。画像は、システムのユーザが、過去の事前決定時点におけるＳＬＡＭの方位及び姿勢に緊密に整合させる位置へとＳＬＡＭの方位を定めるのを補助するためのメトリックとしての役割を果たす。ユーザの方位が事前決定時点におけるＳＬＡＭの過去の方位に近い方位に定められると、ユーザは、ＳＬＡＭのユーザインターフェース上の「実行」ボタンを押すこと等によって走査を再開したいという要望を示すことができる。走査を再開する指令に応答して、ＳＬＡＭは、新しい走査データを利用してＳＬＡＭの位置及び方位の初期推定を形成する処理パイプラインを実行する段階を進めることができる。この処理の最中に、ＳＬＡＭは、新しいデータを走査に追加しない可能性があるが、新しい走査データを用いてユーザの位置の瞬間信頼性レベル並びに新しく取得したデータが前の走査データに対応する度合いの視覚表現を決定して表示することができる。最後に、ＳＬＡＭの場所及び方位が前の走査データに対して十分に決定されたことが確定されると、走査は続行することができる。
上記で説明したように、この巻き戻し機能は、データを格納することによって部分的に可能になる。１秒毎に何個の点が運び込まれるかを推定し、次にどれ程の量を「巻き戻す」かを推定することができる。ユニットは、ｘ秒前にユーザがどこにいたかをユーザに知らせ、ユーザがその場所に移動し、自分が適切な場所にいることを確認するために一部の走査を取得することを可能にすることができる。例えば、行くべきおおよその場所をユーザに伝えることができる（又はユーザが再開したい場所を示す）。ユーザが十分に近い場合に、ユニットはユーザがいる場所を計算して十分な近くにいることをユーザに伝えることができる。
他の例示的実施形態では、ユニットは、空間と空間との間の移行状態で作動することができる。例えば、ユーザが狭い戸口を素早く通り抜ける時に、新しい空間内におけるユーザの場所を決定するのに十分なデータ及び時間がない可能性がある。特に、この例では、ドアフレームの境界は、そこを通って進む前にライダーがユーザの場所を確定するのに十分なドアの向こうの環境の分量を撮像することを阻害する可能性がある。１つのオプションは、この信頼性メトリックの低下を検出し、オペレータが狭い通路に近づく時に減速するように自分の行動を修正するように、例えば、視覚インジケータを点滅させること、画面の色を変更すること、及び同様のことによってオペレータに合図を送ることである。
図２１を参照すると、ＳＬＡＭユニット２１００の概要の例示的かつ非限定的実施形態が示されている。ＳＬＡＭユニット２１００は、ＩＭＵ２１０６のパルス毎秒（ＰＰＳ）信号から導出される複数の信号を発生させるためのタイミングサーバを含むことができる。発生された信号は、ユニット内の様々なセンサから収集されたデータを同期させるために用いることができる。これらの信号を発生させてＣＰＵ２１０４と通信するためにマイクロコントローラ２１０２を用いることができる。このマイクロコントローラ内に又は外部ＩＣ上のいずれかに直交復号器２１０８を構築することができる。
一部の例示的実施形態では、ＩＭＵ２２０６は、システムの他の部分に対するタイミングパルスを発生させるために用いられる立ち上がりエッジＰＰＳ信号を提供する。カメラは、ＩＭＵ ＰＰＳ信号から発生された上記で説明した立ち上がりエッジ信号を１つと、図２２を参照しながら例示するＧＰＩＯ１（持続的な１つのフレーム）及びＧＰＩＯ２（持続的な２つのフレーム）という２つの立ち下がりエッジ信号とを含む３つの信号を受信することができる。
例示するように、各カメラは、約３０Ｈｚ又は４０Ｈｚの高いフレーム速度と、約０．５Ｈｚ～５Ｈｚの高い解像度とを有するＩＭＵ ＰＰＳと同期されたトリガ信号を受信する。
各ＩＭＳ ＰＰＳは、マイクロコントローラ２２０２の内部カウンタをゼロにすることができる。ライダーの同期出力は、以下に続く事象をトリガすることができる。
・直交復号器を通じた現在符号器値の読み出し。
・現在カウンタ値の読み出し。
符号器値及びカウンタ値は、まとめて保存し、ＣＰＵに送ることができる。この段階は、図２３を参照しながら例示するライダー同期出力によって規定された４０Ｈｚ毎に発生することができる。
別の時間同期技術は、センサとコンピュータプロセッサとを同期させるのを容易にするＩＭＵベースのパルス毎秒同期を含むことができる。この種の同期の例示的かつ非限定的実施形態は、図２４を参照しながら示す。
ＩＭＵ２４００は、パルス毎秒（ＰＰＳ）信号２４０６をライダー２４０２に送るように構成することができる。ＰＰＳが送られる度に、コンピュータ２４０４は、ＩＭＵデータストリーム内のフラグを認識することによって通知を受ける。次にコンピュータ２４０４は、追従して時間文字列をライダー２４０２に送る。ライダー２４０２は、ＰＰＳ２４０６に同期し、受信した時間文字列に基づいてライダーデータストリーム内に時間スタンプを符号化する。
第１のＰＰＳ２４０６を受信した時に、コンピュータ２４０４はそれ自体のシステム時間を記録する。第２のＰＰＳから始めて、コンピュータ２４０４は、記録時間を１秒だけ増加し、結果として得られた時間文字列をライダー２４０２に送り、続いてＰＰＳ２５０６を追跡するように自体のシステム時間を補正する。
この時間同期スキームでは、ＩＭＵ２４００は時間サーバとして機能するが、初期時間はコンピュータシステム時間から取得される。ＩＭＵ２４００のデータストリームは、ＩＭＵ自体のクロックに基づく時間スタンプに関連付けられ、第１のＰＰＳ２４０６が送られる時にコンピュータシステム時間で初期化される。従って、ＩＭＵ２４００、ライダー２４０２、及びコンピュータ２４０４は全て時間同期される。実施形態では、ライダー２４０２はＶｅｌｏｄｙｎｅライダーとすることができる。
例示的かつ非限定的実施形態により、ユニットは、走査のためのＣＯＭ ｅｘｐｒｅｓｓボードと１ボタンインターフェースとを含む。
例示的かつ非限定的実施形態により、処理ＩＭＵと視覚データセンサとレーザデータセンサとは結合することができる。ユニットは、暗所又は無構造環境において長期間にわたって稼働することができる。一部の実施形態では、走査整合のための各々がＶｅｌｏｄｙｎｅデータを用いて５Ｈｚで実行される４つのＣＰＵスレッドを用いることができる。上述したように、ユニットの運動は高速であってもよく、ユニットは、以前のマップに対して定位することができ、定位を用いてマップを拡張することができる。ユニットは、マッピングモードで比較的高いＣＰＵ使用率を示し、定位モードで比較的低いＣＰＵ使用率を示し、従って、長期に適するものになる。
例示的かつ非限定的実施形態により、本明細書で説明する方法及びシステムは、地上からのマッピングと空中からのマッピングとの間の各々の特性に従った協調を可能にする。地上ベースのマッピングは、必ずしも空間又は時間の制限を受けやすいとは限らない。一般的に、地上車によって運ばれるマッピングデバイスは、大規模なマッピングに適し、高速で移動することができる。それに対して込み入った区域は、手持ち式配備でマッピングすることができる。しかし、地上ベースのマッピングは、センサの高度による制限を受け、それによって上から見下ろす構成を実現するのが困難になる。図２５に例示するように、地上ベースの実験は、建物の周囲の詳細マップを生成し、それに対して屋根は空中からマッピングしなければならない。小さい飛行体が用いられる時に、空中マッピングは、バッテリの短い寿命に起因して制限を受ける。同じく飛行体が安全に作動するために空間が十分に見通せることも必要である。
本明細書で説明する協調マッピングは、多層最適化によってデータを処理するためにレーザスキャナとカメラと低品質ＩＭＵとを利用することができる。結果として得られる運動推定は、低いドリフト（一般的に進行距離の＜０．１％）しか持たない高速（～２００Ｈｚ）のものとすることができる。
本明細書で説明する高精度処理パイプラインを利用して、地上から発生されたマップと空中から発生されたマップとを実時間又は近実時間で融合することができる。この融合は、空中導出マップからの出力に対する地上導出マップからの１つの出力の定位によって部分的に果たされる。
本明細書で説明する方法は協調マッピングを完遂し、更に空中配備の複雑度を低減する。地上ベースのマップを用いて飛行経路が定義され、飛行体は、自律遂行任務を実施する。一部の実施形態では、飛行体は、困難な飛行タスクを自律的に遂行することができる。
図２６を参照すると、協調マッピングを可能にするために利用することができるセンサ／コンピュータパックの例示的かつ非限定的実施形態が示されている。処理ソフトウェアは、特定のセンサ構成に必ずしも限定されない。図２６（ａ）を参照すると、センサパック２６０１は、０．３百万点／秒を発生させるレーザスキャナ２６０３と、６４０×３６０ピクセルの解像度及び５０Ｈｚのフレーム速度のカメラ２６０５と、２００Ｈｚの低品質ＩＭＵ２６０７とで構成される。搭載コンピュータが、センサからのデータを自己運動推定及びマッピングのための実時間で処理する。図２６（ｂ）及び図２６（ｃ）は、センサ視野を例示している。レーザとカメラとによって重複が共有され、それを用いて処理ソフトウェアは、下記で更に十分に説明するようにレーザからの深度情報を画像特徴部に関連付ける。
例示的かつ非限定的実施形態により、ソフトウェアは、レーザスキャナ、カメラ、及び慣性センサのような距離センサからのデータを処理する。全てのセンサからのデータを大きい本格的な問題へと組み合わせる代わりに、本明細書で説明する方法及びシステムは、問題を複数の小さい問題として構文解析し、これらの問題を粗から細の方式で順番に解く。図２７は、ソフトウェアシステムのブロック図を例示している。そのようなシステムでは、前方にあるモジュールが軽い処理を実施し、高周波数運動推定が過激な運動に対してロバストであることを確実にする。後方にあるモジュールは十分な処理を受け持ち、結果として得られる運動推定及びマップの精度を保証するために低周波数で実行される。
ソフトウェアは、ＩＭＵデータ処理２７０１から始まる。このモジュールは、ＩＭＵ機械化に基づいて運動を予測するようにＩＭＵ周波数で実行される。その結果は、視覚－慣性結合モジュール２７０３によって更に処理される。モジュール２７０３は、画像シーケンスを通して明確な画像特徴部を追跡し、最適化問題において運動に関して求解する。この場合、レーザ距離測定値が深度マップ上で位置合わせされ、それによって深度情報が追跡される画像特徴部に関連付けられる。センサパックは単一のカメラしか含まないことから、レーザからの深度は、運動推定中にスケール曖昧性を解消するのに役立つ。推定された運動は、レーザ走査を局所的に位置合わせするために用いられる。第３のモジュール２７０５では、運動推定を更に精緻化するためにこれらの走査が整合される。整合された走査は、走査がマップに対して整合されている間にマップ上で位置合わせされる。処理を高速化するために、走査整合は、複数のＣＰＵスレッドを並列で利用する。走査整合中の点のクエリを高速化するために、マップはボクセルで格納される。運動は様々な周波数で推定されることから、システム内の第４のモジュール２７０７がこれらの運動推定を統合のために取得する。出力は、車両制御に有益な高精度と低待ち時間との両方を保持する。
モジュール化されたシステムは、最終解を形成する時に「健全」なセンサモードを選択することによってセンサ劣化に関するロバスト性を更に確実にする。例えば、カメラが、低照明環境又は凹凸のない環境内にある時、例えば汚れのない白い壁に向いている時、又はレーザが、長く真直ぐな廊下のような対称又は押出形の環境内にある時には、一般的に処理は、妥当な運動推定を発生し損ねる。システムは、問題状態空間内で劣化した部分空間を自動的に決定することができる。劣化が発生した時には、システムは、各モジュールの十分に条件付けされた部分空間内で問題を部分的にしか解かない。その結果、「健全」な部分が組み合わせられて最終の妥当な運動推定が生成される。
マップが利用可能な時には、上記で説明した方法は、定位のためのこのマップを利用するように拡張することができる。この拡張は、走査整合方法を用いて果たされる。この方法は、２タイプの幾何学的特徴部、特に縁部及び平面上にある点を局所走査内の曲率に基づいて抽出する。特徴点はマップに対して整合される。縁点は、縁線セグメントに対して整合され、平面点は、局所平面パッチに対して整合される。縁線セグメント及び局所平面パッチは、マップ上で、局所点集合に関連付けられた固有値及び固有ベクトルを吟味することによって決定される。マップは、処理を高速化するためにボクセルで格納される。定位は、特徴点とその対応点との間の総距離を最小化する最適化問題を解く。高精度オドメトリ推定が定位に初期推測を与えるために用いられることに起因して、最適化は、通常は２～３回の反復で収束する。
定位は、必ずしも個々の走査を処理するとは限らず、バッチ処理のための一部の走査をスタックする。高精度のオドメトリ推定に起因して、走査は、局所座標フレーム内で綿密に位置合わせされ、ドリフトは、短い期間（例えば数秒）にわたって無視することができる。図２８（８．４）を参照しながら比較を例示し、この図では、図２８（ａ）は、前のセクションで整合された（走査整合は５Ｈｚで実行された）単一の走査であり、図２８（ｂ）は、２秒にわたってスタックされ、定位中に整合された（走査整合は０．５Ｈｚで実行された）走査を示している。スタックされた走査の方が著しく多くの構造詳細を含み、環境変化に対する定位の精度及びロバスト性に寄与することがわかる。それに加えて、低周波数の実行は、機上処理に関するＣＰＵ使用率を最小限に保つ（定位は約１０％のＣＰＵスレッドしか消費しない）。スタックされた走査は、著しく多くの構造詳細を含み、環境変化に対する定位の精度及びロバスト性に寄与する。大まかには、環境のうちの約２５％が変化する又は動的である可能性があり、システムは、良好に作動し続けることになる。
定位は、粒子フィルタベースの実施と比較される。オドメトリ推定は、運動モデルを粒子フィルタに提供する。粒子フィルタは、５０個の粒子を用いる。各更新段階において、粒子は、低分散の再サンプリングに基づいて再サンプリングされる。比較結果を図２９及び表８．１に示している。この場合、誤差は、定位された走査からマップまでの絶対距離として定義される。評価中に、本明細書で説明する方法及びシステムは、一部の平面を選び、定位された走査内の点からマップ上の対応する平面パッチまでの間の距離を用いる。図２９は、誤差分布の例示的かつ非限定的実施形態を示している。粒子フィルタを本説明の方法と同じ周波数（０．５Ｈｚ）で実行する時には、結果として生じる誤差は５倍大きい。それに対して表８．１では、ＣＰＵ処理時間は２倍を上回る。別の試験では、粒子フィルタを５Ｈｚで実行することは、誤差を本発明の開示の方法よりも若干大きいところまで低減するのに役立つ。しかし、対応するＣＰＵ処理時間は、２２倍過まで増大させる。これらの結果は、粒子フィルタベースの方法が必ずしも高精度オドメトリ推定を最大限に活用するとは限らないことを意味している。
（表８）
表８．１ 定位でのＣＰＵ処理時間の比較。粒子フィルタを５Ｈｚで実行する時に、高密度データは、高いＣＰＵ要求に起因して実時間速度の２５％で処理される。
Figure 0007141403000132
図３０を参照すると、センサパックが車庫の中で水平に保持される場合のセンサ調査の例示的かつ非限定的実施形態が示されている。図３０（ａ）は、構築されたマップとセンサ軌道とを示している。図３０（ｂ）は単一の走査である。このシナリオでは、走査は十分な構造情報を含む。カメラ処理モジュールを迂回する時に、システムは、フルパイプラインと同じ軌道を発生させる。その一方で、本明細書で説明する方法及びシステムは、地面に向かって垂直に下方傾斜センサパックを用いて別の試験を実行する。その結果を図３１に示している。このシナリオでは、走査内の構造情報は、図３１（ｂ）に示しているようにかなり疎である。処理は、カメラの使用なしでは失敗し、フルパイプラインを用いて成功する。結果は、センサパックの傾斜が必要とされる高い高度の飛行にとって重要である。
図３２を参照すると、センサパックが、１～２ｍ／ｓの速さで円を通り抜けるオペレータによって保持され、総進行距離が４１０ｍである例示的かつ非限定的実施形態が示されている。図３２（ａ）は、水平方位センサ構成を用いた結果として得られるマップ及びセンサ軌道を示している。センサは同じ位置で開始及び停止される。この試験は、経路を通して０．１８ｍのドリフトを生成し、進行距離と比較して０．０４％の相対位置誤差を生じる。続いてオペレータは、それぞれ４５°及び９０°の角度で保持された２つのセンサパックを用いてこの経路を繰り返す。結果として得られるセンサ軌道を図３２（ｂ）に示している。明らかに、傾斜はより大きいドリフトを導入し、相対位置誤差は４５°で０．６％であり（青色の破線の曲線）、９０°で１．４％である（赤色の一点鎖線の曲線）。最終的に、図３２（ａ）のマップ上における定位によってドリフトは相殺され、両方の構成が黒色の実線の曲線を生じる。
ドローンプラットフォーム３３０１の例示的かつ非限定的実施形態を図３３に示している。この航空機の重量は約６．８ｋｇ（バッテリを含む）であり、最大で４．２ｋｇのペイロードを運ぶことができる。センサ／コンピュータパックは航空機の底部に装着され、１．７ｋｇの重量がある。この図の右下にリモートコントローラが示されている。リモートコントローラは、自律遂行任務の最中に必要に応じて自律遂行を無効にするために安全パイロットによって操作される。この航空機は、ＧＰＳ受信器を有する（航空機の上部に）ように構築されることに注意されたい。ＧＰＳデータは、必ずしもマッピング又は自律遂行において用いられるとは限らない。
第１の協調マッピング実験では、オペレータは、センサパックを保持して建物の方々を歩く。その結果を図２５に示している。図２５（ａ）では、地上ベースのマッピングが、９１４ｍの行程にわたって１～２ｍ／ｓで実施されて建物の周囲を詳細に網羅する。予測されるように、このマップ上では建物の屋根は空白である。次に、ドローンが建物の上方を飛行するように遠隔操作される。図２５（ｂ）では、この飛行が２～３ｍ／ｓで実施され、進行距離は２６９ｍである。この処理は、図２５（ａ）のマップに関する定位を用いる。この手法で、空中マップが地上ベースのマップ（白色の点）と融合される。地上ベースのマップが構築された後に、ドローンの離陸位置がマップ上で決定される。空中マッピングのためのセンサ開始姿勢は既知であり、定位はそこから開始する。図３４は、空中及び地上ベースのセンサ軌道を上面図及び側面図に提示している。
更に、本明細書で説明する方法及びシステムは、空中マッピングを実現するために自律飛行を実施する。図３５を参照すると、最初に、飛行区域の方々への１～２ｍ／ｓで６７２ｍの行程にわたる手持ち式マッピングによって地上ベースのマップが構築される。このマップ及びセンサ軌道を図３５（ａ）に示している。続いてこのマップに基づいて経路点が定義され、ドローンは、これらの経路点を辿って空中マッピングを実施する。図３５（ｂ）に示しているように、曲線は飛行経路であり、曲線上の大きい点は経路点であり、点が空中マップを形成する。この実験では、ドローンは、図の左側にある格納庫から離陸し、現場を横断して飛行し、右側にある別の格納庫を通り抜け、その後、第１の格納庫に戻って着陸する。速度は、現場を横断する時に４ｍ／ｓであり、格納庫を通り抜ける時に２ｍ／ｓである。図３５（ｃ）及び図３５（ｄ）は、ドローンが右にある格納庫に向かって飛行している時及びこの格納庫に進入しようとしている時に搭載カメラによって撮影された２つの画像である。図３５（ｅ）は、任務中の推定速度を示している。
最後に、本明細書で説明する方法及びシステムは、より長い距離にわたって別の実験を実施する。図３６に示しているように、地上ベースのマッピングは、１０ｍ／ｓで左端から右端まで１４６３ｍの行程にわたって走らせた路外車両を含む。地上ベースのマップ及び経路点を用いて、自律飛行はこの現場を横断する。離陸後に、ドローンは地上の上方２０ｍの高さまで１５ｍ／ｓで上昇する。その後、ドローンは地上の上方２ｍまで下降し、並木を１０ｍ／ｓで通り抜ける。飛行経路は、図３６（ｂ）に曲線３６０１で示しているように１１１８ｍ長のものである。２つの画像が、ドローンが木々の上方で高く飛行している時（図３６（ｃ）を参照されたい）及び木々の下で低く飛行している時（図３６（ｄ）を参照されたい）に撮影される。
例示的かつ非限定的実施形態により、ＧＰＳ等からの広域測位データを処理パイプライン内に組み込むことができる。広域測位データは、長い行程距離にわたる自己運動推定ドリフトを相殺し、広域座標フレーム内にマップを位置合わせするのに有用とすることができる。
一部の実施形態では、ＧＰＳデータは、マッピング活動と同時に記録することができる。システムが移動するにつれて、距離に伴って誤差を成長させるある程度のレベルのドリフトが存在する。一般的には０．２％のドリフトしか受けないかもしれないが、それにも関わらず１０００メートル進行する時には、ドリフトは１０００メートルの行程毎に２メートルである。１０ｋｍではドリフトは２０メートルまで成長し、以降同様に成長する。ループを閉じる（従来の意味では、ルートの始端に戻って来る）ことなしには、この誤差は補正することができない。ＧＰＳ座標の形式で外部情報を提供することにより、システムは、それがどこに存在するかを理解し、現在位置推定を補正することができる。一般的にこの補正は後処理の取り組みにおいて行われるが、本発明のシステムは、そのような補正を実時間又は近実時間で遂行することができる。
従って、ＧＰＳを利用することにより、ループを閉じることを可能にする方法が与えられる。当然ながらＧＰＳもある程度の量の誤差を有するが、所与の区域内で通常は一貫しており、今日の多くのＧＰＳシステムは、表面上の位置Ｘ及びＹにおいて３０ｃｍよりも高い精度を与えることができる。他のより高額で精巧なシステムは、ｃｍレベルの測位を施すことができる。
このＧＰＳの使用は、以下の重要な機能を与える。１．地球上における点クラウドの定位。２．自体の位置が理解されることからマップがより一層正確になるように、更に当該位置で取得されたあらゆるデータを同様に収集された一連のＧＰＳ点に関連付けることができるように、進路補正された情報を用いてマップを整合及び「固定」する機能。３．ＧＰＳが失われた時にシステムがＩＭＵとしての役割を果たすための機能。
例示的かつ非限定的実施形態により、過激な運動に関する推定ロバスト性を更に改善するために動的視覚センサを利用することができる。動的視覚センサは、照明変化を有するピクセルのみに関するデータを報告し、高い速度と低い待ち時間との両方を生み出す。
この高い速度（一般的に約１０Ｈｚよりも高いものとして定義される）は、自己運動及び推定システムに高速な情報を素早く供給し、それによって定位、更に引き続いてマッピングに対する値を改善することができる。システムがより多くのデータをより少ない遅延しか伴わずに取り込むことができる時に、システムは、より正確でよりロバストになる。より多くの特徴部及びより高速な更新は、より正確な追跡及び運動推定を可能にすることから、動的視覚センサによって識別されて追跡される特徴部は、より的確な推定を可能にする。
自己運動推定のための動的視覚センサとの画像整合を実現するために、直接法を用いることができる。特に、直接法は、特徴部追跡のための連続画像を画像から画像へと整合させる。その一方で、本明細書に開示する特徴部追跡法は、直接法よりも優れている。
例示的かつ非限定的実施形態により、汎用ＧＰＵ又はＦＰＧＡ上で実行される並列処理を実施することができ、従って、この並列処理は、より大きい量及びより高い周波数でのデータ処理を可能にする。
並列処理技術を用いて、一部の特徴部を同時に追跡することができ、又は予め定められた区域又は方向のデータを複数の小さい問題へと区切ることができる。並列アーキテクチャは、複数のコア、スレッド、プロセッサの形態をとり、又はグラフィック処理ユニット（ＧＰＵ）のような特殊形態を取ることさえもできる。分割統治法を用いることにより、並列アーキテクチャ内の各ノードが同時に問題のうちの当該ノードの部分に専従することから、全体的な処理を著しく高速化することができる。一般的にそのような高速化は線形ではない。すなわち、問題を１６個の部分及び処理へと分割することにより、各部分は、は必ずしも個別に１６倍だけ処理を高速化すると限らない。分割、通信を行い、その後、結果を再編成する上でのオーバーヘッドがある。
それとは対照的に、線形処理を用いる時に、各部分問題又は計算は１度処理され、データはメモリ、ＣＰＵに出し入れされ、その後、メモリ又はストレージに戻される。この処理は低速であるが、パイプラインアーキテクチャは、シストリックアレイ等で正しく構成された時に、パイプラインが充填状態に保たれる限り事態を改善することができる。
例示的かつ非限定的実施形態により、自己運動推定のドリフトを広域平滑化によって除去するためにループ閉鎖を導入することができる。
例えば、開始点に戻った時には、既にそこにいるわけであるから、位置推定を補正することができることがわかる。開始点は原点であり、移動の終端において明らかになる蓄積誤差が存在するはずである。原点（０，０，０）と始端に戻った時にいると考えられる位置（例えば（１，２，３））との間の差を取ることによって誤差値がわかることになる。しかし、この値は、移動にわたって場合によっては均等に蓄積されたものである。
例えば、１００メートル移動して開始点に戻り、ｘｙｚに１メートルだけ外れていることが判明した時に、次に全体の距離の間のどこにこの誤差を適用するかを決定するという問題が生じる。１つの手法は、この誤差を全距離にわたって分散させることであり、この場合、経路上に留まり、更に重要な点として開始した場所に誤差なく戻るために各メートル増分において行程を１ｃｍだけ補正することができる。この補正が広域平滑化である。
別の手法は、移動中にいずれかの他の誤差尺度を用いて、この誤差尺度が高い場所のみに対して補正を適用することである。例えば、共分散行列は、マップ品質に関する好適なメトリックを与え、この誤差を全移動にわたって先見的に分散させるために用いることができる。言い換えれば、共分散行列内に現れる低品質整合を有する場所を用いて、移動にわたる誤差の比率に従ってこの移動全体にわたって誤差を不均等に分散させることができる。この分散は、低品質値が特定の場所に関連付けられた詳細な区域内の誤差を補正するように作用することになる。
以下の条項は、本明細書に開示する実施形態に関する追加の陳述を提供するものである。
条項１．各々に少なくとも地理空間座標とセグメントとが属性として付与された複数の点を含むライダー点クラウドを取得する段階と、各セグメントに同じセグメントが属性として付与された複数の点の計算精度を示す信頼性レベルを割り当てる段階と、同じセグメントが属性として付与された複数の点のうちの少なくとも一部分の各々の地理空間座標を信頼性レベルに少なくとも部分的に基づいて調節する段階とを含む方法。
条項２．信頼性レベルがループ閉鎖の信頼性レベルである条項１の方法。
条項３．地理空間座標を調節する段階が決定されたループ閉鎖誤差に応答する条項１の方法。
条項４．地理空間座標を調節する対象のセグメントが少なくとも１つの他のセグメントよりも低い信頼性レベルを有する条項１の方法。
第５項．開始場所を有し、各々に少なくとも地理空間座標とセグメントとが属性として付与された複数の点を含むライダー点クラウドをＳＬＡＭを用いて取得する段階を開始する段階と、ライダー点クラウドを取得しながらループを移動する段階と、ＳＬＡＭが開始場所に近接する時に走査終了点を決定する段階とを含む方法。
条項６．走査終了点を決定する段階がループを移動し終えたという指示をＳＬＡＭのユーザから受信する段階を含む条項６の方法。
条項７．走査終了点を決定する段階が開始場所に対する近接性を示すセグメント内の点に基づく条項６の方法。
条項８．開始場所を含むセグメント以外のセグメント内の点に開始場所に対して近位である地理空間座標が属性として付与される条項６の方法。
条項９．各々に少なくとも地理空間座標と時間スタンプとが属性として付与された複数の点を含むライダー点クラウドを取得する段階と、各々に地理空間座標及び時間スタンプのうちの少なくとも一方が属性として付与された複数の画像を含む色画像データを取得する段階と、着色される各点の時間スタンプに時間的に近い時間スタンプを有し、更に着色される複数の点の地理空間座標の直近にある地理空間座標を有する画像から導出された色情報を用いて複数の点のうちの少なくとも一部分を着色する段階とを含む方法。
条項１０．着色が実時間及び近実時間のうちの一方で実施される条項９の方法。
条項１１．ＳＬＡＭシステムに関する運動推定をＳＬＡＭシステムの一部を形成するＩＭＵを用いて導出する段階と、精緻化された推定を発生させるために運動推定を視覚－慣性オドメトリ最適化処理によって精緻化する段階と、レーザオドメトリ最適化処理によって現在走査内の少なくとも１つの特徴部と前に走査された少なくとも１つの特徴部との間の少なくとも１つ残余二乗誤差を最小化することによって精緻化された推定を精緻化する段階とを含む方法。
条項１２．運動推定を導出する段階がＩＭＵ更新を約２００Ｈｚの周波数で受信する段階を含む条項１１の方法。
条項１３．ＩＭＵ更新の周波数が１９０Ｈｚと２１０Ｈｚの間にある条項１２の方法。
条項１４．運動推定を精緻化する段階が、ＳＬＡＭシステムの一部を形成するカメラのフレーム速度に等しい速度で運動推定を精緻化する段階を含む条項１１の方法。
条項１５．フレーム速度が３０Ｈｚと４０Ｈｚの間にある条項１４の方法。
条項１６．レーザオドメトリ最適化処理が、ＳＬＡＭの一部を形成するライダー回転機構が完全なデータ半球を走査する際の走査フレーム速度で実施される条項１１の方法。
条項１７．ＳＬＡＭシステムの一部を形成するカメラを用いて複数のカメラフレーム内にある複数の深度追跡視覚特徴部を取得する段階と、複数の視覚特徴部をＳＬＡＭの一部を形成するライダーから取得されたライダー導出点クラウドに関連付ける段階と、少なくとも２つのカメラフレーム間で少なくとも１つの視覚特徴部の深度を三角測位する段階とを含む方法。
条項１８．関連付け段階及び三角測位段階が、並列コンピュータを用いるプロセッサ上で実施される条項１７の方法。
条項１９．マイクロコントローラと、複数のタイミング信号を生成するようになった慣性測定ユニット（ＩＭＵ）と、複数のタイミング信号から導出された複数の同期信号を生成するようになったタイミングサーバとを含むＳＬＡＭデバイスであって、同期信号が、ＳＬＡＭデバイスの一部を形成する少なくとも２つのセンサを同期させるように作動する条項ＳＬＡＭデバイス。
条項２０．少なくとも２つのセンサがライダー、カメラ、及びＩＭＵからなる群から選択される条項２０のＳＬＡＭデバイス。
条項２１．各々に少なくとも地理空間座標と時間スタンプとが属性として付与された複数の点を含むライダー点クラウドを取得する段階と、各々に少なくとも地理空間座標と時間スタンプとが属性として付与された複数の画像を含む色画像データを取得する段階と、着色される各点の時間スタンプに時間的に近い時間スタンプ及び着色される各点の地理空間座標に距離的に近い地理空間座標のうちの少なくとも一方を有する画像から導出された色情報を用いて複数の点のうちの少なくとも一部分を着色する段階と、複数の点のうちの着色部分を表示する段階とを含む方法。
条項２２．カメラからの出力を表示された複数の点の上へのオーバーレイとして表示する段階を更に含む条項２１の方法。
条項２３．各々に少なくとも地理空間座標と時間スタンプとが属性として付与された複数の点を含むライダー点クラウドを取得する段階と、複数の点のうちの少なくとも一部分を色情報を用いて着色する段階とを含み、複数の点の各々が、強度パラメータ、密度パラメータ、時間パラメータ、及び地理空間的場所パラメータからなる群から選択された取得ライダー点クラウドのパラメータに対応する色を用いて着色される方法。
条項２４．対応する近環境から導出された複数の近距離点と、対応する遠距離環境から導出された複数の遠距離点とを含むライダー点クラウドをＳＬＡＭを用いて取得する段階を含み、遠距離点が、近環境内に位置する１又は２以上の要素の間の１又は２以上の空間を通して走査され、ＳＬＡＭが近環境から遠環境へと移動する時に複数の遠距離点を利用してＳＬＡＭの方位が定められる方法。
条項２５．ＳＬＡＭシステムにおいてカメラ及びレーザのうちの少なくとも一方から複数のフィードバック項を含むフィードバックを受信する段階と、各々が複数のフィードバック項に関連付けられた複数のバイアスをモデル化する段階とを含み、複数のバイアスが、バイアスのスライドウィンドウを形成する方法。
条項２６．複数のフィードバック項の各々がＳＬＡＭシステムの推定区分的運動を含む条項２５の方法。
条項２７．複数のバイアス項の各々が区分的運動中に一定であるようにモデル化される条項２６の方法。
条項２８．スライドウィンドウが２００個と１０００個の間のバイアスを含む条項２５の方法。
条項２９．スライドウィンドウが約４００個のバイアスを含む条項２８の方法。
条項３０．スライドウィンドウが複数のバイアスの更新速度を決定するためのパラメータとして機能する条項２５の方法。
条項３１．スライドウィンドウがＳＬＡＭシステムの動的再構成を可能にするようになっている条項２５の方法。
条項３２．複数のバイアスに対してＩＭＵバイアス補正を実施する段階を更に含む条項２５の方法。
条項３３．ＩＭＵバイアス補正を実施する段階がレーザ及びカメラのうちの少なくとも一方からのデータを利用する段階を含む条項３２の方法。
条項３４．レーザ及びカメラがＳＬＡＭシステムの一部を形成する条項３３の方法。
条項３５．レーザ及びカメラの各々が経時的な推定区分的運動を含む条項３３の方法。
条項３６．スライドウィンドウが事前決定個数のバイアスで形成されたアレイであり、バイアスが先入れ／先出し方式で追加及び削除される条項２５の方法。
条項３７．カメラ及びＩＭＵがＳＬＡＭシステムの一部を形成するカメラから視覚データを受信し、かつＩＭＵから慣性データを受信する段階と、ＳＬＡＭシステムの区分的運動を視覚データ及び慣性データを制約として用いて推定する段階と、視覚データから導出された１又は２以上の視覚特徴部に深度情報を関連付ける段階とを含む方法。
条項３８．深度情報がレーザデータから取得される条項３７の方法。
条項３９．レーザデータが取得不能である場合に、深度情報を推定区分的運動を用いた三角測位から計算することができる条項３８の方法。
条項４０．深度情報が位置合わせされた点クラウドを構築するために利用される条項３７の方法。
条項４１．点クラウドに関する１又は２以上の固有ベクトルを計算し、１又は２以上の固有ベクトルを用いて点クラウドの劣化を指定する段階を更に含む条項４０の方法。
条項４２．固有ベクトルのうちの少なくとも１つによって示される状態空間の方向の劣化がその方向の解を破棄するために利用される条項４１の方法。
条項４３．両方共にＳＬＡＭシステムの一部を形成するカメラ座標系を有するカメラとレーザ座標系を有するレーザとの間の相対姿勢をカメラとレーザとに対して単一の座標系を利用することによって決定する段階と、レーザとＳＬＡＭシステムの一部を形成し、ＩＭＵ座標系を有するＩＭＵとの間の相対姿勢を決定する段階とを含む方法。
条項４４．単一の座標系を利用する段階が前処理中に複数のレーザ点をカメラ座標系内に投影する段階を含む条項４３の方法。
条項４５．ＩＭＵ座標系がカメラ座標系に対してほぼ平行である条項４３の方法。
条項４６．ＩＭＵからのＩＭＵデータをその取得時に回転補正する段階を更に含む条項４５の方法。
条項４７．カメラ座標系が、ｘ軸が左を指向し、ｙ軸が上を指向し、ｚ軸が前を指向してカメラの主軸と一致するカメラ光学中心を原点とする条項４３の方法。
条項４８．ＩＭＵ座標系が、ｘ軸、ｙ軸、及びｚ軸が同じ方向に対して平行であり、かつ同じ方向を指向するＩＭＵ測定中心を原点とする条項４３のシステム。
条項４９．世界座標系が、ＳＬＡＭシステムの初期姿勢と一致する座標系である条項４３のシステム。
条項５０．１又は２以上の姿勢制約を利用してモバイルマッピングシステムの運動モデルを確立する段階と、１又は２以上のカメラ制約を利用して１又は２以上のランドマーク位置を含むモバイルマッピングシステムのランドマーク測定モデルを確立する段階と、運動モデル及びランドマーク測定の各々に関して求解する段階とを含む方法。
条項５１．各モデルの求解段階がニュートン勾配降下法を利用する段階を含む条項５０の方法。
条項５２．ニュートン勾配降下法が外れ特徴部の除去のためのロバストな当て嵌めフレームワークに適応される条項５１の方法。
条項５３．求解が複数のランドマーク位置を最適化することなく実施される条項５０の方法。
条項５４．複数の主要点を含むオドメトリ推定をモバイルマッピングシステムの視覚－慣性オドメトリモジュールから受信する段階と、モバイルマッピングシステムの一部を形成するＩＭＵから複数のＩＭＵ測定値を受信する段階と、モバイルマッピングシステムの一部を形成するレーザから収集された複数のレーザ点をＩＭＵ測定値に少なくとも部分的に基づいて位置合わせする段階とを含む方法。
条項５５．位置合わせする段階が、複数の主要点間を内挿するためにＩＭＵ測定値を利用する段階を含む条項５４の方法。
条項５６．内挿段階が、主要点で形成された点クラウドから追跡に用いられる１又は２以上の幾何学特徴部を選択する段階を含む条項５５の方法。
条項５７．点クラウド内の不良点が幾何学形状に基づいて選択されない条項５６の方法。
条項５８．点クラウド内の不良点が１又は２以上の点と点クラウド内の面との関係に基づいて選択されない条項５６の方法。
条項５９．各々が同一のサイズの第１の容積を占有する複数の第１のレベルのボクセル内に点クラウドを含むマップ情報を格納する段階と、各々が同一のサイズの第２の容積を占有する複数の第２のレベルのボクセル内にマップ情報を格納する段階であって、第１の容積が第２の容積よりも大きい条項格納する段階と、モバイルマッピングシステムのレーザスキャナに近接する第２のレベルのボクセルからマップ情報を取り出す段階と、取り出された第２のレベルのボクセルに対して走査整合を実施する段階と、第１のレベルのボクセルで構成されたマップ情報を第１のレベルのボクセルを用いて維持する段階とを含む方法。
条項６０．各第１のレベルのボクセルが複数の第２のレベルのボクセルにマッピングされる条項５９の方法。
条項６１．第２のレベルのボクセルが３Ｄ ＫＤ木内に格納される条項５９の方法。
条項６２．ほぼ一定の点密度を維持するために点クラウドをダウンサイズする段階を更に含む条項５９の方法。
条項６３．地上発生点クラウドマップを与える段階と、空中発生点クラウドマップを発生させる段階と、地上発生点クラウドマップと空中発生点クラウドマップとを実時間又は近実時間で融合する段階とを含む方法。
条項６４．空中発生点クラウドマップが、モバイルマッピングシステムを含むドローンを利用して発生される条項６３の方法。
条項６５．ドローンがレーザスキャナ、カメラ、及び低品質ＩＭＵを含む条項６３の方法。
条項６６．レーザスキャナ、カメラ、及びＩＭＵからのデータが多層最適化処理によって処理される条項６５の方法。
条項６７．融合する段階が、地上導出マップからの少なくとも１つの出力を空中発生点クラウドマップに対して定位する段階を更に含む条項６３の方法。
条項６８．空中発生点クラウドマップを発生させる段階が、ドローン飛行経路を定義する段階と、ドローン飛行経路に従ってドローンを自律飛行させる段階とを含む条項６４の方法。
本発明の開示の少数の実施形態のみを図示して説明したが、以下に続く特許請求の範囲に記載する本発明の開示の精神及び範囲から逸脱することなくこれらの実施形態に多くの変更及び修正を加えることができることが当業者には明らかであろう。本明細書で引用する海外及び国内の両方の全ての特許出願及び特許、並びに全ての他の公開文献は、その全内容が、法律によって許可されている最大限度において本明細書に組み込まれている。
本明細書で説明する方法及びシステムは、プロセッサ上でコンピュータソフトウェア、プログラムコード、及び／又は命令を実行する機械によって部分的又は全体的に配備することができる。本発明の開示内容は、機械上の方法として、機械の一部としての又は機械に関連付けられたシステム又は装置として、又はコンピュータ読取可能媒体に具現化されて機械のうちの１又は２以上の上で実行されるコンピュータプログラム製品として実施することができる。実施形態では、プロセッサは、サーバ、クラウドサーバ、クライアント、ネットワークインフラストラクチャ、モバイルコンピュータプラットフォーム、固定コンピュータプラットフォーム、又は他のコンピュータプラットフォームの一部とすることができる。プロセッサは、プログラム命令、コード、及びバイナリ命令などを実行することができるあらゆるタイプの計算デバイス又は処理デバイスとすることができる。プロセッサは、信号プロセッサ、デジタルプロセッサ、内蔵プロセッサ、マイクロコントローラ、又は内部に格納されたプログラムコード又はプログラム命令の実行を直接的又は間接的に容易にするコプロセッサ（数値演算コプロセッサ、グラフィックコプロセッサ、及び通信コプロセッサなど）などのようなあらゆる変形とすることができる又はこれらを含むことができる。それに加えて、プロセッサは、複数のプログラム、スレッド、及びコードの実行を可能にすることができる。複数のスレッドは、プロセッサの性能を向上させるために又はアプリケーションの同時作動を容易にするために同時に実行することができる。実施により、本明細書で説明する方法、プログラムコード、及びプログラム命令などは、１又は２以上のスレッドに実施することができる。スレッドは、割り当て優先度をそれに関連付けることができる他のスレッドを生み出すことができ、プロセッサは、プログラムコード内で与えられる命令に基づく優先度又はいずれかの他の順序に基づいてこれらのスレッドを実行することができる。プロセッサ又はそれを利用するあらゆる機械は、本明細書及びいずれかの他の場所で説明する方法、コード、命令、及びプログラムを格納する非一時的メモリを含むことができる。プロセッサは、本明細書及びいずれかの他の場所で説明する方法、コード、及び命令を格納することができる非一時的格納媒体にインターフェースを通じてアクセスすることができる。方法、プログラム、コード、プログラム命令、又はコンピュータデバイス又は処理デバイスが実行することができる他のタイプの命令を格納するためにプロセッサに関連付けられた格納媒体は、ＣＤ－ＲＯＭ、ＤＶＤ、メモリ、ハードディスク、フラッシュデバイス、ＲＡＭ、ＲＯＭ、及びキャッシュなどのうちの１又は２以上を含むことができるが、これらに限定されない場合もある。
プロセッサは、マルチプロセッサの速度及び性能を向上させることができる１又は２以上のコアを含むことができる。実施形態では、処理は、２又は３以上の独立したコア（ダイと呼ぶ）を組み合わせるデュアルコアプロセッサ、クワッドコアプロセッサ、及び他のチップレベルマルチプロセッサなどとすることができる。
本明細書で説明する方法及びシステムは、サーバ、クライアント、ファイアウォール、ゲートウェイ、ハブ、ルータ、又は他のそのようなコンピュータ及び／又はネットワーク接続ハードウェア上でコンピュータソフトウェアを実行する機械によって部分的又は全体的に配備することができる。ソフトウェアプログラムは、ファイルサーバ、プリントサーバ、ドメインサーバ、インターネットサーバ、イントラネットサーバ、クラウドサーバ、並びに２次サーバ、ホストサーバ、及び分散サーバなどのような他の変形を含むことができるサーバに関連付けることができる。サーバは、メモリ、プロセッサ、コンピュータ読取可能媒体、格納媒体、ポート（物理的及び仮想的）、通信デバイス、及び他のサーバ、クライアント、機械、及びデバイスに有線又は無線の媒体を通じてアクセスすることができるインターフェースなどのうちの１又は２以上を含むことができる。本明細書及びいずれかの他の場所で説明する方法、プログラム、又はコードは、サーバによって実行することができる。それに加えて、本出願で説明する方法の実行に必要とされる他のデバイスは、サーバに関連付けられたインフラストラクチャの一部と見なすことができる。
サーバは、クライアント、他のサーバ、プリンタ、データベースサーバ、プリントサーバ、ファイルサーバ、通信サーバ、分散サーバ、及びソーシャルネットワークなどを制限なく含む他のデバイスへのインターフェースを提供することができる。それに加えて、この結合及び／又は接続は、ネットワークにわたるプログラムのリモート実行を容易にすることができる。これらのデバイスのうちの一部又は全てのもののネットワーク接続は、本発明の開示の範囲から逸脱することなく１又は２以上の場所でのプログラム又は方法の並列処理を容易にすることができる。それに加えて、インターフェースを通じてサーバに取り付けられたデバイスのうちのいずれも、方法、プログラム、コード、及び／又は命令を格納することができる少なくとも１つの格納媒体を含むことができる。中央リポジトリは、異なるデバイス上で実行されることになるプログラム命令を提供することができる。この実施では、リモートリポジトリは、プログラムコード、命令、及びプログラムに対する格納媒体としての役割を果たすことができる。
ソフトウェアプログラムは、ファイルクライアント、プリントクライアント、ドメインクライアント、インターネットクライアント、イントラネットクライアント、並びに２次クライアント、ホストクライアント、及び分散クライアントなどのような他の変形を含むことができるクライアントに関連付けることができる。クライアントは、メモリ、プロセッサ、コンピュータ読取可能媒体、格納媒体、ポート（物理的及び仮想的）、通信デバイス、及び他のクライアント、サーバ、機械、及びデバイスに有線又は無線の媒体を通じてアクセスすることができるインターフェースなどのうちの１又は２以上を含むことができる。本明細書及びいずれかの他の場所で説明する方法、プログラム、又はコードは、クライアントによって実行することができる。それに加えて、本出願で説明する方法の実行に必要とされる他のデバイスは、クライアントに関連付けられたインフラストラクチャの一部と見なすことができる。
クライアントは、サーバ、他のクライアント、プリンタ、データベースサーバ、プリントサーバ、ファイルサーバ、通信サーバ、及び分散サーバなどを制限なく含む他のデバイスへのインターフェースを提供することができる。それに加えて、この結合及び／又は接続は、ネットワークにわたるプログラムのリモート実行を容易にすることができる。これらのデバイスのうちの一部又は全てのもののネットワーク接続は、本発明の開示の範囲から逸脱することなく１又は２以上の場所でのプログラム又は方法の並列処理を容易にすることができる。それに加えて、インターフェースを通じてクライアントに取り付けられたデバイスのうちのいずれも、方法、プログラム、用途、コード、及び／又は命令を格納することができる少なくとも１つの格納媒体を含むことができる。中央リポジトリは、異なるデバイス上で実行されることになるプログラム命令を提供することができる。この実施では、リモートリポジトリは、プログラムコード、命令、及びプログラムに対する格納媒体としての役割を果たすことができる。
本明細書で説明する方法及びシステムは、ネットワークインフラストラクチャによって部分的又は全体的に配備することができる。ネットワークインフラストラクチャは、コンピュータデバイス、サーバ、ルータ、ハブ、ファイアウォール、クライアント、パーソナルコンピュータ、通信デバイス、ルーティングデバイス、他の能動的又は受動的なデバイス、モジュール、及び／又は当業技術で公知の構成要素のような要素を含むことができる。ネットワークインフラストラクチャに関連付けられたコンピュータデバイス及び／又は非コンピュータデバイスは、他の構成要素はともかく、フラッシュメモリ、バッファ、スタック、ＲＡＭ、及びＲＯＭなどのような格納媒体を含むことができる。本明細書及びいずれかの他の場所で説明する処理、方法、プログラムコード、命令は、ネットワークインフラストラクチャ要素のうちの１又は２以上によって実行することができる。本明細書で説明する方法及びシステムは、ソフトウェア・アズ・ア・サービス（ＳａａＳ）、プラットフォーム・アズ・ア・サービス（ＰａａＳ）、及び／又はインフラストラクチャ・アズ・ア・サービス（ＩａａＳ）の特徴を伴うものを含むいずれかのタイプのプライベート、コミュニティ、又はハイブリッドのクラウドコンピュータネットワーク又はクラウドコンピュータ環境との併用に対して適応させることができる。
本明細書及びいずれかの他の場所で説明する方法、プログラムコード、及び命令は、送信側制御式接触媒体コンテンツ項目複数セルを有するセルラーネットワーク上に実施することができる。セルラーネットワークは、周波数分割多重アクセス（ＦＤＭＡ）ネットワーク又はコード分割多重アクセス（ＣＤＭＡ）ネットワークのいずれかとすることができる。セルラーネットワークは、モバイルデバイス、セルサイト、基地局、リピータ、アンテナ、及び電波塔などを含むことができる。セルネットワークは、ＧＳＭ（登録商標）、ＧＰＲＳ、３Ｇ、ＥＶＤＯ、メッシュ、又は他のネットワークタイプのものとすることができる。
本明細書及びいずれかの他の場所で説明する方法、プログラムコード、及び命令は、モバイルデバイス上で又はこのデバイスによって実施することができる。モバイルデバイスは、ナビゲーションデバイス、セル電話、モバイル電話、モバイル携帯情報端末、ラップトップ、パームトップ、ネットブック、ページャ、電子書籍読取器、及び音楽プレーヤなどを含むことができる。これらのデバイスは、他の構成要素はともかく、フラッシュメモリ、バッファ、ＲＡＭ、ＲＯＭ、及び１又は２以上のコンピュータデバイスのような格納媒体を含むことができる。モバイルデバイスに関連付けられたコンピュータデバイスは、その上に格納されたプログラムコード、方法、及び命令を実行するように作動させることができる。これに代えて、モバイルデバイスは、他のデバイスと協調して命令を実行するように構成することができる。モバイルデバイスは、サーバとインターフェース接続されてプログラムコードを実行するように構成された基地局と通信することができる。モバイルデバイスは、ピアツーピアネットワーク、メッシュネットワーク、又は他の通信ネットワーク上で通信を行うことができる。プログラムコードは、サーバに関連付けられた格納媒体上に格納することができ、サーバの内部に埋め込まれたコンピュータデバイスによって実行することができる。基地局は、コンピュータデバイスと格納デバイスとを含むことができる。格納デバイスは、基地局に関連付けられたコンピュータデバイスによって実行されるプログラムコード及び命令を格納することができる。
コンピュータソフトウェア、プログラムコード、及び／又は命令は、コンピュータ構成要素、デバイス、及びある程度の時間間隔にわたってコンピュータに用いられるデジタルデータを保持する記録媒体と、ランダムアクセスメモリ（ＲＡＭ）として公知の半導体ストレージと、光学ディスクなど、ハードディスク、テープ、ドラム、カード、及び他のタイプのもののような形態の磁気ストレージのようなより永久的な格納のための大容量ストレージと、プロセッサレジスタ、キャッシュメモリ、揮発性メモリ、不揮発性メモリと、ＣＤ、ＤＶＤのような光学ストレージと、フラッシュメモリ（例えばＵＳＢスティック又はＵＳＢキー）、フロッピーディスク、磁気テープ、紙テープ、パンチカード、独立型ＲＡＭディスク、Ｚｉｐドライブ、着脱可能大容量ストレージ、及びオフラインなどのような着脱可能媒体と、動的メモリ、静的メモリ、読取／書込ストレージ、可変ストレージ、読取専用ストレージ、ランダムアクセスストレージ、順次アクセスストレージ、ロケーションアドレス指定可能ストレージ、ファイルアドレス指定可能ストレージ、コンテンツアドレス指定可能ストレージ、ネットワーク接続ストレージ、ストレージエリアネットワーク、バーコード、及び磁気インクなどのような他のコンピュータメモリとを含むことができる機械読取可能媒体上に格納される及び／又はそこにアクセスすることができる。
本明細書で説明する方法及びシステムは、物理的なもの及び／又は無形のものを１つの状態から別の状態へと変換することができる。更に本明細書で説明する方法及びシステムは、物理的なもの及び／又は無形のものを表すデータを１つの状態から別の状態へと変換することもできる。
図を通して流れ図及びブロック図のものを含む本明細書で説明して描いた要素は、要素間の論理境界を含意する。しかし、ソフトウェア工学又はハードウェア工学の慣例によると、描いた要素及びその機能は、モノリシックソフトウェア構造、独立型ソフトウェアモジュール、又は外部のルーチン、コード、及びサービスなどを用いるモジュールとして内部に格納されたプログラム命令を実行することができる送信側制御式接触媒体コンテンツ項目プロセッサを有する機械上にコンピュータ実行可能媒体を通じて実施することができ、全てのそのような実施は、本発明の開示の範囲内にあるものとすることができる。そのような機械の例は、携帯情報端末、ラップトップ、パーソナルコンピュータ、モバイル電話、他の手持ち式コンピュータデバイス、医療機器、有線又は無線の通信デバイス、トランスデューサ、チップ、計算機、衛星、タブレットＰＣ、電子書籍、ガジェット、電子デバイス、送信側制御式接触媒体コンテンツ項目人工知能を有するデバイス、コンピュータデバイス、ネットワーク接続機器、サーバ、及びルータなどを含むことができるが、これらに限定されない場合もある。更に、流れ図及びブロック図に描いている要素又はいずれかの他の論理構成要素は、プログラム命令を実行することができる機械上に実施することができる。従って、明示的に述べない限り又は状況から別途明白でない限り、前述の図面及び説明は本発明の開示のシステムの機能態様を示しているが、これらの機能態様を実施するためのソフトウェアのいずれの特定の配列もこれらの説明から推断すべきではない。同様に、上記で図示して説明した様々な段階は変更することができ、段階の順序を本明細書に開示する技術の特定の用途に適応させることができることは認められるであろう。全てのそのような変更及び修正は、本発明の開示の範囲に収まることを意図している。従って、特定の用途によって必要とされない限り、明示的に述べない限り、又は状況から別途明白でない限り、様々な段階に関する順序の描写及び／又は説明は、これらの段階に対する特定の実行順序を必要とすると理解すべきではない。
上記で説明した方法及び／又は処理、並びに関連の段階は、ハードウェア、ソフトウェア、又は特定の用途に適するハードウェアとソフトウェアのいずれかの組み合わせで実現することができる。ハードウェアは、汎用コンピュータ及び／又は専用コンピュータデバイス、又は特定のコンピュータデバイス又はその特定の態様又は構成要素を含むことができる。処理は、１又は２以上のマイクロプロセッサ、マイクロコントローラ、埋め込みマイクロコントローラ、プログラミング可能デジタル信号プロセッサ、又は他のプログラミング可能デバイスに内部メモリ及び又は外部メモリを併せたものに実現することができる。処理は、同じく又は代わりに特定用途向け集積回路、プログラミング可能ゲートアレイ、プログラミング可能アレイ論理部、又はいずれかの他のデバイス、又は電子信号を処理するように構成することができるデバイスの組み合わせに具現化することができる。処理のうちの１又は２以上は、機械読取可能媒体上で実行することができるコンピュータ実行可能コードとして実現することができることは更に認められるであろう。
コンピュータ実行可能コードは、Ｃのような構造化プログラミング言語、Ｃ＋＋のようなオブジェクト指向プログラミング言語、又は上記のデバイスのうちの１つの上、並びにプロセッサ、プロセッサアーキテクチャの異種組み合わせの上、異なるハードウェアとソフトウェアの組み合わせの上、又はプログラム命令を実行することができるいずれかの他の機械上で実行されるように格納、コンパイル実行、又はインタープリタ実行することができるいずれかの他の高レベル又は低レベルのプログラミング言語（アッセンブリ言語、ハードウェア記述言語、並びにデータベースプログラミングの言語及び技術）を用いて作成することができる。
従って、一態様では、上記で説明した方法及びその組み合わせは、１又は２以上のコンピュータデバイス上で実行された時にこれらの方法の段階を実施するコンピュータ実行可能コードに具現化することができる。別の態様では、本方法は、その段階を実施するシステムに具現化することができ、一部の手法でデバイスにわたって分散させることができ、又は機能の全てを専用の独立型デバイス又は他のハードウェア内に統合することができる。別の態様では、上記で説明した処理に関連付けられた段階を実施するための手段は、上記で説明したハードウェア及び／又はソフトウェアのいずれかを含むことができる。全てのそのような組み替え及び組み合わせは、本発明の開示の範囲に収まることを意図している。
本発明の開示は、詳細に図示して説明した好ましい実施形態に関連して開示したが、当業者には、これらの実施形態に対する様々な修正及び改善が直ちに明らかになるであろう。従って、本発明の開示の精神及び範囲は、前述の例によって限定されるべきではなく、法律によって許容可能な最も広義の意味で理解されるべきである。
本発明の開示を説明する状況（特に以下に続く特許請求の範囲の状況）での用語「ａ」及び「ａｎ」及び「ｔｈｅ」及び類似の指示物は、本明細書で別途示さない限り、又は状況が明らかに矛盾しない限り、単数と複数の両方の指示物を網羅するように解釈されるものとする。「備える」、「送信側制御式接触媒体コンテンツ項目を有する」、「含む」、及び「含有する」という用語は、別途特筆しない限り、非限定的な用語である（すなわち、「～を含むがそれに限定されない」）と解釈されるものとする。本明細書における値範囲の具陳は、当該範囲内に収まる各別個の値を個々に記す上での略記法としての役割を果たすことしか意図しておらず、本明細書で別途示さない限り、各別個の値は、これらの値が本明細書において個々に具陳されているかのように本明細書に組み込まれている。本明細書に開示する全ての方法は、本明細書で別途示さない限り、他に状況が明らかに矛盾しない限り、あらゆる適切な順序で実施することができる。本明細書に提示するいずれかの及び全ての例又は例示的文言（例えば「～等」）の使用は、本発明の開示をより明らかにすることしか意図しておらず、別途主張しない限り、本発明の開示の範囲に対して制限を課することはない。本明細書におけるいずれの文言も、本発明の開示の実施にとっていずれかの非請求要素が不可欠であることを示すものと解釈すべきではない。
前述の書面による説明は、当業者が現時点で最良のモードと考えられるものを作って用いることを可能にするが、当業者は、本明細書における特定の実施形態、方法、及び実施例の変更、組み合わせ、及び均等物の存在を理解及び認識するであろう。従って、本発明の開示は、上記で説明した実施形態、方法、及び実施例によってではなく、本発明の開示の範囲及び精神に入る全ての実施形態及び方法によって限定されるべきである。
本明細書に引用する全ての文献は、これにより引用によって組み込まれる。
２１００ ＳＬＡＭユニット
２１０２ マイクロコントローラ
２１０４ ＣＰＵ
２１０６ ＩＭＵ
２１０８ 直交復号器

Claims (28)

  1. ＩＭＵデバイスからデータを第１の計算モジュールで第１の周波数で受信し、該受信ＩＭＵデータに少なくとも部分的に基づいてモバイルマッピングシステムの第１の推定位置を計算する段階と、
    前記第１の推定位置及び視覚－慣性データを第２の計算モジュールで第２の周波数で受信し、該第１の推定位置及び前記視覚－慣性データに少なくとも部分的に基づいて前記モバイルマッピングシステムの第２の推定位置を計算する段階と、
    前記第２の推定位置及びレーザ走査データを第３の計算モジュールで第３の周波数で受信し、該第２の推定位置及び前記レーザ走査データに少なくとも部分的に基づいて前記モバイルマッピングシステムの第３の推定位置を計算する段階と、を含み、
    少なくとも１つの計算モジュールの作動が迂回される、ことを特徴とする方法。
  2. 前記第１の周波数、前記第２の周波数、及び前記第３の周波数の各々が、互いに異なっていることを特徴とする請求項１に記載の方法。
  3. 前記第１の周波数は、前記第２の周波数よりも高く、該第２の周波数は、前記第３の周波数よりも高いことを特徴とする請求項２に記載の方法。
  4. 残りの迂回されないモジュールの各々によって計算された前記推定位置は、線形様式で組み合わされることを特徴とする請求項１に記載の方法。
  5. 残りの迂回されないモジュールの各々によって計算された前記推定位置は、非線形様式で組み合わされることを特徴とする請求項１に記載の方法。
  6. 前記第１の計算モジュールが迂回される時に、前記第２の推定位置は、前記第１の推定位置を参照することなく前記視覚慣性データから計算されることを特徴とする請求項１に記載の方法。
  7. 前記第２の計算モジュールが迂回される時に、前記第３の推定位置は、前記レーザ走査データ及び前記第１の推定位置から計算されることを特徴とする請求項１に記載の方法。
  8. 問題状態空間内の劣化部分空間を決定し、該劣化部分空間内の推定位置を計算する段階を更に含むことを特徴とする請求項１に記載の方法。
  9. 前記劣化部分空間は、十分に条件付けされた部分空間であることを特徴とする請求項８に記載の方法。
  10. 前記第２の計算モジュール又は前記第３の計算モジュールのうちの少なくとも一方によって計算された前記推定位置は、先行計算モジュールによって入力として受信されることを特徴とする請求項１に記載の方法。
  11. 前記モバイルマッピングシステムは、高速の角速度で作動するようになっていることを特徴とする請求項１に記載の方法。
  12. 前記高速の角速度は、３６０度／秒の回転速度を含むことを特徴とする請求項１１に記載の方法。
  13. 前記モバイルマッピングシステムは、高速の線速度で作動するようになっていることを特徴とする請求項１に記載の方法。
  14. 前記高速の線速度は、１００キロメートル／時の速度を含むことを特徴とする請求項１３に記載の方法。
  15. モバイルマッピングシステムであって、
    ＩＭＵデバイスから第１の周波数でデータを受信し、該受信ＩＭＵデータに少なくとも部分的に基づいてモバイルマッピングシステムの第１の推定位置を計算するようになった第１の計算モジュールと、
    前記第１の推定位置及び視覚－慣性データを第２の周波数で受信し、該第１の推定位置及び前記視覚－慣性データに少なくとも部分的に基づいてモバイルマッピングシステムの第２の推定位置を計算するようになった第２の計算モジュールと、
    前記第２の推定位置及びレーザ走査データを第３の周波数で受信し、該第２の推定位置及び前記レーザ走査データに少なくとも部分的に基づいてモバイルマッピングシステムの第３の推定位置を計算するようになった第３の計算モジュールと、を含み、
    少なくとも１つの計算モジュールの作動が迂回される、ことを特徴とするモバイルマッピングシステム。
  16. 前記第１の周波数、前記第２の周波数、及び前記第３の周波数の各々が、互いに異なっていることを特徴とする請求項１５に記載のモバイルマッピングシステム。
  17. 前記第１の周波数は、前記第２の周波数よりも高く、該第２の周波数は、前記第３の周波数よりも高いことを特徴とする請求項１６に記載のモバイルマッピングシステム。
  18. 残りの迂回されないモジュールの各々によって計算された前記推定位置は、線形様式で組み合わされることを特徴とする請求項１５に記載のモバイルマッピングシステム。
  19. 残りの迂回されないモジュールの各々によって計算された前記推定位置は、非線形様式で組み合わされることを特徴とする請求項１５に記載のモバイルマッピングシステム。
  20. 前記第１の計算モジュールが迂回される時に、前記第２の推定位置は、前記第１の推定位置を参照することなく前記視覚慣性データから計算されることを特徴とする請求項１５に記載のモバイルマッピングシステム。
  21. 前記第２の計算モジュールが迂回される時に、前記第３の推定位置は、前記レーザ走査データ及び前記第１の推定位置から計算されることを特徴とする請求項１５に記載のモバイルマッピングシステム。
  22. 問題状態空間内の劣化部分空間を決定し、該劣化部分空間内の推定位置を計算することを更に含むことを特徴とする請求項１５に記載のモバイルマッピングシステム。
  23. 前記劣化部分空間は、十分に条件付けされた部分空間であることを特徴とする請求項２２に記載のモバイルマッピングシステム。
  24. 前記第２の計算モジュール又は前記第３の計算モジュールのうちの少なくとも一方によって計算された前記推定位置は、先行計算モジュールによって入力として受信されることを特徴とする請求項１５に記載のモバイルマッピングシステム。
  25. 前記モバイルマッピングシステムは、高速の角速度で作動するようになっていることを特徴とする請求項１５に記載のモバイルマッピングシステム。
  26. 前記高速の角速度は、３６０度／秒の回転速度を含むことを特徴とする請求項２５に記載のモバイルマッピングシステム。
  27. 前記モバイルマッピングシステムは、高速の線速度で作動するようになっていることを特徴とする請求項１５に記載のモバイルマッピングシステム。
  28. 前記高速の線速度は、１００キロメートル／時の速度を含むことを特徴とする請求項２７に記載のモバイルマッピングシステム。
JP2019540661A 2017-01-27 2018-01-26 実時間オンライン自己運動推定を備えたレーザスキャナ Active JP7141403B2 (ja)

Applications Claiming Priority (7)

Application Number Priority Date Filing Date Title
US201762451294P 2017-01-27 2017-01-27
US62/451,294 2017-01-27
PCT/US2017/021120 WO2017155970A1 (en) 2016-03-11 2017-03-07 Laser scanner with real-time, online ego-motion estimation
USPCT/US2017/021120 2017-03-07
USPCT/US2017/055938 2017-10-10
PCT/US2017/055938 WO2018071416A1 (en) 2016-10-11 2017-10-10 Laser scanner with real-time, online ego-motion estimation
PCT/US2018/015403 WO2018140701A1 (en) 2017-01-27 2018-01-26 Laser scanner with real-time, online ego-motion estimation

Publications (2)

Publication Number Publication Date
JP2020507072A JP2020507072A (ja) 2020-03-05
JP7141403B2 true JP7141403B2 (ja) 2022-09-22

Family

ID=62979592

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2019540661A Active JP7141403B2 (ja) 2017-01-27 2018-01-26 実時間オンライン自己運動推定を備えたレーザスキャナ

Country Status (3)

Country Link
EP (1) EP3574285A4 (ja)
JP (1) JP7141403B2 (ja)
WO (1) WO2018140701A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2026064764A1 (en) * 2024-09-23 2026-03-26 Faro Technologies, Inc. Methods of using simultaneous localization for angle measurement and tracking

Families Citing this family (48)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11573325B2 (en) 2016-03-11 2023-02-07 Kaarta, Inc. Systems and methods for improvements in scanning and mapping
US10989542B2 (en) 2016-03-11 2021-04-27 Kaarta, Inc. Aligning measured signal data with slam localization data and uses thereof
JP6987797B2 (ja) 2016-03-11 2022-01-05 カールタ インコーポレイテッド リアルタイムオンラインエゴモーション推定を有するレーザスキャナ
WO2019006289A1 (en) * 2017-06-30 2019-01-03 Kaarta, Inc. SYSTEMS AND METHODS FOR SCAN ENHANCEMENT AND CORRECTION
US11567201B2 (en) 2016-03-11 2023-01-31 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
WO2019099605A1 (en) 2017-11-17 2019-05-23 Kaarta, Inc. Methods and systems for geo-referencing mapping systems
WO2019165194A1 (en) 2018-02-23 2019-08-29 Kaarta, Inc. Methods and systems for processing and colorizing point clouds and meshes
WO2019195270A1 (en) 2018-04-03 2019-10-10 Kaarta, Inc. Methods and systems for real or near real-time point cloud map data confidence evaluation
WO2020009826A1 (en) 2018-07-05 2020-01-09 Kaarta, Inc. Methods and systems for auto-leveling of point clouds and 3d models
DE102018214694A1 (de) 2018-08-30 2020-03-05 Continental Automotive Gmbh Lokalisierungsvorrichtung zur visuellen Lokalisierung eines Fahrzeugs
JP2020042329A (ja) * 2018-09-06 2020-03-19 株式会社ユニックス 建築用資材プレ加工システム及び建築用資材プレ加工方法
US11023742B2 (en) * 2018-09-07 2021-06-01 Tusimple, Inc. Rear-facing perception system for vehicles
CN111175775A (zh) 2018-11-09 2020-05-19 阿里巴巴集团控股有限公司 定位数据生成方法、装置以及电子设备
CN111174777A (zh) 2018-11-09 2020-05-19 阿里巴巴集团控股有限公司 定位方法、装置以及电子设备
CA3028653C (en) 2018-11-13 2021-02-16 Beijing Didi Infinity Technology And Development Co., Ltd. Methods and systems for color point cloud generation
CN109945890B (zh) * 2018-11-21 2022-01-25 财团法人车辆研究测试中心 多定位系统切换与融合校正方法及其装置
US10991117B2 (en) * 2018-12-23 2021-04-27 Samsung Electronics Co., Ltd. Performing a loop closure detection
CN111380515B (zh) * 2018-12-29 2023-08-18 纳恩博（常州）科技有限公司 定位方法及装置、存储介质、电子装置
WO2020176838A1 (en) * 2019-02-28 2020-09-03 Brain Corporation Systems, and methods for merging disjointed map and route data with respect to a single origin for autonomous robots
DE102019206036A1 (de) 2019-04-26 2020-10-29 Volkswagen Aktiengesellschaft Verfahren und Vorrichtung zur Bestimmung der geografischen Position und Orientierung eines Fahrzeugs
CN110243358B (zh) * 2019-04-29 2023-01-03 武汉理工大学 多源融合的无人车室内外定位方法及系统
CN110132306B (zh) * 2019-05-20 2021-02-19 广州小鹏汽车科技有限公司 车辆定位误差的纠正方法及系统
CN110132302A (zh) * 2019-05-20 2019-08-16 中国科学院自动化研究所 融合imu信息的双目视觉里程计定位方法、系统
CN110298103A (zh) * 2019-06-25 2019-10-01 中国电建集团成都勘测设计研究院有限公司 基于无人机机载三维激光扫描仪的高陡危岩体调查方法
CN110243375A (zh) * 2019-06-26 2019-09-17 汕头大学 一种同时构建二维地图和三维地图的方法
GB2587794A (en) * 2019-08-27 2021-04-14 Hybird Ltd Inspection apparatus and method
JP7552608B2 (ja) * 2019-10-03 2024-09-18 ソニーグループ株式会社 情報処理装置、情報処理方法、及びプログラム
DE102019216548A1 (de) * 2019-10-28 2021-04-29 DeepUp GmbH Verfahren und mobile Erfassungsvorrichtung zur Erfassung von Infrastrukturelementen eines unterirdischen Leitungsnetzwerks
FR3103283B1 (fr) * 2019-11-20 2022-07-29 Thales Sa Dispositif electronique et procede d'estimation de position(s) d'element(s) de trafic aerien, systeme d'affichage et programme d'ordinateur associes
CN111076724B (zh) * 2019-12-06 2023-12-22 苏州艾吉威机器人有限公司 三维激光定位方法及系统
CN111009036B (zh) * 2019-12-10 2023-11-21 北京歌尔泰克科技有限公司 同步定位与地图构建中栅格地图的修正方法、装置
CN111258313B (zh) * 2020-01-20 2022-06-07 深圳市普渡科技有限公司 多传感器融合slam系统及机器人
DE102020206930A1 (de) * 2020-06-03 2021-12-09 Zf Friedrichshafen Ag Anpassung einer vertikalen Region-of-Interest
US12270662B2 (en) * 2020-06-30 2025-04-08 Lyft, Inc. Map generation using two sources of sensor data
US12437359B2 (en) * 2020-11-13 2025-10-07 Pioneer Corporation Information processing device, control method, program, and storage medium
DE102021117311B4 (de) 2021-07-05 2024-08-22 Spleenlab GmbH Steuer- und Navigationsvorrichtung für ein autonom bewegtes System und autonom bewegtes System
CN113933806A (zh) * 2021-08-31 2022-01-14 西北工业大学 一种天波雷达多路径量测聚类方法
CN114296096B (zh) * 2021-12-23 2024-12-24 优地机器人(无锡)股份有限公司 一种机器人定位方法、系统以及终端
CN115047486B (zh) * 2022-06-14 2025-05-02 中国科学院自动化研究所 基于最大似然估计平滑的激光里程计方法、系统、装置
AU2023200522B1 (en) * 2022-06-24 2023-04-13 Commonwealth Scientific And Industrial Research Organisation Clock synchronisation
GB202210178D0 (en) * 2022-07-11 2022-08-24 Cambridge Entpr Ltd 3d scene capture system
CN115494518B (zh) * 2022-08-15 2025-05-09 杭州羽亿创智科技有限公司 一种基于模式切换的建图方法及激光雷达设备
CN115471570B (zh) * 2022-08-31 2025-06-20 华南理工大学 基于视觉和imu融合的海洋装备水下损伤三维重建方法
TWI866318B (zh) * 2023-07-04 2024-12-11 國立中興大學 雙載具異質協同之定位系統及其定位方法
CN117368888B (zh) * 2023-08-31 2024-11-19 中国矿业大学 一种激光雷达及其精准勘测方法
KR102908165B1 (ko) * 2023-09-13 2026-01-06 삼성이앤에이 주식회사 3d 시뮬레이션 방법 및 장치
WO2025110924A1 (en) * 2023-11-20 2025-05-30 Nanyang Technological University An optimality-aware lidar inertial odometry module with hybrid continuous time optimization
JP7738795B1 (ja) * 2025-05-20 2025-09-12 株式会社 ミックウェア 地理空間情報処理装置および地理空間情報処理方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080033645A1 (en) 2006-08-03 2008-02-07 Jesse Sol Levinson Pobabilistic methods for mapping and localization in arbitrary outdoor environments
WO2010004911A1 (ja) 2008-07-10 2010-01-14 三菱電機株式会社 隊列走行支援装置
JP2013517483A (ja) 2010-01-18 2013-05-16 クアルコム，インコーポレイテッド オブジェクトを使用した慣性航法システムの整合および較正
US20140333741A1 (en) 2013-05-08 2014-11-13 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
WO2017009923A1 (ja) 2015-07-13 2017-01-19 日産自動車株式会社 自己位置推定装置及び自己位置推定方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2787646C (en) * 2010-02-05 2016-12-13 Trimble Navigation Limited Systems and methods for processing mapping and modeling data
WO2015105597A2 (en) 2013-11-27 2015-07-16 The Trustees Of The University Of Pennsylvania Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav)
EP3109589B1 (en) * 2015-06-23 2019-01-30 Volvo Car Corporation A unit and method for improving positioning accuracy
WO2018071416A1 (en) * 2016-10-11 2018-04-19 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
JP6987797B2 (ja) * 2016-03-11 2022-01-05 カールタ インコーポレイテッド リアルタイムオンラインエゴモーション推定を有するレーザスキャナ

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080033645A1 (en) 2006-08-03 2008-02-07 Jesse Sol Levinson Pobabilistic methods for mapping and localization in arbitrary outdoor environments
WO2010004911A1 (ja) 2008-07-10 2010-01-14 三菱電機株式会社 隊列走行支援装置
JP2013517483A (ja) 2010-01-18 2013-05-16 クアルコム，インコーポレイテッド オブジェクトを使用した慣性航法システムの整合および較正
US20140333741A1 (en) 2013-05-08 2014-11-13 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
WO2017009923A1 (ja) 2015-07-13 2017-01-19 日産自動車株式会社 自己位置推定装置及び自己位置推定方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2026064764A1 (en) * 2024-09-23 2026-03-26 Faro Technologies, Inc. Methods of using simultaneous localization for angle measurement and tracking

Also Published As

Publication number Publication date
WO2018140701A1 (en) 2018-08-02
EP3574285A1 (en) 2019-12-04
JP2020507072A (ja) 2020-03-05
EP3574285A4 (en) 2020-12-02

Similar Documents

Publication Publication Date Title
JP7141403B2 (ja) 実時間オンライン自己運動推定を備えたレーザスキャナ
US12493120B2 (en) Laser scanner with real-time, online ego-motion estimation
US11506500B2 (en) Aligning measured signal data with SLAM localization data and uses thereof
US20190346271A1 (en) Laser scanner with real-time, online ego-motion estimation
US11585662B2 (en) Laser scanner with real-time, online ego-motion estimation
WO2018071416A1 (en) Laser scanner with real-time, online ego-motion estimation
EP3656138A1 (en) Aligning measured signal data with slam localization data and uses thereof
US10096129B2 (en) Three-dimensional mapping of an environment
Warren et al. Large scale monocular vision-only mapping from a fixed-wing sUAS
Alliez et al. Indoor localization and mapping: Towards tracking resilience through a multi-slam approach
Ta et al. Monocular parallel tracking and mapping with odometry fusion for mav navigation in feature-lacking environments
Milijas et al. PASTEL: An Aerial Multi-LiDAR Dataset for Research in SLAM Tuning and Robustness
George Analysis of visual-inertial odometry algorithms for outdoor drone applications
Haaparanta ACCURATE POSE ESTIMATION OF MOBILE PLATFORM IN ROUGH TERRAIN
Yi et al. Mastering SLAM: Techniques, Algorithms, and Applications in Artificial Intelligence and Autonomous Systems
Blanchard System of Autonomous Aerial Vehicles for Subterranean Exploration With SLAM Capabilities
Radford Real-time roadway mapping and ground robotic path planning via unmanned aircraft
Li-Chee-Ming Map-Based Localization for Unmanned Aerial Vehicle Navigation
Wendel Scalable visual navigation for micro aerial vehicles using geometric prior knowledge
Tete Robust visual SLAM for autonomous vehicles in challenging environments
Yingcai Multi-Sensor State Estimation for Micro Aerial Vehicles in Complex Environments
Edwards Autonomous 3D Mapping and Surveillance of Mines with MAVs
HK1261850A1 (en) Laser scanner with real-time, online ego-motion estimation
HK1261850B (en) Laser scanner with real-time, online ego-motion estimation

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20210115

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20211027

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20211108

A601 Written request for extension of time

Free format text: JAPANESE INTERMEDIATE CODE: A601

Effective date: 20220201

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20220408

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20220810

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20220909

R150 Certificate of patent or registration of utility model

Ref document number: 7141403

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250