A method and system for controlling a robot arm based on an adaptive force field

Abstract

The application provides a mechanical arm control method and system based on an adaptive force field, wherein the motion state of a patient is monitored, the motion state is compared with a demonstration space trajectory, and training performance information of the patient is obtained. Based on the training performance information and an adaptive force field model constructed, an auxiliary force, including a normal force, a tangential force and a viscous force, applied to the patient is calculated. Based on the end posture of the mechanical arm and a target posture, an auxiliary torque applied is calculated. According to the auxiliary force and the auxiliary torque, a joint control torque is obtained, and a dynamic torque of a robot dynamics model and a null space control torque are obtained. In combination with the joint control torque, the dynamic torque and the null space control torque, a comprehensive control torque for controlling the mechanical arm is obtained. In this way, adaptive adjustment of the auxiliary force is realized based on the position, speed and direction of the patient, time freedom is obtained in the motion of the adaptive force field, and the patient can obtain more active motion freedom under the assistance of the space force field.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, and in particular, to a mechanical arm control method and system based on adaptive force field. BACKGROUND

[0002] The existing robot-assisted rehabilitation training usually adopts impedance control or admittance control to realize human-robot interaction, and generally takes the reference trajectory as the center and the position deviation as the main performance index to adjust the assistance force. Typical methods include: progressive impedance assistance based on motion position, time, and electromyographic threshold; force field control that decomposes the assistance force into tangential and normal forces; workspace constraint that constructs a channel around the ideal trajectory and sets a variable speed threshold; and strategies based on force / torque field quantitative deviation, minimum intervention admittance, and fuzzy-impedance combination.

[0003] Overall, the existing technology takes improving trajectory tracking accuracy as the main goal, and the assistance force provided aims to enable the patient to complete the training task more accurately. Such control strategies often lack individual adaptability and have the problem of assistance force redundancy, and the patient's active participation in autonomous exploration of the space is limited. SUMMARY

[0004] The purpose of the embodiments of the present application is to provide a mechanical arm control method and system based on adaptive force field to provide appropriate assistance force according to the training performance of the patient.

[0005] In a first aspect, the present application provides a mechanical arm control method based on adaptive force field, the method comprising:

[0006] monitoring the motion state of the patient, the motion state comprising motion position, motion speed, and motion direction;

[0007] comparing the motion position, motion speed, and motion direction of the patient with the position information, speed information, and direction information of the demonstration space trajectory respectively to obtain training performance information of the patient;

[0008] calculating the assistance force applied to the patient based on the training performance information and the constructed adaptive force field control model, the assistance force comprising normal force, tangential force, and viscous force of the space force field;

[0009] calculating the assistance torque applied to the patient based on the end pose of the mechanical arm and the target pose;

[0010] obtaining joint control torque according to the assistance force and the assistance torque, and obtaining the dynamics torque and null space control torque of the robot dynamics model;

[0011] The comprehensive control torque for controlling the robot arm is obtained by combining the joint control torque, the dynamic torque and the null space control torque.

[0012] In an optional embodiment, the training performance information includes a position deviation and a pointing angle;

[0013] The normal force of the spatial force field is calculated by:

[0014] The normal force coefficient at the current moment is calculated based on the normal force coefficient at the previous moment, the position deviation and an adaptive gain factor, wherein the adaptive gain factor is calculated based on the position deviation and the pointing angle;

[0015] The normal force of the spatial force field is calculated based on the normal force coefficient at the current moment, the normal force direction and the position deviation.

[0016] In an optional embodiment, the adaptive gain factor is calculated by:

[0017] Based on the relationship between the pointing angle and the set minimum pointing angle and maximum pointing angle, it is determined whether the patient is moving normally;

[0018] Based on the determination result, the amplification coefficient is determined in a corresponding determined manner;

[0019] The position deviation is amplified according to the amplification coefficient, and the adaptive gain factor is calculated based on the fixed parameters of the adaptive model.

[0020] In an optional embodiment, the training performance information includes a position deviation and a tangential motion speed;

[0021] The tangential force of the spatial force field is calculated by:

[0022] The speed difference between the demonstration spatial trajectory speed and the tangential motion speed is obtained, and it is determined whether the patient is in an idle state or an overspeed state based on the tangential motion speed;

[0023] In the case of being in the idle state or the overspeed state, the tangential force scaling coefficient is calculated based on the relationship between the speed difference and the set threshold value, and according to the tangential force scaling coefficient and the set threshold value;

[0024] The tangential force of the spatial force field is calculated based on the tangential force coefficient, the tangential force direction, the tangential force action range coefficient, the position deviation and the tangential force scaling coefficient.

[0025] In an optional embodiment, the training performance information includes a motion trend;

[0026] The viscous force of the spatial force field is calculated by:

[0027] a viscous force coefficient is calculated based on the motion trend according to a corresponding calculation manner;

[0028] a viscous force of the spatial force field is calculated based on the viscous force coefficient, the motion speed of the patient, and speed information of the demonstration spatial trajectory.

[0029] In an optional implementation, the motion trend includes a forward motion trend, no obvious motion trend, and a reverse motion trend.

[0030] The step of calculating the viscous force coefficient based on the motion trend according to the corresponding calculation manner includes:

[0031] In a case where the motion trend is the forward motion trend or the no obvious motion trend, the viscous force coefficient is determined as a set lower limit value of the viscous force coefficient.

[0032] In a case where the motion trend is the reverse motion trend, a reverse speed difference value is obtained, in a case where the reverse speed difference value is less than or equal to a preset difference value, the viscous force coefficient is calculated based on a set lower limit value of the viscous force coefficient, an upper limit value of the viscous force coefficient, the reverse speed difference value, and the preset difference value, and in a case where the reverse speed difference value is greater than the preset difference value, the viscous force coefficient is determined as the set upper limit value of the viscous force coefficient.

[0033] In an optional implementation, the reverse speed difference value is generated by using a pseudo speed generation algorithm, and the step includes:

[0034] an initial value of the pseudo speed is set, and iteration is performed according to an iteration step length on the basis of the initial value, and in each iteration round, a difference value between a pseudo speed of a current round and a real tangential motion speed is calculated.

[0035] If the difference value is greater than a set difference value, the pseudo speed of the current round is output, and iteration of a next round is continued to be performed.

[0036] If the difference value is less than or equal to the set difference value, the real tangential motion speed is output, and the iteration is ended.

[0037] In an optional implementation, the step of calculating the auxiliary torque applied to the patient based on the end posture of the mechanical arm and the target posture includes:

[0038] an end posture and an angular speed of the mechanical arm are obtained, and a target posture is obtained.

[0039] an auxiliary torque applied to the patient is calculated based on the end posture, the angular speed, the target posture, virtual stiffness, and virtual damping.

[0040] In an optional embodiment, the step of obtaining the joint control torque based on the auxiliary force and the auxiliary torque comprises:

[0041] decoupling the mechanical arm Jacobian matrix to obtain a position Jacobian matrix and a posture Jacobian matrix;

[0042] obtaining a first joint torque based on the position Jacobian matrix and the auxiliary force;

[0043] obtaining a second joint torque based on the posture Jacobian matrix and the auxiliary torque;

[0044] combining the first joint torque and the second joint torque to obtain the joint control torque.

[0045] In a second aspect, the present application provides a mechanical arm control system based on an adaptive force field, the system comprising:

[0046] a monitoring module for monitoring and obtaining a motion state of a patient, the motion state comprising a motion position, a motion speed and a motion direction;

[0047] a comparison module for comparing the motion position, the motion speed and the motion direction of the patient with position information, speed information and direction information of a demonstration space trajectory respectively to obtain training performance information of the patient;

[0048] a first calculation module for calculating an auxiliary force applied to the patient based on the training performance information and a constructed adaptive force field control model, the auxiliary force comprising a normal force, a tangential force and a viscous force of a space force field;

[0049] a second calculation module for calculating an auxiliary torque applied to the patient based on an end posture of the mechanical arm and a target posture;

[0050] an obtaining module for obtaining a joint control torque based on the auxiliary force and the auxiliary torque, and obtaining a dynamics torque of a robot dynamics model and a null space control torque;

[0051] a calculation control module for combining the joint control torque, the dynamics torque and the null space control torque to obtain a comprehensive control torque for controlling the mechanical arm.

[0052] The application provides a mechanical arm control method and system based on an adaptive force field, which compares the motion state of a patient with the position, speed and direction of a demonstration space trajectory by monitoring the motion state of the patient to obtain training performance information of the patient. The auxiliary force applied to the patient is calculated based on the training performance information and an adaptive force field model, including normal force, tangential force and viscous force. The auxiliary torque applied to the patient is calculated based on the end posture of the mechanical arm and the target posture, the joint control torque is obtained according to the auxiliary force and the auxiliary torque, and the dynamic torque of a robot dynamics model and the null space control torque are obtained. The comprehensive control torque for controlling the mechanical arm is obtained by combining the joint control torque, the dynamic torque and the null space control torque. According to the scheme, the adaptive adjustment of the auxiliary force is realized based on the position, speed and direction of the patient, the time degree of freedom is obtained in the motion of the adaptive force field, and the patient can obtain more active motion freedom under the assistance of the space force field. BRIEF DESCRIPTION OF DRAWINGS

[0053] In order to more clearly illustrate the technical solutions of the embodiments of the application, the following will briefly introduce the drawings needed to be used in the embodiments of the application. It should be understood that the following drawings only show some of the embodiments of the application, and therefore should not be regarded as a limitation to the scope. For those skilled in the art, other related drawings can also be obtained without creative labor on the basis of these drawings.

[0054] Figure 1 The flow chart of the mechanical arm control method based on the adaptive force field provided in the embodiments of the application is shown in the figure.

[0055] Figure 2 The schematic diagram of the space force field in the embodiments of the application is shown in the figure.

[0056] Figure 3 The change curve diagram of the adaptive gain factor in the embodiments of the application is shown in the figure.

[0057] Figure 4 The schematic diagram of the tangential force under different adaptive gain factors in the embodiments of the application is shown in the figure.

[0058] Figure 5 The pseudo code for adjusting the reverse motion speed in the embodiments of the application is shown in the figure.

[0059] Figure 6 The rehabilitation robot system block diagram based on the space force field control strategy in the embodiments of the application is shown in the figure.

[0060] Figure 7 The function module block diagram of the mechanical arm control system based on the adaptive force field provided in the embodiments of the application is shown in the figure.

[0061] Figure 8A structural block diagram of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0062] The technical solutions in the embodiments of the present application will be described below with reference to the drawings in the embodiments of the present application.

[0063] Please refer to Figure 1 A flowchart of the adaptive force field based robot arm control method provided in the embodiment of the present application is shown in FIG. 6, and the detailed steps of the adaptive force field based robot arm control method are described as follows.

[0064] S11, monitoring to obtain the motion state of the patient, the motion state including the motion position, the motion speed and the motion direction;

[0065] S12, comparing the motion position, the motion speed and the motion direction of the patient with the position information, the speed information and the direction information of the demonstration space trajectory respectively to obtain the training performance information of the patient;

[0066] S13, calculating the auxiliary force applied to the patient based on the training performance information and the constructed adaptive force field control model, the auxiliary force including the normal force, the tangential force and the viscous force of the space force field;

[0067] S14, calculating the auxiliary torque applied to the patient based on the end pose of the robot arm and the target pose;

[0068] S15, obtaining the joint control torque according to the auxiliary force and the auxiliary torque, and obtaining the dynamics torque and the null space control torque of the robot dynamics model;

[0069] S16, obtaining the comprehensive control torque for controlling the robot arm by combining the joint control torque, the dynamics torque and the null space control torque.

[0070] In the embodiment, the implementation of the adaptive assistance strategy needs to solve the key problems of when the patient needs assistance and how much assistance is needed, that is, the training performance of the patient needs to be evaluated first, and then the controller is designed based on this.

[0071] Studies have shown that the adaptive control rate can be constructed according to the difference between the actual trajectory and the expected trajectory, therefore, the adaptive rate in the embodiment quantifies the difference between the position, speed, etc. of the actual training trajectory and the demonstration ADL trajectory, comprehensively measures the training performance of the patient, and adjusts the assistance characteristics of the robot according to the training performance to reduce the redundancy degree of the assistance force. The specific design requirements are as follows:

[0072] (1) When the patient’s hand is close to the demonstration ADL trajectory, the force field provides minimal normal assistance; when the patient’s hand is gradually moved away from the demonstration ADL trajectory and the positional deviation is still small, the normal assistance should be increased slowly and kept at a small value; when the positional deviation is greater than the set threshold, the normal assistance force should be increased rapidly; when the patient’s hand has a strong tendency to move eccentrically, the normal assistance force should be further increased to hinder the movement tendency.

[0073] (2) When the tangential motion speed of the patient's hand is greater than the speed of the demonstrated ADL trajectory, the force field should reduce the tangential assistance; when the tangential motion speed of the patient's hand is less than the speed of the demonstrated ADL trajectory, the force field should increase the tangential assistance; when the patient's hand is in reverse motion, the force field should increase the robot's tangential assistance to hinder its reverse motion.

[0074] (3) When the subject is in forward motion, the viscous force characteristics remain unchanged; when the subject is in reverse motion, it should be subject to a greater viscous force to hinder its reverse motion.

[0075] In this embodiment, the patient's movement position deviation, movement speed, and aiming angle are used as motion evaluation indicators, specifically defined as follows:

[0076] (1) Deviation in motion position ( The minimum distance between the patient's spatial location and the demonstration trajectory is defined by equation (1):

[0077]

[0078] (2) Tangential velocity The patient's tangential velocity refers to the projection of the patient's actual velocity onto the velocity direction of the demonstration trajectory. Real-time monitoring of the patient's tangential velocity can effectively identify abnormal movement states, such as excessively fast movement, excessively slow movement, stagnation, and reverse movement. When the patient is in any of these movement states, the system considers the patient's training performance to be poor. This invention categorizes these situations into idling states. and overspeed state Patient-induced motion arrest and reverse motion are considered as idling. Tangential motion speed. It can be calculated using equation (2).

[0079]

[0080] in, This indicates the patient's actual speed of movement. This indicates the direction of the velocity of the demonstration space trajectory.

[0081] (3) Aiming angle The patient's aiming angle refers to the angle between the patient's current direction of movement and the desired direction of movement, which can be used to quantify the degree of centripetal and eccentric movement. This invention... Based on the patient's current direction of movement and normal direction of motion Calculation of the included angle:

[0082]

[0083] in, Satisfy interval ,when Satisfy interval At that time, the patient performed concentric movements. Satisfy interval At that time, the patient performed eccentric exercises.

[0084] This invention employs a weighted mean filter to preprocess motion evaluation metrics, with a window time of 0.2 seconds, representing 40 data points backward along the time axis (including the current frame). The number of current data samples is defined as... Weight vector Calculated using the following formula:

[0085]

[0086] Additionally, it should be noted that in this embodiment, the patient's movement trend... This means that if a patient maintains the same movement state for a period of time during rehabilitation training, the system assumes that the patient is likely to maintain that movement state for a future period of time. For example, if the patient... If the internal organs are in a state of positive motion, then the patient is judged to have a positive motion trend. If the patient is If the internal organs are in a state of reverse movement, then the patient is judged to have a tendency for reverse movement. If the patient is Alternating anteroposterior and retrograde movements indicate that the patient has no obvious movement tendency. .in It is about determining time, in this invention It can be 0.5s. The reason for defining the motion trend is to prevent the force field from misjudging the reverse movement of the patient's hand. Only when the patient's hand has a reverse movement trend is it considered to be in reverse movement.

[0087] Based on the above, the specific implementation methods of each of the above steps will be explained in detail below.

[0088] In this embodiment, the movement position, movement speed and movement direction of the patient are compared with the position information, speed information and direction information of the demonstration space trajectory respectively by the above method, and the training performance information of the patient is obtained. The training performance information includes the movement position deviation, tangential movement speed, aiming angle, etc. as described above.

[0089] Based on the training performance information and the constructed adaptive force field control model, the auxiliary force required to be applied to the patient is obtained, including the normal force, tangential force and viscous force of the space force field. In this embodiment, the adaptive force field control model can be described by the following formula:

[0090]

[0091] In the formula, are the normal force, tangential force and viscous force of the space force field respectively, is the auxiliary force of the force field applied to the patient's hand. The schematic diagram of the space force field plane around the ADL training trajectory is shown in Figure 2 .

[0092] The specific determination method of the normal force, tangential force and viscous force of the space force field is described below.

[0093] The normal force of the space force field can be calculated by the following method:

[0094] Based on the normal force coefficient, position deviation and adaptive gain factor of the previous moment, the normal force coefficient of the current moment is calculated, wherein the adaptive gain factor is calculated based on the position deviation and the aiming angle; the normal force of the space force field is calculated according to the normal force coefficient of the current moment, the direction of the normal force and the position deviation.

[0095] The adaptive gain factor can be calculated by the following method:

[0096] Based on the relationship between the aiming angle and the set minimum aiming angle and maximum aiming angle, it is judged whether the patient moves normally; based on the determination result, the amplification coefficient is determined in the corresponding determination method; the position deviation is amplified according to the amplification coefficient, and the adaptive gain factor is calculated in combination with the fixed parameters of the adaptive model.

[0097] In this embodiment, the adaptive normal auxiliary force field control model is shown in formula (6):

[0098]

[0099] In formula (6), the is the direction of the normal force, which can be calculated according to formula (10), and in the formula, is the reference position, is the actual position, is the point the shortest distance between the point and the point is the normal force coefficient, which is defined by equation (7) as follows, where is a forgetting factor, is an adaptive gain factor, is the normal force coefficient at the last time step, is the component of the patient hand position deviation in the direction .

[0100] The adaptive gain factor is defined by equation (8), which is an adaptive model taking the aiming angle and the position deviation as input and outputting , where is a fixed parameter of the adaptive model, which is set to by default, and equation (9) gives the rule of scaling the position deviation by , where is the upper limit of the scaling (i.e., the maximum aiming angle), which is set to by default, is the lower limit of the angle of action (i.e., the minimum aiming angle), which is set to by default.

[0101] The relationship between the aiming angle and the maximum and minimum aiming angles is used to determine whether the patient is moving normally. For example, when the aiming angle satisfies , the system considers that the patient is moving normally, and when the aiming angle satisfies , the system considers that the patient has an excessive centrifugal movement tendency and determines that the patient will deviate significantly from the demonstration ADL trajectory. The difference between the aiming angle and the maximum aiming angle is calculated as , and is amplified to change .

[0102] For example, when the fixed parameter of the adaptive model is set to the default parameter and , the change characteristics of are as shown in Figure 3 . Further, when , , the change characteristics of the adaptive normal force field are as shown by the green curve in Figure 4 .

[0103] It can be seen that when the position deviation is small, the normal force of the adaptive force field is similar to that of the conventional force field of , and when the position deviation reaches the set threshold (the threshold is​ ), the normal force increases rapidly, and with the increase of the position deviation, its change characteristic is more and more close to the normal force change characteristic of the conventional force field When the patient performs well, the normal force field applies less intervention, and the patient has more freedom of movement. Under the same control parameters, when the patient performs poorly, the normal force field will give the patient a faster and stronger intervention to prevent the patient from centrifugal movement. If the patient's centrifugal movement is very strong, the intervention degree of the normal force field will be increased by amplifying the current position deviation.

[0104] In addition, the tangential force of the space force field is calculated by the following formula:

[0105] In this embodiment, the adaptive tangential auxiliary force field control model is shown in formula (11), which can dynamically adjust the size of the tangential force according to the movement speed and position of the patient.

[0106]

[0107] In the formula, F is the tangential force, is the tangential force coefficient, is the direction of the tangential force, is the tangential force action range coefficient, wherein is the tangential force scaling coefficient, which adjusts the size of the tangential force in real time according to the movement state of the patient.

[0108] In this embodiment, the speed difference between the demonstration space trajectory speed and the tangential movement speed is obtained, and whether the patient is in an idle state or an overspeed state is determined based on the tangential movement speed (the determination method can be referred to above). In the case of being in an idle state or an overspeed state, the tangential force scaling coefficient is calculated based on the relationship between the speed difference and the set threshold value, and according to the tangential force scaling coefficient and the set threshold value.

[0109] Specifically, when in the idle state (i.e. ), the tangential force scaling coefficient is calculated according to the following formula:

[0110]

[0111] When in the overspeed state (i.e. ), the tangential force scaling coefficient is calculated according to the following formula:

[0112]

[0113] wherein, is the tangential force scaling coefficient, the three coefficients decrease in turn, is the difference between the demonstration ADL trajectory speed and the tangential movement speed of the patient, which satisfies:

[0114]

[0115] in, It is the velocity vector of the taught ADL trajectory, and It is a threshold used to measure The size, in this invention, is the default. .

[0116] Given the tangential force scaling factor, the tangential force of the spatial force field is calculated according to the tangential force coefficient, tangential force direction, tangential force range coefficient, position deviation and tangential force scaling factor, and formula (11).

[0117] During the patient's exercise, It is continuously changing; when the patient is in an idling state and meets the following conditions... At that time, with From 0 to , Will from linearly increase to ,if Continue to increase, to satisfy , Will remain At this time, the tangential force field will increase the assist force on the patient. When the patient is in an overspeed state and meets the following conditions... hour, Will follow The increase, from linear change to ,when Continue to increase beyond back, It will remain at the minimum value If the force remains unchanged, the tangential force field will reduce the auxiliary force on the patient.

[0118] In this embodiment, the viscous force of the spatial force field can be calculated in the following way:

[0119] The viscous force coefficient is calculated based on the motion trend and the corresponding calculation method; the viscous force of the spatial force field is calculated based on the viscous force coefficient, the patient's motion speed and the velocity information of the demonstration spatial trajectory.

[0120] The motion trend includes positive motion trend, no obvious motion trend, and reverse motion trend. The step of calculating the viscous force coefficient based on the motion trend according to the corresponding calculation method can be achieved in the following way:

[0121] When the motion trend is positive or there is no obvious motion trend, the viscosity coefficient is determined to be the set lower limit value of the viscosity coefficient;

[0122] In the case of a reverse motion trend, a reverse speed difference value is obtained, and in the case of the reverse speed difference value being less than or equal to a preset difference value, a viscous force coefficient is calculated based on a set lower limit value of the viscous force coefficient, an upper limit value of the viscous force coefficient, the reverse speed difference value and the preset difference value, and in the case of the reverse speed difference value being greater than the preset difference value, the viscous force coefficient is determined as the set upper limit value of the viscous force coefficient.

[0123] In this embodiment, the adaptive viscous force field control model is shown in formula (15). The viscous force field can adjust the viscous force according to the difference between the motion speed of the patient and the speed of the demonstration ADL trajectory, and respond to the reverse motion of the patient, giving the patient greater resistance to the reverse motion, and strengthening the guidance of the task endpoint.

[0124]

[0125] wherein, is the speed vector of the demonstration ADL trajectory, is the actual motion speed vector, is the viscous force coefficient, The change rule of is shown in formula (16) and formula (17).

[0126] When (i.e. positive motion trend and no obvious motion trend):

[0127]

[0128] When (reverse motion trend):

[0129]

[0130] wherein, is the lower limit value of the viscous force coefficient, is the upper limit value of the viscous force coefficient, is the reverse speed difference value, will be adjusted in real time with the change of . When , will remain unchanged at ; when and satisfies , increases linearly with the increase of ; when continues to increase and satisfies , reaches the maximum value and remains unchanged. By adjusting the size of , the size of The default of the application is that the change is slow If the increase is With the increase of , it increases slowly, and vice versa, With the increase of , it increases quickly.

[0131] In order to make the viscous force coefficient change continuously, the reverse speed difference should change from 0, but when the patient's hand movement changes from a positive movement trend to a reverse movement trend, there is a state of no obvious movement trend for 0.5s, and is still , but real-time movement may have reverse movement, and the reverse movement speed continues to change, when the system judges , does not change from 0, which will cause a sudden change in . In order to solve this problem, the application proposes to make a transition by generating a pseudo speed, which sacrifices some real values to exchange for the smooth continuity of its change. That is, the reverse speed difference described above is generated by a pseudo speed generation algorithm, which can be implemented in the following way:

[0132] Set the initial value of the pseudo speed, and iterate according to the iteration step length based on the initial value. In each iteration, calculate the difference between the pseudo speed of the current round and the real tangential movement speed; if the difference is greater than the set difference, output the pseudo speed of the current round, and continue to execute the next round of iteration; if the difference is less than or equal to the set difference, output the real tangential movement speed, and end the iteration.

[0133] In this embodiment, in the pseudo speed generation algorithm, the reverse speed difference satisfies the following formula:

[0134]

[0135] Wherein, is the pseudo speed generation algorithm, is the pseudo speed, is the actual movement speed, and the pseudo code for generating is shown in Figure 5 .

[0136] Wherein, is the iteration step length, and the threshold is the iteration termination condition, when and the difference between the actual speed satisfies condition, the approximation is successful, and the iteration is ended. Before the iteration is ended, the speed difference ​When approaching success, In the present application A pseudo-velocity straight line with an acceleration of Can be generated to increase To reduce the approaching time, so that The true value is reached as soon as possible, but if It is too large, it will make the slope of the pseudo-velocity straight line too large, affecting The smoothness of the transition.

[0137] The adaptive spatial force field constructed only constrains the position of the patient during rehabilitation training, and does not control the end posture of the mechanical arm. The present application realizes the constraint of the interactive posture through an auxiliary torque realized by a rotation impedance controller.

[0138] In the present embodiment, the step of calculating the auxiliary torque applied to the patient based on the end posture of the mechanical arm and the target posture can be realized in the following manner:

[0139] Based on the end posture, the angular velocity, the target posture, the virtual stiffness and the virtual damping, the auxiliary torque applied to the patient is calculated according to the following calculation formula:

[0140]

[0141] Wherein, The auxiliary torque applied to the patient is represented by The virtual stiffness is used to ensure that the end posture Approaches the target posture And the virtual damping Is used to dissipate the angular velocity .

[0142] On this basis, the step of obtaining the joint control torque based on the auxiliary force and the auxiliary torque can be realized in the following manner:

[0143] Decouple the Jacobian matrix of the mechanical arm to obtain the position Jacobian matrix and the posture Jacobian matrix; based on the position Jacobian matrix and the auxiliary force, obtain the first joint torque; based on the posture Jacobian matrix and the auxiliary torque, obtain the second joint torque; combine the first joint torque and the second joint torque to obtain the joint control torque.

[0144] In the present embodiment, the Jacobian matrix of the mechanical arm is Decoupled, and the result is shown in formula (20), through which the Cartesian force obtained by the spatial force field controller and the rotation impedance controller is converted into the first joint torque And the second joint torque , which are calculated by formula (21) and formula (22) respectively:

[0145]

[0146] The first joint torque and the second joint torque are added to obtain a joint control torque .

[0147] In addition, in the embodiment, a dynamics torque of a robot dynamics model and a null space control torque are obtained. The null space movement of the robot arm needs to be constrained and compensated for the dynamics term of the robot arm The null space control torque is calculated by formula (23):

[0148]

[0149] Finally, the comprehensive control torque for controlling the robot arm is calculated by the following formula:

[0150]

[0151] As shown in Figure 6 , a rehabilitation robot system block diagram of an adaptive space force field control strategy, the right side of the figure is a robot dynamics model part, including gravity term, friction term and Coriolis force term. The actual movement position and speed of the subject's hand are obtained through the robot joint sensor, and are further calculated based on the forward kinematics and Jacobian matrix of the robot. The left side describes the space force field proposed in the embodiment, which together with the output of the rotational impedance controller forms an auxiliary joint control torque .

[0152] In order to more clearly illustrate the implementation effect of the robot arm control method provided in the embodiment, the following will be further described in combination with the experimental test based on the implementation of the robot arm control method.

[0153] The purpose of the experiment is to verify whether the adaptive force field control strategy can adjust the force field assistance according to the training performance of the subject. The experiment has 3 control groups and 3 experimental groups, and the specific experimental arrangement is shown in Table 1. The adaptive force field control parameters in the experiment are shown in Table 2. In the experiment, the subject sat inside the robot, kept the torso still, and assumed that the left side of the subject was the affected limb. The subject needed to hold the handle at the end of the robot with the left hand to perform ADL training.

[0154]

[0155]

[0156] In summary, the adaptive force field based robotic arm control method provided in the embodiment can apply appropriate assistance force according to the training performance of the patient. The adaptive force field can measure the training performance of the patient from multiple angles and provide different assistance for the idling, overspeed, centripetal and centrifugal motion of the patient. The three sub-force fields can be adjusted separately to change their performance in human-machine interaction, so that the rehabilitation needs of patients in different recovery situations can be met.

[0157] Compared with the general force field, the adaptive force field proposed in the scheme provides assistance force that is not only related to the position deviation of the subject, but also changes with the different motion speed and motion direction of the subject. Compared with the traditional impedance-based control strategy, the motion of the subject in the adaptive force field has a time degree of freedom and is not affected by the time-coded desired trajectory. The subject can have more freedom of active motion under the assistance of the spatial force field.

[0158] Based on the same inventive concept, please refer to Figure 7 The embodiment of the present application also provides a functional module schematic diagram of the adaptive force field based robotic arm control system. The adaptive force field based robotic arm control system can be divided into functional modules according to the method embodiment. For example, each functional module can be divided according to each function, or two or more functions can be integrated into one processing module. The integrated module can be realized in the form of hardware or software functional module. It should be noted that the division of modules in the embodiment of the present application is illustrative, and is only a logical functional division. In actual implementation, there can be another division method.

[0159] For example, in the case of dividing each functional module according to each function, Figure 7 The adaptive force field based robotic arm control system shown is only a system schematic diagram. The adaptive force field based robotic arm control system can include a monitoring module, a comparison module, a first calculation module, a second calculation module, an obtaining module and a calculation control module. The functions of each functional module of the adaptive force field based robotic arm control system will be described in detail below.

[0160] The monitoring module is used to monitor the motion state of the patient, and the motion state includes the motion position, the motion speed and the motion direction.

[0161] The comparison module is used to compare the motion position, the motion speed and the motion direction of the patient with the position information, the speed information and the direction information of the demonstration space trajectory respectively, to obtain the training performance information of the patient.

[0162] The first calculation module is configured to calculate the assisting force applied to the patient based on the training performance information and the constructed adaptive force field control model, and the assisting force includes normal force, tangential force and viscous force of the spatial force field.

[0163] The second calculation module is configured to calculate the assisting torque applied to the patient based on the end posture of the robot arm and the target posture.

[0164] The obtaining module is configured to obtain the joint control torque based on the assisting force and the assisting torque, and obtain the dynamic torque of the robot dynamics model and the null space control torque.

[0165] The calculation control module is configured to combine the joint control torque, the dynamic torque and the null space control torque to obtain the comprehensive control torque for controlling the robot arm.

[0166] The adaptive force field based robot arm control system provided by the embodiment can be used to execute the adaptive force field based robot arm control method in any of the above embodiments, and details not described in the embodiment can be referred to the corresponding description of the above embodiments, which will not be described herein.

[0167] Please refer to Figure 8 The electronic device provided by the embodiment of the present application is a structural block diagram of an electronic device, which can be a control device on a robot, or a computer device, a server, etc. in communication with the robot. The electronic device includes a memory, a processor and a communication module. The memory, the processor and the communication module are directly or indirectly electrically connected to each other to realize data transmission or interaction. For example, these elements can be electrically connected to each other through one or more communication buses or signal lines.

[0168] The memory is used to store computer programs or data. The memory can be, but is not limited to, a random access memory (RAM), a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM) and the like.

[0169] The processor is used to read / write data or programs stored in the memory, and to execute the adaptive force field based robot arm control method provided by any embodiment of the present application.

[0170] The communication module is configured to establish a communication connection between the electronic device and other communication terminals via a network, and configured to transmit and receive data via the network.

[0171] It should be understood that, Figure 8 The illustrated structure is only a structural schematic diagram of the electronic device, and the electronic device can further include more or less components than those shown in the drawings, or have a different configuration from that shown in the drawings. Figure 8 Figure 8

[0172] Further, the embodiment of the present application further provides a computer readable storage medium, which stores machine executable instructions, and the machine executable instructions are executed to implement the adaptive force field based robot arm control method provided by the above embodiment.

[0173] Specifically, the computer readable storage medium can be a general storage medium such as a mobile disk, a hard disk, etc., and when the computer program on the computer readable storage medium is run, the adaptive force field based robot arm control method can be executed. For the process involved when the computer readable storage medium and the executable instructions thereof are run, reference can be made to the related description in the above method embodiment, and will not be described in detail here.

[0174] In the embodiments provided by the present application, it should be understood that the disclosed device and method can be implemented in other ways. The device embodiments described above are only schematic. For example, the division of the units is only a logical function division, and there can be another division manner in actual implementation. For example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed mutual couplings or direct couplings or communication connections between the units can be indirect couplings or communication connections through some interfaces, devices or units, and can be electrical, mechanical or other forms.

[0175] In addition, the units described as separate components can or can not be physically separate, and the components displayed as units can or can not be physical units, i.e. can be located in one place, or can be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the embodiment.

[0176] Furthermore, each functional module in each embodiment of the present application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part.

[0177] ​​It should be noted that if the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer readable storage medium. Based on this understanding, the technical solutions of the present application or the parts of the present application that essentially contribute to the prior art or the parts of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes a plurality of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the embodiments of the present application. The aforementioned storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and various media that can store program codes.

[0178] The above is only an embodiment of the present application and is not used to limit the protection scope of the present application. For those skilled in the art, the present application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A robotic arm control method based on an adaptive force field, characterized in that, The method includes: The patient's movement status is monitored, including movement position, movement speed, and movement direction; The patient's movement position, speed, and direction are compared with the position, speed, and direction information of the demonstration spatial trajectory to obtain the patient's training performance information; Based on the training performance information and the constructed adaptive force field control model, the auxiliary force applied to the patient is calculated, and the auxiliary force includes the normal force, tangential force and viscous force of the spatial force field; The auxiliary torque applied to the patient is calculated based on the end-effector posture and target posture of the robotic arm; The joint control torque is obtained based on the auxiliary force and auxiliary torque, and the dynamic torque and zero-space control torque of the robot dynamics model are obtained. By combining the joint control torque, dynamic torque, and zero-space control torque, a comprehensive control torque for controlling the robotic arm is obtained.

2. The robotic arm control method based on adaptive force field according to claim 1, characterized in that, The training performance information includes positional deviation and aiming angle; The normal force of the spatial force field is calculated in the following way: The normal force coefficient at the current moment is calculated based on the normal force coefficient, position deviation, and adaptive gain factor at the previous moment. The normal force of the spatial force field is calculated based on the normal force coefficient, normal force direction, and positional deviation at the current moment. The aiming angle is the angle between the current direction of movement and the desired direction of movement. The adaptive gain factor is calculated based on the position deviation and the aiming angle in the following manner: Based on the relationship between the aiming angle and the set minimum and maximum aiming angles, it is determined whether the patient's movement is normal; based on the determination result, the amplification factor is determined according to the corresponding determination method; the position deviation is amplified according to the amplification factor, and the adaptive gain factor is calculated in combination with the fixed parameters of the adaptive model.

3. The robotic arm control method based on adaptive force field according to claim 1, characterized in that, The training performance information includes positional deviation and tangential motion speed; The tangential force of the spatial force field is calculated in the following way: The velocity difference between the demonstration space trajectory velocity and the tangential motion velocity is obtained, and the patient is determined to be in an idling or overspeeding state based on the tangential motion velocity. In the case of idling or overspeed, the tangential force scaling factor is calculated based on the relationship between the speed difference and the set threshold, and according to the tangential force scaling factor and the set threshold. The tangential force of the spatial force field is calculated based on the tangential force coefficient, tangential force direction, tangential force range coefficient, position deviation, and tangential force scaling factor.

4. The robotic arm control method based on adaptive force field according to claim 1, characterized in that, The training performance information includes movement trends; The viscous force of the spatial force field is calculated in the following way: The viscous force coefficient is calculated based on the aforementioned motion trend using the corresponding calculation method. Based on the viscosity coefficient, the patient's movement speed, and the velocity information of the demonstration spatial trajectory, the viscosity of the spatial force field is calculated.

5. The robotic arm control method based on adaptive force field according to claim 4, characterized in that, The movement trends include positive movement trends, no obvious movement trends, and reverse movement trends; The step of calculating the viscous force coefficient based on the motion trend according to the corresponding calculation method includes: When the motion trend is a positive motion trend or there is no obvious motion trend, the viscosity coefficient is determined to be the set lower limit value of the viscosity coefficient; When the motion trend is a reverse motion trend, a reverse velocity difference is obtained. When the reverse velocity difference is less than or equal to a preset difference, the viscosity coefficient is calculated based on the set lower limit of the viscosity coefficient, the upper limit of the viscosity coefficient, the reverse velocity difference, and the preset difference. When the reverse velocity difference is greater than the preset difference, the viscosity coefficient is determined to be the set upper limit of the viscosity coefficient.

6. The robotic arm control method based on adaptive force field according to claim 5, characterized in that, The reverse velocity difference is generated using a pseudo-velocity generation algorithm, and this step includes: Set an initial value for the pseudo velocity, and iterate according to the iteration step size based on the initial value. In each iteration, calculate the difference between the pseudo velocity and the real tangential motion velocity in the current iteration. If the difference is greater than the set difference, output the pseudo velocity of the current round and continue to execute the next round of iteration; If the difference is less than or equal to the set difference, output the actual tangential velocity and end the iteration.

7. The robotic arm control method based on adaptive force field according to claim 1, characterized in that, The step of calculating the auxiliary torque applied to the patient based on the end-effector posture and target posture of the robotic arm includes: Obtain the end effector attitude and angular velocity of the robotic arm, and obtain the target attitude; The auxiliary torque applied to the patient is calculated based on the end-effector attitude, angular velocity, target attitude, virtual stiffness, and virtual damping.

8. The robotic arm control method based on adaptive force field according to claim 1, characterized in that, The step of obtaining the joint control torque based on the auxiliary force and auxiliary torque includes: Decouple the Jacobian matrix of the robotic arm to obtain the position Jacobian matrix and the attitude Jacobian matrix; Based on the position Jacobian matrix and the auxiliary force, the first joint torque is obtained; Based on the posture Jacobian matrix and the auxiliary torque, the second joint torque is obtained; By combining the torque of the first joint and the torque of the second joint, the joint control torque is obtained.

9. A robotic arm control system based on an adaptive force field, characterized in that, The system is used to implement the robotic arm control method based on an adaptive force field according to any one of claims 1-8, the system comprising: The monitoring module is used to monitor and obtain the patient's motion status, which includes motion position, motion speed, and motion direction; The comparison module is used to compare the patient's movement position, movement speed, and movement direction with the position, speed, and direction information of the demonstration spatial trajectory, respectively, to obtain the patient's training performance information. The first calculation module is used to calculate the auxiliary force applied to the patient based on the training performance information and the constructed adaptive force field control model. The auxiliary force includes the normal force, tangential force and viscous force of the spatial force field. The second calculation module is used to calculate the auxiliary torque applied to the patient based on the end-effector posture and target posture of the robotic arm; The module is used to obtain the joint control torque based on the auxiliary force and auxiliary torque, and to obtain the dynamic torque and zero-space control torque of the robot dynamics model; The calculation control module is used to combine the joint control torque, dynamic torque and zero-space control torque to obtain the comprehensive control torque for controlling the robotic arm.

Images