CN116604565A - Force guiding control method and system for variable admittance of robot - Google Patents
Force guiding control method and system for variable admittance of robot Download PDFInfo
- Publication number
- CN116604565A CN116604565A CN202310731160.2A CN202310731160A CN116604565A CN 116604565 A CN116604565 A CN 116604565A CN 202310731160 A CN202310731160 A CN 202310731160A CN 116604565 A CN116604565 A CN 116604565A
- Authority
- CN
- China
- Prior art keywords
- robot
- force
- control
- speed
- control period
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 58
- 230000003993 interaction Effects 0.000 claims abstract description 70
- 230000001133 acceleration Effects 0.000 claims abstract description 64
- 230000036461 convulsion Effects 0.000 claims abstract description 15
- 230000009471 action Effects 0.000 claims abstract description 11
- 238000013016 damping Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 83
- 230000009466 transformation Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000004422 calculation algorithm Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000005457 optimization Methods 0.000 claims description 3
- 238000005094 computer simulation Methods 0.000 claims 1
- 230000008569 process Effects 0.000 abstract description 11
- 230000000694 effects Effects 0.000 description 6
- 238000012549 training Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000001228 spectrum Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
The invention discloses a force guiding control method and a system of a robot variable admittance, which relate to the technical field of robots, and infer the movement intention direction of a human in real time by acquiring the interaction force of the robot and the human, and enable the robot to move in a compliant way by adjusting inertia parameters and damping parameters, and comprise the following steps: acquiring the tail end motion information and man-machine interaction force information of the robot, predicting the acceleration at the next moment by using a minimum jerk model and a speed curvature mode, and obtaining expected force by combining an admittance control equation and the current speed; generating a virtual inertia and virtual damping adjustment strategy according to the expected force direction; acquiring expected speed and angular speed of the next control period according to the adjusted virtual inertia and virtual damping; the robot tip is controlled to perform compliant actions and reject external disturbances. The method does not need preset tracks and preconditions, improves the accuracy, smoothness and observability of the teaching tracks, improves the applicability of the man-machine interaction process, and can be applied to various unknown scenes.
Description
Technical Field
The invention relates to the technical field of robots, in particular to a force guiding control method and system for a variable admittance of a robot.
Background
The robot has the advantages of intelligence, high operation precision, high dexterity and the like, and is widely applied to various fields of assembly, remote operation, robot rehabilitation and the like. Applications in the above fields are all independent of physical man-machine interaction. The direct teaching is one of the functions widely applied, and a human drags the tail end of the robot to generate a motion trail, so that a more visual and time-saving robot programming method is provided. The reasonable compliant control method not only can ensure the safety and high efficiency of tasks, but also can ensure that the initial track obtained by the teaching of the robot is more natural, smooth and compliant.
Impedance control and admittance control are commonly used in robot teaching due to their active compliance, impedance control being a control method of input displacement output force. However, in the taught scenario, the robot is modeled and controlled as admittance in most scenarios, since the forces are generated by a person, the robot needs to output corresponding displacements to match the force inputs. In addition, the admittance controller can be more easily adapted to current position-controlled robotic systems. In teaching without a preset expected path, although the robot can conform to the operation of a human hand under admittance control, when the track deviates from a real-time planned path of a person aiming at some unavoidable conditions such as external disturbance and noise influence, frequent correction errors can cause the teaching process to be not compliant, discontinuous and have increased errors. Since the admittance control parameters in the existing control scheme are fixed or the admittance parameter values in all directions are adjusted to be the same and changed only by considering the acceleration and deceleration intention of a person, when the system is affected by external interference or noise, the robot system cannot reject or resist the external interference, so that the teaching or interaction effect is poor.
Therefore, how to adjust the admittance control parameters of a robot based on human intention to improve the compliance and anti-jamming capability of the robot is an important research problem in robot teaching or interaction.
Accordingly, those skilled in the art have been directed to developing a method and system for controlling force guidance of a variable admittance of a robot.
Disclosure of Invention
In view of the above-mentioned drawbacks of the prior art, the present invention aims to solve the technical problem that in the robot teaching, the intended direction of the person cannot be inferred without training on the premise of not presetting the expected path, and further the interference influence in the process cannot be suppressed.
In order to achieve the above object, the present invention provides a force guidance control method of a robot variable admittance, the method obtains an interaction force between the robot and a person through a sensor, deduces a motion intention direction of the person in real time according to a track in a past time period and the interaction force, and adjusts an inertia parameter and a damping parameter in admittance control, so that the robot end follows motion and rejects external interference based on the interaction force, the method comprises the following steps:
s101: acquiring terminal motion information and man-machine interaction force information of the robot, predicting acceleration at the next moment by using a minimum jerk model and a speed curvature mode, and obtaining expected force by combining an admittance control equation and the current speed;
s103: generating a virtual inertia and virtual damping adjustment strategy according to the direction of the expected force;
s105: acquiring the expected speed of the tail end of the robot and the expected angular speed of the joint of the robot in the next control period according to the adjusted virtual inertia and the virtual damping;
s107: and controlling the tail end of the robot to perform compliance action and rejecting external interference.
Further, in the step S101, a six-dimensional force sensor is disposed at the end of the robot, where the force sensor is configured to detect the man-machine interaction force information, and send a signal of the force sensor to a controller; the terminal motion information is acquired by each joint angle sensor of the robot, and is obtained through the positive kinematics of the robot.
Further, calculating the speed and acceleration information of the first N control periods through a central difference algorithm,
wherein V is the speed of the device,the method is characterized in that the method comprises the steps of taking acceleration, X as the tail end position of a robot, k as the kth control period, namely the current control period, N as the nth control period before the current control period, n=1, 2 … N, N as the number of control periods of speed and acceleration information calculated by the equation, and T as the control period duration;
calculating speed information at the current moment through forward difference:
wherein V is the speed, X is the end position of the robot, X (k) represents the current end position of the robot, X (k-1) represents the end position of the robot in the first 1 control period, k is the kth control period, and T is the duration of the control period;
the curvature calculation model corresponding to the first N control periods is as follows:
wherein ,Vx Representing the component of velocity in the x-axis, V y Representing the component of the velocity in the y-axis,representing the component of acceleration in the x-axis, +.>The component of acceleration on the y axis is represented, V is a velocity vector, k is the kth control period, n=1, 2 … N, T is the control period duration, and κ is the calculated curvature;
the desired acceleration satisfies the velocity curvature pattern, and the equation is available as follows:
wherein V is a velocity vector, V x Representing the component of velocity in the x-axis, V y Representing the component of the velocity in the y-axis,for the component of the desired acceleration in the x-axis, +.>For the component of the desired acceleration in the y-axis, K is the kth control period, K is the scaling factor, and β is the power exponent.
Further, according to the minimum jerk model of the human planned path and the expected speed direction being between the current force direction and the speed direction, optimizing the expected acceleration, the optimization model is:
wherein V is the speed of the device,for the estimated speed of the next control period, +.>For the expected acceleration, F is the man-machine interaction force vector, k is the kth control period, t k-1 Time t representing the last control period k Time representing the current control period;
the desired force may be derived from the admittance control model as:
wherein ,Fexpected For a desired force vector, V is the velocity,for a desired acceleration, M is a default virtual inertia matrix, B is a default virtual damping matrix, k is the kth control period,
m is a default virtual inertia value, and b is a default virtual damping value.
Further, the step S103 further includes the following sub-steps:
s1031: establishing a coordinate system of a desired force direction;
s1032: establishing a transformation matrix of a coordinate system of the expected force direction and a base coordinate system according to the expected force direction and a coordinate system perpendicular to the expected force direction;
s1033: establishing a strategy for suppressing external disturbance and noise: when a force is applied in a direction in which the force is expected, the admittance parameter of the direction is maintained at a default value; when external noise or disturbance is introduced, the admittance parameter perpendicular to the expected direction becomes larger, so that the movement is more difficult in the direction perpendicular to the expected direction;
wherein the conversion matrix is:
wherein ,Md Is a virtual inertial matrix, B d As a virtual damping matrix, R is a transformation matrix of a desired force direction coordinate system and a base coordinate system, gamma is a proportionality coefficient, and gamma=1+10||V (k) |is a transformation matrix of the desired force direction coordinate system and the base coordinate system 2 V is the speed, k is the kth control period, m is the default virtual inertia value, and b is the default virtual damping value.
Further, in the step S105, the admittance control equation of the robot is:
wherein ,Md Is a virtual inertial matrix, B d The system is a virtual damping matrix, X is a position under a Cartesian coordinate system, and F is human-machine interaction force;
according to the virtual inertia matrix, the virtual damping matrix, the man-machine interaction force and the speed at the current moment, the expected acceleration of the tail end of the robot in the next control period can be calculated to obtain:
wherein ,for a desired tip speed value sent to the robot, < >>For the end speed of the robot in the previous control period, F (k) is man-machine interaction force, M d Is a virtual inertial matrix, B d K represents a kth control period, and T represents the control period duration;
the desired angular velocity of the robotic joint may be obtained from an inverse jacobian matrix:
wherein ,for the desired angular velocity of the joint +.>Is an inverse Jacobian matrix->For a desired tip speed value sent to the robot, k represents the kth control period.
On the other hand, the invention also provides a force guiding control system of the robot variable admittance, which adopts the force guiding control method of the robot variable admittance, and comprises a man-machine interaction module, a human intention prediction module, a variable admittance control module and a robot tail end position control module,
the man-machine interaction module comprises a robot tail end;
the human intention prediction module is used for collecting the tail end motion information and the man-machine interaction force information of the robot, predicting the acceleration at the next moment by applying the minimum jerk model and the speed curvature mode, and obtaining the expected force by combining the admittance control equation and the current speed;
the admittance-changing control module establishes a current expected force coordinate system according to the direction of the expected force to generate the virtual inertia and the virtual damping adjustment strategy;
and the robot tail end position control module acquires the expected speed of the robot tail end in the next control period according to the adjusted virtual inertia and the virtual damping, further acquires the joint angular speed of the robot, and controls the robot tail end to perform compliance action.
Further, the human intent prediction module employs the following calculation model to calculate the expected force:
wherein ,Fexpected For the desired force vector, M is the default virtual inertia matrix, B is the default virtual damping matrix, V is the current velocity vector,for a desired acceleration, k represents the kth control period.
Further, the admittance control equation used by the admittance control module is:
wherein ,Md Is a virtual inertial matrix, B d The system is a virtual damping matrix, X is the position under a Cartesian coordinate system, and F is the man-machine interaction force between the robot and the person;
further, the robot tip position control module calculates the desired speed of the robot tip for the next control cycle using the following method:
wherein ,desired tip speed value sent to the robot for the current control cycle, is->The desired tip speed value sent to the robot for the last control cycle,/for the control cycle>For the end speed of the robot in the previous control period, F (k) is the man-machine interaction force in the current control period, M d Is a virtual inertial matrix, B d Is virtualThe damping matrix, k, represents the kth control period and T represents the control period duration.
In the preferred embodiment of the present invention, compared with the prior art, the present invention has the following beneficial effects:
1. according to the invention, the expected acceleration is predicted through the minimum jerk model and the speed curvature power law spectrum of the human planning path, so that the direction of the force can be predicted, and the force is guided;
2. the invention improves the applicability of the man-machine interaction process and can be widely applied to various unknown scenes.
The conception, specific structure, and technical effects of the present invention will be further described with reference to the accompanying drawings to fully understand the objects, features, and effects of the present invention.
Drawings
FIG. 1 is a flow chart of a method of controlling force guidance of a variable admittance of a robot in accordance with a preferred embodiment of the present invention;
FIG. 2 is a control block diagram of a force-directed control system of a robot variable admittance in accordance with a preferred embodiment of the present invention;
fig. 3 is a schematic view of a force guiding control method of a robot variable admittance according to a preferred embodiment of the present invention.
Detailed Description
The following description of the preferred embodiments of the present invention refers to the accompanying drawings, which make the technical contents thereof more clear and easy to understand. The present invention may be embodied in many different forms of embodiments and the scope of the present invention is not limited to only the embodiments described herein.
In the drawings, like structural elements are referred to by like reference numerals and components having similar structure or function are referred to by like reference numerals. The dimensions and thickness of each component shown in the drawings are arbitrarily shown, and the present invention is not limited to the dimensions and thickness of each component. The thickness of the components is exaggerated in some places in the drawings for clarity of illustration.
In the prior robot teaching, the direction of the intention of a person cannot be deduced without training on the premise of not presetting a desired path, and further the interference influence in the process cannot be restrained.
As shown in fig. 1, the method for controlling force guidance of a robot with variable admittance provided by the embodiment of the present invention obtains the interaction force between the robot and a person through a sensor, and deduces the motion intention direction of the person in real time according to the track and the man-machine interaction force in the past time period, so as to adjust the inertia parameter and the damping parameter in admittance control, so that the tail end of the robot moves in a compliant manner based on the man-machine interaction force, and rejects external interference.
Specifically, the method comprises the following steps:
s101: and acquiring terminal motion information and man-machine interaction force information of the robot, predicting the acceleration at the next moment by using a minimum jerk model and a speed curvature mode, and obtaining expected force by combining an admittance control equation and the current speed.
In the step, a six-dimensional force sensor is arranged at the tail end of the robot, and is used for detecting man-machine interaction force information and sending a force sensor signal to a controller; the end motion information of the robot is acquired by each joint angle sensor of the robot, and the end motion information is obtained through the forward kinematics of the robot.
Velocity and acceleration information for the first N control cycles is calculated by a central difference algorithm,
wherein V is the speed of the device,the method is characterized in that the method comprises the steps of taking acceleration, X as the tail end position of a robot, k as the kth control period, namely the current control period, N as the nth control period before the current control period, n=1, 2 … N, N as the number of control periods of speed and acceleration information calculated by the equation, and T as the control period duration;
calculating speed information at the current moment through forward difference:
wherein V is the speed, X is the end position of the robot, X (k) represents the current end position of the robot, X (k-1) represents the end position of the robot in the first 1 control period, k is the kth control period, and T is the duration of the control period;
the curvature calculation model corresponding to the first N control periods is as follows:
wherein ,Vx Representing the component of velocity in the x-axis, V y Representing the component of the velocity in the y-axis,representing the component of acceleration in the x-axis, +.>The component of acceleration on the y axis is represented, V is a velocity vector, k is the kth control period, n=1, 2 … N, T is the control period duration, and κ is the calculated curvature;
the desired acceleration satisfies the velocity curvature pattern, and the equation is available as follows:
wherein V is a velocity vector, V x Representing the component of velocity in the x-axis, V y Representing the component of the velocity in the y-axis,for the component of the desired acceleration in the x-axis, +.>For the component of the desired acceleration in the y-axis, K is the kth control period, K is the scaling factor, and β is the power exponent.
According to the minimum jerk model of the human planning path and the expected speed direction between the current force direction and the speed direction, optimizing the expected acceleration, wherein the optimization model is as follows:
wherein V is the speed of the device,for the estimated speed of the next control period, +.>For the expected acceleration, F is the man-machine interaction force vector, k is the kth control period, t k-1 Time t representing the last control period k Time representing the current control period;
the desired force may be derived from the admittance control model as:
wherein ,Fexpected For a desired force vector, V is the velocity,for a desired acceleration, M is a default virtual inertia matrix, B is a default virtual damping matrix, k is the kth control period,
m is a default virtual inertia value, and b is a default virtual damping value.
S103: and generating a virtual inertia and virtual damping adjustment strategy according to the direction of the expected force.
The steps include the following sub-steps:
s1031: establishing a coordinate system of a desired force direction;
s1032: establishing a transformation matrix of a coordinate system of the expected force direction and a base coordinate system according to the expected force direction and a direction perpendicular to the expected force direction;
s1033: establishing a strategy for suppressing external disturbance and noise: when a force is applied in a direction in which the force is expected, the admittance parameter of the direction is kept at a default value; when external noise or disturbance is introduced, the admittance parameter perpendicular to the expected direction becomes larger, so that the movement is more difficult in the direction perpendicular to the expected direction;
wherein, the conversion matrix is:
wherein ,Md Is a virtual inertial matrix, B d As a virtual damping matrix, R is a transformation matrix of a desired force direction coordinate system and a base coordinate system, gamma is a proportionality coefficient, and gamma=1+10||V (k) |is a transformation matrix of the desired force direction coordinate system and the base coordinate system 2 V is the speed, k is the kth control period, m is the default virtual inertia value, and b is the default virtual damping value.
S105: and acquiring the expected speed of the tail end of the robot and the expected angular speed of the joints of the robot in the next control period according to the adjusted virtual inertia and virtual damping.
In the above steps, the admittance control equation of the robot is:
wherein ,Md Is a virtual inertial matrix, B d The system is a virtual damping matrix, X is a position under a Cartesian coordinate system, and F is human-machine interaction force;
according to the virtual inertia matrix, the virtual damping matrix, the man-machine interaction force and the speed at the current moment, the expected acceleration of the tail end of the robot in the next control period can be calculated to obtain:
wherein ,for a desired tip speed value sent to the robot, < >>For the end speed of the robot in the previous control period, F (k) is man-machine interaction force, M d Is a virtual inertial matrix, B d K represents a kth control period, and T represents the control period duration;
the desired angular velocity of the robot joint may be obtained from an inverse jacobian matrix:
wherein ,for the desired angular velocity of the joint +.>Is an inverse Jacobian matrix->For a desired tip speed value sent to the robot, k represents the kth control period.
S107: the robot tip is controlled to perform compliant actions and reject external disturbances.
Compared with a control method without considering the intention direction of a human, the force guiding control method for the robot variable admittance, provided by the embodiment of the invention, does not need a preset track and preconditions, improves the accuracy, smoothness and intuitiveness of a teaching track, and lays a foundation for the subsequent reproduction track; compared with the prior patent, the method improves the applicability of the man-machine interaction process and can be widely applied to various unknown scenes.
In addition, the invention utilizes the coordinate system transformation principle to transform the admittance parameter matrix in real time, so that the admittance parameter of the expected force direction is low, the admittance parameter of the direction perpendicular to the expected force direction is high, compared with the independent admittance control of all directions, the robot is more compliant through the low admittance of the expected force direction, the high admittance of the direction perpendicular to the expected force direction enables the robot to resist the force of the direction, a force guiding effect is formed, the interference of the external environment is suppressed, and the teaching track precision and the smoothness are improved.
The embodiment of the invention also provides a force guiding control system of the robot with variable admittance, which adopts the force guiding control method of the robot with variable admittance, and comprises a man-machine interaction module, a human intention prediction module, a variable admittance control module and a robot tail end position control module,
the man-machine interaction module comprises a robot tail end;
the human intention prediction module is used for acquiring terminal motion information and man-machine interaction force information of the robot, predicting acceleration at the next moment by applying a minimum jerk model and a speed curvature mode, and obtaining expected force by combining an admittance control equation and the current speed;
the admittance-changing control module is used for setting up a current expected force coordinate system according to the direction of the expected force to generate a virtual inertia and virtual damping adjustment strategy;
and the robot tail end position control module acquires the expected speed of the robot tail end in the next control period according to the adjusted virtual inertia and virtual damping, further acquires the joint angular speed of the robot, and controls the robot tail end to perform compliance action.
The human intent prediction module uses the following calculation model to calculate the expected force:
wherein ,Fexpected For the desired force vector, M is the default virtual inertia matrix, B is the default virtual damping matrix, V is the current velocity vector,for a desired acceleration, k represents the kth control period.
The admittance control equation used by the admittance control module is:
wherein ,Md Is a virtual inertial matrix, B d The system is a virtual damping matrix, X is the position under a Cartesian coordinate system, and F is the man-machine interaction force between the robot and the person;
the robot tip position control module calculates the desired speed of the robot tip for the next control cycle using the following method:
wherein ,desired tip speed value sent to the robot for the current control cycle, is->The desired tip speed value sent to the robot for the last control cycle,/for the control cycle>For the end speed of the robot in the previous control period, F (k) is the man-machine interaction force in the current control period, M d Is a virtual inertial matrix, B d For the virtual damping matrix, k represents the kth control period and T represents the control period duration.
The present invention will be described in detail with reference to preferred embodiments thereof.
As shown in fig. 1, fig. 2 and fig. 3, the force guiding control method for the variable admittance of the robot provided by the preferred embodiment of the present invention requires that the tail end of the robot is compliant to follow the movement of a person and can resist the interference caused by the outside in the process of man-machine cooperation interaction. According to the invention, the interaction force between the robot and the person is obtained through the sensor, the movement intention direction of the person is deduced in real time according to the track and the interaction force in the past time period, and then the inertia parameter and the damping parameter in admittance control are adjusted, so that the tail end of the robot can conform to movement based on the interaction force of the person, and meanwhile, the action of force guidance of the person is provided to reject external interference.
As shown in fig. 1, the force guidance control method for the variable admittance of the robot provided by the preferred embodiment of the present invention includes the following specific steps:
step one, acquiring the motion information of the tail end of the robot and the man-machine interaction force information, predicting the acceleration at the next moment by using a minimum jerk model and a speed curvature mode, and obtaining the expected force by combining an admittance control equation and the current speed.
Specifically, in the direct teaching process, a person pulls the tail end of the robot to move, a six-dimensional force sensor is fixed at the tail end of the robot, and the force sensor is used for detecting interaction force between the robot and the robot in the robot pulling process and sending a signal of the force sensor to the controller. The end information of the robot is acquired by each joint angle sensor of the robot, and the motion information of the end of the robot is obtained through the forward kinematics of the robot. The speed and acceleration information of the first N control periods is obtained through a central difference algorithm:
wherein n=1, 2 … N, N represents the calculated speed and acceleration information as the nth control period before the current control period, N represents the number of control periods applied to the speed and acceleration information calculated by the above equationThe acceleration is represented by V, the speed is represented by k, the current control period is the kth control period, X represents the position of the tail end of the robot, and T represents the control period duration.
Obtaining speed information at the current moment through forward difference:
wherein V (k) represents the speed of the current control period obtained by calculation, X (k) represents the current end position of the robot, X (k-1) represents the end position of the robot in the first 1 control period, and T represents the duration of the control period.
The calculation model of the curvature corresponding to the first N control periods is as follows:
wherein ,Vx Representing the component of velocity in the x-axis, V y Representing the component of the velocity in the y-axis,representing the component of acceleration in the x-axis, +.>Representing the component of acceleration in the y-axis, V represents the velocity vector, and κ is the calculated curvature.
The proportionality coefficient and the power exponent in the speed curvature mode are obtained by adopting a least square method, wherein kappa (K-N), n=1, 2 … N are taken as input, V (K-N), n=1, 2 … N are taken as output, and the proportionality coefficient K and the power exponent beta are obtained through real-time identification.
The desired acceleration should also satisfy the velocity curvature pattern, the equation can be found as follows:
wherein ,for the component of the desired acceleration in the x-axis, +.>For the component of the desired acceleration in the y-axis, K is a proportionality coefficient, beta is a power exponent, V (K) represents the calculated speed of the current control period, V x Representing the component of velocity in the x-axis, V y Representing the component of velocity in the y-axis.
According to a minimum jerk model of a human planning path, simultaneously meeting the requirement that an expected speed direction is between a current force direction and a speed direction, optimizing expected acceleration, wherein the model is as follows:
wherein ,expressed as an estimate of the speed of the next control period compared to the current control period +.>Third derivative of position in x-direction with respect to time t +.>Representing the third derivative of the position in the y-direction with respect to time t, t k-1 Time t representing the last control period k Representing the time of the current control period, F (k) represents the interaction force vector read by the sensor for the current control period.
The desired force may be derived from the admittance control model as:
/>
wherein ,Fexpected For the desired force vector, M is a default virtual inertia matrix, is a diagonal matrix and has equal elements on the diagonal, M is a default virtual inertia value, B is a default virtual damping matrix, is a diagonal matrix and has equal elements on the diagonal, B is a default virtual damping value,the acceleration vector expected in the current control period is represented, and V (k) represents the velocity value of the current control period obtained through forward differential calculation.
Step two: and according to the direction of the expected force, a current expected force coordinate system is established different from a tool coordinate system and a base coordinate system, and a virtual inertia and virtual damping adjustment strategy is generated.
Specifically, the method of the present invention first establishes a coordinate system of the desired force direction. The conversion matrix of the constructed coordinate system and the base coordinate system can be obtained by constructing the expected force direction and the direction perpendicular to the expected force direction as R. The strategy for suppressing external disturbance and noise in the invention is as follows: when a person applies a force in a direction in which the person desires to apply the force, the admittance parameter of the direction is maintained at a default value; when external noise or disturbances are introduced, the admittance parameter perpendicular to the desired direction of the person will become larger, making it more difficult to move perpendicular to the desired direction.
Wherein R represents a transformation matrix of a coordinate system taking a desired force direction as an x axis and a y axis perpendicular to the desired force direction and a base coordinate system of the robot, and M d Is a virtual inertial matrix, B d For a virtual damping matrix, m is a default virtual inertia value, b is a default virtual damping value, gamma is a proportionality coefficient, and admittance perpendicular to the direction of the expected forceThe parameter gets larger in proportion to the current speed:
γ＝1+10||V(k)|| 2
where γ is a proportionality coefficient, and V (k) is the speed of the current cycle obtained by forward differential calculation.
As shown in fig. 2, the interaction force between the robot and the person is obtained by the sensor, the movement intention direction of the person is deduced in real time according to the track and the interaction force in the past time period, and then the inertia parameter and the damping parameter in admittance control are adjusted, and the adjusted M is obtained d Virtual inertial matrix and B d The virtual damping matrix is transferred to admittance control so that the robot tip can provide a force-directed action by a person to reject external disturbances while conforming to motion based on the person's interaction force. The dashed lines in fig. 2 represent the two parameters m and b of the admittance parameter modulation change, which are in turn passed into the admittance control, which dashed lines are not directed, but represent that the admittance control has been traversed, whereby an update of the parameters is achieved.
And thirdly, acquiring the expected speed of the tail end of the robot in the next control period according to the adjusted virtual inertia and virtual damping, and further acquiring the corresponding expected angular speed of the joint of the robot, so as to control the tail end of the robot to perform compliance action under the force guiding control method of the variable admittance of the robot based on the intentional identification of the robot.
Specifically, the admittance control model of the robot is:
wherein ,Md Is a virtual inertial matrix, B d The virtual damping matrix is formed by a Cartesian coordinate system, X is the position under the Cartesian coordinate system, and F is the interaction force between the robot and the person.
According to the virtual inertia matrix, the virtual damping matrix, the interaction force and the speed at the current moment, the expected acceleration of the tail end of the robot in the next control period can be calculated to obtain:
/>
where k represents a kth control period, T represents a control period duration,a desired end speed value indicative of the current control cycle sent to the robot, < >>Representing the desired end speed value sent to the robot for the last control cycle, F (k) representing the interaction force read by the force sensor for the current control cycle, +.>Representing the calculated robot tip speed for the previous control cycle.
The desired robot joint angular velocity may be obtained by a inverse jacobian matrix:
wherein ,for the desired angular velocity of the joint +.>Is an inverse Jacobian matrix->Indicating the desired tip speed value sent to the robot for the current control cycle.
In a preferred embodiment of the present invention, the present invention also provides a force guidance control system of a robot variable admittance, including a man-machine interaction module, a human intention prediction module, a variable admittance control module, and a robot end position control module, wherein,
the man-machine interaction module comprises a person and a robot tail end, and the person pulls the robot tail end;
the human intention prediction module acquires the tail end motion information and the man-machine interaction force information of the robot, predicts the acceleration at the next moment by using a minimum jerk model and a speed curvature mode, and combines an admittance control equation and the current speed to obtain expected force;
further preferably, the calculation model of the expected force is:
wherein ,Fexpected For the desired force vector, M is a default virtual inertia matrix, is a diagonal matrix and has equal elements on the diagonal, M is a default virtual inertia value, B is a default virtual damping matrix, is a diagonal matrix and has equal elements on the diagonal, B is a default virtual damping value,the acceleration vector expected in the current control period is represented, V (k) represents the velocity value of the current period obtained through forward differential calculation, and k is the kth control period.
The admittance-changing control module is used for distinguishing a tool coordinate system from a base coordinate system according to the direction of an expected force, setting up a current expected force coordinate system and generating a virtual inertia and virtual damping adjustment strategy;
as a further preferred aspect, the robot admittance control model is:
wherein ,Md Is a virtual inertial matrix, B d The virtual damping matrix is formed by a Cartesian coordinate system, X is the position under the Cartesian coordinate system, and F is the interaction force between the robot and the person.
The robot tail end position control module acquires the expected speed of the robot tail end of the next control period according to the adjusted virtual inertia and virtual damping, and further acquires the corresponding expected robot joint angular speed, so that the robot tail end is controlled to perform compliance action under the force guiding control method of the robot variable admittance based on the intentional identification.
As a further preference, the desired speed is:
where k represents a kth control period, T represents a control period duration,a desired end speed value indicative of the current control cycle sent to the robot, < >>Representing the desired tip speed value sent to the robot for the last control cycle, F (k) representing the interaction force read by the force sensor for the current control cycle,
representing the calculated robot tip speed for the previous control cycle.
In the robot teaching in the prior art, on the premise of not presetting a desired path, the intention direction of a person cannot be deduced without training, and further the interference influence in the process cannot be restrained. According to the optimal embodiment of the invention, through a minimum jerk model of a theoretically verified human planning path and a speed curvature power law spectrum phenomenon obtained by observation, expected acceleration is predicted, so that the direction of force can be predicted, and force guidance is performed.
In addition, the admittance parameters in the current admittance control are mostly diagonal arrays, are isotropic, cannot conform to the expected direction of real-time change, and resist the unexpected direction. The preferred embodiment of the invention solves the problem of the expected force direction, utilizes the coordinate system transformation principle to transform the admittance parameter matrix in real time, so that the admittance parameter of the expected force direction is low, the admittance parameter of the direction perpendicular to the expected force direction is high, compared with the independent admittance control of each direction, the low admittance of the expected force direction leads the robot to be more compliant, the high admittance of the direction perpendicular to the expected force direction leads the robot to resist the force of the direction, the force guiding effect is formed, the interference of the external environment is suppressed, and the teaching track precision and the smoothness are improved.
Therefore, compared with the prior art, the method can obtain better technical effects, deduce the intention direction of the person in real time by acquiring the interaction force between the robot and the person and the track in the past time period, does not need to preset the track and preconditions, improves the applicability and intuitiveness of the man-machine interaction process, and is more widely applied to various unknown scenes; the admittance-variable force guiding method based on human intention recognition can improve the compliance of the robot, simultaneously provide force guiding, play a role in inhibiting interference of external environment, and improve the accuracy and smoothness of compliance.
The foregoing describes in detail preferred embodiments of the present invention. It should be understood that numerous modifications and variations can be made in accordance with the concepts of the invention without requiring creative effort by one of ordinary skill in the art. Therefore, all technical solutions which can be obtained by logic analysis, reasoning or limited experiments based on the prior art by the person skilled in the art according to the inventive concept shall be within the scope of protection defined by the claims.
Claims (10)
1. The method is characterized in that the method obtains the interaction force between the robot and a person through a sensor, deduces the movement intention direction of the person in real time according to the track in the past time period and the interaction force, and further adjusts the inertia parameter and the damping parameter in admittance control, so that the tail end of the robot moves in a compliant mode based on the interaction force and rejects external interference, and the method comprises the following steps:
s101: acquiring terminal motion information and man-machine interaction force information of the robot, predicting acceleration at the next moment by using a minimum jerk model and a speed curvature mode, and obtaining expected force by combining an admittance control equation and the current speed;
s103: generating a virtual inertia and virtual damping adjustment strategy according to the direction of the expected force;
s105: acquiring the expected speed of the tail end of the robot and the expected angular speed of the joint of the robot in the next control period according to the adjusted virtual inertia and the virtual damping;
s107: and controlling the tail end of the robot to perform compliance action and rejecting external interference.
2. The control method according to claim 1, wherein in the step S101, a six-dimensional force sensor is provided at the end of the robot, the force sensor is used for detecting the man-machine interaction force information, and the force sensor signal is sent to a controller; the terminal motion information is acquired by each joint angle sensor of the robot, and is obtained through the positive kinematics of the robot.
3. The control method of claim 2, wherein the velocity and acceleration information for the first N control cycles is calculated by a central difference algorithm,
wherein V is the speed of the device,the method is characterized in that the method comprises the steps of taking acceleration, X as the tail end position of a robot, k as the kth control period, namely the current control period, N as the nth control period before the current control period, n=1, 2 … N, N as the number of control periods of speed and acceleration information calculated by the equation, and T as the control period duration;
calculating speed information at the current moment through forward difference:
wherein V is the speed, X is the end position of the robot, X (k) represents the current end position of the robot, X (k-1) represents the end position of the robot in the first 1 control period, k is the kth control period, and T is the duration of the control period;
the curvature calculation model corresponding to the first N control periods is as follows:
wherein ,Vx Representing the component of velocity in the x-axis, V y Representing the component of the velocity in the y-axis,representing the component of acceleration in the x-axis,representing the component of acceleration in the y-axis, V being the velocity vector, k being the kth control period, n=1, 2 …N, T is the control period duration, and kappa is the calculated curvature;
the desired acceleration satisfies the velocity curvature pattern, and the equation is available as follows:
wherein V is a velocity vector, V x Representing the component of velocity in the x-axis, V y Representing the component of the velocity in the y-axis,for the component of the desired acceleration in the x-axis, +.>For the component of the desired acceleration in the y-axis, K is the kth control period, K is the scaling factor, and β is the power exponent.
4. A control method according to claim 3, characterized in that the desired acceleration is optimized on the basis of the minimum jerk model of the human planned path and that the desired speed direction is between the current force direction and the speed direction, the optimization model being:
wherein V is the speed of the device,for the estimated speed of the next control period, +.>For the expected acceleration, F is the man-machine interaction force vector, k is the kth control period, t k-1 Time t representing the last control period k Time representing the current control period;
the desired force may be derived from the admittance control model as:
wherein ,Fexpected For a desired force vector, V is the velocity,for a desired acceleration, M is a default virtual inertia matrix, B is a default virtual damping matrix, k is the kth control period,
m is a default virtual inertia value, and b is a default virtual damping value.
5. The control method according to claim 1, wherein the step S103 further includes the sub-steps of:
s1031: establishing a coordinate system of a desired force direction;
s1032: establishing a transformation matrix of a coordinate system of the expected force direction and a base coordinate system according to the expected force direction and a coordinate system perpendicular to the expected force direction;
s1033: establishing a strategy for suppressing external disturbance and noise: when a force is applied in a direction in which the force is expected, the admittance parameter of the direction is maintained at a default value; when external noise or disturbance is introduced, the admittance parameter perpendicular to the expected direction becomes larger, so that the movement is more difficult in the direction perpendicular to the expected direction;
wherein the conversion matrix is:
wherein ,Md Is a virtual inertial matrix, B d As a virtual damping matrix, R is a transformation matrix of a desired force direction coordinate system and a base coordinate system, gamma is a proportionality coefficient, and gamma=1+10||V (k) |is a transformation matrix of the desired force direction coordinate system and the base coordinate system 2 V is the speed, k is the kth control period, m is the default virtual inertia value, and b is the default virtual damping value.
6. The control method according to claim 1, characterized in that in the step S105, the admittance control equation of the robot is:
wherein ,Md Is a virtual inertial matrix, B d The system is a virtual damping matrix, X is a position under a Cartesian coordinate system, and F is human-machine interaction force;
according to the virtual inertia matrix, the virtual damping matrix, the man-machine interaction force and the speed at the current moment, the expected acceleration of the tail end of the robot in the next control period can be calculated to obtain:
wherein ,for a desired tip speed value sent to the robot, < >>For the end speed of the robot in the previous control period, F (k) is man-machine interaction force, M d Is a virtual inertial matrix, B d K represents a kth control period, and T represents the control period duration;
the desired angular velocity of the robotic joint may be obtained from an inverse jacobian matrix:
wherein ,for the desired angular velocity of the joint +.>Is an inverse Jacobian matrix->For a desired tip speed value sent to the robot, k represents the kth control period.
7. A force guidance control system of a robot with variable admittance, characterized in that the control system adopts the control method according to any one of claims 1-6, comprising a man-machine interaction module, a human intention prediction module, a variable admittance control module and a robot end position control module, wherein,
the man-machine interaction module comprises a robot tail end;
the human intention prediction module is used for collecting the tail end motion information and the man-machine interaction force information of the robot, predicting the acceleration at the next moment by applying the minimum jerk model and the speed curvature mode, and obtaining the expected force by combining the admittance control equation and the current speed;
the admittance-changing control module establishes a current expected force coordinate system according to the direction of the expected force to generate the virtual inertia and the virtual damping adjustment strategy;
and the robot tail end position control module acquires the expected speed of the robot tail end in the next control period according to the adjusted virtual inertia and the virtual damping, further acquires the joint angular speed of the robot, and controls the robot tail end to perform compliance action.
8. The control system of claim 7, wherein the human intent prediction module employs a computational model to calculate the desired force as follows:
wherein ,Fexpected For the desired force vector, M is the default virtual inertia matrix, B is the default virtual damping matrix, V is the current velocity vector,for a desired acceleration, k represents the kth control period.
9. The control system of claim 7, wherein the admittance control equation used by the admittance control module is:
wherein ,Md Is a virtual inertial matrix, B d The virtual damping matrix is formed by a Cartesian coordinate system, X is the position under the Cartesian coordinate system, and F is the man-machine interaction force between the robot and the person.
10. The control system of claim 7, wherein the robot tip position control module calculates the desired speed of the robot tip for the next control cycle using the following method:
wherein ,desired tip speed value sent to the robot for the current control cycle, is->The desired tip speed value sent to the robot for the last control cycle,/for the control cycle>For the end speed of the robot in the previous control period, F (k) is the man-machine interaction force in the current control period, M d Is a virtual inertial matrix, B d For the virtual damping matrix, k represents the kth control period and T represents the control period duration.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310731160.2A CN116604565A (en) | 2023-06-19 | 2023-06-19 | Force guiding control method and system for variable admittance of robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310731160.2A CN116604565A (en) | 2023-06-19 | 2023-06-19 | Force guiding control method and system for variable admittance of robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116604565A true CN116604565A (en) | 2023-08-18 |
Family
ID=87681903
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310731160.2A Pending CN116604565A (en) | 2023-06-19 | 2023-06-19 | Force guiding control method and system for variable admittance of robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116604565A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117921665A (en) * | 2024-01-31 | 2024-04-26 | 北京纳通医用机器人科技有限公司 | Mechanical arm control method, device, equipment and storage medium |
-
2023
- 2023-06-19 CN CN202310731160.2A patent/CN116604565A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117921665A (en) * | 2024-01-31 | 2024-04-26 | 北京纳通医用机器人科技有限公司 | Mechanical arm control method, device, equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Chen et al. | RBFNN-based adaptive sliding mode control design for delayed nonlinear multilateral telerobotic system with cooperative manipulation | |
| Park et al. | ITOMP: Incremental trajectory optimization for real-time replanning in dynamic environments | |
| CN105242533B (en) | A kind of change admittance remote operating control method for merging multi information | |
| Wen et al. | Elman fuzzy adaptive control for obstacle avoidance of mobile robots using hybrid force/position incorporation | |
| CN101825901B (en) | Multi-agent robot cooperative control method based on artificial physics method | |
| Hashemi et al. | Model-based PI–fuzzy control of four-wheeled omni-directional mobile robots | |
| CN112428278A (en) | Control method and device of mechanical arm and training method of man-machine cooperation model | |
| CN109968361B (en) | Variable impedance teleoperation control device and method based on real-time force feedback | |
| CN112894821B (en) | Current method based collaborative robot dragging teaching control method, device and equipment | |
| CN103433924A (en) | High-accuracy position control method for serial robot | |
| CN115256395B (en) | Model uncertain robot safety control method based on control obstacle function | |
| CN116604565A (en) | Force guiding control method and system for variable admittance of robot | |
| Wu et al. | Movement control and attitude adjustment of climbing robot on flexible surfaces | |
| CN111702767A (en) | Manipulator impedance control method based on inversion fuzzy self-adaptation | |
| CN115502966B (en) | Variable admittance control method for robot | |
| CN112782977B (en) | Motor compliance control method based on BP neural network and extended observer | |
| CN112405531B (en) | Position domain impedance control method and device, storage medium and operation robot | |
| Hu et al. | Prescribed time tracking control without velocity measurement for dual-arm robots | |
| Huang | Control approach for tracking a moving target by a wheeled mobile robot with limited velocities | |
| CN115366115B (en) | Mechanical arm control method and control system thereof and computer readable storage medium | |
| Prasad et al. | Multi-agent Polygon Formation using Reinforcement Learning. | |
| Ding et al. | Task-Oriented Adaptive Position/Force Control for Robotic Systems Under Hybrid Constraints | |
| Gan et al. | A speed jumping-free tracking controller with trajectory planner for unmanned underwater vehicle | |
| CN113703456A (en) | Automatic pile returning and charging method of robot based on multiple sensors | |
| JP3121920B2 (en) | Acceleration / deceleration control device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |