(57)【要約】【課題】周囲から特徴点を検出することができない場合であっても移動体の姿勢変化量を計測できるようにする。【解決手段】領域算出部１１１は、移動体のレーザスキャナによって第１時刻に計測された第１計測領域と、前記レーザスキャナによって第２時刻に計測された第２計測領域と、に共通する共通計測領域を算出する。点群抽出部１１２は、前記第１時刻の計測によって生成された第１レーザ点群から、前記共通計測領域のレーザ点群を第１共通点群として抽出し、前記第２時刻の計測によって生成された第２レーザ点群から、前記共通計測領域のレーザ点群を第２共通点群として抽出する。変化量算出部１１３は、前記第１共通点群と前記第２共通点群とを比較することによって、前記第１時刻における前記移動体の姿勢に対する前記第２時刻における前記移動体の姿勢の変化量を算出する。【選択図】図１
2020143994.tif 000002
【特許請求の範囲】
【請求項１】
移動体の姿勢の変化量を計測する移動体計測装置であって、
前記移動体には、レーザスキャナが搭載され、
前記レーザスキャナは、１回の計測で同心円状に周囲の地面の各地点までの距離と方位とを計測することによってレーザ点群を生成し、
前記移動体計測装置は、
前記レーザスキャナによって第１時刻に計測された第１計測領域と、前記レーザスキャナによって第２時刻に計測された第２計測領域と、に共通する共通計測領域を算出する領域算出部と、
前記第１時刻の計測によって生成された第１レーザ点群から、前記共通計測領域のレーザ点群を第１共通点群として抽出し、前記第２時刻の計測によって生成された第２レーザ点群から、前記共通計測領域のレーザ点群を第２共通点群として抽出する点群抽出部と、
前記第１共通点群と前記第２共通点群とを比較することによって、前記第１時刻における前記移動体の姿勢に対する前記第２時刻における前記移動体の姿勢の変化量を算出する変化量算出部と、
を備える移動体計測装置。
【請求項２】
前記移動体には、前記移動体が移動した距離を計測する距離装置が搭載され、
前記移動体は、前記第１時刻から前記第２時刻まで直進し、
前記距離装置は、前記第１時刻から前記第２時刻までの前記移動体の直進距離を計測し、
前記領域算出部は、前記直進距離に基づいて前記共通計測領域を算出する
請求項１に記載の移動体計測装置。
【請求項３】
前記移動体には、前記移動体を測位する測位装置が搭載され、
前記測位装置は、前記第１時刻における前記移動体の位置である第１位置と、前記第２時刻における前記移動体の位置である第２位置とを測位し、
前記領域算出部は、前記第１位置から前記第２位置への前記移動体の移動ベクトルを算出し、算出された移動ベクトルに基づいて前記共通計測領域を算出する
請求項１に記載の移動体計測装置。
【請求項４】
前記変化量算出部は、前記第１共通点群と前記第２共通点群とが成す角度を前記移動体の姿勢変化量として算出する
請求項１から請求項３のいずれか１項に記載の移動体計測装置。
【請求項５】
前記変化量算出部は、前記第１共通点群と前記第２共通点群とがピッチ方向において成す角度を前記移動体のピッチ変化量として算出する
請求項４に記載の移動体計測装置。
【請求項６】
前記変化量算出部は、前記第１共通点群と前記第２共通点群とがロール方向において成す角度を前記移動体のロール変化量として算出する
請求項４または請求項５に記載の移動体計測装置。
【請求項７】
前記移動体には、前記移動体の姿勢を計測する姿勢計測装置が搭載され、
前記姿勢計測装置は、信頼度が付与された姿勢データを生成し、
前記変化量算出部は、前記第１共通点群と前記第２共通点群とを比較することによって第１姿勢変化量を算出し、前記第１時刻における第１姿勢データと前記第２時刻における第２姿勢データとに基づいて第２姿勢変化量を算出し、
前記移動体計測装置は、さらに、
前記第１姿勢変化量の信頼度と前記第２姿勢変化量の信頼度とを算出する信頼度算出部と、
前記第１姿勢変化量と、前記第１姿勢変化量の信頼度と、前記第２姿勢変化量と、前記第２姿勢変化量とに基づいて、前記移動体の姿勢変化量を決定する変化量決定部と、
を備える請求項１から請求項６のいずれか１項に記載の移動体計測装置。
【請求項８】
移動体の姿勢の変化量を計測するための移動体計測プログラムであって、
前記移動体には、レーザスキャナが搭載され、
前記レーザスキャナは、１回の計測で同心円状に周囲の地面の各地点までの距離と方位とを計測することによってレーザ点群を生成し、
前記移動体計測プログラムは、
前記レーザスキャナによって第１時刻に計測された第１計測領域と、前記レーザスキャナによって第２時刻に計測された第２計測領域と、に共通する共通計測領域を算出する領域算出部と、
前記第１時刻の計測によって生成された第１レーザ点群から、前記共通計測領域のレーザ点群を第１共通点群として抽出し、前記第２時刻の計測によって生成された第２レーザ点群から、前記共通計測領域のレーザ点群を第２共通点群として抽出する点群抽出部と、
前記第１共通点群と前記第２共通点群とを比較することによって、前記第１時刻における前記移動体の姿勢に対する前記第２時刻における前記移動体の姿勢の変化量を算出する変化量算出部として、
コンピュータを機能させるための移動体計測プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の姿勢変化量を計測するための技術に関するものである。
【背景技術】
【０００２】
ＳＬＡＭと呼ばれる測位技術がある。
ＳＬＡＭはＳｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  Ａｎｄ  Ｍａｐｐｉｎｇの略称である。
【０００３】
ＳＬＡＭでは、周囲を一度に計測するレーザを用いて周囲の点群から特徴点が抽出される。そして、移動しながら同じ特徴点を重ね合わせることにより、移動距離およびカーブ量が算出される。
ＳＬＡＭを利用することにより、３次元レーザ点群を生成することが可能である。
【０００４】
ＳＬＡＭは、高精度なＧＰＳおよび高価なＩＭＵを用いずにレーザを用いて実施することができる。そのため、ＳＬＡＭを利用することにより、より安価なシステムを構築することが可能となる。
高精度なＧＰＳおよび高価なＩＭＵを使用したシステムでは、ＧＰＳが利用できない状況であっても、ＩＭＵを利用したデットレコニングによって、精度は低下するものの、自己位置を算出することが可能である。
ＧＰＳはＧｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍの略称である。
ＩＭＵはＩｎｅｒｔｉａｌ  Ｍｅａｓｕｒｅｍｅｎｔ  Ｕｎｉｔの略称である。
【０００５】
ＳＬＡＭを利用したシステムとして、ＳＬＡＭ型ＭＭＳが挙げられる。ＭＭＳはモービルマッピングシステムの略称である。
ＳＬＡＭ型ＭＭＳでは、全周囲を一度に計測するレーザが車両の上に設置される。レーザが車両の上に設置されることにより、より広く周囲を計測することができる。
レーザによって取得された点群は、ＳＬＡＭ技術によって基盤地図とマッチングされる。そして、車両の位置および車両の姿勢が推定され、点群を形成される。この点群マッチングでは、周囲の特徴点を取得するために一定数以上の点群が取得されることが必要となる。
【０００６】
特許文献１には、Ｖｉｓｕａｌ−ＳＬＡＭによって移動体の位置姿勢計測の精度を高めるための技術が開示されている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２０１６−１７７３８８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ＳＬＡＭは、周囲に特徴点が少ない場所では機能しない。例えば、平原およびトンネルでは、特徴点が少ないため、ＳＬＡＭが機能しない。
また、高速での移動時には特徴点の検出が難しくなる。そのため、高速での移動時にもＳＬＡＭが機能しない。
【０００９】
本発明は、周囲から特徴点を検出することができない場合であっても移動体の姿勢変化量を計測できるようにすることを目的とする。
【課題を解決するための手段】
【００１０】
本発明の移動体計測装置は、移動体の姿勢の変化量を計測する。
前記移動体には、レーザスキャナが搭載される。
前記レーザスキャナは、１回の計測で同心円状に周囲の地面の各地点までの距離と方位とを計測することによってレーザ点群を生成する。
前記移動体計測装置は、
前記レーザスキャナによって第１時刻に計測された第１計測領域と、前記レーザスキャナによって第２時刻に計測された第２計測領域と、に共通する共通計測領域を算出する領域算出部と、
前記第１時刻の計測によって生成された第１レーザ点群から、前記共通計測領域のレーザ点群を第１共通点群として抽出し、前記第２時刻の計測によって生成された第２レーザ点群から、前記共通計測領域のレーザ点群を第２共通点群として抽出する点群抽出部と、
前記第１共通点群と前記第２共通点群とを比較することによって、前記第１時刻における前記移動体の姿勢に対する前記第２時刻における前記移動体の姿勢の変化量を算出する変化量算出部と、を備える。
【発明の効果】
【００１１】
本発明によれば、周囲から特徴点を検出することができない場合であっても移動体の姿勢変化量を計測ことができる。
【図面の簡単な説明】
【００１２】
【図１】実施の形態１における移動体計測装置１００の構成図。
【図２】実施の形態１における移動体２００の構成図。
【図３】実施の形態１におけるレーザ点群３００を示す図。
【図４】実施の形態１における移動体計測方法のフローチャート。
【図５】実施の形態１における領域算出処理（Ｓ１１０）のフローチャート。
【図６】実施の形態１における共通計測領域３０３を示す図。
【図７】実施の形態１における点群抽出処理（Ｓ１２０）のフローチャート。
【図８】実施の形態１における第１レーザ点群３１０および第２レーザ点群３２０を示す図。
【図９】実施の形態１における第１共通点群３１１を示す図。
【図１０】実施の形態１における第２共通点群３２１を示す図。
【図１１】実施の形態１における変化量算出処理（Ｓ１３０）のフローチャート。
【図１２】実施の形態１における第１時点の移動体２００および第２時点の移動体２００を示す図。
【図１３】実施の形態１における第１共通点群３１１を示す図。
【図１４】実施の形態１における第２共通点群３２１を示す図。
【図１５】実施の形態１における第１時点の移動体２００および第２時点の移動体２００を示す図。
【図１６】実施の形態１における第１共通点群３１１を示す図。
【図１７】実施の形態１における第２共通点群３２１を示す図。
【図１８】実施の形態２における移動体２００の構成図。
【図１９】実施の形態２における移動体計測方法のフローチャート。
【図２０】実施の形態２における領域算出処理（Ｓ２１０）のフローチャート。
【図２１】実施の形態２における共通計測領域３０３を示す図。
【図２２】実施の形態２における共通計測領域３０３を示す図。
【図２３】実施の形態３における移動体計測装置１００の構成図。
【図２４】実施の形態３における移動体２００の構成図。
【図２５】実施の形態３における移動体計測方法のフローチャート。
【発明を実施するための形態】
【００１３】
実施の形態および図面において、同じ要素または対応する要素には同じ符号を付している。説明した要素と同じ符号が付された要素の説明は適宜に省略または簡略化する。図中の矢印はデータの流れ又は処理の流れを主に示している。
【００１４】
実施の形態１．
移動体の姿勢変化量を計測する形態について、図１から図１７に基づいて説明する。
【００１５】
＊＊＊構成の説明＊＊＊
図１に基づいて、移動体計測装置１００の構成を説明する。
移動体計測装置１００は、プロセッサ１０１とメモリ１０２と補助記憶装置１０３と入出力インタフェース１０４といったハードウェアを備えるコンピュータである。これらのハードウェアは、信号線を介して互いに接続されている。
【００１６】
プロセッサ１０１は、演算処理を行うＩＣ（Ｉｎｔｅｇｒａｔｅｄ  Ｃｉｒｃｕｉｔ）であり、他のハードウェアを制御する。例えば、プロセッサ１０１は、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、ＤＳＰ（Ｄｉｇｉｔａｌ  Ｓｉｇｎａｌ  Ｐｒｏｃｅｓｓｏｒ）、またはＧＰＵ（Ｇｒａｐｈｉｃｓ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）である。
メモリ１０２は揮発性の記憶装置である。メモリ１０２は、主記憶装置またはメインメモリとも呼ばれる。例えば、メモリ１０２はＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）である。メモリ１０２に記憶されたデータは必要に応じて補助記憶装置１０３に保存される。
補助記憶装置１０３は不揮発性の記憶装置である。例えば、補助記憶装置１０３は、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）、またはフラッシュメモリである。補助記憶装置１０３に記憶されたデータは必要に応じてメモリ１０２にロードされる。
入出力インタフェース１０４は入力装置および出力装置が接続されるポートである。例えば、入出力インタフェース１０４はＵＳＢ端子であり、入力装置はキーボードおよびマウスであり、出力装置はディスプレイである。ＵＳＢはＵｎｉｖｅｒｓａｌ  Ｓｅｒｉａｌ  Ｂｕｓの略称である。
【００１７】
移動体計測装置１００は、領域算出部１１１と点群抽出部１１２と変化量算出部１１３といった要素を備える。これらの要素はソフトウェアで実現される。
【００１８】
補助記憶装置１０３には、領域算出部１１１と点群抽出部１１２と変化量算出部１１３としてコンピュータを機能させるための移動体計測プログラムが記憶されている。移動体計測プログラムは、メモリ１０２にロードされて、プロセッサ１０１によって実行される。
さらに、補助記憶装置１０３にはＯＳ（Ｏｐｅｒａｔｉｎｇ  Ｓｙｓｔｅｍ）が記憶されている。ＯＳの少なくとも一部は、メモリ１０２にロードされて、プロセッサ１０１によって実行される。
つまり、プロセッサ１０１は、ＯＳを実行しながら、移動体計測プログラムを実行する。
【００１９】
移動体計測プログラムの入出力データは記憶部１２０に記憶される。
メモリ１０２は記憶部１２０として機能する。但し、補助記憶装置１０３、プロセッサ１０１内のレジスタおよびプロセッサ１０１内のキャッシュメモリなどの記憶装置が、メモリ１０２の代わりに、又は、メモリ１０２と共に、記憶部１２０として機能してもよい。
【００２０】
移動体計測装置１００は、プロセッサ１０１を代替する複数のプロセッサを備えてもよい。複数のプロセッサは、プロセッサ１０１の役割を分担する。
【００２１】
移動体計測プログラムは、光ディスクまたはフラッシュメモリ等の不揮発性の記録媒体にコンピュータ読み取り可能に記録（格納）することができる。
【００２２】
図２および図３に基づいて、移動体２００の構成を説明する。図２は側面図であり、図３は平面図である。
移動体２００には、レーザスキャナ２０１と距離計測装置２０２とが搭載される。
具体的には、移動体２００は車両であり、距離計測装置２０２はオドメータであり、地面２１０は道路である。レーザスキャナ２０１は、車両の上に設置される。
【００２３】
距離計測装置２０２は、移動体２００が移動した距離を計測する。
移動体計測装置１００の記憶部１２０には、各時刻における移動体２００の移動距離がデータとして記憶される。
【００２４】
レーザスキャナ２０１は、ＳＬＡＭで使用されるレーザスキャナである。
レーザスキャナ２０１は、１回の計測で同心円状に周囲の地面２１０の各地点までの距離と方位とを計測する。これにより、レーザ点群３００が得られる。
レーザ点群３００は、レーザスキャナ２０１によって計測された複数の地点を表す複数のレーザ点である。
レーザ点は、レーザスキャナ２０１から計測された地点までの距離と、レーザスキャナ２０１から計測された地点への方位とで表される。
【００２５】
移動体計測装置１００の記憶部１２０には、各時刻のレーザ点群３００がデータとして記憶される。
【００２６】
移動体計測装置１００は、移動体２００に搭載されてもよいし、移動体２００の外部に設けられてもよい。
【００２７】
＊＊＊動作の説明＊＊＊
移動体計測装置１００の動作は移動体計測方法に相当する。また、移動体計測方法の手順は移動体計測プログラムの手順に相当する。
【００２８】
図４に基づいて、移動体計測方法を説明する。
ステップＳ１１０において、領域算出部１１１は、第１時刻における第１計測領域と第２時刻における第２計測領域とに共通する共通計測領域を算出する。
第１計測領域は、地面２１０のうちレーザスキャナ２０１によって第１時刻に計測された領域である。
第２計測領域は、地面２１０のうちレーザスキャナ２０１によって第２時刻に計測された領域である。
第１時刻と第２時刻とのそれぞれは任意である。例えば、第１時刻と第２時刻とのそれぞれは、利用者によって指定される。
【００２９】
領域算出部１１１は、共通計測領域を次のように算出する。
移動体２００が、第１時刻から第２時刻まで直進したと仮定する。
距離計測装置２０２は、第１時刻から第２時刻までの移動体２００の直進距離を計測する。
領域算出部１１１は、第１時刻から第２時刻までの移動体２００の直進距離に基づいて共通計測領域を算出する。
【００３０】
図５に基づいて、領域算出処理（Ｓ１１０）の手順を説明する。
ステップＳ１１１において、領域算出部１１１は、計測範囲情報に基づいて、第１計測領域を算出する。
計測範囲情報は、レーザスキャナ２０１によって１回の計測で計測されるレーザ計測範囲の大きさを示す。例えば、レーザ計測範囲の大きさは半径で表される。計測範囲情報は記憶部１２０に予め記憶される。
レーザ計測範囲の大きさは、レーザ位置姿勢とレーザ照射範囲とに基づいて算出される。レーザ位置姿勢は、移動体２００に対するレーザスキャナ２０１の相対位置および相対姿勢である。相対位置と相対姿勢とのそれぞれは三次元で表される。レーザ照射範囲は、レーザスキャナ２０１がレーザ光を照射する角度の範囲である。
【００３１】
領域算出部１１１は、次のように第１計測領域を算出する。
領域算出部１１１は、第１時刻におけるレーザスキャナ２０１の二次元位置を原点としてレーザ計測範囲の大きさを有する領域を算出する。算出される領域が第１計測領域である。例えば、計測範囲の大きさは半径で表される。
レーザスキャナ２０１の二次元位置とは、水平面におけるレーザスキャナ２０１の位置を意味する。
【００３２】
ステップＳ１１２において、領域算出部１１１は、直進距離と計測範囲情報とに基づいて、第２計測領域を算出する。
【００３３】
領域算出部１１１は、次のように第２計測領域を算出する。
領域算出部１１１は、第１計測領域の原点から前方に直進距離だけ進んだ位置を原点としてレーザ計測範囲の大きさを有する領域を算出する。算出される領域が第２計測領域である。
【００３４】
ステップＳ１１３において、領域算出部１１１は、第１計測領域と第２計測領域とに共通する領域を算出する。算出される領域が共通計測領域である。
【００３５】
図６に基づいて、共通計測領域３０３について説明する。
第１計測領域３０１は、第１時刻におけるレーザスキャナ２０１の二次元位置を原点とする円形領域である。
第２計測領域３０２は、第２時刻におけるレーザスキャナ２０１の二次元位置を原点とする円形領域である。第２時刻におけるレーザスキャナ２０１の位置は、第１時刻における移動体２００の位置から前方に直進距離だけ進んだ位置である。
共通計測領域３０３は、第１計測領域３０１と第２計測領域３０２とに共通する領域である。
【００３６】
図４に戻り、ステップＳ１２０から説明を続ける。
ステップＳ１２０において、点群抽出部１１２は、第１レーザ点群から第１共通点群を抽出する。
第１レーザ点群は、第１時刻の計測によって生成されたレーザ点群である。
第１共通点群は、第１レーザ点群のうちの共通計測領域のレーザ点群である。
共通計測領域のレーザ点群は、共通計測領域内の複数の地点を表す複数のレーザ点である。
【００３７】
さらに、点群抽出部１１２は、第２レーザ点群を記憶部１２０から取得し、取得した第２レーザ点群から第２共通点群を抽出する。
第２レーザ点群は、第２時刻の計測によって生成されたレーザ点群である。
第２共通点群は、第２レーザ点群のうちの共通計測領域のレーザ点群である。
【００３８】
図７に基づいて、点群抽出処理（Ｓ１２０）の手順を説明する。
ステップＳ１２１において、点群抽出部１１２は、第１レーザ点群から共通計測領域のレーザ点群を抽出する。抽出されるレーザ点群が第１共通点群である。
【００３９】
例えば、点群抽出部１１２は、第１共通点群を以下のように抽出する。
まず、点群抽出部１１２は、記憶部１２０から第１レーザ点群を取得する。第１レーザ点群に含まれる各レーザ点を第１レーザ点と称する。
次に、点群抽出部１１２は、第１計測領域における各第１レーザ点の位置を算出する。第１計測領域における第１レーザ点の位置は、第１計測領域におけるレーザスキャナ２０１の相対位置から第１レーザ点の方位へ第１レーザ点の距離だけ離れた位置である。
次に、点群抽出部１１２は、各第１レーザ点の位置が共通計測領域に含まれるか判定する。
そして、点群抽出部１１２は、共通計測領域に含まれる複数の第１レーザ点を第１レーザ点群から抽出する。抽出される複数の第１レーザ点が第１共通点群である。
【００４０】
ステップＳ１２２において、点群抽出部１１２は、第２レーザ点群から共通計測領域のレーザ点群を抽出する。抽出されるレーザ点群が第２共通点群である。
第２共通点群を抽出する方法は、第１共通点群を抽出する方法と同じである。
【００４１】
図８、図９および図１０に基づいて、第１共通点群３１１および第２共通点群３２１を説明する。
共通計測領域３０３において、第１レーザ点群３１０の一部と第２レーザ点群３２０の一部とが重複する（図８参照）。
点群抽出部１１２は、第１レーザ点群３１０から共通計測領域３０３に含まれる複数のレーザ点を抽出する。抽出される複数のレーザ点が第１共通点群３１１である（図９参照）。
点群抽出部１１２は、第２レーザ点群３２０から共通計測領域３０３に含まれる複数のレーザ点を抽出する。抽出される複数のレーザが第２共通点群３２１である（図１０参照）。
【００４２】
図４に戻り、ステップＳ１３０を説明する。
ステップＳ１３０において、変化量算出部１１３は、第１共通点群と第２共通点群とを比較することによって、移動体２００の姿勢変化量を算出する。
算出される姿勢変化量は、第１時刻における移動体２００の姿勢に対する第２時刻における移動体２００の姿勢の変化量である。
【００４３】
変化量算出部１１３は、移動体２００の姿勢変化量を次のように算出する。
変化量算出部１１３は、第１共通点群と第２共通点群とが成す角度を算出する。算出される角度が移動体２００の姿勢変化量である。
具体的には、変化量算出部１１３は、第１共通点群と第２共通点群とがピッチ方向において成す角度を算出する。算出される角度が移動体２００のピッチ変化量である。
ピッチ方向は、移動体２００の左右方向を軸とする回転方向である。
ピッチ変化量は、ピッチ方向における姿勢変化量である。
具体的には、変化量算出部１１３は、第１共通点群と第２共通点群とがロール方向において成す角度を算出する。算出される角度が移動体２００のロール変化量である。
ロール方向は、移動体２００の前後方向を軸とする回転方向である。
ロール変化量は、ロール方向における姿勢変化量である。
【００４４】
図１１に基づいて、変化量算出処理（Ｓ１３０）の手順を説明する。
ステップＳ１３１において、変化量算出部１１３は、第１時刻の移動体２００に対して第１共通点群が成す平面を算出する。算出される平面を第１平面と称する。例えば、第１平面は最小二乗法によって算出される。
【００４５】
ステップＳ１３２において、変化量算出部１１３は、第２時刻の移動体２００に対して第２共通点群が成す平面を算出する。算出される平面を第２平面と称する。例えば、第２平面は最小二乗法によって算出される。
【００４６】
ステップＳ１３３において、変化量算出部１１３は、第１平面と第２平面とがピッチ方向において成す角度を算出する。算出される角度が、移動体２００のピッチ変化量である。
【００４７】
ステップＳ１３４において、変化量算出部１１３は、第１平面と第２平面とがロール方向において成す角度を算出する。算出される角度が、移動体２００のロール変化量である。
【００４８】
変化量算出処理（Ｓ１１３）には、ＳＬＡＭのマッチング技術を利用することができる。
【００４９】
図１２、図１３および図１４に基づいて、移動体２００のピッチ変化量を説明する。
図１２に示すように、第１時点に移動体２００が位置していた地面２１１が水平であると仮定する。また、第２時点に移動体２００が位置していた地面２１２がピッチ方向に傾いていたと仮定する。
移動体２００のピッチ変化量は、地面２１１と地面２１２とがピッチ方向において成す角度に相当する。
図１３に示すように、第１時刻における移動体２００の姿勢を基準にすると、第１共通点群３１１は水平な平面（第１平面）を成す。つまり、第１時刻の移動体２００に対して第１共通点群３１１は、水平な平面（第１平面）を成す。
図１４に示すように、第２時刻における移動体２００の姿勢を基準にすると、第２共通点群３２１はピッチ方向に傾いた平面（第２平面）を成す。つまり、第２時刻の移動体２００に対して第２共通点群３２１は、ピッチ方向に傾いた平面（第２平面）を成す。
第１共通点群３１１が成す平面（図１３参照）と第２共通点群３２１が成す平面（図１４参照）とがピッチ方向において成す角度が、移動体２００のピッチ変化量である。
【００５０】
図１５、図１６および図１７に基づいて、移動体２００のロール変化量を説明する。
図１５に示すように、第１時点に移動体２００が位置していた地面２１１が水平であると仮定する。また、第２時点に移動体２００が位置していた地面２１２がロール方向に傾いていたと仮定する。
移動体２００のロール変化量は、地面２１１と地面２１２とがロール方向において成す角度に相当する。
図１６に示すように、第１時刻における移動体２００の姿勢を基準にすると、第１共通点群３１１は水平な平面（第１平面）を成す。つまり、第１時刻の移動体２００に対して第１共通点群３１１は、水平な平面（第１平面）を成す。
図１７に示すように、第２時刻における移動体２００の姿勢を基準にすると、第２共通点群３２１はロール方向に傾いた平面（第２平面）を成す。つまり、第２時刻の移動体２００に対して第２共通点群３２１は、ロール方向に傾いた平面（第２平面）を成す。
第１共通点群３１１が成す平面（図１６参照）と第２共通点群３２１が成す平面（図１７参照）とがロール方向において成す角度が、移動体２００のロール変化量である。
【００５１】
＊＊＊実施の形態１の原理＊＊＊
ＳＬＡＭのみで３次元点群を生成する場合には、６パラメータの推定が必要である。６パラメータは、３次元位置（Ｘ，Ｙ，Ｚ）および３次元姿勢（Ｙａｗ，Ｒｏｌｌ，Ｐｉｔｃｈ）である。
周囲に特徴点が十分にある場合、ＳＬＡＭによって６つのパラメータを推定することが可能である。しかし、周囲に特徴点がない場合、または、特徴点を見つけにくい場合、ＳＬＡＭでは６つのパラメータを推定することが困難である。高速で移動している状況では特徴点を見つけにくい。６つのパラメータを正しく推定できない場合、ＳＬＡＭによる３次元点群の生成は不可能となる。
ただし、推定するパラメータを限定すれば、パラメータの推定が可能となる。周囲に特徴物がない場合でも路上で計測が行われる場合には地面が必ず存在する。もし、移動体の進行量が分かる状況で地面の連続性をＳＬＡＭによって予想すれば、ロール姿勢とピッチ姿勢とを予測することが可能となる。
【００５２】
＊＊＊実施の形態１の効果＊＊＊
実施の形態１により、周囲に特徴点が無い状況であっても移動体の姿勢変化量を計測することが可能となる。
【００５３】
実施の形態１は、ＳＬＡＭに移動距離を計測する装置を付加することにより、ＳＬＡＭの発散を防ぐ方式を提供する。
【００５４】
安価な距離装置を併用すると、少なくても移動距離の推定はＳＬＡＭによって行わなくてもよくなる。ＳＬＡＭが発散したときにも少なくても地面は存在するので、ＳＬＡＭは、地面のみを用いてロール姿勢とピッチ姿勢とのみを演算するように変更する。移動距離は安価な距離計測装置を用いて演算される。
【００５５】
安価な距離装置を併用し、地面に基づいて姿勢を算出することにより、ＳＬＡＭの発散を低減すること、または、ＳＬＡＭの発散を無くすことが可能となる。
【００５６】
実施の形態２．
移動体が直進せずにカーブした場合であっても移動体の姿勢変化量を計測できるようにするための形態について、主に実施の形態１と異なる点を図１８から図２１に基づいて説明する。
【００５７】
＊＊＊構成の説明＊＊＊
移動体計測装置１００の構成は、実施の形態１における構成と同じである（図１参照）。
【００５８】
図２に基づいて、移動体２００の構成を説明する。
移動体２００には、さらに、各時刻における移動体２００を測位する測位装置２０３が搭載される。
具体的には、測位装置２０３はＧＰＳ受信機であり、ＧＰＳ受信機はレーザスキャナ２０１の上に設置される。
【００５９】
＊＊＊動作の説明＊＊＊
図１９に基づいて、移動体計測方法を説明する。
ステップＳ２１０において、領域算出部１１１は、第１時刻における第１計測領域と第２時刻における第２計測領域とに共通する共通計測領域を算出する。
【００６０】
領域算出部１１１は、共通計測領域を次のように算出する。
測位装置２０３は、第１時刻における移動体２００の位置と第２時刻における移動体２００の位置とを測位する。
第１時刻における移動体２００の位置を「第１位置」と称する。
第２時刻における移動体２００の位置を「第２位置」と称する。
領域算出部１１１は、第１位置から第２位置への移動体２００の移動ベクトルを算出し、算出された移動ベクトルに基づいて共通計測領域を算出する。
【００６１】
図２０に基づいて、領域算出処理（Ｓ２１０）の手順を説明する。
ステップＳ２１１において、領域算出部１１１は、計測範囲情報に基づいて、第１計測領域を算出する。
ステップＳ２１１は、実施の形態１におけるステップＳ１１１と同じである（図５参照）。
【００６２】
ステップＳ２１２において、領域算出部１１１は、第１位置から第２位置への移動体２００の移動ベクトルを算出する。
移動ベクトルは、移動方向と移動距離とを示す。
第１位置は、第１時刻における移動体２００の位置である。
第２位置は、第２時刻における移動体２００の位置である。
第１位置および第２位置は記憶部１２０に予め記憶される。
【００６３】
ステップＳ２１３において、領域算出部１１１は、移動ベクトルと計測範囲情報とに基づいて、第２計測領域を算出する。
ステップＳ２１３は、実施の形態１のステップＳ１１２において直進距離の代わりに移動ベクトルを用いる処理に相当する。
【００６４】
領域算出部１１１は、次のように第２計測領域を算出する。
領域算出部１１１は、第１計測領域の原点から移動ベクトルだけ移動した位置を原点としてレーザ計測範囲の大きさを有する領域を算出する。算出される領域が第２計測領域である。
【００６５】
ステップＳ２１４において、領域算出部１１１は、第１計測領域と第２計測領域とに共通する領域を算出する。算出される領域が共通計測領域である。
ステップＳ２１４は、実施の形態１におけるステップＳ１１３と同じである（図５参照）。
【００６６】
図２１および図２２に基づいて、共通計測領域３０３について説明する。
第１計測領域３０１は、第１時刻におけるレーザスキャナ２０１の二次元位置を原点とする円形領域である（図２１参照）。
第２計測領域３０２は、第２時刻におけるレーザスキャナ２０１の二次元位置を原点とする円形領域である（図２１参照）。第２時刻におけるレーザスキャナ２０１の位置は、第１時刻における移動体２００の位置から移動ベクトルだけ移動した位置である。
共通計測領域３０３は、第１計測領域３０１と第２計測領域３０２とに共通する領域である（図２１参照）。
共通計測領域３０３において、第１レーザ点群３１０の一部と第２レーザ点群３２０の一部とが重複する（図２２参照）。
【００６７】
図１９に戻り、ステップＳ２２０から説明を続ける。
ステップＳ２２０において、点群抽出部１１２は、第１レーザ点群から第１共通点群を抽出する。
ステップＳ２２０は、実施の形態１におけるステップＳ１２０と同じである（図４参照）。
【００６８】
ステップＳ２３０において、変化量算出部１１３は、第１共通点群と第２共通点群とを比較することによって、移動体２００の姿勢変化量を算出する。
ステップＳ２３０は、実施の形態１におけるステップＳ１３０と同じである（図４参照）。
【００６９】
＊＊＊実施の形態２の効果＊＊＊
実施の形態２により、移動体が直進せずにカーブした場合であっても移動体の姿勢変化量を計測することが可能となる。
【００７０】
実施の形態２は、ＳＬＡＭに測位装置を付加することにより、ＳＬＡＭの発散を防ぐ方式を提供する。
安価な測位装置を併用し、地面に基づいて姿勢を算出することにより、ＳＬＡＭの発散を低減すること、または、ＳＬＡＭの発散を無くすことが可能となる。
【００７１】
実施の形態２では、測位装置により、移動距離だけではなく位置がわかる。そのため、ヨー姿勢も推定が可能となる。距離計測装置では移動体が曲がったことは判定できない。デットレコニングが可能な測位装置を用いる場合には、どのような状況でも、ＳＬＡＭの発散を防ぐことができる。
【００７２】
実施の形態３．
姿勢変化量の信頼性を高めるための形態について、主に実施の形態１および実施の形態２と異なる点を図２３から図２５に基づいて説明する。
【００７３】
＊＊＊構成の説明＊＊＊
図２３に基づいて、移動体計測装置１００の構成を説明する。
移動体計測装置１００は、さらに、信頼度算出部１１４と変化量決定部１１５といった要素を備える。これらの要素はソフトウェアによって実現される。
移動体計測プログラムは、さらに、信頼度算出部１１４と変化量決定部１１５としてコンピュータを機能させる。
【００７４】
図２４に基づいて、移動体２００の構成を説明する。
移動体２００には、さらに、各時刻における移動体２００の姿勢を計測する姿勢計測装置２０４が搭載される。
姿勢計測装置２０４は、信頼度が付与された姿勢データを生成する。具体的には、姿勢計測装置２０４はＩＭＵである。
【００７５】
＊＊＊動作の説明＊＊＊
図２５に基づいて、移動体計測方法を説明する。
ステップＳ３１０において、領域算出部１１１は、第１時刻における第１計測領域と第２時刻における第２計測領域とに共通する共通計測領域を算出する。
ステップＳ３１０は、実施の形態１におけるステップＳ１１０（図４参照）または実施の形態２におけるステップＳ２１０（図１９参照）と同じである。
【００７６】
ステップＳ３２０において、点群抽出部１１２は、第１レーザ点群から第１共通点群を抽出する。
ステップＳ３２０は、実施の形態１におけるステップＳ１２０と同じである。
【００７７】
ステップＳ３３０において、変化量算出部１１３は、第１共通点群と第２共通点群とを比較することによって、移動体２００の第１姿勢変化量を算出する。
ステップＳ３３０は、実施の形態１におけるステップＳ１３０と同じである。
【００７８】
ステップＳ３４０において、信頼度算出部１１４は、第１姿勢変化量の信頼度を算出する。
【００７９】
信頼度を算出する方法は任意である。例えば、信頼度算出部１１４は第１姿勢変化量の信頼度を以下のように算出する。
（１）信頼度算出部１１４は、ＳＬＡＭにおける信頼度の算出と同じ方法で第１姿勢変化量の信頼度を算出する。
（２）距離計測装置２０２は、計測した直進距離に信頼度を付加する。信頼度算出部１１４は、直進距離に付加された信頼度を第１姿勢変化量の信頼度として扱う。
（３）測位装置２０３は、測位した第１位置と測位した第２位置とのそれぞれに信頼度を付加する。信頼度算出部１１４は、第１位置の信頼度と第２位置の信頼度とに基づいて、第１姿勢変化量の信頼度を算出する。例えば、信頼度算出部１１４は、第１位置の信頼度と第２位置の信頼度との平均を算出する。算出された平均が第１姿勢変化量となる。
【００８０】
ステップＳ３５０において、変化量算出部１１３は、第１時刻における第１姿勢データと第２時刻における第２姿勢データとに基づいて、移動体計測装置１００の第２姿勢変化量を算出する。
【００８１】
変化量算出部１１３は、第２姿勢変化量を次のように算出する。
変化量算出部１１３は、第１姿勢データが示す姿勢と第２姿勢データが示す姿勢との差を算出する。算出される差が第２姿勢変化量である。
【００８２】
ステップＳ３６０において、信頼度算出部１１４は、第２姿勢変化量の信頼度を算出する。
【００８３】
信頼度算出部１１４は、第２姿勢変化量の信頼度を次のように算出する。
信頼度算出部１１４は、第１姿勢データに付加された信頼度と第２姿勢データに付加された信頼度とに基づいて、第２姿勢変化量の信頼度を算出する。例えば、信頼度算出部１１４は、第１姿勢データの信頼度と第２姿勢データの信頼度との平均を算出する。算出される平均が第２姿勢変化量である。
【００８４】
ステップＳ３７０において、変化量決定部１１５は、第１姿勢変化量と、第１姿勢変化量の信頼度と、第２姿勢変化量と、第２姿勢変化量とに基づいて、移動体２００の姿勢変化量を決定する。
【００８５】
例えば、変化量決定部１１５は、移動体２００の姿勢変化量を以下のように決定する。
（１）変化量決定部１１５は、第１姿勢変化量と第２姿勢変化量とのうち、信頼度が高い方の姿勢変化量を選択する。選択される姿勢変化量が移動体２００の姿勢変化量となる。
（２）変化量決定部１１５は、第１姿勢変化量の信頼度に基づいて第１姿勢変化量に重み付けし、第２姿勢変化量の信頼度に基づいて第２姿勢変化量に重み付けする。そして、重み付け後の第１姿勢変化量と重み付け後の第２姿勢変化量との平均を算出する。算出される平均が、移動体２００の姿勢変化量となる。
【００８６】
＊＊＊実施の形態３の効果＊＊＊
実施の形態３により、姿勢変化量の信頼性を高めることが可能となる。
【００８７】
各機器の信頼性情報に基づいて各姿勢変化量に重みづけを行うことにより、複合結果として得られる姿勢変化量の信頼性が上がる。
それぞれの機器の計測状況が悪くなった場合でも、各機器の計測結果に基づく姿勢変化量を複合させることより、信頼性が高い姿勢変化量を得ることができる。
【００８８】
安価なＧＰＳと安価なＩＭＵとが搭載され、位置と姿勢とが算出される移動体も多数存在する。ただし、算出精度はそれほど高いものではない。なお、位置と姿勢とが正確であれば、ＳＬＡＭを利用する必要がない。
これらの装置が移動体に搭載される場合には、ＳＬＡＭによって予測される姿勢（第１姿勢変化量）と、ＧＰＳとＩＭＵとによって予測される姿勢（第２姿勢変化量）とのそれぞれに、性能（信頼度）に基づいて重みづけを行う。そして、両者の姿勢に基づいて姿勢を決定することがより有効である、と考えられる。
ＳＬＡＭの場合もＧＰＳ−ＩＭＵの場合も信頼度が付与されるので、本々の機器の信頼度とその状態での信頼度とを加味し、どちらかのデータを使う手法が考えられる。また、重みづけを行い平均化するなどの手法も有効であると考えられる。
これはＳＬＡＭが発散した場合だけではなく、発散していない状態での姿勢の高度化にも利用可能である。
ＳＬＡＭで算出されるロール姿勢およびピッチ姿勢はあくまで変化分であって、重力などに基づく絶対値ではない。位置も姿勢も特定できる装置が移動体に搭載されていて、装置によって計測される姿勢の信頼度が高い場合、その姿勢を用いることにより、ＳＬＡＭによって算出される絶対姿勢を補正することができる。
【００８９】
実施の形態３は、以下のように利用することができる。
（１）衛星は捉えられないが周囲に特徴点が多い場合（例えば高層ビル街など）、ＳＬＡＭによる位置姿勢推定（第１姿勢変化量を含む）を優位にする。
（２）衛星が捉えられるが周囲に特徴点がない場合（高速道路や田舎道など）、測位システム（衛星測位およびＩＭＵ）による位置姿勢（第２姿勢変化量を含む）を優位にする。
（３）衛星が捉えられ且つ周囲に特徴点が多い場合、より優位とすると決められた方のデータ（第１姿勢変化量または第２姿勢変化量）を利用する。この場合、最適な状況において測位システムとＳＬＡＭとのどちらがより正確か事前に計測しておけばよい。
（４）衛星が捉えられない且つ周囲に特徴点がない場合、デットレコニングによる位置推定と地面部分のＳＬＡＭ（第１姿勢変化量）を利用する。（１）から（４）の中で（４）の信頼性が最も低い。ただし、周囲に特徴点がない場合に点群が生成されない、というＳＬＡＭの問題点を解決することができる。
【００９０】
＊＊＊実施の形態の補足＊＊＊
実施の形態は、好ましい形態の例示であり、本発明の技術的範囲を制限することを意図するものではない。実施の形態は、部分的に実施してもよいし、他の形態と組み合わせて実施してもよい。フローチャート等を用いて説明した手順は、適宜に変更してもよい。
【００９１】
移動体計測装置１００は、複数の装置で実現されてもよい。つまり、移動体計測装置１００は、システムとして実現されてもよい。
移動体計測装置１００の各要素は、ソフトウェア、ハードウェア、ファームウェアまたはこれらの組み合わせのいずれで実現されてもよい。
移動体計測装置１００の各要素について、「部」は「処理」または「工程」と読み替えてもよい。
【符号の説明】
【００９２】
１００  移動体計測装置、１０１  プロセッサ、１０２  メモリ、１０３  補助記憶装置、１０４  入出力インタフェース、１１１  領域算出部、１１２  点群抽出部、１１３  変化量算出部、１１４  信頼度算出部、１１５  変化量決定部、１２０  記憶部、２００  移動体、２０１  レーザスキャナ、２０２  距離計測装置、２０３  測位装置、２０４  姿勢計測装置、２１０  地面、２１１  地面、２１２  地面、３００  レーザ点群、３０１  第１計測領域、３０２  第２計測領域、３０３  共通計測領域、３１０  第１レーザ点群、３１１  第１共通点群、３２０  第２レーザ点群、３２１  第２共通点群。
【図１】
2020143994.tif 000003
【図２】
2020143994.tif 000004
【図３】
2020143994.tif 000005
【図４】
2020143994.tif 000006
【図５】
2020143994.tif 000007
【図６】
2020143994.tif 000008
【図７】
2020143994.tif 000009
【図８】
2020143994.tif 000010
【図９】
2020143994.tif 000011
【図１０】
2020143994.tif 000012
【図１１】
2020143994.tif 000013
【図１２】
2020143994.tif 000014
【図１３】
2020143994.tif 000015
【図１４】
2020143994.tif 000016
【図１５】
2020143994.tif 000017
【図１６】
2020143994.tif 000018
【図１７】
2020143994.tif 000019
【図１８】
2020143994.tif 000020
【図１９】
2020143994.tif 000021
【図２０】
2020143994.tif 000022
【図２１】
2020143994.tif 000023
【図２２】
2020143994.tif 000024
【図２３】
2020143994.tif 000025
【図２４】
2020143994.tif 000026
【図２５】
2020143994.tif 000027