Motion control method of robot, robot and system

Abstract

The invention relates to the technical field of robot control, in particular to a robot motion control method, a robot and a system. The method comprises the following steps: acquiring depth information and color information of a current scene and visual textures and geometric features of a target object through an RGB-D camera, and generating a local environment map; introducing a motor current sampling value, correcting the material category of the target object, and determining an initial set value of the clamping force; the corrected joint torque estimated value is converted into a tail end force corrected value through a mechanical arm jacobian matrix; and on the basis of the force control weight and the position control weight, position control output and impedance force control output are calculated respectively, and a final control instruction is generated in combination with the clamping force initial set value and the tooth space compensation item. A harmonic compensation function based on the joint position, the speed and the current is constructed, periodic torque disturbance caused by the cogging effect of the servo motor is effectively restrained, the tail end force estimation is closer to the real acting force, and the clamping control stability is improved.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a robot motion control method, a robot and a system. Background Art

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

[0003] However, traditional methods often rely on visual features or shallow machine learning classification models to identify object materials, but these methods fail to reflect the true contact mechanical properties. For example, plastic and glass have similar texture characteristics under illumination, and simple visual recognition can easily lead to misjudgment, resulting in a mismatch between the clamping force setting and the actual material, causing unstable clamping or even crushing. Therefore, a robot motion control method, robot, and system are urgently needed. Summary of the Invention

[0004] The present invention aims to provide a robot motion control method, robot, and system to address the problem of traditional methods proposed in the aforementioned background art, which primarily 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, plastic and glass have similar texture characteristics under illumination, and simple visual recognition can easily lead to misjudgment, resulting in a mismatch between the clamping force setting and the actual material, causing unstable clamping or even crushing.

[0005] To achieve the above object, the present invention provides a robot motion control method, comprising the following steps:

[0006] S1. Acquire the current scene depth information, color information, and visual texture and geometric features of the target object through an RGB-D camera, perform instance segmentation and six-degree-of-freedom pose estimation based on the color information and depth information, and obtain the three-dimensional position coordinates and spatial pose parameters of the target object for generating 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. The motor current sampling value is introduced to correct the material category of the target object. The initial setting value of the clamping force is determined based on the corrected material category.

[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 based on the CHOMP algorithm to obtain an updated trajectory point sequence.

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

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

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

[0012] S6. Based on the force control weight and the position control weight, the position control output and the impedance force control output are calculated respectively, and the final control instruction is generated by combining the initial setting value of the clamping force and the tooth gap compensation item.

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

[0014] Based on the current scene depth information and the camera internal and external parameter calibration results, the pixel coordinates are converted to a three-dimensional point cloud in the camera coordinate system by back projection, and then converted to the robot base coordinate system through the hand-eye calibration matrix, and the three-dimensional point cloud is preprocessed to obtain a preprocessed three-dimensional point cloud;

[0015] The color image is input into the YOLOv8-seg network that integrates the depth channel, and the pixel-level instance mask of the target object is output through the network decoder and mask prediction head. ;

[0016] Pixel-level instance mask based on target objects , extract the plane measurement feature points of the color image and the corresponding depth map in the mask area to obtain the measurement three-dimensional point set of the target object;

[0017] The three-dimensional point set of the measured mark 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 result includes the three-dimensional position coordinates, surface normal information and rotation angle information of the target object;

[0019] The occupancy modeling method based on voxel grid mapping discretizes the preprocessed 3D point cloud into spatial grid units, and iteratively updates the occupancy probability of each grid through the 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 of the present technical solution, in S2, the specific steps involved in determining the material type of the target object and the initial setting value of the clamping force are:

[0021] Take visual texture features and geometric features as input and concatenate them to form fusion features;

[0022] Input the fused features into the pre-trained convolutional neural network to obtain the initial material category;

[0023] Real-time current sampling value based on the motor of the robot arm joint , construct material correction function , correct the material category of the target object;

[0024] According to the corrected material category, the matching material parameter set is retrieved from the preset parameter library , where is the friction coefficient of the target object; Indicates the material density of the target object; represents the safety factor; Indicates the recommended clamping force range for the target object material; is the maximum surface pressure of the target object material;

[0025] Based on material parameter sets , and based on the effective contact area between the gripper and the target object , by introducing the comprehensive correction factor , get the final initial setting value of the clamping force ;

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

[0027] As a further improvement of the present technical solution, in S3, the specific steps involved in generating the updated trajectory point sequence are:

[0028] Based on the real-time collection of point cloud data of the surrounding environment by sensors, a signed distance field is constructed in combination with the local environment map;

[0029] According to the starting point and target grasping position of the robot arm, a sampling motion planning algorithm is used to generate the initial trajectory point sequence , where represents the number of discrete trajectory points from the starting point to the end point, is the terminal node; Indicates the starting point node;

[0030] Based on the initial trajectory point sequence , introducing real-time dynamic obstacle sets and target object information, for the initial trajectory point sequence Perform incremental optimization:

[0031] For the initial trajectory point sequence For each trajectory point, the gradient descent is performed by minimizing the trajectory cost function until the minimization trajectory cost function is iterated to convergence, and the optimized trajectory is obtained. , where is the optimized terminal node; Indicates the optimized starting point node;

[0032] In the iterative process, the neighborhood search algorithm is used to calculate the With dynamic obstacle points Euclidean distance ;

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

[0034] Use the target surface normal vector to track the trajectory points close to the target object Perform local fine-tuning and finally output the updated trajectory point sequence .

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

[0036] Define target proximity based on the minimum distance between the end effector and the target surface ,in, Indicates that the end is far away from the target, Indicates that the end is close to the target surface;

[0037] Use Sigmoid mapping function to transform the target proximity Mapped to position control weights respectively and force control weight .

[0038] As a further improvement of the present technical solution, in S5, the specific steps involved in correcting the joint torque estimate and converting the corrected joint torque estimate into the terminal force correction value are:

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

[0040] For each joint , by introducing the cogging effect compensation function , calculate the cogging effect compensation term ;

[0041] Through the cogging effect compensation term Correcting the initial joint torque estimate to obtain a joint torque correction value;

[0042] The robot arm Jacobian matrix is ​​used to map the joint torque correction value to the end force correction value;

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

[0044] As a further improvement of the present technical solution, in S6, the specific steps involved in generating the final control instruction are:

[0045] Based on the expected position of the planned trajectory point and the actual position of the end of the robotic arm, a trajectory following algorithm is used and position control weights are introduced. , calculate the position control output ;

[0046] Based on the end force correction value and force control weight , generating impedance control output ;

[0047] Position control output With impedance control output Fusion is performed and the initial setting value of the clamping force is introduced Apply amplitude constraints to the terminal force component of the fusion output;

[0048] At the same time, the cogging effect compensation term is introduced , perform nonlinear correction on the fused control signal to generate the final control instruction , and output to the robotic arm actuator.

[0049] In another aspect, the present invention provides a robot that performs any one of the above-mentioned robot motion control methods, the robot comprising:

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

[0051] The sensor unit includes an end-user IMU, joint accelerometers, motor current sensors, and an RGB-D camera to collect information about the robot's posture, joint status, motor load, and environment.

[0052] A control unit is electrically connected to the robot arm body and the sensor unit and performs the following functions:

[0053] Identify, estimate the position and distance of target workpieces based on visual information;

[0054] Estimate end-force based on multi-source sensing information and distinguish true contact from noise;

[0055] Dynamically adjust the visual control weight and force control weight according to the end proximity, and realize hybrid motion control with vision leading long-distance positioning and force control leading close-range contact;

[0056] Automatically adjust the clamping force according to the workpiece material to achieve anti-slip and anti-breakage control during the grasping process.

[0057] On the other hand, the present invention provides a robot motion control system, comprising 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 any one of the above-described robot motion control methods.

[0058] Compared with the prior art, the present invention has the following beneficial effects:

[0059] 1. A robot motion control method, robot, and system, based on preliminary classification of visual texture and geometric features, and dynamically correcting the results through a material correction function constructed through motor current increments, can reflect the actual stiffness and friction characteristics of the object, avoiding visual misjudgment and the problem of clamping failure.

[0060] 2. In this robot motion control method, robot and system, based on sampling global planning, the CHOMP incremental optimization algorithm is introduced, and the trajectory points are fine-tuned in combination with the target surface normal, so that a smooth and safe executable path can be quickly generated under the interference of dynamic obstacles, solving the defects of global roughness and local instability of traditional methods.

[0061] 3. In the motion control method, robot and system of this robot, a harmonic compensation function based on joint position, speed and current is constructed, which effectively suppresses the periodic torque disturbance caused by the slot effect of the servo motor, makes the end force estimation closer to the actual force, and improves the stability of the clamping control. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 The figure is a flow chart of the overall method of the present invention. DETAILED DESCRIPTION

[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 robot motion control method, comprising the following steps:

[0065] S1. Acquire the current scene depth information, color information, and visual texture and geometric features of the target object through an RGB-D camera, perform instance segmentation and six-degree-of-freedom pose estimation based on the color information and depth information, and obtain the three-dimensional position coordinates and spatial pose parameters of the target object for generating a local environment map;

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

[0067] Based on the calibration results of the current scene depth information (specifically, the depth map corresponding to the current scene depth information obtained by the RGB-D camera, that is, the two-dimensional image data that encodes the depth (distance) of each pixel in the current scene), combined with the camera's internal and external parameters (the camera's 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 plane coordinates obtained by back-projecting the pixel coordinates via 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 the rotation matrix and translation vector), the pixel coordinates are converted to a three-dimensional point cloud in the camera coordinate system through back-projection, and then converted to the robotic arm base coordinate system via the hand-eye calibration matrix. The three-dimensional point cloud is then preprocessed by performing bilateral filtering, outlier removal, and voxel downsampling on the point cloud to reduce noise and redundant data, thereby obtaining a preprocessed three-dimensional point cloud.

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

[0069] Among them, regarding the multi-scale convolution fusion module:

[0070] Introduced at the output of the C2 and C3 layers of the backbone network 、 、 Three sets of parallel convolution kernels are used to extract local edge features and global contour features under different fields of view;

[0071] The multi-scale convolution results are spliced ​​in 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 the high-resolution details of the object boundary and the semantic information of the overall shape;

[0073] Deep feature enhancement module:

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

[0075] The edge map calculated using the depth gradient is fused into the color features through channel-by-channel addition in the P3 and P4 layers of the backbone network to enhance the boundary recognition ability of weak texture areas.

[0076] The fused multi-scale feature map and the depth encoding feature are gated at the fusion layer to adaptively select the dominance of color features or depth features based on spatial position.

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

[0078]

[0079] Where, Represents pixels on the 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, and when the value is 1, the pixel is located in the background or other object area. Represents the horizontal coordinate (column index); Represents the vertical coordinate (row index); Indicates the image coordinate system The horizontal and vertical coordinates of the pixel point, the unit is pixel; Represents the total number of pixels selected by the mask in the target object area (i.e., the number of non-zero mask points); Represents the index of the pixel point, used for sparse representation enumeration;

[0080] In order to obtain the accurate spatial pose of the target object and ensure that the robotic arm can grasp it with high precision, a pixel-level instance mask based on the target object is used. , extract the planar target feature points of the color image and the corresponding depth map within the mask area to obtain the target object's three-dimensional point set; among them, the three-dimensional point set of the target is a sparse point cloud, which is 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 result can be used to mark the target object position on the local environment map to assist in planning a safe movement path;

[0081] In this embodiment, within the mask area, the plane measurement feature points are extracted from the color image and the corresponding depth map, and the three-dimensional point set of the target object is obtained by back-projection of the camera intrinsic parameters. :

[0082]

[0083] Where, Indicates the 3D points, including spatial coordinate information , corresponding to the image coordinates 3D back-projected point in camera coordinate system; Indicates the horizontal direction of the camera coordinate system; Indicates the vertical direction of the camera coordinate system; Represents the depth direction of the camera coordinate system (usually refers to the direction of the camera optical axis); Indicates the pixels Pixel-level instance mask located at the target object Only the pixels within the mask area will be used to generate 3D measurement points, ensuring that only valid points on the surface of the target object are selected. Indicates the survey points selected from the mask; Indicates the total number of lateral punctuation points;

[0084] The three-dimensional point set of the measured mark 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 result includes the three-dimensional position coordinates, surface normal information, and rotation angle information of the target object, which serves as input for subsequent robot arm path planning and grasping control;

[0086] In this embodiment, the three-dimensional point set The corresponding plane points are used as inputs of the PnP algorithm and the calculation results are:

[0087]

[0088] Where, Represents the pixel coordinates of the corresponding three-dimensional point on the image plane (the plane feature point selected in the mask); Represents the camera's intrinsic parameter matrix (including focal length and principal point coordinates); Represents the PnP algorithm, a function used to solve the object pose, map 3D points to image pixels, and solve the rotation and translation matrix; Represents the rotation matrix of the target object in the camera coordinate system and translation matrices ;

[0089] Based on the checkerboard calibration, by collecting multiple sets of robot end-arm poses and camera observation data, a fixed hand-eye calibration matrix is ​​obtained by solving the AX=XB constraint (where A represents the relative motion matrix between two different positions of the robot end (obtained by the robot arm encoder readings); B represents the relative motion matrix of the camera when it captures the checkerboard twice (calculated by checkerboard image matching); X represents the hand-eye calibration matrix to be obtained. Combined with the hand-eye calibration matrix, the result of the camera coordinate system is mapped to the robot arm base coordinate system:

[0090]

[0091] Where, Represents the hand-eye calibration matrix, which is used to represent the spatial relationship between the camera coordinate system and the robot end coordinate system; Represents the homogeneous pose matrix of the target object in the manipulator base coordinate system, that is, the six-degree-of-freedom pose of the target object in the manipulator base coordinate system, including the position of the target object in the manipulator base coordinate system and the orientation of the target object in the manipulator base coordinate system;

[0092] in:

[0093]

[0094]

[0095] Where, is the rotation matrix, which represents the rotation relationship between the camera coordinate system and the robot end coordinate system; is the translation vector, which represents the translation of the origin of the camera coordinate system relative to the origin of the robot arm's end coordinate system; 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. Simultaneously, the 3D pose markers of the target object are superimposed on the point cloud map to ultimately generate a local environment map.

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

[0098] In this embodiment, the Bayesian occupancy probability algorithm is used to iteratively update the occupancy probability of each grid:

[0099]

[0100] Where, Indicates time The observation data is pre-processed into a 3D point cloud after camera internal and external parameter calibration and hand-eye calibration; Represents the time from the starting time 1 to the current time All observation sequences of Represents a grid At the moment The probability of occupation; Indicates the current time The probability of observation; Indicates the 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 Prior probability of being occupied;

[0101] The target object point cloud The six-degree-of-freedom pose of the target object in the robot arm base coordinate system Perform coordinate transformation and map to grid cells and add labels to the mapped grid cells Marking (used to distinguish common obstacles from target objects), while recording the target object's six-degree-of-freedom pose, the unit surface normal vector of the predetermined contact area, and the pose uncertainty parameters, and finally generating a local environment map that includes the spatial distribution of surrounding obstacles (occupied grid), the target object's three-dimensional position and pose (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 posture parameters of the target object in the robot arm base coordinate system can be obtained, and the unit surface normal vector and posture uncertainty parameters of the predetermined contact area of ​​the target object can be output for the subsequent generation and execution of the control strategy.

[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. The motor current sampling value is introduced to correct the material category of the target object. The initial setting value of the clamping force is determined based on the corrected material category.

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

[0105] Take visual texture features and geometric features as input and concatenate them to form fusion features;

[0106] Input the fused features into the 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 extracted from the color image channel through the pixel-level instance mask of the target object, including local color histogram, edge gradient orientation histogram (HOG), local binary pattern (LBP) and convolution feature map, etc., to describe the surface texture and color distribution characteristics of the target object.

[0108] Geometric features are based on the depth map corresponding to the target object, including geometric information such as the target object's 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 object's shape and spatial structure characteristics;

[0109] The visual texture feature vector and geometric eigenvectors Splicing is performed on the channel dimension to form a fusion feature vector , where Represents the dimension of the visual texture feature vector (i.e., the total number of numerical features used to describe the color, texture, edge, and other information on the surface of the target object), Represents the dimension of the geometric feature vector (i.e., the total number of numerical features used to describe geometric information such as the target object's shape, curvature, normal vector, and sparse point cloud distribution). Before stitching, the visual texture feature vector and the geometric feature vector need to be normalized separately so that the feature values ​​of each dimension are within the same magnitude range.

[0110] The dataset used by the pre-trained convolutional neural network comes from:

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

[0112] Supervised training was completed based on manually annotated material images and point cloud datasets (with fused features as input and manually annotated material categories as labels). The Adam optimizer was used, and the cross-entropy loss function was adopted to minimize the difference between the predicted category and the true label to ensure that the model can effectively distinguish between different material categories. The learning rate was set in a decreasing range from 0.001 to 0.0001. Parameter convergence was achieved through 50 to 200 rounds of iterative training. The trained network model can effectively distinguish a variety of typical materials (including metals, plastics, glass, wood, fabrics, rubber, etc.).

[0113] The fused feature vector As the input of the pre-trained convolutional neural network, high-level representation features are extracted through multi-layer convolution and pooling operations. Then, the high-level features obtained by convolution and pooling are flattened and input into the fully connected layer to integrate global information and generate classification decision feature vectors. The softmax output layer maps the output of the fully connected layer to the material category probability distribution through the softmax function to obtain 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-geometry correlation features;

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

[0116] After each convolution layer, the ReLU activation function is used to achieve nonlinear mapping, improving the model's ability to represent complex textures and geometric shapes;

[0117] Real-time current sampling value based on the motor of the robot arm joint , construct material correction function , correct the material category of the target object;

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

[0119]

[0120] Where, Indicates the reference current during no-load operation. It refers to the normal current value of the motor when the same motion command is given and there is no contact (no load or no trajectory). It can be obtained through offline calibration or online idle cycle estimation. Represents the current increment, which is used to represent the additional load component caused by contact. A positive value usually indicates greater contact / rigidity or higher friction, while a negative value or near zero indicates lighter contact or no resistance. Indicates time The real-time sampling of motor current is obtained when the robot arm contacts the surface of the object with small displacement / low speed, reflecting the instantaneous load / friction / stiffness information of the contact;

[0121]

[0122] Where, Indicates the material category of the corrected target object; Indicates the initial material category prediction result; Represents the material correction function, which is used to determine the threshold mapping, specifically: if the current increment exceeds the current threshold (Current threshold Used to determine the current increment Is it significant? Assign values ​​through calibration experiments, for example: current threshold A, specifically, the current threshold The optimal dividing point for distinguishing soft and hard materials is obtained through calibration experiments on multiple groups of different materials, and the material correction function is used to Initial category Mapped to higher density / higher friction of the same family or superclass (if , then the material category is determined by Adjust to its upper class), otherwise keep ;

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

[0124] According to the corrected material category, the matching material parameter set is retrieved from the preset parameter library , where is the friction coefficient of the target object, which is used to represent the friction characteristics between the contact surface of the gripper and the target object; Indicates the material density of the target object; Indicates the safety factor, which is used to ensure that the clamping force has a certain margin based on the theoretical calculated value and is assigned by expert experience; Indicates the recommended clamping force range for the target object material. This is the clamping force value range given based on the material type in the preset parameter library, which is used to limit the lower and upper limits of the initial clamping force setting value. The maximum surface pressure of the target object material indicates the maximum pressure value that the target material surface can withstand without permanent deformation or damage. It is used to constrain the force per unit area applied by the gripper.

[0125] Based on material parameter sets , and based on the effective contact area between the gripper and the target object , by introducing the comprehensive correction factor , get the final initial setting value of the clamping force ;

[0126] Among them, the comprehensive correction factor It is constructed by the clamping posture angle and the buffer acceleration margin through the Sigmoid smoothing function;

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

[0128]

[0129]

[0130] Where, is the clamping posture angle, which represents the angle between the gripping direction of the gripper and the normal direction of the target object surface. When the angle is too large, the clamping stability decreases and the correction factor needs to be increased; It is the buffer acceleration margin, which indicates the difference between the dynamic acceleration and the safety threshold of the target object during transportation or environmental disturbance. When the dynamic disturbance is large, the clamping force compensation needs to be increased accordingly. Represents the Sigmoid smoothing function, which is used to perform nonlinear smooth mapping of angle and acceleration, so that the correction factor changes continuously and boundedly. ; Represents the weight coefficient of the clamping posture angle; The weight coefficient representing the buffer acceleration margin;

[0131]

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

[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 an updated trajectory point sequence.

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

[0135] Based on sensors (such as RGB-D cameras or LiDAR) to collect point cloud data of the surrounding environment in real time, and build a signed distance field based on the local environment map , which is used to describe the distance relationship between any point in space and the obstacle boundary:

[0136]

[0137] Where, Indicates the obstacle area; Indicates the obstacle boundary; represents the Euclidean distance;

[0138] Based on the starting point and target grasping position of the robot arm, a sampling motion planning algorithm (typical sampling motion planning algorithm RRT*) is used to generate the initial trajectory point sequence , where represents the number of discrete trajectory points from the starting point to the end point, is the end node, which represents the joint state or end pose of the target grasping position; Indicates the starting point node, corresponding to the initial joint state or initial end pose of the robot arm;

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

[0140] Randomly sample joint space nodes ;

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

[0142] To the nearest node Expand to generate new nodes , meeting the maximum step length constraint ;

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

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

[0145] In this embodiment, by comparing the occupancy grid information at consecutive moments, objects whose positions have changed significantly are detected and determined to be dynamic obstacles.

[0146] By using motion estimation or optical flow method, the changed parts of the local occupancy grid are marked as dynamic obstacle sets. , where Indicates the Dynamic obstacles, and each dynamic obstacle contains position, speed and size information;

[0147] Based on the initial trajectory point sequence , introducing real-time dynamic obstacle sets and target object information (target object information includes six degrees of freedom pose, unit surface normal vector and grasping point position set), the initial trajectory point sequence Perform incremental optimization:

[0148] For the initial trajectory point sequence For each trajectory point, the gradient descent is performed by minimizing the trajectory cost function until the minimization trajectory cost function is iterated to convergence, and the optimized trajectory is obtained. , where is the optimized terminal node; Indicates the optimized starting point node;

[0149] Among them, the trajectory cost function is minimized:

[0150]

[0151] Where, Represents the trajectory cost function, represents the current trajectory The comprehensive cost of , the smaller the cost, the smoother the trajectory and the better the obstacle avoidance effect; Represents the trajectory function, which is used to represent the end of the robot or joint over time Spatial position or joint angle sequence; Represents the parameterized time of the trajectory, which is used to continuously represent the changes of trajectory points over time; Represents the acceleration (or curvature) of the trajectory, and the smoothness term ensures the smoothness of the trajectory by minimizing the square of the trajectory acceleration; is the obstacle cost function, which shows the penalty value when the trajectory approaches an obstacle in space, where: Represents the SDF value generated based on the occupancy grid in the local environment map; Represents a function that maps SDF values ​​to cost values, for example, the closer the distance, the greater the cost; Represents the weight coefficient, which is used to balance the importance of smoothness and obstacle avoidance constraints;

[0152] Use gradient descent for iterative updates:

[0153]

[0154] Where, Represents the learning rate, which is used to control the step size of each iterative update; Indicates the iteration The sequence of trajectory points; Indicates the iteration The updated trajectory point sequence; is the gradient of the trajectory cost function with respect to the trajectory, indicating the direction of improvement of the current trajectory in terms of smoothness and obstacle avoidance;

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

[0156] In the iterative process, the neighborhood search algorithm is used to calculate the With dynamic obstacle points Euclidean distance ;

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

[0158] Use the target surface normal vector to track the trajectory points close to the target object Perform local fine-tuning and finally output the updated trajectory point sequence , where Indicates the first A trajectory point, including the robot arm joint angle or end pose information at that point (six-degree-of-freedom pose, including position and attitude rotation matrix or Euler angle); Represents the final updated trajectory point sequence, which includes the executable trajectory of the robot arm after obstacle avoidance optimization and smoothing adjustment, and fine-tuning of the target object 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), a nearest neighbor search algorithm is used to calculate the target proximity, and the force control weight and the 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 the position control weight based on the target proximity are:

[0161] Define target proximity based on the minimum distance between the end effector and the target surface ,in, Indicates that the end is far away from the target, Indicates that the end is close to the target surface;

[0162]

[0163] Where, Indicates the reference distance from the target; Indicates the minimum distance between the end and the target surface; Indicates the safe distance at which contact is allowed;

[0164] Use Sigmoid mapping function to transform the target proximity Mapped to position control weights respectively and force control weight (When the end is far away from the target, higher position tracking accuracy is required (high position control weight). When the end is close to the target, the rigidity needs to be reduced (low position control weight) for smooth contact. And when the end is close to the target, the force control weight needs to be increased to ensure smooth and safe contact).

[0165]

[0166] Where, Represents the force control weight of the end effector, which is used to control the stiffness of the force applied by the robotic arm when approaching the target object; Indicates the minimum value of force control weight; Indicates the maximum value of the force control weight; Represents target proximity, which is used to quantify the real-time distance between the end effector and the surface of the target object; Represents the steepness parameter of the Sigmoid curve, which is used to control the rate of change of nonlinear mapping. The larger the value, the slower the weight The smaller the value, the steeper the change; the smaller the value, the smoother the change; Represents the midpoint of the Sigmoid curve, corresponding to the turning point of the mapping function;

[0167]

[0168] Where, Represents the position control weight of the end effector, which is used to control the priority or stiffness of the robot arm following the trajectory; Indicates the maximum value of position control weight; Indicates the minimum value of the position control weight.

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

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

[0171] In this embodiment, the specific steps involved in correcting the joint torque estimate and converting the corrected joint torque estimate into the terminal force correction value are:

[0172] Based on the state estimation algorithm (extended Kalman filter algorithm), the multi-source signals are fused to calculate the initial joint torque and initial end force estimation values;

[0173] For each joint , by introducing the cogging effect compensation function , calculate the cogging effect compensation term ; Used to reduce the low-speed force estimation error caused by the slot effect;

[0174] Through the cogging effect compensation term Correcting the initial joint torque estimate to obtain a joint torque correction value;

[0175] The robot arm Jacobian matrix is ​​used to map the joint torque correction value to the end force correction value;

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

[0177] In this embodiment, the input vectors 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 of noise;

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

[0180]

[0181] Where, Indicates the joint position (angle), The rotation angle of each joint; Indicates the joint velocity, The angular velocity of each joint (positive and negative indicate the direction of rotation); Represents the joint current, reflecting the actual output torque of the motor (used for state estimation or compensation factor); Indicates the Cogging compensation for each joint;

[0182] Cogging compensation function Specifically:

[0183]

[0184] Where, is the amplitude parameter of the cogging effect compensation term, indicating the The amplitude of the subharmonics depends on the current ; The spatial frequency parameter representing the cogging effect, specifically, ,in, Indicates the number of stator slots, represents the number of pole pairs; Represents the compensation parameters related to joint velocity; is the harmonic order, which indicates the number of harmonics compensated for the cogging effect; is the phase shift, indicating the The initial phase angle of the subharmonics; represents the harmonic index variable ( is the fundamental frequency, for higher harmonics); is the friction direction function, used to solve zero-speed jitter, ,in, Represents the hyperbolic tangent function, with a value range of ; represents the smoothing factor, ; is the friction coefficient, which represents the Coulomb friction amplitude;

[0185] The joint torque correction value is:

[0186]

[0187] Where, Indicates the The joint torque correction value is the result after adding the cogging effect compensation to the original estimate; Indicates the The initial joint torque of each joint;

[0188] Map joint torque correction values ​​to end force correction values:

[0189]

[0190] Where, For the robot arm at the joint position The Jacobian matrix at , describes the mapping relationship between joint velocity and end velocity; represents the inverse transpose of the Jacobian matrix, which is used to map the moment in the joint space to the end force in the Cartesian space; is the terminal force correction value, which represents the terminal force vector calculated after compensation; represents the joint torque vector composed of all joint correction torques;

[0191] The cogging effect will correct the joint torque estimation when the robot arm moves at low speed or the joint moves in a small range by introducing the cogging effect compensation function, making the end force estimation more accurate and stable.

[0192] S6. Based on the force control weight and the position control weight, the position control output and the impedance force control output are calculated respectively, and the final control instruction is generated by combining the initial setting value of the clamping force and the tooth gap compensation item.

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

[0194] Based on the expected position of the planned trajectory point and the actual position of the end of the robotic arm, a trajectory following algorithm is used and position control weights are introduced. , calculate the position control output ;

[0195]

[0196] Where, The joint torque vector representing the position control output; represents the position control weight (obtained in step S4 based on the target proximity); Represents the joint position proportional gain matrix (diagonal matrix), which is used to adjust the feedback strength of the position error; Represents the joint velocity differential gain matrix (diagonal matrix), which is used to adjust the feedback damping of the velocity error; Represents the expected joint position vector of the planned trajectory point (the trajectory point sequence optimized by step S3 supply; Represents the actual joint position vector of the robotic arm (feedback in real time by the joint position sensor or encoder); The expected joint velocity vector representing the planned trajectory point; Represents the actual joint velocity vector of the robot arm (obtained by encoder differential or velocity sensor); Indicates the number of joints of the robotic arm;

[0197] Based on the end force correction value and force control weight , generating impedance control output ;

[0198]

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

[0200] Position control output With impedance control output Fusion is performed and the initial setting value of the clamping force is introduced Apply amplitude constraints to the terminal force component of the fusion output;

[0201] At the same time, the cogging effect compensation term is introduced , perform nonlinear correction on the fused control signal to reduce the torque estimation error during low-speed joint movement and generate the final control command , and output to the robotic arm actuator.

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

[0203]

[0204] Where, is the clamping force vector in the end effector coordinate system (z axis is the grasping direction); represents the initial setting value of the clamping force (obtained in step S2); represents the cogging compensation term (obtained in step S5); represents the clamping force mapping item;

[0205] And the final control instruction The torque command vector is sent directly to the actuators of each joint of the robot arm to achieve trajectory tracking (via Drive the robot arm along the planned trajectory), compliant control (through Achieve precise control of contact force), grasping operation (through Initial gripping force applied) and motor compensation (by Eliminate motor cogging errors)

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

[0207] A combined state vector containing position error and force error is simultaneously constructed, and a Lyapunov function is defined. New force and position weights are calculated by combining the proximity parameter and the end force error (the deviation between the actual force at the end of the manipulator and the expected / target force). The new weights are fed back to step S4, overwriting the original weights. The hybrid control output is then generated using the new weights, and the above operation is repeated in the next cycle.

[0208] Repeat the above steps until the robot arm completes the task of grasping and placing the target object; when the task ends, the end effector and the gripper are reset to their initial positions and the control instruction buffer is cleared.

[0209] Embodiment 2: This embodiment provides a robot, which executes any one of the above-mentioned robot motion control methods, and the robot includes:

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

[0211] The sensor unit includes an end-user IMU, joint accelerometers, motor current sensors, and an RGB-D camera to collect information about the robot's posture, joint status, motor load, and environment.

[0212] A control unit is electrically connected to the robot arm body and the sensor unit and performs the following functions:

[0213] Identify, estimate the position and distance of target workpieces based on visual information;

[0214] Estimate end-force based on multi-source sensing information and distinguish true contact from noise;

[0215] Dynamically adjust the visual control weight and force control weight according to the end proximity, and realize hybrid motion control with vision leading long-distance positioning and force control leading close-range contact;

[0216] Automatically adjust the clamping force according to the workpiece material to achieve anti-slip and anti-breakage control during the grasping process.

[0217] Example 3: This embodiment provides a robot motion control system, 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 any one of the above-described robot motion control methods.

[0218] The above shows and describes 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 above embodiments and descriptions are merely preferred examples of the present invention and are not intended to limit the present invention. Various changes and improvements may be made to the present invention without departing from the spirit and scope of the present invention. Such changes and improvements fall within the scope of the present invention. The scope of protection claimed in the present invention is defined by the appended claims and their equivalents.

Claims

1. A robot motion control method, characterized in that: The following steps are involved: S1. Acquire the current scene depth information, color information, and visual texture and geometric features of the target object through an RGB-D camera, perform instance segmentation and six-degree-of-freedom pose estimation based on the color information and depth information, and obtain the three-dimensional position coordinates and spatial pose parameters of the target object for generating 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. The motor current sampling value is introduced to correct the material category of the target object. The initial setting value of the clamping force is determined based on the corrected material category. 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 based on the CHOMP algorithm to obtain an updated trajectory point sequence. S4. Based on the real-time distance between the end effector and the surface of the target object, a nearest neighbor search algorithm is used to calculate the target proximity, and the force control weight and the position control weight are adjusted based on the target proximity; S5. Based on the state estimation algorithm, multi-source signals are integrated to calculate the estimated values ​​of joint torque and end force. The estimated values ​​of joint torque are corrected by introducing the cogging effect compensation function. The corrected estimated values ​​of joint torque are converted into the corrected values ​​of end force using the manipulator Jacobian matrix. Among them, the multi-source signals include the robot arm joint drive motor current, joint acceleration, end IMU acceleration and angular velocity signals; S6. Based on the force control weight and the position control weight, the position control output and the impedance force control output are calculated respectively, and the final control instruction is generated by combining the initial setting value of the clamping force and the tooth gap compensation item.

2. The robot motion control method according to claim 1, wherein: In S1, the following steps are performed on the color information and the depth information to generate a local environment map: Based on the current scene depth information and the camera internal and external parameter calibration results, the pixel coordinates are converted to a three-dimensional point cloud in the camera coordinate system by back projection, and then converted to the robot base coordinate system through the hand-eye calibration matrix, and the three-dimensional point cloud is preprocessed to obtain a preprocessed three-dimensional point cloud; The color image is input into the YOLOv8-seg network that integrates the depth channel, and the pixel-level instance mask of the target object is output through the network decoder and mask prediction head. ; Pixel-level instance mask based on target objects , extract the plane measurement feature points of the color image and the corresponding depth map in the mask area to obtain the measurement three-dimensional point set of the target object; The three-dimensional point set of the measured mark 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 result includes the three-dimensional position coordinates, surface normal information and rotation angle information of the target object; The occupancy modeling method based on voxel grid mapping discretizes the preprocessed 3D point cloud into spatial grid units, and iteratively updates the occupancy probability of each grid through the 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 robot motion control method according to claim 1, wherein: In S2, the specific steps involved in determining the material type of the target object and the initial setting value of the clamping force are: Take visual texture features and geometric features as input and concatenate them to form fusion features; Input the fused features into the pre-trained convolutional neural network to obtain the initial material category; Real-time current sampling value based on the motor of the robot arm joint , construct material correction function , correct the material category of the target object; According to the corrected material category, the matching material parameter set is retrieved from the preset parameter library , where is the friction coefficient of the target object; Indicates the material density of the target object; represents the safety factor; Indicates the recommended clamping force range for the target object material; is the maximum surface pressure of the target object material; Based on material parameter sets , and based on the effective contact area between the gripper and the target object , by introducing the comprehensive correction factor , get the final initial setting value of the clamping force ; Among them, the comprehensive correction factor It is constructed by the clamping posture angle and the buffer acceleration margin through the Sigmoid smoothing function.

4. The robot motion control method according to claim 1, wherein: In S3, the specific steps involved in generating the updated trajectory point sequence are: Based on the real-time collection of point cloud data of the surrounding environment by sensors, a signed distance field is constructed in combination with the local environment map; According to the starting point and target grasping position of the robot arm, a sampling motion planning algorithm is used to generate the initial trajectory point sequence , where represents the number of discrete trajectory points from the starting point to the end point, is the terminal node; Indicates the starting point node; Based on the initial trajectory point sequence , introducing real-time dynamic obstacle sets and target object information, for the initial trajectory point sequence Perform incremental optimization: For the initial trajectory point sequence For each trajectory point, the gradient descent is performed by minimizing the trajectory cost function until the minimization trajectory cost function is iterated to convergence, and the optimized trajectory is obtained. , where is the optimized terminal node; Indicates the optimized starting point node; In the iterative process, the neighborhood search algorithm is used to calculate the With dynamic obstacle points Euclidean distance ; If the Euclidean distance If the distance is less than or equal to the safety distance threshold, the trajectory point is determined Conflict with obstacles, the entire initial trajectory point sequence The set of all points that meet this condition in is taken as the conflict trajectory point set; Use the target surface normal vector to track the trajectory points close to the target object Perform local fine-tuning and finally output the updated trajectory point sequence .

5. The robot motion control method according to claim 3, wherein: In S4, the specific steps involved in adjusting the force control weight and the position control weight based on the target proximity are: Define target proximity based on the minimum distance between the end effector and the target surface ,in, Indicates that the end is far away from the target, Indicates that the end is close to the target surface; Use Sigmoid mapping function to transform the target proximity Mapped to position control weights respectively and force control weight .

6. The robot motion control method according to claim 5, characterized in that: In S5, the specific steps involved in correcting the joint torque estimate and converting the corrected joint torque estimate into the terminal force correction value are: Based on the state estimation algorithm, multi-source signals are fused to calculate the initial joint torque and initial end force estimation values; For each joint , by introducing the cogging effect compensation function , calculate the cogging effect compensation term ; Through the cogging effect compensation term Correcting the initial joint torque estimate to obtain a joint torque correction value; The robot arm Jacobian matrix is ​​used to map the joint torque correction value to the end force correction value; Among them, the cogging effect compensation function It is a compensation function built based on joint position, joint velocity and joint current.

7. The robot motion control method according to claim 6, characterized in that: In S6, the specific steps involved in generating the final control instruction are: Based on the expected position of the planned trajectory point and the actual position of the end of the robotic arm, a trajectory following algorithm is used and position control weights are introduced. , calculate the position control output ; Based on the end force correction value and force control weight , generating impedance control output ; Position control output With impedance control output Fusion is performed and the initial setting value of the clamping force is introduced Apply amplitude constraints to the terminal force component of the fusion output; At the same time, the cogging effect compensation term is introduced , perform nonlinear correction on the fused control signal to generate the final control instruction , and output to the robotic arm actuator.

8. A robot, said robot executing the robot motion control method according to claims 2-7, characterized in that: The robot comprises: The main body of the robot arm includes multiple joint units and joint drive devices. The joint units are connected in sequence to form a multi-degree-of-freedom structure. The end is provided with an adjustable clamping device for grasping workpieces of different shapes and materials; The sensor unit includes an end-user IMU, joint accelerometers, motor current sensors, and an RGB-D camera to collect information about the robot's posture, joint status, motor load, and environment. A control unit is electrically connected to the robot arm body and the sensor unit and performs the following functions: Identify, estimate the position and distance of target workpieces based on visual information; Estimate end-force based on multi-source sensing information and distinguish true contact from noise; Dynamically adjust the visual control weight and force control weight according to the end proximity, and realize hybrid motion control with vision leading long-distance positioning and force control leading close-range contact; Automatically adjust the clamping force according to the workpiece material to achieve anti-slip and anti-breakage control during the grasping process.

9. A robot motion control system comprising 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 robot motion control method according to any one of claims 2 to 8.

Images