JP2013117392A - 測位システム、測位方法、及びプログラム - Google Patents

Abstract

【課題】トータルステーションと比して簡易な装置を用いて、リアルタイムで移動体の位置及び向きを計測できる測位システムを提供する。
【解決手段】杭打ち機１の位置及び向きを計測する測位システム１０では、杭打ち機１に設定された所定の基準点に対して既定の位置関係をもって、且つ互いに既定の位置関係をもって杭打ち機１に設置された複数のターゲット３０Ａと、各ターゲット３０Ａの表面を光で走査することにより各ターゲット３０Ａの表面上における複数の計測点の位置情報を取得するＬＲＦ２０とが備えられ、ＬＲＦ２０により取得された各ターゲット３０Ａの表面上における複数の計測点の位置情報に基づいて、各ターゲット３０Ａの中心位置が推定され、推定された複数のターゲット３０の中心位置と、前記既定の位置関係とに基づいて、杭打ち機１の位置及び向きが推定される。
【選択図】図１

Description

本発明は、移動体の位置及び向きを計測する測位システム、測位方法、及びプログラムに関する。

杭の打設施工をリアルタイムで打設位置を計測しながら行うためのシステムとして、追尾機能を有するトータルステーションとＧＰＳとを使用したものが知られている（例えば、特許文献１参照）。特許文献１に記載のシステムでは、洋上クレーンで吊られた杭に複数の反射体を設置し、洋上に浮かべた複数のトータルステーションで視準して杭の中心位置を算出し、トータルステーションの位置をＧＰＳで計測してその計測結果に基づいて杭の中心位置を補正する。

また、レーザ光を走査することにより、被計測体上の複数の計測点までの距離を計測するレーザレンジファインダ（光走査式測距装置）が知られている（例えば、特許文献２参照）。

特開２００８―１４８７７号公報 特開２００７−２２５４３４号公報

特許文献１に記載のシステムでは、トータルステーションの計測精度は杭の打設位置の要求精度に対して十分に高いものの、ＧＰＳの計測精度は十分ではないことから、杭の打設位置の要求精度を満足できない可能性がある。また、トータルステーションやＧＰＳは高額であることから、高コストなシステムになる。

また、上述のレーザレンジファインダは、被計測体の表面の複数の計測点までの距離を計測したり、被計測体のプロファイルデータを取得したりといった用途はあるものの、円柱状の杭の中心軸等の被計測体の輪郭に対する特徴点の位置を計測するという用途では使用されていない。

本発明は、上記事情に鑑み、トータルステーションやＧＰＳと比して簡易な計測装置を用いて、リアルタイムで移動体の位置及び向きを計測できる測位システム、測位方法、及びプログラムを提供するものである。

上記課題を解決するために、本発明に係る測位システムは、移動体の位置及び向きを計測する測位システムであって、前記移動体に設定された所定の基準点に対して既定の位置関係をもって、且つ互いに既定の位置関係をもって前記移動体に設置された複数の被計測体と、各被計測体の表面を光で走査することにより各被計測体の表面上における複数の計測点の位置情報を取得する光走査式測距装置と、前記光走査式測距装置により取得された各被計測体の表面上における複数の計測点の位置情報に基づいて、各被計測体の輪郭に対して所定の位置関係にある所定点の位置を推定する第１測位部と、前記第１測位部により推定された前記複数の被計測体の前記所定点の位置と、前記複数の被計測体と前記所定の基準点との既定の位置関係と、前記複数の被計測体の互いの既定の位置関係とに基づいて、前記基準点の位置と、前記移動体の前記基準点を中心とする回転方向の位置とを算出し、前記基準点の位置に基づいて前記移動体の位置を推定し、前記回転方向の位置に基づいて前記移動体の向きを推定する第２測位部と、を備える。

前記測位システムにおいて、４個以上の前記被計測体が前記移動体に設置され、少なくとも１個の前記被計測体が必ず前記光走査式測距装置の死角領域に位置するように設置されてもよく、前記第２測位部は、前記第１測位部により推定された２個の前記被計測体の前記所定点の位置と、前記２個の被計測体と前記所定の基準点との既定の位置関係と、前記２個の被計測体の互いの既定の位置関係とに基づいて、前記基準点の位置と、前記移動体の前記基準点を中心とする回転方向の位置とを算出してもよい。

前記測位システムにおいて、３個以上の前記被計測体が前記移動体に設置され、全ての前記被計測体が常に前記光走査式測距装置の死角領域に位置することがないように設置されてもよく、前記第２測位部は、前記第１測位部により推定された３個の前記被計測体の前記所定点の位置と、前記３個の被計測体と前記所定の基準点との既定の位置関係と、前記３個の被計測体の互いの既定の位置関係とに基づいて、前記基準点の位置と、前記移動体の前記基準点を中心とする回転方向の位置とを算出してもよい。

前記測位システムにおいて、前記複数の被計測体は、相互の距離が全て異なるように前記移動体に設置されてもよい。

前記測位システムにおいて、前記被計測体は円柱体で、前記所定点は、前記円柱体の中心軸上の点であってもよく、前記第１測位部は、最小二乗法又は最尤法を用いて、前記複数の計測点の位置を近似した円の方程式を求め、求めた円の方程式に基づいて、前記中心軸上の点の位置を推定してもよい。

また、本発明に係る測位システムは、移動体の位置及び向きを計測する測位方法であって、複数の被計測体を、前記移動体に設定した所定の基準点に対して既定の位置関係を有し、且つ互いに既定の位置関係を有するように前記移動体に設置するステップと、光走査式測距装置により各被計測体の表面を光で走査することで各被計測体の表面上における複数の計測点の位置情報を取得するステップと、前記光走査式測距装置により取得した各被計測体の表面上における複数の計測点の位置情報に基づいて、各被計測体の輪郭に対して所定の位置関係にある所定点の位置を推定するステップと、推定された前記複数の被計測体の前記所定点の位置と、前記複数の被計測体と前記所定の基準点との既定の位置関係と、前記複数の被計測体の互いの既定の位置関係とに基づいて、前記基準点の位置と、前記移動体の前記基準点を中心とする回転方向の位置とを算出し、前記基準点の位置に基づいて前記移動体の位置を推定し、前記回転方向の位置に基づいて前記移動体の向きを推定するステップと、を備える。

また、本発明に係るプログラムは、移動体に設定された所定の基準点に対して既定の位置関係をもって、且つ互いに既定の位置関係をもって前記移動体に設置された複数の被計測体と、各被計測体の表面を光で走査することにより各被計測体の表面上における複数の計測点の位置情報を取得する光走査式測距装置とを備え、前記移動体の位置及び向きを計測する測位システムに含まれるコンピュータに実行させるプログラムであって、前記光走査式測距装置により取得された各被計測体の表面上における複数の計測点の位置情報に基づいて、各被計測体の輪郭に対して所定の位置関係にある所定点の位置を推定する手順と、推定された前記複数の被計測体の前記所定点の位置と、前記複数の被計測体と前記所定の基準点との既定の位置関係と、前記複数の被計測体の互いの既定の位置関係とに基づいて、前記基準点の位置と、前記移動体の前記基準点を中心とする回転方向の位置とを算出し、前記基準点の位置に基づいて前記移動体の位置を推定し、前記回転方向の位置に基づいて前記移動体の向きを推定する手順と、を前記コンピュータに実行させる。

本発明によれば、トータルステーションやＧＰＳと比して簡易な計測装置を用いて、リアルタイムで移動体の位置及び向きを計測できる。

一実施形態に係る測位システムの構成を示す斜視図である。 測位システムの機能ブロック図である。 各ターゲットの中心位置を算出する処理を説明するためのフローチャートである。 （Ａ）は、カルマンフィルタによるノイズ除去を行っていない場合における円周面の片側のプロファイルの検出結果を示すグラフである。（Ｂ）は、カルマンフィルタによるノイズ除去を行った場合における円周面の片側のプロファイルの検出結果を示すグラフである。 一定距離法の原理を示す図である。 最小二乗法と最尤法とを用いて、クラスタの全データから円周面の中心位置を推定する方法の原理を示す図である。 ＬＲＦにより取得されたデータから、ターゲットの中心位置を推定した結果を示すグラフである。 ＬＲＦによりレーザ光を１０回走査して取得すると共にノイズ成分を除去したデータから、ターゲットの中心位置を推定した結果を示すグラフである。 杭打ち機の位置及び向きを計測する原理を説明するための図である。 杭打ち機の位置及び向きを計測する原理を説明するための図である。 杭打ち機の位置及び向きを計測する原理を説明するための図である。 杭打ち機の位置及び向きを計測する原理を説明するための図である。 杭打ち機の位置及び向きを計測する処理を説明するためのフローチャートである。 Ｘ−Ｙ座標を極座標に変換する方法を説明するための図である。 他の実施形態に係る測位システムの構成を示す図である。 杭打ち機の位置及び向きを計測する処理を説明するためのフローチャートである。 （Ａ）は、他の実施形態に係る測位システムを示す側面図であり、（Ｂ）は、（Ａ）のＢ−Ｂ矢視図である。 レーザポインタを用いて、鉛直軸の位置の初期設定や校正を行っている状態を示す斜視図である。 （Ａ）は、回転台が停止した状態でＬＲＦがレーザ光を１回走査した場合におけるターゲットの円周面上の計測点を示す図である。また、（Ｂ）は、本実施形態に係る高精度計測を実施した場合におけるターゲットの円周面上の計測点を示す図である。 （Ａ）は、回転台を停止させた状態でＬＲＦにレーザ光を１０回走査させることにより取得した円周面上の計測点のデータから、ターゲットの中心位置を推定した結果を示すグラフである。また、（Ｂ）は、回転台を回転させながらＬＲＦにレーザ光を１０回走査させることにより取得した円周面上の計測点のデータから、ターゲットの中心位置を推定した結果を示すグラフである。

以下、本発明の一実施形態を、図面を参照しながら説明する。図１は、一実施形態に係る測位システム１０の構成を示す図である。この図に示すように、測位システム１０は、杭打ち機１の位置及び向き（鉛直軸周りの回転方向の位置）を計測するためのシステムであり、レーザレンジファインダ（以下、ＬＲＦという）２０と、杭打ち機１に設置された４個のターゲット３０とを備えている。

ＬＲＦ２０は、レーザ距離計を鉛直軸２０Ａの周りに回転させることによりレーザ光を水平方向に走査して、上記鉛直軸２０Ａから被計測体の輪郭上の複数の計測点までの距離を計測する水平ラインスキャンタイプの１軸の光走査式測距装置である。ここで、レーザ光の走査とは、レーザ光の射出角度を順次変化させることにより、レーザ光で受光面を所定方向に順次なぞることである。ＬＲＦ２０は、所定角度（例えば、０．２５°）回転される毎に光源からレーザ光を射出し、反射されたレーザ光を受光部が受光して、鉛直軸２０Ａから被計測体の輪郭上の計測点までの距離を計測する。

３個のターゲット３０は、杭打ち機１の機体上に設置されている。この３個のターゲット３０は、半径ｒの円柱体であり、水準器３６が取り付けられた軸体３４の一端部に支持されている。ターゲット３０と軸体３４とは、それぞれの中心軸３０Ｅ、３４Ａが同一直線上で並ぶように配されている。水準器３６は、軸体３４の姿勢が鉛直か否かを確認するための機器である。また、残りの１個のターゲット３０は、半径ｒの円柱体であり、杭と同軸状に配置されている。

また、ターゲット３０の円周面３０Ｆは、レーザ光の乱反射を抑制するべく低輝度仕上げされている。これにより、鉛直軸２０Ａから円周面３０Ｆ上の計測点までの距離を計測する際のノイズを低減できる。また、円周面３０Ｆの色は、レーザ光の反射率を考慮して、黒等の暗色ではなく、白等の明色である。

図２は、測位システム１０の機能ブロック図である。この図に示すように、測位システム１０は、ＬＲＦ２０に無線ＬＡＮや通信ケーブル等により接続された計算処理用のコンピュータ１２と、杭打ち機１の操縦室に設置され、杭打ち機１の位置及び向き並びに杭の打設位置Ｐが表示されるモニタ１８とを備えている。このコンピュータ１２は、計算処理用のプログラムを記憶した不揮発性メモリ等の記憶部１４と、当該プログラムを揮発性メモリ等に読み出して計算処理を実行する情報処理部（ＣＰＵ）４０と、ＬＲＦ２０からの距離情報を入力する入力部（入力インターフェース）１６とを備える。

情報処理部４０は、各ターゲット３０の中心位置を算出する第１測位部４１と、複数のターゲット３０の中心位置に基づいて杭打ち機１の位置及び向き（鉛直軸周りの回転方向の位置）を算出する第２測位部４０１と、杭打ち機１の位置及び向きを、モニタ１８に表示させる表示制御部４０２を備えている。上記プログラムは、情報処理部４０の各部の機能を実現させる。第１測位部４１は、分別部４２と、クラスタリング部４４と、ノイズ除去部４６と、推定部４８と、判定部４９とを備える。

図３は、各ターゲット３０の中心位置を算出する処理を説明するためのフローチャートである。このフローチャートに示すように、入力部１６が、ＬＲＦ２０から距離情報を取得すると（ステップ１）、分別部４２が、ＬＲＦ２０から距離情報が送信された計測点のうち、前景領域としての円周面３０Ｆ上の計測点と、背景領域としての円周面３０Ｆの周辺領域における計測点とを背景差分法により分別する（ステップ２）。

また、クラスタリング部４４は、分別部４２により前景領域に属すると判断された計測点に対して、最短距離法を用いてユーグリッド距離に基づいたクラスタリングを行う（ステップ３）。

また、ノイズ除去部４６は、カルマンフィルタを用いて、クラスタリング部４４によりクラスタリングされた多数の計測点のデータに含まれるノイズ成分を除去する（ステップ４）。なお、当該ノイズ成分の除去方法の詳細については後述する。

また、推定部４８は、一定距離法や最小二条法や最尤法等の推定手法を用いて、クラスタリング部４４によりクラスタリングされノイズ除去部４６によりノイズ成分を除去された多数の計測点から、ターゲット３０の中心位置を推定する（ステップ５）。なお、推定部４８による中心位置の推定は、予め入力されたターゲット３０の円周面３０Ｆのプロファイルデータに基づいて最小二乗法等の推定手法を用いて行われるものであり、ニュートン法による繰り返し計算によって収束値を得るものである。なお、推定部４８による中心位置の推定については、詳細に後述する。

また、判定部４９は、推定部４８がニュートン法による繰り返し計算によって算出した値が、収束値未満になっているか否かを判定し、収束値未満であれば中心点を確定し、収束値以上であれば再度計算を繰り返す（ステップ６、７）。

図４（Ａ）は、カルマンフィルタによるノイズ除去を行っていない場合における円周面３０Ｆの片側のプロファイルの検出結果を示すグラフである。また、図４（Ｂ）は、カルマンフィルタによるノイズ除去を行った場合における円周面３０Ｆのプロファイルの片側の検出結果を示すグラフである。

ここで、円周面３０Ｆの片側の円弧部の両端で反射された鏡面成分の光は、ＬＲＦ２０まで戻らず、当該円弧部の両端で反射された乱反射成分の光のみが、ＬＲＦ２０に戻る。このため、ＬＲＦ２０が検出する反射光の信号雑音比（Ｓ／Ｎ比）が高くなる。従って、カルマンフィルタによるノイズ除去を行っていない場合には、円周面３０Ｆ上の多数の計測点までの距離の検出精度が悪化し、図４（Ａ）に示すように、円周面３０Ｆの円弧部の両端の形状が崩れる。

そこで、反射光強度フィルタをカルマンフィルタとして用い、反射光強度が所定値より低いデータをノイズ成分として除去した。これにより、円周面３０Ｆ上の多数の計測点までの距離の計測精度を向上でき、図４（Ｂ）に示すように、円周面３０Ｆの円弧部の両端の形状の崩れを抑制でき、より正確な円周面３０Ｆのプロファイルデータを得ることができる。

次に、推定部４８によるターゲット３０の中心位置の推定方法の詳細について説明する。
まず、一定距離法について説明する。図５は、一定距離法の原理を示す図である。このグラフに示すように、一定距離法では、クラスタリング部４４によりクラスタリングされた多数の計測点（図中黒丸で示す）の中心（クラスタ中心）の位置（ｘ_ｃｌ，ｙ_ｃｌ）を求める。そして、鉛直軸２０Ａとクラスタ中心とを結ぶ直線Ｌに沿ってクラスタ中心から鉛直軸２０Ａの反対側へ距離ｄだけずらした位置を、円周面３０Ｆの中心点（ｘ_ｏｂｊ，ｙ_ｏｂｊ）とみなす。

ここで、ＬＲＦ２０の計測精度を理由として、ＬＲＦ２０の計測データに基づいて導出されるクラスタ中心点（ｘ_ｃｌ，ｙ_ｃｌ）は、円周面３０Ｆの中心と一致しない。そこで、円周面３０Ｆの半径ｒに基づいて上述のパラメータｄを定め、鉛直軸２０Ａとクラスタ中心とを結ぶ直線Ｌに沿ってクラスタ中心から鉛直軸２０Ａの反対側へ距離ｄだけずらした位置を、円周面３０Ｆの中心点（ｘ_ｏｂｊ，ｙ_ｏｂｊ）とみなしている。

円周面３０Ｆの中心点（ｘ_ｏｂｊ，ｙ_ｏｂｊ）は、下記（式１）で表される。なお、βは、鉛直軸２０Ａとクラスタ中心とを結ぶ直線ＬとＸ軸（但しｘ≧０）との角度である。

ここで、一定距離法は、既知である被計測体の輪郭のプロファイルに応じてパラメータｄを設定して被計測体の輪郭に対して所定の位置関係にある所定点（以下、特徴点と称する）を推定するという手法であり、様々な形状の被計測物の特徴点を推定できるという利便性を有する。しかし、一定距離法は、クラスタ中心のデータとパラメータｄとに基づき、被計測体の特徴点のおおよその位置を推定するという手法である。このため、一定距離法は、推定の精度の面では、クラスタの全てのデータを利用して被計測体の特徴点を推定する、最小二乗法や最尤法等の手法と比して劣る。

そこで、図６に示すように、最小二乗法と最尤法とを用いて、クラスタの全データ（ｘ_ａ，ｙ_ａ），α＝１，…，Ｎから円周面３０Ｆの中心位置を推定する。ここで、円周面３０Ｆの計測領域である円弧部のプロファイルを数式化すると、下記（式２）で表される円の方程式になる。このため、クラスタの全データ（ｘ_ａ，ｙ_ａ），α＝１，…，Ｎから円の方程式のパラメータ（ａ，ｂ，ｒ）を推定する。なお、ａ，ｂは、それぞれターゲット３０の中心点の２次元平面におけるＸ座標、Ｙ座標であり、ｒは、ターゲット３０の半径である。

まず、最小二乗法について説明する。
円の最小二乗法は、下記（式３）で表される誤差の二乗和Ｊ_ＬＳを最小化するパラメータａ，ｂ，ｒを推定する手法である。

ここで、本実施形態では、ターゲット３０の半径ｒが既知であるため、推定するパラメータはａ，ｂの２つとなり、上記（式３）は下記（式４）で表される非線形方程式となる。

そこで、非線形方程式の反復解法としてニュートン・ラフソン法を用いて、パラメータａ、ｂを推定する。その際、ニュートン・ラフソン法の初期値ｕ_０の設定が重要となるが、本実施形態では、上記（式１）で表される一定距離法の推定値（ｘ_ｏｂｊ，ｙ_ｏｂｊ）をニュートン・ラフソン法の初期値ｕ_０に設定する。

ニュートン・ラフソン法の手順として、まず、初期値ｕ_０を代入した関数Ｊ_ＬＳ（ｕ_０）を２次近似した下に凸の曲面の最小値を与える点ｕ_１＝（ａ_１，ｂ_１）を求める。次のステップでは、この点ｕ_１を代入した関数Ｊ_ＬＳ（ｕ_１）を２次近似した曲面の最小値ｕ_２を求める。そして、最終的にこれをｕ_iが収束するまで繰り返す。ｕ_iが収束すれば、そのｕ_iがＪ_ＬＳ（ｕ）の最小値を与える点である。

ｕ_i＝（ａ_i，ｂ_i）を代入した関数Ｊ_ＬＳ（ａ_i，ｂ_i）を２次近似した下に凸の曲面の最小値ｕ_i＋１＝（ａ_i＋１，ｂ_i＋１）は下記（式５）で表される。

ここで上記（式５）の右辺の１次微分を含む勾配ｇ_ＬＳ，iは、下記（式６）（式７）で表される。

_{また、上記（式５）の右辺の２次微分を含むヘッセ行列ＨＬＳ，iは、下記（式８）（式９）（式１０）で表される。}

即ち、ニュートン・ラフソン法を用いた半径ｒが既知の円の最小二乗法の手順では、最初のステップにおいて、初期値ｕ_０＝（ａ_０，ｂ_０）を上記（式５）〜（式１０）に代入し、ヘッセ行列の初期値Ｈ_ＬＳ，０と勾配の初期値ｇ_ＬＳ，０とを算出し、関数Ｊ_ＬＳ（ｕ_０）を２次近似した下に凸の曲面の最小値を与える点ｕ_１＝（ａ_１，ｂ_１）を算出する。そして、次のステップでは、ｕ_１＝（ａ_１，ｂ_１）を上記（式５）〜（式１０）に代入し、ヘッセ行列Ｈ_ＬＳ，１と勾配ｇ_ＬＳ，１とを算出し、関数Ｊ_ＬＳ（ｕ_１）を２次近似した下に凸の曲面の最小値を与える点ｕ_２＝（ａ_２，ｂ_２）を算出する。そして、このステップを、最終的にｕ_iが収束するまで繰り返す。

次に、最尤法について説明する。
円の最尤法は、下記（式１１）で表されるＪ_ＭＬを最小化するパラメータａ，ｂ，ｒを推定する手法である。

ここで、本実施形態では、ターゲット３０の半径ｒが既知であるため、推定するパラメータはａ，ｂの２つとなり、上記（式１１）は下記（式１２）で表される非線形方程式となる。

そこで、最小二乗法の場合と同様、非線形方程式の反復解法としてニュートン・ラフソン法を用いて、パラメータａ、ｂを推定する。その際、上記（式１）で表される一定距離法の推定値（ｘ_ｏｂｊ，ｙ_ｏｂｊ）をニュートン・ラフソン法の初期値ｕ_０に設定する。

ニュートン・ラフソン法の手順として、まず、初期値ｕ_０を代入した関数Ｊ_ＭＬ（ｕ_０）を２次近似した下の凸の曲面の最小値を与える点ｕ_１＝（ａ_１，ｂ_１）を求める。次のステップでは、この点ｕ_１を代入した関数Ｊ_ＭＬ（ｕ_１）を２次近似した曲面の最小値ｕ_２を求める。そして、最終的にこれをｕ_iが収束するまで繰り返す。ｕ_iが収束すれば、そのｕ_iがＪ_ＬＳ（ｕ）の最小値を与える点である。

ここで、上記（式１２）は、分子が２乗和であり、分母も（ｘ_α−ａ）^２＋（ｙ_α−ｂ）^２であるため、下に凸の関数である。従って、ｕ_i＝（ａ_i，ｂ_i）を代入した関数Ｊ_ＭＬ（ａ_i，ｂ_i）を２次近似した曲面の最小値ｕ_i＋１＝（ａ_i＋１，ｂ_i＋１）は下記（式１３）で表される。

ここで上記（式１３）の勾配ｇ_ＭＬ，iは、下記（式１４）（式１５）で表される。

但し、Ａ（ａ_i，ｂ_i）は下記（式１６）で表される。

また、上記（式１３）のヘッセ行列Ｈ_ＭＬ，iは、下記（式１７）（式１８）（式１９）で表される。

即ち、ニュートン・ラフソン法を用いた半径ｒが既知の円の最尤法の手順では、最初のステップにおいて、初期値ｕ_０＝（ａ_０，ｂ_０）を上記（式１３）〜（式１９）に代入し、ヘッセ行列の初期値Ｈ_ＬＳ，０と勾配の初期値ｇ_ＬＳ，０とを算出し、関数Ｊ_ＬＳ（ｕ_０）を２次近似した下に凸の曲面の最小値を与える点ｕ_１＝（ａ_１，ｂ_１）を算出する。そして、次のステップでは、ｕ_１＝（ａ_１，ｂ_１）を上記（式１３）〜（式１９）に代入し、ヘッセ行列Ｈ_ＭＬ，１と勾配ｇ_ＭＬ，１とを算出し、関数Ｊ_ＬＳ（ｕ_１）を２次近似した下に凸の曲面の最小値を与える点ｕ_２＝（ａ_２，ｂ_２）を算出する。そして、このステップを、最終的にｕ_iが収束するまで繰り返す。

図７は、上述の３つの解析方法を用いて、ＬＲＦ２０により取得されたデータから、ターゲット３０の中心位置を推定した結果を示すグラフである。このグラフにおいて、一定距離法を用いた推定結果は実線で示し、最小二乗法を用いた推定結果は破線で示し、最尤法を用いた推定結果は鎖線で示している。また、横軸は鉛直軸２０Ａからターゲット３０の中心軸３０Ｅまでの距離（ｍｍ）であり、縦軸は推定された中心位置の誤差（ｍｍ）である。

本実験では、屋外でＬＲＦ２０を用いて半径ｒ＝２５０ｍｍのターゲット３０の円周面３０Ｆに対してレーザ光を１回走査することにより、円周面３０Ｆ上の計測点のデータ（ｘ_ａ，ｙ_ａ），α＝１，…，Ｎを取得し、取得した全データからターゲット３０の中心位置を推定した。また、本実験では、赤外光に対しても反射率が高い白色の画用紙を円周面３０Ｆに貼り付けた。また、本実験では、ノイズ除去部４６のカルマンフィルタをＯＦＦにしており、多数の計測点のデータは、反射光強度が所定値より小さいノイズ成分を含んでいる。

図７のグラフに示すように、最小二乗法と最尤法とを用いてターゲット３０の中心位置を推定した結果が、一定距離法を用いてターゲット３０の中心位置を推定した結果と比して良好であった。また、ターゲット３０の中心位置が鉛直軸２０Ａから３０ｍ先に位置する場合において、最小二乗法による推定値の誤差が約３．５ｃｍ、最尤法による推定値の誤差が約２．５ｃｍであった。

ここで、本実施形態では、ターゲット３０の半径ｒが既知であることから、最小二乗法と最尤法とを用いて推定するパラメータは、ａ、ｂの２つに減っている。これにより、ターゲット３０の中心位置を推定する精度が向上されている。

図８は、上述の実験とは異なる条件で行った実験の結果を示すグラフである。本実験において上述の実験と異なる点は、ＬＲＦ２０によりターゲット３０の円周面３０Ｆに対してレーザ光を１０回走査した点と、ノイズ除去部４６のカルマンフィルタをＯＮにして多数の計測点のデータから反射光強度が所定値より小さいノイズ成分を除去した点とである。

図８のグラフに示すように、最小二乗法と最尤法とを用いてターゲット３０の中心位置を推定した値は、鉛直軸２０Ａから計測点までの距離と比例関係にあり、図７のグラフに示す推定値と比して安定していることがわかる。

これは、レーザ光を１回のみ走査して取得されたデータからターゲット３０の中心位置を推定した場合には、推定結果に及ぶノイズの影響が大きくなるのに対し、レーザ光を複数回走査して取得されたデータからターゲット３０の中心位置を推定した場合には、ノイズに対してロバストな推定が可能であるためと考えられる。また、反射強度フィルタとしてのカルマンフィルタを用いたことにより、図４（Ｂ）に示すように、円周面３０Ｆの円弧部の両端の形状の崩れを抑制でき、より正確な円周面３０Ｆのプロファイルデータを得ることができるためと考えられる。

以上説明したように、本実施形態に係る測位システム１０によれば、トータルステーションと比して低価であるＬＲＦ２０を用いてターゲット３０の中心位置を計測できる。また、本実施形態に係る測位システム１０によれば、ＬＲＦ２０のレーザ光の走査範囲内に存するターゲット３０であればその中心位置を計測できる。これにより、リアルタイムでターゲット３０の中心位置を計測することができ、また、移動するターゲット３０の中心位置を計測することもでき、さらには、ＬＲＦ２０によるレーザ光の一連の走査中に、複数のターゲット３０の中心位置を計測することもできる。

図９〜図１２は、杭打ち機１の位置及び向きを計測する原理を説明するための図である。これらの図に示すように、４個のターゲット３０Ａ〜３０Ｄを杭打ち機１の所定位置に設置する。ターゲット３０Ａは、杭打ち機１の前方に杭と同軸状に設置し、ターゲット３０Ｂは、杭打ち機１の左側部に設置する。また、ターゲット３０Ｃは、杭打ち機１の右側部に設置し、ターゲット３０Ｄは、杭打ち機１の右後部に設置する。

ここで、４個のターゲット３０Ａ〜３０Ｄは、予め相対的な位置関係を決めて設置されている。即ち、所定の基準点Ｏ（本実施形態では、平面視にて略矩形状の杭打ち機１の機体の４角の点とターゲット３０Ａの中心点とからなる五角形の図心）を原点とする各ターゲット３０Ａ〜３０Ｄの位置座標と、ターゲット３０Ａ〜３０Ｄの相互間の距離とを予め設定している。そして、これらのデータが、コンピュータ１２の記憶部１４（図２参照）に記憶されており、第２測位部４０１はこれらのデータに基づいて処理を行う。

また、ターゲット３０Ａ〜３０Ｄは、杭打ち機１の全高よりも低い高さに設置されており、ターゲット３０Ａ〜３０Ｄの何れか１個は、ＬＲＦ２０のレーザ光Ｌが杭打ち機１で遮られて位置を検出できない死角領域に位置する。例えば、図中実線で示すように、杭打ち機１の前部がＬＲＦ２０と対向してターゲット３０Ａを真ん中に置いてその両側にターゲット３０Ｂ，３０Ｄが位置する状態では、ターゲット３０Ｃが死角領域に位置する。また、図中鎖線で示すように、杭打ち機１の右側部がＬＲＦ２０と対向してターゲット３０Ｄを真ん中に置いてその両側にターゲット３０Ａ，３０Ｄが位置する状態では、ターゲット３０Ｂが死角領域に位置する。

杭打ち機１の位置及び向きを計測する際には、ＬＲＦ２０のレーザ光Ｌを図中反時計周り方向に走査させる。このため、杭打ち機１が図９に示す状態にあるときは、ターゲット３０Ｂ，３０Ａ，３０Ｄの中心位置が、３０Ｂ→３０Ａ→３０Ｄの順序で検出される。また、杭打ち機１が図１０に示す状態にあるときは、ターゲット３０Ａ，３０Ｄ，３０Ｃの中心位置が、３０Ａ→３０Ｄ→３０Ｃの順序で検出される。

また、ターゲット３０Ａ〜３０Ｄの相互間の距離Ｌ_ＡＢ，Ｌ_ＡＣ，Ｌ_ＡＤ，Ｌ_ＢＣ，Ｌ_ＢＤ，Ｌ_ＣＤは、相互に異なる大きさに設定されている。これにより、ターゲット３０Ａ〜３０Ｄの中心点の位置座標からターゲット３０Ａ〜３０Ｄの相互間の距離Ｌ_ＡＢ，Ｌ_ＡＣ，Ｌ_ＡＤ，Ｌ_ＢＣ，Ｌ_ＢＤ，Ｌ_ＣＤを算出すれば、その距離に対応する２個のターゲット３０を判別することができる。

ここで、２個のターゲット３０のＬＲＦ２０を中心とした反時計回り方向の位置が入れ替わる場合、何れか一方のターゲット３０は、ＬＲＦ２０の死角領域に位置する。例えば、図９に示すように、ＬＲＦ２０を中心とした反時計回り方向にターゲット３０Ｂ、ターゲット３０Ａの順序で並んでいたものが、図１０に示すようにその順序が逆になった場合には、ターゲット３０ＢがＬＲＦ２０の死角領域に位置する。即ち、ＬＲＦ２０で検出される２個のターゲット３０のＬＲＦ２０を中心とした反時計回り方向の位置関係は決まっている。このため、当該位置関係に基づいて、ＬＲＦ２０で検出された２個のターゲット３０を判別することができる。

なお、距離Ｌ_ＡＢ，Ｌ_ＡＣ，Ｌ_ＡＤ，Ｌ_ＢＣ，Ｌ_ＢＤ，Ｌ_ＣＤの測定値には誤差が生じるところ、この誤差により距離Ｌ_ＡＢ，Ｌ_ＡＣ，Ｌ_ＡＤ，Ｌ_ＢＣ，Ｌ_ＢＤ，Ｌ_ＣＤの相対的な大小関係が変わることがないように、この誤差よりも距離Ｌ_ＡＢ，Ｌ_ＡＣ，Ｌ_ＡＤ，Ｌ_ＢＣ，Ｌ_ＢＤ，Ｌ_ＣＤの各々の差が大きくなるように、これらの各距離を設定している。

また、図１１に示すように、杭打ち機１の位置及び向きを計測する際には、計測領域に設定された座標系（原点Ｏ_０（０，０））における基準点Ｏの位置座標（Ｘ_Ｐ１，Ｙ_Ｐ１）を計測し、基準点Ｏ（Ｘ_Ｐ１，Ｙ_Ｐ１）を原点とする極座標系におけるターゲット３０Ａの中心点Ａ（Ｘ_Ａ１，Ｙ_Ａ１）の角度（以下、偏角という）θを計測する。ここで、偏角θは、基準点Ｏとターゲット３０Ａの中心点Ａとを結ぶ直線のＸ軸に対する角度である。

ここで、図１２に示すように、２個のターゲット３０がＬＲＦ２０に対して前後に重なる場合があるが、その場合には、ＬＲＦ２０は、重なった２個のターゲット３０のうちの前方のものと、その左側のターゲット３０とを検出する。

図１３は、杭打ち機１の位置及び向きを計測する処理を説明するためのフローチャートである。このフローチャートに示すように、まず、ＬＲＦ２０でレーザ光を走査し、ターゲット３０Ａ〜３０Ｄの何れか２個の中心点の位置座標を検出する（ステップ１１）。この際、第１測位部４１は、レーザ光を反時計回り方向に走査するＬＲＦ２０により１、２番目に検出されたターゲット３０の中心点の位置座標を算出する。例えば、図９に示す状態では、ターゲット３０Ｂ，３０Ａの中心点の位置座標が算出され、図１０に示す状態では、ターゲット３０Ａ，３０Ｄの中心点の位置座標が算出される。

次に、第２測位部４０１が、第１測位部４１により算出された２個のターゲット３０の中心点間距離を算出し（ステップ１２）、記憶部１４に記憶されている既知のデータＬ_ＡＢ，Ｌ_ＡＣ，Ｌ_ＡＤ，Ｌ_ＢＣ，Ｌ_ＢＤ，Ｌ_ＣＤと比較して、対応する２個のターゲット３０を判別する（ステップ１３）。例えば、図９に示す状態では、既知のデータとしてＬ_ＡＢが抽出され、ターゲット３０Ｂ，３０Ａが判別される。また、図１０に示す状態では、既知のデータとしてＬ_ＡＤが抽出され、ターゲット３０Ａ，３０Ｄが判別される。

ここで、上述したように、ＬＲＦ２０で検出された２個のターゲット３０のＬＲＦ２０を中心とした反時計回り方向の位置関係は決まっており、この位置関係が記憶部１４に記憶されている。そして、第２測位部４０１は、記憶部１４に記憶されているターゲット３０Ａ〜３０Ｄの位置関係の情報に基づいて、ＬＲＦ２０で検出された２個のターゲット３０を判別する。

次に、第２測位部４０１は、２個のターゲット３０の中心点の位置から基準点Ｏの位置座標（Ｘ_Ｐ１，Ｙ_Ｐ１）を算出する（ステップ１４）。ここで、各ターゲット３０と基準点Ｏとの位置関係は予め決められてその情報が記憶部１４に記憶されており、第２測位部４０１は、各ターゲット３０と基準点Ｏとの位置関係の情報に基づいて、基準点Ｏの位置座標（Ｘ_Ｐ１，Ｙ_Ｐ１）を算出する。

次に、図１４に示すように、第２測位部４０１は、原点をＯ_０（０，０）とするＸ−Ｙ座標を、基準点Ｏが原点Ｏ_０（０，０）に一致する極座標に変換する（ステップ１５）。そして、第２測位部４０１は、２個のターゲット３０の中心点の位置からターゲット３０Ａの中心点Ａの位置座標（Ｘ_Ａ１−Ｘ_Ｐ１，Ｙ_Ａ１−Ｙ_Ｐ１ ）を算出し、下記式２０に基づいて、ターゲット３０Ａの中心点Ａの偏角θを算出する（ステップ１６）。

ここで、第２測位部４０１は、ターゲット３０Ａの中心点Ａが上記極座標の第２〜４象限に位置する場合には、上記式２０のθをそれぞれ、−θ、π−θ、π＋θに変換して偏角θを算出する。なお、第２測位部４０１は、Ｘ_Ａ１−Ｘ_Ｐ１≧０，Ｙ_Ａ１−Ｙ_Ｐ１＞０の場合に中心点Ａが第１象限に位置すると判定し、Ｘ_Ａ１−Ｘ_Ｐ１＜０，Ｙ_Ａ１−Ｙ_Ｐ１≧０の場合に中心点Ａが第２象限に位置すると判定する。また、第２測位部４０１は、Ｘ_Ａ１−Ｘ_Ｐ１≦０，Ｙ_Ａ１−Ｙ_Ｐ１＜０の場合に中心点Ａが第３象限に位置すると判定し、Ｘ_Ａ１−Ｘ_Ｐ１＞０，Ｙ_Ａ１−Ｙ_Ｐ１≦０の場合に中心点Ａが第４象限に位置すると判定する。

次に、表示制御部４０２が、第２測位部４０１により算出された基準点Ｏの位置座標及びターゲット３０Ａの偏角θに基づいて、杭打ち機１の位置及び向きをモニタ１８に表示させる（ステップ１７）。ここで、杭の打設位置Ｐの情報が予め、コンピュータ１２の記憶部１４（図２参照）に記憶されており、表示制御部４０２は、杭打ち機１の位置及び向きと共に杭の打設位置Ｐをモニタ１８に表示させる（図２参照）。なお、杭打ち機１が打設位置Ｐに近づいたことを音や音声で知らせるようにしてもよい。例えば、杭打ち機１が打設位置Ｐに近づくほど音の周波数が上がったり、距離を音声で知らせたりしてもよい。

以上のステップ１１〜１７が繰り返し実行されることにより、モニタ１８に表示された杭打ち機１の位置及び向きがリアルタイムで更新されることで、杭打ち機１のオペレータは、リアルタイムで杭打ち機１の位置や向きを確認しながら、杭を所定の打設位置まで移動させることができる。

図１５は、他の実施形態に係る測位システム１００の構成を示す図である。この測位システム１００は、ＬＲＦ２０と、杭打ち機１に設置された４個のターゲット３０とを備えている。ここで、ＬＲＦ２０及び４個のターゲット３０は、杭打ち機１の全高よりも高い位置に設置されており、４個のターゲット３０がＬＲＦ２０の死角領域に位置することはない。

ここで、本実施形態では２個のターゲット３０のＬＲＦ２０を中心とした反時計回り方向の位置関係が入れ替わる場合があり、この場合、４個のターゲット３０のうちの特定の２個のターゲット３０を検出したことは判別できるものの、その２個のターゲット３０の各々が何れのターゲット３０であるのかは判別できない。例えば、図９に示す状態と図１０に示す状態とでは、ターゲット３０Ａ，３０Ｂの上記位置関係が入れ替わるため、検出した２個のターゲットがターゲット３０Ａ，３０Ｂであることは判別できるものの、そのうちのどちらがターゲット３０Ａであるのかは判別できない。

そのため、３個のターゲット３０については位置関係が決まっていることから、３個のターゲット３０の中心点の位置を検出して当該３個のターゲット３０の中心点間距離を算出し、３個のターゲット３０の中心点間距離から３個のターゲット３０を判別する。

図１６は、本実施形態における杭打ち機１の位置及び向きを計測する処理を説明するためのフローチャートである。このフローチャートに示すように、まず、ＬＲＦ２０でレーザ光を走査し、第１測位部４１にターゲット３０Ａ〜３０Ｄの何れか３個の中心位置を算出させる（ステップ１０１）。ここで、第１測位部４１は、レーザ光を反時計回り方向に走査するＬＲＦ２０により１〜３番目に検出されたターゲット３０の中心位置を算出する。例えば、図９に示す状態では、ターゲット３０Ｂ，３０Ａ，３０Ｃの中心位置が算出され、図１０に示す状態では、ターゲット３０Ａ，３０Ｄ，３０Ｂの中心位置が算出される。

次に、第２測位部４０１が、第１測位部４１により算出された３個のターゲット３０の中心点間距離を算出し（ステップ１０２）、記憶部１４に記憶されている既知のデータＬ_ＡＢ，Ｌ_ＡＣ，Ｌ_ＡＤ，Ｌ_ＢＣ，Ｌ_ＢＤ，Ｌ_ＣＤと比較して、対応する３個のターゲット３０を判別する（ステップ１０３）。例えば、図９に示す状態では、既知のデータとしてＬ_ＡＢ，Ｌ_ＢＣ，Ｌ_ＡＣが抽出され、ターゲット３０Ｂ，３０Ａ，３０Ｃが判別される。また、図１０に示す状態では、既知のデータとしてＬ_ＡＤ，Ｌ_ＡＢ，Ｌ_ＢＤが抽出され、ターゲット３０Ａ，３０Ｄ，３０Ｂが判別される。

次に、第２測位部４０１は、３個のターゲット３０の中心点の位置から基準点Ｏの位置座標（Ｘ_Ｐ１，Ｙ_Ｐ１）を算出する（ステップ１０４）。ここで、各ターゲット３０と基準点Ｏとの位置関係は予め決められてその情報が記憶部１４に記憶されており、第２測位部４０１は、各ターゲット３０と基準点Ｏとの位置関係の情報に基づいて、基準点Ｏの位置座標（Ｘ_Ｐ１，Ｙ_Ｐ１）を算出する。

次に、図１４に示すように、第２測位部４０１は、基準点Ｏが原点Ｏ_０（０，０）に一致する極座標に変換する（ステップ１０５）。そして、第２測位部４０１は、３個のターゲット３０の中心点の位置からターゲット３０Ａの中心点Ａの位置座標（Ｘ_Ａ１−Ｘ_Ｐ１，Ｙ_Ａ１−Ｙ_Ｐ１ ）を算出し、上記式２０に基づいて、ターゲット３０Ａの中心点Ａの偏角θを算出する（ステップ１０６）。なお、上記実施形態と同様、第２測位部４０１は、中心点Ａが位置する象限に応じてθを変換する。

次に、表示制御部４０２が、第２測位部４０１により算出された基準点Ｏの位置座標及びターゲット３０Ａの偏角θに基づいて、杭打ち機１の位置及び向きをモニタ１８に表示させる（ステップ１０７）。

以上のステップ１０１〜１０７が繰り返し実行されることにより、モニタ１８に表示された杭打ち機１の位置及び向きがリアルタイムで更新されることで、杭打ち機１のオペレータは、リアルタイムで杭打ち機１の位置や向きを確認しながら、杭を所定の打設位置まで移動させることができる。

図１７（Ａ）は、他の実施形態に係る測位システム１００を示す側面図であり、図１７（Ｂ）は、図１７（Ａ）のＢ−Ｂ矢視図である。これらの図に示すように、測位システム１００は、ＬＲＦ２０と、ＬＲＦ２０を鉛直軸２０Ａの周りに回転自在に支持し、回転駆動する回転台５０と、回転台５０を鉛直軸２０Ａの周りに及び水平軸の周りに回転自在に支持する調整台６０と、調整台６０を支持する三脚７０と、三脚７０に支持された日除けカバー７２と、ＬＲＦ２０の上部に取り付けられたレーザポインタ８０とを備えている。

日除けカバー７２は、回転台５０と調整台６０との間に挟み込まれた底板７２Ａと、ＬＲＦ２０の背面側に配された側板７２Ｂと、ＬＲＦ２０の上側に配された天板７２Ｃとが一体で形成されてなる。この日除けカバー７２の天板７２Ｃは、ＬＲＦ２０、レーザポインタ８０への日光の直射を防いでいる。

回転台５０は、底板７２Ａ上に設置された駆動部５２と、駆動部５２により鉛直軸２０Ａ周りに回転自在に支持されると共に回転駆動される回転テーブル５４とを備えている。回転テーブル５４上には、ＬＲＦ２０が設置されており、回転テーブル５４とＬＲＦ２０とが一体となって鉛直軸２０Ａの周りに回転する。

調整台６０は、パンチルト台であり、鉛直軸２０Ａの周りに３６０°回転可能、水平軸の周りにも回転可能であり、回転台５０とＬＲＦ２０とレーザポインタ８０との鉛直軸２０Ａ周りの角度、及び水平軸周りの角度を調整可能である。また、レーザポインタ８０は、ＬＲＦ２０が走査方向の中央部に射出するレーザ光Ｌｆに対して平行に、基準位置検出用のレーザ光Ｌｐを射出する。即ち、調整台６０を鉛直軸２０Ａの周りに回転させることにより、レーザポインタ８０が射出するレーザ光Ｌｐの光軸と、ＬＲＦ２０によるレーザ光Ｌｆの走査範囲とを、鉛直軸２０Ａの周りに調整できる。また、調整台６０を水平軸の周りに回転させることにより、レーザポインタ８０が射出するレーザ光ＬｐとＬＲＦ２０が射出するレーザ光Ｌｆを水平軸の周りに調整できる。

図１８は、レーザポインタ８０を用いて、鉛直軸２０Ａの設置位置や設置角度の初期設定や校正を行っている状態を示す斜視図である。この図に示すように、所定の基準位置に再帰性反射材８２が前面に取り付けられたターゲット８４を設置し、レーザポインタ８０から再帰性反射材に向けてレーザ光Ｌｐを射出させる。再帰性反射材８２は、入射光を光源の方向に反射する性質を有する。この再帰性反射材８２で反射されたレーザ光Ｌｐの輝度に応じて、ＬＲＦ２０のターゲット８４に対する相対位置を検出し、三脚７０の位置を調整したり、調整台６０によりＬＲＦ２０のパン角度及びチルト角度を調整したりする。

図１９（Ａ）は、回転台５０が停止した状態でＬＲＦ２０がレーザ光を１回走査した場合におけるターゲット３０の円周面３０Ｆ上の計測点を示す図である。この図に「＋」でプロットしたように、円周面３０Ｆ上の計測点は、ＬＲＦ２０の角度分解能θ_ｆ°に応じた間隔を空けて離散する。

図１９（Ｂ）は、回転台５０を回転させながらＬＲＦ２０がレーザ光を複数回走査した場合におけるターゲット３０の円周面３０Ｆ上の計測点を示す図である。この計測では、ＬＲＦ２０がレーザ光を複数回走査し、その間、駆動部５２が、レーザ光が１回走査される毎に、ＬＲＦ２０の角度分解能θ_ｆ°（例えば、０．２５°）よりも微小な角度θ_ｔ°（例えば、０．０１５°）ずつ、回転テーブル５４を回転させる。これにより、図１９（Ｂ）に示すように、ＬＲＦ２０の角度分解能θ_ｆ°よりも高分解能であるデータを取得でき、より正確な円周面３０Ｆのプロファイルデータを取得できる。従って、ターゲット３０の中心位置の推定をより高精度に行うことができる。

図２０（Ａ）は、回転台５０を停止させた状態でＬＲＦ２０にレーザ光を１０回走査させることにより取得した円周面３０Ｆ上の計測点のデータから、ターゲット３０の中心位置を推定した結果を示すグラフである。また、図２０（Ｂ）は、回転台５０を回転させながらＬＲＦ２０にレーザ光を１０回走査させることにより取得した円周面３０Ｆ上の計測点のデータから、ターゲット３０の中心位置を推定した結果を示すグラフである。これらのグラフにおいて、一定距離法を用いた推定結果は実線で示し、最小二乗法を用いた推定結果は破線で示し、最尤法を用いた推定結果は鎖線で示している。また、横軸は鉛直軸２０Ａからターゲット３０の中心軸３０Ｅまでの距離（ｍｍ）であり、縦軸は推定されたターゲット３０の中心位置の誤差（ｍｍ）である。

図２０（Ａ）のグラフに示すように、回転台５０を停止させた状態でＬＲＦ２０にレーザ光を１０回走査させることにより取得した円周面３０Ｆ上の計測点のデータから、最小二乗法と最尤法とにより、ターゲット３０の中心位置を推定した場合における誤差の平均値は、１５．３ｍｍであった。一方、図２０（Ｂ）のグラフに示すように、回転台５０を回転させながらＬＲＦ２０にレーザ光を１０回走査させることにより取得した円周面３０Ｆ上の計測点のデータから、最小二乗法と最尤法とにより、ターゲット３０の中心位置を推定した場合における誤差の平均値は、８．３ｍｍであった。これにより、本実施形態に係る円周面３０Ｆ上の計測点のデータの取得方法を用いた場合、ターゲット３０の中心位置をより高精度に推定できることがわかる。

なお、上記実施形態では、被計測体を、円柱体であり上記特徴点が中心軸３０Ｅであるターゲット３０とした。しかし、例えば、断面が多角形の角柱体や角錐体であり特徴点がその中心軸であるものや、球体であり特徴点が中心点であるもの、直方体や立方体であり特徴点がその中心点であるもの等、輪郭上の計測点から特徴点を推定可能であるものであれば、被計測体として採用可能である。

また、被計測体としての円柱体の断面は真円には限られず楕円でもよく、最小二乗法又は最尤法を用いて、複数の計測点の位置を近似した円の方程式は、上述した半径ｒの真円の方程式には限られず楕円の方程式であってもよい。また、解析方法は、上記実施形態で挙げた一定距離法、最小二乗法、及び最尤法に限られるものではない。

また、杭打ち機１の位置及び向きを計測する測位システム１０、１００を例に挙げて本発明を説明したが、掘削機やクレーン等の他の移動体の位置及び向きを計測する測位システムにも本発明を適用できる。

１ 杭打ち機（移動体）、１０ 測位システム、１２ コンピュータ、１４ 記憶部、１６ 入力部、１８ モニタ、２０ ＬＲＦ、２０Ａ 鉛直軸、３０（３０Ａ〜３０Ｄ） ターゲット（被計測体）、３０Ｅ 中心軸、３０Ｆ 円周面、３４ 軸体、３４Ａ 中心軸、３６ 水準器、４０ 情報処理部、４１ 第１測位部、４２ 分別部、４４ クラスタリング部、４６ ノイズ除去部、４８ 推定部、４９ 判定部、５０ 回転台、５２ 駆動部、５４ 回転テーブル、６０ 調整台、７０ 三脚、７２ 日除けカバー、７２Ａ 底板、７２Ｂ 側板、７２Ｃ 天板、８０ レーザポインタ、８２ 再帰性反射材、８４ ターゲット、１００ 測位システム、４０１ 第２測位部、４０２ 表示制御部

Claims (7)

1. 移動体の位置及び向きを計測する測位システムであって、
   前記移動体に設定された所定の基準点に対して既定の位置関係をもって、且つ互いに既定の位置関係をもって前記移動体に設置された複数の被計測体と、
   各被計測体の表面を光で走査することにより各被計測体の表面上における複数の計測点の位置情報を取得する光走査式測距装置と、
   前記光走査式測距装置により取得された各被計測体の表面上における複数の計測点の位置情報に基づいて、各被計測体の輪郭に対して所定の位置関係にある所定点の位置を推定する第１測位部と、
   前記第１測位部により推定された前記複数の被計測体の前記所定点の位置と、前記複数の被計測体と前記所定の基準点との既定の位置関係と、前記複数の被計測体の互いの既定の位置関係とに基づいて、前記基準点の位置と、前記移動体の前記基準点を中心とする回転方向の位置とを算出し、前記基準点の位置に基づいて前記移動体の位置を推定し、前記回転方向の位置に基づいて前記移動体の向きを推定する第２測位部と、
   を備える測位システム。
2. ４個以上の前記被計測体が前記移動体に設置され、１個の前記被計測体が必ず前記光走査式測距装置の死角領域に位置するように設置され、
   前記第２測位部は、前記第１測位部により推定された２個の前記被計測体の前記所定点の位置と、前記２個の被計測体と前記所定の基準点との既定の位置関係と、前記２個の被計測体の互いの既定の位置関係とに基づいて、前記基準点の位置と、前記移動体の前記基準点を中心とする回転方向の位置とを算出する請求項１に記載の測位システム。
3. ３個以上の前記被計測体が前記移動体に設置され、全ての前記被計測体が常に前記光走査式測距装置の死角領域に位置することがないように設置され、
   前記第２測位部は、前記第１測位部により推定された３個の前記被計測体の前記所定点の位置と、前記３個の被計測体と前記所定の基準点との既定の位置関係と、前記３個の被計測体の互いの既定の位置関係とに基づいて、前記基準点の位置と、前記移動体の前記基準点を中心とする回転方向の位置とを算出する請求項１に記載の測位システム。
4. 前記複数の被計測体は、相互の距離が全て異なるように前記移動体に設置されている請求項１から請求項３までの何れか１項に記載の測位システム。
5. 前記被計測体は円柱体であり、前記所定点は、前記円柱体の中心軸上の点であり、
   前記第１測位部は、最小二乗法又は最尤法を用いて、前記複数の計測点の位置を近似した円の方程式を求め、求めた円の方程式に基づいて、前記中心軸上の点の位置を推定する請求項１から請求項４までの何れか１項に記載の測位システム。
6. 移動体の位置及び向きを計測する測位方法であって、
   複数の被計測体を、前記移動体に設定した所定の基準点に対して既定の位置関係を有し、且つ互いに既定の位置関係を有するように前記移動体に設置するステップと、
   光走査式測距装置により各被計測体の表面を光で走査することで各被計測体の表面上における複数の計測点の位置情報を取得するステップと、
   前記光走査式測距装置により取得した各被計測体の表面上における複数の計測点の位置情報に基づいて、各被計測体の輪郭に対して所定の位置関係にある所定点の位置を推定するステップと、
   推定された前記複数の被計測体の前記所定点の位置と、前記複数の被計測体と前記所定の基準点との既定の位置関係と、前記複数の被計測体の互いの既定の位置関係とに基づいて、前記基準点の位置と、前記移動体の前記基準点を中心とする回転方向の位置とを算出し、前記基準点の位置に基づいて前記移動体の位置を推定し、前記回転方向の位置に基づいて前記移動体の向きを推定するステップと、
   を備える測位方法。
7. 移動体に設定された所定の基準点に対して既定の位置関係をもって、且つ互いに既定の位置関係をもって前記移動体に設置された複数の被計測体と、各被計測体の表面を光で走査することにより各被計測体の表面上における複数の計測点の位置情報を取得する光走査式測距装置とを備え、前記移動体の位置及び向きを計測する測位システムに含まれるコンピュータに実行させるプログラムであって、
   前記光走査式測距装置により取得された各被計測体の表面上における複数の計測点の位置情報に基づいて、各被計測体の輪郭に対して所定の位置関係にある所定点の位置を推定する手順と、
   推定された前記複数の被計測体の前記所定点の位置と、前記複数の被計測体と前記所定の基準点との既定の位置関係と、前記複数の被計測体の互いの既定の位置関係とに基づいて、前記基準点の位置と、前記移動体の前記基準点を中心とする回転方向の位置とを算出し、前記基準点の位置に基づいて前記移動体の位置を推定し、前記回転方向の位置に基づいて前記移動体の向きを推定する手順と、
   を前記コンピュータに実行させるプログラム。