JP2023048442A

JP2023048442A - forklift

Info

Publication number: JP2023048442A
Application number: JP2021157757A
Authority: JP
Inventors: 匡弘岸田; Masahiro Kishida
Original assignee: Toyota Industries Corp
Current assignee: Toyota Industries Corp
Priority date: 2021-09-28
Filing date: 2021-09-28
Publication date: 2023-04-07

Abstract

To provide a forklift capable of suppressing the contact of a fork with a palette until a fork is completed to be inserted into a palette.SOLUTION: A forklift comprises a fork, an external sensor, and a control device. The external sensor detects an object position with the coordinates in a three-dimensional coordinates system. The control device acquires point group data from the external sensor. The point group data is a set of the points indicating a defined surface to define an insertion hole of a palette. The control device derives a relative distance between the fork and the insertion hole in the direction perpendicular to the direction that the fork and the palette face each other, and a relative angle between the fork and the insertion hole with respect to the direction that the fork and the palette face each other, from the point group data. The control device adjusts the fork position so that the fork does not contact with the palette during inserting the fork into the insertion hole based on the relative distance and the relative angle.SELECTED DRAWING: Figure 9

Description

本開示は、フォークリフトに関する。 The present disclosure relates to forklifts.

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

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

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

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

点群データは、画定面の位置を表した点の集合である。フォークを差込孔に差し込む過程でフォークが接触するおそれがあるのは、画定面である。点群データからフォークと差込孔との相対距離、及びフォークと差込孔との相対角度を導出することで、フォークの差し込み量に応じた相対距離、及び相対角度を導出することができる。そして、この相対距離、及び相対角度に基づきフォークの位置を調整することで、差込孔へのフォークの差し込み中にフォークの位置を調整することができる。フォークの差し込み中であっても、フォークとパレットとが接触しないようにフォークの位置を調整できるため、フォークの差し込みが完了するまでにフォークとパレットとが接触することを抑制できる。 Point cloud data is a set of points representing the position of the defining plane. It is the defining surface that the fork may come into contact with during the process of 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 the relative distance and the relative angle according to the insertion amount of the fork. By adjusting the position of the fork based on this relative distance and relative angle, the position of the fork can be adjusted while the fork is being inserted into the insertion hole. Since the positions of the forks can be adjusted so that the forks and the pallet do not contact each other even during insertion of the forks, contact between the forks and the pallet can be suppressed until the insertion of the forks is completed.

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

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

本発明によれば、フォークの差し込みが完了するまでにフォークとパレットとが接触することを抑制できる。 ADVANTAGE OF THE INVENTION According to this invention, it can suppress that a fork and a pallet contact by the time insertion of a fork is completed.

トラック、及びフォークリフトを模式的に示す図である。It is a figure which shows a truck and a forklift typically. 図１のトラックが備える荷台に積載されたパレットの正面図である。FIG. 2 is a front view of a pallet loaded on a loading platform of the truck of FIG. 1; 図２のパレットの斜視図である。Figure 3 is a perspective view of the pallet of Figure 2; 図２のパレットの断面図である。Figure 3 is a cross-sectional view of the pallet of Figure 2; 図１の区域で運用されているフォークリフトの側面図である。Figure 2 is a side view of a forklift operating in the area of Figure 1; 図５のフォークリフトが備える荷役装置の斜視図である。FIG. 6 is a perspective view of a cargo handling device included in the forklift of FIG. 5 ; 図５のフォークリフトの概略構成図である。6 is a schematic configuration diagram of the forklift of 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; FIG. 第１実施形態の位置合わせ制御の処理手順を示すフローチャートである。5 is a flow chart showing a processing procedure of alignment control according to the first embodiment; 第２実施形態の位置合わせ制御の処理手順を示すフローチャートである。10 is a flow chart showing a processing procedure of alignment control according to the second embodiment; 変形例の位置合わせ制御の処理手順を示すフローチャートである。11 is a flow chart showing a processing procedure of alignment control according to a modification;

（第１実施形態）
フォークリフトの第１実施形態について説明する。
図１に示すように、区域Ａ１には、トラック１０が停車される。トラック１０は、荷台１１を備える。トラック１０の荷台１１には、パレット２０が積載されている。区域Ａ１は、例えば、工場、港湾、空港、商業施設及び公共施設などの場所の全体、あるいは、一部である。区域Ａ１では、フォークリフト４０が運用されている。フォークリフト４０は、トラック１０に積載されたパレット２０の荷取りを行う。フォークリフト４０は、荷取りを行った後に、別の位置又は別の装置までパレット２０の搬送を行う。 (First embodiment)
A first embodiment of a 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 place such as a factory, a port, an airport, a commercial facility, or a public facility. A forklift 40 is operated in the area A1. The forklift 40 unloads the pallet 20 loaded on the truck 10 . After unloading, the forklift 40 carries the pallet 20 to another position or another device.

図２に示すように、パレット２０は平パレットである。パレット２０は、第１壁部２１と、第２壁部２４と、桁２７，２９と、を備える。
図３に示すように、第１壁部２１及び第２壁部２４は、板状である。第１壁部２１及び第２壁部２４は、四角形状である。第１壁部２１と第２壁部２４とは互いに向かい合っている。第１壁部２１は、第１外面２２と、第１内面２３と、を備える。第１外面２２と第１内面２３とは第１壁部２１の厚み方向に互いに反対となる面である。第２壁部２４は、第２外面２５と、第２内面２６と、を備える。第２外面２５と第２内面２６とは第２壁部２４の厚み方向に互いに反対となる面である。第１壁部２１の第１内面２３と第２壁部２４の第２内面２６とは互いに向かい合っている。第１外面２２、第１内面２３、第２外面２５及び第２内面２６は、互いに平行である。 As shown in FIG. 2, pallet 20 is a flat pallet. The pallet 20 includes a first wall portion 21 , a second wall portion 24 and girders 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 has a first outer surface 22 and a first inner surface 23 . The first outer surface 22 and the first inner surface 23 are surfaces opposite to each other in the thickness direction of the first wall portion 21 . The second wall portion 24 has a second outer surface 25 and a second inner surface 26 . The second outer surface 25 and the second inner surface 26 are surfaces 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 and 29 are provided between the first wall portion 21 and the second wall portion 24 . Digits 27 and 29 include first digit 27 and second digit 29 . Four first digits 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. The second girders 29 are provided one by one between the first girders 27 . The first girder 27 has a first face 28 . The first surface 28 is a surface facing the second girder 29 . The first girder 27 has two first faces 28 . The second girder 29 has a second face 30 . The second surface 30 is a surface facing the first girder 27 . The second girder 29 has two second faces 30 . The first surface 28 and the second surface 30 face each other.

第１内面２３、第２内面２６、第１面２８、及び第２面３０に囲まれる領域は、差込孔３１である。第１内面２３、第２内面２６、第１面２８、及び第２面３０は、差込孔３１を画定している画定面である。 A region surrounded by the first inner surface 23 , the second inner surface 26 , the first surface 28 and the second surface 30 is an insertion hole 31 . The first inner surface 23 , the second inner surface 26 , the first surface 28 , and the second surface 30 are 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 . A 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. A region 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 through the first wall portion 21 .

第２壁部２４は、第２貫通孔画定面３４を備える。第２貫通孔画定面３４は、第２外面２５と第２内面２６との間で延びている。第２貫通孔画定面３４は、環状の面である。第２貫通孔画定面３４に囲まれる領域は、第２貫通孔３５である。第２貫通孔画定面３４は、第２貫通孔３５を画定している。第２貫通孔３５は、第２壁部２４を貫通している。 The second wall portion 24 includes a second through hole defining surface 34 . A 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. A region surrounded by the second through-hole defining surface 34 is the second through-hole 35 . The second through hole defining surface 34 defines a second through hole 35 . The second through hole 35 penetrates through the second wall portion 24 .

図５に示すように、フォークリフト４０は、駆動輪４１と、操舵輪４２と、荷役装置４３と、外界センサ６１と、を備える。荷役装置４３は、マスト４４と、リフトブラケット４７と、２つのフォーク４８と、リフトシリンダ５１と、ティルトシリンダ５２と、を備える。 As shown in FIG. 5 , the forklift 40 includes drive wheels 41 , steering wheels 42 , a cargo handling device 43 , and an external sensor 61 . The cargo handling 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 has an outer mast 45 and an inner mast 46 . The inner mast 46 is provided so as to be able to move up and down with respect to the outer mast 45 .
A lift bracket 47 is attached to the inner mast 46 . Fork 48 is attached to lift bracket 47 . The lift bracket 47 and the fork 48 can move up and down together with the inner mast 46 .

フォーク４８は、差込部４９を備える。差込部４９は、リフトブラケット４７からフォークリフト４０の前方に延びている。差込部４９は、フォーク４８にパレット２０を積載する際に、差込孔３１に差し込まれる部分である。差込部４９は、上面５０を備える。 The fork 48 has an insert 49 . The insertion portion 49 extends forward of the forklift 40 from the lift bracket 47 . The insertion portion 49 is a portion that is inserted into the insertion hole 31 when the pallet 20 is loaded on the forks 48 . The insert 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 longitudinal direction 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 two forks 48 in the vehicle width direction of the forklift 40 . The external sensor 61 is provided so as to move up and down together with the fork 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 using coordinates in a three-dimensional coordinate system. The external sensor 61 is provided to obtain 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 detects coordinates of at least one 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 . is provided so that the As the external sensor 61, for example, a millimeter wave radar, a ToF (Time of Flight) camera, and a LIDAR (Laser Imaging Detection and Ranging) can be used. When there is no object in the depth, such as the insertion hole 31 of the pallet 20, the stereo camera may lower the coordinate detection accuracy. Therefore, as the external sensor 61, it is preferable to use a sensor that utilizes reflection, such as a reflected laser beam or a reflected radio wave.

本実施形態では、外界センサ６１としてLIDARを用いている。外界センサ６１は、周囲にレーザーを照射し、レーザーが当たった点から反射された反射光を受光することで点までの距離を導出する。レーザーが当たった点は、物体の表面の一部を表す。点の位置は、極座標系の座標で表すことができる。極座標系における点の座標は、直交座標系の座標に変換される。外界センサ６１は、センサ座標系での点の座標を導出する。センサ座標系は、外界センサ６１を原点とする３次元座標系である。センサ座標系は、３軸直交座標系であって、Ｘ軸と、Ｙ軸と、Ｚ軸と、を備える。Ｘ軸の延びる方向とフォークリフト４０の車幅方向とは一致している。荷役装置４３が傾動していない状態でＹ軸の延びる方向とフォークリフト４０の前後方向とは一致している。荷役装置４３が傾動していない状態でＺ軸の延びる方向と鉛直方向とは一致している。差込部４９を差込孔３１に差し込む際には、フォークリフト４０の前方にパレット２０が位置した状態でフォークリフト４０が前進する。Ｙ軸の延びる方向は、フォーク４８とパレット２０とが向かい合う方向である。図１、図５、及び図６には、センサ座標系のＸ軸を矢印Ｘ、Ｙ軸を矢印Ｙ、Ｚ軸を矢印Ｚで示している。 In this embodiment, LIDAR is used as the external sensor 61 . The external sensor 61 irradiates the surroundings with a laser beam and receives reflected light reflected from the point hit by the laser beam, thereby deriving the distance to the point. The point hit by the laser represents a portion of the object's surface. The position of a point can be represented by coordinates in a polar coordinate system. The coordinates of the point in the polar coordinate system are transformed into coordinates in the rectangular coordinate system. The external sensor 61 derives the coordinates of points 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 orthogonal 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 . The direction in which the Y-axis extends coincides with the front-rear direction of the forklift 40 when the cargo handling device 43 is not tilted. The direction in which the Z-axis extends and the vertical direction match when the cargo handling device 43 is not tilted. When inserting the insertion part 49 into the insertion hole 31, the forklift 40 moves forward with the pallet 20 positioned in front of the forklift 40. - 特許庁The direction in which the Y-axis extends is the direction in which the fork 48 and the pallet 20 face each other. In FIGS. 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.

図７に示すように、フォークリフト４０は、積載センサ６２と、駆動機構６３と、油圧機構６４と、制御装置６５と、補助記憶装置６８と、を備える。
積載センサ６２は、フォーク４８にパレット２０が積載されていることを検出するためのセンサである。積載センサ６２は、例えば、差込部４９の基端に設けられる。フォークリフト４０がパレット２０の荷取りを行う際には、差込部４９が差込孔３１に差し込まれていく。差込部４９が差込孔３１に差し込まれていくと、パレット２０が差込部４９の基端に近付いていく。これにより、差込部４９が差込孔３１に所定量以上差し込まれると、積載センサ６２によってフォーク４８にパレット２０が積載されていることが検出される。所定量とは、差込部４９の先端から積載センサ６２までの距離である。 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 .
A loading sensor 62 is a sensor for detecting that the pallet 20 is loaded on the fork 48 . The load sensor 62 is provided, for example, at the proximal 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 proximal end of the insertion portion 49 . Accordingly, 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 fork 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 supply and discharge of hydraulic oil to the lift cylinder 51 and the tilt cylinder 52, and includes a cargo handling motor for driving the pump and a control valve. The inner mast 46 is raised and lowered by supplying and discharging hydraulic oil to the lift cylinder 51 . The supply and discharge of hydraulic oil to the tilt cylinder 52 causes the mast 44 to tilt in the front-rear direction of the forklift 40 . The lifting and lowering of the inner mast 46 can also be said to be the lifting and lowering of the fork 48 . The tilting of the mast 44 can also be said to be the tilting of the fork 48 .

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

制御装置６５は、駆動機構６３及び油圧機構６４を制御する。フォークリフト４０は、制御装置６５による制御によって走行、及び荷役が行われる自動運転フォークリフトである。 A 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 readable by the control device 65 . Auxiliary storage device 68 may include, for example, a hard disk drive and a solid state drive.

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

点群マップＭ１は、第１内面２３、第２内面２６、第１面２８、及び第２面３０の位置をセンサ座標系の座標で表したデータである。点群マップＭ１は、フォーク４８と差込孔３１との相対距離が０、かつ、フォーク４８と差込孔３１との相対角度が０の状態で取得されたセンサ座標系の点Ｐ１の座標である。フォーク４８と差込孔３１との相対距離が０、かつ、フォーク４８と差込孔３１との相対角度が０の状態とは、フォークリフト４０とパレット２０とが向かい合っている状態でフォークリフト４０を前進させた場合に、フォーク４８とパレット２０とが接触することなくフォーク４８が差込孔３１に差し込まれる状態である。以下の説明において、フォーク４８と差込孔３１との相対距離を相対距離、フォーク４８と差込孔３１との相対角度を相対角度と称する。 The point cloud map M1 is data representing 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 obtained when 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. be. The state in which 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 the forklift 40 moves forward while the forklift 40 and the pallet 20 face each other. The fork 48 is inserted into the insertion hole 31 without the fork 48 and the pallet 20 coming into contact with each other. In the following description, the relative distance between the fork 48 and the insertion hole 31 will be referred to as the relative distance, and the relative angle between the fork 48 and the insertion hole 31 will be referred to as the relative angle.

相対距離とは、センサ座標系のＸＺ平面でのフォーク４８と差込孔３１との位置のずれ量である。ＸＺ平面とは、センサ座標系のＸ軸とセンサ座標系のＺ軸によって規定される平面である。ＸＺ平面でのフォーク４８の先端の中心位置と、ＸＺ平面での差込孔３１の中心位置とが一致する場合、相対距離は０である。相対距離が０の場合、ＸＺ平面においてフォーク４８と画定面との間には、クリアランスが存在している。ＸＺ平面は、フォーク４８とパレット２０とが向かい合う方向に直交する方向に拡がる平面である。相対距離は、フォーク４８とパレット２０とが向かい合う方向に直交する方向におけるフォーク４８と差込孔３１との相対距離といえる。 The relative distance is the amount of positional deviation 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. The relative distance is zero when the center position of the tip of the fork 48 on the XZ plane and the center position of the insertion hole 31 on the XZ plane match. When the relative distance is 0, there is a clearance between the fork 48 and the defining plane in the XZ plane. The XZ plane is a plane extending 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 amount of 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 upper 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 should include at least a point P1 representing the position of the defining plane in coordinates of the sensor coordinate system. The point cloud map M1 may include points P1 representing the positions of the through-hole defining planes 32 and 34 in coordinates of the sensor coordinate system.

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

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

次に、ステップＳ２において、制御装置６５は、相対距離、及び相対角度を導出する。制御装置６５は、ステップＳ１で取得した点群データと、点群マップＭ１とのスキャンマッチングを行う。スキャンマッチングは、例えば、NDT（Normal Distributions Transform）又はICP(Iterative Closest Point)を用いて行うことができる。 Next, in step S2, the controller 65 derives the relative distance and 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 that the distance between the point of the point cloud data and the point P1 of the point cloud map M1 corresponding to the point of the point cloud data is shortened. . The point P1 of the point cloud map M1 corresponding to the point of the point cloud data is the point P1 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 the point of the point cloud data is the point P1 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 the point cloud data should be moved and how it should be rotated to match the point cloud map M1.

制御装置６５は、点群マップＭ１に対する点群データの距離差を導出できる。点群マップＭ１に対する点群データの距離差は、点群マップＭ１で表される画定面の位置と、点群データで表される画定面の位置との差である。点群マップＭ１は、相対距離が０の状態で取得されている。このため、点群マップＭ１と点群データとの距離差がない場合には、相対距離が０の状態といえる。ＸＺ平面でのフォーク４８と差込孔３１との位置にずれが生じている場合、このずれの分だけ、点群データは点群マップＭ１からずれる。点群マップＭ１と点群データとの距離差は、ＸＺ平面でのフォーク４８と差込孔３１との位置のずれ量を表しているといえる。 The controller 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 defining plane represented by the point cloud map M1 and the position of the defining plane represented by the point cloud data. The point cloud map M1 is obtained with a relative distance of zero. 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 zero. When the positions of the fork 48 and the insertion hole 31 on the XZ plane are deviated, the point cloud data is deviated from the point cloud map M1 by 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 positional deviation between the fork 48 and the insertion hole 31 on the XZ plane.

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

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

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

制御装置６５は、Ｘ軸の延びる方向に対する相対距離が規定範囲に収まっていない場合、フォーク４８を左右方向に移動させる。制御装置６５は、駆動機構６３を制御して操舵を行うことによってフォーク４８を左右方向に移動させてもよい。フォークリフト４０がサイドシフト装置を備えている場合、制御装置６５は、サイドシフト装置によってフォーク４８を左右方向に移動させてもよい。サイドシフト装置は、サイドシフトシリンダを備え、サイドシフトシリンダへの作動油の給排によってフォーク４８を左右方向に移動させる装置である。制御装置６５は、相対距離が短くなるようにフォーク４８を左右方向に移動させる。制御装置６５は、Ｘ軸の延びる方向に対する相対距離が規定範囲内になると、フォーク４８を停止させる。 The control device 65 moves the fork 48 in the left-right direction when the relative distance with respect to the direction in which the X-axis extends does not fall within the specified range. The control device 65 may move the fork 48 in the left-right direction by controlling the drive mechanism 63 to perform steering. If the forklift 40 has 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 a device that includes a side shift cylinder and moves the fork 48 in the left-right direction by supplying and discharging working oil to the side shift cylinder. The control device 65 moves the fork 48 in the left-right direction so that the relative distance is shortened. The control device 65 stops the fork 48 when the relative distance with respect to 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 the specified angle range. The specified angle range is a range set so that the fork 48 and the pallet 20 do not contact each other while the fork 48 is being inserted into the insertion hole 31 . The control device 65 tilts the fork 48 by controlling the hydraulic mechanism 64 when the relative angle does not fall within the specified angle range. The controller 65 tilts the fork 48 so that the relative angle becomes smaller. The control device 65 stops the fork 48 when the relative angle falls within the specified angle range. It can be said that the fork 48 is aligned so that the point cloud data overlaps the point cloud map M1.

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

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

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

第１実施形態の効果について説明する。
（１－１）制御装置６５は、フォーク４８の差し込み中にフォーク４８の位置合わせを行っている。フォーク４８の差し込み中であっても、フォーク４８とパレット２０とが接触しないようにフォーク４８の位置を調整できるため、フォーク４８の差し込みが完了するまでにフォーク４８とパレット２０とが接触することを抑制できる。 Effects of the first embodiment will be described.
(1-1) The controller 65 aligns the fork 48 while the fork 48 is being inserted. Since the position of the fork 48 can be adjusted so that the fork 48 and the pallet 20 do not contact each other even during insertion of the fork 48, contact between the fork 48 and the pallet 20 is prevented until the insertion of the fork 48 is completed. can be suppressed.

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

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

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

図１０に示すように、ステップＳ１１において、制御装置６５は、点群データを取得する。ステップＳ１１の処理は、ステップＳ１の処理と同様の処理である。
次に、ステップＳ１２において、制御装置６５は、点群マップＭ１から平面を検出する。詳細にいえば、制御装置６５は、点群マップＭ１から第１内面２３、第２内面２６、第１面２８、及び第２面３０のうち少なくともいずれかの平面を検出する。制御装置６５は、点群マップＭ１の各点Ｐ１の法線ベクトルを算出する。法線ベクトルは、複数の点Ｐ１に囲まれる平面に対して垂直な方向に向けたベクトルである。法線ベクトルを導出する手法としては各点Ｐ１から曲面を求め、曲面から各点Ｐ１の法線ベクトルを導出する手法や、ベクトルの外積を用いる手法を挙げることができる。例えば、制御装置６５が、１つの点Ｐ１の法線ベクトルを求める場合、この点Ｐ１から所定範囲内に位置する２つの点Ｐ１のそれぞれに向かうベクトルの外積を求める。この外積が法線ベクトルである。 As shown in FIG. 10, in step S11, the control device 65 acquires point cloud data. The process of step S11 is the same as the process of step S1.
Next, in step S12, the control device 65 detects a plane from the point cloud map M1. 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 on the point cloud map M1. A normal vector is a vector directed in a direction perpendicular to a plane surrounded by a plurality of points P1. As a method of deriving the normal vector, a method of obtaining a curved surface from each point P1 and deriving a normal vector of each point P1 from the curved surface, and a method of using an outer product of vectors can be mentioned. For example, when the control device 65 obtains the normal vector of one point P1, it obtains the cross product of vectors directed to each of two points P1 located within a predetermined range from this point P1. This outer 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 the point P1 whose normal vector is oriented in the vertical direction. Specifically, the control device 65 determines whether or not the angle of the normal vector to the XY plane of the sensor coordinate system falls within a predetermined range. The XY plane is a plane defined by the X-axis and Y-axis of the sensor coordinate system. If the point P1 represents 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 depending on the stop position of the truck 10, the accuracy of the external sensor 61, and the like. The range is, for example, 90°±predetermined angle. The predetermined angle is, for example, 10 degrees. The control device 65 extracts a point P1 whose normal vector angle with respect to the XY plane falls within a predetermined range as a point P1 whose normal vector direction is in the vertical direction. This point P1 is the point P1 representing 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 detects the point P1 where the angle of the normal vector with respect to the YZ plane of the sensor coordinate system falls within a predetermined range. should be extracted. The point P1 extracted by this is a point representing the first surface 28 or the second surface 30 . The YZ plane is a plane defined by the Y-axis and 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 the same plane as the plane detected in step S12 from the point cloud data. 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 have been detected in step S12, the first inner surface 23 and the first surface 28 are detected from the point cloud data in step S13. For the plane detection in step S13, the processing performed on the point cloud map M1 in step S12 may be performed on the point cloud data.

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

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

次に、ステップＳ１５において、制御装置６５は、フォーク４８の位置合わせを行う。ステップＳ１５の処理は、ステップＳ３の処理と同様の処理である。
次に、ステップＳ１６において、制御装置６５は、フォーク４８の差し込みが完了したか否かを判定する。ステップＳ１６の処理は、ステップＳ４と同様の処理である。ステップＳ１６の判定結果が否定の場合、制御装置６５は、ステップＳ１１に戻る。ステップＳ１６の判定結果が肯定の場合、制御装置６５は、位置合わせ制御を終了する。 Next, in step S15, the control device 65 aligns the forks 48. As shown in FIG. The process of step S15 is the same as the process of step S3.
Next, in step S16, the control device 65 determines whether or not the insertion of the fork 48 has been completed. The processing 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 in step S16 is affirmative, the control device 65 terminates the alignment control.

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

各実施形態は、以下のように変更して実施することができる。各実施形態及び以下の変形例は、技術的に矛盾しない範囲で互いに組み合わせて実施することができる。
○第１実施形態において、制御装置６５が行う位置合わせ制御は、以下のように変更してもよい。以下のように位置合わせ制御を行う場合、外界センサ６１の検出範囲に常にフォーク４８が位置するように外界センサ６１を配置する。 Each embodiment can be modified and implemented as follows. Each embodiment and the following modifications can be implemented in combination with each other within a technically consistent range.
(circle) in 1st Embodiment, you may change the alignment control which the control apparatus 65 performs as follows. When the positioning control is performed as follows, the external sensor 61 is arranged so that the fork 48 is always positioned within the detection range of the external sensor 61 .

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

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

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

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

○第１実施形態において、制御装置６５は、点群マップＭ１と点群データとの一致性が低い場合には、荷役を中止してもよい。点群マップＭ１の点Ｐ１と点群データの点とのマッチング数が多いほど、点群マップＭ１と点群データとの一致性は高い。制御装置６５は、点群マップＭ１の点Ｐ１と点群データの点とのマッチング数によって荷役を中止するようにしてもよい。点群マップＭ１と点群データとの一致性が低い場合、フォーク４８に積載する目標となるパレット２０とは異なるパレットにフォークリフト４０が近付いている、あるいは、パレット２０が壊れていると予想される。このような場合に、荷役を中止することで、荷役が行えないにも関わらず、フォークリフト４０が荷役を行おうとすることを抑制することができる。 (circle) in 1st Embodiment, the control apparatus 65 may stop cargo handling, when the match|match between the point cloud map M1 and 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 matching between the point cloud map M1 and the point cloud data. The control device 65 may stop cargo handling depending on the number of matches between the point P1 on the point cloud map M1 and the points on the point cloud data. If the point cloud map M1 and the point cloud data are less consistent, it is expected that the forklift 40 is approaching a different pallet than the target pallet 20 to be loaded on the fork 48, or that the pallet 20 is broken. . In such a case, by stopping the cargo handling, it is possible to suppress the forklift 40 from trying to carry out the cargo handling even though the cargo handling cannot be performed.

○各実施形態において、制御装置６５は、Ｚ軸の延びる方向に対するフォーク４８の位置合わせのみを行ってもよい。この場合、制御装置６５は、相対距離として、Ｚ軸の延びる方向に対する相対距離のみを導出するようにしてもよい。 o 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 extending direction of the Z-axis as the relative distance.

○各実施形態において、制御装置６５は、Ｘ軸の延びる方向に対するフォーク４８の位置合わせのみを行ってもよい。この場合、制御装置６５は、相対距離として、Ｘ軸の延びる方向に対する相対距離のみを導出するようにしてもよい。 o 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.

○各実施形態において、点群マップＭ１は、記憶部６７に記憶されていてもよい。この場合、記憶部６７が、記憶装置である。
○各実施形態において、点群マップＭ１は、フォーク４８とパレット２０とが接触しない状態での画定面の位置を、センサ座標系の座標で表した点の集合であればよい。点群マップＭ１は、相対距離が０より大きく、かつ、相対角度が０より大きい状態で取得された座標であってもよい。 (circle) in each embodiment, the point cloud map M1 may be memorize|stored in the memory|storage part 67. FIG. 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 representing the position of the defining plane when the fork 48 and the pallet 20 are not in contact with each other in the coordinates of the sensor coordinate system. The point cloud map M1 may be coordinates acquired with a relative distance greater than zero and a relative angle greater than zero.

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

○各実施形態において、フォークリフト４０は、操作者による操作によって動作するフォークリフトであってもよい。この場合、操作者の操作によってフォークリフト４０がパレット２０に近付いている場合に、制御装置６５は、フォーク４８の位置合わせを行えばよい。 O 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 operation of the operator, the control device 65 may align the forks 48 .

Ｍ１…点群マップ、Ｐ１…点、２０…パレット、２３…画定面としての第１内面、２６…画定面としての第２内面、２８…画定面としての第１面、３０…画定面としての第２面、３１…差込孔、４０…フォークリフト、４８…フォーク、６１…外界センサ、６５…制御装置、６８…記憶装置としての補助記憶装置。 M1... Point cloud map, P1... Points, 20... Palette, 23... First inner surface as defining surface, 26... Second inner surface as defining surface, 28... First surface as defining surface, 30... As defining surface Second surface 31: Insertion hole 40: Forklift 48: Fork 61: External sensor 65: Control device 68: Auxiliary storage device as a storage device.

Claims

a fork;
an external sensor that detects the position of an object with coordinates of a three-dimensional coordinate system;
a controller;
The control device acquires point cloud data, which is a set of points representing the positions of the defining planes defining the insertion holes of the pallet, from the external sensor,
The relative distance between the fork and the insertion hole in the 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 in the direction in which the fork and the pallet face each other are defined as derived from the point cloud data,
A forklift that adjusts the position of the forks based on the relative distance and the relative angle so that the forks and the pallet do not contact each other while the forks are being inserted into the insertion holes.

The forklift is
with storage,
The storage device stores a point cloud map, which is a set of points representing the position of the defining plane in a state in which the fork and the pallet are not in contact with each other in coordinates of the three-dimensional coordinate system,
2. The control device 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. forklift.

The forklift is
with storage,
The storage device stores a point cloud map, which is a set of points representing the position of the defining plane in a state in which the fork and the pallet are not in contact with each other in coordinates of the three-dimensional coordinate system,
The control device is
detecting a plane from the point cloud map;
detecting a plane identical to the plane from the point cloud data;
Deriving the relative distance from a distance difference between the plane detected from the point cloud map and the plane detected from the point cloud data, and detecting the plane detected from the point cloud map from the point cloud data 2. A forklift truck according to claim 1, wherein said relative angle is derived from the inclination of said plane.