以下、本発明の各実施形態を図面に基づいて説明する。
[第1実施形態]
<作業機>
図1、図2、及び図3は、本発明の第1実施形態に係る位置決定装置80が適用される作業機10を示している。本実施形態では、作業機10が、バックホー10であるものとして説明する。なお、作業機10は、バックホーに限られず、可動操作具の先端を有する他の建設機械、農業機械等(ブルドーザ等)であってもよい。図1、図2、及び、図3は、バックホー10の側面図、バックホー10の正面図、及び、バックホー10の平面図である。バックホー10は、小型旋回型のオフセットブーム式バックホーである。以下、バックホー10の構成を詳述する。
バックホー10は、走行車両20、可動操作具30、車両位置検出装置40、第1回転度検出装置50、第2回転度検出装置60、及び、駆動制御装置70を備えている。位置決定装置80は、バックホー10に搭載されている。なお、駆動制御装置70、及び、位置決定装置80は、ECU(Electronic Control Unit)90の一部を構成している。平面視において、走行車両20の車幅方向に沿い、且つ、略水平な方向である第1方向K1が規定される。正面視において、走行車両20の車高方向に沿い、且つ、第1方向K1と直交する第2方向K2が規定される。また、第1方向K1、第2方向K2のそれぞれと直交する第3方向K3が規定される。
走行車両20は、上部旋回体21と、下部走行体22とを有する。上部旋回体21は、下部走行体22に対し、第2方向K2と平行な軸を中心として、旋回可能に搭載されている。上部旋回体21には、可動操作具30、車両位置検出装置40、及び、キャビン21aが備えられている。キャビン21aには、運転席、操作具21b(例えば、レバー、スイッチ、ペダル等、図7を参照)、表示装置21c(例えば、モニタ等、図7を参照)が設けられている。運転者が、キャビン21a内に着座した状態において、運転者の前・後、運転者の左・右、運転者の上・下に対応する方向を、それぞれ前方・後方、左方・右方、上方・下方とする。下部走行体22は、走行装置22aと、ドーザ装置22bとを備えている。走行装置22aは、クローラ式となるよう構成されている。ドーザ装置22bは、走行装置22aの前方に設けられている。上部旋回体21において、左側に寄るようにキャビン21aが配置され、可動操作具30はキャビン21aの右隣に配置されている。
図1に示すように、可動操作具30は、第1ブーム31、第2ブーム32、アーム33、及び、バケット34を備えている。また、可動操作具30は、第1ブームシリンダ36、第2ブームシリンダ37、アームシリンダ38、及び、バケットシリンダ39を備えている。これらのシリンダは、例えば、棒状の油圧シリンダ装置であり、運転者の操作により、一端から他端に向かう直線距離が変化するよう伸縮駆動が可能となっている。
第1ブーム31の後下側の端部は、第1ブーム支軸31aを介して、上部旋回体21の前方に支持されている。第1ブームシリンダ36の一端は、第1ブームシリンダ支軸36aを介して、上部旋回体21の前方で、第1ブーム支軸31aよりも下側に接続されている。第1ブームシリンダ36の他端は、第1ブームシリンダ支軸36bを介して、第1ブーム31を下方から支持している。第1ブーム支軸31a、及び、第1ブームシリンダ支軸36a,36bは、第1方向K1と平行な軸である。第1ブームシリンダ36の伸縮駆動により、第1ブーム31が、第1ブーム支軸31aを中心として回転可能となっている。
第2ブーム32の後側の端部は、第2ブーム支軸32aを介して、第1ブーム31の前上側の端部に支持されている。第2ブームシリンダ37の一端は、第2ブームシリンダ支軸37aを介して、第1ブーム31の左側に接続されている。第2ブームシリンダ37の他端は、第2ブームシリンダ支軸37bを介して、第2ブーム32の左側に接続されている。また、第2ブーム32の左側には、第2ブームシリンダ37に並行して、オフセットリンク32bが設けられている。オフセットリンク32bの一端および他端は、それぞれ第1ブーム31及び第2ブーム32に接続されている。第2ブーム支軸32a、及び、第2ブームシリンダ支軸37a,37bは、第2方向K2と平行な軸である。第2ブームシリンダ37の伸縮駆動により、第2ブーム32が、第2ブーム支軸32aを中心として回転可能となっている。
アーム33の上後側の端部は、アーム支軸33aを介して、第2ブーム32の前側の端部に支持されている。アームシリンダ38の一端は、アームシリンダ支軸38aを介して、アーム支軸33aよりも上後方で、第2ブーム32の上側に接続されている。アームシリンダ38の他端は、アームシリンダ支軸38bを介して、アーム支軸33aよりも上前方で、アーム33を上方から支持している。アーム支軸33a、及び、アームシリンダ支軸38a,38bは、第1方向K1と平行な軸である。アームシリンダ38の伸縮駆動により、アーム33が、アーム支軸33aを中心として回転可能となっている。
バケット34の端部は、バケット支軸34aを介して、アーム33の下端部に支持されている。バケットシリンダ39の一端は、バケットシリンダ支軸39aを介して、アーム支軸33aよりも前方で、アーム33の上側に接続されている。バケットシリンダ39の他端は、バケットシリンダ支軸39bを介して、バケット支軸34aよりも下方で、バケット34を支持している。バケット支軸34a、及び、バケットシリンダ支軸39a,39bは、第1方向K1と平行な軸である。バケットシリンダ39の伸縮駆動により、バケット34が、バケット支軸34aを中心として回転可能となっている。
また、バケット34は、先端部位34bを備えている。先端部位34bは、バケット34の回転径方向における最外部位に相当する。このように、可動操作具30は、走行車両20から前方に離間する先端部位34bに向かって延びるよう、構成されている。
第1ブーム31、アーム33、及び、バケット34のうち、何れか1つ、2つ、又は、全てが回転することで、図2の破線にて示すように、バケット34の先端部位34bは、第2方向K2の移動(上方向又は下方向)が可能となる。これと同時に、バケット34の先端部位34bは、第3方向K3の移動(前方向又は後方向)も可能となる。第1ブーム31、アーム33、及び、バケット34は、第1回転部位に相当する。
第2ブーム32が回転することで、図2及び図3の一点鎖線にて示すように、バケット34の先端部位34bは、第1方向K1の移動(左方向又は右方向)が可能となる。これと同時に、バケット34の先端部位34bは、第3方向K3の移動(前方向又は後方向)も可能となる。このように、第2ブーム32の回転により、バケット34のオフセット移動が可能となる。第2ブーム32は、第2回転部位に相当する。
図1、及び、図7に示すように、車両位置検出装置40は、走行車両20の上部旋回体21に、搭載されている。搭載位置(所定位置に相当)としては、例えば、上部旋回体21において、キャビン21aのルーフ上や、キャビン21aの後方等に、車両位置検出装置40が設けられてもよい。なお、本実施形態では説明を簡易化するため、車両位置検出装置40が、キャビン21aの後方に搭載されているものとする。
車両位置検出装置40は、全球測位衛星からの測位用信号の受信に基づいて、走行車両20の位置として上記搭載位置を検出する。この検出信号は、位置決定装置80に向けて送出されるようになっている。車両位置検出装置40は、例えば、GNSS等の衛星測位システムにより位置検出が可能なものであってもよい。本実施形態では、2周波L1,L2を利用して、車両位置検出装置40の搭載位置に対応する絶対座標が、決定されるようになっている。以下、車両位置検出装置40を、GNSS40と称呼する場合もある。なお、2周波L1,L2に代えて、2周波L1,L5であってもよいし、2周波以上のマルチ周波が用いられてもよい。
第1回転度検出装置50は、走行車両20の上部旋回体21、第1ブーム31、アーム33、及び、バケット34の4か所に、それぞれ1つずつ搭載されている。これら4つの第1回転度検出装置50は、上記4つの部位における、第1方向K1と平行な軸を中心とする回転度合いをそれぞれ検出する。これらの検出信号は、位置決定装置80に向けてそれぞれ送出されるようになっている。第1回転度検出装置50は、例えば、IMU(Inertial Measurement Unit)等の回転角検出が可能なものであってもよい。以下、第1回転度検出装置50を、IMU50と称呼する場合もある。また、走行車両20の上部旋回体21、第1ブーム31、アーム33、及び、バケット34にそれぞれ搭載される第1回転度検出装置50を、IMU51、IMU52、IMU53、IMU54、と称呼する場合もある。
図1、図4、図5、及び、図6に示すように、第2回転度検出装置60は、第1ブーム31、及び、第2ブーム支軸32aのそれぞれに跨りつつ、1つ搭載されている。第2回転度検出装置60は、第2ブーム32における、第2方向K2と平行な軸を中心とする回転度合いを検出する。この検出信号は、位置決定装置80に向けて送出されるようになっている。第2回転度検出装置60は、例えば、ロータリエンコーダ等の回転角検出が可能なものであってもよい。以下、第2回転度検出装置60を、RENC60と称呼する場合もある。
図4に示すように、RENC60は、スリットディスク61、シャフト62、ケーシング63、光源64、及び、受光部65を備えている。スリットディスク61は、円盤状に構成され、円周方向に並ぶ多数の開口スリット61aを有している。シャフト62は、スリットディスク61の中心から、スリットディスク61の面に対し垂直に伸長している。スリットディスク61は、シャフト62と一体回転可能となっている。ケーシング63は、円筒形状の筐体であり、スリットディスク61、及び、シャフト62と同軸的に、これらを内蔵する。ケーシング63は、固定面63aの中心部に開口63bを有している。シャフト62は、開口63bを介して摺動可能に支持されつつ、ケーシング63の内部から外側に向けて、その一部が露出している。これにより、シャフト62(スリットディスク61)と、ケーシング63とは、互いに相対回転するようになっている。
ケーシング63内壁には、光源64、及び、受光部65が、それぞれ離間するように固定されている。スリットディスク61の円周近傍は、光源64、及び、受光部65の間に介装される。光源64から受光部65へ向かう光の方向は、シャフト62の伸長方向と平行となっている。シャフト62及びケーシング63が相対回転した場合に、開口スリット61aにおける光の通過・遮断が、受光部65により交互に検出される。この検出信号は、位置決定装置80に向けて送出されるようになっている。この検出信号に基づいて、シャフト62(スリットディスク61)に対するケーシング63の回転角が取得可能となっている。ここにおいて、シャフト62が軸部材に相当し、ケーシング63が回転部材に相当する。
図5、及び、図6に示すように、第1ブーム31の第2ブーム32側の前側端部において、上下方向に抜ける篏合孔31bが設けられている。第2ブーム32の第1ブーム31側の後側端部には、第2ブーム支軸32aが上方に突出するように固定されている。第2ブーム32の後側端部の一部は、第1ブーム31の前側端部にて覆われつつ、第2ブーム支軸32aが、篏合孔31bに摺動可能に篏合している。これにより、第2ブーム32は、第2ブーム支軸32aを中心として一体的に回転する。換言すると、第1ブーム31は、第2ブーム32の回転に伴って、第2ブーム支軸32aに対して相対回転する部位となる(図3を参照)。
第1ブーム31の篏合孔31bまわりには、環状の固定部位31cが設けられている。篏合孔31bに第2ブーム支軸32aが篏合された場合に、固定部位31cは、第2ブーム支軸32aを囲むように構成されている。また、第2ブーム支軸32a及び固定部位31cの上側頂面が、互いに略面一となっている。第2ブーム支軸32aの上側頂面には、中心にて下方に凹む凹部位32a1が設けられている。凹部位32a1に、RENC60のシャフト62を先端から侵入させて、シャフト62及び第2ブーム支軸32aを同軸的に固定させる。即ち、第2ブーム支軸32a及びシャフト62は、第2方向K2と平行となる。シャフト62の固定とともに、第1ブーム31の固定部位31cに、RENC60の固定面63aを固定させる。
これにより、第2ブーム32が、第2ブーム支軸32aを中心として回転した場合、RENC60のシャフト62に対してケーシング63も、同じ回転角をもって相対回転する。従って、RENC60の検出信号に基づいて、第2ブーム32の回転角を取得可能となっている。
図1、及び、図7に示すように、ECU90は、駆動制御装置70、及び、位置決定装置80で構成されている。駆動制御装置70、及び、位置決定装置80は、CPU、電気回路等で構成されており、上記各種装置等と有線又は無線形式にて電気的に接続されている。具体的には、駆動制御装置70は、操作具21b、可動操作具30、及び、図示しない原動機、変速機等の動力伝達系と接続されている。位置決定装置80は、GNSS40、IMU50、RECN60、及び、表示装置21cと接続されている。
駆動制御装置70は、運転者により操作される操作具21bの操作信号を受信する。駆動制御装置70は、当該操作信号に応じて、可動操作具30の第1ブームシリンダ36、第2ブームシリンダ37、アームシリンダ38、及び、バケットシリンダ39の伸縮駆動を制御する。これにより、第1ブーム31、第2ブーム32、アーム33、及び、バケット34の各支軸を中心とした各回転作動が制御されるようになっている。なお、駆動制御装置70は、図示しない原動機、変速機等の動力伝達系も制御し、走行装置22aの走行制御も実行するようになっている。
位置決定装置80は、車両位置測定手段81、車両回転度取得手段82、第1回転度取得手段83、第2回転度取得手段84、及び、位置決定手段85を備えている。車両位置測定手段81は、GNSS40と接続されて、GNSS40により検出された位置に対応する信号を受信する。車両位置測定手段81は、当該信号に基づいて、走行車両20の位置を測定する。測定される位置としては、例えば、3次元座標等であってもよい。車両回転度取得手段82は、IMU51と接続されて、IMU51により検出された回転角に対応する信号を受信する。車両回転度取得手段82は、当該信号に基づいて、走行車両20の回転角を取得する。
第1回転度取得手段83は、IMU52,53,54とそれぞれ接続されて、IMU52,53,54により検出され送出された回転角に対応する信号を受信する。第1回転度取得手段83は、当該信号に基づいて、第1ブーム31、アーム33、及び、バケット34の回転角をそれぞれ取得する。第2回転度取得手段84は、RECN60と接続されて、RECN60により検出され送出された回転角に対応する信号を受信する。第2回転度取得手段84は、当該信号に基づいて、第2ブーム32の回転角を取得する。
位置決定手段85は、上記測定された走行車両20の位置と、上記取得された走行車両20の回転角と、上記取得された第1ブーム31、アーム33、及び、バケット34の回転角と、上記取得された第2ブーム32の回転角と、に基づいて、可動操作具30のバケット34における先端部位34bの位置を決定する。決定される位置としては、例えば、3次元座標等であってもよい。具体的な位置の決定プロセスについては、後に詳述する。位置決定手段85は、決定された先端部位34bの位置情報を、表示装置21cにて表示する。例えば、表示装置21cのモニタにて、圃場における走行車両20近傍の地図上に、先端部位34bのマークを重ねて表示してもよい。これにより、バケット34の先端部位34bの位置を表示装置21cにて視認でき、運転者の操作をナビゲートすることが可能となる。
<位置の決定プロセス>
図8の破線は、バックホー10が基準となる姿勢、位置にて静止している状態を示している。この状態を基準状態と称呼する。基準状態においては、第2ブーム32は、第1ブーム31の先端部から、第3方向K3に沿って前方へ伸長している。即ち、基準状態では、第2ブーム32は、オフセット移動していない。
位置決定装置80は、先ず、基準状態における部位の座標である基準座標を決定する。基準状態において、GNSS40の搭載位置に対応する基準座標を(Xs0,Ys0,Zs0)とする。GNSS40の基準座標(Xs0,Ys0,Zs0)は、GNSS40にて測位されたものである。なお、座標(L,M,N)の表記において、「L」は第3方向K3、「M」は第1方向K1、「N」は第2方向K2の成分を表している。
基準状態において、GNSS40の搭載位置から第1ブーム支軸31aの位置までの離間距離及び方向が既知である。上記測位されたGNSS40の基準座標(Xs0,Ys0,Zs0)と、既知の離間距離及び方向とに基づいて、第1ブーム支軸31aの位置に対応する基準座標(Xs1,Ys1,Zs1)が決定される。
基準状態において、第1ブーム支軸31aの位置からアーム支軸33aの位置までの離間距離及び方向が既知である。上記決定された第1ブーム支軸31aの基準座標(Xs1,Ys1,Zs1)と、既知の離間距離及び方向とに基づいて、アーム支軸33aの位置に対応する基準座標(Xs2,Ys2,Zs2)が決定される。
基準状態において、アーム支軸33aの位置からバケット支軸34aの位置までの離間距離及び方向が既知である。上記決定されたアーム支軸33aの基準座標(Xs2,Ys2,Zs2)と、既知の離間距離及び方向とに基づいて、バケット支軸34aの位置に対応する基準座標(Xs3,Ys3,Zs3)が決定される。
基準状態において、バケット支軸34aの位置から先端部位34bの位置までの離間距離及び方向が既知である。上記決定されたバケット支軸34aの基準座標(Xs3,Ys3,Zs3)と、既知の離間距離及び方向とに基づいて、先端部位34bの位置に対応する基準座標(Xs4,Ys4,Zs4)が決定される。
上述のように決定された基準座標(Xs0,Ys0,Zs0)、(Xs1,Ys1,Zs1)、(Xs2,Ys2,Zs2)、(Xs3,Ys3,Zs3)、(Xs4,Ys4,Zs4)は、バックホー10の記憶装置に格納されて、必要な演算処理の際、読み出し可能とされる。
次に、基準状態であったバックホー10が作動開始すると、位置決定装置80は、所定のタイミング毎に作動中の部位の座標を決定する。
図9に示すように、バックホー10の作動により、GNSS40の搭載位置に対応する座標が、基準座標(Xs0,Ys0,Zs0)から(X0,Y0,Z0)に移動したものとする。バックホー10の作動により、走行車両20が、第1方向K1の軸を中心として回転角A0だけ基準状態から回転したものとする。GNSS40の搭載位置の移動、及び、走行車両20の回転に応じて、第1ブーム支軸31aの位置に対応する座標が、基準座標(Xs1,Ys1,Zs1)から(X1,Y1,Z1)に移動したものとする。GNSS40の座標(X0,Y0,Z0)は、GNSS40にて測位されたものである。走行車両20の回転角A0は、IMU51にて検出されたものである。
GNSS40の基準座標(Xs0,Ys0,Zs0)から、第1ブーム支軸31aの基準座標(Xs1,Ys1,Zs1)に向かうベクトル(Xs1―Xs0,Ys1―Ys0,Zs1―Zs0)が既知である。上記測位されたGNSS40の座標(X0,Y0,Z0)と、既知の上記ベクトルと、上記検出された回転角A0とに基づいて、第1ブーム支軸31aの位置に対応する座標(X1,Y1,Z1)が決定される。具体的には、下記(1)式、(2)式、(3)式に示すように、座標(X0,Y0,Z0)及び上記ベクトルの和と、回転角A0の回転行列とを用いて、座標(X1,Y1,Z1)が決定される。なお、下記(1)式、(2)式、(3)式を用いるのに代えて、下記(1a)式、(2a)式、(3a)式を用いて、第1ブーム支軸31aの位置に対応する座標(X1,Y1,Z1)を決定すると、精度向上の観点でより好適である。
X1=(X0+(Xs1―Xs0))(cos(A0))+
(Z0+(Zs1―Zs0))(sin(A0)) …(1)
Y1=(Y0+(Ys1―Ys0)) …(2)
Z1=(X0+(Xs1―Xs0))(―sin(A0))+
(Z0+(Zs1―Zs0))(cos(A0)) …(3)
X1=X0+(Xs1―Xs0)(cos(A0))+
(Zs1―Zs0)(sin(A0)) …(1a)
Y1=Y0+(Ys1―Ys0) …(2a)
Z1=X0+(Xs1―Xs0)(―sin(A0))+
(Zs1―Zs0)(cos(A0)) …(3a)
図10に示すように、バックホー10の作動により、第1ブーム支軸31aの位置に対応する座標が、基準座標(Xs1,Ys1,Zs1)から(X1,Y1,Z1)に移動したものとする。バックホー10の作動により、第1ブーム31が、第1方向K1の軸を中心として回転角A1だけ基準状態から回転したものとする。第1ブーム支軸31aの位置の移動、及び、第1ブーム31の回転に応じて、アーム支軸33aの位置に対応する座標が、基準座標(Xs2,Ys2,Zs2)から(X2,Y2,Z2)に移動したものとする。第1ブーム31の回転角A1は、IMU52にて検出されたものである。
第1ブーム支軸31aの基準座標(Xs1,Ys1,Zs1)から、アーム支軸33aの基準座標(Xs2,Ys2,Zs2)に向かうベクトル(Xs2―Xs1,Ys2―Ys1,Zs2―Zs1)が既知である。上記決定された第1ブーム支軸31aの座標(X1,Y1,Z1)と、既知の上記ベクトルと、上記検出された回転角A1とに基づいて、アーム支軸33aの位置に対応する座標(X2,Y2,Z2)が決定される。具体的には、下記(4)式、(5)式、(6)式に示すように、座標(X1,Y1,Z1)及び上記ベクトルの和と、回転角A1の回転行列とを用いて、座標(X2,Y2,Z2)が決定される。なお、第1ブーム支軸31aの座標(X1,Y1,Z1)を決定する際、上記(1a)式、(2a)式、(3a)式を用いた場合には、下記(4)式、(5)式、(6)式を用いるのに代えて、下記(4a)式、(5a)式、(6a)式を用いて、アーム支軸33aの位置に対応する座標(X2,Y2,Z2)を決定すると、精度向上の観点でより好適である。
X2=(X1+(Xs2―Xs1))(cos(A1))+
(Z1+(Zs2―Zs1))(sin(A1)) …(4)
Y2=(Y1+(Ys2―Ys1)) …(5)
Z2=(X1+(Xs2―Xs1))(―sin(A1))+
(Z1+(Zs2―Zs1))(cos(A1)) …(6)
X2=X1+(Xs2―Xs1)(cos(A1))+
(Zs2―Zs1)(sin(A1)) …(4a)
Y2=Y1+(Ys2―Ys1) …(5a)
Z2=X1+(Xs2―Xs1)(―sin(A1))+
(Zs2―Zs1)(cos(A1)) …(6a)
図11に示すように、バックホー10の作動により、アーム支軸33aの位置に対応する座標が、基準座標(Xs2,Ys2,Zs2)から(X2,Y2,Z2)に移動したものとする。バックホー10の作動により、アーム33が、第1方向K1の軸を中心として回転角A2だけ基準状態から回転したものとする。アーム支軸33aの位置の移動、及び、アーム33の回転に応じて、バケット支軸34aの位置に対応する座標が、基準座標(Xs3,Ys3,Zs3)から(X3,Y3,Z3)に移動したものとする。アーム33の回転角A2は、IMU53にて検出されたものである。
アーム支軸33aの基準座標(Xs2,Ys2,Zs2)から、バケット支軸34aの基準座標(Xs3,Ys3,Zs3)に向かうベクトル(Xs3―Xs2,Ys3―Ys2,Zs3―Zs2)が既知である。上記決定されたアーム支軸33aの座標(X2,Y2,Z2)と、既知の上記ベクトルと、上記検出された回転角A2とに基づいて、バケット支軸34aの位置に対応する座標(X3,Y3,Z3)が決定される。具体的には、下記(7)式、(8)式、(9)式に示すように、座標(X2,Y2,Z2)及び上記ベクトルの和と、回転角A2の回転行列とを用いて、座標(X3,Y3,Z3)が決定される。なお、アーム支軸33aの座標(X2,Y2,Z2)を決定する際、上記(4a)式、(5a)式、(6a)式を用いた場合には、下記(7)式、(8)式、(9)式を用いるのに代えて、下記(7a)式、(8a)式、(9a)式を用いて、バケット支軸34aの位置に対応する座標(X3,Y3,Z3)を決定すると、精度向上の観点でより好適である。
X3=(X2+(Xs3―Xs2))(cos(A2))+
(Z2+(Zs3―Zs2))(sin(A2)) …(7)
Y3=(Y2+(Ys3―Ys2)) …(8)
Z3=(X2+(Xs3―Xs2))(―sin(A2))+
(Z2+(Zs3―Zs2))(cos(A2)) …(9)
X3=X2+(Xs3―Xs2)(cos(A2))+
(Zs3―Zs2)(sin(A2)) …(7a)
Y3=Y2+(Ys3―Ys2) …(8a)
Z3=X2+(Xs3―Xs2)(―sin(A2))+
(Zs3―Zs2)(cos(A2)) …(9a)
図12に示すように、バックホー10の作動により、バケット支軸34aの位置に対応する座標が、基準座標(Xs3,Ys3,Zs3)から(X3,Y3,Z3)に移動したものとする。バックホー10の作動により、バケット34が、第1方向K1の軸を中心として回転角A3だけ基準状態から回転したものとする。バケット支軸34aの位置の移動、及び、バケット34の回転に応じて、先端部位34bの位置に対応する座標が、基準座標(Xs4,Ys4,Zs4)から(X4,Y4,Z4)に移動したものとする。バケット34の回転角A3は、IMU54にて検出されたものである。
バケット支軸34aの基準座標(Xs3,Ys3,Zs3)から、先端部位34bの基準座標(Xs4,Ys4,Zs4)に向かうベクトル(Xs4―Xs3,Ys4―Ys3,Zs4―Zs3)が既知である。上記決定されたバケット支軸34aの座標(X3,Y3,Z3)と、既知の上記ベクトルと、上記検出された回転角A3とに基づいて、先端部位34bの位置に対応する座標(X4,Y4,Z4)が決定される。具体的には、下記(10)式、(11)式、(12)式に示すように、座標(X3,Y3,Z3)及び上記ベクトルの和と、回転角A3の回転行列とを用いて、座標(X4,Y4,Z4)が決定される。なお、バケット支軸34aの座標(X3,Y3,Z3)を決定する際、上記(7a)式、(8a)式、(9a)式を用いた場合には、下記(10)式、(11)式、(12)式を用いるのに代えて、下記(10a)式、(11a)式、(12a)式を用いて、先端部位34bの位置に対応する座標(X4,Y4,Z4)を決定すると、精度向上の観点でより好適である。
X4=(X3+(Xs4―Xs3))(cos(A3))+
(Z3+(Zs4―Zs3))(sin(A3)) …(10)
Y4=(Y3+(Ys4―Ys3)) …(11)
Z4=(X3+(Xs4―Xs3))(―sin(A3))+
(Z3+(Zs4―Zs3))(cos(A3)) …(12)
X4=X3+(Xs4―Xs3)(cos(A3))+
(Zs4―Zs3)(sin(A3)) …(10a)
Y4=Y3+(Ys4―Ys3) …(11a)
Z4=X3+(Xs4―Xs3)(―sin(A3))+
(Zs4―Zs3)(cos(A3)) …(12a)
図13に示すように、バックホー10の作動により、第2ブーム32が、第2ブーム支軸32aを中心として回転角A4だけ基準状態から回転したものとする。第2ブーム32の回転に応じて、先端部位34bの位置に対応する座標が、(X4,Y4,Z4)から(X,Y,Z)に移動したものとする。即ち、当該移動は、第2ブーム32の回転に応じた、左方又は右方のオフセット移動に相当する。第2ブーム32の回転角A4は、RENC60にて検出されたものである。右方のオフセット移動における回転角A4を正の値とした場合、左方のオフセット移動における回転角A4を、負の値としてもよい。また、回転角A4の正負をその逆としてもよい。
上記決定された先端部位34bの座標(X4,Y4,Z4)と、上記検出された回転角A4とに基づいて、第2ブーム32が回転する場合の先端部位34bの位置に対応する座標(X,Y,Z)が決定される。具体的には、下記(13)式、(14)式、(15)式に示すように、座標(X4,Y4,Z4)と、回転角A4の回転行列とを用いて、座標(X,Y,Z)が決定される。なお、先端部位34bの座標(X4,Y4,Z4)を決定する際、上記(10a)式、(11a)式、(12a)式を用いた場合には、下記(13)式、(14)式、(15)式を用いるのに代えて、下記(13a)式、(14a)式、(15a)式を用いて、第2ブーム32が回転する場合の先端部位34bの位置に対応する座標(X,Y,Z)を決定すると、精度向上の観点でより好適である。ここにおいて、下記(13a)式、(14a)式、(15a)式におけるX5、Y5は、下記(16a)式、(17a)式にてそれぞれ決定される第2ブーム支軸32aの位置に対応する座標に対応している(図23を参照)。
X=X4(cos(A4))―Y4(sin(A4)) …(13)
Y=X4(sin(A4))+Y4(cos(A4)) …(14)
Z=Z4 …(15)
X=X5+(X4―X5)(cos(A4))―
(Y4―Y5)(sin(A4)) …(13a)
Y=Y5+(X4―X5)(sin(A4))+
(Y4―Y5)(cos(A4)) …(14a)
Z=Z4 …(15a)
以上のようにして、位置決定装置80により、バケット34の先端部位34bの位置に対応する座標(X,Y,Z)が決定される。
<実際の作動>
位置決定装置80の実際の作動について、図14のフローチャートを参照しつつ説明する。位置決定装置80により、バケット34の先端部位34bの位置を決定し、当該位置を表示する場合、先ず、ステップST1にて、位置決定装置80は、基準座標を決定する。位置決定装置80の車両位置測定手段81により、バックホー10が基準状態にて静止しているときに、GNSS40の搭載位置に対応する基準座標(Xs0,Ys0,Zs0)が測位される。位置決定装置80の位置決定手段85により、バックホー10が基準状態にて静止しているときに、第1ブーム支軸31a、アーム支軸33a、バケット支軸34a、及び、先端部位34bのそれぞれの位置に対応する基準座標(Xs1,Ys1,Zs1)、(Xs2,Ys2,Zs2)、(Xs3,Ys3,Zs3)、(Xs4,Ys4,Zs4)が、それぞれ決定される。(図8を参照)。
次に、ステップST2にて、位置決定装置80は、GNSS40の座標を測位する。位置決定装置80の車両位置測定手段81により、バックホー10が作動している場合に、GNSS40の搭載位置に対応する座標(X0,Y0,Z0)が測位される。
次に、ステップST3にて、位置決定装置80は、回転角A0,A1,A2,A3,A4を取得する。位置決定装置80の車両回転度取得手段82により、バックホー10が作動している場合に、走行車両20の回転角A0が取得される。位置決定装置80の第1回転度取得手段83により、バックホー10が作動している場合に、第1ブーム31、アーム33、及び、バケット34の回転角A1,A2,A3が、それぞれ取得される。位置決定装置80の第2回転度取得手段84により、バックホー10が作動している場合に、第2ブーム32の回転角A4が、それぞれ取得される。
次に、ステップST4にて、位置決定装置80は、先端部位34bの座標を決定する。位置決定装置80の位置決定手段85により、バックホー10が作動している場合に、ステップST1にて決定された基準座標、ステップST2にて測位されたGNSS40の座標、ステップST3にて取得された回転角A0,A1,A2,A3,A4、及び、上記(1)~(15)式(好ましくは、上記(1a)~(15a)式)を用いて、バケット34の先端部位34bの位置に対応する座標(X,Y,Z)が決定される(図9~図13を参照)。
そして、ステップST5にて、位置決定装置80は、先端部位34bの位置を表示する。位置決定装置80の位置決定手段85により、ステップST4にて決定された座標(X,Y,Z)の位置が、表示装置21cに表示される(図7を参照)。
<実施形態の効果>
以上説明したように、本発明の第1実施形態に係る位置決定装置80によれば、車両位置測定手段81により、走行車両20のGNSS40の搭載位置が測位される。車両回転度取得手段82により、走行車両20の回転角A0が取得される。第1回転度取得手段83により、第1回転部位(第1ブーム31、アーム33、バケット34)の回転角A1,A2,A3が取得される。第2回転度取得手段84により、第2回転部位(第2ブーム32)の回転角A4が取得される。位置決定手段85により、測定されたGNSS40の搭載位置と、取得された回転角A0,A1,A2,A3,A4と、に基づいて、バケット34の先端部位34bの位置が決定される。
従って、作業機10(バックホー10)の作動に応じた、走行車両20の位置移動、走行車両20の回転、及び、第1回転部位(第1ブーム31、アーム33、バケット34)の回転を反映して、バケット34の先端部位34bの位置を決定できる。更に、作業機10(バックホー10)の作動に応じた、第2回転部位(第2ブーム32)の回転も反映して、バケット34の先端部位34bの位置を決定できる。このため、先端部位34bが第1方向K1にオフセット移動する場合であっても、先端部位34bの位置を精度良く決定できる。決定された先端部位34bの位置を、例えば、表示装置21c等に表示させることで、運転者に可動操作具30の操作を、適切にナビゲートできる。
本発明の第1実施形態に係る位置決定装置80では、特に、第2回転度検出装置60(RENC60)がバックホー10に備えられる。RENC60は、シャフト62、及び、シャフト62と相対回転可能なケーシング63を備えている。シャフト62は、第2ブーム支軸32aに同軸的に固定される。ケーシング63は、第1ブーム31に固定される。第1ブーム31は、第2ブーム32の回転に伴って、第2ブーム支軸32aに対して相対回転する。シャフト62に対するケーシング63の回転度合いが検出されて、第2ブーム32の回転角A4が取得される。これによれば、回転度検出装置を容易に取り付けることができ、第2ブーム32の回転角A4を、簡素な構成で確実に取得できる。
[第2実施形態]
次に、本発明の第2実施形態に係る位置決定装置80について、説明する。第2実施形態では、上記第1実施形態の第2回転度検出装置60(RECN60)に代えて、直線距離検出装置60が用いられて第2ブーム32の回転角A4を取得する。この点で、第2実施形態は、上記第1実施形態と異なり、それ以外については上記第1実施形態と同じである。以下、上記第1実施形態と異なる点について、説明する。
図15に示すように、直線距離検出装置60は、例えば、ワイヤエンコーダ等の直線距離を検出可能なものであってもよい。以下、直線距離検出装置60を、WENC60と称呼する場合もある。WENC60において、上記第1実施形態のRECN60の構成部位と同一又は等価なものについては、同じ符号を付すことで、説明を省略する。
WENC60は、上記第1実施形態のRECN60の構成部位に加え、カバー66、導出口67、及び、ワイヤ68を更に備えている。カバー66は、ケーシング63と同径の円筒形状の筐体であり、シャフト62を覆うように、ケーシング63に固定されている。カバー66の底面が、固定面66aとなっている。ケーシング63の側面には、導出口67が設けられている。導出口67は、シャフト62の軸と直交する方向に突出しており、カバー66の内部空間及び外部を連通している。
ワイヤ68は、可撓性を有し、カバー66の内部にてシャフト62の側面にて巻き取られつつ、導出口67を介して外部に導出されている。シャフト62は、ワイヤ68を巻き取るために、一方の回転方向に常時付勢されている。ワイヤ68がカバー66外側にて引張られると、付勢力に抗しながらシャフト62が回転する。ワイヤ68がカバー66外側にて緩められると、付勢力に応じながらシャフト62が逆回転する。従って、カバー66外側にて、ワイヤ68の先端から導出口67の直線距離の伸縮に応じて、シャフト62(スリットディスク61)と、ケーシング63とは、互いに相対回転するようになっている。シャフト62及びケーシング63が相対回転した場合に、開口スリット61aにおける光の通過・遮断が、受光部65により交互に検出される。この検出信号は、位置決定装置80に向けて送出されるようになっている。この検出信号に基づいて、ワイヤ68の直線距離を取得可能となっている。
図16に示すように、第2ブームシリンダ37のアーム33側の第2ブームシリンダ支軸37bには、上方に突出するワイヤ固定部位37b1が、同軸的に設けられている。ワイヤ固定部位37b1にワイヤ68の先端を固定し、ワイヤ68を第2ブームシリンダ37と平行に張って、WENC60の固定面66aを第2ブームシリンダ支軸37aの上方に固定する。WENC60は、導出口67がワイヤ固定部位37b1に向くように、固定される。第2ブームシリンダ37の一端は、第2ブームシリンダ支軸37bを介して、第2ブーム32における左側面に固定される。第2ブームシリンダ37の他端は、第2ブームシリンダ支軸37aを介して、第1ブーム31における左側面に固定される。
図17に示すように、第2ブームシリンダ37は、短縮するよう駆動されると、一端(第2ブームシリンダ支軸37b)から他端(第2ブームシリンダ支軸37a)に向かう方向の直線距離が短縮する。これに応じて、第2ブーム32も第2ブーム32支軸を中心として回転し、左方へのオフセット移動がなされる。また、第2ブームシリンダ37が伸長するよう駆動されると、上記直線距離が伸長する。これに応じて、第2ブーム32も第2ブーム32支軸を中心として回転し、右方へのオフセット移動がなされる。第2ブームシリンダ37は、棒部材に相当する。
ワイヤ68の直線距離は、第2ブームシリンダ37の直線距離の伸縮に対応して伸縮する。第2ブームシリンダ37の直線距離は、第2ブーム32の回転角に応じて変化する。従って、WENC60の検出信号に基づいて、第2ブーム32の回転角を取得可能となっている。
図17、及び、図18に示すように、本実施形態では、ワイヤ68(第2ブームシリンダ37)の直線距離Dは、基準状態における基準直線距離Dsから最大直線距離D1まで伸長可能となっている。直線距離Dが、DsからD1に遷移するのに応じて、正の回転角A4の絶対値は、大きくなっていく。また、直線距離Dは、基準直線距離Dsから最小直線距離D2まで短縮可能となっている。直線距離Dが、DsからD2に遷移するのに応じて、負の回転角A4の絶対値は、大きくなっていく。
図19に示すように、第2実施形態の第2回転度取得手段84は、WENC60と接続されて、WENC60により検出され送出された直線距離に対応する信号を受信する。第2回転度取得手段84は、当該信号に基づいて、第2ブーム32の回転角を取得する。第2ブーム32の回転角A4は、例えば、検出された直線距離Dと、図18に示す直線距離D及び回転角A4の関係と、に基づいて、取得されてもよい。
本発明の第2実施形態に係る位置決定装置80では、特に、直線距離検出装置60(WENC60)がバックホー10に備えられる。WENC60は、ワイヤ68を備えており、ワイヤ68の直線距離は、第2ブームシリンダ37の直線距離の伸縮に対応して伸縮する。第2ブームシリンダ37の直線距離は、第2ブーム32の回転角に応じて変化する。このことを利用して、直線距離が検出されて、第2ブーム32の回転角A4が取得される。これによれば、直線距離検出装置を容易に取り付けることができ、第2ブーム32の回転角A4を、簡素な構成で確実に取得できる。
なお、上記第2実施形態では、直線距離検出装置60として、WENC(ワイヤエンコーダ)60が用いられているが、第2ブームシリンダ37の直線距離を検出可能なものであれば、何れのものであってもよい。直線距離検出装置60は、例えば、光学式センサ、電波式センサ、音波式センサ等であってもよい。これらのセンサを用い、第2ブームシリンダ支軸37a,37b間の直線距離が、検出されるようにしてもよい。
[第3実施形態]
次に、本発明の第3実施形態に係る位置決定装置80について、説明する。第3実施形態では、上記第1、第2実施形態のRECN60、WENC60に代えて、回転位置検出装置60Aが用いられて第2ブーム32の回転角A4を取得する。また、第3実施形態では、位置決定装置80は、更に、軸位置取得手段86、及び、回転位置測定手段87を備える。この点で、第3実施形態は、上記第1、第2実施形態と異なり、それ以外については上記第1、第2実施形態と同じである。以下、上記第1、第2実施形態と異なる点について、説明する。
図20に示すように、回転位置検出装置60Aは、第2ブーム32に搭載されている。搭載位置は、第2ブーム32の何れでもよいが、例えば、第2ブーム支軸32aから極力離間した部位に、回転位置検出装置60Aが搭載されると、回転角A4の検出精度を向上する観点で好適である。この観点と、搭載容易性の観点から、本実施形態では、回転位置検出装置60Aは、第2ブーム32の上部であって、第2ブームシリンダ支軸37bよりも前側(アーム33側)に、搭載されている。
回転位置検出装置60Aは、全球測位衛星からの測位用信号の受信に基づいて、第2ブーム32の位置として上記搭載位置を検出する。この検出信号は、位置決定装置80に向けて送出されるようになっている。回転位置検出装置60Aは、例えば、GNSS等の衛星測位システムにより位置検出が可能なものであってもよい。本実施形態では、2周波L1,L2を利用して、回転位置検出装置60Aの搭載位置に対応する絶対座標が、決定されるようになっている。以下、回転位置検出装置60Aを、GNSS60Aと称呼する場合もある。なお、2周波L1,L2に代えて、2周波L1,L5であってもよいし、2周波以上のマルチ周波が用いられてもよい。
図21に示すように、位置決定装置80は、上記第1実施形態のものに加え、更に、軸位置取得手段86、及び、回転位置測定手段87を備えている。軸位置取得手段86は、後述のように、第2ブーム32の回転の中心となる第2ブーム支軸32aの位置を取得する。回転位置測定手段87は、GNSS60Aと接続されて、GNSS60Aにより検出され送出された、第2ブーム32の位置に対応する信号を受信する。回転位置測定手段87は、当該信号に基づいて、第2ブーム32の位置を測位する。第2回転度取得手段84は、後述のように、上記取得された第2ブーム支軸32aの位置と、上記測位された第2ブーム32の位置とに基づいて、第2ブーム32の回転角を取得する。
図22に示すように、位置決定装置80は、基準状態において、更に、第2ブーム支軸32aの位置に対応する基準座標(Xs5,Ys5,Zs5)、及び、GNSS60Aの搭載位置に対応する基準座標(Xs6,Ys6,Zs6)を決定する。上記第1実施形態の構成部位、座標と同一又は等価なものについては、同じ符号を付すことで、説明を省略する。
基準状態において、第1ブーム支軸31aの位置から第2ブーム支軸32aの位置までの離間距離及び方向が既知である。GNSS40の基準座標(Xs0,Ys0,Zs0)から決定された第1ブーム支軸31aの基準座標(Xs1,Ys1,Zs1)と、既知の離間距離及び方向とに基づいて、第2ブーム支軸32aの位置に対応する基準座標(Xs5,Ys5,Zs5)が決定される。なお、これに代えて、GNSS40の搭載位置から第2ブーム支軸32aの位置までの離間距離及び方向が既知であることを利用して、GNSS40の基準座標(Xs0,Ys0,Zs0)に基づいて、第2ブーム支軸32aの基準座標(Xs5,Ys5,Zs5)が決定されてもよい。
基準状態において、第2ブーム支軸32aの位置からGNSS60Aの搭載位置までの離間距離及び方向が既知である。上記決定された第2ブーム支軸32aの基準座標(Xs5,Ys5,Zs5)と、既知の離間距離及び方向とに基づいて、GNSS60Aの搭載位置に対応する基準座標(Xs6,Ys6,Zs6)が決定される。
図23に示すように、バックホー10の作動により、第1ブーム支軸31aの位置に対応する座標が、基準座標(Xs1,Ys1,Zs1)から(X1,Y1,Z1)に移動したものとする。バックホー10の作動により、第1ブーム31が、第1方向K1の軸を中心として回転角A1だけ基準状態から回転したものとする。第1ブーム支軸31aの位置の移動、及び、第1ブーム31の回転に応じて、第2ブーム支軸32aの位置に対応する座標が、基準座標(Xs5,Ys5,Zs5)から(X5,Y5,Z5)に移動したものとする。第2ブーム32が基準状態から回転しない(オフセット移動しない)場合に、GNSS60Aの搭載位置に対応する座標が、基準座標(Xs6,Ys6,Zs6)から(X6a,Y6a,Z6a)に移動したものとする。第1ブーム31の回転角A1は、IMU52にて検出されたものである。
第1ブーム支軸31aの基準座標(Xs1,Ys1,Zs1)から、第2ブーム支軸32aの基準座標(Xs5,Ys5,Zs5)に向かうベクトル(Xs5―Xs1,Ys5―Ys1,Zs5―Zs1)が既知である。上記(1)~(3)式(好ましくは、上記(1a)~(3a)式)にて決定された第1ブーム支軸31aの座標(X1,Y1,Z1)と、既知の上記ベクトルと、上記検出された回転角A1とに基づいて、第2ブーム支軸32aの位置に対応する座標(X5,Y5,Z5)が決定される。具体的には、下記(16)式、(17)式、(18)式に示すように、座標(X1,Y1,Z1)及び上記ベクトルの和と、回転角A1の回転行列とを用いて、座標(X5,Y5,Z5)が決定される。軸位置取得手段86は、以上のようにして第2ブーム支軸32aの位置を取得する。なお、下記(16)式、(17)式、(18)式を用いるのに代えて、下記(16a)式、(17a)式、(18a)式を用いて、第2ブーム支軸32aの位置に対応する座標(X5,Y5,Z5)を決定すると、精度向上の観点でより好適である。
X5=(X1+(Xs5―Xs1))(cos(A1))+
(Z1+(Zs5―Zs1))(sin(A1)) …(16)
Y5=(Y1+(Ys5―Ys1)) …(17)
Z5=(X1+(Xs5―Xs1))(―sin(A1))+
(Z1+(Zs5―Zs1))(cos(A1)) …(18)
X5=X1+(Xs5―Xs1)(cos(A1))+
(Zs5―Zs1)(sin(A1)) …(16a)
Y5=Y1+(Ys5―Ys1) …(17a)
Z5=X1+(Xs5―Xs1)(―sin(A1))+
(Zs5―Zs1)(cos(A1)) …(18a)
第1ブーム支軸31aの基準座標(Xs1,Ys1,Zs1)から、第2ブーム支軸32aの基準座標(Xs5,Ys5,Zs5)を経て、GNSS60Aの基準座標(Xs6,Ys6,Zs6)に向かうベクトル((Xs5―Xs1)+(Xs6―Xs5),(Ys5―Ys1)+(Ys6―Ys5),(Zs5―Zs1)+(Zs6―Zs5))=(Xs6―Xs1,Ys6―Ys1,Zs6―Zs1)が既知である。上記(1)~(3)式(好ましくは、上記(1a)~(3a)式)にて決定された第1ブーム支軸31aの座標(X1,Y1,Z1)と、既知の上記ベクトルと、上記検出された回転角A1とに基づいて、GNSS60Aの搭載位置に対応する座標(X6a,Y6a,Z6a)が決定される。具体的には、下記(19)式、(20)式、(21)式に示すように、座標(X1,Y1,Z1)及び上記ベクトルの和と、回転角A1の回転行列とを用いて、座標(X6a,Y6a,Z6a)が決定される。なお、下記(19)式、(20)式、(21)式を用いるのに代えて、下記(19a)式、(20a)式、(21a)式を用いて、GNSS60Aの搭載位置に対応する座標(X6a,Y6a,Z6a)を決定すると、精度向上の観点でより好適である。
X6a=(X1+(Xs6―Xs1))(cos(A1))+
(Z1+(Zs6―Zs1))(sin(A1)) …(19)
Y6a=(Y1+(Ys6―Ys1)) …(20)
Z6a=(X1+(Xs6―Xs1))(―sin(A1))+
(Z1+(Zs6―Zs1))(cos(A1)) …(21)
X6a=X1+(Xs6―Xs1)(cos(A1))+
(Zs6―Zs1)(sin(A1)) …(19a)
Y6a=Y1+(Ys6―Ys1) …(20a)
Z6a=X1+(Xs6―Xs1)(―sin(A1))+
(Zs6―Zs1)(cos(A1)) …(21a)
ここにおいて、図24に示すように、バックホー10の作動によって、第2ブーム32の第2ブーム支軸32aを中心とする回転(オフセット移動)が、実行されたものとする。この場合、GNSS60Aの搭載位置に対応する座標が、(X6a,Y6a,Z6a)から(X6b,Y6b,Z6b)に移動したものとする。GNSS60Aの座標(X6b,Y6b,Z6b)は、GNSS60Aにて測位されたものである。
上記決定された座標(X5,Y5,Z5)と、上記決定された座標(X6a,Y6a,Z6a)と、上記測位された座標(X6b,Y6b,Z6b)とに基づいて、第2ブーム32の回転角A4が取得される。具体的には、下記(22)式、(23)式、(24)式、(25a)式に示すように、ベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)及び(X6b―X5,Y6b―Y5,Z6b―Z5)の内積Sと、ベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)の絶対値Tと、ベクトル(X6b―X5,Y6b―Y5,Z6b―Z5)の絶対値Uとを用いて、回転角A4が決定される。第2回転度取得手段84は、以上のようにして第2ブーム32の回転角A4を取得する。
S=(X6a―X5)(X6b―X5)+(Y6a―Y5)(Y6b―Y5)+
(Z6―Z5a)(Z6b―Z5) …(22)
T=((X6a―X5)2+(Y6a―Y5)2+(Z6a―Z5)2)1/2 …(23)
U=((X6b―X5)2+(Y6b―Y5)2+(Z6b―Z5)2)1/2 …(24)
A4=arccos(S/((T)(U))) …(25a)
第3実施形態における位置決定装置80の実際の作動について、図25のフローチャートを参照しつつ説明する。ステップST1における基準座標の決定においては、バックホー10が基準状態にて静止している状態において、位置決定装置80の位置決定手段85により、上記第1、第2実施形態の基準座標に加え、更に、第2ブーム支軸32a、及び、GNSS60Aのそれぞれの位置に対応する基準座標(Xs5,Ys5,Zs5)、(Xs6,Ys6,Zs6)も、それぞれ決定される。(図22を参照)。
次に、ステップST2を経てステップST21にて、位置決定装置80は、第2ブーム支軸32aの座標を取得する。位置決定装置80の軸位置取得手段86により、バックホー10が作動している場合に、上記(16)~(18)式(好ましくは、上記(16a)~(18a)式)を用いて、第2ブーム支軸32aの位置に対応する座標(X5,Y5,Z5)が決定される(図23を参照)。
次に、ステップST22にて、位置決定装置80は、GNSS60Aの座標を測位する。位置決定装置80の回転位置測定手段87により、バックホー10が作動している場合に、GNSS60Aの搭載位置に対応する座標(X6b,Y6b,Z6b)が測位される。
次のステップST3における回転角の取得においては、位置決定装置80の第2回転度取得手段84により、バックホー10が作動している場合に、上記(19)~(24)式、上記(25a)式(好ましくは、上記(19a)~(21a)式、上記(23)~(24)式、上記(25a)式)を用いて、第2ブーム32の回転角A4が取得される(図24を参照)。なお、回転角A0,A1,A2,A3は、上記第1、第2実施形態と同様に取得される。以降、ステップST4,ST5に進み、上記第1、第2実施形態と同様の各処理が実行される。
本発明の第3実施形態に係る位置決定装置80では、特に、回転位置検出装置60A(GNSS60A)が、バックホー10の第2ブーム32に備えられる。また、位置決定装置80は、更に、軸位置取得手段86、及び、回転位置測定手段87を備える。軸位置取得手段86により、第2ブーム支軸32aの位置が取得される。GNSS60A、及び、回転位置測定手段87により、第2ブーム32におけるGNSS60Aの搭載位置が測位される。取得された第2ブーム支軸32aの位置と、測位された第2ブーム32におけるGNSS60Aの搭載位置とに基づいて、第2ブーム32の回転角A4が取得される。これによれば、回転位置検出装置を、第2ブーム32であれば、何れの部位にも搭載できる。例えば、構造物を避け搭載し易い部位や、回転角の精度を向上可能な部位等に、回転位置検出装置を搭載できる。従って、第2ブーム32の回転角A4を、容易かつ確実に取得できる。
[第4実施形態]
次に、本発明の第4実施形態に係る位置決定装置80について、説明する。第4実施形態では、上記第1、第2実施形態のRECN60、WENC60に代えて、回転位置検出装置60A、及び、軸位置検出装置60Bが用いられて第2ブーム32の回転角A4を取得する。また、第4実施形態では、位置決定装置80は、上記第3実施形態の軸位置取得手段86、及び、回転位置測定手段87に代えて、相対位置取得手段88を備える。この点で、第4実施形態は、上記第1、第2、第3実施形態と異なり、それ以外については上記第1、第2、第3実施形態と同じである。以下、上記第1、第2、第3実施形態と異なる点について、説明する。
図26に示すように、回転位置検出装置60A、及び、軸位置検出装置60Bは、第2ブーム32に搭載されている。軸位置検出装置60Bは、第2ブーム支軸32aの上部に同軸的に搭載されている。回転位置検出装置60Aの搭載位置は、第2ブーム32において、軸位置検出装置60Bよりも前側の部位であれば何れでもよい。例えば、軸位置検出装置60Bから極力離間した部位に、回転位置検出装置60Aが搭載されると、回転角A4の検出精度を向上する観点で好適である。この観点と、搭載容易性の観点から、本実施形態では、回転位置検出装置60Aは、第2ブーム32の上部であって、第2ブームシリンダ支軸37bよりも前側(アーム33側)に、搭載されている。
回転位置検出装置60Aは、全球測位衛星からの測位用信号の受信に基づいて、第2ブーム32の位置として、第2ブーム32における上記搭載位置を検出する。この検出信号は、位置決定装置80に向けて送出されるようになっている。回転位置検出装置60Aは、例えば、GNSS等の衛星測位システムにより位置検出が可能なものであってもよい。軸位置検出装置60Bは、全球測位衛星からの測位用信号の受信に基づいて、第2ブーム支軸32aの位置として、第2ブーム支軸32aにおける上記搭載位置を検出する。この検出信号は、位置決定装置80に向けて送出されるようになっている。軸位置検出装置60Bは、例えば、GNSS等の衛星測位システムにより位置検出が可能なものであってもよい。
本実施形態では、1周波L1を利用した相対測位により、軸位置検出装置60Bの位置に対する回転位置検出装置60Aの相対位置を、取得可能となっている。即ち、第2ブーム支軸32aの位置に対する第2ブーム32の相対位置を、取得可能となっている。相対位置として、例えば、第2ブーム支軸32aの位置(軸位置検出装置60Bの位置)から、第2ブーム32における回転位置検出装置60Aの位置に向かうベクトルが、用いられてもよい。以下、回転位置検出装置60AをGNSS60A、軸位置検出装置60BをGNSS60Bと、それぞれ称呼する場合もある。
本実施形態におけるGNSS60A、及び、GNSS60Bは、上述のように相対測位が可能な構成であればよい。相対測位の場合、多周波を利用して絶対座標を決定する場合に比して、位置検出装置の構成を、より簡素なものとすることができる。このため、GNSS60A、及び、GNSS60Bとして、安価な位置検出装置を用いることができる。
図27に示すように、位置決定装置80は、上記第1実施形態のものに加え、更に、相対位置取得手段88を備えている。相対位置取得手段88は、後述のように、第2ブーム支軸32aの位置に対する第2ブーム32の相対位置を取得する。相対位置取得手段88は、GNSS60A、及び、GNSS60Bとそれぞれ接続され、GNSS60A,60Bによりそれぞれ検出され送出された、第2ブーム32、及び、第2ブーム支軸32aの位置に対応する信号を受信する。相対位置取得手段88は、当該信号に基づいて、上記相対位置を取得する。第2回転度取得手段84は、後述のように、上記取得された相対位置に基づいて、第2ブーム32の回転角を取得する。
図28に示すように、バックホー10の作動によって、第2ブーム32の第2ブーム支軸32aを中心とする回転(オフセット移動)が、実行されたものとする。図28に示す状態は、上述した図24に示す状態に対応するものである。即ち、この状態は、第1ブーム支軸31aの位置が、基準座標(Xs1,Ys1,Zs1)から座標(X1,Y1,Z1)に移動し、第1ブーム31が回転角A1だけ回転している状態である。
この状態において、第2ブーム32の回転が実行される前に、GNSS60A,60Bによる相対測位が実行される。具体的には、第2ブーム支軸32aにおけるGNSS60Bの搭載位置から、第2ブーム32におけるGNSS60Aの搭載位置に向かうベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)が、取得される。また、第2ブーム32の回転が実行されたときにも、GNSS60A,60Bによる相対測位が実行される。具体的には、第2ブーム支軸32aにおけるGNSS60Bの搭載位置から、第2ブーム32におけるGNSS60Aの搭載位置に向かうベクトル(X6b―X5,Y6b―Y5,Z6b―Z5)が、取得される。相対位置取得手段88は、以上のようにして、相対位置としてのベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)、(X6b―X5,Y6b―Y5,Z6b―Z5)を取得する。
なお、上記第3実施形態では、第2ブーム支軸32aの座標(X5,Y5,Z5)、GNSS60Aの座標(X6a,Y6a,Z6a)及び(X6b,Y6b,Z6b)が個別に取得されて、ベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)及び(X6b―X5,Y6b―Y5,Z6b―Z5)が取得される。これに対し、本実施形態では、上述のように各座標を個別に取得することなく、GNSS60A,60Bによる相対測位により、ベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)及び(X6b―X5,Y6b―Y5,Z6b―Z5)が直接取得される。この点で、本実施形態は、上記第3実施形態と異なる。
上記取得されたベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)及び(X6b―X5,Y6b―Y5,Z6b―Z5)に基づいて、第2ブーム32の回転角A4が取得される。具体的には、上記(22)式、(23)式、(24)式、(25a)式に示すように、ベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)及び(X6b―X5,Y6b―Y5,Z6b―Z5)の内積Sと、ベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)の絶対値Tと、ベクトル(X6b―X5,Y6b―Y5,Z6b―Z5)の絶対値Uとを用いて、回転角A4が決定される。第2回転度取得手段84は、以上のようにして第2ブーム32の回転角A4を取得する。
第4実施形態における位置決定装置80の実際の作動について、図29のフローチャートを参照しつつ説明する。ステップST1における基準座標の決定においては、バックホー10が基準状態にて静止している状態において、位置決定装置80の位置決定手段85により、上記第1、第2実施形態と同様に基準座標が決定される。(図8を参照)。
次に、ステップST2を経てステップST23にて、位置決定装置80は、GNSS60A,60Bの相対位置を取得する。位置決定装置80の相対位置取得手段88により、ベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)及び(X6b―X5,Y6b―Y5,Z6b―Z5)が取得される。ここにおいて、ベクトル(X6a―X5,Y6a―Y5,Z6a―Z5)は、第2ブーム32のオフセット移動実施前での、GNSS60Bの搭載位置から、GNSS60Aの搭載位置に向かうベクトルである。ベクトル(X6b―X5,Y6b―Y5,Z6b―Z5)は、第2ブーム32のオフセット移動実施時での、GNSS60Bの搭載位置から、GNSS60Aの搭載位置に向かうベクトルである。
次のステップST3における回転角の取得においては、位置決定装置80の第2回転度取得手段84により、バックホー10が作動している場合に、上記(22)~(24)、上記(25a)式を用いて、第2ブーム32の回転角A4が取得される(図28を参照)。なお、回転角A0,A1,A2,A3は、上記第1、第2実施形態と同様に取得される。以降、ステップST4,ST5に進み、上記第1、第2実施形態と同様の各処理が実行される。
本発明の第4実施形態に係る位置決定装置80では、特に、回転位置検出装置60A(GNSS60A)が、バックホー10の第2ブーム32に備えられ、軸位置検出装置60B(GNSS60B)が、バックホー10の第2ブーム支軸32aに備えられる。また、位置決定装置80は、更に、相対位置取得手段88を備える。GNSS60A,60Bの相対測位、及び、相対位置取得手段88により、第2ブーム支軸32a(GNSS60Bの搭載位置)に対する第2ブーム32(GNSS60Aの搭載位置)の相対位置が取得される。取得された相対位置に基づいて、第2ブーム32の回転角A4が取得される。これによれば、回転位置検出装置、及び、軸位置検出装置にて、相対測位が実行される。相対測位の場合、多周波を利用して絶対座標を決定する場合に比して、位置検出装置の構成を、より簡素なものとすることができる。このため、回転位置検出装置、及び、軸位置検出装置として、安価な位置検出装置を用いることができる。従って、第2ブーム32の回転角A4を、低コストで確実に取得できる。
[変形例]
本発明の実施形態においては、種々の変形例を採用することができる。上記第1~第4実施形態においては、バックホー10の構成として、第1ブーム31は、第1ブーム支軸31aを中心として回転可能となっており、第2ブーム32が、第2ブーム支軸32aを中心として回転可能となっている。第2ブーム32の第1ブーム31に対する相対回転により、バケット34のオフセット移動が可能となっている。
これに代えて、例えば、第1ブーム31及び第2ブーム32が一体的に構成され、第1ブーム31に対する第2ブーム32の相対回転が規制されており、第1ブーム31が、第2方向K2に平行な軸を中心として、回転可能となっていてもよい。より具体的には、第1ブーム31の走行車両20の接続部位が、第1方向K1に平行な軸、及び、第2方向K2に平行な軸の2軸の回転中心となっていてもよい。これにより、第1ブーム31の走行車両20に対する相対回転により、バケット34のオフセット移動が可能となる。この変形例の構成のバックホー10に、位置決定装置80が適用された場合であっても、上記各実施形態の効果と同じ効果を奏する。
今回開示された上記各実施形態は、すべての点で例示であって、制限的なものではないと考えられるべきである。本発明の範囲は、上記した説明ではなく、特許請求の範囲によって示され、特許請求の範囲と均等の意味及び範囲内でのすべての変更が含まれることが、意図される。