Multi-mode control method and system for switchable edge cleaning robot

Abstract

The invention provides a multi-mode control method and system for a switchable edge cleaning robot, and relates to the technical field of industrial control systems.The multi-mode control method comprises the steps that after a switching instruction is received, geometric features are extracted through visual scanning of a tray contour and matched with a mode library, if yes, a corresponding control mode and a nominal track are called, and if not, the switching instruction is executed; after the position offset is verified through low-speed idle running of the edge cleaning robot, formal operation is triggered in a grading mode or re-matching is prompted; the track is predicted and the deviation is dynamically corrected based on the real-time position and speed during formal operation, and if initial matching fails, a teaching mode is switched, the production effect that the control mode can be switched can be achieved, the problems that a traditional control mode is slow in switching and the direct operation risk is high are solved, and the production efficiency is improved. The comprehensive effects of mode switching, beforehand prevention and control, real-time deviation correction and continuous grading treatment are achieved.

Description

Technical Field

[0001] This invention relates to the field of industrial control system technology, and in particular to a multi-mode control method and system for a switchable edge cleaning robot. Background Technology

[0002] With the rapid development of automation and intelligent manufacturing, robots are playing an increasingly important role in industrial production. In existing technologies, robot control methods typically receive operational progress data from external equipment and dynamically adjust the robot's working state. In remote control mode, the robot generates status indication signals based on reference data to indicate whether it enters a working state or a sleep state. Specifically, when the operational progress of upstream or downstream equipment is lower than a preset progress, the robot enters a sleep state. The sleep state is further subdivided into shallow sleep and deep sleep based on the operational duration of upstream or downstream equipment. If the operational duration of upstream or downstream equipment is less than a preset duration, the robot enters a shallow sleep state; if the operational duration of upstream or downstream equipment is greater than or equal to the preset duration, it enters a deep sleep state. This robot control strategy of existing technology has been applied in multiple industrial fields, which can improve the operating efficiency and energy efficiency of production lines.

[0003] For example, the robot control method, device, electronic device and storage medium disclosed in Chinese invention patent with announcement number CN114167823B include: in remote control mode, generating corresponding status indication signals based on the operation process progress of the robot's upstream and / or downstream equipment; and controlling the robot to enter a sleep state or working state according to the state that the robot needs to enter as indicated by the status indication signals.

[0004] For edge cleaning robots with complex structures and varied tasks, the aforementioned general robot control methods have obvious limitations. As a widely used industrial robot, edge cleaning robots are used in industrial production lines to perform edge cleaning, deburring, and other operations. They are mainly used in manufacturing processes involving materials such as wood, metal, plastic, and ceramics to ensure the quality and surface smoothness of workpieces and avoid affecting subsequent processes. In modern production lines, it is often necessary to handle production tasks of multiple specifications and small batches, which places higher demands on edge cleaning robots. Traditional edge cleaning robots usually adopt a fixed working mode and cannot flexibly adjust their working methods according to pallet tasks of different specifications and sizes. Therefore, they cannot effectively adapt to the changing needs of multi-variety, small-batch production environments.

[0005] The above-mentioned technology has at least the following technical problems: To adapt to pallet tasks of different specifications and sizes, the edge cleaning robot introduces a multi-mode control concept, which intelligently switches working modes by analyzing the edge shape of the pallet in real time. However, in practical applications, the shape, size, and position of the pallet often have errors and irregularities, making it difficult for the edge cleaning robot to accurately track and adjust the path when performing edge cleaning tasks. Existing path planning and posture adjustment algorithms rely on preset workpiece models and fixed path patterns, making it difficult to adapt to changes in the pallet in real time. This limitation causes the edge cleaning robot to easily generate path errors when facing pallets of different specifications or complex shapes, which in turn affects the edge cleaning accuracy and restricts the effectiveness of the multi-mode control system. In particular, when switching working modes, the accuracy of path and posture adjustment further affects the overall performance of the system. Summary of the Invention

[0006] On the one hand, a multi-mode control method for a switchable edge cleaning robot is provided, the method comprising: Upon receiving the switching information, the edge-cleaning robot extracts geometric feature information by scanning the tray outline, matches it with the pattern library to call the corresponding edge-cleaning control mode and nominal trajectory. If the pattern matching is successful, the edge-cleaning robot will run at low speed to obtain the position offset and then make a graded judgment to trigger the control path.

[0007] If the control path is in operation, predict the subsequent trajectory points based on the current actual position and speed and compare them with the nominal trajectory to obtain the trajectory deviation. Determine the trajectory correction strategy based on the trajectory deviation. If the control path is a mode mismatch, generate a prompt message.

[0008] If the trajectory correction strategy is to compensate for failure, statistical analysis is performed, and the failure correction strategy is determined based on the mean and variance of the deviation. The failure correction strategy includes generating prompt process inspection information, generating a temporary correction mode, and continuing normal execution. If the trajectory correction strategy is to adjust the joint position, the joint position is adjusted according to the joint command.

[0009] If pattern matching fails, the teaching mode is triggered, where the trajectory is manually set and tested. After verification, it is converted to the formal mode, stored in the database, and applied to the current task.

[0010] On the other hand, a multi-mode control system for a switchable edge cleaning robot is provided. The system includes an edge cleaning control mode recognition and switching module. After receiving the switching information, the edge cleaning robot extracts geometric feature information by scanning the tray outline, matches it with the mode library to call the corresponding edge cleaning control mode and nominal trajectory. If the mode matching is successful, the edge cleaning robot will run at low speed and obtain the position offset. After graded judgment, the control path will be triggered.

[0011] The trajectory prediction and dynamic compensation module is used to predict subsequent trajectory points based on the current actual position and speed if the control path is in normal operation, and compare them with the nominal trajectory to obtain the trajectory deviation. The trajectory correction strategy is determined based on the trajectory deviation. If the control path is a mode mismatch, a prompt message is generated.

[0012] The failure diagnosis and mode reconstruction module is used to perform statistical analysis if the trajectory correction strategy is to compensate for failure. Based on the mean and variance of the deviation, the failure correction strategy is determined. The failure correction strategy includes generating prompt process inspection information, generating temporary correction mode, and continuing normal execution. If the trajectory correction strategy is to adjust the joint position, the joint position is adjusted according to the joint command.

[0013] The teaching and learning and pattern entry module is used to trigger the teaching mode if pattern matching fails. The trajectory is manually set and tested. After verification, it is converted into a formal pattern, entered into the database, and applied to the current task.

[0014] The beneficial effects of the technical solutions provided in the embodiments of the present invention include at least the following: 1. This invention provides a multi-mode control method for a switchable edge cleaning robot. It achieves rapid model changeover capability through vision-driven intelligent mode recognition and switching, avoiding the time loss of traditional manual teaching. It achieves risk pre-control through a hierarchical verification mechanism, intercepting potential deviations during the low-speed idle run stage. It achieves dynamic error correction through trajectory compensation, transforming post-event correction into pre-event prevention. It achieves intelligent decision-making after compensation failure through statistical diagnosis and adaptive correction strategies, distinguishing between systemic deviations and local deformations and triggering corresponding handling procedures. It achieves continuous accumulation of process knowledge through the closed-loop mechanism of the teaching mode, enabling the edge cleaning robot to have self-learning capabilities. While ensuring processing accuracy, it takes into account automation efficiency and flexible manufacturing requirements with multi-layer fault-tolerant design, forming a complete intelligent control closed loop.

[0015] 2. This invention extracts the geometric features of the pallet through visual scanning and intelligently compares them with a pattern library, enabling process calls after the new pallet is loaded. It achieves a production effect with switchable control modes. Through similarity threshold grading, it realizes automatic diversion of unfamiliar workpieces, solves the processing risks caused by pattern mismatch, and achieves the dual guarantee of accurate identification and safe interception.

[0016] 3. This invention achieves risk detection before formal processing through low-speed no-load verification and tolerance grading, thus achieving the decision-making effect of path optimization. By predicting subsequent trajectory points through Taylor expansion and comparing them in real time, it achieves error perception, solves the problem of slow response of hysteresis compensation, and achieves the control effect of dynamic tracking and active prevention. By converting position error into joint compensation commands through inverse kinematics, it achieves precise correction of multi-axis coordination, solves the problem of cumulative deterioration of trajectory deviation, and achieves the execution effect of real-time correction and accuracy maintenance.

[0017] 4. This invention distinguishes between systematic deviations and local deformations through statistical analysis, enabling intelligent decision-making after compensation failures. It solves the rigidity defect of traditional systems that stop immediately upon error, achieving the effect of graded handling and production continuity assurance. Through overall translation and rotation correction of local modifications, it realizes adaptive process generation in scenarios without historical data, solving the dilemma of having no contingency plan for sudden deviations, achieving the effect of flexible response and temporary emergency relief. Through the closed-loop mechanism of formal entry into the system after teaching and verification, it realizes the transformation of human experience into system capabilities, solves the problems of process knowledge loss and repetitive work, and achieves the effect of continuous evolution accumulation. Attached Figure Description

[0018] Figure 1 This is a flowchart of a multi-mode control method for a switchable edge-cleaning robot provided in an embodiment of the present invention; Figure 2 This is a flowchart of the switching control method involved in this embodiment; Figure 3 This is a flowchart of the correction strategy involved in this embodiment; Figure 4 This is a schematic diagram of a multi-mode control system for a switchable edge cleaning robot provided in an embodiment of the present invention. Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of this application clearer, the embodiments of this application will be described in further detail below with reference to the accompanying drawings.

[0020] Embodiment 1 of the present invention Figure 1 The diagram shows a flowchart of a multi-mode control method for a switchable edge-cleaning robot provided in this embodiment. The processing flow of this method includes: after receiving the switching information, scanning the tray outline to extract geometric feature information and matching it with the mode library. If the match is successful, a low-speed idle run is performed to obtain the position offset, and a graded judgment is made to trigger the control path. If the match fails, a teaching mode is triggered, and the trajectory is manually set and verified before being entered into the library. The control path predicts the trajectory points of subsequent trajectory points during formal operation to obtain the trajectory deviation and determine the trajectory correction strategy. When the control path does not match the mode, a prompt message is generated. When the trajectory correction strategy fails, the failure correction strategy is determined based on the mean and variance of the deviation, including generating prompt process inspection information, generating a temporary correction mode, or restoring normal execution. When adjusting the joint position, the joint position is adjusted according to the joint command.

[0021] like Figure 2The flowchart of the switching control method involved in this embodiment is as follows: After receiving the job switching information, the visual perception module is activated to scan the pallet outline and quickly extract its geometric features. By performing real-time matching with the built-in mode library, the current control mode is determined. If the feature matching is successful, the edge cleaning robot will perform low-speed idling to dynamically obtain the actual position offset and perform compensation. Then, the teaching mode is automatically triggered to complete the path planning and finally enter the formal edge cleaning operation process. If the feature matching fails, a prompt message is immediately generated and a correction strategy is activated. According to the actual situation, the control parameters are adjusted or the mode library is updated. After verification, the new operation mode is stored in the library to continuously enrich the system's multi-mode adaptability.

[0022] In this embodiment, a wooden pallet is used as the object of the edge cleaning robot. The wooden pallet is composed of a top plate, a bottom plate and longitudinal beams. There are burrs and overflows on the edges that need to be removed. Due to the difference in density between batches of wood materials, the cutting force fluctuates and the edge deformation after long-term use. Therefore, a switchable multi-mode control is required to adapt to the edge cleaning needs of pallets with different wear conditions.

[0023] After the new pallet is loaded and clamped by the pallet positioning platform, a mode switching message is received and sent out. The edge-cleaning robot acquires 3D point cloud data of the pallet's outer surface through a vision sensor. Statistical filtering is performed on the 3D point cloud data to remove outliers. A random sampling consensus algorithm is used to fit the equations of the top and side planes of the pallet. Specifically, based on the fact that three non-collinear points can uniquely determine the basic geometric properties of a plane, three non-collinear points are randomly selected from the point cloud data to form an initial plane hypothesis. The perpendicular distance formula from a point to a plane is used to calculate the perpendicular distance from all points to the plane. Points with a perpendicular distance less than a preset distance threshold are marked as inliers, and the number of inliers is counted. The random sampling consensus algorithm is iterated repeatedly until a preset number of iterations is reached, and the plane with the most inliers is selected. As the optimal plane, the general form coefficients of its plane equation are output. The unit normal vector is obtained by normalizing the coefficient vector in the general form plane equation. That is, after calculating the magnitude of the vector, each component is divided by the magnitude to obtain the normal vector, making the normal vector magnitude 1, thereby eliminating the influence of scaling of the plane equation coefficients on angle calculation. The directed distance is calculated by substituting the origin coordinates (0,0,0) into the general form plane equations of the top and side surfaces. The distance is positive when the origin is on the side pointed to by the plane normal vector, and negative otherwise, thus distinguishing which side of the plane the origin is on. Then, the spatial relative orientation relationship between the top and side surfaces of the tray is determined by the sign combination of d1 and d2. The random sampling consensus algorithm is applied to the point cloud of the top surface and the point cloud of the side surface of the tray to obtain the top plane equation. equations of the side plane ,in , Let be the unit normal vectors of the two planes, p represent the coordinates of any point in space, d1 be the directed distance from the origin to the top surface, and d2 be the directed distance from the origin to the side surface. The spatial intersection of the two planes is calculated as the contour feature curve. The cross product of the normal vectors of the two planes is then used to obtain the direction vector of the intersection. To ensure that V is a unit vector, solve the equations of the two planes simultaneously, and take a point p on the intersection line as the reference point. At the same time, satisfy the equations of the top plane and the side plane. Sample the contour feature point sequence at equal intervals with a preset step size along the direction of the intersection line. Smooth the contour feature point sequence and perform curvature filtering to obtain a continuous contour feature curve.

[0024] The preset vertical distance threshold in the database is an empirical value manually preset by technicians based on prior knowledge such as the acquisition accuracy of point cloud data and the processing tolerance of pallet surface. It forms a configurable parameter entry that can be called to ensure that the real pallet surface points are effectively included in the inner point set while filtering out outlier noise points, thereby achieving a balance between robustness and accuracy of plane fitting.

[0025] The step size in the database is initially hypothesized based on theoretical analysis or past experience. Then, through comparative experiments with controlled variables, the sampling results under different step sizes are tested to screen out the candidate parameters with the best performance. Finally, these experimental results are submitted to domain experts for review. Based on theoretical knowledge and practical experience, the experts comprehensively evaluate the rationality, stability and generalization ability of the step size setting, and confirm whether it is still effective under extreme conditions. Thus, the step size selected by the experiment is finally established as a scientifically reliable preset value.

[0026] The contour feature curve is downsampled with equal arc length according to a preset step size to obtain each key feature point. The curvature value at each key feature point is calculated based on the three-point method to obtain the edge curvature sequence. The mean and standard deviation of the edge curvature sequence are statistically obtained as edge curvature feature quantities. Principal component analysis is performed on the contour feature curve to solve the eigenvector of the covariance matrix to determine the direction of the first principal axis and the second principal axis. The contour points are projected onto the principal axis coordinate system, and the extreme values ​​of the projected coordinates are found to determine the rectangular boundary. The aspect ratio is obtained by dividing the long side of the rectangular boundary by the short side, and the rectangularity is obtained by dividing the area enclosed by the contour feature curve by the area of ​​the rectangular boundary. Local extreme value detection is performed based on the edge curvature sequence. Points with curvature greater than the preset corner curvature threshold are marked as corners and the number of corners is counted. The aspect ratio, rectangularity, number of corners, and edge curvature feature quantities are combined to construct a four-dimensional geometric feature vector.

[0027] The three-point method calculates the curvature of the intermediate point based on the local geometric relationship formed by three adjacent key feature points. Specifically, it takes three consecutive key feature points P1, P2, and P3, and subtracts the three-dimensional coordinates of P2 from the three-dimensional coordinates of P1 to obtain a vector. The vector is obtained by subtracting the three-dimensional coordinates of P2 from the three-dimensional coordinates of P3. By combining the triangle area formula with the dot product relationship of vectors, the curvature estimate at the midpoint P2 is obtained. , where A is the area of ​​the triangle formed by the three points, and this curvature estimate reflects the degree of local curvature of the contour feature curve at P2.

[0028] The preset logic of the corner curvature threshold in the database is essentially a balance strategy based on geometric features and image noise. First, the mathematical definition of a corner point is to be clarified as a local maximum of curvature. The preset threshold aims to distinguish between real corner points and pseudo-corner points caused by edge response or noise. Therefore, in engineering practice, multiple experimental calibrations are conducted to stabilize the number of corner points extracted from each image within the preset range. Finally, this preset logic must be theoretically verified to ensure that the lower limit of the threshold is higher than the noise response level, while the upper limit will not miss real corner points with small curvature radii, thereby achieving the robustness of the algorithm while ensuring the corner point repetition rate and positioning accuracy.

[0029] Calculate the Euclidean distance between the four-dimensional geometric feature vector and the standard feature vectors of each category in the pattern library. Traverse all categories in the pattern library to obtain the distance set, extract the minimum distance and the maximum distance, use normalization to calculate each similarity between the four-dimensional geometric feature vector and the standard feature vectors of each category in the pattern library, and compare each similarity with the preset similarity threshold.

[0030] The standard feature vectors for each category in the pattern library are obtained through feature extraction and statistical learning from a large number of labeled wooden pallet samples in the factory. First, four-dimensional geometric feature vectors that can represent the essential attributes of each category are extracted from the training samples. Then, methods such as arithmetic mean, cluster center calculation, or category prototype learning in deep learning are used to aggregate the feature vectors of these samples into a most representative vector. This vector can often minimize intra-class differences and maximize inter-class separability. Finally, after manual verification and optimization, it is solidified into the pattern library as the matching benchmark for that category.

[0031] The similarity threshold preset in the database is based on minimizing the balance between statistical distribution and the risk of false positives. First, through a large number of matching experiments with positive and negative samples, the similarity distribution between samples of the same type and between samples of different types are calculated to determine the statistically overlapping area. Then, based on the tolerance of false negatives and false positives in specific application scenarios, professionals define a critical point in the overlapping area as the initial threshold. If security is the priority, the threshold is appropriately increased to ensure matching accuracy. If recall is the priority, the threshold is appropriately relaxed to accommodate more candidate results. Finally, the threshold needs to be verified in actual business scenarios and fine-tuned by analyzing false positive cases of boundary samples until the preset similarity threshold, which is recognized by human-machine collaboration, is solidified under the premise of ensuring the optimal combination of precision and recall.

[0032] If a similarity greater than or equal to the similarity threshold indicates that the Euclidean distance between the geometric feature vector of the current pallet and the standard feature vector of a certain category in the pattern library is within the preset tolerance range, that is, the pallet's outline shape, size specifications, and edge features are highly consistent with the standard pallet of that category, then the pattern matching is considered successful. The edge clearing control mode corresponding to the category with the highest similarity is called from the pattern library, including the optimized nominal trajectory, speed parameters, and control gain information of that category. The nominal trajectory is a three-dimensional coordinate sequence of the tool center point generated based on the standard pallet outline. The speed parameters include feed rate and acceleration limits. The control gain includes the PID coefficients of the servo system position loop and speed loop. This information is sent to the motion controller of the execution layer and the sensor fusion unit of the adaptation layer via industrial Ethernet as the reference parameters for automatic edge clearing execution.

[0033] If the similarity is less than the similarity threshold, it means that the minimum Euclidean distance between the current pallet's geometric feature vector and the standard feature vectors of all categories in the pattern library still exceeds the preset tolerance range. This means that the pallet is an unknown new category, severely deformed, or non-standard customized. The pattern matching is determined to be unsuccessful. At this time, no pre-stored control patterns are available. Forcing the execution may result in tool collision or edge cleaning quality defects. During the pattern matching process, the Euclidean distance between the current pallet's geometric feature vector and the standard feature vectors of all categories in the pattern library must be calculated one by one. The one with the smallest distance is taken as the closest candidate match. If the minimum distance is still greater than the similarity threshold, it means that the difference between the current pallet and all known categories in the library exceeds the tolerance range.

[0034] The adaptation layer is deployed on an independent embedded processor. It is responsible for receiving multi-source data from the vision system, encoder, and force sensor, and synchronizing and unifying the time and coordinates. It fuses heterogeneous sensor data into robot state estimates, monitors trajectory deviation and cutting force fluctuations in real time, and converts the abstract trajectory commands issued by the decision layer into joint angle sequences and servo parameters that the execution layer can resolve. At the same time, it feeds back the execution status to the decision layer. The execution layer consists of servo drivers and permanent magnet synchronous motors. As the underlying motion control unit, it receives joint angle commands issued by the adaptation layer and achieves precise joint servoing through three-loop closed-loop control of current loop, speed loop, and position loop. It drives the robotic arm to complete nominal trajectory tracking and real-time trajectory correction, ensuring the precise following of the edge cleaning tool and pallet contour.

[0035] After completing the edge clearing control mode call, the edge clearing robot reads the nominal trajectory and the preset set of key feature points from the mode library. The edge clearing robot runs along the nominal trajectory at the preset no-run speed, pauses at each key feature point, reads the current actual position coordinates through the feedback of the servo system, compares them point by point with the nominal coordinates of the corresponding key feature points, calculates the component differences between the actual coordinates and the nominal coordinates at each key feature point in the horizontal, vertical and perpendicular directions, obtains the position offset vector, and then calculates the Euclidean modulus of each position offset vector to obtain the position offset amount, constructs the position offset amount set for subsequent hierarchical judgment.

[0036] The key feature point set is formed by performing discrete sampling of the nominal trajectory with equal arc length, calculating the curvature value at each sampling point, extracting local extrema of curvature and curvature abrupt change points as candidate points, superimposing the trajectory start and end points, and removing collinear redundant points.

[0037] A graded judgment is performed. If the maximum position offset is less than or equal to the preset tolerance threshold in the database, it indicates that the pallet clamping position is accurate and the contour deformation is within the allowable range, meeting the accuracy prerequisite for automatic edge clearing. The geometric verification is then deemed successful. At this point, the control path is set to formal operation, and the adaptation layer is started to continuously collect deviation data between the actual position and the nominal trajectory at the normal monitoring frequency, providing feedback information for real-time trajectory correction, and entering the automatic edge clearing execution stage.

[0038] The tolerance threshold preset logic in the database fits the fluctuation range of historical normal data or standard samples through statistical analysis methods. The initial benchmark of tolerance is the mean plus or minus three standard deviations. Then, professionals adjust the sensitivity requirements according to the specific business scenario. Finally, the threshold needs to be verified in the actual operating environment through edge testing to ensure that it can effectively filter abnormal disturbances without being too strict and misjudging normal fluctuations as faults.

[0039] If the maximum position offset is greater than or equal to the tolerance threshold, it indicates that there is a significant deviation in the pallet clamping position or that the contour deformation exceeds the allowable range. The geometric verification fails, and continued automatic execution may cause the edge cleaning tool to collide with the pallet or the edge cleaning quality to be unqualified. At this time, the control path is determined to be a mode mismatch, the automatic execution process is paused, and the decision-making level generates a manual intervention prompt message to avoid blind operation that may cause equipment damage or processing defects.

[0040] If the control path is in formal operation, the execution layer combines the three-dimensional coordinates of the starting trajectory point with the tool posture to form a six-dimensional pose vector, which is then substituted into the inverse kinematics model of the edge cleaning robot. The target angles of each joint are solved by numerical iteration. After the solution is completed, the execution layer sends the target angles of each joint to the servo driver. The servo driver drives the permanent magnet synchronous motor to move each joint through a three-loop closed-loop control of position loop, velocity loop and current loop. The actual joint angles fed back by the encoder are monitored in real time to complete the initial position setting. The end effector of the edge cleaning robot is positioned at the starting trajectory point of the nominal trajectory.

[0041] The establishment of the inverse kinematics model of the edge cleaning robot begins with experts performing kinematic modeling of the robot, establishing coordinate systems for each link and defining geometric parameters such as joint variables and link lengths. The end effector pose is obtained through homogeneous transformation matrix multiplication. For complex structures or redundant degrees of freedom, a numerical iteration method based on the Jacobian matrix is ​​used to map the end effector pose error into joint angle corrections to gradually approximate the target. Finally, edge cleaning process constraints are incorporated for optimization. Taking into account the perpendicularity requirements between the tool and the workpiece surface, joint limits, obstacle avoidance, and force control requirements, the optimal joint configuration with the smoothest path, lowest energy consumption, or smallest torque is selected from multiple feasible solutions, thereby achieving high-precision edge cleaning operations.

[0042] Numerical iterative methods for solving joint target angles are a stepwise approximation strategy. The core idea is to continuously correct the joint angles through iterative calculations, enabling the end effector to converge from the current pose to the target pose. The specific process is as follows: First, establish an error function based on the difference between the target pose and the current pose, calculate the Jacobian matrix under the current joint angle, and then use the inverse or pseudo-inverse of the Jacobian matrix to convert the end effector pose error into a joint angle correction. After updating the joint angles, recalculate the forward kinematics to obtain a new end effector pose, and repeat this process until the minimum error is reached.

[0043] The actual position, velocity, and acceleration of the starting trajectory point are obtained from the positioning system as the initial state. Then, a preset time window is set, and the time-based univariate function is applied to the second-order Taylor expansion formula at position t0. Where P(t0+Δt) represents the expected spatial position of the object after time Δt from the initial time t0, P(t0) represents the actual position vector at the initial time, V(t0) represents the velocity at the initial time, and a(t0) represents the instantaneous acceleration at the initial time. a(t0)Δt 2 This represents the displacement correction term caused by acceleration. The position coordinates of each discrete moment within the time window are calculated sequentially to generate a set of predicted subsequent trajectory point sequences. Finally, the predicted sequence is compared point by point with the corresponding points on the nominal trajectory at the same moment, and the position deviation of each point is calculated to obtain the position error of the subsequent trajectory point sequence.

[0044] The positioning system of the edge cleaning robot is a composite precision assurance system that integrates internal servo feedback and external global measurement to ensure a high degree of consistency in the edge cleaning effect of each pallet. Based on internal sensors such as joint encoders and grating rulers, the system calculates the pose of each link of the robotic arm in real time through forward kinematics to form high-frequency closed-loop control. During operation, the positioning system converts the preset path points into target angles in the joint space, drives the robotic arm to strictly track the trajectory through inverse kinematics model, and combines force control sensors to monitor the contact pressure between the tool and the edge of the pallet in real time to dynamically correct position deviations.

[0045] The preset length of the time window in the database is based on the distribution density of key feature points on the nominal trajectory. This ensures that the window can cover enough sampling points to eliminate random noise and extract the statistically significant average deviation. At the same time, it needs to be combined with the dynamic response time constant of the edge clearing robot so that the window can capture the local trend of deviation changes without smoothing out key features or introducing significant computational delays due to excessive length. Ultimately, the initial value is usually designed by professionals by analyzing the deviation fluctuation patterns of historical trajectory data. During experimental verification, it is dynamically adjusted according to the trade-off between diagnostic accuracy and response speed to make it a configuration parameter that can adapt to different working conditions.

[0046] If the positional error of each subsequent trajectory point sequence is less than the tolerance threshold, it means that the deviation between the actual motion trajectory and the nominal trajectory is always within the allowable range, and the current motion state of the edge clearing robot meets the accuracy requirements. Therefore, there is no need to start any trajectory correction strategy, and the original motion command can be maintained and continued to be executed. In this case, the trajectory correction strategy is recorded as no correction is required.

[0047] If the error of a subsequent trajectory point sequence is greater than or equal to the tolerance threshold, it indicates that the actual trajectory has deviated significantly from the nominal trajectory and entered an unacceptable deviation range. If intervention is not timely, it may lead to mission failure or even safety incidents. Therefore, the trajectory correction strategy is recorded as the activation of the adjustment mechanism for compensation and correction, and the subsequent trajectory point that is greater than or equal to the tolerance threshold is recorded as the trigger point.

[0048] The robot reads its current joint angles from the positioning system and combines them with the Cartesian space position error at the trigger point. Then, using the current joint angles as initial values, it solves the inverse kinematic equations through a numerical iterative algorithm to map the small position error in Cartesian space to the joint space. It calculates the small increment that each joint needs to adjust, i.e., the joint angle compensation vector. Finally, it superimposes the compensation vector with the current joint angle to generate the corrected desired joint angle command, which is then sent to the servo driver for execution. This drives the end effector to converge toward the nominal trajectory, achieving precise compensation for position deviations.

[0049] In this embodiment, the numerical iterative algorithm adopts the Newton-Raphson method, a classic numerical iterative algorithm for solving the roots of nonlinear equations. Its core idea is to gradually approximate the exact solution by using the tangent line of the function at a certain point. In solving the robot's inverse kinematics, this method calculates the forward kinematics Jacobian matrix at the current joint angle to establish a linear approximate relationship between the small increment of the joint angle and the change in the end effector position. Then, it obtains the correction amount of the joint angle by solving the linear equation system and iteratively updates the joint angle until the end effector position error converges to the allowable accuracy range, and finally obtains the joint angle solution that satisfies the desired pose.

[0050] If the magnitude of the compensation vector is within the allowable compensation range, it means that the correction amount is safe and executable. There is no need to switch to a more complex emergency strategy. The end trajectory can be returned to the nominal path by adjusting the position. Therefore, the trajectory correction strategy is recorded as adjusting the joint position. The execution layer adjusts the joint position according to the updated joint instructions.

[0051] The preset logic of the compensation range is set in layers based on the physical characteristics of the joint and the trajectory accuracy requirements: the allowable range of a single joint is determined by the product of the maximum angular velocity of each joint and the control cycle, ensuring that a single compensation does not exceed the limit or speed; the overall threshold maps the maximum allowable position error of the end effector to the joint space through inverse kinematics, and after experimental verification and expert verification, it is stored in the database to ensure the overall coordination of the end effector pose adjustment.

[0052] If the magnitude of the compensation vector exceeds the preset allowable compensation range or the cumulative compensation exceeds the safety threshold, it means that the current deviation has exceeded the limit of the normal adjustment capability of the edge clearing robot. This means that relying solely on joint-level position compensation can no longer safely and effectively pull the trajectory back to the nominal path. There may be risks such as actuator saturation, severe model mismatch, or excessive external interference. Therefore, the trajectory correction strategy is recorded as compensation failure.

[0053] like Figure 3 The flowchart of the correction strategy involved in this embodiment includes: three specific processing paths for the correction strategy type. If it is determined that the joint position adjustment compensation has failed, the joint position will be adjusted according to the joint command. If it is determined that the compensation has failed, the failure correction strategy is determined based on the mean and variance of the deviation. If the failure correction strategy is to generate a temporary correction mode, normal execution will continue. If the generation of a prompt process check is triggered, the prompt information will be directly output to remind the manual process check. If the failure correction strategy is determined to continue normal execution, no adjustment will be made.

[0054] When the trajectory correction strategy is marked as a compensation failure, the edge-clearing robot will take the failure time as the endpoint and extract a continuous historical compensation data segment, i.e., the joint compensation vector recorded within the past data segment length, as an analysis sample. Then, these original compensation data are filtered and smoothed using a low-pass filter to eliminate measurement noise or high-frequency disturbances, resulting in a smoothed three-dimensional deviation vector time series that better reflects the true trend. Finally, statistical analysis is performed on this time series to calculate the mean and variance of the deviation, thereby quantitatively evaluating the statistical characteristics of the deviation before the compensation failure. This provides data basis for subsequent diagnosis of failure causes, adjustment of control parameters, or optimization of compensation algorithms.

[0055] The preset data segment length in the database is determined by first analyzing the typical duration of historical fault data, then having professionals conduct an initial design, followed by experimental verification and optimization, and finally configuring parameters that can flexibly adapt to different pallet categories and storing them in the database, thereby achieving the optimal balance between diagnostic accuracy and response speed.

[0056] If the variance of the deviation is greater than or equal to the preset variance threshold, it means that the deviation fluctuation at the time of compensation failure has exceeded the allowable range of normal random disturbances. This means that there may be non-stationary factors such as abnormal process parameters, increased equipment vibration, or complex and changeable external disturbances during the actual movement of the edge cleaning robot. At this time, the decision-making level will record the failure correction strategy as generating prompt process inspection information to notify the operator or process personnel to check and calibrate the equipment status, processing conditions, or environmental factors, and eliminate the potential anomalies that cause compensation failure from the root.

[0057] The decision-making layer, as an intelligent decision-making unit, is typically deployed within the main control computer of the edge-clearing robot. It is responsible for receiving robot state estimates and trajectory deviation information from the adaptation layer. Its core functions include identifying the current pallet category through a built-in pattern matching algorithm, performing online trajectory planning and replanning based on the identification results to generate abstract trajectory instructions and send them to the adaptation layer, performing hierarchical judgments based on the severity of deviations to decide which control strategy to adopt, triggering failure diagnosis in abnormal scenarios such as compensation failure, locating the root cause of deviations through statistical analysis of historical deviation data, and finally outputting corresponding prompts or correction modes to the adaptation layer, thereby achieving intelligent decision-making and task coordination for the robot system.

[0058] If the variance of the deviation is less than the preset variance threshold and the absolute value of the mean deviation is greater than the preset mean threshold, it indicates that although the fluctuation is within the normal range, there is a continuous and significant directional deviation. This means that the current deviation is not caused by random disturbances, but by systematic factors. At this time, the decision-making level will record the failure correction strategy as generating a temporary correction mode, that is, it is necessary to start parameter self-tuning or introduce a constant value compensation mechanism to eliminate systematic cumulative errors and make the trajectory return to the nominal path.

[0059] If the variance of the deviation is less than the preset variance threshold and the absolute value of the mean deviation is less than or equal to the preset mean threshold, it means that the fluctuation range and directional deviation of the deviation during the compensation failure are within the normal allowable range. This means that the compensation failure was not caused by a systematic abnormal disturbance or model mismatch, but may have originated from occasional instantaneous interference or measurement noise. In this case, the decision-making level will record the failure correction strategy as generating a prompt message and continuing to execute normally. That is, only the prompt message is generated to record the event log, without interrupting the current work process, maintaining the original control instructions to continue to be executed, avoiding unnecessary shutdown checks or redundant corrections, and improving the system's operating efficiency and robustness while ensuring processing quality.

[0060] The average deviation vector at each key feature point on the nominal trajectory is extracted from the smoothed three-dimensional deviation vector time series, and a deviation vector set is constructed.

[0061] Based on the timestamps or location indices corresponding to each key feature point on the nominal trajectory, the time window near each feature point is located in the smoothed deviation time series. Then, the arithmetic mean of all deviation vectors within each window is calculated to obtain the representative average deviation vector at that feature point, in order to eliminate the influence of local random fluctuations. Finally, the average deviation vectors corresponding to all key feature points are summarized in the order of the trajectory to form a set of deviation vectors that can be used for subsequent pattern matching or process analysis.

[0062] The average deviation vectors corresponding to two adjacent feature points are extracted from the deviation vector set in the order of the trajectory. The magnitude of the latter vector is subtracted from the magnitude of the former vector and divided by the arc length of the trajectory between the two feature points to obtain the magnitude increase or decrease per unit distance, which is the rate of change of the magnitude of the average deviation vector at adjacent key feature points. Then, the cosine value of the angle between the two vectors is obtained by solving the dot product of the two vectors. The cosine value is then converted into a spatial angle between 0 and π radians by the inverse cosine function to obtain the spatial angle. Finally, the angle value is divided by the arc length of the trajectory to obtain the directional deflection angle per unit distance, which is the rate of change of the direction of the average deviation vector at adjacent key feature points.

[0063] If the rate of change in all directions is less than the preset threshold for directional change and the rate of change in all amplitudes is less than the preset threshold for amplitude change, it indicates that the current motion state of the edge-clearing robot is stable. The deviation fluctuation is within the allowable range, and no abnormal directional deflection or amplitude oscillation occurs. Therefore, the systematic deviation is determined to be a consistent change. The rotation matrix between the nominal coordinates and the corresponding actual coordinates of each point is analyzed. The rotation matrix is ​​obtained by analyzing the attitude differences between the nominal and actual coordinates of each feature point. Specifically, a local coordinate system is constructed using the nominal trajectory point and its neighboring points with tangential, normal, and binormal vectors as basis vectors. Similarly, a corresponding coordinate system is constructed using the actual measured coordinates. The actual local coordinate system, the transformation relationship between the basis vectors of the two coordinate systems constitutes a 3×3 rotation matrix. If the equivalent rotation angle of the rotation matrix is ​​less than the preset rotation threshold in the database, it means that the actual trajectory of the pallet is basically consistent with the nominal trajectory, there is only a translational offset in spatial position, and no obvious rotational deviation has occurred. Therefore, it is judged as an overall translation. The mean of all average deviation vectors is used as the translation vector, and the entire nominal trajectory is superimposed with this vector. If the equivalent rotation angle is greater than or equal to the preset rotation threshold, it means that the pallet has undergone a significant rotational offset relative to the theoretical coordinate system. Simple translation compensation cannot eliminate the deviation. Therefore, it is judged as an overall rotation.

[0064] The preset logic for the rotation threshold in the database is achieved through a large number of positive and negative sample matching experiments. The initial threshold is set by professionals, verified in actual business scenarios, and finally approved by experts before being stored in the database.

[0065] If the rate of change of direction is greater than or equal to the preset rate of change of direction or the rate of change of amplitude is greater than or equal to the preset rate of change of amplitude, it indicates that the robot has experienced abnormal motion fluctuations in the current operating state, indicating that the current motion state has deviated from the normal operation mode. The systematic deviation is then determined to be a local deformation. Based on the distribution characteristics of the deviation along the path, the nominal trajectory is divided into segments. Based on the deviation vectors at the endpoints of each segment, cubic spline interpolation is used to generate continuous local offset curves. The nominal trajectory is then corrected in segments by point-by-point superposition. The adjusted complete trajectory is then sent to the execution layer as a temporary correction mode.

[0066] If the rotation is determined to be an overall rotation, the nominal coordinates of each key feature point are obtained, and the corresponding actual coordinates are extracted from the smoothed three-dimensional deviation vector time series.

[0067] Once the existence of overall rotation is determined, the nominal coordinates of each key feature point are obtained from the nominal trajectory data. Then, based on the same timestamp, the deviation vector at each feature point is located in the smoothed three-dimensional deviation vector time series. The nominal coordinates and the corresponding deviation vectors are first mapped to the same spatial dimension through a homogeneous transformation matrix, and then added point by point after coordinate system unification. This allows the calculation of the actual coordinates at each feature point, providing a data foundation for subsequent analysis of the overall rotation parameters.

[0068] In this embodiment, the edge-clearing robot operates in three-dimensional space. Both nominal and actual coordinates are three-dimensional vectors. The number of key feature points is denoted as m. The centroid-free coordinate matrix is ​​(m×3), and the transposed actual coordinate matrix is ​​3×m. When multiplying, the intermediate dimension m cancels out, resulting in a 3×3 covariance matrix. The nominal coordinates of the m key feature points are summed over their x, y, and z components and then divided by m to obtain the nominal centroid coordinates. Similarly, the actual coordinates of the m key feature points are calculated in the same way to obtain the actual centroid coordinates. This process is used to calculate the centroid coordinates of all key feature points, combining the nominal and actual coordinates. Then, the corresponding centroid coordinates are subtracted from the nominal and actual coordinates of each key feature point to obtain the nominal decentroid coordinates and actual decentroid coordinates of each key feature point. Next, a 3×3 covariance matrix is ​​constructed by multiplying the centroid-decentroid nominal coordinate matrix by the transpose of the actual coordinate matrix. This matrix characterizes the cross-correlation between the two point sets along each coordinate axis. Singular value decomposition is performed on this covariance matrix to obtain a left singular matrix and a right singular matrix. The rotation matrix is ​​obtained by multiplying the right singular matrix by the transpose of the left singular matrix. Finally, the translation vector is calculated based on the difference between the actual centroid and the rotated nominal centroid. , where w represents the translation vector, q represents the centroid coordinates of the actual coordinates, R represents the rotation matrix, and q0 represents the centroid coordinates of the nominal coordinates.

[0069] For each point on the nominal trajectory, first multiply its coordinates by the rotation matrix to obtain the rotated point, and then superimpose the translation vector to generate the new coordinates of the point after rotation correction; by traversing all points on the nominal trajectory, the complete new trajectory after rotation correction can be obtained.

[0070] After rotation correction is completed, the generated new trajectory coordinate sequence is first encapsulated into joint angle commands that can be parsed by the execution layer, and marked as temporary correction mode and sent to the execution layer, so that it replaces the original nominal trajectory as the benchmark for subsequent tracking; at the same time, the new trajectory is synchronously fed back to the adaptation layer to update the compensation benchmark data stored in its internal storage, ensuring that the adaptation layer can perform error evaluation and feedback based on the latest expected trajectory in subsequent real-time monitoring and deviation calculation, thereby maintaining the consistency of the system correction strategy.

[0071] After the teaching mode is started, the decision-making level receives the sequence of trajectory points and corresponding process parameters manually guided by the human-computer interaction interface in real time. These discrete points are then used to generate a continuous temporary trajectory model according to the preset interpolation rules. The model is then packaged into a teaching temporary mode data package and automatically marked as a state to be verified. It is stored in the temporary mode library and awaits subsequent safety verification through simulation or no-load operation.

[0072] The decision-making layer issues instructions to the execution layer, requiring it to conduct trial operation according to the temporary teaching mode marked as pending verification. At the same time, the adaptation layer starts high-density monitoring according to the preset monitoring density enhancement value, and collects and analyzes trajectory tracking deviation and process parameter fluctuations in real time. If, after stable verification for several consecutive operating cycles, all monitoring data do not trigger compensation failure and the deviation is within the allowable range, the decision-making layer changes the temporary mode from pending verification to formal mode and stores it in the mode library for subsequent production use. If compensation failure occurs during the trial operation, the decision-making layer immediately suspends the trial operation and sends a prompt to the human-machine interface, notifying manual intervention to correct the trajectory. After correction, the verification process is restarted.

[0073] The preset logic for the enhanced monitoring density value is optimized based on the dynamic response characteristics and resource constraints of the edge clearing robot, under the premise of ensuring sufficient verification and safety redundancy. Specifically, by analyzing the fluctuation frequency and amplitude distribution of deviations in historical failure data, the key periods that require enhanced monitoring are determined. Then, combined with the upper limit of the controller's sampling and real-time requirements, the enhanced sampling rate value is set to a reasonable multiple of the normal density to ensure that potential anomalies can be sensitively captured. Finally, the value is verified and optimized through simulation and field experiments, and after being determined by experts, it is fixed in the database as a configurable parameter.

[0074] After the formal mode is entered into the database, the decision-making layer issues an instruction to the execution layer, requiring it to re-execute the current pallet edge clearing task according to the formal mode and enter the normal trajectory tracking and deviation compensation process. At the same time, the adaptation layer restores the monitoring density from the enhanced value to the normal density and continues to perform real-time data collection and deviation calculation to ensure that the system operates efficiently in a stable state.

[0075] like Figure 4 The present invention provides a schematic diagram of a multi-mode control system for a switchable edge cleaning robot, comprising: an edge cleaning control mode recognition and switching module, a trajectory prediction and dynamic compensation module, a failure diagnosis and mode reconstruction module, and a teaching and learning and mode storage module.

[0076] Among them, the edge cleaning control mode recognition and switching module is used to receive switching information. The edge cleaning robot extracts geometric feature information by scanning the tray outline, matches it with the mode library to call the corresponding edge cleaning control mode and nominal trajectory. If the mode matching is successful, the edge cleaning robot will run at low speed and obtain the position offset. After classification, it will trigger the control path.

[0077] The trajectory prediction and dynamic compensation module is used to predict subsequent trajectory points based on the current actual position and speed if the control path is in normal operation, and compare them with the nominal trajectory to obtain the trajectory deviation. The trajectory correction strategy is determined based on the trajectory deviation. If the control path is a mode mismatch, a prompt message is generated.

[0078] The failure diagnosis and mode reconstruction module is used to perform statistical analysis if the trajectory correction strategy is to compensate for failure. Based on the mean and variance of the deviation, the failure correction strategy is determined. The failure correction strategy includes generating prompt process inspection information, generating temporary correction mode, and continuing normal execution. If the trajectory correction strategy is to adjust the joint position, the joint position is adjusted according to the joint command.

[0079] The teaching and learning and pattern entry module is used to trigger the teaching mode if pattern matching fails. The trajectory is manually set and tested. After verification, it is converted into a formal pattern, entered into the database, and applied to the current task.

[0080] In Embodiment 2 of the present invention, while remaining otherwise unchanged from Embodiment 1, the triggering of the control path in Embodiment 1 further includes: triggering the control path, and the specific analysis method is as follows: After completing the edge cleaning control mode call, the edge cleaning robot does not perform low-speed idling. Instead, it directly uses the visual sensor to acquire and process the three-dimensional point cloud data of the outer surface of the pallet during the loading and clamping process. The point cloud data has been extracted by edge detection and plane segmentation to extract the intersection line of the top surface and the side surface of the pallet as the contour feature curve.

[0081] Based on the contour feature curves acquired by the vision sensor, plane fitting is performed on the point cloud data corresponding to the top surface of the pallet to obtain the top surface plane equation. At the same time, plane fitting is performed on the point cloud data corresponding to the two adjacent sides to obtain two side plane equations. Then, the intersection line between the top surface plane and the first side is calculated to obtain the first spatial line equation, and the intersection line between the top surface plane and the second side is calculated to obtain the second spatial line equation. Finally, the intersection point of these two spatial lines is solved. This intersection point is the actual three-dimensional coordinate of a corner point of the pallet in the robot base coordinate system. By processing the combination of each adjacent side and the top surface of the pallet in the same way, the actual coordinates of all corner points of the pallet can be obtained.

[0082] Based on these corner coordinates, an actual pose description of the pallet is established, including the actual position coordinates of the pallet's center point and the direction vector of the pallet's top surface normal.

[0083] The actual pose of the pallet is compared with the nominal trajectory of the pallet of the same category preset in the pattern library. The pose deviation between the two is calculated, including the translational deviation vector and the rotational deviation. Based on the obtained pose deviation, the coordinate transformation is performed on the set of key feature points on the nominal trajectory. The nominal coordinates of each key feature point are sequentially subjected to rotation and translation transformations to obtain the expected actual coordinates corresponding to the actual pose of the pallet. The expected actual coordinates are compared with the nominal coordinates point by point, and the component differences of each point in the horizontal, vertical and perpendicular directions are calculated to obtain the position offset vectors. Then, the Euclidean magnitude of each offset vector is calculated as the position offset, and finally, a set of position offsets is constructed for subsequent hierarchical judgment.

[0084] In Embodiment 3 of the present invention, based on the unchanged aspects of Embodiments 1 and 2, the overall rotation of Embodiment 1 further includes: a trajectory correction method based on quaternions, the specific analysis method of which is as follows: The nominal coordinates of all feature points are extracted from the set of key feature points of the nominal trajectory. At the same time, the actual coordinates corresponding to each key feature point are extracted from the smoothed three-dimensional deviation vector time series. The centroid coordinates of the nominal coordinate point set and the actual coordinate point set are calculated respectively. The corresponding centroid coordinates are subtracted from each point in the two sets of points to obtain the centroid-free coordinate point set, thereby eliminating the influence of translation components on rotation solution.

[0085] A covariance matrix is ​​constructed based on the centroid-free coordinate point set. This matrix describes the correlation between two sets of points. A 3×3 covariance matrix is ​​obtained from the centroid-free coordinate point set. Then, a 4×4 symmetric matrix is ​​constructed based on this matrix: the trace of the covariance matrix is ​​used as the element of the first row and first column of the four-dimensional matrix. The differences in the coordinate directions of each pair of coordinates in the covariance matrix are sequentially filled into the remaining positions in the first row and the corresponding positions in the first column. The 3×3 sub-block in the lower right corner is formed by a linear combination of the covariance matrix and its transpose. The diagonal elements are taken as twice the corresponding diagonal elements of the covariance matrix minus the trace, and the off-diagonal elements are the sum of the corresponding elements in the covariance matrix and its transpose. This matrix contains information such as the trace, symmetric components, and antisymmetric components of the covariance matrix. The constructed four-dimensional symmetric matrix is ​​then characterized. Value decomposition is used to solve for all four eigenvalues ​​of the matrix, and the eigenvalue with the largest value is found. Then, the eigenvector corresponding to the largest eigenvalue is solved. This eigenvector is a four-dimensional vector. According to mathematical principles, this eigenvector is the unit quaternion representing the optimal rotation transformation. The obtained four-dimensional eigenvector is regarded as a quaternion, which contains one real part and three imaginary parts. According to the transformation relationship between quaternions and rotation matrices, the components of the quaternion are combined to form a 3x3 rotation matrix. The specific transformation rule is that the diagonal elements of the rotation matrix are formed by combining the squares of the components of the quaternion, and the off-diagonal elements are formed by combining the products between different components of the quaternion, with the signs adjusted accordingly. After this transformation, the rotation matrix describing the rotation transformation from the nominal point set to the actual point set is obtained.

[0086] Using the centroid coordinates of the nominal and actual coordinate point sets, and the obtained rotation matrix, the translation vector is calculated. Specifically, the translation vector equals the centroid coordinates of the actual coordinate point set minus the result of applying the rotation matrix to the centroid coordinates of the nominal coordinate point set. Thus, the rotation matrix and translation vector required to transform the nominal trajectory to the position of the actual trajectory are obtained.

[0087] The obtained rotation matrix and translation vector are applied sequentially to each point on the nominal trajectory. That is, the nominal trajectory points are first rotated, and then the translation vector is superimposed to generate a new trajectory after rotation correction. After correction, this new trajectory is sent to the execution layer as a temporary correction mode. The execution layer performs motion control according to the new trajectory, and at the same time, the compensation benchmark of the adaptation layer is updated to ensure that subsequent state monitoring and deviation analysis are based on the corrected trajectory.

[0088] The above description is only an optional embodiment of this application and is not intended to limit this application. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A multi-mode control method for a switchable edge-cleaning robot, characterized in that, Includes the following steps: Upon receiving the switching information, the edge cleaning robot extracts geometric feature information by scanning the tray outline, matches it with the pattern library to call the corresponding edge cleaning control mode and nominal trajectory. If the pattern matching is successful, the edge cleaning robot will run at low speed and obtain the position offset. After classification, it will trigger the control path. If the control path is in operation, predict the subsequent trajectory points based on the current actual position and speed and compare them with the nominal trajectory to obtain the trajectory deviation. Determine the trajectory correction strategy based on the trajectory deviation. If the control path is a mode mismatch, generate a prompt message. If the trajectory correction strategy is to compensate for failure, statistical analysis is performed, and the failure correction strategy is determined based on the mean deviation and the variance of the deviation. The failure correction strategy includes generating prompt process inspection information, generating a temporary correction mode, and continuing normal execution. If the trajectory correction strategy is to adjust the joint position, the joint position is adjusted according to the joint command. If pattern matching fails, the teaching mode is triggered, where the trajectory is manually set and tested. After verification, it is converted to the formal mode, stored in the database, and applied to the current task.

2. The multi-mode control method for a switchable edge-cleaning robot as described in claim 1, characterized in that, Upon receiving the switching information, the edge-cleaning robot extracts geometric feature information by scanning the tray outline, matches it with the pattern library to invoke the corresponding edge-cleaning control mode and nominal trajectory. The specific analysis method is as follows: After the pallet is loaded and clamped, the mode switching information is received and sent out. The edge cleaning robot obtains the three-dimensional point cloud data of the outer surface of the pallet through the vision sensor, performs edge detection and plane segmentation on the three-dimensional point cloud data, and extracts the intersection line of the top surface and the side surface of the pallet as the contour feature curve. The contour feature curve is downsampled and its curvature is calculated to extract geometric feature information, including aspect ratio, rectangularity, number of corner points and edge curvature, and a four-dimensional geometric feature vector is constructed. Calculate the Euclidean distance between the four-dimensional geometric feature vector and the preset standard feature vectors of each category in the pattern library, and normalize the Euclidean distance to obtain the similarity between the four-dimensional geometric feature vector and the preset standard feature vectors of each category in the pattern library. If the similarity is greater than or equal to the similarity threshold, the pattern matching is considered successful. The edge clearing control mode corresponding to the category with the highest similarity is called from the pattern library, including the nominal trajectory, speed parameters and control gain information, and is sent to the execution layer and the adaptation layer. If the similarity is less than the similarity threshold, the pattern matching is considered to have failed. The adaptation layer is responsible for sensor data fusion, status monitoring, and command format conversion, while the execution layer, as the underlying motion control unit, is responsible for joint servo control and trajectory execution.

3. The multi-mode control method for a switchable edge-cleaning robot as described in claim 1, characterized in that, The process involves the edge-clearing robot performing a low-speed idle run to obtain its position offset, followed by a tiered assessment to trigger a control path. The specific analysis method is as follows: After completing the edge clearing control mode call, the edge clearing robot runs at low speed in the empty space according to the set of key feature points of the nominal trajectory, collects the actual coordinates of each key feature point and compares them with the nominal coordinates to obtain the offset of each position. A hierarchical judgment is performed. If the maximum position offset is less than or equal to the preset tolerance threshold in the database, the control path is determined to be in formal operation, and the adaptation layer is started for continuous monitoring. If the maximum position offset is greater than or equal to the tolerance threshold, the control path will be determined as a mode mismatch, and a prompt message will be generated.

4. The multi-mode control method for a switchable edge-cleaning robot as described in claim 1, characterized in that, If the control path is in formal operation, the subsequent trajectory points are predicted based on the current actual position and speed and compared with the nominal trajectory to obtain the trajectory deviation. The trajectory correction strategy is determined based on the trajectory deviation. The specific analysis method is as follows: If the control path is in the formal operation, the execution layer sets the initial position based on the starting trajectory point in the nominal trajectory; By reading the actual position, velocity and acceleration of the starting trajectory point, the position coordinates within a preset time window are calculated by second-order Taylor expansion to form a subsequent trajectory point sequence. The position error of the subsequent trajectory point sequence is obtained based on the subsequent trajectory point sequence and the nominal trajectory. If the position error of each subsequent trajectory point sequence is less than the tolerance threshold, then the trajectory correction strategy is recorded as no correction is needed. If the error of a subsequent trajectory point sequence is greater than or equal to the tolerance threshold, the trajectory correction strategy is marked as activating the adjustment mechanism for compensation and correction, and the subsequent trajectory point that is greater than or equal to the tolerance threshold is marked as the trigger point.

5. The multi-mode control method for a switchable edge-cleaning robot as described in claim 4, characterized in that, The aforementioned adjustment mechanism performs compensation and correction; the specific analysis method is as follows: Based on the positional error of the trigger point, it is transformed into a compensation vector of joint angle through inverse kinematics, thereby determining the joint commands of the execution layer; If the magnitude of the compensation vector is within the preset allowable compensation range, the trajectory correction strategy is recorded as adjusting the joint position, and the execution layer adjusts the joint position according to the updated joint command. If the magnitude of the compensation vector exceeds the preset allowable compensation range or the cumulative compensation exceeds the safety threshold, the trajectory correction strategy will be recorded as a compensation failure.

6. The multi-mode control method for a switchable edge-cleaning robot as described in claim 1, characterized in that, If the trajectory correction strategy fails to compensate for the failure, statistical analysis is performed to determine the failure correction strategy based on the mean deviation and the variance of the deviation. The specific analysis method is as follows: The trajectory correction strategy is recorded as compensation failure. The time of compensation failure is taken as the end point. A continuous historical compensation amount data is extracted according to the preset data segment length and recorded as historical compensation amount. The historical compensation amount data is filtered and smoothed to obtain the smoothed three-dimensional deviation vector time series. The three-dimensional deviation vector time series is statistically analyzed to obtain the deviation mean and deviation variance. If the variance of the deviation is greater than or equal to the preset variance threshold, the decision-making layer will record the failure correction strategy as generating prompt process inspection information; The decision-making layer, as an intelligent decision-making unit, is responsible for pattern matching, trajectory planning, hierarchical judgment, and failure diagnosis. If the variance of the deviation is less than the preset variance threshold and the absolute value of the mean deviation is greater than the preset mean threshold, it is determined to be a systematic deviation, and the decision-making layer records the failure correction strategy as generating a temporary correction mode. If the variance of the deviation is less than the preset variance threshold and the absolute value of the mean deviation is less than or equal to the preset mean threshold, the decision layer will record the failure correction strategy as generating a prompt message and continue to execute normally.

7. The multi-mode control method for a switchable edge-cleaning robot as described in claim 6, characterized in that, The specific analysis method for generating the temporary correction mode is as follows: The average deviation vector at each key feature point on the nominal trajectory is extracted from the smoothed three-dimensional deviation vector time series, and a set of deviation vectors is constructed. Analyze the rate of change of direction and the rate of change of magnitude of the average deviation vector at adjacent key feature points; If the rate of change in all directions is less than the preset rate of change in all directions and the rate of change in all magnitudes is less than the preset rate of change in magnitude, then the systematic deviation is determined to be a consistent change. Analyze the rotation matrix between the nominal coordinates and the corresponding actual coordinates of each key feature point. If the equivalent rotation angle of the rotation matrix is ​​less than the preset rotation threshold in the database, then it is determined to be an overall translation. Use the mean of all average deviation vectors as the translation vector and superimpose this vector on the entire nominal trajectory. If the equivalent rotation angle is greater than or equal to the preset rotation threshold, then it is determined to be an overall rotation. If the rate of change of direction is greater than or equal to the preset rate of change of direction or the rate of change of amplitude is greater than or equal to the preset rate of change of amplitude, the systematic deviation is determined to be a local deformation. Based on the distribution characteristics of the deviation along the path, the nominal trajectory is divided into segments. Based on the deviation vector at the endpoints of each segment, cubic spline interpolation is used to generate continuous local offset curves. The nominal trajectory is then corrected in segments by point-by-point superposition. The adjusted complete trajectory is sent to the execution layer as a temporary correction mode.

8. The multi-mode control method for a switchable edge-cleaning robot as described in claim 7, characterized in that, The specific analysis method for the overall rotation is as follows: If it is determined to be an overall rotation, obtain the nominal coordinates of each key feature point, and extract the corresponding actual coordinates from the smoothed three-dimensional deviation vector time series; Calculate the centroid coordinates of the nominal coordinates and the actual coordinates respectively, and subtract the corresponding centroid coordinates from the nominal coordinates and the actual coordinates of each key feature point to obtain the nominal decentroid coordinates and the actual decentroid coordinates of each key feature point. Construct a covariance matrix, perform singular value decomposition on the covariance matrix to obtain the left singular matrix and the right singular matrix, and calculate the rotation matrix by multiplying the right singular matrix and the transpose of the left singular matrix. Calculate the translation vector based on the difference between the centroid of the actual coordinates and the centroid of the nominal coordinates after rotation. The obtained rotation matrix and translation vector are applied to each point on the nominal trajectory. That is, the nominal trajectory points are rotated first, and then the translation vector is superimposed to generate a new trajectory after rotation correction. After the adjustment is completed, the new trajectory after rotation correction is sent to the execution layer as a temporary correction mode, and the compensation benchmark of the adaptation layer is updated simultaneously.

9. The multi-mode control method for a switchable edge-cleaning robot as described in claim 1, characterized in that, If pattern matching fails, a teaching mode is triggered, where the trajectory is manually set and tested. After verification, it is switched to the formal mode, stored in the database, and applied to the current task. The specific analysis method is as follows: After the teaching mode is activated, the decision-making level receives the manually set trajectory points and process parameters, generates a temporary teaching mode, and marks it as pending verification. The decision-making layer and the instruction execution layer run the trial in the teaching temporary mode. The adaptation layer strengthens the monitoring density with the preset enhancement value. After continuous verification and stability, the decision-making layer converts the temporary mode into the formal mode and stores it in the database. If the compensation fails during the verification, the decision-making layer prompts manual correction. After the data is stored in the warehouse, the execution layer re-executes the edge clearing task of the current pallet according to the formal mode and enters the normal execution and compensation process, while the adaptation layer restores the normal monitoring density.

10. A multi-mode control system for a switchable edge-cleaning robot, characterized in that, include: The module includes edge clearing control mode recognition and switching, trajectory prediction and dynamic compensation, failure diagnosis and mode reconstruction, and teaching and learning and mode storage module. The edge clearing control mode identification and switching module is used to receive switching information, and the edge clearing robot extracts geometric feature information by scanning the tray outline, matches it with the mode library to call the corresponding edge clearing control mode and nominal trajectory. If the mode matching is successful, the edge clearing robot will run at low speed, obtain the position offset, make a graded judgment, and trigger the control path. The trajectory prediction and dynamic compensation module is used to predict subsequent trajectory points based on the current actual position and speed and compare them with the nominal trajectory if the control path is in formal operation, and determine the trajectory correction strategy based on the trajectory deviation. If the control path is a mode mismatch, a prompt message is generated. The failure diagnosis and mode reconstruction module is used to perform statistical analysis if the trajectory correction strategy is to compensate for failure, and determine the failure correction strategy based on the mean deviation and variance of the deviation. The failure correction strategy includes generating prompt process inspection information, generating a temporary correction mode, and continuing normal execution. If the trajectory correction strategy is to adjust the joint position, the joint position is adjusted according to the joint command. The teaching and learning and pattern entry module is used to trigger the teaching mode if pattern matching fails. The trajectory is manually set and tested. After verification, it is converted into a formal mode, entered into the database, and applied to the current task.

Images