A control method and device for a multi-degree-of-freedom training robot

Abstract

The present invention provides a control method and device for a multi-degree-of-freedom training robot. The method includes: performing dynamic modeling on the training robot to obtain a dynamic model; decoupling the dynamic model to obtain a single-variable dynamic model; determining an active disturbance rejection position control strategy and a virtual fixture control strategy according to the single-variable dynamic model; when the interaction information received by the training robot meets the forward conversion criterion, switching from the active disturbance rejection position control strategy to the virtual fixture control strategy; when the interaction information received by the training robot meets the reverse conversion criterion, switching from the virtual fixture control strategy to the active disturbance rejection position control strategy, so as to ensure that the training robot can adaptively switch the control strategy according to the training situation of the user.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and in particular, to a control method and device for a multi-degree-of-freedom training robot. Background Art

[0002] The multi-degree-of-freedom training robot is to help users increase the movement ability of the upper limbs. Due to the shortage of professionals and various cost expenditures, a large number of studies have been conducted on the training control method of the robot.

[0003] For the training control of robots based on multiple degrees of freedom, it mainly focuses on the unidirectional master-slave passive training method, which is controlled by position control strategies such as PID controllers and sliding mode controllers, and is trained through pre-set control strategies and trajectories. There is no interaction of any form between the user and the robotic arm, which will make the user lack active participation during training, resulting in unsatisfactory training effects.

[0004] In different training modes, the control strategies of multi-degree-of-freedom robots are not the same. For different users, the training effects brought by different control strategies are also different. In different training periods of the same user, different control strategies are also required to ensure better training effects. However, the current training robot control method does not have a technical solution for adaptively changing the control strategy according to the situation of human-computer interaction. Summary of the Invention

[0005] The problem solved by the present invention is how to adaptively switch the control strategy of the multi-degree-of-freedom training robot.

[0006] To solve the above problems, the present invention provides a control method and device for a multi-degree-of-freedom training robot.

[0007] In a first aspect, the present invention provides a control method for a multi-degree-of-freedom training robot, including:

[0008] Performing dynamic modeling on the training robot to obtain a dynamic model;

[0009] Decoupling the dynamic model to obtain a single-variable dynamic model;

[0010] Determining an active disturbance rejection position control strategy and a virtual fixture control strategy according to the single-variable dynamic model;

[0011] When the interaction information received by the training robot satisfies the forward conversion criterion, switching from the active disturbance rejection position control strategy to the virtual fixture control strategy;

[0012] When the interaction information received by the training robot satisfies the reverse conversion criterion, switching from the virtual fixture control strategy to the active disturbance rejection position control strategy.

[0013] Optionally, decoupling the dynamic model to obtain a single-variable dynamic model includes:

[0014] Transforming the dynamic model, expressed as:

[0015]

[0016]

[0017] A2 = -M -1 D

[0018] u = M -1 τ,

[0019] where q represents the joint angle of the training robot, represents the differential of q, represents the second differential of q, A1 represents the vector combining the inertia term and the gravity term, A2 represents the difficult-to-measure disturbance quantity, C represents the centripetal force and Coriolis matrix, G represents the gravity matrix, M -1 represents the inverse matrix of the inertia matrix, u represents the virtual torque, f represents the friction force, D represents the unmodeled dynamic model and external disturbances, and τ represents the vector of the output torque.

[0020] Optionally, after transforming the dynamic model, it further includes:

[0021] Decoupling to obtain three independent dynamic models, and converting the joint torque according to the virtual torque in the dynamic model;

[0022] The three independent dynamic models are expressed as:

[0023]

[0024] where a 11 , a 12 , a 13 respectively represent the vectors combining the inertia term and the gravity term of the first joint, the second joint, and the third joint, a 21 , a 22 , a 23 respectively represent the difficult-to-measure disturbance quantities of the first joint, the second joint, and the third joint, and u1, u2, and u3 respectively represent the virtual torques of the first joint, the second joint, and the third joint.

[0025] Optionally, determining the active disturbance rejection position control strategy and the virtual fixture control strategy according to the single-variable dynamic model includes:

[0026] The disturbance quantity in the single-variable dynamic model is converted into an extended state through a third-order extended state observer;

[0027] According to the preset observer parameters, equivalent compensation is performed on the extended state to determine the active disturbance rejection control law, where the active disturbance rejection control law includes the relationship between the joint angle error and its proportional coefficient, the angular velocity error and its differential coefficient, the vector combined with the inertia term and the gravity term, and the extended state;

[0028] Determine the active disturbance rejection position control strategy according to the active disturbance rejection control rate.

[0029] Optionally, the determining the active disturbance rejection position control strategy and the virtual fixture control strategy according to the single-variable dynamic model further includes:

[0030] Determine the desired trajectory;

[0031] Sample the desired trajectory to obtain a sampling data set;

[0032] Determine the virtual energy quantum between any point in space and the sampling points according to the sampling point data set, where the size of the virtual energy quantum is positively correlated with the distance between the end position of the robotic arm and the sampling point;

[0033] Determine the weight of the potential energy of the end position of the robotic arm relative to each sampling point according to the Gaussian kernel function and the virtual energy quantum;

[0034] Normalize the weights to obtain a virtual fixture function, and determine the virtual fixture control strategy through the virtual fixture function.

[0035] Optionally, the virtual fixture function is expressed as:

[0036]

[0037] where Φ(p) represents the virtual fixture function, p represents the end position of the robotic arm, and φ i represents the preset virtual fixture parameter, and represents the potential energy weight parameter of the end position of the robotic arm.

[0038] Optionally, the normalizing the weights to obtain a virtual fixture function and determining the virtual fixture control strategy through the virtual fixture function includes:

[0039] Obtain the virtual fixture parameter through a convex optimization solution algorithm.

[0040] Optionally, the switching from the active disturbance rejection position control strategy to the virtual fixture control strategy when the interaction information received by the training robot satisfies the forward conversion criterion includes:

[0041] Obtain the interaction force, motion direction, and target motion direction received by the robotic arm;

[0042] Determine the effective impulse accumulated in the second time period according to the interaction force and the motion direction, where the effective impulse represents the impulse accumulated by the robotic arm in the target motion direction;

[0043] When the effective impulse is greater than a preset impulse threshold, switch from the active disturbance rejection position control strategy to the virtual fixture control strategy.

[0044] Optionally, the step of switching from the virtual fixture control strategy to the active disturbance rejection position control strategy when the interaction information received by the training robot satisfies the reverse conversion criterion includes:

[0045] Sample the end point position of the robotic arm within the first time period to obtain a point position dataset;

[0046] Obtain the central point position of the point position dataset and the farthest sampling point farthest from the central point position;

[0047] When the distance between the central point position and the farthest sampling point is less than a preset switching threshold, switch from the virtual fixture control strategy to the active disturbance rejection position control strategy.

[0048] Compared with the prior art, the present invention performs multi-degree-of-freedom modeling on the training robot, decouples the model, obtains a single-variable dynamic model, simplifies the form of the model, which is beneficial to designing the control strategy and switching strategy of the model; sets the forward conversion criterion and the reverse conversion criterion, so that the training robot judges the obtained interaction information during the training process, and switches the control strategy when the conversion criterion is satisfied, so as to realize the adaptive switching of the control strategy of the training robot.

[0049] On the other hand, the present invention also provides a multi-degree-of-freedom training robot control device, including:

[0050] A modeling module for performing dynamic modeling on the training robot to obtain a dynamic model;

[0051] A decoupling module for decoupling the dynamic model to obtain a single-variable dynamic model;

[0052] A strategy determination module for determining an active disturbance rejection position control strategy and a virtual fixture control strategy according to the single-variable dynamic model;

[0053] A strategy switching module for determining the control strategy to be executed according to a preset classification training control condition;

[0054] A forward conversion module, which is configured to switch from the active disturbance rejection position control strategy to the virtual fixture control strategy when the interaction information received by the training robot meets the forward conversion criterion;

[0055] A reverse conversion module, which is configured to switch from the virtual fixture control strategy to the active disturbance rejection position control strategy when the interaction information received by the training robot meets the reverse conversion criterion.

[0056] The beneficial effects of the multi-degree-of-freedom training robot control device relative to the prior art are the same as those of the multi-degree-of-freedom training robot control method, and will not be elaborated here. Description of the Drawings

[0057] Figure 1 It is a schematic flowchart of the multi-degree-of-freedom training robot control method according to an embodiment of the present invention;

[0058] Figure 2 It is a schematic flowchart after refining step S300 of the multi-degree-of-freedom training robot control method according to an embodiment of the present invention;

[0059] Figure 3 It is another schematic flowchart after refining step S300 of the multi-degree-of-freedom training robot control method according to an embodiment of the present invention;

[0060] Figure 4 It is a schematic flowchart after refining step S400 of the multi-degree-of-freedom training robot control method according to an embodiment of the present invention;

[0061] Figure 5 It is a schematic flowchart after refining step S500 of the multi-degree-of-freedom training robot control method according to an embodiment of the present invention;

[0062] Figure 6 It is a schematic diagram of the principle of the forward conversion criterion of the multi-degree-of-freedom training robot control method according to an embodiment of the present invention. Detailed Description of the Invention

[0063] To make the above objects, features, and advantages of the present invention more obvious and understandable, the following detailed description of the specific embodiments of the present invention is provided in conjunction with the drawings. Although some embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be construed as limited to the embodiments described herein. On the contrary, these embodiments are provided to more thoroughly and completely understand the present invention. It should be understood that the drawings and embodiments of the present invention are only for exemplary purposes and are not used to limit the protection scope of the present invention.

[0064] It should be understood that the various steps described in the method embodiments of the present invention can be executed in different orders and / or in parallel. In addition, the method embodiments may include additional steps and / or omit the steps shown. The scope of the present invention is not limited in this regard.

[0065] As used herein, the term "comprising" and its variations are open-ended, i.e., "including but not limited to". The term "based on" means "at least partially based on". The term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one additional embodiment"; the term "some embodiments" means "at least some embodiments"; the term "optionally" means "optional embodiment". The relevant definitions of other terms will be given in the following description. It should be noted that the concepts such as "first", "second", etc. mentioned in the present invention are only used to distinguish different devices, modules or units, and are not used to limit the order or interdependence of the functions performed by these devices, modules or units.

[0066] It should be noted that the modifications of "one" and "plural" mentioned in the present invention are illustrative rather than restrictive. Those skilled in the art should understand that unless otherwise clearly stated in the context, it should be understood as "one or more".

[0067] As Figure 1 shown, a multi-degree-of-freedom training robot control method provided by an embodiment of the present invention includes:

[0068] Step S100, performing dynamic modeling on the training robot to obtain a dynamic model.

[0069] A multi-degree-of-freedom training robot refers to a robot composed of a robotic arm with at least two degrees of freedom.

[0070] In one embodiment, the training robot is a three-degree-of-freedom training robot, including a first joint, a second joint, and a third joint. Among them, the first joint is rotationally connected to the second joint through a rotating shaft, and the second joint and the third joint are rotationally connected through a rotating shaft.

[0071] In one embodiment, the training robot includes an active arm and a driven arm. Among them, a motor is provided at the joint of the active arm for outputting torque outward to actively control the rotation, force application, etc. of the robotic arm. No motor is provided at the joint of the driven arm for obtaining information about the force applied by the user to the driven arm.

[0072] In one embodiment, before controlling the training robot, dynamic modeling is performed on the training robot to obtain a dynamic model.

[0073] Specifically, the dynamic equation of the robot in the joint space is expressed as:

[0074]

[0075] where \(q\in R\) n×1 is the joint angle, \(M(q)\in R\) n×n represents the inertia matrix, represents the centripetal force and Coriolis matrix, \(G(q)\in R\) n is the gravity matrix, is the friction matrix, \(\tau\in R\) n represents the actuator output torque vector. represents the differentiation with respect to \(q\), represents the second - order differentiation with respect to \(q\), represents the friction term, \(D\) represents the unknown and external disturbance terms of the dynamic part.

[0076] Optionally, the friction term is determined according to the Stribeck friction model

[0077] Step S200, decouple the dynamic model to obtain a single - variable dynamic model.

[0078] Since the control system of a multi - degree - of - freedom training robot is a multi - input multi - output system, which includes input information such as the angle, interaction force, position of each joint, the processing of multiple information is relatively complex. Therefore, in this solution, in order to facilitate the control of each joint of the upper - limb rehabilitation robot through an observer, the dynamic model is decoupled into multiple single - input single - output systems, and each system is independently calculated and controlled. Among them, the single - input single - output system is the single - variable dynamic model.

[0079] Specifically, rewrite the robot dynamic equation as:

[0080]

[0081] where \(M\) -1 represents the inverse matrix of the inertia matrix, \(C\) represents the centripetal force and Coriolis matrix.

[0082] Simplify the above formula to obtain:

[0083]

[0084]

[0085] \(A2=-M\) -1 \(D\)

[0086] \(u = M\) -1 \(\tau\),

[0087] Among them, A1 represents a vector obtained by combining the inertial term and the gravity term, i.e., a known quantity; A2 represents an uncertain dynamic model and external disturbances, i.e., unknown quantities, and this part is difficult to measure; u represents the virtual torque.

[0088] Optionally, after transforming the dynamic model, it further includes:

[0089] Decouple to obtain three independent dynamic models, and convert the virtual torque in the dynamic model to obtain joint torques;

[0090] The three independent dynamic models are expressed as:

[0091]

[0092] Among them, a 11 , a 12 , a 13 respectively represent the vectors obtained by combining the inertial term and the gravity term of the first joint, the second joint, and the third joint, a 21 , a 22 , a 23 respectively represent the immeasurable disturbance quantities of the first joint, the second joint, and the third joint, and u1, u2, and u3 respectively represent the virtual torques of the first joint, the second joint, and the third joint.

[0093] Specifically, after introducing the virtual torque, the dynamic model can be decoupled into multiple independent single-input single-output systems according to the number of joints. When the virtual torque is known, the virtual torque can be converted to obtain the actual joint torque through τ = Mu.

[0094] Step S300, determine the active disturbance rejection position control strategy and the virtual fixture control strategy according to the single-variable dynamic model.

[0095] Specifically, after converting the multi-input multi-output system into multiple single-input single-output systems, the output of the corresponding control strategy can be determined according to the input of each system. In the technical solution of the present invention, it includes at least two control strategies, namely the active disturbance rejection position control strategy and the virtual fixture control strategy. Among them, the active disturbance rejection position control strategy can accurately track the planned target trajectory, provide a passive training mode for the user, and ensure that the user can independently control the movement trajectory of the training robot; while the virtual fixture is used for the motion planning of assisted training. When the user's strength is not enough to completely control the trajectory of the training robot, the virtual fixture control strategy provides a force towards the desired trajectory to assist the user in training.

[0096] To meet the requirements of training mode switching, a self-switching controller for the training mode is determined based on a single-variable dynamic model. When the current control strategy is the active disturbance rejection position control strategy, the target trajectory of passive training is planned by the target trajectory generator; when the current control strategy is the virtual fixture control strategy, the position of the end of the robotic arm in the Cartesian coordinate system is obtained through forward kinematics, and the increment of the motion position in the next control cycle is solved using the virtual fixture function. Then, the mode switching is performed according to the preset training mode switching parameter α.

[0097] Optionally, the target generator is expressed as:

[0098]

[0099] where x0 represents the target position generated by the target trajectory generator, x a represents the actual position of the end of the robotic arm, and x t represents the target motion trajectory planned in the passive training mode.

[0100] Optionally, as Figure 2 shown, the determining of the active disturbance rejection position control strategy and the virtual fixture control strategy according to the single-variable dynamic model includes:

[0101] Step S310, converting the disturbance quantity in the single-variable dynamic model into an extended state through a third-order extended state observer;

[0102] Step S311, performing equivalent compensation on the extended state according to the preset observer parameters to determine the active disturbance rejection control law, where the active disturbance rejection control law includes the relationship between the joint angle error and its proportional coefficient, the angular velocity error and its differential coefficient, the vector combined with the inertia term and the gravity term, and the extended state;

[0103] Step S312, determining the active disturbance rejection position control strategy according to the active disturbance rejection control rate.

[0104] When performing dynamic modeling on the training robot, due to the strong nonlinearity of the system, the parameters in the model are also difficult to measure, and the uncertainty factors of external disturbances are relatively large. Therefore, there are high requirements for the design of the active disturbance rejection position control strategy. The determined factors are transformed and regarded as extended states, and then equivalent compensation is performed on the extended states in the controller to ensure dynamic compensation when realizing the control of the robotic arm.

[0105] In an embodiment, assume that D(t) is differentiable, that is, it represents that the nonlinear time-varying dynamic term of the lower limb exoskeleton is differentiable. Taking the third joint as an example, let x1 = θ3, and a 21 can be expanded into a new state variable x3. Then, the state space equation in the single-variable dynamic model system is:

[0106]

[0107] Among them, θ3 represents the angle of the third joint, represents the differential of θ3, and w(t) is the third-order uncertainty term in the model.

[0108] The uncertainty term and external disturbance a in the model are estimated by a third-order extended state observer 21 and can be expressed as:

[0109]

[0110] Furthermore, the active disturbance rejection control rate can be expressed as:

[0111]

[0112] Among them, β 01 and β 02 and β 03 are preset observer parameters, e1 and represent the angle error and angular velocity error of the third joint, k p and k d represent the proportional and differential coefficients, y represents the observed rotation angle of the third joint, z1 represents the observed value of the angle of the third joint, z2 represents the observed value of the angular velocity of the third joint, and z3 represents the observed value of the angular acceleration of the third joint.

[0113] Optionally, as Figure 3 shown, determining the active disturbance rejection position control strategy and the virtual fixture control strategy according to the single-variable dynamic model further includes:

[0114] Step S320, determining the desired trajectory;

[0115] Step S321, sampling the desired trajectory to obtain a sampling data set;

[0116] Step S322, determining the virtual energy quantum between any point in space and the sampling point according to the sampling point data set, where the size of the virtual energy quantum is positively correlated with the distance between the end position of the robotic arm and the sampling point;

[0117] Step S323, determining the weight of the potential energy of the end position of the robotic arm relative to each sampling point according to the Gaussian kernel function and the virtual energy quantum;

[0118] Step S324, normalizing the weight to obtain a virtual fixture function, and determining the virtual fixture control strategy through the virtual fixture function.

[0119] In the virtual fixture control strategy, it is necessary to determine the control strategy of the virtual fixture based on the desired trajectory. Specifically, a virtual potential field is determined according to the desired trajectory, so that the virtual potential of the desired trajectory is globally minimized, that is, on the target trajectory, the potential field gradient is 0. The potential energy of each point outside the target trajectory increases with the distance from the target trajectory. When deviating from the target trajectory, a normal auxiliary force pointing to the desired trajectory is provided through the virtual potential field, so that the end of the robotic arm tends to move along the desired trajectory.

[0120] Specifically, after determining the desired trajectory, sample the desired trajectory to obtain N sampling points, which together form a sampling data set, denoted as:

[0121]

[0122] Among them, represents the position information of the i-th sampling point of the end of the robotic arm in the Cartesian space, and D p represents the sampling data set.

[0123] Establish the virtual energy quantum between any point p in space and the sampling point as:

[0124]

[0125] Among them, φ i (p) represents the virtual energy quantum between p and the i-th sampling point, represents the virtual fixture parameter, which is a scalar, and K i represents the elastic coefficient of the virtual spring between p and the i-th sampling point.

[0126] It can be known that the virtual spring force is: The elastic potential energy of point p is The farther the space point p is from the greater the elastic potential energy.

[0127] Calculate the weight of the potential energy of point p with respect to N sampling points through the Gaussian kernel function. The closer to the sampling point, the greater the weight. Normalize the N weights to obtain the total virtual fixture function Φ(p), and its potential field gradient is expressed as:

[0128]

[0129] Among them, represents the potential energy weight value of the i-th sampling point p i for the space point p, and σ i represents the Gaussian parameter in the weight formula,

[0130] Because in the sampling data set D pIn this case, the potential field gradient of the virtual fixture is equal to zero. Therefore, the values of the virtual fixture parameters are solved according to the convex optimization problem.

[0131] The virtual fixture function is expressed as:

[0132]

[0133] Among them, Φ(p) represents the virtual fixture function, p represents the position of the end point of the robotic arm, and φ i represents the preset virtual fixture parameter, represents the preset potential energy weight parameter of the position of the end point of the robotic arm.

[0134] In one embodiment, the preset virtual fixture parameter and the preset potential energy weight parameter are determined according to the actual situation.

[0135] Step S400, when the interaction information received by the training robot satisfies the forward conversion criterion, switch from the active disturbance rejection position control strategy to the virtual fixture control strategy.

[0136] After determining the active disturbance rejection position control strategy and the virtual fixture control strategy, it is determined whether it is necessary to switch the control strategy of the training robot from the active disturbance rejection position control strategy to the virtual fixture control strategy according to the forward conversion criterion. When the interaction force or other interaction parameters of the training robot by the user are not ideal, the system automatically judges that it is necessary to intervene in the force of the user to help the user complete the training to improve the training effect.

[0137] Specifically, in the present invention, it is judged by measuring the impulse accumulated by the user on the robotic arm of the training robot. When the impulse meets the requirements of the forward conversion criterion, a forward switch is performed, that is, switching from the active disturbance rejection position control strategy to the virtual fixture control strategy.

[0138] Optionally, as Figure 4 shown, when the interaction information received by the training robot satisfies the forward conversion criterion, switching from the active disturbance rejection position control strategy to the virtual fixture control strategy includes:

[0139] Step S410, obtain the interaction force, the movement direction and the target movement direction received by the robotic arm;

[0140] Step S411, determine the effective impulse accumulated in the second time period according to the interaction force and the movement direction, where the effective impulse represents the impulse accumulated by the robotic arm in the target movement direction;

[0141] Step S412, when the effective impulse is greater than the preset impulse threshold, switch from the active disturbance rejection position control strategy to the virtual fixture control strategy.

[0142] In one embodiment, the human - machine interaction force is measured according to a force sensor installed at the end of the robotic arm of the training robot. When the training robot is in the active disturbance rejection position control strategy, that is, during the passive training mode, if the user exerts effort in the desired motion direction and lasts for a second preset time period, the effective impulse accumulated during this time period is the index for positive conversion.

[0143] For example, as Figure 6 shown, during the two - dimensional plane motion, the calculation formula of I is expressed as:

[0144]

[0145]

[0146]

[0147] where I0 ∈ (0.2I1, 0.5I1) represents the impulse threshold, F ext represents the human - machine interaction force, v r represents the target motion direction, θ represents the angle between F ext and v r F represents the projection of F ext on v r The effective impulse I refers to the accumulation of F within the second time period t2, and α represents a preset value.

[0148] In the calculation formula of the impulse, an absolute - value treatment is performed on F. When θ is an acute angle, it means that the user has done positive work in the target motion direction, and at this time F is positive; when θ is an obtuse angle, it means that the user has done negative work in the target motion direction, and at this time F is taken as negative.

[0149] When I is greater than the impulse threshold I0, the training mode switches to the assisted - motion training, that is, switches to the virtual fixture control strategy. When I is less than or equal to the impulse threshold I0 and the current control strategy is the active disturbance rejection position control strategy, the control strategy remains unchanged.

[0150] Optionally, the second time period t2 and the impulse threshold are determined by the actual training task.

[0151] Step S500, when the interaction information received by the training robot meets the reverse - conversion criterion, switch from the virtual fixture control strategy to the active disturbance rejection position control strategy.

[0152] When the system determines that the user's ability is relatively strong and the assisted - motion training no longer meets the user's training requirements, after being judged by the reverse - conversion criterion, a reverse switch is made to switch the control strategy to the active disturbance rejection position control strategy, and it is determined whether the reverse - conversion criterion is met according to the length of the actual motion trajectory.

[0153] Specifically, when the current control strategy is the virtual fixture control strategy and the reverse conversion criterion is satisfied, the system automatically performs reverse switching, that is, switches from the virtual fixture control strategy to the active disturbance rejection position control strategy.

[0154] Optionally, as Figure 5 shown, when the interaction information received by the training robot satisfies the reverse conversion criterion, the switching from the virtual fixture control strategy to the active disturbance rejection position control strategy includes:

[0155] Step S510, sampling the end point positions of the robotic arm within a first time period to obtain a point position data set;

[0156] Step S511, obtaining the central point position of the point position data set and the farthest sampling point that is the farthest from the central point position;

[0157] Step S512, when the distance between the central point position and the farthest sampling point is less than a preset switching threshold, switching from the virtual fixture control strategy to the active disturbance rejection position control strategy.

[0158] When the user's strength is insufficient, the main training method is to drive the upper limb to move through the torque actively generated by the robotic arm. When the user's strength is sufficient for active movement, the system will automatically switch the control strategy from the virtual fixture control strategy to the active disturbance rejection position control strategy.

[0159] By sampling the actual movement trajectory of the end of the robotic arm within the first time period t1, N sampling point positions of the end of the robotic arm are obtained, and a point position data set is obtained, expressed as:

[0160]

[0161] where D x represents the point position data set, represents the j-th sampling point position, and R 3 represents the trajectory of the third joint.

[0162] The central point position of the point position data set can be expressed as:

[0163] p0 = mean(D x ),

[0164] where p0 represents the central point position.

[0165] The farthest sampling point of the point position data set can be expressed as:

[0166]

[0167] where p max represents the farthest sampling point.

[0168] The distance l between p0 and p max is expressed as:

[0169] l = norm(p0 - p max ),

[0170] When l is less than the switching threshold l0, the training mode is switched to passive training, that is, the control strategy is converted to an active disturbance rejection position control strategy. When l is greater than or equal to the switching threshold l0 and the current control strategy is a virtual fixture control strategy, the control strategy remains unchanged.

[0171] The training mode switching parameter α is determined by comparing l and l0, and then the mode switching is realized. α is expressed as:

[0172]

[0173] A multi-degree-of-freedom training robot control device provided by another embodiment of the present invention includes:

[0174] A modeling module, which is used to perform dynamic modeling on the training robot to obtain a dynamic model;

[0175] A decoupling module, which is used to decouple the dynamic model to obtain a single-variable dynamic model;

[0176] A strategy determination module, which is used to determine an active disturbance rejection position control strategy and a virtual fixture control strategy according to the single-variable dynamic model;

[0177] A strategy switching module, which is used to determine the control strategy to be executed according to preset classification training control conditions;

[0178] A forward conversion module, which is used to switch from the active disturbance rejection position control strategy to the virtual fixture control strategy when the interaction information received by the training robot satisfies the forward conversion criterion;

[0179] A reverse conversion module, which is used to switch from the virtual fixture control strategy to the active disturbance rejection position control strategy when the interaction information received by the training robot satisfies the reverse conversion criterion.

[0180] The beneficial effects of the multi-degree-of-freedom training robot control device relative to the prior art are the same as those of the multi-degree-of-freedom training robot control method, and will not be elaborated here.

[0181] An electronic device provided by another embodiment of the present invention includes a memory and a processor; the memory is used to store a computer program; the processor is used to implement the multi-degree-of-freedom training robot control method as described above when executing the computer program.

[0182] A computer-readable storage medium provided by another embodiment of the present invention stores a computer program, which, when executed by a processor, implements the multi-degree-of-freedom training robot control method described above.

[0183] An electronic device that can be a server or a client of the present invention will now be described, which is an example of a hardware device applicable to various aspects of the present invention. The electronic device is intended to represent various forms of digital electronic computer devices, such as, a laptop computer, a desktop computer, a workbench, a personal digital assistant, a server, a blade server, a mainframe computer, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as, a personal digital processor, a cellular phone, a smart phone, a wearable device, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present invention described and / or claimed herein.

[0184] The electronic device includes a computing unit, which can perform various appropriate actions and processes according to the computer program stored in the read-only memory (ROM) or the computer program loaded from the storage unit into the random access memory (RAM). In the RAM, various programs and data required for device operation can also be stored. The computing unit, the ROM, and the RAM are connected to each other through a bus. The input / output (I / O) interface is also connected to the bus.

[0185] The computer system can include a client and a server. The client and the server are generally far from each other and usually interact through a communication network. The client-server relationship is generated by running computer programs on the respective computers and having a client-server relationship with each other.

[0186] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above methods. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM), etc. In this application, the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they can be located in one place, or they can be distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiments of the present invention. In addition, the functional units in each embodiment of the present invention can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above integrated units can be implemented in the form of hardware or in the form of software functional units.

[0187] Although the present disclosure is disclosed as above, the protection scope of the present disclosure is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present disclosure, and these changes and modifications will all fall within the protection scope of the present invention.

Claims

1. A control method for a multi-degree-of-freedom training robot, characterized in that, Including: Performing dynamic modeling on a training robot to obtain a dynamic model; Decoupling the dynamic model to obtain a single-variable dynamic model, including: transforming the dynamic model and representing it as: ， ， where q represents the joint angle of the training robot, represents the differential of q, represents the second differential of q, represents the vector combining the inertia term and the gravity term, represents the disturbance quantity, represents the centripetal force and the Coriolis matrix, represents the gravity matrix, represents the inverse matrix of the inertia matrix, represents the virtual torque, represents the frictional force, represents the unmodeled dynamic model and the external disturbance, represents the vector of the output torque; Decoupling to obtain three independent dynamic models, and obtaining joint torques according to the conversion of virtual torques in the dynamic model; the three independent dynamic models are represented as: ， Among them, respectively represent vectors combining the inertia terms and gravity terms of the first joint, the second joint, and the third joint, respectively represent the disturbance amounts of the first joint, the second joint, and the third joint, respectively represent the virtual torques of the first joint, the second joint, and the third joint; Determining an active disturbance rejection position control strategy and a virtual fixture control strategy according to the single-variable dynamic model; When the interaction information received by the training robot satisfies the forward conversion criterion, switching from the active disturbance rejection position control strategy to the virtual fixture control strategy; When the interaction information received by the training robot satisfies the reverse conversion criterion, switching from the virtual fixture control strategy to the active disturbance rejection position control strategy.

2. The multi-degree-of-freedom training robot control method according to claim 1, wherein The determining the active disturbance rejection position control strategy and the virtual fixture control strategy according to the single-variable dynamic model includes: Converting the disturbance quantity in the single-variable dynamic model into an extended state through a third-order extended state observer; Performing equivalent compensation on the extended state according to preset observer parameters to determine an active disturbance rejection control law, where the active disturbance rejection control law includes the relationship between the joint angle error and its proportional coefficient, the angular velocity error and its differential coefficient, the vector combined by the inertia term and the gravity term, and the extended state; Determining the active disturbance rejection position control strategy according to the active disturbance rejection control law.

3. The multi-degree-of-freedom training robot control method according to claim 1, characterized in that The determining the active disturbance rejection position control strategy and the virtual fixture control strategy according to the single-variable dynamic model further includes: Determining a desired trajectory; Sampling the desired trajectory to obtain a sampling data set; Determining virtual energy quanta between any point in space and the sampling points according to the sampling data set, where the magnitude of the virtual energy quanta is positively correlated with the distance between the end position of the robotic arm and the sampling points; Determining the weights of the potential energy of the end position of the robotic arm relative to each sampling point according to the Gaussian kernel function and the virtual energy quanta; Performing normalization processing on the weights to obtain a virtual fixture function, and determining the virtual fixture control strategy through the virtual fixture function.

4. The multi-degree-of-freedom training robot control method according to claim 3, wherein, The virtual fixture function is represented as: ， Among them, represents the virtual fixture function, p represents the position of the end point of the robotic arm, represents the preset virtual fixture parameters, represents the potential energy weight parameter of the position of the end point of the robotic arm.

5. The multi-degree-of-freedom training robot control method according to claim 3, characterized in that The performing normalization processing on the weights to obtain a virtual fixture function, and determining the virtual fixture control strategy through the virtual fixture function includes: Obtaining the virtual fixture parameters through a convex optimization solution algorithm.

6. The multi-degree-of-freedom training robot control method according to claim 5, wherein The when the interaction information received by the training robot satisfies the forward conversion criterion, switching from the active disturbance rejection position control strategy to the virtual fixture control strategy includes: Obtaining the interaction force, the motion direction, and the target motion direction received by the robotic arm; Determining the effective impulse accumulated in a second time period according to the interaction force and the motion direction, where the effective impulse represents the impulse accumulated by the robotic arm in the target motion direction; When the effective impulse is greater than a preset impulse threshold, switching from the active disturbance rejection position control strategy to the virtual fixture control strategy.

7. The multi-degree-of-freedom training robot control method according to claim 6, wherein The when the interaction information received by the training robot satisfies the reverse conversion criterion, switching from the virtual fixture control strategy to the active disturbance rejection position control strategy includes: Sample the end point position of the robotic arm within the first time period to obtain a point position data set; Obtain the central point position of the point position data set and the farthest sampling point that is the farthest from the central point position; When the distance between the central point position and the farthest sampling point is less than a preset switching threshold, switch from the virtual fixture control strategy to the active disturbance rejection position control strategy.

8. A control device for a multi-degree-of-freedom training robot, characterized in that, Include: A modeling module for performing dynamic modeling on the training robot to obtain a dynamic model; A decoupling module for decoupling the dynamic model to obtain a single-variable dynamic model, including: transforming the dynamic model, expressed as: ， ， where q represents the joint angle of the training robot, represents the differential of q, represents the second differential of q, represents the vector combining the inertia term and the gravity term, represents the disturbance quantity, represents the centripetal force and the Coriolis matrix, represents the gravity matrix, represents the inverse matrix of the inertia matrix, represents the virtual torque, represents the frictional force, represents the unmodeled dynamic model and the external disturbance, represents the vector of the output torque; Decouple to obtain three independent dynamic models, and obtain joint torques according to the virtual torques in the dynamic models; the three independent dynamic models are expressed as: ， Among them, respectively represent vectors combining the inertia terms and gravity terms of the first joint, the second joint, and the third joint, respectively represent the disturbance quantities of the first joint, the second joint, and the third joint, respectively represent the virtual torques of the first joint, the second joint, and the third joint; A strategy determination module for determining the active disturbance rejection position control strategy and the virtual fixture control strategy according to the single-variable dynamic model; A strategy switching module for determining the control strategy to be executed according to preset classification training control conditions; A forward conversion module for switching from the active disturbance rejection position control strategy to the virtual fixture control strategy when the interaction information received by the training robot satisfies the forward conversion criterion; A reverse conversion module for switching from the virtual fixture control strategy to the active disturbance rejection position control strategy when the interaction information received by the training robot satisfies the reverse conversion criterion.

Images