JP7188196B2

JP7188196B2 - 作業車両及び作業車両の位置検出方法

Info

Publication number: JP7188196B2
Application number: JP2019045296A
Authority: JP
Inventors: 雅紀進藤; 聡一郎深町
Original assignee: Tadano Ltd
Current assignee: Tadano Ltd
Priority date: 2019-03-12
Filing date: 2019-03-12
Publication date: 2022-12-13
Anticipated expiration: 2039-03-12
Also published as: JP2020148564A

Description

本発明は、走行体上にブームを備えた作業車両及び作業車両の位置検出方法に関する。

従来より、走行体と、旋回可能に走行体に支持された旋回体と、伸縮及び起伏可能に旋回体に支持されたブームとを備えるクレーンが知られている。このクレーンはブームの先端位置等を監視しながら、旋回体を旋回させ、ブームを伸縮及び起伏させる。しかしながら、近年のクレーンの大型化に伴って、クレーンに設けられたセンサでブームの先端位置を正確に検出することが難しくなっている。

そこで、全地球航法衛星システム（ＧＮＳＳ：ＧｌｏｂａｌＮａｖｉｇａｔｉｏｎＳａｔｅｌｌｉｔｅＳｙｓｔｅｍ）を用いてブームの先端位置を検出する手法が提案されている。すなわち、クレーンは、ＧＮＳＳアンテナが受信した測位衛星からの測位信号をＧＮＳＳ受信機でＧＮＳＳアンテナの設置位置を示す位置信号に変換することにより、ブームの先端位置を検出することができる。例えば、特許文献１～３にはＧＮＳＳアンテナを取り付けた作業車両が開示されている。

特開２００１－９８５８５号公報特開２００４－１２５５８０号公報特開２００７－１４７５８８号公報

ブームの先端位置を測位衛星からの測位信号を使って制御するためには、車両を基準とする車両座標系と緯度、経度及び高度を基準とするＧＮＳＳ座標系の関係を決定し、座標変換を行う必要がある。つまり、ブームの先端位置をＧＮＳＳ座標系から車両座標系に変換するときには、変換係数として例えば旋回体の旋回中心の座標等が必要になる。この変換係数となる旋回中心の座標を求める際には、ＧＮＳＳアンテナにより測位衛星からの測位信号を取得するために、ブームを大きく旋回させる必要があるが、狭い作業現場では実行することができない。

本発明は、ブームを旋回させずに衛星からの測位信号を取得し、ブームの先端位置を測位座標系から車両座標系に変換するための変換係数を求めることができる作業車両及び作業車両の位置検出方法を提供することを目的とする。

本発明の解決しようとする課題は以上の如くであり、次にこの課題を解決するための手段を説明する。

即ち、本発明は、車両座標系における位置座標及び衛星による測位座標系における位置座標の一方を他方に変換するための変換係数を用いる作業車両であって、走行体と、前記走行体に旋回可能に支持される旋回体と、前記走行体上に配置されたブームと、前記ブームの先端部に設けられ、衛星からの電波を受信して測位信号を出力するアンテナと、前記アンテナから出力された測位信号を受信し、該測位信号に基づいて前記アンテナの設置位置を示す位置信号を出力する受信機と、前記受信機から出力される位置信号に基づいて前記ブームの先端部の位置を算出する制御部と、前記アンテナの車両座標系における位置座標を記憶する記憶部と、を備え、前記走行体が静止している状態において、前記制御部は、オペレータからの操作を受け付けて、単独操作で前記ブームの起伏操作を実行し、前記測位座標系における前記ブームの先端部の位置座標の軌跡データとなる点群を取得し、前記測位座標系において、前記点群を用いて主成分分析を実行することにより、当該点群に最も近くなる平面を求める第１算出処理と、前記測位座標系において、前記第１算出処理で求めた平面上に前記点群を写像し、写像された円弧から前記ブームの起伏支点となる位置座標を求める第２算出処理と、前記作業車両を水平設置した場合に、当該作業車両の水平面の法線ベクトルと、前記第１算出処理で求めた平面の法線ベクトルとの外積により、前方方向ベクトルを求める第３算出処理と、前記測位座標系において、前記前方方向ベクトルから前記ブームの起伏支点の位置座標を前記旋回体の旋回中心までオフセットして当該旋回中心を算出する第４算出処理と、前記前方方向ベクトルと前記旋回体の旋回角度から前記測位座標系と前記車両座標系のなす角度を求める第５算出処理と、を実行するものである。

前記測位座標系は、前記アンテナの設置位置を原点とするものである。

本発明は、車両座標系における位置座標及び衛星による測位座標系における位置座標の一方を他方に変換するための変換係数を用いる作業車両の位置検出方法であって、前記作業車両は、走行体と、前記走行体に旋回可能に支持される旋回体と、前記走行体上に配置されたブームと、前記ブームの先端部に設けられ、衛星からの電波を受信して測位信号を出力するアンテナと、前記アンテナから出力された測位信号を受信し、該測位信号に基づいて前記アンテナの設置位置を示す位置信号を出力する受信機と、前記受信機から出力される位置信号に基づいて前記ブームの先端部の位置を算出する制御部と、前記アンテナの車両座標系における位置座標を記憶する記憶部と、を備え、前記走行体が静止している状態において、前記制御部により単独操作で前記ブームの起伏操作を実行し、前記測位座標系における前記ブームの先端部の位置座標の軌跡データとなる点群を取得する工程と、前記測位座標系において、前記点群を用いて主成分分析を実行することにより、当該点群に最も近くなる平面を求める第１算出処理工程と、前記測位座標系において、前記第１算出処理工程で求めた平面上に前記点群を写像し、写像された円弧から前記ブームの起伏支点となる位置座標を求める第２算出処理工程と、前記作業車両を水平設置した場合に、当該作業車両の水平面の法線ベクトルと、前記第１算出処理工程で求めた平面の法線ベクトルとの外積により、前方方向ベクトルを求める第３算出処理工程と、前記測位座標系において、前記前方方向ベクトルから前記ブームの起伏支点の位置座標を前記旋回体の旋回中心までオフセットして当該旋回中心を算出する第４算出処理工程と、前記前方方向ベクトルと前記旋回体の旋回角度から前記測位座標系と前記車両座標系のなす角度を求める第５算出処理工程と、を有するものである。

本発明によれば、ブームを旋回させずに衛星からの測位信号を取得し、ブームの先端位置を測位座標系から車両座標系に変換する際の変換係数を求めることができる。よって、ブームが旋回できない狭い作業現場であっても、変換係数を容易に取得することができる。

走行時におけるクレーンを示す図である。吊上作業時におけるクレーンを示す図である。ＧＮＳＳアンテナの配置を説明するためのクレーンの模式平面図である。ブームの先端位置検出に関する制御系の構成を示す図である。変換係数の算出に関するクレーンの動作及び算出処理を示すフローチャートである。第２算出処理を説明するための説明図である。主成分分析について説明するための説明図である。第３算出処理を説明するための説明図である。第４算出処理を説明するための説明図である。第５算出処理を説明するための説明図である。

以下では作業車両としてラフテレーンクレーンを例に説明する。なお、作業車両としては、自走可能で且つブームを有していればよく、例えば、オールテレーンクレーン、カーゴクレーン、高所作業車等であってもよい。

＜クレーンの概要＞
まず、クレーン１について簡単に説明する。図１は、走行時におけるクレーン１を示す図である。図２は、吊上作業時におけるクレーン１を示す図である。クレーン１は、主に走行体２と旋回体３で構成されている。

走行体２は、左右一対のフロントタイヤ２１とリヤタイヤ２２を備えている。また、走行体２は、吊上作業を行う際に接地させて安定を図るアウトリガ２３を備えている。また、走行体２は、これらを駆動するためのアクチュエータに加え、エンジン２４やトランスミッションを備えている。また、走行体２は、旋回用モータ２５を備えている。旋回用モータ２５により、走行体２は、その上部に支持する旋回体３を旋回自在としている（図２の矢印Ｄ参照）。

旋回体３は、その後部から前方へ突き出すようにブーム３１を備えている。ブーム３１は、伸縮用シリンダ３２によって伸縮自在となっている（図２の矢印Ｅ参照）。また、ブーム３１は、起伏用シリンダ３３によって起伏自在となっている（図２の矢印Ｆ参照）。また、旋回体３はブーム３１の右方にキャビン３４を備えている。旋回体３は、走行体２に旋回可能に支持されている。
また、ブーム３１の先端部３１ａには、回転自在のシーブ３５が複数取り付けられており、このシーブ３５にはワイヤ（図示せず）が掛けられ、当該ワイヤの先端にはフックブロック３６を介してフック３７が取り付けられている。ワイヤの末端はウインチドラムに巻き掛けられている。

＜ブームの先端位置検出に関する構成＞
図３は、ＧＮＳＳアンテナ４１の配置を説明するためのクレーン１の模式平面図である。クレーン１は、ブーム３１の先端部３１ａに設けられたＧＮＳＳアンテナ４１を有している。
なお、本実施形態におけるブーム３１の先端部３１ａは、図１から図３に示すように、検出したい点であるシーブ３５の中心に設定している。

ＧＮＳＳアンテナ４１は全地球航法衛星システムで使用されるアンテナであり、図示しない測位衛星が出力する電波を受信し、受信電波に基づく測位信号を出力する。

図４は、ブームの先端位置検出に関する制御系の構成を示す図である。クレーン１は、制御部５１を備えている。制御部５１は、ブーム３１の伸縮動作や起伏動作のほか、様々な動作を制御できる。制御部５１は、例えばＣＰＵによって実現することができる。

制御部５１には、記憶部５２と、ＧＮＳＳ受信機５３と、情報取得部５４とが接続されている。ＧＮＳＳ受信機５３にはＧＮＳＳアンテナ４１が接続されている。

制御部５１は、ＧＮＳＳ受信機５３から出力される位置信号に基づいてブーム３１の先端部３１ａの位置を算出する。そして、制御部５１は算出したブーム３１の先端部３１ａの位置に基づいて、ブーム３１の伸縮動作や起伏動作を制御する。

記憶部５２は、ブーム３１の先端位置を検出するための種々のプログラムやデータ（車両を基準とする車両座標系等）を格納している。その１つとして、記憶部５２は後述する起伏操作におけるＧＮＳＳアンテナ４１の測位座標系（ＥＮＵ座標系ともいう）における位置座標（軌跡データ）を記憶する（図６参照）。車両座標系とは、クレーン１の任意の位置（例えば旋回体３の旋回中心）を原点とし、水平方向において互いに直交する方向に延びるｘ軸及びｙ軸と、鉛直上方に延びるｚ軸とで定義される仮想空間である。車両座標系における各座標は（ｘ，ｙ，ｚ）で表現される。

ＧＮＳＳ受信機５３は、ＧＮＳＳアンテナ４１から出力された測位信号を受信し、該測位信号に基づいてＧＮＳＳアンテナ４１の設置位置を示す位置信号を制御部５１へ出力する。

情報取得部５４は、クレーン１に設けられた各種センサや操作スイッチから、クレーン１に関する各種の情報を取得する。この情報としては、例えば、クレーン１が走行モードであるかブーム３１を使用する作業モードであるかを示すモード情報、クレーンの姿勢情報（ブーム３１の起伏角度、伸長長さ、旋回角度、アウトリガ２３の張り出し状態等を示す作業状態情報等）がある。情報取得部５４としては、例えば、クレーン１の過負荷防止装置（ＡＭＬ）等が挙げられる。情報取得部５４は、制御部５１にクレーンの姿勢情報を出力する。

また制御部５１には、操作部５６と、アウトリガ２３と、旋回用モータ２５と、伸縮用シリンダ３２と、起伏用シリンダ３３とが接続されている。操作部５６はキャビン３４に設けられ、オペレータからの操作を受け付けるものである。操作部５６は受け付けた操作に応じた操作信号を制御部５１へ出力する。操作部５６はクレーン１を操作するためのレバー、ハンドル、ペダル、操作パネル等を含んでいる。

制御部５１は操作部５６から出力される各種信号に基づいて、アウトリガ２３と、旋回用モータ２５と、伸縮用シリンダ３２と、起伏用シリンダ３３の動作を制御する。例えば、制御部５１は、アウトリガ２３を張出状態に移動させることを指示する張出操作、逆に格納状態に移動させることを指示する格納操作を受け付ける。また制御部５１は、旋回用モータ２５に旋回体３を旋回させることを指示する旋回操作、伸縮用シリンダ３２にブーム３１を伸長又は縮小させることを指示する伸長操作又は縮小操作、起伏用シリンダ３３にブーム３１を起仰又は倒伏させることを指示する起仰操作又は倒伏操作を受け付ける。

＜クレーンの位置検出方法：ブームの先端位置検出に関する動作及び算出処理＞
次に、クレーン１の位置検出方法について説明する。
具体的には、図５から図１０を参照してブーム３１の先端位置検出に関する動作及び算出処理に関する工程を説明する。ここでは、作業現場の作業位置にクレーン１を停止させ、走行体２及び旋回体３が静止している状態でブーム３１を起こす起伏操作をすることで変換係数を算出し、この変換係数を用いてブーム３１の先端部３１ａの位置を検出する方法について説明する。

図５は、変換係数の算出に関するクレーン１の動作及び算出処理を示すフローチャートである。まず、ステップＳ１０において、制御部５１はアウトリガ２３の接地が完了したか否かを判別する。具体的には、制御部５１は操作部５６から張出操作の指示を受け付け、アウトリガ２３の動作が終了したことをもってアウトリガ２３の接地が完了したと判別することができる。なお、アウトリガ２３の張出状態及び格納状態を検出するセンサを設け、制御部５１はこのセンサの検出信号からアウトリガ２３の接地が完了したか否かを判別してもよい。

アウトリガ２３の接地が完了すると、ステップＳ１０からステップＳ１１へ進んで、制御部５１は、操作部５６を介してオペレータからの操作を受け付けて、単独操作としてブーム３１を起こす起伏操作を実行する。

測位座標系とは、ＧＮＳＳにより決定される座標系のことであり、現在のＧＮＳＳアンテナ４１の設置位置を原点とし、原点から東方向に延びるＥ軸と、原点から北方向に延びるＮ軸と、原点から鉛直上方に延びるＵ軸とで定義される仮想空間であり、ＥＮＵ（Ｅａｓｔ－Ｎｏｒｔｈ－Ｕｐ）座標系である。ＥＮＵ座標系における各座標は（Ｅ，Ｎ，Ｕ）で表現される。以下では、測位座標系のことをＥＮＵ座標系とも呼ぶ。

ステップＳ１１の起伏操作において、制御部５１は、ＧＮＳＳ受信機５３からＧＮＳＳアンテナ４１の位置信号を受信する。すなわち、制御部５１は、ＧＮＳＳアンテナ４１の位置を原点とする測位座標系（ＥＮＵ座標系）において、起伏操作開始時から終了時及びブーム３１の移動時の複数点におけるＧＮＳＳアンテナ４１の位置座標Ｐｉ（Ｅｉ，Ｎｉ，Ｕｉ）をＧＮＳＳ受信機５３から出力される位置信号に基づいて取得する。位置座標Ｐｉ（Ｅｉ，Ｎｉ，Ｕｉ）は、ブーム３１を起こす起伏操作における軌跡データ（測位座標系におけるＧＮＳＳアンテナ４１の移動軌跡を示す複数の位置座標、本実施形態では点群データともいう）となる。制御部５１は、取得された軌跡データを記憶部５２に記憶する。

以下に説明するステップＳ１２～Ｓ１７の算出処理工程は、単独操作としてブーム３１の起伏操作によって取得された点群データ（軌跡データ）の点群から、主成分分析等を用いて変換係数（Ｅ_０，Ｎ_０，Ｕ_０）、回転角度α（図１０参照）を求める算出方法を具体的に示したものである。ステップＳ１２～Ｓ１７の算出処理工程では、制御部５１は、車両座標系における位置座標及び測位座標系における位置座標の一方を他方に変換するための変換係数（Ｅ_０，Ｎ_０，Ｕ_０）、回転角度αを算出する。そして、制御部５１は、算出した変換係数（Ｅ_０，Ｎ_０，Ｕ_０）、回転角度αを記憶部５２に記憶する。

詳細は後述するが、以下に説明するステップＳ１２～Ｓ１７では、以下の（１）～（５）の各処理が実行される。
（１）点群データから平面を求める。
（２）平面に写像される円弧から、当該円弧の中心点Ｑ（以下、単に点Ｑともいう）を求める。ここで、点Ｑは、ブーム３１の根元支点ピン軸（ブーム３１の起伏支点）と、上記（１）で求められた平面との交点の位置座標のことである。
（３）クレーン１の水平設置を前提として、クレーン１の水平面と上記（１）で求められた平面から外積をとり、前方方向のベクトルを取得する。
（４）前方方向ベクトルから、点Ｑをオフセットさせて変換係数として旋回中心Ｃを求める。
（５）前方方向ベクトルと旋回角度から、変換係数の一つである回転角度αを求める。

［第１算出処理：平面推定（点群データから平面を求める）］
次に、ステップＳ１１からステップＳ１２へ進んで、制御部５１は第１算出処理を実行する。第１算出処理では、制御部５１は、記憶部５２に記憶された起伏操作時の軌跡データである点群から、多変量解析の一例である主成分分析により最も点群に近くなる平面を求める。以下に、第１算出処理について詳細に説明する。

ステップＳ１１の起伏操作によって取得されたＮ個の点群データ座標（Ｅｉ，Ｎｉ，Ｕｉ）、ただしｉ＝１，２，．．．，ｎとする。これらの２乗距離が最小となる平面の方程式
ａＥ＋ｂＮ＋ｃＵ＋ｄ＝０
の係数ａ，ｂ，ｃ，ｄを求める。

次に、点群の重心を求める。
点群の重心の求め方としては、点群データ座標と二乗距離最小平面は点群の重心を通ることを利用する。
点群データの各座標軸の平均は以下のように表せる。

上記の平面の方程式を一般形で表すと、
ａ（Ｅ－Ｅ_０）＋ｂ（Ｎ－Ｎ_０）＋ｃ（Ｕ－Ｕ_０）＝０
ここで、（ａ，ｂ，ｃ）は法線ベクトルｎである。

とおくと、係数ｄは以下のように表せる。

続いて、係数ａ，ｂ，ｃを求める。
変換係数である点（Ｅ_０，Ｎ_０，Ｕ_０）と平面ａＥ＋ｂＮ＋ｃＵ＋ｄ＝０の距離は、以下のように表せる。

で表される。

これを最小化するａ，ｂ，ｃ，ｄを求めればよいが、分子、分母に係数があるために計算が困難となる。そこで、等式制約を加えることで分母を１にし、最大化関数を２次式の形にする。
等式制約は、ａ^２＋ｂ^２＋ｃ^２－１＝０
等式制約の極値問題はラグランジュの未定乗数法で解く。
点群データ座標（Ｅｉ，Ｎｉ，Ｕｉ）について、以下のように表す。

まとめると、以下のように表せる。

ここで、ｅはすべての点群データ点と平面の距離の２乗である。このとき、ラグランジュ関数Ｌは、以下のように表せる。

λは未定乗数、||ｎ||^２－１は等式制約ａ^２＋ｂ^２＋ｃ^２－１＝０を表す。

ｎに関するＬの偏微分方程式は、以下のようになる。

２Ｘ^ＴＸｎ－２λｎ＝０は固有値方程式の形式なので、ｎは行列Ｘ^ＴＸの固有ベクトルから得られる。行列Ｘ^ＴＸは半正値対象行列で、固有値はすべてゼロ以上の実数となる。
３×３行列の固有ベクトルなので、答えは３つ出てくるが、分散・共分散行列を使った主成分分析では、固有値（＝データの分散）の大きいものから主成分として選択していく。この場合、平面の法線ベクトル（ａ，ｂ，ｃ）としてふさわしいのはＸ^ＴＸの最小固有値に対応する固有ベクトルである。

ここで、一般的に知られているラグランジュの未定乗数法について説明すると、
ｎ次元の場合、
ｎの変数ｘ_１，ｘ_２，．．．，ｘ_ｎが束縛条件ｇ（ｘ_１，ｘ_２，．．．，ｘ_ｎ）＝０を満たしている場合に、関数ｆ（ｘ_１，ｘ_２，．．．，ｘ_ｎ）を最大化する（ｘ_１，ｘ_２，．．．，ｘ_ｎ）を求めたい。すなわち、
Ｌ（ｘ_１，ｘ_２，．．．，ｘ_ｎ，λ）＝ｆ（ｘ_１，ｘ_２，．．．，ｘ_ｎ）－λｇ（ｘ_１，ｘ_２，．．．，ｘ_ｎ）
で定義されるＬに対して、ｆが束縛条件ｇ＝０の元で最大化されるときは、以下の式が成立する。

上述した第１算出処理において実行した、平面を求める具体的な手順をまとめると、以下のようになる。
１．点群データの重心を計算する。
２．点群データの主成分分析（ＰＣＡ：ＰｒｉｎｃｉｐａｌＣｏｍｐｏｎｅｎｔＡｎａｌｙｓｉｓ）を行う。そして、ラグランジュの未定数乗数法を解き、固有値と固有ベクトルを求める。
３．固有ベクトルから平面の方程式の係数を求める。求めた固有値の中で最小のものに対応する固有ベクトルを法線ベクトル（係数ａ，ｂ，ｃ）とし、点群データの重心を通る平面を計算する（ｄが求まる）。

［第２算出処理：点Ｑの算出（点Ｑを求める）］
次に、ステップＳ１２からステップＳ１３へ進んで、制御部５１は第２算出処理を実行する。第２算出処理では、制御部５１は、ステップＳ１２で求めた平面上に起伏操作による軌跡データを写像し、その円弧から起伏操作の起伏支点であるブーム３１の根元支点ピン軸の位置座標である点Ｑを求める。以下に、第２算出処理について詳細に説明する。

制御部５１は、ステップＳ１２で求めた平面上に点群データを写像する。
すなわち、最小固有値に対応する固有ベクトル（ｎ）と行列Ｘで平面ＡＢ上に点群Ｐ_１，Ｐ_２，．．．，Ｐｎを写像する（図６の左側グラフ参照）。点群Ｐ_１，Ｐ_２，．．．，Ｐｎが写像された平面ＡＢにおける円弧から平面ＡＢ上にある中心点を円フィッティングによって求める（図６の右側グラフ参照）。この中心点が点Ｑとなる。次に、求めた平面ＡＢ上にある点をＥＮＵ座標上の点に戻す。

（主成分分析（ＰＣＡ）について）
本実施形態では、多変量解析である主成分分析を利用している。
本実施形態に係る主成分分析について説明する。
ＥＮＵ座標上の点Ｐｎ＝（Ｅｎ，Ｎｎ，Ｕｎ）、主成分分析で求めた第三主成分をｖ_３＝（α_３，β_３，γ_３）、主成分分析で求めた第二主成分をｖ_２＝（α_２，β_２，γ_２）、主成分分析で求めた第一主成分をｖ_１＝（α_１，β_１，γ_１）とおく。
主成分分析では分散の大きい軸から選択していくので、上のグラフから分散が一番小さい成分が求めたい平面の法線ベクトルとなる。２番目に分散が大きい成分は図７に示すグラフでは上下軸になる。分散が１番大きい成分は図７のグラフでの左右軸になる。
正射影ベクトルの公式から平面ＡＢ上のベクトルは以下で表される。

［第３算出処理：前方向ベクトルの算出（前方向のベクトルを求める）］
次に、ステップＳ１３からステップＳ１４へ進んで、制御部５１は第３算出処理を実行する。第３算出処理では、制御部５１は、クレーン１の水平設置を前提として、クレーン１の水平面とステップＳ１２で求めた平面から外積をとり、前方方向ベクトルを求める。以下に、第３算出処理について詳細に説明する。

制御部５１は、ステップ１２で求めた平面の法線ベクトルｎ＝（ａ，ｂ，ｃ）と、クレーン１の水平設置前提のＥＮ平面の法線ｍ＝（０，０，１）の外積Ｗが前方を示すベクトルとなる（図８参照）。

ここで、クレーン１の水平設置が前提ではなく、クレーン１のフレーム平面がブーム方向に傾斜している場合は、正しいブーム前方方向を求められない。そのため、旋回操作の単独操作を追加し、その軌跡からフレーム平面の法線ベクトルｍ´＝（ｄ，ｅ，ｆ）を求めて、ＥＮ平面の代わりとして外積を計算する。こうして、クレーン１のフレーム平面がブーム方向に傾斜している場合であっても、旋回操作の単独操作を追加することで、ブーム３１の方向を求めることができる。

外積の向きとしては、ＥＮＵ座標系が左手系なので、ＥＮ平面の法線方向はｚの方向、かつ起伏平面の法線方向は起伏方向から右側、となるように計算する。手順としては以下のように行う。
１．起伏動作の軌跡の始点と終点のベクトルを求める。
２．始点と終点の外積をとると、ｎの本来の方向ベクトルｎとなる。
３．ｎと方向ベクトルｎの内積からｃｏｓθを求める。
４．求めたｃｏｓθからθを求め、πと比較して０に近いと同じ方向、πに近いと逆方向とする。

［第４算出処理：旋回中心Ｃの算出（旋回中心Ｃを求める）］
次に、ステップＳ１４からステップＳ１５へ進んで、制御部５１は第４算出処理を実行する。第４算出処理では、制御部５１は、ステップＳ１４で求めた前方方向ベクトルから点Ｑを旋回中心の位置座標Ｃまでオフセットする。以下に、第４算出処理について詳細に説明する。

制御部５１は、ステップＳ１４で求めた前方を示すベクトルＷを使って、点Ｑをオフセットし、旋回中心Ｃを求める。
求める手順としては、図９に示すように、先ず、求めた平面の法線ベクトルｎ方向へオフセットし、Ｘ成分を平行移動させる。

続いて、前方方向ベクトルＷ方向へオフセットし、Ｙ成分を平行移動させる。

ベクトルｎ、Ｗの大きさは１に正規化されているため、そのままオフセット分定数倍すればよい。
オフセット量は、車両座標系（クレーン座標系）で表すと、前方へＸｏｆｆ、右側方へＹｏｆｆである。
こうして、制御部５１は、車両座標系における位置座標及び測位座標系（ＥＮＵ座標系）における位置座標の一方を他方に変換するための変換係数（回転角度を除く）である旋回中心Ｃを算出する。そして、制御部５１は、算出した変換係数（回転角度を除く）である旋回中心Ｃを記憶部５２に記憶する。

［第５算出処理：回転角度αの算出（回転角度αを求める）］
次に、ステップＳ１５からステップＳ１６へ進んで、制御部５１は第５算出処理を実行する。第５算出処理では、制御部５１は、ステップＳ１４で求めた前方方向ベクトルと旋回角度φからＥＮＵ座標系と車両座標系のなす角である回転角度αを求める。以下に、第５算出処理について詳細に説明する。

図１０は、変換係数である位置座標Ｃ（Ｅ_０，Ｎ_０，Ｕ_０）、回転角度αを用いた車両座標系及びＥＮＵ座標系の関係を示す図である。変換係数の角度成分である回転角度αは、車両座標系及びＥＮＵ座標系の対応する軸（ｘ軸とＥ軸、ｙ軸とＮ軸、ｚ軸とＵ軸）のなす角を示す。変換係数である位置座標Ｃ（Ｅ_０，Ｎ_０，Ｕ_０）は、車両座標系の原点と、ＥＮＵ座標系の原点との距離を示す。

制御部５１は、座標の回転角度αを方向ベクトルＷ＝（Ｅ_Ｗ，Ｎ_Ｗ，Ｕ_Ｗ）、旋回角度φを用いて以下のように算出される。

（旋回角度は右回りが＋のため、マイナスをつける）
こうして、制御部５１は、変換係数の角度成分である回転角度αを算出する。制御部５１は、算出した回転角度αを記憶部５２に記憶する。こうして、第４算出処理及び第５算出処理によって、変換係数である旋回中心の位置座標Ｃ（Ｅ_０，Ｎ_０，Ｕ_０）、回転角度αが求められる。

［変換処理：変換係数による座標系の変換処理］
次に、ステップＳ１６からステップＳ１７へ進んで、制御部５１は座標系の変換処理を実行する。制御部５１は、求めた変換係数でＥＮＵ座標におけるＧＮＳＳアンテナ４１の位置座標を車両座標系における位置座標に変換する。以下に、変換処理について詳細に説明する。

記憶部５２に読み込んだＥＮＵ座標系は、ＧＮＳＳアンテナ４１の先端位置がブーム３１の先端部３１ａから離間した分（オフセット分）に相当するアンテナオフセット（ｘｄ，ｙｄ，ｚｄ）を含んでいる。これを考慮して、以下の順でＥＮＵ座標系を車両座標系に変換する。
１．計測データ（Ｅｉ，Ｎｉ，Ｕｉ）から求めた旋回中心Ｃを引く。すなわち、旋回中心を原点にする。
２．回転行列で計測データをＸ軸方向へ向ける。
３．アンテナオフセット（ｘｄ，ｙｄ，ｚｄ）を計測データから引く。
以上のように、本実施形態に係るクレーン１の位置検出方法においては、クレーン１の位置検出に必要な変換係数である旋回中心の位置座標Ｃ（Ｅ_０，Ｎ_０，Ｕ_０）、回転角度αを算出することができる。

以上のように、本実施形態に係るクレーン１及びクレーン１の位置検出方法によれば、ブーム３１を旋回させずに起伏単独操作で衛星からの測位信号を取得し、ブーム３１の先端の位置座標をＥＮＵ座標系からクレーン１の車両座標系へ変換する変換係数として旋回中心の位置座標Ｃ（Ｅ_０，Ｎ_０，Ｕ_０）と、回転角度αを求めることができる。これにより、ブーム３１の先端位置及び姿勢を高精度に検出することができる。また、ブーム３１を旋回させずに起伏単独操作で衛星からの測位信号を取得するため、ブーム３１が旋回できない狭い作業現場であっても、変換係数を容易に取得することができる。また、クレーン１の作業準備時には、起伏動作が含まれるので、作業準備の際の起伏動作時に変換係数の取得が可能であり、別途起伏動作を行う必要がない。

また、ＧＮＳＳアンテナ４１はブーム３１の先端部３１ａに設けられているので、上記ステップＳ１７の変換処理によりブーム３１の先端部３１ａのＥＮＵ座標系における位置座標をクレーン１の車両座標系における位置座標へ変換することができる。図５の一連の動作を短時間に繰り返すことで、刻々と変化するブーム３１の先端部３１ａの位置を適時検出することができる。そして、制御部５１は、検出したブーム３１の先端部３１ａの位置に基づいてブーム３１の移動を精度良く制御することができる。

１クレーン（作業車両）
２走行体
３１ブーム
３１ａ先端部
４１ＧＮＳＳアンテナ
５１制御部
５２記憶部
５３ＧＮＳＳ受信機（受信機）
５６操作部

Claims

車両座標系における位置座標及び衛星による測位座標系における位置座標の一方を他方に変換するための変換係数を用いる作業車両であって、
走行体と、
前記走行体に旋回可能に支持される旋回体と、
前記走行体上に配置されたブームと、
前記ブームの先端部に設けられ、衛星からの電波を受信して測位信号を出力するアンテナと、
前記アンテナから出力された測位信号を受信し、該測位信号に基づいて前記アンテナの設置位置を示す位置信号を出力する受信機と、
前記受信機から出力される位置信号に基づいて前記ブームの先端部の位置を算出する制御部と、
前記アンテナの車両座標系における位置座標を記憶する記憶部と、を備え、
前記走行体が静止している状態において、前記制御部は、オペレータからの操作を受け付けて、単独操作で前記ブームの起伏操作を実行し、
前記測位座標系における前記ブームの先端部の位置座標の軌跡データとなる点群を取得し、
前記測位座標系において、前記点群を用いて主成分分析を実行することにより、当該点群に最も近くなる平面を求める第１算出処理と、
前記測位座標系において、前記第１算出処理で求めた平面上に前記点群を写像し、写像された円弧から前記ブームの起伏支点となる位置座標を求める第２算出処理と、
前記作業車両を水平設置した場合に、当該作業車両の水平面の法線ベクトルと、前記第１算出処理で求めた平面の法線ベクトルとの外積により、前方方向ベクトルを求める第３算出処理と、
前記測位座標系において、前記前方方向ベクトルから前記ブームの起伏支点の位置座標を前記旋回体の旋回中心までオフセットして当該旋回中心を算出する第４算出処理と、
前記前方方向ベクトルと前記旋回体の旋回角度から前記測位座標系と前記車両座標系のなす角度を求める第５算出処理と、を実行することを特徴とする作業車両。
前記測位座標系は、前記アンテナの設置位置を原点とすることを特徴とする請求項１に記載の作業車両。
車両座標系における位置座標及び衛星による測位座標系における位置座標の一方を他方に変換するための変換係数を用いる作業車両の位置検出方法であって、
前記作業車両は、
走行体と、
前記走行体に旋回可能に支持される旋回体と、
前記走行体上に配置されたブームと、
前記ブームの先端部に設けられ、衛星からの電波を受信して測位信号を出力するアンテナと、
前記アンテナから出力された測位信号を受信し、該測位信号に基づいて前記アンテナの設置位置を示す位置信号を出力する受信機と、
前記受信機から出力される位置信号に基づいて前記ブームの先端部の位置を算出する制御部と、
前記アンテナの車両座標系における位置座標を記憶する記憶部と、を備え、
前記走行体が静止している状態において、前記制御部により単独操作で前記ブームの起伏操作を実行し、前記測位座標系における前記ブームの先端部の位置座標の軌跡データとなる点群を取得する工程と、
前記測位座標系において、前記点群を用いて主成分分析を実行することにより、当該点群に最も近くなる平面を求める第１算出処理工程と、
前記測位座標系において、前記第１算出処理工程で求めた平面上に前記点群を写像し、写像された円弧から前記ブームの起伏支点となる位置座標を求める第２算出処理工程と、
前記作業車両を水平設置した場合に、当該作業車両の水平面の法線ベクトルと、前記第１算出処理工程で求めた平面の法線ベクトルとの外積により、前方方向ベクトルを求める第３算出処理工程と、
前記測位座標系において、前記前方方向ベクトルから前記ブームの起伏支点の位置座標を前記旋回体の旋回中心までオフセットして当該旋回中心を算出する第４算出処理工程と、
前記前方方向ベクトルと前記旋回体の旋回角度から前記測位座標系と前記車両座標系のなす角度を求める第５算出処理工程と、を有することを特徴とする作業車両の位置検出方法。