Virtual fixtures for arbitrary differentiable geometric constraints, methods and systems for dynamic control

Abstract

The application discloses an arbitrary differentiable geometric constraint virtual clamp, a dynamic control method and a system, wherein the arbitrary differentiable geometric constraint virtual clamp is characterized in that: in the case that the geometric form of the virtual clamp can be given in an analytical form and is at least second-order differentiable, the constrained geometric form only depends on the spatial position; a scalar constraint equation and a constraint control variable of the virtual clamp are defined, a joint Jacobian matrix of multiple constraints is constructed, a reference constraint control input is calculated by using PD control based on a constraint control expected value and a constraint control error value; the arbitrary differentiable geometric constraint virtual clamp dynamic control method is characterized in that: a null space control input of a mechanical arm is calculated based on a dynamic model, a reference constraint control input is calculated by using the virtual clamp, and an estimated torque is projected to a task space corresponding to a virtual clamp constraint and then compensated by using a disturbance observer. The virtual clamp provided by the application solves the problems of poor universality and heavy calculation burden, simplifies calculation and improves universality.

Description

Technical Field

[0001] The embodiments of the present invention relate to the field of human-robot control technology, and particularly to virtual grippers with arbitrary differentiable geometric constraints, dynamic control methods and systems. Background Technology

[0002] In recent years, advancements in robotics technology, particularly in high motion precision, continuous operation capabilities, and perception systems, have propelled the widespread application of robots across various fields. While challenges remain regarding insufficient intelligence and limited autonomous decision-making in complex environments, robots have demonstrated advantages surpassing human capabilities in numerous tasks. Consequently, an increasing number of researchers are focusing on human-robot collaborative control strategies to effectively assist human operations. However, human-robot physical interaction introduces unpredictable external dynamic disturbances to robot control, making collaborative control more complex than individual robot control. Interaction safety, operational accuracy, and anti-interference capabilities have become core challenges in human-robot interaction scenarios. To address safety issues in complex environments, it is necessary to constrain the robot's motion path and range. Against this backdrop, Virtual Fixture (VF) technology has emerged as an important robotic arm control method, used to guide the robotic arm along a predetermined path or confine its movement within a constrained area.

[0003] Based on constraint implementation methods, existing virtual fixture methods are mainly divided into two categories: reference point methods and projection methods. Reference point methods, based on the geometric relationship between constraints and the tool's current pose, solve for the nearest point satisfying the constraints using analytical methods or numerical optimization algorithms. This method combines impedance models and pose data to generate control inputs to reduce constraint errors. While it can accurately model interaction forces and compliance characteristics, it requires solving the analytical expression of the control quantity separately for each type of geometric constraint, resulting in insufficient versatility and excessively high real-time computation costs. In contrast, projection methods project external forces or velocities towards the constraint direction, transforming constrained motion control into an unconstrained problem for processing. Ultimately, unconstrained motion control strategies can be used to implement virtual fixtures. A variant of the projection method is the Jacobian matrix method, which directly projects constraint errors into joint space. Compared to conventional projection methods, its control law design is more flexible.

[0004] Based on the control model, virtual fixture methods can be divided into kinematic control-based methods and dynamic control-based methods. Kinematic methods rely on the robot's kinematic model, generating virtual constraints through analytical or numerical solutions of geometric relationships. While these methods offer advantages such as ease of implementation and high real-time performance, they do not consider dynamic factors such as inertia and friction. Dynamic control-based methods, on the other hand, consider the system's dynamic characteristics, achieving constraints by directly controlling joint acceleration and torque. These methods typically incorporate robust control strategies to compensate for external disturbances and model uncertainties, thereby achieving more precise and stable control in complex dynamic environments. However, the introduction of nonlinear dynamics increases system complexity, and existing research on dynamic virtual fixtures mainly focuses on simple linear constraints, lacking universal methods applicable to general geometric constraints.

[0005] Despite significant progress in constraint control technology, the following key challenges remain: First, most existing methods are custom-designed for specific geometric constraints, lacking versatility. Therefore, when dealing with diverse and complex geometric constraint systems, extensive modifications and optimizations are often required. Second, existing methods generally rely on kinematic models, which have limited robustness under complex environmental disturbances, especially when handling dynamic uncertainties and external force disturbances. Finally, the real-time performance and computational complexity of virtual fixture methods are also key factors restricting their widespread application, particularly in high-precision tasks. How to optimize these methods remains a pressing problem to be solved. Summary of the Invention

[0006] The purpose of this invention is to provide at least a virtual fixture with arbitrary differentiable geometric constraints, a dynamic control method and system, which can at least solve the technical problems of virtual fixtures lacking versatility, performing poorly in handling dynamic uncertainties and external force disturbances, and reducing the computational burden while ensuring robustness. It can at least improve versatility, enhance robustness in handling dynamic uncertainties and external force disturbances, simplify calculations and improve efficiency.

[0007] To address the aforementioned technical problems, at least one embodiment of this application provides a virtual fixture with arbitrary differentiable geometric constraints, characterized by comprising: assuming that the geometric form of the virtual fixture can be given in analytical form and is at least second-order differentiable, the geometric form of the constraints depends only on the spatial position, defining the virtual fixture form as simultaneously satisfying multiple non-conflicting constraints, defining constraint control quantities, constructing a joint Jacobian matrix of multiple constraints, and calculating the reference control input.

[0008] At least one embodiment of this application also provides a virtual fixture dynamics control method with arbitrary differentiable geometric constraints, comprising: constructing a joint space dynamics model of a robotic arm, which is used to establish a mapping from joint space variables to task space using a Jacobian matrix to obtain the velocity and acceleration in the task space; establishing the relationship between joint space acceleration control input, task space acceleration control input, and zero space acceleration control input; calculating the zero space acceleration control input using a joint space impedance model; constructing task space coordinates, using the constraint control quantities and reference constraint control inputs of the virtual fixture described in this application; and deriving task space torque control commands based on the zero space acceleration control input and the reference constraint control input.

[0009] At least one embodiment of this application also provides a virtual gripper dynamics control system with arbitrary differentiable geometric constraints, including a robotic arm dynamics model, a virtual gripper, and a disturbance observer; the robotic arm dynamics model is used to obtain the joint space parameters of the robotic arm, establishes a mapping from joint space variables to task space coordinates using the Jacobian matrix, maps task space parameters to joint space parameters, decouples joint space control inputs from task space control inputs using null space projection, and calculates null space acceleration control inputs using a joint space impedance model; the virtual gripper is used to define arbitrary differentiable scalar constraint equations and constraint control quantities, and performs scalar constraint equations... Differentiation yields the constraint equations for the velocity and acceleration levels. Using the constraint control quantity, the joint velocity is mapped to the end effector velocity. The constraint Jacobian is defined to represent the mapping from joint velocity to the constraint rate of change, thus obtaining the constraint equations for the acceleration level. PD control is used to calculate the reference constraint control input, constructing the joint space acceleration control input that satisfies the virtual fixture constraints. A disturbance observer is used to establish a realistic robotic arm joint dynamics model considering external disturbances and dynamic uncertainties. Combined with the robotic arm dynamics model, a dynamic model of the disturbance estimation error is obtained. The estimated torque is projected onto the task space corresponding to the virtual fixture constraints, and the disturbance is compensated for in the task space.

[0010] Based on the dynamic model, more precise and stable control can be achieved in complex dynamic environments. By establishing a virtual fixture, the constraint control quantity is simplified and at least second-order differentiable, realizing the calculation of velocity and acceleration levels based on spatial position. Combined with a disturbance observer, the accuracy of torque control is improved.

[0011] At least one embodiment of this application also provides an electronic device, including: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to enable the at least one processor to perform the above-described virtual fixture dynamics control method with arbitrary differentiable geometric constraints.

[0012] At least one embodiment of this application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described virtual fixture dynamics control method with arbitrary differentiable geometric constraints.

[0013] The embodiments of this application provide a virtual fixture with arbitrary differentiable geometric constraints, a dynamic control method, and a system. The virtual fixture is at least second-order differentiable, and the geometric form of the constraint depends only on the spatial position. The constraint control quantities and constraint equations can be adapted to arbitrary curves and surfaces, which improves the versatility of the virtual fixture and reduces the computational burden while ensuring robustness.

[0014] In some alternative embodiments, for general curves or surfaces, the scalar constraint equation of the virtual fixture is as follows:

[0015] F(p) = 0;

[0016] Define constraint control quantity To find the first derivative of the constraint control quantity u, define the velocity Jacobian matrix and the constraint Jacobian matrix, and then differentiate the first derivative to obtain the acceleration level relationship of the constraint control quantity. PD control is then used to calculate the reference constraint control input.

[0017]

[0018] In the formula, K u D u Let represent the proportional-derivative gain of the virtual fixture constraint error, d represent the desired value, e represent the error, and c represent the control. This indicates the position of the end effector in the world coordinate system.

[0019] Based on simplified constraint control quantities, after differentiation and PD calculation, combined with the expected value and error, the reference constraint control input is obtained, which enables the robot system to move with high precision to meet the defined constraints. This ensures that zero-space compliance does not affect the virtual constraint main task. The reference constraint control input is calculated using PD control to ensure that the tracking error of the constraint function converges exponentially.

[0020] In some alternative embodiments, for the case of multiple constraints, the scalar constraint equation of the virtual fixture is as follows:

[0021] In the formula, r represents the number of constraints, which is greater than or equal to 1;

[0022] The constraint control quantity u is defined as follows:

[0023]

[0024] The joint Jacobian matrix with multiple constraints is:

[0025]

[0026] In the formula, n represents the degrees of freedom of the robotic arm. Indicates joint angle, It represents the set of real numbers.

[0027] For virtual fixtures with multiple constraints, the constraint control quantity is a combination of multiple constraints, ensuring that there are no conflicts between the multiple constraints.

[0028] In some optional embodiments, the robotic arm dynamics model includes:

[0029] For an n-DOF robotic arm, its joint space dynamics equations are as follows:

[0030]

[0031] in, Indicates joint angle, Let represent the set of real numbers, and let q be an n-dimensional real vector; n represents the number of degrees of freedom of the robotic arm, and M(q) represents the inertia matrix; G(q) represents the Coriolis force and centrifugal force; G(q) represents the gravity term; τ represents the control torque. ext Indicates external force. Represents joint space velocity. Represents joint space acceleration;

[0032] Using Jacobi matrix Based on joint space velocity Joint velocity in mission space From the kinematic relationship between them, the acceleration in the task space can be obtained. With joint space velocity Joint space acceleration The relationship is:

[0033]

[0034] Based on the general solution of equation (6), the joint space acceleration control input is obtained. As shown in the following formula:

[0035]

[0036] In the formula, It is the task space acceleration control input, and It is a zero-space acceleration control input;

[0037] It is a dynamically consistent right generalized inverse, N = IJ # J represents the null space projection matrix, and M represents the inertia matrix.

[0038] By employing a dynamic model, more precise and stable control can be achieved in complex dynamic environments. This model links joint space control with task space control, decoupling task space control input from zero space control input. (Weight + Effect)

[0039] In some optional embodiments, the null space acceleration control input is calculated as shown in the following equation:

[0040]

[0041] In the formula, d represents the expected value, e represents the error, and c represents the control. Indicates joint damping. D represents joint stiffness. n and K n It is a constant diagonal matrix.

[0042] By employing a joint space impedance model, the zero-space control input was calculated, thus achieving zero-space compliance.

[0043] In some optional embodiments, deriving the mission space torque control command based on the null space acceleration control input and the reference constraint control input includes:

[0044] Joint acceleration control inputs that satisfy virtual fixture constraints:

[0045]

[0046] In the formula, Let I represent the null projection matrix relative to the constraint control quantity u, and let I represent the identity matrix.

[0047] Calculate the acceleration control input to be designed based on equations (9) and (16).

[0048] Establish the inverse dynamic compensation term:

[0049]

[0050] Calculate the control torque to be designed based on equations (9), (16) and (2).

[0051] Based on zero-space acceleration control input and reference constraint control input based on virtual fixture constraints, the acceleration control input and control torque in the task space are calculated, ensuring acceleration level control.

[0052] In some optional embodiments, a disturbance observer is also included for calculating the total disturbance moment, taking into account external disturbances and dynamic uncertainties.

[0053]

[0054] Compensation is performed after projecting the estimated torque onto the task space corresponding to the virtual fixture constraints.

[0055]

[0056] In the formula, Represents the perturbation, projection matrix for:

[0057]

[0058] τ u This represents the disturbance moment in the task space.

[0059] An interference observer is used to project the estimated torque of the null space onto the task space corresponding to the virtual fixture constraint for compensation, thus avoiding the loss of the null space compliance effect due to interference compensation. (Weight + Effect). Attached Figure Description

[0060] One or more embodiments are illustrated by way of example with reference to the accompanying drawings, and these illustrative descriptions do not constitute a limitation on the embodiments.

[0061] Figure 1 This is a schematic diagram of a control method provided in one embodiment of this application;

[0062] Figure 2 This is an experimental result of the control method provided in one embodiment of this application. Figure 1 ;

[0063] Figure 3 This is an experimental result of the control method provided in one embodiment of this application. Figure 2 ;

[0064] Figure 4 This is an experimental result of the control method provided in one embodiment of this application. Figure 3 . Detailed Implementation

[0065] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the various embodiments of this application will be described in detail below with reference to the accompanying drawings. However, those skilled in the art will understand that many technical details have been provided in the various embodiments of this application to help readers better understand this application. However, the technical solutions claimed in this application can be implemented even without these technical details and various changes and modifications based on the following embodiments. The division of the various embodiments below is for the convenience of description and should not constitute any limitation on the specific implementation of this application. The various embodiments can be combined with and referenced by each other without contradiction.

[0066] To address the aforementioned technical problem of optimizing virtual fixtures, this invention proposes a virtual fixture with arbitrary differentiable geometric constraints, a dynamic control method, and a system method. The implementation details of the virtual fixture with arbitrary differentiable geometric constraints, the dynamic control method, and the system method in this embodiment are described below. The following implementation details are provided for ease of understanding and are not essential for implementing this solution.

[0067] Example 1:

[0068] The virtual fixture with arbitrary differentiable geometric constraints in this embodiment can be applied to electronic devices with communication, computing, and data storage capabilities, including:

[0069] Given that the geometry of the virtual fixture can be given analytically and is at least second-order differentiable, thus ensuring feasible acceleration level control, and that the geometry of the constraints depends only on the spatial position, based on the above settings, a typical curve or surface virtual fixture can be defined as a set of scalar constraint equations of the form F(p) = 0. This indicates the position of the end effector in the world coordinate system.

[0070] By continuously differentiating the scalar constraint equations, we obtain the constraint equations for the velocity level and the acceleration level, respectively:

[0071]

[0072] In the formula, H is the gradient of F(p). F F(p) is the Hessian matrix of F(p), which reflects the curvature of the constraint curve or surface.

[0073] Similar to the Cartesian motion control objective x, define the constraint control quantity. x represents the actual value of the Cartesian motion control target. c The control input, x, represents the Cartesian motion control target. d This represents the expected value of the Cartesian motion control target.

[0074] Based on the constraint control quantity u, construct an acceleration control input that satisfies the virtual fixture constraints.

[0075] Solve for the first derivative of the constraint control quantity u:

[0076]

[0077] In the formula, It is a velocity Jacobian matrix that maps joint velocities to end-effector velocities, satisfying...

[0078] definition To constrain the Jacobian matrix, representing the joint velocity To the constrained rate of change The mapping.

[0079] Taking the derivative of equation (12), we obtain the acceleration level relationship of the second derivative of the constraint control quantity:

[0080]

[0081] PD control is used to calculate the reference constraint control input, which can be regarded as the actual control input, ensuring that the tracking error of the constraint function converges exponentially. For the virtual fixture constraint control quantity, the corresponding expected value and its derivatives are as follows:

[0082]

[0083] The derived reference control input is:

[0084]

[0085] In the formula, K u D u Let represent the proportional-derivative gain of the virtual fixture constraint error, d represent the desired value, e represent the error, and c represent the control.

[0086] For virtual grippers with multiple constraints, the motion of the robotic arm's end effector must simultaneously satisfy multiple non-conflicting constraints. In the formula, r represents the number of constraints, the constraint control quantity, and the Jacobian matrix is ​​expanded to include all applied constraints. The constraint control quantity u is defined as follows:

[0087]

[0088] The joint Jacobian matrix with multiple constraints is constructed as follows:

[0089]

[0090] The constraint error proportional differential gain in equation (16) is generalized to a more general form, namely Both are symmetric positive definite matrices. The constraint control quantities under multiple constraints are represented by bold 'u' to distinguish them from those under single constraints.

[0091] The virtual fixture method proposed in this application has a simple constraint system and can effectively implement any second-order differentiable geometric constraints without relying on optimization algorithms, thereby reducing the computational burden while ensuring robustness.

[0092] Example 2:

[0093] The virtual fixture dynamics control method with arbitrary differentiable geometric constraints in this embodiment can be applied to electronic devices with communication, computing, and data storage capabilities, including:

[0094] Construct a spatial dynamics model of the robotic arm joints;

[0095] Specifically, using the Jacobian matrix, a mapping from joint space variables to task space is established. Based on the kinematic relationship between joint space velocity and task space velocity, the joint space includes null space and constraint space. The relationship between joint space acceleration control input, task space acceleration control input, and null space acceleration control input is established. Based on the null space acceleration control input, the acceleration control input of the task space is obtained.

[0096] The zero-space acceleration control input is calculated using a joint space impedance model.

[0097] Construct a virtual fixture and task space coordinates, and derive the task space torque control input under the constraints of the virtual fixture;

[0098] Specifically, assuming the geometry of the virtual fixture can be given analytically and is at least second-order differentiable, and the geometry of the constraints depends only on the spatial position, scalar constraint equations for the virtual fixture are constructed to obtain velocity-level and acceleration-level constraint equations. Constraint control quantities are set, and PD control is used to calculate the reference constraint control inputs. Combined with the acceleration-level constraint equations, the joint accelerations required for robot arm control, or the joint accelerations required for implementing the virtual fixture, are calculated.

[0099] Joint space includes constraint space and zero space.

[0100] Disturbance estimation is performed using a disturbance observer.

[0101] Specifically, the dynamic model of the robotic arm is constructed, including:

[0102] For an n-DOF robotic arm, its joint space dynamics equations are as follows:

[0103]

[0104] in, Indicates joint angle, Let represent the set of real numbers, and let q be an n-dimensional real vector; n represents the number of degrees of freedom of the robotic arm, and M(q) represents the inertia matrix; G(q) represents the Coriolis force and centrifugal force; G(q) represents the gravity term; τ represents the control torque. ext Indicates external force. Represents joint space velocity. This represents the acceleration in the joint space.

[0105] Based on the joint space acceleration control input to be designed, an inverse dynamics compensation term is established to eliminate dynamic coupling, and the following calculations are performed:

[0106]

[0107] In the formula, c represents control. This represents the acceleration control input to be designed.

[0108] The "to be designed" section here is to illustrate that the control framework is given as a concept but not a concrete implementation. The concrete implementation will be given later.

[0109] Let the task space coordinates of the robotic arm be For a robotic arm with redundant tasks, there is m <n。

[0110] Using Jacobi matrix Establish a mapping from joint space variables to task space to obtain the velocity and acceleration in task space.

[0111] Based on joint space velocity Joint velocity in mission space The kinematic relationship between them is given by the following equation:

[0112]

[0113] The general solution of equation (3) is shown in the following equation:

[0114]

[0115] Among them, J # Let J(q) be the Moore-Penrose generalized inverse, and N be the null space projection matrix. Let be any vector; It is the right generalized inverse that is consistent with dynamics; T represents the transpose matrix, and I represents the identity matrix.

[0116] N = IJ # J (5).

[0117] Taking the derivative of equation (3) with respect to time, we can obtain the task space acceleration. Velocity with joint space and acceleration The relationship is:

[0118]

[0119] The general solution to equation (6) is shown in the following equation:

[0120]

[0121] Based on this, the relationship between joint space acceleration control input, task space acceleration control input, and zero space acceleration control input is established, and the joint space acceleration control input... It can be designed as:

[0122]

[0123] In the formula, and This indicates a new control input. It is the task space acceleration control input, and It is a zero-space acceleration control input. The zero-space projection method is used to decouple the task space acceleration control input from the joint space acceleration control input.

[0124] A joint space impedance model is used to calculate the zero-space acceleration control input in order to achieve zero-space compliance.

[0125]

[0126] In the formula, d represents the desired value, e represents the error, and c represents the control. Indicates joint damping. D represents joint stiffness. n and K n It is a constant diagonal matrix. This represents the desired value of the zero-space acceleration control input, which serves as the feedforward input. q represents the zero-space velocity control input error value. e This represents the zero-space joint position error value.

[0127] q e =q d -q (10);

[0128] If the joint stiffness is non-zero, then the desired joint position q d It should not conflict with the desired mission space location, otherwise it will reduce the accuracy of mission space motion control.

[0129] Construct the task space coordinates and derive the task space torque control command under the constraints of the virtual fixture.

[0130] Based on the virtual fixture in Specific Implementation One, the geometry of the virtual fixture can be given analytically and is at least second-order differentiable, thus ensuring feasible acceleration level control. Furthermore, the geometric form of the constraints depends only on the spatial position. Based on the above settings, a typical curve or surface virtual fixture can be defined as a set of scalar constraint equations of the form F(p) = 0. This indicates the position of the end effector in the world coordinate system.

[0131] By continuously differentiating the scalar constraint equations, we obtain the constraint equations for the velocity level and the acceleration level, respectively:

[0132]

[0133] In the formula, H is the gradient of F(p). F F(p) is the Hessian matrix of F(p), which reflects the curvature of the constraint curve or surface.

[0134] Similar to the Cartesian motion control objective x, define the constraint control quantity. x represents the actual value of the Cartesian motion control target. c The control input, x, represents the Cartesian motion control target. d This represents the expected value of the Cartesian motion control target.

[0135] Based on the constraint control quantity u, construct an acceleration control input that satisfies the virtual fixture constraints.

[0136] Find the derivative of the constraint control quantity u:

[0137]

[0138] In the formula, It is a velocity Jacobian matrix that maps joint velocities to end-effector velocities, satisfying...

[0139] definition To constrain the Jacobian, let joint velocity be expressed. To the constrained rate of change The mapping.

[0140] Differentiating equation (12), we obtain the relationship between the acceleration levels of the constraint control quantities:

[0141]

[0142] The reference constraint control input, calculated using PD control, can be considered as the actual control input, ensuring that the tracking error of the constraint function converges exponentially. For the virtual fixture constraint control quantity, the corresponding expected value and its derivatives are as follows:

[0143]

[0144] The derived reference control input is:

[0145]

[0146] In the formula, K u D u The proportional differential gain of the virtual fixture constraint error is represented by the subscripts in equation (9), but the physical properties and effects on the system are similar, and the design principles are consistent. The same symbol is used to emphasize the correlation, and only different subscripts are needed to distinguish them.

[0147] Combining the acceleration level constraint equation (8), and substituting equation (15) into the general solution of equation (13), we can obtain the joint acceleration control input that satisfies the virtual gripper constraint required for the control of the robotic arm:

[0148]

[0149] In the formula, This represents the null space projection matrix relative to the constraint control quantity u, ensuring that null space compliance does not affect the virtual constraint main task.

[0150] Based on equations (9), (16) and (2), the control torque τ is obtained.

[0151] The dynamic control method in this embodiment decouples the constraint space from the zero space, enabling the parallel execution of high-precision constraints on the main task virtual fixture and compliant control in the zero space. The high-precision virtual fixture in the constraint space ensures accurate movement during human-machine interaction. The compliant control in the zero space allows the robotic arm's posture to adapt to external force adjustments, avoiding conflicts with the target in the task space.

[0152] Example 3:

[0153] The virtual fixture control method with arbitrary differentiable geometric constraints in this embodiment can be applied to electronic devices with communication, computing, and data storage capabilities. Compared with Embodiment 2, it further includes:

[0154] Constructing an interference observer:

[0155] Considering external disturbances and dynamic uncertainties, the joint space dynamics equation of the robotic arm can be expressed as:

[0156]

[0157] In the formula, M r (q) represents the inertia matrix corresponding to the actual dynamic parameters of the robotic arm. G represents the Coriolis force and centrifugal torque corresponding to the actual dynamic parameters of the robotic arm. r (q) represents the gravitational torque corresponding to the actual dynamic parameters of the robotic arm.

[0158] Lumped disturbance torque It can be represented as follows:

[0159]

[0160] In the formula, It indicates disturbance.

[0161] The estimation of the disturbance torque acting on the robotic arm can be given as follows:

[0162]

[0163] In the formula, It is the gain matrix of the interference observer, which is a constant diagonal matrix. C is an auxiliary variable added to construct the interference observer. The abbreviation of .

[0164] Differentiate equation (21) and subtract from both sides The dynamic equation for the perturbation estimation error is obtained as follows:

[0165]

[0166] When the interaction speed is low, the disturbance torque changes slowly and can be approximated as... In this case, the observer error (23) is asymptotically stable. When the interaction speed is high and At that time, if and If all are bounded, then the estimation error of the disturbance torque is... It is consistent and ultimately bounded.

[0167] Use directly Interference compensation can cause the zero-space compliance effect to disappear. To maintain zero-space compliance, the estimated torque can be projected onto the task space corresponding to the virtual fixture constraint before compensation, i.e.:

[0168]

[0169] Wherein, projection matrix for:

[0170]

[0171] τ u This represents the disturbance moment in the task space.

[0172] In this embodiment, by integrating an interference observer, the robustness to external interference and dynamic uncertainties is significantly enhanced.

[0173] Example 4:

[0174] The virtual gripper dynamics control system with arbitrary differentiable geometric constraints in this embodiment can be applied to electronic devices with communication, computing and data storage capabilities. It includes: a robotic arm dynamics model, a virtual gripper and a disturbance observer. The virtual gripper includes a constraint error PD controller and a constraint control quantity model.

[0175] A joint space dynamics model of the robotic arm is used to obtain the joint space parameters of the robotic arm, including position, velocity, and acceleration. Using the Jacobian matrix, a mapping from joint space variables to task space coordinates is established, mapping task space parameters to joint space parameters. Null space projection is used to decouple the joint space control input from the task space control input.

[0176] The joint space impedance model is used to calculate the zero-space acceleration control input.

[0177] A reverse dynamics compensation model is set up to calculate the control torque to be designed based on the joint space acceleration control input to be designed, thereby eliminating dynamic coupling.

[0178] The virtual fixture is used to define arbitrary differentiable scalar constraint equations and constraint control quantities. Differentiating the scalar constraint equations yields the constraint equations for the velocity and acceleration levels. Using the constraint control quantities, joint velocities are mapped to end-effector velocities. The constraint Jacobian is defined to represent the mapping from joint velocity to the constraint rate of change, thus obtaining the constraint control quantity acceleration level constraint equations. PD control is used to calculate the reference constraint control input, constructing the joint space acceleration control input that satisfies the virtual fixture constraints.

[0179] The disturbance observer is used to establish a realistic joint dynamic model of the robotic arm when considering external disturbances and dynamic uncertainties. Combined with the robotic arm dynamic model, a dynamic model of the disturbance estimation error is obtained. The estimated torque is projected onto the task space corresponding to the virtual fixture constraint, and the disturbance is compensated in the task space.

[0180] From the joint space dynamics model of the robotic arm, we obtain the robotic arm's parameter information, including its position, velocity, and acceleration. Based on this parameter information, we perform the following tasks:

[0181] Setting up a virtual fixture includes defining the constraint control quantity u and solving for the first derivative of the constraint control quantity u. and second derivative Obtain the constraint rate of change; define the velocity Jacobian matrix to map joint velocities to end-effector velocities; define the constraint Jacobian to represent the joint velocities. To the constrained rate of change Mapping; using PD control to calculate reference constraint control input.

[0182] The joint space impedance model is used to calculate the zero-space acceleration control input.

[0183] Control input based on reference constraints and zero-space acceleration control input Combined with the null projection matrix N relative to the constraint control quantity u u Calculate the joint acceleration control inputs required to satisfy the virtual fixture constraints.

[0184] The control torque of the robotic arm is calculated based on the joint acceleration control input, the inertia matrix, and the inverse dynamics compensation term.

[0185] An interference observer is used to calculate the estimated interference torque on the robotic arm. The estimated torque is then projected onto the task space corresponding to the virtual fixture constraint and compensated to obtain the control torque of the robotic arm after interference compensation, which is the final control torque.

[0186] It is worth mentioning that all modules involved in this embodiment are logical modules. In practical applications, a logical unit can be a physical unit, a part of a physical unit, or a combination of multiple physical units. Furthermore, to highlight the innovative aspects of this application, this embodiment does not introduce units that are not closely related to solving the technical problems proposed in this application; however, this does not mean that other units are absent in this embodiment.

[0187] The control system in this embodiment designs a virtual fixture control law based on arbitrary differentiable geometric constraints. Based on the dynamic model, it obtains joint acceleration control inputs and zero-space acceleration control inputs that satisfy the virtual fixture constraints. It also achieves compensation for unknown disturbances through a disturbance observer, realizing high-precision constrained human-computer interaction and simplifying the calculation process.

[0188] Example 5:

[0189] The virtual gripper dynamics control system with arbitrary differentiable geometric constraints in this embodiment uses a virtual gripper dynamics control method with arbitrary differentiable geometric constraints to realize the control of the robot arm. The stability analysis of the control system in this embodiment is as follows:

[0190] The tracking error caused by the constraint control quantity is defined as u e=u d -u = -u, the velocity error is defined as...

[0191] Substituting equations (16) and (2) into the system dynamics equation (1), the closed-loop constraint error equation can be derived as follows:

[0192]

[0193] To prove the stability of the system, the following Lyapunov energy function is defined:

[0194]

[0195] Its derivative along the system trajectory is:

[0196]

[0197] When the external disturbance is small, τ can be approximated as... ext =0, at this time there is This proves that the linear system is exponentially stable. When the external disturbance is large, i.e., τ... ext When ≠0, it can be proven that the system possesses input-state stability (ISS). The detailed formulation of the ISS theorem is as follows, including Theorem 1 and Theorem 2:

[0198] Theorem 1:

[0199] If a function exists as well as Makes all and All satisfy the following inequalities:

[0200]

[0201] Then the system It has input-state stability.

[0202] Where ||x(t)|| represents the Euclidean norm of the state variable, and ||u|| ∞ Indicates the input L ∞ Norm refers to the time norm of the input signal u, i.e., sup_{sigma∈[0,t]}|mu(sigma)|, omitting the time variable t.

[0203] According to Theorem 1, it can be proven that the system has input-state stability (ISS), as follows:

[0204] Define state vector The second-order error dynamic equation (27) can be rewritten as:

[0205] ξ=Aξ+Bμ (31);

[0206] in,

[0207] Theorem 2:

[0208] Given any positive definite matrix Q, there exists a unique positive definite matrix p that is the Lyapunov equation A T The solution to P+PA=-Q holds if and only if matrix A is a Hurwitz matrix.

[0209] Based on Theorem 2, the Lyapunov function is defined as V′=ξ. T Pξ, where P is the Lyapunov equation A T The unique positive definite solution to P + PA = -Q.

[0210] Find the derivative of V′ along the error dynamic equation (27), and use Rayleigh's inequality (λ) min (P)‖ξ‖ 2 ≤V′≤λ max (P)‖ξ‖ 2 From this, we can obtain:

[0211] V′=ξ T (A T P+PA)ξ+2ξ T PBμ

[0212] =-ξ T Qξ+2ξ T PBμ

[0213] ≤-λ min (Q)‖ξ‖ 2 +2‖ξ‖‖P‖‖B‖‖μ‖ (32);

[0214] For the cross term 2‖ξ‖‖P‖‖B‖‖μ‖, apply Young's inequality (2ab≤∈a) 2 +b 2 / ∈), choose ∈=λ min (Q) / 2, thus obtaining the upper bound of V′:

[0215]

[0216] To simplify the symbolic expression in the subsequent proof, the following variables are defined:

[0217] α1∶=λ min (p) (34);

[0218] α2∶=λ max (p) (35);

[0219]

[0220] By analyzing the inequality ||μ(t)|| 2 By taking the supremum and solving the differential inequality, we obtain the upper bound of V′(t):

[0221]

[0222] In the formula, 0 represents the starting point of the time interval, and the subscript 0 represents the physical quantity corresponding to time 0. V′0 represents the initial Lyapunov energy.

[0223] Next, applying Rayleigh's inequality again to eliminate V′, we obtain ‖ξ(t)‖ 2 Upper bound:

[0224]

[0225] Take the square root of both sides of equation (39), and according to the domain, divide (1-e) -κt Scaling the constraint error ‖ξ(t)‖ to 1, we obtain the upper bound of ‖ξ(t)‖ as follows, which satisfies the conditions of Theorem 1, thus completing the proof of ISS:

[0226]

[0227] System status The time-varying process is constrained by inequality (40), the upper bound of which consists of two parts. The transient term decays exponentially at a rate of κ / 2, the decay rate κ and the gain coefficient All depend on the constraint stiffness coefficient matrix K u and damping coefficient matrix D u The eigenvalues ​​of . The steady-state term represents the worst-case effect of external disturbances on the constraints, and its gain coefficient is .

[0228] μ(t) represents the external torque τ ext The geometric coupling relationship between the constraints imposed by the virtual fixture and the constraint is expressed through the Jacobian matrix J. p (q), inertia matrix M(q), and constraint gradient transfer.

[0229] Equation (40) can be used to analyze the impact of key parameters. External disturbances have a significant impact, therefore choosing an appropriate form for the constraint F(p) is crucial. The Jacobian matrix J... pThe inertial matrix M(q) and the inertial matrix M(q) are matrices related to the robot arm's configuration. Their norms increase under singular poses, amplifying the impact of external disturbances on the system state. Therefore, singular positions should be avoided in motion planning to minimize this effect. Furthermore, a disturbance observer (DO) is used to estimate and compensate for external disturbances in real time, thereby improving constraint accuracy by incorporating disturbance compensation into the control strategy.

[0230] Example 6:

[0231] The virtual gripper dynamics control system with arbitrary differentiable geometric constraints in this embodiment uses a virtual gripper dynamics control method with arbitrary differentiable geometric constraints to realize the control of the robot arm. The experimental results of the control system in this embodiment are as follows:

[0232] Experimental setup:

[0233] External controller: One industrial PC (MIC-770-V2, Advantech, China), equipped with an Intel Core i7-10700 CPU and 8GB of memory. The PC runs Ubuntu 22.04LTS with a PREEMPT_RT real-time kernel. The robotic arm is controlled by an unoptimized C++ framework provided by the manufacturer at a control frequency of 1kHz.

[0234] Franka Emika Research Robot with 7 degrees of freedom.

[0235] Experimental results:

[0236] The initial joint angles of the robotic arm are set to q0 = [0, -π / 4, 0, -3π / 4, 0, π / 2, π / 4]. T The corresponding end effector pose x0 = [2.9025, -1.2022, 0, 0.3069, 0, 0.5903] was obtained through forward kinematics calculation. T The zero-space stiffness and damping coefficient are set to K. n =0 and D n =10I, while the gain of the interference observer is set to Y=I.

[0237] To verify the versatility of the proposed general virtual fixture framework, four typical geometric constraint types were implemented: 1) a planar virtual fixture for planar alignment, 2) a linear virtual fixture for linear guidance, and 3) a linear virtual fixture.

[0238] 3) A circular virtual fixture for simple curve trajectory tracking; 4) A sinusoidal virtual fixture for complex nonlinear path tracking. The selection of these typical constraints aims to demonstrate the adaptability of the method to different constraint shapes.

[0239] The specific constraint formulas for each virtual fixture are listed in Table 1, where the stiffness coefficient is fixed at K. u =3000I, damping coefficient according to calculate.

[0240] Table 1. Constraint Expressions for Four Types of Virtual Fixtures

[0241]

[0242] The experimental verification involved two interactive operation modes: low-speed dragging (average speed: 0.030±0.015m / s), used to evaluate the basic accuracy of the proposed method; and high-speed dragging (average speed: 0.100±0.025m / s), used to evaluate its dynamic disturbance suppression capability. All physical interaction experiments were performed by the same operator to eliminate the influence of different operating habits among different users.

[0243] Experimental validation consists of two parts to systematically evaluate the proposed virtual fixture framework.

[0244] In the first part, low-speed and high-speed drag experiments were conducted on the configuration integrating the Disturbance Observer (DO) under four constraint types. Each experimental configuration was repeated five times to ensure statistical significance. The experiment with the maximum constraint error under each configuration was selected as the representative experiment, and the quantitative results are summarized in Table 2. Note that the error data in the table only calculates the error during the manned / machine interaction period (for unmanned / drone interaction, the error is calculated based on the threshold ||τ|). ext ||>2Nm judgment), so as to pay attention to the impact of external disturbances on the dynamic process.

[0245] Table 2. Constraint errors of representative experiments under different experimental conditions (unit: mm)

[0246]

[0247] Figure 2 The study presents three key pieces of information from a representative experiment: 1) the dragging interaction trajectories under four different virtual constraints, and their corresponding constraint geometries; 2) the constraint error curves of the experiment; and 3) the interaction force τ during the experiment. ext This data is obtained directly from libfranka's RobotState::tau_ext_hat_filtered().

[0248] In the comparative experiment in Part 2, the compensation for the interfering observer was removed, and the resulting constraint error characteristics are as follows: Figure 3 As shown. Under all experimental conditions, the statistical distribution of the constraint error is as follows: Figure 4 A comprehensive comparison was conducted.

[0249] Experimental results demonstrate three characteristics of the proposed method.

[0250] First, this virtual fixture frame achieves sub-millimeter level accuracy (maximum error <1mm) during physical interaction. For example... Figure 2 As shown, the results of the interactive experiment with the observer under four virtual constraint conditions are presented. The first row shows the constraint shapes (reference) and actual trajectories of the ends of the four virtual fixtures; the second row shows the constraint errors (the gray area indicates the sub-millimeter accuracy range); the third row shows the interactive forces during the experiment (cyan and magenta indicate the human-computer interaction ||τ in low-speed / high-speed experiments). ext ||>2Nm of time).

[0251] This demonstrates that, under the experimental configuration with the interference observer, the actual trajectory of the end effector almost perfectly matches the shape of the reference constraint, with its maximum error consistently less than 1 mm. This level of geometric constraint execution accuracy meets the needs of most high-precision physical human-computer interaction scenarios.

[0252] Secondly Figure 3 As shown, the results of the interactive experiment of the interference-free observer are presented under four virtual constraint conditions. Figure 3 The observer-less comparative experiments show that the degree of error accumulation is positively correlated with the constraint complexity: in terms of the maximum constraint error under each experimental configuration, the planar virtual fixture and the linear virtual fixture have the smallest errors, while the sinusoidal virtual fixture produces significantly larger errors (2.04 mm and 3.54 mm) under low-speed and high-speed conditions, respectively. This performance degradation is also accompanied by residual steady-state errors after the force removal phase, and the root cause of this phenomenon lies in the dynamic modeling error of the robotic arm.

[0253] Third, the integrated disturbance observer significantly improves system performance: achieving sub-millimeter accuracy under low-speed conditions, and maintaining maximum error at the sub-millimeter level even when facing sinusoidal trajectories with drastic curvature changes during high-speed operation. Steady-state error is effectively suppressed after external forces are removed. Compared to conditions without an observer, the maximum error of various virtual fixtures is reduced by 52%-78%. The evolution of constraint errors further verifies the dual function of the observer: suppressing transient disturbances during the interactive phase and eliminating steady-state errors during the non-interactive phase.

[0254] Constraint error distribution under different experimental conditions, such as Figure 4 As shown, the error bars represent the interaction process (||τ) ext The mean error ± standard deviation is greater than 2 Nm. An asterisk (*) indicates the maximum error during the interaction process. The reference line at 1 mm marks the sub-millimeter accuracy threshold.

[0255] Example 7:

[0256] Another embodiment of this application relates to an electronic device, including: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to enable the at least one processor to perform the virtual fixture dynamics control method with arbitrary differentiable geometric constraints in the above embodiments.

[0257] The memory and processor are connected via a bus, which can include any number of interconnecting buses and bridges, connecting various circuits of one or more processors and memories. The bus can also connect various other circuits, such as peripheral devices, voltage regulators, and power management circuits, which are well known in the art and will not be described further herein. The bus interface provides an interface between the bus and the transceiver. The transceiver can be a single element or multiple elements, such as multiple receivers and transmitters, providing a unit for communicating with various other devices over a transmission medium. Data processed by the processor is transmitted over the wireless medium via an antenna, which further receives data and transmits it to the processor.

[0258] The processor manages the bus and general processing, and also provides various functions, including timing, peripheral interfaces, voltage regulation, power management, and other control functions. Memory is used to store data used by the processor during operation.

[0259] Example 8:

[0260] Another embodiment of this application relates to a computer-readable storage medium storing a computer program. When executed by a processor, the computer program implements the method embodiments described above.

[0261] That is, those skilled in the art will understand that all or part of the steps in the methods of the above embodiments can be implemented by a program instructing related hardware. This program is stored in a storage medium and includes several instructions to cause a device (which may be a microcontroller, chip, etc.) or processor to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0262] Those skilled in the art will understand that the above embodiments are specific embodiments for implementing this application, and in practical applications, various changes can be made to them in form and detail without departing from the spirit and scope of this application.

Claims

1. A method for controlling the dynamics of a virtual fixture with arbitrary differentiable geometric constraints, characterized in that, include: A joint space dynamics model of the robotic arm is constructed to establish a mapping from joint space variables to task space using the Jacobian matrix, thereby obtaining the velocity and acceleration in the task space. Establish the relationship between joint space acceleration control input, task space acceleration control input, and zero space acceleration control input; The zero-space acceleration control input is calculated using a joint space impedance model. Construct the task space coordinates and use the constraint control quantities and reference constraint control inputs of the virtual fixture; Based on the zero-space acceleration control input and the reference constraint control input, the task space torque control command is derived. The calculation of the null space acceleration control input is shown in the following formula: ； d represents the expected value, and e represents the error. Indicates joint damping. Indicates joint stiffness. and It is a constant diagonal matrix; This indicates the zero-space acceleration control input. Represents joint space acceleration. Represents joint space velocity. Indicates joint angle, Represents the set of real numbers, representing It is an n-dimensional real vector; n represents the number of degrees of freedom of the robotic arm. The inverse matrix of the inertia matrix; The process of deriving the mission space torque control command based on the zero-space acceleration control input and the reference constraint control input includes: Joint acceleration control inputs that satisfy virtual fixture constraints: ； In the formula, , representing relative to the constraint control quantity The null projection matrix, Calculate the acceleration control input to be designed based on equations (9) and (16); Establish the inverse dynamic compensation term: ； Calculate the control torque to be designed based on equations (9), (16) and (2).

2. The virtual fixture dynamics control method with arbitrary differentiable geometric constraints according to claim 1, characterized in that, The robotic arm joint space dynamics model includes: For an n-DOF robotic arm, its joint space dynamics equations are as follows: ； in, Indicates joint angle, Let represent the set of real numbers, and let q be an n-dimensional real vector; n represents the number of degrees of freedom of the robotic arm. Represents the inertia matrix; Represents the Coriolis force and centrifugal force; Represents the gravity term; Indicates control torque. Indicates external force; Using Jacobi matrix According to the joint space velocity Joint velocity in mission space From the kinematic relationship between them, the acceleration in the task space can be obtained. With joint space velocity Joint space acceleration The relationship is: ； Based on the general solution of equation (6), the joint space acceleration control input is obtained. As shown in the following formula: ； In the formula, It is the task space acceleration control input. It is a dynamically consistent right-handed generalized inverse. , represents the null projection matrix. This represents the inertia matrix.

3. The virtual fixture dynamics control method with arbitrary differentiable geometric constraints according to claim 1, characterized in that, It also includes a disturbance observer for calculating the total disturbance moment, taking into account external disturbances and dynamic uncertainties. : ； Compensation is performed after projecting the estimated torque onto the task space corresponding to the virtual fixture constraints. ； In the formula, Represents the perturbation, projection matrix for: ； This represents the disturbance moment in the task space.

4. The virtual fixture dynamics control method with arbitrary differentiable geometric constraints according to claim 1, characterized in that, include: The geometry of the virtual fixture can be given analytically and is at least twice differentiable, provided that the geometry of the constraints depends only on the spatial position: Define the scalar constraint equations and constraint control quantities of the virtual fixture, construct the joint Jacobian matrix of multiple constraints, and calculate the reference constraint control input using PD control based on the constraint control expectation value and constraint control error value.

5. The virtual fixture dynamics control method with arbitrary differentiable geometric constraints according to claim 4, characterized in that, For general curves or surfaces, the scalar constraint equation of the virtual fixture is as follows: ； Define constraint control quantity Solve for the constraint control quantity The first derivative is used to define the velocity Jacobian matrix and the constraint Jacobian matrix. Taking the derivative of the first derivative again yields the acceleration level relationship of the constraint control quantity. PD control is then used to calculate the reference constraint control input. , ； In the formula, , , , Let represent the proportional-derivative gain of the virtual fixture constraint error, d represent the desired value, e represent the error, and c represent the control. This indicates the position of the end effector in the world coordinate system.

6. The virtual fixture dynamics control method with arbitrary differentiable geometric constraints according to claim 4, characterized in that, For the case of multiple constraints, the scalar constraint equation of the virtual fixture is as follows: In the formula, r represents the number of constraints, which is greater than or equal to 1; Constraint control quantity The definition is as follows: ； definition To constrain the Jacobian matrix, representing the joint velocity To the constrained rate of change The mapping, The joint Jacobian matrix with multiple constraints is: ； In the formula, n represents the degrees of freedom of the robotic arm. Indicates joint angle, Represents the set of real numbers; This indicates the position of the end effector in the world coordinate system. express gradient, This represents the velocity Jacobian matrix.

7. A virtual fixture dynamics control system with arbitrary differentiable geometric constraints, characterized in that, A virtual gripper dynamics control method with arbitrary differentiable geometric constraints as described in any one of claims 1-6, comprising: a robotic arm dynamics model, a virtual gripper, and a disturbance observer; The robotic arm dynamics model is used to obtain the joint space parameters of the robotic arm. Using the Jacobian matrix, a mapping from joint space variables to task space coordinates is established, and the task space parameters are mapped to the joint space parameters. Null space projection is used to decouple the joint space control input from the task space control input. The null space acceleration control input is calculated using the joint space impedance model. The virtual fixture is used to define arbitrary differentiable scalar constraint equations and constraint control quantities. Differentiating the scalar constraint equations yields the constraint equations for the velocity and acceleration levels. Using the constraint control quantities, joint velocities are mapped to end-effector velocities. The constraint Jacobian is defined to represent the mapping from joint velocity to constraint rate of change, thus obtaining the constraint control quantity acceleration level constraint equations. PD control is used to calculate the reference constraint control input, and the joint space acceleration control input that satisfies the virtual fixture constraints is constructed. The disturbance observer is used to establish a realistic joint dynamic model of the robotic arm when considering external disturbances and dynamic uncertainties. Combined with the robotic arm dynamic model, a dynamic model of the disturbance estimation error is obtained. The estimated torque is projected onto the task space corresponding to the virtual fixture constraint, and the disturbance is compensated in the task space.

8. An electronic device, characterized in that, include: At least one processor; as well as, A memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor to enable the at least one processor to perform a virtual fixture dynamics control method with arbitrary differentiable geometric constraints as described in any one of claims 1 to 6.

Images