A method, system and medium for collision prevention control of a stacker fork

Abstract

The application discloses a kind of stacker fork anti-collision control method, system and medium, it is related to automated warehouse technical field, the method includes: obtaining target tray position and control fork extension;Real-time driving data of telescopic motor is collected, and whether contact is blocked is judged by comparing with no-load reference curve;Determine blocked after control fork retraction, lifting shaft displacement and low-speed heavy exploration;Record blocked position coordinates to build local blocked distribution point set, calculate safety envelope space to determine insertable area;Based on insertable area, generate multi-axis interpolation smooth curve of fork telescoping and lifting shaft linkage;Control fork and lifting mechanism along curve collaborative operation completes insertion and picking.The application actively senses blocked state through contact feedback, constructs blocked point set and plans multi-axis smooth avoidance path, realizes the intelligent anti-collision and autonomous insertion and picking of stacker fork, improves the reliability and efficiency of warehousing operation.

Description

Technical Field

[0001] This invention relates to the field of automated storage and retrieval systems (AS / RS), and more specifically, to a method, system, and medium for preventing collisions with stacker crane forks. Background Technology

[0002] Stacker cranes are the core storage and retrieval equipment in automated warehouses. When the forks are picking up pallets, collisions often occur between the forks and pallets, goods, or shelves due to pallet misalignment, deformation, or shelf sagging. Existing anti-collision control methods mainly rely on mechanical limit switches or photoelectric sensors installed at the fork tips, and the detection of overcurrent signals from the fork extension motor. When a collision or overload is detected, the system immediately stops the fork movement and issues an alarm, requiring manual intervention.

[0003] However, the above methods have obvious shortcomings: First, the anti-collision action stops at "detection-stop", which cannot actively avoid obstacles, resulting in operation interruption and affecting warehouse operation efficiency; Second, it lacks the ability to perceive the obstruction state in front of the forks and cannot autonomously adjust the movement trajectory to continue to complete the task after a slight contact; Third, the existing methods have not achieved multi-axis linkage avoidance based on contact feedback, which limits the stacker crane's ability to adapt to complex working conditions. Summary of the Invention

[0004] To overcome the aforementioned deficiencies of the prior art, embodiments of the present invention provide a stacker crane fork anti-collision control method, system, and medium. By real-time acquisition of drive data and comparison with the no-load reference curve, it detects minor contact obstruction. After obstruction, it actively executes a trial cycle of retraction, lifting shaft displacement, and low-speed re-probing. It records the coordinates of the obstruction location to construct a local obstruction distribution point set, and then calculates the safety envelope space to determine the insertion area. Finally, it generates a multi-axis interpolation smooth curve that links the fork extension and lifting shaft to complete the collaborative insertion. This solves the problems of existing anti-collision methods that can only stop the machine and alarm, cannot actively avoid obstacles, and lack multi-axis linkage avoidance capabilities.

[0005] To achieve the above objectives, the present invention provides the following technical solution: A stacker crane fork anti-collision control method includes the following steps: acquiring the target pallet position and controlling the fork extension; collecting real-time drive data of the telescopic motor and comparing it with a pre-acquired no-load reference curve to determine whether contact obstruction has occurred; after determining obstruction, controlling the fork retraction, lifting shaft displacement, and low-speed re-probing; recording the coordinates of the obstruction position to construct a local obstruction distribution point set, and calculating the safety envelope space based on the point set to determine the insertable area; generating a multi-axis interpolation smooth curve linking fork extension and lifting shaft based on the insertable area; controlling the fork and lifting mechanism to operate collaboratively along the multi-axis interpolation smooth curve to complete the insertion of the target pallet.

[0006] In a preferred embodiment, obtaining the target pallet position includes: after the stacker crane runs to the front of the target storage location, triggering the vision acquisition device mounted on the loading platform to acquire images of the storage location; extracting features from the acquired images to identify the socket outline feature points of the target pallet; and calculating the three-dimensional offset and attitude deflection angle of the target pallet relative to the forks based on the socket outline feature points and camera calibration parameters to obtain the target pallet position.

[0007] In a preferred embodiment, determining whether contact obstruction has occurred includes: synchronously acquiring the current telescopic position coordinates of the fork and the real-time quadrature shaft current data of the telescopic motor as real-time drive data; extracting the reference quadrature shaft current data corresponding to the current telescopic position coordinates from the no-load reference curve; calculating the absolute value of the deviation between the real-time quadrature shaft current data and the reference quadrature shaft current data; comparing the absolute value of the deviation with a preset dynamic safety threshold; if the absolute value of the deviation is greater than the dynamic safety threshold for N consecutive sampling periods, then it is determined that contact obstruction has occurred.

[0008] In a preferred embodiment, after the obstruction is determined, controlling the fork retraction, lifting shaft displacement, and low-speed re-probing includes: controlling the fork to retract in the opposite direction along the telescopic shaft, so that the fork tip returns to a preset safety buffer distance behind the initial obstruction location; controlling the stacker crane lifting shaft to move a fine-tuning step in the opposite direction of the last probe, so that the lifting shaft alternately moves upward and downward during multiple consecutive re-probing; controlling the fork to re-probe extension at a low speed not exceeding a preset proportion of the initial extension speed; wherein, during multiple consecutive re-probing, the alternating displacement direction of the lifting shaft is used to dynamically scan the obstruction boundary in front of the fork.

[0009] In a preferred embodiment, recording the coordinates of the obstructed location to construct a local obstruction distribution point set includes: obtaining the command position of the fork's telescopic shaft and the command position of the lifting shaft at each time the obstruction is determined; compensating and correcting the command position according to the backlash compensation value of the telescopic shaft and the lifting shaft respectively to obtain the actual physical location coordinates; recording the actual physical location coordinates as an obstruction point, and clustering all obstruction points recorded during multiple re-exploration processes to generate a local obstruction distribution point set.

[0010] In a preferred embodiment, the step of calculating the safety envelope space based on the point set to determine the insertable region includes: fitting and generating an obstacle profile based on the locally obstructed distribution point set; determining the expansion margin based on the current fork extension length and load state; expanding the obstacle profile outward by the expansion margin to construct the safety envelope space; performing a difference operation between the theoretical insertion area of ​​the target pallet and the safety envelope space to obtain an interference-free region; if the interference-free region meets the minimum space required for fork insertion, it is determined as an insertable region.

[0011] In a preferred embodiment, generating a multi-axis interpolation smooth curve for the linkage between the fork extension and the lifting shaft based on the insertable region includes: planning obstacle avoidance nodes according to the insertable region and the outer boundary of the safety envelope space; obtaining the dynamic constraint parameters of the stacker crane's lifting mechanism and extension mechanism, wherein the dynamic constraint parameters include at least the running speed, acceleration, and jerk limit; and generating a multi-axis interpolation smooth curve in which the position, velocity, and acceleration are continuously differentiable in both the spatial and temporal domains, using the obstacle avoidance nodes as control points and the dynamic constraint parameters as boundary conditions.

[0012] In a preferred embodiment, obtaining the dynamic constraint parameters of the stacker crane's lifting mechanism and telescopic mechanism includes: obtaining the current load weight of the forks and the maximum extension length within the insertable area; determining the center of gravity offset based on the load weight and maximum extension length, and calculating the system's equivalent inertia in conjunction with the number of transmission stages; retrieving a preset inertia constraint mapping table, and dynamically generating the speed limit, acceleration limit, and jerk limit corresponding to the current posture based on the system's equivalent inertia; wherein, the larger the system's equivalent inertia, the smaller the acceleration limit and jerk limit.

[0013] A stacker crane fork anti-collision control system includes: a state-sensing hardware terminal for acquiring the three-dimensional spatial coordinates of a target pallet and simultaneously collecting the position of the fork's telescopic shaft and the real-time drive data of the telescopic motor; a multi-axis drive execution terminal including a telescopic shaft servo motor and its reduction mechanism for driving the fork movement, and a lifting shaft servo motor and its transmission mechanism for driving the loading platform movement; and an anti-collision control master station, which is communicatively connected to both the state-sensing hardware terminal and the multi-axis drive execution terminal. The anti-collision control master station stores a control program, which, when executed by a processor, implements the method described above.

[0014] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method described in any of the preceding claims.

[0015] The technical effects and advantages of the stacker crane fork anti-collision control method, system and medium of the present invention are as follows: 1. This invention compares real-time drive data with an unloaded baseline curve to determine contact obstruction, and actively controls fork retraction, lifting shaft displacement, and low-speed re-probing after obstruction. This achieves proactive perception and avoidance of minute contact, solving the problem that existing methods can only stop the machine and issue an alarm but cannot make autonomous adjustments.

[0016] 2. This invention constructs a local obstruction distribution point set by recording the coordinates of the obstruction location, calculates the safety envelope space to determine the insertion area, and generates a multi-axis interpolation smooth curve that links the fork extension and lifting shaft to complete the insertion collaboratively. This realizes multi-axis active avoidance based on contact feedback, improving the stacker crane's adaptability to complex working conditions and the insertion success rate. Attached Figure Description

[0017] Figure 1 This is an overall flowchart of a stacker crane fork anti-collision control method provided in an embodiment of the present invention; Figure 2 This is a structural block diagram of a stacker crane fork anti-collision control system provided in an embodiment of the present invention; Figure 3 This is a schematic diagram illustrating the principle of comparing the cross-axis current with the dynamic safety threshold and determining the de-jitter in an embodiment of the present invention. Figure 4 This is a schematic diagram illustrating the geometric principle of constructing a secure envelope space and an interference-free region based on a set of locally obstructed distribution points in an embodiment of the present invention. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0019] Example 1, Figure 1 The present invention provides a method for preventing collisions with stacker crane forks, comprising the following steps: S1, Obtain the target pallet position and control the forks to extend; In this embodiment, the specific process of obtaining the target pallet position is as follows: S101: After the stacker crane reaches the target storage location, it triggers the vision acquisition device mounted on the loading platform to acquire images of the storage location. In practice, the anti-collision control master station (such as a PLC) reads the coordinates of the stacker crane's traveling and lifting axes in real time via encoders. When the actual coordinates reach the preset theoretical coordinate window of the target storage location (e.g., within the allowable error range of ±5mm), the master station sends a high-level trigger pulse lasting 50ms to the 3D vision camera mounted above the fork base of the loading platform via industrial Ethernet. After receiving the pulse, the camera completes exposure within 10ms. To overcome the light interference of the storage environment, the vision camera turns on its built-in infrared fill light or structured light projector at the moment of exposure synchronization, outputting an RGB color image with a resolution of 1920×1080 and an aligned depth map point cloud matrix, which is then transmitted to the vision processing industrial control computer via gigabit Ethernet.

[0020] S102, Feature extraction is performed on the acquired image to identify the outline feature points of the sockets on the target tray. Specifically, this step includes the following data processing sub-processes: The acquired 2D grayscale image is denoised by Gaussian filtering, and the region of interest (ROI) containing the front of the pallet is extracted from the full-resolution image based on the theoretical prior position of the target pallet in the current storage location, in order to filter out the interference of the background shelves and reduce the amount of computation.

[0021] A deep learning object detection model (such as YOLOv8) pre-trained on a large number of real tray images is used to perform forward inference on the ROI image and output bounding boxes that define the left and right holes respectively.

[0022] Inside the bounding box, the Shi-Tomasi algorithm is used to obtain the initial corner pixel coordinates of the jack outline. Subsequently, a second-order gradient method based on Taylor expansion is used for sub-pixel-level iterative approximation, with the iteration termination condition being the coordinate change. Pixels, ultimately outputting high-precision pixel coordinates of the four vertices of the jack. They are used as the feature points of the socket outline.

[0023] S103, based on the socket contour feature points and camera calibration parameters, calculates the three-dimensional offset and attitude deflection angle of the target pallet relative to the forks to obtain the position of the target pallet.

[0024] Specifically, the camera calibration parameters include the camera's intrinsic parameter matrix (including focal length and principal point coordinates) obtained in advance through Zhang Zhengyou's calibration method, and the extrinsic parameter transformation matrix from the camera coordinate system to the stacker crane fork reference coordinate system obtained through calibration wrench eye calibration.

[0025] Extract feature point pixel coordinates Then, combine the corresponding depth values ​​from the depth image. Convert it to three-dimensional coordinates in the camera coordinate system. The transformation formula is:

[0026] in, This is the intrinsic parameter matrix of a pre-calibrated 3×3 camera.

[0027] Subsequently, the pre-stored hand-eye calibration extrinsic parameter matrix is ​​retrieved. This matrix is ​​a 4×4 homogeneous transformation matrix, representing the rotation and translation relationship from the camera's optical center to the physical zero point of the fork (reference coordinate system). Convert to coordinates in the fork reference coordinate system :

[0028] The three-dimensional position offset of the target pallet is obtained by calculating the difference between the center coordinates of the two sockets and the zero point of the fork. And spatial attitude yaw angles (roll, pitch, yaw).

[0029] After receiving the high-precision pose data, the anti-collision control master station compensates for the three-dimensional offset in the initial extension command of the forks. Combining this with the fork tip reference point in the stacker crane's current base coordinate system, it calculates the actual position of the target pallet. Then, the master station drives the extension shaft servo motor to start, controlling the forks to extend precisely towards the center of the target pallet's insertion hole at a preset initial speed (e.g., 0.2~0.5m / s), and proceeds to the subsequent contact obstruction detection step.

[0030] S2, collect real-time drive data of the telescopic motor and compare it with the pre-acquired no-load reference curve to determine whether contact obstruction has occurred. like Figure 3 The diagram shown is a schematic representation of the principle of comparing the cross-axis current with the dynamic safety threshold and determining the de-jitter in an embodiment of the present invention.

[0031] In this embodiment, S2 is executed continuously throughout the entire fork extension process to detect in real time whether there is any slight contact obstruction at the fork tip. The specific implementation steps are as follows: S201, when the forks begin to extend towards the target pallet, the anti-collision control master station initiates a high-speed data acquisition task. The sampling frequency is set to 1kHz (i.e., once every 1 millisecond). The acquired data includes the current telescopic position coordinates and the real-time quadrature-axis current data of the telescopic motor.

[0032] The current scaling position coordinates The resolution is preferably 0.1mm to 1mm, which is obtained by directly reading the encoder at the tail of the telescopic shaft servo motor or by an external grating ruler to match the mechanical transmission accuracy of the gear rack of the heavy stacker crane.

[0033] The real-time quadrature axis current data The value is obtained through the vector control unit of the servo driver. This value can be read directly through the driver's analog output port (e.g., ±2 times the rated torque current corresponding to -10V to +10V) or fieldbus (e.g., EtherCAT), with a reading delay of no more than 250μs.

[0034] The main station will collect each time As real-time driving data, it is stored in a circular buffer in chronological order, with a buffer depth of no less than 2000 points (corresponding to 2 seconds of data) to prevent data overflow.

[0035] S202, the no-load reference curve is obtained through actual no-load operation during the stacker crane commissioning phase and pre-stored in the main station's non-volatile memory. Its acquisition and retrieval methods are as follows: The forks are controlled to extend and retract completely three times under no-load conditions at a standard speed (e.g., 0.3 m / s). During each extension, the coordinates of the extension and retraction positions and the corresponding cross-axis current data are recorded synchronously, with a sampling frequency of 1 kHz.

[0036] The three data points are interpolated and aligned according to the scaling position coordinates. Then, the average current value of each position point is taken and the median filter is applied (window width 5 points) to obtain a discrete curve with the scaling position coordinates as the independent variable and the reference cross-axis current data as the dependent variable. The curve is stored in array form, with the interval between adjacent points not exceeding 1 mm.

[0037] During runtime, for the currently acquired real-time scaling position coordinates The main site searches for the empty reference curve array that matches... The two closest locations and (satisfy The reference quadrature-axis current data corresponding to this position is calculated using linear interpolation:

[0038] In the formula, This is the reference quadrature-axis current in the current telescoping position coordinates. Position in the no-load reference curve The corresponding reference quadrature-axis current value, Position in the no-load reference curve The corresponding reference quadrature-axis current value, and In the array of unloaded reference curves, and The coordinates of the two closest adjacent positions.

[0039] like If the value is less than the minimum position of the curve or greater than the maximum position, then the boundary value is taken.

[0040] S203, Real-time deviation absolute value Defined as:

[0041] The absolute value of this deviation reflects the additional increase in resistance relative to when unloaded. For example, when the forks touch a pallet or obstacle, the motor needs to output greater torque, causing the real-time quadrature-axis current data to rise and the absolute value of the deviation to increase sharply.

[0042] S204, the preset dynamic security threshold It is not a single constant, but an adaptive value that changes with the scaling position. Its setting method is as follows: During the commissioning phase, while acquiring the no-load reference curve, the standard deviation at each expansion / contraction position coordinate is calculated. (Using data from 3 no-load runs). The dynamic safety threshold is set as follows:

[0043] In the formula, This is a preset coefficient, with a value range of 3 to 5. In this embodiment, it is set to 4.

[0044] This threshold is also stored as a discrete array with the scaling position coordinates as independent variables. At runtime, a dynamic safety threshold corresponding to the current scaling position coordinates is obtained through linear interpolation. .

[0045] S205, at each sampling time, will Dynamic security threshold corresponding to the current location Compare and judge Is it greater than .

[0046] To avoid false triggering caused by occasional current spikes due to transient electromagnetic noise or vibration, this embodiment uses a threshold exceeding criterion for N consecutive sampling periods. Setting: Sampling period Number of consecutive sampling periods (Can be adjusted between 2 and 5). Maintain a sliding Boolean array of length N. Initially, all values ​​are 0.

[0047] Within each sampling period, the comparison result is written: if the absolute value of the deviation in the current period is greater than the dynamic safety threshold, then the current period is... Set to 1; otherwise set to 0.

[0048] Check after each update Are all values ​​in the range 1? If yes, that is, the absolute value of the deviation is greater than the dynamic safety threshold for N consecutive sampling periods, then it is determined that contact obstruction has occurred; if not, it continues to extend normally, and it is determined that no contact obstruction has occurred.

[0049] S206: When contact obstruction is detected, immediately record the current telescopic position coordinates and the lifting shaft position coordinates, and issue an "obstruction sign" signal. At the same time, stop the currently executing normal extension movement.

[0050] If no obstruction is detected during the entire extension process, the forks will insert normally into the pallet slots, and the pickup will be completed according to the standard procedure.

[0051] S3, after determining that the obstruction is encountered, control the fork retraction, the lifting shaft displacement and low-speed re-probe; In this embodiment, after the determination of obstruction, the fork retraction, lifting shaft displacement, and low-speed re-probing are controlled, specifically including the following steps: S301, Read the current position coordinates of the telescopic axis. (Unit: mm) and coordinates of the hoisting shaft position (Unit: mm), serving as the "first point of contact obstruction". This coordinate value will be output to subsequent S4 to construct a set of local obstruction distribution points.

[0052] Issue a rapid deceleration command (e.g., set the deceleration to 1.0 m / s²) to stop the forks from moving forward in the shortest possible time, thus preventing further crushing of obstacles.

[0053] Set the number of attempts variable Maximum number of attempts allowed (This value can be adjusted between 3 and 8 depending on the actual operating conditions). Maintain a trial direction variable. The default initialization is when the first block is encountered. (Represents upward).

[0054] S302 controls the forks to retract in the opposite direction along the telescopic axis, so that the front end of the forks returns to a preset safe buffer distance behind the position where the first contact obstruction occurred.

[0055] Retreat to target position Calculate using the following formula:

[0056] In the formula, The preset safety buffer distance ranges from 5mm to 20mm, and 10mm is preferred in this embodiment. This distance can completely eliminate residual mechanical contact caused by brake overshoot; The fork extension direction is indicated by a symbol (+1 for forward extension and -1 for reverse extension) to ensure compatibility with different coordinate system definitions.

[0057] The retraction of the telescopic shaft is controlled by a trapezoidal speed curve, with the maximum retraction speed not exceeding 0.2 m / s, to ensure a smooth and shock-free process.

[0058] Upon arrival The pullback is complete when the absolute value of the deviation is less than ±0.5mm.

[0059] S303, after the retraction is completed, the main station controls the stacker crane's lifting shaft to move in the opposite direction of the previous probe, causing the lifting shaft to alternately move upward and downward during multiple re-probes, gradually expanding the scanning range. The specific implementation is as follows: The target position of the hoist shaft is the position where it first encounters resistance. Based on the current height, the system expands alternately to the upper and lower sides by increasing offsets, rather than accumulating based on the current height, thus avoiding a dead loop of repeated probing.

[0060] Target height Calculation formula:

[0061] In the formula, This represents the current number of attempts (counting from 1). Direction symbol: when When the number is odd, add 1 (probe upwards). If the number is even, take -1 (trial and error). express Round up to the nearest integer, i.e., a multiple of the offset. To fine-tune the step size, the value ranges from 0.5mm to 2mm, and 1mm is used in this embodiment.

[0062] To illustrate the alternating detection sequence more clearly, Table 1 shows the results. , For example, the target height of each probe and its relationship with the first obstruction point.

[0063] Table 1 Examples of detection sequences for progressively expanding the scanning range of the hoisting shaft

[0064] Therefore, starting from the initial obstruction height, the lifting shaft gradually expands the scanning range outward, scanning at 1, 2, 3 times the step length on both the upper and lower sides... until a suitable insertion position is found or the maximum number of attempts is reached.

[0065] S304, after the lifting shaft displacement is completed, control the forks to re-extend at a low speed not exceeding a preset proportion of the initial extension speed. The specific implementation is as follows: First extension speed The speed at which the forks first extend towards the target pallet, in this embodiment... Preset ratio The value ranges from 10% to 30%, and in this embodiment, it is set to 20%. Therefore, the low-speed reprobe velocity... :

[0066] The purpose of low speed is to reduce the impact force when contacting the obstacle again, while improving the signal-to-noise ratio of the current signal, making it easier to accurately determine whether contact obstruction has occurred again.

[0067] Control the telescopic shaft at speed Extend forward again from the current retraction position, while the contact obstruction detection logic of S2 continues to operate.

[0068] S305, if contact obstruction occurs again during low-speed re-probing, record the coordinates of the current obstruction location as the new obstruction point. Probe counter. Accumulation, if Return to step S302 and execute the trial loop again; if If the insertion fails, the task will be terminated.

[0069] If the forks successfully overcome the obstacle and enter the target pallet slot during the low-speed re-probing process without encountering any contact or obstruction, it indicates that the forks have successfully passed the obstacle and entered the target pallet slot. The subsequent pickup process is then completed according to standard procedures, and the S3 collision avoidance and obstacle avoidance process is successfully concluded.

[0070] S306, the lifting shaft alternately moves upward and downward during multiple re-probing attempts, with the scanning range gradually expanding. When the fork tip encounters an obstacle, the distribution range of the obstacle in the height direction can be detected by changing the height of the lifting shaft and re-probing. Table 2 further shows the results of each probe during the above-mentioned alternating detection process and their corresponding physical meanings.

[0071] Table 2 Examples of the physical meaning of dynamic scanning of obstructed boundaries

[0072] Through these alternating and progressively expanding trials, the system can gradually depict the distribution range of obstacles in the height direction, rather than being limited to the initial obstruction height.

[0073] S4, record the coordinates of the obstructed location to construct a local obstructed distribution point set, and calculate the safe envelope space based on the point set to determine the insertable region; Combination Figure 4 The diagram shown illustrates the geometric principle of constructing a secure envelope space and an interference-free region based on a locally obstructed distribution point set in an embodiment of the present invention.

[0074] In this embodiment, the step of recording the coordinates of the obstructed location to construct a local obstruction distribution point set, and calculating the safe envelope space based on the point set to determine the insertable region, specifically includes the following steps: S401, when a contact obstruction is detected, read the current telescopic shaft command position. (Unit: mm) and hoist shaft command position (Unit: mm). The command position refers to the position command value issued by the master station to the servo drive, rather than the actual position value fed back by the encoder. The reason for using the command position is that: the command position is not affected by physical factors such as mechanical backlash and flexible deformation, and is repeatable; at the same time, backlash compensation is precisely to correct the deviation between the command position and the actual physical position.

[0075] S402. Due to the inherent backlash in the mechanical transmission chain of the stacker crane (gear and rack, lead screw and nut, reducer gear meshing, etc.), a deviation will occur between the commanded position and the actual physical position when the direction of movement changes (e.g., the forks switch from extended to retracted, or the lifting shaft switches from rising to falling). Therefore, backlash compensation correction is required for the commanded position before recording the obstruction point.

[0076] For the telescopic shaft: Read the current real-time movement direction of the forks. The +1 indicates that the direction is extending (forward), and the -1 indicates that the direction is retracting (reverse). This direction can be obtained through the sign of the speed command or encoder differential.

[0077] For the hoisting shaft: Read the current real-time direction of motion. The value is 1, where +1 indicates an increase and -1 indicates a decrease.

[0078] The backlash compensation value of the telescopic shaft The calibration is obtained in advance through offline calibration. The calibration method is as follows: During the stacker crane commissioning phase, the telescopic shaft is controlled to move to multiple target positions in both the forward and reverse directions. The deviation between the commanded position and the actual physical position is measured, and a two-dimensional compensation mapping table associated with the movement direction and travel position is generated. This table is stored in the controller's non-volatile memory, with adjacent position nodes spaced no more than 50 mm apart.

[0079] Hoist shaft backlash compensation value Similarly, a two-dimensional compensation mapping table with position and orientation as independent variables is obtained through offline calibration.

[0080] Furthermore, the actual physical positions of the telescopic shaft and the lifting shaft are as follows:

[0081]

[0082] In the formula, This refers to the actual physical position of the telescopic shaft. This refers to the actual physical position of the hoisting shaft. This is the direction sign function.

[0083] S403, the actual physical location coordinates obtained after compensation and correction As a point of obstruction.

[0084] When the trial cycle of S3 terminates (i.e., the maximum number of trials is reached or an insertion position is successfully found), all blocked points are clustered to generate the final set of locally blocked distribution points.

[0085] If the number of blocked points is less than 3, no clustering is needed, and all points can be used directly as the local blocked distribution point set. If the number of points is greater than or equal to 3, the following clustering steps are performed.

[0086] S403-1, This embodiment uses a neighborhood merging clustering algorithm based on spatial distance thresholds. The specific steps are as follows: Set a spatial distance threshold (in mm), with a range of 5 mm to 15 mm; in this embodiment, 10 mm is used. This threshold should be greater than the position measurement error of a single obstruction point (typically ±1 mm) and less than the minimum feature size of the tray socket (typically the socket width is 30 mm to 60 mm) to ensure that multiple obstruction points caused by the same obstacle can be grouped into one category, while points from different obstacles will not be incorrectly merged.

[0087] For any two blocked points and Calculate its Euclidean distance ,like If the distance is less than or equal to the spatial distance threshold, then it is considered... and Adjacent.

[0088] Starting from the first point, a breadth-first search or depth-first search algorithm is used to group all points connected by adjacency into the same cluster. Each cluster represents a region of physical obstacle obstruction on the stretch-lift plane.

[0089] For each cluster, calculate the geometric center (centroid) of all its points as the representative obstruction point of the obstacle. If a cluster contains only one point (i.e., the distance between this point and all other points is greater than the spatial distance threshold), it is considered an isolated noise point and discarded, and not included in subsequent calculations.

[0090] S403-2 defines the set of all representative blocked points as the local blocked distribution point set.

[0091] S404, Based on a locally obstructed distribution point set, this embodiment uses the AlphaShapes algorithm (a geometric contour extraction algorithm based on point sets) to fit the obstacle contour. The core idea of ​​this algorithm is: given a radius parameter... (Unit: mm) A generalized shape is constructed on the set of locally obstructed distribution points, and the outer boundary of this shape is extracted as the obstacle profile. The radius parameter... The value of should be greater than the average spacing between points in the point set, and less than 1 / 5 of the overall size of the point set, in order to ensure a smooth contour and to avoid losing local features.

[0092] For scenarios where the number of locally obstructed distribution points is small, the minimum circumscribed convex polygon of the point set can be directly used as the obstacle outline.

[0093] S405, the expansion margin (Unit: mm) refers to the distance by which the outline of an obstacle is extended outwards. Its purpose is to provide a safe margin for the actual physical dimensions of the forks (width and height) and dynamic deviations during movement (such as deflection and vibration), ensuring that the forks do not physically contact the obstacle during movement. The method for determining this margin is as follows: Read the current fork extension position coordinates, subtract the zero point position of the extension shaft, and obtain the current extension length. Simultaneously, obtain the current load weight. .

[0094] Based on the geometric half-width of the fork tip (Unit: mm) and half height (Unit: mm) Set the basic expansion amount for:

[0095] In the formula, For safety margin (the value ranges from 3mm to 8mm, and 5mm is used in this embodiment).

[0096] As the fork extension length increases, the fork tip will experience sagging deflection due to its own weight and load. The deflection is proportional to the cube of the extension length and the load weight.

[0097] In the formula, This is the deflection compensation amount. Maximum extension length (rated) and rated load weight Maximum deflection, This is the current extension length. This represents the current load weight.

[0098] The expansion margin Basic expansion With deflection compensation sum.

[0099] S406, For each vertex on the obstacle profile, calculate the average value of the outward normal directions of its adjacent edges, using this as the outward normal direction vector of that vertex, and move the vertex along the direction of the outward normal direction vector. The distance is used to obtain the expanded vertices. Connecting all the expanded vertices forms the boundary of the safe envelope space. The closed region enclosed by the boundary is defined as the safe envelope space.

[0100] S407, the theoretical insertion area of ​​the target pallet is defined as centered on the center of the target pallet insertion hole, and extending outwards to the cross-sectional dimension (width) of the fork tip. and height The area is a rectangular region bounded by (). This region represents the final position that the fork ends need to reach.

[0101] The difference operation is specifically as follows:

[0102] In the formula, This represents the theoretical insertion area for the target tray. The safety envelope space is defined as the area where the forks can pass freely without interference, obtained by subtracting the safety envelope space from the theoretical insertion area of ​​the target pallet. The difference operation can specifically employ polygon clipping algorithms (such as the Sutherland-Hodgman algorithm or the Weiler-Atherton algorithm), inputting two polygons ( and ), outputting the clipped, interference-free polygon.

[0103] S408, Calculation Find the width of the largest inscribed rectangle (with the stretching direction as the longer side and the rising direction as the shorter side). and height If the following conditions are met:

[0104] If an insertion region is found, it is determined that an insertion failure occurs; if the condition is not met and the maximum number of attempts has been reached, an insertion failure alarm is triggered.

[0105] After the judgment is passed, the centerline method (based on Voronoi diagram) is used to extract a continuous spatial line connecting the current starting point of the fork and the ending point of the target pallet socket from the non-interference region. This line is discretized to form a path point sequence, which is then output as an insertable region with physical passage width. It should be noted that the insertable region is geometrically a path with width constraints (i.e., a safety passage), not an arbitrary two-dimensional region. Each point on this path satisfies the requirement of maintaining at least a certain distance from the boundary of the safety envelope space. Conditions for safe distance.

[0106] S5, based on the insertable region, generates a multi-axis interpolation smooth curve that links the fork extension and lifting shaft; In this embodiment, generating a multi-axis interpolation smooth curve that links fork extension and lifting shaft based on the interpolable region specifically includes the following steps: S501, Determine the start and end points of the path. The current actual position of the fork tip on the telescopic-lifting two-dimensional plane. That is the starting point, where This represents the current coordinates of the telescopic axis position (in mm). This is the current position coordinate of the lifting shaft (in mm). This position is obtained in real time from the encoder feedback.

[0107] Position of the center of the target tray socket on the telescopic-lifting two-dimensional plane That is the endpoint. The coordinates of the telescopic axis position when the forks are fully inserted into the pallet insertion holes to the predetermined depth (calculated by combining the pallet position obtained from visual positioning in S1 with the mechanical dimensions, in mm). The coordinates (in mm) of the lifting shaft position corresponding to the center height of the socket.

[0108] By combining the path point sequence of the insertable region with the outer contour of the safe envelope space, key points with significant curvature changes in the path are selected as obstacle avoidance nodes. For each point in the path point sequence, calculate the path curvature at that point. :

[0109] In the formula, For path number Curvature at a point, For path number At each point, the first-order difference of the scaling position (the difference between the two points before and after). For path number The first-order difference of the lifting position at each point. For path number The second-order difference of the scaling position at each point. For path number The second difference of the lifting position at each point.

[0110] Local maxima of curvature (i.e., points where the path bends most sharply) are extracted as candidate obstacle avoidance nodes. If there are fewer than 3 candidate nodes, intermediate nodes are added at the 1 / 3 and 2 / 3 positions of the path to ensure that the curve has sufficient degrees of freedom for control.

[0111] Perform a safety check on candidate nodes: Check whether each candidate node maintains a safety distance of at least 2 mm from the boundary of the safety envelope space. If not, move the node along the path normal direction towards the center of the insertable region until the condition is met.

[0112] Finally obtained Obstacle avoidance nodes , ,in .

[0113] Starting point and the end point Merging with obstacle avoidance nodes forms a complete control point sequence:

[0114] S502 retrieves the current load weight of the forks and their maximum extension length within the insertable area. Load weight Real-time quadrature current of the telescopic shaft servo motor Combining motor torque constant The estimation was obtained. The estimation method is as follows: During the no-load calibration phase, record the quadrature-axis current values ​​corresponding to each extension / retraction position under no-load conditions. During real-time operation, the current scaling position is read. and the corresponding real-time quadrature-axis current Estimated load weight:

[0115] In the formula, This represents the real-time quadrature-axis current at the current telescoping position. The current is the negative no-load quadrature-axis current at the current extension / retraction position. It is the acceleration due to gravity. The effective length of the lever arm is the distance from the center of gravity of the load to the center of rotation of the telescopic shaft.

[0116] Maximum extension length Extracted from the complete control point sequence, it equals the maximum value of the stretch position coordinates of all points on the path minus the stretch coordinates of the starting point.

[0117] The center of gravity offset is determined based on the load weight and maximum extension length. This embodiment uses a simplified engineering model:

[0118] In the formula, The stacker crane structural constant (unit: mm / (kg·mm)) is obtained through offline finite element analysis or experimental calibration, reflecting the forward shift of the center of gravity caused by a unit load per unit extension length. This represents the offset of the center of gravity.

[0119] The equivalent inertia of the system (Unit: kg·m²) is the total moment of inertia referred to the motor shaft, including both fixed and variable components:

[0120] In the formula, The fixed inertia portion includes the inertia of stationary components such as the motor rotor, reducer, and transmission gears. This inertia is obtained by consulting the equipment manual or through offline testing. The variable inertia part, which is related to the load weight and the extension length, is calculated using the following formula:

[0121] In the formula, The number of transmission stages (i.e., the total transmission ratio) is dimensionless. The coefficient representing the effect of center of gravity shift on inertia (unit: kg / m²) was obtained through offline calibration. for The value after conversion to meters.

[0122] S503, retrieve the preset inertia constraint mapping table. The inertia constraint mapping table is a two-dimensional lookup table pre-established during the stacker crane commissioning phase, with the horizontal axis representing the system's equivalent inertia. (Unit: kg·m²), with the vertical axis representing the corresponding dynamic constraint parameters (velocity limits). Acceleration limits accelerometer limits The mapping table is created as follows: During the commissioning phase, the maximum safe speed, acceleration, and jerk were experimentally determined under different system equivalent inertia conditions (achieved by varying the combination of load weight and extension length). The measurement standard was: under the corresponding operating conditions and with these constraint parameters, the vibration amplitude at the top of the stacker crane column should not exceed 0.5 mm, and the servo drive should not trigger an overload alarm. The measurement results were stored in tabular form, with adjacent... The node spacing shall not exceed 0.1 kg·m².

[0123] The equivalent inertia of the current system calculated based on S502 Linear interpolation is performed in the mapping table to obtain the dynamic constraint parameters specific to the current attitude, including the velocity limit. Acceleration limits and accelerometer limits The larger the system's equivalent inertia, the smaller the acceleration limit and jerk limit.

[0124] S504, control point sequence As input, a cubic B-spline curve is used for interpolation fitting. The mathematical expression for the B-spline curve is:

[0125] In the formula, For B-spline curves in parameters Position vector at , The parameters for the normalized curve are dimensionless and range from [0,1]. For control point index, For the first There are 1 control point (corresponding to the control point sequence). This indicates that the total number of control points has decreased by one. Indicates the degree of the B-spline. For the first indivual The B-spline basis functions are calculated using the Cox-deBoor recursive formula:

[0126]

[0127] The generated B-spline curve In the spatial domain, position, velocity, and acceleration are naturally continuous. However, spatial continuity does not equate to temporal continuity—a smooth path in space will still produce impacts if traveled at uneven speeds. Therefore, time allocation is also necessary.

[0128] Furthermore, time information needs to be allocated to ensure that the actual motion meets the dynamic constraints in S503.

[0129] First, calculate the total arc length of the path:

[0130] In the formula, Let be the total arc length of the path. Scaling components of B-spline curves with respect to parameters The first derivative, For the lifting component of the B-spline curve with respect to parameters The first derivative, This represents the differential increment of the curve parameters.

[0131] Speed ​​limit Acceleration limits and accelerometer limits Given the boundary conditions, plan an S-shaped velocity curve. The core characteristics of the S-shaped velocity curve are: smooth velocity change (no abrupt changes), divided into seven stages: acceleration, uniform acceleration, deceleration, uniform speed, acceleration / deceleration, uniform deceleration, and deceleration / deceleration, ensuring continuous acceleration and bounded jerk.

[0132] The planned S-shaped speed curve (Unit: mm / s) and spatial path Coupling, establishing parameters With time Mapping relationship:

[0133] In the formula, Indicates time The corresponding curve parameter values, For time variables, For the curve in parameters The magnitude of the velocity vector at that point (i.e., the derivative of the path arc length with respect to the parameter). Let the curve parameters be the integral variables. For the velocity curve in time The velocity value at that location, This is the time integral variable. That is, the cumulative arc length along the path equals the time integral of the velocity curve.

[0134] The final position-time function of the telescopic shaft and the hoisting shaft is obtained:

[0135] Check curve Is the node completely within the safe envelope space? If a point is outside the safe envelope space, return to S501, increase the number of obstacle avoidance nodes or adjust the node positions and regenerate.

[0136] examine and Are the first derivative (velocity) and second derivative (acceleration) with respect to time continuous, and do their peak values ​​not exceed the velocity limits, respectively? Acceleration limits and accelerometer limits .

[0137] After verification, and The data is stored in the interpolation buffer in time series format and read and executed by the servo driver at time intervals.

[0138] S6 controls the forks and lifting mechanism to work together along a multi-axis interpolation smooth curve to complete the insertion and removal of the target pallet.

[0139] In this embodiment, the control forks and lifting mechanism operate in coordination along a multi-axis interpolation smooth curve to complete the insertion and removal of the target pallet, specifically including the following steps: First, the instruction data in the interpolation buffer is checked for integrity to confirm that the time series is continuous, all position commands are within the mechanical limit range, and the timestamps of the two axes are perfectly aligned. After the check passes, the master station simultaneously sends position commands to the telescopic axis servo driver and the lifting axis servo driver via the EtherCAT bus in a periodic synchronous communication manner. The communication period is consistent with the interpolation time interval (1ms in this embodiment), and a distributed clock mechanism is used to ensure that the synchronization error of the two axes does not exceed ±1μs.

[0140] When the curve reaches its endpoint, the master station waits for the actual positions of both axes to enter the endpoint target window (±0.5mm) and remain stable for at least 50ms before executing the following picking action: Control the lifting axis to rise slightly (e.g., 5mm) so that the forks lift the pallet away from the shelf support surface; After a 100ms delay, the telescopic shaft is controlled to retract in the opposite direction at a standard speed (0.3m / s), pulling the pallet out of the storage location and onto the loading platform; Send a "successful pickup" status signal to the host computer, clear temporary variables, and release resources.

[0141] During the execution of S6, the contact obstruction detection logic of S2 continues to operate with reduced sensitivity. If contact obstruction is detected again during curve tracking, the master station immediately stops moving and re-triggers the S3 to S5 process to replan the avoidance path. If the tracking deviation exceeds the safety threshold (±5mm), an emergency stop is immediately initiated and a fault is reported.

[0142] Example 2: A stacker crane fork anti-collision control system, such as Figure 2 As shown, it includes: The status-aware hardware is used to obtain the three-dimensional spatial coordinates of the target pallet and simultaneously collect the position of the fork extension shaft and the real-time drive data of the extension motor. The multi-axis drive actuator includes a telescopic shaft servo motor and its reduction mechanism for driving the fork movement, and a lifting shaft servo motor and its transmission mechanism for driving the loading platform movement. The collision avoidance control master station is communicatively connected to the state perception hardware terminal and the multi-axis drive execution terminal. The collision avoidance control master station stores a control program. When the control program is executed by the processor, it implements any of the implementation methods in Embodiment 1.

[0143] Example 3: A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements any of the embodiments in Example 1.

[0144] The above formulas are all dimensionless calculations. The formulas are derived from software simulations based on a large amount of collected data to obtain the most recent real-world results. The preset parameters in the formulas are set by those skilled in the art according to the actual situation.

[0145] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, in the form of a computer program product.

[0146] Those skilled in the art will recognize that the modules and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0147] In addition, the functional modules in the various embodiments of this application can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module.

[0148] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0149] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for preventing collisions with stacker crane forks, characterized in that, Includes the following steps: Obtain the target pallet position and control the fork extension; Collect real-time drive data of the telescopic motor and compare it with the pre-acquired no-load reference curve to determine whether contact obstruction has occurred. After determining that the obstruction is detected, control the forks to retract, the lifting shaft to move, and then re-test at low speed; Record the coordinates of the obstructed locations to construct a local obstruction distribution point set, and calculate the safe envelope space based on the point set to determine the insertable region; Based on the insertable region, a multi-axis interpolation smooth curve is generated to link the fork extension and lifting axis. The control forks and lifting mechanism work together along a multi-axis interpolation smooth curve to complete the insertion and removal of the target pallet.

2. The method according to claim 1, characterized in that, The process of obtaining the target tray location includes: After the stacker crane moves to the front of the target storage location, it triggers the vision acquisition device mounted on the loading platform to acquire images of the inside of the storage location. Feature extraction is performed on the acquired images to identify the outline feature points of the sockets on the target tray; Based on the socket contour feature points and camera calibration parameters, the three-dimensional offset and attitude deflection angle of the target pallet relative to the forks are calculated to obtain the position of the target pallet.

3. The method according to claim 2, characterized in that, The determination of whether contact obstruction has occurred includes: The current telescopic position coordinates of the forks and the real-time quadrature shaft current data of the telescopic motor are collected synchronously as real-time drive data; Extract the reference quadrature-axis current data corresponding to the current telescopic position coordinates from the no-load reference curve; Calculate the absolute value of the deviation between the real-time quadrature-axis current data and the reference quadrature-axis current data; The absolute value of the deviation is compared with the preset dynamic safety threshold. If the absolute value of the deviation is greater than the dynamic safety threshold for N consecutive sampling periods, it is determined that contact obstruction has occurred.

4. The method according to claim 3, characterized in that, After the determination is blocked, the forks are controlled to retract, the lifting shaft is displaced, and the object is re-entered at low speed, including: Control the forks to retract in the opposite direction along the telescopic axis, so that the front end of the forks returns to the preset safe buffer distance behind the position where the first contact and obstruction occurred; Control the stacker crane's lifting shaft to move one fine-tuning step in the opposite direction of the last probe, so that the lifting shaft alternates between upward and downward displacement during multiple consecutive probes; Control the forks to re-extend at a low speed not exceeding a preset ratio of the initial extension speed; In the process of repeated re-exploration, the alternating displacement direction of the lifting shaft is used to dynamically scan the obstructed boundary in front of the forks.

5. The method according to claim 4, characterized in that, The process of recording the coordinates of the obstructed locations to construct a set of locally obstructed distribution points includes: Obtain the fork extension shaft command position and lifting shaft command position each time the contact is blocked; Based on the backlash compensation values ​​of the telescopic shaft and the lifting shaft, the commanded position is compensated and corrected to obtain the actual physical position coordinates; The actual physical location coordinates are recorded as an obstruction point. After clustering all obstruction points recorded during multiple re-explorations, a local obstruction distribution point set is generated.

6. The method according to claim 5, characterized in that, The step of calculating the safe envelope space based on the point set to determine the insertable region includes: Obstacle contours are generated by fitting a set of locally obstructed distribution points. Determine the expansion margin based on the current fork extension length and load condition; Expand the obstacle outline outwards by an allowance to construct a safe envelope space; Perform a difference operation between the theoretical insertion area of ​​the target pallet and the safety envelope space to obtain the non-interference area. If the non-interference area meets the minimum space required for fork insertion, it is determined as an insertable area.

7. The method according to claim 6, characterized in that, The generation of a multi-axis interpolation smooth curve based on the interpolable region, linking fork extension and lifting shaft, includes: Based on the insertable region and the outer boundary of the safety envelope space, obstacle avoidance nodes are planned; Obtain the dynamic constraint parameters of the stacker crane's lifting mechanism and telescopic mechanism, wherein the dynamic constraint parameters include at least the running speed, acceleration, and jerk limit; Using the obstacle avoidance node as the control point and the dynamic constraint parameters as the boundary conditions, a multi-axis interpolation smooth curve with continuously differentiable position, velocity and acceleration in both the spatial and temporal domains is generated.

8. The method according to claim 7, characterized in that, The acquisition of the dynamic constraint parameters of the stacker crane's lifting mechanism and telescopic mechanism includes: Get the current load weight of the forks and the maximum extension length within the insertable area; The center of gravity offset is determined based on the load weight and maximum extension length, and the equivalent inertia of the system is calculated in conjunction with the number of transmission stages. The preset inertia constraint mapping table is retrieved, and the velocity limit, acceleration limit, and jerk limit corresponding to the current attitude are dynamically generated based on the system's equivalent inertia. The larger the system's equivalent inertia, the smaller the acceleration limit and jerk limit.

9. A stacker crane fork anti-collision control system, characterized in that, include: The status-aware hardware is used to obtain the three-dimensional spatial coordinates of the target pallet and simultaneously collect the position of the fork extension shaft and the real-time drive data of the extension motor. The multi-axis drive actuator includes a telescopic shaft servo motor and its reduction mechanism for driving the fork movement, and a lifting shaft servo motor and its transmission mechanism for driving the loading platform movement. The collision avoidance control master station is communicatively connected to both the state-sensing hardware terminal and the multi-axis drive execution terminal. The collision avoidance control master station stores a control program, which, when executed by the processor, implements the method as described in any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that, It stores a computer program thereon, which, when executed by a processor, implements the method as described in any one of claims 1 to 8.

Images