Disclosure of Invention
The invention aims to overcome the defects in the prior art, adopts the series elastic actuator with the compliance effect, provides a six-joint mechanical arm contact force control method based on the series elastic actuator, and simultaneously compensates the gravity of the series elastic actuator under different postures in real time to improve the contact force control precision, so that the problem of control over the contact force between the mechanical arm and the environment or a workpiece in the industrial machining process is solved, and the constant force control over the contact between the mechanical arm and the environment is realized.
In order to achieve the purpose, the technical scheme of the invention is as follows:
a contact force control method of a six-joint mechanical arm based on a series elastic actuator comprises the following steps:
step 1: setting a desired contact force Fd;
Step 2: obtaining the actual contact force F during contacts;
Step 3: series elastic actuator open loop transfer function of
By simplifying the velocity loop, k
vp=J
mω
sc,k
vi=B
mω
scThe electromechanical integration of the single-shaft servo is simplified into a model
The closed loop transfer function model of the simplified system is
Wherein, JmIs the rotational inertia of the motor, BmIs the damping coefficient, k, of the motorsIs the elastic coefficient of spring, s is complex variable operator of Laplace transform, N is the conversion coefficient of motor rotation motion to linear displacement, kppIs the proportionality coefficient of the position loop, ωscIs the cut-off frequency, k, of the velocity loopvpIs a proportional parameter of the velocity loop, kviAs an integral parameter of the velocity loop, kpIs the proportionality coefficient of the force controller, kdIs the differential coefficient of the force controller;
step4, determining the gravity mg of the series elastic actuator, and simultaneously calculating cosine values cos α of an included angle α between the gravity and the contact force direction under different postures, wherein the expression of cos α is
Wherein q is
2,q
3,q
4,q
5,q
6The angle of the 2 nd to 6 th joint angles of the mechanical arm and the initial angle of the sixth joint of the mechanical arm are β, and the estimated gravity compensation quantity is obtained
Wherein m is the mass of the series elastic actuator, and g is the gravity acceleration;
step 5: designing PD force controller with transfer function expression of C(s) ═ kp+kds;
Step 6: the expected contact force F in Step1dAnd the actual contact force F in Step2sAfter the comparison, the result is output to the PD force controller in Step5, and the displacement increment Yout is obtained as (k)p+kds)(Fd-Fs);
Step 7: acquiring a current actual position p of the motor;
step 8: the displacement increment Yout in the Step6, the actual position p of the motor in the Step7 and the compensation quantity of the estimated gravity are compared
Is added as an output signal
Output signal mu
1And (4) the motor is fed, so that the next arriving position of the motor is controlled, the compression amount of the spring is kept at a constant value, and the constant-force contact is realized.
Wherein, the relation of contact force and spring compression: fs=ksΔ x, wherein Δ x ═ xm-xl,xmFor outputting linear displacement, x, to the motor sidelFor load end displacement, Δ x is the amount of spring compression.
The relationship D ═ k between the digital signal of the displacement sensor and the spring compression Δ x2Δx+b1. Wherein k is2Coefficient relation between digital signal and spring compression, b1And the digital signal value of the displacement sensor is corresponding to the initial compression amount of the spring.
It follows that the relationship between the contact force and the digital signal of the displacement sensor is
Fs=ks(D-b1)/k2
Preferably, in the above-described aspect, the series elastic actuator in step1 includes: the guide rod penetrates through the sliding support through a through hole, two groups of springs penetrate through the front and the back of the sliding support respectively, and the springs are fixed by the guide rod fixing support; the fixed support is used for further fixing the series elastic actuator on the mechanical arm; the flange is fixed at the foremost end of the guide rod.
The invention has the beneficial effects that: compared with a single position control mode, the flexible performance of the tail end of the mechanical arm can be obviously improved, and the control on the contact force is realized; compared with an active flexible control mode, the force acquisition can be realized by detecting the compression amount of the spring by adopting the linear displacement sensor without using an expensive high-precision force sensor, so that the cost is greatly reduced. The control precision of the contact force is improved by estimating and compensating the gravity.
Detailed Description
The technical solution of the present invention is further described below with reference to the accompanying drawings and examples.
As shown in fig. 1, a contact force control method for a six-joint robot arm based on a series elastic actuator according to the present invention employs a series elastic actuator structure including: the device comprises a sliding support 1, a guide rod 2, a spring 3, a guide rod fixing support 4, a fixing support 5 and a flange 6.
The sliding support 1 is fixed below a sliding block of a ball screw shaft (mainly composed of linear modules) of the serial elastic actuator, and the guide rod 2 is inserted in the sliding support 1 through a through hole; two groups of springs 3 penetrate through the front and the back of the sliding support 1 respectively and are fixed by guide rod fixing supports 4 respectively; the fixed support 5 is U-shaped and is fixed at the tail end of the mechanical arm through a hexagon nut to further fix the series elastic actuator; the flange 6 is in the shape of an oblate cylinder and is fixed at the foremost end of the guide rod, and a threaded hole is formed in the flange and used for installing a TCP contact guide rod.
As shown in fig. 2, the invention is a six-joint mechanical arm contact force control method based on a series elastic actuator, the six-joint mechanical arm adopts stanobil TX90, the series elastic actuator is installed at the tail end of the six-joint mechanical arm, and the principle of the series elastic actuator is as follows: a KS1-15 spring self-resetting linear displacement sensor is adopted for detection, the working range is 15mm, and the standard resistance is 5K. When the spring generates different deformations, the spring reflects different digital signals D (the range of the digital signals is 0-4096). The real-time detection is carried out by using a 12-bit precision ADC of an I/O module bandConverting detected digital signal D of displacement sensor into actual contact force F
sThen contact with the set expected contact force F
dAfter comparison, the result is output to the PD controller. Obtaining the position increment Yout through the PD controller, and then compensating the position increment with the read real-time position p of the motor and the gravity
Added as an output signal mu
1And the output is transmitted to a motor to realize the feedforward compensation control of gravity, so that the position reached by the motor in the next step is controlled, the compression amount of the spring is kept at a constant value, and the constant force control is realized. Wherein, Table 1 shows the contact force F
sAnd the digital signal D.
TABLE 1
Digital signal D
|
Fs(N)
|
300
|
0
|
410
|
0.407
|
670
|
1.37
|
940
|
2.37
|
1210
|
3.37
|
1470
|
4.34
|
2000
|
6.30
|
2540
|
8.30
|
3010
|
10.10
|
3680
|
12.5 |
The method comprises the following specific implementation steps:
step 1: setting a desired contact force Fd;
Step 2: according to the contact force F measured experimentallysAnd spring deformation Δ x ═ xm-xlThe relationship between the contact force and the digital signal D of the displacement sensor is obtained according to the relationship between the spring deformation delta x and the digital signal D of the displacement sensor; according to the relation, the digital signal D of the self-resetting linear displacement sensor collected by the ADC with the resolution of 12 bits is converted into the actual contact force F in the contact processs=ks(D-b1)/k2Wherein k is2Coefficient relation between digital signal and spring compression, b1And the digital signal value of the displacement sensor is corresponding to the initial compression amount of the spring.
Step 3: series elastic actuator open loop transfer function of
By simplifying the velocity loop, k
vp=J
mω
sc,k
vi=B
mω
scThe electromechanical integration of the single-shaft servo is simplified into a model
SimplificationThe closed loop transfer function model of the rear system is
Wherein, JmIs the rotational inertia of the motor, BmIs the damping coefficient, k, of the motorsIs the elastic coefficient of spring, s is complex variable operator of Laplace transform, N is the conversion coefficient of motor rotation motion to linear displacement, kppIs the proportionality coefficient of the position loop, ωscIs the cut-off frequency, k, of the velocity loopvpIs a proportional parameter of the velocity loop, kviAs an integral parameter of the velocity loop, kpIs the proportionality coefficient of the force controller, kdIs the differential coefficient of the force controller;
step4, determining the gravity mg of the series elastic actuator, and calculating cosine values cos α of an included angle α between the gravity and the contact force direction under different postures to obtain an estimated gravity compensation quantity
Wherein m is the mass of the series elastic actuator, and g is the gravity acceleration;
step 5: designing PD force controller transfer function as C(s) ═ kp+kds, and determining parameters of the PD force controller;
step 6: the expected contact force F in Step1dAnd the actual contact force F in Step2sAfter the comparison, the result is output to the PD force controller in Step5, and the displacement increment Yout is obtained as (k)p+kds)(Fd-Fs);
Step 7: acquiring a current actual position p of the motor;
step 8: the displacement increment Yout in the Step6, the actual position p of the motor in the Step7 and the compensation quantity of the estimated gravity are compared
Is added as an output signal
Output signal mu
1And (4) the motor is fed, so that the next arriving position of the motor is controlled, the compression amount of the spring is kept at a constant value, and the constant-force contact is realized.
The control results of the contact force were compared by comparing the results of the experiment in Step5 with and without the addition of the gravity compensation amount. The simulation experiment results are shown in fig. 3, and the experiment results are shown in fig. 4a and 4 b.
The actual contact force in Step2 is obtained by converting the acquired digital signal D of the displacement sensor, and the relationship between the contact force and the digital signal D of the displacement sensor is obtained by measuring for many times by utilizing a Mini45 Net six-dimensional force/moment sensor of ATI company.
Wherein, the relation of contact force and spring compression: fs=ksΔx,ksFor the spring constant, Δ x is the spring compression.
The relation D of the digital signal D of the displacement sensor and the spring compression amount delta x is k2Δx+b1. Wherein k is2=270,b1=300。
From this, the relationship between the contact force and the digital signal of the displacement sensor can be obtained
Fs=ks(D-b1)/k2。
Wherein cosine values cos α of included angles between gravity and contact force directions in Step4 are obtained from all joint angles of the mechanical arm under different postures, and the expression is
Wherein q is2,q3,q4,q5,q6The angle of the 2 nd to 6 th joint angles of the mechanical arm is β, the initial angle of the sixth joint of the mechanical arm is β -59.65.
In Step s 7, the actual position p of the motor is directly read by the MC _ ReadActualPosition function block in the CoDeSys software.
The invention has the advantages that: the control of the compliance force of the mechanical arm is realized by adopting the series elastic actuator, meanwhile, the control precision of the force is improved by compensating the gravity of the actuator, the requirement of the industry on finish machining is met, and the safety and low impedance of the operation of the mechanical arm are improved by adopting the series elastic actuator. The cost is greatly reduced by using a common linear displacement sensor to replace an expensive force sensor.
The embodiments described in this specification are merely illustrative of implementations of the inventive concept and the scope of the present invention should not be considered limited to the specific forms set forth in the embodiments but rather by the equivalents thereof as may occur to those skilled in the art upon consideration of the present inventive concept.