JP2017211265A - 測量装置、移動体、測量方法及びプログラム - Google Patents

Abstract

【課題】レーザ測域センサによる測量において、測量精度を向上するためにレーザ測域センサの位置情報を推定する測量装置を提供する。【解決手段】測量装置は、地面と垂直な方向に旋回可能で伸縮機構を有するアームに設けられ、対象物との距離を計測する測域センサと、アームの伸縮量および旋回角に基づいて、測域センサの位置情報を計算するセンサ位置幾何学計算処理部と、センサ位置幾何学計算処理部が計算した測域センサの位置情報と、測域センサが走査して得た距離情報が示す対象物の位置に対応する点群情報とに基づいてスキャンマッチングを行い、測域センサの位置情報を計算するスキャンマッチング計算部と、を備える。【選択図】図１

Description

本発明は、測量装置、移動体、測量方法及びプログラムに関する。

走行台車やロボット等の移動体にレーザ測域センサを搭載し、周辺の環境に存在する障害物との距離を計測し、移動体と障害物との位置関係から障害物の地図を構築する技術が知られている。レーザ測域センサは、センサ原点を基準として障害物までの距離情報を出力するため、移動しながら障害物の地図を作成するためには、レーザ測域センサが出力する距離情報を絶えず座標変換して絶対座標での値に更新する必要がある。このため、移動体による障害物地図作成では、レーザ測域センサの正確な位置・姿勢を常に計測する手段が必要である。レーザ測域センサの位置情報を計測するために、たとえばＧＰＳ等の位置センサを使う構成が考えられる。

例えば、特許文献１や特許文献２には、走行台車に伸縮機構を有するアームを設け、アームの先端にＧＰＳとレーザ測域センサを搭載して、アーム先端の位置情報を計測できるようにした測量装置が開示されている。この技術を用いると、土木工事の現場、災害現場など、瓦礫や地形の問題で走行台車を測量地点に移動させることが困難な場合や、測量対象がレーザ測距センサの視野角に入らないほど高所の場合でも、アームを伸ばすことで測量が可能になる。また、アーム端のＧＰＳによる測位情報によって、レーザ測域センサの位置を確認することができるので、ＧＰＳによる測位情報を用いてレーザ測距センサの計測値を座標変換することで測量結果の精度を保つことができる。

特開平７−１２８０５７号公報 特開平９−１９６６７２号公報

しかし、特許文献１、２の方法では、センサ位置がＧＰＳによって観測されることが前提となっており，屋内などのＧＰＳ信号が受信できない環境では使用できない。また、走行台車の位置自体は、タイヤの回転速度、操舵角から幾何学的にある程度類推することが可能だが、初期位置からの相対変位を求める計算手法（オドメトリ）となるため走行距離増加に伴い誤差が累積するという課題がある。

さらに、伸縮機構を有するアームの先端にレーザ測域センサを設ける場合、伸縮機構の仰角、およびストローク（伸展量）を計測する手段があれば、センサ位置をある程度幾何学的に推定することが可能だが、センサが設置される先端に向かうほど重力による部材の撓みが大きく影響し、センサ位置を正確に推定することは困難となる。部材の剛性を高めれば撓みの影響を小さくできるが、伸縮機構を含む走行台車全体の重量が大きくなり、移動に要する燃費の悪化、装置全体のコスト増を招いてしまう。あるいは、伸縮機構に歪ゲージ等を追加して、撓み量を直接推定する手段も考えられるが、センサの追加によってコストが増大する。また、全体的な重量軽減のためにアームに軽量部材を使用すると剛性が低くなるため、アームの先端位置は単純な片持ち梁をモデルとして計算すると位置・姿勢推定精度の誤差が大きくなってしまうといった課題がある。

そこでこの発明は、上述の課題を解決することのできる測量装置、移動体、測量方法及びプログラムを提供することを目的としている。

本発明の第１の態様は、地面と垂直な方向に旋回可能で伸縮機構を有するアームに設けられ、対象物との距離を計測する測域センサと、前記アームの長さおよび旋回角に基づいて、前記測域センサの位置情報を計算するセンサ位置幾何学計算処理部と、前記センサ位置幾何学計算処理部が計算した測域センサの位置情報と、前記測域センサが走査して得た距離情報が示す対象物の位置に対応する点群情報と、に基づいてスキャンマッチングを行い、前記測域センサの位置情報を計算するスキャンマッチング計算部と、を備える測量装置である。

本発明の第２の態様における前記測域センサは、所定の時間間隔で走査を行って点群情報を計測し、前記スキャンマッチング計算部は、前記測域センサが所定のタイミングで計測した点群情報と、１つ前のタイミングで計測した点群情報とを比較してスキャンマッチングを行うにあたり、それぞれのタイミングにおいて前記センサ位置幾何学計算処理部が計算した測域センサの位置情報を用いて前記測域センサが計測した点群情報のそれぞれを変位させ、スキャンマッチングを行ってもよい。

本発明の第３の態様における前記測量装置は、前記測域センサの姿勢情報を計測する慣性センサ、をさらに備え、前記スキャンマッチング計算部は、前記測域センサの位置情報に加えて、前記測域センサの姿勢情報を用いて、スキャンマッチングを行ってもよい。

本発明の第４の態様における前記測量装置は、前記アームの長さおよび旋回角に応じた当該アームの撓み量を計算する撓み補償量計算部、をさらに備え、前記スキャンマッチング計算部は、前記撓み補償量計算部が計算した撓み量による補正後の前記測域センサの位置情報を用いて、スキャンマッチングを行ってもよい。

本発明の第５の態様における前記測量装置は、前記アームを有する移動体に備えられたセンサの計測値に基づいて当該移動体の位置情報を計算するオドメトリ計算処理部、をさらに備え、前記スキャンマッチング計算部は、前記オドメトリ計算処理部が計算した前記移動体の位置情報に基づく前記測域センサの位置情報を用いてスキャンマッチングを行ってもよい。

本発明の第６の態様における前記スキャンマッチング計算部は、前記アームが伸展動作および旋回動作が行われていない収納状態にある場合、前記オドメトリ計算処理部が計算した前記移動体の位置情報に基づく前記測域センサの位置情報のみを用いてスキャンマッチングを行い、前記アームが伸展動作および旋回動作のうち少なくとも一方が行われている作業状態にある場合、前記オドメトリ計算処理部が計算した前記測域センサの位置情報に加えて、前記センサ位置幾何学計算処理部が計算した測域センサの位置情報を用いてスキャンマッチングを行ってもよい。

本発明の第７の態様における前記測域センサは、前記アームの軸方向回りに回転可能に設けられ、２次元平面を走査する測域センサであって、前記アームが伸展動作および旋回動作のうち少なくとも一方が行われている作業状態にある場合、前記測域センサが走査する２次元平面が地面と垂直となるよう制御する走査平面方向変更部、をさらに備えていてもよい。

本発明の第８の態様における前記走査平面方向変更部は、前記アームが伸展動作および旋回動作が行われていない収納状態にある場合、前記測域センサの走査平面が地面と平行となるよう制御してもよい。

本発明の第９の態様における前記測量装置では、前記測域センサは、前記アームの軸方向回りに回転可能に設けられ、２次元平面を走査する測域センサであって、前記測域センサの回転量を取得し、前記測域センサが走査して得た点群情報を３次元座標に変換する点群座標変換部と、前記点群座標変換部が変換した点群情報を蓄積し、蓄積した点群情報の集合を用いて３次元の点群情報を生成する点群メモリ部と、をさらに備え、前記スキャンマッチング計算部は、前記点群メモリ部が生成した３次元の点群情報を用いてスキャンマッチングを行ってもよい。

本発明の第１０の態様は、上述の何れかに記載の測量装置と、地面と垂直な方向に旋回可能で伸縮機構を有するアームであって、前記測量装置が備える測域センサが設けられたアームと、を備える移動体であってよい。

本発明の第１１の態様は、地面と垂直な方向に旋回可能で伸縮機構を有するアームに設けられ、対象物との距離を計測する測域センサを備える測量装置が、前記アームの長さおよび旋回角に基づいて、前記測域センサの位置情報を計算し、前記計算した測域センサの位置情報と、前記測域センサが走査して得た距離情報が示す対象物の位置に対応する点群情報と、に基づいてスキャンマッチングを行い、前記測域センサの位置情報を計算する、測量方法であってよい。

本発明の第１２の態様は、地面と垂直な方向に旋回可能で伸縮機構を有するアームに設けられ、対象物との距離を計測する測域センサを備える測量装置のコンピュータを、前記アームの長さおよび旋回角に基づいて、前記測域センサの位置情報を計算する手段、前記計算した測域センサの位置情報と、前記測域センサが走査して得た距離情報が示す対象物の位置に対応する点群情報と、に基づいてスキャンマッチングを行い、前記測域センサの位置情報を計算する手段、として機能させるためのプログラムであってよい。

本発明によれば、測域センサの位置情報を正確に計算することができる。それにより、測域センサの検出結果に基づく地図情報の精度を高めることができる。

本発明に係る第一実施形態における測量装置の一例を示すブロック図である。 本発明に係る第一実施形態における測量装置を搭載した走行台車の一例を示す図である。 従来の方法による測量装置の測域センサの位置情報の計算方法を説明する第一の図である。 本発明に係る第一実施形態における測量装置の測域センサの位置情報の推定方法を説明する第一の図である。 従来の方法による測量装置の測域センサの位置情報の計算方法を説明する第二の図である。 本発明に係る第一実施形態における測量装置の測域センサの位置情報の推定方法を説明する第二の図である。 本発明に係る第一実施形態における測域センサの位置情報推定処理の一例を示す第一のフローチャートである。 本発明に係る第一実施形態における測域センサの位置情報推定処理の一例を示す第二のフローチャートである。 本発明に係る第一実施形態におけるスキャンマッチングの説明を行う図である。 本発明に係る第二実施形態における測量装置の一例を示すブロック図である。 本発明に係る第二実施形態における測量装置を搭載した走行台車の一例を示す図である。 本発明に係る第三実施形態における測量装置の一例を示すブロック図である。 本発明に係る第四実施形態における測量装置の一例を示すブロック図である。 本発明に係る第四実施形態における測量装置を搭載した走行台車の一例を示す図である。 本発明に係る第四実施形態における走査平面を示す第一の図である。 本発明に係る第四実施形態における走査平面を示す第二の図である。 本発明に係る第四実施形態における測域センサの位置情報推定処理の一例を示すフローチャートである。 本発明に係る第五実施形態における測量装置の一例を示すブロック図である。 本発明に係る第五実施形態における走査方法を示す図である。

＜第一実施形態＞
以下、本発明の一実施形態による測量装置を図１〜図９を参照して説明する。
図１は本発明に係る第一実施形態における測量装置の一例を示すブロック図である。
図２は本発明に係る第一実施形態における測量装置を搭載した走行台車の一例を示す図である。
図２に示すように走行台車１は、アーム２、測量装置１００、制御装置５０を備えている。アーム２の一方の端は走行台車１に旋回可能に連結され、アーム２は地面と垂直方向の旋回動作（上下動作）が可能である。また、アーム２の他方の先端付近にはレーザ測域センサ１０１が設けられている。また、アーム２は伸縮機構を有しており伸展動作が可能である。制御装置５０は、走行台車１の走行・停止、アーム２の伸縮・旋回動作を制御する。以下、アーム２が縮んだ状態（アーム２の長さがＬ_０）で且つアーム２が下げられた状態（仰角が略０度）を収納状態と呼ぶ。また、アーム２が伸展または旋回されている状態を作業状態と呼ぶ。アーム２の先端付近に設けられたレーザ測域センサ１０１は、走行台車１の周囲に存在する対象物（例えば、壁、塀、建物、岩など、これらを総称して障害物と呼ぶ）との距離を計測し、その距離情報に基づいて、レーザ測域センサ１０１の周囲の地図情報を作成するために用いられる。アーム２の伸展、旋回が可能なため、高所での測量が可能である。また、走行台車１は走行し、レーザ測域センサ１０１は、走行台車１の走行中にも距離の計測が可能である。従って、走行台車１の走行中に計測された距離情報に基づいて地図情報を作成することも可能であるが、地図情報を作成するためには、計測した距離情報は、走行台車１を基準とする座標系で計測された値であるため、地図情報の基準となる座標系における座標情報に変換しなければならない。つまり、レーザ測域センサ１０１が計測した自身を基準とする距離情報に対して、走行台車１の走行に伴うレーザ測域センサ１０１の移動分を補正する必要がある。また、高所測量時のように走行台車１が停止している場合であっても、アーム２が伸展、旋回した場合、アーム２の動作によるレーザ測域センサ１０１の移動分を補正する必要がある。測量装置１００は、これらの補正を行って、レーザ測域センサ１０１の地図座標系における位置情報を計算する。

次に測量装置１００について詳しく説明する。測量装置１００は、ＣＰＵ（Central Processing Unit）を備えたコンピュータである。図１に示すように測量装置１００は、レーザ測域センサ１０１と、ホイール回転数計測センサ１０２と、操舵角計測センサ１０３と、ストローク計測センサ１０４と、仰角計測センサ１０５と、オドメトリ計算処理部１０６と、センサ位置座標変換部１０７と、センサ位置幾何学計算処理部１０８と、センサ位置地図座標変換部１０９と、センサ位置情報選択部１１０と、スキャンマッチング計算部１１１とを備えている。

レーザ測域センサ１０１は、例えば、３次元のレーザスキャナである。レーザ測域センサ１０１は、レーザ光で走査しながら障害物までの距離を計測する。レーザ測域センサ１０１が走査して得た距離情報が示す障害物の位置の集合を点群情報と呼ぶ。
ホイール回転数計測センサ１０２は、走行台車１が備えるタイヤを回転させるホイールの回転数を計測するセンサである。ホイール回転数計測センサ１０２が計測したホイールの回転数によって走行台車１の走行距離を計算することができる。
操舵角計測センサ１０３は、走行台車１が備えるタイヤの操舵角度を検出するセンサである。操舵角計測センサ１０３が計測した操舵角によって走行台車１の走行方向を計算することができる。

オドメトリ計算処理部１０６は、ホイール回転数計測センサ１０２が計測したホイールの回転数と操舵角計測センサ１０３が計測した操舵角を累積して、ある基準点に対する走行台車１の位置情報、姿勢情報を計算する。なお、位置情報とは、所定位置を原点とする座標系（例えば図２の座標系ａ）における、走行台車１の所定位置（例えば重心位置）の座標情報である。また、姿勢情報とは、例えば所定の方向に対して走行台車１の進行方向がなす角度である。なお、図２の座標系ａとは、作成する地図情報を基準とする座標系である。以下この座標系を地図座標系と呼ぶ場合がある。

センサ位置座標変換部１０７は、アーム２の収納状態におけるレーザ測域センサ１０１の位置情報、姿勢情報を計算する。具体的には、オドメトリ計算処理部１０６が計算した走行台車１の位置情報、姿勢情報に、図２の座標系ｂにおけるレーザ測域センサ１０１の収納状態における位置情報、姿勢情報を加算し、座標系ａにおけるレーザ測域センサ１０１の位置情報、姿勢情報を計算する。なお、図２の座標系ｂとは、走行台車１の所定位置（例えば重心位置）を原点とし、前方方向をｘ軸、地面に対して垂直方向をｚ軸とする走行台車１を基準とする座標系である。

ストローク計測センサ１０４は、アーム２のストロークを計測するセンサである。アーム２の伸縮は、走行台車１が備える制御装置５０によって制御される。ストローク計測センサ１０４は、例えば、制御装置５０からアーム２の伸展量の指令値を取得し、この指令値に基づく伸展量を加算したアーム２の長さ（Ｌ_０＋ΔＬ）を計算する。
仰角計測センサ１０５は、アーム２の垂直方向の旋回角度（仰角）を計測する。アーム２の上下方向の動作は、制御装置５０によって制御される。例えば、仰角計測センサ１０５は、制御装置５０からアーム２の上下動の指令値を取得し、この指令値に基づいてアーム２の仰角（θ_１）を計算する。

センサ位置幾何学計算処理部１０８は、ストローク計測センサ１０４が計測したアーム２の長さ（Ｌ_０＋ΔＬ）と仰角計測センサ１０５が計測した仰角（θ_１）を用いて、幾何学的手法により図２の座標系ｃにおけるレーザ測域センサ１０１の位置情報、姿勢情報を計算する。なお、座標系ｃとは、走行台車１とアーム２の連結位置を原点とし、前方方向をｘ軸、地面に対して垂直方向をｚ軸とするアーム２の連結部を基準とする座標系である。例えば、座標系ｃにおけるレーザ測域センサ１０１のｘ座標は（Ｌ_０＋ΔＬ）ｃｏｓθ_１、ｚ座標は（Ｌ_０＋ΔＬ）ｓｉｎθ_１、姿勢情報はθ_１で表される。
センサ位置地図座標変換部１０９は、アーム２の作業状態におけるレーザ測域センサ１０１の地図座標系における位置情報、姿勢情報を計算する。具体的には、センサ位置地図座標変換部１０９は、センサ位置座標変換部１０７が計算した地図座標系でのレーザ測域センサ１０１の位置情報、姿勢情報と、センサ位置幾何学計算処理部１０８が計算した座標系ｃでのレーザ測域センサ１０１の位置情報、姿勢情報とを取得し、これらを用いて地図座標系におけるレーザ測域センサ１０１の位置情報、姿勢情報を計算する。より具体的には、センサ位置地図座標変換部１０９は、座標系ｃでのレーザ測域センサ１０１の位置情報、姿勢情報を、座標系ｂを基準とする値に座標変換し、さらに座標変換後の値にセンサ位置座標変換部１０７が計算した地図座標系で位置情報、姿勢情報を加算する。

センサ位置情報選択部１１０は、ユーザが入力した現在の走行台車１の走行状態、アーム２の動作状態についての情報を取得し、現在の動作状態に対応するレーザ測域センサ１０１の地図座標系における位置情報、姿勢情報を出力する。具体的には、センサ位置情報選択部１１０は、ユーザが入力した走行台車１が移動しているかアーム２が伸展されているかを示す情報を取得する。また、センサ位置情報選択部１１０は、センサ位置座標変換部１０７が計算した位置情報、姿勢情報と、センサ位置地図座標変換部１０９が計算した位置情報、姿勢情報とを取得する。なお、走行台車１の移動中は、アーム２が収納状態である。この場合、レーザ測域センサ１０１の地図座標系における位置情報、姿勢情報は、センサ位置座標変換部１０７が計算した位置情報、姿勢情報である。この場合、レーザ測域センサ１０１の地図座標系における位置情報は、センサ位置地図座標変換部１０９が計算した位置情報、姿勢情報である。センサ位置情報選択部１１０は、アーム２の動作状態に基づいて、センサ位置座標変換部１０７又はセンサ位置地図座標変換部１０９が計算した位置情報を選択して、スキャンマッチング計算部１１１に出力する。

スキャンマッチング計算部１１１は、オドメトリや幾何学的手法を用いて計算したレーザ測域センサ１０１の地図座標系における位置情報、姿勢情報と、レーザ測域センサ１０１が計測した障害物との距離情報（点群情報）とを取得して、スキャンマッチングによってレーザ測域センサ１０１の位置情報を推定する。スキャンマッチングのアルゴリズムには、例えばＩＣＰ（Iterative Closest Point）アルゴリズムを用いることができる。スキャンマッチング計算部１１１は、ＩＣＰアルゴリズムの初期値として、センサ位置情報選択部１１０から取得したレーザ測域センサ１０１の位置情報、姿勢情報を用いる。スキャンマッチング計算部１１１は、推定したレーザ測域センサ１０１の位置・姿勢の推定値を出力する。出力された推定値は、走行台車１が存在する位置周辺の地図情報の作成に用いられる。
測量装置１００は、この他にも地図情報作成のための種々の機能を有していてもよいが、本実施形態では、地図情報の作成時に必要とされるレーザ測域センサ１０１の位置情報、姿勢情報の推定に必要な構成のみを説明し、他の構成の説明を省略する。

次に図３、図４を用いて、走行台車１の走行時におけるレーザ測域センサ１０１の位置情報の計算方法の概略について説明する。
図３は、従来の方法による測量装置の測域センサの位置情報の計算方法を説明する第一の図である。
従来は、レーザ測域センサ１０１の位置情報を、オドメトリによる測位やＧＰＳの測位情報を用いて計算することが多かった。オドメトリによる測位には、誤差が累積されるという課題があり、ＧＰＳによる測位にはＧＰＳ電波を受信できない環境では測位できない、反射波による精度の低下、測位間隔が長く、測位の間の移動を把握することができない等の課題があった。図３は、屋内や電波環境が良好ではない等のＧＰＳが使用できない環境下でオドメトリのみによってレーザ測域センサ１０１の位置情報を計算した場合の、走行台車１に伴って実際にレーザ測域センサ１０１が移動した軌跡と、オドメトリを用いて計算したレーザ測域センサ１０１の軌跡とを示す図である。図３の軌跡７は、障害物５Ａ、５Ｂの間の通路を走行台車１が移動したときの実際のレーザ測域センサ１０１の移動経路を示している。これに対し、軌跡６は、走行台車１が移動に伴って計測したホイール回転数や操舵角度に基づくオドメトリによるレーザ測域センサ１０１の移動経路を示している。図示するようにオドメトリによる測位のみを用いると誤差が徐々に大きくなる場合が多い。レーザ測域センサ１０１の位置情報の誤差が拡大すると、スキャンマッチングによる走行台車１の周辺の地図情報の精度も低下する。

図４は、本発明に係る第一実施形態における測量装置の測域センサの位置情報の推定方法を説明する第一の図である。
障害物５Ａ、５Ｂ上に表示された点線の丸印は、レーザ測域センサ１０１が時刻ｔ−１に所定の範囲を走査して計測した距離情報に対応する点群情報を示している。同様に実線の丸印は、レーザ測域センサ１０１が時刻ｔに所定の範囲を走査して計測した距離情報に対応する点群情報を示している。スキャンマッチング計算部１１１は、オドメトリによって計算した時刻ｔ−１におけるレーザ測域センサ１０１の位置情報で時刻ｔ−１に得られた点群情報を補正する。また、スキャンマッチング計算部１１１は、オドメトリによって計算した時刻ｔにおけるレーザ測域センサ１０１の位置情報で時刻ｔに得られた点群情報を補正する。スキャンマッチング計算部１１１は、補正後の時刻ｔ−１、ｔにおける点群情報を用いてスキャンマッチングを行いレーザ測域センサ１０１の位置の推定を行う。図４の軌跡８は、スキャンマッチング計算部１１１が推定した位置情報の軌跡を示している。図示するように軌跡７と軌跡８の誤差は、図３で例示した軌跡７と軌跡６の誤差に比べ小さく、精度よくレーザ測域センサ１０１の位置情報を推定できることが分かる。レーザ測域センサ１０１の位置情報の誤差が小さいので、スキャンマッチングによる走行台車１の周辺の地図情報の精度も向上する。本実施形態によれば、レーザ測域センサ１０１の位置情報の平面方向における精度が向上できることが分かる。

次に図５、図６を用いて、走行台車１を停車し、アーム２を伸展させた場合のレーザ測域センサ１０１の位置情報の計算方法の概略について説明する。
図５は、従来の方法による測量装置の測域センサの位置情報の計算方法を説明する第二の図である。
図５は、幾何学的手法のみによって計算した場合のレーザ測域センサ１０１の位置情報と実際のレーザ測域センサ１０１の位置との差を示している。従来は、アーム２の伸展時におけるレーザ測域センサ１０１の位置情報を、伸展長さ（Ｌ_０＋ΔＬ）と仰角θ１による幾何学的な計算を用いて計算することが多かった。つまり、従来の方法では、図５に示す点３の位置を計算する。しかし、実際には、アーム２の撓みによって、レーザ測域センサ１０１は点３´に位置する。従来の方法では、アーム２の撓みを考慮しない為、レーザ測域センサ１０１の位置情報に誤差が生じる。従って、アーム２を伸展させて所定の範囲を走査して得られる高所の障害物９の地図情報の精度も低下する。

図６は、本発明に係る第一実施形態における測量装置の測域センサの位置情報の推定方法を説明する第二の図である。
障害物９上に表示された点線の丸印は、レーザ測域センサ１０１が時刻ｔ−１に所定の範囲を走査して計測した距離情報に対応する位置を示している。実線の丸印は、レーザ測域センサ１０１が時刻ｔに所定の範囲を走査して計測した距離情報に対応する点群情報を示している。スキャンマッチング計算部１１１は、幾何学的手法によって計算した時刻ｔ−１におけるレーザ測域センサ１０１の位置情報を用いて、時刻ｔ−１に得られた点群情報を補正する。また、スキャンマッチング計算部１１１は、幾何学的手法によって計算した時刻ｔにおけるレーザ測域センサ１０１の位置情報を用いて、時刻ｔに得られた点群情報を補正する。スキャンマッチング計算部１１１は、補正後の時刻ｔ−１、ｔにおける点群情報を用いてスキャンマッチングを行いレーザ測域センサ１０１の位置の推定を行う。これにより、スキャンマッチング計算部１１１は、点３´に近い位置をレーザ測域センサ１０１の位置情報として推定できる可能性が高くなる。本実施形態によれば、部材の剛性を上げたり、センサを追加したり、部材を軽量化する等の対応を行うことなく、アーム２の伸展・旋回時におけるレーザ測域センサ１０１の位置情報、姿勢情報の推定精度を向上することができる。レーザ測域センサ１０１の位置情報、姿勢情報をより精度よく推定できるので、スキャンマッチングによる高所の地図情報の精度も向上する。

次にレーザ測域センサ１０１の位置情報の推定処理の流れについて説明する。
図７は、本発明に係る第一実施形態における測域センサの位置情報推定処理の一例を示す第一のフローチャートである。
前提として、走行台車１は、地図情報の作成対象位置（測量対象位置）へ移動して、その場で停車してアーム２を展開し、アーム２を上下させ障害物９に対してレーザ光を走査し、障害物９を対象とした点群情報を計測するものとする。また、走行台車１の移動中、アーム２は収納状態であり、走行台車１が対象位置へ到着した後にアーム２を作業状態に切り替えるものとする。また、ユーザは、アーム２が収納状態か作業状態かを測量装置１００へ入力する。また、レーザ測域センサ１０１は、走行台車１の走行中も走査を行い、走行中の走行台車１の周辺に存在する障害物５Ａ、５Ｂを対象とする点群情報を計測する。また、レーザ測域センサ１０１は、所定の時間間隔で走査を行い、その度に計測した点群情報をスキャンマッチング計算部１１１に出力しているとする。

まず、走行台車１が移動を開始する（ステップＳ１０）。例えば、ユーザは、走行台車１を遠隔操作によって操作し、地図情報の作成対象位置へ向けて移動させる。
走行台車１が移動中、測量装置１００は、レーザ測域センサ１０１の位置情報を推定する（ステップＳ１１）。具体的には、オドメトリ計算処理部１０６は、所定の時間間隔でホイール回転数計測センサ１０２の計測値と、操舵角計測センサ１０３の計測値とを取得し、走行台車１の移動量、移動方向を計算する。次に、オドメトリ計算処理部１０６は、計算した移動量、移動方向を走行台車１の初期位置等に加算して走行台車１の位置情報、姿勢情報を計算する。また、センサ位置座標変換部１０７は、オドメトリ計算処理部１０６が計算した走行台車１の位置情報、姿勢情報と、収納状態におけるレーザ測域センサ１０１の走行台車１を基準とする座標系での位置情報、姿勢情報とを用いて、レーザ測域センサ１０１の地図座標系での位置情報、姿勢情報を計算する。このようにセンサ位置座標変換部１０７は、走行台車１の移動中に、所定の時間間隔でレーザ測域センサ１０１の位置情報（オドメトリによる位置情報）を計算し、センサ位置情報選択部１１０に出力する。センサ位置情報選択部１１０は、アーム２が収納状態にあることに基づいて、センサ位置座標変換部１０７が計算した位置情報を選択してスキャンマッチング計算部１１１に出力する。次にスキャンマッチング計算部１１１は、センサ位置座標変換部１０７が計算したレーザ測域センサ１０１の位置情報、姿勢情報と、レーザ測域センサ１０１が計測した障害物５Ａ、５Ｂに対する点群情報とを用いてＩＣＰスキャンマッチングを行いレーザ測域センサ１０１の位置情報を推定する。このようにスキャンマッチング計算部１１１は、走行台車１の移動中には、オドメトリによる位置情報を用いてスキャンマッチングを行う。図２の軌跡８は、スキャンマッチング計算部１１１が推定したセンサ位置の軌跡を示している。スキャンマッチング計算部１１１は、推定したレーザ測域センサ１０１の位置情報、姿勢情報を制御装置５０等に出力する。

次に、制御装置５０が、走行台車１の移動が完了したかどうかを判定する（ステップＳ１２）。例えば、制御装置５０は、スキャンマッチング計算部１１１から取得したレーザ測域センサ１０１の位置情報に基づいて、走行台車１が所定の対象位置に到達したかどうかを判定する。到達した場合、制御装置５０は移動が完了したと判定する。到達していない場合、制御装置５０は、移動が完了していないと判定する。移動が完了していない場合、ステップＳ１１からの処理を繰り返す。移動が完了した場合、制御装置５０は移動が完了したことを示す信号をユーザへ通知する。このとき、ユーザは、アーム２の動作状態を収納状態から作業状態へ切り替えることを測量装置１００へ入力する。また、ユーザは、アーム２の伸展、旋回動作を指示する情報を制御装置５０へ入力する。制御装置５０は、アーム２の動作を制御し、アーム２は伸展動作・旋回動作を開始する（ステップＳ１３）。

アーム２が作業状態にあるとき、測量装置１００は、レーザ測域センサ１０１の位置情報を推定する（ステップＳ１４）。具体的には、センサ位置幾何学計算処理部１０８は、ストローク計測センサ１０４の計測値と、仰角計測センサ１０５の計測値とを取得し、アーム２のストローク、仰角を計算する。また、センサ位置地図座標変換部１０９は、センサ位置幾何学計算処理部１０８が計算した作業状態におけるレーザ測域センサ１０１の走行台車１を基準とする座標系での位置情報、姿勢情報と、センサ位置座標変換部１０７が計算したレーザ測域センサ１０１の位置情報、姿勢情報とを用いて、レーザ測域センサ１０１の地図座標系での位置情報を計算する。このようにセンサ位置座標変換部１０７は、アーム２が伸縮したり上下に移動したりする度に、レーザ測域センサ１０１の位置情報（幾何学的手法による位置情報）を計算し、センサ位置情報選択部１１０に出力する。センサ位置情報選択部１１０は、アーム２が作業状態にあることに基づいて、センサ位置地図座標変換部１０９が計算した位置情報を選択してスキャンマッチング計算部１１１に出力する。次にスキャンマッチング計算部１１１は、センサ位置地図座標変換部１０９が計算したレーザ測域センサ１０１の位置情報、姿勢情報と、レーザ測域センサ１０１が計測した障害物９に対する点群情報とを用いてＩＣＰスキャンマッチングを行いレーザ測域センサ１０１の位置情報、姿勢情報を推定する。このようにスキャンマッチング計算部１１１は、走行台車１が停止しアーム２が作業状態にあるときは、オドメトリによって計算した走行台車１の位置情報に、幾何学的手法によって計算したアーム２先端の位置情報を加算して得られるレーザ測域センサ１０１の位置情報を用いてスキャンマッチングを行う。スキャンマッチング計算部１１１は、推定したレーザ測域センサ１０１の位置情報を制御装置５０等に出力する。

次に、制御装置５０が、アーム２の伸展が完了したかどうかを判定する（ステップＳ１５）。例えば、制御装置５０は、スキャンマッチング計算部１１１から取得したレーザ測域センサ１０１の位置情報に基づいて、レーザ測域センサ１０１が測量対象位置（高さ）に到達したかどうかを判定する。到達した場合、制御装置５０はアーム２の伸展が完了したと判定する。到達していない場合、制御装置５０は、アーム２の伸展が完了していないと判定する。伸展が完了していない場合、ステップＳ１４の処理を繰り返す。伸展が完了した場合、制御装置５０はアーム２の伸展・旋回動作を停止し、その位置での測量（高所測量）を行う（ステップＳ１６）。つまり、レーザ測域センサ１０１は、所定の範囲を走査し、周辺に存在する障害物９との距離情報を計測する。

次に図７のステップＳ１１、ステップＳ１４のスキャンマッチング処理についてさらに詳しく説明する。
図８は、本発明に係る第一実施形態における測域センサの位置情報推定処理の一例を示す第二のフローチャートである。
図９は、本発明に係る第一実施形態におけるスキャンマッチングの説明を行う図である。
前提として、レーザ測域センサ１０１は、所定の時間間隔で走査を行い、その度に計測した点群情報をスキャンマッチング計算部１１１に出力している。また、センサ位置情報選択部１１０は、センサ位置座標変換部１０７又はセンサ位置地図座標変換部１０９から取得した位置情報等をスキャンマッチング計算部１１１に出力している。スキャンマッチング計算部１１１は、ある周期でスキャンマッチングを行うが、その間にレーザ測域センサ１０１、センサ位置情報選択部１１０から取得した点群情報を記憶部（図示せず）に記録する等して記憶する。

まず、スキャンマッチング計算部１１１が、記憶部より一つ前に計測した点群の情報を読み出して取得する（ステップＳ２０）。次にスキャンマッチング計算部１１１が、今回の点群の情報をレーザ測域センサ１０１から取得する（ステップＳ２１）。次にスキャンマッチング計算部１１１が、一つ前に計測した点群情報と今回計測した点群情報をＩＣＰアルゴリズムに設定する。次に、スキャンマッチング計算部１１１は、初期化処理を行う（ステップＳ２２）。初期化する変数は、例えば、繰り返し回数ｋの上限閾値２、２つの点群の距離εの判定に用いる閾値１、２つの点群情報に与える初期変位量（並進ベクトルｔ_０、回転行列Ｒ_０）である。並進ベクトルｔ_０や回転行列Ｒ_０の初期値は、ステップＳ１１においては、センサ位置座標変換部１０７が計算したレーザ測域センサ１０１のオドメトリによる位置情報、姿勢情報に基づく値である。また、ステップＳ１４においては、センサ位置地図座標変換部１０９が計算したレーザ測域センサ１０１の幾何学的手法による位置情報、姿勢情報に基づく値である。ＩＣＰアルゴリズムを用いたスキャンマッチングでは、適切な初期変位を与えないと局所解に陥るという問題がある。そのため、以降の処理の効率化、安定化のため、ステップＳ２２で与える初期変位が重要となる。例えば、ステップＳ１１の場合、スキャンマッチング計算部１１１は、一つ前のタイミングに計測した点群情報について、そのタイミングにおけるオドメトリによる位置情報を用いて初期変位を与える。同様に今回のタイミングに計測した点群情報について、今回のタイミングにおけるオドメトリによる位置情報を用いて初期変位を与える。

初期変位の具体例を示す。ステップＳ１１について、オドメトリによるレーザ測域センサ１０１の位置情報、姿勢情報を（ｘ_ｏｄ、ｙ_ｏｄ、θ_ｏｄ）とすると、初期変位量としてＲ_０、ｔ_０に以下の値を設定する。

また、ステップＳ１４について、幾何学的手法によるレーザ測域センサ１０１の位置情報、姿勢情報を（ｘ_ａｒｍ、ｚ_ａｒｍ、θ_ａｒｍ）とすると、初期変位量としてＲ_０、ｔ_０に以下の値を設定する。

この例の場合、２次元面内でのスキャンマッチングを例として挙げており、オドメトリによる計算結果を使う場合と、幾何学的手法による計算結果を使う場合では対象平面が異なる。具体的には、オドメトリによる計算結果は図２におけるｘｙ平面、幾何学的手法による計算結果を使う場合は図２におけるｘｚ平面である。したがって，上記のθｏｄは、Ｚ軸周りの角度（ヨー角）、θａｒｍはＹ軸周りの角度（ピッチ角）を意味する。なお、ＩＣＰアルゴリズムのインプットとなる点群も、ステップＳ１１の処理についてはｘｙ平面上の座標情報として与えられ、ステップＳ１４の処理についてはｘｚ平面上の座標情報として与えられる。

図９（ａ）は、ステップＳ２０〜ステップＳ２２の処理を行った後の点群の一例を示したものである。図９（ａ）の星印は、一つ前のタイミングで計測した点群である。丸印は、今回取計測した点群である。

次にスキャンマッチング計算部１１１は、点群間距離が最小となる対応関係を探索する（ステップＳ２３）。図９（ｂ）は、ステップＳ２３の処理を概念的に示したものである。スキャンマッチング計算部１１１は、一つ前のタイミングで計測した点群と今回計測した点群とを比較し、対応点（最も近い点）を探索する。

次にスキャンマッチング計算部１１１は、一つ前のタイミングで計測した点群と今回計測した点群に対し、対応点間距離が最も小さくなるよう、今回計測した点群に並進および回転のうち少なくとも一方の変換を加える（ステップＳ２４）。図９（ｃ）は、ステップＳ２４の処理を概念的に示したものである。

次にスキャンマッチング計算部１１１は、変換後の対応間距離εおよび繰り返し回数ｋを評価する。具体的には、スキャンマッチング計算部１１１は、対応間距離εが所定の閾値１以下か、または、繰り返し回数ｋが所定の閾値２を上回っているかどうかを評価する（ステップＳ２５）。対応間距離εの評価には、例えば、各対応点間距離の総和などの評価関数を用いてもよい。２つの点群が完全に一致すれば対応点間距離はすべて０になり、各対応点間距離の総和も０となる。このような場合、対応間距離εが所定の閾値１以下の条件を満たす。また、評価関数値が閾値１より大きい場合、スキャンマッチング計算部１１１は、対応点の組み合わせを変更して再度評価を行う。例えば、図９（ｄ）の例の場合、点ｄ０の対応点を点Ｄ１から点Ｄ２に変更して評価を行う。スキャンマッチング計算部１１１は、評価関数の値が閾値１以下となるまで，対応点を変えて繰り返し計算する。また、繰り返し回数ｋは、ステップＳ２３からの繰り返し回数である。このような評価を行った結果、何れの条件も満たさない場合、ステップＳ２３からの処理を繰り返す。つまり、並進量・回転量を更新する。また、ステップＳ２５において、どちらかの条件を満たす場合、スキャンマッチングを終了する。図９（ｅ）で例示するような状態となり、評価関数の値が閾値１以下となった場合、ステップＳ２４で計算した並進量・回転量が、２つの点群が計測される間でのレーザ測域センサ１０１の移動量である。スキャンマッチング計算部１１１は、この並進量・回転量からレーザ測域センサ１０１の位置情報、姿勢情報を推定する。

図８で説明したスキャンマッチングの方法によれば、オドメトリや幾何学的な手法によって計算したレーザ測域センサ１０１の位置情報、姿勢情報やを補助的に用いてスキャンマッチングを行うことで、スキャンマッチングの精度を高めることができる。

本実施形態によれば、屋内等のＧＰＳによる測位が不可能な環境下でも走行台車１の移動時、アーム２の伸展時におけるレーザ測域センサ１０１の位置を正確に推測し，正確な測量を行うことが可能となる。また、アーム２の伸展時に重力による撓みが生じてもレーザ測域センサ１０１の位置を正確に推定することができるため、アーム２の剛性をある程度妥協することが可能となり、装置全体の重量減、それによる燃費効率向上、部材コスト削減といった効果が得られる。また、測量のために必要なレーザ測域センサ１０１による計測結果を用いてレーザ測域センサ１０１の位置を推定するので、歪ゲージ等の追加センサが不要でありコスト増を抑制することができる。

＜第二実施形態＞
以下、本発明の第二実施形態による測量装置について図１０〜図１１を参照して説明する。
図１０は、本発明に係る第二実施形態における測量装置の一例を示すブロック図である。図１１は、本発明に係る第二実施形態における測量装置を搭載した走行台車の一例を示す図である。
本発明の第二実施形態に係る構成のうち、本発明の第一実施形態に係る測量装置１００を構成する機能部と同じものには同じ符号を付し、それぞれの説明を省略する。第二実施形態に係る測量装置１００Ａは、第一実施形態の構成に加えて、慣性計測装置１１２を備えている。
慣性計測装置１１２は、図１１に示すようにレーザ測域センサ１０１の近傍に設けられる慣性センサである。慣性計測装置１１２は、レーザ測域センサ１０１の姿勢情報を計測する。例えば、慣性計測装置１１２は、アーム２の作業状態におけるレーザ測域センサ１０１の地面と平行な面に対する角度を計測する。慣性計測装置１１２は、計測した姿勢情報（角度）をスキャンマッチング計算部１１１に出力する。
スキャンマッチング計算部１１１は、図７のステップＳ１４においてレーザ測域センサ１０１の姿勢情報θ_ａｒｍに慣性計測装置１１２の計測値を用いることができる。スキャンマッチング計算部１１１は、慣性計測装置１１２から取得した姿勢情報を用いてスキャンマッチングを行うことで、レーザ測域センサ１０１の位置情報、姿勢情報をさらに正確に推定することができる。例えば、走行台車１の足場の角度が急激に変化した場合など、レーザ測域センサ１０１の姿勢が大きく変化することがある。そのような場合に、レーザ測域センサ１０１の姿勢の変化を考慮せずにスキャンマッチングを行うと、対応点の探索が困難となり、レーザ測域センサ１０１の位置情報が精度よく推定できない可能性がある。本実施形態では、慣性計測装置１１２が計測した姿勢情報を用いて初期変位を与えることができ、スキャンマッチング処理を効率化、安定化することができる。

第二実施施形態によれば、第一実施形態の効果に加え、スキャンマッチング処理の精度向上・高速化が可能となる。また、レーザ測域センサ１０１の姿勢情報を直接計測し、その値を用いることで、環境の影響等に適切に対応し運用性を向上させることができる。

＜第三実施形態＞
以下、本発明の第三実施形態による測量装置について図１２を参照して説明する。
図１２は、本発明に係る第三実施形態における測量装置の一例を示すブロック図である。
本発明の第三実施形態に係る構成のうち、本発明の第二実施形態に係る測量装置１００Ａを構成する機能部と同じものには同じ符号を付し、それぞれの説明を省略する。第三実施形態に係る測量装置１００Ｂは、第二実施形態の構成に加えて、撓み補償量計算部１１３を備えている。
撓み補償量計算部１１３は、ストローク計測センサ１０４から取得したアーム２のストロークおよび仰角計測センサ１０５から取得した仰角に基づいて、アーム２の先端位置の撓み量を計算する。撓み補償量計算部１１３は、例えば、アーム２を片持ち梁とみなした物理モデルに基づいて撓み量を計算してもよい。あるいは、アーム２のストロークおよび仰角を変化させつつ、そのときの撓み量を予め実測し、アーム２のストロークおよび仰角に対応付けて撓み量を記録したルックアップテーブルを作成し、このルックアップテーブルに基づいて撓み量を計算してもよい。撓み補償量計算部１１３は、センサ位置幾何学計算処理部１０８が計算したレーザ測域センサ１０１の位置情報、姿勢情報を、計算した撓み量で補正する。センサ位置地図座標変換部１０９は、補正されたレーザ測域センサ１０１の位置情報、姿勢情報を取得し、以降の処理を行う。
スキャンマッチング計算部１１１は、図７のステップＳ１４において撓み量で補正された後のレーザ測域センサ１０１の位置情報、姿勢情報を用いてスキャンマッチングを行う。これにより、レーザ測域センサ１０１の位置情報、姿勢情報をさらに正確に推定することができる。

第三実施施形態によれば、第二実施形態の効果に加え、スキャンマッチング処理の精度向上・高速化・ロバスト化が可能となり、運用性を向上させることができる。
なお、第三実施形態の撓み補償量計算部１１３を、第一実施形態の構成に追加した構成としてもよい。

＜第四実施形態＞
以下、本発明の第四実施形態による測量装置について図１３〜図１７を参照して説明する。
図１３は、本発明に係る第四実施形態における測量装置の一例を示すブロック図である。
図１４は、本発明に係る第四実施形態における測量装置を搭載した走行台車の一例を示す図である。
本発明の第四実施形態に係る構成のうち、本発明の第三実施形態に係る測量装置１００Ｂを構成する機能部と同じものには同じ符号を付し、それぞれの説明を省略する。第四実施形態に係る測量装置１００Ｃは、第三実施形態の構成に加えて、走査平面方向変更部１１４を備えている。
第四実施形態に係るレーザ測域センサ１０１は、２次元のレーザスキャナである。また、図１４に示すようにアーム２の先端部には回転機構２１が設けられ、レーザ測域センサ１０１をアーム２の軸方向回りに回転させることで走査平面を変更することができる。
走査平面方向変更部１１４は、センサ位置情報選択部１１０に入力されたアーム２の動作状態（収納状態か作業状態か）に応じて、レーザ測域センサ１０１の走査平面を切り替えるよう制御装置５０に指示を行う。

図１５は、本発明に係る第四実施形態における走査平面を示す第一の図である。
図１５は、走行台車１が走行中の場合のレーザ測域センサ１０１の走査平面Ｐ１を示している。走行台車１が走行中の場合、つまり、センサ位置情報選択部１１０に入力されたアーム２の動作状態が収納状態の場合、走査平面方向変更部１１４は、レーザ測域センサ１０１の走査平面Ｐ１が地面と平行な面となるように制御する。

図１６は、本発明に係る第四実施形態における走査平面を示す第二の図である。
図１６は、走行台車１が停止し、アーム２を伸展させている場合のレーザ測域センサ１０１の走査平面Ｐ２を示している。センサ位置情報選択部１１０に入力されたアーム２の動作状態が作業状態の場合、走査平面方向変更部１１４は、レーザ測域センサ１０１の走査平面Ｐ２が地面と垂直な面となるように制御する。

次にレーザ測域センサ１０１の位置情報の推定処理の流れについて説明する。
図１７は、本発明に係る第四実施形態における測域センサの位置情報推定処理の一例を示すフローチャートである。
なお、図７と同様の処理については簡単に説明を行う。まず、走行台車１が移動を開始する（ステップＳ１０）。走行台車１が移動中、測量装置１００は、レーザ測域センサ１０１の位置情報を推定する（ステップＳ１１）。次に、制御装置５０が、走行台車１の移動が完了したかどうかを判定する（ステップＳ１２）。移動が完了していない場合、ステップＳ１１からの処理を繰り返す。移動が完了した場合、制御装置５０は移動が完了したことを示す信号をユーザへ通知する。このとき、ユーザは、アーム２の動作状態を収納状態から作業状態へ切り替えることを測量装置１００へ入力する。センサ位置情報選択部１１０は、ユーザの入力した情報を走査平面方向変更部１１４へ出力する。走査平面方向変更部１１４は、センサ位置情報選択部１１０に入力された収納状態から作業状態への切り替え指示情報に基づいてレーザ測域センサ１０１の走査平面を地面に水平な平面から垂直な平面に切り替えるよう制御装置５０に指示する。制御装置５０は、回転機構２１を制御し、レーザ測域センサ１０１の走査平面を回転させる（ステップＳ１２１）。次にユーザは、アーム２の伸展、旋回動作を指示する情報を制御装置５０へ入力する。制御装置５０は、アーム２の動作を制御し、アーム２は伸展動作・旋回動作を開始する（ステップＳ１３）。次に測量装置１００は、レーザ測域センサ１０１の位置情報を推定する（ステップＳ１４）。次に制御装置５０が、アーム２の伸展が完了したかどうかを判定する（ステップＳ１５）。伸展が完了した場合、制御装置５０はアーム２の伸展・旋回動作を停止し、その位置での測量（高所測量）を行う（ステップＳ１６）。

第四実施施形態によれば、第三実施形態の効果に加え、安価な２次元のレーザ測域センサを用いて走行台車１の移動時およびアーム２の伸展時におけるレーザ測域センサ１０１の位置推定が可能になる。
なお、第四実施形態は、第一実施形態、または、第二実施形態に適用してもよい。

＜第五実施形態＞
以下、本発明の第五実施形態による測量装置について図１８〜図１９を参照して説明する。
図１８は、本発明に係る第五実施形態における測量装置の一例を示すブロック図である。
本発明の第五実施形態に係る構成のうち、本発明の第一実施形態に係る測量装置１００を構成する機能部と同じものには同じ符号を付し、それぞれの説明を省略する。第五実施形態に係る測量装置１００Ｄは、第一実施形態の構成に加えて、アーム先端回転軸エンコーダ１１５、点群座標変換部１１６、点群メモリ部１１７を備えている。
第四実施形態と同様に第五実施形態に係るレーザ測域センサ１０１は、２次元のレーザスキャナである。また、図１４に示すようにアーム２の先端部には回転機構２１が設けられ、レーザ測域センサ１０１をアーム２の軸方向回りに回転させることで走査平面を変更することができる。また、制御装置５０は、回転機構２１を常時回転させながら走行台車１を移動させたり、アーム２を伸展させたりする。
アーム先端回転軸エンコーダ１１５は、回転機構２１の回転量（角度）を計測する。
点群座標変換部１１６は、レーザ測域センサ１０１から点群情報を取得する。また、点群座標変換部１１６は、アーム先端回転軸エンコーダ１１５から回転機構２１の回転量を取得する。点群座標変換部１１６は、取得した回転量の情報を用いて点群情報を３次元空間の座標情報に座標変換する。
点群メモリ部１１７は、点群座標変換部１１６が座標変換した点群情報を一定周期分蓄積し、それらをアセンブリして、疑似的な３次元のレーザスキャナで走査を行った場合の点群情報（３次元の点群）を生成し、３次元の点群情報をスキャンマッチング計算部１１１に出力する。
スキャンマッチング計算部１１１は、３次元点群情報と、センサ位置情報選択部１１０から取得したレーザ測域センサ１０１の位置情報、姿勢情報（オドメトリまたは幾何学的手法による位置情報、姿勢情報）とを用いてスキャンマッチングを行い、レーザ測域センサ１０１の３次元における位置情報（ＸＹＺ座標）、姿勢情報（ロール角、ピッチ角、ヨー角）を求める。

図１９は、本発明に係る第五実施形態における走査方法を示す図である。
図１９（ａ）は、走行台車１が走行中の場合のレーザ測域センサ１０１の走査方法を示している。走行台車１が走行中の場合、制御装置５０は、回転機構２１を回転させながら走行台車１を移動する。レーザ測域センサ１０１は、走査平面を３６０度変化させながら点群情報の計測を行う。点群座標変換部１１６は、レーザ測域センサ１０１が計測した２次元の点群情報に、アーム先端回転軸エンコーダ１１５が計測した回転量を加え、２次元の点群の座標情報を３次元空間内での座標情報に変換する。点群メモリ部１１７は、例えば、レーザ測域センサ１０１の走査平面が３６０度変化する度に、その間蓄積した点群情報をアセンブリして３次元のレーザスキャナが計測した点群情報と同様の点群情報の集合を生成する。スキャンマッチング計算部１１１は、点群メモリ部１１７から３次元の点群情報を取得し、３次元点群情報とオドメトリによる位置情報を用いてスキャンマッチングを行う。

図１９（ｂ）は、アーム２の伸展時におけるレーザ測域センサ１０１の走査方法を示している。この場合も制御装置５０は、回転機構２１を回転させながらアーム２を伸展動作させたり、旋回動作させたりする。走行時と同様に点群メモリ部１１７は、３次元の点群情報を生成し、スキャンマッチング計算部１１１は、点群メモリ部１１７から３次元の点群情報を取得し、３次元点群情報と幾何学的手法による位置情報を用いてスキャンマッチングを行う。

本実施形態によれば、安価な２次元レーザ測域センサを用いて，走行台車１の移動時およびアーム２の伸展時におけるレーザ測域センサ１０１の位置推定が可能となる。これにより、測量装置１００Ｄの製造コストを抑えることができる。また、２次元の点群情報をアセンブリして３次元点群を疑似的に生成することにより、３次元空間でのスキャンマッチング可能となる。これにより、レーザ測域センサ１０１の３次元空間での位置情報、姿勢情報を推定することができ、より精密な測量が可能となる。
なお、第五実施形態は、第二実施形態、第三実施形態の構成に適用することも可能である。

なお上述の測量装置１００、１００Ａ、１００Ｂ、１００Ｃ、１００Ｄは内部に、コンピュータシステムを有している。そして、上述した測量装置１００等における各処理の過程は、プログラムの形式でコンピュータ読み取り可能な記録媒質に記憶されており、このプログラムをコンピュータが読み出して実行することによって、上記処理が行われる。ここでコンピュータ読み取り可能な記録媒質とは、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、半導体メモリ等をいう。また、このコンピュータプログラムを通信回線によってコンピュータに配信し、この配信を受けたコンピュータが当該プログラムを実行するようにしてもよい。

また、上記プログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。

その他、本発明の趣旨を逸脱しない範囲で、上記した実施の形態における構成要素を周知の構成要素に置き換えることは適宜可能である。また、この発明の技術範囲は上記の実施形態に限られるものではなく、本発明の趣旨を逸脱しない範囲において種々の変更を加えることが可能である。走行台車１は移動体の一例である。

１・・・走行台車
２・・・アーム
１００、１００Ａ、１００Ｂ、１００Ｃ、１００Ｄ・・・測量装置
１０１・・・レーザ測域センサ
１０２・・・ホイール回転数計測センサ
１０３・・・操舵角計測センサ
１０４・・・ストローク計測センサ
１０５・・・仰角計測センサ
１０６・・・オドメトリ計算処理部
１０７・・・センサ位置座標変換部
１０８・・・センサ位置幾何学計算処理部
１０９・・・センサ位置地図座標変換部
１１０・・・センサ位置情報選択部
１１１・・・スキャンマッチング計算部
１１２・・・慣性計測装置
１１３・・・撓み補償量計算部
１１４・・・走査平面方向変更部
１１５・・・アーム先端回転軸エンコーダ
１１６・・・点群座標変換部
１１７・・・点群メモリ部

Claims (12)

1. 地面と垂直な方向に旋回可能で伸縮機構を有するアームに設けられ、対象物との距離を計測する測域センサと、
   前記アームの長さおよび旋回角に基づいて、前記測域センサの位置情報を計算するセンサ位置幾何学計算処理部と、
   前記センサ位置幾何学計算処理部が計算した測域センサの位置情報と、前記測域センサが走査して得た距離情報が示す対象物の位置に対応する点群情報と、に基づいてスキャンマッチングを行い、前記測域センサの位置情報を計算するスキャンマッチング計算部と、
   を備える測量装置。
2. 前記測域センサは、所定の時間間隔で走査を行って点群情報を計測し、
   前記スキャンマッチング計算部は、前記測域センサが所定のタイミングで計測した点群情報と、１つ前のタイミングで計測した点群情報とを比較してスキャンマッチングを行うにあたり、それぞれのタイミングにおいて前記センサ位置幾何学計算処理部が計算した測域センサの位置情報を用いて前記測域センサが計測した点群情報のそれぞれを変位させ、スキャンマッチングを行う、
   請求項１に記載の測量装置。
3. 前記測域センサの姿勢情報を計測する慣性センサ、をさらに備え、
   前記スキャンマッチング計算部は、前記測域センサの位置情報に加えて、前記測域センサの姿勢情報を用いて、スキャンマッチングを行う、
   請求項１または請求項２に記載の測量装置。
4. 前記アームの長さおよび旋回角に応じた当該アームの撓み量を計算する撓み補償量計算部、をさらに備え、
   前記スキャンマッチング計算部は、前記撓み補償量計算部が計算した撓み量による補正後の前記測域センサの位置情報を用いて、スキャンマッチングを行う、
   請求項１から請求項３の何れか１項に記載の測量装置。
5. 前記アームを有する移動体に備えられたセンサの計測値に基づいて当該移動体の位置情報を計算するオドメトリ計算処理部、
   をさらに備え、
   前記スキャンマッチング計算部は、前記オドメトリ計算処理部が計算した前記移動体の位置情報に基づく前記測域センサの位置情報を用いてスキャンマッチングを行う、
   請求項１から請求項４の何れか１項に記載の測量装置。
6. 前記スキャンマッチング計算部は、
   前記アームが伸展動作および旋回動作が行われていない収納状態にある場合、前記オドメトリ計算処理部が計算した前記移動体の位置情報に基づく前記測域センサの位置情報のみを用いてスキャンマッチングを行い、
   前記アームが伸展動作および旋回動作のうち少なくとも一方が行われている作業状態にある場合、前記オドメトリ計算処理部が計算した前記測域センサの位置情報に加えて、前記センサ位置幾何学計算処理部が計算した測域センサの位置情報を用いてスキャンマッチングを行う、
   請求項５に記載の測量装置。
7. 前記測域センサは、前記アームの軸方向回りに回転可能に設けられ、２次元平面を走査する測域センサであって、
   前記アームが伸展動作および旋回動作のうち少なくとも一方が行われている作業状態にある場合、前記測域センサが走査する２次元平面が地面と垂直となるよう制御する走査平面方向変更部、
   をさらに備える請求項１から請求項６の何れか１項に記載の測量装置。
8. 前記走査平面方向変更部は、前記アームが伸展動作および旋回動作が行われていない収納状態にある場合、前記測域センサの走査平面が地面と平行となるよう制御する、
   請求項７に記載の測量装置。
9. 前記測域センサは、前記アームの軸方向回りに回転可能に設けられ、２次元平面を走査する測域センサであって、
   前記測域センサの回転量を取得し、前記測域センサが走査して得た点群情報を３次元座標に変換する点群座標変換部と、
   前記点群座標変換部が変換した点群情報を蓄積し、蓄積した点群情報の集合を用いて３次元の点群情報を生成する点群メモリ部と、
   をさらに備え、
   前記スキャンマッチング計算部は、前記点群メモリ部が生成した３次元の点群情報を用いてスキャンマッチングを行う、
   請求項１から請求項６の何れか１項に記載の測量装置。
10. 請求項１から請求項９の何れか１項に記載の測量装置と、
    地面と垂直な方向に旋回可能で伸縮機構を有するアームであって、前記測量装置が備える測域センサが設けられたアームと、
    を備える移動体。
11. 地面と垂直な方向に旋回可能で伸縮機構を有するアームに設けられ、対象物との距離を計測する測域センサを備える測量装置が、
    前記アームの長さおよび旋回角に基づいて、前記測域センサの位置情報を計算し、
    前記計算した測域センサの位置情報と、前記測域センサが走査して得た距離情報が示す対象物の位置に対応する点群情報と、に基づいてスキャンマッチングを行い、前記測域センサの位置情報を計算する、
    測量方法。
12. 地面と垂直な方向に旋回可能で伸縮機構を有するアームに設けられ、対象物との距離を計測する測域センサを備える測量装置のコンピュータを、
    前記アームの長さおよび旋回角に基づいて、前記測域センサの位置情報を計算する手段、
    前記計算した測域センサの位置情報と、前記測域センサが走査して得た距離情報が示す対象物の位置に対応する点群情報と、に基づいてスキャンマッチングを行い、前記測域センサの位置情報を計算する手段、
    として機能させるためのプログラム。

Images