CN110040669B - High-position goods-forking auxiliary method - Google Patents

High-position goods-forking auxiliary method Download PDF

Info

Publication number
CN110040669B
CN110040669B CN201810044528.7A CN201810044528A CN110040669B CN 110040669 B CN110040669 B CN 110040669B CN 201810044528 A CN201810044528 A CN 201810044528A CN 110040669 B CN110040669 B CN 110040669B
Authority
CN
China
Prior art keywords
offset
tray
coordinate system
fork
pallet
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810044528.7A
Other languages
Chinese (zh)
Other versions
CN110040669A (en
Inventor
侯典清
陈清
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Linde China Forklift Truck Corp Ltd
Original Assignee
Linde China Forklift Truck Corp Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Linde China Forklift Truck Corp Ltd filed Critical Linde China Forklift Truck Corp Ltd
Priority to CN201810044528.7A priority Critical patent/CN110040669B/en
Publication of CN110040669A publication Critical patent/CN110040669A/en
Application granted granted Critical
Publication of CN110040669B publication Critical patent/CN110040669B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B66—HOISTING; LIFTING; HAULING
    • B66F—HOISTING, LIFTING, HAULING OR PUSHING, NOT OTHERWISE PROVIDED FOR, e.g. DEVICES WHICH APPLY A LIFTING OR PUSHING FORCE DIRECTLY TO THE SURFACE OF A LOAD
    • B66F17/00—Safety devices, e.g. for limiting or indicating lifting force
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B66—HOISTING; LIFTING; HAULING
    • B66F—HOISTING, LIFTING, HAULING OR PUSHING, NOT OTHERWISE PROVIDED FOR, e.g. DEVICES WHICH APPLY A LIFTING OR PUSHING FORCE DIRECTLY TO THE SURFACE OF A LOAD
    • B66F9/00—Devices for lifting or lowering bulky or heavy goods for loading or unloading purposes
    • B66F9/06—Devices for lifting or lowering bulky or heavy goods for loading or unloading purposes movable, with their loads, on wheels or the like, e.g. fork-lift trucks
    • B66F9/075—Constructional features or details
    • B66F9/0755—Position control; Position detectors

Landscapes

  • Engineering & Computer Science (AREA)
  • Structural Engineering (AREA)
  • Transportation (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Mechanical Engineering (AREA)
  • Civil Engineering (AREA)
  • Forklifts And Lifting Vehicles (AREA)

Abstract

The invention discloses a high-position goods-forking auxiliary method, which comprises the following steps: the operation unit receives cloud point data sent by the 3D TOF camera; calculating the offset angle A of the forklift and the pallet surface in the XY plane by using an iterative method; identifying a tray according to the cloud point data and predefined tray characteristics; extracting coordinate values of the central point of the tray; calculating relative offsets of the pallet center point relative to the center of the fork insertion tip by a normalized coordinate system and a translation coordinate system (X1, Y1, Z1); the forklift controller receives the four offset values (X1, Y1, Z1 and A) sent by the arithmetic unit; and sending a control command to an offset elimination device according to the four offsets so as to eliminate the four offsets. Compared with the prior art, the fork truck improves the safety of fork truck, improves the driving experience of a driver, and improves the working efficiency to a certain extent.

Description

High-position goods-forking auxiliary method
Technical Field
The invention relates to the field of safety of forklifts, in particular to a high-position fork auxiliary method.
Background
When the forklift is used for taking goods at a high position, a driver needs to insert a pallet fork into a pallet jack, the driver often needs to be very concentrated in and try for multiple times to accurately fork the goods due to poor high-position vision, and even the goods fall and other safety accidents occur due to wrong insertion. The current solutions to the above problems mainly include: 1) projecting a laser beam which is flush with the upper surface of the fork; 2) the fork side or the fork carriage is provided with a simulation camera. Both of the above two schemes provide qualitative auxiliary guidance to a certain extent but are not accurate, and the driver still needs to raise his head or observe the relative position of the pallet fork and the pallet through the display screen and make an action according to the judgment. Since the two schemes still depend on the judgment of the driver, the safety problem caused by the problems of visual judgment error, inaccurate observation and the like is inevitable. In addition, the existing scheme requires a driver to respectively adjust the running motor, the side shifting oil cylinder, the lifting oil cylinder and the rotating motor to realize alignment insertion, the number of functional buttons is large, the operation is complex, errors are easy to occur, and the driving experience is poor.
Disclosure of Invention
In view of the above, the present invention provides a high order fork assist method that overcomes or at least partially solves the above mentioned problems, by acquiring 3D cloud point data by a 3D TOF camera; calculating an offset angle a by an arithmetic unit using an iterative method, and calculating relative offsets (X1, Y1, Z1) by normalizing the coordinate system; the deviation value is automatically eliminated through the forklift controller to achieve auxiliary forklift control, compared with the prior art, the forklift control system improves forklift safety, improves driver driving experience, and improves working efficiency to a certain extent.
The technical scheme adopted by the invention for solving the technical problems is as follows:
a high-position fork auxiliary method comprises the following steps:
the operation unit receives cloud point data sent by the 3D TOF camera; calculating the offset angle A of the forklift and the pallet surface in the XY plane by using an iterative method; identifying a tray according to the cloud point data and predefined tray characteristics; extracting coordinate values of the central point of the tray; calculating relative offsets of the pallet center point relative to the center of the fork insertion tip by a normalized coordinate system and a translation coordinate system (X1, Y1, Z1);
the forklift controller receives the four offset values (X1, Y1, Z1 and A) sent by the arithmetic unit; and sending a control command to an offset elimination device according to the four offsets so as to eliminate the four offsets.
Preferably, the calculating the offset angle a of the forklift and the pallet surface in the XY plane by using an iterative method includes:
and according to the cloud point data, circularly iterating within a preset angle range, carrying out feature matching on the iterated angle and the predefined tray features in each iteration process, and determining the iterated angle as an offset angle A when the matching degree meets a set range.
Preferably, the calculating the relative offset of the central point of the pallet relative to the center of the fork insertion tip through the normalized coordinate system and the translation coordinate system includes:
normalizing a camera coordinate system O based on the offset angle A1(ii) a The camera coordinate system O1Is represented by O1A 3DTOF camera coordinate system as origin;
based on the coordinate system O1And a coordinate system O2Relative positional relationship of (a) and normalized camera coordinate system O1Calculating the relative offset of the central point of the pallet relative to the center of the fork insertion tip; said coordinate system O2Is represented by O2Fork truck coordinate system of origin, O2Is the center point of the fork points of the two forks.
Preferably, the camera coordinate system O is normalized based on the offset angle a1The method comprises the following steps:
with O1n＝O1Xr represents a standard coordinate system rotated by an angle a around the Z axis;
where R represents a rotation matrix around the Z-axis due to the rotation angle a, expressed as:
Figure BDA0001550475260000021
preferably, the system is based on a coordinate system O1And a coordinate system O2Relative positional relationship of (a) and normalized camera coordinate system O1Calculating the relative offset of the center point of the pallet relative to the center of the fork insertion tip, comprising:
to the normalized camera coordinate system O1Translating S to obtain a relative coordinate system O of the return point of the camera2Normalized coordinate value O2nThe following are:
O2n＝O1n+S＝O1×R+S
wherein the translation matrix S represents a coordinate system O1And a coordinate system O2The relative positional relationship of (a);
the relative offset of the center point of the pallet from the center of the fork prongs (X1, Y1, Z1) is thus obtained as follows:
(X1,Y1,Z1)＝(X2,Y2,Z2)×R+S
wherein, (X2, Y2, Z2) are coordinate values of the center point of the tray.
Preferably, the forklift controller receives four offset amounts (X1, Y1, Z1, a) transmitted from the arithmetic unit, and transmits a control command to the offset amount canceling device to cancel the four offset amounts according to the four offset amounts, further comprising:
judging the state of a function switch, and if the continuous pressing time of the function switch is longer than the preset time, sending a control command to an offset elimination device by the forklift controller according to the four offsets so as to eliminate the four offsets; otherwise, the forklift controller stops sending the control command to the offset elimination device.
Preferably, the method for assisting high-position fork cargo further comprises: and the forklift controller sends the identified function switch state to the operation unit.
Preferably, the method for assisting high-position fork cargo further comprises: and the display unit receives the four offsets sent by the operation unit and displays the four offsets on a screen in real time.
Preferably, the method for assisting high-position fork cargo further comprises: the display unit receives the real-time video sent by the analog camera and displays the real-time video on a screen in real time.
Preferably, the offset eliminating device comprises a lifting oil cylinder device, a steering motor device, a side shifting oil cylinder device and a running motor device; the forklift controller is connected with the lifting oil cylinder device to eliminate the offset in the Z direction; the forklift controller is connected with the steering motor device to eliminate the deviation in the direction A; the forklift controller is connected with the side shift oil cylinder device to eliminate the offset in the Y direction; the forklift controller is connected with the running motor device to eliminate the offset in the X direction.
The functional switch state of the invention comprises short-time pressing, continuous pressing and releasing; when the function switch is pressed down for a short time, the display unit displays four offsets in real time; when the function switch state is continuously pressed, the display unit displays four offsets in real time and the forklift controller starts auxiliary forklift; and when the function switch state is released in the auxiliary goods forking process, the execution action is cancelled, and the auxiliary goods forking is stopped. When the forklift is near a target position and a driver continuously presses a function switch, the function is triggered, a 3D TOF camera acquires cloud point data in a visual field range, an arithmetic unit performs tray identification and tray center point coordinate value extraction by using the 3D cloud point data in the visual field range acquired by the 3D TOF camera and predefined tray characteristics, calculates a deviation angle A between the forklift and a tray surface in an XY plane by using an iterative method, then obtains tray center point coordinate values (X1, Y1 and Z1) of a coordinate system taking a fork point center as an origin through a normalized coordinate system and a translation coordinate system, the coordinate values are deviation values of the current forklift position and the target tray position in the front-back, left-right and up-down directions, the arithmetic unit sends the four deviation values (X1, Y1, Z1 and A) to a forklift controller, and the forklift controller controls a running motor, The lateral shifting oil cylinder, the lifting oil cylinder and the steering motor respectively and automatically eliminate X, Y, Z and A four-direction offset, and simultaneously the four offsets can be displayed on the display unit in real time. For safety reasons, if the driver releases the function switch at any time, the execution is immediately suspended. Of course, the invention can also have no function switch or the function switch is triggered automatically, when the forklift is near the target position, the function is triggered automatically, and the application of the automatic high-position fork truck auxiliary method comprises the AGV unmanned forklift.
The invention has the following beneficial effects:
(1) the 3D cloud point data are collected through a 3D TOF camera, the tray is identified through an operation unit, the offset of the current forklift position and the target tray position in four directions is calculated, and the offset is sent to a forklift controller to execute control; the operation and the control are both automated; meanwhile, the offsets in the four directions can be displayed in real time, so that quantitative guidance and assistance are provided for a driver to fork goods;
(2) according to the high-position fork auxiliary method, the deviation between the current fork position and the target pallet position is automatically identified by using the 3D TOF camera, and the fork is automatically realized by eliminating the deviation, so that safety accidents such as wrong insertion, deviation insertion, falling of the goods and the like caused by manual experience judgment errors are avoided, and the safety of a warehouse is improved;
(3) according to the high-position fork auxiliary method, a fork flow is simplified into one-step operation from four-step operation (a driver needs to respectively adjust the running motor, the side shifting oil cylinder, the lifting oil cylinder and the rotating motor to realize alignment insertion), the driver only needs to continuously press the function switch, and the oil cylinders/motors in four directions can automatically realize alignment insertion, so that the operation of the driver is simplified, the capacity of the driver is released, the operation efficiency is improved, and the driving experience is improved;
(4) the high-order fork auxiliary method is triggered by the physical key, and automatic fork is immediately stopped if the key is cancelled in the operation process, so that the main driving position and good safety protection of a driver are ensured.
The above description is only an overview of the technical solutions of the present invention, and the embodiments of the present invention are described below in order to make the technical means of the present invention more clearly understood and to make the description of the technical means more comprehensible.
The above and other objects, advantages and features of the present invention will become more apparent to those skilled in the art from the following detailed description of specific embodiments thereof, taken in conjunction with the accompanying drawings.
Drawings
FIG. 1 is a main flow chart of a high level fork assist method according to an embodiment of the present invention;
FIG. 2 is a block diagram of a high-level forklift assist system according to an embodiment of the present invention;
FIG. 3 is a first schematic view of a high-level forklift assist system according to an embodiment of the present invention;
FIG. 4 is a second schematic view of the high-level fork assist system according to the embodiment of the present invention;
FIG. 5 is a third schematic view of a high-level fork assist system according to an embodiment of the present invention;
FIG. 6 is a detailed flowchart of the method for assisting in high-order fork loading according to the embodiment of the present invention;
FIG. 7 is a schematic illustration of pallet identification and offset calculation according to an embodiment of the present invention;
FIG. 8 is an exemplary diagram of camera coordinate system normalization of an embodiment of the present invention;
fig. 9 is a diagram illustrating an example of tray identification according to an embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Referring to fig. 1, the invention relates to a high-order fork auxiliary method, which comprises the following steps:
step 101, an arithmetic unit receives cloud point data sent by a 3D TOF camera; calculating the offset angle A of the forklift and the pallet surface in the XY plane by using an iterative method; identifying a tray according to the cloud point data and predefined tray characteristics; extracting coordinate values of the central point of the tray; calculating relative offsets of the pallet center point relative to the center of the fork insertion tip by a normalized coordinate system and a translation coordinate system (X1, Y1, Z1); the 3D TOF camera is mounted on the fork carriage and parallel to the forks; the relative offset X1 represents an offset value in the X direction (front-rear direction) between the current forklift position and the target pallet position; the relative offset Y1 represents an offset value between the current forklift position and the target pallet position in the Y direction (left-right direction); the relative offset Z1 represents the offset between the current forklift position and the target pallet position in the Z direction (up-down direction).
102, the forklift controller receives four offsets (X1, Y1, Z1 and A) sent by the arithmetic unit; and sending a control command to an offset elimination device according to the four offsets so as to eliminate the four offsets.
Referring to fig. 2 to 5, in the present embodiment, a high-order fork assist method is applied to a high-order fork assist system, and the high-order fork assist system is mounted on a forklift body, and includes: the system comprises a 3D TOF camera 1, a function switch 2, an operation unit 3, a forklift controller 4 and an offset elimination device 5; the 3D TOF camera 1 is mounted on the fork carriage 9 and remains parallel to the forks 10; the operation unit 3 is connected with the 3D TOF camera 1 to receive and process 3D cloud point data to obtain four offsets; the forklift controller 4 is connected with the function switch 2 to identify the state of the function switch 2; the forklift controller 4 is connected with the operation unit 3 to send the state of the function switch 2 and receive four offsets sent by the operation unit 3; the forklift controller 4 is connected with the offset canceling device 5 to cancel the four offsets.
The offset elimination device 5 comprises a lifting oil cylinder device 51, a steering motor device 52, a side shifting oil cylinder device 53 and a running motor device 54; the lifting oil cylinder device 51 comprises a lifting oil cylinder which is arranged on a forklift gantry; and the forklift controller 4 is connected with the lifting oil cylinder to eliminate the offset in the Z direction. The steering motor device 52 comprises a steering motor, and the steering motor is mounted on the frame of the forklift; the forklift controller 4 is connected with the steering motor to eliminate the deviation in the direction a. The side-shifting cylinder device 53 comprises a side-shifting cylinder which is arranged on the fork frame; the forklift controller 4 is connected with the side shift oil cylinder to eliminate the offset in the Y direction. The running motor device 54 includes a running motor mounted on the forklift frame; the forklift controller 4 is connected with the running motor to eliminate the offset in the X direction.
The high-order fork goods auxiliary system still includes: a display unit 6; the display unit 6 is installed in the cab; the display unit 6 is connected with the arithmetic unit 3 to display four offsets in real time. The offset value may be compared to a preset threshold value to display a different color. For example, when the deviation value is smaller than the set threshold, green is displayed, which indicates that the deviation value of the dimension has met the requirement, and the fork can be smoothly carried out in the direction; when red is displayed, it indicates that the offset value is still large; yellow is the transition value between red and green, indicating that the movement amplitude needs to be reduced to reach the target.
The function switch 2 is a button switch and is integrated on the portal control handle 11, so that the operation of a driver is very convenient and the comfort level is improved. The function switch 2 is used for triggering the function to be turned on and turned off, if the switch is pressed down for a short time, only the offset calculated through the algorithm is displayed, if the switch is pressed down continuously, the offset is displayed firstly, then the dropping sound is reported to start assisting in forking, and if the switch is released in the assisting process, the action is executed and cancelled immediately. The forklift controller 4 and the operation unit 3 are in two-way communication, the operation unit 3 transmits the calculated offset to the forklift controller 4, and the forklift controller 4 transmits the current state of the function switch 2 to the operation unit 3.
The above-described identification, calculation and display processes are performed at a fixed frequency (e.g., 10HZ), so that the display unit 6 displays the updated offset value in real time, so that the forklift controller 4 forms a closed-loop control with the lift cylinder device 51, the steering motor device 52 and the side shift cylinder device 53. However, as the forklift moves forward, the camera view is reduced, and therefore a closed loop cannot be formed between the forklift controller 4 and the travel motor device 54. In this embodiment, a distance measuring sensor 8 may be added to the fork carriage 9 to form a closed loop with the direction of advance by measuring the position of the fork carriage 9 to the cargo location.
Generally, the 3D TOF camera 1 returns three-dimensional coordinate values and gray scale values of a cartesian coordinate system, so that the captured image is a gray scale image, in this embodiment, an analog camera 7 may be installed beside the 3D TOF camera 1 to provide instant video on the pallet fork 10 side. The video of the analog camera 7 can be displayed by the display unit 6.
Referring to fig. 6, in the present embodiment, a detailed procedure of performing auxiliary fork loading by using a high-position fork loading auxiliary method includes:
step 601, judging whether the current pallet fork is horizontal, if so, entering step 603, and otherwise, entering step 602;
step 602, automatically leveling the pallet fork, and then entering step 603; the automatic leveling of the fork is the prior art, and the detection and the control of the fork angle are realized by arranging a potentiometer on the fork frame;
step 603, reporting a report that the pallet fork is level currently;
step 604, identifying the tray according to the characteristics of the tray, and if the tray is identified, entering step 605, otherwise, entering step 606;
605, extracting coordinates of the center point of the pallet, and calculating the offset (X1, Y1, Z1) of the center point of the pallet relative to the center of the fork point of the pallet through coordinate system conversion, wherein the offset A is calculated through an iterative algorithm; then proceed to step 607;
step 606, prompting that the tray is not identified and is confirmed to be in the camera view field, and then quitting operation and control;
step 607, displaying the offset (X1, Y1, Z1, A) on the screen;
step 608, detecting the current function switch state, if the current function switch state is closed, entering step 609, otherwise, entering step 610;
step 609, judging whether the current offset X1 is greater than a set threshold value, such as 50mm, if not, clearing the screen and exiting, otherwise, returning to the step 604; the step is used for judging whether the tray is still in the visual field range of the camera, if the tray is larger than the threshold value, the tray is shown in the visual field range, if the tray is smaller than the threshold value, the tray is not in the visual field range, the recognition work cannot be carried out, and therefore operation and control are quitted;
step 610, beeping to indicate that automatic forking is started;
step 611, enabling the steering motor and the lifting oil cylinder to act simultaneously to eliminate the offsets A and Z1;
step 612, detecting the current function switch state, and if the current function switch state is closed, exiting the operation and control, and entering step 613; step 613, detecting whether the offsets a and Z1 are eliminated, if not, entering step 614, otherwise, entering step 615;
step 614, identifying the tray, calculating the offset value and dynamically updating the display offset value, and then returning to step 611;
step 615, enabling the side-shifting oil cylinder to act to eliminate the offset Y1, and then entering step 616;
step 616, detecting the current function switch state, if the current function switch state is closed, exiting the operation and control, otherwise entering step 617;
step 617, detecting whether the offset Y1 is eliminated, if not, entering step 618, otherwise, entering step 619;
step 618, identifying the tray, calculating the offset value and dynamically updating the display offset value, and then returning to step 615;
step 619, prompting that the automatic tray insertion is started, and then entering step 620, wherein the step prompts a driver that the offset A, Z1 and the Y1 are eliminated on one hand and simultaneously prompts the driver to enter the last step, namely X1 elimination because the risk of the motion of the driving motor is relatively large;
step 620, enabling the driving motor to act to eliminate the offset X1 and the length value of the tray;
step 621, detecting the current function switch state, if the current function switch state is closed, exiting the operation and control, otherwise entering step 622;
step 622, calculating and judging whether the pallet fork completes the pallet inserting action or not through the running distance, if so, entering step 623, and otherwise, returning to step 620;
step 623, the semi-automatic auxiliary function of lifting the pallet fork by the beep is completed.
Further, FIG. 7 shows a schematic of tray identification and calculation of four offsets (X1, Y1, Z1, A), shown as O1The coordinate system of origin is 3D TOF camera coordinate system, with O2The coordinate system of the origin is the coordinate system of the forklift, where O2Is the center point of the fork point of the two forks, O1Coordinate system and O2The relative positional relationship of the coordinate system can be described by a translation matrix S, which is related to the mounting positions of the fork of the forklift and the 3D TOF camera, so that O is provided2＝O1+ S. Let tray center point coordinate be T, the nodical T respectively with the tray jack about point T be T1 and T2, and the tray characteristic is shown in the dotted line frame of FIG. 7: the features are formed by the upper and lower faces of part of the pallet and the part of the pallet immediately adjacent to each other, or are understood to mean that two straight lines contain a pair of similar brackets between them "]The marks, upper and lower line segments and brackets of [ "should also conform to the dimensions (i) to (ii), especially the dimensions (i) and (iii) set in the figure. The thickness of the tray can be understood, the distance between the jacks can be understood, the height of the jacks can be understood, and the wall thickness of the jacks can be understood, namely the distance between the lower surface of the jacks and the lower surface of the tray.
Specifically, the offset angle a is calculated by an iteration method, iteration is performed in a set angle a range in a circulating manner, and the offset angle a is compared with a tray feature template (namely, a graph in a dotted frame in fig. 7, namely, an upper line and a lower line which respectively comprise a pair of similar middle brackets [ ], and when four dimensions are matched, a tray is identified) in each iteration process, and when the matching degree meets the set range, the offset angle a is determined as the calculated rotation angle. For example, if the range of a is-20 degrees to +20 degrees, the range is increased from-20 degrees to +20 degrees in steps of 1 degree, and tray feature matching is performed on each angle, where the matching degree is determined as the offset angle a value when the matching degree satisfies the set range, and the matching degree is reflected by the sizes of (i) to (iv) in the dashed-line frame of fig. 7.
The calculation of the offset value is important because the return value of the 3D TOF camera is three-dimensional data, and it is difficult to identify features by directly using spatial three-dimensional data, so that it is necessary to convert the 3D image into a 2D image for further analysis, the function of the offset value a is embodied here, and the rotation matrix R around the Z axis caused by the offset value a can be represented as:
Figure BDA0001550475260000081
the standard coordinate system after a rotation angle A around the Z axis can be represented as O1n＝O1Xr, an example of camera coordinate system normalization is shown in fig. 8.
The 3D problem is translated into 2D identification by normalization of the coordinate system, at which time the target pallet is identified according to predefined pallet features and the coordinate values of the pallet in the camera coordinate system can be extracted (X2, Y2, Z2).
The offset (X1, Y1, Z1) to be found in three dimensions is actually the relative normalized O of the center point of the tray2Coordinate values of the coordinate system, vs. coordinate system O1Firstly, the rotation angle A is made and then the S is translated, so that the relative normalized O of the camera return point can be obtained2Coordinate values of the coordinate system, thus:
O2n＝O1n+S＝O1×R+S
the relative offset of the center point of the pallet from the center of the fork prongs (X1, Y1, Z1) is further found as follows:
(X1,Y1,Z1)＝(X2,Y2,Z2)×R+S
therefore, from the iteratively calculated a values, the pallet center point coordinate values (X1, Y1, Z1) returned by the camera coordinate system and the four dimensional offsets (X1, Y1, Z1, a) are obtained based on the following formula.
[X1,Y1,Z1,A]＝[(X2,Y2,Z2)×R+S,A]
Fig. 9 shows an example of 3D TOF camera return values, where fig. 9(a) is a real object map of the pallet on the left, fig. 9(b) is a grayscale map of 3D TOF camera return, and fig. 9(c) is a pallet cloud point to be recognized after processing.
The 3D TOF camera is used for collecting cloud point data in a visual field range, the cloud point data generally takes a camera lens center as an origin to establish a coordinate system, the camera can return XYZ values and brightness values in the coordinate system, the brightness values can reflect gray values of pixel points, the cloud point data are core input of the 3D TOF camera, objects can be recognized, characteristics can be extracted, characteristic parameters can be calculated and the like through the cloud point data, the resolution of the 3D TOF camera is one of core parameters of the camera, and final recognition and control accuracy is affected. The 3D TOF camera is arranged on the fork frame and is parallel to the fork, and when the 3D TOF camera is away from the pallet by about 1-1.5 m in the fork-loading assisting process, the distance is matched with a proper camera view angle to ensure that the whole pallet is within the visual field range of the camera. Although empty pallet forks can block part of the pallet, the extraction of the characteristics of the pallet is not influenced, the operation unit receives 3D cloud point data collected by the 3D TOF camera, identifies the pallet according to an algorithm stored in the operation unit in advance, extracts coordinate values of the center point of the pallet, and calculates the offset angle of the forklift and the pallet surface in an XY plane by using an iterative method; and calculating the relative offset of the central point of the pallet relative to the center of the fork insertion tip through a normalized coordinate system, and finally outputting four offsets. The four offsets are transmitted to the display unit for real-time display on one hand, and are transmitted to the forklift controller in real time for further action on the other hand. The forklift controller receives the four offsets from the operation unit, eliminates Z-direction offset by controlling a lifting oil cylinder of the lifting oil cylinder device, eliminates A-direction offset by controlling a steering motor of the steering motor device, eliminates Y-direction offset by controlling a side shifting oil cylinder of the side shifting oil cylinder device, and eliminates X-direction offset by controlling a running motor of the running motor device. It should be noted that, the offset in the X direction is added with a preset tray length, so that the fork can be automatically forked into the tray, and the process of eliminating the offset is the process of automatically forking the goods.
Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that changes, modifications, substitutions and alterations can be made in these embodiments without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.

Claims (7)

1. A high-position fork auxiliary method is characterized by comprising the following steps:
the operation unit receives cloud point data sent by the 3D TOF camera; calculating the offset angle A of the forklift and the pallet surface in the XY plane by using an iterative method; identifying a tray according to the cloud point data and predefined tray characteristics; extracting coordinate values of the central point of the tray; calculating relative offsets of the pallet center point relative to the center of the fork insertion tip by a normalized coordinate system and a translation coordinate system (X1, Y1, Z1);
the forklift controller receives the four offset values (X1, Y1, Z1 and A) sent by the arithmetic unit; sending a control command to an offset elimination device according to the four offsets so as to eliminate the four offsets;
the forklift controller receives four offset (X1, Y1, Z1, a) sent by an arithmetic unit, and sends a control command to an offset cancellation device according to the four offset to cancel the four offset, specifically comprising:
judging the state of a function switch, and if the continuous pressing time of the function switch is longer than the preset time, sending a control command to an offset elimination device by the forklift controller according to the four offsets so as to eliminate the four offsets; otherwise, the forklift controller stops sending the control command to the offset elimination device;
the offset eliminating device comprises a lifting oil cylinder device, a steering motor device, a side shifting oil cylinder device and a running motor device; the forklift controller is connected with the lifting oil cylinder device to eliminate the offset in the Z direction; the forklift controller is connected with the steering motor device to eliminate the deviation in the direction A; the forklift controller is connected with the side shift oil cylinder device to eliminate the offset in the Y direction; the forklift controller is connected with the running motor device to eliminate the offset in the X direction;
specifically, when the four offsets are eliminated, the steering motor device and the lifting oil cylinder device are made to act simultaneously to eliminate the offset in the Z direction and the offset in the a direction; then the lateral shifting oil cylinder device is actuated to eliminate the offset in the Y direction; finally, the running motor device is enabled to act to eliminate the deviation in the X direction;
specifically, the detailed process of using a high-position fork auxiliary method to assist fork loading comprises the following steps:
step 601, judging whether the current pallet fork is horizontal, if so, entering step 603, and otherwise, entering step 602;
step 602, automatically leveling the pallet fork, and then entering step 603;
step 603, reporting a report that the pallet fork is level currently;
step 604, identifying the tray according to the characteristics of the tray, and if the tray is identified, entering step 605, otherwise, entering step 606;
605, extracting coordinates of the center point of the pallet, and calculating the offset (X1, Y1, Z1) of the center point of the pallet relative to the center of the fork point of the pallet through coordinate system conversion, wherein the offset A is calculated through an iterative algorithm; then proceed to step 607;
step 606, prompting that the tray is not identified and is confirmed to be in the camera view field, and then quitting operation and control;
step 607, displaying the offset (X1, Y1, Z1, A) on the screen;
step 608, detecting the current function switch state, if the current function switch state is closed, entering step 609, otherwise, entering step 610;
step 609, judging whether the current offset X1 is greater than a set threshold, if not, clearing the screen and exiting, otherwise, returning to step 604; the step is used for judging whether the tray is still in the visual field range of the camera, if the tray is larger than the threshold value, the tray is shown in the visual field range, if the tray is smaller than the threshold value, the tray is not in the visual field range, the recognition work cannot be carried out, and therefore operation and control are quitted;
step 610, beeping to indicate that automatic forking is started;
step 611, enabling the steering motor and the lifting oil cylinder to act simultaneously to eliminate the offsets A and Z1;
step 612, detecting the current function switch state, and if the current function switch state is closed, exiting the operation and control, and entering step 613;
step 613, detecting whether the offsets a and Z1 are eliminated, if not, entering step 614, otherwise, entering step 615;
step 614, identifying the tray, calculating the offset value and dynamically updating the display offset value, and then returning to step 611;
step 615, enabling the side-shifting oil cylinder to act to eliminate the offset Y1, and then entering step 616;
step 616, detecting the current function switch state, if the current function switch state is closed, exiting the operation and control, otherwise entering step 617;
step 617, detecting whether the offset Y1 is eliminated, if not, entering step 618, otherwise, entering step 619;
step 618, identifying the tray, calculating the offset value and dynamically updating the display offset value, and then returning to step 615;
step 619, prompting that the automatic tray insertion is started, and then entering step 620, wherein the step prompts a driver that the offset A, Z1 and the Y1 are eliminated on one hand and simultaneously prompts the driver to enter the last step, namely X1 elimination because the risk of the motion of the driving motor is relatively large;
step 620, enabling the driving motor to act to eliminate the offset X1 and the length value of the tray;
step 621, detecting the current function switch state, if the current function switch state is closed, exiting the operation and control, otherwise entering step 622;
step 622, calculating and judging whether the pallet fork completes the pallet inserting action or not through the running distance, if so, entering step 623, and otherwise, returning to step 620;
step 623, completing a semi-automatic auxiliary function of lifting the pallet fork by beeping sound;
calculating the offset angle A of the forklift and the pallet surface in the XY plane by using an iterative method, and specifically comprising the following steps:
circularly iterating within a set angle A range, comparing with a tray characteristic template in each iteration process, and determining that an offset angle A value is a calculated rotation angle when the matching degree meets the set range; the set angle A ranges from minus 20 degrees to plus 20 degrees, starting from minus 20 degrees, the step length is increased to plus 20 degrees by taking 1 degree as a step length, tray feature matching is carried out on each angle, the matching degree is determined as an offset angle A value when the matching degree meets the set range, and the matching degree is reflected by four sizes from the first dimension to the fourth dimension;
the tray is characterized by consisting of upper and lower surfaces of a part of trays and adjacent part of tray jacks, namely a pair of marks similar to brackets [ ] are arranged between two straight lines, and upper and lower line segments and brackets accord with set sizes (i) to (iv); the thickness of the tray, the distance between the jacks, the height of the jacks and the wall thickness of the jacks refer to the distance between the lower surfaces of the jacks and the lower surface of the tray.
2. The method of claim 1, wherein calculating the relative offset of the center point of the pallet from the center of the fork insertion tip by a normalized coordinate system and a translational coordinate system comprises:
normalizing a camera coordinate system O based on the offset angle A1(ii) a The camera coordinate system O1Is represented by O1A 3D TOF camera coordinate system being an origin;
based on the coordinate system O1And a coordinate system O2Relative positional relationship of (a) and normalized camera coordinate system O1Calculating the relative offset of the central point of the pallet relative to the center of the fork insertion tip; said coordinate system O2Is represented by O2Fork truck coordinate system of origin, O2Is the center point of the fork points of the two forks.
3. The method of claim 2, wherein the normalizing the camera coordinate system O based on the offset angle a is performed1The method comprises the following steps:
with O1n＝O1Xr represents a standard coordinate system rotated by an angle a around the Z axis;
where R represents a rotation matrix around the Z-axis due to the rotation angle a, expressed as:
Figure FDA0003241721960000031
4. the overhead fork assist method of claim 3, wherein the reference frame O is based on1And a coordinate system O2Relative positional relationship of (a) and normalized camera coordinate system O1Calculating the relative offset of the center point of the pallet relative to the center of the fork insertion tip, comprising:
to the normalized camera coordinate system O1Translating S to obtain a relative coordinate system O of the return point of the camera2Normalized coordinate value O2nThe following are:
O2n＝O1n+S＝O1×R+S
wherein the translation matrix S represents a coordinate system O1And a coordinate system O2The relative positional relationship of (a);
the relative offset of the center point of the pallet from the center of the fork prongs (X1, Y1, Z1) is thus obtained as follows:
(X1,Y1,Z1)＝(X2,Y2,Z2)×R+S
wherein, (X2, Y2, Z2) are coordinate values of the center point of the tray.
5. The method of assisting in high forking according to claim 1, further comprising: and the forklift controller sends the identified function switch state to the operation unit.
6. The method of assisting in high forking according to claim 1, further comprising: and the display unit receives the four offsets sent by the operation unit and displays the four offsets on a screen in real time.
7. The method of assisting in high forking according to claim 1, further comprising: the display unit receives the real-time video sent by the analog camera and displays the real-time video on a screen in real time.
CN201810044528.7A 2018-01-17 2018-01-17 High-position goods-forking auxiliary method Active CN110040669B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810044528.7A CN110040669B (en) 2018-01-17 2018-01-17 High-position goods-forking auxiliary method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810044528.7A CN110040669B (en) 2018-01-17 2018-01-17 High-position goods-forking auxiliary method

Publications (2)

Publication Number Publication Date
CN110040669A CN110040669A (en) 2019-07-23
CN110040669B true CN110040669B (en) 2021-12-10

Family

ID=67273519

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810044528.7A Active CN110040669B (en) 2018-01-17 2018-01-17 High-position goods-forking auxiliary method

Country Status (1)

Country Link
CN (1) CN110040669B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110850868A (en) * 2019-10-12 2020-02-28 浙江大华机器人技术有限公司 Deviation rectifying method, device and equipment for AGV (automatic guided vehicle) butt joint of cows and storage medium
CN110852180B (en) * 2019-10-17 2022-08-02 上海快仓智能科技有限公司 TOF camera calibration method for automatic guided vehicle and automatic guided vehicle

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104692293A (en) * 2015-03-11 2015-06-10 武汉汉迪机器人科技有限公司 Full-automatic omni-directional unmanned forklift
CN105223953A (en) * 2015-10-10 2016-01-06 北京微尘嘉业科技有限公司 A kind of control system of Closed loop track object space of view-based access control model information of intelligence
CN105293380A (en) * 2015-11-30 2016-02-03 厦门海普智能科技有限公司 Display method and display device for posture of pallet fork of forklift loader
CN106292653A (en) * 2015-05-21 2017-01-04 中国科学院沈阳自动化研究所 The pipeline vehicle method for correcting error of view-based access control model sensing system
CN107428518A (en) * 2015-04-16 2017-12-01 株式会社丰田自动织机 Fork truck

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104692293A (en) * 2015-03-11 2015-06-10 武汉汉迪机器人科技有限公司 Full-automatic omni-directional unmanned forklift
CN107428518A (en) * 2015-04-16 2017-12-01 株式会社丰田自动织机 Fork truck
CN106292653A (en) * 2015-05-21 2017-01-04 中国科学院沈阳自动化研究所 The pipeline vehicle method for correcting error of view-based access control model sensing system
CN105223953A (en) * 2015-10-10 2016-01-06 北京微尘嘉业科技有限公司 A kind of control system of Closed loop track object space of view-based access control model information of intelligence
CN105293380A (en) * 2015-11-30 2016-02-03 厦门海普智能科技有限公司 Display method and display device for posture of pallet fork of forklift loader

Also Published As

Publication number Publication date
CN110040669A (en) 2019-07-23

Similar Documents

Publication Publication Date Title
US10248123B2 (en) Mobile apparatus
KR101769284B1 (en) Lost vehicle recovery utilizing associated feature pairs
CN110040669B (en) High-position goods-forking auxiliary method
US11718513B2 (en) Position and posture estimation system
US20210041564A1 (en) Position and posture estimation apparatus
US20220254054A1 (en) Construction machine work information generation system and work information generation method
JP2020132431A (en) Travel support device for industrial vehicle
CN110816522B (en) Vehicle attitude control method, apparatus, and computer-readable storage medium
CN114234862A (en) Floor detection device and use method thereof
CN212669124U (en) Forklift truck
CN207713346U (en) A kind of semi-automatic high-order fork goods auxiliary system
US20200307575A1 (en) Vehicle comprising a working equipment, and a working equipment, and a method in relation thereto
WO2020170747A1 (en) Industrial vehicle travel assistance device
JP7192706B2 (en) Position and orientation estimation device
JP2021066539A (en) Cargo handling work support device for cargo handling vehicle
CN110745702B (en) Monocular camera-based crane working space multi-information measuring method and system
CN112614191B (en) Loading and unloading position detection method, device and system based on binocular depth camera
CN114859768A (en) System and method for remote operation of a work machine including an implement
CN111844036A (en) Method for sequencing multi-vehicle type and multi-variety automobile glass assemblies
JP3858703B2 (en) Cargo work support device for industrial vehicle and industrial vehicle
CN116390834A (en) Robot system
JP3900902B2 (en) Cargo work support device for industrial vehicle and industrial vehicle
US20230417581A1 (en) Industrial vehicle control device, industrial vehicle, and industrial vehicle control program
CN221051495U (en) Automatic positioning lifting tray device
WO2024014194A1 (en) Route setting method, program, and route setting system

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant