JP6525148B2 - 軌跡推定方法、軌跡推定装置及び軌跡推定プログラム - Google Patents
軌跡推定方法、軌跡推定装置及び軌跡推定プログラム Download PDFInfo
- Publication number
- JP6525148B2 JP6525148B2 JP2015116954A JP2015116954A JP6525148B2 JP 6525148 B2 JP6525148 B2 JP 6525148B2 JP 2015116954 A JP2015116954 A JP 2015116954A JP 2015116954 A JP2015116954 A JP 2015116954A JP 6525148 B2 JP6525148 B2 JP 6525148B2
- Authority
- JP
- Japan
- Prior art keywords
- frame
- round
- vector
- translation vector
- rotation matrix
- 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
Links
- 238000000034 method Methods 0.000 title claims description 64
- 239000013598 vector Substances 0.000 claims description 353
- 238000013519 translation Methods 0.000 claims description 229
- 239000011159 matrix material Substances 0.000 claims description 180
- 238000000605 extraction Methods 0.000 claims description 48
- 238000012937 correction Methods 0.000 claims description 40
- 238000004364 calculation method Methods 0.000 claims description 36
- 238000005259 measurement Methods 0.000 claims description 19
- 239000000284 extract Substances 0.000 claims description 9
- 230000000295 complement effect Effects 0.000 claims 1
- 238000012545 processing Methods 0.000 description 31
- 238000010586 diagram Methods 0.000 description 29
- 238000005457 optimization Methods 0.000 description 26
- 238000009825 accumulation Methods 0.000 description 16
- 238000000513 principal component analysis Methods 0.000 description 6
- 230000005484 gravity Effects 0.000 description 5
- 238000003384 imaging method Methods 0.000 description 4
- 230000006835 compression Effects 0.000 description 2
- 238000007906 compression Methods 0.000 description 2
- 230000001186 cumulative effect Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000005401 electroluminescence Methods 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000001678 irradiating effect Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Landscapes
- Length Measuring Devices By Optical Means (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
センサから出力された点群から特徴部をフレーム毎に抽出し、所定のフレームに含まれる特徴部の位置と、次のフレームに含まれる特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、1周目の所定のフレームから当該フレームに対応する2周目のフレームまでの並進ベクトル及び回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を2周目の全てのフレームについてそれぞれ算出し、1周目の所定のフレームに含まれる特徴部の位置と当該フレームに対応する2周目のフレームに含まれる特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を2周目の全てのフレームについてそれぞれ算出し、2周目の所定のフレームに関する蓄積並進ベクトル及び蓄積回転行列と直接並進ベクトル及び直接回転行列とから、蓄積並進ベクトルと直接並進ベクトルとの差を示す推定誤差ベクトル及び蓄積回転行例と直接回転行列の差を示す推定誤差行列を2周目の全てのフレームについてそれぞれ算出し、所定のフレームにおける推定誤差ベクトルと次のフレームにおける推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を2周目の全てのフレームについてそれぞれ算出し、2周目の最後のフレームの推定誤差ベクトルに、2周目の全てのフレームについての推定誤差変化量の総和に対する2周目の所定のフレームの推定誤差変化量の比率を乗じて算出された補正ベクトルを、2周目の全てのフレームについてそれぞれ求め、2周目の所定のフレーム間についての並進ベクトルを対応する補正ベクトルに基づいて補正した補正並進ベクトルを2周目の全てのフレーム間についてそれぞれ算出し、補正並進ベクトル及び回転行列を使用して2周目の軌跡を推定する
ことをコンピュータに実行させる。
図1は、本実施形態の位置推定システムの概要を説明するための模式図である。本実施形態の位置推定システムでは、複数の地物の位置を所定の測定時間毎に測定し、測定した地物の複数のセンサ座標系の位置を示す点群PTを出力するセンサを搭載した移動体の軌跡が推定される。位置推定装置は、コンピュータであるが、本発明が適用可能であればどのような形態でもよい。なお、位置推定装置は、軌跡推定装置の一例である。例えば、位置推定装置は、パーソナルコンピュータ、サーバ、ノートPC、多機能携帯電話機(所謂「スマートフォン」)、携帯情報端末(Personal Digital Assistant, PDA)、タブレット端末、タブレットPC、携帯型ゲーム機等でもよい。移動体は、自動車、原動機付自転車、又は軽車両(自転車等)等である。
図2は、位置推定システム1の概略構成の一例を示す図である。
図3は、位置推定装置2の概略構成の一例を示す図である。また、図4は、記憶部21が記憶する点群テーブルのデータ構造の一例を示す図である。
処理部24は、取得部241、抽出部242、平面特定部243、算出部244、最適化部245、及び推定部246を備える。これらの各部は、処理部24が備えるプロセッサで実行されるプログラムにより実現される機能モジュールである。あるいは、これらの各部は、ファームウェアとして位置推定装置2に実装されてもよい。
2 位置推定装置
21 記憶部
22 入力部
23 出力部
24 処理部
241 取得部
242 抽出部
243 平面特定部
244 算出部
245 最適化部
246 推定部
3 センサ
4 移動体
Claims (6)
- ループ状の経路を2周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する方法であって、
前記センサから出力された点群から特徴部をフレーム毎に抽出し、
所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、
1周目の所定のフレームから当該フレームに対応する2周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記2周目の全てのフレームについてそれぞれ算出し、
前記1周目の所定のフレームに含まれる前記特徴部の位置と当該フレームに対応する前記2周目のフレームに含まれる前記特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を前記2周目の全てのフレームについてそれぞれ算出し、
前記2周目の所定のフレームに関する前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び前記蓄積回転行例と前記直接回転行列の差を示す推定誤差行列を前記2周目の全てのフレームについてそれぞれ算出し、
所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を前記2周目の全てのフレームについてそれぞれ算出し、
前記2周目の最後のフレームの推定誤差ベクトルに、前記2周目の全てのフレームについての前記推定誤差変化量の総和に対する前記2周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記2周目の全てのフレームについてそれぞれ求め、
前記2周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した第1の補正並進ベクトルを前記2周目の全てのフレーム間についてそれぞれ算出し、
前記第1の補正並進ベクトル及び前記回転行列を使用して前記2周目の軌跡を推定する
ことを含むことを特徴とする軌跡推定方法。 - 前記特徴部は、平面であり、
前記特徴部を抽出することは、
前記センサから出力された点群をフレーム毎に取得し、
前記取得された点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、
前記平面点群により形成される前記平面を抽出することを含む、請求項1に記載の軌跡推定方法。 - 前記2周目の各フレームにおける推定誤差変化量を、当該フレームに対応付けられた前記1周目の各フレームに対応付け、
前記1周目の最後のフレームの推定誤差ベクトルに、前記1周目の全てのフレームに対応付けられた前記推定誤差変化量の総和に対する前記1周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記1周目の全てのフレームについてそれぞれ求め、
前記1周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した第2の補正並進ベクトルを前記1周目の全てのフレーム間についてそれぞれ算出し、
前記第2の補正並進ベクトル及び前記回転行列を使用して前記1周目の軌跡を推定することを更に含む、請求項1又は2に記載の軌跡推定方法。 - ループ状の経路を2周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する方法であって、
前記センサから出力された点群から特徴部をフレーム毎に抽出し、
所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、
1周目の所定のフレームから当該フレームに対応する2周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記2周目の全てのフレームについてそれぞれ算出し、
前記1周目の所定のフレームに含まれる前記特徴部の位置と当該フレームに対応する前記2周目のフレームに含まれる前記特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を前記2周目の全てのフレームについてそれぞれ算出し、
前記2周目の所定のフレームに関する前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び前記蓄積回転行例と前記直接回転行列の差を示す推定誤差行列を前記2周目の全てのフレームについてそれぞれ算出し、
所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を前記2周目の全てのフレームについてそれぞれ算出し、
前記2周目の各フレームにおける推定誤差変化量を、当該フレームに対応付けられた前記1周目の各フレームに対応付け、
前記1周目の最後のフレームの推定誤差ベクトルに、前記1周目の全てのフレームに対応付けられた前記推定誤差変化量の総和に対する前記1周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記1周目の全てのフレームについてそれぞれ求め、
前記1周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した補正並進ベクトルを前記1周目の全てのフレーム間についてそれぞれ算出し、
前記補正並進ベクトル及び前記回転行列を使用して前記1周目の軌跡を推定する
ことを含むことを特徴とする軌跡推定方法。 - ループ状の経路を2周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する軌跡推定装置であって、
前記センサから出力された点群から特徴部をフレーム毎に抽出する抽出部と、
所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出する算出部と、
1周目の各フレームから当該フレームに対応する2周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記2周目のフレーム毎に算出し、
前記1周目の各フレームに含まれる前記特徴部の位置と当該フレームに対応する前記2周目のフレームに含まれる前記特徴部の位置とから、前記1周目の各フレームに含まれる前記特徴部と前記2周目の対応するフレームに含まれる前記特徴部との間の位置関係を示す直接並進ベクトル及び直接回転行列を前記2周目のフレーム毎に算出し、
前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び推定誤差行列を前記2周目のフレーム毎に算出し、
所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとの間の距離を示す推定誤差変化量を前記2周目のフレーム毎に算出し、
前記2周目の最後のフレームの推定誤差ベクトルに、各フレームの推定誤差変化量に応じた比率を乗じて算出された補正ベクトルに基づいて、前記2周目の前記並進ベクトルを補正した補正並進ベクトルを算出し、
前記補正並進ベクトル及び前記回転行列を使用して前記2周目の軌跡を推定する推定部と
を備えることを特徴とする軌跡推定装置。 - ループ状の経路を2周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する軌跡推定プログラムであって、
前記センサから出力された点群から特徴部をフレーム毎に抽出し、
所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、
1周目の所定のフレームから当該フレームに対応する2周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記2周目の全てのフレームについてそれぞれ算出し、
前記1周目の所定のフレームに含まれる前記特徴部の位置と当該フレームに対応する前記2周目のフレームに含まれる前記特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を前記2周目の全てのフレームについてそれぞれ算出し、
前記2周目の所定のフレームに関する前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び前記蓄積回転行例と前記直接回転行列の差を示す推定誤差行列を前記2周目の全てのフレームについてそれぞれ算出し、
所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を前記2周目の全てのフレームについてそれぞれ算出し、
前記2周目の最後のフレームの推定誤差ベクトルに、前記2周目の全てのフレームについての前記推定誤差変化量の総和に対する前記2周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記2周目の全てのフレームについてそれぞれ求め、
前記2周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した補正並進ベクトルを前記2周目の全てのフレーム間についてそれぞれ算出し、
前記補正並進ベクトル及び前記回転行列を使用して前記2周目の軌跡を推定する
ことをコンピュータに実行させることを特徴とする軌跡推定プログラム。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2015116954A JP6525148B2 (ja) | 2015-06-09 | 2015-06-09 | 軌跡推定方法、軌跡推定装置及び軌跡推定プログラム |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2015116954A JP6525148B2 (ja) | 2015-06-09 | 2015-06-09 | 軌跡推定方法、軌跡推定装置及び軌跡推定プログラム |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2017004228A JP2017004228A (ja) | 2017-01-05 |
JP6525148B2 true JP6525148B2 (ja) | 2019-06-05 |
Family
ID=57752107
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2015116954A Active JP6525148B2 (ja) | 2015-06-09 | 2015-06-09 | 軌跡推定方法、軌跡推定装置及び軌跡推定プログラム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6525148B2 (ja) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6660228B2 (ja) * | 2016-03-31 | 2020-03-11 | セコム株式会社 | 物体検出センサ |
CN108228798B (zh) | 2017-12-29 | 2021-09-17 | 百度在线网络技术(北京)有限公司 | 确定点云数据之间的匹配关系的方法和装置 |
CN108279670B (zh) * | 2017-12-29 | 2021-12-10 | 百度在线网络技术(北京)有限公司 | 用于调整点云数据采集轨迹的方法、设备以及计算机可读介质 |
JP2019152924A (ja) * | 2018-02-28 | 2019-09-12 | 学校法人立命館 | 自己位置同定システム、車両、及び処理装置 |
CN112861308B (zh) * | 2020-12-31 | 2023-03-24 | 中国人民解放军63791部队 | 一种基于时序的弹道曲线平移判断方法 |
WO2023182502A1 (ja) * | 2022-03-24 | 2023-09-28 | 株式会社 東芝 | データ処理装置、移動体システム、移動体、データ処理方法、プログラム、及び記憶媒体 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2010131371A1 (en) * | 2009-05-12 | 2010-11-18 | Toyota Jidosha Kabushiki Kaisha | Object recognition method, object recognition apparatus, and autonomous mobile robot |
JP5434608B2 (ja) * | 2010-01-08 | 2014-03-05 | トヨタ自動車株式会社 | 測位装置及び測位方法 |
JP5652239B2 (ja) * | 2011-02-15 | 2015-01-14 | 株式会社豊田中央研究所 | 運動推定装置及びプログラム |
-
2015
- 2015-06-09 JP JP2015116954A patent/JP6525148B2/ja active Active
Also Published As
Publication number | Publication date |
---|---|
JP2017004228A (ja) | 2017-01-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6525148B2 (ja) | 軌跡推定方法、軌跡推定装置及び軌跡推定プログラム | |
JP6601613B2 (ja) | 位置推定方法、位置推定装置及び位置推定プログラム | |
US9242171B2 (en) | Real-time camera tracking using depth maps | |
JP6434513B2 (ja) | 視覚に基づく慣性ナビゲーション | |
US9953461B2 (en) | Navigation system applying augmented reality | |
TWI467494B (zh) | 使用深度圖進行移動式攝影機定位 | |
US9524436B2 (en) | Augmented reality camera registration | |
US8976172B2 (en) | Three-dimensional scanning using existing sensors on portable electronic devices | |
JP5950122B2 (ja) | キャリブレーション装置、キャリブレーション方法およびキャリブレーションプログラム | |
JP6658001B2 (ja) | 位置推定装置、プログラム、位置推定方法 | |
JP5803367B2 (ja) | 自己位置推定装置、自己位置推定方法およびプログラム | |
JP6782903B2 (ja) | 自己運動推定システム、自己運動推定システムの制御方法及びプログラム | |
RU2017141158A (ru) | Способ моделирования трехмерного пространства | |
KR20220025028A (ko) | 시각적 비콘 기반의 비콘 맵 구축 방법, 장치 | |
JP7420135B2 (ja) | 情報処理装置、情報処理方法、及びプログラム | |
JP2018112506A5 (ja) | ||
US20170108338A1 (en) | Method for geolocating a carrier based on its environment | |
JP2017096813A (ja) | キャリブレーション装置、キャリブレーション方法およびキャリブレーションプログラム | |
JP2015146091A (ja) | 移動ロボットの自己位置推定方法 | |
WO2020054408A1 (ja) | 制御装置、情報処理方法、およびプログラム | |
WO2018148219A1 (en) | Systems and methods for user input device tracking in a spatial operating environment | |
JP2018185239A (ja) | 位置姿勢推定装置及びプログラム | |
US20210402616A1 (en) | Information processing apparatus, information processing method, mobile robot, and non-transitory computer-readable storage medium | |
CN113504385B (zh) | 复数相机测速方法及测速装置 | |
CN115222815A (zh) | 障碍物距离检测方法、装置、计算机设备和存储介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20180215 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A821 Effective date: 20180215 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20181220 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20190108 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20190222 |
|
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: 20190326 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20190423 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6525148 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
S531 | Written request for registration of change of domicile |
Free format text: JAPANESE INTERMEDIATE CODE: R313531 |
|
R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |