A path planning early warning method and system of a wafer box cleaning equipment

Abstract

The present application relates to the field of industrial vision technology, in particular to a wafer box cleaning equipment path planning early warning method and system, comprising, collecting the multi-source running state of the manipulator and the chamber video stream, quantifying the joint wear degree based on vibration and current data, establishing a kinematic constraint model that limits the motion performance; At the same time, industrial vision detection and three-dimensional space reconstruction are performed on the video stream, and fluid disturbance and pollution area are accurately positioned and mapped as forbidden voxel. Through the fusion of health constraints and environmental risks, a composite navigation map is generated and an initial trajectory is planned, a rigid body dynamics model is used for prospective joint torque verification, and when overload risk is detected, secondary inflation re-planning or time scaling degradation strategy is executed, and finally a smooth instruction is used to drive the manipulator. The present application realizes the adaptive safety planning of the manipulator through health constraints and visual perception.

Description

Technical Field

[0001] This invention relates to the field of industrial vision technology, specifically to a path planning and early warning method and system for wafer cell cleaning equipment. Background Technology

[0002] The core technologies of industrial-grade multi-joint robotic arms are program control and trajectory planning. In high-precision, high-value-intensive production environments such as wafer cell cleaning equipment, robotic arms need to perform high-repetition-frequency handling tasks, making the reliability and efficiency of their motion control programs crucial.

[0003] Current robotic arm motion programs primarily rely on offline-established static geometric models and fixed safety parameters when planning trajectories. That is, the trajectory planning algorithm within the program is usually calculated based on a static chamber model and assumes that the motion envelope parameters of the robotic arm actuator (such as maximum operating speed and allowable torque output) remain constant throughout its lifespan.

[0004] However, when the robotic arm operates inside the cleaning chamber, this fixed-parameter program execution logic faces two major challenges: Impact of dynamic obstacles on the program: The high-speed fluid medium accompanying the cleaning process forms non-rigid dynamic obstacles. Existing program safety loops struggle to accurately and in real-time incorporate such dynamic media into trajectory planning and physical avoidance cost calculations within the program. Any statically planned motion program may fail due to dynamic disturbances.

[0005] Performance degradation and motion envelope rigidity: Physical wear and tear on critical actuator components such as robotic joints leads to a decline in their actual torque output capability. Due to a lack of dynamic awareness of the actuator's real-time health status, existing motion programs cannot dynamically adjust the maximum permissible motion envelope based on the component's actual load-bearing capacity. This results in programs that are either overly conservative during execution (sacrificing efficiency) or exhibit unpredictable overload failures (increasing risk).

[0006] To address this, a path planning and early warning method and system for wafer cell cleaning equipment is proposed. Summary of the Invention

[0007] The purpose of this invention is to provide a path planning and early warning method and system for wafer cassette cleaning equipment, which achieves adaptive safety planning for a robotic arm through health constraints and visual perception. This includes: acquiring multi-source operating status data of the robotic arm and video streams from the chambers; quantifying joint wear based on vibration and current data to establish a kinematic constraint model limiting motion performance; simultaneously performing industrial visual inspection and 3D spatial reconstruction on the video stream to accurately locate fluid interference and contamination areas and map them as prohibited voxels; generating a composite navigation map by integrating health constraints and environmental risks and planning an initial trajectory; using a rigid body dynamics model for forward-looking joint torque verification; and executing a secondary expansion replanning or triggering a time scaling degradation strategy when overload risk is detected, ultimately generating smooth commands to drive the robotic arm.

[0008] To achieve the above objectives, the present invention provides the following technical solution: A path planning and early warning method for a wafer cassette cleaning device includes: Collect the current values ​​of the robot arm joints, encoder positions, arm vibration signals, and environmental video streams within the cleaning chamber; calculate the current pose of the robot arm's end effector and analyze the target pose according to instructions; Based on vibration signals and current values, the degree of joint wear is assessed, and a constraint model is established in the joint space. Industrial vision inspection is performed on the environmental video stream to locate the spatial coordinates of the thermal deionized water fluid interference area and the fluorescent contamination area, which are then mapped onto a three-dimensional voxel map and marked as prohibited voxels of the robot arm. An end-reachability potential field generated based on the constraint model is superimposed on the three-dimensional voxel map, and entity safety boundary expansion is performed on the prohibited voxels to generate a three-dimensional composite map. Generate an initial trajectory connecting the current pose and the target pose in the 3D composite map; input the initial trajectory into the rigid body dynamics model of the handling robot to verify the joint torque, calculate the torque load rate, and if the torque load rate exceeds the preset safety threshold, perform secondary boundary expansion on the forbidden voxels and trigger replanning to generate alternative trajectories. The selected trajectory is subjected to motion school verification. If the verification is abnormal, an early warning is triggered. If the verification is normal, control commands are generated to drive the handling robot to perform the operation.

[0009] Preferably, the process of establishing a constraint model in the joint space includes: performing time-domain analysis on the collected vibration time-series signal to obtain the root mean square value characterizing mechanical vibration energy and the kurtosis index characterizing impact damage, while calculating the dynamic load deviation of the current feedback value relative to the ideal standard dynamic model; using the logistic normalization function to comprehensively evaluate the root mean square value, kurtosis index, and dynamic load deviation into a performance degradation coefficient reflecting the actual torque output capability of each joint of the handling robot; constructing a digital model reflecting the physical health state of the handling robot, wherein the digital model adopts the form of a joint performance degradation matrix, and the elements in the matrix are the performance degradation coefficients; based on the joint performance degradation matrix, dynamically adjusting the motion cost weight of the corresponding joint axis in the path planning, and performing motion control parameter derating operations on the maximum allowable angular velocity and maximum allowable angular acceleration of joints with wear, to establish a kinematic constraint model that limits the actual dynamic performance of the robot.

[0010] Preferably, the process of performing industrial visual inspection and 3D spatial reconstruction on the environmental video stream includes: modeling the background of the cleaning chamber using a Gaussian mixture model; extracting the foreground mask by difference between the current frame of the real-time video stream and the background model; calculating the gray-level co-occurrence matrix entropy value of the foreground mask region; determining the region with an entropy value higher than a preset fluid texture threshold as the 2D image contour of the hot deionized water fluid interference region; simultaneously converting the real-time video stream to the HSV color space; extracting the saturation channel S and the brightness channel V respectively; performing dual threshold segmentation and connected component analysis to extract the 2D image contour of the fluorescent contamination region; constructing a spatial projection ray from the camera optical center through each pixel in the 2D image contour based on the pinhole camera imaging model and the camera intrinsic parameter matrix; using the 3D Bressenham algorithm to track the penetration path of the spatial projection ray in the coordinate system of the cleaning chamber; traversing all voxel units along the path; marking the occupancy status of the voxel units as occupied; completing the 3D spatial reconstruction of the fluid interference region and the fluorescent contamination region and defining them as prohibited voxels for the robotic arm.

[0011] Preferably, the process of generating the three-dimensional composite map includes: traversing the center point of each non-obstacle voxel in the map, calculating the joint angle vector required for the end effector of the handling robot to reach the center point using an inverse kinematics solver, weighting and summing the displacement of each joint in the joint angle vector with the weight coefficients in the joint performance degradation matrix to generate an accessibility potential field value that quantifies the cost of motion execution; constructing a spherical structural element with a radius equal to the sum of the maximum cross-sectional radius of the handling robot link and a preset safety buffer distance, performing a three-dimensional morphological dilation operation on the robot's prohibited voxel set using the spherical structural element, defining the area covered by the dilation operation as the robot's physical avoidance zone; normalizing the accessibility potential field value, and linearly superimposing it with the obstacle occupancy probability value of the robot's physical avoidance zone to generate the three-dimensional composite map used to guide path search.

[0012] Preferably, the initial trajectory generation process includes: initializing the path search list, establishing an open list for storing nodes to be expanded and a closed list for expanded nodes, and adding the current pose to the open list; cyclically selecting the node with the smallest comprehensive travel cost in the open list as the current parent node, traversing the neighboring voxel nodes of the parent node, and calculating the comprehensive travel cost of the neighboring voxel nodes; the comprehensive travel cost consists of three parts: the first part is the normalized value of the actual geometric distance accumulated along the path from the current pose, the second part is the normalized value of the straight-line Euclidean distance to the target pose, and the third part is the health and interference cost value read from the three-dimensional composite map; summing the three parts to obtain the comprehensive travel cost, and updating the parent node pointer of the neighboring nodes until the target pose is searched, and extracting the voxel center coordinates by backtracking according to the parent node pointer to generate the initial trajectory.

[0013] Preferably, the process of verifying joint torque and generating alternative trajectories includes: converting the initial trajectory into a sequence of joint angles in joint space using an inverse kinematics solver, and calculating the angular velocity and angular acceleration sequences of each joint using the finite difference method; calling the recursive Newton-Euler algorithm, and combining the link mass, center of mass position, and inertia tensor parameters of the handling robot, using the joint angle, angular velocity, and angular acceleration sequences as input, to predict the dynamic torque demand of each joint when the robot moves along the trajectory; dynamically adjusting the rated peak torque of each joint according to the performance degradation coefficient to determine the torque safety boundary under the current wear state; calculating the ratio of the dynamic torque demand to the torque safety boundary as the dynamic torque load rate; if the maximum load rate exceeds 100%, it is determined that the current trajectory will cause the robot to overload, locking the risky section of the trajectory adjacent to the robot's prohibited voxels, and expanding the prohibited voxels around the risky section by a multiple greater than the initial expansion radius, and re-planning and generating the alternative trajectory in the updated map.

[0014] Preferably, the generation of control commands to drive the handling robot to perform operations specifically includes: using the discrete path points of the finally selected trajectory as model points, inversely calculating B-spline control points, and using cubic B-spline basis functions to fit the discrete path into a spatial motion curve with second-order geometric continuity; performing S-shaped velocity planning on the spatial motion curve, calculating the time parameters of each point on the path, and obtaining a sequence of motion parameters for the entire process including the planned position, planned velocity, and planned acceleration; traversing the sequence of motion parameters for the entire process, detecting whether any motion parameter at any moment exceeds the physical limit threshold of the handling robot, and if so, intercepting the output and alarming; if the verification passes, isochronously sampling the spatial motion curve according to the control cycle of the servo driver, generating a PVT control command sequence including position command, velocity feedforward, and acceleration feedforward, synchronously sending it to each joint servo driver through the fieldbus, and driving each joint motor to drive the handling robot to move smoothly along the planned trajectory through current loop closed-loop control.

[0015] A path planning and early warning system for a wafer cassette cleaning equipment includes: The data monitoring module collects current feedback values ​​from each joint of the robotic arm, encoder position values, arm vibration signals, and environmental video streams within the cleaning chamber; it calculates the current pose of the end effector of the handling robotic arm and analyzes the target pose according to instructions. The visual inspection module quantifies the mechanical wear degree of each joint based on vibration signals and current feedback values, and establishes a constraint model in the joint space; it performs visual inspection and 3D spatial reconstruction on the environmental video stream, locates the spatial coordinates of the thermal deionized water fluid interference area and the fluorescent contamination area, maps them to the 3D voxel map and marks them as forbidden voxels; it overlays the end reachability potential field generated based on the constraint model on the 3D voxel map, performs entity safety boundary expansion on the forbidden voxels, and generates a 3D composite map; The dynamics verification and correction module generates an initial trajectory connecting the current pose and the target pose in a 3D composite map; the initial trajectory is input into the rigid body dynamics model of the handling robot to verify the joint torque, calculate the torque load rate, and if the torque load rate exceeds the preset safety threshold, a secondary boundary expansion is performed on the forbidden voxels and a replanning is triggered to generate alternative trajectories. The motion execution control module performs trajectory smoothing and interpolation calculations on the final selected trajectory, generates control commands, and drives the handling robot to perform the operation.

[0016] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. By accurately collecting vibration and current signals, the physical health degradation of the actuator is assessed in real time and digitized into motion control parameters, such as adjusting the maximum speed / acceleration limit and path planning weight. This method enables the robot control system to dynamically de-rate according to the actual load-bearing capacity of the components, avoiding unpredictable overloads or shutdowns when the performance of critical joints deteriorates. This significantly improves the operational reliability of the equipment throughout its entire life cycle and enables more accurate predictive maintenance, effectively extending the service life of the robot arm.

[0017] 2. By accurately reconstructing the fluid interference zone and contamination zone in three-dimensional space and incorporating it into the cost calculation of the composite navigation map, the trajectory planning is able to proactively avoid any dynamic, non-rigid contamination risk sources in real time. This technology significantly improves the safety margin of the robotic arm in complex wet environments and avoids the risk of secondary contamination of the wafer by the fluid medium in traditional methods.

[0018] 3. By introducing a rigid body dynamics model, a forward-looking torque check is performed on the planned trajectory, and the dynamic load rate is calculated to ensure the physical feasibility of the trajectory. If the initial check fails, the system will expand the safety boundary a second time and trigger replanning to find alternative paths that meet the safety requirements. Finally, B-spline smoothing is performed on the selected trajectory to ensure that the generated control commands have a high level of motion continuity, effectively suppressing the impact during start-up and shutdown, and providing traceable and verifiable physical safety assurance for the handling of high-value wafers. Attached Figure Description

[0019] Figure 1 This is a schematic diagram of the path planning and early warning method for a wafer cassette cleaning device according to the present invention; Figure 2 This is a schematic diagram of the process for generating the final trajectory according to the present invention; Figure 3 This is a schematic diagram of the path planning and early warning system of a wafer cassette cleaning device according to the present invention. Detailed Implementation

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

[0021] Please see Figures 1 to 3 This invention provides a path planning and early warning method and system for wafer cell cleaning equipment, the technical solution of which is as follows: Example 1: A path planning and early warning method for wafer cell cleaning equipment, the specific process of which is as follows: Figure 1 As shown, it includes: Collect the current values ​​of the robot arm joints, encoder positions, arm vibration signals, and environmental video streams within the cleaning chamber; calculate the current pose of the robot arm's end effector and analyze the target pose according to instructions; Based on vibration signals and current values, the degree of joint wear is assessed, and a constraint model is established in the joint space. Industrial vision inspection is performed on the environmental video stream to locate the spatial coordinates of the thermal deionized water fluid interference area and the fluorescent contamination area, which are then mapped onto a three-dimensional voxel map and marked as prohibited voxels of the robot arm. The end reachability potential field generated based on the constraint model is superimposed on the three-dimensional voxel map, and entity safety boundary expansion is performed on the prohibited voxels to generate a three-dimensional composite map. Generate an initial trajectory connecting the current pose and the target pose in the 3D composite map; input the initial trajectory into the rigid body dynamics model of the handling robot to verify the joint torque, calculate the torque load rate, and if the torque load rate exceeds the preset safety threshold, perform secondary boundary expansion on the forbidden voxels and trigger replanning to generate alternative trajectories. The selected trajectory is subjected to motion school verification. If the verification is abnormal, an early warning is triggered. If the verification is normal, control commands are generated to drive the handling robot to perform the operation.

[0022] The data acquisition required by this method is based on an industrial-grade real-time fieldbus, and all data must be strictly synchronized in time.

[0023] The system acquires current components representing dynamic torque requirements and angular position data from absolute encoders from the servo drives of each joint, serving as the basic input for subsequent torque verification and pose calculation. Three-axis accelerometers are deployed at key joints of the robot to collect high-frequency vibration timing signals, which are used as raw data for analyzing component wear characteristics. An industrial vision camera is deployed to acquire real-time video streams from the chamber; the camera requires geometric calibration to provide accurate intrinsic and extrinsic parameter matrices. The initial pose is obtained by using the acquired absolute encoder values, combined with the robot's pre-stored DH kinematic model parameters, to calculate the current three-dimensional coordinates and posture through forward kinematics. The target pose is determined by the main control system based on the currently executed process recipe instructions, which are then analyzed and the workstation coordinates are retrieved to determine the final target point.

[0024] Furthermore, the process of establishing a constraint model in the joint space includes: performing time-domain analysis on the collected vibration time-series signal to obtain the root mean square value characterizing mechanical vibration energy and the kurtosis index characterizing impact damage, while calculating the dynamic load deviation of the current feedback value relative to the ideal standard dynamic model; using the logistic normalization function to comprehensively evaluate the root mean square value, kurtosis index, and dynamic load deviation into a performance degradation coefficient reflecting the actual torque output capability of each joint of the handling robot; constructing a digital model reflecting the physical health state of the handling robot, the model adopting the form of a joint performance degradation matrix, the elements of the matrix being the performance degradation coefficient; based on the joint performance degradation matrix, dynamically adjusting the motion cost weight of the corresponding joint axis in the path planning, and performing motion control parameter derating operations on the maximum allowable angular velocity and maximum allowable angular acceleration of joints with wear, establishing a kinematic constraint model that limits the actual dynamic performance of the robot.

[0025] To assess the real-time physical health status of joints, the acquired signals need to be quantified. The vibration time-series signals are analyzed using standard time-domain statistical methods: the root mean square value representing vibration energy is obtained by calculating the arithmetic square root of the signal's square mean; simultaneously, the kurtosis index representing impact characteristics is obtained by calculating the ratio of the fourth-order central moment of the signal to the square of its variance; furthermore, the dynamic load deviation is calculated by comparing the actual current feedback value with the current required by the ideal standard dynamic model. These characteristic parameters are the basic inputs for subsequent assessments. A logistic regression model is used to fuse and assess multi-source signals. This model uses an sigmoid nonlinear function to map the linearly weighted results into a probabilistic health index.

[0026] Specifically, the model receives the root mean square value, kurtosis index, and dynamic load deviation as input variables. Because the three physical indicators have vastly different units of measurement and magnitudes of magnitude, the system first uses pre-stored historical statistical parameters to perform standard score processing on the three input data, converting them into dimensionless values ​​with a mean of zero and a variance of one, thereby eliminating the interference of different physical units on the model evaluation results. The core of the logistic regression model lies in determining the weights corresponding to each input independent variable, i.e., the regression coefficients. In this embodiment, the regression coefficients are not obtained through traditional mathematical iterative training, but are determined by a combination of offline accelerated life testing and principal component analysis: During the system calibration phase, a full life-cycle wear test is performed on the same type of joint reducer, collecting data from the entire process from health to failure; subsequently, principal component analysis is performed on the data, calculating the variance contribution rate of each physical feature in the first principal component and normalizing it. The system directly sets the normalized variance contribution rate as the regression coefficient corresponding to each input independent variable in the logistic regression model. This method can objectively assign greater weight to indicators sensitive to early failures (such as kurtosis); the model first calculates the linear weighted sum of the above-mentioned standardized input values ​​and the corresponding regression coefficients; then, the linear weighted sum is input into the sigmoid function (i.e., the logistic function). By utilizing the saturation curve characteristics unique to the S-shaped function, the nonlinear evolution of mechanical wear—from a gradual initial decline, to accelerated growth in the middle stage, and finally to a sharp deterioration in the final stage—is simulated. A wide-ranging weighted sum is mapped to a value between zero and one. The final output value of the logistic regression model is defined as the performance degradation coefficient. The closer this coefficient is to one, the higher the probability that the joint is in a healthy state, i.e., the stronger its torque output capability; the closer the coefficient is to zero, the greater the risk of joint failure and the more severe the wear.

[0027] By converting collected vibration and current data into performance degradation coefficients and directly applying them to motion cost weights and physical limiting derating operations in path planning, this method enables the program control system to perceive and respect the actual load-bearing capacity of the robotic arm in real time. This effectively avoids the risk of sudden overloads caused by wear on critical components, significantly improving the reliability and adaptability of the equipment.

[0028] Furthermore, the process of performing industrial visual inspection and 3D spatial reconstruction on the environmental video stream includes: modeling the background of the cleaning chamber using a Gaussian mixture model; extracting the foreground mask by difference between the current frame of the real-time video stream and the background model; calculating the gray-level co-occurrence matrix texture feature vector of the foreground mask region, wherein the texture feature vector consists of three feature components: energy, contrast, and entropy; identifying the region with an entropy value higher than the fluid entropy threshold, energy lower than the fluid uniformity threshold, and contrast higher than the fluid contrast threshold as the 2D image contour of the hot deionized water fluid interference region; and simultaneously converting the real-time video stream to HSV color. In space, the saturation channel S and the brightness channel V are extracted separately, and dual threshold segmentation and connected component analysis are performed to extract the two-dimensional image contour of the fluorescent contamination area. Based on the pinhole camera imaging model and the camera intrinsic parameter matrix, a spatial projection ray is constructed from the optical center of the camera and passes through each pixel in the two-dimensional image contour. The three-dimensional Bressenham algorithm is used to track the penetration path of the spatial projection ray in the coordinate system of the cleaning chamber, traverse all voxel units passed on the path, and mark the occupancy status of the voxel units as occupied, thus completing the three-dimensional spatial reconstruction of the fluid interference area and the fluorescent contamination area and defining them as prohibited voxels for the robot arm.

[0029] First, a Gaussian mixture model is used to dynamically and adaptively statistically model the static background of the cleaning chamber using a Gaussian mixture model. Then, the current frame is differenced from this background model to extract a foreground mask containing all moving objects. To accurately distinguish between non-rigid water flow interference and rigid body motion such as that of a robotic arm, Gray-Level Co-occurrence Matrix (GLCM) analysis is performed on this foreground mask region, and a texture feature vector is calculated. This vector specifically includes three feature components: energy, contrast, and entropy. Because high-speed thermal deionized water fluid visually exhibits highly chaotic texture (corresponding to high entropy), extremely uneven local gray-level distribution (corresponding to low energy), and strong edge changes caused by turbulence and reflection (corresponding to high contrast), ... The surface of a robotic arm typically has stable texture (low entropy) and uniform distribution (high energy). Therefore, a multi-feature joint judgment logic is adopted: regions with entropy values ​​higher than the fluid entropy threshold, energy lower than the fluid uniformity threshold, and contrast higher than the fluid contrast threshold are accurately identified as the two-dimensional image contours of the thermal deionized water fluid interference zone. At the same time, to identify fluorescence contamination, the system converts the video stream from RGB format to HSV color space. This operation decouples color information from brightness, improving the robustness of segmentation. Subsequently, dual threshold segmentation is performed on the saturation channel (S, representing color purity) and the brightness channel (V, representing brightness). Only connected regions where pixel values ​​simultaneously meet the threshold requirements of both channels are identified as the two-dimensional image contours of the fluorescence contamination zone.

[0030] The dual threshold segmentation includes a brightness threshold and a saturation threshold. Based on real-time statistical analysis of the background reference area, the average value and dispersion parameter of the V channel are obtained. The brightness threshold is set as the average brightness plus a safety margin value determined by offline system calibration to adapt to the external environmental baseline. A visual uncertainty index for predicting image noise level is constructed. The visual uncertainty index is a predicted value calculated by multiplying the absolute values ​​of the angular velocity and angular acceleration of the handling robot along the current trajectory by the reciprocal of the performance degradation coefficient, and combining it with camera parameters. The misjudgment rate of real-time visual detection is used as a feedback error signal. The visual uncertainty index and the error signal are fitted online. Based on the fitting results, the value of the saturation threshold is dynamically optimized and determined.

[0031] First, for the brightness threshold, real-time statistical analysis is performed on the pre-selected background reference area to obtain the average value and dispersion parameter of the area in the V channel (brightness). The brightness threshold is set as the average brightness plus a safety margin value determined by offline system calibration. This setting enables the threshold to dynamically follow the real-time changes of the external environmental baseline such as light and water mist in the cleaning chamber, ensuring the adaptability of the detection baseline.

[0032] To dynamically adjust the saturation threshold, a visual uncertainty index prediction model was constructed. This index is calculated by multiplying the absolute values ​​of the angular velocity and angular acceleration of the handling robot along its current trajectory by the reciprocal of the performance degradation coefficient reflecting joint wear, and combining this with camera parameters. This index characterizes the maximum blur and noise level that an image may produce, considering robot wear and motion amplitude. Subsequently, an iteratively optimized adaptive model was used to dynamically determine the value. Specifically, the model first collects the false judgment rate of visual detection in real time as a feedback error signal. Then, using this error signal and the visual uncertainty index at the current moment as input, the model uses a minimum error iterative algorithm to continuously adjust and optimize the internal threshold mapping parameters. Through this real-time data fitting and parameter iteration process, the strictness can continuously track and adapt to the actual motion state and wear of the robot, thus completing the final dynamic coupling setting of the dual thresholds.

[0033] By constructing a visual uncertainty index based on health and motion parameters, online adaptive detection thresholds are achieved, effectively solving the problem that traditional vision systems cannot distinguish between noise and real contamination during high-speed motion and component wear, and improving the anti-interference capability and long-term operational reliability of robot visual inspection.

[0034] Based on a pre-calibrated pinhole camera imaging model, and utilizing the camera's intrinsic and extrinsic parameter matrices, the spatial coordinate system is defined. For each detected two-dimensional image contour point, a spatial projection ray is constructed, extending from the camera's optical center through that point to the spatial depth. Subsequently, an efficient three-dimensional Bressenham line algorithm is used to trace the penetration path of the spatial projection ray in the cleaning chamber coordinate system. This algorithm can accurately traverse all three-dimensional voxel units along the ray path. Finally, the occupancy probability value of all voxel units covered by the ray penetration path is marked as 1.0, thereby completing the spatial physical reconstruction of the fluid interference area and the fluorescent contamination area. The set of voxel units marked as occupied is defined as the robot arm's prohibited voxel, serving as the input for the absolute physical avoidance zone in subsequent path planning.

[0035] By combining Gaussian mixture model and gray-level co-occurrence matrix entropy analysis, the system effectively distinguishes between transparent, non-rigid water flow interference and background noise, solving the problem of fluid medium identification in wet environments using traditional methods. Simultaneously, utilizing a precise camera geometry model (pinhole camera model) and the Bressenham algorithm, the detected two-dimensional obstacle contours are reliably reconstructed into three-dimensional forbidden voxels, providing high-fidelity environmental input for subsequent trajectory planning, thereby improving obstacle avoidance success rate and system robustness.

[0036] Furthermore, the process of generating the three-dimensional composite map includes: traversing the center point of each non-obstacle voxel in the map, calculating the joint angle vector required for the end effector of the handling robot to reach the center point using an inverse kinematics solver, weighting and summing the displacement of each joint in the vector with the weight coefficients in the joint performance degradation matrix to generate an accessibility potential field value that quantifies the cost of motion execution; constructing a spherical structural element with a radius equal to the sum of the maximum cross-sectional radius of the handling robot link and a preset safety buffer distance, performing a three-dimensional morphological dilation operation on the robot's prohibited voxel set using the spherical structural element, defining the area covered by the dilation operation as the robot's physical avoidance zone; normalizing the accessibility potential field value, and linearly superimposing it with the obstacle occupancy probability value of the robot's physical avoidance zone to generate the three-dimensional composite map used to guide path search.

[0037] First, the center point of each non-obstacle voxel in the map is traversed. For each non-obstacle voxel center point, the inverse kinematics (IK) solver is called to determine the joint angle vector required for the end effector of the handling robot to reach that point. Then, the displacement of each joint in the joint vector is weighted and accumulated with the corresponding penalty weight coefficient in the joint health degradation matrix. The accumulated value is the end effector reachability potential field value of the voxel. It successfully transforms the robot's real-time health status into motion cost in the external navigation space.

[0038] The inverse kinematics (IK) solver is an algorithm or computational model used to transform the target pose (position and orientation) of the robot's end effector in a Cartesian coordinate system into the configuration of all joint angles required to achieve that pose. This method requires efficient traversal of the center point of each non-obstacle voxel in the map to generate a potential field and to convert the complete trajectory into a joint sequence in a short time. In this embodiment, an analytical inverse kinematics solver specifically designed for the handling robot is preferred. The analytical method can provide the fastest computation speed and the most stable and accurate solution, ensuring the real-time generation of the accessibility potential field and the accuracy of the data in subsequent dynamic verification. To take into account the physical volume of the robotic arm and the required safety margin during planning, a spherical structural element needs to be constructed. Its radius is defined as the sum of the maximum physical cross-sectional radius of the robotic arm and the preset safety buffer distance. Subsequently, the three-dimensional morphological expansion operation is performed on the set of prohibited voxels of the robotic arm (composed of fluid and contaminants) using this structural element. The expanded area is precisely defined as the physical avoidance zone of the robotic arm, ensuring that the planned trajectory can leave enough safety space.

[0039] After generating two independent risk fields, the system needs to normalize the terminal reachability potential field values ​​(for example, by dividing by the maximum possible cost to map the values ​​to between 0 and 1) so that they have the same dimensions as the obstacle occupancy probability values. Finally, using a linear superposition algorithm, the normalized reachability potential field and the obstacle occupancy probability values ​​of the physical avoidance zone are fused together to generate a three-dimensional composite map for guiding path search.

[0040] By quantifying and superimposing the motion execution costs caused by wear and tear on the internal joints of the robotic arm with the geometric avoidance areas of external fluids and contaminants, the system constructs a unified cost map that simultaneously reflects equipment health and environmental interference. This fusion method ensures that trajectory planning can effectively balance the shortest path and the minimum risk, thereby improving the long-term operational reliability and resource utilization efficiency of the system.

[0041] Furthermore, the initial trajectory generation process includes: initializing the path search list, establishing an open list for storing nodes to be expanded and a closed list for expanded nodes, and adding the current pose to the open list; cyclically selecting the node with the smallest comprehensive travel cost in the open list as the current parent node, traversing the neighboring voxel nodes of the parent node, and calculating the comprehensive travel cost of the neighboring voxel nodes; the comprehensive travel cost consists of three parts: the first part is the normalized value of the actual geometric distance accumulated along the path from the current pose, the second part is the normalized value of the straight-line Euclidean distance to the target pose, and the third part is the health and interference cost value read from the three-dimensional composite map; summing the three parts to obtain the comprehensive travel cost, and updating the parent node pointer of the neighboring nodes until the target pose is searched, and extracting the voxel center coordinates by backtracking according to the parent node pointer to generate the initial trajectory.

[0042] Specifically, firstly, a priority queue-based list of nodes to be expanded is established to store the voxel nodes to be expanded. Simultaneously, a list of visited nodes is established to store the visited nodes. The search begins by adding the voxel node corresponding to the current pose to the list of nodes to be expanded. During the search loop, the node with the lowest overall travel cost in the list of nodes to be expanded is continuously selected for expansion. For each neighboring voxel node, its overall travel cost is the sum of the following three dimensionless values: Part 1 (Historical Cost): The normalized value of the actual cumulative geometric distance from the initial pose to the current node; Part 2 (Heuristic Cost): Normalized value of the straight-line Euclidean distance from the current node to the target pose; Part Three (Environmental Costs): Health and Intervention Risk Values ​​Read from the 3D Composite Map; These three dimensionless numerical values ​​are then weighted and summed. Before summing, each of the three dimensionless numerical values ​​is multiplied by a weighting coefficient determined through offline simulation and system optimization to balance the proportion of path distance and environmental risk in the overall cost, thus determining the final comprehensive passage cost. Based on this cost, the parent node pointers of neighboring nodes are updated, and the node is added to the list of nodes to be expanded. The search continues until the target pose is visited. Finally, the algorithm extracts the coordinates of the center of all traversed voxels by backtracking the parent node pointers, generating a complete initial trajectory.

[0043] The process of summing the three numerical values ​​to obtain the comprehensive passage cost includes: calculating the normalized Euclidean distance from the current node to be expanded to the target pose in real time, defining the distance value as the task distance factor; using an exponential function to calculate the weights of the health and interference cost values; and using a linearly increasing relationship to calculate the weights of the actual accumulated geometric distance and the straight-line Euclidean distance, ensuring that the algorithm prioritizes the shortest path when far from the target area. By dynamically calculating the above weight coefficients in real time, the weighted summation of the comprehensive cost is completed.

[0044] First, the normalized Euclidean distance from the current node to be expanded to the target pose is calculated in real time, and this distance value is defined as the task distance factor. This factor is the basic input for weight adjustment. Based on the task distance factor, the dynamic weight coefficients of each part of the comprehensive passage cost are calculated in real time. Safety Weight (Health and Intervention Costs): This weight is calculated using an exponential function. Its functional characteristics ensure that the weight remains low when far from the target area, while it increases rapidly geometrically as the mission distance factor approaches zero, prioritizing the highest level of operational safety.

[0045] Efficiency weight (geometric and heuristic costs): This weight is calculated using a linear function. Its functional properties ensure that the efficiency weight steadily dominates when far from the target region, thus prioritizing the algorithm's selection of the shortest path.

[0046] By dynamically calculating the aforementioned weighting coefficients in real time and substituting them into the comprehensive cost function for weighted summation, the final comprehensive toll cost is calculated. This process effectively achieves an adaptive trade-off between global efficiency and local safety in path planning.

[0047] By introducing three normalized numerical values, a hybrid cost function was constructed, which solved the problem of inconsistency between path distance and abstract risk dimensions. This enabled the optimal path search algorithm to effectively balance the efficiency of the shortest path with the need to avoid environmental and health risks, greatly improving the reliability of the planning and ensuring that the generated trajectory meets the minimum risk requirements while achieving geometric optimization.

[0048] Furthermore, the process of verifying joint torque and generating alternative trajectories includes: converting the initial trajectory into a sequence of joint angles in joint space using an inverse kinematics solver, and calculating the angular velocity and angular acceleration sequences of each joint using the finite difference method; calling the recursive Newton-Euler algorithm, and combining the link mass, center of mass position, and inertia tensor parameters of the handling robot, using the joint angle, angular velocity, and angular acceleration sequences as input, to predict the dynamic torque requirement of each joint when the robot moves along the trajectory; dynamically adjusting the rated peak torque of each joint according to the performance degradation coefficient, and determining the torque required when... The torque safety boundary under pre-wear conditions is determined; the ratio of the dynamic demand torque to the torque safety boundary is calculated as the dynamic torque load rate. If the maximum load rate exceeds 100%, it is determined that the current trajectory will cause the robot arm to overload. A risky section adjacent to the robot arm's restricted voxels in the trajectory is locked, and the restricted voxels around this section are expanded a second time at a rate greater than the initial expansion radius. The alternative trajectory is re-planned and generated in the updated map. If the dynamic torque load rate of the alternative trajectory still exceeds the preset safety threshold, the path planning is determined to have failed and a motion interlock shutdown alarm is triggered. The specific process is as follows: Figure 2 As shown.

[0049] Specifically, firstly, the discrete coordinates of the initial guide trajectory are converted into a sequence of joint angles in joint space using an inverse kinematics solver. Then, the second-order differential of this angle sequence is performed using the finite difference method to obtain the angular velocity and angular acceleration sequences of each joint. These sequences, along with the manipulator's link mass, center of mass, and inertia tensor parameters, are input into an inverse kinematics solver based on the recursive Newton-Euler algorithm. This solver calculates the dynamic torque required by each joint at each point as the manipulator moves along the trajectory. Based on the performance degradation coefficient obtained from the joint health degradation matrix, the rated peak torque of each joint is dynamically adjusted: the health factor is multiplied by the rated torque to determine the torque safety boundary under the current wear state. Subsequently, the dynamic torque load rate is obtained by calculating the ratio of the predicted dynamic torque requirement to this safety boundary. If this load rate exceeds 100% at any point on the trajectory, it is determined that the current trajectory will lead to joint overload. Once an overload risk is determined, the system immediately locks the trajectory segment with the highest load rate and identifies high-risk voxels adjacent to that segment. To force subsequent search algorithms to avoid this physical conflict point, a new, larger safety penalty multiplier (e.g., 1.5 times) is calculated and applied to the risk voxels around the road segment. A second morphological dilation operation is performed, and in the updated map, the system restarts the path search to generate an alternative trajectory that avoids the newly expanded area.

[0050] The determination of the secondary expansion safety penalty ratio is based on the quantification of risk severity. The calculation process first determines the load over-limit ratio, that is, the degree to which the dynamic torque load rate exceeds the 100% safety boundary. This over-limit ratio is then input into a linear function to determine a penalty ratio greater than 1.0. This adaptive calculation method ensures that the value of the ratio is proportional to the detected overload severity: when the risk is minor (e.g., load 110%), the penalty ratio is moderately increased; while when the risk is severe (e.g., load 150%), the penalty ratio will increase significantly, thereby defining a stronger avoidance zone in space and efficiently guiding the replanning algorithm to avoid the physical conflict point.

[0051] When the dynamic load ratio of the candidate trajectory is still higher than the preset safety threshold or no feasible path can be found in the three-dimensional cost map after secondary expansion, a three-level degradation operation strategy is triggered, specifically including: While maintaining the geometry of the current planned path, global time scaling is performed on the alternative trajectory or the initial optimal spatial trajectory, reducing the overall planned speed and acceleration according to a preset degradation step size. The time-scaled motion parameter sequence is then input back into the rigid body dynamics model to recalculate the dynamic torque load rate. If the recalculated load rate is lower than a preset safety threshold, the current degradation speed parameters are locked, and servo control commands in low-speed mode are generated to drive the handling robot to complete the task. If the load rate is still higher than the preset safety threshold when the speed is reduced to the preset minimum work efficiency threshold, the current working condition is determined to be unattainable, triggering the final motion interlock shutdown alarm.

[0052] If, after re-verification, the dynamic torque load rate of the generated alternative trajectory is still higher than the preset safety threshold, or if a collision-free path cannot be found after secondary expansion, a three-level degradation operation strategy is automatically triggered. This strategy keeps the spatial geometry of the current planned path completely unchanged, only stretching the time dimension of the motion. Specifically, a speed scaling factor is introduced, and the planned speed and acceleration are gradually reduced throughout the entire process according to a preset degradation step size (e.g., 10% reduction each time). The new motion parameter sequence after time scaling is then input into the rigid body dynamics model again, and the dynamic torque load rate is recalculated. If the recalculated load rate is lower than the preset safety threshold, the current degradation speed parameters are locked, and servo control commands in low-speed mode are generated to drive the handling robot to safely complete the operation at a speed lower than the normal cycle time. If the speed has been reduced to the preset minimum work efficiency threshold (e.g., 50% of the original speed), but the load rate is still higher than the safety threshold, the current task is determined to be physically unexecutable, and a motion interlock shutdown alarm is immediately triggered, awaiting manual intervention.

[0053] By introducing a three-level degradation operation strategy based on global time scaling, a flexible fault-tolerant mechanism that "trades time for safety" is established under extreme conditions where geometric detours cannot meet dynamic constraints. This significantly improves the robustness and continuous operation capability of the system and effectively balances production line cycle time with equipment physical safety.

[0054] By inputting the planned trajectory into the rigid body dynamics model, the joint torque is predicted in advance and compared with the safety boundary adjusted by the health factor. This effectively avoids the overload failure of the robotic arm caused by program execution. When an overload risk is detected, a physically feasible alternative path that meets safety constraints can still be found under complex working conditions by performing secondary expansion and replanning of the conflict area.

[0055] Furthermore, the generation of control commands to drive the handling robot to perform operations specifically includes: using the discrete path points of the finally selected trajectory as model points, inversely calculating B-spline control points, and using cubic B-spline basis functions to fit the discrete path into a spatial motion curve with second-order geometric continuity; performing S-shaped velocity planning on the spatial motion curve, calculating the time parameters of each point on the path, and obtaining a sequence of motion parameters for the entire process including the planned position, planned velocity, and planned acceleration; traversing the sequence of motion parameters for the entire process, detecting whether the motion parameters at any moment exceed the physical limit threshold of the handling robot, and if so, intercepting the output and alarming; if the verification passes, isochronously sampling the spatial motion curve according to the control cycle of the servo driver, generating a PVT control command sequence including position command, velocity feedforward, and acceleration feedforward, and synchronously sending it to each joint servo driver through the fieldbus, and driving each joint motor to smoothly move the handling robot along the planned trajectory through current loop closed-loop control.

[0056] The final selected trajectory is the output of joint torque verification and iterative correction, and is determined based on the following conditions: If the initial trajectory passes the torque check, then that initial trajectory is selected; If the initial trajectory fails the verification but a candidate trajectory is successfully generated, then the candidate trajectory will be selected.

[0057] The discrete path points of the final selected trajectory are used as model points to inversely calculate the control points for curve fitting. Then, the cubic B-spline basis function is used for interpolation to generate a spatial motion curve whose geometric shape satisfies second-order geometric continuity (ensuring continuous acceleration). On this geometric curve, the system performs S-shaped velocity planning, calculates the time parameters of each point on the path, and obtains a sequence of motion parameters for the entire process, including the planned position, velocity, and acceleration.

[0058] Before the command is output, the entire sequence of motion parameters is traversed and a kinematic limit check is performed. This check step compares the peak values ​​of the planned angular velocity and angular acceleration with the preset physical limit thresholds of the robot arm. If any parameter exceeds the threshold, the trajectory smoothing is immediately determined to have failed, and the output of all control commands is intercepted. At the same time, a path planning failure warning signal is triggered.

[0059] If the verification passes, the system performs isochronous sampling of the spatial motion curve according to the control cycle of the servo driver, and generates a PVT control command sequence containing target position, velocity and acceleration feedforward. This sequence is synchronously sent to each joint servo driver through a high-speed fieldbus, and the current loop closed-loop control drives each joint motor to drive the handling robot to move smoothly along the planned trajectory.

[0060] By employing cubic B-spline smoothing and S-shaped velocity programming, the motion curve is guaranteed to have second-order continuity, effectively eliminating the impact and vibration during acceleration and deceleration. More importantly, the final kinematic limit check performed before command output can intercept the risk of exceeding limits caused by smoothing calculations, ensuring that the generated PVT commands are accurate and reliable, and improving the safety and stability when handling high-value items.

[0061] By combining motion prediction and health factor feedback, dynamic adaptive adjustment of visual thresholds is achieved, effectively solving visual noise interference caused by water mist, wear and vibration, etc., and ensuring accurate identification and 3D reconstruction of fluid interference areas and pollutants. At the decision-making end, the joint wear cost, environmental risk cost, and geometric distance are dimensionlessly fused to generate a composite navigation map with dual safety attributes. A dynamic weighting algorithm based on task proximity is used to guide path search, balancing global efficiency with the safety requirements of key local areas. Finally, the planned trajectory is verified by a rigid body dynamics model and equipped with a secondary expansion iterative correction mechanism to ensure the physical feasibility of the trajectory. At the same time, the combination of cubic B-spline and S-shaped velocity planning ensures smooth output of commands and reduces the risk of equipment overload and the probability of wafer cell damage.

[0062] Example 2: This embodiment introduces a path planning and early warning system for a wafer cassette cleaning equipment provided by the present invention, and the specific results are as follows: Figure 3 As shown, the robotic arm is responsible for carrying the wafer cassette for spray cleaning. Due to prolonged high-humidity operation, primary wear has appeared inside the reducer of its third joint (elbow joint). The system executes path planning and early warning according to the following complete closed-loop process: The system first collects the vibration time-series signal and driving current of the third joint. Through time-domain analysis, it calculates the root mean square value of the vibration signal (characterizing overall wear) and the kurtosis index (characterizing initial impact). Combined with the rigid body dynamics model, it calculates the dynamic load deviation between the actual current and the theoretical current. The system uses the logistic normalization function to process the above indicators and generates a performance degradation coefficient (estimated at 0.7) that reflects the actual torque output capability of the joint. Based on this coefficient, the system establishes a kinematic constraint model: at the planning layer, the reciprocal of this coefficient is set as the motion cost weight of the third joint, forcing the algorithm to tend to generate a trajectory that reduces the motion amplitude of the joint; at the control layer, the maximum allowable angular velocity and angular acceleration of the joint are both reduced to 70% of the nominal value.

[0063] In the 3D composite map, the planner generated an initial trajectory connecting the cleaning and drying stations. To verify the feasibility of this trajectory under joint wear conditions, the system used inverse kinematics and the finite difference method to analyze the angular velocity and angular acceleration sequence of the trajectory and input it into the rigid body dynamics model. The system predicted the dynamic demand torque of the third joint along the route (peak value 80 Nm) and compared it with the derating safety boundary (rated 100 Nm × 0.7 = 70 Nm). The calculation showed that the dynamic load rate reached 114%, indicating an overload risk. The system immediately locked the high-load section in the trajectory and performed a second morphological dilation on the prohibited voxels around this section (such as the area of ​​the spray water column), calculating a larger safety penalty factor for spatial expansion. In the updated map, the system re-triggered planning and generated an alternative trajectory with a larger detour and smoother movement. After re-verification, the demand torque of this trajectory was reduced to 65 Nm, meeting the safety requirements.

[0064] The system selects candidate trajectories, smooths them using cubic B-spline interpolation, and performs S-shaped velocity planning. Before generating the final command, the system performs a final motion check, triggering an early warning. If the obstacle avoidance path is too tortuous, causing the smoothed instantaneous acceleration peak to still exceed the derating limit (i.e., exceeding 70% of capability), the system determines that execution is impossible, immediately intercepts the command output, and sends a path planning failure warning signal to the host computer, indicating the need for manual inspection or reducing the cleaning cycle time. If the check passes, the system generates a PVT control command sequence containing position, velocity, and acceleration feedforwards, which is synchronously distributed via the fieldbus. Each joint servo driver utilizes current loop closed-loop control to drive the robot to its optimal performance based on its current health condition, smoothly and safely completing the wafer cassette handling operation.

[0065] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A path planning and early warning method for a wafer cell cleaning equipment, characterized in that, include: Collect the current values ​​of the robot arm joints, encoder positions, arm vibration signals, and environmental video streams within the cleaning chamber; calculate the current pose of the robot arm's end effector and analyze the target pose according to instructions; Based on vibration signals and current values, the degree of joint wear is assessed, and a constraint model is established in the joint space. Industrial vision inspection is performed on the environmental video stream to locate the spatial coordinates of the thermal deionized water fluid interference area and the fluorescent contamination area, which are then mapped onto a three-dimensional voxel map and marked as prohibited voxels of the robot arm. An end-reachability potential field generated based on the constraint model is superimposed on the three-dimensional voxel map, and entity safety boundary expansion is performed on the prohibited voxels to generate a three-dimensional composite map. The process of generating the 3D composite map includes: traversing the center point of each non-obstacle voxel in the map, calculating the joint angle vector required for the end effector of the handling robot to reach the center point using an inverse kinematics solver, weighting and summing the displacement of each joint in the joint angle vector with the weight coefficients in the joint performance degradation matrix to generate an accessibility potential field value that quantifies the cost of motion execution; constructing a spherical structural element with a radius equal to the sum of the maximum cross-sectional radius of the handling robot link and a preset safety buffer distance, performing a 3D morphological dilation operation on the robot's prohibited voxel set using the spherical structural element, defining the area covered by the dilation operation as the robot's physical avoidance zone; normalizing the accessibility potential field value and linearly superimposing it with the obstacle occupancy probability value of the robot's physical avoidance zone to generate the 3D composite map used to guide path search. Generate an initial trajectory connecting the current pose and the target pose in the 3D composite map; input the initial trajectory into the rigid body dynamics model of the handling robot to verify the joint torque, calculate the torque load rate, and if the torque load rate exceeds the preset safety threshold, perform secondary boundary expansion on the forbidden voxels and trigger replanning to generate alternative trajectories. The selected trajectory is subjected to motion school verification. If the verification is abnormal, an early warning is triggered. If the verification is normal, control commands are generated to drive the handling robot to perform the operation.

2. The path planning and early warning method for a wafer cassette cleaning equipment according to claim 1, characterized in that, The process of establishing a constraint model in the joint space includes: performing time-domain analysis on the collected vibration time-series signals to obtain the root mean square value characterizing mechanical vibration energy and the kurtosis index characterizing impact damage, while calculating the dynamic load deviation of the current feedback value relative to the ideal standard dynamic model; inputting the root mean square value, kurtosis index, and dynamic load deviation as input variables into a pre-constructed logistic regression model to calculate the performance degradation coefficient reflecting the actual torque output capability of each joint of the handling robot; constructing a digital model reflecting the physical health state of the handling robot, wherein the digital model adopts the form of a joint performance degradation matrix, and the elements in the matrix are the performance degradation coefficients; based on the joint performance degradation matrix, dynamically adjusting the motion cost weight of the corresponding joint axis in the path planning, and performing motion control parameter derating operations on the maximum allowable angular velocity and maximum allowable angular acceleration of joints with wear, to establish a constraint model that limits the actual dynamic performance of the robot.

3. The path planning and early warning method for a wafer cassette cleaning equipment according to claim 1, characterized in that, The process of performing industrial visual inspection on the environmental video stream includes: modeling the background of the cleaning chamber using a Gaussian mixture model; extracting the foreground mask by difference between the current frame of the real-time video stream and the background model; calculating the gray-level co-occurrence matrix texture feature vector of the foreground mask region, wherein the texture feature vector consists of three feature components: energy, contrast, and entropy; identifying the region with an entropy value higher than the fluid entropy threshold, energy lower than the fluid uniformity threshold, and contrast higher than the fluid contrast threshold as the two-dimensional image contour of the hot deionized water fluid interference zone; simultaneously converting the real-time video stream to the HSV color space, extracting the saturation channel S and the brightness channel V respectively, performing dual threshold segmentation and connected component analysis to extract the two-dimensional image contour of the fluorescent contamination zone; constructing a spatial projection ray from the camera optical center through each pixel in the two-dimensional image contour based on the pinhole camera imaging model and the camera intrinsic parameter matrix; using the three-dimensional Bressenham algorithm to track the penetration path of the spatial projection ray in the coordinate system of the cleaning chamber, traversing all voxel units along the path, marking the occupancy status of the voxel units as occupied, completing the three-dimensional spatial reconstruction of the fluid interference zone and the fluorescent contamination zone and defining them as prohibited voxels for the robotic arm.

4. The path planning and early warning method for a wafer cassette cleaning equipment according to claim 1, characterized in that, The initial trajectory generation process includes: initializing the path search list, establishing an open list for storing nodes to be expanded and a closed list for expanded nodes, and adding the current pose to the open list; cyclically selecting the node with the smallest comprehensive travel cost in the open list as the current parent node, traversing the neighboring voxel nodes of the parent node, and calculating the comprehensive travel cost of the neighboring voxel nodes; the comprehensive travel cost consists of three parts: the first part is the normalized value of the actual geometric distance accumulated along the path from the current pose, the second part is the normalized value of the straight-line Euclidean distance to the target pose, and the third part is the health and interference cost value read from the 3D composite map; summing the three parts to obtain the comprehensive travel cost, and updating the parent node pointer of the neighboring nodes until the target pose is searched, and extracting the voxel center coordinates by backtracking according to the parent node pointer to generate the initial trajectory.

5. The path planning and early warning method for a wafer cassette cleaning equipment according to claim 2, characterized in that, The process of verifying joint torque and generating alternative trajectories includes: converting the initial trajectory into a sequence of joint angles in joint space using an inverse kinematics solver, and calculating the angular velocity and angular acceleration sequences of each joint using the finite difference method; calling the recursive Newton-Euler algorithm, and combining the link mass, center of mass position, and inertia tensor parameters of the handling robot, using the joint angle, angular velocity, and angular acceleration sequences as input, to predict the dynamic torque requirement of each joint when the robot moves along the trajectory; dynamically adjusting the rated peak torque of each joint according to the performance degradation coefficient, and determining the torque required when... The torque safety boundary under the pre-wear state is calculated; the ratio of the dynamic demand torque to the torque safety boundary is used as the dynamic torque load rate. If the maximum load rate exceeds 100%, it is determined that the current trajectory will cause the robot arm to overload. The risk section of the trajectory adjacent to the robot arm's restricted voxels is locked, and the restricted voxels around the risk section are expanded a second time at a rate greater than the initial expansion radius. The alternative trajectory is re-planned and generated in the updated map. If the dynamic torque load rate of the alternative trajectory still exceeds the preset safety threshold, the path planning is determined to have failed and the motion interlock shutdown alarm is triggered.

6. The path planning and early warning method for a wafer cassette cleaning equipment according to claim 1, characterized in that, The generation of control commands to drive the handling robot to perform operations specifically includes: using the discrete path points of the finally selected trajectory as model points, inversely calculating B-spline control points, and using cubic B-spline basis functions to fit the discrete path into a spatial motion curve with second-order geometric continuity; performing S-shaped velocity planning on the spatial motion curve, calculating the time parameters of each point on the path, and obtaining a sequence of motion parameters for the entire process including the planned position, planned velocity, and planned acceleration; traversing the sequence of motion parameters for the entire process, detecting whether any motion parameter exceeds the physical limit threshold of the handling robot at any moment, and if so, intercepting the output and issuing an alarm; if the verification passes, isochronously sampling the spatial motion curve according to the control cycle of the servo driver, generating a PVT control command sequence including position command, velocity feedforward, and acceleration feedforward, and synchronously sending it to each joint servo driver through the fieldbus, and driving each joint motor to smoothly move the handling robot along the planned trajectory through current loop closed-loop control.

7. A path planning and early warning system for a wafer cassette cleaning equipment, used to implement the path planning and early warning method for a wafer cassette cleaning equipment as described in claim 1, characterized in that, include: The data monitoring module collects current feedback values ​​from each joint of the robotic arm, encoder position values, arm vibration signals, and environmental video streams within the cleaning chamber; it calculates the current pose of the end effector of the handling robotic arm and analyzes the target pose according to instructions. The visual inspection module quantifies the mechanical wear degree of each joint based on vibration signals and current feedback values, and establishes a constraint model in the joint space; it performs visual inspection and 3D spatial reconstruction on the environmental video stream, locates the spatial coordinates of the thermal deionized water fluid interference area and the fluorescent contamination area, maps them to the 3D voxel map and marks them as forbidden voxels; it overlays the end reachability potential field generated based on the constraint model on the 3D voxel map, performs entity safety boundary expansion on the forbidden voxels, and generates a 3D composite map; The dynamics verification and correction module generates an initial trajectory connecting the current pose and the target pose in a 3D composite map. The initial trajectory is input into the rigid body dynamics model of the handling robot to verify the joint torque and calculate the torque load rate. If the torque load rate exceeds the preset safety threshold, a second boundary expansion is performed on the forbidden voxel and a replanning is triggered to generate alternative trajectories. The motion execution control module performs trajectory smoothing and interpolation calculations on the final selected trajectory, generates control commands, and drives the handling robot to perform the operation.

Images