Computer vision-based trajectory planning and control system for polishing robots

By combining computer vision and sliding mode control, the machine automatically identifies the grindable and obstacle areas on the surface of the workpiece and generates obstacle avoidance trajectories. This solves the problems of accuracy and robustness in trajectory planning in existing technologies and enables high-precision grinding operations.

CN123067528APending Publication Date: 2026-11-10SHANDONG DEVELOPMENT CICC BIG DATA TECHNOLOGY CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202611335214.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2026-08-31
Publication Date
2026-11-10

AI Technical Summary

Technical Problem

Existing trajectory planning methods for grinding robots rely on manual teaching or offline programming, which makes it difficult to accurately identify the grindable and obstacle-avoiding areas on the workpiece surface. Furthermore, the trajectory tracking accuracy is insufficient under external disturbances, posing a risk of collision.

Method used

A computer vision-based trajectory planning and control system is adopted. A three-dimensional surface model is constructed through a binocular vision sensor, a convolutional neural network is used to identify the grinding area and obstacle area, an obstacle avoidance trajectory is generated by combining an ant colony optimization algorithm, and the end effector is tracked in real time through a sliding mode control module.

Benefits of technology

It achieves automatic identification and high-precision trajectory planning of the grinding area and obstacle area, improves the robot's autonomous operation capability and trajectory tracking accuracy, and reduces the impact of human intervention and external disturbances.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN123067528A_ABST
    Figure CN123067528A_ABST
Patent Text Reader

Abstract

This invention discloses a computer vision-based trajectory planning and control system for a grinding robot, belonging to the field of computer vision and robot control technology. The system includes: a visual modeling module, which collects point cloud data of the workpiece surface using binocular vision sensors and constructs a three-dimensional surface model; a feature extraction module, which extracts features from the three-dimensional surface model using a pre-defined convolutional neural network to obtain features of the grinding area and obstacle area; a trajectory generation module, which divides the grindable area and obstacle avoidance area based on the features of the grinding area and obstacle area and generates an initial grinding trajectory; a pose comparison module, which acquires the pose data of the end effector in real time and compares it with the initial grinding trajectory to calculate the pose deviation; and a sliding mode control module, which adjusts the motion parameters based on the pose deviation using a sliding mode variable structure control algorithm to control the grinding robot to move along the initial grinding trajectory and perform grinding operations on the workpiece.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention relates to the fields of computer vision and robot control technology, specifically to a trajectory planning and control system for a grinding robot based on computer vision. Background Technology

[0002] When a grinding robot processes the surface of a workpiece, it needs to acquire the workpiece's shape information to plan the motion path of the end effector. Existing trajectory planning methods rely on manual teaching or offline programming. Operators record grinding path points point by point using a teach pendant, and then generate the robot's motion program. Offline programming uses computer-aided design software to import the theoretical 3D model of the workpiece and generate the grinding trajectory in a virtual environment. These methods have significant drawbacks: manual teaching is time-consuming and labor-intensive; when the workpiece has a complex shape or features such as protrusions or holes, operators struggle to accurately mark the entire grinding area and the obstacle areas to be avoided, resulting in insufficient path accuracy; offline programming, based on a theoretical model, cannot reflect the spatial posture deviations and surface deformations of the workpiece after actual clamping, leading to positional errors between the planned trajectory and the actual workpiece. This makes the robot's end effector prone to collisions with obstacle areas during grinding operations, and trajectory deviations cannot be corrected in real time.

[0003] The grinding area and the obstacle-avoiding area on the workpiece surface are often interspersed. Traditional methods lack efficient automatic identification of these two types of areas, relying heavily on manual observation and judgment, resulting in low identification efficiency and poor consistency. Furthermore, during the grinding process, the robot's end effector is affected by external disturbances and model errors, causing it to deviate from the preset trajectory. Conventional proportional-integral-derivative (PLD-DE) control methods have limited ability to suppress system parameter perturbations and external disturbances, making it difficult to meet the trajectory tracking accuracy requirements for high-precision grinding. Therefore, it is necessary to solve the problems of how to automatically and accurately distinguish between the grindable area and the obstacle-avoiding area on the workpiece surface and generate an obstacle-avoiding grinding trajectory, and how to achieve highly robust dynamic tracking of the preset trajectory by the robot's end effector under external disturbances. Summary of the Invention

[0004] The purpose of this invention is to provide a computer vision-based trajectory planning and control system for a grinding robot, which automatically and accurately distinguishes the grindable area from the workpiece surface and generates an obstacle avoidance grinding trajectory, and achieves highly robust dynamic tracking of the robot end effector to the preset trajectory under external disturbances.

[0005] To achieve the above objectives, the present invention provides the following technical solution: The present invention provides a computer vision-based trajectory planning and control system for a grinding robot. The system includes a visual modeling module, a feature extraction module, a trajectory generation module, a pose comparison module, and a sliding mode control module. The visual modeling module is used to acquire surface point cloud data of the workpiece to be ground using a binocular vision sensor, and to construct a three-dimensional surface model of the workpiece based on the surface point cloud data, thereby providing accurate workpiece geometric information for subsequent trajectory planning.

[0006] As a technical solution of the present invention, the visual modeling module controls the binocular vision sensor to simultaneously capture images of the workpiece being polished from multiple different perspectives, obtaining multiple sets of left-eye and right-eye images; stereo matching is performed on each set of left-eye and right-eye images to calculate the disparity value of corresponding pixels; based on the disparity value, the three-dimensional spatial coordinate values ​​corresponding to each pixel on the surface of the workpiece are calculated using the principle of triangulation, and the three-dimensional spatial coordinate values ​​of all pixels are summarized into surface point cloud data; the surface point cloud data is input into a preset point cloud stitching algorithm to perform coordinate transformation and fusion processing on multiple sets of surface point cloud data corresponding to multiple different perspectives to generate a complete three-dimensional surface model of the workpiece being polished. This method can completely restore the surface morphology of the workpiece from multiple perspectives, and the point cloud stitching and fusion effectively eliminates the occlusion and blind spots of a single perspective, making the three-dimensional model more complete and accurate.

[0007] The feature extraction module is used to extract features from the three-dimensional surface model using a preset convolutional neural network to obtain the polishing area features and obstacle area features of the three-dimensional surface model. Preferably, the feature extraction module converts the three-dimensional surface model into multiple two-dimensional depth projection maps from different perspectives and inputs the two-dimensional depth projection maps into the preset convolutional neural network; the two-dimensional depth projection maps are subjected to layer-by-layer convolution operations through multiple convolutional layers in the convolutional neural network to extract edge features and texture features from the two-dimensional depth projection maps; the extracted edge features and texture features are subjected to dimensionality reduction processing through pooling layers in the convolutional neural network to obtain a dimensionality-reduced abstract feature map; the abstract feature map is classified and identified through fully connected layers in the convolutional neural network to distinguish the first type of features corresponding to the polishing area and the second type of features corresponding to the obstacle area on the surface of the polished workpiece from the abstract feature map, and the first type of features is used as the polishing area features, and the second type of features is used as the obstacle area features. By converting a 3D model into a 2D depth projection map and processing it using a convolutional neural network, the surface to be polished and untouchable obstacles on the workpiece can be quickly and accurately identified, providing a reliable basis for safe trajectory planning.

[0008] The trajectory generation module is used to divide the polishable area and the obstacle avoidance area on the three-dimensional surface model according to the features of the polishing area and the features of the obstacle area, and generate an initial polishing trajectory for the polishable area. Preferably, the trajectory generation module marks all sub-regions to be polished on the three-dimensional surface model according to the features of the polishing area, and merges all the sub-regions to be polished into the polishable area; it marks all prohibited obstacle sub-regions on the three-dimensional surface model according to the features of the obstacle area, and merges all the obstacle sub-regions into the obstacle avoidance area; it randomly selects multiple path key points within the polishable area, and uses a preset ant colony optimization algorithm to iteratively search the multiple path key points to plan the optimal path from the polishing start point to the polishing end point and bypass the obstacle avoidance area; it discretizes the optimal path according to a preset interpolation period to generate the initial polishing trajectory composed of multiple discrete path points. The initial polishing trajectory generated in this way can autonomously avoid obstacle areas while covering the entire polishable area. The ant colony optimization algorithm ensures the global optimality and smoothness of the path, which is beneficial for the smooth execution of the robot.

[0009] The pose comparison module is used to acquire the pose data of the end effector of the grinding robot in real time, compare the pose data of the end effector with the initial grinding trajectory, and calculate the pose deviation between the end effector and the initial grinding trajectory. Preferably, the pose comparison module acquires the joint angle data of each joint in real time through angle encoders installed at each joint of the grinding robot, and obtains the actual spatial position and actual spatial posture of the end effector at the current moment through forward kinematics calculation based on the joint angle data; extracts the expected trajectory point corresponding to the current moment from the initial grinding trajectory, and reads the expected spatial position and expected spatial posture corresponding to the expected trajectory point; calculates the position deviation vector between the actual spatial position and the expected spatial position, and calculates the posture deviation matrix between the actual spatial posture and the expected spatial posture; and combines the position deviation vector and the posture deviation matrix into the pose deviation. Furthermore, the pose comparison module reads the current joint angle values ​​of the corresponding joints from the angle encoders at each joint, and summarizes the current joint angle values ​​of all joints into a current joint angle vector; it reads the link parameters of the grinding robot and establishes a homogeneous transformation matrix between each link based on the link parameters; starting from the base coordinate system, it multiplies the homogeneous transformation matrices corresponding to each link sequentially to obtain the overall transformation matrix from the base coordinate system to the coordinate system where the end effector is located; it extracts the translation component from the overall transformation matrix as the actual spatial position, and extracts the rotation component from the overall transformation matrix as the actual spatial attitude. By acquiring the joint angles in real time and calculating the end effector pose using forward kinematics, the actual motion state can be accurately captured, and the deviation can be obtained by comparing it with the expected trajectory, providing accurate input for subsequent compensation control.

[0010] The sliding mode control module is used to adjust the motion parameters of the end effector according to the pose deviation using a preset sliding mode variable structure control algorithm, and to control the grinding robot to move along the initial grinding trajectory based on the adjusted motion parameters, and to perform grinding operations on the workpiece. Preferably, the sliding mode control module constructs a sliding surface function based on the pose deviation, calculates the derivative of the sliding surface function, and determines a reaching law based on the derivative of the sliding surface function; calculates the joint control torque of the grinding robot based on the sliding surface function and the reaching law, and uses the joint control torque as the motion parameter; sends the joint control torque to the joint servo driver of the grinding robot, and the joint servo driver drives each joint to rotate according to the joint control torque, so as to adjust the actual motion trajectory of the end effector, so that the actual motion trajectory converges to the initial grinding trajectory; during the movement of the end effector along the initial grinding trajectory, the end effector is controlled to perform grinding operations on the surface of the workpiece according to a preset grinding pressure. This sliding mode variable structure control method is highly robust to system parameter perturbations and external disturbances, and can quickly eliminate positional deviations to ensure accurate tracking of the grinding trajectory.

[0011] In a preferred embodiment, the sliding mode control module converts the position deviation vector and attitude deviation matrix in the pose deviation into position deviation scalars and attitude deviation scalars, respectively, and combines the position deviation scalars and attitude deviation scalars into a deviation vector; performs differentiation on the deviation vector to obtain a deviation rate of change vector, and constructs the sliding surface function according to a preset sliding surface equation based on the deviation vector and the deviation rate of change vector; performs differentiation on the sliding surface function to obtain the derivative expression of the sliding surface function; determines the reaching law parameters using the constant velocity reaching law method based on the derivative expression, and substitutes the reaching law parameters into the preset reaching law equation to obtain the reaching law corresponding to the sliding surface function. Furthermore, the sliding mode control module calculates the inertia matrix and Coriolis force matrix of the grinding robot at the current joint angle based on the dynamic model of the grinding robot; according to the sliding surface function and the reaching law, combined with the inertia matrix and the Coriolis force matrix, it calculates the initial control torque of each joint through a preset torque control law equation; the initial control torque is subjected to saturation constraint processing, and the initial control torque exceeding the rated torque limit of each joint is truncated to the corresponding rated torque limit to obtain the joint control torque; the joint control torques are packaged according to the joint number to generate a torque control command containing the torque values ​​of each joint, and the torque control command is used as the motion parameter. The torque control command is sent to the joint servo driver corresponding to each joint via industrial Ethernet. The joint servo driver parses the torque value of the corresponding joint from the torque control command. Based on the parsed torque value, the joint servo driver calculates the corresponding drive current value through its internal current loop regulator and outputs the drive current value to the servo motor of the corresponding joint. The servo motor of each joint outputs the corresponding actual torque based on the received drive current value to drive the corresponding joint to rotate in the direction and magnitude indicated by the joint control torque. After each joint rotates, the actual spatial position and actual spatial attitude of the end effector change. Through real-time closed-loop feedback, the actual motion trajectory of the end effector gradually approaches and converges to the initial grinding trajectory. This closed-loop control architecture combines sliding mode reaching law and dynamic feedforward compensation, which can ensure the speed of trajectory convergence and protect the joint drive unit through saturation constraints, thereby achieving high-precision and high-safety trajectory tracking in complex grinding operations.

[0012] In summary, the control system of this invention achieves automatic identification of the grinding area and obstacle area through visual modeling and convolutional neural network feature extraction. It generates an initial grinding trajectory for smooth obstacle avoidance by using ant colony optimization, and then uses real-time pose comparison and sliding mode variable structure control to perform online closed-loop compensation for the end effector. This effectively overcomes the adverse effects of workpiece shape and position errors, robot motion disturbances and other adverse factors, and significantly improves the autonomous operation capability, trajectory tracking accuracy and grinding quality of the grinding robot.

[0013] The technical effects and advantages provided by the present invention in the above technical solution are as follows: A 3D surface model is constructed by collecting point cloud data of the workpiece surface using a binocular vision sensor. This model is then converted into multiple 2D depth projection images from different perspectives and input into a pre-defined convolutional neural network. The convolutional layers extract edge and texture features layer by layer, and pooling layers reduce the dimensionality to obtain an abstract feature map. Finally, a fully connected layer classifies and identifies the features of the grinding area and obstacle areas from the abstract feature map. This approach transforms the problem of region identification in 3D space into a 2D image classification problem. Utilizing the automatic extraction capability of convolutional neural networks for visual features such as edges and textures, it distinguishes the area to be ground from the obstacle areas that must not be touched from various features on the workpiece surface. This eliminates the need for manual interpretation of each workpiece surface feature, reducing reliance on operator experience. After identifying the features of the grinding area and obstacle areas, the grindable area and obstacle avoidance area are marked and merged on the 3D surface model. Path key points are selected within the grindable area, and an ant colony optimization algorithm iteratively searches for the optimal path to bypass the obstacle avoidance area. Finally, the initial grinding trajectory is discretized, achieving automatic division of the grinding area and obstacle area, as well as automatic generation of the obstacle avoidance path.

[0014] After acquiring the pose data of the end effector in real time and comparing it with the initial grinding trajectory to obtain the pose deviation, the position deviation vector and attitude deviation matrix in the pose deviation are combined into a deviation vector and differentially calculated to obtain the deviation rate of change vector. Based on the deviation vector and the deviation rate of change vector, a sliding surface function is constructed. After differentiation, the approach law parameters are determined by the constant velocity approach law method and substituted into the approach law equation to obtain the approach law corresponding to the sliding surface function. Then, combined with the inertia matrix and Coriolis force matrix in the dynamic model of the grinding robot, the initial control torque of each joint is calculated through the torque control law equation. After saturation constraint processing, the joint control torque is obtained and sent to the servo driver of each joint. Sliding mode variable structure control ensures that once the system state enters the sliding surface, it remains completely invariant to parameter perturbations and external disturbances that meet the matching conditions. When the end effector is subjected to uncertain disturbances such as grinding reaction force fluctuations and joint friction changes, the control law forces the state to move toward the sliding surface and slide along the sliding surface until convergence. The actual motion trajectory of the end effector can gradually approach and converge to the initial grinding trajectory. Compared with conventional linear control methods, this improves the trajectory tracking robustness under complex disturbance environments. Attached Figure Description

[0015] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this invention. For those skilled in the art, other drawings can be obtained based on these drawings.

[0016] Figure 1 This is a schematic diagram of the trajectory planning and control system for a grinding robot based on computer vision. Figure 2 This is a flowchart of a method for modeling the 3D surface of a polished workpiece based on binocular vision and multi-view point cloud stitching; Figure 3 This is a flowchart of a path planning and obstacle avoidance method based on ant colony algorithm. Figure 4 This is a flowchart of the closed-loop sliding mode control for the pose of a grinding robot; Figure 5 This is a flowchart of the normal force resistance control process; Figure 6 This is the curve showing the change in the end effector's pose deviation over time. Figure 7 It is the normal contact force response curve of the grinding robot under normal force impedance control. Detailed Implementation

[0017] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, 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, 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.

[0018] See Figure 1 This invention provides a computer vision-based trajectory planning and control system for a grinding robot, comprising a visual modeling module, a feature extraction module, a trajectory generation module, a pose comparison module, and a sliding mode control module. The visual modeling module acquires surface point cloud data of the workpiece to be ground using a binocular vision sensor and constructs a three-dimensional surface model of the workpiece based on the point cloud data. The feature extraction module extracts features from the three-dimensional surface model using a pre-defined convolutional neural network, obtaining the features of the grinding area and the obstacle area. The trajectory generation module divides the three-dimensional surface model into a grindable area and an obstacle avoidance area based on the grinding area and obstacle area features, and generates an initial grinding trajectory for the grindable area. The pose comparison module acquires the pose data of the end effector of the grinding robot in real time, compares the pose data of the end effector with the initial grinding trajectory, and calculates the pose deviation between the end effector and the initial grinding trajectory. The sliding mode control module adjusts the motion parameters of the end effector according to the pose deviation through a preset sliding mode variable structure control algorithm, and controls the grinding robot to move along the initial grinding trajectory and perform grinding operations on the workpiece based on the adjusted motion parameters.

[0019] Example 1: In specific implementation, please refer to Figure 2 The system controls a binocular vision sensor to simultaneously capture images of the workpiece being polished from multiple different perspectives, obtaining multiple sets of left and right eye images. The binocular vision sensor consists of two monocular cameras with identical parameters, rigidly fixed on the same baseline with parallel optical axes. The binocular vision sensor is mounted on the end effector of a multi-degree-of-freedom robotic arm or a fixed multi-view acquisition bracket. The shooting position and orientation of the binocular vision sensor are adjusted so that the surface area to be modeled on the workpiece is fully or partially covered from each perspective. For each preset shooting perspective, a trigger signal is simultaneously sent to both the left and right eye cameras, causing them to expose at the same time and acquire left and right eye images respectively, forming a set of synchronized image pairs. This process is repeated for all preset perspectives to obtain multiple sets of left and right eye images for the corresponding perspective.

[0020] Stereo matching is performed on each pair of left and right visual images, calculating the disparity values ​​of corresponding pixels in the left and right images. During stereo matching, epipolar correction is first applied to the left and right images to ensure that corresponding points in the left and right images lie on the same horizontal scan line. Then, a semi-global matching algorithm is used to search for the pixel with the minimum matching cost on the horizontal epipolar line of the right image for each pixel in the left image. The calculation of the matching cost combines the grayscale difference of pixels and the mutual information within the neighborhood window to enhance the robustness of matching in areas with weak texture. The semi-global matching algorithm accumulates the matching cost along multiple directional paths and solves for the minimum energy function through dynamic programming to obtain a dense disparity map. The value at each pixel position in the disparity map is the disparity value of the corresponding pixel in the left and right images.

[0021] Based on the parallax value, the three-dimensional spatial coordinates of each pixel on the surface of the workpiece are calculated using the principle of triangulation, and the three-dimensional spatial coordinates of all pixels are summarized into surface point cloud data. Given the calibration parameters of the binocular vision sensor, for each pixel corresponding to an effective parallax value, the depth value of the corresponding point on the surface of the workpiece in the coordinate system of the binocular vision sensor is calculated using the following formula:

[0022] in, This indicates the depth distance of a point on the surface of the workpiece being polished relative to the optical center of the left eye camera in the binocular vision sensor, in millimeters. This represents the calibrated focal lengths of the left and right cameras in the binocular vision sensor, read from the camera intrinsic parameter calibration results, in pixels; This represents the baseline distance between the optical centers of the left and right cameras. It is determined based on the physical structure and assembly dimensions of the binocular vision sensor and is obtained from the sensor's factory parameters or external parameter calibration results. The unit is millimeters. The disparity value corresponding to a pixel is the difference in the horizontal coordinates of that pixel in the left and right eye images, obtained through the stereo matching step, and is expressed in pixels.

[0023] Depth value After the calculation is completed, the image coordinates of the pixel in the left eye image are then combined. Using the intrinsic parameter matrix of the left eye camera, calculate the lateral coordinates of the point relative to the optical center of the left eye camera. and vertical coordinates This allows us to obtain the three-dimensional spatial coordinates of the pixel corresponding to the point on the surface of the workpiece being polished. The above calculation is performed on all pixels covered by valid disparity values, and the three-dimensional spatial coordinates corresponding to all pixels are summarized into a set of surface point cloud data. The above stereo matching and triangulation process is repeated for a set of left and right eye images obtained from each viewpoint to obtain multiple sets of surface point cloud data corresponding to each viewpoint.

[0024] Surface point cloud data is input into a preset point cloud stitching algorithm. Multiple sets of surface point cloud data from different viewpoints undergo coordinate transformation and fusion processing to generate a complete 3D surface model of the workpiece being polished. The preset point cloud stitching algorithm employs an iterative nearest-point algorithm. The processing procedure of the iterative nearest-point algorithm is as follows: from multiple sets of surface point cloud data, the point cloud corresponding to the first viewpoint is selected as the reference point cloud, and the point clouds corresponding to the remaining viewpoints are used as the point clouds to be stitched. For each point cloud to be stitched, search for the nearest corresponding point in the reference point cloud for each point in the point cloud to be stitched, forming a set of point pairs. Based on the set of point pairs, use singular value decomposition to find the rotation matrix and translation vector that minimize the sum of the mean square distances between the point pairs. Use the rotation matrix and translation vector to perform coordinate transformation on the point cloud to be stitched, aligning it to the coordinate system of the reference point cloud. Calculate the average distance error between the aligned point cloud and the reference point cloud. If the average distance error is greater than a preset overlap accuracy threshold, use the aligned point cloud as the new point cloud to be stitched, and repeat the iterative process of nearest point search and coordinate transformation until the average distance error is less than or equal to the overlap accuracy threshold, or the maximum number of iterations is reached. Merge the stitched point cloud with the reference point cloud, and then use the point cloud corresponding to the next viewpoint as the new point cloud to be stitched, repeating the above stitching process until the surface point cloud data corresponding to all views are transformed to a unified coordinate system and fused. Voxel downsampling and statistical outlier removal are performed on the fused point cloud to eliminate redundant and noise points, resulting in a smooth and dense complete 3D surface model of the polished workpiece.

[0025] Example 2: In the specific implementation, the 3D surface model is converted into multiple 2D depth projection maps from different viewpoints, and these 2D depth projection maps are input into a pre-defined convolutional neural network. The conversion process uses virtual camera projection, placing virtual cameras at multiple pre-defined locations such as directly above, above, and to the side of the 3D surface model, with the optical axis of each virtual camera pointing towards the geometric center of the 3D surface model. For each virtual camera, a local coordinate system is established with its optical center as the origin. The coordinate values ​​of each triangular facet vertex in the 3D surface model are calculated in this local coordinate system. Using a depth-buffered rasterization method, the depth value of the nearest surface point corresponding to each pixel position is recorded as the grayscale value of the corresponding pixel in the 2D depth projection map. The global range of depth values ​​is normalized to the [0,1] interval, where 0 represents the closest distance to the virtual camera and 1 represents the farthest distance. All 2D depth projection maps corresponding to the pre-defined viewpoints are uniformly scaled to a fixed resolution of 224 pixels × 224 pixels. All 2D depth projection maps constitute the input image set, which is then fed into the input layer of the pre-defined convolutional neural network.

[0026] The pre-defined convolutional neural network employs an encoder-classifier architecture. The encoder consists of an input layer, multiple convolutional layers, and multiple pooling layers stacked alternately. The input layer receives a single-channel 2D depth projection map with a fixed resolution of 224 pixels × 224 pixels. The first convolutional layer uses 64 7×7 kernels to perform sliding convolutions on the 2D depth projection map with a stride of 2, using rectified linear units (RCUs) as the activation function to generate 64 feature response maps of size 109×109. This is followed by a max-pooling layer with a kernel size of 3×3 and a stride of 2, downsampling the spatial size of the feature response maps to 55×55. The second convolutional layer uses 128 3×3 kernels to perform convolutions with a stride of 1, using RCUs as the activation function to generate 128 feature response maps of size 55×55. This is then followed by a max-pooling layer with a kernel size of 3×3 and a stride of 2, downsampling the feature response maps to 28×28. The third convolutional layer uses 256 3×3 kernels with a stride of 1 and rectified linear units (RCUs) as the activation function, generating 256 feature response maps of size 28×28. This is followed by a max-pooling layer with a 3×3 kernel and a stride of 2, downsampling to 14×14. The fourth convolutional layer uses 512 3×3 kernels with a stride of 1 and RCUs as the activation function, generating 512 feature response maps of size 14×14. This is followed by a global average pooling layer, compressing the spatial dimension of each feature response map to 1×1, resulting in a feature vector of length 512. The classifier consists of a sequence of fully connected layers, including a first fully connected layer, a second fully connected layer, and an output layer. The first fully connected layer has 256 neurons, and the second fully connected layer has 128 neurons. Both layers use RCUs as the activation function, and each layer is followed by a random deactivation unit with a dropout probability of 0.5. The output layer has two neurons, which are activated using a flexible maximum function. The output is a two-dimensional probability distribution vector, where the first dimension represents the confidence that a pixel region belongs to the polishing region and the second dimension represents the confidence that a pixel region belongs to the obstacle region.

[0027] During the training of the pre-defined convolutional neural network, sample data containing various types of workpieces to be polished, various surface shapes, and various obstacle forms were collected. For each sample workpiece, multiple two-dimensional depth projection maps were generated using the virtual camera projection method described above. The pixel-level polishing and obstacle regions in the two-dimensional depth projection maps were then manually labeled pixel by pixel, generating a labeling mask of the same size as the two-dimensional depth projection map. In the labeling mask, a pixel value of 1 indicates that the corresponding pixel belongs to the polishing region, and a pixel value of 0 indicates that it belongs to the obstacle region. 85% of the samples in the training dataset were allocated to the training set, and 15% of the samples were allocated to the validation set. The training process used a mini-batch stochastic gradient descent optimizer with a mini-batch size of 32 and an initial learning rate of 0.001. After every 10 training epochs, the learning rate was decayed to 0.5 times its original value. The cross-entropy loss function was used, with the formula:

[0028] in, This represents the cross-entropy loss value. This represents the number of samples in a mini-batch. Indicates the first in a small batch Index of each sample, Indicates category index, Corresponding to the polishing area category, Corresponding obstacle area categories, Indicates the first Each sample in category The actual labeled values ​​are taken from the category labels of the corresponding pixels in the labeled mask image, using one-hot encoding. This indicates that the output layer of the convolutional neural network is related to the first... Each sample in category The prediction confidence level is calculated using the flexible maximum function.

[0029] Training terminates when the loss value on the validation set does not decrease over 15 consecutive training epochs. After training, the model weights with the lowest loss value on the validation set during training are saved as the final model parameters and loaded into a pre-defined convolutional neural network for inference.

[0030] During the inference phase, multiple convolutional layers in a pre-defined convolutional neural network perform layer-by-layer convolution operations on the 2D depth projection map to extract edge and texture features. Upon inputting the 2D depth projection map, the 64 convolutional kernels of the first convolutional layer perform cross-correlation operations with the map, each kernel outputting a feature response map. Different kernels are sensitive to depth gradients in different directions within the depth map, thus detecting edge features representing surface corners, edges, and contours. The second and third convolutional layers combine local patterns from the previous layer's feature response map to obtain texture features including texture direction and surface roughness distribution. The fourth convolutional layer further abstracts these features, generating high-semantic-level shape description features.

[0031] The extracted edge and texture features are reduced in dimensionality using pooling layers in a pre-defined convolutional neural network to obtain a dimensionality-reduced abstract feature map. Each max-pooling layer retains only the maximum value within the local region covered by the pooling kernel, gradually reducing the spatial resolution of the feature response map while enhancing its robustness to small translations and deformations. The global average pooling layer averages the pixel values ​​used in each feature response map, compressing the spatial dimension and generating a fixed-length abstract feature map vector.

[0032] Abstract feature maps are classified and identified using fully connected layers in a pre-defined convolutional neural network. This distinguishes between first-class features corresponding to the polishing area and second-class features corresponding to the obstacle area on the workpiece surface. The first-class features are used as the polishing area features, and the second-class features as the obstacle area features. The abstract feature map vectors are sequentially passed through weighted summation and nonlinear mapping in the first and second fully connected layers, gradually projected onto a class-separable high-level representation space. A flexible maximum function in the output layer transforms the high-level representation into probability distributions for the two classes. For each pixel location in the 2D depth projection map, the confidence values ​​of the first and second dimensions in the output layer are compared. If the confidence value of the first dimension is greater than that of the second dimension, the pixel location is determined to belong to the polishing area, and its corresponding local response in the abstract feature map is classified as the first-class feature (polishing area feature). Otherwise, the pixel location is determined to belong to the obstacle area, and its corresponding local response in the abstract feature map is classified as the second-class feature (obstacle area feature). The classification results for all pixel locations are summarized to form a complete extraction result of the polishing area features and obstacle area features in the 3D surface model.

[0033] Example 3: In specific implementation, please refer to Figure 3Based on the features of the polishing areas, all sub-regions to be polished are marked on the 3D surface model, and all sub-regions to be polished are merged into a polishable region. The polishing area features are output by a preset convolutional neural network, including the confidence score of each spatial point in the 3D surface model belonging to the polishing area. All triangular faces of the 3D surface model are traversed. For each triangular face, the confidence scores of its three vertices are extracted. If the confidence scores of all three vertices are greater than a preset polishing threshold, the triangular face is marked as a face to be polished. If only some of the three vertices have confidence scores greater than the polishing threshold, linear interpolation is performed along the edges of the triangular face to find the isopleths with confidence scores equal to the polishing threshold. Connecting these isopleths, the portion of the triangular face with a confidence score higher than the polishing threshold is extracted as the sub-region to be polished. The polishing threshold is set to 0.5. All marked surfaces and sub-regions to be polished are subjected to connected component analysis on the topology of the 3D surface model. Connected regions that are spatially adjacent and whose surface normal vectors have an angle less than a preset angle threshold are aggregated into a single sub-region to be polished. The preset angle threshold is set to 30 degrees. All the aggregated sub-regions to be polished are merged into the same set, which constitutes the polishable region.

[0034] Based on the characteristics of the obstacle areas, all prohibited obstacle sub-regions are marked on the 3D surface model, and all obstacle sub-regions are merged into an obstacle avoidance region. The obstacle area characteristics are also presented as the confidence level of each spatial point belonging to the obstacle area. Using the same judgment and segmentation method as for marking the sub-regions to be polished, triangular patches or patches with a confidence level greater than 0.5 are extracted. Connectivity analysis is performed on the extracted obstacle patch set, aggregating adjacent obstacle patches into independent obstacle sub-regions. All obstacle sub-regions are merged into the same set, which constitutes the obstacle avoidance region. In subsequent path planning, the obstacle avoidance region is modeled as an impassable 3D spatial forbidden zone; no path point can be located inside the obstacle avoidance region, and a preset safety distance must be maintained between the path segment and the surface of the obstacle avoidance region. The safety distance is set to 1.2 times the radius of the polishing robot's end effector tool.

[0035] Multiple path key points are randomly selected within the polishable area, and a pre-defined ant colony optimization algorithm is used to iteratively search these key points to plan the optimal path from the polishing start point to the polishing end point, bypassing the obstacle avoidance area. The path key points are selected by uniformly and randomly sampling a number of points within the polishable area. Candidate points, The value is determined based on the surface area of ​​the grindable area and the preset dot density parameter. The dot density parameter is set to 4 dots per square centimeter. The value is calculated by multiplying the surface area of ​​the grindable area by the point density parameter and then rounding down. Subsequently, all candidate points that fall inside the obstacle avoidance area or are less than the safe distance from the obstacle avoidance area surface are removed. The remaining candidate points are designated as path critical points. The grinding start and end points are specified by the operator on the 3D surface model according to the starting and ending requirements of the grinding task. If the grinding task needs to cover the entire grindable area, the grinding start and end points are set as the two points furthest apart on the boundary of the grindable area.

[0036] The pre-defined ant colony optimization algorithm searches on a graph composed of path key points. The graph structure is as follows: each path key point is a node in the graph. If the connecting line between any two nodes does not cross the obstacle avoidance area, and the distance between each point on the line segment and the surface of the obstacle avoidance area is not less than the safe distance, then an undirected edge is established between the two nodes. The initial pheromone concentration on each undirected edge is set to a constant. , Set to 0.01. This represents the number of ants used in the ant colony optimization algorithm. Set to the total number of path key points 0.5 times and rounded down, that is Each ant starts from the polishing starting point and progressively selects the next unvisited node according to the state transition probability formula until it reaches the polishing termination point or there are no more nodes to choose from. The state transition probability formula is:

[0037] in, Indicates the first Only ants at the node Select node The probability of being the next moving target; Represents a node With nodes The pheromone concentration on the undirected edges between them changes dynamically according to the pheromone update rule in each iteration; Represents a node With nodes The heuristic information value of the undirected edge between nodes is set as the node's heuristic information value. With nodes The reciprocal of the Euclidean distance between them; This represents the pheromone heuristic factor, reflecting the weight of the influence of pheromone concentration on ant decision-making. Its value range is [1,3], and it is set to 2. This represents the expected heuristic factor, reflecting the weight of the influence of heuristic information on ant decision-making. Its value range is [2,5], and it is set to 3. Indicates the first Only ants at the node Unvisited locations and from nodes The set of nodes that can be directly reached from the starting point via undirected edges; For set Any node in; Represents a node With nodes The pheromone concentration on the undirected edges between them; Represents a node With nodes Heuristic information values ​​of undirected edges between them.

[0038] After all ants have completed one path construction, the pheromone levels of all undirected edges in the graph are updated. The update rule is: the original pheromone concentration on each undirected edge is first multiplied by the volatile coefficient. Then, for each undirected edge traversed by an ant, the pheromone increment left by that ant is added. The pheromone increment is defined as the reciprocal of the total path length traversed by the ant multiplied by a constant. , Set it to 100. The iterative search process is repeated until the change in the total length of the optimal path obtained in 25 consecutive iterations is less than 0.1%. At this point, the iteration terminates, and the path with the shortest total length during the iteration is taken as the optimal path from the grinding start point to the grinding end point and bypasses the obstacle avoidance area.

[0039] The optimal path is discretized according to a preset interpolation period, generating an initial grinding trajectory composed of multiple discrete path points. The interpolation period is determined based on the motion control cycle of the grinding robot and is set to 4 milliseconds. Starting from the starting point of the optimal path, along the arc length of the optimal path curve, the sampling step size is obtained by multiplying the maximum allowable linear velocity at the grinding termination point by the interpolation period. Equidistant sampling is performed on the continuous curve of the optimal path to generate a series of discrete path points. The position of each discrete path point is calculated using the geometric parameter expression of the optimal path. When the remaining arc length is less than one sampling step size, the grinding termination point is taken as the last discrete path point. All discrete path points are arranged sequentially to form the initial grinding trajectory.

[0040] Example 4: In specific implementation, please refer to Figure 4 The joint angle data of each joint is collected in real time by angle encoders installed at each joint of the grinding robot, and the actual spatial position and actual spatial attitude of the end effector at the current moment are obtained by forward kinematics calculation based on the joint angle data.

[0041] The current joint angle value of each joint is read from the angle encoder at each joint, and the current joint angle values ​​of all joints are summarized into a current joint angle vector. The grinding robot contains six rotary joints, each equipped with an absolute angle encoder with a resolution of 19 bits, capable of detecting the rotation angle of the joint axis relative to the zero reference. In each motion control cycle, the main controller sends a read command to each angle encoder via an industrial Ethernet bus, and each angle encoder returns the current joint angle value of the corresponding joint, in radians. The six joint angle values ​​are arranged in order from the base to the end joint according to their joint numbers, forming a six-dimensional current joint angle vector, denoted as . , The first element corresponds to the joint angle of the base rotation joint, the second element corresponds to the joint angle of the shoulder pitch joint, the third element corresponds to the joint angle of the elbow pitch joint, the fourth element corresponds to the joint angle of the wrist rotation joint, the fifth element corresponds to the joint angle of the wrist pitch joint, and the sixth element corresponds to the joint angle of the wrist distal rotation joint.

[0042] The link parameters of the grinding robot are read, and a homogeneous transformation matrix between each link is established based on these parameters. The link parameters of the grinding robot are described using the Denavit-Hartenberg parameter table, which is stored in the controller's non-volatile memory and contains the link length corresponding to each joint. Linkage torsion angle Linkage offset and joint angle The nominal value. For the first One joint, The value ranges from 1 to 6, and the link length is... Indicates the first The axis of the first joint and the first The length of the common perpendicular between the axes of the joints, and the link twist angle. Indicates the first The axis of the first joint relative to the second joint The angle of rotation of the axis of each joint around the common perpendicular, and the link offset. Indicates the first The distance from the intersection of the previous and current perpendiculars along the axis of each joint, and the joint angle. Indicates the first The rotation angle of each joint about its own axis, when calculating the homogeneous transformation matrix, is the joint angle. Get the current joint angle vector The Middle The value of the element. The joint to the first The homogeneous transformation matrix of each joint is constructed according to the standard form of the Denavit-Hartenberg parameter, and is expressed as follows:

[0043] in, Indicates from the first The link coordinate system of the first joint is connected to the first... Homogeneous transformation matrix of the link coordinate system of each joint For the first The joint angle values ​​of each joint. For the first The torsion angle of each joint For the first The length of the link in each joint For the first Linkage offset of each joint.

[0044] Starting from the base coordinate system, multiply the homogeneous transformation matrices corresponding to each link sequentially to obtain the overall transformation matrix from the base coordinate system to the coordinate system of the end effector. The base coordinate system is the world reference coordinate system of the grinding robot, with its origin located at the geometric center of the base mounting surface. The fixed homogeneous transformation matrix from the base coordinate system to the first joint coordinate system is denoted as... This matrix is ​​determined by the mounting geometry parameters of the base, obtained through a calibration process after robot assembly, and remains constant. Overall transformation matrix. The calculation formula is:

[0045] in, This represents the global transformation matrix from the base coordinate system to the coordinate system where the end effector is located. to These are the homogeneous transformation matrices between adjacent joints constructed using the method described above. The fixed homogeneous transformation matrix from the link coordinate system of the sixth joint to the center point of the end effector tool is determined by the installation dimensions of the end effector and the tool length.

[0046] Translation components are extracted from the overall transformation matrix as the actual spatial position, and rotation components are extracted from the overall transformation matrix as the actual spatial pose. Overall transformation matrix It is a 4×4 homogeneous matrix, and the elements in the first three rows and fourth column represent the center point of the end effector tool in the base coordinate system. Axis coordinates axis coordinates and The three values—axis, coordinates, and coordinates—constitute the actual spatial position vector. The unit is millimeters. The 3×3 submatrix in the upper left corner of the overall transformation matrix constitutes the rotation matrix. The nine elements of the rotation matrix are denoted in row-major order as follows: to The rotation matrix is ​​converted into Euler angles or quaternions to obtain the actual spatial attitude. The Euler angles are represented in a fixed angle order of ZYX, corresponding to the roll angles respectively. Pitch angle and yaw angle The unit is radians. The actual spatial position and actual spatial attitude are collectively referred to as the pose data of the end effector at the current moment.

[0047] The expected trajectory point corresponding to the current moment is extracted from the initial grinding trajectory, and the expected spatial position and expected spatial pose corresponding to the expected trajectory point are read. The initial grinding trajectory consists of a series of discrete path points arranged in chronological order. Each discrete path point contains a timestamp and the corresponding expected spatial position and expected spatial pose. In the pose comparison module, a trajectory point index counter is maintained, which is initialized to the index of the first path point in the initial grinding trajectory at the start of the grinding operation. At the arrival of each motion control cycle, the current system time is read, and the initial grinding trajectory is searched backward from the current index to find the discrete path point with the smallest absolute value of the difference between the timestamp and the current system time. This discrete path point is taken as the expected trajectory point corresponding to the current moment. The expected spatial position vector stored in the expected trajectory point is read. And the desired spatial attitude, which is expressed in Euler angles as roll angles. Pitch angle and yaw angle .

[0048] Calculate the position deviation vector between the actual spatial position and the desired spatial position, and calculate the attitude deviation matrix between the actual spatial attitude and the desired spatial attitude. Position deviation vector For a three-dimensional vector, the calculation method is as follows:

[0049] Attitude deviation matrix Through the rotation matrix corresponding to the actual spatial attitude Rotation matrix corresponding to the desired spatial pose The difference between them is calculated. First, the Euler angles of the desired spatial pose are converted into the desired rotation matrix. The Euler angles of the actual spatial attitude are converted into the actual rotation matrix. Attitude deviation matrix Defined as:

[0050] in, This represents the transpose of the actual rotation matrix. This represents the transpose of the desired rotation matrix. It is a 3×3 antisymmetric matrix. The position deviation vector and attitude deviation matrix are combined into a pose deviation quantity, which is processed uniformly in the sliding mode control module.

[0051] The position deviation vector and attitude deviation matrix in the pose deviation are converted into position deviation scalars and attitude deviation scalars, respectively, and then combined into a deviation vector. Position deviation scalar Take the position deviation vector Euclidean norm:

[0052] Attitude deviation scalar From the attitude deviation matrix Extracting from the attitude deviation matrix involves taking the Frobenius norm of the matrix and dividing it by a constant 2, i.e.:

[0053] in and These represent the attitude deviation matrices respectively. Row index and column index, Represents the attitude deviation matrix The Middle Line number The elements of the column. Combine the position deviation scalar and attitude deviation scalar into a two-dimensional deviation vector. .

[0054] Differentiating the deviation vector yields the deviation rate vector. In a discrete-time system, the deviation rate vector... The calculation is performed using the backward difference approximation. Let the deviation vector of the current motion control cycle be... The deviation vector of the previous motion control cycle is ,in The duration of the motion control cycle. If set to 4 milliseconds, then the deviation change rate vector The calculation formula is:

[0055] Based on the deviation vector and the deviation rate of change vector, a sliding surface function is constructed according to a pre-defined sliding surface equation. Sliding surface function Defined as a linear combination of the components and their rates of change in the deviation vector:

[0056] in, For positional deviation scalar, Vector of deviation change rate The first component in the equation represents the rate of change of the position deviation scalar. For attitude deviation scalar Vector of deviation change rate The second component in the equation represents the rate of change of the attitude deviation scalar. This is the position deviation coefficient. The coefficient for the rate of change of position deviation. This is the attitude deviation coefficient. This is the coefficient for the rate of change of attitude deviation. The value is set to 0.8. The value is set to 0.2. The value is set to 0.6. The value is set to 0.15. The above coefficient is set based on the principle of giving position deviation and attitude deviation a reasonable weight ratio in the sliding surface function, while taking into account the system's response speed to deviation and the need to suppress chattering.

[0057] Differentiating the sliding surface function yields its derivative expression. for:

[0058] in, Let the second rate of change of the position deviation scalar be... The second-order rate of change of the attitude deviation scalar is obtained by further backward difference calculation of the deviation rate of change vector.

[0059] Based on the derivative expression, the reaching law parameters are determined using the constant-velocity reaching law method. These parameters are then substituted into the preset reaching law equation to obtain the reaching law corresponding to the sliding surface function. The constant-velocity reaching law equation is in the form of:

[0060] in, Here, represents the rate at which the system state approaches the sliding surface, (the approximation parameter). The value is set to 2.5. For the sign function, when the sliding surface function When the value is greater than 0 The value is 1, when When equal to 0 The value is 0, when When less than 0 The value is -1. (Touching law parameter) The basis for setting it to 2.5 is that within the rated response bandwidth of the grinding robot joint servo system, this approach rate can ensure that the pose deviation converges to the neighborhood of the sliding surface within 0.3 seconds, while not stimulating high-frequency unmodeled dynamics of the joint servo driver.

[0061] The joint control torques of the grinding robot are calculated based on the sliding surface function and the reaching law, and these torques are used as motion parameters. Based on the dynamic model of the grinding robot, the inertia matrix and Coriolis force matrix of the grinding robot at the current joint angle are calculated. The dynamic model of the grinding robot is established using the Lagrangian method and stored in the model parameter library of the sliding mode control module. (Inertia matrix...) Given a 6×6 symmetric positive definite matrix, the nth matrix... Line number Column elements Indicates the first The acceleration of the first joint affects the acceleration of the second joint. Contribution coefficients of inertial moment for each joint. Coriolis force matrix. It is a 6×6 matrix, where This is the current joint angular velocity vector, calculated by backward difference of the current joint angular vector. The [missing information] in the Coriolis force matrix... Line number The elements of the column represent the first... The speed of the first joint affects the speed of the second joint. The contribution coefficients of Coriolis force and centrifugal torque for each joint. The inertia matrix and Coriolis force matrix are recalculated once in each motion control cycle based on the latest current joint angle vector and current joint angular velocity vector.

[0062] Based on the sliding surface function and the reaching law, combined with the inertia matrix and Coriolis force matrix, the initial control torque of each joint is calculated using a pre-set torque control law equation. The torque control law equation adopts the standard form of the sliding mode control law:

[0063] in, This is a six-dimensional initial control torque vector, with units of Newton-meters. The inertia matrix; The Jacobian matrix for polishing the robot represents the linear mapping relationship from joint space velocity to end effector task space velocity, and is calculated based on the current joint angle vector; This is the pseudo-inverse of the Jacobian matrix; The vector of pose deviation in the mission space is obtained by combining the derivatives of the position deviation vector and the attitude deviation matrix. The task space pose deviation vector is obtained by combining the position deviation vector and the equivalent angle deviation extracted from the attitude deviation matrix. It is a 6×6 positive definite diagonal gain matrix. The first three elements on the diagonal correspond to the position gain, which is set to 80, and the last three elements correspond to the attitude gain, which is set to 50. It is a 6×6 positive definite diagonal damping matrix. The first three elements on the diagonal correspond to position damping, which is set to 40, and the last three elements correspond to attitude damping, which is set to 25. This is a six-dimensional convergence law parameter vector, with each dimension set to 3.0; Let the sliding surface vectors in the task space be defined by the sliding surface function. This is obtained by converting it into a six-dimensional representation in the task space; Indicates to Each component is operated on using a sign function; The Coriolis force matrix; This is the current joint angular velocity vector.

[0064] The initial control torque is saturated and constrained, with any initial control torque exceeding the rated torque limit of each joint being truncated to the corresponding rated torque limit to obtain the joint control torque. The rated torque limit for each joint is read from the specifications of the grinding robot. The rated torque limits for the six joints are as follows: Joint 1: 200 N·m, Joint 2: 180 N·m, Joint 3: 150 N·m, Joint 4: 80 N·m, Joint 5: 80 N·m, Joint 6: 50 N·m. For the initial control torque vector... The first in Each component , The value ranges from 1 to 6, representing the joint control torque vector. The Each component Determine using the following formula: like Greater than the Rated torque limit of each joint ,but ;like Less than ,but ;otherwise .

[0065] The joint control torques are packaged according to the joint numbers to generate torque control commands containing the torque values ​​of each joint, and these torque control commands are used as motion parameters. The data frame format of the torque control command includes a frame header, a command counter, torque value fields for the six joints, and a checksum. The command counter starts at 0 and increments by 1 with each torque control command frame sent, resetting to zero after reaching 65535. The torque value fields for the six joints are filled sequentially according to their joint numbers. to The torque values ​​are 4 bytes each, encoded using 32-bit single-precision floating-point numbers. The checksum is calculated using a cyclic redundancy check algorithm, covering all data from the frame header to the last torque value field. The encapsulated torque control command is output as motion parameters to the communication interface of the joint servo driver.

[0066] Torque control commands are sent to the corresponding joint servo drives via industrial Ethernet. Each joint servo drive then parses the torque value for its respective joint from the torque control commands. Industrial Ethernet uses the EtherCAT protocol, with the main controller acting as the EtherCAT master and each joint servo drive as an EtherCAT slave, connected via a daisy-chain topology. Torque control commands are sent periodically in the form of EtherCAT data frames, with a sending period consistent with the motion control period of 4 milliseconds. Each joint servo drive locates its own sub-message position within the EtherCAT data frame, extracts the torque value field corresponding to the joint number from the data area of ​​the sub-message, and converts this torque value field back to a single-precision floating-point number to obtain the torque value for that joint.

[0067] Each joint's servo driver calculates the corresponding drive current value based on the analyzed torque value using an internal current loop regulator, and outputs the drive current value to the corresponding joint's servo motor. The current loop regulator uses a proportional-integral controller, with a proportional gain set to 2.5 amperes per Newton-meter and an integral time constant set to 0.8 milliseconds. Drive current value The calculation formula is:

[0068] in, For proportional gain, This represents the torque value of this joint. For integral gain, It is obtained by dividing the proportional gain by the integral time constant. This is the current moment. The integral term is accumulated and updated within each motion control cycle.

[0069] Each joint's servo motor outputs the corresponding actual torque based on the received drive current value, driving the corresponding joint to rotate in the direction and magnitude indicated by the joint control torque. The servo motor is a permanent magnet synchronous motor. In vector control mode, the motor driver converts the drive current value into a quadrature-axis current command, while the direct-axis current command remains zero. A three-phase voltage is generated through space vector pulse width modulation, driving the motor to output an electromagnetic torque proportional to the quadrature-axis current amplitude. This electromagnetic torque is amplified by a reducer and then applied to the joint output shaft, driving the joint link to rotate around the joint axis.

[0070] After each joint rotates, the actual spatial position and attitude of the end effector change. Through real-time closed-loop feedback, the actual motion trajectory of the end effector gradually approaches and converges to the initial grinding trajectory. Within each motion control cycle, the above process begins with reading the angle encoder and ends with the servo motor outputting torque, forming a complete posture closed-loop control loop. As the motion control cycle progresses, the posture deviation continuously decreases under the action of sliding mode variable structure control. The deviation between the actual motion trajectory of the end effector and the initial grinding trajectory gradually narrows, and the actual motion trajectory of the end effector is adjusted and maintained within the preset neighborhood of the initial grinding trajectory.

[0071] See Figure 6 In the graph, the horizontal axis represents time in seconds, ranging from 0 to 2 seconds. The left vertical axis represents the position deviation scalar in millimeters, ranging from 0 to 5 millimeters; the right vertical axis represents the attitude deviation scalar in radians, ranging from approximately -0.05 to 0.30 radians. Solid curves represent the trend of position deviation scalars, while dashed curves represent the trend of attitude deviation scalars. The legend corresponding to the curves is located at the top center of the graph.

[0072] At the initial time of 0 seconds, the position deviation scalar is approximately 4.8 mm, and the attitude deviation scalar is approximately 0.27 radians, indicating a significant deviation between the actual pose of the grinding robot's end effector and the desired pose of the initial grinding trajectory. Over time, the position deviation scalar shows a rapid overall decrease, dropping below approximately 1 mm at 0.5 seconds, and then fluctuating between 0 and 0.5 mm after 1 second, stabilizing and remaining at a low level. This indicates that the position control accuracy of the end effector gradually improves, and the actual position gradually approaches the desired trajectory.

[0073] The attitude deviation scalar curve shows an overall trend of rapid decline followed by slow fluctuation. In the first second, it drops from about 0.27 radians to about 0.05 radians, and then fluctuates around 0.05 radians, with a fluctuation range of about ±0.05 radians. This indicates that the attitude control error is reduced and maintained within a low fluctuation range, indicating that the end effector has good attitude stability and can effectively track the desired trajectory attitude.

[0074] Throughout the 2-second period, the position deviation scalar decreased faster than the attitude deviation scalar, but both stabilized after approximately 1 second, with the error amplitude remaining within a small range. This trend indicates that the trajectory tracking control system based on the sliding mode control module has strong convergence capability and robustness to the position and attitude deviation of the end effector, effectively reducing initial deviations and achieving stable motion control of the end effector along the initial grinding trajectory.

[0075] The curve occasionally experiences short-term fluctuations in the middle stage, possibly caused by the system's dynamic response and external disturbances. However, the overall error does not increase drastically, demonstrating the system control strategy's effectiveness in suppressing disturbances. The above-described pose deviation convergence process satisfies the position deviation coefficient requirement of the sliding mode variable structure control algorithm in Example 4. Coefficient of change of position deviation Attitude deviation coefficient and attitude deviation change rate coefficient The design requirements were met, and the effectiveness of the closed-loop control loop, which uses the angle encoder to collect joint angle data and calculate the actual pose using forward kinematics, was verified.

[0076] Example 5: In specific implementation, please refer to Figure 5 During the movement of the end effector along the initial grinding trajectory, the end effector is controlled to perform grinding operations on the surface of the workpiece according to the preset grinding pressure.

[0077] The preset grinding pressure is set by the operator before the grinding operation begins, based on the material type, surface hardness, and grinding process requirements of the workpiece, and is stored in the parameter configuration table of the main controller. The preset grinding pressure is set to 15 Newtons (N) for aluminum alloy, 25 N for stainless steel, and 35 N for titanium alloy. The preset grinding pressure setting can be adjusted in 0.5 N increments within the range of 5 N to 50 N.

[0078] During the grinding operation, a six-dimensional force sensor is installed on the end effector, fixed between the flange of the end effector and the grinding tool fixture. The six-dimensional force sensor collects the force components and torque components acting on the grinding tool in three orthogonal directions in real time during each motion control cycle, with a cycle length of 4 milliseconds. The three force components collected are the force along the X-axis of the six-dimensional force sensor's coordinate system. Force along the Y-axis and force along the Z-axis The unit is Newtons (N). The Z-axis of the six-dimensional force sensor coordinate system coincides with the axis of the grinding tool, pointing towards the surface being ground.

[0079] Extract the normal contact force along the axis of the grinding tool from the force components in the three orthogonal directions. Normal contact force The calculation method is as follows: read the normal vector of the end effector relative to the surface of the workpiece at the grinding contact point. normal vector The normal vector is obtained by querying the triangular facet normal vector corresponding to the current grinding contact point position in the 3D surface model. Transform to the six-dimensional force sensor coordinate system to obtain the transformed normal vector. Then the force vector acquired by the six-dimensional force sensor With the transformed normal vector Perform a dot product operation to obtain the normal contact force. :

[0080] in, This indicates the magnitude of the normal contact force applied by the grinding tool to the surface of the workpiece, measured in Newtons (N). , , These represent the force components along the X, Y, and Z axes of the sensor coordinate system, respectively, acquired by the six-dimensional force sensor. , , They represent the transformed normal vectors respectively. Components in the X, Y, and Z axes of the six-dimensional force sensor coordinate system.

[0081] Calculate normal contact force With preset polishing pressure Pressure deviation between , .

[0082] Pressure deviation The input is sent to the normal force impedance controller. The normal force impedance controller uses a proportional-integral control law, and the input to the controller is the pressure deviation. The output is a pose correction speed command along the axis of the grinding tool. The expression for the proportional-integral control law is:

[0083] in, The pose correction velocity command is in millimeters per second. To control the proportional gain, it is set to 0.8 mm / s / N; The integral gain for force control is set to 0.15 mm / s / N·s; This is the pressure deviation. The cumulative time for force control closed loop is started from the moment the grinding tool contacts the workpiece surface. Proportional gain and integral gain The setting is based on the following: the bandwidth of the pressure closed loop is about 8 Hz, and the normal contact force is adjusted to within ±5% of the preset grinding pressure value within 0.15 seconds after the grinding tool collides with the workpiece. At the same time, it avoids the end effector from bouncing and oscillating on the workpiece surface due to excessive gain.

[0084] Posture correction speed command The direction is along the axis of the grinding tool and points inwards towards the surface of the workpiece being ground. The pose correction speed command is then executed. The velocity command is superimposed on the end effector motion speed command output by the sliding mode control module. The end effector motion speed command output by the sliding mode control module is calculated by the sliding mode variable structure control algorithm based on the pose deviation and is used to drive the end effector to move along the tangential direction of the initial grinding trajectory. The superimposed composite velocity command is then solved in inverse kinematics in Cartesian space to convert it into angular velocity commands for each joint.

[0085] The angular velocity commands for each joint are sent to the speed loop of the joint servo driver. The speed loop of the joint servo driver calculates the drive current for each joint based on the angular velocity commands, and drives the servo motor to rotate. When the servo motor rotates, the end effector, while maintaining its movement along the initial grinding trajectory, generates a small normal position adjustment along the axis of the grinding tool to adjust the normal contact force of the grinding tool on the surface of the workpiece.

[0086] When normal contact force Less than the preset grinding pressure At that time, pressure deviation If positive, the pose correction speed command is executed. When the value is positive, the end effector moves closer to the surface of the workpiece, increasing the contact depth between the grinding tool and the workpiece surface, thus increasing the normal contact force and bringing it closer to the preset grinding pressure. When the normal contact force... Greater than the preset polishing pressure At that time, pressure deviation A negative value indicates a pose correction speed command. When the value is negative, the end effector moves away from the workpiece surface, reducing the contact depth between the grinding tool and the workpiece surface, thus decreasing the normal contact force and bringing it closer to the preset grinding pressure. When the normal contact force... With preset polishing pressure When the absolute value of the difference is less than the preset force control dead zone threshold, the pose correction speed command is activated. When the value is set to zero, the end effector maintains its current contact state and continues the grinding operation. The preset force control dead zone threshold is set to 0.5 Newtons.

[0087] As the grinding tool moves along the initial grinding trajectory and covers the entire grindable area, the normal force impedance controller repeatedly performs the acquisition, calculation, and correction steps in each motion control cycle, so that the normal contact force between the grinding tool and the surface of the workpiece is always maintained near the preset grinding pressure, thus achieving constant force grinding operation.

[0088] See Figure 7In the graph, the horizontal axis represents time in seconds, ranging from 0 to 4 seconds; the vertical axis represents force in Newtons, ranging from 0 to approximately 28 Newtons. The dashed line represents the preset grinding pressure, which is constant at 25 Newtons and represents the target pressure value set when grinding stainless steel. The solid line is the curve showing the actual normal contact force changing over time.

[0089] Starting from 0 seconds, the actual normal contact force rapidly increases from 0 N, exhibiting a relatively steep upward trend, reaching its maximum at approximately 1.2 seconds and then beginning to fluctuate around the preset grinding pressure of 25 N. The solid curve enters a steady-state phase after 1.2 seconds, with small fluctuations, consistently remaining between approximately 23 N and 26 N, indicating that the normal contact force is effectively regulated and stably maintained near the set grinding pressure. The fluctuation range of the normal contact force does not exceed the ±5% error range of the preset pressure, meeting the requirement of a force control dead zone threshold of 0.5 N in Example 5.

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

Claims

1. A computer vision-based trajectory planning and control system for a grinding robot, characterized in that, The system includes: The visual modeling module is used to collect surface point cloud data of the workpiece being polished through a binocular vision sensor, and to construct a three-dimensional surface model of the workpiece being polished based on the surface point cloud data. The feature extraction module is used to extract features from the three-dimensional surface model through a preset convolutional neural network to obtain the polishing area features and obstacle area features of the three-dimensional surface model. The trajectory generation module is used to divide the polishable area and the obstacle avoidance area on the three-dimensional surface model according to the polishing area features and the obstacle area features, and generate the initial polishing trajectory of the polishable area. The pose comparison module is used to acquire the pose data of the end effector of the grinding robot in real time, compare the pose data of the end effector with the initial grinding trajectory, and calculate the pose deviation between the end effector and the initial grinding trajectory. The sliding mode control module is used to adjust the motion parameters of the end effector according to the pose deviation amount through a preset sliding mode variable structure control algorithm, and control the grinding robot to move along the initial grinding trajectory based on the adjusted motion parameters, and to perform grinding operations on the workpiece.

2. The computer vision-based trajectory planning and control system for a grinding robot according to claim 1, characterized in that, The step of acquiring surface point cloud data of the workpiece to be polished using a binocular vision sensor and constructing a three-dimensional surface model of the workpiece based on the surface point cloud data includes: The binocular vision sensor is controlled to simultaneously capture images of the workpiece being polished from multiple different perspectives, thereby obtaining multiple sets of left-eye and right-eye images. Stereo matching is performed on each set of left and right eye images, and the disparity value of corresponding pixels in the left and right eye images is calculated. Based on the parallax value, the three-dimensional spatial coordinates of each pixel on the surface of the workpiece are calculated using the triangulation principle, and the three-dimensional spatial coordinates of all pixels are summarized into the surface point cloud data. The surface point cloud data is input into a preset point cloud stitching algorithm, and coordinate transformation and fusion processing are performed on multiple sets of surface point cloud data corresponding to multiple different perspectives to generate a complete three-dimensional surface model of the workpiece being polished.

3. The computer vision-based trajectory planning and control system for a grinding robot according to claim 2, characterized in that, The step of extracting features from the three-dimensional surface model using a pre-defined convolutional neural network to obtain the polished area features and obstacle area features of the three-dimensional surface model includes: The three-dimensional surface model is converted into multiple two-dimensional depth projection maps from different perspectives, and the two-dimensional depth projection maps are input into the preset convolutional neural network; The convolutional neural network performs layer-by-layer convolution operations on the two-dimensional depth projection map to extract edge features and texture features from the two-dimensional depth projection map. The extracted edge features and texture features are reduced in dimensionality by using the pooling layer in the convolutional neural network to obtain a dimensionality-reduced abstract feature map. The abstract feature map is classified and identified by the fully connected layer in the convolutional neural network. The first type of feature corresponding to the grinding area and the second type of feature corresponding to the obstacle area on the surface of the workpiece are distinguished from the abstract feature map. The first type of feature is used as the grinding area feature and the second type of feature is used as the obstacle area feature.

4. The computer vision-based trajectory planning and control system for a grinding robot according to claim 3, characterized in that, Based on the features of the polishing area and the features of the obstacle area, a polishable area and an obstacle avoidance area are divided on the three-dimensional surface model, and an initial polishing trajectory for the polishable area is generated, including: Based on the characteristics of the polishing area, all sub-regions to be polished are marked on the three-dimensional surface model, and all the sub-regions to be polished are merged into the polishable area. Based on the characteristics of the obstacle area, all prohibited obstacle sub-regions are marked on the three-dimensional surface model, and all the obstacle sub-regions are merged into the obstacle avoidance area; Multiple key path points are randomly selected within the polishable area, and a preset ant colony optimization algorithm is used to iteratively search the multiple key path points to plan the optimal path from the polishing start point to the polishing end point and bypass the obstacle avoidance area. The optimal path is discretized according to a preset interpolation period to generate the initial grinding trajectory composed of multiple discrete path points.

5. The computer vision-based trajectory planning and control system for a grinding robot according to claim 4, characterized in that, The pose data of the end effector of the grinding robot is acquired in real time, and the pose data of the end effector is compared with the initial grinding trajectory to calculate the pose deviation between the end effector and the initial grinding trajectory, including: The joint angle data of each joint is collected in real time by angle encoders installed at each joint of the grinding robot, and the actual spatial position and actual spatial attitude of the end effector at the current moment are obtained by forward kinematics calculation based on the joint angle data. Extract the desired trajectory point corresponding to the current moment from the initial polishing trajectory, and read the desired spatial position and desired spatial attitude corresponding to the desired trajectory point; Calculate the position deviation vector between the actual spatial position and the desired spatial position, and calculate the attitude deviation matrix between the actual spatial attitude and the desired spatial attitude; The position deviation vector and the attitude deviation matrix are combined into the pose deviation amount.

6. The computer vision-based trajectory planning and control system for a grinding robot according to claim 5, characterized in that, Based on the pose deviation, the motion parameters of the end effector are adjusted using a preset sliding mode variable structure control algorithm. The adjusted motion parameters are then used to control the grinding robot to move along the initial grinding trajectory and perform grinding operations on the workpiece, including: Construct a sliding surface function based on the pose deviation, calculate the derivative of the sliding surface function, and determine the reaching law based on the derivative of the sliding surface function; The joint control torque of the grinding robot is calculated based on the sliding surface function and the reaching law, and the joint control torque is used as the motion parameter. The joint control torque is sent to the joint servo driver of the polishing robot, and the joint servo driver drives each joint to rotate according to the joint control torque, so as to adjust the actual motion trajectory of the end effector and make the actual motion trajectory converge to the initial polishing trajectory. During the movement of the end effector along the initial grinding trajectory, the end effector is controlled to perform grinding operations on the surface of the workpiece according to a preset grinding pressure.

7. The computer vision-based trajectory planning and control system for a grinding robot according to claim 5, characterized in that, The joint angle data of each joint is collected in real time by angle encoders installed at each joint of the grinding robot, and the actual spatial position and actual spatial attitude of the end effector at the current moment are obtained by forward kinematics calculation based on the joint angle data, including: The current joint angle value of each joint is read from the angle encoder at each joint, and the current joint angle values ​​of all joints are summarized into a current joint angle vector. Read the link parameters of the grinding robot, and establish a homogeneous transformation matrix between each link based on the link parameters; Starting from the base coordinate system, the homogeneous transformation matrices corresponding to each link are multiplied sequentially to obtain the overall transformation matrix from the base coordinate system to the coordinate system where the end effector is located; The translation component is extracted from the overall transformation matrix as the actual spatial position, and the rotation component is extracted from the overall transformation matrix as the actual spatial pose.

8. The computer vision-based trajectory planning and control system for a grinding robot according to claim 6, characterized in that, Constructing a sliding surface function based on the pose deviation, calculating the derivative of the sliding surface function, and determining the reaching law based on the derivative of the sliding surface function, including: The position deviation vector and attitude deviation matrix in the pose deviation are converted into position deviation scalars and attitude deviation scalars, respectively, and the position deviation scalars and attitude deviation scalars are combined into a deviation vector; Differentiate the deviation vector to obtain the deviation rate of change vector, and construct the sliding surface function according to the deviation vector and the deviation rate of change vector and the preset sliding surface equation. The derivative of the sliding surface function is obtained by performing a derivative operation on the sliding surface function; Based on the derivative function expression, the reaching law parameters are determined using the constant velocity reaching law method, and the reaching law parameters are substituted into the preset reaching law equation to obtain the reaching law corresponding to the sliding surface function.

9. The computer vision-based trajectory planning and control system for a grinding robot according to claim 8, characterized in that, The joint control torque of the grinding robot is calculated based on the sliding surface function and the reaching law, and the joint control torque is used as the motion parameter, including: Based on the dynamic model of the grinding robot, calculate the inertia matrix and Coriolis force matrix of the grinding robot at the current joint angle; Based on the sliding surface function and the approach law, combined with the inertia matrix and the Coriolis force matrix, the initial control torque of each joint is calculated through the preset torque control law equation. The initial control torque is subjected to saturation constraint processing, and the initial control torque exceeding the rated torque limit of each joint is cut off to the corresponding rated torque limit to obtain the joint control torque; The joint control torques are packaged according to the joint numbers to generate torque control commands containing the torque values ​​of each joint, and the torque control commands are used as the motion parameters.

10. The computer vision-based trajectory planning and control system for a grinding robot according to claim 9, characterized in that, The joint control torque is sent to the joint servo driver of the grinding robot, and the joint servo driver drives each joint to rotate according to the joint control torque, so as to adjust the actual motion trajectory of the end effector and make the actual motion trajectory converge to the initial grinding trajectory, including: The torque control command is sent to the joint servo driver corresponding to each joint via industrial Ethernet. The joint servo driver corresponding to each joint parses the torque value of the corresponding joint from the torque control command. The joint servo driver corresponding to each joint calculates the corresponding drive current value through the internal current loop regulator based on the parsed torque value, and outputs the drive current value to the servo motor of the corresponding joint. The servo motor of each joint outputs the corresponding actual torque according to the received drive current value, so as to drive the corresponding joint to rotate in the direction and magnitude indicated by the joint control torque; After each joint rotates, the actual spatial position and actual spatial orientation of the end effector change. Through real-time closed-loop feedback, the actual motion trajectory of the end effector gradually approaches and converges to the initial grinding trajectory.