Arbitrary micro-geometric constraint virtual fixture, dynamics control method and system

Abstract

The invention discloses a random micro-geometric constraint virtual fixture, a dynamic control method and a system, and the random micro-geometric constraint virtual fixture is characterized in that the geometric form of the virtual fixture can be given in an analytical form, and at least two orders are differentiable; under the condition that the geometric form of the constraint only depends on the spatial position, a scalar constraint equation and a constraint control quantity of the virtual fixture are defined, a multi-constraint joint Jacobian matrix is constructed, and reference constraint control input is calculated by adopting PD control based on a constraint control expected value and a constraint control error value. According to the arbitrary micro-geometric constraint virtual fixture dynamic control method, null space control input of a mechanical arm is calculated based on a dynamic model, reference constraint control input is calculated through a virtual fixture, and compensation is carried out after estimated torque is projected to a task space corresponding to virtual fixture constraint through a disturbance observer. According to the virtual fixture, the problems of poor universality and heavy calculation burden are solved, calculation is simplified, and the universality is improved.

Description

Technical Field

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

[0002] In recent years, the improvement of robot technology in terms of high motion accuracy, continuous operation ability, and perception system has promoted the wide application of robots in multiple fields. Although there are still challenges such as insufficient intelligence and limited autonomous decision-making ability in complex environments, robots have shown advantages over humans in a large number of tasks. Therefore, more and more researchers are committed to the research of human-robot collaborative control strategies to achieve effective assistance for human operations. However, human-robot physical interaction brings unpredictable external dynamic disturbances to robot control, making human-robot collaborative control more complex than controlling a robot alone. Interaction safety, operation accuracy, and anti-interference ability have become the core challenges in the human-robot interaction scenario. In order to handle the safety issues of human-robot interaction in complex environments, it is necessary to constrain the motion path and motion range of the robot. In this context, virtual fixture (VF) technology has developed into an important robotic arm control method for guiding the robotic arm along a predetermined path or restricting the robotic arm to move within a constrained area.

[0003] According to the constraint implementation method, the existing virtual fixture methods are mainly divided into two categories: the reference point method and the projection method. The reference point method solves the nearest point satisfying the constraint through an analytical method or a numerical optimization algorithm according to the geometric relationship between the constraint and the current pose of the tool. This method combines an impedance model and pose data to generate control inputs to reduce the constraint error. Although it can accurately model the interaction force and compliance characteristics, it needs to separately solve the analytical expression of the control quantity for each type of geometric constraint, resulting in problems of insufficient generality and too high real-time calculation cost. In contrast, the projection method projects the external force or velocity onto the constraint direction, transforms the constrained motion control into an unconstrained problem for processing, and finally can implement the virtual fixture with an unconstrained motion control strategy. A variant of the projection method is the Jacobian matrix method, which directly projects the constraint error into the joint space. Compared with the conventional projection method, its control law design is more flexible.

[0004] According to the classification of control models, virtual fixture methods can be divided into kinematics-based control methods and dynamics-based control methods. Kinematic methods rely on the robot kinematic model to generate virtual constraints through geometric relation analysis or numerical solutions. Although these methods have the advantages of simple implementation and high real-time performance, they do not consider dynamic factors such as inertia and friction. In contrast, dynamics-based control methods take into account the system dynamics characteristics and achieve constraints by directly regulating joint accelerations and torques. Such methods usually incorporate robust control strategies to compensate for external disturbances and model uncertainties, thereby achieving more accurate and stable control in complex dynamic environments. However, the introduction of non-linear dynamics increases the system complexity, and existing research on dynamic virtual fixtures mainly focuses on simple linear constraints, lacking a general method applicable to general geometric constraints.

[0005] Despite the significant progress made by virtual fixture technology in the field of constraint control, there are still the following key challenges: First, most existing methods are custom-designed for specific geometric constraints and lack generality. Therefore, when dealing with diverse and complex geometric constraint systems, a large number of modifications and optimizations are often required. Second, existing methods generally rely on kinematic models and have limited robustness under complex environmental disturbances, especially when dealing with dynamic uncertainties and external force disturbances. Finally, the real-time performance and computational complexity of virtual fixture methods are also key factors restricting the wide application of virtual fixture methods. Especially in high-precision tasks, how to optimize remains an urgent problem to be solved. Summary of the Invention

[0006] The purpose of the present invention is to provide at least a virtual fixture, a dynamics control method, and a system for any differentiable geometric constraint, which can at least solve the technical problems of the lack of generality of virtual fixtures, poor performance in dealing with dynamic uncertainties and external force disturbances, and reducing the computational burden on the premise of ensuring robustness, and can at least achieve improved generality, enhanced robustness in dealing with dynamic uncertainties and external force disturbances, simplified calculation, and improved efficiency.

[0007] To solve the above technical problems, at least one embodiment of the present application provides a virtual fixture for any differentiable geometric constraint, characterized in that it includes: assuming that the geometric form of the virtual fixture can be given in an analytical form and is at least second-order differentiable, the geometric form of the constraint depends only on the spatial position, defining the virtual fixture form to simultaneously satisfy multiple non-conflicting constraints, defining the constraint control quantity, constructing the joint Jacobian matrix of multiple constraints, and calculating the reference control input.

[0008] At least one embodiment of the present application further provides a virtual fixture dynamics control method for any differentiable geometric constraint, including: constructing a robotic arm joint space dynamics model for using the Jacobian matrix to establish a mapping from joint space variables to the task space, and obtaining the velocity and acceleration of the task space; establishing the relationship between the joint space acceleration control input, the task space acceleration control input, and the null space acceleration control input; using the joint space impedance model to calculate the null space acceleration control input; constructing the task space coordinates, and using the constraint control quantity and the reference constraint control input of the virtual fixture described in the present application; and deriving the task space torque control command according to the null space acceleration control input and the reference constraint control input.

[0009] At least one embodiment of the present application further provides a virtual fixture dynamics control system for any differentiable geometric constraint, including a robotic arm dynamics model, a virtual fixture, and a disturbance observer; the robotic arm dynamics model is used to obtain the joint space parameters of the robotic arm, use the Jacobian matrix to establish a mapping from joint space variables to task space coordinates, map the task space parameters to joint space parameters, use null space projection to decouple the joint space control input and the task space control input, and use the joint space impedance model to calculate the null space acceleration control input; the virtual fixture is used to define any differentiable scalar constraint equation and constraint control quantity, take the derivative of the scalar constraint equation to obtain the constraint equations at the velocity level and the acceleration level; use the constraint control quantity to map the joint velocity to the end velocity, define the constraint Jacobian to represent the mapping from the joint velocity to the constraint change rate, obtain the constraint control quantity acceleration level constraint equation, use PD control to calculate the reference constraint control input, and construct the joint space acceleration control input that satisfies the virtual fixture constraint; the disturbance observer is used to establish a real robotic arm joint dynamics model when considering external disturbances and dynamic uncertainties, combine it with the robotic arm dynamics model to obtain the dynamic model of the disturbance estimation error, project the estimated torque to the task space corresponding to the virtual fixture constraint, and compensate for the disturbance in the task space.

[0010] Based on the dynamics model, it is possible to achieve more precise and stable control in a complex dynamic environment. By establishing a virtual fixture, the constraint control quantity is simplified and at least second-order differentiable, realizing the calculation at the velocity level and the acceleration level based on the spatial position. Combining with the disturbance observer improves the torque control accuracy.

[0011] At least one embodiment of the present application further 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, and the instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned virtual fixture dynamics control method for any differentiable geometric constraint.

[0012] At least one embodiment of the present application further provides a computer-readable storage medium storing a computer program, which when executed by a processor implements the above-mentioned virtual fixture dynamics control method with any differentiable geometric constraint.

[0013] The virtual fixture, dynamics control method and system with any differentiable geometric constraint provided by the embodiments of the present application have a virtual fixture that can be at least second-order differentiable, the geometric form of the constraint only depends on the spatial position, and the constraint control quantity and constraint equation can adapt to any curve and surface, improving the versatility of the virtual fixture and reducing the computational burden on the premise of ensuring robustness.

[0014] In some alternative embodiments, for a general curve or surface, the scalar constraint equation of the virtual fixture is as shown in the following formula:

[0015] F(p) = 0;

[0016] Define the constraint control quantity Solve the first derivative of the constraint control quantity u, define the velocity Jacobian matrix and the constraint Jacobian matrix, and take the derivative of the first derivative again to obtain the acceleration-level relationship of the constraint control quantity, and use PD control to calculate the reference constraint control input

[0017]

[0018] In the formula, K u 、D u respectively represent the proportional and differential gains of the virtual fixture constraint error, d represents the expected value, e represents the error, c represents the control, represents the position of the end effector in the world coordinate system.

[0019] Based on the simplified constraint control quantity, through derivation and PD operation, combined with the expected value and the error, the reference constraint control input is obtained, enabling the robot system to move to meet the defined constraints with high precision, ensuring that the null-space compliance does not affect the virtual constraint main task, and using PD control to calculate the reference constraint control input to ensure that the tracking error of the constraint function converges exponentially.

[0020] In some alternative embodiments, for the multi-constraint case, the scalar constraint equation of the virtual fixture is as shown in the following formula:

[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 as follows:

[0025]

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

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

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

[0029] For an n-degree-of-freedom robotic arm, its joint space dynamics equation is as follows:

[0030]

[0031] Among them, represents the joint angle, represents the set of real numbers, indicating that q is an n-dimensional real vector; n represents the degree of freedom of the robotic arm, and M(q) represents the inertia matrix; represents the Coriolis force and centrifugal force; G(q) represents the gravity term; τ represents the control torque, τ ext represents the external force, represents the joint space velocity, represents the joint space acceleration;

[0032] Using the Jacobian matrix According to the kinematic relationship between the joint space velocity and the task space joint velocity , the task space acceleration and the joint space velocity joint space acceleration The relationship is:

[0033]

[0034] According to the general solution of formula (6), the joint space acceleration control input is shown as follows:

[0035]

[0036] In the formula, is the task space acceleration control input, while is the null space acceleration control input;

[0037] is a right generalized inverse that is kinematically consistent, and N = I - J # J represents the null space projection matrix, and M represents the inertia matrix.

[0038] Using the dynamic model, it is possible to achieve more precise and stable control in a complex dynamic environment, associate joint space control with task space control, and decouple task space control input from null space control input. (Dependent claim + effect).

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

[0040]

[0041] where d represents the expected value, e represents the error, c represents the control, represents the joint damping, represents the joint stiffness, D n and K n are constant diagonal matrices.

[0042] Using the joint space impedance model to calculate the null space control input realizes the compliance of the null space.

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

[0044] The joint acceleration control input that satisfies the virtual fixture constraint:

[0045]

[0046] where represents the null space projection matrix with respect to the constraint control quantity u, and I represents the identity matrix;

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

[0048] Establish the inverse dynamics compensation term:

[0049]

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

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

[0052] In some alternative embodiments, a disturbance observer is further included, which is used to calculate the total disturbance torque while considering external disturbances and dynamic uncertainties.

[0053]

[0054] The estimated torque is projected onto the task space corresponding to the virtual fixture constraint and then compensated:

[0055]

[0056] In the formula, represents the disturbance, and the projection matrix is:

[0057]

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

[0059] By using the disturbance observer, the estimated torque in the null space is projected onto the task space corresponding to the virtual fixture constraint and then compensated, avoiding the disappearance of the null space compliance effect caused by disturbance compensation. (Dependent claim + effect). BRIEF DESCRIPTION OF THE DRAWINGS

[0060] One or more embodiments are exemplarily illustrated by the pictures in the corresponding drawings, and these exemplary illustrations do not constitute limitations on the embodiments.

[0061] Figure 1 FIG. is a schematic diagram of a control method provided by an embodiment of the present application;

[0062] Figure 2 FIG. is the experimental result of the control method provided by an embodiment of the present application Figure 1 ;

[0063] Figure 3 FIG. is the experimental result of the control method provided by an embodiment of the present application Figure 2 ;

[0064] Figure 4 FIG. is the experimental result of the control method provided by an embodiment of the present application Figure 3 . DETAILED DESCRIPTION OF THE EMBODIMENTS

[0065] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the following will elaborate on each embodiment of this application in conjunction with the accompanying drawings. However, those of ordinary skill in the art can understand that in each embodiment of this application, many technical details are provided to help readers better understand this application. However, even without these technical details and various changes and modifications based on the following embodiments, the technical solutions claimed in this application can still be implemented. The following division of each embodiment is for convenience of description and should not impose any limitation on the specific implementation manner of this application. Each embodiment can be combined and cross-referenced with each other on the premise of not being contradictory.

[0066] To solve the above technical problem of how to optimize virtual fixtures, the present invention proposes a virtual fixture with arbitrary differentiable geometric constraints, a dynamic control method, and a system method. The following specifically describes the implementation details of the virtual fixture with arbitrary differentiable geometric constraints, the dynamic control method, and the system method in this embodiment. The following content is only implementation details provided for convenience of understanding and is not necessary for implementing this solution.

[0067] Embodiment 1:

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

[0069] When the geometric form of the virtual fixture can be given in an analytical form and is at least second-order differentiable to ensure feasible acceleration-level control, and at the same time, the geometric form of the constraint only depends on the spatial position, based on the above settings, a general curve or surface virtual fixture can be defined as a set of scalar constraint equations in the form of F(p) = 0. Wherein, represents the position of the end effector in the world coordinate system.

[0070] Differentiate the scalar constraint equation continuously to obtain the velocity-level and acceleration-level constraint equations respectively:

[0071]

[0072] In the formula, is the gradient of F(p), and H 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 target x, define the constraint control quantity x represents the actual value of the Cartesian motion control target, x c represents the control input of the Cartesian motion control target, and x d represents the expected value of the Cartesian motion control target.

[0074] Construct the acceleration control input that satisfies the virtual fixture constraints according to the constraint control quantity u

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

[0076]

[0077] where is the velocity Jacobian matrix that maps the joint velocity to the end - effector velocity and satisfies

[0078] Define as the constraint Jacobian matrix, which represents the mapping from the joint velocity to the constraint rate of change of the mapping.

[0079] Differentiate Equation (12) to obtain the acceleration - level relationship of the second derivative of the constraint control quantity:

[0080]

[0081] Use PD control to calculate the reference constraint control input, which can be regarded as the actual control input to ensure that the tracking error of the constraint function converges exponentially. For the virtual fixture constraint control quantity, the corresponding expected values and their respective derivatives are:

[0082]

[0083] Derive the reference control input as:

[0084]

[0085] where K u and D u respectively represent the proportional - derivative gains of the virtual fixture constraint error, d represents the desired value, e represents the error, and c represents the control.

[0086] For the virtual fixture form with multiple constraints, that is, the movement of the end - effector of the robotic arm must simultaneously satisfy multiple non - conflicting constraints where r represents the number of constraints, the constraint control quantity and the Jacobian matrix are extended to include all applied constraints, and the constraint control quantity u is defined as follows:

[0087]

[0088] Construct the joint Jacobian matrix for multiple constraints as:

[0089]

[0090] Generalize the constraint error proportional derivative gain in Equation (16) to a more general form, that is Both are symmetric positive definite matrices. The constrained control quantity in the case of multiple constraints is denoted by bold u to distinguish it from the constrained control quantity in the case of a single constraint.

[0091] The virtual fixture method of this application has a simple constraint system and can effectively implement any second-order differentiable geometric constraint without relying on an optimization algorithm, which can reduce the computational burden while ensuring robustness.

[0092] Example 2:

[0093] The virtual fixture dynamic control method for any differentiable geometric constraint in this embodiment can be applied to an electronic device with communication, computing, and data storage capabilities, including:

[0094] Construct a dynamic model of the robotic arm joint space;

[0095] Specifically, use the Jacobian matrix to establish a mapping from joint space variables to the task space. According to the kinematic relationship between joint space velocity and task space velocity, the joint space includes a null space and a constraint space. Establish the relationship between the joint space acceleration control input, the task space acceleration control input, and the null space acceleration control input. According to the null space acceleration control input, obtain the acceleration control input of the task space.

[0096] Adopt a joint space impedance model to calculate the null space acceleration control input;

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

[0098] Specifically, assume that the geometric form of the virtual fixture can be given in an analytical form and is at least second-order differentiable, and the geometric form of the constraint depends only on the spatial position. Construct a scalar constraint equation of the virtual fixture to obtain the velocity-level and acceleration-level constraint equations. Set the constrained control quantity, calculate the reference constrained control input using PD control, and combine the acceleration-level constraint equation to calculate the joint acceleration required for robotic arm control or the joint acceleration required for implementing the virtual fixture.

[0099] The joint space includes a constraint space and a null space.

[0100] Adopt a disturbance observer for disturbance estimation.

[0101] Specifically, construct a dynamic model of the robotic arm, including:

[0102] For an n-degree-of-freedom robotic arm, its joint space dynamic equation is as follows:

[0103]

[0104] wherein, represents the joint angle, represents the set of real numbers, indicating that q is an n-dimensional real vector; n represents the degree of freedom of the robotic arm, and M(q) represents the inertia matrix; represents the Coriolis force and centrifugal force; G(q) represents the gravity term; τ represents the control torque, and τ ext represents the external force, represents the joint space velocity, represents the joint space acceleration.

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

[0106]

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

[0108] The to-be-designed here is to present the concept first in the control framework but without specific implementation, and the specific implementation is given later.

[0109] Let the task space coordinates of the robotic arm be For a robotic arm with task redundancy, m < n.

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

[0111] According to the kinematic relationship between the joint space velocity and the task space joint velocity the following equation is obtained:

[0112]

[0113] The general solution of Equation (3) is shown as follows:

[0114]

[0115] wherein, J # represents the Moore-Penrose generalized inverse of J(q), N represents the null space projection matrix, is an arbitrary vector,; is the dynamically consistent right generalized inverse; T represents the transpose matrix, and I represents the identity matrix.

[0116] N = I - J # J (5).

[0117] Taking the derivative of Equation (3) with respect to time, the task - space acceleration can be obtained. and the velocity in the joint space and acceleration are related as follows:

[0118]

[0119] The general solution of Equation (6) is shown as follows:

[0120]

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

[0122]

[0123] where and represent the new control inputs, is the task - space acceleration control input, while is the null - space acceleration control input. The null - space projection method is adopted to decouple the task - space acceleration control input and the joint - space acceleration control input.

[0124] The null - space impedance model is adopted to calculate the null - space acceleration control input to achieve the compliance of the null - space:

[0125]

[0126] where d represents the desired value (Desired), e represents the error (Error), c represents the control, represents the joint damping, represents the joint stiffness, D n and K n are constant diagonal matrices. represents the desired value of the null - space acceleration control input and serves as the feed - forward input; represents the error value of the null - space velocity control input, q e represents the error value of the null - space joint position.

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

[0128] If the joint stiffness is non - zero, the desired joint position q d should not conflict with the desired task - space position, otherwise the accuracy of the task - space motion control will be reduced.

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

[0130] Based on the virtual fixture in the first specific embodiment, when the geometric form of the virtual fixture can be given in an analytical form and is at least second - order differentiable, thus ensuring feasible acceleration - level control, and at the same time, the geometric form of the constraint depends only on the spatial position. Based on the above settings, a general - curve or - surface virtual fixture can be defined as a set of scalar constraint equations in the form of F(p)=0. Where, represents the position of the end - effector in the world coordinate system.

[0131] Differentiate the scalar constraint equation continuously to obtain the velocity - level and acceleration - level constraint equations respectively:

[0132]

[0133] In the formula, is the gradient of F(p), and H 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 target x, define the constraint control quantity x represents the actual value of the Cartesian motion control target, x c represents the control input of the Cartesian motion control target, x d represents the expected value of the Cartesian motion control target.

[0135] According to the constraint control quantity u, construct the acceleration control input that satisfies the virtual - fixture constraint

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

[0137]

[0138] In the formula, is the velocity Jacobian matrix, which maps the joint velocity to the end - effector velocity and satisfies

[0139] Define as the constraint Jacobian, which represents the mapping from the joint velocity to the constraint change rate of.

[0140] Differentiate Equation (12) to obtain the acceleration - level relationship of the constraint control quantity:

[0141]

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

[0143]

[0144] The derived reference control input is:

[0145]

[0146] In the formula, K u and D u respectively represent the proportional differential gains of the virtual fixture constraint error. Although they have different subscripts from those in Equation (9), their physical properties and impacts on the system are similar, and the design principles are the same. Using the same symbol emphasizes the relevance, and only different subscripts are needed for distinction.

[0147] Combined with the acceleration-level constraint equation (8), substituting Equation (15) into the general solution of Equation (13), the joint acceleration control input required to satisfy the virtual fixture constraint for the manipulator control can be solved:

[0148]

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

[0150] According to Equation (9), Equation (16), and Equation (2), the control torque τ is obtained.

[0151] The dynamic control method of this embodiment decouples the constraint space and the null space, realizing the parallel execution of high-precision virtual fixture constraint of the main task and null space compliance control. The high-precision virtual fixture in the constraint space ensures accurate movement under human-machine interaction. The null space compliance control enables the manipulator posture to adapt to external forces and avoid conflicts with the task space target.

[0152] Embodiment 3:

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

[0154] Construct an interference observer:

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

[0156]

[0157] where M r (q) represents the inertia matrix corresponding to the true robotic arm dynamic parameters, represents the Coriolis force and centrifugal torque corresponding to the true robotic arm dynamic parameters, and G r (q) represents the gravity torque corresponding to the true robotic arm dynamic parameters.

[0158] The lumped disturbance torque can be expressed as follows:

[0159]

[0160] where represents the disturbance.

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

[0162]

[0163] where is the gain matrix of the disturbance observer and is a constant diagonal matrix. is the auxiliary variable added for constructing the disturbance observer, and C is an abbreviation of

[0164] Differentiate Equation (21) and subtract from both sides to obtain the dynamic equation of the disturbance estimation error:

[0165]

[0166] When the interaction speed is low, the disturbance torque changes slowly, and it can be approximately considered that In this case, the observer error (23) is asymptotically stable. When the interaction speed is high and If and are both bounded, then the disturbance torque estimation error is uniformly ultimately bounded.

[0167] Directly using for disturbance compensation will cause the null-space compliance effect to disappear. To maintain null-space compliance, the estimated torque can be projected onto the task space corresponding to the virtual fixture constraint and then compensated, i.e.:

[0168]

[0169] where the projection matrix is:

[0170]

[0171] τ u represents the disturbing torque in the task space.

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

[0173] Embodiment 4:

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

[0175] The 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 to map task space parameters to joint space parameters, and null space projection is used to decouple the joint space control input and the task space control input.

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

[0177] An inverse dynamics compensation term model is set up to calculate the to-be-designed control torque based on the to-be-designed joint space acceleration control input to eliminate dynamic coupling.

[0178] The virtual fixture is used to define any differentiable scalar constraint equation and constraint control quantity of the virtual fixture, differentiate the scalar constraint equation to obtain the constraint equations at the velocity level and acceleration level; using the constraint control quantity, map the joint velocity to the end velocity, define the constraint Jacobian to represent the mapping from joint velocity to the constraint change rate, obtain the constraint control quantity acceleration level constraint equation, and use PD control to calculate the reference constraint control input to construct the joint space acceleration control input that satisfies the virtual fixture constraint.

[0179] The disturbance observer is used to establish the real robotic arm joint dynamics model when considering external disturbances and dynamic uncertainties, combine with the robotic arm dynamics model to obtain the dynamic model of the disturbance estimation error, project the estimated torque onto the task space corresponding to the virtual fixture constraint, and compensate for the disturbance in the task space.

[0180] From the joint space dynamics model of the robotic arm, obtain the parameter information of the robotic arm, including the position, velocity, and acceleration of the robotic arm, and based on the parameter information of the robotic arm in the joint space, perform the following operations:

[0181] Set up a virtual fixture, including defining the constraint control quantity u and solving the first derivative of the constraint control quantity u and the second derivative obtain the constraint change rate; define the velocity Jacobian matrix to map the joint velocity to the end - effector velocity, and define the constraint Jacobian to represent the mapping of the joint velocity to the constraint change rate ; adopt PD control to calculate the reference constraint control input

[0182] Adopt the joint - space impedance model to calculate the null - space acceleration control input

[0183] According to the reference constraint control input and the null - space acceleration control input combine with the null - space projection matrix N with respect to the constraint control quantity u u , and calculate the joint acceleration control input required to satisfy the virtual fixture constraint

[0184] According to the joint acceleration control input, the inertia matrix and the inverse - dynamics compensation term, calculate the manipulator control torque.

[0185] Adopt a disturbance observer to calculate the estimated disturbance torque acting on the manipulator, project the estimated torque into the task space corresponding to the virtual fixture constraint and then compensate it to obtain the control torque of the manipulator after compensating for the disturbance, that is, the final control torque.

[0186] It should be noted that each module involved in this embodiment is a logical module. In practical applications, a logical unit can be a physical unit, a part of a physical unit, or can be implemented by a combination of multiple physical units. In addition, in order to highlight the innovative part of this application, units not closely related to solving the technical problems proposed in this application are not introduced in this embodiment, but this does not mean that there are no other units in this embodiment.

[0187] The control system of this embodiment designs a virtual fixture control law based on any differentiable geometric constraint, obtains the joint acceleration control input and the null - space acceleration control input that satisfy the virtual fixture constraint based on the dynamic model, and realizes the compensation of unknown disturbances through a disturbance observer, achieving high - precision constrained human - robot interaction and simplifying the calculation process.

[0188] Example Five:

[0189] The virtual fixture dynamics control system with any differentiable geometric constraint of this embodiment adopts the virtual fixture dynamics control method with any differentiable geometric constraint to realize the control of the robot manipulator. The stability analysis of the control system of 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 speed error is defined as

[0191] Substituting equations (16) and (2) into the system dynamics equation (1), the closed-loop constraint quantity 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, it can be approximately considered that τ ext = 0, and at this time there is Thus, it can be proved that the linear system is exponentially stable. When the external disturbance is large, that is, τ ext ≠ 0, it can be proved that the system has input-to-state stability (ISS). The detailed statement of the ISS theorem is as follows, including Theorem 1 and Theorem 2:

[0198] Theorem 1:

[0199] If there exist functions and such that for all and the following inequalities are satisfied:

[0200]

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

[0202] Among them, ‖x(t)‖ represents the Euclidean norm of the state variable, and ‖u‖ ∞ represents the L ∞ norm of the input, which refers to the norm of the input signal u in time, that is, sup_{sigma∈[0,t]}|mu(sigma)|, and the time variable t is omitted.

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

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

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

[0206] wherein,

[0207] Theorem 2:

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

[0209] Based on Theorem 2, define the Lyapunov function as V' = ξ T Pξ, wherein, P is the unique positive definite solution of the Lyapunov equation A T P + PA = -Q.

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

[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 the Young inequality (2ab ≤ ∈a 2 + b 2 / ∈), select ∈ = λ min (Q) / 2, thus obtaining the upper bound of V':

[0215]

[0216] wherein, to simplify the symbolic expression in the subsequent proof, define the variable:

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

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

[0219]

[0220] By taking the supremum of ‖μ(t)‖ in the inequality 2 and solving the differential inequality, an upper bound for V′(t) is obtained:

[0221]

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

[0223] Next, applying the Rayleigh inequality again to eliminate V′, an upper bound for ‖ξ(t)‖ is obtained: 2 :

[0224]

[0225] Taking the square root of both sides of Equation (39) and scaling (1 - e -κt ) to 1 according to the domain, the upper bound of the constraint error ‖ξ(t)‖ is obtained as follows, satisfying the conditions of Theorem 1, thus completing the ISS proof:

[0226]

[0227] The time-varying process of the system state is constrained by Inequality (40), and the upper bound of this inequality consists of two parts. The transient term decays exponentially at a rate of κ / 2, and the decay rate κ and the gain coefficient both depend on the eigenvalues of the constraint stiffness coefficient matrix K u and the damping coefficient matrix D u . The steady-state term represents the worst influence of the external disturbance on the constraint, and its gain coefficient is

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

[0229] Equation (40) can be used to analyze the influence of key parameters and has an important impact on external disturbances. Therefore, it is crucial to select an appropriate form for the constraint F(p). And the Jacobian matrix J p(q) and the inertia matrix M(q) are matrices related to the configuration of the robotic arm. At singular poses, their norms increase, which will further amplify the impact of external disturbances on the system state. Therefore, singular positions should be avoided in motion planning to minimize this impact. In addition, a Disturbance Observer (DO) is used to estimate and compensate for external disturbances in real time, thereby improving the constraint accuracy by incorporating disturbance compensation into the control strategy.

[0230] Example Six:

[0231] The virtual fixture dynamics control system with any differentiable geometric constraint in this example adopts the virtual fixture dynamics control method with any differentiable geometric constraint to achieve the control of the robotic arm. The experimental results of the control system in this example are as follows:

[0232] Experimental Setup:

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

[0234] A 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 , and the corresponding end-effector pose x0 = [2.9025, -1.2022, 0, 0.3069, 0, 0.5903] is obtained through forward kinematics calculation. T . The null-space stiffness and damping coefficients are set to K n = 0 and D n = 10I, while the gain of the disturbance observer is set to Y = I.

[0237] To verify the generality of the proposed general virtual fixture framework, four typical geometric constraint types are implemented: 1) planar virtual fixture for plane alignment, 2) linear virtual fixture for linear guidance,

[0238] 3) circular virtual fixture for simple curve trajectory tracking, 4) sinusoidal virtual fixture for complex non-linear path tracking. The selection of these typical constraints is intended 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, and the damping coefficient is calculated according to the formula.

[0240] Table 1 Constraint expressions for four types of virtual fixtures

[0241]

[0242] Two types of interactive operation methods are involved in the experimental verification: low-speed dragging (average speed: 0.030 ± 0.015 m / s), which is used to evaluate the basic accuracy of the proposed method; high-speed dragging (average speed 0.100 ± 0.025 m / s), which is used to evaluate its dynamic disturbance rejection ability. All physical interaction experiments are performed by the same operator to eliminate the influence of different operation habits among different users.

[0243] The experimental verification includes two parts to systematically evaluate the proposed virtual fixture framework.

[0244] In the first part, the configuration integrated with the disturbance observer (DO) was subjected to low-speed and high-speed dragging experiments under four constraint types. Each experimental configuration was repeated five times to ensure statistical significance, and the experiment with the largest constraint error in each experimental configuration was selected as the representative experiment. The quantitative results are summarized in Table 2. Note that the error data in the table was only calculated for the error during the period of human-robot interaction (judged by the threshold ||τ ext || > 2 Nm) of the unmanned aerial vehicle interaction, in order to focus on the influence of external disturbances during the dynamic process.

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

[0246]

[0247] Figure 2 Three key pieces of information of the representative experiment are shown: 1) the dragging interaction trajectories under four different virtual constraints and their corresponding constraint geometries; 2) the constraint error curves of the experiment; 3) the interaction force τ ext during the experiment, and this data is directly obtained from RobotState::tau_ext_hat_filtered() of libfranka.

[0248] In the comparative experiment of the second part, the compensation of the disturbance observer was removed, and the obtained constraint error characteristics are as Figure 3 shown. Under all experimental conditions, the statistical distributions of the constraint errors are comprehensively compared in Figure 4 .

[0249] The experimental results have demonstrated three characteristics of the proposed method.

[0250] First, the virtual fixture framework has achieved sub-millimeter accuracy (maximum error < 1 mm) during physical interaction. As Figure 2 shown, the experimental results of interaction with a disturbance observer under four virtual constraint conditions. The first row shows the four virtual fixture constraint shapes (reference) and the actual trajectory of the end-effector; the second row shows the constraint error (the sub-millimeter accuracy range is marked by the gray area); the third row shows the interaction force during the experiment (the time when ||τ ext || > 2 Nm in the low-speed / high-speed experiments is marked in cyan and magenta).

[0251] It can be concluded that under the experimental configuration with a disturbance observer, the actual motion trajectory of the end-effector almost perfectly coincides with the reference constraint shape, and its maximum error is always less than 1 mm. This geometric constraint execution accuracy can meet the requirements of most high-precision physical human-robot interaction scenarios.

[0252] Second, Figure 3 shown, the experimental results of interaction without a disturbance observer under four virtual constraint conditions. Figure 3 The comparison experiment without an observer in [reference] shows that the degree of error accumulation is positively correlated with the constraint complexity: from the maximum constraint error in 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 a residual steady-state error 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 has significantly improved the system performance: achieving sub-millimeter accuracy under low-speed conditions, and still maintaining the maximum error at the sub-millimeter level even when facing a sinusoidal trajectory with a large curvature change during high-speed operation. The steady-state error is effectively suppressed after the external force is removed. Compared with the condition without an observer, the maximum errors of various virtual fixtures are reduced by 52% - 78%. The evolution of the constraint error further verifies the dual functions of the observer: suppressing transient disturbances during the interaction phase and eliminating the steady-state error during the non-interaction phase.

[0254] The distribution of constraint errors under different experimental conditions, as Figure 4 shown, the error bars represent the average error ± standard deviation during the interaction (||τ ext || > 2 Nm). The asterisk (*) represents the maximum error during the interaction. The reference line at 1 mm marks the sub-millimeter accuracy threshold.

[0255] Example 7:

[0256] Another embodiment of the present 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, and the instructions are executed by the at least one processor to enable the at least one processor to execute any of the virtual fixture dynamics control methods with differentiable geometric constraints in the above embodiments.

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

[0258] The processor is responsible for managing the bus and general processing, and can also provide various functions, including timing, peripheral interface, voltage regulation, power management, and other control functions. The memory can be used to store data used by the processor when executing operations.

[0259] Embodiment Eight:

[0260] Another embodiment of the present application relates to a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the above method embodiments are implemented.

[0261] That is, those skilled in the art can understand that all or part of the steps of implementing the above method embodiments can be completed by instructing relevant hardware through a program. The program is stored in a storage medium, including several instructions for causing a device (which may be a single-chip microcomputer, a chip, etc.) or a processor to execute all or part of the steps of the methods described in various embodiments of the present application. The foregoing storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROM), random access memories (RAM), magnetic disks, or optical discs that can store program codes.

[0262] Those of ordinary skill in the art can understand that the above embodiments are specific embodiments for implementing the present application, and in practical applications, various changes can be made in form and details without departing from the spirit and scope of the present application.

Claims

1. A virtual fixture with arbitrary differentiable geometric constraints, characterized in that, including: in the case where the geometric form of the virtual fixture can be given in an analytical form and is at least second-order differentiable, and the geometric form of the constraint depends only on the spatial position: defining the scalar constraint equation and the constraint control quantity of the virtual fixture, constructing the joint Jacobian matrix of multiple constraints, and calculating the reference constraint control input using PD control based on the expected value of the constraint control and the constraint control error value.

2. The virtual fixture with any differentiable geometric constraint according to claim 1, characterized in that, For a general curve or surface, the scalar constraint equation of the virtual fixture is as shown in the following formula: F(p) = 0 Define the constraint control quantity Solve for the first derivative of the constraint control quantity u, define the velocity Jacobian matrix and the constraint Jacobian matrix, take the derivative of the first derivative again to obtain the acceleration-level relationship of the constraint control quantity, and use PD control to calculate the reference constraint control input In the formula, K u , D u respectively represent the proportional differential gain of the virtual fixture constraint error, d represents the expected value, e represents the error, and c represents the control, represents the position of the end effector in the world coordinate system.

3. The virtual fixture with any differentiable geometric constraint according to claim 1, characterized in that, For the case of multiple constraints, the scalar constraint equation of the virtual fixture is as shown in the following formula: where r represents the number of constraints, which is greater than or equal to 1; The constraint control quantity u is defined as follows: The joint Jacobian matrix of multiple constraints is: where n represents the degree of freedom of the robotic arm, represents the joint angle, represents the set of real numbers.

4. A virtual fixture dynamic control method for any differentiable geometric constraint, characterized in that, including: constructing a manipulator joint space dynamics model for using the Jacobian matrix to establish a mapping from joint space variables to the task space, and obtaining the velocity and acceleration in the task space; establishing the relationship between the joint space acceleration control input, the task space acceleration control input, and the null space acceleration control input; using the joint space impedance model to calculate the null space acceleration control input; constructing the task space coordinates, and using the constraint control quantity and the reference constraint control input of the virtual fixture as described in any one of Weights 1-3; deriving the task space torque control command based on the null space acceleration control input and the reference constraint control input.

5. The virtual fixture dynamics control method with any differentiable geometric constraint according to claim 4, characterized in that The manipulator dynamics model includes: For an n-degree-of-freedom manipulator, its joint space dynamics equation is as follows: Among them, represents the joint angle, represents the set of real numbers, indicating that q is an n-dimensional real vector; n represents the degree of freedom of the robotic arm, and M(q) represents the inertia matrix; represents the Coriolis force and centrifugal force; G(q) represents the gravity term; τ represents the control torque, and τ ext represents the external force, represents the joint space velocity, represents the joint space acceleration; Using the Jacobian matrix According to the joint space velocity And the kinematic relationship between The task space joint velocity, the task space acceleration is obtained And the relationship between the joint space velocity Joint space acceleration Is: Based on the general solution of Equation (6), the joint space acceleration control input is obtained as shown below: wherein, is the task space acceleration control input, and is the null space acceleration control input, is the right generalized inverse that is kinematically consistent, N = I - J # J represents the null space projection matrix, and M represents the inertia matrix.

6. The virtual fixture dynamics control method for any differentiable geometric constraint according to claim 5, characterized in that, The calculation of the null space acceleration control input is as shown in the following formula: d represents the expected value, e represents the error, and c represents the control, represents the joint damping, represents the joint stiffness, D n and K n are constant diagonal matrices.

7. The virtual fixture dynamics control method with any differentiable geometric constraint according to claim 6, characterized in that, The derivation of the task space torque control command based on the null space acceleration control input and the reference constraint control input includes: the joint acceleration control input that satisfies the virtual fixture constraint: wherein, represents a null space projection matrix with respect to the constrained control quantity u; calculating the acceleration control input to be designed according to Equations (9) and (16); establishing the inverse dynamics compensation term: calculating the control torque to be designed according to Equations (9), (16), and (2); 8. The virtual fixture dynamics control method with any differentiable geometric constraint according to claim 7, characterized in that It also includes a disturbance observer for calculating the total disturbance torque while considering external disturbances and dynamic uncertainties. projecting the estimated torque onto the task space corresponding to the virtual fixture constraint and then performing compensation: In the formula, represents the perturbation, and the projection matrix is: τ u represents the disturbing torque in the task space.

9. The virtual fixture dynamics control system with arbitrary differentiable geometric constraints is characterized in that including: a manipulator dynamics model, a virtual fixture, and a disturbance observer; a manipulator dynamics model for obtaining the joint space parameters of the manipulator, using the Jacobian matrix to establish a mapping from joint space variables to task space coordinates, mapping the task space parameters to joint space parameters, using null space projection to decouple the joint space control input and the task space control input, and using the joint space impedance model to calculate the null space acceleration control input; a virtual fixture for defining an arbitrary differentiable scalar constraint equation and a constraint control quantity, differentiating the scalar constraint equation to obtain the constraint equations at the velocity level and the acceleration level; using the constraint control quantity to map the joint velocity to the end velocity, defining the constraint Jacobian to represent the mapping from the joint velocity to the constraint change rate, obtaining the constraint control quantity acceleration-level constraint equation, calculating the reference constraint control input using PD control, and constructing the joint space acceleration control input that satisfies the virtual fixture constraint. A disturbance observer is used to establish a true robotic arm joint dynamics model when considering external disturbances and dynamic uncertainties. By combining 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 in the task space.

10. An electronic device, characterized in that, It includes: 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. When the instructions are executed by the at least one processor, the at least one processor is enabled to execute the virtual fixture dynamics control method of any differentiable geometric constraint as described in any one of claims 4 to 7.

Images