(57)【要約】【課題】アームを備える移動体の正確な位置推定が可能な位置推定装置を提供する。【解決手段】位置推定装置は、姿勢変化が可能なアームに設けられ、対象物との距離を計測する測域センサがスキャンして得た距離情報が示す対象物の位置に対応する点群情報に基づいてスキャンマッチングを行い、測域センサの移動量を推定するスキャンマッチング処理部と、アームを有する移動体に備えられた当該移動体の移動に関する情報を計測するセンサの計測値に基づいて移動体の移動量を推定するオドメトリ処理部と、アームの姿勢変化の有無に基づく指示情報に基づいて、オドメトリ処理部が推定した移動量とスキャンマッチング処理部が推定した移動量とのうち一方を選択し、選択した移動量に基づいて移動体の位置情報を推定する位置情報推定部と、を備える。【選択図】図２
2018084544.tif 000002
【特許請求の範囲】
【請求項１】
姿勢変化が可能なアームに設けられ、対象物との距離を計測する測域センサがスキャンして得た距離情報が示す対象物の位置に対応する点群情報に基づいてスキャンマッチングを行い、前記測域センサの移動量を推定するスキャンマッチング処理部と、
前記アームを有する移動体に備えられた当該移動体の移動に関する情報を計測するセンサの計測値に基づいて前記移動体の移動量を推定するオドメトリ処理部と、
前記アームの姿勢変化の有無に基づく指示情報を取得し、当該指示情報に基づいて、前記オドメトリ処理部が推定した移動量と前記スキャンマッチング処理部が推定した移動量とのうち一方を選択し、選択した移動量に基づいて前記移動体の位置情報を推定する位置情報推定部と、
を備える位置推定装置。
【請求項２】
前記位置情報推定部は、オペレータが入力した前記指示情報に基づいて、前記オドメトリ処理部が推定した移動量と前記スキャンマッチング処理部が推定した移動量とのうち一方を選択する、
請求項１に記載の位置推定装置。
【請求項３】
前記アームの姿勢変化を計測するセンサの計測した情報に基づいて、前記アームの姿勢変化の有無を判定し、当該判定に基づいて、前記オドメトリ処理部が推定した移動量と前記スキャンマッチング処理部が推定した移動量とのうち何れを選択するかを指示する指示情報を出力する切り替え指示部、
をさらに備える請求項１に記載の位置推定装置。
【請求項４】
前記切り替え指示部は、前記アームの姿勢変化が生じたと判定した場合に前記オドメトリ処理部が推定した移動量の選択を指示する、
請求項３に記載の位置推定装置。
【請求項５】
前記オドメトリ処理部が推定した移動量と前記スキャンマッチング処理部が推定した移動量とのうち何れを選択するかの指示情報の入力を受け付ける入力受付部と、
前記切り替え指示部が出力した指示情報と前記入力受付部が受け付けた指示情報とのうち、前記入力受付部が受け付けた指示情報に基づいて、前記移動体の移動量の推定を指示するオペレータ指示優先処理部と、
をさらに備える請求項３または請求項４に記載の位置推定装置。
【請求項６】
前記スキャンマッチング処理部は、スキャンマッチングの計算において、前記オドメトリ処理部が推定した前記移動体の微小時間における移動量を初期値に用いる、
請求項１から請求項５の何れか１項に記載の位置推定装置。
【請求項７】
請求項１から請求項６の何れか１項に記載の位置推定装置と、
前記アームの姿勢変化に応じて前記測域センサの前記移動体を基準とする位置情報を計算するセンサ位置・姿勢計算部と、
前記測域センサがスキャンして得た点群情報を、前記センサ位置・姿勢計算部が計算した位置情報と前記位置推定装置が推定した前記移動体の位置情報とを用いて補正し、地図情報を作成する地図情報作成部と、
を備える地図情報作成装置。
【請求項８】
請求項１から請求項６の何れか１項に記載の位置推定装置と、
姿勢変化が可能で前記位置推定装置が備える測域センサが設けられたアームと、
を備える移動体。
【請求項９】
位置推定装置が、
姿勢変化が可能なアームに設けられ、対象物との距離を計測する測域センサがスキャンして得た距離情報が示す対象物の位置に対応する点群情報に基づいてスキャンマッチングを行い、前記測域センサの移動量を推定し、
前記アームを有する移動体に備えられた当該移動体の移動に関する情報を計測するセンサの計測値に基づいて前記移動体の移動量を推定し、
前記アームの姿勢変化の有無に基づく指示情報を取得し、当該指示情報に基づいて、前記移動体の移動量と前記測域センサの移動量とのうち一方を選択し、選択した移動量に基づいて前記移動体の位置情報を推定する、
位置推定方法。
【請求項１０】
位置推定装置のコンピュータを、
姿勢変化が可能なアームに設けられ、対象物との距離を計測する測域センサがスキャンして得た距離情報が示す対象物の位置に対応する点群情報に基づいてスキャンマッチングを行い、前記測域センサの移動量を推定する手段、
前記アームを有する移動体に備えられた当該移動体の移動に関する情報を計測するセンサの計測値に基づいて前記移動体の移動量を推定する手段、
前記アームの姿勢変化の有無に基づく指示情報を取得し、当該指示情報に基づいて、前記移動体の移動量と前記測域センサの移動量とのうち一方を選択し、選択した移動量に基づいて前記移動体の位置情報を推定する手段、
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置推定装置、地図情報作製装置、移動体、位置推定方法及びプログラムに関する。
【背景技術】
【０００２】
走行台車やロボット等の移動体に旋回や直動が可能なアームを設け、そのアームの先端にレーザ測域センサを搭載し、周辺の環境に存在する障害物との距離を計測し、移動体と障害物との位置関係から障害物の地図を構築する技術が知られている。レーザ測域センサは、センサ原点を基準として障害物までの距離情報を出力するため、移動しながら障害物の地図を作成するためには、レーザ測域センサが出力する距離情報を絶えず座標変換して絶対座標（地図座標）での値に更新する必要がある。このため、移動体による地図作成では、レーザ測域センサの正確な位置・姿勢を常に計測する手段が必要である。レーザ測域センサの位置情報を計測するために、たとえばＧＰＳ等の位置センサを使う構成が考えられる。
【０００３】
例えば、特許文献１や特許文献２には、走行台車に伸縮機構を有するアームを設け、アームの先端にＧＰＳとレーザ測域センサを搭載して、アーム先端の位置情報を計測できるようにした測量装置が開示されている。この技術を用いると、土木工事の現場、災害現場など、瓦礫や地形の問題で走行台車を測量地点に移動させることが困難な場合や、測量対象がレーザ測距センサの視野角に入らないほど高所の場合でも、アームを伸ばすことで測量が可能になる。また、アーム端のＧＰＳによる測位情報によって、レーザ測域センサの位置を確認することができるので、ＧＰＳによる測位情報を用いてレーザ測距センサの計測値を座標変換することで測量結果の精度を保つことができる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平７−１２８０５７号公報
【特許文献２】特開平９−１９６６７２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかし、特許文献１、２の方法では、センサ位置がＧＰＳによって観測されることが前提となっており、屋内などのＧＰＳ信号が受信できない環境では使用できない。また、走行台車の位置自体は、タイヤの回転速度、操舵角から幾何学的にある程度類推することが可能だが、初期位置からの相対変位を求める計算手法（オドメトリ）となるため走行距離増加に伴い誤差が累積するという課題がある。
【０００６】
一方、オドメトリの代替技術として，スキャンマッチングという技術が知られている。スキャンマッチングは、レーザ測域センサから取得された点群情報を、時系列で位置合わせすることによりセンサの移動量を逆推定する技術である。このスキャンマッチングを用いるとオドメトリのような誤差の累積を生じることなく移動体の位置を推定することができる。ところで、移動体の走行中にアームが固定された姿勢を保つならば、レーザ測域センサの移動量は移動体の移動量と等しく、スキャンマッチングによって移動体の移動量を推定できる。そして、推定した移動量を積算すれば移動体の位置を推定することができる。ところが、移動体の走行中にアームの姿勢が変化すると、センサの移動量と移動体の移動量が食い違うという問題が発生する。
【０００７】
そこでこの発明は、上述の課題を解決することのできる位置推定装置、地図情報作製装置、移動体、位置推定方法及びプログラムを提供することを目的としている。
【課題を解決するための手段】
【０００８】
本発明の第１の態様は、姿勢変化が可能なアームに設けられ、対象物との距離を計測する測域センサと、前記測域センサがスキャンして得た距離情報が示す対象物の位置に対応する点群情報に基づいてスキャンマッチングを行い、前記測域センサの移動量を推定するスキャンマッチング処理部と、前記アームを有する移動体に備えられた当該移動体の移動に関する情報を計測するセンサの計測値に基づいて前記移動体の移動量を推定するオドメトリ処理部と、前記アームの姿勢変化の有無に基づく指示情報を取得し、当該指示情報に基づいて、前記オドメトリ処理部が推定した移動量と前記スキャンマッチング処理部が推定した移動量とのうち一方を選択し、選択した移動量に基づいて前記移動体の位置情報を推定する位置情報推定部と、を備える位置推定装置である。
【０００９】
本発明の第２の態様における前記位置情報推定部は、オペレータが入力した前記指示情報に基づいて、前記オドメトリ処理部が推定した移動量と前記スキャンマッチング処理部が推定した移動量とのうち一方を選択する。
【００１０】
本発明の第３の態様における前記位置推定装置は、前記アームの姿勢変化を計測するセンサの計測した情報に基づいて、前記アームの姿勢変化の有無を判定し、当該判定に基づいて、前記オドメトリ処理部が推定した移動量と前記スキャンマッチング処理部が推定した移動量とのうち何れを選択するかを指示する指示情報を出力する切り替え指示部、をさらに備える。
【００１１】
本発明の第４の態様における前記切り替え指示部は、前記アームの姿勢変化が生じたと判定した場合に前記オドメトリ処理部が推定した移動量の選択を指示する。
【００１２】
本発明の第５の態様における前記位置推定装置は、前記オドメトリ処理部が推定した移動量と前記スキャンマッチング処理部が推定した移動量とのうち何れを選択するかの指示情報の入力を受け付ける入力受付部と、前記切り替え指示部が出力した指示情報と前記入力受付部が受け付けた指示情報とのうち、前記入力受付部が受け付けた指示情報に基づいて、前記移動体の移動量の推定を指示するオペレータ指示優先処理部と、をさらに備える。
【００１３】
本発明の第６の態様における前記スキャンマッチング処理部は、スキャンマッチングの計算において、前記オドメトリ処理部が推定した前記移動体の微小時間における移動量を初期値に用いる。
【００１４】
本発明の第７の態様は、上記の位置推定装置と、前記アームの姿勢変化に応じて前記測域センサの前記移動体を基準とする位置情報を計算するセンサ位置・姿勢計算部と、前記測域センサがスキャンして得た点群情報を、前記センサ位置・姿勢計算部が計算した位置情報と前記位置推定装置が推定した前記移動体の位置情報とを用いて補正し、地図情報を作成する地図情報作成部と、を備える地図情報作成装置である。
【００１５】
本発明の第８の態様は、上述の何れかに記載の位置推定装置と、姿勢変化が可能で前記位置推定装置が備える測域センサが設けられたアームと、を備える移動体である。
【００１６】
本発明の第９の態様は、姿勢変化が可能なアームに設けられ、対象物との距離を計測する測域センサを備える位置推定装置が、前記測域センサがスキャンして得た距離情報が示す対象物の位置に対応する点群情報に基づいてスキャンマッチングを行い、前記測域センサの移動量を推定し、前記アームを有する移動体に備えられた当該移動体の移動に関する情報を計測するセンサの計測値に基づいて前記移動体の移動量を推定し、前記アームの姿勢変化の有無に基づく指示情報を取得し、当該指示情報に基づいて、前記移動体の移動量と前記測域センサの移動量とのうち一方を選択し、選択した移動量に基づいて前記移動体の位置情報を推定する、位置推定方法である。
【００１７】
本発明の第１０の態様は、姿勢変化が可能なアームに設けられ、対象物との距離を計測する測域センサを備える位置推定装置のコンピュータを、前記測域センサがスキャンして得た距離情報が示す対象物の位置に対応する点群情報に基づいてスキャンマッチングを行い、前記測域センサの移動量を推定する手段、前記アームを有する移動体に備えられた当該移動体の移動に関する情報を計測するセンサの計測値に基づいて前記移動体の移動量を推定する手段、前記アームの姿勢変化の有無に基づく指示情報を取得し、当該指示情報に基づいて、前記移動体の移動量と前記測域センサの移動量とのうち一方を選択し、選択した移動量に基づいて前記移動体の位置情報を推定する手段、として機能させるためのプログラムである。
【発明の効果】
【００１８】
本発明によれば、姿勢変化が可能なアーム（マニピュレータ）を備えた移動体の位置情報について、アームに取り付けられた測域センサの検出結果に基づくスキャンマッチングによってその移動体の位置情報を算出する場合に、アームの姿勢変化にかかわらず、移動体の位置情報を正確に計算することができる。
【図面の簡単な説明】
【００１９】
【図１】本発明に係る第一実施形態における測量装置を搭載した走行台車の一例を示す図である。
【図２】本発明に係る第一実施形態における測量装置の一例を示すブロック図である。
【図３】本発明に係る第一実施形態における測量装置に地図情報の作成処理を説明する第１の図である。
【図４】本発明に係る第一実施形態における測量装置に地図情報の作成処理を説明する第２の図である。
【図５】本発明に係る第一実施形態における測量装置の位置情報の推定方法を説明する第１の図である。
【図６】本発明に係る第一実施形態における測量装置の位置情報の推定方法を説明する第２の図である。
【図７】スキャンマッチングが適さない場合を説明する図である。
【図８】本発明に係る第一実施形態における測量装置の移動量の推定処理を説明する図である。
【図９】本発明に係る第一実施形態における測量装置の移動量の推定処理の一例を示すフローチャートである。
【図１０】本発明に係る第二実施形態における測量装置の一例を示すブロック図である。
【図１１】本発明に係る第三実施形態における測量装置の一例を示す第１のブロック図である。
【図１２】本発明に係る第三実施形態における測量装置の一例を示す第２のブロック図である。
【図１３】本発明に係る第四実施形態における測量装置の一例を示す第１のブロック図である。
【図１４】本発明に係る第四実施形態における測量装置の一例を示す第２のブロック図である。
【発明を実施するための形態】
【００２０】
＜第一実施形態＞
以下、本発明の一実施形態による測量装置を図１〜図９を参照して説明する。
図１は本発明に係る第一実施形態における測量装置を搭載した走行台車の一例を示す図である。
図１に示すように走行台車１は、アーム２（２ａ、２ｂ）、測量装置１００、制御装置４０を備えている。アーム２は、２つのジョイント部３ａ、３ｂを含むリンク機構を有しており、アーム２ａとアーム２ｂは、地面と垂直方向（上下）の旋回動作が可能である。あるいはアーム２は、水平方向（左右）の旋回動作が可能であってもよい。アーム２の機構は図示する機構に限定されず、例えば、さらに多くのリンク部を備えていてもよいし、例えば、直動機構を有し伸展動作が可能であってもよい。
アーム２の一方の端は走行台車１に旋回可能に連結され、他方の端の付近にはレーザ測域センサ１０１が設けられている。制御装置４０は、走行台車１の走行・停止、アーム２の旋回動作（または伸展動作）を制御する。
【００２１】
アーム２の先端付近に設けられたレーザ測域センサ１０１は、走行台車１の周囲に存在する対象物（例えば、壁、塀、建物、岩など、これらを総称して障害物と呼ぶ）との距離を計測し、その距離情報に基づいて、レーザ測域センサ１０１の周囲の地図情報を作成するために用いられる。地図情報の作成は測量装置１００が行う。
アーム２は旋回が可能なため、例えば、高所での測量が可能である。また、レーザ測域センサ１０１は、走行台車１の走行中にも距離の計測が可能である。従って、測量装置１００は、走行台車１の走行中に計測された距離情報に基づいて地図情報を作成することが可能である。その場合、計測した距離情報は走行台車１を基準とする座標系で計測された値であるため、地図情報の作成のためには、地図情報の基準となる座標系における座標情報に変換しなければならない。測量装置１００は、レーザ測域センサ１０１が計測したセンサを基準とする座標系における距離情報に対して、走行台車１の走行に伴うレーザ測域センサ１０１の移動分やアーム２の姿勢に応じた走行台車１とレーザ測域センサ１０１との位置関係分を補正する。測量装置１００は、これらの補正を行って、レーザ測域センサ１０１の地図座標系における位置情報を計算する。さらに、測量装置１００は、レーザ測域センサ１０１が計測した距離情報が示す障害物の位置情報について、レーザ測域センサ１０１の地図座標系における位置分の補正を行い、地図座標系における位置情報に変換し、地図情報を作成する。
【００２２】
ここで、図１の各座標系について説明する。Σｍは地図座標系（固定座標）である。Σ０はロボット座標系である。ロボット座標系とは、走行台車１を基準とする座標系である。ロボット座標系では、例えば、走行台車１の重心を原点とする。Σ１はリンク１座標系である。リンク１座標系とは、アーム２ａと走行台車１を接続するジョイント部３ａの中心を原点とする座標系である。Σ２はリンク２座標系である。リンク２座標系とは、アーム２ｂとアーム２ａを接続するジョイント部３ｂの中心を原点とする座標系である。ΣＳはレーザ座標系である。レーザ座標系とは、レーザ測域センサ１０１を基準とする座標系である。レーザ座標系では、例えば、レーザ測域センサ１０１の重心を原点とする。また、ｍＴ０は、地図座標系（ΣＭ）からロボット座標系（Σ０）への座標変換を示す。０Ｔ１は、ロボット座標系（Σ０）からリンク１座標系（Σ１）への座標変換を示す。１Ｔ２は、リンク１座標系（Σ１）からリンク２座標系（Σ２）への座標変換を示す。２ＴＳは、リンク２座標系（Σ２）からレーザ座標系（ΣＳ）への座標変換を示す。なお、以下の説明において、初期状態（走行台車１の移動開始時）には、地図座標系（Σｍ）の原点とはロボット座標系（Σ０）の原点は一致しているとする。
【００２３】
次に測量装置１００について詳しく説明する。図２は本発明に係る第一実施形態における測量装置の一例を示すブロック図である。
測量装置１００は、ＣＰＵ（Central  Processing  Unit）を備えたコンピュータである。また、後述するように測量装置１００は、位置推定装置であり、地図情報作成装置である。図１に示すように測量装置１００は、レーザ測域センサ１０１と、センサ（リンク機構）１０２と、センサ（移動体）１０３と、オペレータ操作端末１０４と、センサ位置・姿勢計算部１０５と、スキャンマッチング処理部１０６と、オドメトリ処理部１０７と、台車移動量推定切り替え部１０８と、座標変換処理部１０９と、を備えている。
【００２４】
レーザ測域センサ１０１は、例えば、３次元のレーザスキャナである。レーザ測域センサ１０１は、レーザ光で走査しながら障害物までの距離を計測する。レーザ測域センサ１０１が走査して得た距離情報が示す障害物の位置の集合を点群情報と呼ぶ。
【００２５】
センサ（リンク機構）１０２は、アーム２の姿勢情報を計測する１つまたは複数のセンサである。センサ（リンク機構）１０２は、例えば、アーム２ａ、２ｂの各々が水平方向（または垂直方向）となす旋回角度を計測する角度計測センサである。アーム２の上下方向の動作は、制御装置４０によって制御される。例えば、仰角計測センサは、アーム２ａとアーム２ｂの各々に設けられ、アーム２ａが水平方向となす角度（θａ）、アーム２ｂが水平方向となす角度（θｂ）を計測する。なお、アーム２が直動機構を有している場合、センサ（リンク機構）１０２は、さらにアーム２のストローク（長さ）を計測するストローク計測センサを有していてもよい。
【００２６】
センサ位置・姿勢計算部１０５は、アーム２の姿勢に応じたロボット座標系におけるレーザ測域センサ１０１の位置情報、姿勢情報を計算する。センサ位置・姿勢計算部１０５は、角度計測センサが計測した角度（θａ）を用いて、幾何学的手法により、ロボット座標系におけるレーザ測域センサ１０１の位置情報、姿勢情報を計算する。例えば、アーム２ａの長さがＬａ、水平方向となす角度をθａとすると、ジョイント部３ｂのリンク１座標系におけるｘ座標はＬａ・ｃｏｓθａ、ｚ座標はＬａ・ｓｉｎθａ、姿勢情報はθａで表される。レーザ測域センサ１０１についても同様である。センサ位置・姿勢計算部１０５は、０Ｔ１、１Ｔ２、２ＴＳの座標変換の関係を用いて、ロボット座標系におけるレーザ測域センサ１０１の位置情報、姿勢情報を計算する。
【００２７】
スキャンマッチング処理部１０６は、センサ位置・姿勢計算部１０５が計算したレーザ測域センサ１０１のロボット座標系における位置情報、姿勢情報と、レーザ測域センサ１０１が計測した障害物との距離情報（点群情報）とを取得して、スキャンマッチングによってレーザ測域センサ１０１の移動量（移動距離、移動方向）を推定する。スキャンマッチングのアルゴリズムには、例えばＩＣＰ（Iterative Closest Point）アルゴリズムを用いることができる。スキャンマッチング処理部１０６は、推定したレーザ測域センサ１０１の移動量の推定値を台車移動量推定切り替え部１０８へ出力する。なお、移動距離とは、所定時間前の走行台車１の所定位置（例えば重心位置）の位置を基準とする現在の走行台車１の所定位置までの距離である。また、移動方向とは、例えば所定の方向に対して走行台車１の進行方向がなす角度である。
【００２８】
センサ（移動体）１０３は、走行台車１に設けられた１つまたは複数のセンサである。センサ（移動体）１０３は、走行台車１の移動量に関する情報を計測する。センサ（移動体）１０３は、例えば、走行台車１のタイヤを回転させるホイールの回転数を計測するホイール回転数計測センサである。ホイール回転数計測センサが計測したホイールの回転数によって走行台車１の走行距離を計算することができる。また、センサ（移動体）１０３は、例えば、走行台車１のタイヤの操舵角度を検出する操舵角計測センサである。操舵角計測センサが計測した操舵角によって走行台車１の走行方向を計算することができる。
【００２９】
オドメトリ処理部１０７は、ホイール回転数計測センサ（センサ（移動体）１０３）が計測したホイールの回転数と操舵角計測センサ（センサ（移動体）１０３）が計測した総舵角を積算して、走行台車１の移動量（移動距離、移動方向）を計算する。オドメトリ処理部１０７は、計算したレーザ測域センサ１０１の移動量の推定値を台車移動量推定切り替え部１０８へ出力する。
【００３０】
台車移動量推定切り替え部１０８は、スキャンマッチング処理部１０６からレーザ測域センサ１０１の移動量を取得し、オドメトリ処理部１０７から走行台車１の移動量を取得する。また、台車移動量推定切り替え部１０８は、移動量推定切り替え指示情報を、オペレータ操作端末１０４から取得する。移動量推定切り替え指示情報には、スキャンマッチング処理部１０６が推定した移動量とオドメトリ処理部１０７が計算した移動量のうち、何れを選択すべきかを指示する情報が含まれている。
台車移動量推定切り替え部１０８は、取得した２つの移動量のうち何れか１つを、オペレータ操作端末１０４から取得した移動量推定切り替え指示に基づいて選択し、選択した移動量を座標変換処理部１０９へ出力する。
【００３１】
座標変換処理部１０９は、レーザ測域センサ１０１から点群情報を取得し、台車移動量推定切り替え部１０８から所定時間ごとの移動量の情報を取得する。まず、座標変換処理部１０９は、台車移動量推定切り替え部１０８から取得した移動量を積算して走行台車１の地図座標系における位置情報、姿勢情報を計算する。なお、位置情報とは、地図座標系における、走行台車１の所定位置（例えば重心位置）の座標情報である。また、姿勢情報とは、例えば所定の方向に対する走行台車１の進行方向がなす角度である。このとき、後に詳しく説明するようにスキャンマッチング処理部１０６が推定したレーザ測域センサ１０１の移動量はアーム２の姿勢に変更がなければ走行台車１の移動量と同じであるとみなせることを利用する。つまり、アーム２の姿勢に変更がなければ、座標変換処理部１０９は、より高精度で誤差の累積のないスキャンマッチング処理部１０６が推定したレーザ測域センサ１０１の移動量を積算して走行台車１の位置情報、姿勢情報を推定する。一方、アーム２の姿勢に変更がある場合、座標変換処理部１０９は、オドメトリ処理部１０７が計算した移動量を積算して走行台車１の位置情報、姿勢情報を推定する。走行台車１の位置情報などを推定すると、座標変換処理部１０９は、レーザ測域センサ１０１から取得した点群情報を走行台車１の位置情報などで補正して、点群情報の地図座標系における座標情報を計算し、地図情報を作成する。具体的には、座標変換処理部１０９は、レーザ座標系でのレーザ測域センサ１０１が計測した点群情報の座標情報を、リンク２座標系を基準とする値に座標変換し、さらにリンク１座標系を基準とする値、次にロボット座標系における座標情報に変換する。そして、先に推定した走行台車１の位置情報、姿勢情報を加算して点群情報それぞれの地図座標系における座標情報を計算する。計算して得られた点群情報それぞれの座標情報が、何らかの障害物が存在する位置を示している。つまり、地図情報である。座標変換処理部１０９は、作成した地図情報をビューワ部２００へ出力する。ビューワ部２００は、取得した地図情報に基づいて障害物を含んだ画像情報を生成し、表示装置に表示させる。
【００３２】
測量装置１００は、この他にも地図情報作成のための種々の機能を有しているが、本実施形態では、地図情報の作成に必要とされる走行台車１およびレーザ測域センサ１０１の位置情報、姿勢情報の推定処理、地図情報の作成処理に必要な構成のみを説明し、他の構成の説明を省略する。
【００３３】
次に図３〜図４を用いて、測量装置１００による地図情報の作成処理の概略について説明する。
図３は、本発明に係る第一実施形態における測量装置に地図情報の作成処理を説明する第１の図である。
図３は、測量装置１００が地図情報作成のための測量を行っている様子を示している。レーザ測域センサ１０１は、所定の範囲に向けてレーザを照射し、周辺環境に存在する障害物の表面形状までの距離情報を計測する。具体的には、周辺に障害物５０が存在した場合、レーザ測域センサ１０１は、障害物５０の表面からの反射によって、障害物５０との距離を計測する。障害物５０上の表示された点線の丸印ＳＰ１、ＳＰ２・・・は、レーザ測域センサ１０１が走査して計測した距離情報に対応する点群情報を示している。ＳＰ１、ＳＰ２・・・は、レーザ測域センサ１０１が取得したレーザ座標系における各点の座標情報である。座標変換処理部１０９は、各点の座標情報を、以下の式によって地図座標系に変換する。
ＭＰｉ  ＝  ＭＴ０・０Ｔ１・１Ｔ２・２ＴＳ・ＳＰｉ
Ｐｉ  ＝  ［Ｘｉ、Ｙｉ、Ｚｉ］Ｔ
Ｐ  ＝  ｛Ｐ１，Ｐ２，・・・，Ｐｉ，・・・，Ｐｎ｝
計測した点群のそれぞれについて地図座標系での座標情報が計算できるとその集合が、周辺環境に存在する障害物の表面の位置情報を表す地図情報となる。座標変換処理部１０９は、作成した地図情報をビューワ部２００へ出力する。
【００３４】
図４は、本発明に係る第一実施形態における測量装置に地図情報の作成処理を説明する第２の図である。
図４は、ビューワ部２００が表示装置に表示した地図情報の一例を示している。表示された地図情報には障害物５０の形状を模した画像（障害物画像５０´）が表示されている。測量装置１００は、様々な空間を走行して、その走行中にもレーザ測域センサ１０１を介して周辺環境の座標情報（ΣＳ）を取得し、その情報を地図座標系（Σｍ）に変換して、走行した空間の地図情報を作成する。ところで、周辺環境の座標情報（ΣＳ）を地図座標系（Σｍ）に変換するには、リンク２座標系、リンク１座標系、ロボット座標系のそれぞれを基準とする座標系に変換し、最後にロボット座標系から地図座標系に変換しなければならない。ここで、レーザ座標系からリンク２座標系、リンク２座標系からリンク１座標系、リンク１座標系からロボット座標系への変換は、それぞれレーザ測域センサ１０１とアーム２ｂ、アーム２ｂとアーム２ａ、アーム２ａと走行台車１の位置関係から幾何学的関係に基づく計算によって座標変換できる。しかし、ロボット座標系から地図座標系への座標変換には、走行台車１の位置情報と姿勢情報が必要である。次に走行台車１の位置情報等の推定について図５、図６を用いて説明する。
【００３５】
図５は、本発明に係る第一実施形態における測量装置の位置情報の推定方法を説明する第１の図である。
図５は、測量装置１００が測量（レーザ測域センサ１０１による走査）を行いながら走行している様子を示している。ある時刻に位置Ａに存在した走行台車１が、その後、位置Ｂまで移動した様子を示している。この間、アーム２の姿勢に変化はないものとする。つまり、位置Ａから位置Ｂまでの走行中に、レーザ測域センサ１０１と、アーム２ａと、アーム２ｂと、走行台車１との位置関係が一定である。この条件の下で、レーザ測域センサ１０１の移動量ΔＬＳと走行台車１の移動量ΔＬＢは同じ距離である。従って、走行台車１の移動量ΔＬＢは、スキャンマッチング処理部１０６が、レーザ測域センサ１０１が所定の時間間隔で計測した点群情報を取得して、取得した時系列の点群情報に基づくスキャンマッチングによって推定することができる。なお、走行台車１の移動量ΔＬＢは、オドメトリ処理部１０７による移動量の計算結果を用いても算出することができるが、オドメトリには、特に長距離を移動した場合、誤差が大きくなりがちであることが知られており、高い精度が得られない可能性があるためスキャンマッチングを用いて移動量を推定することが望ましい。走行台車１の移動量ΔＬＢを計算できれば、座標変換処理部１０９は、レーザ測域センサ１０１が計測した点群情報を地図座標系に変換することができる。
【００３６】
図６は、本発明に係る第一実施形態における測量装置の位置情報の推定方法を説明する第２の図である。
図６（ａ）は、位置Ａでレーザ測域センサ１０１が計測した点群情報（右図）と、位置Ｂでレーザ測域センサ１０１が計測した点群情報（左図）を示している。それぞれの点群情報は障害物５０の表面形状を示しているが、走行台車１が障害物５０へ近づき、レーザ測域センサ１０１と障害物５０の距離が小さくなるほど走査できる範囲が限定されるので、位置Ａで計測した点群情報は障害物５０の形状をより広い範囲でとらえた図６（ａ）→図のようになり、位置Ｂで計測した点群情報は障害物５０のより狭い範囲をとらえた図６（ａ）の左図のようになる。
【００３７】
図６（ｂ）は、スキャンマッチングの原理を説明する図である。図６（ｂ）は、図６（ｂ）左図が示す点群のそれぞれを、右図の対応する点に重ね合わせるように移動させ処理を示している。全ての点が重ならない場合は、重ね合わせる点の同士の関係を変更して、再度試みる。そして、全ての点が重なり合うときの並進量、回転量を求める。求めた並進量、回転量がレーザ測域センサ１０１の移動量ΔＬＳである。２つの点群を重ね合わせるアルゴリズムとして、例えば、ＩＣＰアルゴリズムを用いることができる。これがスキャンマッチングの原理である。つまり、異なる時刻に計測した点群情報のうち一方を移動させて２つの点群情報の間の距離が最も小さくなったときの移動量を、その間のレーザ測域センサ１０１の移動量とする。なお、上述の通り、アーム２の姿勢に変化がない場合、移動量ΔＬＳは、走行台車１の移動量ΔＬＢとなる。
【００３８】
ところで、走行台車１が位置Ａから位置Ｂへ走行する場合に常に精度の高いスキャンマッチングを用いることができればよいが、スキャンマッチングを適用できない場面が存在する。次に図７を用いてスキャンマッチングが適用できない場合について説明する。
図７は、スキャンマッチングが適さない場合を説明する図である。
図７（ａ）は、アーム２ａを上向きに旋回させてレーザ測域センサ１０１をある高さまで上昇させていた姿勢を、走行台車１をその場に停止したまま、アーム２ａを地面と水平となるまで下降させてアーム２を直線状にする姿勢に変化させる様子を示している。この姿勢変化の前後での移動量を、図６（ｂ）で説明したスキャンマッチングにより推定すると、移動量ΔＬ７で図示した移動量を求めることができる。
【００３９】
図７（ｂ）は、アーム２の姿勢を変化させずに走行台車１が、紙面斜め右下方向にΔＬ７で示される分だけ移動した様子を示している。この移動の前後での移動量を、スキャンマッチングにより推定すると、移動量ΔＬ７を求めることができる。このようにスキャンマッチングにより移動量を推定した場合、走行台車１が移動したのか、アーム２の姿勢が変化したのか、あるいは、アーム２の姿勢が変化しつつ走行台車１も移動したのかが分からない。例えば、スキャンマッチングによる移動量を走行台車１の移動量とするならば、図７（ａ）の場合、走行台車１は移動していないにも関わらず、走行台車１が移動量ΔＬ７だけ移動したと推定してしまうことになる。常にスキャンマッチングにより走行台車１の移動量を推定すると、このような不都合が生じる。次にこの不都合を解決する本実施形態の制御方法について説明する。
【００４０】
図８は、本発明に係る第一実施形態における測量装置の移動量の推定処理を説明する図である。
区間８Ａにおいて、走行台車１は、アーム２の姿勢を固定したまま移動する。区間８Ｂでは走行台車１は、アーム２の姿勢を動作させながら移動する。区間８Ｃでは走行台車１は、再びアーム２の姿勢を固定したまま移動する。
本実施形態の測量装置１００は、スキャンマッチングによって区間８Ａにおける走行台車１の移動量を推定する。つまり、測量装置１００では、区間８Ａの移動量について、台車移動量推定切り替え部１０８が、スキャンマッチング処理部１０６の推定した移動量を選択する。
また、測量装置１００は、オドメトリによって区間８Ｂにおける走行台車１の移動量を推定する。つまり、区間８Ｂの移動量について、台車移動量推定切り替え部１０８が、オドメトリ処理部１０７の算出した移動量を選択する。
また、測量装置１００は、スキャンマッチングによって区間８Ｃにおける走行台車１の移動量を推定する。つまり、区間８Ｃの移動量について、台車移動量推定切り替え部１０８が、スキャンマッチング処理部１０６の推定した移動量を選択する。
なお、各区間での移動量推定方法の切り替えは、台車移動量推定切り替え部１０８が、オペレータ操作端末１０４からオペレータが入力した切り替え指示情報に基づいて行う。
本実施形態では、アーム２が姿勢を変化させる間は、オドメトリによって移動量を算出することで図７を用いて説明した不都合を解決する。
【００４１】
次に測量装置１００の移動量の推定処理の流れについて説明する。
図９は、本発明に係る第一実施形態における測量装置の移動量の推定処理の一例を示すフローチャートである。
前提として、走行台車１は、地図情報の作成対象範囲を走行中であるとする。その間、レーザ測域センサ１０１は、所定の時間間隔でレーザ光による走査を行い、その度に計測した点群情報をスキャンマッチング処理部１０６、座標変換処理部１０９へ出力する。また、センサ（リンク機構）１０２は、所定の時間間隔でアーム２の姿勢に関する情報（ジョイント部の角度）をセンサ位置・姿勢計算部１０５へ出力する。また、センサ（移動体）１０３は、所定の時間間隔で走行台車１の走行状態に関する情報（タイヤ回転数、操舵角）をオドメトリ処理部１０７へ出力する。
また、走行中、オペレータの指示により、必要に応じてアーム２は姿勢を変化させる。また、オペレータは、アーム２の姿勢を変化させる場合、オペレータ操作端末１０４に、移動量の算出方法をオドメトリに切り替えるよう指示情報を入力する。また、アーム２の姿勢を固定させる場合、オペレータは、オペレータ操作端末１０４に、移動量の算出方法をスキャンマッチングに切り替えるよう指示情報を入力する。
【００４２】
（移動量推定）
まず、オペレータは、リンク機構を動作させるかどうかを判定する（ステップＳ１１）。つまり、オペレータは、アーム２の姿勢を変化させるかどうかを判定する。動作させる場合（ステップＳ１１；Ｙｅｓ）、オペレータは、操作端末１０４にオドメトリによる移動量の推定を指示する情報を入力する。台車移動量推定切り替え部１０８は、この情報を取得して、移動量の推定方法をオドメトリに設定する。次にステップＳ１５の処理へ進む。
【００４３】
一方、動作させない場合（ステップＳ１１；Ｎｏ）、オペレータは、操作端末１０４にスキャンマッチングによる移動量の推定を指示する情報を入力する。台車移動量推定切り替え部１０８は、この情報を取得して、移動量の推定方法をスキャンマッチングに設定する。次にオペレータは、走行台車１の移動を終了するかどうかを判定する（ステップＳ１２）。終了する場合（ステップＳ１２；Ｙｅｓ）、本処理フローを終了する。
【００４４】
終了しない場合（ステップＳ１２；Ｎｏ）、測量装置１００は、スキャンマッチングによる移動量推定を行う。まず、センサ位置・姿勢計算部１０５がセンサ（リンク機構）１０２から、ジョイント部３ａ、３ｂの旋回角度などアーム２の姿勢に関するセンサ検出情報を取得する（ステップＳ１３）。また、アーム２が直動機構を有する場合、センサ位置・姿勢計算部１０５は、アーム２のストローク情報を取得する。センサ位置・姿勢計算部１０５は、取得したセンサ検出情報を用いて、レーザ測域センサ１０１のロボット座標系における座標情報を計算する。レーザ測域センサ１０１のロボット座標系における座標情報は、例えば地図情報作成時に点群情報を地図座標に座標変換するときに用いられる。センサ位置・姿勢計算部１０５は、計算した座標情報をスキャンマッチング処理部１０６へ出力する。また、スキャンマッチング処理部１０６は、レーザ測域センサ１０１が計測した点群情報を取得する。スキャンマッチング処理部１０６は、スキャンマッチングを開始する（ステップＳ１４）。より具体的には、スキャンマッチング処理部１０６は、所定時間ごとに取得した点群情報について、隣り合う時刻に取得した点群情報を重ね合わせる処理を行い、２つの点群情報が最も良く重なったとき（２つの点群情報の間の距離が最小となったとき）の並進量、回転量を計算し、微小時間ごとの移動量を求める。スキャンマッチング処理部１０６は、時々刻々と求めた微小時間ごとの移動量を積算して走行台車１の移動量を推定する。台車移動量推定切り替え部１０８は、スキャンマッチング処理部１０６が推定した移動量を取得し、座標変換処理部１０９へ出力する。これが本実施形態におけるスキャンマッチングによる移動量推定である。
スキャンマッチング処理部１０６が移動量を推定するのと並行して、ステップＳ１１からの処理を繰り返す。
【００４５】
ステップＳ１１にて、オペレータがオドメトリによる移動量の推定を指示する情報を入力した場合（ステップＳ１１；Ｙｅｓ）、オドメトリ処理部１０７がオドメトリによる移動量推定を開始する（ステップＳ１５）。より具体的には、オドメトリ処理部１０７が、センサ（移動体）１０３から、走行台車１の移動に関する情報（タイヤ回転数、操舵角）を取得し、オドメトリにより走行台車１の移動量を計算する。台車移動量推定切り替え部１０８は、オドメトリ処理部１０７が計算した移動量を取得し、座標変換処理部１０９へ出力する。これが本実施形態におけるオドメトリによる移動量推定である。
オドメトリ処理部１０７が移動量を計算するのと並行して、オペレータは、リンク機構を動作させるかどうかを判定する（ステップＳ１６）。動作させる場合（ステップＳ１６；Ｙｅｓ）、オドメトリ処理部１０７による移動量の計算を継続する（ステップＳ１７）。ステップＳ１６からの処理を繰り返す。
【００４６】
一方、リンク機構を動作させない場合（ステップＳ１６；Ｎｏ）、オペレータは、操作端末１０４にスキャンマッチングによる移動量の推定を指示する情報を入力する。台車移動量推定切り替え部１０８は、この情報を取得して、移動量の推定をスキャンマッチングに切り替える。
次にオペレータは、走行台車１の移動を終了するかどうかを判定する（ステップＳ１８）。終了する場合（ステップＳ１８；Ｙｅｓ）、本処理フローを終了する。
終了しない場合（ステップＳ１８；Ｎｏ）、スキャンマッチング処理部１０６が前回のスキャンマッチング処理時に最後に記憶した点群情報を忘却（消去）する（ステップＳ１９）。これは、オドメトリによる移動量計算を行った区間を跨いで、前回の点群情報と今回の点群情報との間で移動量を求めないことを意味する。オドメトリによって移動量計算を行った区間を跨ぐと、間が空きすぎていることや、アーム２の姿勢が異なること等の理由により推定した移動量が不正確な値となるためである。
【００４７】
次にセンサ位置・姿勢計算部１０５がセンサ（リンク機構）１０２から、ジョイント部３ａ、３ｂの角度などアーム２の姿勢に関するセンサ情報を取得する（ステップＳ２０）。センサ位置・姿勢計算部１０５は、レーザ測域センサ１０１のロボット座標系における座標情報を計算する。センサ位置・姿勢計算部１０５は、計算した座標情報をスキャンマッチング処理部１０６へ出力する。また、スキャンマッチング処理部１０６は、レーザ測域センサ１０１が計測した点群情報を取得する。スキャンマッチング処理部１０６は、スキャンマッチングを再開する（ステップＳ２１）。台車移動量推定切り替え部１０８は、スキャンマッチング処理部１０６が推定した移動量を取得し、座標変換処理部１０９へ出力する。そしてステップＳ１１からの処理を繰り返す。
【００４８】
第一実施形態における走行台車１の移動量推定の計算概要について記載する。
走行台車１の時刻ｔにおける移動量をｘ（ｔ）とする。すると、ｘ（ｔ）は以下の式で表すことができる。
ｘ（ｔ）＝  ｘ（ｔ−１）＋ｄｘｅｓｔ（ｔ）
ここで、例えば、２次元位置情報・姿勢情報を推定する場合、ｘ（ｔ）はＸＹ座標と姿勢角θを用いて表すことができる。
ｘ（ｔ）＝  ［Ｘ（ｔ）、Ｙ（ｔ）、θ］
なお、ｄｘｅｓｔ（ｔ）は、微小時間での移動量を表し、スキャンマッチングの場合、ｄｘｅｓｔ（ｔ）＝ｄｘｓｍ（ｔ）、オドメトリの場合、ｄｘｅｓｔ（ｔ）＝ｄｘｏｄｏｍ（ｔ）である。
【００４９】
また、スキャンマッチングによる移動量推定は、今回取得した点群Ｐ（ｔ）と前回取得したＰ（ｔ−１）より計算する。
ｄｘｓｍ（ｔ）＝ｆｓｍ（Ｐ（ｔ），Ｐ（ｔ−１））
【００５０】
また、オドメトリによる変化量推定は、車速ｖ（ｔ）（タイヤ回転数）と操舵角φ（ｔ）より計算する。
ｄｘｏｄｏｍ（ｔ）＝ｆｏｄｏｍ（ｖ（ｔ），φ（ｔ））
【００５１】
（自己位置推定・地図情報作成）
なお、座標変換処理部１０９は、台車移動量推定切り替え部１０８から取得した走行台車１の移動量を積算して走行台車１の地図座標系における位置情報・姿勢情報を算出し（自己位置推定）、例えば、ステップＳ２０等で算出したレーザ測域センサ１０１の位置情報・姿勢情報を加算する。これにより、レーザ測域センサ１０１の地図座標系における位置情報・姿勢情報を算出する。さらに座標変換処理部１０９は、レーザ測域センサ１０１が取得した点群情報にレーザ測域センサ１０１の地図座標系における位置情報・姿勢情報を加算して、障害物表面の地図座標系における座標情報を算出する。これにより、地図情報を作成することができる。
【００５２】
本実施形態によれば、アーム２の姿勢を変化させる場面では、オペレータによる操作により、スキャンマッチングを停止し、オドメトリによる移動量推定に切り替える手段を有する。また、アーム２の姿勢を固定させる場面では、オペレータによる操作により、再び、スキャンマッチングを再開する手段を有する。そのため、アーム２の姿勢を変化させるときにはオドメトリによる移動量推定を行い、その他の場面では比較的精度の高いスキャンマッチングを用いた移動量推定を行うので、精度の高い自己位置推測が可能である。また、測量装置１００は、正確な自己位置の測定により正確な地図情報の作成を行うことができる。
【００５３】
また、上記したようなスキャンマッチングとオドメトリとを切り替える手段（台車移動量推定切り替え部１０８）を有するため、スキャンマッチングの誤作動防止のために測量装置１００の運用を制限する必要はない（走行中はアーム２を固定しなければならないといった制限）。また、本実施形態によれば、屋内等のＧＰＳによる測位が不可能な環境下でも走行台車１の位置情報・姿勢情報を正確に推測することができ、正確な地図情報を作成できる。これらにより本実施形態によれば、測量装置１００の運用範囲を向上できる。
【００５４】
なお、測量装置１００による測量は例えば次のようにして行う。つまり、詳細に測量を行う場所へ移動する間はアーム２を固定したまま移動し、詳細な測量が必要な場所では、アーム２の姿勢を変化させて様々な角度・位置からレーザ測域センサ１０１による計測を行って点群情報を得る。このとき計測と並行して走行台車１も細やかに移動させて測量位置の微調整を行う。このような測量方法であれば、オドメトリによる移動量推定は、位置調整時の小さな距離に対してのみ行うことになるので推定誤差を抑え、正確な位置推定が可能になる。
【００５５】
また、一般的に地図情報作成のための測量装置においては、その台車にレーザ測域センサを固定して設けることが多く、この場合、本願のようなアーム２の姿勢変化による課題（図７）が生じない。それに対し、本実施形態の測量装置１００は、アーム２の先端付近にレーザ測域センサ１０１を設けるため、高所の測量を行って高所の地図情報を作成することができる。また、走行台車１が入り込めないような狭隘な場所にレーザ測域センサ１０１を挿入して測量を行い、地図情報を作成することができる。また、本実施形態の位置推定方法によれば、姿勢変化が可能なアーム２の先端に設けたレーザ測域センサ１０１がもたらす図７で説明した課題を、スキャンマッチングによる移動量推定とオドメトリによる移動量推定の切り替えによって克服し、レーザ測域センサ１０１が計測した点群情報を正確な走行台車１の位置情報で座標変換することができる。従って、従来は得ることができなかった高所や狭隘な場所の正確な地図情報を得ることができる。
【００５６】
＜第二実施形態＞
以下、本発明の第二実施形態による測量装置について図１０を参照して説明する。
図１０は、本発明に係る第二実施形態における測量装置の一例を示すブロック図である。
本発明の第二実施形態に係る構成のうち、本発明の第一実施形態に係る測量装置１００を構成する機能部と同じものには同じ符号を付し、それぞれの説明を省略する。第二実施形態に係る測量装置１００Ａは、第一実施形態のスキャンマッチング処理部１０６に替えてスキャンマッチング処理部１０６´を、オドメトリ処理部１０７に替えてオドメトリ処理部１０７´を備えている。
オドメトリ処理部１０７´は、オドメトリによって計算した移動量を、台車移動量推定切り替え部１０８だけでなくスキャンマッチング処理部１０６´へも出力する。
スキャンマッチング処理部１０６´は、オドメトリ処理部１０７´が計算した移動量をスキャンマッチング処理における初期値（初期変位量）に用いて、移動量の推定を行う。以下にスキャンマッチング処理部１０６´の処理について説明する。
【００５７】
この第二実施形態では、スキャンマッチング処理部１０６´は、スキャンマッチングによる変化量推定を以下の式で行う。つまり、今回取得した点群Ｐ（ｔ）と前回取得したＰ（ｔ−１）より計算することは第一実施形態と同様だが、スキャンマッチングの初期値として、ｄｘｏｄｏｍ（ｔ）を与える。
ｄｘｓｍ（ｔ）＝ｆｓｍ（Ｐ（ｔ），Ｐ（ｔ−１），ｄｘｏｄｏｍ（ｔ））
【００５８】
例えばＩＣＰアルゴリズムでスキャンマッチングを行う場合、適切な初期値（２つの点群を重ね合わせるよう移動するときの並進量、回転量の初期値）を与えないと局所解に陥りやすくなったり、計算が収束しにくくなったりする可能性がある。そのため、計算処理の効率化、安定化のためには適切な初期値を与える必要がある。そこで、本実施形態では、一つ前のタイミング（時刻ｔ−１）に点群情報を計測したときのオドメトリによる位置情報と、今回のタイミング（時刻ｔ）に点群情報を計測したときのオドメトリによる位置情報との差（オドメトリによる微小時間の移動量）を初期値として与える。つまり、本実施形態では、スキャンマッチング処理部１０６´が、図９のステップＳ１４、ステップＳ２１の処理において、オドメトリ処理部１０７´が計算した微小時間（時刻ｔ−１〜時刻ｔ）における走行台車１のオドメトリによる移動量を、スキャンマッチングの初期値として与える。長時間にわたってオドメトリによる移動量算出を行うと誤差が累積するが、微小時間であれば比較的精度よくその間の移動量を計算できる。これを初期値（初期変位）として与えることで、計算量を抑え、高速かつ確実にスキャンマッチングの解を求められる可能性が高まる。
【００５９】
なお、第二実施形態のセンサ（移動体）１０３として、ジャイロセンサ等の慣性計測装置（ＩＭＵ）をさらに加えてもよい。そして、オドメトリ処理部１０７´が、オドメトリの他に、ＩＭＵが計測した３軸の加速度、角速度に基づいて微小時間の移動量を計算し、この移動量をスキャンマッチング処理部１０６´が初期値として用いてもよい。あるいは、ＩＭＵによる計測結果に基づいて計算した微小時間の移動量のみをスキャンマッチングの初期値として用いてもよい。
スキャンマッチング処理部１０６´は、オドメトリおよびＩＭＵの少なくとも一方による初期値を用いてスキャンマッチングを行う。これにより、走行台車１の位置情報、姿勢情報をさらに正確かつ高速に推定することができる。例えば、走行台車１の足場の角度が急激に変化した場合など、アーム２の姿勢は固定されていたとしても、レーザ測域センサ１０１の姿勢は大きく変化することがある。そのような場合に、適切な初期値を与えることなくスキャンマッチングを行うと、対応点の探索が困難となり、スキャンマッチングが失敗する可能性がある。本実施形態では、オドメトリやＩＭＵが計測したデータを用いて初期変位を与えることができ、スキャンマッチング処理を効率化、安定化することができる。
【００６０】
第二実施施形態によれば、第一実施形態の効果に加え、スキャンマッチング処理部１０６´によるスキャンマッチング処理の精度向上・高速化・ロバスト化が可能となり運用性を向上することができる。
【００６１】
＜第三実施形態＞
以下、本発明の第三実施形態による測量装置について図１１〜図１２を参照して説明する。
図１１は、本発明に係る第三実施形態における測量装置の一例を示す第１のブロック図である。
本発明の第三実施形態に係る構成のうち、本発明の第一実施形態に係る測量装置１００を構成する機能部と同じものには同じ符号を付し、それぞれの説明を省略する。第三実施形態に係る測量装置１００Ｂは、第一実施形態のオペレータ操作端末１０４に替えて、切り替え指示部１１０を備えている。
切り替え指示部１１０は、走行台車１の移動量推定をオドメトリによって行うか、スキャンマッチングによって行うかを管理する。具体的には、切り替え指示部１１０は、センサ（リンク機構）１０２からアーム２の姿勢変化に関する情報を取得する。例えば、切り替え指示部１１０は、センサ（リンク機構）１０２からジョイント部３ａ、３ｂの角度情報を取得する。また、切り替え指示部１１０は、取得した情報からアーム２の姿勢変化を判定する。切り替え指示部１１０は、アーム２の姿勢変化の判定に基づいて、走行台車１の移動量推定をオドメトリによって行うか、スキャンマッチングによって行うかを決定する。より具体的には、現在、スキャンマッチングによる移動量推定を行っている場合であって、センサ（リンク機構）１０２から取得した情報に基づいてアーム２の姿勢変化があったと判定すると、切り替え指示部１１０は、台車移動量推定切り替え部１０８へオドメトリによる移動量推定への切り替えを指示する。また、現在、オドメトリによる移動量推定を行っている場合であって、センサ（リンク機構）１０２から取得した情報によりアーム２の姿勢変化が所定時間にわたって無いと判定すると、切り替え指示部１１０は、台車移動量推定切り替え部１０８へスキャンマッチングによる移動量推定への切り替えを指示する。なお、例えば、図示しない記憶部に現在の移動量推定方法が記録されており、切り替え指示部１１０は、この情報を読み出して、スキャンマッチングとオドメトリのどちらの方法で移動量推定を行っているかを識別する。
【００６２】
次に第三実施形態の測量装置１００Ｂによる移動量推定処理について説明する。本実施形態の移動量推定処理は、図９のフローチャートのステップＳ１１とステップＳ１６にて、オペレータの代わりに切り替え指示部１１０がアーム２（リンク機構）の動作の有無の判定と、その判定結果に基づく台車移動量推定切り替え部１０８への移動量推定方法の指示（スキャンマッチングかオドメトリか）を行う。他の処理については第一実施形態と同様である。
【００６３】
第三実施施形態に係る測量装置１００Ｂは、スキャンマッチングとオドメトリの切り替えを、アーム２の姿勢変化に応じて自動化する手段を有する。従って、第三実施形態によれば、第一実施形態の効果に加え、スキャンマッチング切り替え処理をオペレータが行うことの作業負担が無くなり，誤操作も防止できる。
【００６４】
なお、図１１では、第一実施形態と組み合わせる例を説明したが、第二実施形態と組み合わせることも可能である。第二実施形態と組み合わせた場合の構成例を図１２に示す。
図１２は、本発明に係る第三実施形態における測量装置の一例を示す第２のブロック図である。図示するように測量装置１００Ｂ´は、第二実施形態の測量装置１００Ａにおけるオペレータ操作端末１０４に替えて、切り替え指示部１１０を備えている。
【００６５】
＜第四実施形態＞
以下、本発明の第四実施形態による測量装置について図１３〜図１４を参照して説明する。
図１３は、本発明に係る第四実施形態における測量装置の一例を示す第１のブロック図である。
本発明の第四実施形態に係る構成のうち、本発明の第一実施形態に係る測量装置１００を構成する機能部と同じものには同じ符号を付し、それぞれの説明を省略する。第四実施形態に係る測量装置１００Ｃは、第一実施形態の構成に加え、切り替え指示部１１０と、オペレータ指示優先処理部１１１とを備えている。
切り替え指示部１１０は、走行台車１の移動量推定をオドメトリによって行うか、スキャンマッチングによって行うかを決定する。切り替え指示部１１０自体の機能については、第三実施形態で説明したものと同様である。本実施形態の切り替え指示部１１０は、決定した移動量推定方法（台車移動量推定切り替え指示情報）をオペレータ指示優先処理部１１１に出力する。
【００６６】
オペレータ指示優先処理部１１１は、切り替え指示部１１０とオペレータ操作端末１０４の両方から台車移動量推定切り替え指示情報を取得する。オペレータ指示優先処理部１１１は、オペレータ操作端末１０４から取得した指示情報が存在する場合、切り替え指示部１１０から取得した指示情報より優先して、オペレータ操作端末１０４から取得した指示情報を台車移動量推定切り替え部１０８へ出力する。
【００６７】
例えば、アーム２の姿勢が固定されたまま走行している場合でも、スキャンマッチングによる移動量の推定が適さない環境が存在する。スキャンマッチングが適さない環境とは、例えば、周囲にレーザ光の反射率が低い障害物に囲まれている場合や、路面の凸凹が大きい場合などである。レーザ光の反射率が低い場合、点群情報が十分に取得できずスキャンマッチングに適さない。また、レーザ測域センサ１０１の姿勢の変化速度が大きい場合、スキャンマッチングは適さない。このような場合、オペレータは、オペレータ操作端末１０４にオドメトリによる移動量推定を指示する情報を入力する。オペレータ操作端末１０４は、この指示情報をオペレータ指示優先処理部１１１へ出力する。オペレータ指示優先処理部１１１は、例えば、少し前に切り替え指示部１１０からスキャンマッチングによる移動量推定に切り替える指示情報を取得し、その指示情報を台車移動量推定切り替え部１０８へ出力した直後であっても、オペレータ操作端末１０４から取得したオドメトリによる移動量推定を指示する情報を台車移動量推定切り替え部１０８へ出力する。台車移動量推定切り替え部１０８は、オドメトリ処理部１０７による移動量推定値を選択して、座標変換処理部１０９へ出力する。
【００６８】
また、オペレータ指示優先処理部１１１は、オペレータ操作端末１０４から先に取得した指示情報の解除を命じる指示情報を明示的に入力するまでの間、切り替え指示部１１０から取得した指示情報を無視するように制御してもよい。例えば、反射率の低い障害物に囲まれた空間で、オペレータがオドメトリによる移動量推定をオペレータ操作端末１０４に入力している場合、その空間内で、アーム２の姿勢が変化したり固定されたりして、その都度、切り替え指示部１１０が移動量推定方法の切り替えを指示する情報をオペレータ指示優先処理部１１１へ出力した場合であっても、オペレータ指示優先処理部１１１は、それらの切り替え指示情報を、台車移動量推定切り替え部１０８へ出力しない。例えばこのような制御を行うことにより、オペレータの意図を確実に反映させることができる。
【００６９】
第四実施施形態に係る測量装置１００Ｃは、スキャンマッチングとオドメトリの切り替えをアーム２の姿勢変化の有無に応じて自動的に切り替える切り替え指示部１１０に加え、切り替え指示部１１０の指示に関わらずオペレータの指示により強制的に移動量の推定方法を切り替え、維持するオペレータ指示優先処理部１１１を有する。本実施形態によれば、第一実施形態の効果に加え、スキャンマッチング切り替え処理をオペレータが行うことの作業負担が無くなり，誤操作も防止できる。さらに、地形条件等によってスキャンマッチング処理が安定しないとオペレータが判断した際、オドメトリによる移動量推定に切り替えることができるので、移動量の推定誤差の低減を図ることができる。
【００７０】
なお、図１３では、第一実施形態と組み合わせる例を説明したが、第二実施形態と組み合わせることも可能である。第二実施形態と組み合わせた場合の構成例を図１４に示す。
図１４は、本発明に係る第四実施形態における測量装置の一例を示す第２のブロック図である。図示するように測量装置１００Ｃ´は、第二実施形態の測量装置１００Ａの構成に加え、切り替え指示部１１０と、オペレータ指示優先処理部１１１とを備えている。
【００７１】
なお上述の測量装置１００、１００Ａ、１００Ｂ、１００Ｂ´、１００Ｃ、１００Ｃ´は内部に、コンピュータシステムを有している。そして、上述した測量装置１００等における各処理の過程は、プログラムの形式でコンピュータ読み取り可能な記録媒質に記憶されており、このプログラムをコンピュータが読み出して実行することによって、上記処理が行われる。ここでコンピュータ読み取り可能な記録媒質とは、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、半導体メモリ等をいう。また、このコンピュータプログラムを通信回線によってコンピュータに配信し、この配信を受けたコンピュータが当該プログラムを実行するようにしてもよい。
【００７２】
また、上記プログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。
【００７３】
その他、本発明の趣旨を逸脱しない範囲で、上記した実施の形態における構成要素を周知の構成要素に置き換えることは適宜可能である。また、この発明の技術範囲は上記の実施形態に限られるものではなく、本発明の趣旨を逸脱しない範囲において種々の変更を加えることが可能である。
例えば、図２等ではオペレータ操作端末１０４が測量装置１００に含まれているが、オペレータ操作端末１０４を測量装置１００に含めず、測量装置１００には、オペレータ操作端末１０４が入力を受け付けた情報を取得する機能部を備える構成としてもよい。
なお、走行台車１は移動体の一例である。台車移動量推定切り替え部１０８および座標変換処理部１０９は位置情報推定部の一例である。座標変換処理部１０９は地図情報作成部の一例である。オペレータ操作端末１０４は入力受付部の一例である。
【符号の説明】
【００７４】
１・・・走行台車
２、２ａ、２ｂ・・・アーム
４０・・・制御装置
５０・・・障害物
５０´・・・障害物画像
１００、１００Ａ、１００Ｂ、１００Ｂ´、１００Ｃ、１００Ｃ´・・・測量装置
１０１・・・レーザ測域センサ
１０２・・・センサ（リンク機構）
１０３・・・センサ（移動体）
１０４・・・オペレータ操作端末
１０５・・・センサ位置・姿勢計算部
１０６、１０６´・・・スキャンマッチング処理部
１０７、１０７´・・・オドメトリ処理部
１０８・・・台車移動量推定切り替え部
１０９・・・座標変換処理部
１１０・・・切り替え指示部
１１１・・・オペレータ指示優先処理部
【図１】
2018084544.tif 000003
【図２】
2018084544.tif 000004
【図３】
2018084544.tif 000005
【図４】
2018084544.tif 000006
【図５】
2018084544.tif 000007
【図６】
2018084544.tif 000008
【図７】
2018084544.tif 000009
【図８】
2018084544.tif 000010
【図９】
2018084544.tif 000011
【図１０】
2018084544.tif 000012
【図１１】
2018084544.tif 000013
【図１２】
2018084544.tif 000014
【図１３】
2018084544.tif 000015
【図１４】
2018084544.tif 000016