CN115509117A - Flexible joint movement speed tracking system for large-space inertia variable load - Google Patents

Abstract

The invention relates to a flexible joint motion speed tracking system facing to space large inertia variable load, which comprises: the device comprises a joint position detection unit, a position ring controller, a speed ring controller, a current ring controller and a motor driving module. The output of the position loop controller is selected as the offset and summed with the desired velocity as the desired input to the velocity loop controller. The invention introduces high-frequency oscillation suppression transfer function in the proportional control to realize the suppression of speed oscillation, increases a negative feedback damping link and improves the adaptability of the controlled object to the large-range change of the load inertia.

Description

A Velocity Tracking System of Flexible Joint Motion for Large Space Inertia and Variable Load

technical field

The invention relates to the field of space robots, in particular to a flexible joint motion speed tracking system for large space inertia variable loads.

Background technique

Load inertia is the load characteristic generated by the moment of inertia of space mechanisms such as joints applied to the motor shaft, which is manifested as inertial force during acceleration and deceleration. Load inertia has a great influence on the control accuracy, stability and dynamic response of the motion control system . When designing the motor, it is necessary to design the load inertia ratio reasonably to avoid situations such as motion shock.

Compared with the gravity environment on the ground, the microgravity environment of space makes the load inertia characteristics the main influencing factor of the motion performance of space mechanisms. Considering the cost and weight requirements of space applications, when designing joints and motors, the load inertia ratio usually exceeds the value range of the ground mechanism by a large margin, which leads to large vibrations during the movement of the space mechanism, especially during acceleration and deceleration. At the same time, the load inertia of the space multi-body system represented by the space manipulator changes all the time during motion, and due to the existence of joint flexibility and nonlinear factors, it greatly increases the difficulty of joint motion control with large space inertia and variable load. The control methods for robot flexible joints mainly include traditional PD control and singular perturbation, cascade system, intelligent control and other methods. Among them, although PD control is simple, it often cannot meet the control accuracy requirements; other methods need to solve the problem of control performance degradation caused by uncertain robot parameters, and need to be implemented in combination with adaptive control or robust control, resulting in many control methods. It is quite complicated, and most of them are still in the stage of theoretical research or simulation, and there are few engineering applications.

Contents of the invention

The technical problem solved by the present invention is: to overcome the deficiencies of the prior art, to provide a flexible joint movement speed tracking system suitable for flexible joints with large inertia and variable loads, and to meet the needs of space flexible joints for motion stability and in-position accuracy.

Technical solution of the present invention is:

A flexible joint motion speed tracking system for large space inertia variable loads, including: a joint position detection unit, a position loop controller, a speed loop controller, a current loop controller and a motor drive module;

Position loop controller: according to the joint expected angle and the actual joint angle collected by the joint position detection unit, the compensation speed V _conp (m) is determined according to the position loop control period T _P and transmitted to the speed loop controller;

Speed loop controller: Receive the compensation speed V _conp (m) transmitted by the position loop controller, receive the input joint expected speed V _cmd (k), and control the joint expectation according to the speed loop control cycle T _v according to the compensation speed V _conp (m) The velocity V _cmd (k) is compensated to obtain the compensated expected velocity V _cmdIn (k); according to the compensated expected velocity V _cmdIn (k) and the motor actual angular velocity V _m (k) fed back by the motor drive module, the output expectation is determined The current i _Qcmd (k) is transmitted to the current loop controller;

Current loop controller: Receive the output expected current i _Qcmd (k) transmitted by the speed loop controller, and receive the actual current i _Q (k) corresponding to the motor output voltage fed back by the motor drive module, according to the output expected current i _Qcmd (k) and The actual current i _Q (k) adjusts the driving voltage output to the motor drive module, so that the actual current i _Q (k) continues to track the output expected current i _Qcmd (k);

Motor drive module: receives the drive voltage output by the current loop controller, drives the motor to drive the joint transmission mechanism to rotate according to the drive voltage; feeds back the actual angular velocity V _m (k) of the motor to the speed loop controller; feeds back the actual current i _Q corresponding to the output voltage of the motor (k) to the current loop controller.

Preferably, the position loop controller determines the compensation speed V _conp (m), specifically:

According to the expected joint angle θ _cmd (m) and the actual joint angle θ _j (m) of the current m-th position loop control cycle, determine the angle error θ _err (m) of the current m-th position loop control cycle;

According to the actual joint angle θ _j (m) of the current m-th position loop control cycle and the actual joint angle θ _j (m-1) of the m-1th position loop control cycle, determine the current m-th position loop control cycle Differential value of joint actual angle

确定补偿速度V_conp(m)。According to the differential value of the angle error θ _err (m) and the actual angle of the joint

Determine the compensation speed V _conp (m).

Preferably, the position loop controller determines the angle error θ _err (m) of the current mth position loop control cycle, specifically:

θ _err (m) = θ _cmd (m) - θ _j (m).

具体为：Preferably, the position loop controller determines the differential value of the actual joint angle of the current mth position loop control cycle

Specifically:

Preferably, the position loop controller determines the compensation speed V _conp (m), specifically:

Among them, K _PP is the proportional coefficient of the position loop controller, and K _PD is the differential coefficient of the position loop controller.

Preferably, the velocity loop controller compensates the expected joint velocity V _cmd (k) to obtain the compensated expected velocity V _cmdIn (k), specifically:

V _cmdIn (k) = V _cmd (k) + V _conp (m)

Among them, when the ratio Q of the position loop control period T _P to the speed loop control period T _v is 1, m=k;

When Q is a positive integer greater than 1:

Wherein, n is a positive integer.

Preferably, the speed loop controller determines the output expected current i _Qcmd (k) according to the compensated expected speed V _cmdIn (k) and the actual angular velocity V _m (k) of the motor, specifically:

The speed loop controller obtains the suppressed motor angular velocity error V _{m_errPF} , speed error integral value _{V m_errI ( k} ), motor angular velocity Differential value V _{m_D} (k), and motor speed damping V _{m_F} (k);

The speed loop controller determines the expected output current according to the obtained suppressed motor angular speed error V _{m_errPF} , speed error integral value V _{m_errI} (k), motor angular speed differential value V _{m_D} (k) and motor speed damping V _{m_F} (k) i _Qcmd (k).

Preferably, the velocity loop controller obtains the suppressed angular velocity error V _{m_errPF} of the motor as follows:

V _{m_err} (k) = V _{m_cmd} (k) - V _m (k)

V _{m_cmd} (k) = V _cmdIn (k) × N

Among them, T _Pf is the shock suppression coefficient, T _Pf is less than 1, and N is the joint transmission ratio;

The speed loop controller obtains the speed error integral value V _{m_errI} (k) specifically as follows:

Preferably, the speed loop controller obtains the motor angular velocity differential value V _{m_D} (k) specifically as follows:

V _{m_err} (k) = V _{m_cmd} (k) - V _m (k)

V _{m_cmd} (k)=V _cmdIn (k)×N.

Among them, N is the joint transmission ratio;

The speed loop controller obtains the motor speed damping V _{m_F} (k) specifically as follows:

V _{m_F} (k) = K _VF × V _m (k)

Among them, K _VF is the damping coefficient of the speed loop controller.

Preferably, the speed loop controller determines the desired output current i _Qcmd (k), specifically:

i _Qcmd (k)＝K _VP ×V _{m_errPF} (k)+K _VI ×V _{m_errI} (k)+K _VD ×V _{m_D} (k)-V _{m_F} (k)

Among them, K _VP is the proportional coefficient of the speed loop controller, K _VI is the integral coefficient of the speed loop controller, and K _VD is the differential coefficient of the speed loop controller.

The advantage of the present invention compared with prior art is:

The present invention can realize speed stability control and motion precision control of flexible mechanisms such as joints under the condition of large space load inertia and variable load, can effectively suppress the speed oscillation when joints start and stop, and adapt to large-scale changes in load inertia.

Description of drawings

Fig. 1 Architecture of flexible joint motion control system for large inertia variable load;

Fig. 2 Block diagram of space flexible joint velocity control.

detailed description

The present invention is a flexible joint motion speed tracking system oriented to space large inertia variable load, using the basic principle of PID controller, according to the characteristics of space flexible joint motion control, a motion control architecture as shown in Figure 1 is proposed, the position loop control The output of the controller is summed with the expected speed in the form of compensation, which is used as the input information of the speed loop; the high-frequency oscillation suppression transfer function is introduced in the proportional item of the speed closed loop; the negative feedback damping control is added in the speed closed loop. The present invention proposes a speed control method as shown in Figure 2, which converts the expected speed of the flexible joint into the expected speed of the motor, and after making a difference with the actual speed of the motor, input the speed loop proportional control, speed loop integral control, and speed loop differential control respectively, Among them, the proportional control of the speed loop is designed with a high-frequency oscillation suppression transfer letter. The speed loop damping link is designed, and the speed negative feedback is realized by introducing the actual speed of the motor. After summing (difference) all the control links, the desired current is output for current control. The invention can be applied to the motion control of space flexible joints with large inertia and variable load characteristics.

The load inertia is the load characteristic caused by the moment of inertia of the joint and other mechanisms applied to the motor shaft. On the ground, the ratio of load inertia to motor inertia is usually less than 10 for joints that use harmonic and planetary reducers. In space applications, considering the space microgravity environment, the ratio of the load inertia of the joint to the inertia of the motor will reach 10,000 or even higher, and in the multi-body system represented by the robot, the load inertia of the joint changes continuously with the change of the configuration.

According to the requirements of large inertia of space flexible joints and variable load motion control, the present invention compensates the joint expected speed V _cmd (k) according to the speed loop control cycle T _v , and obtains the compensated expected speed V _cmdIn (k), which is described as:

V _cmdIn (k) = V _cmd (k) + V _conp (m)

Among them, V _cmd (k) is the expected joint speed, and V _conp (m) is the compensation speed calculated by the position loop controller. The control cycle T _P of the position loop controller should not be less than the control cycle T _v of the speed loop controller, and T _P is Q times of T _v , and Q is a positive integer. When the ratio Q of the position loop control period T _P to the speed loop control period T _v is 1, m=k;

When Q is a positive integer greater than 1:

Wherein, n is a positive integer.

The position loop controller of the mth position loop control period is described as:

K _PP is the proportional coefficient of the position loop controller, K _PD is the differential coefficient of the position loop controller, the two coefficients need to be debugged, the initial value can be set to 1 and 0, and then debug, the response curve of the position loop is obtained during debugging, and Adjust the two coefficients according to the response until the requirements of the response time, stabilization time, stability margin and other indicators are met.

θ _err (m) is the angle error, θ _err (m) = θ _cmd (m) - θ _j (m), θ _cmd (m) is the expected joint angle, θ _j (m) is the joint position detection unit collected Actual joint angle.

为关节实际角度的微分，表示为：

is the differential of the actual joint angle, expressed as:

According to the speed control method shown in Figure 2, the compensated expected speed V _cmdIn (k) is converted into the motor expected speed V _{m_cmd} (k), and after making a difference with the actual speed of the motor, the motor angular speed error V _{m_err} (k) is obtained respectively Input speed loop proportional control, speed loop integral control, speed loop differential control.

Convert the desired speed of the input to the desired speed of the motor:

V _{m_cmd} (k) = V _cmdIn (k) × N

N is the joint transmission ratio.

Calculate the difference between the expected angular velocity V _{m_cmd} (k) of the motor and the actual angular velocity V _m (k) of the motor to obtain the angular velocity error V _{m_err} (k) of the motor:

V _{m_err} (k) = V _{m_cmd} (k) - V _m (k)

Speed loop proportional control includes high-frequency oscillation suppression transfer letter and speed loop proportional link. Among them, the high-frequency oscillation suppression transfer letter can effectively suppress the oscillation characteristics at startup, and the motor angular velocity error V _{m_errPF} after suppression is:

T _Pf is the shock suppression coefficient, and T _Pf is less than 1.

The speed error integral V _{m_errI} (k) is expressed as:

The speed loop differential item differentiates the motor angular velocity error, and the motor angular velocity differential V _{m_D} (k) is expressed as:

T _v is the control period of the speed loop.

The damping link can adjust the damping characteristics of the control system, and the motor speed damping V _{m_F} (k) is described as:

V _{m_F} (k) = K _VF × V _m (k)

Among them, V _m (k) is the actual angular velocity of the motor, which is the measured value fed back by the motor drive module, K _VF is the damping coefficient of the speed loop controller, and its value needs to be adjusted. The initial value can be set to 1 to obtain the motion speed of the flexible joint Track the stability margin of the system under different load conditions, and perform parameter iterative debugging according to the stability margin until the stability margin that meets the index requirements is obtained.

Integrating all the control links, the output expected current i _Qcmd (k) of the speed loop controller is:

i _Qcmd (k)＝K _VP ×V _{m_errPF} (k)+K _VI ×V _{m_errI} (k)+K _VD ×V _{m_D} (k)-V _{m_F} (k)

Among them, K _VP is the proportional coefficient of the speed loop controller, K _VI is the integral coefficient of the speed loop controller, K _VD is the differential coefficient of the speed loop controller, the three coefficients need to be debugged synchronously, and the initial value can be set to 1, 0, 0, The response time, overshoot, stability margin and other parameters of the flexible joint motion speed tracking system are obtained, and the iterative debugging of the three coefficients is carried out accordingly, until the parameters that meet the index requirements are obtained.

Current loop controller: Receive the output expected current i _Qcmd (k) transmitted by the speed loop controller, and receive the actual current i _Q (k) corresponding to the motor output voltage fed back by the motor drive module, according to the output expected current i _Qcmd (k) and The actual current i _Q (k) adjusts the driving voltage output to the motor drive module, so that the actual current i _Q (k) continuously tracks the output expected current i _Qcmd (k). U _D in Fig. 1 is the direct axis voltage, U _Q is the quadrature axis voltage.

The present invention uses the output expected current of the speed loop controller, and different motor driving methods can be used to drive the motor, so as to achieve the effect of motion control on the joints, so as to achieve the effect of tracking the expected positions or speeds of the joints.

Motor drive module: receives the drive voltage output by the current loop controller, drives the motor to drive the joint transmission mechanism to rotate according to the drive voltage; feeds back the actual angular velocity V _m (k) of the motor to the speed loop controller; feeds back the actual current i _Q corresponding to the output voltage of the motor (k) to the current loop controller.

Although the present invention has been disclosed as above with preferred embodiments, it is not intended to limit the present invention, and any person skilled in the art can use the methods disclosed above and technical content to analyze the present invention without departing from the spirit and scope of the present invention. Possible changes and modifications are made in the technical solution. Therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present invention, which do not depart from the content of the technical solution of the present invention, all belong to the technical solution of the present invention. protected range. In the case of no conflict, the embodiments of the present application and the technical features in the embodiments may be combined with each other.

The content that is not described in detail in the description of the present invention belongs to the well-known technology of those skilled in the art.

Claims (10)

1. A flexible joint motion speed tracking system for large space inertial variable loads, comprising: a joint position detection unit, a position loop controller, a speed loop controller, a current loop controller and a motor drive module; Position loop controller: according to the joint expected angle and the actual joint angle collected by the joint position detection unit, the compensation speed V _conp (m) is determined according to the position loop control period T _P and transmitted to the speed loop controller; Speed loop controller: Receive the compensation speed V _conp (m) transmitted by the position loop controller, receive the input joint expected speed V _cmd (k), and control the joint expectation according to the speed loop control cycle T _v according to the compensation speed V _conp (m) The velocity V _cmd (k) is compensated to obtain the compensated expected velocity V _cmdIn (k); according to the compensated expected velocity V _cmdIn (k) and the motor actual angular velocity V _m (k) fed back by the motor drive module, the output expectation is determined The current i _Qcmd (k) is transmitted to the current loop controller; Current loop controller: Receive the output expected current i _Qcmd (k) transmitted by the speed loop controller, and receive the actual current i _Q (k) corresponding to the motor output voltage fed back by the motor drive module, according to the output expected current i _Qcmd (k) and The actual current i _Q (k) adjusts the driving voltage output to the motor drive module, so that the actual current i _Q (k) continues to track the output expected current i _Qcmd (k); Motor drive module: receives the drive voltage output by the current loop controller, drives the motor to drive the joints to rotate according to the drive voltage; feeds back the actual angular velocity V _m (k) of the motor to the speed loop controller; feeds back the actual current i _Q (k) corresponding to the output voltage of the motor ) to the current loop controller. 2. A kind of flexible joint motion speed tracking system facing space big inertia variable load according to claim 1, is characterized in that, position loop controller determines compensation speed V _conp (m), specifically: According to the expected joint angle θ _cmd (m) and the actual joint angle θ _j (m) of the current m-th position loop control cycle, determine the angle error θ _err (m) of the current m-th position loop control cycle; According to the actual joint angle θ _j (m) of the current m-th position loop control cycle and the actual joint angle θ _j (m-1) of the m-1th position loop control cycle, determine the current m-th position loop control cycle Differential value of joint actual angle

According to the differential value of the angle error θ _err (m) and the actual angle of the joint

Determine the compensation speed V _conp (m). 3. A kind of flexible joint motion speed tracking system facing space large inertia variable load according to claim 2, characterized in that, the position loop controller determines the angular error _θerr (m) of the current mth position loop control cycle ,Specifically: θ _err (m) = θ _cmd (m) - θ _j (m). 4. A kind of flexible joint motion speed tracking system facing space large inertia variable load according to claim 2, characterized in that the position loop controller determines the differential value of the actual joint angle of the current mth position loop control cycle

Specifically:

5. A kind of flexible joint motion speed tracking system facing space large inertia variable load according to claim 2, characterized in that, the position loop controller determines the compensation speed V _conp (m), specifically:

Among them, K _PP is the proportional coefficient of the position loop controller, and K _PD is the differential coefficient of the position loop controller. 6. A kind of flexible joint movement velocity tracking system facing space large inertia variable load according to claim 1, characterized in that, the velocity loop controller compensates the joint expected velocity V _cmd (k) to obtain the compensated expected velocity V cmd (k) Velocity V _cmdIn (k), specifically: V _cmdIn (k) = V _cmd (k) + V _conp (m) Among them, when the ratio Q of the position loop control period T _P to the speed loop control period T _v is 1, m=k; When Q is a positive integer greater than 1:

Wherein, n is a positive integer. 7. A kind of flexible joint motion speed tracking system facing space large inertia variable load according to claim 1, characterized in that, the speed loop controller is based on the compensated expected speed V _cmdIn (k) and the actual angular velocity V _m of the motor (k), determine the output expected current i _Qcmd (k), specifically: The speed loop controller obtains the suppressed motor angular velocity error V _{m_errPF} , speed error integral value _{V m_errI ( k} ), motor angular velocity Differential value V _{m_D} (k), and motor speed damping V _{m_F} (k); The speed loop controller determines the expected output current according to the obtained suppressed motor angular speed error V _{m_errPF} , speed error integral value V _{m_errI} (k), motor angular speed differential value V _{m_D} (k) and motor speed damping V _{m_F} (k) i _Qcmd (k). 8. A kind of flexible joint motion speed tracking system facing space large inertia variable load according to claim 7, characterized in that, the speed loop controller obtains the suppressed motor angular velocity error V _{m_errPF} specifically as follows:

V _{m_err} (k) = V _{m_cmd} (k) - V _m (k) V _{m_cmd} (k) = V _cmdIn (k) × N Among them, T _Pf is the shock suppression coefficient, T _Pf is less than 1, and N is the joint transmission ratio; The speed loop controller obtains the speed error integral value V _{m_errI} (k) specifically as follows:

9. A kind of flexible joint motion speed tracking system facing space big inertia variable load according to claim 7, it is characterized in that, speed loop controller obtains motor angular velocity differential value V _{m_D} (k) specifically as follows:

V _{m_err} (k) = V _{m_cmd} (k) - V _m (k) V _{m_cmd} (k)=V _cmdIn (k)×N. Among them, N is the joint transmission ratio; The speed loop controller obtains the motor speed damping V _{m_F} (k) specifically as follows: V _{m_F} (k) = K _VF × V _m (k) Among them, K _VF is the damping coefficient of the speed loop controller. 10. According to any one of claims 7 to 9, a flexible joint motion speed tracking system facing large space inertia and variable loads is characterized in that the speed loop controller determines the output expected current i _Qcmd (k), specifically : i _Qcmd (k)＝K _VP ×V _{m_errPF} (k)+K _VI ×V _{m_errI} (k)+K _VD ×V _{m_D} (k)-V _{m_F} (k) Among them, K _VP is the proportional coefficient of the speed loop controller, K _VI is the integral coefficient of the speed loop controller, and K _VD is the differential coefficient of the speed loop controller.

Images