本開示は、モータグレーダおよびモータグレーダのドローバ姿勢算出方法に関する。This disclosure relates to a motor grader and a method for calculating the drawbar attitude of a motor grader.
従来、モータグレーダが知られている。モータグレーダは、旋回サークルが取り付けられたドローバを備える。旋回サークルには、ブレードが取り付けられている。Motor graders are known in the art. Motor graders have a drawbar to which a turning circle is attached. A blade is attached to the turning circle.
米国特許出願公開第２０１８／００６１０４０Ａ１号明細書（特許文献１）には、上記ブレードをトラッキング可能なモータグレーダが開示されている。具体的には、特許文献１のモータグレーダは、ブレードに動作可能に結合された第１および第２の光学ターゲットと、第１の視野内の第１の光学ターゲットの第１の撮像データを収集する第１のカメラと、第２の視野内で第２の光学ターゲットの第２の撮像データを収集する第２のカメラと、第１または第２の撮像データの少なくとも一方を選択するセレクタと、選択された撮像データに基づいてブレードの向きを決定するプロセッサとを備える。第１のカメラと第２のカメラとは、フロントフレームに設置されている。U.S. Patent Application Publication No. 2018/0061040A1 (Patent Document 1) discloses a motor grader capable of tracking the blade. Specifically, the motor grader in Patent Document 1 includes first and second optical targets operably coupled to the blade, a first camera that collects first imaging data of the first optical target within a first field of view, a second camera that collects second imaging data of the second optical target within a second field of view, a selector that selects at least one of the first and second imaging data, and a processor that determines the orientation of the blade based on the selected imaging data. The first and second cameras are mounted on a front frame.
特許文献１のモータグレーダでは、複数の光学ターゲット（識別用マーカ）を別々のカメラで撮像することにより、ブレードの姿勢を求めている。これに限らず、ブレードの姿勢は、ドローバの姿勢から比較的容易に求めることができる。よって、ドローバの姿勢を算出できれば、ブレードの姿勢を求めることができる。In the motor grader of Patent Document 1, the blade attitude is determined by capturing images of multiple optical targets (identification markers) with separate cameras. This is not a limitation; the blade attitude can be determined relatively easily from the drawbar attitude. Therefore, if the drawbar attitude can be calculated, the blade attitude can be determined.
本開示は、識別用マーカを用いてドローバの姿勢を算出可能なモータグレーダおよびモータグレーダのドローバ姿勢算出方法を提供する。This disclosure provides a motor grader and a method for calculating the drawbar attitude of a motor grader that can calculate the drawbar attitude using an identification marker.
本開示の一態様に係るモータグレーダは、フロントフレームと、ドローバと、ドローバの前方においてドローバをフロントフレームに対して揺動可能に連結する連結部材と、連結部材の近傍となるフロントフレームの第１の位置に取り付けられた第１の識別用マーカと、ドローバに取り付けられ、かつ、第１の識別用マーカを撮像するカメラと、撮像により得られた第１の識別用マーカの第１の画像データに基づいて、ドローバの姿勢を算出するコントローラとを備える。A motor grader according to one aspect of the present disclosure includes a front frame, a drawbar, a connecting member that swingably connects the drawbar to the front frame in front of the drawbar, a first identification marker attached to a first position on the front frame near the connecting member, a camera attached to the drawbar that captures an image of the first identification marker, and a controller that calculates the attitude of the drawbar based on first image data of the first identification marker obtained by the image capture.
本開示の他の態様に係るモータグレーダは、フロントフレームと、フロントフレームに揺動可能に取り付けられたドローバと、ドローバに取り付けられた第１の識別用マーカと、フロントフレームに取り付けられ、かつ、第１の識別用マーカを撮像するカメラと、撮像により得られた第１の識別用マーカの第１の画像データに基づいて、カメラに対するドローバの第１の姿勢を算出するコントローラとを備える。A motor grader according to another aspect of the present disclosure includes a front frame, a drawbar swingably attached to the front frame, a first identification marker attached to the drawbar, a camera attached to the front frame for capturing an image of the first identification marker, and a controller for calculating a first attitude of the drawbar relative to the camera based on first image data of the first identification marker obtained by capturing the image.
本開示によれば、識別用マーカを用いてドローバの姿勢を算出可能となる。According to the present disclosure, the attitude of the drawbar can be calculated using an identification marker.
以下、本発明の各実施形態に係るモータグレーダについて、図に基づいて説明する。以下の説明では、同一部品には、同一の符号を付している。それらの名称および機能も同じである。したがって、それらについての詳細な説明は繰り返さない。Below, motor graders according to each embodiment of the present invention will be described with reference to the drawings. In the following description, identical parts will be assigned the same reference numerals. Their names and functions are also the same. Therefore, detailed descriptions of them will not be repeated.
［実施の形態１］
図１は、本実施形態に係るモータグレーダ１の構成を概略的に示す斜視図である。図１に示すように、モータグレーダ１は、前輪１１と、後輪１２と、車体フレーム２と、キャブ３と、作業機４とを主に備えている。作業機４は、ドローバ４０と、旋回サークル４１と、ブレード４２とを主に有している。モータグレーダ１は、ブレード４２で整地作業、除雪作業、軽切削、材料混合などの作業を行なう。[First Embodiment]
Fig. 1 is a perspective view showing a schematic configuration of a motor grader 1 according to this embodiment. As shown in Fig. 1, the motor grader 1 mainly includes front wheels 11, rear wheels 12, a body frame 2, a cab 3, and a work implement 4. The work implement 4 mainly includes a drawbar 40, a turning circle 41, and a blade 42. The motor grader 1 uses the blade 42 to perform tasks such as ground leveling, snow removal, light cutting, and material mixing.
なお、以下の図の説明において、モータグレーダ１が直進走行する方向を、モータグレーダ１の前後方向という。モータグレーダ１の前後方向において、作業機４に対して前輪１１が配置されている側を、前方向とする。モータグレーダ１の前後方向において、作業機４に対して後輪１２が配置されている側を、後方向とする。In the following description of the figures, the direction in which the motor grader 1 travels straight ahead is referred to as the fore-and-aft direction of the motor grader 1. In the fore-and-aft direction of the motor grader 1, the side where the front wheels 11 are positioned relative to the work equipment 4 is referred to as the front direction. In the fore-and-aft direction of the motor grader 1, the side where the rear wheels 12 are positioned relative to the work equipment 4 is referred to as the rear direction.
モータグレーダ１の左右方向とは、平面視において前後方向と直交する方向である。モータグレーダ１の運転席に着座したオペレータが前方向を見て左右方向の右側、左側が、それぞれ右方向、左方向である。モータグレーダ１の上下方向とは、前後方向および左右方向によって定められる平面に直交する方向である。上下方向において地面のある側が下側、空のある側が上側である。The left-right direction of the motor grader 1 is the direction perpendicular to the front-to-back direction in a plan view. When an operator sitting in the driver's seat of the motor grader 1 looks forward, the right and left sides of the left-to-right direction are the right and left directions, respectively. The up-to-down direction of the motor grader 1 is the direction perpendicular to the plane defined by the front-to-back and left-to-right directions. In the up-to-down direction, the side with the ground is the bottom side, and the side with the sky is the top side.
車体フレーム２は、前後方向に延びている。車体フレーム２は、リアフレーム２１と、フロントフレーム２２とを含んでいる。The vehicle body frame 2 extends in the front-to-rear direction. The vehicle body frame 2 includes a rear frame 21 and a front frame 22.
リアフレーム２１は、外装カバー２５と、エンジン室６に配置されたエンジンなどの構成部品とを支持している。外装カバー２５はエンジン室６を覆っている。リアフレーム２１には、上記のたとえば４つの後輪１２の各々がエンジンからの駆動力によって回転駆動可能に取り付けられている。The rear frame 21 supports an exterior cover 25 and components such as the engine arranged in the engine compartment 6. The exterior cover 25 covers the engine compartment 6. Each of the four rear wheels 12 is mounted on the rear frame 21 so that it can be rotated by driving force from the engine.
フロントフレーム２２は、リアフレーム２１の前方に取り付けられている。フロントフレーム２２は、リアフレーム２１に、回動可能に連結されている。フロントフレーム２２は、前後方向に延びている。フロントフレーム２２は、リアフレーム２１に連結されている基端部２２ｒと、基端部２２ｒと反対側の前端部２２ｆとを有している。フロントフレーム２２の基端部２２ｒは、鉛直なセンタピンにより、リアフレーム２１の前端部と連結されている。The front frame 22 is attached in front of the rear frame 21. The front frame 22 is rotatably connected to the rear frame 21. The front frame 22 extends in the front-to-rear direction. The front frame 22 has a base end 22r connected to the rear frame 21 and a front end 22f opposite the base end 22r. The base end 22r of the front frame 22 is connected to the front end of the rear frame 21 by a vertical center pin.
フロントフレーム２２とリアフレーム２１との間には、アーティキュレートシリンダ（図示せず）が取り付けられている。フロントフレーム２２は、アーティキュレートシリンダの伸縮により、リアフレーム２１に対して回動可能に設けられている。アーティキュレートシリンダは、キャブ３の内部に設けられる操作レバーの操作により、伸縮可能に設けられている。An articulating cylinder (not shown) is attached between the front frame 22 and the rear frame 21. The front frame 22 is rotatable relative to the rear frame 21 by extension and retraction of the articulating cylinder. The articulating cylinder is extendable and retractable by operating an operating lever located inside the cab 3.
フロントフレーム２２の前端部２２ｆには、上記のたとえば２つの前輪１１が回転可能に取り付けられている。前輪１１は、ステアリングシリンダ（図示せず）の伸縮によって、フロントフレーム２２に対して旋回可能に取り付けられている。モータグレーダ１は、ステアリングシリンダの伸縮によって、進行方向を変更することが可能である。ステアリングシリンダは、キャブ３の内部に設けられるハンドルまたはステアリング操作レバーの操作によって、伸縮可能である。The two front wheels 11, for example, are rotatably attached to the front end 22f of the front frame 22. The front wheels 11 are rotatably attached to the front frame 22 by extension and retraction of a steering cylinder (not shown). The motor grader 1 can change its direction of travel by extension and retraction of the steering cylinder. The steering cylinder can be extended and retracted by operating a handle or steering lever located inside the cab 3.
フロントフレーム２２の前端部２２ｆには、カウンタウェイト５１が取り付けられている。カウンタウェイト５１は、フロントフレーム２２に取り付けられるアタッチメントの一種である。カウンタウェイト５１は、フロントフレーム２２の前方に位置する。A counterweight 51 is attached to the front end 22f of the front frame 22. The counterweight 51 is a type of attachment that is attached to the front frame 22. The counterweight 51 is located in front of the front frame 22.
キャブ３はフロントフレーム２２に載置されている。キャブ３の内部には、ハンドル、変速レバー、作業機４の操作レバー、ブレーキ、アクセルペダル、インチングペダル、各種のスイッチなどの操作部（図示せず）が設けられている。The cab 3 is mounted on the front frame 22. Inside the cab 3, there are operating parts (not shown) such as a steering wheel, a gear lever, an operating lever for the work implement 4, a brake, an accelerator pedal, an inching pedal, and various switches.
図２は、作業機４の要部を示す図である。図２は、作業機４が中立位置となった状態を示している。なお、「作業機４が中立位置となった状態」とは、フロントフレーム２２に対してドローバ４０が左右にずれておらず、かつ、ドローバ４０の前後方向に対してブレード４２の長手方向（幅方向）が直角であり、かつ、ブレード４２の幅方向における中心がドローバ４０の前後方向の軸の真下に位置する状態である。換言すれば、「作業機４の中立位置となった状態」とは、フロントフレーム２２に対してドローバ４０が左右にずれておらず、かつ、フロントフレーム２２の前後方向に対してブレード４２の長手方向が直角であり、かつ、ブレード４２の長手方向における中心がフロントフレーム２２の前後方向の軸の真下に位置する状態である。Figure 2 is a diagram showing the main parts of the work implement 4. Figure 2 shows the work implement 4 in a neutral position. The "work implement 4 in a neutral position" refers to a state in which the drawbar 40 is not misaligned left or right with respect to the front frame 22, the longitudinal direction (width direction) of the blade 42 is perpendicular to the fore-and-aft direction of the drawbar 40, and the center of the blade 42 in the width direction is located directly below the axis of the drawbar 40 in the fore-and-aft direction. In other words, the "work implement 4 in a neutral position" refers to a state in which the drawbar 40 is not misaligned left or right with respect to the front frame 22, the longitudinal direction of the blade 42 is perpendicular to the fore-and-aft direction of the front frame 22, and the center of the longitudinal direction of the blade 42 is located directly below the axis of the front frame 22 in the fore-and-aft direction.
図２に示されるように、ドローバ４０は、フロントフレーム２２の下方に配置されている。ドローバ４０は、一対のリフトシリンダ４４，４５によって、フロントフレーム２２に近づく方向（地面からブレード４２が離れる方向）と、フロントフレーム２２から離れる方向とに移動する。As shown in Figure 2, the drawbar 40 is disposed below the front frame 22. The drawbar 40 is moved by a pair of lift cylinders 44, 45 in a direction toward the front frame 22 (a direction in which the blade 42 moves away from the ground) and a direction away from the front frame 22.
ドローバ４０は、前端部４０ｆと後端部４０ｒとを有する。前端部４０ｆは、フロントフレーム２２の前端部２２ｆに、玉軸４０２を用いて連結されている。ドローバ４０の後端部４０ｒは、リフトシリンダ４４，４５によってフロントフレーム２２に支持されている。玉軸４０２は、ドローバ４０の前方において、ドローバ４０をフロントフレーム２２に対して揺動可能に連結する。ドローバ４０は、フロントフレーム２２に揺動可能に取り付けられている。The drawbar 40 has a front end 40f and a rear end 40r. The front end 40f is connected to the front end 22f of the front frame 22 using a ball axle 402. The rear end 40r of the drawbar 40 is supported on the front frame 22 by lift cylinders 44, 45. The ball axle 402 connects the drawbar 40 to the front frame 22 in front of the drawbar 40 so that the drawbar 40 can swing relative to the front frame 22. The drawbar 40 is attached to the front frame 22 so that it can swing.
ドローバ４０の前後方向の軸である中心軸Ｊ１は、前端部４０ｆから旋回サークル４１の旋回中心Ｃ（回転軸Ｊ２上の点）に向かう方向に延びている。中心軸Ｊ１は、作業機４が中立位置の場合、モータグレーダ１の平面視（上面視）において、フロントフレーム２２の前後方向の軸である中心軸（図示せず）と重なる。The central axis J1, which is the longitudinal axis of the drawbar 40, extends from the front end 40f toward the rotation center C (a point on the rotation axis J2) of the turning circle 41. When the work implement 4 is in the neutral position, the central axis J1 overlaps with the central axis (not shown), which is the longitudinal axis of the front frame 22, in a plan view (top view) of the motor grader 1.
リフトシリンダ４４，４５の伸縮によって、ドローバ４０の後端部４０ｒがフロントフレーム２２に対して上下に昇降可能である。ドローバ４０は、リフトシリンダ４４，４５の伸縮によって、車両進行方向に沿った軸を中心に上下に揺動可能である。ドローバ４０は、ドローバシフトシリンダ４６の伸縮によって、フロントフレーム２２に対して左右に移動可能である。ドローバ４０は、ドローバシフトシリンダ４６の伸縮によって、矢印９０３の方向に移動する。The extension and contraction of the lift cylinders 44, 45 allows the rear end 40r of the drawbar 40 to be raised and lowered up and down relative to the front frame 22. The extension and contraction of the lift cylinders 44, 45 allows the drawbar 40 to swing up and down around an axis along the vehicle's traveling direction. The extension and contraction of the drawbar shift cylinder 46 allows the drawbar 40 to move left and right relative to the front frame 22. The extension and contraction of the drawbar shift cylinder 46 causes the drawbar 40 to move in the direction of arrow 903.
リフトシリンダ４４，４５は、ドローバ４０とブラケット５０とに取り付けられている。リフトシリンダ４４，４５のヘッド部４４ｈ，４５ｈは、ブラケット５０に取り付けられている。リフトシリンダ４４のロッド４４ｒの先端部と、リフトシリンダ４５のロッド４５ｒ（図１）の先端部とは、ドローバ４０に取り付けられている。ブラケット５０は、フロントフレーム２２に取り付けられている。The lift cylinders 44, 45 are attached to the draw bar 40 and the bracket 50. The heads 44h, 45h of the lift cylinders 44, 45 are attached to the bracket 50. The tip of the rod 44r of the lift cylinder 44 and the tip of the rod 45r (Figure 1) of the lift cylinder 45 are attached to the draw bar 40. The bracket 50 is attached to the front frame 22.
ドローバシフトシリンダ４６は、ドローバ４０とブラケット５０とに取り付けられている。ドローバシフトシリンダ４６のヘッド４６ｈの先端部は、ドローバ４０に取り付けられている。ドローバシフトシリンダ４６のロッド４６ｒの先端部は、ブラケット５０に取り付けられている。The drawbar shift cylinder 46 is attached to the drawbar 40 and the bracket 50. The tip of the head 46h of the drawbar shift cylinder 46 is attached to the drawbar 40. The tip of the rod 46r of the drawbar shift cylinder 46 is attached to the bracket 50.
旋回サークル４１は、フロントフレーム２２の下方に配置されている。旋回サークル４１は、ドローバ４０の下方に配置されている。旋回サークル４１は、ドローバ４０の後端部に旋回（回転）可能に支持されている。旋回サークル４１は、旋回モータ４９によって、ドローバ４０に対し、車両上方から見て時計方向と反時計方向との両方に、旋回駆動可能である。旋回サークル４１は、矢印９０２の方向に回転する。旋回サークル４１は、ドローバ４０に対して、回転軸Ｊ２を中心に矢印９０２の方向に回転する。The turning circle 41 is disposed below the front frame 22. The turning circle 41 is disposed below the drawbar 40. The turning circle 41 is supported at the rear end of the drawbar 40 so that it can turn (rotate). The turning circle 41 can be driven by a turning motor 49 to turn relative to the drawbar 40 in both clockwise and counterclockwise directions when viewed from above the vehicle. The turning circle 41 rotates in the direction of arrow 902. The turning circle 41 rotates in the direction of arrow 902 relative to the drawbar 40 around rotation axis J2.
ブレード４２は、旋回サークル４１に配設されている。旋回サークル４１の旋回駆動によって、ブレード４２のブレードアングル角が調整される。ブレード４２は、前輪１１と後輪１２との間に配置されている。前輪１１は、ブレード４２よりも前方に配置されている。後輪１２は、ブレード４２よりも後方に配置されている。ブレード４２は、車体フレーム２の前端と、車体フレーム２の後端との間に、配置されている。ブレード４２は、旋回サークル４１に支持されている。ブレード４２は、旋回サークル４１を介して、ドローバ４０に支持されている。ブレード４２は、旋回サークル４１およびドローバ４０を介して、フロントフレーム２２に支持されている。The blade 42 is disposed on the turning circle 41. The blade angle of the blade 42 is adjusted by the turning drive of the turning circle 41. The blade 42 is disposed between the front wheel 11 and the rear wheel 12. The front wheel 11 is disposed forward of the blade 42. The rear wheel 12 is disposed rearward of the blade 42. The blade 42 is disposed between the front end of the body frame 2 and the rear end of the body frame 2. The blade 42 is supported on the turning circle 41. The blade 42 is supported on the drawbar 40 via the turning circle 41. The blade 42 is supported on the front frame 22 via the turning circle 41 and the drawbar 40.
ブレード４２は、旋回サークル４１に対して左右方向に移動可能に支持されている。ブレード４２は、矢印９０１の方向に移動する。ブレード４２は、矢印９０１の方向にストロークする。ブレード４２は、旋回サークル４１の旋回駆動にしたがって、回転軸Ｊ２を中心に回転する。Blade 42 is supported so as to be movable left and right relative to the rotating circle 41. Blade 42 moves in the direction of arrow 901. Blade 42 strokes in the direction of arrow 901. Blade 42 rotates around rotation axis J2 in accordance with the rotating drive of the rotating circle 41.
具体的には、ブレードシフトシリンダ４７は、旋回サークル４１およびブレード４２に取り付けられており、かつブレード４２の長手方向に沿って配置されている。ブレードシフトシリンダ４７によって、ブレード４２は旋回サークル４１に対して左右方向に移動可能である。ブレード４２は、フロントフレーム２２の長手方向に交差する方向に移動可能である。Specifically, the blade shift cylinder 47 is attached to the turning circle 41 and the blade 42, and is arranged along the longitudinal direction of the blade 42. The blade shift cylinder 47 allows the blade 42 to move left and right relative to the turning circle 41. The blade 42 can move in a direction intersecting the longitudinal direction of the front frame 22.
ブレード４２は、旋回サークル４１に対して、ブレード４２の長手方向に延びる軸を中心に揺動可能に支持されている。具体的には、チルトシリンダ４８が、旋回サークル４１およびブレード４２に取り付けられている。チルトシリンダ４８を伸縮させることによって、ブレード４２は旋回サークル４１に対してブレード４２の長手方向に延びる軸を中心に揺動して、車両進行方向に対するブレード４２の傾斜角度（地面に対するブレード４２のすくい込み角度）を変更することができる。The blade 42 is supported so that it can swing relative to the turning circle 41 around an axis extending in the longitudinal direction of the blade 42. Specifically, a tilt cylinder 48 is attached to the turning circle 41 and the blade 42. By extending and retracting the tilt cylinder 48, the blade 42 swings relative to the turning circle 41 around an axis extending in the longitudinal direction of the blade 42, making it possible to change the inclination angle of the blade 42 relative to the direction of vehicle travel (the rake angle of the blade 42 relative to the ground).
以上のように、ブレード４２は、ドローバ４０と旋回サークル４１とを介して、モータグレーダ１に対する上下の昇降、車両進行方向に沿った軸を中心とする揺動、前後方向に対する傾斜角度の変更、左右方向の移動、および、ブレード４２の長手方向に延びる軸を中心とする揺動を行なうことが可能に構成されている。As described above, the blade 42 is configured to be able to move up and down relative to the motor grader 1, swing around an axis along the vehicle's travel direction, change its tilt angle relative to the front-to-rear direction, move left and right, and swing around an axis extending in the longitudinal direction of the blade 42, via the drawbar 40 and the swivel circle 41.
モータグレーダ１は、カメラ８００と、複数の識別用のマーカ７０１～７０３とをさらに備える。本例における各識別用のマーカ７０１～７０３は、カメラ８００の被写体である。なお、マーカの数は、３個に限定されず、１個、２個、または４個以上であってもよい。The motor grader 1 further includes a camera 800 and multiple identification markers 701-703. In this example, each of the identification markers 701-703 is the subject of the camera 800. Note that the number of markers is not limited to three, and may be one, two, four or more.
カメラ８００は、レンズ８０１と、イメージセンサを内蔵する本体部８０２とを有する。レンズ８０１は、作業機４の姿勢にかかわらず、３つのマーカ７０１～７０３のうち少なくとも１つが視野に入る画角を有している。The camera 800 has a lens 801 and a main body 802 that houses an image sensor. The lens 801 has an angle of view that brings at least one of the three markers 701-703 into view, regardless of the attitude of the work machine 4.
カメラ８００は、ドローバ４０の上面に取り付けられている。カメラ８００は、作業機４が中立位置のとき、フロントフレーム２２の直下に位置する。カメラ８００は、ドローバ４０の中心軸Ｊ１上に位置する。The camera 800 is attached to the upper surface of the drawbar 40. When the work implement 4 is in the neutral position, the camera 800 is located directly below the front frame 22. The camera 800 is located on the central axis J1 of the drawbar 40.
カメラ８００は、ドローバ４０の前端部４０ｆと後端部４０ｒとの間に位置する。カメラ８００は、旋回サークル４１よりも前方に位置する。カメラ８００の光軸が中心軸Ｊ１に平行となるように、カメラ８００はドローバ４０に取り付けられている。カメラ８００は、レンズ８０１が本体部８０２よりも前方となるように、ドローバ４０に取り付けられている。The camera 800 is located between the front end 40f and rear end 40r of the drawbar 40. The camera 800 is located forward of the turning circle 41. The camera 800 is attached to the drawbar 40 so that the optical axis of the camera 800 is parallel to the central axis J1. The camera 800 is attached to the drawbar 40 so that the lens 801 is forward of the main body 802.
各マーカ７０１～７０３は、フロントフレーム２２に取り付けられている。各マーカ７０１～７０３は、フロントフレーム２２の前端部２２ｆに取り付けられている。各マーカ７０１～７０３は、ドローバ４０の前方に取り付けられている。各マーカ７０１～７０３は、玉軸４０２の近傍に取り付けられている。Each of the markers 701-703 is attached to the front frame 22. Each of the markers 701-703 is attached to the front end 22f of the front frame 22. Each of the markers 701-703 is attached in front of the drawbar 40. Each of the markers 701-703 is attached near the ball axle 402.
各マーカ７０１～７０３は、マトリクス状に配置された複数の領域（セル）を有し、複数の領域の一部が黒色で塗られている。各マーカ７０１～７０３は、複数の白色のセルと複数の黒色のセルとで構成されるパターンを有する。マーカ７０１のパターンと、マーカ７０２のパターンと、マーカ７０３のパターンとは互いに異なる。各パターンは、板状部材の表面に作成または当該表面に貼り付けられている。なお、図２および後述する図４～図７では、当該パターンは図示していない。Each of markers 701-703 has multiple regions (cells) arranged in a matrix, with some of the multiple regions painted black. Each of markers 701-703 has a pattern made up of multiple white cells and multiple black cells. The pattern of marker 701, the pattern of marker 702, and the pattern of marker 703 are different from one another. Each pattern is created on the surface of a plate-like member or affixed to that surface. Note that these patterns are not shown in Figure 2 and Figures 4-7, which will be described later.
マーカ７０１は、玉軸４０２の近傍となるフロントフレーム２２の第１所定位置に取り付けられている。第１所定位置は、玉軸４０２の上側である。マーカ７０１は、玉軸４０２の上方に設けられている。本例では、マーカ７０１は、玉軸４０２の直上に設けられている。マーカ７０１は、フロントフレーム２２の下面において、左右の前輪１１の中間に設けられている。The marker 701 is attached to a first predetermined position on the front frame 22, near the ball axle 402. The first predetermined position is above the ball axle 402. The marker 701 is provided above the ball axle 402. In this example, the marker 701 is provided directly above the ball axle 402. The marker 701 is provided on the underside of the front frame 22, midway between the left and right front wheels 11.
マーカ７０２は、玉軸４０２の近傍となるフロントフレーム２２の第２所定位置に取り付けられている。第２所定位置は、玉軸４０２の右側である。マーカ７０３は、玉軸４０２の近傍となるフロントフレーム２２の第３所定位置に取り付けられている。第３所定位置は、玉軸４０２の左側である。なお、本例では、第２所定位置と第３所定位置とは、作業機４の平面視において、フロントフレーム２２に関して線対称の関係にある。Marker 702 is attached to a second predetermined position on the front frame 22 near the ball pivot 402. The second predetermined position is to the right of the ball pivot 402. Marker 703 is attached to a third predetermined position on the front frame 22 near the ball pivot 402. The third predetermined position is to the left of the ball pivot 402. In this example, the second predetermined position and the third predetermined position are line-symmetrical with respect to the front frame 22 when viewed from above the work machine 4.
詳しくは、マーカ７０１は、フロントフレーム２２の下面の形状に沿って、傾斜した状態でフロントフレーム２２に取り付けられている。マーカ７０１は、玉軸４０２から離れた箇所ほどフロントフレーム２２の後方側となるように、傾斜している。More specifically, the marker 701 is attached to the front frame 22 in an inclined state, following the shape of the underside of the front frame 22. The marker 701 is inclined so that the further away from the ball axis 402 the marker is, the more rearward it is on the front frame 22.
マーカ７０２とマーカ７０３とは、作業機４の平面視において、フロントフレーム２２に関して線対称の関係にある。マーカ７０２，７０３も、マーカ７０１と同様、玉軸４０２から離れた箇所ほどフロントフレーム２２の後方側となるように、傾斜した状態でフロントフレーム２２に取り付けられている。各マーカ７０１～７０３が上記のように傾斜している理由は、作業機４が中立状態の場合に、各マーカ７０１～７０３がカメラ８００に向くようにするためである。Markers 702 and 703 are linearly symmetrical with respect to the front frame 22 when viewed from above the work machine 4. Like marker 701, markers 702 and 703 are attached to the front frame 22 at an angle so that the further away from the ball axis 402 they are, the more rearward they are on the front frame 22. The reason each of the markers 701-703 is angled as described above is so that each of the markers 701-703 faces the camera 800 when the work machine 4 is in a neutral position.
各マーカ７０１～７０３は、フロントフレーム２２に取り付けられている。それゆえ、フロントフレーム２２および玉軸４０２に対する各マーカ７０１～７０３の位置（相対位置）は、変化しない。一方、カメラ８００は、ドローバ４０に取り付けられている。したがって、少なくともドローバ４０の姿勢が変化すると、カメラ８００に対する各マーカ７０１～７０３の位置（相対位置）は変化する。Each of the markers 701-703 is attached to the front frame 22. Therefore, the position (relative position) of each of the markers 701-703 with respect to the front frame 22 and the ball axis 402 does not change. On the other hand, the camera 800 is attached to the drawbar 40. Therefore, at least when the attitude of the drawbar 40 changes, the position (relative position) of each of the markers 701-703 with respect to the camera 800 changes.
本例では、各マーカ７０１～７０３として、ＡＲ（Augmented Reality）マーカを用いている。ただし、各マーカ７０１～７０３は、ＡＲマーカに限定されるものではない。各マーカ７０１～７０３が互いに識別できるものであればよい。In this example, AR (Augmented Reality) markers are used as the markers 701-703. However, the markers 701-703 are not limited to AR markers. They may be any markers as long as they can be distinguished from one another.
各マーカ７０１～７０３は、各マーカの識別子と、カメラ８００を基準としたときの各マーカ７０１～７０３の姿勢とが判別できるものであればよい。詳しくは、各マーカ７０１～７０３の姿勢に関しては、各マーカ７０１～７０３は、カメラ８００を基準としたときの、各マーカ７０１～７０３の位置（３次元直交座標系における位置）と、各マーカ７０１～７０３の傾きの状態（オイラー角）とが判別できるものであればよい。なお、当該判別は、カメラ８００によって取得された画像データにもとづき、後述するコントローラ１５０（詳しくは、プロセッサ）によって実行される。It is sufficient that the identifier of each marker 701-703 and the orientation of each marker 701-703 when the camera 800 is used as a reference can be determined. Specifically, with regard to the orientation of each marker 701-703, it is sufficient that the position (position in a three-dimensional Cartesian coordinate system) and the tilt state (Euler angle) of each marker 701-703 when the camera 800 is used as a reference can be determined. This determination is made by the controller 150 (more specifically, the processor), described below, based on image data acquired by the camera 800.
なお、上述した各マーカ７０１～７０３の配置は、一例であって、上記に限定されるものではない。なお、玉軸４０２は、本開示の「連結部材」の一例である。フロントフレーム２２に取り付けられた３つのマーカ７０１～７０３のうちの任意の１つは、本開示の「第１の識別用マーカ」の一例である。マーカ７０１～７０３のうちの残りの２つのマーカのうちの任意の１つは、本開示の「第２の識別用マーカ」の一例である。The arrangement of the markers 701-703 described above is an example and is not limited to the above. The ball axle 402 is an example of a "connecting member" in the present disclosure. Any one of the three markers 701-703 attached to the front frame 22 is an example of a "first identification marker" in the present disclosure. Any one of the remaining two markers 701-703 is an example of a "second identification marker" in the present disclosure.
図３は、作業機４が図２の状態にある場合に、カメラ８００による撮像によって得られた画像データに基づく画像を示した図である。図３に示されるように、画像１０２Ｇは、フロントフレーム２２の画像２２Ｇと、ドローバ４０の画像４０Ｇと、玉軸４０２の画像４０２Ｇと、マーカ７０１の画像７０１Ｇと、マーカ７０２の画像７０２Ｇと、マーカ７０３の画像７０３Ｇとを含む。Figure 3 shows an image based on image data captured by camera 800 when work implement 4 is in the state shown in Figure 2. As shown in Figure 3, image 102G includes image 22G of front frame 22, image 40G of drawbar 40, image 402G of ball shaft 402, image 701G of marker 701, image 702G of marker 702, and image 703G of marker 703.
マーカ７０１の全体が撮像されている。カメラ８００から視て、マーカ７０２，７０３よりもドローバ４０が手前に位置するため、マーカ７０２，７０３は全体が撮像されていない。マーカ７０２は、左下部分が撮像されていない。マーカ７０３は、右下部分が撮像されていない。Marker 701 is captured in its entirety. Because the drawbar 40 is positioned closer to camera 800 than markers 702 and 703, markers 702 and 703 are not captured in their entirety. The lower left portion of marker 702 is not captured. The lower right portion of marker 703 is not captured.
仮想線Ｌは、画像１０２Ｇの左右を均等に分割する線分である。作業機４が中立状態にあるため、画像１０２Ｇの左右の中央に画像７０１Ｇが位置している。画像７０１Ｇは、仮想線Ｌに対して左右対称である。作業機４が中立状態にあるため、画像１０２Ｇ内における画像７０２Ｇの位置と画像７０３Ｇの位置とは、仮想線Ｌに対して左右対称である。Virtual line L is a line segment that divides image 102G equally into left and right halves. Because the work implement 4 is in a neutral state, image 701G is located in the center of image 102G. Image 701G is symmetrical with respect to virtual line L. Because the work implement 4 is in a neutral state, the positions of images 702G and 703G within image 102G are symmetrical with respect to virtual line L.
詳細については、後述するが、カメラ８００による撮像によって得られた画像データに基づき、ドローバ４０の姿勢が算出される。Details will be provided later, but the attitude of the drawbar 40 is calculated based on image data captured by the camera 800.
図４は、図２の状態からドローバ４０を右側に移動させた状態を示した図である。図４に示されるように、カメラ８００に対する各マーカ７０１～７０３の位置（相対位置）は、図２に示した位置から変化する。このように、ドローバ４０の移動により、カメラ８００の位置を基準とした各マーカ７０１～７０３の位置が変化する。Figure 4 shows the state in which the drawbar 40 has been moved to the right from the state in Figure 2. As shown in Figure 4, the positions (relative positions) of the markers 701-703 with respect to the camera 800 change from the positions shown in Figure 2. In this way, the movement of the drawbar 40 changes the positions of the markers 701-703 relative to the position of the camera 800.
図５は、作業機４が図４の状態にある場合に、カメラ８００による撮像によって得られた画像データに基づく画像を示した図である。図５に示されるように、画像１０４Ｇは、図３に示した画像１０２Ｇと同様に、画像２２Ｇと、画像４０Ｇと、画像４０２Ｇと、画像７０１Ｇと、画像７０２Ｇと、画像７０３Ｇとを含む。画像１０４Ｇは、カウンタウェイト５１の画像５１Ｇをさらに含む。FIG. 5 shows an image based on image data captured by camera 800 when work implement 4 is in the state shown in FIG. 4. As shown in FIG. 5, image 104G, like image 102G shown in FIG. 3, includes image 22G, image 40G, image 402G, image 701G, image 702G, and image 703G. Image 104G further includes image 51G of counterweight 51.
図３に示した画像１０２Ｇと比べると、図５においては、少なくとも各マーカ７０１～７０３の画像７０１Ｇ～７０３Ｇの姿勢が変化している。詳しくは、画像（詳しくは、画像データ）における画像７０１Ｇ～７０３Ｇの位置と形状とが、図３に示した画像７０１Ｇ～７０３Ｇの位置および形状から変化している。Compared to image 102G shown in Figure 3, in Figure 5, at least the orientation of images 701G-703G of markers 701-703 has changed. More specifically, the position and shape of images 701G-703G in the image (more specifically, image data) have changed from the position and shape of images 701G-703G shown in Figure 3.
図６は、図４の状態からドローバ４０を右上方に移動させた状態を示した図である。図６に示されるように、カメラ８００に対する各マーカ７０１～７０３の位置（相対位置）は、図４に示した位置からさらに変化する。この場合、マーカ７０２の全体がドローバ４０の前方に位置することになり、カメラ８００からはマーカ７０２は見えない。マーカ７０２は、カメラ８００によって撮像されない。Figure 6 shows the state in which the drawbar 40 has been moved to the upper right from the state in Figure 4. As shown in Figure 6, the positions (relative positions) of the markers 701 to 703 relative to the camera 800 change further from the positions shown in Figure 4. In this case, the entire marker 702 is located in front of the drawbar 40, and the marker 702 is not visible to the camera 800. The marker 702 is not captured by the camera 800.
図７は、作業機４が図６の状態にある場合に、カメラ８００による撮像によって得られた画像データに基づく画像を示した図である。図７に示されるように、画像１０６Ｇは、画像２２Ｇと、画像４０Ｇと、画像４０２Ｇと、画像７０１Ｇと、画像７０３Ｇとを含む。画像１０６Ｇは、画像１０２Ｇ，１０４Ｇとは異なり、マーカ７０２の画像７０２Ｇを含まない。Figure 7 shows an image based on image data captured by camera 800 when work machine 4 is in the state shown in Figure 6. As shown in Figure 7, image 106G includes image 22G, image 40G, image 402G, image 701G, and image 703G. Unlike images 102G and 104G, image 106G does not include image 702G of marker 702.
図３および図５に示した画像１０２Ｇ，画像１０４Ｇに比べると、図７においては、少なくとも各マーカ７０１，７０３の画像７０１Ｇ，７０３Ｇの姿勢が異なる。詳しくは、画像（詳しくは、画像データ）における画像７０１Ｇ，７０３Ｇの位置と形状とが、図３および図５に示した画像７０１Ｇ，７０３Ｇの位置および形状から変化している。Compared to images 102G and 104G shown in Figures 3 and 5, in Figure 7, at least the postures of images 701G and 703G of markers 701 and 703 are different. More specifically, the position and shape of images 701G and 703G in the image (more specifically, image data) have changed from the position and shape of images 701G and 703G shown in Figures 3 and 5.
以下、図８および図９に基づいて、カメラ８００による各マーカ７０１～７０３の撮像によって得られた画像データからドローバ４０の姿勢を算出する手法について説明する。さらに、算出されたドローバ４０の姿勢に基づきブレード４２の姿勢およびブレード４２の刃先４２ａ（図２）の位置を算出する処理についても説明する。Below, a method for calculating the attitude of the drawbar 40 from image data obtained by capturing images of the markers 701-703 using the camera 800 will be described with reference to Figures 8 and 9. Furthermore, a process for calculating the attitude of the blade 42 and the position of the cutting edge 42a (Figure 2) of the blade 42 based on the calculated attitude of the drawbar 40 will also be described.
なお、３つのマーカ７０１～７０３のうち、少なくとも１つのマーカの画像データが得られれば、ドローバ４０の姿勢を算出できる。２つ以上のマーカの画像データが得られれば、ドローバ４０の姿勢をより精度よく算出できる。Note that if image data of at least one of the three markers 701-703 is obtained, the attitude of the drawbar 40 can be calculated. If image data of two or more markers is obtained, the attitude of the drawbar 40 can be calculated with greater accuracy.
図８は、モータグレーダ１の機能的構成を説明するブロック図である。図８に示されるように、モータグレーダ１は、カメラ８００と、旋回サークル４１と、ブレードシフトシリンダ４７と、チルトシリンダ４８とに加えて、コントローラ１５０を備える。さらに、モータグレーダ１は、図示しない、旋回サークル４１の回転角度を検出するセンサ（以下、「センサ＃１」と称する。）と、ブレードシフトシリンダ４７のストローク量を検出するセンサ（以下、「センサ＃２」と称する。）と、チルトシリンダ４８のストローク量を検出するセンサ（以下、「センサ＃３」と称する。）とを備える。なお、センサ＃１として、ポテンショメータを用いることができる。Figure 8 is a block diagram explaining the functional configuration of the motor grader 1. As shown in Figure 8, the motor grader 1 is equipped with a camera 800, a turning circle 41, a blade shift cylinder 47, a tilt cylinder 48, and a controller 150. The motor grader 1 also has a sensor (not shown) that detects the rotation angle of the turning circle 41 (hereinafter referred to as "sensor #1"), a sensor (not shown) that detects the stroke amount of the blade shift cylinder 47 (hereinafter referred to as "sensor #2"), and a sensor (not shown) that detects the stroke amount of the tilt cylinder 48 (hereinafter referred to as "sensor #3"). Note that a potentiometer can be used as sensor #1.
コントローラ１５０は、ドローバ姿勢算出部１５１と、ブレード姿勢算出部１５２と、刃先位置算出部１５３とを有する。なお、ドローバ姿勢算出部１５１と、ブレード姿勢算出部１５２と、刃先位置算出部１５３とは、典型的には図示しないプロセッサがプログラムを実行することにより実現される機能ブロック図である。なお、これに限らず、ドローバ姿勢算出部１５１と、ブレード姿勢算出部１５２と、刃先位置算出部１５３とを、ＡＳＩＣ等のハードウェア（集積回路）のみによって実現してもよい。The controller 150 has a drawbar attitude calculation unit 151, a blade attitude calculation unit 152, and a cutting edge position calculation unit 153. Note that the drawbar attitude calculation unit 151, the blade attitude calculation unit 152, and the cutting edge position calculation unit 153 are functional block diagrams that are typically implemented by a processor (not shown) executing a program. Note that this is not a limitation, and the drawbar attitude calculation unit 151, the blade attitude calculation unit 152, and the cutting edge position calculation unit 153 may also be implemented solely by hardware (integrated circuits) such as an ASIC.
ドローバ姿勢算出部１５１は、カメラ８００による撮像によって得られた画像データを、カメラ８００から周期的に取得する。ドローバ姿勢算出部１５１は、取得した画像データに基づいて、ドローバ４０の姿勢を周期的に算出する。なお、ドローバ姿勢算出部１５１がカメラ８００から画像データを取得する周期と、ドローバ４０の姿勢を算出する周期とは同じでなくてもよい。以下では、ドローバ姿勢算出部１５１の処理について説明する。The drawbar attitude calculation unit 151 periodically acquires image data obtained by imaging with the camera 800 from the camera 800. The drawbar attitude calculation unit 151 periodically calculates the attitude of the drawbar 40 based on the acquired image data. Note that the period in which the drawbar attitude calculation unit 151 acquires image data from the camera 800 does not have to be the same as the period in which it calculates the attitude of the drawbar 40. The processing of the drawbar attitude calculation unit 151 is described below.
図９は、ドローバ姿勢算出部１５１における処理の流れを説明するためのフロー図である。図９は、ドローバ４０の姿勢を算出するためのフロー図である。Figure 9 is a flow diagram for explaining the processing flow in the drawbar attitude calculation unit 151. Figure 9 is a flow diagram for calculating the attitude of the drawbar 40.
図９に示されるように、ステップＳ１において、ドローバ姿勢算出部１５１は、カメラ８００から取得した画像データに基づいて、各マーカ７０１，７０２，７０３の３次元直交座標と、各マーカ７０１，７０２，７０３のオイラー角とを算出する。詳しくは、ドローバ姿勢算出部１５１は、作業機４が中立位置にあるときの各マーカ７０１，７０２，７０３の３次元直交座標（固定値）と、各マーカ７０１，７０２，７０３のオイラー角（固定値）とに基づいて、画像データから各マーカ７０１，７０２，７０３の３次元直交座標とオイラー角とを算出する。なお、上記固定値は、コントローラ１５０に予め格納されている。後述する各固定値についても、コントローラ１５０に予め格納されている。As shown in FIG. 9 , in step S1, the drawbar attitude calculation unit 151 calculates the three-dimensional Cartesian coordinates and Euler angles of each marker 701, 702, and 703 based on image data acquired from the camera 800. Specifically, the drawbar attitude calculation unit 151 calculates the three-dimensional Cartesian coordinates and Euler angles of each marker 701, 702, and 703 from the image data based on the three-dimensional Cartesian coordinates (fixed values) of each marker 701, 702, and 703 and the Euler angles (fixed values) of each marker 701, 702, and 703 when the work implement 4 is in the neutral position. Note that the above fixed values are pre-stored in the controller 150. Each fixed value, which will be described later, is also pre-stored in the controller 150.
オイラー角として、たとえば、ＺＹＸオイラー角を用いることができる。ＺＹＸオイラー角は、ｘ軸回りのロール角φと、ｙ軸回りのピッチ角θと、ｚ軸回りのヨー角ψとで規定される。As Euler angles, for example, ZYX Euler angles can be used. ZYX Euler angles are defined by the roll angle φ around the x-axis, the pitch angle θ around the y-axis, and the yaw angle ψ around the z-axis.
ステップＳ２において、ドローバ姿勢算出部１５１は、マーカ７０１の３次元直交座標とマーカ７０１のオイラー角とに基づき、カメラ８００を基準としたときの玉軸４０２の中心の３次元直交座標と玉軸４０２のオイラー角とを算出する。ドローバ姿勢算出部１５１は、カメラ８００の位置を基準として、３次元直交座標とオイラー角とを算出する。詳しくは、ドローバ姿勢算出部１５１は、作業機４が中立位置にあるときの、マーカ７０１の３次元直交座標（固定値）とマーカ７０１のオイラー角（固定値）との情報を用いて、３次元直交座標とオイラー角とを算出する。In step S2, the drawbar attitude calculation unit 151 calculates the three-dimensional Cartesian coordinates of the center of the ball axis 402 and the Euler angles of the ball axis 402 when the camera 800 is used as the reference, based on the three-dimensional Cartesian coordinates of the marker 701 and the Euler angles of the marker 701. The drawbar attitude calculation unit 151 calculates the three-dimensional Cartesian coordinates and Euler angles using the position of the camera 800 as the reference. In more detail, the drawbar attitude calculation unit 151 calculates the three-dimensional Cartesian coordinates and Euler angles using information on the three-dimensional Cartesian coordinates (fixed values) of the marker 701 and the Euler angles (fixed values) of the marker 701 when the work implement 4 is in the neutral position.
同様に、ステップＳ３において、ドローバ姿勢算出部１５１は、マーカ７０２の３次元直交座標とオイラー角とに基づき、カメラ８００を基準としたときの玉軸４０２の中心の３次元直交座標と玉軸４０２のオイラー角とを算出する。詳しくは、ドローバ姿勢算出部１５１は、作業機４が中立位置にあるときの、マーカ７０２の３次元直交座標（固定値）とマーカ７０２のオイラー角（固定値）との情報を用いて、３次元直交座標とオイラー角とを算出する。Similarly, in step S3, the drawbar attitude calculation unit 151 calculates the three-dimensional Cartesian coordinates of the center of the ball axis 402 and the Euler angles of the ball axis 402 when the camera 800 is used as the reference, based on the three-dimensional Cartesian coordinates and Euler angles of the marker 702. In more detail, the drawbar attitude calculation unit 151 calculates the three-dimensional Cartesian coordinates and Euler angles using information on the three-dimensional Cartesian coordinates (fixed values) of the marker 702 and the Euler angles (fixed values) of the marker 702 when the work implement 4 is in the neutral position.
ステップＳ４において、ドローバ姿勢算出部１５１は、マーカ７０３の３次元直交座標とオイラー角とに基づき、カメラ８００を基準としたときの玉軸４０２の中心の３次元直交座標と玉軸４０２のオイラー角とを算出する。詳しくは、ドローバ姿勢算出部１５１は、作業機４が中立位置にあるときの、マーカ７０３の３次元直交座標（固定値）とマーカ７０３のオイラー角（固定値）との情報を用いて、３次元直交座標とオイラー角とを算出する。なお、ステップＳ２，Ｓ３，Ｓ４の順序は、特に限定されない。In step S4, the drawbar attitude calculation unit 151 calculates the three-dimensional Cartesian coordinates of the center of the ball axis 402 and the Euler angles of the ball axis 402 when the camera 800 is used as the reference, based on the three-dimensional Cartesian coordinates and Euler angles of the marker 703. In more detail, the drawbar attitude calculation unit 151 calculates the three-dimensional Cartesian coordinates and Euler angles using information on the three-dimensional Cartesian coordinates (fixed values) of the marker 703 and the Euler angles (fixed values) of the marker 703 when the work implement 4 is in the neutral position. Note that the order of steps S2, S3, and S4 is not particularly limited.
ステップＳ５において、ドローバ姿勢算出部１５１は、各ステップＳ２～Ｓ４で算出された各３次元直交座標と各オイラー角とに異常値が含まれているか否かを判断する。具体的には、ドローバ姿勢算出部１５１は、予め設定された座標範囲内に、各３次元直交座標が収まっているか否かを判断する。ドローバ姿勢算出部１５１は、予め設定されたオイラー角範囲内に、各オイラー角が収まっているか否かを判断する。詳しくは、ドローバ姿勢算出部１５１は、成就地した３つの独立変数ごとに、予め設定された範囲内に収まっているかを判断する。ドローバ姿勢算出部１５１は、予め設定された範囲に収まっていない３次元座標およびオイラー角を異常値と判断する。In step S5, the drawbar attitude calculation unit 151 determines whether each of the three-dimensional orthogonal coordinates and each of the Euler angles calculated in steps S2 to S4 contains an abnormal value. Specifically, the drawbar attitude calculation unit 151 determines whether each of the three-dimensional orthogonal coordinates falls within a preset coordinate range. The drawbar attitude calculation unit 151 determines whether each of the Euler angles falls within a preset Euler angle range. More specifically, the drawbar attitude calculation unit 151 determines whether each of the three independent variables falls within a preset range. The drawbar attitude calculation unit 151 determines that three-dimensional coordinates and Euler angles that do not fall within the preset range are abnormal values.
異常値が含まれていないと判断された場合（ステップＳ５においてＮＯ）、ドローバ姿勢算出部１５１は、処理をステップＳ６に進める。異常値が含まれていると判断された場合（ステップＳ５においてＹＥＳ）、ドローバ姿勢算出部１５１は、ステップＳ８において、精度を高めるため、異常値を削除する。ドローバ姿勢算出部１５１は、その後、処理をステップＳ６に進める。If it is determined that no abnormal values are included (NO in step S5), the drawbar attitude calculation unit 151 proceeds to step S6. If it is determined that an abnormal value is included (YES in step S5), the drawbar attitude calculation unit 151 deletes the abnormal value in step S8 to improve accuracy. The drawbar attitude calculation unit 151 then proceeds to step S6.
ステップＳ６において、ドローバ姿勢算出部１５１は、ステップＳ２～Ｓ４で算出された各３次元直交座標（異常値除く）および各オイラー角（異常値除く）に基づき、カメラ８００を基準としたときの玉軸４０２の中心の座標と玉軸４０２のオイラー角とを決定する。たとえば、ドローバ姿勢算出部１５１は、各３次元直交座標（異常値除く）の平均を算出することにより、カメラ８００を基準としたときの玉軸４０２の中心の座標を決定する。ドローバ姿勢算出部１５１は、各オイラー角（異常値除く）の平均を算出することにより、カメラ８００を基準としたときの玉軸４０２のオイラー角を決定する。In step S6, the drawbar attitude calculation unit 151 determines the coordinates of the center of the ball axis 402 when the camera 800 is used as the reference and the Euler angles of the ball axis 402 based on the three-dimensional Cartesian coordinates (excluding abnormal values) and Euler angles (excluding abnormal values) calculated in steps S2 to S4. For example, the drawbar attitude calculation unit 151 determines the coordinates of the center of the ball axis 402 when the camera 800 is used as the reference by calculating the average of the three-dimensional Cartesian coordinates (excluding abnormal values). The drawbar attitude calculation unit 151 determines the Euler angles of the ball axis 402 when the camera 800 is used as the reference by calculating the average of the Euler angles (excluding abnormal values).
ステップＳ７において、ドローバ姿勢算出部１５１は、ステップＳ６で決定された３次元直交座標とオイラー角とを玉軸４０２の中心を基準としたときの３次元直交座標とオイラー角とに変換することにより、ドローバ４０の姿勢を算出する。ドローバ姿勢算出部１５１は、所定の演算式を用いて逆変換を行うことにより、玉軸４０２の中心を基準としたときのドローバ４０の姿勢を算出する。In step S7, the drawbar attitude calculation unit 151 calculates the attitude of the drawbar 40 by converting the three-dimensional Cartesian coordinates and Euler angles determined in step S6 into three-dimensional Cartesian coordinates and Euler angles based on the center of the ball axis 402. The drawbar attitude calculation unit 151 calculates the attitude of the drawbar 40 based on the center of the ball axis 402 by performing an inverse transformation using a predetermined calculation formula.
以上により、カメラ８００による撮像によって得られた画像データから、ドローバ４０の姿勢が算出される。ドローバ４０の姿勢の算出は、上述したように周期的に実行される。算出されたドローバ４０の姿勢の情報（以下、「ドローバ姿勢情報」とも称する）は、図８に示すように、ブレード姿勢算出部１５２に周期的に送られる。As described above, the attitude of the drawbar 40 is calculated from the image data obtained by imaging with the camera 800. Calculation of the attitude of the drawbar 40 is performed periodically as described above. Information about the calculated attitude of the drawbar 40 (hereinafter also referred to as "drawbar attitude information") is periodically sent to the blade attitude calculation unit 152, as shown in FIG. 8.
次に、ブレード姿勢算出部１５２の処理について説明する。ブレード姿勢算出部１５２は、ドローバ姿勢算出部１５１から、ドローバ姿勢情報を周期的に取得する。ブレード姿勢算出部１５２は、旋回サークル４１の回転角度の情報を、上述したセンサ＃１から周期的に取得する。ブレード姿勢算出部１５２は、ブレードシフトシリンダ４７のストローク量を示した情報を、上述したセンサ＃２から取得する。ブレード姿勢算出部１５２は、チルトシリンダ４８のストローク量を示した情報を、上述したセンサ＃３から取得する。Next, the processing of the blade attitude calculation unit 152 will be described. The blade attitude calculation unit 152 periodically acquires drawbar attitude information from the drawbar attitude calculation unit 151. The blade attitude calculation unit 152 periodically acquires information on the rotation angle of the turning circle 41 from the above-mentioned sensor #1. The blade attitude calculation unit 152 acquires information indicating the stroke amount of the blade shift cylinder 47 from the above-mentioned sensor #2. The blade attitude calculation unit 152 acquires information indicating the stroke amount of the tilt cylinder 48 from the above-mentioned sensor #3.
ブレード姿勢算出部１５２は、ドローバ姿勢情報と、旋回サークル４１の回転角度の情報と、ブレードシフトシリンダ４７のストローク量を示した情報と、チルトシリンダ４８のストローク量を示した情報とに基づいて、ブレード４２の姿勢を周期的に算出する。ブレード姿勢算出部１５２は、算出したブレードの姿勢を示す情報（以下、「ブレード姿勢情報」）を刃先位置算出部１５３に周期的に送る。The blade attitude calculation unit 152 periodically calculates the attitude of the blade 42 based on the drawbar attitude information, information on the rotation angle of the turning circle 41, information indicating the stroke amount of the blade shift cylinder 47, and information indicating the stroke amount of the tilt cylinder 48. The blade attitude calculation unit 152 periodically sends information indicating the calculated blade attitude (hereinafter referred to as "blade attitude information") to the cutting edge position calculation unit 153.
刃先位置算出部１５３は、ブレード姿勢情報をブレード姿勢算出部１５２から取得すると、ブレード姿勢情報に基づき、ブレード４２の刃先４２ａの位置を算出する。刃先位置算出部１５３は、ブレード姿勢情報と予め定められた設計値とに基づき、刃先４２ａの位置を算出する。算出された刃先４２ａの位置の情報は、コントローラ１５０において、各種の処理に利用される。When the cutting edge position calculation unit 153 acquires blade attitude information from the blade attitude calculation unit 152, it calculates the position of the cutting edge 42a of the blade 42 based on the blade attitude information. The cutting edge position calculation unit 153 calculates the position of the cutting edge 42a based on the blade attitude information and predetermined design values. The calculated position information of the cutting edge 42a is used for various processes in the controller 150.
以上のように、モータグレーダ１は、図２等に示すように、玉軸４０２の近傍となるフロントフレーム２２の第１の所定位置に取り付けられたマーカ７０１と、玉軸４０２の近傍となるフロントフレーム２２の第２の所定位置に取り付けられたマーカ７０２と、玉軸４０２の近傍となるフロントフレーム２２の第３の所定位置に取り付けられたマーカ７０３とを備える。モータグレーダ１は、図２等に示すように、ドローバ４０に取り付けられ、かつ、マーカ７０１～７０３を撮像するカメラ８００をさらに備える。モータグレーダ１は、図８に示すように、撮像により得られたマーカ７０１～７０３の各画像データに基づいて、ドローバ４０の姿勢を算出するコントローラ１５０をさらに備える。As described above, the motor grader 1 includes a marker 701 attached to a first predetermined position on the front frame 22 near the ball axis 402, a marker 702 attached to a second predetermined position on the front frame 22 near the ball axis 402, and a marker 703 attached to a third predetermined position on the front frame 22 near the ball axis 402, as shown in FIG. 2 and other figures. The motor grader 1 also includes a camera 800 attached to the drawbar 40 and capturing images of the markers 701-703, as shown in FIG. 8. The motor grader 1 also includes a controller 150 that calculates the attitude of the drawbar 40 based on the image data of the markers 701-703 obtained by capturing the images.
このような構成によれば、フロントフレーム２２に取り付けられたマーカ７０１～７０３をドローバ４０に取り付けられたカメラ８００で撮像することにより、ドローバ４０の姿勢の情報を得ることができる。With this configuration, the markers 701-703 attached to the front frame 22 are captured by a camera 800 attached to the drawbar 40, thereby obtaining information about the attitude of the drawbar 40.
特に、モータグレーダ１によれば、１つのカメラ８００で複数のマーカ７０１～７０３を撮像可能である。それゆえ、モータグレーダ１には、複数のカメラは不要である。なお、マーカ７０１～７０３が取り付けられる位置は玉軸４０２の近傍である。このため、ブレード４２によって削られた土砂等がマーカ７０１～７０３に付着しにくい。よって、マーカ７０１～７０３は、汚れにくい。それゆえ、モータグレーダ１では、マーカ７０１～７０３の読み取り不良の虞が少ない。また、玉軸４０２の上方にはフロントフレーム２２が位置する。それゆえ、マーカ７０１～７０３に埃が溜まることを、フロントフレーム２２によって抑制できる。In particular, with the motor grader 1, multiple markers 701-703 can be captured with a single camera 800. Therefore, the motor grader 1 does not require multiple cameras. The markers 701-703 are attached near the ball shaft 402. Therefore, soil and sand scraped by the blade 42 are less likely to adhere to the markers 701-703. As a result, the markers 701-703 are less likely to become dirty. Therefore, with the motor grader 1, there is little risk of the markers 701-703 not being read properly. Furthermore, the front frame 22 is located above the ball shaft 402. Therefore, the front frame 22 can prevent dust from accumulating on the markers 701-703.
図４および図６からもわかるとおり、モータグレーダ１は極めて自由度の高い作業機姿勢を取り得る作業機械である。しかしながら、マーカ７０１～７０３を玉軸４０２の近傍に配置することにより、作業機４の姿勢が様々に変化しても、ドローバ４０に取り付けたカメラ８００によってマーカ７０１～７０３を撮像することができる。このため、モータグレーダ１によれば、ドローバ４０の姿勢の情報を得ることができる。As can be seen from Figures 4 and 6, the motor grader 1 is a work machine that can adopt a work machine posture with a very high degree of freedom. However, by arranging the markers 701-703 near the ball axis 402, the markers 701-703 can be captured by the camera 800 attached to the drawbar 40, even if the posture of the work machine 4 changes in various ways. Therefore, the motor grader 1 can obtain information on the posture of the drawbar 40.
なお、上記においては、モータグレーダ１が３つのマーカ７０１～７０３を備える構成を例に挙げて説明したが、これに限定されるものではない。モータグレーダ１は、少なくとも１つのマーカを備えていればよい。モータグレーダ１は、少なくとも左右方向において中央のマーカ７０１を備えていることが好ましい。ただし、複数のマーカ７０１～７０３を備えることにより、マーカの汚れと破損とに対する冗長性を確保できるとともに、「ドローバ４０の姿勢」を精度良く算出できる（図９のステップＳ６）。Note that, although the above description has been given using an example of a configuration in which the motor grader 1 is equipped with three markers 701-703, this is not limiting. The motor grader 1 only needs to be equipped with at least one marker. It is preferable that the motor grader 1 is equipped with at least the central marker 701 in the left-right direction. However, by providing multiple markers 701-703, redundancy can be ensured in the event of marker dirt or damage, and the "posture of the drawbar 40" can be calculated with high accuracy (step S6 in Figure 9).
ドローバ４０の姿勢は、玉軸４０２に対するドローバ４０の姿勢である。コントローラ１５０は、図９のステップＳ６に示したように、マーカ７０１～７０３の各画像データに基づいて、カメラ８００に対する玉軸４０２の姿勢を算出する。コントローラ１５０は、図９のステップＳ７に示したように、カメラ８００に対する玉軸４０２の姿勢に基づき、玉軸４０２に対するドローバ４０の姿勢を算出する。The attitude of the drawbar 40 is the attitude of the drawbar 40 relative to the ball axis 402. As shown in step S6 of FIG. 9, the controller 150 calculates the attitude of the ball axis 402 relative to the camera 800 based on the image data of the markers 701 to 703. As shown in step S7 of FIG. 9, the controller 150 calculates the attitude of the drawbar 40 relative to the ball axis 402 based on the attitude of the ball axis 402 relative to the camera 800.
このような構成によれば、ドローバ４０に取り付けたカメラ８００による撮像によって得られた画像データに基づいて、玉軸４０２に対するドローバ４０の姿勢を算出することが可能となる。With this configuration, it is possible to calculate the attitude of the drawbar 40 relative to the ball axis 402 based on image data obtained by capturing an image with the camera 800 attached to the drawbar 40.
カメラ８００は、ドローバ４０の上面に取り付けられている。このような構成によれば、カメラ８００がドローバ４０の側部または底部に取り付けられた構成に比べて、ブレード４２によって削られた土砂等がカメラ８００に付着することを防止できる。さらに、当該構成によれば、作業機４の姿勢が様々に変化しても、マーカ７０１～７０３の少なくとも１つをカメラ８００の画角内に収めることができる。Camera 800 is attached to the top surface of drawbar 40. This configuration prevents soil and sand scraped by blade 42 from adhering to camera 800, compared to a configuration in which camera 800 is attached to the side or bottom of drawbar 40. Furthermore, this configuration allows at least one of markers 701-703 to be kept within the field of view of camera 800, even if the posture of work machine 4 changes in various ways.
モータグレーダ１は、図１，２等に示すように、ブレード４２を支持し、かつ、ドローバ４０に旋回可能に取り付けられた旋回サークル４１をさらに備える。ドローバ４０は、図２に示すように、玉軸４０２に接続される前端部４０ｆを有する。カメラ８００は、図２に示すように、前端部４０ｆから旋回サークル４１の旋回中心Ｃに向かう方向に延びた、ドローバ４０の中心軸Ｊ１上に位置する。詳しくは、カメラ８００のレンズ８０１の光軸が、ドローバ４０の中心軸Ｊ１上に位置する。当該光軸が中心軸Ｊ１と平行になる。As shown in Figures 1 and 2, the motor grader 1 further includes a swivel circle 41 that supports a blade 42 and is rotatably attached to the drawbar 40. As shown in Figure 2, the drawbar 40 has a front end 40f that is connected to a ball shaft 402. As shown in Figure 2, the camera 800 is located on the central axis J1 of the drawbar 40, which extends from the front end 40f in a direction toward the center of rotation C of the swivel circle 41. More specifically, the optical axis of the lens 801 of the camera 800 is located on the central axis J1 of the drawbar 40. The optical axis is parallel to the central axis J1.
このような構成によれば、カメラ８００がドローバ４０の中心軸Ｊ１上に位置するため、ドローバ４０の姿勢に関わらず、カメラ８００によって玉軸４０２の周囲を撮像できる。よって、３つのマーカ７０１～７０３の少なくとも１つを撮像可能となる。With this configuration, the camera 800 is positioned on the central axis J1 of the drawbar 40, so the camera 800 can capture an image of the area around the ball axis 402 regardless of the orientation of the drawbar 40. This makes it possible to capture an image of at least one of the three markers 701 to 703.
さらに、作業機４を中立位置してモータグレーダ１を駐機すると、フロントフレーム２２がカメラ８００の真上に位置する。それゆえ、カメラ８００の上方にフロントフレーム２２がないときに比べて、カメラ８００に埃が溜まることを抑制できる。Furthermore, when the motor grader 1 is parked with the work implement 4 in the neutral position, the front frame 22 is located directly above the camera 800. Therefore, dust accumulation on the camera 800 can be reduced compared to when the front frame 22 is not located above the camera 800.
［実施の形態２］
次に、本実施の形態に係るモータグレーダ１Ａ（図１参照）について説明する。図１０は、モータグレーダ１Ａのフロントフレーム２２側の側面図である。図１１は、図１０の状態におけるモータグレーダ１Ａのフロントフレーム２２側の平面図である。図１０および図１１は、作業機４が中立位置となった状態を示している。[Embodiment 2]
Next, a motor grader 1A (see FIG. 1) according to this embodiment will be described. Fig. 10 is a side view of the front frame 22 side of the motor grader 1A. Fig. 11 is a plan view of the front frame 22 side of the motor grader 1A in the state shown in Fig. 10. Figs. 10 and 11 show a state in which the work implement 4 is in the neutral position.
中心軸Ｊ１は、作業機４が中立位置の場合、モータグレーダ１Ａの平面視（上面視）において、フロントフレーム２２の前後方向の軸である中心軸Ｊ３（図１１）と重なる。When the work implement 4 is in the neutral position, the central axis J1 overlaps with the central axis J3 (Figure 11), which is the longitudinal axis of the front frame 22, in a plan view (top view) of the motor grader 1A.
モータグレーダ１Ａは、カメラ８００Ａと、複数の識別用のマーカ７１１～７１３，７２１，７２２とをさらに備える。本例における各識別用のマーカ７１１～７１３，７２１，７２２は、カメラ８００Ａの被写体である。The motor grader 1A further includes a camera 800A and multiple identification markers 711-713, 721, and 722. In this example, each of the identification markers 711-713, 721, and 722 is the subject of the camera 800A.
カメラ８００Ａは、レンズ８０１Ａと、イメージセンサを内蔵する本体部８０２Ａとを有する。レンズ８０１Ａは、作業機４の姿勢にかかわらず、５つのマーカ７１１～７１３，７２１，７２２が視野に入る画角を有している。レンズ８０１Ａは、広角レンズである。好ましくは、レンズ８０１Ａは、超広角レンズである。本例では、レンズ８０１Ａは、広角レンズの一種である魚眼レンズである。Camera 800A has lens 801A and main body 802A with a built-in image sensor. Lens 801A has an angle of view that brings five markers 711-713, 721, and 722 into view regardless of the attitude of work machine 4. Lens 801A is a wide-angle lens. Preferably, lens 801A is an ultra-wide-angle lens. In this example, lens 801A is a fisheye lens, which is a type of wide-angle lens.
フロントフレーム２２は、ドローバ４０と対向する下端面２２ｕをさらに有する。カメラ８００Ａは、下端面２２ｕに取り付けられている。カメラ８００Ａは、レンズ８０１Ａの光軸が鉛直下向きとなるように、下端面２２ｕに取り付けられている。本例では、モータグレーダ１Ａの平面視において、カメラ８００Ａは、フロントフレーム２２の前後方向に延びた中心軸Ｊ３の直下に位置している。カメラ８００Ａは、モータグレーダ１Ａの側面視（図１０）において、ドローバ４０の前端部４０ｆと後端部４０ｒとの間に位置する。The front frame 22 further has a lower end surface 22u that faces the drawbar 40. The camera 800A is attached to the lower end surface 22u. The camera 800A is attached to the lower end surface 22u so that the optical axis of the lens 801A faces vertically downward. In this example, in a plan view of the motor grader 1A, the camera 800A is located directly below the central axis J3 extending in the fore-and-aft direction of the front frame 22. In a side view of the motor grader 1A (Figure 10), the camera 800A is located between the front end 40f and rear end 40r of the drawbar 40.
各マーカ７１１～７１３は、ドローバ４０に取り付けられている。各マーカ７２１，７２２は、ブレード４２に取り付けられている。なお、ドローバ４０に取り付けるマーカは、３個に限定されず、１個、２個、または４個以上であってもよい。ブレード４２に取り付けるマーカは、２個に限定されず、１個、または３個以上であってもよい。Each of the markers 711 to 713 is attached to the drawbar 40. Each of the markers 721 and 722 is attached to the blade 42. Note that the number of markers attached to the drawbar 40 is not limited to three, and may be one, two, or four or more. The number of markers attached to the blade 42 is not limited to two, and may be one, three, or more.
ただし、ドローバ４０に複数のマーカ７１１～７１３を取り付けることにより、マーカの汚れと破損とに対する冗長性を確保できるとともに、後述する「カメラ８００Ａに対するドローバ４０の姿勢」を精度良く算出できる（図１３のステップＳ１０、特に、図１４のステップＳ１６）。同様に、ブレード４２に複数のマーカ７２１，７２２を取り付けることにより、マーカの汚れと破損とに対する冗長性を確保できるとともに、後述する「カメラ８００Ａに対するブレード４２の姿勢」を精度良く算出できる（図１３のステップＳ２０、特に、図１５のステップＳ２５）。However, by attaching multiple markers 711-713 to the drawbar 40, redundancy can be ensured against marker contamination and damage, and the "attitude of the drawbar 40 relative to the camera 800A" described below can be calculated with high accuracy (step S10 in FIG. 13, and particularly step S16 in FIG. 14). Similarly, by attaching multiple markers 721, 722 to the blade 42, redundancy can be ensured against marker contamination and damage, and the "attitude of the blade 42 relative to the camera 800A" described below can be calculated with high accuracy (step S20 in FIG. 13, and particularly step S25 in FIG. 15).
各マーカ７１１～７１３，７２１，７２２は、マトリクス状に配置された複数の領域（セル）を有し、複数の領域の一部が黒色で塗られている。各マーカ７１１～７１３，７２１，７２２は、複数の白色のセルと複数の黒色のセルとで構成されるパターンを有する。マーカ７１１のパターンと、マーカ７１２のパターンと、マーカ７１３のパターンと、マーカ７２１のパターンと、マーカ７２２のパターンとは、互いに異なる。各パターンは、板状部材の表面に作成または当該表面に貼り付けられている。以下、マーカ７１１～７１３，７２１，７２２の取付位置の詳細について、具体例を挙げて説明する。Each of markers 711-713, 721, and 722 has multiple regions (cells) arranged in a matrix, with some of the multiple regions painted black. Each of markers 711-713, 721, and 722 has a pattern made up of multiple white cells and multiple black cells. The pattern of marker 711, the pattern of marker 712, the pattern of marker 713, the pattern of marker 721, and the pattern of marker 722 are all different from one another. Each pattern is created on the surface of a plate-like member or affixed to that surface. Details of the attachment positions of markers 711-713, 721, and 722 are explained below using specific examples.
マーカ７１１～７１３は、ドローバ４０の上面に取り付けられている。マーカ７１１～７１３は、少なくともドローバ４０が中立位置にある場合において、マーカ７１１～７１３（詳しくは、パターン）が上方を向くように、ドローバ４０に取り付けられている。Markers 711-713 are attached to the top surface of the drawbar 40. Markers 711-713 are attached to the drawbar 40 so that, at least when the drawbar 40 is in the neutral position, markers 711-713 (more specifically, patterns) face upward.
マーカ７１１は、モータグレーダ１Ａの平面視において、カメラ８００Ａよりも前方に設置されている。本例では、マーカ７１１は、作業機４の平面視において、中心軸Ｊ１の直上に配置されている。ドローバ４０が中立位置にある場合、マーカ７１１は、モータグレーダ１Ａの平面視において、中心軸Ｊ３の直下に位置する。The marker 711 is installed in front of the camera 800A in a plan view of the motor grader 1A. In this example, the marker 711 is located directly above the central axis J1 in a plan view of the work implement 4. When the drawbar 40 is in the neutral position, the marker 711 is located directly below the central axis J3 in a plan view of the motor grader 1A.
マーカ７１２，７１３は、モータグレーダ１Ａの平面視において、カメラ８００Ａよりも後方に設置されている。マーカ７１２，７１３は、マーカ７１１よりも後方に位置する。マーカ７１２は、作業機４の平面視において、中心軸Ｊ１から左方向に離間した位置に配置されている。マーカ７１３は、作業機４の平面視において、中心軸Ｊ１から右方向に離間した位置に配置されている。マーカ７１３は、中心軸Ｊ１に関し、マーカ７１２と線対称となる位置に設置されている。Markers 712 and 713 are located behind camera 800A in a plan view of motor grader 1A. Markers 712 and 713 are located behind marker 711. Marker 712 is located at a position spaced to the left of central axis J1 in a plan view of work machine 4. Marker 713 is located at a position spaced to the right of central axis J1 in a plan view of work machine 4. Marker 713 is located at a position that is linearly symmetrical to marker 712 with respect to central axis J1.
なお、マーカ７１１～７１３の設置位置は、上記の位置に限定されるものではない。カメラ８００Ａによって撮像可能な位置であれば、マーカ７１１～７１３の設置位置は特に限定されない。ただし、マーカの汚れと破損とに対する冗長性をより確保する観点からは、マーカ７１１～７１３同士は、所定距離以上離間していることが好ましい。Note that the installation locations of markers 711-713 are not limited to the above locations. As long as they are in a position that can be imaged by camera 800A, there are no particular limitations on the installation locations of markers 711-713. However, from the perspective of ensuring redundancy against marker dirt and damage, it is preferable that markers 711-713 are spaced apart by a predetermined distance or more.
マーカ７２１，７２２は、ブレード４２に取り付けられている。マーカ７２１，７２２は、ブレード４２の上方に取り付けられている。マーカ７２１，７２２は、支持部材７９０によって、ブレード４２に取り付けられている。マーカ７２１，７２２は、ブレード４２の上端面から離間した位置に設置されている。マーカ７２１，７２２は、少なくとも作業機４が中立位置にある場合、ドローバ４０に取り付けられた３つのマーカ７１１～７１３よりも後方に位置する。Markers 721 and 722 are attached to the blade 42. Markers 721 and 722 are attached above the blade 42. Markers 721 and 722 are attached to the blade 42 by support members 790. Markers 721 and 722 are installed at positions spaced apart from the upper end surface of the blade 42. Markers 721 and 722 are located rearward of the three markers 711 to 713 attached to the drawbar 40, at least when the work implement 4 is in the neutral position.
マーカ７２１は、ブレード４２の左端側に取り付けられている。マーカ７２２は、ブレード４２の右端側に取り付けられている。本例では、マーカ７２２は、矢印９０１方向におけるブレード４２の中心に対してマーカ７２１と対称となる位置に設置されている。Marker 721 is attached to the left end of blade 42. Marker 722 is attached to the right end of blade 42. In this example, marker 722 is positioned symmetrically to marker 721 with respect to the center of blade 42 in the direction of arrow 901.
マーカ７２１，７２２は、少なくとも作業機４が中立位置にある場合においてマーカ７２１，７２２の各パターンが前方を向くように、ブレード４２に取り付けられている。マーカ７２１，７２２の各パターン（平面）の法線が中心軸Ｊ１と平行になるように、マーカ７２１，７２２がブレード４２に取り付けられている。ただし、これに限定されず、マーカ７２１，７２２は、上記法線が中心軸Ｊ１と平行とならないようにブレード４２に取り付けられてもよい。作業機４が中立状態において、マーカ７２１，７２２（詳しくは、パターン）が上方を向いていてもよい。The markers 721, 722 are attached to the blade 42 so that the patterns of the markers 721, 722 face forward at least when the work implement 4 is in a neutral position. The markers 721, 722 are attached to the blade 42 so that the normal to each pattern (plane) of the markers 721, 722 is parallel to the central axis J1. However, this is not limited to this, and the markers 721, 722 may be attached to the blade 42 so that the normal is not parallel to the central axis J1. When the work implement 4 is in a neutral state, the markers 721, 722 (more specifically, the patterns) may face upward.
なお、マーカ７２１，７２２の設置位置は、上記の位置に限定されるものではない。カメラ８００Ａによって撮像可能な位置であれば、マーカ７２１，７２２の設置位置は特に限定されない。ただし、ブレード４２によって削られた土砂等による汚れを防ぐ観点から、上述したようにマーカ７２１，７２２をブレード４２の上方に設置することが好ましい。Note that the installation positions of markers 721 and 722 are not limited to the positions described above. As long as the markers are located in a position that can be imaged by camera 800A, there are no particular limitations on the installation positions of markers 721 and 722. However, from the perspective of preventing contamination by soil and sand scraped by blade 42, it is preferable to install markers 721 and 722 above blade 42, as described above.
各マーカ７１１～７１３は、ドローバ４０に取り付けられている。それゆえ、ドローバ４０に対する各マーカ７１１～７１３の位置（相対位置）は、変化しない。各マーカ７２１，７２２は、ブレード４２に取り付けられている。それゆえ、ブレード４２に対する各マーカ７２１，７２２の位置（相対位置）は、変化しない。Each of the markers 711-713 is attached to the drawbar 40. Therefore, the position (relative position) of each of the markers 711-713 with respect to the drawbar 40 does not change. Each of the markers 721 and 722 is attached to the blade 42. Therefore, the position (relative position) of each of the markers 721 and 722 with respect to the blade 42 does not change.
一方、カメラ８００Ａは、フロントフレーム２２に取り付けられている。したがって、少なくともドローバ４０の姿勢が変化すると、カメラ８００Ａに対する各マーカ７１１～７１３，７２１，７２２の位置（相対位置）は変化する。ドローバ４０の姿勢が中立位置である場合であっても、ブレード４２の姿勢が変化すると、カメラ８００Ａに対する各マーカ７２１，７２２の位置（相対位置）は変化する。On the other hand, camera 800A is attached to front frame 22. Therefore, when the attitude of at least drawbar 40 changes, the positions (relative positions) of markers 711-713, 721, and 722 relative to camera 800A change. Even when the attitude of drawbar 40 is in the neutral position, when the attitude of blade 42 changes, the positions (relative positions) of markers 721 and 722 relative to camera 800A change.
本例では、各マーカ７１１～７１３，７２１，７２２として、ＡＲ（Augmented Reality）マーカを用いている。ただし、各マーカ７１１～７１３，７２１，７２２は、ＡＲマーカに限定されるものではない。各マーカ７１１～７１３，７２１，７２２が互いに識別できるものであればよい。In this example, AR (Augmented Reality) markers are used as the markers 711-713, 721, and 722. However, the markers 711-713, 721, and 722 are not limited to AR markers. Any markers can be used as long as they can be distinguished from one another.
各マーカ７１１～７１３，７２１，７２２は、各マーカの識別子と、カメラ８００Ａを基準としたときの各マーカ７１１～７１３，７２１，７２２の姿勢とが判別できるものであればよい。詳しくは、各マーカ７１１～７１３，７２１，７２２の姿勢に関しては、各マーカ７１１～７１３，７２１，７２２は、カメラ８００Ａを基準としたときの、各マーカ７１１～７１３，７２１，７２２の位置（３次元座標系のおける位置）と、各マーカ７１１～７１３，７２１，７２２の傾きの状態（オイラー角）とが判別できるものであればよい。なお、当該判別は、カメラ８００Ａによって取得された画像データにもとづき、後述するコントローラ１５０Ａ（詳しくは、プロセッサ）によって実行される。It is sufficient that the identifier of each marker 711-713, 721, 722 and the orientation of each marker 711-713, 721, 722 when using camera 800A as a reference can be determined. Specifically, with regard to the orientation of each marker 711-713, 721, 722, it is sufficient that the position (position in a three-dimensional coordinate system) and the tilt state (Euler angle) of each marker 711-713, 721, 722 when using camera 800A as a reference can be determined. Note that this determination is performed by controller 150A (more specifically, a processor), described below, based on image data acquired by camera 800A.
なお、ドローバ４０に取り付けられた各マーカ７１１～７１３は、本開示の「第１の識別用マーカ」に相当する。ブレード４２に取り付けられた各マーカ７２１，７２２は、本開示の「第２の識別用マーカ」に相当する。Note that the markers 711-713 attached to the drawbar 40 correspond to the "first identification markers" in this disclosure. The markers 721 and 722 attached to the blade 42 correspond to the "second identification markers" in this disclosure.
図１２は、モータグレーダ１Ａの機能的構成を説明するブロック図である。図１２に示されるように、モータグレーダ１Ａは、カメラ８００Ａと、コントローラ１５０Ａとを備える。コントローラ１５０Ａは、ドローバ姿勢算出部１５１Ａと、ブレード姿勢算出部１５２Ａと、第１刃先位置算出部１５３Ａと、第２刃先位置算出部１５４とを備える。Figure 12 is a block diagram explaining the functional configuration of the motor grader 1A. As shown in Figure 12, the motor grader 1A includes a camera 800A and a controller 150A. The controller 150A includes a drawbar attitude calculation unit 151A, a blade attitude calculation unit 152A, a first cutting edge position calculation unit 153A, and a second cutting edge position calculation unit 154.
なお、ドローバ姿勢算出部１５１Ａと、ブレード姿勢算出部１５２Ａと、第１刃先位置算出部１５３Ａと、第２刃先位置算出部１５４とは、典型的には図示しないプロセッサがプログラムを実行することにより実現される機能ブロック図である。なお、これに限らず、ドローバ姿勢算出部１５１Ａと、ブレード姿勢算出部１５２Ａと、第１刃先位置算出部１５３Ａと、第２刃先位置算出部１５４とを、ＡＳＩＣ等のハードウェア（集積回路）のみによって実現してもよい。Note that the drawbar attitude calculation unit 151A, blade attitude calculation unit 152A, first cutting edge position calculation unit 153A, and second cutting edge position calculation unit 154 are functional block diagrams that are typically implemented by a processor (not shown) executing a program. Note that this is not a limitation, and the drawbar attitude calculation unit 151A, blade attitude calculation unit 152A, first cutting edge position calculation unit 153A, and second cutting edge position calculation unit 154 may also be implemented solely by hardware (integrated circuits) such as an ASIC.
ドローバ姿勢算出部１５１Ａは、カメラ８００Ａによる撮像によって得られた画像データを、カメラ８００Ａから周期的に取得する。ドローバ姿勢算出部１５１Ａは、取得した画像データに基づいて、ドローバ４０の姿勢を周期的に算出する。ドローバ姿勢算出部１５１Ａは、５つのマーカ７１１～７１３，７２１，７２２のうちドローバ４０に取り付けられた３つのマーカ７１１～７１３の画像データを用いて、カメラ８００Ａを基準としたときのドローバ４０の姿勢を算出する。The drawbar attitude calculation unit 151A periodically acquires image data obtained by imaging with the camera 800A from the camera 800A. The drawbar attitude calculation unit 151A periodically calculates the attitude of the drawbar 40 based on the acquired image data. The drawbar attitude calculation unit 151A calculates the attitude of the drawbar 40 when the camera 800A is used as the reference, using image data of three markers 711-713 attached to the drawbar 40 out of the five markers 711-713, 721, and 722.
ドローバ姿勢算出部１５１Ａは、算出したドローバ４０の姿勢（予測位置）を、第１刃先位置算出部１５３Ａに周期的に送る。なお、ドローバ姿勢算出部１５１Ａがカメラ８００Ａから画像データを取得する周期と、ドローバ４０の姿勢を算出する周期とは同じでなくてもよい。ドローバ姿勢算出部１５１Ａの処理の詳細については、後述する。The drawbar attitude calculation unit 151A periodically sends the calculated attitude (predicted position) of the drawbar 40 to the first cutting edge position calculation unit 153A. Note that the period in which the drawbar attitude calculation unit 151A acquires image data from the camera 800A does not have to be the same as the period in which the drawbar attitude calculation unit 151A calculates the attitude of the drawbar 40. Details of the processing by the drawbar attitude calculation unit 151A will be described later.
ブレード姿勢算出部１５２Ａは、カメラ８００Ａによる撮像によって得られた画像データを、カメラ８００Ａから周期的に取得する。ブレード姿勢算出部１５２Ａは、取得した画像データに基づいて、ブレード４２の姿勢を周期的に算出する。ブレード姿勢算出部１５２Ａは、５つのマーカ７１１～７１３，７２１，７２２のうちブレード４２に取り付けられた２つのマーカ７２１，７２２の画像データを用いて、カメラ８００Ａを基準としたときのブレード４２の姿勢を算出する。The blade attitude calculation unit 152A periodically acquires image data captured by the camera 800A from the camera 800A. The blade attitude calculation unit 152A periodically calculates the attitude of the blade 42 based on the acquired image data. The blade attitude calculation unit 152A calculates the attitude of the blade 42 when the camera 800A is used as the reference point, using image data of two markers 721 and 722 attached to the blade 42 out of the five markers 711 to 713, 721, and 722.
ブレード姿勢算出部１５２Ａは、算出したブレード４２の姿勢（予測位置）を、第１刃先位置算出部１５３Ａに周期的に送る。なお、ブレード姿勢算出部１５２Ａがカメラ８００Ａから画像データを取得する周期と、ブレード４２の姿勢を算出する周期とは同じでなくてもよい。ブレード姿勢算出部１５２Ａの処理の詳細については、後述する。The blade attitude calculation unit 152A periodically sends the calculated attitude (predicted position) of the blade 42 to the first cutting edge position calculation unit 153A. Note that the period in which the blade attitude calculation unit 152A acquires image data from the camera 800A does not have to be the same as the period in which the blade attitude calculation unit 152A calculates the attitude of the blade 42. Details of the processing by the blade attitude calculation unit 152A will be described later.
第１刃先位置算出部１５３Ａは、周期的に受け付けるドローバ４０の姿勢（予測位置）とブレード４２の姿勢（予測位置）とに基づき、カメラ８００Ａを基準としたときの刃先４２ａの位置を算出する。なお、刃先４２ａの位置の算出は、周期的に行われる。第１刃先位置算出部１５３Ａは、刃先４２ａの位置を算出すると、算出された刃先４２ａの位置を示す情報を、第２刃先位置算出部１５４に送る。The first cutting edge position calculation unit 153A calculates the position of the cutting edge 42a relative to the camera 800A based on the attitude (predicted position) of the drawbar 40 and the attitude (predicted position) of the blade 42, which are periodically received. The calculation of the position of the cutting edge 42a is performed periodically. Once the first cutting edge position calculation unit 153A calculates the position of the cutting edge 42a, it sends information indicating the calculated position of the cutting edge 42a to the second cutting edge position calculation unit 154.
第２刃先位置算出部１５４は、第１刃先位置算出部１５３Ａから受け取った、カメラ８００Ａを基準としたときの刃先４２ａの位置を示す情報に基づき、玉軸４０２を基準としたときの刃先４２ａの位置を算出する。第２刃先位置算出部１５４にて算出された刃先４２ａの位置の情報は、コントローラ１５０Ａにおいて、各種の処理に利用される。The second cutting edge position calculation unit 154 calculates the position of the cutting edge 42a relative to the ball axis 402, based on information received from the first cutting edge position calculation unit 153A indicating the position of the cutting edge 42a relative to the camera 800A. The information on the position of the cutting edge 42a calculated by the second cutting edge position calculation unit 154 is used for various processes in the controller 150A.
図１３は、ブレード４２の刃先４２ａの位置を算出するためのフロー図である。図１３に示されるように、ステップＳ１０において、コントローラ１５０Ａは、ドローバ４０に取り付けられたマーカ７１１～７１３の画像に基づいて、カメラ８００Ａに対するドローバ４０の姿勢を算出する。コントローラ１５０Ａのドローバ姿勢算出部１５１Ａは、カメラ８００Ａの位置を基準としたときのドローバ４０の姿勢を算出する。Figure 13 is a flow diagram for calculating the position of the cutting edge 42a of the blade 42. As shown in Figure 13, in step S10, the controller 150A calculates the attitude of the drawbar 40 relative to the camera 800A based on images of the markers 711-713 attached to the drawbar 40. The drawbar attitude calculation unit 151A of the controller 150A calculates the attitude of the drawbar 40 when the position of the camera 800A is used as a reference.
ステップＳ２０において、コントローラ１５０Ａは、ブレード４２に取り付けられたマーカ７２１，７２２の画像に基づいて、カメラ８００Ａに対するブレード４２の姿勢を算出する。コントローラ１５０Ａのドローバ姿勢算出部１５１Ａは、カメラ８００Ａの位置を基準としたときのブレード４２の姿勢を算出する。In step S20, controller 150A calculates the attitude of blade 42 relative to camera 800A based on images of markers 721, 722 attached to blade 42. Drawbar attitude calculation unit 151A of controller 150A calculates the attitude of blade 42 when the position of camera 800A is used as a reference.
ステップＳ３０において、コントローラ１５０Ａは、ステップＳ１０で算出されたカメラ８００Ａに対するドローバ４０の姿勢と、ステップＳ２０で算出されたカメラ８００Ａに対するブレード４２の姿勢とに基づき、カメラ８００Ａに対する刃先４２ａの位置を算出する。第１刃先位置算出部１５３Ａは、カメラ８００Ａの位置を基準としたときの刃先４２ａの位置を算出する。In step S30, controller 150A calculates the position of cutting edge 42a relative to camera 800A based on the attitude of drawbar 40 relative to camera 800A calculated in step S10 and the attitude of blade 42 relative to camera 800A calculated in step S20. First cutting edge position calculation unit 153A calculates the position of cutting edge 42a relative to camera 800A when the position of camera 800A is used as a reference.
ステップＳ４０において、コントローラ１５０Ａは、ステップＳ３０で算出された刃先４２ａの位置に基づいて、玉軸４０２に対する刃先４２ａの位置を算出する。第２刃先位置算出部１５４は、玉軸４０２の位置を基準としたときの刃先４２ａの位置を算出する。In step S40, the controller 150A calculates the position of the cutting edge 42a relative to the ball axis 402 based on the position of the cutting edge 42a calculated in step S30. The second cutting edge position calculation unit 154 calculates the position of the cutting edge 42a when the position of the ball axis 402 is used as a reference.
図１４は、図１３のステップＳ１０の処理の詳細を示したフロー図である。図１４に示されるように、ステップＳ１１において、ドローバ姿勢算出部１５１Ａは、画像データに基づいて、ドローバ４０に取り付けた各マーカ７１１～７１３の３次元直交座標と、各マーカ７１１～７１３のオイラー角とを算出する。詳しくは、ドローバ姿勢算出部１５１Ａは、作業機４が中立位置にあるときの各マーカ７１１～７１３の３次元直交座標（固定値）と、各マーカ７１１～７１３のオイラー角（固定値）とに基づいて、画像データから各マーカ７１１～７１３の３次元直交座標とオイラー角とを算出する。なお、上記の各固定値は、コントローラ１５０Ａに予め格納されている。後述する各固定値についても、コントローラ１５０Ａに予め格納されている。FIG. 14 is a flow diagram showing details of the processing of step S10 in FIG. 13. As shown in FIG. 14, in step S11, the drawbar attitude calculation unit 151A calculates the three-dimensional Cartesian coordinates and Euler angles of each of the markers 711-713 attached to the drawbar 40 based on image data. In particular, the drawbar attitude calculation unit 151A calculates the three-dimensional Cartesian coordinates and Euler angles of each of the markers 711-713 from the image data based on the three-dimensional Cartesian coordinates (fixed values) of each of the markers 711-713 when the work implement 4 is in the neutral position and the Euler angles (fixed values) of each of the markers 711-713. Note that each of the above fixed values is pre-stored in the controller 150A. Each of the fixed values described below is also pre-stored in the controller 150A.
オイラー角として、たとえば、ＺＹＸオイラー角を用いることができる。ＺＹＸオイラー角は、ｘ軸回りのロール角φと、ｙ軸回りのピッチ角θと、ｚ軸回りのヨー角ψとで規定される。As Euler angles, for example, ZYX Euler angles can be used. ZYX Euler angles are defined by the roll angle φ around the x-axis, the pitch angle θ around the y-axis, and the yaw angle ψ around the z-axis.
ステップＳ１２において、ドローバ姿勢算出部１５１Ａは、ステップＳ１１で算出されたマーカ７１１の３次元直交座標とオイラー角とに基づき、カメラ８００Ａの位置を基準としたときの旋回サークル４１の旋回中心Ｃの３次元直交座標と、旋回サークル４１のオイラー角とを算出する。詳しくは、ドローバ姿勢算出部１５１Ａは、作業機４が中立位置にあるときの、マーカ７１１の３次元直交座標（固定値）と、マーカ７１１のオイラー角（固定値）と、旋回中心Ｃの３次元直交座標（固定値）との各情報を用いて、当該３次元直交座標と当該オイラー角とを算出する。In step S12, the drawbar attitude calculation unit 151A calculates the three-dimensional Cartesian coordinates of the turning center C of the turning circle 41 when the position of the camera 800A is used as the reference, and the Euler angles of the turning circle 41, based on the three-dimensional Cartesian coordinates and Euler angles of the marker 711 calculated in step S11. In more detail, the drawbar attitude calculation unit 151A calculates the three-dimensional Cartesian coordinates and the Euler angles using information on the three-dimensional Cartesian coordinates (fixed values) of the marker 711, the Euler angles (fixed values) of the marker 711, and the three-dimensional Cartesian coordinates (fixed values) of the turning center C when the work implement 4 is in the neutral position.
より詳しくは、ドローバ姿勢算出部１５１Ａは、作業機４が中立位置にあるときのマーカ７１１の３次元直交座標（固定値）と旋回中心Ｃの３次元直交座標（固定値）との差（オフセット）の情報を用いて、カメラ８００Ａの位置を基準としたときの旋回サークル４１の旋回中心Ｃの３次元直交座標を算出する。この点は、後述するステップＳ１３，Ｓ１４におおける処理でも同様である。More specifically, the drawbar attitude calculation unit 151A calculates the three-dimensional Cartesian coordinates of the turning center C of the turning circle 41 when the position of the camera 800A is used as the reference, using information about the difference (offset) between the three-dimensional Cartesian coordinates (fixed values) of the marker 711 when the work implement 4 is in the neutral position and the three-dimensional Cartesian coordinates (fixed values) of the turning center C. This also applies to the processing in steps S13 and S14 described below.
同様に、ステップＳ１３において、ドローバ姿勢算出部１５１Ａは、ステップＳ１１で算出されたマーカ７１２の３次元直交座標とオイラー角とに基づき、カメラ８００Ａの位置を基準としたときの旋回サークル４１の旋回中心Ｃの３次元直交座標と、旋回サークル４１のオイラー角とを算出する。詳しくは、ドローバ姿勢算出部１５１Ａは、作業機４が中立位置にあるときの、マーカ７１２の３次元直交座標（固定値）と、マーカ７１２のオイラー角（固定値）と、旋回中心Ｃの３次元直交座標（固定値）との各情報を用いて、当該３次元直交座標と当該オイラー角とを算出する。Similarly, in step S13, the drawbar attitude calculation unit 151A calculates the three-dimensional Cartesian coordinates of the turning center C of the turning circle 41 when the position of the camera 800A is used as the reference, and the Euler angles of the turning circle 41, based on the three-dimensional Cartesian coordinates and Euler angles of the marker 712 calculated in step S11. In more detail, the drawbar attitude calculation unit 151A calculates the three-dimensional Cartesian coordinates and the Euler angles using information on the three-dimensional Cartesian coordinates (fixed values) of the marker 712, the Euler angles (fixed values) of the marker 712, and the three-dimensional Cartesian coordinates (fixed values) of the turning center C when the work implement 4 is in the neutral position.
ステップＳ１４において、ドローバ姿勢算出部１５１Ａは、ステップＳ１１で算出されたマーカ７１３の３次元直交座標とオイラー角とに基づき、カメラ８００Ａの位置を基準としたときの旋回サークル４１の旋回中心Ｃの３次元直交座標と、旋回サークル４１のオイラー角とを算出する。詳しくは、ドローバ姿勢算出部１５１Ａは、作業機４が中立位置にあるときの、マーカ７１３の３次元直交座標（固定値）と、マーカ７１３のオイラー角（固定値）と、旋回中心Ｃの３次元直交座標（固定値）との各情報を用いて、当該３次元直交座標と当該オイラー角とを算出する。なお、ステップＳ１２，Ｓ１３，Ｓ１４の順序は、特に限定されない。In step S14, the drawbar attitude calculation unit 151A calculates the three-dimensional Cartesian coordinates of the turning center C of the turning circle 41 when the position of the camera 800A is used as a reference, and the Euler angles of the turning circle 41, based on the three-dimensional Cartesian coordinates and Euler angles of the marker 713 calculated in step S11. In more detail, the drawbar attitude calculation unit 151A calculates the three-dimensional Cartesian coordinates and the Euler angles using information on the three-dimensional Cartesian coordinates (fixed values) of the marker 713, the Euler angles (fixed values) of the marker 713, and the three-dimensional Cartesian coordinates (fixed values) of the turning center C when the work implement 4 is in the neutral position. The order of steps S12, S13, and S14 is not particularly limited.
ステップＳ１５において、ドローバ姿勢算出部１５１Ａは、ステップＳ１２～Ｓ１４で算出された各３次元直交座標と各オイラー角とに異常値が含まれているか否かを判断する。具体的には、ドローバ姿勢算出部１５１Ａは、予め設定された座標範囲内に、各３次元直交座標が収まっているか否かを判断する。ドローバ姿勢算出部１５１Ａは、予め設定されたオイラー角範囲内に、各オイラー角が収まっているか否かを判断する。詳しくは、ドローバ姿勢算出部１５１Ａは、上述した３つの独立変数ごとに、予め設定された範囲内に収まっているかを判断する。ドローバ姿勢算出部１５１Ａは、予め設定された範囲に収まっていない３次元座標およびオイラー角を異常値と判断する。In step S15, the drawbar attitude calculation unit 151A determines whether each of the three-dimensional orthogonal coordinates and each of the Euler angles calculated in steps S12 to S14 contains an abnormal value. Specifically, the drawbar attitude calculation unit 151A determines whether each of the three-dimensional orthogonal coordinates falls within a preset coordinate range. The drawbar attitude calculation unit 151A determines whether each of the Euler angles falls within a preset Euler angle range. More specifically, the drawbar attitude calculation unit 151A determines whether each of the three independent variables described above falls within a preset range. The drawbar attitude calculation unit 151A determines that three-dimensional coordinates and Euler angles that do not fall within the preset ranges are abnormal values.
異常値が含まれていないと判断された場合（ステップＳ１５においてＮＯ）、ドローバ姿勢算出部１５１Ａは、処理をステップＳ１６に進める。異常値が含まれていると判断された場合（ステップＳ１５においてＹＥＳ）、ドローバ姿勢算出部１５１Ａは、ステップＳ１７において、精度を高めるため、異常値を削除する。ドローバ姿勢算出部１５１Ａは、その後、処理をステップＳ１６に進める。If it is determined that no abnormal values are included (NO in step S15), the drawbar attitude calculation unit 151A proceeds to step S16. If it is determined that an abnormal value is included (YES in step S15), the drawbar attitude calculation unit 151A deletes the abnormal value in step S17 to improve accuracy. The drawbar attitude calculation unit 151A then proceeds to step S16.
ステップＳ１６において、ドローバ姿勢算出部１５１Ａは、ステップＳ１２～Ｓ１４で算出された各３次元直交座標（異常値除く）および各オイラー角（異常値除く）に基づき、カメラ８００Ａの位置を基準としたときの旋回サークル４１の旋回中心Ｃの３次元直交座標と旋回サークル４１のオイラー角とを決定する。このように、ドローバ姿勢算出部１５１Ａは、カメラ８００Ａに対するドローバ４０の姿勢を決定する。In step S16, the drawbar attitude calculation unit 151A determines the three-dimensional Cartesian coordinates of the turning center C of the turning circle 41 and the Euler angles of the turning circle 41 when the position of the camera 800A is used as the reference, based on the three-dimensional Cartesian coordinates (excluding abnormal values) and Euler angles (excluding abnormal values) calculated in steps S12 to S14. In this way, the drawbar attitude calculation unit 151A determines the attitude of the drawbar 40 relative to the camera 800A.
たとえば、ドローバ姿勢算出部１５１Ａは、各３次元直交座標（異常値除く）の平均を算出することにより、カメラ８００Ａを基準としたときの旋回サークル４１の旋回中心Ｃの３次元直交座標を決定する。ドローバ姿勢算出部１５１Ａは、各オイラー角（異常値除く）の平均を算出することにより、カメラ８００Ａを基準としたときの旋回サークル４１のオイラー角を決定する。For example, the drawbar attitude calculation unit 151A calculates the average of each three-dimensional Cartesian coordinate (excluding abnormal values) to determine the three-dimensional Cartesian coordinate of the turning center C of the turning circle 41 when the camera 800A is used as the reference. The drawbar attitude calculation unit 151A calculates the average of each Euler angle (excluding abnormal values) to determine the Euler angles of the turning circle 41 when the camera 800A is used as the reference.
図１５は、図１３のステップＳ２０の処理の詳細を示したフロー図である。図１５に示されるように、ステップＳ２１において、ブレード姿勢算出部１５２Ａは、画像データに基づいて、ブレード４２に取り付けられた各マーカ７２１，７２２の３次元直交座標と、各マーカ７２１，７２２のオイラー角とを算出する。Figure 15 is a flow diagram showing the details of the processing of step S20 in Figure 13. As shown in Figure 15, in step S21, the blade attitude calculation unit 152A calculates the three-dimensional Cartesian coordinates of each marker 721, 722 attached to the blade 42 and the Euler angles of each marker 721, 722 based on image data.
ステップＳ２２において、ブレード姿勢算出部１５２Ａは、ステップＳ２１で算出されたマーカ７２１の３次元直交座標とオイラー角とに基づき、カメラ８００Ａの位置を基準としたときのブレード４２の位置とブレード４２のオイラー角とを算出する。詳しくは、ブレード姿勢算出部１５２Ａは、作業機４が中立位置にあるときの、マーカ７２１の３次元直交座標（固定値）とマーカ７２１のオイラー角（固定値）との情報を用いて、３次元直交座標とオイラー角とを算出する。In step S22, the blade attitude calculation unit 152A calculates the position of the blade 42 and the Euler angles of the blade 42 when the position of the camera 800A is used as the reference, based on the three-dimensional Cartesian coordinates and Euler angles of the marker 721 calculated in step S21. In more detail, the blade attitude calculation unit 152A calculates the three-dimensional Cartesian coordinates and Euler angles using information on the three-dimensional Cartesian coordinates (fixed values) of the marker 721 and the Euler angles (fixed values) of the marker 721 when the work implement 4 is in the neutral position.
ステップＳ２３において、ブレード姿勢算出部１５２Ａは、ステップＳ２１で算出されたマーカ７２２の３次元直交座標とオイラー角とに基づき、カメラ８００Ａの位置を基準としたときのブレード４２の位置とブレード４２のオイラー角とを算出する。詳しくは、ブレード姿勢算出部１５２Ａは、作業機４が中立位置にあるときの、マーカ７２２の３次元直交座標（固定値）とマーカ７２２のオイラー角（固定値）との情報を用いて、３次元直交座標とオイラー角とを算出する。なお、ステップＳ２２，Ｓ２３の順序は、特に限定されない。In step S23, the blade attitude calculation unit 152A calculates the position of the blade 42 and the Euler angles of the blade 42 relative to the position of the camera 800A, based on the three-dimensional Cartesian coordinates and Euler angles of the marker 722 calculated in step S21. In more detail, the blade attitude calculation unit 152A calculates the three-dimensional Cartesian coordinates and Euler angles using information on the three-dimensional Cartesian coordinates (fixed values) of the marker 722 and the Euler angles (fixed values) of the marker 722 when the work implement 4 is in the neutral position. Note that the order of steps S22 and S23 is not particularly limited.
ステップＳ２４において、ブレード姿勢算出部１５２Ａは、ステップＳ２２～Ｓ２３で算出された各３次元直交座標と各オイラー角とに異常値が含まれているか否かを判断する。具体的には、ブレード姿勢算出部１５２Ａは、予め設定された座標範囲内に、各３次元直交座標が収まっているか否かを判断する。ブレード姿勢算出部１５２Ａは、予め設定されたオイラー角範囲内に、各オイラー角が収まっているか否かを判断する。詳しくは、ブレード姿勢算出部１５２Ａは、上述した３つの独立変数ごとに、予め設定された範囲内に収まっているかを判断する。ブレード姿勢算出部１５２Ａは、予め設定された範囲に収まっていない３次元座標およびオイラー角を異常値と判断する。In step S24, the blade attitude calculation unit 152A determines whether each of the three-dimensional Cartesian coordinates and each of the Euler angles calculated in steps S22 and S23 contains an abnormal value. Specifically, the blade attitude calculation unit 152A determines whether each of the three-dimensional Cartesian coordinates falls within a preset coordinate range. The blade attitude calculation unit 152A determines whether each of the Euler angles falls within a preset Euler angle range. More specifically, the blade attitude calculation unit 152A determines whether each of the three independent variables described above falls within a preset range. The blade attitude calculation unit 152A determines that three-dimensional coordinates and Euler angles that do not fall within the preset range are abnormal values.
異常値が含まれていないと判断された場合（ステップＳ２４においてＮＯ）、ブレード姿勢算出部１５２Ａは、処理をステップＳ２５に進める。異常値が含まれていると判断された場合（ステップＳ２４においてＹＥＳ）、ブレード姿勢算出部１５２Ａは、ステップＳ２６において、精度を高めるため、異常値を削除する。ブレード姿勢算出部１５２Ａは、その後、処理をステップＳ２５に進める。If it is determined that no abnormal values are included (NO in step S24), the blade attitude calculation unit 152A proceeds to step S25. If it is determined that abnormal values are included (YES in step S24), the blade attitude calculation unit 152A deletes the abnormal values in step S26 to improve accuracy. The blade attitude calculation unit 152A then proceeds to step S25.
ステップＳ２５において、ブレード姿勢算出部１５２Ａは、ステップＳ２２，Ｓ２３で算出された各３次元直交座標（異常値除く）および各オイラー角（異常値除く）に基づき、カメラ８００Ａの位置を基準としたときのブレード４２の３次元直交座標とブレード４２のオイラー角とを決定する。このように、ブレード姿勢算出部１５２Ａは、カメラ８００Ａに対するブレード４２の姿勢を決定する。In step S25, the blade attitude calculation unit 152A determines the three-dimensional Cartesian coordinates and Euler angles of the blade 42 when the position of the camera 800A is used as the reference, based on the three-dimensional Cartesian coordinates (excluding abnormal values) and Euler angles (excluding abnormal values) calculated in steps S22 and S23. In this way, the blade attitude calculation unit 152A determines the attitude of the blade 42 relative to the camera 800A.
たとえば、ブレード姿勢算出部１５２Ａは、各３次元直交座標（異常値除く）の平均を算出することにより、カメラ８００Ａを基準としたときのブレード４２の３次元直交の座標を決定する。ブレード姿勢算出部１５２Ａは、各オイラー角（異常値除く）の平均を算出することにより、カメラ８００Ａを基準としたときのブレード４２のオイラー角を決定する。For example, the blade attitude calculation unit 152A calculates the average of each three-dimensional Cartesian coordinate (excluding outliers) to determine the three-dimensional Cartesian coordinates of the blade 42 when the camera 800A is used as the reference. The blade attitude calculation unit 152A calculates the average of each Euler angle (excluding outliers) to determine the Euler angles of the blade 42 when the camera 800A is used as the reference.
以上のように、モータグレーダ１Ａは、図１０および図１１に示されるように、ドローバ４０に取り付けられたマーカ７１１～７１３と、フロントフレーム２２に取り付けられ、かつ、少なくともマーカ７１１～７１３を撮像するカメラとを備える。モータグレーダ１Ａは、図１２に示されるように、カメラ８００Ａによる撮像により得られたマーカ７１１～７１３の各画像データに基づいて、カメラ８００Ａに対するドローバ４０の姿勢を算出するコントローラをさらに備える。As described above, the motor grader 1A, as shown in Figures 10 and 11, is equipped with markers 711-713 attached to the drawbar 40, and a camera attached to the front frame 22 that captures images of at least the markers 711-713. As shown in Figure 12, the motor grader 1A further includes a controller that calculates the attitude of the drawbar 40 relative to the camera 800A based on the image data of the markers 711-713 captured by the camera 800A.
このような構成によれば、ドローバ４０に取り付けられたマーカ７１１～７１３をフロントフレーム２２に取り付けられたカメラ８００Ａで撮像することにより、カメラ８００Ａに対するドローバ４０の姿勢の情報を得ることができる。With this configuration, by capturing images of the markers 711-713 attached to the drawbar 40 with the camera 800A attached to the front frame 22, information on the attitude of the drawbar 40 relative to the camera 800A can be obtained.
モータグレーダ１Ａは、図１０および図１１に示されるように、ブレード４２に取り付けられたマーカ７２１，７２２をさらに備える。カメラ８００Ａは、マーカ７１１～７１３とマーカ７２１，７２２とを撮像する。コントローラ１５０Ａは、図１３のステップＳ２０に示したように、マーカ７２１，７２１の各画像データに基づいて、カメラ８００Ａに対するブレード４２の姿勢を算出する。As shown in Figures 10 and 11, the motor grader 1A further includes markers 721 and 722 attached to the blade 42. The camera 800A captures images of the markers 711-713 and the markers 721 and 722. As shown in step S20 of Figure 13, the controller 150A calculates the attitude of the blade 42 relative to the camera 800A based on the image data of the markers 721 and 721.
このような構成によれば、ブレード４２に取り付けられたマーカ７２１，７２２をフロントフレーム２２に取り付けられたカメラ８００Ａで撮像することにより、カメラ８００Ａに対するブレード４２の姿勢の情報を得ることができる。With this configuration, the markers 721, 722 attached to the blade 42 are captured by the camera 800A attached to the front frame 22, thereby obtaining information on the attitude of the blade 42 relative to the camera 800A.
特に、モータグレーダ１Ａによれば、１つのカメラ８００Ａで複数のマーカ７１１～７１３，７２１，７２２を撮像可能である。それゆえ、モータグレーダ１Ａには、複数のカメラは不要である。In particular, with the motor grader 1A, a single camera 800A can capture images of multiple markers 711-713, 721, and 722. Therefore, the motor grader 1A does not require multiple cameras.
コントローラ１５０Ａは、カメラ８００Ａに対するドローバ４０の姿勢と、カメラ８００Ａに対するブレード４２の姿勢とに基づき、カメラ８００Ａに対する刃先４２ａの位置を算出する。このような構成によれば、ドローバ４０に取り付けられたマーカ７１１～７１３と、ブレード４２に取り付けられたマーカ７２１，７２２とを、フロントフレーム２２に取り付けられたカメラ８００Ａで撮像することにより、カメラ８００Ａに対する刃先４２ａの位置の情報を得ることができる。The controller 150A calculates the position of the cutting edge 42a relative to the camera 800A based on the attitude of the drawbar 40 relative to the camera 800A and the attitude of the blade 42 relative to the camera 800A. With this configuration, by capturing images of the markers 711-713 attached to the drawbar 40 and the markers 721, 722 attached to the blade 42 with the camera 800A attached to the front frame 22, information on the position of the cutting edge 42a relative to the camera 800A can be obtained.
コントローラ１５０Ａは、カメラ８００Ａに対する刃先４２ａの位置に基づき、連結部材としての玉軸４０２に対する刃先４２ａの位置を算出する。このような構成によれば、ドローバ４０に取り付けられたマーカ７１１～７１３と、ブレード４２に取り付けられたマーカ７２１，７２２とを、フロントフレーム２２に取り付けられたカメラ８００Ａで撮像することにより、玉軸４０２に対する刃先４２ａの位置の情報を得ることができる。The controller 150A calculates the position of the cutting edge 42a relative to the ball axis 402, which serves as the connecting member, based on the position of the cutting edge 42a relative to the camera 800A. With this configuration, information on the position of the cutting edge 42a relative to the ball axis 402 can be obtained by capturing images of the markers 711-713 attached to the drawbar 40 and the markers 721, 722 attached to the blade 42 with the camera 800A attached to the front frame 22.
カメラ８００Ａは、フロントフレーム２２の下端面２２ｕ（図１０）に取り付けられている。このような構成によれば、ドローバ４０に取り付けられたマーカ７１１～７１３と、ブレード４２に取り付けられたマーカ７２１，７２２とを、カメラ８００Ａで撮像可能となる。さらに、カメラ８００Ａに埃が溜まることを、フロントフレーム２２によって抑制できる。Camera 800A is attached to the lower end surface 22u (Figure 10) of the front frame 22. With this configuration, camera 800A can capture images of markers 711-713 attached to the drawbar 40 and markers 721, 722 attached to the blade 42. Furthermore, the front frame 22 can prevent dust from accumulating on camera 800A.
カメラ８００Ａのレンズ８０１Ａは、超広角レンズである。このような構成によれば、ドローバ４０に取り付けられたマーカ７１１～７１３と、ブレード４２に取り付けられたマーカ７２１，７２２とを、視野角内に収めることができる。Lens 801A of camera 800A is an ultra-wide-angle lens. With this configuration, markers 711-713 attached to drawbar 40 and markers 721 and 722 attached to blade 42 can be accommodated within the viewing angle.
マーカ７１１～７１３は、ドローバ４０の上面に設置されている。このような構成によれば、マーカ７１１～７１３をカメラ８００Ａで撮像する際に、ドローバ４０がマーカ７１１～７１３に被ることを防止できる。Markers 711-713 are installed on the top surface of drawbar 40. This configuration prevents markers 711-713 from obscuring drawbar 40 when capturing images of markers 711-713 with camera 800A.
マーカ７２１，７２２は、ブレード４２の上方に設置されている。このような構成によれば、マーカ７２１，７２２がブレード４２の上方以外に取り付けられた構成に比べて、ブレード４２によって削られた土砂等がマーカ７２１，７２２に付着することを防止できる。Markers 721 and 722 are installed above blade 42. This configuration prevents soil and sand scraped by blade 42 from adhering to markers 721 and 722, compared to a configuration in which markers 721 and 722 are installed somewhere other than above blade 42.
＜変形例＞
（第１の変形例）
上記においては、図１３のステップＳ２０に示したように、ブレード４２に取り付けられた２つのマーカ７２１，７２２を用いて、カメラ８００Ａに対するブレード４２の姿勢を算出した。以下では、モータグレーダ１Ａに備えられた複数のセンサを用いて、旋回サークル４１の旋回中心Ｃに対するブレード４２の姿勢を算出する手法について説明する。本例では、マーカ７２１，７２２が不要となる。<Modification>
(First Modification)
In the above, as shown in step S20 of Fig. 13, the attitude of the blade 42 relative to the camera 800A is calculated using two markers 721, 722 attached to the blade 42. Below, a method for calculating the attitude of the blade 42 relative to the turning center C of the turning circle 41 using multiple sensors provided on the motor grader 1A will be described. In this example, the markers 721, 722 are not necessary.
図１６は、ブレード４２の刃先４２ａの位置を算出するための本変形例でのフロー図である。図１６に示されるように、ステップＳ１０において、コントローラ１５０Ａは、３つのマーカ７１１～７１３の画像に基づいて、カメラ８００Ａに対するドローバ４０の姿勢を算出する。Figure 16 is a flow diagram of this modified example for calculating the position of the cutting edge 42a of the blade 42. As shown in Figure 16, in step S10, the controller 150A calculates the attitude of the drawbar 40 relative to the camera 800A based on the images of the three markers 711-713.
ステップＳ２０Ａにおいて、コントローラ１５０Ａは、モータグレーダ１Ａに備えられた複数のセンサ（図示せず）からの出力に基づいて、旋回サークル４１の旋回中心Ｃに対するブレード４２の姿勢を算出する。具体的には、コントローラ１５０Ａは、旋回サークル４１の回転角度を検出するセンサ（典型的には、ポテンショメータ）と、ブレードシフトシリンダ４７のストローク量を検出するセンサと、チルトシリンダ４８のストローク量を検出するセンサとの出力に基づいて、旋回サークル４１の旋回中心Ｃに対するブレード４２の姿勢を算出する。In step S20A, the controller 150A calculates the attitude of the blade 42 relative to the center of rotation C of the turning circle 41 based on the outputs from multiple sensors (not shown) provided on the motor grader 1A. Specifically, the controller 150A calculates the attitude of the blade 42 relative to the center of rotation C of the turning circle 41 based on the outputs of a sensor (typically a potentiometer) that detects the rotation angle of the turning circle 41, a sensor that detects the stroke amount of the blade shift cylinder 47, and a sensor that detects the stroke amount of the tilt cylinder 48.
ステップＳ３０Ａにおいて、コントローラ１５０Ａは、ステップＳ１０で算出されたカメラ８００Ａに対するドローバ４０の姿勢と、ステップＳ２０Ａで算出された旋回サークル４１の旋回中心Ｃに対するブレード４２の姿勢とに基づいて、カメラ８００Ａに対する刃先４２ａの位置を算出する。ステップＳ４０において、コントローラ１５０Ａは、ステップＳ３０Ａで算出された刃先４２ａの位置に基づいて、玉軸４０２に対する刃先４２ａの位置を算出する。In step S30A, controller 150A calculates the position of cutting edge 42a relative to camera 800A based on the attitude of drawbar 40 relative to camera 800A calculated in step S10 and the attitude of blade 42 relative to center of rotation C of turning circle 41 calculated in step S20A. In step S40, controller 150A calculates the position of cutting edge 42a relative to ball axis 402 based on the position of cutting edge 42a calculated in step S30A.
図１７は、図１６のステップＳ２０Ａの処理の詳細を示したフロー図である。図１７に示されるように、ステップＳ２８において、コントローラ１５０Ａは、旋回サークル４１の回転角を計測するセンサの出力に基づき、旋回中心Ｃ回りのブレード４２の回転角度と、ブレード４２のオイラー角とを算出する。Figure 17 is a flow diagram showing the details of the processing of step S20A in Figure 16. As shown in Figure 17, in step S28, the controller 150A calculates the rotation angle of the blade 42 around the center of rotation C and the Euler angles of the blade 42 based on the output of the sensor that measures the rotation angle of the turning circle 41.
ステップＳ２９において、旋回中心Ｃ回りのブレード４２の回転角度およびブレード４２のオイラー角と、ブレードシフトシリンダ４７のストローク量を検出するセンサの出力と、ブレードシフトシリンダ４７のストローク量を検出するセンサの出力とに基づき、旋回サークル４１の旋回中心Ｃを基準としたときのブレード４２の３次元直交座標とブレード４２のオイラー角とを決定する。このように、コントローラ１５０Ａは、旋回サークル４１の旋回中心Ｃに対するブレード４２の姿勢を算出する。In step S29, the three-dimensional Cartesian coordinates of the blade 42 and the Euler angles of the blade 42 are determined based on the rotation angle of the blade 42 around the center of rotation C, the output of the sensor that detects the stroke amount of the blade shift cylinder 47, and the output of the sensor that detects the stroke amount of the blade shift cylinder 47. In this way, the controller 150A calculates the attitude of the blade 42 relative to the center of rotation C of the rotation circle 41.
このような構成であっても、玉軸４０２に対する刃先４２ａの位置の情報を得ることができる。Even with this configuration, information on the position of the cutting edge 42a relative to the ball shaft 402 can be obtained.
（第２の変形例）
図１３のステップＳ１０において、３つのマーカ７１１～７１３の画像に基づいて、カメラ８００Ａに対するドローバ４０の姿勢（３次元直交座標およびオイラー角）を算出した。以下では、カメラ８００Ａに対するドローバ４０の姿勢を補正するための処理について説明する。なお、本例における「補正」とは、上記のように３つのマーカ７１１～７１３の画像に基づいて算出されたドローバ４０の姿勢を、精度を高める観点から、異なる手法にて算出されたドローバ４０の姿勢を用いて調整することを意味する。以下、当該異なる手法について説明する。(Second Modification)
In step S10 of Fig. 13, the attitude (three-dimensional Cartesian coordinates and Euler angles) of the drawbar 40 with respect to the camera 800A was calculated based on the images of the three markers 711 to 713. Below, a process for correcting the attitude of the drawbar 40 with respect to the camera 800A will be described. Note that "correction" in this example means adjusting the attitude of the drawbar 40 calculated based on the images of the three markers 711 to 713 as described above, using the attitude of the drawbar 40 calculated by a different method, from the viewpoint of improving accuracy. Below, the different method will be described.
コントローラ１５０Ａは、マーカ７１１～７１３に基づいて算出されたカメラ８００Ａに対するドローバ４０の姿勢（以下、「第１の姿勢」と称する）と、マーカ７２１，７２２に基づいて算出されたカメラ８００Ａに対するブレード４２の姿勢とに基づき、ブレード４２のシフト量とブレード４２のチルト量とを算出する。コントローラ１５０Ａは、算出されたカメラ８００Ａに対するブレード４２の姿勢と、算出されたブレード４２のシフト量と、算出されたブレード４２のチルト量とに基づき、カメラ８００Ａに対するドローバ４０姿勢（以下、「第２の姿勢」と称する）を算出する。Controller 150A calculates the shift amount and tilt amount of blade 42 based on the attitude of drawbar 40 relative to camera 800A calculated based on markers 711-713 (hereinafter referred to as the "first attitude") and the attitude of blade 42 relative to camera 800A calculated based on markers 721 and 722. Controller 150A calculates the attitude of drawbar 40 relative to camera 800A (hereinafter referred to as the "second attitude") based on the calculated attitude of blade 42 relative to camera 800A, the calculated shift amount of blade 42, and the calculated tilt amount of blade 42.
コントローラ１５０Ａは、第１の姿勢を第２の姿勢を用いて補正する。コントローラ１５０Ａは、たとえば第１の姿勢と第２の姿勢との平均を、カメラ８００Ａに対するドローバ４０の姿勢に決定する。補正の方法は特に限定されない。コントローラ１５０Ａは、図１０のステップＳ３０における「カメラ８００Ａに対するドローバ４０の姿勢」（すなわち、第１の姿勢）の代わりに、補正後のドローバ４０の姿勢を用いて、カメラ８００Ａに対する刃先４２ａの位置を算出する。The controller 150A corrects the first attitude using the second attitude. For example, the controller 150A determines the average of the first attitude and the second attitude as the attitude of the drawbar 40 relative to the camera 800A. The correction method is not particularly limited. The controller 150A calculates the position of the cutting edge 42a relative to the camera 800A using the corrected attitude of the drawbar 40 instead of the "attitude of the drawbar 40 relative to the camera 800A" (i.e., the first attitude) in step S30 of FIG. 10.
このような構成によれば、ドローバ４０の姿勢を精度よく算出可能となる。それゆえ、カメラ８００Ａに対する刃先４２ａの位置、および玉軸４０２に対する刃先４２ａの位置を精度よく算出可能となる。This configuration makes it possible to accurately calculate the attitude of the drawbar 40. As a result, it is possible to accurately calculate the position of the cutting edge 42a relative to the camera 800A and the position of the cutting edge 42a relative to the ball axis 402.
（第３の変形例）
上記においては、ドローバ姿勢算出部１５１Ａは、作業機４が中立位置にあるときのマーカ７１１の３次元直交座標（固定値）と旋回中心Ｃの３次元直交座標（固定値）との差（オフセット）の情報を用いて、カメラ８００Ａの位置を基準としたときの旋回サークル４１の旋回中心Ｃの３次元直交座標を算出した。しかしながら、当該オフセットの情報を必ずしも用いる必要はない。(Third Modification)
In the above description, the drawbar attitude calculation unit 151A calculates the three-dimensional orthogonal coordinates of the turning center C of the turning circle 41 when the position of the camera 800A is used as a reference, using information about the difference (offset) between the three-dimensional orthogonal coordinates (fixed values) of the marker 711 when the work implement 4 is in the neutral position and the three-dimensional orthogonal coordinates (fixed values) of the turning center C. However, it is not necessarily necessary to use the offset information.
作業機４は、旋回サークル４１の中心にスイベルジョイント（図示せず）を有する。スイベルジョイントの上面にマーカ（図示せず）を取付け、当該マーカをカメラ８００Ａで撮像することにより、旋回サークル４１の旋回中心Ｃの３次元直交座標を算出してもよい。The work machine 4 has a swivel joint (not shown) at the center of the turning circle 41. A marker (not shown) may be attached to the upper surface of the swivel joint, and the three-dimensional Cartesian coordinates of the turning center C of the turning circle 41 may be calculated by capturing an image of the marker with the camera 800A.
スイベルジョイントに貼り付けマーカが破損または汚染により当該マーカの識別性が低下することもある。この場合の冗長性を確保するために、作業機４が中立状態にあるときの、スイベルジョイントに貼り付けマーカと各マーカ７１１～７１３との各３次元座標（固定値）を各マーカの識別性が十分にあるときの状態(車両が綺麗な状態)で保存しておくことにより、スイベルジョイントに貼り付けマーカが識別できない状態になっても、旋回サークル４１の旋回中心Ｃの３次元直交座標を算出することが可能となる。The markers attached to the swivel joints may become less identifiable due to damage or contamination. To ensure redundancy in this case, the three-dimensional coordinates (fixed values) of the markers attached to the swivel joints and each of the markers 711-713 are saved when the work implement 4 is in a neutral position and each marker is sufficiently identifiable (when the vehicle is clean). This makes it possible to calculate the three-dimensional Cartesian coordinates of the turning center C of the turning circle 41 even if the markers attached to the swivel joints become unidentifiable.
今回開示された実施の形態は例示であって、上記内容のみに制限されるものではない。本発明の範囲は請求の範囲によって示され、請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。The embodiments disclosed herein are illustrative and are not limited to the above. The scope of the present invention is defined by the claims, and it is intended to include all modifications that are equivalent to and within the scope of the claims.
１，１Ａ モータグレーダ、２ 車体フレーム、３ キャブ、４ 作業機、６ エンジン室、１１ 前輪、１２ 後輪、２１ リアフレーム、２２ フロントフレーム、２２Ｇ，４０Ｇ，５１Ｇ，１０２Ｇ，１０４Ｇ，１０６Ｇ，７０１Ｇ，７０２Ｇ，７０３Ｇ 画像、２２ｆ 前端部、２２ｒ 基端部、２２ｕ 下端面、２５ 外装カバー、４０ ドローバ、４０ｆ 前端部、４０ｒ 後端部、４１ 旋回サークル、４２ ブレード、４２ａ 刃先、４４，４５ リフトシリンダ、４６ ドローバシフトシリンダ、４７ ブレードシフトシリンダ、４８ チルトシリンダ、４９ 旋回モータ、５０ ブラケット、５１ カウンタウェイト、１５０，１５０Ａ コントローラ、１５１，１５１Ａ ドローバ姿勢算出部、１５２，１５２Ａ ブレード姿勢算出部、１５３ 刃先位置算出部、１５３Ａ 第１刃先位置算出部、１５４ 第２刃先位置算出部、４０２ 玉軸、７０１，７０２，７０３，７１１～７１３，７２１，７２２ マーカ、７９０ 支持部材、８００，８００Ａ カメラ、８０１，８０１Ａ レンズ、８０２，８０２Ａ 本体部、Ｃ 旋回中心、Ｊ１，Ｊ３ 中心軸、Ｊ２ 回転軸、Ｌ 仮想線。1, 1A Motor Grader, 2 Body Frame, 3 Cab, 4 Work Equipment, 6 Engine Compartment, 11 Front Wheel, 12 Rear Wheel, 21 Rear Frame, 22 Front Frame, 22G, 40G, 51G, 102G, 104G, 106G, 701G, 702G, 703G Image, 22f Front End, 22r Base End, 22u Lower End, 25 Exterior Cover, 40 Draw Bar, 40f Front End, 40r Rear End, 41 Turning Circle, 42 Blade, 42a Cutting Edge, 44, 45 Lift Cylinder, 46 Draw Bar Shift Cylinder, 47 Blade Shift Cylinder, 48 Tilt cylinder, 49: Swivel motor, 50: Bracket, 51: Counterweight, 150, 150A: Controller, 151, 151A: Drawbar attitude calculation unit, 152, 152A: Blade attitude calculation unit, 153: Cutting edge position calculation unit, 153A: First cutting edge position calculation unit, 154: Second cutting edge position calculation unit, 402: Ball axis, 701, 702, 703, 711-713, 721, 722: Markers, 790: Support member, 800, 800A: Camera, 801, 801A: Lens, 802, 802A: Main body, C: Swivel center, J1, J3: Central axis, J2: Rotation axis, L: Virtual line.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2024-032121 | 2024-03-04 | ||
| JP2024032121AJP2025134297A (en) | 2024-03-04 | 2024-03-04 | Motor grader and method for calculating drawbar attitude of motor grader |
| JP2024-032118 | 2024-03-04 | ||
| JP2024032118AJP2025134295A (en) | 2024-03-04 | 2024-03-04 | Motor grader and method for calculating drawbar attitude of motor grader |
| Publication Number | Publication Date |
|---|---|
| WO2025187259A1true WO2025187259A1 (en) | 2025-09-12 |
| WO2025187259A8 WO2025187259A8 (en) | 2025-10-02 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/JP2025/002632PendingWO2025187259A1 (en) | 2024-03-04 | 2025-01-28 | Motor grader and method for calculating drawbar attitude of motor grader |
| Country | Link |
|---|---|
| WO (1) | WO2025187259A1 (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20190032305A1 (en)* | 2017-07-31 | 2019-01-31 | Deere & Company | Work machines and methods and systems to control and determine a position of an associated implement |
| WO2021065135A1 (en)* | 2019-09-30 | 2021-04-08 | 株式会社小松製作所 | Control system, work vehicle control method, and work vehicle |
| WO2023135917A1 (en)* | 2022-01-13 | 2023-07-20 | 株式会社小松製作所 | Work machine, and method and system for controlling work machine |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20190032305A1 (en)* | 2017-07-31 | 2019-01-31 | Deere & Company | Work machines and methods and systems to control and determine a position of an associated implement |
| WO2021065135A1 (en)* | 2019-09-30 | 2021-04-08 | 株式会社小松製作所 | Control system, work vehicle control method, and work vehicle |
| WO2023135917A1 (en)* | 2022-01-13 | 2023-07-20 | 株式会社小松製作所 | Work machine, and method and system for controlling work machine |
| Publication number | Publication date |
|---|---|
| WO2025187259A8 (en) | 2025-10-02 |
| Publication | Publication Date | Title |
|---|---|---|
| JP6754594B2 (en) | Motor grader | |
| JP5546427B2 (en) | Work machine ambient monitoring device | |
| US10378162B2 (en) | Automotive construction machine and method for displaying the surroundings of an automotive construction machine | |
| US20130162830A1 (en) | SURROUNDING AREA MONITORING DEVICE FOR WORK VEHICLE (as amended) | |
| US20200299934A1 (en) | Method of controlling motor grader, motor grader, and work management system for motor grader | |
| US11713560B2 (en) | Hydraulic excavator and system | |
| US12209389B2 (en) | Work vehicle having a work implement and sensors for maintaining a view of an area of interest throughout movement of the work implement | |
| US20170120820A1 (en) | Device and Method for Providing a Vehicle Surround View | |
| US20240052602A1 (en) | Work machine surroundings monitoring system, work machine, and work machine surroundings monitoring method | |
| JP6850078B2 (en) | Motor grader | |
| WO2022124158A1 (en) | Work vehicle | |
| US20240337089A1 (en) | Work machine and method for controlling work machine | |
| JP6782895B2 (en) | In-vehicle camera mounting direction parameter calculation device and mounting direction parameter calculation method | |
| WO2025187259A1 (en) | Motor grader and method for calculating drawbar attitude of motor grader | |
| JP7478616B2 (en) | Work Machine | |
| JP2025134297A (en) | Motor grader and method for calculating drawbar attitude of motor grader | |
| JP2025134295A (en) | Motor grader and method for calculating drawbar attitude of motor grader | |
| WO2021090678A1 (en) | Periphery monitoring device for working machine | |
| JP2002046533A (en) | Vehicular rear view support device | |
| JP2020176460A (en) | Surroundings monitoring device for work machine | |
| US12077948B2 (en) | System and method for maintaining a view of an area of interest proximate a work vehicle | |
| US20230339402A1 (en) | Selectively utilizing multiple imaging devices to maintain a view of an area of interest proximate a work vehicle | |
| WO2023136070A1 (en) | Remote operation support system and remote operation support method | |
| JP2024051730A (en) | System and method for displaying surrounding images of a work vehicle, and work vehicle | |
| JP2004113027A (en) | Rolling control device of agricultural work machine |