JP5516204B2 - 位置決め方法及びシステム - Google Patents

Description

本発明は、位置決め対象物を目標位置に位置決めする方法及びシステムに関する。

墨出し装置により墨出しを行う際、墨出し装置に設けられたレーザ式やインク式等の墨出し器の位置の計測が必要である。その位置計測は、トランシット（光波測量器）やトランシットによる位置計測を自動化したトータルステーション（例えば、特許文献１参照）等を使用して行うことができる。トランシットによる位置計測では、測距儀から墨出し装置に設置した反射プリズムに向けて光波が発射され、反射プリズムで反射された光波の発信回数に基づいて計測点までの距離が算出される。

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

上述のトランシットによる位置計測では、反射プリズムが測距儀を向いていないときには位置計測を実施できず、リアルタイムで位置計測を実施できない時間が生まれる。このため、墨出し器等の位置決め対象物を、その位置を検出しながら検出された位置に基づいて目標位置に向けて移動させることができず、従って、墨出し装置等の位置決め装置を目標位置まで自律走行させること等ができない。

本発明は、上記事情に鑑みてなされたものであり、位置決め対象物を、その位置を検出しながら検出された位置に基づいて目標位置に向けて移動させることができ、移動させた位置決め対象物を、正確に該目標位置に位置決めすることができる位置決め方法及びシステムを提供するものである。

上記課題を解決するために、本発明に係る位置決め方法は、位置決め対象物を目標位置に位置決めする位置決め方法であって、レーザレンジファインダを使用して、前記位置決め対象物を、その位置を計測しながら前記目標位置に向けて移動させる移動工程と、前記移動工程の後に、トータルステーションを使用して、前記位置決め対象物を、その位置を計測しながら前記目標位置に位置決めする位置決め工程と、を備えることを特徴とする。

上記位置決め方法において、前記移動工程では、前記位置決め対象物を二次元平面内で位置決めする位置決め装置を、前記目標位置に向けて移動させてもよく、前記位置決め工程では、前記位置決め装置により前記位置決め対象物を、前記目標位置に位置決めしてもよい。

上記位置決め方法において、前記移動工程は、前記位置決め装置に設けられた被計測体の表面上における複数の計測点の位置情報を、前記レーザレンジファインダから取得する情報取得工程と、前記レーザレンジファインダから取得した前記複数の計測点の位置情報に基づいて、前記被計測体の輪郭に対して所定の位置関係に設定された前記位置決め対象物の位置を推定する推定工程と、を備えてもよい。

上記位置決め方法において、前記被計測体は円柱体であって、前記位置決め対象物の位置は、前記円柱体の中心軸の延長線上に設定されてもよく、その場合、前記推定工程において、最小二乗法又は最小二乗法又は最尤法を用いて、前記複数の計測点の位置を近似した円の方程式を求め、求めた円の方程式に基づいて、前記中心軸上の点の位置を推定してもよい。また、上記位置決め方法において、前記位置決め対象物は、墨出し器であってもよい。

また、上記課題を解決するために、本発明に係る位置決めシステムは、位置決め対象物を目標位置に位置決めする位置決めシステムであって、レーザレンジファインダを使用して計測される前記位置決め対象物の位置に基づいて、前記位置決め対象物を前記目標位置に向けて移動させる移動機構と、前記移動機構により前記位置決め対象物が移動された後に、トータルステーションを使用して計測される前記位置決め対象物の位置に基づいて、前記位置決め対象物を前記目標位置に位置決めする位置決め機構と、を備えることを特徴とする。

本発明に係る位置決め方法及びシステムによれば、位置決め対象物を、その位置を検出しながら検出された位置に基づいて目標位置に向けて移動させることができ、移動させた位置決め対象物を、正確に該目標位置に位置決めすることができる。

墨出しシステムを示す斜視図である。 一実施形態に係る墨出し装置を示す斜視図である。 位置調整機構を示す正面図である。 位置調整機構を示す側面図である 位置調整機構の作用を示す平面図である。 位置調整機構の作用を示す平面図である。 位置調整機構の作用を示す平面図である。 位置調整機構の作用を示す平面図である。 墨出しシステムの概略構成を示すブロック図である。 プログラムにより位置計測部の各部の機能が実現されて行われる処理を説明するためのフローチャートである。 （Ａ）は、カルマンフィルタによるノイズ除去を行っていない場合における円周面の片側のプロファイルの検出結果を示すグラフである。（Ｂ）は、カルマンフィルタによるノイズ除去を行った場合における円周面の片側のプロファイルの検出結果を示すグラフである。 一定距離法の原理を示す図である。 最小二乗法と最尤法とを用いて、クラスタの全データから円周面の中心位置を推定する方法の原理を示す図である。 ＬＲＦにより取得されたデータから、ターゲットの中心位置を推定した結果を示すグラフである。 墨出しを行う現場の座標系を示す図である。 トータルステーションにより墨出し装置の原点の位置を計測する原理を説明するための図である。 現場の座標系（ｘ´−ｙ´座標系）と墨出し装置の座標系（ｕ−ｖ座標系）との関係を示す図である。 ｕ−ｖ座標系とｘ´‐ｙ´座標系との関係を示す図である。 トータルステーションにより墨出し装置の原点Ｏ´を計測してから、スタンプ部で地面にマークを押印するまでの処理を説明するためのフローチャートである。

以下、本発明の一実施形態を、図面を参照しながら説明する。図１は、一実施形態に係る墨出しシステム１００の構成を示す斜視図である。この図に示すように、墨出しシステム１００は、自律走行型の墨出し装置１０と、レーザーレンジファインダ（以下、ＬＲＦという）１１０と、トータルステーション１２０と、墨出し装置１０をコントロールするパーソナルコンピュータ（以下、ＰＣという）１３０とを備えている。

図２は、墨出し装置１０を示す斜視図である。この図に示すように、墨出し装置１０は、杭芯の打設位置を示すマークを地面に押印するスタンプ機構１２と、水平方向の位置調整を可能にスタンプ機構１２を支持する位置調整機構２０と、位置調整機構２０を支持するフレーム５０と、フレーム５０の底部に配された走行機構６０と、フレーム５０の上部に支持された一対の計測プリズム７０Ａ、７０Ｂとを備えている。

スタンプ機構１２は、上記マークが刻印されインクが充填されたスタンプ部１４と、スタンプ部１４を昇降させる昇降機構１６とを備えている。昇降機構１６は、スランプ部１４を鉛直方向（図中ｚ方向）に移動可能に支持するレール１６Ｒと、スタンプ部１４をレール１６Ｒに沿って移動させるモータ１６Ｍとを備えている。この昇降機構１６によりスタンプ部１４が地面まで下降されることにより上記マークが地面に押印される。

位置調整機構２０は、スタンプ機構１２を互いに直交する２方向（図中ｘ方向及びｙ方向）に移動させるｘ−ｙ調整機構３０と、ｘ−ｙ調整機構３０を鉛直軸周りの方向（図中θ方向）に回転させる回転角調整機構４０とを備えている。ｘ−ｙ調整機構３０は、昇降機構１６をモータ駆動によりｙ方向に移動させるｙ軸ステージ３２と、ｙ軸ステージ３２をモータ駆動によりｘ方向に移動させるｘ軸ステージ３４とを備えている。

ｙ軸ステージ３２は、ｙ方向に延び、昇降機構１６をｙ方向に移動可能に支持するレール３２Ｒと、昇降機構１６をレール３２Ｒに沿って移動させるモータ３２Ｍとを備えている。また、ｘ軸ステージ３４は、ｘ方向に延び、レール３２Ｒをｘ方向に移動可能に支持するレール３４Ｒと、レール３２Ｒをレール３４Ｒに沿って移動させるモータ３４Ｍとを備えている。

また、回転角調整機構４０は、ｘ軸ステージ３４をフレーム５０の中段の台板５２に鉛直軸周りに回転自在に支持する回転支軸４２と、回転支軸４２をその軸心周りに回転させるモータ４４とを備えている。回転支軸４２は、フレーム５０の中心を通る鉛直軸上に配されている。

フレーム５０は、ｘ−ｙ調整機構３０の周囲に配された４本の脚部５４と、４本の脚部５４の上端に固定された円盤状の上記台板５２と、台板５２の周縁部に下端が固定された４本の脚部５６と、４本の脚部５４の上端に固定された円盤状の台板５８とを備えている。４本の脚部５４は、正方形状に配されている。また、台板５２上には、上記モータ４４やコントローラ１８等が設置されている。また、回転支軸４２は、台板５２の中心に回転自在に支持されている。また、台板５８上には、上記一対の計測プリズム７０Ａ、７０Ｂが設置されている。

走行機構６０は、複数の多方向移動ホイール（例えば、オムニホイール（登録商標））６２を備えている。多方向移動ホイール６２は、車輪本体の円周方向に複数の樽型の小輪が取り付けられた車輪であり、車輪本体の回転により前後方向に移動し、小輪の回転により左右方向に移動できるようになっている。各多方向移動ホイール６２に対応して不図示のモータが設けられており、各モータが個別に制御されることにより、走行機構６０及びその上に設置されているフレーム５０等の進行方向が制御される。

一対の計測プリズム７０Ａ、７０Ｂは、トータルステーション用のプリズム反射鏡であり、トータルステーションの本体から投射された測距測角用の光を反射してその本体へ送り返す。

図３は、位置調整機構２０を示す正面図である。また、図４は、位置調整機構２０を示す側面図である。図３に示すように、回転支軸４２の下端が、ｘ軸ステージ３４のレール３４Ｒの上面に固定されている。ここで、回転支軸４２の中心は、レール３４Ｒの長手方向中央部から一方側にずれた位置に配されている。また、レール３４Ｒの長手方向一方側にはモータ３４Ｍが設けられている。

また、図４に示すように、レール３４Ｒのスライダ３４Ｓが、レール３２Ｒの上面に固定されている。ここで、スライダ３４Ｓは、レール３２Ｒの長手方向中央部から一方側にずれた位置に配されている。また、レール３２Ｒの長手方向一方側にはモータ３２Ｍが設けられている。

また、レール３２Ｒのスライダ３２Ｓが、昇降機構１６のレール１６Ｒの上部に固定されている。ここで、スタンプ部１２は、レール１６Ｒよりもレール３２Ｒの長手方向一方側に配されている。また、回転角調整機構４０のモータ４４と回転支軸４２とは減速機構４６を介して連結されており、回転支軸４２の中心とモータ４４の回転軸とは互いにはずれて配されている。

図５から図８までは、位置調整機構２０の作用を示す平面図である。図５に示すように、位置調整機構２０では、回転支軸４２の中心を原点Ｏ´とするｘ´−ｙ´座標が設定されている。このｘ´−ｙ´座標は、回転支軸４２が回転することにより、図５に示す初期位置から原点Ｏ´の周りに回転する。なお、ｘ´−ｙ´座標のｘ´＞０，ｙ´＞０の領域を第１象限、ｘ´−ｙ´座標のｘ´＜０，ｙ´＞０の領域を第２象限、ｘ´−ｙ´座標のｘ´＜０，ｙ´＜０の領域を第３象限、ｘ´−ｙ´座標のｘ´＞０，ｙ´＜０の領域を第４象限という。

ｘ軸ステージ３４のレール３４Ｒは、ｘ´軸上に配されている。ｙ軸ステージ３２のレール３２Ｒは、ｙ´軸と平行に配されており、レール３４Ｒに沿ってｘ´方向に移動する。これにより、スタンプ機構１２がｘ´方向に移動する。また、スタンプ機構１２は、レール３２Ｒに沿ってｙ´方向に移動する。即ち、スタンプ部１４によるマークＭの押印位置が、図中破線のハッチングで示すｘ´−ｙ´平面Ａ内で移動する。

ここで、ｘ軸ステージ３４では、モータ３４Ｍがレール３４Ｒの長手方向一方側（図中−ｘ´側）に配され、ｙ軸ステージ３２では、モータ３２Ｍがレール３２Ｒの長手方向一方側（図中−ｙ´側）に配されている。このため、ｙ軸ステージ３２のレール３２Ｒの−ｘ´側への移動が制限され、スタンプ機構１２の‐ｙ´側への移動が制限される。従って、スタンプ機構１２の可動範囲であるｘ´−ｙ´平面Ａは、第１象限側に偏り、第２から第４象限では、ｘ´−ｙ´平面Ａに含まれない範囲が広くなる。

ここで、ｘ´−ｙ´平面Ａの中心は、第１象限に配されており、ｘ´−ｙ´平面Ａの回転中心である回転支軸４２は、ｘ´−ｙ´平面Ａの中心に対して第３象限側にずらして配されている。このため、ｘ´−ｙ´平面Ａの回転半径Ｒは、ｘ´−ｙ´平面Ａの中心から頂点までの距離（対角線の１／２の長さ）よりも長くなる。

図６は、ｘ−ｙ調整機構３０を図４に示す初期位置から９０°反時計周りに回転させた状態を示している。この図に示すように、ｘ´−ｙ´平面Ａは、第２象限側に偏り、第１、第３及び第４象限では、ｘ´−ｙ´平面Ａに含まれない範囲が広くなる。ここで、ｘ´−ｙ´平面Ａを図４に示す初期位置から反時計周り方向に回転させることにより、第１及び第２象限における上端の領域を、ｘ´−ｙ´平面Ａに含めることができる。

図７は、ｘ−ｙ調整機構３０を図５に示す初期位置から１８０°反時計周り方向に回転させた状態を示している。この図に示すように、ｘ´−ｙ´平面Ａは、第３象限側に偏り、第１、第２及び第４象限では、ｘ´−ｙ´平面Ａに含まれない範囲が広くなる。ここで、ｘ´−ｙ´平面Ａを図６に示す位置から反時計周り方向に回転させることにより、第２及び第３象限における左端の領域を、ｘ´−ｙ´平面Ａに含めることができる。

図８は、ｘ−ｙ調整機構３０を図５に示す初期位置から２７０°反時計周り方向に回転させた状態を示している。この図に示すように、ｘ´−ｙ´平面Ａは、第４象限側に偏り、第１から第３象限では、ｘ´−ｙ´平面Ａに含まれない範囲が広くなる。ここで、ｘ´−ｙ´平面Ａを図７に示す位置から反時計周り方向に回転させることにより、第３及び第４象限における下端の領域をｘ´−ｙ´平面Ａに含めることができる。

そして、図５に示すように、ｘ−ｙ調整機構３０を、図８に示す位置から反時計周り方向に回転させて初期位置に戻すことにより、第４及び第１象限における右端の領域を´−ｙ´平面Ａに含めることができる。以上、ｘ−ｙ調整機構３０を回転支軸４２の周りに回転させることにより、墨出し装置１０の内部でのスタンプ機構１２の可動範囲を拡張できる。

ここで、図１に示すように、墨出しシステム１００では、ＬＲＦ１１０で墨出し装置１０の位置を計測しながら、その計測値に基づいて墨出し装置１０を目標位置に向けて走行させる。その後、目標位置に向けて走行した墨出し装置１０の位置を、トータルステーション１２０で計測する。そして、その計測された位置と目標位置との誤差を補正するべく、位置調整機構２０を作動させる。そして、スタンプ部１４を下降させてマークＭを地面に押印する。以上で現場における墨出しが完了する。

トータルステーション１２０は、所定位置に設置された視準器１２２までの距離や、計測プリズム７０Ａ、７０Ｂまでの距離や、トータルステーション１２０の本体を始点として視準器１２２や計測プリズム７０Ａ、７０Ｂを終点とする各ベクトルの角度等に基づいて、計測プリズム７０Ａ、７０Ｂの位置を算出する。そして、トータルステーション１２０は、計測プリズム７０Ａ、７０Ｂの位置に基づいて、墨出し装置１０の原点Ｏ´の位置を算出する。

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

本実施形態における被計測物は、墨出し装置１０の上下の台板５２、５８の周りにシート材を固定することにより形成された半径ｒの円柱形状のターゲット１１２である。また、ターゲット１１２の円周面１１２Ｂは、レーザ光の乱反射を抑制するべく低輝度仕上げされている。これにより、ＬＲＦ１１０の鉛直軸から円周面１１２Ｂ上の計測点までの距離を計測する際のノイズを低減できる。また、円周面１１２Ｂの色は、レーザ光の反射率を考慮して、黒等の暗色ではなく、白等の明色である。

図９は、墨出しシステム１００の概略構成を示すブロック図である。この図に示すように、ＰＣ１３０は、ＬＲＦ１１０及びトータルステーション１２０に無線ＬＡＮや通信ケーブル等により接続されている。ＰＣ１３０は、ＬＲＦ１１００からの距離情報やトータルステーション１２０からの位置情報等を入力する入力部（入力インターフェース）１３１と、計算処理用のプログラムを記憶した不揮発性メモリ等の記憶部１３３と、当該プログラムを揮発性メモリ等に読み出して計算処理を実行する情報処理部（ＣＰＵ）１４０とを備える。

情報処理部１４０は、位置計測部１４１と、走行制御部１３２と、位置調整制御部１３４とを備えている。位置計測部１４１は、分別部１４２と、クラスタリング部１４４と、ノイズ除去部１４６と、推定部１４８と、判定部１４９とを備える。上記プログラムは、分別部１４２の分別機能、クラスタリング部１４４のクラスタリング機能、ノイズ除去部１４６のノイズ除去機能、推定部１４８の推定機能、及び判定部１４９の判定機能を実現させる。

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

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

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

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

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

図９に示すように、ＰＣ１３０は、走行機構６０を制御する走行制御部１３２を備えている。この走行制御部１３２は、判定部１４９によって確定された墨出し装置１０の中心座標（原点Ｏ´）と、予め設定されている目標座標Ｒとに基づいて、墨出し装置１０の原点Ｏ´に対する目標座標Ｒの方向及び距離を算出し、算出した結果に基づいて走行機構６０の走行方向及び走行距離を設定する。これにより、墨出し装置１０が、目標座標Ｒに向けて移動する。

また、ＰＣ１３０は、位置調整機構２０を制御する位置調整制御部１３４を備えている。この位置調整制御部１３４は、トータルステーション１２０により計測された墨出し装置１０の原点Ｏ´の位置座標と、予め設定されている目標座標Ｒとに基づいて、墨出し装置１０の原点Ｏ´の位置座標に対する目標座標Ｒの方向及び距離を算出し、算出した結果に基づいてスタンプ機構１２の移動方向及び移動距離を設定する。これにより、スタンプ部１４が、目標座標Ｒまで移動する。

ここで、ＬＲＦ１１０を使用して墨出し装置１０の原点Ｏ´の位置を計測する方法について詳細に説明する。図１１（Ａ）は、カルマンフィルタによるノイズ除去を行っていない場合における円周面１１２Ｂの片側のプロファイルの検出結果を示すグラフである。また、図１１（Ｂ）は、カルマンフィルタによるノイズ除去を行った場合における円周面１１２Ｂのプロファイルの片側の検出結果を示すグラフである。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

図１４のグラフに示すように、最小二乗法と最尤法とを用いてターゲット１１２の中心位置（原点Ｏ´）を推定した結果が、一定距離法を用いてターゲット１１２の中心位置を推定した結果と比して良好であった。

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

次に、トータルステーション１２０を使用して墨出し装置１０の原点Ｏ´の位置を計測しながら、スタンプ部１４を目標座標Ｒに位置決めする方法について詳細に説明する。図１５は、墨出しを行う現場の座標系を示す図である。この図に示すように、墨出しを行う現場について、ＬＲＦ１１０の鉛直軸を原点Ｏとするｘ−ｙ座標が設定されている。このため、ＬＲＦ１１０は、ｘ−ｙ座標における原点Ｏから計測点までの距離を計測する。また、トータルステーション１２０は、ｘ−ｙ座標における墨出し装置１０の原点Ｏ´の位置を計測する。

図１６は、トータルステーション１２０により墨出し装置１０の原点Ｏ´の位置を計測する原理を説明するための図である。この図に示すように、トータルステーション１２０は、所定位置Ｔ（ｘｔ，ｙｔ）に設置されている。また、視準器１２２は、所定位置Ｃ（ｘｃ，ｙｃ）に設置されている。

トータルステーション１２０により墨出し装置１０の原点Ｏ´の位置を計測するに際して、その最初に、既知点である基準点Ｒ（ｘｒ，ｙｒ）、所定位置Ｃ（ｘｃ，ｙｃ）に基づいて、ベクトルＴＯ、ベクトルＴＣ、これらがなす角α１を計測し、その計測結果に基づいて所定位置Ｔ（ｘｔ，ｙｔ）を算出する。

次に、ベクトルＴＢ、ベクトルＴＡ、ベクトルＴＯとベクトルＴＢとがなす角α２、及びベクトルＴＯとベクトルＴＡとがなす角α３を計測し、その計測結果に基づいて、計測プリズム７０Ａ、７０Ｂの位置（ｘａ，ｙａ）、（ｘｂ，ｙｂ）を算出する。そして、計測プリズム７０Ａ、７０Ｂの位置に基づいて墨出し装置１０の原点Ｏ´（ｘｏ，ｙｏ）の位置を算出する。ここで、原点Ｏ´（ｘｏ，ｙｏ）は下記（２０）（２１）式で表される。

図１７は、原点をＯ´（ｘｏ，ｙｏ）とする現場の座標系（ｘ´−ｙ´座標系）と墨出し装置１０の座標系（ｕ−ｖ座標系）との関係を示す図である。ｘ−ｙ座標系における目標座標Ｒ（ｘ，ｙ）は、移動ベクトルＯＯ´（ｘｏ，ｙｏ）で移動させ、回転角θだけ回転させることにより、下記（２２）式で示すｕ−ｖ座標系における目標座標Ｒ（ｕ，ｖ）に同次変換することができる。

そして、回転角θ＝‐θ´を代入して上記（２２）式を逆行列に変換し、上記（２０）（２１）式で表す値を代入することにより、ｕ−ｖ座標系における目標座標Ｒ（ｕ，ｖ）を下記（２３）式のように表すことができる。

ここで、ｄｅｔＴ＝１であることから、ｕ−ｖ座標系における目標座標Ｒ（ｕ，ｖ）は、下記（２４）（２５）式で表すことができる。

また、θ´は、ベクトルＯＡ´（（ｘａ−ｘｂ）／２，（ｙａ−ｙｂ）／２）と、ベクトルＯ´ｘ´の単位ベクトルすなわちベクトルＯｘの単位ベクトル（１，０）とのなす角であることから、ｃｏｓθ´とｓｉｎθ´とはそれぞれ下記（２６）（２７）式で表すことができる。但し、０≦θ´≦１８０°，ｙａ−ｙｂ≧０とする。

ここで、ｕ＜ｂｕ、ｕ＞ａｕかつｖ＜ｂｖ、ｖ＞ｂｖである場合に、目標座標Ｒ（ｕ，ｖ）は、ｕ−ｖ座標系の範囲外に位置する。この場合、走行制御部１３２は、ベクトルＯ´Ｒから、原点Ｏ´と目標座標Ｒ（ｕ，ｖ）との距離と、原点Ｏ´に対する目標座標Ｒ（ｕ，ｖ）の方向とを算出する。そして、走行制御部１３２は、算出した方向に算出した距離だけ走行機構６０を走行させる。これにより、目標座標Ｒ（ｕ，ｖ）がｕ−ｖ座標系の範囲内に入る。

一方、ｂｕ≦ｕ≦ａｕ、かつ、ｂｖ≦ｖ≦ａｖである場合に、目標座標Ｒ（ｕ，ｖ）は、ｕ−ｖ座標系の範囲内に位置する。この場合、位置調整制御部１３４は、目標座標Ｒ（ｕ，ｖ）の象限を判定する。

図１８は、ｕ−ｖ座標系とｘ´‐ｙ´座標系との関係を示す図である。この図に示すように、ｕ−ｖ座標系の第１象限は、０≦ｕ≦ａｕかつ０≦ｖ≦ａｖの領域であり、ｕ−ｖ座標系の第２象限は、ｂｕ≦ｕ＜０かつ０≦ｖ≦ａｖの領域である。また、ｕ−ｖ座標系の第３象限は、ｂｕ≦ｕ≦０かつＢＶ≦ｖ＜０の領域であり、ｕ−ｖ座標系の第４象限は、０＜ｕ＜ａｕかつｂｖ≦ｖ＜０の領域である。

位置調整制御部１３４は、目標座標Ｒ（ｕ，ｖ）が、上述の第１〜第４象限の何れに位置するかを判定し、その判定結果に応じて、ｘ´−ｙ´座標系のｕ−ｖ座標系に対する回転角θ“を設定する。位置調整制御部１３４は、目標座標Ｒ（ｕ，ｖ）が、第１象限に位置する場合には、回転角θ”を０°に設定する。この場合、ｕ−ｖ座標系とｘ´−ｙ´座標系とが一致することから、スタンプ部１４のｘ´−ｙ´座標系における座標（ｘ´，ｙ´）は（ｕ，ｖ）となる。

位置調整制御部１３４は、目標座標Ｒ（ｕ，ｖ）が、第２象限に位置する場合には、回転角θ“を９０°に設定する。この場合、スタンプ部１４のｘ´−ｙ´座標系における座標（ｘ´，ｙ´）は（−ｖ，ｕ）となる。また、位置調整制御部１３４は、目標座標Ｒ（ｕ，ｖ）が、第３象限に位置する場合には、回転角θ”を１８０°に設定する。この場合、スタンプ部１４のｘ´−ｙ´座標系における座標（ｘ´，ｙ´）は（−ｕ，−ｖ）となる。さらに、位置調整制御部１３４は、目標座標Ｒ（ｕ，ｖ）が、第４象限に位置する場合には、回転角θ“を２７０°に設定する。この場合、スタンプ部１４のｘ´−ｙ´座標系における座標（ｘ´，ｙ´）は（ｖ，−ｕ）となる。

図１９は、トータルステーション１２０により墨出し装置１０の原点Ｏ´を計測してから、スタンプ部１４で地面にマークＭを押印するまでの処理を説明するためのフローチャートである。なお、上述したように、トータルステーション１２０による墨出し装置１０の原点Ｏ´を計測する前に、ＬＲＦ１１０で墨出し装置１０の位置を計測しながら、その計測値に基づいて墨出し装置１０を目標座標Ｒに向けて走行させる。

本処理ルーチンは、トータルステーション１２０により原点Ｏ´の位置が計測された後に開始されてステップ１へ移行する。ステップ１１では、走行制御部１３２が、上記（２０）〜（２７）式に基づいて、ｘ−ｙ座標系における目標座標Ｒ（ｘ，ｙ）を、ｕ−ｖ座標系における目標座標Ｒ（ｕ，ｖ）に同次変換する。次に、ステップ１２では、走行制御部１３２が、目標座標Ｒ（ｕ，ｖ）が、ｕ−ｖ座標系の範囲内であるか否か、即ち、ｂｕ≦ｕ≦ａｕかつｂｖ≦ｖ≦ａｖであるか否かを判定する。その結果、肯定判定された場合にはステップ１３へ移行する一方、否定判定された場合にはステップ２０へ移行する。

ステップ２０では、走行制御部１３２が、ベクトルＯ´Ｒから、原点Ｏ´と目標座標Ｒ（ｕ，ｖ）との距離と、原点Ｏ´に対する目標座標Ｒ（ｕ，ｖ）の方向とを算出し、算出した方向に算出した距離だけ走行機構６０を走行させる。即ち、墨出し装置１０を目標座標Ｒに向けて移動させる。そして、ステップ１２へ移行する。

ステップ１３では、位置調整制御部１３４が、目標座標Ｒがｕ−ｖ座標系の第１〜第４象限の何れに位置するかを判定し、その判定結果に応じて、ｘ´−ｙ´座標系のｕ−ｖ座標系に対する回転角θ“を設定する。上述したように、目標座標Ｒ（ｕ，ｖ）が第１象限に位置する場合、回転角θ”は０°に設定され、目標座標Ｒ（ｕ，ｖ）が第２象限に位置する場合、回転角θ”は９０°に設定される。また、目標座標Ｒ（ｕ，ｖ）が第３象限に位置する場合、回転角θ”は１８０°に設定され、目標座標Ｒ（ｕ，ｖ）が第４象限に位置する場合、回転角θ”は２７０°に設定される。

次に、ステップ１４では、位置調整制御部１３４が、ｘ−ｙ調整機構３０を作動させ、スタンプ機構１２をｘ´−ｙ´平面内でｘ´軸方向及びｙ´軸方向に移動させることにより、スタンプ機構１２を目標座標Ｒ（ｕ，ｖ）まで移動させる。次に、ステップ１５では、位置調整制御部１３４が、昇降機構１６を作動させ、スタンプ部１４を地面まで下降させ、地面にマークＭを押印させる。以上で処理ルーチンを終了する。

以上、本実施形態に係る墨出しシステム１０では、ＬＲＦ１１０のレーザ光の走査範囲内に存するターゲット１１２であればその中心位置（原点Ｏ´）を計測できる。これにより、リアルタイムでターゲット３０の中心位置を計測することができ、墨出し装置１０の原点Ｏ´の位置を計測しながら、その計測値に基づいて、墨出し装置１０を目標位置へ向けて移動させることができる。そして、墨出し装置１０を目標位置へ向けて移動させた後、トータルステーション１２０を使用して、墨出し装置１０の原点Ｏ´の位置を高精度に計測することにより、スタンプ部１４を、位置調整機構２０で目標位置との誤差を補正するべく移動させ、正確に目標位置に位置決めすることができる。

なお、本実施形態では、インクでマークＭを押印するスタンプ型の墨出し器が、位置決め対象物である墨出し方法を例にとって本発明を説明した。しかし、レーザ墨出し器が、位置決め対象物である墨出し方法や、ドリルが位置決め対象物である加工装置の位置決め法法等の他の位置決め方法にも本発明を適用可能である。

また、本実施形態では、地面に墨出しを行う墨出し方法を例にとって本発明を説明した。しかし、壁面に墨出しを行う墨出し方法や、天井に墨出しを行う墨出し方法等の他の墨出し方法にも本発明を適用可能である。

また、本実施形態では、墨出し装置１０を自律走行させる例をとって本発明を説明したが必須ではなく、手動で移動させてもよい。さらに、本実施形態では、スタンプ部１４を二次元平面内で位置決めするｘ−ｙ調整機構３０を鉛直軸周りに回転させることにより、スタンプ部１４の墨出し装置１０内における可動範囲を拡張したが、必須ではない。

１０ 墨出し装置、１２ スタンプ機構、１４ スタンプ部（位置決め対象物、墨出し器）、１６ 昇降機構、２０ 位置調整機構、３０ ｘ−ｙ調整機構（位置決め装置）、３２ ｙ軸ステージ、３２Ｒ レール、３２Ｍ モータ、３４ ｘ軸ステージ、３４Ｒ レール、３４Ｍ モータ、４０ 回転角調整機構、４２ 回転支軸、４４ モータ、５０フレーム、５２ 台板、５４、５６ 脚部、５８ 台板、６０ 走行機構（移動機構）、６２ 多方向移動ホイール、７０Ａ、７０Ｂ 計測プリズム、１００ 墨出しシステム、１１０ ＬＲＦ、１１２ ターゲット、１１２Ｂ 円周面、１２０ トータルステーション、１２２ 視準器、１３０ ＰＣ、１３１ 入力部、１３２ 走行制御部、１３３ 記憶部、１３４ 位置調整制御部、１４０ 情報処理部、１４１ 位置計測部、１４２ 分別部、１４４ クラスタリング部、１４６ ノイズ除去部、１４８ 推定部、１４９ 判定部

Claims (6)

1. 位置決め対象物を目標位置に位置決めする位置決め方法であって、
   レーザレンジファインダを使用して、前記位置決め対象物を、その位置を計測しながら前記目標位置に向けて移動させる移動工程と、
   前記移動工程の後に、トータルステーションを使用して、前記位置決め対象物を、その位置を計測しながら前記目標位置に位置決めする位置決め工程と、
   を備えることを特徴とする位置決め方法。
2. 前記移動工程では、前記位置決め対象物を二次元平面内で位置決めする位置決め装置を、前記目標位置に向けて移動させ、
   前記位置決め工程では、前記位置決め装置により前記位置決め対象物を、前記目標位置に位置決めすることを特徴とする請求項１に記載の位置決め方法。
3. 前記移動工程は、
   前記位置決め装置に設けられた被計測体の表面上における複数の計測点の位置情報を、前記レーザレンジファインダから取得する情報取得工程と、
   前記レーザレンジファインダから取得した前記複数の計測点の位置情報に基づいて、前記被計測体の輪郭に対して所定の位置関係に設定された前記位置決め対象物の位置を推定する推定工程と、
   を備える請求項２に記載の位置決め方法。
4. 前記被計測体は円柱体であり、前記位置決め対象物の位置は、前記円柱体の中心軸の延長線上に設定されており、
   前記推定工程において、最小二乗法又は最小二乗法又は最尤法を用いて、前記複数の計測点の位置を近似した円の方程式を求め、求めた円の方程式に基づいて、前記中心軸上の点の位置を推定する請求項３に記載の位置決め方法。
5. 前記位置決め対象物は、墨出し器である請求項１から請求項４までの何れか１項に記載の位置決め方法。
6. 位置決め対象物を目標位置に位置決めする位置決めシステムであって、
   レーザレンジファインダを使用して計測される前記位置決め対象物の位置に基づいて、前記位置決め対象物を前記目標位置に向けて移動させる移動機構と、
   前記移動機構により前記位置決め対象物が移動された後に、トータルステーションを使用して計測される前記位置決め対象物の位置に基づいて、前記位置決め対象物を前記目標位置に位置決めする位置決め機構と、
   を備えることを特徴とする位置決めシステム。