Sensor-free robot compliance control method

Abstract

The application discloses a sensor-free robot compliance control method, comprising the following steps: obtaining the mapping relationship between the encoder variation of a robot and the external force suffered by the robot through force analysis calculation, so as to calculate the external force suffered by the joint in combination with the collected encoder variation; and constructing a mobility control model, calculating the target torque of the joint and controlling the joint movement. The application maps the variation of the encoder and the size and direction variation of the force applied by the dragging point by using the encoder in the joint of the surgical robot, does not need to install a sensor, has low cost and is suitable for the joint structure provided with the encoder.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot technology, in particular to a sensor-free robot compliance control method. BACKGROUND

[0002] In the development of today's society, robots play an increasingly important role, and people's requirements for the intelligence and safety of robots are also increasingly high. The intelligence mainly manifests as the active compliance control of human hands to robots, and the robots can flexibly realize the requirements of the operators, while the safety mainly manifests as the collision detection of the robots to avoid secondary damage to the external environment and itself. Especially in the application of medical robots, intelligence and safety are particularly important.

[0003] Whether it is compliance control or collision detection, force interaction between humans and robots will occur, and corresponding functions are realized through the processing and judgment of external forces. At present, there are mainly two implementation methods based on torque compensation and external sensors. The method based on torque compensation needs to compensate for the joint link gravity torque, friction torque, coriolis torque, centrifugal torque, etc. If you want to accurately and real-time obtain the torque size, you need to model the dynamics of the robot, which is tedious and complex. Moreover, the accuracy of the dynamic model is affected by many conditions, and the accuracy of the final model cannot be ensured. The method based on external sensors is more intuitive, and the implementation process is relatively simple and easy to understand, but this method is limited by the quality of the sensor. High-quality sensors are expensive, increasing production costs. SUMMARY

[0004] The present application provides a sensor-free robot compliance control method, which does not require the installation of sensors, is simple to operate, has low cost, and has wide applicability.

[0005] Technical scheme: A sensor-free robot compliance control method, comprising:

[0006] According to the force analysis, the mapping relationship between the encoder change amount of the robot and the external force subjected to the drag is calculated, so as to calculate the external force subjected to the joint in combination with the collected encoder change amount;

[0007] A steering control model is constructed, the target torque of the joint is calculated, and the joint movement is controlled.

[0008] The mapping relationship between the encoder change amount of the robot and the external force subjected to the drag is specifically:

[0009] According to the structural design parameters of each link, the external force subjected to the drag is mapped to the external force subjected to the encoder;

[0010] Based on the preset mapping relationship between encoder change and external force applied to the encoder, the mapping relationship between the applied external force and encoder change is obtained.

[0011] The preset mapping relationship between encoder change and external force on encoder is the mapping relationship between encoder change and the component of external force that causes encoder transformation, and is calculated by collecting multiple sets of data.

[0012] The admittance control model is as follows:

[0013] ;

[0014] In the formula, The matrix of the external forces acting on the vehicle is respectively One-dimensional elements of the inertia matrix M, damping matrix B, and stiffness matrix K; These represent the desired acceleration, desired velocity, and desired position of the joint, respectively. These represent the joint's current acceleration, current velocity, and current position, respectively.

[0015] The calculation of the target torque of the joint and the control of the joint movement accordingly specifically involves: calculating the target torque of the joint through the PD controller, calculating the desired position of the joint under the action of external force through the admittance control model, and controlling the motor movement through the position loop.

[0016] The specific steps of calculating the target torque of the joint and controlling the joint movement accordingly are as follows:

[0017] The gravitational torque and frictional torque were calculated based on the dynamic model.

[0018] The desired velocity of the robot joint is calculated using the admittance control model.

[0019] When the desired speed is within a preset range, the joint movement is controlled according to the gravitational torque and frictional torque.

[0020] When the desired speed is not within the preset range, the PD controller calculates the compensating static friction torque of the joint, and controls the joint movement by combining the gravitational torque and the friction torque.

[0021] The preset range is , The velocity collected when the joint starts. The velocity is the velocity collected during joint reversal.

[0022] Beneficial effects: the present application ingeniously utilizes the encoder in the joint of the surgical robot, maps the variation of the encoder with the variation of the size and direction of the force applied by the dragging position, obtains the desired position or speed of the joint through the admittance control principle, controls the motor movement through the position loop or current loop, does not need to install sensors, has low cost, and is suitable for the joint structure with the encoder. BRIEF DESCRIPTION OF DRAWINGS

[0023] Figure 1 It is a schematic diagram of the end mechanical arm configuration;

[0024] Figure 2 It is a compliant control flowchart of the present application;

[0025] Figure 3 It is a compliant control flowchart of another embodiment of the present application;

[0026] Figure 4 It is a force analysis diagram of the robot end link;

[0027] Figure 5 It is a block diagram of the admittance control principle.

[0028] Wherein, 1 is a motor, 2 is a first link, 3 is a second link, 4 is a third link, 5 is an encoder, and 6 is a dragging position. DETAILED DESCRIPTION

[0029] The present application will be further illustrated below in combination with the drawings and specific embodiments.

[0030] The end joint of the surgical robot generally adopts Figure 1 The mechanical arm configuration shown in the figure, the first link 2, the second link 3 and the third link 4 are sequentially hinged and connected and controlled by the same steel belt, the steel belt is controlled by the motor 1 at the front end of the first link 2 of the end joint, the third link 4 is hinged at the end of the instrument arm, and the encoder is installed at the hinge between the third link 4 and the instrument arm; the motor 1 drives the steel belt to move, so that the three links present a stretching movement, thereby making the instrument arm make a pitching movement around the telecentric fixed point. In the present application, the dragging position of the instrument arm is at the top end of the instrument arm, so that the encoder installation position and the external force application position are slightly deviated, the variation of the encoder and the size of the external force need to be mapped, and the desired movement position, speed and acceleration of the joint are obtained through the admittance control principle analysis, and then the motor movement is controlled through the current loop or the position loop in combination with the constructed dynamics model.

[0031] The sensor-free robot compliant control method of the present application is as shown in Figure 2 It includes the following steps:

[0032] (1) the mapping relationship between the encoder variation of each joint of the robot and the external force acting thereon is calculated.

[0033] As Figure 3 shown, according to the encoder installation position and the analysis of the external force of dragging, we have:

[0034] θ1+θ3=θ2-π / 2;

[0035] f x =f3sinθ3=-f3cos(θ2-θ1);

[0036] f y =f3cosθ3=f3sin(θ2-θ1);

[0037] Wherein, taking the instrument arm axis direction as the x direction and the direction perpendicular to the instrument arm axis direction as the y direction, then the components of the external force f3 of dragging in the directions parallel and perpendicular to the instrument arm axis are f x and f y ; θ3 is the included angle between the external force direction of dragging and the direction perpendicular to the instrument arm axis, θ1 is the angle between the external force direction of dragging and the axis of the link where the encoder is located, and θ2 is the included angle between the axis of the link where the encoder is located and the instrument arm axis.

[0038] Thus, the calculation results are:

[0039] ;

[0040] Wherein, is the external force on the instrument arm at the dragging position, is the external force on the third link at the hinge position on the instrument arm; I is a 3*3 unit matrix, is and the mapping matrix between them;

[0041] ;

[0042] Wherein, f1 is the external force on the encoder; is the mapping matrix between f1 and , and according to the force analysis of Figure 4 , we can obtain ;

[0043] Thus, we can obtain:

[0044] .

[0045] As can be seen from Figure 4 , only the force in the y direction drives the instrument arm to perform the pitching motion, and the force in the x direction has no effect on the change of the encoder value, so the change of the encoder value is only related to f 1y .

[0046] Let the relationship between the encoder variation and the external force on the encoder be:

[0047] ;

[0048] wherein, represents the encoder variation, represents the component force in the y direction, u and v can be obtained through experimental testing, that is, by collecting multiple sets of data of the external force on the encoder and the encoder variation, the sizes of u and v are calculated, and reasonable mapping is performed.

[0049] (2) Obtain the encoder variation, and calculate the external force on the joint according to the mapping relationship of step (1).

[0050] (3) Construct a mobility control model, calculate the target torque of the joint, and control the joint movement.

[0051] As shown in Figure 5 , the desired position of the robot joint can be calculated in real time according to the mobility control model, and the mobility model can be represented as:

[0052] ;

[0053] wherein, is the joint external force matrix calculated in step (2), and the present application obtains the component force corresponding to the x and y directions; are the desired acceleration, desired speed and desired position of the joint respectively, are the current acceleration, current speed and current position of the joint respectively, M, B and K are the inertia matrix, damping matrix and stiffness matrix respectively.

[0054] In the embodiment of the present application, the instrument arm makes pitching motion around the remote fixed point, and only has one pitching degree of freedom, so only the mobility control model of the single joint of the robot needs to be considered, and the above formula can be represented as:

[0055] ;

[0056] wherein, are the one-dimensional elements corresponding to the y direction in the matrix respectively.

[0057] The above formula can be applied to the controller in the following discretization manner:

[0058] ;

[0059] wherein, and are respectively actual position and expected position of the robot joint at the end of the nth sampling period, and T is the sampling period.

[0060] The target torque of the joint is calculated by the PD controller , and the joint motion is controlled as follows:

[0061]

[0062] wherein, , are respectively the coefficients of the position variation and the speed variation.

[0063] In the embodiment of the present application, the expected position of the robot joint under the action of the external force is obtained by the admittance control model analysis, and the motor motion is controlled by the position loop.

[0064] In another embodiment, as shown in Figure 3 , the expected speed of the robot joint under the action of the external force can also be obtained by the admittance control model analysis, and the motor motion is controlled by the current loop, which is specifically as follows:

[0065] The gravity torque and the friction torque are calculated according to the dynamics model;

[0066] The expected speed of the robot joint is calculated by the admittance control model, when the expected speed is within the preset range, the joint motion is controlled according to the gravity torque and the friction torque; when the expected speed is not within the preset range, the target torque of the joint is calculated by the aforementioned PD controller (i.e. compensating the static friction torque), and the joint motion is controlled in combination with the gravity torque and the friction torque.

[0067] wherein, the preset range is , is the speed collected when the robot joint is started, is the speed collected when the robot joint is reversed.

[0068] The encoder in the joint of the surgical robot is ingeniously utilized, the change of the encoder is analyzed, the size and direction of the force applied by the drag point are mapped, then the expected motion position, speed and acceleration of the joint are obtained by the admittance control principle analysis, and then the motor motion is controlled by the current loop or the position loop in combination with the constructed dynamics model.

[0069] The preferred embodiments of the present application are described in detail above, but the present application is not limited to the specific details in the above-described embodiments, and various equivalent transformations (such as quantity, shape, position, etc.) can be made to the technical solutions of the present application within the technical concept range of the present application, and these equivalent transformations all belong to the protection range of the present application.​

Claims

1. A sensorless robot compliance control method, a robot arm of the robot comprising: The first connecting rod, the second connecting rod and the third connecting rod are sequentially hinged and connected and controlled by the same steel belt, the steel belt is controlled by the motor at the front end of the first connecting rod, the third connecting rod is hinged with the instrument arm at the tail end, and the encoder is installed at the hinge between the third connecting rod and the instrument arm; the motor drives the steel belt to move, so that the first connecting rod, the second connecting rod and the third connecting rod present stretching movement, so that the instrument arm makes pitching movement around the telecentric fixed point; characterized by comprising the following steps: (1) according to the force analysis and the structural design parameters of each connecting rod, the mapping relationship between the encoder change and the external force acting on it is calculated, as follows: θ1+θ3=θ2-π / 2; f x = f3sin θ3 = -f3cos(θ2-θ1); f y = f3cosθ3= f3sin(θ2-θ1); Wherein, taking the direction of the instrument arm axis as the x direction and the direction perpendicular to the instrument arm axis as the y direction, the components of the external force f3 of the dragging in the directions parallel and perpendicular to the instrument arm axis are f x and f y ; θ3 is the included angle between the direction of the external force of the dragging and the direction perpendicular to the instrument arm axis, θ1 is the angle between the direction of the external force and the axis of the third connecting rod, and θ2 is the included angle between the axis of the third connecting rod and the instrument arm axis. thereby calculating: ； Wherein, f2 is the external force on the instrument arm at the joint with the third link; I is a 3x3 unit matrix, is a mapping matrix between f2 and f3; ； wherein f1 is an external force on the encoder; is a mapping matrix between f1 and f2, ; then obtaining: ； Only the force in the y direction drives the pitch movement of the instrument arm, the force in the x direction has no influence on the encoder value change, so the encoder value change is only related to The component force f 1y Related; Encoder variation The mapping relationship between the external force and the encoder is: ； wherein u and v are calculated by collecting a plurality of groups of data of the external force acting on the encoder and the encoder change; (2) obtaining the encoder change, and calculating the external force acting on the joint; (3) constructing a mobility control model, and calculating the gravity torque and the friction torque according to the dynamic model; calculating the expected speed of the robot joint through the mobility control model; when the expected speed is within the preset range, controlling the joint movement according to the gravity torque and the friction torque; when the expected speed is not within the preset range, calculating the compensation static friction torque of the joint through the PD controller, and controlling the joint movement in combination with the gravity torque and the friction torque.

2. The sensorless robot compliance control method of claim 1, wherein, The preset range is v1 is the speed collected when the joint is started, and v2 is the speed collected when the joint is reversed.

Images