Robot motion control method, robot and system

Abstract

This invention relates to the field of robot control technology, specifically to a motion control method, robot, and system for a robot. It includes the following steps: acquiring depth information, color information, and visual texture and geometric features of the target object using an RGB-D camera to generate a local environment map; introducing motor current sampling values, correcting the material type of the target object, and determining the initial setting value of the clamping force; converting the corrected joint torque estimate into an end-effector force correction value using the robotic arm Jacobian matrix; calculating the position control output and impedance force control output based on force control weights and position control weights, and combining the initial setting value of the clamping force with a cogging compensation term to generate the final control command. A harmonic compensation function based on joint position, velocity, and current is constructed, effectively suppressing periodic torque disturbances caused by the cogging effect of the servo motor, making the end-effector force estimate closer to the actual force, and improving the stability of the clamping control.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and more specifically, to a motion control method, robot, and system for a robot. Background Art

[0002] In recent years, with the rapid development of artificial intelligence, sensor technology, and low-cost servo drives, the demand for desktop robots (robotic arms) has continued to grow in fields such as education and scientific research, precision assembly, medical assistance, laboratory automation, and service industries. Compared with industrial-grade equipment, these robotic arms have advantages such as small size, flexible deployment, and low cost, making them suitable for performing high-precision repetitive operations in limited spaces.

[0003] However, traditional methods often identify material objects through visual features or shallow machine learning classification models, but these methods cannot reflect the true contact mechanical properties. For example, plastic and glass have similar textures under illumination, and simple visual recognition is prone to misjudgment, leading to a mismatch between the clamping force setting and the actual material, resulting in problems such as unstable clamping or breakage. Therefore, there is an urgent need for a motion control method, robot, and system for robots. Summary of the Invention

[0004] The purpose of this invention is to provide a motion control method, robot, and system for a robot, addressing the shortcomings of traditional methods mentioned in the background art, which often rely on visual features or shallow machine learning classification models to identify object materials. However, these methods fail to reflect the true contact mechanical properties. For example, plastics and glass have similar textures under illumination, making simple visual recognition prone to misjudgment. This leads to a mismatch between the clamping force setting and the actual material, resulting in unstable clamping or breakage.

[0005] To achieve the above objectives, the present invention aims to provide a motion control method for a robot, comprising the following steps:

[0006] S1. Acquire the current scene's depth information, color information, and the visual texture and geometric features of the target object using an RGB-D camera. Perform instance segmentation and six-degree-of-freedom pose estimation based on the color and depth information to obtain the target object's three-dimensional position coordinates and spatial pose parameters, which are then used to generate a local environment map.

[0007] S2. Input the visual texture and geometric features of the target object into a pre-trained convolutional neural network to determine the material category of the target object, and introduce the motor current sampling value to correct the material category of the target object. Based on the corrected material category, determine the initial setting value of the clamping force.

[0008] S3. Based on the local environment map, a sampling motion planning algorithm is used to perform global sampling planning to generate an initial trajectory point sequence. Based on the obstacle information updated in real time by the sensor, the local path information is incrementally optimized using the CHOMP algorithm to obtain the updated trajectory point sequence.

[0009] S4. Based on the real-time distance between the end effector and the surface of the target object, the nearest neighbor search algorithm is used to calculate the target proximity, and the force control weight and position control weight are adjusted based on the target proximity.

[0010] S5. Based on the state estimation algorithm, multi-source signals are fused to calculate the estimated values ​​of joint torque and end force. The estimated values ​​of joint torque are corrected by introducing a tooth cogging effect compensation function. The corrected estimated values ​​of joint torque are converted into the corrected values ​​of end force by using the Jacobian matrix of the robotic arm.

[0011] Among them, the multi-source signals include the current of the robotic arm joint drive motor, joint acceleration, end effector IMU acceleration and angular velocity signals;

[0012] S6. Based on the force control weight and position control weight, calculate the position control output and impedance force control output respectively, and generate the final control command by combining the initial setting value of clamping force and the tooth cogging compensation term.

[0013] As a further improvement to this technical solution, in step S1, the following steps are performed on the color information and depth information to generate a local environment map:

[0014] Based on the current scene depth information and the camera intrinsic and extrinsic parameter calibration results, the pixel coordinates are transformed into a 3D point cloud in the camera coordinate system through back projection, and then transformed into the robot arm base coordinate system through the hand-eye calibration matrix. The 3D point cloud is then preprocessed to obtain a preprocessed 3D point cloud.

[0015] A color image is input into a YOLOv8-seg network that fuses depth channels. The network decoder and mask prediction head then output a pixel-level instance mask of the target object. ;

[0016] Pixel-level instance mask based on target object Within the masked area, planar target feature points of the color image and corresponding depth map are extracted to obtain the target object's three-dimensional target point set;

[0017] The three-dimensional point set of the target is solved by the PnP algorithm to obtain the spatial pose of the target object in the camera coordinate system. Combined with the hand-eye calibration matrix, the six-degree-of-freedom pose estimation result of the target object in the robot arm base coordinate system is obtained.

[0018] The six-degree-of-freedom pose estimation results include the three-dimensional position coordinates of the target object, surface normal information, and rotation angle information.

[0019] The occupancy modeling method based on voxel grid mapping discretizes the preprocessed 3D point cloud into spatial grid cells and iteratively updates the occupancy probability of each grid cell using a Bayesian occupancy probability algorithm. At the same time, the 3D pose markers of the target object are superimposed on the point cloud map to finally generate a local environment map.

[0020] As a further improvement to this technical solution, the specific steps involved in S2, which determine the target object material type and the initial setting value of the clamping force, are as follows:

[0021] Visual texture features and geometric features are used as inputs and spliced ​​together to form a fused feature;

[0022] The fused features are input into a pre-trained convolutional neural network to obtain the initial material category;

[0023] Real-time current sampling value based on the joint motor of the robotic arm Constructing material correction functions Correct the material category of the target object;

[0024] Based on the corrected material category, retrieve a matching set of material parameters from the preset parameter library. In the formula, The coefficient of friction of the target object; Indicates the density of the target object's material; Indicates the safety factor; Indicates the recommended clamping force range for the target object material; The maximum surface pressure of the target object's material;

[0025] Based on material parameter set And based on the effective contact area between the gripper and the target object. By introducing a comprehensive correction factor The final initial setting value of the clamping force is obtained. ;

[0026] Among them, the comprehensive correction factor It is obtained by constructing the clamping attitude angle and the buffer acceleration margin through the Sigmoid smoothing function.

[0027] As a further improvement to this technical solution, the specific steps involved in generating the updated trajectory point sequence in step S3 are as follows:

[0028] The system collects point cloud data of the surrounding environment in real time using sensors and constructs a signed distance field by combining it with a local environment map.

[0029] Based on the starting point and target grasping position of the robotic arm, a sampling motion planning algorithm is used to generate an initial trajectory point sequence. In the formula, This represents the number of discrete trajectory points from the starting point to the ending point. The endpoint node; Indicates the starting node;

[0030] Based on the initial trajectory point sequence Introducing a real-time dynamic obstacle set Information about the target object, and the initial trajectory point sequence. Perform incremental optimization:

[0031] For the initial trajectory point sequence For each trajectory point, gradient descent is performed using the minimum trajectory cost function until the minimum trajectory cost function converges, resulting in the optimized trajectory. In the formula, This is the optimized endpoint node; This represents the optimized starting node;

[0032] During the iteration process, a neighborhood search algorithm is used to calculate the value of each trajectory point. With dynamic obstacle points European distance ;

[0033] If Euclidean distance If the distance is less than or equal to the safe distance threshold, then the trajectory point is determined. Conflict with obstacles will disrupt the entire initial trajectory point sequence. The set of all points that satisfy this condition is taken as the set of conflict trajectory points;

[0034] Using the target surface normal vector to track points close to the target object After local fine-tuning, the updated trajectory point sequence is finally output. .

[0035] As a further improvement to this technical solution, the specific steps involved in adjusting the force control weight and position control weight based on the target proximity in step S4 are as follows:

[0036] Target proximity is defined based on the minimum distance between the end effector and the target surface. ,in, This indicates that the end point is far from the target. This indicates that the end is close to the target surface;

[0037] Using the Sigmoid mapping function, the target proximity is... Mapped to position control weights respectively and force control weight .

[0038] As a further improvement to this technical solution, the specific steps involved in S5 for correcting the joint torque estimate and converting the corrected joint torque estimate into an end-effector force correction value are as follows:

[0039] Based on the state estimation algorithm, multi-source signals are fused to calculate the estimated values ​​of initial joint torque and initial end force.

[0040] For each joint By introducing a cogging effect compensation function The cogging effect compensation term was calculated. ;

[0041] Compensation term based on cogging effect The initial joint moment estimate is corrected to obtain the corrected joint moment value;

[0042] The Jacobian matrix of the robotic arm is used to map the joint torque correction values ​​to the end effector force correction values;

[0043] Among them, the tooth cogging effect compensation function This is a compensation function constructed based on joint position, joint velocity, and joint current.

[0044] As a further improvement to this technical solution, the specific steps involved in generating the final control command in step S6 are as follows:

[0045] Based on the expected position of the planned trajectory points and the actual position of the robotic arm's end effector, a trajectory following algorithm is adopted, and position control weights are introduced. The position control output is calculated. ;

[0046] Based on end force correction value and force control weight Generate impedance control output ;

[0047] Position control output With impedance control output The process involves fusion and the introduction of an initial clamping force setting. Amplitude constraints are applied to the end force component of the fused output;

[0048] Simultaneously, a cogging effect compensation term is introduced. The fused control signals are nonlinearly corrected to generate the final control commands. And output it to the robotic arm actuator.

[0049] On the other hand, the present invention provides a robot that executes the motion control method for a robot described in any of the preceding claims, the robot comprising:

[0050] The main body of the robotic arm includes multiple joint units and joint drive devices. The joint units are connected in sequence to form a multi-degree-of-freedom structure. An adjustable clamping device is provided at the end for gripping workpieces of different shapes and materials.

[0051] The sensor unit includes an end-effector IMU, a joint accelerometer, a motor current sensor, and an RGB-D camera, used to collect information on the robot arm's posture, joint status, motor load, and environmental conditions.

[0052] The control unit is electrically connected to the robotic arm body and the sensor unit, and performs the following functions:

[0053] Identification, pose estimation, and distance measurement of target workpieces based on visual information;

[0054] Estimating end force based on multi-source sensor information and distinguishing between real contact and noise;

[0055] The visual control weight and force control weight are dynamically adjusted according to the proximity of the end effector to achieve hybrid motion control that is vision-driven for long-distance positioning and force-driven for close-range contact.

[0056] The clamping force is automatically adjusted according to the material of the workpiece to achieve anti-slip and anti-damage control during the gripping process.

[0057] On the other hand, the present invention provides a motion control system for a robot, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the motion control method for the robot described in any of the above-mentioned embodiments.

[0058] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0059] 1. The robot motion control method, robot and system of the present invention, based on the preliminary classification of visual texture and geometric features, and the results are dynamically corrected by a material correction function constructed by the motor current increment. This can reflect the actual stiffness and friction characteristics of the object and avoid visual misjudgment that leads to clamping failure.

[0060] 2. The proposed motion control method, robot, and system for a robot introduce the CHOMP incremental optimization algorithm based on sampling global planning, and fine-tune the trajectory points by combining the target surface normal. This enables the rapid generation of smooth and safe executable paths even under dynamic obstacle interference, thus solving the defects of global coarseness and local instability in traditional methods.

[0061] 3. In the motion control method, robot and system of this robot, a harmonic compensation function based on joint position, velocity and current is constructed, which effectively suppresses the periodic torque disturbance caused by the cogging effect of the servo motor, makes the end force estimation closer to the real force, and improves the stability of clamping control. Attached Figure Description

[0062] Figure 1 This is a flowchart of the overall method of the present invention. Detailed Implementation

[0063] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0064] Example 1: Please refer to Figure 1 As shown, this embodiment provides a motion control method for a robot, including the following steps:

[0065] S1. Acquire the current scene's depth information, color information, and the visual texture and geometric features of the target object using an RGB-D camera. Perform instance segmentation and six-degree-of-freedom pose estimation based on the color and depth information to obtain the target object's three-dimensional position coordinates and spatial pose parameters, which are then used to generate a local environment map.

[0066] In this embodiment, the following steps are performed on the color information and depth information to generate a local environment map:

[0067] Based on the current scene depth information (specifically, the depth map corresponding to the current scene depth information acquired by the RGB-D camera, i.e., two-dimensional image data encoding the depth (distance) of each pixel in the current scene) combined with the camera's intrinsic and extrinsic parameters (camera focal length in the horizontal and vertical directions, pixel coordinates (points in the image's two-dimensional coordinate system), the position of the intersection of the optical axis and the imaging plane in the pixel coordinate system (image center offset), normalized coordinates (normalized planar coordinates obtained by back-projecting pixel coordinates through the intrinsic parameter matrix), and the extrinsic parameter matrix; the extrinsic parameter matrix is ​​used to describe the rigid transformation relationship between the camera coordinate system and the world / robotic arm coordinate system, including rotation matrix and translation vector) calibration results, the pixel coordinates are transformed to a 3D point cloud in the camera coordinate system through back-projection, and then transformed to the robotic arm's base coordinate system through the hand-eye calibration matrix. The 3D point cloud is then preprocessed by performing bilateral filtering, outlier removal, and voxel downsampling to reduce noise and redundant data, resulting in a preprocessed 3D point cloud.

[0068] A color image is input into a YOLOv8-seg network that fuses depth channels. The network decoder and mask prediction head then output a pixel-level instance mask of the target object. Among them, the YOLOv8-seg network that integrates depth channels introduces a multi-scale convolutional fusion and depth feature enhancement module in the feature extraction stage to improve the segmentation accuracy in weak texture regions. Specifically, the depth image is used as an additional input channel and is concatenated with the RGB image at the channel level in the input layer of the Backbone network. A multi-scale convolutional structure is introduced in the Feature Pyramid Network (FPN) to achieve the fusion and enhancement of depth features and color features.

[0069] Regarding the multi-scale convolutional fusion module:

[0070] Introduce at the outputs of layers C2 and C3 of the backbone network respectively , , Three sets of parallel convolutional kernels are used to extract local edge features and global contour features from different viewpoints;

[0071] The multi-scale convolution results are concatenated along the channel dimension and input into the channel attention mechanism (SE block) to adaptively adjust the weights of features at each scale.

[0072] Generate a fused multi-scale feature map while preserving high-resolution details of the target boundary and semantic information of the overall shape;

[0073] Deep feature enhancement module:

[0074] The depth channel in the input tensor is extracted separately and fed into a three-layer convolutional encoder to extract the depth gradient, curvature and normal rate of change features respectively.

[0075] The edge map calculated using the deep gradient is fused into the color features through channel-wise addition in layers P3 and P4 of the backbone network, thereby enhancing the boundary recognition capability of weak texture regions.

[0076] The fused multi-scale feature map and deep encoded features are subjected to a gating mechanism (GatedFusion) in the fusion layer, which adaptively selects the dominance of color features or depth features based on spatial location;

[0077] The fused features are fed into the decoder and mask prediction head of the YOLOv8-seg network (a semantic segmentation / instance segmentation variant of the YOLOv8 series of object detection models, capable of simultaneously providing the location (bounding box), category (classification label), and pixel-level segmentation results of the target region in the input image) to generate a pixel-level instance mask of the target object. ;

[0078]

[0079] In the formula, Represents pixels in an image The mask value is used to determine whether a pixel belongs to the target object. The value is 0 or 1. When the value is 0, the pixel is located in the target object area. When the value is 1, the pixel is located in the background or other object area. Represents the horizontal coordinate (column index); Indicates the vertical coordinate (row index); Represents the first in the image coordinate system The horizontal and vertical coordinates of each pixel are in pixels. This represents the total number of pixels selected by the mask within the target object area (i.e., the number of non-zero mask points). The index of a pixel is used for sparse representation.

[0080] To obtain the precise spatial pose of the target object and ensure high-precision grasping by the robotic arm, pixel-level instance masks of the target object are used. Within the masked area, planar target feature points of the color image and corresponding depth map are extracted to obtain the target object's three-dimensional point set. The target three-dimensional point set is a sparse point cloud used for six-degree-of-freedom pose estimation. Even if the target surface texture is weak or there is occlusion, the accuracy and stability of the pose solution can still be guaranteed. The pose results can be used to mark the target object's position on the local environment map to assist in planning a safe movement path.

[0081] In this embodiment, within the masked area, planar target feature points are extracted from the color image and the corresponding depth map, and the target object's three-dimensional point set is obtained through backprojection using camera intrinsic parameters. :

[0082]

[0083] In the formula, Indicates the first A three-dimensional point containing spatial coordinate information. Corresponding to image coordinates Three-dimensional back-projection points in the camera coordinate system; Indicates the horizontal direction of the camera coordinate system; Indicates the vertical direction of the camera coordinate system; Indicates the depth direction of the camera coordinate system (usually referring to the direction of the camera's optical axis); Indicates the first 1 pixel Pixel-level instance mask of the target object Within the mask area, only pixels are used to generate 3D measurement points to ensure that only valid points on the surface of the target object are selected. This indicates the measurement points selected from the mask; Indicates the total number of punctuation marks;

[0084] The three-dimensional point set of the target is solved by the PnP algorithm to obtain the spatial pose of the target object in the camera coordinate system. Combined with the hand-eye calibration matrix, the six-degree-of-freedom pose estimation result of the target object in the robot arm base coordinate system is obtained.

[0085] The six-degree-of-freedom pose estimation results include the three-dimensional position coordinates, surface normal information, and rotation angle information of the target object, which serve as inputs for subsequent robotic arm path planning and grasping control.

[0086] In this embodiment, the three-dimensional point set of the measurement target is... Using the corresponding plane points as input to the PnP algorithm, the result is:

[0087]

[0088] In the formula, This represents the pixel coordinates of the corresponding 3D measurement point on the image plane (planar feature points selected within the mask); This represents the camera intrinsic parameter matrix (including focal length and principal point coordinates). This represents the PnP algorithm, a function used to solve for the pose of an object, which maps 3D points to image pixels and solves for the rotation and translation matrices. The rotation matrix of the target object in the camera coordinate system Translation matrix ;

[0089] Based on checkerboard calibration, by collecting multiple sets of robotic arm end-effector pose and camera observation data, a fixed hand-eye calibration matrix is ​​obtained using the constraint AX=XB (where A represents the relative motion matrix of the robotic arm end-effector between two different positions (obtained by reading the robotic arm encoder); B represents the relative motion matrix of the camera when corresponding to two checkerboard images (calculated by checkerboard image matching); X represents the hand-eye calibration matrix to be determined). The results from the camera coordinate system are then mapped to the robotic arm base coordinate system using the hand-eye calibration matrix.

[0090]

[0091] In the formula, This represents the hand-eye calibration matrix, used to represent the spatial relationship between the camera coordinate system and the robotic arm end effector coordinate system; The homogeneous pose matrix of the target object in the robot arm base coordinate system is represented by the six-degree-of-freedom pose of the target object in the robot arm base coordinate system, which includes the position of the target object in the robot arm base coordinate system and the orientation of the target object in the robot arm base coordinate system.

[0092] in:

[0093]

[0094]

[0095] In the formula, Let be the rotation matrix, representing the rotation relationship between the camera coordinate system and the robot arm end effector coordinate system; The translation vector represents the translation of the origin of the camera coordinate system relative to the origin of the robot arm's end effector coordinate system; This represents the homogeneous pose matrix of the object in the camera coordinate system, obtained by the PnP algorithm.

[0096] The occupancy modeling method based on voxel grid mapping discretizes the preprocessed 3D point cloud into spatial grid cells and iteratively updates the occupancy probability of each grid cell using a Bayesian occupancy probability algorithm. At the same time, the 3D pose markers of the target object are superimposed on the point cloud map to finally generate a local environment map.

[0097] The local environment map not only contains the three-dimensional position and orientation of the target object, but also the spatial distribution information of the obstacles around it, which can provide environmental perception basis for subsequent robotic arm path planning and obstacle avoidance control.

[0098] In this embodiment, the occupancy probability of each grid cell is iteratively updated using a Bayesian occupancy probability algorithm:

[0099]

[0100] In the formula, Indicates time The observation data, after being calibrated by camera intrinsic and extrinsic parameters and by hand-eye calibration, is used to preprocess a three-dimensional point cloud; This represents the time from the start time 1 to the current time. All observation sequences; Represents a grid At any moment The probability of occupancy; Indicates the current time The probability of observation; Indicates the first A voxel grid represents a discretized voxel unit in three-dimensional space; Indicates time Combining all previous observation data In the case of voxel grid The prior probability of being occupied;

[0101] Point cloud of the target object Based on the six-degree-of-freedom pose of the target object in the robot arm's base coordinate system Perform coordinate transformation and mapping to grid cells Above, and add labels to the mapped raster cells. The system is marked (to distinguish between ordinary obstacles and target objects), and records the target object's six-degree-of-freedom pose, the unit surface normal vector of the predetermined contact area, and the pose uncertainty parameters. Finally, it generates a local environment map that includes the spatial distribution of surrounding obstacles (occupying grids), the target object's three-dimensional position and attitude (six-degree-of-freedom pose), the unit surface normal vector of the target's predetermined contact area, and the pose uncertainty parameters.

[0102] Through the above steps, the three-dimensional position coordinates and spatial attitude parameters of the target object in the base coordinate system of the robotic arm can be obtained, and the unit surface normal vector and pose uncertainty parameters of the predetermined contact area of ​​the target object can be output for subsequent control strategy generation and execution.

[0103] S2. Input the visual texture and geometric features of the target object into a pre-trained convolutional neural network to determine the material category of the target object, and introduce the motor current sampling value to correct the material category of the target object. Based on the corrected material category, determine the initial setting value of the clamping force.

[0104] In this embodiment, the specific steps involved in determining the target object material type and the initial setting value of the clamping force are as follows:

[0105] Visual texture features and geometric features are used as inputs and spliced ​​together to form a fused feature;

[0106] The fused features are input into a pre-trained convolutional neural network (as a material classification model) to obtain the initial material category;

[0107] In this embodiment, the visual texture features are information such as local color histogram, edge gradient direction histogram (HOG), local binary pattern (LBP) and convolutional feature map extracted from the color image channel through the pixel-level instance mask of the target object, which are used to describe the surface texture and color distribution features of the target.

[0108] Geometric features are based on the depth map corresponding to the target object, including geometric information such as the surface normal vector, curvature, surface convexity and concavity, local point cloud density, and spatial distribution of sparse three-dimensional measurement points, which are used to characterize the shape and spatial structure features of the object.

[0109] Visual texture feature vectors With geometric eigenvectors The data is concatenated along the channel dimension to form a fused feature vector. In the formula, The dimension of the visual texture feature vector (i.e., the total number of numerical features used to describe information such as color, texture, and edges of the target object's surface). The dimension of the geometric feature vector (i.e., the total number of numerical features used to describe the geometric information of the target object, such as shape, curvature, normal vector, and sparse point cloud distribution) is represented. Before stitching, the visual texture feature vector and the geometric feature vector need to be normalized to ensure that the feature values ​​of each dimension are within the same order of magnitude.

[0110] The datasets used in the pre-trained convolutional neural networks come from:

[0111] Color images and depth point clouds were acquired using an RGB-D camera, and material categories were manually labeled. Each sample contained (a local color image fragment + corresponding point cloud geometric features) and a corresponding material category label (such as metal, plastic, glass, wood, fabric, rubber, etc.).

[0112] Supervised training was performed using manually labeled material images and point cloud datasets (input was fused features, labels were manually labeled material categories). The Adam optimizer was used, and the cross-entropy loss function was employed to minimize the difference between the predicted category and the true label, ensuring that the model could effectively distinguish different material categories. The learning rate decreased between 0.001 and 0.0001. Parameter convergence was achieved through 50 to 200 iterations of training. The trained network model can effectively distinguish various typical materials (including metals, plastics, glass, wood, fabrics, rubber, etc.).

[0113] fuse feature vectors As input to the pre-trained convolutional neural network, high-order representation features are extracted through multiple convolution and pooling operations. Then, the high-order features obtained by convolution and pooling are flattened and input into the fully connected layer to integrate global information and generate a classification feature vector. Finally, the softmax output layer is used to map the output of the fully connected layer to a material category probability distribution through the softmax function, thus obtaining the initial material category of the target object.

[0114] In the pre-trained convolutional neural network, high-order representation features are extracted through multi-layer one-dimensional convolution (Conv1D) operations, and the convolution kernel slides in the channel dimension to capture local texture-geometric correlation features.

[0115] Each convolutional layer is followed by a pooling layer (MaxPooling or AveragePooling) to reduce feature dimensionality and enhance robustness;

[0116] After each convolutional layer, a non-linear mapping is achieved through the ReLU activation function, which improves the model's ability to represent complex textures and geometric shapes.

[0117] Real-time current sampling value based on the joint motor of the robotic arm Constructing material correction functions Correct the material category of the target object;

[0118] When attempting to lightly touch the surface of the target object, based on real-time current sampling values... Calculate the motor current increment:

[0119]

[0120] In the formula, The reference current represents the current under no-load operation. It refers to the normal current value of the motor when there is no contact (no load or no track) under the same motion command. It can be obtained through offline calibration or online idle cycle estimation. It represents the current increment and is used to indicate the additional load component caused by contact. Positive values ​​usually indicate greater contact / rigidity or higher friction, while negative values ​​or near zero indicate lighter contact or no resistance. Indicates time The real-time sampling motor current is obtained when the robotic arm contacts the object surface with a small displacement / low speed, reflecting the load / friction / stiffness information at the moment of contact.

[0121]

[0122] In the formula, Indicates the material category of the target object after correction; This indicates the initial material category prediction result; This represents the material correction function used to determine the threshold mapping, specifically: if the current increment exceeds the current threshold... (Current threshold) Used to determine current increment Is it significant? Values ​​are assigned through calibration experiments, for example, the current threshold. A, specifically, current threshold The optimal boundary point for distinguishing between soft and hard materials was obtained through calibration experiments using multiple sets of different materials. This boundary point is then determined using a material correction function. Put the initial category Mapped to the same family or higher class with higher density / higher friction (if) The material category is determined by Adjust it to its parent class; otherwise, keep it. ;

[0123] Specifically, due to the different densities, hardness, and coefficients of friction of different materials, the motor current of the robotic arm will vary when grasping the same clamping force (for example, harder or more frictional materials will generate a higher motor current, while softer or less frictional materials will generate a lower current). The motor current provides actual load information during the grasping process and can serve as real-time feedback on material stiffness and frictional characteristics. The motor current is a real-time indirect feedback signal of material physical properties. Combined with visual judgment, it can improve the reliability of material classification, thereby generating a more reasonable initial setting value for clamping force.

[0124] Based on the corrected material category, retrieve a matching set of material parameters from the preset parameter library. In the formula, The coefficient of friction of the target object is used to represent the frictional characteristics between the gripper and the contact surface of the target object; Indicates the density of the target object's material; This represents the safety factor, used to ensure that the clamping force has a certain margin based on the theoretically calculated value, and is assigned a value through expert experience. This indicates the recommended clamping force range for the target object material. It is a range of clamping force values ​​given by the preset parameter library based on the material category, used to limit the lower and upper limits of the initial clamping force setting value. The maximum surface pressure of the target material represents the maximum pressure that the surface of the target material can withstand without permanent deformation or damage, and is used to constrain the force per unit area applied by the gripper.

[0125] Based on material parameter set And based on the effective contact area between the gripper and the target object. By introducing a comprehensive correction factor The final initial setting value of the clamping force is obtained. ;

[0126] Among them, the comprehensive correction factor It is obtained by constructing the clamping attitude angle and the buffer acceleration margin using the Sigmoid smoothing function;

[0127] In this embodiment, the comprehensive correction factor :

[0128]

[0129]

[0130] In the formula, The clamping posture angle represents the angle between the gripper's grasping direction and the normal to the target object's surface. When the angle is too large, the clamping stability decreases, and a correction factor needs to be added. The buffer acceleration margin represents the difference between the dynamic acceleration of the target object and the safety threshold under transportation or environmental disturbance conditions. When the dynamic disturbance is large, the clamping force needs to be increased accordingly to compensate. The Sigmoid function is used to perform nonlinear smoothing mapping on the included angle and acceleration, making the change of the correction factor continuous and bounded. ; The weighting coefficients represent the clamping posture angle; The weighting coefficients represent the buffer acceleration margin;

[0131]

[0132] In the formula, This represents the minimum value function, used to ensure that the clamping force does not exceed the maximum allowable pressure limit of the material; This represents the effective contact area between the gripper and the target object (which is derived from: the local surface morphology of the target object obtained by visual geometric feature recognition (based on the contact plane fitted by point cloud, combined with the gripper size); and the effective area of ​​the contact region calculated by the tactile sensor array or contact model during actual contact).

[0133] S3. Based on the local environment map, a sampling motion planning algorithm (typical sampling motion planning algorithm RRT*) is used to perform global sampling planning to generate an initial trajectory point sequence. Based on the obstacle information updated in real time by the sensor, the local path information is incrementally optimized using the CHOMP algorithm to obtain the updated trajectory point sequence.

[0134] In this embodiment, the specific steps involved in generating the updated trajectory point sequence are as follows:

[0135] Point cloud data of the surrounding environment is acquired in real time using sensors (such as RGB-D cameras or LiDAR), and a signed distance field is constructed by combining it with a local environment map. This is used to describe the distance relationship from any point in space to the boundary of an obstacle:

[0136]

[0137] In the formula, Indicates the area of ​​obstacles; Indicates the boundary of an obstacle; Indicates Euclidean distance;

[0138] Based on the starting point and target grasping position of the robotic arm, an initial trajectory point sequence is generated using a sampling motion planning algorithm (typically RRT*). In the formula, This represents the number of discrete trajectory points from the starting point to the ending point. The endpoint node represents the joint state or end pose of the target grasping position; This indicates the starting node, which corresponds to the initial joint state or initial end-effector pose of the robotic arm.

[0139] In this embodiment, the sampling motion planning algorithm RRT* generates an initial trajectory point sequence. The specific steps involved are as follows:

[0140] Random sampling of joint space nodes ;

[0141] Find the nearest node in the existing tree. ;

[0142] To the nearest node Expand to generate new nodes Satisfying the maximum step size limit ;

[0143] If the new node If feasible, add it to the path tree;

[0144] Repeat the iteration until the starting point and the target position are connected, forming an initial trajectory sequence;

[0145] In this embodiment, objects whose positions change significantly are detected by comparing the occupancy grid information at consecutive times, and are thus identified as dynamic obstacles.

[0146] By using motion estimation or optical flow methods, the changed portions of the locally occupied grid are labeled as a set of dynamic obstacles. In the formula, Indicates the first Each dynamic obstacle contains information on its position, velocity, and size.

[0147] Based on the initial trajectory point sequence Introducing a real-time dynamic obstacle set And target object information (including six-DOF pose, unit surface normal vector, and set of grab point positions), for the initial trajectory point sequence Perform incremental optimization:

[0148] For the initial trajectory point sequence For each trajectory point, gradient descent is performed using the minimum trajectory cost function until the minimum trajectory cost function converges, resulting in the optimized trajectory. In the formula, This is the optimized endpoint node; This represents the optimized starting node;

[0149] Wherein, the trajectory cost function is minimized:

[0150]

[0151] In the formula, The trajectory cost function represents the current trajectory. The overall cost is considered, and a smaller cost indicates a smoother trajectory and better obstacle avoidance performance. The trajectory function represents the trajectory of a robotic arm's end effector or joint over time. Spatial position or joint angle sequence; Indicates the trajectory parameterization time, used to continuously represent the changes of trajectory points over time; The acceleration (or curvature) of the trajectory is represented by the smoothing term, which ensures the smoothness of the trajectory by minimizing the square of the trajectory acceleration. Let be the obstacle cost function, representing the penalty value when the trajectory approaches an obstacle in space, where: This represents the SDF value generated based on the occupied grid in the local environment map; This represents a function that maps SDF values ​​to cost values; for example, the closer the distance, the higher the cost. This represents the weighting coefficient, used to balance the importance of smoothness and obstacle avoidance constraints;

[0152] Iterative updates are performed using gradient descent:

[0153]

[0154] In the formula, This represents the learning rate, used to control the step size of each iteration update; Indicates the iteration number The sequence of trajectory points; Indicates the iteration number The updated sequence of trajectory points; The gradient of the trajectory cost function with respect to the trajectory represents the direction of improvement of the current trajectory in terms of smoothness and obstacle avoidance.

[0155] Iterate until the cost function converges, that is, until the smoothness and obstacle avoidance constraints reach an optimal balance, to obtain the optimized trajectory. ;

[0156] During the iteration process, a neighborhood search algorithm is used to calculate the value of each trajectory point. With dynamic obstacle points European distance ;

[0157] If Euclidean distance If the distance is less than or equal to the safety distance threshold (the safety distance threshold is the minimum safe distance between the end effector of the robotic arm or the trajectory point and the obstacle, and the safety distance threshold is set based on a comprehensive consideration of the maximum possible range of motion of the dynamic obstacle, the maximum possible range of motion of the dynamic obstacle, and control accuracy and safety margin), then the trajectory point is determined to be... Conflict with obstacles will disrupt the entire initial trajectory point sequence. The set of all points that satisfy this condition is taken as the set of conflict trajectory points;

[0158] Using the target surface normal vector to track points close to the target object After local fine-tuning, the updated trajectory point sequence is finally output. In the formula, Indicates the first in the sequence Each trajectory point contains the robot arm joint angles or end-effector pose information at that point (a six-degree-of-freedom pose, including position and orientation rotation matrices or Euler angles). This represents the final updated sequence of trajectory points, which includes the robotic arm's executable trajectory after obstacle avoidance optimization and smoothing adjustments, combined with fine-tuning of the target object's surface normal.

[0159] S4. Based on the real-time distance between the end effector and the surface of the target object (output results of steps S1-S3), the nearest neighbor search algorithm is used to calculate the target proximity, and the force control weight and position control weight are adjusted based on the target proximity.

[0160] In this embodiment, the specific steps involved in adjusting the force control weight and position control weight based on target proximity are as follows:

[0161] Target proximity is defined based on the minimum distance between the end effector and the target surface. ,in, This indicates that the end point is far from the target. This indicates that the end is close to the target surface;

[0162]

[0163] In the formula, Indicates the reference distance from the target; Indicates the minimum distance between the end point and the target surface; Indicates the safe distance at which contact is permitted;

[0164] Using the Sigmoid mapping function, the target proximity is... Mapped to position control weights respectively and force control weight (When the end effector is far from the target, higher position following accuracy is required (high position control weight); when the end effector is close to the target, rigidity needs to be reduced (low position control weight) in order to make compliant contact; and when the end effector is close to the target, force control weight needs to be increased to ensure compliance and safety during contact.)

[0165]

[0166] In the formula, This indicates the force control weight of the end effector, used to control the stiffness of the force applied by the robotic arm when it approaches a target object; This represents the minimum value of the force control weight; This represents the maximum value of the force control weight; Indicates target proximity, used to quantify the real-time distance between the end effector and the surface of the target object; This represents the steepness parameter of the Sigmoid curve, used to control the rate of change of the nonlinear mapping. The larger the value, the greater the weight. The steeper the change, the gentler the change; the smaller the value, the smoother the change. This represents the midpoint of the Sigmoid curve, corresponding to the inflection point of the mapping function;

[0167]

[0168] In the formula, This indicates the position control weight of the end effector, used to control the priority or stiffness of the robotic arm following the trajectory; This represents the maximum value of the position control weight; This represents the minimum value of the position control weight.

[0169] S5. Based on the state estimation algorithm, multi-source signals are fused to calculate the estimated values ​​of joint torque and end force. The estimated values ​​of joint torque are corrected by introducing a tooth cogging effect compensation function. The corrected estimated values ​​of joint torque are converted into the corrected values ​​of end force by using the Jacobian matrix of the robotic arm.

[0170] Among them, the multi-source signals include the current of the robotic arm joint drive motor, joint acceleration, end effector IMU acceleration and angular velocity signals;

[0171] In this embodiment, the specific steps involved in correcting the joint moment estimate and converting the corrected joint moment estimate into an end-effector force correction value are as follows:

[0172] Based on the state estimation algorithm (extended Kalman filter algorithm) and the fusion of multi-source signals, the estimated values ​​of initial joint torque and initial end force are calculated;

[0173] For each joint By introducing a cogging effect compensation function The cogging effect compensation term was calculated. Used to reduce low-speed force estimation errors caused by cogging effect;

[0174] Compensation term based on cogging effect The initial joint moment estimate is corrected to obtain the corrected joint moment value;

[0175] The Jacobian matrix of the robotic arm is used to map the joint torque correction values ​​to the end effector force correction values;

[0176] Among them, the tooth cogging effect compensation function The compensation function is constructed based on joint position, joint velocity, and joint current.

[0177] In this embodiment, the input vector joint position, joint velocity, and joint current are first normalized (mapped to [-1,1]).

[0178] At the same time, the current and speed signals are low-pass filtered to remove high-frequency noise and avoid compensation amplification noise;

[0179] Among them, the tooth cogging effect compensation term :

[0180]

[0181] In the formula, Indicates the joint position (angle), indicating the first... The rotation angle of each joint; Represents joint velocity, and represents the first joint velocity. Angular velocity of each joint (indicated by positive and negative signs to show the direction of rotation); This represents the joint current, which reflects the magnitude of the actual output torque of the motor (used for state estimation or compensation factor). Indicates the first Alveolar effect compensation term for each joint;

[0182] Tooth cogging effect compensation function Specifically:

[0183]

[0184] In the formula, Let be the amplitude parameter of the cogging effect compensation term, representing the first... The amplitude of the subharmonic depends on the current. ; The spatial frequency parameter representing the cogging effect, specifically... ,in, Indicates the number of stator slots. Represents the extreme logarithm; This represents the joint velocity-related compensation parameters; The harmonic order represents the number of harmonics compensated by the cogging effect. For phase shift, indicating the first The initial phase angle of the subharmonic; Represents harmonic index variables ( As the fundamental frequency, (Higher harmonics); This is a friction direction function used to solve zero-speed vibration. ,in, Represents the hyperbolic tangent function, with a range of values. ; Represents the smoothing factor. ; is the coefficient of friction, and represents the Coulomb friction amplitude;

[0185] The joint torque correction value is:

[0186]

[0187] In the formula, Indicates the first Each joint torque correction value is the result after adding cogging effect compensation to the original estimate; Indicates the first Initial joint torques of each joint;

[0188] Map the joint torque correction value to the end-effector force correction value:

[0189]

[0190] In the formula, For the robotic arm at the joint position The Jacobian matrix at the point describes the mapping relationship between joint velocity and end effector velocity; This represents the inverse of the Jacobian matrix, used to map the torques in joint space to the end forces in Cartesian space. This is the end force correction value, representing the end force vector calculated after compensation; This represents the joint torque vector consisting of all joint correction torques;

[0191] The cogging effect can correct the joint torque estimate by introducing a cogging effect compensation function when the robotic arm moves at low speed or the joint moves with small amplitude, making the end force estimate more accurate and stable.

[0192] S6. Based on the force control weight and position control weight, calculate the position control output and impedance force control output respectively, and generate the final control command by combining the initial setting value of clamping force and the tooth cogging compensation term.

[0193] In this embodiment, the specific steps involved in generating the final control command are as follows:

[0194] Based on the expected position of the planned trajectory points and the actual position of the robotic arm's end effector, a trajectory following algorithm is adopted, and position control weights are introduced. The position control output is calculated. ;

[0195]

[0196] In the formula, This represents the joint torque vector of the position control output; This represents the position control weight (obtained from step S4 based on target proximity); The joint position proportional gain matrix (diagonal matrix) is used to adjust the feedback strength of the position error; This represents the differential gain matrix of the joint velocity (diagonal matrix), used to adjust the feedback damping of the velocity error; This represents the desired joint position vector of the planned trajectory points (the sequence of trajectory points obtained from step S3). supply; This represents the actual joint position vector of the robotic arm (provided in real time by joint position sensors or encoders). This represents the expected joint velocity vector of the planned trajectory point; This represents the actual joint velocity vector of the robotic arm (obtained by encoder differential or velocity sensor). Indicates the number of joints in the robotic arm;

[0197] Based on end force correction value and force control weight Generate impedance control output ;

[0198]

[0199] In the formula, This represents the joint torque vector of the impedance control output; This represents the force control weight (obtained from step S4 based on target proximity); Indicates the position of the robotic arm at the joint. The Jacobian matrix at the point is used to establish the mapping relationship between the end force and the joint torque; This represents the end force correction value (from step S5, the end force estimation result obtained based on multi-source signal fusion and tooth cogging compensation). This represents the desired end force vector, defined by the task. The force error gain matrix (usually a diagonal matrix) is used to adjust the deviation between the desired force and the corrected force.

[0200] Position control output With impedance control output The process involves fusion and the introduction of an initial clamping force setting. Amplitude constraints are applied to the end force component of the fused output;

[0201] Simultaneously, a cogging effect compensation term is introduced. The fused control signal is nonlinearly corrected to reduce torque estimation errors during low-speed joint movement, and the final control command is generated. And output it to the robotic arm actuator.

[0202] In this embodiment, the generated final control command is:

[0203]

[0204] In the formula, This is the clamping force vector in the end effector coordinate system (z-axis is the gripping direction); This indicates the initial setting value of the clamping force (obtained from step S2); This represents the cogging effect compensation term (obtained from step S5); Indicates the clamping force mapping term;

[0205] And the final control command Torque command vectors are sent directly to the actuators of each joint of the robotic arm to achieve trajectory tracking (through... (Drive the robotic arm to move along the planned trajectory), compliant control (through...) Achieving precise control of contact force and grasping operation (through...) Applying initial clamping force) and motor compensation (by Unified implementation of eliminating motor cogging effect error.

[0206] As another preferred embodiment of the present invention, while the robotic arm executes control commands, the system collects the status information of the robotic arm end effector and joints in real time, including end effector position, end effector force, joint angle and joint torque, etc.

[0207] Simultaneously construct a combined state vector containing position error and force error, and define a Lyapunov function; combine the proximity parameter and the end force error (the deviation between the actual force on the end of the robotic arm and the expected / target force) to calculate new force control weights and position weights, return the new weight values ​​to step S4 to overwrite the original weights, continue to generate hybrid control output with the new weights, and repeat the above operation in the next cycle;

[0208] Repeat the above steps until the robotic arm completes the task of grasping and placing the target object; when the task is terminated, reset the end effector and gripper to the initial position and clear the control command buffer.

[0209] Example 2: This example provides a robot that executes the motion control method for a robot described in any of the above embodiments. The robot includes:

[0210] The main body of the robotic arm includes multiple joint units and joint drive devices. The joint units are connected in sequence to form a multi-degree-of-freedom structure. An adjustable clamping device is provided at the end for gripping workpieces of different shapes and materials.

[0211] The sensor unit includes an end-effector IMU, a joint accelerometer, a motor current sensor, and an RGB-D camera, used to collect information on the robot arm's posture, joint status, motor load, and environmental conditions.

[0212] The control unit is electrically connected to the robotic arm body and the sensor unit, and performs the following functions:

[0213] Identification, pose estimation, and distance measurement of target workpieces based on visual information;

[0214] Estimating end force based on multi-source sensor information and distinguishing between real contact and noise;

[0215] The visual control weight and force control weight are dynamically adjusted according to the proximity of the end effector to achieve hybrid motion control that is vision-driven for long-distance positioning and force-driven for close-range contact.

[0216] The clamping force is automatically adjusted according to the material of the workpiece to achieve anti-slip and anti-damage control during the gripping process.

[0217] Example 3: This example provides a motion control system for a robot, including a memory, a processor, and a computer program stored in the memory and executable on the processor. The processor executes the computer program to implement the motion control method for the robot described in any of the above examples.

[0218] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely preferred examples and are not intended to limit the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A motion control method for a robot, characterized in that, Includes the following steps: S1. Acquire the current scene's depth information, color information, and the visual texture and geometric features of the target object using an RGB-D camera. Perform instance segmentation and six-degree-of-freedom pose estimation based on the color and depth information to obtain the target object's three-dimensional position coordinates and spatial pose parameters, which are then used to generate a local environment map. S2. Input the visual texture and geometric features of the target object into a pre-trained convolutional neural network to determine the material category of the target object, and introduce the motor current sampling value to correct the material category of the target object. Based on the corrected material category, determine the initial setting value of the clamping force. S3. Based on the local environment map, a sampling motion planning algorithm is used to perform global sampling planning to generate an initial trajectory point sequence. Based on the obstacle information updated in real time by the sensor, the local path information is incrementally optimized using the CHOMP algorithm to obtain the updated trajectory point sequence. S4. Based on the real-time distance between the end effector and the surface of the target object, the nearest neighbor search algorithm is used to calculate the target proximity, and the force control weight and position control weight are adjusted based on the target proximity. S5. Based on the state estimation algorithm, multi-source signals are fused to calculate the estimated values ​​of joint torque and end force. The estimated values ​​of joint torque are corrected by introducing a tooth cogging effect compensation function. The corrected estimated values ​​of joint torque are converted into the corrected values ​​of end force by using the Jacobian matrix of the robotic arm. Among them, the multi-source signals include the current of the robotic arm joint drive motor, joint acceleration, end effector IMU acceleration and angular velocity signals; S6. Based on the force control weight and position control weight, calculate the position control output and impedance force control output respectively, and generate the final control command by combining the initial setting value of clamping force and the tooth cogging compensation term.

2. The motion control method for a robot according to claim 1, characterized in that: In step S1, the following steps are performed on the color information and depth information to generate a local environment map: Based on the current scene depth information and the camera intrinsic and extrinsic parameter calibration results, the pixel coordinates are transformed into a 3D point cloud in the camera coordinate system through back projection, and then transformed into the robot arm base coordinate system through the hand-eye calibration matrix. The 3D point cloud is then preprocessed to obtain a preprocessed 3D point cloud. A color image is input into a YOLOv8-seg network that fuses depth channels. The network decoder and mask prediction head then output a pixel-level instance mask of the target object. ; Pixel-level instance mask based on target object Within the masked area, planar target feature points of the color image and corresponding depth map are extracted to obtain the target object's three-dimensional target point set; The three-dimensional point set of the target is solved by the PnP algorithm to obtain the spatial pose of the target object in the camera coordinate system. Combined with the hand-eye calibration matrix, the six-degree-of-freedom pose estimation result of the target object in the robot arm base coordinate system is obtained. The six-degree-of-freedom pose estimation results include the three-dimensional position coordinates of the target object, surface normal information, and rotation angle information. The occupancy modeling method based on voxel grid mapping discretizes the preprocessed 3D point cloud into spatial grid cells and iteratively updates the occupancy probability of each grid cell using a Bayesian occupancy probability algorithm. At the same time, the 3D pose markers of the target object are superimposed on the point cloud map to finally generate a local environment map.

3. The motion control method for a robot according to claim 1, characterized in that: In step S2, the specific steps involved in determining the target object material type and the initial setting value of the clamping force are as follows: Visual texture features and geometric features are used as inputs and spliced ​​together to form a fused feature; The fused features are input into a pre-trained convolutional neural network to obtain the initial material category; Real-time current sampling value based on the joint motor of the robotic arm Constructing material correction functions Correct the material category of the target object; Based on the corrected material category, retrieve a matching set of material parameters from the preset parameter library. In the formula, The coefficient of friction of the target object; Indicates the density of the target object's material; Indicates the safety factor; Indicates the recommended clamping force range for the target object material; The maximum surface pressure of the target object's material; Based on material parameter set And based on the effective contact area between the gripper and the target object. By introducing a comprehensive correction factor The final initial setting value of the clamping force is obtained. ; Among them, the comprehensive correction factor It is obtained by constructing the clamping attitude angle and the buffer acceleration margin through the Sigmoid smoothing function.

4. The motion control method for a robot according to claim 1, characterized in that: In step S3, the specific steps involved in generating the updated trajectory point sequence are as follows: The system collects point cloud data of the surrounding environment in real time using sensors and constructs a signed distance field by combining it with a local environment map. Based on the starting point and target grasping position of the robotic arm, a sampling motion planning algorithm is used to generate an initial trajectory point sequence. In the formula, This represents the number of discrete trajectory points from the starting point to the ending point. The endpoint node; Indicates the starting node; Based on the initial trajectory point sequence Introducing a real-time dynamic obstacle set Information about the target object, and the initial trajectory point sequence. Perform incremental optimization: For the initial trajectory point sequence For each trajectory point, gradient descent is performed using the minimum trajectory cost function until the minimum trajectory cost function converges, resulting in the optimized trajectory. In the formula, This is the optimized endpoint node; This represents the optimized starting node; During the iteration process, a neighborhood search algorithm is used to calculate the value of each trajectory point. With dynamic obstacle points European distance ; If Euclidean distance If the distance is less than or equal to the safe distance threshold, then the trajectory point is determined. Conflict with obstacles will disrupt the entire initial trajectory point sequence. The set of all points that satisfy this condition is taken as the set of conflict trajectory points; Using the target surface normal vector to track points close to the target object After local fine-tuning, the updated trajectory point sequence is finally output. .

5. The motion control method for a robot according to claim 3, characterized in that: In step S4, the specific steps involved in adjusting the force control weight and position control weight based on target proximity are as follows: Target proximity is defined based on the minimum distance between the end effector and the target surface. ,in, This indicates that the end point is far from the target. This indicates that the end is close to the target surface; Using the Sigmoid mapping function, the target proximity is... Mapped to position control weights respectively and force control weight .

6. The motion control method for a robot according to claim 5, characterized in that: In step S5, the specific steps involved in correcting the joint moment estimate and converting the corrected joint moment estimate into an end-effector force correction value are as follows: Based on the state estimation algorithm, multi-source signals are fused to calculate the estimated values ​​of initial joint torque and initial end force. For each joint By introducing a cogging effect compensation function The cogging effect compensation term was calculated. ; Compensation term based on cogging effect The initial joint moment estimate is corrected to obtain the corrected joint moment value; The Jacobian matrix of the robotic arm is used to map the joint torque correction values ​​to the end effector force correction values; Among them, the tooth cogging effect compensation function This is a compensation function constructed based on joint position, joint velocity, and joint current.

7. The motion control method for a robot according to claim 6, characterized in that: In step S6, the specific steps involved in generating the final control command are as follows: Based on the expected position of the planned trajectory points and the actual position of the robotic arm's end effector, a trajectory following algorithm is adopted, and position control weights are introduced. The position control output is calculated. ; Based on end force correction value and force control weight Generate impedance control output ; Position control output With impedance control output The process involves fusion and the introduction of an initial clamping force setting. Amplitude constraints are applied to the end force component of the fused output; Simultaneously, a cogging effect compensation term is introduced. The fused control signals are nonlinearly corrected to generate the final control commands. And output it to the robotic arm actuator.

8. A robot, said robot performing a motion control method for a robot as described in any one of claims 2-7, characterized in that: The robot includes: The main body of the robotic arm includes multiple joint units and joint drive devices. The joint units are connected in sequence to form a multi-degree-of-freedom structure. An adjustable clamping device is provided at the end for gripping workpieces of different shapes and materials. The sensor unit includes an end-effector IMU, a joint accelerometer, a motor current sensor, and an RGB-D camera, used to collect information on the robot arm's posture, joint status, motor load, and environmental conditions. The control unit is electrically connected to the robotic arm body and the sensor unit, and performs the following functions: Identification, pose estimation, and distance measurement of target workpieces based on visual information; Estimating end force based on multi-source sensor information and distinguishing between real contact and noise; The visual control weight and force control weight are dynamically adjusted according to the proximity of the end effector to achieve hybrid motion control that is vision-driven for long-distance positioning and force-driven for close-range contact. The clamping force is automatically adjusted according to the material of the workpiece to achieve anti-slip and anti-damage control during the gripping process.

9. A motion control system for a robot, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: The processor executes a computer program to implement the motion control method for the robot as described in any one of claims 2-7.

Images