A robot contact control method and device based on geometric dynamic motion primitives, a terminal device, and a storage medium

Abstract

This invention discloses a robot contact control method, device, terminal equipment, and storage medium based on geometric dynamic motion primitives, belonging to the field of robot control technology. The method includes inputting real-time observed images into a trajectory generation model to predict the end-effector pose and time-varying contact stiffness trajectory. After encoding and solving, the contact trajectory and contact stiffness are obtained. The end-effector contact force is then used to control the robot to perform contact operations. The trajectory generation model includes a diffusion model and a geometric dynamic motion primitive model. Training the trajectory generation model involves using a weight matrix fitted based on historical end-effector positions, rotational attitudes, and contact stiffness matrices as action input, historical observed images and historical robot trajectory information as the observation space, and repeatedly performing diffusion training operations on the diffusion model based on the current noise component to obtain a trained trajectory generation model. By implementing this invention, the problem of operation failure caused by disturbances is solved.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a robot contact control method, device, terminal equipment, and storage medium based on geometric dynamic motion primitives. Background Technology

[0002] In power system maintenance scenarios, robots need to perform contact tasks such as plugging and unplugging connectors, operating switches, and precision assembly. These contact tasks are often subject to high environmental uncertainty. In actual operation, due to issues such as lighting and occlusion, there are perception errors in the target position, making it difficult to directly execute the pre-planned trajectory. Currently, robots acquire environmental information through cameras and radar, plan a path to move to the target based on the environmental information, and avoid obstacles along the planned path to perform the grasping operation.

[0003] Existing grasping methods mainly target path planning problems in the non-contact or weak contact phases, and can achieve obstacle avoidance to a certain extent, but lack systematic modeling of the contact operation process. In tasks such as plugging and unplugging connectors in power cabinets and toggling switches, there is a significant force interaction between the robot's end effector and the environment. Simply relying on pre-planned geometric trajectories is difficult to adapt to uncertain disturbances during the contact process, which can easily lead to operation failure. Summary of the Invention

[0004] This invention provides a robot contact control method, device, terminal equipment, and storage medium based on geometric dynamic motion primitives, which can effectively solve the problem that existing technologies cannot avoid operation failure caused by uncertain disturbances during the contact process.

[0005] One embodiment of the present invention provides a robot contact control method based on geometric dynamic motion primitives, comprising: Acquire real-time observation images and end-effector contact force when the robot under control comes into contact with the target object; The real-time observed image is input into a preset trajectory generation model to predict the motion trajectory, thereby obtaining the end pose trajectory and the time-varying contact stiffness trajectory. The contact trajectory and the corresponding contact stiffness are obtained by performing joint trajectory encoding and solving based on the end pose trajectory and the time-varying contact stiffness trajectory. Based on the end contact force, contact trajectory, and corresponding contact stiffness, the robot under control is controlled to perform contact correction. The trajectory generation model includes a diffusion model and a geometric dynamic motion primitive model. The training of the trajectory generation model includes: Acquire historical observation images and historical robot trajectory information; the historical robot trajectory information includes historical end-effector position, historical rotational attitude, and historical contact stiffness matrix. Based on the geometric dynamic motion primitive model, the historical end position, historical rotational posture and historical contact stiffness matrix are encoded, mapped and projected to obtain a weight matrix for characterizing the robot's contact operation skills. Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, the diffusion training operation is repeatedly performed on the diffusion model based on the current noise component until the preset loss function value converges, thus obtaining the trained diffusion model and geometric dynamic motion primitive model. Based on the trained diffusion model and the geometric dynamic motion primitive model, a trained trajectory generation model is constructed.

[0006] Furthermore, the diffusion training operation includes: Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, Gaussian noise is superimposed on the weight matrix based on the current noise component to obtain noisy weight data under different diffusion step sizes. Using historical observation images corresponding to different diffusion step sizes as conditional observation spatial data, and training based on noisy weighted data, a multimodal action sequence is obtained; Based on the multimodal action sequence, integral reconstruction is performed to obtain the predicted end-effector pose trajectory and the predicted time-varying contact stiffness trajectory. The preset loss function value is calculated based on the predicted end-effector pose trajectory, the predicted time-varying contact stiffness trajectory, and historical robot trajectory information, and it is then determined whether the loss function value has converged. If yes, the trained diffusion model is obtained; otherwise, the current noise component is updated based on the loss function value.

[0007] Furthermore, the end-effector pose trajectory includes: an end-effector translation trajectory and a rotational attitude trajectory; Based on the end-effector pose trajectory and the time-varying contact stiffness trajectory, joint trajectory encoding and solving are performed to obtain the contact trajectory and the corresponding contact stiffness, including: After temporally coupling and aligning the end translation trajectory, the rotational attitude trajectory, and the time-varying contact stiffness trajectory, the aligned end translation trajectory is mapped to Euclidean space, the aligned rotational attitude trajectory is mapped to a three-dimensional special orthogonal group manifold space, and the aligned time-varying contact stiffness trajectory is mapped to a symmetric positive definite manifold space, thereby obtaining the corresponding translational features, rotational attitude features, and contact stiffness features, respectively. Based on the preset Gaussian function, the translation features, rotational attitude features and contact stiffness features are jointly encoded to obtain the shape forcing term used to characterize the target contact operation; The second-order dynamic online integral is performed based on the shape forcing term to obtain the pose and stiffness solutions in Euclidean space. An exponential mapping operation is performed based on the pose and stiffness solutions to obtain the contact trajectory and corresponding contact stiffness.

[0008] Furthermore, the weight matrix includes: the first weight component corresponding to the end translation trajectory, the second weight component corresponding to the rotational attitude trajectory, and the third weight component corresponding to the time-varying contact stiffness trajectory; Based on the current noise components and the geometric dynamic motion primitive model, the historical end-effector position, historical rotational posture, and historical contact stiffness matrix are encoded, mapped, and projected to obtain a weight matrix for characterizing the robot's contact operation skills, including: The historical end position is integrated into the historical end translation trajectory, the historical rotation attitude is integrated into the historical rotation attitude trajectory, and the historical contact stiffness matrix is ​​integrated into the historical time-varying contact stiffness trajectory. After performing a logarithmic mapping operation on the historical end translation trajectory, historical rotational attitude trajectory, and historical time-varying contact stiffness trajectory to Euclidean space, the current noise component, the mapped historical end translation trajectory, historical rotational attitude trajectory, and historical time-varying contact stiffness trajectory are input into the geometric dynamic motion primitive model for projection fitting, and the corresponding first weight component, second weight component, and third weight component are obtained respectively. By concatenating the first, second, and third weight components, a weight matrix is ​​obtained to characterize the robot's contact operation skills.

[0009] Furthermore, based on the end-effector contact force, contact trajectory, and corresponding contact stiffness, the robot under control is controlled to perform contact correction, including: The end contact force, contact trajectory, and corresponding contact stiffness are input to the preset admittance controller. The contact trajectory is used as the desired reference trajectory, and the contact stiffness is used as the real-time stiffness parameter. The end pose correction is calculated based on the end contact force, the preset inertia matrix, and the damping matrix. The desired reference trajectory is adjusted based on the end-effector pose correction amount, and the robot under control is controlled to perform contact correction based on the adjusted desired reference trajectory.

[0010] Furthermore, the loss function values ​​include: the mean square error between the predicted noise component and the current noise component, the first trajectory fitting error between the predicted end-effector pose trajectory and the historical robot trajectory information, and the second trajectory fitting error between the predicted time-varying contact stiffness trajectory and the historical robot trajectory information.

[0011] As an improvement to the above solution, another embodiment of the present invention provides a robot contact control device based on geometric dynamic motion primitives, comprising: The real-time data acquisition module is used to acquire real-time observation images and end-effector contact forces when the robot under control comes into contact with the target object. The motion trajectory prediction module is used to input the real-time observed image into a preset trajectory generation model to predict the motion trajectory, thereby obtaining the end pose trajectory and the time-varying contact stiffness trajectory. The contact information generation module is used to perform joint trajectory encoding and solving based on the end pose trajectory and the time-varying contact stiffness trajectory to obtain the contact trajectory and the corresponding contact stiffness. The robot control module is used to control the robot under control to perform contact correction based on the end contact force, contact trajectory and corresponding contact stiffness. The model training module is used to train the trajectory generation model; the trajectory generation model includes a diffusion model and a geometric dynamic motion primitive model. The training of the trajectory generation model includes: Acquire historical observation images and historical robot trajectory information; the historical robot trajectory information includes historical end-effector position, historical rotational attitude, and historical contact stiffness matrix. Based on the geometric dynamic motion primitive model, the historical end position, historical rotational posture and historical contact stiffness matrix are encoded, mapped and projected to obtain a weight matrix for characterizing the robot's contact operation skills. Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, the diffusion training operation is repeatedly performed on the diffusion model based on the current noise component until the preset loss function value converges, thus obtaining the trained diffusion model and geometric dynamic motion primitive model. Based on the trained diffusion model and the geometric dynamic motion primitive model, a trained trajectory generation model is constructed.

[0012] Furthermore, the diffusion training operation includes: Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, Gaussian noise is superimposed on the weight matrix based on the current noise component to obtain noisy weight data under different diffusion step sizes. Using historical observation images corresponding to different diffusion step sizes as conditional observation spatial data, and training based on noisy weighted data, a multimodal action sequence is obtained; Based on the multimodal action sequence, integral reconstruction is performed to obtain the predicted end-effector pose trajectory and the predicted time-varying contact stiffness trajectory. The preset loss function value is calculated based on the predicted end-effector pose trajectory, the predicted time-varying contact stiffness trajectory, and historical robot trajectory information, and it is then determined whether the loss function value has converged. If yes, the trained diffusion model is obtained; otherwise, the current noise component is updated based on the loss function value.

[0013] Another embodiment of the present invention provides a terminal device, including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, it implements a robot contact control method based on geometric dynamic motion primitives as described in the above embodiments.

[0014] Another embodiment of the present invention provides a computer-readable storage medium including a stored computer program, wherein, when the computer program is executed, it controls the device where the computer-readable storage medium is located to execute the robot contact control method based on geometric dynamic motion primitives described in the above embodiment.

[0015] By implementing this invention, at least the following beneficial effects are achieved: This invention provides a robot contact control method, device, terminal equipment, and storage medium based on geometric dynamic motion primitives. The method overcomes the limitations of focusing solely on geometric path planning by constructing a trajectory generation model that includes a diffusion model and a geometric dynamic motion primitive model. It simultaneously predicts and generates end-effector pose trajectories and time-varying contact stiffness trajectories, incorporating the force interaction characteristics between the robot and its environment into the entire contact operation modeling process, achieving integrated fusion of motion trajectory planning and contact force adaptation. Through encoding, mapping, and projection fitting of historical end-effector positions, historical rotational attitudes, and historical contact stiffness matrices using the geometric dynamic motion primitive model, a weight matrix that fully represents the robot's contact operation skills is obtained. Further diffusion training of the diffusion model is then performed, achieving a unified parameterized representation of motion and force control characteristics in contact operations. This ensures that the generated contact trajectory and corresponding contact stiffness accurately match the differentiated requirements of the contact operation process. Simultaneously, combined with real-time acquired end-effector contact forces, closed-loop control of the robot's contact operations is achieved, adapting online to environmental changes and uncertain disturbances during the contact process. This addresses the problem of insufficient adaptability of fixed-planned trajectories to uncertain disturbances, improving the success rate of robot operations in contact scenarios. Attached Figure Description

[0016] Figure 1 This is a flowchart illustrating a robot contact control method based on geometric dynamic motion primitives according to an embodiment of the present invention. Figure 2 This is a schematic diagram of a robot planning and control process provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of the trajectory dimensionality reduction and reproduction control process provided in an embodiment of the present invention; Figure 4 This is a schematic diagram of the diffusion model generation process provided in an embodiment of the present invention; Figure 5This is a schematic diagram of a one-dimensional admittance controller provided in an embodiment of the present invention; Figure 6 This is a schematic diagram of the robot GDMP planning and execution process provided in an embodiment of the present invention; Figure 7 This is a schematic diagram of a robot contact control device based on geometric dynamic motion primitives according to an embodiment of the present invention. Detailed Implementation

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

[0018] See Figure 1 To address the problem of operational failure caused by unpredictable disturbances during the contact process in existing technologies, an embodiment of the present invention provides a flowchart illustrating a robot contact control method based on geometric dynamic motion primitives, comprising: S1. Acquire real-time observation images and end-effector contact force when the robot to be controlled comes into contact with the target object; Specifically, real-time observation images are acquired by cameras installed at the end of the robot body or at fixed positions in the operation scene. The sampling frequency is matched with the robot control cycle, preferably 100Hz. The acquired content includes the operation scene environment, the operation object (such as power cabinet connector, switch), and the relative pose images of the robot end effector. The end contact force is acquired by a six-dimensional force sensor installed at the end of the robot, with a sampling frequency preferably 1kHz. It includes three-dimensional contact force and three-dimensional contact torque, which are used for subsequent closed-loop compliant control.

[0019] Specifically, the robot to be controlled is a six-degree-of-freedom serial robotic arm, equipped with a six-dimensional force sensor and an industrial camera at the end, and the controller supports a real-time control cycle of more than 1 kHz.

[0020] S2. Input the real-time observed image into the preset trajectory generation model to predict the motion trajectory and obtain the end pose trajectory and time-varying contact stiffness trajectory. Specifically, the trajectory generation model includes a diffusion model and a geometric dynamic motion primitive model (GDMP model). The diffusion model is responsible for multimodal prediction of operational skills from visual observations, while the GDMP model is responsible for reconstructing skill parameters into trajectories and stiffness, jointly achieving end-to-end generation from environmental observations to executable trajectories and stiffness parameters. The geometric dynamic motion primitive model (GDMP model) is a geometrically enhanced extension of the traditional DMP, specifically designed for encoding non-Euclidean space data. It can uniformly reduce the dimensionality of heterogeneous data such as robot end-effector pose and contact stiffness, ensuring that geometric constraints do not fail through Riemannian manifold mapping, and achieving temporally coupled joint encoding of pose and stiffness, serving as the parameterized carrier of robot contact operation skills. The diffusion model adopts a conditional generative model, also known as the conditional diffusion model. Its core function is to learn the conditional probability distribution between visual observations and the GDMP weight matrix, generating multimodal contact operation skill parameters under similar observations, adapting to different contact operation strategies. The weight matrix is ​​a low-dimensional compressed representation of contact operation skills. It is obtained by fitting the teaching trajectory encoding by GDMP and includes weight components for translation, rotation, and stiffness. It can completely characterize the motion law and force control law of the entire contact operation.

[0021] Specifically, the end-effector pose trajectory is the continuous motion trajectory of the robot's end effector in Cartesian space, including the end-effector translation trajectory and rotational attitude trajectory. The end-effector translation trajectory describes the robot's three-dimensional spatial position changes, the rotational attitude trajectory describes the three-dimensional spatial attitude changes, and the end-effector pose trajectory describes the complete motion law of the robot's end effector. The time-varying contact stiffness trajectory is a sequence of contact stiffness parameters that dynamically change over time throughout the entire robot contact operation process. It corresponds to the stiffness setpoint of the admittance controller and is used to adjust the compliance of the robot's end effector at different operation stages. High stiffness in the approach stage ensures positioning accuracy, while low stiffness in the contact stage improves compliance.

[0022] Schematic, real-time acquired observation images are preprocessed and used as conditional input to a pre-trained trajectory generation model. The trajectory generation model generates a GDMP weight matrix through inverse denoising using a diffusion model, and then initially expands it into a continuous end-effector pose trajectory and a time-varying contact stiffness trajectory using a GDMP model. Preprocessing includes denoising and enhancement.

[0023] S3. Perform joint trajectory encoding and solving based on the end pose trajectory and the time-varying contact stiffness trajectory to obtain the contact trajectory and the corresponding contact stiffness; Specifically, joint trajectory encoding and solving refers to the unified encoding of the end-effector pose trajectory and time-varying contact stiffness trajectory through the GDMP model, coupled in time. Through manifold mapping, basis function fitting, and second-order system integration, the executable contact trajectory and matching contact stiffness that satisfy geometric constraints are finally obtained.

[0024] Schematic, the initially generated pose and stiffness trajectories are temporally coupled and aligned using a GDMP model. Riemannian manifold mapping ensures the geometric constraints of rotational attitude and stiffness parameters. Second-order dynamic system integration is used to solve for a smooth, continuous, temporally synchronized, and operable contact trajectory that satisfies operational geometric constraints, along with the time-varying contact stiffness for each control cycle. Geometric constraint verification and optimization are performed on the initially generated trajectory and stiffness to ensure the output can be directly used for robot closed-loop control, avoiding issues such as trajectory distortion and force control timing mismatch.

[0025] Preferably, the end effector pose trajectory includes: an end effector translation trajectory and a rotational attitude trajectory; Based on the end-effector pose trajectory and the time-varying contact stiffness trajectory, joint trajectory encoding and solving are performed to obtain the contact trajectory and the corresponding contact stiffness, including: After temporally coupling and aligning the end translation trajectory, the rotational attitude trajectory, and the time-varying contact stiffness trajectory, the aligned end translation trajectory is mapped to Euclidean space, the aligned rotational attitude trajectory is mapped to a three-dimensional special orthogonal group manifold space, and the aligned time-varying contact stiffness trajectory is mapped to a symmetric positive definite manifold space, thereby obtaining the corresponding translational features, rotational attitude features, and contact stiffness features, respectively. Based on the preset Gaussian function, the translation features, rotational attitude features and contact stiffness features are jointly encoded to obtain the shape forcing term used to characterize the target contact operation; The second-order dynamic online integral is performed based on the shape forcing term to obtain the pose and stiffness solutions in Euclidean space. An exponential mapping operation is performed based on the pose and stiffness solutions to obtain the contact trajectory and corresponding contact stiffness.

[0026] Specifically, temporal coupling alignment refers to aligning the time axes of the end-effector translation trajectory, rotational attitude trajectory, and time-varying contact stiffness trajectory in a unified manner, ensuring that the number of sampling points and time steps of the three are completely consistent, ensuring that the temporal sequence of motion trajectory and stiffness change is strictly synchronized, and avoiding the problem of motion and force control being disconnected. The three-dimensional special orthogonal group manifold (SO(3) manifold) represents a smooth Riemann manifold composed of 3×3 orthogonal matrices. It is the natural geometric space describing the pure rotation of rigid bodies in three-dimensional space. Each point corresponds to a unique three-dimensional rotational attitude, which can completely avoid the gimbaling problem of Euler angles. The symmetric positive definite manifold (SPD manifold) represents a Riemann manifold composed of symmetric and positive definite 3×3 matrices. It is the natural geometric space describing the robot contact stiffness matrix, which can ensure the positive definiteness of the stiffness matrix and avoid the problems of stiffness failure and control instability. Logarithmic mapping is the core operation in Riemann manifolds. It can map the non-Euclidean data of the curved space on the manifold to the tangent space (flat Euclidean space) of the origin of the manifold, realize the linearization of nonlinear data, and facilitate subsequent encoding and fitting. The Gaussian radial basis function (GDB) is used in the GDMP model to fit the trajectory shape. Multiple basis functions are evenly distributed and moderately overlapped along the time axis, allowing for accurate fitting of arbitrarily complex trajectory shapes. The shape forcing term is the core term in the GDMP model that characterizes the trajectory shape. It is obtained by weighted summation of the GDB and its corresponding weights, determining the trajectory's motion shape and stiffness variation, and is a key feature distinguishing different operational skills. Second-order dynamic online integration involves substituting the shape forcing term into the second-order spring-damped attractor system of the GDMP model and performing continuous-time integration to obtain a smooth and continuous trajectory and stiffness sequence, ensuring the system's global convergence and disturbance resistance to the target point. Exponential mapping, the inverse operation, maps the solution results in Euclidean and tangent spaces back to the original Riemannian manifold, restoring the valid rotational attitude and stiffness matrices, ensuring that geometric constraints remain valid.

[0027] Schematic, firstly, the time axes of the three trajectories are uniformly normalized to the [0,1] interval, and resampled to the same number of sampling points to complete the temporal coupling alignment; the end translation trajectory is directly mapped to the three-dimensional Euclidean R3 space to obtain translation features; the quaternion of the rotational attitude trajectory is converted into a rotation matrix and mapped to the SO(3) manifold space; the 3×3 matrix of the contact stiffness trajectory is mapped to the SPD manifold space; the data on the SO(3) and SPD manifolds are converted into Euclidean linearized data in the corresponding tangent space through logarithmic mapping to obtain rotational attitude features and contact stiffness features. At the same time, the element movement of different tangent spaces is completed through parallel transmission operators, and the feature dimension is unified through Mandel vectorization processing.

[0028] K pre-defined Gaussian functions are used to fit translational, rotational, and contact stiffness features, respectively, and the corresponding weights are obtained by solving the least squares method. The weights of the three feature sets are then weighted and summed with the Gaussian functions to obtain a unified shape forcing term. This shape forcing term simultaneously includes the trajectory shape features of translation, rotation, and stiffness. By using Gaussian functions to jointly encode the three types of features, the core shape features of the trajectory are compressed into the shape forcing term, achieving a unified representation of operational skills.

[0029] Substituting the shape forcing term into the second-order spring-damped attractor system of the GDMP model, continuous-time online integration is performed with a fixed control cycle to obtain smooth and continuous pose and stiffness solutions in Euclidean space. Solving the second-order dynamic system ensures that the generated trajectory is smooth and continuous, converges globally to the target point, and possesses anti-disturbance capabilities. The rotational attitude solution is transformed back to the SO(3) manifold through exponential mapping to obtain a valid rotation matrix; the stiffness solution is transformed back to the SPD manifold through exponential mapping to obtain a symmetric positive definite contact stiffness matrix; combined with the translational trajectory solution, a complete contact trajectory satisfying geometric constraints is finally obtained, along with the contact stiffness corresponding to each control cycle. By restoring the Euclidean space solution to the valid pose and stiffness data required for robot control, the geometric constraints are ensured to remain valid, avoiding control instability caused by pose distortion and non-positive definite stiffness matrices.

[0030] Schematic, the number of sampling points for time-coupled alignment is preferably 1000 points, matching the robot's 1kHz control cycle to ensure trajectory smoothness; K=50 Gaussian functions are set, with their centers uniformly distributed in the [0,1] interval, and the width parameter is set to h=1 / (2K). 2 ), ensuring that adjacent basis functions overlap appropriately; the logarithmic mapping of SO(3) manifold is achieved through axis-angle transformation of rotation matrix, and the logarithmic mapping of SPD manifold is achieved through matrix eigenvalue decomposition.

[0031] This embodiment solves the problem of distortion in the encoding of rotational attitude and contact stiffness in traditional DMP. It ensures the encoding accuracy of non-Euclidean data through Riemannian manifold mapping, and avoids problems such as gimbal lock and non-positive definite stiffness matrix. It realizes the temporal coupling joint encoding of translation, rotation and stiffness data, ensuring strict temporal synchronization between motion trajectory and force control parameters, and realizing deep coupling between trajectory and force control. Through the second-order spring damping attractor system, it ensures that the generated trajectory converges globally to the target point and has good robustness to environmental disturbances.

[0032] S4. Based on the end contact force, contact trajectory and corresponding contact stiffness, control the robot to be controlled to perform contact correction; Indicatively, the contact trajectory is used as the desired reference trajectory, and the contact stiffness is used as the real-time compliance parameter. Combined with the real-time acquired end contact force feedback, the compliance closed-loop control is realized through the admittance controller, which outputs robot joint space control commands to drive the robot to perform contact operations.

[0033] Preferably, the robot under control is controlled to perform contact correction based on the end contact force, contact trajectory, and corresponding contact stiffness, including: The end contact force, contact trajectory, and corresponding contact stiffness are input to the preset admittance controller. The contact trajectory is used as the desired reference trajectory, and the contact stiffness is used as the real-time stiffness parameter. The end pose correction is calculated based on the end contact force, the preset inertia matrix, and the damping matrix. The desired reference trajectory is adjusted based on the end-effector pose correction amount, and the robot under control is controlled to perform contact correction based on the adjusted desired reference trajectory.

[0034] Specifically, the admittance controller employs a six-dimensional compliant controller in Cartesian space. Its core function is to convert the contact force feedback from the robot's end effector into a pose correction value, simulating the dynamic characteristics of a second-order mass-spring-damped system. This achieves compliant contact control at the robot's end effector, avoiding jamming and equipment damage caused by rigid contact. The inertia matrix, a fixed parameter of the admittance controller, is a 6×6 diagonal matrix corresponding to the virtual inertia of three-dimensional translation and rotation in Cartesian space, determining the system's response speed to contact forces. The damping matrix, also a fixed parameter of the admittance controller, is a 6×6 diagonal matrix corresponding to the virtual damping of three-dimensional translation and rotation, used to suppress system oscillations and ensure control stability. The pose correction value is the end effector pose adjustment calculated by the admittance controller based on the contact force feedback, including three-dimensional position correction and three-dimensional attitude correction, used to adjust the desired reference trajectory and achieve compliant adaptation.

[0035] Schematic, a six-dimensional admittance controller in Cartesian space is employed. The inputs include real-time acquired end-effector contact force, the desired contact trajectory generated by GDMP, and time-varying contact stiffness. The contact trajectory serves as the desired reference trajectory, and the time-varying contact stiffness is used as the controller's real-time stiffness parameter. The inertia matrix and damping matrix are preset fixed diagonal matrices. The end-effector pose correction is calculated through the admittance control equation. By converting the contact force feedback into pose correction through the admittance controller, compliant control of the contact force is achieved. The stiffness parameters are generated in real-time by GDMP, rather than being manually fixed, thus realizing the intrinsic coupling between trajectory and force control.

[0036] The calculated pose correction is superimposed on the original desired reference trajectory to obtain the adjusted compliant reference trajectory. The adjusted Cartesian space trajectory is converted into angle commands in the robot joint space through inverse kinematics, and input into the robot's underlying position controller to drive the robotic arm to perform contact operations. At the same time, the end contact force is collected in real time to enter the next round of control closed loop.

[0037] In a preferred embodiment of the present invention, the diagonal element of the inertial matrix M of the admittance controller in the translation direction is preferably 1kg, and the diagonal element in the rotation direction is preferably... The damping matrix D is set according to the critical damping ratio, and the optimal damping coefficient in the translation direction is [missing value]. The preferred direction of rotation The optimal control cycle for the admittance controller is 1ms, matching the robot's underlying control cycle to ensure real-time force control. A numerical iterative method is used to solve the robot's inverse kinematics, ensuring both accuracy and real-time performance while avoiding singular robot configurations. The rotational error between the current and desired postures is calculated using a logarithmic mapping to avoid the gimbaling problem of Euler angles, and together with the position error, forms a six-dimensional error vector.

[0038] This embodiment achieves the intrinsic coupling of trajectory generation and force control. The stiffness parameters are generated in real time by the GDMP model according to the operation stage, eliminating the need for manual adjustment and significantly improving the compliance and adaptability of contact operation. The six-dimensional admittance controller realizes full-degree-of-freedom compliant control in Cartesian space, which can simultaneously adapt to contact force feedback in translation and rotation directions. It can automatically adjust the posture when faced with positioning errors and environmental disturbances, avoiding problems such as jamming and excessive impact force. By using a fixed inertia matrix and damping matrix, compliance is adjusted only through time-varying stiffness, simplifying the difficulty of controller parameter debugging and ensuring the stability of the system, without the problem of control divergence.

[0039] The trajectory generation model includes a diffusion model and a geometric dynamic motion primitive model. The training of the trajectory generation model includes: S501. Acquire historical observation images and historical robot trajectory information; the historical robot trajectory information includes historical end-effector position, historical rotational attitude, and historical contact stiffness matrix. Specifically, through remote teaching, operators drag the robot to complete typical contact operations such as plugging and unplugging connectors in the power cabinet, toggling switches, and rotating knobs. Simultaneously, camera images, the robot's end effector position, rotational attitude, and contact stiffness matrix data are collected during the operation. Each set of images is paired with the trajectory information of the corresponding operation to form a standardized teaching dataset. Alternatively, historical images, end effector positions, rotational attitudes, and contact stiffness matrices from the robot's past operations can be directly acquired.

[0040] Specifically, the number of historical robot trajectory information for a single type of contact operation is no less than 50 sets, covering different target poses, contact angles, and operation speeds to ensure the generalization ability of the diffusion model.

[0041] In a preferred embodiment of the present invention, such as Figure 2 As shown, the robot's mechanical system should include a camera, an end effector, and an end force sensor. Historical robot trajectory information is collected when the robot performs typical operations on the power cabinet, such as plugging / unplugging connectors, toggling switches, or rotating buttons. The historical contact stiffness matrix in the historical robot trajectory information includes historical contact force information. The historical observation images and historical robot trajectory information form a robot body dataset. ,in Represents historical observation images collected from cameras. = Represents historical trajectory information from the robot itself. It is the amount of trajectory information. Including the end of history Historical Rotation Posture and historical contact stiffness matrix The data structures are respectively , and At the end of history Historical Rotation Posture and historical contact stiffness matrix G-DMP is used for encoding, and trajectories with different mathematical structures are mapped to Euclidean space through logarithmic mapping operations. The Euclidean space projection is fitted as a second-order dynamic system integral, and basis functions and forcing terms are established to fit the shape.

[0042] S502. Based on the geometric dynamic motion primitive model, the historical end position, historical rotational posture and historical contact stiffness matrix are encoded, mapped and projected to obtain a weight matrix for characterizing the robot's contact operation skills. Specifically, the basis functions and second-order dynamic system parameters of the GDMP model are preset before training, and the GDMP model structure remains fixed during training. Historical end-effector positions, historical rotational attitudes, and historical contact stiffness matrices are mapped to their corresponding Riemannian manifolds, converted to Euclidean linearized data through logarithmic mapping, and fitted using Gaussian function projection to obtain a weight matrix containing translation, rotation, and stiffness components. GDMP compresses high-dimensional, heterogeneous teaching trajectory data into a low-dimensional weight matrix, achieving a standardized parameterized representation of contact manipulation skills and providing a learning target for the diffusion model.

[0043] Specifically, the number of Gausky functions K in the GDMP model is preferably 50, the stiffness gain of the second-order dynamic system is preferably 25, and the damping gain is preferably 10, to ensure that the system is critically damped and converges without oscillation.

[0044] Preferably, the weight matrix includes: a first weight component corresponding to the end translation trajectory, a second weight component corresponding to the rotational attitude trajectory, and a third weight component corresponding to the time-varying contact stiffness trajectory; Based on the current noise components and the geometric dynamic motion primitive model, the historical end-effector position, historical rotational posture, and historical contact stiffness matrix are encoded, mapped, and projected to obtain a weight matrix for characterizing the robot's contact operation skills, including: The historical end position is integrated into the historical end translation trajectory, the historical rotation attitude is integrated into the historical rotation attitude trajectory, and the historical contact stiffness matrix is ​​integrated into the historical time-varying contact stiffness trajectory. After performing a logarithmic mapping operation on the historical end translation trajectory, historical rotational attitude trajectory, and historical time-varying contact stiffness trajectory to Euclidean space, the current noise component, the mapped historical end translation trajectory, historical rotational attitude trajectory, and historical time-varying contact stiffness trajectory are input into the geometric dynamic motion primitive model for projection fitting, and the corresponding first weight component, second weight component, and third weight component are obtained respectively. By concatenating the first, second, and third weight components, a weight matrix is ​​obtained to characterize the robot's contact operation skills.

[0045] Specifically, the first weight component is the Gaussian function weight corresponding to the end-effector translational trajectory, with a dimension of K×3, where 3 represents the three degrees of freedom of translation, used to characterize the shape characteristics of the translational trajectory. The second weight component is the Gaussian function weight corresponding to the rotational attitude trajectory, with a dimension of K×3, where 3 represents the three degrees of freedom of rotation, used to characterize the shape characteristics of the rotational attitude trajectory. The third weight component is the Gaussian function weight corresponding to the time-varying contact stiffness trajectory, with a dimension of K×6, where 6 is the Mandel vectorization dimension of the symmetric positive definite stiffness matrix, used to characterize the temporal variation law of contact stiffness.

[0046] Schematic, discrete historical endpoints are stitched together in chronological order to form a continuous three-dimensional translational trajectory; discrete historical rotational attitudes are stitched together in chronological order to form a continuous rotational attitude trajectory; and discrete historical contact stiffness matrices are stitched together in chronological order to form a continuous time-varying contact stiffness trajectory. The time axes of the three trajectories are uniformly normalized to the [0,1] interval, completing temporal alignment. The translational trajectory is directly normalized and mapped to Euclidean R3 space; the rotational attitude trajectory and the time-varying contact stiffness trajectory are respectively logarithmically mapped and converted into linearized data in Euclidean space; the mapped three types of trajectory data are input into the GDMP model, and projection fitting is performed using a preset Gausky function. The first weight component of the corresponding translational trajectory, the second weight component of the rotational attitude trajectory, and the third weight component of the contact stiffness trajectory are obtained by the least squares method. Finally, the three independent weight components are integrated into a unified skill parameter vector to achieve a unified representation of contact operation motion characteristics and force control characteristics.

[0047] In a preferred embodiment of the present invention, when the number of Gaussian functions K=50, the weight matrix is ​​a 50×12 two-dimensional matrix, which, after being flattened, becomes a 600-dimensional one-dimensional vector. Compared with the original high-dimensional trajectory data with 1000 sampling points, the dimensionality is compressed by more than 90%. The weights of the Gaussian functions are solved by the least squares method to minimize the mean square error between the fitted trajectory and the original trajectory, ensuring that the encoded weights can accurately restore the original trajectory. Horizontal splicing along the feature dimension is preferred to ensure that the temporal correspondence of the three types of weight components is not destroyed, avoiding temporal misalignment during subsequent restoration.

[0048] This embodiment compresses the original trajectory data of tens of thousands of dimensions into a weight vector of several hundred dimensions, which greatly reduces the learning difficulty and computational load of the diffusion model, and improves the model training efficiency and online inference speed. By using a unified weight matrix, the binding of motion features and force control features is realized, ensuring that the weight matrix generated by the diffusion model can simultaneously restore the trajectory and stiffness, and realize the integrated generation of trajectory and force control.

[0049] In a preferred embodiment of the present invention, the SPD symmetric positive definite matrix group is taken as an example. ,in and For the gain parameters of the second-order dynamic system. , These are all calculation formulas on Riemannian manifolds. It is the shortest geodesic distance between two matrices, representing the distance between them. The corresponding Riemannian tangent space. Moving elements between different tangent spaces is performed by the parallel transfer operator, which here refers to moving from... The corresponding Riemannian space moves to The corresponding tangent space, vec, represents the vectorization in Mandel. , and These represent the target stiffness matrix, the initial stiffness matrix, and the current stiffness matrix of the trajectory, respectively. The vectorization of the geodesic distance between SPD stiffness matrices. The shape term is represented by the fit of the basis functions. for The velocity term represents the rate of evolution of the stiffness trajectory in tangent space, which is equivalent to the velocity variable in a second-order Euclidean system.

[0050] in, , For basis functions, the Gaussian radial function is usually chosen, i.e. . This indicates that each basis function corresponds to a weight component, namely the first weight component, the second weight component, and the third weight component. The set array is the weight matrix Dimensions of the weight matrix K is the number of artificially given basis functions, and N is the total number of all Gaussian radial basis functions. The central parameter of the function represents the function's effect over time period t, and the width parameter of the basis functions. The influence range is controlled to allow for moderate overlap between adjacent basis functions. To ensure uniform coverage of the time interval, the centers of the basis functions are evenly distributed across the interval [0,1]. The width parameter is set based on the spacing between adjacent basis functions, resulting in moderate overlap and thus maintaining good expressiveness while ensuring trajectory smoothness. , is the overlap coefficient of the basis functions.

[0051] The same process is performed on the rotation matrix SO(3) group, and the R3 data is translated to obtain the dimensionality-reduced weight matrix. The original information contains different data structures, including historical end positions, historical rotational attitudes, and historical contact stiffness matrices, with thousands of sampling points, each represented by three different data structures.

[0052] S503. Using the weight matrix as the action input and the historical observation images and historical robot trajectory information as the observation space, repeatedly perform diffusion training operations on the diffusion model based on the current noise component until the preset loss function value converges, and obtain the trained diffusion model and geometric dynamic motion primitive model. Specifically, the observation space is the set of conditional inputs for the diffusion model, including environmental and robot state information such as historical observation images and historical robot trajectory information. This ensures that the input dimensions are consistent between the training and online execution phases, avoiding model distribution shifts. The diffusion training operation is the iterative training process of the diffusion model, including two core steps: forward diffusion noise addition and backward denoising training. By gradually adding Gaussian noise to the weight matrix, the model's ability to predict noise and reconstruct the weight matrix under observation constraints is trained. The current noise component is the isotropic Gaussian noise added during the diffusion training process, used for forward diffusion noise addition. The first training uses a preset initial noise component, and the noise components in subsequent steps follow a preset variance scheduling strategy.

[0053] Specifically, the diffusion step size t1 of the diffusion model is preferably 100, a cosine noise scheduling strategy is adopted, and the backbone network adopts a U-Net structure.

[0054] Schematic, using paired historical observation images and historical robot trajectory information as input, and a weight matrix obtained through GDMP encoding as the prediction target, forward noise is added to the weight matrix based on the current noise component to train the diffusion model's ability to reverse denoising and reconstruct the weight matrix. The diffusion training operation is iteratively performed until the loss function value stabilizes and converges, resulting in a trained diffusion model. The trained geometric dynamic motion primitive model refers to the fixed-parameter GDMP model that has completed the teaching trajectory encoding and determined the mapping relationship between the weight matrix and the trajectory. Training the diffusion model learns the conditional mapping of visual observation to the weights of operational skills, achieving the ability to generate multimodal contact operational skills.

[0055] Preferably, the diffusion training operation includes: Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, Gaussian noise is superimposed on the weight matrix based on the current noise component to obtain noisy weight data under different diffusion step sizes. Using historical observation images corresponding to different diffusion step sizes as conditional observation spatial data, and training based on noisy weighted data, a multimodal action sequence is obtained; Based on the multimodal action sequence, integral reconstruction is performed to obtain the predicted end-effector pose trajectory and the predicted time-varying contact stiffness trajectory. The preset loss function value is calculated based on the predicted end-effector pose trajectory, the predicted time-varying contact stiffness trajectory, and historical robot trajectory information, and it is then determined whether the loss function value has converged. If yes, the trained diffusion model is obtained; otherwise, the current noise component is updated based on the loss function value.

[0056] Specifically, the noisy weight data represents the weight data obtained by adding Gaussian noise to the original clean weight matrix according to the noise component at the current diffusion step size. The larger the diffusion step size, the higher the noise proportion, and the closer the weight data is to a pure Gaussian distribution. The multimodal action sequence is a series of different GDMP weight matrix sequences output by the diffusion model under observation constraints through reverse denoising. These correspond to various reasonable contact operation strategies under the same visual scene, such as different approach paths and different stiffness variation patterns. Integral reconstruction refers to the process of integrating the weight matrix output by the diffusion model online through the second-order dynamic system of the GDMP model, restoring the low-dimensional weight matrix to a high-dimensional continuous end-effector pose trajectory and time-varying contact stiffness trajectory. Noise component update refers to adjusting the noise variance scheduling strategy of subsequent diffusion steps based on the convergence of the loss function. When the loss convergence is slow, the decay rate of the noise variance is reduced to accelerate model convergence.

[0057] Schematic, the weight matrix encoded by the GDMP model is used as the prediction target (action input) of the diffusion model, and the paired historical observation images and robot trajectory information are used as the conditional observation space; for each diffusion step t1, isotropic Gaussian noise is superimposed on the clean weight matrix based on the noise component corresponding to the current diffusion step, and the calculation follows the forward diffusion formula: ,in This is the original weight matrix. For the current noise component, Using preset noise scheduling coefficients, the noisy weighted data at diffusion step size t1 is finally obtained. .

[0058] The historical observation image corresponding to the current diffusion step size, the diffusion time step encoding, and the noisy weight data are jointly input into the U-Net backbone network of the diffusion model. Under the constraint of the historical observation image, the model predicts the noise component added at the current step size. Through inverse denoising, multiple different weight matrices are obtained, i.e., multimodal action sequences, corresponding to various contact operation strategies. Each weight matrix output by the diffusion model is input into the second-order dynamic system of the GDMP model for continuous-time online integration. The Euclidean space solution is mapped back to the corresponding Riemannian manifold through exponential mapping to obtain the predicted end-effector pose trajectory and predicted time-varying contact stiffness trajectory. The low-dimensional weight matrix is ​​restored to verifiable trajectory and stiffness data, providing a basis for loss function calculation and verifying the rationality of the operation strategy generated by the model.

[0059] The fitting error between the predicted trajectory and historical trajectory information is calculated, and combined with the mean squared error of noise prediction, the total loss function value is obtained. It is then determined whether the fluctuation of the loss function value over 100 consecutive training rounds is less than a preset threshold; if so, convergence is considered achieved. If the loss function converges, the model parameters are saved, resulting in a trained diffusion model. If convergence fails, the noise variance scheduling strategy for subsequent diffusion steps is adjusted based on the convergence of the loss function. For example, when the loss decreases slowly, the decay step size of the noise variance is increased to accelerate model convergence, and then the next round of diffusion training iterations begins.

[0060] To illustrate, the noise scheduling strategy adopts a cosine noise scheduling strategy, which, compared with linear scheduling, can maintain more stable noise attenuation in the middle of the diffusion step, thus improving the model's generation performance. The diffusion model network structure adopts a 6-layer U-Net structure, including downsampling, upsampling, and residual connections. At the same time, a time-step encoding module is introduced to encode the diffusion step as a feature vector and integrate it into the network, thereby improving the noise prediction accuracy. The generation of multimodal action sequences can generate at least 10 different weight matrices under the same observation conditions by setting different random noise seeds, corresponding to 10 different contact operation strategies.

[0061] Preferably, the loss function values ​​include: the mean square error between the predicted noise component and the current noise component, the first trajectory fitting error between the predicted end-effector pose trajectory and the historical robot trajectory information, and the second trajectory fitting error between the predicted time-varying contact stiffness trajectory and the historical robot trajectory information.

[0062] Specifically, the mean squared error (MSE) is the error between the noise component predicted by the diffusion model and the actual added noise component. It is the core loss term for training the diffusion model and is used to optimize the model's noise prediction accuracy. The first trajectory fitting error is the MSE between the predicted end-effector pose trajectory obtained by restoring the weight matrix generated by the diffusion model and the historical end-effector pose trajectory. It is used to constrain the consistency between the trajectory generated by the model and the original trajectory. The second trajectory fitting error is the MSE between the predicted time-varying contact stiffness trajectory obtained by restoring the weight matrix generated by the diffusion model and the historical contact stiffness trajectory. It is used to constrain the consistency of the stiffness law generated by the model.

[0063] S504. Based on the trained diffusion model and the geometric dynamic motion primitive model, a trained trajectory generation model is constructed.

[0064] Specifically, the trained diffusion model and the GDMP model are combined to form a complete trajectory generation model. The initial noise component of the first diffusion training is preset to be standard Gaussian noise with a mean of 0 and a variance of 1, and the subsequent step noise follows a cosine variance scheduling strategy.

[0065] In a preferred embodiment of the present invention, such as Figure 3As shown, the historical robot trajectory information, after being encoded by the Geometric Dynamic Motion Primitive Model (G-DMP), is uniformly represented as a weight matrix. The weight matrix is ​​used to compactly represent the robot's contact operation skill, including three parts: end-effector translational trajectory, rotational attitude trajectory, and contact stiffness trajectory. The translational trajectory is represented by Euclidean space basis function weights, the rotational attitude trajectory is obtained by performing a logarithmic mapping on the SO(3) manifold, and the contact stiffness trajectory is obtained by performing a Riemann logarithmic mapping and vectorization on the SPD symmetric positive definite matrix manifold. Finally, the above three weights are concatenated along the feature dimension to form a unified skill parameter vector, denoted as W.

[0066] Corresponding to the action input, each historical robot trajectory information includes historical observation images, specifically environmental observation information (o). The data input to the diffusion model can be represented as observation-action parameter pairs (o, W). For example... Figure 4 As shown, Gaussian white noise is added to the real data W, and the noise term is predicted. A trajectory is generated by progressively denoising through a reverse process, resulting in noisy skill parameters. Then, the current observation information will be... Diffusion time step encoding 2 and noisy skill parameters Collaborative learning to predict the noise component added at the current time step The diffusion model is trained end-to-end using the mean square error between predicted and actual noise as the objective function. Through this training process, the diffusion model gradually learns the conditional probability distribution of the skill parameter weight matrix under given environmental observation conditions. This allows us to characterize multiple contact operation strategies that may exist in the same visual scene.

[0067] In a preferred embodiment of the present invention, the diffusion model does not directly generate the robot's original trajectory points at each sampling moment, but instead generates trajectory parameters encoded by the Geometric Dynamic Motion Primitives (G-DMP). Specifically, the trajectory parameters are weight matrices that provide a compact representation of the entire trajectory, including information on the end-effector position trajectory, rotational attitude trajectory, and contact stiffness trajectory. Therefore, they can serve as a unified parameterized representation of the robot's contact manipulation skills. In other words, the training objective of the diffusion model is to learn and generate a G-DMP weight parameter distribution that matches the current visual observation information.

[0068] In a preferred embodiment of the present invention, such as Figure 5As shown, the admittance controller in Cartesian space converts external forces or torques acting on the robot's end effector into the displacement, velocity, and acceleration responses that the end effector should produce. Essentially, it imagines the robot's end effector as a virtual mechanical system with a second-order mass-spring-damped system, where the mass is M1, the stiffness is K1, and the damping is D1. For example, if positioning errors caused by lighting effects lead to premature contact with the environment, the force information will regulate the robot to retreat and move away from the environment. The admittance control model is... ,in For a mass matrix with fixed parameters, is the stiffness matrix, generated by the G-DMP above, and s is the task progress variable. The damping inertia matrix ensures that the system exhibits a constant damping ratio, resulting in force-controlled stability. Indicates the translational position of the end effector. Indicates the desired location in the plan. and Indicates translational velocity and acceleration. and Represents angular velocity and angular acceleration. This represents the rotation error of the current attitude relative to the desired attitude. , Indicates external force. This represents the external torque. When the robot's end effector is subjected to an external force or torque, its pose no longer rigidly follows the reference trajectory, but instead deviates according to a virtual mass-spring-damped system. This deviation is then tracked by the lower-level position controller or inverse kinematics. Essentially, a virtual six-dimensional mass-damped-spring system is constructed at the robot's end effector. If the current end effector pose deviates from the desired pose, a restoring force is generated by the stiffness term; once the end effector moves, the damping term provides dissipation to suppress oscillations; external contact forces and torques serve as system inputs, driving the virtual system to generate displacement, velocity, and acceleration responses. Since three-dimensional pose does not belong to ordinary Euclidean space, pose errors cannot be directly subtracted. Therefore, the formula uses a logarithmic mapping to calculate the error, thus forming a unified six-dimensional error vector together with the position error.

[0069] This embodiment can generate multiple feasible strategies for the same operational task. When faced with perception errors or environmental disturbances, it can quickly switch execution schemes through resampling, significantly reducing the risk of task failure due to initial trajectory mismatch. By introducing dynamic constraints during trajectory generation through G-DMP, the trajectory possesses continuity and stable convergence characteristics, avoiding oscillations or instability issues in the contact phase of traditional interpolated trajectories, thereby improving overall operational stability. A diffusion model is used to establish a mapping relationship between visual observation and trajectory parameters, enabling the robot to adaptively generate operational strategies based on real-time environmental perception results. This eliminates the need to design dedicated tooling or predefined trajectories for different types of power cabinets or operational objects, significantly improving the system's versatility under multiple scenarios and tasks. Through G-DMP's unified modeling method based on manifold space, it can simultaneously process various heterogeneous data such as position, attitude, and stiffness, enhancing the model's expressive ability for complex tasks. By parametrically compressing the trajectory using G-DMP, the high-dimensional trajectory representation is transformed into low-dimensional weight parameters, reducing the learning and inference complexity of the diffusion model and improving model training efficiency and online generation speed. This approach reduces reliance on specialized end-effectors and complex parameter tuning processes, lowering system deployment and maintenance costs. Furthermore, by improving operational success rates and automation levels, it effectively reduces manual intervention, enhancing the overall efficiency of power line inspection and maintenance operations, demonstrating significant engineering application value and promising prospects. The trajectory pose SE3 group and contact stiffness SPD matrix are unified into Euclidean space for dynamic system modeling through logarithmic and exponential mappings. Unlike existing technologies that separate trajectory and force control, in this embodiment, stiffness is not a post-parameter of the controller but rather the contact stiffness generated synchronously during trajectory generation, thereby improving the smoothness and stability of robot control.

[0070] Indicatively, such as Figure 6 As shown, during actual execution, the robot will adaptively handle disturbances. The stiffness trajectory generated by G-DMP can automatically reduce stiffness and improve compliance during the contact phase, thereby avoiding jamming or excessive impact. If the current trajectory execution fails, a new trajectory can be sampled from the diffusion model. It allows for quick switching to another operational strategy, such as changing the end stiffness or path.

[0071] By implementing this embodiment, a trajectory generation model that includes a diffusion model and a geometric dynamic motion primitive model is constructed. This overcomes the limitation of focusing solely on geometric path planning and simultaneously achieves the prediction and generation of end-effector pose trajectory and time-varying contact stiffness trajectory. The force interaction characteristics between the robot to be controlled and the environment are incorporated into the entire process modeling of contact operation, realizing the integrated fusion of motion trajectory planning and contact force adaptation. Through the geometric dynamic motion primitive model, historical end-effector positions, historical rotational postures, and historical contact stiffness matrices are encoded, mapped, and projected to obtain a weight matrix that can fully represent the robot's contact operation skills. Further diffusion training of the diffusion model is then completed, achieving a unified parameterized representation of motion characteristics and force control characteristics in contact operation. This ensures that the generated contact trajectory and corresponding contact stiffness can accurately match the differentiated needs of the contact operation process. At the same time, combined with the real-time acquired end-effector contact force, closed-loop control of the robot's contact operation is completed, adapting online to environmental changes and uncertain disturbances during the contact process. This solves the problem of insufficient adaptability of fixed planned trajectories to uncertain disturbances and improves the success rate of robot operations in contact scenarios.

[0072] See Figure 7 This is a schematic diagram of a robot contact control device based on geometric dynamic motion primitives according to an embodiment of the present invention, comprising: The real-time data acquisition module is used to acquire real-time observation images and end-effector contact forces when the robot under control comes into contact with the target object. The motion trajectory prediction module is used to input the real-time observed image into a preset trajectory generation model to predict the motion trajectory, thereby obtaining the end pose trajectory and the time-varying contact stiffness trajectory. The contact information generation module is used to perform joint trajectory encoding and solving based on the end pose trajectory and the time-varying contact stiffness trajectory to obtain the contact trajectory and the corresponding contact stiffness. The robot control module is used to control the robot under control to perform contact correction based on the end contact force, contact trajectory and corresponding contact stiffness. The model training module is used to train the trajectory generation model; the trajectory generation model includes a diffusion model and a geometric dynamic motion primitive model. The training of the trajectory generation model includes: Acquire historical observation images and historical robot trajectory information; the historical robot trajectory information includes historical end-effector position, historical rotational attitude, and historical contact stiffness matrix. Based on the geometric dynamic motion primitive model, the historical end position, historical rotational posture and historical contact stiffness matrix are encoded, mapped and projected to obtain a weight matrix for characterizing the robot's contact operation skills. Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, the diffusion training operation is repeatedly performed on the diffusion model based on the current noise component until the preset loss function value converges, thus obtaining the trained diffusion model and geometric dynamic motion primitive model. Based on the trained diffusion model and the geometric dynamic motion primitive model, a trained trajectory generation model is constructed.

[0073] Preferably, the diffusion training operation includes: Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, Gaussian noise is superimposed on the weight matrix based on the current noise component to obtain noisy weight data under different diffusion step sizes. Using historical observation images corresponding to different diffusion step sizes as conditional observation spatial data, and training based on noisy weighted data, a multimodal action sequence is obtained; Based on the multimodal action sequence, integral reconstruction is performed to obtain the predicted end-effector pose trajectory and the predicted time-varying contact stiffness trajectory. The preset loss function value is calculated based on the predicted end-effector pose trajectory, the predicted time-varying contact stiffness trajectory, and historical robot trajectory information, and it is then determined whether the loss function value has converged. If yes, the trained diffusion model is obtained; otherwise, the current noise component is updated based on the loss function value.

[0074] This invention provides a robot contact control device based on geometric dynamic motion primitives. By constructing a trajectory generation model including a diffusion model and a geometric dynamic motion primitive model, it overcomes the limitations of focusing solely on geometric path planning, simultaneously predicting and generating end-effector pose trajectories and time-varying contact stiffness trajectories. It incorporates the force interaction characteristics between the robot and its environment into the entire contact operation modeling process, achieving integrated fusion of motion trajectory planning and contact force adaptation. Through encoding, mapping, and projection fitting of historical end-effector positions, historical rotational postures, and historical contact stiffness matrices using the geometric dynamic motion primitive model, a weight matrix that fully characterizes the robot's contact operation skills is obtained. Further diffusion training of the diffusion model is then performed, achieving a unified parameterized representation of motion and force control characteristics in contact operations. This ensures that the generated contact trajectory and corresponding contact stiffness accurately match the differentiated requirements of the contact operation process. Simultaneously, combined with real-time acquired end-effector contact forces, closed-loop control of the robot's contact operations is achieved, adapting online to environmental changes and uncertain disturbances during the contact process. This addresses the problem of insufficient adaptability of fixed-planned trajectories to uncertain disturbances, improving the success rate of robot operations in contact scenarios.

[0075] It should be noted that the device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Furthermore, in the accompanying drawings of the device embodiments provided by this invention, the connection relationships between modules indicate that they have communication connections, which can be specifically implemented as one or more communication buses or signal lines. Those skilled in the art can understand and implement this without any creative effort.

[0076] Those skilled in the art will understand that, for convenience and brevity, the specific working process of the device described above can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0077] Another embodiment of the present invention provides a terminal device, including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, it implements a robot contact control method based on geometric dynamic motion primitives as described in the above embodiments. The terminal device may be a desktop computer, laptop, handheld computer, or cloud server, etc. The terminal device may include, but is not limited to, a processor and a memory.

[0078] The processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor. The processor is the control center of the terminal device, connecting all parts of the terminal device via various interfaces and lines.

[0079] The memory can be used to store the computer program. The processor implements various functions of the terminal device by running or executing the computer program stored in the memory and calling data stored in the memory. The memory may mainly include a program storage area and a data storage area. The program storage area may store the operating system, at least one application program required for a function, etc.; the data storage area may store data created based on the use of the mobile phone, etc. In addition, the memory may include high-speed random access memory, and may also include non-volatile memory, such as hard disk, RAM, plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, at least one disk storage device, flash memory device or other volatile solid-state storage device.

[0080] Another embodiment of the present invention provides a computer-readable storage medium including a stored computer program, wherein, when the computer program is executed, it controls the device where the computer-readable storage medium is located to execute the robot contact control method based on geometric dynamic motion primitives described in the above embodiment.

[0081] The storage medium is a computer-readable storage medium, and the computer program is stored in the computer-readable storage medium. When the computer program is executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable file, or some intermediate form. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drive, portable hard drive, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc.

[0082] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A robot contact control method based on geometric dynamic motion primitives, characterized in that, include: Acquire real-time observation images and end-effector contact force when the robot under control comes into contact with the target object; The real-time observed image is input into a preset trajectory generation model to predict the motion trajectory, thereby obtaining the end pose trajectory and the time-varying contact stiffness trajectory. The contact trajectory and the corresponding contact stiffness are obtained by performing joint trajectory encoding and solving based on the end pose trajectory and the time-varying contact stiffness trajectory. Based on the end contact force, contact trajectory, and corresponding contact stiffness, the robot under control is controlled to perform contact correction. The trajectory generation model includes a diffusion model and a geometric dynamic motion primitive model. The training of the trajectory generation model includes: Acquire historical observation images and historical robot trajectory information; the historical robot trajectory information includes historical end-effector position, historical rotational attitude, and historical contact stiffness matrix. Based on the geometric dynamic motion primitive model, the historical end position, historical rotational posture and historical contact stiffness matrix are encoded, mapped and projected to obtain a weight matrix for characterizing the robot's contact operation skills. Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, the diffusion training operation is repeatedly performed on the diffusion model based on the current noise component until the preset loss function value converges, thus obtaining the trained diffusion model and geometric dynamic motion primitive model. Based on the trained diffusion model and the geometric dynamic motion primitive model, a trained trajectory generation model is constructed.

2. The robot contact control method based on geometric dynamic motion primitives as described in claim 1, characterized in that, The diffusion training operation includes: Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, Gaussian noise is superimposed on the weight matrix based on the current noise component to obtain noisy weight data under different diffusion step sizes. Using historical observation images corresponding to different diffusion step sizes as conditional observation spatial data, and training based on noisy weighted data, a multimodal action sequence is obtained; Based on the multimodal action sequence, integral reconstruction is performed to obtain the predicted end-effector pose trajectory and the predicted time-varying contact stiffness trajectory. The preset loss function value is calculated based on the predicted end-effector pose trajectory, the predicted time-varying contact stiffness trajectory, and historical robot trajectory information, and it is then determined whether the loss function value has converged. If yes, the trained diffusion model is obtained; otherwise, the current noise component is updated based on the loss function value.

3. The robot contact control method based on geometric dynamic motion primitives as described in claim 1, characterized in that, The end effector pose trajectory includes: end effector translation trajectory and rotational pose trajectory; Based on the end-effector pose trajectory and the time-varying contact stiffness trajectory, joint trajectory encoding and solving are performed to obtain the contact trajectory and the corresponding contact stiffness, including: After temporally coupling and aligning the end translation trajectory, the rotational attitude trajectory, and the time-varying contact stiffness trajectory, the aligned end translation trajectory is mapped to Euclidean space, the aligned rotational attitude trajectory is mapped to a three-dimensional special orthogonal group manifold space, and the aligned time-varying contact stiffness trajectory is mapped to a symmetric positive definite manifold space, thereby obtaining the corresponding translational features, rotational attitude features, and contact stiffness features, respectively. Based on the preset Gaussian function, the translation features, rotational attitude features and contact stiffness features are jointly encoded to obtain the shape forcing term used to characterize the target contact operation; The second-order dynamic online integral is performed based on the shape forcing term to obtain the pose and stiffness solutions in Euclidean space. An exponential mapping operation is performed based on the pose and stiffness solutions to obtain the contact trajectory and corresponding contact stiffness.

4. The robot contact control method based on geometric dynamic motion primitives as described in claim 3, characterized in that, The weight matrix includes: the first weight component corresponding to the end translation trajectory, the second weight component corresponding to the rotational attitude trajectory, and the third weight component corresponding to the time-varying contact stiffness trajectory; Based on the current noise components and the geometric dynamic motion primitive model, the historical end-effector position, historical rotational posture, and historical contact stiffness matrix are encoded, mapped, and projected to obtain a weight matrix for characterizing the robot's contact operation skills, including: The historical end position is integrated into the historical end translation trajectory, the historical rotation attitude is integrated into the historical rotation attitude trajectory, and the historical contact stiffness matrix is ​​integrated into the historical time-varying contact stiffness trajectory. After performing a logarithmic mapping operation on the historical end translation trajectory, historical rotational attitude trajectory, and historical time-varying contact stiffness trajectory to Euclidean space, the current noise component, the mapped historical end translation trajectory, historical rotational attitude trajectory, and historical time-varying contact stiffness trajectory are input into the geometric dynamic motion primitive model for projection fitting, and the corresponding first weight component, second weight component, and third weight component are obtained respectively. By concatenating the first, second, and third weight components, a weight matrix is ​​obtained to characterize the robot's contact operation skills.

5. The robot contact control method based on geometric dynamic motion primitives as described in claim 1, characterized in that, Based on the end-effector contact force, contact trajectory, and corresponding contact stiffness, the robot under control is controlled to perform contact correction, including: The end contact force, contact trajectory, and corresponding contact stiffness are input to the preset admittance controller. The contact trajectory is used as the desired reference trajectory, and the contact stiffness is used as the real-time stiffness parameter. The end pose correction is calculated based on the end contact force, the preset inertia matrix, and the damping matrix. The desired reference trajectory is adjusted based on the end-effector pose correction amount, and the robot under control is controlled to perform contact correction based on the adjusted desired reference trajectory.

6. The robot contact control method based on geometric dynamic motion primitives as described in claim 1, characterized in that, The loss function values ​​include: the mean square error between the predicted noise component and the current noise component, the first trajectory fitting error between the predicted end-effector pose trajectory and the historical robot trajectory information, and the second trajectory fitting error between the predicted time-varying contact stiffness trajectory and the historical robot trajectory information.

7. A robot contact control device based on geometric dynamic motion primitives, characterized in that, include: The real-time data acquisition module is used to acquire real-time observation images and end-effector contact forces when the robot under control comes into contact with the target object. The motion trajectory prediction module is used to input the real-time observed image into a preset trajectory generation model to predict the motion trajectory, thereby obtaining the end pose trajectory and the time-varying contact stiffness trajectory. The contact information generation module is used to perform joint trajectory encoding and solving based on the end pose trajectory and the time-varying contact stiffness trajectory to obtain the contact trajectory and the corresponding contact stiffness. The robot control module is used to control the robot under control to perform contact correction based on the end contact force, contact trajectory and corresponding contact stiffness. The model training module is used to train the trajectory generation model; the trajectory generation model includes a diffusion model and a geometric dynamic motion primitive model. The training of the trajectory generation model includes: Acquire historical observation images and historical robot trajectory information; the historical robot trajectory information includes historical end-effector position, historical rotational attitude, and historical contact stiffness matrix. Based on the geometric dynamic motion primitive model, the historical end position, historical rotational posture and historical contact stiffness matrix are encoded, mapped and projected to obtain a weight matrix for characterizing the robot's contact operation skills. Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, the diffusion training operation is repeatedly performed on the diffusion model based on the current noise component until the preset loss function value converges, thus obtaining the trained diffusion model and geometric dynamic motion primitive model. Based on the trained diffusion model and the geometric dynamic motion primitive model, a trained trajectory generation model is constructed.

8. The robot contact control device based on geometric dynamic motion primitives as described in claim 7, characterized in that, The diffusion training operation includes: Using the weight matrix as the action input and historical observation images and historical robot trajectory information as the observation space, Gaussian noise is superimposed on the weight matrix based on the current noise component to obtain noisy weight data under different diffusion step sizes. Using historical observation images corresponding to different diffusion step sizes as conditional observation spatial data, and training based on noisy weighted data, a multimodal action sequence is obtained; Based on the multimodal action sequence, integral reconstruction is performed to obtain the predicted end-effector pose trajectory and the predicted time-varying contact stiffness trajectory. The preset loss function value is calculated based on the predicted end-effector pose trajectory, the predicted time-varying contact stiffness trajectory, and historical robot trajectory information, and it is then determined whether the loss function value has converged. If yes, the trained diffusion model is obtained; otherwise, the current noise component is updated based on the loss function value.

9. A terminal device, characterized in that, The system includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein the processor, when executing the computer program, implements a robot contact control method based on geometric dynamic motion primitives as described in any one of claims 1 to 6.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium includes a stored computer program, wherein, when the computer program is executed, it controls the device containing the computer-readable storage medium to perform a robot contact control method based on geometric dynamic motion primitives as described in any one of claims 1 to 6.

Images