Robot skill learning and control method and device for rich-contact operation task

Abstract

The application provides a robot skill learning and control method and device for a rich contact operation task. The method provided by the application comprises: collecting human demonstration data; the human demonstration data at least comprises end effector pose data and stiffness matrix data; based on the end effector pose data and the stiffness matrix data, a generalized motion trajectory and a generalized stiffness trajectory are obtained through time alignment, probability distribution modeling and new state constraint adaptation; an obstacle model is constructed through environment perception; candidate trajectories are sampled with the generalized motion trajectory and the generalized stiffness trajectory as references; collision-free trajectories are screened and iteratively optimized in combination with motion consistency, stiffness consistency and smoothness cost to obtain optimized motion trajectories and optimized stiffness trajectories; the optimized motion trajectories and the optimized stiffness trajectories are converted into robot joint torque instructions based on variable impedance control; and the robot is controlled to perform a rich contact operation task based on the robot joint torque instructions.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and in particular to methods and apparatus for robot skill learning and control for tasks involving rich contact operations. Background Technology

[0002] In fields such as industrial assembly, agricultural product harvesting, and medical assistance, the demands for coordinated precision, compliance, and safety in robot-intensive contact manipulation tasks are becoming increasingly stringent. For example, fruit harvesting requires avoiding obstacles like tree branches while using compliant force control to prevent fruit damage, while industrial assembly requires precise alignment while adjusting stiffness to accommodate part tolerances. Therefore, research on robot skill learning and control in intensive contact manipulation tasks has become a trend.

[0003] Currently, robot skill learning and control are mainly achieved through two technical paths: First, demonstration learning (LfD) technology, which collects the motion trajectories demonstrated by humans (such as end effector pose sequences) and uses methods such as dynamic motion primitives (DMP) and Gaussian mixture models (GMM) to perform trajectory modeling and generalization, thereby reproducing human motion intentions; Second, impedance control technology, which constructs the force-motion response relationship between the robot and the environment by presetting fixed or segmented stiffness and damping parameters, thereby achieving compliant control during the contact process. However, existing methods still have three shortcomings: First, the skill learning dimension is singular. Traditional demonstration learning often focuses only on the reproduction of motion trajectories, ignoring stiffness, a key feature that determines contact compliance. Although joint learning is attempted, it is difficult to handle the heterogeneous data characteristics of motion and stiffness, resulting in a large deviation between the generalized stiffness trajectory and the human demonstration. Second, the environmental adaptability is weak. Existing obstacle avoidance planning methods are mostly based on motion trajectory optimization and do not incorporate stiffness characteristics into obstacle avoidance constraints. When unknown obstacles not covered by the demonstration appear in the scene, the optimized trajectory is prone to losing the compliant contact characteristics of the human demonstration. Third, it is difficult to balance control accuracy and compliance. Traditional impedance control uses fixed or coarsely segmented impedance parameters, which cannot adapt to the dynamically changing contact requirements in contact-rich tasks. Moreover, dynamic modeling often ignores the additional inertia of the end effector, leading to deviations in control force derivation and causing shaking or excessive impact when the robot comes into contact with the environment.

[0004] Therefore, there is an urgent need for a method to improve the motion accuracy and contact compliance of robots in contact-intensive tasks. Summary of the Invention

[0005] In view of this, this application provides a robot skill learning and control method and apparatus for contact-rich operation tasks, in order to improve the motion accuracy and contact compliance of robots in contact-rich operation tasks.

[0006] Specifically, this application is implemented through the following technical solution:

[0007] The first aspect of this application provides a method for robot skill learning and control for tasks involving rich contact operations, the method comprising:

[0008] Collect human demonstration data; the human demonstration data includes at least end effector pose data and stiffness matrix data;

[0009] Based on the end effector pose data and stiffness matrix data, the generalized motion trajectory and generalized stiffness trajectory are obtained through time alignment, probability distribution modeling and new state constraint adaptation.

[0010] An obstacle model is constructed through environmental perception. The generalized motion trajectory and generalized stiffness trajectory are used as references to sample candidate trajectories. The collision-free trajectories are screened by combining motion consistency, stiffness consistency and smoothness cost and iterative optimization to obtain the optimized motion trajectory and optimized stiffness trajectory.

[0011] Based on variable impedance control, the optimized motion trajectory and optimized stiffness trajectory are converted into robot joint torque commands, and the robot is controlled to perform contact-rich operation tasks based on the robot joint torque commands.

[0012] A second aspect of this application provides a robot skill learning and control device for contact-rich operation tasks, the device comprising a data acquisition module, a processing module, and a control module;

[0013] The acquisition module is used to acquire human demonstration data; the human demonstration data includes at least end effector pose data and stiffness matrix data.

[0014] The processing module is used to obtain the generalized motion trajectory and stiffness trajectory based on the end effector pose data and stiffness matrix data through time alignment, probability distribution modeling and new state constraint adaptation.

[0015] The processing module is also used to construct an obstacle model through environmental perception, sample candidate trajectories with the generalized motion trajectory and stiffness trajectory as references, and filter collision-free trajectories by combining motion consistency, stiffness consistency and smoothness cost and iteratively optimize to obtain optimized motion trajectory and optimized stiffness trajectory.

[0016] The control module is used to convert the optimized motion trajectory and optimized stiffness trajectory into robot joint torque commands based on variable impedance control, and to control the robot to perform contact-rich operation tasks based on the robot joint torque commands.

[0017] The robot skill learning and control method and apparatus provided in this application for contact-rich operation tasks, in the first aspect, collects human demonstration data to provide real operation samples for subsequent processes. Based on this, the time dimension of multiple sets of demonstrations is unified through time alignment. The inherent statistical laws of motion and stiffness are mined by probability distribution modeling. Then, a generalized trajectory is generated by adapting to new state constraints. Next, candidate trajectories are sampled based on the generalized trajectory. An obstacle model is constructed through environmental perception and the optimized trajectory is screened by multiple cost functions. Finally, the optimized trajectory is converted into joint torque commands based on variable impedance control. The whole process is progressive, which allows the robot to accurately reproduce the motion and stiffness coordination logic of human operation and achieve compliant force interaction through variable impedance in contact-rich scenarios. In the end, the robot achieves precise and compliant control. Secondly, time alignment can eliminate the inconsistency in time dimension caused by differences in operation speed among multiple human demonstrations, enabling subsequent modeling to be based on synchronized time series; probability distribution modeling can capture the typical patterns and changes in motion trajectory and stiffness matrix in human demonstrations, and uncover the potential correlation between "where to move and how to adjust stiffness"; new state constraint adaptation allows the generalized trajectory to break away from the specific scenario of the original demonstration and be adjusted according to constraints such as the waypoints of the new task, so that the robot can quickly adapt and generate reasonable motion and stiffness reference trajectories without the need for humans to repeat demonstrations for new states, providing a "general operation template" with scene transfer capabilities for subsequent control. Thirdly, by combining obstacle models built through environmental perception with collision detection, collision-free trajectories can be selected from a large number of candidate trajectories, avoiding interference between the robot and obstacles in the environment, and preventing damage to itself or the object being operated. The costs of motion consistency and stiffness consistency ensure that the optimized trajectory is consistent with the core motion patterns and stiffness adjustment logic demonstrated by humans, inheriting the essence of human operation skills. The cost of smoothness can eliminate abrupt changes in the trajectory (such as sudden stops and turns, and sudden increases and decreases in stiffness), making the robot's movement more stable and stiffness changes more compliant, providing a "safe and human-friendly" trajectory input for subsequent variable impedance control, and improving the reliability and operational quality of robot control in contact-rich tasks. Attached Figure Description

[0018] Figure 1 A flowchart of a robot skill learning and control method for contact-rich operation tasks provided in Embodiment 1 of this application;

[0019] Figure 2 This is a schematic diagram of the structure of a robot skill learning and control device for contact-rich operation tasks provided in Embodiment 2 of this application. Detailed Implementation

[0020] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application.

[0021] The terminology used in this application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. The singular forms “a,” “the,” and “the” used herein are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used herein refers to and includes any and all possible combinations of one or more of the associated listed items.

[0022] It should be understood that although the terms first, second, third, etc., may be used in this application to describe various information, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from one another. For example, without departing from the scope of this application, first information may also be referred to as second information, and similarly, second information may also be referred to as first information. Depending on the context, the word "if" as used herein may be interpreted as "when," "when," or "in response to determination."

[0023] The following specific embodiments are given to illustrate the technical solution of this application in detail.

[0024] Figure 1 This is a flowchart illustrating a robot skill learning and control method for contact-rich operation tasks provided in Embodiment 1 of this application. Please refer to... Figure 1 The method provided in this embodiment may include:

[0025] S101. Collect human demonstration data.

[0026] The human demonstration data includes at least end effector pose data and stiffness matrix data.

[0027] Specifically, human demonstration data refers to a set of key data collected through sensor devices that relates to the motion state and contact compliance of the robot's end effector during the completion of target contact-rich tasks (such as industrial assembly, agricultural product harvesting, and medical assistance). The core role of human demonstration data is to provide robots with "learning models." By modeling, generalizing, and optimizing human demonstration data, robots can reproduce the accuracy (motion trajectory) and compliance (contact stiffness) of human operations, thereby adapting to the needs of complex contact-rich scenarios.

[0028] Furthermore, the human demonstration data includes at least end-effector pose data and stiffness matrix data. The end-effector pose data describes the dynamic sequence of position and orientation of the robot's end-effector (the part that directly contacts the target object / environment, such as a gripper or tool) in three-dimensional Cartesian space. The stiffness matrix data describes the robot's end-effector's "resistance to deformation" against external contact forces / torques during contact-rich processes.

[0029] In practical implementation, a demonstration system is built, including a robot, a 6-axis force / torque sensor, a depth camera, and data acquisition software. The force / torque sensor is fixed to the robot's end effector, and the pose relationship between the sensor coordinate system and the robot's base coordinate system is calibrated. A contact-rich task scenario is set up (e.g., assembling parts, harvesting fruit and tree branches). The sampling frequency is set using the data acquisition software, with the end effector pose data sampling frequency no less than 1kHz (calculated in real-time by the robot's joint encoder) and the stiffness matrix data sampling frequency no less than 800Hz (derived from force / torque sensor data). The data storage format is set to ensure that the timestamps of the pose data and stiffness matrix data are synchronized. A human operator, using a robot teach pendant or force-guided method, manipulates the robot's end effector to complete the contact-rich task (e.g., picking parts from the parts placement area and assembling them to a designated position, harvesting fruit from a tree branch). During the operation, the operator dynamically adjusts the end effector's speed and contact force based on human experience to ensure the operation meets task requirements (e.g., assembly without jamming, fruit without damage). During human operation, the data acquisition software acquires joint encoder data in real time through the robot controller and calculates the pose data of the end effector by combining it with the robot's forward kinematics model. At the same time, it acquires contact force / torque data between the end effector and the environment in real time through force / torque sensors, and derives real-time stiffness matrix data based on a preset impedance identification algorithm (such as the least squares method). Data timestamps are recorded in real time during the acquisition process to ensure that the pose data and stiffness matrix data correspond one-to-one.

[0030] S102. Based on the end effector pose data and stiffness matrix data, the generalized motion trajectory and generalized stiffness trajectory are obtained through time alignment, probability distribution modeling and new state constraint adaptation.

[0031] Specifically, the generalized motion trajectory and the generalized stiffness trajectory are robot operation reference trajectories that can be adapted to "non-demonstration scenarios" after being processed by specific technologies based on human demonstration data (end-effector pose data and stiffness matrix data). Together, they constitute the core execution basis for the robot's "motion accuracy" and "contact compliance" in contact-rich tasks.

[0032] In specific implementation, based on the end effector pose data and stiffness matrix data, the generalized motion trajectory and generalized stiffness trajectory are obtained through time alignment, probability distribution modeling, and adaptation to new state constraints, including:

[0033] (1) The global optimal reparameterization algorithm is used to perform time alignment on the end effector pose data and stiffness matrix data respectively to obtain time-synchronized aligned pose sequence and aligned stiffness sequence.

[0034] Specifically, the Global Optimal Reparameterization Algorithm (GORA) is used to perform time alignment on the end effector pose data and stiffness matrix data. For the pose data, the pose distance at different time points is calculated based on the double invariant metric of the Lie group SE(3); for the stiffness matrix data, the stiffness distance at different time points is calculated based on the logarithmic Euclidean distance in SPD space. A time warp cost matrix is ​​constructed, and the optimal time mapping path is searched through dynamic programming to minimize the total matching cost. Based on the optimal mapping relationship, the pose sequence and stiffness sequence are interpolated and resampled (the pose is interpolated using Lie group, and the stiffness is interpolated using logarithmic Euclidean), generating time-synchronized aligned pose and stiffness sequences.

[0035] For example, in one embodiment, the global optimal reparameterization process can be represented as:

[0036] ;

[0037] ;

[0038] Among them, the The total cost of time warping; For the stiffness matrix to vary The rate of change index; the The time warp rate; The stiffness matrix; The reparameterization result is the globally optimal result; Used to build from Time The mapping relationship; the It is the integral variable.

[0039] (2) For the aligned pose sequence, the relative increment of motion at adjacent time steps is defined by the group operation of the Lie group.

[0040] Specifically, for the aligned pose sequence, the relative increment of motion between adjacent time steps is defined by the group inverse operation and group multiplication of the Lie group SE(3), that is, the group operation difference between the pose of the next time step and the pose of the previous time step.

[0041] (3) For the aligned stiffness sequence, the relative stiffness increment of adjacent time steps is defined by logarithmic Euclidean operation.

[0042] Specifically, for the aligned stiffness sequence, the relative stiffness increment between adjacent time steps is defined by taking the logarithm of the stiffness matrix, subtracting the logarithm, and then taking the exponent through logarithmic Euclidean operation.

[0043] (4) Calculate the mean values ​​of the relative increment of motion and the relative increment of stiffness respectively, map the relative increment to the linear space and convert it into a vector, and calculate the local covariance of motion and the local covariance of stiffness based on the vector sample deviation.

[0044] In specific implementation, for the relative motion increments of multiple sets of demonstration data, the mean of the relative motion increments is calculated using the logarithmic Euclidean mean algorithm for Lie group space; for the relative stiffness increments of multiple sets of demonstration data, the mean of the relative stiffness increments is calculated using the logarithmic Euclidean mean algorithm for symmetric positive definite matrix space; for each relative motion increment, it is mapped to Lie algebra space through matrix logarithm, and then the Lie algebra elements are transformed into 6-dimensional vectors through vectorization operations; for each relative stiffness increment, it is mapped to symmetric matrix space through matrix logarithm, and the upper triangular elements of the symmetric matrix are extracted and transformed into 6-dimensional vectors; the deviation between the 6-dimensional vector corresponding to each relative motion increment and the mapped vector of the mean of the relative motion increment is calculated, and the deviation is subjected to an outer product operation and averaged to obtain the local covariance of motion; the deviation between the 6-dimensional vector corresponding to each relative stiffness increment and the mapped vector of the mean of the relative stiffness increment is calculated, and the deviation is subjected to an outer product operation and averaged to obtain the local covariance of stiffness.

[0045] Specifically, the relative motion increments of all adjacent time steps are extracted from multiple sets of demonstration data (e.g., the pose change Δg from time step i to i+1 in the k-th demonstration). i，i+1 k (Based on group multiplication and group inverse operation of Lie group SE(3)). For each motion relative increment Δg i，i+1 k Perform matrix logarithm operation log(Δg) i，i+1 k Map it to the space of the Lie algebra se(3) (to obtain elements in antisymmetric matrix form). Calculate the arithmetic mean of all Lie algebra elements: (1 / m)×Σlog(Δg) i，i+1 k (m is the number of demonstrations). Perform a matrix exponentiation operation exp(•) on this average value, map it back to the Lie group SE(3) space, and obtain the mean value μg of the relative motion increment. i，i+1 Extract the relative stiffness increments for all adjacent time steps from multiple sets of demonstration data (e.g., the stiffness change ΔS from time step i to i+1 in the k-th demonstration). i，i+1 kFor each relative stiffness increment ΔS i，i+1 k Perform matrix logarithm operation log(ΔS) i，i+1 k Map it to the symmetric matrix space S(3). Calculate the arithmetic mean of all elements of the symmetric matrix: (1 / m)×Σlog(ΔS) i，i+1 k Performing a matrix exponentiation operation exp(•) on this average value maps it back to the symmetric positive definite matrix space SPD(3), yielding the mean value μS of the relative stiffness increment. i，i+1 For each motion relative increment Δg i，i+1 k First, use the matrix logarithm log(Δg) ，i+1 k Map this to the Lie algebra se(3) (an antisymmetric matrix). Then perform vectorization operations on this antisymmetric matrix (representing the non-redundant elements [ω1, ω2, ω3, v1, v2, v3] of the 3×3 antisymmetric matrix). T Extracted as a 6-dimensional vector, resulting in vector v_g k The mean of the relative increment of motion, μg i，i+1 Perform the same operation to obtain the mean-mapped vector μ_vg. For each relative stiffness increment ΔS i，i+1 k First, use the matrix logarithm log(ΔS) i，i+1 k Map to the symmetric matrix space S(3) (a 3×3 symmetric matrix). Perform a semi-vectorization operation on this symmetric matrix (extract the upper triangular elements, according to [S 11 S 12 S 13 S 22 S 23 S 33 ] T (arranged in order), resulting in a 6-dimensional vector v_s k The mean μS of the relative stiffness increment. i，i+1 Perform the same operation (log first, then semi-vectorize) to obtain the mean-mapped vector μ_vs. For each vector v_g k Calculate the deviation from the mean mapping vector μ_vg: δg k =v_g k -μ_vg. For each deviation δg k Perform the outer product operation: δg k ×(δg k ) T (Resulting in a 6×6 matrix). Calculate the average of all outer product matrices: (1 / m)×Σ[δg k ×(δg k ) T], thus obtaining the local covariance Σg of motion. i，i+1 For each vector v_s k Calculate the deviation from the mean mapping vector μ_vs: δs k =v_s k -μ_vs. For each deviation δs k Perform the outer product operation: δs k ×(δs k ) T (This results in a 6×6 matrix). Calculate the average of all outer product matrices: (1 / m)×Σ[δs] k ×(δs k ) T The local covariance of stiffness Σs is obtained. i，i+1 .

[0046] For example, for time-aligned demonstration data, if the first... The stiffness matrix data for each time step are as follows: The logarithm of the stiffness at that time step, the Euclidean sample mean, can be calculated using the following formula. :

[0047] ;

[0048] Among them, the For the first The logarithmic Euclidean sample mean of the stiffness at each time step; The number of demonstrations; the This is the stiffness matrix data.

[0049] Define the group of symmetric positive definite matrices The relative increase in stiffness in is The logarithmic Euclidean mean is .

[0050] Through semi-vectorized mapping ,in The relative increments are transformed into vector form, and then the local covariance is calculated:

[0051] ;

[0052] in, For the first The time step to the The local covariance of stiffness between time steps; The number of demonstrations; the For the first The time step to the The mean of the relative stiffness increments between time steps; For the first The time step to the The relative increase in stiffness between time steps.

[0053] Under logarithmic Euclidean composition, the group It is an Abelian group, therefore the left and right increments are completely identical, and no adjoint transformation is needed. Furthermore, according to the definition of the logarithmic Euclidean metric, Therefore, the group of symmetric positive definite matrices middle .

[0054] (5) Map the alignment pose sequence and alignment stiffness sequence to centered coordinates, and combine the motion local covariance and stiffness local covariance to obtain the joint probability distribution of motion trajectory and stiffness trajectory.

[0055] In specific implementation, each pose element in the aligned pose sequence is mapped to pose coordinates through the group inverse operation with the pose mean, and each stiffness matrix in the aligned stiffness sequence is mapped to centralized stiffness coordinates through the difference between the logarithmic Euclidean distance and the stiffness mean. A topological structure of a Gaussian Markov random field is constructed with time steps as nodes, and the conditional probability dependency between adjacent time step nodes is defined by the motion local covariance and the stiffness local covariance. Based on the pose coordinates, centralized stiffness coordinates, and the conditional probability dependency between nodes, a joint probability density function containing the motion and stiffness coupling relationship is constructed, and the joint probability distribution of the motion trajectory and stiffness trajectory is determined based on the off-diagonal covariance term in the joint probability density function.

[0056] Specifically, the pose mean of the aligned pose sequence at each time step is calculated (based on the log-Euclidean mean algorithm of the Lie group SE(3)). For each pose element in the aligned pose sequence, the pose coordinates (located in the Lie algebra se(3) space) are mapped by the group inverse operation of the Lie group (multiplying the pose element by the group inverse of the pose mean). The stiffness mean of the aligned stiffness sequence at each time step is calculated (based on the log-Euclidean mean algorithm of the SPD space). For each stiffness matrix in the aligned stiffness sequence, the centered stiffness coordinates (located in the symmetric matrix space) are mapped by the log-Euclidean difference operation (subtracting the logarithm of the stiffness matrix from the logarithm of the stiffness mean). A chain-like topology is constructed in chronological order with time steps as nodes (each node is only connected to adjacent time step nodes). The conditional probability dependence strength of the pose nodes of adjacent time steps is calculated based on the local covariance of motion (represented by the inverse covariance matrix). The conditional probability dependence strength of the stiffness nodes of adjacent time steps is calculated based on the local stiffness covariance (represented by the inverse covariance matrix). The dependence strength of motion and stiffness is integrated to define the conditional probabilistic dependencies between adjacent time-step nodes. The pose coordinates of all time steps are stacked with the centered stiffness coordinates to form a joint state vector. Based on the conditional probabilistic dependencies between nodes, a precision matrix (containing motion-motion, stiffness-stiffness, and motion-stiffness coupling terms) of the joint state vector is constructed. The joint covariance matrix is ​​obtained by inverting the precision matrix, where off-diagonal elements characterize the dynamic correlation between motion and stiffness in the time dimension. Using the joint state vector as the variable, zero as the mean, and the joint covariance matrix as the parameter, a multivariate normal joint probability density function is constructed as the joint probability distribution of the motion trajectory and stiffness trajectory.

[0057] For example, in one embodiment, the first is defined The centered coordinates of the stiffness at each time step in the logarithmic Euclidean coordinate system are: By stacking the centered coordinates of all time steps, we can obtain Assuming the changes between adjacent time steps are small and satisfy a first-order Markov structure, the joint prior distribution is a zero-mean Gaussian Markov random field (GMRF):

[0058] ;

[0059] ;

[0060] Among them, the This is a vector formed by stacking the stiffness-related centered coordinates of all time steps; For stacked logarithmic stiffness vectors The accuracy matrix; the block structure of this matrix consists of local stiffness and local covariance. The decision is made that the boundary conditions are defined as follows: Within the logarithmic Euclidean framework, groups It is an Abelian group, with an accompanying transformation , It is an identity matrix.

[0061] (6) Input the preset path point pose constraints and path point stiffness constraints in the new state, update the joint probability distribution using Gaussian conditionalization, and generate the generalized motion trajectory and generalized stiffness trajectory.

[0062] In specific implementation, the preset pathpoint pose constraints in the new state are transformed into pose constraint parameters in Lie group space, and the pathpoint stiffness constraints are transformed into stiffness constraint parameters in symmetric positive definite matrix space. The pose constraint parameters and stiffness constraint parameters are mapped into constraint vectors in coordinate space to construct a linear measurement model containing constraint uncertainties. Based on the linear measurement model, the Kalman gain is calculated, and the joint probability distribution of the Gaussian Markov random field is updated posteriorly using the Kalman gain to obtain the posterior mean and posterior covariance that satisfy the pathpoint constraints. Based on the posterior mean, a generalized motion trajectory and a generalized stiffness trajectory adapted to the new state are generated.

[0063] Specifically, for the new state, the preset pose constraints of the waypoints (such as the position to be reached in the j-th time step) and posture ), which can be represented as an element in the Lie group SE(3). ( (where is the position vector), serving as pose constraint parameters. Pre-defined path point stiffness constraints for the new state (e.g., the target stiffness matrix at the j-th time step). ), and directly use it as an element in the symmetric positive definite matrix space SPD(3) as a stiffness constraint parameter. For pose constraint parameters Calculate the average pose at the j-th time step. Group inverse operation: Then, through matrix logarithm and VEE mapping, it is transformed into a 6-dimensional pose constraint vector. For stiffness constraint parameters Calculate the mean stiffness at the j-th time step. Logarithmic Euclidean difference: Then, it is transformed into a 6-dimensional stiffness constraint vector through semi-vectorization. .merge and Obtain the total constraint vector Setting constraints on uncertainty covariance (Reflecting the reliability of the constraints), construct a linear measurement model: ,in To select the matrix (extract only the state component at the j-th time step). Given the joint state vector, calculate the Kalman gain. ,in Let be the prior covariance of the Gaussian Markov random field. Calculate the posterior mean: ( (This is the prior mean). Calculate the posterior covariance: For the posterior mean In the pose component, the centered coordinates of each time step i are transformed into Lie algebra elements through the VEE inverse mapping, then into Lie group elements through matrix exponent mapping, and finally combined with the pose mean. By performing group multiplication, we obtain the pose sequence of the generalized motion trajectory. For the posterior mean The stiffness components in the matrix, the centered coordinates of each time step i are transformed into a symmetric matrix through a semi-vectorized inverse mapping, and... After addition and matrix exponential mapping, the stiffness matrix sequence of the generalized stiffness trajectory is obtained. .

[0064] For example, in one embodiment, to account for the uncertainty of the target stiffness (path point) constraint, it is assumed that the first... At each time step, there exists a target stiffness. And its uncertainty is used The target stiffness, mapped to the same log-Euclidean space, can be expressed as:

[0065] ;

[0066] Among them, the The mapped constraint vector; For the first The logarithmic Euclidean mean of stiffness at each time step; For target stiffness; stacking state The measurement process is modeled as follows:

[0067] ;

[0068] Among them, the The mapped constraint vector; For the first There are standard basis vectors, therefore (Right now Its function is to extract The Middle indivual (dimensional block vector); For measuring noise; It is an identity matrix.

[0069] set up yes If the prior covariance is given, then the posterior mean and posterior covariance can be expressed as:

[0070] ;

[0071] ;

[0072] ;

[0073] in, Kalman gain; and These are the posterior mean and the posterior covariance, respectively.

[0074] The method provided in this embodiment achieves time alignment by employing a globally optimal reparameterization algorithm, which can accurately synchronize the time dimension of pose and stiffness data, laying the foundation for subsequent joint analysis. It defines relative increments using Lie groups and log-Euclidean operations, aligning with the mathematical characteristics of pose (Lie group structure) and stiffness (symmetric positive definite matrix structure), ensuring the rationality of data processing. Calculating the mean of the relative increments, mapping it to a vector, and obtaining the local covariance effectively extracts the motion and stiffness variation patterns from multiple sets of demonstration data. Mapping the data to centralized coordinates and constructing a Gaussian Markov random field using the local covariance allows for a probabilistic model to characterize the joint distribution and dynamic correlation of motion and stiffness. Inputting new state constraints and updating the joint probability distribution through Gaussian conditionalization allows for flexible adaptation to the pose and stiffness requirements of new states while adhering to the demonstration patterns. These methods work together to ensure that the generated generalized motion trajectories and generalized stiffness trajectories retain the coordinated characteristics of motion and stiffness in human demonstrations while adapting to new task scenarios. This provides precise and scenario-adaptive motion and stiffness guidance for robot control, enabling robots to accurately complete motion trajectory planning in contact-rich tasks and achieve compliant, safe, and efficient operation through reasonable stiffness adjustments. This enhances the robot's task execution capabilities in complex and variable scenarios.

[0075] S103. Construct an obstacle model through environmental perception, sample candidate trajectories using the generalized motion trajectory and generalized stiffness trajectory as references, and filter collision-free trajectories by combining motion consistency, stiffness consistency and smoothness cost, and iteratively optimize to obtain optimized motion trajectory and optimized stiffness trajectory.

[0076] Specifically, the obstacle model is a digital model constructed by collecting environmental data through environmental perception devices (such as depth cameras, LiDAR, and 6-axis force / torque sensors) and processing the data to describe the spatial position, geometry, and physical properties of obstacles in the scene. The core function of the obstacle model is to provide a "collision judgment benchmark" for subsequent trajectory selection, ensuring that the robot can accurately identify and avoid obstacles during movement. The candidate trajectory refers to a combination of multiple motion-stiffness trajectories that cover the "reasonable spatial range around the reference trajectory" and are generated by random sampling, with reference to the generalized motion trajectory (position-attitude sequence in Lie group SE(3) space) and the generalized stiffness trajectory (6×6 stiffness matrix sequence in SPD(3) space). Motion consistency cost is an evaluation index used to quantify the "degree of deviation between the candidate motion trajectory and the generalized motion trajectory (reference trajectory)". Its core is to ensure that the candidate trajectory does not deviate from the motion law demonstrated by humans. Stiffness consistency cost is an evaluation index used to quantify the "degree of deviation between the candidate stiffness trajectory and the generalized stiffness trajectory (reference trajectory)". Its core is to preserve the adaptation logic of stiffness and contact scene in human demonstration. Smoothness cost is an evaluation metric used to measure the "smoothness of change of candidate trajectories (including motion trajectories and stiffness trajectories) in the time dimension". Its core is to avoid mechanical vibration and contact impact caused by sudden changes in robot motion or stiffness.

[0077] In specific implementation, point cloud data of the new state is collected, and noise reduction is performed sequentially using the random sampling consensus algorithm, registration and stitching using the iterative nearest point algorithm, and Poisson surface reconstruction to generate an obstacle collision detection model. Inverse kinematics calculations are performed on the generalized motion trajectory to obtain the reference joint trajectory. Inverse stiffness mapping is performed on the generalized stiffness trajectory combined with the robot Jacobian matrix to obtain the reference joint stiffness. Multiple sets of candidate joint trajectories and candidate joint stiffnesses are sampled according to a preset Gaussian noise distribution, centered on the reference joint trajectory and reference joint stiffness. The candidate joint trajectories are mapped to candidate motion trajectories in Cartesian space using forward kinematics, and the candidate joint stiffnesses are mapped to Cartesian space using stiffness mapping. Candidate stiffness trajectories are generated; a collision detection library is used to perform collision detection on the candidate motion trajectories and candidate stiffness trajectories based on the obstacle collision detection model, and effective candidate motion trajectories and effective candidate stiffness trajectories with no collisions throughout the process are selected; a cost function is defined; the cost function includes motion consistency cost, stiffness consistency cost and smoothness cost, and the total cost function is obtained by weight fusion; the total cost of each effective candidate trajectory is calculated, and the cost is normalized by a smoothing function to obtain the trajectory weight. The effective candidate trajectories are updated according to the trajectory weight. When the joint angle deviation between the updated trajectory and the previous trajectory is less than a preset threshold, the iteration is determined to be converged, and the optimized motion trajectory and optimized stiffness trajectory are output.

[0078] Optionally, the process of determining the cost function includes: determining the motion consistency cost based on the sum of the Frobenius norm of the rotation matrix and the Euclidean norm of the translation vector of the candidate motion trajectory and the generalized motion trajectory; determining the stiffness consistency cost based on the Frobenius norm of the difference between the matrix logarithms of the candidate stiffness trajectory and the generalized stiffness trajectory; determining the smoothness cost through the second-order difference Euclidean norm of the joint angle sequence; determining the weight of each sub-cost according to the rich contact task type; and performing weighted fusion based on the weights of each sub-cost to obtain the cost function.

[0079] Specifically, a depth camera or LiDAR is used to acquire 3D point cloud data of the new state, including the original spatial coordinate information of obstacles in the scene. The Random Sample Consensus (RANSAC) algorithm is employed, setting a distance threshold and iteratively selecting a subset of points from the point set to fit a planar model. Points with a distance less than the threshold are identified as inliers (valid obstacle points), while outliers (noise points) are removed, thus completing point cloud denoising. The Iterative Closest Point (ICP) algorithm is then used to spatially register and stitch the denoised point clouds acquired from multiple perspectives. One set is used as the target point cloud, and the rest as source point clouds. By calculating the Euclidean distance minimization transformation matrix (rotation + translation) of corresponding point pairs, the multi-view point clouds are spatially registered and stitched together to form a complete scene point cloud. Poisson surface reconstruction is then performed. Based on the stitched point cloud data, a Poisson equation for implicit feature functions is constructed. Solving the equation generates a continuous 3D mesh surface, resulting in a collision detection model that includes the geometric boundaries of obstacles. For the generalized motion trajectory (position-attitude sequence in Cartesian space), an inverse kinematics solver (such as damped least squares method) is invoked to calculate the robot joint angles that satisfy the pose constraints at each time step, obtaining the reference joint trajectory (a sequence of joint angles changing over time). For the generalized stiffness trajectory (a 6×6 stiffness matrix in Cartesian space), combined with the Jacobian matrix J under the robot's current joint configuration, the reference joint stiffness at each time step is calculated using the inverse stiffness mapping formula (k=J^TSJ) (k is the joint space stiffness, S is the Cartesian space stiffness) (in diagonal matrix form, containing the stiffness values ​​of each joint). The standard deviation of Gaussian noise is set (e.g., 0.01 rad for joint angle noise, 50 N / m for joint stiffness noise). Using the joint angles at each time step of the reference joint trajectory as the mean, multiple sets (e.g., 50 sets) of noisy candidate joint trajectories are sampled and generated. Similarly, using the mean stiffness value of the reference joint stiffness at each time step, candidate joint stiffnesses are generated by sampling according to a preset Gaussian distribution, matching the number of candidate joint trajectories (ensuring a one-to-one correspondence between the motion and stiffness time steps). For each group of candidate joint trajectories, the joint angles at each time step are converted into the position (x / y / z) and orientation (rotation matrix / quaternion) of the end effector in Cartesian space using a robot forward kinematics model (such as the DH parameter method), resulting in the candidate motion trajectory. For each group of candidate joint stiffnesses, combined with the Jacobian matrix J of the corresponding time step, the stiffness positive mapping formula (S=J^{-T}kJ^{-1}) is used to convert it into a 6×6 candidate stiffness matrix in Cartesian space, resulting in the candidate stiffness trajectory. A collision detection library (such as FCL, Bullet) is called to import the obstacle collision detection model and the robot link model (including the end effector) into the detection environment. For each group of candidate motion trajectories, the minimum distance between the end effector and the obstacle model is calculated step by step. If the minimum distance at all time steps is greater than the safety threshold (e.g., 0.02m), it is determined to be a collision-free trajectory, and the corresponding valid candidate motion trajectory and valid candidate stiffness trajectory are retained.The cost functions are defined and calculated as follows: Motion consistency cost is calculated for each time step of the valid candidate motion trajectory and the generalized motion trajectory (using the Frobenius norm for rotation and the Euclidean norm for translation), weighted summation, and then normalized. Stiffness consistency cost is calculated for each time step of the valid candidate stiffness trajectory and the generalized stiffness trajectory (using the logarithmic Euclidean distance of the logarithmic matrix's Frobenius norm), weighted summation, and then normalized. Smoothness cost is calculated for valid candidate joint trajectories by summing the squares of the first-order differences (velocity) and second-order differences (acceleration) of joint angles at adjacent time steps; for valid candidate joint stiffness, it is calculated by summing the squares of the first-order differences of stiffness values ​​at adjacent time steps, weighted summation, and then normalized. The total cost function is obtained by linearly fusing the above three costs using preset weights (e.g., motion 0.4, stiffness 0.3, smoothness 0.3) to obtain the total cost for each set of valid candidate trajectories. The total cost is smoothed using an exponential function (e.g., w = exp(-λcost)), where λ is a scaling factor. After normalization, the weights of each valid candidate trajectory are obtained (the sum of the weights is 1). A weighted average is then calculated for the valid candidate joint trajectories and candidate joint stiffnesses to generate updated joint trajectories and joint stiffnesses. The joint angle deviation between the updated joint trajectory and the previous trajectory at each time step is calculated. If the maximum value of all deviations is less than a preset threshold (e.g., 0.005 rad), the iteration is considered converged. The converged joint trajectory is mapped to an optimized motion trajectory in Cartesian space using forward kinematics, and the joint stiffness is converted to an optimized stiffness trajectory through stiffness mapping and output.

[0080] For example, in one embodiment, the cost function is based on the unfolded trajectory and stiffness in each iteration and the learned motion and stiffness distribution in a special Euclidean group. and symmetric positive definite matrix group The distance metric calculated in the data yields the following:

[0081] ;

[0082] ;

[0083] ;

[0084] Among them, the Rotation matrix , Distance metric between; For Frobenius function; the Translation vector , Distance metric between; For the Euclidean norm; the stated It is a symmetric positive definite matrix , Distance metric between; This refers to the matrix logarithm operation.

[0085] First, this embodiment explores the space around the initial trajectory by generating noisy trajectories (motion and stiffness), and then fuses these noisy trajectories to obtain a lower-cost updated trajectory. Second, in each iteration, impedance random trajectory optimization defines a set of random samples in the joint space based on the generalized trajectory; each set of samples is called a rollout trajectory, denoted as . and To calculate the distance metric between each deployed trajectory and the learned motion distribution, the motion of the end effector is calculated using forward kinematics, denoted as... To calculate the distance metric between each deployed trajectory and the learned stiffness distribution, the stiffness of the end effector is calculated using the following formula, denoted as: .

[0086] ;

[0087] in, The Cartesian stiffness matrix of the end effector; is the diagonal stiffness matrix in the joint space; Let Jacobian matrix be used for the robot.

[0088] Finally, the Cost function at each time step The calculation is as follows:

[0089] ;

[0090] Among them, the The cost function; , The weights for motion consistency cost and stiffness consistency cost; The cost of motion consistency; This represents the stiffness consistency cost. The formula for calculating the stiffness consistency cost is as follows:

[0091] ;

[0092] in, This comes at the cost of maintaining stiffness consistency. The number of random samples; In time step The stiffness matrix obtained by sampling from the reference stiffness distribution; Let be the Cartesian stiffness matrix of the end effector.

[0093] The method provided in this embodiment offers triple protection of "safety, accuracy, and stability" for robot control through a technical path of "obstacle modeling → joint space sampling → collision screening → cost optimization → iterative convergence". First, an obstacle collision detection model is generated by denoising using a random sampling consensus algorithm, registering and stitching using an iterative nearest point algorithm, and reconstructing a Poisson surface. This accurately captures the three-dimensional geometric features of obstacles in new states, providing a high-precision benchmark for subsequent collision detection. Combined with the screening of candidate trajectories using a collision detection library, it ensures that the final optimized trajectory is collision-free throughout, preventing mechanical interference between the robot and obstacles in the environment and ensuring operational safety from a physical perspective. The generalized trajectory is converted into reference joint space data through inverse kinematics and stiffness inverse mapping. Then, candidate joint trajectories and stiffness are generated by sampling with Gaussian noise. This retains the core features of the generalized trajectory (derived from the motion and stiffness logic demonstrated by humans) while introducing reasonable variations through noise, ensuring that the candidate trajectory covers the feasible space surrounding the reference trajectory. This avoids the potential local optima problem caused by directly applying the generalized trajectory. Simultaneously, the candidate joint space data is converted back to Cartesian space through forward kinematics and stiffness mapping, ensuring that the trajectory operates within the operational space. The physical meaning between them is clear, providing a basis for the precise positioning and force control of the robot's end effector. A cost function is defined that includes motion consistency (deviation between candidate and generalized motion trajectories), stiffness consistency (deviation between candidate and generalized stiffness trajectories), and smoothness (smoothness of trajectory changes over time), and weighted fusion is performed. The cost is normalized through the smoothing function to obtain the trajectory weights, and the candidate trajectories are updated in a weighted manner and iterated until convergence. This ensures that the optimized trajectory is consistent with the laws of human demonstration (avoiding deviations in motion or stiffness from reasonable ranges), and eliminates abrupt changes in the trajectory (such as sudden changes in joint angles or stiffness jumps) through smoothness constraints. This reduces mechanical vibration and contact impact during robot movement, making the end effector's motion more coherent and the force control more compliant. Ultimately, this enables the robot to safely avoid obstacles in new states, accurately reproduce the motion precision and stiffness adaptation characteristics of human operation, and maintain the smoothness of the action, significantly improving the reliability and practicality of robot control in complex environments.

[0094] S104. Based on variable impedance control, the optimized motion trajectory and optimized stiffness trajectory are converted into robot joint torque commands, and the robot is controlled to perform contact-rich operation tasks based on the robot joint torque commands.

[0095] Specifically, variable impedance control is a robot control method that allows the robot's impedance (including stiffness, damping, and other characteristics) to be dynamically adjusted according to requirements during movement. High-contact manipulation tasks refer to tasks in which the robot has frequent and complex contact and interaction with the environment or the object being manipulated during execution.

[0096] In specific implementation, the robot joint space dynamic parameters and the end effector's additional inertial parameters are acquired, and a Cartesian space dynamic model is constructed by combining the robot's Jacobian matrix. The optimized stiffness trajectory is used as the desired stiffness for impedance control. Desired damping is calculated based on the robot's inertial parameters and a preset damping coefficient. The desired inertia is set to be consistent with the inertial parameters in the Cartesian space dynamic model, and a second-order impedance model is constructed. The pose error and error derivative of the robot's end effector are calculated in real time. Combined with external forces, these are substituted into the Cartesian space dynamic model and the second-order impedance model to derive the Cartesian space control force. The Cartesian space control force is mapped to robot joint torque through the transpose of the Jacobian matrix. The robot joint torque is clamped based on a maximum torque threshold to obtain optimized joint torque commands. The Jacobian matrix, dynamic parameters, and joint torque commands are updated in real time based on the acquired data. When an external force exceeds a preset contact force threshold, the desired damping is increased.

[0097] Optionally, the real-time calculation of the robot end effector's pose error and error derivative, combined with external forces, and substituted into the Cartesian space dynamics model and second-order impedance model, derives the Cartesian space control force, including: comparing the actual pose of the robot end effector with the desired pose of the optimized motion trajectory to calculate the pose error; the pose error includes position error and rotation error, the position error is calculated by the Euclidean space difference between the actual position coordinates and the desired position coordinates, and the rotation error is obtained by converting the deviation between the actual rotation matrix and the desired rotation matrix; based on the time derivative of the actual pose and the desired velocity of the optimized motion trajectory, the derivative of the pose error is calculated; the derivative of the pose error includes position error and rotation error. The position error derivative and rotation error derivative are calculated. The position error derivative is the difference between the actual position velocity and the desired position velocity, and the rotation error derivative is calculated using the time rate of change of the rotation error. The external contact force between the robot's end effector and the environment is collected. The position error, its derivative, and the external contact force are substituted into the Cartesian space dynamics model and the second-order impedance model to derive the Cartesian space control force, which includes trajectory tracking dynamic force, impedance error compensation force, and external force adaptation term. The trajectory tracking dynamic force is used to ensure that the end effector dynamically follows the optimized motion trajectory. The impedance error compensation force is used to correct the position deviation according to the preset stiffness and damping characteristics. The external force adaptation term is used to reduce the interference of the external contact force on the motion trajectory.

[0098] Specifically, the robot first obtains joint space dynamic parameters such as rotational inertia, link mass, and center of gravity position of each joint through robot manuals or parameter identification experiments. At the same time, it measures additional inertial parameters such as mass and inertia distribution of the end effector (including tools). Then, based on the current angles of each joint of the robot, the Jacobian matrix (which reflects the relationship between joint motion and end effector motion) is obtained through kinematic calculations. Finally, these parameters are integrated and a model is built according to the principles of dynamics. This model can reflect the relationship between factors such as inertia, gravity, and additional forces generated by motion and the required control force when the end effector moves in Cartesian space (i.e., three-dimensional operating space). From the previously obtained optimized stiffness trajectory, the stiffness data corresponding to each time step is extracted and directly used as the "desired stiffness" (such as the softness or hardness required when contacting objects) that the robot is expected to achieve in impedance control. Referring to the robot's own inertial parameters and combining actual operational requirements, a damping coefficient (such as a resistance parameter to make the movement smoother) is preset, and the desired damping is calculated through the two. At the same time, the desired inertia is set to be the same as the inertial parameters in the Cartesian space dynamics model that was just constructed to ensure that the model inertia matches the desired motion characteristics. Based on these three parameters (desired stiffness, desired damping, and desired inertia), a second-order impedance model is built, which can describe the adaptation relationship between the end effector position deviation, velocity deviation, and contact force.

[0099] Furthermore, the actual position and velocity of the end effector are captured in real time by a visual sensor (such as a camera), and compared with the desired position and velocity in the optimized motion trajectory to calculate the pose error (the difference between the actual position and the desired position) and the error derivative (the difference between the actual velocity and the desired velocity, i.e., the velocity deviation). The external forces generated by the robot's contact with the environment (such as the manipulated object or obstacle) are collected in real time by a force sensor installed at the end effector. The pose error, error derivative, and external force data are simultaneously substituted into the previously constructed Cartesian space dynamics model and second-order impedance model. According to the derivation logic of the dynamic equation, the Cartesian space control force (i.e., the force that the end effector needs to apply) required for the end effector to achieve the desired motion and force control in Cartesian space is calculated. The Jacobian matrix is ​​recalculated based on the current joint angles of the robot. The transpose of this matrix is ​​then used to convert the derived Cartesian space control force into the original joint torque that each joint of the robot needs to output. The maximum torque value that each joint can withstand (i.e., the maximum torque threshold) is obtained by consulting the robot manual. The original joint torque is compared with the maximum torque threshold: if the original torque does not exceed the threshold, it is directly retained; if it exceeds the threshold, the joint torque is adjusted to the maximum threshold (i.e., clamping) to avoid damage to the joint due to excessive torque. After clamping, the optimized joint torque command is obtained. The robot acquires joint angle and velocity data at high frequency (e.g., 1000 times per second), recalculates the Jacobian matrix based on the newly acquired joint angles, and updates parameters such as inertia and gravity in the Cartesian space dynamics model. Based on the updated model and real-time acquired pose and force data, the control force is re-derived and converted into new joint torque commands to ensure that the commands can adapt to the robot's real-time motion state. At the same time, the external force acquired by the force sensor is monitored in real time and compared with the preset contact force threshold (e.g., the maximum contact force to avoid damaging the manipulated object). If the external force does not exceed the threshold, the desired damping remains unchanged. If it exceeds the threshold, the value of the desired damping is immediately increased to reduce the end effector speed by increasing motion resistance, thereby reducing the impact of contact force and preventing damage to the robot or the manipulated object.

[0100] For example, in one embodiment, establishing Dynamic model of a robot with degrees of freedom in Cartesian space:

[0101] ;

[0102] in, and These are relative to coordinates The inertia matrix and the Coriolis / centrifugal force matrix; It is the equivalent gravity in the task space; It is the new input vector; It is a generalized external force vector; Let be the dimension of Cartesian space; The coordinates are for the end effector.

[0103] To define the desired impedance characteristics, the second-order differential impedance model between the robot's end effector and the environment can be expressed as:

[0104] ;

[0105] in, This is the error term; , , These are symmetric positive definite matrices representing desired stiffness, damping, and inertia, respectively; desired stiffness The acquisition method is the same as described in the above embodiments; , The desired inertia can be calculated based on the robot's inertial parameters and expert experience. With robot inertia Same, that is Expected damping ,in, Is with the first Damping coefficients corresponding to generalized eigenvectors yes The One diagonal element.

[0106] End effector error term Among them, position error Calculated directly using vector subtraction, i.e. At the same time, set and Let these be the rotation matrices representing the actual attitude and the desired attitude, respectively. The rotation error is determined by... Once determined, it is then converted to Euler angles to obtain the rotation error components. .

[0107] Combining the Cartesian dynamics model and the impedance control model, the impedance control law can be expressed as:

[0108] ;

[0109] in, For the robot's joint torque commands; This is the term related to gravity. Let be the inertia matrix in Cartesian space; For the desired Cartesian space acceleration; Coriolis / centrifugal force matrix; Let be the desired Cartesian space inertial matrix; Let be the desired stiffness matrix; This refers to the positional error; Let be the desired damping matrix; For speed error; It is an external contact force.

[0110] The Cartesian impedance controller ultimately controls the joint torque. The implementation is as follows:

[0111] ;

[0112] in, Joint torque; It is the gravitational torque vector; It is a Jacobian matrix; This is a virtual equilibrium pose; It is the identity matrix; Let be the inertia matrix in Cartesian space; For the desired Cartesian space acceleration; Coriolis / centrifugal force matrix; Let be the desired Cartesian space inertial matrix; Let be the desired stiffness matrix; This refers to the positional error; Let be the desired damping matrix; For speed error; It is an external contact force.

[0113] Equivalent stiffness matrix in Cartesian space It can be calculated as:

[0114] ;

[0115] in, and It is a constant symmetric positive definite matrix; It is a constant matrix; It is a positive definite stiffness matrix; A sufficient condition for positive definiteness is ,in, is the smallest eigenvalue of the matrix. It is the largest singular value of the matrix.

[0116] The method provided in this embodiment, in its first aspect, constructs a complete "skill learning - trajectory optimization - command execution" chain for robot control through a closely linked and inseparable multi-step process of "collecting human demonstration data → generating generalized trajectory → optimizing trajectory → executing variable impedance control": In the acquisition stage, human demonstration data containing the pose and stiffness matrix of the end effector is acquired through sensors to provide real operation samples for subsequent operation; In the generalization stage, the difference in demonstration speed is eliminated based on time alignment, the coordination law between motion and stiffness is explored through probabilistic modeling, and cross-scene migration is achieved through scene adaptation to generate a basic reference trajectory; In the optimization stage, environmental perception modeling and multi-cost screening are combined to ensure the safety and stability of the trajectory; In the control stage, the trajectory is converted into torque commands through variable impedance. Each step is progressive, which not only allows the robot to accurately reproduce the motion logic of human operation, but also achieves compliant contact through stiffness coordination and dynamic impedance adjustment, ultimately achieving the synergy of precise control and compliant control. Secondly, a globally optimal reparameterization algorithm is used to achieve time synchronization of pose and stiffness data, avoiding distortion of the cooperative relationship caused by time misalignment. Relative increments are defined by Lie group operations and log-Euclidean operations to adapt to the heterogeneous data characteristics of pose (Lie group) and stiffness (symmetric positive definite matrix), ensuring the physical rationality of incremental calculations. A Gaussian Markov random field is constructed to characterize the joint probability distribution of motion and stiffness, capturing the dynamic coupling relationship between the two. Combined with new state constraints, the distribution is updated through Gaussian conditionalization, so that the generalized trajectory can not only retain the core skills demonstrated by humans (such as the stiffness adjustment logic during contact), but also adapt to new tasks without leaving the original scene, providing the robot with an operational reference with scene transfer capabilities without repeated demonstrations. Thirdly, a high-precision obstacle model is constructed through random sampling consistency denoising, iterative nearest point registration, and Poisson reconstruction. This model is then combined with a collision detection library to screen collision-free trajectories, thus avoiding interference between the robot and the environment from a physical perspective. Candidate trajectories are sampled using generalized trajectories as a reference and constrained by motion and stiffness consistency costs to ensure that the optimized trajectory does not deviate from the core principles demonstrated by humans. Smoothness costs are introduced to eliminate trajectory abrupt changes (such as sharp joint rotations and stiffness jumps), reducing mechanical vibration and contact impacts. Ultimately, this provides a "safe, compliant, and stable" high-quality trajectory input for subsequent control, improving the reliability and quality of robot operation.Fourthly, when converting the optimized trajectory into joint torque commands based on variable impedance control, a precise Cartesian space dynamic model is constructed by combining robot dynamic parameters and the Jacobian matrix to ensure the accuracy of control force derivation. Using the optimized stiffness trajectory as the desired stiffness, and combining inertial parameters to calculate the desired damping and inertia, a second-order impedance model is constructed, enabling the robot to dynamically adapt to the force-motion response requirements of contact-rich scenarios. The pose error and external force are calculated in real time and substituted into the model to derive the control force, which is then mapped to joint torque via the Jacobian transpose. Simultaneously, torque clamping is performed to avoid joint overload. Dynamic parameters and torque commands are updated frequently, and damping is increased when external forces exceed the threshold, ensuring trajectory tracking accuracy while buffering contact impacts through compliant impedance adjustment. Ultimately, this allows the robot to accurately perform contact-rich tasks (such as assembly and picking), balancing operational accuracy and contact safety.

[0117] Corresponding to the aforementioned embodiment of a robot skill learning and control method for rich contact operation tasks, this application also provides an embodiment of a robot skill learning and control device for rich contact operation tasks.

[0118] Figure 2 This is a schematic diagram of the robot skill learning and control device for contact-intensive tasks provided in Embodiment 2 of this application. Please refer to... Figure 2 The device provided in this embodiment includes a data acquisition module 210, a processing module 220, and a control module 230.

[0119] The acquisition module 210 is used to acquire human demonstration data; the human demonstration data includes at least end effector pose data and stiffness matrix data.

[0120] The processing module 220 is used to obtain the generalized motion trajectory and stiffness trajectory based on the end effector pose data and stiffness matrix data through time alignment, probability distribution modeling and new state constraint adaptation.

[0121] The processing module 220 is also used to construct an obstacle model through environmental perception, sample candidate trajectories with the generalized motion trajectory and stiffness trajectory as references, and filter collision-free trajectories by combining motion consistency, stiffness consistency and smoothness cost and iteratively optimize to obtain optimized motion trajectory and optimized stiffness trajectory.

[0122] The control module 230 is used to convert the optimized motion trajectory and optimized stiffness trajectory into robot joint torque commands based on variable impedance control, and to control the robot to perform contact-rich operation tasks based on the robot joint torque commands.

[0123] The apparatus of this embodiment can be used to perform... Figure 1The steps of the method embodiment shown are similar in principle and process, and will not be repeated here.

[0124] The specific implementation process of the functions and roles of each unit in the above device can be found in the implementation process of the corresponding steps in the above method, and will not be repeated here.

[0125] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to in the description of the method embodiments. 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 application according to actual needs. Those skilled in the art can understand and implement this without creative effort.

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

Claims

1. A method for robot skill learning and control for tasks involving rich contact operations, characterized in that, The method includes: Collect human demonstration data; the human demonstration data includes at least end effector pose data and stiffness matrix data; Based on the end effector pose data and stiffness matrix data, a generalized motion trajectory and a generalized stiffness trajectory are obtained through time alignment, probability distribution modeling, and new state constraint adaptation. The probability distribution modeling is to construct a joint probability distribution of Gaussian Markov random fields that includes the coupling relationship between motion and stiffness. The new state constraint adaptation is a Gaussian conditional update that integrates the pose constraints and stiffness constraints of the path points. An obstacle model is constructed through environmental perception. With the generalized motion trajectory and generalized stiffness trajectory as references, multiple sets of candidate joint trajectories and candidate joint stiffness are sampled in the joint space according to a preset Gaussian noise distribution. The trajectories are mapped to candidate motion trajectories in Cartesian space through forward kinematics mapping and then transformed into candidate stiffness trajectories in Cartesian space through stiffness mapping. Define a total cost function, which includes motion consistency cost, stiffness consistency cost, and smoothness cost; the motion consistency cost is calculated based on the sum of the Frobenius norm of the rotation matrix and the Euclidean norm of the translation vector, the stiffness consistency cost is calculated based on the Frobenius norm of the difference of the logarithms of the matrices, and the smoothness cost is calculated based on the second difference Euclidean norm of the joint angle sequence. Based on the obstacle model, collision detection is performed on candidate motion trajectories and candidate stiffness trajectories to screen out valid candidate trajectories that have no collisions throughout the entire process. The total cost of the effective candidate trajectory is calculated according to the total cost function. The trajectory weight is obtained by normalization through a smoothing function. The weighted iterative optimization is performed until the trajectory converges, resulting in the optimized motion trajectory and the optimized stiffness trajectory. Based on variable impedance control, the optimized motion trajectory and optimized stiffness trajectory are converted into robot joint torque commands, and the robot is controlled to perform contact-rich operation tasks based on the robot joint torque commands.

2. The method according to claim 1, characterized in that, The process of obtaining the generalized motion trajectory and generalized stiffness trajectory based on the end effector pose data and stiffness matrix data through time alignment, probability distribution modeling, and adaptation to new state constraints includes: A global optimal reparameterization algorithm is used to perform time alignment on the end effector pose data and stiffness matrix data to obtain time-synchronized aligned pose sequences and aligned stiffness sequences. For the aligned pose sequence, the relative increment of motion between adjacent time steps is defined by the group operation of the Lie group; For the aligned stiffness sequence, the relative stiffness increment between adjacent time steps is defined by logarithmic Euclidean operation; The mean values ​​of the relative increment of motion and the relative increment of stiffness are calculated respectively. The relative increments are mapped to a linear space and converted into vectors. The local covariance of motion and the local covariance of stiffness are calculated based on the vector sample deviation. The alignment pose sequence and alignment stiffness sequence are mapped to centered coordinates, and the motion local covariance and stiffness local covariance are combined to obtain the joint probability distribution of the motion trajectory and stiffness trajectory. Input the preset pathpoint pose constraints and pathpoint stiffness constraints in the new state, update the joint probability distribution using Gaussian conditionalization, and generate the generalized motion trajectory and generalized stiffness trajectory.

3. The method according to claim 2, characterized in that, The steps of calculating the mean values ​​of the relative motion increment and the relative stiffness increment, mapping the relative increments to a linear space to transform them into vectors, and calculating the local covariance of motion and the local covariance of stiffness based on the vector sample deviation include: For the relative increments of motion in multiple sets of demonstration data, the mean of the relative increments of motion is calculated using the logarithmic Euclidean mean algorithm for Lie group space; For the relative stiffness increments of multiple sets of demonstration data, the mean of the relative stiffness increments is calculated using the logarithmic Euclidean mean algorithm in symmetric positive definite matrix space. For each relative increment of motion, the Lie algebra space is mapped to the matrix logarithm, and then the Lie algebra elements are transformed into 6-dimensional vectors through vectorization operations. For each relative stiffness increment, the upper triangular elements of the symmetric matrix are extracted and transformed into a 6-dimensional vector by mapping the matrix logarithm to the symmetric matrix space. Calculate the deviation between the 6-dimensional vector corresponding to each relative motion increment and the mapping vector of the mean relative motion increment, perform an outer product operation on the deviation and take the average to obtain the local covariance of motion; Calculate the deviation between the 6-dimensional vector corresponding to each relative stiffness increment and the mapping vector of the mean relative stiffness increment, perform an outer product operation on the deviation and take the average to obtain the local stiffness covariance.

4. The method according to claim 2, characterized in that, The step of mapping the aligned pose sequence and aligned stiffness sequence to centered coordinates, and combining the motion local covariance and stiffness local covariance to obtain the joint probability distribution of the motion trajectory and stiffness trajectory includes: Each pose element in the aligned pose sequence is mapped to pose coordinates through the group inverse operation with the pose mean, and each stiffness matrix in the aligned stiffness sequence is mapped to centered stiffness coordinates through the difference between the logarithm of the stiffness mean and the Euclidean mean. The topology of a Gaussian Markov random field is constructed with time steps as nodes. The conditional probability dependency between adjacent time step nodes is defined by the motion local covariance and the stiffness local covariance. Based on the pose coordinates, centered stiffness coordinates, and conditional probability dependencies between nodes, a joint probability density function containing the coupling relationship between motion and stiffness is constructed. The joint probability distribution of motion trajectory and stiffness trajectory is determined based on the off-diagonal covariance term in the joint probability density function.

5. The method according to claim 2, characterized in that, The preset pathpoint pose constraints and pathpoint stiffness constraints in the new input state are updated using Gaussian conditionalization to generate a generalized motion trajectory and a generalized stiffness trajectory, including: The preset path point pose constraints in the new state are transformed into pose constraint parameters in Lie group space, and the path point stiffness constraints are transformed into stiffness constraint parameters in symmetric positive definite matrix space. The pose constraint parameters and stiffness constraint parameters are mapped to constraint vectors in coordinate space to construct a linear measurement model that includes constraint uncertainties. The Kalman gain is calculated based on the linear measurement model, and the joint probability distribution of the Gaussian Markov random field is updated posteriorly using the Kalman gain to obtain the posterior mean and posterior covariance that satisfy the path point constraint. Generate a generalized motion trajectory and a generalized stiffness trajectory to adapt to the new state based on the posterior mean.

6. The method according to claim 1, characterized in that, The process of converting the optimized motion trajectory and optimized stiffness trajectory into robot joint torque commands based on variable impedance control includes: Obtain the robot joint spatial dynamic parameters and the end effector's additional inertial parameters, and construct a Cartesian spatial dynamic model by combining the robot's Jacobian matrix; Using the optimized stiffness trajectory as the desired stiffness for impedance control, the desired damping is calculated based on the robot's inertial parameters and the preset damping coefficient. The desired inertia is set to be consistent with the inertial parameters in the Cartesian space dynamics model, and a second-order impedance model is constructed. The pose error and error derivative of the robot end effector are calculated in real time. Combined with external forces, the Cartesian space dynamics model and the second-order impedance model are substituted to derive the Cartesian space control force. The Cartesian space control force is mapped to robot joint torque by transposing the Jacobian matrix, and the robot joint torque is clamped based on the maximum torque threshold to obtain the optimized joint torque command. Based on the collected data, the Jacobian matrix, dynamic parameters and joint torque commands are updated in real time, and the desired damping is increased when the external force exceeds the preset contact force threshold.

7. The method according to claim 6, characterized in that, The real-time calculation of the robot's end effector's pose error and error derivative, combined with external forces, and substituted into the Cartesian space dynamics model and second-order impedance model, derives the Cartesian space control force, including: The actual pose of the robot's end effector is compared with the desired pose of the optimized motion trajectory to calculate the pose error. The pose error includes position error and rotation error. The position error is calculated by the Euclidean space difference between the actual position coordinates and the desired position coordinates, and the rotation error is obtained by the deviation between the actual rotation matrix and the desired rotation matrix. Based on the time derivative of the actual pose and the expected velocity of the optimized motion trajectory, the derivative of the pose error is calculated; the derivative of the pose error includes the position error derivative and the rotation error derivative. The position error derivative is the difference between the actual position velocity and the expected position velocity, and the rotation error derivative is calculated through the time rate of change of the rotation error. The external contact force between the robot's end effector and the environment is collected. The pose error, the derivative of the pose error, and the external contact force are substituted into the Cartesian space dynamics model and the second-order impedance model to derive the Cartesian space control force, which includes trajectory tracking dynamic force, impedance error compensation force, and external force adaptation term. The trajectory tracking dynamic force is used to ensure that the end effector dynamically follows the optimized motion trajectory. The impedance error compensation force is used to correct the pose deviation according to the preset stiffness and damping characteristics. The external force adaptation term is used to reduce the interference of the external contact force on the motion trajectory.

8. A robot skill learning and control device for tasks involving rich contact operations, characterized in that, The device includes a data acquisition module, a processing module, and a control module; The acquisition module is used to acquire human demonstration data; the human demonstration data includes at least end effector pose data and stiffness matrix data. The processing module is used to obtain a generalized motion trajectory and a generalized stiffness trajectory based on the end effector pose data and stiffness matrix data through time alignment, probability distribution modeling, and new state constraint adaptation. The probability distribution modeling is to construct a joint probability distribution of Gaussian Markov random fields that includes the coupling relationship between motion and stiffness. The new state constraint adaptation is to perform a Gaussian conditional update that integrates the path point pose constraint and the path point stiffness constraint. The processing module is also used to construct an obstacle model through environmental perception, and with the generalized motion trajectory and generalized stiffness trajectory as references, to sample multiple sets of candidate joint trajectories and candidate joint stiffness in the joint space according to a preset Gaussian noise distribution, and to map them into candidate motion trajectories in Cartesian space through forward kinematics, and to transform them into candidate stiffness trajectories in Cartesian space through stiffness mapping. Define a total cost function, which includes motion consistency cost, stiffness consistency cost, and smoothness cost; the motion consistency cost is calculated based on the sum of the Frobenius norm of the rotation matrix and the Euclidean norm of the translation vector, the stiffness consistency cost is calculated based on the Frobenius norm of the difference of the logarithms of the matrices, and the smoothness cost is calculated based on the second difference Euclidean norm of the joint angle sequence. Based on the obstacle model, collision detection is performed on candidate motion trajectories and candidate stiffness trajectories to screen out valid candidate trajectories that have no collisions throughout the entire process. The total cost of the effective candidate trajectory is calculated according to the total cost function. The trajectory weight is obtained by normalization through a smoothing function. The weighted iterative optimization is performed until the trajectory converges, resulting in the optimized motion trajectory and the optimized stiffness trajectory. The control module is used to convert the optimized motion trajectory and optimized stiffness trajectory into robot joint torque commands based on variable impedance control, and to control the robot to perform contact-rich operation tasks based on the robot joint torque commands.

Images