Automatic pick-and-place control method based on OHT positioning

Abstract

The invention relates to the technical field of automatic control and multi-sensor fusion positioning of a semiconductor manufacturing material handling system, in particular to an automatic pick-and-place control method based on OHT positioning, which comprises the following steps: collecting each observed quantity in a preset sampling period, and obtaining a pose mean vector and a pose covariance matrix through fusion estimation; based on the pose covariance matrix and a preset collision probability upper limit threshold value, calculating a final segment safety expansion amount set and generating a final segment safety boundary, and determining a platform target alignment pose; constructing a probability constraint rolling predictive control model by taking a preset sampling period as a discrete step length under the constraint of a final-section safety boundary, and solving and outputting a walking axis control quantity and a lifting axis control quantity; and issuing a control quantity, updating the pose mean vector and the pose covariance matrix in a rolling manner in a next sampling period, and outputting a pick-and-place action instruction when an alignment residual error is located in a final segment safety boundary and a butt joint distance measurement value is smaller than a preset butt joint distance threshold value, so that the final segment collision risk is reduced.

Description

Technical Field

[0001] This invention relates to the technical field of automated control and multi-sensor fusion positioning in semiconductor manufacturing material handling systems, and particularly to an automatic pick-and-place control method based on OHT positioning. Background Technology

[0002] As semiconductor wafer manufacturing evolves towards higher cleanliness, faster turnaround times, and higher-density equipment layouts, the vehicle transfer in automated material handling systems (AMHS), centered on aerial work platforms (OHTs), is gradually expanding from track-based movement to end-of-line alignment, docking, and pick-and-place control stages. Current OHT end-of-line alignment typically employs a combination of axis coding measurement, inertial measurement, visual identification, and end-of-line distance measurement to estimate the vehicle-platform relative pose, and then performs fine-tuning and gripper pick-and-place actions based on this. This type of solution, in an ideal... While a high cycle time can be achieved under certain conditions, in situations involving cleanroom vibration, track micro-deformation, visual obstruction / reflection, vehicle attitude disturbance, and ranging drift, the final alignment often exhibits unclear estimation confidence, overly conservative control margin values, and an increased number of repeated fine-tunings. At the same time, traditional final alignment safety strategies based on fixed safety distances or deterministic thresholds are insufficient to provide quantifiable upper bound constraints on collision risks, leading to a long-term reliance on empirical compromises between alignment speed and safety margin, further increasing the risks of scratches, collisions, and accidental removal / placement.

[0003] US20240096676A1 discloses an automatic alignment of the loading port of an aerial transport vehicle and a semiconductor manufacturing tool. However, it does not explicitly incorporate the uncertainty of the fused positioning output into the safety domain construction in the form of covariance, and it also lacks a unified calculation method for deriving the set of final-stage safety expansion and the final-stage safety boundary from the upper limit threshold of the collision probability. Therefore, when the confidence of multi-source observations fluctuates with the operating conditions, the final-stage alignment is still prone to problems such as conservative allowance and increased number of fine-tuning times.

[0004] CN107357168A discloses an obstacle avoidance method for unmanned vehicles based on chance-constrained model predictive control. However, its object is obstacle avoidance and path optimization for ground unmanned vehicles. The constraint construction and state variable organization revolve around the road obstacle scenario. It does not involve the process of target alignment pose, dual-axis linkage control quantity, and docking distance threshold triggering pick-up and release at the OHT terminal station. It also does not provide the uncertainty-driven generation method of the terminal safety domain, which is the pose covariance matrix → terminal safety expansion quantity set → terminal safety boundary. It is difficult to directly cover the control requirements of alignment cycle and collision risk in the semiconductor loading port scenario.

[0005] In summary, existing OHT automatic alignment technologies generally suffer from problems in the final alignment stage, such as safety margin relying on fixed thresholds, difficulty in providing quantifiable upper bounds for collision risk, and susceptibility to repeated fine-tuning and cycle time drops when observation confidence fluctuates. While existing chance-constrained model predictive control technologies have probabilistic constraints, they lack a tightly coupled modeling path with OHT multi-source fusion positioning uncertainty and the final docking process. Therefore, this invention proposes an automatic pick-and-place control method based on OHT positioning. This method uses the pose mean vector and pose covariance matrix estimated by multi-source observation fusion as a unified state representation. Based on the upper limit threshold of collision probability, a safety expansion calculation is performed on the pose covariance matrix to form the final safety boundary. Under the constraints of the final safety boundary and the alignment pose of the platform target, a probabilistic constraint rolling predictive control model is constructed to output the walking axis control quantity and the lifting axis control quantity. The pose mean vector and pose covariance matrix are updated in the next sampling cycle. Combined with the final safety boundary and the docking distance threshold, pick-and-place action commands are given, thus forming a computable control method between the final alignment time and collision risk. Summary of the Invention

[0006] The purpose of this section is to outline some aspects of the embodiments of the present invention and to briefly introduce some preferred embodiments. Some simplifications or omissions may be made in this section, as well as in the abstract and title of the present application, to avoid obscuring the purpose of this section, the abstract and title of the invention. Such simplifications or omissions shall not be used to limit the scope of the present invention.

[0007] In view of the aforementioned existing problems, the present invention is proposed.

[0008] To solve the above-mentioned technical problems, the present invention provides the following technical solution: As a preferred embodiment of the automatic pick-up and place control method based on OHT positioning described in this invention, the following steps are taken: the displacement of the OHT walking axis incremental encoder, the angular velocity and linear acceleration measurement values ​​of the inertial measurement unit, the visual measurement values ​​of the platform reference markers, and the docking distance measurement values ​​of the end range sensor are collected under a preset sampling period, and then fused and recursively updated to obtain the pose mean vector and the pose covariance matrix. The final segment safety expansion set is determined based on the pose covariance matrix and the preset collision probability upper limit threshold, and the final segment safety boundary is generated from the final segment safety expansion set. At the same time, the platform target alignment pose is determined based on the pose mean vector. Based on the alignment pose of the terminal safety boundary and the platform target, a probabilistic constraint rolling predictive control model is constructed using the preset sampling period as the discrete step length, and the walking axis control quantity and the lifting axis control quantity of the current sampling period are solved and output. The system issues the travel axis control quantity and the lifting axis control quantity, and updates the pose mean vector and the pose covariance matrix in the next sampling period. When the alignment residual of the updated pose mean vector relative to the platform target alignment pose is within the final safety boundary and the docking distance measurement value is less than the preset docking distance threshold, the system outputs a pick-and-place action command.

[0009] The beneficial effects of this invention are as follows: By collecting and fusing various observations within a preset sampling period, this invention obtains the pose mean vector and pose covariance matrix, thereby quantifying the pose result and uncertainty of the final alignment simultaneously, reducing alignment jitter caused by single sensor errors; by calculating the final safety expansion set based on the pose covariance matrix and a preset collision probability upper limit threshold and generating the final safety boundary, while simultaneously determining the alignment pose of the platform target, the safety margin is adaptively adjusted with uncertainty, reducing the risk of scratches and collisions; by constructing a probabilistic constraint rolling predictive control model under the constraint of the final safety boundary and outputting the control quantities of the walking axis and the lifting axis, the dual-axis linkage alignment converges under risk-controlled conditions, reducing repeated fine-tuning and time loss; by issuing control quantities and updating the pose mean vector and pose covariance matrix in the next sampling period, the pick-and-place action command is triggered when the alignment residual falls into the final safety boundary and the docking distance measurement value meets the threshold, improving the success rate of one-time alignment and the cycle stability. Attached Figure Description

[0010] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. Wherein: Figure 1 This is a flowchart illustrating the automatic pick-up and place control method based on OHT positioning as shown in this invention. Figure 2 This is a schematic diagram of the predicted trajectory generated by the rolling solution as shown in this invention. Detailed Implementation

[0011] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0012] Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without inventive effort should fall within the scope of protection of this invention.

[0013] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0014] According to an embodiment of the present invention, in combination Figure 1 The flowchart shown illustrates an automatic pick-and-place control method based on OHT positioning, comprising: In a preferred embodiment, the OHT includes a traveling axis actuator, a lifting axis actuator, an end effector, and a positioning and measurement component; the platform is a docking station structure, including at least a platform base, a docking guide structure, a limiting structure, and a workpiece bearing structure; a platform reference marker is provided on the platform base, and the platform reference marker adopts a uniquely identifiable visual mark form, preferably a coded marker plate or a reflective dot matrix marker; when a coded marker plate is used, the coded area contains marker number information, and the pattern area contains corner or edge feature information, so as to simultaneously provide identity matching information and pose calculation feature information in the visual measurement value.

[0015] In this embodiment, the vision measurement device is fixedly installed on the OHT body or the end effector. The external parameter calibration results of the vision measurement device are written into the equipment calibration parameter table and solidified before alignment operation. The end distance sensor is installed on the end effector, and its distance measurement direction is consistent with the docking normal direction of the platform docking guide structure. The docking normal direction is the docking direction axis of the end effector towards the platform. The installation offset and distance measurement zero point of the end distance sensor are calibrated during the debugging stage and written into the end calibration parameter table, so that there is a consistent correspondence between the docking distance measurement value and the displacement component of the end docking posture along the docking normal direction.

[0016] In this embodiment, the incremental encoder of the walking axis and the feedback encoder of the lifting axis are respectively installed on the walking axis and the lifting axis actuator; the inertial measurement unit is fixedly installed at the reference position of the OHT body; in order to facilitate data fusion and recursive updates, each sensor data is accompanied by a data acquisition time stamp, the controller real-time task generates a sampling period index with a preset sampling period, and uses the sampling period index and the aligned time window as a unified benchmark for constructing the observation vector.

[0017] S1. Under a preset sampling period, acquire the displacement of the OHT walking axis incremental encoder, the angular velocity and linear acceleration measurements of the inertial measurement unit, the visual measurements of the platform reference markers, and the docking distance measurements of the end-effector distance sensor, and fuse and recursively update them to obtain the pose mean vector and pose covariance matrix. The pose mean vector includes at least the walking axis position component, the lifting axis position component, and the attitude component of the end-effector relative to the platform. The pose covariance matrix corresponds one-to-one with the pose mean vector. It should be noted that the following points are important in this step: S1.1. Based on the preset sampling period, the displacement, angular velocity and linear acceleration measurements of the OHT walking axis incremental encoder, the visual measurement values ​​of the platform reference markers, and the docking distance measurement values ​​are timestamped to obtain the observation vector corresponding to the same sampling period.

[0018] In this embodiment, the preset sampling period is determined by the real-time task cycle of the motion controller, preferably 20ms. A sampling period index is generated at the beginning of each sampling period, and the start and end time windows corresponding to the sampling period index are determined as the alignment time windows. All types of measurements are recorded with a timestamp, and timestamp alignment is performed according to the following rules to obtain the observation vectors corresponding to the same sampling period: For the displacement of the OHT walking axis incremental encoder and the angular velocity and linear acceleration measurements of the inertial measurement unit, when the sampling frequency is not lower than the preset sampling period, the nearest neighbor representative value is taken within the alignment time window; when multiple samples fall into the alignment time window, the weighted average value within the window is taken as the representative value of that sampling period, and the weight is set to be greater the closer to the center of the alignment time window. For visual measurements of platform reference markers, when the visual frame timestamp falls within the alignment time window, it is directly bound to that sampling period; when the visual frame does not fall within the alignment time window and the interval between it and two adjacent frames is less than the preset maximum visual interval threshold (e.g., 80 ms), the visually calculated platform reference pose observations are interpolated and bound according to the time ratio; when the interval between it and two adjacent frames is greater than or equal to the preset maximum visual interval threshold, the visual component of that sampling period is marked as missing and written into the missing measurement flag. For the docking distance measurement value of the end-point ranging sensor, when it is a continuous ranging, the weighted average value within the window is bound to the sampling period; when it is a triggered ranging, the trigger event timestamp is mapped to the index of the most recent sampling period and bound. The observation vector is organized in a fixed field order, preferably "encoder displacement component - inertial measurement unit component - visual measurement component - docking distance component", and a missing measurement identifier field is written into the observation vector so that subsequent extrapolation propagation and recursive fusion update have a consistent processing unit for the missing measurement component.

[0019] It should be noted that, through the above alignment rules, this embodiment avoids the amplification of residuals caused by time mismatch of different frequencies and different time delay measurements, making the consistency statistics and subsequent observation weight parameters reproducible; compared with the method of directly mixing and superimposing visual frames with inertial navigation / encoder, this embodiment provides alignment time windows, interpolation boundaries and missing measurement judgment rules, making the fusion recursion method more stable.

[0020] S1.2. Based on the OHT discrete kinematics model, the components related to motion extrapolation in the observation vector are integrated and the state is recursively calculated. The pose mean vector and pose covariance matrix of the previous sampling period are used as the initial values ​​for propagation to obtain the prior pose mean vector and prior pose covariance matrix.

[0021] In this embodiment, the components related to motion extrapolation include at least: the displacement of the incremental encoder of the walking axis, the angular velocity measurement value of the inertial measurement unit, and the linear acceleration measurement value; wherein the displacement of the incremental encoder of the walking axis provides the constraint of the incremental displacement of the walking axis, and the angular velocity and linear acceleration provide the constraint of short-term attitude and velocity change.

[0022] In a preferred embodiment, the prior pose mean vector and the prior pose covariance matrix are calculated using the following formula:

[0023]

[0024] in, This is the pose mean vector of the previous sampling period; Let be the pose covariance matrix of the previous sampling period; k is the sampling period index. The prior pose mean vector; The prior pose covariance matrix; This is the set of extrapolated input quantities obtained by organizing the extrapolated related components in the observation vector; This represents the recursive relation for discrete kinematic states. This is the state transition matrix obtained by linearizing the recurrence relation; Let be the process noise covariance matrix.

[0025] For example, when the preset sampling period is 20ms, the upper limit of the speed at the end of the walking axis is 0.3 m / s, the encoder resolution is 1μm, and the nominal value of the angular velocity noise of the inertial measurement unit is 0.02° / s, the upper limit of the process uncertainty of the walking axis position component can be set according to the order of the upper limit of the single-cycle displacement, and the inertial zero-bias drift can be converted to the upper limit of the single cycle according to the nominal drift rate of the equipment, so that the prior pose covariance matrix grows reasonably with the motion extrapolation.

[0026] S1.3 Establish an observation mapping relationship based on the visual measurement value of the platform reference marker and the docking distance measurement value, and calculate the observation residual and consistency statistics based on the preset measurement noise parameter set.

[0027] In this embodiment, the observation mapping relationship includes two parts. The first part is the visual observation mapping: when the visual measurement value is the set of corner pixel coordinates of the marker, based on the camera intrinsic and extrinsic parameter calibration results and the geometric model of the marker, the set of corner pixel coordinates is solved into the platform reference pose observation, and the observation is written into the visual measurement component of the observation vector; when the visual measurement value directly gives the platform reference pose observation, it is written directly. The second part is the distance measurement observation mapping: the docking distance measurement value is mapped into the displacement observation of the end docking pose along the docking normal axis direction, where the docking normal axis direction is the docking direction axis of the end towards the platform, and the displacement observation is written into the docking distance component of the observation vector as a constraint.

[0028] It should be noted that the preset measurement noise parameter set in this embodiment is set separately for visual measurement noise parameters and ranging noise parameters. The visual measurement noise parameters can be obtained from the camera resolution, the upper bound of the corner point extraction error, and the statistical analysis of the external parameter calibration residuals, such as a standard deviation of 0.5 mm for translation and 0.05° for rotation. The ranging noise parameters can be determined from the upper bound of the ranging repeatability and the installation sway error, such as a standard deviation of 0.3 mm for distance. The observation residuals are obtained by mapping the prior pose mean vector to the observation space through the observation mapping relationship and then subtracting it from the actual observation. The consistency statistic is determined by the observation residuals and their statistical scale, and is used as the observation reliability criterion input into the subsequent weight generation step.

[0029] S1.4. Compare the consistency statistic with the preset chi-square threshold. When the consistency statistic is greater than the preset chi-square threshold, apply a preset expansion coefficient to the measurement noise parameter set to obtain an expanded measurement noise parameter set, and generate the observation weight parameter accordingly. When the consistency statistic is less than or equal to the preset chi-square threshold, generate the observation weight parameter based on the measurement noise parameter set.

[0030] It should be noted that by comparing the consistency statistic with the preset chi-square threshold as described above, when visual occlusion, reflection, or ranging jumps cause residual abnormalities, the participation weight of abnormal observations decreases, thus suppressing abrupt changes in the pose mean vector.

[0031] In a preferred implementation, the threshold comparison and noise dilation are performed according to the following formula:

[0032]

[0033] in, This is a consistency statistic; d represents the preset chi-square threshold; d represents the residual degrees of freedom. The significance level; The measurement noise covariance matrix is ​​obtained by expanding the preset measurement noise parameter set; The weighted measurement noise covariance matrix obtained by expanding the set of expansion measurement noise parameters; This is the preset expansion coefficient.

[0034] As an example, the method for setting the preset chi-square threshold includes: determining the residual degrees of freedom d based on the number of valid observation components participating in the fusion in this sampling period, excluding missing components; the significance level is preferably set to 0.05; for example, when the degrees of freedom are 3, the preset chi-square threshold is set to 7.815; when the degrees of freedom are 6, the preset chi-square threshold is set to 12.592.

[0035] As an example, the preset inflation coefficient is set in different levels according to the degree of exceeding the threshold of the consistency statistic: 2 when it exceeds the threshold but does not exceed twice the threshold; 5 when it exceeds twice the threshold but does not exceed four times the threshold; and 10 when it exceeds four times the threshold. The observation weight parameter is determined by the inflation noise level, and corresponding weights are generated for the visual measurement component and the docking distance component respectively, so that the worse the observation quality, the smaller the weight.

[0036] S1.5 Calculate the recursive fusion update coefficients based on the prior pose covariance matrix, observation mapping relationship and observation weight parameters, and recursively update the prior pose mean vector and prior pose covariance matrix with the observation residuals, and output the pose mean vector and pose covariance matrix.

[0037] Specifically, the calculation of the recursive fusion update coefficients includes: linearizing the prior pose mean vector based on the observation mapping relationship to obtain the linearized observation relationship; weighting the measurement noise parameter set based on the observation weight parameters to obtain the weighted measurement noise parameter set; calculating the recursive fusion update coefficients based on the linearized observation relationship, the prior pose covariance matrix, and the weighted measurement noise parameter set; and performing a recursive update on the prior pose mean vector and the prior pose covariance matrix based on the recursive fusion update coefficients.

[0038] When performing recursive updates, the observation residuals are allocated as corrections to each component of the pose according to the recursive fusion update coefficients, and the uncertainty is updated simultaneously: when the observation weight parameters are low, the correction magnitude of the pose mean vector decreases and the convergence magnitude of the pose covariance matrix decreases; when the observation weight parameters are high, the correction and convergence magnitudes increase accordingly.

[0039] S2. Determine the final segment safety expansion set based on the pose covariance matrix and a preset collision probability upper limit threshold, and generate the final segment safety boundary from the final segment safety expansion set. Simultaneously, determine the platform target alignment pose based on the pose mean vector. Note that the following should be noted in this step: S2.1 Determine the confidence coefficient based on the preset upper limit threshold of collision probability, and set the confidence coefficient as the boundary scale parameter corresponding to the risk level of the final stage.

[0040] It should be noted that the preset collision probability upper limit threshold in this embodiment is given by the final alignment process and the platform safety specifications, such as 0.001; the final risk level refers to the risk level of interference between the end and the platform structural components, material box boundaries, or limiting mechanisms after the end enters the platform docking area, which corresponds to the preset collision probability upper limit threshold; the confidence coefficient is determined by the preset collision probability upper limit threshold under the preset distribution assumption or empirical safety factor table, for example, 3.29 corresponds to a more stringent risk level, or 2.58 corresponds to a general risk level, and the confidence coefficient is determined as the boundary scale parameter corresponding to the final risk level, so that the subsequent safety boundary changes with the risk level.

[0041] S2.2 Perform eigenvalue decomposition on the pose covariance matrix to obtain the set of principal axis directions of covariance and the set of variance eigenvalues ​​corresponding to each principal axis direction.

[0042] Specifically, the eigenvalue decomposition includes: performing symmetry correction on the pose covariance matrix to obtain a symmetric pose covariance matrix; in this embodiment, the symmetry correction preferably adopts the method of averaging the corresponding elements with their transposes to suppress the asymmetric terms introduced by numerical propagation; if a minimum negative eigenvalue appears after correction, the negative eigenvalue is truncated to 0 or a preset minimum positive number to satisfy the positive semi-definite property of the covariance matrix; then, the eigenvalue decomposition operation is performed on the symmetric pose covariance matrix to obtain a set of variance eigenvalues ​​and a set of corresponding eigenvectors; each eigenvector in the eigenvector set is normalized to obtain a set of covariance principal axis directions; the set of covariance principal axis directions is uniformly sorted in descending order of the set of variance eigenvalues, and the sorted set of covariance principal axis directions and the set of variance eigenvalues ​​corresponding to each principal axis direction are output; through sorting, the first principal axis direction corresponds to the direction of maximum uncertainty, which facilitates the subsequent construction of a safety boundary in a direction-dependent manner.

[0043] S2.3 Calculate the expansion components along each principal axis direction based on the confidence coefficient and the set of variance eigenvalues, and associate each expansion component with the set of covariance principal axis directions to form the final segment safe expansion amount set.

[0044] Specifically, the standard deviation of each variance eigenvalue is calculated and multiplied by the confidence coefficient to obtain the expansion component in that principal axis direction; the expansion components in all principal axis directions are associated with the corresponding set of covariance principal axis directions to form the final safe expansion set.

[0045] By generating an expansion component along the principal axis of the covariance, the final safety boundary is consistent with the shape of the pose uncertainty space, avoiding directional mismatch caused by a fixed radius boundary. Compared to methods that only take the diagonal component of the covariance or a fixed safety distance, this embodiment incorporates attitude and position correlation into the boundary construction, making the boundary more closely match the actual risk distribution of the final segment.

[0046] S2.4. The set of final segment safety expansion amounts constitutes a confidence ellipsoid boundary centered on the alignment pose of the platform target, and the confidence ellipsoid boundary is determined as the final segment safety boundary.

[0047] In this embodiment, the direction of each half-axis of the confidence ellipsoid boundary is determined by the set of principal covariance axis directions, and the length of each half-axis is determined by the corresponding dilation component. For example, when the uncertainty of the walking direction is large, the half-axis of the ellipsoid in that direction is longer, and the controller has a larger buffer in that direction. When the uncertainty of the lifting direction is small and the ranging is reliable, the half-axis of the ellipsoid in that direction is shorter, which is beneficial to improving the alignment accuracy.

[0048] Furthermore, in this embodiment, determining the platform target alignment pose based on the pose mean vector specifically includes the following steps: The platform reference pose is obtained by matching the visual measurement value of the platform reference marker with the preset platform pose parameter table. The matching is preferably performed according to the three-level key value of "marker number - platform workstation number - camera field of view area". The visual measurement value is first decoded to obtain the marker number, and then the corresponding platform workstation number is retrieved from the marker number to read the platform reference pose entry. The example fields of the preset platform pose parameter table include: workstation number, marker number, platform reference pose, platform allowable alignment tolerance, definition of docking normal axis direction, basic dimensions of platform structural safety boundary, and process configuration fields related to grippers / material boxes. The relative pose of the OHT to the platform is calculated based on the pose mean vector and the platform reference pose: The relative pose calculation is performed according to the unified coordinate system convention: the pose mean vector is converted to the platform coordinate system, and then the relative relationship with the platform reference pose is solved to obtain the relative pose result. The preset end geometry offset vector is mapped to the relative pose and superimposed to obtain the end docking pose. The preset end geometry offset vector is obtained from the end assembly calibration. Examples include the fixed translation offset of the end tool center point relative to the machine reference point, the offset of the end docking surface normal direction, and the offset of the gripper center line. The above offset vectors are written into the end calibration parameter table and solidified during the equipment debugging stage. The displacement component of the end docking posture along the docking normal axis is corrected based on the docking distance measurement value: the docking normal axis is the docking direction axis of the end towards the platform. The displacement is corrected by the docking distance measurement value so that the distance constraint of the end in the feed direction is consistent with the actual measurement. The alignment residual is calculated based on the end docking pose and the platform reference pose, and the alignment residual is substituted back into the pose mean vector to generate the platform target alignment pose.

[0049] In a preferred embodiment, calculating the alignment residual and back-substituting based on the end-of-line docking pose and the platform reference pose includes: representing the end-of-line docking pose and the platform reference pose in the same platform coordinate system; extracting the differences between the two in the traveling direction component, the lifting direction component, and the attitude deflection component according to the alignment component set to obtain the alignment residual; writing the alignment residual into the corresponding component of the pose mean vector according to the component correspondence to form the back-substituting correction amount; updating the pose mean vector with the back-substituting correction amount to output the platform target alignment pose consistent with the platform reference pose; when any component in the alignment residual exceeds the alignment tolerance item, the back-substituting correction amount corresponding to that component is reduced proportionally and written in so that the platform target alignment pose is consistent with the scale of the final safety boundary.

[0050] S3. Based on the alignment pose of the final safety boundary and the platform target, a probabilistic constrained rolling predictive control model is constructed using a preset sampling period as the discrete step length, and the control quantities of the traveling axis and the lifting axis for the current sampling period are solved and output. Note that the following should be noted in this step: S3.1. Establish the discrete state update relationship between the walking axis and the lifting axis based on the preset sampling period, and write the platform target alignment pose and the mean vector of the current pose into the prediction initial value to obtain the final segment discrete prediction model.

[0051] In this embodiment, the discrete state update relationship preferably includes the sub-states of the walking axis position and speed and the sub-states of the lifting axis position and speed, and the servo speed command or position increment command is used as the control input within the discrete step length; the platform target alignment pose and the mean vector of the current pose are written into the prediction initial value, so that the final discrete prediction model evaluates the alignment residual with the same reference target in each rolling window.

[0052] S3.2. Generate a pose prediction sequence in the prediction time domain based on the final segment discrete prediction model, and correspond the final segment safety boundary to a set of probability constraints in the prediction time domain according to the pose covariance matrix and the preset collision probability upper limit threshold.

[0053] In this embodiment, the prediction time domain length is determined by the maximum braking distance of the final segment and the sampling period. For example, 10 discrete step lengths are taken, corresponding to 200 ms. The pose prediction sequence is obtained by recursively calculating the discrete step lengths from the initial prediction value.

[0054] Specifically, the lower bound of the probability that the predicted pose falls into the final safety boundary is written into each step of the prediction time domain as a constraint meaning, and the boundary scale parameter is consistent with the uncertainty level of the current period; when visual missing measurements or the consistency statistic exceeds the threshold, resulting in an increase in uncertainty, the probability constraint set is tightened accordingly.

[0055] S3.3. Introduce a set of probabilistic constraints into the final discrete prediction model, and take the alignment residual corresponding to the alignment pose of the platform target as the optimization objective to form a probabilistic constraint rolling prediction control model.

[0056] For example, the mathematical expression for a probabilistically constrained rolling predictive control model is:

[0057]

[0058]

[0059]

[0060] Where U is the control sequence decision variable in the prediction time domain; N is the step size in the prediction time domain; For the first Step alignment residual vector; W is the alignment residual weighting matrix; The difference between two adjacent control steps; S is the weighting matrix of control change; For the first Predict the state step by step; For the first Predictive control quantity; The state update relationship for the final discrete prediction model; For the first The feasible region of the final safe boundary corresponding to the step; Probability operators with probability constraints; This refers to the risk margin parameter corresponding to the preset upper limit threshold for collision probability. This is an operator that forms a diagonal matrix using the elements within the parentheses; Allowable alignment tolerance value in the direction of travel; This refers to the allowable alignment tolerance value in the lifting direction; This represents the allowable alignment tolerance value in the attitude direction.

[0061] When the alignment residual vector contains more components, the alignment residual weighting matrix W expands its diagonal elements according to the same rules as above, and each diagonal element takes the reciprocal of the square of the corresponding allowable alignment tolerance value, so that the component with smaller tolerance has a higher weight in the objective function.

[0062] S3.4. The probabilistic constraint rolling predictive control model is solved using a rolling algorithm to obtain the control quantity sequence. The first term of the control quantity sequence is taken as the travel axis control quantity and the rise / fall axis control quantity for the current sampling period. This includes: The initial value of the final discrete prediction model is used as the initial state of the current rolling window, and the set of probability constraints is written into the constraint terms of the probability constraint rolling prediction control model; when the uncertainty increases, the scale parameter of the constraint terms is tightened synchronously. The alignment residuals of each step length in the prediction time domain are calculated based on the alignment pose of the platform target, and the weighted accumulation of the alignment residuals is determined as the objective function value of the current scrolling window. The weights can be set according to the rule that the weights are larger as they are closer to the end of the prediction step. The weights of the last two steps can be set to twice the weights of the previous steps. Under the condition of satisfying the set of probabilistic constraints, the objective function value is iteratively solved, and the candidate control quantity sequence corresponding to the minimum objective function value is output. The iterative solution preferably adopts the sequential quadratic programming or interior point method, and the maximum number of iterations is set to 30 in the example. When the constraints are violated, the step size of the control quantity change is reduced according to the degree of violation until the convergence condition is met or the maximum number of iterations is reached. The candidate control quantity sequence is substituted back into the final discrete prediction model to generate the predicted trajectory, and the predicted trajectory is checked against the final safety boundary in the full time domain. The full time domain constraint check rule is as follows: check step by step whether the error of the predicted pose relative to the station target pose at each step in the prediction time domain falls into the corresponding final safety boundary; when any step violates the rule, the error direction of the violation step size is used as the constraint tightening direction for the next iteration and the candidate control quantity sequence is updated until the full time domain check is passed; after the full time domain check is passed, the candidate control quantity sequence is output as the control quantity sequence. By using a rolling solution and taking the first term for distribution, the controller recalculates the control quantity based on the latest pose mean vector and pose covariance matrix in each sampling period. The impact of terminal disturbances and changes in observation quality on the control output is limited to a short window.

[0063] Reference Figure 2 Taking the OHT final alignment as an example, the platform target alignment pose is taken as the position center point (0,0) in the platform coordinate system. The pose mean vector obtained by fusion in the current sampling period corresponds to the position error of the end relative to the target: -40mm in the walking direction and +10mm in the lifting direction. The end-segment safety boundary allowed during the alignment process is a confidence ellipse with the target center as the center: the half-axis lengths are a=18mm in the walking direction and b=7mm in the lifting direction. The candidate control quantity sequence obtained by rolling solution is the acceleration sequence of the walking axis and lifting axis in the prediction time domain. Substituting this candidate control quantity sequence back into the final discrete prediction model, the predicted position points of each discrete step length in the prediction time domain are obtained, forming the prediction trajectory. Then, the prediction trajectory is checked against the full time domain constraints: whether the constraints are satisfied point by point. If any predicted point falls outside the ellipse, the candidate control quantity sequence is deemed to have failed and the control quantity change needs to be tightened or the safety constraint weight increased in the next rolling iteration. If the entire time domain is within the ellipse, the candidate control quantity sequence is output, and the first term is taken as the control quantity for the current sampling period.

[0064] S4. Issue the travel axis control quantity and the lifting axis control quantity, and continuously update the pose mean vector and pose covariance matrix in the next sampling period; when the alignment residual of the continuously updated pose mean vector relative to the platform target alignment pose is within the final safety boundary and the docking distance measurement value is less than the preset docking distance threshold, output the pick-up and release action command. It should be noted that in this step: S4.1 Send the travel axis control quantity and the lifting axis control quantity to the actuator, and record the control quantity timestamp corresponding to the preset sampling period.

[0065] In this embodiment, the actuator includes at least a travel axis servo driver, a lifting axis servo driver, and a gripper driver, and records the control quantity timestamps corresponding to the preset sampling period. The control quantity timestamps are written into the dispatch record structure. Example fields include: sampling period index, dispatch timestamp, travel axis command position value, travel axis command speed limit, lifting axis command position value, lifting axis command speed limit, control mode identifier, and verification status identifier. Through the control quantity timestamps, the corresponding observation vector can be retrieved according to the same index in the next sampling period to form a closed loop.

[0066] S4.2 When the next sampling period arrives, obtain the observation vector corresponding to the sampling period based on the control quantity time scale, and use the pose mean vector and pose covariance matrix of the previous sampling period as the initial update values.

[0067] In this embodiment, when the next sampling period arrives, the alignment time window is determined based on the sampling period index in the control quantity time scale, and the observation vector corresponding to the sampling period is generated according to the alignment rule of S1.1; and the pose mean vector and pose covariance matrix output in the previous sampling period are written into the fusion module as initial update values, so that the rolling update is continuous with the state of the previous period.

[0068] S4.3. Based on the observation vector and the OHT discrete kinematic model, extrapolate and propagate the updated initial values ​​to obtain the prior pose mean vector and prior pose covariance matrix for the next sampling period.

[0069] In this embodiment, when the encoder displacement increment is detected to exceed the upper limit of the single-cycle displacement determined by the upper limit of the travel axis speed and the preset sampling period, the encoder component of that sampling period is marked as low confidence and the process uncertainty increase rule is triggered, so that the prior pose covariance matrix is ​​increased accordingly to suppress the interference of abnormal displacement increment on subsequent fusion updates.

[0070] S4.4 Calculate the observation residuals and consistency statistics based on the prior pose mean vector, prior pose covariance matrix and observation vector, and generate observation weight parameters based on the consistency statistics, and then perform recursive fusion update to output the pose mean vector and pose covariance matrix for the next sampling period.

[0071] For example, when short-term visual occlusion causes the consistency statistic to exceed the threshold, the observation weight parameter of the visual component decreases, the observation weight parameter of the ranging component becomes dominant, and the correction of the pose mean vector in the docking normal axis direction is mainly provided by the docking distance measurement value, so that the end effector converges faster to meet the distance condition triggered by the pick-up and put-down action.

[0072] Specifically, the pick-up and put-down action instructions are generated and issued according to the following instruction rules: (1) When the alignment residual of the average pose vector after rolling update relative to the target pose of the platform is within the final safety boundary and the measured docking distance is less than the preset docking distance threshold, the clamping opening and closing displacement setting value, clamping force limit value, lifting shaft picking and placing height setting value, execution timeout threshold and back displacement setting value are retrieved from the preset picking and placing action parameter table according to the picking and placing type identifier.

[0073] The preset docking distance threshold is given by the safe distance between the end and the workpiece before contact between the pick-up and place process, and the example is 2.0 mm; the setting rule is: not more than 30% of the end buffer stroke, and not more than 6 times the standard deviation of the distance measurement noise.

[0074] The opening and closing displacement setting value of the gripper is determined by the workpiece's external dimensions and the gripper structure. For example, the opening displacement setting value is 28 mm and the clamping displacement setting value is 18 mm. The setting rule is: the opening displacement setting value is not less than the maximum external width of the workpiece plus the assembly allowance (such as 3 mm), and the clamping displacement setting value is not greater than the minimum external width of the workpiece minus the clamping margin (such as 1 mm).

[0075] The clamping force limit of the gripper is determined by the allowable clamping force of the workpiece and the driving capability of the gripper, and is set to 35 N in the example. The setting rule is: it shall not exceed 80% of the upper limit of the allowable clamping force of the workpiece, and is calibrated in combination with the gripper force sensor or current estimation model.

[0076] The lifting shaft pick-up and drop-off height setting value is obtained by calibrating the platform docking surface height and the end geometric offset. The example includes two levels: pre-approach height and pick-up and drop-off height, with the pre-approach height being higher than the pick-up and drop-off height. The setting rule is that the difference between the two height levels is not less than 5 times the maximum displacement of the lifting shaft in a single cycle.

[0077] The execution timeout threshold is determined by statistics of the maximum motion time of the gripper drive, and is set to 1.2 s in the example. The setting rule is: not less than 1.5 times the average time to complete one full stroke under rated conditions.

[0078] The set value of the retraction displacement is determined by the platform backoff space and the re-alignment requirement, and is 15 mm in the example. The setting rule is: greater than 3 times the end safety clearance and less than 80% of the upper limit of the platform backoff space.

[0079] Example fields in the preset pick-and-place action parameter table include: pick-and-place type identifier, workpiece type identifier, opening displacement setting value, clamping displacement setting value, clamping force limit value, pre-approach height setting value, pick-and-place height setting value, execution timeout threshold, retraction displacement setting value, gripper status tolerance range, and action sequence number set.

[0080] (2) Write the travel axis control quantity and the lifting axis control quantity into the dwell control setting to generate the travel axis dwell control quantity and the lifting axis dwell control quantity. The dwell control setting includes a speed setting value of zero and a position setting value of the command position value of the previous sampling period. (3) When the pick-up and place type is identified as pick-up, the pick-up and place action command is written in the order of switching the lifting shaft pick-up and place height setting value and the gripper opening and closing displacement setting value from opening displacement to clamping displacement, and the gripper clamping force limit value is used as one of the clamping termination conditions. (4) When the pick-up and place type identifier is the material placement identifier, write the pick-up and place action instruction in the order of switching the clamping displacement to the opening displacement of the lifting shaft pick-up and place height setting value and the gripper opening and closing displacement setting value. (5) When the execution timeout threshold is reached and the gripper status quantity has not reached the tolerance range corresponding to the gripper opening and closing displacement setting value, the retraction displacement setting value is generated into the travel axis retraction control quantity and sent out, and the lifting axis dwell control quantity is maintained at the value sent out in the previous sampling cycle.

[0081] The tolerance range is determined by the upper limit of the repeatability error of the gripper position sensor and the upper limit of the mechanical clearance of the gripper, for example, ±0.5 mm. The setting method is to take three times the upper limit of the repeatability error of the sensor and the upper limit of the mechanical clearance, and write this range into the preset pick-and-place action parameter table.

[0082] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. An automatic pick-and-place control method based on OHT positioning, characterized in that, include: Under a preset sampling period, the displacement of the OHT walking axis incremental encoder, the angular velocity and linear acceleration measurement values ​​of the inertial measurement unit, the visual measurement values ​​of the platform reference markers, and the docking distance measurement values ​​of the end range sensor are collected and fused and recursively updated to obtain the pose mean vector and pose covariance matrix. The final segment safety expansion set is determined based on the pose covariance matrix and the preset collision probability upper limit threshold, and the final segment safety boundary is generated from the final segment safety expansion set. At the same time, the platform target alignment pose is determined based on the pose mean vector. Based on the alignment pose of the terminal safety boundary and the platform target, a probabilistic constraint rolling predictive control model is constructed using the preset sampling period as the discrete step length, and the walking axis control quantity and the lifting axis control quantity of the current sampling period are solved and output. The system issues the walking axis control quantity and the lifting axis control quantity, and updates the pose mean vector and pose covariance matrix in the next sampling period. When the alignment residual of the mean pose vector after rolling updates relative to the alignment pose of the platform target is within the final safety boundary and the measured docking distance is less than the preset docking distance threshold, a pick-and-place action command is output.

2. The automatic pick-up and place control method based on OHT positioning according to claim 1, characterized in that, Obtaining the pose mean vector and the pose covariance matrix includes: Based on the preset sampling period, the displacement of the OHT walking axis incremental encoder, the measured values ​​of angular velocity and linear acceleration, the visual measurement value of the platform reference marker, and the measured value of docking distance are timestamped to obtain the observation vector corresponding to the same sampling period; Based on the OHT discrete kinematics model, the components related to motion extrapolation in the observed vector are integrated and the state is recursively derived. The pose mean vector and pose covariance matrix of the previous sampling period are used as the initial values ​​for propagation to obtain the prior pose mean vector and prior pose covariance matrix. An observation mapping relationship is established based on the visual measurement value of the platform reference marker and the docking distance measurement value, and the observation residual and consistency statistics are calculated according to the preset measurement noise parameter set. The consistency statistic is compared with a preset chi-square threshold. When the consistency statistic is greater than the preset chi-square threshold, a preset expansion coefficient is applied to the measurement noise parameter set to obtain an expanded measurement noise parameter set, and observation weight parameters are generated accordingly. When the consistency statistic is less than or equal to the preset chi-square threshold, the observation weight parameters are generated based on the measurement noise parameter set. The recursive fusion update coefficients are calculated based on the prior pose covariance matrix, the observation mapping relationship, and the observation weight parameters. The prior pose mean vector and the prior pose covariance matrix are then recursively updated using the observation residuals, and the pose mean vector and the pose covariance matrix are output.

3. The automatic pick-up and place control method based on OHT positioning according to claim 2, characterized in that, Calculating the recursive fusion update coefficients includes: The observation mapping relationship is linearized at the prior pose mean vector to obtain the linearized observation relationship; The measurement noise parameter set is weighted and corrected based on the observation weight parameters to obtain a weighted measurement noise parameter set. The recursive fusion update coefficients are calculated based on the linearized observation relationship, the prior pose covariance matrix, and the weighted measurement noise parameter set. The recursive update is then performed on the prior pose mean vector and the prior pose covariance matrix based on the recursive fusion update coefficients.

4. The automatic pick-and-place control method based on OHT positioning according to claim 1 or 2, characterized in that, Generating the final segment security boundary includes: The confidence coefficient is determined based on the preset upper limit threshold of the collision probability, and the confidence coefficient is determined as the boundary scale parameter corresponding to the terminal risk level; The pose covariance matrix is ​​subjected to eigenvalue decomposition to obtain the set of principal axis directions of covariance and the set of variance eigenvalues ​​corresponding to each principal axis direction; Based on the confidence coefficient and the set of variance eigenvalues, the expansion components along each principal axis direction are calculated, and each expansion component is associated with the set of covariance principal axis directions to form a set of final segment safe expansion amounts. The set of final segment safety expansion amounts constitutes a confidence ellipsoidal boundary centered on the alignment pose of the platform target, and the confidence ellipsoidal boundary is determined as the final segment safety boundary.

5. The automatic pick-up and place control method based on OHT positioning according to claim 4, characterized in that, The feature decomposition includes: The pose covariance matrix is ​​subjected to symmetry correction to obtain a symmetric pose covariance matrix; Eigenvalue decomposition is performed on the symmetric pose covariance matrix to obtain the set of variance eigenvalues ​​and the corresponding set of eigenvectors. Normalize each feature vector in the feature vector set to obtain the set of covariance principal axis directions; The set of covariance principal axis directions is uniformly sorted in descending order according to the set of variance eigenvalues, and the sorted set of covariance principal axis directions and the set of variance eigenvalues ​​corresponding to each principal axis direction are output.

6. The automatic pick-and-place control method based on OHT positioning according to claim 1 or 2, characterized in that, Determining the target alignment pose of the platform based on the pose mean vector includes: The platform reference pose is obtained by matching the visual measurement value of the platform reference marker with the preset platform pose parameter table. The relative pose of the OHT to the platform is calculated based on the mean pose vector and the platform reference pose. The preset end geometry offset vector is mapped to the relative pose and superimposed to obtain the end docking pose. The displacement component of the end docking pose along the docking normal axis is corrected based on the docking distance measurement value. The alignment residual is calculated based on the end docking pose and the platform reference pose, and the alignment residual is substituted back into the pose mean vector to generate the platform target alignment pose.

7. The automatic pick-up and place control method based on OHT positioning according to claim 6, characterized in that, The output of the travel axis control quantity and the lifting axis control quantity for the current sampling period includes: Based on the preset sampling period, a discrete state update relationship between the walking axis and the lifting axis is established, and the platform target alignment pose and the mean vector of the current pose are written into the prediction initial value to obtain the final segment discrete prediction model. Based on the terminal discrete prediction model, a pose prediction sequence in the prediction time domain is generated, and according to the pose covariance matrix and the preset collision probability upper limit threshold, the terminal safety boundary is mapped to a set of probability constraints in the prediction time domain. The probability constraint set is introduced into the terminal discrete prediction model, and the alignment residual corresponding to the alignment pose of the platform target is used as the optimization objective to form the probability constraint rolling prediction control model. The probabilistic constraint rolling predictive control model is solved in a rolling manner to obtain a sequence of control quantities, and the first term of the sequence of control quantities is taken as the control quantity of the walking axis and the control quantity of the lifting axis in the current sampling period.

8. The automatic pick-up and place control method based on OHT positioning according to claim 7, characterized in that, The rolling solution of the rolling predictive control model includes: The initial prediction value of the terminal discrete prediction model is used as the initial state of the current rolling window, and the set of probability constraints is written into the constraint terms of the probability constraint rolling prediction control model. Based on the alignment pose of the platform target, the alignment residuals of each discrete step size in the prediction time domain are calculated, and the weighted accumulation of the alignment residuals is determined as the objective function value of the current scrolling window; Under the condition of satisfying the set of probability constraints, the objective function value is solved iteratively, and the candidate control quantity sequence corresponding to the minimum objective function value is output. The candidate control quantity sequence is substituted back into the terminal discrete prediction model to generate a prediction trajectory, and the prediction trajectory is checked against the terminal safety boundary in the full time domain. When the full time domain constraint check passes, the candidate control quantity sequence is output as the control quantity sequence.

9. The automatic pick-up and place control method based on OHT positioning according to claim 1, characterized in that, A method for continuously updating the pose mean vector and the pose covariance matrix in the next sampling period includes: The control quantities of the traveling axis and the lifting axis are sent to the actuator, and the control quantity timestamps corresponding to the preset sampling period are recorded. When the next sampling period arrives, the observation vector corresponding to the sampling period is obtained based on the control quantity time stamp, and the pose mean vector and the pose covariance matrix of the previous sampling period are used as the initial update values. The initial update value is extrapolated and propagated based on the observation vector and the OHT discrete kinematic model to obtain the prior pose mean vector and prior pose covariance matrix for the next sampling period; Based on the prior pose mean vector, the prior pose covariance matrix, and the observation vector, the observation residual and consistency statistic are calculated. Then, the observation weight parameters are generated according to the consistency statistic and recursively fused and updated to output the pose mean vector and pose covariance matrix for the next sampling period.

10. The automatic pick-up and place control method based on OHT positioning according to claim 1, characterized in that, The pick-and-place action instructions are generated and issued according to the following instruction rules: When the alignment residual of the average pose vector after rolling update relative to the alignment pose of the platform target is within the final safety boundary and the measured docking distance is less than the preset docking distance threshold, the clamping opening and closing displacement setting value, clamping force limit value, lifting shaft pick-up and put-out height setting value, execution timeout threshold and retraction displacement setting value are retrieved from the preset pick-up and put-out action parameter table according to the pick-up and put-out type identifier. The travel axis control quantity and the lifting axis control quantity are written into the dwell control setting to generate the travel axis dwell control quantity and the lifting axis dwell control quantity. The dwell control setting includes a speed setting value of zero and a position setting value of the command position value of the previous sampling period. When the pick-up and place type identifier is a pick-up identifier, the pick-up and place action instruction is written in the order of switching the lifting shaft pick-up and place height setting value and the gripper opening and closing displacement setting value from opening displacement to clamping displacement, and the gripper clamping force limit value is used as one of the clamping termination conditions. When the pick-up and place type identifier is a material placement identifier, the pick-up and place action instruction is written in the order of switching the lifting shaft pick-up and place height setting value and the gripper opening and closing displacement setting value from clamping displacement to opening displacement; When the execution timeout threshold is reached and the gripper status value has not reached the tolerance range corresponding to the gripper opening and closing displacement setting value, the retraction displacement setting value is used to generate the travel axis retraction control value and sent out, and the lifting axis dwell control value is maintained at the value sent out in the previous sampling cycle.

Images