JP7655176B2

JP7655176B2 - forklift

Info

Publication number: JP7655176B2
Application number: JP2021157757A
Authority: JP
Inventors: 匡弘岸田
Original assignee: Toyota Industries Corp
Current assignee: Toyota Industries Corp
Priority date: 2021-09-28
Filing date: 2021-09-28
Publication date: 2025-04-02
Anticipated expiration: 2041-09-28
Also published as: JP2023048442A

Description

本開示は、フォークリフトに関する。 This disclosure relates to a forklift.

フォークリフトは、フォークと、制御装置と、を備える。フォークリフトは、フォークにパレットを積載する。フォークにパレットを積載する際には、パレットの備える差込孔にフォークを差し込んだ後に、フォークを上昇させる。差込孔にフォークを差し込む過程でフォークがパレットに接触すると、円滑にパレットの積載を行えない場合がある。このため、差込孔にフォークを差し込む前には、制御装置がフォークの位置合わせを行う。特許文献１に記載のフォークリフトは、差込口の形状及び寸法に基づいてパレットの傾き量を導出している。差込口とは、差込孔の開口部である。 The forklift includes a fork and a control device. The forklift loads a pallet onto the fork. When loading the pallet onto the fork, the fork is inserted into an insertion hole provided on the pallet, and then the fork is raised. If the fork comes into contact with the pallet during the process of inserting the fork into the insertion hole, the pallet may not be loaded smoothly. For this reason, the control device aligns the fork before inserting the fork into the insertion hole. The forklift described in Patent Document 1 derives the amount of inclination of the pallet based on the shape and dimensions of the insertion hole. The insertion hole is the opening of the insertion hole.

特開２０１９－１５６６４１号公報JP 2019-156641 A

フォークの位置合わせを行う際には、フォークの位置、及びフォークの傾きが調整される。特許文献１のように、差込口の形状、及び寸法に基づいてパレットの位置、及びパレットの傾きを検出することで、フォークの位置合わせを行うことができる。この場合、フォークの位置合わせは、フォークと差込口とが向かい合い、かつ、フォークとパレットとの傾きが一致するように行われる。しかしながら、フォークの位置合わせを行った場合であっても、差込孔へのフォークの差し込みが完了するまでに、フォークとパレットとが接触するおそれがある。 When aligning the forks, the position and inclination of the forks are adjusted. As in Patent Document 1, the forks can be aligned by detecting the position and inclination of the pallet based on the shape and dimensions of the insertion hole. In this case, the forks are aligned so that the forks and the insertion hole face each other and the inclinations of the forks and the pallet match. However, even when the forks are aligned, there is a risk that the forks and the pallet may come into contact before the forks are completely inserted into the insertion holes.

上記課題を解決するフォークリフトは、フォークと、３次元座標系の座標で物体の位置を検出する外界センサと、制御装置と、を備え、前記制御装置は、前記外界センサからパレットの差込孔を画定する画定面の位置を表した点の集合である点群データを取得し、前記フォークと前記パレットとが向かい合う方向に直交する方向における前記フォークと前記差込孔との相対距離、及び前記フォークと前記パレットとが向かい合う方向に対する前記フォークと前記差込孔との相対角度を前記点群データから導出し、前記相対距離及び前記相対角度に基づき、前記差込孔への前記フォークの差し込み中に、前記フォークと前記パレットとが接触しないように前記フォークの位置を調整する。 The forklift that solves the above problem includes a fork, an external sensor that detects the position of an object in coordinates of a three-dimensional coordinate system, and a control device. The control device acquires point cloud data, which is a set of points representing the position of a boundary surface that defines the insertion hole of a pallet, from the external sensor, derives from the point cloud data the relative distance between the fork and the insertion hole in a direction perpendicular to the direction in which the fork and the pallet face each other, and the relative angle between the fork and the insertion hole with respect to the direction in which the fork and the pallet face each other, and adjusts the position of the fork based on the relative distance and relative angle so that the fork and the pallet do not come into contact while the fork is being inserted into the insertion hole.

点群データは、画定面の位置を表した点の集合である。フォークを差込孔に差し込む過程でフォークが接触するおそれがあるのは、画定面である。点群データからフォークと差込孔との相対距離、及びフォークと差込孔との相対角度を導出することで、フォークの差し込み量に応じた相対距離、及び相対角度を導出することができる。そして、この相対距離、及び相対角度に基づきフォークの位置を調整することで、差込孔へのフォークの差し込み中にフォークの位置を調整することができる。フォークの差し込み中であっても、フォークとパレットとが接触しないようにフォークの位置を調整できるため、フォークの差し込みが完了するまでにフォークとパレットとが接触することを抑制できる。 Point cloud data is a collection of points that represent the position of a demarcation surface. It is the demarcation surface that the fork may come into contact with while inserting the fork into the insertion hole. By deriving the relative distance between the fork and the insertion hole and the relative angle between the fork and the insertion hole from the point cloud data, it is possible to derive a relative distance and relative angle according to the amount of insertion of the fork. Then, by adjusting the position of the fork based on this relative distance and relative angle, it is possible to adjust the position of the fork while it is being inserted into the insertion hole. Even while the fork is being inserted, the position of the fork can be adjusted so that it does not come into contact with the pallet, so it is possible to prevent contact between the fork and the pallet before the fork is completely inserted.

上記フォークリフトについて、前記フォークリフトは、記憶装置を備え、前記記憶装置は、前記フォークと前記パレットとが接触しない状態での前記画定面の位置を、前記３次元座標系の座標で表した点の集合である点群マップを記憶しており、前記制御装置は、前記点群マップに対する前記点群データの距離差から前記相対距離を導出し、前記点群マップに対する前記点群データの傾きから前記相対角度を導出してもよい。 The forklift may be equipped with a storage device that stores a point cloud map, which is a set of points that represent the positions of the demarcation plane when the fork and the pallet are not in contact, in coordinates of the three-dimensional coordinate system, and the control device may derive the relative distance from a distance difference of the point cloud data with respect to the point cloud map, and derive the relative angle from an inclination of the point cloud data with respect to the point cloud map.

上記フォークリフトについて、前記フォークリフトは、記憶装置を備え、前記記憶装置は、前記フォークと前記パレットとが接触しない状態での前記画定面の位置を、前記３次元座標系の座標で表した点の集合である点群マップを記憶しており、前記制御装置は、前記点群マップから平面を検出し、前記点群データから、前記平面と同一の平面を検出し、前記点群マップから検出された前記平面と前記点群データから検出された前記平面との距離差から前記相対距離を導出し、前記点群マップから検出された前記平面に対する前記点群データから検出された前記平面の傾きから前記相対角度を導出してもよい。 Regarding the forklift described above, the forklift may be equipped with a storage device, and the storage device may store a point cloud map, which is a set of points that represent the positions of the demarcation plane when the fork and the pallet are not in contact, in the coordinates of the three-dimensional coordinate system. The control device may detect a plane from the point cloud map, detect a plane identical to the plane from the point cloud data, derive the relative distance from the difference in distance between the plane detected from the point cloud map and the plane detected from the point cloud data, and derive the relative angle from the inclination of the plane detected from the point cloud data relative to the plane detected from the point cloud map.

本発明によれば、フォークの差し込みが完了するまでにフォークとパレットとが接触することを抑制できる。 This invention prevents the forks from coming into contact with the pallet until the forks are completely inserted.

トラック、及びフォークリフトを模式的に示す図である。FIG. 2 is a diagram illustrating a truck and a forklift. 図１のトラックが備える荷台に積載されたパレットの正面図である。FIG. 2 is a front view of a pallet loaded on a loading platform of the truck of FIG. 1 . 図２のパレットの斜視図である。FIG. 3 is a perspective view of the pallet of FIG. 2 . 図２のパレットの断面図である。FIG. 3 is a cross-sectional view of the pallet of FIG. 2. 図１の区域で運用されているフォークリフトの側面図である。FIG. 2 is a side view of a forklift operating in the area of FIG. 1 . 図５のフォークリフトが備える荷役装置の斜視図である。FIG. 6 is a perspective view of a loading device provided on the forklift of FIG. 5 . 図５のフォークリフトの概略構成図である。FIG. 6 is a schematic diagram of the forklift shown in FIG. 5 . 図５のフォークリフトが備える補助記憶装置に記憶された点群マップの一例を示す図である。FIG. 6 is a diagram showing an example of a point cloud map stored in an auxiliary storage device included in the forklift of FIG. 5 . 第１実施形態の位置合わせ制御の処理手順を示すフローチャートである。5 is a flowchart showing a processing procedure of alignment control according to the first embodiment. 第２実施形態の位置合わせ制御の処理手順を示すフローチャートである。10 is a flowchart showing a processing procedure of alignment control according to a second embodiment. 変形例の位置合わせ制御の処理手順を示すフローチャートである。13 is a flowchart showing a processing procedure of alignment control in a modified example.

（第１実施形態）
フォークリフトの第１実施形態について説明する。
図１に示すように、区域Ａ１には、トラック１０が停車される。トラック１０は、荷台１１を備える。トラック１０の荷台１１には、パレット２０が積載されている。区域Ａ１は、例えば、工場、港湾、空港、商業施設及び公共施設などの場所の全体、あるいは、一部である。区域Ａ１では、フォークリフト４０が運用されている。フォークリフト４０は、トラック１０に積載されたパレット２０の荷取りを行う。フォークリフト４０は、荷取りを行った後に、別の位置又は別の装置までパレット２０の搬送を行う。 First Embodiment
A first embodiment of the forklift will be described.
As shown in Fig. 1, a truck 10 is parked in area A1. The truck 10 has a loading platform 11. A pallet 20 is loaded on the loading platform 11 of the truck 10. Area A1 is, for example, the whole or part of a location such as a factory, a port, an airport, a commercial facility, or a public facility. A forklift 40 is in operation in area A1. The forklift 40 unloads the pallet 20 loaded on the truck 10. After unloading, the forklift 40 transports the pallet 20 to another location or another device.

図２に示すように、パレット２０は平パレットである。パレット２０は、第１壁部２１と、第２壁部２４と、桁２７，２９と、を備える。
図３に示すように、第１壁部２１及び第２壁部２４は、板状である。第１壁部２１及び第２壁部２４は、四角形状である。第１壁部２１と第２壁部２４とは互いに向かい合っている。第１壁部２１は、第１外面２２と、第１内面２３と、を備える。第１外面２２と第１内面２３とは第１壁部２１の厚み方向に互いに反対となる面である。第２壁部２４は、第２外面２５と、第２内面２６と、を備える。第２外面２５と第２内面２６とは第２壁部２４の厚み方向に互いに反対となる面である。第１壁部２１の第１内面２３と第２壁部２４の第２内面２６とは互いに向かい合っている。第１外面２２、第１内面２３、第２外面２５及び第２内面２６は、互いに平行である。 2, the pallet 20 is a flat pallet. The pallet 20 includes a first wall portion 21, a second wall portion 24, and beams 27 and 29.
As shown in FIG. 3, the first wall portion 21 and the second wall portion 24 are plate-shaped. The first wall portion 21 and the second wall portion 24 are rectangular. The first wall portion 21 and the second wall portion 24 face each other. The first wall portion 21 includes a first outer surface 22 and a first inner surface 23. The first outer surface 22 and the first inner surface 23 are surfaces that are opposite to each other in the thickness direction of the first wall portion 21. The second wall portion 24 includes a second outer surface 25 and a second inner surface 26. The second outer surface 25 and the second inner surface 26 are surfaces that are opposite to each other in the thickness direction of the second wall portion 24. The first inner surface 23 of the first wall portion 21 and the second inner surface 26 of the second wall portion 24 face each other. The first outer surface 22, the first inner surface 23, the second outer surface 25, and the second inner surface 26 are parallel to each other.

桁２７，２９は、第１壁部２１と第２壁部２４との間に設けられている。桁２７，２９は、第１桁２７と、第２桁２９と、を含む。第１桁２７は４つ設けられている。第１桁２７は、パレット２０の四隅に１つずつ設けられている。第２桁２９は、４つ設けられている。第２桁２９は、第１桁２７同士の間に１つずつ設けられている。第１桁２７は、第１面２８を備える。第１面２８は、第２桁２９に向かい合う面である。第１桁２７は、２つの第１面２８を備える。第２桁２９は、第２面３０を備える。第２面３０は、第１桁２７に向かい合う面である。第２桁２９は、２つの第２面３０を備える。第１面２８と第２面３０とは互いに向かい合っている。 The girders 27, 29 are provided between the first wall portion 21 and the second wall portion 24. The girders 27, 29 include a first girder 27 and a second girder 29. Four first girders 27 are provided. One first girder 27 is provided at each of the four corners of the pallet 20. Four second girders 29 are provided. One second girder 29 is provided between each of the first girders 27. The first girders 27 have a first surface 28. The first surface 28 is a surface facing the second girders 29. The first girders 27 have two first surfaces 28. The second girders 29 have a second surface 30. The second surface 30 is a surface facing the first girders 27. The second girders 29 have two second surfaces 30. The first surface 28 and the second surface 30 face each other.

第１内面２３、第２内面２６、第１面２８、及び第２面３０に囲まれる領域は、差込孔３１である。第１内面２３、第２内面２６、第１面２８、及び第２面３０は、差込孔３１を画定している画定面である。 The area surrounded by the first inner surface 23, the second inner surface 26, the first surface 28, and the second surface 30 is the insertion hole 31. The first inner surface 23, the second inner surface 26, the first surface 28, and the second surface 30 are the defining surfaces that define the insertion hole 31.

図４に示すように、第１壁部２１は、第１貫通孔画定面３２を備える。第１貫通孔画定面３２は、第１外面２２と第１内面２３との間で延びている。第１貫通孔画定面３２は、環状の面である。第１貫通孔画定面３２に囲まれる領域は、第１貫通孔３３である。第１貫通孔画定面３２は、第１貫通孔３３を画定している。第１貫通孔３３は、第１壁部２１を貫通している。 As shown in FIG. 4, the first wall portion 21 has a first through hole defining surface 32. The first through hole defining surface 32 extends between the first outer surface 22 and the first inner surface 23. The first through hole defining surface 32 is an annular surface. The area surrounded by the first through hole defining surface 32 is the first through hole 33. The first through hole defining surface 32 defines the first through hole 33. The first through hole 33 penetrates the first wall portion 21.

第２壁部２４は、第２貫通孔画定面３４を備える。第２貫通孔画定面３４は、第２外面２５と第２内面２６との間で延びている。第２貫通孔画定面３４は、環状の面である。第２貫通孔画定面３４に囲まれる領域は、第２貫通孔３５である。第２貫通孔画定面３４は、第２貫通孔３５を画定している。第２貫通孔３５は、第２壁部２４を貫通している。 The second wall portion 24 has a second through hole defining surface 34. The second through hole defining surface 34 extends between the second outer surface 25 and the second inner surface 26. The second through hole defining surface 34 is an annular surface. The area surrounded by the second through hole defining surface 34 is the second through hole 35. The second through hole defining surface 34 defines the second through hole 35. The second through hole 35 penetrates the second wall portion 24.

図５に示すように、フォークリフト４０は、駆動輪４１と、操舵輪４２と、荷役装置４３と、外界センサ６１と、を備える。荷役装置４３は、マスト４４と、リフトブラケット４７と、２つのフォーク４８と、リフトシリンダ５１と、ティルトシリンダ５２と、を備える。 As shown in FIG. 5, the forklift 40 includes drive wheels 41, steering wheels 42, a loading device 43, and an external sensor 61. The loading device 43 includes a mast 44, a lift bracket 47, two forks 48, a lift cylinder 51, and a tilt cylinder 52.

マスト４４は、アウタマスト４５と、インナマスト４６と、を備える。インナマスト４６は、アウタマスト４５に対して昇降可能に設けられている。
リフトブラケット４７は、インナマスト４６に取り付けられている。フォーク４８は、リフトブラケット４７に取り付けられている。リフトブラケット４７及びフォーク４８は、インナマスト４６とともに昇降可能である。 The mast 44 includes an outer mast 45 and an inner mast 46. The inner mast 46 is provided so as to be capable of ascending and descending relative to the outer mast 45.
The lift bracket 47 is attached to the inner mast 46. The fork 48 is attached to the lift bracket 47. The lift bracket 47 and the fork 48 can be raised and lowered together with the inner mast 46.

フォーク４８は、差込部４９を備える。差込部４９は、リフトブラケット４７からフォークリフト４０の前方に延びている。差込部４９は、フォーク４８にパレット２０を積載する際に、差込孔３１に差し込まれる部分である。差込部４９は、上面５０を備える。 The fork 48 has an insertion portion 49. The insertion portion 49 extends from the lift bracket 47 to the front of the forklift 40. The insertion portion 49 is inserted into the insertion hole 31 when loading the pallet 20 onto the fork 48. The insertion portion 49 has an upper surface 50.

リフトシリンダ５１は、インナマスト４６を昇降動作させる。ティルトシリンダ５２は、マスト４４をフォークリフト４０の前後方向に傾動動作させる。リフトシリンダ５１及びティルトシリンダ５２は、油圧シリンダである。 The lift cylinder 51 raises and lowers the inner mast 46. The tilt cylinder 52 tilts the mast 44 in the forward and backward directions of the forklift 40. The lift cylinder 51 and the tilt cylinder 52 are hydraulic cylinders.

図６に示すように、外界センサ６１は、フォークリフト４０の車幅方向において２つのフォーク４８の間に設けられている。外界センサ６１は、フォーク４８とともに昇降するように設けられている。本実施形態において、外界センサ６１はリフトブラケット４７に設けられている。 As shown in FIG. 6, the external sensor 61 is provided between the two forks 48 in the vehicle width direction of the forklift 40. The external sensor 61 is provided so as to rise and fall together with the forks 48. In this embodiment, the external sensor 61 is provided on the lift bracket 47.

外界センサ６１は、３次元座標系での座標で物体の位置を検出する。外界センサ６１は、第１内面２３、第２内面２６、第１面２８、及び第２面３０の３次元座標系での座標を取得するために設けられている。外界センサ６１は、差込部４９が差込孔３１に差し込まれている状態であっても、第１内面２３、第２内面２６、第１面２８、及び第２面３０の少なくとも１つの座標を取得できるように設けられている。外界センサ６１としては、例えば、ミリ波レーダー、ToF（Time of Flight）カメラ、及びLIDAR（Laser Imaging Detection and Ranging）を用いることができる。パレット２０の差込孔３１のように、奥行きに物体が存在していない場合、ステレオカメラでは座標の検出精度が低下する場合がある。このため、外界センサ６１としてはレーザーの反射光又は電波の反射波のように、反射を利用したものを用いることが好ましい。 The external sensor 61 detects the position of an object in terms of coordinates in a three-dimensional coordinate system. The external sensor 61 is provided to acquire the coordinates of the first inner surface 23, the second inner surface 26, the first surface 28, and the second surface 30 in the three-dimensional coordinate system. The external sensor 61 is provided so as to acquire at least one coordinate of the first inner surface 23, the second inner surface 26, the first surface 28, and the second surface 30 even when the insertion portion 49 is inserted into the insertion hole 31. For example, a millimeter wave radar, a ToF (Time of Flight) camera, and a LIDAR (Laser Imaging Detection and Ranging) can be used as the external sensor 61. When there is no object in the depth, such as the insertion hole 31 of the pallet 20, the detection accuracy of the coordinates may decrease with a stereo camera. For this reason, it is preferable to use a sensor that utilizes reflection, such as reflected light of a laser or reflected waves of radio waves, as the external sensor 61.

本実施形態では、外界センサ６１としてLIDARを用いている。外界センサ６１は、周囲にレーザーを照射し、レーザーが当たった点から反射された反射光を受光することで点までの距離を導出する。レーザーが当たった点は、物体の表面の一部を表す。点の位置は、極座標系の座標で表すことができる。極座標系における点の座標は、直交座標系の座標に変換される。外界センサ６１は、センサ座標系での点の座標を導出する。センサ座標系は、外界センサ６１を原点とする３次元座標系である。センサ座標系は、３軸直交座標系であって、Ｘ軸と、Ｙ軸と、Ｚ軸と、を備える。Ｘ軸の延びる方向とフォークリフト４０の車幅方向とは一致している。荷役装置４３が傾動していない状態でＹ軸の延びる方向とフォークリフト４０の前後方向とは一致している。荷役装置４３が傾動していない状態でＺ軸の延びる方向と鉛直方向とは一致している。差込部４９を差込孔３１に差し込む際には、フォークリフト４０の前方にパレット２０が位置した状態でフォークリフト４０が前進する。Ｙ軸の延びる方向は、フォーク４８とパレット２０とが向かい合う方向である。図１、図５、及び図６には、センサ座標系のＸ軸を矢印Ｘ、Ｙ軸を矢印Ｙ、Ｚ軸を矢印Ｚで示している。 In this embodiment, a LIDAR is used as the external sensor 61. The external sensor 61 irradiates the surroundings with a laser and derives the distance to the point by receiving the reflected light reflected from the point where the laser hits. The point where the laser hits represents a part of the surface of the object. The position of the point can be expressed by the coordinates of a polar coordinate system. The coordinates of the point in the polar coordinate system are converted to the coordinates of a Cartesian coordinate system. The external sensor 61 derives the coordinates of the point in the sensor coordinate system. The sensor coordinate system is a three-dimensional coordinate system with the external sensor 61 as the origin. The sensor coordinate system is a three-axis Cartesian coordinate system and includes an X axis, a Y axis, and a Z axis. The direction in which the X axis extends coincides with the vehicle width direction of the forklift 40. When the loading device 43 is not tilted, the direction in which the Y axis extends coincides with the front-rear direction of the forklift 40. When the loading device 43 is not tilted, the direction in which the Z axis extends coincides with the vertical direction. When inserting the insertion portion 49 into the insertion hole 31, the forklift 40 moves forward with the pallet 20 positioned in front of the forklift 40. The direction along which the Y axis extends is the direction in which the forks 48 and the pallet 20 face each other. In Figures 1, 5, and 6, the X-axis of the sensor coordinate system is indicated by the arrow X, the Y-axis by the arrow Y, and the Z-axis by the arrow Z.

図７に示すように、フォークリフト４０は、積載センサ６２と、駆動機構６３と、油圧機構６４と、制御装置６５と、補助記憶装置６８と、を備える。
積載センサ６２は、フォーク４８にパレット２０が積載されていることを検出するためのセンサである。積載センサ６２は、例えば、差込部４９の基端に設けられる。フォークリフト４０がパレット２０の荷取りを行う際には、差込部４９が差込孔３１に差し込まれていく。差込部４９が差込孔３１に差し込まれていくと、パレット２０が差込部４９の基端に近付いていく。これにより、差込部４９が差込孔３１に所定量以上差し込まれると、積載センサ６２によってフォーク４８にパレット２０が積載されていることが検出される。所定量とは、差込部４９の先端から積載センサ６２までの距離である。 As shown in FIG. 7 , the forklift 40 includes a load sensor 62 , a drive mechanism 63 , a hydraulic mechanism 64 , a control device 65 , and an auxiliary storage device 68 .
The loading sensor 62 is a sensor for detecting that a pallet 20 is loaded on the forks 48. The loading sensor 62 is provided, for example, at the base end of the insertion portion 49. When the forklift 40 picks up the pallet 20, the insertion portion 49 is inserted into the insertion hole 31. As the insertion portion 49 is inserted into the insertion hole 31, the pallet 20 approaches the base end of the insertion portion 49. As a result, when the insertion portion 49 is inserted into the insertion hole 31 by a predetermined amount or more, the loading sensor 62 detects that the pallet 20 is loaded on the forks 48. The predetermined amount is the distance from the tip of the insertion portion 49 to the loading sensor 62.

駆動機構６３は、フォークリフト４０を走行動作させるための部材であり、駆動輪４１を駆動させるための走行用モータや、操舵輪４２を操舵させるための操舵機構を含む。
油圧機構６４は、リフトシリンダ５１及びティルトシリンダ５２への作動油の給排を制御するための部材であり、ポンプを駆動させるための荷役モータや、コントロールバルブを含む。リフトシリンダ５１への作動油の給排によってインナマスト４６は昇降する。ティルトシリンダ５２への作動油の給排によってマスト４４はフォークリフト４０の前後方向に対して傾動する。インナマスト４６の昇降とはフォーク４８の昇降ともいえる。マスト４４の傾動とは、フォーク４８の傾動ともいえる。 The drive mechanism 63 is a member for causing the forklift 40 to travel, and includes a travel motor for driving the drive wheels 41 and a steering mechanism for steering the steering wheels 42 .
The hydraulic mechanism 64 is a member for controlling the supply and discharge of hydraulic oil to the lift cylinder 51 and the tilt cylinder 52, and includes a loading motor for driving a pump and a control valve. The inner mast 46 is raised and lowered by the supply and discharge of hydraulic oil to the lift cylinder 51. The mast 44 is tilted in the front-rear direction of the forklift 40 by the supply and discharge of hydraulic oil to the tilt cylinder 52. The raising and lowering of the inner mast 46 can also be referred to as the raising and lowering of the forks 48. The tilting of the mast 44 can also be referred to as the tilting of the forks 48.

制御装置６５は、プロセッサ６６と、記憶部６７と、を備える。記憶部６７は、RAM（Random Access Memory）、及びROM（Read Only Memory）を含む。記憶部６７は、処理をプロセッサ６６に実行させるように構成されたプログラムコードまたは指令を格納している。記憶部６７、即ち、コンピュータ可読媒体は、汎用または専用のコンピュータでアクセスできるあらゆる利用可能な媒体を含む。制御装置６５は、ASICやFPGA等のハードウェア回路によって構成されていてもよい。処理回路である制御装置６５は、コンピュータプログラムに従って動作する１つ以上のプロセッサ、ASICやFPGA等の１つ以上のハードウェア回路、或いは、それらの組み合わせを含み得る。 The control device 65 includes a processor 66 and a storage unit 67. The storage unit 67 includes a random access memory (RAM) and a read only memory (ROM). The storage unit 67 stores program code or instructions configured to cause the processor 66 to execute processes. The storage unit 67, i.e., the computer readable medium, includes any available medium accessible by a general-purpose or special-purpose computer. The control device 65 may be configured by a hardware circuit such as an ASIC or an FPGA. The control device 65, which is a processing circuit, may include one or more processors that operate according to a computer program, one or more hardware circuits such as an ASIC or an FPGA, or a combination thereof.

制御装置６５は、駆動機構６３及び油圧機構６４を制御する。フォークリフト４０は、制御装置６５による制御によって走行、及び荷役が行われる自動運転フォークリフトである。 The control device 65 controls the drive mechanism 63 and the hydraulic mechanism 64. The forklift 40 is an automatically operated forklift that travels and handles cargo under the control of the control device 65.

補助記憶装置６８は、制御装置６５が読み取り可能な情報を記憶している。補助記憶装置６８としては、例えば、ハードディスクドライブ、及びソリッドステートドライブを挙げることができる。 The auxiliary storage device 68 stores information that can be read by the control device 65. Examples of the auxiliary storage device 68 include a hard disk drive and a solid state drive.

補助記憶装置６８は、点群マップＭ１を記憶している。図８に示すように、点群マップＭ１は、パレット２０の内部をセンサ座標系での座標で表した点Ｐ１の集合である。図８に示す点群マップＭ１は、図４に示すパレット２０の断面に対応した点群マップＭ１である。説明の便宜上、図８に示す点群マップＭ１では、パレット２０の断面に対応した点Ｐ１を図示しているが、点群マップＭ１は、画定面の全体に亘って所定の間隔で得られた点Ｐ１の集合である。補助記憶装置６８は、記憶装置である。 The auxiliary storage device 68 stores a point cloud map M1. As shown in FIG. 8, the point cloud map M1 is a collection of points P1 that represent the interior of the pallet 20 by coordinates in the sensor coordinate system. The point cloud map M1 shown in FIG. 8 is a point cloud map M1 that corresponds to the cross section of the pallet 20 shown in FIG. 4. For ease of explanation, the point cloud map M1 shown in FIG. 8 illustrates a point P1 that corresponds to the cross section of the pallet 20, but the point cloud map M1 is a collection of points P1 obtained at a predetermined interval across the entire defined surface. The auxiliary storage device 68 is a storage device.

点群マップＭ１は、第１内面２３、第２内面２６、第１面２８、及び第２面３０の位置をセンサ座標系の座標で表したデータである。点群マップＭ１は、フォーク４８と差込孔３１との相対距離が０、かつ、フォーク４８と差込孔３１との相対角度が０の状態で取得されたセンサ座標系の点Ｐ１の座標である。フォーク４８と差込孔３１との相対距離が０、かつ、フォーク４８と差込孔３１との相対角度が０の状態とは、フォークリフト４０とパレット２０とが向かい合っている状態でフォークリフト４０を前進させた場合に、フォーク４８とパレット２０とが接触することなくフォーク４８が差込孔３１に差し込まれる状態である。以下の説明において、フォーク４８と差込孔３１との相対距離を相対距離、フォーク４８と差込孔３１との相対角度を相対角度と称する。 The point cloud map M1 is data that represents the positions of the first inner surface 23, the second inner surface 26, the first surface 28, and the second surface 30 in coordinates of the sensor coordinate system. The point cloud map M1 is the coordinates of the point P1 in the sensor coordinate system acquired in a state where the relative distance between the fork 48 and the insertion hole 31 is 0 and the relative angle between the fork 48 and the insertion hole 31 is 0. The state where the relative distance between the fork 48 and the insertion hole 31 is 0 and the relative angle between the fork 48 and the insertion hole 31 is 0 means that when the forklift 40 and the pallet 20 face each other and the forklift 40 is moved forward, the fork 48 is inserted into the insertion hole 31 without contacting the pallet 20. In the following description, the relative distance between the fork 48 and the insertion hole 31 is referred to as the relative distance, and the relative angle between the fork 48 and the insertion hole 31 is referred to as the relative angle.

相対距離とは、センサ座標系のＸＺ平面でのフォーク４８と差込孔３１との位置のずれ量である。ＸＺ平面とは、センサ座標系のＸ軸とセンサ座標系のＺ軸によって規定される平面である。ＸＺ平面でのフォーク４８の先端の中心位置と、ＸＺ平面での差込孔３１の中心位置とが一致する場合、相対距離は０である。相対距離が０の場合、ＸＺ平面においてフォーク４８と画定面との間には、クリアランスが存在している。ＸＺ平面は、フォーク４８とパレット２０とが向かい合う方向に直交する方向に拡がる平面である。相対距離は、フォーク４８とパレット２０とが向かい合う方向に直交する方向におけるフォーク４８と差込孔３１との相対距離といえる。 The relative distance is the amount of deviation in position between the fork 48 and the insertion hole 31 on the XZ plane of the sensor coordinate system. The XZ plane is a plane defined by the X axis of the sensor coordinate system and the Z axis of the sensor coordinate system. When the center position of the tip of the fork 48 on the XZ plane coincides with the center position of the insertion hole 31 on the XZ plane, the relative distance is 0. When the relative distance is 0, there is a clearance between the fork 48 and the demarcated surface on the XZ plane. The XZ plane is a plane that extends in a direction perpendicular to the direction in which the fork 48 and the pallet 20 face each other. The relative distance can be said to be the relative distance between the fork 48 and the insertion hole 31 in the direction perpendicular to the direction in which the fork 48 and the pallet 20 face each other.

相対角度とは、Ｙ軸に対するフォーク４８の傾きと、Ｙ軸に対する差込孔３１の傾きとのずれ量である。Ｙ軸に対するフォーク４８の傾きとは、フォーク４８の上面５０とＹ軸とのなす角である。Ｙ軸に対する差込孔３１の傾きとは、パレット２０の第１内面２３とＹ軸とのなす角である。第１内面２３、第２内面２６、第１外面２２、及び第２外面２５は、互いに平行である。従って、Ｙ軸に対する差込孔３１の傾きは、Ｙ軸に対する第２内面２６、第１外面２２、及び第２外面２５の傾きともいえる。Ｙ軸の延びる方向は、フォーク４８とパレット２０とが向かい合う方向である。相対角度は、フォーク４８とパレット２０とが向かい合う方向に対するフォーク４８と差込孔３１との相対角度といえる。 The relative angle is the deviation between the inclination of the fork 48 with respect to the Y axis and the inclination of the insertion hole 31 with respect to the Y axis. The inclination of the fork 48 with respect to the Y axis is the angle between the top surface 50 of the fork 48 and the Y axis. The inclination of the insertion hole 31 with respect to the Y axis is the angle between the first inner surface 23 of the pallet 20 and the Y axis. The first inner surface 23, the second inner surface 26, the first outer surface 22, and the second outer surface 25 are parallel to each other. Therefore, the inclination of the insertion hole 31 with respect to the Y axis can also be said to be the inclination of the second inner surface 26, the first outer surface 22, and the second outer surface 25 with respect to the Y axis. The direction in which the Y axis extends is the direction in which the fork 48 and the pallet 20 face each other. The relative angle can be said to be the relative angle between the fork 48 and the insertion hole 31 with respect to the direction in which the fork 48 and the pallet 20 face each other.

点群マップＭ１は、少なくとも画定面の位置をセンサ座標系の座標で表した点Ｐ１を含んでいればよい。点群マップＭ１は、貫通孔画定面３２，３４の位置をセンサ座標系の座標で表した点Ｐ１を含んでいてもよい。 The point cloud map M1 may include at least a point P1 that represents the position of the demarcation surface in coordinates of the sensor coordinate system. The point cloud map M1 may also include a point P1 that represents the position of the through-hole demarcation surface 32, 34 in coordinates of the sensor coordinate system.

制御装置６５が行う位置合わせ制御について説明を行う。位置合わせ制御は、フォーク４８が差込孔３１に円滑に差し込まれるようにフォーク４８の昇降、及びフォーク４８の傾動を行う制御である。位置合わせ制御では、フォーク４８の左右方向への移動が行われてもよい。フォークリフト４０は、例えば、上位制御装置の指令に基づき、トラック１０に近付く。そして、フォークリフト４０とトラック１０との離間距離が所定離間距離未満になると、位置合わせ制御が開始される。所定離間距離は、例えば、外界センサ６１によってパレット２０が検出できる距離である。位置合わせ制御が行われている間、フォークリフト４０はパレット２０に向けて前進している。 The alignment control performed by the control device 65 will now be described. The alignment control is a control for raising and lowering the forks 48 and tilting the forks 48 so that the forks 48 are smoothly inserted into the insertion holes 31. In the alignment control, the forks 48 may be moved left and right. For example, the forklift 40 approaches the truck 10 based on a command from a higher-level control device. Then, when the distance between the forklift 40 and the truck 10 becomes less than a predetermined distance, the alignment control is started. The predetermined distance is, for example, a distance at which the pallet 20 can be detected by the external sensor 61. While the alignment control is being performed, the forklift 40 moves forward toward the pallet 20.

図９に示すように、ステップＳ１において、制御装置６５は、外界センサ６１から点群データを取得する。外界センサ６１から照射されたレーザーは、差込孔３１の内部に照射されている。点群データは、画定面にレーザーが照射されることで得られた点を含んだデータである。 As shown in FIG. 9, in step S1, the control device 65 acquires point cloud data from the external sensor 61. The laser emitted from the external sensor 61 is irradiated inside the insertion hole 31. The point cloud data is data including points obtained by irradiating the laser onto the demarcated surface.

次に、ステップＳ２において、制御装置６５は、相対距離、及び相対角度を導出する。制御装置６５は、ステップＳ１で取得した点群データと、点群マップＭ１とのスキャンマッチングを行う。スキャンマッチングは、例えば、NDT（Normal Distributions Transform）又はICP(Iterative Closest Point)を用いて行うことができる。 Next, in step S2, the control device 65 derives the relative distance and the relative angle. The control device 65 performs scan matching between the point cloud data acquired in step S1 and the point cloud map M1. Scan matching can be performed using, for example, NDT (Normal Distributions Transform) or ICP (Iterative Closest Point).

制御装置６５は、点群データの点と、点群データの点に対応する点群マップＭ１の点Ｐ１との距離が短くなるように、点群データと点群マップＭ１とのスキャンマッチングを行う。点群データの点に対応する点群マップＭ１の点Ｐ１とは、点群データの点に対応する座標と同一位置を表していると想定される点Ｐ１である。言い換えれば、点群データの点に対応する点群マップＭ１の点Ｐ１とは、点群データの点と同一物体を表していると想定される点Ｐ１である。制御装置６５は、スキャンマッチングを行うことによって点群データをどのように移動させ、どのように回転させれば点群マップＭ１に一致するかを特定することができる。 The control device 65 performs scan matching between the point cloud data and the point cloud map M1 so as to shorten the distance between a point of the point cloud data and a point P1 of the point cloud map M1 corresponding to the point of the point cloud data. The point P1 of the point cloud map M1 corresponding to a point of the point cloud data is a point P1 that is assumed to represent the same position as the coordinates corresponding to the point of the point cloud data. In other words, the point P1 of the point cloud map M1 corresponding to a point of the point cloud data is a point P1 that is assumed to represent the same object as the point of the point cloud data. By performing scan matching, the control device 65 can specify how to move and rotate the point cloud data so that it matches the point cloud map M1.

制御装置６５は、点群マップＭ１に対する点群データの距離差を導出できる。点群マップＭ１に対する点群データの距離差は、点群マップＭ１で表される画定面の位置と、点群データで表される画定面の位置との差である。点群マップＭ１は、相対距離が０の状態で取得されている。このため、点群マップＭ１と点群データとの距離差がない場合には、相対距離が０の状態といえる。ＸＺ平面でのフォーク４８と差込孔３１との位置にずれが生じている場合、このずれの分だけ、点群データは点群マップＭ１からずれる。点群マップＭ１と点群データとの距離差は、ＸＺ平面でのフォーク４８と差込孔３１との位置のずれ量を表しているといえる。 The control device 65 can derive the distance difference of the point cloud data with respect to the point cloud map M1. The distance difference of the point cloud data with respect to the point cloud map M1 is the difference between the position of the demarcation surface represented by the point cloud map M1 and the position of the demarcation surface represented by the point cloud data. The point cloud map M1 is acquired in a state where the relative distance is 0. Therefore, when there is no distance difference between the point cloud map M1 and the point cloud data, it can be said that the relative distance is in a state of 0. When there is a deviation in the positions of the fork 48 and the insertion hole 31 on the XZ plane, the point cloud data is deviated from the point cloud map M1 by the amount of this deviation. It can be said that the distance difference between the point cloud map M1 and the point cloud data represents the amount of deviation in the positions of the fork 48 and the insertion hole 31 on the XZ plane.

制御装置６５は、点群マップＭ１に対する点群データの傾きを導出できる。点群マップＭ１に対する点群データの傾きは、点群マップＭ１で表される画定面と、点群データで表される画定面との姿勢の差である。本実施形態の姿勢とは、センサ座標系のＹ軸に対する傾きである。点群マップＭ１に対して点群データが傾いていない場合には、相対角度が０といえる。フォーク４８の姿勢と画定面の姿勢とにずれが生じている場合、このずれの分だけ、点群データは点群マップＭ１に対して傾く。点群マップＭ１に対する点群データの傾きは、相対角度を表しているといえる。 The control device 65 can derive the inclination of the point cloud data relative to the point cloud map M1. The inclination of the point cloud data relative to the point cloud map M1 is the difference in attitude between the demarcation surface represented by the point cloud map M1 and the demarcation surface represented by the point cloud data. In this embodiment, the attitude is the inclination relative to the Y axis of the sensor coordinate system. If the point cloud data is not inclined relative to the point cloud map M1, the relative angle can be said to be 0. If there is a deviation between the attitude of the fork 48 and the attitude of the demarcation surface, the point cloud data will be inclined relative to the point cloud map M1 by the amount of this deviation. The inclination of the point cloud data relative to the point cloud map M1 can be said to represent a relative angle.

次に、ステップＳ３において、制御装置６５は、フォーク４８の位置合わせを行う。フォーク４８の位置合わせとは、差込部４９と画定面とが接触しないように、フォーク４８の位置を調整することである。制御装置６５は、相対距離が規定範囲に収まるようにフォーク４８の位置を調整する。規定範囲とは、差込孔３１へのフォーク４８の差し込み中にフォーク４８とパレット２０とが接触しないように設定された範囲である。 Next, in step S3, the control device 65 aligns the fork 48. Aligning the fork 48 means adjusting the position of the fork 48 so that the insertion portion 49 does not come into contact with the demarcation surface. The control device 65 adjusts the position of the fork 48 so that the relative distance falls within a specified range. The specified range is a range that is set so that the fork 48 does not come into contact with the pallet 20 while the fork 48 is being inserted into the insertion hole 31.

制御装置６５は、Ｚ軸の延びる方向に対する相対距離が規定範囲に収まっていない場合、油圧機構６４を制御することでフォーク４８を昇降させる。制御装置６５は、相対距離が短くなるようにフォーク４８を昇降させる。制御装置６５は、Ｚ軸の延びる方向に対する相対距離が規定範囲内になると、フォーク４８を停止させる。 When the relative distance in the direction in which the Z axis extends is not within a specified range, the control device 65 controls the hydraulic mechanism 64 to raise and lower the fork 48. The control device 65 raises and lowers the fork 48 so that the relative distance becomes shorter. The control device 65 stops the fork 48 when the relative distance in the direction in which the Z axis extends falls within a specified range.

制御装置６５は、Ｘ軸の延びる方向に対する相対距離が規定範囲に収まっていない場合、フォーク４８を左右方向に移動させる。制御装置６５は、駆動機構６３を制御して操舵を行うことによってフォーク４８を左右方向に移動させてもよい。フォークリフト４０がサイドシフト装置を備えている場合、制御装置６５は、サイドシフト装置によってフォーク４８を左右方向に移動させてもよい。サイドシフト装置は、サイドシフトシリンダを備え、サイドシフトシリンダへの作動油の給排によってフォーク４８を左右方向に移動させる装置である。制御装置６５は、相対距離が短くなるようにフォーク４８を左右方向に移動させる。制御装置６５は、Ｘ軸の延びる方向に対する相対距離が規定範囲内になると、フォーク４８を停止させる。 When the relative distance in the direction in which the X-axis extends is not within a specified range, the control device 65 moves the fork 48 in the left-right direction. The control device 65 may also move the fork 48 in the left-right direction by controlling the drive mechanism 63 to perform steering. When the forklift 40 is equipped with a side shift device, the control device 65 may move the fork 48 in the left-right direction by the side shift device. The side shift device is equipped with a side shift cylinder, and is a device that moves the fork 48 in the left-right direction by supplying and discharging hydraulic oil to the side shift cylinder. The control device 65 moves the fork 48 in the left-right direction so that the relative distance becomes shorter. The control device 65 stops the fork 48 when the relative distance in the direction in which the X-axis extends falls within a specified range.

制御装置６５は、相対角度が規定角度範囲に収まるようにフォーク４８の位置を調整する。規定角度範囲とは、差込孔３１へのフォーク４８の差し込み中にフォーク４８とパレット２０とが接触しないように設定された範囲である。制御装置６５は、相対角度が規定角度範囲に収まっていない場合、油圧機構６４を制御することでフォーク４８を傾動させる。制御装置６５は、相対角度が小さくなるようにフォーク４８を傾動させる。制御装置６５は、相対角度が規定角度範囲内になると、フォーク４８を停止させる。フォーク４８の位置合わせは、点群データが点群マップＭ１と重なり合うように行われているといえる。 The control device 65 adjusts the position of the fork 48 so that the relative angle falls within a specified angle range. The specified angle range is a range that is set so that the fork 48 and the pallet 20 do not come into contact while the fork 48 is being inserted into the insertion hole 31. If the relative angle does not fall within the specified angle range, the control device 65 tilts the fork 48 by controlling the hydraulic mechanism 64. The control device 65 tilts the fork 48 so that the relative angle becomes smaller. When the relative angle falls within the specified angle range, the control device 65 stops the fork 48. It can be said that the positioning of the fork 48 is performed so that the point cloud data overlaps with the point cloud map M1.

次に、ステップＳ４において、制御装置６５は、フォーク４８の差し込みが完了したか否かを判定する。フォーク４８の差し込みが完了したか否かは、積載センサ６２の検出結果から判定することができる。制御装置６５は、フォーク４８にパレット２０が積載されている場合、差し込みが完了したと判定する。制御装置６５は、フォーク４８にパレット２０が積載されていない場合、差し込みが完了していないと判定する。ステップＳ４の判定結果が否定の場合、制御装置６５は、ステップＳ１に戻る。ステップＳ４の判定結果が肯定の場合、制御装置６５は、位置合わせ制御を終了する。 Next, in step S4, the control device 65 determines whether or not the insertion of the forks 48 has been completed. Whether or not the insertion of the forks 48 has been completed can be determined from the detection result of the loading sensor 62. If a pallet 20 is loaded on the forks 48, the control device 65 determines that the insertion has been completed. If a pallet 20 is not loaded on the forks 48, the control device 65 determines that the insertion has not been completed. If the determination result of step S4 is negative, the control device 65 returns to step S1. If the determination result of step S4 is positive, the control device 65 ends the alignment control.

位置合わせ制御が終了すると、フォークリフト４０の前進は停止される。制御装置６５は、フォークリフト４０が前進している間、相対距離及び相対角度を導出する。制御装置６５は、フォークリフト４０が前進している間、相対距離及び相対角度に基づき、フォーク４８の位置合わせを行う。フォークリフト４０の前進中には、フォーク４８が差込孔３１に差し込まれている。制御装置６５は、差込孔３１へのフォーク４８の差し込み中に、相対距離及び相対角度を導出している。制御装置６５は、差込孔３１へのフォーク４８の差し込み中に、フォーク４８とパレット２０とが接触しないようにフォーク４８の位置を調整している。 When the alignment control ends, the forward movement of the forklift 40 is stopped. The control device 65 derives the relative distance and relative angle while the forklift 40 is moving forward. The control device 65 aligns the fork 48 based on the relative distance and relative angle while the forklift 40 is moving forward. While the forklift 40 is moving forward, the fork 48 is inserted into the insertion hole 31. The control device 65 derives the relative distance and relative angle while the fork 48 is being inserted into the insertion hole 31. While the fork 48 is being inserted into the insertion hole 31, the control device 65 adjusts the position of the fork 48 so that the fork 48 does not come into contact with the pallet 20.

第１実施形態の作用について説明する。
フォーク４８を差込孔３１に差し込む過程でフォーク４８が接触するおそれがあるのは、画定面である。点群データから相対距離、及び相対角度を導出することで、フォーク４８の差し込み量に応じた相対距離、及び相対角度を導出することができる。差込孔３１へのフォーク４８の差し込み中に相対距離、及び相対角度が大きくなった場合、これに合わせてフォーク４８の位置を調整することができる。仮に、差込孔３１の開口部である差込口を用いて相対距離、及び相対角度を導出した場合、フォーク４８の差し込み中に相対距離及び相対角度を導出したとしても、パレット２０の内部でのパレット２０とフォーク４８との位置関係を把握することができない。従って、パレット２０とフォーク４８とが接触するおそれがある。これに対し、画定面の座標を用いて相対距離及び相対角度を導出すると、フォーク４８の差し込み量に応じて、リアルタイムにパレット２０とフォーク４８との位置関係を把握することができる。制御装置６５は、相対距離、及び相対角度に基づきフォーク４８の位置を調整することで、差込孔３１へのフォーク４８の差し込み中にフォーク４８の位置を調整することができる。 The operation of the first embodiment will be described.
The fork 48 may come into contact with the demarcated surface during the process of inserting the fork 48 into the insertion hole 31. By deriving the relative distance and the relative angle from the point cloud data, the relative distance and the relative angle according to the insertion amount of the fork 48 can be derived. If the relative distance and the relative angle increase during the insertion of the fork 48 into the insertion hole 31, the position of the fork 48 can be adjusted accordingly. If the relative distance and the relative angle are derived using the insertion port, which is the opening of the insertion hole 31, the positional relationship between the pallet 20 and the fork 48 inside the pallet 20 cannot be grasped even if the relative distance and the relative angle are derived during the insertion of the fork 48. Therefore, there is a risk that the pallet 20 and the fork 48 may come into contact with each other. In contrast, if the relative distance and the relative angle are derived using the coordinates of the demarcated surface, the positional relationship between the pallet 20 and the fork 48 can be grasped in real time according to the insertion amount of the fork 48. The control device 65 can adjust the position of the fork 48 while the fork 48 is being inserted into the insertion hole 31 by adjusting the position of the fork 48 based on the relative distance and the relative angle.

第１実施形態の効果について説明する。
（１－１）制御装置６５は、フォーク４８の差し込み中にフォーク４８の位置合わせを行っている。フォーク４８の差し込み中であっても、フォーク４８とパレット２０とが接触しないようにフォーク４８の位置を調整できるため、フォーク４８の差し込みが完了するまでにフォーク４８とパレット２０とが接触することを抑制できる。 The effects of the first embodiment will be described.
(1-1) The control device 65 aligns the position of the fork 48 while the fork 48 is being inserted. Even while the fork 48 is being inserted, the position of the fork 48 can be adjusted so that the fork 48 does not come into contact with the pallet 20. This makes it possible to prevent the fork 48 from coming into contact with the pallet 20 until the insertion of the fork 48 is completed.

（１－２）制御装置６５は、点群マップＭ１と点群データとのスキャンマッチングによって相対距離、及び相対角度を導出している。相対距離が０、かつ、相対角度が０の状態で取得された点群マップＭ１を用いることで、点群マップＭ１と点群データとの距離差を相対距離とすることができる。点群マップＭ１に対する点群データの傾きを相対角度とすることができる。従って、制御装置６５は、スキャンマッチングを行うことによって、容易に相対距離、及び相対角度を導出することができる。 (1-2) The control device 65 derives the relative distance and relative angle by scan matching between the point cloud map M1 and the point cloud data. By using the point cloud map M1 acquired when the relative distance is 0 and the relative angle is 0, the distance difference between the point cloud map M1 and the point cloud data can be regarded as the relative distance. The inclination of the point cloud data relative to the point cloud map M1 can be regarded as the relative angle. Therefore, the control device 65 can easily derive the relative distance and relative angle by performing scan matching.

また、点群マップＭ１と点群データとのスキャンマッチングを行うことで、画定面に起伏が存在している場合など、画定面が複雑な形状をしている場合であっても、相対距離、及び相対角度を導出しやすい。実施形態のように、パレット２０が貫通孔画定面３２，３４を備えている場合、画定面に起伏が存在する。このような場合、点群データから平面を検出しにくく、平面の検出による相対距離及び相対角度の導出を行いにくい場合がある。これに対して、点群マップＭ１と点群データとのスキャンマッチングでは、平面を検出することなく相対距離、及び相対角度を導出することができる。従って、画定面の形状に依存せずに、相対距離、及び相対角度を導出することができる。 Furthermore, by performing scan matching between the point cloud map M1 and the point cloud data, it is easy to derive the relative distance and relative angle even when the demarcation surface has a complex shape, such as when the demarcation surface has undulations. When the pallet 20 has through-hole demarcation surfaces 32, 34 as in the embodiment, the demarcation surface has undulations. In such cases, it may be difficult to detect a plane from the point cloud data, and it may be difficult to derive the relative distance and relative angle by detecting the plane. In contrast, by scan matching between the point cloud map M1 and the point cloud data, it is possible to derive the relative distance and relative angle without detecting a plane. Therefore, it is possible to derive the relative distance and relative angle without depending on the shape of the demarcation surface.

（第２実施形態）
フォークリフトの第２実施形態について説明する。第２実施形態では、制御装置が行う位置合わせ制御が第１実施形態とは異なる。第２実施形態の位置合わせ制御について説明を行う。 Second Embodiment
A second embodiment of the forklift will be described. In the second embodiment, the positioning control performed by the control device is different from that in the first embodiment. The positioning control in the second embodiment will be described.

図１０に示すように、ステップＳ１１において、制御装置６５は、点群データを取得する。ステップＳ１１の処理は、ステップＳ１の処理と同様の処理である。
次に、ステップＳ１２において、制御装置６５は、点群マップＭ１から平面を検出する。詳細にいえば、制御装置６５は、点群マップＭ１から第１内面２３、第２内面２６、第１面２８、及び第２面３０のうち少なくともいずれかの平面を検出する。制御装置６５は、点群マップＭ１の各点Ｐ１の法線ベクトルを算出する。法線ベクトルは、複数の点Ｐ１に囲まれる平面に対して垂直な方向に向けたベクトルである。法線ベクトルを導出する手法としては各点Ｐ１から曲面を求め、曲面から各点Ｐ１の法線ベクトルを導出する手法や、ベクトルの外積を用いる手法を挙げることができる。例えば、制御装置６５が、１つの点Ｐ１の法線ベクトルを求める場合、この点Ｐ１から所定範囲内に位置する２つの点Ｐ１のそれぞれに向かうベクトルの外積を求める。この外積が法線ベクトルである。 10, in step S11, the control device 65 acquires point cloud data. The process of step S11 is similar to the process of step S1.
Next, in step S12, the control device 65 detects a plane from the point cloud map M1. More specifically, the control device 65 detects at least one of the first inner surface 23, the second inner surface 26, the first surface 28, and the second surface 30 from the point cloud map M1. The control device 65 calculates a normal vector of each point P1 of the point cloud map M1. The normal vector is a vector that is oriented in a direction perpendicular to the plane surrounded by the multiple points P1. Methods for deriving the normal vector include a method of determining a curved surface from each point P1 and deriving the normal vector of each point P1 from the curved surface, and a method using the cross product of vectors. For example, when the control device 65 determines the normal vector of one point P1, it determines the cross product of vectors that are directed from this point P1 to each of two points P1 located within a predetermined range. This cross product is the normal vector.

点群マップＭ１から第１内面２３、及び第２内面２６の少なくとも一方を検出する場合、制御装置６５は、法線ベクトルの向きが上下方向の点Ｐ１を抽出する。詳細にいえば、制御装置６５は、センサ座標系のＸＹ平面に対する法線ベクトルの角度が予め定められた範囲となるか否かを判定する。ＸＹ平面とは、センサ座標系のＸ軸とＹ軸とによって規定される平面である。点Ｐ１が第１内面２３、又は第２内面２６を表す点Ｐ１であれば、ＸＹ平面に対する当該点Ｐ１の法線ベクトルの角度は９０°となる。本実施形態では、トラック１０の停車位置による荷台１１の傾斜や外界センサ６１の精度等に基づき予め定められた範囲を設定している。範囲は、例えば、９０°±所定角度である。所定角度は、例えば、１０°である。制御装置６５は、ＸＹ平面に対する法線ベクトルの角度が予め定められた範囲に収まる点Ｐ１を、法線ベクトルの向きが上下方向の点Ｐ１として抽出する。この点Ｐ１は、第１内面２３、又は第２内面２６を表す点Ｐ１である。 When detecting at least one of the first inner surface 23 and the second inner surface 26 from the point cloud map M1, the control device 65 extracts a point P1 whose normal vector is oriented in the vertical direction. More specifically, the control device 65 determines whether the angle of the normal vector with respect to the XY plane of the sensor coordinate system is within a predetermined range. The XY plane is a plane defined by the X-axis and the Y-axis of the sensor coordinate system. If the point P1 is the point P1 representing the first inner surface 23 or the second inner surface 26, the angle of the normal vector of the point P1 with respect to the XY plane is 90°. In this embodiment, a predetermined range is set based on the inclination of the loading platform 11 due to the stopping position of the truck 10 and the accuracy of the external sensor 61. The range is, for example, 90°±a predetermined angle. The predetermined angle is, for example, 10°. The control device 65 extracts a point P1 whose angle of the normal vector with respect to the XY plane falls within a predetermined range as a point P1 whose normal vector is oriented in the vertical direction. This point P1 is point P1 that represents the first inner surface 23 or the second inner surface 26.

点群マップＭ１から第１面２８、及び第２面３０の少なくとも一方を検出する場合、制御装置６５は、センサ座標系のＹＺ平面に対する法線ベクトルの角度が予め定められた範囲となる点Ｐ１を抽出すればよい。これによって抽出された点Ｐ１は、第１面２８、又は第２面３０を表す点である。ＹＺ平面とは、センサ座標系のＹ軸とＺ軸とによって規定される平面である。 When detecting at least one of the first surface 28 and the second surface 30 from the point cloud map M1, the control device 65 may extract a point P1 whose normal vector angle with respect to the YZ plane of the sensor coordinate system is within a predetermined range. The point P1 extracted in this way is a point that represents the first surface 28 or the second surface 30. The YZ plane is a plane defined by the Y axis and the Z axis of the sensor coordinate system.

次に、ステップＳ１３において、制御装置６５は、点群データから平面を検出する。制御装置６５は、点群データから、ステップＳ１２で検出した平面と同一の平面を検出する。ステップＳ１２で第１内面２３を検出している場合、ステップＳ１３では点群データから第１内面２３が検出される。ステップＳ１２で第１内面２３、及び第１面２８が検出されている場合には、ステップＳ１３では点群データから第１内面２３、及び第１面２８が検出される。ステップＳ１３での平面の検出は、ステップＳ１２で点群マップＭ１に対して行われた処理を点群データに行えばよい。 Next, in step S13, the control device 65 detects a plane from the point cloud data. The control device 65 detects from the point cloud data a plane identical to the plane detected in step S12. If the first inner surface 23 is detected in step S12, the first inner surface 23 is detected from the point cloud data in step S13. If the first inner surface 23 and the first surface 28 are detected in step S12, the first inner surface 23 and the first surface 28 are detected from the point cloud data in step S13. The detection of the plane in step S13 can be achieved by applying to the point cloud data the processing performed on the point cloud map M1 in step S12.

次に、ステップＳ１４において、制御装置６５は、相対距離、及び相対角度を導出する。制御装置６５は、ステップＳ１２で抽出された点Ｐ１によって表される平面と、ステップＳ１３で抽出された点によって表される平面との距離差を相対距離とする。距離差を算出する際には、同一の平面同士の距離差を算出する。例えば、ステップＳ１２、及びステップＳ１３のそれぞれで第１内面２３、及び第１面２８を検出している場合、制御装置６５は、第１内面２３同士で距離差を算出し、第１面２８同士で距離差を算出する。複数の平面を用いて距離差を算出する場合、制御装置６５は、距離差の平均値を算出してもよい。 Next, in step S14, the control device 65 derives the relative distance and the relative angle. The control device 65 determines the relative distance to be the distance difference between the plane represented by point P1 extracted in step S12 and the plane represented by the point extracted in step S13. When calculating the distance difference, the control device 65 calculates the distance difference between the same planes. For example, if the first inner surface 23 and the first surface 28 are detected in step S12 and step S13, respectively, the control device 65 calculates the distance difference between the first inner surfaces 23, and calculates the distance difference between the first surfaces 28. When calculating the distance difference using multiple planes, the control device 65 may calculate the average value of the distance differences.

制御装置６５は、点群マップＭ１から検出された平面に対する点群データから検出された平面の傾きを相対角度とする。傾きを算出する際には、同一の平面同士の傾きを算出する。複数の平面を用いて傾きを算出する場合、制御装置６５は、傾きの平均値を算出してもよい。 The control device 65 determines the inclination of the plane detected from the point cloud data relative to the plane detected from the point cloud map M1 as a relative angle. When calculating the inclination, the control device 65 calculates the inclination between the same planes. When calculating the inclination using multiple planes, the control device 65 may calculate the average value of the inclination.

次に、ステップＳ１５において、制御装置６５は、フォーク４８の位置合わせを行う。ステップＳ１５の処理は、ステップＳ３の処理と同様の処理である。
次に、ステップＳ１６において、制御装置６５は、フォーク４８の差し込みが完了したか否かを判定する。ステップＳ１６の処理は、ステップＳ４と同様の処理である。ステップＳ１６の判定結果が否定の場合、制御装置６５は、ステップＳ１１に戻る。ステップＳ１６の判定結果が肯定の場合、制御装置６５は、位置合わせ制御を終了する。 Next, in step S15, the control device 65 aligns the forks 48. The process of step S15 is similar to the process of step S3.
Next, in step S16, the control device 65 determines whether or not the insertion of the fork 48 is complete. The process of step S16 is the same as that of step S4. If the determination result of step S16 is negative, the control device 65 returns to step S11. If the determination result of step S16 is positive, the control device 65 ends the alignment control.

第２実施形態の効果について記載する。第２実施形態では、第１実施形態の効果（１－１）に加えて、以下の効果を得ることができる。
（２－１）制御装置６５は、点群マップＭ１から検出した平面と点群データから検出した平面とに基づき、相対距離及び相対角度を導出している。点群マップＭ１と点群データとのスキャンマッチングにより相対距離及び相対角度を導出する場合に比べて、制御装置６５の計算コストを低減できる。 The effects of the second embodiment will be described below. In addition to the effect (1-1) of the first embodiment, the second embodiment can provide the following effects.
(2-1) The control device 65 derives the relative distance and the relative angle based on the plane detected from the point cloud map M1 and the plane detected from the point cloud data. This reduces the calculation cost of the control device 65 compared to deriving the relative distance and the relative angle by scan matching between the point cloud map M1 and the point cloud data.

各実施形態は、以下のように変更して実施することができる。各実施形態及び以下の変形例は、技術的に矛盾しない範囲で互いに組み合わせて実施することができる。
○第１実施形態において、制御装置６５が行う位置合わせ制御は、以下のように変更してもよい。以下のように位置合わせ制御を行う場合、外界センサ６１の検出範囲に常にフォーク４８が位置するように外界センサ６１を配置する。 Each embodiment can be modified as follows: Each embodiment and the following modified examples can be combined with each other to the extent that there is no technical contradiction.
In the first embodiment, the positioning control performed by the control device 65 may be modified as follows. When performing the positioning control as described below, the external sensor 61 is disposed so that the fork 48 is always located within the detection range of the external sensor 61.

図１１に示すように、ステップＳ２１において、制御装置６５は、点群データを取得する。ステップＳ２１の処理は、ステップＳ１と同様の処理である。
次に、ステップＳ２２において、制御装置６５は、フォーク４８の位置、及びフォーク４８の姿勢を検出する。フォーク４８の位置とは、センサ座標系でのフォーク４８の座標である。フォーク４８の姿勢とは、センサ座標系のＹ軸に対するフォーク４８の傾きである。制御装置６５は、例えば、点群データからフォーク４８の上面５０を表す点を抽出する。外界センサ６１とフォーク４８との相対位置が一定の場合、点群データから、予め定められた位置の点を抽出すればよい。外界センサ６１とフォーク４８との相対位置が変化する場合、DNN（Deep Neural Network）などのアルゴリズムによって機械学習を行った学習済みモデルを用いて、フォーク４８の上面５０を表す点を抽出すればよい。制御装置６５は、フォーク４８の上面５０からフォーク４８の位置を導出する。制御装置６５は、フォーク４８の上面５０のＹ軸に対する傾きからフォーク４８の姿勢を導出する。 11, in step S21, the control device 65 acquires point cloud data. The process in step S21 is similar to that in step S1.
Next, in step S22, the control device 65 detects the position and attitude of the fork 48. The position of the fork 48 is the coordinates of the fork 48 in the sensor coordinate system. The attitude of the fork 48 is the inclination of the fork 48 with respect to the Y axis of the sensor coordinate system. The control device 65, for example, extracts a point representing the upper surface 50 of the fork 48 from the point cloud data. When the relative position between the external sensor 61 and the fork 48 is constant, a point at a predetermined position may be extracted from the point cloud data. When the relative position between the external sensor 61 and the fork 48 changes, a point representing the upper surface 50 of the fork 48 may be extracted using a trained model that has been machine-learned using an algorithm such as DNN (Deep Neural Network). The control device 65 derives the position of the fork 48 from the upper surface 50 of the fork 48. The control device 65 derives the attitude of the fork 48 from the inclination of the upper surface 50 of the fork 48 with respect to the Y axis.

次に、ステップＳ２３において、制御装置６５は、差込孔３１の位置、及び差込孔３１の姿勢を検出する。差込孔３１の位置とは、センサ座標系での差込孔３１の座標である。差込孔３１の姿勢とは、センサ座標系のＹ軸に対する差込孔３１の傾きである。制御装置６５は、例えば、点群データから画定面を表す点を抽出する。画定面を表す点の抽出は、ステップＳ１３と同様に、平面を検出することによって行うことができる。制御装置６５は、画定面から差込孔３１の位置を導出する。制御装置６５は、画定面のＹ軸に対する傾きから差込孔３１の姿勢を導出する。 Next, in step S23, the control device 65 detects the position and orientation of the insertion hole 31. The position of the insertion hole 31 is the coordinates of the insertion hole 31 in the sensor coordinate system. The orientation of the insertion hole 31 is the inclination of the insertion hole 31 with respect to the Y axis of the sensor coordinate system. The control device 65, for example, extracts points representing the demarcated surface from the point cloud data. The extraction of the points representing the demarcated surface can be performed by detecting a plane, as in step S13. The control device 65 derives the position of the insertion hole 31 from the demarcated surface. The control device 65 derives the orientation of the insertion hole 31 from the inclination of the demarcated surface with respect to the Y axis.

次に、ステップＳ２４において、制御装置６５は、相対距離と相対角度を導出する。制御装置６５は、フォーク４８の位置と差込孔３１の位置との差を相対距離とする。制御装置６５は、フォーク４８の姿勢と差込孔３１の姿勢との差を相対角度とする。 Next, in step S24, the control device 65 derives the relative distance and the relative angle. The control device 65 determines the difference between the position of the fork 48 and the position of the insertion hole 31 as the relative distance. The control device 65 determines the difference between the posture of the fork 48 and the posture of the insertion hole 31 as the relative angle.

次に、ステップＳ２５において、制御装置６５は、フォーク４８の位置合わせを行う。ステップＳ２５の処理は、ステップＳ３の処理と同様の処理である。
次に、ステップＳ２６において、制御装置６５は、フォーク４８の差し込みが完了したか否かを判定する。ステップＳ２６の処理は、ステップＳ４と同様の処理である。ステップＳ２６の判定結果が否定の場合、制御装置６５は、ステップＳ２１に戻る。ステップＳ２６の判定結果が肯定の場合、制御装置６５は、位置合わせ制御を終了する。 Next, in step S25, the control device 65 aligns the forks 48. The process of step S25 is similar to the process of step S3.
Next, in step S26, the control device 65 judges whether or not the insertion of the fork 48 is complete. The process of step S26 is the same as that of step S4. If the judgment result of step S26 is negative, the control device 65 returns to step S21. If the judgment result of step S26 is positive, the control device 65 ends the alignment control.

上記のように位置合わせ制御を行う場合、点群マップＭ１を用いることなく相対距離、及び相対角度の導出が可能である。従って、補助記憶装置６８は、点群マップＭ１を記憶していなくてもよい。 When performing alignment control as described above, it is possible to derive the relative distance and relative angle without using the point cloud map M1. Therefore, the auxiliary storage device 68 does not need to store the point cloud map M1.

○第１実施形態において、制御装置６５は、点群マップＭ１と点群データとの一致性が低い場合には、荷役を中止してもよい。点群マップＭ１の点Ｐ１と点群データの点とのマッチング数が多いほど、点群マップＭ１と点群データとの一致性は高い。制御装置６５は、点群マップＭ１の点Ｐ１と点群データの点とのマッチング数によって荷役を中止するようにしてもよい。点群マップＭ１と点群データとの一致性が低い場合、フォーク４８に積載する目標となるパレット２０とは異なるパレットにフォークリフト４０が近付いている、あるいは、パレット２０が壊れていると予想される。このような場合に、荷役を中止することで、荷役が行えないにも関わらず、フォークリフト４０が荷役を行おうとすることを抑制することができる。 In the first embodiment, the control device 65 may halt the loading operation if the correspondence between the point cloud map M1 and the point cloud data is low. The greater the number of matches between the point P1 of the point cloud map M1 and the points of the point cloud data, the higher the correspondence between the point cloud map M1 and the point cloud data. The control device 65 may halt the loading operation depending on the number of matches between the point P1 of the point cloud map M1 and the points of the point cloud data. If the correspondence between the point cloud map M1 and the point cloud data is low, it is expected that the forklift 40 is approaching a pallet other than the pallet 20 to be loaded onto the fork 48, or that the pallet 20 is broken. In such a case, by halting the loading operation, it is possible to prevent the forklift 40 from attempting to load even though loading is not possible.

○各実施形態において、制御装置６５は、Ｚ軸の延びる方向に対するフォーク４８の位置合わせのみを行ってもよい。この場合、制御装置６５は、相対距離として、Ｚ軸の延びる方向に対する相対距離のみを導出するようにしてもよい。 In each embodiment, the control device 65 may only align the fork 48 with respect to the direction in which the Z axis extends. In this case, the control device 65 may derive only the relative distance with respect to the direction in which the Z axis extends as the relative distance.

○各実施形態において、制御装置６５は、Ｘ軸の延びる方向に対するフォーク４８の位置合わせのみを行ってもよい。この場合、制御装置６５は、相対距離として、Ｘ軸の延びる方向に対する相対距離のみを導出するようにしてもよい。 In each embodiment, the control device 65 may only align the fork 48 with respect to the direction in which the X-axis extends. In this case, the control device 65 may derive only the relative distance with respect to the direction in which the X-axis extends as the relative distance.

○各実施形態において、点群マップＭ１は、記憶部６７に記憶されていてもよい。この場合、記憶部６７が、記憶装置である。
○各実施形態において、点群マップＭ１は、フォーク４８とパレット２０とが接触しない状態での画定面の位置を、センサ座標系の座標で表した点の集合であればよい。点群マップＭ１は、相対距離が０より大きく、かつ、相対角度が０より大きい状態で取得された座標であってもよい。 In each embodiment, the point cloud map M1 may be stored in the storage unit 67. In this case, the storage unit 67 is a storage device.
In each embodiment, the point cloud map M1 may be a set of points that represent the positions of the demarcation plane in the coordinates of the sensor coordinate system when the forks 48 are not in contact with the pallet 20. The point cloud map M1 may be coordinates acquired when the relative distance is greater than 0 and the relative angle is greater than 0.

○各実施形態において、フォークリフト４０は、任意の位置に置かれたパレット２０をフォーク４８に積載することができる。任意の位置としては、例えば、棚及び地面を挙げることができる。 In each embodiment, the forklift 40 can load the pallet 20 placed in any position onto the forks 48. Examples of any position include a shelf and the ground.

○各実施形態において、フォークリフト４０は、操作者による操作によって動作するフォークリフトであってもよい。この場合、操作者の操作によってフォークリフト４０がパレット２０に近付いている場合に、制御装置６５は、フォーク４８の位置合わせを行えばよい。 In each embodiment, the forklift 40 may be a forklift that is operated by an operator. In this case, when the forklift 40 is approaching the pallet 20 by the operator, the control device 65 may align the forks 48.

Ｍ１…点群マップ、Ｐ１…点、２０…パレット、２３…画定面としての第１内面、２６…画定面としての第２内面、２８…画定面としての第１面、３０…画定面としての第２面、３１…差込孔、４０…フォークリフト、４８…フォーク、６１…外界センサ、６５…制御装置、６８…記憶装置としての補助記憶装置。 M1...point cloud map, P1...point, 20...pallet, 23...first inner surface as demarcating surface, 26...second inner surface as demarcating surface, 28...first surface as demarcating surface, 30...second surface as demarcating surface, 31...insertion hole, 40...forklift, 48...fork, 61...external sensor, 65...control device, 68...auxiliary memory device as memory device.

Claims

Fork and
An external sensor that detects the position of an object in a three-dimensional coordinate system;
A control device,
The control device acquires point cloud data, which is a set of points representing the positions of a demarcation surface that defines an insertion hole of a pallet and includes a surface extending in a horizontal direction and a surface extending in a vertical direction , from the external sensor;
Deriving from the point cloud data a relative distance between the fork and the insertion hole in a direction perpendicular to the direction in which the fork and the pallet face each other, and a relative angle between the fork and the insertion hole with respect to the direction in which the fork and the pallet face each other;
A forklift that adjusts positions of the forks based on the relative distance and the relative angle so that the forks do not come into contact with the pallet while inserting the forks into the insertion holes.

The forklift is
A storage device is provided,
the storage device stores a point cloud map that is a set of points that represent the positions of the demarcation plane when the forks and the pallet are not in contact with each other, using coordinates in the three-dimensional coordinate system;
The forklift according to claim 1 , wherein the control device derives the relative distance from a distance difference of the point cloud data with respect to the point cloud map, and derives the relative angle from an inclination of the point cloud data with respect to the point cloud map.

The forklift is
A storage device is provided,
the storage device stores a point cloud map that is a set of points that represent the positions of the demarcation plane when the forks and the pallet are not in contact with each other, using coordinates in the three-dimensional coordinate system;
The control device includes:
Detecting a plane from the point cloud map;
Detecting a plane identical to the plane from the point cloud data;
2. The forklift according to claim 1, wherein the relative distance is derived from a distance difference between the plane detected from the point cloud map and the plane detected from the point cloud data, and the relative angle is derived from an inclination of the plane detected from the point cloud data with respect to the plane detected from the point cloud map.