An automatic pick-and-place control method based on OHT positioning

Abstract

The present application relates to the technical field of automation control and multi-sensor fusion positioning of semiconductor manufacturing material handling systems, and particularly relates to an automatic pick-and-place control method based on OHT positioning, comprising: collecting each observation at a preset sampling period, obtaining a pose mean vector and a pose covariance matrix through fusion estimation; calculating a terminal safety inflation set and generating a terminal safety boundary based on the pose covariance matrix and a preset upper limit threshold of collision probability, and simultaneously determining a station target-to-target pose; under the constraint of the terminal safety boundary, taking the preset sampling period as a discrete step to construct a probability-constrained rolling prediction control model and solve output walking shaft control quantity and lifting shaft control quantity; issuing the control quantity and rolling updating the pose mean vector and the pose covariance matrix at the next sampling period, and outputting a pick-and-place action instruction when the alignment residual is located within the terminal safety boundary and the docking distance measurement value is less than a preset docking distance threshold, thereby reducing the terminal collision risk.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic control and multi-sensor fusion positioning of semiconductor manufacturing material handling systems, and in particular to an automatic pick-and-place control method based on OHT positioning. BACKGROUND

[0002] With the evolution of semiconductor wafer manufacturing towards high cleanliness, high tact and high density equipment layout, the carrier transfer in the factory automation material handling system (AMHS) with high air transport vehicles (OHT) as the core has gradually expanded from track walking to end-point positioning, docking, pick-and-place control. The existing OHT end-point positioning usually comprehensively uses walking shaft code measurement, inertial measurement, visual mark recognition and end-point distance measurement to estimate the relative pose of the vehicle-station, and on this basis, performs fine positioning and gripper pick-and-place actions. Such a scheme can obtain a high tact in ideal conditions, but in the working conditions where clean room vibration, track micro-deformation, visual obstruction / reflection, carrier attitude disturbance and ranging drift coexist, the end-point positioning often appears the phenomena of unclear estimation confidence, conservative control margin value, and increased number of repeated fine tuning. At the same time, the traditional end-point safety strategy mainly based on fixed safety distance or deterministic threshold value cannot give a quantifiable upper bound constraint to the collision risk, leading to a long-term compromise between positioning speed and safety margin depending on experience, further causing the risk of scratches, collisions and mis-pick-and-place to rise.

[0003] US20240096676A1 discloses an automatic alignment of a high air transport vehicle and a loading port of a semiconductor manufacturing tool, which does not explicitly include the uncertainty of the fusion positioning output in the form of covariance into the safety domain construction, and lacks a unified calculation method for deriving the set of end-point safety inflation and the end-point safety boundary from the upper limit threshold of collision probability. Therefore, when the confidence of multi-source observation fluctuates with the working condition, the end-point positioning is still prone to conservative margin and increased number of fine tuning.

[0004] CN107357168A discloses an obstacle avoidance method for unmanned vehicles based on opportunity constraint model predictive control, but its object is the obstacle avoidance and path optimization of ground unmanned vehicles, the constraint construction and state variable organization are carried out around the road obstacle scene, and it does not involve the process of OHT end-point station target positioning pose, two-axis linkage control variable, docking distance threshold triggering pick-and-place. It also does not give the end-point safety domain generation method driven by uncertainty from pose covariance matrix→set of end-point safety inflation→end-point safety boundary, which is difficult to directly cover the control demand of positioning tact and risk of scratches and collisions in the semiconductor loading port scene.

[0005] In summary, the existing OHT automatic alignment technology generally has the problems of relying on a fixed threshold for safety margin in the final alignment stage, being difficult to give a quantifiable upper bound of collision risk, and being prone to repeated fine-tuning and beat reduction when the observation confidence fluctuates. Although the existing opportunity constraint model predictive control technology has the idea of probability constraint, it lacks a tightly coupled modeling path for the positioning uncertainty of OHT multi-source fusion and the final docking process. Therefore, the present application proposes an automatic picking and placing control method based on OHT positioning, which uses the mean vector of the pose and the pose covariance matrix estimated by multi-source observation fusion as a unified state representation, performs safety inflation calculation on the pose covariance matrix based on a collision probability upper threshold to form a final safety boundary, constructs a probability constraint rolling predictive control model under the constraints of the final safety boundary and the target alignment pose of the station to output the walking axis control quantity and the lifting axis control quantity, and updates the mean vector of the pose and the pose covariance matrix in the next sampling period. The picking and placing action instructions are given in combination with the final safety boundary and the docking distance threshold, so as to form a calculable control method between the final alignment time and the collision risk. SUMMARY

[0006] This section is intended to summarize some aspects of the embodiments of the present application and briefly introduce some preferred embodiments. Some simplifications or omissions may be made in this section and the abstract and title of the specification to avoid obscuring the purpose of this section, the abstract and the title, and such simplifications or omissions cannot be used to limit the scope of the present application.

[0007] In view of the above-mentioned existing problems, the present application is proposed.

[0008] To solve the above technical problems, the present application provides the following technical solutions.

[0009] As a preferred scheme of the automatic picking and placing control method based on OHT positioning, 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 station reference marker, and the docking distance measurement values of the end ranging sensor are collected and recursively updated at a preset sampling period to obtain the mean vector of the pose and the pose covariance matrix.

[0010] The final safety inflation quantity set is determined based on the pose covariance matrix and a preset upper limit threshold of collision probability, and the final safety boundary is generated from the final safety inflation quantity set, and the target alignment pose of the station is determined based on the mean vector of the pose.

[0011] The probability constraint rolling predictive control model is constructed based on the final safety boundary and the target alignment pose of the station with the preset sampling period as the discrete step, and the walking axis control quantity and the lifting axis control quantity in the current sampling period are solved.

[0012] The walking axis control amount and the lifting axis control amount are issued, and the pose mean vector and the pose covariance matrix are updated in the next sampling period; when the alignment residual of the updated pose mean vector relative to the target alignment pose of the station is within the terminal safety boundary, and the docking distance measurement value is less than the preset docking distance threshold, a pick-and-place action instruction is output.

[0013] The present application has the following advantages: the pose mean vector and the pose covariance matrix are obtained by collecting each observation and performing fusion estimation within a preset sampling period, thereby synchronously quantifying the pose result and uncertainty of the terminal alignment, reducing alignment jitter caused by single sensor error; the terminal safety boundary is generated by calculating a set of terminal safety expansion amounts based on the pose covariance matrix and a preset upper limit threshold of collision probability, and the target alignment pose of the station is determined, so that the safety margin is adaptively adjusted according to the uncertainty, and the risk of scratching and collision is reduced; the probability constraint rolling prediction control model is constructed under the constraint of the terminal safety boundary, and the walking axis control amount and the lifting axis control amount are output, so that the biaxial linkage alignment converges under the condition of controlled risk, and the repeated fine-tuning and time loss are reduced; the control amount is issued, and the pose mean vector and the pose covariance matrix are updated in the next sampling period, and the pick-and-place action instruction is triggered when the alignment residual falls within the terminal safety boundary and the docking distance measurement value meets the threshold, thereby improving the success rate of one-time alignment and the beat stability. BRIEF DESCRIPTION OF DRAWINGS

[0014] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the drawings needed to be used in the embodiment description. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor. Among them:

[0015] Figure 1 The flowchart of the automatic pick-and-place control method based on OHT positioning shown in the present application;

[0016] Figure 2 The predicted trajectory generated by the rolling solution shown in the present application. DETAILED DESCRIPTION

[0017] In order to make the above-mentioned purposes, features and advantages of the present application more apparent and easy to understand, the specific embodiments of the present application will be described in detail below with reference to the drawings of the specification. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments.

[0018] Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor should be within the scope of protection of the present application.

[0019] In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present application. However, it will be apparent to one skilled in the art that the present application can be practiced without the specific details set forth in this description. In other instances, well-known methods, procedures, components, and circuits have not been described in detail so as not to unnecessarily obscure aspects of the present application.

[0020] According to an embodiment of the present application, in combination with the flow chart shown, an automatic pick-and-place control method based on OHT positioning comprises: Figure 1 The flow chart shown, an automatic pick-and-place control method based on OHT positioning comprises:

[0021] In a preferred embodiment, the OHT includes a walking shaft actuator, a lifting shaft actuator, an end effector, and a positioning measurement assembly; the station is a docking station structure, including at least a station base, a docking guide structure, a limiting structure, and a workpiece bearing structure; the station base is provided with a station reference marker, which is in the form of a uniquely identifiable visual marker, preferably a coded marker plate or a reflective dot matrix marker; when a coded marker plate is used, the coding area contains marker number information, and the pattern area contains corner or edge feature information, to provide identity matching information and pose solving feature information in the visual measurement value.

[0022] In this embodiment, the visual measurement device is fixedly installed on the OHT body or the end effector, and the external parameter calibration result of the visual measurement device is written into the device calibration parameter table and is fixed before the alignment operation; the end range sensor is installed on the end effector, and its range-finding direction is consistent with the docking normal direction of the docking guide structure of the station; the docking normal direction is the docking direction axis of the end effector towards the station; the installation offset of the end range sensor and the range-finding zero point are calibrated in the debugging stage and written into the end calibration parameter table, so that the docking distance measurement value and the displacement component of the end docking pose along the docking normal direction have a consistent corresponding relationship.

[0023] In this embodiment, the walking shaft incremental encoder and the lifting shaft feedback encoder are respectively installed on the walking shaft actuator and the lifting shaft actuator; the inertial measurement unit is fixedly installed at a reference position of the OHT body; in order to facilitate data fusion recursive updating, the data collected by each sensor is attached with a collection time stamp, and the controller generates a sampling period index with a preset sampling period in real time, and uses the sampling period index and the alignment time window as the observation vector to construct a unified reference.

[0024] S1, collect the displacement of the OHT walking shaft incremental encoder, the angular velocity and linear acceleration measurement values of the inertial measurement unit, the visual measurement values of the station reference marker, and the docking distance measurement values of the end ranging sensor under a preset sampling period, and recursively update them to obtain a mean pose vector and a pose covariance matrix; the mean pose vector at least includes a walking shaft position component, a lifting shaft position component, and an end attitude component relative to the station, and the pose covariance matrix corresponds to the mean pose vector one by one. It should be noted that in this step:

[0025] S1.1, time stamp alignment is performed on the displacement of the OHT walking shaft incremental encoder, the angular velocity and linear acceleration measurement values, the visual measurement values of the station reference marker, and the docking distance measurement values based on the preset sampling period to obtain an observation vector corresponding to the same sampling period.

[0026] In this embodiment, the preset sampling period is determined by the real-time task period of the motion controller, preferably 20 ms, a sampling period index is generated at the beginning of each sampling period, and the start and end time window corresponding to the sampling period index is determined as the alignment time window; the collection time stamp is written when collecting each type of measurement, and the time stamp alignment is completed according to the following rules to obtain an observation vector corresponding to the same sampling period:

[0027] For the displacement of the OHT walking shaft incremental encoder and the angular velocity and linear acceleration measurement values 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 there are multiple samples falling into the alignment time window, the weighted mean value within the window is taken as the representative value of the sampling period, and the weight is set to be greater when the distance from the center of the alignment time window is closer;

[0028] For the visual measurement values of the station reference marker, when the visual frame time stamp falls into the alignment time window, it is directly bound to the sampling period; when the visual frame does not fall into the alignment time window and the interval between the adjacent two frames is less than the preset visual maximum interval threshold (such as 80 ms), the station reference pose observation obtained by visual calculation is interpolated according to the time ratio and bound; when the interval between the adjacent two frames is greater than or equal to the preset visual maximum interval threshold, the visual component of the sampling period is marked as missing and written into the missing identification;

[0029] For the docking distance measurement values of the end ranging sensor, when it is continuous ranging, it is bound to the sampling period according to the weighted mean value within the window; when it is triggered ranging, the trigger event time stamp is mapped to the nearest sampling period index and bound;

[0030] 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 data identification field is written in the observation vector, so that the subsequent extrapolation propagation and recursive fusion update have a consistent processing unit for missing components.

[0031] It should be noted that the alignment rules described above avoid the introduction of residual amplification caused by time mismatch of different frequencies and different time delay measurement quantities, so that the consistency statistics and subsequent observation weight parameter generation have reproducibility; compared with the way of directly mixing and superimposing visual frames and inertial navigation / encoder, the alignment time window, interpolation boundary and missing data determination rule are given in the embodiment, so that the fusion recursive method is more stable.

[0032] S1.2, according to the OHT discrete kinematics model, the components related to motion extrapolation in the observation vector are integrated and state recursive, and the pose mean vector and pose covariance matrix of the last sampling period are used as the propagation initial value, to obtain the prior pose mean vector and prior pose covariance matrix.

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

[0034] In a preferred embodiment, the prior pose mean vector and the prior pose covariance matrix are calculated as follows:

[0035]

[0036]

[0037] wherein, is the pose mean vector of the last sampling period; is the pose covariance matrix of the last sampling period; k is the sampling period index; is the prior pose mean vector; is the prior pose covariance matrix; is the set of extrapolation input quantities obtained by sorting the extrapolation related components in the observation vector; is the discrete kinematics state recursive relationship; is the state transition matrix obtained by linearizing the recursive relationship; is the process noise covariance matrix.

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

[0039] S1.3, an observation mapping relationship is established based on the visual measurement value of the platform reference marker and the docking distance measurement value, and an observation residual and a consistency statistic are calculated according to a preset measurement noise parameter set.

[0040] In this embodiment, the observation mapping relationship includes two parts. The first part is a visual observation mapping. When the visual measurement value is a set of marker corner pixel coordinates, the set of corner pixel coordinates is solved into a platform reference pose observation based on the camera internal and external parameter calibration results and the marker geometric model, 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 directly written. The second part is a ranging observation mapping. The docking distance measurement value is mapped into a displacement observation of the end docking pose along the docking normal axis direction, and the docking normal axis direction is the docking direction axis of the end towards the platform, and the displacement observation is written as a constraint into the docking distance component of the observation vector.

[0041] It should be noted that the preset measurement noise parameter set in this embodiment is set according to the visual measurement noise parameter and the ranging noise parameter respectively. The visual measurement noise parameter can be obtained from the camera resolution, the upper limit of the corner extraction error, and the external parameter calibration residual statistics, such as a translation direction standard deviation of 0.5 mm and a rotation direction standard deviation of 0.05°. The ranging noise parameter can be determined by the ranging repeatability and the upper limit of the installation deflection error, such as a distance standard deviation of 0.3 mm. The observation residual is obtained by subtracting the actual observation from the prior pose mean vector mapped to the observation space by the observation mapping relationship. The consistency statistic is determined by the observation residual and its statistical scale, and is input as an observation reliability criterion to the subsequent weight generation step.

[0042] S1.4, the consistency statistic is compared with a preset chi-square threshold value, when the consistency statistic is greater than the preset chi-square threshold value, a preset inflation coefficient is applied to the measurement noise parameter set to obtain an inflated 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 value, the observation weight parameters are generated based on the measurement noise parameter set.

[0043] It should be noted that by the above method of comparing the consistency statistic with the preset chi-square threshold value, when the visual occlusion, reflection or ranging jump causes abnormal residual, the participation weight of the abnormal observation is reduced, and the mutation of the pose mean vector is inhibited.

[0044] In a preferred implementation, the threshold comparison and the noise inflation are performed as follows:

[0045]

[0046]

[0047] wherein, is a consistency statistic; is a preset chi-square threshold; d is a degree of freedom of residual; is a significance level; is a measurement noise covariance matrix expanded from a preset measurement noise parameter set; is a weighted measurement noise covariance matrix expanded from an inflated measurement noise parameter set; is a preset inflation coefficient.

[0048] As an example, the preset chi-square threshold is set by determining the degree of freedom d of residual from the number of effective observation components participating in fusion in the current sampling period, and the missing components are not counted; the significance level is preferably 0.05; for example, when the degree of freedom is 3, the preset chi-square threshold is 7.815; when the degree of freedom is 6, the preset chi-square threshold is 12.592.

[0049] As an example, the preset inflation coefficient is set according to the degree of exceeding the threshold of the consistency statistic: 2 when exceeding the threshold but not more than 2 times the threshold; 5 when exceeding 2 times but not more than 4 times; 10 when exceeding 4 times; the observation weight parameter is determined by the inflated noise level, and corresponding weights are generated for visual measurement components and docking distance components respectively, so that the worse the observation quality, the smaller the weight.

[0050] S1.5, based on the prior pose covariance matrix, the observation mapping relationship and the observation weight parameter, calculate the recursive fusion update coefficient, and perform recursive update on the prior pose mean vector and the prior pose covariance matrix with the observation residual, output the pose mean vector and the pose covariance matrix.

[0051] Specifically, the recursive fusion update coefficient is calculated, including: linearizing at the prior pose mean vector based on the observation mapping relationship to obtain a linearized observation relationship; performing weighted correction on the measurement noise parameter set based on the observation weight parameter to obtain a weighted measurement noise parameter set; calculating the recursive fusion update coefficient according to the linearized observation relationship, the prior pose covariance matrix and the weighted measurement noise parameter set, and performing recursive update on the prior pose mean vector and the prior pose covariance matrix based on the recursive fusion update coefficient.

[0052] When performing the recursive update, the observation residual is assigned to the correction of each component of the pose according to the recursive fusion update coefficient, and the uncertainty is updated synchronously: when the observation weight parameter is low, the correction amplitude of the pose mean vector is reduced and the convergence amplitude of the pose covariance matrix is reduced; when the observation weight parameter is high, the correction and convergence amplitudes are increased accordingly.

[0053] S2, determine a set of terminal safety inflation amounts based on the pose covariance matrix and a preset upper limit threshold of collision probability, and generate a terminal safety boundary from the set of terminal safety inflation amounts, and determine a station target-to-pose pose based on the pose mean vector. It needs to be explained that this step is:

[0054] S2.1, determine a confidence coefficient according to the preset upper limit threshold of collision probability, and determine the confidence coefficient as a boundary scale parameter corresponding to the terminal risk level.

[0055] It needs to be explained that the preset upper limit threshold of collision probability in the embodiment is given by the terminal-to-pose process and the station safety specification, such as 0.001; the terminal risk level refers to the risk level of interference between the terminal and the station structure, the material box boundary or the limiting mechanism after the terminal enters the station docking area, which corresponds to the preset upper limit threshold of collision probability; the confidence coefficient is determined by the preset upper limit threshold of collision probability under a preset distribution assumption or an empirical safety coefficient table, such as 3.29 corresponding to a more stringent risk level, or 2.58 corresponding to a general risk level, and the confidence coefficient is determined as a boundary scale parameter corresponding to the terminal risk level, so that the subsequent safety boundary changes with the risk level.

[0056] S2.2, perform eigenvalue decomposition on the pose covariance matrix to obtain a set of covariance principal axis directions and a set of variance eigenvalues corresponding to each principal axis direction.

[0057] Specifically, the eigenvalue decomposition includes: performing symmetry correction processing on the pose covariance matrix to obtain a symmetric pose covariance matrix; in the embodiment, the symmetry correction preferably adopts the way of averaging the corresponding elements and their transposed elements to suppress the non-symmetry items introduced by numerical propagation; if a very small negative eigenvalue appears after correction, the negative eigenvalue is truncated to 0 or a preset very small positive number to satisfy the semi-positive definite property of the covariance matrix; then, 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 set of eigenvectors is normalized to obtain a set of covariance principal axis directions; the set of covariance principal axis directions is 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; through sorting, the first principal axis direction corresponds to the direction of maximum uncertainty, which facilitates subsequent construction of a safety boundary in a direction-dependent manner.

[0058] S2.3, calculate expansion components along each principal axis direction based on the confidence coefficient and the set of eigenvalues of the covariance, and associate each expansion component with the set of principal axis directions of the covariance to form a set of final safe expansion amounts.

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

[0060] By generating the expansion component in the principal axis direction of the covariance, the final safe boundary is consistent with the shape of the pose uncertainty space, avoiding the directional mismatch caused by the fixed radius boundary; compared with the way of only taking the diagonal components of the covariance or fixing the safe distance, the pose and position correlation are taken into account in the boundary construction in this embodiment, and the boundary is more consistent with the actual risk distribution of the final stage.

[0061] S2.4, construct a confidence ellipsoid boundary centered on the target-to-stage pose with the set of final safe expansion amounts, and determine the confidence ellipsoid boundary as the final safe boundary.

[0062] In this embodiment, the directions of the semi-axes of the confidence ellipsoid boundary are determined by the set of principal axis directions of the covariance, and the lengths of the semi-axes are determined by the corresponding expansion components; for example, when the uncertainty of the walking direction is large, the semi-axis of the ellipsoid in this direction is longer, and the controller leaves a larger buffer in this direction; when the uncertainty of the lifting direction is small and the distance measurement is reliable, the semi-axis of the ellipsoid in this direction is shorter, which is beneficial to improve the alignment accuracy.

[0063] Further, in this embodiment, the target-to-stage pose is determined based on the pose mean vector, specifically including the following steps:

[0064] The visual measurement value of the stage reference marker is matched in the preset stage pose parameter table to obtain the stage reference pose: the matching is preferably performed according to the three-level key value of "marker number-stage work station number-camera field of view area": the visual measurement value is first decoded to obtain the marker number, and then the marker number is used to retrieve the corresponding stage work station number and read the stage reference pose entry; an example field of the preset stage pose parameter table includes: work station number, marker number, stage reference pose, stage allowed alignment tolerance, docking normal axis direction definition, stage structure safe boundary basic size, and process configuration field related to gripper / box;

[0065] The relative pose of the OHT relative to the stage is calculated based on the pose mean vector and the stage reference pose: the relative pose calculation is performed according to the unified coordinate system convention: the pose mean vector is converted to the stage coordinate system, and then the relative relationship with the stage reference pose is solved to obtain the relative pose result;

[0066] mapping the preset end geometry bias vector to the relative pose and superimposing to obtain the end docking pose: the preset end geometry bias vector is obtained by end assembly calibration, examples include fixed translation bias of end tool center point relative to body reference point, end docking face normal direction bias, and jaw center line bias; the above bias vector is written into the end calibration parameter table and solidified by the device debugging stage;

[0067] correcting the displacement component of the end docking pose along the docking normal axis direction based on the docking distance measurement value: the docking normal axis direction is the docking direction axis of the end towards the station, and the displacement correction is performed on the docking distance measurement value to make the distance constraint of the end in the feeding direction consistent with the actual measurement;

[0068] calculating the alignment residual based on the end docking pose and the station reference pose, and generating the station target alignment pose by substituting the alignment residual into the pose mean vector.

[0069] In a preferred embodiment, calculating the alignment residual based on the end docking pose and the station reference pose and substituting includes: uniformly representing the end docking pose and the station reference pose in the same station coordinate system; extracting the difference of the two in the walking 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 same component of the pose mean vector according to the component correspondence relationship to form the substitution correction; updating the pose mean vector with the substitution correction to output the station target alignment pose consistent with the station reference pose; when any component of the alignment residual exceeds the alignment tolerance item, the substitution correction corresponding to the component is reduced in proportion and written to make the station target alignment pose consistent with the size of the terminal safety boundary.

[0070] S3, based on the terminal safety boundary and the station target alignment pose, a probability constraint rolling prediction control model is constructed with a preset sampling period as the discrete step, and the walking axis control quantity and the lifting axis control quantity in the current sampling period are solved. Wherein, it needs to be explained that this step is:

[0071] S3.1, based on the preset sampling period, the discrete state update relationship of the walking axis and the lifting axis is established, and the station target alignment pose and the current pose mean vector are written into the prediction initial value to obtain the terminal discrete prediction model.

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

[0073] 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.

[0074] 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.

[0075] 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.

[0076] 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.

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

[0078]

[0079]

[0080]

[0081]

[0082] 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; allowing alignment tolerance value in the lifting direction; allowing alignment tolerance value in the posture direction.

[0083] When the alignment residual vector contains more components, the alignment residual weighting matrix W is extended according to the same rule as the above formula, and each diagonal element is taken as the reciprocal of the square of the corresponding allowed alignment tolerance value, so that the components with smaller tolerance have higher weights in the objective function.

[0084] S3.4, the control quantity sequence is obtained by rolling solving the probability constraint rolling prediction control model, and the first item of the control quantity sequence is taken as the walking axis control quantity and the lifting axis control quantity in the current sampling period. It includes:

[0085] The prediction initial value of the terminal discrete prediction model is taken as the initial state of the current rolling window, and the probability constraint set is written into the constraint term of the probability constraint rolling prediction control model; when the uncertainty rises, the scale parameter of the constraint term is tightened synchronously;

[0086] The alignment residual of each discrete step in the prediction time domain is calculated based on the target alignment pose of the platform, and the weighted cumulative value of the alignment residual is determined as the objective function value of the current rolling window; the weight can be set according to the rule that the weight of the step closer to the end of the prediction is greater, and the weight of the last two steps can be set as twice the weight of the previous steps;

[0087] The objective function value is iteratively solved under the condition of meeting the probability constraint set, and the candidate control quantity sequence corresponding to the minimum objective function value is output; the sequence quadratic programming or interior point method process is preferably used for iterative solving, and the maximum iteration number is set to 30 times; when the constraint is violated, the control quantity change step is contracted according to the violation degree until the convergence condition is met or the maximum iteration number is reached;

[0088] The candidate control quantity sequence is substituted into the terminal discrete prediction model to generate a prediction trajectory, and the prediction trajectory is subjected to full-time domain constraint checking with the terminal safety boundary; the full-time domain constraint checking rule is: the error of each step prediction pose in the prediction time domain relative to the target alignment pose of the platform is checked step by step to determine whether it falls within the corresponding terminal safety boundary of the step; when there is any step violation, the error direction of the violating step is taken as the constraint tightening direction of the next iteration and the candidate control quantity sequence is updated until the full-time domain checking passes; after the full-time domain checking passes, the candidate control quantity sequence is output as the control quantity sequence;

[0089] Through rolling solving and taking the first item, the controller re-calculates the control quantity based on the latest pose mean vector and pose covariance matrix in each sampling period, and the influence of terminal disturbance and observation quality change on the control output is limited within a short window.

[0090] Refer to Figure 2, the pose of the target alignment position in the platform coordinate system is taken as the position center point (0, 0), the mean vector of the pose obtained by fusion in the current sampling period corresponds to the end relative target position error: the walking direction is -40 mm, the lifting direction is +10 mm, and the end is allowed to be in the terminal segment safety boundary of the confidence ellipse with the target center as the ellipse center during the alignment process: the semi-axis lengths are a = 18 mm in the walking direction and b = 7 mm in the lifting direction; the candidate control quantity sequence obtained by rolling solution is the walking shaft and lifting shaft acceleration sequence in the prediction time domain, the candidate control quantity sequence is substituted back into the terminal segment discrete prediction model to obtain the predicted position points of each discrete step in the prediction time domain, and a prediction trajectory is formed; then, full-time domain constraint checking is performed on the prediction trajectory: whether each point meets the condition of ; if any predicted point falls outside the ellipse, the candidate control quantity sequence is determined as failed, and the control quantity change needs to be tightened or the safety constraint weight needs to be improved in the next rolling iteration; if all the time domains are within the ellipse, the candidate control quantity sequence is output, and the first item is taken as the current sampling period control quantity for issuing.

[0091] S4, the walking shaft control quantity and the lifting shaft control quantity are issued, and the pose mean vector and the pose covariance matrix are updated in the next sampling period; when the alignment residual of the updated pose mean vector relative to the pose of the target alignment position of the platform is within the terminal segment safety boundary and the docking distance measurement value is less than the preset docking distance threshold, a taking and placing action instruction is output. It should be noted that in this step:

[0092] S4.1, the walking shaft control quantity and the lifting shaft control quantity are issued to the actuator, and the control quantity time stamp corresponding to the preset sampling period is recorded.

[0093] In this embodiment, the actuator at least includes a walking shaft servo driver, a lifting shaft servo driver and a gripper driver, and the control quantity time stamp corresponding to the preset sampling period is recorded, the control quantity time stamp is written into the issuing record structure, and example fields include: a sampling period index, an issuing time stamp, a walking shaft instruction position value, a walking shaft instruction speed upper limit, a lifting shaft instruction position value, a lifting shaft instruction speed upper limit, a control mode identifier and a checking state identifier; through the control quantity time stamp, the next sampling period can retrieve the corresponding observation vector according to the same index and form a closed loop.

[0094] S4.2, 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 last sampling period are taken as the initial values for updating.

[0095] 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 this sampling period is generated according to the alignment rule of S1.1; and the pose mean vector and the pose covariance matrix output in the last sampling period are written into the fusion module as the update initial value, so that the rolling update is continuous with the state of the last period.

[0096] S4.3, extrapolate and propagate the update initial value according to the observation vector and the OHT discrete kinematic model to obtain the prior pose mean vector and the prior pose covariance matrix of the next sampling period.

[0097] In this embodiment, when it is detected that the encoder displacement increment exceeds the single-period displacement upper limit determined by the walking shaft speed upper limit and the preset sampling period, the encoder component of this sampling period is marked as low trust 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 update.

[0098] S4.4, calculate the observation residual and the consistency statistic based on the prior pose mean vector, the prior pose covariance matrix and the observation vector, and then perform recursive fusion update according to the observation weight parameter generated according to the consistency statistic to output the pose mean vector and the pose covariance matrix of the next sampling period.

[0099] For example, when the visual short-time occlusion causes the consistency statistic to exceed the threshold value, the observation weight parameter of the visual component is reduced, the observation weight parameter of the ranging component is 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 is faster converged to the distance condition that meets the pick-and-place action trigger.

[0100] Specifically, the pick-and-place action instruction is generated and issued according to the following instruction rule:

[0101] (1) When the pose mean vector after rolling update is located within the terminal safety boundary relative to the alignment residual of the station target pose, and the docking distance measurement value is less than the preset docking distance threshold value, the jaw opening displacement set value, the jaw clamping force limit value, the lifting shaft pick-and-place height set value, the execution timeout threshold value and the back-off displacement set value are retrieved from the preset pick-and-place action parameter table according to the pick-and-place type identifier.

[0102] The preset docking distance threshold value is given by the safe distance before the end contacts the workpiece allowed by the pick-and-place process, and an example is 2.0 mm; its setting rule is: no more than 30% of the end buffer stroke, and no more than 6 times of the standard deviation of the ranging noise.

[0103] The jaw opening and closing displacement setting value is determined by the workpiece outer dimension and the jaw structure. For example, the opening displacement setting value is 28 mm, and the clamping displacement setting value is 18 mm. The setting rule is that the opening displacement setting value is not less than the maximum workpiece outer dimension plus the assembly allowance (e.g. 3 mm), and the clamping displacement setting value is not greater than the minimum workpiece outer dimension minus the clamping allowance (e.g. 1 mm).

[0104] The jaw clamping force limit value is determined by the workpiece allowable clamping force and the jaw driving capability. For example, the value is 35 N. The setting rule is that it is not more than 80% of the upper limit of the workpiece allowable clamping force, and is obtained by combining the jaw force sensor or the current estimation model calibration.

[0105] The lifting shaft taking and placing height setting value is obtained by the docking surface height of the station and the end geometry offset calibration. For example, it includes two levels of pre-approaching height and taking and placing height, and the pre-approaching height is higher than the taking and placing height. The setting rule is that the difference between the two levels of height is not less than 5 times the maximum displacement amount of the lifting shaft in a single cycle.

[0106] The execution timeout threshold is determined by the maximum action time statistics of the jaw driving. For example, the value is 1.2 s. The setting rule is that it is not less than 1.5 times the average time of completing a full stroke action under the rated condition.

[0107] The back-off displacement setting value is determined by the station retreat space and the re-alignment requirement. For example, the value is 15 mm. The setting rule is that it is greater than 3 times the end safety gap and less than 80% of the upper limit of the station retreat space.

[0108] The example fields of the preset taking and placing action parameter table include: taking and placing type identification, workpiece type identification, opening displacement setting value, clamping displacement setting value, clamping force limit value, pre-approaching height setting value, taking and placing height setting value, execution timeout threshold, back-off displacement setting value, jaw state tolerance interval, and action sequence number set.

[0109] (2) Write the walking shaft control amount and the lifting shaft control amount into the resident control setting to generate the walking shaft resident control amount and the lifting shaft resident control amount, wherein the resident control setting includes a speed setting value of zero and a position setting value taking the instruction position value of the last sampling period;

[0110] (3) When the taking and placing type identification is the taking identification, write the lifting shaft taking and placing height setting value and the jaw opening and closing displacement setting value into the taking and placing action instruction in the order of switching the opening displacement to the clamping displacement, and take the jaw clamping force limit value as one of the clamping termination conditions;

[0111] (4) When the taking and placing type identification is the placing identification, write the lifting shaft taking and placing height setting value and the jaw opening and closing displacement setting value into the taking and placing action instruction in the order of switching the clamping displacement to the opening displacement;

[0112] (5) When the timeout threshold is reached and the gripper state quantity does not reach the tolerance interval corresponding to the gripper opening and closing displacement set value, a back-off displacement set value is generated to generate a walking shaft back-off control quantity and is issued, and the lifting shaft dwell control quantity is maintained as the last sampling period issued value.

[0113] The tolerance interval is determined by the upper bound of the repeatability error of the gripper position sensor and the upper bound of the mechanical gap of the gripper, and an example is ±0.5 mm; the setting method is to take the sum of three times the upper bound of the repeatability error of the sensor and the upper bound of the mechanical gap, and write this interval into the preset pick-and-place action parameter table.

[0114] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present application and are not limiting. Although the present application has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that the technical solutions of the present application can be modified or replaced equivalently without departing from the spirit and scope of the technical solutions of the present application, and they should be covered in the scope of the claims of the present application.

Claims

1. An automatic pick-and-place control method based on OHT positioning, characterized by, The method comprises the following steps: Collecting the displacement of the OHT walking shaft incremental encoder, the angular velocity and linear acceleration measurement values of the inertial measurement unit, the visual measurement values of the station reference marker, and the docking distance measurement values of the terminal ranging sensor under a preset sampling period, and performing fusion recursive update to obtain the mean pose vector and the pose covariance matrix; Determining a set of terminal safety inflation amounts based on the pose covariance matrix and a preset upper limit threshold of collision probability, and generating a terminal safety boundary from the set of terminal safety inflation amounts, and determining the station target alignment pose based on the mean pose vector; Based on the terminal safety boundary and the station target alignment pose, a probability constraint rolling prediction control model is constructed with the preset sampling period as the discrete step, and the walking shaft control amount and the lifting shaft control amount in the current sampling period are solved; The walking shaft control amount and the lifting shaft control amount are issued and the mean pose vector and the pose covariance matrix are updated in the next sampling period; When the alignment residual of the mean pose vector after rolling update relative to the station target alignment pose is located within the terminal safety boundary and the docking distance measurement value is less than a preset docking distance threshold, output the pick-and-place action instruction.

2. The OHT positioning based automatic pick and place control method of claim 1, wherein, The mean pose vector and the pose covariance matrix are obtained, comprising: Timestamp alignment is performed on the OHT walking shaft incremental encoder displacement, the angular velocity and linear acceleration measurement values, the visual measurement values of the station reference marker, and the docking distance measurement values based on the preset sampling period to obtain an observation vector corresponding to the same sampling period; According to the OHT discrete kinematics model, the components related to motion extrapolation in the observation vector are integrated and state propagated, and the mean pose vector and the pose covariance matrix of the last sampling period are used as the propagation initial value to obtain the prior mean pose vector and the prior pose covariance matrix; An observation mapping relationship is established based on the visual measurement values of the station reference marker and the docking distance measurement values, and observation residuals and consistency statistics are calculated according to a preset measurement noise parameter set; The consistency statistics are compared with a preset chi-square threshold, when the consistency statistics are greater than the preset chi-square threshold, a preset inflation coefficient is applied to the measurement noise parameter set to obtain an inflation measurement noise parameter set, and observation weight parameters are generated accordingly; when the consistency statistics are 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 coefficient is calculated based on the prior pose covariance matrix, the observation mapping relationship and the observation weight parameters, and the prior mean pose vector and the prior pose covariance matrix are recursively updated with the observation residuals to output the mean pose vector and the pose covariance matrix.

3. The OHT positioning based automatic pick and place control method of claim 2, wherein, The recursive fusion update coefficient is calculated, comprising: Linearization is performed on the prior mean pose vector based on the observation mapping relationship to obtain a 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 coefficient is calculated according to the linearized observation relationship, the prior pose covariance matrix and the set of weighted measurement noise parameters, and the recursive update is performed on the prior pose mean vector and the prior pose covariance matrix based on the recursive fusion update coefficient.

4. The OHT positioning-based automatic pick-and-place control method according to claim 1 or 2, characterized by, The terminal safety boundary is generated, including: A confidence coefficient is determined according to the preset upper limit threshold of the collision probability, and the confidence coefficient is determined as a boundary scale parameter corresponding to a terminal risk level; The pose covariance matrix is subjected to eigenvalue decomposition to obtain a set of covariance principal axis directions and a set of variance eigenvalues corresponding to each principal axis direction; Based on the confidence coefficient and the set of variance eigenvalues, an expansion component along each principal axis direction is calculated, and each expansion component is associated with the set of covariance principal axis directions to form a set of terminal safety expansion amounts; The set of terminal safety expansion amounts constitutes a confidence ellipsoid boundary centered on the target-to-position pose of the platform, and the confidence ellipsoid boundary is determined as the terminal safety boundary.

5. The OHT positioning based automatic pick and place control method according to claim 4, wherein, The eigenvalue decomposition includes: The symmetric pose covariance matrix is obtained by performing symmetry correction processing on the pose covariance matrix; The set of variance eigenvalues and the corresponding set of eigenvectors are obtained by performing eigenvalue decomposition operation on the symmetric pose covariance matrix; Each eigenvector in the set of eigenvectors is normalized to obtain the set of covariance principal axis directions; The set of covariance principal axis directions is 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.

6. The OHT positioning based automatic pick-and-place control method according to claim 1 or 2, characterized by, The target-to-position pose of the platform is determined based on the pose mean vector, including: A platform reference pose is matched in a preset platform pose parameter table according to the visual measurement value of the platform reference marker; A relative pose of the OHT relative to the platform is calculated based on the pose mean vector and the platform reference pose; A preset end geometric bias vector is mapped to the relative pose and superimposed to obtain an end docking pose, and a displacement component of the end docking pose along the docking normal axis direction is corrected based on the docking distance measurement value; A docking residual is calculated based on the end docking pose and the platform reference pose, and the docking residual is substituted back to the pose mean vector to generate the target-to-position pose of the platform.

7. The OHT positioning based automatic pick-and-place control method of claim 6, wherein, The output of the walking axis control amount and the lifting axis control amount of the current sampling period includes: A discrete state update relationship of the walking axis and the lifting axis is established based on the preset sampling period, and the target-to-position pose of the platform and the current pose mean vector are written into a prediction initial value to obtain a terminal discrete prediction model; A pose prediction sequence in a prediction time domain is generated based on the terminal discrete prediction model, and the terminal safety boundary is corresponded to a probability constraint set in the prediction time domain according to the pose covariance matrix and the preset upper limit threshold of the collision probability; The probability constraint set is introduced into the terminal discrete prediction model, and a docking residual corresponding to the target-to-position pose of the platform is taken as an optimization target 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 OHT positioning based automatic pick-and-place control method of claim 7, wherein, 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 OHT positioning based automatic pick and place control method of claim 1, wherein, 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 OHT positioning based automatic pick and place control method of claim 1, wherein, 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 taking and placing type is identified as the material placing type, the taking and placing height setting value of the lifting shaft, the opening and closing displacement setting value of the gripper, and the sequence of switching the gripper displacement from the clamping displacement to the opening displacement are written into the taking and placing action instruction; When the execution timeout threshold is reached and the gripper state quantity does not reach the tolerance interval corresponding to the gripper opening and closing displacement setting value, the rollback displacement setting value is generated to generate a walking shaft rollback control quantity and is delivered, and the lifting shaft dwell control quantity is maintained as the delivered value in the last sampling period.

Images